Add dock_pose field, improve description, add dependencies

panther_manager/CMakeLists.txt

@@ -11,15 +11,17 @@ set(PACKAGE_DEPENDENCIES
     behaviortree_cpp
     behaviortree_ros2
     libssh
+    geometry_msgs
+    opennav_docking_msgs
     panther_msgs
     panther_utils
     rclcpp
     rclcpp_action
     sensor_msgs
     std_msgs
     std_srvs
-    yaml-cpp
-    opennav_docking_msgs)
+    tf2_geometry_msgs
+    yaml-cpp)
 
 foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES})
   find_package(${PACKAGE} REQUIRED)
@@ -78,16 +80,19 @@ foreach(bt_plugin ${plugin_libs})
 endforeach()
 
 add_executable(
-  safety_manager_node src/safety_manager_node_main.cpp
-                      src/safety_manager_node.cpp src/behavior_tree_manager.cpp)
+  safety_manager_node
+  src/safety_manager_node_main.cpp src/safety_manager_node.cpp
+  src/behavior_tree_manager.cpp src/behavior_tree_manager.cpp)
 ament_target_dependencies(
   safety_manager_node
   behaviortree_ros2
+  geometry_msgs
   panther_msgs
   panther_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 target_link_libraries(safety_manager_node ${plugin_libs})
 
 add_executable(
@@ -96,11 +101,13 @@ add_executable(
 ament_target_dependencies(
   lights_manager_node
   behaviortree_ros2
+  geometry_msgs
   panther_msgs
   panther_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 target_link_libraries(lights_manager_node ${plugin_libs})
 
 add_executable(
@@ -110,11 +117,13 @@ add_executable(
 ament_target_dependencies(
   robot_states_manager_node
   behaviortree_ros2
+  geometry_msgs
   panther_msgs
   panther_utils
   rclcpp
   sensor_msgs
-  std_msgs)
+  std_msgs
+  tf2_geometry_msgs)
 target_link_libraries(robot_states_manager_node ${plugin_libs})
 
 install(TARGETS ${plugin_libs} DESTINATION lib)
@@ -222,8 +231,9 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_behavior_tree_utils
     PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
            $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${PROJECT_NAME}_test_behavior_tree_utils
-                            behaviortree_cpp behaviortree_ros2 panther_utils)
+  ament_target_dependencies(
+    ${PROJECT_NAME}_test_behavior_tree_utils behaviortree_cpp behaviortree_ros2
+    geometry_msgs panther_utils tf2_geometry_msgs)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_behavior_tree_manager
@@ -246,10 +256,12 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_manager_node
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     panther_msgs
     panther_utils
     rclcpp
     sensor_msgs
+    tf2_geometry_msgs
     std_msgs)
 
   ament_add_gtest(
@@ -264,11 +276,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_lights_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     panther_msgs
     panther_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_safety_manager_node test/test_safety_manager_node.cpp
@@ -281,11 +295,13 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_manager_node
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     panther_msgs
     panther_utils
     rclcpp
     sensor_msgs
-    std_msgs)
+    std_msgs
+    tf2_geometry_msgs)
 
   ament_add_gtest(
     ${PROJECT_NAME}_test_safety_behavior_tree
@@ -299,12 +315,14 @@ if(BUILD_TESTING)
     ${PROJECT_NAME}_test_safety_behavior_tree
     behaviortree_cpp
     behaviortree_ros2
+    geometry_msgs
     panther_msgs
     panther_utils
     rclcpp
     sensor_msgs
     std_msgs
-    std_srvs)
+    std_srvs
+    tf2_geometry_msgs)
 endif()
 
 ament_package()

panther_manager/config/robot_states_manager.yaml

@@ -3,7 +3,7 @@
     ros__parameters:
       timer_frequency: 50.0
       ros_communication_timeout:
-        availability: 1.0
+        availability: 2.0
         response: 1.0
       ros_plugin_libs:
         - call_trigger_service_bt_node

panther_manager/include/panther_manager/behavior_tree_utils.hpp

@@ -26,6 +26,9 @@
 #include "behaviortree_cpp/utils/shared_library.h"
 #include "behaviortree_ros2/plugins.hpp"
 
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
 namespace panther_manager::behavior_tree_utils
 {
 
@@ -112,6 +115,57 @@ inline std::vector<float> convertFromString<std::vector<float>>(StringView str)
   return output;
 }
 
+/**
+ * @brief Converts a string to a PoseStamped message.
+ *
+ * The string format should be "roll,pitch,yaw,x,y,z,frame_id" where:
+ *  - roll, pitch, yaw: Euler angles in radians.
+ *  - x, y, z: Position coordinates.
+ *  - frame_id: Coordinate frame ID (string).
+ *
+ * @param str The string to convert.
+ * @return geometry_msgs::msg::PoseStamped The converted PoseStamped message.
+ *
+ * @throw BT::RuntimeError Throws if the input is invalid or cannot be parsed.
+ */
+template <>
+inline geometry_msgs::msg::PoseStamped convertFromString<geometry_msgs::msg::PoseStamped>(
+  StringView str)
+{
+  auto parts = splitString(str, ';');
+
+  if (parts.size() != 7) {
+    throw BT::RuntimeError(
+      "Invalid input for PoseStamped. Expected 7 values: x;y;z;roll;pitch;yaw;frame_id");
+  }
+
+  geometry_msgs::msg::PoseStamped pose_stamped;
+
+  try {
+    // Position (x, y, z)
+    pose_stamped.pose.position.x = convertFromString<double>(parts[0]);
+    pose_stamped.pose.position.y = convertFromString<double>(parts[1]);
+    pose_stamped.pose.position.z = convertFromString<double>(parts[2]);
+
+    // Orientation (R,P,Y -> Quaternion)
+    double roll = convertFromString<double>(parts[3]);
+    double pitch = convertFromString<double>(parts[4]);
+    double yaw = convertFromString<double>(parts[5]);
+    tf2::Quaternion quaternion;
+    quaternion.setRPY(roll, pitch, yaw);
+    pose_stamped.pose.orientation = tf2::toMsg(quaternion);
+
+    // Frame ID and current time
+    pose_stamped.header.frame_id = convertFromString<std::string>(parts[6]);
+    pose_stamped.header.stamp = rclcpp::Clock().now();
+
+  } catch (const std::exception & e) {
+    throw BT::RuntimeError("Failed to convert string to PoseStamped: " + std::string(e.what()));
+  }
+
+  return pose_stamped;
+}
+
 }  // namespace BT
 
 #endif  // PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_

panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp

@@ -36,7 +36,8 @@ class CallSetBoolService : public BT::RosServiceNode<std_srvs::srv::SetBool>
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({BT::InputPort<bool>("data", "true / false value")});
+    return providedBasicPorts(
+      {BT::InputPort<bool>("data", "Boolean value to send with the service request.")});
   }
 
   virtual bool setRequest(typename Request::SharedPtr & request) override;

panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp

@@ -36,11 +36,11 @@ class CallSetLedAnimationService : public BT::RosServiceNode<panther_msgs::srv::
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({
-      BT::InputPort<unsigned>("id", "animation ID"),
-      BT::InputPort<std::string>("param", "optional parameter"),
-      BT::InputPort<bool>("repeating", "indicates if animation should repeat"),
-    });
+    return providedBasicPorts(
+      {BT::InputPort<unsigned>("id", "Animation ID to trigger."),
+       BT::InputPort<std::string>("param", "Optional animation parameter."),
+       BT::InputPort<bool>(
+         "repeating", "Specifies whether the animation should repeated continuously.")});
   }
 
   virtual bool setRequest(typename Request::SharedPtr & request) override;

panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp

@@ -48,20 +48,31 @@ class DockRobot : public BT::RosActionNode<opennav_docking_msgs::action::DockRob
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({
-      BT::InputPort<bool>("use_dock_id", true, "Whether to use the dock's ID or dock pose fields"),
-      BT::InputPort<std::string>("dock_id", "Dock ID or name to use"),
-      BT::InputPort<std::string>("dock_type", "The dock plugin type, if using dock pose"),
-      BT::InputPort<float>(
-        "max_staging_time", 1000.0, "Maximum time to navigate to the staging pose"),
-      BT::InputPort<bool>(
-        "navigate_to_staging_pose", true, "Whether to autonomously navigate to staging pose"),
+    return providedBasicPorts(
+      {BT::InputPort<bool>(
+         "use_dock_id", true, "Determines whether to use the dock's ID or dock pose fields."),
+       BT::InputPort<std::string>(
+         "dock_id",
+         "Specifies the dock's ID or name from the dock database (used if 'use_dock_id' is true)."),
+       BT::InputPort<geometry_msgs::msg::PoseStamped>(
+         "dock_pose",
+         "Specifies the dock's pose (used if 'use_dock_id' is false). Format: "
+         "\"x;y;z;roll;pitch;yaw;frame_id\""),
+       BT::InputPort<std::string>(
+         "dock_type",
+         "Defines the type of dock being used when docking via pose. Not needed if only one dock "
+         "type is available."),
+       BT::InputPort<float>(
+         "max_staging_time", 120.0,
+         "Maximum time allowed (in seconds) for navigating to the dock's staging pose."),
+       BT::InputPort<bool>(
+         "navigate_to_staging_pose", true,
+         "Specifies whether the robot should autonomously navigate to the staging pose."),
 
-      BT::OutputPort<DockRobotActionResult::_error_code_type>(
-        "error_code", "Contextual error code, if any"),
-      BT::OutputPort<DockRobotActionResult::_num_retries_type>(
-        "num_retries", "Number of retries attempted"),
-    });
+       BT::OutputPort<DockRobotActionResult::_error_code_type>(
+         "error_code", "Returns an error code indicating the reason for failure, if any."),
+       BT::OutputPort<DockRobotActionResult::_num_retries_type>(
+         "num_retries", "Reports the number of retry attempts made during the docking process.")});
   }
 };
 

panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp

@@ -39,10 +39,8 @@ class ShutdownHostsFromFile : public ShutdownHosts
 
   static BT::PortsList providedPorts()
   {
-    return {
-      BT::InputPort<std::string>(
-        "shutdown_hosts_file", "global path to YAML file with hosts to shutdown"),
-    };
+    return {BT::InputPort<std::string>(
+      "shutdown_hosts_file", "Absolute path to a YAML file listing the hosts to shut down.")};
   }
 
 private:

panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp

@@ -38,14 +38,17 @@ class ShutdownSingleHost : public ShutdownHosts
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::string>("ip", "ip of the host to shutdown"),
-      BT::InputPort<std::string>("username", "user to log into while executing shutdown command"),
-      BT::InputPort<unsigned>("port", "SSH communication port"),
-      BT::InputPort<std::string>("command", "command to execute on shutdown"),
-      BT::InputPort<float>("timeout", "time in seconds to wait for host to shutdown"),
+      BT::InputPort<std::string>("ip", "IP address of the host to shut down."),
+      BT::InputPort<std::string>(
+        "username", "Username to use for logging in and executing the shutdown command."),
+      BT::InputPort<unsigned>("port", "SSH port used for communication (default is usually 22)."),
+      BT::InputPort<std::string>("command", "Shutdown command to execute on the remote host."),
+      BT::InputPort<float>(
+        "timeout", "Maximum time (in seconds) to wait for the host to shut down."),
       BT::InputPort<bool>(
-        "ping_for_success", "ping host until it is not available or timeout is reached"),
-    };
+        "ping_for_success",
+        "Whether to continuously ping the host until it becomes unreachable or the timeout is "
+        "reached.")};
   }
 
 private:

panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp

@@ -35,7 +35,7 @@ class SignalShutdown : public BT::SyncActionNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<std::string>("reason", "", "reason to shutdown robot"),
+      BT::InputPort<std::string>("reason", "", "Reason to shutdown robot."),
     };
   }
 

panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp

@@ -49,14 +49,17 @@ class UndockRobot : public BT::RosActionNode<opennav_docking_msgs::action::Undoc
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({
-      BT::InputPort<std::string>(
-        "dock_type", "The dock plugin type, if not previous instance used for docking"),
-      BT::InputPort<float>(
-        "max_undocking_time", 30.0, "Maximum time to get back to the staging pose"),
+    return providedBasicPorts(
+      {BT::InputPort<std::string>(
+         "dock_type",
+         "Specifies the dock plugin type to use for undocking. If empty, the previously used dock "
+         "type is assumed."),
+       BT::InputPort<float>(
+         "max_undocking_time", 30.0,
+         "Maximum allowable time (in seconds) to undock and return to the staging pose."),
 
-      BT::OutputPort<UndockRobotActionResult::_error_code_type>("error_code", "Error code"),
-    });
+       BT::OutputPort<UndockRobotActionResult::_error_code_type>(
+         "error_code", "Returns an error code if the undocking process fails.")});
   }
 };
 

panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp

@@ -45,7 +45,8 @@ class CheckBoolMsg : public BT::RosTopicSubNode<std_msgs::msg::Bool>
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({BT::InputPort<bool>("data", "state of data to accept a condition")});
+    return providedBasicPorts(
+      {BT::InputPort<bool>("data", "Specifies the expected state of the data field.")});
   }
 };
 

panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp

@@ -45,9 +45,16 @@ class CheckJoyMsg : public BT::RosTopicSubNode<sensor_msgs::msg::Joy>
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts(
-      {BT::InputPort<std::vector<float>>("axes", "state of axes to accept a condition"),
-       BT::InputPort<std::vector<int>>("buttons", "state of buttons to accept a condition"),
-       BT::InputPort<double>("timeout", 0.0, "max time delay to accept a condition")});
+      {BT::InputPort<std::vector<float>>(
+         "axes", "",
+         "Specifies the expected state of the axes field. An empty string (\"\") means the value "
+         "is ignored."),
+       BT::InputPort<std::vector<int>>(
+         "buttons", "",
+         "Specifies the expected state of the buttons field. An empty string (\"\") means the "
+         "value is ignored."),
+       BT::InputPort<double>(
+         "timeout", 0.0, "Maximum allowable time delay to accept the condition.")});
   }
 
 private: