How to inform RMF that lift or door are not available? (#414)

Archive-o-matic · January 13, 2024, 1:42pm

Posted by @cwrx777:

Sometimes lifts are not available due to servicing/maintenance, etc and should not be used in the traffic planning.
How to inform RMF that lifts, or even doors are not available without reconfiguring the adapters config file and restarting RMF?

Archive-o-matic · January 13, 2024, 3:36pm

Posted by @mxgrey:

I would recommend using the lane closing feature.

Identify which lanes enter the lift or pass through the door and set them as closed.

Archive-o-matic · January 14, 2024, 8:15am

Posted by @cwrx777:

Hi @mxgrey ,

How does lane closing feature work?
In the rmf demos fleet adapter, there is a closed_lane publisher and lane_closed_requests subscriber .
Who is supposed to publish lane_closed_requests and when is it supposed to be published? and what is the result of fleet adapter publishing closed_lane ?

Archive-o-matic · January 14, 2024, 9:33am

Posted by @mxgrey:

The API is designed to let you make all of these decisions about when and how to call this feature. As a system integrator you’ll know best what flow of information will make the most sense for your application.

In the demo fleet adapter we use ROS 2 topics because it already has a ROS 2 dependency, and as developers it’s easy to ros2 topic pub a message from the command line for testing.

In your application maybe your fleet adapter should receive a REST API request to open/close a door/lift, then you can use the nav graph information to identify which lanes are associated with that door/lift and then trigger the rmf_fleet_adapter API to open or close those lanes.

Archive-o-matic · January 24, 2024, 6:53am

Posted by @cwrx777:

Hi @mxgrey ,

I am thinking that the flow would be as follows:

flowchart TD
    A([lift_adapter]) -->|lift_states| B([Node#1])
    B -->|lane_closure_requests| C([fleet_adapter])
    C -->|lane_states| D([visualizer])
    C -->|closed_lanes| E([Node#2])

lift adapter will get the lift status from the lift system and publish lift_states with status = OFFLINE.
Another node (Node#1) that subscribers the lift_states topic and publishes lane_closure_requests .
Fleet adapter subscribes to lane_closure_requests topic. it calls fleet adapted API and publishes closed_lanes topic.
fleet adapter publishes lane_states, which are used by visualizer. I notice lane_states and closed_lanes are very similar.

Questions:

The lane_closure_request has fleet_name. How can I get the fleet names from the lift name?
The lane_closure_requests also has open_lanes or close_lanes array. How do I get this info?
What should subscribe (Node#2) to the closed_lanes topic?

Archive-o-matic · January 24, 2024, 9:22am

Posted by @mxgrey:

Another node (Node#1) that subscribers the lift_states topic and publishes lane_closure_requests .

This node would have to know about the graph and existing fleets in order to make decisions about what lanes to close and what fleets to command.

Instead of doing it like this, I’d suggest having the Python side of your fleet adapter subscribe to lift_states and look for the OFFLINE message, then use its access to the nav graph to close the correct lanes and call the close_lanes API directly.

Archive-o-matic · January 24, 2024, 9:37am

Posted by @cwrx777:

@mxgrey

I’m referencing this code snippet as starting point. I believe the index is within the range of graph.num_lanes. But how do I identify which lane index to close?
I tried to check the waypoint name of lane.entry and lane.exit and check if it contain the lift names, but it seems that waypoints inside the lift do not have any name.

Archive-o-matic · January 24, 2024, 10:07am

Posted by @mxgrey:

Put some logic into these functions to mark when the event listener has come across a lane that passes into or through a lift, e.g.

def lift_door_open_execute(self, lift_door_open):
    if lift_door_open.lift_name == self.relevant_lift_name:
        self.close_lane = True

def lift_door_close_execute(self, lift_door_close):
    if lift_door_close.lift_name == self.relevant_lift_name:
        self.close_lane = True

def lift_move_execute(self, lift_move):
    if lift_move.lift_name == self.relevant_lift_name:
        self.close_lane = True

Then after you call execute check your executor to see if self.close_lane is True or False.

Archive-o-matic · January 25, 2024, 3:45am

Posted by @cwrx777:

Hi @mxgrey ,

I put the following EventListener in fleet_adapter.py, before the initialize_fleet

EventListener

class EventListener(graph.lane.Executor):
    def __init__(self, event, name, lane_index, node):
        graph.lane.Executor.__init__(self)

        self.event = event
        self.name = name
        self.lane_index = lane_index
        self.node = node

    def lift_door_open_execute(self, lift_door_open):
        self.node.get_logger().info(f'[{self.event}][{self.name}][{self.lane_index}]-lift_door_open_execute [{lift_door_open.lift_name}]')
        return

    def lift_door_close_execute(self, lift_door_close):
        self.node.get_logger().info(f'[{self.event}][{self.name}][{self.lane_index}]-lift_door_close_execute [{lift_door_close.lift_name}]')
        return

    def lift_session_begin_execute(self, lift_session_begin):
        self.node.get_logger().info(f'[{self.event}][{self.name}][{self.lane_index}]-lift_session_begin_execute [{lift_session_begin.lift_name}]')
        return

    def lift_session_end_execute(self, lift_session_end):
        self.node.get_logger().info(f'[{self.event}][{self.name}][{self.lane_index}]-lift_session_end_execute [{lift_session_end.lift_name}]')
        return

    def lift_move_execute(self, lift_move):
        self.node.get_logger().info(f'[{self.event}][{self.name}][{self.lane_index}]-lift_move_execute [{lift_move.lift_name}]')
        return

and the following code here, after nav_graph is set.

Traverse lanes

for i in range(nav_graph.num_lanes):
    lane = nav_graph.get_lane(i)

    entry_waypoint = nav_graph.get_waypoint(lane.entry.waypoint_index)
    exit_waypoint = nav_graph.get_waypoint(lane.exit.waypoint_index)

    if lane.entry.event:
        executor = EventListener(f'enter',entry_waypoint.waypoint_name, i, node)
        try:
            lane.entry.event.execute(executor)
        except Exception as ex:
            node.get_logger().error(f'setting entry event executor {ex}')

    if lane.exit.event:
        executor = EventListener(f'exit',exit_waypoint.waypoint_name, i, node)
        try:
            lane.exit.event.execute(executor)
        except Exception as ex:
            node.get_logger().error(f'setting exit event executor {ex}')

and the output is as follows:

Output

[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][0]-lift_door_open_execute [lift_SL06]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [exit][None][0]-lift_session_end_execute [lift_SL06]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][1]-lift_session_begin_execute [lift_SL06]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][4]-lift_door_open_execute [lift_SL07]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [exit][None][4]-lift_session_end_execute [lift_SL07]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][5]-lift_session_begin_execute [lift_SL07]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][16]-lift_door_open_execute [lift_SL06]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [exit][None][16]-lift_session_end_execute [lift_SL06]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][17]-lift_session_begin_execute [lift_SL06]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][18]-lift_door_open_execute [lift_SL07]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [exit][None][18]-lift_session_end_execute [lift_SL07]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][19]-lift_session_begin_execute [lift_SL07]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][40]-lift_move_execute [lift_SL07]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][41]-lift_move_execute [lift_SL07]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][42]-lift_move_execute [lift_SL06]
[fleet_adapter-4] [DeliveryRobot_command_handle][INFO] [enter][None][43]-lift_move_execute [lift_SL06]

From the output I notice that lift_name are associated to multiple lane indexes.
From which event the lane index that needs to be closed for each lift_name?

Archive-o-matic · January 27, 2024, 4:06am

Posted by @cwrx777:

My latest code:

EventListener


class EventListener(graph.lane.Executor):
    def __init__(self, event, lane_index, dock_to_wp, lift_lane_index, entry_waypoint, exit_waypoint, node):
        graph.lane.Executor.__init__(self)

        self.event = event
        self.entry = entry_waypoint
        self.exit = exit_waypoint
        self.lane_index = lane_index
        self.dock_to_wp = dock_to_wp
        self.lift_lane_index = lift_lane_index
        self.node = node
        
    def dock_execute(self, dock):
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-dock_execute [{dock.dock_name}] [{dock.duration}]')
        
        #to capture dock_name to exit waypoint
        self.dock_to_wp[dock.dock_name] = self.exit 
        return

    def wait_execute(self, wait):
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-wait_execute [{wait.duration}]')
        return

    def door_open_execute(self, door_open):            
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-door_open_execute [{door_open.name}] [{door_open.duration}]')
        return

    def door_close_execute(self, door_close):
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-door_close_execute [{door_close.name}] [{door_close.duration}]')
        return

    def lift_door_open_execute(self, lift_door_open):
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-lift_door_open_execute [{lift_door_open.lift_name}]')
        return

    def lift_door_close_execute(self, lift_door_close):
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-lift_door_close_execute [{lift_door_close.lift_name}]')
        return

    def lift_session_begin_execute(self, lift_session_begin):
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-lift_session_begin_execute [{lift_session_begin.lift_name}]')
        if self.lift_lane_index.get(lift_session_begin.lift_name, None) is None:
            self.lift_lane_index[lift_session_begin.lift_name] = []
        self.lift_lane_index[lift_session_begin.lift_name].append(self.lane_index)
        return

    def lift_session_end_execute(self, lift_session_end):
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-lift_session_end_execute [{lift_session_end.lift_name}]')
        if self.lift_lane_index.get(lift_session_end.lift_name, None) is None:
            self.lift_lane_index[lift_session_end.lift_name] = []
        self.lift_lane_index[lift_session_end.lift_name].append(self.lane_index)
        return

    def lift_move_execute(self, lift_move):
        self.node.get_logger().info(f'[{self.event}][{self.entry.waypoint_name}]=>[{self.exit.waypoint_name}][{self.lane_index}]-lift_move_execute [{lift_move.lift_name}]')
        return

Traverse lanes

dock_to_wp = {}
lift_lane_index = {}
    
for i in range(nav_graph.num_lanes):
    lane = nav_graph.get_lane(i)
    entry_waypoint = nav_graph.get_waypoint(lane.entry.waypoint_index)
    exit_waypoint = nav_graph.get_waypoint(lane.exit.waypoint_index)
    # lane.entry and lane.exit are a Lane::Node wrappers
    if lane.entry.event:
        executor = EventListener(f'enter', i, dock_to_wp, lift_lane_index, entry_waypoint, exit_waypoint, node)
        try:
            lane.entry.event.execute(executor)
        except Exception as ex:
            node.get_logger().error(f'setting entry event executor {ex}')

    if lane.exit.event:
        executor = EventListener(f'exit', i, dock_to_wp, lift_lane_index, entry_waypoint, exit_waypoint, node)
        try:
            lane.exit.event.execute(executor)
        except Exception as ex:
            node.get_logger().error(f'setting exit event executor {ex}')

_lift_state_sub_cb

def _lift_state_sub_cb(msg: LiftState):
    if msg.current_mode == LiftState.MODE_OFFLINE:

        lift_name = msg.lift_name          
        node.get_logger().info(
                    f"Lift [{msg.lift_name}] is [{msg.current_mode}]. publishing close lane requests")           
        lane_closure_requests_msg =  LaneRequest()
        lane_closure_requests_msg.fleet_name = fleet_name
        lane_closure_requests_msg.close_lanes = []
        lane_closure_requests_msg.close_lanes.extend(lift_lane_index[lift_name])            
        lane_closure_requests_pub.publish(lane_closure_requests_msg)

And I modified the lift plugin so that the state it is reporting can be overridden, here using this message.

I can see that when the lanes are closed, it will be shown as below in rviz.

My issue now, when there are no lifts available, when there is a task that requires using a lift, the fleet adapter crashed. No issue if the task does not require lift.

Error Log (for dispatch task request)

[rmf_task_dispatcher-8] [rmf_dispatcher_node][INFO] Add Task [patrol.dispatch-0] to a bidding queue
[rmf_task_dispatcher-8] [rmf_dispatcher_node][INFO]  - Start new bidding task: patrol.dispatch-0
[fleet_adapter-4] [DeliveryRobot_fleet_adapter][INFO] [Bidder] Received Bidding notice for task_id [patrol.dispatch-0]
[fleet_adapter-4] [DeliveryRobot_fleet_adapter][INFO] Planning for [4] robot(s) and [1] request(s)
[fleet_adapter-4] [DeliveryRobot_fleet_adapter][INFO] Submitted BidProposal to accommodate task [patrol.dispatch-0] by robot [DeliveryRobot_02] with new cost [196.601681]
[rmf_task_dispatcher-8] [rmf_dispatcher_node][INFO] Determined winning Fleet Adapter: [DeliveryRobot], from 1 responses
[rmf_task_dispatcher-8] [rmf_dispatcher_node][INFO] Dispatcher Bidding Result: task [patrol.dispatch-0] is awarded to fleet adapter [DeliveryRobot], with expected robot [DeliveryRobot_02].
[fleet_adapter-4] [DeliveryRobot_fleet_adapter][INFO] Bid for task_id [patrol.dispatch-0] awarded to fleet [DeliveryRobot]. Processing request...
[fleet_adapter-4] [DeliveryRobot_fleet_adapter][INFO] Beginning next task [patrol.dispatch-0] for robot [DeliveryRobot/DeliveryRobot_02]
[ERROR] [fleet_adapter-4]: process has died [pid 186718, exit code -11, ...].

Error Log (for direct task request)

fleet_adapter-4] [DeliveryRobot_fleet_adapter][INFO] Direct request [patrol_d316d645-8b5c-4f9a-b561-84cddf8f46a3] successfully queued for robot [DeliveryRobot_01]
[fleet_adapter-4] [DeliveryRobot_fleet_adapter][INFO] Beginning next task [patrol_d316d645-8b5c-4f9a-b561-84cddf8f46a3] for robot [DeliveryRobot/DeliveryRobot_01]
[ERROR] [fleet_adapter-4]: process has died [pid 190405, exit code -11, ...].

Archive-o-matic · February 7, 2024, 8:43am

Posted by @mxgrey:

Sorry for the delay in following up on this. The crashing is obviously wrong behavior. The intended behavior is for the fleet adapter to keep periodically retrying and log an error until a plan can be found.

You must be hitting an edge case that we haven’t come across yet. If you’re able to, it would be very helpful if you can run the fleet adapter with gdb to pinpoint the cause of the crash (which I’m assuming is a segmentation fault). You can refer back to here for instructions on running the fleet adapter with gdb. Be sure to recompile the fleet adpater in debug mode by running

$ colcon build --packages-select rmf_traffic rmf_traffic_ros2 rmf_fleet_adater --cmake-args -DCMAKE_BUILD_TYPE=Debug

in your colcon workspace.

Archive-o-matic · February 7, 2024, 3:23pm

Posted by @cwrx777:

hi @mxgrey

based on the stack trace: (first line is last executed)

Archive-o-matic · February 26, 2024, 10:21am

Posted by @cwrx777:

@mxgrey
Hi,
I’m wondering if you are able to replicate this issue and provide a fix. in our upcoming deployment, there’ll be only one lift for a particular building and this lift will be shared with AGV. when it’s being used by AGV, I intend to use the close lane feature.

Archive-o-matic · February 26, 2024, 11:24am

Posted by @mxgrey:

I haven’t replicated but I can roughly see how the problem can happen based on your stack trace. I can also see an easy solution that I can get in quickly, although after that I’ll need to think carefully about whether there are any other side effects to watch out for.

Archive-o-matic · February 27, 2024, 5:22pm

Posted by @mxgrey:

I still haven’t recreated the exact stack trace you logged, but I did manage to recreate a very similar one under similar circumstances.

I’ve opened this PR which should fix this whole category of problem. You can try it out right away by checking out the branch, but I expect it to be merged into main within a day.

Topic		Replies	Views
Lane closure (#13) Open-RMF General rmf-github-discuss	3	4	March 29, 2021
Lift session issues (#454) Open-RMF General rmf-github-discuss	11	8	August 21, 2024
Sending robot back to liftlobby before boarding lift (#536) Open-RMF General rmf-github-discuss	10	4	October 3, 2024
Lanes in Free Fleet (#675) Open-RMF General rmf-github-discuss	24	6	June 4, 2025
Registering a New Node as Part of the TinyRobot Fleet Adapter (#35) Open-RMF General rmf-github-discuss	6	0	May 9, 2022

How to inform RMF that lift or door are not available? (#414)

Related topics