Merge pull request #137 in SWDEV/franka_ros from fix_combined_hw_reco…

…very_behavior to develop

* commit '6172dcfde9dc8ff9cc9a947d2064d947a35cb047':
  fix typo in doc
  cmake version 3.1.3 in meta package franka_ros for melodic and noetic support
  make recovery print debug
  reset command buffer on recover, keep updating while in reflex
  remove redundant . in print
  revert cmake version update in meta package to fix build issue with catkin_make
  only execute controller update when not in error state

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;

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<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>>
       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};
 };

franka_hw/src/franka_combinable_hw.cpp

@@ -86,8 +86,10 @@ void FrankaCombinableHW::controlLoop() {
 
     // Reset commands
     {
-      std::lock_guard<std::mutex> command_lock(libfranka_cmd_mutex_);
+      std::lock_guard<std::mutex> libfranka_command_lock(libfranka_cmd_mutex_);
+      std::lock_guard<std::mutex> 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 {

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()