From 5bc0683aceec4e06daae74bc97df116222dd8495 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20J=C3=A4hne?= <christoph.jaehne@franka.de>
Date: Thu, 9 Jan 2020 09:40:29 +0100
Subject: [PATCH] use lock_guards for command and state acess

---
 .../include/franka_hw/franka_combinable_hw.h  | 11 +++++-----
 franka_hw/src/franka_combinable_hw.cpp        | 20 ++++++++++---------
 2 files changed, 17 insertions(+), 14 deletions(-)

diff --git a/franka_hw/include/franka_hw/franka_combinable_hw.h b/franka_hw/include/franka_hw/franka_combinable_hw.h
index 75605c3..0c8ff36 100644
--- a/franka_hw/include/franka_hw/franka_combinable_hw.h
+++ b/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);
     }
diff --git a/franka_hw/src/franka_combinable_hw.cpp b/franka_hw/src/franka_combinable_hw.cpp
index e172327..665df3f 100644
--- a/franka_hw/src/franka_combinable_hw.cpp
+++ b/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();
-- 
GitLab