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