From 5bf88e3a112e203491015100e4ca216b115cf314 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 8 Jan 2024 15:36:34 +0100 Subject: [PATCH 1/4] Add button as gpio: - gpio for HW - add publisher in - only one button supported Note: refractored version of CarlDegio/forcedimension_ros2 (see PR #4). Co-authored-by: CarlDegio <2536890272@qq.com> --- .../ee_pose_broadcaster/CMakeLists.txt | 3 ++ .../ee_pose_broadcaster.hpp | 15 ++++-- .../src/ee_pose_broadcaster.cpp | 28 ++++++++++- fd_description/config/fd_controllers.yaml | 4 ++ .../ros2_control/fd.r2c_hardware.xacro | 5 ++ .../include/fd_hardware/fd_effort_hi.hpp | 1 + fd_hardware/src/fd_effort_hi.cpp | 48 +++++++++++++++++++ 7 files changed, 99 insertions(+), 5 deletions(-) diff --git a/fd_controllers/ee_pose_broadcaster/CMakeLists.txt b/fd_controllers/ee_pose_broadcaster/CMakeLists.txt index ffc27dc..5f5ce52 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(example_interfaces REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) @@ -35,6 +36,7 @@ ament_target_dependencies(ee_pose_broadcaster geometry_msgs eigen3_cmake_module Eigen3 + example_interfaces ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -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..f5feb64 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 "example_interfaces/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/src/ee_pose_broadcaster.cpp b/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp index 5730014..0d8638b 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..dd8934b 100644 --- a/fd_description/config/fd_controllers.yaml +++ b/fd_description/config/fd_controllers.yaml @@ -30,6 +30,9 @@ fd/fd_controller: ## Note: uncomment the following if a clutch is present (e.g., omega 6/7 device) # - fd_clutch + gpios: + - button + fd/fd_ee_broadcaster: ros__parameters: transform_translation: @@ -48,3 +51,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 { From b8fce473b994fb021d722da597ec35c1f579fc99 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 8 Jan 2024 15:57:54 +0100 Subject: [PATCH 2/4] use "std_msgs/msg/bool" --- fd_controllers/ee_pose_broadcaster/CMakeLists.txt | 2 ++ .../include/ee_pose_broadcaster/ee_pose_broadcaster.hpp | 6 +++--- fd_controllers/ee_pose_broadcaster/package.xml | 3 ++- .../ee_pose_broadcaster/src/ee_pose_broadcaster.cpp | 4 ++-- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/fd_controllers/ee_pose_broadcaster/CMakeLists.txt b/fd_controllers/ee_pose_broadcaster/CMakeLists.txt index 5f5ce52..a225fc4 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(example_interfaces REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) @@ -34,6 +35,7 @@ ament_target_dependencies(ee_pose_broadcaster rcutils realtime_tools geometry_msgs + std_msgs eigen3_cmake_module Eigen3 example_interfaces 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 f5feb64..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,7 +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 "example_interfaces/msg/bool.hpp" +#include "std_msgs/msg/bool.hpp" namespace ee_pose_broadcaster { @@ -77,8 +77,8 @@ class EePoseBroadcaster : public controller_interface::ControllerInterface std::shared_ptr> realtime_ee_pose_publisher_; - std::shared_ptr> fd_button_publisher_; - std::shared_ptr> + std::shared_ptr> fd_button_publisher_; + std::shared_ptr> realtime_fd_button_publisher_; }; 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 0d8638b..9b31fab 100644 --- a/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp +++ b/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp @@ -157,11 +157,11 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state std::make_shared>( ee_pose_publisher_); - fd_button_publisher_ = get_node()->create_publisher( + fd_button_publisher_ = get_node()->create_publisher( "button_state", rclcpp::SystemDefaultsQoS()); realtime_fd_button_publisher_ = - std::make_shared>( + std::make_shared>( fd_button_publisher_); } catch (const std::exception & e) { // get_node() may throw, logging raw here From 61b2f9385df2fb258cba0871c9fc26c3c2c625a3 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 8 Jan 2024 16:01:04 +0100 Subject: [PATCH 3/4] fix CmakeLists --- fd_controllers/ee_pose_broadcaster/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/fd_controllers/ee_pose_broadcaster/CMakeLists.txt b/fd_controllers/ee_pose_broadcaster/CMakeLists.txt index a225fc4..54522d7 100644 --- a/fd_controllers/ee_pose_broadcaster/CMakeLists.txt +++ b/fd_controllers/ee_pose_broadcaster/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(rcutils REQUIRED) find_package(realtime_tools REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) -find_package(example_interfaces REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3) @@ -38,7 +37,6 @@ ament_target_dependencies(ee_pose_broadcaster std_msgs eigen3_cmake_module Eigen3 - example_interfaces ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. From 60ebb36984c3d8a31856ee34c183e1b5c98cbdd3 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 8 Jan 2024 16:01:12 +0100 Subject: [PATCH 4/4] clean-up config --- fd_description/config/fd_controllers.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/fd_description/config/fd_controllers.yaml b/fd_description/config/fd_controllers.yaml index dd8934b..42f5ae6 100644 --- a/fd_description/config/fd_controllers.yaml +++ b/fd_description/config/fd_controllers.yaml @@ -30,9 +30,6 @@ fd/fd_controller: ## Note: uncomment the following if a clutch is present (e.g., omega 6/7 device) # - fd_clutch - gpios: - - button - fd/fd_ee_broadcaster: ros__parameters: transform_translation: