Adding support for lane level speed limits

[luca-della-vedova]

Feature request

Description

There are applications in which different traffic lanes would have different speed limits and vehicles are not expected to always move as fast as possible. For example, if we wanted to simulate road networks we would expect larger roads (i.e. highways) to have higher speed limits than smaller roads.

Implementation considerations

We would need to implement a new parameter at the lane level to save the speed limit and take into account the maximum allowed speed in the heuristics when finding the best path to a place. For a complete RMF demonstration followup PRs would be needed in different packages in the stack:

• rmf_traffic to implement the functionality at the planning level. Add lane speed limits to the graph and factor them into the planning #44
• rmf_traffic_editor to add the parameter to the GUI, as well as adding the capability in rmf_building_map_tools to add it to the generated nav graphs. Add speed limit to navgraph rmf_traffic_editor#397
• rmf_ros2, specifically in the parsing functions, to parse it from the nav graph and pass it to rmf_traffic Add lane speed limit to graph parsing function rmf_ros2#124.
• rmf_ros2, get the fleet adapter to publish the speed limit to slotcar for navigation Add speed limit publishing to full control fleet adapter rmf_ros2#132
• rmf_internal_msgs, to add a maximum speed to location messages Add approach_speed field to Location message rmf_internal_msgs#28
• rmf_simulation to add support in slotcar to respect the speed limit of the specific lane. We could either add a field to the PathRequest or calculate in the plugin the maximum speed, interpolating between the start and end time of every lane. Follow speed limits for slotcar robots rmf_simulation#56
• rmf_demos to add a sample world where robots can travel at different speeds (i.e. a narrow / wide corridor, or ackermann steering vehicles on different roads). TODO

These PRs go as a set, to test them in any demo edit the map, set a different speed limit (lower than the maximum robot speed or it won't have any effect) for a lane from the traffic_editor GUI then run the demo, you should see the planner taking it into account (i.e. prioritizing lanes with higher speed limits) and the slotcar vehicle respecting it.
Specifically, with the following office demo (all default except the highlighted lane set to 0.1):

With the yaml file here, the behavior when asking for coke delivery changes in the following way:

• If a first request is submitted, the robot on the left hand side will go for it since the robot on the right hand side needs to go through a lane with much lower speed limit that will make its task execution be slower.
• If a second request is submitted immediately afterwards, the robot on the right hand side will start to navigate towards the pantry and while going through the low speed lane it will go at 0.1 m/s instead of its nominal drive speed of 0.5 m/s so you should see a noticeable slowdown.

Example of the behavior is below

tinyrobot_speed_limit.mp4

Without the PR the robot on the right side always executes the task first and all the robots navigate at maximum speed at all times.