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
• Important Updates
• Acknowledgments
• License
• Maintenance

0. Quick Start within 3 Minutes

Tested on ubuntu 20.04 LTS with ROS Noetic.

1. Install ROS, (suggested)Desktop-Full.

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. Copy or move model files in ./src/sim_env/models/ into ~/.gazebo/models/.

6. Compile the code.

   cd ros_motion_planning/
   # catkin_make -DCATKIN_WHITELIST_PACKAGES="gazebo_sfm_plugin;pedsim_msgs"
   # catkin_make -DCATKIN_WHITELIST_PACKAGES=""
   
   # Afterwards, everytime you just need to
   catkin_make

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

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

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

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

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

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

07. License

The source code is released under GPLv3 license.

08. Maintenance

Feel free to contact us if you have any question.