Add ecobot lidar map to office floor plan

rmf_demos/launch/include/office/office.rviz

@@ -1,13 +1,14 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 138
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /MarkerArray3/Topic1
         - /MarkerArray4/Topic1
+        - /Map2
       Splitter Ratio: 0.5
-    Tree Height: 270
+    Tree Height: 205
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -53,7 +54,7 @@ Visualization Manager:
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        fleet_markers: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -65,8 +66,7 @@ Visualization Manager:
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        participant 0: true
-        participant location 0: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -91,20 +91,15 @@ Visualization Manager:
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        coe_door: true
-        coe_door_text: true
-        hardware_door: true
-        hardware_door_text: true
-        main_door: true
-        main_door_text: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Transient Local
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /building_systems_markers
       Value: true
-    - Alpha: 0.7
+    - Alpha: 0.699999988079071
       Class: rviz_default_plugins/Map
       Color Scheme: map
       Draw Behind: true
@@ -113,6 +108,7 @@ Visualization Manager:
       Topic:
         Depth: 5
         Durability Policy: Transient Local
+        Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /floorplan
@@ -123,7 +119,28 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /floorplan_updates
       Use Timestamp: false
-      Value: true      
+      Value: true
+    - Alpha: 0.25
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /ecobot_office/map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /ecobot_office/map_updates
+      Use Timestamp: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -139,6 +156,9 @@ Visualization Manager:
     - Class: rviz_default_plugins/Measure
       Line color: 128; 128; 0
     - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -176,29 +196,29 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 95.94175720214844
+      Scale: 57.53582763671875
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz_default_plugins)
       X: 14.163299560546875
       Y: -7.102552890777588
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
+    collapsed: true
   DoorPanel:
-    collapsed: false
-  Height: 1726
-  Hide Left Dock: false
+    collapsed: true
+  Height: 1016
+  Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000033400000622fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000006e000001910000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc0000006e000006220000062201000039fa000000020100000003fb0000001a005300630068006500640075006c006500500061006e0065006c0100000000ffffffff0000028300fffffffb000000120044006f006f007200500061006e0065006c0100000000ffffffff0000029300fffffffb000000120052004d0046002000500061006e0065006c0100000000000003340000033400ffffff000000010000015f00000765fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007650000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000006440000062200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001a900000392fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000004300000115000000d400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc0000015e000002770000000000fffffffa000000020100000003fb0000001a005300630068006500640075006c006500500061006e0065006c0000000000ffffffff000001a900fffffffb000000120044006f006f007200500061006e0065006c0000000000ffffffff0000016a00fffffffb000000120052004d0046002000500061006e0065006c0100000000000003340000000000000000000000010000015f00000765fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e00000765000000b500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000007380000039200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   SchedulePanel:
-    collapsed: false
+    collapsed: true
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2436
-  X: 4064
-  Y: 116
+  Width: 1848
+  X: 72
+  Y: 27

rmf_demos/launch/map_server.launch.py

@@ -0,0 +1,130 @@
+#!/usr/bin/env python3
+
+# Copyright 2022 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import launch
+import launch_ros
+import lifecycle_msgs.msg
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+# supply args: map_name, transform [tx, ty, yaw]
+# if use_launch_config, this means the input param is from cli launch arg
+def create_map_server(
+    ld,
+    map_name,
+    tx,
+    ty,
+    yaw,
+    world_name,
+    use_launch_config=False,
+):
+    # static pub mode
+    tf_pub_node = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name=f"static_transform_publisher",
+        namespace=map_name,
+        arguments=[tx, ty, '0', yaw, '0', '0', 'map', map_name])
+
+    pkg_share_dir = get_package_share_directory('rmf_demos_maps')
+
+    if use_launch_config:
+        yaml_filename = \
+            [pkg_share_dir, "/", world_name, "/", map_name, ".yaml"]
+    else:
+        yaml_filename = f"{pkg_share_dir}/{world_name}/{map_name}.yaml"
+
+    # map server node
+    map_server_node = launch_ros.actions.LifecycleNode(
+        package="nav2_map_server",
+        executable="map_server",
+        name=f"map_server",
+        namespace=map_name,
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            {"yaml_filename": yaml_filename},
+            {"frame_id": map_name},
+            {"topic_name": "map"},
+        ],
+        )
+
+    configure_trans_event = launch.actions.EmitEvent(
+        event=launch_ros.events.lifecycle.ChangeState(
+            lifecycle_node_matcher=launch.events.matches_action(
+                map_server_node),
+            transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
+        )
+    )
+
+    activate_trans_event = launch.actions.EmitEvent(
+        event=launch_ros.events.lifecycle.ChangeState(
+            lifecycle_node_matcher=launch.events.matches_action(
+                map_server_node),
+            transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
+        )
+    )
+
+    # Add the actions to the launch description.
+    # The order should reflects the order in which they will be executed.
+    ld.add_action(tf_pub_node)
+    ld.add_action(map_server_node)
+    ld.add_action(configure_trans_event)
+    ld.add_action(activate_trans_event)
+    return ld
+
+
+def generate_launch_description():
+
+    map_name = LaunchConfiguration('map_name')
+    tx = LaunchConfiguration('tx')
+    ty = LaunchConfiguration('ty')
+    yaw = LaunchConfiguration('yaw')
+    world_name = LaunchConfiguration('world_name')
+
+    declare_map_name_cmd = DeclareLaunchArgument(
+        'map_name',
+        default_value='',
+        description='map name, should also corespond to the .yaml file name',
+    )
+    declare_tx_cmd = DeclareLaunchArgument(
+        'tx', default_value='0.0', description='x-axis translation')
+    declare_ty_cmd = DeclareLaunchArgument(
+        'ty', default_value='0.0', description='y-axis translation')
+    declare_yaw_cmd = DeclareLaunchArgument(
+        'yaw', default_value='0.0', description='yaw angle in radian')
+    declare_tx_cmd = DeclareLaunchArgument(
+        'world_name', default_value='office', description='name of the world')
+
+    ld = launch.LaunchDescription()
+    ld.add_action(declare_map_name_cmd)
+    ld.add_action(declare_tx_cmd)
+    ld.add_action(declare_ty_cmd)
+    ld.add_action(declare_yaw_cmd)
+
+    # With this, can be launched with cli:
+    #    ros2 launch rmf_demos map_server.launch.py \
+    #        map_name:=ecobot_office tx:=1.33 ty:=0.057 yaw:=-1.598
+    # Note: transformation value is obtained from traffic-editor
+    ld = create_map_server(
+        ld, map_name, tx, ty, yaw, world_name, use_launch_config=True)
+    return ld

rmf_demos/launch/office.launch.xml

@@ -3,6 +3,7 @@
 <launch>
   <arg name="use_sim_time" default="false"/>
   <arg name="failover_mode" default="false"/>
+  <arg name="run_fleet_adapters" default="true"/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/common.launch.xml">
@@ -13,7 +14,7 @@
   </include>
 
   <!-- TinyRobot fleet adapter and robot state aggregator needed for the TinyRobot slotcar_plugin -->
-  <group>
+  <group if="$(var run_fleet_adapters)">
     <let name="fleet_name" value="tinyRobot"/>
     <include file="$(find-pkg-share rmf_demos)/include/adapters/tinyRobot_adapter.launch.xml">
       <arg name="fleet_name" value="$(var fleet_name)"/>

rmf_demos_maps/maps/office/ecobot_office.yaml

@@ -0,0 +1,6 @@
+image: ecobot_office.png
+resolution: 0.049  # Note: this is the scale value calculated by traffic-editor
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

rmf_demos_maps/maps/office/office.building.yaml

@@ -16,13 +16,23 @@ graphs:
   {}
 levels:
   L1:
+    constraints:
+      - {ids: ["{aa7269f1-f9c5-438e-b229-53a908d7cee7}", "{6afec236-c8a4-4cc3-babf-e3389bde50a9}"]}
+      - {ids: ["{760fa0a7-da43-4bc1-8dbc-ed115b46423d}", "{8ab8fc65-501d-4f8d-adbe-10b34a1560ee}"]}
+      - {ids: ["{10b7d64a-84f0-4e52-96ab-798c1b965d7c}", "{edc3f43b-8c9d-4344-9bc8-b9b53bc4d483}"]}
+      - {ids: ["{673ea8d8-017c-4302-8a7a-c934319933c4}", "{aeaad865-28f8-4169-a5a1-70eafbf230c2}"]}
     doors:
       - [31, 13, {motion_axis: [1, start], motion_degrees: [3, 90], motion_direction: [2, 1], name: [1, main_door], plugin: [1, normal], right_left_ratio: [3, 1], type: [1, double_hinged]}]
       - [35, 37, {motion_axis: [1, start], motion_degrees: [3, 90], motion_direction: [2, 1], name: [1, coe_door], plugin: [1, normal], right_left_ratio: [3, 1], type: [1, hinged]}]
       - [18, 17, {motion_axis: [1, start], motion_degrees: [3, 90], motion_direction: [2, 1], name: [1, hardware_door], plugin: [1, normal], right_left_ratio: [3, 1], type: [1, hinged]}]
     drawing:
       filename: office.png
     elevation: 0
+    features:
+      - {id: "{8ab8fc65-501d-4f8d-adbe-10b34a1560ee}", name: "", x: 1017.955, y: 353.594}
+      - {id: "{6afec236-c8a4-4cc3-babf-e3389bde50a9}", name: "", x: 2803.503, y: 342.141}
+      - {id: "{673ea8d8-017c-4302-8a7a-c934319933c4}", name: "", x: 2293.382, y: 1362.382}
+      - {id: "{10b7d64a-84f0-4e52-96ab-798c1b965d7c}", name: "", x: 664.289, y: 1280.104}
     floors:
       - parameters: {ceiling_scale: [3, 1], ceiling_texture: [1, blue_linoleum], indoor: [2, 0], texture_name: [1, blue_linoleum], texture_rotation: [3, 0], texture_scale: [3, 1]}
         vertices: [27, 28, 29, 30, 31, 13, 14, 15, 21, 20, 19, 22, 23, 24, 38, 25, 26, 36]
@@ -58,7 +68,20 @@ levels:
       - [59, 67, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], orientation: [1, ""], speed_limit: [3, 0]}]
       - [59, 45, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], orientation: [1, ""], speed_limit: [3, 0]}]
     layers:
-      {}
+      ecobot:
+        color: [1, 0, 0, 0.5]
+        features:
+          - {id: "{aa7269f1-f9c5-438e-b229-53a908d7cee7}", name: "", x: 47.961, y: 18.983}
+          - {id: "{aeaad865-28f8-4169-a5a1-70eafbf230c2}", name: "", x: 228.371, y: 101.391}
+          - {id: "{edc3f43b-8c9d-4344-9bc8-b9b53bc4d483}", name: "", x: 218.899, y: 385.571}
+          - {id: "{760fa0a7-da43-4bc1-8dbc-ed115b46423d}", name: "", x: 60.043, y: 330.204}
+        filename: ecobot_office.png
+        transform:
+          scale: 0.04873508376309721
+          translation_x: 24.71499709457617
+          translation_y: 0.5794233827190223
+          yaw: -1.598022090952649
+        visible: true
     measurements:
       - [1, 0, {distance: [3, 9.315]}]
       - [5, 4, {distance: [3, 8.409000000000001]}]