From 0ca87e65a037dae31eb2747ffbe43543badc06ec Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 7 May 2025 05:49:01 -0400 Subject: [PATCH] pami protection --- .../launch/test.modelec.launch.yml | 23 ++-- .../launch/test.no_gui.modelec.launch.yml | 21 ++-- .../launch/test.no_strat.modelec.launch.yml | 21 ++-- src/modelec_strat/CMakeLists.txt | 10 ++ src/modelec_strat/data/config.xml | 4 + src/modelec_strat/data/pami_zone.xml | 3 + .../include/modelec_strat/pami_manager.hpp | 45 +++++++ src/modelec_strat/src/pami_manager.cpp | 114 ++++++++++++++++++ 8 files changed, 216 insertions(+), 25 deletions(-) create mode 100644 src/modelec_strat/data/pami_zone.xml create mode 100644 src/modelec_strat/include/modelec_strat/pami_manager.hpp create mode 100644 src/modelec_strat/src/pami_manager.cpp diff --git a/src/modelec_core/launch/test.modelec.launch.yml b/src/modelec_core/launch/test.modelec.launch.yml index 7ceb5d1..3d45396 100644 --- a/src/modelec_core/launch/test.modelec.launch.yml +++ b/src/modelec_core/launch/test.modelec.launch.yml @@ -1,31 +1,36 @@ launch: -- node: - pkg: 'modelec_com' - exec: "serial_listener" - name: "serial_listener" + - node: + pkg: 'modelec_com' + exec: "serial_listener" + name: "serial_listener" -- node: + - node: pkg: 'modelec_com' exec: 'pcb_odo_interface' name: 'pcb_odo_interface' -- node: + - node: pkg: 'modelec_com' exec: 'pcb_alim_interface' name: 'pcb_alim_interface' -- node: + - node: pkg: 'modelec_gui' exec: "modelec_gui" name: "modelec_gui" -- node: + - node: pkg: 'modelec_core' exec: 'speed_result' name: 'speed_result' -- node: + - node: pkg: 'modelec_strat' exec: 'strat_fsm' name: 'strat_fsm' + + - node: + pkg: 'modelec_strat' + exec: 'pami_manager' + name: 'pami_manager' diff --git a/src/modelec_core/launch/test.no_gui.modelec.launch.yml b/src/modelec_core/launch/test.no_gui.modelec.launch.yml index 4fdc65b..8a20dc6 100644 --- a/src/modelec_core/launch/test.no_gui.modelec.launch.yml +++ b/src/modelec_core/launch/test.no_gui.modelec.launch.yml @@ -1,26 +1,31 @@ launch: -- node: - pkg: 'modelec_com' - exec: "serial_listener" - name: "serial_listener" + - node: + pkg: 'modelec_com' + exec: "serial_listener" + name: "serial_listener" -- node: + - node: pkg: 'modelec_com' exec: 'pcb_odo_interface' name: 'pcb_odo_interface' -- node: + - node: pkg: 'modelec_com' exec: 'pcb_alim_interface' name: 'pcb_alim_interface' -- node: + - node: pkg: 'modelec_core' exec: 'speed_result' name: 'speed_result' -- node: + - node: pkg: 'modelec_strat' exec: 'strat_fsm' name: 'strat_fsm' + + - node: + pkg: 'modelec_strat' + exec: 'pami_manager' + name: 'pami_manager' diff --git a/src/modelec_core/launch/test.no_strat.modelec.launch.yml b/src/modelec_core/launch/test.no_strat.modelec.launch.yml index b122144..59ed218 100644 --- a/src/modelec_core/launch/test.no_strat.modelec.launch.yml +++ b/src/modelec_core/launch/test.no_strat.modelec.launch.yml @@ -1,26 +1,31 @@ launch: -- node: - pkg: 'modelec_com' - exec: "serial_listener" - name: "serial_listener" + - node: + pkg: 'modelec_com' + exec: "serial_listener" + name: "serial_listener" -- node: + - node: pkg: 'modelec_com' exec: 'pcb_odo_interface' name: 'pcb_odo_interface' -- node: + - node: pkg: 'modelec_com' exec: 'pcb_alim_interface' name: 'pcb_alim_interface' -- node: + - node: pkg: 'modelec_gui' exec: "modelec_gui" name: "modelec_gui" -- node: + - node: pkg: 'modelec_core' exec: 'speed_result' name: 'speed_result' + + - node: + pkg: 'modelec_strat' + exec: 'pami_manager' + name: 'pami_manager' diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 28f267a..2110d8e 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -31,6 +31,15 @@ target_include_directories(enemy_manager PUBLIC $ ) + +add_executable(pami_manager src/pami_manager.cpp src/obstacle/obstacle.cpp) +ament_target_dependencies(pami_manager rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp) +target_link_libraries(pami_manager modelec_utils::utils modelec_utils::config) +target_include_directories(pami_manager PUBLIC + $ + $ +) + if (BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) @@ -41,6 +50,7 @@ endif () install(TARGETS strat_fsm enemy_manager + pami_manager DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 36434e7..23e34ad 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -67,4 +67,8 @@ 10 + + 77 + 65 + diff --git a/src/modelec_strat/data/pami_zone.xml b/src/modelec_strat/data/pami_zone.xml new file mode 100644 index 0000000..2ac8913 --- /dev/null +++ b/src/modelec_strat/data/pami_zone.xml @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/pami_manager.hpp b/src/modelec_strat/include/modelec_strat/pami_manager.hpp new file mode 100644 index 0000000..4751a76 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include +#include + +#include + +#include + +#include + +namespace Modelec { + + class PamiManger : public rclcpp::Node + { + public: + PamiManger(); + + bool ReadFromXML(const std::string& filename); + + protected: + Obstacle zone; + + int time_to_put_zone_ = 0; + int time_to_remove_top_pot_ = 0; + + int score_to_add_ = 0; + + int score_goupie_ = 0; + int score_superstar_ = 0; + int score_all_party_ = 0; + int score_free_zone_ = 0; + + rclcpp::TimerBase::SharedPtr timer_add_; + rclcpp::TimerBase::SharedPtr timer_remove_; + + rclcpp::Subscription::SharedPtr start_time_sub_; + rclcpp::Publisher::SharedPtr add_obs_pub_; + rclcpp::Publisher::SharedPtr remove_obs_pub_; + rclcpp::Publisher::SharedPtr score_pub_; + }; + +} \ No newline at end of file diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp new file mode 100644 index 0000000..82be537 --- /dev/null +++ b/src/modelec_strat/src/pami_manager.cpp @@ -0,0 +1,114 @@ +#include +#include + +namespace Modelec +{ + PamiManger::PamiManger() : Node("pami_manager") + { + std::string config_file = ament_index_cpp::get_package_share_directory("modelec_strat") + + "/data/config.xml"; + + if (!Config::load(config_file)) + { + RCLCPP_ERROR(get_logger(), "Failed to load configuration"); + } + + time_to_put_zone_ = Config::get("config.pami.time_to_put_zone", 80); + time_to_remove_top_pot_ = Config::get("config.pami.time_to_remove_top_pot", 70); + + score_goupie_ = Config::get("config.mission_score.pami.goupie"); + score_superstar_ = Config::get("config.mission_score.pami.superstar"); + score_all_party_ = Config::get("config.mission_score.pami.all_party"); + score_free_zone_ = 0; + + score_to_add_ = score_goupie_ + score_superstar_ + score_all_party_ + score_free_zone_; + + std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + + "/data/pami_zone.xml"; + if (!ReadFromXML(obstacles_path)) + { + RCLCPP_ERROR(get_logger(), "Failed to load obstacles from XML"); + } + + start_time_sub_ = create_subscription( + "/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr msg) + { + auto now = std::chrono::system_clock::now().time_since_epoch(); + auto goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_remove_top_pot_); + auto second_goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_put_zone_); + + timer_remove_ = create_wall_timer( + goal - now, + [this]() + { + modelec_interfaces::msg::Obstacle topLeft; + topLeft.id = 10; + remove_obs_pub_->publish(topLeft); + + modelec_interfaces::msg::Obstacle topRight; + topRight.id = 20; + remove_obs_pub_->publish(topRight); + + timer_remove_->cancel(); + }); + + timer_add_ = create_wall_timer( + second_goal - now, + [this]() + { + add_obs_pub_->publish(zone.toMsg()); + + std_msgs::msg::Int64 msg; + msg.data = score_to_add_; + score_pub_->publish(msg); + + timer_add_->cancel(); + }); + }); + + add_obs_pub_ = create_publisher( + "obstacle/add", 10); + + remove_obs_pub_ = create_publisher( + "obstacle/remove", 10); + + score_pub_ = create_publisher("/strat/score", 10); + } + + bool PamiManger::ReadFromXML(const std::string& filename) + { + tinyxml2::XMLDocument doc; + if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) + { + RCLCPP_ERROR(get_logger(), "Failed to load obstacle XML file: %s", filename.c_str()); + return false; + } + + tinyxml2::XMLElement* root = doc.FirstChildElement("pami_zone"); + if (!root) + { + RCLCPP_ERROR(get_logger(), "No root element in file"); + return false; + } + + tinyxml2::XMLElement* elem = root->FirstChildElement("middle"); + + if (!elem) + { + RCLCPP_ERROR(get_logger(), "No element in file"); + return false; + } + + zone = Obstacle(elem); + + RCLCPP_INFO(get_logger(), "Loaded zone obstacles from XML"); + return true; + } +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}