diff --git a/fd_bringup/launch/fd.launch.py b/fd_bringup/launch/fd.launch.py index 1a14bef..29ec889 100644 --- a/fd_bringup/launch/fd.launch.py +++ b/fd_bringup/launch/fd.launch.py @@ -90,7 +90,7 @@ def generate_launch_description(): # Load controllers load_controllers = [] - for controller in ['fd_controller', 'joint_state_broadcaster']: + for controller in ['fd_controller', 'joint_state_broadcaster', 'fd_ee_broadcaster']: load_controllers += [ Node( package='controller_manager', diff --git a/fd_bringup/package.xml b/fd_bringup/package.xml index 8ba3bf0..8577134 100644 --- a/fd_bringup/package.xml +++ b/fd_bringup/package.xml @@ -20,6 +20,7 @@ xacro controller_manager joint_state_publisher + effort_controllers fd_description fd_hardware urdf 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 95537c5..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,13 +66,20 @@ class EePoseBroadcaster : public controller_interface::ControllerInterface const rclcpp_lifecycle::State & previous_state) override; protected: - // For the PoseStamped message, + 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::unordered_map> name_if_value_mapping_; - Eigen::Matrix4d transform_, pose_; + 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 3eb36bd..9b31fab 100644 --- a/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp +++ b/fd_controllers/ee_pose_broadcaster/src/ee_pose_broadcaster.cpp @@ -56,6 +56,8 @@ EePoseBroadcaster::on_init() try { // 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) { @@ -76,13 +78,40 @@ EePoseBroadcaster::command_interface_configuration() const controller_interface::InterfaceConfiguration EePoseBroadcaster::state_interface_configuration() const { - return controller_interface::InterfaceConfiguration{ - controller_interface::interface_configuration_type::ALL}; + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + if (joints_.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "No joint name provided!"); + + } else { + for (const auto & joint : joints_) { + state_interfaces_config.names.push_back(joint + "/" + HW_IF_POSITION); + } + } + + 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!"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + auto transform_trans_param = get_node()->get_parameter("transform_translation").as_double_array(); auto transform_rot_param = get_node()->get_parameter("transform_rotation").as_double_array(); Eigen::Quaternion q; @@ -119,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", @@ -128,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()); @@ -139,12 +174,20 @@ EePoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn EePoseBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + if (state_interfaces_.size() != (joints_.size())) { + RCLCPP_WARN( + get_node()->get_logger(), + "Not all requested interfaces exists."); + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn EePoseBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + joints_.clear(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -166,18 +209,50 @@ controller_interface::return_type EePoseBroadcaster::update( const rclcpp::Duration & /*period*/) { for (const auto & state_interface : state_interfaces_) { - name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] = + name_if_value_mapping_[ + state_interface.get_prefix_name()][state_interface.get_interface_name()] = state_interface.get_value(); RCLCPP_DEBUG( - get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_name().c_str(), + get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_prefix_name().c_str(), state_interface.get_interface_name().c_str(), state_interface.get_value()); } if (realtime_ee_pose_publisher_ && realtime_ee_pose_publisher_->trylock()) { pose_ = Eigen::Matrix4d::Identity(); - pose_(0, 3) = get_value(name_if_value_mapping_, "fd_x", HW_IF_POSITION); - pose_(1, 3) = get_value(name_if_value_mapping_, "fd_y", HW_IF_POSITION); - pose_(2, 3) = get_value(name_if_value_mapping_, "fd_z", HW_IF_POSITION); + + if (joints_.size() >= 3) { + double p_x = get_value(name_if_value_mapping_, joints_[0], HW_IF_POSITION); + double p_y = get_value(name_if_value_mapping_, joints_[1], HW_IF_POSITION); + double p_z = get_value(name_if_value_mapping_, joints_[2], HW_IF_POSITION); + + if (std::isnan(p_x) || std::isnan(p_y) || std::isnan(p_z)) { + RCLCPP_DEBUG( + get_node()->get_logger(), "Failed to retrieve fd pose! (fd_x, fd_y, fd_z)!"); + return controller_interface::return_type::ERROR; + } + pose_(0, 3) = p_x; + pose_(1, 3) = p_y; + pose_(2, 3) = p_z; + } + + if (joints_.size() >= 6) { + double roll = get_value(name_if_value_mapping_, joints_[3], HW_IF_POSITION); + double pitch = get_value(name_if_value_mapping_, joints_[4], HW_IF_POSITION); + double yaw = get_value(name_if_value_mapping_, joints_[5], HW_IF_POSITION); + + if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) { + RCLCPP_DEBUG( + get_node()->get_logger(), "Failed to retrieve fd pose! (fd_x, fd_y, fd_z)!"); + return controller_interface::return_type::ERROR; + } + + Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); + + Eigen::Quaterniond q = rollAngle * pitchAngle * yawAngle; + pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix(); + } pose_ = transform_ * pose_; Eigen::Quaternion q(pose_.block<3, 3>(0, 0)); @@ -198,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 e50facd..42f5ae6 100644 --- a/fd_description/config/fd_controllers.yaml +++ b/fd_description/config/fd_controllers.yaml @@ -5,6 +5,9 @@ fd/controller_manager: fd_controller: type: effort_controllers/JointGroupEffortController # ForwardCommandController + fd_ee_broadcaster: + type: ee_pose_broadcaster/EePoseBroadcaster + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -20,8 +23,29 @@ fd/fd_controller: - fd_x - fd_y - fd_z - ## Note: uncomment the following if using an omega 6/7 device - # - fd_roll - # - fd_pitch - # - fd_yaw - # - fd_clutch + ## Note: uncomment the following if using an omega 7 device (e.g., 7 controlled DoF) + # - fd_roll + # - fd_pitch + # - fd_yaw + ## Note: uncomment the following if a clutch is present (e.g., omega 6/7 device) + # - fd_clutch + +fd/fd_ee_broadcaster: + ros__parameters: + transform_translation: + - 0.0 + - 0.0 + - 0.0 + transform_rotation: + - 0.0 + - 0.0 + - 0.0 + joints: + - fd_x + - fd_y + - fd_z + ## Note: uncomment the following if orientation is enabled (e.g., omega 6/7 device) + # - 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 6fd65cc..21280e0 100644 --- a/fd_hardware/include/fd_hardware/fd_effort_hi.hpp +++ b/fd_hardware/include/fd_hardware/fd_effort_hi.hpp @@ -69,6 +69,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 faf0ea5..2c61211 100644 --- a/fd_hardware/src/fd_effort_hi.cpp +++ b/fd_hardware/src/fd_effort_hi.cpp @@ -56,6 +56,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) { @@ -106,6 +107,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; } @@ -129,6 +162,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; } @@ -229,6 +267,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 {