ROS2 docking utils

[delihus]

Description

These changes were made in a docking project development and reviewed to ros2-docking branch.

• BT plugins,

Requirements

• Code style guidelines followed
• Documentation updated

Tests 🧪

• Robot
• Container
• Simulation

Summary by CodeRabbit

Release Notes

• New Features

  • Introduced new parameters bt_server_port in both lights_manager and safety_manager configurations.
  • Added new plugins: CheckBoolMsg and CheckJoyMsg for handling boolean and joystick messages, respectively.
  • Enhanced PluginTestUtils to support action servers.

• Bug Fixes

  • Updated descriptions for various input ports across multiple service nodes for better clarity.

• Tests

  • Added unit tests for CheckBoolMsg and CheckJoyMsg plugins to validate their functionality.
  • Enhanced test coverage for the convertFromString function in behavior tree utilities.

• Chores

  • Updated dependencies in package.xml to include geometry_msgs, sensor_msgs, and tf2_geometry_msgs.

[coderabbitai]

Actionable comments posted: 17

🧹 Outside diff range and nitpick comments (41)

husarion_ugv_utils/CMakeLists.txt (1)

75-76: Consider improving formatting for better readability

The ament_target_dependencies command spans two lines with inconsistent indentation. Consider reformatting for better readability:

-  ament_target_dependencies(${PROJECT_NAME}_test_tf2_utils std_msgs tf2_ros
-                            tf2_geometry_msgs)
+  ament_target_dependencies(${PROJECT_NAME}_test_tf2_utils
+    std_msgs
+    tf2_ros
+    tf2_geometry_msgs
+  )

husarion_ugv_manager/config/lights_manager.yaml (1)

5-5: Document the purpose and requirements of bt_server_port.

While the port number is now configurable, it would be helpful to:

1. Document the purpose of this port (e.g., "Port for behavior tree server communication")
2. Consider potential port conflicts with other services

Add a comment above the parameter:

      timer_frequency: 10.0
+     # Port for behavior tree server communication
      bt_server_port: 5555

husarion_ugv_manager/src/lights_manager_node.cpp (1)

95-99: Consider consolidating timeout conversions.

While the conversion to milliseconds is correct, consider creating a helper function to reduce code duplication, as this pattern appears in both manager nodes.

+ template<typename T>
+ std::chrono::milliseconds ToMilliseconds(T seconds) {
+   return std::chrono::duration_cast<std::chrono::milliseconds>(
+     std::chrono::duration<double>(seconds));
+ }

  BT::RosNodeParams params;
  params.nh = this->shared_from_this();
- auto wait_for_server_timeout_s = std::chrono::duration<double>(service_availability_timeout);
- params.wait_for_server_timeout =
-   std::chrono::duration_cast<std::chrono::milliseconds>(wait_for_server_timeout_s);
- auto server_timeout_s = std::chrono::duration<double>(service_response_timeout);
- params.server_timeout = std::chrono::duration_cast<std::chrono::milliseconds>(server_timeout_s);
+ params.wait_for_server_timeout = ToMilliseconds(service_availability_timeout);
+ params.server_timeout = ToMilliseconds(service_response_timeout);

husarion_ugv_utils/test/test_tf2_utils.cpp (4)

30-70: Add documentation and use named constants for transform values.

While the test fixture is well-structured, consider these improvements for better maintainability:

1. Add class-level documentation explaining the purpose of the fixture and the significance of the transform values
2. Define named constants for the transform values (0.3, 0.2, 0.1) to improve readability and make it easier to update test values

 static constexpr char kBaseFrame[] = "base_link";
 static constexpr char kOdomFrame[] = "odom";
+
+// Transform values for testing
+static constexpr double kTransformX = 0.3;
+static constexpr double kTransformY = 0.2;
+static constexpr double kTransformZ = 0.1;
+
+/**
+ * @brief Test fixture for transform pose operations
+ * 
+ * Sets up a transform between base_link and odom frames with a fixed translation
+ * (kTransformX, kTransformY, kTransformZ) and identity rotation for testing
+ * pose transformation operations.
+ */
 class TestTransformPose : public ::testing::Test

---

72-138: Consider additional test cases for better coverage.

The transform pose tests cover most scenarios well, but consider adding these test cases:

1. Test with non-identity rotations in the transform to verify correct rotation handling
2. Test with negative timeout values to verify proper error handling
3. Test with zero timeout to verify boundary condition

Would you like me to help generate these additional test cases?

🧰 Tools

🪛 cppcheck (2.10-2)

[error] 100-100: There is an unknown macro here somewhere. Configuration is required. If ; is a macro then please configure it.

(unknownMacro)

---

201-276: Add edge cases and invalid value tests.

The pose comparison tests are comprehensive, but consider adding these test cases:

1. Test poses exactly at the distance/angle thresholds
2. Test with NaN or invalid values in the poses
3. Test with zero thresholds

Would you like me to help generate these additional test cases?

---

278-287: Add error handling for ROS 2 initialization.

Consider adding try-catch block to handle potential initialization errors:

 int main(int argc, char ** argv)
 {
-  rclcpp::init(argc, argv);
+  try {
+    rclcpp::init(argc, argv);
+  } catch (const std::exception & e) {
+    std::cerr << "Failed to initialize ROS 2: " << e.what() << std::endl;
+    return 1;
+  }
   testing::InitGoogleTest(&argc, argv);

   auto run_tests = RUN_ALL_TESTS();

   rclcpp::shutdown();
   return run_tests;
 }

husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp (3)

38-43: Correct the log message to reflect exact expected size

The warning message states "expected at least" but the check requires the number of axes to be exactly equal. This could lead to confusion when debugging.

Apply this fix to clarify the expected size in the log message:

 << " axes, expected at least " << expected_axes.size());
+<< " axes, expected exactly " << expected_axes.size());

---

46-49: Use an appropriate tolerance for floating-point comparison

Comparing floating-point numbers using std::numeric_limits<float>::epsilon() may be too strict for practical applications, as it represents a very small value (~1e-7). Consider using a larger tolerance value that reflects the precision of joystick inputs.

For example, you could define a tolerance like 1e-5:

 [](float a, float b) { return std::fabs(a - b) <= std::numeric_limits<float>::epsilon(); });
+[](float a, float b) { return std::fabs(a - b) <= 1e-5; });

Alternatively, make the tolerance a configurable parameter.

---

60-65: Correct the log message to reflect exact expected size

The warning message indicates "expected at least" when it should specify "expected exactly," as the function requires an exact match in the number of buttons.

Apply this fix to clarify the expected size in the log message:

 << " buttons, expected at least " << expected_buttons.size());
+<< " buttons, expected exactly " << expected_buttons.size());

husarion_ugv_utils/include/husarion_ugv_utils/tf2_utils.hpp (2)

15-16: Align the namespace and include guard with the file structure

The include guard PANTHER_UTILS_TF2_UTILS_HPP_ and the namespace panther_utils::tf2_utils do not match the file path husarion_ugv_utils/include/husarion_ugv_utils/tf2_utils.hpp. For consistency and to prevent potential confusion, consider updating the include guard and namespace to align with the directory structure or adjust the file location to match the namespace.

---

60-60: Handle potential exceptions from tf2_buffer->transform

Between the canTransform check and the actual transform, the transform might become unavailable, leading to an exception. To make the code more robust, consider wrapping the tf2_buffer->transform call in a try-catch block to handle any potential tf2::TransformException.

Example:

try {
  tf2_buffer->transform(pose, transformed_pose, target_frame);
} catch (const tf2::TransformException &ex) {
  throw std::runtime_error("Failed to transform pose: " + std::string(ex.what()));
}

husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp (2)

15-16: Update the include guard to match the project naming convention

The include guard PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_ does not reflect the file's directory structure or the project naming convention (husarion_ugv_manager). Consider updating it to align with the project's namespace and file path for consistency and to prevent potential conflicts.

Apply this diff to update the include guard:

-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_UNDOCK_ROBOT_NODE_HPP_

---

27-29: Align the namespace with the project structure

The namespace panther_manager may not align with the directory structure husarion_ugv_manager. To maintain consistency across the codebase, consider updating the namespace to match the project or directory name.

Apply this diff to update the namespace:

-namespace panther_manager
+namespace husarion_ugv_manager
 {

husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp (2)

15-16: Include paths may not match the directory structure

The include statements use paths starting with "panther_manager/", whereas the file resides in husarion_ugv_manager. Ensure that the include paths correctly reflect the actual directory structure to prevent compilation issues.

Apply this diff to update the include paths:

-#include "panther_manager/plugins/action/dock_robot_node.hpp"
-#include "panther_manager/behavior_tree_utils.hpp"
+#include "husarion_ugv_manager/plugins/action/dock_robot_node.hpp"
+#include "husarion_ugv_manager/behavior_tree_utils.hpp"

---

74-79: Override 'onFailure' method without 'virtual' specifier

The onFailure method is already declared as override, so the virtual specifier is unnecessary. Removing it enhances code clarity and adheres to best practices.

Apply this diff to remove the virtual keyword:

 BT::NodeStatus DockRobot::onFailure(BT::ActionNodeErrorCode error)
 {
   RCLCPP_ERROR_STREAM(
     this->logger(), GetLoggerPrefix(name()) << ": onFailure with error: " << toStr(error));
   return BT::NodeStatus::FAILURE;
 }
 

husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp (2)

15-16: Update the include guard to match the project naming convention

Similar to previous files, the include guard PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_ should reflect the project's directory structure for consistency.

Apply this diff to update the include guard:

-#ifndef PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
-#define PANTHER_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
+#ifndef HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_
+#define HUSARION_UGV_MANAGER_PLUGINS_ACTION_DOCK_ROBOT_NODE_HPP_

---

27-29: Align the namespace with the project structure

The namespace panther_manager may not match the project's directory husarion_ugv_manager. For consistency, consider updating the namespace.

-namespace panther_manager
+namespace husarion_ugv_manager
 {

husarion_ugv_gazebo/launch/spawn_charging_docks.launch.py (2)

73-73: Avoid magic numbers by defining constants for offsets

The value -2.0 used as an offset in the Y-coordinate can be unclear to readers. Define it as a named constant or add a comment explaining its purpose to improve code readability.

Apply this diff to define a constant:

+    MAP_OFFSET_Y = -2.0  # Offset between world and map frames

     for dock_name in docks_names:
         pose = ros_parameters[dock_name]["pose"]

         spawn_station = Node(
             package="ros_gz_sim",
             executable="create",
             arguments=[
                 "-name",
                 [dock_name, "_station"],
                 "-topic",
                 [dock_name, "_station_description"],
                 "-x",
                 str(pose[0]),
                 "-y",
-                str(pose[1] - 2.0),  # -2.0 is the offset between world and map
+                str(pose[1] + MAP_OFFSET_Y),  # Apply Y-axis offset
                 "-z",
                 "0.5",  # station z is not in 0.0

---

75-75: Clarify hardcoded Z-coordinate value

The hardcoded value "0.5" for the Z-coordinate may cause confusion. Consider defining it as a constant or adding a comment to explain why the station's Z-coordinate is not at 0.0.

Apply this diff to add a comment:

                 "-y",
                 str(pose[1] + MAP_OFFSET_Y),  # Apply Y-axis offset
                 "-z",
-                "0.5",  # station z is not in 0.0
+                "0.5",  # Station's base is 0.5 meters above the ground level
                 "-R",
                 "1.57",

husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp (2)

15-18: Add class and method documentation.

Consider adding documentation to describe:

• The purpose and behavior of the CheckBoolMsg condition node
• The expected input parameters and their constraints
• The possible return values and their conditions

Example documentation:

/**
 * @brief A behavior tree condition node that checks if a received boolean message matches an expected value
 * 
 * This node compares the data field of the last received BoolMsg against an expected boolean value
 * provided through the "data" input port.
 */

---

22-29: Consider adding input validation for the "data" parameter.

The current implementation assumes the "data" parameter is always available. Consider adding validation to handle cases where the parameter is missing.

 BT::NodeStatus CheckBoolMsg::onTick(const BoolMsg::SharedPtr & last_msg)
 {
   bool expected_data;
-  getInput<bool>("data", expected_data);
+  if (!getInput<bool>("data", expected_data)) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "Missing required input [data]. Check the BT XML definition.");
+    return BT::NodeStatus::FAILURE;
+  }

husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp (2)

Line range hint 28-31: Add class-level documentation.

Consider adding comprehensive documentation for the CallSetBoolService class to describe its purpose, usage, and integration with the behavior tree framework.

/**
 * @brief A behavior tree action node that calls a ROS2 SetBool service
 *
 * This node integrates with ROS2 services of type std_srvs::srv::SetBool,
 * allowing behavior trees to control boolean states through service calls.
 *
 * @note The node will return FAILURE if the service call fails or returns false,
 *       and SUCCESS if the service call succeeds and returns true.
 */

---

Line range hint 44-45: Document virtual methods and add override keyword.

The virtual methods lack documentation and the providedPorts method should be marked with override.

-  static BT::PortsList providedPorts()
+  /**
+   * @brief Declares the input/output ports of the node
+   * @return PortsList containing the "data" input port
+   */
+  static BT::PortsList providedPorts() override

-  virtual bool setRequest(typename Request::SharedPtr & request) override;
-  virtual BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override;
+  /**
+   * @brief Sets up the service request with the input boolean value
+   * @param request The service request to be populated
+   * @return true if request was set successfully, false otherwise
+   */
+  bool setRequest(typename Request::SharedPtr & request) override;
+
+  /**
+   * @brief Processes the service response
+   * @param response The received service response
+   * @return SUCCESS if service succeeded, FAILURE otherwise
+   */
+  BT::NodeStatus onResponseReceived(const typename Response::SharedPtr & response) override;

husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp (1)

33-51: Implementation follows BT plugin best practices

The class implementation correctly:

• Inherits from RosTopicSubNode for subscription handling
• Provides clear port documentation
• Uses appropriate types for message handling

However, consider adding validation for the 'data' port's default value in providedPorts():

 return providedBasicPorts(
-  {BT::InputPort<bool>("data", "Specifies the expected state of the data field.")});
+  {BT::InputPort<bool>("data", false, "Specifies the expected state of the data field.")});

husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp (2)

21-36: Input validation aligns with previous learnings

The implementation correctly validates the dock_type parameter without default values, which aligns with the retrieved learning from PR #394. The validation logic is thorough and includes appropriate error logging.

Consider adding range validation for max_undocking_time:

 if (!this->getInput<float>("max_undocking_time", goal.max_undocking_time)) {
   RCLCPP_ERROR_STREAM(
     this->logger(), GetLoggerPrefix(name()) << "Failed to get input [max_undocking_time]");
   return false;
 }
+if (goal.max_undocking_time <= 0.0) {
+  RCLCPP_ERROR_STREAM(
+    this->logger(), GetLoggerPrefix(name()) << "Invalid max_undocking_time value");
+  return false;
+}

---

38-49: Result handling could be more informative

The error handling is functional but could provide more detailed feedback.

Consider enhancing the error reporting:

 if (result->success) {
   return BT::NodeStatus::SUCCESS;
 }
+RCLCPP_WARN_STREAM(
+  this->logger(),
+  GetLoggerPrefix(name()) << "Undocking failed with error code: " << result->error_code);
 return BT::NodeStatus::FAILURE;

husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp (2)

47-58: Port configuration is comprehensive but could be improved

The port configuration is well-documented but could benefit from additional validation.

Consider adding:

1. Range validation for timeout
2. Size validation for axes and buttons vectors

 return providedBasicPorts(
   {BT::InputPort<std::vector<float>>(
      "axes", "",
      "Specifies the expected state of the axes field. An empty string (\"\") means the values "
      "are ignored."),
    BT::InputPort<std::vector<int>>(
      "buttons", "",
      "Specifies the expected state of the buttons field. An empty string (\"\") means values "
      "are ignored."),
    BT::InputPort<double>(
-      "timeout", 0.0, "Maximum allowable time delay to accept the condition.")});
+      "timeout", 0.0, "Maximum allowable time delay to accept the condition. Must be >= 0.0")});

---

61-63: Consider adding method documentation

The private validation methods would benefit from documentation explaining their return values and validation criteria.

Add documentation for the validation methods:

+  /// @brief Validates the axes values against expected values
+  /// @return true if axes match or are ignored, false otherwise
   bool checkAxes(const JoyMsg::SharedPtr & last_msg);
+  /// @brief Validates the button states against expected values
+  /// @return true if buttons match or are ignored, false otherwise
   bool checkButtons(const JoyMsg::SharedPtr & last_msg);
+  /// @brief Checks if the message is within the timeout window
+  /// @return true if message is recent enough, false if too old
   bool checkTimeout(const JoyMsg::SharedPtr & last_msg);

panther_manager/test/plugins/condition/test_check_bool_msg.cpp (2)

55-59: Consider using initialization list in constructor

The publisher initialization can be moved to the initialization list for better performance.

-TestCheckBoolMsg::TestCheckBoolMsg()
-{
-  RegisterNodeWithParams<panther_manager::CheckBoolMsg>(PLUGIN);
-  publisher_ = bt_node_->create_publisher<BoolMsg>(TOPIC, 10);
-}
+TestCheckBoolMsg::TestCheckBoolMsg()
+  : publisher_(bt_node_->create_publisher<BoolMsg>(TOPIC, 10))
+{
+  RegisterNodeWithParams<panther_manager::CheckBoolMsg>(PLUGIN);
+}

🧰 Tools

🪛 cppcheck (2.10-2)

[performance] 58-58: Variable 'publisher_' is assigned in constructor body. Consider performing initialization in initialization list.

(useInitializationList)

---

68-95: Consider documenting timeout behavior

The test cases are well-structured and cover essential scenarios. However, it would be helpful to document the expected timeout behavior when waiting for messages.

Add a comment explaining the timeout behavior:

 TEST_F(TestCheckBoolMsg, NoMessageArrived)
 {
+  // Verifies that the node returns FAILURE when no message is received within the timeout period
   bt_ports input = {{"topic_name", TOPIC}, {"data", "true"}};

husarion_ugv_manager/test/test_behavior_tree_utils.cpp (2)

126-146: Consider making the time verification more robust

The time difference check might be flaky in CI environments where system load can cause delays.

Consider increasing the time threshold or using a mock clock:

-  constexpr double time_threshold = 0.1;
+  constexpr double time_threshold = 1.0;  // More tolerant threshold for CI environments

---

148-158: Consider adding more edge cases

The test covers basic invalid inputs well. Consider adding tests for:

• Invalid numeric values (NaN, Inf)
• Malformed quaternions

 TEST(TestConvertFromStringPoseStamped, WrongInput)
 {
   // Existing tests...
+  str = "1;2;3;nan;0.2;0.1;pose";
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
+  str = "1;2;3;0;0;0;pose";  // Zero quaternion
+  EXPECT_THROW(BT::convertFromString<geometry_msgs::msg::PoseStamped>(str), BT::RuntimeError);
 }

panther_manager/test/plugins/condition/test_check_joy_msg.cpp (1)

60-64: Consider using initialization list in constructor

For better performance, consider moving the publisher initialization to the initialization list.

-TestCheckJoyMsg::TestCheckJoyMsg()
-{
-  RegisterNodeWithParams<panther_manager::CheckJoyMsg>(PLUGIN);
-  joy_publisher_ = bt_node_->create_publisher<JoyMsg>(TOPIC, 10);
-}
+TestCheckJoyMsg::TestCheckJoyMsg()
+  : joy_publisher_(bt_node_->create_publisher<JoyMsg>(TOPIC, 10))
+{
+  RegisterNodeWithParams<panther_manager::CheckJoyMsg>(PLUGIN);
+}

🧰 Tools

🪛 cppcheck (2.10-2)

[performance] 63-63: Variable 'joy_publisher_' is assigned in constructor body. Consider performing initialization in initialization list.

(useInitializationList)

husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp (2)

37-61: Enhance action server implementation for better test coverage.

The CreateActionServer method could be improved to:

1. Validate goal parameters in handle_goal
2. Simulate actual execution time before succeeding
3. Add error injection capabilities

Consider this enhancement:

 void CreateActionServer(
   GoalResponse goal_response, CancelResponse cancel_response, bool success,
   std::uint16_t error_code)
 {
   auto handle_goal =
     [&, goal_response](
       const rclcpp_action::GoalUUID & /*uuid*/,
-      std::shared_ptr<const Action::Goal> /*goal*/) -> rclcpp_action::GoalResponse {
+      std::shared_ptr<const Action::Goal> goal) -> rclcpp_action::GoalResponse {
+      // Validate goal parameters
+      if (goal->dock_id.empty() && goal->use_dock_id) {
+        return rclcpp_action::GoalResponse::REJECT;
+      }
       return goal_response;
     };

   auto handle_accepted = [&, success,
                           error_code](const std::shared_ptr<GoalHandle> goal_handle) -> void {
     ActionResult::SharedPtr result = std::make_shared<ActionResult>();
     result->success = success;
     result->error_code = error_code;
     result->num_retries = 0;
+    // Simulate execution time
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
     goal_handle->succeed(result);
   };

---

66-193: Reduce code duplication in test cases.

Multiple test cases share similar parameter setup code. Consider extracting common parameters into a helper method or fixture.

Consider this refactor:

+  std::map<std::string, std::string> GetDefaultParams() {
+    return {
+      {"action_name", "test_dock_action"},
+      {"use_dock_id", "true"},
+      {"dock_id", "main_dock"},
+      {"dock_type", "test_dock_type"},
+      {"max_staging_time", "5.0"},
+      {"navigate_to_staging_pose", "true"},
+    };
+  }

Then use it in test cases:

-  std::map<std::string, std::string> params = {
-    {"action_name", "test_dock_action"},
-    {"use_dock_id", "true"},
-    {"dock_id", "main_dock"},
-    {"dock_type", "test_dock_type"},
-    {"max_staging_time", "5.0"},
-    {"navigate_to_staging_pose", "true"},
-  };
+  auto params = GetDefaultParams();

husarion_ugv_manager/CMakeLists.txt (1)

15-24: LGTM! Consider grouping related dependencies.

The new dependencies are correctly added. Consider grouping related dependencies together for better organization:

 set(PACKAGE_DEPENDENCIES
     ament_cmake
     ament_index_cpp
     behaviortree_cpp
     behaviortree_ros2
     generate_parameter_library
     libssh
-    geometry_msgs
-    opennav_docking_msgs
     panther_msgs
     husarion_ugv_utils
     rclcpp
     rclcpp_action
     sensor_msgs
-    std_msgs
     std_srvs
-    tf2_geometry_msgs
-    yaml-cpp)
+    yaml-cpp
+    # Message packages
+    geometry_msgs
+    opennav_docking_msgs
+    std_msgs
+    tf2_geometry_msgs)

husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp (1)

39-43: Fix typo in repeating parameter description

The port descriptions are clear and well-documented, but there's a minor grammatical error in the repeating parameter description.

-         "repeating", "Specifies whether the animation should repeated continuously.")});
+         "repeating", "Specifies whether the animation should be repeated continuously.")});

husarion_ugv_manager/test/utils/plugin_test_utils.hpp (1)

157-161: Initialize member pointers to nullptr

The member pointers server_node_, executor_, service_server_, action_server_, and executor_thread_ should be initialized to nullptr to avoid undefined behavior when checking their states.

Initialize the member pointers in the constructor:

 PluginTestUtils()
 {
   rclcpp::init(0, nullptr);
   bt_node_ = std::make_shared<rclcpp::Node>("test_husarion_ugv_manager_node");
+  server_node_ = nullptr;
+  executor_ = nullptr;
+  service_server_ = nullptr;
+  action_server_ = nullptr;
+  executor_thread_ = nullptr;
 }

husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp (2)

159-159: Specify the clock type when initializing rclcpp::Clock

Using rclcpp::Clock() without specifying a clock type defaults to RCL_ROS_TIME, which may not be appropriate if the ROS time is not active. This could lead to uninitialized or incorrect timestamps.

Consider specifying the clock type explicitly or using a node's clock to ensure correct time stamping:

- pose_stamped.header.stamp = rclcpp::Clock().now();
+ pose_stamped.header.stamp = rclcpp::Clock(RCL_SYSTEM_TIME).now();

Or, if you have access to a node:

pose_stamped.header.stamp = node->now();

---

98-116: Add boundary checks for convertFromString<std::vector<float>>

The function does not handle empty input strings or provide feedback for invalid float conversions, which might lead to exceptions or incorrect behavior.

Consider adding checks for empty inputs and handle exceptions during float conversion:

template <>
inline std::vector<float> convertFromString<std::vector<float>>(StringView str)
{
  if (str.empty()) {
    throw BT::RuntimeError("Input string is empty for vector<float> conversion.");
  }
  auto parts = splitString(str, ';');
  std::vector<float> output;
  output.reserve(parts.size());
  try {
    for (const StringView & part : parts) {
      output.push_back(convertFromString<float>(part));
    }
  } catch (const std::exception & e) {
    throw BT::RuntimeError("Failed to convert string to vector<float>: " + std::string(e.what()));
  }
  return output;
}

📜 Review details

Configuration used: .coderabbit.yaml
Review profile: CHILL

📥 Commits

Reviewing files that changed from the base of the PR and between bdbfd7f and 624d8ad.

📒 Files selected for processing (46)

• husarion_ugv/hardware_deps.repos (1 hunks)
• husarion_ugv/simulation_deps.repos (1 hunks)
• husarion_ugv_gazebo/launch/simulation.launch.py (2 hunks)
• husarion_ugv_gazebo/launch/spawn_charging_docks.launch.py (1 hunks)
• husarion_ugv_manager/CMakeLists.txt (3 hunks)
• husarion_ugv_manager/config/lights_manager.yaml (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/behavior_tree_utils.hpp (2 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_bool_service_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp (1 hunks)
• husarion_ugv_manager/package.xml (1 hunks)
• husarion_ugv_manager/src/lights_manager_node.cpp (3 hunks)
• husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp (1 hunks)
• husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp (1 hunks)
• husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp (1 hunks)
• husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp (1 hunks)
• husarion_ugv_manager/src/safety_manager_node.cpp (2 hunks)
• husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/test_shutdown_host.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp (1 hunks)
• husarion_ugv_manager/test/test_behavior_tree_utils.cpp (2 hunks)
• husarion_ugv_manager/test/utils/plugin_test_utils.hpp (3 hunks)
• husarion_ugv_utils/CMakeLists.txt (2 hunks)
• husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp (3 hunks)
• husarion_ugv_utils/include/husarion_ugv_utils/tf2_utils.hpp (1 hunks)
• husarion_ugv_utils/package.xml (1 hunks)
• husarion_ugv_utils/test/test_common_utilities.cpp (2 hunks)
• husarion_ugv_utils/test/test_tf2_utils.cpp (1 hunks)
• panther_description/config/docking_components.yaml (1 hunks)
• panther_manager/test/plugins/condition/test_check_bool_msg.cpp (1 hunks)
• panther_manager/test/plugins/condition/test_check_joy_msg.cpp (1 hunks)

✅ Files skipped from review due to trivial changes (15)

• husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
• husarion_ugv_manager/test/plugins/action/test_shutdown_single_host_node.cpp
• husarion_ugv_manager/test/plugins/test_shutdown_host.cpp
• husarion_ugv_manager/test/plugins/action/test_signal_shutdown_node.cpp
• husarion_ugv_manager/test/plugins/action/test_call_trigger_service_node.cpp
• husarion_ugv_manager/test/plugins/action/test_shutdown_hosts_from_file_node.cpp
• husarion_ugv_manager/test/plugins/decorator/test_tick_after_timeout_node.cpp
• husarion_ugv_manager/test/plugins/action/test_call_set_bool_service_node.cpp
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_hosts_from_file_node.hpp
• husarion_ugv/hardware_deps.repos
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/decorator/tick_after_timeout_node.hpp
• husarion_ugv_manager/test/plugins/test_shutdown_hosts_node.cpp
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/signal_shutdown_node.hpp
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/shutdown_single_host_node.hpp
• husarion_ugv/simulation_deps.repos

🧰 Additional context used

📓 Learnings (3)

husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp (1)

Learnt from: delihus
PR: husarion/panther_ros#394
File: panther_manager/src/plugins/action/undock_robot_action_node.cpp:22-37
Timestamp: 2024-11-12T09:25:48.706Z
Learning: The `dock_type` parameter in the `setGoal` method of the `UndockRobotAction` class cannot have a default value because it is essential for specifying the type of charging dock.

husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp (1)

Learnt from: delihus
PR: husarion/panther_ros#394
File: panther_manager/src/plugins/action/undock_robot_action_node.cpp:22-37
Timestamp: 2024-11-12T09:25:48.706Z
Learning: The `dock_type` parameter in the `setGoal` method of the `UndockRobotAction` class cannot have a default value because it is essential for specifying the type of charging dock.

husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp (1)

Learnt from: delihus
PR: husarion/panther_ros#394
File: panther_manager/src/plugins/action/undock_robot_action_node.cpp:22-37
Timestamp: 2024-11-12T09:25:48.706Z
Learning: The `dock_type` parameter in the `setGoal` method of the `UndockRobotAction` class cannot have a default value because it is essential for specifying the type of charging dock.

🪛 Ruff (0.8.0)

husarion_ugv_gazebo/launch/spawn_charging_docks.launch.py

35-35: Use a context manager for opening files

(SIM115)

🪛 cppcheck (2.10-2)

panther_manager/test/plugins/condition/test_check_bool_msg.cpp

[performance] 58-58: Variable 'publisher_' is assigned in constructor body. Consider performing initialization in initialization list.

(useInitializationList)

husarion_ugv_utils/test/test_tf2_utils.cpp

[error] 100-100: There is an unknown macro here somewhere. Configuration is required. If ; is a macro then please configure it.

(unknownMacro)

panther_manager/test/plugins/condition/test_check_joy_msg.cpp

[performance] 63-63: Variable 'joy_publisher_' is assigned in constructor body. Consider performing initialization in initialization list.

(useInitializationList)

🔇 Additional comments (23)

husarion_ugv_utils/CMakeLists.txt (2)

15-16: LGTM! Dependencies correctly added

The addition of tf2_geometry_msgs and tf2_ros dependencies aligns with the PR objectives for implementing tf2 library utilities. The dependencies are properly integrated into the existing build system.

---

70-74: LGTM! Test configuration properly structured

The test target is correctly configured with appropriate include directories, following the same pattern as other test targets in the file.

husarion_ugv_utils/include/husarion_ugv_utils/ros_utils.hpp (2)

15-16: LGTM! Include guards and includes are properly updated.

The changes follow best practices:

• More specific include guard naming reduces potential conflicts
• Angle brackets for external dependencies follow C++ conventions

Also applies to: 20-20

---

Line range hint 93-116: Add input validation and handle edge cases.

While the function implementation is generally good, it could be more robust by handling additional edge cases:

Consider applying these improvements:

 std::string AddNamespaceToFrameID(const std::string & frame_id, const std::string & node_namespace)
 {
+  if (frame_id.empty()) {
+    throw std::invalid_argument("frame_id cannot be empty");
+  }
+
   std::string tf_prefix = node_namespace;
 
   if (tf_prefix != "/") {
     tf_prefix += '/';
   }
 
   if (tf_prefix.front() == '/') {
     tf_prefix.erase(0, 1);
   }
 
+  // Clean up consecutive slashes
+  size_t pos;
+  while ((pos = tf_prefix.find("//")) != std::string::npos) {
+    tf_prefix.erase(pos, 1);
+  }
+
   return tf_prefix + frame_id;
 }

Also, consider verifying this function's behavior with:

husarion_ugv_manager/src/lights_manager_node.cpp (2)

51-54: LGTM! Good improvement in configurability.

The port number is now properly retrieved from configuration instead of being hardcoded.

---

75-78: LGTM! Improved timer initialization.

Good use of std::chrono::duration for type-safe time handling.

husarion_ugv_utils/test/test_tf2_utils.cpp (1)

1-29: LGTM! Well-organized includes and proper license header.

The includes are logically grouped and all necessary headers are present for the test functionality.

husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp (1)

22-27: onTick function implementation is concise and effective

The onTick method correctly checks for a valid message and sequentially validates the timeout, axes, and buttons. The use of short-circuit evaluation ensures efficiency.

husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp (1)

53-54: Ensure 'dock_type' parameter remains mandatory without a default value

The dock_type input port is correctly defined without a default value, ensuring it must be provided explicitly. This aligns with previous learnings that dock_type is essential for specifying the charging dock type and should not have a default.

husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp (1)

52-75: Consistent default values and descriptions for input ports

Ensure that the default values and descriptions for input ports are consistent and clear. For instance, the use_dock_id port defaults to true, while navigate_to_staging_pose also defaults to true. Verify that these defaults align with expected behavior.

husarion_ugv_manager/package.xml (1)

23-34: LGTM! Dependencies are well-structured and align with project needs.

The new dependencies are appropriate for implementing docking utilities:

• geometry_msgs: Required for pose and transform operations
• opennav_docking_msgs: Essential for docking functionality
• sensor_msgs: Needed for sensor data processing
• tf2_geometry_msgs: Required for geometric transformations

panther_manager/test/plugins/condition/test_check_bool_msg.cpp (2)

61-66: LGTM!

The helper methods are well-implemented and serve their purpose clearly.

---

97-102: LGTM!

The main function follows the standard Google Test pattern.

husarion_ugv_gazebo/launch/simulation.launch.py (1)

113-113: LGTM!

The charging dock spawn action is correctly integrated into the launch sequence.

husarion_ugv_manager/test/test_behavior_tree_utils.cpp (1)

160-179: LGTM!

The vector conversion tests are well-implemented and cover both successful and error cases appropriately.

husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp (2)

37-63: LGTM! Well-structured test helper method

The CreateActionServer method is well-designed with clear separation of concerns for handling goals, cancellations, and acceptance.

---

66-164: LGTM! Comprehensive test coverage

The test suite effectively covers:

• Successful plugin loading
• Error handling for missing parameters
• Server initialization checks
• Various success/failure scenarios

panther_manager/test/plugins/condition/test_check_joy_msg.cpp (1)

88-178: LGTM! Excellent test coverage

The test suite thoroughly covers:

• Timeout scenarios including edge cases
• Various joystick message configurations
• Input validation
• Error conditions

husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp (1)

238-245: LGTM!

The main function correctly initializes the Google Test framework and runs the tests.

husarion_ugv_manager/CMakeLists.txt (2)

59-72: LGTM! Libraries are properly configured.

The new libraries are correctly added following the existing pattern in the file. The libraries are:

1. dock_robot_bt_node
2. undock_robot_bt_node
3. check_bool_msg_bt_node
4. check_joy_msg_bt_node

---

159-182: LGTM! Test targets are properly configured.

The new test targets are correctly added and follow the existing pattern in the file. Each library has a corresponding test target.

husarion_ugv_utils/package.xml (1)

22-23: New dependencies added appropriately

The dependencies for tf2_geometry_msgs and tf2_ros are correctly added to the package.xml, ensuring that the package will have access to the required transformation functionalities.

husarion_ugv_utils/test/test_common_utilities.cpp (1)

17-17: LGTM!

The addition of the <limits> header is necessary for using std::numeric_limits in the version comparison tests.

[coderabbitai]

🛠️ Refactor suggestion

Improve consistency and readability of test values.

1. Use M_PI consistently instead of mixing with hardcoded values (1.57)
2. Define named constants for test values to improve readability and maintenance

+// Test values for offset pose
+static constexpr double kTestTranslationX = 0.7;
+static constexpr double kTestTranslationY = 0.2;
+static constexpr double kTestTranslationZ = 0.3;
+static constexpr double kTestPoseX = 0.1;
+static constexpr double kTestPoseY = 0.9;
+static constexpr double kTestPoseZ = 0.2;
+
 TEST(OffsetPose, CheckDefinedPose)
 {
-  const auto translation = tf2::Vector3(0.7, 0.2, 0.3);
+  const auto translation = tf2::Vector3(kTestTranslationX, kTestTranslationY, kTestTranslationZ);
   tf2::Quaternion rotation;
-  rotation.setRPY(0.0, M_PI / 2.0, M_PI);
+  rotation.setRPY(0.0, M_PI_2, M_PI);  // Use M_PI_2 instead of M_PI / 2.0

Committable suggestion skipped: line range outside the PR's diff.

[delihus]

I don't know it is necessary. Let's wait for @rafal-gorecki

[coderabbitai]

Seems like the humans are having a chat. I'll hop back into my burrow for now. If you need me again, just tag @coderabbitai in a new comment, and I'll come hopping out!

[coderabbitai]

🛠️ Refactor suggestion

Add test case for cancel functionality.

The test suite lacks coverage for action cancellation scenarios.

Add this test case:

TEST_F(TestDockRobot, CallDockRobotServerWithCancellation)
{
  CreateActionServer(
    GoalResponse::ACCEPT_AND_EXECUTE, CancelResponse::ACCEPT, false, 
    ActionResult::CANCELED);

  auto params = GetDefaultParams();
  RegisterNodeWithParams<panther_manager::DockRobot>("DockRobot");
  CreateTree("DockRobot", params);

  auto & tree = GetTree();
  auto status = tree.tickWhileRunning(std::chrono::milliseconds(100));
  EXPECT_EQ(status, BT::NodeStatus::FAILURE);
}

[delihus]

There is no ActionResult::CANCELED

[coderabbitai]

@delihus, thank you for pointing that out. You're correct—ActionResult::CANCELED does not exist. I apologize for the oversight.

To test the cancellation functionality, we can simulate the cancellation by adjusting the action server in the test case. Here's the updated test case:

TEST_F(TestDockRobot, CallDockRobotServerWithCancellation)
{
  // Simulate accepting the goal and then canceling it
  auto handle_goal = [](const rclcpp_action::GoalUUID & /*uuid*/,
                        std::shared_ptr<const Action::Goal> /*goal*/) {
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  };

  auto handle_cancel = [](const std::shared_ptr<GoalHandle> goal_handle) {
    // Accept the cancellation request
    return rclcpp_action::CancelResponse::ACCEPT;
  };

  auto handle_accepted = [](const std::shared_ptr<GoalHandle> goal_handle) {
    // Simulate goal being canceled after acceptance
    auto result = std::make_shared<ActionResult>();
    goal_handle->canceled(result);
  };

  CreateActionServer(handle_goal, handle_cancel, handle_accepted);

  std::map<std::string, std::string> params = GetDefaultParams();

  RegisterNodeWithParams<panther_manager::DockRobot>("DockRobot");
  CreateTree("DockRobot", params);

  auto & tree = GetTree();
  auto status = tree.tickWhileRunning(std::chrono::milliseconds(100));

  EXPECT_EQ(status, BT::NodeStatus::FAILURE);
}

In this test case:

• We simulate the action server accepting the goal and then immediately canceling it.
• The handle_accepted callback calls goal_handle->canceled(result); to indicate that the goal was canceled.
• We adjust CreateActionServer to accept custom handlers for goal acceptance, cancellation, and result processing.

Make sure your CreateActionServer function can accept these handlers:

void CreateActionServer(
  std::function<GoalResponse(const rclcpp_action::GoalUUID &, std::shared_ptr<const Action::Goal>)> handle_goal,
  std::function<CancelResponse(const std::shared_ptr<GoalHandle>)> handle_cancel,
  std::function<void(const std::shared_ptr<GoalHandle>)> handle_accepted)
{
  CreateAction<Action>("test_dock_action", handle_goal, handle_cancel, handle_accepted);
}

Let me know if this helps!

[delihus]

I think we can skip this

[coderabbitai]

@delihus, understood. We'll skip adding the cancellation test. Let us know if there's anything else you need.

---

✏️ Learnings added

Learnt from: delihus
PR: husarion/panther_ros#462
File: husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp:0-0
Timestamp: 2024-12-20T14:13:39.233Z
Learning: User delihus decided to skip the cancellation test case for DockRobot in test_dock_robot_node.cpp.

Note: Learnings are effective only in the context of similar code segments. To apply general coding guidelines, please configure review instructions. You can manage existing learnings in the app.

[coderabbitai]

⚠️ Potential issue

Potential resource leaks and race conditions due to overwriting shared resources

The methods CreateService and CreateAction assign to server_node_, executor_, and executor_thread_ without checking if these resources are already initialized. Multiple invocations will overwrite the existing resources without proper cleanup, potentially leading to resource leaks or race conditions.

Consider modifying the implementation to handle multiple services and actions appropriately. You can check if the resources are already initialized and perform necessary cleanup before reassigning them. Alternatively, design the class to manage multiple nodes and executors without overwriting shared members.

Also applies to: 129-133

[delihus]

I think it is very edgecase

[coderabbitai]

@delihus Entendido. Se for confortável manter assim, tudo bem; se mais tarde você quiser garantir um tratamento mais robusto de recursos, basta avisar para que possamos revisar juntos.

[coderabbitai]

Actionable comments posted: 0

🧹 Outside diff range and nitpick comments (17)

husarion_ugv_utils/test/test_tf2_utils.cpp (3)

33-45: Add documentation for the test fixture class.

Consider adding documentation to explain the purpose of the test fixture and its methods.

+/**
+ * @brief Test fixture for transform pose operations
+ *
+ * Sets up a transform between base_link and odom frames with predefined values
+ * for testing transform operations.
+ */
 class TestTransformPose : public ::testing::Test

---

57-63: Define named constants for transform values.

Consider defining named constants for the transform values to improve readability and maintainability.

+  // Constants for transform setup
+  static constexpr double kTransformX = 0.3;
+  static constexpr double kTransformY = 0.2;
+  static constexpr double kTransformZ = 0.1;
+
   transform_.transform.translation.x = 0.3;
   transform_.transform.translation.y = 0.2;
   transform_.transform.translation.z = 0.1;

---

221-236: Improve test case naming and documentation.

The test case name "TooFarDistance" could be more descriptive. Consider adding a brief comment explaining the test scenario and expected behavior.

+/**
+ * @brief Test that poses with distance greater than threshold are not considered near
+ *
+ * Creates two poses with a distance of ~0.141 units (sqrt(0.1^2 + 0.1^2)) and
+ * verifies they are not considered near with a threshold of 0.05 units.
+ */
 TEST(TestArePosesNear, TooFarDistance)

husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp (4)

32-32: Consider a more descriptive alias name for bt_ports

The alias bt_ports could be more descriptive to better reflect its purpose as behavior tree port parameters. Consider renaming to BehaviorTreePorts or BTPortParameters.

---

41-42: Consider namespace scoping for constants

To prevent potential naming conflicts and improve organization, consider moving these constants into the test class scope or a dedicated namespace.

-constexpr auto TOPIC = "bool";
-constexpr auto PLUGIN = "CheckBoolMsg";
+namespace test_constants {
+constexpr auto TOPIC = "bool";
+constexpr auto PLUGIN = "CheckBoolMsg";
+}  // namespace test_constants

---

55-59: Use initialization list for member initialization

Consider moving the publisher initialization to the constructor's initialization list for better performance and cleaner code.

-TestCheckBoolMsg::TestCheckBoolMsg()
-{
-  RegisterNodeWithParams<husarion_ugv_manager::CheckBoolMsg>(PLUGIN);
-  publisher_ = bt_node_->create_publisher<BoolMsg>(TOPIC, 10);
-}
+TestCheckBoolMsg::TestCheckBoolMsg()
+  : publisher_(RegisterNodeWithParams<husarion_ugv_manager::CheckBoolMsg>(PLUGIN)->
+              create_publisher<BoolMsg>(TOPIC, 10))
+{
+}

🧰 Tools

🪛 cppcheck (2.10-2)

[performance] 58-58: Variable 'publisher_' is assigned in constructor body. Consider performing initialization in initialization list.

(useInitializationList)

---

80-84: Consider adding more edge cases to test suite

While the current test cases cover basic functionality, consider adding tests for:

• Invalid topic names
• Empty topic names
• Multiple rapid message publications
• Message timeout scenarios

Would you like me to provide example test cases for these scenarios?

husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp (4)

41-60: Consider improving lambda function definitions.

While the implementation is functional, there are a few potential improvements:

1. The lambda captures could be more explicit rather than using [&]
2. The commented-out parameter names could be meaningful names instead

Consider this improvement:

    auto handle_goal =
-      [&, goal_response](
-        const rclcpp_action::GoalUUID & /*uuid*/,
-        std::shared_ptr<const Action::Goal> /*goal*/) -> rclcpp_action::GoalResponse {
+      [goal_response](
+        const rclcpp_action::GoalUUID & request_uuid,
+        std::shared_ptr<const Action::Goal> request_goal) -> rclcpp_action::GoalResponse {
      return goal_response;
    };

    auto handle_cancel =
-      [&, cancel_response](
-        const std::shared_ptr<GoalHandleAction> /*goal_handle*/) -> rclcpp_action::CancelResponse {
+      [cancel_response](
+        const std::shared_ptr<GoalHandleAction> request_handle) -> rclcpp_action::CancelResponse {
      return cancel_response;
    };

    auto handle_accepted =
-      [&, success, error_code](const std::shared_ptr<GoalHandleAction> goal_handle) -> void {
+      [success, error_code](const std::shared_ptr<GoalHandleAction> goal_handle) -> void {

---

106-107: Consider extracting timeout constants.

Multiple test cases use magic numbers for timeouts (100ms, 1000ms). Consider extracting these into named constants for better maintainability.

+ static constexpr auto SHORT_TIMEOUT = std::chrono::milliseconds(100);
+ static constexpr auto DEFAULT_TIMEOUT = std::chrono::milliseconds(1000);

- auto status = tree.tickWhileRunning(std::chrono::milliseconds(100));
+ auto status = tree.tickWhileRunning(SHORT_TIMEOUT);

Also applies to: 124-126, 142-144, 161-163

---

68-72: Consider reducing test parameter duplication.

The test parameters are duplicated across multiple test cases. Consider creating a helper method or fixture setup to reduce duplication.

+ void SetUp() override {
+   base_params_ = {
+     {"action_name", "test_undock_action"},
+     {"dock_type", "test_dock_type"},
+     {"max_undocking_time", "5.0"},
+   };
+ }
+
+ std::map<std::string, std::string> GetTestParams(
+   const std::map<std::string, std::string>& overrides = {}) {
+   auto params = base_params_;
+   for (const auto& [key, value] : overrides) {
+     params[key] = value;
+   }
+   return params;
+ }
+
+ private:
+   std::map<std::string, std::string> base_params_;

Then in tests:

- std::map<std::string, std::string> params = {
-   {"action_name", "test_undock_action"},
-   {"dock_type", "test_dock_type"},
-   {"max_undocking_time", "5.0"},
- };
+ auto params = GetTestParams();

Also applies to: 81-85, 94-98, 115-119, 133-137, 152-156

---

66-164: Consider adding tests for missing scenarios.

While the test coverage is good, consider adding the following test cases:

1. Test cancellation handling
2. Test timeout scenario when max_undocking_time is reached

Example structure for additional tests:

TEST_F(TestUndockRobot, HandleCancellation) {
  CreateActionServer(
    GoalResponse::ACCEPT_AND_EXECUTE, 
    CancelResponse::ACCEPT, 
    false, 
    ActionResult::CANCELED);
  // Test implementation
}

TEST_F(TestUndockRobot, HandleTimeout) {
  CreateActionServer(
    GoalResponse::ACCEPT_AND_EXECUTE, 
    CancelResponse::ACCEPT, 
    false, 
    ActionResult::TIMED_OUT);
  // Test implementation with longer duration than max_undocking_time
}

husarion_ugv_manager/config/safety_manager.yaml (1)

5-5: Consider using environment variable for bt_server_port configuration

While the port number is consistent with safety_manager_parameters.yaml, hardcoding it here reduces flexibility. Consider using an environment variable reference to make it configurable across different environments.

-      bt_server_port: 6666
+      bt_server_port: ${SAFETY_BT_SERVER_PORT:-6666}

husarion_ugv_manager/config/lights_manager_parameters.yaml (1)

47-52: Consider adding upper bound for port validation

While the current validation ensures positive values, it's recommended to add an upper bound to prevent potential issues with system ports. Standard practice is to use ports between 1024 and 65535 for user applications.

-    validation: { gt<>: 0 }
+    validation: { bounds<>: [1024, 65535] }

husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp (3)

39-42: Nitpick: Correct the wording in the warning message

In the warning message, it says "expected at least", but the code checks for exact equality of the sizes. Consider changing the message to "expected exactly" for clarity.

Apply this diff to correct the message:

 << " axes, expected at least " << expected_axes.size());
+<< " axes, expected exactly " << expected_axes.size());

---

46-48: Consider using a tolerance for floating-point comparisons

The current comparison uses std::fabs(a - b) <= std::numeric_limits<float>::epsilon(), which may not be sufficient due to floating-point precision issues. Floating-point representations can introduce small inaccuracies, and using machine epsilon might not account for these effectively.

Consider using a larger tolerance or a relative comparison to handle floating-point inaccuracies more robustly.

Apply this diff to improve the comparison:

 return std::equal(
   expected_axes.begin(), expected_axes.end(), last_msg->axes.begin(),
-  [](float a, float b) { return std::fabs(a - b) <= std::numeric_limits<float>::epsilon(); });
+  [](float a, float b) { return std::fabs(a - b) <= 1e-5; });

Alternatively, define a tolerance constant or use a relative comparison method:

const float TOLERANCE = 1e-5;
[](float a, float b) { return std::fabs(a - b) <= TOLERANCE * std::max(std::fabs(a), std::fabs(b)); });

---

61-64: Nitpick: Correct the wording in the warning message

In the warning message, it says "expected at least", but the code checks for exact equality of the sizes. Consider changing the message to "expected exactly" for clarity.

Apply this diff to correct the message:

 << " buttons, expected at least " << expected_buttons.size());
+<< " buttons, expected exactly " << expected_buttons.size());

husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp (1)

63-63: Initialize joy_publisher_ using the constructor initialization list

The variable joy_publisher_ is assigned in the constructor body. Initializing it in the constructor's initialization list enhances clarity and can improve performance by avoiding an extra assignment after default construction.

Apply this diff to move the initialization to the initialization list:

 TestCheckJoyMsg::TestCheckJoyMsg()
-{
+  : joy_publisher_(bt_node_->create_publisher<JoyMsg>(TOPIC, 10))
+{
   RegisterNodeWithParams<husarion_ugv_manager::CheckJoyMsg>(PLUGIN);
-  joy_publisher_ = bt_node_->create_publisher<JoyMsg>(TOPIC, 10);
 }

🧰 Tools

🪛 cppcheck (2.10-2)

[performance] 63-63: Variable 'joy_publisher_' is assigned in constructor body. Consider performing initialization in initialization list.

(useInitializationList)

📜 Review details

Configuration used: .coderabbit.yaml
Review profile: CHILL
Plan: Pro

📥 Commits

Reviewing files that changed from the base of the PR and between 624d8ad and 863c835.

📒 Files selected for processing (19)

• husarion_ugv_manager/CMakeLists.txt (10 hunks)
• husarion_ugv_manager/config/lights_manager_parameters.yaml (1 hunks)
• husarion_ugv_manager/config/safety_manager.yaml (1 hunks)
• husarion_ugv_manager/config/safety_manager_parameters.yaml (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp (1 hunks)
• husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp (1 hunks)
• husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp (1 hunks)
• husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp (1 hunks)
• husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp (1 hunks)
• husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp (1 hunks)
• husarion_ugv_utils/include/husarion_ugv_utils/tf2_utils.hpp (1 hunks)
• husarion_ugv_utils/test/test_common_utilities.cpp (2 hunks)
• husarion_ugv_utils/test/test_tf2_utils.cpp (1 hunks)

✅ Files skipped from review due to trivial changes (1)

• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/dock_robot_node.hpp

🚧 Files skipped from review as they are similar to previous changes (8)

• husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp
• husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp
• husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/undock_robot_node.hpp
• husarion_ugv_utils/include/husarion_ugv_utils/tf2_utils.hpp
• husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp

🧰 Additional context used

🪛 cppcheck (2.10-2)

husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp

[performance] 58-58: Variable 'publisher_' is assigned in constructor body. Consider performing initialization in initialization list.

(useInitializationList)

husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp

[performance] 63-63: Variable 'joy_publisher_' is assigned in constructor body. Consider performing initialization in initialization list.

(useInitializationList)

husarion_ugv_utils/test/test_tf2_utils.cpp

[error] 100-100: There is an unknown macro here somewhere. Configuration is required. If ; is a macro then please configure it.

(unknownMacro)

🔇 Additional comments (16)

husarion_ugv_utils/test/test_common_utilities.cpp (1)

199-208: ⚠️ Potential issue

Fix test naming pattern and remove duplicate scenarios.

The test case has the following issues:

1. Test name doesn't follow the pattern (missing 'Test' prefix)
2. Contains duplicate test scenarios that are already covered by existing tests:
   • Line 204: Duplicates TestMeetsVersionRequirement, SameVersion
   • Lines 205-206: Duplicates TestMeetsVersionRequirement, NaNVersionRequired
   • Line 203: Similar to TestMeetsVersionRequirement, HigherVersion

Apply this diff to fix the naming and remove duplicates:

-TEST(MeetsVersionRequirement, CorrectlyComparesVersions)
+TEST(TestMeetsVersionRequirement, ZeroVersionRequired)
 {
   float panther_version = 1.06;
   EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 0.0));
-  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.0));
-  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.06));
-  EXPECT_FALSE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(
-    panther_version, std::numeric_limits<float>::quiet_NaN()));
-  EXPECT_FALSE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.2));
 }

husarion_ugv_utils/test/test_tf2_utils.cpp (3)

1-32: LGTM! Well-structured file setup.

The file has proper licensing, includes all necessary headers, and uses descriptive constants for frame names.

---

144-144: Use M_PI consistently for angle values.

Replace hardcoded values with M_PI constants for better readability and precision.

-  rotation.setRPY(0.0, 1.57, 3.14);
+  rotation.setRPY(0.0, M_PI_2, M_PI);

---

278-287: LGTM! Well-structured main function.

The main function properly handles ROS2 initialization, test execution, and cleanup.

husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp (1)

61-66: LGTM! Well-structured helper methods

The helper methods are concise, focused, and follow good practices.

husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp (2)

1-27: LGTM! Well-structured headers and licensing.

The file includes all necessary headers, properly grouped by functionality (standard library, external dependencies, and project-specific headers). The copyright and license information is correctly formatted.

---

166-173: LGTM! Standard Google Test main function.

The main function correctly initializes the Google Test framework and handles the test results.

husarion_ugv_manager/config/safety_manager_parameters.yaml (1)

15-20: Enhance port validation and verify availability

1. Similar to the lights manager, consider adding an upper bound to the port validation.
2. We should verify that these ports are available in the ROS2 environment.

-    validation: { gt<>: 0 }
+    validation: { bounds<>: [1024, 65535] }

Let's verify port availability and usage:

✅ Verification successful

Let me check the content of these files to better understand the port configurations.

---

Port configuration is properly separated and non-conflicting

The verification shows that:

1. The lights manager uses port 5555
2. The safety manager uses port 6666
3. Both ports are consistently defined in their respective configuration files
4. The ports are in a safe range for user applications (above 1024)

The suggestion to add bounds validation is still valid, but there are no port conflicts between components.

• Consider adding bounds validation to both managers: validation: { bounds<>: [1024, 65535] }

🏁 Scripts executed

The following scripts were executed for the analysis:

Script:

#!/bin/bash
# Description: Check if the BT server ports are already in use by other ROS2 components

# Search for port numbers in launch files and other configurations
echo "Searching for port usage in launch files..."
rg -l "port.*[56]666" --type yaml --type xml

# Search for other port configurations in the codebase
echo "Checking for other port configurations..."
rg -l "bt_server_port" --type yaml

# Note: In a real environment, you would also need to check if these ports
# are actually available using netstat or similar tools

Length of output: 533

---

Script:

#!/bin/bash
# Check the content of the found files for port configurations
echo "=== safety_manager.yaml ==="
cat husarion_ugv_manager/config/safety_manager.yaml

echo -e "\n=== lights_manager_parameters.yaml ==="
cat husarion_ugv_manager/config/lights_manager_parameters.yaml

echo -e "\n=== lights_manager.yaml ==="
cat husarion_ugv_manager/config/lights_manager.yaml

Length of output: 4253

husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp (3)

32-32: Handle the return value of getInput to prevent undefined behavior

The code assumes that getInput<std::vector<float>>("axes", expected_axes) always succeeds. If the input port "axes" is missing or invalid, expected_axes may contain unintended values, leading to incorrect behavior. It's important to check the return value of getInput and handle any errors appropriately.

Apply this fix to ensure safe usage of expected_axes:

 std::vector<float> expected_axes;
-getInput<std::vector<float>>("axes", expected_axes);
+if (!getInput<std::vector<float>>("axes", expected_axes)) {
+  RCLCPP_ERROR(this->logger(), "Input port 'axes' is missing or invalid.");
+  return false;
+}

---

54-54: Handle the return value of getInput to prevent undefined behavior

The code assumes that getInput<std::vector<int>>("buttons", expected_buttons) always succeeds. If the input port "buttons" is missing or invalid, expected_buttons may contain unintended values, leading to incorrect behavior. It's important to check the return value of getInput and handle any errors appropriately.

Apply this fix to ensure safe usage of expected_buttons:

 std::vector<int> expected_buttons;
-getInput<std::vector<int>>("buttons", expected_buttons);
+if (!getInput<std::vector<int>>("buttons", expected_buttons)) {
+  RCLCPP_ERROR(this->logger(), "Input port 'buttons' is missing or invalid.");
+  return false;
+}

---

74-74: Verify success of getInput before using max_timeout

The function uses max_timeout without checking if getInput<double>("timeout", max_timeout) succeeded. This can lead to incorrect timeout handling if the input is missing or invalid.

Apply this fix to handle potential errors:

 double max_timeout;
-getInput<double>("timeout", max_timeout);
+if (!getInput<double>("timeout", max_timeout)) {
+  RCLCPP_ERROR(this->logger(), "Input port 'timeout' is missing or invalid.");
+  return false;
+}

husarion_ugv_manager/CMakeLists.txt (5)

15-16: LGTM: Dependencies align with docking functionality

The added dependencies are appropriate for implementing docking operations:

• opennav_docking_msgs for docking-specific messages
• geometry_msgs and tf2_geometry_msgs for handling spatial transformations
• std_msgs for standard message types

Also applies to: 22-22, 24-24

---

88-94: LGTM: Node dependencies are consistent

The dependencies added to both safety_manager_node and lights_manager_node are consistent with the package dependencies and follow the project's formatting style.

Also applies to: 106-112

---

229-229: LGTM: Test dependencies are consistent

The dependencies added to test targets are consistent and properly aligned with the package dependencies.

Also applies to: 250-256, 271-277, 291-297, 312-319

---

59-72: Verify behavior tree plugin registration

The new BT node libraries are properly defined. Let's verify their plugin registration.

✅ Verification successful

BT plugins are properly registered and implemented

All four behavior tree plugins are correctly registered using CreateRosNodePlugin macro:

• DockRobot registered as "DockRobot"
• UndockRobot registered as "UndockRobot"
• CheckBoolMsg registered as "CheckBoolMsg"
• CheckJoyMsg registered as "CheckJoyMsg"

Each plugin follows the proper implementation pattern with required node registration. No issues found.

🏁 Scripts executed

The following scripts were executed for the analysis:

Script:

#!/bin/bash
# Description: Verify that the new BT plugins are properly registered in XML

# Check for plugin registration files
echo "Checking for plugin registration files..."
fd -e xml

# Search for plugin class registrations
echo -e "\nChecking plugin class registrations..."
rg -l "BT_REGISTER_NODES|<BehaviorTree>" --type xml

Length of output: 964

---

Script:

#!/bin/bash
# Let's check the source files for BT_REGISTER_NODES macro usage
echo "Checking for BT_REGISTER_NODES in source files..."
rg "BT_REGISTER_NODES" src/plugins/action/dock_robot_node.cpp src/plugins/action/undock_robot_node.cpp src/plugins/condition/check_bool_msg.cpp src/plugins/condition/check_joy_msg.cpp

# Let's also check behavior tree XML files for node usage
echo -e "\nChecking node usage in behavior trees..."
rg "<(Dock|Undock)Robot|<CheckBoolMsg|<CheckJoyMsg" husarion_ugv_manager/behavior_trees/

Length of output: 801

---

Script:

#!/bin/bash
# Let's first find the correct paths to the source files
echo "Finding source files..."
fd -t f "dock_robot_node.cpp|undock_robot_node.cpp|check_bool_msg.cpp|check_joy_msg.cpp"

# Let's also check the content of behavior tree XML files
echo -e "\nChecking behavior tree XML contents..."
for file in husarion_ugv_manager/behavior_trees/*.xml; do
    echo "=== $file ==="
    cat "$file" 2>/dev/null
done

Length of output: 12256

---

Script:

#!/bin/bash
# Now let's check the content of the source files for BT_REGISTER_NODES
echo "Checking BT_REGISTER_NODES in dock_robot_node.cpp..."
cat husarion_ugv_manager/src/plugins/action/dock_robot_node.cpp

echo -e "\nChecking BT_REGISTER_NODES in undock_robot_node.cpp..."
cat husarion_ugv_manager/src/plugins/action/undock_robot_node.cpp

echo -e "\nChecking BT_REGISTER_NODES in check_bool_msg.cpp..."
cat husarion_ugv_manager/src/plugins/condition/check_bool_msg.cpp

echo -e "\nChecking BT_REGISTER_NODES in check_joy_msg.cpp..."
cat husarion_ugv_manager/src/plugins/condition/check_joy_msg.cpp

Length of output: 9796

---

163-186: Verify test coverage for new BT nodes

The test targets are properly defined for all new BT nodes. Let's verify their test coverage.

✅ Verification successful

Let me verify if the test files exist in a different location by searching for them.

---

Let me check if the test directory exists and search for test files in the entire repository.

---

Let me check the content of these test files to verify their implementation.

---

Test coverage is comprehensive for all new BT nodes

All test files are properly implemented with good coverage:

• test_dock_robot_node.cpp: 9 test cases covering plugin loading, error cases, and success scenarios
• test_undock_robot_node.cpp: 6 test cases for plugin loading, server initialization, and execution paths
• test_check_bool_msg.cpp: Tests for message handling and tick behavior
• test_check_joy_msg.cpp: Tests for timeout scenarios, message handling, and tick behavior

🏁 Scripts executed

The following scripts were executed for the analysis:

Script:

#!/bin/bash
# Description: Check test coverage for new BT nodes

# List test files and their contents
echo "Checking test file contents..."
for test_file in test/plugins/action/test_dock_robot_node.cpp \
                 test/plugins/action/test_undock_robot_node.cpp \
                 test/plugins/condition/test_check_bool_msg.cpp \
                 test/plugins/condition/test_check_joy_msg.cpp; do
    echo -e "\nChecking $test_file..."
    if [ -f "$test_file" ]; then
        # Look for test cases and assertions
        rg "TEST|EXPECT_|ASSERT_" "$test_file"
    else
        echo "File not found: $test_file"
    fi
done

Length of output: 2183

---

Script:

#!/bin/bash
# Search for test files with similar names
echo "Searching for test files..."
fd "test.*\.(cpp|hpp)$" test/

Length of output: 189

---

Script:

#!/bin/bash
# First, find all test-related files in the repository
echo "Searching for test files in the repository..."
fd -t f "test.*\.(cpp|hpp)$"

# Also check the directory structure
echo -e "\nChecking directory structure..."
fd -t d "test"

Length of output: 5424

---

Script:

#!/bin/bash
# Check content of the specific test files we're interested in
echo "Checking test_dock_robot_node.cpp..."
rg "TEST|EXPECT_|ASSERT_" "husarion_ugv_manager/test/plugins/action/test_dock_robot_node.cpp"

echo -e "\nChecking test_undock_robot_node.cpp..."
rg "TEST|EXPECT_|ASSERT_" "husarion_ugv_manager/test/plugins/action/test_undock_robot_node.cpp"

echo -e "\nChecking test_check_bool_msg.cpp..."
rg "TEST|EXPECT_|ASSERT_" "husarion_ugv_manager/test/plugins/condition/test_check_bool_msg.cpp"

echo -e "\nChecking test_check_joy_msg.cpp..."
rg "TEST|EXPECT_|ASSERT_" "husarion_ugv_manager/test/plugins/condition/test_check_joy_msg.cpp"

Length of output: 3055

[coderabbitai]

Actionable comments posted: 1

🧹 Outside diff range and nitpick comments (1)

husarion_ugv_manager/CMakeLists.txt (1)

59-72: LGTM: New BT plugins follow established patterns

The new behavior tree plugins are properly structured and integrated:

• Consistent with existing plugin architecture
• Correctly defined as shared libraries
• Properly added to plugin_libs list

Consider documenting the plugin interfaces in a README.md file to help other developers understand:

• The purpose of each plugin
• Required inputs/outputs
• Example usage in behavior trees

📜 Review details

Configuration used: .coderabbit.yaml
Review profile: CHILL
Plan: Pro

📥 Commits

Reviewing files that changed from the base of the PR and between 863c835 and 39a7f12.

📒 Files selected for processing (1)

• husarion_ugv_manager/CMakeLists.txt (10 hunks)

🔇 Additional comments (2)

husarion_ugv_manager/CMakeLists.txt (2)

229-229: LGTM: Test dependencies properly updated

Dependencies are consistently updated across all test targets, following CMake best practices.

Also applies to: 250-256, 271-277, 291-297, 312-319

---

15-16: LGTM: Dependencies align with docking functionality requirements

The added dependencies are appropriate for implementing docking-related functionality:

• geometry_msgs & tf2_geometry_msgs for pose and transform operations
• opennav_docking_msgs for docking operations
• std_msgs for common message types

Also applies to: 22-22, 24-24

✅ Verification successful

Dependencies are actively used across the codebase

The dependencies are well-justified and actively used throughout the codebase, particularly in:

• Docking functionality: dock_robot_node.hpp and undock_robot_node.hpp
• BT plugins: Various condition and action nodes
• Core utilities: tf2_utils.hpp, ros_utils.hpp
• Test files validating the functionality

🏁 Scripts executed

The following scripts were executed for the analysis:

Script:

#!/bin/bash
# Verify that the dependencies are used in the source files
rg -l "geometry_msgs|opennav_docking_msgs|std_msgs|tf2_geometry_msgs" src/

Length of output: 121

---

Script:

#!/bin/bash
# First, let's find the correct source directory structure
fd -t d "src"

# Then, let's search for these dependencies in all files
rg -l "geometry_msgs|opennav_docking_msgs|std_msgs|tf2_geometry_msgs"

# Also check for header includes and message types
rg -l "#include.*(?:geometry_msgs|opennav_docking_msgs|std_msgs|tf2_geometry_msgs)"

Length of output: 4472

[coderabbitai]

Actionable comments posted: 1

♻️ Duplicate comments (1)

husarion_ugv_manager/test/utils/plugin_test_utils.hpp (1)

110-115: ⚠️ Potential issue

Resource management issues in CreateService method

The method still overwrites shared resources (server_node_, executor_, executor_thread_) without proper cleanup, which could lead to resource leaks and race conditions.

Consider implementing proper resource cleanup:

 template <typename ServiceT>
 void CreateService(
   const std::string & service_name,
   std::function<
     void(const typename ServiceT::Request::SharedPtr, typename ServiceT::Response::SharedPtr)>
     service_callback)
 {
+  if (executor_thread_) {
+    executor_->cancel();
+    executor_thread_->join();
+    executor_thread_.reset();
+  }
+  if (server_node_) {
+    executor_->remove_node(server_node_);
+  }
   server_node_ = std::make_shared<rclcpp::Node>("test_node_for_" + service_name);
   service_server_ = server_node_->create_service<ServiceT>(service_name, service_callback);
   executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
   executor_->add_node(server_node_);
   executor_thread_ = std::make_unique<std::thread>([this]() { executor_->spin(); });
 }

🧹 Nitpick comments (4)

husarion_ugv_manager/test/utils/plugin_test_utils.hpp (1)

157-161: LGTM with suggestion for thread safety

The member variable changes improve clarity by sharing the node between service and action servers. However, consider adding mutex protection for thread-safe access to these shared resources.

protected:
+ mutable std::mutex mutex_;  // Add at the top of protected section
  rclcpp::Node::SharedPtr server_node_;
  rclcpp::executors::SingleThreadedExecutor::UniquePtr executor_;
  rclcpp::ServiceBase::SharedPtr service_server_;
  rclcpp_action::ServerBase::SharedPtr action_server_;
  std::unique_ptr<std::thread> executor_thread_;

husarion_ugv_gazebo/launch/spawn_charging_docks.launch.py (3)

30-37: Consider parameterizing the AprilTag scale value

The scale value of 1000 is hardcoded. Consider making it configurable through launch parameters to allow flexibility in tag size based on different simulation scenarios.

-def generate_apriltag_and_get_path(tag_id):
+def generate_apriltag_and_get_path(tag_id, scale=1000):
     from moms_apriltag import TagGenerator2
 
     tag_generator = TagGenerator2("tag36h11")
-    tag_image = tag_generator.generate(tag_id, scale=1000)
+    tag_image = tag_generator.generate(tag_id, scale=scale)
     with NamedTemporaryFile(suffix=f"_tag_{tag_id}.png", delete=False) as temp_file:
         imageio.imwrite(temp_file.name, tag_image)
         return temp_file.name

---

66-66: Use f-strings for node naming

The current list-based string concatenation for node names could be simplified using f-strings.

-                [dock_name, "_station"],
+                f"{dock_name}_station",
-                [dock_name, "_station_description"],
+                f"{dock_name}_station_description",

Also applies to: 68-68

---

92-96: Improve device_namespace parameter description

The current description is lengthy and could be clearer. Consider breaking it down into key points and providing an example.

     declare_device_namespace = DeclareLaunchArgument(
         "device_namespace",
         default_value="",
-        description="Device namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.",
+        description=(
+            "Namespace prefix for topics and TF frames. "
+            "Used to support multiple robots in simulation. "
+            "Example: 'robot1/' or 'robot2/'"
+        ),
     )

📜 Review details

Configuration used: .coderabbit.yaml
Review profile: CHILL
Plan: Pro

📥 Commits

Reviewing files that changed from the base of the PR and between 9345045 and 6eda511.

📒 Files selected for processing (11)

• husarion_ugv/hardware_deps.repos (1 hunks)
• husarion_ugv/simulation_deps.repos (1 hunks)
• husarion_ugv_gazebo/launch/simulation.launch.py (2 hunks)
• husarion_ugv_gazebo/launch/spawn_charging_docks.launch.py (1 hunks)
• husarion_ugv_manager/CMakeLists.txt (10 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp (1 hunks)
• husarion_ugv_manager/package.xml (1 hunks)
• husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp (1 hunks)
• husarion_ugv_manager/test/utils/plugin_test_utils.hpp (3 hunks)
• husarion_ugv_utils/package.xml (1 hunks)
• husarion_ugv_utils/test/test_common_utilities.cpp (2 hunks)

🚧 Files skipped from review as they are similar to previous changes (7)

• husarion_ugv_manager/test/plugins/action/test_call_set_led_animation_service_node.cpp
• husarion_ugv_gazebo/launch/simulation.launch.py
• husarion_ugv_manager/package.xml
• husarion_ugv/simulation_deps.repos
• husarion_ugv_utils/package.xml
• husarion_ugv/hardware_deps.repos
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/action/call_set_led_animation_service_node.hpp

🔇 Additional comments (5)

husarion_ugv_utils/test/test_common_utilities.cpp (1)

199-208: 🛠️ Refactor suggestion

Consider removing duplicate test scenarios.

This test case duplicates several existing test scenarios:

• Testing version 1.06 is already covered by SameVersion
• NaN scenario is already covered by NaNVersionRequired
• Version 1.0 comparison is already covered by HigherVersion

Consider keeping only the unique test scenario (version 0.0) and moving it to a new appropriately named test case.

-TEST(TestMeetsVersionRequirement, CorrectlyComparesVersions)
-{
-  float panther_version = 1.06;
-  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 0.0));
-  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.0));
-  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.06));
-  EXPECT_FALSE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(
-    panther_version, std::numeric_limits<float>::quiet_NaN()));
-  EXPECT_FALSE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 1.2));
-}
+TEST(TestMeetsVersionRequirement, ZeroVersionRequired)
+{
+  float panther_version = 1.06;
+  EXPECT_TRUE(husarion_ugv_utils::common_utilities::MeetsVersionRequirement(panther_version, 0.0));
+}

Likely invalid or redundant comment.

husarion_ugv_manager/CMakeLists.txt (1)

15-16: LGTM! Well-structured CMake configuration.

The changes follow the existing patterns in the file:

• Dependencies are consistently added across all relevant targets
• New BT plugin libraries are properly configured
• Test targets are correctly set up with existing source files

Also applies to: 22-22, 24-24, 59-72, 165-187

husarion_ugv_manager/test/utils/plugin_test_utils.hpp (2)

29-29: LGTM: Header inclusion follows standard conventions

The addition of rclcpp_action header is appropriate for the new action server functionality.

---

117-134: 🛠️ Refactor suggestion

⚠️ Potential issue

Resource management issues in CreateAction method

The new CreateAction method has the same resource management issues as CreateService. Additionally, while the implementation is technically correct, it could benefit from some improvements.

1. Add resource cleanup:

 template <typename ActionT>
 void CreateAction(
   const std::string & action_name,
   std::function<rclcpp_action::GoalResponse(
     const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const typename ActionT::Goal> goal)>
     handle_goal,
   std::function<rclcpp_action::CancelResponse(
     const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)>
     handle_cancel,
   std::function<void(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle)>
     handle_accepted)
 {
+  if (executor_thread_) {
+    executor_->cancel();
+    executor_thread_->join();
+    executor_thread_.reset();
+  }
+  if (server_node_) {
+    executor_->remove_node(server_node_);
+  }
   server_node_ = std::make_shared<rclcpp::Node>("test_node_for_" + action_name);
   action_server_ = rclcpp_action::create_server<ActionT>(
     server_node_, action_name, handle_goal, handle_cancel, handle_accepted);
   executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
   executor_->add_node(server_node_);
   executor_thread_ = std::make_unique<std::thread>([this]() { executor_->spin(); });
 }

2. Consider adding error handling for action server creation:

   action_server_ = rclcpp_action::create_server<ActionT>(
     server_node_, action_name, handle_goal, handle_cancel, handle_accepted);
+  if (!action_server_) {
+    throw std::runtime_error("Failed to create action server for " + action_name);
+  }

Likely invalid or redundant comment.

husarion_ugv_gazebo/launch/spawn_charging_docks.launch.py (1)

70-80: Verify pose transformation logic

The pose transformation includes several hardcoded values and rotations:

• Z position is fixed at 0.5
• Roll is fixed at 1.57 (π/2)
• Yaw has a -1.57 offset

Please verify if these transformations are correct for all possible docking station orientations.

[rafal-gorecki]

Waiting for your opinion

[rafal-gorecki]

Why we keep it?

[delihus]

Just the implementations of plugins. Should I move it to husarion_ugv_docking?

[delihus]

moved

[rafal-gorecki]

I'm not sure, how we should spawn station. This solutions require duplicating config from docking_parameters.
Other option can be

1. Moving it into husarion_ugv_docking and run it only if use_sim=true.
2. In other hand we can add some yaml with station position in husarion_gz_worlds.

[delihus]

1. Okay!
2. I think it will make a dependency between panther-navigation repo and husarion_gz_worlds

[delihus]

moved spawn to husarion_ugv_docking

[coderabbitai]

Actionable comments posted: 0

🧹 Nitpick comments (1)

husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp (1)

46-50: Consider enhancing port documentation

While the implementation is correct, consider adding documentation about the possible return values and their implications for the behavior tree's decision-making process.

   static BT::PortsList providedPorts()
   {
     return providedBasicPorts(
-      {BT::InputPort<bool>("data", "Specifies the expected state of the data field.")});
+      {BT::InputPort<bool>("data", 
+        "Specifies the expected state of the data field. "
+        "Returns SUCCESS when the message data matches the expected state, "
+        "FAILURE otherwise.")});
   }

📜 Review details

Configuration used: .coderabbit.yaml
Review profile: CHILL
Plan: Pro

📥 Commits

Reviewing files that changed from the base of the PR and between f2ec111 and 47df893.

📒 Files selected for processing (3)

• husarion_ugv_manager/CMakeLists.txt (11 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp (1 hunks)
• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp (1 hunks)

🚧 Files skipped from review as they are similar to previous changes (1)

• husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_joy_msg.hpp

🔇 Additional comments (6)

husarion_ugv_manager/include/husarion_ugv_manager/plugins/condition/check_bool_msg.hpp (3)

15-28: LGTM: Header structure and includes are well-organized

The header guards follow the project's naming convention, and all necessary dependencies are properly included.

---

32-34: QoS profile configuration limitation is tracked

The FIXME comment regarding QoS profile configuration for e_stop subscription is being addressed in BehaviorTree.ROS2 PR #99 and tracked in panther_ros issue #434.

---

38-42: LGTM: Constructor is properly implemented

The constructor correctly initializes the base class with the required parameters.

husarion_ugv_manager/CMakeLists.txt (3)

15-24: LGTM: Dependencies are properly configured

The new dependencies (geometry_msgs, std_msgs, tf2_geometry_msgs) are correctly added and align with the project's requirements.

---

58-65: LGTM: New BT plugin libraries are properly configured

The check_bool_msg_bt_node and check_joy_msg_bt_node libraries are correctly added and follow the project's plugin architecture pattern.

---

158-169: LGTM: Test targets are properly configured

The test targets for check_bool_msg and check_joy_msg are correctly integrated into the testing framework, maintaining consistency with the project's testing approach.