Merged
Conversation
- Create physical_ai_bt ROS2 package with generic BT framework - Implement base classes: BTNode, BaseAction, BaseControl, BaseDecorator - Add control nodes: Sequence and Fallback - Implement InferenceAction: monitors AI Server status and manages VLA inference - Detects inference start from AI Server via /task/status topic - Maintains inference for configurable duration (default 5s) - Sends FINISH command to properly terminate inference - Adds 0.5s delay after stop to ensure AI Server cleanup - Implement RuleAction: moves robot to fixed target positions - Publishes to /leader/joint_trajectory - Monitors /follower/joint_states for position feedback - Configurable position threshold and timeout - Add tree_builder for constructing BT structures - Create generic bt_node executor with config integration - Add launch file with omx_f_config.yaml parameter loading - Integrate with physical_ai_server's joint_order configuration Architecture: Sequential execution (Inference -> Rule-based movement) - AI Server starts inference independently - BT detects and monitors inference state - After timeout, BT stops inference and executes rule-based control Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Remove unused parameters from InferenceAction: - policy_path (not used in current implementation) - task_instruction (not used in current implementation) - Remove excessive debug logging: - Status callback no longer logs every tick - Removed initialization logs - Removed periodic waiting logs - Make TreeBuilder more flexible: - Accept current_positions and target_positions as parameters - Remove hardcoded position values - Update BT Node parameters: - Add current_positions and target_positions parameters - Remove policy_path and task_instruction parameters - Update launch file: - Remove unused launch arguments - Simplify parameter passing - Update XML tree definition: - Remove unused input ports from InferenceAction - Reflect actual implementation parameters Result: Cleaner, more maintainable code with only necessary functionality Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Major fixes: - Fix joint_states topic name: /follower/joint_states -> /joint_states - Add joint order reordering logic to handle config mismatch - joint_states publishes: [gripper_joint_1, joint1-5] - Config expects: [joint1-5, gripper_joint_1] - Automatically reorders positions to match config order - Increase AI Server cleanup wait time: 0.5s -> 2.0s - Prevents trajectory command conflicts after inference stops Code cleanup: - Remove excessive debug logging in InferenceAction - Remove "Inference duration reached" logs - Remove "Waiting for AI Server" logs - Remove "AI Server cleanup completed" logs - Remove verbose logging in RuleAction - Remove "Sent fixed trajectory command" log - Remove "Published trajectory to joints" log - Remove position comparison logs during execution - Keep only essential logs (target reached, timeout, errors) - Simplify trajectory publishing logic - Add error logging guard to prevent log spam Result: Clean, production-ready code with successful BT execution - Inference: 5s monitoring -> FINISH command -> 2s cleanup - Rule-based: 2s trajectory execution -> SUCCESS Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Add xml_loader.py for Groot-compatible XML tree parsing
- Replace programmatic tree_builder.py with XML-driven approach
- Implement runtime parameter injection via placeholders ({param_name})
- Add dynamic robot config loading based on robot_type
- Support multiple robot types (omx_f, ffw_bg2_rev4, etc.)
- Add bt_node executable entry point
- Update launch file with OpaqueFunction for dynamic config path
- XML tree structure editable in Groot, runtime params via ROS2
Features:
- XML placeholders: {inference_timeout}, {current_positions}, etc.
- Auto-load {robot_type}_config.yaml
- Fallback to omx_f_config.yaml if not found
- joint_names injected from config at runtime
- Launch params override XML defaults
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Add ControlActionPublish service to pause/resume AI Server action publishing - Implement PauseActionPublishAction and ResumeActionPublishAction BT nodes - Add execution_active flag to InferenceAction to prevent stale callback processing - Clean up unused code and debug logs - Fix InferenceAction timing issues with proper state management This allows BT to control when AI Server publishes actions, enabling seamless integration of rule-based movements without topic conflicts. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Add omx_test.xml and omx_test_2.xml for testing BT workflows - Update bt_node launch file configuration - Update robot_control_tree.xml Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Update omx_test.xml with proper Pause/Resume pattern - Remove omx_test_2.xml and robot_control_tree.xml (obsolete) - Update bt_node launch configuration Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Removed joint_names input port definition from TreeNodesModel - Removed joint_names="" attributes from all RuleAction instances - joint_names is automatically loaded from physical_ai_server config - Cleaner XML for Groot2 tree design - Changed default tree_xml back to omx_test.xml in launch file Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…ibute errors Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…e_from_start to zero, only publish valid start/goal points, match inference topic structure. Also update goal positions and swerve rotation logic. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…ig loading clarified, indentation and parameter handling improved Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…er; now runtime_params is not used and only managed in XMLTreeLoader if needed. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…on, improved Twist publishing, and elapsed time logging Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
… RuleSwerve, fix XML and loader references, and remove Fallback control node. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…oint1 >= 1.0 for 2 seconds to end inference Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…erver inference control Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…ence and refactor related code Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…icate definitions, and ensure proper initialization and callback logic. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…efaults Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…te RuleSwerve tolerance and register CameraDepth in loader. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…int state feedback. Update tick_rate and swerve tolerance. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…ach target positions; remove timeout fallback. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
…rove feedback logic, and ensure BT loader/XML integration Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Combine left and right arm joint checking into a single loop by merging joint_names and positions lists. This eliminates code duplication and improves maintainability. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
The Rotate action uses Twist messages for mobile base rotation, not JointTrajectory messages. Removed the unused trajectory_pub and its import to improve code clarity. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Add AGENTS.md to .gitignore for project documentation - Remove redundant bt_node entry point from setup.py The physical_ai_bt entry point is sufficient and more descriptive than the generic bt_node name. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Add else clause to _load_node method to raise ValueError when encountering unknown node types or IDs. This prevents silent failures and makes debugging easier. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Reorganize behavior tree base classes into clearer file structure: - Create bt_core.py for BTNode and NodeStatus (core classes) - Update actions/base_action.py to only contain BaseAction - Create controls/base_control.py for BaseControl and BaseDecorator - Update all imports across action and control files This improves code maintainability by placing each class in an appropriately named location that reflects its purpose. Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Create physical_ai_bt/constants.py with descriptive constant names - Update all action files (move_arms, move_head, move_lift, rotate) to use constants - Improve code self-documentation and maintainability - No functional changes, purely refactoring Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Add DEFAULT joint name constants to MoveArms, MoveHead, MoveLift - Update action constructors to accept optional joint_names parameters - Add TreeLoader helper to extract joint names from topic_config - Pass configured joint names from TreeLoader to action instances - Maintain backward compatibility with hardcoded defaults - Support different robot types through configuration alone Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Add threading.Lock to protect shared result state - Add threading.Event for clean thread signaling and shutdown - Replace _thread_done/_thread_success with lock-protected _result - Ensure thread-safe access in tick(), _control_loop(), and reset() - Apply fix to all action classes (MoveArms, MoveHead, MoveLift, Rotate) - Prevent torn reads, stale values, and undefined behavior Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
- Remove control_action_publish_callback method (never registered as service) - Remove action_publish_enabled flag (always True, never changed) - Remove inference_paused flag (no service to control it) - Simplify publish_action method to always publish - No functional impact as BT never uses this functionality Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Signed-off-by: Seongwoo Kim <kimsw@robotis.com>
Add Behavior Tree
Summary of ChangesHello @Seongoo, I'm Gemini Code Assist1! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed! This pull request significantly enhances the robot's autonomous capabilities by integrating a new behavior tree system. It provides a structured and extensible framework for defining complex robot behaviors through XML configurations and a set of pre-defined action nodes for various robot movements, streamlining the development of rule-based control strategies. This foundational change is accompanied by a synchronized version update across related packages. Highlights
🧠 New Feature in Public Preview: You can now enable Memory to help Gemini Code Assist learn from your team's feedback. This makes future code reviews more consistent and personalized to your project's style. Click here to enable Memory in your admin console. Ignored Files
Using Gemini Code AssistThe full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips. Invoking Gemini You can request assistance from Gemini at any point by creating a comment using either
Customization To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a Limitations & Feedback Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for Github and other Google products, sign up here. You can also get AI-powered code generation, chat, as well as code reviews directly in the IDE at no cost with the Gemini Code Assist IDE Extension. Footnotes
|
![gemini-code-assist[bot]](https://avatars.githubusercontent.com/in/956858?s=60&v=4){kind=small}
There was a problem hiding this comment.
Code Review
This pull request introduces a new physical_ai_bt package, implementing a behavior tree system for robot control. It includes base classes for actions and control nodes, specific action implementations for moving arms, head, lift, and rotating the base, an XML-based tree loader, and a ROS 2 node to execute these trees. The changes also involve version bumps across several related packages and minor adjustments to configuration files. Overall, the new package provides a structured and extensible way to define robot behaviors. However, there are several areas where code quality and maintainability can be improved, particularly regarding Python import practices and parameter handling.
There was a problem hiding this comment.
Pull request overview
This PR bumps the version to 0.8.0 and introduces the new physical_ai_bt package, which implements a Python-based Behavior Tree system for robotic control. The package provides rule-based sequential execution of robot actions including arm movements, head control, lift positioning, and mobile base rotation.
Changes:
- Version bumped to 0.8.0 across all packages (rosbag_recorder, physical_ai_server, physical_ai_interfaces, physical_ai_manager, physical_ai_tools)
- Added new
physical_ai_btpackage with Behavior Tree implementation including Sequence control node and trajectory-based actions - Added
physical_ai_btas dependency in physical_ai_tools meta-package and CI workflow
Reviewed changes
Copilot reviewed 35 out of 39 changed files in this pull request and generated 24 comments.
Show a summary per file
| File | Description |
|---|---|
| rosbag_recorder/package.xml | Version bump to 0.8.0 |
| rosbag_recorder/CHANGELOG.rst | Added changelog entry for 0.8.0 |
| physical_ai_tools/package.xml | Version bump to 0.8.0 and added physical_ai_bt dependency |
| physical_ai_tools/CHANGELOG.rst | Added changelog documenting physical_ai_bt release |
| physical_ai_server/setup.py | Version bump to 0.8.0 |
| physical_ai_server/physical_ai_server.py | Minor formatting change (added blank line) |
| physical_ai_server/package.xml | Version bump to 0.8.0 |
| physical_ai_server/config/ffw_sg2_rev1_config.yaml | Minor whitespace fix |
| physical_ai_server/CHANGELOG.rst | Added changelog entry for 0.8.0 |
| physical_ai_manager/package.json | Version bump to 0.8.0 |
| physical_ai_manager/package-lock.json | Version bump to 0.8.0 |
| physical_ai_manager/CHANGELOG.rst | Added changelog entry for 0.8.0 |
| physical_ai_interfaces/package.xml | Version bump to 0.8.0 |
| physical_ai_interfaces/CHANGELOG.rst | Added changelog entry for 0.8.0 |
| physical_ai_bt/trees/ffw_sg2_rev1.xml | New behavior tree XML configuration |
| physical_ai_bt/setup.py | New package setup configuration |
| physical_ai_bt/setup.cfg | New package setup configuration |
| physical_ai_bt/resource/physical_ai_bt | New resource marker file |
| physical_ai_bt/physical_ai_bt/controls/sequence.py | New Sequence control node implementation |
| physical_ai_bt/physical_ai_bt/controls/base_control.py | New base control node class |
| physical_ai_bt/physical_ai_bt/controls/init.py | Controls package initialization |
| physical_ai_bt/physical_ai_bt/constants.py | Constants for BT configuration |
| physical_ai_bt/physical_ai_bt/bt_nodes_loader.py | XML-based tree loader |
| physical_ai_bt/physical_ai_bt/bt_node.py | Main ROS 2 BT node implementation |
| physical_ai_bt/physical_ai_bt/bt_core.py | Core BT classes and status enum |
| physical_ai_bt/physical_ai_bt/blackboard.py | Singleton blackboard for shared data |
| physical_ai_bt/physical_ai_bt/actions/rotate.py | Mobile base rotation action |
| physical_ai_bt/physical_ai_bt/actions/move_lift.py | Lift positioning action |
| physical_ai_bt/physical_ai_bt/actions/move_head.py | Head movement action |
| physical_ai_bt/physical_ai_bt/actions/move_arms.py | Dual arm movement action |
| physical_ai_bt/physical_ai_bt/actions/base_action.py | Base action class |
| physical_ai_bt/physical_ai_bt/actions/init.py | Actions package initialization |
| physical_ai_bt/physical_ai_bt/init.py | Package initialization |
| physical_ai_bt/package.xml | Package manifest |
| physical_ai_bt/bt_bringup/params/bt_node_params.yaml | Node parameter configuration |
| physical_ai_bt/bt_bringup/launch/bt_node.launch.py | Launch file for BT node |
| physical_ai_bt/CHANGELOG.rst | New package changelog |
| .gitignore | Removed extra blank line |
| .github/workflows/ros-ci.yml | Added physical_ai_bt to CI workflow |
Files not reviewed (1)
- physical_ai_manager/package-lock.json: Language not supported
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| self._thread = None | ||
| with self._lock: | ||
| self._result = None | ||
| self.joint_state = None |
There was a problem hiding this comment.
Potential race condition: self.joint_state is reset to None in the reset() method (line 181) without lock protection, while it may be concurrently accessed or modified by the _joint_state_callback running in a different thread. Consider protecting this assignment with self._lock.
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
2 participants
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.
No description provided.