Is Abort service working?

[MFernandezCarmona]

Hi @FedericoPecora It seems that Abort service is not working as I thought it would. Steps to reproduce:

• Start ijcai demo:

  rosrun iliad_launch_system start-ijcai-coordinator.sh  
  rosrun iliad_launch_system start-ijcai-robot4.sh       
  rosrun iliad_launch_system start-ijcai-robot5.sh     

• In RVIZ, give a goal to robot4, give a goal to robot5. I provide goals in the other room to have long trajectories:
• Call abort service from terminal: rosservice call /coordinator/abort "robotID: 4"

Result: robot4 keeps moving, but envelope seems to be cut until its position when abort was called.

I'm trying to use this service as part of a replan service. Replan basically calls abort service and computes a new task to the same goal from current position. (see https://github.com/mfernandezcarmona/coordination_oru_ros.git)

And merry Christmas

[MFernandezCarmona]

@ksatyaki can you have a look at this?

[FedericoPecora]

Dear Manuel,

so sorry for the late reply. The abort service triggers the truncation of a trajectory envelope by the coordinator. Truncating means: (1) compute the earlies dynamically-feasible path point; (2) truncate the trajectory and its envelope there; and (3) communicate the updated trajectory to the robot via a call to updateTrajecotryEnvelope.

Point (1) is done using the forward model of the robot - the same model that is used to ascertain, for instance, if a robot can stop before a critical section. In order for this estimation to be guaranteed correct, it makes conservative assumptions on the communication channel (that communication happens once per T_c, which is the period at which coordination happens). So this means that the earliest dynamically feasible path point at which to stop is the result of (a) the deceleration profile in the forward model, (b) assuming that the robot first continues at full speed for another T_c and then gets the updated trajectory at the next T_c. That is why you probably see a very long slowing down, sometimes even reaching the end.

I have checked, the updated trajectory is indeed forwarded to the vehicle exec node, because the TrajectoryEnvelopeTrackerROS (which implements the interface between coordinator and robots) implements the method "onTrajectoryEnvelopeUpdate(TrajectoryEnvelope te)". This is, by the way, the same mechanism used to replan (that is, when replanning, the current envelope is truncated as explained above and a new plan is computed from that point on).

So, in conclusion, if you want to reduce the time to stop when you send an abort, you could:

1. reduce the T_c, which will make the whole system more reactive

2. change the forward model to reflect the fact that deceleration can be done faster. But beware, this has to be a conservative estimate of the robot dynamics, otherwise, the system becomes unsafe.

3. AT YOUR OWN PERIL: you could modify the call to truncateEnvelope in the abort service implementation. Specifically, this currently calls the method "truncateEnvelope(int robotID)", which makes sure that envelopes are truncated at the earliest dynamically feasible point; there is as of yesterday another method in the current master branch of cooridnation_oru which lats you choose whether dynamics are considered. This method is "truncateEnvelope(int robotID, boolean ensureDynamicFeasibility)", where setting the boolean to false allows to truncate at the curren tpath point. You then have to also send an emergency brake command to the robot, so that it does indeed stop as soon as possible by slamming on the brakes.

Hope this helps, and sorry again for the delay!

Cheers, F.

[MFernandezCarmona]

Hi, No worries, a good answer is always better than a fast one! Let me explain a little better. My problem is not really the abort itself (but I´ll try what you mention here to speed things up) but how to tell the coordinator that it has to replan current task for a robot.

You mention a replan mechanism:

This is, by the way, the same mechanism used to replan (that is, when replanning, the current envelope is truncated as explained above and a new plan is computed from that point on).

Do you have that service working? Because I havent seen onTrajectoryEnvelopeUpdate used anywhere.

Thanks

[FedericoPecora]

Hi,

well, it's not exposed as a service, rather it is an internal mechanism of the coordinator by which replanning is called automatically if there is a deadlock. The coordinator knows to use a given motion planner for this prupose, which is set here.

A service that is provided is the updateTask service (see here). As you see, this service instructs the coordinator to replace the current path, as explained earlier.

Does this help?

Best, F.

[MFernandezCarmona]

Yes, I thought about using the updateTask service as a reference, but I didn't go on because I saw some problems with that:

My first idea for replanTask was to copy updateTask service, but instead of reading the new path from the request, call callComputeTaskService to get a new one from current position. But then I realized that the starting position was ambiguous, because the robot was still moving and changed approach.

I decided to go for AbortTask + callComputeTaskService to be sure about the robot starting position. And because this was probably stopping the robot, which is safer in any case.

Anyway, like you said before, changing TEMPORAL_RESOLUTION seems to be the simplest and safest solution. Regarding that, it has to be set at the beginning, as a rosparam, right?

I'll let you know how it goes. Cheers

[FedericoPecora]

This sounds reasonable, as the abort will indeed stop as early as dynamically feasible.

Note that by T_c I meant the control_period parameter, not the temporal_resolution parameter (which determines the unit of measure of the control_period). E.g., you can set temporal_resolution = 1000 for milliseconds (this is the default), and control_period = 500 to obtain an update frequency of the coordination loop of 2 Hz.

Cheers, F.

[MFernandezCarmona]

Hi, I've tried temporal_resolution = 500 and and control_period = 250 and still no changes: Trajectories are finished even after calling abort msgs. I have one theory about that. What happens if there is no critical point until the end of the trajectory? I think the robot just carries on ...

[MFernandezCarmona]

I'm moving the discussion a level down. I've detected that Vehicle execution node does not handle abort flag, so we need to address it there before.