Prepare for testing.

Author: livanov93

ur_controllers/include/ur_controllers/force_torque_sensor_controller.h

@@ -33,7 +33,7 @@ class ForceTorqueStateController : public controller_interface::ControllerInterf
 
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
 
-    controller_interface::return_type init(const std::string &controller_name) override;
+  controller_interface::return_type init(const std::string& controller_name) override;
 
 protected:
   bool init_sensor_data();

ur_controllers/src/force_torque_sensor_controller.cpp

@@ -6,23 +6,22 @@
 
 namespace ur_controllers
 {
-
 ForceTorqueStateController::ForceTorqueStateController() : controller_interface::ControllerInterface()
 {
 }
 
-    controller_interface::return_type ForceTorqueStateController::init(const std::string &controller_name) {
-
-      auto ret = ControllerInterface::init(controller_name);
-      if (ret != controller_interface::return_type::SUCCESS) {
-        return ret;
-      }
-
-      return controller_interface::return_type::SUCCESS;
+controller_interface::return_type ForceTorqueStateController::init(const std::string& controller_name)
+{
+  auto ret = ControllerInterface::init(controller_name);
+  if (ret != controller_interface::return_type::SUCCESS)
+  {
+    return ret;
+  }
 
-    }
+  return controller_interface::return_type::SUCCESS;
+}
 
-    controller_interface::InterfaceConfiguration ForceTorqueStateController::command_interface_configuration() const
+controller_interface::InterfaceConfiguration ForceTorqueStateController::command_interface_configuration() const
 {
   controller_interface::InterfaceConfiguration config;
   config.type = controller_interface::interface_configuration_type::NONE;

ur_robot_driver/src/hardware_interface.cpp

@@ -128,6 +128,15 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
         info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &urcl_joint_efforts_[i]));
   }
 
+  for (uint i = 0; i < info_.sensors.size(); i++)
+  {
+    for (uint j = 0; j < info_.sensors[i].state_interfaces.size(); ++j)
+    {
+      state_interfaces.emplace_back(hardware_interface::StateInterface(
+          info_.sensors[i].name, info_.sensors[i].state_interfaces[j].name, &urcl_ft_sensor_measurements_[i]));
+    }
+  }
+
   return state_interfaces;
 }
 

ur_ros2_control_demos/urdf/ur.ros2_control.xacro

@@ -109,6 +109,17 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
+      <sensor name="tcp_fts_sensor">
+        <state_interface name="fx"/>
+        <state_interface name="fy"/>
+        <state_interface name="fz"/>
+        <state_interface name="tx"/>
+        <state_interface name="ty"/>
+        <state_interface name="tz"/>
+        <param name="frame_id">tool0</param>
+        <param name="lower_limits">-100</param>
+        <param name="upper_limits">100</param>
+      </sensor>
     </ros2_control>
   </xacro:macro>