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

Add behavior for i2cnode and choosing side

Browse Source
This commit is contained in:
Pimpest
2025-05-07 21:06:34 +02:00
parent 0677b9568e
commit 07cc056853
8 changed files with 138 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mg_bt/src/mg_tree_executor.cpp
View File

@ -2,6 +2,7 @@
#include "behaviortree_cpp/xml_parsing.h"
#include "tree_nodes/calib.hpp"
#include "tree_nodes/choose_tactic.hpp"
#include "tree_nodes/i2c.hpp"
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/rotate.hpp"
@ -30,6 +31,7 @@ namespace mg {
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
}

22
mg_bt/src/tree_nodes/choose_tactic.hpp Normal file
View File

@ -0,0 +1,22 @@
#include "behaviortree_cpp/action_node.h"
namespace mg {
class TacticsChooser : public BT::SyncActionNode {
public:
TacticsChooser(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<int>("number"), BT::OutputPort<int>("tactic"), BT::OutputPort<bool>("IsBlue") };
}
// You must override the virtual function tick()
BT::NodeStatus tick() override {
const int num = getInput<int>("number").value();
setOutput<int>("tactic", (num + 1) / 2);
setOutput<bool>("IsBlue", num % 2);
return BT::NodeStatus::SUCCESS;
}
};
}

18
mg_bt/src/tree_nodes/i2c.hpp
View File

@ -1,13 +1,13 @@
#pragma once
#include "behaviortree_ros2/bt_service_node.hpp"
#include "mg_msgs/srv/i2c.hpp"
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/i2c.hpp"
namespace mg {
class I2cNode : public BT::RosServiceNode<mg_msgs::srv::I2c> {
class I2cNode : public BT::RosActionNode<mg_msgs::action::I2c> {
public:
I2cNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosServiceNode<mg_msgs::srv::I2c>(name, conf, params) { }
BT::RosActionNode<mg_msgs::action::I2c>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<int>("Address", 42, {}),
@ -15,14 +15,14 @@ namespace mg {
BT::OutputPort<int>("Result") });
}
bool setRequest(typename Request::SharedPtr& req) override {
req->addr = getInput<int>("Address").value_or(42);
req->data = getInput<int>("Result").value_or(0);
bool setGoal(Goal& req) override {
req.addr = getInput<int>("Address").value_or(42);
req.data = getInput<int>("Data").value_or(0);
return true;
}
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr& resp) override {
setOutput<int>("Result", resp->resp.front());
BT::NodeStatus onResultReceived(const WrappedResult& resp) override {
setOutput<int>("Result", resp.result->resp.front());
return BT::NodeStatus::SUCCESS;
}
};