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

30
mg_bt/behavior_trees/mg_bt.btproj
View File

@ -1,16 +1,23 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4" project_name="Project">
<include path="calib_bt.xml"/>
<include path="tactics.xml"/>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="CalibWidth">
<input_port name="PreviousWidth" type="double"/>
<input_port name="Count" default="1" type="int"/>
<input_port name="Count" type="int" default="1"/>
<output_port name="NewWidth" type="double"/>
<input_port name="service_name" type="std::string">Service name</input_port>
<input_port name="service_name" type="std::string" default="">Service name</input_port>
</Action>
<Action ID="I2CSignal">
<input_port name="Address" type="int" default="42"/>
<input_port name="Data" type="int" default="0"/>
<output_port name="Result" type="int"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
</Action>
<Action ID="MovePoint">
<input_port name="action_name" type="std::string">Action server name</input_port>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="tolerance" type="double"/>
<input_port name="obst_mult_a" type="double"/>
<input_port name="ornt_mult" type="double"/>
@ -20,7 +27,7 @@
<input_port name="obst_mult_b" type="double"/>
<input_port name="max_vel" type="double"/>
<input_port name="max_angular" type="double"/>
<input_port name="IgnoreList" type="std::string"/>
<input_port name="IgnoreList" type="std::string" default=""/>
<input_port name="max_wheel_speed" type="double"/>
<input_port name="x_goal" type="double"/>
<input_port name="y_goal" type="double"/>
@ -29,7 +36,7 @@
<input_port name="angle" type="double"/>
<input_port name="pos_mult" type="double"/>
<input_port name="max_wheel_speed" type="double"/>
<input_port name="IgnoreList" type="std::string"/>
<input_port name="IgnoreList" type="std::string" default=""/>
<input_port name="max_angular" type="double"/>
<input_port name="max_vel" type="double"/>
<input_port name="obst_mult_b" type="double"/>
@ -38,10 +45,19 @@
<input_port name="t_ornt_mult" type="double"/>
<input_port name="obst_mult_a" type="double"/>
<input_port name="tolerance" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
<input_port name="action_name" type="std::string" default="">Action server 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>
</Condition>
<Action ID="TacticChooser">
<output_port name="IsBlue" type="int"/>
<output_port name="tactic" type="int"/>
<input_port name="number" type="int"/>
</Action>
<Action ID="ZeroNode">
<input_port name="service_name" type="std::string">Service name</input_port>
<input_port name="service_name" type="std::string" default="">Service name</input_port>
</Action>
</TreeNodesModel>
</root>

61
mg_bt/behavior_trees/tactics.xml Normal file
View File

@ -0,0 +1,61 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="tactic_base">
<Sequence>
<I2CSignal Address="66"
Data="1"
Result="{tactic_id}"
action_name="/i2c_action"/>
<SideObstaclePub tactic="{tactic_id}"
topic_name="/side"/>
<TacticChooser IsBlue="{isBlue}"
tactic="{tactic}"
number="{tactic_id}"/>
<SubTree ID="tactic_default_yellow"
_skipIf="isBlue"
_autoremap="false"/>
<SubTree ID="tatic_default_blue"
_skipIf="!isBlue"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="tactic_default_yellow">
<AlwaysSuccess/>
</BehaviorTree>
<BehaviorTree ID="tatic_default_blue">
<AlwaysFailure/>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="I2CSignal">
<input_port name="Address"
default="42"
type="int"/>
<input_port name="Data"
default="0"
type="int"/>
<output_port name="Result"
type="int"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic"
type="int"/>
<input_port name="topic_name"
default="__default__placeholder__"
type="std::string">Topic name</input_port>
</Condition>
<Action ID="TacticChooser">
<output_port name="IsBlue"
type="int"/>
<output_port name="tactic"
type="int"/>
<input_port name="number"
type="int"/>
</Action>
</TreeNodesModel>
</root>

19
mg_bt/behavior_trees/tactics_blue.xml Normal file
View File

@ -0,0 +1,19 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="tactic_base">
<SideObstaclePub tactic="1"
topic_name="/side"/>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Condition ID="SideObstaclePub">
<input_port name="tactic"
type="int"/>
<input_port name="topic_name"
default="__default__placeholder__"
type="std::string">Topic name</input_port>
</Condition>
</TreeNodesModel>
</root>

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

0
mg_obstacles/obstacles/yellow-json.json → mg_obstacles/obstacles/yellow-side.json
View File

4
mg_obstacles/src/mg_obstacles.cpp
View File

@ -28,7 +28,7 @@ namespace mg {
"/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts);
side_sub_ = node_->create_subscription<std_msgs::msg::String>(
"/side", rclcpp::QoS(1).keep_last(1).transient_local(), [this](std_msgs::msg::String::ConstSharedPtr msg) {
"/side", rclcpp::QoS(1).keep_last(1), [this](std_msgs::msg::String::ConstSharedPtr msg) {
Json::Value json;
RCLCPP_INFO(node_->get_logger(), "Read file %s", (base_path + "/" + msg->data).c_str());
std::ifstream json_document(base_path + "/" + msg->data);
@ -38,7 +38,7 @@ namespace mg {
});
std::string obstacle_pkg;
std::string file_suffix = "/obstacles/obstacles.json";
std::string file_suffix = "/obstacles/yellow-side.json";
node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles");
ulong n = obstacle_pkg.find_first_of('/');