From 4420dbb15c0d623f613685ea89ce655dcf658d0b Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 1 May 2025 23:54:08 -0400 Subject: [PATCH] config + strat --- src/modelec_com/src/pcb_alim_interface.cpp | 2 +- .../include/modelec_gui/pages/map_page.hpp | 11 +- src/modelec_gui/src/pages/map_page.cpp | 45 ++++--- src/modelec_strat/CMakeLists.txt | 49 ++++--- src/modelec_strat/data/config.xml | 31 +++++ src/modelec_strat/{map => data}/obstacles.xml | 38 +++--- .../include/modelec_strat/config.hpp | 50 +++++++ .../include/modelec_strat/enemy_manager.hpp | 3 + .../modelec_strat/navigation_helper.hpp | 4 +- .../include/modelec_strat/obstacle.hpp | 37 +++++ .../include/modelec_strat/pathfinding.hpp | 19 ++- src/modelec_strat/src/config.cpp | 52 ++++++++ src/modelec_strat/src/enemy_manager.cpp | 17 ++- src/modelec_strat/src/navigation_helper.cpp | 7 +- src/modelec_strat/src/obstacle.cpp | 43 ++++++ src/modelec_strat/src/pathfinding.cpp | 126 ++++++++++-------- src/modelec_strat/src/strat_fms.cpp | 9 ++ 17 files changed, 417 insertions(+), 126 deletions(-) create mode 100644 src/modelec_strat/data/config.xml rename src/modelec_strat/{map => data}/obstacles.xml (51%) create mode 100644 src/modelec_strat/include/modelec_strat/config.hpp create mode 100644 src/modelec_strat/include/modelec_strat/obstacle.hpp create mode 100644 src/modelec_strat/src/config.cpp create mode 100644 src/modelec_strat/src/obstacle.cpp diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 281a1ae..8d4e0ca 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_alim"; request->bauds = 115200; - request->serial_port = "/dev/pts/11"; // TODO : check the real serial port + request->serial_port = "/dev/pts/10"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { diff --git a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp index b0645f4..dacd364 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -22,6 +22,7 @@ namespace ModelecGUI { class MapPage : public QWidget { Q_OBJECT + public: MapPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr); @@ -38,6 +39,12 @@ namespace ModelecGUI { void mousePressEvent(QMouseEvent* event) override; + void mouseReleaseEvent(QMouseEvent* event) override; + + void mouseDoubleClickEvent(QMouseEvent* event) override; + + void mouseMoveEvent(QMouseEvent* event) override; + void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg); @@ -49,10 +56,10 @@ namespace ModelecGUI { QPoint robotPosPoint = QPoint(0, 0); std::vector qpoints; - std::vector points; + //std::vector points; modelec_interfaces::msg::OdometryPos go_to_point; - bool lastWapointWasEnd = false; + bool lastWapointWasEnd = true; std::map obstacle_; bool show_obstacle_ = true; diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 7b9be0d..edcd25e 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -20,7 +20,6 @@ namespace ModelecGUI this->setLayout(v_layout); qpoints = {}; - points = {}; enemy_pos_.x = 1775; enemy_pos_.y = 200; @@ -31,8 +30,9 @@ namespace ModelecGUI if (lastWapointWasEnd) { qpoints.clear(); - points.clear(); lastWapointWasEnd = false; + + qpoints.push_back(QPoint(robotPosPoint.x(), robotPosPoint.y())); } if (msg->is_end) @@ -42,7 +42,6 @@ namespace ModelecGUI qpoints.push_back(QPoint(Modelec::mapValue(static_cast(msg->x), 0, 3000, 0, width()), height() - Modelec::mapValue(static_cast(msg->y), 0, 2000, 0, height()))); - points.push_back(*msg); update(); }); @@ -82,13 +81,6 @@ namespace ModelecGUI RCLCPP_INFO(node_->get_logger(), "Map received: %d x %d", res->width, res->height); map_width_ = res->width; map_height_ = res->height; - /* TODO - obstacle - * idea - * send only the size of the map - * and then send the obstacle with id throw the topic obstacle - * problem : if not initialized, idk what to do - * maybe solution ask to send back the obstacle throw an other service - */ } } @@ -133,22 +125,15 @@ namespace ModelecGUI // --- Draw lines --- painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide - if (!points.empty()) - painter.drawLine(robotPosPoint, qpoints[0]); - - for (size_t i = 0; i + 1 < points.size(); ++i) { + for (size_t i = 0; i + 1 < qpoints.size(); ++i) { painter.drawLine(qpoints[i], qpoints[i + 1]); } // --- Draw colored points --- const int radius = 5; - painter.setBrush(Qt::green); - painter.setPen(Qt::NoPen); - painter.drawEllipse(robotPosPoint, radius, radius); // Robot position - - for (size_t i = 0; i < points.size(); ++i) { - if (i == points.size() - 1) + for (size_t i = 0; i < qpoints.size(); ++i) { + if (i == qpoints.size() - 1) painter.setBrush(Qt::blue); // Last = blue else painter.setBrush(Qt::red); // Middle = red @@ -157,6 +142,10 @@ namespace ModelecGUI painter.drawEllipse(qpoints[i], radius, radius); } + painter.setBrush(Qt::green); + painter.setPen(Qt::NoPen); + painter.drawEllipse(robotPosPoint, radius, radius); // Robot position + if (show_obstacle_) { float ratioBetweenMapAndWidget = height() / 2000.0f; @@ -184,6 +173,7 @@ namespace ModelecGUI go_to_pub_->publish(msg); } + else if (event->button() == Qt::RightButton) { enemy_pos_.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000); @@ -194,6 +184,21 @@ namespace ModelecGUI } } + void MapPage::mouseReleaseEvent(QMouseEvent* event) + { + QWidget::mouseReleaseEvent(event); + } + + void MapPage::mouseDoubleClickEvent(QMouseEvent* event) + { + QWidget::mouseDoubleClickEvent(event); + } + + void MapPage::mouseMoveEvent(QMouseEvent* event) + { + QWidget::mouseMoveEvent(event); + } + void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) { obstacle_.emplace(msg->id, *msg); diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 00fa44e..c672b4d 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.8) project(modelec_strat) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () find_package(ament_cmake REQUIRED) find_package(std_msgs REQUIRED) @@ -11,37 +11,44 @@ find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tinyxml2 REQUIRED) find_package(rclcpp REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) -add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp) -ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces) +add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp src/obstacle.cpp src/config.cpp) +ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp) target_link_libraries(strat_fsm modelec_utils::modelec_utils tinyxml2::tinyxml2) target_include_directories(strat_fsm PUBLIC - $ - $ + $ + $ ) -add_executable(enemy_manager src/enemy_manager.cpp) -ament_target_dependencies(enemy_manager rclcpp std_msgs sensor_msgs std_srvs modelec_interfaces) -target_link_libraries(enemy_manager modelec_utils::modelec_utils) +add_executable(enemy_manager src/enemy_manager.cpp src/config.cpp) +ament_target_dependencies(enemy_manager rclcpp std_msgs sensor_msgs std_srvs modelec_interfaces ament_index_cpp) +target_link_libraries(enemy_manager modelec_utils::modelec_utils tinyxml2::tinyxml2) target_include_directories(enemy_manager PUBLIC - $ - $ + $ + $ ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif () install(TARGETS - strat_fsm - enemy_manager - DESTINATION lib/${PROJECT_NAME} + strat_fsm + enemy_manager + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY data/ + DESTINATION share/${PROJECT_NAME}/data + FILES_MATCHING PATTERN "*.xml" ) ament_package() diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml new file mode 100644 index 0000000..f51ddac --- /dev/null +++ b/src/modelec_strat/data/config.xml @@ -0,0 +1,31 @@ + + + + 300 + 300 + + 50 + + + 50 + 0.5 + + + + + + 300 + 300 + + + + + + 300 + 200 + + 3000 + 2000 + + + diff --git a/src/modelec_strat/map/obstacles.xml b/src/modelec_strat/data/obstacles.xml similarity index 51% rename from src/modelec_strat/map/obstacles.xml rename to src/modelec_strat/data/obstacles.xml index de909b6..71150d3 100644 --- a/src/modelec_strat/map/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -1,25 +1,33 @@ - - - + - - - - - + - - - - - + - - + diff --git a/src/modelec_strat/include/modelec_strat/config.hpp b/src/modelec_strat/include/modelec_strat/config.hpp new file mode 100644 index 0000000..3736101 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/config.hpp @@ -0,0 +1,50 @@ +#pragma once +#include +#include +#include + +#include "obstacle.hpp" + +namespace Modelec +{ + class Config + { + + public: + static bool load(const std::string& filepath); + + template + static T get(const std::string& key, const T& default_value = T()); + + static void printAll(); + + private: + static void parseNode(tinyxml2::XMLElement* element, const std::string& prefix); + + static inline std::unordered_map values_; + }; + + + template + T Config::get(const std::string& key, const T& default_value) { + auto it = values_.find(key); + if (it == values_.end()) return default_value; + + std::istringstream iss(it->second); + T result; + if (!(iss >> result)) return default_value; + return result; + } + + template<> + inline std::string Config::get(const std::string& key, const std::string& default_value) { + auto it = values_.find(key); + return it != values_.end() ? it->second : default_value; + } + + template<> + inline bool Config::get(const std::string& key, const bool& default_value) { + auto str = get(key, default_value ? "true" : "false"); + return str == "true" || str == "1"; + } +} diff --git a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp index ce6c148..5889fa4 100644 --- a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp @@ -31,5 +31,8 @@ namespace Modelec modelec_interfaces::msg::OdometryPos last_enemy_pos_; bool enemy_initialized_ = false; rclcpp::Time last_publish_time_; + + float min_move_threshold_mm_ = 0.0f; + float refresh_rate_s_ = 0.0f; }; } diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 99341a5..9062fd5 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -17,6 +17,8 @@ namespace Modelec { rclcpp::Node::SharedPtr getNode() const; + std::shared_ptr getPathfinding() const; + void SendWaypoint() const; void SendWaypoint(const std::vector &waypoints) const; @@ -42,7 +44,7 @@ namespace Modelec { private: rclcpp::Node::SharedPtr node_; - std::unique_ptr pathfinding_; + std::shared_ptr pathfinding_; std::list waypoints_; diff --git a/src/modelec_strat/include/modelec_strat/obstacle.hpp b/src/modelec_strat/include/modelec_strat/obstacle.hpp new file mode 100644 index 0000000..942593e --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/obstacle.hpp @@ -0,0 +1,37 @@ +#pragma once +#include + +#include +#include + +#include + +namespace Modelec { + class Obstacle { + public: + Obstacle() : id_(-1), x_(0), y_(0), w_(0), h_(0), type_("unknown") {} + Obstacle(int id, int x, int y, int w, int h, const std::string& type); + Obstacle(tinyxml2::XMLElement * obstacleElem); + Obstacle(const modelec_interfaces::msg::Obstacle& msg); + Obstacle(const Obstacle& other) = default; + + modelec_interfaces::msg::Obstacle toMsg() const; + + int id() const { return id_; } + int x() const { return x_; } + int y() const { return y_; } + int width() const { return w_; } + int height() const { return h_; } + const std::string& type() const { return type_; } + + void setId(int id) { id_ = id; } + void setX(int x) { x_ = x; } + void setY(int y) { y_ = y; } + void setWidth(int w) { w_ = w; } + void setHeight(int h) { h_ = h; } + void setType(const std::string& type) { type_ = type; } + private: + int id_, x_, y_, w_, h_; + std::string type_; + }; +} diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index ea686de..25916eb 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -14,6 +14,8 @@ #include #include +#include "obstacle.hpp" + namespace Modelec { @@ -45,10 +47,10 @@ namespace Modelec { WaypointListMsg FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal); - bool LoadObstaclesFromXML(const std::string& filename); - //void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal); + bool LoadObstaclesFromXML(const std::string& filename); + void SetCurrentPos(const PosMsg::SharedPtr& pos); protected: @@ -77,10 +79,14 @@ namespace Modelec { int grid_height_ = 0; int map_width_mm_ = 0; int map_height_mm_ = 0; - float robot_width_mm_ = 0; - float robot_length_mm_ = 0; + int robot_width_mm_ = 0; + int robot_length_mm_ = 0; + int enemy_width_mm_ = 0; + int enemy_length_mm_ = 0; + int enemy_margin_mm_ = 0; - std::map obstacle_map_; + + std::map obstacle_map_; PosMsg::SharedPtr current_start_; PosMsg::SharedPtr current_goal_; @@ -93,6 +99,9 @@ namespace Modelec { rclcpp::Publisher::SharedPtr map_pub_; rclcpp::Publisher::SharedPtr waypoint_pub_; + rclcpp::Subscription::SharedPtr obstacle_add_sub_; + rclcpp::Subscription::SharedPtr obstacle_remove_sub_; + rclcpp::Service::SharedPtr map_srv_; rclcpp::Service::SharedPtr map_size_srv_; rclcpp::Service::SharedPtr ask_obstacle_srv_; diff --git a/src/modelec_strat/src/config.cpp b/src/modelec_strat/src/config.cpp new file mode 100644 index 0000000..aa6f401 --- /dev/null +++ b/src/modelec_strat/src/config.cpp @@ -0,0 +1,52 @@ +#include + +namespace Modelec +{ + bool Config::load(const std::string& filepath) + { + tinyxml2::XMLDocument doc; + if (doc.LoadFile(filepath.c_str()) != tinyxml2::XML_SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger("Config"), "Failed to load XML file: %s", doc.ErrorName()); + return false; + } + + auto* root = doc.RootElement(); + if (!root) + { + RCLCPP_ERROR(rclcpp::get_logger("Config"), "No root element found in XML file"); + return false; + } + + parseNode(root, ""); + return true; + } + + void Config::parseNode(tinyxml2::XMLElement* element, const std::string& prefix) { + std::string key_prefix = prefix.empty() ? element->Name() : prefix + "." + element->Name(); + + const char* text = element->GetText(); + if (text && std::string(text).find_first_not_of(" \n\t") != std::string::npos) { + values_[key_prefix] = text; + } + + const tinyxml2::XMLAttribute* attr = element->FirstAttribute(); + while (attr) { + std::string attr_key = key_prefix + "@" + attr->Name(); + values_[attr_key] = attr->Value(); + attr = attr->Next(); + } + + for (tinyxml2::XMLElement* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) { + parseNode(child, key_prefix); + } + } + + void Config::printAll() + { + RCLCPP_INFO(rclcpp::get_logger("Config"), "Loaded %zu configuration values", values_.size()); + + for (const auto& pair : values_) { + RCLCPP_INFO(rclcpp::get_logger("Config"), "%s: %s", pair.first.c_str(), pair.second.c_str()); + } + } +} diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 97a6172..3412e22 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -1,10 +1,22 @@ #include #include +#include +#include namespace Modelec { EnemyManager::EnemyManager() : Node("enemy_manager") { + std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; + if (!Config::load(config_path)) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); + } + + min_move_threshold_mm_ = Config::get("config.enemy.detection.min_move_threshold_mm", 50); + + refresh_rate_s_ = Config::get("config.enemy.detection.refresh_rate", 1.0); + current_pos_sub_ = this->create_subscription( "odometry/position", 10, [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { @@ -106,8 +118,7 @@ namespace Modelec float dy = enemy_pos.y - last_enemy_pos_.y; float distance_squared = dx * dx + dy * dy; - constexpr float min_move_threshold_mm = 50.0f; - if (distance_squared > min_move_threshold_mm * min_move_threshold_mm) + if (distance_squared > min_move_threshold_mm_ * min_move_threshold_mm_) { need_publish = true; } @@ -133,7 +144,7 @@ namespace Modelec return; rclcpp::Duration duration_since_last = this->now() - last_publish_time_; - if (duration_since_last.seconds() >= 2.0) + if (duration_since_last.seconds() >= refresh_rate_s_) { enemy_pos_pub_->publish(last_enemy_pos_); last_publish_time_ = this->now(); diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index cd2556e..3242cd5 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -8,7 +8,7 @@ namespace Modelec { NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node) { - pathfinding_ = std::make_unique(node); + pathfinding_ = std::make_shared(node); waypoint_reach_sub_ = node_->create_subscription( "odometry/waypoint_reach", 10, @@ -35,6 +35,11 @@ namespace Modelec { return node_; } + std::shared_ptr NavigationHelper::getPathfinding() const + { + return pathfinding_; + } + void NavigationHelper::SendWaypoint() const { for (auto & w : waypoints_) diff --git a/src/modelec_strat/src/obstacle.cpp b/src/modelec_strat/src/obstacle.cpp new file mode 100644 index 0000000..7b124ca --- /dev/null +++ b/src/modelec_strat/src/obstacle.cpp @@ -0,0 +1,43 @@ +#include + +namespace Modelec +{ + Obstacle::Obstacle(int id, int x, int y, int w, int h, const std::string& type) + : id_(id), x_(x), y_(y), w_(w), h_(h), type_(type) + { + } + + Obstacle::Obstacle(tinyxml2::XMLElement* obstacleElem) + { + const char* type = nullptr; + if (obstacleElem->QueryIntAttribute("id", &id_) != tinyxml2::XML_SUCCESS || + obstacleElem->QueryIntAttribute("x", &x_) != tinyxml2::XML_SUCCESS || + obstacleElem->QueryIntAttribute("y", &y_) != tinyxml2::XML_SUCCESS || + obstacleElem->QueryIntAttribute("width", &w_) != tinyxml2::XML_SUCCESS || + obstacleElem->QueryIntAttribute("height", &h_) != tinyxml2::XML_SUCCESS || + obstacleElem->QueryStringAttribute("type", &type) != tinyxml2::XML_SUCCESS) + { + RCLCPP_WARN(rclcpp::get_logger("Obstacle"), "Failed to parse obstacle element"); + return; + } + type_ = type; + } + + Obstacle::Obstacle(const modelec_interfaces::msg::Obstacle& msg) + : id_(msg.id), x_(msg.x), y_(msg.y), w_(msg.width), h_(msg.height), type_("unknown") + { + } + + modelec_interfaces::msg::Obstacle Obstacle::toMsg() const + { + modelec_interfaces::msg::Obstacle msg; + + msg.id = id_; + msg.x = x_; + msg.y = y_; + msg.width = w_; + msg.height = h_; + + return msg; + } +} diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 8286d33..58cda8f 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -1,5 +1,8 @@ +#include #include +#include + namespace Modelec { struct AStarNode @@ -27,23 +30,44 @@ namespace Modelec { Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr& node) : node_(node) { - map_width_mm_ = 3000.0f; - map_height_mm_ = 2000.0f; + map_width_mm_ = Config::get("config.map.size.map_width_mm", 3000); + map_height_mm_ = Config::get("config.map.size.map_height_mm", 2000); - robot_length_mm_ = 300.0f; - robot_width_mm_ = 300.0f; + robot_length_mm_ = Config::get("config.robot.size.length_mm", 300); + robot_width_mm_ = Config::get("config.robot.size.width_mm", 300); - grid_width_ = 300; - grid_height_ = 200; + enemy_length_mm_ = Config::get("config.enemy.size.length_mm", 300); + enemy_width_mm_ = Config::get("config.enemy.size.width_mm", 300); - if (!LoadObstaclesFromXML("/home/acki/ros2_ws/src/modelec_strat/map/obstacles.xml")) + enemy_margin_mm_ = Config::get("config.enemy.size.margin_mm", 50); + + grid_width_ = Config::get("config.map.size.grid_width", 300); + grid_height_ = Config::get("config.map.size.grid_height", 200); + + std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/obstacles.xml"; + if (!LoadObstaclesFromXML(obstacles_path)) { - RCLCPP_WARN(node_->get_logger(), "Failed to load obstacles from XML"); + RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML"); } map_pub_ = node_->create_publisher( "nav/obstacle", 40); + obstacle_add_sub_ = node_->create_subscription( + "nav/obstacle/add", 10, + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + RCLCPP_INFO(node_->get_logger(), "Obstacle add request received"); + obstacle_map_[msg->id] = Obstacle(*msg); + map_pub_->publish(*msg); + }); + + obstacle_remove_sub_ = node_->create_subscription( + "nav/obstacle/remove", 10, + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { + RCLCPP_INFO(node_->get_logger(), "Obstacle remove request received"); + obstacle_map_.erase(msg->id); + }); + ask_obstacle_srv_ = node_->create_service( "nav/ask_map_obstacle", [this](const std::shared_ptr request, @@ -139,11 +163,11 @@ namespace Modelec { if (has_enemy_pos_) { - int ex = (last_enemy_pos_.x / cell_size_mm_x) - inflate_x; - int ey = (grid_height_ - 1) - ((last_enemy_pos_.y / cell_size_mm_y) - inflate_y); + int ex = (last_enemy_pos_.x / cell_size_mm_x); + int ey = ((map_height_mm_ - last_enemy_pos_.y) / cell_size_mm_y); - const int inflate_enemy_x = std::ceil(150.0 / cell_size_mm_x + inflate_x); - const int inflate_enemy_y = std::ceil(150.0 / cell_size_mm_y + inflate_y); + const int inflate_enemy_x = std::ceil((enemy_margin_mm_ + (enemy_width_mm_/2)) / cell_size_mm_x) + inflate_x; + const int inflate_enemy_y = std::ceil((enemy_margin_mm_ + (enemy_length_mm_/2)) / cell_size_mm_y) + inflate_y; for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y) { @@ -180,10 +204,10 @@ namespace Modelec { // 2. Fill obstacles with inflation for (const auto& [id, obstacle] : obstacle_map_) { - int x_start = std::max(0, (int)(obstacle.x / cell_size_mm_x) - inflate_x); - int y_start = std::max(0, (int)(obstacle.y / cell_size_mm_y) - inflate_y); - int x_end = std::min(grid_width_ - 1, (int)((obstacle.x + obstacle.width) / cell_size_mm_x) + inflate_x); - int y_end = std::min(grid_height_ - 1, (int)((obstacle.y + obstacle.height) / cell_size_mm_y) + inflate_y); + int x_start = std::max(0, (int)(obstacle.x() / cell_size_mm_x) - inflate_x); + int y_start = std::max(0, (int)(obstacle.y() / cell_size_mm_y) - inflate_y); + int x_end = std::min(grid_width_ - 1, (int)((obstacle.x() + obstacle.width()) / cell_size_mm_x) + inflate_x); + int y_end = std::min(grid_height_ - 1, (int)((obstacle.y() + obstacle.height()) / cell_size_mm_y) + inflate_y); // Inverser l'axe Y y_start = (grid_height_ - 1) - y_start; @@ -396,46 +420,6 @@ namespace Modelec { return waypoints; } - - bool Pathfinding::LoadObstaclesFromXML(const std::string& filename) - { - tinyxml2::XMLDocument doc; - if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str()); - return false; - } - - tinyxml2::XMLElement* root = doc.FirstChildElement("Obstacles"); - if (!root) - { - RCLCPP_ERROR(node_->get_logger(), "No root element in file"); - return false; - } - - for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Obstacle"); - obstacleElem != nullptr; - obstacleElem = obstacleElem->NextSiblingElement("Obstacle")) - { - modelec_interfaces::msg::Obstacle obs; - - if (obstacleElem->QueryIntAttribute("id", &obs.id) != tinyxml2::XML_SUCCESS || - obstacleElem->QueryIntAttribute("x", &obs.x) != tinyxml2::XML_SUCCESS || - obstacleElem->QueryIntAttribute("y", &obs.y) != tinyxml2::XML_SUCCESS || - obstacleElem->QueryIntAttribute("width", &obs.width) != tinyxml2::XML_SUCCESS || - obstacleElem->QueryIntAttribute("height", &obs.height) != tinyxml2::XML_SUCCESS) - { - RCLCPP_WARN(node_->get_logger(), "Incomplete obstacle definition in XML, skipping one obstacle"); - continue; - } - - obstacle_map_[obs.id] = obs; - } - - RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size()); - return true; - } - /*void Pathfinding::SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal) { current_start_ = start; @@ -480,7 +464,7 @@ namespace Modelec { { for (auto & [index, obs] : obstacle_map_) { - map_pub_->publish(obs); + map_pub_->publish(obs.toMsg()); } } @@ -498,6 +482,34 @@ namespace Modelec { }*/ } + bool Pathfinding::LoadObstaclesFromXML(const std::string& filename) + { + tinyxml2::XMLDocument doc; + if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str()); + return false; + } + + tinyxml2::XMLElement* root = doc.FirstChildElement("Obstacles"); + if (!root) + { + RCLCPP_ERROR(node_->get_logger(), "No root element in file"); + return false; + } + + for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Obstacle"); + obstacleElem != nullptr; + obstacleElem = obstacleElem->NextSiblingElement("Obstacle")) + { + Obstacle obs(obstacleElem); + obstacle_map_[obs.id()] = obs; + } + + RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size()); + return true; + } + Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast) { id = index; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index ca17649..5397d56 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -1,3 +1,5 @@ +#include +#include #include namespace Modelec @@ -15,6 +17,12 @@ namespace Modelec }); state_pub_ = create_publisher("/strat/state", 10); + + std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; + if (!Config::load(config_path)) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); + } } void StratFMS::Init() @@ -22,6 +30,7 @@ namespace Modelec nav_ = std::make_shared(shared_from_this()); mission_manager_ = std::make_unique(shared_from_this()); action_executor_ = std::make_unique(shared_from_this()); + RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized"); timer_ = create_wall_timer(std::chrono::milliseconds(100), [this]