The ITS Planner is a global path planner module for ROS2 Navigation based on Intelligent Sampling and Two-Way Search (ITS). This plugin is designed for efficient path planning using either Probabilistic Road Map (PRM) or Deterministic Road Map (DRM) approaches.
The inputs for the ITS planner are global 2d_costmap (nav2_costmap_2d::Costmap2D), start and goal pose
(geometry_msgs::msg::PoseStamped). The outputs are 2D waypoints of the path. The ITS planner converts the
2d_costmap to a roadmap which can be saved in a txt file and reused for multiple inquiries. Once a roadmap is
generated, the ITS conducts a two-way search to find a path from the source to destination. Either the smoothing
filter or catmull spline interpolation can be used to create a smooth and continuous path. The generated smooth
path is in the form of ROS navigation message type (nav_msgs::msg).
Currently, the ITS plugin does not support continuous replanning. To use this plugin, a simple behavior tree with compute path to pose and follow path should be used.
Prepare the target system following the official documentation.
To build the ITS Planner packages, set ROS_DISTRO to one of the supported ROS 2 distributions (humble or jazzy) and run make build:
ROS_DISTRO=humble make build
# or
ROS_DISTRO=jazzy make buildThis will build the following packages (using the selected ROS distribution prefix):
ros-${ROS_DISTRO}-its-plannerros-${ROS_DISTRO}-its-relocalizationros-${ROS_DISTRO}-its-send-localizationros-${ROS_DISTRO}-nav2-bringup-collab
The built packages will be available in the root directory.
To clean all build artifacts:
make cleanIf Ubuntu 22.04 with Humble or Ubuntu 24.04 with Jazzy is used, source the matching ROS setup:
source /opt/ros/$ROS_DISTRO/setup.bash # ROS_DISTRO=humble or jazzyInstall the ros-${ROS_DISTRO}-its-planner Debian package from the Intel Robotics AI Suite APT repo:
sudo apt install ros-${ROS_DISTRO}-its-plannerOr install the locally built Debian package:
sudo apt update
sudo apt install ./ros-${ROS_DISTRO}-its-planner_*_amd64.debThere is a set of prepared Makefile targets to speed up the development.
In particular, use the following Makefile target to run code linters:
make lintTo run license compliance validation:
make license-checkTo see a full list of available Makefile targets:
$ make help
Target Description
------ -----------
build Build ITS Planner and related packages
build-nav2-amcl Build patched nav2-amcl package
build-nav2-msgs Build patched nav2-msgs package
help Display this help message
license-check Perform a REUSE license check using docker container https://hub.docker.com/r/fsfe/reuse
lint Run all sub-linters using super-linter (using linters defined for this repo only)
lint-all Run super-linter over entire repository (auto-detects code to lint)
source-package Create source package tarballTo use this plugin, add the following parameters to the distro-specific nav2 params file (e.g., nav2_params_${ROS_DISTRO}.yaml):
planner_server:
ros__parameters:
expected_planner_frequency: 0.01
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "its_planner::ITSPlanner"
interpolation_resolution: 0.05
catmull_spline: False
smoothing_window: 15
buffer_size: 10
build_road_map_once: True
min_samples: 250
roadmap: "PROBABILISTIC"
w: 32
h: 32
n: 2Parameter Descriptions:
catmull_spline: if true, the generated path from the ITS will be interpolated with catmull spline method; otherwise smoothing filter will be used to smooth the pathsmoothing_window: window size for the smoothing filter; unit is grid sizebuffer_size: during the roadmap generation, the samples are generated away from obstacles. The buffer size dictates how far the roadmap samples should be away from obstaclesbuild_road_map_once: If true, the roadmap will be loaded from the saved file, otherwise a new roadmap will be generatedmin_samples: minimum number of samples required to generate the roadmaproadmap: can be eitherPROBABILISTICorDETERMINISTICw: the width of the window for intelligent samplingh: the height of the window for intelligent samplingn: the minimum number of samples that is required in an area defined bywandh
Run the following script to set environment variables (select your distro):
source /opt/ros/$ROS_DISTRO/setup.bash # ROS_DISTRO=humble or jazzy
export TURTLEBOT3_MODEL=waffle
# Set Gazebo model path (variable name differs between distributions)
if [ "$ROS_DISTRO" = "jazzy" ]; then
export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:/opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models
else
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models
fiTo launch the default ITS planner which is based on differential drive robot, run:
ros2 launch nav2_bringup tb3_simulation_launch.py \
headless:=False \
params_file:=/opt/ros/$ROS_DISTRO/share/its_planner/nav2_params_${ROS_DISTRO}.yaml \
default_bt_xml_filename:=/opt/ros/$ROS_DISTRO/share/its_planner/navigate_w_recovery_${ROS_DISTRO}.xmlITS Planner also supports Ackermann steering; to launch the Ackermann ITS planner run:
ros2 launch nav2_bringup tb3_simulation_launch.py \
headless:=False \
params_file:=/opt/ros/$ROS_DISTRO/share/its_planner/nav2_params_dubins_${ROS_DISTRO}.yaml \
default_bt_xml_filename:=/opt/ros/$ROS_DISTRO/share/its_planner/navigate_w_recovery_${ROS_DISTRO}.xmlAfter launching the ROS2 navigation and the ITS planner plugin, look at where the robot is in the Gazebo world, and find that spot on the Rviz display.
- From Rviz set the initial pose by clicking the "2D Pose Estimate" button
- Click on the map where the robot is
- Click the "Navigation2 Goal" button and choose goal position
Now a path will be generated and the robot will start following the path to navigate toward the goal position.
For detailed instructions, follow the ROS2 Navigation usage guide: Navigation usage
This plugin also supports a global path planner based on ITS for Ackermann steering vehicles, which maneuver with car-like controls and a limited turning radius. This version of the planner is based on the concept of Dubins Paths, and uses an adapted version of AndrewWalker's Dubins Curves implementation.
The Ackermann steering version of this plugin utilizes some additional parameters which can be found in nav2_params_dubins_${ROS_DISTRO}.yaml:
planner_server:
ros__parameters:
expected_planner_frequency: 0.01
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "its_planner::ITSPlanner"
interpolation_resolution: 0.05
catmull_spline: False
smoothing_window: 15
buffer_size: 10
build_road_map_once: True
min_samples: 250
roadmap: "PROBABILISTIC"
w: 32
h: 32
n: 2
-- Dubins Specific --
dubins_path: True
turn_radius: .22
robot_radius: .25
yaw_tolerance: .125
use_final_heading: True
Dubins-Specific Parameters:
dubins_path: If true, the ITS algorithm will utilize Dubins Paths to form a global path that can be followed by an Ackermann steering vehicle.turn_radius: The minimum turning radius of the robot, in world scale.robot_radius: The radius of the robot, in world scale.yaw_tolerance: The amount (+/-) by which the heading angles of the end positions of the intermediate Dubins curves may vary, in radians. Does not apply to the final Goal heading.use_final_heading: Whether to use the goal heading specified by thegeometry_msgs::msg::PoseStampedmessage or not.
Comprehensive documentation on this component is available here: dev guide.
its-planner is licensed under Apache 2.0 License.