Backport/Update Foxy README (#22)

Author: munseng-flexiv

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

@@ -3,11 +3,9 @@ name: Foxy Binary Build
 on:
   push:
     branches:
-      - main
       - foxy
   pull_request:
     branches:
-      - main
       - foxy
 
 jobs:

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

@@ -0,0 +1,52 @@
+name: Humble Binary Build
+
+on:
+  push:
+    branches:
+      - humble
+  pull_request:
+    branches:
+      - humble
+
+jobs:
+  humble_binary:
+    name: Humble binary job
+    runs-on: ubuntu-22.04
+    container:
+      image: osrf/ros:humble-desktop
+    strategy:
+      fail-fast: false
+    env:
+      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-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]
+        with:
+          target-ros2-distro: ${{ env.ROS_DISTRO }}
+          package-name: |
+            flexiv_bringup
+            flexiv_controllers
+            flexiv_description
+            flexiv_hardware
+            flexiv_moveit_config
+            flexiv_msgs
+            flexiv_test_nodes
+          skip-tests: true

README.md

@@ -1,25 +1,33 @@
 # Flexiv ROS 2
 
-[![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) | [humble](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=humble)](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 Foxy via Debian Packages](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html)
+
 2. Install `colcon` and additional ROS packages:
 
    ```bash
@@ -40,6 +48,7 @@ This project was developed for ROS 2 Foxy on Ubuntu 20.04. Other versions of Ubu
    ros-foxy-robot-state-publisher \
    ros-foxy-rviz2
    ```
+
 3. Setup workspace:
 
    ```bash
@@ -49,13 +58,15 @@ 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
    ```
+
 5. Build and source the workspace:
 
    ```bash
@@ -106,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:
@@ -151,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`
+**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_hardware/src/flexiv_hardware_interface.cpp

@@ -443,8 +443,8 @@ return_type FlexivHardwareInterface::prepare_command_mode_switch(
 }
 
 return_type FlexivHardwareInterface::perform_command_mode_switch(
-    const std::vector<std::string>& start_interfaces,
-    const std::vector<std::string>& stop_interfaces)
+    const std::vector<std::string>& /*start_interfaces*/,
+    const std::vector<std::string>& /*stop_interfaces*/)
 {
     if (stop_modes_.size() != 0
         && std::find(stop_modes_.begin(), stop_modes_.end(),

flexiv_test_nodes/flexiv_test_nodes/sine_sweep_impedance_controller.py

@@ -19,12 +19,12 @@ def __init__(self):
         # Declare all parameters
         self.declare_parameter("controller_name", "joint_impedance_controller")
         self.declare_parameter("wait_sec_between_publish", 0.001)
-        self.declare_parameter("joints")
+        self.declare_parameter("speed_scaling", 1.0)
 
         # Read parameters
         controller_name = self.get_parameter("controller_name").value
         wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value
-        self.joints = self.get_parameter("joints").value
+        self.speed_scaling = self.get_parameter("speed_scaling").value
 
         publish_topic = "/" + controller_name + "/" + "commands"
 
@@ -44,7 +44,7 @@ def timer_callback(self):
             target_pos = self.init_pos.copy()
             for i in range(7):
                 target_pos[i] = self.init_pos[i] + SWING_AMP * math.sin(
-                    2 * math.pi * SWING_FREQ * self.loop_time
+                    2 * math.pi * SWING_FREQ * self.speed_scaling * self.loop_time
                 )
             msg = JointPosVel()
             msg.positions = target_pos