feat(vla_sim): collect demonstrations with the scripted oracle - #830
feat(vla_sim): collect demonstrations with the scripted oracle#830danwahl wants to merge 2 commits into
Conversation
📝 WalkthroughSummary by CodeRabbit
WalkthroughThe PR adds a cube-stack VLA simulation workflow with MuJoCo layouts, BehaviorTree recording objectives, MoveIt behavior plugins, joint-spline planning, gripper and recording controls, and a ROS 2 joint-state bridge. ChangesCube-stack VLA workflow
Possibly related PRs
Suggested reviewers: Caution Pre-merge checks failedPlease resolve all errors before merging. Addressing warnings is optional.
❌ Failed checks (1 error)
✅ Passed checks (3 passed)
Comment |
|
Consider whether the change should land upstream in Overlapping files
|
|
[written by AI] Docs for this branch: PickNikRobotics/moveit_pro#21138 (stacked on the training-data guide, #20932). |
417fdb3 to
011b7be
Compare
41e62ae to
d2e550c
Compare
|
360 train + 150 eval randomized cube layouts, reachable by name through /mujoco_system/reset_keyframe. The eval set is held out from the shipped checkpoint's demonstrations, so it is the only fair set to score on.
d2e550c to
42fdd29
Compare
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
|
42fdd29 to
f33dd0d
Compare
|
[written by AI] For anyone following the |
|
f33dd0d to
331742d
Compare
|
Adds the recording half of the config, so a replacement policy can be trained without leaving it: a scripted stacking oracle, per-prompt sweeps over the training layouts, and the joint-command bridge the Trainer needs to label `action` from commands rather than next states. vla_sim_behaviors carries the four Behaviors the oracle needs. Its SendGripperCommand test aborted the whole binary about one run in three: the stalling action server it stands up was destroyed while the executor could still dispatch its callbacks. The test now stops the executor first.
331742d to
a6e5f27
Compare
|
There was a problem hiding this comment.
Actionable comments posted: 10
🧹 Nitpick comments (2)
src/vla_sim/description/mujoco/keyframes.xml (1)
22-33: 📐 Maintainability & Code Quality | 🔵 Trivial | 💤 Low valueConsider moving each prompt comment above the key it describes.
Each prompt comment follows its
<key>element. The comment on line 27 therefore belongs toeval_0, not toeval_1that starts on line 28. The header on lines 13-15 documents this, and the final comment on line 3081 aftertrain_359confirms it. A reader who assumes the usual leading-comment convention pairs every layout with the wrong prompt, and a wrong prompt is not detectable from the layout data.This file is generated, so the fix belongs in the generator. The current form is correct, so treat this as a readability change only.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@src/vla_sim/description/mujoco/keyframes.xml` around lines 22 - 33, Move each prompt comment in the keyframe generator so it appears immediately before the corresponding <key> element rather than after it. Preserve the generated keyframe content and ordering, including the existing association between each prompt and key such as eval_0 and eval_1; this is a readability-only change.src/vla_sim_behaviors/test/test_send_gripper_command.cpp (1)
45-48: 🩺 Stability & Availability | 🔵 Trivial | 💤 Low value
received_.set_valuethrows if a second goal arrives.The goal callback calls
set_valueon every accepted goal. A second goal makesstd::promise::set_valuethrowstd::future_errorinside an rclcpp callback. Only one goal is sent today, so this is latent. If you add a test that sends two goals, guard the promise with astd::once_flagor aboolundermutex_.🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the rest with a brief reason, keep changes minimal, and validate. In `@src/vla_sim_behaviors/test/test_send_gripper_command.cpp` around lines 45 - 48, Update the goal callback’s received_ fulfillment so it records only the first accepted goal; guard set_value with the existing mutex_ and a bool or std::once_flag, preventing subsequent goals from calling std::promise::set_value again.
🤖 Prompt for all review comments with AI agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
Inline comments:
In `@src/vla_sim_behaviors/src/compute_top_down_keyposes.cpp`:
- Around line 225-240: Update the cost_of lambda to validate IK for every
keypose generated from heights, while retaining the existing jointDistanceCost
based on the first approach pose. Return std::nullopt if IK fails for any later
height, so yaw selection excludes candidates that PlanJointSplineThroughPoses
cannot execute. Add a regression test covering a yaw that succeeds at
heights.front() but fails at a subsequent height.
In `@src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp`:
- Around line 360-367: Validate joint_velocity_scale before constructing
velocity_cap, requiring it to be finite and within the inclusive range (0.0,
1.0]. Reject invalid values before the bounded-joint velocity-cap calculation so
zero, NaN, and values above the model limit cannot reach the timing helpers.
- Around line 224-237: The duration calculation in splineDuration must retain
the minimum-duration floor without upper-clamping the required duration; update
the planner at src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp
lines 224-237 and return FAILURE when that required duration exceeds
kMaximumDuration at lines 393-394. Update
src/vla_sim_behaviors/test/test_plan_joint_spline_through_poses.cpp lines
176-180 to assert planner failure instead of expecting a 60-second trajectory.
- Around line 393-400: Validate the duration-to-sample calculation in the
trajectory generation flow before converting it to std::size_t or reserving
points. Enforce a defined maximum point count (including the final sample) and
return FAILURE when the configured sampling_rate or resulting count exceeds that
limit; otherwise preserve the existing loop and trajectory construction
behavior.
In `@src/vla_sim_behaviors/src/send_gripper_command.cpp`:
- Around line 100-104: Update SendGripperCommand to use an asynchronous or
stateful execution model instead of SyncActionNode, retaining the
goal-acceptance future from client_->async_send_goal. Return RUNNING while
acceptance is pending, FAILURE when the resolved goal handle is null, and
SUCCESS only after a valid handle is received; do not wait for the action result
or gripper motion.
In `@src/vla_sim_behaviors/src/wait_for_episode_start.cpp`:
- Around line 102-136: Move deadline creation before client_->initialize in the
episode-start flow, then pass the remaining time until that deadline to
waitForServiceServer and each syncSendRequest call instead of fixed five-second
limits. Before sleeping, cap kPollPeriod to the remaining budget, and preserve
the existing timeout error behavior when the deadline is reached.
In `@src/vla_sim_behaviors/test/test_wait_for_episode_start.cpp`:
- Around line 84-91: Make executor shutdown scope-bound in both test fixtures:
in src/vla_sim_behaviors/test/test_wait_for_episode_start.cpp lines 84-91,
extract idempotent stopSpinning() logic from ~WaitForEpisodeStartTest() and call
it in every test before the local trainer is destroyed; in
src/vla_sim_behaviors/test/test_send_gripper_command.cpp lines 143-158, declare
a scope guard after server that invokes stopSpinning(), ensuring cleanup also
runs when ASSERT_EQ exits the test early.
In `@src/vla_sim/launch/simulated_extras.launch.py`:
- Around line 38-43: Update the JointCommandBridge Node configuration in
simulated_extras.launch.py to pass the MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC
environment variable as observation_state_topic, using EnvironmentVariable with
/observed_joint_states as the default value.
In `@src/vla_sim/objectives/record_cube_stack_episode.xml`:
- Around line 18-25: Ensure the episode sequence always invokes the idempotent
StopRecording action when WaitForEpisodeStart times out or the behavior tree is
halted, including when episode start fails and the normal Sequence path is
skipped. Update the RecordEpisode/WaitForEpisodeStart flow to attach cleanup
that runs on both timeout and halt while preserving the existing successful
episode path.
In `@src/vla_sim/script/joint_command_bridge.py`:
- Around line 65-66: Update is_reference_fresh to require the elapsed time (now
- stamp) to be non-negative as well as below timeout, so future timestamps are
rejected after simulated-clock resets; add a test covering a stamp later than
now.
---
Nitpick comments:
In `@src/vla_sim_behaviors/test/test_send_gripper_command.cpp`:
- Around line 45-48: Update the goal callback’s received_ fulfillment so it
records only the first accepted goal; guard set_value with the existing mutex_
and a bool or std::once_flag, preventing subsequent goals from calling
std::promise::set_value again.
In `@src/vla_sim/description/mujoco/keyframes.xml`:
- Around line 22-33: Move each prompt comment in the keyframe generator so it
appears immediately before the corresponding <key> element rather than after it.
Preserve the generated keyframe content and ordering, including the existing
association between each prompt and key such as eval_0 and eval_1; this is a
readability-only change.
🪄 Autofix
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro Plus
Run ID: dddadb72-918e-4873-a90a-62f3be007c7e
📒 Files selected for processing (41)
docker-compose.yamlsrc/vla_sim/CMakeLists.txtsrc/vla_sim/README.mdsrc/vla_sim/config/config.yamlsrc/vla_sim/description/mujoco/cube_stack_scene.xmlsrc/vla_sim/description/mujoco/keyframes.xmlsrc/vla_sim/launch/simulated_extras.launch.pysrc/vla_sim/objectives/collect_cube_stack_demo.xmlsrc/vla_sim/objectives/command_cube_stack_gripper.xmlsrc/vla_sim/objectives/execute_cube_stack_oracle.xmlsrc/vla_sim/objectives/move_along_cube_stack_keyposes.xmlsrc/vla_sim/objectives/prepare_cube_stack_scene.xmlsrc/vla_sim/objectives/record_cube_stack_blue_on_green.xmlsrc/vla_sim/objectives/record_cube_stack_blue_on_red.xmlsrc/vla_sim/objectives/record_cube_stack_episode.xmlsrc/vla_sim/objectives/record_cube_stack_green_on_blue.xmlsrc/vla_sim/objectives/record_cube_stack_green_on_red.xmlsrc/vla_sim/objectives/record_cube_stack_red_on_blue.xmlsrc/vla_sim/objectives/record_cube_stack_red_on_green.xmlsrc/vla_sim/objectives/run_cube_stack_oracle.xmlsrc/vla_sim/package.xmlsrc/vla_sim/script/joint_command_bridge.pysrc/vla_sim/test/test_joint_command_bridge.pysrc/vla_sim_behaviors/CMakeLists.txtsrc/vla_sim_behaviors/include/vla_sim_behaviors/compute_top_down_keyposes.hppsrc/vla_sim_behaviors/include/vla_sim_behaviors/plan_joint_spline_through_poses.hppsrc/vla_sim_behaviors/include/vla_sim_behaviors/send_gripper_command.hppsrc/vla_sim_behaviors/include/vla_sim_behaviors/wait_for_episode_start.hppsrc/vla_sim_behaviors/package.xmlsrc/vla_sim_behaviors/src/compute_top_down_keyposes.cppsrc/vla_sim_behaviors/src/plan_joint_spline_through_poses.cppsrc/vla_sim_behaviors/src/register_behaviors.cppsrc/vla_sim_behaviors/src/send_gripper_command.cppsrc/vla_sim_behaviors/src/wait_for_episode_start.cppsrc/vla_sim_behaviors/test/CMakeLists.txtsrc/vla_sim_behaviors/test/test_behavior_plugins.cppsrc/vla_sim_behaviors/test/test_compute_top_down_keyposes.cppsrc/vla_sim_behaviors/test/test_plan_joint_spline_through_poses.cppsrc/vla_sim_behaviors/test/test_send_gripper_command.cppsrc/vla_sim_behaviors/test/test_wait_for_episode_start.cppsrc/vla_sim_behaviors/vla_sim_behaviors_plugin_description.xml
| // Score each candidate where the arm arrives first, so the cost is the motion actually spent | ||
| // getting there rather than to the end of the segment. | ||
| const double approach_height = heights.front(); | ||
| const std::string ik_tip_link = tip_link; | ||
| const auto cost_of = [&](double yaw) -> std::optional<double> { | ||
| const Eigen::Quaterniond orientation = topDownGraspOrientation(yaw); | ||
| const Eigen::Isometry3d keypose = topDownKeypose(aim_pose, orientation, held_object_offset, approach_height); | ||
| moveit_pro::base::RobotState candidate(seed_state); | ||
| if (!candidate.setFromIK(joint_group, keypose, ik_tip_link)) | ||
| { | ||
| return std::nullopt; | ||
| } | ||
| std::vector<double> solution; | ||
| candidate.copyJointGroupPositions(joint_group, solution); | ||
| return jointDistanceCost(seed_positions, solution); | ||
| }; |
There was a problem hiding this comment.
🎯 Functional Correctness | 🟠 Major | ⚡ Quick win
Validate IK for every generated keypose before selecting a yaw.
cost_of rejects a yaw only when IK fails at heights.front(). A later keypose can still fail IK. The behavior can then select a cheap yaw that PlanJointSplineThroughPoses cannot execute, although another cube-symmetry yaw is valid.
Keep the first-pose distance as the cost. Reject the candidate if IK fails at any remaining height. Add a regression test with a yaw that reaches the approach pose but not a later pose.
Proposed fix
std::vector<double> solution;
candidate.copyJointGroupPositions(joint_group, solution);
+ for (std::size_t index = 1; index < heights.size(); ++index)
+ {
+ const Eigen::Isometry3d later_keypose =
+ topDownKeypose(aim_pose, orientation, held_object_offset, heights[index]);
+ if (!candidate.setFromIK(joint_group, later_keypose, ik_tip_link))
+ {
+ return std::nullopt;
+ }
+ }
return jointDistanceCost(seed_positions, solution);📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| // Score each candidate where the arm arrives first, so the cost is the motion actually spent | |
| // getting there rather than to the end of the segment. | |
| const double approach_height = heights.front(); | |
| const std::string ik_tip_link = tip_link; | |
| const auto cost_of = [&](double yaw) -> std::optional<double> { | |
| const Eigen::Quaterniond orientation = topDownGraspOrientation(yaw); | |
| const Eigen::Isometry3d keypose = topDownKeypose(aim_pose, orientation, held_object_offset, approach_height); | |
| moveit_pro::base::RobotState candidate(seed_state); | |
| if (!candidate.setFromIK(joint_group, keypose, ik_tip_link)) | |
| { | |
| return std::nullopt; | |
| } | |
| std::vector<double> solution; | |
| candidate.copyJointGroupPositions(joint_group, solution); | |
| return jointDistanceCost(seed_positions, solution); | |
| }; | |
| // Score each candidate where the arm arrives first, so the cost is the motion actually spent | |
| // getting there rather than to the end of the segment. | |
| const double approach_height = heights.front(); | |
| const std::string ik_tip_link = tip_link; | |
| const auto cost_of = [&](double yaw) -> std::optional<double> { | |
| const Eigen::Quaterniond orientation = topDownGraspOrientation(yaw); | |
| const Eigen::Isometry3d keypose = topDownKeypose(aim_pose, orientation, held_object_offset, approach_height); | |
| moveit_pro::base::RobotState candidate(seed_state); | |
| if (!candidate.setFromIK(joint_group, keypose, ik_tip_link)) | |
| { | |
| return std::nullopt; | |
| } | |
| std::vector<double> solution; | |
| candidate.copyJointGroupPositions(joint_group, solution); | |
| for (std::size_t index = 1; index < heights.size(); ++index) | |
| { | |
| const Eigen::Isometry3d later_keypose = | |
| topDownKeypose(aim_pose, orientation, held_object_offset, heights[index]); | |
| if (!candidate.setFromIK(joint_group, later_keypose, ik_tip_link)) | |
| { | |
| return std::nullopt; | |
| } | |
| } | |
| return jointDistanceCost(seed_positions, solution); | |
| }; |
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim_behaviors/src/compute_top_down_keyposes.cpp` around lines 225 -
240, Update the cost_of lambda to validate IK for every keypose generated from
heights, while retaining the existing jointDistanceCost based on the first
approach pose. Return std::nullopt if IK fails for any later height, so yaw
selection excludes candidates that PlanJointSplineThroughPoses cannot execute.
Add a regression test covering a yaw that succeeds at heights.front() but fails
at a subsequent height.
| double splineDuration(const JointSpline& spline, double cartesian_length, double cartesian_speed, | ||
| const Eigen::VectorXd& joint_velocity_cap) | ||
| { | ||
| const double cartesian = cartesian_speed > 0.0 ? cartesian_length / cartesian_speed : 0.0; | ||
| const Eigen::VectorXd peak = spline.peakSpeed(); | ||
| double joint = 0.0; | ||
| for (Eigen::Index j = 0; j < peak.size() && j < joint_velocity_cap.size(); ++j) | ||
| { | ||
| if (joint_velocity_cap[j] > 0.0) | ||
| { | ||
| joint = std::max(joint, peak[j] / joint_velocity_cap[j]); | ||
| } | ||
| } | ||
| return std::clamp(std::max(cartesian, joint), kMinimumDuration, kMaximumDuration); |
There was a problem hiding this comment.
🎯 Functional Correctness | 🟠 Major | ⚡ Quick win
Reject trajectories that cannot meet the configured maximum duration.
The duration calculation converts a velocity-limit violation into a 60-second successful trajectory. The generated trajectory can then exceed joint velocity caps.
src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp#L224-L237: preserve the minimum-duration floor, but do not upper-clamp the required duration.src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp#L393-L394: returnFAILUREwhen the required duration exceedskMaximumDuration.src/vla_sim_behaviors/test/test_plan_joint_spline_through_poses.cpp#L176-L180: replace the 60-second expectation with a test for planner failure.
📍 Affects 2 files
src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp#L224-L237(this comment)src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp#L393-L394src/vla_sim_behaviors/test/test_plan_joint_spline_through_poses.cpp#L176-L180
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp` around lines
224 - 237, The duration calculation in splineDuration must retain the
minimum-duration floor without upper-clamping the required duration; update the
planner at src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp lines
224-237 and return FAILURE when that required duration exceeds kMaximumDuration
at lines 393-394. Update
src/vla_sim_behaviors/test/test_plan_joint_spline_through_poses.cpp lines
176-180 to assert planner failure instead of expecting a 60-second trajectory.
| Eigen::VectorXd velocity_cap(knots.front().size()); | ||
| const auto& bounds = joint_group->getActiveJointModelsBounds(); | ||
| for (Eigen::Index j = 0; j < velocity_cap.size(); ++j) | ||
| { | ||
| const auto index = static_cast<std::size_t>(j); | ||
| const bool bounded = index < bounds.size() && !bounds[index]->empty() && bounds[index]->front().velocity_bounded_; | ||
| velocity_cap[j] = | ||
| bounded ? bounds[index]->front().max_velocity_ * joint_velocity_scale : std::numeric_limits<double>::infinity(); |
There was a problem hiding this comment.
🎯 Functional Correctness | 🟠 Major | ⚡ Quick win
Validate joint_velocity_scale before creating velocity caps.
A value of 0 or NaN causes the timing helpers to ignore bounded joints. A value greater than 1.0 permits speeds above the model limit. Require a finite value in (0.0, 1.0] before this calculation.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp` around lines
360 - 367, Validate joint_velocity_scale before constructing velocity_cap,
requiring it to be finite and within the inclusive range (0.0, 1.0]. Reject
invalid values before the bounded-joint velocity-cap calculation so zero, NaN,
and values above the model limit cannot reach the timing helpers.
| const double duration = splineDuration(*spline, cartesian_length, cartesian_speed, velocity_cap); | ||
| const auto steps = static_cast<std::size_t>(std::ceil(duration * sampling_rate)); | ||
|
|
||
| trajectory_msgs::msg::JointTrajectory trajectory; | ||
| trajectory.header = path.front().header; | ||
| trajectory.joint_names = joint_group->getVariableNames(); | ||
| trajectory.points.reserve(steps + 1); | ||
| for (std::size_t step = 0; step <= steps; ++step) |
There was a problem hiding this comment.
🩺 Stability & Availability | 🟠 Major | ⚡ Quick win
Bound the number of sampled trajectory points.
Any nonzero sampling_rate is accepted. A large configured value makes steps large enough for trajectory.points.reserve() to exhaust memory or throw. Define a maximum point count or sampling rate, validate the product before conversion, and return FAILURE when it exceeds the limit.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim_behaviors/src/plan_joint_spline_through_poses.cpp` around lines
393 - 400, Validate the duration-to-sample calculation in the trajectory
generation flow before converting it to std::size_t or reserving points. Enforce
a defined maximum point count (including the final sample) and return FAILURE
when the configured sampling_rate or resulting count exceeds that limit;
otherwise preserve the existing loop and trajectory construction behavior.
| // The goal handle is dropped, since waiting on it is the blocking this Behavior exists to | ||
| // avoid. A rejected goal surfaces as a gripper that did not move. | ||
| client_->async_send_goal(goal); | ||
|
|
||
| return BT::NodeStatus::SUCCESS; |
There was a problem hiding this comment.
🎯 Functional Correctness | 🟠 Major | 🏗️ Heavy lift
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
set -e
printf '%s\n' '--- target file ---'
cat -n src/vla_sim_behaviors/src/send_gripper_command.cpp | sed -n '1,180p'
printf '%s\n' '--- related declarations and implementations ---'
rg -n -C 4 'SendGripperCommand|async_send_goal|class .*Behavior|NodeStatus::RUNNING|goal_response_callback' \
src/vla_sim_behaviors
printf '%s\n' '--- action-client API references ---'
rg -n -C 3 'async_send_goal|ClientGoalHandle|SendGoalOptions' . \
-g '*.cpp' -g '*.hpp' -g '*.h' -g '*.xml' || trueRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 25954
🏁 Script executed:
#!/bin/bash
set -e
printf '%s\n' '--- SendGripperCommand declaration ---'
cat -n src/vla_sim_behaviors/include/vla_sim_behaviors/send_gripper_command.hpp
printf '%s\n' '--- SendGripperCommand tests ---'
cat -n src/vla_sim_behaviors/test/test_send_gripper_command.cpp | sed -n '1,230p'
printf '%s\n' '--- behavior-base usage and dependency versions ---'
rg -n -C 4 'SharedResourcesNode|AsyncBehaviorBase|behaviortree|BehaviorTree|rclcpp_action' \
src/vla_sim_behaviors package.xml CMakeLists.txt \
-g '*.hpp' -g '*.h' -g '*.cpp' -g '*.xml' -g 'CMakeLists.txt' || true
printf '%s\n' '--- repository history metadata for the target files ---'
git status --short
git log -5 --oneline -- src/vla_sim_behaviors/src/send_gripper_command.cpp \
src/vla_sim_behaviors/include/vla_sim_behaviors/send_gripper_command.hpp \
src/vla_sim_behaviors/test/test_send_gripper_command.cppRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 34539
🌐 Web query:
rclcpp_action Client async_send_goal return future goal response rejected null goal handle SyncActionNode BehaviorTree.CPP SyncActionNode RUNNING
💡 Result:
In the context of ROS 2 and BehaviorTree.CPP, the behavior you are observing involves two distinct mechanisms: the rclcpp_action client's handling of goal responses and the strict execution model of BehaviorTree.CPP's SyncActionNode [1][2][3]. 1. rclcpp_action Client Behavior: When you call async_send_goal, it returns a std::shared_future [1][4]. If the action server rejects the goal, the future resolves to a nullptr rather than a valid GoalHandle [1][5][6]. This is the expected behavior defined by the ROS 2 rclcpp_action API [1][7]. 2. BehaviorTree.CPP SyncActionNode: A SyncActionNode is explicitly designed to perform operations synchronously [2][3]. According to the library's design, it must never return the status RUNNING [2][8]. If a node implementation attempts to return RUNNING from its executeTick method, the SyncActionNode base class will throw a LogicError [8][3]. 3. Integration Conflict: The situation typically arises when an asynchronous ROS 2 action (which inherently involves waiting for a response) is incorrectly wrapped in a synchronous BehaviorTree node, or when the asynchronous logic fails to handle the nullptr result from a rejected goal appropriately before the tree tries to re-tick the node [9][10]. If your implementation is attempting to wait for the future within the tick (e.g., using spin_until_future_complete) and failing to resolve it, or if it is mismanaging the node's state transition, the tree execution may stall or error out [9]. To resolve this, ensure that: - You are not using SyncActionNode for operations that require waiting for an asynchronous ROS 2 action response; consider using an asynchronous action node implementation (such as RosActionNode or BT::AsyncActionNode) instead [3][11]. - Your code explicitly checks if the goal handle is null immediately upon receiving the goal response, and transitions the tree node to a FAILURE status if the goal was rejected [9][11]. - You avoid blocking calls like spin_until_future_complete inside the tick function, as these can interfere with the executor and lead to timeouts or deadlocks [9][10].
Citations:
- 1: https://docs.ros2.org/latest/api/rclcpp_action/classrclcpp__action_1_1Client.html
- 2: https://behaviortree.github.io/BehaviorTree.CPP/da/d73/class_b_t_1_1_sync_action_node.html
- 3: https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/include/behaviortree_cpp/action_node.h
- 4: https://github.com/ros2/rclcpp/blob/rolling/rclcpp_action/include/rclcpp_action/client.hpp
- 5: https://docs.ros2.org/latest/api/rclcpp_action/structrclcpp__action_1_1Client_1_1SendGoalOptions.html
- 6: https://docs.ros.org/en/rolling/p/rclcpp_action/generated/classrclcpp__action_1_1Client.html
- 7: https://docs.ros.org/en/jazzy/p/rclcpp_action/generated/classrclcpp__action_1_1Client.html
- 8: https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/src/action_node.cpp
- 9: BT action client of type nav2_msgs::action::NavigateToPose sends ActionNodeErrorCode SEND_GOAL_TIMEOUT BehaviorTree/BehaviorTree.ROS2#76
- 10: https://docs.ros.org/en/kilted/p/nav2_behavior_tree/generated/program_listing_file_include_nav2_behavior_tree_bt_action_node.hpp.html
- 11: https://github.com/AutoAPMS/auto-apms/blob/0542dc16/auto_apms_behavior_tree_core/include/auto_apms_behavior_tree_core/node/ros_action_node.hpp
🏁 Script executed:
#!/bin/bash
set -e
python3 - <<'PY'
from urllib.request import urlopen
urls = {
"rclcpp_action_client": "https://raw.githubusercontent.com/ros2/rclcpp/rolling/rclcpp_action/include/rclcpp_action/client.hpp",
"btcpp_action_node": "https://raw.githubusercontent.com/BehaviorTree/BehaviorTree.CPP/master/include/behaviortree_cpp/action_node.h",
"btcpp_action_impl": "https://raw.githubusercontent.com/BehaviorTree/BehaviorTree.CPP/master/src/action_node.cpp",
}
for name, url in urls.items():
print(f"--- {name} ---")
text = urlopen(url, timeout=10).read().decode()
lines = text.splitlines()
needles = (
"async_send_goal",
"SyncActionNode",
"MUST never return RUNNING",
"goal_handle",
"nullptr",
)
for i, line in enumerate(lines):
if any(needle in line for needle in needles):
start = max(0, i - 4)
end = min(len(lines), i + 8)
print(f"[lines {start + 1}-{end}]")
print("\n".join(f"{j + 1}: {lines[j]}" for j in range(start, end)))
PYRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 2491
🏁 Script executed:
#!/bin/bash
set -e
fetch_and_extract() {
name="$1"
url="$2"
tmp="$(mktemp)"
curl -fsSLk "$url" -o "$tmp"
echo "--- $name ---"
rg -n -C 5 'async_send_goal|SyncActionNode|MUST never return RUNNING|goal_handle|nullptr' "$tmp" || true
rm -f "$tmp"
}
fetch_and_extract \
rclcpp_action_client \
https://raw.githubusercontent.com/ros2/rclcpp/rolling/rclcpp_action/include/rclcpp_action/client.hpp
fetch_and_extract \
btcpp_action_node \
https://raw.githubusercontent.com/BehaviorTree/BehaviorTree.CPP/master/include/behaviortree_cpp/action_node.h
fetch_and_extract \
btcpp_action_impl \
https://raw.githubusercontent.com/BehaviorTree/BehaviorTree.CPP/master/src/action_node.cppRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 21409
Gate SUCCESS on goal acceptance.
async_send_goal returns a future that resolves to a goal handle on acceptance and nullptr on rejection. The current code discards the future and returns SUCCESS immediately, so downstream motion can continue after a rejected goal. Because SendGripperCommand derives from BT::SyncActionNode, convert it to an asynchronous or stateful node before returning RUNNING while acceptance is pending. Return FAILURE for a null goal handle and SUCCESS only after acceptance. Do not wait for the action result or gripper motion.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim_behaviors/src/send_gripper_command.cpp` around lines 100 - 104,
Update SendGripperCommand to use an asynchronous or stateful execution model
instead of SyncActionNode, retaining the goal-acceptance future from
client_->async_send_goal. Return RUNNING while acceptance is pending, FAILURE
when the resolved goal handle is null, and SUCCESS only after a valid handle is
received; do not wait for the action result or gripper motion.
| halted_ = false; | ||
| client_->initialize(service_name, kServerTimeout, kResponseTimeout); | ||
| if (!client_->waitForServiceServer()) | ||
| { | ||
| return tl::make_unexpected(fmt::format("No Trainer active_recording service on '{}'.", service_name)); | ||
| } | ||
| // Nothing here holds a goal, so a halt may interrupt the poll as soon as it arrives. | ||
| notifyCanHalt(); | ||
|
|
||
| const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout); | ||
| std::string last_state; | ||
| while (!halted_) | ||
| { | ||
| const auto response = client_->syncSendRequest(GetActiveRecordingSrv::Request{}); | ||
| if (!response.has_value()) | ||
| { | ||
| return tl::make_unexpected("Failed to read the active recording: " + response.error()); | ||
| } | ||
| if (!response.value().status.success) | ||
| { | ||
| return tl::make_unexpected("The Trainer refused to report the active recording: " + | ||
| response.value().status.error_message); | ||
| } | ||
| last_state = stateOf(response.value().session_json); | ||
| if (last_state == kRecordingState) | ||
| { | ||
| return true; | ||
| } | ||
| if (std::chrono::steady_clock::now() >= deadline) | ||
| { | ||
| return tl::make_unexpected( | ||
| fmt::format("No recording episode opened within {:g}s; the session's last state was '{}'.", timeout, | ||
| last_state.empty() ? "no active session" : last_state)); | ||
| } | ||
| std::this_thread::sleep_for(kPollPeriod); |
There was a problem hiding this comment.
🩺 Stability & Availability | 🟠 Major | 🏗️ Heavy lift
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
set -eu
file="src/vla_sim_behaviors/src/wait_for_episode_start.cpp"
printf '%s\n' '--- target file outline ---'
ast-grep outline "$file" || true
printf '%s\n' '--- target implementation ---'
cat -n "$file" | sed -n '70,155p'
printf '%s\n' '--- timeout declarations and client API uses ---'
rg -n -C 3 'kServerTimeout|kResponseTimeout|waitForServiceServer|syncSendRequest|initialize\(' \
src/vla_sim_behaviors src | head -n 300Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 17201
🏁 Script executed:
#!/bin/bash
set -eu
file="src/vla_sim_behaviors/src/wait_for_episode_start.cpp"
printf '%s\n' '--- target file outline ---'
ast-grep outline "$file" || true
printf '%s\n' '--- target implementation ---'
cat -n "$file" | sed -n '70,155p'
printf '%s\n' '--- timeout declarations and client API uses ---'
rg -n -C 3 'kServerTimeout|kResponseTimeout|waitForServiceServer|syncSendRequest|initialize\(' \
src_vla_sim_behaviors src 2>/dev/null | head -n 300Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 14942
🏁 Script executed:
#!/bin/bash
set -eu
printf '%s\n' '--- client interface definitions ---'
rg -n -C 8 'class RclcppClientInterface|struct RclcppClientInterface|waitForServiceServer|syncSendRequest|cancelRequest' \
--glob '*.{hpp,hh,h,cpp,cc,cxx}' . | head -n 500
printf '%s\n' '--- wait behavior implementations ---'
rg -n -C 12 'waitForServiceServer\s*\(' --glob '*.{hpp,hh,h,cpp,cc,cxx}' . | head -n 500
printf '%s\n' '--- timeout input validation and related behaviors ---'
rg -n -C 8 'getRequiredInputs\(.*timeout|kPortIDTimeout|duration<double>|waitFor.*timeout|syncSendRequest' \
src/vla_sim_behaviors --glob '*.{hpp,hh,h,cpp,cc,cxx}' | head -n 500Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 15456
🌐 Web query:
PickNikRobotics moveit_pro_example_ws RclcppClientInterface syncSendRequest waitForServiceServer
💡 Result:
The RclcppClientInterface class, part of the MoveIt Pro software suite, provides a wrapper around ROS 2 service clients to facilitate communication within Behavior Trees [1][2]. It is frequently utilized in custom behaviors developed within the moveit_pro_example_ws workspace [3][4][5]. Key components of the RclcppClientInterface include: waitForServiceServer: This method is an implementation of the ClientInterfaceBase::waitForServiceServer pure virtual function [1][2]. It blocks the execution thread until the specified service server becomes available or a defined timeout is reached, returning true if the server is found and false otherwise [2]. syncSendRequest: This method implements the ClientInterfaceBase::syncSendRequest pure virtual function [1][2]. It sends a ROS 2 service request and blocks the calling thread until a response is received or the result timeout is exceeded [2]. It returns a tl::expected object containing the service response or an error string [1]. Important technical considerations include: Integration: These interfaces are typically used within classes derived from ServiceClientBehaviorBase, which handles much of the boilerplate for creating and managing service requests, such as defining service names and processing responses [6][5]. Threading Warning: The base class, ClientInterfaceBase, explicitly notes that it does not currently support calling syncSendRequest asynchronously from multiple threads [7][2]. Development Workspace: The moveit_pro_example_ws is the standard location for managing, building, and testing custom behaviors that incorporate these interfaces [3][4][5]. To ensure custom behaviors are compiled, users must ensure the package is not excluded (e.g., by a COLCON_IGNORE file) [5].
Citations:
- 1: https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_rclcpp_client_interface.html
- 2: https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_client_interface_base.html
- 3: https://github.com/PickNikRobotics/moveit_pro_example_ws
- 4: https://docs.picknik.ai/6/getting_started/motion_planning_bt_tutorials/create_service_client_behavior/
- 5: https://docs.picknik.ai/next/how_to/custom_behaviors/additional_behavior_classes/
- 6: https://docs.picknik.ai/concepts/creating_behaviors/
- 7: https://docs.picknik.ai/doxygen/service__client__interface_8hpp.html
🏁 Script executed:
#!/bin/bash
set -eu
python3 - <<'PY'
from urllib.request import urlopen
from html import unescape
import re
urls = [
"https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_rclcpp_client_interface.html",
"https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_client_interface_base.html",
]
for url in urls:
print(f"--- {url} ---")
try:
text = urlopen(url, timeout=15).read().decode("utf-8", "replace")
except Exception as exc:
print(f"fetch failed: {exc}")
continue
text = unescape(re.sub(r"<[^>]+>", " ", text))
text = re.sub(r"\s+", " ", text)
for term in ("initialize", "waitForServiceServer", "syncSendRequest", "cancelRequest"):
pos = text.find(term)
print(f"{term}: {text[pos:pos+700] if pos >= 0 else 'not found'}")
PY
printf '%s\n' '--- dependency references in the repository ---'
rg -n -C 5 'client_interface|RclcppClientInterface|moveit_pro_behaviors|moveit_pro_behavior' \
src/vla_sim_behaviors CMakeLists.txt src --glob '*.{cmake,xml,h,hpp,cpp,cc,cxx}' | head -n 400Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 35120
🏁 Script executed:
#!/bin/bash
set -eu
for url in \
'https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_rclcpp_client_interface.html' \
'https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_client_interface_base.html' \
'https://docs.picknik.ai/doxygen/service__client__interface_8hpp.html'
do
printf '%s\n' "--- $url ---"
curl -k -L --fail --silent "$url" |
sed 's/<[^>]*>/ /g' |
tr -s '[:space:]' ' ' |
grep -o -E '.{0,180}(initialize|waitForServiceServer|syncSendRequest|cancelRequest).{0,500}' |
head -n 20 || true
done
printf '%s\n' '--- wait behavior tests ---'
cat -n src/vla_sim_behaviors/test/test_wait_for_episode_start.cpp | sed -n '1,260p'Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 18398
🏁 Script executed:
#!/bin/bash
set -eu
python3 - <<'PY'
from pathlib import Path
import re
path = Path("src/vla_sim_behaviors/src/wait_for_episode_start.cpp")
text = path.read_text()
server = float(re.search(r"kServerTimeout\{ ([0-9.]+) \}", text).group(1))
response = float(re.search(r"kResponseTimeout\{ ([0-9.]+) \}", text).group(1))
poll_ms = int(re.search(r"kPollPeriod\{ ([0-9]+) \}", text).group(1))
initialize = text.index("client_->initialize")
server_wait = text.index("client_->waitForServiceServer")
deadline = text.index("const auto deadline")
request = text.index("client_->syncSendRequest")
deadline_check = text.index("std::chrono::steady_clock::now() >= deadline")
assert initialize < server_wait < deadline < request < deadline_check
assert server == 5.0
assert response == 5.0
for timeout in (0.0, 0.01, 0.5, 1.0):
absent_service_elapsed = server
hung_response_elapsed = server + response
successful_response_then_poll_elapsed = response + poll_ms / 1000
print(
f"timeout={timeout:g}s: "
f"absent_service>={absent_service_elapsed:g}s, "
f"hung_response>={hung_response_elapsed:g}s, "
f"response+poll>={successful_response_then_poll_elapsed:g}s"
)
PYRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 472
Bound every blocking wait by timeout.
waitForServiceServer() and syncSendRequest() use the five-second limits configured by initialize(). The deadline is created only after the service wait. A short timeout can therefore be exceeded by several seconds. Create the deadline before initialize(), use the remaining budget for each service and response wait, and cap sleep_for(kPollPeriod) by the remaining budget.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim_behaviors/src/wait_for_episode_start.cpp` around lines 102 - 136,
Move deadline creation before client_->initialize in the episode-start flow,
then pass the remaining time until that deadline to waitForServiceServer and
each syncSendRequest call instead of fixed five-second limits. Before sleeping,
cap kPollPeriod to the remaining budget, and preserve the existing timeout error
behavior when the deadline is reached.
| ~WaitForEpisodeStartTest() override | ||
| { | ||
| executor_.cancel(); | ||
| if (spin_thread_.joinable()) | ||
| { | ||
| spin_thread_.join(); | ||
| } | ||
| } |
There was a problem hiding this comment.
🩺 Stability & Availability | 🟡 Minor | ⚡ Quick win
Executor shutdown is not bound to scope in either test fixture. Both fixtures spin an executor on node_, and both let tests own the server object as a local. If the server is destroyed while the executor still dispatches callbacks, the callback touches destroyed members. test_send_gripper_command.cpp Lines 105-120 already document that this aborts the whole binary.
src/vla_sim_behaviors/test/test_wait_for_episode_start.cpp#L84-L91: extract an idempotentstopSpinning()from the destructor, then call it in each test beforetrainerleaves scope.src/vla_sim_behaviors/test/test_send_gripper_command.cpp#L143-L158: bindstopSpinning()to scope with a guard declared afterserver, so theASSERT_EQat Line 154 cannot skip it.
📍 Affects 2 files
src/vla_sim_behaviors/test/test_wait_for_episode_start.cpp#L84-L91(this comment)src/vla_sim_behaviors/test/test_send_gripper_command.cpp#L143-L158
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim_behaviors/test/test_wait_for_episode_start.cpp` around lines 84 -
91, Make executor shutdown scope-bound in both test fixtures: in
src/vla_sim_behaviors/test/test_wait_for_episode_start.cpp lines 84-91, extract
idempotent stopSpinning() logic from ~WaitForEpisodeStartTest() and call it in
every test before the local trainer is destroyed; in
src/vla_sim_behaviors/test/test_send_gripper_command.cpp lines 143-158, declare
a scope guard after server that invokes stopSpinning(), ensuring cleanup also
runs when ASSERT_EQ exits the test early.
| Node( | ||
| package="vla_sim", | ||
| executable="joint_command_bridge.py", | ||
| name="joint_command_bridge", | ||
| output="log", | ||
| ), |
There was a problem hiding this comment.
🗄️ Data Integrity & Integration | 🟠 Major | ⚡ Quick win
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
set -eu
printf '%s\n' '--- candidate files ---'
git ls-files | rg '(^|/)(simulated_extras\.launch\.py|joint_command_bridge\.py|docker-compose\.yaml)$'
printf '%s\n' '--- topic references ---'
rg -n -C 5 'MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC|observation_state_topic|observed_joint_states|joint_command_bridge' .
printf '%s\n' '--- launch file ---'
sed -n '1,120p' src/vla_sim/launch/simulated_extras.launch.py
printf '%s\n' '--- bridge candidates ---'
for f in $(git ls-files | rg '(^|/)joint_command_bridge\.py$'); do
echo "### $f"
sed -n '1,240p' "$f"
done
printf '%s\n' '--- compose references ---'
for f in $(git ls-files | rg '(^|/)docker-compose\.yaml$'); do
echo "### $f"
rg -n -C 8 'MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC' "$f"
doneRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 25126
🏁 Script executed:
#!/bin/bash
set -eu
printf '%s\n' '--- compose service wiring ---'
sed -n '1,180p' docker-compose.yaml
printf '%s\n' '--- all launch and Trainer topic references ---'
rg -n -C 6 'MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC|simulated_extras|ros2 launch|launch.*simulated|Trainer|trainer|joint_states' \
--glob '!*.xml' --glob '!*.pyc' .
printf '%s\n' '--- environment propagation candidates ---'
rg -n -C 5 'environment:|env_file:|docker compose|docker-compose|runtime:|vla_sim' \
--glob 'Dockerfile*' --glob '*.yaml' --glob '*.yml' --glob '*.sh' --glob '*.md' --glob '*.py' .
printf '%s\n' '--- launch imports and package metadata ---'
rg -n -C 4 'from launch\.substitutions|EnvironmentVariable|launch_ros|package.xml|exec_depend' \
src/vla_sim package.xml .
printf '%s\n' '--- installed launch API availability (read-only) ---'
python3 - <<'PY'
try:
from launch.substitutions import EnvironmentVariable
except Exception as exc:
print(f"EnvironmentVariable import failed: {type(exc).__name__}: {exc}")
else:
import inspect
print(f"EnvironmentVariable: {EnvironmentVariable}")
print(f"signature: {inspect.signature(EnvironmentVariable)}")
print(f"doc: {inspect.getdoc(EnvironmentVariable)}")
PYRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 50396
🏁 Script executed:
#!/bin/bash
set -eu
python3 - <<'PY'
from pathlib import Path
import re
files = [
Path("docker-compose.yaml"),
Path("src/vla_sim/config/config.yaml"),
Path("src/vla_sim/launch/simulated_extras.launch.py"),
Path("src/vla_sim/script/joint_command_bridge.py"),
]
for path in files:
print(f"--- {path} ---")
text = path.read_text()
for i, line in enumerate(text.splitlines(), 1):
if any(token in line for token in (
"MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC",
"additional_agent_launch_file",
"simulated_extras.launch.py",
"observation_state_topic",
"observed_joint_states",
"environment:",
"runtime:",
)):
print(f"{i}: {line}")
print("--- launch substitution API ---")
try:
from launch.substitutions import EnvironmentVariable
import inspect
print("import: ok")
print("signature:", inspect.signature(EnvironmentVariable))
except Exception as exc:
print("import:", type(exc).__name__, str(exc))
PY
printf '%s\n' '--- package and documentation ownership ---'
git ls-files | rg '(^|/)(package\.xml|README[^/]*|.*\.md)$' | rg 'vla_sim|moveit_pro' | head -80
printf '%s\n' '--- narrow topic configuration references ---'
rg -n 'MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC|additional_agent_launch_file|simulated_extras\.launch\.py|observation_state_topic|observed_joint_states' \
docker-compose.yaml src/vla_sim/config src/vla_sim/launch src/vla_sim/script \
--glob '*.yaml' --glob '*.yml' --glob '*.py' --glob '*.md'Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 3789
🌐 Web query:
ROS 2 launch EnvironmentVariable substitution default_value constructor official documentation
💡 Result:
In ROS 2, the EnvironmentVariable substitution is used to retrieve the value of an environment variable within a launch file. The class constructor supports an optional default_value parameter to handle cases where the environment variable is not defined [1][2][3]. The constructor signature is: EnvironmentVariable(name, *, default_value=None) Parameters: - name: The name of the environment variable to look up. This can be a string or a list of substitutions [2][3]. - default_value (optional): The value to use if the specified environment variable does not exist. If this is not provided (None), the substitution is considered mandatory; if the environment variable is missing at runtime, a launch.substitutions.substitution_failure.SubstitutionFailure exception will be raised [1][4][5]. If default_value is provided, it is used as a fallback if the environment variable is not found in the launch context's environment [1][2][3]. Note that the environment variable lookup is performed against the launch context's environment (context.environment), which may differ from the environment of the process running the launch file [4][6].
Citations:
- 1: https://docs.ros.org/en/rolling/p/launch/launch.substitutions.environment_variable.html
- 2: https://docs.ros.org/en/jazzy/p/launch/_modules/launch/substitutions/environment_variable.html
- 3: https://docs.ros.org/en/rolling/p/launch/_modules/launch/substitutions/environment_variable.html
- 4: https://docs.ros.org/en/rolling/p/launch/launch.substitutions.html
- 5: https://docs.ros.org/en/ros2_packages/rolling/api/launch/launch.substitutions.environment_variable.html
- 6: https://docs.ros.org/en/kilted/p/launch/launch.substitutions.environment_variable.html
Forward MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC to JointCommandBridge.
If the variable is overridden, the Trainer records that topic while the bridge publishes /observed_joint_states. Set observation_state_topic with EnvironmentVariable(..., default_value="/observed_joint_states").
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim/launch/simulated_extras.launch.py` around lines 38 - 43, Update
the JointCommandBridge Node configuration in simulated_extras.launch.py to pass
the MOVEIT_PRO_TRAIN_JOINT_STATES_TOPIC environment variable as
observation_state_topic, using EnvironmentVariable with /observed_joint_states
as the default value.
| <Action | ||
| ID="RecordEpisode" | ||
| dataset_name="{dataset_name}" | ||
| task="{task}" | ||
| num_episodes="1" | ||
| recording_id="{recording_id}" | ||
| /> | ||
| <Action ID="WaitForEpisodeStart" timeout="30.0" /> |
There was a problem hiding this comment.
🩺 Stability & Availability | 🟠 Major | ⚡ Quick win
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
set -euo pipefail
ast-grep outline src/vla_sim_behaviors/src/wait_for_episode_start.cpp --items all
rg -n -C 6 'WaitForEpisodeStart|RecordEpisode|StopRecording|onHalted|halt|timeout' srcRepository: PickNikRobotics/moveit_pro_example_ws
Length of output: 50394
🏁 Script executed:
#!/bin/bash
set -euo pipefail
printf '%s\n' '--- target files ---'
fd -i 'record_cube_stack_episode|wait_for_episode_start|recording|episode' src
printf '%s\n' '--- focused identifiers ---'
rg -n -C 8 --glob '!**/assets/**' --glob '!*.mtl' --glob '!*.dae' \
'WaitForEpisodeStart|RecordEpisode|StopRecording|onHalted|halt' \
src/vla_sim src | head -n 1200Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 49042
🏁 Script executed:
#!/bin/bash
set -euo pipefail
printf '%s\n' '--- recording-related references ---'
rg -n -C 5 --glob '!**/assets/**' --glob '!*.mtl' --glob '!*.dae' \
'active_recording|stop_recording|record_episode|RecordEpisode|StopRecording|recording_id|num_episodes' \
src CMakeLists.txt package.xml README.md 2>/dev/null | head -n 1600
printf '%s\n' '--- target XML ---'
cat -n src/vla_sim/objectives/record_cube_stack_episode.xml
printf '%s\n' '--- WaitForEpisodeStart implementation ---'
cat -n src/vla_sim_behaviors/src/wait_for_episode_start.cpp | sed -n '90,155p'
printf '%s\n' '--- relevant tests ---'
cat -n src/vla_sim_behaviors/test/test_wait_for_episode_start.cpp | sed -n '125,210p'Repository: PickNikRobotics/moveit_pro_example_ws
Length of output: 14349
🌐 Web query:
MoveIt Pro RecordEpisode StopRecording Trainer active_recording behavior
💡 Result:
In MoveIt Pro, the RecordEpisode, SaveEpisode, and StopRecording behaviors work together to manage demonstration collection sessions via the trainer server. RecordEpisode The RecordEpisode behavior initiates a trainer recording session [1]. It calls the /trainer/start_recording service, which is the same session controlled by the training panel in the user interface [1]. This allows recordings started by a behavior to be monitored live in the UI [1]. Configuration details such as camera topics, joint topics, and frame rates are determined by a named training config or provided defaults [1]. SaveEpisode The SaveEpisode behavior ends the current episode of an active recording session and snapshots it to the dataset [2]. It performs the same action as the Save button in the training panel [2]. It is necessary to call SaveEpisode after completing a demonstrated task but before calling StopRecording; otherwise, the episode is discarded [2]. SaveEpisode triggers an ordered snapshot that flushes in the background [2]. StopRecording The StopRecording behavior terminates the active trainer recording session and registers all previously saved episodes as a dataset [3][4]. It calls the /trainer/stop_recording service [3]. StopRecording waits for any pending background flushes from SaveEpisode before ending the session [2]. This behavior is idempotent; it succeeds even if no recording is active, making it safe to include in cleanup branches of an Objective [3]. Any episode currently in progress when StopRecording is called is not snapshotted [3].
Citations:
- 1: https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_record_episode.html
- 2: https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_save_episode.html
- 3: https://docs.picknik.ai/doxygen/classmoveit__pro_1_1behaviors_1_1_stop_recording.html
- 4: https://docs.picknik.ai/doxygen/stop__recording_8hpp.html
Guarantee StopRecording after episode-start failure or halt.
WaitForEpisodeStart only cancels its status request when it times out or is halted. The Sequence then skips StopRecording, while RecordEpisode has already started the Trainer session. Add cleanup that always calls the idempotent StopRecording action on timeout and tree halt.
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim/objectives/record_cube_stack_episode.xml` around lines 18 - 25,
Ensure the episode sequence always invokes the idempotent StopRecording action
when WaitForEpisodeStart times out or the behavior tree is halted, including
when episode start fails and the normal Sequence path is skipped. Update the
RecordEpisode/WaitForEpisodeStart flow to attach cleanup that runs on both
timeout and halt while preserving the existing successful episode path.
| def is_reference_fresh(now: float, stamp: float, timeout: float) -> bool: | ||
| return (now - stamp) < timeout |
There was a problem hiding this comment.
🗄️ Data Integrity & Integration | 🟠 Major | ⚡ Quick win
Reject timestamps that are later than now.
If simulated time moves backward during an episode reset, (now - stamp) < timeout is true for cached prior-episode data. The bridge can publish stale actions and observations into the next recording. Require a non-negative elapsed duration, and add a backward-clock test.
Proposed fix
def is_reference_fresh(now: float, stamp: float, timeout: float) -> bool:
- return (now - stamp) < timeout
+ elapsed = now - stamp
+ return 0.0 <= elapsed < timeout📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
| def is_reference_fresh(now: float, stamp: float, timeout: float) -> bool: | |
| return (now - stamp) < timeout | |
| def is_reference_fresh(now: float, stamp: float, timeout: float) -> bool: | |
| elapsed = now - stamp | |
| return 0.0 <= elapsed < timeout |
🤖 Prompt for AI Agents
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.
In `@src/vla_sim/script/joint_command_bridge.py` around lines 65 - 66, Update
is_reference_fresh to require the elapsed time (now - stamp) to be non-negative
as well as below timeout, so future timestamps are rejected after
simulated-clock resets; add a test covering a stamp later than now.
1 participant
[written (mostly) by AI]
Motivation
#815 ships
vla_simwith a checkpoint you can run, but no way to record the demonstrations behind one. This adds a scripted oracle that stacks the cubes and records itself, plus the randomized layouts it sweeps.Training stays out of scope. Epic PickNikRobotics/moveit_pro#20583 assigns it to the
train-modelClaude skill (PickNikRobotics/moveit_pro#20586), not to Pro, so the recipe behind the shipped checkpoint is parked onreference/vla-sim-train-recipefor that skill to build against.Part of PickNikRobotics/moveit_pro#20907, and the worked example that PickNikRobotics/moveit_pro#21138 documents. Targets 10.0.0.
How it works
Layouts.
keyframes.xmlcarries 360 train and 150 eval cube layouts, reachable by name through/mujoco_system/reset_keyframe. Every Objective resets to one before it runs, so the scene varies without anyone touching the simulator. The shipped checkpoint saw only the train layouts, which is what makes the eval set fair to score on.The oracle. Three ways in:
Run Cube-Stack Oracleperforms one stack with no recording. Quickest way to see whether a change to the scene or the planner still produces a clean demonstration.Collect Cube-Stack Demonstrationrecords one episode.Record Cube-Stack <held> On <target>Objectives each sweep the 60 training layouts drawn for their prompt, producing one dataset per prompt.All three are built from four new Behaviors in
vla_sim_behaviors:ComputeTopDownKeyposesderives the approach, grasp, lift, and place poses from where the cubes actually are, picking whichever of the cube's four equivalent yaws costs the arm least.PlanJointSplineThroughPosesfits one joint-space spline through them, so the recorded motion flows through the waypoints instead of stopping at each.SendGripperCommandsends a goal and succeeds as soon as the server accepts it, matching howExecutePolicydrives the gripper at deploy time.WaitForEpisodeStartholds the arm until the Trainer's recording marker lands, so the reset motion stays out of the episode.What gets recorded.
joint_command_bridge.pyfixes both halves of the recorded pair. Both defaults fail silently: the recording succeeds either way, and the damage surfaces only in the trained policy.action: the Trainer labels it from/joint_commandsand falls back to next-state labels when that topic is silent. Onlyquest_oculus_teleoppublishes it, so an Objective-driven recording would take the fallback without saying so. The bridge republishes the controller's reference trajectory there, so the datasets carry real commanded actions.observation.state: the default/joint_statescarries all 15 joints, 8 of them passive Robotiq linkage, so a dataset recorded from it trains against a state vector the deployed policy never sees. The bridge also publishes the 8 policy joints on/observed_joint_states, anddocker-compose.yamlpointsMOVEIT_PRO_TRAIN_JOINT_STATES_TOPICat it.config.yamlhosts the bridge throughadditional_agent_launch_file, so it comes up on both the dev and runtime paths.Manual verification
Collect Cube-Stack Demonstrationproduces an MCAP with aligned command, state, and three camera streams, and conversion labelsactionfrom commands rather than falling back to next states.pi05_kinova_gen3_cube_stack_simcheckpoint was trained on datasets recorded this way.colcon buildandcolcon testforvla_simandvla_sim_behaviorsgreen: 127 tests, 0 failures. TheSendGripperCommandtest used to abort the whole binary about one run in three, because the stalling action server it stands up was destroyed while the executor could still dispatch its callbacks. It now stops the executor first.pre-commit run --from-ref origin/main --to-ref HEADclean.The branch sits directly on current
main, with the two commits below.