Merge lp:~shadowrobot/sr-ros-interface-ethercat/tactiles_software_version into lp:sr-ros-interface-ethercat

[Ugo]

Fixed the software version for the tactiles.

[lp:~shadowrobot/sr-ros-interface-ethercat/tactiles_software_version]

521. By Hand <hand@syntouch-big> on 2011-12-07

TESTED: Small corrections + documentation

[Ugo (ugocupcic)]

this should now work.

[Hugo Elias (hugo-shadowrobot)]

review: Approve

=== modified file 'sr_robot_lib/include/sr_robot_lib/tactile_sensors.hpp'
--- sr_robot_lib/include/sr_robot_lib/tactile_sensors.hpp	2011-10-26 18:08:05 +0000
+++ sr_robot_lib/include/sr_robot_lib/tactile_sensors.hpp	2011-12-07 14:07:26 +0000
@@ -27,6 +27,9 @@
 #ifndef _TACTILE_SENSORS_HPP_
 #define _TACTILE_SENSORS_HPP_
 
+#include <iostream>
+#include <sstream>
+#include <string>
 #include <string>
 #include <vector>
 
@@ -41,11 +44,15 @@
 
     GenericTactileData(bool tactile_data_valid, int sample_frequency,
                        std::string manufacturer, std::string serial_number,
-                       int software_version, int pcb_version)
+                       int software_version_current, int software_version_server,
+                       bool software_version_modified, int pcb_version)
       : tactile_data_valid(tactile_data_valid), sample_frequency(sample_frequency),
-        manufacturer(manufacturer), serial_number(serial_number), 
-        software_version(software_version), pcb_version(pcb_version) 
-      {};
+        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)
+    {};
 
     ~GenericTactileData() {};
 
@@ -56,8 +63,36 @@
     std::string manufacturer;
     std::string serial_number;
 
-    int software_version;
+    int software_version_current;
+    int software_version_server;
+    bool software_version_modified;
+
+    /**
+     * Parses the version string received
+     *  from the tactiles and fill in the 
+     *  variables.
+     *
+     * @version The raw version string.
+     */
+    void set_software_version( std::string version );
+    /**
+     * Formats the software version for the 
+     *  diagnostics.
+     *
+     * @return the formatted string
+     */
+    virtual std::string get_software_version();
+
     int pcb_version;
+
+    inline double convertToInt(std::string const& s)
+    {
+      std::istringstream i(s);
+      int x;
+      if (!(i >> x))
+        x = -1;
+      return x;
+    }
   };
 
   class PST3Data
@@ -103,6 +138,14 @@
     int tac; //int16u in word[2]
     int tdc; //int16u in word[2]
     boost::array<short int, 19ul> electrodes; //int16u in word[2]
+
+    /**
+     * Formats the software version for the 
+     *  diagnostics.
+     *
+     * @return the formatted string
+     */
+    virtual std::string get_software_version();
   };
 }
 

=== modified file 'sr_robot_lib/src/biotac.cpp'
--- sr_robot_lib/src/biotac.cpp	2011-12-05 18:12:11 +0000
+++ sr_robot_lib/src/biotac.cpp	2011-12-07 14:07:26 +0000
@@ -192,7 +192,7 @@
       case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
         {
-          tactiles_vector->at(id_sensor).software_version = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
+          tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
         }
         break;
 
@@ -280,7 +280,7 @@
       d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str());
       d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str());
 
-      d.addf("Software Version", "%d", tactiles_vector->at(id_tact).software_version);
+      d.addf("Software Version", "%s", tactiles_vector->at(id_tact).get_software_version().c_str());
       d.addf("PCB Version", "%d", tactiles_vector->at(id_tact).pcb_version);
 
       vec.push_back(d);

=== modified file 'sr_robot_lib/src/generic_tactiles.cpp'
--- sr_robot_lib/src/generic_tactiles.cpp	2011-12-05 18:12:11 +0000
+++ sr_robot_lib/src/generic_tactiles.cpp	2011-12-07 14:07:26 +0000
@@ -102,7 +102,7 @@
         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
         {
           if( tactiles_vector != NULL )
-            tactiles_vector->at(id_sensor).software_version = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
+            tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
         }
         break;
 

=== modified file 'sr_robot_lib/src/shadow_PSTs.cpp'
--- sr_robot_lib/src/shadow_PSTs.cpp	2011-12-05 18:12:11 +0000
+++ sr_robot_lib/src/shadow_PSTs.cpp	2011-12-07 14:07:26 +0000
@@ -120,7 +120,7 @@
       case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
         {
-          tactiles_vector->at(id_sensor).software_version = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
+          tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
         }
         break;
 
@@ -187,7 +187,7 @@
       d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str());
       d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str());
 
-      d.addf("Software Version", "%d", tactiles_vector->at(id_tact).software_version);
+      d.addf("Software Version", "%d", tactiles_vector->at(id_tact).get_software_version().c_str());
       d.addf("PCB Version", "%d", tactiles_vector->at(id_tact).pcb_version);
 
       d.addf("Pressure Raw", "%d", tactiles_vector->at(id_tact).pressure_raw);

=== modified file 'sr_robot_lib/src/sr_robot_lib.cpp'
--- sr_robot_lib/src/sr_robot_lib.cpp	2011-12-06 18:43:27 +0000
+++ sr_robot_lib/src/sr_robot_lib.cpp	2011-12-07 14:07:26 +0000
@@ -606,7 +606,7 @@
 	  if( joint_tmp->motor->motor_id == 8 )
 	    {
 	      //ROS_ERROR_STREAM("Current " <<actuator->state_.last_measured_current_);
-	      msg_debug.data = actuator->state_.last_measured_current_;
+	      msg_debug.data = static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
 	      debug_publishers[2].publish(msg_debug);
 	    }
 #endif
@@ -619,7 +619,7 @@
 	  if( joint_tmp->motor->motor_id == 8 )
 	    {
 	      //ROS_ERROR_STREAM("Voltage " <<actuator->state_.motor_voltage_);
-	      msg_debug.data = actuator->state_.motor_voltage_;
+	      msg_debug.data = static_cast<int16u>(status_data->motor_data_packet[index_motor_in_msg].misc);
 	      debug_publishers[3].publish(msg_debug);
 	    }
 #endif
@@ -745,9 +745,20 @@
       }
 
       if (read_torque)
+      {
         actuator->state_.last_measured_effort_ =
             static_cast<double>(static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque));
 
+#ifdef DEBUG_PUBLISHER
+	if( joint_tmp->motor->motor_id == 8 )
+	  {
+	    //ROS_ERROR_STREAM("Torque " << static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque));
+	    msg_debug.data = static_cast<int16s>(status_data->motor_data_packet[index_motor_in_msg].torque);
+	    debug_publishers[4].publish(msg_debug);
+	  }
+#endif
+      }
+
       //Check the message to see if everything has already been received
       if (motor_current_state == operation_mode::device_update_state::INITIALIZATION)
       {

=== modified file 'sr_robot_lib/src/tactile_sensors.cpp'
--- sr_robot_lib/src/tactile_sensors.cpp	2011-12-05 12:36:19 +0000
+++ sr_robot_lib/src/tactile_sensors.cpp	2011-12-07 14:07:26 +0000
@@ -25,10 +25,46 @@
  */
 
 #include "sr_robot_lib/tactile_sensors.hpp"
+#include <boost/algorithm/string.hpp>
+#include <boost/algorithm/string/find_iterator.hpp>
+#include <sstream>
 
+#include <ros/ros.h>
 namespace tactiles
 {
 
+  void GenericTactileData::set_software_version( std::string version )
+  {
+    //split the string to fill the different versions
+    std::vector<std::string> splitted_string;
+    boost::split(splitted_string, version, boost::is_any_of("\n"));
+
+    BOOST_ASSERT(splitted_string.size() >= 3);
+    software_version_current = convertToInt(splitted_string[0]);
+    software_version_server = convertToInt(splitted_string[1]);
+
+    if( splitted_string[2] == "No")
+      software_version_modified = false;
+    else
+      software_version_modified = true;
+  }
+
+  std::string GenericTactileData::get_software_version()
+  {
+    //concatenate versions in a string.
+    std::string full_version;
+
+    std::stringstream ss;
+    if( software_version_modified )
+      ss << "current: " << software_version_current << " / server: " << software_version_server << " / MODIFIED";
+    else
+      ss << "current: " << software_version_current << " / server: " << software_version_server << " / not modified";
+
+    full_version = ss.str();
+
+    return full_version;
+  }
+
   PST3Data::PST3Data()
     : GenericTactileData()
   {};
@@ -36,7 +72,10 @@
   PST3Data::PST3Data(const PST3Data& pst3)
     : GenericTactileData(pst3.tactile_data_valid, pst3.sample_frequency,
                          pst3.manufacturer, pst3.serial_number,
-                         pst3.software_version, pst3.pcb_version),
+                         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)
@@ -45,7 +84,10 @@
   PST3Data::PST3Data(const GenericTactileData& gtd)
     : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
                          gtd.manufacturer, gtd.serial_number,
-                         gtd.software_version, gtd.pcb_version)
+                         gtd.software_version_current,
+                         gtd.software_version_server,
+                         gtd.software_version_modified,
+                         gtd.pcb_version)
   {};
 
   BiotacData::BiotacData()
@@ -55,7 +97,10 @@
   BiotacData::BiotacData(const BiotacData& btac)
     : GenericTactileData(btac.tactile_data_valid, btac.sample_frequency,
                          btac.manufacturer, btac.serial_number,
-                         btac.software_version, btac.pcb_version),
+                         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)
@@ -69,8 +114,25 @@
   BiotacData::BiotacData(const GenericTactileData& gtd)
     : GenericTactileData(gtd.tactile_data_valid, gtd.sample_frequency,
                          gtd.manufacturer, gtd.serial_number,
-                         gtd.software_version, gtd.pcb_version)
+                         gtd.software_version_current,
+                         gtd.software_version_server,
+                         gtd.software_version_modified,
+                         gtd.pcb_version)
   {};
+
+  std::string BiotacData::get_software_version()
+  {
+    //concatenate versions in a string.
+    std::string full_version;
+
+    std::stringstream ss;
+    ss << "current: " << software_version_current;
+
+    full_version = ss.str();
+
+    return full_version;
+  }
+
 }