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

5
mg_obstacles/include/mg_obstacles/mg_obstacles.hpp
View File

@ -6,6 +6,7 @@
#include "mg_obstacles/static_obstacle.hpp"
#include "std_msgs/msg/u_int64.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "tf2_ros/buffer.h"
@ -51,9 +52,13 @@ namespace mg {
rclcpp::CallbackGroup::SharedPtr no_exec_cbg;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr side_sub_;
rclcpp::Subscription<StaticListMsg>::SharedPtr static_obst_sub_;
rclcpp::Subscription<DynamicPosMsg>::SharedPtr dynamic_obst_sub_;
std::string base_path;
void load_permanent(Json::Value& json);
void load_static(Json::Value& json);
};

94
mg_obstacles/obstacles/blue-side.json Normal file
View File

@ -0,0 +1,94 @@
{
"staticObstacleHeight": 0.4,
"staticObstacleWidth": 0.1,
"staticObstacles": [
{
"horizontal": true,
"x": 0.625,
"y": 1.775
},
{
"horizontal": false,
"x": 0,
"y": 0.6
},
{
"horizontal": false,
"x": 0,
"y": 1.525
},
{
"horizontal": true,
"x": 0.575,
"y": 0.3
},
{
"horizontal": true,
"x": 2.025,
"y": 0.3
},
{
"horizontal": false,
"x": 2.9,
"y": 0.6
},
{
"horizontal": false,
"x": 2.9,
"y": 1.525
},
{
"horizontal": true,
"x": 1.975,
"y": 1.775
},
{
"horizontal": true,
"x": 0.9,
"y": 1
},
{
"horizontal": true,
"x": 1.7,
"y": 1
}
],
"obstacles": [
{
"height": 0.45,
"width": 1.95,
"x": 0,
"y": 2
},
{
"height": 0.45,
"width": 0.45,
"x": 1,
"y": 0.45
},
{
"height": 0.2,
"width": 0.4,
"x": 1.95,
"y": 2
},
{
"height": 0.15,
"width": 0.45,
"x": 0.55,
"y": 0.15
},
{
"height": 0.15,
"width": 0.45,
"x": 2.55,
"y": 0.15
},
{
"height": 0.45,
"width": 0.45,
"x": 2.55,
"y": 1.1
}
]
}

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

18
mg_obstacles/src/mg_obstacles.cpp
View File

@ -27,23 +27,33 @@ namespace mg {
dynamic_obst_sub_ = node_->create_subscription<DynamicPosMsg>(
"/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) {
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);
json_document >> json;
load_permanent(json);
load_static(json);
});
std::string obstacle_pkg;
std::string file_suffix = "/obstacles/obstacles.json";
node_->declare_parameter("obstacles", rclcpp::PARAMETER_STRING);
node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles/obstacles.json");
node_->get_parameter_or<std::string>("obstacles", obstacle_pkg, "mg_obstacles/obstacles");
ulong n = obstacle_pkg.find_first_of('/');
if (n < obstacle_pkg.size()) {
file_suffix = obstacle_pkg.substr(n);
obstacle_pkg = obstacle_pkg.substr(0, n);
}
base_path = ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix;
Json::Value json;
RCLCPP_INFO(node_->get_logger(),
"Read file %s",
(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix).c_str());
std::ifstream json_document(ament_index_cpp::get_package_share_directory(obstacle_pkg) + file_suffix);
std::ifstream json_document(base_path + "/yellow-side.json");
json_document >> json;
load_permanent(json);
load_static(json);
}
@ -183,6 +193,7 @@ namespace mg {
}
void ObstacleManager::load_permanent(Json::Value& json) {
permanent_obstacles_.clear();
Json::Value& obstacles = json["obstacles"];
@ -193,6 +204,7 @@ namespace mg {
}
void ObstacleManager::load_static(Json::Value& json) {
static_obstacles_.clear();
StaticObstacle::width = json["staticObstacleWidth"].asDouble();
StaticObstacle::height = json["staticObstacleHeight"].asDouble();