lidar_auto_docking

The lidar_auto_docking package is a heavily refactored version of the fetch_open_auto_dock package. The package would send appropriate cmd_vel messages to bring the robot to a dock with a trapezium-shaped landmark. The distance between dock and base_link is used to determine if the robot has reached the dock.
There are two versions of this package. ROS2 Foxy on the (foxy-main) branch, ROS2 Humble on the (humble-main) branch. Do take note that the humble version of this package requires a separate package for messages, whereas the foxy version does not.

Prerequisites

This package would require the nav2 stack to be set-up on your robot.

This package requires a trapezium-shaped landmark with the following dimensions:

The front face of the trapezium is 300mm long
Each side of the trapezium is 100mm long at a 45 degree angle

You can create this trapezium out of any material you desire. Of course, the trapezium should be placed in the scan plane of the lidar.

Subscribed Topics

/scan (laserscan topic)
/tf (transformation from map->odom->base_link)
/joy (joystick topic. Used to reset the chosen dock by the dock saver program)

Published Topics

/autodock/cmd_vel (topic used to send velocity commands to the robot.)

Installation

git clone the following messages package into your src folder:

git clone https://github.com/SynapseProgramming/lidar_auto_docking_messages.git

Next, git clone this package into your colcon workspace and then run colcon build. If there are some dependencies that are not met, please run rosdep before building.

Usage

Firstly, run nav2 on your robot. Place the robot about 1 metre in front of the dock.

Next, open a new terminal and launch:

ros2 launch lidar_auto_docking dockpose_saver_launch.py

Once the robot has detected a suitable dock candidate, the dock frame would appear in rviz2.

Furthermore, a GUI window would pop up. The GUI would display the absolute distance(in metres) between base_link and the dock frame. You may wish to use this tool to decide a suitable distance for the robot to stop at the dock.

Once you are satisfied with the selected dock, click on the save dock and bot pose button. This button would save the dock's position in the map frame. Also, the transformation between base_link and map would be saved.

Next, please modify the config/autodock_params.yaml file and key in suitable parameters. Please run colcon build once suitable parameters have been keyed in.

Docking

please position the robot about 1-1.5m in front of the dock and run the following command to send the robot to the dock.

ros2 launch lidar_auto_docking dockrobot_launch.py

The robot should begin to move towards the dock. The robot would stop once the distance threshold(docked_distance_threshold) between dock and base_link has been reached.

Undocking

To undock the robot, please run the following command:

ros2 launch lidar_auto_docking undock_robot_launch.py

The robot would undock from the dock, and it should turn 180 degrees.

Testing in simulation

Before testing docking on real hardware, I strongly recommend testing this package in simulation first(gazebo etc.)

Firstly, place the Dock.stl file in the same folder as your .world file.
Next, you may wish to copy and paste this code and place it in the world tag. To add the dock into your world.

<model name="dock">
                <!-- x y z r p y-->
                    <pose>-1.0 -0.35 0 1.57 0 1.57</pose>
                    <static>true</static>
                    <link name="body">
                      <visual name="visual">
                        <geometry>
                          <mesh><uri>file://Dock.stl</uri>
                          <scale>0.001 0.001 0.001</scale>
                        </mesh>
                        </geometry>
                      </visual>
                      <collision name="collision1">
                                <geometry>
                                  <mesh><uri>file://Dock.stl</uri>
                                   <scale>0.001 0.001 0.001</scale>
                                   </mesh>
                                </geometry>
                              </collision>

                    </link>
                  </model>

System Integration

As the main docking program is an action server, you could write action clients to customise where and when you want the robot to engage in the docking/undocking sequence.

If you wish, you could refer to dock_robot.py and undock_robot.py in the scripts folder as a reference.

SynapseProgramming / lidar_auto_docking