diff --git a/io_gripper_controller/CMakeLists.txt b/io_gripper_controller/CMakeLists.txt new file mode 100644 index 0000000000..7446421bcd --- /dev/null +++ b/io_gripper_controller/CMakeLists.txt @@ -0,0 +1,106 @@ +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() + +# 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 + ament_cmake + ament_cmake_gmock + 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 +) + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(io_gripper_controller_parameters + src/io_gripper_controller.yaml +) +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..d2daf2148c --- /dev/null +++ b/io_gripper_controller/README.md @@ -0,0 +1,5 @@ +# io_gripper_controller package + +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..585d2c3754 --- /dev/null +++ b/io_gripper_controller/doc/userdoc.rst @@ -0,0 +1,102 @@ +: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 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 +--------------------------------------- + +- ``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. + + +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 +.................... + +.. 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: [Release_Break_valve] + low: [] + command: + high: [Open_valve] + low: [Close_valve] + state: + high: [Opened_signal] + low: [Closed_signal] + set_after_command: + high: [] + low: [Release_Break_valve] + + possible_closed_states: ['empty_close', 'full_close'] + + close: + set_before_command: + high: [Release_Break_valve] + low: [] + command: + high: [Close_valve] + low: [Open_valve] + state: + empty_close: + joint_states: [0.16] + high: [Closed_signal] + low: [Part_Grasped_signal] + set_after_command_high: [] + set_after_command_low: [Release_Break_valve] + full_close: + joint_states: [0.08] + high: [Part_Grasped_signal] + low: [Closed_signal] + set_after_command_high: [] + set_after_command_low: [Release_Break_valve] + + configurations: ["narrow_objects", "wide_objects"] + configuration_joints: [gripper_gripper_distance_joint] + + configuration_setup: + 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: + part_sensor_top: + input: "Part_Sensor_Top_signal" + part_sensor: + input: "Part_Grasped_signal" 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..a970d163ab --- /dev/null +++ b/io_gripper_controller/include/io_gripper_controller/io_gripper_controller.hpp @@ -0,0 +1,437 @@ +// Copyright (c) 2025, b>>robotized by Stogl Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ +#define GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "control_msgs/action/io_gripper_command.hpp" +#include "control_msgs/action/set_io_gripper_config.hpp" +#include "control_msgs/msg/dynamic_interface_values.hpp" +#include "control_msgs/msg/interface_value.hpp" +<<<<<<< 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" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace io_gripper_controller +{ +/** + * @enum service_mode_type + * @brief Represents the various service modes of the gripper. These modes represents the high level + * states of the gripper. + * + * - IDLE: The gripper is in an idle state, not performing any action. + * - OPEN: The gripper is in the process of opening. + * - CLOSE: The gripper is in the process of closing. + */ +enum class service_mode_type : std::uint8_t +{ + IDLE = 0, + OPEN = 1, + CLOSE = 2, +}; + +/** + * @enum gripper_state_type + * @brief Represents the various states of the gripper. + * + * - IDLE: The gripper is in an idle state, not performing any action. + * - SET_BEFORE_COMMAND: Executing commands for io defined in the yaml which are required before + * opening the gripper. + * - CLOSE_GRIPPER: Executing commands to close the gripper. + * - CHECK_GRIPPER_STATE: Checking the state of the gripper to make sure the gripper is closed. + * - OPEN_GRIPPER: Executing commands to open the gripper. + * - SET_AFTER_COMMAND: Setting the gripper state after executing a command. + * - HALTED: The gripper operation is halted. + */ +enum class gripper_state_type : std::uint8_t +{ + IDLE = 0, + SET_BEFORE_COMMAND = 1, + CLOSE_GRIPPER = 2, + CHECK_GRIPPER_STATE = 3, + OPEN_GRIPPER = 4, + SET_AFTER_COMMAND = 5, + HALTED = 6, +}; + +/** + * @enum reconfigure_state_type + * @brief Represents the various states of the reconfiguration process, which means that the gripper + * is reconfiguring to new state based on the configuration defined in the yaml params. + * + * - IDLE: The reconfiguration process is idle, not performing any action. + * - SET_COMMAND: Setting the command based on the configuration. + * - CHECK_STATE: Checking the state after setting the command. + */ +enum class reconfigure_state_type : std::uint8_t +{ + IDLE = 0, + SET_COMMAND = 1, + CHECK_STATE = 2, +}; + +class IOGripperController : public controller_interface::ControllerInterface +/** + * @brief IOGripperController class handles the control of an IO-based gripper. + */ +{ +public: + /** + * @brief Constructor for IOGripperController. + */ + IOGripperController(); + io_gripper_controller::Params params_; + + /** + * @brief Initializes the controller. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_init() override; + + /** + * @brief Configures the command interfaces. + * @return InterfaceConfiguration for command interfaces. + */ + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief Configures the state interfaces. + * @return InterfaceConfiguration for state interfaces. + */ + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + /** + * @brief Configures the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Activates the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Deactivates the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Updates the controller state. + * @param time The current time. + * @param period The time since the last update. + * @return return_type indicating success or failure. + */ + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +<<<<<<< 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; + std::vector set_after_command_open; + std::vector reconfigure_command; + std::vector command_ios_open_values; + std::vector command_ios_close_values; + std::vector set_before_command_open_values; + std::vector set_after_command_open_values; + std::vector reconfigure_command_values; + std::vector state_ios_open; + std::vector state_ios_close; + std::vector set_before_command_close; + std::vector set_after_command_close; + std::vector state_ios_open_values; + std::vector state_ios_close_values; + std::vector set_before_command_close_values; + std::vector set_after_command_close_values; + std::vector set_after_command_open_values_original_; + std::string status_joint_name; + bool is_open; + std::unordered_map command_if_ios_after_opening; + std::unordered_map original_ios_after_opening; + std::unordered_map command_if_ios_before_closing; + std::unordered_map original_ios_before_closing; + std::unordered_set command_if_ios; + std::unordered_set state_if_ios; + bool setResult; + + using ControllerModeSrvType = std_srvs::srv::SetBool; + using OpenCloseSrvType = std_srvs::srv::Trigger; + using ConfigSrvType = control_msgs::srv::SetIOGripperConfig; + using JointStateMsg = sensor_msgs::msg::JointState; + using DynInterfaceMsg = control_msgs::msg::DynamicInterfaceValues; + using GripperAction = control_msgs::action::IOGripperCommand; + using GoalHandleGripper = rclcpp_action::ServerGoalHandle; + using GripperConfigAction = control_msgs::action::SetIOGripperConfig; + using GoalHandleGripperConfig = rclcpp_action::ServerGoalHandle; + +protected: + std::shared_ptr param_listener_; + rclcpp::Service::SharedPtr open_service_; + rclcpp::Service::SharedPtr close_service_; + rclcpp::Service::SharedPtr configure_gripper_service_; + rclcpp_action::Server::SharedPtr gripper_action_server_; + rclcpp_action::Server::SharedPtr gripper_config_action_server_; + + // Realtime buffer to store the state for outer gripper_service (e.g. idle, open, close) + realtime_tools::RealtimeBuffer gripper_service_buffer_; + // Realtime buffer to store the state for switching the gripper state (e.g. idle, + // set_before_command, close_gripper, check_gripper_state, open_gripper, set_after_command, + // halted) + realtime_tools::RealtimeBuffer gripper_state_buffer_; +<<<<<<< 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, + // set_command, check_state) + realtime_tools::RealtimeBuffer reconfigure_state_buffer_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr g_j_s_publisher_; + std::unique_ptr gripper_joint_state_publisher_; + std::vector joint_state_values_; + using InterfacePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr if_publisher_; + std::unique_ptr interface_publisher_; +<<<<<<< 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}; + +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. + */ +<<<<<<< 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. + * @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_; + double state_value_; + std::string configuration_key_; + bool check_state_ios_; + std::string closed_state_name_; + io_gripper_controller::Params::Close::State::MapPossibleClosedStates closed_state_values_; + io_gripper_controller::Params::ConfigurationSetup::MapConfigurations conf_it_; + std::vector::iterator config_index_; + rclcpp::CallbackGroup::SharedPtr open_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr close_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr reconfigure_service_callback_group_; + std::shared_ptr gripper_feedback_; + std::shared_ptr gripper_result_; + std::shared_ptr gripper_config_feedback_; + std::shared_ptr gripper_config_result_; + + /** + * @brief Handles the goal for the gripper action. + * @param uuid The UUID of the goal. + * @param goal The goal to handle. + * @return GoalResponse indicating acceptance or rejection of the goal. + */ + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + /** + * @brief Handles the cancellation of the gripper action. + * @param goal_handle The handle of the goal to cancel. + * @return CancelResponse indicating acceptance or rejection of the cancellation. + */ + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + + /** + * @brief Handles the acceptance of the gripper action. + * @param goal_handle The handle of the accepted goal. + */ + void handle_accepted(const std::shared_ptr goal_handle); + + /** + * @brief Executes the gripper action. + * @param goal_handle The handle of the goal to execute. + */ + void execute(const std::shared_ptr goal_handle); + + /** + * @brief Handles the goal for the gripper configuration action. + * @param uuid The UUID of the goal. + * @param goal The goal to handle. + * @return GoalResponse indicating acceptance or rejection of the goal. + */ + rclcpp_action::GoalResponse config_handle_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + /** + * @brief Handles the cancellation of the gripper configuration action. + * @param goal_handle The handle of the goal to cancel. + * @return CancelResponse indicating acceptance or rejection of the cancellation. + */ + rclcpp_action::CancelResponse config_handle_cancel( + const std::shared_ptr goal_handle); + + /** + * @brief Handles the acceptance of the gripper configuration action. + * @param goal_handle The handle of the accepted goal. + */ + void config_handle_accepted(const std::shared_ptr goal_handle); + + /** + * @brief Executes the gripper configuration action. + * @param goal_handle The handle of the goal to execute. + */ + void config_execute(const std::shared_ptr goal_handle); +}; + +} // namespace io_gripper_controller + +#endif // GRIPPER_IO_CONTROLLER__GRIPPER_IO_CONTROLLER_HPP_ diff --git a/io_gripper_controller/io_gripper_controller.xml b/io_gripper_controller/io_gripper_controller.xml new file mode 100644 index 0000000000..dcb5f07b18 --- /dev/null +++ b/io_gripper_controller/io_gripper_controller.xml @@ -0,0 +1,8 @@ + + + + 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..8439ff92cf --- /dev/null +++ b/io_gripper_controller/package.xml @@ -0,0 +1,43 @@ + + + + io_gripper_controller + 0.0.0 + 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 + + generate_parameter_library + + control_msgs + controller_interface + controller_manager + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + ros2_control_test_assets + sensor_msgs + std_msgs + std_srvs + + 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..db83c5b58b --- /dev/null +++ b/io_gripper_controller/src/io_gripper_controller.cpp @@ -0,0 +1,1509 @@ +// Copyright (c) 2025, b>>robotized by Stogl Robotics +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "io_gripper_controller/io_gripper_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "rclcpp/version.h" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 17 +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); +#else +static const rmw_qos_profile_t qos_services = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; +#endif + +} // namespace + +namespace io_gripper_controller +{ +IOGripperController::IOGripperController() : controller_interface::ControllerInterface() {} + +controller_interface::CallbackReturn IOGripperController::on_init() +{ + gripper_service_buffer_.initRT(service_mode_type::IDLE); + configuration_key_ = ""; + configure_gripper_buffer_.initRT(configuration_key_); + gripper_state_buffer_.initRT(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; + } + + prepare_command_and_state_ios(); + + result = prepare_publishers_and_services(); + if (result != controller_interface::CallbackReturn::SUCCESS) + { + return result; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration IOGripperController::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(command_if_ios.size()); + for (const auto & command_io : command_if_ios) + { + command_interfaces_config.names.push_back(command_io); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration IOGripperController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_if_ios.size()); + for (const auto & state_io : state_if_ios) + { + state_interfaces_config.names.push_back(state_io); + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn IOGripperController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + check_gripper_and_reconfigure_state(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn IOGripperController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joint_state_values_.resize( + params_.open_close_joints.size() + params_.configuration_joints.size(), + std::numeric_limits::quiet_NaN()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type IOGripperController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + if (reconfigureFlag_.load()) + { + configuration_key_ = *(configure_gripper_buffer_.readFromRT()); + handle_reconfigure_state_transition(*(reconfigure_state_buffer_.readFromRT())); + } + + switch (*(gripper_service_buffer_.readFromRT())) + { + case service_mode_type::IDLE: + // do nothing + break; + case service_mode_type::OPEN: +<<<<<<< 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; + + default: + break; + } + + publish_gripper_joint_states(); + publish_dynamic_interface_values(); + + return controller_interface::return_type::OK; +} + +bool IOGripperController::find_and_set_command(const std::string & name, const double value) +{ + auto it = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), + [&](const hardware_interface::LoanedCommandInterface & command_interface) + { return command_interface.get_name() == name; }); + + if (it != command_interfaces_.end()) + { + return it->set_value(value); + } + 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: + for (size_t i = 0; i < set_before_command_close.size(); ++i) + { + setResult = + find_and_set_command(set_before_command_close[i], set_before_command_close_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "CLOSE - SET_BEFORE_COMMAND: Failed to set the command state for %s", + set_before_command_close[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CLOSE_GRIPPER); + break; + case gripper_state_type::CLOSE_GRIPPER: + for (size_t i = 0; i < command_ios_close.size(); ++i) + { + setResult = find_and_set_command(command_ios_close[i], command_ios_close_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "CLOSE_GRIPPER: Failed to set the command state for %s", + command_ios_close[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); + break; + case gripper_state_type::CHECK_GRIPPER_STATE: + for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) + { + check_state_ios_ = false; + for (const auto & high_val : state_params.high) + { + setResult = find_and_get_state(high_val, state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", high_val.c_str()); + } + else + { + if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) + { + check_state_ios_ = true; + } + else + { + check_state_ios_ = false; + break; + } + } + } + for (const auto & low_val : state_params.low) + { + setResult = find_and_get_state(low_val, state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "CLOSE - CHECK_GRIPPER_STATE: Failed to get the state for %s", low_val.c_str()); + } + else + { + if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) + { + check_state_ios_ = true; + } + else + { + check_state_ios_ = false; + break; + } + } + } + if (check_state_ios_) + { + closed_state_name_ = state_name; + gripper_state_buffer_.writeFromNonRT(gripper_state_type::SET_AFTER_COMMAND); + break; + } + } + break; + case gripper_state_type::SET_AFTER_COMMAND: + closed_state_values_ = params_.close.state.possible_closed_states_map.at(closed_state_name_); + + for (const auto & high_val : closed_state_values_.set_after_command_high) + { + setResult = find_and_set_command(high_val, 1.0); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", high_val.c_str()); + } + } + + for (const auto & low_val : closed_state_values_.set_after_command_low) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "CLOSE - SET_AFTER_COMMAND: set low after command %s", + low_val.c_str()); + setResult = find_and_set_command(low_val, 0.0); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "CLOSE - SET_AFTER_COMMAND: Failed to set the command state for %s", low_val.c_str()); + } + } + for (size_t i = 0; i < params_.close.state.possible_closed_states_map.at(closed_state_name_) + .joint_states.size(); + ++i) + { + joint_state_values_[i] = + params_.close.state.possible_closed_states_map.at(closed_state_name_).joint_states[i]; + } + gripper_state_buffer_.writeFromNonRT(gripper_state_type::IDLE); + closeFlag_.store(false); + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); + break; + default: + break; + } +} + +<<<<<<< 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) + { + setResult = + find_and_set_command(set_before_command_open[i], set_before_command_open_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "OPEN - SET_BEFORE_COMMAND: Failed to set the command state for %s", + set_before_command_open[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::OPEN_GRIPPER); + break; + case gripper_state_type::OPEN_GRIPPER: + // now open the gripper + for (size_t i = 0; i < command_ios_open.size(); ++i) + { + setResult = find_and_set_command(command_ios_open[i], command_ios_open_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "OPEN_GRIPPER: Failed to set the command state for %s", + command_ios_open[i].c_str()); + } + } + + gripper_state_buffer_.writeFromNonRT(gripper_state_type::CHECK_GRIPPER_STATE); +======= + case IOGripperState::SET_BEFORE_COMMAND: + set_commands(ios.set_before_command_ios, transition_name + " - SET_BEFORE_COMMAND"); + // TODO(destogl): check to use other Realtime sync object to have write from RT + 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) + { + setResult = find_and_get_state(state_ios_open[i], state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "OPEN - CHECK_GRIPPER_STATE: Failed to get the state for %s", + state_ios_open[i].c_str()); + } + else + { + if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) + { + check_state_ios_ = true; + } + else + { + check_state_ios_ = false; +======= + bool check_state_ios = true; + if (ios.has_multiple_end_states) + { + 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) + { + setResult = + find_and_set_command(set_after_command_open[i], set_after_command_open_values[i]); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "OPEN - SET_AFTER_COMMAND: Failed to set the command state for %s", + set_after_command_open[i].c_str()); + } + } + for (size_t i = 0; i < params_.open.joint_states.size(); ++i) +======= + else if ((current_time - last_transition_time_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + 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; + } +} + +void IOGripperController::handle_reconfigure_state_transition(const reconfigure_state_type & state) +{ + switch (state) + { + case reconfigure_state_type::IDLE: + // do nothing + break; + case reconfigure_state_type::SET_COMMAND: + config_index_ = + std::find(configurations_list_.begin(), configurations_list_.end(), configuration_key_); + if (config_index_ == configurations_list_.end()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Configuration not found"); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); + break; + } + else + { + conf_it_ = config_map_[std::distance(configurations_list_.begin(), config_index_)]; + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); + } + setResult = false; + for (const auto & io : conf_it_.command_high) + { + setResult = find_and_set_command(io, 1.0); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", io.c_str()); + } + } + for (const auto & io : conf_it_.command_low) + { + setResult = find_and_set_command(io, 0.0); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to set the command state for %s", io.c_str()); + } + } + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::CHECK_STATE); + break; + + case reconfigure_state_type::CHECK_STATE: + check_state_ios_ = false; + for (const auto & io : conf_it_.state_high) + { + setResult = find_and_get_state(io, state_value_); + if (!setResult) + { + check_state_ios_ = false; + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) + { + check_state_ios_ = false; + RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + break; + } + else + { + check_state_ios_ = true; + } + } + } + + for (const auto & io : conf_it_.state_low) + { + setResult = find_and_get_state(io, state_value_); + if (!setResult) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) + { + check_state_ios_ = false; + RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + break; + } + else + { + check_state_ios_ = true; + } + } + } + + if (check_state_ios_) + { + for (size_t i = 0; i < conf_it_.joint_states.size(); ++i) + { + joint_state_values_[i + params_.open_close_joints.size()] = conf_it_.joint_states[i]; + } + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::IDLE); + configure_gripper_buffer_.writeFromNonRT(""); + reconfigureFlag_.store(false); + } + break; + default: + break; + } +} + +controller_interface::CallbackReturn IOGripperController::check_parameters() +{ + if (params_.open.command.high.empty() 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; + } + + 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; + } + + // configurations parameter + if (!params_.configurations.empty()) + { + if (!params_.configuration_joints.empty()) + { + // configuration setup parameter + if (params_.configuration_setup.configurations_map.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of configuration map parameter cannot be zero if 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()) + { + // sensors interfaces parameter + if (params_.sensors_interfaces.gripper_specific_sensors_map.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of sensors interfaces parameter cannot be zero if gripper 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; +} + +void IOGripperController::prepare_command_and_state_ios() +{ + auto parse_interfaces_from_params = []( + const std::vector & parameter_values, + const double & value, + std::unordered_map & ios, + std::unordered_set & interface_list) + { + for (const auto & itf : parameter_values) + { +<<<<<<< Updated upstream + set_before_command_open.push_back(key); + set_before_command_open_values.push_back(1.0); + command_if_ios.insert(key); + } + } + + for (const auto & key : params_.open.set_before_command.low) + { + if (!key.empty()) + { + set_before_command_open.push_back(key); + set_before_command_open_values.push_back(0.0); + command_if_ios.insert(key); + } + } + for (const auto & key : params_.open.command.high) + { + if (!key.empty()) + { + command_ios_open.push_back(key); + command_ios_open_values.push_back(1.0); + command_if_ios.insert(key); +======= + if (!itf.empty()) + { + ios[itf] = value; + interface_list.insert(itf); + } +>>>>>>> Stashed changes + } + }; + +<<<<<<< Updated upstream + for (const auto & key : params_.open.command.low) + { + if (!key.empty()) + { + command_ios_open.push_back(key); + command_ios_open_values.push_back(0.0); + command_if_ios.insert(key); + } + } + + for (const auto & key : params_.open.set_after_command.high) + { + if (!key.empty()) + { + set_after_command_open.push_back(key); + set_after_command_open_values.push_back(1.0); + command_if_ios.insert(key); + } + } + + for (const auto & key : params_.open.set_after_command.low) + { + if (!key.empty()) + { + set_after_command_open.push_back(key); + set_after_command_open_values.push_back(0.0); + command_if_ios.insert(key); + } + } +======= + // make full command ios lists -- just once + parse_interfaces_from_params( + params_.open.set_before_command.high, 1.0, open_ios_.set_before_command_ios, command_if_ios); + 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) + { + if (!key.empty()) + { + set_before_command_close.push_back(key); + set_before_command_close_values.push_back(1.0); + command_if_ios.insert(key); + } + } + + for (const auto & key : params_.close.set_before_command.low) + { + if (!key.empty()) + { + set_before_command_close.push_back(key); + set_before_command_close_values.push_back(0.0); + command_if_ios.insert(key); + } + } + + for (const auto & key : params_.close.command.high) + { + command_ios_close.push_back(key); + command_ios_close_values.push_back(1.0); + command_if_ios.insert(key); + } + + for (const auto & key : params_.close.command.low) + { + if (!key.empty()) + { + command_ios_close.push_back(key); + command_ios_close_values.push_back(0.0); + command_if_ios.insert(key); + } + } + + // make full state ios lists -- just once + parse_interfaces_from_params(params_.open.state.high, 1.0, open_ios_.state_ios, state_if_ios); + parse_interfaces_from_params(params_.open.state.low, 0.0, open_ios_.state_ios, state_if_ios); + + for (const auto & [name, value] : params_.close.state.possible_closed_states_map) + { + for (const auto & key : value.high) + { + if (!key.empty()) + { + state_if_ios.insert(key); + } + } + for (const auto & key : value.low) + { + if (!key.empty()) + { + state_if_ios.insert(key); + } + } + for (const auto & key : value.set_after_command_high) + { + if (!key.empty()) + { + command_if_ios.insert(key); + } + } + for (const auto & key : value.set_after_command_low) + { + if (!key.empty()) + { + command_if_ios.insert(key); + } + } + } + + // get the configurations for different io which needs to be high or low + for (const auto & [key, val] : params_.configuration_setup.configurations_map) + { + config_map_.push_back(val); + } + + // get the configurations list + configurations_list_ = params_.configurations; + + for (const auto & config : config_map_) + { + for (const auto & io : config.command_high) + { + command_if_ios.insert(io); + } + for (const auto & io : config.command_low) + { + command_if_ios.insert(io); + } + for (const auto & io : config.state_high) + { + state_if_ios.insert(io); + } + for (const auto & io : config.state_low) + { + state_if_ios.insert(io); + } + } + + for (size_t i = 0; i < params_.gripper_specific_sensors.size(); ++i) + { + state_if_ios.insert(params_.sensors_interfaces.gripper_specific_sensors_map + .at(params_.gripper_specific_sensors[i]) + .input); + } +} + +controller_interface::CallbackReturn IOGripperController::prepare_publishers_and_services() +{ + reconfigureFlag_.store(false); + + // reset service buffer + gripper_service_buffer_.writeFromNonRT(service_mode_type::IDLE); + + // reset gripper state buffer + gripper_state_buffer_.writeFromNonRT(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); + gripper_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, qos_services, open_service_callback_group_); + + // close service + auto close_service_callback = [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + try + { + if (reconfigureFlag_.load()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); + response->success = false; + return; + } + gripper_service_buffer_.writeFromNonRT(service_mode_type::CLOSE); + gripper_state_buffer_.writeFromNonRT(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, qos_services, close_service_callback_group_); + + // configure gripper service + auto configure_gripper_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + try + { + std::string conf = request->config_name; + configure_gripper_buffer_.writeFromNonRT(conf.c_str()); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); + reconfigureFlag_.store(true); + while (reconfigureFlag_.load()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + response->result = true; + response->status = "Gripper reconfigured"; + } + catch (const std::exception & e) + { + response->result = false; + response->status = "Failed to reconfigure gripper"; + } + }; + + configure_gripper_service_ = get_node()->create_service( + "~/reconfigure_to", configure_gripper_service_callback, qos_services, + reconfigure_service_callback_group_); + } + else + { + // open close action server + gripper_feedback_ = std::make_shared(); + gripper_result_ = std::make_shared(); + gripper_action_server_ = rclcpp_action::create_server( + get_node(), "~/gripper_action", + std::bind( + &IOGripperController::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&IOGripperController::handle_cancel, this, std::placeholders::_1), + std::bind(&IOGripperController::handle_accepted, this, std::placeholders::_1)); + + // reconfigure action server + gripper_config_feedback_ = std::make_shared(); + gripper_config_result_ = std::make_shared(); + gripper_config_action_server_ = rclcpp_action::create_server( + get_node(), "~/reconfigure_gripper_action", + std::bind( + &IOGripperController::config_handle_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&IOGripperController::config_handle_cancel, this, std::placeholders::_1), + std::bind(&IOGripperController::config_handle_accepted, this, std::placeholders::_1)); + } + + try + { + // Gripper Joint State publisher + g_j_s_publisher_ = + get_node()->create_publisher("/joint_states", rclcpp::SystemDefaultsQoS()); + gripper_joint_state_publisher_ = std::make_unique(g_j_s_publisher_); + + auto final_joint_size = params_.open_close_joints.size() + params_.configuration_joints.size(); + + gripper_joint_state_publisher_->msg_.name.resize(final_joint_size); + gripper_joint_state_publisher_->msg_.position.resize(final_joint_size); + + joint_state_values_.resize(final_joint_size, std::numeric_limits::quiet_NaN()); + + for (size_t i = 0; i < params_.open_close_joints.size(); ++i) + { + gripper_joint_state_publisher_->msg_.name[i] = params_.open_close_joints[i]; + gripper_joint_state_publisher_->msg_.position[i] = joint_state_values_[i]; + } + for (size_t i = 0; i < params_.configuration_joints.size(); ++i) + { + gripper_joint_state_publisher_->msg_.name[i + params_.open_close_joints.size()] = + params_.configuration_joints[i]; + gripper_joint_state_publisher_->msg_.position[i + params_.open_close_joints.size()] = + joint_state_values_[i + params_.open_close_joints.size()]; + } + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + try + { + // interface publisher + if_publisher_ = get_node()->create_publisher( + "~/dynamic_interface", rclcpp::SystemDefaultsQoS()); + interface_publisher_ = std::make_unique(if_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +void IOGripperController::publish_gripper_joint_states() +{ + if (gripper_joint_state_publisher_ && gripper_joint_state_publisher_->trylock()) + { + gripper_joint_state_publisher_->msg_.header.stamp = get_node()->get_clock()->now(); + + // publish gripper joint state values + for (size_t i = 0; i < joint_state_values_.size(); ++i) + { + gripper_joint_state_publisher_->msg_.position[i] = joint_state_values_[i]; + } + } + gripper_joint_state_publisher_->unlockAndPublish(); +} + +void IOGripperController::publish_dynamic_interface_values() +{ + if (interface_publisher_ && interface_publisher_->trylock()) + { + interface_publisher_->msg_.header.stamp = + get_node()->get_clock()->now(); // Make sure it works and discuss with Dr. Denis + interface_publisher_->msg_.states.interface_names.clear(); + interface_publisher_->msg_.states.values.clear(); + interface_publisher_->msg_.states.values.resize(state_interfaces_.size()); + for (size_t i = 0; i < state_interfaces_.size(); ++i) + { + interface_publisher_->msg_.states.interface_names.push_back( + state_interfaces_.at(i).get_name()); // this can be done in a separate function one time. + // Change it later TODO (Sachin) : + interface_publisher_->msg_.states.values.at(i) = + static_cast(state_interfaces_.at(i).get_value()); + } + + interface_publisher_->msg_.commands.interface_names.clear(); + interface_publisher_->msg_.commands.values.clear(); + interface_publisher_->msg_.commands.values.resize(command_interfaces_.size()); + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + interface_publisher_->msg_.commands.interface_names.push_back( + command_interfaces_.at(i).get_name()); // this can be done in a separate function one time. + // Change it later TODO (Sachin) : + interface_publisher_->msg_.commands.values.at(i) = + static_cast(command_interfaces_.at(i).get_value()); + } + interface_publisher_->unlockAndPublish(); + } +} + +rclcpp_action::GoalResponse IOGripperController::handle_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) +{ + try + { + if (reconfigureFlag_.load()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Cannot close the gripper while reconfiguring"); + return rclcpp_action::GoalResponse::REJECT; + } + gripper_service_buffer_.writeFromNonRT( + (goal->open) ? service_mode_type::OPEN : service_mode_type::CLOSE); + gripper_state_buffer_.writeFromNonRT(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; +} + +void IOGripperController::handle_accepted(const std::shared_ptr goal_handle) +{ + std::thread{std::bind(&IOGripperController::execute, this, std::placeholders::_1), goal_handle} + .detach(); +} + +void IOGripperController::execute(std::shared_ptr goal_handle) +{ + auto result = std::make_shared(); + auto feedback = std::make_shared(); + while (true) + { + if (*(gripper_state_buffer_.readFromRT()) == 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->transition.state = static_cast(*(gripper_state_buffer_.readFromRT())); + goal_handle->publish_feedback(feedback); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } +} + +rclcpp_action::GoalResponse IOGripperController::config_handle_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) +{ + try + { + std::string conf = goal->config_name; + configure_gripper_buffer_.writeFromNonRT(conf.c_str()); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); + reconfigureFlag_.store(true); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during action handle goal with message: %s", + e.what()); + return rclcpp_action::GoalResponse::REJECT; + } + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse IOGripperController::config_handle_cancel( + const std::shared_ptr goal_handle) +{ + (void)goal_handle; + configure_gripper_buffer_.writeFromNonRT(""); + reconfigure_state_buffer_.writeFromNonRT(reconfigure_state_type::SET_COMMAND); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void IOGripperController::config_handle_accepted( + const std::shared_ptr goal_handle) +{ + std::thread{ + std::bind(&IOGripperController::config_execute, this, std::placeholders::_1), goal_handle} + .detach(); +} + +void IOGripperController::config_execute(std::shared_ptr goal_handle) +{ + auto result = std::make_shared(); + auto feedback = std::make_shared(); + while (true) + { + if (*(reconfigure_state_buffer_.readFromRT()) == reconfigure_state_type::IDLE) + { + result->result = true; + result->status = "Gripper reconfigured"; + goal_handle->succeed(result); + break; + } + else + { + feedback->transition.state = static_cast(*(reconfigure_state_buffer_.readFromRT())); + goal_handle->publish_feedback(feedback); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } +} + +void IOGripperController::check_gripper_and_reconfigure_state() +{ + bool gripper_state_found = false; + + for (size_t i = 0; i < state_ios_open.size(); ++i) + { + setResult = find_and_get_state(state_ios_open[i], state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", state_ios_open[i].c_str()); + } + else + { + if (abs(state_value_ - state_ios_open_values[i]) < std::numeric_limits::epsilon()) + { + gripper_state_found = true; + } + else + { + gripper_state_found = false; + } + } + } + + if (gripper_state_found) + { + for (size_t i = 0; i < params_.open.joint_states.size(); ++i) + { + joint_state_values_[i] = params_.open.joint_states[i]; + } + } + else + { + for (const auto & [state_name, state_params] : params_.close.state.possible_closed_states_map) + { + for (const auto & high_val : state_params.high) + { + setResult = find_and_get_state(high_val, state_value_); + if (!setResult) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to get the state for %s", high_val.c_str()); + } + else + { + if (abs(state_value_ - 1.0) < std::numeric_limits::epsilon()) + { + gripper_state_found = true; + } + else + { + gripper_state_found = false; + break; + } + } + } + for (const auto & low_val : state_params.low) + { + setResult = find_and_get_state(low_val, state_value_); + if (!setResult) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", low_val.c_str()); + } + else + { + if (abs(state_value_ - 0.0) < std::numeric_limits::epsilon()) + { + gripper_state_found = true; + } + else + { + gripper_state_found = false; + break; + } + } + } + + if (gripper_state_found) + { + for (size_t i = 0; + i < params_.close.state.possible_closed_states_map.at(state_name).joint_states.size(); + ++i) + { + joint_state_values_[i] = + params_.close.state.possible_closed_states_map.at(state_name).joint_states[i]; + } + break; + } + } + } + + bool reconfigure_state_found = false; + for (const auto & [key, val] : params_.configuration_setup.configurations_map) + { + for (const auto & io : val.state_high) + { + setResult = find_and_get_state(io, state_value_); + if (!setResult) + { + reconfigure_state_found = false; + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 1.0) < std::numeric_limits::epsilon())) + { + reconfigure_state_found = false; + RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + break; + } + else + { + reconfigure_state_found = true; + } + } + } + + for (const auto & io : val.state_low) + { + setResult = find_and_get_state(io, state_value_); + if (!setResult) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get the state for %s", io.c_str()); + } + else + { + if (!(std::abs(state_value_ - 0.0) < std::numeric_limits::epsilon())) + { + reconfigure_state_found = false; + RCLCPP_DEBUG(get_node()->get_logger(), "value for state doesn't match %s", io.c_str()); + break; + } + else + { + reconfigure_state_found = true; + } + } + } + if (reconfigure_state_found) + { + for (size_t i = 0; i < val.joint_states.size(); ++i) + { + joint_state_values_[i + params_.open_close_joints.size()] = val.joint_states[i]; + } + break; + } + } +} +} // namespace io_gripper_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + io_gripper_controller::IOGripperController, controller_interface::ControllerInterface) diff --git a/io_gripper_controller/src/io_gripper_controller.yaml b/io_gripper_controller/src/io_gripper_controller.yaml new file mode 100644 index 0000000000..ecf10e005d --- /dev/null +++ b/io_gripper_controller/src/io_gripper_controller.yaml @@ -0,0 +1,270 @@ +# Copyright (c) 2025, b>>robotized by Stogl Robotics +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# 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, +<<<<<<< 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, + description: "List of joint names that should change values according to open or close state of the gripper", + validation: { + unique<>: null, + not_empty<>: null, + } + } + open: + joint_states: { + type: double_array, + description: "List of joint values when gripper is open. The order of the values should match the order of the joint names in open_close_joints", + validation: { + unique<>: null, + not_empty<>: null, + } + } + set_before_command: + high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) before opening the gripper", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to low (0.0) before opening the gripper", + validation: { + unique<>: null, + } + } + command: + high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) to open the gripper.", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of command interface names that have to be see to low (0.0) to open the gripper.", + validation: { + unique<>: null, + } + } + state: + high: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to equal high (1.0) to represent the gripper is open.", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to equal low (0.0) to represent the gripper is open.", + validation: { + unique<>: null, + } + } + set_after_command: + high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) after opening the gripper", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of command interface names that have to be set to low (0.0) after opening the gripper.", + validation: { + unique<>: null, + } + } + possible_closed_states: { + type: string_array, + default_value: [], + description: "List of possible closed states e.g. empty_close and full close", + validation: { + unique<>: null, + } + } + close: + set_before_command: + high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) before closing the gripper", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to low (0.0) before closing the gripper", + validation: { + unique<>: null, + } + } + command: + high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) to close the gripper", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to low (0.0) to close the gripper", + validation: { + unique<>: null, + } + } + state: + __map_possible_closed_states: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint values when gripper is closed. The order of the values should match the order of the joint names in open_close_joints", + validation: { + unique<>: null, + } + } + high: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to equal high (1.0) to represent the gripper is closed", + validation: { + unique<>: null, + } + } + low: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to equal low (0.0) to represent the gripper is closed", + validation: { + unique<>: null, + } + } + set_after_command_high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) after closing the gripper", + validation: { + unique<>: null, + } + } + set_after_command_low: { + type: string_array, + default_value: [], + description: "(optional) list of command interface names that have to be set to low (0.0) after closing the gripper.", + validation: { + unique<>: null, + } + } + configuration_joints: { + type: string_array, + default_value: [], + description: "List of joint names that are used to switch between different configurations of the gripper", + validation: { + unique<>: null, + } + } + + configurations: { + type: string_array, + default_value: [], + description: "Configuration names that can be used to switch between different configurations of the gripper", + validation: { + unique<>: null, + } + } + configuration_setup: + __map_configurations: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint states that open the gripper", + validation: { + unique<>: null, + } + } + command_high: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to high (1.0) for this configuration.", + } + command_low: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to low (0.0) for this configuration.", + } + state_high: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to be high (1.0) to represent this configuration.", + } + state_low: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to be low (0.0) to represent this configuration.", + } + + gripper_specific_sensors: { + type: string_array, + default_value: [], + description: "List of sensor names that are specific to the gripper", + validation: { + unique<>: null, + } + } + sensors_interfaces: + __map_gripper_specific_sensors: + input: { + type: string, + default_value: "", + description: "Name of the input interface that is specific to the gripper", + } diff --git a/io_gripper_controller/test/test_io_gripper_controller.cpp b/io_gripper_controller/test/test_io_gripper_controller.cpp new file mode 100644 index 0000000000..6ba836e79e --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller.cpp @@ -0,0 +1,46 @@ +// 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. +// 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..fd10c70c66 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller.hpp @@ -0,0 +1,436 @@ +// 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. +// 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 "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "io_gripper_controller/io_gripper_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +// TODO(anyone): replace the state and command message types +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 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..ce76fa2847 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_all_param_set.cpp @@ -0,0 +1,53 @@ +// 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. +// 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 "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.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); +} 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..e07f196c59 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_close.cpp @@ -0,0 +1,140 @@ +// 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. +// 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 "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.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_->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_->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_->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(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_->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 new file mode 100644 index 0000000000..1f5099cd73 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_open.cpp @@ -0,0 +1,130 @@ +// 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. +// 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 "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.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_->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_->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_->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(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_->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 new file mode 100644 index 0000000000..9e3b1aca84 --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_open_close_action.cpp @@ -0,0 +1,225 @@ +// 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. +// 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 "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.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_->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_->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_->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_->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 + 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_->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_->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_->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_->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 + 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); +} 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..109071ed0a --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure.cpp @@ -0,0 +1,106 @@ +// 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. +// 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 "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.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::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); +} 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..b9f71b1acb --- /dev/null +++ b/io_gripper_controller/test/test_io_gripper_controller_reconfigure_action.cpp @@ -0,0 +1,113 @@ +// 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. +// 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 "rclcpp/rclcpp.hpp" +#include "test_io_gripper_controller.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::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); +} 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..ffaa94f2b0 --- /dev/null +++ b/io_gripper_controller/test/test_load_io_gripper_controller.cpp @@ -0,0 +1,40 @@ +// 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(); +}