search

PilzDE / pilz_robots

PILZ robot manipulator module PRBT 6 in ROS
https://wiki.ros.org/pilz_robots
52 stars 25 forks source link

Hold service waits until recover returns #151

Closed jschleicher closed 5 years ago

jschleicher commented 5 years ago

Commit

836eeb30d93de4bd873a61373753bf4e61c163ab

Steps to reproduce

How can the issue/bug be reproduced?

  1. roslaunch prbt_moveit_config moveit_planning_execution.launch sim:=false has_braketest_support:=true has_operation_mode_support:=true pipeline:=pilz_command_planner
  2. Press and immediately release enabling switch

Expected behavior

Hold service is called as soon as it is detected via modbus.

Observed behavior

Call of /hold and subsequent /halt service is delayed by more than 50ms.

Logfiles

[DEBUG] [1561374871.068122937]: Sending new ROS-message.
[DEBUG] [1561374871.069153026]: Calling Unhold (Service: /prbt/manipulator_joint_trajectory_controller/unhold)
[DEBUG] [1561374871.070522761]: Calling Recover (Service: /prbt/driver/recover)
[ INFO] [1561374871.071264156]: Recovering XXX
[DEBUG] [1561374871.088116191]: Sending new ROS-message.
EMCY: 83#0000000000000000
EMCY: 84#0000000000000000
EMCY: 85#0000000000000000
EMCY: 86#0000000000000000
EMCY: 87#0000000000000000
EMCY: 88#0000000000000000
[ INFO] [1561374871.156928772]: Performing STOP due to STO signal
[DEBUG] [1561374871.156962179]: Calling Hold on controller (Service: /prbt/manipulator_joint_trajectory_controller/hold)
[DEBUG] [1561374871.358027119]: Calling Halt on driver (Service: /prbt/driver/halt)
[ INFO] [1561374871.359182521]: Halting down XXX

The delay between "Sending new ROS message" and "Performing STOP" is about 68 milliseconds in this log.

The delay triggers bug #93.

Probably the reason is the blocking /recover call. Is it possible to abort that or overrule it by a multithreaded /halt call?

jschleicher commented 5 years ago

For comparison: A slow normal run gives the following log:

  1. press enabling switch (for some seconds)
  2. release enabling switch
[DEBUG] [1561375790.240632492]: Calling Unhold (Service: /prbt/manipulator_joint_trajectory_controller/unhold)
[DEBUG] [1561375790.242061900]: Calling Recover (Service: /prbt/driver/recover)
[ INFO] [1561375790.242838459]: Recovering XXX
EMCY: 83#0000000000000000
EMCY: 84#0000000000000000
EMCY: 85#0000000000000000
EMCY: 86#0000000000000000
EMCY: 87#0000000000000000
EMCY: 88#0000000000000000
[DEBUG] [1561375793.049824326]: Sending new ROS-message.
Warning: recursive print statement has occurred.  Throwing out recursive print.
[ INFO] [1561375793.050019671]: Performing STOP due to STO signal
[DEBUG] [1561375793.050061011]: Calling Hold on controller (Service: /prbt/manipulator_joint_trajectory_controller/hold)
[DEBUG] [1561375793.251132146]: Calling Halt on driver (Service: /prbt/driver/halt)
[ INFO] [1561375793.252249256]: Halting down XXX

Note: The /hold service call happens < 0.5ms after "Sending new ROS-message".

martiniil commented 5 years ago

@jschleicher Thank you for this insight. We'll have to do some research. I don't know if a /recover call can be aborted.

martiniil commented 5 years ago

The recover and halt callbacks are mutually exclusive ...

jschleicher commented 5 years ago

The recover and halt callbacks are mutually exclusive ...

What are you referring to? Could you add a link? Should we open an issue in upstream ros_canopen?

martiniil commented 5 years ago

https://github.com/ros-industrial/ros_canopen/blob/67172327d83e6308172b09d21916282cb23b99f1/canopen_chain_node/src/ros_chain.cpp#L131 and following

I don't have a deep understanding of the processes in ros_canopen. At first glance it seems that this callback mainly sets a state in the canopen layer. If this is true I think a solution could be found upstream.

jschleicher commented 5 years ago

As other approach: Skip /hold service call, if bRunpermitted already goes to false during /recover.

martiniil commented 5 years ago

WIP: https://github.com/PilzDE/pilz_robots/tree/fix/delayed_sto

martiniil commented 5 years ago

Is it also valid to skip the /halt service call if Runpermitted goes to true during /hold?

agutenkunst commented 5 years ago

So to recap this happens with: 101 Build 11345 Date:2018-12-17 07:33:065

agutenkunst commented 5 years ago

@jschleicher @martiniil I could really need a statement on the used hardware / software running on this hardware in order to reproduce...