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

Added the ability to set postion

Browse Source
This commit is contained in:
Pimpest
2025-05-08 02:25:34 +02:00
parent 07cc056853
commit da3d1d9d15
9 changed files with 131 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mg_bt/src/mg_tree_executor.cpp
View File

@ -7,6 +7,7 @@
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/rotate.hpp"
#include "tree_nodes/side_pub.hpp"
#include "tree_nodes/set_pos.hpp"
#include "tree_nodes/zero.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@ -31,6 +32,7 @@ namespace mg {
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::TacticsChooser>("TacticChooser");
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
}

35
mg_bt/src/tree_nodes/set_pos.hpp Normal file
View File

@ -0,0 +1,35 @@
#pragma once
#include "behaviortree_ros2/bt_service_node.hpp"
#include "mg_msgs/srv/send_pose2d.hpp"
namespace mg {
class SendPoseNode : public BT::RosServiceNode<mg_msgs::srv::SendPose2d> {
public:
SendPoseNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosServiceNode<mg_msgs::srv::SendPose2d>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x"),
BT::InputPort<double>("y"),
BT::InputPort<double>("angle"),
BT::InputPort<bool>("isDegree", "True", {}) });
}
bool setRequest(typename Request::SharedPtr& req) override {
req->x = getInput<double>("x").value();
req->y = getInput<double>("y").value();
req->theta = getInput<double>("angle").value();
if (getInput<bool>("isDegree").value()) {
req->theta *= M_PI / 180;
}
return true;
}
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
return BT::NodeStatus::SUCCESS;
}
};
}