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 @@
+
+
+
+ The fd inertia broadcaster publishes the current cartesian inertia from ros2_control fd system.
+
+
+
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
+#include
+
+#include
+#include
+#include
+#include
+
+#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> clutch_publisher_;
+ std::shared_ptr>
+ 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 @@
+
+
+ fd_clutch_broadcaster
+ 0.0.0
+ Broadcaster to publish the clutch state (1 = engaged / 0 = disengaged)
+ 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_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
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#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("clutch_interface_name", std::string("button/position"));
+ auto_declare("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(
+ "fd_clutch",
+ rclcpp::SystemDefaultsQoS());
+ realtime_clutch_publisher_ =
+ std::make_shared>(
+ 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