use lock_guards for command and state acess

franka_hw/include/franka_hw/franka_combinable_hw.h

@@ -131,12 +131,13 @@ class FrankaCombinableHW : public FrankaHW {
       throw std::invalid_argument(error_message);
     }
     checkJointLimits();
-    libfranka_state_mutex_.lock();
-    robot_state_libfranka_ = robot_state;
-    libfranka_state_mutex_.unlock();
-    libfranka_cmd_mutex_.lock();
+    {
+      std::lock_guard<std::mutex> state_lock(libfranka_state_mutex_);
+      robot_state_libfranka_ = robot_state;
+    }
+
+    std::lock_guard<std::mutex> command_lock(libfranka_cmd_mutex_);
     T current_cmd = command;
-    libfranka_cmd_mutex_.unlock();
     if (has_error_ || !controller_active_) {
       return franka::MotionFinished(current_cmd);
     }

franka_hw/src/franka_combinable_hw.cpp

@@ -54,12 +54,13 @@ void FrankaCombinableHW::controlLoop() {
 
       checkJointLimits();
 
-      ros_state_mutex_.lock();
-      libfranka_state_mutex_.lock();
-      robot_state_libfranka_ = robot_->readOnce();
-      robot_state_ros_ = robot_->readOnce();
-      libfranka_state_mutex_.unlock();
-      ros_state_mutex_.unlock();
+      {
+        std::lock_guard<std::mutex> ros_state_lock(ros_state_mutex_);
+        std::lock_guard<std::mutex> libfranka_state_lock(libfranka_state_mutex_);
+        robot_state_libfranka_ = robot_->readOnce();
+        robot_state_ros_ = robot_->readOnce();
+      }
+
       if (!ros::ok()) {
         return;
       }
@@ -67,9 +68,10 @@ void FrankaCombinableHW::controlLoop() {
     ROS_INFO("FrankaCombinableHW::%s::control_loop(): controller is active.", arm_id_.c_str());
 
     // Reset commands
-    libfranka_cmd_mutex_.lock();
-    effort_joint_command_libfranka_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.});
-    libfranka_cmd_mutex_.unlock();
+    {
+      std::lock_guard<std::mutex> command_lock(libfranka_cmd_mutex_);
+      effort_joint_command_libfranka_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.});
+    }
 
     try {
       control();