From 73645dc2d414b59eb8b1d9405b742acc2a61a629 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 21 Oct 2024 22:08:54 +0200 Subject: [PATCH] Improve descriptions --- .../panther_manager/behavior_tree_utils.hpp | 57 +++++++++++++++++++ .../action/call_set_bool_service_node.hpp | 3 +- .../call_set_led_animation_service_node.hpp | 10 ++-- .../plugins/action/dock_robot_node.hpp | 37 +++++++----- .../action/shutdown_hosts_from_file_node.hpp | 6 +- .../action/shutdown_single_host_node.hpp | 17 +++--- .../plugins/action/signal_shutdown_node.hpp | 2 +- .../plugins/action/undock_robot_node.hpp | 17 +++--- .../plugins/condition/check_bool_msg.hpp | 3 +- .../plugins/condition/check_joy_msg.hpp | 6 +- .../decorator/tick_after_timeout_node.hpp | 3 +- 11 files changed, 118 insertions(+), 43 deletions(-) diff --git a/panther_manager/include/panther_manager/behavior_tree_utils.hpp b/panther_manager/include/panther_manager/behavior_tree_utils.hpp index 655a004f..184215cf 100644 --- a/panther_manager/include/panther_manager/behavior_tree_utils.hpp +++ b/panther_manager/include/panther_manager/behavior_tree_utils.hpp @@ -26,6 +26,11 @@ #include "behaviortree_cpp/utils/shared_library.h" #include "behaviortree_ros2/plugins.hpp" +#include +#include +#include +#include + namespace panther_manager::behavior_tree_utils { @@ -112,6 +117,58 @@ inline std::vector convertFromString>(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( + StringView str) +{ + auto parts = splitString(str, ';'); + + if (parts.size() != 7) { + throw BT::RuntimeError( + "Invalid input for PoseStamped. Expected 7 values: roll;pitch;yaw;x;y;z;frame_id"); + } + + geometry_msgs::msg::PoseStamped pose_stamped; + + try { + double roll = convertFromString(parts[0]); + double pitch = convertFromString(parts[1]); + double yaw = convertFromString(parts[2]); + + // Orientation form R,P,Y to Quaternion + tf2::Quaternion quaternion; + quaternion.setRPY(roll, pitch, yaw); + pose_stamped.pose.orientation = tf2::toMsg(quaternion); + + // Position (x, y, z) + pose_stamped.pose.position.x = convertFromString(parts[3]); + pose_stamped.pose.position.y = convertFromString(parts[4]); + pose_stamped.pose.position.z = convertFromString(parts[5]); + + // Frame ID and current time + pose_stamped.header.frame_id = parts[6].to_string(); + 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_ diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp index adafe1d4..96f40482 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_bool_service_node.hpp @@ -36,7 +36,8 @@ class CallSetBoolService : public BT::RosServiceNode static BT::PortsList providedPorts() { - return providedBasicPorts({BT::InputPort("data", "true / false value")}); + return providedBasicPorts( + {BT::InputPort("data", "Boolean value to send with the service request.")}); } virtual bool setRequest(typename Request::SharedPtr & request) override; diff --git a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp index 38a915a5..a0dcff1e 100644 --- a/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/call_set_led_animation_service_node.hpp @@ -36,11 +36,11 @@ class CallSetLedAnimationService : public BT::RosServiceNode("id", "animation ID"), - BT::InputPort("param", "optional parameter"), - BT::InputPort("repeating", "indicates if animation should repeat"), - }); + return providedBasicPorts( + {BT::InputPort("id", "Animation ID to trigger."), + BT::InputPort("param", "Optional animation parameter."), + BT::InputPort( + "repeating", "Specifies whether the animation should repeated continuously.")}); } virtual bool setRequest(typename Request::SharedPtr & request) override; diff --git a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp index 58933c06..670807bb 100644 --- a/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/dock_robot_node.hpp @@ -48,20 +48,31 @@ class DockRobot : public BT::RosActionNode("use_dock_id", true, "Whether to use the dock's ID or dock pose fields"), - BT::InputPort("dock_id", "Dock ID or name to use"), - BT::InputPort("dock_type", "The dock plugin type, if using dock pose"), - BT::InputPort( - "max_staging_time", 1000.0, "Maximum time to navigate to the staging pose"), - BT::InputPort( - "navigate_to_staging_pose", true, "Whether to autonomously navigate to staging pose"), + return providedBasicPorts( + {BT::InputPort( + "use_dock_id", true, "Determines whether to use the dock's ID or dock pose fields."), + BT::InputPort( + "dock_id", + "Specifies the dock's ID or name from the dock database (used if 'use_dock_id' is true)."), + BT::InputPort( + "dock_pose", + "Specifies the dock's pose (used if 'use_dock_id' is false). Format: " + "\"roll,pitch,yaw,x,y,z,frame_id\""), + BT::InputPort( + "dock_type", + "Defines the type of dock being used when docking via pose. Not needed if only one dock " + "type is available."), + BT::InputPort( + "max_staging_time", 120.0, + "Maximum time allowed (in seconds) for navigating to the dock's staging pose."), + BT::InputPort( + "navigate_to_staging_pose", true, + "Specifies whether the robot should autonomously navigate to the staging pose."), - BT::OutputPort( - "error_code", "Contextual error code, if any"), - BT::OutputPort( - "num_retries", "Number of retries attempted"), - }); + BT::OutputPort( + "error_code", "Returns an error code indicating the reason for failure, if any."), + BT::OutputPort( + "num_retries", "Reports the number of retry attempts made during the docking process.")}); } }; diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp index 949565f6..c9eaa79e 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_hosts_from_file_node.hpp +++ b/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( - "shutdown_hosts_file", "global path to YAML file with hosts to shutdown"), - }; + return {BT::InputPort( + "shutdown_hosts_file", "Absolute path to a YAML file listing the hosts to shut down.")}; } private: diff --git a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp b/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp index 630db650..680e849d 100644 --- a/panther_manager/include/panther_manager/plugins/action/shutdown_single_host_node.hpp +++ b/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("ip", "ip of the host to shutdown"), - BT::InputPort("username", "user to log into while executing shutdown command"), - BT::InputPort("port", "SSH communication port"), - BT::InputPort("command", "command to execute on shutdown"), - BT::InputPort("timeout", "time in seconds to wait for host to shutdown"), + BT::InputPort("ip", "IP address of the host to shut down."), + BT::InputPort( + "username", "Username to use for logging in and executing the shutdown command."), + BT::InputPort("port", "SSH port used for communication (default is usually 22)."), + BT::InputPort("command", "Shutdown command to execute on the remote host."), + BT::InputPort( + "timeout", "Maximum time (in seconds) to wait for the host to shut down."), BT::InputPort( - "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: diff --git a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp b/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp index 7bc9709d..abee0f74 100644 --- a/panther_manager/include/panther_manager/plugins/action/signal_shutdown_node.hpp +++ b/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("reason", "", "reason to shutdown robot"), + BT::InputPort("reason", "", "Reason to shutdown robot."), }; } diff --git a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp index 238fa5f3..820ad3c2 100644 --- a/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp +++ b/panther_manager/include/panther_manager/plugins/action/undock_robot_node.hpp @@ -49,14 +49,17 @@ class UndockRobot : public BT::RosActionNode( - "dock_type", "The dock plugin type, if not previous instance used for docking"), - BT::InputPort( - "max_undocking_time", 30.0, "Maximum time to get back to the staging pose"), + return providedBasicPorts( + {BT::InputPort( + "dock_type", + "Specifies the dock plugin type to use for undocking. If empty, the previously used dock " + "type is assumed."), + BT::InputPort( + "max_undocking_time", 30.0, + "Maximum allowable time (in seconds) to undock and return to the staging pose."), - BT::OutputPort("error_code", "Error code"), - }); + BT::OutputPort( + "error_code", "Returns an error code if the undocking process fails.")}); } }; diff --git a/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp index a2d953c4..fbe52a88 100644 --- a/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp +++ b/panther_manager/include/panther_manager/plugins/condition/check_bool_msg.hpp @@ -45,7 +45,8 @@ class CheckBoolMsg : public BT::RosTopicSubNode static BT::PortsList providedPorts() { - return providedBasicPorts({BT::InputPort("data", "state of data to accept a condition")}); + return providedBasicPorts( + {BT::InputPort("data", "Specifies the expected state of the data field.")}); } }; diff --git a/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp b/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp index dded1c29..6b343811 100644 --- a/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp +++ b/panther_manager/include/panther_manager/plugins/condition/check_joy_msg.hpp @@ -45,9 +45,9 @@ class CheckJoyMsg : public BT::RosTopicSubNode static BT::PortsList providedPorts() { return providedBasicPorts( - {BT::InputPort>("axes", "state of axes to accept a condition"), - BT::InputPort>("buttons", "state of buttons to accept a condition"), - BT::InputPort("timeout", 0.0, "max time delay to accept a condition")}); + {BT::InputPort>("axes", "", "Specifies the expected state of the axes field. An empty string (\"\") means the value is ignored."), + BT::InputPort>("buttons", "", "Specifies the expected state of the buttons field. An empty string (\"\") means the value is ignored."), + BT::InputPort("timeout", 0.0, "Maximum allowable time delay to accept the condition."}); } private: diff --git a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp index 20a273e5..7661abf9 100644 --- a/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp +++ b/panther_manager/include/panther_manager/plugins/decorator/tick_after_timeout_node.hpp @@ -31,7 +31,8 @@ class TickAfterTimeout : public BT::DecoratorNode static BT::PortsList providedPorts() { - return {BT::InputPort("timeout", "time in s to wait before ticking child again")}; + return {BT::InputPort( + "timeout", "Time in seconds to wait before ticking the child node again")}; } private: