From 295d1231ce325452db343ff1ae9f4b9b2cbb7c95 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 20 Aug 2024 10:50:42 +0200 Subject: [PATCH] Emulate button, cleanup parameters and logging (#25) (#26) * 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 432de6da739f6d12581450efefe31419d0e48694) Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> --- .../src/fd_clutch_broadcaster.cpp | 3 + .../src/fd_inertia_broadcaster.cpp | 6 ++ .../ros2_control/fd.r2c_hardware.xacro | 8 +- .../include/fd_hardware/fd_effort_hi.hpp | 10 +- fd_hardware/src/fd_effort_hi.cpp | 96 ++++++++++++------- 5 files changed, 87 insertions(+), 36 deletions(-) diff --git a/fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp b/fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp index bb850b3..e5e7ac5 100644 --- a/fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp +++ b/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(); } diff --git a/fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp b/fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp index 690aa38..4744a13 100644 --- a/fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp +++ b/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(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::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(); } diff --git a/fd_description/ros2_control/fd.r2c_hardware.xacro b/fd_description/ros2_control/fd.r2c_hardware.xacro index 850ce1b..40e269c 100644 --- a/fd_description/ros2_control/fd.r2c_hardware.xacro +++ b/fd_description/ros2_control/fd.r2c_hardware.xacro @@ -1,6 +1,8 @@ - + @@ -8,7 +10,9 @@ fd_hardware/FDEffortHardwareInterface - -1 + ${interface_id} + ${emulate_button} + fd_inertia diff --git a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp index 5330050..4c8c90c 100644 --- a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp +++ b/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 hw_commands_effort_; std::vector hw_states_position_; diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp index 323f856..7ba3a64 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/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,7 +74,7 @@ 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; @@ -78,7 +82,7 @@ CallbackReturn FDEffortHardwareInterface::on_init( 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; @@ -86,7 +90,7 @@ CallbackReturn FDEffortHardwareInterface::on_init( 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; @@ -94,21 +98,21 @@ CallbackReturn FDEffortHardwareInterface::on_init( 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; } }