def recover():
pub = rospy.Publisher('/franka_control/error_recovery/goal',
ErrorRecoveryActionGoal,
queue_size=10)
goal = ErrorRecoveryActionGoal()
goal.goal = {}
pub.publish(goal)
def check_rosout(log):
client = actionlib.SimpleActionClient("franka_control/error_recovery",
ErrorRecoveryAction)
client.wait_for_server()
goal = ErrorRecoveryActionGoal()
robot_state = log.level
goal.header.stamp.secs = 0
goal.header.stamp.nsecs = 0
goal.header.frame_id = ''
goal.goal_id.stamp.secs = 0
goal.goal_id.stamp.nsecs = 0
goal.goal_id.id = ''
goal.goal = {}
print 'Pandas Level: ', robot_state
if robot_state == 8:
print 'Sending state recover to our kind Panda!'
# 2s delay to wait for the robot state to be published as ERROR on the Logging and then send the Action
time.sleep(2)
client.send_goal(goal)
'''