diff --git a/rmf_demos/launch/include/office/office.rviz b/rmf_demos/launch/include/office/office.rviz index 031e18f9..a0896e57 100644 --- a/rmf_demos/launch/include/office/office.rviz +++ b/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,12 +91,7 @@ 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 @@ -104,7 +99,7 @@ Visualization Manager: 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,7 +196,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 95.94175720214844 + Scale: 57.53582763671875 Target Frame: Value: TopDownOrtho (rviz_default_plugins) X: 14.163299560546875 @@ -184,21 +204,21 @@ Visualization Manager: 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 diff --git a/rmf_demos/launch/map_server.launch.py b/rmf_demos/launch/map_server.launch.py new file mode 100644 index 00000000..13847eb6 --- /dev/null +++ b/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 diff --git a/rmf_demos/launch/office.launch.xml b/rmf_demos/launch/office.launch.xml index 430fe800..82d0a3f0 100644 --- a/rmf_demos/launch/office.launch.xml +++ b/rmf_demos/launch/office.launch.xml @@ -3,6 +3,7 @@ + @@ -13,7 +14,7 @@ - + diff --git a/rmf_demos_maps/maps/office/ecobot_office.png b/rmf_demos_maps/maps/office/ecobot_office.png new file mode 100644 index 00000000..319f6c49 Binary files /dev/null and b/rmf_demos_maps/maps/office/ecobot_office.png differ diff --git a/rmf_demos_maps/maps/office/ecobot_office.yaml b/rmf_demos_maps/maps/office/ecobot_office.yaml new file mode 100644 index 00000000..91467b67 --- /dev/null +++ b/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 diff --git a/rmf_demos_maps/maps/office/office.building.yaml b/rmf_demos_maps/maps/office/office.building.yaml index b5ef3fad..1e9c552a 100644 --- a/rmf_demos_maps/maps/office/office.building.yaml +++ b/rmf_demos_maps/maps/office/office.building.yaml @@ -16,6 +16,11 @@ 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]}] @@ -23,6 +28,11 @@ levels: 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]}]