Add fd inertia broadcaster (#21)

* add fd_inertia_broadcaster

* update example

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',

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()

fd_controllers/fd_inertia_broadcaster/fd_inertia_broadcaster_plugin.xml

@@ -0,0 +1,7 @@
+<library path="fd_inertia_broadcaster">
+  <class name="fd_inertia_broadcaster/FdInertiaBroadcaster" type="fd_inertia_broadcaster::FdInertiaBroadcaster" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    The fd inertia broadcaster publishes the current cartesian inertia from ros2_control fd system.
+  </description>
+  </class>
+</library>

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 <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#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<double, 6, 6> inertia_in_base_, inertia_;
+
+  //  Publishers
+  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64MultiArray>> inertia_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::msg::Float64MultiArray>>
+  realtime_inertia_publisher_;
+};
+
+}  // namespace fd_inertia_broadcaster
+
+#endif  // FD_INERTIA_BROADCASTER__FD_INERTIA_BROADCASTER_HPP_

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_

fd_controllers/fd_inertia_broadcaster/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>fd_inertia_broadcaster</name>
+  <version>0.0.0</version>
+  <description>Broadcaster to publish cartesian inertia</description>
+  <maintainer email="thibault.poignonec@gmail.com">Thibault Poignonec</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <build_depend>rcutils</build_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+  <build_depend>eigen</build_depend>
+
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>rcutils</exec_depend>
+
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>rclcpp</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
+  <build_export_depend>eigen</build_export_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>