Release/Flexiv ROS 2 Humble 0.9.1 (#27)

Author: munseng-flexiv

.pre-commit-config.yaml

@@ -0,0 +1,45 @@
+# To run checks over all the files in the repo manually:
+#
+#     pre-commit run -a
+#
+# Or run checks automatically every time before commit:
+#
+#     pre-commit install
+#
+# See https://pre-commit.com for more information
+# See https://pre-commit.com/hooks.html for more hooks
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.4.0
+    hooks:
+      - id: trailing-whitespace
+      - id: end-of-file-fixer
+      - id: check-yaml
+        args: [--allow-multiple-documents]
+      - id: check-added-large-files
+        args: [--maxkb=500]
+      - id: pretty-format-json
+        args: [--no-sort-keys, --autofix, --indent=4]
+
+  - repo: https://github.com/myint/docformatter
+    rev: v1.7.5
+    hooks:
+      - id: docformatter
+        args: [--in-place]
+
+  - repo: https://github.com/psf/black
+    rev: 23.9.1
+    hooks:
+      - id: black
+        language_version: python3
+        args: [--line-length=88]
+
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format -i
+        language: system
+        files: \.(c|cc|cxx|cpp|cu|h|hh|hpp|hxx|java|js|m|proto)$
+        args: ["-fallback-style=none"]

README.md

@@ -76,16 +76,17 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04) and Humble (Ubuntu 22.0
    source install/setup.bash
    ```
 
-**NOTE**: Remember to source the setup file and the workspace whenever a new terminal is opened:
-
-```bash
-source /opt/ros/humble/setup.bash
-source ~/flexiv_ros2_ws/install/setup.bash
-```
+> [!NOTE]
+> Remember to source the setup file and the workspace whenever a new terminal is opened:
+> ```bash
+> source /opt/ros/humble/setup.bash
+> source ~/flexiv_ros2_ws/install/setup.bash
+> ```
 
 ## 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.
+> [!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 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.
 
@@ -111,13 +112,15 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
      ros2 launch flexiv_bringup rizon.launch.py robot_ip:=[robot_ip] local_ip:=[local_ip] rizon_type:=rizon4
      ```
 
-     **NOTE**: To test whether the connection between ROS and the robot is established, you could disable the starting of RViz first by setting the `start_rviz` launch argument to false.
    - 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
      ```
 
+> [!TIP]
+> To test whether the connection between ROS and the robot is established, you could disable the starting of RViz first by setting the `start_rviz` launch argument to false.
+
 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:
@@ -133,8 +136,6 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
      ros2 launch flexiv_bringup rizon.launch.py robot_ip:=[robot_ip] local_ip:=[local_ip] robot_controller:=joint_impedance_controller
      ```
 
-     **NOTE**: The command starts the robot in the joint torque mode. In this mode, gravity and friction are compensated **only** for the robot **without** any attached objects (e.g. the gripper, camera).
-
      Open a new terminal and run the launch file:
 
      ```bash
@@ -143,7 +144,11 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
 
      The robot should run a sine-sweep motion with joint impedance control.
 
-     **NOTE**: joint impedance control is not supported in fake/simulated hardware.
+> [!NOTE]
+> The command starts the robot in the joint torque mode. In this mode, gravity and friction are compensated **only** for the robot **without** any attached objects (e.g. the gripper, camera).
+
+> [!NOTE]
+> Joint impedance control is not supported in fake/simulated hardware.
 
 ### Using MoveIt
 
@@ -169,4 +174,15 @@ The robot driver (`rizon.launch.py`) publishes the following feedback states to
 - `/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`
+
+### GPIO
+
+All digital inputs on the robot control box can be accessed via the ROS topic `/gpio_controller/gpio_inputs`, which publishes the current state of all the 16 digital input ports *(True: port high, false: port low)*.
+
+The digital output ports on the control box can be set by publishing to the topic `/gpio_controller/gpio_outputs`. For example:
+
+```bash
+ros2 topic pub /gpio_controller/gpio_outputs flexiv_msgs/msg/GPIOStates "{states: [{pin: 0, state: true}, {pin: 2, state: true}]}"
+```

flexiv_bringup/config/rizon_controllers.yaml

@@ -5,27 +5,30 @@ controller_manager:
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
-    joint_impedance_controller:
-      type: flexiv_controllers/JointImpedanceController
+    rizon_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
 
     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
+
+    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
 
-    rizon_arm_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
 force_torque_sensor_broadcaster:
   ros__parameters:
     sensor_name: force_torque_sensor
@@ -40,7 +43,7 @@ external_wrench_in_tcp_broadcaster:
 external_wrench_in_base_broadcaster:
   ros__parameters:
     sensor_name: external_wrench_in_base
-    frame_id: base_link
+    frame_id: flange
     topic_name: external_wrench_in_base
 
 tcp_pose_state_broadcaster:
@@ -91,7 +94,6 @@ rizon_arm_controller:
     state_publish_rate: 100.0
     action_monitor_rate: 20.0
     allow_partial_joints_goal: false
-    allow_integration_in_goal_trajectories: true
     constraints:
       stopped_velocity_tolerance: 0.01
       goal_time: 0.0

flexiv_bringup/launch/rizon.launch.py

@@ -2,7 +2,12 @@
 from launch.actions import DeclareLaunchArgument, RegisterEventHandler
 from launch.conditions import IfCondition
 from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
@@ -118,7 +123,7 @@ def generate_launch_description():
     )
     robot_description = {"robot_description": robot_description_content}
 
-    # RViZ    
+    # RViZ
     rviz_config_file = PathJoinSubstitution(
         [FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"]
     )
@@ -165,50 +170,79 @@ def generate_launch_description():
     joint_state_broadcaster_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+        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"],
+        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"],
+        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"],
+        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=[
+            "tcp_pose_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    # Run gpio controller
+    gpio_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["gpio_controller", "--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],
+    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = (
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=joint_state_broadcaster_spawner,
+                on_exit=[robot_controller_spawner],
+            )
         )
     )
 
@@ -220,6 +254,7 @@ def generate_launch_description():
         external_wrench_in_base_broadcaster_spawner,
         external_wrench_in_tcp_broadcaster_spawner,
         tcp_pose_state_broadcaster_spawner,
+        gpio_controller_spawner,
         delay_rviz_after_joint_state_broadcaster_spawner,
         delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
     ]