feat(visualization): add TrajectoryToPath behavior#12
Merged
Conversation
Converts a trajectory_msgs::msg::JointTrajectory into a vector of PoseStamped via forward kinematics on a chosen tip link, so the existing VisualizePath behavior can render a joint-space trajectory directly. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
- Hoist RobotState out of the FK loop to avoid per-iteration deep copies. - Document why PlanningScene::getCurrentState() is used rather than URDF defaults. - Polish the behavior description and clarify the single-stamp policy in doxygen. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Mechanical line-wrap cleanup so the file matches the project's clang-format config (CI was rejecting the prior formatting). Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
There was a problem hiding this comment.
Pull request overview
Adds a new TrajectoryToPath BehaviorTree node to convert joint-space trajectories into tip-link Cartesian pose paths (via forward kinematics) so they can be visualized through MoveIt Pro’s existing VisualizePath behavior.
Changes:
- Introduces
TrajectoryToPathbehavior implementation and public header. - Registers
TrajectoryToPathin the plugin loader and adds a plugin-load test instantiation. - Updates build/package dependencies for the new behavior (MoveIt Pro base + trajectory messages).
Reviewed changes
Copilot reviewed 6 out of 6 changed files in this pull request and generated 2 comments.
Show a summary per file
| File | Description |
|---|---|
src/trajectory_to_path.cpp |
Implements trajectory-to-pose-path conversion via FK on a selected tip link. |
include/experimental_behaviors/trajectory_to_path.hpp |
Declares the TrajectoryToPath behavior node and its ports/metadata API. |
src/register_behaviors.cpp |
Registers TrajectoryToPath with the behavior factory. |
test/test_behavior_plugins.cpp |
Ensures the new behavior can be instantiated by the BT factory. |
CMakeLists.txt |
Adds the new source file and updates dependency list. |
package.xml |
Declares new runtime/build dependencies for the behavior. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| moveit_studio_package() | ||
|
|
||
| set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs geometry_msgs moveit_pro_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected) | ||
| set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs geometry_msgs moveit_pro_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected trajectory_msgs moveit_pro_base) |
| <depend>pluginlib</depend> | ||
| <depend>behaviortree_cpp</depend> | ||
| <depend>tl_expected</depend> | ||
| <depend>trajectory_msgs</depend> |
henrygerardmoore
approved these changes
May 8, 2026
henrygerardmoore
left a comment
henrygerardmoore
left a comment
Contributor
There was a problem hiding this comment.
Sorry, meant to approve this before
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
TrajectoryToPathbehavior: converts atrajectory_msgs::msg::JointTrajectoryinto a vector ofPoseStampedby running forward kinematics on a chosen tip link.VisualizePathbehavior in MoveIt Pro core, enabling visualization of any joint-space trajectory as a tip-link polyline.RobotStateis hoisted out of the FK loop andPlanningScene::getCurrentState()is documented as the seed source (so joints absent from the trajectory — e.g. the rail or other arm in dual-arm setups — keep their actual current values).Consumer
Used by autowash_config's new dual-arm path visualization (sibling PR forthcoming on PickNikRoboticsServices/autowash_config) to render planned tip paths for both KUKA arms during MoveToWaypoint and wash-pass execution.
Test plan
🤖 Generated with Claude Code