From 7cea102467bc6e7bbaf52b309ac18bb089598301 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Sat, 21 Dec 2024 18:57:20 +0100 Subject: [PATCH 01/75] io gripper controller added which provides functionality to control gripper using ios --- io_gripper_controller/CMakeLists.txt | 95 ++ io_gripper_controller/README.md | 9 + io_gripper_controller/doc/.gitkeep | 0 .../io_gripper_controller.hpp | 260 +++ .../visibility_control.h | 54 + .../io_gripper_controller.xml | 28 + io_gripper_controller/package.xml | 38 + .../src/io_gripper_controller.cpp | 1441 +++++++++++++++++ .../src/io_gripper_controller.yaml | 258 +++ .../test/test_io_gripper_controller.cpp | 48 + .../test/test_io_gripper_controller.hpp | 397 +++++ ...st_io_gripper_controller_all_param_set.cpp | 56 + .../test/test_io_gripper_controller_close.cpp | 125 ++ .../test/test_io_gripper_controller_open.cpp | 119 ++ ...o_gripper_controller_open_close_action.cpp | 189 +++ ...test_io_gripper_controller_reconfigure.cpp | 112 ++ ..._gripper_controller_reconfigure_action.cpp | 115 ++ .../test/test_load_io_gripper_controller.cpp | 41 + 18 files changed, 3385 insertions(+) create mode 100644 io_gripper_controller/CMakeLists.txt create mode 100644 io_gripper_controller/README.md create mode 100644 io_gripper_controller/doc/.gitkeep create mode 100644 io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp create mode 100644 io_gripper_controller/include/io_gripper_controller/visibility_control.h create mode 100644 io_gripper_controller/io_gripper_controller.xml create mode 100644 io_gripper_controller/package.xml create mode 100644 io_gripper_controller/src/io_gripper_controller.cpp create mode 100644 io_gripper_controller/src/io_gripper_controller.yaml create mode 100644 io_gripper_controller/test/test_io_gripper_controller.cpp create mode 100644 io_gripper_controller/test/test_io_gripper_controller.hpp create mode 100644 io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp create mode 100644 io_gripper_controller/test/test_io_gripper_controller_close.cpp create mode 100644 io_gripper_controller/test/test_io_gripper_controller_open.cpp create mode 100644 io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp create mode 100644 io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp create mode 100644 io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp create mode 100644 io_gripper_controller/test/test_load_io_gripper_controller.cpp diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..7bcae6a24e --- /dev/null +++ b/io_gripper_controller/CMakeLists.txt @@ -0,0 +1,95 @@ +cmake_minimum_required(VERSION 3.8) +project(io_gripper_controller) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + sensor_msgs + controller_manager + ros2_control_test_assets + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + ament_cmake + generate_parameter_library + ament_cmake_gmock + std_msgs + control_msgs +) + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +# Add io_gripper_controller library related compile commands +generate_parameter_library(io_gripper_controller_parameters + src/io_gripper_controller.yaml +) +add_library( + io_gripper_controller + SHARED + src/io_gripper_controller.cpp +) +target_include_directories(io_gripper_controller PUBLIC + "$" + "$") +target_link_libraries(io_gripper_controller io_gripper_controller_parameters) +ament_target_dependencies(io_gripper_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(io_gripper_controller PRIVATE "io_gripper_controller_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface io_gripper_controller.xml) + +install( + TARGETS + io_gripper_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +if(BUILD_TESTING) + + ament_add_gmock(test_load_io_gripper_controller test/test_load_io_gripper_controller.cpp) + target_include_directories(test_load_io_gripper_controller PRIVATE include) + ament_target_dependencies( + test_load_io_gripper_controller + controller_manager + hardware_interface + ros2_control_test_assets + ) + + ament_add_gmock(test_io_gripper_controller test/test_io_gripper_controller.cpp test/test_io_gripper_controller_open.cpp test/test_io_gripper_controller_close.cpp test/test_io_gripper_controller_all_param_set.cpp test/test_io_gripper_controller_open_close_action.cpp test/test_io_gripper_controller_reconfigure.cpp test/test_io_gripper_controller_reconfigure_action.cpp) + target_include_directories(test_io_gripper_controller PRIVATE include) + target_link_libraries(test_io_gripper_controller io_gripper_controller) + ament_target_dependencies( + test_io_gripper_controller + controller_interface + hardware_interface + ) + +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + io_gripper_controller +) + +ament_package() diff --git a/io_gripper_controller/README.md b/io_gripper_controller/README.md new file mode 100644 index 0000000000..acc005853d --- /dev/null +++ b/io_gripper_controller/README.md @@ -0,0 +1,9 @@ +# IO Gripper Controller + +The IO Gripper Controller is provides implementation to control the gripper using IOs. It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes `joint_states` of gripper and also `dynamic_interfaces` for all command and state interfaces. + +## Description of controller's interfaces + +- `joint_states` [`sensor_msgs::msg::JointState`]: Publishes the state of gripper joint and configuration joint +- `dynamic_interfaces` [`control_msgs::msg::DynamicInterfaceValues`]: Publishes all command and state interface of the IOs and sensors of gripper. + diff --git a/io_gripper_controller/doc/.gitkeep b/io_gripper_controller/doc/.gitkeep new file mode 100644 index 0000000000..e69de29bb2 diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp new file mode 100644 index 0000000000..01d1b1d197 --- /dev/null +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -0,0 +1,260 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#ifndef GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ +#define GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "io_gripper_controller_parameters.hpp" +#include "io_gripper_controller/visibility_control.h" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +// TODO(anyone): Replace with controller specific messages +#include + +#include "control_msgs/srv/set_config.hpp" +#include "control_msgs/msg/io_gripper_sensor.hpp" +#include "control_msgs/msg/interface_value.hpp" +#include "control_msgs/msg/dynamic_interface_values.hpp" +#include "control_msgs/action/gripper.hpp" +#include "control_msgs/action/set_gripper_config.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace io_gripper_controller +{ + +// TODO(anyone: example setup for control mode (usually you will use some enums defined in messages) +enum class control_mode_type : std::uint8_t +{ + FAST = 0, + SLOW = 1, +}; + +enum class service_mode_type : std::uint8_t +{ + IDLE = 0, + OPEN = 1, + CLOSE = 2, +}; + +// TODO : rearrange it later +enum class gripper_state_type : std::uint8_t +{ + IDLE = 0, + STORE_ORIGINAL_STATE = 1, + SET_BEFORE_COMMAND = 2, + CLOSE_GRIPPER = 3, + CHECK_GRIPPER_STATE = 4, + RESTORE_ORIGINAL_STATE = 5, + CHECK_RESTORE_STATE = 6, + OPEN_GRIPPER = 7, + START_CLOSE_GRIPPER = 8, + SET_AFTER_COMMAND = 9, + HALTED = 10, +}; + +enum class reconfigure_state_type : std::uint8_t +{ + IDLE = 0, + RECONFIGURE = 1, + FIND_CONFIG = 2, + SET_COMMAND = 3, + CHECK_STATE = 4, +}; + +class IOGripperController : public controller_interface::ControllerInterface +{ +public: + GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC + IOGripperController(); + io_gripper_controller::Params params_; + + GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_init() override; + + GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + std::vector command_ios_open, command_ios_close, set_before_command_open, set_after_command_open, reconfigure_command; + std::vector command_ios_open_values, command_ios_close_values, set_before_command_open_values, set_after_command_open_values, reconfigure_command_values; + std::vector state_ios_open, state_ios_close, set_before_command_close, set_after_command_close; + std::vector state_ios_open_values, state_ios_close_values, set_before_command_close_values, set_after_command_close_values, set_after_command_open_values_original_; + std::string status_joint_name; + bool is_open; + std::unordered_map command_if_ios_after_opening; + std::unordered_map original_ios_after_opening; + std::unordered_map command_if_ios_before_closing; + std::unordered_map original_ios_before_closing; + + std::unordered_set command_if_ios, state_if_ios; + + bool setResult; + + + using ControllerModeSrvType = std_srvs::srv::SetBool; + using OpenSrvType = std_srvs::srv::Trigger; + using ConfigSrvType = control_msgs::srv::SetConfig; + using ControllerStateMsg = sensor_msgs::msg::JointState; + using EventStateMsg = sensor_msgs::msg::JointState; + using ConfigJointMsg = sensor_msgs::msg::JointState; + using InterfaceMsg = control_msgs::msg::DynamicInterfaceValues; + using GripperAction = control_msgs::action::Gripper; + using GoalHandleGripper = rclcpp_action::ServerGoalHandle; + using GripperConfigAction = control_msgs::action::SetGripperConfig; + using GoalHandleGripperConfig = rclcpp_action::ServerGoalHandle; + +protected: + std::shared_ptr param_listener_; + + rclcpp::Service::SharedPtr set_slow_control_mode_service_; + rclcpp::Service::SharedPtr open_service_; + rclcpp::Service::SharedPtr close_service_; + rclcpp::Service::SharedPtr configure_gripper_service_; + + rclcpp_action::Server::SharedPtr gripper_action_server_; + rclcpp_action::Server::SharedPtr gripper_config_action_server_; + + realtime_tools::RealtimeBuffer control_mode_; + realtime_tools::RealtimeBuffer service_buffer_; + realtime_tools::RealtimeBuffer configure_gripper_buffer_; + realtime_tools::RealtimeBuffer gripper_state_buffer_; + realtime_tools::RealtimeBuffer reconfigure_state_buffer_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + using EventPublisher = realtime_tools::RealtimePublisher; + + using ConfigPublisher = realtime_tools::RealtimePublisher; + using InterfacePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr g_j_s_publisher_; + std::unique_ptr gripper_joint_state_publisher_; + + std::vector joint_state_values_; + + rclcpp::Publisher::SharedPtr if_publisher_; + std::unique_ptr interface_publisher_; + + + rclcpp::Publisher::SharedPtr e_publisher_; + std::unique_ptr event_publisher_; + + std::atomic reconfigureFlag_{false}, openFlag_{false}, closeFlag_{false}; + // std::atomic reconfigFlag{false}; + +private: + bool find_and_set_command(const std::string & name, const double value); + bool find_and_get_state(const std::string & name, double& value); + bool find_and_get_command(const std::string & name, double& value); + void init_msgs(); + void handle_gripper_state_transition_open(const gripper_state_type & state); + void handle_gripper_state_transition_close(const gripper_state_type & state); + void handle_reconfigure_state_transition(const reconfigure_state_type & state); + /// \brief Function to check the parameters + controller_interface::CallbackReturn check_parameters(); + /// Preparing the command ios and states ios vars for the command/state interface configuraiton + void prepare_command_and_state_ios(); + controller_interface::CallbackReturn prepare_publishers_and_services(); + void publish_gripper_joint_states(); + void publish_dynamic_interface_values(); + void publish_reconfigure_gripper_joint_states(); + void check_gripper_and_reconfigure_state(); + + std::vector configurations_list_; + std::vector config_map_; + std::vector sensors_map_; + double state_value_; + std::string configuration_key_; + bool check_state_ios_; + std::string closed_state_name_; + io_gripper_controller::Params::Close::State::MapPossibleClosedStates closed_state_values_; + io_gripper_controller::Params::ConfigurationSetup::MapConfigurations conf_it_; + std::vector::iterator config_index_; + + rclcpp::CallbackGroup::SharedPtr open_service_callback_group_, close_service_callback_group_, reconfigure_service_callback_group_; + + + std::shared_ptr gripper_feedback_; + std::shared_ptr gripper_result_; + std::shared_ptr gripper_config_feedback_; + std::shared_ptr gripper_config_result_; + + + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr goal_handle); + + void handle_accepted(const std::shared_ptr goal_handle); + void execute(const std::shared_ptr goal_handle); + + rclcpp_action::GoalResponse config_handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + + + rclcpp_action::CancelResponse config_handle_cancel( + const std::shared_ptr goal_handle); + + void config_handle_accepted(const std::shared_ptr goal_handle); + void config_execute(const std::shared_ptr goal_handle); + + + + + + +}; + +} // namespace io_gripper_controller + +#endif // GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ diff --git a/io_gripper_controller/include/io_gripper_controller/visibility_control.h b/io_gripper_controller/include/io_gripper_controller/visibility_control.h new file mode 100644 index 0000000000..199423764a --- /dev/null +++ b/io_gripper_controller/include/io_gripper_controller/visibility_control.h @@ -0,0 +1,54 @@ +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#ifndef GRIPPER_IO_CONTROLLER__VISIBILITY_CONTROL_H_ +#define GRIPPER_IO_CONTROLLER__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 GRIPPER_IO_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define GRIPPER_IO_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define GRIPPER_IO_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define GRIPPER_IO_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef GRIPPER_IO_CONTROLLER__VISIBILITY_BUILDING_DLL +#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC GRIPPER_IO_CONTROLLER__VISIBILITY_EXPORT +#else +#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC GRIPPER_IO_CONTROLLER__VISIBILITY_IMPORT +#endif +#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC_TYPE GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC +#define GRIPPER_IO_CONTROLLER__VISIBILITY_LOCAL +#else +#define GRIPPER_IO_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define GRIPPER_IO_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define GRIPPER_IO_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC +#define GRIPPER_IO_CONTROLLER__VISIBILITY_LOCAL +#endif +#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // GRIPPER_IO_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/io_gripper_controller/io_gripper_controller.xml b/io_gripper_controller/io_gripper_controller.xml new file mode 100644 index 0000000000..660677e458 --- /dev/null +++ b/io_gripper_controller/io_gripper_controller.xml @@ -0,0 +1,28 @@ + + + + + + IOGripperController ros2_control controller used to control the gripper using IOs. + + + diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml new file mode 100644 index 0000000000..7d00ccbac0 --- /dev/null +++ b/io_gripper_controller/package.xml @@ -0,0 +1,38 @@ + + + + io_gripper_controller + 0.0.0 + gripper io controller used to control the gripper using IOs + Yara Shahin + Sachin Kumar + Apache License 2.0 + + ament_cmake + + generate_parameter_library + + control_msgs + controller_manager + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_cmake_gmock + ament_lint_auto + ament_lint_common + controller_interface + hardware_interface + + + ament_cmake + + diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp new file mode 100644 index 0000000000..f1a64d63cd --- /dev/null +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -0,0 +1,1441 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "io_gripper_controller/io_gripper_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" + +namespace +{ // utility + +// TODO(destogl): remove this when merged upstream +// Changed services history QoS to keep all so we don't lose any client service calls +static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; + +} // namespace + +namespace io_gripper_controller +{ +IOGripperController::IOGripperController() : controller_interface::ControllerInterface() {} + +controller_interface::CallbackReturn IOGripperController::on_init() +{ + service_buffer_.initRT(service_mode_type::IDLE); + configuration_key_ = ""; + configure_gripper_buffer_.initRT(configuration_key_); + gripper_state_buffer_.initRT(gripper_state_type::IDLE); + reconfigure_state_buffer_.initRT(reconfigure_state_type::IDLE); + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn IOGripperController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + + auto result = check_parameters(); + if (result != controller_interface::CallbackReturn::SUCCESS) + { + return result; + } + + /** + * realtime publisher for the gripper_specific sensors, type publishing is boolean + */ + + prepare_command_and_state_ios(); + + result = prepare_publishers_and_services(); + if (result != controller_interface::CallbackReturn::SUCCESS) + { + return result; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration IOGripperController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(command_if_ios.size()); + for (const auto & command_io : command_if_ios) + { + command_interfaces_config.names.push_back(command_io); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration IOGripperController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_if_ios.size()); + for (const auto & state_io : state_if_ios) + { + state_interfaces_config.names.push_back(state_io); + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn IOGripperController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + check_gripper_and_reconfigure_state(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn IOGripperController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joint_state_values_.resize(params_.open_close_joints.size() + params_.configuration_joints.size(), std::numeric_limits::quiet_NaN()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type IOGripperController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + if (reconfigureFlag_.load()) + { + configuration_key_ = *(configure_gripper_buffer_.readFromRT()); + handle_reconfigure_state_transition(*(reconfigure_state_buffer_.readFromRT())); + } + + auto current_command = service_buffer_.readFromRT(); + switch (*(service_buffer_.readFromRT())) + { + case service_mode_type::IDLE: + // do nothing + break; + case service_mode_type::OPEN: + handle_gripper_state_transition_open(*(gripper_state_buffer_.readFromRT())); + break; + case service_mode_type::CLOSE: + handle_gripper_state_transition_close(*(gripper_state_buffer_.readFromRT())); + break; + + default: + break; + } + + // this is publishing the joint states for the gripper and reconfigure + publish_gripper_joint_states(); + publish_dynamic_interface_values(); + + return controller_interface::return_type::OK; +} + +bool IOGripperController::find_and_set_command(const std::string & name, const double value) +{ + auto it = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&](const hardware_interface::LoanedCommandInterface & command_interface) + { + return command_interface.get_name() == name; + }); + + if (it != command_interfaces_.end()) + { + it->set_value(value); + return true; + } + return false; +} + +bool IOGripperController::find_and_get_state(const std::string & name, double& value) +{ + auto it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), + [&](const hardware_interface::LoanedStateInterface & state_interface) + { + return state_interface.get_name() == name; + }); + + if (it != state_interfaces_.end()) + { + value = it->get_value(); + return true; + } + value = 0.0f; + return false; +} + +bool IOGripperController::find_and_get_command(const std::string & name, double& value) +{ + auto it = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&](const hardware_interface::LoanedCommandInterface & command_interface) + { + return command_interface.get_name() == name; + }); + + if (it != command_interfaces_.end()) + { + value = it->get_value(); + return true; + } + value = 0.0f; + return false; +} + + +void IOGripperController::handle_gripper_state_transition_close(const gripper_state_type & state) +{ + switch (state) + { + case gripper_state_type::IDLE: + // do nothing + break; + case gripper_state_type::SET_BEFORE_COMMAND: + // set before closeing + for (size_t i = 0; i < set_before_command_close.size(); ++i) + { + setResult = find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", set_before_command_close[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CLOSE_GRIPPER); + break; + case gripper_state_type::CLOSE_GRIPPER: + for (size_t i = 0; i < command_ios_close.size(); ++i) + { + setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", command_ios_close[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); + break; + case gripper_state_type::CHECK_GRIPPER_STATE: + for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) + { + check_state_ios_ = false; + for (const auto & high_val : state_params.high) + { + setResult = find_and_get_state(high_val, state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); + } + else { + if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) + { + check_state_ios_ = true; + } + else { + check_state_ios_ = false; + break; + } + } + } + for (const auto & low_val : state_params.low) + { + setResult = find_and_get_state(low_val, state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); + } + else { + if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) + { + check_state_ios_ = true; + } + else { + check_state_ios_ = false; + break; + } + } + } + if (check_state_ios_) + { + closed_state_name_ = state_name; + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); + break; + } + } + break; + case gripper_state_type::SET_AFTER_COMMAND: + closed_state_values_ = params_.close.state.possible_closed_states_map.at(closed_state_name_); + + for (const auto & high_val : closed_state_values_.set_after_command_high) + { + setResult = find_and_set_command(high_val, 1.0); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", high_val.c_str()); + } + } + + for (const auto & low_val : closed_state_values_.set_after_command_low) + { + setResult = find_and_set_command(low_val, 0.0); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", low_val.c_str()); + } + } + for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states.size(); ++i) + { + joint_state_values_[i] = params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states[i]; + } + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + closeFlag_.store(false); + service_buffer_.writeFromNonRT(service_mode_type::IDLE); + break; + default: + break; + } +} + + +void IOGripperController::handle_gripper_state_transition_open(const gripper_state_type & state) +{ + switch (state) + { + case gripper_state_type::IDLE: + // do nothing + break; + case gripper_state_type::SET_BEFORE_COMMAND: + // set before opening + for (size_t i = 0; i < set_before_command_open.size(); ++i) + { + setResult = find_and_set_command(set_before_command_open[i], set_before_command_open_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", set_before_command_open[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::OPEN_GRIPPER); + break; + case gripper_state_type::OPEN_GRIPPER: + // now open the gripper + for (size_t i = 0; i < command_ios_open.size(); ++i) + { + setResult = find_and_set_command(command_ios_open[i], command_ios_open_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", command_ios_open[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); + break; + case gripper_state_type::CHECK_GRIPPER_STATE: + // check the state of the gripper + check_state_ios_ = false; + for (size_t i = 0; i < state_ios_open.size(); ++i) + { + setResult = find_and_get_state(state_ios_open[i], state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); + } + else { + if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) + { + check_state_ios_ = true; + } + else { + check_state_ios_ = false; + break; + } + } + } + if(check_state_ios_) + { + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); + } + break; + case gripper_state_type::SET_AFTER_COMMAND: + // set after opening + for (size_t i = 0; i < set_after_command_open.size(); ++i) + { + setResult = find_and_set_command(set_after_command_open[i], set_after_command_open_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", set_after_command_open[i].c_str()); + } + } + for (size_t i = 0; i < params_.open.joint_states.size(); ++i) + { + joint_state_values_[i] = params_.open.joint_states[i]; + } + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + openFlag_.store(false); + service_buffer_.writeFromNonRT(service_mode_type::IDLE); + break; + default: + break; + } +} + +void IOGripperController::handle_reconfigure_state_transition(const reconfigure_state_type & state) +{ + switch (state) + { + case reconfigure_state_type::IDLE: + // do nothing + break; + case reconfigure_state_type::FIND_CONFIG: + // find the configuration + config_index_ = std::find(configurations_list_.begin(), configurations_list_.end(), configuration_key_); + if (config_index_ == configurations_list_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Configuration not found"); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); + } + else + { + conf_it_ = config_map_[std::distance(configurations_list_.begin(), config_index_)]; + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); + } + + break; + + case reconfigure_state_type::SET_COMMAND: + setResult = false; + for (const auto & io : conf_it_.command_high) + { + setResult = find_and_set_command(io, 1.0); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", io.c_str()); + } + } + for (const auto & io : conf_it_.command_low) + { + setResult = find_and_set_command(io, 0.0); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", io.c_str()); + } + } + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::CHECK_STATE); + break; + + case reconfigure_state_type::CHECK_STATE: + check_state_ios_ = false; + // implement the code read the state of the gripper + for (const auto & io : conf_it_.state_high) + { + // read the state of the gripper + setResult = find_and_get_state(io, state_value_); + // if the state is not as expected, then set the state to the expected state + if (!setResult) + { + check_state_ios_ = false; + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) + { + check_state_ios_ = false; + RCLCPP_ERROR( + get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + break; + } + else { + check_state_ios_ = true; + } + } + // if the state is as expected, then do nothing + } + + for (const auto & io : conf_it_.state_low) + { + // read the state of the gripper + setResult = find_and_get_state(io, state_value_); + // if the state is not as expected, then set the state to the expected state + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) + { + RCLCPP_ERROR( + get_node()->get_logger(), "value doesn't match %s", io.c_str()); + check_state_ios_ = false; + break; + } + else + { + check_state_ios_ = true; + } + } + // if the state is as expected, then do nothing + } + + if (check_state_ios_) + { + for (size_t i = 0; i < conf_it_.joint_states.size(); ++i) + { + joint_state_values_[i + params_.open_close_joints.size()] = conf_it_.joint_states[i]; + } + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); + configure_gripper_buffer_.writeFromNonRT(""); // this is not required, remove later TODO (Sachin) :s + reconfigureFlag_.store(false); + } + break; + default: + break; + } +} + +controller_interface::CallbackReturn IOGripperController::check_parameters() +{ + /// Param validation + + if (params_.open_close_joints.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of open close joints parameter cannot be zero."); + return CallbackReturn::FAILURE; + } + + + // size of open_close_joint should match with the open.joint_states and close.joint_states + // if (params_.open_close_joints.size() != params_.open.joint_states.size() && + // params_.open_close_joints.size() != params_.close.joint_states.size()) + // { + // RCLCPP_FATAL( + // get_node()->get_logger(), + // "Size of open close joints parameter should match with the open.joint_states and close.joint_states."); + // return CallbackReturn::FAILURE; + // } + + + + if (params_.open.joint_states.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of joint states parameters cannot be zero."); + return CallbackReturn::FAILURE; + } + + if (params_.open.command.high.empty() and params_.open.command.low.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of open command high and low parameters cannot be zero."); + return CallbackReturn::FAILURE; + } + + if (params_.open.state.high.empty() and params_.open.state.low.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of open state high and low parameters cannot be zero."); + return CallbackReturn::FAILURE; + } + + // close parameters + // if (params_.close.joint_states.empty()) + // { + // RCLCPP_FATAL( + // get_node()->get_logger(), + // "Size of joint states parameter cannot be zero."); + // return CallbackReturn::FAILURE; + // } + + if (params_.close.set_before_command.high.empty() and params_.close.set_before_command.low.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of set before command high and low parameters cannot be zero."); + return CallbackReturn::FAILURE; + } + + + if (params_.close.command.high.empty() and params_.close.command.low.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of close command high and low parameters cannot be zero."); + return CallbackReturn::FAILURE; + } + + // if (params_.close.state.high.empty() and params_.close.state.low.empty()) + // { + // RCLCPP_FATAL( + // get_node()->get_logger(), + // "Size of close state high and low parameters cannot be zero."); + // return CallbackReturn::FAILURE; + // } + + + // configurations parameter + if (params_.configurations.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of configurations parameter cannot be zero."); + return CallbackReturn::FAILURE; + } + + // configuration joints parameter + if (params_.configuration_joints.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of configuration joints parameter cannot be zero."); + return CallbackReturn::FAILURE; + } + + // configuration setup parameter + if (params_.configuration_setup.configurations_map.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of configuration map parameter cannot be zero."); + return CallbackReturn::FAILURE; + } + + // gripper_specific_sensors parameter + if (params_.gripper_specific_sensors.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of gripper specific sensors parameter cannot be zero."); + return CallbackReturn::FAILURE; + } + + // sensors interfaces parameter + if (params_.sensors_interfaces.gripper_specific_sensors_map.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of sensors interfaces parameter cannot be zero."); + return CallbackReturn::FAILURE; + } + + // if sensor input string is empty then return failure + for (const auto& [key, val] : params_.sensors_interfaces.gripper_specific_sensors_map) + { + if (val.input == "") + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of sensor input string parameter cannot be empty ("")."); + return CallbackReturn::FAILURE; + } + } + return CallbackReturn::SUCCESS; +} + +void IOGripperController::prepare_command_and_state_ios() +{ + // make full command ios lists -- just once + for (const auto& key : params_.open.set_before_command.high) { + if(!key.empty()) + { + set_before_command_open.push_back(key); + set_before_command_open_values.push_back(1.0); + command_if_ios.insert(key); + state_if_ios.insert(key); + } + } + + for (const auto& key : params_.open.set_before_command.low) { + if(!key.empty()) + { + set_before_command_open.push_back(key); + set_before_command_open_values.push_back(0.0); + command_if_ios.insert(key); + state_if_ios.insert(key); + } + } + for (const auto& key : params_.open.command.high) { + command_ios_open.push_back(key); + command_ios_open_values.push_back(1.0); + // command_ios_open[key] = 1.0; + command_if_ios.insert(key); + state_if_ios.insert(key); + } + + for (const auto& key : params_.open.command.low) { + command_ios_open.push_back(key); + command_ios_open_values.push_back(0.0); + // command_ios_open[key] = 0.0; + command_if_ios.insert(key); + state_if_ios.insert(key); + } + + for (const auto& key : params_.open.set_after_command.high) { + if(!key.empty()) + { + set_after_command_open.push_back(key); + set_after_command_open_values.push_back(1.0); + command_if_ios.insert(key); + state_if_ios.insert(key); + } + } + + for (const auto& key : params_.open.set_after_command.low) { + if(!key.empty()) + { + set_after_command_open.push_back(key); + set_after_command_open_values.push_back(0.0); + command_if_ios.insert(key); + state_if_ios.insert(key); + } + } + + for (const auto& key : params_.close.set_before_command.high) { + if(!key.empty()) + { + set_before_command_close.push_back(key); + set_before_command_close_values.push_back(1.0); + command_if_ios.insert(key); + state_if_ios.insert(key); + } + } + + for (const auto& key : params_.close.set_before_command.low) { + if(!key.empty()) + { + set_before_command_close.push_back(key); + set_before_command_close_values.push_back(0.0); + command_if_ios.insert(key); + state_if_ios.insert(key); + } + } + + for (const auto& key : params_.close.command.high) { + command_ios_close.push_back(key); + command_ios_close_values.push_back(1.0); + // command_ios_close[key] = 1.0; + command_if_ios.insert(key); + state_if_ios.insert(key); + } + for (const auto& key : params_.close.command.low) { + command_ios_close.push_back(key); + command_ios_close_values.push_back(0.0); + // command_ios_close[key] = 0.0; + command_if_ios.insert(key); + state_if_ios.insert(key); + } + + + // make full state ios lists -- just once + for (const auto& key : params_.open.state.high) { + if(!key.empty()) + { + state_ios_open.push_back(key); + state_ios_open_values.push_back(1.0); + state_if_ios.insert(key); + } + } + for (const auto& key : params_.open.state.low) { + if(!key.empty()) + { + state_ios_open.push_back(key); + state_ios_open_values.push_back(0.0); + state_if_ios.insert(key); + } + } + // for (const auto& key : params_.close.state.high) { + // if(!key.empty()) + // { + // state_ios_close.push_back(key); + // state_ios_close_values.push_back(1.0); + // state_if_ios.insert(key); + // } + // } + // for (const auto& key : params_.close.state.low) { + // if(!key.empty()) + // { + // state_ios_close.push_back(key); + // state_ios_close_values.push_back(0.0); + // state_if_ios.insert(key); + // } + // } + + // for (const auto& key : params_.close.set_after_command.high) { + // if(!key.empty()) + // { + // set_after_command_close.push_back(key); + // set_after_command_close_values.push_back(1.0); + // command_if_ios.insert(key); + // state_if_ios.insert(key); + // } + // } + + // for (const auto& key : params_.close.set_after_command.low) { + // if(!key.empty()) + // { + // set_after_command_close.push_back(key); + // set_after_command_close_values.push_back(0.0); + // command_if_ios.insert(key); + // state_if_ios.insert(key); + // } + // } + + // get the configurations for different io which needs to be high or low + for (const auto & [key, val] : params_.configuration_setup.configurations_map) + { + config_map_.push_back(val); + } + + // get the configurations list + configurations_list_ = params_.configurations; + + for (const auto & config : config_map_) + { + for (const auto & io : config.command_high) + { + command_if_ios.insert(io); + state_if_ios.insert(io); + } + for (const auto & io : config.command_low) + { + command_if_ios.insert(io); + state_if_ios.insert(io); + } + for (const auto & io : config.state_high) + { + state_if_ios.insert(io); + } + for (const auto & io : config.state_low) + { + state_if_ios.insert(io); + } + } + + // get the configurations for different io which needs to be high or low + for (const auto & [key, val] : params_.sensors_interfaces.gripper_specific_sensors_map) + { + sensors_map_.push_back(val); + } + + + // TODO (Sachin) : Add the gripper specific sensors to the state_if_ios, not able to access the values, discuss this with Dr. Denis + for (size_t i = 0; i < params_.gripper_specific_sensors.size(); ++i) + { + state_if_ios.insert(params_.sensors_interfaces.gripper_specific_sensors_map.at(params_.gripper_specific_sensors[i]).input); + } +} + +controller_interface::CallbackReturn IOGripperController::prepare_publishers_and_services() +{ + + reconfigureFlag_.store(false); + + // reset service buffer + service_buffer_.writeFromNonRT(service_mode_type::IDLE); + + // reset gripper state buffer + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + + if (!params_.use_action) + { + // callback groups for each service + open_service_callback_group_ = get_node()->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + close_service_callback_group_ = get_node()->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + reconfigure_service_callback_group_ = get_node()->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + // open service + auto open_service_callback = + [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + try + { + if (reconfigureFlag_.load()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); + response->success = false; + return; + } + if (closeFlag_.load()) + { + closeFlag_.store(false); + } + openFlag_.store(true); + service_buffer_.writeFromNonRT(service_mode_type::OPEN); + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); + while(openFlag_.load()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if ((*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED)) + { + response->success = false; + break; + } + else { + response->success = true; + } + } + } + catch (const std::exception & e) + { + response->success = false; + } + }; + + open_service_ = get_node()->create_service( + "~/gripper_open", open_service_callback, + rmw_qos_profile_services_hist_keep_all, open_service_callback_group_); + + // close service + auto close_service_callback = + [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + try + { + if (reconfigureFlag_.load()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); + response->success = false; + return; + } + service_buffer_.writeFromNonRT(service_mode_type::CLOSE); + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); + if (openFlag_.load()) + { + openFlag_.store(false); + } + closeFlag_.store(true); + while(closeFlag_.load()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if ((*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED)) + { + response->success = false; + break; + } + else { + response->success = true; + } + } + } + catch (const std::exception & e) + { + response->success = false; + } + }; + + close_service_ = get_node()->create_service( + "~/gripper_close", close_service_callback, + rmw_qos_profile_services_hist_keep_all, close_service_callback_group_); + + // configure gripper service + // TODO (Sachin) : Change service type to string + auto configure_gripper_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + try + { + std::string conf = request->config_name; + configure_gripper_buffer_.writeFromNonRT(conf.c_str()); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); + reconfigureFlag_.store(true); + // wait with thread sleep, until certain flag is not set + while(reconfigureFlag_.load()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + response->result = true; + response->status = "Gripper reconfigured"; + } + catch (const std::exception & e) + { + response->result = false; + response->status = "Failed to reconfigure gripper"; + } + }; + + configure_gripper_service_ = get_node()->create_service( + "~/reconfigure_to", configure_gripper_service_callback, + rmw_qos_profile_services_hist_keep_all, reconfigure_service_callback_group_); + + } + else + { + // open close action server + gripper_feedback_ = std::make_shared(); + gripper_result_ = std::make_shared(); + gripper_action_server_ = rclcpp_action::create_server( + get_node(), "~/gripper_action", + std::bind(&IOGripperController::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&IOGripperController::handle_cancel, this, std::placeholders::_1), + std::bind(&IOGripperController::handle_accepted, this, std::placeholders::_1)); + + // reconfigure action server + gripper_config_feedback_ = std::make_shared(); + gripper_config_result_ = std::make_shared(); + gripper_config_action_server_ = rclcpp_action::create_server( + get_node(), "~/reconfigure_gripper_action", + std::bind(&IOGripperController::config_handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&IOGripperController::config_handle_cancel, this, std::placeholders::_1), + std::bind(&IOGripperController::config_handle_accepted, this, std::placeholders::_1)); + } + + try + { + // Gripper Joint State publisher + g_j_s_publisher_ = + get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); + gripper_joint_state_publisher_ = std::make_unique(g_j_s_publisher_); + + gripper_joint_state_publisher_->msg_.name.resize(params_.open_close_joints.size() + params_.configuration_joints.size()); + gripper_joint_state_publisher_->msg_.position.resize(params_.open_close_joints.size() + params_.configuration_joints.size()); + + joint_state_values_.resize(params_.open_close_joints.size() + params_.configuration_joints.size(), std::numeric_limits::quiet_NaN()); + + for (size_t i = 0; i < params_.open_close_joints.size(); ++i) + { + gripper_joint_state_publisher_->msg_.name[i] = params_.open_close_joints[i]; + gripper_joint_state_publisher_->msg_.position[i] = joint_state_values_[i]; + } + for (size_t i = 0; i < params_.configuration_joints.size(); ++i) + { + gripper_joint_state_publisher_->msg_.name[i + params_.open_close_joints.size()] = params_.configuration_joints[i]; + gripper_joint_state_publisher_->msg_.position[i + params_.open_close_joints.size()] = joint_state_values_[i + params_.open_close_joints.size()]; + } + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + try + { + // interface publisher + if_publisher_ = get_node()->create_publisher( + "~/dynamic_interface", rclcpp::SystemDefaultsQoS()); + interface_publisher_ = std::make_unique(if_publisher_); + } + catch(const std::exception& e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void IOGripperController::publish_gripper_joint_states() +{ + if (gripper_joint_state_publisher_ && gripper_joint_state_publisher_->trylock()) + { + gripper_joint_state_publisher_->msg_.header.stamp = get_node()->get_clock()->now(); + + // publish gripper joint state values + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + gripper_joint_state_publisher_->msg_.position[i] = joint_state_values_[i]; + } + } + gripper_joint_state_publisher_->unlockAndPublish(); + +} + +void IOGripperController::publish_dynamic_interface_values() +{ + if (interface_publisher_ && interface_publisher_->trylock()) + { + interface_publisher_->msg_.header.stamp = get_node()->get_clock()->now(); // Make sure it works and discuss with Dr. Denis + interface_publisher_->msg_.states.interface_names.clear(); + interface_publisher_->msg_.states.values.clear(); + interface_publisher_->msg_.states.values.resize(state_interfaces_.size()); + for (size_t i = 0; i < state_interfaces_.size(); ++i) + { + interface_publisher_->msg_.states.interface_names.push_back(state_interfaces_.at(i).get_name()); // this can be done in a separate function one time. Change it later TODO (Sachin) : + interface_publisher_->msg_.states.values.at(i) = static_cast(state_interfaces_.at(i).get_value()); + } + + interface_publisher_->msg_.commands.interface_names.clear(); + interface_publisher_->msg_.commands.values.clear(); + interface_publisher_->msg_.commands.values.resize(command_interfaces_.size()); + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + interface_publisher_->msg_.commands.interface_names.push_back(command_interfaces_.at(i).get_name()); // this can be done in a separate function one time. Change it later TODO (Sachin) : + interface_publisher_->msg_.commands.values.at(i) = static_cast(command_interfaces_.at(i).get_value()); + } + interface_publisher_->unlockAndPublish(); + } +} + + + rclcpp_action::GoalResponse IOGripperController::handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + try + { + if (reconfigureFlag_.load()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); + return rclcpp_action::GoalResponse::REJECT; + } + service_buffer_.writeFromNonRT((goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse IOGripperController::handle_cancel( + const std::shared_ptr goal_handle) + { + (void)goal_handle; + service_buffer_.writeFromNonRT(service_mode_type::IDLE); + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + return rclcpp_action::CancelResponse::ACCEPT; + } + + void IOGripperController::handle_accepted(const std::shared_ptr goal_handle) + { + // don't think need to do anything here as it is handled in the update function + std::thread{std::bind(&IOGripperController::execute, this, std::placeholders::_1), goal_handle}.detach(); + + } + + void IOGripperController::execute(std::shared_ptr goal_handle) + { + auto result = std::make_shared(); + auto feedback = std::make_shared(); + while (true) + { + if (*(gripper_state_buffer_.readFromRT()) == gripper_state_type::IDLE) + { + result->success = true; + result->message = "Gripper action executed"; + goal_handle->succeed(result); + break; + } + else if (*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED) + { + result->success = false; + result->message = "Gripper action halted"; + goal_handle->abort(result); + break; + } + else + { + feedback->state = static_cast (*(gripper_state_buffer_.readFromRT())); + goal_handle->publish_feedback(feedback); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + + + rclcpp_action::GoalResponse IOGripperController::config_handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + try + { + std::string conf = goal->config_name; + configure_gripper_buffer_.writeFromNonRT(conf.c_str()); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); + reconfigureFlag_.store(true); + + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); + return rclcpp_action::GoalResponse::REJECT; + + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + + } + + + rclcpp_action::CancelResponse IOGripperController::config_handle_cancel( + const std::shared_ptr goal_handle) + { + (void)goal_handle; + configure_gripper_buffer_.writeFromNonRT(""); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); + return rclcpp_action::CancelResponse::ACCEPT; + + } + + void IOGripperController::config_handle_accepted(const std::shared_ptr goal_handle) + { + std::thread{std::bind(&IOGripperController::config_execute, this, std::placeholders::_1), goal_handle}.detach(); + } + void IOGripperController::config_execute(std::shared_ptr goal_handle) + { + auto result = std::make_shared(); + auto feedback = std::make_shared(); + // wait with thread sleep, until certain flag is not set + while (true) + { + if(*(reconfigure_state_buffer_.readFromRT()) == reconfigure_state_type::IDLE) + { + result->result = true; + result->status = "Gripper reconfigured"; + goal_handle->succeed(result); + break; + } + else + { + feedback->state = static_cast (*(reconfigure_state_buffer_.readFromRT())); + goal_handle->publish_feedback(feedback); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + } + + void IOGripperController::check_gripper_and_reconfigure_state() + { + // check the gripper state + bool gripper_state_found = false; + + for (size_t i = 0; i < state_ios_open.size(); ++i) + { + setResult = find_and_get_state(state_ios_open[i], state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); + } + else { + if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) + { + gripper_state_found = true; + } + else { + gripper_state_found = false; + } + } + } + + if (gripper_state_found) + { + for (size_t i = 0; i < params_.open.joint_states.size(); ++i) + { + joint_state_values_[i] = params_.open.joint_states[i]; + } + } + else + { + for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) + { + for (const auto & high_val : state_params.high) + { + setResult = find_and_get_state(high_val, state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); + } + else { + if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) + { + gripper_state_found = true; + } + else { + gripper_state_found = false; + break; + } + } + } + for (const auto & low_val : state_params.low) + { + setResult = find_and_get_state(low_val, state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); + } + else { + if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) + { + gripper_state_found = true; + } + else { + gripper_state_found = false; + break; + } + } + } + + if (gripper_state_found) + { + for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(state_name).joint_states.size(); ++i) + { + joint_state_values_[i] = params_.close.state.possible_closed_states_map.at(state_name).joint_states[i]; + } + break; + } + } + } + + // check the reconfigure state + bool reconfigure_state_found = false; + + for (const auto & [key, val] : params_.configuration_setup.configurations_map) + { + for (const auto & io : val.state_high) + { + // read the state of the gripper + setResult = find_and_get_state(io, state_value_); + // if the state is not as expected, then set the state to the expected state + if (!setResult) + { + reconfigure_state_found = false; + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) + { + reconfigure_state_found = false; + RCLCPP_ERROR( + get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + break; + } + else { + reconfigure_state_found = true; + } + } + // if the state is as expected, then do nothing + } + + for (const auto & io : val.state_low) + { + // read the state of the gripper + setResult = find_and_get_state(io, state_value_); + // if the state is not as expected, then set the state to the expected state + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) + { + RCLCPP_ERROR( + get_node()->get_logger(), "value doesn't match %s", io.c_str()); + reconfigure_state_found = false; + break; + } + else + { + reconfigure_state_found = true; + } + } + } + if (reconfigure_state_found) + { + for (size_t i = 0; i < val.joint_states.size(); ++i) + { + joint_state_values_[i + params_.open_close_joints.size()] = val.joint_states[i]; + } + break; + } + } + } + + +// void IOGripperController::init_msgs() +// { +// for (const auto & name : state_interfaces_) +// { +// interface_msg_.interface_names.push_back(name.get_name()); +// } +// interface_msg_.values.resize(state_interfaces_.size()); +// } + +} // namespace io_gripper_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + io_gripper_controller::IOGripperController, controller_interface::ControllerInterface) diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml new file mode 100644 index 0000000000..3af2f14f18 --- /dev/null +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -0,0 +1,258 @@ +# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# + +io_gripper_controller: + use_action: { + type: bool, + default_value: false, + description: "True for using action server and false service server for the gripper", + } + open_close_joints: { + type: string_array, + default_value: [], + description: "List of joint names that should change values according to open or close state of the gripper", + validation: { + unique<>: null, + } + } + open: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint values when gripper is open. The order of the values should match the order of the joint names in open_close_joints", + validation: { + unique<>: null, + } + } + set_before_command: + high: { + type: string_array, + default_value: [""], + description: "(optional) list of command interfaces that have to be set to high (1.0) before opening the gripper", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [""], + description: "(optional) list of command interfaces that have to be set to low (0.0) before opening the gripper", + validation: { + unique<>: null, + } + } + command: + high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) to open the gripper.", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of command interface names that have to be see to low (0.0) to open the gripper.", + validation: { + unique<>: null, + } + } + state: + high: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to equal high (1.0) to represent the gripper is open.", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to equal low (0.0) to represent the gripper is open.", + validation: { + unique<>: null, + } + } + set_after_command: + high: { + type: string_array, + default_value: [""], + description: "(optional) list of command interfaces that have to be set to high (1.0) after opening the gripper", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [""], + description: "(optional) list of command interface names that have to be set to low (0.0) after opening the gripper.", + validation: { + unique<>: null, + } + } + possible_closed_states: { + type: string_array, + default_value: [""], + description: "List of possible closed states e.g. empty_close and full close", + validation: { + unique<>: null, + } + } + close: + set_before_command: + high: { + type: string_array, + default_value: [""], + description: "(optional) list of command interfaces that have to be set to high (1.0) before closing the gripper", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [""], + description: "(optional) list of command interfaces that have to be set to low (0.0) before closing the gripper", + validation: { + unique<>: null, + } + } + command: + high: { + type: string_array, + default_value: [""], + description: "(optional) list of command interfaces that have to be set to high (1.0) to close the gripper", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [""], + description: "(optional) list of command interfaces that have to be set to low (0.0) to close the gripper", + validation: { + unique<>: null, + } + } + state: + __map_possible_closed_states: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint values when gripper is closed. The order of the values should match the order of the joint names in open_close_joints", + validation: { + unique<>: null, + } + } + high: { + type: string_array, + default_value: [""], + description: "(optional) list of state interfaces that have to equal high (1.0) to represent the gripper is closed", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [""], + description: "(optional) list of state interfaces that have to equal low (0.0) to represent the gripper is closed", + validation: { + unique<>: null, + } + } + set_after_command_high: { + type: string_array, + default_value: [""], + description: "(optional) list of command interfaces that have to be set to high (1.0) after closing the gripper", + validation: { + unique<>: null, + } + } + set_after_command_low: { + type: string_array, + default_value: [""], + description: "(optional) list of command interface names that have to be set to low (0.0) after closing the gripper.", + validation: { + unique<>: null, + } + } + configuration_joints: { + type: string_array, + default_value: [], + description: "List of joint names that are used to switch between different configurations of the gripper", + validation: { + unique<>: null, + } + } + + configurations: { + type: string_array, + default_value: [], + description: "Configuration names that can be used to switch between different configurations of the gripper", + validation: { + unique<>: null, + } + } + configuration_setup: + __map_configurations: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint states that open the gripper", + validation: { + unique<>: null, + } + } + command_high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) for this configuration.", + } + command_low: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to low (0.0) for this configuration.", + } + state_high: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to be high (1.0) to represent this configuration.", + } + state_low: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to be low (0.0) to represent this configuration.", + } + + gripper_specific_sensors: { + type: string_array, + default_value: [], + description: "List of sensor names that are specific to the gripper", + validation: { + unique<>: null, + } + } + sensors_interfaces: + __map_gripper_specific_sensors: + input: { + type: string, + default_value: "", + description: "Name of the input interface that is specific to the gripper", + } diff --git a/io_gripper_controller/test/test_io_gripper_controller.cpp b/io_gripper_controller/test/test_io_gripper_controller.cpp new file mode 100644 index 0000000000..0dd06d5950 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller.cpp @@ -0,0 +1,48 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "test_io_gripper_controller.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + + +// joint +// joint states +// dynamic state msg to status +// action server +// reconfire -> one or two configurations +// open gripper error when not expected behavior +// set_before and set_after commands +// add test for action and service open/close +// + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} + diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp new file mode 100644 index 0000000000..9f7a938fc7 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -0,0 +1,397 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_GRIPPER_IO_CONTROLLER_HPP_ +#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_GRIPPER_IO_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "io_gripper_controller/io_gripper_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerStateMsg = io_gripper_controller::IOGripperController::ControllerStateMsg; +using OpenSrvType = io_gripper_controller::IOGripperController::OpenSrvType; +using ControllerModeSrvType = io_gripper_controller::IOGripperController::ControllerModeSrvType; +using EventStateMsg = io_gripper_controller::IOGripperController::EventStateMsg; +using ConfigSrvType = control_msgs::srv::SetConfig; + +using GripperAction = io_gripper_controller::IOGripperController::GripperAction; +using GoalHandleGripperAction = rclcpp_action::ClientGoalHandle; +using GripperConfigAction = io_gripper_controller::IOGripperController::GripperConfigAction; +using GoalHandleGripperConfigAction = rclcpp_action::ClientGoalHandle; +using JointStateMsg = sensor_msgs::msg::JointState; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +} // namespace + +// subclassing and friending so we can access member variables +class TestableIOGripperController : public io_gripper_controller::IOGripperController +{ + FRIEND_TEST(IOGripperControllerTest, AllParamsSetSuccess); + FRIEND_TEST(IOGripperControllerTest, AllParamNotSetFailure); + FRIEND_TEST(IOGripperControllerTest, OpenGripperService); + FRIEND_TEST(IOGripperControllerTest, CloseGripperService); + FRIEND_TEST(IOGripperControllerTest, OpenCloseGripperAction); + FRIEND_TEST(IOGripperControllerTest, ReconfigureGripperService); + FRIEND_TEST(IOGripperControllerTest, ReconfigureGripperAction); + + FRIEND_TEST(IOGripperControllerTest, DefaultParametersNotSet); + FRIEND_TEST(IOGripperControllerTest, OpeningCommandParametersNotSet); + FRIEND_TEST(IOGripperControllerTest, ClosingCommandsParametersNotSet); + FRIEND_TEST(IOGripperControllerTest, DifferentCommandsParametersSet); + FRIEND_TEST(IOGripperControllerTest, OpenedStatesParametersNotSet); + FRIEND_TEST(IOGripperControllerTest, ClosedStatesParametersNotSet); + FRIEND_TEST(IOGripperControllerTest, DifferentStatesParametersNotSet); + FRIEND_TEST(IOGripperControllerTest, all_parameters_set_configure_success); + + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = io_gripper_controller::IOGripperController::on_configure(previous_state); + return ret; + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class IOGripperControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + + subscription_caller_node_ = std::make_shared("subscription_caller"); + joint_state_sub_ = subscription_caller_node_->create_subscription( + "/joint_states", 1, + [this](const JointStateMsg::SharedPtr msg) { + joint_state_sub_msg_ = msg; + RCLCPP_INFO(rclcpp::get_logger("test_io_gripper_controller"), "Received joint state"); + }); + + + service_caller_node_ = std::make_shared("service_caller"); + close_gripper_service_client_ = service_caller_node_->create_client( + "/test_io_gripper_controller/gripper_close"); + open_gripper_service_client_ = service_caller_node_->create_client( + "/test_io_gripper_controller/gripper_open"); + + configure_gripper_service_client_ = service_caller_node_->create_client( + "/test_io_gripper_controller/reconfigure_to"); + + // action client + action_caller_node_ = std::make_shared("action_caller"); + + gripper_action_client_ = rclcpp_action::create_client( + action_caller_node_, "/test_io_gripper_controller/gripper_action"); + + gripper_config_action_client_ = rclcpp_action::create_client( + action_caller_node_, "/test_io_gripper_controller/reconfigure_gripper_action"); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_io_gripper_controller") + { + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + // setting the command state interfaces manually + std::vector command_itfs; + command_itfs.reserve(3); // TODO (Sachin) : change this some variable later + + command_itfs.emplace_back(greif_oeffen_wqg1_cmd_); + command_itfs.emplace_back(greif_schliess_wqg2_cmd_); + command_itfs.emplace_back(bremse_wqg7_cmd_); + command_itfs.emplace_back(stich_125_wqg5_cmd_); + command_itfs.emplace_back(stich_250_wqg6_cmd_); + + std::vector state_itfs_; + state_itfs_.reserve(2); + + state_itfs_.emplace_back(greif_geoff_bg01_state_); + state_itfs_.emplace_back(greif_geschl_bg02_state_); + state_itfs_.emplace_back(stich_125_bg03_state_); + state_itfs_.emplace_back(stich_250_bg04_state_); + state_itfs_.emplace_back(bau_teil_abfrage_bg06_state_); + + controller_->assign_interfaces(std::move(command_itfs), std::move(state_itfs_)); + } + + std::shared_future>> callOpenGripperAction(rclcpp::Executor & executor) + { + auto goal = GripperAction::Goal(); + goal.open = true; + + bool wait_for_server_ret = gripper_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_server_ret); + if (!wait_for_server_ret) + { + throw std::runtime_error("Action server is not available!"); + } + + auto future = gripper_action_client_->async_send_goal(goal); + + return future; + + // goal->open = true; + // auto future = gripper_action_client_->async_send_goal(goal); + // EXPECT_EQ(executor.spin_until_future_complete(future), rclcpp::FutureReturnCode::SUCCESS); + } + + std::shared_ptr call_close_service(rclcpp::Executor & executor) + { + auto request = std::make_shared(); + + bool wait_for_service_ret = + close_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Services is not available!"); + } + auto result = close_gripper_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + + std::shared_ptr call_open_service(rclcpp::Executor & executor) + { + auto request = std::make_shared(); + + bool wait_for_service_ret = + open_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Services is not available!"); + } + auto result = open_gripper_service_client_->async_send_request(request); + EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + + return result.get(); + } + + void setup_parameters() + { + controller_->get_node()->set_parameter({"use_action", false}); + controller_->get_node()->set_parameter({"open_close_joints", open_close_joints}); + controller_->get_node()->set_parameter({"open.joint_states", open_joint_states}); + controller_->get_node()->set_parameter({"open.set_before_command.high", open_set_before_command_high}); + controller_->get_node()->set_parameter({"open.set_before_command.low", open_set_before_command_low}); + controller_->get_node()->set_parameter({"open.command.high", open_command_high}); + controller_->get_node()->set_parameter({"open.command.low", open_command_low}); + controller_->get_node()->set_parameter({"open.state.high", open_state_high}); + controller_->get_node()->set_parameter({"open.state.low", open_state_low}); + controller_->get_node()->set_parameter({"open.set_after_command.high", open_set_after_command_high}); + controller_->get_node()->set_parameter({"open.set_after_command.low", open_set_after_command_low}); + + controller_->get_node()->set_parameter({"possible_closed_states", possible_closed_states}); + controller_->get_node()->set_parameter({"close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter({"close.set_before_command.high", close_set_before_command_high}); + controller_->get_node()->set_parameter({"close.set_before_command.low", close_set_before_command_low}); + controller_->get_node()->set_parameter({"close.command.high", close_command_high}); + controller_->get_node()->set_parameter({"close.command.low", close_command_low}); + controller_->get_node()->set_parameter({"close.state.empty_close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter({"close.state.empty_close.high", close_state_high}); + controller_->get_node()->set_parameter({"close.state.empty_close.low", close_state_low}); + controller_->get_node()->set_parameter({"close.state.empty_close.set_after_command_high", close_set_after_command_high}); + controller_->get_node()->set_parameter({"close.state.empty_close.set_after_command_low", close_set_after_command_low}); + controller_->get_node()->set_parameter({"close.state.full_close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter({"close.state.full_close.high", close_state_low}); + controller_->get_node()->set_parameter({"close.state.full_close.low", close_state_high}); + controller_->get_node()->set_parameter({"close.state.full_close.set_after_command_high", close_set_after_command_high}); + controller_->get_node()->set_parameter({"close.state.full_close.set_after_command_low", close_set_after_command_low}); + + + controller_->get_node()->set_parameter({"configurations", configurations_list}); + controller_->get_node()->set_parameter({"configuration_joints", configuration_joints}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.joint_states", stichmass_joint_states}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.command_high", stichmass_command_high}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.command_low", stichmass_command_low}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.state_high", stichmass_state_high}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.state_low", stichmass_state_low}); + + controller_->get_node()->set_parameter({"gripper_specific_sensors", gripper_specific_sensors}); + controller_->get_node()->set_parameter({"sensors_interfaces.hohenabfrage.input", gripper_interfaces_input}); + } + + void setup_parameters_fail() + { + controller_->get_node()->set_parameter({"use_action", false}); + controller_->get_node()->set_parameter({"open_close_joints", ""}); + controller_->get_node()->set_parameter({"open.joint_states", open_joint_states}); + controller_->get_node()->set_parameter({"open.set_before_command.high", open_set_before_command_high}); + controller_->get_node()->set_parameter({"open.set_before_command.low", open_set_before_command_low}); + controller_->get_node()->set_parameter({"open.command.high", open_command_high}); + controller_->get_node()->set_parameter({"open.command.low", open_command_low}); + controller_->get_node()->set_parameter({"open.state.high", open_state_high}); + controller_->get_node()->set_parameter({"open.state.low", open_state_low}); + controller_->get_node()->set_parameter({"open.set_after_command.high", open_set_after_command_high}); + controller_->get_node()->set_parameter({"open.set_after_command.low", open_set_after_command_low}); + + controller_->get_node()->set_parameter({"possible_closed_states", possible_closed_states}); + controller_->get_node()->set_parameter({"close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter({"close.set_before_command.high", close_set_before_command_high}); + controller_->get_node()->set_parameter({"close.set_before_command.low", close_set_before_command_low}); + controller_->get_node()->set_parameter({"close.command.high", close_command_high}); + controller_->get_node()->set_parameter({"close.command.low", close_command_low}); + controller_->get_node()->set_parameter({"close.state.empty_close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter({"close.state.empty_close.high", close_state_high}); + controller_->get_node()->set_parameter({"close.state.empty_close.low", close_state_low}); + controller_->get_node()->set_parameter({"close.state.empty_close.set_after_command_high", close_set_after_command_high}); + controller_->get_node()->set_parameter({"close.state.empty_close.set_after_command_low", close_set_after_command_low}); + controller_->get_node()->set_parameter({"close.state.full_close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter({"close.state.full_close.high", close_state_low}); + controller_->get_node()->set_parameter({"close.state.full_close.low", close_state_high}); + controller_->get_node()->set_parameter({"close.state.full_close.set_after_command_high", close_set_after_command_high}); + controller_->get_node()->set_parameter({"close.state.full_close.set_after_command_low", close_set_after_command_low}); + + + controller_->get_node()->set_parameter({"configurations", configurations_list}); + controller_->get_node()->set_parameter({"configuration_joints", configuration_joints}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.joint_states", stichmass_joint_states}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.command_high", stichmass_command_high}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.command_low", stichmass_command_low}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.state_high", stichmass_state_high}); + controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.state_low", stichmass_state_low}); + + controller_->get_node()->set_parameter({"gripper_specific_sensors", gripper_specific_sensors}); + controller_->get_node()->set_parameter({"sensors_interfaces.hohenabfrage.input", gripper_interfaces_input}); + } + +protected: + // Controller-related parameters + std::vector open_close_joints = {"gripper_clamp_jaw"}; + + std::vector open_joint_states = {0.0}; + std::vector open_set_before_command_high = {"EL2008/Bremse_WQG7"}; + std::vector open_set_before_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; + std::vector open_set_after_command_high = {"EL2008/Bremse_WQG7"}; + std::vector open_set_after_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; + std::vector open_command_high = {"EL2008/Greiferteil_Oeffnen_WQG1"}; + std::vector open_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; + std::vector open_state_high = {"EL1008/Greifer_Geoeffnet_BG01"}; + std::vector open_state_low = {"EL1008/Greifer_Geschloschen_BG02"}; + + std::vector possible_closed_states = {"empty_close", "full_close"}; + std::vector close_joint_states = {0.08}; + std::vector close_set_before_command_high = {"EL2008/Bremse_WQG7"}; + std::vector close_set_before_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; + std::vector close_set_after_command_high = {"EL2008/Bremse_WQG7"}; + std::vector close_set_after_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; + std::vector close_command_high = {"EL2008/Greiferteil_Schliessen_WQG2"}; + std::vector close_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; + std::vector close_state_high = {"EL1008/Greifer_Geschloschen_BG02"}; + std::vector close_state_low = {"EL1008/Bauteilabfrage_BG06"}; + + std::vector configurations_list = {"stichmass_125"}; + std::vector configuration_joints = {"gripper_gripper_distance_joint"}; + + std::vector stichmass_joint_states = {0.125}; + std::vector stichmass_command_high = {"EL2008/Stichmass_125_WQG5"}; + std::vector stichmass_command_low = {"EL2008/Stichmass_250_WQG6"}; + std::vector stichmass_state_high = {"EL1008/Stichmass_125mm_BG03"}; + std::vector stichmass_state_low = {"EL1008/Stichmass_250mm_BG04"}; + + std::vector gripper_specific_sensors = {"hohenabfrage"}; + std::string gripper_interfaces_input = {"EL1008/Hohenabfrage_BG5"}; + + + + + + + std::vector joint_names_ = {"gripper_joint", "finger_joint"}; + std::vector state_joint_names_ = {"gripper_joint"}; + std::string interface_name_ = "gpio"; + double joint_value_opened_ = 75.0; + double joint_value_closed_ = 30.0; + std::array joint_command_values_ = {0.0, 0.0}; + std::array joint_state_values_ = {0.0}; + + std::array joint_command_opened = {1.0, 0.0}; + std::array joint_command_closed = {0.0, 0.0}; + + hardware_interface::CommandInterface joint_1_gpio_cmd_{joint_names_[0], interface_name_, &joint_command_values_[0]}; + hardware_interface::CommandInterface joint_2_gpio_cmd_{joint_names_[1], interface_name_, &joint_command_values_[1]}; + + std::array command_ios_values_ = {0.0, 1.0, 0.0, 0.0, 0.0}; + std::array state_ios_values_ = {1.0, 0.0, 1.0, 0.0, 1.0}; + + hardware_interface::CommandInterface greif_oeffen_wqg1_cmd_{"EL2008", "Greiferteil_Oeffnen_WQG1", &command_ios_values_[0]}; + hardware_interface::CommandInterface greif_schliess_wqg2_cmd_{"EL2008", "Greiferteil_Schliessen_WQG2", &command_ios_values_[1]}; + hardware_interface::CommandInterface bremse_wqg7_cmd_{"EL2008", "Bremse_WQG7", &command_ios_values_[2]}; + hardware_interface::CommandInterface stich_125_wqg5_cmd_{"EL2008", "Stichmass_125_WQG5", &command_ios_values_[3]}; + hardware_interface::CommandInterface stich_250_wqg6_cmd_{"EL2008", "Stichmass_250_WQG6", &command_ios_values_[4]}; + + hardware_interface::StateInterface greif_geoff_bg01_state_{"EL1008", "Greifer_Geoeffnet_BG01", &state_ios_values_[0]}; + hardware_interface::StateInterface greif_geschl_bg02_state_{"EL1008", "Greifer_Geschloschen_BG02", &state_ios_values_[1]}; + hardware_interface::StateInterface stich_125_bg03_state_{"EL1008", "Stichmass_125mm_BG03", &state_ios_values_[2]}; + hardware_interface::StateInterface stich_250_bg04_state_{"EL1008", "Stichmass_250mm_BG04", &state_ios_values_[3]}; + hardware_interface::StateInterface bau_teil_abfrage_bg06_state_{"EL1008", "Bauteilabfrage_BG06", &state_ios_values_[4]}; + + JointStateMsg::SharedPtr joint_state_sub_msg_ = std::make_shared(); + + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Subscription::SharedPtr joint_state_sub_; + rclcpp::Client::SharedPtr close_gripper_service_client_; + rclcpp::Client::SharedPtr open_gripper_service_client_; + rclcpp::Client::SharedPtr configure_gripper_service_client_; + rclcpp_action::Client::SharedPtr gripper_action_client_; + rclcpp_action::Client::SharedPtr gripper_config_action_client_; + rclcpp::Node::SharedPtr subscription_caller_node_, service_caller_node_, action_caller_node_; +}; + + +class IOGripperControllerTest : public IOGripperControllerFixture +{ +}; +#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_GRIPPER_IO_CONTROLLER_HPP_ diff --git a/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp new file mode 100644 index 0000000000..0d09d19c1d --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp @@ -0,0 +1,56 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "test_io_gripper_controller.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + + + +// Test setting all params and getting success +TEST_F(IOGripperControllerTest, AllParamsSetSuccess) +{ + SetUpController(); + + setup_parameters(); + + // configure success. + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} + + +// Test not setting the one param and getting failure +TEST_F(IOGripperControllerTest, AllParamNotSetFailure) +{ + SetUpController(); + + setup_parameters_fail(); + + // configure success. remaining parameters are default + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::FAILURE); +} \ No newline at end of file diff --git a/io_gripper_controller/test/test_io_gripper_controller_close.cpp b/io_gripper_controller/test/test_io_gripper_controller_close.cpp new file mode 100644 index 0000000000..e562608e29 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_close.cpp @@ -0,0 +1,125 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "test_io_gripper_controller.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, CloseGripperService) +{ + SetUpController(); + + setup_parameters(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(subscription_caller_node_->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto request = std::make_shared(); + + bool wait_for_service_ret = + close_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Services is not available!"); + } + auto future = close_gripper_service_client_->async_send_request(request); + + std::thread spinner([&executor, &future]() { + executor.spin_until_future_complete(future); + }); + spinner.detach(); + + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CLOSE_GRIPPER); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(future.get()->success, true); + // update to make sure the publisher value is updated + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + executor.spin_some(std::chrono::milliseconds(2000)); // this solve the issue related to subscriber not able to get the message + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = subscription_caller_node_->get_clock()->now() + timeout; + while (!joint_state_sub_msg_ && subscription_caller_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (subscription_caller_node_.get()) + { + break; + } + } + executor.spin_some(std::chrono::milliseconds(2000)); // this solve the issue related to subscriber not able to get the message + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(joint_state_sub_msg_); + + EXPECT_EQ(joint_state_sub_msg_->position.size(), 2); + EXPECT_EQ(joint_state_sub_msg_->position[0], 0.08); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); +} \ No newline at end of file diff --git a/io_gripper_controller/test/test_io_gripper_controller_open.cpp b/io_gripper_controller/test/test_io_gripper_controller_open.cpp new file mode 100644 index 0000000000..29ab00680e --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_open.cpp @@ -0,0 +1,119 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + + +#include "test_io_gripper_controller.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, OpenGripperService) +{ + SetUpController(); + + setup_parameters(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(subscription_caller_node_->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto request = std::make_shared(); + + bool wait_for_service_ret = + open_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Services is not available!"); + } + auto future = open_gripper_service_client_->async_send_request(request); + + std::thread spinner([&executor, &future]() { + executor.spin_until_future_complete(future); + }); + spinner.detach(); + + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::OPEN_GRIPPER); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(future.get()->success, true); + + + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = subscription_caller_node_->get_clock()->now() + timeout; + while (!joint_state_sub_msg_ && subscription_caller_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (joint_state_sub_msg_.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(joint_state_sub_msg_); + + ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); + ASSERT_EQ(joint_state_sub_msg_->position[0], 0.0); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); +} \ No newline at end of file diff --git a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp new file mode 100644 index 0000000000..f46bf9388d --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp @@ -0,0 +1,189 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "test_io_gripper_controller.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, OpenCloseGripperAction) +{ + SetUpController(); + + setup_parameters(); + + controller_->get_node()->set_parameter({"use_action", true}); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(subscription_caller_node_->get_node_base_interface()); + executor.add_node(action_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto goal = std::make_shared(); + + bool wait_for_action_ret = gripper_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_action_ret); + if (!wait_for_action_ret) + { + throw std::runtime_error("Action server is not available!"); + } + + goal->open = true; + + gripper_action_client_->async_send_goal(*goal); + + executor.spin_some(std::chrono::milliseconds(5000)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::OPEN_GRIPPER); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); + + // update to make sure the publisher value is updated + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = subscription_caller_node_->get_clock()->now() + timeout; + while (!joint_state_sub_msg_ && subscription_caller_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (max_sub_check_loop_count == 0) + { + break; + } + } + executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + ASSERT_TRUE(joint_state_sub_msg_); + + ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); + EXPECT_EQ(joint_state_sub_msg_->position[0], 0.0); + + // now sending action goal to close the gripper + goal->open = false; + + gripper_action_client_->async_send_goal(*goal); + + executor.spin_some(std::chrono::milliseconds(5000)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CLOSE_GRIPPER); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); + // update to make sure the publisher value is updated + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // since update doesn't guarantee a published message, republish until received + max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.1)); + const auto timeout = std::chrono::milliseconds{500}; + const auto until = subscription_caller_node_->get_clock()->now() + timeout; + while (!joint_state_sub_msg_ && subscription_caller_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (max_sub_check_loop_count == 0) + { + break; + } + } + executor.spin_some(std::chrono::milliseconds(2000)); // this solve the issue related to subscriber not able to get the message + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + ASSERT_TRUE(joint_state_sub_msg_); + + ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); + EXPECT_EQ(joint_state_sub_msg_->position[0], 0.08); +} \ No newline at end of file diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp new file mode 100644 index 0000000000..68532d1e1c --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp @@ -0,0 +1,112 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "test_io_gripper_controller.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, ReconfigureGripperService) +{ + SetUpController(); + setup_parameters(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(subscription_caller_node_->get_node_base_interface()); + executor.add_node(service_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto request = std::make_shared(); + request->config_name = "stichmass_125"; + + bool wait_for_service_ret = + configure_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_service_ret); + if (!wait_for_service_ret) + { + throw std::runtime_error("Services is not available!"); + } + auto future = configure_gripper_service_client_->async_send_request(request); + + std::thread spinner([&executor, &future]() { + executor.spin_until_future_complete(future); + }); + spinner.detach(); + + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::SET_COMMAND); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::CHECK_STATE); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::IDLE); + + ASSERT_EQ(future.get()->result, true); + + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = subscription_caller_node_->get_clock()->now() + timeout; + while (!joint_state_sub_msg_ && subscription_caller_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (joint_state_sub_msg_.get()) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + ASSERT_TRUE(joint_state_sub_msg_); + + ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); + ASSERT_EQ(joint_state_sub_msg_->position[1], 0.125); + +} \ No newline at end of file diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp new file mode 100644 index 0000000000..1ed413c535 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp @@ -0,0 +1,115 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +// +// Source of this file are templates in +// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +// + +#include "test_io_gripper_controller.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, ReconfigureGripperAction) +{ + SetUpController(); + + setup_parameters(); + + controller_->get_node()->set_parameter({"use_action", true}); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + executor.add_node(subscription_caller_node_->get_node_base_interface()); + executor.add_node(action_caller_node_->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto goal = std::make_shared(); + + bool wait_for_action_ret = gripper_config_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); + EXPECT_TRUE(wait_for_action_ret); + if (!wait_for_action_ret) + { + throw std::runtime_error("Action server is not available!"); + } + + goal->config_name = "stichmass_125"; + + + gripper_config_action_client_->async_send_goal(*goal); + + executor.spin_some(std::chrono::milliseconds(5000)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::SET_COMMAND); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::CHECK_STATE); + + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::IDLE); + + executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + const auto timeout = std::chrono::milliseconds{5}; + const auto until = subscription_caller_node_->get_clock()->now() + timeout; + while (!joint_state_sub_msg_ && subscription_caller_node_->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + // check if message has been received + if (max_sub_check_loop_count == 0) + { + break; + } + } + executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + // ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + // "controller/broadcaster update loop"; + ASSERT_TRUE(joint_state_sub_msg_); + + ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); + ASSERT_EQ(joint_state_sub_msg_->position[1], 0.125); +} \ No newline at end of file diff --git a/io_gripper_controller/test/test_load_io_gripper_controller.cpp b/io_gripper_controller/test/test_load_io_gripper_controller.cpp new file mode 100644 index 0000000000..d3ea9e8b1a --- /dev/null +++ b/io_gripper_controller/test/test_load_io_gripper_controller.cpp @@ -0,0 +1,41 @@ +// Copyright 2020 PAL Robotics SL. +// +// 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 "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadIOGripperController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_io_gripper_controller", "io_gripper_controller/IOGripperController"), + nullptr); + + rclcpp::shutdown(); +} \ No newline at end of file From 8010ca54966901f2ce74d6ab049cea56a11d0925 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Mon, 23 Dec 2024 15:13:57 +0100 Subject: [PATCH 02/75] cleaned the code to impove the readabiligy and consistency --- .../io_gripper_controller.hpp | 48 +--- .../src/io_gripper_controller.cpp | 243 ++++++------------ 2 files changed, 88 insertions(+), 203 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 01d1b1d197..b8e8ab674a 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -37,7 +37,6 @@ #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" -// TODO(anyone): Replace with controller specific messages #include #include "control_msgs/srv/set_config.hpp" @@ -51,13 +50,6 @@ namespace io_gripper_controller { -// TODO(anyone: example setup for control mode (usually you will use some enums defined in messages) -enum class control_mode_type : std::uint8_t -{ - FAST = 0, - SLOW = 1, -}; - enum class service_mode_type : std::uint8_t { IDLE = 0, @@ -65,29 +57,24 @@ enum class service_mode_type : std::uint8_t CLOSE = 2, }; -// TODO : rearrange it later enum class gripper_state_type : std::uint8_t { IDLE = 0, - STORE_ORIGINAL_STATE = 1, - SET_BEFORE_COMMAND = 2, - CLOSE_GRIPPER = 3, - CHECK_GRIPPER_STATE = 4, - RESTORE_ORIGINAL_STATE = 5, - CHECK_RESTORE_STATE = 6, - OPEN_GRIPPER = 7, - START_CLOSE_GRIPPER = 8, - SET_AFTER_COMMAND = 9, - HALTED = 10, + SET_BEFORE_COMMAND = 1, + CLOSE_GRIPPER = 2, + CHECK_GRIPPER_STATE = 3, + OPEN_GRIPPER = 4, + START_CLOSE_GRIPPER = 5, + SET_AFTER_COMMAND = 6, + HALTED = 7, }; enum class reconfigure_state_type : std::uint8_t { IDLE = 0, - RECONFIGURE = 1, - FIND_CONFIG = 2, - SET_COMMAND = 3, - CHECK_STATE = 4, + FIND_CONFIG = 1, + SET_COMMAND = 2, + CHECK_STATE = 3, }; class IOGripperController : public controller_interface::ControllerInterface @@ -153,7 +140,6 @@ class IOGripperController : public controller_interface::ControllerInterface protected: std::shared_ptr param_listener_; - rclcpp::Service::SharedPtr set_slow_control_mode_service_; rclcpp::Service::SharedPtr open_service_; rclcpp::Service::SharedPtr close_service_; rclcpp::Service::SharedPtr configure_gripper_service_; @@ -161,7 +147,6 @@ class IOGripperController : public controller_interface::ControllerInterface rclcpp_action::Server::SharedPtr gripper_action_server_; rclcpp_action::Server::SharedPtr gripper_config_action_server_; - realtime_tools::RealtimeBuffer control_mode_; realtime_tools::RealtimeBuffer service_buffer_; realtime_tools::RealtimeBuffer configure_gripper_buffer_; realtime_tools::RealtimeBuffer gripper_state_buffer_; @@ -186,13 +171,11 @@ class IOGripperController : public controller_interface::ControllerInterface std::unique_ptr event_publisher_; std::atomic reconfigureFlag_{false}, openFlag_{false}, closeFlag_{false}; - // std::atomic reconfigFlag{false}; private: bool find_and_set_command(const std::string & name, const double value); bool find_and_get_state(const std::string & name, double& value); bool find_and_get_command(const std::string & name, double& value); - void init_msgs(); void handle_gripper_state_transition_open(const gripper_state_type & state); void handle_gripper_state_transition_close(const gripper_state_type & state); void handle_reconfigure_state_transition(const reconfigure_state_type & state); @@ -219,14 +202,11 @@ class IOGripperController : public controller_interface::ControllerInterface rclcpp::CallbackGroup::SharedPtr open_service_callback_group_, close_service_callback_group_, reconfigure_service_callback_group_; - std::shared_ptr gripper_feedback_; std::shared_ptr gripper_result_; std::shared_ptr gripper_config_feedback_; std::shared_ptr gripper_config_result_; - - - + rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); @@ -247,12 +227,6 @@ class IOGripperController : public controller_interface::ControllerInterface void config_handle_accepted(const std::shared_ptr goal_handle); void config_execute(const std::shared_ptr goal_handle); - - - - - - }; } // namespace io_gripper_controller diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index f1a64d63cd..9e91c23e94 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -80,10 +80,6 @@ controller_interface::CallbackReturn IOGripperController::on_configure( return result; } - /** - * realtime publisher for the gripper_specific sensors, type publishing is boolean - */ - prepare_command_and_state_ios(); result = prepare_publishers_and_services(); @@ -137,7 +133,7 @@ controller_interface::CallbackReturn IOGripperController::on_deactivate( } controller_interface::return_type IOGripperController::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { if (reconfigureFlag_.load()) { @@ -145,7 +141,6 @@ controller_interface::return_type IOGripperController::update( handle_reconfigure_state_transition(*(reconfigure_state_buffer_.readFromRT())); } - auto current_command = service_buffer_.readFromRT(); switch (*(service_buffer_.readFromRT())) { case service_mode_type::IDLE: @@ -162,7 +157,6 @@ controller_interface::return_type IOGripperController::update( break; } - // this is publishing the joint states for the gripper and reconfigure publish_gripper_joint_states(); publish_dynamic_interface_values(); @@ -222,7 +216,6 @@ bool IOGripperController::find_and_get_command(const std::string & name, double& return false; } - void IOGripperController::handle_gripper_state_transition_close(const gripper_state_type & state) { switch (state) @@ -231,29 +224,28 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st // do nothing break; case gripper_state_type::SET_BEFORE_COMMAND: - // set before closeing - for (size_t i = 0; i < set_before_command_close.size(); ++i) - { - setResult = find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); - if (!setResult) + for (size_t i = 0; i < set_before_command_close.size(); ++i) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", set_before_command_close[i].c_str()); + setResult = find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", set_before_command_close[i].c_str()); + } } - } gripper_state_buffer_.writeFromNonRT(gripper_state_type::CLOSE_GRIPPER); break; case gripper_state_type::CLOSE_GRIPPER: for (size_t i = 0; i < command_ios_close.size(); ++i) - { - setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); - if (!setResult) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", command_ios_close[i].c_str()); + setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", command_ios_close[i].c_str()); + } } - } gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); break; @@ -342,7 +334,6 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st } } - void IOGripperController::handle_gripper_state_transition_open(const gripper_state_type & state) { switch (state) @@ -351,7 +342,6 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta // do nothing break; case gripper_state_type::SET_BEFORE_COMMAND: - // set before opening for (size_t i = 0; i < set_before_command_open.size(); ++i) { setResult = find_and_set_command(set_before_command_open[i], set_before_command_open_values[i]); @@ -406,7 +396,6 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta } break; case gripper_state_type::SET_AFTER_COMMAND: - // set after opening for (size_t i = 0; i < set_after_command_open.size(); ++i) { setResult = find_and_set_command(set_after_command_open[i], set_after_command_open_values[i]); @@ -437,7 +426,6 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ // do nothing break; case reconfigure_state_type::FIND_CONFIG: - // find the configuration config_index_ = std::find(configurations_list_.begin(), configurations_list_.end(), configuration_key_); if (config_index_ == configurations_list_.end()) { @@ -449,7 +437,6 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ conf_it_ = config_map_[std::distance(configurations_list_.begin(), config_index_)]; reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); } - break; case reconfigure_state_type::SET_COMMAND: @@ -477,12 +464,9 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ case reconfigure_state_type::CHECK_STATE: check_state_ios_ = false; - // implement the code read the state of the gripper for (const auto & io : conf_it_.state_high) { - // read the state of the gripper setResult = find_and_get_state(io, state_value_); - // if the state is not as expected, then set the state to the expected state if (!setResult) { check_state_ios_ = false; @@ -502,14 +486,11 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ check_state_ios_ = true; } } - // if the state is as expected, then do nothing } for (const auto & io : conf_it_.state_low) { - // read the state of the gripper setResult = find_and_get_state(io, state_value_); - // if the state is not as expected, then set the state to the expected state if (!setResult) { RCLCPP_ERROR( @@ -529,7 +510,6 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ check_state_ios_ = true; } } - // if the state is as expected, then do nothing } if (check_state_ios_) @@ -539,7 +519,7 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ joint_state_values_[i + params_.open_close_joints.size()] = conf_it_.joint_states[i]; } reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); - configure_gripper_buffer_.writeFromNonRT(""); // this is not required, remove later TODO (Sachin) :s + configure_gripper_buffer_.writeFromNonRT(""); reconfigureFlag_.store(false); } break; @@ -550,8 +530,6 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ controller_interface::CallbackReturn IOGripperController::check_parameters() { - /// Param validation - if (params_.open_close_joints.empty()) { RCLCPP_FATAL( @@ -560,19 +538,6 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() return CallbackReturn::FAILURE; } - - // size of open_close_joint should match with the open.joint_states and close.joint_states - // if (params_.open_close_joints.size() != params_.open.joint_states.size() && - // params_.open_close_joints.size() != params_.close.joint_states.size()) - // { - // RCLCPP_FATAL( - // get_node()->get_logger(), - // "Size of open close joints parameter should match with the open.joint_states and close.joint_states."); - // return CallbackReturn::FAILURE; - // } - - - if (params_.open.joint_states.empty()) { RCLCPP_FATAL( @@ -597,15 +562,6 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() return CallbackReturn::FAILURE; } - // close parameters - // if (params_.close.joint_states.empty()) - // { - // RCLCPP_FATAL( - // get_node()->get_logger(), - // "Size of joint states parameter cannot be zero."); - // return CallbackReturn::FAILURE; - // } - if (params_.close.set_before_command.high.empty() and params_.close.set_before_command.low.empty()) { RCLCPP_FATAL( @@ -614,7 +570,6 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() return CallbackReturn::FAILURE; } - if (params_.close.command.high.empty() and params_.close.command.low.empty()) { RCLCPP_FATAL( @@ -623,15 +578,6 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() return CallbackReturn::FAILURE; } - // if (params_.close.state.high.empty() and params_.close.state.low.empty()) - // { - // RCLCPP_FATAL( - // get_node()->get_logger(), - // "Size of close state high and low parameters cannot be zero."); - // return CallbackReturn::FAILURE; - // } - - // configurations parameter if (params_.configurations.empty()) { @@ -700,7 +646,6 @@ void IOGripperController::prepare_command_and_state_ios() set_before_command_open.push_back(key); set_before_command_open_values.push_back(1.0); command_if_ios.insert(key); - state_if_ios.insert(key); } } @@ -710,23 +655,24 @@ void IOGripperController::prepare_command_and_state_ios() set_before_command_open.push_back(key); set_before_command_open_values.push_back(0.0); command_if_ios.insert(key); - state_if_ios.insert(key); } } for (const auto& key : params_.open.command.high) { - command_ios_open.push_back(key); - command_ios_open_values.push_back(1.0); - // command_ios_open[key] = 1.0; - command_if_ios.insert(key); - state_if_ios.insert(key); + if (!key.empty()) + { + command_ios_open.push_back(key); + command_ios_open_values.push_back(1.0); + command_if_ios.insert(key); + } } for (const auto& key : params_.open.command.low) { - command_ios_open.push_back(key); - command_ios_open_values.push_back(0.0); - // command_ios_open[key] = 0.0; - command_if_ios.insert(key); - state_if_ios.insert(key); + if (!key.empty()) + { + command_ios_open.push_back(key); + command_ios_open_values.push_back(0.0); + command_if_ios.insert(key); + } } for (const auto& key : params_.open.set_after_command.high) { @@ -735,7 +681,6 @@ void IOGripperController::prepare_command_and_state_ios() set_after_command_open.push_back(key); set_after_command_open_values.push_back(1.0); command_if_ios.insert(key); - state_if_ios.insert(key); } } @@ -745,7 +690,6 @@ void IOGripperController::prepare_command_and_state_ios() set_after_command_open.push_back(key); set_after_command_open_values.push_back(0.0); command_if_ios.insert(key); - state_if_ios.insert(key); } } @@ -755,7 +699,6 @@ void IOGripperController::prepare_command_and_state_ios() set_before_command_close.push_back(key); set_before_command_close_values.push_back(1.0); command_if_ios.insert(key); - state_if_ios.insert(key); } } @@ -765,26 +708,24 @@ void IOGripperController::prepare_command_and_state_ios() set_before_command_close.push_back(key); set_before_command_close_values.push_back(0.0); command_if_ios.insert(key); - state_if_ios.insert(key); } } for (const auto& key : params_.close.command.high) { command_ios_close.push_back(key); command_ios_close_values.push_back(1.0); - // command_ios_close[key] = 1.0; command_if_ios.insert(key); - state_if_ios.insert(key); } + for (const auto& key : params_.close.command.low) { - command_ios_close.push_back(key); - command_ios_close_values.push_back(0.0); - // command_ios_close[key] = 0.0; - command_if_ios.insert(key); - state_if_ios.insert(key); + if(!key.empty()) + { + command_ios_close.push_back(key); + command_ios_close_values.push_back(0.0); + command_if_ios.insert(key); + } } - // make full state ios lists -- just once for (const auto& key : params_.open.state.high) { if(!key.empty()) @@ -794,6 +735,7 @@ void IOGripperController::prepare_command_and_state_ios() state_if_ios.insert(key); } } + for (const auto& key : params_.open.state.low) { if(!key.empty()) { @@ -802,43 +744,39 @@ void IOGripperController::prepare_command_and_state_ios() state_if_ios.insert(key); } } - // for (const auto& key : params_.close.state.high) { - // if(!key.empty()) - // { - // state_ios_close.push_back(key); - // state_ios_close_values.push_back(1.0); - // state_if_ios.insert(key); - // } - // } - // for (const auto& key : params_.close.state.low) { - // if(!key.empty()) - // { - // state_ios_close.push_back(key); - // state_ios_close_values.push_back(0.0); - // state_if_ios.insert(key); - // } - // } - - // for (const auto& key : params_.close.set_after_command.high) { - // if(!key.empty()) - // { - // set_after_command_close.push_back(key); - // set_after_command_close_values.push_back(1.0); - // command_if_ios.insert(key); - // state_if_ios.insert(key); - // } - // } - - // for (const auto& key : params_.close.set_after_command.low) { - // if(!key.empty()) - // { - // set_after_command_close.push_back(key); - // set_after_command_close_values.push_back(0.0); - // command_if_ios.insert(key); - // state_if_ios.insert(key); - // } - // } - + + for (const auto & [name, value] : params_.close.state.possible_closed_states_map) + { + for (const auto & key : value.high) + { + if(!key.empty()) + { + state_if_ios.insert(key); + } + } + for (const auto & key : value.low) + { + if(!key.empty()) + { + state_if_ios.insert(key); + } + } + for (const auto & key: value.set_after_command_high) + { + if(!key.empty()) + { + command_if_ios.insert(key); + } + } + for (const auto & key: value.set_after_command_low) + { + if(!key.empty()) + { + command_if_ios.insert(key); + } + } + } + // get the configurations for different io which needs to be high or low for (const auto & [key, val] : params_.configuration_setup.configurations_map) { @@ -870,14 +808,6 @@ void IOGripperController::prepare_command_and_state_ios() } } - // get the configurations for different io which needs to be high or low - for (const auto & [key, val] : params_.sensors_interfaces.gripper_specific_sensors_map) - { - sensors_map_.push_back(val); - } - - - // TODO (Sachin) : Add the gripper specific sensors to the state_if_ios, not able to access the values, discuss this with Dr. Denis for (size_t i = 0; i < params_.gripper_specific_sensors.size(); ++i) { state_if_ios.insert(params_.sensors_interfaces.gripper_specific_sensors_map.at(params_.gripper_specific_sensors[i]).input); @@ -996,7 +926,6 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and rmw_qos_profile_services_hist_keep_all, close_service_callback_group_); // configure gripper service - // TODO (Sachin) : Change service type to string auto configure_gripper_service_callback = [&]( const std::shared_ptr request, @@ -1008,7 +937,6 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and configure_gripper_buffer_.writeFromNonRT(conf.c_str()); reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); reconfigureFlag_.store(true); - // wait with thread sleep, until certain flag is not set while(reconfigureFlag_.load()) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -1055,11 +983,13 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and g_j_s_publisher_ = get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); gripper_joint_state_publisher_ = std::make_unique(g_j_s_publisher_); + + auto final_joint_size = params_.open_close_joints.size() + params_.configuration_joints.size(); - gripper_joint_state_publisher_->msg_.name.resize(params_.open_close_joints.size() + params_.configuration_joints.size()); - gripper_joint_state_publisher_->msg_.position.resize(params_.open_close_joints.size() + params_.configuration_joints.size()); + gripper_joint_state_publisher_->msg_.name.resize(final_joint_size); + gripper_joint_state_publisher_->msg_.position.resize(final_joint_size); - joint_state_values_.resize(params_.open_close_joints.size() + params_.configuration_joints.size(), std::numeric_limits::quiet_NaN()); + joint_state_values_.resize(final_joint_size, std::numeric_limits::quiet_NaN()); for (size_t i = 0; i < params_.open_close_joints.size(); ++i) { @@ -1160,6 +1090,7 @@ void IOGripperController::publish_dynamic_interface_values() RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); return rclcpp_action::GoalResponse::REJECT; } + (void)uuid; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -1174,7 +1105,6 @@ void IOGripperController::publish_dynamic_interface_values() void IOGripperController::handle_accepted(const std::shared_ptr goal_handle) { - // don't think need to do anything here as it is handled in the update function std::thread{std::bind(&IOGripperController::execute, this, std::placeholders::_1), goal_handle}.detach(); } @@ -1208,7 +1138,6 @@ void IOGripperController::publish_dynamic_interface_values() } } - rclcpp_action::GoalResponse IOGripperController::config_handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) @@ -1227,8 +1156,8 @@ void IOGripperController::publish_dynamic_interface_values() return rclcpp_action::GoalResponse::REJECT; } + (void)uuid; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } @@ -1246,11 +1175,11 @@ void IOGripperController::publish_dynamic_interface_values() { std::thread{std::bind(&IOGripperController::config_execute, this, std::placeholders::_1), goal_handle}.detach(); } + void IOGripperController::config_execute(std::shared_ptr goal_handle) { auto result = std::make_shared(); auto feedback = std::make_shared(); - // wait with thread sleep, until certain flag is not set while (true) { if(*(reconfigure_state_buffer_.readFromRT()) == reconfigure_state_type::IDLE) @@ -1272,7 +1201,6 @@ void IOGripperController::publish_dynamic_interface_values() void IOGripperController::check_gripper_and_reconfigure_state() { - // check the gripper state bool gripper_state_found = false; for (size_t i = 0; i < state_ios_open.size(); ++i) @@ -1353,18 +1281,15 @@ void IOGripperController::publish_dynamic_interface_values() break; } } - } + } - // check the reconfigure state bool reconfigure_state_found = false; for (const auto & [key, val] : params_.configuration_setup.configurations_map) { for (const auto & io : val.state_high) { - // read the state of the gripper setResult = find_and_get_state(io, state_value_); - // if the state is not as expected, then set the state to the expected state if (!setResult) { reconfigure_state_found = false; @@ -1384,14 +1309,11 @@ void IOGripperController::publish_dynamic_interface_values() reconfigure_state_found = true; } } - // if the state is as expected, then do nothing } for (const auto & io : val.state_low) { - // read the state of the gripper setResult = find_and_get_state(io, state_value_); - // if the state is not as expected, then set the state to the expected state if (!setResult) { RCLCPP_ERROR( @@ -1422,17 +1344,6 @@ void IOGripperController::publish_dynamic_interface_values() } } } - - -// void IOGripperController::init_msgs() -// { -// for (const auto & name : state_interfaces_) -// { -// interface_msg_.interface_names.push_back(name.get_name()); -// } -// interface_msg_.values.resize(state_interfaces_.size()); -// } - } // namespace io_gripper_controller #include "pluginlib/class_list_macros.hpp" From 3f6151cd42372b7d909031996cd721af086e024f Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Mon, 23 Dec 2024 15:21:39 +0100 Subject: [PATCH 03/75] using result from set_value - minor code clean --- .../src/io_gripper_controller.cpp | 392 +++++++++--------- 1 file changed, 194 insertions(+), 198 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 9e91c23e94..19196dfc93 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -174,8 +174,7 @@ bool IOGripperController::find_and_set_command(const std::string & name, const d if (it != command_interfaces_.end()) { - it->set_value(value); - return true; + return it->set_value(value); } return false; } @@ -816,7 +815,6 @@ void IOGripperController::prepare_command_and_state_ios() controller_interface::CallbackReturn IOGripperController::prepare_publishers_and_services() { - reconfigureFlag_.store(false); // reset service buffer @@ -1070,280 +1068,278 @@ void IOGripperController::publish_dynamic_interface_values() } } - - rclcpp_action::GoalResponse IOGripperController::handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) +rclcpp_action::GoalResponse IOGripperController::handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + try { - try + if (reconfigureFlag_.load()) { - if (reconfigureFlag_.load()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); - return rclcpp_action::GoalResponse::REJECT; - } - service_buffer_.writeFromNonRT((goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); - } - catch (const std::exception & e) - { - RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); + RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); return rclcpp_action::GoalResponse::REJECT; } - (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + service_buffer_.writeFromNonRT((goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); } - - rclcpp_action::CancelResponse IOGripperController::handle_cancel( - const std::shared_ptr goal_handle) + catch (const std::exception & e) { - (void)goal_handle; - service_buffer_.writeFromNonRT(service_mode_type::IDLE); - gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); - return rclcpp_action::CancelResponse::ACCEPT; + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); + return rclcpp_action::GoalResponse::REJECT; } + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - void IOGripperController::handle_accepted(const std::shared_ptr goal_handle) +rclcpp_action::CancelResponse IOGripperController::handle_cancel( + const std::shared_ptr goal_handle) { - std::thread{std::bind(&IOGripperController::execute, this, std::placeholders::_1), goal_handle}.detach(); - + (void)goal_handle; + service_buffer_.writeFromNonRT(service_mode_type::IDLE); + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + return rclcpp_action::CancelResponse::ACCEPT; } - void IOGripperController::execute(std::shared_ptr goal_handle) +void IOGripperController::handle_accepted(const std::shared_ptr goal_handle) +{ + std::thread{std::bind(&IOGripperController::execute, this, std::placeholders::_1), goal_handle}.detach(); + +} + +void IOGripperController::execute(std::shared_ptr goal_handle) +{ + auto result = std::make_shared(); + auto feedback = std::make_shared(); + while (true) { - auto result = std::make_shared(); - auto feedback = std::make_shared(); - while (true) + if (*(gripper_state_buffer_.readFromRT()) == gripper_state_type::IDLE) { - if (*(gripper_state_buffer_.readFromRT()) == gripper_state_type::IDLE) - { - result->success = true; - result->message = "Gripper action executed"; - goal_handle->succeed(result); - break; - } - else if (*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED) - { - result->success = false; - result->message = "Gripper action halted"; - goal_handle->abort(result); - break; - } - else - { - feedback->state = static_cast (*(gripper_state_buffer_.readFromRT())); - goal_handle->publish_feedback(feedback); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + result->success = true; + result->message = "Gripper action executed"; + goal_handle->succeed(result); + break; } - } - - rclcpp_action::GoalResponse IOGripperController::config_handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - try + else if (*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED) { - std::string conf = goal->config_name; - configure_gripper_buffer_.writeFromNonRT(conf.c_str()); - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); - reconfigureFlag_.store(true); - + result->success = false; + result->message = "Gripper action halted"; + goal_handle->abort(result); + break; } - catch (const std::exception & e) + else { - RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); - return rclcpp_action::GoalResponse::REJECT; - - } - (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + feedback->state = static_cast (*(gripper_state_buffer_.readFromRT())); + goal_handle->publish_feedback(feedback); } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } +} - - rclcpp_action::CancelResponse IOGripperController::config_handle_cancel( - const std::shared_ptr goal_handle) - { - (void)goal_handle; - configure_gripper_buffer_.writeFromNonRT(""); +rclcpp_action::GoalResponse IOGripperController::config_handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) + { + try + { + std::string conf = goal->config_name; + configure_gripper_buffer_.writeFromNonRT(conf.c_str()); reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); - return rclcpp_action::CancelResponse::ACCEPT; - - } + reconfigureFlag_.store(true); + + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); + return rclcpp_action::GoalResponse::REJECT; + + } + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } - void IOGripperController::config_handle_accepted(const std::shared_ptr goal_handle) +rclcpp_action::CancelResponse IOGripperController::config_handle_cancel( + const std::shared_ptr goal_handle) { - std::thread{std::bind(&IOGripperController::config_execute, this, std::placeholders::_1), goal_handle}.detach(); + (void)goal_handle; + configure_gripper_buffer_.writeFromNonRT(""); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); + return rclcpp_action::CancelResponse::ACCEPT; + } - void IOGripperController::config_execute(std::shared_ptr goal_handle) +void IOGripperController::config_handle_accepted(const std::shared_ptr goal_handle) +{ +std::thread{std::bind(&IOGripperController::config_execute, this, std::placeholders::_1), goal_handle}.detach(); +} + +void IOGripperController::config_execute(std::shared_ptr goal_handle) +{ + auto result = std::make_shared(); + auto feedback = std::make_shared(); + while (true) { - auto result = std::make_shared(); - auto feedback = std::make_shared(); - while (true) + if(*(reconfigure_state_buffer_.readFromRT()) == reconfigure_state_type::IDLE) { - if(*(reconfigure_state_buffer_.readFromRT()) == reconfigure_state_type::IDLE) - { - result->result = true; - result->status = "Gripper reconfigured"; - goal_handle->succeed(result); - break; - } - else - { - feedback->state = static_cast (*(reconfigure_state_buffer_.readFromRT())); - goal_handle->publish_feedback(feedback); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + result->result = true; + result->status = "Gripper reconfigured"; + goal_handle->succeed(result); + break; } - + else + { + feedback->state = static_cast (*(reconfigure_state_buffer_.readFromRT())); + goal_handle->publish_feedback(feedback); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - void IOGripperController::check_gripper_and_reconfigure_state() - { - bool gripper_state_found = false; +} - for (size_t i = 0; i < state_ios_open.size(); ++i) +void IOGripperController::check_gripper_and_reconfigure_state() +{ + bool gripper_state_found = false; + + for (size_t i = 0; i < state_ios_open.size(); ++i) + { + setResult = find_and_get_state(state_ios_open[i], state_value_); + if (!setResult) { - setResult = find_and_get_state(state_ios_open[i], state_value_); - if (!setResult) + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); + } + else { + if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); + gripper_state_found = true; } else { - if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) - { - gripper_state_found = true; - } - else { - gripper_state_found = false; - } + gripper_state_found = false; } } + } - if (gripper_state_found) + if (gripper_state_found) + { + for (size_t i = 0; i < params_.open.joint_states.size(); ++i) { - for (size_t i = 0; i < params_.open.joint_states.size(); ++i) - { - joint_state_values_[i] = params_.open.joint_states[i]; - } + joint_state_values_[i] = params_.open.joint_states[i]; } - else + } + else + { + for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) { - for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) + for (const auto & high_val : state_params.high) { - for (const auto & high_val : state_params.high) + setResult = find_and_get_state(high_val, state_value_); + if (!setResult) { - setResult = find_and_get_state(high_val, state_value_); - if (!setResult) + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); + } + else { + if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); + gripper_state_found = true; } else { - if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) - { - gripper_state_found = true; - } - else { - gripper_state_found = false; - break; - } + gripper_state_found = false; + break; } } - for (const auto & low_val : state_params.low) + } + for (const auto & low_val : state_params.low) + { + setResult = find_and_get_state(low_val, state_value_); + if (!setResult) { - setResult = find_and_get_state(low_val, state_value_); - if (!setResult) + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); + } + else { + if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); + gripper_state_found = true; } else { - if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) - { - gripper_state_found = true; - } - else { - gripper_state_found = false; - break; - } + gripper_state_found = false; + break; } } + } - if (gripper_state_found) + if (gripper_state_found) + { + for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(state_name).joint_states.size(); ++i) { - for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(state_name).joint_states.size(); ++i) - { - joint_state_values_[i] = params_.close.state.possible_closed_states_map.at(state_name).joint_states[i]; - } - break; + joint_state_values_[i] = params_.close.state.possible_closed_states_map.at(state_name).joint_states[i]; } + break; } } + } - bool reconfigure_state_found = false; + bool reconfigure_state_found = false; - for (const auto & [key, val] : params_.configuration_setup.configurations_map) + for (const auto & [key, val] : params_.configuration_setup.configurations_map) + { + for (const auto & io : val.state_high) { - for (const auto & io : val.state_high) + setResult = find_and_get_state(io, state_value_); + if (!setResult) { - setResult = find_and_get_state(io, state_value_); - if (!setResult) + reconfigure_state_found = false; + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) { reconfigure_state_found = false; RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + break; } - else - { - if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) - { - reconfigure_state_found = false; - RCLCPP_ERROR( - get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); - break; - } - else { - reconfigure_state_found = true; - } + else { + reconfigure_state_found = true; } } + } - for (const auto & io : val.state_low) + for (const auto & io : val.state_low) + { + setResult = find_and_get_state(io, state_value_); + if (!setResult) { - setResult = find_and_get_state(io, state_value_); - if (!setResult) + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + get_node()->get_logger(), "value doesn't match %s", io.c_str()); + reconfigure_state_found = false; + break; } - else - { - if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) - { - RCLCPP_ERROR( - get_node()->get_logger(), "value doesn't match %s", io.c_str()); - reconfigure_state_found = false; - break; - } - else - { - reconfigure_state_found = true; - } + else + { + reconfigure_state_found = true; } } - if (reconfigure_state_found) + } + if (reconfigure_state_found) + { + for (size_t i = 0; i < val.joint_states.size(); ++i) { - for (size_t i = 0; i < val.joint_states.size(); ++i) - { - joint_state_values_[i + params_.open_close_joints.size()] = val.joint_states[i]; - } - break; + joint_state_values_[i + params_.open_close_joints.size()] = val.joint_states[i]; } + break; } } +} } // namespace io_gripper_controller #include "pluginlib/class_list_macros.hpp" From 55e523728545fb3fb834e9e4581142255c5a097b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 2 Jan 2025 12:59:55 +0100 Subject: [PATCH 04/75] Delete io_gripper_controller/include/io_gripper_controller/visibility_control.h --- .../visibility_control.h | 54 ------------------- 1 file changed, 54 deletions(-) delete mode 100644 io_gripper_controller/include/io_gripper_controller/visibility_control.h diff --git a/io_gripper_controller/include/io_gripper_controller/visibility_control.h b/io_gripper_controller/include/io_gripper_controller/visibility_control.h deleted file mode 100644 index 199423764a..0000000000 --- a/io_gripper_controller/include/io_gripper_controller/visibility_control.h +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. - -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - -#ifndef GRIPPER_IO_CONTROLLER__VISIBILITY_CONTROL_H_ -#define GRIPPER_IO_CONTROLLER__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 GRIPPER_IO_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define GRIPPER_IO_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define GRIPPER_IO_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define GRIPPER_IO_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef GRIPPER_IO_CONTROLLER__VISIBILITY_BUILDING_DLL -#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC GRIPPER_IO_CONTROLLER__VISIBILITY_EXPORT -#else -#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC GRIPPER_IO_CONTROLLER__VISIBILITY_IMPORT -#endif -#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC_TYPE GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC -#define GRIPPER_IO_CONTROLLER__VISIBILITY_LOCAL -#else -#define GRIPPER_IO_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) -#define GRIPPER_IO_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) -#define GRIPPER_IO_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) -#else -#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC -#define GRIPPER_IO_CONTROLLER__VISIBILITY_LOCAL -#endif -#define GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // GRIPPER_IO_CONTROLLER__VISIBILITY_CONTROL_H_ From 4f6de013d34189b2c1031c8beb86000603c362cf Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 2 Jan 2025 13:00:23 +0100 Subject: [PATCH 05/75] Delete io_gripper_controller/doc/.gitkeep --- io_gripper_controller/doc/.gitkeep | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 io_gripper_controller/doc/.gitkeep diff --git a/io_gripper_controller/doc/.gitkeep b/io_gripper_controller/doc/.gitkeep deleted file mode 100644 index e69de29bb2..0000000000 From 0338ff06565eba5c5412c8219a8abab536ec5afd Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:06:06 +0100 Subject: [PATCH 06/75] - removed visibility macros and used solution S1 for visibility macros - removed the template from license - added one variable per line - documented the enums - updated the doc folder as per `joint_trajectory_controller` --- io_gripper_controller/CMakeLists.txt | 4 + io_gripper_controller/README.md | 10 +- io_gripper_controller/doc/userdoc.rst | 99 ++++++++++++++++ .../io_gripper_controller.hpp | 110 ++++++++++-------- .../src/io_gripper_controller.cpp | 2 +- .../test/test_io_gripper_controller.cpp | 2 +- .../test/test_io_gripper_controller.hpp | 2 +- ...st_io_gripper_controller_all_param_set.cpp | 2 +- .../test/test_io_gripper_controller_close.cpp | 2 +- .../test/test_io_gripper_controller_open.cpp | 2 +- ...o_gripper_controller_open_close_action.cpp | 2 +- ...test_io_gripper_controller_reconfigure.cpp | 2 +- ..._gripper_controller_reconfigure_action.cpp | 2 +- 13 files changed, 174 insertions(+), 67 deletions(-) create mode 100644 io_gripper_controller/doc/userdoc.rst diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt index 7bcae6a24e..ac778f9ab6 100644 --- a/io_gripper_controller/CMakeLists.txt +++ b/io_gripper_controller/CMakeLists.txt @@ -5,6 +5,10 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() +# using this instead of visibility macros +# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053 +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS sensor_msgs diff --git a/io_gripper_controller/README.md b/io_gripper_controller/README.md index acc005853d..d2daf2148c 100644 --- a/io_gripper_controller/README.md +++ b/io_gripper_controller/README.md @@ -1,9 +1,5 @@ -# IO Gripper Controller +# io_gripper_controller package -The IO Gripper Controller is provides implementation to control the gripper using IOs. It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes `joint_states` of gripper and also `dynamic_interfaces` for all command and state interfaces. - -## Description of controller's interfaces - -- `joint_states` [`sensor_msgs::msg::JointState`]: Publishes the state of gripper joint and configuration joint -- `dynamic_interfaces` [`control_msgs::msg::DynamicInterfaceValues`]: Publishes all command and state interface of the IOs and sensors of gripper. +The package implements controllers to control the gripper using IOs. +For detailed documentation check the `docs` folder or [ros2_control documentation](https://control.ros.org/). diff --git a/io_gripper_controller/doc/userdoc.rst b/io_gripper_controller/doc/userdoc.rst new file mode 100644 index 0000000000..288e2f9993 --- /dev/null +++ b/io_gripper_controller/doc/userdoc.rst @@ -0,0 +1,99 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/io_gripper_controller/doc/userdoc.rst + +.. _io_gripper_controller_userdoc: + +io_gripper_controller +============================= + +The IO Gripper Controller provides implementation to control the gripper using IOs. It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes `joint_states` of gripper and also `dynamic_interfaces` for all command and state interfaces. + +Description of controller's interfaces +--------------------------------------- + +- `joint_states` [`sensor_msgs::msg::JointState`]: Publishes the state of gripper joint and configuration joint +- `dynamic_interfaces` [`control_msgs::msg::DynamicInterfaceValues`]: Publishes all command and state interface of the IOs and sensors of gripper. + + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +This controller adds the following parameters: + +.. generate_parameter_library_details:: ../src/io_gripper_controller.yaml + + +Example parameters +.................... + +``` +io_gripper_controller: + + ros__parameters: + + use_action: true + + open_close_joints: [gripper_clamp_jaw] + + open: + joint_states: [0.0] + set_before_command: + high: [EL2008/Bremse_WQG7] + low: [] + command: + high: [EL2008/Greiferteil_Oeffnen_WQG1] + low: [EL2008/Greiferteil_Schliessen_WQG2] + state: + high: [EL1008/Greifer_Geoeffnet_BG01] + low: [EL1008/Greifer_Geschloschen_BG02] + set_after_command: + high: [] + low: [EL2008/Bremse_WQG7] + + possible_closed_states: ['empty_close', 'full_close'] + + close: + set_before_command: + high: [EL2008/Bremse_WQG7] + low: [EL2008/Greiferteil_Oeffnen_WQG1] + command: + high: [EL2008/Greiferteil_Schliessen_WQG2] + low: [EL2008/Greiferteil_Oeffnen_WQG1] + state: + empty_close: + joint_states: [0.08] + high: [EL1008/Greifer_Geschloschen_BG02] + low: [EL1008/Bauteilabfrage_BG06] + set_after_command_high: [EL2008/Bremse_WQG7] + set_after_command_low: [EL2008/Bremse_WQG7] + full_close: + joint_states: [0.08] + high: [EL1008/Bauteilabfrage_BG06] + low: [EL1008/Greifer_Geschloschen_BG02] + set_after_command_high: [EL2008/Bremse_WQG7] + set_after_command_low: [EL2008/Bremse_WQG7] + + configurations: ["stichmass_125", "stichmass_250"] + configuration_joints: [gripper_gripper_distance_joint] + + configuration_setup: + stichmass_125: + joint_states: [0.125] + command_high: [EL2008/Stichmass_125_WQG5] + command_low: [EL2008/Stichmass_250_WQG6] + state_high: [EL1008/Stichmass_125mm_BG03] + state_low: [EL1008/Stichmass_250mm_BG04] + stichmass_250: + joint_states: [0.250] + command_high: [EL2008/Stichmass_250_WQG6] + command_low: [EL2008/Stichmass_125_WQG5] + state_high: [EL1008/Stichmass_250mm_BG04] + state_low: [EL1008/Stichmass_125mm_BG03] + + gripper_specific_sensors: ["hohenabfrage", "bauteilabfrage"] + sensors_interfaces: + hohenabfrage: + input: "EL1008/Hohenabfrage_BG5" + bauteilabfrage: + input: "EL1008/Bauteilabfrage_BG06" \ No newline at end of file diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index b8e8ab674a..aba6acee9f 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,11 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - #ifndef GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ #define GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ @@ -29,7 +24,6 @@ #include "controller_interface/controller_interface.hpp" #include "io_gripper_controller_parameters.hpp" -#include "io_gripper_controller/visibility_control.h" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" @@ -50,6 +44,14 @@ namespace io_gripper_controller { +/** + * @enum service_mode_type + * @brief Represents the various service modes of the gripper. These modes represents the high level states of the gripper. + * + * - IDLE: The gripper is in an idle state, not performing any action. + * - OPEN: The gripper is in the process of opening. + * - CLOSE: The gripper is in the process of closing. + */ enum class service_mode_type : std::uint8_t { IDLE = 0, @@ -57,6 +59,18 @@ enum class service_mode_type : std::uint8_t CLOSE = 2, }; +/** + * @enum gripper_state_type + * @brief Represents the various states of the gripper. + * + * - IDLE: The gripper is in an idle state, not performing any action. + * - SET_BEFORE_COMMAND: Executing commands for io defined in the yaml which are required before opening the gripper. + * - CLOSE_GRIPPER: Executing commands to close the gripper. + * - CHECK_GRIPPER_STATE: Checking the state of the gripper to make sure the gripper is closed. + * - OPEN_GRIPPER: Executing commands to open the gripper. + * - SET_AFTER_COMMAND: Setting the gripper state after executing a command. + * - HALTED: The gripper operation is halted. + */ enum class gripper_state_type : std::uint8_t { IDLE = 0, @@ -64,11 +78,19 @@ enum class gripper_state_type : std::uint8_t CLOSE_GRIPPER = 2, CHECK_GRIPPER_STATE = 3, OPEN_GRIPPER = 4, - START_CLOSE_GRIPPER = 5, - SET_AFTER_COMMAND = 6, - HALTED = 7, + SET_AFTER_COMMAND = 5, + HALTED = 6, }; +/** + * @enum reconfigure_state_type + * @brief Represents the various states of the reconfiguration process, which means that the gripper is reconfiguring to new state based on the configuration defined in the yaml params. + * + * - IDLE: The reconfiguration process is idle, not performing any action. + * - FIND_CONFIG: Finding the configuration settings. + * - SET_COMMAND: Setting the command based on the configuration. + * - CHECK_STATE: Checking the state after setting the command. + */ enum class reconfigure_state_type : std::uint8_t { IDLE = 0, @@ -80,51 +102,56 @@ enum class reconfigure_state_type : std::uint8_t class IOGripperController : public controller_interface::ControllerInterface { public: - GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC IOGripperController(); io_gripper_controller::Params params_; - GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; - GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; - GRIPPER_IO_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - std::vector command_ios_open, command_ios_close, set_before_command_open, set_after_command_open, reconfigure_command; - std::vector command_ios_open_values, command_ios_close_values, set_before_command_open_values, set_after_command_open_values, reconfigure_command_values; - std::vector state_ios_open, state_ios_close, set_before_command_close, set_after_command_close; - std::vector state_ios_open_values, state_ios_close_values, set_before_command_close_values, set_after_command_close_values, set_after_command_open_values_original_; + std::vector command_ios_open; + std::vector command_ios_close; + std::vector set_before_command_open; + std::vector set_after_command_open; + std::vector reconfigure_command; + std::vector command_ios_open_values; + std::vector command_ios_close_values; + std::vector set_before_command_open_values; + std::vector set_after_command_open_values; + std::vector reconfigure_command_values; + std::vector state_ios_open; + std::vector state_ios_close; + std::vector set_before_command_close; + std::vector set_after_command_close; + std::vector state_ios_open_values; + std::vector state_ios_close_values; + std::vector set_before_command_close_values; + std::vector set_after_command_close_values; + std::vector set_after_command_open_values_original_; std::string status_joint_name; bool is_open; std::unordered_map command_if_ios_after_opening; std::unordered_map original_ios_after_opening; std::unordered_map command_if_ios_before_closing; std::unordered_map original_ios_before_closing; - - std::unordered_set command_if_ios, state_if_ios; - + std::unordered_set command_if_ios; + std::unordered_set state_if_ios; bool setResult; - using ControllerModeSrvType = std_srvs::srv::SetBool; using OpenSrvType = std_srvs::srv::Trigger; using ConfigSrvType = control_msgs::srv::SetConfig; @@ -139,38 +166,29 @@ class IOGripperController : public controller_interface::ControllerInterface protected: std::shared_ptr param_listener_; - rclcpp::Service::SharedPtr open_service_; rclcpp::Service::SharedPtr close_service_; rclcpp::Service::SharedPtr configure_gripper_service_; - rclcpp_action::Server::SharedPtr gripper_action_server_; rclcpp_action::Server::SharedPtr gripper_config_action_server_; - realtime_tools::RealtimeBuffer service_buffer_; realtime_tools::RealtimeBuffer configure_gripper_buffer_; realtime_tools::RealtimeBuffer gripper_state_buffer_; realtime_tools::RealtimeBuffer reconfigure_state_buffer_; - using ControllerStatePublisher = realtime_tools::RealtimePublisher; using EventPublisher = realtime_tools::RealtimePublisher; - using ConfigPublisher = realtime_tools::RealtimePublisher; using InterfacePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr g_j_s_publisher_; std::unique_ptr gripper_joint_state_publisher_; - std::vector joint_state_values_; - rclcpp::Publisher::SharedPtr if_publisher_; std::unique_ptr interface_publisher_; - - rclcpp::Publisher::SharedPtr e_publisher_; std::unique_ptr event_publisher_; - - std::atomic reconfigureFlag_{false}, openFlag_{false}, closeFlag_{false}; + std::atomic reconfigureFlag_{false}; + std::atomic openFlag_{false}; + std::atomic closeFlag_{false}; private: bool find_and_set_command(const std::string & name, const double value); @@ -179,16 +197,13 @@ class IOGripperController : public controller_interface::ControllerInterface void handle_gripper_state_transition_open(const gripper_state_type & state); void handle_gripper_state_transition_close(const gripper_state_type & state); void handle_reconfigure_state_transition(const reconfigure_state_type & state); - /// \brief Function to check the parameters controller_interface::CallbackReturn check_parameters(); - /// Preparing the command ios and states ios vars for the command/state interface configuraiton void prepare_command_and_state_ios(); controller_interface::CallbackReturn prepare_publishers_and_services(); void publish_gripper_joint_states(); void publish_dynamic_interface_values(); void publish_reconfigure_gripper_joint_states(); void check_gripper_and_reconfigure_state(); - std::vector configurations_list_; std::vector config_map_; std::vector sensors_map_; @@ -199,32 +214,25 @@ class IOGripperController : public controller_interface::ControllerInterface io_gripper_controller::Params::Close::State::MapPossibleClosedStates closed_state_values_; io_gripper_controller::Params::ConfigurationSetup::MapConfigurations conf_it_; std::vector::iterator config_index_; - - rclcpp::CallbackGroup::SharedPtr open_service_callback_group_, close_service_callback_group_, reconfigure_service_callback_group_; - + rclcpp::CallbackGroup::SharedPtr open_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr close_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr reconfigure_service_callback_group_; std::shared_ptr gripper_feedback_; std::shared_ptr gripper_result_; std::shared_ptr gripper_config_feedback_; std::shared_ptr gripper_config_result_; - rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr goal_handle); - void handle_accepted(const std::shared_ptr goal_handle); void execute(const std::shared_ptr goal_handle); - rclcpp_action::GoalResponse config_handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - - rclcpp_action::CancelResponse config_handle_cancel( const std::shared_ptr goal_handle); - void config_handle_accepted(const std::shared_ptr goal_handle); void config_execute(const std::shared_ptr goal_handle); }; diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 19196dfc93..649638e286 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/io_gripper_controller/test/test_io_gripper_controller.cpp b/io_gripper_controller/test/test_io_gripper_controller.cpp index 0dd06d5950..89aafa421c 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index 9f7a938fc7..1c2a707ecd 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp index 0d09d19c1d..f2f90d7711 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/io_gripper_controller/test/test_io_gripper_controller_close.cpp b/io_gripper_controller/test/test_io_gripper_controller_close.cpp index e562608e29..3aa0bba009 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_close.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_close.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/io_gripper_controller/test/test_io_gripper_controller_open.cpp b/io_gripper_controller/test/test_io_gripper_controller_open.cpp index 29ab00680e..cadb1dea0e 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp index f46bf9388d..b5001335d7 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp index 68532d1e1c..41c410225c 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp index 1ed413c535..94a7c10966 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From e85cc1ee113af547cdf7fc7986e51fdf242b6785 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:11:37 +0100 Subject: [PATCH 07/75] Update io_gripper_controller/CMakeLists.txt Co-authored-by: Dr. Denis --- io_gripper_controller/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt index ac778f9ab6..5110c1c4f3 100644 --- a/io_gripper_controller/CMakeLists.txt +++ b/io_gripper_controller/CMakeLists.txt @@ -32,7 +32,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -# Add io_gripper_controller library related compile commands generate_parameter_library(io_gripper_controller_parameters src/io_gripper_controller.yaml ) From 5e99ee569fe1e438ca097c61ac062f311636b956 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:12:16 +0100 Subject: [PATCH 08/75] Update io_gripper_controller/CMakeLists.txt Co-authored-by: Dr. Denis --- io_gripper_controller/CMakeLists.txt | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt index 5110c1c4f3..f2da06ec34 100644 --- a/io_gripper_controller/CMakeLists.txt +++ b/io_gripper_controller/CMakeLists.txt @@ -74,7 +74,15 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock(test_io_gripper_controller test/test_io_gripper_controller.cpp test/test_io_gripper_controller_open.cpp test/test_io_gripper_controller_close.cpp test/test_io_gripper_controller_all_param_set.cpp test/test_io_gripper_controller_open_close_action.cpp test/test_io_gripper_controller_reconfigure.cpp test/test_io_gripper_controller_reconfigure_action.cpp) + ament_add_gmock(test_io_gripper_controller + test/test_io_gripper_controller.cpp + test/test_io_gripper_controller_open.cpp + test/test_io_gripper_controller_close.cpp + test/test_io_gripper_controller_all_param_set.cpp + test/test_io_gripper_controller_open_close_action.cpp + test/test_io_gripper_controller_reconfigure.cpp + test/test_io_gripper_controller_reconfigure_action.cpp + ) target_include_directories(test_io_gripper_controller PRIVATE include) target_link_libraries(test_io_gripper_controller io_gripper_controller) ament_target_dependencies( From cd6f8cd8bd097fcd67152848be786b24b7c5f488 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:14:00 +0100 Subject: [PATCH 09/75] removed interface package deps Co-authored-by: Dr. Denis --- io_gripper_controller/package.xml | 4 ---- 1 file changed, 4 deletions(-) diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml index 7d00ccbac0..a8c8e51ad9 100644 --- a/io_gripper_controller/package.xml +++ b/io_gripper_controller/package.xml @@ -22,10 +22,6 @@ realtime_tools std_srvs - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - ament_cmake_gmock ament_lint_auto ament_lint_common From 6e31a330d5cd66be6f0ec2d5ead4db869fcb1b91 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:21:06 +0100 Subject: [PATCH 10/75] removed license from xml Co-authored-by: Dr. Denis --- .../io_gripper_controller.xml | 20 ------------------- 1 file changed, 20 deletions(-) diff --git a/io_gripper_controller/io_gripper_controller.xml b/io_gripper_controller/io_gripper_controller.xml index 660677e458..dcb5f07b18 100644 --- a/io_gripper_controller/io_gripper_controller.xml +++ b/io_gripper_controller/io_gripper_controller.xml @@ -1,23 +1,3 @@ - - From e5af267bd57cb989777fb669f34ce82e2a632d0a Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:18:12 +0100 Subject: [PATCH 11/75] deps changed alphabetically --- io_gripper_controller/CMakeLists.txt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt index f2da06ec34..b7e050733e 100644 --- a/io_gripper_controller/CMakeLists.txt +++ b/io_gripper_controller/CMakeLists.txt @@ -11,21 +11,21 @@ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) # find dependencies set(THIS_PACKAGE_INCLUDE_DEPENDS - sensor_msgs - controller_manager - ros2_control_test_assets + ament_cmake + ament_cmake_gmock + control_msgs controller_interface + controller_manager + generate_parameter_library hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools - std_srvs - ament_cmake - generate_parameter_library - ament_cmake_gmock + ros2_control_test_assets + sensor_msgs std_msgs - control_msgs + std_srvs ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) From fa22df49ecd9fe8df26e5fcc164838230abd1fcd Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:18:38 +0100 Subject: [PATCH 12/75] author and maintainer list updated --- io_gripper_controller/package.xml | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml index a8c8e51ad9..32cee71b04 100644 --- a/io_gripper_controller/package.xml +++ b/io_gripper_controller/package.xml @@ -4,8 +4,14 @@ io_gripper_controller 0.0.0 gripper io controller used to control the gripper using IOs - Yara Shahin - Sachin Kumar + Yara Shahin + Sachin Kumar + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + Apache License 2.0 ament_cmake From 1d61e0426a1acfaf6561ba3fc5d8c8fc13904ae8 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:28:18 +0100 Subject: [PATCH 13/75] doc strings with descript of inputs and outputs updated --- .../io_gripper_controller.hpp | 151 ++++++++++++++++++ 1 file changed, 151 insertions(+) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index aba6acee9f..764950d728 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -100,26 +100,65 @@ enum class reconfigure_state_type : std::uint8_t }; class IOGripperController : public controller_interface::ControllerInterface +/** + * @brief IOGripperController class handles the control of an IO-based gripper. + */ { public: + /** + * @brief Constructor for IOGripperController. + */ IOGripperController(); io_gripper_controller::Params params_; + /** + * @brief Initializes the controller. + * @return CallbackReturn indicating success or failure. + */ controller_interface::CallbackReturn on_init() override; + /** + * @brief Configures the command interfaces. + * @return InterfaceConfiguration for command interfaces. + */ controller_interface::InterfaceConfiguration command_interface_configuration() const override; + /** + * @brief Configures the state interfaces. + * @return InterfaceConfiguration for state interfaces. + */ controller_interface::InterfaceConfiguration state_interface_configuration() const override; + /** + * @brief Configures the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; + /** + * @brief Activates the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ controller_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; + /** + * @brief Deactivates the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ controller_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state) override; + /** + * @brief Updates the controller state. + * @param time The current time. + * @param period The time since the last update. + * @return return_type indicating success or failure. + */ controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; @@ -191,19 +230,85 @@ class IOGripperController : public controller_interface::ControllerInterface std::atomic closeFlag_{false}; private: + /** + * @brief Finds and sets a command value. + * @param name The name of the command. + * @param value The value to set. + * @return True if the command was found and set, false otherwise. + */ bool find_and_set_command(const std::string & name, const double value); + + /** + * @brief Finds and gets a state value. + * @param name The name of the state. + * @param value The value to get. + * @return True if the state was found and retrieved, false otherwise. + */ bool find_and_get_state(const std::string & name, double& value); + + /** + * @brief Finds and gets a command value. + * @param name The name of the command. + * @param value The value to get. + * @return True if the command was found and retrieved, false otherwise. + */ bool find_and_get_command(const std::string & name, double& value); + + /** + * @brief Handles the state transition when opening the gripper. + * @param state The current state of the gripper. + */ void handle_gripper_state_transition_open(const gripper_state_type & state); + + /** + * @brief Handles the state transition when closing the gripper. + * @param state The current state of the gripper. + */ void handle_gripper_state_transition_close(const gripper_state_type & state); + + /** + * @brief Handles the state transition for reconfiguration. + * @param state The current reconfiguration state. + */ void handle_reconfigure_state_transition(const reconfigure_state_type & state); + + /** + * @brief Checks the parameters of the controller. + * @return CallbackReturn indicating success or failure. + */ controller_interface::CallbackReturn check_parameters(); + + /** + * @brief Prepares the command and state IOs. + */ void prepare_command_and_state_ios(); + + /** + * @brief Prepares the publishers and services. + * @return CallbackReturn indicating success or failure. + */ controller_interface::CallbackReturn prepare_publishers_and_services(); + + /** + * @brief Publishes the gripper joint states. + */ void publish_gripper_joint_states(); + + /** + * @brief Publishes the dynamic interface values. + */ void publish_dynamic_interface_values(); + + /** + * @brief Publishes the reconfigure gripper joint states. + */ void publish_reconfigure_gripper_joint_states(); + + /** + * @brief Checks the gripper and reconfigure state. + */ void check_gripper_and_reconfigure_state(); + std::vector configurations_list_; std::vector config_map_; std::vector sensors_map_; @@ -221,19 +326,65 @@ class IOGripperController : public controller_interface::ControllerInterface std::shared_ptr gripper_result_; std::shared_ptr gripper_config_feedback_; std::shared_ptr gripper_config_result_; + + /** + * @brief Handles the goal for the gripper action. + * @param uuid The UUID of the goal. + * @param goal The goal to handle. + * @return GoalResponse indicating acceptance or rejection of the goal. + */ rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + /** + * @brief Handles the cancellation of the gripper action. + * @param goal_handle The handle of the goal to cancel. + * @return CancelResponse indicating acceptance or rejection of the cancellation. + */ rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr goal_handle); + + /** + * @brief Handles the acceptance of the gripper action. + * @param goal_handle The handle of the accepted goal. + */ void handle_accepted(const std::shared_ptr goal_handle); + + /** + * @brief Executes the gripper action. + * @param goal_handle The handle of the goal to execute. + */ void execute(const std::shared_ptr goal_handle); + + /** + * @brief Handles the goal for the gripper configuration action. + * @param uuid The UUID of the goal. + * @param goal The goal to handle. + * @return GoalResponse indicating acceptance or rejection of the goal. + */ rclcpp_action::GoalResponse config_handle_goal( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + /** + * @brief Handles the cancellation of the gripper configuration action. + * @param goal_handle The handle of the goal to cancel. + * @return CancelResponse indicating acceptance or rejection of the cancellation. + */ rclcpp_action::CancelResponse config_handle_cancel( const std::shared_ptr goal_handle); + + /** + * @brief Handles the acceptance of the gripper configuration action. + * @param goal_handle The handle of the accepted goal. + */ void config_handle_accepted(const std::shared_ptr goal_handle); + + /** + * @brief Executes the gripper configuration action. + * @param goal_handle The handle of the goal to execute. + */ void config_execute(const std::shared_ptr goal_handle); }; From aa1b523232e1fcc2fb5ffebb42494faa23d7e7b1 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:30:31 +0100 Subject: [PATCH 14/75] pkg deps aranged alphabatically --- io_gripper_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml index 32cee71b04..be9d59e9f3 100644 --- a/io_gripper_controller/package.xml +++ b/io_gripper_controller/package.xml @@ -19,8 +19,8 @@ generate_parameter_library control_msgs - controller_manager controller_interface + controller_manager hardware_interface pluginlib rclcpp From 8de0e37eab659170315e0fa32133f992de3051c1 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:36:44 +0100 Subject: [PATCH 15/75] deps updated as per cmakelist --- io_gripper_controller/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml index be9d59e9f3..799fd7cb3b 100644 --- a/io_gripper_controller/package.xml +++ b/io_gripper_controller/package.xml @@ -21,11 +21,15 @@ control_msgs controller_interface controller_manager + generate_parameter_library hardware_interface pluginlib rclcpp rclcpp_lifecycle realtime_tools + ros2_control_test_assets + sensor_msgs + std_msgs std_srvs ament_cmake_gmock From c2295921fcbe057632c9bd55612840f00426c4a2 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:43:21 +0100 Subject: [PATCH 16/75] OpenSrvType changed to OpenCloseSrvType Co-authored-by: Dr. Denis --- .../include/io_gripper_controller/io_gripper_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 764950d728..e4aa04dcbb 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -192,7 +192,7 @@ class IOGripperController : public controller_interface::ControllerInterface bool setResult; using ControllerModeSrvType = std_srvs::srv::SetBool; - using OpenSrvType = std_srvs::srv::Trigger; + using OpenCloseSrvType = std_srvs::srv::Trigger; using ConfigSrvType = control_msgs::srv::SetConfig; using ControllerStateMsg = sensor_msgs::msg::JointState; using EventStateMsg = sensor_msgs::msg::JointState; From 3f719fe360c17b8809e540a8fbecbc4f976ddc94 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:51:09 +0100 Subject: [PATCH 17/75] opensrvtype changed to openclosesrvtype --- .../io_gripper_controller.hpp | 4 ++-- io_gripper_controller/package.xml | 1 - .../src/io_gripper_controller.cpp | 12 ++++++------ .../test/test_io_gripper_controller.hpp | 18 +++++++++--------- .../test/test_io_gripper_controller_close.cpp | 2 +- .../test/test_io_gripper_controller_open.cpp | 2 +- 6 files changed, 19 insertions(+), 20 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index e4aa04dcbb..17a9c15e7f 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -205,8 +205,8 @@ class IOGripperController : public controller_interface::ControllerInterface protected: std::shared_ptr param_listener_; - rclcpp::Service::SharedPtr open_service_; - rclcpp::Service::SharedPtr close_service_; + rclcpp::Service::SharedPtr open_service_; + rclcpp::Service::SharedPtr close_service_; rclcpp::Service::SharedPtr configure_gripper_service_; rclcpp_action::Server::SharedPtr gripper_action_server_; rclcpp_action::Server::SharedPtr gripper_config_action_server_; diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml index 799fd7cb3b..39a5b3167e 100644 --- a/io_gripper_controller/package.xml +++ b/io_gripper_controller/package.xml @@ -21,7 +21,6 @@ control_msgs controller_interface controller_manager - generate_parameter_library hardware_interface pluginlib rclcpp diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 649638e286..2d7c042e20 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -838,8 +838,8 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and // open service auto open_service_callback = [&]( - const std::shared_ptr /*request*/, - std::shared_ptr response) + const std::shared_ptr /*request*/, + std::shared_ptr response) { try { @@ -875,15 +875,15 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and } }; - open_service_ = get_node()->create_service( + open_service_ = get_node()->create_service( "~/gripper_open", open_service_callback, rmw_qos_profile_services_hist_keep_all, open_service_callback_group_); // close service auto close_service_callback = [&]( - const std::shared_ptr /*request*/, - std::shared_ptr response) + const std::shared_ptr /*request*/, + std::shared_ptr response) { try { @@ -919,7 +919,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and } }; - close_service_ = get_node()->create_service( + close_service_ = get_node()->create_service( "~/gripper_close", close_service_callback, rmw_qos_profile_services_hist_keep_all, close_service_callback_group_); diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index 1c2a707ecd..4022c43361 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -41,7 +41,7 @@ // TODO(anyone): replace the state and command message types using ControllerStateMsg = io_gripper_controller::IOGripperController::ControllerStateMsg; -using OpenSrvType = io_gripper_controller::IOGripperController::OpenSrvType; +using OpenCloseSrvType = io_gripper_controller::IOGripperController::OpenCloseSrvType; using ControllerModeSrvType = io_gripper_controller::IOGripperController::ControllerModeSrvType; using EventStateMsg = io_gripper_controller::IOGripperController::EventStateMsg; using ConfigSrvType = control_msgs::srv::SetConfig; @@ -111,9 +111,9 @@ class IOGripperControllerFixture : public ::testing::Test service_caller_node_ = std::make_shared("service_caller"); - close_gripper_service_client_ = service_caller_node_->create_client( + close_gripper_service_client_ = service_caller_node_->create_client( "/test_io_gripper_controller/gripper_close"); - open_gripper_service_client_ = service_caller_node_->create_client( + open_gripper_service_client_ = service_caller_node_->create_client( "/test_io_gripper_controller/gripper_open"); configure_gripper_service_client_ = service_caller_node_->create_client( @@ -183,9 +183,9 @@ class IOGripperControllerFixture : public ::testing::Test // EXPECT_EQ(executor.spin_until_future_complete(future), rclcpp::FutureReturnCode::SUCCESS); } - std::shared_ptr call_close_service(rclcpp::Executor & executor) + std::shared_ptr call_close_service(rclcpp::Executor & executor) { - auto request = std::make_shared(); + auto request = std::make_shared(); bool wait_for_service_ret = close_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); @@ -200,9 +200,9 @@ class IOGripperControllerFixture : public ::testing::Test return result.get(); } - std::shared_ptr call_open_service(rclcpp::Executor & executor) + std::shared_ptr call_open_service(rclcpp::Executor & executor) { - auto request = std::make_shared(); + auto request = std::make_shared(); bool wait_for_service_ret = open_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); @@ -382,8 +382,8 @@ class IOGripperControllerFixture : public ::testing::Test // Test related parameters std::unique_ptr controller_; rclcpp::Subscription::SharedPtr joint_state_sub_; - rclcpp::Client::SharedPtr close_gripper_service_client_; - rclcpp::Client::SharedPtr open_gripper_service_client_; + rclcpp::Client::SharedPtr close_gripper_service_client_; + rclcpp::Client::SharedPtr open_gripper_service_client_; rclcpp::Client::SharedPtr configure_gripper_service_client_; rclcpp_action::Client::SharedPtr gripper_action_client_; rclcpp_action::Client::SharedPtr gripper_config_action_client_; diff --git a/io_gripper_controller/test/test_io_gripper_controller_close.cpp b/io_gripper_controller/test/test_io_gripper_controller_close.cpp index 3aa0bba009..68dd1de35e 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_close.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_close.cpp @@ -42,7 +42,7 @@ TEST_F(IOGripperControllerTest, CloseGripperService) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto request = std::make_shared(); + auto request = std::make_shared(); bool wait_for_service_ret = close_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); diff --git a/io_gripper_controller/test/test_io_gripper_controller_open.cpp b/io_gripper_controller/test/test_io_gripper_controller_open.cpp index cadb1dea0e..8878093b1e 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open.cpp @@ -43,7 +43,7 @@ TEST_F(IOGripperControllerTest, OpenGripperService) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); - auto request = std::make_shared(); + auto request = std::make_shared(); bool wait_for_service_ret = open_gripper_service_client_->wait_for_service(std::chrono::milliseconds(500)); From b4c7852fc5d9bc49edbda86495c6c88950b4bc5a Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:52:29 +0100 Subject: [PATCH 18/75] controllerStateMsg changed to jointStateMsg Co-authored-by: Dr. Denis --- .../include/io_gripper_controller/io_gripper_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 17a9c15e7f..d9e6fb2be7 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -194,7 +194,7 @@ class IOGripperController : public controller_interface::ControllerInterface using ControllerModeSrvType = std_srvs::srv::SetBool; using OpenCloseSrvType = std_srvs::srv::Trigger; using ConfigSrvType = control_msgs::srv::SetConfig; - using ControllerStateMsg = sensor_msgs::msg::JointState; + using JointStateMsg = sensor_msgs::msg::JointState; using EventStateMsg = sensor_msgs::msg::JointState; using ConfigJointMsg = sensor_msgs::msg::JointState; using InterfaceMsg = control_msgs::msg::DynamicInterfaceValues; From 73267ac2f622fa7e2413d2b90a97796ae922c8fb Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:56:00 +0100 Subject: [PATCH 19/75] controllerStateMsg changed to jointStateMsg for other files --- .../include/io_gripper_controller/io_gripper_controller.hpp | 4 ++-- io_gripper_controller/src/io_gripper_controller.cpp | 2 +- io_gripper_controller/test/test_io_gripper_controller.hpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index d9e6fb2be7..403b0e52c8 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -214,11 +214,11 @@ class IOGripperController : public controller_interface::ControllerInterface realtime_tools::RealtimeBuffer configure_gripper_buffer_; realtime_tools::RealtimeBuffer gripper_state_buffer_; realtime_tools::RealtimeBuffer reconfigure_state_buffer_; - using ControllerStatePublisher = realtime_tools::RealtimePublisher; + using ControllerStatePublisher = realtime_tools::RealtimePublisher; using EventPublisher = realtime_tools::RealtimePublisher; using ConfigPublisher = realtime_tools::RealtimePublisher; using InterfacePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr g_j_s_publisher_; + rclcpp::Publisher::SharedPtr g_j_s_publisher_; std::unique_ptr gripper_joint_state_publisher_; std::vector joint_state_values_; rclcpp::Publisher::SharedPtr if_publisher_; diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 2d7c042e20..c156f2142e 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -979,7 +979,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and { // Gripper Joint State publisher g_j_s_publisher_ = - get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); + get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); gripper_joint_state_publisher_ = std::make_unique(g_j_s_publisher_); auto final_joint_size = params_.open_close_joints.size() + params_.configuration_joints.size(); diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index 4022c43361..4db6470e9e 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -40,7 +40,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" // TODO(anyone): replace the state and command message types -using ControllerStateMsg = io_gripper_controller::IOGripperController::ControllerStateMsg; +using JointStateMsg = io_gripper_controller::IOGripperController::JointStateMsg; using OpenCloseSrvType = io_gripper_controller::IOGripperController::OpenCloseSrvType; using ControllerModeSrvType = io_gripper_controller::IOGripperController::ControllerModeSrvType; using EventStateMsg = io_gripper_controller::IOGripperController::EventStateMsg; From a5a6e3a57f2c7132df9cde9c9164737bfdbd265d Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:57:30 +0100 Subject: [PATCH 20/75] InterfaceMsg renamed to DynInterfaceMsg Co-authored-by: Dr. Denis --- .../include/io_gripper_controller/io_gripper_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 403b0e52c8..ea327b2c49 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -197,7 +197,7 @@ class IOGripperController : public controller_interface::ControllerInterface using JointStateMsg = sensor_msgs::msg::JointState; using EventStateMsg = sensor_msgs::msg::JointState; using ConfigJointMsg = sensor_msgs::msg::JointState; - using InterfaceMsg = control_msgs::msg::DynamicInterfaceValues; + using DynInterfaceMsg = control_msgs::msg::DynamicInterfaceValues; using GripperAction = control_msgs::action::Gripper; using GoalHandleGripper = rclcpp_action::ServerGoalHandle; using GripperConfigAction = control_msgs::action::SetGripperConfig; From 8f0de5c837f59f2d4975a6514eae0da3ccf85048 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 14:03:19 +0100 Subject: [PATCH 21/75] InterfaceMsg renamed to DynInterfaceMsg for all files --- .../include/io_gripper_controller/io_gripper_controller.hpp | 4 ++-- io_gripper_controller/src/io_gripper_controller.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index ea327b2c49..9f5fed85f6 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -217,11 +217,11 @@ class IOGripperController : public controller_interface::ControllerInterface using ControllerStatePublisher = realtime_tools::RealtimePublisher; using EventPublisher = realtime_tools::RealtimePublisher; using ConfigPublisher = realtime_tools::RealtimePublisher; - using InterfacePublisher = realtime_tools::RealtimePublisher; + using InterfacePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr g_j_s_publisher_; std::unique_ptr gripper_joint_state_publisher_; std::vector joint_state_values_; - rclcpp::Publisher::SharedPtr if_publisher_; + rclcpp::Publisher::SharedPtr if_publisher_; std::unique_ptr interface_publisher_; rclcpp::Publisher::SharedPtr e_publisher_; std::unique_ptr event_publisher_; diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index c156f2142e..0922089b32 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -1011,7 +1011,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and try { // interface publisher - if_publisher_ = get_node()->create_publisher( + if_publisher_ = get_node()->create_publisher( "~/dynamic_interface", rclcpp::SystemDefaultsQoS()); interface_publisher_ = std::make_unique(if_publisher_); } From f1350dd9df560350453f9253abe05c1c91a55c42 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 22:47:01 +0100 Subject: [PATCH 22/75] interface named changes as per control_msgs --- .../io_gripper_controller.hpp | 21 +++++++++---------- .../test/test_io_gripper_controller.hpp | 2 +- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 9f5fed85f6..eff2a9180b 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -33,12 +33,11 @@ #include -#include "control_msgs/srv/set_config.hpp" -#include "control_msgs/msg/io_gripper_sensor.hpp" +#include "control_msgs/srv/set_io_gripper_config.hpp" #include "control_msgs/msg/interface_value.hpp" #include "control_msgs/msg/dynamic_interface_values.hpp" -#include "control_msgs/action/gripper.hpp" -#include "control_msgs/action/set_gripper_config.hpp" +#include "control_msgs/action/io_gripper_command.hpp" +#include "control_msgs/action/set_io_gripper_config.hpp" #include "rclcpp_action/rclcpp_action.hpp" namespace io_gripper_controller @@ -193,14 +192,14 @@ class IOGripperController : public controller_interface::ControllerInterface using ControllerModeSrvType = std_srvs::srv::SetBool; using OpenCloseSrvType = std_srvs::srv::Trigger; - using ConfigSrvType = control_msgs::srv::SetConfig; + using ConfigSrvType = control_msgs::srv::SetIOGripperConfig; using JointStateMsg = sensor_msgs::msg::JointState; using EventStateMsg = sensor_msgs::msg::JointState; using ConfigJointMsg = sensor_msgs::msg::JointState; using DynInterfaceMsg = control_msgs::msg::DynamicInterfaceValues; - using GripperAction = control_msgs::action::Gripper; + using GripperAction = control_msgs::action::IOGripperCommand; using GoalHandleGripper = rclcpp_action::ServerGoalHandle; - using GripperConfigAction = control_msgs::action::SetGripperConfig; + using GripperConfigAction = control_msgs::action::SetIOGripperConfig; using GoalHandleGripperConfig = rclcpp_action::ServerGoalHandle; protected: @@ -322,10 +321,10 @@ class IOGripperController : public controller_interface::ControllerInterface rclcpp::CallbackGroup::SharedPtr open_service_callback_group_; rclcpp::CallbackGroup::SharedPtr close_service_callback_group_; rclcpp::CallbackGroup::SharedPtr reconfigure_service_callback_group_; - std::shared_ptr gripper_feedback_; - std::shared_ptr gripper_result_; - std::shared_ptr gripper_config_feedback_; - std::shared_ptr gripper_config_result_; + std::shared_ptr gripper_feedback_; + std::shared_ptr gripper_result_; + std::shared_ptr gripper_config_feedback_; + std::shared_ptr gripper_config_result_; /** * @brief Handles the goal for the gripper action. diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index 4db6470e9e..c867f4b675 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -44,7 +44,7 @@ using JointStateMsg = io_gripper_controller::IOGripperController::JointStateMsg; using OpenCloseSrvType = io_gripper_controller::IOGripperController::OpenCloseSrvType; using ControllerModeSrvType = io_gripper_controller::IOGripperController::ControllerModeSrvType; using EventStateMsg = io_gripper_controller::IOGripperController::EventStateMsg; -using ConfigSrvType = control_msgs::srv::SetConfig; +using ConfigSrvType = io_gripper_controller::IOGripperController::ConfigSrvType; // control_msgs::srv::SetIOGripperConfig; using GripperAction = io_gripper_controller::IOGripperController::GripperAction; using GoalHandleGripperAction = rclcpp_action::ClientGoalHandle; From 39ef8cbccb1ae49cda2d828edb0d9a11c71d19ed Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 22:48:32 +0100 Subject: [PATCH 23/75] removed unused code Co-authored-by: Dr. Denis --- .../include/io_gripper_controller/io_gripper_controller.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index eff2a9180b..614314c235 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -194,8 +194,6 @@ class IOGripperController : public controller_interface::ControllerInterface using OpenCloseSrvType = std_srvs::srv::Trigger; using ConfigSrvType = control_msgs::srv::SetIOGripperConfig; using JointStateMsg = sensor_msgs::msg::JointState; - using EventStateMsg = sensor_msgs::msg::JointState; - using ConfigJointMsg = sensor_msgs::msg::JointState; using DynInterfaceMsg = control_msgs::msg::DynamicInterfaceValues; using GripperAction = control_msgs::action::IOGripperCommand; using GoalHandleGripper = rclcpp_action::ServerGoalHandle; From 393a014b8370a4e1f711f9edb7d57228ba979fc8 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 23:06:04 +0100 Subject: [PATCH 24/75] - doc string added for rt buffer members - renamed service_buffer_ to gripper_service_buffer_ --- .../io_gripper_controller.hpp | 10 ++++++++-- .../src/io_gripper_controller.cpp | 18 +++++++++--------- .../test/test_io_gripper_controller_close.cpp | 8 ++++---- .../test/test_io_gripper_controller_open.cpp | 8 ++++---- ...io_gripper_controller_open_close_action.cpp | 16 ++++++++-------- 5 files changed, 33 insertions(+), 27 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 614314c235..545fe273cd 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -207,10 +207,16 @@ class IOGripperController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr configure_gripper_service_; rclcpp_action::Server::SharedPtr gripper_action_server_; rclcpp_action::Server::SharedPtr gripper_config_action_server_; - realtime_tools::RealtimeBuffer service_buffer_; - realtime_tools::RealtimeBuffer configure_gripper_buffer_; + + // Realtime buffer to store the state for outer gripper_service (e.g. idel, open, close) + realtime_tools::RealtimeBuffer gripper_service_buffer_; + // Realtime buffer to store the state for switching the gripper state (e.g. idle, set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, halted) realtime_tools::RealtimeBuffer gripper_state_buffer_; + // Realtime buffer to store the name of the configuration which needs to be set + realtime_tools::RealtimeBuffer configure_gripper_buffer_; + // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, find_config, set_command, check_state) realtime_tools::RealtimeBuffer reconfigure_state_buffer_; + using ControllerStatePublisher = realtime_tools::RealtimePublisher; using EventPublisher = realtime_tools::RealtimePublisher; using ConfigPublisher = realtime_tools::RealtimePublisher; diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 0922089b32..348b02f22f 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -50,7 +50,7 @@ IOGripperController::IOGripperController() : controller_interface::ControllerInt controller_interface::CallbackReturn IOGripperController::on_init() { - service_buffer_.initRT(service_mode_type::IDLE); + gripper_service_buffer_.initRT(service_mode_type::IDLE); configuration_key_ = ""; configure_gripper_buffer_.initRT(configuration_key_); gripper_state_buffer_.initRT(gripper_state_type::IDLE); @@ -141,7 +141,7 @@ controller_interface::return_type IOGripperController::update( handle_reconfigure_state_transition(*(reconfigure_state_buffer_.readFromRT())); } - switch (*(service_buffer_.readFromRT())) + switch (*(gripper_service_buffer_.readFromRT())) { case service_mode_type::IDLE: // do nothing @@ -326,7 +326,7 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st } gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); closeFlag_.store(false); - service_buffer_.writeFromNonRT(service_mode_type::IDLE); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); break; default: break; @@ -410,7 +410,7 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta } gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); openFlag_.store(false); - service_buffer_.writeFromNonRT(service_mode_type::IDLE); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); break; default: break; @@ -818,7 +818,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and reconfigureFlag_.store(false); // reset service buffer - service_buffer_.writeFromNonRT(service_mode_type::IDLE); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); // reset gripper state buffer gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); @@ -854,7 +854,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and closeFlag_.store(false); } openFlag_.store(true); - service_buffer_.writeFromNonRT(service_mode_type::OPEN); + gripper_service_buffer_.writeFromNonRT(service_mode_type::OPEN); gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); while(openFlag_.load()) { @@ -893,7 +893,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and response->success = false; return; } - service_buffer_.writeFromNonRT(service_mode_type::CLOSE); + gripper_service_buffer_.writeFromNonRT(service_mode_type::CLOSE); gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); if (openFlag_.load()) { @@ -1079,7 +1079,7 @@ rclcpp_action::GoalResponse IOGripperController::handle_goal( RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); return rclcpp_action::GoalResponse::REJECT; } - service_buffer_.writeFromNonRT((goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); + gripper_service_buffer_.writeFromNonRT((goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); } catch (const std::exception & e) @@ -1095,7 +1095,7 @@ rclcpp_action::CancelResponse IOGripperController::handle_cancel( const std::shared_ptr goal_handle) { (void)goal_handle; - service_buffer_.writeFromNonRT(service_mode_type::IDLE); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); return rclcpp_action::CancelResponse::ACCEPT; } diff --git a/io_gripper_controller/test/test_io_gripper_controller_close.cpp b/io_gripper_controller/test/test_io_gripper_controller_close.cpp index 68dd1de35e..a28eb29e09 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_close.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_close.cpp @@ -63,21 +63,21 @@ TEST_F(IOGripperControllerTest, CloseGripperService) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CLOSE_GRIPPER); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); ASSERT_EQ( @@ -120,6 +120,6 @@ TEST_F(IOGripperControllerTest, CloseGripperService) EXPECT_EQ(joint_state_sub_msg_->position.size(), 2); EXPECT_EQ(joint_state_sub_msg_->position[0], 0.08); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); } \ No newline at end of file diff --git a/io_gripper_controller/test/test_io_gripper_controller_open.cpp b/io_gripper_controller/test/test_io_gripper_controller_open.cpp index 8878093b1e..a65ff9b06e 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open.cpp @@ -65,21 +65,21 @@ TEST_F(IOGripperControllerTest, OpenGripperService) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::OPEN_GRIPPER); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); ASSERT_EQ( @@ -114,6 +114,6 @@ TEST_F(IOGripperControllerTest, OpenGripperService) ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); ASSERT_EQ(joint_state_sub_msg_->position[0], 0.0); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); } \ No newline at end of file diff --git a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp index b5001335d7..2c2836e254 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp @@ -63,28 +63,28 @@ TEST_F(IOGripperControllerTest, OpenCloseGripperAction) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::OPEN_GRIPPER); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); // update to make sure the publisher value is updated @@ -131,28 +131,28 @@ TEST_F(IOGripperControllerTest, OpenCloseGripperAction) controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CLOSE_GRIPPER); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); // update to make sure the publisher value is updated ASSERT_EQ( From c12f9c88a98c852e95463249a2db2a7238695c91 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 23:10:39 +0100 Subject: [PATCH 25/75] removed unused code --- .../include/io_gripper_controller/io_gripper_controller.hpp | 4 ---- io_gripper_controller/test/test_io_gripper_controller.hpp | 1 - 2 files changed, 5 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 545fe273cd..89ff7940b2 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -218,16 +218,12 @@ class IOGripperController : public controller_interface::ControllerInterface realtime_tools::RealtimeBuffer reconfigure_state_buffer_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; - using EventPublisher = realtime_tools::RealtimePublisher; - using ConfigPublisher = realtime_tools::RealtimePublisher; using InterfacePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr g_j_s_publisher_; std::unique_ptr gripper_joint_state_publisher_; std::vector joint_state_values_; rclcpp::Publisher::SharedPtr if_publisher_; std::unique_ptr interface_publisher_; - rclcpp::Publisher::SharedPtr e_publisher_; - std::unique_ptr event_publisher_; std::atomic reconfigureFlag_{false}; std::atomic openFlag_{false}; std::atomic closeFlag_{false}; diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index c867f4b675..bc7278b44b 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -43,7 +43,6 @@ using JointStateMsg = io_gripper_controller::IOGripperController::JointStateMsg; using OpenCloseSrvType = io_gripper_controller::IOGripperController::OpenCloseSrvType; using ControllerModeSrvType = io_gripper_controller::IOGripperController::ControllerModeSrvType; -using EventStateMsg = io_gripper_controller::IOGripperController::EventStateMsg; using ConfigSrvType = io_gripper_controller::IOGripperController::ConfigSrvType; // control_msgs::srv::SetIOGripperConfig; using GripperAction = io_gripper_controller::IOGripperController::GripperAction; From d3f39acb5f39ebd406fb6c335aba517c50df8917 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 3 Jan 2025 02:02:42 +0100 Subject: [PATCH 26/75] pre-commit changes --- io_gripper_controller/CMakeLists.txt | 2 +- io_gripper_controller/doc/userdoc.rst | 2 +- .../io_gripper_controller.hpp | 50 +- io_gripper_controller/package.xml | 4 +- .../src/io_gripper_controller.cpp | 749 +++++++++--------- .../src/io_gripper_controller.yaml | 2 +- .../test/test_io_gripper_controller.cpp | 6 +- .../test/test_io_gripper_controller.hpp | 184 +++-- ...st_io_gripper_controller_all_param_set.cpp | 7 +- .../test/test_io_gripper_controller_close.cpp | 59 +- .../test/test_io_gripper_controller_open.cpp | 55 +- ...o_gripper_controller_open_close_action.cpp | 108 ++- ...test_io_gripper_controller_reconfigure.cpp | 30 +- ..._gripper_controller_reconfigure_action.cpp | 40 +- .../test/test_load_io_gripper_controller.cpp | 5 +- 15 files changed, 721 insertions(+), 582 deletions(-) diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt index b7e050733e..7446421bcd 100644 --- a/io_gripper_controller/CMakeLists.txt +++ b/io_gripper_controller/CMakeLists.txt @@ -74,7 +74,7 @@ if(BUILD_TESTING) ros2_control_test_assets ) - ament_add_gmock(test_io_gripper_controller + ament_add_gmock(test_io_gripper_controller test/test_io_gripper_controller.cpp test/test_io_gripper_controller_open.cpp test/test_io_gripper_controller_close.cpp diff --git a/io_gripper_controller/doc/userdoc.rst b/io_gripper_controller/doc/userdoc.rst index 288e2f9993..9fefd8566f 100644 --- a/io_gripper_controller/doc/userdoc.rst +++ b/io_gripper_controller/doc/userdoc.rst @@ -96,4 +96,4 @@ io_gripper_controller: hohenabfrage: input: "EL1008/Hohenabfrage_BG5" bauteilabfrage: - input: "EL1008/Bauteilabfrage_BG06" \ No newline at end of file + input: "EL1008/Bauteilabfrage_BG06" diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 89ff7940b2..5b494cbcd7 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -15,12 +15,12 @@ #ifndef GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ #define GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ +#include +#include #include +#include #include #include -#include -#include -#include #include "controller_interface/controller_interface.hpp" #include "io_gripper_controller_parameters.hpp" @@ -33,11 +33,11 @@ #include -#include "control_msgs/srv/set_io_gripper_config.hpp" -#include "control_msgs/msg/interface_value.hpp" -#include "control_msgs/msg/dynamic_interface_values.hpp" #include "control_msgs/action/io_gripper_command.hpp" #include "control_msgs/action/set_io_gripper_config.hpp" +#include "control_msgs/msg/dynamic_interface_values.hpp" +#include "control_msgs/msg/interface_value.hpp" +#include "control_msgs/srv/set_io_gripper_config.hpp" #include "rclcpp_action/rclcpp_action.hpp" namespace io_gripper_controller @@ -45,7 +45,8 @@ namespace io_gripper_controller /** * @enum service_mode_type - * @brief Represents the various service modes of the gripper. These modes represents the high level states of the gripper. + * @brief Represents the various service modes of the gripper. These modes represents the high level + * states of the gripper. * * - IDLE: The gripper is in an idle state, not performing any action. * - OPEN: The gripper is in the process of opening. @@ -63,7 +64,8 @@ enum class service_mode_type : std::uint8_t * @brief Represents the various states of the gripper. * * - IDLE: The gripper is in an idle state, not performing any action. - * - SET_BEFORE_COMMAND: Executing commands for io defined in the yaml which are required before opening the gripper. + * - SET_BEFORE_COMMAND: Executing commands for io defined in the yaml which are required before + * opening the gripper. * - CLOSE_GRIPPER: Executing commands to close the gripper. * - CHECK_GRIPPER_STATE: Checking the state of the gripper to make sure the gripper is closed. * - OPEN_GRIPPER: Executing commands to open the gripper. @@ -83,7 +85,8 @@ enum class gripper_state_type : std::uint8_t /** * @enum reconfigure_state_type - * @brief Represents the various states of the reconfiguration process, which means that the gripper is reconfiguring to new state based on the configuration defined in the yaml params. + * @brief Represents the various states of the reconfiguration process, which means that the gripper + * is reconfiguring to new state based on the configuration defined in the yaml params. * * - IDLE: The reconfiguration process is idle, not performing any action. * - FIND_CONFIG: Finding the configuration settings. @@ -207,14 +210,17 @@ class IOGripperController : public controller_interface::ControllerInterface rclcpp::Service::SharedPtr configure_gripper_service_; rclcpp_action::Server::SharedPtr gripper_action_server_; rclcpp_action::Server::SharedPtr gripper_config_action_server_; - - // Realtime buffer to store the state for outer gripper_service (e.g. idel, open, close) + + // Realtime buffer to store the state for outer gripper_service (e.g. idle, open, close) realtime_tools::RealtimeBuffer gripper_service_buffer_; - // Realtime buffer to store the state for switching the gripper state (e.g. idle, set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, halted) + // Realtime buffer to store the state for switching the gripper state (e.g. idle, + // set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, + // halted) realtime_tools::RealtimeBuffer gripper_state_buffer_; // Realtime buffer to store the name of the configuration which needs to be set realtime_tools::RealtimeBuffer configure_gripper_buffer_; - // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, find_config, set_command, check_state) + // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, find_config, + // set_command, check_state) realtime_tools::RealtimeBuffer reconfigure_state_buffer_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; @@ -243,7 +249,7 @@ class IOGripperController : public controller_interface::ControllerInterface * @param value The value to get. * @return True if the state was found and retrieved, false otherwise. */ - bool find_and_get_state(const std::string & name, double& value); + bool find_and_get_state(const std::string & name, double & value); /** * @brief Finds and gets a command value. @@ -251,7 +257,7 @@ class IOGripperController : public controller_interface::ControllerInterface * @param value The value to get. * @return True if the command was found and retrieved, false otherwise. */ - bool find_and_get_command(const std::string & name, double& value); + bool find_and_get_command(const std::string & name, double & value); /** * @brief Handles the state transition when opening the gripper. @@ -310,7 +316,8 @@ class IOGripperController : public controller_interface::ControllerInterface std::vector configurations_list_; std::vector config_map_; - std::vector sensors_map_; + std::vector + sensors_map_; double state_value_; std::string configuration_key_; bool check_state_ios_; @@ -325,7 +332,7 @@ class IOGripperController : public controller_interface::ControllerInterface std::shared_ptr gripper_result_; std::shared_ptr gripper_config_feedback_; std::shared_ptr gripper_config_result_; - + /** * @brief Handles the goal for the gripper action. * @param uuid The UUID of the goal. @@ -333,16 +340,14 @@ class IOGripperController : public controller_interface::ControllerInterface * @return GoalResponse indicating acceptance or rejection of the goal. */ rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); /** * @brief Handles the cancellation of the gripper action. * @param goal_handle The handle of the goal to cancel. * @return CancelResponse indicating acceptance or rejection of the cancellation. */ - rclcpp_action::CancelResponse handle_cancel( - const std::shared_ptr goal_handle); + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); /** * @brief Handles the acceptance of the gripper action. @@ -363,8 +368,7 @@ class IOGripperController : public controller_interface::ControllerInterface * @return GoalResponse indicating acceptance or rejection of the goal. */ rclcpp_action::GoalResponse config_handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); /** * @brief Handles the cancellation of the gripper configuration action. diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml index 39a5b3167e..8439ff92cf 100644 --- a/io_gripper_controller/package.xml +++ b/io_gripper_controller/package.xml @@ -6,12 +6,12 @@ gripper io controller used to control the gripper using IOs Yara Shahin Sachin Kumar - + Bence Magyar Denis Štogl Christoph Froehlich Sai Kishor Kothakota - + Apache License 2.0 ament_cmake diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 348b02f22f..2cdcf49806 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -90,7 +90,8 @@ controller_interface::CallbackReturn IOGripperController::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::InterfaceConfiguration IOGripperController::command_interface_configuration() const +controller_interface::InterfaceConfiguration IOGripperController::command_interface_configuration() + const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -104,7 +105,8 @@ controller_interface::InterfaceConfiguration IOGripperController::command_interf return command_interfaces_config; } -controller_interface::InterfaceConfiguration IOGripperController::state_interface_configuration() const +controller_interface::InterfaceConfiguration IOGripperController::state_interface_configuration() + const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -114,7 +116,7 @@ controller_interface::InterfaceConfiguration IOGripperController::state_interfac { state_interfaces_config.names.push_back(state_io); } - + return state_interfaces_config; } @@ -128,7 +130,9 @@ controller_interface::CallbackReturn IOGripperController::on_activate( controller_interface::CallbackReturn IOGripperController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - joint_state_values_.resize(params_.open_close_joints.size() + params_.configuration_joints.size(), std::numeric_limits::quiet_NaN()); + joint_state_values_.resize( + params_.open_close_joints.size() + params_.configuration_joints.size(), + std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } @@ -143,20 +147,20 @@ controller_interface::return_type IOGripperController::update( switch (*(gripper_service_buffer_.readFromRT())) { - case service_mode_type::IDLE: - // do nothing - break; - case service_mode_type::OPEN: - handle_gripper_state_transition_open(*(gripper_state_buffer_.readFromRT())); - break; - case service_mode_type::CLOSE: - handle_gripper_state_transition_close(*(gripper_state_buffer_.readFromRT())); - break; - - default: - break; + case service_mode_type::IDLE: + // do nothing + break; + case service_mode_type::OPEN: + handle_gripper_state_transition_open(*(gripper_state_buffer_.readFromRT())); + break; + case service_mode_type::CLOSE: + handle_gripper_state_transition_close(*(gripper_state_buffer_.readFromRT())); + break; + + default: + break; } - + publish_gripper_joint_states(); publish_dynamic_interface_values(); @@ -168,9 +172,7 @@ bool IOGripperController::find_and_set_command(const std::string & name, const d auto it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [&](const hardware_interface::LoanedCommandInterface & command_interface) - { - return command_interface.get_name() == name; - }); + { return command_interface.get_name() == name; }); if (it != command_interfaces_.end()) { @@ -179,14 +181,12 @@ bool IOGripperController::find_and_set_command(const std::string & name, const d return false; } -bool IOGripperController::find_and_get_state(const std::string & name, double& value) +bool IOGripperController::find_and_get_state(const std::string & name, double & value) { auto it = std::find_if( state_interfaces_.begin(), state_interfaces_.end(), [&](const hardware_interface::LoanedStateInterface & state_interface) - { - return state_interface.get_name() == name; - }); + { return state_interface.get_name() == name; }); if (it != state_interfaces_.end()) { @@ -197,14 +197,12 @@ bool IOGripperController::find_and_get_state(const std::string & name, double& v return false; } -bool IOGripperController::find_and_get_command(const std::string & name, double& value) +bool IOGripperController::find_and_get_command(const std::string & name, double & value) { auto it = std::find_if( command_interfaces_.begin(), command_interfaces_.end(), [&](const hardware_interface::LoanedCommandInterface & command_interface) - { - return command_interface.get_name() == name; - }); + { return command_interface.get_name() == name; }); if (it != command_interfaces_.end()) { @@ -217,215 +215,233 @@ bool IOGripperController::find_and_get_command(const std::string & name, double& void IOGripperController::handle_gripper_state_transition_close(const gripper_state_type & state) { - switch (state) + switch (state) + { + case gripper_state_type::IDLE: + // do nothing + break; + case gripper_state_type::SET_BEFORE_COMMAND: + for (size_t i = 0; i < set_before_command_close.size(); ++i) { - case gripper_state_type::IDLE: - // do nothing - break; - case gripper_state_type::SET_BEFORE_COMMAND: - for (size_t i = 0; i < set_before_command_close.size(); ++i) + setResult = + find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); + if (!setResult) { - setResult = find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", set_before_command_close[i].c_str()); - } + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", + set_before_command_close[i].c_str()); } + } - gripper_state_buffer_.writeFromNonRT(gripper_state_type::CLOSE_GRIPPER); - break; - case gripper_state_type::CLOSE_GRIPPER: - for (size_t i = 0; i < command_ios_close.size(); ++i) + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CLOSE_GRIPPER); + break; + case gripper_state_type::CLOSE_GRIPPER: + for (size_t i = 0; i < command_ios_close.size(); ++i) + { + setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); + if (!setResult) { - setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", + command_ios_close[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); + break; + case gripper_state_type::CHECK_GRIPPER_STATE: + for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) + { + check_state_ios_ = false; + for (const auto & high_val : state_params.high) + { + setResult = find_and_get_state(high_val, state_value_); if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", command_ios_close[i].c_str()); + get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); } - } - - gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); - break; - case gripper_state_type::CHECK_GRIPPER_STATE: - for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) - { - check_state_ios_ = false; - for (const auto & high_val : state_params.high) + else { - setResult = find_and_get_state(high_val, state_value_); - if (!setResult) + if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); - } - else { - if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) - { - check_state_ios_ = true; - } - else { - check_state_ios_ = false; - break; - } + check_state_ios_ = true; } - } - for (const auto & low_val : state_params.low) - { - setResult = find_and_get_state(low_val, state_value_); - if (!setResult) + else { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); - } - else { - if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) - { - check_state_ios_ = true; - } - else { - check_state_ios_ = false; - break; - } + check_state_ios_ = false; + break; } } - if (check_state_ios_) - { - closed_state_name_ = state_name; - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); - break; - } } - break; - case gripper_state_type::SET_AFTER_COMMAND: - closed_state_values_ = params_.close.state.possible_closed_states_map.at(closed_state_name_); - - for (const auto & high_val : closed_state_values_.set_after_command_high) + for (const auto & low_val : state_params.low) { - setResult = find_and_set_command(high_val, 1.0); + setResult = find_and_get_state(low_val, state_value_); if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", high_val.c_str()); + get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); + } + else + { + if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) + { + check_state_ios_ = true; + } + else + { + check_state_ios_ = false; + break; + } } } + if (check_state_ios_) + { + closed_state_name_ = state_name; + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); + break; + } + } + break; + case gripper_state_type::SET_AFTER_COMMAND: + closed_state_values_ = params_.close.state.possible_closed_states_map.at(closed_state_name_); - for (const auto & low_val : closed_state_values_.set_after_command_low) + for (const auto & high_val : closed_state_values_.set_after_command_high) + { + setResult = find_and_set_command(high_val, 1.0); + if (!setResult) { - setResult = find_and_set_command(low_val, 0.0); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", low_val.c_str()); - } + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", high_val.c_str()); } - for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states.size(); ++i) + } + + for (const auto & low_val : closed_state_values_.set_after_command_low) + { + setResult = find_and_set_command(low_val, 0.0); + if (!setResult) { - joint_state_values_[i] = params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states[i]; + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", low_val.c_str()); } - gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); - closeFlag_.store(false); - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); - break; - default: - break; } + for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(closed_state_name_) + .joint_states.size(); + ++i) + { + joint_state_values_[i] = + params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states[i]; + } + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + closeFlag_.store(false); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); + break; + default: + break; + } } void IOGripperController::handle_gripper_state_transition_open(const gripper_state_type & state) { - switch (state) + switch (state) + { + case gripper_state_type::IDLE: + // do nothing + break; + case gripper_state_type::SET_BEFORE_COMMAND: + for (size_t i = 0; i < set_before_command_open.size(); ++i) { - case gripper_state_type::IDLE: - // do nothing - break; - case gripper_state_type::SET_BEFORE_COMMAND: - for (size_t i = 0; i < set_before_command_open.size(); ++i) - { - setResult = find_and_set_command(set_before_command_open[i], set_before_command_open_values[i]); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", set_before_command_open[i].c_str()); - } - } - - gripper_state_buffer_.writeFromNonRT(gripper_state_type::OPEN_GRIPPER); - break; - case gripper_state_type::OPEN_GRIPPER: - // now open the gripper - for (size_t i = 0; i < command_ios_open.size(); ++i) + setResult = + find_and_set_command(set_before_command_open[i], set_before_command_open_values[i]); + if (!setResult) { - setResult = find_and_set_command(command_ios_open[i], command_ios_open_values[i]); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", command_ios_open[i].c_str()); - } + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", + set_before_command_open[i].c_str()); } - - gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); - break; - case gripper_state_type::CHECK_GRIPPER_STATE: - // check the state of the gripper - check_state_ios_ = false; - for (size_t i = 0; i < state_ios_open.size(); ++i) + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::OPEN_GRIPPER); + break; + case gripper_state_type::OPEN_GRIPPER: + // now open the gripper + for (size_t i = 0; i < command_ios_open.size(); ++i) + { + setResult = find_and_set_command(command_ios_open[i], command_ios_open_values[i]); + if (!setResult) { - setResult = find_and_get_state(state_ios_open[i], state_value_); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); - } - else { - if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) - { - check_state_ios_ = true; - } - else { - check_state_ios_ = false; - break; - } - } + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", + command_ios_open[i].c_str()); } - if(check_state_ios_) + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); + break; + case gripper_state_type::CHECK_GRIPPER_STATE: + // check the state of the gripper + check_state_ios_ = false; + for (size_t i = 0; i < state_ios_open.size(); ++i) + { + setResult = find_and_get_state(state_ios_open[i], state_value_); + if (!setResult) { - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); } - break; - case gripper_state_type::SET_AFTER_COMMAND: - for (size_t i = 0; i < set_after_command_open.size(); ++i) + else { - setResult = find_and_set_command(set_after_command_open[i], set_after_command_open_values[i]); - if (!setResult) + if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", set_after_command_open[i].c_str()); + check_state_ios_ = true; + } + else + { + check_state_ios_ = false; + break; } } - for (size_t i = 0; i < params_.open.joint_states.size(); ++i) + } + if (check_state_ios_) + { + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); + } + break; + case gripper_state_type::SET_AFTER_COMMAND: + for (size_t i = 0; i < set_after_command_open.size(); ++i) + { + setResult = + find_and_set_command(set_after_command_open[i], set_after_command_open_values[i]); + if (!setResult) { - joint_state_values_[i] = params_.open.joint_states[i]; + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", + set_after_command_open[i].c_str()); } - gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); - openFlag_.store(false); - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); - break; - default: - break; } + for (size_t i = 0; i < params_.open.joint_states.size(); ++i) + { + joint_state_values_[i] = params_.open.joint_states[i]; + } + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + openFlag_.store(false); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); + break; + default: + break; + } } void IOGripperController::handle_reconfigure_state_transition(const reconfigure_state_type & state) { switch (state) - { + { case reconfigure_state_type::IDLE: // do nothing break; case reconfigure_state_type::FIND_CONFIG: - config_index_ = std::find(configurations_list_.begin(), configurations_list_.end(), configuration_key_); + config_index_ = + std::find(configurations_list_.begin(), configurations_list_.end(), configuration_key_); if (config_index_ == configurations_list_.end()) { RCLCPP_ERROR(get_node()->get_logger(), "Configuration not found"); @@ -469,19 +485,18 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ if (!setResult) { check_state_ios_ = false; - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); } - else + else { - if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) + if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) { check_state_ios_ = false; - RCLCPP_ERROR( - get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); break; } - else { + else + { check_state_ios_ = true; } } @@ -492,19 +507,17 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ setResult = find_and_get_state(io, state_value_); if (!setResult) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); } else - { + { if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) { - RCLCPP_ERROR( - get_node()->get_logger(), "value doesn't match %s", io.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "value doesn't match %s", io.c_str()); check_state_ios_ = false; break; } - else + else { check_state_ios_ = true; } @@ -515,7 +528,7 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ { for (size_t i = 0; i < conf_it_.joint_states.size(); ++i) { - joint_state_values_[i + params_.open_close_joints.size()] = conf_it_.joint_states[i]; + joint_state_values_[i + params_.open_close_joints.size()] = conf_it_.joint_states[i]; } reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); configure_gripper_buffer_.writeFromNonRT(""); @@ -524,44 +537,39 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ break; default: break; - } + } } controller_interface::CallbackReturn IOGripperController::check_parameters() { if (params_.open_close_joints.empty()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of open close joints parameter cannot be zero."); + RCLCPP_FATAL(get_node()->get_logger(), "Size of open close joints parameter cannot be zero."); return CallbackReturn::FAILURE; } if (params_.open.joint_states.empty()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of joint states parameters cannot be zero."); + RCLCPP_FATAL(get_node()->get_logger(), "Size of joint states parameters cannot be zero."); return CallbackReturn::FAILURE; } if (params_.open.command.high.empty() and params_.open.command.low.empty()) { RCLCPP_FATAL( - get_node()->get_logger(), - "Size of open command high and low parameters cannot be zero."); + get_node()->get_logger(), "Size of open command high and low parameters cannot be zero."); return CallbackReturn::FAILURE; } if (params_.open.state.high.empty() and params_.open.state.low.empty()) { RCLCPP_FATAL( - get_node()->get_logger(), - "Size of open state high and low parameters cannot be zero."); + get_node()->get_logger(), "Size of open state high and low parameters cannot be zero."); return CallbackReturn::FAILURE; } - if (params_.close.set_before_command.high.empty() and params_.close.set_before_command.low.empty()) + if ( + params_.close.set_before_command.high.empty() and params_.close.set_before_command.low.empty()) { RCLCPP_FATAL( get_node()->get_logger(), @@ -572,17 +580,14 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() if (params_.close.command.high.empty() and params_.close.command.low.empty()) { RCLCPP_FATAL( - get_node()->get_logger(), - "Size of close command high and low parameters cannot be zero."); + get_node()->get_logger(), "Size of close command high and low parameters cannot be zero."); return CallbackReturn::FAILURE; } // configurations parameter if (params_.configurations.empty()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of configurations parameter cannot be zero."); + RCLCPP_FATAL(get_node()->get_logger(), "Size of configurations parameter cannot be zero."); return CallbackReturn::FAILURE; } @@ -590,17 +595,14 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() if (params_.configuration_joints.empty()) { RCLCPP_FATAL( - get_node()->get_logger(), - "Size of configuration joints parameter cannot be zero."); + get_node()->get_logger(), "Size of configuration joints parameter cannot be zero."); return CallbackReturn::FAILURE; } // configuration setup parameter if (params_.configuration_setup.configurations_map.empty()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of configuration map parameter cannot be zero."); + RCLCPP_FATAL(get_node()->get_logger(), "Size of configuration map parameter cannot be zero."); return CallbackReturn::FAILURE; } @@ -608,28 +610,26 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() if (params_.gripper_specific_sensors.empty()) { RCLCPP_FATAL( - get_node()->get_logger(), - "Size of gripper specific sensors parameter cannot be zero."); + get_node()->get_logger(), "Size of gripper specific sensors parameter cannot be zero."); return CallbackReturn::FAILURE; } // sensors interfaces parameter if (params_.sensors_interfaces.gripper_specific_sensors_map.empty()) { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of sensors interfaces parameter cannot be zero."); + RCLCPP_FATAL(get_node()->get_logger(), "Size of sensors interfaces parameter cannot be zero."); return CallbackReturn::FAILURE; } // if sensor input string is empty then return failure - for (const auto& [key, val] : params_.sensors_interfaces.gripper_specific_sensors_map) + for (const auto & [key, val] : params_.sensors_interfaces.gripper_specific_sensors_map) { if (val.input == "") { RCLCPP_FATAL( get_node()->get_logger(), - "Size of sensor input string parameter cannot be empty ("")."); + "Size of sensor input string parameter cannot be empty (" + ")."); return CallbackReturn::FAILURE; } } @@ -639,8 +639,9 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() void IOGripperController::prepare_command_and_state_ios() { // make full command ios lists -- just once - for (const auto& key : params_.open.set_before_command.high) { - if(!key.empty()) + for (const auto & key : params_.open.set_before_command.high) + { + if (!key.empty()) { set_before_command_open.push_back(key); set_before_command_open_values.push_back(1.0); @@ -648,15 +649,17 @@ void IOGripperController::prepare_command_and_state_ios() } } - for (const auto& key : params_.open.set_before_command.low) { - if(!key.empty()) + for (const auto & key : params_.open.set_before_command.low) + { + if (!key.empty()) { set_before_command_open.push_back(key); set_before_command_open_values.push_back(0.0); command_if_ios.insert(key); } - } - for (const auto& key : params_.open.command.high) { + } + for (const auto & key : params_.open.command.high) + { if (!key.empty()) { command_ios_open.push_back(key); @@ -665,7 +668,8 @@ void IOGripperController::prepare_command_and_state_ios() } } - for (const auto& key : params_.open.command.low) { + for (const auto & key : params_.open.command.low) + { if (!key.empty()) { command_ios_open.push_back(key); @@ -674,8 +678,9 @@ void IOGripperController::prepare_command_and_state_ios() } } - for (const auto& key : params_.open.set_after_command.high) { - if(!key.empty()) + for (const auto & key : params_.open.set_after_command.high) + { + if (!key.empty()) { set_after_command_open.push_back(key); set_after_command_open_values.push_back(1.0); @@ -683,8 +688,9 @@ void IOGripperController::prepare_command_and_state_ios() } } - for (const auto& key : params_.open.set_after_command.low) { - if(!key.empty()) + for (const auto & key : params_.open.set_after_command.low) + { + if (!key.empty()) { set_after_command_open.push_back(key); set_after_command_open_values.push_back(0.0); @@ -692,8 +698,9 @@ void IOGripperController::prepare_command_and_state_ios() } } - for (const auto& key : params_.close.set_before_command.high) { - if(!key.empty()) + for (const auto & key : params_.close.set_before_command.high) + { + if (!key.empty()) { set_before_command_close.push_back(key); set_before_command_close_values.push_back(1.0); @@ -701,8 +708,9 @@ void IOGripperController::prepare_command_and_state_ios() } } - for (const auto& key : params_.close.set_before_command.low) { - if(!key.empty()) + for (const auto & key : params_.close.set_before_command.low) + { + if (!key.empty()) { set_before_command_close.push_back(key); set_before_command_close_values.push_back(0.0); @@ -710,14 +718,16 @@ void IOGripperController::prepare_command_and_state_ios() } } - for (const auto& key : params_.close.command.high) { + for (const auto & key : params_.close.command.high) + { command_ios_close.push_back(key); command_ios_close_values.push_back(1.0); command_if_ios.insert(key); } - for (const auto& key : params_.close.command.low) { - if(!key.empty()) + for (const auto & key : params_.close.command.low) + { + if (!key.empty()) { command_ios_close.push_back(key); command_ios_close_values.push_back(0.0); @@ -726,8 +736,9 @@ void IOGripperController::prepare_command_and_state_ios() } // make full state ios lists -- just once - for (const auto& key : params_.open.state.high) { - if(!key.empty()) + for (const auto & key : params_.open.state.high) + { + if (!key.empty()) { state_ios_open.push_back(key); state_ios_open_values.push_back(1.0); @@ -735,8 +746,9 @@ void IOGripperController::prepare_command_and_state_ios() } } - for (const auto& key : params_.open.state.low) { - if(!key.empty()) + for (const auto & key : params_.open.state.low) + { + if (!key.empty()) { state_ios_open.push_back(key); state_ios_open_values.push_back(0.0); @@ -748,28 +760,28 @@ void IOGripperController::prepare_command_and_state_ios() { for (const auto & key : value.high) { - if(!key.empty()) + if (!key.empty()) { state_if_ios.insert(key); } } for (const auto & key : value.low) { - if(!key.empty()) + if (!key.empty()) { state_if_ios.insert(key); } } - for (const auto & key: value.set_after_command_high) + for (const auto & key : value.set_after_command_high) { - if(!key.empty()) + if (!key.empty()) { command_if_ios.insert(key); } } - for (const auto & key: value.set_after_command_low) + for (const auto & key : value.set_after_command_low) { - if(!key.empty()) + if (!key.empty()) { command_if_ios.insert(key); } @@ -809,7 +821,9 @@ void IOGripperController::prepare_command_and_state_ios() for (size_t i = 0; i < params_.gripper_specific_sensors.size(); ++i) { - state_if_ios.insert(params_.sensors_interfaces.gripper_specific_sensors_map.at(params_.gripper_specific_sensors[i]).input); + state_if_ios.insert(params_.sensors_interfaces.gripper_specific_sensors_map + .at(params_.gripper_specific_sensors[i]) + .input); } } @@ -826,20 +840,19 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and if (!params_.use_action) { // callback groups for each service - open_service_callback_group_ = get_node()->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); + open_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - close_service_callback_group_ = get_node()->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); + close_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - reconfigure_service_callback_group_ = get_node()->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); + reconfigure_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); // open service - auto open_service_callback = - [&]( - const std::shared_ptr /*request*/, - std::shared_ptr response) + auto open_service_callback = [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) { try { @@ -856,7 +869,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and openFlag_.store(true); gripper_service_buffer_.writeFromNonRT(service_mode_type::OPEN); gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); - while(openFlag_.load()) + while (openFlag_.load()) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); if ((*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED)) @@ -864,7 +877,8 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and response->success = false; break; } - else { + else + { response->success = true; } } @@ -876,14 +890,13 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and }; open_service_ = get_node()->create_service( - "~/gripper_open", open_service_callback, - rmw_qos_profile_services_hist_keep_all, open_service_callback_group_); + "~/gripper_open", open_service_callback, rmw_qos_profile_services_hist_keep_all, + open_service_callback_group_); // close service - auto close_service_callback = - [&]( - const std::shared_ptr /*request*/, - std::shared_ptr response) + auto close_service_callback = [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) { try { @@ -900,7 +913,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and openFlag_.store(false); } closeFlag_.store(true); - while(closeFlag_.load()) + while (closeFlag_.load()) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); if ((*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED)) @@ -908,7 +921,8 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and response->success = false; break; } - else { + else + { response->success = true; } } @@ -920,8 +934,8 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and }; close_service_ = get_node()->create_service( - "~/gripper_close", close_service_callback, - rmw_qos_profile_services_hist_keep_all, close_service_callback_group_); + "~/gripper_close", close_service_callback, rmw_qos_profile_services_hist_keep_all, + close_service_callback_group_); // configure gripper service auto configure_gripper_service_callback = @@ -935,7 +949,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and configure_gripper_buffer_.writeFromNonRT(conf.c_str()); reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); reconfigureFlag_.store(true); - while(reconfigureFlag_.load()) + while (reconfigureFlag_.load()) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } @@ -952,7 +966,6 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and configure_gripper_service_ = get_node()->create_service( "~/reconfigure_to", configure_gripper_service_callback, rmw_qos_profile_services_hist_keep_all, reconfigure_service_callback_group_); - } else { @@ -961,7 +974,8 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and gripper_result_ = std::make_shared(); gripper_action_server_ = rclcpp_action::create_server( get_node(), "~/gripper_action", - std::bind(&IOGripperController::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind( + &IOGripperController::handle_goal, this, std::placeholders::_1, std::placeholders::_2), std::bind(&IOGripperController::handle_cancel, this, std::placeholders::_1), std::bind(&IOGripperController::handle_accepted, this, std::placeholders::_1)); @@ -970,7 +984,9 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and gripper_config_result_ = std::make_shared(); gripper_config_action_server_ = rclcpp_action::create_server( get_node(), "~/reconfigure_gripper_action", - std::bind(&IOGripperController::config_handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind( + &IOGripperController::config_handle_goal, this, std::placeholders::_1, + std::placeholders::_2), std::bind(&IOGripperController::config_handle_cancel, this, std::placeholders::_1), std::bind(&IOGripperController::config_handle_accepted, this, std::placeholders::_1)); } @@ -983,7 +999,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and gripper_joint_state_publisher_ = std::make_unique(g_j_s_publisher_); auto final_joint_size = params_.open_close_joints.size() + params_.configuration_joints.size(); - + gripper_joint_state_publisher_->msg_.name.resize(final_joint_size); gripper_joint_state_publisher_->msg_.position.resize(final_joint_size); @@ -996,8 +1012,10 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and } for (size_t i = 0; i < params_.configuration_joints.size(); ++i) { - gripper_joint_state_publisher_->msg_.name[i + params_.open_close_joints.size()] = params_.configuration_joints[i]; - gripper_joint_state_publisher_->msg_.position[i + params_.open_close_joints.size()] = joint_state_values_[i + params_.open_close_joints.size()]; + gripper_joint_state_publisher_->msg_.name[i + params_.open_close_joints.size()] = + params_.configuration_joints[i]; + gripper_joint_state_publisher_->msg_.position[i + params_.open_close_joints.size()] = + joint_state_values_[i + params_.open_close_joints.size()]; } } catch (const std::exception & e) @@ -1015,7 +1033,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and "~/dynamic_interface", rclcpp::SystemDefaultsQoS()); interface_publisher_ = std::make_unique(if_publisher_); } - catch(const std::exception& e) + catch (const std::exception & e) { fprintf( stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", @@ -1037,23 +1055,26 @@ void IOGripperController::publish_gripper_joint_states() { gripper_joint_state_publisher_->msg_.position[i] = joint_state_values_[i]; } - } + } gripper_joint_state_publisher_->unlockAndPublish(); - } void IOGripperController::publish_dynamic_interface_values() { if (interface_publisher_ && interface_publisher_->trylock()) { - interface_publisher_->msg_.header.stamp = get_node()->get_clock()->now(); // Make sure it works and discuss with Dr. Denis + interface_publisher_->msg_.header.stamp = + get_node()->get_clock()->now(); // Make sure it works and discuss with Dr. Denis interface_publisher_->msg_.states.interface_names.clear(); interface_publisher_->msg_.states.values.clear(); interface_publisher_->msg_.states.values.resize(state_interfaces_.size()); for (size_t i = 0; i < state_interfaces_.size(); ++i) { - interface_publisher_->msg_.states.interface_names.push_back(state_interfaces_.at(i).get_name()); // this can be done in a separate function one time. Change it later TODO (Sachin) : - interface_publisher_->msg_.states.values.at(i) = static_cast(state_interfaces_.at(i).get_value()); + interface_publisher_->msg_.states.interface_names.push_back( + state_interfaces_.at(i).get_name()); // this can be done in a separate function one time. + // Change it later TODO (Sachin) : + interface_publisher_->msg_.states.values.at(i) = + static_cast(state_interfaces_.at(i).get_value()); } interface_publisher_->msg_.commands.interface_names.clear(); @@ -1061,49 +1082,54 @@ void IOGripperController::publish_dynamic_interface_values() interface_publisher_->msg_.commands.values.resize(command_interfaces_.size()); for (size_t i = 0; i < command_interfaces_.size(); ++i) { - interface_publisher_->msg_.commands.interface_names.push_back(command_interfaces_.at(i).get_name()); // this can be done in a separate function one time. Change it later TODO (Sachin) : - interface_publisher_->msg_.commands.values.at(i) = static_cast(command_interfaces_.at(i).get_value()); + interface_publisher_->msg_.commands.interface_names.push_back( + command_interfaces_.at(i).get_name()); // this can be done in a separate function one time. + // Change it later TODO (Sachin) : + interface_publisher_->msg_.commands.values.at(i) = + static_cast(command_interfaces_.at(i).get_value()); } interface_publisher_->unlockAndPublish(); } } rclcpp_action::GoalResponse IOGripperController::handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) +{ + try { - try - { - if (reconfigureFlag_.load()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); - return rclcpp_action::GoalResponse::REJECT; - } - gripper_service_buffer_.writeFromNonRT((goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); - } - catch (const std::exception & e) + if (reconfigureFlag_.load()) { - RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); + RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); return rclcpp_action::GoalResponse::REJECT; } - (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + gripper_service_buffer_.writeFromNonRT( + (goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", + e.what()); + return rclcpp_action::GoalResponse::REJECT; + } + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} rclcpp_action::CancelResponse IOGripperController::handle_cancel( const std::shared_ptr goal_handle) - { +{ (void)goal_handle; - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); - gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); - return rclcpp_action::CancelResponse::ACCEPT; - } + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + return rclcpp_action::CancelResponse::ACCEPT; +} void IOGripperController::handle_accepted(const std::shared_ptr goal_handle) { - std::thread{std::bind(&IOGripperController::execute, this, std::placeholders::_1), goal_handle}.detach(); - + std::thread{std::bind(&IOGripperController::execute, this, std::placeholders::_1), goal_handle} + .detach(); } void IOGripperController::execute(std::shared_ptr goal_handle) @@ -1126,50 +1152,51 @@ void IOGripperController::execute(std::shared_ptr goal_handle goal_handle->abort(result); break; } - else + else { - feedback->state = static_cast (*(gripper_state_buffer_.readFromRT())); + feedback->state = static_cast(*(gripper_state_buffer_.readFromRT())); goal_handle->publish_feedback(feedback); } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } rclcpp_action::GoalResponse IOGripperController::config_handle_goal( - const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal) - { - try + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) +{ + try { std::string conf = goal->config_name; configure_gripper_buffer_.writeFromNonRT(conf.c_str()); reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); reconfigureFlag_.store(true); - } catch (const std::exception & e) { - RCLCPP_ERROR(get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", e.what()); + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", + e.what()); return rclcpp_action::GoalResponse::REJECT; - } (void)uuid; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - } +} rclcpp_action::CancelResponse IOGripperController::config_handle_cancel( const std::shared_ptr goal_handle) - { +{ (void)goal_handle; configure_gripper_buffer_.writeFromNonRT(""); reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); return rclcpp_action::CancelResponse::ACCEPT; +} - } - -void IOGripperController::config_handle_accepted(const std::shared_ptr goal_handle) +void IOGripperController::config_handle_accepted( + const std::shared_ptr goal_handle) { -std::thread{std::bind(&IOGripperController::config_execute, this, std::placeholders::_1), goal_handle}.detach(); + std::thread{ + std::bind(&IOGripperController::config_execute, this, std::placeholders::_1), goal_handle} + .detach(); } void IOGripperController::config_execute(std::shared_ptr goal_handle) @@ -1178,21 +1205,20 @@ void IOGripperController::config_execute(std::shared_ptr(); while (true) { - if(*(reconfigure_state_buffer_.readFromRT()) == reconfigure_state_type::IDLE) + if (*(reconfigure_state_buffer_.readFromRT()) == reconfigure_state_type::IDLE) { result->result = true; result->status = "Gripper reconfigured"; goal_handle->succeed(result); break; } - else + else { - feedback->state = static_cast (*(reconfigure_state_buffer_.readFromRT())); + feedback->state = static_cast(*(reconfigure_state_buffer_.readFromRT())); goal_handle->publish_feedback(feedback); } std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - } void IOGripperController::check_gripper_and_reconfigure_state() @@ -1207,13 +1233,15 @@ void IOGripperController::check_gripper_and_reconfigure_state() RCLCPP_ERROR( get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); } - else { + else + { if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) { gripper_state_found = true; } - else { - gripper_state_found = false; + else + { + gripper_state_found = false; } } } @@ -1237,12 +1265,14 @@ void IOGripperController::check_gripper_and_reconfigure_state() RCLCPP_ERROR( get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); } - else { + else + { if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) { gripper_state_found = true; } - else { + else + { gripper_state_found = false; break; } @@ -1253,15 +1283,16 @@ void IOGripperController::check_gripper_and_reconfigure_state() setResult = find_and_get_state(low_val, state_value_); if (!setResult) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); } - else { + else + { if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) { gripper_state_found = true; } - else { + else + { gripper_state_found = false; break; } @@ -1270,9 +1301,12 @@ void IOGripperController::check_gripper_and_reconfigure_state() if (gripper_state_found) { - for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(state_name).joint_states.size(); ++i) + for (size_t i = 0; + i < params_.close.state.possible_closed_states_map.at(state_name).joint_states.size(); + ++i) { - joint_state_values_[i] = params_.close.state.possible_closed_states_map.at(state_name).joint_states[i]; + joint_state_values_[i] = + params_.close.state.possible_closed_states_map.at(state_name).joint_states[i]; } break; } @@ -1289,19 +1323,18 @@ void IOGripperController::check_gripper_and_reconfigure_state() if (!setResult) { reconfigure_state_found = false; - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); } - else + else { - if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) + if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) { reconfigure_state_found = false; - RCLCPP_ERROR( - get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); break; } - else { + else + { reconfigure_state_found = true; } } @@ -1312,19 +1345,17 @@ void IOGripperController::check_gripper_and_reconfigure_state() setResult = find_and_get_state(io, state_value_); if (!setResult) { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); } else - { + { if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) { - RCLCPP_ERROR( - get_node()->get_logger(), "value doesn't match %s", io.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "value doesn't match %s", io.c_str()); reconfigure_state_found = false; break; } - else + else { reconfigure_state_found = true; } @@ -1334,7 +1365,7 @@ void IOGripperController::check_gripper_and_reconfigure_state() { for (size_t i = 0; i < val.joint_states.size(); ++i) { - joint_state_values_[i + params_.open_close_joints.size()] = val.joint_states[i]; + joint_state_values_[i + params_.open_close_joints.size()] = val.joint_states[i]; } break; } diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml index 3af2f14f18..af4bf6d8d6 100644 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -201,7 +201,7 @@ io_gripper_controller: unique<>: null, } } - + configurations: { type: string_array, default_value: [], diff --git a/io_gripper_controller/test/test_io_gripper_controller.cpp b/io_gripper_controller/test/test_io_gripper_controller.cpp index 89aafa421c..6ba836e79e 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller.cpp @@ -26,8 +26,7 @@ #include #include - -// joint +// joint // joint states // dynamic state msg to status // action server @@ -35,7 +34,7 @@ // open gripper error when not expected behavior // set_before and set_after commands // add test for action and service open/close -// +// int main(int argc, char ** argv) { @@ -45,4 +44,3 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return result; } - diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index bc7278b44b..fd10c70c66 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -28,14 +28,14 @@ #include #include -#include "io_gripper_controller/io_gripper_controller.hpp" #include "gmock/gmock.h" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "io_gripper_controller/io_gripper_controller.hpp" +#include "rclcpp/executor.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" -#include "rclcpp/executor.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -43,7 +43,8 @@ using JointStateMsg = io_gripper_controller::IOGripperController::JointStateMsg; using OpenCloseSrvType = io_gripper_controller::IOGripperController::OpenCloseSrvType; using ControllerModeSrvType = io_gripper_controller::IOGripperController::ControllerModeSrvType; -using ConfigSrvType = io_gripper_controller::IOGripperController::ConfigSrvType; // control_msgs::srv::SetIOGripperConfig; +using ConfigSrvType = io_gripper_controller::IOGripperController:: + ConfigSrvType; // control_msgs::srv::SetIOGripperConfig; using GripperAction = io_gripper_controller::IOGripperController::GripperAction; using GoalHandleGripperAction = rclcpp_action::ClientGoalHandle; @@ -67,7 +68,7 @@ class TestableIOGripperController : public io_gripper_controller::IOGripperContr FRIEND_TEST(IOGripperControllerTest, OpenCloseGripperAction); FRIEND_TEST(IOGripperControllerTest, ReconfigureGripperService); FRIEND_TEST(IOGripperControllerTest, ReconfigureGripperAction); - + FRIEND_TEST(IOGripperControllerTest, DefaultParametersNotSet); FRIEND_TEST(IOGripperControllerTest, OpeningCommandParametersNotSet); FRIEND_TEST(IOGripperControllerTest, ClosingCommandsParametersNotSet); @@ -77,7 +78,6 @@ class TestableIOGripperController : public io_gripper_controller::IOGripperContr FRIEND_TEST(IOGripperControllerTest, DifferentStatesParametersNotSet); FRIEND_TEST(IOGripperControllerTest, all_parameters_set_configure_success); - public: controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override @@ -99,16 +99,15 @@ class IOGripperControllerFixture : public ::testing::Test // initialize controller controller_ = std::make_unique(); - subscription_caller_node_ = std::make_shared("subscription_caller"); joint_state_sub_ = subscription_caller_node_->create_subscription( "/joint_states", 1, - [this](const JointStateMsg::SharedPtr msg) { + [this](const JointStateMsg::SharedPtr msg) + { joint_state_sub_msg_ = msg; RCLCPP_INFO(rclcpp::get_logger("test_io_gripper_controller"), "Received joint state"); }); - service_caller_node_ = std::make_shared("service_caller"); close_gripper_service_client_ = service_caller_node_->create_client( "/test_io_gripper_controller/gripper_close"); @@ -141,7 +140,7 @@ class IOGripperControllerFixture : public ::testing::Test // setting the command state interfaces manually std::vector command_itfs; - command_itfs.reserve(3); // TODO (Sachin) : change this some variable later + command_itfs.reserve(3); // TODO (Sachin) : change this some variable later command_itfs.emplace_back(greif_oeffen_wqg1_cmd_); command_itfs.emplace_back(greif_schliess_wqg2_cmd_); @@ -159,14 +158,16 @@ class IOGripperControllerFixture : public ::testing::Test state_itfs_.emplace_back(bau_teil_abfrage_bg06_state_); controller_->assign_interfaces(std::move(command_itfs), std::move(state_itfs_)); - } + } - std::shared_future>> callOpenGripperAction(rclcpp::Executor & executor) + std::shared_future>> + callOpenGripperAction(rclcpp::Executor & executor) { auto goal = GripperAction::Goal(); goal.open = true; - bool wait_for_server_ret = gripper_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); + bool wait_for_server_ret = + gripper_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); EXPECT_TRUE(wait_for_server_ret); if (!wait_for_server_ret) { @@ -199,7 +200,7 @@ class IOGripperControllerFixture : public ::testing::Test return result.get(); } - std::shared_ptr call_open_service(rclcpp::Executor & executor) + std::shared_ptr call_open_service(rclcpp::Executor & executor) { auto request = std::make_shared(); @@ -215,49 +216,66 @@ class IOGripperControllerFixture : public ::testing::Test return result.get(); } - + void setup_parameters() { controller_->get_node()->set_parameter({"use_action", false}); controller_->get_node()->set_parameter({"open_close_joints", open_close_joints}); controller_->get_node()->set_parameter({"open.joint_states", open_joint_states}); - controller_->get_node()->set_parameter({"open.set_before_command.high", open_set_before_command_high}); - controller_->get_node()->set_parameter({"open.set_before_command.low", open_set_before_command_low}); + controller_->get_node()->set_parameter( + {"open.set_before_command.high", open_set_before_command_high}); + controller_->get_node()->set_parameter( + {"open.set_before_command.low", open_set_before_command_low}); controller_->get_node()->set_parameter({"open.command.high", open_command_high}); controller_->get_node()->set_parameter({"open.command.low", open_command_low}); controller_->get_node()->set_parameter({"open.state.high", open_state_high}); controller_->get_node()->set_parameter({"open.state.low", open_state_low}); - controller_->get_node()->set_parameter({"open.set_after_command.high", open_set_after_command_high}); - controller_->get_node()->set_parameter({"open.set_after_command.low", open_set_after_command_low}); + controller_->get_node()->set_parameter( + {"open.set_after_command.high", open_set_after_command_high}); + controller_->get_node()->set_parameter( + {"open.set_after_command.low", open_set_after_command_low}); controller_->get_node()->set_parameter({"possible_closed_states", possible_closed_states}); controller_->get_node()->set_parameter({"close.joint_states", close_joint_states}); - controller_->get_node()->set_parameter({"close.set_before_command.high", close_set_before_command_high}); - controller_->get_node()->set_parameter({"close.set_before_command.low", close_set_before_command_low}); + controller_->get_node()->set_parameter( + {"close.set_before_command.high", close_set_before_command_high}); + controller_->get_node()->set_parameter( + {"close.set_before_command.low", close_set_before_command_low}); controller_->get_node()->set_parameter({"close.command.high", close_command_high}); controller_->get_node()->set_parameter({"close.command.low", close_command_low}); - controller_->get_node()->set_parameter({"close.state.empty_close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter( + {"close.state.empty_close.joint_states", close_joint_states}); controller_->get_node()->set_parameter({"close.state.empty_close.high", close_state_high}); controller_->get_node()->set_parameter({"close.state.empty_close.low", close_state_low}); - controller_->get_node()->set_parameter({"close.state.empty_close.set_after_command_high", close_set_after_command_high}); - controller_->get_node()->set_parameter({"close.state.empty_close.set_after_command_low", close_set_after_command_low}); - controller_->get_node()->set_parameter({"close.state.full_close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter( + {"close.state.empty_close.set_after_command_high", close_set_after_command_high}); + controller_->get_node()->set_parameter( + {"close.state.empty_close.set_after_command_low", close_set_after_command_low}); + controller_->get_node()->set_parameter( + {"close.state.full_close.joint_states", close_joint_states}); controller_->get_node()->set_parameter({"close.state.full_close.high", close_state_low}); controller_->get_node()->set_parameter({"close.state.full_close.low", close_state_high}); - controller_->get_node()->set_parameter({"close.state.full_close.set_after_command_high", close_set_after_command_high}); - controller_->get_node()->set_parameter({"close.state.full_close.set_after_command_low", close_set_after_command_low}); - + controller_->get_node()->set_parameter( + {"close.state.full_close.set_after_command_high", close_set_after_command_high}); + controller_->get_node()->set_parameter( + {"close.state.full_close.set_after_command_low", close_set_after_command_low}); controller_->get_node()->set_parameter({"configurations", configurations_list}); controller_->get_node()->set_parameter({"configuration_joints", configuration_joints}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.joint_states", stichmass_joint_states}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.command_high", stichmass_command_high}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.command_low", stichmass_command_low}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.state_high", stichmass_state_high}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.state_low", stichmass_state_low}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.joint_states", stichmass_joint_states}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.command_high", stichmass_command_high}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.command_low", stichmass_command_low}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.state_high", stichmass_state_high}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.state_low", stichmass_state_low}); controller_->get_node()->set_parameter({"gripper_specific_sensors", gripper_specific_sensors}); - controller_->get_node()->set_parameter({"sensors_interfaces.hohenabfrage.input", gripper_interfaces_input}); + controller_->get_node()->set_parameter( + {"sensors_interfaces.hohenabfrage.input", gripper_interfaces_input}); } void setup_parameters_fail() @@ -265,43 +283,60 @@ class IOGripperControllerFixture : public ::testing::Test controller_->get_node()->set_parameter({"use_action", false}); controller_->get_node()->set_parameter({"open_close_joints", ""}); controller_->get_node()->set_parameter({"open.joint_states", open_joint_states}); - controller_->get_node()->set_parameter({"open.set_before_command.high", open_set_before_command_high}); - controller_->get_node()->set_parameter({"open.set_before_command.low", open_set_before_command_low}); + controller_->get_node()->set_parameter( + {"open.set_before_command.high", open_set_before_command_high}); + controller_->get_node()->set_parameter( + {"open.set_before_command.low", open_set_before_command_low}); controller_->get_node()->set_parameter({"open.command.high", open_command_high}); controller_->get_node()->set_parameter({"open.command.low", open_command_low}); controller_->get_node()->set_parameter({"open.state.high", open_state_high}); controller_->get_node()->set_parameter({"open.state.low", open_state_low}); - controller_->get_node()->set_parameter({"open.set_after_command.high", open_set_after_command_high}); - controller_->get_node()->set_parameter({"open.set_after_command.low", open_set_after_command_low}); + controller_->get_node()->set_parameter( + {"open.set_after_command.high", open_set_after_command_high}); + controller_->get_node()->set_parameter( + {"open.set_after_command.low", open_set_after_command_low}); controller_->get_node()->set_parameter({"possible_closed_states", possible_closed_states}); controller_->get_node()->set_parameter({"close.joint_states", close_joint_states}); - controller_->get_node()->set_parameter({"close.set_before_command.high", close_set_before_command_high}); - controller_->get_node()->set_parameter({"close.set_before_command.low", close_set_before_command_low}); + controller_->get_node()->set_parameter( + {"close.set_before_command.high", close_set_before_command_high}); + controller_->get_node()->set_parameter( + {"close.set_before_command.low", close_set_before_command_low}); controller_->get_node()->set_parameter({"close.command.high", close_command_high}); controller_->get_node()->set_parameter({"close.command.low", close_command_low}); - controller_->get_node()->set_parameter({"close.state.empty_close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter( + {"close.state.empty_close.joint_states", close_joint_states}); controller_->get_node()->set_parameter({"close.state.empty_close.high", close_state_high}); controller_->get_node()->set_parameter({"close.state.empty_close.low", close_state_low}); - controller_->get_node()->set_parameter({"close.state.empty_close.set_after_command_high", close_set_after_command_high}); - controller_->get_node()->set_parameter({"close.state.empty_close.set_after_command_low", close_set_after_command_low}); - controller_->get_node()->set_parameter({"close.state.full_close.joint_states", close_joint_states}); + controller_->get_node()->set_parameter( + {"close.state.empty_close.set_after_command_high", close_set_after_command_high}); + controller_->get_node()->set_parameter( + {"close.state.empty_close.set_after_command_low", close_set_after_command_low}); + controller_->get_node()->set_parameter( + {"close.state.full_close.joint_states", close_joint_states}); controller_->get_node()->set_parameter({"close.state.full_close.high", close_state_low}); controller_->get_node()->set_parameter({"close.state.full_close.low", close_state_high}); - controller_->get_node()->set_parameter({"close.state.full_close.set_after_command_high", close_set_after_command_high}); - controller_->get_node()->set_parameter({"close.state.full_close.set_after_command_low", close_set_after_command_low}); - + controller_->get_node()->set_parameter( + {"close.state.full_close.set_after_command_high", close_set_after_command_high}); + controller_->get_node()->set_parameter( + {"close.state.full_close.set_after_command_low", close_set_after_command_low}); controller_->get_node()->set_parameter({"configurations", configurations_list}); controller_->get_node()->set_parameter({"configuration_joints", configuration_joints}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.joint_states", stichmass_joint_states}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.command_high", stichmass_command_high}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.command_low", stichmass_command_low}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.state_high", stichmass_state_high}); - controller_->get_node()->set_parameter({"configuration_setup.stichmass_125.state_low", stichmass_state_low}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.joint_states", stichmass_joint_states}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.command_high", stichmass_command_high}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.command_low", stichmass_command_low}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.state_high", stichmass_state_high}); + controller_->get_node()->set_parameter( + {"configuration_setup.stichmass_125.state_low", stichmass_state_low}); controller_->get_node()->set_parameter({"gripper_specific_sensors", gripper_specific_sensors}); - controller_->get_node()->set_parameter({"sensors_interfaces.hohenabfrage.input", gripper_interfaces_input}); + controller_->get_node()->set_parameter( + {"sensors_interfaces.hohenabfrage.input", gripper_interfaces_input}); } protected: @@ -341,11 +376,6 @@ class IOGripperControllerFixture : public ::testing::Test std::vector gripper_specific_sensors = {"hohenabfrage"}; std::string gripper_interfaces_input = {"EL1008/Hohenabfrage_BG5"}; - - - - - std::vector joint_names_ = {"gripper_joint", "finger_joint"}; std::vector state_joint_names_ = {"gripper_joint"}; std::string interface_name_ = "gpio"; @@ -357,26 +387,37 @@ class IOGripperControllerFixture : public ::testing::Test std::array joint_command_opened = {1.0, 0.0}; std::array joint_command_closed = {0.0, 0.0}; - hardware_interface::CommandInterface joint_1_gpio_cmd_{joint_names_[0], interface_name_, &joint_command_values_[0]}; - hardware_interface::CommandInterface joint_2_gpio_cmd_{joint_names_[1], interface_name_, &joint_command_values_[1]}; + hardware_interface::CommandInterface joint_1_gpio_cmd_{ + joint_names_[0], interface_name_, &joint_command_values_[0]}; + hardware_interface::CommandInterface joint_2_gpio_cmd_{ + joint_names_[1], interface_name_, &joint_command_values_[1]}; std::array command_ios_values_ = {0.0, 1.0, 0.0, 0.0, 0.0}; std::array state_ios_values_ = {1.0, 0.0, 1.0, 0.0, 1.0}; - hardware_interface::CommandInterface greif_oeffen_wqg1_cmd_{"EL2008", "Greiferteil_Oeffnen_WQG1", &command_ios_values_[0]}; - hardware_interface::CommandInterface greif_schliess_wqg2_cmd_{"EL2008", "Greiferteil_Schliessen_WQG2", &command_ios_values_[1]}; - hardware_interface::CommandInterface bremse_wqg7_cmd_{"EL2008", "Bremse_WQG7", &command_ios_values_[2]}; - hardware_interface::CommandInterface stich_125_wqg5_cmd_{"EL2008", "Stichmass_125_WQG5", &command_ios_values_[3]}; - hardware_interface::CommandInterface stich_250_wqg6_cmd_{"EL2008", "Stichmass_250_WQG6", &command_ios_values_[4]}; - - hardware_interface::StateInterface greif_geoff_bg01_state_{"EL1008", "Greifer_Geoeffnet_BG01", &state_ios_values_[0]}; - hardware_interface::StateInterface greif_geschl_bg02_state_{"EL1008", "Greifer_Geschloschen_BG02", &state_ios_values_[1]}; - hardware_interface::StateInterface stich_125_bg03_state_{"EL1008", "Stichmass_125mm_BG03", &state_ios_values_[2]}; - hardware_interface::StateInterface stich_250_bg04_state_{"EL1008", "Stichmass_250mm_BG04", &state_ios_values_[3]}; - hardware_interface::StateInterface bau_teil_abfrage_bg06_state_{"EL1008", "Bauteilabfrage_BG06", &state_ios_values_[4]}; + hardware_interface::CommandInterface greif_oeffen_wqg1_cmd_{ + "EL2008", "Greiferteil_Oeffnen_WQG1", &command_ios_values_[0]}; + hardware_interface::CommandInterface greif_schliess_wqg2_cmd_{ + "EL2008", "Greiferteil_Schliessen_WQG2", &command_ios_values_[1]}; + hardware_interface::CommandInterface bremse_wqg7_cmd_{ + "EL2008", "Bremse_WQG7", &command_ios_values_[2]}; + hardware_interface::CommandInterface stich_125_wqg5_cmd_{ + "EL2008", "Stichmass_125_WQG5", &command_ios_values_[3]}; + hardware_interface::CommandInterface stich_250_wqg6_cmd_{ + "EL2008", "Stichmass_250_WQG6", &command_ios_values_[4]}; + + hardware_interface::StateInterface greif_geoff_bg01_state_{ + "EL1008", "Greifer_Geoeffnet_BG01", &state_ios_values_[0]}; + hardware_interface::StateInterface greif_geschl_bg02_state_{ + "EL1008", "Greifer_Geschloschen_BG02", &state_ios_values_[1]}; + hardware_interface::StateInterface stich_125_bg03_state_{ + "EL1008", "Stichmass_125mm_BG03", &state_ios_values_[2]}; + hardware_interface::StateInterface stich_250_bg04_state_{ + "EL1008", "Stichmass_250mm_BG04", &state_ios_values_[3]}; + hardware_interface::StateInterface bau_teil_abfrage_bg06_state_{ + "EL1008", "Bauteilabfrage_BG06", &state_ios_values_[4]}; JointStateMsg::SharedPtr joint_state_sub_msg_ = std::make_shared(); - // Test related parameters std::unique_ptr controller_; @@ -389,7 +430,6 @@ class IOGripperControllerFixture : public ::testing::Test rclcpp::Node::SharedPtr subscription_caller_node_, service_caller_node_, action_caller_node_; }; - class IOGripperControllerTest : public IOGripperControllerFixture { }; diff --git a/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp index f2f90d7711..ce76fa2847 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp @@ -17,8 +17,8 @@ // [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. // -#include "test_io_gripper_controller.hpp" #include "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.hpp" #include #include @@ -26,8 +26,6 @@ #include #include - - // Test setting all params and getting success TEST_F(IOGripperControllerTest, AllParamsSetSuccess) { @@ -41,7 +39,6 @@ TEST_F(IOGripperControllerTest, AllParamsSetSuccess) controller_interface::CallbackReturn::SUCCESS); } - // Test not setting the one param and getting failure TEST_F(IOGripperControllerTest, AllParamNotSetFailure) { @@ -53,4 +50,4 @@ TEST_F(IOGripperControllerTest, AllParamNotSetFailure) ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::FAILURE); -} \ No newline at end of file +} diff --git a/io_gripper_controller/test/test_io_gripper_controller_close.cpp b/io_gripper_controller/test/test_io_gripper_controller_close.cpp index a28eb29e09..e07f196c59 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_close.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_close.cpp @@ -17,8 +17,8 @@ // [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. // -#include "test_io_gripper_controller.hpp" #include "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.hpp" #include #include @@ -26,7 +26,6 @@ #include #include - // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, CloseGripperService) { @@ -53,44 +52,55 @@ TEST_F(IOGripperControllerTest, CloseGripperService) } auto future = close_gripper_service_client_->async_send_request(request); - std::thread spinner([&executor, &future]() { - executor.spin_until_future_complete(future); - }); + std::thread spinner([&executor, &future]() { executor.spin_until_future_complete(future); }); spinner.detach(); std::this_thread::sleep_for(std::chrono::milliseconds(5000)); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CLOSE_GRIPPER); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::CLOSE_GRIPPER); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); ASSERT_EQ(future.get()->success, true); // update to make sure the publisher value is updated ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - executor.spin_some(std::chrono::milliseconds(2000)); // this solve the issue related to subscriber not able to get the message + executor.spin_some(std::chrono::milliseconds( + 2000)); // this solve the issue related to subscriber not able to get the message std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // since update doesn't guarantee a published message, republish until received @@ -111,7 +121,8 @@ TEST_F(IOGripperControllerTest, CloseGripperService) break; } } - executor.spin_some(std::chrono::milliseconds(2000)); // this solve the issue related to subscriber not able to get the message + executor.spin_some(std::chrono::milliseconds( + 2000)); // this solve the issue related to subscriber not able to get the message std::this_thread::sleep_for(std::chrono::milliseconds(2000)); ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " "controller/broadcaster update loop"; @@ -120,6 +131,10 @@ TEST_F(IOGripperControllerTest, CloseGripperService) EXPECT_EQ(joint_state_sub_msg_->position.size(), 2); EXPECT_EQ(joint_state_sub_msg_->position[0], 0.08); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); -} \ No newline at end of file + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::IDLE); +} diff --git a/io_gripper_controller/test/test_io_gripper_controller_open.cpp b/io_gripper_controller/test/test_io_gripper_controller_open.cpp index a65ff9b06e..1f5099cd73 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open.cpp @@ -17,9 +17,8 @@ // [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. // - -#include "test_io_gripper_controller.hpp" #include "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.hpp" #include #include @@ -27,7 +26,6 @@ #include #include - // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, OpenGripperService) { @@ -54,41 +52,50 @@ TEST_F(IOGripperControllerTest, OpenGripperService) } auto future = open_gripper_service_client_->async_send_request(request); - std::thread spinner([&executor, &future]() { - executor.spin_until_future_complete(future); - }); + std::thread spinner([&executor, &future]() { executor.spin_until_future_complete(future); }); spinner.detach(); std::this_thread::sleep_for(std::chrono::milliseconds(5000)); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::OPEN_GRIPPER); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::OPEN_GRIPPER); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); ASSERT_EQ(future.get()->success, true); - // since update doesn't guarantee a published message, republish until received int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop while (max_sub_check_loop_count--) @@ -113,7 +120,11 @@ TEST_F(IOGripperControllerTest, OpenGripperService) ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); ASSERT_EQ(joint_state_sub_msg_->position[0], 0.0); - - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); -} \ No newline at end of file + + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::IDLE); +} diff --git a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp index 2c2836e254..9e3b1aca84 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp @@ -17,8 +17,8 @@ // [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. // -#include "test_io_gripper_controller.hpp" #include "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.hpp" #include #include @@ -26,7 +26,6 @@ #include #include - // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, OpenCloseGripperAction) { @@ -46,7 +45,8 @@ TEST_F(IOGripperControllerTest, OpenCloseGripperAction) auto goal = std::make_shared(); - bool wait_for_action_ret = gripper_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); + bool wait_for_action_ret = + gripper_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); EXPECT_TRUE(wait_for_action_ret); if (!wait_for_action_ret) { @@ -60,38 +60,55 @@ TEST_F(IOGripperControllerTest, OpenCloseGripperAction) executor.spin_some(std::chrono::milliseconds(5000)); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::OPEN_GRIPPER); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::OPEN_GRIPPER); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::OPEN); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::OPEN); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::IDLE); // update to make sure the publisher value is updated ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + controller_interface::return_type::OK); + executor.spin_some(std::chrono::milliseconds( + 1000)); // this solve the issue related to subscriber not able to get the message std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // since update doesn't guarantee a published message, republish until received @@ -112,7 +129,8 @@ TEST_F(IOGripperControllerTest, OpenCloseGripperAction) break; } } - executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + executor.spin_some(std::chrono::milliseconds( + 1000)); // this solve the issue related to subscriber not able to get the message std::this_thread::sleep_for(std::chrono::milliseconds(1000)); ASSERT_TRUE(joint_state_sub_msg_); @@ -128,38 +146,55 @@ TEST_F(IOGripperControllerTest, OpenCloseGripperAction) executor.spin_some(std::chrono::milliseconds(5000)); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CLOSE_GRIPPER); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::CLOSE_GRIPPER); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::CHECK_GRIPPER_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::CLOSE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::CLOSE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::SET_AFTER_COMMAND); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->gripper_service_buffer_.readFromRT()), io_gripper_controller::service_mode_type::IDLE); - ASSERT_EQ(*(controller_->gripper_state_buffer_.readFromRT()), io_gripper_controller::gripper_state_type::IDLE); + ASSERT_EQ( + *(controller_->gripper_service_buffer_.readFromRT()), + io_gripper_controller::service_mode_type::IDLE); + ASSERT_EQ( + *(controller_->gripper_state_buffer_.readFromRT()), + io_gripper_controller::gripper_state_type::IDLE); // update to make sure the publisher value is updated ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + executor.spin_some(std::chrono::milliseconds( + 1000)); // this solve the issue related to subscriber not able to get the message std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // since update doesn't guarantee a published message, republish until received @@ -180,10 +215,11 @@ TEST_F(IOGripperControllerTest, OpenCloseGripperAction) break; } } - executor.spin_some(std::chrono::milliseconds(2000)); // this solve the issue related to subscriber not able to get the message + executor.spin_some(std::chrono::milliseconds( + 2000)); // this solve the issue related to subscriber not able to get the message std::this_thread::sleep_for(std::chrono::milliseconds(2000)); ASSERT_TRUE(joint_state_sub_msg_); ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); EXPECT_EQ(joint_state_sub_msg_->position[0], 0.08); -} \ No newline at end of file +} diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp index 41c410225c..f3b0b26ea0 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp @@ -17,8 +17,8 @@ // [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. // -#include "test_io_gripper_controller.hpp" #include "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.hpp" #include #include @@ -26,7 +26,6 @@ #include #include - // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, ReconfigureGripperService) { @@ -53,34 +52,38 @@ TEST_F(IOGripperControllerTest, ReconfigureGripperService) } auto future = configure_gripper_service_client_->async_send_request(request); - std::thread spinner([&executor, &future]() { - executor.spin_until_future_complete(future); - }); + std::thread spinner([&executor, &future]() { executor.spin_until_future_complete(future); }); spinner.detach(); std::this_thread::sleep_for(std::chrono::milliseconds(5000)); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::SET_COMMAND); + ASSERT_EQ( + *(controller_->reconfigure_state_buffer_.readFromRT()), + io_gripper_controller::reconfigure_state_type::SET_COMMAND); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::CHECK_STATE); + ASSERT_EQ( + *(controller_->reconfigure_state_buffer_.readFromRT()), + io_gripper_controller::reconfigure_state_type::CHECK_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::IDLE); + ASSERT_EQ( + *(controller_->reconfigure_state_buffer_.readFromRT()), + io_gripper_controller::reconfigure_state_type::IDLE); ASSERT_EQ(future.get()->result, true); @@ -108,5 +111,4 @@ TEST_F(IOGripperControllerTest, ReconfigureGripperService) ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); ASSERT_EQ(joint_state_sub_msg_->position[1], 0.125); - -} \ No newline at end of file +} diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp index 94a7c10966..f7588ebb5b 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp @@ -17,8 +17,8 @@ // [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. // -#include "test_io_gripper_controller.hpp" #include "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.hpp" #include #include @@ -26,14 +26,13 @@ #include #include - // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, ReconfigureGripperAction) { SetUpController(); setup_parameters(); - + controller_->get_node()->set_parameter({"use_action", true}); rclcpp::executors::MultiThreadedExecutor executor; @@ -46,7 +45,8 @@ TEST_F(IOGripperControllerTest, ReconfigureGripperAction) auto goal = std::make_shared(); - bool wait_for_action_ret = gripper_config_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); + bool wait_for_action_ret = + gripper_config_action_client_->wait_for_action_server(std::chrono::milliseconds(500)); EXPECT_TRUE(wait_for_action_ret); if (!wait_for_action_ret) { @@ -55,35 +55,40 @@ TEST_F(IOGripperControllerTest, ReconfigureGripperAction) goal->config_name = "stichmass_125"; - gripper_config_action_client_->async_send_goal(*goal); executor.spin_some(std::chrono::milliseconds(5000)); ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::SET_COMMAND); + ASSERT_EQ( + *(controller_->reconfigure_state_buffer_.readFromRT()), + io_gripper_controller::reconfigure_state_type::SET_COMMAND); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::CHECK_STATE); - + ASSERT_EQ( + *(controller_->reconfigure_state_buffer_.readFromRT()), + io_gripper_controller::reconfigure_state_type::CHECK_STATE); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); + controller_interface::return_type::OK); - ASSERT_EQ(*(controller_->reconfigure_state_buffer_.readFromRT()), io_gripper_controller::reconfigure_state_type::IDLE); + ASSERT_EQ( + *(controller_->reconfigure_state_buffer_.readFromRT()), + io_gripper_controller::reconfigure_state_type::IDLE); - executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + executor.spin_some(std::chrono::milliseconds( + 1000)); // this solve the issue related to subscriber not able to get the message std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // since update doesn't guarantee a published message, republish until received @@ -104,12 +109,13 @@ TEST_F(IOGripperControllerTest, ReconfigureGripperAction) break; } } - executor.spin_some(std::chrono::milliseconds(1000)); // this solve the issue related to subscriber not able to get the message + executor.spin_some(std::chrono::milliseconds( + 1000)); // this solve the issue related to subscriber not able to get the message std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " - // "controller/broadcaster update loop"; + // "controller/broadcaster update loop"; ASSERT_TRUE(joint_state_sub_msg_); ASSERT_EQ(joint_state_sub_msg_->position.size(), 2); ASSERT_EQ(joint_state_sub_msg_->position[1], 0.125); -} \ No newline at end of file +} diff --git a/io_gripper_controller/test/test_load_io_gripper_controller.cpp b/io_gripper_controller/test/test_load_io_gripper_controller.cpp index d3ea9e8b1a..ffaa94f2b0 100644 --- a/io_gripper_controller/test/test_load_io_gripper_controller.cpp +++ b/io_gripper_controller/test/test_load_io_gripper_controller.cpp @@ -33,9 +33,8 @@ TEST(TestLoadIOGripperController, load_controller) executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); ASSERT_NE( - cm.load_controller( - "test_io_gripper_controller", "io_gripper_controller/IOGripperController"), + cm.load_controller("test_io_gripper_controller", "io_gripper_controller/IOGripperController"), nullptr); rclcpp::shutdown(); -} \ No newline at end of file +} From 128a0546177a16464da68e4a87352fa2709a1fa7 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 3 Jan 2025 02:21:47 +0100 Subject: [PATCH 27/75] user doc reformatted --- io_gripper_controller/doc/userdoc.rst | 145 +++++++++++++------------- 1 file changed, 73 insertions(+), 72 deletions(-) diff --git a/io_gripper_controller/doc/userdoc.rst b/io_gripper_controller/doc/userdoc.rst index 9fefd8566f..b51ae43000 100644 --- a/io_gripper_controller/doc/userdoc.rst +++ b/io_gripper_controller/doc/userdoc.rst @@ -5,13 +5,13 @@ io_gripper_controller ============================= -The IO Gripper Controller provides implementation to control the gripper using IOs. It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes `joint_states` of gripper and also `dynamic_interfaces` for all command and state interfaces. +The IO Gripper Controller provides implementation to control the gripper using IOs. It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes ``joint_states`` of gripper and also ``dynamic_interfaces`` for all command and state interfaces. Description of controller's interfaces --------------------------------------- -- `joint_states` [`sensor_msgs::msg::JointState`]: Publishes the state of gripper joint and configuration joint -- `dynamic_interfaces` [`control_msgs::msg::DynamicInterfaceValues`]: Publishes all command and state interface of the IOs and sensors of gripper. +- ``joint_states`` [``sensor_msgs::msg::JointState``]: Publishes the state of gripper joint and configuration joint +- ``dynamic_interfaces`` [``control_msgs::msg::DynamicInterfaceValues``]: Publishes all command and state interface of the IOs and sensors of gripper. Parameters @@ -27,73 +27,74 @@ This controller adds the following parameters: Example parameters .................... -``` -io_gripper_controller: - - ros__parameters: - - use_action: true - - open_close_joints: [gripper_clamp_jaw] - - open: - joint_states: [0.0] - set_before_command: - high: [EL2008/Bremse_WQG7] - low: [] - command: - high: [EL2008/Greiferteil_Oeffnen_WQG1] - low: [EL2008/Greiferteil_Schliessen_WQG2] - state: - high: [EL1008/Greifer_Geoeffnet_BG01] - low: [EL1008/Greifer_Geschloschen_BG02] - set_after_command: - high: [] - low: [EL2008/Bremse_WQG7] - - possible_closed_states: ['empty_close', 'full_close'] - - close: - set_before_command: - high: [EL2008/Bremse_WQG7] - low: [EL2008/Greiferteil_Oeffnen_WQG1] - command: - high: [EL2008/Greiferteil_Schliessen_WQG2] - low: [EL2008/Greiferteil_Oeffnen_WQG1] - state: - empty_close: - joint_states: [0.08] - high: [EL1008/Greifer_Geschloschen_BG02] - low: [EL1008/Bauteilabfrage_BG06] - set_after_command_high: [EL2008/Bremse_WQG7] - set_after_command_low: [EL2008/Bremse_WQG7] - full_close: - joint_states: [0.08] - high: [EL1008/Bauteilabfrage_BG06] +.. code-block:: yaml + + io_gripper_controller: + + ros__parameters: + + use_action: true + + open_close_joints: [gripper_clamp_jaw] + + open: + joint_states: [0.0] + set_before_command: + high: [EL2008/Bremse_WQG7] + low: [] + command: + high: [EL2008/Greiferteil_Oeffnen_WQG1] + low: [EL2008/Greiferteil_Schliessen_WQG2] + state: + high: [EL1008/Greifer_Geoeffnet_BG01] low: [EL1008/Greifer_Geschloschen_BG02] - set_after_command_high: [EL2008/Bremse_WQG7] - set_after_command_low: [EL2008/Bremse_WQG7] - - configurations: ["stichmass_125", "stichmass_250"] - configuration_joints: [gripper_gripper_distance_joint] - - configuration_setup: - stichmass_125: - joint_states: [0.125] - command_high: [EL2008/Stichmass_125_WQG5] - command_low: [EL2008/Stichmass_250_WQG6] - state_high: [EL1008/Stichmass_125mm_BG03] - state_low: [EL1008/Stichmass_250mm_BG04] - stichmass_250: - joint_states: [0.250] - command_high: [EL2008/Stichmass_250_WQG6] - command_low: [EL2008/Stichmass_125_WQG5] - state_high: [EL1008/Stichmass_250mm_BG04] - state_low: [EL1008/Stichmass_125mm_BG03] - - gripper_specific_sensors: ["hohenabfrage", "bauteilabfrage"] - sensors_interfaces: - hohenabfrage: - input: "EL1008/Hohenabfrage_BG5" - bauteilabfrage: - input: "EL1008/Bauteilabfrage_BG06" + set_after_command: + high: [] + low: [EL2008/Bremse_WQG7] + + possible_closed_states: ['empty_close', 'full_close'] + + close: + set_before_command: + high: [EL2008/Bremse_WQG7] + low: [EL2008/Greiferteil_Oeffnen_WQG1] + command: + high: [EL2008/Greiferteil_Schliessen_WQG2] + low: [EL2008/Greiferteil_Oeffnen_WQG1] + state: + empty_close: + joint_states: [0.08] + high: [EL1008/Greifer_Geschloschen_BG02] + low: [EL1008/Bauteilabfrage_BG06] + set_after_command_high: [EL2008/Bremse_WQG7] + set_after_command_low: [EL2008/Bremse_WQG7] + full_close: + joint_states: [0.08] + high: [EL1008/Bauteilabfrage_BG06] + low: [EL1008/Greifer_Geschloschen_BG02] + set_after_command_high: [EL2008/Bremse_WQG7] + set_after_command_low: [EL2008/Bremse_WQG7] + + configurations: ["stichmass_125", "stichmass_250"] + configuration_joints: [gripper_gripper_distance_joint] + + configuration_setup: + stichmass_125: + joint_states: [0.125] + command_high: [EL2008/Stichmass_125_WQG5] + command_low: [EL2008/Stichmass_250_WQG6] + state_high: [EL1008/Stichmass_125mm_BG03] + state_low: [EL1008/Stichmass_250mm_BG04] + stichmass_250: + joint_states: [0.250] + command_high: [EL2008/Stichmass_250_WQG6] + command_low: [EL2008/Stichmass_125_WQG5] + state_high: [EL1008/Stichmass_250mm_BG04] + state_low: [EL1008/Stichmass_125mm_BG03] + + gripper_specific_sensors: ["hohenabfrage", "bauteilabfrage"] + sensors_interfaces: + hohenabfrage: + input: "EL1008/Hohenabfrage_BG5" + bauteilabfrage: + input: "EL1008/Bauteilabfrage_BG06" From e15cd1e435bd5eebd0f442bcbfabb132df3a2c19 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:30:08 +0100 Subject: [PATCH 28/75] user doc updated Co-authored-by: Dr. Denis --- io_gripper_controller/doc/userdoc.rst | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/io_gripper_controller/doc/userdoc.rst b/io_gripper_controller/doc/userdoc.rst index b51ae43000..bdf45844a2 100644 --- a/io_gripper_controller/doc/userdoc.rst +++ b/io_gripper_controller/doc/userdoc.rst @@ -5,7 +5,9 @@ io_gripper_controller ============================= -The IO Gripper Controller provides implementation to control the gripper using IOs. It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes ``joint_states`` of gripper and also ``dynamic_interfaces`` for all command and state interfaces. +The IO Gripper Controller provides implementation to control the grippers that are commanded using IOs. +This is often the case for pneumatic gripper in the industy, that can range from simple parallel gripper up to custom, multi-dof grippers for manipulating specific parts. + It provides functionalities like open, close and reconfigure which can be used either though action server or service server and also publishes gripper's joint values if any and provides output for all gripper's command and state interfaces. Description of controller's interfaces --------------------------------------- From 4f0ff44f7e702b1644df966ed7f19184e19b991c Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:30:30 +0100 Subject: [PATCH 29/75] Update io_gripper_controller/doc/userdoc.rst Co-authored-by: Dr. Denis --- io_gripper_controller/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/doc/userdoc.rst b/io_gripper_controller/doc/userdoc.rst index bdf45844a2..427c74d068 100644 --- a/io_gripper_controller/doc/userdoc.rst +++ b/io_gripper_controller/doc/userdoc.rst @@ -12,7 +12,7 @@ This is often the case for pneumatic gripper in the industy, that can range from Description of controller's interfaces --------------------------------------- -- ``joint_states`` [``sensor_msgs::msg::JointState``]: Publishes the state of gripper joint and configuration joint +- ``joint_states`` [``sensor_msgs::msg::JointState``]: Publishes the state of gripper's open/close joint if any and configuration joints that might influece the geometry and kinematics of the gripper. - ``dynamic_interfaces`` [``control_msgs::msg::DynamicInterfaceValues``]: Publishes all command and state interface of the IOs and sensors of gripper. From 94ca4bb9707dd5b0912987fa67c5b56a8ce09173 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:31:38 +0100 Subject: [PATCH 30/75] params updated Co-authored-by: Dr. Denis --- io_gripper_controller/doc/userdoc.rst | 76 +++++++++++++-------------- 1 file changed, 38 insertions(+), 38 deletions(-) diff --git a/io_gripper_controller/doc/userdoc.rst b/io_gripper_controller/doc/userdoc.rst index 427c74d068..585d2c3754 100644 --- a/io_gripper_controller/doc/userdoc.rst +++ b/io_gripper_controller/doc/userdoc.rst @@ -42,61 +42,61 @@ Example parameters open: joint_states: [0.0] set_before_command: - high: [EL2008/Bremse_WQG7] + high: [Release_Break_valve] low: [] command: - high: [EL2008/Greiferteil_Oeffnen_WQG1] - low: [EL2008/Greiferteil_Schliessen_WQG2] + high: [Open_valve] + low: [Close_valve] state: - high: [EL1008/Greifer_Geoeffnet_BG01] - low: [EL1008/Greifer_Geschloschen_BG02] + high: [Opened_signal] + low: [Closed_signal] set_after_command: high: [] - low: [EL2008/Bremse_WQG7] + low: [Release_Break_valve] possible_closed_states: ['empty_close', 'full_close'] close: set_before_command: - high: [EL2008/Bremse_WQG7] - low: [EL2008/Greiferteil_Oeffnen_WQG1] + high: [Release_Break_valve] + low: [] command: - high: [EL2008/Greiferteil_Schliessen_WQG2] - low: [EL2008/Greiferteil_Oeffnen_WQG1] + high: [Close_valve] + low: [Open_valve] state: empty_close: - joint_states: [0.08] - high: [EL1008/Greifer_Geschloschen_BG02] - low: [EL1008/Bauteilabfrage_BG06] - set_after_command_high: [EL2008/Bremse_WQG7] - set_after_command_low: [EL2008/Bremse_WQG7] + joint_states: [0.16] + high: [Closed_signal] + low: [Part_Grasped_signal] + set_after_command_high: [] + set_after_command_low: [Release_Break_valve] full_close: joint_states: [0.08] - high: [EL1008/Bauteilabfrage_BG06] - low: [EL1008/Greifer_Geschloschen_BG02] - set_after_command_high: [EL2008/Bremse_WQG7] - set_after_command_low: [EL2008/Bremse_WQG7] + high: [Part_Grasped_signal] + low: [Closed_signal] + set_after_command_high: [] + set_after_command_low: [Release_Break_valve] - configurations: ["stichmass_125", "stichmass_250"] + configurations: ["narrow_objects", "wide_objects"] configuration_joints: [gripper_gripper_distance_joint] configuration_setup: - stichmass_125: - joint_states: [0.125] - command_high: [EL2008/Stichmass_125_WQG5] - command_low: [EL2008/Stichmass_250_WQG6] - state_high: [EL1008/Stichmass_125mm_BG03] - state_low: [EL1008/Stichmass_250mm_BG04] - stichmass_250: - joint_states: [0.250] - command_high: [EL2008/Stichmass_250_WQG6] - command_low: [EL2008/Stichmass_125_WQG5] - state_high: [EL1008/Stichmass_250mm_BG04] - state_low: [EL1008/Stichmass_125mm_BG03] - - gripper_specific_sensors: ["hohenabfrage", "bauteilabfrage"] + narrow_objects: + joint_states: [0.1] + command_high: [Narrow_Configuration_Cmd] + command_low: [Wide_Configuration_Cmd] + state_high: [Narrow_Configuraiton_Signal] + state_low: [Wide_Configuration_Signal] + wide_objects: + joint_states: [0.2] + command_high: [Wide_Configuration_Cmd] + command_low: [Narrow_Configuration_Cmd] + state_high: [Wide_Configuration_Signal] + state_low: [Narrow_Configuraiton_Signal] + + gripper_specific_sensors: ["part_sensor_top", "part_sensor"] sensors_interfaces: - hohenabfrage: - input: "EL1008/Hohenabfrage_BG5" - bauteilabfrage: - input: "EL1008/Bauteilabfrage_BG06" + part_sensor_top: + input: "Part_Sensor_Top_signal" + part_sensor: + input: "Part_Grasped_signal" From efbc005de4b627163a5701356e513fedeaf39396 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:32:10 +0100 Subject: [PATCH 31/75] copyright update Co-authored-by: Dr. Denis --- .../include/io_gripper_controller/io_gripper_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 5b494cbcd7..fd10aca446 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b>>robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From c875effbca3f4b92e332b483e04ea2505d5006ff Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:33:08 +0100 Subject: [PATCH 32/75] sort headers alphabatically Co-authored-by: Dr. Denis --- .../include/io_gripper_controller/io_gripper_controller.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index fd10aca446..e35db23280 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -30,9 +30,7 @@ #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" - -#include - +#include sensor_msgs/msg/joint_state.hpp #include "control_msgs/action/io_gripper_command.hpp" #include "control_msgs/action/set_io_gripper_config.hpp" #include "control_msgs/msg/dynamic_interface_values.hpp" From b7d882b5a4b3bc84099fa783bdb930f9dd77466b Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 10 Jan 2025 00:46:45 +0100 Subject: [PATCH 33/75] sort headers alphabatically and remove find_config state --- .../io_gripper_controller.hpp | 23 ++++++++----------- .../src/io_gripper_controller.cpp | 12 ++++------ 2 files changed, 15 insertions(+), 20 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index e35db23280..b3c39ae8a3 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -22,25 +22,24 @@ #include #include +#include "control_msgs/action/io_gripper_command.hpp" +#include "control_msgs/action/set_io_gripper_config.hpp" +#include "control_msgs/msg/dynamic_interface_values.hpp" +#include "control_msgs/msg/interface_value.hpp" +#include "control_msgs/srv/set_io_gripper_config.hpp" #include "controller_interface/controller_interface.hpp" #include "io_gripper_controller_parameters.hpp" +#include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" +#include "sensor_msgs/msg/joint_state.hpp" #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" -#include sensor_msgs/msg/joint_state.hpp -#include "control_msgs/action/io_gripper_command.hpp" -#include "control_msgs/action/set_io_gripper_config.hpp" -#include "control_msgs/msg/dynamic_interface_values.hpp" -#include "control_msgs/msg/interface_value.hpp" -#include "control_msgs/srv/set_io_gripper_config.hpp" -#include "rclcpp_action/rclcpp_action.hpp" namespace io_gripper_controller { - /** * @enum service_mode_type * @brief Represents the various service modes of the gripper. These modes represents the high level @@ -87,16 +86,14 @@ enum class gripper_state_type : std::uint8_t * is reconfiguring to new state based on the configuration defined in the yaml params. * * - IDLE: The reconfiguration process is idle, not performing any action. - * - FIND_CONFIG: Finding the configuration settings. * - SET_COMMAND: Setting the command based on the configuration. * - CHECK_STATE: Checking the state after setting the command. */ enum class reconfigure_state_type : std::uint8_t { IDLE = 0, - FIND_CONFIG = 1, - SET_COMMAND = 2, - CHECK_STATE = 3, + SET_COMMAND = 1, + CHECK_STATE = 2, }; class IOGripperController : public controller_interface::ControllerInterface @@ -217,7 +214,7 @@ class IOGripperController : public controller_interface::ControllerInterface realtime_tools::RealtimeBuffer gripper_state_buffer_; // Realtime buffer to store the name of the configuration which needs to be set realtime_tools::RealtimeBuffer configure_gripper_buffer_; - // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, find_config, + // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, // set_command, check_state) realtime_tools::RealtimeBuffer reconfigure_state_buffer_; diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 2cdcf49806..7595909cc1 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -439,22 +439,20 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ case reconfigure_state_type::IDLE: // do nothing break; - case reconfigure_state_type::FIND_CONFIG: + case reconfigure_state_type::SET_COMMAND: config_index_ = std::find(configurations_list_.begin(), configurations_list_.end(), configuration_key_); if (config_index_ == configurations_list_.end()) { RCLCPP_ERROR(get_node()->get_logger(), "Configuration not found"); reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); + break; } else { conf_it_ = config_map_[std::distance(configurations_list_.begin(), config_index_)]; reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); } - break; - - case reconfigure_state_type::SET_COMMAND: setResult = false; for (const auto & io : conf_it_.command_high) { @@ -947,7 +945,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and { std::string conf = request->config_name; configure_gripper_buffer_.writeFromNonRT(conf.c_str()); - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); reconfigureFlag_.store(true); while (reconfigureFlag_.load()) { @@ -1168,7 +1166,7 @@ rclcpp_action::GoalResponse IOGripperController::config_handle_goal( { std::string conf = goal->config_name; configure_gripper_buffer_.writeFromNonRT(conf.c_str()); - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); reconfigureFlag_.store(true); } catch (const std::exception & e) @@ -1187,7 +1185,7 @@ rclcpp_action::CancelResponse IOGripperController::config_handle_cancel( { (void)goal_handle; configure_gripper_buffer_.writeFromNonRT(""); - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::FIND_CONFIG); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); return rclcpp_action::CancelResponse::ACCEPT; } From 66ae103d0cad42f28bf67783ba9b423dec74a64e Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 10 Jan 2025 00:47:14 +0100 Subject: [PATCH 34/75] remove set_command state asset --- .../test/test_io_gripper_controller_reconfigure.cpp | 8 -------- .../test_io_gripper_controller_reconfigure_action.cpp | 8 -------- 2 files changed, 16 deletions(-) diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp index f3b0b26ea0..109071ed0a 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp @@ -57,14 +57,6 @@ TEST_F(IOGripperControllerTest, ReconfigureGripperService) std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ASSERT_EQ( - *(controller_->reconfigure_state_buffer_.readFromRT()), - io_gripper_controller::reconfigure_state_type::SET_COMMAND); - ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp index f7588ebb5b..b9f71b1acb 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp @@ -59,14 +59,6 @@ TEST_F(IOGripperControllerTest, ReconfigureGripperAction) executor.spin_some(std::chrono::milliseconds(5000)); - ASSERT_EQ( - controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), - controller_interface::return_type::OK); - - ASSERT_EQ( - *(controller_->reconfigure_state_buffer_.readFromRT()), - io_gripper_controller::reconfigure_state_type::SET_COMMAND); - ASSERT_EQ( controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), controller_interface::return_type::OK); From 062e5f56754ddd6da0933c7cbff8a68b3ac118c0 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 17:28:59 +0100 Subject: [PATCH 35/75] copyright update Co-authored-by: Dr. Denis --- io_gripper_controller/src/io_gripper_controller.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml index af4bf6d8d6..b5c24df7f0 100644 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -1,4 +1,4 @@ -# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) +# Copyright (c) 2025, b>>robotized by Stogl Robotics # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From d1436bf094f23f16d4186a9e3987cb0bf54f8a04 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 17:29:37 +0100 Subject: [PATCH 36/75] not_empty validation added Co-authored-by: Dr. Denis --- io_gripper_controller/src/io_gripper_controller.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml index b5c24df7f0..3e74b94bd0 100644 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -29,6 +29,7 @@ io_gripper_controller: description: "List of joint names that should change values according to open or close state of the gripper", validation: { unique<>: null, + not_empty<>: null, } } open: From e20b4fab689062713053fb18858a8b8b1ba9f8f4 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 17:29:59 +0100 Subject: [PATCH 37/75] not_empty validation added Co-authored-by: Dr. Denis --- io_gripper_controller/src/io_gripper_controller.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml index 3e74b94bd0..7f6258cfe6 100644 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -39,6 +39,7 @@ io_gripper_controller: description: "List of joint values when gripper is open. The order of the values should match the order of the joint names in open_close_joints", validation: { unique<>: null, + not_empty<>: null, } } set_before_command: From 80de84dbe2ee3e7b797dad575a47340ac64610f8 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 19:12:52 +0100 Subject: [PATCH 38/75] removed for merge upstream Co-authored-by: Dr. Denis --- .../src/io_gripper_controller.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 7595909cc1..7a7f93741f 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -26,23 +26,6 @@ #include "controller_interface/helpers.hpp" -namespace -{ // utility - -// TODO(destogl): remove this when merged upstream -// Changed services history QoS to keep all so we don't lose any client service calls -static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; - -} // namespace namespace io_gripper_controller { From 752766813b718f231c33ac3181d77dd53bddbf62 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 19:20:36 +0100 Subject: [PATCH 39/75] license update Co-authored-by: Dr. Denis --- io_gripper_controller/src/io_gripper_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 7a7f93741f..c73266a961 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b>>robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From e981d6a9465f07366a5438b609a7c3c0b63dacc5 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 19:21:07 +0100 Subject: [PATCH 40/75] removed unused code Co-authored-by: Dr. Denis --- io_gripper_controller/src/io_gripper_controller.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index c73266a961..bc9be3bc83 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -12,11 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - #include "io_gripper_controller/io_gripper_controller.hpp" #include From 5dcae089e3f50ecc4c3a2a4b49f4079b6e2865be Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 11:57:02 +0100 Subject: [PATCH 41/75] Fix services QoS --- .../src/io_gripper_controller.cpp | 34 ++++++++++++++++--- 1 file changed, 29 insertions(+), 5 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index bc9be3bc83..c25b6e4b50 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -20,7 +20,32 @@ #include #include "controller_interface/helpers.hpp" - +#include "rclcpp/version.h" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 17 +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); +#else +static const rmw_qos_profile_t qos_services = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; +#endif + +} // namespace namespace io_gripper_controller { @@ -866,8 +891,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and }; open_service_ = get_node()->create_service( - "~/gripper_open", open_service_callback, rmw_qos_profile_services_hist_keep_all, - open_service_callback_group_); + "~/gripper_open", open_service_callback, qos_services, open_service_callback_group_); // close service auto close_service_callback = [&]( @@ -940,8 +964,8 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and }; configure_gripper_service_ = get_node()->create_service( - "~/reconfigure_to", configure_gripper_service_callback, - rmw_qos_profile_services_hist_keep_all, reconfigure_service_callback_group_); + "~/reconfigure_to", configure_gripper_service_callback, qos_services, + reconfigure_service_callback_group_); } else { From 807c563d971faf2bdb41a4d85790f064542f5cf3 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 12:02:26 +0100 Subject: [PATCH 42/75] Update headers to `hpp` to avoid warnings. --- .../include/io_gripper_controller/io_gripper_controller.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index b3c39ae8a3..eda30bdec0 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -32,8 +32,8 @@ #include "rclcpp_action/rclcpp_action.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_publisher.h" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" #include "sensor_msgs/msg/joint_state.hpp" #include "std_srvs/srv/set_bool.hpp" #include "std_srvs/srv/trigger.hpp" From 058a18c98785543d10ce7edde2bf66046bff6c18 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 13:50:07 +0100 Subject: [PATCH 43/75] Remove confusing output when checking states. --- io_gripper_controller/src/io_gripper_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index c25b6e4b50..3c15ced237 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -493,7 +493,6 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) { check_state_ios_ = false; - RCLCPP_ERROR(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); break; } else @@ -514,7 +513,6 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ { if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) { - RCLCPP_ERROR(get_node()->get_logger(), "value doesn't match %s", io.c_str()); check_state_ios_ = false; break; } From 3fca0bc50901265c3dbf30a7b194e79f7153f415 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 14:18:13 +0100 Subject: [PATCH 44/75] Correct output that was wrong. --- io_gripper_controller/src/io_gripper_controller.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 3c15ced237..0637227b95 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -1312,7 +1312,6 @@ void IOGripperController::check_gripper_and_reconfigure_state() } bool reconfigure_state_found = false; - for (const auto & [key, val] : params_.configuration_setup.configurations_map) { for (const auto & io : val.state_high) @@ -1328,7 +1327,7 @@ void IOGripperController::check_gripper_and_reconfigure_state() if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) { reconfigure_state_found = false; - RCLCPP_ERROR(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); break; } else @@ -1349,8 +1348,8 @@ void IOGripperController::check_gripper_and_reconfigure_state() { if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) { - RCLCPP_ERROR(get_node()->get_logger(), "value doesn't match %s", io.c_str()); reconfigure_state_found = false; + RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); break; } else From d68f2fe56330fe43a5ebc45e6cffe6e43bc9cdf6 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 16:09:56 +0100 Subject: [PATCH 45/75] Update of gripper after testing with simulator. --- .../src/io_gripper_controller.cpp | 133 +++++++++--------- .../src/io_gripper_controller.yaml | 28 ++-- 2 files changed, 76 insertions(+), 85 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 0637227b95..bcdfa1dfcd 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -231,7 +231,8 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", + get_node()->get_logger(), + "CLOSE - SET_BEFORE_COMMAND: Failed to set the command state for %s", set_before_command_close[i].c_str()); } } @@ -245,7 +246,7 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", + get_node()->get_logger(), "CLOSE_GRIPPER: Failed to set the command state for %s", command_ios_close[i].c_str()); } } @@ -262,7 +263,8 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); + get_node()->get_logger(), + "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", high_val.c_str()); } else { @@ -283,7 +285,8 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); + get_node()->get_logger(), + "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", low_val.c_str()); } else { @@ -315,17 +318,22 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", high_val.c_str()); + get_node()->get_logger(), + "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", high_val.c_str()); } } for (const auto & low_val : closed_state_values_.set_after_command_low) { + RCLCPP_DEBUG( + get_node()->get_logger(), "CLOSE - SET_AFTER_COMMAND: set low after command %s", + low_val.c_str()); setResult = find_and_set_command(low_val, 0.0); if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", low_val.c_str()); + get_node()->get_logger(), + "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", low_val.c_str()); } } for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(closed_state_name_) @@ -359,7 +367,8 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", + get_node()->get_logger(), + "OPEN - SET_BEFORE_COMMAND: Failed to set the command state for %s", set_before_command_open[i].c_str()); } } @@ -374,7 +383,7 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", + get_node()->get_logger(), "OPEN_GRIPPER: Failed to set the command state for %s", command_ios_open[i].c_str()); } } @@ -390,7 +399,8 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); + get_node()->get_logger(), "OPEN - CHECK_GRIPPER_STATE: Failed to get the state for %s", + state_ios_open[i].c_str()); } else { @@ -418,7 +428,8 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta if (!setResult) { RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", + get_node()->get_logger(), + "OPEN - SET_AFTER_COMMAND: Failed to set the command state for %s", set_after_command_open[i].c_str()); } } @@ -493,6 +504,7 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) { check_state_ios_ = false; + RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); break; } else @@ -514,6 +526,7 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) { check_state_ios_ = false; + RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); break; } else @@ -541,18 +554,6 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ controller_interface::CallbackReturn IOGripperController::check_parameters() { - if (params_.open_close_joints.empty()) - { - RCLCPP_FATAL(get_node()->get_logger(), "Size of open close joints parameter cannot be zero."); - return CallbackReturn::FAILURE; - } - - if (params_.open.joint_states.empty()) - { - RCLCPP_FATAL(get_node()->get_logger(), "Size of joint states parameters cannot be zero."); - return CallbackReturn::FAILURE; - } - if (params_.open.command.high.empty() and params_.open.command.low.empty()) { RCLCPP_FATAL( @@ -567,15 +568,6 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() return CallbackReturn::FAILURE; } - if ( - params_.close.set_before_command.high.empty() and params_.close.set_before_command.low.empty()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of set before command high and low parameters cannot be zero."); - return CallbackReturn::FAILURE; - } - if (params_.close.command.high.empty() and params_.close.command.low.empty()) { RCLCPP_FATAL( @@ -584,53 +576,55 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() } // configurations parameter - if (params_.configurations.empty()) + if (!params_.configurations.empty()) { - RCLCPP_FATAL(get_node()->get_logger(), "Size of configurations parameter cannot be zero."); - return CallbackReturn::FAILURE; - } - - // configuration joints parameter - if (params_.configuration_joints.empty()) - { - RCLCPP_FATAL( - get_node()->get_logger(), "Size of configuration joints parameter cannot be zero."); - return CallbackReturn::FAILURE; - } - - // configuration setup parameter - if (params_.configuration_setup.configurations_map.empty()) - { - RCLCPP_FATAL(get_node()->get_logger(), "Size of configuration map parameter cannot be zero."); - return CallbackReturn::FAILURE; + if (!params_.configuration_joints.empty()) + { + // configuration setup parameter + if (params_.configuration_setup.configurations_map.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of configuration map parameter cannot be zero if configuraitons are defined."); + return CallbackReturn::FAILURE; + } + } + else + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Configuraiton joints have to be defined if configuraitons are provided."); + return CallbackReturn::FAILURE; + } } // gripper_specific_sensors parameter - if (params_.gripper_specific_sensors.empty()) - { - RCLCPP_FATAL( - get_node()->get_logger(), "Size of gripper specific sensors parameter cannot be zero."); - return CallbackReturn::FAILURE; - } - - // sensors interfaces parameter - if (params_.sensors_interfaces.gripper_specific_sensors_map.empty()) - { - RCLCPP_FATAL(get_node()->get_logger(), "Size of sensors interfaces parameter cannot be zero."); - return CallbackReturn::FAILURE; - } - - // if sensor input string is empty then return failure - for (const auto & [key, val] : params_.sensors_interfaces.gripper_specific_sensors_map) + if (!params_.gripper_specific_sensors.empty()) { - if (val.input == "") + // sensors interfaces parameter + if (params_.sensors_interfaces.gripper_specific_sensors_map.empty()) { RCLCPP_FATAL( get_node()->get_logger(), - "Size of sensor input string parameter cannot be empty (" - ")."); + "Size of sensors interfaces parameter cannot be zero if gripper sepcific sensors are " + "defined."); return CallbackReturn::FAILURE; } + else + { + // if sensor input string is empty then return failure + for (const auto & [key, val] : params_.sensors_interfaces.gripper_specific_sensors_map) + { + if (val.input == "") + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of sensor input string parameter cannot be empty (" + ")."); + return CallbackReturn::FAILURE; + } + } + } } return CallbackReturn::SUCCESS; } @@ -932,8 +926,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and }; close_service_ = get_node()->create_service( - "~/gripper_close", close_service_callback, rmw_qos_profile_services_hist_keep_all, - close_service_callback_group_); + "~/gripper_close", close_service_callback, qos_services, close_service_callback_group_); // configure gripper service auto configure_gripper_service_callback = diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml index 7f6258cfe6..dcae0892f9 100644 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -25,7 +25,6 @@ io_gripper_controller: } open_close_joints: { type: string_array, - default_value: [], description: "List of joint names that should change values according to open or close state of the gripper", validation: { unique<>: null, @@ -35,7 +34,6 @@ io_gripper_controller: open: joint_states: { type: double_array, - default_value: [], description: "List of joint values when gripper is open. The order of the values should match the order of the joint names in open_close_joints", validation: { unique<>: null, @@ -45,7 +43,7 @@ io_gripper_controller: set_before_command: high: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interfaces that have to be set to high (1.0) before opening the gripper", validation: { unique<>: null, @@ -53,7 +51,7 @@ io_gripper_controller: } low: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interfaces that have to be set to low (0.0) before opening the gripper", validation: { unique<>: null, @@ -96,7 +94,7 @@ io_gripper_controller: set_after_command: high: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interfaces that have to be set to high (1.0) after opening the gripper", validation: { unique<>: null, @@ -104,7 +102,7 @@ io_gripper_controller: } low: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interface names that have to be set to low (0.0) after opening the gripper.", validation: { unique<>: null, @@ -112,7 +110,7 @@ io_gripper_controller: } possible_closed_states: { type: string_array, - default_value: [""], + default_value: [], description: "List of possible closed states e.g. empty_close and full close", validation: { unique<>: null, @@ -122,7 +120,7 @@ io_gripper_controller: set_before_command: high: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interfaces that have to be set to high (1.0) before closing the gripper", validation: { unique<>: null, @@ -130,7 +128,7 @@ io_gripper_controller: } low: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interfaces that have to be set to low (0.0) before closing the gripper", validation: { unique<>: null, @@ -139,7 +137,7 @@ io_gripper_controller: command: high: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interfaces that have to be set to high (1.0) to close the gripper", validation: { unique<>: null, @@ -147,7 +145,7 @@ io_gripper_controller: } low: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interfaces that have to be set to low (0.0) to close the gripper", validation: { unique<>: null, @@ -165,7 +163,7 @@ io_gripper_controller: } high: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of state interfaces that have to equal high (1.0) to represent the gripper is closed", validation: { unique<>: null, @@ -173,7 +171,7 @@ io_gripper_controller: } low: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of state interfaces that have to equal low (0.0) to represent the gripper is closed", validation: { unique<>: null, @@ -181,7 +179,7 @@ io_gripper_controller: } set_after_command_high: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interfaces that have to be set to high (1.0) after closing the gripper", validation: { unique<>: null, @@ -189,7 +187,7 @@ io_gripper_controller: } set_after_command_low: { type: string_array, - default_value: [""], + default_value: [], description: "(optional) list of command interface names that have to be set to low (0.0) after closing the gripper.", validation: { unique<>: null, From 25b3cf2a259e8033b88b361893108fce013a2ac4 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 17 Jan 2025 16:09:20 +0100 Subject: [PATCH 46/75] Code simplification and adding more user-readable state message. --- .../io_gripper_controller.hpp | 48 ++++- .../src/io_gripper_controller.cpp | 192 +++++++++++++++--- .../src/io_gripper_controller.yaml | 12 ++ 3 files changed, 226 insertions(+), 26 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index eda30bdec0..a970d163ab 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -26,6 +26,11 @@ #include "control_msgs/action/set_io_gripper_config.hpp" #include "control_msgs/msg/dynamic_interface_values.hpp" #include "control_msgs/msg/interface_value.hpp" +<<<<<<< Updated upstream +======= +#include "control_msgs/msg/io_gripper_controller_state.hpp" +#include "control_msgs/msg/io_gripper_state.hpp" +>>>>>>> Stashed changes #include "control_msgs/srv/set_io_gripper_config.hpp" #include "controller_interface/controller_interface.hpp" #include "io_gripper_controller_parameters.hpp" @@ -159,6 +164,30 @@ class IOGripperController : public controller_interface::ControllerInterface controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; +<<<<<<< Updated upstream +======= + struct GripperTransitionIOs + { + std::unordered_map command_ios; + std::unordered_map state_ios; + + bool has_multiple_end_states; + std::vector possible_states; + std::unordered_map> multiple_states_ios; + + std::unordered_map set_before_command_ios; + std::unordered_map set_before_state_ios; + + std::unordered_map set_after_command_ios; + std::unordered_map set_after_state_ios; + }; + + GripperTransitionIOs open_ios_; + GripperTransitionIOs closeios_; + + rclcpp::Time last_transition_time_; + +>>>>>>> Stashed changes std::vector command_ios_open; std::vector command_ios_close; std::vector set_before_command_open; @@ -212,6 +241,10 @@ class IOGripperController : public controller_interface::ControllerInterface // set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, // halted) realtime_tools::RealtimeBuffer gripper_state_buffer_; +<<<<<<< Updated upstream +======= + realtime_tools::RealtimeBuffer gripper_open_state_buffer_; +>>>>>>> Stashed changes // Realtime buffer to store the name of the configuration which needs to be set realtime_tools::RealtimeBuffer configure_gripper_buffer_; // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, @@ -219,12 +252,19 @@ class IOGripperController : public controller_interface::ControllerInterface realtime_tools::RealtimeBuffer reconfigure_state_buffer_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; - using InterfacePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr g_j_s_publisher_; std::unique_ptr gripper_joint_state_publisher_; std::vector joint_state_values_; + using InterfacePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr if_publisher_; std::unique_ptr interface_publisher_; +<<<<<<< Updated upstream +======= + using GripperStatePublisher = + realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr g_s_publisher_; + std::unique_ptr gripper_state_publisher_; +>>>>>>> Stashed changes std::atomic reconfigureFlag_{false}; std::atomic openFlag_{false}; std::atomic closeFlag_{false}; @@ -258,7 +298,13 @@ class IOGripperController : public controller_interface::ControllerInterface * @brief Handles the state transition when opening the gripper. * @param state The current state of the gripper. */ +<<<<<<< Updated upstream void handle_gripper_state_transition_open(const gripper_state_type & state); +======= + void handle_gripper_state_transition( + const rclcpp::Time & current_time, const GripperTransitionIOs & ios, const uint & state, + const std::string & transition_name, std::vector after_joint_states); +>>>>>>> Stashed changes /** * @brief Handles the state transition when closing the gripper. diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index bcdfa1dfcd..c95d7eca0a 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -154,9 +154,18 @@ controller_interface::return_type IOGripperController::update( // do nothing break; case service_mode_type::OPEN: +<<<<<<< Updated upstream handle_gripper_state_transition_open(*(gripper_state_buffer_.readFromRT())); +======= + handle_gripper_state_transition( + time, open_ios_, *(gripper_state_buffer_.readFromRT()), "open", params_.open.joint_states); +>>>>>>> Stashed changes break; case service_mode_type::CLOSE: + // handle_gripper_state_transition( + // time, close_ios_, *(gripper_state_buffer_.readFromRT()), "close", + // []); // here joint states should be empty as we have multiple + // states handle_gripper_state_transition_close(*(gripper_state_buffer_.readFromRT())); break; @@ -352,13 +361,67 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st } } +<<<<<<< Updated upstream void IOGripperController::handle_gripper_state_transition_open(const gripper_state_type & state) { +======= +bool IOGripperController::set_commands( + const std::map & command_states, const std::string & transition_name) +{ + bool all_successful = true; + for (const auto & [command_name, command_value] : command_states) + { + if (!find_and_set_command(command_name, command_value)) + { + RCLCPP_ERROR( + get_node()->get_logger(), "%s: Failed to set the command state for %s", + transition_name.c_str(), command_name.c_str()); + all_successful = false; + } + } + return all_successful; +} + +bool IOGripperController::check_states( + const std::map & state_ios, const std::string & transition_name) +{ + bool all_correct = true; + for (const auto & [state_name, expected_state_value] : state_ios) + { + double current_state_value; + if (!find_and_get_state(state_name, current_state_value)) + { + RCLCPP_ERROR( + get_node()->get_logger(), "%s: Failed to get the state for %s", transition_name.c_str(), + state_name.c_str()); + all_correct = false; + } + else + { + if (abs(current_state_value - expected_state_value) > std::numeric_limits::epsilon()) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: State value for %s doesn't match", transition_name.c_str(), + state_name.c_str()); + all_correct = false; + } + } + } + return all_correct; +} + +void IOGripperController::handle_gripper_state_transition( + const rclcpp::Time & current_time, const GripperTransitionIOs & ios, const uint & state, + const std::string & transition_name, std::vector after_joint_states) +{ + using control_msgs::msg::IOGripperState; +>>>>>>> Stashed changes switch (state) { case gripper_state_type::IDLE: // do nothing break; +<<<<<<< Updated upstream case gripper_state_type::SET_BEFORE_COMMAND: for (size_t i = 0; i < set_before_command_open.size(); ++i) { @@ -389,9 +452,24 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta } gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); +======= + case IOGripperState::SET_BEFORE_COMMAND: + set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND"); + // TODO(destogl): check to use other Realtime sync object to have write from RT + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); + last_transition_time_ = current_time; + break; + case IOGripperState::SET_COMMAND: + // now execute the command on the gripper + set_commands(ios.command_ios, transition_name + " - SET_COMMAND"); + // TODO(destogl): check to use other Realtime sync object to have write from RT + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); + last_transition_time_ = current_time; +>>>>>>> Stashed changes break; case gripper_state_type::CHECK_GRIPPER_STATE: // check the state of the gripper +<<<<<<< Updated upstream check_state_ios_ = false; for (size_t i = 0; i < state_ios_open.size(); ++i) { @@ -411,14 +489,40 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta else { check_state_ios_ = false; +======= + bool check_state_ios = true; + if (ios.has_multiple_end_states) + { + for (const auto & possible_end_state : ios.possible_states) + { + if (check_states( + ios.multiple_states_ios.at(possible_end_state), + transition_name + " - CHECK_COMMAND");) + { + check_state_ios = true; + after_joint_states = + params_.close.sate.possible_closed_states_map.at(possible_end_state).joint_states; + // TODO: store possible_end_state in a variable to publish on status topic +>>>>>>> Stashed changes break; } + check_state_ios = false; } } +<<<<<<< Updated upstream if (check_state_ios_) +======= + else // only single end state + { + check_state_ios = check_states(ios.state_ios, transition_name + " - CHECK_COMMAND"); + } + + if (check_state_ios) +>>>>>>> Stashed changes { gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); } +<<<<<<< Updated upstream break; case gripper_state_type::SET_AFTER_COMMAND: for (size_t i = 0; i < set_after_command_open.size(); ++i) @@ -434,12 +538,38 @@ void IOGripperController::handle_gripper_state_transition_open(const gripper_sta } } for (size_t i = 0; i < params_.open.joint_states.size(); ++i) +======= + else if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - CHECK_COMMAND: Gripper didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + } + break; + case IOGripperState::SET_AFTER_COMMAND: + set_commands(ios.set_after_command_ios, transition_name + " - SET_AFTER_COMMAND"); + + // set joint states + for (size_t i = 0; i < after_joint_states.size(); ++i) +>>>>>>> Stashed changes { joint_state_values_[i] = params_.open.joint_states[i]; } +<<<<<<< Updated upstream gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); openFlag_.store(false); gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); +======= + + // Finish up the transition + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::IDLE); + openFlag_.store(false); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); + last_transition_time_ = current_time; + +>>>>>>> Stashed changes break; default: break; @@ -631,11 +761,15 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() void IOGripperController::prepare_command_and_state_ios() { - // make full command ios lists -- just once - for (const auto & key : params_.open.set_before_command.high) + auto parse_interfaces_from_params = []( + const std::vector & parameter_values, + const double & value, + std::unordered_map & ios, + std::unordered_set & interface_list) { - if (!key.empty()) + for (const auto & itf : parameter_values) { +<<<<<<< Updated upstream set_before_command_open.push_back(key); set_before_command_open_values.push_back(1.0); command_if_ios.insert(key); @@ -658,9 +792,17 @@ void IOGripperController::prepare_command_and_state_ios() command_ios_open.push_back(key); command_ios_open_values.push_back(1.0); command_if_ios.insert(key); +======= + if (!itf.empty()) + { + ios[itf] = value; + interface_list.insert(itf); + } +>>>>>>> Stashed changes } - } + }; +<<<<<<< Updated upstream for (const auto & key : params_.open.command.low) { if (!key.empty()) @@ -690,6 +832,23 @@ void IOGripperController::prepare_command_and_state_ios() command_if_ios.insert(key); } } +======= + // make full command ios lists -- just once + parse_interfaces_from_params( + params_.open.set_before_command.high, 1.0, open_ios_.set_before_command_ios, command_if_ios); + parse_interfaces_from_params( + params_.open.set_before_command.low, 0.0, open_ios_.set_before_command_ios, command_if_ios); + + parse_interfaces_from_params( + params_.open.command.high, 1.0, open_ios_.command_ios, command_if_ios); + parse_interfaces_from_params( + params_.open.command.low, 0.0, open_ios_.command_ios, command_if_ios); + + parse_interfaces_from_params( + params_.open.set_after_command.high, 1.0, open_ios_.set_after_command_ios, command_if_ios); + parse_interfaces_from_params( + params_.open.set_after_command.low, 0.0, open_ios_.set_after_command_ios, command_if_ios); +>>>>>>> Stashed changes for (const auto & key : params_.close.set_before_command.high) { @@ -729,25 +888,8 @@ void IOGripperController::prepare_command_and_state_ios() } // make full state ios lists -- just once - for (const auto & key : params_.open.state.high) - { - if (!key.empty()) - { - state_ios_open.push_back(key); - state_ios_open_values.push_back(1.0); - state_if_ios.insert(key); - } - } - - for (const auto & key : params_.open.state.low) - { - if (!key.empty()) - { - state_ios_open.push_back(key); - state_ios_open_values.push_back(0.0); - state_if_ios.insert(key); - } - } + parse_interfaces_from_params(params_.open.state.high, 1.0, open_ios_.state_ios, state_if_ios); + parse_interfaces_from_params(params_.open.state.low, 0.0, open_ios_.state_ios, state_if_ios); for (const auto & [name, value] : params_.close.state.possible_closed_states_map) { @@ -1145,7 +1287,7 @@ void IOGripperController::execute(std::shared_ptr goal_handle } else { - feedback->state = static_cast(*(gripper_state_buffer_.readFromRT())); + feedback->transition.state = static_cast(*(gripper_state_buffer_.readFromRT())); goal_handle->publish_feedback(feedback); } std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -1205,7 +1347,7 @@ void IOGripperController::config_execute(std::shared_ptrstate = static_cast(*(reconfigure_state_buffer_.readFromRT())); + feedback->transition.state = static_cast(*(reconfigure_state_buffer_.readFromRT())); goal_handle->publish_feedback(feedback); } std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml index dcae0892f9..ecf10e005d 100644 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -21,7 +21,19 @@ io_gripper_controller: use_action: { type: bool, default_value: false, +<<<<<<< Updated upstream description: "True for using action server and false service server for the gripper", +======= + description: "True for using action server and false service server for the gripper" + } + timeout: { + type: double, + default_value: 5.0, + description: "Timeout for the waiting on signals from gripper about reached state.", + validation: { + gt<>: [0.0], + } +>>>>>>> Stashed changes } open_close_joints: { type: string_array, From 54b3666424e5a10aed8c74c9310b54aaad986c46 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 20 Jan 2025 16:53:38 +0100 Subject: [PATCH 47/75] Apply suggestions from code review --- io_gripper_controller/src/io_gripper_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index c95d7eca0a..db83c5b58b 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -937,12 +937,10 @@ void IOGripperController::prepare_command_and_state_ios() for (const auto & io : config.command_high) { command_if_ios.insert(io); - state_if_ios.insert(io); } for (const auto & io : config.command_low) { command_if_ios.insert(io); - state_if_ios.insert(io); } for (const auto & io : config.state_high) { From 41e0ae2b6ea7171c2a60029366bbdfaa49c0c7d3 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Mon, 20 Jan 2025 17:48:08 +0100 Subject: [PATCH 48/75] accepting upcoming changes --- .../io_gripper_controller.hpp | 24 +-- .../src/io_gripper_controller.cpp | 170 ++---------------- .../src/io_gripper_controller.yaml | 4 - 3 files changed, 16 insertions(+), 182 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index a970d163ab..8e42cba50d 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ -#define GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ +#ifndef IO_GRIPPER_CONTROLLER__IO_GRIPPER_CONTROLLER_HPP_ +#define IO_GRIPPER_CONTROLLER__IO_GRIPPER_CONTROLLER_HPP_ #include #include @@ -22,15 +22,14 @@ #include #include +#include +#include #include "control_msgs/action/io_gripper_command.hpp" #include "control_msgs/action/set_io_gripper_config.hpp" #include "control_msgs/msg/dynamic_interface_values.hpp" #include "control_msgs/msg/interface_value.hpp" -<<<<<<< Updated upstream -======= #include "control_msgs/msg/io_gripper_controller_state.hpp" #include "control_msgs/msg/io_gripper_state.hpp" ->>>>>>> Stashed changes #include "control_msgs/srv/set_io_gripper_config.hpp" #include "controller_interface/controller_interface.hpp" #include "io_gripper_controller_parameters.hpp" @@ -164,8 +163,6 @@ class IOGripperController : public controller_interface::ControllerInterface controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; -<<<<<<< Updated upstream -======= struct GripperTransitionIOs { std::unordered_map command_ios; @@ -187,7 +184,6 @@ class IOGripperController : public controller_interface::ControllerInterface rclcpp::Time last_transition_time_; ->>>>>>> Stashed changes std::vector command_ios_open; std::vector command_ios_close; std::vector set_before_command_open; @@ -241,10 +237,7 @@ class IOGripperController : public controller_interface::ControllerInterface // set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, // halted) realtime_tools::RealtimeBuffer gripper_state_buffer_; -<<<<<<< Updated upstream -======= realtime_tools::RealtimeBuffer gripper_open_state_buffer_; ->>>>>>> Stashed changes // Realtime buffer to store the name of the configuration which needs to be set realtime_tools::RealtimeBuffer configure_gripper_buffer_; // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, @@ -258,13 +251,10 @@ class IOGripperController : public controller_interface::ControllerInterface using InterfacePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr if_publisher_; std::unique_ptr interface_publisher_; -<<<<<<< Updated upstream -======= using GripperStatePublisher = realtime_tools::RealtimePublisher; rclcpp::Publisher::SharedPtr g_s_publisher_; std::unique_ptr gripper_state_publisher_; ->>>>>>> Stashed changes std::atomic reconfigureFlag_{false}; std::atomic openFlag_{false}; std::atomic closeFlag_{false}; @@ -298,13 +288,9 @@ class IOGripperController : public controller_interface::ControllerInterface * @brief Handles the state transition when opening the gripper. * @param state The current state of the gripper. */ -<<<<<<< Updated upstream - void handle_gripper_state_transition_open(const gripper_state_type & state); -======= void handle_gripper_state_transition( const rclcpp::Time & current_time, const GripperTransitionIOs & ios, const uint & state, const std::string & transition_name, std::vector after_joint_states); ->>>>>>> Stashed changes /** * @brief Handles the state transition when closing the gripper. @@ -434,4 +420,4 @@ class IOGripperController : public controller_interface::ControllerInterface } // namespace io_gripper_controller -#endif // GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ +#endif // IO_GRIPPER_CONTROLLER__IO_GRIPPER_CONTROLLER_HPP_ diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index db83c5b58b..0cf6749286 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -140,7 +140,7 @@ controller_interface::CallbackReturn IOGripperController::on_deactivate( } controller_interface::return_type IOGripperController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { if (reconfigureFlag_.load()) { @@ -154,12 +154,9 @@ controller_interface::return_type IOGripperController::update( // do nothing break; case service_mode_type::OPEN: -<<<<<<< Updated upstream - handle_gripper_state_transition_open(*(gripper_state_buffer_.readFromRT())); -======= handle_gripper_state_transition( - time, open_ios_, *(gripper_state_buffer_.readFromRT()), "open", params_.open.joint_states); ->>>>>>> Stashed changes + time, open_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "open", + params_.open.joint_states); break; case service_mode_type::CLOSE: // handle_gripper_state_transition( @@ -361,10 +358,6 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st } } -<<<<<<< Updated upstream -void IOGripperController::handle_gripper_state_transition_open(const gripper_state_type & state) -{ -======= bool IOGripperController::set_commands( const std::map & command_states, const std::string & transition_name) { @@ -415,44 +408,11 @@ void IOGripperController::handle_gripper_state_transition( const std::string & transition_name, std::vector after_joint_states) { using control_msgs::msg::IOGripperState; ->>>>>>> Stashed changes switch (state) { case gripper_state_type::IDLE: // do nothing break; -<<<<<<< Updated upstream - case gripper_state_type::SET_BEFORE_COMMAND: - for (size_t i = 0; i < set_before_command_open.size(); ++i) - { - setResult = - find_and_set_command(set_before_command_open[i], set_before_command_open_values[i]); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "OPEN - SET_BEFORE_COMMAND: Failed to set the command state for %s", - set_before_command_open[i].c_str()); - } - } - - gripper_state_buffer_.writeFromNonRT(gripper_state_type::OPEN_GRIPPER); - break; - case gripper_state_type::OPEN_GRIPPER: - // now open the gripper - for (size_t i = 0; i < command_ios_open.size(); ++i) - { - setResult = find_and_set_command(command_ios_open[i], command_ios_open_values[i]); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "OPEN_GRIPPER: Failed to set the command state for %s", - command_ios_open[i].c_str()); - } - } - - gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); -======= case IOGripperState::SET_BEFORE_COMMAND: set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND"); // TODO(destogl): check to use other Realtime sync object to have write from RT @@ -465,31 +425,9 @@ void IOGripperController::handle_gripper_state_transition( // TODO(destogl): check to use other Realtime sync object to have write from RT gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); last_transition_time_ = current_time; ->>>>>>> Stashed changes break; case gripper_state_type::CHECK_GRIPPER_STATE: // check the state of the gripper -<<<<<<< Updated upstream - check_state_ios_ = false; - for (size_t i = 0; i < state_ios_open.size(); ++i) - { - setResult = find_and_get_state(state_ios_open[i], state_value_); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "OPEN - CHECK_GRIPPER_STATE: Failed to get the state for %s", - state_ios_open[i].c_str()); - } - else - { - if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) - { - check_state_ios_ = true; - } - else - { - check_state_ios_ = false; -======= bool check_state_ios = true; if (ios.has_multiple_end_states) { @@ -501,44 +439,22 @@ void IOGripperController::handle_gripper_state_transition( { check_state_ios = true; after_joint_states = - params_.close.sate.possible_closed_states_map.at(possible_end_state).joint_states; - // TODO: store possible_end_state in a variable to publish on status topic ->>>>>>> Stashed changes + params_.close.state.possible_closed_states_map.at(possible_end_state).joint_states; + // TODO(Sachin): store possible_end_state in a variable to publish on status topic break; } check_state_ios = false; } } -<<<<<<< Updated upstream - if (check_state_ios_) -======= else // only single end state { check_state_ios = check_states(ios.state_ios, transition_name + " - CHECK_COMMAND"); } if (check_state_ios) ->>>>>>> Stashed changes { gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); } -<<<<<<< Updated upstream - break; - case gripper_state_type::SET_AFTER_COMMAND: - for (size_t i = 0; i < set_after_command_open.size(); ++i) - { - setResult = - find_and_set_command(set_after_command_open[i], set_after_command_open_values[i]); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "OPEN - SET_AFTER_COMMAND: Failed to set the command state for %s", - set_after_command_open[i].c_str()); - } - } - for (size_t i = 0; i < params_.open.joint_states.size(); ++i) -======= else if ((current_time - last_transition_time_).seconds() > params_.timeout) { RCLCPP_ERROR( @@ -553,15 +469,9 @@ void IOGripperController::handle_gripper_state_transition( // set joint states for (size_t i = 0; i < after_joint_states.size(); ++i) ->>>>>>> Stashed changes { joint_state_values_[i] = params_.open.joint_states[i]; } -<<<<<<< Updated upstream - gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); - openFlag_.store(false); - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); -======= // Finish up the transition gripper_open_state_buffer_.writeFromNonRT(IOGripperState::IDLE); @@ -569,7 +479,6 @@ void IOGripperController::handle_gripper_state_transition( gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); last_transition_time_ = current_time; ->>>>>>> Stashed changes break; default: break; @@ -684,21 +593,21 @@ void IOGripperController::handle_reconfigure_state_transition(const reconfigure_ controller_interface::CallbackReturn IOGripperController::check_parameters() { - if (params_.open.command.high.empty() and params_.open.command.low.empty()) + if (params_.open.command.high.empty() && params_.open.command.low.empty()) { RCLCPP_FATAL( get_node()->get_logger(), "Size of open command high and low parameters cannot be zero."); return CallbackReturn::FAILURE; } - if (params_.open.state.high.empty() and params_.open.state.low.empty()) + if (params_.open.state.high.empty() && params_.open.state.low.empty()) { RCLCPP_FATAL( get_node()->get_logger(), "Size of open state high and low parameters cannot be zero."); return CallbackReturn::FAILURE; } - if (params_.close.command.high.empty() and params_.close.command.low.empty()) + if (params_.close.command.high.empty() && params_.close.command.low.empty()) { RCLCPP_FATAL( get_node()->get_logger(), "Size of close command high and low parameters cannot be zero."); @@ -715,7 +624,7 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() { RCLCPP_FATAL( get_node()->get_logger(), - "Size of configuration map parameter cannot be zero if configuraitons are defined."); + "Size of configuration map parameter cannot be zero if configurations are defined."); return CallbackReturn::FAILURE; } } @@ -723,7 +632,7 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() { RCLCPP_FATAL( get_node()->get_logger(), - "Configuraiton joints have to be defined if configuraitons are provided."); + "Configuration joints have to be defined if configurations are provided."); return CallbackReturn::FAILURE; } } @@ -736,7 +645,7 @@ controller_interface::CallbackReturn IOGripperController::check_parameters() { RCLCPP_FATAL( get_node()->get_logger(), - "Size of sensors interfaces parameter cannot be zero if gripper sepcific sensors are " + "Size of sensors interfaces parameter cannot be zero if gripper specific sensors are " "defined."); return CallbackReturn::FAILURE; } @@ -769,70 +678,14 @@ void IOGripperController::prepare_command_and_state_ios() { for (const auto & itf : parameter_values) { -<<<<<<< Updated upstream - set_before_command_open.push_back(key); - set_before_command_open_values.push_back(1.0); - command_if_ios.insert(key); - } - } - - for (const auto & key : params_.open.set_before_command.low) - { - if (!key.empty()) - { - set_before_command_open.push_back(key); - set_before_command_open_values.push_back(0.0); - command_if_ios.insert(key); - } - } - for (const auto & key : params_.open.command.high) - { - if (!key.empty()) - { - command_ios_open.push_back(key); - command_ios_open_values.push_back(1.0); - command_if_ios.insert(key); -======= if (!itf.empty()) { ios[itf] = value; interface_list.insert(itf); } ->>>>>>> Stashed changes } }; -<<<<<<< Updated upstream - for (const auto & key : params_.open.command.low) - { - if (!key.empty()) - { - command_ios_open.push_back(key); - command_ios_open_values.push_back(0.0); - command_if_ios.insert(key); - } - } - - for (const auto & key : params_.open.set_after_command.high) - { - if (!key.empty()) - { - set_after_command_open.push_back(key); - set_after_command_open_values.push_back(1.0); - command_if_ios.insert(key); - } - } - - for (const auto & key : params_.open.set_after_command.low) - { - if (!key.empty()) - { - set_after_command_open.push_back(key); - set_after_command_open_values.push_back(0.0); - command_if_ios.insert(key); - } - } -======= // make full command ios lists -- just once parse_interfaces_from_params( params_.open.set_before_command.high, 1.0, open_ios_.set_before_command_ios, command_if_ios); @@ -848,7 +701,6 @@ void IOGripperController::prepare_command_and_state_ios() params_.open.set_after_command.high, 1.0, open_ios_.set_after_command_ios, command_if_ios); parse_interfaces_from_params( params_.open.set_after_command.low, 0.0, open_ios_.set_after_command_ios, command_if_ios); ->>>>>>> Stashed changes for (const auto & key : params_.close.set_before_command.high) { diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml index ecf10e005d..34a4185abd 100644 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -21,9 +21,6 @@ io_gripper_controller: use_action: { type: bool, default_value: false, -<<<<<<< Updated upstream - description: "True for using action server and false service server for the gripper", -======= description: "True for using action server and false service server for the gripper" } timeout: { @@ -33,7 +30,6 @@ io_gripper_controller: validation: { gt<>: [0.0], } ->>>>>>> Stashed changes } open_close_joints: { type: string_array, From 361162385eea00821a7e63db14d196d83a7c0606 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Sun, 26 Jan 2025 23:25:59 +0100 Subject: [PATCH 49/75] compilation issue fixed --- .../io_gripper_controller.hpp | 10 +++++++++- .../src/io_gripper_controller.cpp | 15 ++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 8e42cba50d..b511a2ea8b 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -237,7 +237,7 @@ class IOGripperController : public controller_interface::ControllerInterface // set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, // halted) realtime_tools::RealtimeBuffer gripper_state_buffer_; - realtime_tools::RealtimeBuffer gripper_open_state_buffer_; + realtime_tools::RealtimeBuffer gripper_open_state_buffer_; // Realtime buffer to store the name of the configuration which needs to be set realtime_tools::RealtimeBuffer configure_gripper_buffer_; // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, @@ -259,6 +259,8 @@ class IOGripperController : public controller_interface::ControllerInterface std::atomic openFlag_{false}; std::atomic closeFlag_{false}; + bool check_state_ios{false}; + private: /** * @brief Finds and sets a command value. @@ -341,6 +343,12 @@ class IOGripperController : public controller_interface::ControllerInterface */ void check_gripper_and_reconfigure_state(); + bool set_commands( + const std::unordered_map & command_states, + const std::string & transition_name); + bool check_states( + const std::unordered_map & state_ios, const std::string & transition_name); + std::vector configurations_list_; std::vector config_map_; std::vector diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 0cf6749286..e70d139380 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -359,7 +359,8 @@ void IOGripperController::handle_gripper_state_transition_close(const gripper_st } bool IOGripperController::set_commands( - const std::map & command_states, const std::string & transition_name) + const std::unordered_map & command_states, + const std::string & transition_name) { bool all_successful = true; for (const auto & [command_name, command_value] : command_states) @@ -376,7 +377,7 @@ bool IOGripperController::set_commands( } bool IOGripperController::check_states( - const std::map & state_ios, const std::string & transition_name) + const std::unordered_map & state_ios, const std::string & transition_name) { bool all_correct = true; for (const auto & [state_name, expected_state_value] : state_ios) @@ -410,13 +411,13 @@ void IOGripperController::handle_gripper_state_transition( using control_msgs::msg::IOGripperState; switch (state) { - case gripper_state_type::IDLE: + case IOGripperState::IDLE: // do nothing break; case IOGripperState::SET_BEFORE_COMMAND: set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND"); // TODO(destogl): check to use other Realtime sync object to have write from RT - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_AFTER_COMMAND); last_transition_time_ = current_time; break; case IOGripperState::SET_COMMAND: @@ -426,16 +427,16 @@ void IOGripperController::handle_gripper_state_transition( gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); last_transition_time_ = current_time; break; - case gripper_state_type::CHECK_GRIPPER_STATE: + case IOGripperState::CHECK_COMMAND: // check the state of the gripper - bool check_state_ios = true; + check_state_ios = true; if (ios.has_multiple_end_states) { for (const auto & possible_end_state : ios.possible_states) { if (check_states( ios.multiple_states_ios.at(possible_end_state), - transition_name + " - CHECK_COMMAND");) + transition_name + " - CHECK_COMMAND")) { check_state_ios = true; after_joint_states = From c684b6dfe85fdbc08d9e36346b76d154d5c88c0f Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Tue, 28 Jan 2025 18:21:48 +0100 Subject: [PATCH 50/75] changes to use the message IOGripperState intsead of enum --- .../io_gripper_controller.hpp | 31 +- .../src/io_gripper_controller.cpp | 303 +++++++++--------- 2 files changed, 159 insertions(+), 175 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index b511a2ea8b..8b7ad7b47c 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -60,30 +60,6 @@ enum class service_mode_type : std::uint8_t CLOSE = 2, }; -/** - * @enum gripper_state_type - * @brief Represents the various states of the gripper. - * - * - IDLE: The gripper is in an idle state, not performing any action. - * - SET_BEFORE_COMMAND: Executing commands for io defined in the yaml which are required before - * opening the gripper. - * - CLOSE_GRIPPER: Executing commands to close the gripper. - * - CHECK_GRIPPER_STATE: Checking the state of the gripper to make sure the gripper is closed. - * - OPEN_GRIPPER: Executing commands to open the gripper. - * - SET_AFTER_COMMAND: Setting the gripper state after executing a command. - * - HALTED: The gripper operation is halted. - */ -enum class gripper_state_type : std::uint8_t -{ - IDLE = 0, - SET_BEFORE_COMMAND = 1, - CLOSE_GRIPPER = 2, - CHECK_GRIPPER_STATE = 3, - OPEN_GRIPPER = 4, - SET_AFTER_COMMAND = 5, - HALTED = 6, -}; - /** * @enum reconfigure_state_type * @brief Represents the various states of the reconfiguration process, which means that the gripper @@ -180,7 +156,7 @@ class IOGripperController : public controller_interface::ControllerInterface }; GripperTransitionIOs open_ios_; - GripperTransitionIOs closeios_; + GripperTransitionIOs close_ios_; rclcpp::Time last_transition_time_; @@ -222,6 +198,7 @@ class IOGripperController : public controller_interface::ControllerInterface using GoalHandleGripper = rclcpp_action::ServerGoalHandle; using GripperConfigAction = control_msgs::action::SetIOGripperConfig; using GoalHandleGripperConfig = rclcpp_action::ServerGoalHandle; + using IOGripperState = control_msgs::msg::IOGripperState; protected: std::shared_ptr param_listener_; @@ -236,7 +213,7 @@ class IOGripperController : public controller_interface::ControllerInterface // Realtime buffer to store the state for switching the gripper state (e.g. idle, // set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, // halted) - realtime_tools::RealtimeBuffer gripper_state_buffer_; + realtime_tools::RealtimeBuffer gripper_state_buffer_; realtime_tools::RealtimeBuffer gripper_open_state_buffer_; // Realtime buffer to store the name of the configuration which needs to be set realtime_tools::RealtimeBuffer configure_gripper_buffer_; @@ -298,7 +275,7 @@ class IOGripperController : public controller_interface::ControllerInterface * @brief Handles the state transition when closing the gripper. * @param state The current state of the gripper. */ - void handle_gripper_state_transition_close(const gripper_state_type & state); + // void handle_gripper_state_transition_close(const gripper_state_type & state); /** * @brief Handles the state transition for reconfiguration. diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index e70d139380..6055242285 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -56,7 +56,7 @@ controller_interface::CallbackReturn IOGripperController::on_init() gripper_service_buffer_.initRT(service_mode_type::IDLE); configuration_key_ = ""; configure_gripper_buffer_.initRT(configuration_key_); - gripper_state_buffer_.initRT(gripper_state_type::IDLE); + gripper_state_buffer_.initRT(IOGripperState::IDLE); reconfigure_state_buffer_.initRT(reconfigure_state_type::IDLE); try @@ -163,7 +163,10 @@ controller_interface::return_type IOGripperController::update( // time, close_ios_, *(gripper_state_buffer_.readFromRT()), "close", // []); // here joint states should be empty as we have multiple // states - handle_gripper_state_transition_close(*(gripper_state_buffer_.readFromRT())); + // handle_gripper_state_transition_close(*(gripper_state_buffer_.readFromRT())); + handle_gripper_state_transition( + time, close_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "close", + params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states); break; default: @@ -222,141 +225,146 @@ bool IOGripperController::find_and_get_command(const std::string & name, double return false; } -void IOGripperController::handle_gripper_state_transition_close(const gripper_state_type & state) -{ - switch (state) - { - case gripper_state_type::IDLE: - // do nothing - break; - case gripper_state_type::SET_BEFORE_COMMAND: - for (size_t i = 0; i < set_before_command_close.size(); ++i) - { - setResult = - find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "CLOSE - SET_BEFORE_COMMAND: Failed to set the command state for %s", - set_before_command_close[i].c_str()); - } - } - - gripper_state_buffer_.writeFromNonRT(gripper_state_type::CLOSE_GRIPPER); - break; - case gripper_state_type::CLOSE_GRIPPER: - for (size_t i = 0; i < command_ios_close.size(); ++i) - { - setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "CLOSE_GRIPPER: Failed to set the command state for %s", - command_ios_close[i].c_str()); - } - } - - gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); - break; - case gripper_state_type::CHECK_GRIPPER_STATE: - for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) - { - check_state_ios_ = false; - for (const auto & high_val : state_params.high) - { - setResult = find_and_get_state(high_val, state_value_); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", high_val.c_str()); - } - else - { - if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) - { - check_state_ios_ = true; - } - else - { - check_state_ios_ = false; - break; - } - } - } - for (const auto & low_val : state_params.low) - { - setResult = find_and_get_state(low_val, state_value_); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", low_val.c_str()); - } - else - { - if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) - { - check_state_ios_ = true; - } - else - { - check_state_ios_ = false; - break; - } - } - } - if (check_state_ios_) - { - closed_state_name_ = state_name; - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); - break; - } - } - break; - case gripper_state_type::SET_AFTER_COMMAND: - closed_state_values_ = params_.close.state.possible_closed_states_map.at(closed_state_name_); - - for (const auto & high_val : closed_state_values_.set_after_command_high) - { - setResult = find_and_set_command(high_val, 1.0); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", high_val.c_str()); - } - } - - for (const auto & low_val : closed_state_values_.set_after_command_low) - { - RCLCPP_DEBUG( - get_node()->get_logger(), "CLOSE - SET_AFTER_COMMAND: set low after command %s", - low_val.c_str()); - setResult = find_and_set_command(low_val, 0.0); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", low_val.c_str()); - } - } - for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(closed_state_name_) - .joint_states.size(); - ++i) - { - joint_state_values_[i] = - params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states[i]; - } - gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); - closeFlag_.store(false); - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); - break; - default: - break; - } -} +// void IOGripperController::handle_gripper_state_transition_close(const gripper_state_type & state) +// { +// switch (state) +// { +// case gripper_state_type::IDLE: +// // do nothing +// break; +// case gripper_state_type::SET_BEFORE_COMMAND: +// for (size_t i = 0; i < set_before_command_close.size(); ++i) +// { +// setResult = +// find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); +// if (!setResult) +// { +// RCLCPP_ERROR( +// get_node()->get_logger(), +// "CLOSE - SET_BEFORE_COMMAND: Failed to set the command state for %s", +// set_before_command_close[i].c_str()); +// } +// } + +// gripper_state_buffer_.writeFromNonRT(gripper_state_type::CLOSE_GRIPPER); +// break; +// case gripper_state_type::CLOSE_GRIPPER: +// for (size_t i = 0; i < command_ios_close.size(); ++i) +// { +// setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); +// if (!setResult) +// { +// RCLCPP_ERROR( +// get_node()->get_logger(), "CLOSE_GRIPPER: Failed to set the command state for %s", +// command_ios_close[i].c_str()); +// } +// } + +// gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); +// break; +// case gripper_state_type::CHECK_GRIPPER_STATE: +// for (const auto & [state_name, state_params] : +// params_.close.state.possible_closed_states_map) +// { +// check_state_ios_ = false; +// for (const auto & high_val : state_params.high) +// { +// setResult = find_and_get_state(high_val, state_value_); +// if (!setResult) +// { +// RCLCPP_ERROR( +// get_node()->get_logger(), +// "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", high_val.c_str()); +// } +// else +// { +// if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) +// { +// check_state_ios_ = true; +// } +// else +// { +// check_state_ios_ = false; +// break; +// } +// } +// } +// for (const auto & low_val : state_params.low) +// { +// setResult = find_and_get_state(low_val, state_value_); +// if (!setResult) +// { +// RCLCPP_ERROR( +// get_node()->get_logger(), +// "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", low_val.c_str()); +// } +// else +// { +// if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) +// { +// check_state_ios_ = true; +// } +// else +// { +// check_state_ios_ = false; +// break; +// } +// } +// } +// if (check_state_ios_) +// { +// closed_state_name_ = state_name; +// gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); +// break; +// } +// } +// break; +// case gripper_state_type::SET_AFTER_COMMAND: +// closed_state_values_ = +// params_.close.state.possible_closed_states_map.at(closed_state_name_); + +// for (const auto & high_val : closed_state_values_.set_after_command_high) +// { +// setResult = find_and_set_command(high_val, 1.0); +// if (!setResult) +// { +// RCLCPP_ERROR( +// get_node()->get_logger(), +// "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", +// high_val.c_str()); +// } +// } + +// for (const auto & low_val : closed_state_values_.set_after_command_low) +// { +// RCLCPP_DEBUG( +// get_node()->get_logger(), "CLOSE - SET_AFTER_COMMAND: set low after command %s", +// low_val.c_str()); +// setResult = find_and_set_command(low_val, 0.0); +// if (!setResult) +// { +// RCLCPP_ERROR( +// get_node()->get_logger(), +// "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", +// low_val.c_str()); +// } +// } +// for (size_t i = 0; i < +// params_.close.state.possible_closed_states_map.at(closed_state_name_) +// .joint_states.size(); +// ++i) +// { +// joint_state_values_[i] = +// params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states[i]; +// } +// gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); +// closeFlag_.store(false); +// gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); +// break; +// default: +// break; +// } +// } bool IOGripperController::set_commands( const std::unordered_map & command_states, @@ -408,7 +416,6 @@ void IOGripperController::handle_gripper_state_transition( const rclcpp::Time & current_time, const GripperTransitionIOs & ios, const uint & state, const std::string & transition_name, std::vector after_joint_states) { - using control_msgs::msg::IOGripperState; switch (state) { case IOGripperState::IDLE: @@ -454,7 +461,7 @@ void IOGripperController::handle_gripper_state_transition( if (check_state_ios) { - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); + gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_AFTER_COMMAND); } else if ((current_time - last_transition_time_).seconds() > params_.timeout) { @@ -821,7 +828,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); // reset gripper state buffer - gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); if (!params_.use_action) { @@ -854,11 +861,11 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and } openFlag_.store(true); gripper_service_buffer_.writeFromNonRT(service_mode_type::OPEN); - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); + gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_BEFORE_COMMAND); while (openFlag_.load()) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - if ((*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED)) + if ((*(gripper_state_buffer_.readFromRT()) == IOGripperState::HALTED)) { response->success = false; break; @@ -892,7 +899,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and return; } gripper_service_buffer_.writeFromNonRT(service_mode_type::CLOSE); - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); + gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_BEFORE_COMMAND); if (openFlag_.load()) { openFlag_.store(false); @@ -901,7 +908,7 @@ controller_interface::CallbackReturn IOGripperController::prepare_publishers_and while (closeFlag_.load()) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); - if ((*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED)) + if ((*(gripper_state_buffer_.readFromRT()) == IOGripperState::HALTED)) { response->success = false; break; @@ -1088,7 +1095,7 @@ rclcpp_action::GoalResponse IOGripperController::handle_goal( } gripper_service_buffer_.writeFromNonRT( (goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); - gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_BEFORE_COMMAND); + gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_BEFORE_COMMAND); } catch (const std::exception & e) { @@ -1106,7 +1113,7 @@ rclcpp_action::CancelResponse IOGripperController::handle_cancel( { (void)goal_handle; gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); - gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); return rclcpp_action::CancelResponse::ACCEPT; } @@ -1122,14 +1129,14 @@ void IOGripperController::execute(std::shared_ptr goal_handle auto feedback = std::make_shared(); while (true) { - if (*(gripper_state_buffer_.readFromRT()) == gripper_state_type::IDLE) + if (*(gripper_state_buffer_.readFromRT()) == IOGripperState::IDLE) { result->success = true; result->message = "Gripper action executed"; goal_handle->succeed(result); break; } - else if (*(gripper_state_buffer_.readFromRT()) == gripper_state_type::HALTED) + else if (*(gripper_state_buffer_.readFromRT()) == IOGripperState::HALTED) { result->success = false; result->message = "Gripper action halted"; From 7021394eb675acca53191956cbe02875348f8a75 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Tue, 28 Jan 2025 18:27:56 +0100 Subject: [PATCH 51/75] Set ros2_control_test_assets as test_depend --- io_gripper_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml index 8439ff92cf..2bdeb561db 100644 --- a/io_gripper_controller/package.xml +++ b/io_gripper_controller/package.xml @@ -26,7 +26,6 @@ rclcpp rclcpp_lifecycle realtime_tools - ros2_control_test_assets sensor_msgs std_msgs std_srvs @@ -36,6 +35,7 @@ ament_lint_common controller_interface hardware_interface + ros2_control_test_assets ament_cmake From 7c1d644572b08777c60fa9cf3c3dd8534014bdaa Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Sun, 16 Feb 2025 11:00:28 +0100 Subject: [PATCH 52/75] email updated Co-authored-by: Dr. Denis --- io_gripper_controller/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/package.xml b/io_gripper_controller/package.xml index 2bdeb561db..9e3e0501d1 100644 --- a/io_gripper_controller/package.xml +++ b/io_gripper_controller/package.xml @@ -8,7 +8,7 @@ Sachin Kumar Bence Magyar - Denis Štogl + Denis Štogl Christoph Froehlich Sai Kishor Kothakota From fd4107b4aee6b7e69cae02ce014085ba76c5728c Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Sun, 16 Feb 2025 11:01:00 +0100 Subject: [PATCH 53/75] Update io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp minor changes Co-authored-by: Dr. Denis --- .../include/io_gripper_controller/io_gripper_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 8b7ad7b47c..f2fc4e195f 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025, b>>robotized by Stogl Robotics +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 4864e10e0004fe6226cafbaccc29b9e4c151ba90 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Sun, 16 Feb 2025 11:04:49 +0100 Subject: [PATCH 54/75] Apply suggestions from code review Co-authored-by: Dr. Denis --- io_gripper_controller/src/io_gripper_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 6055242285..3446ad82af 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025, b>>robotized by Stogl Robotics +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 4a842856e3173523b44a8f2279346e857a0b6606 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Sun, 16 Feb 2025 12:59:08 +0100 Subject: [PATCH 55/75] udpate handle_gripper_transisition - parse method for adding multiple states - update to store configuraiton in new struct for closed_ios_ --- .../io_gripper_controller.hpp | 2 +- .../src/io_gripper_controller.cpp | 136 +++++++++++++++--- .../src/io_gripper_controller.yaml | 7 +- 3 files changed, 121 insertions(+), 24 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index f2fc4e195f..2afc8b7849 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -144,7 +144,7 @@ class IOGripperController : public controller_interface::ControllerInterface std::unordered_map command_ios; std::unordered_map state_ios; - bool has_multiple_end_states; + bool has_multiple_end_states = false; std::vector possible_states; std::unordered_map> multiple_states_ios; diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 3446ad82af..964f338017 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -422,16 +422,52 @@ void IOGripperController::handle_gripper_state_transition( // do nothing break; case IOGripperState::SET_BEFORE_COMMAND: - set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND"); - // TODO(destogl): check to use other Realtime sync object to have write from RT - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_AFTER_COMMAND); + if (set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND")) + { + // TODO(destogl): check to use other Realtime sync object to have write from RT + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_BEFORE_COMMAND); + } + else if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + } + last_transition_time_ = current_time; + break; + case IOGripperState::CHECK_BEFORE_COMMAND: + // check the state of the gripper + if (check_states(ios.set_before_state_ios, transition_name + " - CHECK_BEFORE_COMMAND")) + { + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); + } + else if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + } last_transition_time_ = current_time; break; case IOGripperState::SET_COMMAND: // now execute the command on the gripper - set_commands(ios.command_ios, transition_name + " - SET_COMMAND"); - // TODO(destogl): check to use other Realtime sync object to have write from RT - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); + if (set_commands(ios.command_ios, transition_name + " - SET_COMMAND")) + { + // TODO(destogl): check to use other Realtime sync object to have write from RT + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); + } + else if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - SET_COMMAND: Gripper didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + } last_transition_time_ = current_time; break; case IOGripperState::CHECK_COMMAND: @@ -451,7 +487,10 @@ void IOGripperController::handle_gripper_state_transition( // TODO(Sachin): store possible_end_state in a variable to publish on status topic break; } - check_state_ios = false; + else + { + check_state_ios = false; + } } } else // only single end state @@ -471,23 +510,48 @@ void IOGripperController::handle_gripper_state_transition( transition_name.c_str(), params_.timeout); gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } + last_transition_time_ = current_time; break; case IOGripperState::SET_AFTER_COMMAND: - set_commands(ios.set_after_command_ios, transition_name + " - SET_AFTER_COMMAND"); - - // set joint states - for (size_t i = 0; i < after_joint_states.size(); ++i) + if (set_commands(ios.set_after_command_ios, transition_name + " - SET_AFTER_COMMAND")) { - joint_state_values_[i] = params_.open.joint_states[i]; + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_AFTER_COMMAND); + } + else if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - SET_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } - - // Finish up the transition - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::IDLE); - openFlag_.store(false); - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); last_transition_time_ = current_time; + break; + case IOGripperState::CHECK_AFTER_COMMAND: + if (check_states(ios.set_after_state_ios, transition_name + " - CHECK_AFTER_COMMAND")) + { + // set joint states + for (size_t i = 0; i < after_joint_states.size(); ++i) + { + joint_state_values_[i] = params_.open.joint_states[i]; + } + // Finish up the transition + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::IDLE); + openFlag_.store(false); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); + } + else if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + } + last_transition_time_ = current_time; break; + default: break; } @@ -694,6 +758,15 @@ void IOGripperController::prepare_command_and_state_ios() } }; + auto parse_possible_states = [&](const std::string & name, const auto & value) + { + close_ios_.possible_states.push_back(name); + parse_interfaces_from_params( + value.high, 1.0, close_ios_.multiple_states_ios[name], state_if_ios); + parse_interfaces_from_params( + value.low, 0.0, close_ios_.multiple_states_ios[name], state_if_ios); + }; + // make full command ios lists -- just once parse_interfaces_from_params( params_.open.set_before_command.high, 1.0, open_ios_.set_before_command_ios, command_if_ios); @@ -710,6 +783,32 @@ void IOGripperController::prepare_command_and_state_ios() parse_interfaces_from_params( params_.open.set_after_command.low, 0.0, open_ios_.set_after_command_ios, command_if_ios); + // make full command ios lists for close -- just once + parse_interfaces_from_params( + params_.close.set_before_command.high, 1.0, close_ios_.set_before_command_ios, command_if_ios); + parse_interfaces_from_params( + params_.close.set_before_command.low, 0.0, close_ios_.set_before_command_ios, command_if_ios); + + parse_interfaces_from_params( + params_.close.command.high, 1.0, close_ios_.command_ios, command_if_ios); + parse_interfaces_from_params( + params_.close.command.low, 0.0, close_ios_.command_ios, command_if_ios); + + if (params_.possible_closed_states.size() > 0) + { + close_ios_.has_multiple_end_states = true; + close_ios_.possible_states = params_.possible_closed_states; + for (const auto & [name, value] : params_.close.state.possible_closed_states_map) + { + parse_possible_states(name, value); + } + } + + // parse_interfaces_from_params( + // params_.close.set_after_command_high, 1.0, close_ios_.set_after_command_ios, command_if_ios); + // parse_interfaces_from_params( + // params_.close.set_after_command_low, 0.0, close_ios_.set_after_command_ios, command_if_ios); + for (const auto & key : params_.close.set_before_command.high) { if (!key.empty()) @@ -751,6 +850,9 @@ void IOGripperController::prepare_command_and_state_ios() parse_interfaces_from_params(params_.open.state.high, 1.0, open_ios_.state_ios, state_if_ios); parse_interfaces_from_params(params_.open.state.low, 0.0, open_ios_.state_ios, state_if_ios); + // parse_interfaces_from_params(params_.close.state.high, 1.0, close_ios_.state_ios, + // state_if_ios); + for (const auto & [name, value] : params_.close.state.possible_closed_states_map) { for (const auto & key : value.high) diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml index 34a4185abd..125b1e7904 100644 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -1,4 +1,4 @@ -# Copyright (c) 2025, b>>robotized by Stogl Robotics +# Copyright (c) 2025, b»robotized by Stogl Robotics # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,11 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# - io_gripper_controller: use_action: { type: bool, From b16d309bceab986360ec201895062c7ef6759ad0 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Wed, 19 Feb 2025 00:41:45 +0100 Subject: [PATCH 56/75] fix logic for open gripper --- .../src/io_gripper_controller.cpp | 73 +++++++++++++------ 1 file changed, 49 insertions(+), 24 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 964f338017..c0994239cc 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -164,6 +164,7 @@ controller_interface::return_type IOGripperController::update( // []); // here joint states should be empty as we have multiple // states // handle_gripper_state_transition_close(*(gripper_state_buffer_.readFromRT())); + RCLCPP_INFO(get_node()->get_logger(), "CLOSE state is triggered"); handle_gripper_state_transition( time, close_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "close", params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states); @@ -190,6 +191,7 @@ bool IOGripperController::find_and_set_command(const std::string & name, const d { return it->set_value(value); } + RCLCPP_ERROR(get_node()->get_logger(), "Failed to set the command state for %s", name.c_str()); return false; } @@ -421,27 +423,40 @@ void IOGripperController::handle_gripper_state_transition( case IOGripperState::IDLE: // do nothing break; + + case IOGripperState::HALTED: + // do nothing + RCLCPP_ERROR( + get_node()->get_logger(), "%s - HALTED: Gripper is in HALTED state", + transition_name.c_str()); + break; + case IOGripperState::SET_BEFORE_COMMAND: + // RCLCPP_INFO(get_node()->get_logger(), "%s - SET_BEFORE_COMMAND: Setting the command + // states", transition_name.c_str()); if (set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND")) { // TODO(destogl): check to use other Realtime sync object to have write from RT - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_BEFORE_COMMAND); - } - else if ((current_time - last_transition_time_).seconds() > params_.timeout) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_BEFORE_COMMAND); } - last_transition_time_ = current_time; + // if ((current_time - last_transition_time_).seconds() > params_.timeout) + // { + // RCLCPP_ERROR( + // get_node()->get_logger(), + // "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", + // transition_name.c_str(), params_.timeout); + // gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + // } + // last_transition_time_ = current_time; break; case IOGripperState::CHECK_BEFORE_COMMAND: + // RCLCPP_INFO(get_node()->get_logger(), "%s - CHECK_BEFORE_COMMAND: Checking the state of the + // gripper", transition_name.c_str()); + gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); // check the state of the gripper if (check_states(ios.set_before_state_ios, transition_name + " - CHECK_BEFORE_COMMAND")) { - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); + gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); } else if ((current_time - last_transition_time_).seconds() > params_.timeout) { @@ -449,25 +464,32 @@ void IOGripperController::handle_gripper_state_transition( get_node()->get_logger(), "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", transition_name.c_str(), params_.timeout); - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } last_transition_time_ = current_time; break; case IOGripperState::SET_COMMAND: + RCLCPP_INFO( + get_node()->get_logger(), "%s - SET_COMMAND: Setting the command states", + transition_name.c_str()); // now execute the command on the gripper if (set_commands(ios.command_ios, transition_name + " - SET_COMMAND")) { // TODO(destogl): check to use other Realtime sync object to have write from RT - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); + gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); } - else if ((current_time - last_transition_time_).seconds() > params_.timeout) + else { - RCLCPP_ERROR( - get_node()->get_logger(), - "%s - SET_COMMAND: Gripper didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + RCLCPP_ERROR(get_node()->get_logger(), "failed"); } + // else if ((current_time - last_transition_time_).seconds() > params_.timeout) + // { + // RCLCPP_ERROR( + // get_node()->get_logger(), + // "%s - SET_COMMAND: Gripper didin't reached target state within %.2f seconds.", + // transition_name.c_str(), params_.timeout); + // gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + // } last_transition_time_ = current_time; break; case IOGripperState::CHECK_COMMAND: @@ -508,14 +530,14 @@ void IOGripperController::handle_gripper_state_transition( get_node()->get_logger(), "%s - CHECK_COMMAND: Gripper didin't reached target state within %.2f seconds.", transition_name.c_str(), params_.timeout); - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } last_transition_time_ = current_time; break; case IOGripperState::SET_AFTER_COMMAND: if (set_commands(ios.set_after_command_ios, transition_name + " - SET_AFTER_COMMAND")) { - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::CHECK_AFTER_COMMAND); + gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_AFTER_COMMAND); } else if ((current_time - last_transition_time_).seconds() > params_.timeout) { @@ -523,7 +545,7 @@ void IOGripperController::handle_gripper_state_transition( get_node()->get_logger(), "%s - SET_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", transition_name.c_str(), params_.timeout); - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } last_transition_time_ = current_time; break; @@ -537,9 +559,12 @@ void IOGripperController::handle_gripper_state_transition( joint_state_values_[i] = params_.open.joint_states[i]; } // Finish up the transition - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::IDLE); + gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); openFlag_.store(false); gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); + RCLCPP_INFO( + get_node()->get_logger(), "%s - CHECK_AFTER_COMMAND: Gripper reached target state", + transition_name.c_str()); } else if ((current_time - last_transition_time_).seconds() > params_.timeout) { @@ -547,7 +572,7 @@ void IOGripperController::handle_gripper_state_transition( get_node()->get_logger(), "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", transition_name.c_str(), params_.timeout); - gripper_open_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } last_transition_time_ = current_time; break; From f2dc9cc37aac316839fe7d6103411fa620e8a0b7 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Wed, 19 Feb 2025 01:09:41 +0100 Subject: [PATCH 57/75] close state and open state code updated --- .../io_gripper_controller.hpp | 4 +- .../src/io_gripper_controller.cpp | 165 +----------------- 2 files changed, 9 insertions(+), 160 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 2afc8b7849..7cee45c8c1 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -159,7 +159,7 @@ class IOGripperController : public controller_interface::ControllerInterface GripperTransitionIOs close_ios_; rclcpp::Time last_transition_time_; - + std::vector after_joint_states_; std::vector command_ios_open; std::vector command_ios_close; std::vector set_before_command_open; @@ -269,7 +269,7 @@ class IOGripperController : public controller_interface::ControllerInterface */ void handle_gripper_state_transition( const rclcpp::Time & current_time, const GripperTransitionIOs & ios, const uint & state, - const std::string & transition_name, std::vector after_joint_states); + const std::string & transition_name); /** * @brief Handles the state transition when closing the gripper. diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index c0994239cc..cd97eccba8 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -155,19 +155,12 @@ controller_interface::return_type IOGripperController::update( break; case service_mode_type::OPEN: handle_gripper_state_transition( - time, open_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "open", - params_.open.joint_states); + time, open_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "open"); break; case service_mode_type::CLOSE: - // handle_gripper_state_transition( - // time, close_ios_, *(gripper_state_buffer_.readFromRT()), "close", - // []); // here joint states should be empty as we have multiple - // states - // handle_gripper_state_transition_close(*(gripper_state_buffer_.readFromRT())); RCLCPP_INFO(get_node()->get_logger(), "CLOSE state is triggered"); handle_gripper_state_transition( - time, close_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "close", - params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states); + time, close_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "close"); break; default: @@ -227,147 +220,6 @@ bool IOGripperController::find_and_get_command(const std::string & name, double return false; } -// void IOGripperController::handle_gripper_state_transition_close(const gripper_state_type & state) -// { -// switch (state) -// { -// case gripper_state_type::IDLE: -// // do nothing -// break; -// case gripper_state_type::SET_BEFORE_COMMAND: -// for (size_t i = 0; i < set_before_command_close.size(); ++i) -// { -// setResult = -// find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); -// if (!setResult) -// { -// RCLCPP_ERROR( -// get_node()->get_logger(), -// "CLOSE - SET_BEFORE_COMMAND: Failed to set the command state for %s", -// set_before_command_close[i].c_str()); -// } -// } - -// gripper_state_buffer_.writeFromNonRT(gripper_state_type::CLOSE_GRIPPER); -// break; -// case gripper_state_type::CLOSE_GRIPPER: -// for (size_t i = 0; i < command_ios_close.size(); ++i) -// { -// setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); -// if (!setResult) -// { -// RCLCPP_ERROR( -// get_node()->get_logger(), "CLOSE_GRIPPER: Failed to set the command state for %s", -// command_ios_close[i].c_str()); -// } -// } - -// gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); -// break; -// case gripper_state_type::CHECK_GRIPPER_STATE: -// for (const auto & [state_name, state_params] : -// params_.close.state.possible_closed_states_map) -// { -// check_state_ios_ = false; -// for (const auto & high_val : state_params.high) -// { -// setResult = find_and_get_state(high_val, state_value_); -// if (!setResult) -// { -// RCLCPP_ERROR( -// get_node()->get_logger(), -// "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", high_val.c_str()); -// } -// else -// { -// if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) -// { -// check_state_ios_ = true; -// } -// else -// { -// check_state_ios_ = false; -// break; -// } -// } -// } -// for (const auto & low_val : state_params.low) -// { -// setResult = find_and_get_state(low_val, state_value_); -// if (!setResult) -// { -// RCLCPP_ERROR( -// get_node()->get_logger(), -// "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", low_val.c_str()); -// } -// else -// { -// if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) -// { -// check_state_ios_ = true; -// } -// else -// { -// check_state_ios_ = false; -// break; -// } -// } -// } -// if (check_state_ios_) -// { -// closed_state_name_ = state_name; -// gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); -// break; -// } -// } -// break; -// case gripper_state_type::SET_AFTER_COMMAND: -// closed_state_values_ = -// params_.close.state.possible_closed_states_map.at(closed_state_name_); - -// for (const auto & high_val : closed_state_values_.set_after_command_high) -// { -// setResult = find_and_set_command(high_val, 1.0); -// if (!setResult) -// { -// RCLCPP_ERROR( -// get_node()->get_logger(), -// "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", -// high_val.c_str()); -// } -// } - -// for (const auto & low_val : closed_state_values_.set_after_command_low) -// { -// RCLCPP_DEBUG( -// get_node()->get_logger(), "CLOSE - SET_AFTER_COMMAND: set low after command %s", -// low_val.c_str()); -// setResult = find_and_set_command(low_val, 0.0); -// if (!setResult) -// { -// RCLCPP_ERROR( -// get_node()->get_logger(), -// "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", -// low_val.c_str()); -// } -// } -// for (size_t i = 0; i < -// params_.close.state.possible_closed_states_map.at(closed_state_name_) -// .joint_states.size(); -// ++i) -// { -// joint_state_values_[i] = -// params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states[i]; -// } -// gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); -// closeFlag_.store(false); -// gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); -// break; -// default: -// break; -// } -// } - bool IOGripperController::set_commands( const std::unordered_map & command_states, const std::string & transition_name) @@ -416,7 +268,7 @@ bool IOGripperController::check_states( void IOGripperController::handle_gripper_state_transition( const rclcpp::Time & current_time, const GripperTransitionIOs & ios, const uint & state, - const std::string & transition_name, std::vector after_joint_states) + const std::string & transition_name) { switch (state) { @@ -432,8 +284,6 @@ void IOGripperController::handle_gripper_state_transition( break; case IOGripperState::SET_BEFORE_COMMAND: - // RCLCPP_INFO(get_node()->get_logger(), "%s - SET_BEFORE_COMMAND: Setting the command - // states", transition_name.c_str()); if (set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND")) { // TODO(destogl): check to use other Realtime sync object to have write from RT @@ -450,8 +300,6 @@ void IOGripperController::handle_gripper_state_transition( // last_transition_time_ = current_time; break; case IOGripperState::CHECK_BEFORE_COMMAND: - // RCLCPP_INFO(get_node()->get_logger(), "%s - CHECK_BEFORE_COMMAND: Checking the state of the - // gripper", transition_name.c_str()); gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); // check the state of the gripper if (check_states(ios.set_before_state_ios, transition_name + " - CHECK_BEFORE_COMMAND")) @@ -504,7 +352,7 @@ void IOGripperController::handle_gripper_state_transition( transition_name + " - CHECK_COMMAND")) { check_state_ios = true; - after_joint_states = + after_joint_states_ = params_.close.state.possible_closed_states_map.at(possible_end_state).joint_states; // TODO(Sachin): store possible_end_state in a variable to publish on status topic break; @@ -517,6 +365,7 @@ void IOGripperController::handle_gripper_state_transition( } else // only single end state { + after_joint_states_ = params_.open.joint_states; check_state_ios = check_states(ios.state_ios, transition_name + " - CHECK_COMMAND"); } @@ -554,9 +403,9 @@ void IOGripperController::handle_gripper_state_transition( if (check_states(ios.set_after_state_ios, transition_name + " - CHECK_AFTER_COMMAND")) { // set joint states - for (size_t i = 0; i < after_joint_states.size(); ++i) + for (size_t i = 0; i < after_joint_states_.size(); ++i) { - joint_state_values_[i] = params_.open.joint_states[i]; + joint_state_values_[i] = after_joint_states_[i]; } // Finish up the transition gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); From a1c8cb89c3d2abb95b889fd6c35bfcf9dd04afd5 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Wed, 19 Feb 2025 01:28:56 +0100 Subject: [PATCH 58/75] minor changes for timeout function --- .../src/io_gripper_controller.cpp | 40 +++++++++---------- 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index cd97eccba8..43c5ddee09 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -284,23 +284,23 @@ void IOGripperController::handle_gripper_state_transition( break; case IOGripperState::SET_BEFORE_COMMAND: + + last_transition_time_ = current_time; if (set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND")) { // TODO(destogl): check to use other Realtime sync object to have write from RT gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_BEFORE_COMMAND); } - // if ((current_time - last_transition_time_).seconds() > params_.timeout) - // { - // RCLCPP_ERROR( - // get_node()->get_logger(), - // "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", - // transition_name.c_str(), params_.timeout); - // gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); - // } - // last_transition_time_ = current_time; + if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + } break; case IOGripperState::CHECK_BEFORE_COMMAND: - gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); // check the state of the gripper if (check_states(ios.set_before_state_ios, transition_name + " - CHECK_BEFORE_COMMAND")) { @@ -330,15 +330,14 @@ void IOGripperController::handle_gripper_state_transition( { RCLCPP_ERROR(get_node()->get_logger(), "failed"); } - // else if ((current_time - last_transition_time_).seconds() > params_.timeout) - // { - // RCLCPP_ERROR( - // get_node()->get_logger(), - // "%s - SET_COMMAND: Gripper didin't reached target state within %.2f seconds.", - // transition_name.c_str(), params_.timeout); - // gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); - // } - last_transition_time_ = current_time; + if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - SET_COMMAND: Gripper didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); + } break; case IOGripperState::CHECK_COMMAND: // check the state of the gripper @@ -381,7 +380,6 @@ void IOGripperController::handle_gripper_state_transition( transition_name.c_str(), params_.timeout); gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } - last_transition_time_ = current_time; break; case IOGripperState::SET_AFTER_COMMAND: if (set_commands(ios.set_after_command_ios, transition_name + " - SET_AFTER_COMMAND")) @@ -396,7 +394,6 @@ void IOGripperController::handle_gripper_state_transition( transition_name.c_str(), params_.timeout); gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } - last_transition_time_ = current_time; break; case IOGripperState::CHECK_AFTER_COMMAND: @@ -423,7 +420,6 @@ void IOGripperController::handle_gripper_state_transition( transition_name.c_str(), params_.timeout); gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } - last_transition_time_ = current_time; break; default: From a12c335a641012f9ea44cb427a648a383e8b74d3 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Mon, 24 Feb 2025 00:01:02 +0100 Subject: [PATCH 59/75] timeout feature updated updated the param for yaml --- .../io_gripper_controller.hpp | 3 +- .../src/io_gripper_controller.cpp | 30 ++++++++++--------- .../test/test_io_gripper_controller.hpp | 9 +++--- 3 files changed, 23 insertions(+), 19 deletions(-) diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp index 7cee45c8c1..9bd314a984 100644 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -236,7 +236,8 @@ class IOGripperController : public controller_interface::ControllerInterface std::atomic openFlag_{false}; std::atomic closeFlag_{false}; - bool check_state_ios{false}; + std::atomic check_gripper_state_ios_{false}; + std::atomic transition_time_updated_{false}; private: /** diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp index 43c5ddee09..28cbb5ad6c 100644 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -277,21 +277,26 @@ void IOGripperController::handle_gripper_state_transition( break; case IOGripperState::HALTED: - // do nothing + gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); RCLCPP_ERROR( get_node()->get_logger(), "%s - HALTED: Gripper is in HALTED state", transition_name.c_str()); + transition_time_updated_.store(false); break; case IOGripperState::SET_BEFORE_COMMAND: - last_transition_time_ = current_time; + if (!transition_time_updated_.load()) + { + last_transition_time_ = current_time; + transition_time_updated_.store(true); + } if (set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND")) { // TODO(destogl): check to use other Realtime sync object to have write from RT gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_BEFORE_COMMAND); } - if ((current_time - last_transition_time_).seconds() > params_.timeout) + else if ((current_time - last_transition_time_).seconds() > params_.timeout) { RCLCPP_ERROR( get_node()->get_logger(), @@ -314,7 +319,6 @@ void IOGripperController::handle_gripper_state_transition( transition_name.c_str(), params_.timeout); gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); } - last_transition_time_ = current_time; break; case IOGripperState::SET_COMMAND: RCLCPP_INFO( @@ -326,11 +330,7 @@ void IOGripperController::handle_gripper_state_transition( // TODO(destogl): check to use other Realtime sync object to have write from RT gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); } - else - { - RCLCPP_ERROR(get_node()->get_logger(), "failed"); - } - if ((current_time - last_transition_time_).seconds() > params_.timeout) + else if ((current_time - last_transition_time_).seconds() > params_.timeout) { RCLCPP_ERROR( get_node()->get_logger(), @@ -341,7 +341,7 @@ void IOGripperController::handle_gripper_state_transition( break; case IOGripperState::CHECK_COMMAND: // check the state of the gripper - check_state_ios = true; + check_gripper_state_ios_.store(true); if (ios.has_multiple_end_states) { for (const auto & possible_end_state : ios.possible_states) @@ -350,7 +350,7 @@ void IOGripperController::handle_gripper_state_transition( ios.multiple_states_ios.at(possible_end_state), transition_name + " - CHECK_COMMAND")) { - check_state_ios = true; + check_gripper_state_ios_.store(true); after_joint_states_ = params_.close.state.possible_closed_states_map.at(possible_end_state).joint_states; // TODO(Sachin): store possible_end_state in a variable to publish on status topic @@ -358,17 +358,18 @@ void IOGripperController::handle_gripper_state_transition( } else { - check_state_ios = false; + check_gripper_state_ios_.store(false); } } } else // only single end state { after_joint_states_ = params_.open.joint_states; - check_state_ios = check_states(ios.state_ios, transition_name + " - CHECK_COMMAND"); + check_gripper_state_ios_.store( + check_states(ios.state_ios, transition_name + " - CHECK_COMMAND")); } - if (check_state_ios) + if (check_gripper_state_ios_.load()) { gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_AFTER_COMMAND); } @@ -411,6 +412,7 @@ void IOGripperController::handle_gripper_state_transition( RCLCPP_INFO( get_node()->get_logger(), "%s - CHECK_AFTER_COMMAND: Gripper reached target state", transition_name.c_str()); + transition_time_updated_.store(false); // resetting the flag } else if ((current_time - last_transition_time_).seconds() > params_.timeout) { diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index fd10c70c66..f8727c8be1 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -17,8 +17,8 @@ // [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. // -#ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_GRIPPER_IO_CONTROLLER_HPP_ -#define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_GRIPPER_IO_CONTROLLER_HPP_ +#ifndef TEST_IO_GRIPPER_CONTROLLER_HPP_ +#define TEST_IO_GRIPPER_CONTROLLER_HPP_ #include #include @@ -140,7 +140,7 @@ class IOGripperControllerFixture : public ::testing::Test // setting the command state interfaces manually std::vector command_itfs; - command_itfs.reserve(3); // TODO (Sachin) : change this some variable later + command_itfs.reserve(3); // TODO(Sachin) : change this some variable later command_itfs.emplace_back(greif_oeffen_wqg1_cmd_); command_itfs.emplace_back(greif_schliess_wqg2_cmd_); @@ -220,6 +220,7 @@ class IOGripperControllerFixture : public ::testing::Test void setup_parameters() { controller_->get_node()->set_parameter({"use_action", false}); + controller_->get_node()->set_parameter({"timeout", 5.0}); controller_->get_node()->set_parameter({"open_close_joints", open_close_joints}); controller_->get_node()->set_parameter({"open.joint_states", open_joint_states}); controller_->get_node()->set_parameter( @@ -433,4 +434,4 @@ class IOGripperControllerFixture : public ::testing::Test class IOGripperControllerTest : public IOGripperControllerFixture { }; -#endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_GRIPPER_IO_CONTROLLER_HPP_ +#endif // TEST_IO_GRIPPER_CONTROLLER_HPP_ From 0fe4a2352c9639ad508078927a69d6086a3af6fb Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 21 Mar 2025 15:52:26 +0100 Subject: [PATCH 60/75] test yaml file added --- io_gripper_controller/CMakeLists.txt | 1 + .../test/test_io_gripper_controller.yaml | 67 +++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 io_gripper_controller/test/test_io_gripper_controller.yaml diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt index 7446421bcd..32a584aaba 100644 --- a/io_gripper_controller/CMakeLists.txt +++ b/io_gripper_controller/CMakeLists.txt @@ -64,6 +64,7 @@ install( ) if(BUILD_TESTING) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") ament_add_gmock(test_load_io_gripper_controller test/test_load_io_gripper_controller.cpp) target_include_directories(test_load_io_gripper_controller PRIVATE include) diff --git a/io_gripper_controller/test/test_io_gripper_controller.yaml b/io_gripper_controller/test/test_io_gripper_controller.yaml new file mode 100644 index 0000000000..f9b4d470f4 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller.yaml @@ -0,0 +1,67 @@ +test_io_gripper_controller: + ros__parameters: + use_action: true + timeout: 5.0 + open_close_joints: [gripper_clamp_jaw] + + open: + joint_states: [0.0] + set_before_command: + high: [Release_Break_valve] + low: [Release_Something] # comment out otherwise throws an error + command: + high: [Open_valve] + low: [Close_valve] + state: + high: [Opened_signal] + low: [Closed_signal] + set_after_command: + high: [Release_Something] + low: [Release_Break_valve] + + possible_closed_states: ['empty_close', 'full_close'] + + close: + set_before_command: + high: [Release_Break_valve] + low: [Release_Something] + command: + high: [Close_valve] + low: [Open_valve] + state: + empty_close: + joint_states: [0.16] + high: [Closed_signal] + low: [Part_Grasped_signal] + set_after_command_high: [a] + set_after_command_low: [Release_Break_valve] + full_close: + joint_states: [0.08] + high: [Part_Grasped_signal] + low: [Closed_signal] + set_after_command_high: [Release_Something] + set_after_command_low: [Release_Break_valve] + + configurations: ["narrow_objects", "wide_objects"] + configuration_joints: [gripper_gripper_distance_joint] + + configuration_setup: + stichmass_125: + joint_states: [0.1] + command_high: [Narrow_Configuration_Cmd] + command_low: [Wide_Configuration_Cmd] + state_high: [Narrow_Configuraiton_Signal] + state_low: [Wide_Configuration_Signal] + stichmass_250: + joint_states: [0.2] + command_high: [Wide_Configuration_Cmd] + command_low: [Narrow_Configuration_Cmd] + state_high: [Wide_Configuration_Signal] + state_low: [Narrow_Configuraiton_Signal] + + gripper_specific_sensors: ["part_sensor_top", "part_sensor"] + sensors_interfaces: + hohenabfrage: + input: "Part_Sensor_Top_signal" + bauteilabfrage: + input: "Part_Grasped_signal" From 1f4bbb49e108473af0c27e98e2a61ac1cd29864f Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 21 Mar 2025 15:54:40 +0100 Subject: [PATCH 61/75] fixing header files --- .../test/test_io_gripper_controller.hpp | 81 +++++++++---------- 1 file changed, 40 insertions(+), 41 deletions(-) diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index f8727c8be1..f4bccc3a27 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - #ifndef TEST_IO_GRIPPER_CONTROLLER_HPP_ #define TEST_IO_GRIPPER_CONTROLLER_HPP_ +// #include "io_gripper_controller/io_gripper_controller.hpp" + +#include + #include #include #include @@ -28,16 +27,14 @@ #include #include -#include "gmock/gmock.h" -#include "hardware_interface/loaned_command_interface.hpp" -#include "hardware_interface/loaned_state_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "io_gripper_controller/io_gripper_controller.hpp" -#include "rclcpp/executor.hpp" -#include "rclcpp/parameter_value.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include +#include +#include +#include +#include +#include +#include +#include // TODO(anyone): replace the state and command message types using JointStateMsg = io_gripper_controller::IOGripperController::JointStateMsg; @@ -134,9 +131,11 @@ class IOGripperControllerFixture : public ::testing::Test protected: void SetUpController(const std::string controller_name = "test_io_gripper_controller") { + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "initializing controller"); ASSERT_EQ( controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), controller_interface::return_type::OK); + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "initialized successfully"); // setting the command state interfaces manually std::vector command_itfs; @@ -237,7 +236,7 @@ class IOGripperControllerFixture : public ::testing::Test {"open.set_after_command.low", open_set_after_command_low}); controller_->get_node()->set_parameter({"possible_closed_states", possible_closed_states}); - controller_->get_node()->set_parameter({"close.joint_states", close_joint_states}); + // controller_->get_node()->set_parameter({"close.joint_states", close_joint_states}); controller_->get_node()->set_parameter( {"close.set_before_command.high", close_set_before_command_high}); controller_->get_node()->set_parameter( @@ -345,37 +344,37 @@ class IOGripperControllerFixture : public ::testing::Test std::vector open_close_joints = {"gripper_clamp_jaw"}; std::vector open_joint_states = {0.0}; - std::vector open_set_before_command_high = {"EL2008/Bremse_WQG7"}; - std::vector open_set_before_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; - std::vector open_set_after_command_high = {"EL2008/Bremse_WQG7"}; - std::vector open_set_after_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; - std::vector open_command_high = {"EL2008/Greiferteil_Oeffnen_WQG1"}; - std::vector open_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; - std::vector open_state_high = {"EL1008/Greifer_Geoeffnet_BG01"}; - std::vector open_state_low = {"EL1008/Greifer_Geschloschen_BG02"}; + std::vector open_set_before_command_high = {"Release_Break_valve"}; + std::vector open_set_before_command_low = {"Release_Something"}; + std::vector open_set_after_command_high = {"Release_Break_valve"}; + std::vector open_set_after_command_low = {"Release_Something"}; + std::vector open_command_high = {"Open_valve"}; + std::vector open_command_low = {"Close_valve"}; + std::vector open_state_high = {"Opened_signal"}; + std::vector open_state_low = {"Closed_signal"}; std::vector possible_closed_states = {"empty_close", "full_close"}; std::vector close_joint_states = {0.08}; - std::vector close_set_before_command_high = {"EL2008/Bremse_WQG7"}; - std::vector close_set_before_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; - std::vector close_set_after_command_high = {"EL2008/Bremse_WQG7"}; - std::vector close_set_after_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; - std::vector close_command_high = {"EL2008/Greiferteil_Schliessen_WQG2"}; - std::vector close_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; - std::vector close_state_high = {"EL1008/Greifer_Geschloschen_BG02"}; - std::vector close_state_low = {"EL1008/Bauteilabfrage_BG06"}; - - std::vector configurations_list = {"stichmass_125"}; + std::vector close_set_before_command_high = {"Release_Break_valve"}; + std::vector close_set_before_command_low = {"Open_valve"}; + std::vector close_set_after_command_high = {"Release_Break_valve"}; + std::vector close_set_after_command_low = {"Open_valve"}; + std::vector close_command_high = {"Release_Something"}; + std::vector close_command_low = {"Open_valve"}; + std::vector close_state_high = {"Closed_signal"}; + std::vector close_state_low = {"Part_Grasped_signal"}; + + std::vector configurations_list = {"narrow_objects"}; std::vector configuration_joints = {"gripper_gripper_distance_joint"}; std::vector stichmass_joint_states = {0.125}; - std::vector stichmass_command_high = {"EL2008/Stichmass_125_WQG5"}; - std::vector stichmass_command_low = {"EL2008/Stichmass_250_WQG6"}; - std::vector stichmass_state_high = {"EL1008/Stichmass_125mm_BG03"}; - std::vector stichmass_state_low = {"EL1008/Stichmass_250mm_BG04"}; + std::vector stichmass_command_high = {"Narrow_Configuration_Cmd"}; + std::vector stichmass_command_low = {"Wide_Configuration_Cmd"}; + std::vector stichmass_state_high = {"Narrow_Configuraiton_Signal"}; + std::vector stichmass_state_low = {"Narrow_Configuraiton_Signal"}; - std::vector gripper_specific_sensors = {"hohenabfrage"}; - std::string gripper_interfaces_input = {"EL1008/Hohenabfrage_BG5"}; + std::vector gripper_specific_sensors = {"part_sensor_top"}; + std::string gripper_interfaces_input = {"Part_Sensor_Top_signal"}; std::vector joint_names_ = {"gripper_joint", "finger_joint"}; std::vector state_joint_names_ = {"gripper_joint"}; From 2fc1622f9e6bbf7360c25a4e187739ed24342bf7 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 21 Mar 2025 15:55:03 +0100 Subject: [PATCH 62/75] fixing header files --- io_gripper_controller/test/test_io_gripper_controller.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/io_gripper_controller/test/test_io_gripper_controller.hpp index f4bccc3a27..e6e19ace33 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.hpp +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -15,8 +15,6 @@ #ifndef TEST_IO_GRIPPER_CONTROLLER_HPP_ #define TEST_IO_GRIPPER_CONTROLLER_HPP_ -// #include "io_gripper_controller/io_gripper_controller.hpp" - #include #include @@ -35,6 +33,7 @@ #include #include #include +#include "io_gripper_controller/io_gripper_controller.hpp" // TODO(anyone): replace the state and command message types using JointStateMsg = io_gripper_controller::IOGripperController::JointStateMsg; From a0f5067581494096330825bbf22ccdd5ada36499 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 21 Mar 2025 15:58:31 +0100 Subject: [PATCH 63/75] header files changed for unit tests --- .../test/test_io_gripper_controller.cpp | 21 +++---------------- 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/io_gripper_controller/test/test_io_gripper_controller.cpp b/io_gripper_controller/test/test_io_gripper_controller.cpp index 6ba836e79e..ce0b14d062 100644 --- a/io_gripper_controller/test/test_io_gripper_controller.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,29 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - -#include "test_io_gripper_controller.hpp" -#include "rclcpp/rclcpp.hpp" - #include #include #include #include #include -// joint -// joint states -// dynamic state msg to status -// action server -// reconfire -> one or two configurations -// open gripper error when not expected behavior -// set_before and set_after commands -// add test for action and service open/close -// +#include +#include "test_io_gripper_controller.hpp" int main(int argc, char ** argv) { From 48ca870aac667027a025b5e1866f3c0eb6db52ed Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 21 Mar 2025 16:00:05 +0100 Subject: [PATCH 64/75] header files changed for unit tests --- .../test/test_load_io_gripper_controller.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/io_gripper_controller/test/test_load_io_gripper_controller.cpp b/io_gripper_controller/test/test_load_io_gripper_controller.cpp index ffaa94f2b0..a7bb98cbee 100644 --- a/io_gripper_controller/test/test_load_io_gripper_controller.cpp +++ b/io_gripper_controller/test/test_load_io_gripper_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 PAL Robotics SL. +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,6 +13,7 @@ // limitations under the License. #include + #include #include "controller_manager/controller_manager.hpp" @@ -31,10 +32,15 @@ TEST(TestLoadIOGripperController, load_controller) controller_manager::ControllerManager cm( executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/test_io_gripper_controller.yaml"; + + cm.set_parameter({"test_io_gripper_controller.params_file", test_file_path}); + + cm.set_parameter( + {"test_io_gripper_controller.type", "io_gripper_controller/IOGripperController"}); - ASSERT_NE( - cm.load_controller("test_io_gripper_controller", "io_gripper_controller/IOGripperController"), - nullptr); + ASSERT_NE(cm.load_controller("test_io_gripper_controller"), nullptr); rclcpp::shutdown(); } From e60b0833ea8f4fa9a2093a96d4a8f8c1fbf260ec Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 21 Mar 2025 16:00:43 +0100 Subject: [PATCH 65/75] header files changed for unit tests --- ...est_io_gripper_controller_all_param_set.cpp | 18 +++++++++--------- .../test/test_io_gripper_controller_close.cpp | 13 ++++--------- .../test/test_io_gripper_controller_open.cpp | 13 ++++--------- ...io_gripper_controller_open_close_action.cpp | 13 ++++--------- .../test_io_gripper_controller_reconfigure.cpp | 13 ++++--------- ...o_gripper_controller_reconfigure_action.cpp | 13 ++++--------- 6 files changed, 29 insertions(+), 54 deletions(-) diff --git a/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp index ce76fa2847..f919191953 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,31 +12,31 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - -#include "rclcpp/rclcpp.hpp" -#include "test_io_gripper_controller.hpp" - #include #include #include #include #include +#include +#include "test_io_gripper_controller.hpp" + // Test setting all params and getting success TEST_F(IOGripperControllerTest, AllParamsSetSuccess) { + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "Setting all parameters"); SetUpController(); + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "Setting up controllers"); setup_parameters(); + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "Setup parameters successfully"); + // configure success. ASSERT_EQ( controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "Setup parameters successfully"); } // Test not setting the one param and getting failure diff --git a/io_gripper_controller/test/test_io_gripper_controller_close.cpp b/io_gripper_controller/test/test_io_gripper_controller_close.cpp index e07f196c59..5b4fad4087 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_close.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_close.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,20 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - -#include "rclcpp/rclcpp.hpp" -#include "test_io_gripper_controller.hpp" - #include #include #include #include #include +#include +#include "test_io_gripper_controller.hpp" + // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, CloseGripperService) { diff --git a/io_gripper_controller/test/test_io_gripper_controller_open.cpp b/io_gripper_controller/test/test_io_gripper_controller_open.cpp index 1f5099cd73..6472eb1550 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,20 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - -#include "rclcpp/rclcpp.hpp" -#include "test_io_gripper_controller.hpp" - #include #include #include #include #include +#include +#include "test_io_gripper_controller.hpp" + // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, OpenGripperService) { diff --git a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp index 9e3b1aca84..0b466cf6f1 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,20 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - -#include "rclcpp/rclcpp.hpp" -#include "test_io_gripper_controller.hpp" - #include #include #include #include #include +#include +#include "test_io_gripper_controller.hpp" + // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, OpenCloseGripperAction) { diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp index 109071ed0a..7cd857e67a 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,20 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - -#include "rclcpp/rclcpp.hpp" -#include "test_io_gripper_controller.hpp" - #include #include #include #include #include +#include +#include "test_io_gripper_controller.hpp" + // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, ReconfigureGripperService) { diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp index b9f71b1acb..6cf8706411 100644 --- a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2025, b»robotized by Stogl Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,20 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - -#include "rclcpp/rclcpp.hpp" -#include "test_io_gripper_controller.hpp" - #include #include #include #include #include +#include +#include "test_io_gripper_controller.hpp" + // Test open gripper service sets command its as expected and publishes msg TEST_F(IOGripperControllerTest, ReconfigureGripperAction) { From 630db233df9219d4043aa66de50a1df2e2de26ef Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 13 Jun 2025 06:37:18 +0200 Subject: [PATCH 66/75] Rewrote IO Gripper Controller to GPIO Tool Controller. --- gpio_controllers/CMakeLists.txt | 56 +- gpio_controllers/gpio_controllers_plugin.xml | 9 + .../gpio_controllers/gpio_tool_controller.hpp | 316 ++++ gpio_controllers/package.xml | 6 +- gpio_controllers/src/gpio_tool_controller.cpp | 1080 +++++++++++++ .../src/gpio_tool_controller.yaml | 363 +++++ .../test_gpio_tool_controller.cpp | 0 .../test_gpio_tool_controller.hpp | 0 ...est_gpio_tool_controller_all_param_set.cpp | 0 .../test_gpio_tool_controller_close.cpp | 0 ..._gpio_tool_controller_gripper_example.yaml | 82 + ...est_gpio_tool_controller_lift_example.yaml | 37 + .../test_gpio_tool_controller_open.cpp | 0 ...gpio_tool_controller_open_close_action.cpp | 0 .../test_gpio_tool_controller_reconfigure.cpp | 0 ...pio_tool_controller_reconfigure_action.cpp | 0 .../test_load_gpio_tool_controller.cpp | 0 io_gripper_controller/CMakeLists.txt | 62 +- io_gripper_controller/COLCON_IGNORE | 0 io_gripper_controller/doc/userdoc.rst | 4 + .../io_gripper_controller.hpp | 409 ----- .../src/io_gripper_controller.cpp | 1343 ----------------- .../src/io_gripper_controller.yaml | 261 ---- .../test/test_io_gripper_controller.yaml | 67 - 24 files changed, 1975 insertions(+), 2120 deletions(-) create mode 100644 gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp create mode 100644 gpio_controllers/src/gpio_tool_controller.cpp create mode 100644 gpio_controllers/src/gpio_tool_controller.yaml rename io_gripper_controller/test/test_io_gripper_controller.cpp => gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.cpp (100%) rename io_gripper_controller/test/test_io_gripper_controller.hpp => gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.hpp (100%) rename io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp => gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_all_param_set.cpp (100%) rename io_gripper_controller/test/test_io_gripper_controller_close.cpp => gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_close.cpp (100%) create mode 100644 gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_gripper_example.yaml create mode 100644 gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml rename io_gripper_controller/test/test_io_gripper_controller_open.cpp => gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open.cpp (100%) rename io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp => gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open_close_action.cpp (100%) rename io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp => gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure.cpp (100%) rename io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp => gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure_action.cpp (100%) rename io_gripper_controller/test/test_load_io_gripper_controller.cpp => gpio_controllers/test/gpio_tool_controller/test_load_gpio_tool_controller.cpp (100%) create mode 100644 io_gripper_controller/COLCON_IGNORE delete mode 100644 io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp delete mode 100644 io_gripper_controller/src/io_gripper_controller.cpp delete mode 100644 io_gripper_controller/src/io_gripper_controller.yaml delete mode 100644 io_gripper_controller/test/test_io_gripper_controller.yaml diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index a19f7586af..3a6c7cf3e4 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -7,26 +7,49 @@ export_windows_symbols() # find dependencies find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) -find_package(generate_parameter_library REQUIRED) -find_package(control_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) generate_parameter_library(gpio_command_controller_parameters src/gpio_command_controller_parameters.yaml ) -add_library(gpio_controllers - SHARED +add_library(gpio_controllers SHARED src/gpio_command_controller.cpp ) target_include_directories(gpio_controllers PRIVATE include) -target_link_libraries(gpio_controllers PUBLIC - gpio_command_controller_parameters +target_link_libraries(gpio_controllers PUBLIC gpio_command_controller_parameters) +ament_target_dependencies(gpio_controllers PUBLIC + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + control_msgs +) + +generate_parameter_library(gpio_tool_controller_parameters + src/gpio_tool_controller.yaml +) +add_library(gpio_tool_controllers SHARED + src/gpio_tool_controller.cpp +) +target_compile_features(gpio_tool_controllers PUBLIC cxx_std_20) +target_include_directories(gpio_tool_controllers PUBLIC + "$" + "$") +target_link_libraries(gpio_tool_controllers PUBLIC + gpio_tool_controller_parameters controller_interface::controller_interface hardware_interface::hardware_interface pluginlib::pluginlib @@ -34,7 +57,9 @@ target_link_libraries(gpio_controllers PUBLIC rclcpp_lifecycle::rclcpp_lifecycle realtime_tools::realtime_tools ${control_msgs_TARGETS} - ${builtin_interfaces_TARGETS}) + ${builtin_interfaces_TARGETS} + ${std_srvs_TARGETS} + ) pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) @@ -46,6 +71,7 @@ install( install( TARGETS gpio_controllers + gpio_tool_controllers RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -78,6 +104,21 @@ if(BUILD_TESTING) gpio_controllers ros2_control_test_assets::ros2_control_test_assets ) + + # ament_add_gmock(test_gpio_tool_controller + # test/gpio_tool_controller/test_gpio_tool_controller.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_open.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_close.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_all_param_set.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_open_close_action.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_reconfigure.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_reconfigure_action.cpp + # ) + # target_include_directories(test_gpio_tool_controller PRIVATE include) + # target_link_libraries(test_gpio_tool_controller + # gpio_tool_controller + # ros2_control_test_assets::ros2_control_test_assets + # ) endif() ament_export_dependencies( @@ -92,5 +133,6 @@ ament_export_include_directories( ) ament_export_libraries( gpio_controllers + gpio_tool_controllers ) ament_package() diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml index 03fd3e1ee0..e0e0102128 100644 --- a/gpio_controllers/gpio_controllers_plugin.xml +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -5,3 +5,12 @@ + + + + + The gpio tool controllers enables control of the tools that are controlled using GPIOs. For example, pneumatic grippers, lifts, etc. It enables management of multiple engaged state, e.g., if gripper is closed with or without an object. + The controller can be also be used in the simple form where certian states of a device are set through GPIOs, e.g., manual and automatic mode. + + + diff --git a/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp new file mode 100644 index 0000000000..8b10e6b667 --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp @@ -0,0 +1,316 @@ +// Copyright (c) 2025, b»robotized Group +// +// 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 gpio_tool_controller__gpio_tool_controller_HPP_ +#define gpio_tool_controller__gpio_tool_controller_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include "control_msgs/action/gpio_tool_command.hpp" +#include "control_msgs/action/set_gpio_tool_config.hpp" +#include "control_msgs/msg/dynamic_interface_values.hpp" +#include "control_msgs/msg/interface_value.hpp" +#include "control_msgs/msg/gpio_tool_controller_state.hpp" +#include "control_msgs/msg/gpio_tool_transition.hpp" +#include "control_msgs/srv/set_gpio_tool_config.hpp" +#include "controller_interface/controller_interface.hpp" +#include "gpio_controllers/gpio_tool_controller_parameters.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace gpio_tool_controller +{ +/** + * @enum service_mode_type + * @brief Represents the various service modes of the tool. These modes represents the high level + * states of the tool. + * + * - IDLE: The tool is in an idle state, not performing any action. + * - DISENGAGING: The tool is in the process of opening. + * - ENGAGING: The tool is in the process of closing. + * - RECONFIGURING: The tool is in the process of reconfiguring to a new state. + */ +enum class ToolAction : std::uint8_t +{ + IDLE = 0, + DISENGAGING = 1, + ENGAGING = 2, + RECONFIGURING = 3, + CANCELING = 4, +}; + +class GpioToolController : public controller_interface::ControllerInterface +/** + * @brief GpioToolController class handles the control of an IO-based tools, like grippers, lifts, mode control. + */ +{ +public: + /** + * @brief Constructor for GpioToolController. + */ + GpioToolController(); + + /** + * @brief Initializes the controller. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_init() override; + + /** + * @brief Configures the command interfaces. + * @return InterfaceConfiguration for command interfaces. + */ + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief Configures the state interfaces. + * @return InterfaceConfiguration for state interfaces. + */ + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + /** + * @brief Configures the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Activates the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Deactivates the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Updates the controller state. + * @param time The current time. + * @param period The time since the last update. + * @return return_type indicating success or failure. + */ + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + gpio_tool_controller::Params params_; + + struct ToolTransitionIOs + { + std::vector possible_states; + + std::unordered_map> set_before_commands; + std::unordered_map set_before_commands_start_index; + std::unordered_map> set_before_states; + std::unordered_map set_before_states_start_index; + + std::unordered_map> commands; + std::unordered_map commands_start_index; + + std::unordered_map> states; + std::unordered_map states_start_index; + std::unordered_map> states_joint_states; + std::unordered_map> set_after_commands; + std::unordered_map set_after_commands_start_index; + std::unordered_map> set_after_states; + std::unordered_map set_after_states_start_index; + }; + + ToolTransitionIOs disengaged_gpios_; + ToolTransitionIOs engaged_gpios_; + ToolTransitionIOs reconfigure_gpios_; + + rclcpp::Time state_change_start_; + std::string current_configuration_; + std::string current_state_; + + std::unordered_set command_if_ios; + std::unordered_set state_if_ios; + + using EngagingSrvType = example_interfaces::srv::Trigger; + using ResetSrvType = example_interfaces::srv::Trigger; + using ConfigSrvType = control_msgs::srv::SetGPIOToolConfig; + using DynInterfaceMsg = control_msgs::msg::DynamicInterfaceValues; + using EngagingActionType = control_msgs::action::GPIOToolCommand; + using ConfigActionType = control_msgs::action::SetGPIOToolConfig; + using GPIOToolTransition = control_msgs::msg::GPIOToolTransition; + using ControllerStateMsg = control_msgs::msg::GPIOToolControllerState; + + std::shared_ptr param_listener_; + rclcpp::Service::SharedPtr disengaged_service_; + rclcpp::Service::SharedPtr engaged_service_; + rclcpp::Service::SharedPtr reconfigure_tool_service_; + rclcpp_action::Server::SharedPtr engaging_action_server_; + rclcpp_action::Server::SharedPtr config_action_server_; + rclcpp::Service::SharedPtr reset_service_; + + // Store current action tool is executing + std::atomic current_tool_action_{ToolAction::IDLE}; + std::atomic current_tool_transition_{GPIOToolTransition::IDLE}; + std::atomic reset_halted_{false}; + std::atomic transition_time_updated_{false}; + std::atomic> target_configuration_; + + using ToolJointStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr t_js_publisher_; + std::unique_ptr tool_joint_state_publisher_; + std::vector joint_states_values_; + using InterfacePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr if_publisher_; + std::unique_ptr interface_publisher_; + using ControllerStatePublisher = + realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr t_s_publisher_; + std::unique_ptr controller_state_publisher_; + + EngagingSrvType::Response process_engaging_request( + const ToolAction & requested_action, const std::string & requested_action_name); + + EngagingSrvType::Response process_reconfigure_request(const std::string & config_name); + + EngagingSrvType::Response service_wait_for_transition_end( + const std::string & requested_action_name); + +private: + /** + * @brief Handles the state transition when enaging the tool. + * @param state The current transition of the tool. + */ + void handle_tool_state_transition( + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, + const std::string & target_state, std::vector & joint_states, + const size_t joint_states_start_index, std::string & end_state); + + void check_tool_state_and_switch( + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, + std::vector & joint_states, const size_t joint_states_start_index, + const std::string & output_prefix, const uint8_t next_transition, + std::string & found_state_name); + + /** + * @brief Prepares the command and state IOs. + */ + void prepare_command_and_state_ios(); + + /** + * @brief Prepares the publishers and services. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn prepare_publishers_and_services(); + + /** + * @brief Publishes the the values from the RT loop. + */ + void publish_topics(const rclcpp::Time & current_time); + + /** + * @brief Checks the tools state. + */ + void check_tool_state(const rclcpp::Time & current_time); + + bool set_commands( + const std::unordered_map & commands, + const size_t start_index, + const std::string & transition_name, + const uint8_t next_transition); + bool check_states( + const rclcpp::Time & current_time, const std::unordered_map & states, const size_t start_index, const std::string & transition_name, const uint8_t next_transition); + + std::vector configurations_list_; + std::vector config_map_; + double state_value_; + std::string configuration_key_; + bool check_state_ios_; + std::string closed_state_name_; + std::vector::iterator config_index_; + rclcpp::CallbackGroup::SharedPtr disengaging_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr engaging_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr reconfigure_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr reset_service_callback_group_; + + /** + * @brief Handles the goal for the tool action. + * @param uuid The UUID of the goal. + * @param goal The goal to handle. + * @return GoalResponse indicating acceptance or rejection of the goal. + */ + rclcpp_action::GoalResponse handle_engaging_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + /** + * @brief Handles the cancellation of the tool action. + * @param goal_handle The handle of the goal to cancel. + * @return CancelResponse indicating acceptance or rejection of the cancellation. + */ + rclcpp_action::CancelResponse handle_engaging_cancel( + std::shared_ptr> goal_handle); + + /** + * @brief Handles the accepted goal for the tool action. + * @param goal_handle The handle of the accepted goal. + */ + void handle_engaging_accepted( + std::shared_ptr> goal_handle); + + /** + * @brief Handles the goal for the gripper configuration action. + * @param uuid The UUID of the goal. + * @param goal The goal to handle. + * @return GoalResponse indicating acceptance or rejection of the goal. + */ + rclcpp_action::GoalResponse handle_config_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + /** + * @brief Handles the cancellation of the gripper configuration action. + * @param goal_handle The handle of the goal to cancel. + * @return CancelResponse indicating acceptance or rejection of the cancellation. + */ + rclcpp_action::CancelResponse handle_config_cancel( + std::shared_ptr> goal_handle); + + /** + * @brief Handles the accepted goal for the tool state and configuration changes. + * @param goal_handle The handle of the accepted goal. + */ + template + void handle_action_accepted( + std::shared_ptr> goal_handle); +}; + +} // namespace gpio_tool_controller + +#endif // gpio_tool_controller__gpio_tool_controller_HPP_ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index d64a123439..e2f0f2dd2a 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -23,13 +23,15 @@ ros2_control_cmake + control_msgs controller_interface + generate_parameter_library hardware_interface rclcpp rclcpp_lifecycle - control_msgs realtime_tools - generate_parameter_library + sensor_msgs + std_srvs pluginlib diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp new file mode 100644 index 0000000000..d9d6aa2679 --- /dev/null +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -0,0 +1,1080 @@ +// Copyright (c) 2025, b»robotized Group +// +// 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 "gpio_controllers/gpio_tool_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "rclcpp/version.h" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 17 +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); +#else +static const rmw_qos_profile_t qos_services = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; +#endif + +} // namespace + +namespace gpio_tool_controller +{ +GpioToolController::GpioToolController() : controller_interface::ControllerInterface() {} + +controller_interface::CallbackReturn GpioToolController::on_init() +{ + current_tool_action_.store(ToolAction::IDLE); + current_tool_transition_.store(GPIOToolTransition::IDLE); + target_configuration_.store(std::make_shared("")); + current_state_ = ""; + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GpioToolController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + bool all_good = true; + + // TODO(destogl): this can be resolved with parameter pairs too! We can remove this method. + auto check_joint_states_sizes = [this](const size_t joint_states_size, const size_t joint_states_values_size, const std::string & param_name) { + if (joint_states_size != joint_states_values_size) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of '%s' joint values and defined states must be equal. " + "Joint states values size: %zu, expected size: %zu.", + param_name.c_str(), joint_states_values_size, joint_states_size); + return false; + } + return true; + }; + + if (!check_joint_states_sizes( + params_.engaged_joints.size(), params_.disengaged.joint_states.size(), "engaged")) + { + all_good = false; + } + + if (params_.possible_engaged_states.size() != params_.engaged.states.possible_engaged_states_map.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'possible_engaged_states' and 'engaged.states' parameters must be equal. " + "Possible engaged states size: %zu, engaged states size: %zu.", + params_.possible_engaged_states.size(), params_.engaged.states.possible_engaged_states_map.size()); + all_good = false; + } + + for (const auto & [name, info] : params_.engaged.states.possible_engaged_states_map) + { + if (!check_joint_states_sizes( + params_.engaged_joints.size(), info.joint_states.size(), "engaged " + name + " state")) + { + all_good = false; + } + } + + auto check_parameter_pairs = [this](const std::vector & interfaces, const std::vector & values, const std::string & parameter_name) { + if (interfaces.size() != values.size()) { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of parameter's '%s' interfaces and values must be equal.", parameter_name.c_str()); + return false; + } + return true; + }; + + if (!check_parameter_pairs(params_.disengaged.set_before_command.interfaces, params_.disengaged.set_before_command.values, "disengaged before command") || + !check_parameter_pairs(params_.disengaged.set_before_state.interfaces, params_.disengaged.set_before_state.values, "disengaged before state") || + !check_parameter_pairs(params_.disengaged.command.interfaces, params_.disengaged.command.values, "disengaged command") || + !check_parameter_pairs(params_.disengaged.state.interfaces, params_.disengaged.state.values, "disengaged state") || + !check_parameter_pairs(params_.disengaged.set_after_command.interfaces, params_.disengaged.set_after_command.values, "disengaged after command") || + !check_parameter_pairs(params_.disengaged.set_after_state.interfaces, params_.disengaged.set_after_state.values, "disengaged after state")) + { + all_good = false; + } + + if (!check_parameter_pairs(params_.engaged.set_before_command.interfaces, params_.engaged.set_before_command.values, "engaged before command") || + !check_parameter_pairs(params_.engaged.set_before_state.interfaces, params_.engaged.set_before_state.values, "engaged before state") || + !check_parameter_pairs(params_.engaged.command.interfaces, params_.engaged.command.values, "engaged command")) + { + all_good = false; + } + + for (const auto & [name, info] : params_.engaged.states.possible_engaged_states_map) + { + if (!check_parameter_pairs(info.interfaces, info.values, "engaged " + name + " states") || + !check_parameter_pairs(info.set_after_command_interfaces, info.set_after_command_values, "engaged " + name + " after command") || + !check_parameter_pairs(info.set_after_state_interfaces, info.set_after_state_values, "engaged " + name + " after state")) + { + all_good = false; + } + } + + if (params_.configurations.size() != params_.configuration_setup.configurations_map.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'configurations' and 'configuration_setup' parameters must be equal. " + "Configurations size: %zu, configuration joints size: %zu.", + params_.configurations.size(), params_.configuration_joints.size()); + all_good = false; + } + for (const auto & [name, info] : params_.configuration_setup.configurations_map) + { + if (!check_joint_states_sizes( + params_.configuration_joints.size(), info.joint_states.size(), "configuration " + name) || + !check_parameter_pairs(info.set_before_command_interfaces, info.set_before_command_values, "configuration " + name + " before commands") || + !check_parameter_pairs(info.set_before_state_interfaces, info.set_before_state_values, "configuration " + name + " before states") || + !check_parameter_pairs(info.command_interfaces, info.command_values, "configuration " + name + " commands") || + !check_parameter_pairs(info.state_interfaces, info.state_values, "configuration " + name + " states") || + !check_parameter_pairs(info.set_after_command_interfaces, info.set_after_command_values, "configuration " + name + " after commands") || + !check_parameter_pairs(info.set_after_state_interfaces, info.set_after_state_values, "configuration " + name + " after states") + ) + { + all_good = false; + } + } + + if (params_.tool_specific_sensors.size() != params_.sensors_interfaces.tool_specific_sensors_map.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'tool_specific_sensors' and 'sensors_interfaces' parameters must be equal. " + "Tool specific sensors size: %zu, sensors interfaces size: %zu.", + params_.tool_specific_sensors.size(), params_.sensors_interfaces.tool_specific_sensors_map.size()); + all_good = false; + } + for (const auto & [name, info] : params_.sensors_interfaces.tool_specific_sensors_map) + { + if (info.interface.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Interfaces for tool specific sensor '%s' cannot be empty.", name.c_str()); + all_good = false; + } + } + + if (!all_good) + { + RCLCPP_FATAL( + get_node()->get_logger(), "One or more parameters are not configured correctly. See above messges for details."); + return CallbackReturn::FAILURE; + } + + prepare_command_and_state_ios(); + + auto result = prepare_publishers_and_services(); + if (result != controller_interface::CallbackReturn::SUCCESS) + { + return result; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration GpioToolController::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(command_if_ios.size()); + for (const auto & command_io : command_if_ios) + { + command_interfaces_config.names.push_back(command_io); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration GpioToolController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_if_ios.size()); + for (const auto & state_io : state_if_ios) + { + state_interfaces_config.names.push_back(state_io); + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn GpioToolController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + check_tool_state(get_node()->now()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GpioToolController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joint_states_values_.resize( + params_.engaged_joints.size() + params_.configuration_joints.size(), + std::numeric_limits::quiet_NaN()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioToolController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) + { + switch (current_tool_action_.load()) + { + case ToolAction::IDLE: + { + // do nothing + break; + } + case ToolAction::DISENGAGING: + { + handle_tool_state_transition( + time, disengaged_gpios_, params_.disengaged.name, joint_states_values_, 0, current_state_); + break; + } + case ToolAction::ENGAGING: + { + handle_tool_state_transition( + time, engaged_gpios_, params_.engaged.name, joint_states_values_, 0, current_state_); + break; + } + case ToolAction::RECONFIGURING: + { + handle_tool_state_transition( + time, reconfigure_gpios_, *(target_configuration_.load()), joint_states_values_, params_.engaged_joints.size(), current_configuration_); + break; + } + case ToolAction::CANCELING: + { + RCLCPP_ERROR( + get_node()->get_logger(), "%s - CANCELING: Tool action is being canceled, " + "going to HALTED. Reset the tool using '~/reset_halted' service. After that set sensible state.", current_state_.c_str()); + current_tool_transition_.store(GPIOToolTransition::HALTED); + check_tool_state(time); + std::vector tmp_vec; + std::string tmp_str; + handle_tool_state_transition( + time, ToolTransitionIOs(), "", tmp_vec, 0, + tmp_str); // parameters don't matter as end up processing only the halted state + break; + } + default: + { + break; + } + } + + publish_topics(time); + + return controller_interface::return_type::OK; +} + +bool GpioToolController::set_commands( + const std::unordered_map & commands, const size_t start_index, const std::string & transition_name, const uint8_t next_transition) +{ + bool all_successful = true; + if (commands.empty()) + { + return all_successful; // nothing to set, return true + } + for (size_t i = start_index; i < (start_index + commands.size()); ++i) + { + if (!command_interfaces_[i].set_value(commands.at(command_interfaces_[i].get_name()))) + { + RCLCPP_ERROR( + get_node()->get_logger(), "%s: Failed to set the command state for %s", + transition_name.c_str(), command_interfaces_[i].get_name().c_str()); + all_successful = false; + } + } + + if (all_successful) + { + RCLCPP_INFO( + get_node()->get_logger(), "%s: Setting the before command states", + transition_name.c_str()); + current_tool_transition_.store(next_transition); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s: Error occured when setting commands - see above for details.", + transition_name.c_str()); + } + + return all_successful; +} + +bool GpioToolController::check_states( + const rclcpp::Time & current_time, const std::unordered_map & states, const size_t start_index, const std::string & transition_name, const uint8_t next_transition) +{ + bool all_correct = true; + if (states.empty()) + { + return all_correct; // nothing to check, return true + } + for (size_t i = start_index; i < (start_index + states.size()); ++i) + { + const double current_state_value = state_interfaces_.at(i).get_optional().value_or(std::numeric_limits::quiet_NaN()); + if ( + abs(current_state_value - states.at(state_interfaces_.at(i).get_name())) > params_.tolerance) + { + RCLCPP_INFO( + get_node()->get_logger(), "%s: State value for %s doesn't match", + transition_name.c_str(), state_interfaces_[i].get_name().c_str()); + all_correct = false; + } + } + + if (all_correct) + { + RCLCPP_INFO( + get_node()->get_logger(), "%s: Tool reached target state.", + transition_name.c_str()); + // if the Tool is in the correct state, we can set the command + current_tool_transition_.store(next_transition); + } + if ((current_time - state_change_start_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s: Tool didin't reached target state within %.2f seconds.", + transition_name.c_str(), params_.timeout); + current_tool_transition_.store(GPIOToolTransition::HALTED); + } + + return all_correct; +} + +void GpioToolController::check_tool_state_and_switch( + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, std::vector & joint_states, const size_t joint_states_start_index, const std::string & output_prefix, const uint8_t next_transition, std::string & found_state_name) +{ + for (const auto & [state_name, states] : ios.states) + { + bool state_exists = ios.states_start_index.find(state_name) != ios.states_start_index.end(); + RCLCPP_INFO( + get_node()->get_logger(), "%s - CHECK_COMMAND: Checking state '%s' for tool. State exists %s", + output_prefix.c_str(), state_name.c_str(), (state_exists ? "true" : "false")); + + if (check_states(current_time, states, ios.states_start_index.at(state_name), output_prefix, next_transition)) + { + const auto new_joint_states = ios.states_joint_states.at(state_name); + std::copy(new_joint_states.begin(), new_joint_states.end(), joint_states.begin() + joint_states_start_index); + found_state_name = state_name; + break; + } + } +} + +void GpioToolController::handle_tool_state_transition( + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, + const std::string & target_state, std::vector & joint_states, const size_t joint_states_start_index, std::string & end_state) +{ + bool finish_transition_to_state = false; + switch (current_tool_transition_.load()) + { + case GPIOToolTransition::IDLE: + // do nothing + break; + + case GPIOToolTransition::HALTED: + RCLCPP_DEBUG( + get_node()->get_logger(), "%s - HALTED: Tool aborted in HALTED state - don't ", + target_state.c_str()); + if (reset_halted_.load()) + { + // check here the state of the tool if you can figure it out. If not - set unknown. + reset_halted_.store(false); + finish_transition_to_state = true; + } + break; + + case GPIOToolTransition::SET_BEFORE_COMMAND: + if (!transition_time_updated_.load()) + { + state_change_start_ = current_time; + transition_time_updated_.store(true); + } + set_commands( + ios.set_before_commands.at(target_state), ios.set_before_commands_start_index.at(target_state), + target_state + " - SET_BEFORE_COMMAND", GPIOToolTransition::CHECK_BEFORE_COMMAND); + break; + case GPIOToolTransition::CHECK_BEFORE_COMMAND: + check_states( + current_time, ios.set_before_states.at(target_state), + ios.set_before_states_start_index.at(target_state), + target_state + " - CHECK_BEFORE_COMMAND", GPIOToolTransition::SET_COMMAND); + break; + case GPIOToolTransition::SET_COMMAND: + set_commands(ios.commands.at(target_state), ios.commands_start_index.at(target_state), target_state + " - SET_COMMAND", GPIOToolTransition::CHECK_COMMAND); + break; + case GPIOToolTransition::CHECK_COMMAND: + check_tool_state_and_switch(current_time, ios, joint_states, joint_states_start_index, target_state + " - CHECK_COMMAND", GPIOToolTransition::SET_AFTER_COMMAND, end_state); + break; + case GPIOToolTransition::SET_AFTER_COMMAND: + set_commands( + ios.set_after_commands.at(end_state), ios.set_after_commands_start_index.at(end_state), + target_state + " - SET_AFTER_COMMAND", GPIOToolTransition::CHECK_AFTER_COMMAND); + break; + + case GPIOToolTransition::CHECK_AFTER_COMMAND: + if (check_states(current_time, ios.set_after_states.at(end_state), ios.set_after_states_start_index.at(end_state), target_state + " - CHECK_AFTER_COMMAND", GPIOToolTransition::IDLE)) + { + finish_transition_to_state = true; + } + break; + + default: + break; + } + + if (finish_transition_to_state) + { + current_tool_action_.store(ToolAction::IDLE); + current_tool_transition_.store(GPIOToolTransition::IDLE); + transition_time_updated_.store(false); // resetting the flag + + RCLCPP_INFO( + get_node()->get_logger(), "%s: Transitions finished!", + target_state.c_str()); + } +} + +void GpioToolController::prepare_command_and_state_ios() +{ + auto parse_interfaces_from_params = [&]( + const std::vector & interfaces, + const std::vector & values, + const std::string & param_name, + std::unordered_map & ios, + std::unordered_set & interface_list, + size_t & ios_start_index) + { + if (interfaces.size() != values.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of interfaces and values parameters for '%s' must be the same. " + "Interfaces size: %zu, Values size: %zu", + param_name.c_str(), interfaces.size(), values.size()); + return; + } + + if (interfaces.empty()) + { + ios_start_index = -1; + return; + } + + ios_start_index = interface_list.size(); + ios.reserve(interfaces.size()); + interface_list.reserve(interface_list.size() + interfaces.size()); + + for (size_t i = 0; i < interfaces.size(); ++i) + { + if (!interfaces[i].empty()) + { + ios[interfaces[i]] = values[i]; + interface_list.insert(interfaces[i]); + } + } + }; + + // Disengaged IOs + disengaged_gpios_.possible_states.push_back(params_.disengaged.name); + + disengaged_gpios_.set_before_commands[params_.disengaged.name] = + std::unordered_map(); + parse_interfaces_from_params( + params_.disengaged.set_before_command.interfaces, params_.disengaged.set_before_command.values, "disengaged.set_before_command", disengaged_gpios_.set_before_commands[params_.disengaged.name], command_if_ios, disengaged_gpios_.set_before_commands_start_index[params_.disengaged.name]); + + disengaged_gpios_.set_before_states[params_.disengaged.name] = + std::unordered_map(); + parse_interfaces_from_params( + params_.disengaged.set_before_state.interfaces, params_.disengaged.set_before_state.values, "disengaged.set_before_state", disengaged_gpios_.set_before_states[params_.disengaged.name], state_if_ios, disengaged_gpios_.set_before_states_start_index[params_.disengaged.name]); + + disengaged_gpios_.commands[params_.disengaged.name] = + std::unordered_map(); + parse_interfaces_from_params( + params_.disengaged.command.interfaces, params_.disengaged.command.values, "disengaged.command", disengaged_gpios_.commands[params_.disengaged.name], command_if_ios, disengaged_gpios_.commands_start_index[params_.disengaged.name]); + + disengaged_gpios_.states[params_.disengaged.name] = + std::unordered_map(); + + parse_interfaces_from_params( + params_.disengaged.state.interfaces, params_.disengaged.state.values, "disengaged.state", disengaged_gpios_.states[params_.disengaged.name], state_if_ios, disengaged_gpios_.states_start_index[params_.disengaged.name]); + + disengaged_gpios_.states_joint_states[params_.disengaged.name] = + params_.disengaged.joint_states; + + disengaged_gpios_.set_after_commands[params_.disengaged.name] = + std::unordered_map(); + disengaged_gpios_.set_after_states[params_.disengaged.name] = + std::unordered_map(); + + parse_interfaces_from_params( + params_.disengaged.set_after_command.interfaces, params_.disengaged.set_after_command.values, + "disengaged.set_after_command", disengaged_gpios_.set_after_commands[params_.disengaged.name], command_if_ios, + disengaged_gpios_.set_after_commands_start_index[params_.disengaged.name]); + parse_interfaces_from_params( + params_.disengaged.set_after_state.interfaces, params_.disengaged.set_after_state.values, + "disengaged.set_after_state", disengaged_gpios_.set_after_states[params_.disengaged.name], state_if_ios, + disengaged_gpios_.set_after_states_start_index[params_.disengaged.name]); + + // Engaged IOs + engaged_gpios_.set_before_commands[params_.engaged.name] = + std::unordered_map(); + parse_interfaces_from_params( + params_.engaged.set_before_command.interfaces, params_.engaged.set_before_command.values, + "engaged.set_before_command", engaged_gpios_.set_before_commands[params_.engaged.name], command_if_ios, engaged_gpios_.set_before_commands_start_index[params_.engaged.name]); + engaged_gpios_.set_before_states[params_.engaged.name] = + std::unordered_map(); + parse_interfaces_from_params( + params_.engaged.set_before_state.interfaces, params_.engaged.set_before_state.values, + "engaged.set_before_state", engaged_gpios_.set_before_states[params_.engaged.name], state_if_ios, engaged_gpios_.set_before_states_start_index[params_.engaged.name]); + engaged_gpios_.commands[params_.engaged.name] = std::unordered_map(); + parse_interfaces_from_params( + params_.engaged.command.interfaces, params_.engaged.command.values, "engaged.command", + engaged_gpios_.commands[params_.engaged.name], command_if_ios, engaged_gpios_.commands_start_index[params_.engaged.name]); + + engaged_gpios_.possible_states = params_.possible_engaged_states; + for (const auto & state : params_.possible_engaged_states) + { + engaged_gpios_.states[state] = std::unordered_map(); + parse_interfaces_from_params( + params_.engaged.states.possible_engaged_states_map.at(state).interfaces, + params_.engaged.states.possible_engaged_states_map.at(state).values, "engaged.states." + state, + engaged_gpios_.states[state], state_if_ios, engaged_gpios_.states_start_index[state]); + + engaged_gpios_.states_joint_states[state] = + params_.engaged.states.possible_engaged_states_map.at(state).joint_states; + + engaged_gpios_.set_after_commands[state] = + std::unordered_map(); + engaged_gpios_.set_after_states[state] = + std::unordered_map(); + parse_interfaces_from_params( + params_.engaged.states.possible_engaged_states_map.at(state).set_after_command_interfaces, + params_.engaged.states.possible_engaged_states_map.at(state).set_after_command_values, + "engaged.set_after_command." + state, engaged_gpios_.set_after_commands[state], command_if_ios, + engaged_gpios_.set_after_commands_start_index[state]); + parse_interfaces_from_params( + params_.engaged.states.possible_engaged_states_map.at(state).set_after_state_interfaces, + params_.engaged.states.possible_engaged_states_map.at(state).set_after_state_values, + "engaged.set_after_state." + state, engaged_gpios_.set_after_states[state], state_if_ios, + engaged_gpios_.set_after_states_start_index[state]); + } + + // Configurations IOs + reconfigure_gpios_.possible_states = params_.configurations; + for (const auto & state : reconfigure_gpios_.possible_states) + { + reconfigure_gpios_.set_before_commands[state] = + std::unordered_map(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).set_before_command_interfaces, + params_.configuration_setup.configurations_map.at(state).set_before_command_values, + "reconfigure.set_before_command." + state, reconfigure_gpios_.set_before_commands[state], + command_if_ios, reconfigure_gpios_.set_before_commands_start_index[state]); + + reconfigure_gpios_.set_before_states[state] = + std::unordered_map(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).set_before_state_interfaces, + params_.configuration_setup.configurations_map.at(state).set_before_state_values, + "reconfigure.set_before_state." + state, reconfigure_gpios_.set_before_states[state], state_if_ios, + reconfigure_gpios_.set_before_states_start_index[state]); + + reconfigure_gpios_.commands[state] = + std::unordered_map(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).command_interfaces, + params_.configuration_setup.configurations_map.at(state).command_values, + "reconfigure.command." + state, reconfigure_gpios_.commands[state], command_if_ios, + reconfigure_gpios_.commands_start_index[state]); + + reconfigure_gpios_.states[state] = + std::unordered_map(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).state_interfaces, + params_.configuration_setup.configurations_map.at(state).state_values, + "reconfigure.state." + state, reconfigure_gpios_.states[state], state_if_ios, + reconfigure_gpios_.states_start_index[state]); + + reconfigure_gpios_.states_joint_states[state] = + params_.configuration_setup.configurations_map.at(state).joint_states; + + reconfigure_gpios_.set_after_commands[state] = + std::unordered_map(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).set_after_command_interfaces, + params_.configuration_setup.configurations_map.at(state).set_after_command_values, + "reconfigure.set_after_command." + state, reconfigure_gpios_.set_after_commands[state], + command_if_ios, reconfigure_gpios_.set_after_commands_start_index[state]); + reconfigure_gpios_.set_after_states[state] = + std::unordered_map(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).set_after_state_interfaces, + params_.configuration_setup.configurations_map.at(state).set_after_state_values, + "reconfigure.set_after_state." + state, reconfigure_gpios_.set_after_states[state], state_if_ios, + reconfigure_gpios_.set_after_states_start_index[state]); + } + + for (const auto & [name, data] : params_.sensors_interfaces.tool_specific_sensors_map) + { + state_if_ios.insert(data.interface); + } +} + +GpioToolController::EngagingSrvType::Response GpioToolController::process_engaging_request( + const ToolAction & requested_action, const std::string & requested_action_name) +{ + EngagingSrvType::Response response; + if (current_tool_action_.load() == ToolAction::RECONFIGURING) + { + response.success = false; + response.message = "Cannot engage the Tool while reconfiguring"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", response.message.c_str()); + return response; + } + + if (current_tool_action_.load() != ToolAction::IDLE) + { + const auto & current_action_name = + (current_tool_action_.load() == ToolAction::ENGAGING) ? params_.engaged.name : params_.disengaged.name; + + if (current_tool_action_.load() != requested_action) + { + RCLCPP_WARN(get_node()->get_logger(), "Stopping tool '%s' and starting '%s'.", + current_action_name.c_str(), requested_action_name.c_str()); + } + else + { + response.success = false; + response.message = "Tool is already executing action '" + requested_action_name + "'. Nothing to do."; + RCLCPP_INFO(get_node()->get_logger(), "%s", response.message.c_str()); + return response; + } + } + current_tool_action_.store(requested_action); + current_tool_transition_.store(GPIOToolTransition::SET_BEFORE_COMMAND); + + response.success = true; + response.message = "Tool action '" + requested_action_name + "' started."; + return response; +} + +GpioToolController::EngagingSrvType::Response GpioToolController::process_reconfigure_request( + const std::string & config_name) +{ + EngagingSrvType::Response response; + response.success = true; + if (config_name.empty()) + { + response.success = false; + response.message = "Configuration name cannot be empty"; + } + if (response.success && std::find(params_.configurations.begin(), params_.configurations.end(), config_name) == params_.configurations.end()) + { + response.success = false; + response.message = "Configuration '" + config_name + "' does not exist"; + } + if (response.success && current_tool_action_.load() != ToolAction::IDLE) + { + response.success = false; + response.message = "Tool is currently reconfiguring or executing '" + + params_.engaged.name + "' or '" + params_.disengaged.name + + "' action.Please wait until it finishes."; + } + if (response.success) + { + current_tool_action_.store(ToolAction::RECONFIGURING); + current_tool_transition_.store(GPIOToolTransition::SET_BEFORE_COMMAND); + target_configuration_.store(std::make_shared(config_name)); + response.message = "Tool reconfigfuration to '" + config_name + "' has started."; + RCLCPP_INFO(get_node()->get_logger(), "%s", response.message.c_str()); + } + else + { + RCLCPP_ERROR(get_node()->get_logger(), "%s", response.message.c_str()); + } + + return response; +} + +GpioToolController::EngagingSrvType::Response GpioToolController::service_wait_for_transition_end( + const std::string & requested_action_name) +{ + EngagingSrvType::Response response; + while (current_tool_action_.load() != ToolAction::IDLE) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (current_tool_transition_.load() == GPIOToolTransition::HALTED) + { + response.success = false; + response.message = "Tool action or reconfiguration '" + requested_action_name + "' was halted. Reset it with '~/reset_halted' service."; + return response; + } + } + response.success = true; + response.message = "Tool action or reconfiguration '" + requested_action_name + "' completed successfully."; + return response; +} + +controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_services() +{ + if (!params_.use_action) + { + // callback groups for each service + disengaging_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + engaging_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + reconfigure_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto disengaged_service_callback = [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + auto result = process_engaging_request(ToolAction::DISENGAGING, params_.disengaged.name); + if (result.success) + { + result = service_wait_for_transition_end(params_.disengaged.name); + } + response->success = result.success; + response->message = result.message; + }; + + disengaged_service_ = get_node()->create_service( + "~/" + params_.disengaged.name, disengaged_service_callback, qos_services, disengaging_service_callback_group_); + + auto engaged_service_callback = [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + auto result = process_engaging_request(ToolAction::ENGAGING, params_.engaged.name); + if (result.success) + { + result = service_wait_for_transition_end(params_.engaged.name); + } + response->success = result.success; + response->message = result.message; + }; + + engaged_service_ = get_node()->create_service( + "~/" + params_.engaged.name, engaged_service_callback, qos_services, engaging_service_callback_group_); + + // configure tool service + auto reconfigure_tool_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + auto result = process_reconfigure_request(request->config_name); + + if (result.success) + { + result = service_wait_for_transition_end(request->config_name); + + } + response->success = result.success; + response->message = result.message; + }; + reconfigure_tool_service_ = get_node()->create_service( + "~/reconfigure", reconfigure_tool_service_callback, qos_services, + reconfigure_service_callback_group_); + } + else + { + // disengaged close action server + engaging_action_server_ = rclcpp_action::create_server( + get_node(), "~/" + params_.engaged.name + "_" + params_.disengaged.name, + std::bind( + &GpioToolController::handle_engaging_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&GpioToolController::handle_engaging_cancel, this, std::placeholders::_1), + std::bind(&GpioToolController::handle_action_accepted, this, std::placeholders::_1)); + + // reconfigure action server + config_action_server_ = rclcpp_action::create_server( + get_node(), "~/reconfigure", + std::bind( + &GpioToolController::handle_config_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&GpioToolController::handle_config_cancel, this, std::placeholders::_1), + std::bind(&GpioToolController::handle_action_accepted, this, std::placeholders::_1)); + } + + reset_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto reset_callback = [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + reset_halted_.store(true); + response->success = true; + }; + reset_service_ = get_node()->create_service( + "~/reset_halted", reset_callback, qos_services, reset_service_callback_group_); + + try + { + t_js_publisher_ = + get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); + tool_joint_state_publisher_ = std::make_unique(t_js_publisher_); + + if_publisher_ = get_node()->create_publisher( + "~/dynamic_interfaces", rclcpp::SystemDefaultsQoS()); + interface_publisher_ = std::make_unique(if_publisher_); + + t_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = std::make_unique(t_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + const size_t nr_joints = params_.engaged_joints.size() + params_.configuration_joints.size(); + tool_joint_state_publisher_->msg_.name.reserve(nr_joints); + tool_joint_state_publisher_->msg_.position.resize(nr_joints, std::numeric_limits::quiet_NaN()); + + for (const auto & joint_name : params_.engaged_joints) + { + tool_joint_state_publisher_->msg_.name.push_back(joint_name); + } + for (const auto & joint_name : params_.configuration_joints) + { + tool_joint_state_publisher_->msg_.name.push_back(joint_name); + } + + interface_publisher_->msg_.states.interface_names.resize(state_interfaces_.size()); + interface_publisher_->msg_.states.values.resize(state_interfaces_.size()); + interface_publisher_->msg_.commands.interface_names.resize(command_interfaces_.size()); + interface_publisher_->msg_.commands.values.resize(command_interfaces_.size()); + for (size_t i = 0; i < state_interfaces_.size(); ++i) + { + interface_publisher_->msg_.states.interface_names[i] = state_interfaces_[i].get_name(); + } + + controller_state_publisher_->msg_.state = current_state_; + controller_state_publisher_->msg_.configuration = current_configuration_; + controller_state_publisher_->msg_.current_transition.state = current_tool_transition_.load(); + + return controller_interface::CallbackReturn::SUCCESS; +} + +void GpioToolController::publish_topics(const rclcpp::Time & time) +{ + if (tool_joint_state_publisher_ && tool_joint_state_publisher_->trylock()) + { + tool_joint_state_publisher_->msg_.header.stamp = time; + tool_joint_state_publisher_->msg_.position = joint_states_values_; + } + tool_joint_state_publisher_->unlockAndPublish(); + + if (interface_publisher_ && interface_publisher_->trylock()) + { + interface_publisher_->msg_.header.stamp = time; + for (size_t i = 0; i < state_interfaces_.size(); ++i) + { + interface_publisher_->msg_.states.values.at(i) = + static_cast(state_interfaces_.at(i).get_optional().value_or( + std::numeric_limits::quiet_NaN())); + } + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + interface_publisher_->msg_.commands.values.at(i) = + static_cast(command_interfaces_.at(i).get_optional().value_or( + std::numeric_limits::quiet_NaN())); + } + interface_publisher_->unlockAndPublish(); + } + + if (controller_state_publisher_ && controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.state = current_state_; + controller_state_publisher_->msg_.configuration = current_configuration_; + controller_state_publisher_->msg_.current_transition.state = current_tool_transition_.load(); + controller_state_publisher_->unlockAndPublish(); + } +} + +rclcpp_action::GoalResponse GpioToolController::handle_engaging_goal( + const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr goal) +{ + auto engaging_action = ToolAction::DISENGAGING; + std::string action_name = params_.disengaged.name; + if (goal->engage) + { + engaging_action = ToolAction::ENGAGING; + action_name = params_.engaged.name; + } + + auto result = process_engaging_request(engaging_action, action_name); + + if (!result.success) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to process '%s_%s' request: %s", params_.engaged.name.c_str(), params_.disengaged.name.c_str(), result.message.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GpioToolController::handle_engaging_cancel( + std::shared_ptr> /*goal_handle*/) +{ + RCLCPP_WARN( + get_node()->get_logger(), + "Tool action is being canceled, going to HALTED state. If you want to reset the Tool, use '~/reset_halted' service."); + current_tool_action_.store(ToolAction::CANCELING); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void GpioToolController::handle_engaging_accepted( + std::shared_ptr> goal_handle) +{ + handle_action_accepted(goal_handle); +} + +rclcpp_action::GoalResponse GpioToolController::handle_config_goal( + const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr goal) +{ + auto result = process_reconfigure_request(goal->config_name); + if (!result.success) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to process reconfigure request: %s", result.message.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GpioToolController::handle_config_cancel( + std::shared_ptr> /*goal_handle*/) +{ + RCLCPP_WARN( + get_node()->get_logger(), + "Tool action is being canceled, going to HALTED state. If you want to reset the Tool, use '~/reset_halted' service."); + current_tool_action_.store(ToolAction::CANCELING); + return rclcpp_action::CancelResponse::ACCEPT; +} + +template +void GpioToolController::handle_action_accepted( + std::shared_ptr> goal_handle) +{ + auto result = std::make_shared(); + auto feedback = std::make_shared(); + + while (true) + { + if (current_tool_action_.load() == ToolAction::IDLE) + { + result->success = true; + result->message = "Tool action successfully executed!"; + goal_handle->succeed(result); + break; + } + else if (current_tool_transition_.load() == GPIOToolTransition::HALTED) + { + result->success = false; + result->message = "Tool action canceled or halted! Check the error, reset the tool using '~/reset_halted' service and set to sensible state."; + goal_handle->abort(result); + break; + } + else + { + feedback->transition.state = current_tool_transition_.load(); + goal_handle->publish_feedback(feedback); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } +} + +void GpioToolController::check_tool_state(const rclcpp::Time & current_time) +{ + current_state_ = ""; + check_tool_state_and_switch(current_time, disengaged_gpios_, joint_states_values_, 0, params_.disengaged.name + " - CHECK STATES", GPIOToolTransition::IDLE, current_state_); + if (current_state_.empty()) + { + check_tool_state_and_switch(current_time, engaged_gpios_, joint_states_values_, 0, params_.engaged.name + " - CHECK STATES", GPIOToolTransition::IDLE, current_state_); + } + if (current_state_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tool state can not be determined, triggering CANCELING action sand HALTED transition."); + current_tool_action_.store(ToolAction::CANCELING); + } + + current_configuration_ = ""; + check_tool_state_and_switch(current_time, reconfigure_gpios_, joint_states_values_, params_.engaged_joints.size(), "reconfigure - CHECK STATES", GPIOToolTransition::IDLE, current_configuration_); + if (current_configuration_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tool configuration can not be determined, triggering CANCELING action and HALTED transition."); + current_tool_action_.store(ToolAction::CANCELING); + } +} + +} // namespace gpio_tool_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gpio_tool_controller::GpioToolController, controller_interface::ControllerInterface) diff --git a/gpio_controllers/src/gpio_tool_controller.yaml b/gpio_controllers/src/gpio_tool_controller.yaml new file mode 100644 index 0000000000..544df2a6b0 --- /dev/null +++ b/gpio_controllers/src/gpio_tool_controller.yaml @@ -0,0 +1,363 @@ +# Copyright (c) 2025, b»robotized by Stogl Robotics +# +# 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. + +gpio_tool_controller: + use_action: { + type: bool, + default_value: false, + description: "If set controllers provides actions instead of services for setting it to disengaged/engaged state and configurations." + } + timeout: { + type: double, + default_value: 5.0, + description: "Timeout for the waiting on signals from the tool about reached state.", + validation: { + gt<>: [0.0], + } + } + tolerance: { + type: double, + default_value: 0.01, + description: "Tolerance for the state values to be considered reached.", + validation: { + gt<>: [0.0], + } + } + engaged_joints: { + type: string_array, + default_value: [], + description: "List of joint names that should change values according to enaged state of the tool.", + validation: { + unique<>: null, + } + } + disengaged: + name: { + type: string, + default_value: "disengaged", + description: "Name of the disengaged state if you want to use different on in the application, e.g., 'open'.", + } + joint_states: { + type: double_array, + default_value: [], + description: "List of values that are published for the joint to `joint_states` topic when tool is disengaged. The order of the values must match the order of the joint names in `engaged_joints` parameter.", + } + set_before_command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value before disengageding the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set before disengageding the tool", + } + set_before_state: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that confirm the reached state 'before' disengaging the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for state interfaces that confirm the reached state 'before' disengaging the tool", + } + command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value to disengage the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set to disengage the tool", + } + state: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm the tool is disengaged", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm the tool is disengaged", + } + set_after_command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set after the tool is disengaged.", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set set after the tool is disengaged.", + } + set_after_state: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm 'after' the tool is disengaged.", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm 'after' the tool is disengaged.", + } + + possible_engaged_states: { + type: string_array, + description: "List of possible engaged states, e.g., a gripper can be `close_empty` or `close_full`.", + validation: { + unique<>: null, + not_empty<>: null, + } + } + + engaged: + name: { + type: string, + default_value: "engaged", + description: "Name of the engaged state if you want to use different on in the application, e.g., 'close'.", + } + set_before_command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value before engageing the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set before engageing the tool", + } + set_before_state: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that confirm the reached state 'before' engageing the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for state interfaces that confirm the reached state 'before' engageing the tool", + } + command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value to engage the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set to engage the tool", + } + states: + __map_possible_engaged_states: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint values when tool is engaged. The order of the values must match the order of the joint names in engaged_joints parameter." + } + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm the tool is engaged in a specific state.", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm the tool is engaged in a specific state.", + } + set_after_command_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set after the tool is enaged in a specific state.", + validation: { + unique<>: null, + } + } + set_after_command_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set after the tool is enaged in a specific state.", + } + set_after_state_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm 'after' the tool is engaged in a specific state.", + validation: { + unique<>: null, + } + } + set_after_state_values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm 'after' the tool is engaged in a specific state.", + } + + configurations: { + type: string_array, + default_value: [], + description: "Configuration names that can be used to switch between different configurations of the tool", + validation: { + unique<>: null, + } + } + + configuration_joints: { + type: string_array, + default_value: [], + description: "List of joint names that are used to switch between different configurations of the tool", + validation: { + unique<>: null, + } + } + + configuration_setup: + __map_configurations: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint states values that tool has in certian configurations. The order of the values must match the order of the joint names in `engaged_joints` parameter.", + } + set_before_command_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value before switching to a specific configuration.", + validation: { + unique<>: null, + } + } + set_before_command_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set before switching to a specific configuration.", + } + set_before_state_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that confirm the reached state 'before' switching to a specific configuration.", + validation: { + unique<>: null, + } + } + set_before_state_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for state interfaces that confirm the reached state 'before' switching to a specific configuration.", + } + command_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value for switching to a specific configuration.", + validation: { + unique<>: null, + } + } + command_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set for swicthing to aa specific configuration.", + } + state_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm the tool is in a specific configuration.", + validation: { + unique<>: null, + } + } + state_values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm the tool is in a specific configuration.", + } + set_after_command_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set after switching to a specific configuration.", + validation: { + unique<>: null, + } + } + set_after_command_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set after switching to a specific configuration.", + } + set_after_state_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm 'after' switching to a specific configuration.", + validation: { + unique<>: null, + } + } + set_after_state_values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm 'after' switching to a specific configuration.", + } + + tool_specific_sensors: { + type: string_array, + default_value: [], + description: "List of sensor names that are specific to the tool", + validation: { + unique<>: null, + } + } + sensors_interfaces: + __map_tool_specific_sensors: + interface: { + type: string, + default_value: "", + description: "Name of the state interface that is specific to a tool sensor.", + } diff --git a/io_gripper_controller/test/test_io_gripper_controller.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.cpp similarity index 100% rename from io_gripper_controller/test/test_io_gripper_controller.cpp rename to gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.cpp diff --git a/io_gripper_controller/test/test_io_gripper_controller.hpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.hpp similarity index 100% rename from io_gripper_controller/test/test_io_gripper_controller.hpp rename to gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.hpp diff --git a/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_all_param_set.cpp similarity index 100% rename from io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp rename to gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_all_param_set.cpp diff --git a/io_gripper_controller/test/test_io_gripper_controller_close.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_close.cpp similarity index 100% rename from io_gripper_controller/test/test_io_gripper_controller_close.cpp rename to gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_close.cpp diff --git a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_gripper_example.yaml b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_gripper_example.yaml new file mode 100644 index 0000000000..3e773395bd --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_gripper_example.yaml @@ -0,0 +1,82 @@ +test_io_gripper_controller: + ros__parameters: + use_action: true + timeout: 5.0 + tollerance: 0.00001 # binary data on the interfaces therefore small tollerance + engaged_joints: [gripper_clamp_jaw] + + disengaged: + name: 'open' + joint_states: [0.0] + set_before_command: + interfaces: [Release_Break_valve, Release_Something] + values: [1.0, 0.0] + set_before_state: + interfaces: [Break_Engaged] + values: [0.0] + command: + interfaces: [Open_valve, Close_valve] + values: [1.0, 0.0] + state: + interfaces: [Opened_signal, Closed_signal] + values: [1.0, 0.0] + set_after_command: + interfaces: [Release_Break_valve, Release_Something] + values: [0.0, 1.0] + set_after_state: + interfaces: [Break_Engaged] + values: [1.0] + + possible_engaged_states: ['close_empty', 'close_full'] + + engaged: + set_before_command: + interfaces: [Release_Break_valve, Release_Something] + low: [1.0, 0.0] + set_before_state: + interfaces: [Break_Engaged] + values: [0.0] + command: + interfaces: [Close_valve, Open_valve] + values: [1.0, 0.0] + states: + close_empty: + joint_states: [0.16] + interfaces: [Closed_signal, Part_Grasped_signal] + values: [1.0, 0.0] + set_after_command_interfaces: [Release_Something, Release_Break_valve] + set_after_command_values: [1.0, 0.0] + set_after_state_interfaces: [Break_Engaged] + set_after_state_values: [1.0] + close_full: + joint_states: [0.08] + interfaces: [Closed_signal, Part_Grasped_signal] + values: [0.0, 1.0] + set_after_command_interfaces: [Release_Something, Release_Break_valve] + set_after_command_values: [1.0, 0.0] + set_after_state_interfaces: [Break_Engaged] + set_after_state_values: [1.0] + + configurations: ["narrow_objects", "wide_objects"] + configuration_joints: [gripper_distance_joint] + + configuration_setup: + narrow_objects: + joint_states: [0.1] + command_interfaces: [Narrow_Configuration_Cmd, Wide_Configuration_Cmd] + command_values: [1.0, 0.0] + state_interfaces: [Narrow_Configuraiton_Signal, Wide_Configuration_Signal] + state_values: [1.0, 0.0] + wide_objects: + joint_states: [0.2] + command_interfaces: [Narrow_Configuration_Cmd, Wide_Configuration_Cmd] + command_values: [0.0, 1.0] + state_interfaces: [Narrow_Configuraiton_Signal, Wide_Configuration_Signal] + state_values: [0.0, 1.0] + + tool_specific_sensors: ["part_sensor_top", "part_sensor"] + sensors_interfaces: + part_sensor_top: + interface: "Part_Sensor_Top_signal" + part_sensor: + interface: "Part_Grasped_signal" diff --git a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml new file mode 100644 index 0000000000..141445e310 --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml @@ -0,0 +1,37 @@ +test_io_gripper_controller: + ros__parameters: + use_action: true + timeout: 5.0 + tolerance: 0.05 + engaged_joints: [gripper_clamp_jaw] + + disengaged: + name: 'low' + joint_states: [0.0] + command: + interfaces: [SetLowCommand] + values: [1.0] + state: + interfaces: [LiftState] + values: [0.0] + + possible_engaged_states: ['high_empty', 'high_payload'] + + engaged: + command: + interfaces: [SetHighCommand] + values: [1.0] + states: + high_empty: + joint_states: [1.5] + interfaces: [LiftState, PaylodDetected] + values: [1.5, 0.0] + high_payload: + joint_states: [1.48] + interfaces: [LiftState, PaylodDetected] + values: [1.5, 1.0] + + gripper_specific_sensors: ["payload_detected"] + sensors_interfaces: + payload_detected: + interface: "PaylodDetected" diff --git a/io_gripper_controller/test/test_io_gripper_controller_open.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open.cpp similarity index 100% rename from io_gripper_controller/test/test_io_gripper_controller_open.cpp rename to gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open.cpp diff --git a/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open_close_action.cpp similarity index 100% rename from io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp rename to gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open_close_action.cpp diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure.cpp similarity index 100% rename from io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp rename to gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure.cpp diff --git a/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure_action.cpp similarity index 100% rename from io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp rename to gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure_action.cpp diff --git a/io_gripper_controller/test/test_load_io_gripper_controller.cpp b/gpio_controllers/test/gpio_tool_controller/test_load_gpio_tool_controller.cpp similarity index 100% rename from io_gripper_controller/test/test_load_io_gripper_controller.cpp rename to gpio_controllers/test/gpio_tool_controller/test_load_gpio_tool_controller.cpp diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt index 32a584aaba..8d46fa57d8 100644 --- a/io_gripper_controller/CMakeLists.txt +++ b/io_gripper_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(io_gripper_controller) +project(gpio_tool_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) @@ -32,27 +32,27 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() -generate_parameter_library(io_gripper_controller_parameters - src/io_gripper_controller.yaml +generate_parameter_library(gpio_tool_controller_parameters + src/gpio_tool_controller.yaml ) add_library( - io_gripper_controller + gpio_tool_controllers SHARED - src/io_gripper_controller.cpp + src/gpio_tool_controller.cpp ) -target_include_directories(io_gripper_controller PUBLIC +target_include_directories(gpio_tool_controllers PUBLIC "$" "$") -target_link_libraries(io_gripper_controller io_gripper_controller_parameters) -ament_target_dependencies(io_gripper_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_compile_definitions(io_gripper_controller PRIVATE "io_gripper_controller_BUILDING_DLL") +target_link_libraries(gpio_tool_controllers gpio_tool_controller_parameters) +ament_target_dependencies(gpio_tool_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_definitions(gpio_tool_controllers PRIVATE "gpio_tool_controller_BUILDING_DLL") pluginlib_export_plugin_description_file( - controller_interface io_gripper_controller.xml) + controller_interface gpio_tool_controller.xml) install( TARGETS - io_gripper_controller + gpio_tool_controller RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -66,31 +66,31 @@ install( if(BUILD_TESTING) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") - ament_add_gmock(test_load_io_gripper_controller test/test_load_io_gripper_controller.cpp) - target_include_directories(test_load_io_gripper_controller PRIVATE include) + ament_add_gmock(test_load_gpio_tool_controller test/test_load_gpio_tool_controller.cpp) + target_include_directories(test_load_gpio_tool_controller PRIVATE include) ament_target_dependencies( - test_load_io_gripper_controller + test_load_gpio_tool_controller controller_manager hardware_interface ros2_control_test_assets ) - ament_add_gmock(test_io_gripper_controller - test/test_io_gripper_controller.cpp - test/test_io_gripper_controller_open.cpp - test/test_io_gripper_controller_close.cpp - test/test_io_gripper_controller_all_param_set.cpp - test/test_io_gripper_controller_open_close_action.cpp - test/test_io_gripper_controller_reconfigure.cpp - test/test_io_gripper_controller_reconfigure_action.cpp - ) - target_include_directories(test_io_gripper_controller PRIVATE include) - target_link_libraries(test_io_gripper_controller io_gripper_controller) - ament_target_dependencies( - test_io_gripper_controller - controller_interface - hardware_interface - ) + # ament_add_gmock(test_gpio_tool_controller + # test/test_gpio_tool_controller.cpp + # test/test_gpio_tool_controller_open.cpp + # test/test_gpio_tool_controller_close.cpp + # test/test_gpio_tool_controller_all_param_set.cpp + # test/test_gpio_tool_controller_open_close_action.cpp + # test/test_gpio_tool_controller_reconfigure.cpp + # test/test_gpio_tool_controller_reconfigure_action.cpp + # ) + # target_include_directories(test_gpio_tool_controller PRIVATE include) + # target_link_libraries(test_gpio_tool_controller gpio_tool_controller) + # ament_target_dependencies( + # test_gpio_tool_controller + # controller_interface + # hardware_interface + # ) endif() @@ -101,7 +101,7 @@ ament_export_dependencies( ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_export_libraries( - io_gripper_controller + gpio_tool_controller ) ament_package() diff --git a/io_gripper_controller/COLCON_IGNORE b/io_gripper_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/io_gripper_controller/doc/userdoc.rst b/io_gripper_controller/doc/userdoc.rst index 585d2c3754..5efac3de86 100644 --- a/io_gripper_controller/doc/userdoc.rst +++ b/io_gripper_controller/doc/userdoc.rst @@ -16,6 +16,10 @@ Description of controller's interfaces - ``dynamic_interfaces`` [``control_msgs::msg::DynamicInterfaceValues``]: Publishes all command and state interface of the IOs and sensors of gripper. +When preemting acitons make sure that those are defined fully in the controller's parameter. +This means that you shoul make sure that the actuators for *engage* are disabled when *disengage* is called and vice versa. +Otherwise it can happen that two actuators acting "against each other" are turned on at the same time which can lead to unexpected behavior and even damage of the tool. + Parameters ,,,,,,,,,,, diff --git a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp deleted file mode 100644 index 9bd314a984..0000000000 --- a/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp +++ /dev/null @@ -1,409 +0,0 @@ -// Copyright (c) 2025, b»robotized by Stogl Robotics -// -// 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 IO_GRIPPER_CONTROLLER__IO_GRIPPER_CONTROLLER_HPP_ -#define IO_GRIPPER_CONTROLLER__IO_GRIPPER_CONTROLLER_HPP_ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include "control_msgs/action/io_gripper_command.hpp" -#include "control_msgs/action/set_io_gripper_config.hpp" -#include "control_msgs/msg/dynamic_interface_values.hpp" -#include "control_msgs/msg/interface_value.hpp" -#include "control_msgs/msg/io_gripper_controller_state.hpp" -#include "control_msgs/msg/io_gripper_state.hpp" -#include "control_msgs/srv/set_io_gripper_config.hpp" -#include "controller_interface/controller_interface.hpp" -#include "io_gripper_controller_parameters.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" -#include "realtime_tools/realtime_publisher.hpp" -#include "sensor_msgs/msg/joint_state.hpp" -#include "std_srvs/srv/set_bool.hpp" -#include "std_srvs/srv/trigger.hpp" - -namespace io_gripper_controller -{ -/** - * @enum service_mode_type - * @brief Represents the various service modes of the gripper. These modes represents the high level - * states of the gripper. - * - * - IDLE: The gripper is in an idle state, not performing any action. - * - OPEN: The gripper is in the process of opening. - * - CLOSE: The gripper is in the process of closing. - */ -enum class service_mode_type : std::uint8_t -{ - IDLE = 0, - OPEN = 1, - CLOSE = 2, -}; - -/** - * @enum reconfigure_state_type - * @brief Represents the various states of the reconfiguration process, which means that the gripper - * is reconfiguring to new state based on the configuration defined in the yaml params. - * - * - IDLE: The reconfiguration process is idle, not performing any action. - * - SET_COMMAND: Setting the command based on the configuration. - * - CHECK_STATE: Checking the state after setting the command. - */ -enum class reconfigure_state_type : std::uint8_t -{ - IDLE = 0, - SET_COMMAND = 1, - CHECK_STATE = 2, -}; - -class IOGripperController : public controller_interface::ControllerInterface -/** - * @brief IOGripperController class handles the control of an IO-based gripper. - */ -{ -public: - /** - * @brief Constructor for IOGripperController. - */ - IOGripperController(); - io_gripper_controller::Params params_; - - /** - * @brief Initializes the controller. - * @return CallbackReturn indicating success or failure. - */ - controller_interface::CallbackReturn on_init() override; - - /** - * @brief Configures the command interfaces. - * @return InterfaceConfiguration for command interfaces. - */ - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - - /** - * @brief Configures the state interfaces. - * @return InterfaceConfiguration for state interfaces. - */ - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - /** - * @brief Configures the controller. - * @param previous_state The previous state of the lifecycle. - * @return CallbackReturn indicating success or failure. - */ - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; - - /** - * @brief Activates the controller. - * @param previous_state The previous state of the lifecycle. - * @return CallbackReturn indicating success or failure. - */ - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - /** - * @brief Deactivates the controller. - * @param previous_state The previous state of the lifecycle. - * @return CallbackReturn indicating success or failure. - */ - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - /** - * @brief Updates the controller state. - * @param time The current time. - * @param period The time since the last update. - * @return return_type indicating success or failure. - */ - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - struct GripperTransitionIOs - { - std::unordered_map command_ios; - std::unordered_map state_ios; - - bool has_multiple_end_states = false; - std::vector possible_states; - std::unordered_map> multiple_states_ios; - - std::unordered_map set_before_command_ios; - std::unordered_map set_before_state_ios; - - std::unordered_map set_after_command_ios; - std::unordered_map set_after_state_ios; - }; - - GripperTransitionIOs open_ios_; - GripperTransitionIOs close_ios_; - - rclcpp::Time last_transition_time_; - std::vector after_joint_states_; - std::vector command_ios_open; - std::vector command_ios_close; - std::vector set_before_command_open; - std::vector set_after_command_open; - std::vector reconfigure_command; - std::vector command_ios_open_values; - std::vector command_ios_close_values; - std::vector set_before_command_open_values; - std::vector set_after_command_open_values; - std::vector reconfigure_command_values; - std::vector state_ios_open; - std::vector state_ios_close; - std::vector set_before_command_close; - std::vector set_after_command_close; - std::vector state_ios_open_values; - std::vector state_ios_close_values; - std::vector set_before_command_close_values; - std::vector set_after_command_close_values; - std::vector set_after_command_open_values_original_; - std::string status_joint_name; - bool is_open; - std::unordered_map command_if_ios_after_opening; - std::unordered_map original_ios_after_opening; - std::unordered_map command_if_ios_before_closing; - std::unordered_map original_ios_before_closing; - std::unordered_set command_if_ios; - std::unordered_set state_if_ios; - bool setResult; - - using ControllerModeSrvType = std_srvs::srv::SetBool; - using OpenCloseSrvType = std_srvs::srv::Trigger; - using ConfigSrvType = control_msgs::srv::SetIOGripperConfig; - using JointStateMsg = sensor_msgs::msg::JointState; - using DynInterfaceMsg = control_msgs::msg::DynamicInterfaceValues; - using GripperAction = control_msgs::action::IOGripperCommand; - using GoalHandleGripper = rclcpp_action::ServerGoalHandle; - using GripperConfigAction = control_msgs::action::SetIOGripperConfig; - using GoalHandleGripperConfig = rclcpp_action::ServerGoalHandle; - using IOGripperState = control_msgs::msg::IOGripperState; - -protected: - std::shared_ptr param_listener_; - rclcpp::Service::SharedPtr open_service_; - rclcpp::Service::SharedPtr close_service_; - rclcpp::Service::SharedPtr configure_gripper_service_; - rclcpp_action::Server::SharedPtr gripper_action_server_; - rclcpp_action::Server::SharedPtr gripper_config_action_server_; - - // Realtime buffer to store the state for outer gripper_service (e.g. idle, open, close) - realtime_tools::RealtimeBuffer gripper_service_buffer_; - // Realtime buffer to store the state for switching the gripper state (e.g. idle, - // set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, - // halted) - realtime_tools::RealtimeBuffer gripper_state_buffer_; - realtime_tools::RealtimeBuffer gripper_open_state_buffer_; - // Realtime buffer to store the name of the configuration which needs to be set - realtime_tools::RealtimeBuffer configure_gripper_buffer_; - // Realtime buffer to store the state for switching the reconfigure state (e.g. idle, - // set_command, check_state) - realtime_tools::RealtimeBuffer reconfigure_state_buffer_; - - using ControllerStatePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr g_j_s_publisher_; - std::unique_ptr gripper_joint_state_publisher_; - std::vector joint_state_values_; - using InterfacePublisher = realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr if_publisher_; - std::unique_ptr interface_publisher_; - using GripperStatePublisher = - realtime_tools::RealtimePublisher; - rclcpp::Publisher::SharedPtr g_s_publisher_; - std::unique_ptr gripper_state_publisher_; - std::atomic reconfigureFlag_{false}; - std::atomic openFlag_{false}; - std::atomic closeFlag_{false}; - - std::atomic check_gripper_state_ios_{false}; - std::atomic transition_time_updated_{false}; - -private: - /** - * @brief Finds and sets a command value. - * @param name The name of the command. - * @param value The value to set. - * @return True if the command was found and set, false otherwise. - */ - bool find_and_set_command(const std::string & name, const double value); - - /** - * @brief Finds and gets a state value. - * @param name The name of the state. - * @param value The value to get. - * @return True if the state was found and retrieved, false otherwise. - */ - bool find_and_get_state(const std::string & name, double & value); - - /** - * @brief Finds and gets a command value. - * @param name The name of the command. - * @param value The value to get. - * @return True if the command was found and retrieved, false otherwise. - */ - bool find_and_get_command(const std::string & name, double & value); - - /** - * @brief Handles the state transition when opening the gripper. - * @param state The current state of the gripper. - */ - void handle_gripper_state_transition( - const rclcpp::Time & current_time, const GripperTransitionIOs & ios, const uint & state, - const std::string & transition_name); - - /** - * @brief Handles the state transition when closing the gripper. - * @param state The current state of the gripper. - */ - // void handle_gripper_state_transition_close(const gripper_state_type & state); - - /** - * @brief Handles the state transition for reconfiguration. - * @param state The current reconfiguration state. - */ - void handle_reconfigure_state_transition(const reconfigure_state_type & state); - - /** - * @brief Checks the parameters of the controller. - * @return CallbackReturn indicating success or failure. - */ - controller_interface::CallbackReturn check_parameters(); - - /** - * @brief Prepares the command and state IOs. - */ - void prepare_command_and_state_ios(); - - /** - * @brief Prepares the publishers and services. - * @return CallbackReturn indicating success or failure. - */ - controller_interface::CallbackReturn prepare_publishers_and_services(); - - /** - * @brief Publishes the gripper joint states. - */ - void publish_gripper_joint_states(); - - /** - * @brief Publishes the dynamic interface values. - */ - void publish_dynamic_interface_values(); - - /** - * @brief Publishes the reconfigure gripper joint states. - */ - void publish_reconfigure_gripper_joint_states(); - - /** - * @brief Checks the gripper and reconfigure state. - */ - void check_gripper_and_reconfigure_state(); - - bool set_commands( - const std::unordered_map & command_states, - const std::string & transition_name); - bool check_states( - const std::unordered_map & state_ios, const std::string & transition_name); - - std::vector configurations_list_; - std::vector config_map_; - std::vector - sensors_map_; - double state_value_; - std::string configuration_key_; - bool check_state_ios_; - std::string closed_state_name_; - io_gripper_controller::Params::Close::State::MapPossibleClosedStates closed_state_values_; - io_gripper_controller::Params::ConfigurationSetup::MapConfigurations conf_it_; - std::vector::iterator config_index_; - rclcpp::CallbackGroup::SharedPtr open_service_callback_group_; - rclcpp::CallbackGroup::SharedPtr close_service_callback_group_; - rclcpp::CallbackGroup::SharedPtr reconfigure_service_callback_group_; - std::shared_ptr gripper_feedback_; - std::shared_ptr gripper_result_; - std::shared_ptr gripper_config_feedback_; - std::shared_ptr gripper_config_result_; - - /** - * @brief Handles the goal for the gripper action. - * @param uuid The UUID of the goal. - * @param goal The goal to handle. - * @return GoalResponse indicating acceptance or rejection of the goal. - */ - rclcpp_action::GoalResponse handle_goal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - - /** - * @brief Handles the cancellation of the gripper action. - * @param goal_handle The handle of the goal to cancel. - * @return CancelResponse indicating acceptance or rejection of the cancellation. - */ - rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); - - /** - * @brief Handles the acceptance of the gripper action. - * @param goal_handle The handle of the accepted goal. - */ - void handle_accepted(const std::shared_ptr goal_handle); - - /** - * @brief Executes the gripper action. - * @param goal_handle The handle of the goal to execute. - */ - void execute(const std::shared_ptr goal_handle); - - /** - * @brief Handles the goal for the gripper configuration action. - * @param uuid The UUID of the goal. - * @param goal The goal to handle. - * @return GoalResponse indicating acceptance or rejection of the goal. - */ - rclcpp_action::GoalResponse config_handle_goal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - - /** - * @brief Handles the cancellation of the gripper configuration action. - * @param goal_handle The handle of the goal to cancel. - * @return CancelResponse indicating acceptance or rejection of the cancellation. - */ - rclcpp_action::CancelResponse config_handle_cancel( - const std::shared_ptr goal_handle); - - /** - * @brief Handles the acceptance of the gripper configuration action. - * @param goal_handle The handle of the accepted goal. - */ - void config_handle_accepted(const std::shared_ptr goal_handle); - - /** - * @brief Executes the gripper configuration action. - * @param goal_handle The handle of the goal to execute. - */ - void config_execute(const std::shared_ptr goal_handle); -}; - -} // namespace io_gripper_controller - -#endif // IO_GRIPPER_CONTROLLER__IO_GRIPPER_CONTROLLER_HPP_ diff --git a/io_gripper_controller/src/io_gripper_controller.cpp b/io_gripper_controller/src/io_gripper_controller.cpp deleted file mode 100644 index 28cbb5ad6c..0000000000 --- a/io_gripper_controller/src/io_gripper_controller.cpp +++ /dev/null @@ -1,1343 +0,0 @@ -// Copyright (c) 2025, b»robotized by Stogl Robotics -// -// 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 "io_gripper_controller/io_gripper_controller.hpp" - -#include -#include -#include -#include - -#include "controller_interface/helpers.hpp" -#include "rclcpp/version.h" - -namespace -{ // utility - -// Changed services history QoS to keep all so we don't lose any client service calls -// \note The versions conditioning is added here to support the source-compatibility with Humble -#if RCLCPP_VERSION_MAJOR >= 17 -rclcpp::QoS qos_services = - rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) - .reliable() - .durability_volatile(); -#else -static const rmw_qos_profile_t qos_services = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; -#endif - -} // namespace - -namespace io_gripper_controller -{ -IOGripperController::IOGripperController() : controller_interface::ControllerInterface() {} - -controller_interface::CallbackReturn IOGripperController::on_init() -{ - gripper_service_buffer_.initRT(service_mode_type::IDLE); - configuration_key_ = ""; - configure_gripper_buffer_.initRT(configuration_key_); - gripper_state_buffer_.initRT(IOGripperState::IDLE); - reconfigure_state_buffer_.initRT(reconfigure_state_type::IDLE); - - try - { - param_listener_ = std::make_shared(get_node()); - } - catch (const std::exception & e) - { - fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn IOGripperController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - params_ = param_listener_->get_params(); - - auto result = check_parameters(); - if (result != controller_interface::CallbackReturn::SUCCESS) - { - return result; - } - - prepare_command_and_state_ios(); - - result = prepare_publishers_and_services(); - if (result != controller_interface::CallbackReturn::SUCCESS) - { - return result; - } - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration IOGripperController::command_interface_configuration() - const -{ - controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - command_interfaces_config.names.reserve(command_if_ios.size()); - for (const auto & command_io : command_if_ios) - { - command_interfaces_config.names.push_back(command_io); - } - - return command_interfaces_config; -} - -controller_interface::InterfaceConfiguration IOGripperController::state_interface_configuration() - const -{ - controller_interface::InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - state_interfaces_config.names.reserve(state_if_ios.size()); - for (const auto & state_io : state_if_ios) - { - state_interfaces_config.names.push_back(state_io); - } - - return state_interfaces_config; -} - -controller_interface::CallbackReturn IOGripperController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - check_gripper_and_reconfigure_state(); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn IOGripperController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - joint_state_values_.resize( - params_.open_close_joints.size() + params_.configuration_joints.size(), - std::numeric_limits::quiet_NaN()); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::return_type IOGripperController::update( - const rclcpp::Time & time, const rclcpp::Duration & /*period*/) -{ - if (reconfigureFlag_.load()) - { - configuration_key_ = *(configure_gripper_buffer_.readFromRT()); - handle_reconfigure_state_transition(*(reconfigure_state_buffer_.readFromRT())); - } - - switch (*(gripper_service_buffer_.readFromRT())) - { - case service_mode_type::IDLE: - // do nothing - break; - case service_mode_type::OPEN: - handle_gripper_state_transition( - time, open_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "open"); - break; - case service_mode_type::CLOSE: - RCLCPP_INFO(get_node()->get_logger(), "CLOSE state is triggered"); - handle_gripper_state_transition( - time, close_ios_, static_cast(*(gripper_state_buffer_.readFromRT())), "close"); - break; - - default: - break; - } - - publish_gripper_joint_states(); - publish_dynamic_interface_values(); - - return controller_interface::return_type::OK; -} - -bool IOGripperController::find_and_set_command(const std::string & name, const double value) -{ - auto it = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), - [&](const hardware_interface::LoanedCommandInterface & command_interface) - { return command_interface.get_name() == name; }); - - if (it != command_interfaces_.end()) - { - return it->set_value(value); - } - RCLCPP_ERROR(get_node()->get_logger(), "Failed to set the command state for %s", name.c_str()); - return false; -} - -bool IOGripperController::find_and_get_state(const std::string & name, double & value) -{ - auto it = std::find_if( - state_interfaces_.begin(), state_interfaces_.end(), - [&](const hardware_interface::LoanedStateInterface & state_interface) - { return state_interface.get_name() == name; }); - - if (it != state_interfaces_.end()) - { - value = it->get_value(); - return true; - } - value = 0.0f; - return false; -} - -bool IOGripperController::find_and_get_command(const std::string & name, double & value) -{ - auto it = std::find_if( - command_interfaces_.begin(), command_interfaces_.end(), - [&](const hardware_interface::LoanedCommandInterface & command_interface) - { return command_interface.get_name() == name; }); - - if (it != command_interfaces_.end()) - { - value = it->get_value(); - return true; - } - value = 0.0f; - return false; -} - -bool IOGripperController::set_commands( - const std::unordered_map & command_states, - const std::string & transition_name) -{ - bool all_successful = true; - for (const auto & [command_name, command_value] : command_states) - { - if (!find_and_set_command(command_name, command_value)) - { - RCLCPP_ERROR( - get_node()->get_logger(), "%s: Failed to set the command state for %s", - transition_name.c_str(), command_name.c_str()); - all_successful = false; - } - } - return all_successful; -} - -bool IOGripperController::check_states( - const std::unordered_map & state_ios, const std::string & transition_name) -{ - bool all_correct = true; - for (const auto & [state_name, expected_state_value] : state_ios) - { - double current_state_value; - if (!find_and_get_state(state_name, current_state_value)) - { - RCLCPP_ERROR( - get_node()->get_logger(), "%s: Failed to get the state for %s", transition_name.c_str(), - state_name.c_str()); - all_correct = false; - } - else - { - if (abs(current_state_value - expected_state_value) > std::numeric_limits::epsilon()) - { - RCLCPP_DEBUG( - get_node()->get_logger(), "%s: State value for %s doesn't match", transition_name.c_str(), - state_name.c_str()); - all_correct = false; - } - } - } - return all_correct; -} - -void IOGripperController::handle_gripper_state_transition( - const rclcpp::Time & current_time, const GripperTransitionIOs & ios, const uint & state, - const std::string & transition_name) -{ - switch (state) - { - case IOGripperState::IDLE: - // do nothing - break; - - case IOGripperState::HALTED: - gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); - RCLCPP_ERROR( - get_node()->get_logger(), "%s - HALTED: Gripper is in HALTED state", - transition_name.c_str()); - transition_time_updated_.store(false); - break; - - case IOGripperState::SET_BEFORE_COMMAND: - - if (!transition_time_updated_.load()) - { - last_transition_time_ = current_time; - transition_time_updated_.store(true); - } - if (set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND")) - { - // TODO(destogl): check to use other Realtime sync object to have write from RT - gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_BEFORE_COMMAND); - } - else if ((current_time - last_transition_time_).seconds() > params_.timeout) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); - gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); - } - break; - case IOGripperState::CHECK_BEFORE_COMMAND: - // check the state of the gripper - if (check_states(ios.set_before_state_ios, transition_name + " - CHECK_BEFORE_COMMAND")) - { - gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_COMMAND); - } - else if ((current_time - last_transition_time_).seconds() > params_.timeout) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); - gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); - } - break; - case IOGripperState::SET_COMMAND: - RCLCPP_INFO( - get_node()->get_logger(), "%s - SET_COMMAND: Setting the command states", - transition_name.c_str()); - // now execute the command on the gripper - if (set_commands(ios.command_ios, transition_name + " - SET_COMMAND")) - { - // TODO(destogl): check to use other Realtime sync object to have write from RT - gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_COMMAND); - } - else if ((current_time - last_transition_time_).seconds() > params_.timeout) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "%s - SET_COMMAND: Gripper didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); - gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); - } - break; - case IOGripperState::CHECK_COMMAND: - // check the state of the gripper - check_gripper_state_ios_.store(true); - if (ios.has_multiple_end_states) - { - for (const auto & possible_end_state : ios.possible_states) - { - if (check_states( - ios.multiple_states_ios.at(possible_end_state), - transition_name + " - CHECK_COMMAND")) - { - check_gripper_state_ios_.store(true); - after_joint_states_ = - params_.close.state.possible_closed_states_map.at(possible_end_state).joint_states; - // TODO(Sachin): store possible_end_state in a variable to publish on status topic - break; - } - else - { - check_gripper_state_ios_.store(false); - } - } - } - else // only single end state - { - after_joint_states_ = params_.open.joint_states; - check_gripper_state_ios_.store( - check_states(ios.state_ios, transition_name + " - CHECK_COMMAND")); - } - - if (check_gripper_state_ios_.load()) - { - gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_AFTER_COMMAND); - } - else if ((current_time - last_transition_time_).seconds() > params_.timeout) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "%s - CHECK_COMMAND: Gripper didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); - gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); - } - break; - case IOGripperState::SET_AFTER_COMMAND: - if (set_commands(ios.set_after_command_ios, transition_name + " - SET_AFTER_COMMAND")) - { - gripper_state_buffer_.writeFromNonRT(IOGripperState::CHECK_AFTER_COMMAND); - } - else if ((current_time - last_transition_time_).seconds() > params_.timeout) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "%s - SET_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); - gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); - } - break; - - case IOGripperState::CHECK_AFTER_COMMAND: - if (check_states(ios.set_after_state_ios, transition_name + " - CHECK_AFTER_COMMAND")) - { - // set joint states - for (size_t i = 0; i < after_joint_states_.size(); ++i) - { - joint_state_values_[i] = after_joint_states_[i]; - } - // Finish up the transition - gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); - openFlag_.store(false); - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); - RCLCPP_INFO( - get_node()->get_logger(), "%s - CHECK_AFTER_COMMAND: Gripper reached target state", - transition_name.c_str()); - transition_time_updated_.store(false); // resetting the flag - } - else if ((current_time - last_transition_time_).seconds() > params_.timeout) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "%s - CHECK_AFTER_COMMAND: Gripper didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); - gripper_state_buffer_.writeFromNonRT(IOGripperState::HALTED); - } - break; - - default: - break; - } -} - -void IOGripperController::handle_reconfigure_state_transition(const reconfigure_state_type & state) -{ - switch (state) - { - case reconfigure_state_type::IDLE: - // do nothing - break; - case reconfigure_state_type::SET_COMMAND: - config_index_ = - std::find(configurations_list_.begin(), configurations_list_.end(), configuration_key_); - if (config_index_ == configurations_list_.end()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Configuration not found"); - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); - break; - } - else - { - conf_it_ = config_map_[std::distance(configurations_list_.begin(), config_index_)]; - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); - } - setResult = false; - for (const auto & io : conf_it_.command_high) - { - setResult = find_and_set_command(io, 1.0); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", io.c_str()); - } - } - for (const auto & io : conf_it_.command_low) - { - setResult = find_and_set_command(io, 0.0); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to set the command state for %s", io.c_str()); - } - } - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::CHECK_STATE); - break; - - case reconfigure_state_type::CHECK_STATE: - check_state_ios_ = false; - for (const auto & io : conf_it_.state_high) - { - setResult = find_and_get_state(io, state_value_); - if (!setResult) - { - check_state_ios_ = false; - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); - } - else - { - if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) - { - check_state_ios_ = false; - RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); - break; - } - else - { - check_state_ios_ = true; - } - } - } - - for (const auto & io : conf_it_.state_low) - { - setResult = find_and_get_state(io, state_value_); - if (!setResult) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); - } - else - { - if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) - { - check_state_ios_ = false; - RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); - break; - } - else - { - check_state_ios_ = true; - } - } - } - - if (check_state_ios_) - { - for (size_t i = 0; i < conf_it_.joint_states.size(); ++i) - { - joint_state_values_[i + params_.open_close_joints.size()] = conf_it_.joint_states[i]; - } - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); - configure_gripper_buffer_.writeFromNonRT(""); - reconfigureFlag_.store(false); - } - break; - default: - break; - } -} - -controller_interface::CallbackReturn IOGripperController::check_parameters() -{ - if (params_.open.command.high.empty() && params_.open.command.low.empty()) - { - RCLCPP_FATAL( - get_node()->get_logger(), "Size of open command high and low parameters cannot be zero."); - return CallbackReturn::FAILURE; - } - - if (params_.open.state.high.empty() && params_.open.state.low.empty()) - { - RCLCPP_FATAL( - get_node()->get_logger(), "Size of open state high and low parameters cannot be zero."); - return CallbackReturn::FAILURE; - } - - if (params_.close.command.high.empty() && params_.close.command.low.empty()) - { - RCLCPP_FATAL( - get_node()->get_logger(), "Size of close command high and low parameters cannot be zero."); - return CallbackReturn::FAILURE; - } - - // configurations parameter - if (!params_.configurations.empty()) - { - if (!params_.configuration_joints.empty()) - { - // configuration setup parameter - if (params_.configuration_setup.configurations_map.empty()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of configuration map parameter cannot be zero if configurations are defined."); - return CallbackReturn::FAILURE; - } - } - else - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Configuration joints have to be defined if configurations are provided."); - return CallbackReturn::FAILURE; - } - } - - // gripper_specific_sensors parameter - if (!params_.gripper_specific_sensors.empty()) - { - // sensors interfaces parameter - if (params_.sensors_interfaces.gripper_specific_sensors_map.empty()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of sensors interfaces parameter cannot be zero if gripper specific sensors are " - "defined."); - return CallbackReturn::FAILURE; - } - else - { - // if sensor input string is empty then return failure - for (const auto & [key, val] : params_.sensors_interfaces.gripper_specific_sensors_map) - { - if (val.input == "") - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of sensor input string parameter cannot be empty (" - ")."); - return CallbackReturn::FAILURE; - } - } - } - } - return CallbackReturn::SUCCESS; -} - -void IOGripperController::prepare_command_and_state_ios() -{ - auto parse_interfaces_from_params = []( - const std::vector & parameter_values, - const double & value, - std::unordered_map & ios, - std::unordered_set & interface_list) - { - for (const auto & itf : parameter_values) - { - if (!itf.empty()) - { - ios[itf] = value; - interface_list.insert(itf); - } - } - }; - - auto parse_possible_states = [&](const std::string & name, const auto & value) - { - close_ios_.possible_states.push_back(name); - parse_interfaces_from_params( - value.high, 1.0, close_ios_.multiple_states_ios[name], state_if_ios); - parse_interfaces_from_params( - value.low, 0.0, close_ios_.multiple_states_ios[name], state_if_ios); - }; - - // make full command ios lists -- just once - parse_interfaces_from_params( - params_.open.set_before_command.high, 1.0, open_ios_.set_before_command_ios, command_if_ios); - parse_interfaces_from_params( - params_.open.set_before_command.low, 0.0, open_ios_.set_before_command_ios, command_if_ios); - - parse_interfaces_from_params( - params_.open.command.high, 1.0, open_ios_.command_ios, command_if_ios); - parse_interfaces_from_params( - params_.open.command.low, 0.0, open_ios_.command_ios, command_if_ios); - - parse_interfaces_from_params( - params_.open.set_after_command.high, 1.0, open_ios_.set_after_command_ios, command_if_ios); - parse_interfaces_from_params( - params_.open.set_after_command.low, 0.0, open_ios_.set_after_command_ios, command_if_ios); - - // make full command ios lists for close -- just once - parse_interfaces_from_params( - params_.close.set_before_command.high, 1.0, close_ios_.set_before_command_ios, command_if_ios); - parse_interfaces_from_params( - params_.close.set_before_command.low, 0.0, close_ios_.set_before_command_ios, command_if_ios); - - parse_interfaces_from_params( - params_.close.command.high, 1.0, close_ios_.command_ios, command_if_ios); - parse_interfaces_from_params( - params_.close.command.low, 0.0, close_ios_.command_ios, command_if_ios); - - if (params_.possible_closed_states.size() > 0) - { - close_ios_.has_multiple_end_states = true; - close_ios_.possible_states = params_.possible_closed_states; - for (const auto & [name, value] : params_.close.state.possible_closed_states_map) - { - parse_possible_states(name, value); - } - } - - // parse_interfaces_from_params( - // params_.close.set_after_command_high, 1.0, close_ios_.set_after_command_ios, command_if_ios); - // parse_interfaces_from_params( - // params_.close.set_after_command_low, 0.0, close_ios_.set_after_command_ios, command_if_ios); - - for (const auto & key : params_.close.set_before_command.high) - { - if (!key.empty()) - { - set_before_command_close.push_back(key); - set_before_command_close_values.push_back(1.0); - command_if_ios.insert(key); - } - } - - for (const auto & key : params_.close.set_before_command.low) - { - if (!key.empty()) - { - set_before_command_close.push_back(key); - set_before_command_close_values.push_back(0.0); - command_if_ios.insert(key); - } - } - - for (const auto & key : params_.close.command.high) - { - command_ios_close.push_back(key); - command_ios_close_values.push_back(1.0); - command_if_ios.insert(key); - } - - for (const auto & key : params_.close.command.low) - { - if (!key.empty()) - { - command_ios_close.push_back(key); - command_ios_close_values.push_back(0.0); - command_if_ios.insert(key); - } - } - - // make full state ios lists -- just once - parse_interfaces_from_params(params_.open.state.high, 1.0, open_ios_.state_ios, state_if_ios); - parse_interfaces_from_params(params_.open.state.low, 0.0, open_ios_.state_ios, state_if_ios); - - // parse_interfaces_from_params(params_.close.state.high, 1.0, close_ios_.state_ios, - // state_if_ios); - - for (const auto & [name, value] : params_.close.state.possible_closed_states_map) - { - for (const auto & key : value.high) - { - if (!key.empty()) - { - state_if_ios.insert(key); - } - } - for (const auto & key : value.low) - { - if (!key.empty()) - { - state_if_ios.insert(key); - } - } - for (const auto & key : value.set_after_command_high) - { - if (!key.empty()) - { - command_if_ios.insert(key); - } - } - for (const auto & key : value.set_after_command_low) - { - if (!key.empty()) - { - command_if_ios.insert(key); - } - } - } - - // get the configurations for different io which needs to be high or low - for (const auto & [key, val] : params_.configuration_setup.configurations_map) - { - config_map_.push_back(val); - } - - // get the configurations list - configurations_list_ = params_.configurations; - - for (const auto & config : config_map_) - { - for (const auto & io : config.command_high) - { - command_if_ios.insert(io); - } - for (const auto & io : config.command_low) - { - command_if_ios.insert(io); - } - for (const auto & io : config.state_high) - { - state_if_ios.insert(io); - } - for (const auto & io : config.state_low) - { - state_if_ios.insert(io); - } - } - - for (size_t i = 0; i < params_.gripper_specific_sensors.size(); ++i) - { - state_if_ios.insert(params_.sensors_interfaces.gripper_specific_sensors_map - .at(params_.gripper_specific_sensors[i]) - .input); - } -} - -controller_interface::CallbackReturn IOGripperController::prepare_publishers_and_services() -{ - reconfigureFlag_.store(false); - - // reset service buffer - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); - - // reset gripper state buffer - gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); - - if (!params_.use_action) - { - // callback groups for each service - open_service_callback_group_ = - get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - close_service_callback_group_ = - get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - reconfigure_service_callback_group_ = - get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - - // open service - auto open_service_callback = [&]( - const std::shared_ptr /*request*/, - std::shared_ptr response) - { - try - { - if (reconfigureFlag_.load()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); - response->success = false; - return; - } - if (closeFlag_.load()) - { - closeFlag_.store(false); - } - openFlag_.store(true); - gripper_service_buffer_.writeFromNonRT(service_mode_type::OPEN); - gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_BEFORE_COMMAND); - while (openFlag_.load()) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - if ((*(gripper_state_buffer_.readFromRT()) == IOGripperState::HALTED)) - { - response->success = false; - break; - } - else - { - response->success = true; - } - } - } - catch (const std::exception & e) - { - response->success = false; - } - }; - - open_service_ = get_node()->create_service( - "~/gripper_open", open_service_callback, qos_services, open_service_callback_group_); - - // close service - auto close_service_callback = [&]( - const std::shared_ptr /*request*/, - std::shared_ptr response) - { - try - { - if (reconfigureFlag_.load()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); - response->success = false; - return; - } - gripper_service_buffer_.writeFromNonRT(service_mode_type::CLOSE); - gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_BEFORE_COMMAND); - if (openFlag_.load()) - { - openFlag_.store(false); - } - closeFlag_.store(true); - while (closeFlag_.load()) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - if ((*(gripper_state_buffer_.readFromRT()) == IOGripperState::HALTED)) - { - response->success = false; - break; - } - else - { - response->success = true; - } - } - } - catch (const std::exception & e) - { - response->success = false; - } - }; - - close_service_ = get_node()->create_service( - "~/gripper_close", close_service_callback, qos_services, close_service_callback_group_); - - // configure gripper service - auto configure_gripper_service_callback = - [&]( - const std::shared_ptr request, - std::shared_ptr response) - { - try - { - std::string conf = request->config_name; - configure_gripper_buffer_.writeFromNonRT(conf.c_str()); - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); - reconfigureFlag_.store(true); - while (reconfigureFlag_.load()) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - response->result = true; - response->status = "Gripper reconfigured"; - } - catch (const std::exception & e) - { - response->result = false; - response->status = "Failed to reconfigure gripper"; - } - }; - - configure_gripper_service_ = get_node()->create_service( - "~/reconfigure_to", configure_gripper_service_callback, qos_services, - reconfigure_service_callback_group_); - } - else - { - // open close action server - gripper_feedback_ = std::make_shared(); - gripper_result_ = std::make_shared(); - gripper_action_server_ = rclcpp_action::create_server( - get_node(), "~/gripper_action", - std::bind( - &IOGripperController::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&IOGripperController::handle_cancel, this, std::placeholders::_1), - std::bind(&IOGripperController::handle_accepted, this, std::placeholders::_1)); - - // reconfigure action server - gripper_config_feedback_ = std::make_shared(); - gripper_config_result_ = std::make_shared(); - gripper_config_action_server_ = rclcpp_action::create_server( - get_node(), "~/reconfigure_gripper_action", - std::bind( - &IOGripperController::config_handle_goal, this, std::placeholders::_1, - std::placeholders::_2), - std::bind(&IOGripperController::config_handle_cancel, this, std::placeholders::_1), - std::bind(&IOGripperController::config_handle_accepted, this, std::placeholders::_1)); - } - - try - { - // Gripper Joint State publisher - g_j_s_publisher_ = - get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); - gripper_joint_state_publisher_ = std::make_unique(g_j_s_publisher_); - - auto final_joint_size = params_.open_close_joints.size() + params_.configuration_joints.size(); - - gripper_joint_state_publisher_->msg_.name.resize(final_joint_size); - gripper_joint_state_publisher_->msg_.position.resize(final_joint_size); - - joint_state_values_.resize(final_joint_size, std::numeric_limits::quiet_NaN()); - - for (size_t i = 0; i < params_.open_close_joints.size(); ++i) - { - gripper_joint_state_publisher_->msg_.name[i] = params_.open_close_joints[i]; - gripper_joint_state_publisher_->msg_.position[i] = joint_state_values_[i]; - } - for (size_t i = 0; i < params_.configuration_joints.size(); ++i) - { - gripper_joint_state_publisher_->msg_.name[i + params_.open_close_joints.size()] = - params_.configuration_joints[i]; - gripper_joint_state_publisher_->msg_.position[i + params_.open_close_joints.size()] = - joint_state_values_[i + params_.open_close_joints.size()]; - } - } - catch (const std::exception & e) - { - fprintf( - stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", - e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - try - { - // interface publisher - if_publisher_ = get_node()->create_publisher( - "~/dynamic_interface", rclcpp::SystemDefaultsQoS()); - interface_publisher_ = std::make_unique(if_publisher_); - } - catch (const std::exception & e) - { - fprintf( - stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", - e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - return controller_interface::CallbackReturn::SUCCESS; -} - -void IOGripperController::publish_gripper_joint_states() -{ - if (gripper_joint_state_publisher_ && gripper_joint_state_publisher_->trylock()) - { - gripper_joint_state_publisher_->msg_.header.stamp = get_node()->get_clock()->now(); - - // publish gripper joint state values - for (size_t i = 0; i < joint_state_values_.size(); ++i) - { - gripper_joint_state_publisher_->msg_.position[i] = joint_state_values_[i]; - } - } - gripper_joint_state_publisher_->unlockAndPublish(); -} - -void IOGripperController::publish_dynamic_interface_values() -{ - if (interface_publisher_ && interface_publisher_->trylock()) - { - interface_publisher_->msg_.header.stamp = - get_node()->get_clock()->now(); // Make sure it works and discuss with Dr. Denis - interface_publisher_->msg_.states.interface_names.clear(); - interface_publisher_->msg_.states.values.clear(); - interface_publisher_->msg_.states.values.resize(state_interfaces_.size()); - for (size_t i = 0; i < state_interfaces_.size(); ++i) - { - interface_publisher_->msg_.states.interface_names.push_back( - state_interfaces_.at(i).get_name()); // this can be done in a separate function one time. - // Change it later TODO (Sachin) : - interface_publisher_->msg_.states.values.at(i) = - static_cast(state_interfaces_.at(i).get_value()); - } - - interface_publisher_->msg_.commands.interface_names.clear(); - interface_publisher_->msg_.commands.values.clear(); - interface_publisher_->msg_.commands.values.resize(command_interfaces_.size()); - for (size_t i = 0; i < command_interfaces_.size(); ++i) - { - interface_publisher_->msg_.commands.interface_names.push_back( - command_interfaces_.at(i).get_name()); // this can be done in a separate function one time. - // Change it later TODO (Sachin) : - interface_publisher_->msg_.commands.values.at(i) = - static_cast(command_interfaces_.at(i).get_value()); - } - interface_publisher_->unlockAndPublish(); - } -} - -rclcpp_action::GoalResponse IOGripperController::handle_goal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) -{ - try - { - if (reconfigureFlag_.load()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); - return rclcpp_action::GoalResponse::REJECT; - } - gripper_service_buffer_.writeFromNonRT( - (goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); - gripper_state_buffer_.writeFromNonRT(IOGripperState::SET_BEFORE_COMMAND); - } - catch (const std::exception & e) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", - e.what()); - return rclcpp_action::GoalResponse::REJECT; - } - (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse IOGripperController::handle_cancel( - const std::shared_ptr goal_handle) -{ - (void)goal_handle; - gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); - gripper_state_buffer_.writeFromNonRT(IOGripperState::IDLE); - return rclcpp_action::CancelResponse::ACCEPT; -} - -void IOGripperController::handle_accepted(const std::shared_ptr goal_handle) -{ - std::thread{std::bind(&IOGripperController::execute, this, std::placeholders::_1), goal_handle} - .detach(); -} - -void IOGripperController::execute(std::shared_ptr goal_handle) -{ - auto result = std::make_shared(); - auto feedback = std::make_shared(); - while (true) - { - if (*(gripper_state_buffer_.readFromRT()) == IOGripperState::IDLE) - { - result->success = true; - result->message = "Gripper action executed"; - goal_handle->succeed(result); - break; - } - else if (*(gripper_state_buffer_.readFromRT()) == IOGripperState::HALTED) - { - result->success = false; - result->message = "Gripper action halted"; - goal_handle->abort(result); - break; - } - else - { - feedback->transition.state = static_cast(*(gripper_state_buffer_.readFromRT())); - goal_handle->publish_feedback(feedback); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } -} - -rclcpp_action::GoalResponse IOGripperController::config_handle_goal( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) -{ - try - { - std::string conf = goal->config_name; - configure_gripper_buffer_.writeFromNonRT(conf.c_str()); - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); - reconfigureFlag_.store(true); - } - catch (const std::exception & e) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", - e.what()); - return rclcpp_action::GoalResponse::REJECT; - } - (void)uuid; - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -rclcpp_action::CancelResponse IOGripperController::config_handle_cancel( - const std::shared_ptr goal_handle) -{ - (void)goal_handle; - configure_gripper_buffer_.writeFromNonRT(""); - reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); - return rclcpp_action::CancelResponse::ACCEPT; -} - -void IOGripperController::config_handle_accepted( - const std::shared_ptr goal_handle) -{ - std::thread{ - std::bind(&IOGripperController::config_execute, this, std::placeholders::_1), goal_handle} - .detach(); -} - -void IOGripperController::config_execute(std::shared_ptr goal_handle) -{ - auto result = std::make_shared(); - auto feedback = std::make_shared(); - while (true) - { - if (*(reconfigure_state_buffer_.readFromRT()) == reconfigure_state_type::IDLE) - { - result->result = true; - result->status = "Gripper reconfigured"; - goal_handle->succeed(result); - break; - } - else - { - feedback->transition.state = static_cast(*(reconfigure_state_buffer_.readFromRT())); - goal_handle->publish_feedback(feedback); - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } -} - -void IOGripperController::check_gripper_and_reconfigure_state() -{ - bool gripper_state_found = false; - - for (size_t i = 0; i < state_ios_open.size(); ++i) - { - setResult = find_and_get_state(state_ios_open[i], state_value_); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); - } - else - { - if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) - { - gripper_state_found = true; - } - else - { - gripper_state_found = false; - } - } - } - - if (gripper_state_found) - { - for (size_t i = 0; i < params_.open.joint_states.size(); ++i) - { - joint_state_values_[i] = params_.open.joint_states[i]; - } - } - else - { - for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) - { - for (const auto & high_val : state_params.high) - { - setResult = find_and_get_state(high_val, state_value_); - if (!setResult) - { - RCLCPP_ERROR( - get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); - } - else - { - if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) - { - gripper_state_found = true; - } - else - { - gripper_state_found = false; - break; - } - } - } - for (const auto & low_val : state_params.low) - { - setResult = find_and_get_state(low_val, state_value_); - if (!setResult) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); - } - else - { - if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) - { - gripper_state_found = true; - } - else - { - gripper_state_found = false; - break; - } - } - } - - if (gripper_state_found) - { - for (size_t i = 0; - i < params_.close.state.possible_closed_states_map.at(state_name).joint_states.size(); - ++i) - { - joint_state_values_[i] = - params_.close.state.possible_closed_states_map.at(state_name).joint_states[i]; - } - break; - } - } - } - - bool reconfigure_state_found = false; - for (const auto & [key, val] : params_.configuration_setup.configurations_map) - { - for (const auto & io : val.state_high) - { - setResult = find_and_get_state(io, state_value_); - if (!setResult) - { - reconfigure_state_found = false; - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); - } - else - { - if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) - { - reconfigure_state_found = false; - RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); - break; - } - else - { - reconfigure_state_found = true; - } - } - } - - for (const auto & io : val.state_low) - { - setResult = find_and_get_state(io, state_value_); - if (!setResult) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); - } - else - { - if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) - { - reconfigure_state_found = false; - RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); - break; - } - else - { - reconfigure_state_found = true; - } - } - } - if (reconfigure_state_found) - { - for (size_t i = 0; i < val.joint_states.size(); ++i) - { - joint_state_values_[i + params_.open_close_joints.size()] = val.joint_states[i]; - } - break; - } - } -} -} // namespace io_gripper_controller - -#include "pluginlib/class_list_macros.hpp" - -PLUGINLIB_EXPORT_CLASS( - io_gripper_controller::IOGripperController, controller_interface::ControllerInterface) diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml deleted file mode 100644 index 125b1e7904..0000000000 --- a/io_gripper_controller/src/io_gripper_controller.yaml +++ /dev/null @@ -1,261 +0,0 @@ -# Copyright (c) 2025, b»robotized by Stogl Robotics -# -# 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. - -io_gripper_controller: - use_action: { - type: bool, - default_value: false, - description: "True for using action server and false service server for the gripper" - } - timeout: { - type: double, - default_value: 5.0, - description: "Timeout for the waiting on signals from gripper about reached state.", - validation: { - gt<>: [0.0], - } - } - open_close_joints: { - type: string_array, - description: "List of joint names that should change values according to open or close state of the gripper", - validation: { - unique<>: null, - not_empty<>: null, - } - } - open: - joint_states: { - type: double_array, - description: "List of joint values when gripper is open. The order of the values should match the order of the joint names in open_close_joints", - validation: { - unique<>: null, - not_empty<>: null, - } - } - set_before_command: - high: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to high (1.0) before opening the gripper", - validation: { - unique<>: null, - } - } - low: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to low (0.0) before opening the gripper", - validation: { - unique<>: null, - } - } - command: - high: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to high (1.0) to open the gripper.", - validation: { - unique<>: null, - } - } - low: { - type: string_array, - default_value: [], - description: "(optional) list of command interface names that have to be see to low (0.0) to open the gripper.", - validation: { - unique<>: null, - } - } - state: - high: { - type: string_array, - default_value: [], - description: "(optional) list of state interfaces that have to equal high (1.0) to represent the gripper is open.", - validation: { - unique<>: null, - } - } - low: { - type: string_array, - default_value: [], - description: "(optional) list of state interfaces that have to equal low (0.0) to represent the gripper is open.", - validation: { - unique<>: null, - } - } - set_after_command: - high: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to high (1.0) after opening the gripper", - validation: { - unique<>: null, - } - } - low: { - type: string_array, - default_value: [], - description: "(optional) list of command interface names that have to be set to low (0.0) after opening the gripper.", - validation: { - unique<>: null, - } - } - possible_closed_states: { - type: string_array, - default_value: [], - description: "List of possible closed states e.g. empty_close and full close", - validation: { - unique<>: null, - } - } - close: - set_before_command: - high: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to high (1.0) before closing the gripper", - validation: { - unique<>: null, - } - } - low: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to low (0.0) before closing the gripper", - validation: { - unique<>: null, - } - } - command: - high: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to high (1.0) to close the gripper", - validation: { - unique<>: null, - } - } - low: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to low (0.0) to close the gripper", - validation: { - unique<>: null, - } - } - state: - __map_possible_closed_states: - joint_states: { - type: double_array, - default_value: [], - description: "List of joint values when gripper is closed. The order of the values should match the order of the joint names in open_close_joints", - validation: { - unique<>: null, - } - } - high: { - type: string_array, - default_value: [], - description: "(optional) list of state interfaces that have to equal high (1.0) to represent the gripper is closed", - validation: { - unique<>: null, - } - } - low: { - type: string_array, - default_value: [], - description: "(optional) list of state interfaces that have to equal low (0.0) to represent the gripper is closed", - validation: { - unique<>: null, - } - } - set_after_command_high: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to high (1.0) after closing the gripper", - validation: { - unique<>: null, - } - } - set_after_command_low: { - type: string_array, - default_value: [], - description: "(optional) list of command interface names that have to be set to low (0.0) after closing the gripper.", - validation: { - unique<>: null, - } - } - configuration_joints: { - type: string_array, - default_value: [], - description: "List of joint names that are used to switch between different configurations of the gripper", - validation: { - unique<>: null, - } - } - - configurations: { - type: string_array, - default_value: [], - description: "Configuration names that can be used to switch between different configurations of the gripper", - validation: { - unique<>: null, - } - } - configuration_setup: - __map_configurations: - joint_states: { - type: double_array, - default_value: [], - description: "List of joint states that open the gripper", - validation: { - unique<>: null, - } - } - command_high: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to high (1.0) for this configuration.", - } - command_low: { - type: string_array, - default_value: [], - description: "(optional) list of command interfaces that have to be set to low (0.0) for this configuration.", - } - state_high: { - type: string_array, - default_value: [], - description: "(optional) list of state interfaces that have to be high (1.0) to represent this configuration.", - } - state_low: { - type: string_array, - default_value: [], - description: "(optional) list of state interfaces that have to be low (0.0) to represent this configuration.", - } - - gripper_specific_sensors: { - type: string_array, - default_value: [], - description: "List of sensor names that are specific to the gripper", - validation: { - unique<>: null, - } - } - sensors_interfaces: - __map_gripper_specific_sensors: - input: { - type: string, - default_value: "", - description: "Name of the input interface that is specific to the gripper", - } diff --git a/io_gripper_controller/test/test_io_gripper_controller.yaml b/io_gripper_controller/test/test_io_gripper_controller.yaml deleted file mode 100644 index f9b4d470f4..0000000000 --- a/io_gripper_controller/test/test_io_gripper_controller.yaml +++ /dev/null @@ -1,67 +0,0 @@ -test_io_gripper_controller: - ros__parameters: - use_action: true - timeout: 5.0 - open_close_joints: [gripper_clamp_jaw] - - open: - joint_states: [0.0] - set_before_command: - high: [Release_Break_valve] - low: [Release_Something] # comment out otherwise throws an error - command: - high: [Open_valve] - low: [Close_valve] - state: - high: [Opened_signal] - low: [Closed_signal] - set_after_command: - high: [Release_Something] - low: [Release_Break_valve] - - possible_closed_states: ['empty_close', 'full_close'] - - close: - set_before_command: - high: [Release_Break_valve] - low: [Release_Something] - command: - high: [Close_valve] - low: [Open_valve] - state: - empty_close: - joint_states: [0.16] - high: [Closed_signal] - low: [Part_Grasped_signal] - set_after_command_high: [a] - set_after_command_low: [Release_Break_valve] - full_close: - joint_states: [0.08] - high: [Part_Grasped_signal] - low: [Closed_signal] - set_after_command_high: [Release_Something] - set_after_command_low: [Release_Break_valve] - - configurations: ["narrow_objects", "wide_objects"] - configuration_joints: [gripper_gripper_distance_joint] - - configuration_setup: - stichmass_125: - joint_states: [0.1] - command_high: [Narrow_Configuration_Cmd] - command_low: [Wide_Configuration_Cmd] - state_high: [Narrow_Configuraiton_Signal] - state_low: [Wide_Configuration_Signal] - stichmass_250: - joint_states: [0.2] - command_high: [Wide_Configuration_Cmd] - command_low: [Narrow_Configuration_Cmd] - state_high: [Wide_Configuration_Signal] - state_low: [Narrow_Configuraiton_Signal] - - gripper_specific_sensors: ["part_sensor_top", "part_sensor"] - sensors_interfaces: - hohenabfrage: - input: "Part_Sensor_Top_signal" - bauteilabfrage: - input: "Part_Grasped_signal" From 7bfce93b976b6b784122d82a22f8acca117eb88c Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 13 Jun 2025 06:43:33 +0200 Subject: [PATCH 67/75] Ignore all other packages. --- ackermann_steering_controller/COLCON_IGNORE | 0 admittance_controller/COLCON_IGNORE | 0 bicycle_steering_controller/COLCON_IGNORE | 0 diff_drive_controller/COLCON_IGNORE | 0 effort_controllers/COLCON_IGNORE | 0 force_torque_sensor_broadcaster/COLCON_IGNORE | 0 forward_command_controller/COLCON_IGNORE | 0 gripper_controllers/COLCON_IGNORE | 0 imu_sensor_broadcaster/COLCON_IGNORE | 0 joint_state_broadcaster/COLCON_IGNORE | 0 joint_trajectory_controller/COLCON_IGNORE | 0 mecanum_drive_controller/COLCON_IGNORE | 0 parallel_gripper_controller/COLCON_IGNORE | 0 pid_controller/COLCON_IGNORE | 0 pose_broadcaster/COLCON_IGNORE | 0 position_controllers/COLCON_IGNORE | 0 range_sensor_broadcaster/COLCON_IGNORE | 0 ros2_controllers/COLCON_IGNORE | 0 ros2_controllers_test_nodes/COLCON_IGNORE | 0 rqt_joint_trajectory_controller/COLCON_IGNORE | 0 steering_controllers_library/COLCON_IGNORE | 0 tricycle_controller/COLCON_IGNORE | 0 tricycle_steering_controller/COLCON_IGNORE | 0 velocity_controllers/COLCON_IGNORE | 0 24 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 ackermann_steering_controller/COLCON_IGNORE create mode 100644 admittance_controller/COLCON_IGNORE create mode 100644 bicycle_steering_controller/COLCON_IGNORE create mode 100644 diff_drive_controller/COLCON_IGNORE create mode 100644 effort_controllers/COLCON_IGNORE create mode 100644 force_torque_sensor_broadcaster/COLCON_IGNORE create mode 100644 forward_command_controller/COLCON_IGNORE create mode 100644 gripper_controllers/COLCON_IGNORE create mode 100644 imu_sensor_broadcaster/COLCON_IGNORE create mode 100644 joint_state_broadcaster/COLCON_IGNORE create mode 100644 joint_trajectory_controller/COLCON_IGNORE create mode 100644 mecanum_drive_controller/COLCON_IGNORE create mode 100644 parallel_gripper_controller/COLCON_IGNORE create mode 100644 pid_controller/COLCON_IGNORE create mode 100644 pose_broadcaster/COLCON_IGNORE create mode 100644 position_controllers/COLCON_IGNORE create mode 100644 range_sensor_broadcaster/COLCON_IGNORE create mode 100644 ros2_controllers/COLCON_IGNORE create mode 100644 ros2_controllers_test_nodes/COLCON_IGNORE create mode 100644 rqt_joint_trajectory_controller/COLCON_IGNORE create mode 100644 steering_controllers_library/COLCON_IGNORE create mode 100644 tricycle_controller/COLCON_IGNORE create mode 100644 tricycle_steering_controller/COLCON_IGNORE create mode 100644 velocity_controllers/COLCON_IGNORE diff --git a/ackermann_steering_controller/COLCON_IGNORE b/ackermann_steering_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/admittance_controller/COLCON_IGNORE b/admittance_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/bicycle_steering_controller/COLCON_IGNORE b/bicycle_steering_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/diff_drive_controller/COLCON_IGNORE b/diff_drive_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/effort_controllers/COLCON_IGNORE b/effort_controllers/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/force_torque_sensor_broadcaster/COLCON_IGNORE b/force_torque_sensor_broadcaster/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/forward_command_controller/COLCON_IGNORE b/forward_command_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/gripper_controllers/COLCON_IGNORE b/gripper_controllers/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/imu_sensor_broadcaster/COLCON_IGNORE b/imu_sensor_broadcaster/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/joint_state_broadcaster/COLCON_IGNORE b/joint_state_broadcaster/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/joint_trajectory_controller/COLCON_IGNORE b/joint_trajectory_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/mecanum_drive_controller/COLCON_IGNORE b/mecanum_drive_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/parallel_gripper_controller/COLCON_IGNORE b/parallel_gripper_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/pid_controller/COLCON_IGNORE b/pid_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/pose_broadcaster/COLCON_IGNORE b/pose_broadcaster/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/position_controllers/COLCON_IGNORE b/position_controllers/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/range_sensor_broadcaster/COLCON_IGNORE b/range_sensor_broadcaster/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ros2_controllers/COLCON_IGNORE b/ros2_controllers/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ros2_controllers_test_nodes/COLCON_IGNORE b/ros2_controllers_test_nodes/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rqt_joint_trajectory_controller/COLCON_IGNORE b/rqt_joint_trajectory_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/steering_controllers_library/COLCON_IGNORE b/steering_controllers_library/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/tricycle_controller/COLCON_IGNORE b/tricycle_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/tricycle_steering_controller/COLCON_IGNORE b/tricycle_steering_controller/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/velocity_controllers/COLCON_IGNORE b/velocity_controllers/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 From cdd8add54c51e19f71547bcda3aabcc2a6162f4f Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Sat, 16 Aug 2025 21:05:43 +0200 Subject: [PATCH 68/75] Fully working on the Gripper example. --- gpio_controllers/CMakeLists.txt | 11 + gpio_controllers/doc/userdoc.rst | 23 + .../gpio_controllers/gpio_tool_controller.hpp | 37 +- gpio_controllers/src/gpio_tool_controller.cpp | 488 +++++++++++------- ...est_gpio_tool_controller_lift_example.yaml | 2 +- .../test/test_load_gpio_tool_controller.cpp | 37 ++ 6 files changed, 390 insertions(+), 208 deletions(-) create mode 100644 gpio_controllers/test/test_load_gpio_tool_controller.cpp diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index 3a6c7cf3e4..5885155755 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -105,6 +105,17 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) + ament_add_gmock( + test_load_gpio_tool_controller + test/test_load_gpio_tool_controller.cpp + ) + target_include_directories(test_load_gpio_tool_controller PRIVATE include) + target_link_libraries(test_load_gpio_tool_controller + gpio_tool_controllers + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + # ament_add_gmock(test_gpio_tool_controller # test/gpio_tool_controller/test_gpio_tool_controller.cpp # test/gpio_tool_controller/test_gpio_tool_controller_open.cpp diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index a59da906bb..f747539476 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -57,3 +57,26 @@ The controller expects at least one GPIO interface and the corresponding command - ana.2 With the above-defined controller configuration, the controller will accept commands for all gpios' interfaces and will only publish the state of Gpio2. + + + +### gpio_tool_controller + +The controller enables control of tools connected to the robot or components of a robot via general-purpose inputs and outputs (GPIO), digital or analog. +Concretely the controller is made for controlling the custom-made industrial grippers or tools, or components like lifts or breaks. +The IOs might even be virtual, like controlling the state machine of a robot that is implemented on a PLC. + +The controller provides high-level interface for engaging or disengaging a tool or a component, as well as for reconfiguring it. +The reconfiguration is possible only in *disengaged* state. +In the example of gripper this would be *open* state. + + + +The controller implements state machine for the transition between states and + + +1. Tool Actions + +1. Tool state machine + +2. Transitions diff --git a/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp index 8b10e6b667..31c28ee0f0 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp @@ -132,33 +132,28 @@ class GpioToolController : public controller_interface::ControllerInterface { std::vector possible_states; - std::unordered_map> set_before_commands; - std::unordered_map set_before_commands_start_index; - std::unordered_map> set_before_states; - std::unordered_map set_before_states_start_index; + std::unordered_map>> set_before_commands; + std::unordered_map>> set_before_states; - std::unordered_map> commands; - std::unordered_map commands_start_index; + std::unordered_map>> commands; - std::unordered_map> states; - std::unordered_map states_start_index; + std::unordered_map>> states; std::unordered_map> states_joint_states; - std::unordered_map> set_after_commands; - std::unordered_map set_after_commands_start_index; - std::unordered_map> set_after_states; - std::unordered_map set_after_states_start_index; + std::unordered_map>> set_after_commands; + std::unordered_map>> set_after_states; }; ToolTransitionIOs disengaged_gpios_; ToolTransitionIOs engaged_gpios_; ToolTransitionIOs reconfigure_gpios_; + // Not needed to be atomic or protected as used only in the RT loop rclcpp::Time state_change_start_; std::string current_configuration_; std::string current_state_; - std::unordered_set command_if_ios; - std::unordered_set state_if_ios; + std::unordered_set command_if_ios_; + std::unordered_set state_if_ios_; using EngagingSrvType = example_interfaces::srv::Trigger; using ResetSrvType = example_interfaces::srv::Trigger; @@ -218,12 +213,13 @@ class GpioToolController : public controller_interface::ControllerInterface const rclcpp::Time & current_time, const ToolTransitionIOs & ios, std::vector & joint_states, const size_t joint_states_start_index, const std::string & output_prefix, const uint8_t next_transition, - std::string & found_state_name); + std::string & found_state_name, const bool warning_output = false); /** * @brief Prepares the command and state IOs. + * \returns true if successful, false otherwise. Check the output if error has happend. */ - void prepare_command_and_state_ios(); + bool prepare_command_and_state_ios(); /** * @brief Prepares the publishers and services. @@ -239,15 +235,14 @@ class GpioToolController : public controller_interface::ControllerInterface /** * @brief Checks the tools state. */ - void check_tool_state(const rclcpp::Time & current_time); + void check_tool_state(const rclcpp::Time & current_time, const bool warning_output = false); bool set_commands( - const std::unordered_map & commands, - const size_t start_index, - const std::string & transition_name, + const std::unordered_map> & commands, + const std::string & output_prefix, const uint8_t next_transition); bool check_states( - const rclcpp::Time & current_time, const std::unordered_map & states, const size_t start_index, const std::string & transition_name, const uint8_t next_transition); + const rclcpp::Time & current_time, const std::unordered_map> & states, const std::string & output_prefix, const uint8_t next_transition, const bool warning_output = false); std::vector configurations_list_; std::vector config_map_; diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp index d9d6aa2679..b5d4a43426 100644 --- a/gpio_controllers/src/gpio_tool_controller.cpp +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -116,6 +116,11 @@ controller_interface::CallbackReturn GpioToolController::on_configure( } } + // Initialize storage of joint state values + joint_states_values_.resize( + params_.engaged_joints.size() + params_.configuration_joints.size(), + std::numeric_limits::quiet_NaN()); + auto check_parameter_pairs = [this](const std::vector & interfaces, const std::vector & values, const std::string & parameter_name) { if (interfaces.size() != values.size()) { RCLCPP_FATAL( @@ -205,7 +210,12 @@ controller_interface::CallbackReturn GpioToolController::on_configure( return CallbackReturn::FAILURE; } - prepare_command_and_state_ios(); + if (!prepare_command_and_state_ios()) + { + RCLCPP_FATAL( + get_node()->get_logger(), "Failed to prepare command and state GPIOs. See above messages for details."); + return CallbackReturn::FAILURE; + } auto result = prepare_publishers_and_services(); if (result != controller_interface::CallbackReturn::SUCCESS) @@ -221,8 +231,8 @@ controller_interface::InterfaceConfiguration GpioToolController::command_interfa controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(command_if_ios.size()); - for (const auto & command_io : command_if_ios) + command_interfaces_config.names.reserve(command_if_ios_.size()); + for (const auto & command_io : command_if_ios_) { command_interfaces_config.names.push_back(command_io); } @@ -236,8 +246,8 @@ controller_interface::InterfaceConfiguration GpioToolController::state_interface controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(state_if_ios.size()); - for (const auto & state_io : state_if_ios) + state_interfaces_config.names.reserve(state_if_ios_.size()); + for (const auto & state_io : state_if_ios_) { state_interfaces_config.names.push_back(state_io); } @@ -248,7 +258,14 @@ controller_interface::InterfaceConfiguration GpioToolController::state_interface controller_interface::CallbackReturn GpioToolController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - check_tool_state(get_node()->now()); + state_change_start_ = get_node()->now(); + check_tool_state(state_change_start_); + // TODO(denis): update ros2_control and then enable this in this version the interfaces are not released when controller fails to activate + // if (current_state_.empty() || current_configuration_.empty()) + // { + // RCLCPP_FATAL(get_node()->get_logger(), "Controller can not be started as tool state can not be determined! Make sure the hardware is connected properly and controller's configuration is correct, than try to activate the controller again."); + // return controller_interface::CallbackReturn::FAILURE; + // } return controller_interface::CallbackReturn::SUCCESS; } @@ -263,129 +280,141 @@ controller_interface::CallbackReturn GpioToolController::on_deactivate( controller_interface::return_type GpioToolController::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + switch (current_tool_action_.load()) { - switch (current_tool_action_.load()) + case ToolAction::IDLE: { - case ToolAction::IDLE: - { - // do nothing - break; - } - case ToolAction::DISENGAGING: - { - handle_tool_state_transition( - time, disengaged_gpios_, params_.disengaged.name, joint_states_values_, 0, current_state_); - break; - } - case ToolAction::ENGAGING: - { - handle_tool_state_transition( - time, engaged_gpios_, params_.engaged.name, joint_states_values_, 0, current_state_); - break; - } - case ToolAction::RECONFIGURING: - { - handle_tool_state_transition( - time, reconfigure_gpios_, *(target_configuration_.load()), joint_states_values_, params_.engaged_joints.size(), current_configuration_); - break; - } - case ToolAction::CANCELING: - { - RCLCPP_ERROR( - get_node()->get_logger(), "%s - CANCELING: Tool action is being canceled, " - "going to HALTED. Reset the tool using '~/reset_halted' service. After that set sensible state.", current_state_.c_str()); - current_tool_transition_.store(GPIOToolTransition::HALTED); - check_tool_state(time); - std::vector tmp_vec; - std::string tmp_str; - handle_tool_state_transition( - time, ToolTransitionIOs(), "", tmp_vec, 0, - tmp_str); // parameters don't matter as end up processing only the halted state - break; - } - default: - { - break; - } + // do nothing + break; } - + case ToolAction::DISENGAGING: + { + handle_tool_state_transition( + time, disengaged_gpios_, params_.disengaged.name, joint_states_values_, 0, current_state_); + break; + } + case ToolAction::ENGAGING: + { + handle_tool_state_transition( + time, engaged_gpios_, params_.engaged.name, joint_states_values_, 0, current_state_); + break; + } + case ToolAction::RECONFIGURING: + { + handle_tool_state_transition( + time, reconfigure_gpios_, *(target_configuration_.load()), joint_states_values_, params_.engaged_joints.size(), current_configuration_); + break; + } + case ToolAction::CANCELING: + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "%s - CANCELING: Tool action is being canceled, " + "going to HALTED. Reset the tool using '~/reset_halted' service. After that set sensible state.", current_state_.c_str()); + current_tool_transition_.store(GPIOToolTransition::HALTED); + check_tool_state(time, true); + std::vector tmp_vec; + std::string tmp_str; + handle_tool_state_transition( + time, ToolTransitionIOs(), "", tmp_vec, 0, + tmp_str); // parameters don't matter as end up processing only the halted state + break; + } + default: + { + break; + } + } publish_topics(time); return controller_interface::return_type::OK; } bool GpioToolController::set_commands( - const std::unordered_map & commands, const size_t start_index, const std::string & transition_name, const uint8_t next_transition) + const std::unordered_map> & commands, const std::string & output_prefix, const uint8_t next_transition) { bool all_successful = true; - if (commands.empty()) - { - return all_successful; // nothing to set, return true - } - for (size_t i = start_index; i < (start_index + commands.size()); ++i) + if (!commands.empty()) // only set commands if present { - if (!command_interfaces_[i].set_value(commands.at(command_interfaces_[i].get_name()))) + for (const auto & [name, pair] : commands) { - RCLCPP_ERROR( - get_node()->get_logger(), "%s: Failed to set the command state for %s", - transition_name.c_str(), command_interfaces_[i].get_name().c_str()); - all_successful = false; + const auto & [value, index] = pair; + if (!command_interfaces_[index].set_value(value)) + { + RCLCPP_ERROR( + get_node()->get_logger(), "%s: Failed to set the command state for %s", + output_prefix.c_str(), command_interfaces_[index].get_name().c_str()); + all_successful = false; + } } } if (all_successful) { - RCLCPP_INFO( - get_node()->get_logger(), "%s: Setting the before command states", - transition_name.c_str()); - current_tool_transition_.store(next_transition); + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: Transitioning after setting commands to: %d", + output_prefix.c_str(), next_transition); + // when canceling we don't continue the transition + if (current_tool_action_.load() != ToolAction::CANCELING) + { + current_tool_transition_.store(next_transition); + } } else { RCLCPP_ERROR( get_node()->get_logger(), "%s: Error occured when setting commands - see above for details.", - transition_name.c_str()); + output_prefix.c_str()); + current_tool_transition_.store(GPIOToolTransition::HALTED); } return all_successful; } bool GpioToolController::check_states( - const rclcpp::Time & current_time, const std::unordered_map & states, const size_t start_index, const std::string & transition_name, const uint8_t next_transition) + const rclcpp::Time & current_time, const std::unordered_map> & states, const std::string & output_prefix, const uint8_t next_transition, const bool warning_output) { bool all_correct = true; - if (states.empty()) - { - return all_correct; // nothing to check, return true - } - for (size_t i = start_index; i < (start_index + states.size()); ++i) + if (!states.empty()) // only check the states if they are present { - const double current_state_value = state_interfaces_.at(i).get_optional().value_or(std::numeric_limits::quiet_NaN()); - if ( - abs(current_state_value - states.at(state_interfaces_.at(i).get_name())) > params_.tolerance) + for (const auto& [name, pair] : states) { - RCLCPP_INFO( - get_node()->get_logger(), "%s: State value for %s doesn't match", - transition_name.c_str(), state_interfaces_[i].get_name().c_str()); - all_correct = false; + const auto & [value, index] = pair; + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: Looking for state interface '%s' on index %zu, and state size is %zu, and state_interface size is %zu", + output_prefix.c_str(), state_interfaces_.at(index).get_name().c_str(), index, states.size(), state_interfaces_.size()); + const double current_state_value = state_interfaces_.at(index).get_optional().value_or(std::numeric_limits::quiet_NaN()); + if (std::isnan(current_state_value) || + abs(current_state_value - value) > params_.tolerance) + { + RCLCPP_WARN_EXPRESSION( + get_node()->get_logger(), warning_output, "%s: State value for %s doesn't match. Is %.2f, expected %.2f", + output_prefix.c_str(), state_interfaces_.at(index).get_name().c_str(), + current_state_value, value); + all_correct = false; + } } } if (all_correct) { - RCLCPP_INFO( - get_node()->get_logger(), "%s: Tool reached target state.", - transition_name.c_str()); - // if the Tool is in the correct state, we can set the command - current_tool_transition_.store(next_transition); + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: Transitionning after reaching state to: %d", + output_prefix.c_str(), next_transition); + // when canceling we don't continue transition + if (current_tool_action_.load() != ToolAction::CANCELING) + { + current_tool_transition_.store(next_transition); + } } - if ((current_time - state_change_start_).seconds() > params_.timeout) + else if ((current_time - state_change_start_).seconds() > params_.timeout) { RCLCPP_ERROR( get_node()->get_logger(), "%s: Tool didin't reached target state within %.2f seconds.", - transition_name.c_str(), params_.timeout); + output_prefix.c_str(), params_.timeout); current_tool_transition_.store(GPIOToolTransition::HALTED); } @@ -393,20 +422,33 @@ bool GpioToolController::check_states( } void GpioToolController::check_tool_state_and_switch( - const rclcpp::Time & current_time, const ToolTransitionIOs & ios, std::vector & joint_states, const size_t joint_states_start_index, const std::string & output_prefix, const uint8_t next_transition, std::string & found_state_name) + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, std::vector & joint_states, const size_t joint_states_start_index, const std::string & output_prefix, const uint8_t next_transition, std::string & found_state_name, const bool warning_output) { for (const auto & [state_name, states] : ios.states) { - bool state_exists = ios.states_start_index.find(state_name) != ios.states_start_index.end(); - RCLCPP_INFO( - get_node()->get_logger(), "%s - CHECK_COMMAND: Checking state '%s' for tool. State exists %s", - output_prefix.c_str(), state_name.c_str(), (state_exists ? "true" : "false")); + bool state_exists = states.size() > 0; + RCLCPP_DEBUG( + get_node()->get_logger(), "%s - CHECK_STATE: Checking state '%s' for tool. States _%s_ exist. Used number of state interfaces %zu", + output_prefix.c_str(), state_name.c_str(), (state_exists ? "do" : "do not"), states.size()); - if (check_states(current_time, states, ios.states_start_index.at(state_name), output_prefix, next_transition)) + if (check_states(current_time, states, output_prefix, next_transition, warning_output)) { - const auto new_joint_states = ios.states_joint_states.at(state_name); - std::copy(new_joint_states.begin(), new_joint_states.end(), joint_states.begin() + joint_states_start_index); found_state_name = state_name; + const auto & js_val = ios.states_joint_states.at(state_name); + if (joint_states_start_index + js_val.size() <= joint_states.size()) + { + std::copy( + js_val.begin(), + js_val.end(), + joint_states.begin() + joint_states_start_index); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - CHECK_STATE: Joint states size (%zu) is not enough to copy the values for state '%s' (size: %zu) from starting index %zu.", + output_prefix.c_str(), joint_states.size(), state_name.c_str(), js_val.size(), joint_states_start_index); + } break; } } @@ -424,9 +466,6 @@ void GpioToolController::handle_tool_state_transition( break; case GPIOToolTransition::HALTED: - RCLCPP_DEBUG( - get_node()->get_logger(), "%s - HALTED: Tool aborted in HALTED state - don't ", - target_state.c_str()); if (reset_halted_.load()) { // check here the state of the tool if you can figure it out. If not - set unknown. @@ -436,35 +475,37 @@ void GpioToolController::handle_tool_state_transition( break; case GPIOToolTransition::SET_BEFORE_COMMAND: + RCLCPP_DEBUG( + get_node()->get_logger(), "%s - SET_BEFORE_COMMAND: Tool action is being set before command.", + target_state.c_str()); if (!transition_time_updated_.load()) { state_change_start_ = current_time; transition_time_updated_.store(true); } set_commands( - ios.set_before_commands.at(target_state), ios.set_before_commands_start_index.at(target_state), + ios.set_before_commands.at(target_state), target_state + " - SET_BEFORE_COMMAND", GPIOToolTransition::CHECK_BEFORE_COMMAND); break; case GPIOToolTransition::CHECK_BEFORE_COMMAND: check_states( current_time, ios.set_before_states.at(target_state), - ios.set_before_states_start_index.at(target_state), target_state + " - CHECK_BEFORE_COMMAND", GPIOToolTransition::SET_COMMAND); break; case GPIOToolTransition::SET_COMMAND: - set_commands(ios.commands.at(target_state), ios.commands_start_index.at(target_state), target_state + " - SET_COMMAND", GPIOToolTransition::CHECK_COMMAND); + set_commands(ios.commands.at(target_state), target_state + " - SET_COMMAND", GPIOToolTransition::CHECK_COMMAND); break; case GPIOToolTransition::CHECK_COMMAND: check_tool_state_and_switch(current_time, ios, joint_states, joint_states_start_index, target_state + " - CHECK_COMMAND", GPIOToolTransition::SET_AFTER_COMMAND, end_state); break; case GPIOToolTransition::SET_AFTER_COMMAND: set_commands( - ios.set_after_commands.at(end_state), ios.set_after_commands_start_index.at(end_state), + ios.set_after_commands.at(end_state), target_state + " - SET_AFTER_COMMAND", GPIOToolTransition::CHECK_AFTER_COMMAND); break; case GPIOToolTransition::CHECK_AFTER_COMMAND: - if (check_states(current_time, ios.set_after_states.at(end_state), ios.set_after_states_start_index.at(end_state), target_state + " - CHECK_AFTER_COMMAND", GPIOToolTransition::IDLE)) + if (check_states(current_time, ios.set_after_states.at(end_state), target_state + " - CHECK_AFTER_COMMAND", GPIOToolTransition::IDLE)) { finish_transition_to_state = true; } @@ -481,20 +522,20 @@ void GpioToolController::handle_tool_state_transition( transition_time_updated_.store(false); // resetting the flag RCLCPP_INFO( - get_node()->get_logger(), "%s: Transitions finished!", + get_node()->get_logger(), "%s: Tool state or configuration change finished!", target_state.c_str()); } } -void GpioToolController::prepare_command_and_state_ios() +bool GpioToolController::prepare_command_and_state_ios() { auto parse_interfaces_from_params = [&]( const std::vector & interfaces, const std::vector & values, const std::string & param_name, - std::unordered_map & ios, - std::unordered_set & interface_list, - size_t & ios_start_index) + std::unordered_map> & ios, + std::unordered_set & interface_list + ) -> void { if (interfaces.size() != values.size()) { @@ -506,13 +547,6 @@ void GpioToolController::prepare_command_and_state_ios() return; } - if (interfaces.empty()) - { - ios_start_index = -1; - return; - } - - ios_start_index = interface_list.size(); ios.reserve(interfaces.size()); interface_list.reserve(interface_list.size() + interfaces.size()); @@ -520,7 +554,8 @@ void GpioToolController::prepare_command_and_state_ios() { if (!interfaces[i].empty()) { - ios[interfaces[i]] = values[i]; + ios[interfaces[i]].first = values[i]; + ios[interfaces[i]].second = -1; // -1 means not set yet interface_list.insert(interfaces[i]); } } @@ -530,85 +565,82 @@ void GpioToolController::prepare_command_and_state_ios() disengaged_gpios_.possible_states.push_back(params_.disengaged.name); disengaged_gpios_.set_before_commands[params_.disengaged.name] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( - params_.disengaged.set_before_command.interfaces, params_.disengaged.set_before_command.values, "disengaged.set_before_command", disengaged_gpios_.set_before_commands[params_.disengaged.name], command_if_ios, disengaged_gpios_.set_before_commands_start_index[params_.disengaged.name]); + params_.disengaged.set_before_command.interfaces, params_.disengaged.set_before_command.values, + "disengaged.set_before_command", disengaged_gpios_.set_before_commands[params_.disengaged.name], + command_if_ios_); disengaged_gpios_.set_before_states[params_.disengaged.name] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( - params_.disengaged.set_before_state.interfaces, params_.disengaged.set_before_state.values, "disengaged.set_before_state", disengaged_gpios_.set_before_states[params_.disengaged.name], state_if_ios, disengaged_gpios_.set_before_states_start_index[params_.disengaged.name]); + params_.disengaged.set_before_state.interfaces, params_.disengaged.set_before_state.values, "disengaged.set_before_state", disengaged_gpios_.set_before_states[params_.disengaged.name], state_if_ios_); disengaged_gpios_.commands[params_.disengaged.name] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( - params_.disengaged.command.interfaces, params_.disengaged.command.values, "disengaged.command", disengaged_gpios_.commands[params_.disengaged.name], command_if_ios, disengaged_gpios_.commands_start_index[params_.disengaged.name]); + params_.disengaged.command.interfaces, params_.disengaged.command.values, "disengaged.command", disengaged_gpios_.commands[params_.disengaged.name], command_if_ios_); disengaged_gpios_.states[params_.disengaged.name] = - std::unordered_map(); - + std::unordered_map>(); parse_interfaces_from_params( - params_.disengaged.state.interfaces, params_.disengaged.state.values, "disengaged.state", disengaged_gpios_.states[params_.disengaged.name], state_if_ios, disengaged_gpios_.states_start_index[params_.disengaged.name]); + params_.disengaged.state.interfaces, params_.disengaged.state.values, "disengaged.state", disengaged_gpios_.states[params_.disengaged.name], state_if_ios_); disengaged_gpios_.states_joint_states[params_.disengaged.name] = params_.disengaged.joint_states; disengaged_gpios_.set_after_commands[params_.disengaged.name] = - std::unordered_map(); + std::unordered_map>(); disengaged_gpios_.set_after_states[params_.disengaged.name] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.disengaged.set_after_command.interfaces, params_.disengaged.set_after_command.values, - "disengaged.set_after_command", disengaged_gpios_.set_after_commands[params_.disengaged.name], command_if_ios, - disengaged_gpios_.set_after_commands_start_index[params_.disengaged.name]); + "disengaged.set_after_command", disengaged_gpios_.set_after_commands[params_.disengaged.name], command_if_ios_); parse_interfaces_from_params( params_.disengaged.set_after_state.interfaces, params_.disengaged.set_after_state.values, - "disengaged.set_after_state", disengaged_gpios_.set_after_states[params_.disengaged.name], state_if_ios, - disengaged_gpios_.set_after_states_start_index[params_.disengaged.name]); + "disengaged.set_after_state", disengaged_gpios_.set_after_states[params_.disengaged.name], state_if_ios_); // Engaged IOs engaged_gpios_.set_before_commands[params_.engaged.name] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.engaged.set_before_command.interfaces, params_.engaged.set_before_command.values, - "engaged.set_before_command", engaged_gpios_.set_before_commands[params_.engaged.name], command_if_ios, engaged_gpios_.set_before_commands_start_index[params_.engaged.name]); + "engaged.set_before_command", engaged_gpios_.set_before_commands[params_.engaged.name], command_if_ios_); engaged_gpios_.set_before_states[params_.engaged.name] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.engaged.set_before_state.interfaces, params_.engaged.set_before_state.values, - "engaged.set_before_state", engaged_gpios_.set_before_states[params_.engaged.name], state_if_ios, engaged_gpios_.set_before_states_start_index[params_.engaged.name]); - engaged_gpios_.commands[params_.engaged.name] = std::unordered_map(); + "engaged.set_before_state", engaged_gpios_.set_before_states[params_.engaged.name], state_if_ios_); + engaged_gpios_.commands[params_.engaged.name] = std::unordered_map>(); parse_interfaces_from_params( params_.engaged.command.interfaces, params_.engaged.command.values, "engaged.command", - engaged_gpios_.commands[params_.engaged.name], command_if_ios, engaged_gpios_.commands_start_index[params_.engaged.name]); + engaged_gpios_.commands[params_.engaged.name], command_if_ios_); engaged_gpios_.possible_states = params_.possible_engaged_states; for (const auto & state : params_.possible_engaged_states) { - engaged_gpios_.states[state] = std::unordered_map(); + engaged_gpios_.states[state] = std::unordered_map>(); parse_interfaces_from_params( params_.engaged.states.possible_engaged_states_map.at(state).interfaces, params_.engaged.states.possible_engaged_states_map.at(state).values, "engaged.states." + state, - engaged_gpios_.states[state], state_if_ios, engaged_gpios_.states_start_index[state]); + engaged_gpios_.states[state], state_if_ios_); engaged_gpios_.states_joint_states[state] = params_.engaged.states.possible_engaged_states_map.at(state).joint_states; engaged_gpios_.set_after_commands[state] = - std::unordered_map(); + std::unordered_map>(); engaged_gpios_.set_after_states[state] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.engaged.states.possible_engaged_states_map.at(state).set_after_command_interfaces, params_.engaged.states.possible_engaged_states_map.at(state).set_after_command_values, - "engaged.set_after_command." + state, engaged_gpios_.set_after_commands[state], command_if_ios, - engaged_gpios_.set_after_commands_start_index[state]); + "engaged.set_after_command." + state, engaged_gpios_.set_after_commands[state], command_if_ios_); parse_interfaces_from_params( params_.engaged.states.possible_engaged_states_map.at(state).set_after_state_interfaces, params_.engaged.states.possible_engaged_states_map.at(state).set_after_state_values, - "engaged.set_after_state." + state, engaged_gpios_.set_after_states[state], state_if_ios, - engaged_gpios_.set_after_states_start_index[state]); + "engaged.set_after_state." + state, engaged_gpios_.set_after_states[state], state_if_ios_); } // Configurations IOs @@ -616,60 +648,125 @@ void GpioToolController::prepare_command_and_state_ios() for (const auto & state : reconfigure_gpios_.possible_states) { reconfigure_gpios_.set_before_commands[state] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.configuration_setup.configurations_map.at(state).set_before_command_interfaces, params_.configuration_setup.configurations_map.at(state).set_before_command_values, "reconfigure.set_before_command." + state, reconfigure_gpios_.set_before_commands[state], - command_if_ios, reconfigure_gpios_.set_before_commands_start_index[state]); + command_if_ios_); reconfigure_gpios_.set_before_states[state] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.configuration_setup.configurations_map.at(state).set_before_state_interfaces, params_.configuration_setup.configurations_map.at(state).set_before_state_values, - "reconfigure.set_before_state." + state, reconfigure_gpios_.set_before_states[state], state_if_ios, - reconfigure_gpios_.set_before_states_start_index[state]); + "reconfigure.set_before_state." + state, reconfigure_gpios_.set_before_states[state], state_if_ios_); reconfigure_gpios_.commands[state] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.configuration_setup.configurations_map.at(state).command_interfaces, params_.configuration_setup.configurations_map.at(state).command_values, - "reconfigure.command." + state, reconfigure_gpios_.commands[state], command_if_ios, - reconfigure_gpios_.commands_start_index[state]); + "reconfigure.command." + state, reconfigure_gpios_.commands[state], command_if_ios_); reconfigure_gpios_.states[state] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.configuration_setup.configurations_map.at(state).state_interfaces, params_.configuration_setup.configurations_map.at(state).state_values, - "reconfigure.state." + state, reconfigure_gpios_.states[state], state_if_ios, - reconfigure_gpios_.states_start_index[state]); + "reconfigure.state." + state, reconfigure_gpios_.states[state], state_if_ios_); reconfigure_gpios_.states_joint_states[state] = params_.configuration_setup.configurations_map.at(state).joint_states; reconfigure_gpios_.set_after_commands[state] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.configuration_setup.configurations_map.at(state).set_after_command_interfaces, params_.configuration_setup.configurations_map.at(state).set_after_command_values, "reconfigure.set_after_command." + state, reconfigure_gpios_.set_after_commands[state], - command_if_ios, reconfigure_gpios_.set_after_commands_start_index[state]); + command_if_ios_); reconfigure_gpios_.set_after_states[state] = - std::unordered_map(); + std::unordered_map>(); parse_interfaces_from_params( params_.configuration_setup.configurations_map.at(state).set_after_state_interfaces, params_.configuration_setup.configurations_map.at(state).set_after_state_values, - "reconfigure.set_after_state." + state, reconfigure_gpios_.set_after_states[state], state_if_ios, - reconfigure_gpios_.set_after_states_start_index[state]); + "reconfigure.set_after_state." + state, reconfigure_gpios_.set_after_states[state], state_if_ios_); } for (const auto & [name, data] : params_.sensors_interfaces.tool_specific_sensors_map) { - state_if_ios.insert(data.interface); + state_if_ios_.insert(data.interface); } + + auto store_indices_of_interfaces = [&](std::unordered_map> & ios_map, const std::unordered_set & interfaces) -> bool { + for (auto & [itf_name, pair] : ios_map) + { + auto & [value, index] = pair; + auto it = std::find(interfaces.begin(), interfaces.end(), itf_name); + if (it != interfaces.end()) + { + index = std::distance(interfaces.begin(), it); + } + else + { + RCLCPP_ERROR(get_node()->get_logger(), "Interface '%s' not found in the list of interfaces.", itf_name.c_str()); + return false; + } + } + return true; + }; + + bool ret = true; + + // Disengaged IOs + ret &= store_indices_of_interfaces( + disengaged_gpios_.set_before_commands[params_.disengaged.name], command_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.set_before_states[params_.disengaged.name], state_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.commands[params_.disengaged.name], command_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.states[params_.disengaged.name], state_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.set_after_commands[params_.disengaged.name], command_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.set_after_states[params_.disengaged.name], state_if_ios_); + + // Engaged IOs + ret &= store_indices_of_interfaces( + engaged_gpios_.set_before_commands[params_.engaged.name], command_if_ios_); + ret &= store_indices_of_interfaces( + engaged_gpios_.set_before_states[params_.engaged.name], state_if_ios_); + ret &= store_indices_of_interfaces( + engaged_gpios_.commands[params_.engaged.name], command_if_ios_); + for (const auto & state : params_.possible_engaged_states) + { + ret &= store_indices_of_interfaces(engaged_gpios_.states[state], state_if_ios_); + ret &= store_indices_of_interfaces( + engaged_gpios_.set_after_commands[state], command_if_ios_); + ret &= store_indices_of_interfaces( + engaged_gpios_.set_after_states[state], state_if_ios_); + } + + // Reconfigure IOs + for (const auto & state : reconfigure_gpios_.possible_states) + { + ret &= store_indices_of_interfaces( + reconfigure_gpios_.set_before_commands[state], command_if_ios_); + ret &= store_indices_of_interfaces( + reconfigure_gpios_.set_before_states[state], state_if_ios_); + ret &= store_indices_of_interfaces( + reconfigure_gpios_.commands[state], command_if_ios_); + ret &= store_indices_of_interfaces( + reconfigure_gpios_.states[state], state_if_ios_); + ret &= store_indices_of_interfaces( + reconfigure_gpios_.set_after_commands[state], command_if_ios_); + ret &= store_indices_of_interfaces( + reconfigure_gpios_.set_after_states[state], state_if_ios_); + } + + return ret; } GpioToolController::EngagingSrvType::Response GpioToolController::process_engaging_request( @@ -732,12 +829,18 @@ GpioToolController::EngagingSrvType::Response GpioToolController::process_reconf + params_.engaged.name + "' or '" + params_.disengaged.name + "' action.Please wait until it finishes."; } + // This is OK to access `current_state_` as we are in the IDLE state and it is not being modified + if (response.success && current_state_ != params_.disengaged.name) + { + response.success = false; + response.message = "Tool can be reconfigured only in '" + params_.disengaged.name + "' state. Current state is '" + current_state_ + "'."; + } if (response.success) { current_tool_action_.store(ToolAction::RECONFIGURING); current_tool_transition_.store(GPIOToolTransition::SET_BEFORE_COMMAND); target_configuration_.store(std::make_shared(config_name)); - response.message = "Tool reconfigfuration to '" + config_name + "' has started."; + response.message = "Tool reconfiguration to '" + config_name + "' has started."; RCLCPP_INFO(get_node()->get_logger(), "%s", response.message.c_str()); } else @@ -886,9 +989,19 @@ controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_ e.what()); return controller_interface::CallbackReturn::ERROR; } - const size_t nr_joints = params_.engaged_joints.size() + params_.configuration_joints.size(); - tool_joint_state_publisher_->msg_.name.reserve(nr_joints); - tool_joint_state_publisher_->msg_.position.resize(nr_joints, std::numeric_limits::quiet_NaN()); + + if (joint_states_values_.size() == 0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Joint states values are empty, resizing to match the number of engaged and configuration joints. This should not happen here, as the joint states should be set in the configure method."); + joint_states_values_.resize( + params_.engaged_joints.size() + params_.configuration_joints.size(), + std::numeric_limits::quiet_NaN()); + } + + tool_joint_state_publisher_->msg_.name.reserve(joint_states_values_.size()); + tool_joint_state_publisher_->msg_.position = joint_states_values_; for (const auto & joint_name : params_.engaged_joints) { @@ -899,13 +1012,17 @@ controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_ tool_joint_state_publisher_->msg_.name.push_back(joint_name); } - interface_publisher_->msg_.states.interface_names.resize(state_interfaces_.size()); - interface_publisher_->msg_.states.values.resize(state_interfaces_.size()); - interface_publisher_->msg_.commands.interface_names.resize(command_interfaces_.size()); - interface_publisher_->msg_.commands.values.resize(command_interfaces_.size()); - for (size_t i = 0; i < state_interfaces_.size(); ++i) + interface_publisher_->msg_.states.interface_names.reserve(state_if_ios_.size()); + interface_publisher_->msg_.states.values.resize(state_if_ios_.size()); + interface_publisher_->msg_.commands.interface_names.reserve(command_if_ios_.size()); + interface_publisher_->msg_.commands.values.resize(command_if_ios_.size()); + for (const auto & state_io : state_if_ios_) { - interface_publisher_->msg_.states.interface_names[i] = state_interfaces_[i].get_name(); + interface_publisher_->msg_.states.interface_names.push_back(state_io); + } + for (const auto & command_io : command_if_ios_) + { + interface_publisher_->msg_.commands.interface_names.push_back(command_io); } controller_state_publisher_->msg_.state = current_state_; @@ -930,18 +1047,17 @@ void GpioToolController::publish_topics(const rclcpp::Time & time) for (size_t i = 0; i < state_interfaces_.size(); ++i) { interface_publisher_->msg_.states.values.at(i) = - static_cast(state_interfaces_.at(i).get_optional().value_or( - std::numeric_limits::quiet_NaN())); - } - for (size_t i = 0; i < command_interfaces_.size(); ++i) - { - interface_publisher_->msg_.commands.values.at(i) = - static_cast(command_interfaces_.at(i).get_optional().value_or( - std::numeric_limits::quiet_NaN())); - } + static_cast(state_interfaces_.at(i).get_optional().value_or( + std::numeric_limits::quiet_NaN())); + } + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + interface_publisher_->msg_.commands.values.at(i) = + static_cast(command_interfaces_.at(i).get_optional().value_or( + std::numeric_limits::quiet_NaN())); + } interface_publisher_->unlockAndPublish(); } - if (controller_state_publisher_ && controller_state_publisher_->trylock()) { controller_state_publisher_->msg_.state = current_state_; @@ -1045,30 +1161,30 @@ void GpioToolController::handle_action_accepted( } } -void GpioToolController::check_tool_state(const rclcpp::Time & current_time) +void GpioToolController::check_tool_state(const rclcpp::Time & current_time, const bool warning_output) { current_state_ = ""; - check_tool_state_and_switch(current_time, disengaged_gpios_, joint_states_values_, 0, params_.disengaged.name + " - CHECK STATES", GPIOToolTransition::IDLE, current_state_); + check_tool_state_and_switch(current_time, disengaged_gpios_, joint_states_values_, 0, params_.disengaged.name + " - CHECK STATES", GPIOToolTransition::IDLE, current_state_, warning_output); if (current_state_.empty()) { - check_tool_state_and_switch(current_time, engaged_gpios_, joint_states_values_, 0, params_.engaged.name + " - CHECK STATES", GPIOToolTransition::IDLE, current_state_); + check_tool_state_and_switch(current_time, engaged_gpios_, joint_states_values_, 0, params_.engaged.name + " - CHECK STATES", GPIOToolTransition::IDLE, current_state_, warning_output); } if (current_state_.empty()) { RCLCPP_ERROR( get_node()->get_logger(), - "Tool state can not be determined, triggering CANCELING action sand HALTED transition."); + "Tool state can not be determined, triggering CANCELING action and HALTED transition."); current_tool_action_.store(ToolAction::CANCELING); } current_configuration_ = ""; - check_tool_state_and_switch(current_time, reconfigure_gpios_, joint_states_values_, params_.engaged_joints.size(), "reconfigure - CHECK STATES", GPIOToolTransition::IDLE, current_configuration_); + check_tool_state_and_switch(current_time, reconfigure_gpios_, joint_states_values_, params_.engaged_joints.size(), "reconfigure - CHECK STATES", GPIOToolTransition::IDLE, current_configuration_, warning_output); if (current_configuration_.empty()) { RCLCPP_ERROR( get_node()->get_logger(), "Tool configuration can not be determined, triggering CANCELING action and HALTED transition."); - current_tool_action_.store(ToolAction::CANCELING); + current_tool_action_.store(ToolAction::CANCELING); } } diff --git a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml index 141445e310..714029c705 100644 --- a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml @@ -3,7 +3,7 @@ test_io_gripper_controller: use_action: true timeout: 5.0 tolerance: 0.05 - engaged_joints: [gripper_clamp_jaw] + engaged_joints: [lift_joint] disengaged: name: 'low' diff --git a/gpio_controllers/test/test_load_gpio_tool_controller.cpp b/gpio_controllers/test/test_load_gpio_tool_controller.cpp new file mode 100644 index 0000000000..25b3b57b79 --- /dev/null +++ b/gpio_controllers/test/test_load_gpio_tool_controller.cpp @@ -0,0 +1,37 @@ +// Copyright 2025 b»robotized Group +// +// 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 "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGpioCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NO_THROW( + cm.load_controller("test_gpio_tool_controller", "gpio_controllers/GpioToolController")); + + rclcpp::shutdown(); +} From a3c9553101aa9abc7751e99936080a7ade548bd5 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Sat, 16 Aug 2025 21:11:03 +0200 Subject: [PATCH 69/75] print warnings about states also on activation. --- gpio_controllers/src/gpio_tool_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp index b5d4a43426..b3981f7d54 100644 --- a/gpio_controllers/src/gpio_tool_controller.cpp +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -259,7 +259,7 @@ controller_interface::CallbackReturn GpioToolController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { state_change_start_ = get_node()->now(); - check_tool_state(state_change_start_); + check_tool_state(state_change_start_, true); // TODO(denis): update ros2_control and then enable this in this version the interfaces are not released when controller fails to activate // if (current_state_.empty() || current_configuration_.empty()) // { From 2575a90f13c160eebfa4ec69372c2a25690a65ca Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 1 Sep 2025 16:32:24 +0200 Subject: [PATCH 70/75] Using the name of the final state of configuration when using action. --- .../include/gpio_controllers/gpio_tool_controller.hpp | 11 ++--------- gpio_controllers/src/gpio_tool_controller.cpp | 8 ++------ 2 files changed, 4 insertions(+), 15 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp index 31c28ee0f0..d6cb4a33e5 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp @@ -155,8 +155,8 @@ class GpioToolController : public controller_interface::ControllerInterface std::unordered_set command_if_ios_; std::unordered_set state_if_ios_; - using EngagingSrvType = example_interfaces::srv::Trigger; - using ResetSrvType = example_interfaces::srv::Trigger; + using EngagingSrvType = std_srvs::srv::Trigger; + using ResetSrvType = std_srvs::srv::Trigger; using ConfigSrvType = control_msgs::srv::SetGPIOToolConfig; using DynInterfaceMsg = control_msgs::msg::DynamicInterfaceValues; using EngagingActionType = control_msgs::action::GPIOToolCommand; @@ -273,13 +273,6 @@ class GpioToolController : public controller_interface::ControllerInterface rclcpp_action::CancelResponse handle_engaging_cancel( std::shared_ptr> goal_handle); - /** - * @brief Handles the accepted goal for the tool action. - * @param goal_handle The handle of the accepted goal. - */ - void handle_engaging_accepted( - std::shared_ptr> goal_handle); - /** * @brief Handles the goal for the gripper configuration action. * @param uuid The UUID of the goal. diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp index b3981f7d54..3f0ffc2556 100644 --- a/gpio_controllers/src/gpio_tool_controller.cpp +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -1099,12 +1099,6 @@ rclcpp_action::CancelResponse GpioToolController::handle_engaging_cancel( return rclcpp_action::CancelResponse::ACCEPT; } -void GpioToolController::handle_engaging_accepted( - std::shared_ptr> goal_handle) -{ - handle_action_accepted(goal_handle); -} - rclcpp_action::GoalResponse GpioToolController::handle_config_goal( const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr goal) { @@ -1140,6 +1134,7 @@ void GpioToolController::handle_action_accepted( if (current_tool_action_.load() == ToolAction::IDLE) { result->success = true; + result->resulting_state_name = current_state_; result->message = "Tool action successfully executed!"; goal_handle->succeed(result); break; @@ -1147,6 +1142,7 @@ void GpioToolController::handle_action_accepted( else if (current_tool_transition_.load() == GPIOToolTransition::HALTED) { result->success = false; + result->resulting_state_name = current_state_; result->message = "Tool action canceled or halted! Check the error, reset the tool using '~/reset_halted' service and set to sensible state."; goal_handle->abort(result); break; From 55d06b77ce0c1fc7c6dc297c594e2b6637b551d1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 2 Sep 2025 11:06:59 +0200 Subject: [PATCH 71/75] Removed colcone ignore files. --- ackermann_steering_controller/COLCON_IGNORE | 0 admittance_controller/COLCON_IGNORE | 0 bicycle_steering_controller/COLCON_IGNORE | 0 diff_drive_controller/COLCON_IGNORE | 0 effort_controllers/COLCON_IGNORE | 0 force_torque_sensor_broadcaster/COLCON_IGNORE | 0 forward_command_controller/COLCON_IGNORE | 0 gripper_controllers/COLCON_IGNORE | 0 imu_sensor_broadcaster/COLCON_IGNORE | 0 io_gripper_controller/COLCON_IGNORE | 0 joint_state_broadcaster/COLCON_IGNORE | 0 joint_trajectory_controller/COLCON_IGNORE | 0 mecanum_drive_controller/COLCON_IGNORE | 0 parallel_gripper_controller/COLCON_IGNORE | 0 pid_controller/COLCON_IGNORE | 0 pose_broadcaster/COLCON_IGNORE | 0 position_controllers/COLCON_IGNORE | 0 range_sensor_broadcaster/COLCON_IGNORE | 0 ros2_controllers/COLCON_IGNORE | 0 ros2_controllers_test_nodes/COLCON_IGNORE | 0 rqt_joint_trajectory_controller/COLCON_IGNORE | 0 steering_controllers_library/COLCON_IGNORE | 0 tricycle_controller/COLCON_IGNORE | 0 tricycle_steering_controller/COLCON_IGNORE | 0 velocity_controllers/COLCON_IGNORE | 0 25 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 ackermann_steering_controller/COLCON_IGNORE delete mode 100644 admittance_controller/COLCON_IGNORE delete mode 100644 bicycle_steering_controller/COLCON_IGNORE delete mode 100644 diff_drive_controller/COLCON_IGNORE delete mode 100644 effort_controllers/COLCON_IGNORE delete mode 100644 force_torque_sensor_broadcaster/COLCON_IGNORE delete mode 100644 forward_command_controller/COLCON_IGNORE delete mode 100644 gripper_controllers/COLCON_IGNORE delete mode 100644 imu_sensor_broadcaster/COLCON_IGNORE delete mode 100644 io_gripper_controller/COLCON_IGNORE delete mode 100644 joint_state_broadcaster/COLCON_IGNORE delete mode 100644 joint_trajectory_controller/COLCON_IGNORE delete mode 100644 mecanum_drive_controller/COLCON_IGNORE delete mode 100644 parallel_gripper_controller/COLCON_IGNORE delete mode 100644 pid_controller/COLCON_IGNORE delete mode 100644 pose_broadcaster/COLCON_IGNORE delete mode 100644 position_controllers/COLCON_IGNORE delete mode 100644 range_sensor_broadcaster/COLCON_IGNORE delete mode 100644 ros2_controllers/COLCON_IGNORE delete mode 100644 ros2_controllers_test_nodes/COLCON_IGNORE delete mode 100644 rqt_joint_trajectory_controller/COLCON_IGNORE delete mode 100644 steering_controllers_library/COLCON_IGNORE delete mode 100644 tricycle_controller/COLCON_IGNORE delete mode 100644 tricycle_steering_controller/COLCON_IGNORE delete mode 100644 velocity_controllers/COLCON_IGNORE diff --git a/ackermann_steering_controller/COLCON_IGNORE b/ackermann_steering_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/admittance_controller/COLCON_IGNORE b/admittance_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/bicycle_steering_controller/COLCON_IGNORE b/bicycle_steering_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/diff_drive_controller/COLCON_IGNORE b/diff_drive_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/effort_controllers/COLCON_IGNORE b/effort_controllers/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/force_torque_sensor_broadcaster/COLCON_IGNORE b/force_torque_sensor_broadcaster/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/forward_command_controller/COLCON_IGNORE b/forward_command_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/gripper_controllers/COLCON_IGNORE b/gripper_controllers/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/imu_sensor_broadcaster/COLCON_IGNORE b/imu_sensor_broadcaster/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/io_gripper_controller/COLCON_IGNORE b/io_gripper_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/joint_state_broadcaster/COLCON_IGNORE b/joint_state_broadcaster/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/joint_trajectory_controller/COLCON_IGNORE b/joint_trajectory_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/mecanum_drive_controller/COLCON_IGNORE b/mecanum_drive_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/parallel_gripper_controller/COLCON_IGNORE b/parallel_gripper_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/pid_controller/COLCON_IGNORE b/pid_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/pose_broadcaster/COLCON_IGNORE b/pose_broadcaster/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/position_controllers/COLCON_IGNORE b/position_controllers/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/range_sensor_broadcaster/COLCON_IGNORE b/range_sensor_broadcaster/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/ros2_controllers/COLCON_IGNORE b/ros2_controllers/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/ros2_controllers_test_nodes/COLCON_IGNORE b/ros2_controllers_test_nodes/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/rqt_joint_trajectory_controller/COLCON_IGNORE b/rqt_joint_trajectory_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/steering_controllers_library/COLCON_IGNORE b/steering_controllers_library/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/tricycle_controller/COLCON_IGNORE b/tricycle_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/tricycle_steering_controller/COLCON_IGNORE b/tricycle_steering_controller/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/velocity_controllers/COLCON_IGNORE b/velocity_controllers/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 From 93bd467189eb58c627e9d1d7b79108758a848869 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Tue, 2 Sep 2025 14:57:17 +0200 Subject: [PATCH 72/75] Improve output messages and add automatic state chnage by external influence. --- gpio_controllers/src/gpio_tool_controller.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp index 3f0ffc2556..4df5a2a722 100644 --- a/gpio_controllers/src/gpio_tool_controller.cpp +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -286,6 +286,8 @@ controller_interface::return_type GpioToolController::update( case ToolAction::IDLE: { // do nothing + state_change_start_ = time; + check_tool_state(time, false); break; } case ToolAction::DISENGAGING: @@ -413,7 +415,7 @@ bool GpioToolController::check_states( { RCLCPP_ERROR( get_node()->get_logger(), - "%s: Tool didin't reached target state within %.2f seconds.", + "%s: Tool didin't reached target state within %.2f seconds. Try resetting the tool using '~/reset_halted' service. After that set sensible state.", output_prefix.c_str(), params_.timeout); current_tool_transition_.store(GPIOToolTransition::HALTED); } @@ -428,11 +430,14 @@ void GpioToolController::check_tool_state_and_switch( { bool state_exists = states.size() > 0; RCLCPP_DEBUG( - get_node()->get_logger(), "%s - CHECK_STATE: Checking state '%s' for tool. States _%s_ exist. Used number of state interfaces %zu", + get_node()->get_logger(), "%s: Checking state '%s' for tool. States _%s_ exist. Used number of state interfaces %zu.", output_prefix.c_str(), state_name.c_str(), (state_exists ? "do" : "do not"), states.size()); if (check_states(current_time, states, output_prefix, next_transition, warning_output)) { + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: SUCCESS! state '%s' for tool is confirmed!", + output_prefix.c_str(), state_name.c_str()); found_state_name = state_name; const auto & js_val = ios.states_joint_states.at(state_name); if (joint_states_start_index + js_val.size() <= joint_states.size()) From 6f4c31532e007a9d2a82138e18f416d06d2b56f2 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 3 Sep 2025 11:27:25 +0200 Subject: [PATCH 73/75] Fix the time substraciton issues. --- gpio_controllers/src/gpio_tool_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp index 4df5a2a722..393522346f 100644 --- a/gpio_controllers/src/gpio_tool_controller.cpp +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -287,7 +287,7 @@ controller_interface::return_type GpioToolController::update( { // do nothing state_change_start_ = time; - check_tool_state(time, false); + check_tool_state(state_change_start_, false); break; } case ToolAction::DISENGAGING: From 7c71238dc8dc33eeb6c48c69a1cbc9c588e0a615 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 3 Sep 2025 14:08:20 +0200 Subject: [PATCH 74/75] Fix the time substraciton issues. --- gpio_controllers/src/gpio_tool_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp index 393522346f..331f48ca26 100644 --- a/gpio_controllers/src/gpio_tool_controller.cpp +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -287,7 +287,7 @@ controller_interface::return_type GpioToolController::update( { // do nothing state_change_start_ = time; - check_tool_state(state_change_start_, false); + check_tool_state(time, false); break; } case ToolAction::DISENGAGING: @@ -467,6 +467,8 @@ void GpioToolController::handle_tool_state_transition( switch (current_tool_transition_.load()) { case GPIOToolTransition::IDLE: + // reset time to avoid any time-source related crashing + state_change_start_ = current_time; // do nothing break; From b32be1088fd1faf92de1f7d23a4072193a72c8e7 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 5 Sep 2025 08:10:24 +0200 Subject: [PATCH 75/75] Ignore configuration checks if none defined and distable js publisher is not required. --- .../gpio_controllers/gpio_tool_controller.hpp | 3 + gpio_controllers/src/gpio_tool_controller.cpp | 169 ++++++++++++------ 2 files changed, 115 insertions(+), 57 deletions(-) diff --git a/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp index d6cb4a33e5..7c7a1d675d 100644 --- a/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp +++ b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp @@ -200,6 +200,9 @@ class GpioToolController : public controller_interface::ControllerInterface const std::string & requested_action_name); private: + bool configuration_control_enabled_ = true; + bool joint_states_need_publishing_ = true; + /** * @brief Handles the state transition when enaging the tool. * @param state The current transition of the tool. diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp index 331f48ca26..6c14b4924f 100644 --- a/gpio_controllers/src/gpio_tool_controller.cpp +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -57,6 +57,7 @@ controller_interface::CallbackReturn GpioToolController::on_init() current_tool_transition_.store(GPIOToolTransition::IDLE); target_configuration_.store(std::make_shared("")); current_state_ = ""; + current_configuration_ = ""; try { @@ -77,7 +78,18 @@ controller_interface::CallbackReturn GpioToolController::on_configure( params_ = param_listener_->get_params(); bool all_good = true; - // TODO(destogl): this can be resolved with parameter pairs too! We can remove this method. + if (params_.engaged_joints.empty() && params_.configuration_joints.empty()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "No joints defined therefore no joint states publisher is created."); + joint_states_need_publishing_ = false; + } + else + { + joint_states_need_publishing_ = true; + } + auto check_joint_states_sizes = [this](const size_t joint_states_size, const size_t joint_states_values_size, const std::string & param_name) { if (joint_states_size != joint_states_values_size) { @@ -121,6 +133,7 @@ controller_interface::CallbackReturn GpioToolController::on_configure( params_.engaged_joints.size() + params_.configuration_joints.size(), std::numeric_limits::quiet_NaN()); + // check sizes of all other parameters auto check_parameter_pairs = [this](const std::vector & interfaces, const std::vector & values, const std::string & parameter_name) { if (interfaces.size() != values.size()) { RCLCPP_FATAL( @@ -158,13 +171,25 @@ controller_interface::CallbackReturn GpioToolController::on_configure( } } + if (params_.configurations.empty()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "No configurations defined. Configuration control will be disabled."); + configuration_control_enabled_ = false; + } + else + { + configuration_control_enabled_ = true; + } + if (params_.configurations.size() != params_.configuration_setup.configurations_map.size()) { RCLCPP_FATAL( get_node()->get_logger(), "Size of 'configurations' and 'configuration_setup' parameters must be equal. " "Configurations size: %zu, configuration joints size: %zu.", - params_.configurations.size(), params_.configuration_joints.size()); + params_.configurations.size(), params_.configuration_setup.configurations_map.size()); all_good = false; } for (const auto & [name, info] : params_.configuration_setup.configurations_map) @@ -888,8 +913,11 @@ controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_ engaging_service_callback_group_ = get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - reconfigure_service_callback_group_ = - get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + if (configuration_control_enabled_) + { + reconfigure_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + } auto disengaged_service_callback = [&]( const std::shared_ptr /*request*/, @@ -923,25 +951,28 @@ controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_ engaged_service_ = get_node()->create_service( "~/" + params_.engaged.name, engaged_service_callback, qos_services, engaging_service_callback_group_); - // configure tool service - auto reconfigure_tool_service_callback = - [&]( - const std::shared_ptr request, - std::shared_ptr response) + if (configuration_control_enabled_) { - auto result = process_reconfigure_request(request->config_name); - - if (result.success) + // configure tool service + auto reconfigure_tool_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) { - result = service_wait_for_transition_end(request->config_name); - - } - response->success = result.success; - response->message = result.message; - }; - reconfigure_tool_service_ = get_node()->create_service( - "~/reconfigure", reconfigure_tool_service_callback, qos_services, - reconfigure_service_callback_group_); + auto result = process_reconfigure_request(request->config_name); + + if (result.success) + { + result = service_wait_for_transition_end(request->config_name); + + } + response->success = result.success; + response->message = result.message; + }; + reconfigure_tool_service_ = get_node()->create_service( + "~/reconfigure", reconfigure_tool_service_callback, qos_services, + reconfigure_service_callback_group_); + } } else { @@ -953,14 +984,17 @@ controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_ std::bind(&GpioToolController::handle_engaging_cancel, this, std::placeholders::_1), std::bind(&GpioToolController::handle_action_accepted, this, std::placeholders::_1)); - // reconfigure action server - config_action_server_ = rclcpp_action::create_server( - get_node(), "~/reconfigure", - std::bind( - &GpioToolController::handle_config_goal, this, std::placeholders::_1, - std::placeholders::_2), - std::bind(&GpioToolController::handle_config_cancel, this, std::placeholders::_1), - std::bind(&GpioToolController::handle_action_accepted, this, std::placeholders::_1)); + if (configuration_control_enabled_) + { + // reconfigure action server + config_action_server_ = rclcpp_action::create_server( + get_node(), "~/reconfigure", + std::bind( + &GpioToolController::handle_config_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&GpioToolController::handle_config_cancel, this, std::placeholders::_1), + std::bind(&GpioToolController::handle_action_accepted, this, std::placeholders::_1)); + } } reset_service_callback_group_ = @@ -977,9 +1011,12 @@ controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_ try { - t_js_publisher_ = - get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); - tool_joint_state_publisher_ = std::make_unique(t_js_publisher_); + if (joint_states_need_publishing_) + { + t_js_publisher_ = + get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); + tool_joint_state_publisher_ = std::make_unique(t_js_publisher_); + } if_publisher_ = get_node()->create_publisher( "~/dynamic_interfaces", rclcpp::SystemDefaultsQoS()); @@ -997,26 +1034,38 @@ controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_ return controller_interface::CallbackReturn::ERROR; } - if (joint_states_values_.size() == 0) + if (!params_.engaged.name.empty() || !params_.disengaged.name.empty()) { - RCLCPP_WARN( + RCLCPP_INFO( get_node()->get_logger(), - "Joint states values are empty, resizing to match the number of engaged and configuration joints. This should not happen here, as the joint states should be set in the configure method."); - joint_states_values_.resize( - params_.engaged_joints.size() + params_.configuration_joints.size(), - std::numeric_limits::quiet_NaN()); + "No joints defined, so no joint states will be published, althrough the publisher is " + "initialized."); } - tool_joint_state_publisher_->msg_.name.reserve(joint_states_values_.size()); - tool_joint_state_publisher_->msg_.position = joint_states_values_; + // if (joint_states_values_.size() == 0) + // { + // RCLCPP_DEBUG( + // get_node()->get_logger(), + // "Joint states values are empty, resizing to match the number (%zu) of engaged and configuration joints. This should not happen here, as the joint states should be set in the configure method.", + // params_.engaged_joints.size() + params_.configuration_joints.size()); + // joint_states_values_.resize( + // params_.engaged_joints.size() + params_.configuration_joints.size(), + // std::numeric_limits::quiet_NaN()); + // } - for (const auto & joint_name : params_.engaged_joints) - { - tool_joint_state_publisher_->msg_.name.push_back(joint_name); - } - for (const auto & joint_name : params_.configuration_joints) + if (joint_states_need_publishing_) { - tool_joint_state_publisher_->msg_.name.push_back(joint_name); + tool_joint_state_publisher_->msg_.name.reserve(joint_states_values_.size()); + tool_joint_state_publisher_->msg_.position = joint_states_values_; + + for (const auto & joint_name : params_.engaged_joints) + { + tool_joint_state_publisher_->msg_.name.push_back(joint_name); + } + for (const auto & joint_name : params_.configuration_joints) + { + tool_joint_state_publisher_->msg_.name.push_back(joint_name); + } } interface_publisher_->msg_.states.interface_names.reserve(state_if_ios_.size()); @@ -1041,12 +1090,15 @@ controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_ void GpioToolController::publish_topics(const rclcpp::Time & time) { - if (tool_joint_state_publisher_ && tool_joint_state_publisher_->trylock()) + if (joint_states_need_publishing_) { - tool_joint_state_publisher_->msg_.header.stamp = time; - tool_joint_state_publisher_->msg_.position = joint_states_values_; + if (tool_joint_state_publisher_ && tool_joint_state_publisher_->trylock()) + { + tool_joint_state_publisher_->msg_.header.stamp = time; + tool_joint_state_publisher_->msg_.position = joint_states_values_; + } + tool_joint_state_publisher_->unlockAndPublish(); } - tool_joint_state_publisher_->unlockAndPublish(); if (interface_publisher_ && interface_publisher_->trylock()) { @@ -1180,14 +1232,17 @@ void GpioToolController::check_tool_state(const rclcpp::Time & current_time, con current_tool_action_.store(ToolAction::CANCELING); } - current_configuration_ = ""; - check_tool_state_and_switch(current_time, reconfigure_gpios_, joint_states_values_, params_.engaged_joints.size(), "reconfigure - CHECK STATES", GPIOToolTransition::IDLE, current_configuration_, warning_output); - if (current_configuration_.empty()) + if (configuration_control_enabled_) { - RCLCPP_ERROR( - get_node()->get_logger(), - "Tool configuration can not be determined, triggering CANCELING action and HALTED transition."); - current_tool_action_.store(ToolAction::CANCELING); + current_configuration_ = ""; + check_tool_state_and_switch(current_time, reconfigure_gpios_, joint_states_values_, params_.engaged_joints.size(), "reconfigure - CHECK STATES", GPIOToolTransition::IDLE, current_configuration_, warning_output); + if (current_configuration_.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tool configuration can not be determined, triggering CANCELING action and HALTED transition."); + current_tool_action_.store(ToolAction::CANCELING); + } } }