isaac_ros_occupancy_grid_localizer

Source code available on GitHub.

The Occupancy Grid Localizer processes a planar range scan to estimate pose in an occupancy grid map; this occurs in less than 1 second for most maps. This initial pose can be used to bootstrap navigation for mobile robots and has been integrated and tested with Nav2. This can remove the need for upwards of 30 seconds to manually estimate the position and direction of a robot with RViz, for example.

The Occupancy Grid Localizer is designed to work with planar and 3D LIDARs. It uses Flatscan for input to the GPU-accelerated computation estimating pose. Flatscan allows for representation of 3D LIDARs, which have variable angular increments between multiple beams.

LaserScan to Flatscan provides conversion from LaserScan, which by definition has equal angle increment between beams, to Flatscan.

PointCloud to FlatScan provides conversion from pointcloud output from 3D LIDARs to Flatscan.

Isaac ROS NITROS Acceleration

This package is powered by NVIDIA Isaac Transport for ROS (NITROS), which leverages type adaptation and negotiation to optimize message formats and dramatically accelerate communication between participating nodes.

Note

Localization can be performed multiple times during navigation.

Note

The input FlatScan Message header/frame_id is used to get the transform of the lidar with respect to the robot base_link frame.

Note

The output localization_result is the transform of base_link with respect to the frame specified in the loc_result_frame (map) ROS parameter.

Note

Localization can be triggered in one of two ways:

1. Buffer FlatScan messages received on a topic and trigger the localization using an std_srvs/Empty service call.

2. Trigger localization every time a FlatScan message is sent to a topic.

Performance

Sample Graph	Input Size	AGX Orin	Orin NX	Orin Nano Super 8GB	x86_64 w/ RTX 4090
Occupancy Grid Localizer Node	~50 sq. m	19.6 fps 57 ms @ 30Hz	8.36 fps 130 ms @ 30Hz	9.02 fps 120 ms @ 30Hz	50.1 fps 8.5 ms @ 30Hz

Quickstart

Set Up Development Environment

1. Set up your development environment by following the instructions in getting started.

2. (Optional) Install dependencies for any sensors you want to use by following the sensor-specific guides.

   Note

   We strongly recommend installing all sensor dependencies before starting any quickstarts. Some sensor dependencies require restarting the development environment during installation, which will interrupt the quickstart process.

Download Quickstart Assets

1. Download quickstart data from NGC:

   Make sure required libraries are installed.

   sudo apt-get install -y curl jq tar
   

   Then, run these commands to download the asset from NGC:

   NGC_ORG="nvidia"
   NGC_TEAM="isaac"
   PACKAGE_NAME="isaac_ros_occupancy_grid_localizer"
   NGC_RESOURCE="isaac_ros_occupancy_grid_localizer_assets"
   NGC_FILENAME="quickstart.tar.gz"
   MAJOR_VERSION=4
   MINOR_VERSION=0
   VERSION_REQ_URL="https://catalog.ngc.nvidia.com/api/resources/versions?orgName=$NGC_ORG&teamName=$NGC_TEAM&name=$NGC_RESOURCE&isPublic=true&pageNumber=0&pageSize=100&sortOrder=CREATED_DATE_DESC"
   AVAILABLE_VERSIONS=$(curl -s \
       -H "Accept: application/json" "$VERSION_REQ_URL")
   LATEST_VERSION_ID=$(echo $AVAILABLE_VERSIONS | jq -r "
       .recipeVersions[]
       | .versionId as \$v
       | \$v | select(test(\"^\\\\d+\\\\.\\\\d+\\\\.\\\\d+$\"))
       | split(\".\") | {major: .[0]|tonumber, minor: .[1]|tonumber, patch: .[2]|tonumber}
       | select(.major == $MAJOR_VERSION and .minor <= $MINOR_VERSION)
       | \$v
       " | sort -V | tail -n 1
   )
   if [ -z "$LATEST_VERSION_ID" ]; then
       echo "No corresponding version found for Isaac ROS $MAJOR_VERSION.$MINOR_VERSION"
       echo "Found versions:"
       echo $AVAILABLE_VERSIONS | jq -r '.recipeVersions[].versionId'
   else
       mkdir -p ${ISAAC_ROS_WS}/isaac_ros_assets && \
       FILE_REQ_URL="https://api.ngc.nvidia.com/v2/resources/$NGC_ORG/$NGC_TEAM/$NGC_RESOURCE/\
   versions/$LATEST_VERSION_ID/files/$NGC_FILENAME" && \
       curl -LO --request GET "${FILE_REQ_URL}" && \
       tar -xf ${NGC_FILENAME} -C ${ISAAC_ROS_WS}/isaac_ros_assets && \
       rm ${NGC_FILENAME}
   fi
   

Build isaac_ros_occupancy_grid_localizer

Binary PackageBuild from Source

1. Activate the Isaac ROS environment:

   isaac-ros activate
   

2. Install the prebuilt Debian package:

   sudo apt-get update
   

   sudo apt-get install -y ros-jazzy-isaac-ros-occupancy-grid-localizer
   

Run Launch File

1. Continuing inside the Isaac ROS environment, rviz2:

   rviz2 -d  $(ros2 pkg prefix isaac_ros_occupancy_grid_localizer --share)/rviz/quickstart.rviz
   

2. Open a second terminal and activate the Isaac ROS environment:

   isaac-ros activate
   

3. Run the launch file to spin up a demo of this package:

   ros2 launch isaac_ros_occupancy_grid_localizer isaac_ros_occupancy_grid_localizer_quickstart.launch.py
   

4. Open another terminal and activate the Isaac ROS environment:

   isaac-ros activate
   

5. Run the rosbag:

   ros2 bag play -l ${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_occupancy_grid_localizer/rosbags/flatscan
   

6. Open another terminal and activate the Isaac ROS environment:

   isaac-ros activate
   

7. Trigger the localization using a command line service call:

   ros2 service call trigger_grid_search_localization std_srvs/srv/Empty {}
   

8. Verify that you see a frame being generated in the map showing the

   position of the LIDAR.

Try More Examples

To continue your exploration, check out the following suggested examples:

• Tutorial with Isaac Sim

Troubleshooting

Isaac ROS Troubleshooting

For solutions to problems with Isaac ROS, see here.

API

Usage

ros2 launch isaac_ros_occupancy_grid_localizer isaac_ros_occupancy_grid_localizer.launch.py

Note

Use the flatscan topic with the trigger_grid_search_localization service to trigger localization using a service.

Or publish directly to the flatscan_localization topic to trigger localization every time a FlatScan message is received on this topic.

Do not publish FlatScan messages to both flatscan and flatscan_localization topics.

OccupancyGridLocalizerNode

ROS Parameters

Note

The ROS parameter names are the same as the Nav2 map_server YAML parameters. This allows to load and pass the same YAML file to both Nav2 and isaac_ros_occupancy_grid_localizer as shown in the Isaac Sim Launch File

ROS Parameter	Type	Default	Description
loc_result_frame	std::string	map	frame_id of localization result
resolution	double	0.05	The meters per pixel of the .png map being loaded. This parameter is loaded from the the map YAML file.
origin	std::vector<double>	[0.0, 0.0, 0.0]	The origin of the map loaded. Used to transform the output to compensate for the same transform made to the PNG file loaded by the Nav2 map_server.
occupied_thresh	double	0.65	Pixels with occupancy probability greater than this threshold are considered completely occupied. This parameter is loaded from the the map YAML file. Supported values: [0,1)
image	std::string	""	Name of the PNG file used to load map. This should be in the same directory as the map YAML file specified in map_yaml_path. This parameter is loaded from the the map YAML file.
map_yaml_path	std::string	""	Absolute path to the map YAML file. From which we load the resolution and occupied_thresh
max_points	int	20000	Maximum number of points in FlatScan Message that can be received used to pre-allocate GPU memory.
robot_radius	double	0.25	The radius of the robot. This parameter is used to exclude poses which are too close to an obstacle.memory.
min_output_error	double	0.22	The minimal output error used to normalize and compute confidence, if output error from best sample smaller or equal to this, the confidence is 1
max_output_error	double	0.35	The max output error from our best sample, if output error larger than this threshold, we conclude localization failed
max_beam_error	double	0.5	The maximum beam error used when comparing range scans.
num_beams_gpu	int	512	The GPU accelerated scan-and-match function can only handle a certain number of beams per range scan. The allowed values are {32, 64, 128, 256, 512}. If the number of beams in the range scan does not match this number a subset of beams will be taken.
batch_size	int	512	This is the number of scans to collect into a batch for the GPU kernel. Choose a value which matches your GPU well.
sample_distance	double	0.1	Distance between sample points in meters. The smaller this number, the more sample poses will be considered. This leads to a higher accuracy and lower performance.
out_of_range_threshold	double	100.0	Points range larger than this threshold will be marked as out of range and not used.
invalid_range_threshold	double	0.0	Points range smaller than this threshold will be marked as invalid and not used.
min_scan_fov_degrees	double	270.0	Minimal required scan FoV to run the localizer.
use_closest_beam	bool	true	Whether or not pick the closest angle beam in angle bucket, if not pick the average within an angular bucket

ROS Topics Subscribed

ROS Topic	Type	Description
flatscan	isaac_ros_pointcloud_interfaces::msg::FlatScan	The input FlatScan messages buffer. The last message on this topic will be used as input for localization when the trigger_grid_search_localization service is called.
flatscan_localization	isaac_ros_pointcloud_interfaces::msg::FlatScan	The topic to trigger localization directly without a buffer. Localization will be triggered every time a FlatScan message is received on this topic.

ROS Topic	Interface	Description
localization_result	geometry_msgs::msg::PoseWithCovarianceStamped	Pose of the scan data with respect to the map origin, as specified in the first note in the overview section

ROS Services Advertised

ROS Service	Interface	Description
trigger_grid_search_localization	std_srvs::srv::Empty	The service to trigger the global localization using the last scan received on the flatscan input topic.