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
+  industrial_robot_motion_interfaces
+)
+
+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
+  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}
+)
+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)
+  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_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,38 @@
+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 `ExecuteMotion.action` action from the [industrial_robot_motion_interfaces](https://github.com/b-robotized-forks/industrial_robot_motion_interfaces/tree/helper-types) package. 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/b-robotized-forks/Universal_Robots_ROS2_Driver_MotionPrimitive) 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.
+
+This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/b-robotized-forks/Universal_Robots_ROS2_Driver_MotionPrimitive/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/b-robotized-forks/Universal_Robots_ROS2_Driver_MotionPrimitive) is used.
+
+![UR Robot Architecture](doc/ros2_control_motion_primitives_ur_integrated.drawio.png)
+
+
+# Related packages/ repos
+- [industrial_robot_motion_interfaces](https://github.com/b-robotized-forks/industrial_robot_motion_interfaces/tree/helper-types)
+- [ros2_controllers with motion_primitives_forward_controller](https://github.com/b-robotized-forks/ros2_controllers/tree/motion_primitive_forward_controller/motion_primitives_forward_controller)
+- [Universal_Robots_ROS2_Driver with motion_primitive_ur_driver](https://github.com/b-robotized-forks/Universal_Robots_ROS2_Driver_MotionPrimitive)
+- [Universal_Robots_Client_Library](https://github.com/UniversalRobots/Universal_Robots_Client_Library)
+
+# TODOs/ improvements
+- Extend the tests
+   - Test for a sequence of multiple primitives
+   - Test for canceling movement

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,98 @@
+// 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/action/execute_motion.hpp"
+#include "industrial_robot_motion_interfaces/msg/motion_primitive.hpp"
+#include "rclcpp_action/rclcpp_action.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;
+
+protected:
+  std::shared_ptr<motion_primitives_forward_controller::ParamListener> param_listener_;
+  motion_primitives_forward_controller::Params params_;
+
+  using MotionPrimitiveMsg = industrial_robot_motion_interfaces::msg::MotionPrimitive;
+  std::queue<std::shared_ptr<MotionPrimitiveMsg>> moprim_queue_;
+
+  using ExecuteMotion = industrial_robot_motion_interfaces::action::ExecuteMotion;
+  rclcpp_action::Server<ExecuteMotion>::SharedPtr action_server_;
+  rclcpp_action::GoalResponse goal_received_callback(
+    const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const ExecuteMotion::Goal> goal);
+  rclcpp_action::CancelResponse goal_cancelled_callback(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle);
+  void goal_accepted_callback(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle);
+  void execute_goal(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle);
+
+  void reset_command_interfaces();
+  bool set_command_interfaces();
+
+  size_t queue_size_ = 0;
+  std::mutex command_mutex_;
+  bool print_error_once_ = true;
+  bool robot_stopped_ = false;
+  bool was_executing_ = false;
+  uint8_t execution_status_;
+};
+
+}  // 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_

motion_primitives_forward_controller/include/motion_primitives_forward_controller/validate_motion_primitives_forward_controller_parameters.hpp

@@ -0,0 +1,38 @@
+// Copyright (c) 2025, b»robotized
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_PRIMITIVES_FORWARD_CONTROLLER__VALIDATE_MOTION_PRIMITIVES_FORWARD_CONTROLLER_PARAMETERS_HPP_
+#define MOTION_PRIMITIVES_FORWARD_CONTROLLER__VALIDATE_MOTION_PRIMITIVES_FORWARD_CONTROLLER_PARAMETERS_HPP_
+
+#include <string>
+
+#include "parameter_traits/parameter_traits.hpp"
+
+namespace parameter_traits
+{
+Result forbidden_interface_name_prefix(rclcpp::Parameter const & parameter)
+{
+  auto const & interface_name = parameter.as_string();
+
+  if (interface_name.rfind("blup_", 0) == 0)
+  {
+    return ERROR("'interface_name' parameter can not start with 'blup_'");
+  }
+
+  return OK;
+}
+
+}  // namespace parameter_traits
+
+#endif  // MOTION_PRIMITIVES_FORWARD_CONTROLLER__VALIDATE_MOTION_PRIMITIVES_FORWARD_CONTROLLER_PARAMETERS_HPP_