diff --git a/src/PandaUtil.cpp b/src/PandaUtil.cpp index bf4a11a472f0db1154451c33f1a9c325233fb51f..e211de8c77f6b5fe8bf382a9c09d4064e0d1f3d3 100644 --- a/src/PandaUtil.cpp +++ b/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()); + 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; }