Added Joint callback mutex

RethinkRobotics · May 10, 2017 · 6ed9e86 · 6ed9e86
1 parent 4db0c19
commit 6ed9e86
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 2 deletions.
diff --git a/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_position_controller.h b/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_position_controller.h
@@ -17,6 +17,8 @@
 #ifndef SAWYER_POSITION_CONTROLLER_H
 #define SAWYER_POSITION_CONTROLLER_H
 
+#include <mutex>
+
 #include <sawyer_sim_controllers/joint_group_position_controller.h>
 #include <intera_core_msgs/JointCommand.h>
 #include <ros/node_handle.h>
@@ -33,6 +35,8 @@ namespace sawyer_sim_controllers
     virtual bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n);
 
   private:
+    // mutex for re-entrant calls to modeCommandCallback
+    std::mutex mtx_;
     typedef std::unique_ptr<std::vector<Command>> CommandsPtr;
     ros::Subscriber sub_joint_command_;
     ros::Subscriber sub_speed_ratio_;

diff --git a/sawyer_sim_controllers/src/sawyer_position_controller.cpp b/sawyer_sim_controllers/src/sawyer_position_controller.cpp
@@ -65,8 +65,13 @@ namespace sawyer_sim_controllers {
     for (size_t i = 0; i < msg->names.size(); i++)
     {
       current_position[i] = position_controllers_[msg->names[i]]->joint_.getPosition();
-      delta_time[i] = std::abs(msg->position[i] - current_position[i]) /
-                   (position_controllers_[msg->names[i]]->joint_urdf_->limits->velocity);
+      if(position_controllers_[msg->names[i]]->joint_urdf_->limits->velocity > 0){
+        delta_time[i] = std::abs(msg->position[i] - current_position[i]) /
+                     (position_controllers_[msg->names[i]]->joint_urdf_->limits->velocity);
+      }
+      else {
+        delta_time[i] = 0.0;
+      }
       if (delta_time[i] > delta_time_max) {
         delta_time_max = delta_time[i];
       }
@@ -114,6 +119,8 @@ namespace sawyer_sim_controllers {
   }
 
   void SawyerPositionController::jointCommandCB(const intera_core_msgs::JointCommandConstPtr& msg) {
+    // lock out other thread(s) which are getting called back via ros.
+    std::lock_guard<std::mutex> guard(mtx_);
     CommandsPtr commands;
     // TODO: Verify commands are valid (names of joints are correct, within URDF limits, lengths of command fields are the same)
     if(msg->mode == intera_core_msgs::JointCommand::POSITION_MODE) {
@@ -122,6 +129,7 @@ namespace sawyer_sim_controllers {
     else if(msg->mode == intera_core_msgs::JointCommand::TRAJECTORY_MODE) {
       commands = cmdTrajectoryMode(msg);
     }
+    // Only write the command if this is the correct command mode, with a valid CommandPtr
     if(commands.get()) {
       position_command_buffer_.writeFromNonRT(*commands.get());
       new_command_ = true;