- TurtleBot with RRT* path planning algorithm
mkdir -p ~/turtlebot_ws/src/ && cd ~/turtlebot_ws/src/
git clone https://github.com/mmcza/TurtleBot-RRT-Star
sudo apt update
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup
sudo apt install ros-humble-turtlebot3-gazebo
mkdir -p ~/turtlebot_ws/src/ && cd ~/turtlebot_ws/src/
git clone https://github.com/mmcza/TurtleBot-RRT-Star
cd TurtleBot-RRT-Star && docker build -t rrt_star_tb3 .
bash start_container.sh
Note
The turtlebot_ws directory is shared between the host and container. In the result files inside of it might require sudo privileges to save any changes.
Note
Dockerfile and script for running container are based on Rafał Staszak's repository
cp /path/to/the/repository/TurtleBot-RRT-Star/nav2_params.yaml /opt/ros/humble/share/nav2_bringup/params/nav2_params.yaml
Example: in Docker it is:
cp ~/Shared/src/TurtleBot-RRT-Star/nav2_params.yaml /opt/ros/humble/share/nav2_bringup/params/nav2_params.yaml
source /opt/ros/humble/setup.bash
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
source /usr/share/gazebo/setup.bash
By default the path planner runs once every second and because RRT* is based on random points the path may differ (with number of points equal navigate_to_pose_w_replanning_and_recovery.xml. Below is an example that disable (by setting rate = 0.0) the periodic replanner but the path may be replanned if robot get stuck.
sed -i '11s|.*| <RateController hz="0.0">|' /opt/ros/humble/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
When inside the workspace:
colcon build --symlink-install
source /opt/ros/humble/setup.bash
source install/setup.bash
To start the simulation you need to execute the following command
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False params_file:=/opt/ros/humble/share/nav2_bringup/params/nav2_params.yaml
It may take some time to start the Gazebo (if it's first time running than even more) and once it's working you have to check in the Gazebo where on the map the robot is located and in RVIZ click 2D Pose Estimate button and later place it in the place of the robot (adjust the arrow to show the orientation of the robot). After that click Nav2 Goal and once again click on the map (you can adjust the end orientation with the arrow). After that the robot should start moving (and in RVIZ you should see the planned path).
The difference between RRT* and default RRT Algorithm is the fact that RRT* takes cost of the robot moving from one node to another and that it can rewire the nodes when new one appears and cost of going through it is lower than through an existing one. Also basic RRT algorithm stops when there is a path between a node and the end point while RRT* continue for certain number of iterations. As a result RRT* algorithm is asymptotically optimal (with number of nodes going towards
Pseudocode for RRT* is shown below [1].
Example of RRT Star in action can be seen below (code available here) - however the change of radius wasn't implemented and there was added a constant step distance between newly added vertex.
- initalizes the RRTStar planner by setting up necessary parameters, such as the interpolation resolution and costmap.
- calculateBallEadiusConstant computes a constant used for determining the ball radius during the rewire step, based on the free space in the costmap.
- calculateBallRadius calculates the actual ball radius used to find nearby vertices during the rewire step. The radius is determined based on the size of the tree and ensures that it does not exceed a specified maximum connection distance.
- vertices_inside_circle the function returns a vector of indices representing the vertices that lie within the specified radius from the center point. This is done by iteratively following the parent pointers from the given vertex back to the start vertex.
- calculate_distance this function calculates the Euclidean distance between a given point and a vertex in the tree.
- nearest_neighbor this function finds the nearest vertex in the tree to a given point.
- connectible checks whether a straight-line path between two vertices is free of obstacles. It determines the number of interpolation steps based on the distance and resolution, and then incrementally checks each point for obstacles in the costmap.
- calculate_cost_from_start computes the total cost from the start vertex to a given vertex by summing the costs along the path.
- createPlan function generates a path from the start pose to the goal pose using the RRT* algorithm.
- in the main loop of createPlan function, the algorithm generates a random point within the costmap and creates a new vertex. It finds the nearest existing vertex in the tree, check if it's possible to connect them with a straight line and assigns it as the parent of the new vertex and calculates the cost to reach it. If the connection is possible, it calculates a ball radius to find nearby vertices, adds the new vertex to the tree, and calculates its total cost from the start. The algorithm then attempts to rewire the tree by finding more efficient paths through the new vertex to existing vertices or through existing vertices to the vertex. If the tree is rewired it's updating parent pointers and costs accordingly. If the path is obstructed, the new vertex is discarded, and the iterations is retired.
If there is a total of 1000 vertices in the tree, the algorithm terminates the loop and proceeds to find the optimal path to the goal using the vertices that are in the tree by recursively going through parents of the vertices all the way to the start point. This ensures that the algorithm does not run indefinitely and provides a solution within a reasonable amount of time.
The issue might be caused by a corrupted package or by not sourcing the gazebo.
Solution might be using the following command
source /usr/share/gazebo/setup.bash
or using a Docker container with clean system.
This issue (caused by wrong timestamps) made that it was impossible to set initial pose of the robot (and as a result to plan a path).
Example of the error message:
[component_container_isolated-6] [WARN] [1715109253.077329208] [amcl]: Failed to transform initial pose in time (Lookup would require extrapolation into the future. Requested time 1715109253.077081 but the latest data is at time 70.142000, when looking up transform from frame [odom] to frame [base_footprint])
Solution was (as mentioned in this comment) to comment in the cpp file the lines that captured time.
The issue was that pointers were pointing address in a vector that could be relocated.
The solution was to reserve the memory.
The problem was caused by the fact that next pose in the path had to be in certain radius of the robot so it can go towards it.
The solution was to add more waypoints between each two poses.
One of the possibilities is to experiment with the radius of the circle, because when the value is too low and there are many obstacles than the algorithm may return a non optimal path.
Other potential upgrade would be to use RRT* Informed algorithm and limit the space where new vertices are being added after the initial path is found.
The structure of the tree could also be improved to make searching for vertices inside the circle faster. A good example would be to use a k-d tree.
[1] Karaman, Sertac and Frazzoli, Emilio. Sampling-based Algorithms for Optimal Motion Planning. 2011, https://doi.org/10.48550/arXiv.1105.1186