feat(robotiq_driver): expose object detection and effort as state interfaces

grippers/robotiq_description/urdf/2f_140.ros2_control.xacro

@@ -57,6 +57,13 @@
                     <param name="initial_value">${gripper_closed_position}</param>
                 </state_interface>
                 <state_interface name="velocity"/>
+                <!-- Fed from the status block the driver already exchanges:
+                     effort from gCU (a motor-current proxy, on the same full
+                     scale as the force command), object_status from gOBJ
+                     (0 moving, 1 held while opening, 2 held while closing,
+                     3 at the requested position). -->
+                <state_interface name="effort"/>
+                <state_interface name="object_status"/>
             </joint>
             <!-- When simulating we need to include the rest of the gripper joints -->
             <xacro:if value="${sim_isaac or sim_gazebo}">

grippers/robotiq_description/urdf/2f_85.ros2_control.xacro

@@ -69,6 +69,13 @@
                     <param name="initial_value">${gripper_closed_position}</param>
                 </state_interface>
                 <state_interface name="velocity"/>
+                <!-- Fed from the status block the driver already exchanges:
+                     effort from gCU (a motor-current proxy, on the same full
+                     scale as the force command), object_status from gOBJ
+                     (0 moving, 1 held while opening, 2 held while closing,
+                     3 at the requested position). -->
+                <state_interface name="effort"/>
+                <state_interface name="object_status"/>
             </joint>
             <!-- When simulating we need to include the rest of the gripper joints -->
             <xacro:if value="${sim_isaac or sim_gazebo}">

grippers/robotiq_driver/include/robotiq_driver/gripper_scaling.hpp

@@ -92,4 +92,12 @@ inline constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos;
    }
    return static_cast<uint8_t>(std::min(value / maximum, 1.0) * 0xFF);
 }
+
+//! The inverse: a 0x00..0xFF register read back as the fraction of \p maximum
+//! it stands for. Total where registerFromFractionOf is not — every byte names
+//! a fraction, so there is no reading the arithmetic cannot map.
+[[nodiscard]] inline double fractionOfFromRegister(uint8_t register_value, double maximum)
+{
+   return maximum * register_value / 0xFF;
+}
 } // namespace robotiq_driver

grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp

@@ -58,6 +58,11 @@
 #include <rclcpp/rclcpp.hpp>
 
 namespace robotiq_driver {
+//! Name of the state interface carrying gOBJ. ros2_control defines HW_IF_
+//! constants for position, velocity and effort but has nothing for object
+//! detection, so the URDF and every consumer spell this one out.
+inline constexpr const char* kObjectStatusInterface = "object_status";
+
 //! ros2_control hardware interface for a Robotiq 2F gripper, driven through the
 //! gripper SDK.
 //! Member order carries an invariant: recovery_ is declared after gripper_ so
@@ -114,7 +119,8 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa
    CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override;
 
    /**
-    * This method exposes position and velocity of joints for reading.
+    * This method exposes position, velocity, effort and object status of joints
+    * for reading.
     */
    ROBOTIQ_DRIVER_PUBLIC
    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
@@ -179,6 +185,17 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa
 
    double gripper_position_ = 0.0;
    double gripper_velocity_ = 0.0;
+
+   // gCU read back through the same full-scale force the rFR command uses, so
+   // the effort reported and the effort requested are on one scale. It is a
+   // motor-current proxy either way, not a measurement of the grip.
+   double gripper_effort_ = 0.0;
+
+   // gOBJ as it comes off the wire: 0 moving, 1 held while opening, 2 held
+   // while closing, 3 at the requested position. A double because that is what
+   // a state interface carries; consumers compare it against those four values.
+   double gripper_object_status_ = 0.0;
+
    double gripper_position_command_ = 0.0;
 
    // The last command write() sent. A register the arithmetic cannot produce

grippers/robotiq_driver/src/hardware_interface.cpp

@@ -161,27 +161,24 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
       return CallbackReturn::ERROR;
    }
 
-   // There are two state interfaces: position and velocity.
-   if(joint.state_interfaces.size() != 2)
+   // export_state_interfaces() exports the four below whatever the description
+   // says, so this checks that the description names no fifth one the hardware
+   // cannot answer for. A description is free to declare fewer.
+   for(const hardware_interface::InterfaceInfo& state_interface : joint.state_interfaces)
    {
-      RCLCPP_FATAL(kLogger,
-                   "Joint '%s' has %zu state interface. 2 expected.",
-                   joint.name.c_str(),
-                   joint.state_interfaces.size());
-      return CallbackReturn::ERROR;
-   }
-
-   for(int i = 0; i < 2; ++i)
-   {
-      if(!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION
-           || joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
+      if(!(state_interface.name == hardware_interface::HW_IF_POSITION
+           || state_interface.name == hardware_interface::HW_IF_VELOCITY
+           || state_interface.name == hardware_interface::HW_IF_EFFORT
+           || state_interface.name == kObjectStatusInterface))
       {
          RCLCPP_FATAL(kLogger,
-                      "Joint '%s' has %s state interface. Expected %s or %s.",
+                      "Joint '%s' has %s state interface. Expected %s, %s, %s or %s.",
                       joint.name.c_str(),
-                      joint.state_interfaces.at(i).name.c_str(),
+                      state_interface.name.c_str(),
                       hardware_interface::HW_IF_POSITION,
-                      hardware_interface::HW_IF_VELOCITY);
+                      hardware_interface::HW_IF_VELOCITY,
+                      hardware_interface::HW_IF_EFFORT,
+                      kObjectStatusInterface);
          return CallbackReturn::ERROR;
       }
    }
@@ -258,6 +255,10 @@ std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface:
       hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_));
    state_interfaces.emplace_back(
       hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_));
+   state_interfaces.emplace_back(
+      hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_EFFORT, &gripper_effort_));
+   state_interfaces.emplace_back(
+      hardware_interface::StateInterface(info_.joints[0].name, kObjectStatusInterface, &gripper_object_status_));
 
    return state_interfaces;
 }
@@ -418,6 +419,8 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc
    // The status block carries no velocity — the gripper reports position and
    // motor current only.
    gripper_velocity_ = 0.0;
+   gripper_effort_ = fractionOfFromRegister(status.current, parameters_.max_force);
+   gripper_object_status_ = static_cast<uint8_t>(status.gripperStatus.objectDetection());
 
    // A faulted link recovers by itself on the next successful exchange, so
    // this warns rather than errors; the position above is the last good

grippers/robotiq_driver/tests/test_gripper_scaling.cpp

@@ -133,4 +133,27 @@ TEST(GripperScaling, whenTheMaximumIsNearZero_theRegisterIsTheMaxAllowedValue)
 {
    EXPECT_EQ(255, registerFromFractionOf(0.15, std::numeric_limits<double>::denorm_min()));
 }
+
+TEST(GripperScaling, ReadingAFractionBackSpansTheWholeScale)
+{
+   constexpr double kMaxForce = 235.0;
+   EXPECT_DOUBLE_EQ(0.0, fractionOfFromRegister(0, kMaxForce));
+   EXPECT_DOUBLE_EQ(kMaxForce, fractionOfFromRegister(255, kMaxForce));
+   EXPECT_DOUBLE_EQ(kMaxForce * 127 / 255, fractionOfFromRegister(127, kMaxForce));
+}
+
+TEST(GripperScaling, ReadingBackTheRegisterACommandProducedRecoversTheCommand)
+{
+   constexpr double kMaxForce = 235.0;
+   constexpr double kRequested = 100.0;
+
+   const auto register_value = registerFromFractionOf(kRequested, kMaxForce);
+   ASSERT_TRUE(register_value.has_value());
+
+   // registerFromFractionOf truncates, so the value read back sits at most one
+   // count low — the same tolerance the position round trip carries.
+   const double recovered = fractionOfFromRegister(*register_value, kMaxForce);
+   EXPECT_LE(recovered, kRequested);
+   EXPECT_GT(recovered, kRequested - kMaxForce / 0xFF);
+}
 } // namespace robotiq_driver::test

grippers/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp

@@ -92,6 +92,8 @@ std::string minimalRobotUrdf(const std::string& extra_hardware_params = "")
                 <param name="initial_value">0.7929</param>
               </state_interface>
               <state_interface name="velocity"/>
+              <state_interface name="effort"/>
+              <state_interface name="object_status"/>
             </joint>
             <gpio name="reactivate_gripper">
               <command_interface name="reactivate_gripper_cmd" />
@@ -151,6 +153,31 @@ TEST(TestRobotiqGripperHardwareInterface, ExportsExpectedCommandInterfaces)
                                       "reactivate_gripper/reactivate_gripper_response"}));
 }
 
+/**
+ * A grasping client reads object_status to tell a caught part from a missed one
+ * and effort to judge the grip, both by name. object_status in particular is not
+ * a ros2_control standard interface, so nothing but this test pins its spelling.
+ */
+TEST(TestRobotiqGripperHardwareInterface, ExportsExpectedStateInterfaces)
+{
+   const std::string urdf = minimalRobotUrdf();
+
+   rclcpp::Node node{"test_robotiq_gripper_hardware_interface"};
+
+#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0)
+   hardware_interface::ResourceManager rm(urdf, node.get_node_clock_interface(), node.get_node_logging_interface());
+#else
+   hardware_interface::ResourceManager rm(urdf);
+#endif
+
+   const auto keys = rm.state_interface_keys();
+   EXPECT_THAT(keys,
+               testing::IsSupersetOf({"robotiq_85_left_knuckle_joint/position",
+                                      "robotiq_85_left_knuckle_joint/velocity",
+                                      "robotiq_85_left_knuckle_joint/effort",
+                                      "robotiq_85_left_knuckle_joint/object_status"}));
+}
+
 /**
  * use_dummy brings the component all the way up with no gripper and no serial
  * port, and the joint position then follows the commanded position. It is the