Add mutex to joint state interface reading robot's state and writing

ctrl.

include/gz_gep_tools/joint_state_interface.hh

+#pragma once
 /// Standard includes
 #include <vector>
 #include <string>
@@ -8,13 +9,40 @@
 
 namespace gz_transport_hw_tools {
 
+class LastState {
+ public:
+  /// Time
+  int64_t time_sec_;
+  int64_t time_nsec_;
+
+  /// Position
+  std::vector<double> positions_;
+
+  /// Velocity
+  std::vector<double> velocities_;
+
+  void resize(std::size_t asize);
+
+  std::mutex lock_state_access_;
+  LastState();
+};
+
+class JointValues {
+  double pos_mes;
+  double vel_mes;
+  double force_ctrl;
+  std::string cmd_force_topic;
+  gz::transport::Node::Publisher;
+};
+
 /// This class handles the interface to the joints and the sensors
 /// of a robot simulated by Gazebo (Harmonic)
 class JointStateInterface {
  public:
   /// Constructor
   JointStateInterface(std::string &a_prefix_model_root,
-                      std::string &a_prefix_world);
+                      std::string &a_prefix_world,
+                      bool debug_level=false);
   /// Destructor
   ~JointStateInterface();
 
@@ -24,26 +52,27 @@ class JointStateInterface {
 
   bool SetCmd(const std::vector<double> &cmd_vec);
 
-  void GetPosVel(std::vector<double> &pos_vecd,
-                 std::vector<double> &vel_vecd);
-  
+  bool GetPosVel(std::vector<double> &pos_vecd,
+                 std::vector<double> &vel_vecd,
+                 double &time);
+
   private:
 
   /// Callback function for state model update.
   void CallbackJointState(const gz::msgs::Model &a_gz_model_msg);
-  
+
   /// Root of the prefix model
   std::string prefix_model_root_;
 
   /// Prefix world
   std::string prefix_world_;
-  
+
   /// List of joints
   std::vector<std::string> list_of_joints_;
 
   /// Map joints to index in list
   std::map<std::string, std::size_t> map_name_2_indx_;
-  
+
   /// Vector of string describing the topics to command forces on actuators.
   std::vector<std::string> cmd_force_topics_;
 
@@ -56,11 +85,10 @@ class JointStateInterface {
   /// GZ node
   gz::transport::Node node_;
 
-  /// Position
-  std::vector<double> positions_;
+  /// Last state of the robot
+  LastState last_state_;
 
-  /// Velocity
-  std::vector<double> velocities_;
+  bool debug_level_;
 };
 
 }

src/joint_state_interface.cc

@@ -4,8 +4,27 @@
 
 namespace gz_transport_hw_tools {
 
+LastState::LastState() :
+    time_sec_(0),
+    time_nsec_(0)
+{}
+
+void LastState::resize(size_t asize) {
+  positions_.clear();
+  positions_.resize(asize);
+  velocities_.clear();
+  velocities_.resize(asize);
+  for(unsigned i=0;i<positions_.size();i++)
+  {
+    positions_[i]  = 0.0;
+    velocities_[i] = 0.0;
+  }
+}
+
 JointStateInterface::JointStateInterface(std::string &a_prefix_model_root,
-                                         std::string &a_prefix_world) {
+                                         std::string &a_prefix_world,
+                                         bool debug_level):
+    debug_level_(debug_level) {
   prefix_model_root_ = a_prefix_model_root;
   prefix_world_ = a_prefix_world ;
   joint_state_topic_ =a_prefix_world +  a_prefix_model_root + "joint_state";
@@ -43,32 +62,48 @@ void JointStateInterface::SetListOfJoints(
     map_name_2_indx_[*jointItr] = idx;
     idx++;
   }
-  positions_.clear();
-  positions_.resize(a_list_of_joints.size());
-  velocities_.clear();
-  velocities_.resize(a_list_of_joints.size());
+
+  last_state_.resize(a_list_of_joints.size());
+
 }
 
 void JointStateInterface::CallbackJointState(
     const gz::msgs::Model&a_gz_model_msg
     ) {
+
+  std::lock_guard(last_state_.lock_state_access_);
   unsigned int idx_joints=0;
   for (auto jointItr = a_gz_model_msg.joint().begin();
        jointItr != a_gz_model_msg.joint().end();
        jointItr++) {
 
+
     const ::gz::msgs::Axis &axis1 = jointItr->axis1();
 
     long unsigned int local_joint_idx = map_name_2_indx_[jointItr->name()];
 
-    if (positions_.size()!=0)
-      positions_[local_joint_idx] = axis1.position();
-    if (velocities_.size()!=0)
-      velocities_[local_joint_idx] = axis1.velocity();
+    if (last_state_.positions_.size()!=0)
+      last_state_.positions_[local_joint_idx] = axis1.position();
+    if (last_state_.velocities_.size()!=0)
+      last_state_.velocities_[local_joint_idx] = axis1.velocity();
 
     idx_joints++;
   }
 
+  if (debug_level_)
+  {
+    std::cerr << "JointStateInterface::CallbackJointState: time: "
+              << " " << last_state_.time_sec_
+              << " " << last_state_.time_nsec_;
+    if (last_state_.positions_.size()>1)
+      std::cerr << " " << last_state_.positions_[1];
+
+    std::cerr << std::endl;
+  }
+  last_state_.time_sec_ = a_gz_model_msg.header().stamp().sec();
+  last_state_.time_nsec_ = a_gz_model_msg.header().stamp().nsec();
+
+
 }
 
 bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
@@ -76,7 +111,8 @@ bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
   if (a_cmd_vec_d.size()!=gz_pub_cmd_forces_.size())
     return false;
 
-  for (unsigned int i = 0; i < a_cmd_vec_d.size(); i++) {
+  for (unsigned int i = 0; i < a_cmd_vec_d.size(); i++)
+  {
     gz::msgs::Double msg;
     msg.set_data(a_cmd_vec_d[i]);
 
@@ -84,20 +120,37 @@ bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
       std::cerr << "Unable to publish on "
                << cmd_force_topics_[i]
                << std::endl;
+      return false;
     }
   }
   return true;
 }
 
-void JointStateInterface::GetPosVel(std::vector<double> &pos_vecd,
-                                    std::vector<double> &vel_vecd)
+bool JointStateInterface::GetPosVel(std::vector<double> &pos_vecd,
+                                    std::vector<double> &vel_vecd,
+                                    double &time)
 {
+  if ((pos_vecd.size()!= last_state_.positions_.size()) ||
+      (vel_vecd.size()!= last_state_.velocities_.size()))
+      return false;
+
+  std::lock_guard(last_state_.lock_state_access_);
+  time=last_state_.time_sec_ + 1e-9*last_state_.time_nsec_;
   for(std::vector<double>::size_type idx=0;
       idx < pos_vecd.size();
       idx++)
   {
-    pos_vecd[idx] = positions_[idx];
-    vel_vecd[idx] = velocities_[idx];
+    pos_vecd[idx] = last_state_.positions_[idx];
+    vel_vecd[idx] = last_state_.velocities_[idx];
   }
+  if (debug_level_)
+    std::cerr << "JointStateInterface::GetPosVel: time: "
+              << time << " "
+              << pos_vecd[1] << " "
+              << std::endl;
+
+  return true;
 }
+
+
 };