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.

Components

This application uses the following components:

Messages

This application uses the Pose2GraphProto message.

Getting Started

Run the following command to create a graph for the map for the factory of the future (demo_5) in /tmp/:

bazel run packages/planner/apps:pose2_graph_builder

Then, run flatsim with the new planner:

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

Customizing the Graph

To customize the graph, edit the packages/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.

Using a Custom Map

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
      }
    },
    ...
  }
}

Adding the Channel to the Configuration

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