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