Sim rejects undock after successful dock at end of explore

[slowrunner]

I performed repeated coverage server explore goals until the bot ended up docked.

Next I sent the sim an undock goal. The sim motion_control rejected the undock goal:

Coverage action resulted in docking:
...
[motion_control-13] [INFO] [1635806767.200327952] [motion_control]: Received new dock servo goal
[motion_control-13] [INFO] [1635806825.107624130] [motion_control]: Dock Servo Goal Succeeded

Sent undock goal:

[motion_control-13] [INFO] [1635809785.227185078] [motion_control]: Received new undock goal
[motion_control-13] [WARN] [1635809785.227501148] [motion_control]: Robot already undocked, reject

Tried again:

[motion_control-13] [INFO] [1635809941.818917710] [motion_control]: Received new undock goal
[motion_control-13] [WARN] [1635809941.819046534] [motion_control]: Robot already undocked, reject

[slowrunner]

This is really a pain to repro because coverage goal does not see the dock very often, but I confirmed this issue is happening.

Method:

• Repeated short max_duration explores till one finished with is_docked: true.
• Send /undock goal to bot

move_short_explore.sh issues:

ros2 action send_goal /coverage create3_examples_msgs/action/Coverage "{explore_duration:{sec: 60, nanosec: 0}, max_duration:{sec: 120, nanosec: 0}}"

This is the move_short that docked:

$ ./move_short_explore.sh 
1635860055.630689 [0]       ros2: using network interface wlan0 (udp/10.0.0.29) selected arbitrarily from: wlan0, wlan1
Waiting for an action server to become available...
Sending goal:
     explore_duration:
  sec: 60
  nanosec: 0
max_duration:
  sec: 120
  nanosec: 0

Goal accepted with ID: 1daf22925e72413c9f925bf871662b35

Result:
    success: true
is_docked: true
duration:
  sec: 159
  nanosec: 333841204

Goal finished with status: SUCCEEDED

Coverage Server logged:

[INFO] [1635860056.037624343] [create3_coverage]: Accepting goal request
...
[INFO] [1635860135.540674044] [create3_coverage]: Rotation completed: from -0.666271 to 2.469646
[INFO] [1635860135.574859781] [create3_coverage]: Sending docking goal!
[INFO] [1635860215.308334524] [create3_coverage]: Docking succeeded!
[INFO] [1635860215.373907229] [create3_coverage]: Coverage action terminated

Sim logged:

[motion_control-13] [INFO] [1635860135.578554033] [motion_control]: Received new dock servo goal
[motion_control-13] [INFO] [1635860215.297843477] [motion_control]: Dock Servo Goal Succeeded

undock_it.sh sends:

ros2 action send_goal /undock irobot_create_msgs/action/Undock "{ }"

Sending /undock goals will fail, even though the last docking reported success:

$ ./undock_it.sh 
1635860240.354535 [0]       ros2: using network interface wlan0 (udp/10.0.0.29) selected arbitrarily from: wlan0, wlan1
Waiting for an action server to become available...
Sending goal:
     {}

Goal was rejected.
ubuntu@CREATE3RPI:~/create3ros/create3_examples_ws$ ./undock_it.sh 
1635860285.128759 [0]       ros2: using network interface wlan0 (udp/10.0.0.29) selected arbitrarily from: wlan0, wlan1
Waiting for an action server to become available...
Sending goal:
     {}

Goal was rejected.

Sim reports:

[motion_control-13] [INFO] [1635860240.803464673] [motion_control]: Received new undock goal
[motion_control-13] [WARN] [1635860240.803599514] [motion_control]: Robot already undocked, reject
[motion_control-13] [INFO] [1635860285.576984825] [motion_control]: Received new undock goal
[motion_control-13] [WARN] [1635860285.577073084] [motion_control]: Robot already undocked, reject

Note: Sending an explore goal will undock and execute properly. Performing the dance example will also undock and execute properly so the impact of this issue could be considered low.

[justinIRBT]

Next time you get into this state, can you try ros2 topic echo /dock To see if the robot is reporting is_docked = true

[slowrunner]

Indeed successful docking sometime causes /dock to publish is_docked: true, sometimes false.

I don't know how to reliably repro the does not publish is_docked: true case.

Closing this issue.

[alsora]

I think that we should keep this issue open, even if it's not reliably reproducible it appears to be a valid problem.

The problem may be that:

• the is_docked simulated topic is unreliable
• the simulated dock action terminates too early, assuming that it's already on the dock while it is not true