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

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;
}
};
}