error_recovery action server accepts goals when no error is present

[HumbertoE]

Context

System version: 5.2.1 libfranka version: 0.10.0 franka_ros version: 0.10.1 controller: position_joint_trajectory_controller

Problem

When a goal is sent to the error_recovery action server and no error is present in the robot, the goal is accepted, and when then an error happens (for example a cartesian reflex), it is confirmed almost immediately. This can be dangerous.

Steps to reproduce

1. Start all necessary nodes to run the robot including the franka_control node
2. With the robot not having any error, send a goal to the error_recovery action server with:

   rostopic pub /franka_control/error_recovery/goal franka_msgs/ErrorRecoveryActionGoal "header:
   seq: 0
   stamp:
   secs: 0
   nsecs: 0
   frame_id: ''
   goal_id:
   stamp:
   secs: 0
   nsecs: 0
   id: ''
   goal: {}"

3. Produce an error in the robot. We commanded some movement to the robot and produced a cartesian reflex by perturbing the movement of the robot.
4. In the terminal where the franka_control node was launched, checked the timestamps between the logs: libfranka: Move command aborted: motion aborted by reflex! ["cartesian_reflex"] and Recovered from error

The simplest way to reproduce the error we think is the one described above, but the error can also be reproduced sending the goals from a script in python.

[Maverobot]

@HumbertoE I create a PR for fixing this issue. Feel free to take a look at it or try it out.

[HumbertoE]

Thank you. We will and will report back if it fixes the error for us

[Maverobot]

@HumbertoE We merged the PR because locally we can confirm that it fixed the issue. If you still have the issue, feel free to reopen this.

[HumbertoE]

Sorry for taking long to check this solution.

I repeated the experiment with the latest franka ros version (358406952597c3735d43625595650901be84a5d8) and it worked well. Now a message is shown when an error recovery goal is sent without an active error.

Thank you for the support.