diff --git a/franka_control/src/franka_combined_control_node.cpp b/franka_control/src/franka_combined_control_node.cpp index dfcb9ba4f..375eedcda 100644 --- a/franka_control/src/franka_combined_control_node.cpp +++ b/franka_control/src/franka_combined_control_node.cpp @@ -39,7 +39,13 @@ int main(int argc, char** argv) { ros::Time now = ros::Time::now(); franka_control.read(now, period); cm.update(now, period, franka_control.controllerNeedsReset()); - franka_control.write(now, period); + if (!franka_control.hasError()) { + franka_control.write(now, period); + } else { + ROS_DEBUG_THROTTLE(5, + "franka_combined_control_node: The HW is in error state." + "To recover, call the recovery action."); + } } return 0; diff --git a/franka_hw/include/franka_hw/franka_combined_hw.h b/franka_hw/include/franka_hw/franka_combined_hw.h index b3f404257..a0f7112f1 100644 --- a/franka_hw/include/franka_hw/franka_combined_hw.h +++ b/franka_hw/include/franka_hw/franka_combined_hw.h @@ -60,6 +60,12 @@ class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW { */ bool disconnect(); + /** + * Checks whether the robots are in error or reflex mode. + * @return true if in error state, false otherwise. + */ + bool hasError(); + protected: std::unique_ptr> combined_recovery_action_server_; @@ -68,7 +74,6 @@ class FrankaCombinedHW : public combined_robot_hw::CombinedRobotHW { private: void handleError(); - bool hasError(); void triggerError(); bool is_recovering_{false}; }; diff --git a/franka_hw/src/franka_combinable_hw.cpp b/franka_hw/src/franka_combinable_hw.cpp index 03c56018e..603078209 100644 --- a/franka_hw/src/franka_combinable_hw.cpp +++ b/franka_hw/src/franka_combinable_hw.cpp @@ -86,8 +86,10 @@ void FrankaCombinableHW::controlLoop() { // Reset commands { - std::lock_guard command_lock(libfranka_cmd_mutex_); + std::lock_guard libfranka_command_lock(libfranka_cmd_mutex_); + std::lock_guard ros_command_lock(ros_cmd_mutex_); effort_joint_command_libfranka_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.}); + effort_joint_command_ros_ = franka::Torques({0., 0., 0., 0., 0., 0., 0.}); } try { diff --git a/franka_ros/CMakeLists.txt b/franka_ros/CMakeLists.txt index 680baafd5..3bbd61f39 100644 --- a/franka_ros/CMakeLists.txt +++ b/franka_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.4) +cmake_minimum_required(VERSION 3.1.3) project(franka_ros) find_package(catkin REQUIRED) catkin_metapackage()