Improve lift and door marker display for multi-level nav-graphs

[Kevinskwk]

This PR improves the lift and door marker display for multi-level nav-graphs. For door markers, only doors on the current floor being monitored will be shown. Lift markers' color scheme has been changed and uses green and blue to represent open and not open lift cabins. The lift markers will appear transparent when the corresponding cabins are moving or not on the currently monitored floor.

[Yadunund]

Thanks for getting started with this enhancement. I think the deletion process can be a bit more elegant.

1. Change self.door_states to nested dictionary and initialized with self.door_states[self.map_name] = {}. In door_cb() if msg.door_name is not found in self.building_doors[self.map_name], we can return from the function. We then update the message in self.door_states[self.map_name].
2. Have another dictionary self.active_markers (door name : marker) to to cache markers after they are published (door and text markers). In param_cb() if a valid map name is received, pop markers from this dictionary, set the marker.action to DELETE and publish. Clear self.door_states[self.map_name] & self.lift_states. Update self.map_name. Clearing the states will automatically publish the latest door & lift markers in the subsequent callback.

Let me know what you think.

[Yadunund]

Suggest to use variable name map_name instead for consistency with rmf_schedule_visualizer. Further, let's not hardcode the value. Instead this can be passed to the node constructor. The main() function can accept a -m map name argument. This way when the visualizer.xml is launched, the same map_name can be passed as an argument for this executable. We can extend the same logic to fleet_state_visualizer in the future.

rmf_schedule_visualizer/visualizer/launch/visualizer.xml

Line 12 in 54c96de

<node pkg="rmf_schedule_visualizer" exec="rviz2" args="-r $(var rate) -m $(var map_name)">

[Yadunund]

This variable can become redundant with changes to param_cb()

[Yadunund]

We can cache the BuildingMap msg in self.building_map in the map_cb(). Here update self.map_name if msg.map_name is a valid level name in self.building_map. This update can happen after publishing the deletion markers

[Kevinskwk]

1. Change self.door_states to nested dictionary and initialized with self.door_states[self.map_name] = {}. In door_cb() if msg.door_name is not found in self.building_doors[self.map_name], we can return from the function. We then update the message in self.door_states[self.map_name].

Thanks for the suggestions. While I don't think this point here is necessary. In this case, only the door states in the current map (floor) are updates. Since the frequency of door_state publishing is 1Hz, occasions might occur when users change the map to view during the door_state publishing interval. Then the door state in that new map will not be the latest until the next publishing & update.

[Kevinskwk]

Thanks for your comments. I have finished refactoring the code. I've also implemented the feature to only display robot markers in the current map. In addition, Inputting invalid map name will now keep the original map instead of showing nothing.

Please let me know if any further changes are required.

[Kevinskwk]

In building_systems_visualizer.py, I implemented a append_door_markers() function because in param_cb(), publishing each marker alone in one marker array msg will cause problems like markers not being deleted cleanly. I'm not very sure about the reason but sending multiple markers in one marker array solves the problem.

[Yadunund]

Thanks for making the requested changes. I think we're missing exec dependencies for building_map_msgs and rmf_schedule_visualizer_msgs packages in the package.xml of fleet_state_visualizer. After that, we should be good to merge.

[Yadunund]

Suggested change

	# clearing door states and lift states to that markers can be updated
	# clearing door states and lift states so that markers can be updated

[Kevinskwk]

Thanks for noticing, previously it didn't give any error so I forgot about the dependencies. Anyway, I have fixed the issues, and thanks for reviewing for me!