diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9e5474800f..ca2404daf3 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -113,7 +113,11 @@ repos: rev: v2.0.0 hooks: - id: doc8 - args: ['--max-line-length=100', '--ignore=D001'] + args: [ + '--max-line-length=100', + '--ignore=D001', + '--ignore-path=motion_primitives_forward_controller/userdoc.rst' # D000 fails with myst_parser + ] exclude: CHANGELOG\.rst$ - repo: https://github.com/pre-commit/pygrep-hooks diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index dd404da17c..1acb19f7ce 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -58,6 +58,7 @@ The controllers are using `common hardware interface definitions`_, and may use Position Controllers <../position_controllers/doc/userdoc.rst> Velocity Controllers <../velocity_controllers/doc/userdoc.rst> Gpio Command Controller <../gpio_controllers/doc/userdoc.rst> + Motion Primitive Controller <../motion_primitives_forward_controller/userdoc.rst> Broadcasters diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 94fb089c59..23ed5e8eaa 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -31,3 +31,7 @@ pid_controller * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy. * A new ``error_deadband`` parameter stops integration when the error is within a specified range. * PID state publisher can be turned off or on by using ``activate_state_publisher`` parameter. (`#1823 `_). + +motion_primitives_forward_controller +******************************************* +* 🚀 The motion_primitives_forward_controller was added 🎉 (`#1636 `_). diff --git a/motion_primitives_forward_controller/CMakeLists.txt b/motion_primitives_forward_controller/CMakeLists.txt new file mode 100644 index 0000000000..35736c9bfd --- /dev/null +++ b/motion_primitives_forward_controller/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.10) +project(motion_primitives_forward_controller) + +find_package(ros2_control_cmake REQUIRED) +set_compiler_options() +export_windows_symbols() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs +) + +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_parameter.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} +) + +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 + motion_primitives_forward_controller_parameters + EXPORT motion_primitives_forward_controller_targets + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) +ament_export_targets(motion_primitives_forward_controller_targets HAS_LIBRARY_TARGET) + +ament_export_include_directories( + include +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_libraries( + motion_primitives_forward_controller +) + +ament_package() + +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 + ) +endif() diff --git a/motion_primitives_forward_controller/README.md b/motion_primitives_forward_controller/README.md new file mode 100644 index 0000000000..e5e24bcbae --- /dev/null +++ b/motion_primitives_forward_controller/README.md @@ -0,0 +1,68 @@ +motion_primitive_controllers +========================================== + +Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC) + +# Description +This project provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotionPrimitiveSequence.action` action from [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action). 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_ur10e.py) in the `Universal_Robots_ROS2_Driver` package. + +## Command and State Interfaces +To transmit the motion primitives, the following command and state interfaces are required. All interfaces use the naming scheme `tf_prefix_ + "motion_primitive/"` where the `tf_prefix` is provided to the controller as a parameter. + +### Command Interfaces +These interfaces are used to send motion primitive data to the hardware interface: +- `motion_type`: Type of motion primitive (LINEAR_JOINT, LINEAR_CARTESIAN, CIRCULAR_CARTESIAN) +- `q1` – `q6`: Target joint positions for joint-based motion +- `pos_x`, `pos_y`, `pos_z`: Target Cartesian position +- `pos_qx`, `pos_qy`, `pos_qz`, `pos_qw`: Orientation quaternion of the target pose +- `pos_via_x`, `pos_via_y`, `pos_via_z`: Intermediate via-point position for circular motion +- `pos_via_qx`, `pos_via_qy`, `pos_via_qz`, `pos_via_qw`: Orientation quaternion of via-point +- `blend_radius`: Blending radius for smooth transitions +- `velocity`: Desired motion velocity +- `acceleration`: Desired motion acceleration +- `move_time`: Optional duration for time-based execution (For LINEAR_JOINT and LINEAR_CARTESIAN. If move_time > 0, velocity and acceleration are ignored) + +### State Interfaces +These interfaces are used to communicate the internal status of the hardware interface back to the `motion_primitives_forward_controller`. +- `execution_status`: Indicates the current execution state of the primitive. Possible values are: + - `IDLE`: No motion in progress + - `EXECUTING`: Currently executing a primitive + - `SUCCESS`: Last command finished successfully + - `ERROR`: An error occurred during execution + - `STOPPING`: The hardware interface has received the `STOP_MOTION` command, but the robot has not yet come to a stop. + - `STOPPED`: The robot was stopped using the `STOP_MOTION` command and must be reset with the `RESET_STOP` command before executing new commands. +- `ready_for_new_primitive`: Boolean flag indicating whether the interface is ready to receive a new motion primitive + + + ## Parameters + This controller uses the [`generate_parameter_library`](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. The parameter [definition file located in the src folder](https://github.com/ros-controls/ros2_controllers/blob/master/motion_primitives_forward_controller/src/motion_primitives_forward_controller_parameter.yaml) contains descriptions for all the parameters used by the controller. + An example parameter file for this controller can be found in [the test directory](https://github.com/ros-controls/ros2_controllers/blob/master/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml). + + +# Architecture Overview +Architecture for a UR robot with [`Universal_Robots_ROS2_Driver` in motion primitives mode](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver). + +![UR Robot Architecture](doc/ros2_control_motion_primitives_ur.drawio.png) + +Architecture for a KUKA robot with [`kuka_eki_motion_primitives_hw_interface`](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver/kuka_eki_motion_primitives_hw_interface). + +![KUKA Robot Architecture](doc/ros2_control_motion_primitives_kuka.drawio.png) + +# Demo-Video with UR10e +[![UR demo video](doc/moprim_forward_controller_ur_demo_thumbnail.png)](https://youtu.be/SKz6LFvJmhQ) + +# Demo-Video with KR3 +[![KUKA demo video](doc/moprim_forward_controller_kuka_demo_thumbnail.png)](https://youtu.be/_BWCO36j9bg) diff --git a/motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio b/motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio new file mode 100644 index 0000000000..dbbde2e7b2 --- /dev/null +++ b/motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/moprim_forward_controller_kuka_demo_thumbnail.png b/motion_primitives_forward_controller/doc/moprim_forward_controller_kuka_demo_thumbnail.png new file mode 100644 index 0000000000..91f6750d15 Binary files /dev/null and b/motion_primitives_forward_controller/doc/moprim_forward_controller_kuka_demo_thumbnail.png differ diff --git a/motion_primitives_forward_controller/doc/moprim_forward_controller_ur_demo_thumbnail.png b/motion_primitives_forward_controller/doc/moprim_forward_controller_ur_demo_thumbnail.png new file mode 100644 index 0000000000..008702d4d5 Binary files /dev/null and b/motion_primitives_forward_controller/doc/moprim_forward_controller_ur_demo_thumbnail.png differ diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio new file mode 100644 index 0000000000..8a354af5a2 --- /dev/null +++ b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio @@ -0,0 +1,237 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio.png b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio.png new file mode 100644 index 0000000000..ffc2044308 Binary files /dev/null and b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio.png differ diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.drawio b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.drawio new file mode 100644 index 0000000000..e83febae7b --- /dev/null +++ b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.drawio @@ -0,0 +1,369 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.drawio.png b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.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.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..476d383040 --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp @@ -0,0 +1,128 @@ +// 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 +#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 "realtime_tools/realtime_thread_safe_box.hpp" + +#include "control_msgs/action/execute_motion_primitive_sequence.hpp" +#include "control_msgs/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, + STOPPING = 4, + STOPPED = 5 +}; + +using MotionType = control_msgs::msg::MotionPrimitive; +enum class MotionHelperType : uint8_t +{ + 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_; + std::string tf_prefix_; + + using MotionPrimitive = control_msgs::msg::MotionPrimitive; + realtime_tools::LockFreeSPSCQueue moprim_queue_; + + using ExecuteMotionAction = control_msgs::action::ExecuteMotionPrimitiveSequence; + 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( + std::shared_ptr> goal_handle); + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + realtime_tools::RealtimeThreadSafeBox> rt_goal_handle_; + std::atomic has_active_goal_ = false; + rclcpp::TimerBase::SharedPtr goal_handle_timer_; + rclcpp::Duration action_monitor_period_ = rclcpp::Duration(std::chrono::milliseconds(20)); + + 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_; + MotionPrimitive current_moprim_; +}; + +} // 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..768ab930cc --- /dev/null +++ b/motion_primitives_forward_controller/package.xml @@ -0,0 +1,42 @@ + + + + motion_primitives_forward_controller + 5.5.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 + 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..480144441a --- /dev/null +++ b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp @@ -0,0 +1,545 @@ +// 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" +#include "lifecycle_msgs/msg/state.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) + { + RCLCPP_ERROR( + get_node()->get_logger(), "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(); + tf_prefix_ = params_.tf_prefix; + + using namespace std::placeholders; + 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, _1, _2), + std::bind(&MotionPrimitivesForwardController::goal_cancelled_callback, this, _1), + std::bind(&MotionPrimitivesForwardController::goal_accepted_callback, this, _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(25); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/motion_type"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q1"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q2"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q3"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q4"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q5"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q6"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_x"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_y"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_z"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qx"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qy"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qz"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qw"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_x"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_y"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_z"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qx"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qy"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qz"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qw"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/blend_radius"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/velocity"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/acceleration"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/move_time"); + 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(2); + state_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/execution_status"); + state_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/ready_for_new_primitive"); + return state_interfaces_config; +} + +controller_interface::CallbackReturn MotionPrimitivesForwardController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reset_command_interfaces(); + 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(MotionHelperType::STOP_MOTION)); + // clear the queue (ignore return value) + static_cast(moprim_queue_.get_latest(current_moprim_)); + 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 (has_active_goal_ && was_executing_) + { + rt_goal_handle_.try_get( + [&](const std::shared_ptr & goal_handle) + { + was_executing_ = false; + auto result = std::make_shared(); + result->error_code = ExecuteMotionAction::Result::SUCCESSFUL; + result->error_string = "Motion primitives executed successfully"; + goal_handle->setSucceeded(result); + has_active_goal_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Motion primitives executed successfully."); + }); + } + + break; + + case ExecutionState::STOPPING: + RCLCPP_DEBUG(get_node()->get_logger(), "Execution state: STOPPING"); + print_error_once_ = true; + was_executing_ = false; + break; + + case ExecutionState::STOPPED: + print_error_once_ = true; + was_executing_ = false; + if (has_active_goal_) + { + rt_goal_handle_.try_get( + [&](const std::shared_ptr & goal_handle) + { + auto result = std::make_shared(); + goal_handle->setCanceled(result); + has_active_goal_ = false; + 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(MotionHelperType::RESET_STOP)); + robot_stop_requested_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Robot stopped, ready for new motion primitives."); + } + + break; + + case ExecutionState::ERROR: + was_executing_ = false; + 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 (!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 + { + 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 + if (!moprim_queue_.pop(current_moprim_)) + { + RCLCPP_WARN(get_node()->get_logger(), "Failed to pop motion primitive from queue."); + 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 == 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.name == "velocity") + { + (void)command_interfaces_[22].set_value(arg.value); + } + else if (arg.name == "acceleration") + { + (void)command_interfaces_[23].set_value(arg.value); + } + else if (arg.name == "move_time") + { + (void)command_interfaces_[24].set_value(arg.value); + } + else + { + RCLCPP_WARN(get_node()->get_logger(), "Unknown additional argument: %s", arg.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 new action goal"); + + // Precondition: Running controller + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Can't accept new trajectories. Controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (has_active_goal_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Already has an active goal, rejecting new goal request."); + return rclcpp_action::GoalResponse::REJECT; + } + + 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 (primitives.empty()) + { + RCLCPP_WARN(get_node()->get_logger(), "Goal rejected: no motion primitives provided."); + return rclcpp_action::GoalResponse::REJECT; + } + + if ( + (primitives.size() == 1) && + primitives.size() > (moprim_queue_.capacity() - moprim_queue_.size())) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Remaining queue capacity (%zu) is not enough for the requested %zu motion primitive.", + moprim_queue_.capacity() - moprim_queue_.size(), primitives.size()); + return rclcpp_action::GoalResponse::REJECT; + } + else if ((primitives.size() + 2) > (moprim_queue_.capacity() - moprim_queue_.size())) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Remaining queue capacity (%zu) is not enough for the requested %zu motion primitives (+ " + "start and stop marker).", + moprim_queue_.capacity() - moprim_queue_.size(), primitives.size()); + 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; + } + } + RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse MotionPrimitivesForwardController::goal_cancelled_callback( + const std::shared_ptr>) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + cancel_requested_ = true; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void MotionPrimitivesForwardController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + const auto & primitives = goal_handle->get_goal()->trajectory.motions; + + auto add_motions = [this](const std::vector & motion_primitives) + { + for (const auto & primitive : motion_primitives) + { + if (!moprim_queue_.push(primitive)) + { + RCLCPP_WARN(get_node()->get_logger(), "Failed to push motion primitive to queue."); + } + } + }; + + if (primitives.size() > 1) + { + MotionPrimitive start_marker; + start_marker.type = static_cast(MotionHelperType::MOTION_SEQUENCE_START); + if (!moprim_queue_.push(start_marker)) + { + RCLCPP_WARN( + get_node()->get_logger(), "Failed to push motion sequence start marker to queue."); + } + + add_motions(primitives); + + MotionPrimitive end_marker; + end_marker.type = static_cast(MotionHelperType::MOTION_SEQUENCE_END); + if (!moprim_queue_.push(end_marker)) + { + RCLCPP_WARN(get_node()->get_logger(), "Failed to push motion sequence end marker to queue."); + } + } + else + { + add_motions(primitives); + } + + auto rt_goal = std::make_shared(goal_handle); + rt_goal_handle_.set( + [&](auto & handle) + { + handle = rt_goal; + has_active_goal_ = true; + }); + + rt_goal->execute(); + + goal_handle_timer_.reset(); + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); + + 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_parameter.yaml b/motion_primitives_forward_controller/src/motion_primitives_forward_controller_parameter.yaml new file mode 100644 index 0000000000..be829b503f --- /dev/null +++ b/motion_primitives_forward_controller/src/motion_primitives_forward_controller_parameter.yaml @@ -0,0 +1,7 @@ +motion_primitives_forward_controller: + tf_prefix: { + type: string, + default_value: "", + description: "tf_prefix", + read_only: true, + } 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..c22b2eb470 --- /dev/null +++ b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml @@ -0,0 +1,3 @@ +test_motion_primitives_forward_controller: + ros__parameters: + tf_prefix: "" 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..e7729711fa --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp @@ -0,0 +1,165 @@ +// 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_.tf_prefix.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(MotionPrimitivesForwardControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_interfaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_interfaces.names.size(), command_values_.size()); + for (size_t i = 0; i < command_interfaces.names.size(); ++i) + { + EXPECT_EQ( + command_interfaces.names[i], interface_namespace_ + "/" + command_interface_names_[i]); + } + + auto state_interfaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_interfaces.names.size(), state_values_.size()); + for (size_t i = 0; i < state_interfaces.names.size(); ++i) + { + EXPECT_EQ(state_interfaces.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..0f857b561d --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp @@ -0,0 +1,219 @@ +// 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 "control_msgs/action/execute_motion_primitive_sequence.hpp" +#include "control_msgs/msg/motion_primitive.hpp" + +using MotionPrimitive = control_msgs::msg::MotionPrimitive; +using ExecuteMotion = control_msgs::action::ExecuteMotionPrimitiveSequence; + +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() + { + 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].name = "velocity"; + primitive.additional_arguments[0].value = velocity; + primitive.additional_arguments[1].name = "acceleration"; + primitive.additional_arguments[1].value = acceleration; + primitive.additional_arguments[2].name = "move_time"; + primitive.additional_arguments[2].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/userdoc.rst b/motion_primitives_forward_controller/userdoc.rst new file mode 100644 index 0000000000..23e4ff4e91 --- /dev/null +++ b/motion_primitives_forward_controller/userdoc.rst @@ -0,0 +1,7 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/motion_primitives_forward_controller/doc/userdoc.rst + +.. _motion_primitives_forward_controller_userdoc: + + +.. include:: ../README.md + :parser: myst_parser.sphinx_