Release/Flexiv ROS 2 Humble v1.4 (#37)

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

@@ -43,10 +43,12 @@ jobs:
           target-ros2-distro: ${{ env.ROS_DISTRO }}
           package-name: |
             flexiv_bringup
-            flexiv_controllers
             flexiv_description
             flexiv_hardware
             flexiv_moveit_config
             flexiv_msgs
+            flexiv_robot_states_broadcaster
             flexiv_test_nodes
+            gpio_controller
+            joint_impedance_controller
           skip-tests: true

.pre-commit-config.yaml

@@ -17,7 +17,7 @@ repos:
       - id: check-yaml
         args: [--allow-multiple-documents]
       - id: check-added-large-files
-        args: [--maxkb=500]
+        args: [--maxkb=600]
       - id: pretty-format-json
         args: [--no-sort-keys, --autofix, --indent=4]
 

README.md

@@ -67,12 +67,31 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04) and Humble (Ubuntu 22.0
    rosdep install --from-paths src --ignore-src --rosdistro humble -r -y
    ```
 
-5. Build and source the workspace:
+> [!NOTE]
+> Skip step 5 and 6 if you have compile and install [flexiv_rdk](https://github.com/flexivrobotics/flexiv_rdk).
+
+5. Choose a directory for installing `flexiv_rdk` library and all its dependencies. For example, a new folder named `rdk_install` under the home directory: `~/rdk_install`. Compile and install to the installation directory:
+
+   ```bash
+   cd ~/flexiv_ros2_ws/src/flexiv_ros2/flexiv_hardware/rdk/thirdparty
+   bash build_and_install_dependencies.sh ~/rdk_install
+   ```
+
+6. Configure and install `flexiv_rdk`:
+
+   ```bash
+   cd ~/flexiv_ros2_ws/src/flexiv_ros2/flexiv_hardware/rdk
+   mkdir build && cd build
+   cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install
+   cmake --build . --target install --config Release
+   ```
+
+7. Build and source the workspace:
 
    ```bash
    cd ~/flexiv_ros2_ws
    source /opt/ros/humble/setup.bash
-   colcon build --symlink-install
+   colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=~/rdk_install
    source install/setup.bash
    ```
 
@@ -86,15 +105,14 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04) and Humble (Ubuntu 22.0
 ## Usage
 
 > [!NOTE]
-> The instruction below is only a quick reference, see the [Flexiv ROS 2 Documentation](https://rdk.flexiv.com/manual/ros2_packages.html) for more information.
+> The instruction below is only a quick reference, see the [Flexiv ROS 2 Documentation](https://rdk.flexiv.com/manual/ros2_bridge.html) for more information.
 
-The prerequisites of using ROS 2 with Flexiv Rizon robot are [enable RDK on the robot server](https://rdk.flexiv.com/manual/getting_started.html#activate-rdk-server) and [establish connection](https://rdk.flexiv.com/manual/getting_started.html#establish-connection) between the workstation PC and the robot.
+The prerequisites of using ROS 2 with Flexiv Rizon robot are [enable RDK on the robot server](https://rdk.flexiv.com/manual/activate_rdk_server.html) and [establish connection](https://rdk.flexiv.com/manual/establish_connection.html) between the workstation PC and the robot.
 
-The main launch file to start the robot driver is the `rizon.launch.py` - it loads and starts the robot hardware, joint states broadcaster, force torque sensor broadcaster, TCP wrench and TCP pose broadcasters, and robot controller and opens RViZ. The arguments for the launch file are as follows:
+The main launch file to start the robot driver is the `rizon.launch.py` - it loads and starts the robot hardware, joint states broadcaster, Flexiv robot states broadcasters, and robot controller and opens RViZ. The arguments for the launch file are as follows:
 
-- `robot_ip` (*required*) - IP address of the robot server (remote).
-- `local_ip` (*required*) - IP address of the workstation PC (local).
-- `rizon_type` (default: *rizon4*) - type of the Flexiv Rizon robot. (rizon4, rizon4s or rizon10)
+- `robot_sn` (*required*) - Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456
+- `rizon_type` (default: *rizon4*) - type of the Flexiv Rizon robot. (rizon4, rizon4s, rizon10 or rizon10s)
 - `use_fake_hardware` (default: *false*) - starts `FakeSystem` instead of real hardware. This is a simple simulation that mimics joint command to their states.
 - `start_rviz` (deafult: *true*) - starts RViz automatically with the launch file.
 - `fake_sensor_commands` (default: *false*) - enables fake command interfaces for sensors used for simulations. Used only if `use_fake_hardware` parameter is true.
@@ -109,13 +127,13 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
    - Test with real robot:
 
      ```bash
-     ros2 launch flexiv_bringup rizon.launch.py robot_ip:=[robot_ip] local_ip:=[local_ip] rizon_type:=rizon4
+     ros2 launch flexiv_bringup rizon.launch.py robot_sn:=[robot_sn] rizon_type:=rizon4
      ```
 
    - Test with fake hardware (`ros2_control` capability):
 
      ```bash
-     ros2 launch flexiv_bringup rizon.launch.py robot_ip:=dont-care local_ip:=dont-care use_fake_hardware:=true
+     ros2 launch flexiv_bringup rizon.launch.py robot_sn:=dont-care use_fake_hardware:=true
      ```
 
 > [!TIP]
@@ -133,7 +151,7 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
    - To test another controller, define it using the `robot_controller` launch argument, for example the `joint_impedance_controller`:
 
      ```bash
-     ros2 launch flexiv_bringup rizon.launch.py robot_ip:=[robot_ip] local_ip:=[local_ip] robot_controller:=joint_impedance_controller
+     ros2 launch flexiv_bringup rizon.launch.py robot_sn:=[robot_sn] robot_controller:=joint_impedance_controller
      ```
 
      Open a new terminal and run the launch file:
@@ -155,27 +173,24 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
 You can also run the MoveIt example and use the `MotionPlanning` plugin in RViZ to start planning:
 
 ```bash
-ros2 launch flexiv_bringup rizon_moveit.launch.py robot_ip:=[robot_ip] local_ip:=[local_ip]
+ros2 launch flexiv_bringup rizon_moveit.launch.py robot_sn:=[robot_sn]
 ```
 
 Test with fake hardware:
 
 ```bash
-ros2 launch flexiv_bringup rizon_moveit.launch.py robot_ip:=dont-care local_ip:=dont-care use_fake_hardware:=true
+ros2 launch flexiv_bringup rizon_moveit.launch.py robot_sn:=dont-care use_fake_hardware:=true
 ```
 
 ### Robot States
 
 The robot driver (`rizon.launch.py`) publishes the following feedback states to the respective ROS topics:
 
+- `/${robot_sn}/flexiv_robot_states`: [Flexiv robot states](https://rdk.flexiv.com/api/structflexiv_1_1rdk_1_1_robot_states.html#details) including the joint- and Cartesian-space robot states. [[`flexiv_msgs/msg/RobotStates.msg`](flexiv_msgs/msg/RobotStates.msg)]
 - `/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_robot_states_broadcaster/tcp_pose`: Measured TCP pose expressed in world 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)]
+- `/flexiv_robot_states_broadcaster/external_wrench_in_tcp`: Estimated external wrench applied on TCP and expressed in TCP frame $^{TCP}F_{ext}$ in force $[N]$ and torque $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
+- `/flexiv_robot_states_broadcaster/external_wrench_in_world`: Estimated external wrench applied on TCP and expressed in world frame $^{0}F_{ext}$ in force $[N]$ and torque $[Nm]$. [[`geometry_msgs/WrenchStamped.msg`](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/WrenchStamped.html)]
 
 ### GPIO
 

flexiv_bringup/README.md

@@ -2,7 +2,7 @@
 
 This package contains launch files: the main driver launcher, the MoveIt launch file and demo examples:
 
-- `rizon.launch.py` - the main launcher: starts *ros2_control* node including hardware interface, runs joint states, force torque sensor, external TCP wrench and TCP pose broadcasters, and a controller, and visualizes the current robot pose in RViZ. The default controller is `rizon_arm_controller`, a joint trajectory controller.
+- `rizon.launch.py` - the main launcher: starts *ros2_control* node including hardware interface, runs joint states, Flexiv robot states broadcaster, and a controller, and visualizes the current robot pose in RViZ. The default controller is `rizon_arm_controller`, a joint trajectory controller.
 - `rizon_moveit.launch.py` - runs MoveIt together with the driver. The controller for robot joints started in this launch file is *rizon_arm_controller*.
 - `test_joint_trajectory_controller.launch` - sends joint trajectory goals to the *rizon_arm_controller*.
 - `sine_sweep_position.launch.py` - gets current joint states and then performs a sine-sweep motion with *forward_position_controller*.

flexiv_bringup/config/rizon_controllers.yaml

@@ -11,46 +11,14 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    force_torque_sensor_broadcaster:
-      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
-
     joint_impedance_controller:
-      type: flexiv_controllers/JointImpedanceController
+      type: joint_impedance_controller/JointImpedanceController
 
     gpio_controller:
-      type: flexiv_controllers/GPIOController
-
-    external_wrench_in_base_broadcaster:
-      type: flexiv_controllers/ExternalTcpWrenchBroadcaster
-
-    external_wrench_in_tcp_broadcaster:
-      type: flexiv_controllers/ExternalTcpWrenchBroadcaster
-
-    tcp_pose_state_broadcaster:
-      type: flexiv_controllers/TcpPoseStateBroadcaster
-
-force_torque_sensor_broadcaster:
-  ros__parameters:
-    sensor_name: force_torque_sensor
-    frame_id: flange
+      type: gpio_controller/GPIOController
 
-external_wrench_in_tcp_broadcaster:
-  ros__parameters:
-    sensor_name: external_wrench_in_tcp
-    frame_id: flange
-    topic_name: external_wrench_in_tcp
-
-external_wrench_in_base_broadcaster:
-  ros__parameters:
-    sensor_name: external_wrench_in_base
-    frame_id: flange
-    topic_name: external_wrench_in_base
-
-tcp_pose_state_broadcaster:
-  ros__parameters:
-    sensor_name: tcp_pose
-    frame_id: flange
-    topic_name: tcp_pose
+    flexiv_robot_states_broadcaster:
+      type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
 
 forward_position_controller:
   ros__parameters:

flexiv_bringup/launch/rizon.launch.py

@@ -1,6 +1,6 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, RegisterEventHandler
-from launch.conditions import IfCondition
+from launch.conditions import IfCondition, UnlessCondition
 from launch.event_handlers import OnProcessExit
 from launch.substitutions import (
     Command,
@@ -14,8 +14,7 @@
 
 def generate_launch_description():
     rizon_type_param_name = "rizon_type"
-    robot_ip_param_name = "robot_ip"
-    local_ip_param_name = "local_ip"
+    robot_sn_param_name = "robot_sn"
     start_rviz_param_name = "start_rviz"
     use_fake_hardware_param_name = "use_fake_hardware"
     fake_sensor_commands_param_name = "fake_sensor_commands"
@@ -29,21 +28,14 @@ def generate_launch_description():
             rizon_type_param_name,
             description="Type of the Flexiv Rizon robot.",
             default_value="rizon4",
-            choices=["rizon4", "rizon4s", "rizon10"],
+            choices=["rizon4", "rizon4s", "rizon10", "rizon10s"],
         )
     )
 
     declared_arguments.append(
         DeclareLaunchArgument(
-            robot_ip_param_name,
-            description="IP address of the robot server (remote).",
-        )
-    )
-
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            local_ip_param_name,
-            description="IP address of the workstation PC (local).",
+            robot_sn_param_name,
+            description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456",
         )
     )
 
@@ -82,8 +74,7 @@ def generate_launch_description():
 
     # Initialize Arguments
     rizon_type = LaunchConfiguration(rizon_type_param_name)
-    robot_ip = LaunchConfiguration(robot_ip_param_name)
-    local_ip = LaunchConfiguration(local_ip_param_name)
+    robot_sn = LaunchConfiguration(robot_sn_param_name)
     start_rviz = LaunchConfiguration(start_rviz_param_name)
     use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
     fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
@@ -101,11 +92,8 @@ def generate_launch_description():
             " ",
             flexiv_urdf_xacro,
             " ",
-            "robot_ip:=",
-            robot_ip,
-            " ",
-            "local_ip:=",
-            local_ip,
+            "robot_sn:=",
+            robot_sn,
             " ",
             "name:=",
             "rizon",
@@ -145,7 +133,7 @@ def generate_launch_description():
     ros2_control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[robot_description, robot_controllers],
+        parameters=[robot_description, robot_controllers, {"robot_sn": robot_sn}],
         output="both",
     )
 
@@ -176,48 +164,13 @@ def generate_launch_description():
         ],
     )
 
-    # 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(
+    # Run Flexiv robot states broadcaster
+    flexiv_robot_states_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",
-        ],
+        arguments=["flexiv_robot_states_broadcaster"],
+        parameters=[{"robot_sn": robot_sn}],
+        condition=UnlessCondition(use_fake_hardware),
     )
 
     # Run gpio controller
@@ -249,10 +202,7 @@ def generate_launch_description():
         ros2_control_node,
         robot_state_publisher_node,
         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,
+        flexiv_robot_states_broadcaster_spawner,
         gpio_controller_spawner,
         delay_rviz_after_joint_state_broadcaster_spawner,
         delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,