MotoROS2 Point Queue Proxy Server

Learn how to overcome MotoROS2's 200-point trajectory limit and execute precision trajectories with thousands of points seamlessly. We share our Proxy Server design that simultaneously overcomes Micro-ROS structural limitations and QueueTrajPoint service data loss issues.

The Problem: MotoROS2's Structural Limitations

Operating Yaskawa industrial robots in a ROS 2 environment requires the official driver MotoROS2. MotoROS2 provides two interfaces for robot motion control:

Interface	Method	Purpose
FollowJointTrajectory Action	Batch transfer	Send entire trajectory at once
QueueTrajPoint Service	Point-by-Point	Send points one by one sequentially

Applications requiring high-resolution trajectories with thousands of points - such as precision welding, 3D printing paths, or complex assembly processes - face serious limitations with both methods.

Understanding MotoROS2 Architecture

To identify the root cause, we need to understand MotoROS2's internal architecture.

Key Components:

1. Micro-ROS Agent: Runs on PC, bridges standard ROS 2 DDS with XRCE-DDS for embedded environments
2. MotoROS2: Runs inside YRC1000 controller as a MotoPlus application
3. Motion API: Interpolates received points at 4ms intervals and delivers to servos

MotoROS2 uses Micro-ROS to implement ROS 2 communication in the embedded environment. This is where the first constraint arises.

Micro-ROS Memory Constraints

Micro-ROS is a ROS 2 implementation for resource-constrained microcontrollers. It uses the lightweight Micro XRCE-DDS instead of standard Fast-DDS, resulting in strict message size limitations.

Parameter	Default	Description
MTU (Maximum Transmission Unit)	512 bytes	Single transmission unit size
RMW_UXRCE_STREAM_HISTORY	4-8 slots	Reliable stream history
Maximum Message Size	~64 KB	XRCE protocol limit

Result: The maximum number of points that can be included in a FollowJointTrajectory message is limited to approximately 200.

JointTrajectory message size ~ N x (joints x 8 bytes x 4 + timestamp)
200 points x 6 joints x 32 bytes ~ 38.4 KB (< 64 KB limit)

Problem 1: FollowJointTrajectory's 200-Point Limit

The ROS 2 trajectory_msgs/JointTrajectory message specification itself has no point count limit. However, due to Micro-ROS buffer constraints, approximately 200 points per trajectory is the upper limit in MotoROS2.

Failure Mode Analysis

Phenomena when sending messages exceeding 200 points:

Symptoms:

• Messages "disappear into the void" (official GitHub Issue description)
• Action Server enters lock-up state
• Goal receipt cannot be confirmed

Common Workaround Attempts and Limitations

1. Trajectory Downsampling

# Reduce 2000 -> 200 points
downsampled = trajectory[::10]  # Select 1 every 10 points

• Problem: Precision degradation. Unsuitable for processes requiring high resolution

2. Trajectory Chunking

# Split and send sequentially in groups of 200
for chunk in [trajectory[i:i+200] for i in range(0, len(trajectory), 200)]:
    send_fjt_goal(chunk)
    wait_for_completion()  # Wait for completion

• Problem: Robot stutters at Action transition points. Continuity cannot be guaranteed

Problem 2: QueueTrajPoint Service BUSY Response

MotoROS2 provides the queue_traj_point service as an alternative to the 200-point limit. Despite the name, there is no actual internal queue.

BUSY Response Mechanism

Physical Consequences of Data Loss

If point $P_n$ is discarded with BUSY and the client doesn't retransmit:

1. Robot attempts to move directly from $P_{n-1}$ to $P_{n+1}$
2. Discontinuity occurs in trajectory
3. Sudden position change -> Motor torque overload

Results:

• Risk of mechanical damage from physical shock
• Controller error and emergency stop (E-Stop) activation
• Process interruption and productivity loss

Mathematical Analysis of Trajectory Discontinuity

In continuous trajectories, continuity of position $q$, velocity $\dot{q}$, and acceleration $\ddot{q}$ is important:

$$\text{Position: } \lim_{t \to t_n^-} q(t) = \lim_{t \to t_n^+} q(t)$$ $$\text{Velocity: } \lim_{t \to t_n^-} \dot{q}(t) = \lim_{t \to t_n^+} \dot{q}(t)$$

When point $P_n$ is lost, the controller targets $P_{n+1}$ in the next time slot. Assuming equally-spaced trajectory ($q_{n+1} - q_n \approx q_n - q_{n-1}$):

$$\text{Normal: } \dot{q} = \frac{q_n - q_{n-1}}{\Delta t}, \quad \text{Lost: } \dot{q}' = \frac{q_{n+1} - q_{n-1}}{\Delta t} \approx \frac{2(q_n - q_{n-1})}{\Delta t} = 2\dot{q}$$

Approximately twice the distance must be traveled within the same control period $\Delta t$, so required velocity doubles suddenly.

From a robot dynamics perspective, this leads to rapid torque changes:

$$\tau = M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q)$$

When $\ddot{q}$ spikes, $\tau$ (joint torque) also spikes, activating motor protection circuits or causing mechanical shock.

Solution: Point Queue Proxy Server

To solve both problems simultaneously, we designed a Proxy Server that acts as a mediator between MoveIt2 and MotoROS2.

Design Goals

Goal	Description
Transparent Proxy	Upper controllers (MoveIt2) use it like a standard robot controller
Unlimited Trajectory	Receive trajectories without point count limits
Lossless Streaming	All points delivered to robot in order without loss
Continuous Motion	Seamless operation in continuous processes

System Architecture

Interface Design:

• Northbound (To Planner): FollowJointTrajectory Action Server

  • Receives entire trajectories as Batch from upper planners like MoveIt2
  • No point count limit

• Southbound (To Robot): QueueTrajPoint Service Client

  • Sends points to MotoROS2 one at a time
  • Flow Control applied

• Feedback Loop: /joint_states Subscriber

  • Real-time monitoring of robot's actual joint positions
  • Convergence determination for motion completion

Core Mechanism 1: Reliable Streaming with Flow Control

A Smart Retry mechanism borrowing TCP flow control principles.

Key Parameters:

Parameter	Recommended	Description
busy_wait_time	4-8 ms	Wait time between retries (based on motion period)
max_retries	50-100	Maximum retry count
service_timeout	100 ms	Service response timeout

Flow Control Principle:

The robot controller consumes points at a 4ms period. The Proxy Server interprets BUSY responses as "consumer still processing" signals and retransmits after appropriate delay. This is producer-consumer rate synchronization similar to TCP's sliding window.

// Backpressure-based flow control
int queue_point_with_retry(const trajectory_msgs::msg::JointTrajectoryPoint& point)
{
    int attempt = 0;
    while (rclcpp::ok() && attempt < max_retries_) {
        auto request = std::make_shared<motoros2_interfaces::srv::QueueTrajPoint::Request>();
        request->point = point;

        auto future = queue_point_client_->async_send_request(request);
        auto status = future.wait_for(service_timeout_);

        if (status == std::future_status::timeout) {
            RCLCPP_WARN(get_logger(), "Service timeout, attempt %d/%d",
                        attempt + 1, max_retries_);
            ++attempt;
            std::this_thread::sleep_for(busy_wait_time_);  // Wait after timeout too
            continue;
        }

        auto result = future.get();

        // Return result if not BUSY (SUCCESS or error)
        if (result->result_code != ResultCode::BUSY) {
            return result->result_code;
        }

        // Backpressure: Wait until consumer is ready, then retry
        ++attempt;
        std::this_thread::sleep_for(busy_wait_time_);
    }

    RCLCPP_ERROR(get_logger(), "Max retries (%d) exceeded", max_retries_);
    return ResultCode::ERROR;
}

Core Mechanism 2: Goal Queueing

Complex processes require continuous execution of multiple trajectories. The conventional approach required waiting for the current motion to finish before sending the next command.

Goal Queueing accepts new Goals even during motion execution, stores them in an internal queue, and automatically starts the next motion immediately upon current motion completion.

Advantages of Asynchronous Queueing:

1. Upper controller can pre-send commands without checking robot state
2. Minimized transition delays between motions -> Seamless continuous processes
3. Robust architecture against network delays

Core Mechanism 3: Convergence Detection

All points sent != Physical motion complete

We must confirm the robot has actually reached the last point before safely starting the next trajectory.

Convergence Determination Algorithm:

$$\text{converged} = \max_{i \in \text{joints}} |q_i^{\text{actual}} - q_i^{\text{target}}| < \epsilon$$

Where:

• $q_i^{\text{actual}}$: Actual joint position received from /joint_states
• $q_i^{\text{target}}$: Last point of trajectory
• $\epsilon$: Convergence threshold (e.g., 0.001 rad = 0.057 deg)

bool check_convergence(const std::vector<double>& target_positions)
{
    auto current_positions = get_current_joint_positions();

    // Joint count mismatch check
    if (current_positions.size() != target_positions.size()) {
        RCLCPP_ERROR(get_logger(), "Joint count mismatch: %zu vs %zu",
                     current_positions.size(), target_positions.size());
        return false;
    }

    for (size_t i = 0; i < target_positions.size(); ++i) {
        double error = std::abs(current_positions[i] - target_positions[i]);
        if (error > convergence_threshold_) {
            return false;
        }
    }
    return true;
}

void wait_for_convergence(const std::vector<double>& target_positions,
                          std::chrono::milliseconds timeout)
{
    auto start = std::chrono::steady_clock::now();

    while (rclcpp::ok()) {
        if (check_convergence(target_positions)) {
            RCLCPP_INFO(get_logger(), "Convergence achieved");
            return;
        }

        auto elapsed = std::chrono::steady_clock::now() - start;
        if (elapsed > timeout) {
            RCLCPP_WARN(get_logger(), "Convergence timeout");
            return;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

Implementation Details

ROS 2 Action Server and Threading Model

ROS 2's Executor manages callback execution. In the default Single-Threaded Executor, long loops within handle_accepted callbacks block other callbacks.

Solution: Create a worker thread in handle_accepted for asynchronous processing

class PointQueueProxy : public rclcpp::Node
{
public:
    PointQueueProxy() : Node("point_queue_proxy")
    {
        // Create Action Server
        action_server_ = rclcpp_action::create_server<FollowJointTrajectory>(
            this,
            "follow_joint_trajectory",
            std::bind(&PointQueueProxy::handle_goal, this, _1, _2),
            std::bind(&PointQueueProxy::handle_cancel, this, _1),
            std::bind(&PointQueueProxy::handle_accepted, this, _1)
        );

        // Create Service Client
        queue_point_client_ = create_client<QueueTrajPoint>("queue_traj_point");

        // Joint States Subscriber
        joint_states_sub_ = create_subscription<JointState>(
            "joint_states", 10,
            std::bind(&PointQueueProxy::joint_states_callback, this, _1)
        );
    }

private:
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID& uuid,
        std::shared_ptr<const FollowJointTrajectory::Goal> goal)
    {
        RCLCPP_INFO(get_logger(),
            "Received goal with %zu points",
            goal->trajectory.points.size());
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    void handle_accepted(const std::shared_ptr<GoalHandleFJT> goal_handle)
    {
        std::lock_guard<std::mutex> lock(queue_mutex_);

        if (is_executing_) {
            // If executing, store in queue
            goal_queue_.push_back(goal_handle);
            RCLCPP_INFO(get_logger(), "Goal queued. Queue size: %zu",
                        goal_queue_.size());
        } else {
            // If idle, execute immediately
            is_executing_ = true;
            std::thread(&PointQueueProxy::execute_trajectory, this, goal_handle)
                .detach();
        }
    }

    void execute_trajectory(std::shared_ptr<GoalHandleFJT> goal_handle)
    {
        auto goal = goal_handle->get_goal();
        auto result = std::make_shared<FollowJointTrajectory::Result>();

        RCLCPP_INFO(get_logger(),
            "Executing trajectory with %zu points",
            goal->trajectory.points.size());

        // Stream all points
        for (size_t i = 0; i < goal->trajectory.points.size(); ++i) {
            if (goal_handle->is_canceling()) {
                goal_handle->canceled(result);
                process_next_goal();
                return;
            }

            int ret = queue_point_with_retry(goal->trajectory.points[i]);
            if (ret != ResultCode::SUCCESS) {
                result->error_code = FollowJointTrajectory::Result::INVALID_GOAL;
                goal_handle->abort(result);
                process_next_goal();
                return;
            }

            // Publish Feedback (optional)
            if (i % 100 == 0) {
                auto feedback = std::make_shared<FollowJointTrajectory::Feedback>();
                feedback->desired = goal->trajectory.points[i];
                goal_handle->publish_feedback(feedback);
            }
        }

        // Wait for Convergence
        auto last_point = goal->trajectory.points.back();
        wait_for_convergence(last_point.positions,
                             std::chrono::seconds(30));

        result->error_code = FollowJointTrajectory::Result::SUCCESSFUL;
        goal_handle->succeed(result);

        process_next_goal();
    }

    void process_next_goal()
    {
        std::lock_guard<std::mutex> lock(queue_mutex_);

        if (!goal_queue_.empty()) {
            auto next_goal = goal_queue_.front();
            goal_queue_.pop_front();
            std::thread(&PointQueueProxy::execute_trajectory, this, next_goal)
                .detach();
        } else {
            is_executing_ = false;
        }
    }

    // Member variables
    rclcpp_action::Server<FollowJointTrajectory>::SharedPtr action_server_;
    rclcpp::Client<QueueTrajPoint>::SharedPtr queue_point_client_;
    rclcpp::Subscription<JointState>::SharedPtr joint_states_sub_;

    std::mutex queue_mutex_;
    std::deque<std::shared_ptr<GoalHandleFJT>> goal_queue_;
    std::atomic<bool> is_executing_{false};

    // Configuration
    std::chrono::milliseconds busy_wait_time_{5};
    int max_retries_{100};
    double convergence_threshold_{0.001};  // rad
};

Parameter Tuning Guide

Parameter	Low Value	High Value	Recommended
busy_wait_time	Fast send, frequent BUSY	Slow send, stable	4-8 ms
max_retries	Fast failure, unstable	Slow failure, stable	50-100
convergence_threshold	Precise, long wait	Loose, fast transition	0.001 rad

Tuning Principles:

1. busy_wait_time: Set based on MotoROS2's internal Motion API operating at 4ms period
2. max_retries: Set generously considering network delays and complex trajectories
3. convergence_threshold: Match with subsequent trajectory starting point tolerance to prevent discontinuity

Experimental Results

Test Environment

Item	Specification
Robot	Yaskawa GP12 (6-axis)
Controller	YRC1000
MotoROS2 Version	0.1.1
ROS 2 Distribution	Humble
Network	1 Gbps Ethernet (direct)

Quantitative Results

Metric	Before (Default MotoROS2)	After (Proxy Server)
Max points/trajectory	200	10,000+
Trajectory continuity	Stuttering when split	Fully continuous
Point loss rate	Variable (when BUSY unhandled)	0%
Continuous process wait time	~500ms (Action transition)	< 10ms

Real Application Cases

Precision Welding Pattern

• Complex seam tracking: 3,200 points
• Before: Split into 16 Actions -> Welding quality degradation
• Proxy Server: Single Action continuous execution -> Uniform weld bead

3D Printing Path

• Average 5,000+ points per layer
• Seamless extrusion improves layer quality

Limitations and Future Work

Current Limitations

1. Real-time Servoing Not Supported

   • Current architecture is optimized for pre-planned trajectory execution
   • Additional development needed for real-time path correction based on sensor feedback

2. Single Robot Control

   • Multi-robot synchronization not supported
   • Goal synchronization mechanism planned for future

3. Error Recovery

   • Rollback mechanism for partially executed trajectories on mid-execution failure not implemented

Future Improvements

• MotoROS2 Point Streaming Interface (v0.0.15+) integration
• Multi-robot synchronization support
• Trajectory Blending for smoother transitions

Key Takeaways

1. MotoROS2's 200-point limit stems from Micro-ROS message size constraints (~64KB).

2. QueueTrajPoint service has no actual queue, causing data loss on BUSY responses.

3. Point Queue Proxy Server solves both problems simultaneously:

   • Reliable Streaming: Flow control prevents point loss
   • Goal Queueing: Seamless execution of continuous processes
   • Convergence Detection: Confirms physical motion completion

4. Core Technology: Applying TCP flow control principles to robot motion control

Precise trajectory control can overcome driver limitations through appropriate middleware design.