add streaming position controller

flexiv_bringup/config/rizon_controllers.yaml

@@ -5,6 +5,9 @@ controller_manager:
     rizon_arm_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
+    streaming_position_controller:
+      type: streaming_position_controller/StreamingPositionController
+
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
@@ -35,3 +38,14 @@ rizon_arm_controller:
     constraints:
       stopped_velocity_tolerance: 0.01
       goal_time: 0.0
+
+streaming_position_controller:
+  ros__parameters:
+    joints:
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7

flexiv_bringup/launch/rizon.launch.py

@@ -111,7 +111,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             robot_controller_param_name,
             default_value="rizon_arm_controller",
-            description="Robot controller to start. Available: rizon_arm_controller",
+            description="Robot controller to start. Available: rizon_arm_controller, streaming_position_controller",
         )
     )
 

flexiv_controllers/streaming_position_controller/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.5)
+project(streaming_position_controller LANGUAGES CXX)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_interface
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  sensor_msgs
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/streaming_position_controller.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PRIVATE include)
+
+ament_target_dependencies(${PROJECT_NAME} PUBLIC
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+pluginlib_export_plugin_description_file(controller_interface streaming_position_controller.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
+ament_package()

flexiv_controllers/streaming_position_controller/include/streaming_position_controller/streaming_position_controller.hpp

@@ -0,0 +1,57 @@
+/**
+ * @file streaming_position_controller.hpp
+ * @brief Minimal ros2_control controller that accepts sensor_msgs/JointState
+ *        on a topic and writes positions directly to the hardware command
+ *        interface.
+ */
+
+#ifndef STREAMING_POSITION_CONTROLLER__STREAMING_POSITION_CONTROLLER_HPP_
+#define STREAMING_POSITION_CONTROLLER__STREAMING_POSITION_CONTROLLER_HPP_
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "controller_interface/controller_interface.hpp"
+#include "rclcpp/subscription.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "realtime_tools/realtime_thread_safe_box.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+
+namespace streaming_position_controller {
+
+class StreamingPositionController : public controller_interface::ControllerInterface
+{
+public:
+    StreamingPositionController();
+    ~StreamingPositionController() override = default;
+
+    controller_interface::CallbackReturn on_init() override;
+    controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
+    controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
+    controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
+
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+    controller_interface::return_type update(
+        const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+    std::vector<std::string> joint_names_;
+    std::vector<std::string> command_interface_types_;
+    std::unordered_map<std::string, std::size_t> name_to_index_;
+
+    realtime_tools::RealtimeThreadSafeBox<sensor_msgs::msg::JointState> rt_command_;
+    sensor_msgs::msg::JointState last_command_;
+
+    std::vector<std::size_t> pos_state_idx_;
+    std::vector<double> current_positions_;
+    bool positions_initialized_;
+
+    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr command_subscriber_;
+};
+
+}  // namespace streaming_position_controller
+
+#endif  // STREAMING_POSITION_CONTROLLER__STREAMING_POSITION_CONTROLLER_HPP_

flexiv_controllers/streaming_position_controller/package.xml

@@ -0,0 +1,27 @@
+<?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>streaming_position_controller</name>
+  <version>1.0.0</version>
+  <description>
+    Lightweight ros2_control controller that accepts sensor_msgs/JointState
+    on a topic and forwards joint positions to the hardware command interface
+    with configurable velocity clamping to prevent torque limit faults.
+  </description>
+  <maintainer email="support@flexiv.com">Flexiv</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>realtime_tools</depend>
+  <depend>sensor_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>