Release/Flexiv ROS 2 Humble v1.6 (#43)

munseng-flexiv · web-flow · commit c0e815ea2692 · 2025-01-13T18:05:55.000+08:00
diff --git a/README.md b/README.md
@@ -207,10 +207,10 @@ ros2 topic pub /gpio_controller/gpio_outputs flexiv_msgs/msg/GPIOStates "{states
 
 The gripper control is implemented in the `flexiv_gripper` package to interface with the gripper that is connected to the robot.
 
-Start the `flexiv_gripper_node` with the following launch file:
+Start the `flexiv_gripper_node` with the following launch file, the default gripper is Flexiv Grav (Flexiv-GN01):
 
 ```bash
-ros2 launch flexiv_gripper flexiv_gripper.launch.py robot_sn:=[robot_sn]
+ros2 launch flexiv_gripper flexiv_gripper.launch.py robot_sn:=[robot_sn] gripper_name:=Flexiv-GN01
 ```
 
 Or, you can also start the gripper control with the robot driver if the gripper is Flexiv Grav:
diff --git a/flexiv_bringup/launch/rizon.launch.py b/flexiv_bringup/launch/rizon.launch.py
@@ -23,6 +23,7 @@ def generate_launch_description():
     robot_sn_param_name = "robot_sn"
     start_rviz_param_name = "start_rviz"
     load_gripper_param_name = "load_gripper"
+    gripper_name_param_name = "gripper_name"
     use_fake_hardware_param_name = "use_fake_hardware"
     fake_sensor_commands_param_name = "fake_sensor_commands"
     robot_controller_param_name = "robot_controller"
@@ -50,7 +51,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             start_rviz_param_name,
             default_value="true",
-            description="start RViz automatically with the launch file",
+            description="Start RViz automatically with the launch file",
         )
     )
 
@@ -62,6 +63,14 @@ def generate_launch_description():
         )
     )
 
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            gripper_name_param_name,
+            default_value="Flexiv-GN01",
+            description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
+        )
+    )
+
     declared_arguments.append(
         DeclareLaunchArgument(
             use_fake_hardware_param_name,
@@ -92,6 +101,7 @@ def generate_launch_description():
     robot_sn = LaunchConfiguration(robot_sn_param_name)
     start_rviz = LaunchConfiguration(start_rviz_param_name)
     load_gripper = LaunchConfiguration(load_gripper_param_name)
+    gripper_name = LaunchConfiguration(gripper_name_param_name)
     use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
     fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
     robot_controller = LaunchConfiguration(robot_controller_param_name)
@@ -121,6 +131,9 @@ def generate_launch_description():
                 "load_gripper:=",
                 load_gripper,
                 " ",
+                "gripper_name:=",
+                gripper_name,
+                " ",
                 "use_fake_hardware:=",
                 use_fake_hardware,
                 " ",
@@ -226,6 +239,7 @@ def generate_launch_description():
         ),
         launch_arguments={
             "robot_sn": robot_sn,
+            "gripper_name": gripper_name,
             "use_fake_hardware": use_fake_hardware,
         }.items(),
         condition=IfCondition(load_gripper),
diff --git a/flexiv_bringup/launch/rizon_moveit.launch.py b/flexiv_bringup/launch/rizon_moveit.launch.py
@@ -40,6 +40,7 @@ def generate_launch_description():
     robot_sn_param_name = "robot_sn"
     start_rviz_param_name = "start_rviz"
     load_gripper_param_name = "load_gripper"
+    gripper_name_param_name = "gripper_name"
     use_fake_hardware_param_name = "use_fake_hardware"
     fake_sensor_commands_param_name = "fake_sensor_commands"
     warehouse_sqlite_path_param_name = "warehouse_sqlite_path"
@@ -68,7 +69,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             start_rviz_param_name,
             default_value="true",
-            description="start RViz automatically with the launch file",
+            description="Start RViz automatically with the launch file",
         )
     )
 
@@ -80,6 +81,14 @@ def generate_launch_description():
         )
     )
 
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            gripper_name_param_name,
+            default_value="Flexiv-GN01",
+            description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
+        )
+    )
+
     declared_arguments.append(
         DeclareLaunchArgument(
             use_fake_hardware_param_name,
@@ -117,6 +126,7 @@ def generate_launch_description():
     robot_sn = LaunchConfiguration(robot_sn_param_name)
     start_rviz = LaunchConfiguration(start_rviz_param_name)
     load_gripper = LaunchConfiguration(load_gripper_param_name)
+    gripper_name = LaunchConfiguration(gripper_name_param_name)
     use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
     fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
     warehouse_sqlite_path = LaunchConfiguration(warehouse_sqlite_path_param_name)
@@ -147,6 +157,9 @@ def generate_launch_description():
                 "load_gripper:=",
                 load_gripper,
                 " ",
+                "gripper_name:=",
+                gripper_name,
+                " ",
                 "use_fake_hardware:=",
                 use_fake_hardware,
                 " ",
@@ -360,6 +373,7 @@ def generate_launch_description():
         ),
         launch_arguments={
             "robot_sn": robot_sn,
+            "gripper_name": gripper_name,
             "use_fake_hardware": use_fake_hardware,
         }.items(),
         condition=IfCondition(load_gripper),
diff --git a/flexiv_bringup/package.xml b/flexiv_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexiv_bringup</name>
-  <version>1.5.1</version>
+  <version>1.6.0</version>
   <description>Package with launch files and run-time configurations for Flexiv robots with `ros2_control`</description>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
   <license>Apache License 2.0</license>
diff --git a/flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states.hpp b/flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states.hpp
@@ -65,7 +65,6 @@ class FlexivRobotStates : public SemanticComponentInterface<flexiv_msgs::msg::Ro
     void init_robot_states_message(flexiv_msgs::msg::RobotStates& message)
     {
         message.tcp_pose.header.frame_id = kWorldFrameId;
-        message.tcp_pose_des.header.frame_id = kWorldFrameId;
         message.tcp_vel.header.frame_id = kWorldFrameId;
         message.flange_pose.header.frame_id = kWorldFrameId;
         message.ft_sensor_raw.header.frame_id = kFlangeFrameId;
@@ -96,7 +95,6 @@ class FlexivRobotStates : public SemanticComponentInterface<flexiv_msgs::msg::Ro
 
         // Update timestamps
         message.tcp_pose.header.stamp = message.header.stamp;
-        message.tcp_pose_des.header.stamp = message.header.stamp;
         message.tcp_vel.header.stamp = message.header.stamp;
         message.flange_pose.header.stamp = message.header.stamp;
         message.ft_sensor_raw.header.stamp = message.header.stamp;
@@ -114,7 +112,6 @@ class FlexivRobotStates : public SemanticComponentInterface<flexiv_msgs::msg::Ro
         message.tau_ext = toJointStateMsg(flexiv_robot_states_ptr->tau_ext);
 
         message.tcp_pose.pose = toPoseMsg(flexiv_robot_states_ptr->tcp_pose);
-        message.tcp_pose_des.pose = toPoseMsg(flexiv_robot_states_ptr->tcp_pose_des);
         message.tcp_vel.accel = toAccelMsg(flexiv_robot_states_ptr->tcp_vel);
         message.flange_pose.pose = toPoseMsg(flexiv_robot_states_ptr->flange_pose);
         message.ft_sensor_raw.wrench = toWrenchMsg(flexiv_robot_states_ptr->ft_sensor_raw);
diff --git a/flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states_broadcaster.hpp b/flexiv_controllers/flexiv_robot_states_broadcaster/include/flexiv_robot_states_broadcaster/flexiv_robot_states_broadcaster.hpp
@@ -17,7 +17,7 @@
 #include "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.h"
+#include "realtime_tools/realtime_publisher.hpp"
 
 namespace flexiv_robot_states_broadcaster {
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
@@ -63,7 +63,6 @@ class FlexivRobotStatesBroadcaster : public controller_interface::ControllerInte
 
     using PoseStampedPublisher = rclcpp::Publisher<geometry_msgs::msg::PoseStamped>;
     std::shared_ptr<PoseStampedPublisher> tcp_pose_publisher_;
-    std::shared_ptr<PoseStampedPublisher> tcp_pose_desired_publisher_;
     std::shared_ptr<PoseStampedPublisher> flange_pose_publisher_;
 
     using AccelStampedPublisher = rclcpp::Publisher<geometry_msgs::msg::AccelStamped>;
diff --git a/flexiv_controllers/flexiv_robot_states_broadcaster/package.xml b/flexiv_controllers/flexiv_robot_states_broadcaster/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexiv_robot_states_broadcaster</name>
-  <version>1.5.1</version>
+  <version>1.6.0</version>
   <description>The robot states broadcaster publishes the Flexiv robot states.</description>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
   <license>Apache License 2.0</license>
diff --git a/flexiv_controllers/flexiv_robot_states_broadcaster/src/flexiv_robot_states_broadcaster.cpp b/flexiv_controllers/flexiv_robot_states_broadcaster/src/flexiv_robot_states_broadcaster.cpp
@@ -68,8 +68,6 @@ CallbackReturn FlexivRobotStatesBroadcaster::on_configure(
     // Create the publishers for the robot states
     tcp_pose_publisher_ = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
         kTcpPoseTopic, rclcpp::SystemDefaultsQoS());
-    tcp_pose_desired_publisher_ = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
-        kTcpPoseDesiredTopic, rclcpp::SystemDefaultsQoS());
     tcp_velocity_publisher_ = get_node()->create_publisher<geometry_msgs::msg::AccelStamped>(
         kTcpVelocityTopic, rclcpp::SystemDefaultsQoS());
     flange_pose_publisher_ = get_node()->create_publisher<geometry_msgs::msg::PoseStamped>(
@@ -120,7 +118,6 @@ controller_interface::return_type FlexivRobotStatesBroadcaster::update(
 
         const auto& flexiv_robot_states_msg = realtime_flexiv_robot_states_publisher_->msg_;
         tcp_pose_publisher_->publish(flexiv_robot_states_msg.tcp_pose);
-        tcp_pose_desired_publisher_->publish(flexiv_robot_states_msg.tcp_pose_des);
         tcp_velocity_publisher_->publish(flexiv_robot_states_msg.tcp_vel);
         flange_pose_publisher_->publish(flexiv_robot_states_msg.flange_pose);
         ft_sensor_publisher_->publish(flexiv_robot_states_msg.ft_sensor_raw);
diff --git a/flexiv_controllers/gpio_controller/package.xml b/flexiv_controllers/gpio_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>gpio_controller</name>
-  <version>1.5.1</version>
+  <version>1.6.0</version>
   <description>Flexiv custom gpio controller to control the GPIOs of the robot.</description>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
   <license>Apache License 2.0</license>
diff --git a/flexiv_controllers/joint_impedance_controller/include/joint_impedance_controller/joint_impedance_controller.hpp b/flexiv_controllers/joint_impedance_controller/include/joint_impedance_controller/joint_impedance_controller.hpp
@@ -20,7 +20,7 @@
 #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
-#include "realtime_tools/realtime_buffer.h"
+#include "realtime_tools/realtime_buffer.hpp"
 
 namespace joint_impedance_controller {
 using CmdType = flexiv_msgs::msg::JointPosVel;
diff --git a/flexiv_controllers/joint_impedance_controller/package.xml b/flexiv_controllers/joint_impedance_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>joint_impedance_controller</name>
-  <version>1.5.1</version>
+  <version>1.6.0</version>
   <description> The joint impedance controller commands a group of joints in effort interface.</description>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
   <license>Apache License 2.0</license>
diff --git a/flexiv_description/launch/view_rizon.launch.py b/flexiv_description/launch/view_rizon.launch.py
@@ -16,6 +16,7 @@ def generate_launch_description():
     pkg_share = FindPackageShare("flexiv_description")
     rizon_type = LaunchConfiguration("rizon_type")
     load_gripper = LaunchConfiguration("load_gripper")
+    gripper_name = LaunchConfiguration("gripper_name")
     default_rviz_config_path = PathJoinSubstitution(
         [pkg_share, "rviz", "view_rizon.rviz"]
     )
@@ -36,6 +37,9 @@ def generate_launch_description():
                 " ",
                 "load_gripper:=",
                 load_gripper,
+                " ",
+                "gripper_name:=",
+                gripper_name,
             ]
         ),
         value_type=str,
@@ -86,6 +90,11 @@ def generate_launch_description():
                 default_value="False",
                 description="Flag to load the Flexiv Grav gripper",
             ),
+            DeclareLaunchArgument(
+                name="gripper_name",
+                default_value="Flexiv-GN01",
+                description="Full name of the gripper to be controlled",
+            ),
             DeclareLaunchArgument(
                 name="gui",
                 default_value="False",
diff --git a/flexiv_description/package.xml b/flexiv_description/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexiv_description</name>
-  <version>1.5.1</version>
+  <version>1.6.0</version>
   <description>URDF description for Flexiv robots</description>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
   <license>Apache License 2.0</license>
diff --git a/flexiv_description/urdf/rizon.urdf.xacro b/flexiv_description/urdf/rizon.urdf.xacro
@@ -14,6 +14,7 @@
   <xacro:arg name="prefix" default="" />
   <xacro:arg name="robot_sn" default="" />
   <xacro:arg name="load_gripper" default="false" />
+  <xacro:arg name="gripper_name" default="Flexiv-GN01" />
   <xacro:arg name="use_fake_hardware" default="false" />
   <xacro:arg name="fake_sensor_commands" default="false" />
   <xacro:arg name="initial_positions_file" default="$(find flexiv_description)/config/initial_positions.yaml"/>
@@ -26,6 +27,7 @@
     prefix="$(arg prefix)"
     robot_sn="$(arg robot_sn)"
     load_gripper="$(arg load_gripper)"
+    gripper_name="$(arg gripper_name)"
     use_fake_hardware="$(arg use_fake_hardware)"
     fake_sensor_commands="$(arg fake_sensor_commands)"
     initial_positions="${xacro.load_yaml(initial_positions_file)}">
diff --git a/flexiv_description/urdf/rizon_macro.xacro b/flexiv_description/urdf/rizon_macro.xacro
@@ -6,13 +6,14 @@
     prefix
     robot_sn
     load_gripper:=false
+    gripper_name:='Flexiv-GN01'
     use_fake_hardware:=false
     fake_sensor_commands:=false
     initial_positions:=${dict(joint1=0.0,joint2=-0.69813,joint3=0.0,joint4=1.570796,joint5=0.0,joint6=0.69813,joint7=0.0)}
     ">
 
     <!-- Load gripper -->
-    <xacro:if value="${load_gripper}">
+    <xacro:if value="${load_gripper and gripper_name=='Flexiv-GN01'}">
       <xacro:include filename="$(find flexiv_description)/urdf/grav_macro.xacro" />
       <xacro:flexiv_GN01 prefix="${prefix}" mesh_prefix_path="package://flexiv_description/" />
     </xacro:if>
diff --git a/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp b/flexiv_gripper/include/flexiv_gripper/gripper_action_server.hpp
@@ -27,6 +27,7 @@
 #include "flexiv/rdk/data.hpp"
 #include "flexiv/rdk/gripper.hpp"
 #include "flexiv/rdk/robot.hpp"
+#include "flexiv/rdk/tool.hpp"
 
 namespace {
 
@@ -95,6 +96,7 @@ class GripperActionServer : public rclcpp::Node
     // Flexiv RDK
     std::unique_ptr<flexiv::rdk::Robot> robot_;
     std::unique_ptr<flexiv::rdk::Gripper> gripper_;
+    std::unique_ptr<flexiv::rdk::Tool> tool_;
 
     rclcpp_action::Server<Grasp>::SharedPtr grasp_action_server_;
     rclcpp_action::Server<Move>::SharedPtr move_action_server_;
@@ -104,7 +106,6 @@ class GripperActionServer : public rclcpp::Node
 
     std::mutex gripper_states_mutex_;
     flexiv::rdk::GripperStates current_gripper_states_;
-    std::atomic<bool> is_gripper_moving_ = {false};
 
     double default_velocity_;
     double default_max_force_;
@@ -242,7 +243,7 @@ class GripperActionServer : public rclcpp::Node
         std::lock_guard<std::mutex> lock(gripper_states_mutex_);
         feedback->current_width = current_gripper_states_.width;
         feedback->current_force = current_gripper_states_.force;
-        feedback->moving = is_gripper_moving_;
+        feedback->moving = current_gripper_states_.is_moving;
         goal_handle->publish_feedback(feedback);
     }
 
diff --git a/flexiv_gripper/launch/flexiv_gripper.launch.py b/flexiv_gripper/launch/flexiv_gripper.launch.py
@@ -11,6 +11,7 @@
 
 def generate_launch_description():
     robot_sn_param_name = "robot_sn"
+    gripper_name_param_name = "gripper_name"
     use_fake_hardware_param_name = "use_fake_hardware"
     gripper_joint_names_param_name = "gripper_joint_names"
 
@@ -24,6 +25,14 @@ def generate_launch_description():
         )
     )
 
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            gripper_name_param_name,
+            description="Full name of the gripper to be controlled, can be found in Flexiv Elements -> Settings -> Device",
+            default_value="Flexiv-GN01",
+        )
+    )
+
     declared_arguments.append(
         DeclareLaunchArgument(
             use_fake_hardware_param_name,
@@ -42,6 +51,7 @@ def generate_launch_description():
 
     # Initialize arguments
     robot_sn = LaunchConfiguration(robot_sn_param_name)
+    gripper_name = LaunchConfiguration(gripper_name_param_name)
     use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
     gripper_joint_names = LaunchConfiguration(gripper_joint_names_param_name)
 
@@ -55,7 +65,11 @@ def generate_launch_description():
         executable="flexiv_gripper_node",
         name="flexiv_gripper_node",
         parameters=[
-            {"robot_sn": robot_sn, "gripper_joint_names": gripper_joint_names},
+            {
+                "robot_sn": robot_sn,
+                "gripper_name": gripper_name,
+                "gripper_joint_names": gripper_joint_names,
+            },
             gripper_config_file,
         ],
         condition=UnlessCondition(use_fake_hardware),
diff --git a/flexiv_gripper/package.xml b/flexiv_gripper/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>flexiv_gripper</name>
-  <version>1.5.1</version>
+  <version>1.6.0</version>
   <description>Package implementing the action server of Flexiv gripper control</description>
   <maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
   <license>Apache License 2.0</license>
diff --git a/flexiv_gripper/src/gripper_action_server.cpp b/flexiv_gripper/src/gripper_action_server.cpp
diff --git a/flexiv_hardware/CMakeLists.txt b/flexiv_hardware/CMakeLists.txt
diff --git a/flexiv_hardware/package.xml b/flexiv_hardware/package.xml
diff --git a/flexiv_hardware/rdk b/flexiv_hardware/rdk
diff --git a/flexiv_moveit_config/package.xml b/flexiv_moveit_config/package.xml
diff --git a/flexiv_msgs/msg/RobotStates.msg b/flexiv_msgs/msg/RobotStates.msg
diff --git a/flexiv_msgs/package.xml b/flexiv_msgs/package.xml
diff --git a/flexiv_test_nodes/package.xml b/flexiv_test_nodes/package.xml
