diff --git a/gpio_controllers/CMakeLists.txt b/gpio_controllers/CMakeLists.txt index a19f7586af..5cb933c2f3 100644 --- a/gpio_controllers/CMakeLists.txt +++ b/gpio_controllers/CMakeLists.txt @@ -7,34 +7,59 @@ export_windows_symbols() # find dependencies find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) find_package(controller_interface REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) -find_package(generate_parameter_library REQUIRED) -find_package(control_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) generate_parameter_library(gpio_command_controller_parameters src/gpio_command_controller_parameters.yaml ) -add_library(gpio_controllers - SHARED +add_library(gpio_controllers SHARED src/gpio_command_controller.cpp ) target_include_directories(gpio_controllers PRIVATE include) -target_link_libraries(gpio_controllers PUBLIC - gpio_command_controller_parameters - controller_interface::controller_interface - hardware_interface::hardware_interface - pluginlib::pluginlib - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle - realtime_tools::realtime_tools - ${control_msgs_TARGETS} - ${builtin_interfaces_TARGETS}) +target_link_libraries(gpio_controllers PUBLIC gpio_command_controller_parameters) +ament_target_dependencies(gpio_controllers PUBLIC + builtin_interfaces + controller_interface + hardware_interface + pluginlib + rclcpp_lifecycle + rcutils + realtime_tools + control_msgs +) + +generate_parameter_library(gpio_tool_controller_parameters + src/gpio_tool_controller.yaml +) +add_library(gpio_tool_controllers SHARED + src/gpio_tool_controller.cpp +) +target_compile_features(gpio_tool_controllers PUBLIC cxx_std_20) +target_include_directories(gpio_tool_controllers PUBLIC + "$" + "$") +target_link_libraries(gpio_tool_controllers PUBLIC + gpio_tool_controller_parameters + controller_interface::controller_interface + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools + ${control_msgs_TARGETS} + ${builtin_interfaces_TARGETS} + ${std_srvs_TARGETS} +) pluginlib_export_plugin_description_file(controller_interface gpio_controllers_plugin.xml) @@ -46,6 +71,7 @@ install( install( TARGETS gpio_controllers + gpio_tool_controllers RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -78,19 +104,48 @@ if(BUILD_TESTING) gpio_controllers ros2_control_test_assets::ros2_control_test_assets ) + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock( + test_load_gpio_tool_controller + test/test_load_gpio_tool_controller.cpp + ) + target_include_directories(test_load_gpio_tool_controller PRIVATE include) + target_link_libraries(test_load_gpio_tool_controller + gpio_tool_controllers + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + + ament_add_gmock(test_gpio_tool_controller + test/gpio_tool_controller/test_gpio_tool_controller.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_open.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_close.cpp + test/gpio_tool_controller/test_gpio_tool_controller_all_param_set.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_open_close_action.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_reconfigure.cpp + # test/gpio_tool_controller/test_gpio_tool_controller_reconfigure_action.cpp + ) + target_include_directories(test_gpio_tool_controller PRIVATE include) + target_link_libraries(test_gpio_tool_controller + gpio_tool_controllers + controller_manager::controller_manager + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets + ) endif() ament_export_dependencies( - controller_interface - hardware_interface - rclcpp - rclcpp_lifecycle - realtime_tools + controller_interface::controller_interface + hardware_interface::hardware_interface + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + realtime_tools::realtime_tools ) ament_export_include_directories( include ) ament_export_libraries( gpio_controllers + gpio_tool_controllers ) ament_package() diff --git a/gpio_controllers/doc/userdoc.rst b/gpio_controllers/doc/userdoc.rst index a59da906bb..f747539476 100644 --- a/gpio_controllers/doc/userdoc.rst +++ b/gpio_controllers/doc/userdoc.rst @@ -57,3 +57,26 @@ The controller expects at least one GPIO interface and the corresponding command - ana.2 With the above-defined controller configuration, the controller will accept commands for all gpios' interfaces and will only publish the state of Gpio2. + + + +### gpio_tool_controller + +The controller enables control of tools connected to the robot or components of a robot via general-purpose inputs and outputs (GPIO), digital or analog. +Concretely the controller is made for controlling the custom-made industrial grippers or tools, or components like lifts or breaks. +The IOs might even be virtual, like controlling the state machine of a robot that is implemented on a PLC. + +The controller provides high-level interface for engaging or disengaging a tool or a component, as well as for reconfiguring it. +The reconfiguration is possible only in *disengaged* state. +In the example of gripper this would be *open* state. + + + +The controller implements state machine for the transition between states and + + +1. Tool Actions + +1. Tool state machine + +2. Transitions diff --git a/gpio_controllers/gpio_controllers_plugin.xml b/gpio_controllers/gpio_controllers_plugin.xml index 03fd3e1ee0..26468d5bb3 100644 --- a/gpio_controllers/gpio_controllers_plugin.xml +++ b/gpio_controllers/gpio_controllers_plugin.xml @@ -5,3 +5,12 @@ + + + + + The gpio tool controllers enables control of the tools that are controlled using GPIOs. For example, pneumatic grippers, lifts, etc. It enables management of multiple engaged state, e.g., if gripper is closed with or without an object. + The controller can be also be used in the simple form where certain states of a device are set through GPIOs, e.g., manual and automatic mode. + + + diff --git a/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp new file mode 100644 index 0000000000..2e100ab940 --- /dev/null +++ b/gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp @@ -0,0 +1,340 @@ +// Copyright (c) 2025, b»robotized Group +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef gpio_tool_controller__gpio_tool_controller_HPP_ +#define gpio_tool_controller__gpio_tool_controller_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include "control_msgs/action/gpio_tool_command.hpp" +#include "control_msgs/action/set_gpio_tool_config.hpp" +#include "control_msgs/msg/dynamic_interface_values.hpp" +#include "control_msgs/msg/gpio_tool_controller_state.hpp" +#include "control_msgs/msg/gpio_tool_transition.hpp" +#include "control_msgs/msg/interface_value.hpp" +#include "control_msgs/srv/set_gpio_tool_config.hpp" +#include "controller_interface/controller_interface.hpp" +#include "gpio_controllers/gpio_tool_controller_parameters.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "std_srvs/srv/set_bool.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace gpio_tool_controller +{ +/** + * @enum service_mode_type + * @brief Represents the various service modes of the tool. These modes represents the high level + * states of the tool. + * + * - IDLE: The tool is in an idle state, not performing any action. + * - DISENGAGING: The tool is in the process of opening. + * - ENGAGING: The tool is in the process of closing. + * - RECONFIGURING: The tool is in the process of reconfiguring to a new state. + */ +enum class ToolAction : std::uint8_t +{ + IDLE = 0, + DISENGAGING = 1, + ENGAGING = 2, + RECONFIGURING = 3, + CANCELING = 4, +}; + +class GpioToolController : public controller_interface::ControllerInterface +/** + * @brief GpioToolController class handles the control of an IO-based tools, like grippers, lifts, + * mode control. + */ +{ +public: + /** + * @brief Constructor for GpioToolController. + */ + GpioToolController(); + + /** + * @brief Initializes the controller. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_init() override; + + /** + * @brief Configures the command interfaces. + * @return InterfaceConfiguration for command interfaces. + */ + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + /** + * @brief Configures the state interfaces. + * @return InterfaceConfiguration for state interfaces. + */ + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + /** + * @brief Configures the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Activates the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Deactivates the controller. + * @param previous_state The previous state of the lifecycle. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + /** + * @brief Updates the controller state. + * @param time The current time. + * @param period The time since the last update. + * @return return_type indicating success or failure. + */ + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + gpio_tool_controller::Params params_; + + struct ToolTransitionIOs + { + std::vector possible_states; + + std::unordered_map>> + set_before_commands; + std::unordered_map>> + set_before_states; + + std::unordered_map>> + commands; + + std::unordered_map>> + states; + std::unordered_map> states_joint_states; + std::unordered_map>> + set_after_commands; + std::unordered_map>> + set_after_states; + }; + + ToolTransitionIOs disengaged_gpios_; + ToolTransitionIOs engaged_gpios_; + ToolTransitionIOs reconfigure_gpios_; + + rclcpp::Time state_change_start_; + realtime_tools::RealtimeThreadSafeBox current_configuration_; + realtime_tools::RealtimeThreadSafeBox current_state_; + + std::unordered_set command_if_ios_; + std::unordered_set state_if_ios_; + + using EngagingSrvType = std_srvs::srv::Trigger; + using ResetSrvType = std_srvs::srv::Trigger; + using ConfigSrvType = control_msgs::srv::SetGPIOToolConfig; + using DynInterfaceMsg = control_msgs::msg::DynamicInterfaceValues; + using EngagingActionType = control_msgs::action::GPIOToolCommand; + using ConfigActionType = control_msgs::action::SetGPIOToolConfig; + using GPIOToolTransition = control_msgs::msg::GPIOToolTransition; + using ControllerStateMsg = control_msgs::msg::GPIOToolControllerState; + + std::shared_ptr param_listener_; + rclcpp::Service::SharedPtr disengaged_service_; + rclcpp::Service::SharedPtr engaged_service_; + rclcpp::Service::SharedPtr reconfigure_tool_service_; + rclcpp_action::Server::SharedPtr engaging_action_server_; + rclcpp_action::Server::SharedPtr config_action_server_; + rclcpp::Service::SharedPtr reset_service_; + + // Store current action tool is executing + std::atomic current_tool_action_{ToolAction::IDLE}; + std::atomic current_tool_transition_{GPIOToolTransition::IDLE}; + std::atomic reset_halted_{false}; + std::atomic transition_time_updated_{false}; + realtime_tools::RealtimeThreadSafeBox target_configuration_; + + using ToolJointStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr t_js_publisher_; + std::unique_ptr tool_joint_state_publisher_; + std::vector joint_states_values_; + using InterfacePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr if_publisher_; + std::unique_ptr interface_publisher_; + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + rclcpp::Publisher::SharedPtr t_s_publisher_; + std::unique_ptr controller_state_publisher_; + + EngagingSrvType::Response process_engaging_request( + const ToolAction & requested_action, const std::string & requested_action_name); + + EngagingSrvType::Response process_reconfigure_request(const std::string & config_name); + + EngagingSrvType::Response service_wait_for_transition_end( + const std::string & requested_action_name); + +private: + bool configuration_control_enabled_ = true; + bool joint_states_need_publishing_ = true; + + /** + * @brief Handles the state transition when enaging the tool. + * @param current_time [in] Current time for checking the transition time. + * @param ios [in] ToolTransitionIOs structure containing the IOs configurations for the + * transition. + * @param target_state [in] State name to target when checking reached states. + * @param joint_states [in/out] Joint states vector to write the joint states to. + * @param joint_states_start_index [in] Start index in the joint_states vector to write the joint + * states to. + * @param end_state [out] Currently determined state during transition. If empty, tool is + * currently in transition and no known state has been reached. + */ + void handle_tool_state_transition( + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, + const std::string & target_state, std::vector & joint_states, + const size_t joint_states_start_index, std::string & end_state); + + /** + * @brief + * + * @param current_time [in] Current time for checking the transition time. + * @param ios [in] ToolTransitionIOs structure containing the IOs configurations for the + * transition. + * @param joint_states [in/out] Joint states vector to write the joint states to. + * @param joint_states_start_index [in] Start index in the joint_states vector to write the joint + * states to. + * @param output_prefix [in] Prefix to add to the output messages. + * @param next_transition [in] Next transition to set if the current transition is completed. + * @param target_and_found_state_name [in/out] State name to target when checking reached states, + * and feedback on the found state. If the argument is not empty, the next transition will be + * triggered only when that state is reached. If any state is acceptable, empty string shall be + * passed. In that case the next transition will be triggered if any state if reached. In either + * case the output is the found state name. + * @param warning_output [in] If true, warning messages will be printed. + */ + void check_tool_state_and_switch( + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, + std::vector & joint_states, const size_t joint_states_start_index, + const std::string & output_prefix, const uint8_t next_transition, + std::string & target_and_found_state_name, const bool warning_output = false); + + /** + * @brief Prepares the command and state IOs. + * \returns true if successful, false otherwise. Check the output if error has happened. + */ + bool prepare_command_and_state_ios(); + + /** + * @brief Prepares the publishers and services. + * @return CallbackReturn indicating success or failure. + */ + controller_interface::CallbackReturn prepare_publishers_and_services(); + + /** + * @brief Publishes the the values from the RT loop. + */ + void publish_topics(const rclcpp::Time & current_time); + + /** + * @brief Checks the tools state. + */ + void check_tool_state(const rclcpp::Time & current_time, const bool warning_output = false); + + bool set_commands( + const std::unordered_map> & commands, + const std::string & output_prefix, const uint8_t next_transition); + bool check_states( + const rclcpp::Time & current_time, + const std::unordered_map> & states, + const std::string & output_prefix, const uint8_t next_transition, + const bool warning_output = false); + + std::vector configurations_list_; + std::vector config_map_; + double state_value_; + std::string configuration_key_; + bool check_state_ios_; + std::string closed_state_name_; + std::vector::iterator config_index_; + rclcpp::CallbackGroup::SharedPtr disengaging_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr engaging_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr reconfigure_service_callback_group_; + rclcpp::CallbackGroup::SharedPtr reset_service_callback_group_; + + /** + * @brief Handles the goal for the tool action. + * @param uuid The UUID of the goal. + * @param goal The goal to handle. + * @return GoalResponse indicating acceptance or rejection of the goal. + */ + rclcpp_action::GoalResponse handle_engaging_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + /** + * @brief Handles the cancellation of the tool action. + * @param goal_handle The handle of the goal to cancel. + * @return CancelResponse indicating acceptance or rejection of the cancellation. + */ + rclcpp_action::CancelResponse handle_engaging_cancel( + std::shared_ptr> goal_handle); + + /** + * @brief Handles the goal for the gripper configuration action. + * @param uuid The UUID of the goal. + * @param goal The goal to handle. + * @return GoalResponse indicating acceptance or rejection of the goal. + */ + rclcpp_action::GoalResponse handle_config_goal( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + + /** + * @brief Handles the cancellation of the gripper configuration action. + * @param goal_handle The handle of the goal to cancel. + * @return CancelResponse indicating acceptance or rejection of the cancellation. + */ + rclcpp_action::CancelResponse handle_config_cancel( + std::shared_ptr> goal_handle); + + /** + * @brief Handles the accepted goal for the tool state and configuration changes. + * @param goal_handle The handle of the accepted goal. + */ + template + void handle_action_accepted( + std::shared_ptr> goal_handle); +}; + +} // namespace gpio_tool_controller + +#endif // gpio_tool_controller__gpio_tool_controller_HPP_ diff --git a/gpio_controllers/package.xml b/gpio_controllers/package.xml index 9ffeda6f1a..1cc4c7db61 100644 --- a/gpio_controllers/package.xml +++ b/gpio_controllers/package.xml @@ -23,13 +23,15 @@ ros2_control_cmake + control_msgs controller_interface + generate_parameter_library hardware_interface rclcpp rclcpp_lifecycle - control_msgs realtime_tools - generate_parameter_library + sensor_msgs + std_srvs pluginlib diff --git a/gpio_controllers/src/gpio_tool_controller.cpp b/gpio_controllers/src/gpio_tool_controller.cpp new file mode 100644 index 0000000000..d818673cb5 --- /dev/null +++ b/gpio_controllers/src/gpio_tool_controller.cpp @@ -0,0 +1,1427 @@ +// Copyright (c) 2025, b»robotized Group +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gpio_controllers/gpio_tool_controller.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "rclcpp/version.h" + +namespace +{ // utility + +// Changed services history QoS to keep all so we don't lose any client service calls +// \note The versions conditioning is added here to support the source-compatibility with Humble +#if RCLCPP_VERSION_MAJOR >= 17 +rclcpp::QoS qos_services = + rclcpp::QoS(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_ALL, 1)) + .reliable() + .durability_volatile(); +#else +static const rmw_qos_profile_t qos_services = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; +#endif + +} // namespace + +namespace gpio_tool_controller +{ +GpioToolController::GpioToolController() : controller_interface::ControllerInterface() {} + +controller_interface::CallbackReturn GpioToolController::on_init() +{ + current_tool_action_.store(ToolAction::IDLE); + current_tool_transition_.store(GPIOToolTransition::IDLE); + target_configuration_.set(""); + current_state_.set(""); + current_configuration_.set(""); + + try + { + param_listener_ = std::make_shared(get_node()); + } + catch (const std::exception & e) + { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GpioToolController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + params_ = param_listener_->get_params(); + bool all_good = true; + + if (params_.engaged_joints.empty() && params_.configuration_joints.empty()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "No joints defined therefore no joint states publisher is created."); + joint_states_need_publishing_ = false; + } + else + { + joint_states_need_publishing_ = true; + } + + auto check_joint_states_sizes = [this]( + const size_t joint_states_size, + const size_t joint_states_values_size, + const std::string & param_name) + { + if (joint_states_size != joint_states_values_size) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of '%s' joint values and defined states must be equal. " + "Joint states values size: %zu, expected size: %zu.", + param_name.c_str(), joint_states_values_size, joint_states_size); + return false; + } + return true; + }; + + if (!check_joint_states_sizes( + params_.engaged_joints.size(), params_.disengaged.joint_states.size(), "engaged")) + { + all_good = false; + } + + if ( + params_.possible_engaged_states.size() != + params_.engaged.states.possible_engaged_states_map.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'possible_engaged_states' and 'engaged.states' parameters must be equal. " + "Possible engaged states size: %zu, engaged states size: %zu.", + params_.possible_engaged_states.size(), + params_.engaged.states.possible_engaged_states_map.size()); + all_good = false; + } + + for (const auto & [name, info] : params_.engaged.states.possible_engaged_states_map) + { + if (!check_joint_states_sizes( + params_.engaged_joints.size(), info.joint_states.size(), "engaged " + name + " state")) + { + all_good = false; + } + } + + // Initialize storage of joint state values + joint_states_values_.resize( + params_.engaged_joints.size() + params_.configuration_joints.size(), + std::numeric_limits::quiet_NaN()); + + // check sizes of all other parameters + auto check_parameter_pairs = [this]( + const std::vector & interfaces, + const std::vector & values, + const std::string & parameter_name) + { + if (interfaces.size() != values.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), "Size of parameter's '%s' interfaces and values must be equal.", + parameter_name.c_str()); + return false; + } + return true; + }; + + if ( + !check_parameter_pairs( + params_.disengaged.set_before_command.interfaces, + params_.disengaged.set_before_command.values, "disengaged before command") || + !check_parameter_pairs( + params_.disengaged.set_before_state.interfaces, params_.disengaged.set_before_state.values, + "disengaged before state") || + !check_parameter_pairs( + params_.disengaged.command.interfaces, params_.disengaged.command.values, + "disengaged command") || + !check_parameter_pairs( + params_.disengaged.state.interfaces, params_.disengaged.state.values, "disengaged state") || + !check_parameter_pairs( + params_.disengaged.set_after_command.interfaces, params_.disengaged.set_after_command.values, + "disengaged after command") || + !check_parameter_pairs( + params_.disengaged.set_after_state.interfaces, params_.disengaged.set_after_state.values, + "disengaged after state")) + { + all_good = false; + } + + if ( + !check_parameter_pairs( + params_.engaged.set_before_command.interfaces, params_.engaged.set_before_command.values, + "engaged before command") || + !check_parameter_pairs( + params_.engaged.set_before_state.interfaces, params_.engaged.set_before_state.values, + "engaged before state") || + !check_parameter_pairs( + params_.engaged.command.interfaces, params_.engaged.command.values, "engaged command")) + { + all_good = false; + } + + for (const auto & [name, info] : params_.engaged.states.possible_engaged_states_map) + { + if ( + !check_parameter_pairs(info.interfaces, info.values, "engaged " + name + " states") || + !check_parameter_pairs( + info.set_after_command_interfaces, info.set_after_command_values, + "engaged " + name + " after command") || + !check_parameter_pairs( + info.set_after_state_interfaces, info.set_after_state_values, + "engaged " + name + " after state")) + { + all_good = false; + } + } + + if (params_.configurations.empty()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "No configurations defined. Configuration control will be disabled."); + configuration_control_enabled_ = false; + } + else + { + configuration_control_enabled_ = true; + } + + if (params_.configurations.size() != params_.configuration_setup.configurations_map.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'configurations' and 'configuration_setup' parameters must be equal. " + "Configurations size: %zu, configuration joints size: %zu.", + params_.configurations.size(), params_.configuration_setup.configurations_map.size()); + all_good = false; + } + for (const auto & [name, info] : params_.configuration_setup.configurations_map) + { + if ( + !check_joint_states_sizes( + params_.configuration_joints.size(), info.joint_states.size(), "configuration " + name) || + !check_parameter_pairs( + info.set_before_command_interfaces, info.set_before_command_values, + "configuration " + name + " before commands") || + !check_parameter_pairs( + info.set_before_state_interfaces, info.set_before_state_values, + "configuration " + name + " before states") || + !check_parameter_pairs( + info.command_interfaces, info.command_values, "configuration " + name + " commands") || + !check_parameter_pairs( + info.state_interfaces, info.state_values, "configuration " + name + " states") || + !check_parameter_pairs( + info.set_after_command_interfaces, info.set_after_command_values, + "configuration " + name + " after commands") || + !check_parameter_pairs( + info.set_after_state_interfaces, info.set_after_state_values, + "configuration " + name + " after states")) + { + all_good = false; + } + } + + if ( + params_.tool_specific_sensors.size() != + params_.sensors_interfaces.tool_specific_sensors_map.size()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Size of 'tool_specific_sensors' and 'sensors_interfaces' parameters must be equal. " + "Tool specific sensors size: %zu, sensors interfaces size: %zu.", + params_.tool_specific_sensors.size(), + params_.sensors_interfaces.tool_specific_sensors_map.size()); + all_good = false; + } + for (const auto & [name, info] : params_.sensors_interfaces.tool_specific_sensors_map) + { + if (info.interface.empty()) + { + RCLCPP_FATAL( + get_node()->get_logger(), "Interfaces for tool specific sensor '%s' cannot be empty.", + name.c_str()); + all_good = false; + } + } + + if (!all_good) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "One or more parameters are not configured correctly. See above messages for details."); + return CallbackReturn::FAILURE; + } + + if (!prepare_command_and_state_ios()) + { + RCLCPP_FATAL( + get_node()->get_logger(), + "Failed to prepare command and state GPIOs. See above messages for details."); + return CallbackReturn::FAILURE; + } + + auto result = prepare_publishers_and_services(); + if (result != controller_interface::CallbackReturn::SUCCESS) + { + return result; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration GpioToolController::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(command_if_ios_.size()); + for (const auto & command_io : command_if_ios_) + { + command_interfaces_config.names.push_back(command_io); + } + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration GpioToolController::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(state_if_ios_.size()); + for (const auto & state_io : state_if_ios_) + { + state_interfaces_config.names.push_back(state_io); + } + + return state_interfaces_config; +} + +controller_interface::CallbackReturn GpioToolController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + state_change_start_ = get_node()->now(); + check_tool_state(state_change_start_, true); + // TODO(denis): update ros2_control and then enable this in this version the interfaces are not + // released when controller fails to activate if (current_state_.empty() || + // current_configuration_.empty()) + // { + // RCLCPP_FATAL(get_node()->get_logger(), "Controller can not be started as tool state can not + // be determined! Make sure the hardware is connected properly and controller's configuration is + // correct, than try to activate the controller again."); return + // controller_interface::CallbackReturn::FAILURE; + // } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn GpioToolController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + joint_states_values_.resize( + params_.engaged_joints.size() + params_.configuration_joints.size(), + std::numeric_limits::quiet_NaN()); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type GpioToolController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + switch (current_tool_action_.load()) + { + case ToolAction::IDLE: + { + // do nothing + state_change_start_ = time; + check_tool_state(time, false); + break; + } + case ToolAction::DISENGAGING: + { + // A local copy is used because handle_tool_state_transition modifies its state argument + std::string current_state = current_state_.get(); + handle_tool_state_transition( + time, disengaged_gpios_, params_.disengaged.name, joint_states_values_, 0, current_state); + current_state_.set(current_state); + break; + } + case ToolAction::ENGAGING: + { + // A local copy is used because handle_tool_state_transition modifies its state argument + std::string current_state = current_state_.get(); + handle_tool_state_transition( + time, engaged_gpios_, params_.engaged.name, joint_states_values_, 0, current_state); + current_state_.set(current_state); + break; + } + case ToolAction::RECONFIGURING: + { + // A local copy is used because handle_tool_state_transition modifies its state argument + std::string current_configuration = current_configuration_.get(); + // realtime_tools::RealtimeThreadSafeBox - only way to access box contents in READ-ONLY mode + // without copying. In the provided lambda we operate on the data. + target_configuration_.try_get( + [&](const std::string & target_config) + { + handle_tool_state_transition( + time, reconfigure_gpios_, target_config, joint_states_values_, + params_.engaged_joints.size(), current_configuration); + }); + current_configuration_.set(current_configuration); + break; + } + case ToolAction::CANCELING: + { + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, + "%s - CANCELING: Tool action is being canceled, " + "going to HALTED. Reset the tool using '~/reset_halted' service. After that set sensible " + "state.", + current_state_.get().c_str()); + current_tool_transition_.store(GPIOToolTransition::HALTED); + check_tool_state(time, true); + std::vector tmp_vec; + std::string tmp_str; + handle_tool_state_transition( + time, ToolTransitionIOs(), "", tmp_vec, 0, + tmp_str); // parameters don't matter as end up processing only the halted state + break; + } + default: + { + break; + } + } + publish_topics(time); + + return controller_interface::return_type::OK; +} + +bool GpioToolController::set_commands( + const std::unordered_map> & commands, + const std::string & output_prefix, const uint8_t next_transition) +{ + bool all_successful = true; + if (!commands.empty()) // only set commands if present + { + for (const auto & [name, pair] : commands) + { + const auto & [value, index] = pair; + if (!command_interfaces_[index].set_value(value)) + { + RCLCPP_ERROR( + get_node()->get_logger(), "%s: Failed to set the command state for %s", + output_prefix.c_str(), command_interfaces_[index].get_name().c_str()); + all_successful = false; + } + } + } + + if (all_successful) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: Transitioning after setting commands to: %d", + output_prefix.c_str(), next_transition); + // when canceling we don't continue the transition + if (current_tool_action_.load() != ToolAction::CANCELING) + { + current_tool_transition_.store(next_transition); + } + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), "%s: Error occurred when setting commands - see above for details.", + output_prefix.c_str()); + current_tool_transition_.store(GPIOToolTransition::HALTED); + } + + return all_successful; +} + +bool GpioToolController::check_states( + const rclcpp::Time & current_time, + const std::unordered_map> & states, + const std::string & output_prefix, const uint8_t next_transition, const bool warning_output) +{ + bool all_correct = true; + if (!states.empty()) // only check the states if they are present + { + for (const auto & [name, pair] : states) + { + const auto & [value, index] = pair; + RCLCPP_DEBUG( + get_node()->get_logger(), + "%s: Looking for state interface '%s' on index %zu, and state size is %zu, and " + "state_interface size is %zu", + output_prefix.c_str(), state_interfaces_.at(index).get_name().c_str(), index, states.size(), + state_interfaces_.size()); + const double current_state_value = + state_interfaces_.at(index).get_optional().value_or( + std::numeric_limits::quiet_NaN()); + if (std::isnan(current_state_value) || abs(current_state_value - value) > params_.tolerance) + { + RCLCPP_WARN_EXPRESSION( + get_node()->get_logger(), warning_output, + "%s: State value for %s doesn't match. Is %.2f, expected %.2f", output_prefix.c_str(), + state_interfaces_.at(index).get_name().c_str(), current_state_value, value); + all_correct = false; + } + } + } + + if (all_correct) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: Transitioning after reaching state to: %d", + output_prefix.c_str(), next_transition); + // when canceling we don't continue transition + if (current_tool_action_.load() != ToolAction::CANCELING) + { + current_tool_transition_.store(next_transition); + } + } + else if ((current_time - state_change_start_).seconds() > params_.timeout) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s: Tool didin't reached target state within %.2f seconds. Try resetting the tool using " + "'~/reset_halted' service. After that set sensible state.", + output_prefix.c_str(), params_.timeout); + current_tool_transition_.store(GPIOToolTransition::HALTED); + } + + return all_correct; +} + +void GpioToolController::check_tool_state_and_switch( + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, + std::vector & joint_states, const size_t joint_states_start_index, + const std::string & output_prefix, const uint8_t next_transition, + std::string & target_and_found_state_name, const bool warning_output) +{ + for (const auto & [state_name, states] : ios.states) + { + if (!target_and_found_state_name.empty() && state_name != target_and_found_state_name) + { + // if we are looking for specific state and this is not it - skip + continue; + } + RCLCPP_DEBUG( + get_node()->get_logger(), + "%s: Checking state '%s' for tool. States _%s_ exist. Used number of state interfaces %zu.", + output_prefix.c_str(), state_name.c_str(), (states.size() ? "do" : "do not"), states.size()); + + if (check_states(current_time, states, output_prefix, next_transition, warning_output)) + { + RCLCPP_DEBUG( + get_node()->get_logger(), "%s: SUCCESS! state '%s' for tool is confirmed!", + output_prefix.c_str(), state_name.c_str()); + target_and_found_state_name = state_name; + const auto & js_val = ios.states_joint_states.at(state_name); + if (joint_states_start_index + js_val.size() <= joint_states.size()) + { + std::copy(js_val.begin(), js_val.end(), joint_states.begin() + joint_states_start_index); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), + "%s - CHECK_STATE: Joint states size (%zu) is not enough to copy the values for state " + "'%s' (size: %zu) from starting index %zu.", + output_prefix.c_str(), joint_states.size(), state_name.c_str(), js_val.size(), + joint_states_start_index); + } + break; + } + } +} + +void GpioToolController::handle_tool_state_transition( + const rclcpp::Time & current_time, const ToolTransitionIOs & ios, + const std::string & target_state, std::vector & joint_states, + const size_t joint_states_start_index, std::string & current_state) +{ + bool finish_transition_to_state = false; + switch (current_tool_transition_.load()) + { + case GPIOToolTransition::IDLE: + // reset time to avoid any time-source related crashing + state_change_start_ = current_time; + // do nothing + break; + + case GPIOToolTransition::HALTED: + if (reset_halted_.load()) + { + // check here the state of the tool if you can figure it out. If not - set unknown. + reset_halted_.store(false); + finish_transition_to_state = true; + } + break; + + case GPIOToolTransition::SET_BEFORE_COMMAND: + RCLCPP_DEBUG( + get_node()->get_logger(), + "%s - SET_BEFORE_COMMAND: Tool action is being set before command.", target_state.c_str()); + if (!transition_time_updated_.load()) + { + state_change_start_ = current_time; + transition_time_updated_.store(true); + } + set_commands( + ios.set_before_commands.at(target_state), target_state + " - SET_BEFORE_COMMAND", + GPIOToolTransition::CHECK_BEFORE_COMMAND); + break; + case GPIOToolTransition::CHECK_BEFORE_COMMAND: + check_states( + current_time, ios.set_before_states.at(target_state), + target_state + " - CHECK_BEFORE_COMMAND", GPIOToolTransition::SET_COMMAND); + break; + case GPIOToolTransition::SET_COMMAND: + set_commands( + ios.commands.at(target_state), target_state + " - SET_COMMAND", + GPIOToolTransition::CHECK_COMMAND); + break; + case GPIOToolTransition::CHECK_COMMAND: + // if the target state is not in the state map, any state is valid - + // used for engaging where multiple final state are possible + if (ios.states.find(target_state) == ios.states.end()) + { + current_state = ""; + } + else + { + current_state = target_state; + } + check_tool_state_and_switch( + current_time, ios, joint_states, joint_states_start_index, + target_state + " - CHECK_COMMAND", GPIOToolTransition::SET_AFTER_COMMAND, current_state); + break; + case GPIOToolTransition::SET_AFTER_COMMAND: + // after the main command we can end up in arbitrary state during engaging therefore we are + // using current state that is determined by checking the sensors. + set_commands( + ios.set_after_commands.at(current_state), target_state + " - SET_AFTER_COMMAND", + GPIOToolTransition::CHECK_AFTER_COMMAND); + break; + + case GPIOToolTransition::CHECK_AFTER_COMMAND: + if (check_states( + current_time, ios.set_after_states.at(current_state), + target_state + " - CHECK_AFTER_COMMAND", GPIOToolTransition::IDLE)) + { + finish_transition_to_state = true; + } + break; + + default: + break; + } + + if (finish_transition_to_state) + { + current_tool_action_.store(ToolAction::IDLE); + current_tool_transition_.store(GPIOToolTransition::IDLE); + transition_time_updated_.store(false); // resetting the flag + + RCLCPP_INFO( + get_node()->get_logger(), "%s: Tool state or configuration change finished!", + target_state.c_str()); + } +} + +bool GpioToolController::prepare_command_and_state_ios() +{ + auto parse_interfaces_from_params = + [&]( + const std::vector & interfaces, const std::vector & values, + const std::string & param_name, + std::unordered_map> & ios, + std::unordered_set & interface_list) -> void + { + if (interfaces.size() != values.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Size of interfaces and values parameters for '%s' must be the same. " + "Interfaces size: %zu, Values size: %zu", + param_name.c_str(), interfaces.size(), values.size()); + return; + } + + ios.reserve(interfaces.size()); + interface_list.reserve(interface_list.size() + interfaces.size()); + + for (size_t i = 0; i < interfaces.size(); ++i) + { + if (!interfaces[i].empty()) + { + ios[interfaces[i]].first = values[i]; + ios[interfaces[i]].second = -1; // -1 means not set yet + interface_list.insert(interfaces[i]); + } + } + }; + + // Disengaged IOs + disengaged_gpios_.possible_states.push_back(params_.disengaged.name); + + disengaged_gpios_.set_before_commands[params_.disengaged.name] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.disengaged.set_before_command.interfaces, params_.disengaged.set_before_command.values, + "disengaged.set_before_command", disengaged_gpios_.set_before_commands[params_.disengaged.name], + command_if_ios_); + + disengaged_gpios_.set_before_states[params_.disengaged.name] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.disengaged.set_before_state.interfaces, params_.disengaged.set_before_state.values, + "disengaged.set_before_state", disengaged_gpios_.set_before_states[params_.disengaged.name], + state_if_ios_); + + disengaged_gpios_.commands[params_.disengaged.name] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.disengaged.command.interfaces, params_.disengaged.command.values, "disengaged.command", + disengaged_gpios_.commands[params_.disengaged.name], command_if_ios_); + + disengaged_gpios_.states[params_.disengaged.name] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.disengaged.state.interfaces, params_.disengaged.state.values, "disengaged.state", + disengaged_gpios_.states[params_.disengaged.name], state_if_ios_); + + disengaged_gpios_.states_joint_states[params_.disengaged.name] = params_.disengaged.joint_states; + + disengaged_gpios_.set_after_commands[params_.disengaged.name] = + std::unordered_map>(); + disengaged_gpios_.set_after_states[params_.disengaged.name] = + std::unordered_map>(); + + parse_interfaces_from_params( + params_.disengaged.set_after_command.interfaces, params_.disengaged.set_after_command.values, + "disengaged.set_after_command", disengaged_gpios_.set_after_commands[params_.disengaged.name], + command_if_ios_); + parse_interfaces_from_params( + params_.disengaged.set_after_state.interfaces, params_.disengaged.set_after_state.values, + "disengaged.set_after_state", disengaged_gpios_.set_after_states[params_.disengaged.name], + state_if_ios_); + + // Engaged IOs + engaged_gpios_.set_before_commands[params_.engaged.name] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.engaged.set_before_command.interfaces, params_.engaged.set_before_command.values, + "engaged.set_before_command", engaged_gpios_.set_before_commands[params_.engaged.name], + command_if_ios_); + engaged_gpios_.set_before_states[params_.engaged.name] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.engaged.set_before_state.interfaces, params_.engaged.set_before_state.values, + "engaged.set_before_state", engaged_gpios_.set_before_states[params_.engaged.name], + state_if_ios_); + engaged_gpios_.commands[params_.engaged.name] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.engaged.command.interfaces, params_.engaged.command.values, "engaged.command", + engaged_gpios_.commands[params_.engaged.name], command_if_ios_); + + engaged_gpios_.possible_states = params_.possible_engaged_states; + for (const auto & state : params_.possible_engaged_states) + { + engaged_gpios_.states[state] = std::unordered_map>(); + parse_interfaces_from_params( + params_.engaged.states.possible_engaged_states_map.at(state).interfaces, + params_.engaged.states.possible_engaged_states_map.at(state).values, + "engaged.states." + state, engaged_gpios_.states[state], state_if_ios_); + + engaged_gpios_.states_joint_states[state] = + params_.engaged.states.possible_engaged_states_map.at(state).joint_states; + + engaged_gpios_.set_after_commands[state] = + std::unordered_map>(); + engaged_gpios_.set_after_states[state] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.engaged.states.possible_engaged_states_map.at(state).set_after_command_interfaces, + params_.engaged.states.possible_engaged_states_map.at(state).set_after_command_values, + "engaged.set_after_command." + state, engaged_gpios_.set_after_commands[state], + command_if_ios_); + parse_interfaces_from_params( + params_.engaged.states.possible_engaged_states_map.at(state).set_after_state_interfaces, + params_.engaged.states.possible_engaged_states_map.at(state).set_after_state_values, + "engaged.set_after_state." + state, engaged_gpios_.set_after_states[state], state_if_ios_); + } + + // Configurations IOs + reconfigure_gpios_.possible_states = params_.configurations; + for (const auto & state : reconfigure_gpios_.possible_states) + { + reconfigure_gpios_.set_before_commands[state] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).set_before_command_interfaces, + params_.configuration_setup.configurations_map.at(state).set_before_command_values, + "reconfigure.set_before_command." + state, reconfigure_gpios_.set_before_commands[state], + command_if_ios_); + + reconfigure_gpios_.set_before_states[state] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).set_before_state_interfaces, + params_.configuration_setup.configurations_map.at(state).set_before_state_values, + "reconfigure.set_before_state." + state, reconfigure_gpios_.set_before_states[state], + state_if_ios_); + + reconfigure_gpios_.commands[state] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).command_interfaces, + params_.configuration_setup.configurations_map.at(state).command_values, + "reconfigure.command." + state, reconfigure_gpios_.commands[state], command_if_ios_); + + reconfigure_gpios_.states[state] = std::unordered_map>(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).state_interfaces, + params_.configuration_setup.configurations_map.at(state).state_values, + "reconfigure.state." + state, reconfigure_gpios_.states[state], state_if_ios_); + + reconfigure_gpios_.states_joint_states[state] = + params_.configuration_setup.configurations_map.at(state).joint_states; + + reconfigure_gpios_.set_after_commands[state] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).set_after_command_interfaces, + params_.configuration_setup.configurations_map.at(state).set_after_command_values, + "reconfigure.set_after_command." + state, reconfigure_gpios_.set_after_commands[state], + command_if_ios_); + reconfigure_gpios_.set_after_states[state] = + std::unordered_map>(); + parse_interfaces_from_params( + params_.configuration_setup.configurations_map.at(state).set_after_state_interfaces, + params_.configuration_setup.configurations_map.at(state).set_after_state_values, + "reconfigure.set_after_state." + state, reconfigure_gpios_.set_after_states[state], + state_if_ios_); + } + + for (const auto & [name, data] : params_.sensors_interfaces.tool_specific_sensors_map) + { + state_if_ios_.insert(data.interface); + } + + auto store_indices_of_interfaces = + [&]( + std::unordered_map> & ios_map, + const std::unordered_set & interfaces) -> bool + { + for (auto & [itf_name, pair] : ios_map) + { + auto & [value, index] = pair; + auto it = std::find(interfaces.begin(), interfaces.end(), itf_name); + if (it != interfaces.end()) + { + index = std::distance(interfaces.begin(), it); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), "Interface '%s' not found in the list of interfaces.", + itf_name.c_str()); + return false; + } + } + return true; + }; + + bool ret = true; + + // Disengaged IOs + ret &= store_indices_of_interfaces( + disengaged_gpios_.set_before_commands[params_.disengaged.name], command_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.set_before_states[params_.disengaged.name], state_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.commands[params_.disengaged.name], command_if_ios_); + ret &= + store_indices_of_interfaces(disengaged_gpios_.states[params_.disengaged.name], state_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.set_after_commands[params_.disengaged.name], command_if_ios_); + ret &= store_indices_of_interfaces( + disengaged_gpios_.set_after_states[params_.disengaged.name], state_if_ios_); + + // Engaged IOs + ret &= store_indices_of_interfaces( + engaged_gpios_.set_before_commands[params_.engaged.name], command_if_ios_); + ret &= store_indices_of_interfaces( + engaged_gpios_.set_before_states[params_.engaged.name], state_if_ios_); + ret &= + store_indices_of_interfaces(engaged_gpios_.commands[params_.engaged.name], command_if_ios_); + for (const auto & state : params_.possible_engaged_states) + { + ret &= store_indices_of_interfaces(engaged_gpios_.states[state], state_if_ios_); + ret &= store_indices_of_interfaces(engaged_gpios_.set_after_commands[state], command_if_ios_); + ret &= store_indices_of_interfaces(engaged_gpios_.set_after_states[state], state_if_ios_); + } + + // Reconfigure IOs + for (const auto & state : reconfigure_gpios_.possible_states) + { + ret &= + store_indices_of_interfaces(reconfigure_gpios_.set_before_commands[state], command_if_ios_); + ret &= store_indices_of_interfaces(reconfigure_gpios_.set_before_states[state], state_if_ios_); + ret &= store_indices_of_interfaces(reconfigure_gpios_.commands[state], command_if_ios_); + ret &= store_indices_of_interfaces(reconfigure_gpios_.states[state], state_if_ios_); + ret &= + store_indices_of_interfaces(reconfigure_gpios_.set_after_commands[state], command_if_ios_); + ret &= store_indices_of_interfaces(reconfigure_gpios_.set_after_states[state], state_if_ios_); + } + + return ret; +} + +GpioToolController::EngagingSrvType::Response GpioToolController::process_engaging_request( + const ToolAction & requested_action, const std::string & requested_action_name) +{ + EngagingSrvType::Response response; + if (current_tool_action_.load() == ToolAction::RECONFIGURING) + { + response.success = false; + response.message = "Cannot engage the Tool while reconfiguring"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", response.message.c_str()); + return response; + } + + if (current_tool_action_.load() != ToolAction::IDLE) + { + const auto & current_action_name = (current_tool_action_.load() == ToolAction::ENGAGING) + ? params_.engaged.name + : params_.disengaged.name; + + if (current_tool_action_.load() != requested_action) + { + RCLCPP_WARN( + get_node()->get_logger(), "Stopping tool '%s' and starting '%s'.", + current_action_name.c_str(), requested_action_name.c_str()); + } + else + { + response.success = false; + response.message = + "Tool is already executing action '" + requested_action_name + "'. Nothing to do."; + RCLCPP_INFO(get_node()->get_logger(), "%s", response.message.c_str()); + return response; + } + } + else // if already in desired state - nothing to do + { + const std::string current_state = current_state_.get(); + if ( + (requested_action == ToolAction::ENGAGING && + (std::find( + params_.possible_engaged_states.begin(), params_.possible_engaged_states.end(), + current_state) != params_.possible_engaged_states.end())) || + (requested_action == ToolAction::DISENGAGING && current_state == params_.disengaged.name)) + { + response.success = true; + response.message = + "Tool is already in the desired state '" + requested_action_name + "'. Nothing to do."; + RCLCPP_INFO(get_node()->get_logger(), "%s", response.message.c_str()); + return response; + } + } + + current_tool_action_.store(requested_action); + current_tool_transition_.store(GPIOToolTransition::SET_BEFORE_COMMAND); + + response.success = true; + response.message = "Tool action '" + requested_action_name + "' started."; + return response; +} + +GpioToolController::EngagingSrvType::Response GpioToolController::process_reconfigure_request( + const std::string & config_name) +{ + EngagingSrvType::Response response; + response.success = true; + if (config_name.empty()) + { + response.success = false; + response.message = "Configuration name cannot be empty"; + } + if ( + response.success && + std::find(params_.configurations.begin(), params_.configurations.end(), config_name) == + params_.configurations.end()) + { + response.success = false; + response.message = "Configuration '" + config_name + "' does not exist"; + } + if (response.success && current_tool_action_.load() != ToolAction::IDLE) + { + response.success = false; + response.message = "Tool is currently reconfiguring or executing '" + params_.engaged.name + + "' or '" + params_.disengaged.name + + "' action.Please wait until it finishes."; + } + // This is OK to access `current_state_` as we are in the IDLE state and it is not being modified + if (response.success && current_state_.get() != params_.disengaged.name) + { + response.success = false; + response.message = "Tool can be reconfigured only in '" + params_.disengaged.name + + "' state. Current state is '" + current_state_.get() + "'."; + } + if (response.success) + { + current_tool_action_.store(ToolAction::RECONFIGURING); + current_tool_transition_.store(GPIOToolTransition::SET_BEFORE_COMMAND); + target_configuration_.set(config_name); + response.message = "Tool reconfiguration to '" + config_name + "' has started."; + RCLCPP_INFO(get_node()->get_logger(), "%s", response.message.c_str()); + } + else + { + RCLCPP_ERROR(get_node()->get_logger(), "%s", response.message.c_str()); + } + + return response; +} + +GpioToolController::EngagingSrvType::Response GpioToolController::service_wait_for_transition_end( + const std::string & requested_action_name) +{ + EngagingSrvType::Response response; + while (current_tool_action_.load() != ToolAction::IDLE) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (current_tool_transition_.load() == GPIOToolTransition::HALTED) + { + response.success = false; + response.message = "Tool action or reconfiguration '" + requested_action_name + + "' was halted. Reset it with '~/reset_halted' service."; + return response; + } + } + response.success = true; + response.message = + "Tool action or reconfiguration '" + requested_action_name + "' completed successfully."; + return response; +} + +controller_interface::CallbackReturn GpioToolController::prepare_publishers_and_services() +{ + if (!params_.use_action) + { + // callback groups for each service + disengaging_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + engaging_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + if (configuration_control_enabled_) + { + reconfigure_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + } + + auto disengaged_service_callback = + [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + auto result = process_engaging_request(ToolAction::DISENGAGING, params_.disengaged.name); + if (result.success) + { + result = service_wait_for_transition_end(params_.disengaged.name); + } + response->success = result.success; + response->message = result.message; + }; + + disengaged_service_ = get_node()->create_service( + "~/" + params_.disengaged.name, disengaged_service_callback, qos_services, + disengaging_service_callback_group_); + + auto engaged_service_callback = [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + auto result = process_engaging_request(ToolAction::ENGAGING, params_.engaged.name); + if (result.success) + { + result = service_wait_for_transition_end(params_.engaged.name); + } + response->success = result.success; + response->message = result.message; + }; + + engaged_service_ = get_node()->create_service( + "~/" + params_.engaged.name, engaged_service_callback, qos_services, + engaging_service_callback_group_); + + if (configuration_control_enabled_) + { + // configure tool service + auto reconfigure_tool_service_callback = + [&]( + const std::shared_ptr request, + std::shared_ptr response) + { + auto result = process_reconfigure_request(request->config_name); + + if (result.success) + { + result = service_wait_for_transition_end(request->config_name); + } + response->success = result.success; + response->message = result.message; + }; + reconfigure_tool_service_ = get_node()->create_service( + "~/reconfigure", reconfigure_tool_service_callback, qos_services, + reconfigure_service_callback_group_); + } + } + else + { + // disengaged close action server + engaging_action_server_ = rclcpp_action::create_server( + get_node(), "~/" + params_.engaged.name + "_" + params_.disengaged.name, + std::bind( + &GpioToolController::handle_engaging_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&GpioToolController::handle_engaging_cancel, this, std::placeholders::_1), + std::bind( + &GpioToolController::handle_action_accepted, this, + std::placeholders::_1)); + + if (configuration_control_enabled_) + { + // reconfigure action server + config_action_server_ = rclcpp_action::create_server( + get_node(), "~/reconfigure", + std::bind( + &GpioToolController::handle_config_goal, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&GpioToolController::handle_config_cancel, this, std::placeholders::_1), + std::bind( + &GpioToolController::handle_action_accepted, this, + std::placeholders::_1)); + } + } + + reset_service_callback_group_ = + get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto reset_callback = [&]( + const std::shared_ptr /*request*/, + std::shared_ptr response) + { + reset_halted_.store(true); + response->success = true; + }; + reset_service_ = get_node()->create_service( + "~/reset_halted", reset_callback, qos_services, reset_service_callback_group_); + + try + { + if (joint_states_need_publishing_) + { + t_js_publisher_ = get_node()->create_publisher( + "/joint_states", rclcpp::SystemDefaultsQoS()); + tool_joint_state_publisher_ = std::make_unique(t_js_publisher_); + } + + if_publisher_ = get_node()->create_publisher( + "~/dynamic_interfaces", rclcpp::SystemDefaultsQoS()); + interface_publisher_ = std::make_unique(if_publisher_); + + t_s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + controller_state_publisher_ = std::make_unique(t_s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + if (!params_.engaged.name.empty() || !params_.disengaged.name.empty()) + { + RCLCPP_INFO( + get_node()->get_logger(), + "No joints defined, so no joint states will be published, althrough the publisher is " + "initialized."); + } + + // if (joint_states_values_.size() == 0) + // { + // RCLCPP_DEBUG( + // get_node()->get_logger(), + // "Joint states values are empty, resizing to match the number (%zu) of engaged and + // configuration joints. This should not happen here, as the joint states should be set in the + // configure method.", params_.engaged_joints.size() + params_.configuration_joints.size()); + // joint_states_values_.resize( + // params_.engaged_joints.size() + params_.configuration_joints.size(), + // std::numeric_limits::quiet_NaN()); + // } + + if (joint_states_need_publishing_) + { + tool_joint_state_publisher_->msg_.name.reserve(joint_states_values_.size()); + tool_joint_state_publisher_->msg_.position = joint_states_values_; + + for (const auto & joint_name : params_.engaged_joints) + { + tool_joint_state_publisher_->msg_.name.push_back(joint_name); + } + for (const auto & joint_name : params_.configuration_joints) + { + tool_joint_state_publisher_->msg_.name.push_back(joint_name); + } + } + + interface_publisher_->msg_.states.interface_names.reserve(state_if_ios_.size()); + interface_publisher_->msg_.states.values.resize(state_if_ios_.size()); + interface_publisher_->msg_.commands.interface_names.reserve(command_if_ios_.size()); + interface_publisher_->msg_.commands.values.resize(command_if_ios_.size()); + for (const auto & state_io : state_if_ios_) + { + interface_publisher_->msg_.states.interface_names.push_back(state_io); + } + for (const auto & command_io : command_if_ios_) + { + interface_publisher_->msg_.commands.interface_names.push_back(command_io); + } + + controller_state_publisher_->msg_.state = current_state_.get(); + controller_state_publisher_->msg_.configuration = current_configuration_.get(); + controller_state_publisher_->msg_.current_transition.state = current_tool_transition_.load(); + + return controller_interface::CallbackReturn::SUCCESS; +} + +void GpioToolController::publish_topics(const rclcpp::Time & time) +{ + if (joint_states_need_publishing_) + { + if (tool_joint_state_publisher_ && tool_joint_state_publisher_->trylock()) + { + tool_joint_state_publisher_->msg_.header.stamp = time; + tool_joint_state_publisher_->msg_.position = joint_states_values_; + } + tool_joint_state_publisher_->unlockAndPublish(); + } + + if (interface_publisher_ && interface_publisher_->trylock()) + { + interface_publisher_->msg_.header.stamp = time; + for (size_t i = 0; i < state_interfaces_.size(); ++i) + { + interface_publisher_->msg_.states.values.at(i) = + static_cast(state_interfaces_.at(i).get_optional().value_or( + std::numeric_limits::quiet_NaN())); + } + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + interface_publisher_->msg_.commands.values.at(i) = + static_cast(command_interfaces_.at(i).get_optional().value_or( + std::numeric_limits::quiet_NaN())); + } + interface_publisher_->unlockAndPublish(); + } + if (controller_state_publisher_ && controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.state = current_state_.get(); + controller_state_publisher_->msg_.configuration = current_configuration_.get(); + controller_state_publisher_->msg_.current_transition.state = current_tool_transition_.load(); + controller_state_publisher_->unlockAndPublish(); + } +} + +rclcpp_action::GoalResponse GpioToolController::handle_engaging_goal( + const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr goal) +{ + auto engaging_action = ToolAction::DISENGAGING; + std::string action_name = params_.disengaged.name; + if (goal->engage) + { + engaging_action = ToolAction::ENGAGING; + action_name = params_.engaged.name; + } + + auto result = process_engaging_request(engaging_action, action_name); + + if (!result.success) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to process '%s_%s' request: %s", + params_.engaged.name.c_str(), params_.disengaged.name.c_str(), result.message.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GpioToolController::handle_engaging_cancel( + std::shared_ptr> /*goal_handle*/) +{ + RCLCPP_WARN( + get_node()->get_logger(), + "Tool action is being canceled, going to HALTED state. If you want to reset the Tool, use " + "'~/reset_halted' service."); + current_tool_action_.store(ToolAction::CANCELING); + return rclcpp_action::CancelResponse::ACCEPT; +} + +rclcpp_action::GoalResponse GpioToolController::handle_config_goal( + const rclcpp_action::GoalUUID & /*uuid*/, std::shared_ptr goal) +{ + auto result = process_reconfigure_request(goal->config_name); + if (!result.success) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Failed to process reconfigure request: %s", + result.message.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse GpioToolController::handle_config_cancel( + std::shared_ptr> /*goal_handle*/) +{ + RCLCPP_WARN( + get_node()->get_logger(), + "Tool action is being canceled, going to HALTED state. If you want to reset the Tool, use " + "'~/reset_halted' service."); + current_tool_action_.store(ToolAction::CANCELING); + return rclcpp_action::CancelResponse::ACCEPT; +} + +template +void GpioToolController::handle_action_accepted( + std::shared_ptr> goal_handle) +{ + auto result = std::make_shared(); + auto feedback = std::make_shared(); + + while (true) + { + if (current_tool_action_.load() == ToolAction::IDLE) + { + result->success = true; + result->resulting_state_name = current_state_.get(); + result->message = "Tool action successfully executed!"; + goal_handle->succeed(result); + break; + } + else if (current_tool_transition_.load() == GPIOToolTransition::HALTED) + { + result->success = false; + result->resulting_state_name = current_state_.get(); + result->message = + "Tool action canceled or halted! Check the error, reset the tool using '~/reset_halted' " + "service and set to sensible state."; + goal_handle->abort(result); + break; + } + else + { + feedback->transition.state = current_tool_transition_.load(); + goal_handle->publish_feedback(feedback); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } +} + +void GpioToolController::check_tool_state( + const rclcpp::Time & current_time, const bool warning_output) +{ + std::string current_state = ""; + check_tool_state_and_switch( + current_time, disengaged_gpios_, joint_states_values_, 0, + params_.disengaged.name + " - CHECK STATES", GPIOToolTransition::IDLE, current_state, + warning_output); + if (current_state.empty()) + { + check_tool_state_and_switch( + current_time, engaged_gpios_, joint_states_values_, 0, + params_.engaged.name + " - CHECK STATES", GPIOToolTransition::IDLE, current_state, + warning_output); + } + + current_state_.set(current_state); + + if (current_state.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tool state can not be determined, triggering CANCELING action and HALTED transition."); + current_tool_action_.store(ToolAction::CANCELING); + } + + if (configuration_control_enabled_) + { + std::string current_configuration = ""; + check_tool_state_and_switch( + current_time, reconfigure_gpios_, joint_states_values_, params_.engaged_joints.size(), + "reconfigure - CHECK STATES", GPIOToolTransition::IDLE, current_configuration, + warning_output); + + current_configuration_.set(current_configuration); + + if (current_configuration.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tool configuration can not be determined, triggering CANCELING action and HALTED " + "transition."); + current_tool_action_.store(ToolAction::CANCELING); + } + } +} + +} // namespace gpio_tool_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + gpio_tool_controller::GpioToolController, controller_interface::ControllerInterface) diff --git a/gpio_controllers/src/gpio_tool_controller.yaml b/gpio_controllers/src/gpio_tool_controller.yaml new file mode 100644 index 0000000000..48c64dde35 --- /dev/null +++ b/gpio_controllers/src/gpio_tool_controller.yaml @@ -0,0 +1,363 @@ +# Copyright (c) 2025, b»robotized by Stogl Robotics +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +gpio_tool_controller: + use_action: { + type: bool, + default_value: false, + description: "If set controllers provides actions instead of services for setting it to disengaged/engaged state and configurations." + } + timeout: { + type: double, + default_value: 5.0, + description: "Timeout for the waiting on signals from the tool about reached state.", + validation: { + gt<>: [0.0], + } + } + tolerance: { + type: double, + default_value: 0.01, + description: "Tolerance for the state values to be considered reached.", + validation: { + gt<>: [0.0], + } + } + engaged_joints: { + type: string_array, + default_value: [], + description: "List of joint names that should change values according to enaged state of the tool.", + validation: { + unique<>: null, + } + } + disengaged: + name: { + type: string, + default_value: "disengaged", + description: "Name of the disengaged state if you want to use different on in the application, e.g., 'open'.", + } + joint_states: { + type: double_array, + default_value: [], + description: "List of values that are published for the joint to `joint_states` topic when tool is disengaged. The order of the values must match the order of the joint names in `engaged_joints` parameter.", + } + set_before_command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value before disengageding the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set before disengageding the tool", + } + set_before_state: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that confirm the reached state 'before' disengaging the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for state interfaces that confirm the reached state 'before' disengaging the tool", + } + command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value to disengage the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set to disengage the tool", + } + state: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm the tool is disengaged", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm the tool is disengaged", + } + set_after_command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set after the tool is disengaged.", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set set after the tool is disengaged.", + } + set_after_state: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm 'after' the tool is disengaged.", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm 'after' the tool is disengaged.", + } + + possible_engaged_states: { + type: string_array, + description: "List of possible engaged states, e.g., a gripper can be `close_empty` or `close_full`.", + validation: { + unique<>: null, + not_empty<>: null, + } + } + + engaged: + name: { + type: string, + default_value: "engaged", + description: "Name of the engaged state if you want to use different on in the application, e.g., 'close'.", + } + set_before_command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value before engageing the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set before engageing the tool", + } + set_before_state: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that confirm the reached state 'before' engageing the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for state interfaces that confirm the reached state 'before' engageing the tool", + } + command: + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value to engage the tool", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set to engage the tool", + } + states: + __map_possible_engaged_states: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint values when tool is engaged. The order of the values must match the order of the joint names in engaged_joints parameter." + } + interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm the tool is engaged in a specific state.", + validation: { + unique<>: null, + } + } + values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm the tool is engaged in a specific state.", + } + set_after_command_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set after the tool is enaged in a specific state.", + validation: { + unique<>: null, + } + } + set_after_command_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set after the tool is enaged in a specific state.", + } + set_after_state_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm 'after' the tool is engaged in a specific state.", + validation: { + unique<>: null, + } + } + set_after_state_values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm 'after' the tool is engaged in a specific state.", + } + + configurations: { + type: string_array, + default_value: [], + description: "Configuration names that can be used to switch between different configurations of the tool", + validation: { + unique<>: null, + } + } + + configuration_joints: { + type: string_array, + default_value: [], + description: "List of joint names that are used to switch between different configurations of the tool", + validation: { + unique<>: null, + } + } + + configuration_setup: + __map_configurations: + joint_states: { + type: double_array, + default_value: [], + description: "List of joint states values that tool has in certain configurations. The order of the values must match the order of the joint names in `engaged_joints` parameter.", + } + set_before_command_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value before switching to a specific configuration.", + validation: { + unique<>: null, + } + } + set_before_command_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set before switching to a specific configuration.", + } + set_before_state_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that confirm the reached state 'before' switching to a specific configuration.", + validation: { + unique<>: null, + } + } + set_before_state_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for state interfaces that confirm the reached state 'before' switching to a specific configuration.", + } + command_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set to a specific value for switching to a specific configuration.", + validation: { + unique<>: null, + } + } + command_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set for switching to aa specific configuration.", + } + state_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm the tool is in a specific configuration.", + validation: { + unique<>: null, + } + } + state_values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm the tool is in a specific configuration.", + } + set_after_command_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of command interfaces that have to be set after switching to a specific configuration.", + validation: { + unique<>: null, + } + } + set_after_command_values: { + type: double_array, + default_value: [], + description: "(optional) list of values for command interfaces that have to be set after switching to a specific configuration.", + } + set_after_state_interfaces: { + type: string_array, + default_value: [], + description: "(optional) list of state interfaces that have to have a specific value to confirm 'after' switching to a specific configuration.", + validation: { + unique<>: null, + } + } + set_after_state_values: { + type: double_array, + default_value: [], + description: "(optional) list of values that state interfaces have to have to confirm 'after' switching to a specific configuration.", + } + + tool_specific_sensors: { + type: string_array, + default_value: [], + description: "List of sensor names that are specific to the tool", + validation: { + unique<>: null, + } + } + sensors_interfaces: + __map_tool_specific_sensors: + interface: { + type: string, + default_value: "", + description: "Name of the state interface that is specific to a tool sensor.", + } diff --git a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.cpp new file mode 100644 index 0000000000..9ee5d00d4b --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.cpp @@ -0,0 +1,30 @@ +// 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 +#include +#include +#include +#include + +#include "test_gpio_tool_controller.hpp" + +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/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.hpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.hpp new file mode 100644 index 0000000000..f4fbd0588e --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller.hpp @@ -0,0 +1,204 @@ +// 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 GPIO_TOOL_CONTROLLER__TEST_GPIO_TOOL_CONTROLLER_HPP_ +#define GPIO_TOOL_CONTROLLER__TEST_GPIO_TOOL_CONTROLLER_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include "gpio_controllers/gpio_tool_controller.hpp" + +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 TestableGpioToolController : public gpio_tool_controller::GpioToolController +{ + FRIEND_TEST(GpioToolControllerTest, AllParamsSetSuccess); + FRIEND_TEST(GpioToolControllerTest, AllParamNotSetFailure); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = gpio_tool_controller::GpioToolController::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(); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController( + const std::string controller_name = "test_gpio_tool_controller", + const std::vector & parameters = {}) + { + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "initializing controller"); + auto node_options = controller_->define_custom_node_options(); + node_options.parameter_overrides(parameters); + + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", node_options), + controller_interface::return_type::OK); + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "initialized successfully"); + } + + void setup_parameters() + { + controller_->get_node()->set_parameter({"use_action", true}); + controller_->get_node()->set_parameter({"timeout", 5.0}); + controller_->get_node()->set_parameter({"tolerance", 0.00001}); + controller_->get_node()->set_parameter( + {"engaged_joints", std::vector{"gripper_clamp_jaw"}}); + + // Disengaged state + controller_->get_node()->set_parameter({"disengaged.name", std::string("open")}); + controller_->get_node()->set_parameter({"disengaged.joint_states", std::vector{0.0}}); + controller_->get_node()->set_parameter( + {"disengaged.set_before_command.interfaces", + std::vector{"Release_Break_valve", "Release_Something"}}); + controller_->get_node()->set_parameter( + {"disengaged.set_before_command.values", std::vector{1.0, 0.0}}); + controller_->get_node()->set_parameter( + {"disengaged.set_before_state.interfaces", std::vector{"Break_Engaged"}}); + controller_->get_node()->set_parameter( + {"disengaged.set_before_state.values", std::vector{0.0}}); + controller_->get_node()->set_parameter( + {"disengaged.command.interfaces", std::vector{"Open_valve", "Close_valve"}}); + controller_->get_node()->set_parameter( + {"disengaged.command.values", std::vector{1.0, 0.0}}); + controller_->get_node()->set_parameter( + {"disengaged.state.interfaces", std::vector{"Opened_signal", "Closed_signal"}}); + controller_->get_node()->set_parameter( + {"disengaged.state.values", std::vector{1.0, 0.0}}); + controller_->get_node()->set_parameter( + {"disengaged.set_after_command.interfaces", + std::vector{"Release_Break_valve", "Release_Something"}}); + controller_->get_node()->set_parameter( + {"disengaged.set_after_command.values", std::vector{0.0, 1.0}}); + controller_->get_node()->set_parameter( + {"disengaged.set_after_state.interfaces", std::vector{"Break_Engaged"}}); + controller_->get_node()->set_parameter( + {"disengaged.set_after_state.values", std::vector{1.0}}); + + // Possible engaged states + controller_->get_node()->set_parameter( + {"possible_engaged_states", std::vector{"close_empty", "close_full"}}); + + // Engaged state + controller_->get_node()->set_parameter( + {"engaged.set_before_command.interfaces", + std::vector{"Release_Break_valve", "Release_Something"}}); + controller_->get_node()->set_parameter( + {"engaged.set_before_command.values", std::vector{1.0, 0.0}}); + controller_->get_node()->set_parameter( + {"engaged.set_before_state.interfaces", std::vector{"Break_Engaged"}}); + controller_->get_node()->set_parameter( + {"engaged.set_before_state.values", std::vector{0.0}}); + controller_->get_node()->set_parameter( + {"engaged.command.interfaces", std::vector{"Close_valve", "Open_valve"}}); + controller_->get_node()->set_parameter( + {"engaged.command.values", std::vector{1.0, 0.0}}); + + // Engaged states: close_empty + controller_->get_node()->set_parameter( + {"engaged.states.close_empty.joint_states", std::vector{0.16}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_empty.interfaces", + std::vector{"Closed_signal", "Part_Grasped_signal"}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_empty.values", std::vector{1.0, 0.0}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_empty.set_after_command_interfaces", + std::vector{"Release_Something", "Release_Break_valve"}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_empty.set_after_command_values", std::vector{1.0, 0.0}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_empty.set_after_state_interfaces", + std::vector{"Break_Engaged"}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_empty.set_after_state_values", std::vector{1.0}}); + + // Engaged states: close_full + controller_->get_node()->set_parameter( + {"engaged.states.close_full.joint_states", std::vector{0.08}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_full.interfaces", + std::vector{"Closed_signal", "Part_Grasped_signal"}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_full.values", std::vector{0.0, 1.0}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_full.set_after_command_interfaces", + std::vector{"Release_Something", "Release_Break_valve"}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_full.set_after_command_values", std::vector{1.0, 0.0}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_full.set_after_state_interfaces", + std::vector{"Break_Engaged"}}); + controller_->get_node()->set_parameter( + {"engaged.states.close_full.set_after_state_values", std::vector{1.0}}); + + // Configurations + controller_->get_node()->set_parameter({"configurations", std::vector{}}); + controller_->get_node()->set_parameter({"configuration_joints", std::vector{}}); + + // Tool specific sensors + controller_->get_node()->set_parameter({"tool_specific_sensors", std::vector{}}); + } + +protected: + const std::vector possible_engaged_states = {"close_empty", "close_full"}; + + // Test related parameters + std::unique_ptr controller_; +}; + +class GpioToolControllerTest : public IOGripperControllerFixture +{ +}; +#endif // GPIO_TOOL_CONTROLLER__TEST_GPIO_TOOL_CONTROLLER_HPP_ diff --git a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_all_param_set.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_all_param_set.cpp new file mode 100644 index 0000000000..bbb2ba7580 --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_all_param_set.cpp @@ -0,0 +1,76 @@ +// 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 +#include +#include +#include +#include + +#include +#include "test_gpio_tool_controller.hpp" + +// Test setting all params and getting success +TEST_F(GpioToolControllerTest, AllParamsSetSuccess) +{ + SetUpController( + "test_gpio_tool_controller", + {rclcpp::Parameter("possible_engaged_states", possible_engaged_states)}); + setup_parameters(); + + // configure success. + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); + + RCLCPP_INFO(rclcpp::get_logger("GpioToolControllerTest"), "Setup parameters successfully"); +} + +// Test not setting the one param and getting failure +TEST_F(GpioToolControllerTest, AllParamNotSetFailure) +{ + SetUpController( + "test_gpio_tool_controller", + {rclcpp::Parameter("possible_engaged_states", possible_engaged_states)}); + + // Engaged joint size not equal to Disengaged joint states size + controller_->get_node()->set_parameter({"engaged_joints", std::vector{}}); + controller_->get_node()->set_parameter({"disengaged.joint_states", std::vector{0.0}}); + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::FAILURE); + + controller_->get_node()->set_parameter( + {"engaged_joints", std::vector{"gripper_clamp_jaw"}}); + + // engaged.close_empty state + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::FAILURE); + + controller_->get_node()->set_parameter( + {"engaged.states.close_empty.joint_states", std::vector{0.16}}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::FAILURE); + + // engaged.close_full state + controller_->get_node()->set_parameter( + {"engaged.states.close_full.joint_states", std::vector{0.08}}); + + ASSERT_EQ( + controller_->on_configure(rclcpp_lifecycle::State()), + controller_interface::CallbackReturn::SUCCESS); +} diff --git a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_close.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_close.cpp new file mode 100644 index 0000000000..407b904014 --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_close.cpp @@ -0,0 +1,137 @@ +// 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 +#include +#include +#include +#include + +#include +#include "test_io_gripper_controller.hpp" + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, CloseGripperService) +{ + 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/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_gripper_example.yaml b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_gripper_example.yaml new file mode 100644 index 0000000000..ee0fa6a2a0 --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_gripper_example.yaml @@ -0,0 +1,82 @@ +test_gpio_tool_controller: + ros__parameters: + use_action: true + timeout: 5.0 + tolerance: 0.00001 # binary data on the interfaces therefore small tolerance + engaged_joints: [gripper_clamp_jaw] + + disengaged: + name: 'open' + joint_states: [0.0] + set_before_command: + interfaces: [Release_Break_valve, Release_Something] + values: [1.0, 0.0] + set_before_state: + interfaces: [Break_Engaged] + values: [0.0] + command: + interfaces: [Open_valve, Close_valve] + values: [1.0, 0.0] + state: + interfaces: [Opened_signal, Closed_signal] + values: [1.0, 0.0] + set_after_command: + interfaces: [Release_Break_valve, Release_Something] + values: [0.0, 1.0] + set_after_state: + interfaces: [Break_Engaged] + values: [1.0] + + possible_engaged_states: ['close_empty', 'close_full'] + + engaged: + set_before_command: + interfaces: [Release_Break_valve, Release_Something] + low: [1.0, 0.0] + set_before_state: + interfaces: [Break_Engaged] + values: [0.0] + command: + interfaces: [Close_valve, Open_valve] + values: [1.0, 0.0] + states: + close_empty: + joint_states: [0.16] + interfaces: [Closed_signal, Part_Grasped_signal] + values: [1.0, 0.0] + set_after_command_interfaces: [Release_Something, Release_Break_valve] + set_after_command_values: [1.0, 0.0] + set_after_state_interfaces: [Break_Engaged] + set_after_state_values: [1.0] + close_full: + joint_states: [0.08] + interfaces: [Closed_signal, Part_Grasped_signal] + values: [0.0, 1.0] + set_after_command_interfaces: [Release_Something, Release_Break_valve] + set_after_command_values: [1.0, 0.0] + set_after_state_interfaces: [Break_Engaged] + set_after_state_values: [1.0] + + configurations: ["narrow_objects", "wide_objects"] + configuration_joints: [gripper_distance_joint] + + configuration_setup: + narrow_objects: + joint_states: [0.1] + command_interfaces: [Narrow_Configuration_Cmd, Wide_Configuration_Cmd] + command_values: [1.0, 0.0] + state_interfaces: [Narrow_Configuraiton_Signal, Wide_Configuration_Signal] + state_values: [1.0, 0.0] + wide_objects: + joint_states: [0.2] + command_interfaces: [Narrow_Configuration_Cmd, Wide_Configuration_Cmd] + command_values: [0.0, 1.0] + state_interfaces: [Narrow_Configuraiton_Signal, Wide_Configuration_Signal] + state_values: [0.0, 1.0] + + tool_specific_sensors: ["part_sensor_top", "part_sensor"] + sensors_interfaces: + part_sensor_top: + interface: "Part_Sensor_Top_signal" + part_sensor: + interface: "Part_Grasped_signal" diff --git a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml new file mode 100644 index 0000000000..714029c705 --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_lift_example.yaml @@ -0,0 +1,37 @@ +test_io_gripper_controller: + ros__parameters: + use_action: true + timeout: 5.0 + tolerance: 0.05 + engaged_joints: [lift_joint] + + disengaged: + name: 'low' + joint_states: [0.0] + command: + interfaces: [SetLowCommand] + values: [1.0] + state: + interfaces: [LiftState] + values: [0.0] + + possible_engaged_states: ['high_empty', 'high_payload'] + + engaged: + command: + interfaces: [SetHighCommand] + values: [1.0] + states: + high_empty: + joint_states: [1.5] + interfaces: [LiftState, PaylodDetected] + values: [1.5, 0.0] + high_payload: + joint_states: [1.48] + interfaces: [LiftState, PaylodDetected] + values: [1.5, 1.0] + + gripper_specific_sensors: ["payload_detected"] + sensors_interfaces: + payload_detected: + interface: "PaylodDetected" diff --git a/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open.cpp new file mode 100644 index 0000000000..6472eb1550 --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open.cpp @@ -0,0 +1,125 @@ +// 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 +#include +#include +#include +#include + +#include +#include "test_io_gripper_controller.hpp" + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, OpenGripperService) +{ + 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/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open_close_action.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open_close_action.cpp new file mode 100644 index 0000000000..da3b496509 --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_open_close_action.cpp @@ -0,0 +1,224 @@ +// 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 +#include +#include +#include +#include + +#include +#include "test_io_gripper_controller.hpp" + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, OpenCloseGripperAction) +{ + 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/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure.cpp new file mode 100644 index 0000000000..7cd857e67a --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure.cpp @@ -0,0 +1,101 @@ +// 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 +#include +#include +#include +#include + +#include +#include "test_io_gripper_controller.hpp" + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, ReconfigureGripperService) +{ + 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/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure_action.cpp b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure_action.cpp new file mode 100644 index 0000000000..8da397faf5 --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_gpio_tool_controller_reconfigure_action.cpp @@ -0,0 +1,110 @@ +// 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 +#include +#include +#include +#include + +#include +#include "test_io_gripper_controller.hpp" + +// Test open gripper service sets command its as expected and publishes msg +TEST_F(IOGripperControllerTest, ReconfigureGripperAction) +{ + 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/gpio_controllers/test/gpio_tool_controller/test_load_gpio_tool_controller.cpp b/gpio_controllers/test/gpio_tool_controller/test_load_gpio_tool_controller.cpp new file mode 100644 index 0000000000..a7bb98cbee --- /dev/null +++ b/gpio_controllers/test/gpio_tool_controller/test_load_gpio_tool_controller.cpp @@ -0,0 +1,46 @@ +// 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 + +#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"); + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + "/test_io_gripper_controller.yaml"; + + cm.set_parameter({"test_io_gripper_controller.params_file", test_file_path}); + + cm.set_parameter( + {"test_io_gripper_controller.type", "io_gripper_controller/IOGripperController"}); + + ASSERT_NE(cm.load_controller("test_io_gripper_controller"), nullptr); + + rclcpp::shutdown(); +} diff --git a/gpio_controllers/test/test_load_gpio_tool_controller.cpp b/gpio_controllers/test/test_load_gpio_tool_controller.cpp new file mode 100644 index 0000000000..c8a15296b6 --- /dev/null +++ b/gpio_controllers/test/test_load_gpio_tool_controller.cpp @@ -0,0 +1,44 @@ +// Copyright 2025 b»robotized Group +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadGpioCommandController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + const std::string test_file_path = + std::string(TEST_FILES_DIRECTORY) + + "/gpio_tool_controller/test_gpio_tool_controller_gripper_example.yaml"; + + cm.set_parameter({"test_gpio_tool_controller.params_file", test_file_path}); + + ASSERT_NE( + cm.load_controller("test_gpio_tool_controller", "gpio_tool_controller/GpioToolController"), + nullptr); + + rclcpp::shutdown(); +}