diff --git a/nexus_demos/README.md b/nexus_demos/README.md index 4a0ddb2..c3ed923 100644 --- a/nexus_demos/README.md +++ b/nexus_demos/README.md @@ -1,5 +1,37 @@ # nexus_demos +Nexus demo map using the [Depot](https://app.gazebosim.org/OpenRobotics/fuel/models/Depot) world, integrated with Open-RMF. + +## Build + +Build `nexus_demos`, + +```bash +colcon build --packages-up-to nexus_demos +``` + +## Launch + +Launch the demo, + +```bash +source ~/ws_nexus/install/setup.bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +ros2 launch nexus_demos depot.launch.xml headless:=0 +``` + +Start the `pick_and_place_rmf` work order, + +```bash +source ~/ws_nexus/install/setup.bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +cd ~/ws_nexus/src/nexus_demos + +ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {id: '23', work_order: '$(cat config/pick_and_place_rmf.json)'}}" +``` + ## 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. @@ -10,6 +42,7 @@ Generate world file, # 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 \ @@ -21,23 +54,13 @@ ros2 run rmf_building_map_tools building_map_generator gazebo \ Generate navigation graphs, ```bash +cd ~/ws_nexus/src/nexus/nexus_demos + 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, +## Troubleshooting -```bash -ros2 launch nexus_demos depot.launch.xml headless:=0 -``` +* If any of the commands give an error regarding `Failed to find a free participant index`, please also set the cyclonedds config with `export CYCLONEDDS_URI=$HOME/ws_nexus/src/nexus_integration_tests/config/cyclonedds/cyclonedds.xml`, which increases the maximum number of participants. diff --git a/nexus_demos/launch/depot.launch.xml b/nexus_demos/launch/depot.launch.xml index b931531..b089740 100644 --- a/nexus_demos/launch/depot.launch.xml +++ b/nexus_demos/launch/depot.launch.xml @@ -30,7 +30,7 @@ <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"/> + <param name="bt_path" value="$(find-pkg-share nexus_demos)/config/rmf_bts"/> </node> <!-- Simulator launch --> @@ -42,15 +42,14 @@ <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"> + <!-- Bringup orchestrators --> + <include file="$(find-pkg-share nexus_demos)/launch/include/depot/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]}"/> + <arg name="system_orchestrator_remap_task_types" value="{pick_and_place_rmf: [place_on_amr, pick_from_amr]}"/> </include> </launch> diff --git a/nexus_demos/launch/include/depot/launch.py b/nexus_demos/launch/include/depot/launch.py new file mode 100644 index 0000000..6141956 --- /dev/null +++ b/nexus_demos/launch/include/depot/launch.py @@ -0,0 +1,250 @@ +# 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 launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + GroupAction, + IncludeLaunchDescription, + LogInfo, + OpaqueFunction, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + if ( + "RMW_IMPLEMENTATION" not in os.environ + or os.environ["RMW_IMPLEMENTATION"] != "rmw_cyclonedds_cpp" + ): + print( + "Only cycloneDDS is supported, the environment variable RMW_IMPLEMENTATION must be set to rmw_cyclonedds_cpp", + file=sys.stderr, + ) + exit(1) + + headless = LaunchConfiguration("headless") + use_zenoh_bridge = LaunchConfiguration("use_zenoh_bridge") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + robot1_ip = LaunchConfiguration("robot1_ip") + robot2_ip = LaunchConfiguration("robot2_ip") + run_workcell_1 = LaunchConfiguration("run_workcell_1") + run_workcell_2 = LaunchConfiguration("run_workcell_2") + + control_center_domain_id = 0 + workcell_1_domain_id = 0 + workcell_2_domain_id = 0 + log_msg = "" + + if "ROS_DOMAIN_ID" in os.environ: + control_center_domain_id = int(os.environ["ROS_DOMAIN_ID"]) + if not 0 < control_center_domain_id < 230: + log_msg += ( + "ROS_DOMAIN_ID not within the range of 0 to 230, setting it to 0. \n" + ) + control_center_domain_id = 0 + + if use_zenoh_bridge.perform(context).lower() == "true": + log_msg += "Using the zenoh bridge\n" + workcell_1_domain_id = control_center_domain_id + 1 + workcell_2_domain_id = control_center_domain_id + 2 + else: + log_msg += "Not using zenoh bridge\n" + if ( + run_workcell_1.perform(context).lower() == "true" + and run_workcell_2.perform(context).lower() == "true" + ): + print("To run both workcells, enable the Zenoh Bridge") + sys.exit(1) + workcell_1_domain_id = control_center_domain_id + workcell_2_domain_id = control_center_domain_id + log_msg += f"Control Center has ROS_DOMAIN_ID {control_center_domain_id}\n" + if run_workcell_1.perform(context).lower() == "true": + log_msg += f"Workcell 1 has ROS_DOMAIN_ID {workcell_1_domain_id}\n" + if run_workcell_2.perform(context).lower() == "true": + log_msg += f"Workcell 2 has ROS_DOMAIN_ID {workcell_2_domain_id}\n" + + launch_control_center = GroupAction( + actions=[ + IncludeLaunchDescription( + [ + PathJoinSubstitution( + [ + FindPackageShare("nexus_integration_tests"), + "launch", + "control_center.launch.py", + ] + ) + ], + launch_arguments={ + "ros_domain_id": str(control_center_domain_id), + "zenoh_config_package": "nexus_integration_tests", + "zenoh_config_filename": "config/zenoh/system_orchestrator.json5", + "transporter_plugin": "nexus_transporter::MockTransporter", + "activate_system_orchestrator": headless, + "main_bt_package": "nexus_demos", + "main_bt_filename": "main_rmf.xml", + "remap_task_types": """{ + pick_and_place_rmf: [place_on_amr, pick_from_amr] + }""", + "headless": headless, + }.items(), + ), + ], + ) + + launch_workcell_1 = GroupAction( + actions=[ + IncludeLaunchDescription( + [ + PathJoinSubstitution( + [ + FindPackageShare("nexus_integration_tests"), + "launch", + "workcell.launch.py", + ] + ) + ], + launch_arguments={ + "workcell_id": "workcell_1", + "bt_path": ( + FindPackageShare("nexus_demos"), + "/config/workcell_1_bts", + ), + "task_checker_plugin": "nexus::task_checkers::FilepathChecker", + "ros_domain_id": str(workcell_1_domain_id), + "headless": headless, + "use_zenoh_bridge": use_zenoh_bridge, + "controller_config_package": "nexus_integration_tests", + "planner_config_package": "nexus_integration_tests", + "planner_config_file": "abb_irb910sc_planner_params.yaml", + "support_package": "abb_irb910sc_support", + "robot_xacro_file": "irb910sc_3_45.xacro", + "moveit_config_package": "abb_irb910sc_3_45_moveit_config", + "moveit_config_file": "abb_irb910sc_3_45.srdf.xacro", + "controllers_file": "abb_irb910sc_controllers.yaml", + "tf_publisher_launch_file": "workcell_1_tf.launch.py", + "sku_detection_params_file": "product_detector_config.yaml", + "dispenser_properties": "productA", + "use_fake_hardware": use_fake_hardware, + "robot_ip": robot1_ip, + "zenoh_config_package": "nexus_integration_tests", + "zenoh_config_filename": "config/zenoh/workcell_1.json5", + }.items(), + condition=IfCondition(run_workcell_1), + ) + ], + ) + + launch_workcell_2 = GroupAction( + actions=[ + IncludeLaunchDescription( + [ + PathJoinSubstitution( + [ + FindPackageShare("nexus_integration_tests"), + "launch", + "workcell.launch.py", + ] + ) + ], + launch_arguments={ + "workcell_id": "workcell_2", + "bt_path": ( + FindPackageShare("nexus_demos"), + "/config/workcell_2_bts", + ), + "task_checker_plugin": "nexus::task_checkers::FilepathChecker", + "ros_domain_id": str(workcell_2_domain_id), + "headless": headless, + "use_zenoh_bridge": use_zenoh_bridge, + "controller_config_package": "nexus_integration_tests", + "planner_config_package": "nexus_integration_tests", + "planner_config_file": "abb_irb1300_planner_params.yaml", + "support_package": "abb_irb1300_support", + "robot_xacro_file": "irb1300_10_115.xacro", + "moveit_config_package": "abb_irb1300_10_115_moveit_config", + "moveit_config_file": "abb_irb1300_10_115.srdf.xacro", + "controllers_file": "abb_irb1300_controllers.yaml", + "tf_publisher_launch_file": "workcell_2_tf.launch.py", + "sku_detection_params_file": "product_detector_config.yaml", + "dispenser_properties": "productB", + "use_fake_hardware": use_fake_hardware, + "robot_ip": robot2_ip, + "zenoh_config_package": "nexus_integration_tests", + "zenoh_config_filename": "config/zenoh/workcell_2.json5", + }.items(), + condition=IfCondition(run_workcell_2), + ) + ], + ) + + return [ + LogInfo(msg=log_msg), + launch_control_center, + launch_workcell_1, + launch_workcell_2, + ] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "headless", + default_value="true", + description="Launch in headless mode (no gui)", + ), + DeclareLaunchArgument( + "use_zenoh_bridge", + default_value="true", + description="Set true to launch the Zenoh DDS Bridge with each orchestrator\ + in different domains. Else, all orchestrators are launched under the \ + same Domain ID without a Zenoh bridge.", + ), + DeclareLaunchArgument( + "use_fake_hardware", + default_value="true", + description="Set True if running with real hardware.", + ), + DeclareLaunchArgument( + "robot1_ip", + default_value="", + description="The IP address of workcell 1 robot when use_fake_hardware is False.", + ), + DeclareLaunchArgument( + "robot2_ip", + default_value="", + description="The IP address of workcell 1 robot when use_fake_hardware is False.", + ), + DeclareLaunchArgument( + "run_workcell_1", + default_value="true", + description="Whether to run workcell_1", + ), + DeclareLaunchArgument( + "run_workcell_2", + default_value="true", + description="Whether to run workcell_2", + ), + SetEnvironmentVariable("RCUTILS_COLORIZED_OUTPUT", "1"), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/nexus_integration_tests/launch/control_center.launch.py b/nexus_integration_tests/launch/control_center.launch.py index f3ce326..51377fb 100644 --- a/nexus_integration_tests/launch/control_center.launch.py +++ b/nexus_integration_tests/launch/control_center.launch.py @@ -13,10 +13,12 @@ # limitations under the License. import os + from ament_index_python.packages import get_package_share_directory import launch_ros from launch_ros.actions import Node, LifecycleNode +from launch_ros.descriptions import ParameterValue from launch_ros.event_handlers import OnStateTransition from launch_ros.substitutions import FindPackageShare import lifecycle_msgs @@ -112,9 +114,9 @@ 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") + remap_task_types = LaunchConfiguration("remap_task_types") nexus_panel_rviz_path = os.path.join( get_package_share_directory("nexus_integration_tests"), "rviz", "nexus_panel.rviz" @@ -131,10 +133,10 @@ def launch_setup(context, *args, **kwargs): FindPackageShare(main_bt_package), "/config/system_bts", ), - "remap_task_types": remap_task_types, + "remap_task_types": ParameterValue(remap_task_types, value_type=str), "main_bt_filename": main_bt_filename, "max_jobs": 2, - } + }, ], ) @@ -244,13 +246,6 @@ 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", @@ -261,6 +256,11 @@ def generate_launch_description(): default_value="main.xml", description="File name of the main system orchestrator behavior tree", ), + DeclareLaunchArgument( + "remap_task_types", + default_value=str("{pick_and_place: [place_on_conveyor, pick_from_conveyor]}"), + description="A string yaml containing a dictionary of task types and an array of remaps", + ), OpaqueFunction(function=launch_setup), ] ) diff --git a/nexus_integration_tests/launch/launch.py b/nexus_integration_tests/launch/launch.py index 3fb50a8..ff4567b 100644 --- a/nexus_integration_tests/launch/launch.py +++ b/nexus_integration_tests/launch/launch.py @@ -41,7 +41,6 @@ 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") @@ -100,7 +99,6 @@ 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(), ), @@ -209,13 +207,6 @@ 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",