Class ModeSwitch

Defined in File mode_switch.h

Class Documentation

class ModeSwitch

Class managing planning mode switches in the HATEB local planner.

ModeSwitch implements a behavior tree-based decision system for switching between different planning modes (single band, dual band, velocity obstacles, etc.) based on the current situation. It processes information about nearby agents, goals, and passage types to determine the most appropriate planning strategy.

Public Functions

ModeSwitch(): Default constructor.

~ModeSwitch() = default: Default destructor.

void initialize(ros::NodeHandle &nh, std::string &xml_path, std::shared_ptr<agents::Agents> &agents_ptr, std::shared_ptr<Backoff> &backoff_ptr)

Initializes the mode switch with required components.

Parameters:

nh – ROS node handle
xml_path – Path to the behavior tree XML description
agents_ptr – Pointer to the agents management class
backoff_ptr – Pointer to the backoff behavior handler

BT::NodeStatus tickBT()

Executes one tick of the behavior tree.

Returns:: Status of the behavior tree after the tick

void resetBT(): Resets the behavior tree to its initial state.

inline BT::Tree *BTree()

Gets a pointer to the behavior tree.

Returns:: Pointer to the behavior tree instance

hateb_local_planner::PlanningMode tickAndGetMode()

Ticks the behavior tree and returns the resulting planning mode.

Returns:: The selected planning mode after the tree evaluation

Private Functions

void registerNodes(): Registers custom nodes with the behavior tree factory.

void agentsInfoCB(const agent_path_prediction::AgentsInfo &info_msg)

Callback for processing agent information updates.

Parameters:: info_msg – Message containing updated agent information

void goalMoveBaseCB(const move_base_msgs::MoveBaseActionGoal &goal_msg)

Callback for processing new navigation goals.

Parameters:: goal_msg – Message containing the new goal

void resultMoveBaseCB(const move_base_msgs::MoveBaseActionResult &result_msg)

Callback for processing navigation results.

Parameters:: result_msg – Message containing the navigation result

void passageCB(const cohan_msgs::PassageType &passage_msg)

Callback for processing passage type information.

Parameters:: passage_msg – Message containing passage classification

void updateMode(int duration = 0)

Updates the current planning mode.

Parameters:: duration – Optional duration parameter for the update

inline void printTreeStatus(const BT::TreeNode *node, int level = 0)

Private Members

bool goal_reached_: Flag indicating if current goal was reached.

bool initialized_: Flag indicating if class was properly initialized.

bool goal_update_: Flag indicating if goal was updated.

ros::NodeHandle nh_: ROS node handle.

ros::Subscriber agents_info_sub_: Subscriber for agent information.

ros::Subscriber goal_sub_: Subscriber for navigation goals.

ros::Subscriber result_sub_: Subscriber for navigation results.

ros::Subscriber passage_detect_sub_: Subscriber for passage detection.

ros::Publisher planning_mode_pub_: Publisher for current planning mode.

ros::ServiceServer backoff_srv_: Service server for backoff requests.

geometry_msgs::PoseStamped goal_: Current navigation goal.

agent_path_prediction::AgentsInfo agents_info_: Current agent information.

actionlib_msgs::GoalStatusArray result_msg_: Latest navigation result.

BT::BehaviorTreeFactory bhv_factory_: Factory for creating BT nodes.

BT::Tree bhv_tree_: The behavior tree instance.

std::mutex pub_mutex_: Mutex for thread-safe publishing.

std::string name_: Name of this node.

hateb_local_planner::PlanningMode plan_mode_: Current planning mode.

ModeInfo mode_info_: Detailed mode information.

std::shared_ptr<Backoff> backoff_ptr_: Pointer to backoff behavior handler.

std::string ns_: Namespace of the node.

std::string agents_info_sub_topic_: Topic for agents information.

std::string goal_sub_topic_: Topic for robot goal.

std::string result_sub_topic_: Topic for robot goal result.

std::string passage_sub_topic_: Topic for passage detection (from invisible humans)