Make libfrabka work after reactivate button

[arnauddomes]

Hello,

I try to find a way to make moveit work after pressing the user mode button.

I get that log when I press the mode button :

[ WARN] [1649941682.505520841]: FrankaHW:
	panda_joint2: 53.374586 degrees to joint limits (limits: [-1.762800, 1.762800] q: 0.831238)
	panda_joint4: 56.967684 degrees to joint limits (limits: [-3.071800, -0.069800] q: -2.077526)
[ WARN] [1649941687.506378752]: FrankaHW:
	panda_joint2: 53.374697 degrees to joint limits (limits: [-1.762800, 1.762800] q: 0.831236)
	panda_joint4: 56.967684 degrees to joint limits (limits: [-3.071800, -0.069800] q: -2.077526)
[ERROR] [1649941689.010903952]: libfranka: Move command aborted: User Stop pressed!
[ INFO] [1649941689.010939682]: franka_control, main loop
[ WARN] [1649941692.507226839]: FrankaHW:
	panda_joint2: 39.557334 degrees to joint limits (limits: [-1.762800, 1.762800] q: 1.072394)
[ WARN] [1649941697.508178959]: FrankaHW:
	panda_joint2: 39.556824 degrees to joint limits (limits: [-1.762800, 1.762800] q: 1.072403)
[ WARN] [1649941702.508989287]: FrankaHW:
	panda_joint2: 39.556632 degrees to joint limits (limits: [-1.762800, 1.762800] q: 1.072407)

After this, I have to restart my roslaunch file after unlock the button to be able to use moveit planner again.

Is there a way to avoid restarting the launch file ?

[gollth]

When you press the enabling device/user stop the robot will switch from execution mode (blue) -> guiding mode (white) in which your control is paused. What you can try is:

1. Start your launch file
2. Press enabling device/user stop so the goes in the guiding mode. Optionally guide the robot away
3. Release the enabling device/user stop so the robot is again in execution mode.
4. rostopic pub --once /franka_control/error_recovery/goal franka_msgs/ErrorRecoveryActionGoal {}

Let me know if this solves your problem

[arnauddomes]

Thank you @gollth It works properly.
How can I catch the enabling/user device press button?

[gollth]

This information is published in the FrankaStates:

$ rosmsg show franka_msgs/FrankaState | grep MODE
uint8 ROBOT_MODE_OTHER=0
uint8 ROBOT_MODE_IDLE=1
uint8 ROBOT_MODE_MOVE=2
uint8 ROBOT_MODE_GUIDING=3
uint8 ROBOT_MODE_REFLEX=4
uint8 ROBOT_MODE_USER_STOPPED=5
uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6

You can query the /franka_state_controller/franka_states#robot_mode field:

$ rostopic echo /franka_state_controller/franka_states | grep robot_mode
robot_mode: 1
robot_mode: 1
robot_mode: 1
robot_mode: 1
robot_mode: 1
robot_mode: 5   # <-- here the enabling device pressed
robot_mode: 5
robot_mode: 5

... and react accordingly.

[arnauddomes]

Great, thank you.