ROS2 docking nav2 #443
ROS2 docking nav2 #443
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]>
Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
…e publisher Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
…charging-dock Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
Signed-off-by: Jakub Delicat <[email protected]>
…| Spawning multiple stations 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 several modifications across multiple files in the Changes
Possibly related PRs
Suggested reviewers
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
|
Signed-off-by: Jakub Delicat <[email protected]>
|
@coderabbitai can you review? |
|
@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: 11
🧹 Outside diff range and nitpick comments (18)
panther_docking/src/dock_pose_publisher_main.cpp (1)
27-28: Consider making the node name configurable.The node name is currently hardcoded. Consider making it configurable through command-line arguments or ROS parameters to improve reusability and flexibility.
+ const char * node_name = argc > 1 ? argv[1] : "dock_pose_publisher_node"; auto dock_pose_publisher_node = - std::make_shared<panther_docking::DockPosePublisherNode>("dock_pose_publisher_node"); + std::make_shared<panther_docking::DockPosePublisherNode>(node_name);panther_docking/package.xml (1)
31-31: Document the environment variable requirement.Consider adding documentation about the
HUSARION_ROS_BUILD_TYPEenvironment variable:
- Its purpose
- Expected values ("simulation" and others)
- Where and how it should be set
Add this information to the package's README or documentation.
panther_docking/include/panther_docking/dock_pose_publisher_node.hpp (2)
28-31: Add documentation for the minimal detection distance constant.Please add a brief comment explaining the purpose and rationale for the 1.0-meter minimal detection distance. This will help future maintainers understand the significance of this value.
namespace panther_docking { +// Minimum distance in meters at which the dock should be detected. +// Values below this threshold may indicate unreliable detection. constexpr double kMinimalDetectionDistance = 1.0;
37-48: Add documentation for private members and consider exposing timer period.The private members would benefit from documentation, especially the timer period which affects the publishing frequency.
private: + /// @brief Publishes the pose of the closest detected dock void publishPose(); + /// @brief Timeout for TF lookups double timeout_; + /// @brief Frame ID where the dock pose will be published std::string target_frame_; + /// @brief List of source frames to look for dock tags std::vector<std::string> source_frames_; + /// @brief Robot's base frame std::string base_frame_; rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_; std::shared_ptr<tf2_ros::TransformListener> tf_listener_; std::unique_ptr<tf2_ros::Buffer> tf_buffer_; + /// @brief Timer for periodic pose publishing rclcpp::TimerBase::SharedPtr timer_;Consider making the timer period configurable through a parameter:
/// @brief Timer period for pose publishing (in seconds) double publish_period_;panther_docking/CMakeLists.txt (1)
Line range hint
31-34: Address the TODO comment regarding library linking.The TODO comment indicates uncertainty about linking a library that isn't a package. Consider using
find_library()to locate the pose_filter library instead of hardcoding its path:-# TODO @delihus how to link the library what is not a name of a package -target_link_libraries(panther_charging_dock - /opt/ros/humble/lib/libpose_filter.so) +find_library(POSE_FILTER_LIB pose_filter + PATHS /opt/ros/humble/lib + NO_DEFAULT_PATH + REQUIRED) +target_link_libraries(panther_charging_dock ${POSE_FILTER_LIB})This approach is more robust as it:
- Validates library existence at configure time
- Follows CMake best practices
- Makes the build more portable
panther_docking/README.md (2)
56-58: Enhance pose parameter documentationThe parameters are well-documented, but consider adding more detail about the pose parameter format:
- Specify if the pose values are [x, y, yaw] or [x, y, z]
- Indicate the units (meters, radians)
- Consider adding an example value
Example enhancement:
-- `<dock_name>.pose` [*list*, default: **[0.0, 0.0, 0.0]**]: A pose of a dock on the map. If the simulation is used a dock is spawned in this pose. ++ `<dock_name>.pose` [*list*, default: **[0.0, 0.0, 0.0]**]: A pose of a dock on the map [x, y, yaw] in meters and radians. If the simulation is used, a dock is spawned in this pose. Example: [1.0, -0.5, 3.14]🧰 Tools
🪛 LanguageTool
[uncategorized] ~58-~58: A comma might be missing here.
Context: ...a dock on the map. If the simulation is used a dock is spawned in this pose.(AI_EN_LECTOR_MISSING_PUNCTUATION_COMMA)
58-58: Fix grammar in parameter descriptionAdd a comma after the conditional clause for better readability.
-- `<dock_name>.pose` [*list*, default: **[0.0, 0.0, 0.0]**]: A pose of a dock on the map. If the simulation is used a dock is spawned in this pose. ++ `<dock_name>.pose` [*list*, default: **[0.0, 0.0, 0.0]**]: A pose of a dock on the map. If the simulation is used, a dock is spawned in this pose.🧰 Tools
🪛 LanguageTool
[uncategorized] ~58-~58: A comma might be missing here.
Context: ...a dock on the map. If the simulation is used a dock is spawned in this pose.(AI_EN_LECTOR_MISSING_PUNCTUATION_COMMA)
panther_gazebo/README.md (2)
17-17: Consider expanding the configuration file documentation.While the basic description is provided, consider adding more details about the key parameters available in
panther_docking_server.yaml, such as:
- The docking poses configuration structure
- Available parameters (e.g., external_detection_timeout, docking_yaw_threshold)
- The distinction between main and backup docks
- Required frame settings
This would help users better understand and configure the docking functionality.
-- [`panther_docking_server.yaml`](../panther_docking/config/panther_docking_server.yaml): Defines poses for charging docks. +- [`panther_docking_server.yaml`](../panther_docking/config/panther_docking_server.yaml): Defines configuration for charging docks, including: + - Dock poses and frame settings + - Detection timeout and yaw threshold parameters + - Support for multiple docks (main/backup)🧰 Tools
🪛 LanguageTool
[uncategorized] ~17-~17: Loose punctuation mark.
Context: ...king/config/panther_docking_server.yaml): Defines poses for charging docks. ## R...(UNLIKELY_OPENING_PUNCTUATION)
17-17: Fix punctuation: Remove extra space after the period.There's an extra space after the period at the end of the line.
-- [`panther_docking_server.yaml`](../panther_docking/config/panther_docking_server.yaml): Defines poses for charging docks. +- [`panther_docking_server.yaml`](../panther_docking/config/panther_docking_server.yaml): Defines poses for charging docks.🧰 Tools
🪛 LanguageTool
[uncategorized] ~17-~17: Loose punctuation mark.
Context: ...king/config/panther_docking_server.yaml): Defines poses for charging docks. ## R...(UNLIKELY_OPENING_PUNCTUATION)
panther_gazebo/launch/spawn_charging_docks.launch.py (1)
58-81: Document magic numbers and improve node namingThe spawn configuration contains several hardcoded values that need documentation and could be made configurable.
Consider these improvements:
- Extract magic numbers as named constants with documentation:
# Offset between world and map coordinates WORLD_MAP_Y_OFFSET = -2.0 # Height of the charging station from ground STATION_Z_HEIGHT = 0.2 # Roll angle for station orientation (π/2) STATION_ROLL = 1.57
- Use string formatting for node names:
- [dock_name, "_station"], + f"{dock_name}_station",
- Add validation for pose values:
if not all(isinstance(x, (int, float)) for x in pose): print(f"Invalid pose values for dock {dock_name}: {pose}") continuepanther_manager/launch/manager.launch.py (1)
Line range hint
90-101: Missing launch argument declaration foruse_dockingThe
use_dockingconfiguration is used but not properly declared. Add aDeclareLaunchArgumentfor it similar to other launch arguments.Add this before the
use_dockingconfiguration:+ declare_use_docking_arg = DeclareLaunchArgument( + "use_docking", + default_value="False", + description="Whether to enable the docking manager functionality", + ) use_docking = LaunchConfiguration("use_docking")Also add
declare_use_docking_argto theactionslist:actions = [ declare_docking_bt_project_path_arg, declare_lights_bt_project_path_arg, declare_safety_bt_project_path_arg, declare_namespace_arg, declare_shutdown_hosts_config_path_arg, declare_use_sim_arg, + declare_use_docking_arg, docking_manager_node, lights_manager_node, safety_manager_node, ]panther_docking/include/panther_docking/panther_charging_dock.hpp (1)
Line range hint
1-200: Consider enhancing frame transformation documentation.The class uses multiple frame-related variables (
base_frame_name_,fixed_frame_name_,dock_frame_). Consider adding documentation that explains:
- The relationship between these frames
- The expected transformation chain
- Any assumptions about frame hierarchies
This would help future maintainers understand the pose transformation flow, especially in the context of nav2 integration.
Add documentation like this to the class description:
/** * @class PantherChargingDock * @brief A class to represent a Panther charging dock. + * + * Frame Transformations: + * - base_frame_name_: Robot's base frame (typically 'base_link') + * - fixed_frame_name_: World-fixed frame (typically 'map' or 'odom') + * - dock_frame_: Frame attached to the charging dock + * + * The transformation chain is: + * fixed_frame -> dock_frame -> staging_pose */panther_docking/src/panther_charging_dock.cpp (3)
162-163: LGTM: Improved logging with detailed timing informationThe enhanced logging provides better visibility into timeout issues by including both the actual duration and timeout threshold.
Consider using
RCLCPP_DEBUG_STREAMfor this message when detection timeout is expected behavior, or keepRCLCPP_WARN_STREAMif it indicates a potential issue.
232-246: Consider splitting the method for better maintainabilityWhile the implementation is correct, the method handles multiple responsibilities:
- Logging
- Pose transformation
- Publishing
Consider refactoring into smaller, focused methods for better maintainability and testability.
+ void logDockPose() { + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + RCLCPP_DEBUG_STREAM( + logger_, "Dock pose x: " << dock_pose_.pose.position.x + << " y: " << dock_pose_.pose.position.y + << " yaw: " << yaw); + } + + PoseStampedMsg calculateStagingPose(const std::string & frame) { + PoseStampedMsg staging_pose = dock_pose_; + const double yaw = tf2::getYaw(dock_pose_.pose.orientation); + + staging_pose.header.frame_id = frame; + staging_pose.header.stamp = node_.lock()->now(); + staging_pose.pose.position.x += std::cos(yaw) * staging_x_offset_; + staging_pose.pose.position.y += std::sin(yaw) * staging_x_offset_; + + tf2::Quaternion orientation; + orientation.setRPY(0.0, 0.0, yaw); + staging_pose.pose.orientation = tf2::toMsg(orientation); + + return staging_pose; + } + void updateAndPublishStagingPose(const std::string & frame) { - const double yaw = tf2::getYaw(dock_pose_.pose.orientation); - RCLCPP_DEBUG_STREAM( - logger_, "Dock pose x: " << dock_pose_.pose.position.x - << " y: " << dock_pose_.pose.position.y - << " yaw: " << yaw); - - staging_pose_ = dock_pose_; - staging_pose_.header.frame_id = frame; - staging_pose_.header.stamp = node_.lock()->now(); - staging_pose_.pose.position.x += std::cos(yaw) * staging_x_offset_; - staging_pose_.pose.position.y += std::sin(yaw) * staging_x_offset_; - tf2::Quaternion orientation; - orientation.setRPY(0.0, 0.0, yaw); - staging_pose_.pose.orientation = tf2::toMsg(orientation); + logDockPose(); + staging_pose_ = calculateStagingPose(frame); staging_pose_pub_->publish(staging_pose_); }
Line range hint
1-264: Consider enhancing test coverageThe implementation looks solid, but consider adding the following types of tests:
- Unit tests for the pose calculation logic
- Integration tests with nav2
- Parameter validation tests
- Mock tests for TF2 transformations
This will ensure reliability as the docking functionality evolves.
Would you like me to help create a test suite template for this implementation?
panther_docking/launch/station.launch.py (3)
74-78: Duplicate assignment ofapriltag_sizevariableThe variable
apriltag_sizeis assigned fromLaunchConfiguration("apriltag_size").perform(context)twice at lines 74 and 78. This is redundant and the second assignment can be removed.Apply this diff to remove the redundant assignment:
74 apriltag_size = LaunchConfiguration("apriltag_size").perform(context) 75 use_docking = LaunchConfiguration("use_docking").perform(context) 77 docking_server_config_path = LaunchConfiguration("docking_server_config_path").perform(context) -78 apriltag_size = LaunchConfiguration("apriltag_size").perform(context)
Line range hint
111-139: Missing declaration fordocking_server_config_pathlaunch argumentThe
docking_server_config_pathlaunch argument is used in thelaunch_stations_descriptionsfunction but is not declared ingenerate_launch_description(). This will cause aKeyErrorwhen trying to access this configuration. To fix this, declare the launch argument.Apply this diff to declare the missing launch argument:
def generate_launch_description(): declare_use_docking_arg = DeclareLaunchArgument( "use_docking", default_value="True", description="Enable docking server.", choices=["True", "False", "true", "false"], ) declare_apriltag_id = DeclareLaunchArgument( "apriltag_id", default_value="0", description="ID of a generated apriltag on the station", ) declare_apriltag_size = DeclareLaunchArgument( "apriltag_size", default_value="0.06", description="Size in meters of a generated apriltag on the station", ) + declare_docking_server_config_path = DeclareLaunchArgument( + "docking_server_config_path", + default_value="None", + description="Path to the docking server configuration file", + ) return LaunchDescription( [ declare_use_docking_arg, declare_apriltag_id, declare_apriltag_size, + declare_docking_server_config_path, OpaqueFunction(function=launch_stations_descriptions), ] )
73-91: Avoid reusing variable nameapriltag_idto prevent confusionThe variable
apriltag_idis first assigned from the launch configuration at line 73, and then reassigned inside the loop at line 91 with a potentially different value. Reusing the variable name can cause confusion and unintended bugs. Consider using a different variable name for theapriltag_idinside the loop.Apply this change to use a different variable name inside the loop:
73 apriltag_id_global = int(LaunchConfiguration("apriltag_id").perform(context)) 74 apriltag_size = LaunchConfiguration("apriltag_size").perform(context) 75 use_docking = LaunchConfiguration("use_docking").perform(context) 76 77 docking_server_config_path = LaunchConfiguration("docking_server_config_path").perform(context) 78 # Removed redundant assignment of apriltag_size 79 80 docking_server_config = None 81 if docking_server_config_path == "None": 82 return [] 83 84 # ... (rest of the code) 90 for dock_name in docks_names: 91 dock_apriltag_id = ros_parameters[dock_name]["apriltag_id"] 92 station_description_content = generate_urdf(dock_name, dock_apriltag_id, apriltag_size)
📜 Review details
Configuration used: .coderabbit.yaml
Review profile: CHILL
📒 Files selected for processing (17)
panther_description/config/docking_components.yaml(1 hunks)panther_docking/CMakeLists.txt(1 hunks)panther_docking/README.md(1 hunks)panther_docking/config/apriltag.yaml(1 hunks)panther_docking/config/panther_docking_server.yaml(1 hunks)panther_docking/include/panther_docking/dock_pose_publisher_node.hpp(1 hunks)panther_docking/include/panther_docking/panther_charging_dock.hpp(1 hunks)panther_docking/launch/docking.launch.py(3 hunks)panther_docking/launch/station.launch.py(4 hunks)panther_docking/package.xml(1 hunks)panther_docking/src/dock_pose_publisher_main.cpp(1 hunks)panther_docking/src/dock_pose_publisher_node.cpp(2 hunks)panther_docking/src/panther_charging_dock.cpp(3 hunks)panther_gazebo/README.md(1 hunks)panther_gazebo/launch/simulation.launch.py(3 hunks)panther_gazebo/launch/spawn_charging_docks.launch.py(1 hunks)panther_manager/launch/manager.launch.py(2 hunks)
✅ Files skipped from review due to trivial changes (1)
- panther_docking/config/apriltag.yaml
🧰 Additional context used
📓 Learnings (5)
panther_docking/include/panther_docking/dock_pose_publisher_node.hpp (3)
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:38-47
Timestamp: 2024-11-12T09:25:48.706Z
Learning: In `panther_docking/src/dock_pose_publisher_node.cpp`, `DockPosePublisherNode` will be updated to handle multiple docks in future versions.
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:51-58
Timestamp: 2024-11-12T09:25:48.706Z
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-11-12T09:25:48.706Z
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.
panther_docking/include/panther_docking/panther_charging_dock.hpp (1)
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/panther_charging_dock.cpp:114-114
Timestamp: 2024-11-12T09:25:48.706Z
Learning: In `panther_docking/src/panther_charging_dock.cpp`, the direct comparison of `geometry_msgs::msg::Pose` in the `getStagingPose` method will be updated in a future version when testing with nav2.
panther_docking/src/dock_pose_publisher_main.cpp (1)
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:38-47
Timestamp: 2024-11-12T09:25:48.706Z
Learning: In `panther_docking/src/dock_pose_publisher_node.cpp`, `DockPosePublisherNode` will be updated to handle multiple docks in future versions.
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:38-47
Timestamp: 2024-11-12T09:25:48.706Z
Learning: In `panther_docking/src/dock_pose_publisher_node.cpp`, `DockPosePublisherNode` will be updated to handle multiple docks in future versions.
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:51-58
Timestamp: 2024-11-12T09:25:48.706Z
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-11-12T09:25:48.706Z
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.
panther_docking/src/panther_charging_dock.cpp (3)
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/panther_charging_dock.cpp:114-114
Timestamp: 2024-11-12T09:25:48.706Z
Learning: In `panther_docking/src/panther_charging_dock.cpp`, the direct comparison of `geometry_msgs::msg::Pose` in the `getStagingPose` method will be updated in a future version when testing with nav2.
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:38-47
Timestamp: 2024-11-12T09:25:48.706Z
Learning: In `panther_docking/src/dock_pose_publisher_node.cpp`, `DockPosePublisherNode` will be updated to handle multiple docks in future versions.
Learnt from: delihus
PR: husarion/panther_ros#431
File: panther_docking/src/dock_pose_publisher_node.cpp:51-58
Timestamp: 2024-11-12T09:25:48.706Z
Learning: The `DockPosePublisherNode` should be converted to a `LifecycleNode` and activated in `PantherChargingDock`.
🪛 LanguageTool
panther_docking/README.md
[uncategorized] ~58-~58: A comma might be missing here.
Context: ...a dock on the map. If the simulation is used a dock is spawned in this pose.
(AI_EN_LECTOR_MISSING_PUNCTUATION_COMMA)
panther_gazebo/README.md
[uncategorized] ~17-~17: Loose punctuation mark.
Context: ...king/config/panther_docking_server.yaml): Defines poses for charging docks. ## R...
(UNLIKELY_OPENING_PUNCTUATION)
🔇 Additional comments (25)
panther_description/config/docking_components.yaml (2)
20-20: Verify main_lidar namespace usage
The component is configured to use the main_lidar namespace. Please ensure this aligns with the navigation stack's expectations and doesn't conflict with other components.
Let's check for potential namespace conflicts:
✅ Verification successful
main_lidar namespace is consistently used and appears to be the standard
The verification shows that main_lidar namespace is used consistently in the configuration files:
- It's defined in
docking_components.yaml - It's referenced (though commented) in the main
components.yaml
There are no conflicting namespace definitions or unexpected usage patterns found in the codebase, indicating this is the standard namespace for the main lidar sensor.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Search for main_lidar namespace usage across the codebase
# to verify proper integration
# Check for main_lidar namespace in launch files
rg -A 5 "main_lidar" --type xml
# Check for main_lidar namespace in configuration files
rg -A 5 "main_lidar" --type yaml
# Check for main_lidar references in source code
ast-grep --pattern 'main_lidar'
Length of output: 659
18-19: Verify the component placement coordinates
The LDR06 component is configured with zero offset position and orientation relative to cover_link. Please verify if this placement is intentional and correct for proper sensor operation.
Let's check other component configurations and related launch files:
✅ Verification successful
Zero offset for LDR06 appears to be correct
Based on the findings:
- Found a commented reference in
components.yamlshowing similar zero orientation (rpy) but with a small y-offset (-0.1) - The
cover_linkis mounted at a fixed height (z: 0.1715) above thebody_link, providing an elevated base position - Zero offset relative to
cover_linkis reasonable as it allows the LDR06 to be mounted directly on the cover surface
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Search for other LDR06 configurations and cover_link references
# to verify typical mounting positions
# Check for other LDR06 configurations
rg -A 5 "type:\s*LDR06" --type yaml
# Check for cover_link references in launch files and URDF
rg -A 5 "cover_link" --type xml
Length of output: 837
Script:
#!/bin/bash
# Let's check the URDF files for cover_link definition and its position
fd -e urdf -e xacro | xargs rg -A 5 "link.*cover_link|<joint.*cover"
# Also check for any documentation about sensor placement
fd "README|INSTALL" | xargs rg -i "ldr06|lidar|sensor.*placement"
Length of output: 796
panther_docking/src/dock_pose_publisher_main.cpp (1)
1-21: LGTM! Headers are well-organized and license is properly included.
The includes are properly organized (standard libraries, then ROS2, then local headers) and all are necessary for the implementation.
panther_docking/config/panther_docking_server.yaml (4)
21-22: Verify docking approach parameters against physical constraints
The changes to docking parameters suggest a more conservative approach:
- Increased yaw threshold (0.15 → 0.2 rad ≈ 8.6° → 11.5°)
- Larger staging distance (-0.5m → -0.8m)
Please ensure these values:
- Allow sufficient clearance for the robot's turning radius
- Work with the available space around the charging stations
Let's check for any related parameters that might need adjustment:
✅ Verification successful
Docking parameters are within established defaults and constraints
The changes to the docking parameters in panther_docking_server.yaml are actually more conservative than the default values defined in the codebase:
- The new yaw threshold (0.2 rad) is stricter than the default (0.3 rad)
- The staging offset (-0.8m) is only slightly larger than the default (-0.7m)
These values maintain a good balance between safety and functionality:
- The reduced yaw threshold (compared to default) ensures more precise alignment
- The staging distance is within reasonable bounds for approach maneuvers
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for related docking parameters and their usage
rg "docking_yaw|staging_x"
Length of output: 1751
35-40: Validate backup dock configuration
The backup dock configuration mirrors the main dock structure but with different pose and AprilTag ID. Consider:
- Ensuring sufficient separation between docks to avoid confusion
- Validating that both AprilTag IDs (0 and 1) are unique in the environment
Let's check the AprilTag configuration:
✅ Verification successful
Configuration validated: Docks are well-separated with unique AprilTag IDs
The verification confirms:
- Main dock at [1.0, 1.5, 1.57] with AprilTag ID 0
- Backup dock at [-1.0, 1.5, 1.57] with AprilTag ID 1
- 2-meter separation on x-axis provides sufficient clearance
- AprilTag IDs are unique in the environment (0 and 1)
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for AprilTag configurations
rg "apriltag.*id.*[01]"
Length of output: 271
Script:
#!/bin/bash
# Let's check the dock poses to validate separation
rg "pose:.*\[.*\]" panther_docking/config/panther_docking_server.yaml -A 1
Length of output: 345
19-19: Verify impact of increased detection timeout
The external detection timeout has been doubled from 0.1s to 0.2s. While this might improve reliability by allowing more time for detection, it could also impact the system's responsiveness.
Let's check if there are any related timing parameters or dependencies in the codebase:
✅ Verification successful
External detection timeout change is safe and well-documented
The increased timeout (0.2s) is actually the documented default value in the README and is used for looking up transformations from april tags to base frame. The code shows proper error handling with clear warning messages when the timeout is exceeded. The change aligns with:
- Default value in documentation (
README.md) - Implementation in
PantherChargingDockthat uses it for transformation lookups - Test cases that work with even higher timeouts (0.5s)
- Usage in
DockPosePublisherthat actually divides it by 10 (timeout_ = external_detection_timeout * 0.1)
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for references to external_detection_timeout to understand its usage
rg -A 3 "external_detection_timeout"
Length of output: 4686
27-34: Verify frame transformations and main dock pose
The main dock configuration now uses the map frame and includes AprilTag detection. Please ensure:
- The transformation chain from map → dock_frame is properly defined
- The pose [1.0, 1.5, 1.57] is validated in the actual environment
Let's check the frame definitions:
✅ Verification successful
Frame transformations are properly handled in the codebase
The code implements proper frame transformation handling through:
- A TF2 buffer and listener setup in the DockPosePublisherNode
- Transform lookups between map frame and dock frames with error handling
- Proper inclusion of TF2 dependencies and geometry message types
The pose validation in the actual environment remains a manual task that cannot be verified through code analysis.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for frame definitions and transformations
rg "tf2?_static_publisher|TransformBroadcaster.*map.*main_wibotic"
Length of output: 68
Script:
#!/bin/bash
# Let's search for frame transformations more broadly
rg -i "transform.*main_wibotic|frame.*main_wibotic" -A 3
# Also search for any TF-related code
rg "tf2?_static_publisher|TransformBroadcaster" -A 3
# Look for launch files that might configure transforms
fd -e launch | xargs rg "tf.*static.*publisher|transform.*static"
Length of output: 1657
Script:
#!/bin/bash
# Let's check the dock pose publisher implementation
ast-grep --pattern 'class DockPosePublisher {
$$$
}'
# Also look for any TF-related code in the dock pose publisher
rg "tf2" panther_docking/src/dock_pose_publisher_node.cpp -A 3
# Check for any transform listeners or broadcasters
rg "transform_broadcaster|transform_listener" -A 3
Length of output: 2157
panther_docking/package.xml (1)
31-31: LGTM! Verify the environment variable handling.
The conditional dependency on apriltag_ros for simulation builds is correctly implemented. However, let's verify the environment variable handling.
✅ Verification successful
Conditional dependency on apriltag_ros is correctly implemented
The environment variable HUSARION_ROS_BUILD_TYPE is well-documented and consistently used across the codebase:
- It's documented in the main README.md with clear instructions for both
hardwareandsimulationvalues - Other packages in the repository (panther, panther_gazebo, panther_bringup, panther_controller) use the same conditional pattern
- The condition aligns with the repository's dual-purpose design for both real hardware and simulation environments
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Verify the environment variable usage and related files
# Check if the environment variable is set in any launch files
echo "Checking launch files for HUSARION_ROS_BUILD_TYPE..."
rg -l "HUSARION_ROS_BUILD_TYPE"
# Check if there's documentation about this environment variable
echo "Checking documentation for environment variable..."
fd -t f "README|\.md$" | xargs rg "HUSARION_ROS_BUILD_TYPE"
Length of output: 820
Script:
#!/bin/bash
# Check the content of panther_*.repos files to understand the dependency management
echo "Checking .repos files content..."
fd -e repos | xargs cat
# Check other package.xml files to see how they use the condition
echo "Checking other package.xml files for similar conditions..."
cat panther/package.xml panther_gazebo/package.xml panther_bringup/package.xml panther_controller/package.xml
Length of output: 10193
panther_docking/include/panther_docking/dock_pose_publisher_node.hpp (2)
1-27: LGTM! Well-structured header with appropriate includes.
The file structure follows best practices with proper copyright notice, header guards, and minimal necessary includes.
32-48: Verify timer period and parameter declarations in the implementation.
Let's check how the timer period is configured and what parameters are declared in the implementation.
✅ Verification successful
Timer and parameters are properly configured in the implementation
The implementation shows:
- Timer is created with a configurable publish rate parameter
- All necessary parameters are declared:
publish_rate(default: 10.0)docks(default: ["main"])fixed_frame(default: "odom")base_frame(default: "base_link")- Per-dock parameters for type and frame
The timer period is correctly derived from the publish rate parameter, and all member variables declared in the header are properly initialized through parameters.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Search for timer creation and parameter declarations
echo "Searching for timer creation..."
rg -A 5 "create_wall_timer|create_timer" panther_docking/src/
echo "Searching for parameter declarations..."
rg -A 5 "declare_parameter" panther_docking/src/
Length of output: 5102
panther_docking/CMakeLists.txt (2)
64-65: LGTM! Proper separation of node implementation from main function.
The addition of a separate main file (dock_pose_publisher_main.cpp) follows ROS2 best practices by separating the node implementation from the main function, improving code organization and testability.
64-65: Verify the existence and structure of the source files.
Let's ensure both source files exist and follow the expected ROS2 node structure.
✅ Verification successful
Source files exist and follow proper ROS2 structure
The files are properly structured and contain all expected components:
- Main file implements correct ROS2 node initialization, spinning, and shutdown
- Node class properly inherits from
rclcpp::Nodeand implements required functionality - Header file declares the interface with appropriate includes and class structure
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Verify source files exist and follow ROS2 node patterns
# Check if files exist
echo "Checking source files existence:"
ls -l src/dock_pose_publisher_node.cpp src/dock_pose_publisher_main.cpp || echo "Files not found"
# Check main file structure
echo -e "\nChecking main file structure:"
rg "int main.*rclcpp::init.*rclcpp::spin" src/dock_pose_publisher_main.cpp || echo "Main function pattern not found"
# Check node implementation
echo -e "\nChecking node implementation:"
ast-grep --pattern 'class $_ : public rclcpp::Node' src/dock_pose_publisher_node.cpp || echo "Node class pattern not found"
Length of output: 889
Script:
#!/bin/bash
# Let's check if these files exist in a different location
echo "Searching for dock_pose_publisher files:"
fd "dock_pose_publisher.*\.(cpp|hpp)$"
echo -e "\nChecking the directory structure:"
ls -R panther_docking/
Length of output: 963
Script:
#!/bin/bash
# Let's verify the content of these files
echo "Checking main file structure:"
rg "int main.*rclcpp::init.*rclcpp::spin" panther_docking/src/dock_pose_publisher_main.cpp
echo -e "\nChecking node implementation:"
ast-grep --pattern 'class $_ : public rclcpp::Node' panther_docking/src/dock_pose_publisher_node.cpp
echo -e "\nChecking node header:"
cat panther_docking/include/panther_docking/dock_pose_publisher_node.hpp
Length of output: 2094
Script:
#!/bin/bash
# Let's check the actual implementation files content
echo "Checking main file content:"
cat panther_docking/src/dock_pose_publisher_main.cpp
echo -e "\nChecking node implementation content:"
cat panther_docking/src/dock_pose_publisher_node.cpp
Length of output: 6215
panther_gazebo/launch/simulation.launch.py (3)
19-19: LGTM: Import addition is appropriate
The addition of IfCondition import is correctly placed and necessary for the new conditional docking functionality.
94-94: LGTM: Proper addition to launch sequence
The spawn_charging_docks is correctly added to the launch sequence after the main simulation setup.
77-84: Verify the existence of spawn_charging_docks.launch.py
The launch description is properly implemented following ROS2 best practices. Let's verify the existence of the referenced launch file.
✅ Verification successful
Launch file exists at the expected location
The spawn_charging_docks.launch.py file is present at the correct location in the panther_gazebo/launch directory, confirming that the launch description's path reference is valid.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Verify the existence of spawn_charging_docks.launch.py
# Check if the launch file exists
fd -t f "spawn_charging_docks.launch.py" --exec echo "Launch file found: {}"
Length of output: 150
panther_gazebo/launch/spawn_charging_docks.launch.py (3)
1-27: LGTM! Well-organized imports and proper license header.
The imports are logically grouped and all necessary dependencies are included.
88-109: LGTM! Well-structured launch configuration.
The launch description is well-organized with clear argument declarations and proper path handling.
1-109: Verify ROS2 naming conventions and launch file structure
Let's ensure the implementation follows ROS2 best practices.
✅ Verification successful
Based on the analysis of the shell script outputs, I can now generate the final response regarding ROS2 naming conventions and launch file structure.
Node naming and launch file structure follows ROS2 best practices
The implementation adheres to ROS2 best practices:
- Proper use of namespacing with
LaunchConfiguration("device_namespace") - Consistent node naming pattern using descriptive names (e.g.,
[dock_name, "_station"]) - Correct parameter handling through
DeclareLaunchArgumentandLaunchConfiguration - Launch file structure follows ROS2 conventions with proper imports and launch description generation
- Node spawning uses appropriate conditions and parameter passing
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Check for ROS2 launch file patterns and naming conventions
# Check for other launch files in the package for consistency
echo "Checking launch file patterns..."
fd -e launch.py -x cat {} \; | grep -E "Node|LaunchDescription|DeclareLaunchArgument"
# Check for parameter files referenced in launch files
echo "Checking parameter file references..."
rg -l "config.*yaml" --type py
# Check for node naming patterns
echo "Checking node naming patterns..."
rg "Node\(" -A 5
Length of output: 51832
panther_manager/launch/manager.launch.py (1)
Line range hint 90-101: Verify docking manager configuration file exists
The node configuration references a YAML file at config/docking_manager.yaml. Let's verify this file exists and contains the necessary parameters.
✅ Verification successful
Configuration file exists and contains valid parameters
The verification confirms that docking_manager.yaml exists at the expected location panther_manager/config/docking_manager.yaml and contains proper ROS2 parameter configuration including timer frequency, BT server port, timeouts, and required plugin libraries.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Check if the docking manager config file exists
fd -t f "docking_manager.yaml" -x echo "Found config file: {}"
# Check the content of the config file if found
fd -t f "docking_manager.yaml" -x cat {}
Length of output: 466
panther_docking/launch/docking.launch.py (2)
171-182: LGTM! Launch description components are properly ordered.
The launch description maintains the correct ordering with declarations before node instances, and all components are properly included.
46-53: LGTM! Verify the existence of the default config file.
The AprilTag configuration argument is well-structured and follows ROS2 launch file best practices.
Let's verify the existence of the default config file:
✅ Verification successful
✓ Default AprilTag configuration file exists and is properly structured
The default configuration file exists at the correct location (panther_docking/config/apriltag.yaml) and contains valid YAML with proper AprilTag parameters. The file in panther_gazebo appears to be a simulation-specific variant, which aligns with the launch argument's description about simulation usage. Both files follow the same structure but with appropriate differences for their respective contexts.
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Verify the existence of the default AprilTag config file
# Find the package share directory and check for the config file
fd -t f "apriltag.yaml" -x echo "Found config file: {}"
Length of output: 170
Script:
#!/bin/bash
# Let's verify the content of both config files to ensure they are valid YAML
# and understand their relationship
echo "=== Content of panther_docking/config/apriltag.yaml ==="
cat ./panther_docking/config/apriltag.yaml
echo -e "\n=== Content of panther_gazebo/config/apriltag.yaml ==="
cat ./panther_gazebo/config/apriltag.yaml
Length of output: 1053
panther_docking/include/panther_docking/panther_charging_dock.hpp (1)
152-155: LGTM! The frame parameter addition improves flexibility.
The addition of the frame parameter to updateAndPublishStagingPose aligns well with the nav2 compatibility goals and provides better control over pose transformations.
Let's verify that all callers of this method have been updated with the new parameter:
✅ Verification successful
All callers have been updated with the new frame parameter
The verification shows that all calls to updateAndPublishStagingPose are passing the required frame parameter:
- In
panther_charging_dock.cpp:139: Called withframeparameter - In
panther_charging_dock.cpp:168: Called withfixed_frame_name_parameter
🏁 Scripts executed
The following scripts were executed for the analysis:
Script:
#!/bin/bash
# Description: Find all calls to updateAndPublishStagingPose to ensure they've been updated
# with the new frame parameter
# Search for method calls in the codebase
ast-grep --pattern 'updateAndPublishStagingPose($$$)'
# Also search for the string in case it appears in comments or strings
rg "updateAndPublishStagingPose"
Length of output: 712
panther_docking/src/panther_charging_dock.cpp (2)
139-140: LGTM: Proper staging pose update with frame propagation
The staging pose is correctly updated with the provided frame parameter.
168-168: LGTM: Proper frame handling for staging pose
The staging pose is correctly updated using the fixed frame, which aligns with ROS2 TF2 best practices.
panther_docking/src/dock_pose_publisher_node.cpp (1)
43-43: Potential unintended scaling of timeout_ value
In line 43, the timeout parameter is being multiplied by 0.1, which reduces its value by a factor of 10:
timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double() * 0.1;Ensure that this scaling is intentional. Multiplying by 0.1 may lead to a very short timeout, potentially causing frequent transform lookup failures.
If the scaling is not intended, consider adjusting the code as follows:
-timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double() * 0.1;
+timeout_ = get_parameter("panther_charging_dock.external_detection_timeout").as_double();Signed-off-by: Jakub Delicat <[email protected]>
Description
Requirements
Tests 🧪
Summary by CodeRabbit
Release Notes
New Features
Improvements
Bug Fixes
Documentation