Emulate button, cleanup parameters and logging (#25)

* add debug logs

* add parameters for emulate_button and interface_id

* make fd_inertia a parameter also

* emulate button

* update description file

* linting

* cleanup logging

(cherry picked from commit 432de6d)

fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp

@@ -130,7 +130,9 @@ controller_interface::return_type FdClutchBroadcaster::update(
   const rclcpp::Time & /*time*/,
   const rclcpp::Duration & /*period*/)
 {
+  RCLCPP_DEBUG(get_node()->get_logger(), "Entering update()");
   if (realtime_clutch_publisher_ && realtime_clutch_publisher_->trylock()) {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Lock acquired");
     // Read provided state interface
     bool is_interface_a_button = get_node()->get_parameter("is_interface_a_button").as_bool();
     double read_value = state_interfaces_[0].get_value();
@@ -148,6 +150,7 @@ controller_interface::return_type FdClutchBroadcaster::update(
     // Publish clucth
     auto & fd_clutch_msg = realtime_clutch_publisher_->msg_;
     fd_clutch_msg.data = clutch_state;
+    RCLCPP_DEBUG(get_node()->get_logger(), "publish and unlock");
     realtime_clutch_publisher_->unlockAndPublish();
   }
 

fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp

@@ -141,6 +141,7 @@ FdInertiaBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_st
   Eigen::Vector3d trans = Eigen::Vector3d::Zero();
 
   if (transform_trans_param.size() == 0) {
+    RCLCPP_INFO(get_node()->get_logger(), "No (linear) transformation provided. Using t = 0,0,0");
     trans << 0.0, 0.0, 0.0;
   } else if (transform_trans_param.size() == 3) {
     trans << transform_trans_param[0], transform_trans_param[1], transform_trans_param[2];
@@ -151,6 +152,8 @@ FdInertiaBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_st
 
   if (transform_rot_param.size() == 0) {
     q = Eigen::Quaternion<double>(1, 0, 0, 0);
+    RCLCPP_INFO(get_node()->get_logger(),
+        "No (angular) transformation provided. Using q = 1,0,0,0");
   } else if (transform_rot_param.size() == 3) {
     Eigen::AngleAxisd rollAngle(transform_rot_param[0], Eigen::Vector3d::UnitZ());
     Eigen::AngleAxisd yawAngle(transform_rot_param[1], Eigen::Vector3d::UnitY());
@@ -212,7 +215,9 @@ controller_interface::return_type FdInertiaBroadcaster::update(
   const rclcpp::Time & /*time*/,
   const rclcpp::Duration & /*period*/)
 {
+  RCLCPP_DEBUG(get_node()->get_logger(), "Entering update()");
   if (realtime_inertia_publisher_ && realtime_inertia_publisher_->trylock()) {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Lock acquired");
     inertia_in_base_ = Eigen::Matrix<double, 6, 6>::Zero();
 
     // Map upper triangular part of inertia to inertia state interface
@@ -234,6 +239,7 @@ controller_interface::return_type FdInertiaBroadcaster::update(
     // Publish inertia
     auto & fd_inertia_msg = realtime_inertia_publisher_->msg_;
     matrixEigenToMsg(inertia_, fd_inertia_msg);
+    RCLCPP_DEBUG(get_node()->get_logger(), "publish and unlock");
     realtime_inertia_publisher_->unlockAndPublish();
   }
 

fd_description/ros2_control/fd.r2c_hardware.xacro

@@ -1,14 +1,18 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-    <xacro:macro name="fd_ros2_control" params="robot_id plugin_name:='FDHapticInterface' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false'">
+    <xacro:macro
+        name="fd_ros2_control"
+        params="robot_id plugin_name:='FDHapticInterface' interface_id:='-1' use_fake_hardware:='false' use_orientation:='false' orientation_is_actuated:='false' use_clutch:='false' emulate_button:='false'">
         <ros2_control name="${plugin_name}" type="system">
             <hardware>
                 <xacro:if value="${use_fake_hardware}">
                     <plugin>fake_components/GenericSystem</plugin>
                 </xacro:if>
                 <xacro:unless value="${use_fake_hardware}">
                     <plugin>fd_hardware/FDEffortHardwareInterface</plugin>
-                    <param name="interface_ID">-1</param>
+                    <param name="interface_id">${interface_id}</param>
+                    <param name="emulate_button">${emulate_button}</param>
+                    <param name="inertia_interface_name">fd_inertia</param>
                 </xacro:unless>
             </hardware>
 

fd_hardware/include/fd_hardware/fd_effort_hi.hpp

@@ -59,11 +59,17 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface
   hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
 
 private:
-  // ID of the interface (Rq: "-1" = invalid/any that is connected)
+  /// ID of the interface (Rq: "-1" = invalid/any that is connected)
   char interface_ID_ = -1;
-  // Turned to true after the connection
+
+  /// Turned to true after the connection
   bool isConnected_ = false;
 
+  /// If true, the button will be emulated from clutch joint (for omega 6 / sigma 7, see SDK doc)
+  bool emulate_button_ = false;
+
+  std::string inertia_interface_name_;
+
   // Store the command for the robot
   std::vector<double> hw_commands_effort_;
   std::vector<double> hw_states_position_;

fd_hardware/src/fd_effort_hi.cpp

@@ -28,6 +28,8 @@
 #include "fd_sdk_vendor/dhd.hpp"
 #include "fd_sdk_vendor/drd.hpp"
 
+#include "hardware_interface/component_parser.hpp"
+#include "hardware_interface/lexical_casts.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/rclcpp.hpp"
 
@@ -36,6 +38,8 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
 namespace fd_hardware
 {
 
+rclcpp::Logger LOGGER = rclcpp::get_logger("FDEffortHardwareInterface");
+
 unsigned int flattened_index_from_triangular_index(unsigned int i, unsigned int j)
 {
   return i * (i - 1) / 2 + j;
@@ -70,45 +74,45 @@ CallbackReturn FDEffortHardwareInterface::on_init(
     // PHI has currently exactly 3 states and 1 command interface on each joint
     if (joint.command_interfaces.size() != 1) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Joint '%s' has %lu command interfaces found. 1 expected.", joint.name.c_str(),
         joint.command_interfaces.size());
       return CallbackReturn::ERROR;
     }
 
     if (joint.command_interfaces[0].name != hardware_interface::HW_IF_EFFORT) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
         joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT);
       return CallbackReturn::ERROR;
     }
 
     if (joint.state_interfaces.size() != 3) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Joint '%s' has %ld state interface. 3 expected.", joint.name.c_str(),
         joint.state_interfaces.size());
       return CallbackReturn::ERROR;
     }
 
     if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
         joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
       return CallbackReturn::ERROR;
     }
     if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
         joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
       return CallbackReturn::ERROR;
     }
     if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(),
         joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT);
       return CallbackReturn::ERROR;
@@ -117,14 +121,14 @@ CallbackReturn FDEffortHardwareInterface::on_init(
   for (const hardware_interface::ComponentInfo & button : info_.gpios) {
     if (button.state_interfaces.size() != 1) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Button '%s' has %lu state interface. 1 expected.", button.name.c_str(),
         button.state_interfaces.size());
       return CallbackReturn::ERROR;
     }
     if (button.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Button '%s' have %s state interface. '%s' expected.", button.name.c_str(),
         button.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
       return CallbackReturn::ERROR;
@@ -133,20 +137,44 @@ CallbackReturn FDEffortHardwareInterface::on_init(
   for (const hardware_interface::ComponentInfo & button : info_.gpios) {
     if (button.state_interfaces.size() != 1) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Button '%s' has %lu state interface. 1 expected.", button.name.c_str(),
         button.state_interfaces.size());
       return CallbackReturn::ERROR;
     }
     if (button.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
       RCLCPP_FATAL(
-        rclcpp::get_logger("FDEffortHardwareInterface"),
+        LOGGER,
         "Button '%s' have %s state interface. '%s' expected.", button.name.c_str(),
         button.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
       return CallbackReturn::ERROR;
     }
   }
 
+
+  // Get parameters
+  auto it_interface_id = info_.hardware_parameters.find("interface_id");
+  if (it_interface_id != info_.hardware_parameters.end()) {
+    interface_ID_ = stoi(it_interface_id->second);
+    RCLCPP_INFO(LOGGER, "Using interface ID: %d", interface_ID_);
+  } else {
+    interface_ID_ = -1;
+  }
+
+  auto it_emulate_button = info_.hardware_parameters.find("emulate_button");
+  if (it_emulate_button != info_.hardware_parameters.end()) {
+    emulate_button_ = hardware_interface::parse_bool(it_emulate_button->second);
+  } else {
+    emulate_button_ = false;
+  }
+
+  auto it_fd_inertia = info_.hardware_parameters.find("inertia_interface_name");
+  if (it_fd_inertia != info_.hardware_parameters.end()) {
+    inertia_interface_name_ = it_fd_inertia->second;
+  } else {
+    inertia_interface_name_ = "fd_inertia";
+  }
+
   return CallbackReturn::SUCCESS;
 }
 // ------------------------------------------------------------------------------------------
@@ -175,15 +203,12 @@ FDEffortHardwareInterface::export_state_interfaces()
         info_.gpios[i].name, hardware_interface::HW_IF_POSITION, &hw_button_state_[i]));
   }
 
-  // TODO(tpoignonec) Make a parameter or somehow retrieve robot prefix
-  std::string inertia_interface_name = "fd_inertia";
-
   // Map upper triangular part of inertia to inertia state interface
   for (uint row = 0; row < 6; row++) {
     for (uint col = row; col < 6; col++) {
       state_interfaces.emplace_back(
         hardware_interface::StateInterface(
-          inertia_interface_name,
+          inertia_interface_name_,
           std::to_string(row) + "" + std::to_string(col),
           &hw_states_inertia_[flattened_index_from_triangular_index(row, col)]));
     }
@@ -210,15 +235,13 @@ CallbackReturn FDEffortHardwareInterface::on_activate(
 {
   (void)previous_state;  // hush "-Wunused-parameter" warning
 
-  RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "Starting ...please wait...");
+  RCLCPP_INFO(LOGGER, "Starting ...please wait...");
 
   if (connectToDevice()) {
-    RCLCPP_INFO(
-      rclcpp::get_logger("FDEffortHardwareInterface"), "System Successfully started!");
+    RCLCPP_INFO(LOGGER, "System Successfully started!");
     return CallbackReturn::SUCCESS;
   } else {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("FDEffortHardwareInterface"), "System Not started!");
+    RCLCPP_ERROR(LOGGER, "System Not started!");
     return CallbackReturn::ERROR;
   }
 }
@@ -227,15 +250,13 @@ CallbackReturn FDEffortHardwareInterface::on_deactivate(
   const rclcpp_lifecycle::State & previous_state)
 {
   (void)previous_state;  // hush "-Wunused-parameter" warning
-  RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "Stopping ...please wait...");
+  RCLCPP_INFO(LOGGER, "Stopping ...please wait...");
 
   if (disconnectFromDevice()) {
-    RCLCPP_INFO(
-      rclcpp::get_logger("FDEffortHardwareInterface"), "System successfully stopped!");
+    RCLCPP_INFO(LOGGER, "System successfully stopped!");
     return CallbackReturn::SUCCESS;
   } else {
-    RCLCPP_ERROR(
-      rclcpp::get_logger("FDEffortHardwareInterface"), "System Not stopped!");
+    RCLCPP_ERROR(LOGGER, "System Not stopped!");
     return CallbackReturn::ERROR;
   }
 }
@@ -318,7 +339,7 @@ hardware_interface::return_type FDEffortHardwareInterface::read(
   if (flag >= 0) {
     return hardware_interface::return_type::OK;
   } else {
-    RCLCPP_ERROR(rclcpp::get_logger("FDEffortHardwareInterface"), "Updating from system failed!");
+    RCLCPP_ERROR(LOGGER, "Updating from system failed!");
     return hardware_interface::return_type::ERROR;
   }
 }
@@ -372,9 +393,9 @@ bool FDEffortHardwareInterface::connectToDevice()
 
     // Check if the device has 3 dof or more
     if (dhdHasWrist(interface_ID_)) {
-      RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Rotation enabled ");
+      RCLCPP_INFO(LOGGER, "dhd : Rotation enabled ");
     } else {
-      RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Rotation disabled ");
+      RCLCPP_INFO(LOGGER, "dhd : Rotation disabled ");
     }
 
     // Retrieve the mass of the device
@@ -411,9 +432,20 @@ bool FDEffortHardwareInterface::connectToDevice()
         rclcpp::get_logger(
           "FDEffortHardwareInterface"), "dhd : Gravity compensation enabled");
     }
-    RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Device connected !");
-
+    RCLCPP_INFO(LOGGER, "dhd : Device connected !");
 
+    if (emulate_button_ && dhdHasGripper(interface_ID_)) {
+      RCLCPP_INFO(
+        rclcpp::get_logger(
+          "FDEffortHardwareInterface"), "dhd : Emulating button from clutch joint !");
+      if (dhdEmulateButton(DHD_ON, interface_ID_) < DHD_NO_ERROR) {
+        RCLCPP_WARN(
+        rclcpp::get_logger(
+          "FDEffortHardwareInterface"), "dhd : Could not enable button emulation!");
+        disconnectFromDevice();
+      }
+    }
+    // Set force to zero
     if (dhdSetForceAndTorqueAndGripperForce(
         0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
         interface_ID_) < DHD_NO_ERROR)
@@ -437,7 +469,7 @@ bool FDEffortHardwareInterface::connectToDevice()
 // ------------------------------------------------------------------------------------------
 bool FDEffortHardwareInterface::disconnectFromDevice()
 {
-  RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : stopping the device.");
+  RCLCPP_INFO(LOGGER, "dhd : stopping the device.");
   // Stop the device: disables the force on the haptic device and puts it into BRAKE mode.
   int hasStopped = -1;
   while (hasStopped < 0) {
@@ -449,11 +481,11 @@ bool FDEffortHardwareInterface::disconnectFromDevice()
   // close device connection
   int connectionIsClosed = dhdClose(interface_ID_);
   if (connectionIsClosed >= 0) {
-    RCLCPP_INFO(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd :  Disconnected ! ");
+    RCLCPP_INFO(LOGGER, "dhd :  Disconnected ! ");
     interface_ID_ = false;
     return true;
   } else {
-    RCLCPP_ERROR(rclcpp::get_logger("FDEffortHardwareInterface"), "dhd : Failed to disconnect !");
+    RCLCPP_ERROR(LOGGER, "dhd : Failed to disconnect !");
     return false;
   }
 }