Lane closure (#13)

Posted by @txlei:

Thank you for providing this new feature. I have just tested it out using the older office map. Works as intended but like to ask the following:

  1. there’s a caveat that the feature is only relevant for legacy fleet adapter (aka the full_control adapter). Can I clarify if it meant that fleets using full control implementation via the rmf_fleet_adapter C++ API, this feature cannot be used?
  2. Specifically, the lane closure closes the lane segment excluding its start and end waypoints. Is this explicit in the code implementation of this feature?
  3. Would there be any planning challenge if lane is closed when there are existing robot tasks (i.e. robot replanning) versus lane closure before any submission of robot tasks (i.e. first-time path planning ard closed lane). I have tested that both scenarios works fine but just like to ask if there are anything to take note.
  4. Currently is there any API to get a list of fleet_name for fleets “enrolled”?
Chosen answer

Answer chosen by @Yadunund at 2021-04-01T02:58:14Z.
Answered by @mxgrey:

there’s a caveat that the feature is only relevant for legacy fleet adapter (aka the full_control adapter)

That caveat only applies to the rmf_fleet_msgs::LaneRequest message API.

Can I clarify if it meant that fleets using full control implementation via the rmf_fleet_adapter C++ API, this feature cannot be used?

There is still a lane closure feature for the rmf_fleet_adapter C++ API for everyone who is developing a fleet adapter off of that, but you need to use these functions, and you should also remember to check whether the lane closure is relevant to any of your robots and trigger the RobotUpdateHandle::interrupted function on any that need to be rerouted. If your robot is in the middle of the lane that needs to be closed, then before triggering interrupted, you should also use one of these update_position calls to make sure the planner will reroute it the way you want.

I think the way all of this is implemented in the legacy fleet adapter [1] [2] is pretty reasonable, so you could use that as a reference point for how to make the lane closing work for your own custom fleet adapter.

Specifically, the lane closure closes the lane segment excluding its start and end waypoints. Is this explicit in the code implementation of this feature?

Right, closing a lane just means the planner will not attempt to use that lane to move the robot from the lane’s start waypoint to its end waypoint. If you want to close off a specific waypoint entirely, then closing every lane that leads to that waypoint would have the desired effect.

Would there be any planning challenge if lane is closed when there are existing robot tasks (i.e. robot replanning) versus lane closure before any submission of robot tasks (i.e. first-time path planning ard closed lane).

The only difference is that in the replanning case, the robot will take longer to finish its task than it originally predicted. If the lane closure happens before a task is submitted, then the fleet manager will know to account for the lane closure when estimating the task completion time.

Currently is there any API to get a list of fleet_name for fleets “enrolled”?

This is sort of an initial rough draft of the feature, so we haven’t firmed up what the “Standard Practices” should be regarding lane closure. I kind of expect different vendors or system integrators will have their own preferences about how lane closures should be handled. But if you want to work off of the ROS2 approach that we’re using for the legacy fleet adapter, you could infer which fleets will probably support the rmf_fleet_msgs::msg::LaneRequest API based on which ones are publishing rmf_fleet_msgs::msg::ClosedLanes messages to the "closed_lanes" topic. The ClosedLanes message contains the fleet name of each publisher, so if you gather up all the fleet names publishing to that topic, then there’s your list.

Posted by @mxgrey:

there’s a caveat that the feature is only relevant for legacy fleet adapter (aka the full_control adapter)

That caveat only applies to the rmf_fleet_msgs::LaneRequest message API.

Can I clarify if it meant that fleets using full control implementation via the rmf_fleet_adapter C++ API, this feature cannot be used?

There is still a lane closure feature for the rmf_fleet_adapter C++ API for everyone who is developing a fleet adapter off of that, but you need to use these functions, and you should also remember to check whether the lane closure is relevant to any of your robots and trigger the RobotUpdateHandle::interrupted function on any that need to be rerouted. If your robot is in the middle of the lane that needs to be closed, then before triggering interrupted, you should also use one of these update_position calls to make sure the planner will reroute it the way you want.

I think the way all of this is implemented in the legacy fleet adapter [1] [2] is pretty reasonable, so you could use that as a reference point for how to make the lane closing work for your own custom fleet adapter.

Specifically, the lane closure closes the lane segment excluding its start and end waypoints. Is this explicit in the code implementation of this feature?

Right, closing a lane just means the planner will not attempt to use that lane to move the robot from the lane’s start waypoint to its end waypoint. If you want to close off a specific waypoint entirely, then closing every lane that leads to that waypoint would have the desired effect.

Would there be any planning challenge if lane is closed when there are existing robot tasks (i.e. robot replanning) versus lane closure before any submission of robot tasks (i.e. first-time path planning ard closed lane).

The only difference is that in the replanning case, the robot will take longer to finish its task than it originally predicted. If the lane closure happens before a task is submitted, then the fleet manager will know to account for the lane closure when estimating the task completion time.

Currently is there any API to get a list of fleet_name for fleets “enrolled”?

This is sort of an initial rough draft of the feature, so we haven’t firmed up what the “Standard Practices” should be regarding lane closure. I kind of expect different vendors or system integrators will have their own preferences about how lane closures should be handled. But if you want to work off of the ROS2 approach that we’re using for the legacy fleet adapter, you could infer which fleets will probably support the rmf_fleet_msgs::msg::LaneRequest API based on which ones are publishing rmf_fleet_msgs::msg::ClosedLanes messages to the "closed_lanes" topic. The ClosedLanes message contains the fleet name of each publisher, so if you gather up all the fleet names publishing to that topic, then there’s your list.


Edited by @mxgrey at 2021-03-26T09:38:25Z


This is the chosen answer.

Posted by @txlei:

There seemed be a path request msg sent on a fixed interval telling the robots to go to its default start point i.e. charger. I am not sure whether this issue happened after I included the lane closure feature and its related PR.

/robot_path_request topic. each robot having 2 path location sent though its the same xy coordinate

fleet_name: tinyRobot
robot_name: tinyRobot1
path:
- t:
    sec: 4479
    nanosec: 416000000
  x: 11.55336856842041
  y: -11.315971374511719
  yaw: 1.5609171390533447
  level_name: L1
  index: 0
- t:
    sec: 4509
    nanosec: 416000000
  x: 11.55336856842041
  y: -11.315971374511719
  yaw: 1.5609171390533447
  level_name: L1
  index: 0
task_id: '150'
---
fleet_name: tinyRobot
robot_name: tinyRobot2
path:
- t:
    sec: 4479
    nanosec: 416000000
  x: 15.15718936920166
  y: -11.227091789245605
  yaw: 1.5659936666488647
  level_name: L1
  index: 0
- t:
    sec: 4509
    nanosec: 416000000
  x: 15.15718936920166
  y: -11.227091789245605
  yaw: 1.5659936666488647
  level_name: L1
  index: 0
task_id: '150'
---

repeating logs

[gzserver-13] [INFO] [1617003413.878577247] [slotcar_tinyRobot2]: tinyRobot2 reached waypoint 1/1
[gzserver-13] [INFO] [1617003413.878653259] [slotcar_tinyRobot2]: tinyRobot2 reached goal -- rotating to face target
[gzserver-13] [INFO] [1617003413.878676199] [slotcar_tinyRobot1]: tinyRobot1 reached waypoint 1/1
[gzserver-13] [INFO] [1617003413.878684987] [slotcar_tinyRobot1]: tinyRobot1 reached goal -- rotating to face target
[gzserver-13] [INFO] [1617003414.038742329] [slotcar_tinyRobot1]: tinyRobot1 received a path request with 2 waypoints
[gzserver-13] [INFO] [1617003414.039590121] [slotcar_tinyRobot2]: tinyRobot2 received a path request with 2 waypoints
[gzserver-13] [INFO] [1617003414.538097832] [slotcar_tinyRobot1]: 76 already received task [tinyRobot1] -- continuing as normal

what is happening? I suspect it is related to the update_position calls? This isn’t a serious issue but would it have any other implications?

Posted by @mxgrey:

That’s an intentional behavior introduced by the ResponsiveWait feature. That change was introduced independently of the lane closure feature. It was actually motivated by your issue report last month, which should now be fixed by this new behavior.

In the long-term we will replace the responsive waiting behavior with an automatic parking reservation system, but the parking system will take some time to develop as it is significantly more complex to implement than the responsive waiting.