Costmap Planner

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/:

Copy
Copied!
            

bazel run packages/planner/apps:pose2_graph_builder

Then, run flatsim with the new planner:

Copy
Copied!
            

bazel run //packages/flatsim/apps:flatsim -- --demo demo_5

To customize the graph, edit the packages/planner/apps/pose2_graph_builder.app.json file.

To choose a map, edit the “map” configuration:

Copy
Copied!
            

"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:

Copy
Copied!
            

{ "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:

Copy
Copied!
            

"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:

Copy
Copied!
            

"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”:

Copy
Copied!
            

{ "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:

Copy
Copied!
            

"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.planner.DifferentialBaseLqrPlanner/plan" } ] } },

© Copyright 2018-2020, NVIDIA Corporation. Last updated on Feb 1, 2023.