Load the parking waypoint into RobotContext; access it in adapter and FleetUpdateHandle

[CarlyyyChen]

Bug fix

Fixed bug

Currently if we set the parking waypoint of a robot to a waypoint that is not a charger, and configure the finishing request as "park", the robot will still go back to its charger, instead of its parking spot, after finishing a task. This bug is raised in issue open-rmf/rmf_demos#219 and open-rmf/rmf_task#115.

Fix applied

This issue happens because when we generate a parking request for the robot, we did not pass in a parking spot waypoint, and therefore the robot will head to its charger as default. My approach is the similar as suggested by Yadunund in open-rmf/rmf_task#115 (the only difference is because I am on the humble branch instead of the main branch).

Basically, the Fleet adapter will read the dedicated parking waypoint for each robot from the configuration file, pass that waypoint to RobotCommandHandle and RobotUpdateHandle so that this waypoint will be set as the dedicated parking waypoint for each robot. When sending requests to robots, fleet adapter needs to know each robot's State, which can be retrieved through RobotContext. The robots' current state with a dedicated parking waypoint will be sent back to the fleet adapter. When creating finishing request, if the finishing request is configured as "park", fleet adapter will call the ParkRobotFactory class to use that parking waypoint as a finishing waypoint.

To implement the fix, I need to make changes to three repos: rmf_demos, rmf_task, and rmf_ros2. And therefore I have three pull requests. Please review them together.

[mxgrey]

Hi, @CarlyyyChen thanks for participating in Open-RMF and submitting this PR.

After thinking it over carefully internally, we settled on taking a more general approach to this problem which has been merged in #379.

The root issue that created problems for you wasn't really the absence of a dedicated parking spot concept. The root issue was that every robot in a fleet is forced to have the same finishing task generator. This is an unnecessary limitation and may stifle many different strategies that users may want to exercise for defining the idle behavior of their robots.

In #379 we introduced an API that allows each robot to be assigned its own separate finishing task generator. With this you can assign each robot its own parking spot and change that parking spot at any time using RobotUpdateHandle::set_finishing_request[C++][Python].

With that feature merged, I'll be closing this PR. I understand that you may still be using ROS Humble, so I'll mention that the main branches of all the core Open-RMF libraries are still compatible with ROS humble, but they need to be built from source instead of pulled from the ROS build farm.