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

Added behavior that publishes tactic id

Browse Source
This commit is contained in:
Pimpest
2025-05-07 17:01:11 +02:00
parent 8648a96bce
commit 19f5d06d06
6 changed files with 138 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mg_bt/src/mg_tree_executor.cpp
View File

@ -5,6 +5,7 @@
#include "tree_nodes/i2c.hpp"
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/rotate.hpp"
#include "tree_nodes/side_pub.hpp"
#include "tree_nodes/zero.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@ -28,6 +29,7 @@ namespace mg {
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::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
}

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

@ -0,0 +1,22 @@
#include "behaviortree_ros2/bt_topic_pub_node.hpp"
#include "std_msgs/msg/string.hpp"
namespace mg {
class SidePub : public BT::RosTopicPubNode<std_msgs::msg::String> {
public:
SidePub(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosTopicPubNode<std_msgs::msg::String>(name, conf, params) { }
static BT::PortsList providedPorts() { return providedBasicPorts({ BT::InputPort<int>("tactic") }); }
bool setMessage(std_msgs::msg::String& msg) override {
if (getInput<int>("tactic").has_value()) {
msg.data = (getInput<int>("tactic").value() % 2 == 0) ? "/blue-side.json" : "/yellow-side.json";
return true;
} else {
return false;
}
}
};
}