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

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