Add Controller Benchmark to tools/benchmarks then break out the benchmarks into a new package

Probably the same random environment with some static path we use and then take metrics like time to goal, distance traversed, "smoothness" of velocities and behaviors in some metrics, closeness to obstacles, maybe maximum possible run-time. Maybe add option to have random dynamic obstacles in the way too.

Then add tutorials for use / customization with different maps / robots / parameters.

I would gladly contribute! I'm not sure I'll have time before weekend though

Awesome!! You can find examples in the tools directory for planning and smoothing to get a feel for it. Another metric might be path tracking accuracy, though obviously that’s highly dependent on the controller’s tuning (but might be another metric to add to try to test head-to-head the same algorithm with many parameters to tune)

So I've taken a look at what you guys did, I must say I did get a feel for it! I like the metric you proposed, unfortunately I'm still setting up stuff.

Controller requires non static tf and odometry, so I had to add some nodes, I hope this is not an issue- I've used the cmd_vel twist itself to back-compute odometry and update the tf (odom-> base_link). Currently I "trust" the cmd_vel, meaning I do not check max acceleration or anything. I had no other ideas to do this, a part from integrating with Gazebo, which I'd personally avoid. Hence in my metric.py file I've got a a cmd_vel subscriber, an odometry publisher and a tf broadcaster. Of course I'm writing this just to be sure what I'm doing it's not insane, any feedback it's appreciated!

So here's a video of what's going on, simply the controlling following the plan (sorry for the 21/9' ).

https://user-images.githubusercontent.com/51802424/197409583-0d1997dc-f271-478e-957d-21eb4e08e7b7.mp4

These are my nexts:

[ ] Moving obstacles. I was thinking of some object moving randomly in the costmap, or spawning statically in free space of the global costmap... to be honest I think there are a thousand ways to do this.
[ ] Benchmark speed. While with planning and smoothers it's instantaneous, here we have to wait for the controller to end the task, so it's a couple orders of magnitude longer. The only thing coming to my mind is to use simulation clock for all nodes, the real time factor being a parameter.
[ ] Add some check on odometry computation, e.g. adding saturations on acceleration
[ ] Well, actually gathering metrics
[ ] Creating a dedicated package
[ ] Documenting

Hey there, I've a first version of the whole thing working, though I cannot say it is complete. I've added these metrics:

time to goal
distance traversed
average linear speed (not really useful, I'll convert to smoothness somehow)

What I find "hard" is to interact with the local costmap. The easiest thing (not to say the laziest) is to start providing static obstacle via the static layer: Practically I have two maps, one with less obstacle, which is used by the global costmap (when the planner at first compute the paths), and one with more obstacles, which is used by the local costmap. This have two downsides: 1) It's static, it would require a map update to move obstacle. 2) For some reason I don't understand, inflation layer doesn't work in this configuration and it sucks because for my understanding the DWB (but also I guess all controllers) works much better with inflation.

So an alternative could be creating a fake sensors to interact with the costmap,which could be more easily dynamic, but it would take a bit of time to develop...

Just for anyone passing by, an update video. (I've increased max robot speed to make the benchmark faster)

https://user-images.githubusercontent.com/51802424/198857207-1c849f98-dd4f-466b-9f83-768e5cd96309.mp4

Hi,

Took some a few days for me to get enough time to review this carefully. First off, I'm impressed by what you've gotten done so far, thanks for the time and genuine thought that went into this! Some thoughts to continue the discussion below:

I've used the cmd_vel twist itself to back-compute odometry and update the tf (odom-> base_link). Currently I "trust" the cmd_vel, meaning I do not check max acceleration or anything.

Why not set the global frame of everything to odom, so there's no need for localization (e.g. map->base_link). For this, you shouldn't need the planner server at all, since you're setting it with a particular path(s) for the test in a given environment. If you're running a full gazebo simulation, which I think you'd have to for this, that should be publishing out the odometry and other relevant TF frames. It would be good though to find a way to set the odometric error to 0 so that we only have the contribution due to the controller behavior and not simulated drift.

I had no other ideas to do this, a part from integrating with Gazebo, which I'd personally avoid.

If you didn't use a simulator, I'm not sure how you could have accomplished this, but perhaps what you suggest works similarly as what I'm suggesting, just without the simulator in the loop. The simulator's contribution here isn't just TF/odometry, but also taking in the commands and applying some model of vehicle dynamics (which may or may not be very good with our TB3 simulation out of the box, admittedly) rather than assuming the robot can instantaneously match whatever required velocity from the controller. Also being able to simulate sensor data for dynamic obstacle-like tasks.

That is, however, a good question of philosophy whether we'd rather benchmark in the most idealistic environment where the robot can instantaneously achieve requests to see purist behavior of a controller, or whether dynamics are required to give (as good as your robot model in the simulator is) 'realistic' outputs from the robot - minus odometric drift which I agree we should ignore for benchmarking local trajectory planners, since that trends to effectively zero in local time horizons for professional robot systems.

Metrics

Time to goal / Distance traversed / ave speed are good metrics. I think we can do some better ones though. Measuring smoothness would be a critical metric I think. As well as max / mean cost (or distance from obstacle) during the path. Storing the trajectories (like you show in rviz in the first window) in the test pickle file would be great for later analysis / plotting. That would be, I think, the right place to compute those additional metrics (at the end). Because the costmap is rolling, the zero-drift odometry would be critical (or grabbing ground truth from the simulator at the same time to be able to correlate)

On maps:

I think the maps you're using are a little too large scaled for the actual thing being measured. It seems like the robot is super tiny w.r.t. the obstacles in the map, which I'm not sure will be a representative thing to play with here. Or perhaps this needs to be run in a couple of maps: one that's large and open like this, another that's more structured and confined, and other with dynamic obstacles?

An empty map could be of some utility here too - by having a place to basically just ask the robot to follow a straightline path and measure the chatter in linear/angular velocities in the most ideal case. And/or do that experiment with a dynamic obstacle moving straight at the robot at the same speed as the robot's velocity on that straightline heading how it deals with going around collision to plot. And/or similar experiment, but perpendicular rather than parallel.

Practically I have two maps, one with less obstacle, which is used by the global costmap (when the planner at first compute the paths), and one with more obstacles, which is used by the local costmap. For some reason I don't understand, inflation layer doesn't work in this configuration and it sucks because for my understanding the DWB (but also I guess all controllers) works much better with inflation.

Does your local costmap configuration have an inflation layer defined in it? Also, do you have a link to the code?

So an alternative could be creating a fake sensors to interact with the costmap,which could be more easily dynamic, but it would take a bit of time to develop...

That's getting awfully close to simulation

First of all thank you for the great feedback and my apologies for not sharing a link to the code. Here it is: https://github.com/enricosutera/navigation2/tree/benchmark_controllers

TLDR;

I think I'll use Gazebo in the end to avoid reinventing stuff.
Whether to consider ideal condition or considering robots dynamics, may be an expansion: we could have specific metrics, e.g. robustness to delay, saturation,etc
I'll verify that I'm not the problem with the static layer

Concerning the points you underlined:

If you're running a full gazebo simulation, which I think you'd have to for this, that should be publishing out the odometry and other relevant TF frames

To be honest I wanted to keep this as much simple and light as possible, but I must admit I had (and I would have to again) reinvent a little bit the wheel by not using Gazebo and in the end it's likely going to be more "complex" than using the simulator. I'll probably go on this way and then convert to a Gazebo oriented benchmark.

For this, you shouldn't need the planner server at all [...] I agree I don't need the planner server, it actually seemed was the fastest way to do this, since I could widely reuse your previous work. I'd say this is at the end of the list among stuff to do/change: Anyway, I like the idea of being agnostic to planner.

we'd rather benchmark in the most idealistic environment where the robot can instantaneously achieve requests

I think it's debatable. It depends on what this benchmark should provide and on assumptions. If we assume the robots are well done, I'd say we shouldn't really care about taking into account robot dynamics and approximation. However I think it would be of greater interest to know that in theory a controller works better than all the others but when you actually add some saturation o a little a small (actuation?) delay another controller is more suitable. This could be explored at the end.

I think the maps you're using are a little too large scaled for the actual thing being measured

Indeed, in the second video i crop them and still are huge.

I agree on the rest of what you wrote without any comment.

I'd like to share another though.

While planning took a fraction of seconds, controls requires much more time. I've tried to use the topic /clock to speed up everything but the simple navigator API is not compliant (if I'm not wrong). Should we fix this? I can investigate.

Does your local costmap configuration have an inflation layer defined in it? Also, do you have a link to the code?

I think so: https://github.com/enricosutera/navigation2/blob/2234f17efc18b1011069460b427e71b3878b30b1/nav2_bringup/params/nav2_params.yaml#L170

I'll check again and report if it's the case. Thank you

The costmap is ordered, so the inflation has to be after the static layer for it to be applied :smile:

I don't really understand what you mean by clock or what you're trying to accomplish with that.

Thanks for the update - I'm not sure there's anything in particular otherwise I needed to respond to there?

The costmap is ordered, so the inflation has to be after the static layer for it to be applied smile

Thank you for the costmap hint. To be honest, I have never had this issue.. I was missing it.

I don't really understand what you mean by clock or what you're trying to accomplish with that.

Sorry for not being clear, I was referring to speed up simulation, for which I needed to use use_sim_time:=true and publishing on /clock appropriately. Never mind anyway, I was making a mistake.

TLDR I'm using gazebo, I've adding some metrics (see the table below). I still have to add dynamic obstacles. Maybe on that a feedback could be appreciated!

Done

Unfortunately I did not have much time. Anyway, I've done some progress.

I'm now working with Gazebo
I've got these metrics:
- Success rate ⁰
- Average linear speed (m/s)
- Average path controller length (m)
- Average time taken to reach target (s)
- Minimum distance from obstacle (based on costmap)
- Avg integrated linear (x) jerk¹
- Avg integrated angular (z) jerk¹
I've added a separated configuration file that I pass to the controller server after the nav2_params.yaml. This way the latter can remain untouched. ⁰) As a controller can fail a mission (e.g. aborting or colliding (?)), related metrics would be highly misleading and are hence excluded: for instance path length may be much shorter since the robot does not get to the goal. ¹) Concerning smoothness, there are many metrics that can be used, this is the mean squared jerk (Mean Squared jerk Wininger, Kim, & Craelius (2009)), but I must say it's quite a random choice.

root@97521177a1c7:/home/nav2_ws/src/navigation2/tools/controller_benchmarking# python3 process_data.py 
Read data
local_costmap_resolution  0.05000000074505806
Planned average len:  4.145486750635261
Total number of tasks:  4
-------------  -------  -------------------  ------------------  ----------------------  --------------------  ---------------------  ---------------------
Controller     Success  Average linear       Average controller  Average time taken(s))  Minimum distance (m)  Avg integrated x jerk  Avg integrated z jerk
               rate     speed (m/s)          path len (m)                                from obstacle         (m^2/s^6)              (m/s^6)
DWB_benchmark  2        0.20561715430471011  4.061622768315052   18.713000000000193      10000000000.0         263.7213073202777      6700.944961646043
RPP_benchmark  2        0.21526590187734002  3.9400478972390736  15.61200000000008       10000000000.0         298.39920587131576     11827.30725243777
-------------  -------  -------------------  ------------------  ----------------------  --------------------  ---------------------  ---------------------

ToDos

Dynamic obstacles

For what concerns dynamic obstacles, I have currently some issues with addding the gazebo_ros_state plugin to the world. Gazebo fails to start and I'm not sure why, since I've already used it without any troubles ( it's likely to be my poor performance setup) In any case, I was thinking to have three separated benchmarks, maybe launched in sequence:

world with no obstacle
static obstacle
dynamic obstacle
- obstacle coming toward the robot²
- random obstacles³

²) I was thinking of spawning them in the goal pose and making the move directly toward the start pose. ³) I could just spawn randomly with random twist. Given that the map is not that much large, it's still likely to have interactions

Average path controller length

I assume this is the path that the robot actually executed instead of the global path length, correct? Something like taking the robot pose every 10ms (just making up a number) and creating a "path" From that to compute distance traversed?

Make sure that when you do this that the path is static - e.g. we compute the path 1 time at the start the robot is trying to track without replanning.

Minimum and average distances (or costs) would be helpful, they tell me slightly different things. Also providing the variance or STD to that mean.

You can also just not use nav2_params.yaml and just have the param file you propose used instead alone in the launch file. That might be more straight forward. Conflict resolution of parameters in different files is not something well established in ROS 2 yet.

I think an empty world (freespace), a maze-ish world (lots of turns), and a realistic-ish world (a mix) are good baselines. Then for the empty and realistic-ish, having some obstacles not on the map (but static) that can be put in the gazebo sdf file could be helpful for how they deal with things in the way. Then additionally the dynamic obstacles not in the map would be a step up from that as well, but might involve writing a custom gazebo plugin for triggering particular actions when the robot gets to a particular spot. Or just total random :shrug:, but you mention good ideas there too with different behavioral actions for the dynamic obstacles.

Hello there. I'm aware I was off for a long time and I'm sorry I could not close this issue as quickly as I hoped .

Anyway, thank you again for all the feedback you've given.

I assume this is the path that the robot actually executed instead of the global path length, correct? Something like taking the robot pose every 10ms (just making up a number) and creating a "path" From that to compute distance traversed?

I compute at the same frequency of the feedback (of the navigator action), by means of odometry. So slightly higher than 10 ms.

Make sure that when you do this that the path is static - e.g. we compute the path 1 time at the start the robot is trying to track without replanning.

It is done this way indeed. Also, I exclude data in case of failures, when computing metrics (but for success rate).

You can also just not use nav2_params.yaml and just have the param file you propose used instead alone in the launch file. That might be more straight forward. Conflict resolution of parameters in different files is not something well established in ROS 2 yet.

Done!

At this point, to give an overview, I've got:

Metrics:
- Success rate
- Average linear speed
- Average path lenght (m)
- Average time taken (to reach target)
- Minimum distance from obstacles
- Average minimum distance from obstacles (New!)
- STD for the above mean (New!)
- Average Integrated linear x jerk
- Average Integrated angular z jerk
Worlds (Gazebo):
- Empty world
- Empty world with obstacles moving toward robot*
- Maze-like world (New!)
Usage:
- I've done three separated bring-up launch files for empty world, empty world with obstacles and and maze-like static environment.

*) It can be enabled via a ros parameter. A cylinder is spawned at goal position and starts moving towards the robot . It stop when at a certain distance from the robot. Obstacle speed and stop distance are defined at the top of the script. Some notes:

Both DWB and RRP, as expected, simply stop and fail in the scenario this obstacles, with default configuration.

As for the realistic-ish world, as you suggest, maybe it's simply better to explain in documentation how to add a world, since it's likely that a user is going to benchmark in its own environment.

In terms of functionalities I think we're there, aren't we? Anything else? Of course I've still to do some test and fix something here and there.

The maze

https://user-images.githubusercontent.com/51802424/222260143-2e0785dc-89a1-4e35-bdc2-8d6338129db9.mp4

Dynamic obstacles

https://user-images.githubusercontent.com/51802424/222266108-f6161517-5d77-4797-8279-ad84e18bb12c.mp4

The metrics:

    Controller       Success    Average linear    Average controller    Average time     Min dist (m)    --> avg (m)    --> std (m)    Avg integrated x jerk    Avg integrated z jerk
                        rate       speed (m/s)         path len (m)        taken(s)     from obstacle                                              (m^2/s^6)                  (m/s^6)
--  -------------  ---------  ----------------  --------------------  --------------  ---------------  -------------  -------------  -----------------------  -----------------------
 0  DWB_benchmark      1.000             0.020                 1.382          67.511            0.000          1.414          0.440                    0.076                    0.752
 1  RPP_benchmark      0.000             0.000                 0.000           0.000            0.000          0.648          0.145                    0.000                    0.000

Hi, sorry for the delay here, but I'm in an odd situation and I haven't had cycles to get to this which requires a good chunk of time to refamiliarize myself and review your progress - apologies. But this all looks really good!

ros-navigation / navigation2