Vì sao nên bọc cuRobo bằng ROS 2 action?
Planner có latency biến thiên, có thể fail, và cần feedback. ROS 2 action phù hợp hơn service khi goal cần cancel, monitor progress hoặc timeout. Node cuRobo nên là một lifecycle node: configure để load config, activate để warmup, deactivate để dừng nhận goal.
Interface đề xuất
/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
Đừng để perception node gọi thẳng Python planner object trong cùng process nếu bạn chưa kiểm soát GIL, CUDA context và crash isolation. Một process planner riêng giúp restart dễ hơn.
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
PoseStamped từ ROS dùng quaternion xyzw. Bridge phải đổi sang wxyz trước khi gọi 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)
Watchdog và cancel
Ba timeout cần tách:
| Timeout | Ý nghĩa |
|---|---|
| joint state age | Không plan từ state cũ |
| planning deadline | Solver không được treo callback |
| controller execution | Robot không được chạy vô hạn |
Cancel action phải dừng cả planner request và controller goal. Nếu không cancel được CUDA computation ngay lập tức, ít nhất phải bỏ kết quả late result và không publish trajectory cũ.
Retiming và clamp
cuRobo trả trajectory hình học/smooth, nhưng controller thật cần giới hạn cụ thể:
for point in traj.points:
point.velocities = clamp(point.velocities, velocity_limits)
point.accelerations = clamp(point.accelerations, acceleration_limits)
Nếu robot arm có controller nội bộ, gửi điểm thưa hơn và để controller interpolate. Nếu controller chỉ nhận setpoint thô, cần spline/time parameterization phía bridge.
Diagnostics
Publish tối thiểu:
planner_ready;cuda_memory_allocated;last_plan_latency_ms;last_plan_success;last_failure_status;joint_state_age_ms;scene_age_ms.
Kết luận
ROS 2 bridge là đường biên safety quan trọng nhất giữa optimizer GPU và robot thật. Nếu bridge rõ interface, reorder joint theo tên, đổi quaternion đúng và có watchdog, bạn có thể thay đổi planner config mà không phá controller. Bài tiếp theo sẽ thêm depth scene mapping vào pipeline.
