Use pose_broadcaster to publish the TCP pose

Universal_Robots_ROS2_Driver.jazzy.repos

@@ -5,8 +5,8 @@ repositories:
     version: master
   Universal_Robots_ROS2_Description:
     type: git
-    url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git
-    version: rolling
+    url: https://github.com/fmauch/Universal_Robots_ROS2_Description.git
+    version: pose_broadcaster
   ur_msgs:
     type: git
     url: https://github.com/ros-industrial/ur_msgs.git
@@ -17,8 +17,8 @@ repositories:
     version: master
   ros2_controllers:
     type: git
-    url: https://github.com/ros-controls/ros2_controllers
-    version: master
+    url: https://github.com/RobertWilbrandt/ros2_controllers
+    version: pose_broadcaster
   kinematics_interface:
     type: git
     url: https://github.com/ros-controls/kinematics_interface.git

Universal_Robots_ROS2_Driver.rolling.repos

@@ -5,8 +5,8 @@ repositories:
     version: master
   Universal_Robots_ROS2_Description:
     type: git
-    url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git
-    version: rolling
+    url: https://github.com/fmauch/Universal_Robots_ROS2_Description.git
+    version: pose_broadcaster
   ur_msgs:
     type: git
     url: https://github.com/ros-industrial/ur_msgs.git
@@ -17,8 +17,8 @@ repositories:
     version: master
   ros2_controllers:
     type: git
-    url: https://github.com/ros-controls/ros2_controllers
-    version: master
+    url: https://github.com/RobertWilbrandt/ros2_controllers
+    version: pose_broadcaster
   kinematics_interface:
     type: git
     url: https://github.com/ros-controls/kinematics_interface.git

ur_robot_driver/config/ur_controllers.yaml

@@ -24,6 +24,8 @@ controller_manager:
     forward_position_controller:
       type: position_controllers/JointGroupPositionController
 
+    tcp_pose_broadcaster:
+      type: pose_broadcaster/PoseBroadcaster
 
 speed_scaling_state_broadcaster:
   ros__parameters:
@@ -124,3 +126,8 @@ forward_position_controller:
       - $(var tf_prefix)wrist_1_joint
       - $(var tf_prefix)wrist_2_joint
       - $(var tf_prefix)wrist_3_joint
+
+tcp_pose_broadcaster:
+  ros__parameters:
+    frame_id: $(var tf_prefix)base
+    pose_name: $(var tf_prefix)tcp_pose

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -144,6 +144,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector6d_t urcl_joint_efforts_;
   urcl::vector6d_t urcl_ft_sensor_measurements_;
   urcl::vector6d_t urcl_tcp_pose_;
+  urcl::vector3d_t tcp_orientation_rpy_;
+  tf2::Quaternion tcp_rotation_quat_;
 
   bool packet_read_;
 
@@ -172,7 +174,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   // transform stuff
   tf2::Vector3 tcp_force_;
   tf2::Vector3 tcp_torque_;
-  geometry_msgs::msg::TransformStamped tcp_transform_;
 
   // asynchronous commands
   std::array<double, 18> standard_dig_out_bits_cmd_;

ur_robot_driver/launch/ur_control.launch.py

@@ -127,6 +127,7 @@ def launch_setup():
                     "force_torque_sensor_broadcaster",
                     "joint_state_broadcaster",
                     "speed_scaling_state_broadcaster",
+                    "tcp_pose_broadcaster",
                 ]
             },
         ],
@@ -162,6 +163,7 @@ def controller_spawner(controllers, active=True):
         "io_and_status_controller",
         "speed_scaling_state_broadcaster",
         "force_torque_sensor_broadcaster",
+        "tcp_pose_broadcaster",
     ]
     controllers_inactive = ["forward_position_controller"]
 

ur_robot_driver/src/hardware_interface.cpp

@@ -169,9 +169,14 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
                                                                    &speed_scaling_combined_));
 
   for (auto& sensor : info_.sensors) {
-    for (uint j = 0; j < sensor.state_interfaces.size(); ++j) {
-      state_interfaces.emplace_back(hardware_interface::StateInterface(sensor.name, sensor.state_interfaces[j].name,
-                                                                       &urcl_ft_sensor_measurements_[j]));
+    if (sensor.name == tf_prefix + "tcp_fts_sensor") {
+      const std::vector<std::string> fts_names = {
+        "force.x", "force.y", "force.z", "torque.x", "torque.y", "torque.z"
+      };
+      for (uint j = 0; j < 6; ++j) {
+        state_interfaces.emplace_back(
+            hardware_interface::StateInterface(sensor.name, fts_names[j], &urcl_ft_sensor_measurements_[j]));
+      }
     }
   }
 
@@ -231,6 +236,19 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
   state_interfaces.emplace_back(
       hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_));
 
+  state_interfaces.emplace_back(
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.x", &urcl_tcp_pose_[0]));
+  state_interfaces.emplace_back(
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.y", &urcl_tcp_pose_[1]));
+  state_interfaces.emplace_back(
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "position.z", &urcl_tcp_pose_[2]));
+  state_interfaces.emplace_back(
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.r", &tcp_orientation_rpy_[0]));
+  state_interfaces.emplace_back(
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.p", &tcp_orientation_rpy_[1]));
+  state_interfaces.emplace_back(
+      hardware_interface::StateInterface(tf_prefix + "tcp_pose", "orientation.y", &tcp_orientation_rpy_[2]));
+
   return state_interfaces;
 }
 
@@ -765,10 +783,8 @@ void URPositionHardwareInterface::transformForceTorque()
   tcp_torque_.setValue(urcl_ft_sensor_measurements_[3], urcl_ft_sensor_measurements_[4],
                        urcl_ft_sensor_measurements_[5]);
 
-  tf2::Quaternion rotation_quat;
-  tf2::fromMsg(tcp_transform_.transform.rotation, rotation_quat);
-  tcp_force_ = tf2::quatRotate(rotation_quat.inverse(), tcp_force_);
-  tcp_torque_ = tf2::quatRotate(rotation_quat.inverse(), tcp_torque_);
+  tcp_force_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_force_);
+  tcp_torque_ = tf2::quatRotate(tcp_rotation_quat_.inverse(), tcp_torque_);
 
   urcl_ft_sensor_measurements_ = { tcp_force_.x(),  tcp_force_.y(),  tcp_force_.z(),
                                    tcp_torque_.x(), tcp_torque_.y(), tcp_torque_.z() };
@@ -781,17 +797,13 @@ void URPositionHardwareInterface::extractToolPose()
       std::sqrt(std::pow(urcl_tcp_pose_[3], 2) + std::pow(urcl_tcp_pose_[4], 2) + std::pow(urcl_tcp_pose_[5], 2));
 
   tf2::Vector3 rotation_vec(urcl_tcp_pose_[3], urcl_tcp_pose_[4], urcl_tcp_pose_[5]);
-  tf2::Quaternion rotation;
   if (tcp_angle > 1e-16) {
-    rotation.setRotation(rotation_vec.normalized(), tcp_angle);
+    tcp_rotation_quat_.setRotation(rotation_vec.normalized(), tcp_angle);
   } else {
-    rotation.setValue(0.0, 0.0, 0.0, 1.0);  // default Quaternion is 0,0,0,0 which is invalid
+    tcp_rotation_quat_.setValue(0.0, 0.0, 0.0, 1.0);  // default Quaternion is 0,0,0,0 which is invalid
   }
-  tcp_transform_.transform.translation.x = urcl_tcp_pose_[0];
-  tcp_transform_.transform.translation.y = urcl_tcp_pose_[1];
-  tcp_transform_.transform.translation.z = urcl_tcp_pose_[2];
-
-  tcp_transform_.transform.rotation = tf2::toMsg(rotation);
+  auto rot_mat = tf2::Matrix3x3(tcp_rotation_quat_);
+  rot_mat.getRPY(tcp_orientation_rpy_[0], tcp_orientation_rpy_[1], tcp_orientation_rpy_[2]);
 }
 
 hardware_interface::return_type URPositionHardwareInterface::prepare_command_mode_switch(