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!
- Quick Start within 3 Minutes
- File Tree
- Dynamic Configuration
- Version
- Papers
- Application on a Real Robot
- Important Updates
- Acknowledgments
- License
- Maintenance
Tested on ubuntu 20.04 LTS with ROS Noetic.
-
Install ROS (Desktop-Full suggested).
-
Install git.
sudo apt install git
-
Clone this reposity.
cd <your_workspace>/ git clone https://github.com/ai-winter/ros_motion_planning.git
-
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
-
Compile the code.
cd ros_motion_planning/ catkin_make
-
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 runmain.sh
. Therefore, you should change configurations inuser_config.yaml
instead of launch files. -
Use 2D Nav Goal to select the goal.
-
Moving!
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
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 insrc/sim_env/map/
, ifmap: ""
, map_server will not publish map message which often used in SLAM. -
world
: gazebo world,located insrc/sim_env/worlds/
, ifworld: ""
, Gazebo will be disabled which often used in real world. -
rviz_file
: RVIZ configure, automatically generated ifrviz_file
is not set. -
robots_config
: robotic configuration.-
type
: robotic type,such asturtlebot3_burger
,turtlebot3_waffle
andturtlebot3_waffle_pi
. -
global_planner
: global algorithm, details in SectionVersion
. -
local_planner
: local algorithm, details in SectionVersion
. -
xyz_pos and yaw
: initial pose.
-
-
plugins
: other applications using in simulationpedestrians
: 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 higherupdate_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
orSPHERE
pose
: fixed pose of the obstaclecolor
: color of the obstacleprops
: property of the obstaclem
: massw
: widthd
: depthh
: heightr
: radius
Planner | Version | Animation |
---|---|---|
GBFS | ![]() |
|
Dijkstra | ![]() |
|
A* | ![]() |
|
JPS | ![]() |
|
D* | ![]() |
|
LPA* | ![]() |
|
D* Lite | ![]() |
|
Voronoi | ![]() |
|
RRT | ![]() |
|
RRT* | ![]() |
|
Informed RRT | ![]() |
|
RRT-Connect | ![]() |
Planner | Version | Animation |
---|---|---|
PID | ![]() |
|
APF | ||
DWA | ![]() |
|
TEB | ||
MPC | ||
Lattice |
Planner | Version | Animation |
---|---|---|
ACO | ![]() |
|
GA | ||
PSO | ||
ABC |
- 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.
- 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.
- ACO: Ant Colony Optimization: A New Meta-Heuristic
In a word, compile our repository and make it visible to the robot, and then replace it just like other planner in
ROS navigation
.
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).
-
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
-
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 currentsetup.bash
contain former sourced workspaces, i.e. they are also included when you only source this current workspace later.REMEMBER: Remove the old
build/
anddevel/
of current workspace before doing this, otherwise it will not work. -
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>
-
Run! But maybe there are still some details that you have to deal with...
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 |
- 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.
The source code is released under GPLv3 license.
Feel free to contact us if you have any question.