Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Added mg_bt

Browse Source
This commit is contained in:
Pimpest
2025-04-11 12:10:02 +02:00
parent f064bcfc69
commit 2d0f8279c0
15 changed files with 604 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
mg_bt/src/mg_bt.cpp Normal file
View File

@ -0,0 +1,23 @@
#include <iostream>
#include "mg_tree_executor.hpp"
int main(const int argc, const char** argv) {
rclcpp::init(argc, argv);
std::cout << "Starting up Behavior Tree Executor" << std::endl;
rclcpp::NodeOptions options;
auto bt_exec_server = std::make_shared<mg::MgTreeExecutor>(options);
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
executor.add_node(bt_exec_server->node());
executor.spin();
executor.remove_node(bt_exec_server->node());
rclcpp::shutdown();
return 0;
}

59
mg_bt/src/mg_tree_executor.cpp Normal file
View File

@ -0,0 +1,59 @@
#include "mg_tree_executor.hpp"
#include "behaviortree_cpp/xml_parsing.h"
#include "tree_nodes/calib.hpp"
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/rotate.hpp"
#include "tree_nodes/zero.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/LinearMath/Transform.h"
#include "tf2/convert.h"
namespace mg {
MgTreeExecutor::MgTreeExecutor(const rclcpp::NodeOptions opts) : BT::TreeExecutionServer(opts) {
executeRegistration();
std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
const std::function<std::pair<glm::vec2, double>()> func = std::bind(&MgTreeExecutor::position, this);
}
void MgTreeExecutor::onTreeCreated(BT::Tree& tree) { logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree); }
void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
factory.registerNodeType<mg::RotateNode>("RotateNode", node());
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
}
std::pair<glm::vec2, double> MgTreeExecutor::position() {
double x = NAN;
double y = NAN;
double theta;
glm::vec2 pos;
auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero);
tf2::Transform t;
tf2::convert(tmsg.transform, t);
t.getBasis().getRPY(x, y, theta);
auto vec3 = tmsg.transform.translation;
pos = glm::dvec2(vec3.x, vec3.y);
return { pos, theta };
}
std::optional<std::string> MgTreeExecutor::onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) {
(void)status;
logger_cout_.reset();
if (was_cancelled) {
std::cout << "Behavior tree was cancled" << std::endl;
}
return std::nullopt;
}
}

25
mg_bt/src/mg_tree_executor.hpp Normal file
View File

@ -0,0 +1,25 @@
#pragma once
#include "behaviortree_ros2/tree_execution_server.hpp"
#include "behaviortree_cpp/loggers/bt_cout_logger.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "glm/glm.hpp"
namespace mg {
class MgTreeExecutor : public BT::TreeExecutionServer {
public:
MgTreeExecutor(const rclcpp::NodeOptions opts);
void onTreeCreated(BT::Tree& tree) override;
void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override;
std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override;
std::pair<glm::vec2, double> position();
private:
std::shared_ptr<BT::StdCoutLogger> logger_cout_;
tf2_ros::Buffer::SharedPtr tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
}

44
mg_bt/src/tree_nodes/calib.hpp Normal file
View File

@ -0,0 +1,44 @@
#pragma once
#include "behaviortree_ros2/bt_service_node.hpp"
#include "mg_msgs/srv/send_double.hpp"
#include "glm/glm.hpp"
namespace mg {
using PosFuncSig = std::function<std::pair<glm::vec2, double>()>;
class CalibWidthNode : public BT::RosServiceNode<mg_msgs::srv::SendDouble> {
public:
CalibWidthNode(const std::string& name,
const BT::NodeConfig& conf,
const BT::RosNodeParams& params,
const PosFuncSig pos_query) :
BT::RosServiceNode<mg_msgs::srv::SendDouble>(name, conf, params), pos_query_(pos_query) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("PreviousWidth"),
BT::InputPort<int>("Count", 1, {}),
BT::OutputPort<double>("NewWidth") });
}
bool setRequest(typename Request::SharedPtr& request) override {
auto [pos, theta] = pos_query_();
double width = getInput<double>("PreviousWidth").value();
int count = getInput<int>("Count").value();
double new_width = width * (1 + (theta / (2 * M_PI * count)));
RCLCPP_INFO(logger(), "Setting width to: %lf", new_width);
request->set__data(count);
setOutput("Count", new_width);
return true;
}
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
return BT::NodeStatus::SUCCESS;
}
private:
const PosFuncSig pos_query_;
};
}

70
mg_bt/src/tree_nodes/move_point.hpp Normal file
View File

@ -0,0 +1,70 @@
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/move_point.hpp"
namespace mg {
class MovePointNode : public BT::RosActionNode<mg_msgs::action::MovePoint> {
public:
MovePointNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosActionNode<mg_msgs::action::MovePoint>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
BT::InputPort<double>("y_goal"),
BT::InputPort<double>("tolerance"),
BT::InputPort<double>("max_wheel_speed"),
BT::InputPort<double>("max_angular"),
BT::InputPort<double>("max_vel"),
BT::InputPort<double>("pos_mult"),
BT::InputPort<double>("ornt_mult"),
BT::InputPort<double>("t_ornt_mult"),
BT::InputPort<double>("obst_mult_a"),
BT::InputPort<double>("obst_mult_b"),
BT::InputPort<double>("obst_mult_c"),
BT::InputPort<std::string>("IgnoreList", "", {}) });
}
bool setGoal(Goal& goal) override {
auto x_goal = getInput<double>("x_goal");
auto y_goal = getInput<double>("y_goal");
auto tolerance = getInput<double>("tolerance");
auto max_wheel_speed = getInput<double>("max_wheel_speed");
auto max_angular = getInput<double>("max_angular");
auto max_vel = getInput<double>("max_vel");
auto pos_mult = getInput<double>("pos_mult");
auto ornt_mult = getInput<double>("ornt_mult");
auto t_ornt_mult = getInput<double>("t_ornt_mult");
auto obst_mult_a = getInput<double>("obst_mult_a");
auto obst_mult_b = getInput<double>("obst_mult_b");
auto obst_mult_c = getInput<double>("obst_mult_c");
goal.x = x_goal.value();
goal.y = y_goal.value();
goal.tolerance = tolerance.value_or(goal.tolerance);
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
goal.max_angular = max_angular.value_or(goal.max_angular);
goal.max_vel = max_vel.value_or(goal.max_vel);
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
return true;
}
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
BT::NodeStatus::FAILURE;
}
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
return BT::NodeStatus::FAILURE;
}
};
}

67
mg_bt/src/tree_nodes/rotate.hpp Normal file
View File

@ -0,0 +1,67 @@
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/rotate.hpp"
namespace mg {
class RotateNode : public BT::RosActionNode<mg_msgs::action::Rotate> {
public:
RotateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("angle"),
BT::InputPort<double>("tolerance"),
BT::InputPort<double>("max_wheel_speed"),
BT::InputPort<double>("max_angular"),
BT::InputPort<double>("max_vel"),
BT::InputPort<double>("pos_mult"),
BT::InputPort<double>("ornt_mult"),
BT::InputPort<double>("t_ornt_mult"),
BT::InputPort<double>("obst_mult_a"),
BT::InputPort<double>("obst_mult_b"),
BT::InputPort<double>("obst_mult_c"),
BT::InputPort<std::string>("IgnoreList", "", {}) });
}
bool setGoal(Goal& goal) override {
auto angle = getInput<double>("angle");
auto tolerance = getInput<double>("tolerance");
auto max_wheel_speed = getInput<double>("max_wheel_speed");
auto max_angular = getInput<double>("max_angular");
auto max_vel = getInput<double>("max_vel");
auto pos_mult = getInput<double>("pos_mult");
auto ornt_mult = getInput<double>("ornt_mult");
auto t_ornt_mult = getInput<double>("t_ornt_mult");
auto obst_mult_a = getInput<double>("obst_mult_a");
auto obst_mult_b = getInput<double>("obst_mult_b");
auto obst_mult_c = getInput<double>("obst_mult_c");
goal.angle = angle.value();
goal.tolerance = tolerance.value_or(goal.tolerance);
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
goal.max_angular = max_angular.value_or(goal.max_angular);
goal.max_vel = max_vel.value_or(goal.max_vel);
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
return true;
}
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
BT::NodeStatus::FAILURE;
}
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
return BT::NodeStatus::FAILURE;
}
};
}

20
mg_bt/src/tree_nodes/zero.hpp Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include "behaviortree_ros2/bt_service_node.hpp"
#include "std_srvs/srv/empty.hpp"
namespace mg {
class ZeroNode : public BT::RosServiceNode<std_srvs::srv::Empty> {
public:
ZeroNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosServiceNode<std_srvs::srv::Empty>(name, conf, params) { }
bool setRequest(typename Request::SharedPtr&) override { return true; }
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
return BT::NodeStatus::SUCCESS;
}
};
}