Release/Flexiv ROS 2 Humble v1.7 (#46)

Author: munseng-flexiv

README.md

@@ -112,7 +112,7 @@ The prerequisites of using ROS 2 with Flexiv Rizon robot are [enable RDK on the
 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_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)
+- `rizon_type` (default: *Rizon4*) - type of the Flexiv Rizon robot. (Rizon4, Rizon4M, Rizon4R, Rizon4s, Rizon10 or Rizon10s)
 - `load_gripper` (default: *false*) - loads the Flexiv Grav gripper as the end-effector of the robot and the gripper control node.
 - `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.
@@ -128,7 +128,7 @@ 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_sn:=[robot_sn] rizon_type:=rizon4
+     ros2 launch flexiv_bringup rizon.launch.py robot_sn:=[robot_sn] rizon_type:=Rizon4
      ```
 
    - Test with fake hardware (`ros2_control` capability):

flexiv_bringup/launch/rizon.launch.py

@@ -35,8 +35,8 @@ def generate_launch_description():
         DeclareLaunchArgument(
             rizon_type_param_name,
             description="Type of the Flexiv Rizon robot.",
-            default_value="rizon4",
-            choices=["rizon4", "rizon4s", "rizon10", "rizon10s"],
+            default_value="Rizon4",
+            choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
         )
     )
 
@@ -255,14 +255,6 @@ def generate_launch_description():
         condition=UnlessCondition(use_fake_hardware),
     )
 
-    # 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(
@@ -273,6 +265,14 @@ def generate_launch_description():
         )
     )
 
+    # Delay rviz start after `robot_controller_spawner`
+    delay_rviz_after_robot_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=robot_controller_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
     nodes = [
         ros2_control_node,
         joint_state_publisher_node,
@@ -281,8 +281,8 @@ def generate_launch_description():
         flexiv_robot_states_broadcaster_spawner,
         load_gripper_launch,
         gpio_controller_spawner,
-        delay_rviz_after_joint_state_broadcaster_spawner,
         delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
+        delay_rviz_after_robot_controller_spawner,
     ]
 
     return LaunchDescription(declared_arguments + nodes)

flexiv_bringup/launch/rizon_moveit.launch.py

@@ -116,6 +116,8 @@ def launch_setup(context):
         "robot_description_semantic": robot_description_semantic_content
     }
 
+    publish_robot_description_semantic = {"publish_robot_description_semantic": True}
+
     robot_description_kinematics = PathJoinSubstitution(
         [FindPackageShare("flexiv_moveit_config"), "config", "kinematics.yaml"]
     )
@@ -179,12 +181,13 @@ def launch_setup(context):
         parameters=[
             robot_description,
             robot_description_semantic,
+            publish_robot_description_semantic,
             robot_description_kinematics,
+            joint_limits_yaml,
             ompl_planning_pipeline_config,
             trajectory_execution,
             moveit_controllers,
             planning_scene_monitor_parameters,
-            joint_limits_yaml,
             warehouse_ros_config,
         ],
     )
@@ -331,14 +334,6 @@ def launch_setup(context):
         condition=UnlessCondition(use_fake_hardware),
     )
 
-    # 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(
@@ -349,8 +344,23 @@ def launch_setup(context):
         )
     )
 
+    # Delay move_group start after `robot_controller_spawner`
+    delay_move_group_after_robot_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=robot_controller_spawner,
+            on_exit=[move_group_node],
+        )
+    )
+
+    # Delay rviz start after `robot_controller_spawner`
+    delay_rviz_after_robot_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=robot_controller_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
     nodes = [
-        move_group_node,
         ros2_control_node,
         joint_state_publisher_node,
         robot_state_publisher_node,
@@ -359,8 +369,9 @@ def launch_setup(context):
         load_gripper_launch,
         gpio_controller_spawner,
         servo_node,
-        delay_rviz_after_joint_state_broadcaster_spawner,
         delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
+        delay_move_group_after_robot_controller_spawner,
+        delay_rviz_after_robot_controller_spawner,
     ]
 
     return nodes
@@ -374,8 +385,8 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "rizon_type",
             description="Type of the Flexiv Rizon robot.",
-            default_value="rizon4",
-            choices=["rizon4", "rizon4s", "rizon10", "rizon10s"],
+            default_value="Rizon4",
+            choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
         )
     )
 

flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states_broadcaster.hpp

@@ -14,7 +14,7 @@
 #include "controller_interface/controller_interface.hpp"
 #include "flexiv_msgs/msg/robot_states.hpp"
 #include "flexiv_robot_states_broadcaster/flexiv_robot_states.hpp"
-#include "flexiv_robot_states_broadcaster_parameters.hpp"
+#include "flexiv_robot_states_broadcaster/flexiv_robot_states_broadcaster_parameters.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_publisher.hpp"

flexiv_controllers/gpio_controller/include/gpio_controller/gpio_controller.hpp

@@ -14,7 +14,7 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
-#include "gpio_controller_parameters.hpp"
+#include "gpio_controller/gpio_controller_parameters.hpp"
 #include "flexiv_msgs/msg/gpio_states.hpp"
 
 namespace gpio_controller {

flexiv_description/launch/view_rizon.launch.py

@@ -86,9 +86,16 @@ def generate_launch_description():
             ),
             DeclareLaunchArgument(
                 name="rizon_type",
-                default_value="rizon4",
+                default_value="Rizon4",
                 description="Type of the Flexiv Rizon robot.",
-                choices=["rizon4", "rizon4s", "rizon10", "rizon10s"],
+                choices=[
+                    "Rizon4",
+                    "Rizon4M",
+                    "Rizon4R",
+                    "Rizon4s",
+                    "Rizon10",
+                    "Rizon10s",
+                ],
             ),
             DeclareLaunchArgument(
                 name="load_gripper",