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