Release/Flexiv ROS 2 Humble 0.8 (#19)

.github/workflows/foxy-binary-build.yml → .github/workflows/humble-binary-build.yml

@@ -1,43 +1,41 @@
-name: Foxy Binary Build
+name: Humble Binary Build
 
 on:
   push:
     branches:
       - main
-      - foxy
   pull_request:
     branches:
       - main
-      - foxy
 
 jobs:
-  foxy_binary:
-    name: Foxy binary job
-    runs-on: ubuntu-20.04
+  humble_binary:
+    name: Humble binary job
+    runs-on: ubuntu-22.04
     container:
-      image: osrf/ros:foxy-desktop
+      image: osrf/ros:humble-desktop
     strategy:
       fail-fast: false
     env:
-      ROS_DISTRO: foxy
+      ROS_DISTRO: humble
     steps:
       - name: Checkout
         uses: actions/checkout@v3
       - name: Install dependencies
         run: |
           sudo apt-get update && sudo apt-get install -y \
           libeigen3-dev \
-          ros-foxy-xacro \
-          ros-foxy-tinyxml2-vendor \
-          ros-foxy-ros2-control \
-          ros-foxy-realtime-tools \
-          ros-foxy-control-toolbox \
-          ros-foxy-moveit \
-          ros-foxy-ros2-controllers \
-          ros-foxy-test-msgs \
-          ros-foxy-joint-state-publisher \
-          ros-foxy-joint-state-publisher-gui \
-          ros-foxy-robot-state-publisher
+          ros-humble-xacro \
+          ros-humble-tinyxml2-vendor \
+          ros-humble-ros2-control \
+          ros-humble-realtime-tools \
+          ros-humble-control-toolbox \
+          ros-humble-moveit \
+          ros-humble-ros2-controllers \
+          ros-humble-test-msgs \
+          ros-humble-joint-state-publisher \
+          ros-humble-joint-state-publisher-gui \
+          ros-humble-robot-state-publisher
       - name: Build and run tests
         id: action-ros-ci
         uses: ros-tooling/[email protected]

README.md

@@ -1,46 +1,54 @@
 # Flexiv ROS 2
 
-[![Foxy Source Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-source-build.yml/badge.svg)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-source-build.yml)
-[![Foxy Binary Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-binary-build.yml)
-[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![docs](https://img.shields.io/badge/docs-sphinx-yellow)](https://rdk.flexiv.com/manual/ros2_packages.html)
 
-For ROS 2 users to easily work with [RDK](https://github.com/flexivrobotics/flexiv_rdk), the APIs of RDK are wrapped into ROS packages in `flexiv_ros2`. Key functionalities like real-time joint torque and position control are supported, and the integration with `ros2_control` framework and MoveIt 2 is also implemented.
+For ROS 2 users to easily work with [RDK](https://github.com/flexivrobotics/flexiv_rdk), the APIs of RDK are wrapped into ROS packages in `flexiv_ros2`. Key functionalities like real-time joint torque and position control are supported, and the integration with `ros2_control` framework and MoveIt! 2 is also implemented.
 
 ## References
 
 [Flexiv RDK main webpage](https://rdk.flexiv.com/) contains important information like RDK user manual and network setup.
 
 ## Compatibility
 
-| **Supported OS** | **Supported ROS 2 distribution**               |
-| ---------------------- | ---------------------------------------------------- |
-| Ubuntu 20.04           | [Foxy Fitzroy](https://docs.ros.org/en/foxy/index.html) |
+| **Supported OS** | **Supported ROS 2 distribution**                              |
+| ---------------- | ------------------------------------------------------------- |
+| Ubuntu 20.04     | [Foxy Fitzroy](https://docs.ros.org/en/foxy/index.html)       |
+| Ubuntu 22.04     | [Humble Hawksbill](https://docs.ros.org/en/humble/index.html) |
+
+### Release Status
+
+| **ROS 2 Distro**   | Foxy                                                            | Humble                                                |
+| ------------------ | --------------------------------------------------------------- | ----------------------------------------------------- |
+| **Branch**         | [foxy](https://github.com/flexivrobotics/flexiv_ros2/tree/foxy) | [main](https://github.com/flexivrobotics/flexiv_ros2) |
+| **Release Status** | [![Foxy Binary Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-binary-build.yml/badge.svg?branch=foxy)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/foxy-binary-build.yml) | [![Humble Binary Build](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/humble-binary-build.yml/badge.svg?branch=main)](https://github.com/flexivrobotics/flexiv_ros2/actions/workflows/humble-binary-build.yml) |
 
 ## Getting Started
 
-This project was developed for ROS 2 Foxy on Ubuntu 20.04. Other versions of Ubuntu and ROS 2 may work, but are not officially supported.
+This project was developed for ROS 2 Foxy (Ubuntu 20.04) and Humble (Ubuntu 22.04). Other versions of Ubuntu and ROS 2 may work, but are not officially supported.
+
+1. Install [ROS 2 Humble via Debian Packages](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html)
 
-1. Install [ROS 2 Foxy via Debian Packages](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
 2. Install `colcon` and additional ROS packages:
 
    ```bash
    sudo apt install -y \
    python3-colcon-common-extensions \
    python3-rosdep2 \
    libeigen3-dev \
-   ros-foxy-xacro \
-   ros-foxy-tinyxml2-vendor \
-   ros-foxy-ros2-control \
-   ros-foxy-realtime-tools \
-   ros-foxy-control-toolbox \
-   ros-foxy-moveit \
-   ros-foxy-ros2-controllers \
-   ros-foxy-test-msgs \
-   ros-foxy-joint-state-publisher \
-   ros-foxy-joint-state-publisher-gui \
-   ros-foxy-robot-state-publisher \
-   ros-foxy-rviz2
+   ros-humble-xacro \
+   ros-humble-tinyxml2-vendor \
+   ros-humble-ros2-control \
+   ros-humble-realtime-tools \
+   ros-humble-control-toolbox \
+   ros-humble-moveit \
+   ros-humble-ros2-controllers \
+   ros-humble-test-msgs \
+   ros-humble-joint-state-publisher \
+   ros-humble-joint-state-publisher-gui \
+   ros-humble-robot-state-publisher \
+   ros-humble-rviz2
    ```
+
 3. Setup workspace:
 
    ```bash
@@ -50,26 +58,28 @@ This project was developed for ROS 2 Foxy on Ubuntu 20.04. Other versions of Ubu
    cd flexiv_ros2/
    git submodule update --init --recursive
    ```
+
 4. Install dependencies:
 
    ```bash
    cd ~/flexiv_ros2_ws
    rosdep update
-   rosdep install --from-paths src --ignore-src --rosdistro foxy -r -y
+   rosdep install --from-paths src --ignore-src --rosdistro humble -r -y
    ```
+
 5. Build and source the workspace:
 
    ```bash
    cd ~/flexiv_ros2_ws
-   source /opt/ros/foxy/setup.bash
+   source /opt/ros/humble/setup.bash
    colcon build --symlink-install
    source install/setup.bash
    ```
 
 **NOTE**: Remember to source the setup file and the workspace whenever a new terminal is opened:
 
 ```bash
-source /opt/ros/foxy/setup.bash
+source /opt/ros/humble/setup.bash
 source ~/flexiv_ros2_ws/install/setup.bash
 ```
 
@@ -107,6 +117,7 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
      ```bash
      ros2 launch flexiv_bringup rizon.launch.py robot_ip:=dont-care local_ip:=dont-care use_fake_hardware:=true
      ```
+
 2. Publish commands to controllers
 
    - To send the goal position to the controller by using the node from `flexiv_test_nodes`, start the following command in a new terminal:
@@ -152,10 +163,10 @@ ros2 launch flexiv_bringup rizon_moveit.launch.py robot_ip:=dont-care local_ip:=
 
 The robot driver (`rizon.launch.py`) publishes the following feedback states to the respective ROS topics:
 
-* `/joint_states`: Measured joint states of the robot: joint position, velocity and torque. [[`sensor_msgs/JointState.msg`](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/JointState.html)]
-* `/external_wrench_in_tcp_broadcaster/external_wrench_in_tcp`: Estimated external wrench applied on TCP and expressed in TCP frame $^{TCP}F_{ext}$ in force $[N]$ and moment $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
-* `/external_wrench_in_base_broadcaster/external_wrench_in_base`: Estimated external wrench applied on TCP and expressed in base frame $^{0}F_{ext}$ in force $[N]$ and moment $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
-* `/force_torque_sensor_broadcaster/wrench`: Force-torque (FT) sensor raw reading in flange frame: $^{flange}F_{raw}$ in force $[N]$ and moment $[Nm]$. The value is 0 if no FT sensor is installed. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
-* `/tcp_pose_broadcaster/tcp_pose`: Measured TCP pose expressed in base frame $^{0}T_{TCP}$ in position $[m]$ and quaternion. [[`geometry_msgs/PoseStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)]
+- `/joint_states`: Measured joint states of the robot: joint position, velocity and torque. [[`sensor_msgs/JointState.msg`](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/JointState.html)]
+- `/external_wrench_in_tcp_broadcaster/external_wrench_in_tcp`: Estimated external wrench applied on TCP and expressed in TCP frame $^{TCP}F_{ext}$ in force $[N]$ and moment $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
+- `/external_wrench_in_base_broadcaster/external_wrench_in_base`: Estimated external wrench applied on TCP and expressed in base frame $^{0}F_{ext}$ in force $[N]$ and moment $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
+- `/force_torque_sensor_broadcaster/wrench`: Force-torque (FT) sensor raw reading in flange frame: $^{flange}F_{raw}$ in force $[N]$ and moment $[Nm]$. The value is 0 if no FT sensor is installed. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
+- `/tcp_pose_broadcaster/tcp_pose`: Measured TCP pose expressed in base frame $^{0}T_{TCP}$ in position $[m]$ and quaternion. [[`geometry_msgs/PoseStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/PoseStamped.html)]
 
 **NOTE**: The topic names of the broadcasters are specified in `flexiv_bringup/config/rizon_controllers.yaml`

flexiv_bringup/config/sine_sweep_impedance_config.yaml

@@ -3,11 +3,4 @@ sine_sweep_impedance_controller:
 
     controller_name: "joint_impedance_controller"
     wait_sec_between_publish: 0.001
-    joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
+    speed_scaling: 1.0

flexiv_bringup/launch/rizon.launch.py

@@ -1,11 +1,10 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
 from launch.conditions import IfCondition
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
@@ -87,14 +86,14 @@ def generate_launch_description():
     robot_controller = LaunchConfiguration("robot_controller")
 
     # Get URDF via xacro
-    flexiv_urdf_xacro = os.path.join(
-        get_package_share_directory("flexiv_description"), "urdf", "rizon.urdf.xacro"
+    flexiv_urdf_xacro = PathJoinSubstitution(
+        [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"]
     )
 
     # Get URDF via xacro
     robot_description_content = Command(
         [
-            FindExecutable(name="xacro"),
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             flexiv_urdf_xacro,
             " ",
@@ -119,9 +118,10 @@ def generate_launch_description():
     )
     robot_description = {"robot_description": robot_description_content}
 
-    # RViZ
-    rviz_base = os.path.join(get_package_share_directory("flexiv_description"), "rviz")
-    rviz_config_file = os.path.join(rviz_base, "view_rizon.rviz")
+    # RViZ    
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"]
+    )
 
     rviz_node = Node(
         package="rviz2",
@@ -133,22 +133,16 @@ def generate_launch_description():
     )
 
     # Robot controllers
-    robot_controllers = [
-        get_package_share_directory("flexiv_bringup"),
-        "/config/",
-        controllers_file,
-    ]
+    robot_controllers = PathJoinSubstitution(
+        [FindPackageShare("flexiv_bringup"), "config", controllers_file]
+    )
 
     # Controller Manager
     ros2_control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[robot_description, robot_controllers],
-        output={
-            "stdout": "screen",
-            "stderr": "screen",
-        },
-        on_exit=Shutdown(),
+        output="both",
     )
 
     # Robot state publisher
@@ -163,32 +157,71 @@ def generate_launch_description():
     # Run robot controller
     robot_controller_spawner = Node(
         package="controller_manager",
-        executable="spawner.py",
-        arguments=[robot_controller, "-c", "/controller_manager"],
-    )
-
-    # Load broadcasters
-    load_controllers = []
-    for controller in [
-        "joint_state_broadcaster",
-        "force_torque_sensor_broadcaster",
-        "external_wrench_in_base_broadcaster",
-        "external_wrench_in_tcp_broadcaster",
-        "tcp_pose_state_broadcaster",
-    ]:
-        load_controllers += [
-            ExecuteProcess(
-                cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
-                shell=True,
-                output="screen",
-            )
-        ]
+        executable="spawner",
+        arguments=[robot_controller, "--controller-manager", "/controller_manager"],
+    )
+
+    # Run joint state broadcaster
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    # Run force torque sensor broadcaster
+    force_torque_sensor_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["force_torque_sensor_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    # Run external wrench in base broadcaster
+    external_wrench_in_base_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["external_wrench_in_base_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    # Run external wrench in tcp broadcaster
+    external_wrench_in_tcp_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["external_wrench_in_tcp_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    # Run tcp pose state broadcaster
+    tcp_pose_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["tcp_pose_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    # Delay rviz start after `joint_state_broadcaster`
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
+    # Delay start of robot_controller after `joint_state_broadcaster`
+    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[robot_controller_spawner],
+        )
+    )
 
     nodes = [
         ros2_control_node,
         robot_state_publisher_node,
-        rviz_node,
-        robot_controller_spawner,
+        joint_state_broadcaster_spawner,
+        force_torque_sensor_broadcaster_spawner,
+        external_wrench_in_base_broadcaster_spawner,
+        external_wrench_in_tcp_broadcaster_spawner,
+        tcp_pose_state_broadcaster_spawner,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
     ]
 
-    return LaunchDescription(declared_arguments + nodes + load_controllers)
+    return LaunchDescription(declared_arguments + nodes)