From 262775048d7a774d8946e1b20d06d76c85b48d05 Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Wed, 17 Mar 2021 19:08:57 +0530 Subject: [PATCH] Add doxygen coverage to nav2_behavior_tree Signed-off-by: Sarthak Mittal --- .../behavior_tree_engine.hpp | 47 ++++++++++- .../nav2_behavior_tree/bt_action_node.hpp | 79 +++++++++++++++---- .../nav2_behavior_tree/bt_action_server.hpp | 20 ++--- .../nav2_behavior_tree/bt_conversions.hpp | 20 +++++ .../nav2_behavior_tree/bt_service_node.hpp | 46 +++++++++-- .../plugins/action/back_up_action.hpp | 16 ++++ .../plugins/action/clear_costmap_service.hpp | 46 +++++++++++ .../action/compute_path_to_pose_action.hpp | 19 +++++ .../plugins/action/follow_path_action.hpp | 20 +++++ .../action/navigate_to_pose_action.hpp | 17 +++- ...initialize_global_localization_service.hpp | 8 ++ .../plugins/action/spin_action.hpp | 16 ++++ .../plugins/action/truncate_path_action.hpp | 21 ++++- .../plugins/action/wait_action.hpp | 17 +++- .../condition/distance_traveled_condition.hpp | 17 ++++ .../condition/goal_reached_condition.hpp | 30 +++++++ .../condition/goal_updated_condition.hpp | 17 ++++ .../initial_pose_received_condition.hpp | 4 + .../condition/is_battery_low_condition.hpp | 21 +++++ .../plugins/condition/is_stuck_condition.hpp | 38 +++++++++ .../condition/time_expired_condition.hpp | 18 ++++- .../transform_available_condition.hpp | 20 +++++ .../plugins/control/pipeline_sequence.hpp | 24 ++++++ .../plugins/control/recovery_node.hpp | 21 ++++- .../plugins/control/round_robin_node.hpp | 24 ++++++ .../plugins/decorator/distance_controller.hpp | 18 ++++- .../plugins/decorator/goal_updater_node.hpp | 22 +++++- .../plugins/decorator/rate_controller.hpp | 17 +++- .../plugins/decorator/single_trigger_node.hpp | 18 ++++- .../plugins/decorator/speed_controller.hpp | 28 ++++++- .../nav2_behavior_tree/ros_topic_logger.hpp | 18 +++++ 31 files changed, 702 insertions(+), 45 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 61816a9950..711a87110c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -29,37 +29,82 @@ namespace nav2_behavior_tree { +/** + * @enum nav2_behavior_tree::BtStatus + * @brief An enum class representing BT execution status + */ enum class BtStatus { SUCCEEDED, FAILED, CANCELED }; +/** + * @class nav2_behavior_tree::BehaviorTreeEngine + * @brief A class to create and handle behavior trees + */ class BehaviorTreeEngine { public: + /** + * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine + * @param plugin_libraries vector of BT plugin library names to load + */ explicit BehaviorTreeEngine(const std::vector & plugin_libraries); virtual ~BehaviorTreeEngine() {} + /** + * @brief Function to execute a BT at a specific rate + * @param tree BT to execute + * @param onLoop Function to execute on each iteration of BT execution + * @param cancelRequested Function to check if cancel was requested during BT execution + * @param loopTimeout Time period for each iteration of BT execution + * @return nav2_behavior_tree::BtStatus Status of BT execution + */ BtStatus run( BT::Tree * tree, std::function onLoop, std::function cancelRequested, std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10)); + /** + * @brief Function to create a BT from a XML string + * @param xml_string XML string representing BT + * @param blackboard Blackboard for BT + * @return BT::Tree Created behavior tree + */ BT::Tree createTreeFromText( const std::string & xml_string, BT::Blackboard::Ptr blackboard); + /** + * @brief Function to create a BT from an XML file + * @param file_path Path to BT XML file + * @param blackboard Blackboard for BT + * @return BT::Tree Created behavior tree + */ BT::Tree createTreeFromFile( const std::string & file_path, BT::Blackboard::Ptr blackboard); + /** + * @brief Add groot monitor to publish BT status changes + * @param tree BT to monitor + * @param publisher_port ZMQ publisher port for the Groot monitor + * @param server_port ZMQ server port for the Groot monitor + * @param max_msg_per_second Maximum number of messages that can be sent per second + */ void addGrootMonitoring( BT::Tree * tree, uint16_t publisher_port, uint16_t server_port, uint16_t max_msg_per_second = 25); + /** + * @brief Reset groot monitor + */ void resetGrootMonitor(); - // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state + /** + * @brief Function to explicitly reset all BT nodes to initial state + * @param root_node Pointer to BT root node + */ void haltAllActions(BT::TreeNode * root_node); protected: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 77aa8836d4..a63ac79037 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -26,10 +26,20 @@ namespace nav2_behavior_tree { +/** + * @brief Abstract class representing an action based BT node + * @tparam ActionT Type of action + */ template class BtActionNode : public BT::ActionNodeBase { public: + /** + * @brief A nav2_behavior_tree::BtActionNode constructor + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ BtActionNode( const std::string & xml_tag_name, const std::string & action_name, @@ -63,7 +73,10 @@ class BtActionNode : public BT::ActionNodeBase { } - // Create instance of an action server + /** + * @brief Create instance of an action client + * @param action_name Action name to create client for + */ void createActionClient(const std::string & action_name) { // Now that we have the ROS node to use, create the action client for this BT action @@ -74,8 +87,12 @@ class BtActionNode : public BT::ActionNodeBase action_client_->wait_for_action_server(); } - // Any subclass of BtActionNode that accepts parameters must provide a providedPorts method - // and call providedBasicPorts in it. + /** + * @brief Any subclass of BtActionNode that accepts parameters must provide a + * providedPorts method and call providedBasicPorts in it. + * @param addition Additional ports to add to BT port list + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedBasicPorts(BT::PortsList addition) { BT::PortsList basic = { @@ -87,6 +104,10 @@ class BtActionNode : public BT::ActionNodeBase return basic; } + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts({}); @@ -95,39 +116,54 @@ class BtActionNode : public BT::ActionNodeBase // Derived classes can override any of the following methods to hook into the // processing for the action: on_tick, on_wait_for_result, and on_success - // Could do dynamic checks, such as getting updates to values on the blackboard + /** + * @brief Function to perform some user-defined operation on tick + * Could do dynamic checks, such as getting updates to values on the blackboard + */ virtual void on_tick() { } - // There can be many loop iterations per tick. Any opportunity to do something after - // a timeout waiting for a result that hasn't been received yet + /** + * @brief Function to perform some user-defined operation after a timeout + * waiting for a result that hasn't been received yet + */ virtual void on_wait_for_result() { } - // Called upon successful completion of the action. A derived class can override this - // method to put a value on the blackboard, for example. + /** + * @brief Function to perform some user-defined operation upon successful + * completion of the action. Could put a value on the blackboard. + * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value + */ virtual BT::NodeStatus on_success() { return BT::NodeStatus::SUCCESS; } - // Called when a the action is aborted. By default, the node will return FAILURE. - // The user may override it to return another value, instead. + /** + * @brief Function to perform some user-defined operation whe the action is aborted. + * @return BT::NodeStatus Returns FAILURE by default, user may override return another value + */ virtual BT::NodeStatus on_aborted() { return BT::NodeStatus::FAILURE; } - // Called when a the action is cancelled. By default, the node will return SUCCESS. - // The user may override it to return another value, instead. + /** + * @brief Function to perform some user-defined operation when the action is cancelled. + * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value + */ virtual BT::NodeStatus on_cancelled() { return BT::NodeStatus::SUCCESS; } - // The main override required by a BT action + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override { // first step to be done only at the beginning of the Action @@ -178,8 +214,10 @@ class BtActionNode : public BT::ActionNodeBase } } - // The other (optional) override required by a BT action. In this case, we - // make sure to cancel the ROS2 action if it is still running. + /** + * @brief The other (optional) override required by a BT action. In this case, we + * make sure to cancel the ROS2 action if it is still running. + */ void halt() override { if (should_cancel_goal()) { @@ -197,6 +235,10 @@ class BtActionNode : public BT::ActionNodeBase } protected: + /** + * @brief Function to check if current goal should be cancelled + * @return bool True if current goal should be cancelled, false otherwise + */ bool should_cancel_goal() { // Shut the node down if it is currently running @@ -212,7 +254,9 @@ class BtActionNode : public BT::ActionNodeBase status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; } - + /** + * @brief Function to send new goal to action server + */ void on_new_goal_received() { goal_result_available_ = false; @@ -242,6 +286,9 @@ class BtActionNode : public BT::ActionNodeBase } } + /** + * @brief Function to increment recovery count on blackboard if this node wraps a recovery + */ void increment_recovery_count() { int recovery_count = 0; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 1042b59fc4..36be688285 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -59,48 +59,47 @@ class BtActionServer /** * @brief Configures member variables - * * Initializes action server for, builds behavior tree from xml file, * and calls user-defined onConfigure. - * @return true on SUCCESS and false on FAILURE + * @return bool true on SUCCESS and false on FAILURE */ bool on_configure(); /** * @brief Activates action server - * @return true on SUCCESS and false on FAILURE + * @return bool true on SUCCESS and false on FAILURE */ bool on_activate(); /** * @brief Deactivates action server - * @return true on SUCCESS and false on FAILURE + * @return bool true on SUCCESS and false on FAILURE */ bool on_deactivate(); /** * @brief Resets member variables - * @return true on SUCCESS and false on FAILURE + * @return bool true on SUCCESS and false on FAILURE */ bool on_cleanup(); /** * @brief Called when in shutdown state - * @return true on SUCCESS and false on FAILURE + * @return bool true on SUCCESS and false on FAILURE */ bool on_shutdown(); /** * @brief Replace current BT with another one * @param bt_xml_filename The file containing the new BT, uses default filename if empty - * @return true if the resulting BT correspond to the one in bt_xml_filename. false + * @return bool true if the resulting BT correspond to the one in bt_xml_filename. false * if something went wrong, and previous BT is maintained */ bool loadBehaviorTree(const std::string & bt_xml_filename = ""); /** * @brief Getter function for BT Blackboard - * @return shared pointer to current BT blackboard + * @return BT::Blackboard::Ptr Shared pointer to current BT blackboard */ BT::Blackboard::Ptr getBlackboard() const { @@ -109,7 +108,7 @@ class BtActionServer /** * @brief Getter function for current BT XML filename - * @return string + * @return string Containing current BT XML filename */ std::string getCurrentBTFilename() const { @@ -118,6 +117,7 @@ class BtActionServer /** * @brief Wrapper function to accept pending goal if a preempt has been requested + * @return Shared pointer to pending action goal */ const std::shared_ptr acceptPendingGoal() { @@ -126,6 +126,7 @@ class BtActionServer /** * @brief Wrapper function to get current goal + * @return Shared pointer to current action goal */ const std::shared_ptr getCurrentGoal() const { @@ -142,6 +143,7 @@ class BtActionServer /** * @brief Getter function for the current BT tree + * @return BT::Tree Current behavior tree */ BT::Tree getTree() const { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp index f16e61ec99..c109c4f0e2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_conversions.hpp @@ -30,6 +30,11 @@ namespace BT // in our BT XML files. They parse the strings in the XML into their corresponding // data type. +/** + * @brief Parse XML string to geometry_msgs::msg::Point + * @param key XML string + * @return geometry_msgs::msg::Point + */ template<> inline geometry_msgs::msg::Point convertFromString(const StringView key) { @@ -46,6 +51,11 @@ inline geometry_msgs::msg::Point convertFromString(const StringView key) } } +/** + * @brief Parse XML string to geometry_msgs::msg::Quaternion + * @param key XML string + * @return geometry_msgs::msg::Quaternion + */ template<> inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) { @@ -63,6 +73,11 @@ inline geometry_msgs::msg::Quaternion convertFromString(const StringView key) } } +/** + * @brief Parse XML string to geometry_msgs::msg::PoseStamped + * @param key XML string + * @return geometry_msgs::msg::PoseStamped + */ template<> inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) { @@ -85,6 +100,11 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) } } +/** + * @brief Parse XML string to std::chrono::milliseconds + * @param key XML string + * @return std::chrono::milliseconds + */ template<> inline std::chrono::milliseconds convertFromString(const StringView key) { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 8931783df6..97d95b953c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -26,10 +26,19 @@ namespace nav2_behavior_tree { +/** + * @brief Abstract class representing a service based BT node + * @tparam ServiceT Type of service + */ template class BtServiceNode : public BT::SyncActionNode { public: + /** + * @brief A nav2_behavior_tree::BtServiceNode constructor + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ BtServiceNode( const std::string & service_node_name, const BT::NodeConfiguration & conf) @@ -66,8 +75,12 @@ class BtServiceNode : public BT::SyncActionNode { } - // Any subclass of BtServiceNode that accepts parameters must provide a providedPorts method - // and call providedBasicPorts in it. + /** + * @brief Any subclass of BtServiceNode that accepts parameters must provide a + * providedPorts method and call providedBasicPorts in it. + * @param addition Additional ports to add to BT port list + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedBasicPorts(BT::PortsList addition) { BT::PortsList basic = { @@ -79,12 +92,19 @@ class BtServiceNode : public BT::SyncActionNode return basic; } + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts({}); } - // The main override required by a BT service + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override { on_tick(); @@ -92,12 +112,19 @@ class BtServiceNode : public BT::SyncActionNode return check_future(future_result); } - // Fill in service request with information if necessary + /** + * @brief Function to perform some user-defined operation on tick + * Fill in service request with information if necessary + */ virtual void on_tick() { } - // Check the future and decide the status of Behaviortree + /** + * @brief Check the future and decide the status of BT + * @param future_result shared_future of service response + * @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise + */ virtual BT::NodeStatus check_future( std::shared_future future_result) { @@ -116,13 +143,18 @@ class BtServiceNode : public BT::SyncActionNode return BT::NodeStatus::FAILURE; } - // An opportunity to do something after - // a timeout waiting for a result that hasn't been received yet + /** + * @brief Function to perform some user-defined operation after a timeout waiting + * for a result that hasn't been received yet + */ virtual void on_wait_for_result() { } protected: + /** + * @brief Function to increment recovery count on blackboard if this node wraps a recovery + */ void increment_recovery_count() { int recovery_count = 0; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp index e49dec78ab..8584daa910 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp @@ -23,16 +23,32 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp + */ class BackUpAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::BackUpAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ BackUpAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp index 8f44e3d803..0bb8ad0bb2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp @@ -25,26 +25,55 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::ClearEntireCostmap + */ class ClearEntireCostmapService : public BtServiceNode { public: + /** + * @brief A constructor for nav2_behavior_tree::ClearEntireCostmapService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ ClearEntireCostmapService( const std::string & service_node_name, const BT::NodeConfiguration & conf); + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ void on_tick() override; }; +/** + * @brief A nav2_behavior_tree::BtServiceNode class that + * wraps nav2_msgs::srv::ClearCostmapExceptRegion + */ class ClearCostmapExceptRegionService : public BtServiceNode { public: + /** + * @brief A constructor for nav2_behavior_tree::ClearCostmapExceptRegionService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ ClearCostmapExceptRegionService( const std::string & service_node_name, const BT::NodeConfiguration & conf); + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ void on_tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( @@ -56,15 +85,32 @@ class ClearCostmapExceptRegionService } }; +/** + * @brief A nav2_behavior_tree::BtServiceNode class that + * wraps nav2_msgs::srv::ClearCostmapAroundRobot + */ class ClearCostmapAroundRobotService : public BtServiceNode { public: + /** + * @brief A constructor for nav2_behavior_tree::ClearCostmapAroundRobotService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ ClearCostmapAroundRobotService( const std::string & service_node_name, const BT::NodeConfiguration & conf); + /** + * @brief The main override required by a BT service + * @return BT::NodeStatus Status of tick execution + */ void on_tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index 8539b98432..2528a5ce20 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -24,18 +24,37 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathToPose + */ class ComputePathToPoseAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ ComputePathToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; + /** + * @brief Function to perform some user-defined operation upon successful completion of the action + */ BT::NodeStatus on_success() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 951c948c8e..d3870b0b22 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -23,18 +23,38 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath + */ class FollowPathAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::FollowPathAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ FollowPathAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; + /** + * @brief Function to perform some user-defined operation after a timeout + * waiting for a result that hasn't been received yet + */ void on_wait_for_result() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp index 383dd3d14e..aa5f7a1e82 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp @@ -25,17 +25,32 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateToPose + */ class NavigateToPoseAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::NavigateToPoseAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ NavigateToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp index c241687261..8d90327f20 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp @@ -23,9 +23,17 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Empty + */ class ReinitializeGlobalLocalizationService : public BtServiceNode { public: + /** + * @brief A constructor for nav2_behavior_tree::ReinitializeGlobalLocalizationService + * @param service_node_name Service name this node creates a client for + * @param conf BT node configuration + */ ReinitializeGlobalLocalizationService( const std::string & service_node_name, const BT::NodeConfiguration & conf); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index 4120cad9c5..b3982d2631 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -23,16 +23,32 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Spin + */ class SpinAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::SpinAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ SpinAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp index f76b7df4d5..2604bcfea1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/truncate_path_action.hpp @@ -26,14 +26,25 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ActionNodeBase to shorten path by some distance + */ class TruncatePath : public BT::ActionNodeBase { public: + /** + * @brief A nav2_behavior_tree::TruncatePath constructor + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ TruncatePath( const std::string & xml_tag_name, const BT::NodeConfiguration & conf); - + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -44,7 +55,15 @@ class TruncatePath : public BT::ActionNodeBase } private: + /** + * @brief The other (optional) override required by a BT action. + */ void halt() override {} + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; double distance_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp index a6e753da2e..f452d24d32 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp @@ -23,17 +23,32 @@ namespace nav2_behavior_tree { +/** + * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait + */ class WaitAction : public BtActionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::WaitAction + * @param xml_tag_name Name for the XML tag for this node + * @param action_name Action name this node creates a client for + * @param conf BT node configuration + */ WaitAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf); + /** + * @brief Function to perform some user-defined operation on tick + */ void on_tick() override; - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp index 7bd9b8627a..6373a5600c 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp @@ -28,17 +28,34 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS every time the robot + * travels a specified distance and FAILURE otherwise + */ class DistanceTraveledCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::DistanceTraveledCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ DistanceTraveledCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); DistanceTraveledCondition() = delete; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp index fc69d53d33..a4d41437d8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp @@ -25,23 +25,50 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS when a specified goal + * is reached and FAILURE otherwise + */ class GoalReachedCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::GoalReachedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ GoalReachedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); GoalReachedCondition() = delete; + /** + * @brief A destructor for nav2_behavior_tree::GoalReachedCondition + */ ~GoalReachedCondition() override; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Function to read parameters and initialize class variables + */ void initialize(); + /** + * @brief Checks if the current robot pose lies within a given distance from the goal + * @return bool true when goal is reached, false otherwise + */ bool isGoalReached(); + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -52,6 +79,9 @@ class GoalReachedCondition : public BT::ConditionNode } protected: + /** + * @brief Cleanup function + */ void cleanup() {} diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 45a6704ef9..542183ff50 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -23,17 +23,34 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS when goal is + * updated on the blackboard and FAILURE otherwise + */ class GoalUpdatedCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::GoalUpdatedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ GoalUpdatedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); GoalUpdatedCondition() = delete; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return {}; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp index 9bed7e6ac4..2bb7d995b8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp @@ -19,6 +19,10 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS if initial pose + * has been received and FAILURE otherwise + */ BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp index 0bc30cd563..6d785f13c9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp @@ -27,17 +27,34 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that listens to a battery topic and + * returns SUCCESS when battery is low and FAILURE otherwise + */ class IsBatteryLowCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::IsBatteryLowCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ IsBatteryLowCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); IsBatteryLowCondition() = delete; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -50,6 +67,10 @@ class IsBatteryLowCondition : public BT::ConditionNode } private: + /** + * @brief Callback function for battery topic + * @param msg Shared pointer to sensor_msgs::msg::BatteryState message + */ void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg); rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp index e46f03cccc..48dbe5027b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp @@ -26,23 +26,61 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS + * if robot is stuck somewhere and FAILURE otherwise + */ class IsStuckCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::IsStuckCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ IsStuckCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); IsStuckCondition() = delete; + /** + * @brief A destructor for nav2_behavior_tree::IsStuckCondition + */ ~IsStuckCondition() override; + /** + * @brief Callback function for odom topic + * @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message + */ void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + + /** + * @brief Function to log status when robot is stuck/free + */ void logStuck(const std::string & msg) const; + + /** + * @brief Function to approximate acceleration from the odom history + */ void updateStates(); + + /** + * @brief Detect if robot bumped into something by checking for abnormal deceleration + * @return bool true if robot is stuck, false otherwise + */ bool isStuck(); + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() {return {};} private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp index ec40b9db43..9f8c47afab 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/time_expired_condition.hpp @@ -24,18 +24,34 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS every time a specified + * time period passes and FAILURE otherwise + */ class TimeExpiredCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::TimeExpiredCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ TimeExpiredCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); TimeExpiredCondition() = delete; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp index 1a624a4cd2..64572e21b7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/transform_available_condition.hpp @@ -26,19 +26,39 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::ConditionNode that returns SUCCESS if there is a valid transform + * between two specified frames and FAILURE otherwise + */ class TransformAvailableCondition : public BT::ConditionNode { public: + /** + * @brief A constructor for nav2_behavior_tree::TransformAvailableCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ TransformAvailableCondition( const std::string & condition_name, const BT::NodeConfiguration & conf); TransformAvailableCondition() = delete; + /** + * @brief A destructor for nav2_behavior_tree::TransformAvailableCondition + */ ~TransformAvailableCondition(); + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp index 666d119164..0384381d2a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pipeline_sequence.hpp @@ -55,13 +55,37 @@ namespace nav2_behavior_tree class PipelineSequence : public BT::ControlNode { public: + /** + * @brief A constructor for nav2_behavior_tree::PipelineSequence + * @param name Name for the XML tag for this node + */ explicit PipelineSequence(const std::string & name); + + /** + * @brief A constructor for nav2_behavior_tree::PipelineSequence + * @param name Name for the XML tag for this node + * @param config BT node configuration + */ PipelineSequence(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief The other (optional) override required by a BT action to reset node state + */ void halt() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() {return {};} protected: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + std::size_t last_child_ticked_ = 0; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp index 331a19d807..89c0cfa300 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/recovery_node.hpp @@ -35,13 +35,24 @@ namespace nav2_behavior_tree class RecoveryNode : public BT::ControlNode { public: + /** + * @brief A constructor for nav2_behavior_tree::RecoveryNode + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ RecoveryNode( const std::string & name, const BT::NodeConfiguration & conf); + /** + * @brief A destructor for nav2_behavior_tree::RecoveryNode + */ ~RecoveryNode() override = default; - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -54,7 +65,15 @@ class RecoveryNode : public BT::ControlNode unsigned int number_of_retries_; unsigned int retry_count_; + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + + /** + * @brief The other (optional) override required by a BT action to reset node state + */ void halt() override; }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp index 8ebc2ab26c..8c374ce6af 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/round_robin_node.hpp @@ -54,10 +54,34 @@ namespace nav2_behavior_tree class RoundRobinNode : public BT::ControlNode { public: + /** + * @brief A constructor for nav2_behavior_tree::RoundRobinNode + * @param name Name for the XML tag for this node + */ explicit RoundRobinNode(const std::string & name); + + /** + * @brief A constructor for nav2_behavior_tree::RoundRobinNode + * @param name Name for the XML tag for this node + * @param config BT node configuration + */ RoundRobinNode(const std::string & name, const BT::NodeConfiguration & config); + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + + /** + * @brief The other (optional) override required by a BT action to reset node state + */ void halt() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ static BT::PortsList providedPorts() {return {};} private: diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp index 299833f555..2d571597ea 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -27,14 +27,26 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that ticks its child every time the robot + * travels a specified distance + */ class DistanceController : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::DistanceController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ DistanceController( const std::string & name, const BT::NodeConfiguration & conf); - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -45,6 +57,10 @@ class DistanceController : public BT::DecoratorNode } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; rclcpp::Node::SharedPtr node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 1aba0ccd89..8c9af22012 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -28,14 +28,26 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that subscribes to a goal topic and updates + * the current goal on the blackboard + */ class GoalUpdater : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::GoalUpdater + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ GoalUpdater( const std::string & xml_tag_name, const BT::NodeConfiguration & conf); - + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -47,8 +59,16 @@ class GoalUpdater : public BT::DecoratorNode } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; + /** + * @brief Callback function for goal update topic + * @param msg Shared pointer to geometry_msgs::msg::PoseStamped message + */ void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg); rclcpp::Subscription::SharedPtr goal_sub_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp index 01cb268766..201f4868cf 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/rate_controller.hpp @@ -23,14 +23,25 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that ticks its child at a specified rate + */ class RateController : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::RateController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ RateController( const std::string & name, const BT::NodeConfiguration & conf); - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -39,6 +50,10 @@ class RateController : public BT::DecoratorNode } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; std::chrono::time_point start_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp index 1101acac3e..c16ef63efa 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp @@ -23,20 +23,36 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that triggers its child only once and returns FAILURE + * for every succeeding tick + */ class SingleTrigger : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::SingleTrigger + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ SingleTrigger( const std::string & name, const BT::NodeConfiguration & conf); - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return {}; } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; bool first_time_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 5af0429117..0feed97b60 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -28,14 +28,27 @@ namespace nav2_behavior_tree { +/** + * @brief A BT::DecoratorNode that ticks its child every at a rate proportional to + * the speed of the robot. If the robot travels faster, this node will tick its child at a + * higher frequency and reduce the tick frequency if the robot slows down + */ class SpeedController : public BT::DecoratorNode { public: + /** + * @brief A constructor for nav2_behavior_tree::SpeedController + * @param name Name for the XML tag for this node + * @param conf BT node configuration + */ SpeedController( const std::string & name, const BT::NodeConfiguration & conf); - // Any BT node that accepts parameters must provide a requiredNodeParameters method + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ static BT::PortsList providedPorts() { return { @@ -48,9 +61,16 @@ class SpeedController : public BT::DecoratorNode } private: + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ BT::NodeStatus tick() override; - // Scale the rate based speed + /** + * @brief Scale the rate based speed + * @return double Rate scaled by speed limits and clamped + */ inline double getScaledRate(const double & speed) { return std::max( @@ -59,7 +79,9 @@ class SpeedController : public BT::DecoratorNode max_rate_), min_rate_); } - // Update period based on current smoothed speed and reset timer + /** + * @brief Update period based on current smoothed speed and reset timer + */ inline void updatePeriod() { auto velocity = odom_smoother_->getTwist(); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp index 089cfe67b6..d71a57bc04 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp @@ -28,9 +28,17 @@ namespace nav2_behavior_tree { +/** + * @brief A class to publish BT logs on BT status change + */ class RosTopicLogger : public BT::StatusChangeLogger { public: + /** + * @brief A constructor for nav2_behavior_tree::RosTopicLogger + * @param ros_node Weak pointer to parent rclcpp::Node + * @param tree BT to monitor + */ RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree) : StatusChangeLogger(tree.rootNode()) { @@ -42,6 +50,13 @@ class RosTopicLogger : public BT::StatusChangeLogger rclcpp::QoS(10)); } + /** + * @brief Callback function which is called each time BT changes status + * @param timestamp Timestamp of BT status change + * @param node Node that changed status + * @param prev_status Previous status of the node + * @param status Current status of the node + */ void callback( BT::Duration timestamp, const BT::TreeNode & node, @@ -66,6 +81,9 @@ class RosTopicLogger : public BT::StatusChangeLogger toStr(status, true).c_str() ); } + /** + * @brief Clear log buffer if any + */ void flush() override { if (!event_log_.empty()) {