The standard navigation planner in Isaac SDK instructs the robot to take the shortest possible route to a target while avoiding obstacles. However, for many environments, you may need to designate restricted areas or provide traffic rules for robots.
The Costmap Planner application allows you to create navigation behavior that is more fine grained. It offers three main types of FlatmapCost:
OccupancyFlatmapCost, which can be used to provide a map
PolygonFlatmapCost, which can be used to implement a restricted area or penality area
PolylineFlatmapCost, which can be used to define highways
These layers can be multiplied or added together using MultiplicationFlatmapCost or the AdditionFlatmapCost, and you can add your own custom FlatmapCost by implementing the FlatmapCost interface.
This application uses the following components:
This application uses the Pose2GraphProto message.
Run the following command to create a graph for the map for the factory of the future
(demo_5) in /tmp/:
bazel run packages/path_planner/apps:pose2_graph_builder
Then, run flatsim with the new planner:
bazel run //packages/flatsim/apps:flatsim -- --demo demo_5
To customize the graph, edit the packages/path_planner/apps/pose2_graph_builder.app.json
file.
To choose a map, edit the “map” configuration:
"map": {
"occupancy": {
"cell_size": 0.05,
"filename": "apps/assets/maps/virtual_factory_1.png"
}
},
To add flatmaps to the list, edit the “flatmap_cost” configuration:
{
"name": "flatmap_cost",
"start_order": -100,
"components": [
{
"name": "OccupancyFlatmapCost",
"type": "isaac::map::OccupancyFlatmapCost"
},
{
"name": "inside_round",
"type": "isaac::map::PolylineFlatmapCost"
},
{
"name": "outside_round",
"type": "isaac::map::PolylineFlatmapCost"
},
{
"name": "restricted_area",
"type": "isaac::map::PolygonFlatmapCost"
},
{
"name": "MultiplicationFlatmapCost",
"type": "isaac::map::MultiplicationFlatmapCost"
}
]
}
The MultiplicationFlatmapCost component is currently mandatory. However, you can remove or add more custom layers.
The OccupancyFlatmapCost component ensures the robot is not colliding in the map.
“inside_round” is the the cost map that instructs the robot to drive in the inner ring.
“outside_round” is the cost map that instructs the robot to drive in the outer ring.
“restricted_area” forbids driving in the middle area.
Changing the Planner
To switch the GlobalPlanner to a new planner, you need to switch the behavior after creating a
graph, as shown in //packages/flasim/apps/demo_5.json:
"flatsim.navigation.planner.planner_switch_behavior": {
"SwitchBehavior": {
"desired_behavior": "$(fullname flatsim.navigation.planner.pose2_graph_planner)"
}
},
You will also need to edit the SwitchBehavior node to ensure the node name matches the new app.
You might need to also change the config of the new planner to make sure it loads the right graph:
"navigation.planner.pose2_graph_planner": {
"Pose2GraphPlanner": {
"bucket_size": 0.5,
"graph_filename": "/tmp/pose2_grid_graph.capnp.bin" }
},
Adding the Costmap to Sight
This step is optional, but it might be useful to visualize the different areas used for the cost map in Sight. Simply add the Costmap used in the application that builds the graph. Note that you only need to add costmaps that render something–i.e. only the “PolylineFlatmapCost”:
{
"graph": {
"nodes: [
{
"name": "flatmap_cost",
"start_order": -100,
"components": [
{
"name": "inside_round",
"type": "isaac::map::PolylineFlatmapCost"
},
{
"name": "outside_round",
"type": "isaac::map::PolylineFlatmapCost"
}
]
},
...
"config": {
"flatmap_cost": {
"inside_round": {
"polyline": [
[36.0, 109.5], [36.0, 16.0], [34.5, 14.5], [16.0, 14.5], [14.5, 16.0],
[14.5, 109.5], [16.5, 111.0], [34.5, 111.0], [36.0, 109.5]
],
"width": 0.8,
"penality_angle": 1.0,
"penality_distance": 2.0,
"outside_weight": 10.0
},
"outside_round": {
"polyline": [
[38.0, 110.0], [35.0, 113.0], [16.0, 113.0], [12.5, 110.0],
[12.5, 15.5], [15.5, 12.5], [35.0, 12.5], [38.0, 15.5], [38.0, 110.0]
],
"width": 0.8,
"penality_angle": 1.0,
"penality_distance": 2.0,
"outside_weight": 10.0
}
},
...
}
}
If you want to enable the configuration by default, you need to create your own SightRenderer or modify an existing one:
"flatsim.navigation.sight_widgets": {
"Map View": {
"type": "2d",
"channels": [
{ "name": "map/occupancy/map" },
{ "name": "flatmap_cost/outside_round/area" },{ "name": "flatmap_cost/outside_round/polyline" }, { "name": "flatmap_cost/restricted_area/area" },
{ "name": "flatmap_cost/inside_round/polyline" },{ "name": "flatmap_cost/inside_round/area" }, { "name": "map/waypoints/waypoints" },
{ "name": "flatsim.navigation.localization.viewers/FlatscanViewer/beam_endpoints" },
{ "name": "flatsim.navigation.localization.viewers/FlatscanViewer2/beam_endpoints" },
{ "name": "flatsim.navigation.go_to.goal_viewer/GoalViewer/goal" },
{ "name": "flatsim.navigation.localization.viewers/RobotViewer/robot_model" },
{ "name": "flatsim.navigation.localization.viewers/RobotViewer/robot" },
{ "name": "flatsim.navigation.planner.pose2_graph_planner/Pose2GraphPlanner/path" }, { "name": "flatsim.navigation.control.lqr/isaac.lqr.DifferentialBaseLqrPlanner/plan" }
]
}
},