diff --git a/motion_primitives_forward_controller/CMakeLists.txt b/motion_primitives_forward_controller/CMakeLists.txt new file mode 100644 index 0000000000..5c11dd3850 --- /dev/null +++ b/motion_primitives_forward_controller/CMakeLists.txt @@ -0,0 +1,125 @@ +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 +) + +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(ament_cmake_gmock REQUIRED) +find_package(controller_manager REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(ros2_control_test_assets REQUIRED) +find_package(industrial_robot_motion_interfaces 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 + include/motion_primitives_forward_controller/validate_motion_primitives_forward_controller_parameters.hpp +) +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 motion_primitives_forward_controller_parameters) +ament_target_dependencies(motion_primitives_forward_controller + ${THIS_PACKAGE_INCLUDE_DEPENDS} + industrial_robot_motion_interfaces +) +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 +) + +if(BUILD_TESTING) + ament_add_gmock( + test_load_motion_primitives_forward_controller + test/test_load_motion_primitives_forward_controller.cpp + ) + target_include_directories( + test_load_motion_primitives_forward_controller PRIVATE include + ) + ament_target_dependencies( + test_load_motion_primitives_forward_controller + controller_manager + hardware_interface + 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_include_directories( + test_motion_primitives_forward_controller PRIVATE include + ) + target_link_libraries( + test_motion_primitives_forward_controller motion_primitives_forward_controller + ) + ament_target_dependencies( + test_motion_primitives_forward_controller + controller_interface + hardware_interface + ) + + 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_include_directories( + test_motion_primitives_forward_controller_preceeding PRIVATE include + ) + target_link_libraries( + test_motion_primitives_forward_controller_preceeding motion_primitives_forward_controller + ) + ament_target_dependencies( + test_motion_primitives_forward_controller_preceeding + controller_interface + hardware_interface + ) +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..e301e690fb --- /dev/null +++ b/motion_primitives_forward_controller/README.md @@ -0,0 +1,49 @@ +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 `MotionPrimitive.msg` message type from the [industrial_robot_motion_interfaces](https://github.com/StoglRobotics-forks/industrial_robot_motion_interfaces/tree/helper-types) package. A custom fork of this package is used, which includes additional helper types to enhance motion command flexibility and sequencing. + +## Features + +- Support for basic motion primitives: + - `LINEAR_JOINT` + - `LINEAR_CARTESIAN` + - `CIRCULAR_CARTESIAN` +- Additional helper types: + - `STOP_MOTION`: Immediately stops the current robot motion and clears all pending primitives in the controller's queue. + - `MOTION_SEQUENCE_START` / `MOTION_SEQUENCE_END`: Define a motion sequence block. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives. + +## Architecture Overview +The following diagram shows the architecture for a UR robot. +For this setup, the [`motion_primitive_ur_driver`](https://github.com/StoglRobotics-forks/Universal_Robots_ROS2_Driver_MotionPrimitive) is used. + +![UR Robot Architecture](doc/ros2_control_motion_primitives_ur_whiteBackground.drawio.png) + + +1. **Command Reception** + Motion primitives can be published to the `~/reference` topic. These messages are received by the `reference_callback()` method in the controller. + This can be done, for example, via a Python script as demonstrated in the [`motion_primitive_ur_driver`](https://github.com/StoglRobotics-forks/Universal_Robots_ROS2_Driver_MotionPrimitive/blob/main/ur_robot_driver/examples/send_dummy_motion_primitives.py) package. + +2. **Command Handling Logic** + - If the received primitive is of type `STOP_MOTION`, it is directly forwarded to the hardware interface through the command interface, and all queued primitives in the controller are discarded. + - If the primitive is of any other type, it is appended to the internal motion primitive queue. The maximum queue size is configurable via a YAML configuration file. + +3. **Motion Execution Flow** + The `update()` method in the controller: + - Reads the current `execution_state` from the hardware interface via the state interface and publishes it to the `~/state` topic. + - Reads the `ready_for_new_primitive` state flag. If `true`, the next primitive from the queue is sent to the hardware interface for execution. + +4. **Sequencing Logic** + Sequencing logic for grouped execution (between `MOTION_SEQUENCE_START` and `MOTION_SEQUENCE_END`) is handled within the hardware interface layer. The controller itself only manages queueing and forwarding logic. + + +# Related packages/ repos +- [industrial_robot_motion_interfaces (with additional helper types for stop and motion sequence)](https://github.com/StoglRobotics-forks/industrial_robot_motion_interfaces/tree/helper-types) +- [ros2_controllers with motion_primitives_forward_controller](https://github.com/StoglRobotics-forks/ros2_controllers/tree/motion_primitive_forward_controller/motion_primitives_forward_controller) +- [Universal_Robots_ROS2_Driver with motion_primitive_ur_driver](https://github.com/StoglRobotics-forks/Universal_Robots_ROS2_Driver_MotionPrimitive) +- [Universal_Robots_Client_Library with movec from urfeex](https://github.com/urfeex/Universal_Robots_Client_Library/tree/movec_movep) 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..a33053b6b2 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/doc/ros2_control_motion_primitives_ur_whiteBackground.drawio.png b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur_whiteBackground.drawio.png new file mode 100644 index 0000000000..6b731fbcff Binary files /dev/null and b/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur_whiteBackground.drawio.png differ diff --git a/motion_primitives_forward_controller/include/motion_primitives_forward_controller/execution_state.hpp b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/execution_state.hpp new file mode 100644 index 0000000000..3557c56101 --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/execution_state.hpp @@ -0,0 +1,29 @@ +// 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__EXECUTION_STATE_HPP_ +#define MOTION_PRIMITIVES_FORWARD_CONTROLLER__EXECUTION_STATE_HPP_ + +namespace ExecutionState +{ +static constexpr uint8_t IDLE = 0; +static constexpr uint8_t EXECUTING = 1; +static constexpr uint8_t SUCCESS = 2; +static constexpr uint8_t ERROR = 3; +static constexpr uint8_t STOPPED = 4; +} // namespace ExecutionState + +#endif // MOTION_PRIMITIVES_FORWARD_CONTROLLER__EXECUTION_STATE_HPP_ 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..e7f1a4a01c --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp @@ -0,0 +1,95 @@ +// 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/msg/motion_primitive.hpp" +#include "std_msgs/msg/int8.hpp" + +namespace motion_primitives_forward_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_MY_ITFS = 0; + +// name constants for command interfaces +static constexpr size_t CMD_MY_ITFS = 0; + +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; + + using ControllerReferenceMsg = industrial_robot_motion_interfaces::msg::MotionPrimitive; + using ControllerStateMsg = std_msgs::msg::Int8; + +protected: + std::shared_ptr param_listener_; + motion_primitives_forward_controller::Params params_; + + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + std::queue> msg_queue_; + +private: + void reference_callback(const std::shared_ptr msg); + void reset_command_interfaces(); + bool set_command_interfaces(); + void reset_controller_reference_msg(std::shared_ptr & msg); + + size_t queue_size_ = 0; + std::mutex command_mutex_; + bool print_error_once_ = true; +}; + +} // namespace motion_primitives_forward_controller + +#endif // MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ diff --git a/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_type.hpp b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_type.hpp new file mode 100644 index 0000000000..3eb70221f5 --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_type.hpp @@ -0,0 +1,34 @@ +// 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_TYPE_HPP_ +#define MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_TYPE_HPP_ + +namespace MotionType +{ +// Motion Primitives +static constexpr uint8_t LINEAR_JOINT = 10; // changed to 10 because 0 is default value +static constexpr uint8_t LINEAR_CARTESIAN = 50; +static constexpr uint8_t CIRCULAR_CARTESIAN = 51; + +// Helper types +static constexpr uint8_t STOP_MOTION = 66; +static constexpr uint8_t RESET_STOP = 67; +// indicate motion sequence instead of executing single primitives +static constexpr uint8_t MOTION_SEQUENCE_START = 100; +static constexpr uint8_t MOTION_SEQUENCE_END = 101; +} // namespace MotionType +#endif // MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_TYPE_HPP_ diff --git a/motion_primitives_forward_controller/include/motion_primitives_forward_controller/ready_for_new_primitive.hpp b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/ready_for_new_primitive.hpp new file mode 100644 index 0000000000..3a487b7463 --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/ready_for_new_primitive.hpp @@ -0,0 +1,26 @@ +// 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__READY_FOR_NEW_PRIMITIVE_HPP_ +#define MOTION_PRIMITIVES_FORWARD_CONTROLLER__READY_FOR_NEW_PRIMITIVE_HPP_ + +namespace ReadyForNewPrimitive +{ +static constexpr uint8_t NOT_READY = 0; +static constexpr uint8_t READY = 1; +} // namespace ReadyForNewPrimitive + +#endif // MOTION_PRIMITIVES_FORWARD_CONTROLLER__READY_FOR_NEW_PRIMITIVE_HPP_ diff --git a/motion_primitives_forward_controller/include/motion_primitives_forward_controller/validate_motion_primitives_forward_controller_parameters.hpp b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/validate_motion_primitives_forward_controller_parameters.hpp new file mode 100644 index 0000000000..c2ff279035 --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/validate_motion_primitives_forward_controller_parameters.hpp @@ -0,0 +1,38 @@ +// 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. + +#ifndef MOTION_PRIMITIVES_FORWARD_CONTROLLER__VALIDATE_MOTION_PRIMITIVES_FORWARD_CONTROLLER_PARAMETERS_HPP_ +#define MOTION_PRIMITIVES_FORWARD_CONTROLLER__VALIDATE_MOTION_PRIMITIVES_FORWARD_CONTROLLER_PARAMETERS_HPP_ + +#include + +#include "parameter_traits/parameter_traits.hpp" + +namespace parameter_traits +{ +Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter) +{ + auto const & interface_name = parameter.as_string(); + + if (interface_name.rfind("blup_", 0) == 0) + { + return ERROR("'interface_name' parameter can not start with 'blup_'"); + } + + return OK; +} + +} // namespace parameter_traits + +#endif // MOTION_PRIMITIVES_FORWARD_CONTROLLER__VALIDATE_MOTION_PRIMITIVES_FORWARD_CONTROLLER_PARAMETERS_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..eb107bd920 --- /dev/null +++ b/motion_primitives_forward_controller/package.xml @@ -0,0 +1,38 @@ + + + + motion_primitives_forward_controller + 4.23.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 + + 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..822ae6c6c9 --- /dev/null +++ b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp @@ -0,0 +1,485 @@ +// 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 "motion_primitives_forward_controller/execution_state.hpp" +#include "motion_primitives_forward_controller/motion_type.hpp" +#include "motion_primitives_forward_controller/ready_for_new_primitive.hpp" + +namespace +{ // utility + +// TODO(destogl): remove this when merged upstream +// Changed services history QoS to keep all so we don't lose any client service calls +static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1, // message queue depth + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false}; +} // namespace + +namespace motion_primitives_forward_controller +{ +MotionPrimitivesForwardController::MotionPrimitivesForwardController() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::CallbackReturn MotionPrimitivesForwardController::on_init() +{ + RCLCPP_INFO(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_INFO(get_node()->get_logger(), "Configuring Motion Primitives Forward Controller"); + + params_ = param_listener_->get_params(); + + // Check if the name is not empty + if (params_.name.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error: A name must be provided!"); + return controller_interface::CallbackReturn::ERROR; + } + + // 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; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&MotionPrimitivesForwardController::reference_callback, this, std::placeholders::_1)); + RCLCPP_INFO( + get_node()->get_logger(), "Subscribed to reference topic: %s", + ref_subscriber_->get_topic_name()); + + std::shared_ptr msg = std::make_shared(); + reset_controller_reference_msg(msg); + + queue_size_ = params_.queue_size; + if (queue_size_ == 0) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error: Queue size must be greater than 0!"); + return controller_interface::CallbackReturn::ERROR; + } + + try + { + // State publisher + s_publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } + catch (const std::exception & e) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +// Function gets called when a new message is received +void MotionPrimitivesForwardController::reference_callback( + const std::shared_ptr msg) +{ + // Check if the type is one of the allowed motion types + switch (msg->type) + { + case MotionType::STOP_MOTION: + { + RCLCPP_INFO(get_node()->get_logger(), "Received motion type: STOP_MOTION"); + reset_command_interfaces(); + std::lock_guard guard(command_mutex_); + (void)command_interfaces_[0].set_value( + static_cast(msg->type)); // send stop command immediately to the hw-interface + while (!msg_queue_.empty()) + { // clear the queue + msg_queue_.pop(); + } + return; + } + + case MotionType::RESET_STOP: + { + RCLCPP_INFO(get_node()->get_logger(), "Received motion type: RESET_STOP"); + reset_command_interfaces(); + std::lock_guard guard(command_mutex_); + (void)command_interfaces_[0].set_value( + static_cast(msg->type)); // send reset stop command immediately to the hw-interface + return; + } + + case MotionType::LINEAR_JOINT: + RCLCPP_INFO(get_node()->get_logger(), "Received motion type: LINEAR_JOINT (PTP)"); + if (msg->joint_positions.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Received LINEAR_JOINT motion type without joint positions"); + return; + } + break; + + case MotionType::LINEAR_CARTESIAN: + RCLCPP_INFO(get_node()->get_logger(), "Received motion type: LINEAR_CARTESIAN (LIN)"); + if (msg->poses.empty()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Received LINEAR_CARTESIAN motion type without poses"); + return; + } + break; + + case MotionType::CIRCULAR_CARTESIAN: + RCLCPP_INFO(get_node()->get_logger(), "Received motion type: CIRCULAR_CARTESIAN (CIRC)"); + if (msg->poses.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Received CIRCULAR_CARTESIAN motion type without two poses"); + return; + } + break; + + case MotionType::MOTION_SEQUENCE_START: + RCLCPP_INFO(get_node()->get_logger(), "Received motion type: MOTION_SEQUENCE_START"); + break; + + case MotionType::MOTION_SEQUENCE_END: + RCLCPP_INFO(get_node()->get_logger(), "Received motion type: MOTION_SEQUENCE_END"); + break; + + default: + RCLCPP_ERROR(get_node()->get_logger(), "Received unknown motion type: %u", msg->type); + return; + } + + if (msg_queue_.size() >= queue_size_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Queue size exceeded. Can't add new motion primitive."); + return; + } + + msg_queue_.push(msg); +} + +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(params_.name + "/" + 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(params_.name + "/" + interface_name); + } + return state_interfaces_config; +} + +controller_interface::CallbackReturn MotionPrimitivesForwardController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reset_command_interfaces(); + RCLCPP_INFO(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_INFO(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*/) +{ + // 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; + } + uint8_t execution_status = static_cast(std::round(opt_value_execution.value())); + + switch (execution_status) + { + case ExecutionState::IDLE: + // RCLCPP_INFO(get_node()->get_logger(), "Execution state: IDLE"); + print_error_once_ = true; + break; + case ExecutionState::EXECUTING: + // RCLCPP_INFO(get_node()->get_logger(), "Execution state: EXECUTING"); + print_error_once_ = true; + break; + + case ExecutionState::SUCCESS: + // RCLCPP_INFO(get_node()->get_logger(), "Execution state: SUCCESS"); + print_error_once_ = true; + break; + + case ExecutionState::STOPPED: + // RCLCPP_INFO(get_node()->get_logger(), "Execution state: STOPPED"); + print_error_once_ = true; + break; + + case ExecutionState::ERROR: + 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", execution_status); + return controller_interface::return_type::ERROR; + } + + // publish the execution_status + state_publisher_->lock(); + state_publisher_->msg_.data = execution_status; + state_publisher_->unlockAndPublish(); + + 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; + } + uint8_t ready_for_new_primitive = static_cast(std::round(opt_value_ready.value())); + + if (!msg_queue_.empty()) // check if new command is available + { + switch (ready_for_new_primitive) + { + case ReadyForNewPrimitive::NOT_READY: + { + return controller_interface::return_type::OK; + } + case ReadyForNewPrimitive::READY: + { + 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", + 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() +{ + std::lock_guard guard(command_mutex_); + // Get the oldest message from the queue + std::shared_ptr current_ref = msg_queue_.front(); + msg_queue_.pop(); + + // Check if the message is valid + if (!current_ref) + { + 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_ref->type)); + + // Process joint positions if available + if (!current_ref->joint_positions.empty()) + { + for (size_t i = 0; i < current_ref->joint_positions.size(); ++i) + { + (void)command_interfaces_[i + 1].set_value(current_ref->joint_positions[i]); // q1 to q6 + } + } + + // Process Cartesian poses if available + if (!current_ref->poses.empty()) + { + const auto & goal_pose = current_ref->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_ref->type == MotionType::CIRCULAR_CARTESIAN && current_ref->poses.size() == 2) + { + const auto & via_pose = current_ref->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_ref->blend_radius); // blend_radius + + // Read additional arguments + for (const auto & arg : current_ref->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; +} + +// reset the controller reference message to NaN +void MotionPrimitivesForwardController::reset_controller_reference_msg( + std::shared_ptr & msg) +{ + msg->type = 0; + msg->blend_radius = std::numeric_limits::quiet_NaN(); + + for (auto & arg : msg->additional_arguments) + { + arg.argument_name = ""; + arg.argument_value = std::numeric_limits::quiet_NaN(); + } + + for (auto & pose : msg->poses) + { + pose.pose.position.x = std::numeric_limits::quiet_NaN(); + pose.pose.position.y = std::numeric_limits::quiet_NaN(); + pose.pose.position.z = std::numeric_limits::quiet_NaN(); + + pose.pose.orientation.x = std::numeric_limits::quiet_NaN(); + pose.pose.orientation.y = std::numeric_limits::quiet_NaN(); + pose.pose.orientation.z = std::numeric_limits::quiet_NaN(); + pose.pose.orientation.w = std::numeric_limits::quiet_NaN(); + } +} + +} // 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..62db1ccb09 --- /dev/null +++ b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.yaml @@ -0,0 +1,33 @@ +motion_primitives_forward_controller: + name: { + type: string, + default_value: "", + description: "Name for /", + read_only: true, + } + 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, + } + } + queue_size: { + type: int, + default_value: 10, + description: "Queue size to buffer incoming commands", + 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..aaa5ef1638 --- /dev/null +++ b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml @@ -0,0 +1,33 @@ +test_motion_primitives_forward_controller: + ros__parameters: + name: motion_primitive + 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 + queue_size: 20 # queue size to buffer incoming commands 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..aaa5ef1638 --- /dev/null +++ b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_preceeding_params.yaml @@ -0,0 +1,33 @@ +test_motion_primitives_forward_controller: + ros__parameters: + name: motion_primitive + 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 + queue_size: 20 # queue size to buffer incoming commands 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..f9d71b55c7 --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp @@ -0,0 +1,192 @@ +// 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" + +using motion_primitives_forward_controller::CMD_MY_ITFS; +using motion_primitives_forward_controller::STATE_MY_ITFS; + +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_TRUE(controller_->params_.name.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_)); + ASSERT_EQ(controller_->params_.name, interface_namespace_); +} + +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 msg_queue = controller_->msg_queue_; + ASSERT_TRUE(msg_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_[CMD_MY_ITFS].get_optional(); + ASSERT_TRUE(std::isnan(val_opt.value())); + + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + val_opt = controller_->command_interfaces_[CMD_MY_ITFS].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, publish_status_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); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.data, ExecutionState::IDLE); +} + +TEST_F(MotionPrimitivesForwardControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.data, ExecutionState::IDLE); + + publish_commands(); + controller_->wait_for_commands(executor); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_EQ(command_values_[0], 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 + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.data, ExecutionState::IDLE); +} + +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..b259d3cdc0 --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp @@ -0,0 +1,260 @@ +// 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 "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_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "industrial_robot_motion_interfaces/msg/motion_primitive.hpp" +#include "motion_primitives_forward_controller/execution_state.hpp" +#include "motion_primitives_forward_controller/motion_type.hpp" +#include "motion_primitives_forward_controller/ready_for_new_primitive.hpp" +#include "std_msgs/msg/int8.hpp" + +using ControllerReferenceMsg = industrial_robot_motion_interfaces::msg::MotionPrimitive; +using ControllerStateMsg = std_msgs::msg::Int8; + +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_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + return motion_primitives_forward_controller::MotionPrimitivesForwardController::on_configure( + previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + */ + void wait_for_command( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + auto until = get_node()->get_clock()->now() + timeout; + while (get_node()->get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds(10)); + } + } + + void wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, timeout); + } +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class MotionPrimitivesForwardControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_motion_primitives_forward_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController( + const std::string controller_name = "test_motion_primitives_forward_controller") + { + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector command_ifs; + 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()); + } + + std::vector state_ifs; + 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 subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_motion_primitives_forward_controller/state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands( + 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 << "Publishing command message ..." << std::endl; + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + std::cout << "Found subscriber for topic: " << topic_name << std::endl; + }; + + auto topic_name = command_publisher_->get_topic_name(); + std::cout << "Waiting for subscriber on topic: " << topic_name << std::endl; + wait_for_topic(topic_name); + + ControllerReferenceMsg msg; + + // TODO(mathias31415): Add other tests for other motion types + msg.type = MotionType::LINEAR_JOINT; + msg.joint_positions = joint_positions; + msg.blend_radius = blend_radius; + + msg.additional_arguments.resize(3); + msg.additional_arguments[0].argument_name = "velocity"; + msg.additional_arguments[0].argument_value = velocity; + msg.additional_arguments[1].argument_name = "acceleration"; + msg.additional_arguments[1].argument_value = acceleration; + msg.additional_arguments[2].argument_name = "move_time"; + msg.additional_arguments[2].argument_value = move_time; + + command_publisher_->publish(msg); + } + +protected: + // Controller-related parameters + 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_ = {ExecutionState::IDLE, 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_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Node::SharedPtr service_caller_node_; +}; + +#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..c83f658be0 --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller_preceeding.cpp @@ -0,0 +1,78 @@ +// 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 + +using motion_primitives_forward_controller::CMD_MY_ITFS; +using motion_primitives_forward_controller::STATE_MY_ITFS; + +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_TRUE(controller_->params_.name.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_)); + ASSERT_EQ(controller_->params_.name, interface_namespace_); +} + +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; +}