ROS Motion Planning

Motion planning is a computational problem to find a sequence of valid configurations that moves the object from the source to destination. Generally, it includes Path Searching and Trajectory Optimization.

• Path Searching: Based on path constraints, e.g. obstacles, it searches an optimal path sequence for the robot to travel without conflicts between source and destination.

• Trajectory Planning: Based on the kinematics, dynamics and obstacles, it optimizes a motion state trajectory from the source to destination according to the path sequence.

This repository provides the implementation of common Motion Planning algorithms. The theory analysis can be found at motion-planning. Furthermore, we provide Python and MATLAB version.

Your stars, forks and PRs are welcome!

Table of Contents

• Quick Start within 3 Minutes
• File Tree
• Dynamic Configuration
• Version
• Papers
• Application on a Real Robot
• Important Updates
• Acknowledgments
• License
• Maintenance

0. Quick Start within 3 Minutes

Tested on ubuntu 20.04 LTS with ROS Noetic.

1. Install ROS (Desktop-Full suggested).

2. Install git.

   sudo apt install git

3. Clone this reposity.

   cd <your_workspace>/
   git clone https://github.com/ai-winter/ros_motion_planning.git

4. Other dependence.

   sudo apt install python-is-python3
   ros-noetic-amcl \
   ros-noetic-base-local-planner \
   ros-noetic-map-server \
   ros-noetic-move-base \
   ros-noetic-navfn

5. Compile the code.

   cd ros_motion_planning/
   catkin_make

6. Run the scripts in ./src/sim_env/scripts/, i.e.

   cd ./src/sim_env/scripts/
   ./main.sh

   NOTE: Changing some launch files DOES NOT work, because some of them are re-generated according to the ./src/user_config/user_config.yaml by a python script when you run main.sh. Therefore, you should change configurations in user_config.yaml instead of launch files.

7. Use 2D Nav Goal to select the goal.

8. Moving!

1. File Tree

The file structure is shown below.

ros_motion_planner
├── assets
└── src
    ├── planner
    │   ├── global_planner
    │   ├── local_planner
    │   └── utils
    ├── sim_env             # simulation environment
    │   ├── config
    │   ├── launch
    │   ├── maps
    │   ├── meshes
    │   ├── models
    │   ├── rviz
    │   ├── scripts
    │   ├── urdf
    │   └── worlds
    ├── third_party
    │   ├── dynamic_rviz_config
    │   ├── dynamic_xml_config
    │   ├── gazebo_plugins
    │   └── rviz_plugins
    └── user_config         # user configure file

02. Dynamic Configuration

In this reposity, you can simply change configs through modifing the ./src/user_config/user_config.yaml. When you run ./main.sh, our python script will re-generated .launch, .world and so on, according to your configs in that file.

Below is an example of user_config.yaml

map: "warehouse"
world: "warehouse"
robots_config:
  - robot1_type: "turtlebot3_burger"
    robot1_global_planner: "astar"
    robot1_local_planner: "dwa"
    robot1_x_pos: "0.0"
    robot1_y_pos: "0.0"
    robot1_z_pos: "0.0"
    robot1_yaw: "-1.57"
  - robot2_type: "turtlebot3_burger"
    robot2_global_planner: "jps"
    robot2_local_planner: "pid"
    robot2_x_pos: "-5.0"
    robot2_y_pos: "-7.5"
    robot2_z_pos: "0.0"
    robot2_yaw: "0.0"
robots_init: "robots_rviz_init.yaml"
rviz_file: "sim_env.rviz"
pedestrians: "pedestrian_config.yaml"

Explanation:

• map: static map，located in src/sim_env/map/, if map: "", map_server will not publish map message which often used in SLAM.

• world: gazebo world，located in src/sim_env/worlds/, if world: "", Gazebo will be disabled which often used in real world.

• robots_config: robotic configuration.

  • type: robotic type，such as turtlebot3_burger, turtlebot3_waffle and turtlebot3_waffle_pi.

  • global_planner: global algorithm, details in Section Version.

  • local_planner: local algorithm, details in Section Version.

  • xyz_pos and yaw: initial pose.

• robots_init: initial pose in RVIZ.

• rviz_file: RVIZ configure, automatically generated if rviz_file is not set.

• pedestrians: configure file to add dynamic obstacles(e.g. pedestrians).

03. Version

Global Planner

Planner	Version	Animation
GBFS
Dijkstra
A*
JPS
D*
LPA*
D* Lite
RRT
RRT*
Informed RRT
RRT-Connect

Local Planner

Planner	Version	Animation
PID
APF
DWA
TEB
MPC
Lattice

Intelligent Algorithm

Planner	Version	Animation
ACO
GA
PSO
ABC

04. Papers

Search-based Planning

• A*: A Formal Basis for the heuristic Determination of Minimum Cost Paths.
• JPS: Online Graph Pruning for Pathfinding On Grid Maps.
• Lifelong Planning A*: Lifelong Planning A*.
• D*: Optimal and Efficient Path Planning for Partially-Known Environments.
• D* Lite: D* Lite.

Sample-based Planning

• RRT: Rapidly-Exploring Random Trees: A New Tool for Path Planning.
• RRT-Connect: RRT-Connect: An Efficient Approach to Single-Query Path Planning.
• RRT*: Sampling-based algorithms for optimal motion planning.
• Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal heuristic.

Local Planning

• DWA: The Dynamic Window Approach to Collision Avoidance.

05. Application on a Real Robot

In a word, compile our repository and make it visible to the robot, and then replace it just like other planner in ROS navigation.

Example

We use another gazebo simulation as an example, like we have a robot which has the capacity of localization, mapping and navigation (using move_base).

1. Download and compile this repository.

   cd <your_workspace>/
   git clone https://github.com/ai-winter/ros_motion_planning.git
   
   cd ros_motion_planning/
   catkin_make

2. Download and compile the 'real robot' software.

   cd <your_workspace>/
   git clone https://github.com/ZhanyuGuo/ackermann_ws.git
   
   cd ackermann_ws/
   # ---- IMPORTANT HERE, reasons in NOTE ----
   source <your_workspace>/ros_motion_planning/devel/setup.bash
   
   catkin_make

   NOTE: Sourcing other workspaces before catkin_make will make the current setup.bash contain former sourced workspaces, i.e. they are also included when you only source this current workspace later.

   REMEMBER: Remove the old build/ and devel/ of current workspace before doing this, otherwise it will not work.

3. Change the base_global_planner and base_local_planner in real robot's move_base as you need.

   <?xml version="1.0"?>
   <launch>
       <!-- something else ... -->
       <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
           <!-- something else ... -->
   
           <!-- Params -->
           <!-- for graph_planner -->
           <rosparam file="$(find sim_env)/config/planner/graph_planner_params.yaml" command="load" />
           <!-- for sample_planner -->
           <rosparam file="$(find sim_env)/config/planner/sample_planner_params.yaml" command="load" />
           <!-- for dwa_planner -->
           <rosparam file="$(find sim_env)/config/planner/dwa_planner_params.yaml" command="load" />
           <!-- for pid_planner -->
           <rosparam file="$(find sim_env)/config/planner/pid_planner_params.yaml" command="load" />
   
           <!-- Default Global Planner -->
           <!-- <param name="base_global_planner" value="global_planner/GlobalPlanner" /> -->
           <!-- GraphPlanner -->
           <param name="base_global_planner" value="graph_planner/GraphPlanner" />
           <!-- options: a_star, jps, gbfs, dijkstra, d_star, lpa_star, d_star_lite -->
           <param name="GraphPlanner/planner_name" value="a_star" />
           <!-- SamplePlanner -->
           <!-- <param name="base_global_planner" value="sample_planner/SamplePlanner" /> -->
           <!-- options: rrt, rrt_star, informed_rrt, rrt_connect -->
           <!-- <param name="SamplePlanner/planner_name" value="rrt_star" /> -->
   
           <!-- Default Local Planner -->
           <!-- <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" /> -->
           <param name="base_local_planner" value="pid_planner/PIDPlanner" />
           <!-- <param name="base_local_planner" value="dwa_planner/DWAPlanner" /> -->
   
           <!-- something else ... -->
       </node>
       <!-- something else ... -->
   </launch>

4. Run! But maybe there are still some details that you have to deal with...

06. Important Updates

Date	Update
2023.1.13	cost of motion nodes is set to NEUTRAL_COST, which is unequal to that of heuristics, so there is no difference between A* and Dijkstra. This bug has been solved in A* C++ v1.1
2023.1.18	update RRT C++ v1.1, adding heuristic judgement when generating random nodes
2023.2.25	update PID C++ v1.1, making desired theta the weighted combination of theta error and theta on the trajectory
2023.3.16	support dynamic simulation environment, user can add pedestrians by modifing pedestrian_config.yaml

07. Acknowledgments

• Our robot and world models are from Dataset-of-Gazebo-Worlds-Models-and-Maps and aws-robomaker-small-warehouse-world. Thanks for these open source models sincerely.
• Pedestrians in this environment are using social force model(sfm), which comes from https://github.com/robotics-upo/lightsfm.

08. License

The source code is released under GPLv3 license.

09. Maintenance

Feel free to contact us if you have any question.