From 338e28c1d0884d3c0c7343e1a43c083d009eefb9 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Fri, 9 Aug 2024 19:14:00 +0200
Subject: [PATCH] Add fd clutch publisher (#22) (#23)

* add fd_clutch_broadcaster

* update example

* update clutching rule for Omega 6

(cherry picked from commit 990b71bc0922c72fe9d5884b496559402b17b313)

Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com>
---
 fd_bringup/launch/fd.launch.py                |   1 +
 .../fd_clutch_broadcaster/CMakeLists.txt      |  73 ++++++++
 .../fd_clutch_broadcaster_plugin.xml          |  10 ++
 .../fd_clutch_broadcaster.hpp                 |  80 +++++++++
 .../visibility_control.h                      |  56 ++++++
 .../fd_clutch_broadcaster/package.xml         |  38 ++++
 .../src/fd_clutch_broadcaster.cpp             | 162 ++++++++++++++++++
 fd_description/config/fd_controllers.yaml     |  13 +-
 8 files changed, 431 insertions(+), 2 deletions(-)
 create mode 100644 fd_controllers/fd_clutch_broadcaster/CMakeLists.txt
 create mode 100644 fd_controllers/fd_clutch_broadcaster/fd_clutch_broadcaster_plugin.xml
 create mode 100644 fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/fd_clutch_broadcaster.hpp
 create mode 100644 fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/visibility_control.h
 create mode 100644 fd_controllers/fd_clutch_broadcaster/package.xml
 create mode 100644 fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp

diff --git a/fd_bringup/launch/fd.launch.py b/fd_bringup/launch/fd.launch.py
index 146a850..14837b8 100644
--- a/fd_bringup/launch/fd.launch.py
+++ b/fd_bringup/launch/fd.launch.py
@@ -95,6 +95,7 @@ def generate_launch_description():
         'joint_state_broadcaster',
         'fd_ee_broadcaster',
         'fd_inertia_broadcaster',
+        'fd_clutch_broadcaster',
     ]:
         load_controllers += [
             Node(
diff --git a/fd_controllers/fd_clutch_broadcaster/CMakeLists.txt b/fd_controllers/fd_clutch_broadcaster/CMakeLists.txt
new file mode 100644
index 0000000..adaad6b
--- /dev/null
+++ b/fd_controllers/fd_clutch_broadcaster/CMakeLists.txt
@@ -0,0 +1,73 @@
+cmake_minimum_required(VERSION 3.5)
+project(fd_clutch_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_clutch_broadcaster
+  SHARED
+  src/fd_clutch_broadcaster.cpp
+)
+target_include_directories(fd_clutch_broadcaster PRIVATE include)
+ament_target_dependencies(fd_clutch_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_clutch_broadcaster PRIVATE "FD_CLUTCH_BROADCASTER_BUILDING_DLL")
+# prevent pluginlib from using boost
+target_compile_definitions(fd_clutch_broadcaster PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+pluginlib_export_plugin_description_file(controller_interface fd_clutch_broadcaster_plugin.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS
+  fd_clutch_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_clutch_broadcaster
+)
+ament_package()
diff --git a/fd_controllers/fd_clutch_broadcaster/fd_clutch_broadcaster_plugin.xml b/fd_controllers/fd_clutch_broadcaster/fd_clutch_broadcaster_plugin.xml
new file mode 100644
index 0000000..4b2aa1e
--- /dev/null
+++ b/fd_controllers/fd_clutch_broadcaster/fd_clutch_broadcaster_plugin.xml
@@ -0,0 +1,10 @@
+<library path="fd_clutch_broadcaster">
+  <class
+    name="fd_clutch_broadcaster/FdClutchBroadcaster"
+    type="fd_clutch_broadcaster::FdClutchBroadcaster"
+    base_class_type="controller_interface::ControllerInterface">
+    <description>
+      The fd inertia broadcaster publishes the current cartesian inertia from ros2_control fd system.
+    </description>
+  </class>
+</library>
diff --git a/fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/fd_clutch_broadcaster.hpp b/fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/fd_clutch_broadcaster.hpp
new file mode 100644
index 0000000..238bde8
--- /dev/null
+++ b/fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/fd_clutch_broadcaster.hpp
@@ -0,0 +1,80 @@
+// 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_CLUTCH_BROADCASTER__FD_CLUTCH_BROADCASTER_HPP_
+#define FD_CLUTCH_BROADCASTER__FD_CLUTCH_BROADCASTER_HPP_
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "fd_clutch_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/bool.hpp"
+
+namespace fd_clutch_broadcaster
+{
+class FdClutchBroadcaster : public controller_interface::ControllerInterface
+{
+public:
+  FD_CLUTCH_BROADCASTER_PUBLIC
+  FdClutchBroadcaster();
+
+  FD_CLUTCH_BROADCASTER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override;
+
+  FD_CLUTCH_BROADCASTER_PUBLIC
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  FD_CLUTCH_BROADCASTER_PUBLIC
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  FD_CLUTCH_BROADCASTER_PUBLIC
+  controller_interface::return_type update(
+    const rclcpp::Time & time,
+    const rclcpp::Duration & period) override;
+
+  FD_CLUTCH_BROADCASTER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  FD_CLUTCH_BROADCASTER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  FD_CLUTCH_BROADCASTER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+  std::string clutch_interface_name_;
+
+  //  Publishers
+  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>> clutch_publisher_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>
+  realtime_clutch_publisher_;
+};
+
+}  // namespace fd_clutch_broadcaster
+
+#endif  // FD_CLUTCH_BROADCASTER__FD_CLUTCH_BROADCASTER_HPP_
diff --git a/fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/visibility_control.h b/fd_controllers/fd_clutch_broadcaster/include/fd_clutch_broadcaster/visibility_control.h
new file mode 100644
index 0000000..47ce18e
--- /dev/null
+++ b/fd_controllers/fd_clutch_broadcaster/include/fd_clutch_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_CLUTCH_BROADCASTER__VISIBILITY_CONTROL_H_
+#define FD_CLUTCH_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_CLUTCH_BROADCASTER_EXPORT __attribute__((dllexport))
+#define FD_CLUTCH_BROADCASTER_IMPORT __attribute__((dllimport))
+#else
+#define FD_CLUTCH_BROADCASTER_EXPORT __declspec(dllexport)
+#define FD_CLUTCH_BROADCASTER_IMPORT __declspec(dllimport)
+#endif
+#ifdef FD_CLUTCH_BROADCASTER_BUILDING_DLL
+#define FD_CLUTCH_BROADCASTER_PUBLIC FD_CLUTCH_BROADCASTER_EXPORT
+#else
+#define FD_CLUTCH_BROADCASTER_PUBLIC FD_CLUTCH_BROADCASTER_IMPORT
+#endif
+#define FD_CLUTCH_BROADCASTER_PUBLIC_TYPE FD_CLUTCH_BROADCASTER_PUBLIC
+#define FD_CLUTCH_BROADCASTER_LOCAL
+#else
+#define FD_CLUTCH_BROADCASTER_EXPORT __attribute__((visibility("default")))
+#define FD_CLUTCH_BROADCASTER_IMPORT
+#if __GNUC__ >= 4
+#define FD_CLUTCH_BROADCASTER_PUBLIC __attribute__((visibility("default")))
+#define FD_CLUTCH_BROADCASTER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define FD_CLUTCH_BROADCASTER_PUBLIC
+#define FD_CLUTCH_BROADCASTER_LOCAL
+#endif
+#define FD_CLUTCH_BROADCASTER_PUBLIC_TYPE
+#endif
+
+#endif  // FD_CLUTCH_BROADCASTER__VISIBILITY_CONTROL_H_
diff --git a/fd_controllers/fd_clutch_broadcaster/package.xml b/fd_controllers/fd_clutch_broadcaster/package.xml
new file mode 100644
index 0000000..8463e29
--- /dev/null
+++ b/fd_controllers/fd_clutch_broadcaster/package.xml
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>fd_clutch_broadcaster</name>
+  <version>0.0.0</version>
+  <description>Broadcaster to publish the clutch state (1 = engaged / 0 = disengaged)</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>
diff --git a/fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp b/fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp
new file mode 100644
index 0000000..74f417d
--- /dev/null
+++ b/fd_controllers/fd_clutch_broadcaster/src/fd_clutch_broadcaster.cpp
@@ -0,0 +1,162 @@
+// 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 <Eigen/Dense>
+
+#include <stddef.h>
+#include <limits>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "fd_clutch_broadcaster/fd_clutch_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_clutch_broadcaster
+{
+
+FdClutchBroadcaster::FdClutchBroadcaster() {}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+FdClutchBroadcaster::on_init()
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration
+FdClutchBroadcaster::command_interface_configuration() const
+{
+  return controller_interface::InterfaceConfiguration{
+    controller_interface::interface_configuration_type::NONE};
+}
+
+controller_interface::InterfaceConfiguration FdClutchBroadcaster::state_interface_configuration()
+const
+{
+  controller_interface::InterfaceConfiguration state_interfaces_config;
+  state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  if (clutch_interface_name_.empty()) {
+    RCLCPP_WARN(get_node()->get_logger(), "No clutch interface name provided!");
+
+  } else {
+    state_interfaces_config.names.push_back(clutch_interface_name_);
+  }
+  return state_interfaces_config;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+FdClutchBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  // Declare parameters
+  try {
+    auto_declare<std::string>("clutch_interface_name", std::string("button/position"));
+    auto_declare<bool>("is_interface_a_button", true);
+  } catch (const std::exception & e) {
+    fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what());
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
+  // Get interface name from parameters
+  clutch_interface_name_ = get_node()->get_parameter("clutch_interface_name").as_string();
+  if (clutch_interface_name_.empty()) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Please provide the clutch interface name!");
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
+  try {
+    clutch_publisher_ = get_node()->create_publisher<std_msgs::msg::Bool>(
+      "fd_clutch",
+      rclcpp::SystemDefaultsQoS());
+    realtime_clutch_publisher_ =
+      std::make_shared<realtime_tools::RealtimePublisher<std_msgs::msg::Bool>>(
+      clutch_publisher_);
+  } catch (const std::exception & e) {
+    // get_node() may throw, logging raw here
+    fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what());
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+FdClutchBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  if (state_interfaces_.size() != 1) {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "Expecting exactly one state interface.");
+  }
+
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+FdClutchBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type FdClutchBroadcaster::update(
+  const rclcpp::Time & /*time*/,
+  const rclcpp::Duration & /*period*/)
+{
+  if (realtime_clutch_publisher_ && realtime_clutch_publisher_->trylock()) {
+    // Read provided state interface
+    bool is_interface_a_button = get_node()->get_parameter("is_interface_a_button").as_bool();
+    double read_value = state_interfaces_[0].get_value();
+
+    bool clutch_state = false;
+    if (is_interface_a_button) {
+      // Clutched (workspace engaged) if button is pressed
+      clutch_state = (read_value > 0.5) ? true : false;
+    } else {
+      // Clutched (workspace engaged) if handle angle is small (i.e., physically clutched)
+      // E.g., 7th "joint" of Omega 6 / Sigma 7
+      clutch_state = (read_value > 0.03) ? true : false;
+    }
+
+    // Publish clucth
+    auto & fd_clutch_msg = realtime_clutch_publisher_->msg_;
+    fd_clutch_msg.data = clutch_state;
+    realtime_clutch_publisher_->unlockAndPublish();
+  }
+
+  return controller_interface::return_type::OK;
+}
+
+}  // namespace fd_clutch_broadcaster
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  fd_clutch_broadcaster::FdClutchBroadcaster, controller_interface::ControllerInterface)
diff --git a/fd_description/config/fd_controllers.yaml b/fd_description/config/fd_controllers.yaml
index ae05d7b..0f53bc1 100644
--- a/fd_description/config/fd_controllers.yaml
+++ b/fd_description/config/fd_controllers.yaml
@@ -2,6 +2,9 @@ fd/controller_manager:
   ros__parameters:
     update_rate: 1000  # Hz
 
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
     fd_controller:
       type: effort_controllers/JointGroupEffortController # ForwardCommandController
 
@@ -11,8 +14,9 @@ fd/controller_manager:
     fd_inertia_broadcaster:
       type: fd_inertia_broadcaster/FdInertiaBroadcaster
 
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
+    fd_clutch_broadcaster:
+      type: fd_clutch_broadcaster/FdClutchBroadcaster
+
 
 fd/fd_controller:
   ros__parameters:
@@ -60,3 +64,8 @@ fd/fd_inertia_broadcaster:
       - 0.0
       - 0.0
     inertia_interface_name: fd_inertia
+
+fd/fd_clutch_broadcaster:
+  ros__parameters:
+    clutch_interface_name: "button/position"
+    is_interface_a_button: true