diff --git a/motion_primitives_forward_controller/CMakeLists.txt b/motion_primitives_forward_controller/CMakeLists.txt new file mode 100644 index 0000000000..0f5ab87f9d --- /dev/null +++ b/motion_primitives_forward_controller/CMakeLists.txt @@ -0,0 +1,120 @@ +cmake_minimum_required(VERSION 3.8) +project(motion_primitives_forward_controller) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + industrial_robot_motion_interfaces +) + +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library(motion_primitives_forward_controller_parameters + src/motion_primitives_forward_controller.yaml +) +add_library( + motion_primitives_forward_controller + SHARED + src/motion_primitives_forward_controller.cpp +) +target_include_directories(motion_primitives_forward_controller PUBLIC + $ + $ +) +target_link_libraries(motion_primitives_forward_controller PUBLIC + motion_primitives_forward_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} + ${std_srvs_TARGETS} + ${industrial_robot_motion_interfaces_TARGETS} +) + +target_compile_definitions(motion_primitives_forward_controller PRIVATE "MOTION_PRIMITIVES_FORWARD_CONTROLLER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface motion_primitives_forward_controller.xml) + +install( + TARGETS + motion_primitives_forward_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + ament_add_gmock( + test_load_motion_primitives_forward_controller + test/test_load_motion_primitives_forward_controller.cpp + ) + target_link_libraries(test_load_motion_primitives_forward_controller + motion_primitives_forward_controller + controller_manager::controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_motion_primitives_forward_controller + test/test_motion_primitives_forward_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/motion_primitives_forward_controller_params.yaml + ) + target_link_libraries(test_motion_primitives_forward_controller + motion_primitives_forward_controller + controller_interface::controller_interface + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets + ) + + add_rostest_with_parameters_gmock( + test_motion_primitives_forward_controller_preceeding + test/test_motion_primitives_forward_controller_preceeding.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/motion_primitives_forward_controller_preceeding_params.yaml + ) + target_link_libraries(test_motion_primitives_forward_controller_preceeding + motion_primitives_forward_controller + controller_interface::controller_interface + hardware_interface::hardware_interface + ros2_control_test_assets::ros2_control_test_assets + ) +endif() + +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + motion_primitives_forward_controller +) + +ament_package() diff --git a/motion_primitives_forward_controller/README.md b/motion_primitives_forward_controller/README.md new file mode 100644 index 0000000000..54a62d5b14 --- /dev/null +++ b/motion_primitives_forward_controller/README.md @@ -0,0 +1,35 @@ +motion_primitives_forward_controller +========================================== + +Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC) + +[![Licence](https://img.shields.io/badge/License-Apache-2.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) + + +This project provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotion.action` action from the [industrial_robot_motion_interfaces](https://github.com/UniversalRobots/industrial_robot_motion_interfaces) package. The controller receives the primitives via the action interface and forwards them through command interfaces to the robot-specific hardware interface. Currently, hardware interfaces for [Universal Robots](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and [KUKA Robots](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver) are implemented. + +- Supported motion primitives: + - `LINEAR_JOINT` + - `LINEAR_CARTESIAN` + - `CIRCULAR_CARTESIAN` + +If multiple motion primitives are passed to the controller via the action, the controller forwards them to the hardware interface as a sequence. To do this, it first sends `MOTION_SEQUENCE_START`, followed by each individual primitive, and finally `MOTION_SEQUENCE_END`. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives. + +The action interface also allows stopping the current execution of motion primitives. When a stop request is received, the controller sends `STOP_MOTION` to the hardware interface, which then halts the robot's movement. Once the controller receives confirmation that the robot has stopped, it sends `RESET_STOP` to the hardware interface. After that, new commands can be sent. + +![Action Image](doc/Moprim_Controller_ExecuteMotion_Action.drawio.png) + +This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/examples/send_dummy_motion_primitives_hka_ur10e.py) in the `Universal_Robots_ROS2_Driver` package. + +## Architecture Overview +The following diagram shows the architecture for a UR robot. +For this setup, the [motion primitives mode of the `Universal_Robots_ROS2_Driver`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) is used. + +![UR Robot Architecture](doc/ros2_control_motion_primitives_ur_integrated.drawio.png) + + +# TODOs/ improvements +- Use references for command and state interfaces to improve code readability and less error-prone. +- Extend the tests + - Test for a sequence of multiple primitives + - Test for canceling movement diff --git a/motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio.png b/motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio.png new file mode 100644 index 0000000000..9b12b4ee2a Binary files /dev/null and b/motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio.png differ diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur_integrated.drawio.png b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur_integrated.drawio.png new file mode 100644 index 0000000000..ff591c8901 Binary files /dev/null and b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur_integrated.drawio.png differ diff --git a/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp new file mode 100644 index 0000000000..d6363cacc0 --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp @@ -0,0 +1,123 @@ +// Copyright (c) 2025, b»robotized +// +// 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. +// +// Authors: Mathias Fuhrer + +#ifndef MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ +#define MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include +#include "controller_interface/controller_interface.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 "industrial_robot_motion_interfaces/action/execute_motion.hpp" +#include "industrial_robot_motion_interfaces/msg/motion_primitive.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +namespace motion_primitives_forward_controller +{ +enum class ExecutionState : uint8_t +{ + IDLE = 0, + EXECUTING = 1, + SUCCESS = 2, + ERROR = 3, + STOPPED = 4 +}; + +enum class MotionType : uint8_t +{ + LINEAR_JOINT = 10, + LINEAR_CARTESIAN = 50, + CIRCULAR_CARTESIAN = 51, + + STOP_MOTION = 66, + RESET_STOP = 67, + + MOTION_SEQUENCE_START = 100, + MOTION_SEQUENCE_END = 101 +}; + +enum class ReadyForNewPrimitive : uint8_t +{ + NOT_READY = 0, + READY = 1 +}; + +class MotionPrimitivesForwardController : public controller_interface::ControllerInterface +{ +public: + MotionPrimitivesForwardController(); + + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::shared_ptr param_listener_; + motion_primitives_forward_controller::Params params_; + + using MotionPrimitive = industrial_robot_motion_interfaces::msg::MotionPrimitive; + std::queue> moprim_queue_; + + using ExecuteMotion = industrial_robot_motion_interfaces::action::ExecuteMotion; + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::GoalResponse goal_received_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + void goal_accepted_callback( + const std::shared_ptr> goal_handle); + std::shared_ptr> pending_action_goal_; + + void reset_command_interfaces(); + bool set_command_interfaces(); + + bool print_error_once_ = true; + // cancel requested by the action server + std::atomic cancel_requested_ = false; + // robot stop command sent to the hardware interface + std::atomic robot_stop_requested_ = false; + bool was_executing_ = false; + ExecutionState execution_status_; + ReadyForNewPrimitive ready_for_new_primitive_; + + std::atomic moprim_queue_write_enabled_ = false; +}; + +} // namespace motion_primitives_forward_controller + +#endif // MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ diff --git a/motion_primitives_forward_controller/motion_primitives_forward_controller.xml b/motion_primitives_forward_controller/motion_primitives_forward_controller.xml new file mode 100644 index 0000000000..eab3a4113f --- /dev/null +++ b/motion_primitives_forward_controller/motion_primitives_forward_controller.xml @@ -0,0 +1,8 @@ + + + + MotionPrimitivesForwardController ros2_control controller. + + + diff --git a/motion_primitives_forward_controller/package.xml b/motion_primitives_forward_controller/package.xml new file mode 100644 index 0000000000..d4406cc736 --- /dev/null +++ b/motion_primitives_forward_controller/package.xml @@ -0,0 +1,43 @@ + + + + motion_primitives_forward_controller + 5.2.0 + Package to control robots using motion primitives like PTP, LIN and CIRC + + Bence Magyar + Denis Štogl + Christoph Froehlich + Sai Kishor Kothakota + + Apache License 2.0 + + https://control.ros.org + https://github.com/ros-controls/ros2_controllers/issues + https://github.com/ros-controls/ros2_controllers/ + + Mathias Fuhrer + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + industrial_robot_motion_interfaces + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + + ament_cmake_gmock + controller_manager + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp new file mode 100644 index 0000000000..a3ea102f39 --- /dev/null +++ b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp @@ -0,0 +1,512 @@ +// Copyright (c) 2025, b»robotized +// +// 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. +// +// Authors: Mathias Fuhrer + +#include "motion_primitives_forward_controller/motion_primitives_forward_controller.hpp" +#include +#include +#include +#include +#include "controller_interface/helpers.hpp" + +namespace motion_primitives_forward_controller +{ +MotionPrimitivesForwardController::MotionPrimitivesForwardController() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::CallbackReturn MotionPrimitivesForwardController::on_init() +{ + RCLCPP_DEBUG(get_node()->get_logger(), "Initializing Motion Primitives Forward Controller"); + + 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 MotionPrimitivesForwardController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_DEBUG(get_node()->get_logger(), "Configuring Motion Primitives Forward Controller"); + + params_ = param_listener_->get_params(); + + // Check if there are exactly 25 command interfaces + if (params_.command_interfaces.size() != 25) + { // motion_type + 6 joints + 2*7 positions + blend_radius + velocity + acceleration + move_time + RCLCPP_ERROR( + get_node()->get_logger(), "Error: Exactly 25 command interfaces must be provided!"); + return controller_interface::CallbackReturn::ERROR; + } + + // Check if there are exactly 2 state interfaces + if (params_.state_interfaces.size() != 2) + { // execution_status + ready_for_new_primitive + RCLCPP_ERROR(get_node()->get_logger(), "Error: Exactly two state interfaces must be provided!"); + return controller_interface::CallbackReturn::ERROR; + } + + action_server_ = rclcpp_action::create_server( + get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), + get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), + std::string(get_node()->get_name()) + "/motion_sequence", + std::bind( + &MotionPrimitivesForwardController::goal_received_callback, this, std::placeholders::_1, + std::placeholders::_2), + std::bind( + &MotionPrimitivesForwardController::goal_cancelled_callback, this, std::placeholders::_1), + std::bind( + &MotionPrimitivesForwardController::goal_accepted_callback, this, std::placeholders::_1)); + + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +MotionPrimitivesForwardController::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(params_.command_interfaces.size()); + + // Iterate over all command interfaces from the config yaml file + for (const auto & interface_name : params_.command_interfaces) + { + command_interfaces_config.names.push_back("motion_primitive/" + interface_name); + } + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +MotionPrimitivesForwardController::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(params_.state_interfaces.size()); + + // Iterate over all state interfaces from the config yaml file + for (const auto & interface_name : params_.state_interfaces) + { + state_interfaces_config.names.push_back("motion_primitive/" + interface_name); + } + return state_interfaces_config; +} + +controller_interface::CallbackReturn MotionPrimitivesForwardController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reset_command_interfaces(); + moprim_queue_write_enabled_ = true; + RCLCPP_DEBUG(get_node()->get_logger(), "Controller activated"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MotionPrimitivesForwardController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reset_command_interfaces(); + RCLCPP_DEBUG(get_node()->get_logger(), "Controller deactivated"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type MotionPrimitivesForwardController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) +{ + if (cancel_requested_) + { + RCLCPP_INFO(get_node()->get_logger(), "Cancel requested, stopping execution."); + cancel_requested_ = false; + reset_command_interfaces(); + // send stop command immediately to the hw-interface + (void)command_interfaces_[0].set_value(static_cast(MotionType::STOP_MOTION)); + while (!moprim_queue_.empty()) + { // clear the queue + moprim_queue_.pop(); + } + robot_stop_requested_ = true; + } + + // read the status from the state interface + auto opt_value_execution = state_interfaces_[0].get_optional(); + if (!opt_value_execution.has_value()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "State interface 0 (execution_state) returned no value."); + return controller_interface::return_type::ERROR; + } + execution_status_ = + static_cast(static_cast(std::round(opt_value_execution.value()))); + switch (execution_status_) + { + case ExecutionState::IDLE: + print_error_once_ = true; + was_executing_ = false; + break; + case ExecutionState::EXECUTING: + if (!was_executing_) + { + was_executing_ = true; + } + print_error_once_ = true; + break; + + case ExecutionState::SUCCESS: + print_error_once_ = true; + + if (pending_action_goal_ && was_executing_) + { + was_executing_ = false; + auto result = std::make_shared(); + result->error_code = ExecuteMotion::Result::SUCCESSFUL; + result->error_string = "Motion primitives executed successfully"; + pending_action_goal_->succeed(result); + pending_action_goal_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "Motion primitives executed successfully."); + } + + break; + + case ExecutionState::STOPPED: + print_error_once_ = true; + was_executing_ = false; + + if (pending_action_goal_) + { + auto result = std::make_shared(); + result->error_code = ExecuteMotion::Result::CANCELED; + result->error_string = "Motion primitives execution canceled"; + pending_action_goal_->succeed(result); + pending_action_goal_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "Motion primitives execution canceled."); + } + + if (robot_stop_requested_) + { + // If the robot was stopped by a stop command, reset the command interfaces + // to allow new motion primitives to be sent. + reset_command_interfaces(); + (void)command_interfaces_[0].set_value(static_cast(MotionType::RESET_STOP)); + robot_stop_requested_ = false; + moprim_queue_write_enabled_ = true; + RCLCPP_INFO(get_node()->get_logger(), "Robot stopped, ready for new motion primitives."); + } + + break; + + case ExecutionState::ERROR: + was_executing_ = false; + + if (pending_action_goal_) + { + auto result = std::make_shared(); + result->error_code = ExecuteMotion::Result::FAILED; + result->error_string = "Motion primitives execution failed"; + pending_action_goal_->succeed(result); + pending_action_goal_.reset(); + RCLCPP_INFO(get_node()->get_logger(), "Motion primitives execution failed"); + } + + if (print_error_once_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Execution state: ERROR"); + print_error_once_ = false; + } + break; + + default: + RCLCPP_ERROR( + get_node()->get_logger(), "Error: Unknown execution status: %d", + static_cast(execution_status_)); + return controller_interface::return_type::ERROR; + } + + auto opt_value_ready = state_interfaces_[1].get_optional(); + if (!opt_value_ready.has_value()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "State interface 1 (ready_for_new_primitive) returned no value."); + return controller_interface::return_type::ERROR; + } + ready_for_new_primitive_ = + static_cast(static_cast(std::round(opt_value_ready.value()))); + + if (!moprim_queue_write_enabled_ && !cancel_requested_) + { + switch (ready_for_new_primitive_) + { + case ReadyForNewPrimitive::NOT_READY: + { + return controller_interface::return_type::OK; + } + case ReadyForNewPrimitive::READY: + { + if (moprim_queue_.empty()) // check if new command is available + { + // all primitives read, queue ready to get filled with new primitives + moprim_queue_write_enabled_ = true; + return controller_interface::return_type::OK; + } + else + { + if (!set_command_interfaces()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error: set_command_interfaces() failed"); + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; + } + } + default: + RCLCPP_ERROR( + get_node()->get_logger(), "Error: Unknown state for ready_for_new_primitive: %d", + static_cast(ready_for_new_primitive_)); + return controller_interface::return_type::ERROR; + } + } + return controller_interface::return_type::OK; +} + +// Reset Command-Interfaces to nan +void MotionPrimitivesForwardController::reset_command_interfaces() +{ + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN())) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to reset command interface %ld", i); + } + } +} + +// Set command interfaces from the message, gets called in the update function +bool MotionPrimitivesForwardController::set_command_interfaces() +{ + // Get the oldest message from the queue + std::shared_ptr current_moprim = moprim_queue_.front(); + moprim_queue_.pop(); + + // Check if the message is valid + if (!current_moprim) + { + RCLCPP_WARN(get_node()->get_logger(), "No valid reference message received"); + return false; + } + + // Set the motion_type + (void)command_interfaces_[0].set_value(static_cast(current_moprim->type)); + + // Process joint positions if available + if (!current_moprim->joint_positions.empty()) + { + for (size_t i = 0; i < current_moprim->joint_positions.size(); ++i) + { + (void)command_interfaces_[i + 1].set_value(current_moprim->joint_positions[i]); // q1 to q6 + } + } + + // Process Cartesian poses if available + if (!current_moprim->poses.empty()) + { + const auto & goal_pose = current_moprim->poses[0].pose; // goal pose + (void)command_interfaces_[7].set_value(goal_pose.position.x); // pos_x + (void)command_interfaces_[8].set_value(goal_pose.position.y); // pos_y + (void)command_interfaces_[9].set_value(goal_pose.position.z); // pos_z + (void)command_interfaces_[10].set_value(goal_pose.orientation.x); // pos_qx + (void)command_interfaces_[11].set_value(goal_pose.orientation.y); // pos_qy + (void)command_interfaces_[12].set_value(goal_pose.orientation.z); // pos_qz + (void)command_interfaces_[13].set_value(goal_pose.orientation.w); // pos_qw + + // Process via poses if available (only for circular motion) + if ( + current_moprim->type == static_cast(MotionType::CIRCULAR_CARTESIAN) && + current_moprim->poses.size() == 2) + { + const auto & via_pose = current_moprim->poses[1].pose; // via pose + (void)command_interfaces_[14].set_value(via_pose.position.x); // pos_via_x + (void)command_interfaces_[15].set_value(via_pose.position.y); // pos_via_y + (void)command_interfaces_[16].set_value(via_pose.position.z); // pos_via_z + (void)command_interfaces_[17].set_value(via_pose.orientation.x); // pos_via_qx + (void)command_interfaces_[18].set_value(via_pose.orientation.y); // pos_via_qy + (void)command_interfaces_[19].set_value(via_pose.orientation.z); // pos_via_qz + (void)command_interfaces_[20].set_value(via_pose.orientation.w); // pos_via_qw + } + } + + (void)command_interfaces_[21].set_value(current_moprim->blend_radius); // blend_radius + + // Read additional arguments + for (const auto & arg : current_moprim->additional_arguments) + { + if (arg.argument_name == "velocity") + { + (void)command_interfaces_[22].set_value(arg.argument_value); + } + else if (arg.argument_name == "acceleration") + { + (void)command_interfaces_[23].set_value(arg.argument_value); + } + else if (arg.argument_name == "move_time") + { + (void)command_interfaces_[24].set_value(arg.argument_value); + } + else + { + RCLCPP_WARN( + get_node()->get_logger(), "Unknown additional argument: %s", arg.argument_name.c_str()); + } + } + return true; +} + +rclcpp_action::GoalResponse MotionPrimitivesForwardController::goal_received_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received goal request"); + + const auto & primitives = goal->trajectory.motions; + + if (robot_stop_requested_) + { + RCLCPP_WARN(get_node()->get_logger(), "Robot requested to stop. Discarding the new command."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (!moprim_queue_write_enabled_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Queue is not ready to write. Discarding the new command."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (primitives.empty()) + { + RCLCPP_WARN(get_node()->get_logger(), "Goal rejected: no motion primitives provided."); + return rclcpp_action::GoalResponse::REJECT; + } + + for (size_t i = 0; i < primitives.size(); ++i) + { + const auto & primitive = primitives[i]; + + switch (static_cast(primitive.type)) + { + case MotionType::LINEAR_JOINT: + RCLCPP_INFO(get_node()->get_logger(), "Primitive %zu: LINEAR_JOINT (PTP)", i); + if (primitive.joint_positions.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Primitive %zu invalid: LINEAR_JOINT requires joint_positions.", i); + return rclcpp_action::GoalResponse::REJECT; + } + break; + + case MotionType::LINEAR_CARTESIAN: + RCLCPP_INFO(get_node()->get_logger(), "Primitive %zu: LINEAR_CARTESIAN (LIN)", i); + if (primitive.poses.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Primitive %zu invalid: LINEAR_CARTESIAN requires at least one pose.", i); + return rclcpp_action::GoalResponse::REJECT; + } + break; + + case MotionType::CIRCULAR_CARTESIAN: + RCLCPP_INFO(get_node()->get_logger(), "Primitive %zu: CIRCULAR_CARTESIAN (CIRC)", i); + if (primitive.poses.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Primitive %zu invalid: CIRCULAR_CARTESIAN requires exactly two poses.", i); + return rclcpp_action::GoalResponse::REJECT; + } + break; + + default: + RCLCPP_ERROR( + get_node()->get_logger(), "Primitive %zu invalid: unknown motion type %u.", i, + primitive.type); + return rclcpp_action::GoalResponse::REJECT; + } + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse MotionPrimitivesForwardController::goal_cancelled_callback( + const std::shared_ptr>) +{ + cancel_requested_ = true; + moprim_queue_write_enabled_ = false; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void MotionPrimitivesForwardController::goal_accepted_callback( + const std::shared_ptr> goal_handle) +{ + pending_action_goal_ = goal_handle; // Store the goal handle for later result feedback + + const auto & primitives = goal_handle->get_goal()->trajectory.motions; + + auto add_motions = [this](const std::vector & motion_primitives) + { + for (const auto & primitive : motion_primitives) + { + moprim_queue_.push(std::make_shared(primitive)); + } + }; + + if (primitives.size() > 1) + { + std::shared_ptr start_marker = std::make_shared(); + start_marker->type = static_cast(MotionType::MOTION_SEQUENCE_START); + moprim_queue_.push(start_marker); + + add_motions(primitives); + + std::shared_ptr end_marker = std::make_shared(); + end_marker->type = static_cast(MotionType::MOTION_SEQUENCE_END); + moprim_queue_.push(end_marker); + } + else + { + add_motions(primitives); + } + moprim_queue_write_enabled_ = false; + + RCLCPP_INFO( + get_node()->get_logger(), "Accepted goal with %zu motion primitives.", primitives.size()); +} + +} // namespace motion_primitives_forward_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + motion_primitives_forward_controller::MotionPrimitivesForwardController, + controller_interface::ControllerInterface) diff --git a/motion_primitives_forward_controller/src/motion_primitives_forward_controller.yaml b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.yaml new file mode 100644 index 0000000000..b0c0aaa38d --- /dev/null +++ b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.yaml @@ -0,0 +1,21 @@ +motion_primitives_forward_controller: + command_interfaces: { + type: string_array, + default_value: [], + description: "Names of the command interfaces used by the controller.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } + state_interfaces: { + type: string_array, + default_value: [], + description: "Names of the state interfaces used by the controller.", + read_only: true, + validation: { + unique<>: null, + not_empty<>: null, + } + } diff --git a/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml new file mode 100644 index 0000000000..6a46535909 --- /dev/null +++ b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml @@ -0,0 +1,31 @@ +test_motion_primitives_forward_controller: + ros__parameters: + command_interfaces: + - motion_type + - q1 + - q2 + - q3 + - q4 + - q5 + - q6 + - pos_x + - pos_y + - pos_z + - pos_qx + - pos_qy + - pos_qz + - pos_qw + - pos_via_x + - pos_via_y + - pos_via_z + - pos_via_qx + - pos_via_qy + - pos_via_qz + - pos_via_qw + - blend_radius + - velocity + - acceleration + - move_time + state_interfaces: + - execution_status + - ready_for_new_primitive diff --git a/motion_primitives_forward_controller/test/motion_primitives_forward_controller_preceeding_params.yaml b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_preceeding_params.yaml new file mode 100644 index 0000000000..6a46535909 --- /dev/null +++ b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_preceeding_params.yaml @@ -0,0 +1,31 @@ +test_motion_primitives_forward_controller: + ros__parameters: + command_interfaces: + - motion_type + - q1 + - q2 + - q3 + - q4 + - q5 + - q6 + - pos_x + - pos_y + - pos_z + - pos_qx + - pos_qy + - pos_qz + - pos_qw + - pos_via_x + - pos_via_y + - pos_via_z + - pos_via_qx + - pos_via_qy + - pos_via_qz + - pos_via_qw + - blend_radius + - velocity + - acceleration + - move_time + state_interfaces: + - execution_status + - ready_for_new_primitive diff --git a/motion_primitives_forward_controller/test/test_load_motion_primitives_forward_controller.cpp b/motion_primitives_forward_controller/test/test_load_motion_primitives_forward_controller.cpp new file mode 100644 index 0000000000..5024c67cad --- /dev/null +++ b/motion_primitives_forward_controller/test/test_load_motion_primitives_forward_controller.cpp @@ -0,0 +1,40 @@ +// Copyright (c) 2025, b»robotized +// +// 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(TestLoadMotionPrimitivesForwardController, load_controller) +{ + rclcpp::init(0, nullptr); + + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"); + + ASSERT_NO_THROW(cm.load_controller( + "test_motion_primitives_forward_controller", + "motion_primitives_forward_controller/MotionPrimitivesForwardController")); + + rclcpp::shutdown(); +} diff --git a/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp new file mode 100644 index 0000000000..c1c980c021 --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2025, b»robotized +// +// 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. +// +// Authors: Mathias Fuhrer + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "test_motion_primitives_forward_controller.hpp" + +class MotionPrimitivesForwardControllerTest +: public MotionPrimitivesForwardControllerFixture +{ +}; + +TEST_F(MotionPrimitivesForwardControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.command_interfaces.empty()); + ASSERT_TRUE(controller_->params_.state_interfaces.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.command_interfaces, testing::ElementsAreArray(command_interface_names_)); + ASSERT_THAT( + controller_->params_.state_interfaces, testing::ElementsAreArray(state_interface_names_)); +} + +TEST_F(MotionPrimitivesForwardControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], interface_namespace_ + "/" + command_interface_names_[i]); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], interface_namespace_ + "/" + state_interface_names_[i]); + } +} + +TEST_F(MotionPrimitivesForwardControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message queue is reset + auto moprim_queue_ = controller_->moprim_queue_; + ASSERT_TRUE(moprim_queue_.empty()); +} + +TEST_F(MotionPrimitivesForwardControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MotionPrimitivesForwardControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(MotionPrimitivesForwardControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto val_opt = controller_->command_interfaces_[0].get_optional(); + ASSERT_TRUE(std::isnan(val_opt.value())); + + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + val_opt = controller_->command_interfaces_[0].get_optional(); + ASSERT_TRUE(std::isnan(val_opt.value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(MotionPrimitivesForwardControllerTest, receive_single_action_goal) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + send_single_motion_sequence_goal(); // filling with default values from the function + + // Wait for the command value to be set + // This is necessary because the action server might take some time to process the goal + auto start = std::chrono::steady_clock::now(); + while (std::isnan(command_values_[1]) && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(5)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ( + command_values_[0], + static_cast( + motion_primitives_forward_controller::MotionType::LINEAR_JOINT)); // motion type + EXPECT_EQ(command_values_[1], 0.1); // q1 - q6 + EXPECT_EQ(command_values_[2], 0.2); + EXPECT_EQ(command_values_[3], 0.3); + EXPECT_EQ(command_values_[4], 0.4); + EXPECT_EQ(command_values_[5], 0.5); + EXPECT_EQ(command_values_[6], 0.6); + EXPECT_EQ(command_values_[21], 3.0); // blend radius + EXPECT_EQ(command_values_[22], 0.7); // velocity + EXPECT_EQ(command_values_[23], 1.0); // acceleration + EXPECT_EQ(command_values_[24], 2.0); // move time +} + +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/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp new file mode 100644 index 0000000000..d0e06ace19 --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp @@ -0,0 +1,220 @@ +// Copyright (c) 2025, b»robotized +// +// 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. +// +// Authors: Mathias Fuhrer + +#ifndef TEST_MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ +#define TEST_MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "motion_primitives_forward_controller/motion_primitives_forward_controller.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "industrial_robot_motion_interfaces/action/execute_motion.hpp" +#include "industrial_robot_motion_interfaces/msg/motion_primitive.hpp" + +using MotionPrimitive = industrial_robot_motion_interfaces::msg::MotionPrimitive; +using ExecuteMotion = industrial_robot_motion_interfaces::action::ExecuteMotion; + +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 TestableMotionPrimitivesForwardController +: public motion_primitives_forward_controller::MotionPrimitivesForwardController +{ + FRIEND_TEST(MotionPrimitivesForwardControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(MotionPrimitivesForwardControllerTest, activate_success); + FRIEND_TEST(MotionPrimitivesForwardControllerTest, reactivate_success); + FRIEND_TEST(MotionPrimitivesForwardControllerTest, receive_single_action_goal); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return motion_primitives_forward_controller::MotionPrimitivesForwardController::on_configure( + previous_state); + } +}; +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MotionPrimitivesForwardControllerFixture : public ::testing::Test +{ +public: + void SetUp() override + { + controller_ = std::make_unique(); + ASSERT_EQ( + controller_->init( + "test_motion_primitives_forward_controller", "", 0, "", + controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + node_ = std::make_shared("test_node"); + + executor_ = std::make_shared(); + executor_->add_node(node_); + executor_->add_node(controller_->get_node()->get_node_base_interface()); + + executor_thread_ = std::thread([this]() { executor_->spin(); }); + + action_client_ = rclcpp_action::create_client( + node_->get_node_base_interface(), node_->get_node_graph_interface(), + node_->get_node_logging_interface(), node_->get_node_waitables_interface(), + "/test_motion_primitives_forward_controller/motion_sequence"); + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + controller_.reset(); + node_.reset(); + executor_.reset(); + } + +protected: + void SetUpController( + const std::string controller_name = "test_motion_primitives_forward_controller") + { + std::vector command_ifs; + std::vector state_ifs; + + command_itfs_.clear(); + command_itfs_.reserve(command_values_.size()); + command_ifs.reserve(command_values_.size()); + + for (size_t i = 0; i < command_values_.size(); ++i) + { + command_itfs_.emplace_back( + hardware_interface::CommandInterface( + interface_namespace_, command_interface_names_[i], &command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + + state_itfs_.clear(); + state_itfs_.reserve(state_values_.size()); + state_ifs.reserve(state_values_.size()); + + for (size_t i = 0; i < state_values_.size(); ++i) + { + state_itfs_.emplace_back( + hardware_interface::StateInterface( + interface_namespace_, state_interface_names_[i], &state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void send_single_motion_sequence_goal( + const std::vector & joint_positions = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6}, + double velocity = 0.7, double acceleration = 1.0, double move_time = 2.0, + double blend_radius = 3.0) + { + std::cout << "Send motion sequence goal..." << std::endl; + + if (!action_client_->wait_for_action_server(std::chrono::seconds(5))) + { + throw std::runtime_error("Action server not available"); + } + + auto goal_msg = ExecuteMotion::Goal(); + MotionPrimitive primitive; + primitive.type = + static_cast(motion_primitives_forward_controller::MotionType::LINEAR_JOINT); + primitive.joint_positions = joint_positions; + primitive.blend_radius = blend_radius; + + primitive.additional_arguments.resize(3); + primitive.additional_arguments[0].argument_name = "velocity"; + primitive.additional_arguments[0].argument_value = velocity; + primitive.additional_arguments[1].argument_name = "acceleration"; + primitive.additional_arguments[1].argument_value = acceleration; + primitive.additional_arguments[2].argument_name = "move_time"; + primitive.additional_arguments[2].argument_value = move_time; + + goal_msg.trajectory.motions.push_back(primitive); + + auto goal_future = action_client_->async_send_goal(goal_msg); + + if (goal_future.wait_for(std::chrono::seconds(5)) != std::future_status::ready) + { + throw std::runtime_error("Failed to send goal (future timeout)"); + } + + auto goal_handle = goal_future.get(); + if (!goal_handle) + { + throw std::runtime_error("Goal was rejected by the action server"); + } + + std::cout << "Goal accepted by the action server." << std::endl; + } + + std::vector command_interface_names_ = { + "motion_type", "q1", "q2", "q3", "q4", + "q5", "q6", "pos_x", "pos_y", "pos_z", + "pos_qx", "pos_qy", "pos_qz", "pos_qw", "pos_via_x", + "pos_via_y", "pos_via_z", "pos_via_qx", "pos_via_qy", "pos_via_qz", + "pos_via_qw", "blend_radius", "velocity", "acceleration", "move_time"}; + + std::vector state_interface_names_ = {"execution_status", "ready_for_new_primitive"}; + + std::string interface_namespace_ = "motion_primitive"; + std::array state_values_ = { + static_cast(motion_primitives_forward_controller::ExecutionState::IDLE), + static_cast(motion_primitives_forward_controller::ReadyForNewPrimitive::READY)}; + std::array command_values_ = { + 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, + 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, + 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101}; + + std::vector state_itfs_; + std::vector command_itfs_; + + std::unique_ptr controller_; + rclcpp_action::Client::SharedPtr action_client_; + rclcpp::Node::SharedPtr node_; + std::shared_ptr executor_; + std::thread executor_thread_; +}; + +#endif // TEST_MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ diff --git a/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller_preceeding.cpp b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller_preceeding.cpp new file mode 100644 index 0000000000..c738b31c5c --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller_preceeding.cpp @@ -0,0 +1,73 @@ +// Copyright (c) 2025, b»robotized +// +// 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. +// +// Authors: Mathias Fuhrer + +#include "test_motion_primitives_forward_controller.hpp" + +#include +#include +#include +#include +#include + +class MotionPrimitivesForwardControllerTest +: public MotionPrimitivesForwardControllerFixture +{ +}; + +TEST_F(MotionPrimitivesForwardControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->params_.command_interfaces.empty()); + ASSERT_TRUE(controller_->params_.state_interfaces.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.command_interfaces, testing::ElementsAreArray(command_interface_names_)); + ASSERT_THAT( + controller_->params_.state_interfaces, testing::ElementsAreArray(state_interface_names_)); +} + +TEST_F(MotionPrimitivesForwardControllerTest, check_exported_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), command_values_.size()); + for (size_t i = 0; i < command_intefaces.names.size(); ++i) + { + EXPECT_EQ(command_intefaces.names[i], interface_namespace_ + "/" + command_interface_names_[i]); + } + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), state_values_.size()); + for (size_t i = 0; i < state_intefaces.names.size(); ++i) + { + EXPECT_EQ(state_intefaces.names[i], interface_namespace_ + "/" + state_interface_names_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}