Charging Dock Plugin based on dock pose subscriber #431
Conversation
Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
|
Important Review skippedAuto reviews are disabled on base/target branches other than the default branch. 🗂️ Base branches to auto review (1)
Please check the settings in the CodeRabbit UI or the You can disable this status message by setting the WalkthroughThe pull request introduces significant changes to the Changes
Possibly related PRs
Thank you for using CodeRabbit. We offer it for free to the OSS community and would appreciate your support in helping us grow. If you find it useful, would you consider giving us a shout-out on your favorite social media? 🪧 TipsChatThere are 3 ways to chat with CodeRabbit:
Note: Be mindful of the bot's finite context window. It's strongly recommended to break down tasks such as reading entire modules into smaller chunks. For a focused discussion, use review comments to chat about specific files and their changes, instead of using the PR comments. CodeRabbit Commands (Invoked using PR comments)
Other keywords and placeholders
Documentation and Community
|
|
@coderabbitai review this |
|
@delihus: I will review the changes. ✅ Actions performedReview triggered.
|
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 6
🧹 Outside diff range and nitpick comments (11)
panther_docking/README.md (3)
16-17: Improve node descriptions for clarity and correctness.
The node descriptions should be revised for better clarity and grammar:
-- `DockPosePublisherNode`: A node listens to `tf` and republishes position of `dock_pose` in the fixed frame.
-- `PantherChargingDock`: A plugin for a Panther robot what is responsible for a charger service.
+- `DockPosePublisherNode`: A node that listens to `tf` and republishes the position of `dock_pose` in the fixed frame.
+- `PantherChargingDock`: A plugin for the Panther robot that manages charging station interactions.🧰 Tools
🪛 LanguageTool
[uncategorized] ~16-~16: Loose punctuation mark.
Context: ... ## ROS Nodes - DockPosePublisherNode: A node listens to tf and republishes ...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Loose punctuation mark.
Context: ...the fixed frame. - PantherChargingDock: A plugin for a Panther robot what is r...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Possible missing comma found.
Context: ...rChargingDock`: A plugin for a Panther robot what is responsible for a charger servi...
(AI_HYDRA_LEO_MISSING_COMMA)
19-28: Consider adding more context about the node's role and benefits.
Since this node represents a significant architectural change from the previous tf-based implementation, it would be helpful to:
- Explain why this approach is preferred over the previous tf-based implementation
- Document any specific requirements or prerequisites
- Add example usage or common scenarios
🧰 Tools
🪛 LanguageTool
[grammar] ~23-~23: “Dock” is a singular noun. It appears that the verb form is incorrect.
Context: ...etry_msgs/PoseStamped*]: An offset dock pose. #### Subscribers - tf [*tf2_msgs/T...
(PCT_SINGULAR_NOUN_PLURAL_VERB_AGREEMENT)
41-43: Enhance parameter documentation with usage details.
The new parameters would benefit from additional context:
-- `~panther_version` [*double*, default: **1.21**]: A version of Panther robot.
-- `~base_frame` [*string*, default: **base_link**]: A base frame id of a robot.
-- `~external_detection_timeout` [*double*, default: **0.2**]: A timeout in seconds for looking up a transformation from an april tag of a dock to a base frame id.
+- `~panther_version` [*double*, default: **1.21**]: Version of the Panther robot. Used to handle version-specific docking behavior.
+- `~base_frame` [*string*, default: **base_link**]: The robot's base frame ID used for pose transformations and navigation.
+- `~external_detection_timeout` [*double*, default: **0.2**]: Maximum time (in seconds) to wait for a transformation between the AprilTag and base frame. Exceeding this timeout indicates detection loss.panther_docking/src/dock_pose_publisher_node.cpp (1)
28-29: Consider using a more specific node name.
The current node name "pose_publisher_node" is too generic. Consider using something more descriptive like "dock_pose_publisher_node" to better reflect its specific purpose.
- DockPosePublisherNode() : Node("pose_publisher_node")
+ DockPosePublisherNode() : Node("dock_pose_publisher_node")panther_docking/launch/docking.launch.py (1)
53-58: Enhance the log level argument description.
While the log level argument is correctly implemented, consider making the description more informative by including the available log levels.
- description="Logging level",
+ description="Logging level (debug, info, warn, error, fatal)",panther_docking/test/unit/test_panther_charging_dock.cpp (5)
53-66: Consider using initialization list in the constructor.
For better performance, consider moving member initializations to the initialization list:
-TestPantherChargingDock::TestPantherChargingDock()
+TestPantherChargingDock::TestPantherChargingDock()
+ : node_(std::make_shared<rclcpp_lifecycle::LifecycleNode>("test_node")),
+ tf_buffer_(std::make_shared<tf2_ros::Buffer>(node_->get_clock())),
+ dock_(std::make_shared<PantherChargingDockWrapper>())
{
- node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test_node");
- tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
-
// Silence error about dedicated thread's being necessary
tf_buffer_->setUsingDedicatedThread(true);
- dock_ = std::make_shared<PantherChargingDockWrapper>();
dock_pose_pub = node_->create_publisher<geometry_msgs::msg::PoseStamped>("dock_pose", 10);
node_->configure();
node_->activate();
}🧰 Tools
🪛 cppcheck
[performance] 55-55: Variable 'node_' is assigned in constructor body. Consider performing initialization in initialization list.
(useInitializationList)
130-132: Implement the missing GetStagingPoseGlobal test.
The TODO comment indicates a missing test for global staging pose functionality. This gap in test coverage could leave potential issues undetected.
Would you like me to help implement the GetStagingPoseGlobal test case? I can provide a template that follows the same pattern as GetStagingPoseLocal but tests the global pose scenarios.
136-136: Consider defining timeout constant.
The magic number 0.5 used for external_detection_timeout should be defined as a named constant at the class or file level for better maintainability and clarity.
+static constexpr double kExternalDetectionTimeout = 0.5;
+
TEST_F(TestPantherChargingDock, GetRefinedPose)
{
- node_->declare_parameter("dock.external_detection_timeout", 0.5);
+ node_->declare_parameter("dock.external_detection_timeout", kExternalDetectionTimeout);168-168: Use the same timeout constant here.
For consistency, use the same constant for external_detection_timeout as suggested in the previous comment.
- node_->declare_parameter("dock.external_detection_timeout", 0.5);
+ node_->declare_parameter("dock.external_detection_timeout", kExternalDetectionTimeout);193-195: Document filter stabilization and define iteration constant.
The magic number 10 used for filter stabilization should be defined as a named constant. Additionally, consider adding a comment explaining why this specific number of iterations is needed.
+// Number of iterations needed for the pose filter to stabilize
+static constexpr std::size_t kFilterStabilizationIterations = 10;
+
- for (std::size_t i = 0; i < 10; i++) {
+ for (std::size_t i = 0; i < kFilterStabilizationIterations; i++) {panther_docking/include/panther_docking/panther_charging_dock.hpp (1)
143-143: Consider passing PoseStampedMsg by constant reference
In the setDockPose method, consider changing the parameter to a constant reference:
-void setDockPose(const PoseStampedMsg::SharedPtr pose);
+void setDockPose(const PoseStampedMsg & pose);If ownership sharing or modification of the shared pointer is not required within the method, passing by constant reference can improve efficiency by avoiding the overhead of shared pointers.
📜 Review details
Configuration used: .coderabbit.yaml
Review profile: CHILL
📒 Files selected for processing (11)
- panther_docking/CMakeLists.txt (2 hunks)
- panther_docking/README.md (1 hunks)
- panther_docking/config/panther_docking_server.yaml (2 hunks)
- panther_docking/include/panther_docking/panther_charging_dock.hpp (3 hunks)
- panther_docking/launch/docking.launch.py (4 hunks)
- panther_docking/launch/station.launch.py (1 hunks)
- panther_docking/package.xml (1 hunks)
- panther_docking/src/dock_pose_publisher_node.cpp (1 hunks)
- panther_docking/src/panther_charging_dock.cpp (3 hunks)
- panther_docking/test/test_panther_charging_dock.cpp (0 hunks)
- panther_docking/test/unit/test_panther_charging_dock.cpp (1 hunks)
💤 Files with no reviewable changes (1)
- panther_docking/test/test_panther_charging_dock.cpp
🧰 Additional context used
🪛 LanguageTool
panther_docking/README.md
[uncategorized] ~16-~16: Loose punctuation mark.
Context: ... ## ROS Nodes - DockPosePublisherNode: A node listens to tf and republishes ...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Loose punctuation mark.
Context: ...the fixed frame. - PantherChargingDock: A plugin for a Panther robot what is r...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Possible missing comma found.
Context: ...rChargingDock`: A plugin for a Panther robot what is responsible for a charger servi...
(AI_HYDRA_LEO_MISSING_COMMA)
[grammar] ~23-~23: “Dock” is a singular noun. It appears that the verb form is incorrect.
Context: ...etry_msgs/PoseStamped*]: An offset dock pose. #### Subscribers - tf [*tf2_msgs/T...
(PCT_SINGULAR_NOUN_PLURAL_VERB_AGREEMENT)
[grammar] ~37-~37: “Dock” is a singular noun. It appears that the verb form is incorrect.
Context: ...etry_msgs/PoseStamped*]: An offset dock pose. #### Parameters - ~panther_version...
(PCT_SINGULAR_NOUN_PLURAL_VERB_AGREEMENT)
🪛 cppcheck
panther_docking/test/unit/test_panther_charging_dock.cpp
[performance] 55-55: Variable 'node_' is assigned in constructor body. Consider performing initialization in initialization list.
(useInitializationList)
🔇 Additional comments (27)
panther_docking/config/panther_docking_server.yaml (3)
19-20: Verify if tighter docking tolerances are achievable.
The docking thresholds have been significantly reduced:
- Distance threshold: 0.15m → 0.08m
- Yaw threshold: 0.15 rad → 0.1 rad
While tighter tolerances could improve docking precision, please ensure these values have been thoroughly tested with the real robot under various conditions (different battery levels, floor surfaces, etc.).
Consider implementing a fallback mechanism or gradual tolerance adjustment if the robot fails to achieve these precise alignments after multiple attempts.
21-22: Review the reduced staging distance and detection timeout.
The changes:
- Staging distance reduced by 37.5% (-0.8m → -0.5m)
- External detection timeout reduced by 66% (0.3s → 0.1s)
These significant reductions might impact the system's reliability:
- Shorter staging distance gives less room for alignment
- Faster timeout might not accommodate real-world sensor latency
Could you share the testing results that validated these new values? Particularly interested in:
- Success rate with the shorter staging distance
- False timeout occurrences with the 0.1s detection window
33-35: Review velocity parameters for compatibility with tighter tolerances.
The velocity changes appear to be in tension with the tighter docking tolerances:
- Increased max linear velocity (0.2 → 0.3 m/s)
- Reduced max angular velocity (0.3 → 0.15 rad/s)
- Zero minimum linear velocity (0.05 → 0.0 m/s)
While allowing complete stops (v_linear_min = 0.0) is good for precise positioning, the higher maximum linear velocity might make it harder to achieve the tighter tolerances you've set (0.08m, 0.1rad).
Consider implementing a multi-phase approach:
- Faster approach phase with current velocities
- Switch to lower velocities (perhaps 0.1 m/s max) for final precise alignment
panther_docking/package.xml (1)
31-31: LGTM! Verify xacro usage in the codebase.
The addition of xacro as an execution dependency is correctly implemented. Let's verify its usage in the codebase.
✅ Verification successful
Xacro dependency is correctly added and actively used
The xacro dependency is actively used in the panther_docking package's launch file station.launch.py to process the wibotic_station.urdf.xacro file. This aligns with the package's purpose of integrating Panther with the Wibotic charger.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Verify that xacro is actually used in the codebase
# Expected: Find xacro file references or xacro-related code
# Test 1: Search for .xacro files
echo "Searching for .xacro files..."
fd -e xacro
# Test 2: Search for xacro-related code or launch files that might use xacro
echo "Searching for xacro usage in code and launch files..."
rg -g '!package.xml' -i 'xacro'
Length of output: 17000
panther_docking/CMakeLists.txt (3)
43-45: LGTM! Well-structured include directories.
The target_include_directories setup correctly uses generator expressions for build and install interfaces.
55-60: LGTM! Better test organization.
Moving the test file to a unit/ subdirectory improves the test organization structure.
Line range hint 1-72: Verify dependencies for the new implementation.
Since this PR depends on changes from ros_components_description and requires .repos file updates, we should verify the package dependencies.
✅ Verification successful
Let me gather more information about the dependencies by checking the source files.
Let me check the specific dependencies and usage of ros_components_description in the relevant files.
Dependencies are properly configured for the new implementation.
The verification shows that:
- The
ros_components_descriptiondependency is correctly declared inpackage.xml - The dependency is properly used in
station.launch.pyto load the Wibotic station URDF - All required dependencies from CMakeLists.txt are properly declared in package.xml
- The .repos file includes the correct repository for
ros_components_description
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Check if all required dependencies are declared
echo "Checking package.xml for dependencies..."
cat package.xml
# Check .repos file for updates
echo "Checking .repos file..."
find . -name "*.repos" -exec cat {} \;
Length of output: 2385
Script:
#!/bin/bash
# Check source files for dependencies
echo "Checking source files for dependencies..."
fd -e cpp -e hpp -e h . src/ include/ -x cat {}
# Check if ros_components_description is used
echo "Checking for ros_components_description usage..."
rg -l "ros_components_description" .
# Check for any potential missing dependencies
echo "Looking for potential missing dependencies..."
rg -l "include|import" . | grep -v "node_modules\|build\|dist"
Length of output: 14852
Script:
#!/bin/bash
# Check the content of launch files using ros_components_description
echo "Checking launch files content..."
cat ./panther_docking/launch/station.launch.py
# Check package.xml content for ros_components_description dependency
echo "Checking package.xml content..."
cat ./panther_docking/package.xml
# Check if there are any direct includes from ros_components_description
echo "Checking for direct includes..."
rg -A 5 "ros_components_description" ./panther_docking/
Length of output: 6324
panther_docking/README.md (1)
Line range hint 1-43: Add simulation testing documentation.
The PR mentions that simulation tests are pending. Consider adding a section about:
- How to set up and run simulation tests
- Expected behavior in simulation
- Any differences between simulation and real robot behavior
🧰 Tools
🪛 LanguageTool
[uncategorized] ~16-~16: Loose punctuation mark.
Context: ... ## ROS Nodes - DockPosePublisherNode: A node listens to tf and republishes ...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Loose punctuation mark.
Context: ...the fixed frame. - PantherChargingDock: A plugin for a Panther robot what is r...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Possible missing comma found.
Context: ...rChargingDock`: A plugin for a Panther robot what is responsible for a charger servi...
(AI_HYDRA_LEO_MISSING_COMMA)
[grammar] ~23-~23: “Dock” is a singular noun. It appears that the verb form is incorrect.
Context: ...etry_msgs/PoseStamped*]: An offset dock pose. #### Subscribers - tf [*tf2_msgs/T...
(PCT_SINGULAR_NOUN_PLURAL_VERB_AGREEMENT)
[grammar] ~37-~37: “Dock” is a singular noun. It appears that the verb form is incorrect.
Context: ...etry_msgs/PoseStamped*]: An offset dock pose. #### Parameters - ~panther_version...
(PCT_SINGULAR_NOUN_PLURAL_VERB_AGREEMENT)
panther_docking/src/dock_pose_publisher_node.cpp (3)
1-24: LGTM! Well-organized includes and proper license header.
The includes are logically grouped and all necessary for the node's functionality.
79-86: LGTM! Standard ROS 2 node main function.
The implementation follows ROS 2 best practices for node initialization and shutdown.
1-86: Verify consistency with other pose publisher implementations.
Let's check if there are similar pose publisher implementations in the codebase to ensure consistency in approach and naming.
✅ Verification successful
Let me check the content of the other files to verify the implementation consistency.
Let me check one more aspect to verify the frame naming and topic patterns.
Implementation is consistent with the codebase patterns
The verification shows that the implementation follows consistent patterns across the codebase:
- Frame naming follows the standard pattern using namespaced odom frame
- Topic naming follows the pattern with "docking/" prefix as seen in other components
- Namespace handling is consistent with other nodes, properly handling both device and robot namespaces
- Transform lookup and pose publishing patterns align with the codebase conventions
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for similar pose publisher implementations
echo "Searching for similar pose publisher implementations..."
rg -l "create_publisher.*PoseStamped" --type cpp
echo "Searching for similar transform lookup patterns..."
ast-grep --pattern 'try {
$_ = $_->lookupTransform($_, $_, $_);
} catch (tf2::TransformException & $_) {
$$$
}'
Length of output: 552
Script:
#!/bin/bash
# Check content of other pose publisher implementations
echo "Content of panther_charging_dock.cpp:"
rg -A 10 "create_publisher.*PoseStamped" panther_docking/src/panther_charging_dock.cpp
echo -e "\nContent of test_panther_charging_dock.cpp:"
rg -A 10 "create_publisher.*PoseStamped" panther_docking/test/unit/test_panther_charging_dock.cpp
echo -e "\nChecking frame naming patterns:"
rg "frame_id.*=.*odom" --type cpp
Length of output: 1191
Script:
#!/bin/bash
# Check topic and frame naming patterns more broadly
echo "Checking frame naming in the codebase:"
rg "target_frame_.*=|frame_id.*=" --type cpp -A 1
echo -e "\nChecking docking-related topic patterns:"
rg "create_publisher.*docking" --type cpp -A 1
echo -e "\nChecking namespace handling:"
rg "get_namespace|device_namespace" --type cpp -A 2
Length of output: 7120
panther_docking/launch/station.launch.py (2)
102-102: LGTM: AprilTag ID default value change.
The change of default AprilTag ID from "1" to "0" is acceptable and appears to align with the system's conventions.
108-108: Verify physical AprilTag dimensions.
The AprilTag size has been changed from 15cm to 7cm. Please verify that this matches the actual physical dimensions of the AprilTags mounted on the charging dock hardware, as this parameter is critical for accurate pose estimation during docking.
panther_docking/launch/docking.launch.py (3)
72-72: LGTM: Consistent logging configuration.
The logging configuration is consistently applied across both nodes, following ROS 2 best practices by:
- Using the common log level argument
- Setting explicit rcl logging levels
Also applies to: 99-99
120-120: LGTM: Proper launch description structure.
The launch description is well-organized with:
- Log level declaration properly placed with other declarations
- Dock pose publisher added after its dependencies
Also applies to: 124-124
93-100: Verify the dock pose publisher node integration.
The node configuration looks correct, but let's verify its integration with the docking system.
Run this script to verify the node's integration:
Consider documenting the following in the package's README:
- The purpose and responsibilities of this node
- Its interaction with the docking server
- The topics it publishes/subscribes to
✅ Verification successful
The dock pose publisher node is properly integrated with the docking system.
The verification shows that:
- The dock pose publisher correctly publishes to
docking/dock_posetopic which is consumed by thePantherChargingDockplugin - The node properly transforms poses between the robot's TF frames (from
<robot_namespace>/<device_namespace>_wibotic_receiver_requested_pose_linkto<robot_namespace>/odom) - The docking system uses this pose information for:
- Generating staging poses for approach
- Verifying docking status
- Filtering and refining dock poses for precise docking
The integration is complete and follows the expected architecture of the docking system.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Verify the dock pose publisher's topics and parameters
# Check for any topic subscriptions or publications related to dock poses
rg -A 5 'create_\w*\s*[publisher|subscription].*dock.*pose'
# Check for any parameters related to dock poses in the config
fd -e yaml | xargs rg 'dock.*pose'
Length of output: 1018
Script:
#!/bin/bash
# Let's check the dock pose publisher implementation and its integration
# Search for the dock pose publisher source file
fd dock_pose_publisher
# Search for any dock pose related topics in the source code
rg -A 5 "dock.*pose" --type cpp --type python
# Check for any launch file configurations related to docking
fd -e launch.py | xargs rg "dock"
# Look for any test files related to docking
fd test | grep -i dock
Length of output: 2446
Script:
#!/bin/bash
# Let's examine the dock pose publisher implementation
cat panther_docking/src/dock_pose_publisher_node.cpp
# Check the docking server configuration
fd -e yaml | grep -i dock | xargs cat
# Look for any integration tests or examples
rg -A 10 "dock.*pose" --type cpp
Length of output: 31250
panther_docking/include/panther_docking/panther_charging_dock.hpp (2)
28-28: Include <rclcpp_lifecycle/lifecycle_node.hpp> is appropriate
The inclusion of <rclcpp_lifecycle/lifecycle_node.hpp> is necessary for using rclcpp_lifecycle::LifecycleNode references in the class.
156-156: Ensure consistency after replacing dock_pose_pub_ with dock_pose_sub_
Since dock_pose_pub_ has been replaced with dock_pose_sub_, ensure that all references to dock_pose_pub_ have been updated throughout the codebase to prevent any potential errors.
Run the following script to check for any remaining references to dock_pose_pub_:
✅ Verification successful
No inconsistencies found after publisher to subscriber change
The verification shows that dock_pose_pub_ has been completely replaced with dock_pose_sub_ throughout the codebase. The subscription is properly:
- Declared in the header file
- Created in the
activate()method - Reset in both
cleanup()anddeactivate()methods
All references are consistent with the change from a publisher to a subscriber.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Verify that no references to `dock_pose_pub_` remain in the codebase.
# Test: Search for references to `dock_pose_pub_`. Expect: No matches found.
rg 'dock_pose_pub_'
Length of output: 19
Script:
#!/bin/bash
# Let's verify both the old and new variable names in the codebase
# First, check for any references to either name in header files
echo "=== Checking header files ==="
fd -e hpp -e h --exec rg -l "dock_pose_(pub|sub)_"
# Then check implementation files
echo "=== Checking implementation files ==="
fd -e cpp --exec rg -l "dock_pose_(pub|sub)_"
# Finally, get the actual context of any matches to understand the usage
echo "=== Detailed matches with context ==="
rg -C 3 "dock_pose_(pub|sub)_"
Length of output: 2682
panther_docking/src/panther_charging_dock.cpp (9)
54-55: Proper Resource Cleanup
The cleanup() method correctly resets the dock_pose_sub_ and staging_pose_pub_ to release resources when the node is shutting down. This ensures there are no dangling subscriptions or publishers.
61-65: Initializing Subscribers and Publishers in activate()
The activate() method appropriately initializes the subscriber dock_pose_sub_ to listen to the "docking/dock_pose" topic and binds it to the setDockPose callback. It also sets up the staging_pose_pub_ publisher. This aligns with the new subscription-based model for dock pose updates.
76-78: Simplified Parameter Declaration
The updated parameter declarations remove unnecessary parameters and add essential ones like "fixed_frame". This simplifies configuration and reduces potential confusion.
Also applies to: 85-87
99-101: Accurate Parameter Retrieval
Parameters "base_frame" and "fixed_frame" are correctly retrieved and stored in member variables. This ensures that the class operates using the correct frames defined in the configuration.
114-125: Robust Staging Pose Calculation with Error Handling
The getStagingPose() method now includes a check for an empty dock_pose_. By throwing a FailedToDetectDock exception when no dock pose is detected, it prevents undefined behavior downstream. The calculation of the staging pose using the dock pose and offsets is correctly implemented.
135-155: Improved Dock Pose Validation in getRefinedPose()
The method now includes checks for an empty dock_pose_ and validates the timestamp against a timeout. This enhances reliability by ensuring that outdated or missing dock poses are not used.
161-165: Correct Frame Transformation in isDocked()
Transforming the robot's pose to the fixed frame before proximity checking is essential for accurate docking status determination. The use of TransformPose() with tf2_buffer_ and fixed_frame_name_ is appropriate.
185-188: Efficient Dock Pose Updating with Pose Filter
The setDockPose() method now updates the dock pose using pose_filter_->update(*pose), which helps in smoothing the pose data and reducing noise. This change enhances the accuracy of the docking process.
118-120: Verify Exception Class Definition
The code throws opennav_docking_core::FailedToDetectDock when no dock pose is detected. Ensure that the FailedToDetectDock exception class is properly defined and included in the codebase to prevent compilation errors.
Run the following script to confirm the existence of the FailedToDetectDock exception class:
Also applies to: 139-140
✅ Verification successful
The exception class appears to be from an external dependency
Based on the investigation:
- The
FailedToDetectDockexception is used consistently in the codebase - It's imported from
opennav_docking_core/docking_exceptions.hppwhich is an external dependency - The header file properly includes this dependency:
#include <opennav_docking_core/docking_exceptions.hpp> - The code is used in both implementation and tests, suggesting it's a valid part of the API
The exception class is properly included and appears to be part of the OpenNav Docking Core library's public API, not requiring definition in this codebase.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Verify that 'FailedToDetectDock' is defined in the codebase.
# Test: Search for the class definition. Expect to find the class declaration.
rg -t cpp 'class FailedToDetectDock'
Length of output: 38
Script:
#!/bin/bash
# Let's try a broader search for any mention of FailedToDetectDock
rg "FailedToDetectDock"
# Also search for header files that might contain the exception definition
fd -e hpp -e h | xargs cat | grep -A 5 -B 5 "FailedToDetectDock"
# Check if there are any exception-related headers in the opennav_docking_core
fd -e hpp -e h "exception" -p opennav_docking_core
Length of output: 859
Script:
#!/bin/bash
# Let's check the includes in the file to find where the exception is defined
rg -A 5 "^#include" panther_docking/src/panther_charging_dock.cpp
# Also check the test file's includes since it uses the exception
rg -A 5 "^#include" panther_docking/test/unit/test_panther_charging_dock.cpp
# Look for any exception-related headers in the entire codebase
fd -e hpp -e h | grep -i exception
Length of output: 1027
Script:
#!/bin/bash
# Let's check the header file of panther_charging_dock
cat panther_docking/include/panther_docking/panther_charging_dock.hpp
# Also check if this is inheriting from ChargingDock which might define the exception
fd -e hpp -e h "charging_dock" -I
Length of output: 5836
Script:
#!/bin/bash
# The header file includes opennav_docking_core/docking_exceptions.hpp, let's check its content
rg -p "opennav_docking_core/docking_exceptions.hpp" -l
rg -g "docking_exceptions.hpp" "class FailedToDetectDock"
# Let's also check if this file exists in the repository
fd "docking_exceptions.hpp"
Length of output: 355
Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
…e publisher Signed-off-by: Jakub Delicat <[email protected]>
|
@coderabbitai could you review after the suggestions? |
|
@delihus: You requested a review after your suggestions. I will re-review all the changes. ✅ Actions performedFull review triggered. |
There was a problem hiding this comment.
Actionable comments posted: 8
🧹 Outside diff range and nitpick comments (7)
panther_docking/config/panther_docking_server.yaml (1)
25-28: LGTM! Consider updating documentationThe changes to dock configuration look good:
- Simplified dock naming
- Updated frame reference to use receiver's requested pose instead of station position, which aligns with moving away from tf-based approach
Consider updating the documentation to explain the significance of using
wibotic_receiver_requested_pose_linkand how it differs from the previous approach.panther_docking/README.md (2)
16-17: Improve node descriptions for clarity.The node descriptions have grammar issues and could be clearer:
-- `DockPosePublisherNode`: A node listens to `tf` and republishes position of `dock_pose` in the fixed frame. -- `PantherChargingDock`: A plugin for a Panther robot what is responsible for a charger service. +- `DockPosePublisherNode`: A node that listens to `tf` and republishes the position of `dock_pose` in the fixed frame. +- `PantherChargingDock`: A plugin for the Panther robot that manages charging dock interactions.🧰 Tools
🪛 LanguageTool
[uncategorized] ~16-~16: Loose punctuation mark.
Context: ... ## ROS Nodes -DockPosePublisherNode: A node listens totfand republishes ...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Loose punctuation mark.
Context: ...the fixed frame. -PantherChargingDock: A plugin for a Panther robot what is r...(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Possible missing comma found.
Context: ...rChargingDock`: A plugin for a Panther robot what is responsible for a charger servi...(AI_HYDRA_LEO_MISSING_COMMA)
31-33: Improve parameter documentation format and clarity.The parameter descriptions need consistent formatting and better clarity:
-- `fixed_frame` [*string*, default: **odom**]: A fixed frame id of a robot. -- `<dock_name>.type` [*string*, default: **panther_charging_dock**]: It checks if this dock with name `dock_name` is a type of `panther_charging_dock`. -- `<dock_name>.frame` [*string*, default: **main_wibotic_receiver_requested_pose_link** ]: Then look for transformation between `fixed_frame` and `<dock_name>.frame` to publish `dock_pose` +- `fixed_frame` [*string*, default: **odom**]: The fixed frame ID of the robot. +- `<dock_name>.type` [*string*, default: **panther_charging_dock**]: The type identifier for the dock named `dock_name`. Must be set to `panther_charging_dock` for this plugin. +- `<dock_name>.frame` [*string*, default: **main_wibotic_receiver_requested_pose_link**]: The frame ID used to compute transformation between `fixed_frame` and dock position for publishing `dock_pose`.panther_docking/src/dock_pose_publisher_node.cpp (1)
29-29: Consider renaming the node for consistency.The node name
"pose_publisher_node"could be more descriptive and consistent with the class name. Consider renaming it to"dock_pose_publisher_node".Apply this diff to rename the node:
- DockPosePublisherNode() : Node("pose_publisher_node") + DockPosePublisherNode() : Node("dock_pose_publisher_node")panther_docking/include/panther_docking/panther_charging_dock.hpp (2)
143-143: Add documentation forupdateAndPublishStagingPosemethodPlease provide a brief description and documentation for the new
updateAndPublishStagingPose()method. Adding comments will enhance code readability and help other developers understand its purpose and usage.
145-145: Add documentation forsetDockPosemethodSimilarly, consider adding documentation for the
setDockPose(const PoseStampedMsg::SharedPtr pose)method. Clear comments will improve maintainability and clarify how this method should be used within the class.panther_docking/test/unit/test_panther_charging_dock.cpp (1)
130-133: Address the TODO: Implement 'GetStagingPoseGlobal' testThere's a TODO comment indicating that the
GetStagingPoseGlobaltest needs to be implemented. Adding this test will enhance coverage and ensure the global staging pose functionality is verified.Would you like assistance in implementing this test, or should a GitHub issue be opened to track this task?
📜 Review details
Configuration used: .coderabbit.yaml
Review profile: CHILL
📒 Files selected for processing (12)
panther_description/config/docking_components.yaml(1 hunks)panther_docking/CMakeLists.txt(2 hunks)panther_docking/README.md(1 hunks)panther_docking/config/panther_docking_server.yaml(1 hunks)panther_docking/include/panther_docking/panther_charging_dock.hpp(3 hunks)panther_docking/launch/docking.launch.py(4 hunks)panther_docking/launch/station.launch.py(2 hunks)panther_docking/package.xml(1 hunks)panther_docking/src/dock_pose_publisher_node.cpp(1 hunks)panther_docking/src/panther_charging_dock.cpp(3 hunks)panther_docking/test/test_panther_charging_dock.cpp(0 hunks)panther_docking/test/unit/test_panther_charging_dock.cpp(1 hunks)
💤 Files with no reviewable changes (1)
- panther_docking/test/test_panther_charging_dock.cpp
🧰 Additional context used
📓 Learnings (3)
panther_docking/include/panther_docking/panther_charging_dock.hpp (1)
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:51-58
Timestamp: 2024-10-30T12:18:08.310Z
Learning: The `DockPosePublisherNode` should be converted to a `LifecycleNode` and activated in `PantherChargingDock`.
panther_docking/src/dock_pose_publisher_node.cpp (3)
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:51-58
Timestamp: 2024-10-30T12:18:08.310Z
Learning: The `DockPosePublisherNode` should be converted to a `LifecycleNode` and activated in `PantherChargingDock`.
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:51-58
Timestamp: 2024-10-30T12:18:08.311Z
Learning: Avoid using `WARN` level logging for transform lookup failures in `DockPosePublisherNode` since the node is always active even when the robot doesn't want to dock.
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:60-68
Timestamp: 2024-10-30T12:10:28.618Z
Learning: In `panther_docking/src/dock_pose_publisher_node.cpp`, the robot in the odometry frame should always have `pose_msg.pose.position.z = 0.0`, so it's appropriate to hardcode the Z-coordinate to 0.0.
panther_docking/src/panther_charging_dock.cpp (1)
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:51-58
Timestamp: 2024-10-30T12:18:08.310Z
Learning: The `DockPosePublisherNode` should be converted to a `LifecycleNode` and activated in `PantherChargingDock`.
🪛 LanguageTool
panther_docking/README.md
[uncategorized] ~16-~16: Loose punctuation mark.
Context: ... ## ROS Nodes - DockPosePublisherNode: A node listens to tf and republishes ...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Loose punctuation mark.
Context: ...the fixed frame. - PantherChargingDock: A plugin for a Panther robot what is r...
(UNLIKELY_OPENING_PUNCTUATION)
[uncategorized] ~17-~17: Possible missing comma found.
Context: ...rChargingDock`: A plugin for a Panther robot what is responsible for a charger servi...
(AI_HYDRA_LEO_MISSING_COMMA)
[grammar] ~23-~23: “Dock” is a singular noun. It appears that the verb form is incorrect.
Context: ...etry_msgs/PoseStamped*]: An offset dock pose. #### Subscribers - tf [*tf2_msgs/T...
(PCT_SINGULAR_NOUN_PLURAL_VERB_AGREEMENT)
[grammar] ~43-~43: “Dock” is a singular noun. It appears that the verb form is incorrect.
Context: ...etry_msgs/PoseStamped*]: An offset dock pose. #### Parameters - base_frame [*str...
(PCT_SINGULAR_NOUN_PLURAL_VERB_AGREEMENT)
🪛 cppcheck
panther_docking/test/unit/test_panther_charging_dock.cpp
[performance] 55-55: Variable 'node_' is assigned in constructor body. Consider performing initialization in initialization list.
(useInitializationList)
🔇 Additional comments (17)
panther_description/config/docking_components.yaml (2)
17-18: Verify the new camera mounting configuration.
The camera component (CAM01) has been relocated with:
- New parent link:
front_bumper_link(previouslylights_channel_1_link) - Updated position: slightly lower on z-axis (-0.06)
Please confirm:
- These changes match the physical mounting point on the robot
- The transformation chain remains valid with the new parent link
#!/bin/bash
# Search for dependencies on the camera's parent link and position
rg -A 5 "front_bumper_link|lights_channel_1_link"
# Look for any camera pose calculations that might need updates
ast-grep --pattern 'camera.*pose'12-12: Verify the inverted orientation of the wireless charger component.
The RPY values for WCH02 have been inverted, which could significantly impact the docking alignment. Please ensure this matches the physical orientation of the charging dock in the real robot setup.
✅ Verification successful
Let me generate another verification script to search for more context about WCH02 and wireless charger related transformations.
Based on the search results, I can now generate the final response:
The inverted orientation for WCH02 is correct and matches the world-frame setup
The WCH02 component is properly configured with its new orientation values (rpy: 1.57 0.0 -1.57) as it's attached to the world frame (parent_link: world), not the robot frame. This is a valid transformation setup for a static charging dock in the environment. The inverted RPY values ensure correct alignment between the robot and the charging station during docking.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for any related transformation or pose calculations that might be affected
rg -A 5 "WCH02"
Length of output: 3127
Script:
#!/bin/bash
# Search for wireless charger related configurations and transformations
rg -A 5 "wireless_charger|WCH02" --type yaml --type xml
# Look for any docking pose calculations
rg -A 5 "dock.*pose" --type cpp --type python
Length of output: 1005
panther_docking/config/panther_docking_server.yaml (2)
19-23: Verify the impact of tighter thresholds and timeouts
The changes introduce significantly tighter tolerances and shorter timeouts:
external_detection_timeoutreduced by 66% (0.3 → 0.1)docking_distance_thresholdreduced by 47% (0.15 → 0.08)docking_yaw_thresholdreduced by 33% (0.15 → 0.1)
Please ensure these stricter values don't impact docking reliability in real-world conditions with sensor noise and delays.
Additionally, the new filter_coef parameter needs documentation explaining its purpose and how to tune it.
34-36: Verify the impact of velocity limit changes
The controller velocity limits have been significantly modified:
v_linear_min: 0.05 → 0.0 (allows complete stops)v_linear_max: 0.2 → 0.3 (50% increase)v_angular_max: 0.3 → 0.15 (50% decrease)
Please verify:
- Setting
v_linear_minto 0.0 doesn't cause stuttering motion - The increased linear speed doesn't compromise docking precision
- The reduced angular speed is sufficient for efficient alignment
panther_docking/package.xml (1)
31-31: LGTM! The xacro dependency addition is appropriate.
The addition of xacro as an execution dependency is well-placed and aligns with the PR's goal of improving dock pose management. The dependency is correctly specified as an exec_depend since it's needed at runtime for processing XML descriptions.
panther_docking/CMakeLists.txt (3)
43-45: LGTM! Well-structured include directory configuration.
The include directory configuration properly separates build and install interfaces using generator expressions, following CMake best practices.
55-60: LGTM! Verify test file structure.
The test configuration is properly set up with correct dependencies and linking.
#!/bin/bash
# Verify test file exists and check its basic structure
if [ -f "test/unit/test_panther_charging_dock.cpp" ]; then
echo "Test file exists"
# Check if it includes gtest and the main component
rg -A 5 "include.*gtest" test/unit/test_panther_charging_dock.cpp
rg -A 5 "include.*panther_charging_dock" test/unit/test_panther_charging_dock.cpp
fi63-66: LGTM! Verify source file implementation.
The executable target is properly configured with correct dependencies and installation path.
#!/bin/bash
# Verify source file exists and check its basic node structure
if [ -f "src/dock_pose_publisher_node.cpp" ]; then
echo "Source file exists"
# Check for proper node implementation
ast-grep --pattern 'class $_ : public rclcpp::Node {
$$$
}' src/dock_pose_publisher_node.cpp
fipanther_docking/launch/station.launch.py (3)
108-108: LGTM: AprilTag size matches real hardware specifications.
The reduction in AprilTag size from 15cm to 6cm aligns with the PR objective of improving compatibility with the real robot setup. However, please verify that the detection reliability is maintained at this smaller size across various operating distances.
#!/bin/bash
# Description: Check for any documentation or test specifications about minimum detection distances
# Search for detection distance specifications
rg -i "detection.*distance|viewing.*distance"
rg -i "minimum.*distance.*tag"
# Search for test configurations
fd -e test.py -e test.cpp -x grep -l "apriltag.*size" {}36-36: Verify the impact of increased AprilTag scale.
Increasing the scale from 100 to 1000 will generate higher resolution AprilTag images, which could improve detection accuracy. However, we should verify that this doesn't impact system performance.
✅ Verification successful
AprilTag scale increase appears safe to proceed
The scale parameter only affects the generated reference image resolution and doesn't impact runtime detection performance since:
- It's only used during launch setup to generate the reference tag image
- The physical tag size (0.06m) remains unchanged
- No performance concerns were found in the codebase related to AprilTag scale
While we couldn't directly measure the image size difference, the scale increase should be safe as it's a one-time generation cost during launch.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Check the size and memory impact of generated AprilTag images
# Generate both scales and compare
python3 << EOF
from moms_apriltag import TagGenerator2
import imageio
import os
tag_generator = TagGenerator2("tag36h11")
# Generate with old scale
tag_old = tag_generator.generate(0, scale=100)
imageio.imwrite("/tmp/tag_old.png", tag_old)
# Generate with new scale
tag_new = tag_generator.generate(0, scale=1000)
imageio.imwrite("/tmp/tag_new.png", tag_new)
# Print sizes
print(f"Old scale image size: {os.path.getsize('/tmp/tag_old.png')} bytes")
print(f"New scale image size: {os.path.getsize('/tmp/tag_new.png')} bytes")
EOF
Length of output: 84
Script:
#!/bin/bash
# Let's verify if there are any performance concerns or memory usage patterns
# by searching for related discussions, comments, or issues about AprilTag scale
# Search for any scale-related comments or discussions
rg -i "scale.*apriltag|apriltag.*scale" -g "!*.pyc" -g "!*.png" -g "!*.jpg"
# Search for any performance or memory related comments
rg -i "performance.*apriltag|memory.*apriltag" -g "!*.pyc" -g "!*.png" -g "!*.jpg"
# Look for any test files or documentation about AprilTag configuration
fd -e py -e md -e rst -e txt | rg -i "apriltag|tag.*test|test.*tag"
Length of output: 232
102-102: Verify compatibility of new default AprilTag ID.
The change of default AprilTag ID from 1 to 0 needs verification to ensure it doesn't conflict with existing configurations or other components in the system.
✅ Verification successful
Default AprilTag ID change appears safe to implement
The search results show that:
- The AprilTag ID is only configured as a launch argument with no hardcoded dependencies found
- The ID is passed as a configurable parameter, allowing users to override it if needed
- No other components in the codebase have explicit dependencies on the specific tag ID value
The change from ID 1 to 0 should not cause any compatibility issues as the system is designed to be configurable through launch parameters.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Search for hardcoded references to AprilTag ID 1
# Look for potential hardcoded references to the old ID
rg -l "tag_?id.*1" --type yaml
rg -l "apriltag.*id.*1" --type yaml
# Look for launch file overrides
fd -e launch.py -x grep -l "apriltag_id.*1" {}
Length of output: 244
Script:
#!/bin/bash
# Let's try a more comprehensive search for AprilTag configurations
# Search for any AprilTag related configurations in yaml files
rg -i "apriltag|tag_id|tag.*id" --type yaml
# Search for AprilTag configurations in launch files
fd -e launch.py -x grep -i "apriltag|tag_id" {}
# Search for any numeric tag IDs in launch files
fd -e launch.py -x grep -A 2 -B 2 "default_value.*[0-9]" {}
# Look for test configurations
fd -e test -x grep -i "apriltag|tag_id" {}
Length of output: 2802
panther_docking/launch/docking.launch.py (4)
53-59: LGTM: Well-structured log level declaration.
The log level argument is properly declared with appropriate validation choices and a sensible default value.
73-73: Previous discussion resolved the log level configuration.
The dual log level configuration (general and rcl-specific) was previously discussed and justified with evidence showing its benefit in preventing excessive debug logs.
125-129: LGTM: Launch description properly updated.
The new components are correctly added to the launch description in the appropriate order.
94-105: Verify topic configuration for dock_pose_publisher.
The node configuration looks good, but consider whether any topic remappings are needed to ensure proper communication between nodes.
Let's check the node's topic configuration:
✅ Verification successful
No topic remappings needed for dock_pose_publisher
The node publishes the dock pose on the topic docking/dock_pose which is already correctly namespaced in the code. Since the topic is published with the correct namespace structure and there are no conflicting topics, no additional remappings are required in the launch file.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for topic definitions in the package
rg -t cpp -t python "docking/dock_pose" $(fd . -t f)
Length of output: 17814
Script:
#!/bin/bash
# Search for topic definitions in the dock_pose_publisher node
rg -t cpp "dock_pose" panther_docking/src/dock_pose_publisher_node.cpp -A 5
Length of output: 999
panther_docking/include/panther_docking/panther_charging_dock.hpp (2)
28-28: Ensure proper implementation of LifecycleNode
With the inclusion of <rclcpp_lifecycle/lifecycle_node.hpp>, please verify that the DockPosePublisherNode has been converted to a LifecycleNode and is properly activated within PantherChargingDock, as mentioned in the learnings. Ensure that all lifecycle transitions (configure, activate, deactivate, cleanup) are correctly handled in accordance with ROS 2 best practices.
158-158: Verify proper initialization and usage of dock_pose_sub_
Ensure that the subscription dock_pose_sub_ is correctly initialized in the configure method, and that the callback function for updating the dock pose is properly implemented. Additionally, confirm that the subscription respects the node's lifecycle, activating and deactivating appropriately during state transitions.
Signed-off-by: Jakub Delicat <[email protected]>
|
Please add changes to vcs |
…charging-dock Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
Description
The previous implementation based on tfs didn't work with a real robot. This approach is more similar to
SimpleDockCharger.Requirements
Tests 🧪
Summary by CodeRabbit
Release Notes
New Features
DockPosePublisherNodefor publishing docking pose information.dock_pose_publisher.PantherChargingDockclass to utilize a subscription-based model for pose updates.Documentation
DockPosePublisherNodeand its parameters.Bug Fixes
PantherChargingDockclass for pose detection.panther_charging_dockconfiguration.Chores
xacroin the package configuration.