diff --git a/motion_primitives_forward_controller/CMakeLists.txt b/motion_primitives_forward_controller/CMakeLists.txt new file mode 100644 index 0000000000..c73778c958 --- /dev/null +++ b/motion_primitives_forward_controller/CMakeLists.txt @@ -0,0 +1,112 @@ +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() + +# find dependencies +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() + +# Add motion_primitives_forward_controller library related compile commands +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/${PROJECT_NAME} +# ) + +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..372eed8e29 --- /dev/null +++ b/motion_primitives_forward_controller/README.md @@ -0,0 +1,48 @@ +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-BSD-3-Clause-blue.svg) + + +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** + Python scripts can publish motion primitives to the `~/reference` topic. These messages are received by the `reference_callback()` method in the controller. + +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) \ No newline at end of file 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..38c1458365 --- /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 EXECUTION_STATE_HPP +#define 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; +} + +#endif // 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..add7ad4ee0 --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp @@ -0,0 +1,102 @@ +// 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 "controller_interface/controller_interface.hpp" +#include +#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; + + // state and command message types + 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_; + + // Command subscribers and Controller State publisher + rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; + // realtime_tools::RealtimeBuffer> input_ref_; + + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + +private: + // callback for topic interface + void reference_callback(const std::shared_ptr msg); // callback for reference message + // std::atomic new_msg_available_ = false; // flag to indicate if new message is available + void reset_command_interfaces(); // Reset all command interfaces to NaN() + bool set_command_interfaces(); // Set command interfaces from the message + + std::queue> msg_queue_; + size_t queue_size_ = 0; + + std::mutex command_mutex_; + + bool print_error_once_ = true; // Flag to print error message only once if ExecutionState::ERROR +}; + +} // 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..9074f4d45b --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_type.hpp @@ -0,0 +1,33 @@ +// 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_TYPE_HPP +#define 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; // added to stop motion + static constexpr uint8_t MOTION_SEQUENCE_START = 100; // added to indicate motion sequence instead of executing single primitives + static constexpr uint8_t MOTION_SEQUENCE_END = 101; // added to indicate end of motion sequence +} + +#endif // 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..f8054f3dbc --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/ready_for_new_primitive.hpp @@ -0,0 +1,27 @@ +// 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 READY_FOR_NEW_PRIMITIVE_HPP +#define READY_FOR_NEW_PRIMITIVE_HPP + + +namespace ReadyForNewPrimitive +{ + static constexpr uint8_t NOT_READY = 0; + static constexpr uint8_t READY = 1; +} + +#endif // 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..aad3111815 --- /dev/null +++ b/motion_primitives_forward_controller/include/motion_primitives_forward_controller/validate_motion_primitives_forward_controller_parameters.hpp @@ -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. +// +// Authors: + +#ifndef MOTION_PRIMITIVES_CONTTROLLER_PKG__VALIDATE_MOTION_PRIMITIVES_FORWARD_CONTROLLER_PARAMETERS_HPP_ +#define MOTION_PRIMITIVES_CONTTROLLER_PKG__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_CONTTROLLER_PKG__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..a17ee48a3a --- /dev/null +++ b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp @@ -0,0 +1,436 @@ +// 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/motion_type.hpp" +#include "motion_primitives_forward_controller/execution_state.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}; + +using ControllerReferenceMsg = motion_primitives_forward_controller::MotionPrimitivesForwardController::ControllerReferenceMsg; + +// reset the controller reference message to NaN +void 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 + + +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 one state interface 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)); + + 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(); // Reset all command interfaces to NaN + std::lock_guard guard(command_mutex_); + (void)command_interfaces_[0].set_value(static_cast(msg->type)); // send stop command to the driver + while (!msg_queue_.empty()) { // clear the queue + msg_queue_.pop(); + } + return; + } + + case MotionType::LINEAR_JOINT: + RCLCPP_INFO(get_node()->get_logger(), "Received motion type: LINEAR_JOINT (PTP)"); + // Check if joint positions are provided + 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)"); + // Check if poses are provided + 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)"); + // Check if poses are provided + 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; + + // Additional motion types can be added here + default: + RCLCPP_ERROR(get_node()->get_logger(), "Received unknown motion type: %u", msg->type); + return; + } + + // Check if the queue size is exceeded + 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; + + // Reserve memory for the command interfaces + 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) + { + // Combine the joint with the interface name and add it to the list + 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; + + // Reserve memory for the state interfaces + 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) + { + // Combine the joint with the interface name and add it to the list + 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(); // Reset all command interfaces to NaN + 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(); // Reset all command interfaces to NaN + 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 + // TODO(mathias31415) Is check needed if the value is .0? + uint8_t execution_status = static_cast(std::round(state_interfaces_[0].get_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::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(); + + // TODO(mathias31415) Is check needed if the value is .0? + uint8_t ready_for_new_primitive = static_cast(std::round(state_interfaces_[1].get_value())); + + // sending new command? + if(!msg_queue_.empty()) // check if new command is available + { + switch (ready_for_new_primitive) { + case ReadyForNewPrimitive::NOT_READY:{ // hw-interface is not ready for a new command + return controller_interface::return_type::OK; + } + case ReadyForNewPrimitive::READY:{ // hw-interface is ready for a new command + if(!set_command_interfaces()){ // Set the command interfaces with next motion primitive + 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 + } + } + + // Set blend_radius + (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; +} + + +} // 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..d63573e0c1 --- /dev/null +++ b/motion_primitives_forward_controller/src/motion_primitives_forward_controller.yaml @@ -0,0 +1,50 @@ +# 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 + + +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..573eb4364a --- /dev/null +++ b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml @@ -0,0 +1,7 @@ +test_motion_primitives_forward_controller: + ros__parameters: + + joints: + - joint1 + + interface_name: acceleration 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..e3113c2042 --- /dev/null +++ b/motion_primitives_forward_controller/test/motion_primitives_forward_controller_preceeding_params.yaml @@ -0,0 +1,9 @@ +test_motion_primitives_forward_controller: + ros__parameters: + joints: + - joint1 + + interface_name: acceleration + + state_joints: + - joint1state 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..a7bb555bd0 --- /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. +// +// Authors: +#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..ad0442f74c --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp @@ -0,0 +1,274 @@ +// 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: + +// #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::control_mode_type; +// 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_.joints.empty()); +// ASSERT_TRUE(controller_->params_.state_joints.empty()); +// ASSERT_TRUE(controller_->params_.interface_name.empty()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); +// ASSERT_TRUE(controller_->params_.state_joints.empty()); +// ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(joint_names_)); +// ASSERT_EQ(controller_->params_.interface_name, interface_name_); +// } + +// 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(), joint_command_values_.size()); +// for (size_t i = 0; i < command_intefaces.names.size(); ++i) +// { +// EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); +// } + +// auto state_intefaces = controller_->state_interface_configuration(); +// ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); +// for (size_t i = 0; i < state_intefaces.names.size(); ++i) +// { +// EXPECT_EQ(state_intefaces.names[i], joint_names_[i] + "/" + interface_name_); +// } +// } + +// 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 is reset +// auto msg = controller_->input_ref_.readFromNonRT(); +// EXPECT_EQ((*msg)->displacements.size(), joint_names_.size()); +// for (const auto & cmd : (*msg)->displacements) +// { +// EXPECT_TRUE(std::isnan(cmd)); +// } +// EXPECT_EQ((*msg)->velocities.size(), joint_names_.size()); +// for (const auto & cmd : (*msg)->velocities) +// { +// EXPECT_TRUE(std::isnan(cmd)); +// } + +// ASSERT_TRUE(std::isnan((*msg)->duration)); +// } + +// 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_->command_interfaces_[CMD_MY_ITFS].get_value(), 101.101); +// ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_TRUE(std::isnan(controller_->command_interfaces_[CMD_MY_ITFS].get_value())); + +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); +// } + +// TEST_F(MotionPrimitivesForwardControllerTest, test_setting_slow_mode_service) +// { +// SetUpController(); + +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); +// executor.add_node(service_caller_node_->get_node_base_interface()); + +// // initially set to false +// ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// // should stay false +// ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); + +// // set to true +// ASSERT_NO_THROW(call_service(true, executor)); +// ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); + +// // set back to false +// ASSERT_NO_THROW(call_service(false, executor)); +// ASSERT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +// } + +// TEST_F(MotionPrimitivesForwardControllerTest, test_update_logic_fast) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); +// executor.add_node(service_caller_node_->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// // set command statically +// static constexpr double TEST_DISPLACEMENT = 23.24; +// std::shared_ptr msg = std::make_shared(); +// msg->joint_names = joint_names_; +// msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); +// msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); +// msg->duration = std::numeric_limits::quiet_NaN(); +// controller_->input_ref_.writeFromNonRT(msg); + +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT); +// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); +// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::FAST); +// } + +// TEST_F(MotionPrimitivesForwardControllerTest, test_update_logic_slow) +// { +// SetUpController(); +// rclcpp::executors::MultiThreadedExecutor executor; +// executor.add_node(controller_->get_node()->get_node_base_interface()); +// executor.add_node(service_caller_node_->get_node_base_interface()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); +// ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// // set command statically +// static constexpr double TEST_DISPLACEMENT = 23.24; +// std::shared_ptr msg = std::make_shared(); +// // When slow mode is enabled command ends up being half of the value +// msg->joint_names = joint_names_; +// msg->displacements.resize(joint_names_.size(), TEST_DISPLACEMENT); +// msg->velocities.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); +// msg->duration = std::numeric_limits::quiet_NaN(); +// controller_->input_ref_.writeFromNonRT(msg); +// controller_->control_mode_.writeFromNonRT(control_mode_type::SLOW); + +// EXPECT_EQ(*(controller_->control_mode_.readFromRT()), control_mode_type::SLOW); +// ASSERT_EQ((*(controller_->input_ref_.readFromRT()))->displacements[0], TEST_DISPLACEMENT); +// ASSERT_EQ( +// controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)), +// controller_interface::return_type::OK); + +// EXPECT_EQ(joint_command_values_[STATE_MY_ITFS], TEST_DISPLACEMENT / 2); +// EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->displacements[0])); +// } + +// 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.set_point, 101.101); +// } + +// 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.set_point, 101.101); + +// publish_commands(); +// ASSERT_TRUE(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(joint_command_values_[CMD_MY_ITFS], 0.45); + +// subscribe_and_get_messages(msg); + +// ASSERT_EQ(msg.set_point, 0.45); +// } + +// 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..c63549ce49 --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp @@ -0,0 +1,271 @@ +// 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: + +// #ifndef TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ +// #define TEMPLATES__ROS2_CONTROL__CONTROLLER__TEST_MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ + +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include "motion_primitives_forward_controller/motion_primitives_forward_controller.hpp" +// #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 "rclcpp/executor.hpp" +// #include "rclcpp/parameter_value.hpp" +// #include "rclcpp/time.hpp" +// #include "rclcpp/utilities.hpp" +// #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +// // TODO(anyone): replace the state and command message types +// using ControllerStateMsg = motion_primitives_forward_controller::MotionPrimitivesForwardController::ControllerStateMsg; +// using ControllerReferenceMsg = motion_primitives_forward_controller::MotionPrimitivesForwardController::ControllerReferenceMsg; +// using ControllerModeSrvType = motion_primitives_forward_controller::MotionPrimitivesForwardController::ControllerModeSrvType; + +// 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, test_setting_slow_mode_service); +// FRIEND_TEST(MotionPrimitivesForwardControllerTest, test_update_logic_fast); +// FRIEND_TEST(MotionPrimitivesForwardControllerTest, test_update_logic_slow); + +// public: +// controller_interface::CallbackReturn on_configure( +// const rclcpp_lifecycle::State & previous_state) override +// { +// auto ret = motion_primitives_forward_controller::MotionPrimitivesForwardController::on_configure(previous_state); +// // Only if on_configure is successful create subscription +// if (ret == CallbackReturn::SUCCESS) +// { +// ref_subscriber_wait_set_.add_subscription(ref_subscriber_); +// } +// return ret; +// } + +// /** +// * @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. +// * +// * @return true if new ControllerReferenceMsg msg was received, false if timeout. +// */ +// bool wait_for_command( +// rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, +// const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) +// { +// bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; +// if (success) +// { +// executor.spin_some(); +// } +// return success; +// } + +// bool wait_for_commands( +// rclcpp::Executor & executor, +// const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) +// { +// return wait_for_command(executor, ref_subscriber_wait_set_, timeout); +// } + +// // TODO(anyone): add implementation of any methods of your controller is needed + +// private: +// rclcpp::WaitSet ref_subscriber_wait_set_; +// }; + +// // 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/commands", rclcpp::SystemDefaultsQoS()); + +// service_caller_node_ = std::make_shared("service_caller"); +// slow_control_service_client_ = service_caller_node_->create_client( +// "/test_motion_primitives_forward_controller/set_slow_control_mode"); +// } + +// 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(joint_command_values_.size()); +// command_ifs.reserve(joint_command_values_.size()); + +// for (size_t i = 0; i < joint_command_values_.size(); ++i) +// { +// command_itfs_.emplace_back(hardware_interface::CommandInterface( +// joint_names_[i], interface_name_, &joint_command_values_[i])); +// command_ifs.emplace_back(command_itfs_.back()); +// } +// // TODO(anyone): Add other command interfaces, if any + +// std::vector state_ifs; +// state_itfs_.reserve(joint_state_values_.size()); +// state_ifs.reserve(joint_state_values_.size()); + +// for (size_t i = 0; i < joint_state_values_.size(); ++i) +// { +// state_itfs_.emplace_back(hardware_interface::StateInterface( +// joint_names_[i], interface_name_, &joint_state_values_[i])); +// state_ifs.emplace_back(state_itfs_.back()); +// } +// // TODO(anyone): Add other state interfaces, if any + +// 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)); +// } + +// // TODO(anyone): add/remove arguments as it suites your command message type +// void publish_commands( +// const std::vector & displacements = {0.45}, +// const std::vector & velocities = {0.0}, const double duration = 1.25) +// { +// 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; +// } +// }; + +// wait_for_topic(command_publisher_->get_topic_name()); + +// ControllerReferenceMsg msg; +// msg.joint_names = joint_names_; +// msg.displacements = displacements; +// msg.velocities = velocities; +// msg.duration = duration; + +// command_publisher_->publish(msg); +// } + +// std::shared_ptr call_service( +// const bool slow_control, rclcpp::Executor & executor) +// { +// auto request = std::make_shared(); +// request->data = slow_control; + +// bool wait_for_service_ret = +// slow_control_service_client_->wait_for_service(std::chrono::milliseconds(500)); +// EXPECT_TRUE(wait_for_service_ret); +// if (!wait_for_service_ret) +// { +// throw std::runtime_error("Services is not available!"); +// } +// auto result = slow_control_service_client_->async_send_request(request); +// EXPECT_EQ(executor.spin_until_future_complete(result), rclcpp::FutureReturnCode::SUCCESS); + +// return result.get(); +// } + +// protected: +// // TODO(anyone): adjust the members as needed + +// // Controller-related parameters +// std::vector joint_names_ = {"joint1"}; +// std::vector state_joint_names_ = {"joint1state"}; +// std::string interface_name_ = "acceleration"; +// std::array joint_state_values_ = {1.1}; +// std::array joint_command_values_ = {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_; +// rclcpp::Client::SharedPtr slow_control_service_client_; +// }; + +// #endif // TEMPLATES__ROS2_CONTROL__CONTROLLER__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..1182c14ea1 --- /dev/null +++ b/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller_preceeding.cpp @@ -0,0 +1,77 @@ +// 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: + +// #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::control_mode_type; +// 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_.joints.empty()); +// ASSERT_TRUE(controller_->params_.state_joints.empty()); +// ASSERT_TRUE(controller_->params_.interface_name.empty()); + +// ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + +// ASSERT_THAT(controller_->params_.joints, testing::ElementsAreArray(joint_names_)); +// ASSERT_THAT(controller_->params_.state_joints, testing::ElementsAreArray(state_joint_names_)); +// ASSERT_THAT(controller_->state_joints_, testing::ElementsAreArray(state_joint_names_)); +// ASSERT_EQ(controller_->params_.interface_name, interface_name_); +// } + +// 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(), joint_command_values_.size()); +// for (size_t i = 0; i < command_intefaces.names.size(); ++i) +// { +// EXPECT_EQ(command_intefaces.names[i], joint_names_[i] + "/" + interface_name_); +// } + +// auto state_intefaces = controller_->state_interface_configuration(); +// ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); +// for (size_t i = 0; i < state_intefaces.names.size(); ++i) +// { +// EXPECT_EQ(state_intefaces.names[i], state_joint_names_[i] + "/" + interface_name_); +// } +// } + +// int main(int argc, char ** argv) +// { +// ::testing::InitGoogleTest(&argc, argv); +// rclcpp::init(argc, argv); +// int result = RUN_ALL_TESTS(); +// rclcpp::shutdown(); +// return result; +// }