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

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();