Add motion_primitives_forward_controller for interfacing motion primitive messages with hardware interfaces

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
+  "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+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()

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)

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_

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 <memory>
+#include <queue>
+#include <string>
+#include <vector>
+
+#include <motion_primitives_forward_controller/motion_primitives_forward_controller_parameters.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 "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<motion_primitives_forward_controller::ParamListener> param_listener_;
+  motion_primitives_forward_controller::Params params_;
+
+  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
+
+  using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
+
+  rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
+  std::unique_ptr<ControllerStatePublisher> state_publisher_;
+
+  std::queue<std::shared_ptr<ControllerReferenceMsg>> msg_queue_;
+
+private:
+  void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
+  void reset_command_interfaces();
+  bool set_command_interfaces();
+  void reset_controller_reference_msg(std::shared_ptr<ControllerReferenceMsg> & 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_

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_

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_