From 19f5d06d060a18a849e70d3a188f5099a16b043a Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Wed, 7 May 2025 17:01:11 +0200 Subject: [PATCH] Added behavior that publishes tactic id --- mg_bt/src/mg_tree_executor.cpp | 2 + mg_bt/src/tree_nodes/side_pub.hpp | 22 +++++ .../include/mg_obstacles/mg_obstacles.hpp | 5 + mg_obstacles/obstacles/blue-side.json | 94 +++++++++++++++++++ .../{obstacles.json => yellow-json.json} | 0 mg_obstacles/src/mg_obstacles.cpp | 18 +++- 6 files changed, 138 insertions(+), 3 deletions(-) create mode 100644 mg_bt/src/tree_nodes/side_pub.hpp create mode 100644 mg_obstacles/obstacles/blue-side.json rename mg_obstacles/obstacles/{obstacles.json => yellow-json.json} (100%) diff --git a/mg_bt/src/mg_tree_executor.cpp b/mg_bt/src/mg_tree_executor.cpp index 24d41d5..b81a453 100644 --- a/mg_bt/src/mg_tree_executor.cpp +++ b/mg_bt/src/mg_tree_executor.cpp @@ -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("RotateNode", node()); factory.registerNodeType("ZeroNode", node()); factory.registerNodeType("I2CSignal", node()); + factory.registerNodeType("SideObstaclePub", node()); factory.registerNodeType("CalibWidth", node(), [this]() { return this->position(); }); } diff --git a/mg_bt/src/tree_nodes/side_pub.hpp b/mg_bt/src/tree_nodes/side_pub.hpp new file mode 100644 index 0000000..a8f337b --- /dev/null +++ b/mg_bt/src/tree_nodes/side_pub.hpp @@ -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 { + public: + SidePub(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) : + BT::RosTopicPubNode(name, conf, params) { } + + static BT::PortsList providedPorts() { return providedBasicPorts({ BT::InputPort("tactic") }); } + + bool setMessage(std_msgs::msg::String& msg) override { + if (getInput("tactic").has_value()) { + msg.data = (getInput("tactic").value() % 2 == 0) ? "/blue-side.json" : "/yellow-side.json"; + return true; + } else { + return false; + } + } + }; +} \ No newline at end of file diff --git a/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp b/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp index 9088596..a5195e6 100644 --- a/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp +++ b/mg_obstacles/include/mg_obstacles/mg_obstacles.hpp @@ -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::SharedPtr side_sub_; + rclcpp::Subscription::SharedPtr static_obst_sub_; rclcpp::Subscription::SharedPtr dynamic_obst_sub_; + std::string base_path; + void load_permanent(Json::Value& json); void load_static(Json::Value& json); }; diff --git a/mg_obstacles/obstacles/blue-side.json b/mg_obstacles/obstacles/blue-side.json new file mode 100644 index 0000000..3f80c5c --- /dev/null +++ b/mg_obstacles/obstacles/blue-side.json @@ -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 + } + ] +} \ No newline at end of file diff --git a/mg_obstacles/obstacles/obstacles.json b/mg_obstacles/obstacles/yellow-json.json similarity index 100% rename from mg_obstacles/obstacles/obstacles.json rename to mg_obstacles/obstacles/yellow-json.json diff --git a/mg_obstacles/src/mg_obstacles.cpp b/mg_obstacles/src/mg_obstacles.cpp index 9bce3f1..04c3da1 100644 --- a/mg_obstacles/src/mg_obstacles.cpp +++ b/mg_obstacles/src/mg_obstacles.cpp @@ -27,23 +27,33 @@ namespace mg { dynamic_obst_sub_ = node_->create_subscription( "/dynamicObstacle", rclcpp::QoS(1).keep_last(1), dynamic_cb, sub_opts); + side_sub_ = node_->create_subscription( + "/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("obstacles", obstacle_pkg, "mg_obstacles/obstacles/obstacles.json"); + node_->get_parameter_or("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();