A ROS 2 bridge for cuRobo: from goal pose to JointTrajectory

Why wrap cuRobo with a ROS 2 action?

Planning has variable latency, can fail, and needs feedback. A ROS 2 action is a better fit than a service when goals need cancelation, progress, or timeout. The cuRobo node should be a lifecycle node: configure loads config, activate runs warmup, deactivate stops accepting goals.

Proposed interface

/curobo/plan_pose        action: PlanPose
/curobo/plan_grasp       action: PlanGrasp
/curobo/world            topic: PlanningScene
/joint_states            subscription
/arm_controller/follow_joint_trajectory action client
/curobo/status           topic: diagnostics

Avoid letting perception call the Python planner object directly in the same process unless you have handled the GIL, CUDA context ownership, and crash isolation. A dedicated planner process is easier to restart.

PlanPose action

geometry_msgs/PoseStamped goal_pose
string link_name
float32 max_planning_time
bool execute
---
bool success
string status
trajectory_msgs/JointTrajectory trajectory
---
string phase
float32 progress

ROS PoseStamped uses xyzw quaternions. The bridge must convert to wxyz before calling cuRobo.

Callback flow

def on_goal(goal):
    assert active
    joint_msg = wait_latest_joint_state(max_age=0.05)
    q = reorder_joint_state(joint_msg, curobo_joint_names)
    scene = latest_scene_snapshot()
    pose = ros_pose_to_curobo_pose(goal.goal_pose.pose)

    result = planner.plan_pose(q, pose, scene)
    if not result.success:
        return failed(result.status)

    traj = retime_and_clamp(result.interpolated_plan)
    if goal.execute:
        send_to_follow_joint_trajectory(traj)
    return success(traj)

Watchdogs and cancelation

Keep three timeouts separate:

Canceling an action should cancel both the planner request and the controller goal. If the CUDA computation cannot be interrupted immediately, discard late results and never publish an old trajectory.

Retiming and clamps

cuRobo returns a smooth geometric trajectory, but hardware controllers need concrete limits:

for point in traj.points:
    point.velocities = clamp(point.velocities, velocity_limits)
    point.accelerations = clamp(point.accelerations, acceleration_limits)

If the robot arm has an internal controller, send sparser points and let it interpolate. If it only accepts raw setpoints, add spline or time parameterization in the bridge.

Diagnostics

Publish at least:

• planner_ready;
• cuda_memory_allocated;
• last_plan_latency_ms;
• last_plan_success;
• last_failure_status;
• joint_state_age_ms;
• scene_age_ms.

Conclusion

The ROS 2 bridge is the safety boundary between a GPU optimizer and real hardware. With a clear interface, name-based joint reordering, quaternion conversion, and watchdogs, you can tune planner config without breaking the controller.