Adding new tactile sensor data structure to list of tactile sensors hardware interface (shadow-robot#232)

* Adding MST data structure to list of tactile sensors hardware interface

* Changing new sensor message.

* Increase git_version by one byte to accomodate new sensor status check

Author: rnzenha-s

sr_hardware_interface/include/sr_hardware_interface/tactile_sensors.hpp

@@ -1,5 +1,5 @@
 /*
-* Copyright 2011 Shadow Robot Company Ltd.
+* Copyright 2011, 2023 Shadow Robot Company Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
@@ -33,6 +33,7 @@
 #include <boost/algorithm/string.hpp>
 #include <boost/algorithm/string/find_iterator.hpp>
 #include <boost/circular_buffer.hpp>
+#include <geometry_msgs/Point.h>
 #include <sstream>
 
 #include <ros/ros.h>
@@ -49,13 +50,15 @@ class GenericTactileData
   GenericTactileData(bool tactile_data_valid, int sample_frequency,
                      std::string manufacturer, std::string serial_number,
                      int software_version_current, int software_version_server,
-                     bool software_version_modified, std::string pcb_version)
-          : tactile_data_valid(tactile_data_valid), sample_frequency(sample_frequency),
-            manufacturer(manufacturer), serial_number(serial_number),
-            software_version_current(software_version_current),
-            software_version_server(software_version_server),
-            software_version_modified(software_version_modified),
-            pcb_version(pcb_version)
+                     bool software_version_modified, std::string pcb_version) :
+    tactile_data_valid(tactile_data_valid),
+    sample_frequency(sample_frequency),
+    manufacturer(manufacturer),
+    serial_number(serial_number),
+    software_version_current(software_version_current),
+    software_version_server(software_version_server),
+    software_version_modified(software_version_modified),
+    pcb_version(pcb_version)
   {
   };
 
@@ -85,12 +88,12 @@ class GenericTactileData
    */
   void set_software_version(char* raw_version)
   {
-    // New Git format: \n\n 20 bytes Git revision
+    // New Git format: \n\n 20 bytes Git revision + 1 byte for status check enable flag
     if (raw_version[0] == '\n' && raw_version[1] == '\n')
     {
       // Convert Git revision to hexadecimal long hash
       std::stringstream git_revision;
-      for (int i = 2; i < 22; i++)
+      for (int i = 2; i < 23; i++)
       {
         git_revision << std::setfill('0') << std::setw(2) << std::hex <<
           static_cast<int>(static_cast<uint8_t>(raw_version[i]));
@@ -177,32 +180,35 @@ class GenericTactileData
   }
 };
 
-class PST3Data
-        :
-                public GenericTactileData
+class PST3Data :
+        public GenericTactileData
 {
 public:
-  PST3Data()
-          : GenericTactileData()
+  PST3Data() :
+    GenericTactileData()
   {
   };
 
-  PST3Data(const PST3Data &pst3)
-          : GenericTactileData(pst3.tactile_data_valid, pst3.sample_frequency,
-                               pst3.manufacturer, pst3.serial_number,
-                               pst3.software_version_current,
-                               pst3.software_version_server,
-                               pst3.software_version_modified,
-                               pst3.pcb_version),
-            pressure(pst3.pressure), temperature(pst3.temperature),
-            debug_1(pst3.debug_1), debug_2(pst3.debug_2),
-            pressure_raw(pst3.pressure_raw), zero_tracking(pst3.zero_tracking), dac_value(pst3.dac_value)
+  PST3Data(const PST3Data &pst3) :
+    GenericTactileData(pst3.tactile_data_valid, pst3.sample_frequency,
+                       pst3.manufacturer, pst3.serial_number,
+                       pst3.software_version_current,
+                       pst3.software_version_server,
+                       pst3.software_version_modified,
+                       pst3.pcb_version),
+    pressure(pst3.pressure),
+    temperature(pst3.temperature),
+    debug_1(pst3.debug_1),
+    debug_2(pst3.debug_2),
+    pressure_raw(pst3.pressure_raw),
+    zero_tracking(pst3.zero_tracking),
+    dac_value(pst3.dac_value)
   {
   };
 
 
-  explicit PST3Data(const GenericTactileData &gtd)
-          : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
+  explicit PST3Data(const GenericTactileData &gtd) :
+            GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
                                gtd.manufacturer, gtd.serial_number,
                                gtd.software_version_current,
                                gtd.software_version_server,
@@ -245,41 +251,42 @@ class PST3Data
   };
 };
 
-class BiotacData
-        :
-                public GenericTactileData
+class BiotacData :
+        public GenericTactileData
 {
 public:
-  BiotacData()
-          : GenericTactileData()
+  BiotacData() :
+    GenericTactileData()
   {
     pac_buffer_ = boost::circular_buffer<int16_t>(pac_size_);
     pac_vector_.reserve(pac_size_);
   };
 
-  BiotacData(const BiotacData &btac)
-          : GenericTactileData(btac.tactile_data_valid, btac.sample_frequency,
-                               btac.manufacturer, btac.serial_number,
-                               btac.software_version_current,
-                               btac.software_version_server,
-                               btac.software_version_modified,
-                               btac.pcb_version),
-            pac0(btac.pac0), pac1(btac.pac1),
-            pdc(btac.pdc), tac(btac.tac),
-            tdc(btac.tdc)
+  BiotacData(const BiotacData &btac) :
+    GenericTactileData(btac.tactile_data_valid, btac.sample_frequency,
+                       btac.manufacturer, btac.serial_number,
+                       btac.software_version_current,
+                       btac.software_version_server,
+                       btac.software_version_modified,
+                       btac.pcb_version),
+    pac0(btac.pac0),
+    pac1(btac.pac1),
+    pdc(btac.pdc),
+    tac(btac.tac),
+    tdc(btac.tdc)
   {
     electrodes = std::vector<int16_t>(btac.electrodes);
     pac_vector_ = std::vector<int16_t>(btac.pac_vector_);
     pac_buffer_ = boost::circular_buffer<int16_t>(btac.pac_buffer_);
   };
 
-  explicit BiotacData(const GenericTactileData &gtd)
-          : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
-                               gtd.manufacturer, gtd.serial_number,
-                               gtd.software_version_current,
-                               gtd.software_version_server,
-                               gtd.software_version_modified,
-                               gtd.pcb_version)
+  explicit BiotacData(const GenericTactileData &gtd) :
+    GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
+                       gtd.manufacturer, gtd.serial_number,
+                       gtd.software_version_current,
+                       gtd.software_version_server,
+                       gtd.software_version_modified,
+                       gtd.pcb_version)
   {
     pac_buffer_ = boost::circular_buffer<int16_t>(pac_size_);
     pac_vector_.reserve(pac_size_);
@@ -321,23 +328,22 @@ class BiotacData
   std::vector<int16_t> pac_vector_;
 };
 
-class UBI0Data
-        :
-                public GenericTactileData
+class UBI0Data :
+        public GenericTactileData
 {
 public:
-  UBI0Data()
-          : GenericTactileData()
+  UBI0Data() :
+    GenericTactileData()
   {
   };
 
-  UBI0Data(const UBI0Data &ubi0)
-          : GenericTactileData(ubi0.tactile_data_valid, ubi0.sample_frequency,
-                               ubi0.manufacturer, ubi0.serial_number,
-                               ubi0.software_version_current,
-                               ubi0.software_version_server,
-                               ubi0.software_version_modified,
-                               ubi0.pcb_version)
+  UBI0Data(const UBI0Data &ubi0) :
+    GenericTactileData(ubi0.tactile_data_valid, ubi0.sample_frequency,
+                       ubi0.manufacturer, ubi0.serial_number,
+                       ubi0.software_version_current,
+                       ubi0.software_version_server,
+                       ubi0.software_version_modified,
+                       ubi0.pcb_version)
   {
     for (unsigned int i = 0; i < ubi0.distal.size(); i++)
     {
@@ -353,8 +359,8 @@ class UBI0Data
     }
   };
 
-  explicit UBI0Data(const GenericTactileData &gtd)
-          : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
+  explicit UBI0Data(const GenericTactileData &gtd) :
+            GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
                                gtd.manufacturer, gtd.serial_number,
                                gtd.software_version_current,
                                gtd.software_version_server,
@@ -395,11 +401,40 @@ class UBI0PalmData
   boost::array<uint16_t, 16ul> palm;
 };
 
+class MSTData :
+        public GenericTactileData
+{
+public:
+  MSTData() :
+    GenericTactileData()
+  {
+  };
+
+  explicit MSTData(const GenericTactileData &gtd) :
+    GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
+                       gtd.manufacturer, gtd.serial_number,
+                       gtd.software_version_current,
+                       gtd.software_version_server,
+                       gtd.software_version_modified,
+                       gtd.pcb_version)
+  {
+  };
+
+  ~MSTData()
+  {
+  };
+
+  std::vector<geometry_msgs::Point> magnetic_data;
+  std::vector<float> temperature_data;
+  int8_t status_check;
+};
+
 struct AllTactileData
 {
   std::string type;
   BiotacData biotac;
   PST3Data pst;
+  MSTData mst;
   UBI0Data ubi0;
 };
 }  // namespace tactiles