From cc387e8444fdbca51077bc635104bf08851984e9 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Mon, 8 Jan 2024 16:07:12 +0100 Subject: [PATCH] Add button as gpio (#15) * Add button as gpio: - gpio for HW - add publisher in ee_pose_brd - !!! only one button supported !!! Note: refractored version of CarlDegio/forcedimension_ros2 (see PR #4). Co-authored-by: CarlDegio <2536890272@qq.com> * use "std_msgs/msg/bool" * fix CmakeLists * clean-up config --------- Co-authored-by: CarlDegio <2536890272@qq.com> --- .../ee_pose_broadcaster/CMakeLists.txt | 3 ++ .../ee_pose_broadcaster.hpp | 15 ++++-- .../ee_pose_broadcaster/package.xml | 3 +- .../src/ee_pose_broadcaster.cpp | 28 ++++++++++- fd_description/config/fd_controllers.yaml | 1 + .../ros2_control/fd.r2c_hardware.xacro | 5 ++ .../include/fd_hardware/fd_effort_hi.hpp | 1 + fd_hardware/src/fd_effort_hi.cpp | 48 +++++++++++++++++++ 8 files changed, 98 insertions(+), 6 deletions(-) diff --git a/fd_controllers/ee_pose_broadcaster/CMakeLists.txt b/fd_controllers/ee_pose_broadcaster/CMakeLists.txt index ffc27dc..54522d7 100644 --- a/fd_controllers/ee_pose_broadcaster/CMakeLists.txt +++ b/fd_controllers/ee_pose_broadcaster/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) @@ -33,6 +34,7 @@ ament_target_dependencies(ee_pose_broadcaster rcutils realtime_tools geometry_msgs + std_msgs eigen3_cmake_module Eigen3 ) @@ -62,6 +64,7 @@ ament_export_dependencies( geometry_msgs eigen3_cmake_module Eigen3 + example_interfaces ) ament_export_include_directories( include diff --git a/fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp b/fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp index b88d550..ef4597b 100644 --- a/fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp +++ b/fd_controllers/ee_pose_broadcaster/include/ee_pose_broadcaster/ee_pose_broadcaster.hpp @@ -29,6 +29,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "realtime_tools/realtime_publisher.h" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "std_msgs/msg/bool.hpp" namespace ee_pose_broadcaster { @@ -65,14 +66,20 @@ class EePoseBroadcaster : public controller_interface::ControllerInterface const rclcpp_lifecycle::State & previous_state) override; protected: - // For the PoseStamped message, - std::shared_ptr> ee_pose_publisher_; - std::shared_ptr> - realtime_ee_pose_publisher_; std::vector joints_; + std::vector buttons_; std::unordered_map> name_if_value_mapping_; Eigen::Matrix4d transform_, pose_; + + // Publishers + std::shared_ptr> ee_pose_publisher_; + std::shared_ptr> + realtime_ee_pose_publisher_; + + std::shared_ptr> fd_button_publisher_; + std::shared_ptr> + realtime_fd_button_publisher_; }; } // namespace ee_pose_broadcaster diff --git a/fd_controllers/ee_pose_broadcaster/package.xml b/fd_controllers/ee_pose_broadcaster/package.xml index 638e710..3fc2c8d 100644 --- a/fd_controllers/ee_pose_broadcaster/package.xml +++ b/fd_controllers/ee_pose_broadcaster/package.xml @@ -20,8 +20,9 @@ controller_interface hardware_interface rclcpp_lifecycle - geometry_msgs realtime_tools + geometry_msgs + std_msgs ament_cmake_gmock controller_manager diff --git a/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp b/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp index 5730014..9b31fab 100644 --- a/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp +++ b/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp @@ -57,6 +57,7 @@ EePoseBroadcaster::on_init() // definition of the parameters that need to be queried from the // controller configuration file with default values auto_declare>("joints", std::vector()); + auto_declare>("buttons", std::vector()); auto_declare>("transform_translation", std::vector()); auto_declare>("transform_rotation", std::vector()); } catch (const std::exception & e) { @@ -89,12 +90,22 @@ const } } + if (buttons_.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "No button name provided!"); + + } else { + for (const auto & button : buttons_) { + state_interfaces_config.names.push_back(button + "/" + HW_IF_POSITION); + } + } + return state_interfaces_config; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + buttons_ = get_node()->get_parameter("buttons").as_string_array(); joints_ = get_node()->get_parameter("joints").as_string_array(); if (joints_.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "Please provide list of joints in config!"); @@ -137,7 +148,6 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state std::cout << transform_ << std::endl; - try { ee_pose_publisher_ = get_node()->create_publisher( "ee_pose", @@ -146,6 +156,13 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state realtime_ee_pose_publisher_ = std::make_shared>( ee_pose_publisher_); + + fd_button_publisher_ = get_node()->create_publisher( + "button_state", rclcpp::SystemDefaultsQoS()); + + realtime_fd_button_publisher_ = + std::make_shared>( + fd_button_publisher_); } catch (const std::exception & e) { // get_node() may throw, logging raw here fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); @@ -256,6 +273,15 @@ controller_interface::return_type EePoseBroadcaster::update( realtime_ee_pose_publisher_->unlockAndPublish(); } + if (!buttons_.empty()) { + if (realtime_fd_button_publisher_ && realtime_fd_button_publisher_->trylock()) { + auto & ee_button_msg = realtime_fd_button_publisher_->msg_; + double button_status = get_value(name_if_value_mapping_, buttons_[0], HW_IF_POSITION); + ee_button_msg.data = button_status > 0.5; + realtime_fd_button_publisher_->unlockAndPublish(); + } + } + return controller_interface::return_type::OK; } diff --git a/fd_description/config/fd_controllers.yaml b/fd_description/config/fd_controllers.yaml index 14ca6c4..42f5ae6 100644 --- a/fd_description/config/fd_controllers.yaml +++ b/fd_description/config/fd_controllers.yaml @@ -48,3 +48,4 @@ fd/fd_ee_broadcaster: # - fd_roll # - fd_pitch # - fd_yaw + buttons: [button] # Note: only one button supported for now diff --git a/fd_description/ros2_control/fd.r2c_hardware.xacro b/fd_description/ros2_control/fd.r2c_hardware.xacro index b628e33..850ce1b 100644 --- a/fd_description/ros2_control/fd.r2c_hardware.xacro +++ b/fd_description/ros2_control/fd.r2c_hardware.xacro @@ -12,6 +12,11 @@ + + + + + diff --git a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp index 4186b93..2edfe2e 100644 --- a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp +++ b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp @@ -67,6 +67,7 @@ class FDEffortHardwareInterface : public hardware_interface::SystemInterface std::vector hw_states_position_; std::vector hw_states_velocity_; std::vector hw_states_effort_; + std::vector hw_button_state_; /** Initiate the USB communication with the device. diff --git a/fd_hardware/src/fd_effort_hi.cpp b/fd_hardware/src/fd_effort_hi.cpp index 8c8b2f7..4e2eeaf 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/fd_hardware/src/fd_effort_hi.cpp @@ -48,6 +48,7 @@ CallbackReturn FDEffortHardwareInterface::on_init( hw_states_velocity_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_states_effort_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_commands_effort_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_button_state_.resize(info_.gpios.size(), std::numeric_limits::quiet_NaN()); for (const hardware_interface::ComponentInfo & joint : info_.joints) { @@ -98,6 +99,38 @@ CallbackReturn FDEffortHardwareInterface::on_init( return CallbackReturn::ERROR; } } + for (const hardware_interface::ComponentInfo & button : info_.gpios) { + if (button.state_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger("FDEffortHardwareInterface"), + "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"), + "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; + } + } + for (const hardware_interface::ComponentInfo & button : info_.gpios) { + if (button.state_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger("FDEffortHardwareInterface"), + "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"), + "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; + } + } return CallbackReturn::SUCCESS; } @@ -121,6 +154,11 @@ FDEffortHardwareInterface::export_state_interfaces() hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i])); } + for (uint i = 0; i < info_.gpios.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.gpios[i].name, hardware_interface::HW_IF_POSITION, &hw_button_state_[i])); + } return state_interfaces; } @@ -221,6 +259,16 @@ hardware_interface::return_type FDEffortHardwareInterface::read( hw_states_effort_[6] = gripper_force; } + // Get button status, TODO multiple buttons from index + int button_status = dhdGetButton(0, interface_ID_); + if (button_status == 1) { + hw_button_state_[0] = 1.0; + } else if (button_status == 0) { + hw_button_state_[0] = 0.0; + } else { + flag += -1; + } + if (flag >= 0) { return hardware_interface::return_type::OK; } else {