From 6c754b0fec19150873cc5e6e8b770b3c359e0cbd Mon Sep 17 00:00:00 2001 From: Aaron Chong <aaronchongth@gmail.com> Date: Wed, 8 Jan 2025 18:30:45 +0800 Subject: [PATCH] nexus_demos, move all files mentioning rmf to new package, parameterize launch files from integration tests Signed-off-by: Aaron Chong <aaronchongth@gmail.com> --- nexus_demos/CMakeLists.txt | 25 ++ nexus_demos/README.md | 43 +++ .../config/depot/deliveryRobot_config.yaml | 0 .../config/pick_and_place_rmf.json | 0 .../config/rmf_bts/transportation.xml | 0 .../config/system_bts/main_rmf.xml | 0 .../config/system_bts/pick_and_place_rmf.xml | 0 .../config/workcell_1_bts/place_on_amr.xml | 0 .../config/workcell_2_bts/pick_from_amr.xml | 0 nexus_demos/launch/depot.launch.xml | 56 ++++ .../launch/include/depot/depot.rviz | 0 .../maps/depot/depot.building.yaml | 0 nexus_demos/maps/depot/depot.world | 311 ++++++++++++++++++ .../maps/depot/depot_scan.png | Bin .../maps/depot/models/depot_L1/model.config | 11 + .../maps/depot/models/depot_L1/model.sdf | 6 + nexus_demos/maps/depot/nav_graphs/0.yaml | 229 +++++++++++++ .../maps/depot/template}/depot_world.sdf | 0 nexus_demos/package.xml | 27 ++ nexus_integration_tests/CMakeLists.txt | 78 +---- .../launch/control_center.launch.py | 22 +- nexus_integration_tests/launch/launch.py | 9 + nexus_integration_tests/package.xml | 1 - .../test_pick_and_place_rmf.py | 109 ------ 24 files changed, 734 insertions(+), 193 deletions(-) create mode 100644 nexus_demos/CMakeLists.txt create mode 100644 nexus_demos/README.md rename {nexus_integration_tests => nexus_demos}/config/depot/deliveryRobot_config.yaml (100%) rename {nexus_integration_tests => nexus_demos}/config/pick_and_place_rmf.json (100%) rename {nexus_integration_tests => nexus_demos}/config/rmf_bts/transportation.xml (100%) rename {nexus_integration_tests => nexus_demos}/config/system_bts/main_rmf.xml (100%) rename {nexus_integration_tests => nexus_demos}/config/system_bts/pick_and_place_rmf.xml (100%) rename {nexus_integration_tests => nexus_demos}/config/workcell_1_bts/place_on_amr.xml (100%) rename {nexus_integration_tests => nexus_demos}/config/workcell_2_bts/pick_from_amr.xml (100%) create mode 100644 nexus_demos/launch/depot.launch.xml rename {nexus_integration_tests => nexus_demos}/launch/include/depot/depot.rviz (100%) rename {nexus_integration_tests => nexus_demos}/maps/depot/depot.building.yaml (100%) create mode 100644 nexus_demos/maps/depot/depot.world rename {nexus_integration_tests => nexus_demos}/maps/depot/depot_scan.png (100%) create mode 100644 nexus_demos/maps/depot/models/depot_L1/model.config create mode 100644 nexus_demos/maps/depot/models/depot_L1/model.sdf create mode 100644 nexus_demos/maps/depot/nav_graphs/0.yaml rename {nexus_integration_tests/templates => nexus_demos/maps/depot/template}/depot_world.sdf (100%) create mode 100644 nexus_demos/package.xml delete mode 100644 nexus_integration_tests/test_pick_and_place_rmf.py diff --git a/nexus_demos/CMakeLists.txt b/nexus_demos/CMakeLists.txt new file mode 100644 index 0000000..df146a7 --- /dev/null +++ b/nexus_demos/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.16) +set(PROJECT_NAME nexus_demos) +project(${PROJECT_NAME}) + +find_package(ament_cmake REQUIRED) + +set(CMAKE_CXX_FLAGS "-Wall -Wpedantic") + +install(DIRECTORY launch config maps DESTINATION share/${PROJECT_NAME}) + +# Refactored from rmf_demos_maps, we just need to download models +file(GLOB_RECURSE traffic_editor_paths "maps/*.building.yaml") +foreach(building_map_path ${traffic_editor_paths}) + ############################################################################## + # Generate Gz world and download Models + ############################################################################## + if (NOT NO_DOWNLOAD_MODELS) + message("DOWNLOADING MODELS WITH COMMAND: ros2 run rmf_building_map_tools building_map_model_downloader ${building_map_path}") + execute_process( + COMMAND ros2 run rmf_building_map_tools building_map_model_downloader ${building_map_path} -e ~/.gazebo/models + ) + endif() +endforeach() + +ament_package() diff --git a/nexus_demos/README.md b/nexus_demos/README.md new file mode 100644 index 0000000..4a0ddb2 --- /dev/null +++ b/nexus_demos/README.md @@ -0,0 +1,43 @@ +# nexus_demos + +## Regenerating the simulation world file and nav graph + +Source build of `rmf_building_map_tools` is required, at least from commit hash [0d18f59](https://github.com/open-rmf/rmf_traffic_editor/tree/0d18f593356fa2e4de0dbfa297ae1fba66b8e101) onwards. + +Generate world file, + +```bash +# Source the workspace where rmf_building_map_tools is built + +cd ~/ws_nexus/src/nexus/nexus_demos +ros2 run rmf_building_map_tools building_map_generator gazebo \ + maps/depot/depot.building.yaml \ + maps/depot/depot.world \ + maps/depot/models \ + --TEMPLATE_WORLD_FILE maps/depot/template/depot_world.sdf \ + --SKIP_CAMERA_POSE +``` + +Generate navigation graphs, + +```bash +ros2 run rmf_building_map_tools building_map_generator nav \ + maps/depot/depot.building.yaml \ + maps/depot/nav_graphs +``` + +## Build + +Build `nexus_demos`, + +```bash +colcon build --packages-up-to nexus_demos +``` + +## Launch + +Launch the demo, + +```bash +ros2 launch nexus_demos depot.launch.xml headless:=0 +``` diff --git a/nexus_integration_tests/config/depot/deliveryRobot_config.yaml b/nexus_demos/config/depot/deliveryRobot_config.yaml similarity index 100% rename from nexus_integration_tests/config/depot/deliveryRobot_config.yaml rename to nexus_demos/config/depot/deliveryRobot_config.yaml diff --git a/nexus_integration_tests/config/pick_and_place_rmf.json b/nexus_demos/config/pick_and_place_rmf.json similarity index 100% rename from nexus_integration_tests/config/pick_and_place_rmf.json rename to nexus_demos/config/pick_and_place_rmf.json diff --git a/nexus_integration_tests/config/rmf_bts/transportation.xml b/nexus_demos/config/rmf_bts/transportation.xml similarity index 100% rename from nexus_integration_tests/config/rmf_bts/transportation.xml rename to nexus_demos/config/rmf_bts/transportation.xml diff --git a/nexus_integration_tests/config/system_bts/main_rmf.xml b/nexus_demos/config/system_bts/main_rmf.xml similarity index 100% rename from nexus_integration_tests/config/system_bts/main_rmf.xml rename to nexus_demos/config/system_bts/main_rmf.xml diff --git a/nexus_integration_tests/config/system_bts/pick_and_place_rmf.xml b/nexus_demos/config/system_bts/pick_and_place_rmf.xml similarity index 100% rename from nexus_integration_tests/config/system_bts/pick_and_place_rmf.xml rename to nexus_demos/config/system_bts/pick_and_place_rmf.xml diff --git a/nexus_integration_tests/config/workcell_1_bts/place_on_amr.xml b/nexus_demos/config/workcell_1_bts/place_on_amr.xml similarity index 100% rename from nexus_integration_tests/config/workcell_1_bts/place_on_amr.xml rename to nexus_demos/config/workcell_1_bts/place_on_amr.xml diff --git a/nexus_integration_tests/config/workcell_2_bts/pick_from_amr.xml b/nexus_demos/config/workcell_2_bts/pick_from_amr.xml similarity index 100% rename from nexus_integration_tests/config/workcell_2_bts/pick_from_amr.xml rename to nexus_demos/config/workcell_2_bts/pick_from_amr.xml diff --git a/nexus_demos/launch/depot.launch.xml b/nexus_demos/launch/depot.launch.xml new file mode 100644 index 0000000..b931531 --- /dev/null +++ b/nexus_demos/launch/depot.launch.xml @@ -0,0 +1,56 @@ +<?xml version='1.0' ?> + +<launch> + <arg name="headless" default="true"/> + <arg name="use_simulator" default="true"/> + <arg name="sim_update_rate" default='100'/> + + <!-- RMF Common launch --> + <include file="$(find-pkg-share rmf_demos)/common.launch.xml"> + <arg name="headless" value="$(var headless)" /> + <arg name="use_sim_time" value="$(var use_simulator)"/> + <arg name="viz_config_file" value ="$(find-pkg-share nexus_demos)/launch/include/depot/depot.rviz"/> + <arg name="config_file" value="$(find-pkg-share nexus_demos)/maps/depot/depot.building.yaml"/> + </include> + + <!-- Nexus Common launch --> + <include file="$(find-pkg-share nexus_integration_tests)/launch/common.launch.xml"> + <arg name="headless" value="$(var headless)" /> + <arg name="use_sim_time" value="$(var use_simulator)"/> + </include> + + <!-- DeliveryRobot fleet adapter --> + <group> + <include file="$(find-pkg-share rmf_demos_fleet_adapter)/launch/fleet_adapter.launch.xml"> + <arg name="use_sim_time" value="$(var use_sim_time)"/> + <arg name="nav_graph_file" value="$(find-pkg-share nexus_demos)/maps/depot/nav_graphs/0.yaml" /> + <arg name="config_file" value="$(find-pkg-share nexus_demos)/config/depot/deliveryRobot_config.yaml"/> + </include> + </group> + + <node pkg="nexus_workcell_orchestrator" exec="nexus_workcell_orchestrator" name="rmf_nexus_transporter" output="both"> + <param name="capabilities" value="[nexus::capabilities::RMFRequestCapability]"/> + <param name="bt_path" value="$(find-pkg-share nexus_integration_tests)/config/rmf_bts"/> + </node> + + <!-- Simulator launch --> + <include if="$(var use_simulator)" file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml"> + <arg name="headless" value="$(var headless)" /> + <arg name="map_package" value="nexus_demos" /> + <arg name="map_name" value="depot" /> + <arg name="gazebo_version" value="8" /> + <arg name="sim_update_rate" value="$(var sim_update_rate)"/> + </include> + + <!-- Bringup from integration tests --> + <!-- TODO(luca) should we move the integration launch here instead? --> + <include file="$(find-pkg-share nexus_integration_tests)/launch/launch.py"> + <!-- Set as true to make it auto bringup and remove all the extra rviz launches --> + <arg name="headless" value="true" /> + <arg name="use_sim_time" value="$(var use_simulator)"/> + <arg name="main_bt_package" value="nexus_demos"/> + <arg name="main_bt_filename" value="main_rmf.xml"/> + <arg name="remap_task_types" value="{pick_and_place_rmf: [place_on_amr, pick_from_amr]}"/> + </include> + +</launch> diff --git a/nexus_integration_tests/launch/include/depot/depot.rviz b/nexus_demos/launch/include/depot/depot.rviz similarity index 100% rename from nexus_integration_tests/launch/include/depot/depot.rviz rename to nexus_demos/launch/include/depot/depot.rviz diff --git a/nexus_integration_tests/maps/depot/depot.building.yaml b/nexus_demos/maps/depot/depot.building.yaml similarity index 100% rename from nexus_integration_tests/maps/depot/depot.building.yaml rename to nexus_demos/maps/depot/depot.building.yaml diff --git a/nexus_demos/maps/depot/depot.world b/nexus_demos/maps/depot/depot.world new file mode 100644 index 0000000..de2dccf --- /dev/null +++ b/nexus_demos/maps/depot/depot.world @@ -0,0 +1,311 @@ +<sdf version="1.7"> + <world name="sim_world"> + <physics name="10ms" type="ode"> + <max_step_size>0.01</max_step_size> + <real_time_factor>1.0</real_time_factor> + </physics> + <plugin filename="libgz-sim-physics-system.so" name="gz::sim::systems::Physics"> + </plugin> + <plugin filename="libgz-sim-user-commands-system.so" name="gz::sim::systems::UserCommands"> + </plugin> + <plugin filename="libgz-sim-scene-broadcaster-system.so" name="gz::sim::systems::SceneBroadcaster"> + </plugin> + <plugin filename="libdoor.so" name="door"> + </plugin> + <plugin filename="liblift.so" name="lift"> + </plugin> + <scene> + <ambient>1 1 1</ambient> + <background>0.8 0.8 0.8</background> + <grid>false</grid> + </scene> + <gui fullscreen="0"> + <window> + <width>1000</width> + <height>845</height> + <style material_theme="Light" material_primary="DeepOrange" material_accent="LightBlue" toolbar_color_light="#f3f3f3" toolbar_text_color_light="#111111" toolbar_color_dark="#414141" toolbar_text_color_dark="#f3f3f3" plugin_toolbar_color_light="#bbdefb" plugin_toolbar_text_color_light="#111111" plugin_toolbar_color_dark="#607d8b" plugin_toolbar_text_color_dark="#eeeeee" /> + <menus> + <drawer default="false"> + </drawer> + </menus> + </window> + <plugin filename="MinimalScene" name="3D View"> + <gz-gui> + <title>3D View</title> + <property type="bool" key="showTitleBar">false</property> + <property type="string" key="state">docked</property> + </gz-gui> + <engine>ogre2</engine> + <scene>scene</scene> + <ambient_light>0.4 0.4 0.4</ambient_light> + <background_color>0.8 0.8 0.8</background_color> + <camera_pose>5.2453 -7.8138 3.8271 0 0.5849817 0.2531771</camera_pose> + </plugin> + <plugin filename="EntityContextMenuPlugin" name="Entity context menu"> + <gz-gui> + <property key="state" type="string">floating</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="showTitleBar" type="bool">false</property> + </gz-gui> + </plugin> + <plugin filename="GzSceneManager" name="Scene Manager"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </gz-gui> + </plugin> + <plugin filename="InteractiveViewControl" name="Interactive view control"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </gz-gui> + </plugin> + <plugin filename="CameraTracking" name="Camera Tracking"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </gz-gui> + </plugin> + <plugin filename="MarkerManager" name="Marker manager"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </gz-gui> + </plugin> + <plugin filename="SelectEntities" name="Select Entities"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </gz-gui> + </plugin> + <plugin filename="Spawn" name="Spawn Entities"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </gz-gui> + </plugin> + <plugin filename="VisualizationCapabilities" name="Visualization Capabilities"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="width" type="double">5</property> + <property key="height" type="double">5</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + </gz-gui> + </plugin> + <plugin filename="WorldControl" name="World control"> + <gz-gui> + <title>World control</title> + <property type="bool" key="showTitleBar">false</property> + <property type="bool" key="resizable">false</property> + <property type="double" key="height">72</property> + <property type="double" key="z">1</property> + <property type="string" key="state">floating</property> + <anchors target="3D View"> + <line own="left" target="left" /> + <line own="bottom" target="bottom" /> + </anchors> + </gz-gui> + <play_pause>true</play_pause> + <step>true</step> + <start_paused>true</start_paused> + <use_event>true</use_event> + </plugin> + <plugin filename="WorldStats" name="World stats"> + <gz-gui> + <title>World stats</title> + <property type="bool" key="showTitleBar">false</property> + <property type="bool" key="resizable">false</property> + <property type="double" key="height">110</property> + <property type="double" key="width">290</property> + <property type="double" key="z">1</property> + <property type="string" key="state">floating</property> + <anchors target="3D View"> + <line own="right" target="right" /> + <line own="bottom" target="bottom" /> + </anchors> + </gz-gui> + <sim_time>true</sim_time> + <real_time>true</real_time> + <real_time_factor>true</real_time_factor> + <iterations>true</iterations> + </plugin> + <plugin filename="Shapes" name="Shapes"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="x" type="double">0</property> + <property key="y" type="double">0</property> + <property key="width" type="double">250</property> + <property key="height" type="double">50</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + <property key="cardBackground" type="string">#666666</property> + </gz-gui> + </plugin> + <plugin filename="Lights" name="Lights"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="x" type="double">250</property> + <property key="y" type="double">0</property> + <property key="width" type="double">150</property> + <property key="height" type="double">50</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + <property key="cardBackground" type="string">#666666</property> + </gz-gui> + </plugin> + <plugin filename="TransformControl" name="Transform control"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="x" type="double">0</property> + <property key="y" type="double">50</property> + <property key="width" type="double">250</property> + <property key="height" type="double">50</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + <property key="cardBackground" type="string">#777777</property> + </gz-gui> + </plugin> + <plugin filename="Screenshot" name="Screenshot"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="x" type="double">250</property> + <property key="y" type="double">50</property> + <property key="width" type="double">50</property> + <property key="height" type="double">50</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + <property key="cardBackground" type="string">#777777</property> + </gz-gui> + </plugin> + <plugin filename="CopyPaste" name="CopyPaste"> + <gz-gui> + <property key="resizable" type="bool">false</property> + <property key="x" type="double">300</property> + <property key="y" type="double">50</property> + <property key="width" type="double">100</property> + <property key="height" type="double">50</property> + <property key="state" type="string">floating</property> + <property key="showTitleBar" type="bool">false</property> + <property key="cardBackground" type="string">#777777</property> + </gz-gui> + </plugin> + <plugin filename="ComponentInspector" name="Component inspector"> + <gz-gui> + <property type="bool" key="showTitleBar">false</property> + <property type="string" key="state">docked</property> + </gz-gui> + </plugin> + <plugin filename="EntityTree" name="Entity tree"> + <gz-gui> + <property type="bool" key="showTitleBar">false</property> + <property type="string" key="state">docked</property> + </gz-gui> + </plugin> + <plugin filename="toggle_charging" name="toggle_charging" /> + <plugin name="toggle_floors" filename="toggle_floors"> + <floor name="L1" model_name="depot_L1"> + <model name="OpenRobotics/FoodCourtTable1" /> + <model name="OpenRobotics/FoodCourtTable1_2" /> + <model name="OpenRobotics/FoodCourtTable1_3" /> + <model name="OpenRobotics/FoodCourtTable1_4" /> + <model name="OpenRobotics/FoodCourtTable1_5" /> + <model name="OpenRobotics/FoodCourtTable1_6" /> + </floor> + </plugin> + </gui> + <light type="directional" name="sun"> + <cast_shadows>true</cast_shadows> + <pose>0 0 10 0 0 0</pose> + <diffuse>1 1 1 1</diffuse> + <specular>0.2 0.2 0.2 1</specular> + <attenuation> + <range>1000</range> + <constant>0.09</constant> + <linear>0.001</linear> + <quadratic>0.001</quadratic> + </attenuation> + <direction>-0.5 0.1 -0.9</direction> + </light> + <include> + <uri> + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Depot + </uri> + <pose>15.15 -7.525 0 0 0 0</pose> + </include> + <include> + <name>OpenRobotics/FoodCourtTable1</name> + <uri>model://FoodCourtTable1</uri> + <pose>7.638448208027225 -4.775442805381185 0.0 0 0 3.1416</pose> + <static>True</static> + </include> + <include> + <name>OpenRobotics/FoodCourtTable1_2</name> + <uri>model://FoodCourtTable1</uri> + <pose>7.634647736251055 -5.717359731380535 0.0 0 0 3.1416</pose> + <static>True</static> + </include> + <include> + <name>OpenRobotics/FoodCourtTable1_3</name> + <uri>model://FoodCourtTable1</uri> + <pose>7.6395983508015926 -6.661476930513458 0.0 0 0 3.1416</pose> + <static>True</static> + </include> + <include> + <name>OpenRobotics/FoodCourtTable1_4</name> + <uri>model://FoodCourtTable1</uri> + <pose>7.644648977767293 -8.584365630633071 0.0 0 0 3.1416</pose> + <static>True</static> + </include> + <include> + <name>OpenRobotics/FoodCourtTable1_5</name> + <uri>model://FoodCourtTable1</uri> + <pose>7.647999393675233 -10.483101332490968 0.0 0 0 -3.1416</pose> + <static>True</static> + </include> + <include> + <name>OpenRobotics/FoodCourtTable1_6</name> + <uri>model://FoodCourtTable1</uri> + <pose>7.647999393675233 -9.53203327050426 0.0 0 0 3.1416</pose> + <static>True</static> + </include> + <include> + <name>deliveryRobot1</name> + <uri>model://DeliveryRobot</uri> + <pose>28.2650087111177 -5.6303989363968405 0.0 0 0 -2.804903289267812</pose> + </include> + <include> + <name>deliveryRobot2</name> + <uri>model://DeliveryRobot</uri> + <pose>28.29596255361046 -6.807245025612636 0.0 0 0 2.811792456645858</pose> + </include> + <include> + <name>depot_L1</name> + <uri>model://depot_L1</uri> + <pose>0 0 0.0 0 0 0</pose> + </include> + <rmf_charger_waypoints name="charger_waypoints"> + <rmf_vertex name="deliveryRobotCharger1" x="28.2650087111177" y="-5.6303989363968405" level="L1" /> + <rmf_vertex name="deliveryRobotCharger2" x="28.29596255361046" y="-6.807245025612636" level="L1" /> + </rmf_charger_waypoints> + </world> +</sdf> diff --git a/nexus_integration_tests/maps/depot/depot_scan.png b/nexus_demos/maps/depot/depot_scan.png similarity index 100% rename from nexus_integration_tests/maps/depot/depot_scan.png rename to nexus_demos/maps/depot/depot_scan.png diff --git a/nexus_demos/maps/depot/models/depot_L1/model.config b/nexus_demos/maps/depot/models/depot_L1/model.config new file mode 100644 index 0000000..aceae89 --- /dev/null +++ b/nexus_demos/maps/depot/models/depot_L1/model.config @@ -0,0 +1,11 @@ +<?xml version='1.0' encoding='utf-8'?> +<model> + <name>depot_L1</name> + <version>1.0.0</version> + <sdf version="1.6">model.sdf</sdf> + <author> + <name>automatically generated from the Great Editor</name> + <email>info@openrobotics.org</email> + </author> + <description>level depot_L1 (automatically generated)</description> +</model> diff --git a/nexus_demos/maps/depot/models/depot_L1/model.sdf b/nexus_demos/maps/depot/models/depot_L1/model.sdf new file mode 100644 index 0000000..5780a66 --- /dev/null +++ b/nexus_demos/maps/depot/models/depot_L1/model.sdf @@ -0,0 +1,6 @@ +<?xml version='1.0' encoding='utf-8'?> +<sdf version="1.7"> + <model name="depot_L1"> + <static>true</static> + </model> +</sdf> diff --git a/nexus_demos/maps/depot/nav_graphs/0.yaml b/nexus_demos/maps/depot/nav_graphs/0.yaml new file mode 100644 index 0000000..65853bf --- /dev/null +++ b/nexus_demos/maps/depot/nav_graphs/0.yaml @@ -0,0 +1,229 @@ +building_name: depot +doors: {} +levels: + L1: + lanes: + - - 0 + - 1 + - {} + - - 1 + - 0 + - {} + - - 0 + - 2 + - {} + - - 2 + - 0 + - {} + - - 3 + - 1 + - {} + - - 1 + - 3 + - {} + - - 4 + - 5 + - {} + - - 5 + - 4 + - {} + - - 6 + - 4 + - {} + - - 4 + - 6 + - {} + - - 7 + - 4 + - {} + - - 4 + - 7 + - {} + - - 4 + - 8 + - {} + - - 8 + - 4 + - {} + - - 5 + - 9 + - {} + - - 9 + - 5 + - {} + - - 9 + - 10 + - {} + - - 10 + - 9 + - {} + - - 10 + - 11 + - {} + - - 11 + - 10 + - {} + - - 1 + - 12 + - {} + - - 12 + - 1 + - {} + - - 12 + - 13 + - {} + - - 13 + - 12 + - {} + - - 13 + - 8 + - {} + - - 8 + - 13 + - {} + - - 9 + - 14 + - {} + - - 14 + - 9 + - {} + - - 8 + - 15 + - {} + - - 15 + - 8 + - {} + - - 15 + - 16 + - {} + - - 16 + - 15 + - {} + - - 15 + - 2 + - {} + - - 2 + - 15 + - {} + - - 10 + - 17 + - {} + - - 17 + - 10 + - {} + - - 17 + - 2 + - {} + - - 2 + - 17 + - {} + - - 18 + - 19 + - {} + - - 19 + - 18 + - {} + - - 19 + - 20 + - {} + - - 20 + - 19 + - {} + - - 20 + - 21 + - {} + - - 21 + - 20 + - {} + - - 21 + - 22 + - {} + - - 22 + - 21 + - {} + - - 11 + - 19 + - {} + - - 19 + - 11 + - {} + - - 20 + - 2 + - {} + - - 2 + - 20 + - {} + vertices: + - - 13.33740565501788 + - -14.10070040753721 + - {name: ''} + - - 16.840090465042543 + - -14.07084670161124 + - {name: ''} + - - 13.458170646326463 + - -8.33778502105021 + - {name: ''} + - - 16.82153816202992 + - -12.180562048834563 + - {is_holding_point: true, name: ''} + - - 26.58044959633005 + - -6.220022129986687 + - {name: ''} + - - 26.57194854104125 + - -1.2916103356146331 + - {name: ''} + - - 28.2650087111177 + - -5.6303989363968405 + - {is_charger: true, is_holding_point: true, is_parking_spot: true, name: deliveryRobotCharger1, + spawn_robot_name: deliveryRobot1, spawn_robot_type: DeliveryRobot} + - - 28.29596255361046 + - -6.807245025612636 + - {is_charger: true, is_holding_point: true, is_parking_spot: true, name: deliveryRobotCharger2, + spawn_robot_name: deliveryRobot2, spawn_robot_type: DeliveryRobot} + - - 26.54099469854849 + - -8.329934046459963 + - {name: ''} + - - 19.31729797619968 + - -1.3309652209810319 + - {name: ''} + - - 15.63949142722568 + - -1.3100626262120934 + - {name: ''} + - - 9.154186366020765 + - -1.314863222139888 + - {name: ''} + - - 28.227704080262132 + - -14.062995727020994 + - {name: ''} + - - 28.22345355261773 + - -8.349686498454533 + - {name: ''} + - - 19.31694793274661 + - -3.377019210373123 + - {is_holding_point: true, name: ''} + - - 22.41743281519574 + - -8.359137671687378 + - {name: ''} + - - 22.41743281519574 + - -9.855923477007646 + - {is_holding_point: true, name: ''} + - - 15.660444028202201 + - -6.246025357928907 + - {name: ''} + - - 8.47745235882448 + - -4.746889260852323 + - {is_holding_point: true, is_parking_spot: false, name: workcell_1, pickup_dispenser: workcell_1} + - - 9.121082256602016 + - -4.749089533985896 + - {name: ''} + - - 9.109780853688667 + - -7.5656391697890095 + - {name: ''} + - - 9.116931741372778 + - -10.4492471299585 + - {name: ''} + - - 8.468701272497773 + - -10.464949079138995 + - {dropoff_ingestor: workcell_2, is_holding_point: true, is_parking_spot: false, + name: workcell_2, pickup_dispenser: workcell_2} +lifts: {} diff --git a/nexus_integration_tests/templates/depot_world.sdf b/nexus_demos/maps/depot/template/depot_world.sdf similarity index 100% rename from nexus_integration_tests/templates/depot_world.sdf rename to nexus_demos/maps/depot/template/depot_world.sdf diff --git a/nexus_demos/package.xml b/nexus_demos/package.xml new file mode 100644 index 0000000..960ff4c --- /dev/null +++ b/nexus_demos/package.xml @@ -0,0 +1,27 @@ +<?xml version="1.0"?> +<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> +<package format="3"> + <name>nexus_demos</name> + <version>0.1.1</version> + <description>Demos for system and workcell orchestrators</description> + <maintainer email="aaronchong@intrinsic.ai">Aaron Chong</maintainer> + <license>Apache License 2.0</license> + + <buildtool_depend>ament_cmake</buildtool_depend> + + <build_depend>rmf_building_map_tools</build_depend> + + <exec_depend>launch</exec_depend> + <exec_depend>launch_ros</exec_depend> + <exec_depend>launch_xml</exec_depend> + <exec_depend>nexus_integration_tests</exec_depend> + <exec_depend>nexus_workcell_orchestrator</exec_depend> + <exec_depend>rmf_building_map_tools</exec_depend> + <exec_depend>rmf_demos</exec_depend> + <exec_depend>rmf_demos_fleet_adapter</exec_depend> + <exec_depend>rmf_demos_gz</exec_depend> + + <export> + <build_type>ament_cmake</build_type> + </export> +</package> diff --git a/nexus_integration_tests/CMakeLists.txt b/nexus_integration_tests/CMakeLists.txt index 986a01b..ea52012 100644 --- a/nexus_integration_tests/CMakeLists.txt +++ b/nexus_integration_tests/CMakeLists.txt @@ -115,7 +115,7 @@ add_custom_target(generate_zenoh_bridge_configs ALL COMMAND ros2 run nexus_network_configuration nexus_network_configuration -n ${nexus_network_cfg_path} -o ${zenoh_cfg_output_dir} ) -install(DIRECTORY launch config rviz scripts templates maps/ DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch config rviz scripts DESTINATION share/${PROJECT_NAME}) # Install the zenoh config directory containing generated configs. message("zenoh_cfg_output_dir: " ${zenoh_cfg_output_dir}) install(DIRECTORY ${zenoh_cfg_output_dir} DESTINATION share/${PROJECT_NAME}/config/) @@ -176,80 +176,4 @@ if(BUILD_TESTING) endif() endif() -# Taken from rmf_demos_maps -file(GLOB_RECURSE traffic_editor_paths "maps/*.building.yaml") - -foreach(path ${traffic_editor_paths}) - - # Get the output world name - string(REGEX REPLACE "\\.[^.]*\.[^.]*$" "" no_extension_path ${path}) - string(REGEX MATCH "[^\/]+$" world_name ${no_extension_path}) - - set(map_path ${path}) - set(output_world_name ${world_name}) - set(output_dir ${CMAKE_CURRENT_BINARY_DIR}/maps/${output_world_name}) - set(output_world_path ${output_dir}/${output_world_name}.world) - set(output_model_dir ${output_dir}/models) - set(world_template_path ${CMAKE_CURRENT_SOURCE_DIR}/templates/depot_world.sdf) - - ############################################################################## - # Generate Gz world and download Models - ############################################################################## - - message("BUILDING WORLDFILE WITH COMMAND: ros2 run rmf_building_map_tools building_map_generator gazebo ${map_path} ${output_world_path} ${output_model_dir}") - if (NO_DOWNLOAD_MODELS) - add_custom_command( - DEPENDS ${map_path} - COMMAND ros2 run rmf_building_map_tools building_map_generator gazebo ${map_path} ${output_world_path} ${output_model_dir} --TEMPLATE_WORLD_FILE ${world_template_path} --SKIP_CAMERA_POSE - OUTPUT ${output_world_path} - ) - else() - message("DOWNLOADING MODELS WITH COMMAND: ros2 run rmf_building_map_tools building_map_model_downloader ${map_path}") - add_custom_command( - DEPENDS ${map_path} - COMMAND ros2 run rmf_building_map_tools building_map_generator gazebo ${map_path} ${output_world_path} ${output_model_dir} --TEMPLATE_WORLD_FILE ${world_template_path} --SKIP_CAMERA_POSE - COMMAND ros2 run rmf_building_map_tools building_map_model_downloader ${map_path} -e ~/.gazebo/models - OUTPUT ${output_world_path} - ) - endif() - - ############################################################################## - # generate the navmesh and required files for crowd simulation for gz - ############################################################################## - set(crowd_sim_config_resource ${output_dir}/config_resource/) - - add_custom_command( - OUTPUT ${world_name}_crowdsim - COMMAND ros2 run rmf_building_map_tools building_crowdsim ${map_path} ${crowd_sim_config_resource} ${output_world_path} - DEPENDS ${output_world_path} - ) - - # This will initiate both custom commands: ${output_world_path} and ${world_name}_crowdsim - add_custom_target(generate_${world_name}_crowdsim ALL - DEPENDS ${world_name}_crowdsim - ) - - ############################################################################## - # Generate the nav graphs - ############################################################################## - - set(output_nav_graphs_dir ${output_dir}/nav_graphs/) - set(output_nav_graphs_phony ${output_nav_graphs_dir}/phony) - add_custom_command( - OUTPUT ${output_nav_graphs_phony} - COMMAND ros2 run rmf_building_map_tools building_map_generator nav ${map_path} ${output_nav_graphs_dir} - DEPENDS ${map_path} - ) - - add_custom_target(generate_${output_world_name}_nav_graphs ALL - DEPENDS ${output_nav_graphs_phony} - ) - - install( - DIRECTORY ${output_dir} - DESTINATION share/${PROJECT_NAME}/maps - ) - -endforeach() - ament_package() diff --git a/nexus_integration_tests/launch/control_center.launch.py b/nexus_integration_tests/launch/control_center.launch.py index 3d7aa7c..f3ce326 100644 --- a/nexus_integration_tests/launch/control_center.launch.py +++ b/nexus_integration_tests/launch/control_center.launch.py @@ -112,6 +112,8 @@ def launch_setup(context, *args, **kwargs): transporter_plugin = LaunchConfiguration("transporter_plugin") activate_system_orchestrator = LaunchConfiguration("activate_system_orchestrator") headless = LaunchConfiguration("headless") + remap_task_types = LaunchConfiguration("remap_task_types") + main_bt_package = LaunchConfiguration("main_bt_package") main_bt_filename = LaunchConfiguration("main_bt_filename") nexus_panel_rviz_path = os.path.join( @@ -126,14 +128,10 @@ def launch_setup(context, *args, **kwargs): parameters=[ { "bt_path": ( - FindPackageShare("nexus_integration_tests"), + FindPackageShare(main_bt_package), "/config/system_bts", ), - "remap_task_types": - """{ - pick_and_place: [place_on_conveyor, pick_from_conveyor], - pick_and_place_rmf: [place_on_amr, pick_from_amr], - }""", + "remap_task_types": remap_task_types, "main_bt_filename": main_bt_filename, "max_jobs": 2, } @@ -246,6 +244,18 @@ def generate_launch_description(): default_value="true", description="Launch in headless mode (no gui)", ), + DeclareLaunchArgument( + "remap_task_types", + default_value= """{ + pick_and_place: [place_on_conveyor, pick_from_conveyor], + }""", + description="A yaml containing a dictionary of task types and an array of remaps", + ), + DeclareLaunchArgument( + "main_bt_package", + default_value="nexus_integration_tests", + description="Package containing the main system orchestrator behavior tree", + ), DeclareLaunchArgument( "main_bt_filename", default_value="main.xml", diff --git a/nexus_integration_tests/launch/launch.py b/nexus_integration_tests/launch/launch.py index ff4567b..3fb50a8 100644 --- a/nexus_integration_tests/launch/launch.py +++ b/nexus_integration_tests/launch/launch.py @@ -41,6 +41,7 @@ def launch_setup(context, *args, **kwargs): exit(1) headless = LaunchConfiguration("headless") + remap_task_types = LaunchConfiguration("remap_task_types") use_zenoh_bridge = LaunchConfiguration("use_zenoh_bridge") use_fake_hardware = LaunchConfiguration("use_fake_hardware") robot1_ip = LaunchConfiguration("robot1_ip") @@ -99,6 +100,7 @@ def launch_setup(context, *args, **kwargs): "zenoh_config_filename": "config/zenoh/system_orchestrator.json5", "transporter_plugin": "nexus_transporter::MockTransporter", "activate_system_orchestrator": headless, + "remap_task_types": remap_task_types, "headless": headless, }.items(), ), @@ -207,6 +209,13 @@ def generate_launch_description(): default_value="true", description="Launch in headless mode (no gui)", ), + DeclareLaunchArgument( + "remap_task_types", + default_value= """{ + pick_and_place: [place_on_conveyor, pick_from_conveyor], + }""", + description="A yaml containing a dictionary of task types and an array of remaps", + ), DeclareLaunchArgument( "use_zenoh_bridge", default_value="true", diff --git a/nexus_integration_tests/package.xml b/nexus_integration_tests/package.xml index 753937b..d334e82 100644 --- a/nexus_integration_tests/package.xml +++ b/nexus_integration_tests/package.xml @@ -9,7 +9,6 @@ <build_depend>nexus_endpoints</build_depend> <build_depend>nexus_network_configuration</build_depend> - <build_depend>rmf_building_map_tools</build_depend> <buildtool_depend>ament_cmake</buildtool_depend> diff --git a/nexus_integration_tests/test_pick_and_place_rmf.py b/nexus_integration_tests/test_pick_and_place_rmf.py deleted file mode 100644 index 2782ae2..0000000 --- a/nexus_integration_tests/test_pick_and_place_rmf.py +++ /dev/null @@ -1,109 +0,0 @@ -# Copyright (C) 2022 Johnson & Johnson -# -# 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 sys -from typing import cast - -from action_msgs.msg import GoalStatus -from managed_process import managed_process -from nexus_orchestrator_msgs.action import ExecuteWorkOrder -from nexus_orchestrator_msgs.msg import TaskState -from nexus_test_case import NexusTestCase -from rclpy import Future -from rclpy.action import ActionClient -from rclpy.action.client import ClientGoalHandle, GoalStatus -from ros_testcase import RosTestCase -import subprocess - -class PickAndPlaceTest(NexusTestCase): - @RosTestCase.timeout(60) - async def asyncSetUp(self): - # todo(YV): Find a better fix to the problem below. - # zenoh-bridge was bumped to 0.72 as part of the upgrade to - # ROS 2 Iron. However with this upgrade, the bridge does not clearly - # terminate when a SIGINT is received leaving behind zombie bridge - # processes from previous test cases. As a result, workcell registration - # fails for this testcase due to multiple bridges remaining active. - # Hence we explicitly kill any zenoh processes before launching the test. - subprocess.Popen('pkill -9 -f zenoh', shell=True) - - self.proc = managed_process( - ("ros2", "launch", "nexus_integration_tests", "depot.launch.xml", "sim_update_rate:=10000", "main_bt_filename:=main_rmf.xml"), - ) - self.proc.__enter__() - print("waiting for nodes to be ready...", file=sys.stderr) - self.wait_for_nodes("system_orchestrator") - await self.wait_for_lifecycle_active("system_orchestrator") - - await self.wait_for_workcells("workcell_1", "workcell_2", "rmf_nexus_transporter") - print("all workcells are ready") - await self.wait_for_transporters("transporter_node") - print("all transporters are ready") - await self.wait_for_robot_state() - print("AMRs are ready") - - # give some time for discovery to happen - await self.ros_sleep(5) - - # create action client to send work order - self.action_client = ActionClient( - self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" - ) - - def tearDown(self): - self.proc.__exit__(None, None, None) - - @RosTestCase.timeout(600) # 10min - async def test_pick_and_place_wo(self): - self.action_client.wait_for_server() - goal_msg = ExecuteWorkOrder.Goal() - with open(f"{os.path.dirname(__file__)}/config/pick_and_place_rmf.json") as f: - goal_msg.order.work_order = f.read() - feedbacks: list[ExecuteWorkOrder.Feedback] = [] - fb_fut = Future() - - def on_fb(msg): - feedbacks.append(msg.feedback) - if len(feedbacks) >= 5: - fb_fut.set_result(None) - - goal_handle = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_fb) - ) - self.assertTrue(goal_handle.accepted) - - results = await goal_handle.get_result_async() - self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) - - # check that we receive the correct feedbacks - # FIXME(koonpeng): First few feedbacks are sometimes missed when the system in under - # high load so we only check the last feedback as a workaround. - self.assertGreater(len(feedbacks), 0) - for msg in feedbacks: - # The first task is transportation - self.assertEqual(len(msg.task_states), 3) - state: TaskState = msg.task_states[1] # type: ignore - self.assertEqual(state.workcell_id, "workcell_1") - self.assertEqual(state.task_id, "1") - state: TaskState = msg.task_states[2] # type: ignore - self.assertEqual(state.workcell_id, "workcell_2") - self.assertEqual(state.task_id, "2") - - state: TaskState = feedbacks[-1].task_states[0] # type: ignore - self.assertEqual(state.status, TaskState.STATUS_FINISHED) - state: TaskState = feedbacks[-1].task_states[1] # type: ignore - self.assertEqual(state.status, TaskState.STATUS_FINISHED) - state: TaskState = feedbacks[-1].task_states[2] # type: ignore - self.assertEqual(state.status, TaskState.STATUS_FINISHED)