Class IsStuckCondition

Inheritance Relationships

Base Type

  • public BT::ConditionNode

Class Documentation

class IsStuckCondition : public BT::ConditionNode

A BT::ConditionNode that tracks robot odometry and returns SUCCESS if robot is stuck somewhere and FAILURE otherwise.

Public Functions

IsStuckCondition(const std::string &condition_name, const BT::NodeConfiguration &conf)

A constructor for nav2_behavior_tree::IsStuckCondition.

Parameters:
  • condition_name – Name for the XML tag for this node

  • conf – BT node configuration

IsStuckCondition() = delete
~IsStuckCondition() override

A destructor for nav2_behavior_tree::IsStuckCondition.

void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg)

Callback function for odom topic.

Parameters:

msg – Shared pointer to nav_msgs::msg::Odometry::SharedPtr message

BT::NodeStatus tick() override

The main override required by a BT action.

Returns:

BT::NodeStatus Status of tick execution

void logStuck(const std::string &msg) const

Function to log status when robot is stuck/free.

void updateStates()

Function to approximate acceleration from the odom history.

bool isStuck()

Detect if robot bumped into something by checking for abnormal deceleration.

Returns:

bool true if robot is stuck, false otherwise

Public Static Functions

static inline BT::PortsList providedPorts()

Creates list of BT ports.

Returns:

BT::PortsList Containing node-specific ports