Introduction

Motion Planning plans the state sequence of the robot without conflicts between the start and goal.

Motion Planning mainly includes Path Planning and Trajectory Planning.

• Path Planning: Based on path constraints (such as obstacles), it plans the optimal path sequence for the robot to travel without conflicts between the start and goal.

• Trajectory Planning: Base on kinematics, dynamics constraints and path sequence, it plans the motion state to approach the global path.

This repository provides the implement of common Motion Planning algorithms. Welcome your star, fork and PR!

The theory analysis can be found at motion-planning.

We also provide Python and MATLAB version.

Quick Start

The file structure is shown below.

ros_motion_planner
├─gif                     # Animation for README
└─src
    ├─planner
    │  ├─global_utils     # global planner tools
    │  ├─graph_planner    # global algorithm based on graph searching
    │  ├─sample_planner   # global algorithm based on sample searching
    │  └─local_planner    # local algorithm
    ├─sim_env             # simulation environment
    │  ├─config
    │  ├─launch
    │  ├─maps
    │  ├─meshes
    │  ├─models
    │  ├─rviz
    │  ├─scripts
    │  ├─urdf
    │  └─worlds
    ├─third_party
    └─user_config         # user configure file

To start simulation, compile using catkin_make. You can directly using this folder as workspace.

cd ros_motion_planning/
catkin_make

Or, we assume that your workspace is ~/sim_platform/.

cd ros_motion_planning/
mv src/ ~/sim_platform/
cd ~/sim_platform/
catkin_make

Edit user configure

cd src/user_config/
touch user_config.yaml

Below is the 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"

Explanation:

• map: static map，located in src/sim_env/map/.
• world: gazebo world，located in src/sim_env/worlds/.
• 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.

Then create a new terminal and build the environment.

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

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

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.

Update

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

Acknowledgment

• 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.