Behavior Tree - Weber-State-Submarine-Project/Submarine GitHub Wiki
This section provides an overview of the behavior tree (BT) implementation for autonomous navigation, highlighting its components and functionality.
The behavior tree controls the robot's navigation and mapping process by integrating sensor data, decision-making logic, and motor control actions. The tree handles user input, obstacle detection, turning, and map completion while ensuring robustness and modularity.
- Modularity: Each task or decision point is encapsulated in individual nodes, enabling easy maintenance and updates.
- Retry Mechanisms: Ensures robustness by retrying failed tasks until successful.
- Integration: Combines sensor inputs (IMU, sonar, map data) and control outputs (ESC commands) for seamless navigation.
-
WaitForSeconds: Delays the system startup to allow hardware and sensors to stabilize. -
StartPos: Extracts the starting position from the map to initialize localization.
-
CheckStart: Monitors for the "start" signal from the web interface. Navigation begins once the user initiates the process.
-
CheckFront: Ensures the path ahead is clear. If an obstacle is detected, the robot:- Executes a
Turn90Degreesaction to avoid the obstacle. - Stops the motors temporarily for safety.
- Executes a
-
AdjustAngle: Aligns the robot’s orientation with respect to walls for accurate forward motion. -
Forward: Commands the robot to move forward when the path is clear.
-
MapFinished: Checks if the robot has returned to the starting position, indicating map completion.
-
StopMotors: Ensures the robot halts safely at any point. -
Stop: Executes the final stopping sequence after completing navigation.
-
PublishLog: Logs significant events (e.g., user input, mapping completion) for debugging and record-keeping.
The following nodes form the building blocks of the behavior tree:
-
Front: Monitors front sonar data to detect obstacles. -
Side: Tracks the distance to the wall using side sonar data. -
Odom: Reads IMU data to calculate orientation and yaw. -
Start: Checks for the user's "start" command. -
Velocity: Captures velocity data for stopping decisions. -
Map: Retrieves map data for localization and map completion.
-
Turn90Degrees: Turns the robot 90 degrees to avoid obstacles. -
AdjustAngle: Aligns the robot’s orientation during navigation. -
AdjustDistance: Adjusts the robot's distance from the wall for accurate path following. -
Forward: Commands the robot to move forward. -
Stop: Gradually slows and stops the robot. -
StopMotors: Immediately halts motor movement. -
WaitForSeconds: Introduces delays for timing or stabilization.
-
CheckFront: Verifies the path ahead is clear. -
CheckStart: Ensures navigation starts only after receiving user input. -
MapFinished: Checks if the robot has completed its mapping task.
-
PublishLog: Logs messages to the web interface for monitoring.
The behavior tree executes in a loop, ticking each node to check its status (SUCCESS, FAILURE, or RUNNING). This modular execution ensures robustness and clear decision-making.
#include <tf2/LinearMath/Quaternion.h>
#include <iomanip>
#include <chrono>
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "behaviortree_ros2/bt_topic_sub_node.hpp"
#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_ros2/bt_action_node.hpp"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/wait.hpp"
#include "nav_msgs/srv/get_map.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "std_msgs/msg/float64.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav2_util/robot_utils.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "std_msgs/msg/string.hpp"
#include "std_msgs/msg/int32.hpp"
#include <thread>
#include "orientation_msg/msg/esc.hpp"
namespace chr = std::chrono;
using namespace BT;
using GetMap = nav_msgs::srv::GetMap;
using geometry_msgs::msg::Pose;
using namespace std::literals::chrono_literals;
using OccupancyGrid = nav_msgs::msg::OccupancyGrid;
int check_start = 0;
int pressed_stop = 0;
// SyncActionNode (synchronous action) with an input port.
/*
a SyncActionNode can only return SUCCESS or FAILURE
it cannot return RUNNING. so it is only good for simple, fast operations
*/
// SyncActionNode (synchronous action) with an input port.
class Front : public rclcpp::Node
{
public:
Front() : Node("front_node")
{
subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"/scan/front", 10,
std::bind(&Front::topic_callback, this, std::placeholders::_1));
}
float get_latest_distance() const
{
return latest_distance_;
}
private:
void topic_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
latest_distance_ = msg->ranges[0];
}
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
float latest_distance_;
};
class Side : public rclcpp::Node
{
public:
Side() : Node("side_node")
{
subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"/scan/left", 10,
std::bind(&Side::topic_callback, this, std::placeholders::_1));
}
float get_latest_distance() const
{
return latest_distance_;
}
private:
void topic_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
latest_distance_ = msg->ranges[0];
}
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
float latest_distance_;
};
class Odom : public rclcpp::Node
{
public:
Odom() : Node("odom_node")
{
subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
"/imu", 10,
std::bind(&Odom::topic_callback, this, std::placeholders::_1));
}
std::shared_ptr<sensor_msgs::msg::Imu> get_latest_imu() const
{
return latest_imu_;
}
private:
void topic_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
latest_imu_ = msg;
}
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscription_;
std::shared_ptr<sensor_msgs::msg::Imu> latest_imu_;
};
//check if start button has been pressed on the web UI
class Start: public rclcpp::Node
{
public:
Start() : Node("start_node")
{
subscription_ = this->create_subscription<std_msgs::msg::Int32>(
"/start", 10,
std::bind(&Start::topic_callback, this, std::placeholders::_1));
}
int get_latest_start() const
{
return latest_start_;
}
private:
void topic_callback(const std_msgs::msg::Int32::SharedPtr msg)
{
latest_start_ = msg->data;
}
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_;
int latest_start_ = 0;
};
class Velocity : public rclcpp::Node
{
public:
Velocity() : Node("velocity_node")
{
subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/odom", 10,
std::bind(&Velocity::topic_callback, this, std::placeholders::_1));
}
std::shared_ptr<nav_msgs::msg::Odometry> get_latest_velocity() const
{
return latest_velocity_;
}
private:
void topic_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
latest_velocity_ = msg;
}
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
std::shared_ptr<nav_msgs::msg::Odometry> latest_velocity_;
};
class Map : public rclcpp::Node
{
public:
Map()
: Node("map_node")
{
subscription_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>(
"/map", 10,
std::bind(&Map::topic_callback, this, std::placeholders::_1));
}
std::shared_ptr<nav_msgs::msg::OccupancyGrid> get_latest_map() const
{
return latest_map_;
}
private:
void topic_callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
{
latest_map_ = msg;
}
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr subscription_;
std::shared_ptr<nav_msgs::msg::OccupancyGrid> latest_map_;
};
class CheckFront : public SyncActionNode
{
public:
CheckFront(const std::string &name, const NodeConfig &config, std::shared_ptr<Front> front_node)
: SyncActionNode(name, config), front_node_(front_node)
{
}
static PortsList providedPorts()
{
return {};
}
NodeStatus tick() override
{
// May need to change min distance to fit hardware turn radius
if (front_node_->get_latest_distance() > 2.5)
{
std::cout << "Distance from front wall is good: " << front_node_->get_latest_distance() << "\n";
return NodeStatus::SUCCESS;
}
else
{
std::cout << "Front sonar distance threshold failed: " << front_node_->get_latest_distance() << "\n";
return NodeStatus::FAILURE;
}
}
private:
std::shared_ptr<Front> front_node_;
};
class CheckStart : public SyncActionNode
{
public:
CheckStart(const std::string &name, const NodeConfig &config, std::shared_ptr<Start> start_node)
: SyncActionNode(name, config), start_node_(start_node)
{
}
static PortsList providedPorts()
{
return {};
}
NodeStatus tick() override
{
int start = start_node_->get_latest_start();
if (start == 0) {
std::cout << "Waiting for user to start navigation\n";
return NodeStatus::FAILURE;
} else if (start == 1) {
std::cout << "User started navigation\n";
return NodeStatus::SUCCESS;
} else {
std::cout << "Error, unable to get user input from website\n";
return NodeStatus::FAILURE;
}
}
private:
std::shared_ptr<Start> start_node_;
};
class Turn90Degrees : public SyncActionNode
{
public:
Turn90Degrees(const std::string& name, const NodeConfig& config, std::shared_ptr<Odom> odom_node)
: SyncActionNode(name, config),
node_(rclcpp::Node::make_shared("turn_90_degrees_node")),
odom_node_(odom_node)
{
publisher_ = node_->create_publisher<orientation_msg::msg::Esc>("/esc_topic", 10);
turn_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/turning", 10);
}
static PortsList providedPorts()
{
return {};
}
NodeStatus tick() override
{
auto msg = odom_node_->get_latest_imu();
if (!msg) {
return NodeStatus::FAILURE;
}
auto current_orientation = msg->orientation;
tf2::Quaternion current_quaternion(current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w);
// Convert the current orientation quaternion to roll, pitch, yaw
double roll, pitch, yaw;
tf2::Matrix3x3(current_quaternion).getRPY(roll, pitch, yaw);
// Calculate the target yaw by subtracting pi/2 from the current yaw
if (target_yaw == 0) {
if(yaw < -M_PI_4 && yaw > -3*M_PI_4){
target_yaw = -M_PI;
}
else if(yaw > M_PI_4 && yaw < 3*M_PI_4){
target_yaw = 0;
}
else if(std::abs(yaw) <= M_PI_4){
target_yaw = -M_PI_2;
}
else if(std::abs(yaw) >= 3*M_PI_4){
target_yaw = M_PI_2;
}
}
//enable the behavior tree to start checking if we are back at the start pos,
//this is an easy way to prevent it from detecting that we are at the start pos on starting navigation
check_start = 1;
// Calculate the angle difference between the current yaw and the target yaw
double angle_difference = std::atan2(std::sin(yaw - target_yaw), std::cos(yaw - target_yaw));
if(std::fabs(angle_difference) < angle_tolerance_) {
// Corrects the drift from the turn by full turning left for a short time
auto esc = orientation_msg::msg::Esc();
esc.motor_selection = "Left";
esc.power_percentage = -20;
publisher_->publish(esc);
esc.motor_selection = "Right";
esc.power_percentage = 20;
publisher_->publish(esc);
std::cout << "Turn Completed\n";
target_yaw = 0;
return NodeStatus::SUCCESS;
}
// turning right, left motor forward, right motor reverse, may need to adjust speeds
else{
auto esc = orientation_msg::msg::Esc();
esc.motor_selection = "Left";
esc.power_percentage = 20;
publisher_->publish(esc);
esc.motor_selection = "Right";
esc.power_percentage = -20;
publisher_->publish(esc);
turn_msg.data = 1;
turn_publisher_->publish(turn_msg);
std::cout << "Current angle: "<<yaw<<", target angle: " << target_yaw << "\n";
std::cout << "Angle Difference: " << angle_difference << "\n";
return NodeStatus::FAILURE;
}
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<orientation_msg::msg::Esc>::SharedPtr publisher_;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr turn_publisher_;
std::shared_ptr<Odom> odom_node_;
double target_yaw = 0;
std_msgs::msg::Int32 turn_msg;
const double angle_tolerance_ = 0.4; // radians
};
class AdjustAngle : public SyncActionNode
{
public:
AdjustAngle(const std::string& name, const NodeConfig& config, std::shared_ptr<Odom> odom_node)
: SyncActionNode(name, config),
node_(rclcpp::Node::make_shared("adjust_angle_node")),
odom_node_(odom_node)
{
publisher_ = node_->create_publisher<orientation_msg::msg::Esc>("/esc_topic", 10);
}
// It is mandatory to define this STATIC method.
static PortsList providedPorts()
{
return {};
}
NodeStatus tick() override
{
auto msg = odom_node_->get_latest_imu();
if (!msg) {
return NodeStatus::FAILURE;
}
auto current_orientation = msg->orientation;
tf2::Quaternion current_quaternion(current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w);
// Convert the current orientation quaternion to roll, pitch, yaw
double roll, pitch, yaw;
tf2::Matrix3x3(current_quaternion).getRPY(roll, pitch, yaw);
// Determine target yaw
if(yaw < -M_PI_4 && yaw > -3*M_PI_4){
target_yaw = -M_PI_2;
}
else if(yaw > M_PI_4 && yaw < 3*M_PI_4){
target_yaw = M_PI_2;
}
else if(std::abs(yaw) <= M_PI_4){
target_yaw = 0;
}
else if(std::abs(yaw) >= 3*M_PI_4){
target_yaw = M_PI;
}
// Calculate the angle difference between the current yaw and the target yaw
double angle_difference = std::atan2(std::sin(yaw - target_yaw), std::cos(yaw - target_yaw));
if(std::fabs(angle_difference) <= angle_tolerance_) {
//start going streight again, may need to adjust speeds
auto esc = orientation_msg::msg::Esc();
esc.motor_selection = "Left";
esc.power_percentage = 20;
publisher_->publish(esc);
esc.motor_selection = "Right";
esc.power_percentage = 20;
publisher_->publish(esc);
return NodeStatus::SUCCESS;
}
// The target yaw is to the left of the current yaw so adjust right
else if(angle_difference < 0){
auto esc = orientation_msg::msg::Esc();
esc.motor_selection = "Left";
esc.power_percentage = 20;
publisher_->publish(esc);
esc.motor_selection = "Right";
esc.power_percentage = 30;
publisher_->publish(esc);
std::cout << "Adjusting left, Current angle: "<<yaw<<", target angle: " << target_yaw << "\n";
std::cout << "Angle Difference: " << angle_difference << "\n";
return NodeStatus::FAILURE;
}
else{
auto esc = orientation_msg::msg::Esc();
esc.motor_selection = "Left";
esc.power_percentage = 30;
publisher_->publish(esc);
esc.motor_selection = "Right";
esc.power_percentage = 20;
publisher_->publish(esc);
std::cout << "Adjusting right, Current angle: "<<yaw<<", target angle: " << target_yaw << "\n";
std::cout << "Angle Difference: " << angle_difference << "\n";
return NodeStatus::FAILURE;
}
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<orientation_msg::msg::Esc>::SharedPtr publisher_;
std::shared_ptr<Odom> odom_node_;
double target_yaw = 0;
std_msgs::msg::Int32 turn_msg;
const double angle_tolerance_ = 0.2; // radians
};
class AdjustDistance : public SyncActionNode
{
public:
AdjustDistance(const std::string& name, const NodeConfig& config, std::shared_ptr<Side> side_node)
: SyncActionNode(name, config),
node_(rclcpp::Node::make_shared("adjust_distance_node")),
side_node_(side_node)
{
publisher_ = node_->create_publisher<orientation_msg::msg::Esc>("/esc_topic", 10);
}
// It is mandatory to define this STATIC method.
static PortsList providedPorts()
{
return {};
}
NodeStatus tick() override
{
auto dist = side_node_->get_latest_distance();
auto msg = orientation_msg::msg::Esc();
if(dist < 1)
{
std::cout <<"Distance from left wall: " << dist <<"\n";
std::cout << "Adjusting right\n";
msg.motor_selection = "Left";
msg.power_percentage = 20;
publisher_->publish(msg);
msg.motor_selection = "Right";
msg.power_percentage = 0;
publisher_->publish(msg);
return NodeStatus::FAILURE;
}
else if(dist > 3)
{
std::cout <<"Distance from left wall: " << dist <<"\n";
std::cout << "Adjusting left\n";
msg.motor_selection = "Left";
msg.power_percentage = 0;
publisher_->publish(msg);
msg.motor_selection = "Right";
msg.power_percentage = 20;
publisher_->publish(msg);
return NodeStatus::FAILURE;
}
else
{
std::cout << "Distance from side wall is good: "<< dist <<"\n";
msg.motor_selection = "Left";
msg.power_percentage = 20;
publisher_->publish(msg);
msg.motor_selection = "Right";
msg.power_percentage = 20;
publisher_->publish(msg);
return NodeStatus::SUCCESS;
}
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<orientation_msg::msg::Esc>::SharedPtr publisher_;
std::shared_ptr<Side> side_node_;
};
class Forward : public SyncActionNode
{
public:
Forward(const std::string& name, const NodeConfig& config)
: SyncActionNode(name, config),
node_(rclcpp::Node::make_shared("forward_node"))
{
publisher_ = node_->create_publisher<orientation_msg::msg::Esc>("/esc_topic", 10);
}
// It is mandatory to define this STATIC method.
static PortsList providedPorts()
{
return {};
}
NodeStatus tick() override
{
// may need to adjust motor speed
std::cout << "Moving Forward\n";
auto msg = orientation_msg::msg::Esc();
msg.motor_selection = "Left";
msg.power_percentage = 20;
publisher_->publish(msg);
msg.motor_selection = "Right";
msg.power_percentage = 20;
publisher_->publish(msg);
return NodeStatus::SUCCESS;
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<orientation_msg::msg::Esc>::SharedPtr publisher_;
};
class Stop : public SyncActionNode
{
public:
Stop(const std::string& name, const NodeConfig& config, std::shared_ptr<Velocity> velocity_node)
: SyncActionNode(name, config),
node_(rclcpp::Node::make_shared("stop_node")),
velocity_node_(velocity_node)
{
publisher_ = node_->create_publisher<orientation_msg::msg::Esc>("/esc_topic", 10);
publisher3_ = node_->create_publisher<std_msgs::msg::Int32>("/done", 10);
}
static PortsList providedPorts()
{
return {};
}
NodeStatus tick() override
{
//turn done on and off to have the octomap interpolate data once
std_msgs::msg::Int32 val3;
val3.data = 1;
publisher3_->publish(val3);
val3.data = 0;
publisher3_->publish(val3);
auto esc = orientation_msg::msg::Esc();
auto velocity_msg = velocity_node_->get_latest_velocity();
if (!velocity_msg) {
return NodeStatus::FAILURE;
}
double velocity = velocity_msg->twist.twist.linear.x;
if(std::abs(velocity) > 0.1){
std::cout<<"Slowing Down\n";
esc.motor_selection = "Left";
esc.power_percentage = -20;
publisher_->publish(esc);
esc.motor_selection = "Right";
esc.power_percentage = -20;
publisher_->publish(esc);
return NodeStatus::FAILURE;
}
else{
std::cout<<"Stop Complete";
esc.motor_selection = "Left";
esc.power_percentage = 0;
publisher_->publish(esc);
esc.motor_selection = "Right";
esc.power_percentage = 0;
publisher_->publish(esc);
return NodeStatus::SUCCESS;
}
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<orientation_msg::msg::Esc>::SharedPtr publisher_;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher3_;
std::shared_ptr<Velocity> velocity_node_;
};
class SaySomething : public SyncActionNode
{
public:
// If your Node has ports, you must use this constructor signature
SaySomething(const std::string& name, const NodeConfig& config)
: SyncActionNode(name, config)
{ }
// It is mandatory to define this STATIC method.
static PortsList providedPorts()
{
// This action has a single input port called "message"
return { InputPort<std::string>("message") };
}
// Override the virtual function tick()
NodeStatus tick() override
{
Expected<std::string> msg = getInput<std::string>("message");
// Check if expected is valid. If not, throw its error
if (!msg)
{
throw RuntimeError("missing required input [message]: ",
msg.error() );
}
// use the method value() to extract the valid message.
std::cout << "Robot says: " << msg.value() << std::endl;
return NodeStatus::SUCCESS;
}
};
class MapFinished : public SyncActionNode
{
public:
MapFinished(const std::string& name, const NodeConfig& config, std::shared_ptr<Map> map_node)
: SyncActionNode(name, config),
node_(rclcpp::Node::make_shared("stop_node")),
tf_buffer_(std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME)),
tf_listener_(tf_buffer_),
map_node_(map_node)
{
publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/done", 10);
turn_publisher_ = node_->create_publisher<std_msgs::msg::Int32>("/turning", 10);
// Initialize the starting position
starting_position_ = Pose();
}
static PortsList providedPorts()
{
return {
InputPort<Pose>("starting_position")
};
}
NodeStatus tick() override
{
turn_msg.data = 0;
turn_publisher_->publish(turn_msg);
// Get the inputs
auto map_input = map_node_->get_latest_map();
auto starting_position_input = getInput<Pose>("starting_position");
std_msgs::msg::Int32 val;
val.data = 0;
if (!map_input)
{
throw RuntimeError("missing required input [map]: ");
}
if (!starting_position_input)
{
throw RuntimeError("missing required input [starting_position]: ", starting_position_input.error());
}
// Update the starting position
starting_position_ = starting_position_input.value();
// Get the current pose of the robot
geometry_msgs::msg::PoseStamped current_pose;
// Wait for the transform to be available
if(!tf_buffer_.canTransform("map", "base_link", tf2::TimePointZero, tf2::durationFromSec(1.0)))
{
std::cout << "Waiting for transform...\n";
publisher_->publish(val);
return NodeStatus::FAILURE;
}
if(nav2_util::getCurrentPose(current_pose, tf_buffer_, "map", "base_link", tf2::Duration(30s).count()))
{
// current_pose now contains the pose of the robot in the map frame
// You can use current_pose for your purposes here
std::cout << "Current pose retrieved successfully.\n";
}
else
{
// Handle the case where the current pose could not be retrieved
std::cout << "Failed to retrieve current pose.\n";
publisher_->publish(val);
return NodeStatus::FAILURE;
}
// Check if the current position is approximately equal to the starting position
if (isApproximatelyEqual(current_pose.pose, starting_position_) && check_start == 1)
{
val.data = 1;
std::cout << "We are back at the starting position!\n";
publisher_->publish(val);
return NodeStatus::SUCCESS;
}
else
{
std::cout << "Current Pose X: "<< current_pose.pose.position.x << " Y: " << current_pose.pose.position.y << "\nStart POS X: "<< starting_position_.position.x << " Y: " << starting_position_.position.y <<"\n";
publisher_->publish(val);
return NodeStatus::FAILURE;
}
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr publisher_;
Pose starting_position_;
rclcpp::Clock::SharedPtr clock_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
std::shared_ptr<Map> map_node_;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr turn_publisher_;
std_msgs::msg::Int32 turn_msg;
bool isApproximatelyEqual(const Pose& pos1, const Pose& pos2)
{
// Define a small threshold for position comparison
const double THRESHOLD = 1; // meters
// Use the TF2 library to calculate the difference between the two poses
tf2::Transform tf1, tf2;
tf2::fromMsg(pos1, tf1);
tf2::fromMsg(pos2, tf2);
// Calculate the distance between the two positions
double distance = tf2::tf2Distance(tf1.getOrigin(), tf2.getOrigin());
std::cout << "Distance to start pos: "<<distance<<"\n";
// Check if the distance is within the threshold
return distance < THRESHOLD;
}
};
class WaitForSeconds : public StatefulActionNode
{
public:
WaitForSeconds(const std::string& name, const NodeConfig& config)
: StatefulActionNode(name, config)
{}
static PortsList providedPorts()
{
return{ InputPort<float>("seconds") };
}
NodeStatus onStart() override;
NodeStatus onRunning() override;
void onHalted() override;
private:
float _seconds;
chr::system_clock::time_point _completion_time;
};
NodeStatus WaitForSeconds::onStart()
{
if ( !getInput<float>("seconds", _seconds))
{
throw RuntimeError("missing required input [seconds]");
}
printf("[ WaitForSeconds: ] seconds = %f\n",_seconds);
_completion_time = chr::system_clock::now() + chr::milliseconds(int(_seconds*1000));
return NodeStatus::RUNNING;
}
NodeStatus WaitForSeconds::onRunning()
{
std::this_thread::sleep_for(chr::milliseconds(1000));
if(chr::system_clock::now() >= _completion_time)
{
std::cout << "[ WaitForSeconds: FINISHED ]" << std::endl;
return NodeStatus::SUCCESS;
}
return NodeStatus::RUNNING;
}
void WaitForSeconds::onHalted()
{
printf("[ WaitForSeconds: ABORTED ]");
}
class StartPos : public SyncActionNode
{
public:
StartPos(const std::string& name, const NodeConfig& config, std::shared_ptr<Map> map_node)
: SyncActionNode(name, config),
map_node_(map_node)
{
// Initialize the starting position
starting_position_ = Pose();
}
static PortsList providedPorts()
{
return {OutputPort<Pose>("starting_position")};
}
NodeStatus tick() override
{
auto latest_map = map_node_->get_latest_map();
if (!latest_map) {
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "No map received!");
return NodeStatus::FAILURE;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Processing the map!");
// Extract the starting position from the map metadata
starting_position_.position.x = latest_map->info.origin.position.x;
starting_position_.position.y = latest_map->info.origin.position.y;
// Set the output starting position
setOutput("starting_position", starting_position_);
return NodeStatus::SUCCESS;
}
private:
std::shared_ptr<Map> map_node_;
Pose starting_position_;
};
class PublishLog : public SyncActionNode
{
public:
PublishLog(const std::string& name, const NodeConfig& config)
: SyncActionNode(name, config),
node_(rclcpp::Node::make_shared("publish_log_node"))
{
publisher_ = node_->create_publisher<std_msgs::msg::String>("/web_message", 10);
}
// It is mandatory to define this STATIC method.
static PortsList providedPorts()
{
return {InputPort<std::string>("message")};
}
NodeStatus tick() override
{
Expected<std::string> msg = getInput<std::string>("message");
auto now = std::chrono::system_clock::now();
auto now_c = std::chrono::system_clock::to_time_t(now);
std::stringstream ss;
if(msg.value() == "place holder"){
if(pressed_stop){
ss << std::put_time(std::localtime(&now_c), "%F %T") << " - User canceled navigation";
std_msgs::msg::String message;
message.data = ss.str();
publisher_->publish(message);
return NodeStatus::SUCCESS;
}
else{
ss << std::put_time(std::localtime(&now_c), "%F %T") << " - The submarine has completed mapping!";
std_msgs::msg::String message;
message.data = ss.str();
publisher_->publish(message);
return NodeStatus::SUCCESS;
}
}
else{
ss << std::put_time(std::localtime(&now_c), "%F %T") << msg.value();
std_msgs::msg::String message;
message.data = ss.str();
publisher_->publish(message);
return NodeStatus::SUCCESS;
}
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};
class StopMotors : public SyncActionNode
{
public:
StopMotors(const std::string& name, const NodeConfig& config)
: SyncActionNode(name, config),
node_(rclcpp::Node::make_shared("stop_motors_node"))
{
publisher_ = node_->create_publisher<orientation_msg::msg::Esc>("/esc_topic", 10);
}
// It is mandatory to define this STATIC method.
static PortsList providedPorts()
{
return {};
}
NodeStatus tick() override
{
auto esc = orientation_msg::msg::Esc();
esc.motor_selection = "Left";
esc.power_percentage = 0;
publisher_->publish(esc);
esc.motor_selection = "Right";
esc.power_percentage = 0;
publisher_->publish(esc);
return NodeStatus::SUCCESS;
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<orientation_msg::msg::Esc>::SharedPtr publisher_;
};
static const char* xml_text = R"(
<root BTCPP_format="4" >
<BehaviorTree ID="MainTree">
<Sequence>
<WaitForSeconds seconds="10"/>
<StartPos starting_position="{starting_pos}"/>
<!--Loop until user presses start on the website-->
<RetryUntilSuccessful num_attempts="20000">
<Sequence>
<WaitForSeconds seconds="1"/>
<CheckStart/>
</Sequence>
</RetryUntilSuccessful>
<PublishLog message=" - User pressed start, navigation has begun."/>
<RetryUntilSuccessful num_attempts="50000000">
<Sequence>
<Fallback name="turn">
<CheckFront/>
<Inverter>
<RetryUntilSuccessful num_attempts="200">
<Sequence>
<WaitForSeconds seconds="0.001"/>
<Turn90Degrees/>
</Sequence>
</RetryUntilSuccessful>
</Inverter>
<Inverter>
<WaitForSeconds seconds="0.001"/>
</Inverter>
<Inverter>
<StopMotors/>
</Inverter>
<WaitForSeconds seconds="5"/>
</Fallback>
<RetryUntilSuccessful num_attempts="200">
<Fallback>
<MapFinished starting_position="{starting_pos}"/>
<Inverter>
<CheckStart/>
</Inverter>
<Inverter>
<CheckFront/>
</Inverter>
<Sequence>
<WaitForSeconds seconds="0.001"/>
<!--<AdjustDistance/>-->
<AdjustAngle/>
</Sequence>
</Fallback>
</RetryUntilSuccessful>
<Forward/>
<Fallback>
<Inverter>
<CheckStart/>
</Inverter>
<MapFinished starting_position="{starting_pos}"/>
</Fallback>
</Sequence>
</RetryUntilSuccessful>
<RetryUntilSuccessful num_attempts = "200">
<Sequence>
<WaitForSeconds seconds="0.1"/>
<Stop/>
</Sequence>
</RetryUntilSuccessful>
<PublishLog message="place holder"/>
</Sequence>
</BehaviorTree>
</root>
)";
void spin_node(std::shared_ptr<rclcpp::Node> node) {
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
executor.spin();
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto front_node = std::make_shared<Front>();
std::thread front_spin(spin_node, front_node);
auto side_node = std::make_shared<Side>();
std::thread side_spin(spin_node, side_node);
auto odom_node = std::make_shared<Odom>();
std::thread odom_spin(spin_node, odom_node);
auto start_node = std::make_shared<Start>();
std::thread start_spin(spin_node, start_node);
auto velocity_node = std::make_shared<Velocity>();
std::thread velocity_spin(spin_node, velocity_node);
auto map_node = std::make_shared<Map>();
std::thread map_spin(spin_node, map_node);
// Initialize the BehaviorTreeFactory
BehaviorTreeFactory factory;
factory.registerNodeType<WaitForSeconds>("WaitForSeconds");
factory.registerNodeType<SaySomething>("SaySomething");
factory.registerNodeType<MapFinished>("MapFinished",map_node);
factory.registerNodeType<Turn90Degrees>("Turn90Degrees",odom_node);
//factory.registerNodeType<AdjustDistance>("AdjustDistance",side_node);
factory.registerNodeType<AdjustAngle>("AdjustAngle",odom_node);
factory.registerNodeType<Forward>("Forward");
factory.registerNodeType<Stop>("Stop",velocity_node);
factory.registerNodeType<PublishLog>("PublishLog");
factory.registerNodeType<CheckFront>("CheckFront",front_node);
factory.registerNodeType<CheckStart>("CheckStart",start_node);
factory.registerNodeType<StartPos>("StartPos",map_node);
factory.registerNodeType<StopMotors>("StopMotors");
// Create the behavior tree from the XML text
auto tree = factory.createTreeFromText(xml_text);
// Behavior tree execution loop
NodeStatus status = tree.tickOnce();
while(status == NodeStatus::RUNNING)
{
tree.sleep(std::chrono::milliseconds(100));
status = tree.tickOnce();
}
rclcpp::shutdown();
front_spin.join();
side_spin.join();
odom_spin.join();
start_spin.join();
velocity_spin.join();
map_spin.join();
return 0;
}
- Modularity: Each behavior is encapsulated in its own node, making it reusable and easy to modify.
- Fallback Mechanism: Provides robustness by ensuring alternative actions are taken when primary ones fail.
- Clarity: The hierarchical structure improves readability and debugging.
- Scalability: Additional behaviors can be incorporated without affecting the existing logic.
This behavior tree ensures smooth navigation and mapping by combining conditional checks, obstacle avoidance, and user interaction in a structured and efficient manner.