Timed Costmap

[leoniegadner]

See my bachelor thesis for a more detailed explanation.

🕐 Idea

The ROS Navigation Stack uses a costmap to represents the current state of the robot’s environment. However, this costmap treats the environment as static, disregarding movement of perceived obstacles. To allow the planner and controller to consider this movement information, we developed a timed costmap approach. The entire costmap is here augmented with a time dimension, by creating multiple costmaps, one for each timestep, instead of one. The planners can then find a path in this spatio-temporal map, allowing them to plan ahead in space and time resulting in more intuitive and overall safer navigation.

🕑 Implementation

Inside the LayeredCostmaps class we create a vector of costmaps std::vector<costmap_2d::Costmap2D> timed_costmaps_and get rid of the single Costmap2D object costmap_. The Map Update updateMap() is then modified to update all costmaps. The update happens in two steps:

1. Static Costmap: We create a private static costmap, which includes all the plugins that are not time dependent. Note, that all plugins after the first timed plugin are considered time dependent, even if they don't have a timed logic themselves, since later layers costs depend on previous ones. (E.g. Inflation Layer costs will change for each timestep because it inflates previous plugins costs, even though inflation layer itself does not have a time logic)
2. Timed Costmap: Here we loop through the actual timed costmaps and update them. First we copy the precomputed static costmap into the timed costmap. Then we update the costmap using the remaining (timed) layers

To make use of this time dimension in the costmap, we also need layers with a timed logic!!! The most obvious idea for such a layer is a dynamic obstacle layer, which displaces moving obstacles along their trajectory given their velocity information. Two PoC's for timed layers can be found here:

• https://github.com/rapyuta-robotics/rr_navigation/pull/1540
• https://github.com/rapyuta-robotics/rr_navigation/pull/1538

Now that the time information is incorporated into the planner's and controller's search space, they also need to make use of it. In this first implementation only DWA was modified to work with the Timed Costmap and the global costmap is thus kept non-timed (prediction_time = 0).

DWA generates trajectories by sampling different angular and linear velocities. The trajectories are scored with forward kinematic projection, using the parameters sim_time, which defines how far to simulate forward, and time_granularity, defining the time increment between two points on the trajectory. To calculate a trajectory’s ‘distance to obstacles’ score, DWA loops through the points on the trajectory and gets each point’s cost from the costmap. The time t for each point on the trajectory is thus given by t = i*time_delta_

When the planner calls the footprintCost() function inside the ObstacleCostFunction class, we can then pass this time argument and use the costmap inside the timed_costmaps_ that corresponds to the given time t to get the footprint cost.

🕒 Evalutation

Comparison of Approaches

To compare some of the different approaches I experimented with I wrote a short python script that sends the robot back and forth and creates 3 obstacles that cross its path. I then let the script run for 2 minutes for each configuration and 'measured' some metrics displayed in the table at the end. First I'll explain the configurations tested...

1. No dynamic obstacle handling
   No explenation needed, we've all seen this one. If the obstacle does not avoid the robot, it will collide. 💥

2. Original Dynamic Obstacle Layer
   A Dynamic Obstacle Layer Approach implemented here simply creates a 2D gaussian shape around the obstacles, extending them forward. For testing I used the original parameters.

3. Improved Dynamic Obstacle Layer
   This approach was discussed in https://github.com/rapyuta-robotics/rr_navigation/pull/1407. The idea is to take the 2D Gaussian shape created in 2 but displace the center to the earliest point on the obstacles trajectory where the robot could collide with it (using the robot's current position and maximum speed). For this approach there are a lot of parameters that affect the performance and can be tuned to achieve different behaviors. For testing I ran it once with the original DOL parameters (IDOL0), once with a smaller gaussian shape (IDOL1) and once with a bigger one (IDOL2)

4. Timed Costmap
   I tested this once with local costmap prediction_time = 1.2 (Timed 1) and once with prediction_time = 2.0 (Timed 2) with DWA sim_time = 1.2s (Timed 1) and once with sim_time = 2.0s (Timed 2). The global costmap prediction_time = 0.0. For this we are using the Timed Gradient Layer. Note, that we use this layer only in the local costmap, and don't add dynamic obstacles to the global costmap at all (not even at their current position!).

5. Only Local
   To test the effectiveness of the Timed Costmap, I also tested a configuration where we use the normal non-timed costmap but dynamic obstacles are added only to the local costmap and not to the global one. For this the same parameters as above where used (Only Local 1, Only Local 2).

In the table below you can see the results I got.

	Current	Only Local 1	Only Local 2	DOL	IDOL 0	IDOL 1	IDOL 2	Timed Map 1	Timed Map 2
Coll.	2	2	1	2	1		2
Almost coll. *	1	1	3	5	1	1		1	1
Weird rotations*	5			5	1	2	2	3	1
Speeding up & stopping*		6	6	3	2
timeout*	1					3
Goals reached	7	7	5	7	9	4	8	8	7
Distance to lethal*	medium	low	low	low	medium	medium	medium	medium	medium

*Note that these are kinda subjective and I tend to be very biased in my judgement… 😇

In the video below, where I ran the Timed Costmap with sim_time_ = 3, you can see how the robot waits for the obstacles to pass or even moves out of the way when the obstacles are coming towards it, but doesn't slow down when the obstacles are moving out of its way.

timed_costmap_3-2023-05-25_09.58.27.mp4

The behaviour of the three different approaches is visualised in the image below:

CPU

One big disadvantage of the Timed Costmap is the increased computation. The CPU usage is strongly affected by the parameters aswell as the order that the plugins are loaded in:

CPU usage with robot standing:

1. Prediction time = 0
   → CPU ~ 40%

2. Prediction time 1.2
   Timestep 0.3
   static_costmap: static
   other plugins: obstacle (timed), obstacle_back, stvl, route, paint, semantic
   → CPU: ~45%

3. prediction_time: 1.2
   timestep: 0.1
   static_costmap: static, route, paint, semantic
   other plugins: obstacle (timed), obstacle_back, stvl, inflation, dynamic
   → CPU ~53%

4. prediction_time: 1.2
   timestep: 0.1
   static_costmap: static, semantic,
   other plugins: obstacle (timed), obstacle_back, stvl, route, paint, inflation, dynamic
   → CPU: ~60%

5. prediction_time: 1.2
   timestep: 0.1
   static_costmap: static
   other plugins: obstacle (timed), obstacle_back, stvl, route, paint, semantic, inflation, dynamic
   → CPU: ~85%

...

🕓 Future Steps

To-Do:
Timed Costmap:

• F: Fix CI
• S: Optimize…
• The way we get the bounds to update only the area of the map that changed could be modified to get multiple bounds for smaller separate areas in the costmap, so that we don't update a bigger area than necessary...
• Or maybe only calculate bounds for the first and last timed costmap? (but then we might update a bigger area than necessary?) → BUT currently the updateBounds function in the timed layer is the one that displaces the obstacles… this logic would have to be moved to updateCosts!!!
• Maybe look at opencv for better updateCost implementation
• S: Timed inflation (inflate also upwards in the time dimension!)

Timed Planners:

• S: Interpolate cost values for points in between timesteps (take average cost value between two costmaps)
• S: Test timed costmap with TEB
• S: Timed global planner! (but maybe add timed obstacle layer only to area within prediction time, that can be reached by robot)

Timed Layers:

• F: Gradient Layer: fix that later costs don’t override previous ones!
• S: Save costs, and make them decay gradually (so obstacles arent jumping around…)
• Maybe look at stvl layer for inspiration
• S: Add gradient to Costmap Converter Layer*
• Make costmap converter publish obstacle center point: Add center point to obstacle msg (see Nav2 gradient layer implementation)
• Use displaced center point to create gaussian
• S: Combine obstacle layer and dynamic obstacle layer: Add timed logic to normal obstacle layer*
• Make timed_gradient_layer work with and without timed costmap!

*might not be necessary, depends on perception component…

[siferati]

F: Need to add documentation to every new function and attribute
F: CI seems to be failing