Add motion_primitives_forward_controller for interfacing motion primitive messages with hardware interfaces

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
+  "$<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/${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()

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)

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

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 <memory>
+#include <string>
+#include <vector>
+#include <queue>
+
+#include "controller_interface/controller_interface.hpp"
+#include <motion_primitives_forward_controller/motion_primitives_forward_controller_parameters.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;
+
+    // state and command message types
+    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_;
+
+  // Command subscribers and Controller State publisher
+  rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
+  // realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
+
+  using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
+
+  rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
+  std::unique_ptr<ControllerStatePublisher> state_publisher_;
+
+private:
+  // callback for topic interface
+  void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg); // callback for reference message
+  // std::atomic<bool> 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<std::shared_ptr<ControllerReferenceMsg>> 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_

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

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