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

7
mg_bt/behavior_trees/mg_bt.btproj
View File

@ -47,6 +47,13 @@
<input_port name="tolerance" type="double"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
</Action>
<Action ID="SendPose">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="angle" type="double"/>
<input_port name="isDegree" type="bool" default="true"/>
<input_port name="service_name" type="std::string" default="">Service name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic" type="int"/>
<input_port name="topic_name" type="std::string" default="__default__placeholder__">Topic name</input_port>

30
mg_bt/behavior_trees/tactics.xml
View File

@ -27,6 +27,23 @@
<AlwaysFailure/>
</BehaviorTree>
<BehaviorTree ID="testbed">
<Sequence>
<SendPose x="1.0"
y="1.0"
angle="90"
isDegree="true"
service_name="/set_pose"/>
<Delay delay_msec="5000">
<SendPose x="2.0"
y="0.5"
angle="-1"
isDegree="false"
service_name="/set_pose"/>
</Delay>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="I2CSignal">
@ -41,6 +58,19 @@
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="SendPose">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="angle"
type="double"/>
<input_port name="isDegree"
default="true"
type="bool"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic"
type="int"/>

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