The waypoint graph generator builds a sparse waypoint graph from a map for route planning. A waypoint graph consists of nodes as positions in the free space on the map, and edges connecting the nodes.
The waypoint graph generator consumes the following data to create a waypoint graph:
Map: the map that the robot is on. It can be a semantic map or an occupancy map.
Robot shape: a multi-polygon describing the shape of the robot.
The waypoint graph generator returns the waypoint graph in the Compressed Sparse Row (CSR) format. The CSR graph consists of the following:
Nodes: a list of 3D coordinates of each node. The z coordinates are set to 0 if the map is an occupancy map.
Edges: a list of edge destination as end node indices in the nodes list.
Weights: a list of floating-point cost of each edge.
Offset: a list of start index in the edges list for each node. For example, the ith node has edges from the (
offset[i])th to the (offset[i+1]-1)th elements in the edges list.
Illustration of the CSR representation:
If the input map is a semantic map, the waypoint graph also contains a list of the IDs of semantic objects that each node is on.
To facilitate route planning, the waypoint graph generator provides a function to quickly query nodes for start or goal locations.
There are two waypoint graph generator implementations for the two supported map types.
Semantic Map Based Waypoint Graph Generator
This waypoint graph generator consumes a semantic map and builds the graph in regions that are labeled as accessible and are safe for the robots to navigate in. The cost of each edge is the length of the straight line between two nodes, weighted by the semantic object label of the regions it crosses. For example, edges crossing a narrow corridor have a higher cost than those on a general navigable surface of the same length.
Example of a waypoint graph generated from a semantic map:
Occupancy Map Based Waypoint Graph Generator
This waypoint graph generator splits the free space on an occupancy map into regions, each covered by a node. Nodes on adjacent regions are connected with edges. Note that the edges only indicate connectivity between regions, not directionality. Robots navigating from one node to another may not be able to take the straight line.
Example of a waypoint graph generated from an occupancy map:
The two waypoint graph generator implementations are wrapped in two GXF
component classes, WaypointGraphGeneratorSemanticMap and
WaypointGraphGeneratorGridMap under the nvidia::isaac namespace.
Both of them are derived from the nvidia::isaac::WaypointGraphGenerator
interface.
Example
To use the waypoint graph generator in your codelet, you can register as
a GXF parameter that is a handle of the WaypointGraphGenerator GXF
component. The waypoint graph is built during the initialization of the
component, and hence it is available when your codelet starts.
In the following examples, suppose the codelet you are developing is
MyCodelet and the name of the parameter is graph_generator.
Building a waypoint graph with a semantic map
%YAML 1.2
---
# This file contains required GXF components that initialize a waypoint
# graph generator to build a graph from an semantic map.
---
# This section defines the pose tree frames required to perform transforms.
name: pose_tree_frames
components:
- name: pose_tree
type: nvidia::isaac::PoseTree
- type: nvidia::isaac::PoseTreeSetup
parameters:
pose_tree: pose_tree
- name: world
type: nvidia::isaac::PoseTreeFrame
parameters:
pose_tree: pose_tree
---
# This section sets up the components that load the semantic map and set its
# transform from the world.
name: semantic_map
components:
- name: map
type: nvidia::isaac::deepmap::SemanticMapAccessor
parameters:
# Change to path to your semantic map file.
map_data_path: "path/to/map/data"
etc_dir_path: "placeholder"
map_frame: frame
- name: frame
type: nvidia::isaac::PoseTreeFrame
parameters:
parent_frame: pose_tree_frames/world
pose_tree: pose_tree_frames/pose_tree
frame_name: "semantic_map"
# Update according to the transform between your world and map frames.
initial_pose:
translation: [0.0, 0.0, 0.0]
rotation_rpy: [0.0, 0.0, 0.0]
---
# This section describes the shape of the robot used in your application.
name: robot_shape
components:
- name: polygons
type: nvidia::isaac::MultiPolygon
parameters:
# Change to your robot's shape.
polygons: [[[0.3, 0.0], [0.0, 0.3], [-0.3, 0.0], [0.0, -0.3]]]
---
# This section sets up the waypoint graph generator that creates a graph
# from the semantic map defined above.
name: graph_generator
components:
- name: graph_generator
type: nvidia::isaac::WaypointGraphGeneratorSemanticMap
parameters:
semantic_map_accessor: semantic_map/map
robot_shape: robot_shape/polygons
---
# This section sets up your codelet.
name: my_entity
components:
- name: my_codelet
type: MyCodelet
parameters:
graph_generator: graph_generator/graph_generator
Building a waypoint graph with an occupancy map
%YAML 1.2
---
# This file contains required GXF components that initialize a waypoint
# graph generator to build a graph from an occupancy map.
---
# This section defines the pose tree frames required to perform transforms.
name: atlas
components:
- name: frontend
type: nvidia::isaac::AtlasFrontend
parameters:
pose_tree: pose_tree
occupancy_grid_map: map/occupancy_grid_map
- name: pose_tree
type: nvidia::isaac::PoseTree
- type: nvidia::isaac::PoseTreeSetup
parameters:
pose_tree: pose_tree
- name: world
type: nvidia::isaac::PoseTreeFrame
parameters:
pose_tree: pose_tree
- name: map
type: nvidia::isaac::PoseTreeFrame
parameters:
pose_tree: pose_tree
parent_frame: world
# Update according to the transform between your world and map frames.
initial_pose:
translation: [0.0, 0.0, 0.0]
rotation_rpy: [0.0, 0.0, 0.0]
---
# This section sets up the components that load and serve the occupancy map.
name: map
components:
- name: occupancy_map_image
type: nvidia::gxf::Tensor
- name: host_allocator
type: nvidia::gxf::UnboundedAllocator
- name: device_allocator
type: nvidia::gxf::UnboundedAllocator
- name: image_loader
type: nvidia::isaac::ImageLoader
parameters:
allocator: host_allocator
tensor: occupancy_map_image
# Change this to your map image path.
filename: "path/to/map/image.png"
- name: occupancy_grid_map
type: nvidia::isaac::OccupancyGridMap
parameters:
# Update cell_size with your map's resolution.
cell_size: 0.05
# Update threshold based on your map.
threshold: 129
occupancy_map_image: occupancy_map_image
map_frame: map
device_allocator: device_allocator
allocator: host_allocator
---
# This section describes the shape of the robot used in your application.
name: robot_shape
components:
- name: polygons
type: nvidia::isaac::MultiPolygon
parameters:
# Change to your robot's shape.
polygons: [[[0.3, 0.0], [0.0, 0.3], [-0.3, 0.0], [0.0, -0.3]]]
---
# This section sets up the waypoint graph generator that creates a graph from the occupancy map
# defined above.
name: graph_generator
components:
- name: graph_generator
type: nvidia::isaac::WaypointGraphGeneratorGridMap
parameters:
occupancy_grid_map: map/occupancy_grid_map
robot_shape: robot_shape/polygons
---
# This section sets up your codelet.
name: my_entity
components:
- name: my_codelet
type: MyCodelet
parameters:
graph_generator: graph_generator/graph_generator