Add motion_primitives_forward_controller for interfacing motion primitive messages with hardware interfaces

motion_primitives_forward_controller/CMakeLists.txt

@@ -0,0 +1,118 @@
+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)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(motion_primitives_forward_controller_parameters
+  src/motion_primitives_forward_controller.yaml
+)
+add_library(
+  motion_primitives_forward_controller
+  SHARED
+  src/motion_primitives_forward_controller.cpp
+)
+target_include_directories(motion_primitives_forward_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(motion_primitives_forward_controller PUBLIC
+                      motion_primitives_forward_controller_parameters
+                      controller_interface::controller_interface
+                      hardware_interface::hardware_interface
+                      pluginlib::pluginlib
+                      rclcpp::rclcpp
+                      rclcpp_lifecycle::rclcpp_lifecycle
+                      realtime_tools::realtime_tools
+                      ${control_msgs_TARGETS}
+                      ${std_srvs_TARGETS}
+)
+
+target_compile_definitions(motion_primitives_forward_controller PRIVATE "MOTION_PRIMITIVES_FORWARD_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(
+  controller_interface motion_primitives_forward_controller.xml)
+
+install(
+  TARGETS
+    motion_primitives_forward_controller
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(
+    test_load_motion_primitives_forward_controller
+    test/test_load_motion_primitives_forward_controller.cpp
+  )
+  target_link_libraries(test_load_motion_primitives_forward_controller
+    motion_primitives_forward_controller
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(
+    test_motion_primitives_forward_controller
+    test/test_motion_primitives_forward_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/motion_primitives_forward_controller_params.yaml
+  )
+  target_link_libraries(test_motion_primitives_forward_controller
+    motion_primitives_forward_controller
+    controller_interface::controller_interface
+    hardware_interface::hardware_interface
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(
+    test_motion_primitives_forward_controller_preceeding
+    test/test_motion_primitives_forward_controller_preceeding.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/motion_primitives_forward_controller_preceeding_params.yaml
+  )
+  target_link_libraries(test_motion_primitives_forward_controller_preceeding
+    motion_primitives_forward_controller
+    controller_interface::controller_interface
+    hardware_interface::hardware_interface
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_dependencies(
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+ament_export_libraries(
+  motion_primitives_forward_controller
+)
+
+ament_package()

motion_primitives_forward_controller/README.md

@@ -0,0 +1,35 @@
+motion_primitives_forward_controller
+==========================================
+
+Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC)
+
+[![Licence](https://img.shields.io/badge/License-Apache-2.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
+
+
+This project provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotionPrimitiveSequence.action` action from [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action). The controller receives the primitives via the action interface and forwards them through command interfaces to the robot-specific hardware interface. Currently, hardware interfaces for [Universal Robots](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and [KUKA Robots](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver) are implemented.
+
+- Supported motion primitives:
+  - `LINEAR_JOINT`
+  - `LINEAR_CARTESIAN`
+  - `CIRCULAR_CARTESIAN`
+
+If multiple motion primitives are passed to the controller via the action, the controller forwards them to the hardware interface as a sequence. To do this, it first sends `MOTION_SEQUENCE_START`, followed by each individual primitive, and finally `MOTION_SEQUENCE_END`. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives.
+
+The action interface also allows stopping the current execution of motion primitives. When a stop request is received, the controller sends `STOP_MOTION` to the hardware interface, which then halts the robot's movement. Once the controller receives confirmation that the robot has stopped, it sends `RESET_STOP` to the hardware interface. After that, new commands can be sent.
+
+![Action Image](doc/Moprim_Controller_ExecuteMotion_Action.drawio.png)
+
+This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/examples/send_dummy_motion_primitives_hka_ur10e.py) in the `Universal_Robots_ROS2_Driver` package.
+
+## Architecture Overview
+The following diagram shows the architecture for a UR robot.
+For this setup, the [motion primitives mode of the `Universal_Robots_ROS2_Driver`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) is used.
+
+![UR Robot Architecture](doc/ros2_control_motion_primitives_ur_integrated.drawio.png)
+
+
+# TODOs/ improvements
+- Use references for command and state interfaces to improve code readability and less error-prone.
+- Extend the tests
+   - Test for a sequence of multiple primitives
+   - Test for canceling movement

motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp

@@ -0,0 +1,124 @@
+// 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 <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <motion_primitives_forward_controller/motion_primitives_forward_controller_parameters.hpp>
+#include <realtime_tools/lock_free_queue.hpp>
+#include <realtime_tools/realtime_server_goal_handle.hpp>
+#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 "control_msgs/action/execute_motion_primitive_sequence.hpp"
+#include "control_msgs/msg/motion_primitive.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+namespace motion_primitives_forward_controller
+{
+enum class ExecutionState : uint8_t
+{
+  IDLE = 0,
+  EXECUTING = 1,
+  SUCCESS = 2,
+  ERROR = 3,
+  STOPPED = 4
+};
+
+using MotionType = control_msgs::msg::MotionPrimitive;
+enum class MotionHelperType : uint8_t
+{
+  STOP_MOTION = 66,
+  RESET_STOP = 67,
+
+  MOTION_SEQUENCE_START = 100,
+  MOTION_SEQUENCE_END = 101
+};
+
+enum class ReadyForNewPrimitive : uint8_t
+{
+  NOT_READY = 0,
+  READY = 1
+};
+
+class MotionPrimitivesForwardController : public controller_interface::ControllerInterface
+{
+public:
+  MotionPrimitivesForwardController();
+
+  controller_interface::CallbackReturn on_init() override;
+
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+protected:
+  std::shared_ptr<motion_primitives_forward_controller::ParamListener> param_listener_;
+  motion_primitives_forward_controller::Params params_;
+
+  using MotionPrimitive = control_msgs::msg::MotionPrimitive;
+  realtime_tools::LockFreeSPSCQueue<MotionPrimitive, 1024> moprim_queue_;
+
+  using ExecuteMotionAction = control_msgs::action::ExecuteMotionPrimitiveSequence;
+  rclcpp_action::Server<ExecuteMotionAction>::SharedPtr action_server_;
+  rclcpp_action::GoalResponse goal_received_callback(
+    const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const ExecuteMotionAction::Goal> goal);
+  rclcpp_action::CancelResponse goal_cancelled_callback(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotionAction>> goal_handle);
+  void goal_accepted_callback(
+    std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotionAction>> goal_handle);
+  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<ExecuteMotionAction>;
+  std::shared_ptr<RealtimeGoalHandle> realtime_goal_handle_;
+  rclcpp::TimerBase::SharedPtr goal_handle_timer_;
+  rclcpp::Duration action_monitor_period_ = rclcpp::Duration(std::chrono::milliseconds(20));
+
+  void reset_command_interfaces();
+  bool set_command_interfaces();
+
+  bool print_error_once_ = true;
+  // cancel requested by the action server
+  std::atomic<bool> cancel_requested_ = false;
+  // robot stop command sent to the hardware interface
+  std::atomic<bool> robot_stop_requested_ = false;
+  bool was_executing_ = false;
+  ExecutionState execution_status_;
+  ReadyForNewPrimitive ready_for_new_primitive_;
+  MotionPrimitive current_moprim_;
+};
+
+}  // namespace motion_primitives_forward_controller
+
+#endif  // MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_

motion_primitives_forward_controller/motion_primitives_forward_controller.xml

@@ -0,0 +1,8 @@
+<library path="motion_primitives_forward_controller">
+  <class name="motion_primitives_forward_controller/MotionPrimitivesForwardController"
+         type="motion_primitives_forward_controller::MotionPrimitivesForwardController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    MotionPrimitivesForwardController ros2_control controller.
+  </description>
+  </class>
+</library>

motion_primitives_forward_controller/package.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>motion_primitives_forward_controller</name>
+  <version>5.2.0</version>
+  <description>Package to control robots using motion primitives like PTP, LIN and CIRC</description>
+
+  <maintainer email="[email protected]">Bence Magyar</maintainer>
+  <maintainer email="[email protected]">Denis Štogl</maintainer>
+  <maintainer email="[email protected]">Christoph Froehlich</maintainer>
+  <maintainer email="[email protected]">Sai Kishor Kothakota</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://control.ros.org</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
+  <url type="repository">https://github.com/ros-controls/ros2_controllers/</url>
+
+  <author email="[email protected]">Mathias Fuhrer</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>generate_parameter_library</build_depend>
+
+  <depend>control_msgs</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>std_srvs</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>