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. To compile and execute the code, run the scripts in ./src/sim_env/scripts/. The make.sh is for compilation and the main.sh is for execution.

   cd ros_motion_planning/src/sim_env/scripts/
   ./make.sh
   ./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.

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

7. 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"
rviz_file: "sim_env.rviz"

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"

plugins:
  pedestrians: "pedestrian_config.yaml"
  obstacles: "obstacles_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.

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

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

• plugins: other applications using in simulation

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

For pedestrians and obstacles configuration files, the examples are shown below

## pedestrians_config.yaml

# sfm algorithm configure
social_force:
  animation_factor: 5.1
  # only handle pedestrians within `people_distance`
  people_distance: 6.0
  # weights of social force model
  goal_weight: 2.0
  obstacle_weight: 80.0
  social_weight: 15
  group_gaze_weight: 3.0
  group_coh_weight: 2.0
  group_rep_weight: 1.0

# pedestrians setting
pedestrians:
  update_rate: 5
  ped_property:
    - name: human_1
      pose: 5 -2 1 0 0 1.57
      velocity: 0.9
      radius: 0.4
      cycle: true
      time_delay: 5
      ignore:
        model_1: ground_plane
        model_2: turtlebot3_waffle
      trajectory:
        goal_point_1: 5 -2 1 0 0 0
        goal_point_2: 5 2 1 0 0 0
    - name: human_2
      pose: 6 -3 1 0 0 0
      velocity: 1.2
      radius: 0.4
      cycle: true
      time_delay: 3
      ignore:
        model_1: ground_plane
        model_2: turtlebot3_waffle
      trajectory:
        goal_point_1: 6 -3 1 0 0 0
        goal_point_2: 6 4 1 0 0 0

Explanation:

• social_force: The weight factors that modify the navigation behavior. See the Social Force Model for further information.
• pedestrians/update_rate: Update rate of pedestrains presentation. The higher update_rate, the more sluggish the environment becomes.
• pedestrians/ped_property: Pedestrians property configuration.
  • name: The id for each human.
  • pose: The initial pose for each human.
  • velocity: Maximum velocity (m/s) for each human.
  • radius: Approximate radius of the human's body (m).
  • cycle: If true, the actor will start the goal point sequence when the last goal point is reached.
  • time_delay: This is time in seconds to wait before starting the human motion.
  • ignore_obstacles: All the models that must be ignored as obstacles, must be indicated here. The other actors in the world are included automatically.
  • trajectory. The list of goal points that the actor must reach must be indicated here. The goals will be post into social force model.

## obstacles_config.yaml 

# static obstacles
obstacles:
  - type: BOX
    pose: 5 2 0 0 0 0
    color: Grey
    props:
      m: 1.00
      w: 0.25
      d: 0.50
      h: 0.80

Explanation:

• type: model type of specific obstacle, Optional: BOX, CYLINDER or SPHERE
• pose: fixed pose of the obstacle
• color: color of the obstacle
• props: property of the obstacle
  • m: mass
  • w: width
  • d: depth
  • h: height
  • r: radius

03. Version

Global Planner

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

Local Planner

Planner	Version	Animation
PID
DWA
APF
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.
• Theta*: Theta*: Any-Angle Path Planning on Grids, Any-angle path planning on non-uniform costmaps.
• Lazy Theta*: Lazy Theta*: Any-Angle Path Planning and Path Length Analysis in 3D.

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.

Evolutionary-based Planning

• ACO: Ant Colony Optimization: A New Meta-Heuristic.

Local Planning

• DWA: The Dynamic Window Approach to Collision Avoidance.
• APF: Real-time obstacle avoidance for manipulators and mobile robots.

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.
• A ROS costmap plugin for dynamicvoronoi presented by Boris Lau.

08. License

The source code is released under GPLv3 license.

09. Maintenance

Feel free to contact us if you have any question.