From fe3511e9e5d85adbc35da9f479ff7cde9ee19983 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Date: Tue, 23 Jul 2024 18:53:03 +0200 Subject: [PATCH] Add fd inertia broadcaster (#21) * add fd_inertia_broadcaster * update example --- fd_bringup/launch/fd.launch.py | 7 +- .../fd_inertia_broadcaster/CMakeLists.txt | 73 ++++++ .../fd_inertia_broadcaster_plugin.xml | 7 + .../fd_inertia_broadcaster.hpp | 83 ++++++ .../visibility_control.h | 56 ++++ .../fd_inertia_broadcaster/package.xml | 38 +++ .../src/fd_inertia_broadcaster.cpp | 248 ++++++++++++++++++ fd_description/config/fd_controllers.yaml | 11 + 8 files changed, 522 insertions(+), 1 deletion(-) create mode 100644 fd_controllers/fd_inertia_broadcaster/CMakeLists.txt create mode 100644 fd_controllers/fd_inertia_broadcaster/fd_inertia_broadcaster_plugin.xml create mode 100644 fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/fd_inertia_broadcaster.hpp create mode 100644 fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/visibility_control.h create mode 100644 fd_controllers/fd_inertia_broadcaster/package.xml create mode 100644 fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp diff --git a/fd_bringup/launch/fd.launch.py b/fd_bringup/launch/fd.launch.py index 29ec889..146a850 100644 --- a/fd_bringup/launch/fd.launch.py +++ b/fd_bringup/launch/fd.launch.py @@ -90,7 +90,12 @@ def generate_launch_description(): # Load controllers load_controllers = [] - for controller in ['fd_controller', 'joint_state_broadcaster', 'fd_ee_broadcaster']: + for controller in [ + 'fd_controller', + 'joint_state_broadcaster', + 'fd_ee_broadcaster', + 'fd_inertia_broadcaster', + ]: load_controllers += [ Node( package='controller_manager', diff --git a/fd_controllers/fd_inertia_broadcaster/CMakeLists.txt b/fd_controllers/fd_inertia_broadcaster/CMakeLists.txt new file mode 100644 index 0000000..e2ae8fc --- /dev/null +++ b/fd_controllers/fd_inertia_broadcaster/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.5) +project(fd_inertia_broadcaster) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(controller_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rcutils REQUIRED) +find_package(realtime_tools REQUIRED) +find_package(std_msgs REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3) + +add_library(fd_inertia_broadcaster + SHARED + src/fd_inertia_broadcaster.cpp +) +target_include_directories(fd_inertia_broadcaster PRIVATE include) +ament_target_dependencies(fd_inertia_broadcaster + builtin_interfaces + controller_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + std_msgs + eigen3_cmake_module + Eigen3 +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(fd_inertia_broadcaster PRIVATE "FD_INERTIA_BROADCASTER_BUILDING_DLL") +# prevent pluginlib from using boost +target_compile_definitions(fd_inertia_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(controller_interface fd_inertia_broadcaster_plugin.xml) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + fd_inertia_broadcaster + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_dependencies( + controller_interface + rclcpp_lifecycle + geometry_msgs + eigen3_cmake_module + Eigen3 + example_interfaces +) +ament_export_include_directories( + include +) +ament_export_libraries( + fd_inertia_broadcaster +) +ament_package() diff --git a/fd_controllers/fd_inertia_broadcaster/fd_inertia_broadcaster_plugin.xml b/fd_controllers/fd_inertia_broadcaster/fd_inertia_broadcaster_plugin.xml new file mode 100644 index 0000000..b41ebae --- /dev/null +++ b/fd_controllers/fd_inertia_broadcaster/fd_inertia_broadcaster_plugin.xml @@ -0,0 +1,7 @@ + + + + The fd inertia broadcaster publishes the current cartesian inertia from ros2_control fd system. + + + diff --git a/fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/fd_inertia_broadcaster.hpp b/fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/fd_inertia_broadcaster.hpp new file mode 100644 index 0000000..7451d57 --- /dev/null +++ b/fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/fd_inertia_broadcaster.hpp @@ -0,0 +1,83 @@ +// Copyright 2022, ICube Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FD_INERTIA_BROADCASTER__FD_INERTIA_BROADCASTER_HPP_ +#define FD_INERTIA_BROADCASTER__FD_INERTIA_BROADCASTER_HPP_ + +#include +#include + +#include +#include +#include +#include + +#include "fd_inertia_broadcaster/visibility_control.h" + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "realtime_tools/realtime_publisher.h" + +#include "std_msgs/msg/float64_multi_array.hpp" + +namespace fd_inertia_broadcaster +{ +class FdInertiaBroadcaster : public controller_interface::ControllerInterface +{ +public: + FD_INERTIA_BROADCASTER_PUBLIC + FdInertiaBroadcaster(); + + FD_INERTIA_BROADCASTER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + + FD_INERTIA_BROADCASTER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + FD_INERTIA_BROADCASTER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + FD_INERTIA_BROADCASTER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; + + FD_INERTIA_BROADCASTER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + FD_INERTIA_BROADCASTER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + FD_INERTIA_BROADCASTER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + +protected: + std::string inertia_interface_name_; + + Eigen::Matrix4d transform_; + Eigen::Matrix inertia_in_base_, inertia_; + + // Publishers + std::shared_ptr> inertia_publisher_; + std::shared_ptr> + realtime_inertia_publisher_; +}; + +} // namespace fd_inertia_broadcaster + +#endif // FD_INERTIA_BROADCASTER__FD_INERTIA_BROADCASTER_HPP_ diff --git a/fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/visibility_control.h b/fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/visibility_control.h new file mode 100644 index 0000000..519cf7c --- /dev/null +++ b/fd_controllers/fd_inertia_broadcaster/include/fd_inertia_broadcaster/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef FD_INERTIA_BROADCASTER__VISIBILITY_CONTROL_H_ +#define FD_INERTIA_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define FD_INERTIA_BROADCASTER_EXPORT __attribute__((dllexport)) +#define FD_INERTIA_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define FD_INERTIA_BROADCASTER_EXPORT __declspec(dllexport) +#define FD_INERTIA_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef FD_INERTIA_BROADCASTER_BUILDING_DLL +#define FD_INERTIA_BROADCASTER_PUBLIC FD_INERTIA_BROADCASTER_EXPORT +#else +#define FD_INERTIA_BROADCASTER_PUBLIC FD_INERTIA_BROADCASTER_IMPORT +#endif +#define FD_INERTIA_BROADCASTER_PUBLIC_TYPE FD_INERTIA_BROADCASTER_PUBLIC +#define FD_INERTIA_BROADCASTER_LOCAL +#else +#define FD_INERTIA_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define FD_INERTIA_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define FD_INERTIA_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define FD_INERTIA_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define FD_INERTIA_BROADCASTER_PUBLIC +#define FD_INERTIA_BROADCASTER_LOCAL +#endif +#define FD_INERTIA_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // FD_INERTIA_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/fd_controllers/fd_inertia_broadcaster/package.xml b/fd_controllers/fd_inertia_broadcaster/package.xml new file mode 100644 index 0000000..328d729 --- /dev/null +++ b/fd_controllers/fd_inertia_broadcaster/package.xml @@ -0,0 +1,38 @@ + + + fd_inertia_broadcaster + 0.0.0 + Broadcaster to publish cartesian inertia + Thibault Poignonec + + Apache License 2.0 + + ament_cmake + + pluginlib + rcutils + eigen3_cmake_module + eigen + + pluginlib + rcutils + + controller_interface + hardware_interface + rclcpp_lifecycle + realtime_tools + geometry_msgs + std_msgs + + ament_cmake_gmock + controller_manager + rclcpp + ros2_control_test_assets + + eigen3_cmake_module + eigen + + + ament_cmake + + diff --git a/fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp b/fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp new file mode 100644 index 0000000..690aa38 --- /dev/null +++ b/fd_controllers/fd_inertia_broadcaster/src/fd_inertia_broadcaster.cpp @@ -0,0 +1,248 @@ +// Copyright 2022, ICube Laboratory, University of Strasbourg +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include + +#include +#include +#include +#include +#include +#include + +#include "fd_inertia_broadcaster/fd_inertia_broadcaster.hpp" + +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rcpputils/split.hpp" +#include "rcutils/logging_macros.h" +#include "std_msgs/msg/header.hpp" + + +namespace rclcpp_lifecycle +{ +class State; +} // namespace rclcpp_lifecycle + +namespace fd_inertia_broadcaster +{ +const auto kUninitializedValue = std::numeric_limits::quiet_NaN(); +const size_t sizeFlattenedInertia = 15; + +unsigned int flattened_index_from_triangular_index(unsigned int idx_row, unsigned int idx_col) +{ + unsigned int i = idx_row; + unsigned int j = idx_col; + if (idx_col < idx_row) { + i = idx_col; + j = idx_row; + } + return i * (i - 1) / 2 + j; +} + +template +void matrixEigenToMsg(const Eigen::MatrixBase & e, std_msgs::msg::Float64MultiArray & m) +{ + if (m.layout.dim.size() != 2) { + m.layout.dim.resize(2); + } + m.layout.dim[0].stride = e.rows() * e.cols(); + m.layout.dim[0].size = e.rows(); + m.layout.dim[1].stride = e.cols(); + m.layout.dim[1].size = e.cols(); + if (static_cast(m.data.size()) != e.size()) { + m.data.resize(e.size()); + } + int ii = 0; + for (int i = 0; i < e.rows(); ++i) { + for (int j = 0; j < e.cols(); ++j) { + m.data[ii++] = e.coeff(i, j); + } + } +} + +FdInertiaBroadcaster::FdInertiaBroadcaster() {} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +FdInertiaBroadcaster::on_init() +{ + try { + auto_declare("inertia_interface_name", std::string("fd_inertia")); + auto_declare>("transform_translation", std::vector()); + auto_declare>("transform_rotation", std::vector()); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +FdInertiaBroadcaster::command_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; +} + +controller_interface::InterfaceConfiguration FdInertiaBroadcaster::state_interface_configuration() +const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + if (inertia_interface_name_.empty()) { + RCLCPP_WARN(get_node()->get_logger(), "No inertia interface name provided!"); + + } else { + // 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_config.names.push_back( + inertia_interface_name_ + + "/" + + std::to_string(row) + "" + std::to_string(col) + ); + } + } + } + return state_interfaces_config; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +FdInertiaBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) +{ + inertia_interface_name_ = get_node()->get_parameter("inertia_interface_name").as_string(); + if (inertia_interface_name_.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "Please provide the inertia interface name!"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + + // Get transform parameters + 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; + Eigen::Vector3d trans = Eigen::Vector3d::Zero(); + + if (transform_trans_param.size() == 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]; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Wrong translation format"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + + if (transform_rot_param.size() == 0) { + q = Eigen::Quaternion(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()); + Eigen::AngleAxisd pitchAngle(transform_rot_param[2], Eigen::Vector3d::UnitX()); + q = rollAngle * yawAngle * pitchAngle; + } else if (transform_rot_param.size() == 4) { + q = Eigen::Quaternion( + transform_rot_param[3], transform_rot_param[0], + transform_rot_param[1], transform_rot_param[2]); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Wrong rotation format: supported rpy, quaternion"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + transform_ = Eigen::Matrix4d::Identity(); + transform_.block<3, 3>(0, 0) = q.matrix(); + transform_.block<3, 1>(0, 3) = trans; + + std::cout << transform_ << std::endl; + + try { + inertia_publisher_ = get_node()->create_publisher( + "fd_inertia", + rclcpp::SystemDefaultsQoS()); + realtime_inertia_publisher_ = + std::make_shared>( + inertia_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()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + + + inertia_in_base_ = Eigen::Matrix::Zero(); + inertia_ = Eigen::Matrix::Zero(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +FdInertiaBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (state_interfaces_.size() != sizeFlattenedInertia) { + 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 +FdInertiaBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type FdInertiaBroadcaster::update( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) +{ + if (realtime_inertia_publisher_ && realtime_inertia_publisher_->trylock()) { + inertia_in_base_ = Eigen::Matrix::Zero(); + + // Map upper triangular part of inertia to inertia state interface + for (uint row = 0; row < 6; row++) { + for (uint col = 0; col < 6; col++) { + inertia_in_base_(row, col) = + state_interfaces_[flattened_index_from_triangular_index(row, col)].get_value(); + } + } + + // Registration of inertia values + inertia_.block<3, 3>(0, 0) = + transform_.block<3, 3>(0, 0) * inertia_in_base_.block<3, 3>(0, 0) * + transform_.block<3, 3>(0, 0).transpose(); + inertia_.block<3, 3>(3, 3) = + transform_.block<3, 3>(0, 0) * inertia_in_base_.block<3, 3>(3, 3) * + transform_.block<3, 3>(0, 0).transpose(); + + // Publish inertia + auto & fd_inertia_msg = realtime_inertia_publisher_->msg_; + matrixEigenToMsg(inertia_, fd_inertia_msg); + realtime_inertia_publisher_->unlockAndPublish(); + } + + return controller_interface::return_type::OK; +} + +} // namespace fd_inertia_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + fd_inertia_broadcaster::FdInertiaBroadcaster, controller_interface::ControllerInterface) diff --git a/fd_description/config/fd_controllers.yaml b/fd_description/config/fd_controllers.yaml index 42f5ae6..ae05d7b 100644 --- a/fd_description/config/fd_controllers.yaml +++ b/fd_description/config/fd_controllers.yaml @@ -8,6 +8,9 @@ fd/controller_manager: fd_ee_broadcaster: type: ee_pose_broadcaster/EePoseBroadcaster + fd_inertia_broadcaster: + type: fd_inertia_broadcaster/FdInertiaBroadcaster + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -49,3 +52,11 @@ fd/fd_ee_broadcaster: # - fd_pitch # - fd_yaw buttons: [button] # Note: only one button supported for now + +fd/fd_inertia_broadcaster: + ros__parameters: + transform_rotation: + - 0.0 + - 0.0 + - 0.0 + inertia_interface_name: fd_inertia