Feature/Set robot_sn as default prefix to the robot joint and link names (#45)

Author: munseng-flexiv

README.md

@@ -134,7 +134,7 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
    - Test with fake hardware (`ros2_control` capability):
 
      ```bash
-     ros2 launch flexiv_bringup rizon.launch.py robot_sn:=dont-care use_fake_hardware:=true
+     ros2 launch flexiv_bringup rizon.launch.py robot_sn:=Rizon4-123456 use_fake_hardware:=true
      ```
 
 > [!TIP]
@@ -145,7 +145,7 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
    - To send the goal position to the controller by using the node from `flexiv_test_nodes`, start the following command in a new terminal:
 
      ```bash
-     ros2 launch flexiv_bringup test_joint_trajectory_controller.launch.py
+     ros2 launch flexiv_bringup test_joint_trajectory_controller.launch.py robot_sn:=[robot_sn]
      ```
 
      The joint position goals can be changed in `flexiv_bringup/config/joint_trajectory_position_publisher.yaml`
@@ -158,7 +158,7 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
      Open a new terminal and run the launch file:
 
      ```bash
-     ros2 launch flexiv_bringup sine_sweep_impedance.launch.py
+     ros2 launch flexiv_bringup sine_sweep_impedance.launch.py robot_sn:=[robot_sn]
      ```
 
      The robot should run a sine-sweep motion with joint impedance control.
@@ -180,7 +180,7 @@ 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_sn:=dont-care use_fake_hardware:=true
+ros2 launch flexiv_bringup rizon_moveit.launch.py robot_sn:=Rizon4-123456 use_fake_hardware:=true
 ```
 
 ### Robot States
@@ -189,18 +189,18 @@ The robot driver (`rizon.launch.py`) publishes the following feedback states to
 
 - `/${robot_sn}/flexiv_robot_states`: [Flexiv robot states](https://www.flexiv.com/software/rdk/api/structflexiv_1_1rdk_1_1_robot_states.html) 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)]
-- `/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)]
+- `/${robot_sn}/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)]
+- `/${robot_sn}/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)]
+- `/${robot_sn}/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
 
-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 18 *(16 on control box + 2 inside the wrist connector)* digital input ports *(True: port high, false: port low)*.
+All digital inputs on the robot control box can be accessed via the ROS topic `/{robot_sn}/gpio_inputs`, which publishes the current state of all the 18 *(16 on control box + 2 inside the wrist connector)* 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:
+The digital output ports on the control box can be set by publishing to the topic `/{robot_sn}/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}]}"
+ros2 topic pub /Rizon4_123456/gpio_outputs flexiv_msgs/msg/GPIOStates "{states: [{pin: 0, state: true}, {pin: 2, state: true}]}"
 ```
 
 ### Gripper Control

flexiv_bringup/config/joint_trajectory_position_publisher.yaml

@@ -11,12 +11,12 @@ publisher_joint_trajectory_controller:
     pos4: [-0.698, -0.698, 0.698, 1.57, 0.698, 0.698, 0.698]
 
     joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7
 
     check_starting_point: true

flexiv_bringup/config/rizon_controllers.yaml

@@ -23,37 +23,37 @@ controller_manager:
 forward_position_controller:
   ros__parameters:
     joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7
 
 joint_impedance_controller:
   ros__parameters:
     joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7
     k_p: [3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0]
     k_d: [80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0]
 
 rizon_arm_controller:
   ros__parameters:
     joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7
     command_interfaces:
       - position
     state_interfaces:

flexiv_bringup/config/sine_sweep_impedance_config.yaml

@@ -3,12 +3,12 @@ sine_sweep_impedance_controller:
 
     controller_name: "joint_impedance_controller"
     joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7
     wait_sec_between_publish: 0.001
     speed_scaling: 1.0

flexiv_bringup/config/sine_sweep_position_config.yaml

@@ -3,12 +3,12 @@ sine_sweep_position_controller:
 
     controller_name: "forward_position_controller"
     joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
-      - joint7
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7
     wait_sec_between_publish: 0.001
     speed_scaling: 1.0

flexiv_bringup/launch/rizon.launch.py

@@ -8,7 +8,7 @@
 from launch.event_handlers import OnProcessExit
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
-from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
 from launch_ros.substitutions import FindPackageShare
 from launch.substitutions import (
     Command,
@@ -122,9 +122,6 @@ def generate_launch_description():
                 "robot_sn:=",
                 robot_sn,
                 " ",
-                "name:=",
-                "rizon",
-                " ",
                 "rizon_type:=",
                 rizon_type,
                 " ",
@@ -169,7 +166,11 @@ def generate_launch_description():
     ros2_control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[robot_description, robot_controllers, {"robot_sn": robot_sn}],
+        parameters=[
+            robot_description,
+            ParameterFile(robot_controllers, allow_substs=True),
+            {"robot_sn": robot_sn},
+        ],
         remappings=[("joint_states", "flexiv_arm/joint_states")],
         output="both",
     )
@@ -250,6 +251,8 @@ def generate_launch_description():
         package="controller_manager",
         executable="spawner",
         arguments=["gpio_controller", "--controller-manager", "/controller_manager"],
+        parameters=[{"robot_sn": robot_sn}],
+        condition=UnlessCondition(use_fake_hardware),
     )
 
     # Delay rviz start after `joint_state_broadcaster`