C3 Undock Action Never Quit?

[slowrunner]

Please provide the following information:

• OS: Ubuntu 20.04
• ROS Distro: Galactic - apt installed

• Package version:

  $ dpkg -l | grep turtlebot
  ii  ros-galactic-turtlebot4-description                 0.1.2-1focal.20220916.170821          amd64        Turtlebot4 Description package
  ii  ros-galactic-turtlebot4-desktop                     0.1.0-1focal.20220916.172304          amd64        Turtlebot4 Desktop Metapackage
  ii  ros-galactic-turtlebot4-ignition-bringup            0.1.1-1focal.20221016.143037          amd64        TurtleBot 4 Ignition Simulator bringup
  ii  ros-galactic-turtlebot4-ignition-gui-plugins        0.1.1-1focal.20220509.183119          amd64        Turtlebot4 Ignition Simulator GUI Plugins
  ii  ros-galactic-turtlebot4-ignition-toolbox            0.1.1-1focal.20220916.171656          amd64        Turtlebot4 Ignition Toolbox
  ii  ros-galactic-turtlebot4-msgs                        0.1.2-1focal.20220916.170727          amd64        Turtlebot4 Messages
  ii  ros-galactic-turtlebot4-navigation                  0.1.2-1focal.20221016.142546          amd64        Turtlebot4 Navigation
  ii  ros-galactic-turtlebot4-node                        0.1.2-1focal.20220916.172120          amd64        Turtlebot4 Node
  ii  ros-galactic-turtlebot4-simulator                   0.1.1-1focal.20221016.143508          amd64        TODO: Package description
  ii  ros-galactic-turtlebot4-viz                         0.1.0-1focal.20220916.171830          amd64        Visualization launchers and helpers for Turtlebot4
  

  Expected behaviour With TB4lite on dock, /undock action should complete normally, and allow /dock action next.

  Actual behaviour Undock action request did not complete, blocking any further dock or undock requests:

  [motion_control-20] [INFO] [1668451075.748019655] [motion_control]: Received new dock servo goal
  [motion_control-20] [WARN] [1668451075.748066533] [motion_control]: Robot already docked, reject
  [motion_control-20] [INFO] [1668451090.540853326] [motion_control]: Received new undock goal
  [motion_control-20] [WARN] [1668451121.555487868] [motion_control]: Reached backup limit! Stop Driving robot backward or disable from safety_override parameter!
  [motion_control-20] [INFO] [1668451143.828225832] [motion_control]: Received new dock servo goal
  [motion_control-20] [WARN] [1668451143.828283231] [motion_control]: A docking behavior is already running, reject
  [motion_control-20] [WARN] [1668451400.645608257] [motion_control]: Reflex Exceeded Runtime without clearing hazard
  [motion_control-20] [INFO] [1668453511.526804943] [motion_control]: Received new dock servo goal
  [motion_control-20] [WARN] [1668453511.526852128] [motion_control]: A docking behavior is already running, reject

To Reproduce Provide the steps to reproduce:

1. ros2 launch turtlebot4_ignition_bringup ignition.launch.py model:=lite
2. ros2 action send_goal /undock irobot_create_msgs/action/Undock "{}"

   
   [motion_control-20] [INFO] [1668450851.359360079] [motion_control]: Received new undock goal
   [motion_control-20] [WARN] [1668450851.367452845] [motion_control]: Robot already undocked, reject

3. ros2 action send_goal /dock irobot_create_msgs/action/DockServo "{}"

[motion_control-20] [INFO] [1668451075.748019655] [motion_control]: Received new dock servo goal [motion_control-20] [WARN] [1668451075.748066533] [motion_control]: Robot already docked, reject

4. ros2 action send_goal /undock irobot_create_msgs/action/Undock "{}"
**(TB4lite backed off dock - now undocked but threw warning)**

[motion_control-20] [INFO] [1668451090.540853326] [motion_control]: Received new undock goal [motion_control-20] [WARN] [1668451121.555487868] [motion_control]: Reached backup limit! Stop Driving robot backward or disable from safety_override parameter!

5. ros2 action send_goal /dock irobot_create_msgs/action/DockServo "{}"

[motion_control-20] [INFO] [1668451143.828225832] [motion_control]: Received new dock servo goal [motion_control-20] [WARN] [1668451143.828283231] [motion_control]: A docking behavior is already running, reject

All further dock or undock action requests will be rejected - this was about 1 hr later:

[motion_control-20] [INFO] [1668454646.997781504] [motion_control]: Received new dock servo goal [motion_control-20] [WARN] [1668454646.997822201] [motion_control]: A docking behavior is already running, reject

**Other notes**

[slowrunner]

Closing: Could not reproduce when starting sim with "mymaze" world (where bot and dock are not half embeded in obstacle). Issuing the /undock action goal after the sim is running, resulted in a successful undock and result msg:

[motion_control-20] [INFO] [1668635439.087642126] [motion_control]: Received new undock goal [motion_control-20] [INFO] [1668635508.194021526] [motion_control]: Undock Goal Succeeded

ubuntu@tb4desk:~/tb4desk$ ./c3_action_undock.sh

Send Create3 Undock Action ros2 action send_goal /undock irobot_create_msgs/action/Undock "{}" Waiting for an action server to become available... Sending goal: {}

Goal accepted with ID: 3ad77d5b2b624a6d8f26f771a5fe2765

Result: is_docked: false

Goal finished with status: SUCCEEDED