From 3f6a373bedb74781e056fd9e945e773df93905b6 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Sat, 21 Dec 2024 18:57:20 +0100 Subject: [PATCH 01/47] 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 353b42a4bb222e386375d33d415499fd08c6139c Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Mon, 23 Dec 2024 15:13:57 +0100 Subject: [PATCH 02/47] 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 2a5fd5e91e56183f024e216156a855bc994fade6 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Mon, 23 Dec 2024 15:21:39 +0100 Subject: [PATCH 03/47] 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 913ab888f7495640c94f88cbe46daf9702fdc5ce Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 2 Jan 2025 12:59:55 +0100 Subject: [PATCH 04/47] 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 e07988916a46f865f4730158cc7a400daa96aeef Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 2 Jan 2025 13:00:23 +0100 Subject: [PATCH 05/47] 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 46406094052b5616f4775c8aa543057faa72707d Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:06:06 +0100 Subject: [PATCH 06/47] - 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 0ebb49916c7decfee8067647ad7641b651cbd365 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:11:37 +0100 Subject: [PATCH 07/47] 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 b13c5074fe59b0be6bca1ea96bbd7ed7c208640d Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:12:16 +0100 Subject: [PATCH 08/47] 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 8571d6fcfda9b02203650f58b110c3529f3b9523 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:14:00 +0100 Subject: [PATCH 09/47] 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 69c47ce9811247d43d2d585b0fc19ec16f3cc2cc Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:21:06 +0100 Subject: [PATCH 10/47] 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 f357e97f9798d66f2b621589af1175eebf29f7d4 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:18:12 +0100 Subject: [PATCH 11/47] 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 8acbec59f476a473a9f74cf888f14cc24a88c51f Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:18:38 +0100 Subject: [PATCH 12/47] 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 099337cf9ede3c213e7d5391005fff1f743b254f Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:28:18 +0100 Subject: [PATCH 13/47] 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 d4facbad5b010eff5f8aa87123273d09c87c3135 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:30:31 +0100 Subject: [PATCH 14/47] 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 f7b487578ad82a49d8bca6cf5478883d06f37541 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:36:44 +0100 Subject: [PATCH 15/47] 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 dc5e828c26e226aaad18432b187a98d93729a66f Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:43:21 +0100 Subject: [PATCH 16/47] 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 94c9b811fc19e923c1e9493b6127eb20aa051a8d Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:51:09 +0100 Subject: [PATCH 17/47] 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 534a723cbf28365f26ca0d53bfe5433fc31f396c Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:52:29 +0100 Subject: [PATCH 18/47] 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 0c082843d48206f4c01abb2bf91d213c7c8f1b71 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 13:56:00 +0100 Subject: [PATCH 19/47] 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 cd0d6a5fc8a3ff3505687fdc82611e904c518b96 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 13:57:30 +0100 Subject: [PATCH 20/47] 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 485abbfecdbfc831cae2a8ea46384d50616153e7 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 14:03:19 +0100 Subject: [PATCH 21/47] 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 d7394e45669aa4c1cb0e052bfb2a6edf825429f8 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 22:47:01 +0100 Subject: [PATCH 22/47] 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 2bd2bd2255fef1201ed374065f8b38b49a6b3eaa Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Thu, 2 Jan 2025 22:48:32 +0100 Subject: [PATCH 23/47] 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 ac2fc9907e092eae6319d0f0dba06d6885abb789 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 23:06:04 +0100 Subject: [PATCH 24/47] - 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 545d28d3977ce24fdea82505255af16ec2a4c046 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Thu, 2 Jan 2025 23:10:39 +0100 Subject: [PATCH 25/47] 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 3943affdf7f60210720f1160ef1211326fbc9148 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 3 Jan 2025 02:02:42 +0100 Subject: [PATCH 26/47] 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 37874af43b5e254e89a33c8140277d02a96d57db Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 3 Jan 2025 02:21:47 +0100 Subject: [PATCH 27/47] 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 a20a98122876e917455f660087101b98e4d356e7 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:30:08 +0100 Subject: [PATCH 28/47] 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 9c625394021f2858a211dd17611cb22fe25dc8f9 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:30:30 +0100 Subject: [PATCH 29/47] 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 b0a6922e5c15dfaa1c759ea844103450a61e2f85 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:31:38 +0100 Subject: [PATCH 30/47] 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 e40f164ec2bd555f79840cbf20a0d8588a45e398 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:32:10 +0100 Subject: [PATCH 31/47] 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 3de5224cd7604a4d96362c67f0f3d9a069508129 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Fri, 10 Jan 2025 00:33:08 +0100 Subject: [PATCH 32/47] 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 7d6cd6f34ad9cc3ff110016197a05292670a8388 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 10 Jan 2025 00:46:45 +0100 Subject: [PATCH 33/47] 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 c1df617258a2f0453de018ad80c12890bcd58859 Mon Sep 17 00:00:00 2001 From: sachinkum0009 Date: Fri, 10 Jan 2025 00:47:14 +0100 Subject: [PATCH 34/47] 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 2551d41f1ed3af7a12fe48fe5fa8833d340eae62 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 17:28:59 +0100 Subject: [PATCH 35/47] 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 947da5916ac2c9894c1e5274683937a79647e2f5 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 17:29:37 +0100 Subject: [PATCH 36/47] 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 6034aebdd744756efb6d87420c913f5e33446e39 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 17:29:59 +0100 Subject: [PATCH 37/47] 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 2eb73a24f0d0122e2c5b1fec85c0c92f46270630 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 19:12:52 +0100 Subject: [PATCH 38/47] 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 e82f6a6e402ac70a99b71fbc22fe93c59d6134d5 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 19:20:36 +0100 Subject: [PATCH 39/47] 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 06829eee2eb716908417300fc9be5a5cbcb1c047 Mon Sep 17 00:00:00 2001 From: Sachin Kumar Date: Wed, 15 Jan 2025 19:21:07 +0100 Subject: [PATCH 40/47] 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 217a6d82e51a5bc5a3975daf209c0b55f80ee783 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 11:57:02 +0100 Subject: [PATCH 41/47] 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 2e42b895995f6bfee28741235bbd79c758145eaf Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 12:02:26 +0100 Subject: [PATCH 42/47] 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 032c7d938d4bc4394e05d96c12c5e5091a112715 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 13:50:07 +0100 Subject: [PATCH 43/47] 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 ce0ab6c1ca69abbf2b306d4acd0a44ea17bee562 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 14:18:13 +0100 Subject: [PATCH 44/47] 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 6b063182b49c570c724ad4da18e55b58954453f7 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Thu, 16 Jan 2025 16:09:56 +0100 Subject: [PATCH 45/47] 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 62577627919e7cdfdc67417ddef3dbf7a42dbaa8 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 17 Jan 2025 16:09:20 +0100 Subject: [PATCH 46/47] 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 ede058d5a78cf167f506c587f100036da6516208 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 20 Jan 2025 16:53:38 +0100 Subject: [PATCH 47/47] 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) {