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

First set of behaviors

Browse Source
This commit is contained in:
Pimpest
2025-05-08 12:10:13 +02:00
parent da3d1d9d15
commit bce371dba2
13 changed files with 710 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
mg_bt/src/mg_tree_executor.cpp
View File

@ -2,9 +2,13 @@
#include "behaviortree_cpp/xml_parsing.h"
#include "tree_nodes/calib.hpp"
#include "tree_nodes/choose_can.hpp"
#include "tree_nodes/choose_tactic.hpp"
#include "tree_nodes/i2c.hpp"
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/move_local.hpp"
#include "tree_nodes/move_global.hpp"
#include "tree_nodes/look_at.hpp"
#include "tree_nodes/rotate.hpp"
#include "tree_nodes/side_pub.hpp"
#include "tree_nodes/set_pos.hpp"
@ -28,11 +32,15 @@ namespace mg {
void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
factory.registerNodeType<mg::MoveGlobalNode>("MoveGlobal", node());
factory.registerNodeType<mg::RotateNode>("RotateNode", node());
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
factory.registerNodeType<mg::SendPoseNode>("SendPose", node());
factory.registerNodeType<mg::LookAtNode>("LookAtNode", node());
factory.registerNodeType<mg::MoveLocalNode>("MoveLocal", node(), [this]() { return this->position(); });
factory.registerNodeType<mg::CanChooser>("CanChooser");
factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
}

49
mg_bt/src/tree_nodes/choose_can.hpp Normal file
View File

@ -0,0 +1,49 @@
#include "behaviortree_cpp/action_node.h"
namespace mg {
class CanChooser : public BT::SyncActionNode {
struct cans {
double x;
double y;
int orientation;
};
public:
CanChooser(const std::string& name, const BT::NodeConfig config) : BT::SyncActionNode(name, config) { }
static BT::PortsList providedPorts() {
// This action has a single input port called "message"
return { BT::InputPort<std::string>("canlist"),
BT::OutputPort<double>("x_loc"),
BT::OutputPort<double>("y_loc"),
BT::OutputPort<int>("orient") };
}
// You must override the virtual function tick()
BT::NodeStatus tick() override {
constexpr std::array<cans, 10> c{ { { 0.825, 1.425, 3 },
{ 0.35, 0.4, 2 },
{ 0.35, 1.325, 2 },
{ 0.775, 0.55, 1 },
{ 2.225, 0.55, 1 },
{ 2.65, 0.4, 0 },
{ 2.65, 0.4, 0 },
{ 2.175, 1.425, 3 },
{ 1.1, 0.65, 3 },
{ 1.9, 0.65, 3 } } };
int idx = 0;
// Todo iterate through all cans and use a passthrough function that returns current can state
std::istringstream ss(getInput<std::string>("canlist").value());
ss >> idx;
setOutput<double>("x_loc", c[idx].x);
setOutput<double>("y_loc", c[idx].y);
setOutput<int>("orient", c[idx].orientation);
return BT::NodeStatus::SUCCESS;
}
};
}

50
mg_bt/src/tree_nodes/look_at.hpp Normal file
View File

@ -0,0 +1,50 @@
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/look_at.hpp"
namespace mg {
class LookAtNode : public BT::RosActionNode<mg_msgs::action::LookAt> {
public:
LookAtNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosActionNode<mg_msgs::action::LookAt>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x"),
BT::InputPort<double>("y"),
BT::InputPort<double>("tolerance", 0.001, {}),
BT::InputPort<double>("max_wheel_speed", 3, {}),
BT::InputPort<double>("max_angular", 3.14, {})
});
}
bool setGoal(Goal& goal) override {
auto x = getInput<double>("x");
auto y = getInput<double>("y");
auto tolerance = getInput<double>("tolerance");
auto max_wheel_speed = getInput<double>("max_wheel_speed");
auto max_angular = getInput<double>("max_angular");
goal.x = x.value();
goal.y = y.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);
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;
}
};
}

58
mg_bt/src/tree_nodes/move_global.hpp Normal file
View File

@ -0,0 +1,58 @@
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/move_global.hpp"
namespace mg {
class MoveGlobalNode : public BT::RosActionNode<mg_msgs::action::MoveGlobal> {
public:
MoveGlobalNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosActionNode<mg_msgs::action::MoveGlobal>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
BT::InputPort<double>("y_goal"),
BT::InputPort<double>("tolerance", 0.05, {}),
BT::InputPort<double>("max_wheel_speed", 6.0, {}),
BT::InputPort<double>("max_angular", 3.14, {}),
BT::InputPort<double>("max_vel", 4.0, {}),
BT::InputPort<double>("ornt_mult", 3.0, {}) });
}
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");
goal.x.push_back(x_goal.value());
goal.y.push_back(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);
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/move_local.hpp Normal file
View File

@ -0,0 +1,67 @@
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/move_point.hpp"
#include "glm/glm.hpp"
namespace mg {
class MoveLocalNode : public BT::RosActionNode<mg_msgs::action::MovePoint> {
using PosFuncSig = std::function<std::pair<glm::vec2, double>()>;
PosFuncSig pos_query_;
public:
MoveLocalNode(const std::string& name,
const BT::NodeConfig& conf,
const BT::RosNodeParams& params,
const PosFuncSig pos_query) :
BT::RosActionNode<mg_msgs::action::MovePoint>(name, conf, params), pos_query_(pos_query) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
BT::InputPort<double>("y_goal"),
BT::InputPort<double>("tolerance", 0.001, {}),
BT::InputPort<double>("max_wheel_speed", 3, {}),
BT::InputPort<double>("max_angular", 3.14, {}),
BT::InputPort<double>("max_vel", 2, {}),
BT::InputPort<double>("pos_mult", 15, {}),
BT::InputPort<double>("ornt_mult", 4, {}) });
}
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 [pos, theta] = pos_query_();
goal.x = x_goal.value() + pos.x;
goal.y = y_goal.value() + pos.y;
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);
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;
}
};
}

24
mg_bt/src/tree_nodes/move_point.hpp
View File

@ -13,17 +13,11 @@ namespace mg {
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", "", {}) });
BT::InputPort<double>("tolerance", 0.001, {}),
BT::InputPort<double>("max_wheel_speed", 3, {}),
BT::InputPort<double>("max_angular", 3.14, {}),
BT::InputPort<double>("max_vel", 2, {}),
BT::InputPort<double>("ornt_mult", 4, {}) });
}
bool setGoal(Goal& goal) override {
@ -35,10 +29,6 @@ namespace mg {
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);
@ -47,10 +37,6 @@ namespace mg {
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;
}

32
mg_bt/src/tree_nodes/rotate.hpp
View File

@ -11,18 +11,12 @@ namespace mg {
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", "", {}) });
return providedBasicPorts({
BT::InputPort<double>("angle"),
BT::InputPort<double>("tolerance", 0.001, {}),
BT::InputPort<double>("max_wheel_speed", 3, {}),
BT::InputPort<double>("max_angular", 3.14, {}),
});
}
bool setGoal(Goal& goal) override {
@ -30,24 +24,10 @@ namespace mg {
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;
}