// // Created by Johannes Mey on 12/10/21. // #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() { 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))) { 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))) { actionlib::SimpleClientGoalState state = ac.getState(); ROS_INFO_STREAM("Panda error recovery finished: " << state.toString()); return state == actionlib::SimpleClientGoalState::SUCCEEDED; } else { ROS_ERROR("Panda error recovery failed (with timeout)."); return false; } }