do not recover if there are no errors

src/PandaUtil.cpp

@@ -5,21 +5,68 @@
 #include "../include/PandaUtil.h"
 
 #include "franka_msgs/ErrorRecoveryAction.h"
+#include "franka_msgs/FrankaState.h"
+#include "franka_msgs/Errors.h"
 #include <actionlib/client/simple_action_client.h>
 
-bool PandaUtil::recoverFromErrors() {
+bool PandaUtil::recoverFromErrors()
+{
+  ros::NodeHandle n;
+  bool hasErrors;
+  bool receivedState = false;
+  ros::Subscriber state_subscription = n.subscribe<franka_msgs::FrankaState>(
+      "/franka_state_controller/franka_states", 1000, [&receivedState, &hasErrors, &state_subscription](auto& s) {
+        receivedState = true;
+        uint8_t robot_mode = s->robot_mode;
+        hasErrors = robot_mode == franka_msgs::FrankaState::ROBOT_MODE_OTHER ||
+                    robot_mode == franka_msgs::FrankaState::ROBOT_MODE_REFLEX;
+        state_subscription.shutdown();
+      });
+
+  for (int maxSpins = 10; !receivedState && maxSpins && !state_subscription.getNumPublishers(); maxSpins--)
+  {
+    ros::Duration(.5).sleep();
+  }
+
+  if (!receivedState && !state_subscription.getNumPublishers())
+  {
+    ROS_WARN_STREAM("No robot publishes a state. Aborting error recovery.");
+    return false;
+  }
+
+  for (int maxSpins = 10; maxSpins && !receivedState; maxSpins--)
+  {
+    ros::Duration(.05).sleep();
+    ros::spinOnce();
+  }
+  if (!receivedState)
+  {
+    ROS_WARN_STREAM("Unable to obtain state from robot. Aborting error recovery.");
+    return false;
+  }
+
+  if (!hasErrors)
+  {
+    ROS_WARN_STREAM("Robot has no errors. Skipping error recovery.");
+    return false;
+  }
+
   actionlib::SimpleActionClient<franka_msgs::ErrorRecoveryAction> ac("/franka_control/error_recovery", true);
-  if (!ac.waitForServer(ros::Duration(10.0))) {
+  if (!ac.waitForServer(ros::Duration(10.0)))
+  {
     ROS_ERROR("Panda error recovery failed (action server not available).");
     return false;
   }
   franka_msgs::ErrorRecoveryGoal goal;
   ac.sendGoal(goal);
-  if (ac.waitForResult(ros::Duration(5.0))) {
+  if (ac.waitForResult(ros::Duration(5.0)))
+  {
     actionlib::SimpleClientGoalState state = ac.getState();
     ROS_INFO_STREAM("Panda error recovery finished: " << state.toString());
     return state == actionlib::SimpleClientGoalState::SUCCEEDED;
-  } else {
+  }
+  else
+  {
     ROS_ERROR("Panda error recovery failed (with timeout).");
     return false;
   }