F hand control publish sg fast

sr_robot_lib/include/sr_robot_lib/sr_motor_hand_lib.hpp

@@ -139,7 +139,6 @@ class SrMotorHandLib :
    */
   std::vector<int> read_joint_to_motor_mapping();
 
-
   static const int nb_motor_data;
   static const char *human_readable_motor_data_types[];
   static const int32u motor_data_types[];

sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp

@@ -26,6 +26,8 @@
 #ifndef _SR_ROBOT_LIB_HPP_
 #define _SR_ROBOT_LIB_HPP_
 
+#include <realtime_tools/realtime_publisher.h>
+
 #include <boost/smart_ptr.hpp>
 #include <boost/ptr_container/ptr_vector.hpp>
 #include <boost/thread/thread.hpp>
@@ -35,9 +37,10 @@
 #include <string>
 #include <deque>
 #include <map>
-
+#include <realtime_tools/realtime_publisher.h>
 // used to publish debug values
 #include <std_msgs/Int16.h>
+#include <std_msgs/Int16MultiArray.h>
 
 #include <sr_hardware_interface/sr_actuator.hpp>
 
@@ -313,7 +316,6 @@ class SrRobotLib
   /// Id of the ethercat device (alias)
   std::string device_id_;
 
-
 #ifdef DEBUG_PUBLISHER
   // These publishers are useful for debugging
   static const int nb_debug_publishers_const;
@@ -331,6 +333,10 @@ class SrRobotLib
   ros::NodeHandle node_handle;
   std_msgs::Int16 msg_debug;
 #endif
+  std_msgs::Int16 msg_debug_tom;
+  std_msgs::Int16MultiArray msg_array_tom_r;
+  std_msgs::Int16MultiArray msg_array_tom_l;
+  std_msgs::Int16MultiArray msg_array_tom_pwm;
 
   // The update rate for each sensor information type
   std::vector<generic_updater::UpdateConfig> generic_sensor_update_rate_configs_vector;
@@ -354,7 +360,9 @@ class SrRobotLib
   static const double tactile_timeout;
   ros::Duration tactile_init_max_duration;
   ros::Timer tactile_check_init_timeout_timer;
-
+  boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray> > rt_pub_all_r;
+  boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray> > rt_pub_all_l;
+  boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray> > rt_pub_all_pwm;
   /// A mutual exclusion object to ensure that the intitialization timeout event does work without threading issues
   boost::shared_ptr<boost::mutex> lock_tactile_init_timeout_;
 

sr_robot_lib/src/sr_motor_robot_lib.cpp

@@ -54,6 +54,7 @@ using generic_updater::MotorDataChecker;
 using boost::shared_ptr;
 using boost::static_pointer_cast;
 
+
 namespace shadow_robot
 {
 
@@ -89,6 +90,44 @@ namespace shadow_robot
       ROS_INFO("Using TORQUE control.");
     }
 
+const unsigned int data_sz = 20;
+this->msg_array_tom_r.layout.dim.push_back(std_msgs::MultiArrayDimension());
+this->msg_array_tom_l.layout.dim.push_back(std_msgs::MultiArrayDimension());
+this->msg_array_tom_pwm.layout.dim.push_back(std_msgs::MultiArrayDimension());
+
+this->msg_array_tom_pwm.layout.dim[0].size = data_sz;
+this->msg_array_tom_pwm.layout.dim[0].stride = 1;
+this->msg_array_tom_pwm.layout.dim[0].label = "bla";
+
+this->msg_array_tom_r.layout.dim[0].size = data_sz;
+this->msg_array_tom_r.layout.dim[0].stride = 1;
+this->msg_array_tom_r.layout.dim[0].label = "bla";
+
+this->msg_array_tom_l.layout.dim[0].size = data_sz;
+this->msg_array_tom_l.layout.dim[0].stride = 1;
+this->msg_array_tom_l.layout.dim[0].label = "bla";
+
+this->msg_array_tom_l.data.resize(data_sz);
+this->msg_array_tom_r.data.resize(data_sz);
+this->msg_array_tom_pwm.data.resize(data_sz);
+
+
+    this->rt_pub_all_l = boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray> >(
+            new realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray>(this->nh_tilde, "rt_sg_all_l", 100));
+
+    this->rt_pub_all_r = boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray> >(
+            new realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray>(this->nh_tilde, "rt_sg_all_r", 100));
+
+
+    this->rt_pub_all_pwm = boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray> >(
+            new realtime_tools::RealtimePublisher<std_msgs::Int16MultiArray>(this->nh_tilde, "rt_sg_all_pwm", 100));
+
+    this->rt_pub_all_pwm->msg_ = this->msg_array_tom_pwm;
+    this->rt_pub_all_l->msg_ = this->msg_array_tom_l;
+    this->rt_pub_all_r->msg_ = this->msg_array_tom_r;
+
+
+
 #ifdef DEBUG_PUBLISHER
     this->debug_motor_indexes_and_data.resize(this->nb_debug_publishers_const);
     for (int i = 0; i < this->nb_debug_publishers_const; ++i)
@@ -188,6 +227,17 @@ namespace shadow_robot
       }
     }  // end for joint
 
+  if (this->rt_pub_all_l->trylock())
+      this->rt_pub_all_l->unlockAndPublish();
+
+
+  if (this->rt_pub_all_r->trylock())
+      this->rt_pub_all_r->unlockAndPublish();
+
+
+  if (this->rt_pub_all_pwm->trylock())
+      this->rt_pub_all_pwm->unlockAndPublish();
+
     // then we read the tactile sensors information
     this->update_tactile_info(status_data);
   }  // end update()
@@ -681,6 +731,14 @@ namespace shadow_robot
           actuator->motor_state_.strain_gauge_left_ =
                   static_cast<int16s> (status_data->motor_data_packet[index_motor_in_msg].misc);
 
+  //if (this->rt_pub_all_l->trylock())
+      this->rt_pub_all_l->msg_.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_left_;
+
+
+ // if (this->rt_pub_all_r->trylock())
+      this->rt_pub_all_r->msg_.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_right_;
+
+
 #ifdef DEBUG_PUBLISHER
         if (actuator_wrapper->motor_id == 19)
         {
@@ -694,6 +752,7 @@ namespace shadow_robot
           actuator->motor_state_.strain_gauge_right_ =
                   static_cast<int16s> (status_data->motor_data_packet[index_motor_in_msg].misc);
 
+
 #ifdef DEBUG_PUBLISHER
         if (actuator_wrapper->motor_id == 19)
         {
@@ -706,6 +765,7 @@ namespace shadow_robot
         case MOTOR_DATA_PWM:
           actuator->motor_state_.pwm_ =
                   static_cast<int> (static_cast<int16s> (status_data->motor_data_packet[index_motor_in_msg].misc));
+      this->rt_pub_all_pwm->msg_.data[actuator_wrapper->motor_id] = actuator->motor_state_.pwm_;
 
 #ifdef DEBUG_PUBLISHER
         if (actuator_wrapper->motor_id == 19)