Resolve merge conflict with main

Signed-off-by: Yadunund <yadunund@openrobotics.org>

.github/workflows/nexus_integration_tests.yaml

@@ -11,10 +11,10 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        docker_image: ['ros:humble-ros-base']
+        docker_image: ['ros:iron-ros-base']
     container:
       image: ${{ matrix.docker_image }}
-    timeout-minutes: 30
+    timeout-minutes: 60
     steps:
     - uses: actions/checkout@v2
     - uses: actions/cache@v3

.github/workflows/style.yaml

@@ -11,16 +11,16 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        docker_image: ['ros:humble-ros-base']
+        docker_image: ['ros:iron-ros-base']
     container:
       image: ${{ matrix.docker_image }}
     steps:
       - name: checkout
         uses: actions/checkout@v2
       - name: uncrustify
         run: |
-          sudo apt update && sudo apt install -y ros-humble-rmf-utils
-          /ros_entrypoint.sh ament_uncrustify -c /opt/ros/humble/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp
+          sudo apt update && sudo apt install -y ros-iron-rmf-utils
+          /ros_entrypoint.sh ament_uncrustify -c /opt/ros/iron/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp
       - name: pycodestyle
         run: |
           sudo apt update && sudo apt install -y pycodestyle curl

README.md

@@ -9,7 +9,7 @@ A ROS 2 framework which enables configuration and orchestration of process workf
 For details on architecture and concepts [see](./docs/concepts.md).
 
 ## Requirements
-* [ROS 2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) on `Ubuntu 22.04`
+* [ROS 2 Iron](https://docs.ros.org/en/iron/Installation/Ubuntu-Install-Debians.html) on `Ubuntu 22.04`
 
 ## Setup
 
@@ -21,12 +21,12 @@ cd ~/ws_nexus/src/
 git clone git@github.com:OpenSourceRobotics/nexus
 vcs import . < nexus/abb.repos
 cd ~/ws_nexus
-rosdep install --from-paths src --ignore-src --rosdistro humble -y -r
+rosdep install --from-paths src --ignore-src --rosdistro iron -y -r
 ```
 
 ### Build the NEXUS workspace
 ```bash
-source /opt/ros/humble/setup.bash
+source /opt/ros/iron/setup.bash
 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 
@@ -90,10 +90,10 @@ The linter of choice is `uncrustify` and the configuration used may be reference
 Instead of invoking `uncrustify` directly, use `ament_uncrustify` instead which is a wrapper around a specific version of `uncrustify`.
 You may locally run the linter as follows
 ```bash
-sudo apt update && sudo apt install -y ros-humble-rmf-utils # This is a one-time step
-source /opt/ros/humble/setup.bash
+sudo apt update && sudo apt install -y ros-iron-rmf-utils # This is a one-time step
+source /opt/ros/iron/setup.bash
 cd ~/ws_nexus/src/nexus
-ament_uncrustify -c /opt/ros/humble/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp
+ament_uncrustify -c /opt/ros/iron/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp
 ```
 To automatically reformat the code, append `--reformat` to the `ament_uncrustify` line above.
 It is highly recommended to audit the changes by the linter before committing.
@@ -105,12 +105,12 @@ A linter such as `pycodestyle` may be used for linting.
 ## Known Issues
 
 ### Unable to scale speeds of cartesian motions in Humble
-The `humble` version of `moveit2` [lacks support](https://github.com/ros-planning/moveit2/issues/1967) for scaling velocity and accelerations of motion plans generated from cartesian interpolation.
-The fixes are likely to land in further distro releases.
+`moveit2` version 2.7.4 and below has a bug that prevents using `cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin`.
+The fixes will be available when [a new release into ROS Iron is made](https://github.com/ros-planning/moveit2/issues/2327).
 
-The workaround for now is to rely on fixes incorporated into custom forks of `moveit2` packages which are compatible with `humble`.
+The workaround for now is to use the `iron` branch on the upstream repository directly.
 
->Note: First make sure all `moveit2` binaries are purged from the system via `sudo apt purge ros-humble-moveit*`.
+>Note: First make sure all `moveit2` binaries are purged from the system via `sudo apt purge ros-iron-moveit*`.
 
 Then as part of the workspace setup, clone in repositories from the [thirdparty.repos](./thirdparty.repos) file.
 Follow the reset of the Setup and Build instructions from above.

abb.repos

@@ -18,4 +18,4 @@ repositories:
   thirdparty/abb/abb_ros2:
     type: git
     url: https://github.com/yadunund/abb_ros2.git
-    version: humble
+    version: main

nexus_common/include/nexus_common/logging.hpp

@@ -177,8 +177,10 @@ public: static std::string to_string(const BtLogging::LogReport& report);
  * @param log_level The log level to set to, if not provided it will be read from
  * NEXUS_LOG_LEVEL environment variable. If the log level is invalid, it will be ignored.
  *
- * Note: ros2 humble still does not support externally setting log level, using env var
- * best supports scenarios like `ros2 launch`.
+ * Note: ROS Iron only supports externally setting log level using the
+ * `--log-level` CLI flag.
+ * This function allows it to be set via an environment variable
+ * `NEXUS_LOG_LEVEl`.
  */
 template<typename NodePtrT>
 void configure_logging(NodePtrT node,

nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml

@@ -5,7 +5,7 @@
       <dispense_item.DispenseItem name="dispense_item" item="productA" />
 
       <ApplyTransform base_pose="pickup_pose" x="0" y="0" z="-0.02" qx="0" qy="0" qz="0" qw="1" local="true" result="{pickup_top_goal}" />
-      <plan_motion.PlanMotion name="plan_to_pickup_top" robot_name="abb_irb910sc" goal="{pickup_top_goal}" scale_speed="0.5" result="{pickup_top_traj}" />
+      <plan_motion.PlanMotion name="plan_to_pickup_top" robot_name="abb_irb910sc" goal="{pickup_top_goal}" scale_speed="0.5" cartesian="true" result="{pickup_top_traj}" />
       <GetJointConstraints trajectory="{pickup_top_traj}" joint_constraints="{joint_constraints}" />
       <execute_trajectory.ExecuteTrajectory name="execute_pickup_top" trajectory="{pickup_top_traj}" />
 
@@ -14,14 +14,14 @@
       <execute_trajectory.ExecuteTrajectory name="execute_pickup" trajectory="{pickup_traj}" />
       <gripper.GripperControl name="pickup_product" gripper="workcell_1_mock_gripper" position="0.0" max_effort="1.0" />
 
-      <plan_motion.PlanMotion name="plan_to_dropoff" robot_name="abb_irb910sc" goal="dropoff_pose" scale_speed="0.5" start_constraints="{joint_constraints}" result="{dropoff_traj}" />
+      <plan_motion.PlanMotion name="plan_to_dropoff" robot_name="abb_irb910sc" goal="dropoff_pose" scale_speed="0.5" cartesian="true" start_constraints="{joint_constraints}" result="{dropoff_traj}" />
       <GetJointConstraints trajectory="{dropoff_traj}" joint_constraints="{joint_constraints}" />
 
       <WaitForSignal name="wait_for_pallet" signal="transporter_done" clear="true"/>
       <execute_trajectory.ExecuteTrajectory name="execute_dropoff" trajectory="{dropoff_traj}" />
       <gripper.GripperControl name="dropoff_product" gripper="workcell_1_mock_gripper" position="1.0" max_effort="0.0" />
 
-      <plan_motion.PlanMotion name="plan_to_home" robot_name="abb_irb910sc" goal="home" scale_speed="0.5" start_constraints="{joint_constraints}" result="{home_traj}" />
+      <plan_motion.PlanMotion name="plan_to_home" robot_name="abb_irb910sc" goal="home" scale_speed="0.5" cartesian="true" start_constraints="{joint_constraints}" result="{home_traj}" />
       <execute_trajectory.ExecuteTrajectory name="execute_home" trajectory="{home_traj}" />
 
 

nexus_integration_tests/launch/workcell.launch.py

@@ -196,10 +196,9 @@ def launch_setup(context, *args, **kwargs):
                 IncludeLaunchDescription(
                     [
                         PathJoinSubstitution([
-                            FindPackageShare("nexus_integration_tests"),
+                            FindPackageShare("abb_bringup"),
                             'launch',
-                            'include',
-                            'nexus_control.launch.py',
+                            'abb_control.launch.py',
                         ])
                     ],
                     launch_arguments=[
@@ -271,7 +270,6 @@ def launch_setup(context, *args, **kwargs):
             condition=IfCondition(use_zenoh_bridge),
         ),
         activate_node_service("motion_planner_server", ros_domain_id.perform(context)),
-        activate_node_service("robot_controller_server", ros_domain_id.perform(context)),
     ]
 
 

nexus_integration_tests/launch/zenoh_bridge.launch.py

@@ -24,7 +24,10 @@
     LaunchConfiguration,
     PathJoinSubstitution,
 )
-from launch_ros.substitutions import FindPackageShare
+from launch_ros.substitutions import (
+    ExecutableInPackage,
+    FindPackageShare,
+)
 
 
 def launch_setup(context, *args, **kwargs):
@@ -35,28 +38,27 @@ def launch_setup(context, *args, **kwargs):
     log_level = LaunchConfiguration("log_level")
     ros_domain_id = LaunchConfiguration("ros_domain_id")
 
-    dds_localhost_only_text = ""
+    cmd = [
+        ExecutableInPackage(
+            executable="zenoh_bridge_dds",
+            package="nexus_zenoh_bridge_dds_vendor",
+        ),
+        "--config",
+        PathJoinSubstitution([
+            FindPackageShare(zenoh_config_package),
+            zenoh_config_filename
+        ]),
+        "--domain",
+        ros_domain_id,
+    ]
 
     if IfCondition(dds_localhost_only).evaluate(context):
-        dds_localhost_only_text = " --dds-localhost-only"
+        cmd.append("--dds-localhost-only")
 
     zenoh_bridge_exec = ExecuteProcess(
-        cmd=[
-            "RUST_LOG="+log_level.perform(context),
-            FindExecutable(name="ros2"),
-            "run",
-            "nexus_zenoh_bridge_dds_vendor",
-            "zenoh_bridge_dds",
-            "--config",
-            PathJoinSubstitution([
-                FindPackageShare(zenoh_config_package),
-                zenoh_config_filename
-            ]),
-            "--domain",
-            ros_domain_id,
-            dds_localhost_only_text,
-        ],
+        cmd=cmd,
         shell=True,
+        additional_env={"RUST_LOG": log_level.perform(context)},
     )
 
     return [zenoh_bridge_exec]

nexus_integration_tests/package.xml

@@ -36,7 +36,6 @@
   <exec_depend>launch_xml</exec_depend>
   <exec_depend>nexus_capabilities</exec_depend>
   <exec_depend>nexus_motion_planner</exec_depend>
-  <exec_depend>nexus_robot_controller</exec_depend>
   <exec_depend>nexus_rviz_plugins</exec_depend>
   <exec_depend>nexus_system_orchestrator</exec_depend>
   <exec_depend>nexus_workcell_orchestrator</exec_depend>

nexus_integration_tests/test_parallel_wo.py

@@ -22,11 +22,21 @@
 from rclpy.action import ActionClient
 from rclpy.action.client import ClientGoalHandle
 from ros_testcase import RosTestCase
+import subprocess
 
 
 class ParallelWoTest(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", "launch.py"),
         )

nexus_integration_tests/test_pick_and_place.py

@@ -25,11 +25,20 @@
 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", "launch.py"),
         )

nexus_motion_planner/src/motion_planner_server.cpp

@@ -269,8 +269,8 @@ bool MotionPlannerServer::initialize_move_group_interfaces()
       MoveGroupInterface::Options options{std::move(group_name)};
       if (_use_namespace)
       {
-        options.move_group_namespace_ = name;
-        options.robot_description_ = name + ".robot_description";
+        options.move_group_namespace = name;
+        options.robot_description = name + ".robot_description";
       }
       try
       {
@@ -520,9 +520,9 @@ void MotionPlannerServer::plan_with_move_group(
   {
     MoveGroupInterface::Plan plan;
     res->result.error_code = interface->plan(plan);
-    res->result.trajectory_start = std::move(plan.start_state_);
-    res->result.trajectory = std::move(plan.trajectory_);
-    res->result.planning_time = std::move(plan.planning_time_);
+    res->result.trajectory_start = std::move(plan.start_state);
+    res->result.trajectory = std::move(plan.trajectory);
+    res->result.planning_time = std::move(plan.planning_time);
   }
   if (_execute_trajectory)
   {

nexus_network_configuration/README.md

@@ -54,5 +54,3 @@ workcell_orchestrators:
 ```bash
 ros2 run nexus_network_configuration nexus_network_configuration -n <PATH_TO_NEXUS_NETWORK_CONFIG> -r <PATH_TO_REDF_CONFIGS> -o <ZENOH_CONFIGS_OUTPUT_DIRECTORY>
 ```
-
-> Note: The latest version of [zenoh-plugin-dds](https://github.com/eclipse-zenoh/zenoh-plugin-dds) does not require a custom CycloneDDS configuration to listen to DDS traffic on the loopback interface, and this could simply be toggled by using either `ROS_LOCALHOST_ONLY=1` or specifying the `dds-localhost-only` flag. However, the humble binary does have have these changes in as of 2/10/2022

nexus_network_configuration/scripts/set_up_network.sh

@@ -1,11 +1,11 @@
 #!/bin/bash
 
 # This script does the following:
-# 1. Install CycloneDDS (Humble dist.) if not already installed
+# 1. Install CycloneDDS (Iron dist.) if not already installed
 # 2. Change RMW_IMPLEMENTATION to CycloneDDS
 # 3. Enable multicast on loopback interface
 
-RMW_PACKAGE="ros-humble-rmw-cyclonedds-cpp"
+RMW_PACKAGE="ros-iron-rmw-cyclonedds-cpp"
 
 PKG_OK=$(dpkg-query -W --showformat='${Status}\n' $RMW_PACKAGE|grep "install ok installed")
 

nexus_zenoh_bridge_dds_vendor/CMakeLists.txt

@@ -1,22 +1,25 @@
 cmake_minimum_required(VERSION 3.16)
-set(PROJECT_NAME nexus_zenoh_bridge_dds_vendor)
-project(${PROJECT_NAME} VERSION 0.6.0)
+project(nexus_zenoh_bridge_dds_vendor)
 
-include(ExternalProject)
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_vendor_package REQUIRED)
 
-ExternalProject_Add(zenoh_bridge_dds
-  URL https://github.com/koonpeng/zenoh-plugin-dds/releases/download/0.6.0-beta.1/zenoh_bridge_dds-0.6.0-beta.1.tar.gz
-  PREFIX "zenoh_bridge_dds-bin"
-  CONFIGURE_COMMAND ""
-  BUILD_COMMAND ""
-  INSTALL_COMMAND ""
+ament_vendor(zeno_bridge_dds_vendor
+  VCS_URL https://github.com/eclipse-zenoh/zenoh-plugin-dds.git
+  VCS_VERSION 0.7.2-rc
 )
 
+# TODO(sloretz) make a nice way to get this path from ament_vendor
+set(INSTALL_DIR "${CMAKE_CURRENT_BINARY_DIR}/zeno_bridge_dds_vendor-prefix/install")
 install(
-  DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/zenoh_bridge_dds-bin/src/zenoh_bridge_dds/lib/zenoh_bridge_dds"
-  DESTINATION "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}"
+  DIRECTORY "${INSTALL_DIR}/lib/zenoh_bridge_dds/"
+  DESTINATION "lib/${PROJECT_NAME}"
   USE_SOURCE_PERMISSIONS
 )
 

nexus_zenoh_bridge_dds_vendor/package.xml

@@ -2,12 +2,16 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nexus_zenoh_bridge_dds_vendor</name>
-  <version>0.5.0</version>
+  <version>0.0.1</version>
   <description>Newer version of zenoh_bridge_dds for NEXUS</description>
   <maintainer email="koonpeng@openrobotics.org">Teo Koon Peng</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_vendor_package</buildtool_depend>
+
+  <build_depend>cargo</build_depend>
+  <build_depend>clang</build_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

thirdparty.repos

@@ -5,17 +5,17 @@ repositories:
     version: 2.2.2
   thirdparty/moveit/moveit2:
     type: git
-    url: https://github.com/koonpeng/moveit2.git
-    version: kp/scale_get_cartesian_path_humble
+    url: https://github.com/ros-planning/moveit2.git
+    version: iron
   thirdparty/moveit/moveit_resources:
     type: git
     url: https://github.com/ros-planning/moveit_resources.git
-    version: humble
+    version: 2.1.1
   thirdparty/moveit/object_recognition_msgs:
     type: git
     url: https://github.com/wg-perception/object_recognition_msgs
-    version: ros2
+    version: 2.0.0
   thirdparty/PickNikRobotics/pick_ik:
     type: git
     url: https://github.com/picknikrobotics/pick_ik
-    version: 1.0.0
+    version: 1.0.2