From d9b108051603953a2f83991dff94eb34a3cf73d0 Mon Sep 17 00:00:00 2001 From: Ackimixs Date: Wed, 11 Mar 2026 16:38:23 +0100 Subject: [PATCH] Develop strat 7 (#32) Co-authored-by: modelec --- Desktop/cam.desktop | 8 ++ build.rasp.sh | 2 +- install.sh | 4 +- simulated_pcb/serial.modelec.py | 43 ++++-- src/modelec_com/CMakeLists.txt | 24 ++++ .../include/modelec_com/color_detector.hpp | 61 ++++++++ .../modelec_com/pcb_action_interface.hpp | 29 +++- .../include/modelec_com/pcb_odo_interface.hpp | 29 +++- src/modelec_com/src/color_detector.cpp | 120 +++++++++------- src/modelec_com/src/pcb_action_interface.cpp | 131 +++++++----------- src/modelec_com/src/pcb_odo_interface.cpp | 28 ++-- src/modelec_core/launch/modelec.launch.py | 21 +-- src/modelec_gui/CMakeLists.txt | 2 + .../include/modelec_gui/modelec_gui.hpp | 9 +- src/modelec_gui/resource.qrc | 1 + src/modelec_gui/sound/test.mp3 | Bin 0 -> 52662 bytes src/modelec_gui/src/main.cpp | 14 +- src/modelec_gui/src/modelec_gui.cpp | 28 +++- src/modelec_gui/src/pages/map_page.cpp | 14 +- src/modelec_gui/src/pages/test_map_page.cpp | 13 +- .../msg/Action/ActionServoTimed.msg | 3 +- src/modelec_strat/CMakeLists.txt | 5 +- src/modelec_strat/data/action.xml | 118 ++++++++++++++++ src/modelec_strat/data/config.xml | 107 ++++++++++++-- src/modelec_strat/data/deposite_zone.xml | 16 +-- src/modelec_strat/data/obstacles.xml | 20 +-- .../modelec_strat/action/base_action.hpp | 28 ++++ .../modelec_strat/action/down_action.hpp | 8 ++ .../modelec_strat/action/free_action.hpp | 9 ++ .../modelec_strat/action/look_on_action.hpp | 38 ----- .../modelec_strat/action/take_action.hpp | 9 ++ .../modelec_strat/action/thermo_action.hpp | 8 ++ .../action/toggle_servo_action.hpp | 9 ++ .../modelec_strat/action/up_action.hpp | 8 ++ .../include/modelec_strat/action_executor.hpp | 16 +-- .../include/modelec_strat/deposite_zone.hpp | 8 +- .../modelec_strat/missions/action_mission.hpp | 26 ++++ .../modelec_strat/missions/free_mission.hpp | 25 +--- .../missions/go_home_mission.hpp | 18 +-- .../missions/min_time_mission.hpp | 25 ++++ .../modelec_strat/missions/mission_base.hpp | 15 +- .../modelec_strat/missions/move_mission.hpp | 30 ++++ .../modelec_strat/missions/take_mission.hpp | 26 +--- .../modelec_strat/missions/thermo_mission.hpp | 23 +-- .../modelec_strat/navigation_helper.hpp | 18 +-- .../include/modelec_strat/obstacle/box.hpp | 7 +- .../modelec_strat/obstacle/obstacle.hpp | 8 +- .../include/modelec_strat/pami_manager.hpp | 28 ---- .../include/modelec_strat/pathfinding.hpp | 8 +- src/modelec_strat/src/action/down_action.cpp | 73 +++++----- src/modelec_strat/src/action/free_action.cpp | 20 ++- .../src/action/look_on_action.cpp | 69 --------- src/modelec_strat/src/action/take_action.cpp | 22 ++- .../src/action/thermo_action.cpp | 37 +++-- .../src/action/toggle_servo_action.cpp | 22 ++- src/modelec_strat/src/action/up_action.cpp | 71 ++++------ src/modelec_strat/src/action_executor.cpp | 67 +++------ src/modelec_strat/src/deposite_zone.cpp | 36 ++--- src/modelec_strat/src/enemy_manager.cpp | 33 ++--- .../src/missions/action_mission.cpp | 47 +++++++ .../src/missions/free_mission.cpp | 68 +++------ .../src/missions/go_home_mission.cpp | 56 ++------ .../src/missions/mine_time_mission.cpp | 44 ++++++ .../src/missions/mission_base.cpp | 19 +++ .../src/missions/move_mission.cpp | 65 +++++++++ .../src/missions/take_mission.cpp | 66 +++------ .../src/missions/thermo_mission.cpp | 60 ++------ src/modelec_strat/src/navigation_helper.cpp | 118 +++++----------- src/modelec_strat/src/obstacle/box.cpp | 16 ++- src/modelec_strat/src/obstacle/obstacle.cpp | 42 +++--- src/modelec_strat/src/pami_manager.cpp | 86 ------------ src/modelec_strat/src/pathfinding.cpp | 108 ++++++--------- src/modelec_strat/src/strat_fms.cpp | 31 +++-- .../include/modelec_utils/config.hpp | 93 +++++++++++-- .../include/modelec_utils/point.hpp | 4 +- src/modelec_utils/src/config.cpp | 43 +++++- src/modelec_utils/test/config.test.cpp | 32 +++++ start_cam_ros2.sh | 12 ++ start_ros2.sh | 4 +- start_ros2.zsh | 9 ++ 80 files changed, 1566 insertions(+), 1155 deletions(-) create mode 100644 Desktop/cam.desktop create mode 100644 src/modelec_gui/sound/test.mp3 create mode 100644 src/modelec_strat/data/action.xml delete mode 100644 src/modelec_strat/include/modelec_strat/action/look_on_action.hpp create mode 100644 src/modelec_strat/include/modelec_strat/missions/action_mission.hpp create mode 100644 src/modelec_strat/include/modelec_strat/missions/min_time_mission.hpp create mode 100644 src/modelec_strat/include/modelec_strat/missions/move_mission.hpp delete mode 100644 src/modelec_strat/src/action/look_on_action.cpp create mode 100644 src/modelec_strat/src/missions/action_mission.cpp create mode 100644 src/modelec_strat/src/missions/mine_time_mission.cpp create mode 100644 src/modelec_strat/src/missions/mission_base.cpp create mode 100644 src/modelec_strat/src/missions/move_mission.cpp create mode 100755 start_cam_ros2.sh create mode 100755 start_ros2.zsh diff --git a/Desktop/cam.desktop b/Desktop/cam.desktop new file mode 100644 index 0000000..b48c19c --- /dev/null +++ b/Desktop/cam.desktop @@ -0,0 +1,8 @@ +[Desktop Entry] +Type=Application +Name=Cam +Comment=Cam +Exec=/home/modelec/Modelec-ROS2/start_cam_ros2.sh +Icon=utilities-terminal +Terminal=true +Categories=Utility; diff --git a/build.rasp.sh b/build.rasp.sh index 92cfa84..8ff766c 100755 --- a/build.rasp.sh +++ b/build.rasp.sh @@ -1,4 +1,4 @@ #!/bin/bash -MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential # --cmake-args -DCMAKE_BUILD_TYPE=Debug +MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential --cmake-args -DBUILD_FOR_RPI=ON --no-warn-unused-cli source install/setup.bash diff --git a/install.sh b/install.sh index 7cffb6b..2308872 100755 --- a/install.sh +++ b/install.sh @@ -21,7 +21,7 @@ sudo apt upgrade -y sudo apt install ros-dev-tools ros-jazzy-desktop -y -sudo apt-get install qt6-base-dev qt6-svg-dev libxml2-dev socat -y +sudo apt-get install qt6-base-dev qt6-svg-dev qt6-multimedia-dev libxml2-dev socat -y git submodule init git submodule update @@ -45,4 +45,4 @@ gio set ~/Desktop/no_lidar.joy.ros2_launch_marcel.desktop "metadata::trusted" tr gio set ~/Desktop/no_lidar.ros2_launch_marcel.desktop "metadata::trusted" true gio set ~/Desktop/joy.ros2_launch_marcel.desktop "metadata::trusted" true gio set ~/Desktop/ros2_launch_marcel.desktop "metadata::trusted" true -gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true \ No newline at end of file +gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true diff --git a/simulated_pcb/serial.modelec.py b/simulated_pcb/serial.modelec.py index ef22fa6..84fb4ba 100644 --- a/simulated_pcb/serial.modelec.py +++ b/simulated_pcb/serial.modelec.py @@ -35,16 +35,19 @@ def read_serial(ser, log_lines, log_lock): log_lines.append(f"[Serial Error] {e}") time.sleep(0.05) -def curses_main(stdscr, port, baudrate): +def curses_main(stdscr, port, baudrate, filter_start=None): global stop_thread curses.curs_set(1) stdscr.nodelay(True) stdscr.timeout(100) + if filter_start is None: + filter_start = ["SET;POS"] + log_lines = [] log_lock = threading.Lock() cmd_history = [] - history_index = -1 # For navigating command history + history_index = -1 with serial.Serial(port, baudrate, timeout=1) as ser: time.sleep(2) @@ -57,24 +60,36 @@ def curses_main(stdscr, port, baudrate): stdscr.clear() h, w = stdscr.getmaxyx() - # Split screen horizontally (70% logs, 30% command history) - split_x = int(w * 0.7) + # --- Split into 3 vertical columns --- + col_width = w // 3 + col1_x = 0 + col2_x = col_width + col3_x = 2 * col_width + log_height = h - 2 - # --- Left panel: logs --- + # --- Separate logs into filtered and others --- with log_lock: - visible_logs = log_lines[-log_height:] - for i, line in enumerate(visible_logs): - stdscr.addnstr(i, 0, line, split_x - 1) + filtered_logs = [line for line in log_lines if any(line.startswith(p) for p in filter_start)] + other_logs = [line for line in log_lines if not any(line.startswith(p) for p in filter_start)] + visible_filtered = filtered_logs[-log_height:] + visible_other = other_logs[-log_height:] + visible_history = cmd_history[-log_height:] - # --- Right panel: command history --- - stdscr.vline(0, split_x, "|", log_height) - history_start_x = split_x + 2 - stdscr.addstr(0, history_start_x, "Command History:") + # --- Column 1: Filtered serial output --- + stdscr.addstr(0, col1_x, f"Filtered ({', '.join(filter_start)}):") + for i, line in enumerate(visible_filtered): + stdscr.addnstr(i + 1, col1_x, line, col_width - 1) - visible_history = cmd_history[-(log_height - 2):] + # --- Column 2: Other serial output --- + stdscr.addstr(0, col2_x, "Other Logs:") + for i, line in enumerate(visible_other): + stdscr.addnstr(i + 1, col2_x, line, col_width - 1) + + # --- Column 3: Command history --- + stdscr.addstr(0, col3_x, "Command History:") for i, cmd in enumerate(visible_history): - stdscr.addnstr(i + 1, history_start_x, cmd, w - history_start_x - 1) + stdscr.addnstr(i + 1, col3_x, cmd, col_width - 1) # --- Input line --- stdscr.addstr(log_height, 0, "-" * (w - 1)) diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index 329f175..0ebc74b 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +option(BUILD_FOR_RPI "Build with Raspberry Pi libcamera support" OFF) + # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -16,6 +18,18 @@ find_package(ament_index_cpp REQUIRED) find_package(fmt) find_package(OpenCV REQUIRED) +find_package(PkgConfig REQUIRED) + +if(BUILD_FOR_RPI) + message(STATUS "Configuring for Raspberry Pi Hardware...") + add_definitions(-DRPI_BUILD) + pkg_check_modules(LIBCAMERA REQUIRED libcamera libcamera-base) + include_directories(${LIBCAMERA_INCLUDE_DIRS}) + link_directories(${LIBCAMERA_LIBRARY_DIRS} /usr/local/lib/aarch64-linux-gnu) + add_definitions(${LIBCAMERA_CFLAGS_OTHER}) +else() + message(STATUS "Configuring for PC (standard OpenCV)...") +endif() find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) @@ -57,6 +71,16 @@ target_link_libraries(color_detector modelec_utils::config ) +if(BUILD_FOR_RPI) + target_link_libraries(color_detector + -Wl,--no-as-needed + "/usr/local/lib/aarch64-linux-gnu/libcamera.so" + "/usr/local/lib/aarch64-linux-gnu/libcamera-base.so" + -Wl,--as-needed + cam2opencv + ) +endif() + target_include_directories(color_detector PUBLIC $ $ diff --git a/src/modelec_com/include/modelec_com/color_detector.hpp b/src/modelec_com/include/modelec_com/color_detector.hpp index 8c8c8fc..bf3e8f1 100644 --- a/src/modelec_com/include/modelec_com/color_detector.hpp +++ b/src/modelec_com/include/modelec_com/color_detector.hpp @@ -5,9 +5,18 @@ #include #include #include +#include +#include +#include + +#ifdef RPI_BUILD +#include +#endif namespace Modelec { + struct CamCallback; + struct ColorSetting { std::string name; @@ -34,6 +43,17 @@ namespace Modelec std::string generateImagePath() const; + #ifdef RPI_BUILD + Libcam2OpenCV camera_; + std::unique_ptr my_callback_; + #else + cv::VideoCapture pc_cap_; + #endif + + cv::Mat latest_frame_; + + std::mutex frame_mutex_; + rclcpp::Service::SharedPtr service_; rclcpp::Subscription::SharedPtr ask_sub_; @@ -49,4 +69,45 @@ namespace Modelec std::vector color_configs_; }; + + template<> + inline std::vector + Config::get>( + const std::string& prefix, + const std::vector& default_value, bool) + { + auto result = Config::getArray( + prefix, + [](const std::string& base) + { + return ColorSetting{ + Config::get(base + "@name"), + Config::get(base + "@hue_min"), + Config::get(base + "@hue_max") + }; + }); + + return result.empty() ? default_value : result; + } + + template<> + inline std::vector + Config::get>( + const std::string& prefix, + const std::vector& default_value, bool) + { + auto result = Config::getArray( + prefix, + [](const std::string& base) + { + return cv::Rect( + Config::get(base + "@x", 0), + Config::get(base + "@y", 0), + Config::get(base + "@w", 100), + Config::get(base + "@h", 100) + ); + }); + + return result.empty() ? default_value : result; + } } diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp index f73aa0c..f0d1919 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -12,6 +12,8 @@ #include #include +#include "modelec_utils/config.hpp" + #define TIMER_SERVO_TIMED_MS 10 // 100 Hz namespace Modelec @@ -37,10 +39,10 @@ namespace Modelec std::map asc_value_mapper_; int asc_state_ = 0; - std::map servo_value_; - std::map relay_value_; + std::unordered_map servo_value_; + std::unordered_map relay_value_; - std::map servo_timed_buffer_; + std::vector servo_timed_buffer_; rclcpp::TimerBase::SharedPtr servo_timed_timer_; @@ -82,10 +84,29 @@ namespace Modelec void SendToPCB(const std::string& order, const std::string& elem, const std::vector& data = {}); - // TODO redo thos func to accept arrays, poc without them atm void GetData(const std::string& elem, const std::vector& data = {}); void SendOrder(const std::string& elem, const std::vector& data = {}); void SendMove(const std::string& elem, const std::vector& data = {}); void RespondEvent(const std::string& elem, const std::vector& data = {}); }; + + template<> + inline std::unordered_map + Config::get>( + const std::string& prefix, + const std::unordered_map& default_value, bool) + { + auto result = Config::getMap( + prefix, + [](const std::string& base) + { + return std::make_pair( + Config::get(base + "@id"), + Config::get(base + "@angle") + ); + }); + + return result.empty() ? default_value : result; + } + } \ No newline at end of file diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index 4e5ab2e..79e19fe 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -19,6 +19,8 @@ #include +#include "modelec_utils/config.hpp" + namespace Modelec { class PCBOdoInterface : public rclcpp::Node, public SerialListener @@ -37,9 +39,11 @@ namespace Modelec struct PIDData { - float p; - float i; - float d; + float p = 0.0f; + float i = 0.0f; + float d = 0.0f; + std::optional min; + std::optional max; }; private: @@ -95,7 +99,26 @@ namespace Modelec void GetPID(); void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg); void SetPID(std::string name, float p, float i, float d, std::optional min = std::nullopt, std::optional max = std::nullopt); + void SetPID(std::string name, const PIDData& pid_data); void SetMotor(int left, int right); }; + + template<> + inline PCBOdoInterface::PIDData + Config::get( + const std::string& prefix, + const PCBOdoInterface::PIDData& default_value, bool) + { + PCBOdoInterface::PIDData result; + result.p = Config::get(prefix + "@p", default_value.p); + result.i = Config::get(prefix + "@i", default_value.i); + result.d = Config::get(prefix + "@d", default_value.d); + if (Config::has(prefix + "@min")) + result.min = Config::get(prefix + "@min"); + if (Config::has(prefix + "@max")) + result.max = Config::get(prefix + "@max"); + return result; + } + } // namespace Modelec diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp index 80615d8..c8d7d19 100644 --- a/src/modelec_com/src/color_detector.cpp +++ b/src/modelec_com/src/color_detector.cpp @@ -1,48 +1,42 @@ #include #include #include +#include + +#ifdef RPI_BUILD +#include +#endif namespace Modelec { +#ifdef RPI_BUILD + struct CamCallback : Libcam2OpenCV::Callback { + cv::Mat* target_frame; + std::mutex* frame_mutex; + CamCallback(cv::Mat& frame, std::mutex& mtx) : target_frame(&frame), frame_mutex(&mtx) {} + void hasFrame(const cv::Mat &frame, const libcamera::ControlList &) override { + std::lock_guard lock(*frame_mutex); + frame.copyTo(*target_frame); + } + }; +#endif ColorDetector::ColorDetector() - : Node("color_detector") + : Node("color_detector") { - this->declare_parameter("enabled", true); - this->declare_parameter("link", ""); - this->declare_parameter("headless", true); - this->declare_parameter("save_to_file.enabled", true); - this->declare_parameter("save_to_file.path", "./"); - this->declare_parameter("rois", rclcpp::PARAMETER_INTEGER_ARRAY); - this->declare_parameter("colors.blue", rclcpp::PARAMETER_INTEGER_ARRAY); - this->declare_parameter("colors.yellow", rclcpp::PARAMETER_INTEGER_ARRAY); + 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()); + } - enable_ = this->get_parameter("enabled").as_bool(); - link_ = this->get_parameter("link").as_string(); - headless_ = this->get_parameter("headless").as_bool(); - save_to_file_ = this->get_parameter("save_to_file.enabled").as_bool(); - save_directory_ = this->get_parameter("save_to_file.path").as_string(); + enable_ = Config::get("config.cam.enabled", false); + link_ = Config::get("config.cam.link", "/dev/video0"); + headless_ = Config::get("config.cam.headless", true); + save_to_file_ = Config::get("config.cam.save_to_file.enabled", false); + save_directory_ = Config::get("config.cam.save_to_file.directory", "./"); - auto rois_param = this->get_parameter("rois").as_integer_array(); - for (size_t i = 0; i + 3 < rois_param.size(); i += 4) - { - rois_.emplace_back(rois_param[i], rois_param[i + 1], rois_param[i + 2], rois_param[i + 3]); - } - - auto blue_param = this->get_parameter("colors.blue").as_integer_array(); - if (blue_param.size() >= 2) - { - color_configs_.push_back({ - "blue", static_cast(blue_param[0]), static_cast(blue_param[1]) - }); - } - - auto yellow_param = this->get_parameter("colors.yellow").as_integer_array(); - if (yellow_param.size() >= 2) - { - color_configs_.push_back({ - "yellow", static_cast(yellow_param[0]), static_cast(yellow_param[1]) - }); - } + rois_ = Config::get>("config.cam.rois.roi", {}); + color_configs_ = Config::get>("config.cam.colors.color", {}); if (!enable_) { @@ -51,6 +45,22 @@ namespace Modelec return; } +#ifdef RPI_BUILD + RCLCPP_INFO(get_logger(), "Starting RPi Libcamera (Full FOV mode)..."); + Libcam2OpenCVSettings settings; + settings.width = 2304; + settings.height = 1296; + settings.framerate = 30; + settings.cameraIndex = 0; + + my_callback_ = std::make_unique(latest_frame_, frame_mutex_); + camera_.registerCallback(my_callback_.get()); + camera_.start(settings); +#else + RCLCPP_INFO(get_logger(), "Starting PC Webcam (OpenCV)..."); + pc_cap_.open(link_); +#endif + service_ = create_service( "action/detect_color", std::bind(&ColorDetector::onRequest, this, @@ -85,6 +95,8 @@ namespace Modelec if (!headless_) { cv::namedWindow("color_detector", cv::WINDOW_NORMAL); + + cv::setWindowProperty("color_detector", cv::WND_PROP_AUTOSIZE, cv::WINDOW_GUI_EXPANDED); } RCLCPP_INFO(get_logger(), "Color detector service ready"); @@ -92,6 +104,12 @@ namespace Modelec ColorDetector::~ColorDetector() { +#ifdef RPI_BUILD + camera_.stop(); +#else + if(pc_cap_.isOpened()) pc_cap_.release(); +#endif + if (!headless_) { cv::destroyAllWindows(); @@ -100,21 +118,21 @@ namespace Modelec bool ColorDetector::processSnapshot(std::vector& colors, std::string& error) { - cv::VideoCapture cap(link_); - - if (!cap.isOpened()) - { - RCLCPP_ERROR(get_logger(), "Failed to open camera at %s", link_.c_str()); - error = "Failed to open camera"; - return false; - } - - cap.set(cv::CAP_PROP_BUFFERSIZE, 1); - cv::Mat frame; - - for (int i = 0; i < 3; ++i) - cap >> frame; +#ifdef RPI_BUILD + { + std::lock_guard lock(frame_mutex_); + if (latest_frame_.empty()) { + RCLCPP_WARN(get_logger(), "No frame received from libcamera yet"); + error = "Empty frame"; + return false; + } + latest_frame_.copyTo(frame); + } +#else +if(!pc_cap_.isOpened()) { error = "PC Cam not open"; return false; } +pc_cap_ >> frame; +#endif if (frame.empty()) { @@ -139,11 +157,9 @@ namespace Modelec { std::string path = save_directory_ + generateImagePath(); cv::imwrite(path, frame); - RCLCPP_DEBUG(get_logger(), "Saved snapshot to %s", path.c_str()); + RCLCPP_INFO(get_logger(), "Saved snapshot to %s", path.c_str()); } - cap.release(); - return true; } diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 7f6b17e..3a4a2b5 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -1,19 +1,31 @@ #include #include #include +#include +#include +#include namespace Modelec { PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface") { - // Service to create a new serial listener - declare_parameter("serial_port", "/dev/USB_ACTION"); - declare_parameter("baudrate", 115200); + 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()); + } - auto serial_port = get_parameter("serial_port").as_string(); - auto baudrate = get_parameter("baudrate").as_int(); + std::string action_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/action.xml"; + if (!Config::load(action_path)) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", action_path.c_str()); + } - RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate); + auto serial_port = Config::get("config.usb.action.port", "/dev/ttyUSB0"); + auto baudrate = Config::get("config.usb.action.baudrate", 115200); + auto timed_servo_timer_ms = Config::get("config.timer.action.timed_servo.ms", TIMER_SERVO_TIMED_MS); + + RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %d", serial_port.c_str(), baudrate); asc_get_sub_ = this->create_subscription( "action/get/asc", 10, @@ -157,14 +169,14 @@ namespace Modelec { ServoTimedSet servo_timed_set; servo_timed_set.servo_timed = item; - servo_timed_set.start_time = now; - servo_timed_set.end_time = now + rclcpp::Duration::from_seconds(item.duration_s); + servo_timed_set.start_time = now + rclcpp::Duration::from_seconds(item.delay_s); + servo_timed_set.end_time = now + rclcpp::Duration::from_seconds(item.duration_s + item.delay_s); servo_timed_set.active = true; RCLCPP_DEBUG(this->get_logger(), "Scheduled timed move for Servo ID %d from %.3f to %.3f over %.3f seconds", item.id, item.start_angle, item.end_angle, item.duration_s); - servo_timed_buffer_[item.id] = servo_timed_set; + servo_timed_buffer_.push_back(servo_timed_set); } }); @@ -172,23 +184,32 @@ namespace Modelec "action/move/servo/timed/res", 10); servo_timed_timer_ = this->create_wall_timer( - std::chrono::milliseconds(TIMER_SERVO_TIMED_MS), + std::chrono::milliseconds(timed_servo_timer_ms), [this]() { + if (servo_timed_buffer_.empty()) { + return; + } + rclcpp::Time now = this->now(); modelec_interfaces::msg::ActionServoTimedArray servo_timed_msg; - std::map to_send; + std::vector> to_send; - for (auto& [id, servo_timed_set] : servo_timed_buffer_) + for (auto& servo_timed_set : servo_timed_buffer_) { + if (servo_timed_set.active && now.nanoseconds() < servo_timed_set.start_time.nanoseconds()) + { + continue; + } if (servo_timed_set.active && now.nanoseconds() >= servo_timed_set.end_time.nanoseconds()) { RCLCPP_DEBUG(this->get_logger(), "Timed move for Servo ID %d completed. Setting to final angle %.3f", - id, servo_timed_set.servo_timed.end_angle); + servo_timed_set.servo_timed.id, servo_timed_set.servo_timed.end_angle); + + to_send.emplace_back(servo_timed_set.servo_timed.id, servo_timed_set.servo_timed.end_angle); - to_send[id] = servo_timed_set.servo_timed.end_angle; servo_timed_set.active = false; servo_timed_msg.items.push_back(servo_timed_set.servo_timed); @@ -199,12 +220,13 @@ namespace Modelec double duration = (servo_timed_set.end_time - servo_timed_set.start_time).seconds(); double progress = elapsed / duration; - RCLCPP_DEBUG(this->get_logger(), "Servo ID %d progress: %.3f | %.3f %.3f", id, progress, elapsed, duration); + RCLCPP_DEBUG(this->get_logger(), "Servo ID %d progress: %.3f | %.3f %.3f", + servo_timed_set.servo_timed.id, progress, elapsed, duration); double intermediate_angle = servo_timed_set.servo_timed.start_angle + - progress * (servo_timed_set.servo_timed.end_angle - servo_timed_set.servo_timed.start_angle); + progress * (servo_timed_set.servo_timed.end_angle - servo_timed_set.servo_timed.start_angle); - to_send[id] = intermediate_angle; + to_send.emplace_back(servo_timed_set.servo_timed.id, intermediate_angle); } } @@ -228,47 +250,22 @@ namespace Modelec servo_move_timed_res_pub_->publish(servo_timed_msg); } + servo_timed_buffer_.erase( + std::remove_if( + servo_timed_buffer_.begin(), + servo_timed_buffer_.end(), + [&](const ServoTimedSet& s) + { + return s.active == false; + }), + servo_timed_buffer_.end()); + + }); this->open(baudrate, serial_port, MAX_MESSAGE_LEN); - // TODO : check for real value there - /*asc_value_mapper_ = { - {0, 0}, - {1, 100}, - {2, 200}, - {3, 300} - };*/ - /*for (auto & [id, v] : asc_value_mapper_) - { - SendOrder("ASC", {std::to_string(id), std::to_string(v)}); - }*/ - - // asc_state_ = 3; - - // SendMove("ASC", {std::to_string(asc_state_)}); - - // rclcpp::sleep_for(std::chrono::milliseconds(100)); - - /*servo_value_ = { - {0, 1}, - {1, 1}, - {2, 0}, - {3, 0}, - {4, 1}, - {5, 0} - };*/ - - servo_value_ = { - {0, 2.93}, - {1, 0.91}, - {2, 3.05}, - {3, 0.3}, - {4, 1}, - {5, 1}, - {6, 1}, - {7, 1}, - }; + servo_value_ = Config::get>("action.init.servo"); std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";"; @@ -280,31 +277,6 @@ namespace Modelec data += "\n"; SendToPCB(data); - - /*relay_value_ = { - {1, false}, - {2, false}, - {3, false}, - };*/ - - /*rclcpp::sleep_for(std::chrono::milliseconds(100)); - - data = "SET;RELAY;STATE;"; - data += std::to_string(relay_value_.size()) + ";"; - - for (auto & [id, v] : relay_value_) - { - data += std::to_string(id) + ";" + std::to_string(v) + ";"; - } - - SendToPCB(data);*/ - - /*for (auto & [id, v] : relay_value_) - { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - - SendMove("RELAY" + std::to_string(id), {std::to_string(v)}); - }*/ } PCBActionInterface::~PCBActionInterface() @@ -533,7 +505,6 @@ namespace Modelec SendToPCB(command); } - // TODO CHANGE TO AAA;BBB;N;X;Y;X;Y;X;Y where N is number of elem you want to send void PCBActionInterface::GetData(const std::string& elem, const std::vector& data) { SendToPCB("GET", elem, data); diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 138a3c6..514fc98 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -1,17 +1,22 @@ #include #include +#include +#include namespace Modelec { PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface") { - declare_parameter("serial_port", "/dev/USB_ODO"); - declare_parameter("baudrate", 115200); + 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()); + } - auto serial_port = get_parameter("serial_port").as_string(); - auto baudrate = get_parameter("baudrate").as_int(); + auto serial_port = Config::get("config.usb.odo.port", "/dev/ttyUSB0"); + auto baudrate = Config::get("config.usb.odo.baudrate", 115200); - RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate); + RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %d", serial_port.c_str(), baudrate); odo_pos_publisher_ = this->create_publisher( "odometry/position", 10); @@ -104,10 +109,10 @@ namespace Modelec this->open(baudrate, serial_port, MAX_MESSAGE_LEN); - SetPID("THETA", 14, 0, 0); - SetPID("POS", 5, 0, 0); - SetPID("LEFT", 5, 0, 0); - SetPID("RIGHT", 5, 0, 0); + SetPID("THETA", Config::get("config.odo.pid.theta")); + SetPID("POS", Config::get("config.odo.pid.pos")); + SetPID("LEFT", Config::get("config.odo.pid.left")); + SetPID("RIGHT", Config::get("config.odo.pid.right")); } PCBOdoInterface::~PCBOdoInterface() @@ -431,6 +436,11 @@ namespace Modelec SendOrder("PID", data); } + void PCBOdoInterface::SetPID(std::string name, const PIDData& pid_data) + { + SetPID(name, pid_data.p, pid_data.i, pid_data.d, pid_data.min, pid_data.max); + } + void PCBOdoInterface::SetMotor(int left, int right) { std::vector data = { diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 72c9fb1..c53d8a0 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -14,7 +14,6 @@ from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - pkg_name = 'modelec_core' # ------------------------------------------------- # Launch arguments # ------------------------------------------------- @@ -50,12 +49,6 @@ def generate_launch_description(): angle_compensate = LaunchConfiguration('angle_compensate', default='false') scan_mode = LaunchConfiguration('scan_mode', default='Standard') - param_file = os.path.join( - get_package_share_directory(pkg_name), - 'config', - 'config.yml' - ) - # ------------------------------------------------- # RPLIDAR with auto-restart # ------------------------------------------------- @@ -64,7 +57,7 @@ def generate_launch_description(): package='rplidar_ros', executable='rplidar_node', name='rplidar_node', - parameters=[param_file, { + parameters=[{ 'channel_type': channel_type, 'serial_port': serial_port, 'serial_baudrate': serial_baudrate, @@ -113,7 +106,6 @@ def generate_launch_description(): package='modelec_gui', executable='modelec_gui', name='modelec_gui', - parameters=[param_file], ) shutdown = RegisterEventHandler( @@ -135,19 +127,17 @@ def generate_launch_description(): package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface', - parameters=[param_file], ), Node( package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface', - parameters=[param_file], ), ] return [] # ------------------------------------------------- - # Strategy nodes (WITHOUT enemy_manager) + # Strategy nodes # ------------------------------------------------- def launch_strat(context, *args, **kwargs): if context.launch_configurations.get('with_strat') == 'true': @@ -156,20 +146,18 @@ def generate_launch_description(): package='modelec_strat', executable='strat_fsm', name='strat_fsm', - parameters=[param_file], # prefix=['xterm -e gdb -ex run --args'], ), Node( package='modelec_strat', executable='pami_manager', name='pami_manager', - parameters=[param_file], ), ] return [] # ------------------------------------------------- - # Enemy manager (standalone) + # Enemy manager # ------------------------------------------------- def launch_enemy_manager(context, *args, **kwargs): if context.launch_configurations.get('with_enemy_manager') == 'true': @@ -178,7 +166,6 @@ def generate_launch_description(): package='modelec_strat', executable='enemy_manager', name='enemy_manager', - parameters=[param_file], ) ] return [] @@ -193,7 +180,6 @@ def generate_launch_description(): package='joy', executable='joy_node', name='joy_node', - parameters=[param_file], ) ] return [] @@ -205,7 +191,6 @@ def generate_launch_description(): package='modelec_com', executable='color_detector', name='color_detector', - parameters=[param_file], ) ] return [] diff --git a/src/modelec_gui/CMakeLists.txt b/src/modelec_gui/CMakeLists.txt index 1128147..3e10d48 100644 --- a/src/modelec_gui/CMakeLists.txt +++ b/src/modelec_gui/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(Qt6 COMPONENTS Gui Widgets Svg + Multimedia REQUIRED) # Add the executable @@ -63,6 +64,7 @@ target_link_libraries(modelec_gui Qt6::Gui Qt6::Widgets Qt6::Svg + Qt6::Multimedia modelec_utils::utils modelec_utils::config ) diff --git a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp index 3336b87..83cf35c 100644 --- a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp +++ b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp @@ -10,7 +10,8 @@ #include #include #include - +#include +#include namespace ModelecGUI { @@ -50,5 +51,11 @@ namespace ModelecGUI QAction* toggle_show_obstacle_action_; + QMediaPlayer* media_player_; + QAudioOutput* audio_output_; + + rclcpp::Subscription::SharedPtr audio_sub_; + rclcpp::Subscription::SharedPtr quit_sub_; + }; } diff --git a/src/modelec_gui/resource.qrc b/src/modelec_gui/resource.qrc index 327da12..c0828fc 100644 --- a/src/modelec_gui/resource.qrc +++ b/src/modelec_gui/resource.qrc @@ -9,5 +9,6 @@ img/logo/ISEN-Nantes-fond-blanc.png img/logo/modelec.png img/wood.jpg + sound/test.mp3 \ No newline at end of file diff --git a/src/modelec_gui/sound/test.mp3 b/src/modelec_gui/sound/test.mp3 new file mode 100644 index 0000000000000000000000000000000000000000..87404f3d5e130b9db6ae6579b7be60106b7ab8b5 GIT binary patch literal 52662 zcmXV%Wl$V#*R313L4pMf?gWBMut9>mYjAgW7+iw81Pv10-JRf0aEIU!g3BD9_dD}z zs=BJHrl+sH_FjA4(5Gor06@YrT^OX;Per`py3`0(1H`3FY-=7p?3sB1oR$uD=$MqVPKjI){+ID{Ew*v=Ww1p z{Ioo>h@+Z}OPmNY3o23q!?ld^?rhtDKJSo${#qI^J=hfFKvVdm+&HCYWa)F`^ z@rCW~xHe(`muH^~T>qD+r_Gt2&g|DeSG%JIGds0ff73j#-tNJ$-Qjr&cMSl5qahG? z2_ShIA`M^O^#DBpyh#th@OFU?S(NNinTXTwOau-`pOBzJgUK6`PD_lJ^g|+s2p*XN z)HoA`02?R@4-&=6y$qq)2Ho*ZS`q|z_g?zu2`OQLP+&0k;Yf1tzmo~GkZpytK%ww( zaQTQz@`QXTIAk$gY-03SJaei<2Jp#gcsUtHY|&8}(}AIZ9FVO`?O|B!0W3686JP<5 z%G!rTkToaIM{#crf@hZ_8Xr+FZkNjz%wk$=CGwR*=5tnZ^Y3T;JZx|T=Q+00cc6u3 z$S7Aw&wfNLCxa4yjAwUfb;_pNV>ivJL865YQ$DyCV8WG$%)Wq0Q)hKAPyO%oq!M=i z=gC-p>4%Cx`$~hEjb6zwqYwV&hTHlnGDR~>=8>1>jAvQ6E~)L;btfJy+vaEIvJR{D zEY+l;A`1WTM~lwK%R*m@Qpak`#|-Z7a-nWxUfa30>#zDR{=%{9R!;Rkcda5xyrVT-Ds3#Tw^~BhGVl%yjA+G_ zk!^?uxwM?~R>@#CDV)Pq9JZ5p3C_Qu4?w*B-uzKcVeGe(>v4FTy3b4UEirzs`r?a) zaNQ)_8Wq=i@=Ci@xF7q|h%*3sTMK>tX$6y~@!F8V z&~(iEz)gi=S5PT4-ZLpz{UhzI zupX)r?+JaH6(UfQk(5#-fubA{KQcE%SwHMBTPi)RVMim`C-p4;H z-&HjHI=@DGEDLTIWcFs~5mOm*h-g=+_<@t!RIQi}lKSCHEGSUl9b@`yrNnfteRERy z_Qnf|ulnt&U);3ZP3rFmLnENCiKx=_NfcZpq5O7jXH8tNVBH}{n%1Z=WB81jV+;&l zEhz*w5TcoA`M)fuG-HOu0{jq~Naa*Hb?bZNgGTB27o7y&ce2ZxpXm^G#9nG6J8R*m zShq4VDjSy{dsg~6en^c`pnpMz zf8&LD!N2(Oa`R`2Gh~ce?lCgp84BG19%LudxH`%+49Sk+HwWkIfuTG(O6Sdi^bKFqWr5dso z=6qQ=jS51r71_Cj3FZKH5v|@`7^3{xp`fd#vgQsB}se1IZ zXKUoWaFV}d5|IB|6h+p0syX;%5$VY{PGQ14%y{}@FRX66vt>$p_z$g>r<#i6gN$t4 zLLW(3cgT`VZPr5psiN|i2imo@O^n>_Ej?1Rju0-gx(+qYI0RICWAcXmhW%E%51i)sXIEso_-?$6NGQywDoj zz`cwyFq1$2V+apGmkb(~DLKwnkvpPv ztGll==YYHaw02#;ABROqQ+`3?U;cN>k85i4KE8lg#LKTc%?(OaPX@j*#S4X*w772y`H(}{QX8imxrx;`mB;Aw|F2e6uRI2S_6y7C{2SKMO~0m zJ8Z|T8NZX1r6mdYlM~<~(;9_QLqKAsAdi?f_I+YxAP9Yts-Tn$t_oM_q+T4= znwJd9NEPR+1m`5w?!wKpxgUHtk!6~HRO0B$RVyoslbf5#!APHM)HTNXDP5&q33psJ z%y^v9clL7g=^S%=KMSX$Ud`jLh3;)IOh3_5fbinWO)GUo&f+U`*#!vM|LXgu)0IRi z#m++)L)qa4;nR@YE7wiT(jqSm!gZma$R;!gdcz4L@)Pn_k^!Ljd+e&5MvNGy_*OdB z6-q=n2&@E>=0>Dvb*!Z@3q}MIu$W3pvqI@%9uzcbEZKUfV7Aaoj{2iTsL8Xj3cPdd z+bzO1-|6!pYZofIkZP|>gj4+~f}`-}RINxTw;Ng3mrjovs`HvsIhK_7f83Z?qBHbi zHWqSds3p*)>KLm3Np`SWMP+qP@1}{W6)SVJGhnj7^&OhC0Bh4$=%Bk@s%ayUI&Jz_ z{~tk|k@|@vIBNsIMINeAKHU?3Wq|(8LstV}S1IBl(u=2Q{)VTN`ADd|+r*cB;>qt~ zuu#JZz-5XdsIY*hr=zy3v8D)brm1KF8+^5$IyESBC*lplGPS4V#nBd_QuecY<2Zv2pk;;<8jL+uvZJxPiheS`TUti zgOOnRrG}lSY=^D2FKV&e&@}d1@q*Oy#>OS0=PDK#QA%Y@YPrISLSUTR{ivL@c@3GT zi&FRp)ll;VshRdiUb!^Q$?v`u5!Km2l-W~kdy5V+iq`aGsb#QPY9!DcGeA~4nJ$<wc+aipt1IqUMV;^_GDs21R{EQ{(4SR0hHs@P7TV)*w7?<@=tt zY#?xkMYN_iP3;(pmr{Eoj%YFUb!e?QT-v;e#J$Si!Kk=g$Z%&d<;3T%>qIuaL0^YU z`>SX$k{oAa-e9Qpd1EnGhr^QgP^tZx20^9=lU{=AH1@$s@@MlJ6E8cO0>v~f!!_w^ z$(8<7vBpwrpRe3UoDX-!du_t6^zw!Pkj8SLl##Ay3`S&a;d+k@eg~mkY7|=eDy90&Rhmj4igj?USLAXaO`GZp+J)f~bZrjV z`P~?^Or3(Z_12C31ojr?(G7|t8#}#(r`iS%=m~lJSsDpZTxDmARCas5)|(C%-_};w zXhJcl2^!41GdYPZevw`{nU`MR{j&_fGZ&|sTDmD;9eDETu)nL0={)ce>ReY8*x-~E zFd8&jWpgQ8G!bbIxGw^5Z$IM0!eFCVtL&wyX#svTk^%%3aUJiAN-p1lhhm?dN84UbkA3qpY)qmQO6!XOf=jEVA`pqw)E>u3pkPE;mtVAf(3Pt$`o>)S#XsLg$-v z7tK@r&<}S5-SlzUN@8o}!)X?4)7#P-TKaw>B3aLk^VpC4$$-;as&h)Ki_?`#$?v=Wv zjpVpJv|i)s=(K8UW+LIzRo9i8y%*gKD$@4(tYyV$vIcP@Y~(V^-n8K&#OU&wN){UdChh20T5SW8>Vf=Nx0k9 z8`!*u-s`^?gYRbEVUEKw9F5Xy%+X+HD{g5a3!y>M#py7IqtE$xXmv7GnL&?Lq zpr(WIi-uJD#*g0OEx#qa!7K0c)ZlKP9a6W8xJ+4Qwx5@Ga`p`i*KxKNyU^?Qlf!4| z9dzT)sz9D}m*uT{>4#^8E#jQF*eaB;2qLKuf}lefNvuSvgGTDczdIYhN3a>x%8D>) z#U1V)vJAQ3MN+B!0@CXl!bKE=9O?Zu8TQqJRp$7-1CNsQ?d+!!77J$WCiEru_6n7JFADrtO_QY)#%P*d8W3S2 zgfo$rG4sHH2hBR*fz7mA_WjL_l*vgunXJ7SjeYKLveNth7u>MURUaKZi{(J@r}1L>kA;EfGNv^j zwO>n7nu()-$Nk5U2n4CjbjjWU0B&sCN+uBKlVfQw?>>8>!}43_Tx>(^oh{% zxhqk*vj-7oX)EV~j_&8iw)q(bIrB7~_A#|(`|j25hdbeveMMQDy6nHYF<4T!4R2!l z2o41R7%+-<_td51NHBV-ZGT_yK~E9ebb%q8F}qP8u8T4wD-4Y-N6lU^K8t^^nEaZO zmDCA_O&DK(I4_1q^ySbUb)CeP(Ry0ntc8oLO_xN&TUh65dHru_{&ww?`EK z(RH&AH;eygwx0Z@dftn+0B>19xi_q!Ub(us+!+r;+Qg+{(Quih%NtL_glM)6)iT+f zgQ2)Hj_=+g`Rf914uyBDa&lQ#`;dBDT~_WbS1!llHR;r(l8Fn-7~pvrm1yOnU;gQrMri>nKZ#rgXRt+wV$f%ZxEYb6e};qVW;G!;**Uso1~r-uJ+ zF1}W87|NO&w5xX|o=WU& zL_tMyy6@a+^B%F3vyHqVup!EOf54Z(Q^zPHvV4%Qs4+tL$=gc#Zo^ zSm(U>@;&%_sn?D!wnvwI@GyW*EnYZ$USwc6x-P7BZYZnb@($~C{|k8}B8T4&?o*-A z?`DFc{mnfN8jR$Y`CRvrx50dRWQIO=4YH;5{g(-QkCdmsp`o=;hGCfT)pLks_7h!8-elbuI&0f1qYN64fhK)4{Qx7_jODO54@uwTNQ7216KI9;kK z%MU3`uJmKde6qt-Y}gl+TiiL#ltdS-t--4D)-v>hud79IHm5-8Vq4x zAdL$BX;4n5K-b=_CjHLrshwH#t+MyaqrU1wL1mZ*SEb&$!RltL_M0}Hd#P^6_+-9p zSMXb(b3MdVU4Wdb{}#>?chtMmdo(cc_%Q$7d>+Vs@ddkvY6tfo7_g+flKZeeP7uNp zjK_YYVTp$zCsB*y0PrJ0D2okB)F7C`NR_;7Eil^5@}#IN<0sczIUTg*hUFjh2F%)v z=Bgl+rogX~jk{4JS@j9*=Pl?ufZ9x9Cr)baV^URr<6;hiI-35NX2=p<^gY0c<1Ibm=lW}K~`OfahXw`HLrr;5UD?SFf5%MnQu}W zuu}174+cPz)4hiD@Y}92!SX4rP9k!!s7T0$5<#Xn5K^LV%JY`-Mi9nPmUrH{zi?Ho z44MeIcJ)sOnbet*CyFgho0(YMrS_CH(jAL1)bI!cTp9)C17v5?MxG?!H#0Qy%d%86 zCroNzyqxY|Kb17|xo;frrg`XOLfM|LHk~FJ!mSuSSts7KEzu}fx~or`#XiOHkMrjS zd}MffSVaro;*es*Vf=uQLrIq04SgLzNaTP~X4lQ!Y~F-IQSRYO^C+=$`Ud5Vb#`z= zK`waSUqrznL}L734hVP15rKZblo>)4m?+>LbYM-%`ivv0#SHU-C$>~?{{(Lj^6piuGR9#0K88K7!x(2!vc5R)pv`Oa582O63NK)>d z{L^z4x_JlVF{0yDRn|9uadq;LvS0TzR&DW+WKt9ocTnHkof%du87VN+{6Z3>2ptUZkuU!E_!(+6O<%tKBC#Z=K69iz> zQNppQI~WYL3WbqHGfLxIqGyN9s-*Lv>bz_ps7tkq75D@rapykomQQhUE&kh2X&1AV zZtY}W+@Sbt_cw~ub|D5$OGC`X|Eg&B7gMv`$M%~*&%0ol=fu|i$>z>G=_Bsd4s*I0 zid`4h*$?>}l^FrpQW=gxuQe}MKNp*)78_rgG8Z4>?9BGvIu0(e$<>M6G67gd+i&7l zteL6W%|Ae?`*dUE>-9vy` zYyWBf_Ie6*xk0i)2F>+?K8`_IYz!U(p#DQzei2&_Bk|~O-3UOAzhl)(%_$A@OEJie zoikK6np1o|oS7}mo;Of~FD$Ij@i4n)Wnt60<$!6^nIz+q|Us5&}57)QdC^EYIi0t`?#x`dk1ozPI5f{L7;nmE7qS~eJB|Eb1xrJ9J8RvctIc6v{&iy? z8&?Wcn#(i3CGlE*hNFf%$AyK*I_!cXq>A)%OKBO#;-@IcdeV^hM1skUzF@eg)>E^= zg(Gy<)=^`T^jzoJ{(l|P)%mSp1Hg{RW$C|p+wWh5HI|zpXe2dEHr$smjpMkWIsl-) zMz>)_V~i3Q`AN-d^<4}fk8z=y!DSz7wA<* z=qt?`JPZ{?6o{f?!lWh@z#xDHN>I9%7qKOxz%z_Nr`QnmU+J7u5Z$z1I0Z>W#|K`i z0n+hOh(3xDLv&r03P&wlc`QeK3r!3nE|=FC`Jt}1Qf7p4;fd4dsWZACT}@{GH|zUh zxk}~tuF^|1JVF3slhq@$d7$?2u$ae3ru@(h9 ztK*J^3kKp>T2#uKpuV59#=&3=IndW{e<&a>%)=3ZCBN7LfGrCc}rl({$_<4TRF)ia@5Zl%dA+?2~ z-nd?=B4q1g@Vps0Y+T0b|0JEP_uB^ok<`O0WLB1~IPB&KQWJayWqmhJ+43wLYbEDK zXfE$2x<&HF1XoUUb2Q%eYi@vuRHBhnD|P%DoE$IKM3C;>H&Xdt%1h_cpm!Z1y>|MV zc_Uy)c*0Rz>gexiKOw9?(O@QU|Dn-Hna0;EZP=^lpbm;wIvTUrzb^uxMmUQi&C*1B zPrm1-vGz+5!INuOn<1YJE7=%KeTeuA&v`;(Mcy3;vvB#VvC)Tyw?q(q@?4ng|pQ&T?N}G&6`Psi|4_oMLUOzrwKB*A-6l7 z&7J#r*xz={u<8HRGHtUmUMw*zRQ6!lJ!0~f_>*W201@VsOd0{8G%XdMPHl>+L;yeA zV?wEe9OoX@g8Cmr#1OtMb&M8yYD>ItB`BiN|?w%3Ewh}Ff~rDO|5A1$Q}8I+c32M|#60j=k&Gi;bTSrN;f z?~|4b?&bJ|*+`?NdR596%U-_Q%5wQvTJuScIj1Vf+)Qc=pKFYfoW0Eo?L(Q)1{(WE zWlRgah!{6sJR6Nb z13;mFkr3fpms$_kU>W0CA2zp4tP|D_tOgjqDI&EMWF$vD4%9!VQu5SjWi!OOz(W!d zk{W9e&SaL;-BvWz)vVeSaIsn)DmD03y|VkzN8aK>>!~~K(EpWica5v#YAAD&XxJbp zAVpAN6WR}|{$0{pzE;wj#&o{VLNS{c|6WGgY&fr9UI_rwXVF8%MSd99uACLaps^sh z8yuG&;o-_Ec)+s5Y=NjB8b}#ip?#HoBeS0DPK+hoZu3*mlA+D8Z z4P+gogoq5uh@#JoeNAS>;+>dc3>@AmIWsB=H~4%3;<>15Tp6vQFTTsBGWD!&#VH>v zl(dYvo_^`u(;5$iw_c}vhuqYGhu-02xG{U|xgjsrJjDo7i~FsXJ;qKP}CeocYh+9DpDCX+@h zFR{ohxfH1{TFlHDOS+!2ky6`1Jn)TabU04v#_qgkYdGz5SAC_(ns9_azK!gz_QU<6 z@Zi-e^k4BS$JMuAmX2KBs!neUAC0mf=4#mF2*v3TR)iZekv{}3|R zkdD5b)-Ne<=|6_>00gM~LaGBf>@XVY(i=nCDP~%4F$K7fH02&V5a9kLBfhi^du3** zPqgd@mh~Ua&mB1h>$L3ETAKK~)Han2jiGgEO~&O*u)LN2n&7jRN=BxnULHQeuyPgI zd}lOV=U@6`X;d6I*pVMP8aJN#_%#|M)omGsPkx^vy79zv`xtm|EJV8rdSo}+H~I<{ zPOy+HazY<0Hmf_X*Q>5i<~x0m*?sS>)^?Nl`qmG0J8yeE0SF+?HnNr|W%XqUl>iQ4 z?YTzK6|Hep+Mv-<*wRNynl5#$_Nc)6|io_EAG~-Br<(ff7ho=on92Z+)kOcR{cKch|b=g)U!-DV{S2O zF==&ZYNMM+mWM0cr=wYyS=Hu5#&I&!ht$RAu~n_xctYXO;U@erw$x?c`g>vYuBN)p zw(e!O2fyzDpIP#SO6G6Muvx@8CrW!%f>nv{E?l5!<1EerXZCv z3CJ5^51dr2>ER*og+oY8O=AB1{R203oQ(ZOhmwvTJ#3n~V`U5TDCPM#>NZ14X)+55 z{jcVT4)@6s=Q&ZJbzNK;Rh;z5c?b7bdv!rJ;@AO(gX{GTIko($mQ9bnzstDRVa0sg zohKK+pIL4fpNzf7OHl7lSEd3c90V5Z^aY>?7 z5`kaP(Qwf7jcVGkK0zOoQvg&j3=CkNS;l~eLCATN1P7o;vAu(*Ls!U9F)~xpW7F^i{(D$IyTKAq@q)E!Yt>#$RtcR_$zki`EJNj-;IFgbM-#{`bwPOAeBRxwDL{ zoFjRx1XtT(4rhC7N)aKBM?WZ%{)OrgaS)Zj5)0{^et&d(#>hGLdn$iPg9g8URdjfi zE*xC3I~V_%Zij=tSuj3-7TkVRQ@3#(r`F@7)~;Ag^h!Jv8bQU{WO9GPcfa#Ovx z@8B!uOOn%OU(~jbWawLcxPd~SpKW}eHo9IrWXwNo5w!FIFrN4jK==iENR5n+z{Tkr zfSJa2xr+e91vAG;Z(R(Ar3R6s=ofP%rW9iVdF0HH4g(pH4Y(dEBN-zI7?z1}g9x?g zGYqWf2lKcLOi?2-@ApzdnE0lyZ7Q6OH0WJIisdonb12ef_yh_bF8%dLMHu1W+c)VEj z3RKm)cFZ^NCa*dVKM%3_DHd^@k6)4&0{YOe_E91XC$dM`@BI z%4CcU+#F&YI{TkdTYC>!hmpGGx56cHDVo1XXQf_ui#V5+=uMbC`x#iSuU($k?Pv0t zbvZa6wK3{qAE}cxd-=Wj2^{r&Ul+L_Qk|AT*6MW0<6~y(MeAeG>&4d11>xhQtIdzj zgze0cMnoBBy6JQ3qsjX7ZDh*Fi0Q@<9e{-ZfKkAmOP$s>I+TpSE)oFdqFGIJ@mVe2 z5fHt(EPZznq&Q}?O53d-m-i6d_yM8U$aix(Qt4^ZJFzvoPoVk|a~?s_Q`SSXU$%@X zZ{;&Skd(%lRo-F{l)W7_|^U!Kysc|d4#*VXJb zVGu@p$54x(DwNP?`>zbZ*aKoY?=Awk|13}EG=%Y(0^;Ma$&T@ZvULYVs+>_H$`97@ z38EXq|CdvMAc~l2DZF_K{YS?&lcoT{ps-5oH~`03Y%R$L0U*Uphp0yl+4(&bp`JW= z5d+Z(41!EzAdt~G`pkF-%STk#){0aL6pHD#_mq_a&iNizksRNQ^Cij^i)TY`7+sY0 z`ejX2m9F*EUt(R&cc{%Rs_A5+Yj0%yLj40U%G^T zUwzv?&)3APc)V4B2ZHcG22FM}(7u!ip&M^w$#+KSZO zV)ZJ`2XW6%z1NLOZNt(s&%Kv?a;}~oho(Mxw2huwH(8Hb3JZ@lHbSAdf17i-Z>IWE z2W(Wz-73{9miS)7=0+i!cLC6Ls6HpN6^NG#dL51j;{AfW7Zn;4>tj>2z=2{qJOl?D zg!DxmMwKucr!%ZBwO<CU(BZD{eZPNj=>?P z)k&Q^rRjHEZ?YUWaE}-@o0|#i8zGa8m4`1N3^a0Xm8<*uo0%ir=y$k&8QEF83h(nb zQtMPEre^7{JpPq}HXZ>?tL4tQ=f|ov{XSZmBo9w#zTC z*G*S{J+H!ZR+zoND|t@{Lk}@vF-g$par#DMO0;u*ieT1F{geP}H(ThpC9kT(@VngS#_{W6vV*F%(S=Lj3^-IHBpVb} zsVJlQXw5f?J!^tdWQPHOC2d!P$RD+NF)&}yr`eQ)!{SS2XXQBn*k5#*l##3%-_XK= z46t+<^l4iBJ%EU*tvUIY^%;@<{P5Px5$R``{HH^hb^puDiNwF_ee=3#Jq^C`Ix?&- zA?g^);fR_e>_|vENX86_GV55A>=nw(_Xg?dm3pAltTQfL;q5CksO1bZhF+fuG~%3~ z+*wJY%ps@9Nqzcyyfnz%2pf1RF&urh;iY9q%pFSn0=@ap*4^PQIIzCP(9R|@Pgm`f z-aIwgB@A_7;AY^2VqRQ$L*a?=F(TpuHlc^fFhYG<>D{t~M`)KIL;#lgH+oO(c3b?x_J zlub*figEq$`1%rUy?m)2*h@H|HKwepsD8iZB+G;Y#kOB8aCD_bJWjW#v&E0bivU24r1`+ zPOC!UDp12HB`2oLC!w0`$ZR>dGG+3q_~(B6zlD;U*c+SV#FgJmUaK6XqLbvvgB4t< z1|8OBWs3VN3U=8;J6F=C1)61d%-ij4^)^Bt8M_q>%mLT6UV3&T?yTSDZr8hq@7}tC zmjLKRcUX6BE)=?1n5vbwdaiX`G^x4-U@;rf_I@eOXws=norBP*V=yea1`zyl&JT9= z9sO?4AU+Klmkk?8#1p_qLrho{r_&p%r>OS>27TJ0S@@mf`8;5j<5GrYD@LY&{@{ueD;zb^|DcmFEs9U&2Ig=A_b z?$p*T@9eLWFpsMpRV4=Oi%;bfMEasdr8G{y7hVOO26hf?4%RjSI@NE%jDXiY=#$Bu z(B@qn{p9Q8`i^*H&@_;Yg^a+RY%h%|y=OyrnbMoDS*J*6oEDf4364}3HFWG}(Haa3 zt^_FmYvu3(gf+FL#kZ(Wi}jfNcn6>{!=>|IYJ0r;?7#I802bNEKg*NNp@5~P3DvUZ zBbV0NU@^wQn{e!hvpY6HI|`Hoiw-DX{NUB7am{tI{$ z8kxJ6A=`%=!_Ie#WPZ&xj&&hpIV4>@=wCPi04piYhN}$tZ%W)-Z-!T08jF(i)} zcqx?Z+GYpW@7YDQsC-^5K5dU-M{a^P;U}h5{PK`)zCAeh$l9GOQ+uUhy9!@n6u zmecwvfMQhgRWqy6e3EaW=ouPtA8_q8tD@DNTCS?v zI*V#OKV_duSd|dToDow|A$Ew}&0oedJB@rs&R$#}s_ceDr~&GbRJDOxPa!Br%Z@ac?vJgclDA!uI|dd=x)qS~mQx0gG>YIREA4zR=+u~ZO16W*Q zZ;&5{&aKVCtT=*V5U@{8<*miYdC&#?rsK3<0`s{kh>K$ZQpjes2bB1TDx7`157C(EdEa`!pz2dJ;fAh%wZ*K7S^rvL zDllwJF|s3@X{o#&ety-V&`A`|X;$muK0IJnN5+Z*_qKBf&?Qe2Lktl)2(Z>=K}4c7;k$> zbWuNMBqqd)!KD6H>A^B9QEXW37&YN8Or_c1(vRJh(HIQU8TtONomZwzQI^oyt=sj? zZ>%V`<>SGDUwa(Cxg=rO;h~w!<@mx=tG`~e$yIqu%w}n7x9~#VmO8AF6?sH!9%ocs zjPh6lgj3z_%JGelDlgtY%EBJ_39bZ@&@*{zB${Xe z0;jdFZ0`@UqesMWAVR^tlN+V%3SVZn-UU90uWNoN-EC;Ce;bFIqO%0wbV&V3+d?}H0y{!)gT#{u=rX5HC#ZSaH~f452MiMm zSKPZj^g$^5$N-*yd@v0&n21PTPXrq|xDr*f+#bExmM^d0*Fy!iX6rrsSFZ-XGQ8^P zeHR+rO1?=> zv3e*Qe~T$TTe55V?Fs=(dTh7GKitSA9Ar~k!AFDNxlMNrzw5TT?UMp3UR?x~@-$** zoxxeTDJ(guOViD1QC7y4rJGzW*`+wQQlCJIib zAnE`uajubN9<(YEw$XYaZo@Gf*|&pxS?24&o^v|(*LjLaomqKHgano< znSv(wli>>!VLIOre^`i$n~8=RJC%QzWEB7URb2IYt7<+hEM&vz%fI1}++<^O^H^R1 z^?{z>RSySDSj2L+m&fSYxIA=ve0t`Bh7mzuY`mXu^k=7*gT2^EGZoD?At1WLMJlw4q(q-Vx#-@?u zz|X^ew(ipX{CxMdk@mgaYO4>4Tbf6gSq{=X6Y_WX%lo!9=qvOQWeiZz zM$o03YHvb={Zu;=NVP@2^%F2Nl99HkM??nTBfDcF8`vlVu};SG07`smSUOz$g*$3; z7bOT5YG7<>vJ0_?DvTfgj5P(rvLjxbnC$z4d<6-{JnKNbefb7P*p%f;d7Ui87mtDT z5K)Bx7>WcCETlB6-d3!2R*YK{djQ@$HZ{Ty0Jf)?=`C^!W5%BDF0EnHad8-~e;=L0 z!16t9ZN2(&+2LXH;zkk+>z9;NO1ls_YKc#$Zf?WyzHQTK z3EerMP;UZ(S3!Aotw24)`#6WrLnm84%bsz8zhgvFAQG#S^WJr4g&(x9HISs z7q53=`mX^enETst()0W~YNxZ8)LH;60MC~Ny?8!Al>@Mb5LHeOMA0NnjPg`n5NuEb z*(?Yu2vewqOu#lIWb}c3ifFBr)KV%+NBZI{gAibW3$&>8{ORN%MT4M7CZ5Io0Xv0$iYzC&mtT*bzS(uo(pFVew`^7YNb)IOG5 zQEW}qnp7O9QjBH!{w+eKjzq2{)S_)0kZ3TC>eP`i|DvW-+Zo-oRc_&!qVY|YlfAy2 zV#vpGe(Zo>`IQN!O2R64*vsn^e#@eVwA2OKU0%b+$urNL2dI@%%iUBLg#h% z630-)tL9t#r}8fZwY9On6-j|4r~X;+pElOQM#|cBlW4xBCr1RMS$z0Mv0i~hy#u?-#Y$PpGTz` z2X-U(KPDU!qq9X=XQ%Y?U*jJ)7CL|B9iBaB#?8r_W!oEU^4;8w7tJL;yGdec9L|!P^zG!;7?L2$edt8mT-woT}`ryq79Kc(X(NxN!H z{}d$7@YZAf{757CCZR8~a7rRD_Ni}QIVGuJdHJ`mDc{qGAWO2Vamb^v_60|xzBwFt z@wlu`2*F}~V8g8*yQnzJFV)5jAlJ4P8|6rlKvw)e*t`j&E(I;@Pc41lI3#30sN>h@ z7ga2oj6nWz4YlK1Il!&#ipjORMOilOBCSkDmP1T(H93Y z4LBH$;b1>AUU*%n36p$OFVAjEb=s#bUmoViubA8CTF$obs=PRxA1dPZQc;kxUdXPz ztE*mL=)YUvg?2nZ4M^(FD=)PC$#U@0^mbyE=H>I0{#_2K5@U*>x==+(qz&nU(?_Pc zyjXpMnSSG|jfWN^17L=V_c;WBvzZ#0`sSS>L)o33AvP zhse;NG=@?Uz8|xJR6Tj`{$^qIgkdNFKV?XTU|L|ke#1)7hBXzOdg6R3_}v%6_v531 ztpfX`Yqk_Dwwa0bgqWwFF7hv>q%XK#H+!I6t_mqsznL^^zMY|=#<*;o#hR{0%k~O8 z(W{)rgtu4CWRdAba_K1)abY;8vFJ?a@HybR+Xf1KdK!L6dbcCUm>SsAZz?)uA&O;8 zX%2Dx$SxjCoJ>t&1yV||Q4SUvyETU?mQd?RZUFJiu+ng)S)G%RljJoYQ1nn5W|$I> zrM!cOr+1PDil+X(#~o8?lYwWDqFta?%@q9fBSpNX)gYy;4zJ)3SM%9=<~llaTe8c& zi*kwLwa*siqv7^g-TmtFw7GZ_fcfH*J(_B5-BB=p!>km{ zrJlauR|pfYM2ZmD^tPh}0;$>3%Lqm~xWx$LAHq1>-G-mg^2O%4rm;tP~a)p&h^ci#?)UUSKyKF=?T zQaBF0P{Piu>eqmW#qQhG>z+(|*SUy{AN)cFg#yTQZ=64SouH;eyh5tNsK^L1QW6QQ zwSS;zPLK2u2EqLx^dV51&H`B}{`?+!Ifjn)!r$9Qlz5L>aHW(?g6sZ%u?Q|q9-E;}Vke_79N`y2 z6abliOR%5^KyxxgG840cSy7VZlcg;Cf>_AaeRjpRX^Sqw#`}@7un}70mu3YDH z-}gD^KDVC^NDaQCp)_}5Lt)E>NFhvcciF`Rl8T5^nUu|EoT$tV8mNa5gZ3nQ!OibV zPw}JHX9eZcG%XQw$@o7rmY%uUa?f(74jj<`)u9~#nVr5F`3k99rk(eIG|0TQc&dYQ<>hWjT^`O9tON&&~#Ft!rg?qNq`xG3X9nN&EPWNq|8-luo)8b7HwryzjBN z<1cGER+Ub9mS{3B4-Q{*ir~s5*7l_^WXa^kg+DQW;WRe+v*Ia}=XprYc7er1;K09e zjel#^*YDnwGrB*gZ~RVfI=j)U=}VYSxm3iDfzWj&l})I%&5Mj|T{16akv9O^lRzDISGnD{H0)kBl}#{=TaeTjNze+jQ0 zp8o9_FKm~6&}RHIL5BSfeR$*7JX73VDYqjx-ns`&`=a*#(#aH7fl=faFYk3EHto1i zT&HA~?~R&0B@7lC9-6NerqozW6)#{Jl?5wLU7ON?6M61N!DKOcFIVv%#XqqDjCHu2VWIxf*pTJ|6;$ zp*!Vf-H!J4`i;vxBu~{BXPPu)w}H&PjpH7-?euPW{X8O7zI`~H9#Za>43~;->G-mS zPlIh15HK}Qug>qZ-0KL%I9`@LdDJ2hr^x;(u-1UWsP~1KOQ!Yme#YwMxRj0d6Stl? z_f&%W#Mpp=t+VCKrA+9py~}g1W>F;%N}3z96?LYwWwod!Wf=O&3_S)Ns;5-uVJeM> zkvlk4zEV;p^qL3V6?$00@?iS=OZAet)ZN!DFwn)l|1flbB{%X|{&s_*Df~w7*M5Md zOu1QOi;OB`>ycnrl^~CqBL6RQ4u-{9j48)UOP<`h8C>~*?y6-t-=*8vnx&!5*c=Pa z$9S=2T2adov}I{oz0Ooqev6F<4q4WEW{=Fmp2xRlTWSBf%M~A)k!Bmp!Jn8@x}lvz znd^gHj0;3qI>!4_^**u3k?iHS=@z`#x7k`=6!-0{)qdzN318NK_Qx0-r33&JWPQ9t zgd!7t28V{h+UmvJ+($Ce>WoX4B$7tNFscNUWWD&Pp+1Wim~doCeLfn2Yq7@(_>9X{ zgq7;H4hK~&n50QNlY*gD`UTHR1P%A&+ahteU&BYP&MsxzWOUe3avNv|=!LMG0CjTI z0Zw&M5jVl!jY@S5jLT1}^O)S-U~!ByS@+hvHi0%S8unLK8cE%2EvMmC!9?YlsVr?6 zp;FDNPc~Kz0 zP8Gcd`7THmOK9beX@YB(&cT~JYkIe3V}9xuITFhO9HbLLxiK^J3ONpy)SDw5IRaXV z0)jgU$ayet!v!B$`6n@kU&ouri)93K&{jiP+*A78Oh|xrp{!&Gb4oy^jdN+wL~9P! za5Rv?!t{pfv;{<`Ua0Y;^^!x4qGZY0$SNzr@ty`R$WPU_-|%O>F#hhpAvkLTk4HXq z5qOd1^k8K^HW}6Ua@9K3wE4zY31AA!_p@|WLP=Yzq0yB1ApkEO)?@-CNKQ$LB~{93 zwmo~8!N37Y*geQA+>*~o=L2$gW1}D7KUp3A5osww6TrKCTxHny8gETelw-5pt)teb zA3}zl)vLKiwWS+{4xpx?T}JfKv?P1Nh%#2K4>j9EQ(LuS=c1X8#=K9eJt&J8G%xl? zZ9b-K%DHUx9B7wl|MLnxiJJbBWeJN-gY=nnuOW`kc5fqIEZ+o)gz0cn)QbfnQh?p6m#fG4HX-AYrs zx%G(NP>a5$wc-Q09P!_n^s`iT1Y>@llY|IP_l2gI)o;T5$S3gPB+V_2!J-y(qLQIm zx*cyT%T7IrB9dR_Oo&CuFkA&>5;2iFsPWkr=0rV%Q(F=Y7IQXu_#Q|2 zJkHasp&BfW<%aPmenrTZ4dP-_Rjg@q;>>ai&@z`NLrw=GB}Z;oKyE7|rEsy#1cgyf zoDe-7lMwpJGfYici=3Gc7;+0)X!~0zmd_~2g*k<%*Dk)gp1#C;a%JeTqK!_Uzx89W zp`ai4UynWtdaifTgu890xo8x06Fsa`H+J-HmzL%YkH<;d5U(h| z(2rebFS+-77k_FoRcUAdKv3tg9M{Fnbpe-SRBARES1c^13W^H@C@IbIl1>)ZMpBzY zcn}PpzKNry=sp}vM~1pr%V*ji^$k_tY$RgbLZ1Uyt!AZY&NyzZdpiC1PYIR~D0?wM zo~b&F5leUAe<>%&9|sScnb>V7OK`{Ob8zz78*3NatQfr}IrFIL!H*igb~kx15i7RP zIkU=N&!UL`8tb7qU)y?_+hq>DbC_ndPgT3_#n+*>D&%=27g&5tfJLo^r0QKG06p=1RS_y~i8v+hcnmj0NsgC{h5_JG zF+55-T&bBa-@GMjiKgAV<)waNeLN#)+i9NkvVaL?S#DWve-G6oHH`X=Bec4+?dS`Y9?(P}C!8TT^rKh+N zb647vwu(KCIhgd#`Q5}BMk|K7x>eGdwU(J=tEs!QRrM4 zG=nm`R4IpzZ%_My(kKVB#w3q+^pyRzq72wwG%B)rBjfYs8)pua( za=L3Glhr2sqU+Dch75HEd9xgYSmKE3f{1v;6P60rnydxKw^vmBUs=pB%6jTjViITw z@aCN+szXdvW%B}2cnBQ8V_1{vp&gDN1!vB)vcKk*qkt_&-8sWQCE#TwvfQzwmbU5PSv#>wV|P@eh8 zASbYL%HMZshfK1s10C>{MA@v7HQ0fXuZidc;b(EK*TV{HZ~Joe5l?oGU)_6mx!nY- z!BXHiZ2~9r3lUKyZR>l=(MCSlI>gD4{P61||E^7&-HXuoNz}%#UX|X|8^-OFpDh*j zH@_&_f7n#bA=+7a1p=5JzHQ7+nw+L6m;{N9-R6qo#013fQbJIwntE4SeQb+61Mhw~ zy7xl91Z9axo$lWEqWscwM?L?wc~zmp@QcAEEpj>@QdLIDmaZmiYkNbunxrlz4I>R_ z2w~GFK{qRjuC)hrMqL-|j&i2;&#YK9FtR*3^6_S>PifMgvag7Ny(-vM$iio`DtSh` zJu>7@;>DRFwuNaLHnqgHz8N|*1|17iS4cQM7mDxAhz9~PWAW<;kTptf!Z{Q*mS^VR zAyw9K0HjrFfvJ41Rplq6(vdOKciDu&Y!}}3HbC0yLRJgD8y7kuCL=^l>yxF~hHd!R z%Z^@(PlXO={IuXU-Pi8jhMqbZuvZQLhoPT9H#HAqT?6T@iG~deSKNZ7Yq`!>#bw}` zWUXBeJI_IRIOJ#1Y1NIDWj>&jcWGi|aCS96_5L_eQU>=LBzpHNzVX~xXiCM_ z7n2oT=Eio&Lk`77W&w`i5(z$eSK~~5|HnPoYh%RUhyNEmXtV$Gx#7hD06tZ{!C?|k zEvrxD!~q_Y@_CRC-jfAk4uj}f>Xg4g?6c0RC`GN7AC&S;E~BwQdg`k%rBpbW#uVN&>YeHo5!%B4c2|4JjjC@-7Icr zjc^hLb2IzV@1HL5g{^Az2jh-gKsG}a_AX{AXh7si!>mW#ZHl8%*z zn^G-`LMg^Kl^#T&l2HJR5$g^>Z)e?bS@FXBUzb(V-1yTL6I zI#PvAbfLrOXry_fXPFMPR^nj`Fu*ili+m(b&A?{;;dSvRkszgWlJDGhkIK4v4qgRG zk52M<>GzGA=up2G?IzC2q_sM0v3u!MUF=3N_UfdMiEt_dFFVzgt_pJA6%;VrtRNSB zcJMNE7bW+<7Ua&>Vt?yplvJ$7Hg%+OsE1OyK;DZ)R=(B>frOSc24dyzMMJx&RN&GE`d*hhn zonfu7-=8LSn62Ed@|D(pm!?jY#T0d0I#rjiGnI~SJU?lsx@C5Z zO{<#81;8L=)6)yK&wvP}my^;Ia@q87bHY@w}Gb#D^u@biDYHjV;{wo;C+|r z_4@P4dQ7fTyWhZz2dW_-LS`VhBYYgk>X8X`*&8@3mt^+-{EnQP#6)+n5>FSKrQNvw zWTB_)5)UYJHa4hE+}Oz#Q!E&AbmLA0ixs;e-s6v)jN3PmxdUO%Ey-KrZkIi8|rRJcFFBgV$n09ob8*xJDa~s2qa`YtV z{Lbcq++#&=*^!ww0G78>Ez(NHgDG66{n}CkennQ^kzi4EQAmhOjlU5>&cUxAvS%nTt7`gqK>v#qAXnn!GSeUPLt+x zSI1#NK=ZGsRk(@#zvKIZa+7if~BMs;^4@qWv&Q8r)B)1XSCsK zRR=ASM>8@0qH@@mrai z_OQsx*l#IWlLuTev@s(7epSqI@tI@-hqKAhaV(1K z+6)s&loFH4Dd}z_gzR=c@kF0%bkg-^4E7%S70n8*x};{+?h8Scq8S6H zUD495WbtfBq4XaK1xP0o(q5*c6s2>}vZyrY^ZMEO{zoBImU*uuc!v6CUy~fV=05lK zvs_)T>{46Ib@`@Zp=9@-<5;Vq%tF0q>5QYpQ*G`6e==8Uhfu9TVX;peNsqAB(nGdT z3l|8Za@q^Mh`<$pasc^p*9B$T&9trCL*ev6%$as5;|)=ZC!3;-L>jHW3Oaxo{8Q8}o#bZ!mR%mdMijtWXxM|x&ifycZIrMWs( zQaVp1nv&nDC#A)kScMrQb8!n(a8r!6BTJv8VO!u_{2LV34EVE5v~Y4Hphce%W%Yy@xr{d{i|{Xv9Zu)1PQ2c1K8ucOsBtqx zcW^0M4qsLOhoOIB$phWy3Nbf(Q7K?AS6l(BYq?&-cFc%wy9ZS#ds?p0UGbOZBOjYS z*?-Gfdq4GsGwdN<1FGsWZnMuS`tx!}?yG@#Vqixnuf+$~CjyTK^2&uQnse=DDq5z? z-nv)*5GM<2aF$86_UgY9@d5A?H)O$Z8c9i`Gt&UHu@OlU%>;Ma?s&4X+Hj(>GIo#@ zLMOpcn&H%}$__}E=ZAAawlLsb)h`3Z?LWRdaK$B_3MO^P>_27*F|Rl5;wea2KZiaS zuSvdBOX*YJlP~;u<&%OQJl^nlq_mkqp>yD$KV$IyDfXT`ek0d$r|VX z)0X|}g1_w}y-KrP%H6^III*9!?)UU~n{AmyqIsM1f0Ti7_s@fk3t&AGZz@Qd8PZU@ zUbJlLO`1^S_lVih`4PsCTYW34e^SB zMI@hmONQ-84DpJfGTw54bC))1*j=^};oX+qRo0SR3RZ6n3aEg)j4pIL)#DsSo~|mo zFIOlISrNO0A$MD5!d1AeUdJmqOVYjR6*V*?a-lxqUza9us#5W9eo&iUN!O~&{k;_C zv^~+mm!FK_$mnE}|A!FRF?Gk==J%b%3~c4QcYlFSOhhLelRzmH;&Xw^C>96g%wb)k zOnN!_7~Y4_(_~iGAY95+isyFmUlt9eaRhFKe0ebs!$*~t^}bwtsuAl%4q40&kkO`M ztkgERh3?(?AEf}~f9hdSXeo6%<4bksxnGydQ?8BK78oIIL)BLCZ)fx3J4fSks!I8c z?bzM3jv~+t2x*8_NO1J%^JGDhS4u_cVtH{0Ro&Y~*6w{BK}e_|=uo>*iLj{29vtBt zqGzo^S#wGF239q5ws{~{@mt<{{+?vU)q%c_K+SvMPyilK7`9?YCPvN=ej&j>;}M$+ zf}o=f(Jr13b#rh!eyI}}2ri1+4=hfDLm+Hl`-!YMR@6R;Y!uTJov!q7n;wn%e&~!~ zPpDYv*04}yichDBFqLgW68J3n5*x~RZ@2s?5Eg%CGzK@icAERK)M&z}F7V@L6>G&F z!S^=ouGdSV;-XG)(y1=p1C>zV3-HutU%SUQ|HAJj`m1g}Za=Tbb^m@{L_GHF6m^Q* zeFr{5w5nz}w$9g9sQN|YTmj&g>75KyAPEgE)CU*<;zXaua0p6%YAH2#92KRcmH@~` zM^Q$Z)DYNMq6k$S0F1N{JaJgra=@EB6-Qo*79jN$Gi*U51U9HJl7mR4x8$BX4u|I_ z@hL|#$!V{-cMH6&Da*K%{FtFAygGlLd}oSJ-Mzj_Et0b)k3*2HRdj2p?ax|3MG-8@ z8{?2rHZ9gJpYxnSVTS*kr_omrYE(v#ySmL6?o_TvbUbq-I`>w&3R-sFi>I}(C42r3 zrtHX_>FoA@lpLupWS4^_vEl>B57A(J@Z&%d#3ydR&H~bzT^4OPT!W^RpikbHhCrzq zK_Ea>jEPxNUgCQaKXU|?iDMLR&^2>x?L!l}l0#z~K>uEGo_i}!6(9TxotyO;o4}(y zQKrWACR#5Jtdg3!b*h#X@8jI?Z+^JrSe;n}v9F%pj?L9Qqo+Y{$L_&Bi`MM|8MmzC zeyyyjdHf=zd~N|a{Sx$r8;Gj;_t2k^SOrv*l}cf4vdm?JOdL`Wbc z0(9PHM-Nw7d1#0=e@7l)K6Hrv2*!;WTbeo!;;>;^slJ&>yO3~7Q=FNbMxU{-Iis5X zdMLsU`M+KcTNUW>mdRhnaD=#i=DT0V`n_DeVE{d%BJKXZfv=l4U6l=O7jqN01Fgy+ zYWrpScCK)u-T7ZN&b=gh38`C1C1|zHBnh}yY+5t2^XwV_Ltpvg=cXA?3qms-90StS zf?r*$$QO_cKAx0cuJo=jM$;1guZzoe$Uv1dMkDvM(2yU5R^!dobTU#h0NuS36g7ZS zbk9P2+a)R$2!S^Vp@akvcvLNsD_Vah)44n zbr5KA~h4&THSx(vlH0ZSJNSc_3ElmLbXWz z?XvyLF@^;L>7+unl*2o~htwT1!xzy+LWuf~a>6JLr^V0Ph1DADAq;sj!gW+zVG9_R zML_`&btOQd3yoR89q{_B4Bh$%T?WeYGYYwt;jF8ujmnn!ryN_ z*FREf_HrfOR<^zzE!|49^RWLG01$tn{-%E#AsI~DOA^f!!mSZyF=*%p%ao-MAQNFz zbX99_`IYP7Pg0tiT#Y4wd#uNjDp^%>%Pd9 zupR&PdIkL6osG!)6op>0@GghPA7K*XN?VY_Ogay>eDiT0hnm22awH^C`5}oC1x?N; zF)>-)1ymj59E`jXweuy5p%i5eTy$bZ8Rulo>=_7Kx|ng#$}&)Jj(TQ{%?*{E?rBEA z;xp&}*UKgCSGwp+g*gXIi&8CkQ$2;f50#&B6dk^5{ybB~KdDqe)5$=laHeeu%6pO} zls6$Jj9WG?@SKFDB(Agx#|RDQYD}eP-ng%it`Cfs1AE{F{|rg^K|zT+!m^xn}nrs8umT2Y5^G$FGyQ~qzb{-SX=c^({{be-uD z2STPmz7Oq#(Ax`6>Rsj)6ocM#Vs}ZI7}~{!NJdCy$IAUihjOzdgo8ig60XHLMh)PZ zZx}?hsAeg+-@Q=3HA4Z6AWS)yNtX7pvBS9eFt&u6K^dSbBO=Si($r3kUZYna+sd*Q z_G0?29sFgK8{c(hN2elRazzUKRkmZ2bsYT)A7A*R_Og21wd8G1R_V!}x`MWzgv^2} znxiS{Lfuaj{!)?ZOq+vefe*8jvj4x9?*FoL|L-^2>V|R%a)$YA(cMnO{8r`HLgxzx zLuDNj*>K^5qx0Q8BmiSJcBc16gbh4Dc+@cAdryyW%jf4UCPR(U;q%(jL#+;BFSS3v zxRs)1?-u_@l~hcKv+*9IK155>kZ~^15!u}Tm8bA?LaSw4Z;zzvCu%!y0jo}OJMVOf zd@ieT#v-5us7T9{#7iwHlNwPLnny-Zrgl2F8bKpZaB9OHa)Oz{foaLYz)GP72@$io z(if+VFDU(?jZ@+;);z#B+O|zs9zTVujl?MJ;9UOh9VrvGb!r$}jccPp2+qk1_}VFP zH1}(B`ipGKiTrm}v>amq| zpDJdXGJu0NdPiux6tPgcJx?QzWTs8O)nRP7W#bEV&3vQ(e07zz^?wrUDVFT7$J<4W zjA5TB9PWs#V^uD{uZLb%m6mqDk%tIfowJ>JBg?5-`mOABo?dNPGf0ZBpou)$GF|H* z3b18`xX(Yt4ow_qo!+0Q2V*i}OZQ$uJl;io8F}DIAR^V$>cyH&zkG;Re$->({cd5i z&pHzYAki7oz_W%}vQ?{**t=z9G>Vdv$&e@yEAv7CYJXXwb|uUP8X(7sWMMGnFr8@G ziU9ibk?1F!Uf0RyeY>Uhdali2+Y_C|gca#m`m+eHJI8<5z2~*`q^KrdI^o?Eu2$*IEMN3wa7|^mI7~DV%aftCZQMv6N<=SCN1aljG3&fju zAP~Aan$l%#o#&}W4Fz+oqW8-iUq*RO?H-%IM#Y5Eu0c6W-#KJUON^GuTC%dz^6DmA z#y70auoPGQ^-$CQT|-&28Tp4-LH<=dUa6*he9a++FVhE6r)8t4#L2|iWEXnnuP0#6 zNjt|aRJN+`~Jv;?S^(2W;){mV)d z;DR%wTM%*4o^XYTxVVCHG}lDHXipf((Iwkw3xAXd=N6NT5wcffHG?XJn38F-1*bde zv+lAoU~<7qXN}8&l7B?poO%y@-Zn@fN{T&BO{KKSWE=o!X=h7(Ig4YRQo-*07N zTrV{}8iPj@W)^*^h4g3^?>&BOyx4GZ-^bmUFI`}Cb}Xo_@iW6-Mc=9~&$4Kz}kBdDs5LzMu+Z~%M1%9H}F-luXLL{(&n#Rzjtl&ezHvs6kc z4{zi-$eD7O+A7h~L`N;1)))a804g&Q+;d8>IaD)`*F0q&&xICpx_ef^YZX;&DTXD+V1H~F=JV?;uiG%p>yj>~(}-a1Hu2Xk`RK}* zPJ_b>S9YKB4F@Vd&))BQSFGxVfQ%YMCCl-(XyqhPesnl;i_G%%WGPWgVaI3KwkOS)oZN{*VMu956VHbDhwP1TxVxdUC05w$ z^mP)A3XDV4`wXAQ6xS0)$hu!pPA71u`U2sV5dcY@-gr%oUbx_#i!ujdcJ3O7uKY zWOQe6d~0-+P(e_WAY`L$krGAT5{aCW4l3xZ`{)3u2x%2Z4EGl0B)xcmifnvd8z2CWc>5}khLjE~lNGWdZoCMBs zg^v#~DIuX@9lm~wL)^^o5M@3wLYfWv(an+UWqzh?)#+OXGv{RVoZd7? zAh_Vb`s`G?Pj>JcgO^47YYlYUP@6(fPjSfp%JqY8hOY{tnf}??wo=2vA;0*2hDR*e z%L~`}9agMguKgCB>u{C)dfxTJ#Ve#evOhIljekbn}@T>_@;(1HdVIvL#^`r zbK}dx$U9^{>3?DwUxy2huZ6F_7P2!RB=I30j1He^4G}^>+AzFXo);xdjWK~-owW2h zArL#Ngt~<$Akf`h=s7M>6p#Q=Y9*JY!>Iz#ifUej@UT&+pYTgdY)PqTCP1v5mK%mm zFv|JRQirVf`hyx>l0T}bUviVcO1-JrO~|A~TR*#a!L#W$(5-y5tx-qY{e~f;CQG7U z5?_7{4~znDS*GT@yRzBOTU+^v zjdCHK?SGZ|vIBWUGArLj?q8=UiYIx0jUW#(Gzn*TQsrFz0WCJTEX2h)hM89x$v z$|YJZq`xOVQX;umNi2|6cmB0d$2uBdA*0dDBJ|{m3C+I*@~j15SPobg75Lu`XoQ~50vNUP^P4&g>mQYlHDcjmZ zU&E>x!C;Ma^Mf8NVkuwwjm{s~;1U%(gsZp?km7UX`l(TN?43?n1btb4C463zFIL4@ zMHnaBP^4Ie6eyTi!6D3;lMXKF>Y^BdqR~Kim6;CNd(;!LNlbfv2Ii=ovFW-U9+J@X zN!lUNX?`_*E7Y;+R}GGdwLCvNy6*^Ly*Pe%=dF*-O1#*ld6ZFs53@Mx4>JRKEcfth9pNN@?N6V%UC z(Wy=f$TW3=Vc7ZYy^SOmFXzq3UNzPkN4tvqm*aXNl1Ty?JYAC4m4BXUGMDMBcr}P| z0Lpa!szCAQs1`DrH};gc!8W=l)ZfO)4qRJ?J*ysQAG$d2hkkGlMQEB|S#t4$*;gMvMw?rHbK2V?=0)cS*Hx&Xp-$ z?K_6i2%%uM0x&T;FxB=r3J@rHXU3P;xqGl2*!+l0yK+#8SC+hK%i*f9G3Gr}&^KRj2r^BO&Na<2Qpvf^HA!|3Ny>=BlsCGBV8XH+3w402Gi=$@cdhx-;PDND(uVd{D+}rI*)zd3DRCnFo&oh%OlKFtI2W^ zz5cZk70Jq(u0q3IPn%!57>OI@ong2szc!vNW@rKsD*aB?7x9e8jGv-PPCM5CsbcqWC8S0N0Q>p@~{85mYtp3|5^M`I** z#gI8`H7 zrp*KL;Gk^RT7vCMdYE=@ns(gS2NF_bDHfl^jEKUKGC;HFxS2grVa<}7IMS*lOI~9- zk-gMneE1z!)lLGj@~_?4nd9_6!+jNC0}nDQ>-Yt;*O&^vJ7?j_J|mH75+67kgehnC zsJeX{sb||c-EAEu`kkTfsC&`s5A)=e8_V2N%w6qen<|4hZ+F-3&z|VYwWr9Frs;}A zDTq@s#jmLiKeWG9H94`x7N5_!Sa_M=d%`)t@oLO=W~P4g-yZQ$aYw~((SOZDfTEQ;RJ@(iIaD3Q!rG#6rx9t@XYnA*Dr&jSjm%Gf99LW zNX?i@QJ*Z+G43NfY{x8PWZcrYuS`4jM*1{b+=jvmh_BhCj;_@r68O2KpteTp5Fscw0(>u8!NfwG27OIkVi!w8R+*w=TLVhGo?zHOu zxzay2+UZ!IgAnP+u#RDJ;=Au#>B#7kvm0djABMzf+}J&4*tPGVXZ6^EedCwldL<&h z&F^CPrQJAh^dYVUxpm3dasX5V=fqlWI-!(PV~BCh@~8^6_+p?bt6;d4t8rn9Q<yD#(+|X$7rTt=TP=GyFmIA+6jC9y7InA27=>um@+fZy;UsX*`92O6gS>k9z8p z!-mG*=0Ub@CZwW2kd=8{Qa%8eQ7-L7;K^Z#izQCvT&pBP<_4AHu)x86az<`)3J^k& z66k@>KwfYe=J+HM91y2ykVLf@la2Gf z!s6{iep)t=M<>0dA6IV{0gEVz%9=+Rmw#IhWEa-96nTEXaktz^E|^UFrQfYQd)RQk zemcXTFIB6)cfR$L4n}skTD4z9+0=)E%Glw*eQDmt>I+V{Rdt;AL=4Kpd7Cjk+B9{L zoaaq9WB643Opd2|67)8#)T6c66C9AM^Z5VkB0r zUrGhW-VbEy!%M;&`Co-N@rt*2^grC_((`2*ZG22z4`vEU*7eUez`d!>`b9KP?d>RN zef2xdZy|6s)=}!ZhCN##w?drmp^mBD7o#pKU7p^rFYSjMBAL>}^zJ?(Y98p`^}8|0 zl-##`eFPdmEXUkfoK5i)q*R7+D#M@1RaiJuRlp)o3#w4i2bA9&-*z~T-ulY+IGkfsuo$=x;{coCFnv5C1!mI(IrwZH@g+{Vz*ITl zgs4Rx$5&Ctkcv@C<0rkiD0sSD; z_bD02ngp)}M2h01?(LT)->fkHy%}1m`gki^ecdFxC(yYl-q42Qt_mz)%3$ty!Jv#s zwk6g-56kH636>!cZ8yT{QyS+P$=CjO)m?4-)Br$gf0#X1gnLWPkOo^OM2O)BIhcjx z)TAw^N$BYCfHWs=vNrwiTi>Km25)}!18 znuK6@X#xD{=pJ^#%B5a`*PEJs$MAqA5if~i&dSW|q{L-e3#?&G%`|m6pLi^7y)zBT ztu58~na&7J&eLR(&#y=>in+_sVI*}6W+r_5RnfY)s4~X!sI#4%_;!~5EB!P!lvCM^#LPFj=zwJh|B-K*Xt%1w496T*si6<4O zjD;-hcIkt7#e!A@)2bx^Xlf$FNYOJc>T2aWm zHlUPQHqGveJPUciyj6Gv+7vvpq9z?P^D&=k7s$7fAd|-FicQ2p3kqOuIJlMPxVAVKPT z9ArN4H@2#v_gVfkd~`P_oR=yCA!+EkZ8G)H|FZ6b9XLULS?uOX(@EW-!3X1w>7m1Z zk}9_X@C=P=NRoIGN^SphS8JvbhreJc(-8*?-7=E z;xdj}|9m;D;7fVMPLfls@W_Hh5JV-ed<@+EG?gy$>Z!eLm>gZ~ z%HULV6#A}^unJ^KwQLX50<1iwMqUUMhUx{zW!`m*DEys$CI7MHnO$Aq!QUpCgcn{c zLG97$_Vke<9U=cs2C z$EghiuW^)wotC&5en|#c2;3oqDrY+W7Ci;U+!_b1QE5(`3JTFdi}tHs04SqOzIRYa zG~M`|gxpx?Z^2dN{Ekr1F)R||k$m|oSyM`b`3s>EIlq7*>!fn&ckYN@o1CHgoF5)I zlWv`Iu_Q4R@kYS|Vo$ymBTOE?kPYQ=k`jJ(>2==Fbg^jNdiCh&WBj7)gyqEmcsf$% z_|YrhX6F!ZpD!n0>Mayb27WIj9G`cey|t21#y{4JBt&iwoK2f2xy#Er14Loqa2-5l zS77E!o>rP^2;AIxc#lkjQALTv+wzeY6ln>0V@PASo<-wcq0V~u**&FnB16XIKM#Hw zX3TT8fAn3ucwih9<>|0+uQZ>9jNfs>NexI$(s6>t>M1HXz-226qN|xQ-u&PQeVk=~ zcAg~uC^?|$m&`;&*iMA`iqGNLTaK@59{cwmNbj?iH;fP?`01`^_uU0Oo=Jc9KN=7b z&}`!2ONppxD!OBi8d&X}Uc0h?bhFmi*|8eA+}|@1dJ}S}Gu`>>^lp9~z>o?60q7CK zWSA*XMCL{ zTa3`xZNX`}wmr`F!bIU>rb@bL$uB%b@?!6w^XK=F84j#9`DweiR`YA5JD=gB&r)q4 zfAd^;PtzT`S;Al%8e-6{d)~CWWW|yo^sBhd%qsB_R`LsFWr1GN?%Ax^Z-pPo&(Jqx zBz!klp}~I6Lj*073{$87lEDx0EI>wgC7iS*&$Jy`zz|4H`Z`hr+iUR4R8-|y#V+Ua zX~)t~QYk)WmxK~d!USz9VnTgSk1M>7pyxfDoJfIPrM(_q+^fuvr^J7dH@shOby_8< zA7O3yt4n`f#l9=(pXsLP)?hl-@cx3hPj8mg(=xB}1nRHd#8)%Dt&0jf4N z#LaAO!6XcJhUqQCm7zXAkI(R8KvUIV-mFlA$q>6b2< zaxvh|$s}OB(e8lY?2p!*SMdh$WBa$6ojV#I_G{@Y>%Y>q%Q7;^vHQa~jCQfxhq}l} zCu08ZZ1dv!`RQzKPvZJ_iwgfe(RsI+yk9)!wpnG6TOG9XV&{Q1r~9;A zngpa70I+K-cuC;iD-+L;1?=Z-9iWlj9;b;@jM|7Cd0Ci5 zrEYj3!s5tz4ASFft<#|>iyge4mp%1hppg`0tHerBk&IwmS&G#8(nqiBhJbI_;XCoT zsEcXNrzHp9Yt$zVd?nT9^;~y1CfKcsqPHt=n`p#Dj4`D$4TWxMf0RUI5k2AJvFjK*^ab# z>)-e$KUn?9|M_xm!YmfR4baIr)u?n}ijdYCDqC%}?Rh;Sahxy7uHgH8M>tlnBjHTBH>f zkm)~>_#N zGsbhr&4-MUd_3p;&fa^ix#k?M5NQ$WFme@&UJG*<24bmp^vrEt){yQjD|!wE*rN2L zdC~yG7koS+87eMYujD?|Y$jwgHhA1U-S93%I&LOU{RIx4BAAsRSY2dSCx(fWyZgBS zu1TZNWoS@!XJlFid@aB2wpK*B+po7bl%gyRzYos=0ER&aXSAFME;kZ)2nIM#N2R17 zO}(A*$M`>nyZ{~)?6A$bLZk@Z)GF#wikXZ0Lyn5=l11caYbiq)4F-a48#BKd)%DThvn*2SHY~9|`?Sp^~hs5Em@HwxyHC}U7 zLjHCk&Z_&vh5@dB!h+5(=X}pzaU^8VR(-9w9ddW6|8n^O3osf$HRatiNO0w<#^`6F zqqTsTR$ibtw79dsyl%0o%w-}$fO_?<&PlRG^`gSuYt6+iB*zzwo+^V^xk%8}`GCj~ z;Rb8g_-rkTUVKRJ&!Me3$FFa7voDALf%@eO+B6Evq;h#mnLd(i(r8^jGi(#Jl`b~T0X?S-3MO=w>#P|Wg8)HT;r)qJNOmRz8;Xs zMr|~-&X#oSO5~+dKoh5X(1J-7P)xaIDl}%WKRMdUl_EpfSl{%Il3+6+=29AGUXAK2 zhP8A4!rm>HU8Bhj#Ln4sj+!nc)4R2Wulsj&^h=*agDx-fT}BAEh!57;{B5E6`y(^CSJYl)m4Guz?-#|)|r)W9!V!R z2+CLW7F?EkD3NQ<+T|j0A;rdJRVG#eJT=QrVx}LL z61qAMW_X@Tp)*xxld$Mo@u7M7T+HtzADYks&z*2mPFZOdLGjMK-qYVPg$ms(jG$LJ z003}8a3nhS!f2c+gGnC96v+bc9OC{^M$@k|4I|d-i!8KPL?oKo2~)+uBA$5%C+G~r zQA%?>aW83}zRV0RVs#_qGT&9mbHvB(sPunt)OS~9?u|Xu8hCOrcB)t2`}Ho1v&l!Z zdF;AxWB-2)WrApn-N&=>{H-m9NM)adE^8wTp(FH)!GeUlM>P*yxcgWAiAv_`r}4bm z;_jkM^fXr)>`lx&j^{1dy5wFc>jjz08^4yP(JNei{>%JDloaA2JH(fzg*hn3-8;PC zug?X;!3CdPu^@{yW&ndMnVEV)9k5s`tH-rQWrZvfW2~0=k%ur(ZWb|*v#=v@@4xeD z4#|7s9UjIng(@Ek$YD`;g!<%T?G0$J{GXYLa+N%94`Z8)!%vw6tUJ zy5mtfu$5UH=2(_a%+!t0h|ue+xgG_eiUug|r2I&xz69`$hs(RC_E(U-*Cj$>VF(vq zJnZ7q2O}r3mwnAe^o2R7XbV=!$S@ncLN)mnt(EAz#Wh^D$j)Pk=e>imjr?iGSgnat ztZ%Dr|9A#9Oco(_B+fy5SZRJ0&2AYum zs_2i?F2l5JJnS)iP$1;GY(-U*jF!9>!KE!{!LhC^OYw>rua;4`fKnaHwU%G?vt>OH z%)gEb?4xLOG&mt%j1o;I=UR~HqGgY4iSJ6;TAU)T9HKt#}$27zW)As zy{XIwVvH_y607Y8TnwT23>L9DGJ=CL0Ci~y7aEE}bS0*8PG?I*=5HH zXUk?dwmWG6t-dpsTY@M$5cltDEkH{e@-97U51vHjcS01(X}(&wqAx5M*gJP%NVGN^Ep1vY z_zT~E4D|tM1MMa(^cCB?rh4H%2|Zi{D}f{Q8uhfq7P1|j8?h7CNC^@#ZmfEWC07O%{=VB+siLWzio146G`y-k4Od&Rh&3a6Tj|R zoaxS?LU$P|^`!ymt#p~3>wW5R!xXar$jj2Do|~gq-;4Z=3!3;V86B;Zo^0caAr-TV z;Kh#3Z!CCg+?@Bjgu}nH7uy&B*9WD?Q#%Ym zPbg*B(~>ejbYa3R=BNpBF9OCHyYwVf3s66*(w~OX0ciVO#KN2uUzM_CE4XA{I#*eH zLwCUHJ^&QIlomfXyn5WY0%Nw4Vw{fYDiseGCO-}f@%;YXUstNnr7d_K8u`zP5MH9{ z_56dz*IKS9#Hem*h3;ah1GHm=;`tihUS4}%p~Ou-eTLah)2=yV7ha2$|fX=G6nBK*V_U|h_85*vTXah41aaVP4)F)ro(Gz1VP zC9`9n`ztXhk+5X^Yto0O zam(kp{pE*A@VuNm^IFBoWiN(a0~w8(MLNAW&2>67sI1Lylwk%Q7iGpnKg=ep#Pgm7 zyY+|B;fH}Dts;-9AJ>7uytoye#o9_u%>#~mdKuE?=0nr19->pDSxetsiU&PrB)m_u zws92!D}VSdvPN4EtyxG>($nj}(b;GLf)x$39ePV36sCZh$|4JR>j+S>Kv*3wq!G1D zSvOr`y_O_n78o>Gp)hJ%R&AFJ#nugE1f$3o;OH7t=ask+sRO;>lbD>2=Q3}?TYGi> zV`wK@G|E!nLPh`hX+35B216jc$_{F@kv@1*?`Bt1?Jf9xl5oj-XRF_Dq`KU~NN|j- z=W5oabs}~klBZvJbYb07f6E|wK~Q%LWYty7s_T5Z>uARW{c7OH>k?ucG|b>%cN6&) zk*vaNZFvW~rMCDWWQ*Ii(i}5w%_fa~ zmU)6U9jG<>P8*B2E`_`YYg9n6jqXUGQ$D}h^MGFEl!LB_5 zv|qAVVrWDXet4E)covF1B_`4g^f~M~5Tg!Oee%N-n5}z4D@bD2?HxZ|4%G|;mEDC8 zSC)MTJcAdAbx@qKo;-BU(M#4Un;e7lF@}oLTD|oP4 ziXn&BSs=I-g!UfYzi70Ipnw=K+Y%^-ij)fwkfRGK0JAZU#CP}?3$p5q6>C@G846&~ zxDz_ZS)(}c!>gLTCA9Ed%8h&~BTIB$q1{WN!)z^r!o$?~6P3OnKI2dk3Cwt1hxThe zoHlSRqAWJ+#;t(v5k;3&IJC&H0XnDIT}%8@9U#iw{74t7XF%-JU7O#173Z=*$X47) z6iNGvhk70aZ?p9h`K`>a*+x^RX}UY$7`gmscbp9r$#M`=34bO`?VwN)anVwas{0T| z7*<(Na7;L_E*!uypXaP^@ak!DyH+Ie!UlQr>Oc#2JDLaj%7PxrEOwo+uJrApAXlT! z%NM3N{dVPrtzom4Q%BA6HwQQZ4IlFbV2PWvA0ircon>gYiEKXZ4~j?Tgjn?;`uaaZM2l zAFulNtCmT+8i_?x7NWz{h3^j!=N*XW6m>Vi%s?tWM1Wn3w}TEi%q2%nj47lRC>;er z+5mutO4>w?e)Q98lU+U&>)cgHvvzkTT5f1nTQjW(vB7Y0!Cyscp2yGFemM2@e++$N z@iem5<}ks}E*jY8-arMyWF5Ey2r@$=60Z7vqTeHUpib{A`)0Vm_)0=T9_G_sgWT9W zfoW~LvbRh8jM$PLhtSWdBthhKW(v&iW~!I&13md2p=2gqnu+G`yxH1<-V$9Xw)S$C zkt=Nru^=}lrO8d|99dfnU%1HSPxW_%t~m8-j7Qwy_ZzY)@{P_g$#g#oGNeb0tIBMfHV&5pT+4O zSV1`xqcnwB7L*r=VM4&tKm$}44tNl z$ywpDZz}}#C--z+w1?nU*(}l?G7HQSA0G6{_9)&(G46{|B{mKoAlu)ibnFh-O^|h* zb>~^`E%S#6awg528oV}^xr&f;O@ZMnITdWNnSH=y1?;47>K76tJPGHf3P1Ig-{mnt z8|^CJw_i-uMjDC?zix~V9NlZl{qwRe+~Lo6o4=p`UF6xk5V82%`NySybb|Do+mt)) z5-r2Dh~5-%dNyZ*$#mUKoQgpJu&(EbLcsfsm=#2I=iWT7J%)UEO@cz1kcD<+*(rCa z0KX>tTfX!)F?O5NpN4C^#NU6bT8Z0aeYAC`%XiCHY}oQkEa5!oVp7J}R*s>(_XEBz zW-ZE%KWA<*;zM%o$<*ttjx3YQm;L>&8jzQe`#;(2%uo-T{K}HvIYhLVx4cbcFGV&z zP`iFyw_5+j$zOdQ%ASDR{V?xfAyjEv=V#2!0Fl~ua-W2`NO%343G;1N z<Hik`U0d)7Ei@dH2VxMOsmWZ623Q$%@_D1Wu$PoRvOi*oL8S zGi&ObRDEyM$WG16PKC1J>kV9^&j`M6UU2JwDB?VY5>63>+`+iNn@}yxZ~$7iQWu=9XqRDFk5G zZqf)*8m6#!6O$jxNp2DaT%%Poi8lmWq(O}7=1OOi28ps6Eaz2hPAy-Pzoq8Ax)VmP z{+q>)LU8V)pI@m+v``84ZppO|-LGb_n|T{)iYlpEkn3u(ztW9+p=ILnh&3XiEv)&c zdE8?tiyAAvSX_Pry%t4Xh~V09+3f1NiWp{lMkP5!Rl5GYlnq0NwuMSx=Cb9r6x_>U z^x38i5aRLd!5bs{u0nE$LA+pFVLfk_9#gI4DZVavH1@c4M410!?DboV&nB|Dj645h z=nD&-w2f9ufm=|YM9<_6Fdjaq!6@w`Gh{2ip*>yo-P%ve0ANAyYdN`k#^5w7dF2-T z7hUATUdj8O=zXnp`EGUrId`!q%y4bl?GbMd_Yb4+@*lpNU*FCP3KN>A+28XqU#SEIt=-HU|9bsPodkgHqJo}U z5#BBrjXfk&sPtc>*%W~OoGfzLG0g=f@fKp1Qeq1BE*PArOoJrCRcB8gQrqd+lSLhqVJ0@K)8Mb#iSJl@dh=h~Lv zGS{w=5Wg(<=A;SC(_pG7p>aD)CZ6(*?+H8ZdF|NsfF-Yn=giLo&Oa9ZNjrJ>da*Gy zuX|~K=H{bIA^p406bqjnQ*SB$wjI^df)c?1R(?iUF%A|hyN+VilFfL^nSjHTW&=3s z%LcBdP^qsjec_ijeK^14*gl2Gs;@l|55?oPfB!`%d*$ZNJ~>V5wlXfOVOqR{aQWdM zJVMrLCUb2zdHjMVKD?!L(@{9)kUc6{E`2oiWxA`;9 zk~?WX2locO@}5ee&MjzI&+X$7+xZo4El!Si4K?MjEhbG)UCef7 z|9#`%DnO2oe$X?8p+-Y6|5Q-=t{%^!&|Bu1;Fb?%coxlQkT#g_USa|^qu70b8E?ei zy`F~NV*^0>Y%Lw{QlFNIbopg!F0xqHmRgrT70BE3@qeE3;|g~Xp#Zl;8HBCdBNYX0F9Z7 zdn>yFdJ@oo77MQmQnfPmimq2x-9rsQm!wT`Rl&Ypb#=&t3DobJU%rKJzc2Mpe0tki z-}TY!YLU&~=4ahYV^;g?j7v3E@X*~SR${p(! z_d4OjoIqn>NCK$Z)KVT~Lp+GDq`A^s7xGh_HT?nJ{VrAJLZw^C*+?SC^I^!~A6I&Q zLjGq(&wEV5o;^$N_J8y6iJi`m--XBW@IpyZN6As#t>0&kOdl%Ud`k_W>aV3k(vfKg z^x{BZFyty4Sxez^t=PzA6@x)wj*l?_Gyn)=lCmSxxb*inx@>siX|RP)3kV5dhuOzl z{q%CavCS!_mFrd(rWC6FR0V&ovqVjl#V2*MmKJvda%;_b+p|ePDOfeKWwJTsL-C!m zgaJsxN2goNKXO)Fnk~EF&uuZGC01-#k0aUjO}y0TbwtJjXAGF9lx&jLq75vBT7F%Y z3BBz_^!Q6))K5X{iApT@koqxxZxBfzmh@2$I!D+0K*~^;RtUO(U0Rrvk>Y6XEdvQU%AUQO+5@aN0Wgdp=C1#PuWPJ03>?QE&pa*l(d}yRNuIkOHSRN zYVQgZQ3)2(Tu5-dA?e!XB#BIZd+2~mcNof6d>afYNMupaef>7rN#~vNpEG#WhsV`X zk+DNWY?-)eAg8D}=*YW#F4HS-dE%*ZfPgHit)Krf zbeuw`V?5AMAb&i*H?A2fBh7ts4ah<`80-{w^=U-^FkB6WTwjlE)FTx1Hh3f{&=F}v z=#b;HiIAABH$vNg=5XmU!*9;jSqA-`IT(SB)`~|?zYi!`d{RLk$2LZzzNxg%3IJB+8 zZ0;odR#qAyNoq86<&DcQD4jupv5X$0L``L&`3F)UkJ(47FO*wX1anw&^X8uMY|5D2 zc2KK{&J(6A7cC2XYzWl}N)a0Py-2bYq+b!|xg0}K*AuGekH&(OFCUXb?zKi5k2eai z(hD9lWLrJVO*dvg-9r_MX&GC~;X0RX+BhxD7AjAya3>D{uepH@%oieNLfO=&abN%Y>MijkKjNZa8xyPSX(9nz2fBXfO~KPLF7W5zy>YUwj)P+W z(D6$Vm!7Zz0$NW2ql!k8F#@VOvR+pbJz|pL_AF?C164-UHf;{^t})FXBrS+fV^wgq z+`mBZBs*Od&E(D#eNg&fbWL(@=SSqXplUv@zKKJ_j1e6^-B<0)`7b;3_Ma1`SUs-3 z{azbJH<_qav~5btH9L`5nk^@U*1*EHUqyKST3EQT=GKt0W5*{DN{G`~e1Exr`tOmH zue-*PhW~5%Lz9*8o>EIT5q&^if0iu9h_%KtyY?b=e9!?MrZ=281W5-_XRD`7^HR(A zJSA~M?Ooc~;}TujWaEZ!u*YNRE?x}0#^KU|yrBs+2upxzKh#JMV%Qx0RdPr^L+OljdU0>N0vk>o%4rtN3D8UdM~)2|w-CJ(>X zC1fHLnk#-EshHcWKJ!J2JyY)hYjRdqisU(_+JBqxZZr7Vrue(_v%b&x`*#tkuE-yg z&Q7TTh~?%6r@X+fmCowF$BGW-uVG05U6m)A@mwr*&FOwm>*}nz9GP!slatZ&{UaQ5 z{K|P$R`Sx$+M!I6vda+BYze|%&jAnQJoz7Q`myXtE#0`aJQQT=DKlnVeyu;%mB;Ojyeo61Fs; z{T0bH!N0b0)3Pvhzs>2Fk-wA5vS`|LU~pW~O~-BX_(LV1hVH`8t?|DC{SN2EcCPa% ze~K&itkyb|U%ub<_r7b?bB3D={rErZ&D9ZTuJ2?Oetj~+Wh|ZoqYQz-j8iRNu@z8R0U7tz$TrZz#Tp3N|lp0k8k#}4% z1o0!p!$UcX*8Ir+M__2I1p|w!MZIXmQ(rm9uTeLabZd1d!zNgOg5;l zSx=sO!urMM;9ia3lOW*~LnKMFgJM|#l=a0!a~}*aT;ieu`{ULV-`be(A!9e&N8a%| zv!8aImK99L6!WQLOsP@c()aGfCS?aq9)Ovh@IxNc?&`zr)2F~;YUaJ-QNYq1rJIIA zCh`s?kYdRK0BNv@wM(r399RF8_cM0^I!1x$@^*CVnIvw#e_3c~?!irUrp|9BwGCX> z|1CqGqjw~oHRW$84o=jsLT=EcamTCtmAaT7LLje?)D)S*cQYmzR&&`rHVOW;^KYDI z7aoKW_$m}WnCH+EhT%E({_jIn1o#Xr5W#hc`lnrJpSipP_#x-RZ1qLF>SS`;}^I zO$C$O#G{NykOe90ykAeh)e7AxiCu{8YfyqUnE`GulvQLv9fRP^!Bi&y-z;v58p5U(j|8J$15M^9!oyoo3!K+-}zSy zfEXzj5}1FMTmeIADCpa|j-X-9|bbS3_!ODIq-=d+V1nYUA)u}zEs?Yy``F)AyS&j`YFe287}^HY zd0StTr0|9eCw)5(1>NI(vji~q*qp3^m3s8`^_z#CJ|Ut##Z zSHRw8`M}dE-!g7lI2W_%TD%?4Z(>>5u`ly+zwg1Nlt%8cOx%lH$g69WRcWVlm5!3- zAG~kB>qIeW7TN)xu?&_tEhJfLZpA zFi9DQ5&q?t_K1{_7r(`fZ@8m|63Op)>!KsI>$}vcVa}P1@ zdFf$F?(}3rhM2kWPR=E9NKjPnM9!qlNFxI%_=MiRX7$-S4zvz_szt%vg3!6<*6Aphb*WVr+HP; ztef8988Q%<9CrCJw?dC2rL$axs3%$1!1*7Z>MlJ@#|xj6F|1t_6b=7GTep& z;cN3{dNrIBlltTzX5pjT`5XR{G}1ON{XPtC8yUzu56<1UGuZIqa_U+5y!PsLuCLxw zeOHawhT`Eb7Ao|QOI)Mbzt}@B&R%9UGVU8YbRDxwMhe5v)t5yxjib~nO9s4^Dvy{B zHsKdN5=^wu7byeItE5LESgwd`9i~)(w1@%4KtPFvsHJVHzAJg^6Ii7%4V(!>S0S3dfHhFT7_Md@T5Y^ zEYB?GTX-#F87?GLR5zPqm}Wd!iTdQk4^BO7}2(t6ZAB6Y+x+2Jjl+L9H}Oe zL={8B1tv>y7-87LEF)-Xr~svd0IHNhPsUIK>Rla*3jlY$b#Lj^C{2F?t}0WF=Q$ z-M}5YCEKOmayR^Jv`IyKLi@7O@&kL)r$2AJvg_VR9^KdYd(X@iF|^X77E^7KkeX8P zc}lQQ`EM-%qx#deV3*sSnU5C`2;^rXQDEFD#X0OS5IjrH6(ieT7o!t{l@QAq0E`L& zoCLf>x@tqq)#5NRPI(-Tv4sRQ50JQFy82RWGX51@%UD6>Q~{KYskY%ymWTGDQ?hcg zex28>an$`U>0pOJSgYDf9L~Z_$T$C!%~bJP=R>)9&NBD9c75Tb4a-jfd3VemQz>V- zf6H0LVq5R}b#&GK?VnraY^H?v43vb)QPs)5;JjG{z&}hRb6*0)3=lVhqX$po*XxL- zX7MiTIB9)I%#&W(U{{#;J;>t=&zYFJ`Jf6v8PGtI%~5}uBhT!dtm7W^ zdl9}?>YJ5zSyFL2Y=Qc!sW^}OU#KoY3H9?cY+=iIN4__-fR+1w6VHn+zSH@Yo=1@C zwU*W~yshlB4Y6!^NzJNKHsC8Y(3p4pfj(Afy)|eV z62My$R$_{j5Y-m^I+gdcQR#y) z0dJhzn}a-fieB6f2?s<8E!$wu#MD>zDFHs>v>Gp;R@yH6n#Xu8{tkpsJyE=+>hOuZ z>e4H%kLG=UWUU|4PA&2yq;GHjY%=kycEG^+dDk5y74yDH#n;@)D3E@;?;xtp>_WO^aQ}T|h zhek@T{FgZc-O^jgdoU;4FUAC^DPE*={2SSx+lB3`Z#)P^ab5GPu1Bsx3t4*=TIqn{ zZQ4N#oN?4U1{}Qh(t^knZg4plZI3CG|1y|nTg)dL7oS#|ndMqNCp6SFs2kedV5K{0 z@ODO_=Dm@Db8uh&548}B=c@t|V3)-7!>_;q6MWY`_sJt&zraVTFOSsHj+{EZ z>c?JR{iV7<8~iDmDWD}XV(IBEPY`^IQ*hYU;>R z8X~Mv{iCv$-IrIe{HLIrfKXPQT1jm=VBP`fDep`Q?+ZpMTRY0g_L*`if zLy|zeSLxcl>^TIfAhuR-oNU=(d_YoH(XNVXzu>NlQn%02NVxEeQSiLsko=8be3?y98HcD7%$&zA_faP?3aY`GLsN)X)~`O^_S zuSm7;It;M~inH)KJg|5JO3L+Z$eELPkoK#`i}8Wm9|$Z#=q^}bFV%r7;-&^f>ErjI zht1rb1Upmhoc&S-ukiFRb86Adw5`lvx=g@t_kkJC@1+0tNk@K_M=8GE!{Z?nzgeN2 z8$nHFE{DN*clBx3D}1N2u#*Y92+3Al+;OdkyF^4nW_;XM5G&*={tD$$c|+|@MwDVg z>OjS~AqruK=o@U*sQ5X@-+YtSn8L2!zq*md5$Y`;Pdg4uzg(PS#4!uc>|xE;4B?0+ zK+3fwY>glzdKNkb6o$p%Y`vMvR4C4SrbCc+1kb))8+f5>*(PE8Vr^q}x zUhHR{hRWp|S{PPL6SE%@KX}OnnUl*^lXuO13(?J9J)+B>(y`ymOkRv%Nz1S)-j8=I z$mOzh4faUQuH1PIo@-<3SVK0x*^K}qu%XF*){aD&*TF9>M+f%W*_W60MJkq?BoIh~|J1xy^X;}$1 zOR8S*T-q01^KH^;tHkMYuaDv*52{3vEz_vQY%`Jeq_Y8C7SZVFez;-e5IJw|*y^q<3!8BzOm3nF- z8EE5F!OT-VM?Kd&yi9ZaQVTg`VOeduHLTc6n4zy+>yN0JqCUwyvDop6cbU)awNP_$0Bt-&h@WeEvoqk$2L5S0`#%COhDN;tq&) zm6g7y^tlr$fY5X6rKXnSTK}Vs4Uwvr>g%$eDmp0K%k!G*EHi3P2_q^VK3M5beF&q? zj=AGtEu%tVrJ1uvkFl}OQO{3=erVm=Ik;pzqtS^6*i+Q3o@V=TPzUID`+>{hY*?P2R6 z=KQ60UO5NV`bELx*nu_0P1kmg;n+0~ErfWL-g}o3)iZFzHRHzdW&?~Rv*92WD#6!W zs;J*WN~+QLlAZ1Cfk;Z-OF6k1S<|=N^?r)}j;FB|oRYbPA5E4OR4#Km^REYWwMc}K z$Jf3y%nDwvRadjT{ogv|{*ROZij-+XDTTCrw&vm%o^kB_-lZd}Je79MIy$HAyD%Uc_=!zYU-2D6)Qm2vaxZH|uwG^*QPJc}q1>8sJX-C~aM zH;addM}9FcvOk5OG5z<%r@Pi$QJ5#L-IoLu#5$2WEJ=)TtuijoOWIH`Gc2U48+6lHB}|NmH_x^c@6Elo?`VI_A|;gCL{R;H+EXstONAfN_m328Z`vPKRUST|s64p4O%=o#+9fZ%Xc^R~tkaHP{)w_Qrxt>~Usp)ZQJRG6q6F z3R&hK&@K%M8j%YEKFRg=vN7g$hm|)=$Fa)(S;%WV#?)r;$_}hLt+y$#Ob)@3fM~ zf?i5VTJhs9Q`Thf87X2;7F$PV)mv0OIbr-tu(`+Rm!-Pw z0c>i8Tw53_c|9{xxO%OZqejRQJ<1{^j{i~PfrQH4E$*8whkTbCt6;{3VyUU~ftXfCW%!sFl(nAw5Gjv(g^%M9Z%|UN zS8g|>nzJ1Ca&?Xyi4RnZVCV{0($=L|p3~#qUt@jH-)lFr6i5ZgG&J_{eyJQ2Mf7SP zG(w&UTH0q5zySV_p;K~#*ga`LzaDkp%aLtSu^CwS_dbyViKFk-K`wV3=U=FRlS|-j zd^Z@Izl{Wt4X%ljR&?Lq@VoFl@G^NC1x}~6;|FJQ^ss6ectvf zyV_XoTJ=XFV#{Fm^_M`I8+quymAhb_ygVtq|NNMXnudiNTm=7=^1gSzb1v_mi%UM^ z&43PLr=}jhBH%k2efE!{U}+?ZOXwv&A8Y#12HPtt-gy}(J&l`9vUTF^7&VJV=bKp? z+6mU4DzWv7xSj=H8vzgUJNwiQT_y4BWQ`>8R%mYKk>m_+Pi_t-$7jXAV1F%(Svxr~ z)0qTCprw4JxljRR!_!O|k1|E1c?y*d6^yLCWNS{4)&Kgk|dmwD(C>Q z^TT|g7Ot0y-J&Sx+#ZQz_pUs zx7TJuj*5M9xd=RZ?-fg3J@i>KWW9pfCM8(h)N(pU{D;7lt$a~~$`mX2d5<4f%XU;e zwR6d&+b23>NgRkT^vMacJh-yjgJLJ3GbY15PT1Y{6|%Q_vOaPnM452&B?I7;lU7Q^ z6R)%4w4-g*TnQgx5&7~0ZuL>NIw*HI_R;9XBs)ft252Z|X+lqdyUK2Ev^x{A-KjFlVQrMs+T*WR`+jR5sexWa`XxvVe(7r*C9elm+a1DVl zH283uU7Koz8fl!gJkL(mc+YL(pkSve4i)u9Sp-yYCR=g{;VfE>Rd8^qI{D3_Fz2Wb z%9PYAxOqM?AHe!r)5LbRj;L62JKWL9%Ku7uI#;QeVVy~o#)?R#?xb~~?#H>+1xZYuVDg<(m!mi;J2gC?hGQQvg~* zsZ@|FS==S2*c&QECNx#Wt?V(fqJ>E#{?$gQ-9W^*`U;NR`mpFSc)hQkPVH9_pAa2e zi7eFw)7&0z&V0vCPJOpcPq;s&9MC?Xoms|M>hf1WD}${o6^Ffo%rnSqt)1}Rs1xGnj%vw-G9CQoY>I+#RW4##ez^xDiWQ zj185o6);|q6Li8&Fev_vV|gUNa0k^M$ltV^`;|;~lq@%bW_SQxZ=G(XY80 zv_3v8Xt%|DQYrtW^FgdOUyp*BtYI5*_u~wnPlmMXm^g`2$)8^5WFNu0T9X|0jh+VU zi%av^)*H{qdG-crKlp|;Mf?^|+DDI4f*swgY~)Z`?yn5glU^V)?adJ#W{~?nk7p)u zm)QFFIILZ3+kPha|KOy?);We|m z2~YY!q^0-0A1flpc?c=anE=9Wqvt}>c!KlHvi+ls0WRw|qp0|)cWv*U4L!B}kXF<^ zYWL)7hS1WvjcxH(lIv^-D9yd66WjK2Z4oe;Z{Q^|&-*RPi4dAh!!8pz$S3MK zmCcb}z3;=Fb;K%mwzZQKuQz(U8!I~YJtLKULD~(wYsbn#ztf5%R51wYRgS-)*a@ww zUZc&uN&$FZW*rHpQf3@DV4q>d5R$8NJAcYlAjs9phZvB9;$YtVAbB6nT2h zOd=>#OwRQcu8uu7T?9$Uv&`Dfz}Kf+!Kusl*-Nhy2jDcb%(a4=xRk~uN{45K@$4@h z5>u03$4+B`mX9t}ym?ODv+S0#6v24Bw)lx_NUq;nk3glGPQFNugM{lSMchNDzQC;G zZbt>EK7-UhRP}f4ITa*1gg+e&N>+{c1Tb0IXr9zw?G{wlSzmrGN?N&IrBr8`Ar$gb zr}TRUDXZKGRQmX%X9HvZcr7Zv50M!Xg1z}-pIr@?GC6KmnPDYFBvy(@y{x)OFQ+Z? zmHZvRMp|Gr2Km_PVcXL(EsQ#fiDS#r5^GQqrTGhwZ6e*fop|08<9lM@QZ5g`cs=3& zw0EZ8Y@llZS6hX&1VKZ!S!A(KMJye?LK0yF2_Y&JH)7W|icm_`z7$C;scMA?NolLp zw60N0bf|LCR;jkPRa-l457$(O-d>ZL5BHq;1MWHVVLra!p7WmbJI^`K@_UF>)%Hhh z0OpK3W+h<;o@8*DPb$Z%dNspoFc{jP%(J%YwgtLvK@eeqU1Yp3;}Am4u0 zGHBw5q(C#%K78-cP^V=_Pb&6>Y$TO->_zvg-|5zVPeGva%WnD!R<#ei>#7mh5L=(R zj}6Jwp86W&^9W`>T>v8yrvQ_Nh(w(6`;w`~TwTIqonvp}F`IPgbNuP8axSiW`uf$? z;PI&682X@;pPOadi81B$-}s#&y71i?9FY~Db!=A7x`}5!b;2}dd6Iw7q8!1AXQfB4 zIFYMeO*0@vsqD7%ePO7&gyuaLhIErlu7O8bOPxlOtJxWOS(UGsuiP*hY|#`t>g26; zO3sW<$^vV@f_z}4j-G@}iXI^aZcC+i ze~$j8KS1FH5yf)X#_oqZertj8qLEXQ47*9TCev_Qx40u=2)BiE_d$oFx35tl7lR%eXf71o?bI^GWsYO(wHSCn<$iO1E+wEu!FQ)PW z3+#NA+SReK!3k^kn*R4EH2I{aU|X^N`SBew8&t;Z^#xgx;n$d7ynJWx49>7lbuJ~i zEdz%H1O42~0}TM7?W?oQT?jT!V?ZLsy95IVD$jK4nF$Kaj_{uKq!44+B#Q^psWX|H zB|OeK&TSq7p#-AWTU)rnTyGr-DwR1>ln+$x7tvT|Om7?%hKPWymKvLp)EdQq-*brr zVnhu>`OZK6l>5*${5TFT8TH}A%)&L4?Hy>Aeu1iwG>tD!=dqezY~90YdpI0S935U0 zaQ2vxMVQ4}ZqBP4^Kdkn`6@>y;~_W8H$V0$7Bz`hL&%(7)CnZp0uL- zBb(xQ^5Wp{JW&5wrp9^s~Bwsj}); zsF@a8IF!V&cfMtG(9F#*UEWsBkQI7F#j4k6xoGu-y#0+KMO|*?oK>-<&rGjPk2-M7 zE2v?s&mD2LWb4>dGOJ$N6230Al{!EjnJI=vn!tPeg|SWg)?gR*R9Qg;OrjF1JCQ?k z_taLl%&(8m?CY>sAr}|rJRTJ!_^VygEX@BGpA9EHDZP!OmxG*pjSX57e zi$(t9H_d}(jy1PD^ugF=vxyE^n4Shax5o!%V5cfQZU%!l{09 zc?$-Y?N^|mfqM+gQ0;y`DQX(y3p0t zm6W}kcCudZ;j;s}$hOi>&z&O?zDeHw^7TRLi<<1+*wAdA8v*7_<(d2>q!!`F{uZE7B3e!~g&Q literal 0 HcmV?d00001 diff --git a/src/modelec_gui/src/main.cpp b/src/modelec_gui/src/main.cpp index 6015fdb..a6fdab9 100644 --- a/src/modelec_gui/src/main.cpp +++ b/src/modelec_gui/src/main.cpp @@ -4,6 +4,7 @@ #include "modelec_gui/modelec_gui.hpp" #include +#include int main(int argc, char **argv) { @@ -13,14 +14,11 @@ int main(int argc, char **argv) auto node = rclcpp::Node::make_shared("qt_gui_node"); - node->declare_parameter("map.size.map_width_mm", 3000); - node->declare_parameter("map.size.map_height_mm", 2000); - - node->declare_parameter("robot.size.length_mm", 200); - node->declare_parameter("robot.size.width_mm", 300); - - node->declare_parameter("enemy.size.length_mm", 200); - node->declare_parameter("enemy.size.width_mm", 300); + std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; + if (!Modelec::Config::load(config_path)) + { + RCLCPP_ERROR(node->get_logger(), "Failed to load config file: %s", config_path.c_str()); + } ModelecGUI::ROS2QtGUI window(node); window.show(); diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index a34e384..895f731 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -2,7 +2,7 @@ #include #include #include - +#include namespace ModelecGUI { @@ -34,6 +34,32 @@ namespace ModelecGUI { { stackedWidget->setCurrentWidget(map_page_); }); + + if (Modelec::Config::get("config.ihm.sound.enabled", false)) + { + RCLCPP_INFO(get_node()->get_logger(), "Sound enabled, setting up audio player"); + + media_player_ = new QMediaPlayer(this); + audio_output_ = new QAudioOutput(this); + media_player_->setAudioOutput(audio_output_); + + audio_sub_ = node_->create_subscription( + "audio/play", 10, [this](const std_msgs::msg::String::SharedPtr msg) + { + if (Modelec::Config::has("config.ihm.sound." + msg->data + "@path")) + { + QString audio_file = QString::fromStdString(Modelec::Config::get("config.ihm.sound." + msg->data + "@path")); + media_player_->setSource(QUrl::fromLocalFile(audio_file)); + media_player_->play(); + } + }); + } + + quit_sub_ = node_->create_subscription( + "gui/quit", 10, [this](const std_msgs::msg::Empty::SharedPtr) + { + close(); + }); } void ROS2QtGUI::setupMenu() { diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 7ff6f4c..d34bec0 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -5,6 +5,8 @@ #include #include +#include + namespace ModelecGUI { MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), @@ -48,14 +50,14 @@ namespace ModelecGUI qpoints = {}; - map_height_ = node_->get_parameter("map.size.map_height_mm").as_int(); - map_width_ = node_->get_parameter("map.size.map_width_mm").as_int(); + map_height_ = Modelec::Config::get("config.map.size.map_height_mm", 2000); + map_width_ = Modelec::Config::get("config.map.size.map_width_mm", 3000); - robot_length_ = node_->get_parameter("robot.size.length_mm").as_int(); - robot_width_ = node_->get_parameter("robot.size.width_mm").as_int(); + robot_length_ = Modelec::Config::get("config.robot.size.length_mm", 300); + robot_width_ = Modelec::Config::get("config.robot.size.width_mm", 200); - enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int(); - enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int(); + enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); + enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); add_waypoint_sub_ = node_->create_subscription( "odometry/add_waypoint", 100, diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index a31af6b..005d02e 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -22,14 +23,14 @@ namespace ModelecGUI qpoints = {}; - map_height_ = node_->get_parameter("map.size.map_height_mm").as_int(); - map_width_ = node_->get_parameter("map.size.map_width_mm").as_int(); + map_height_ = Modelec::Config::get("config.map.size.map_height_mm", 2000); + map_width_ = Modelec::Config::get("config.map.size.map_width_mm", 3000); - robot_length_ = node_->get_parameter("robot.size.length_mm").as_int(); - robot_width_ = node_->get_parameter("robot.size.width_mm").as_int(); + robot_length_ = Modelec::Config::get("config.robot.size.length_mm", 300); + robot_width_ = Modelec::Config::get("config.robot.size.width_mm", 200); - enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int(); - enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int(); + enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); + enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); add_waypoint_sub_ = node_->create_subscription( "odometry/add_waypoint", 100, diff --git a/src/modelec_interfaces/msg/Action/ActionServoTimed.msg b/src/modelec_interfaces/msg/Action/ActionServoTimed.msg index b54ca83..974cf7c 100644 --- a/src/modelec_interfaces/msg/Action/ActionServoTimed.msg +++ b/src/modelec_interfaces/msg/Action/ActionServoTimed.msg @@ -2,4 +2,5 @@ int32 id float64 start_angle float64 end_angle float64 duration_s -bool success \ No newline at end of file +float64 delay_s +bool success diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 7db4dee..7b3fa60 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -26,13 +26,16 @@ set(strat_fsm_sources src/action/free_action.cpp src/action/take_action.cpp src/action/toggle_servo_action.cpp - src/action/look_on_action.cpp src/action/thermo_action.cpp src/missions/go_home_mission.cpp src/missions/take_mission.cpp src/missions/free_mission.cpp src/missions/thermo_mission.cpp + src/missions/action_mission.cpp + src/missions/move_mission.cpp + src/missions/mine_time_mission.cpp + src/missions/mission_base.cpp src/obstacle/obstacle.cpp src/obstacle/box.cpp diff --git a/src/modelec_strat/data/action.xml b/src/modelec_strat/data/action.xml new file mode 100644 index 0000000..13bf0f7 --- /dev/null +++ b/src/modelec_strat/data/action.xml @@ -0,0 +1,118 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 18 +
1
+ 0 + 2 + 0.5 + + + + + + + + + + + + \ No newline at end of file diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 738bc91..60b5b42 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -3,7 +3,7 @@ 300 300 - 50 + 75 50 @@ -12,21 +12,20 @@ 300 50 - -2 300 300 - 100 + 20 - 600 - 400 + 3000 + 2000 3000 2000 @@ -50,18 +49,61 @@ 450 + 10 + + + + 10 + + + + + 0.1 + + + + + 3 + + + 2 + + + 10 + + + 77 65 - false + + false + + + + + + 20 0.6 0.7 + -2 - 20 + + + 20 + + + + + 10 + + + + @@ -75,21 +117,56 @@ true - ./cam/ + ./cam/ true - http://192.168.1.21:8080/video - true + http://192.168.1.10:8080/video + false - - - - + + + + + + + + + + + + + - \ No newline at end of file + + + /dev/USB_ODO + 115200 + + + /dev/USB_ACTION + 115200 + + 300 + 5 + + + + false + + + + + + + + + + + + diff --git a/src/modelec_strat/data/deposite_zone.xml b/src/modelec_strat/data/deposite_zone.xml index 5decfea..b78fd8d 100644 --- a/src/modelec_strat/data/deposite_zone.xml +++ b/src/modelec_strat/data/deposite_zone.xml @@ -1,23 +1,23 @@ - + -1.5708 - + -1.5708 - + 0 - - + + 0 @@ -25,7 +25,7 @@ -1.5708 3.1415 - + @@ -50,12 +50,12 @@ 3.1415 - + 1.5708 - + diff --git a/src/modelec_strat/data/obstacles.xml b/src/modelec_strat/data/obstacles.xml index d4ccf6d..4fb819b 100644 --- a/src/modelec_strat/data/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -3,20 +3,20 @@ - - + - - + + - - - + + + diff --git a/src/modelec_strat/include/modelec_strat/action/base_action.hpp b/src/modelec_strat/include/modelec_strat/action/base_action.hpp index 944f34a..c049a66 100644 --- a/src/modelec_strat/include/modelec_strat/action/base_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/base_action.hpp @@ -12,6 +12,8 @@ #include #include +#include + namespace Modelec { class ActionExecutor; @@ -79,4 +81,30 @@ namespace Modelec return nullptr; } + + template<> + inline ActionServoTimed + Config::get( + const std::string& prefix, + const ActionServoTimed&, bool) + { + ActionServoTimed msg; + msg.id = get(prefix + "@id", 0); + msg.start_angle = get(prefix + "@start_angle", 0); + msg.end_angle = get(prefix + "@end_angle", 0); + msg.duration_s = get(prefix + "@duration_s", 0); + msg.delay_s = get(prefix + "@delay_s", 0, true); + return msg; + } + + template<> + inline std::vector + Config::get>( + const std::string& prefix, + const std::vector& default_value, bool) + { + auto result = getArray(prefix); + + return result.empty() ? default_value : result; + } } diff --git a/src/modelec_strat/include/modelec_strat/action/down_action.hpp b/src/modelec_strat/include/modelec_strat/action/down_action.hpp index 0884b7a..bbf010d 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -15,6 +15,7 @@ namespace Modelec void Init(const std::vector& params) override; void SetSide(Side side); void SetInverted(bool inverted); + static void InitConfig(); void End() override; @@ -27,6 +28,13 @@ namespace Modelec std::queue steps_; + static inline std::vector front_unrotated_msg_; + static inline std::vector front_rotated_msg_; + static inline std::vector back_unrotated_msg_; + static inline std::vector back_rotated_msg_; + + static inline bool isConfigInit_ = false; + inline static const bool registered_ = []() { diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index fcfb79e..da714e1 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -18,6 +18,7 @@ namespace Modelec void AddServo(int id, Side side); void AddServo(std::pair servo); void AddServos(const std::vector>& servos); + static void InitConfig(); void End() override; @@ -28,6 +29,14 @@ namespace Modelec std::queue steps_; + static inline int first_servo_; + static inline int second_servo_; + static inline double start_angle_; + static inline double end_angle_; + static inline double duration_s_; + + static inline bool isConfigInit_ = false; + inline static const bool registered_ = []() { diff --git a/src/modelec_strat/include/modelec_strat/action/look_on_action.hpp b/src/modelec_strat/include/modelec_strat/action/look_on_action.hpp deleted file mode 100644 index 0534cbf..0000000 --- a/src/modelec_strat/include/modelec_strat/action/look_on_action.hpp +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -#include -#include - -namespace Modelec -{ - class LookOnAction : public BaseAction - { - public: - LookOnAction(const std::shared_ptr& action_executor); - LookOnAction(const std::shared_ptr& action_executor, Side side); - - void Next() override; - void Init(const std::vector& params) override; - void SetSide(Side side); - - void End() override; - - inline static const std::string Name = ActionExec::LOOK_ON; - - private: - Side side_ = CENTER; - - std::queue steps_; - - inline static const bool registered_ = - []() - { - BaseAction::Registry()[Name] = - [](const std::shared_ptr& exec) - { - return std::make_shared(exec); - }; - return true; - }(); - }; -} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index 0ad3de6..6b63bcf 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -18,6 +18,7 @@ namespace Modelec void AddServo(int id, Side side); void AddServo(std::pair servo); void AddServos(const std::vector>& servos); + static void InitConfig(); void End() override; @@ -28,6 +29,14 @@ namespace Modelec std::queue steps_; + static inline int first_servo_; + static inline int second_servo_; + static inline double start_angle_; + static inline double end_angle_; + static inline double duration_s_; + + static inline bool isConfigInit_ = false; + inline static const bool registered_ = []() { diff --git a/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp b/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp index 9cf1ac6..c6a0e08 100644 --- a/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp @@ -15,6 +15,7 @@ namespace Modelec void Init(const std::vector& params) override; void SetSide(Side side); void SetDeploy(bool deploy); + static void InitConfig(); void End() override; @@ -26,6 +27,13 @@ namespace Modelec std::queue steps_; + static inline std::vector left_deploy_msg_; + static inline std::vector left_undeploy_msg_; + static inline std::vector right_deploy_msg_; + static inline std::vector right_undeploy_msg_; + + static inline bool isConfigInit_ = false; + inline static const bool registered_ = []() { diff --git a/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp index 76c8484..7a1fe2d 100644 --- a/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp @@ -18,6 +18,7 @@ namespace Modelec void AddServo(int id, Side side); void AddServo(std::pair servo); void AddServos(const std::vector>& servos); + static void InitConfig(); void End() override; @@ -28,6 +29,14 @@ namespace Modelec std::queue steps_; + static inline int first_servo_; + static inline int second_servo_; + static inline double start_angle_; + static inline double end_angle_; + static inline double duration_s_; + + static inline bool isConfigInit_ = false; + inline static const bool registered_ = []() { diff --git a/src/modelec_strat/include/modelec_strat/action/up_action.hpp b/src/modelec_strat/include/modelec_strat/action/up_action.hpp index 46b980d..8f2b8b2 100644 --- a/src/modelec_strat/include/modelec_strat/action/up_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/up_action.hpp @@ -14,6 +14,7 @@ namespace Modelec void Next() override; void Init(const std::vector& params) override; void SetSide(Side side); + static void InitConfig(); void End() override; @@ -24,6 +25,13 @@ namespace Modelec std::queue steps_; + static inline std::vector front_rotated_msg_; + static inline std::vector front_unrotated_msg_; + static inline std::vector back_rotated_msg_; + static inline std::vector back_unrotated_msg_; + + static inline bool isConfigInit_ = false; + inline static const bool registered_ = []() { diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index becc6fc..0ef8a0d 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -9,11 +9,11 @@ #include #include +#include #include #include #include "action/base_action.hpp" -#include "missions/thermo_mission.hpp" #include "obstacle/box.hpp" namespace Modelec @@ -55,8 +55,6 @@ namespace Modelec void ActivateThermo(BaseAction::Side side, bool deploy, bool force = false); - void LookOn(BaseAction::Side side, bool force = false); - void ActionFinished(const std::shared_ptr& action); std::array, 2> box_obstacles_; @@ -68,7 +66,10 @@ namespace Modelec bool down; bool rotated; }; - std::array arm_pos_; + std::array arm_pos_ = { + ArmState{false, false}, + ArmState{false, false}, + }; std::array servo_value_ = { 2.91, @@ -86,10 +87,6 @@ namespace Modelec {BaseAction::RIGHT, false}, }; - BaseAction::Side cam_side_ = BaseAction::Side::CENTER; - - bool looking_on_front_ = true; - bool IsEmpty() const; bool IsFull() const; @@ -143,6 +140,9 @@ namespace Modelec float last_left_trig = 1.0f; float last_right_trig = 1.0f; + bool last_l3 = false; + bool last_r3 = false; + private: rclcpp::Node::SharedPtr node_; }; diff --git a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp index 1876e40..c44e3ba 100644 --- a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp +++ b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp @@ -1,7 +1,6 @@ #pragma once -#include -#include +#include #include namespace Modelec @@ -9,7 +8,7 @@ namespace Modelec class DepositeZone { public: - DepositeZone(tinyxml2::XMLElement* obstacleElem); + DepositeZone(int id); Point GetPosition() const; @@ -20,13 +19,12 @@ namespace Modelec bool Validate() const; int GetId() const; - int GetMaxPot() const; int GetWidth() const; int GetHeight() const; protected: - int id_, max_pot_; + int id_; int w_, h_; Point position_; diff --git a/src/modelec_strat/include/modelec_strat/missions/action_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/action_mission.hpp new file mode 100644 index 0000000..a8d386f --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/action_mission.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include +#include + +namespace Modelec { + class ActionMission : virtual public Mission { + public: + ActionMission(const std::shared_ptr& action_executor); + + static void InitConfig(); + + void Start(const rclcpp::Node::SharedPtr& node) override; + + bool Update() override; + + std::string GetName() const override { return "Action"; } + + protected: + std::shared_ptr action_executor_; + rclcpp::Time deploy_time_; + + static double TIMEOUT; + static bool IsInit; + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp index 783eb3a..08fa1d4 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -1,20 +1,19 @@ #pragma once -#include -#include -#include +#include +#include +#include namespace Modelec { - class FreeMission : public Mission { + class FreeMission : public ActionMission, public MoveMission, public MinTimeMission { public: FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, BaseAction::Side side = BaseAction::FRONT); - void Start(rclcpp::Node::SharedPtr node) override; - void Update() override; + void Start(const rclcpp::Node::SharedPtr& node) override; + bool Update() override; void Clear() override; - MissionStatus GetStatus() const override; std::string GetName() const override { return "Free"; } private: @@ -33,20 +32,8 @@ namespace Modelec { BaseAction::Side side_; - MissionStatus status_; - std::shared_ptr nav_; - std::shared_ptr action_executor_; - rclcpp::Node::SharedPtr node_; - std::shared_ptr target_deposite_zone_; double angle_; - - rclcpp::Time go_timeout_; - rclcpp::Time deploy_time_; - - std::optional min_time_; - - rclcpp::Time last_ask_waypoint_time_; }; } \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp index 0ba87a9..ea3b1bd 100644 --- a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp @@ -1,20 +1,18 @@ #pragma once -#include -#include +#include #include namespace Modelec { - class GoHomeMission : public Mission + class GoHomeMission : public MoveMission { public: GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time); - void Start(rclcpp::Node::SharedPtr node) override; - void Update() override; + void Start(const rclcpp::Node::SharedPtr& node) override; + bool Update() override; void Clear() override; - MissionStatus GetStatus() const override; std::string GetName() const override { return "GoHome"; } private: @@ -26,18 +24,10 @@ namespace Modelec DONE, }; - MissionStatus status_; - std::shared_ptr nav_; rclcpp::Time go_home_time_; - rclcpp::Node::SharedPtr node_; rclcpp::Time start_time_; Point home_point_; rclcpp::Publisher::SharedPtr score_pub_; int mission_score_ = 0; - rclcpp::Time go_timeout_; - - std::optional min_time_; - - rclcpp::Time last_ask_waypoint_time_; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/min_time_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/min_time_mission.hpp new file mode 100644 index 0000000..c7db03b --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/min_time_mission.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +namespace Modelec { + class MinTimeMission : virtual public Mission { + public: + MinTimeMission(); + + static void InitConfig(); + + void Start(const rclcpp::Node::SharedPtr& node) override; + + bool Update() override; + + std::string GetName() const override { return "MinTime"; } + + protected: + std::optional min_time_; + + static double MIN_TIME_DURATION; + static bool IsInit; + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp index fa0e360..c706a71 100644 --- a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp @@ -2,7 +2,7 @@ #include #include - +#include namespace Modelec { @@ -18,14 +18,19 @@ namespace Modelec class Mission { public: + Mission(MissionStatus status = MissionStatus::READY); virtual ~Mission() = default; - virtual void Start(rclcpp::Node::SharedPtr node) = 0; - virtual void Update() = 0; + + virtual void Start(const rclcpp::Node::SharedPtr& node); + virtual bool Update() = 0; virtual void Clear() = 0; - virtual MissionStatus GetStatus() const = 0; + + virtual MissionStatus GetStatus() const; virtual std::string GetName() const = 0; protected: std::queue steps_; + rclcpp::Node::SharedPtr node_; + MissionStatus status_; }; -} +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/move_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/move_mission.hpp new file mode 100644 index 0000000..938e2d4 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/move_mission.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include +#include + +namespace Modelec { + class MoveMission : virtual public Mission { + public: + MoveMission(const std::shared_ptr& nav); + + static void InitConfig(); + + void Start(const rclcpp::Node::SharedPtr& node) override; + + bool Update() override; + + std::string GetName() const override { return "Move"; } + + protected: + std::shared_ptr nav_; + + rclcpp::Time go_timeout_; + rclcpp::Time last_ask_waypoint_time_; + + static double MIN_ASK_WAYPOINT; + static double ASK_WAYPOINT_INTERVAL; + static double GO_TIMEOUT; + static bool IsInit; + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp index 494cc6d..4df7d4f 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -1,20 +1,19 @@ #pragma once -#include -#include -#include +#include +#include +#include namespace Modelec { - class TakeMission : public Mission { + class TakeMission : public MinTimeMission, public ActionMission, public MoveMission { public: TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, BaseAction::Side side = BaseAction::FRONT); - void Start(rclcpp::Node::SharedPtr node) override; - void Update() override; + void Start(const rclcpp::Node::SharedPtr& node) override; + bool Update() override; void Clear() override; - MissionStatus GetStatus() const override; std::string GetName() const override { return "Take"; } private: @@ -31,16 +30,5 @@ namespace Modelec { BaseAction::Side side_; std::shared_ptr closestBox; - MissionStatus status_; - std::shared_ptr nav_; - std::shared_ptr action_executor_; - rclcpp::Node::SharedPtr node_; - - rclcpp::Time go_timeout_; - rclcpp::Time deploy_time_; - - std::optional min_time_; - - rclcpp::Time last_ask_waypoint_time_; }; -} \ No newline at end of file +} diff --git a/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp index b2d90ea..529d5d0 100644 --- a/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp @@ -1,19 +1,17 @@ #pragma once -#include -#include -#include +#include +#include namespace Modelec { - class ThermoMission : public Mission { + class ThermoMission : public ActionMission, public MoveMission { public: ThermoMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor); - void Start(rclcpp::Node::SharedPtr node) override; - void Update() override; + void Start(const rclcpp::Node::SharedPtr& node) override; + bool Update() override; void Clear() override; - MissionStatus GetStatus() const override; std::string GetName() const override { return "Thermo"; } static bool IsThermoDone; @@ -32,16 +30,5 @@ namespace Modelec { std::array thermo_positions_; std::shared_ptr closestBox; - MissionStatus status_; - std::shared_ptr nav_; - std::shared_ptr action_executor_; - rclcpp::Node::SharedPtr node_; - - rclcpp::Time go_timeout_; - rclcpp::Time deploy_time_; - - std::optional min_time_; - - rclcpp::Time last_ask_waypoint_time_; }; } \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 8ca9a91..c97d716 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -85,26 +85,26 @@ namespace Modelec PosMsg::SharedPtr GetCurrentPos() const; - bool LoadDepositeZoneFromXML(const std::string& filename); + void LoadDepositeZoneFromXML(); std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr& pos, const std::vector& blacklistedId = {}, bool only_free = false); template ::value>> - std::shared_ptr GetClosestObstacle(const PosMsg::SharedPtr& pos) const; + std::shared_ptr GetClosestObstacle(const PosMsg::SharedPtr& pos, bool countObjective = false) const; PosMsg::SharedPtr GetHomePosition(); std::array GetThermoPositions(); - void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + void OnEnemyPosition(modelec_interfaces::msg::OdometryPos::SharedPtr msg); - void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + void OnEnemyPositionLongTime(modelec_interfaces::msg::OdometryPos::SharedPtr msg); bool DoesLineIntersectCircle(const Point& start, const Point& end, const Point& center, float radius); - bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg); + bool EnemyOnPath(modelec_interfaces::msg::OdometryPos msg); bool Replan(bool force = false); @@ -152,7 +152,7 @@ namespace Modelec PosMsg::SharedPtr current_pos_; - std::map> deposite_zones_; + std::vector> deposite_zones_; rclcpp::Subscription::SharedPtr waypoint_reach_sub_; rclcpp::Publisher::SharedPtr waypoint_pub_; @@ -181,7 +181,7 @@ namespace Modelec }; template - std::shared_ptr NavigationHelper::GetClosestObstacle(const PosMsg::SharedPtr& pos) const + std::shared_ptr NavigationHelper::GetClosestObstacle(const PosMsg::SharedPtr& pos, bool countObjective) const { std::shared_ptr closest_obstacle = nullptr; auto robotPos = Point(pos->x, pos->y, pos->theta); @@ -190,9 +190,9 @@ namespace Modelec for (const auto& obstacle : GetPathfinding()->GetObstacles()) { - if (auto obs = std::dynamic_pointer_cast(obstacle.second)) + if (auto obs = std::dynamic_pointer_cast(obstacle)) { - if (!obs->IsAtObjective()) + if (!obs->IsAtObjective() || countObjective) { auto obsPoint = obs->GetPosition(); double distance = Point::distance(robotPos, obsPoint); diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 4cb70a2..3a6713e 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -2,6 +2,7 @@ #include "obstacle.hpp" #include +#include namespace Modelec { @@ -18,7 +19,7 @@ namespace Modelec BoxObstacle(const BoxObstacle&) = default; BoxObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type); - BoxObstacle(tinyxml2::XMLElement* obstacleElem); + BoxObstacle(int id); BoxObstacle(const modelec_interfaces::msg::Obstacle& msg); Point GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const; @@ -42,8 +43,8 @@ namespace Modelec std::array colors_ = { BLUE, BLUE, - BLUE, - BLUE + YELLOW, + YELLOW }; }; } diff --git a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp index 130c3ed..16efa1e 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp @@ -18,8 +18,8 @@ namespace Modelec } Obstacle(int id, int x, int y, double theta, int w, int h, const std::string& type); - Obstacle(tinyxml2::XMLElement* obstacleElem); Obstacle(const modelec_interfaces::msg::Obstacle& msg); + Obstacle(int id); Obstacle(const Obstacle& other) = default; virtual ~Obstacle() = default; @@ -32,7 +32,7 @@ namespace Modelec double GetTheta() const; int GetWidth() const; int GetHeight() const; - const std::string& Type() const; + const std::string& GetType() const; Point GetPosition() const; void SetId(int id); @@ -52,11 +52,15 @@ namespace Modelec bool IsAtObjective() const; void SetAtObjective(bool atObjective); + bool ShouldTakeCountInPathfinding() const; + void SetTakeCountInPathfinding(bool takeCount); + protected: int id_, x_, y_, w_, h_; double theta_; std::string type_; bool isAtObjective = false; + bool takeCountInPathfinding = true; }; } diff --git a/src/modelec_strat/include/modelec_strat/pami_manager.hpp b/src/modelec_strat/include/modelec_strat/pami_manager.hpp index c46aa34..f260c79 100644 --- a/src/modelec_strat/include/modelec_strat/pami_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp @@ -2,39 +2,11 @@ #include -#include - -#include - -#include - -#include -#include - namespace Modelec { class PamiManger : public rclcpp::Node { public: PamiManger(); - - bool ReadFromXML(const std::string& filename); - - protected: - std::vector zones_; - - int time_to_put_zone_ = 0; - int time_to_remove_top_pot_ = 0; - - int score_to_add_ = 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_; - rclcpp::Subscription::SharedPtr reset_strat_sub_; }; } diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index fe15f90..5299bf2 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -73,14 +73,14 @@ namespace Modelec //void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal); - bool LoadObstaclesFromXML(const std::string& filename); + void LoadObstaclesFromXML(); void SetCurrentPos(const PosMsg::SharedPtr& pos); [[nodiscard]] std::shared_ptr GetObstacle(int id) const; - std::map> GetObstacles() const { - return obstacle_map_; + std::vector> GetObstacles() const { + return obstacles_; } void RemoveObstacle(int id); @@ -114,7 +114,7 @@ namespace Modelec std::vector> grid_; - std::map> obstacle_map_; + std::vector> obstacles_; PosMsg::SharedPtr current_start_; PosMsg::SharedPtr current_goal_; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index f8a93a3..4688dc1 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -2,13 +2,17 @@ #include "modelec_strat/action_executor.hpp" -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor) : BaseAction(action_executor), side_(BOTH), inverted_(false) +Modelec::DownAction::DownAction(const std::shared_ptr& action_executor) : BaseAction(action_executor), + side_(BOTH), inverted_(false) { steps_.push(ActionExec::DOWN_STEP); steps_.push(ActionExec::DONE_STEP); + + InitConfig(); } -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Side side, bool inverted) : DownAction(action_executor) +Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Side side, + bool inverted) : DownAction(action_executor) { side_ = side; inverted_ = inverted; @@ -30,53 +34,27 @@ void Modelec::DownAction::Next() case ActionExec::DOWN_STEP: { ActionServoTimedArray msg; - msg.items.resize(side_ == BOTH ? 8 : 4); if (side_ == FRONT || side_ == BOTH) { - msg.items[0].id = 0; - msg.items[0].start_angle = 1.76; - msg.items[0].end_angle = 2.93; - msg.items[0].duration_s = 2; - - msg.items[1].id = 1; - msg.items[1].start_angle = 2.06; - msg.items[1].end_angle = 0.91; - msg.items[1].duration_s = 2; - - msg.items[2].id = 2; - msg.items[2].start_angle = 0.5; - msg.items[2].end_angle = inverted_ ? 0 : 3.05; - msg.items[2].duration_s = 1.7; - - msg.items[3].id = 3; - msg.items[3].start_angle = 2.6; - msg.items[3].end_angle = inverted_ ? 3.1 : 0.3; - msg.items[3].duration_s = 1.7; + if (inverted_) + { + msg.items.insert(msg.items.end(), front_rotated_msg_.begin(), front_rotated_msg_.end()); + } else + { + msg.items.insert(msg.items.end(), front_unrotated_msg_.begin(), front_unrotated_msg_.end()); + } } if (side_ == BACK || side_ == BOTH) { - int i = side_ == BOTH ? 4 : 0; - msg.items[i].id = 8; - msg.items[i].start_angle = 0; - msg.items[i].end_angle = 0; - msg.items[i].duration_s = 0.5; - - msg.items[i+1].id = 9; - msg.items[i+1].start_angle = 0; - msg.items[i+1].end_angle = 0; - msg.items[i+1].duration_s = 0.5; - - msg.items[i+2].id = 10; - msg.items[i+2].start_angle = 0; - msg.items[i+2].end_angle = 0; - msg.items[i+2].duration_s = 0.5; - - msg.items[i+3].id = 11; - msg.items[i+3].start_angle = 0; - msg.items[i+3].end_angle = 0; - msg.items[i+3].duration_s = 0.5; + if (inverted_) + { + msg.items.insert(msg.items.end(), back_rotated_msg_.begin(), back_rotated_msg_.end()); + } else + { + msg.items.insert(msg.items.end(), back_unrotated_msg_.begin(), back_unrotated_msg_.end()); + } } action_executor_->MoveServoTimed(msg); @@ -111,6 +89,17 @@ void Modelec::DownAction::SetInverted(bool inverted) inverted_ = inverted; } +void Modelec::DownAction::InitConfig() +{ + if (isConfigInit_) return; + isConfigInit_ = true; + + front_unrotated_msg_ = Config::get>("action.down.front.unrotated.msg"); + front_rotated_msg_ = Config::get>("action.down.front.rotated.msg"); + back_unrotated_msg_ = Config::get>("action.down.back.unrotated.msg"); + back_rotated_msg_ = Config::get>("action.down.back.rotated.msg"); +} + void Modelec::DownAction::End() { if (side_ == BOTH) diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 30cb1a4..ade120b 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -6,6 +6,8 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex { steps_.push(ActionExec::FREE_STEP); steps_.push(ActionExec::DONE_STEP); + + InitConfig(); } Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, Side side, int n) : FreeAction(action_executor) @@ -45,9 +47,9 @@ void Modelec::FreeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = 3; - msg.items[i].end_angle = 1; - msg.items[i].duration_s = 0.5; + msg.items[i].start_angle = start_angle_; + msg.items[i].end_angle = end_angle_; + msg.items[i].duration_s = duration_s_; } action_executor_->MoveServoTimed(msg); @@ -91,6 +93,18 @@ void Modelec::FreeAction::AddServos(const std::vector>& ser servos_.insert(servos_.end(), servos.begin(), servos.end()); } +void Modelec::FreeAction::InitConfig() +{ + if (isConfigInit_) return; + isConfigInit_ = true; + + first_servo_ = Config::get("action.free.id@first", 0); + second_servo_ = Config::get("action.free.id@second", 0); + start_angle_ = Config::get("action.free.msg@start_angle", 3); + end_angle_ = Config::get("action.free.msg@end_angle", 1); + duration_s_ = Config::get("action.free.msg@duration_s", 0.5); +} + void Modelec::FreeAction::End() { for (auto servo : servos_) diff --git a/src/modelec_strat/src/action/look_on_action.cpp b/src/modelec_strat/src/action/look_on_action.cpp deleted file mode 100644 index 5663bd9..0000000 --- a/src/modelec_strat/src/action/look_on_action.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include - -#include "modelec_strat/action_executor.hpp" - -Modelec::LookOnAction::LookOnAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) -{ - steps_.push(ActionExec::LOOK_ON_STEP); - steps_.push(ActionExec::DONE_STEP); -} - -Modelec::LookOnAction::LookOnAction(const std::shared_ptr& action_executor, Side side) : LookOnAction(action_executor) -{ - side_ = side; -} - -void Modelec::LookOnAction::Next() -{ - if (steps_.empty()) - { - done_ = true; - return; - } - - auto step = steps_.front(); - steps_.pop(); - - switch (step) - { - case ActionExec::LOOK_ON_STEP: - { - modelec_interfaces::msg::ActionServoTimedArray msg; - - msg.items.resize(1); - - msg.items[0].id = 18; - msg.items[0].start_angle = action_executor_->cam_side_ == CENTER ? 1 : action_executor_->cam_side_ == FRONT ? 0 : 2; - msg.items[0].end_angle = side_ == CENTER ? 1 : side_ == FRONT ? 0 : 2; - msg.items[0].duration_s = 0.5; - - action_executor_->MoveServoTimed(msg); - } - break; - case ActionExec::DONE_STEP: - { - done_ = true; - } - break; - default: - break; - } -} - -void Modelec::LookOnAction::Init(const std::vector& params) -{ - if (!params.empty()) - { - SetSide(static_cast(std::stoi(params[0]))); - } -} - -void Modelec::LookOnAction::SetSide(Side side) -{ - side_ = side; -} - -void Modelec::LookOnAction::End() -{ - action_executor_->cam_side_ = side_; -} diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 91752e5..0679bbb 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -6,6 +6,8 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_ex { steps_.push(ActionExec::TAKE_STEP); steps_.push(ActionExec::DONE_STEP); + + InitConfig(); } Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, Side side, int n) : TakeAction(action_executor) @@ -44,10 +46,10 @@ void Modelec::TakeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { - msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = 0; - msg.items[i].end_angle = 3; - msg.items[i].duration_s = 0.5; + msg.items[i].id = servos_[i].first + (servos_[i].second ? first_servo_ : second_servo_); + msg.items[i].start_angle = start_angle_; + msg.items[i].end_angle = end_angle_; + msg.items[i].duration_s = duration_s_; } action_executor_->MoveServoTimed(msg); @@ -91,6 +93,18 @@ void Modelec::TakeAction::AddServos(const std::vector>& ser servos_.insert(servos_.end(), servos.begin(), servos.end()); } +void Modelec::TakeAction::InitConfig() +{ + if (isConfigInit_) return; + isConfigInit_ = true; + + first_servo_ = Config::get("action.take.id@first", 0); + second_servo_ = Config::get("action.take.id@second", 0); + start_angle_ = Config::get("action.take.msg@start_angle", 3); + end_angle_ = Config::get("action.take.msg@end_angle", 1); + duration_s_ = Config::get("action.take.msg@duration_s", 0.5); +} + void Modelec::TakeAction::End() { for (auto servo : servos_) diff --git a/src/modelec_strat/src/action/thermo_action.cpp b/src/modelec_strat/src/action/thermo_action.cpp index 3a395bc..9f5bfd8 100644 --- a/src/modelec_strat/src/action/thermo_action.cpp +++ b/src/modelec_strat/src/action/thermo_action.cpp @@ -6,6 +6,8 @@ Modelec::ThermoAction::ThermoAction(const std::shared_ptr& actio { steps_.push(ActionExec::THERMO_STEP); steps_.push(ActionExec::DONE_STEP); + + InitConfig(); } Modelec::ThermoAction::ThermoAction(const std::shared_ptr& action_executor, Side side, bool deploy) : ThermoAction(action_executor) @@ -31,22 +33,26 @@ void Modelec::ThermoAction::Next() { modelec_interfaces::msg::ActionServoTimedArray msg; - msg.items.resize(side_ == BOTH ? 2 : 1); - if (side_ == LEFT || side_ == BOTH) { - msg.items[0].id = 16; - msg.items[0].start_angle = deploy_ ? 1 : 2; - msg.items[0].end_angle = deploy_ ? 2 : 1; - msg.items[0].duration_s = 0.5; + if (deploy_) + { + msg.items.insert(msg.items.end(), left_deploy_msg_.begin(), left_deploy_msg_.end()); + } else + { + msg.items.insert(msg.items.end(), left_undeploy_msg_.begin(), left_undeploy_msg_.end()); + } } if (side_ == RIGHT || side_ == BOTH) { - msg.items[side_ == BOTH ? 1 : 0].id = 17; - msg.items[side_ == BOTH ? 1 : 0].start_angle = deploy_ ? 1 : 2; - msg.items[side_ == BOTH ? 1 : 0].end_angle = deploy_ ? 2 : 1; - msg.items[side_ == BOTH ? 1 : 0].duration_s = 0.5; + if (deploy_) + { + msg.items.insert(msg.items.end(), right_deploy_msg_.begin(), right_deploy_msg_.end()); + } else + { + msg.items.insert(msg.items.end(), right_undeploy_msg_.begin(), right_undeploy_msg_.end()); + } } action_executor_->MoveServoTimed(msg); @@ -81,6 +87,17 @@ void Modelec::ThermoAction::SetDeploy(bool deploy) deploy_ = deploy; } +void Modelec::ThermoAction::InitConfig() +{ + if (isConfigInit_) return; + isConfigInit_ = true; + + left_deploy_msg_ = Config::get>("action.thermo.left.deploy"); + left_undeploy_msg_ = Config::get>("action.thermo.left.undeploy"); + right_deploy_msg_ = Config::get>("action.thermo.right.deploy"); + right_undeploy_msg_ = Config::get>("action.thermo.right.undeploy"); +} + void Modelec::ThermoAction::End() { action_executor_->thermo_state_[side_] = deploy_; diff --git a/src/modelec_strat/src/action/toggle_servo_action.cpp b/src/modelec_strat/src/action/toggle_servo_action.cpp index 03778a3..9f7aa0c 100644 --- a/src/modelec_strat/src/action/toggle_servo_action.cpp +++ b/src/modelec_strat/src/action/toggle_servo_action.cpp @@ -6,6 +6,8 @@ Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, Side side, int n) : ToggleServoAction(action_executor) @@ -44,10 +46,10 @@ void Modelec::ToggleServoAction::Next() for (size_t i = 0; i < servos_.size(); i++) { - msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? 1 : 3; - msg.items[i].end_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? 3 : 1; - msg.items[i].duration_s = 0.5; + msg.items[i].id = servos_[i].first + (servos_[i].second ? first_servo_ : second_servo_); + msg.items[i].start_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? start_angle_ : end_angle_; + msg.items[i].end_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? end_angle_ : start_angle_; + msg.items[i].duration_s = duration_s_; } action_executor_->MoveServoTimed(msg); @@ -91,6 +93,18 @@ void Modelec::ToggleServoAction::AddServos(const std::vector("action.toggle.id@first", 0); + second_servo_ = Config::get("action.toggle.id@second", 0); + start_angle_ = Config::get("action.toggle.msg@start_angle", 3); + end_angle_ = Config::get("action.toggle.msg@end_angle", 1); + duration_s_ = Config::get("action.toggle.msg@duration_s", 0.5); +} + void Modelec::ToggleServoAction::End() { for (auto servo : servos_) diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index bef7095..24ab533 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -6,6 +6,8 @@ Modelec::UPAction::UPAction(const std::shared_ptr& action_execut { steps_.push(ActionExec::UP_STEP); steps_.push(ActionExec::DONE_STEP); + + InitConfig(); } Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, Side side) : UPAction(action_executor) @@ -29,53 +31,27 @@ void Modelec::UPAction::Next() case ActionExec::UP_STEP: { ActionServoTimedArray msg; - msg.items.resize(side_ == BOTH ? 8 : 4); if (side_ == FRONT || side_ == BOTH) { - msg.items[0].id = 0; - msg.items[0].start_angle = 2.93; - msg.items[0].end_angle = 1.76; - msg.items[0].duration_s = 2; - - msg.items[1].id = 1; - msg.items[1].start_angle = 0.91; - msg.items[1].end_angle = 2.06; - msg.items[1].duration_s = 2; - - msg.items[2].id = 2; - msg.items[2].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 0 : 3.2; - msg.items[2].end_angle = 0.5; - msg.items[2].duration_s = 2; - - msg.items[3].id = 3; - msg.items[3].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 3.1 : 0; - msg.items[3].end_angle = 2.6; - msg.items[3].duration_s = 2; + if (action_executor_->arm_pos_[FRONT].rotated) + { + msg.items.insert(msg.items.end(), front_rotated_msg_.begin(), front_rotated_msg_.end()); + } else + { + msg.items.insert(msg.items.end(), front_unrotated_msg_.begin(), front_unrotated_msg_.end()); + } } - if (side_ == BACK || side_ == BOTH) { - int i = side_ == BOTH ? 4 : 0; - - msg.items[i].id = 8; - msg.items[i].start_angle = 0; - msg.items[i].end_angle = 0; - msg.items[i].duration_s = 0.5; - - msg.items[i+1].id = 9; - msg.items[i+1].start_angle = 0; - msg.items[i+1].end_angle = 0; - msg.items[i+1].duration_s = 0.5; - - msg.items[i+2].id = 10; - msg.items[i+2].start_angle = 0; - msg.items[i+2].end_angle = 0; - msg.items[i+2].duration_s = 0.5; - - msg.items[i+3].id = 11; - msg.items[i+3].start_angle = 0; - msg.items[i+3].end_angle = 0; - msg.items[i+3].duration_s = 0.5; + if (side_ == BACK || side_ == BOTH) + { + if (action_executor_->arm_pos_[BACK].rotated) + { + msg.items.insert(msg.items.end(), back_rotated_msg_.begin(), back_rotated_msg_.end()); + } else + { + msg.items.insert(msg.items.end(), back_unrotated_msg_.begin(), back_unrotated_msg_.end()); + } } action_executor_->MoveServoTimed(msg); @@ -104,6 +80,17 @@ void Modelec::UPAction::SetSide(Side side) side_ = side; } +void Modelec::UPAction::InitConfig() +{ + if (isConfigInit_) return; + isConfigInit_ = true; + + front_rotated_msg_ = Config::get>("action.up.front.rotated.msg"); + front_unrotated_msg_ = Config::get>("action.up.front.unrotated.msg"); + back_rotated_msg_ = Config::get>("action.up.back.rotated.msg"); + back_unrotated_msg_ = Config::get>("action.up.back.unrotated.msg"); +} + void Modelec::UPAction::End() { if (side_ == BOTH) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index c927d76..df77e7d 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -5,7 +5,6 @@ #include "modelec_strat/action/free_action.hpp" #include "modelec_strat/action/take_action.hpp" #include "modelec_strat/action/toggle_servo_action.hpp" -#include "modelec_strat/action/look_on_action.hpp" #include "modelec_strat/action/thermo_action.hpp" #include "modelec_utils/utils.hpp" @@ -87,7 +86,6 @@ namespace Modelec joy_sub_ = node_->create_subscription( "/joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr msg) { - // use game controller to manually control all the action. make it carefully if (msg->buttons.size() >= 15) { if (msg->buttons[0] == 1) // A button @@ -132,13 +130,19 @@ namespace Modelec RotateArm(BaseAction::FRONT, false, !arm_pos_[BaseAction::FRONT].rotated); } } - else if (msg->buttons[14] == 1) // LT button + else if (msg->buttons[13] == 1) // LT button { - ActivateThermo(BaseAction::Side::LEFT, !thermo_state_[BaseAction::Side::LEFT]); + if (action_done_) + { + ActivateThermo(BaseAction::Side::LEFT, !thermo_state_[BaseAction::Side::LEFT]); + } } - else if (msg->buttons[15] == 1) // LR button + else if (msg->buttons[14] == 1) // LR button { - ActivateThermo(BaseAction::Side::RIGHT, !thermo_state_[BaseAction::Side::RIGHT]); + if (action_done_) + { + ActivateThermo(BaseAction::Side::RIGHT, !thermo_state_[BaseAction::Side::RIGHT]); + } } } if (msg->axes.size() == 8) @@ -223,10 +227,18 @@ namespace Modelec RCLCPP_DEBUG(node_->get_logger(), "Color detection succeeded: %s", res[1].c_str()); - if (box_obstacles_[cam_side_] != nullptr) { - box_obstacles_[cam_side_]->ParseColor(res[1]); + auto color = split(res[1], ';'); + + if (box_obstacles_[BaseAction::FRONT] != nullptr) { + box_obstacles_[BaseAction::FRONT]->ParseColor(std::vector(color.begin(), color.begin() + 4)); } else { - RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side"); + RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box obstacle found to update"); + } + + if (box_obstacles_[BaseAction::BACK] != nullptr) { + box_obstacles_[BaseAction::BACK]->ParseColor(std::vector(color.begin() + 4, color.begin() + 8)); + } else { + RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box obstacle found to update"); } }); @@ -476,22 +488,6 @@ namespace Modelec } } - void ActionExecutor::LookOn(BaseAction::Side side, bool force) - { - if (cam_side_ != side || force) - { - auto action = std::make_shared(shared_from_this(), side); - action_.push(action); - if (action_done_) - { - step_running_ = 0; - } - action_done_ = false; - - Update(); - } - } - void ActionExecutor::ActionFinished(const std::shared_ptr& action) { RCLCPP_DEBUG( @@ -539,27 +535,6 @@ namespace Modelec void ActionExecutor::AskColor() const { - /*if (!ask_color_client_->service_is_ready()) { - RCLCPP_ERROR(node_->get_logger(), "Color detection service not ready"); - return; - } - - auto request = std::make_shared(); - - ask_color_client_->async_send_request(request, - [this](rclcpp::Client::SharedFuture future) { - auto response = future.get(); - if (response->success) { - RCLCPP_INFO(node_->get_logger(), "Color detection succeeded: %s", response->message.c_str()); - if (box_obstacles_[cam_side_] != nullptr) { - box_obstacles_[cam_side_]->ParseColor(response->message); - } else { - RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side"); - } - } else { - RCLCPP_ERROR(node_->get_logger(), "Color detection failed: %s", response->message.c_str()); - } - });*/ ask_pub_->publish(std_msgs::msg::Empty()); } } diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index cfd388b..ef72a44 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -4,27 +4,24 @@ namespace Modelec { - DepositeZone::DepositeZone(tinyxml2::XMLElement* obstacleElem) + DepositeZone::DepositeZone(int id) { - obstacleElem->QueryIntAttribute("id", &id_); + id_ = Config::get("Map.DepositeZone[" + std::to_string(id) + "]@id", id); + w_ = Config::get("Map.DepositeZone[" + std::to_string(id) + "].Pos@w"); + h_ = Config::get("Map.DepositeZone[" + std::to_string(id) + "].Pos@h"); + position_ = Point( + Config::get("Map.DepositeZone[" + std::to_string(id) + "].Pos@x"), + Config::get("Map.DepositeZone[" + std::to_string(id) + "].Pos@y"), + Config::get("Map.DepositeZone[" + std::to_string(id) + "].Pos@theta") + ); - auto posElem = obstacleElem->FirstChildElement("Pos"); - if (posElem) + if (Config::count("Map.DepositeZone[" + std::to_string(id) + "].TakeAngle.Angle") > 0) { - posElem->QueryIntAttribute("x", &position_.x); - posElem->QueryIntAttribute("y", &position_.y); - posElem->QueryDoubleAttribute("theta", &position_.theta); - posElem->QueryIntAttribute("w", &w_); - posElem->QueryIntAttribute("h", &h_); - - if (auto TakeAngleElem = obstacleElem->FirstChildElement("TakeAngle")) - { - for (auto takePos = TakeAngleElem->FirstChildElement("Angle"); takePos; takePos = takePos->NextSiblingElement("Angle")) - { - double angle = takePos->DoubleText(0); - take_angle_.push_back(angle); - } - } + take_angle_ = Config::getArray("Map.DepositeZone[" + std::to_string(id) + "].TakeAngle.Angle"); + } else + { + auto take_angle = Config::get("Map.DepositeZone[" + std::to_string(id) + "].TakeAngle.Angle", position_.theta); + take_angle_ = {take_angle}; } } @@ -76,9 +73,6 @@ namespace Modelec int DepositeZone::GetId() const { return id_; } - int DepositeZone::GetMaxPot() const - { return max_pot_; } - int DepositeZone::GetWidth() const { return w_; } diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 8f325c4..ecd19fa 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -7,29 +7,22 @@ namespace Modelec { EnemyManager::EnemyManager() : Node("enemy_manager") { - this->declare_parameter("enemy.detection.min_move_threshold_mm", 50.0); - this->declare_parameter("enemy.detection.refresh_rate", 1.0); - this->declare_parameter("enemy.detection.max_stationary_time_s", 10.0); - this->declare_parameter("enemy.detection.margin_detection_table_mm", 50.0); - this->declare_parameter("enemy.detection.min_emergency_distance_mm", 500.0); + if (!Config::load(ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml")) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file"); + } - this->declare_parameter("map.size.map_width_mm", 3000.0); - this->declare_parameter("map.size.map_height_mm", 2000.0); + min_move_threshold_mm_ = Config::get("config.enemy.detection.min_move_threshold_mm"); + refresh_rate_s_ = Config::get("config.enemy.detection.refresh_rate"); + max_stationary_time_s_ = Config::get("config.enemy.detection.max_stationary_time_s"); + margin_detection_table_ = Config::get("config.enemy.detection.margin_detection_table_mm"); + min_emergency_distance_ = Config::get("config.enemy.detection.min_emergency_distance_mm"); - this->declare_parameter("robot.size.width_mm", 500.0); - this->declare_parameter("robot.size.length_mm", 500.0); + map_width_ = Config::get("config.map.size.map_width_mm"); + map_height_ = Config::get("config.map.size.map_height_mm"); - min_move_threshold_mm_ = this->get_parameter("enemy.detection.min_move_threshold_mm").as_double(); - refresh_rate_s_ = this->get_parameter("enemy.detection.refresh_rate").as_double(); - max_stationary_time_s_ = this->get_parameter("enemy.detection.max_stationary_time_s").as_double(); - margin_detection_table_ = this->get_parameter("enemy.detection.margin_detection_table_mm").as_double(); - min_emergency_distance_ = this->get_parameter("enemy.detection.min_emergency_distance_mm").as_double(); - - map_width_ = this->get_parameter("map.size.map_width_mm").as_double(); - map_height_ = this->get_parameter("map.size.map_height_mm").as_double(); - - robot_width_ = this->get_parameter("robot.size.width_mm").as_double(); - robot_length_ = this->get_parameter("robot.size.length_mm").as_double(); + robot_width_ = Config::get("config.robot.size.width_mm"); + robot_length_ = Config::get("config.robot.size.length_mm"); current_pos_sub_ = this->create_subscription( "odometry/position", 10, diff --git a/src/modelec_strat/src/missions/action_mission.cpp b/src/modelec_strat/src/missions/action_mission.cpp new file mode 100644 index 0000000..db645d5 --- /dev/null +++ b/src/modelec_strat/src/missions/action_mission.cpp @@ -0,0 +1,47 @@ +#include +#include +#include + +namespace Modelec { + + double ActionMission::TIMEOUT = 5.0; + bool ActionMission::IsInit = false; + + ActionMission::ActionMission(const std::shared_ptr& action_executor) + : action_executor_(action_executor) + { + } + + void ActionMission::InitConfig() + { + TIMEOUT = Config::get("config.mission.action.timeout.s", 5.0); + IsInit = true; + } + + void ActionMission::Start(const rclcpp::Node::SharedPtr& node) + { + Mission::Start(node); + + if (!IsInit) + { + InitConfig(); + } + + deploy_time_ = node->now(); + } + + bool ActionMission::Update() + { + if (!action_executor_->IsActionDone()) + { + if ((node_->now() - deploy_time_).seconds() > TIMEOUT) + { + RCLCPP_WARN(node_->get_logger(), "ActionMission Update TIMEOUT"); + return true; + } + return false; + } + + return true; + } +} \ No newline at end of file diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index e53cbdf..e42d280 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -4,19 +4,17 @@ namespace Modelec { FreeMission::FreeMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor, - BaseAction::Side side) - : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + const std::shared_ptr& action_executor, BaseAction::Side side) : + Mission(MissionStatus::READY), ActionMission(action_executor), MoveMission(nav), + side_(side) { } - void FreeMission::Start(rclcpp::Node::SharedPtr node) + void FreeMission::Start(const rclcpp::Node::SharedPtr& node) { - node_ = node; - - go_timeout_ = node_->now(); - deploy_time_ = node_->now(); - last_ask_waypoint_time_ = node_->now(); + ActionMission::Start(node); + MoveMission::Start(node); + MinTimeMission::Start(node); status_ = MissionStatus::RUNNING; @@ -34,40 +32,13 @@ namespace Modelec { steps_.push(DONE); } - void FreeMission::Update() + bool FreeMission::Update() { - if (!action_executor_->IsActionDone()) + if (!ActionMission::Update() || !MoveMission::Update() || !MinTimeMission::Update()) { - return; + return false; } - if (!nav_->HasArrived()) - { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) - { - nav_->AskWaypoint(); - last_ask_waypoint_time_ = node_->now(); - return; - } - if ((node_->now() - go_timeout_).seconds() < 10) - { - return; - } - } - - if (min_time_.has_value()) - { - if ((node_->now() - min_time_.value()).seconds() < 0.1) - { - return; - } - else - { - min_time_.reset(); - } - } - - auto step_ = steps_.front(); steps_.pop(); @@ -82,7 +53,7 @@ namespace Modelec { if (target_deposite_zone_ == nullptr) { status_ = MissionStatus::FAILED; - return; + return false; } auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); @@ -96,12 +67,10 @@ namespace Modelec { if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - return; + return false; } } - action_executor_->LookOn(BaseAction::Side::CENTER); - go_timeout_ = node_->now(); } break; @@ -195,7 +164,7 @@ namespace Modelec { if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - return; + return false; } go_timeout_ = node_->now(); @@ -210,8 +179,8 @@ namespace Modelec { auto pos = nav_->GetCurrentPos(); obs->SetPosition( - pos->x + 300 * cos(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)), - pos->y + 300 * sin(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)), + target_deposite_zone_->GetPosition().x, + target_deposite_zone_->GetPosition().y, pos->theta); obs->SetAtObjective(true); @@ -226,15 +195,12 @@ namespace Modelec { default: break; } + + return true; } void FreeMission::Clear() { } - MissionStatus FreeMission::GetStatus() const - { - return status_; - } - } diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 9b11d74..32e3a93 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -5,23 +5,19 @@ namespace Modelec { GoHomeMission::GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time) : - status_(MissionStatus::READY), nav_(nav), start_time_(start_time) + Mission(MissionStatus::READY), MoveMission(nav), + start_time_(start_time) { } - void GoHomeMission::Start(rclcpp::Node::SharedPtr node) + void GoHomeMission::Start(const rclcpp::Node::SharedPtr& node) { - node_ = node; + MoveMission::Start(node); - node->declare_parameter("mission_score.go_home", 0); - - mission_score_ = node->get_parameter("mission_score.go_home").as_int(); + mission_score_ = Config::get("config.mission_score.go_home", 0); score_pub_ = node_->create_publisher("/strat/score", 10); - go_timeout_ = node_->now(); - last_ask_waypoint_time_ = node_->now(); - status_ = MissionStatus::RUNNING; std::queue empty; @@ -33,32 +29,11 @@ namespace Modelec steps_.push(DONE); } - void GoHomeMission::Update() + bool GoHomeMission::Update() { - if (!nav_->HasArrived()) + if (!MoveMission::Update()) { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) - { - nav_->AskWaypoint(); - last_ask_waypoint_time_ = node_->now(); - return; - } - if ((node_->now() - go_timeout_).seconds() < 10) - { - return; - } - } - - if (min_time_.has_value()) - { - if ((node_->now() - min_time_.value()).seconds() < 0.1) - { - return; - } - else - { - min_time_.reset(); - } + return false; } auto step_ = steps_.front(); @@ -75,7 +50,7 @@ namespace Modelec if (nav_->CanGoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - return; + return false; } } nav_->RotateTo(home_point_); @@ -90,7 +65,7 @@ namespace Modelec if (nav_->GoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - return; + return false; } } @@ -99,11 +74,6 @@ namespace Modelec break; case GO_CLOSE: { - /*if ((node_->now() - start_time_).seconds() < 94) - { - break; - }*/ - nav_->GoTo(home_point_, true); go_timeout_ = node_->now(); @@ -121,14 +91,12 @@ namespace Modelec default: break; } + + return true; } void GoHomeMission::Clear() { } - MissionStatus GoHomeMission::GetStatus() const - { - return status_; - } } diff --git a/src/modelec_strat/src/missions/mine_time_mission.cpp b/src/modelec_strat/src/missions/mine_time_mission.cpp new file mode 100644 index 0000000..0563658 --- /dev/null +++ b/src/modelec_strat/src/missions/mine_time_mission.cpp @@ -0,0 +1,44 @@ +#include +#include + +namespace Modelec { + + double MinTimeMission::MIN_TIME_DURATION = 0.1; + bool MinTimeMission::IsInit = false; + + MinTimeMission::MinTimeMission() : min_time_(std::nullopt) + { + } + + void MinTimeMission::InitConfig() + { + MIN_TIME_DURATION = Config::get("config.mission.min_time.min_time_duration.s", 0.1); + IsInit = true; + } + + void MinTimeMission::Start(const rclcpp::Node::SharedPtr& node) + { + Mission::Start(node); + + if (!IsInit) + { + InitConfig(); + } + } + + bool MinTimeMission::Update() + { + if (min_time_.has_value()) + { + if ((node_->now() - min_time_.value()).seconds() < MIN_TIME_DURATION) + { + return false; + } + else + { + min_time_.reset(); + } + } + return true; + } +} \ No newline at end of file diff --git a/src/modelec_strat/src/missions/mission_base.cpp b/src/modelec_strat/src/missions/mission_base.cpp new file mode 100644 index 0000000..2e8bfdc --- /dev/null +++ b/src/modelec_strat/src/missions/mission_base.cpp @@ -0,0 +1,19 @@ +#include + +namespace Modelec +{ + Mission::Mission(MissionStatus status) + : status_(status) + { + } + + void Mission::Start(const rclcpp::Node::SharedPtr& node) + { + node_ = node; + } + + MissionStatus Mission::GetStatus() const + { + return status_; + } +} \ No newline at end of file diff --git a/src/modelec_strat/src/missions/move_mission.cpp b/src/modelec_strat/src/missions/move_mission.cpp new file mode 100644 index 0000000..16c8c40 --- /dev/null +++ b/src/modelec_strat/src/missions/move_mission.cpp @@ -0,0 +1,65 @@ +#include +#include +#include + +namespace Modelec { + + double MoveMission::MIN_ASK_WAYPOINT = 3.0; + double MoveMission::ASK_WAYPOINT_INTERVAL = 2.0; + double MoveMission::GO_TIMEOUT = 10.0; + bool MoveMission::IsInit = false; + + MoveMission::MoveMission(const std::shared_ptr& nav) + : nav_(nav) + { + } + + void MoveMission::InitConfig() + { + MIN_ASK_WAYPOINT = Config::get("config.mission.move.min_ask_waypoint.s", 3.0); + ASK_WAYPOINT_INTERVAL = Config::get("config.mission.move.ask_waypoint_interval.s", 2.0); + GO_TIMEOUT = Config::get("config.mission.move.go_timeout.s", 10.0); + + IsInit = true; + } + + void MoveMission::Start(const rclcpp::Node::SharedPtr& node) + { + Mission::Start(node); + + if (!IsInit) + { + InitConfig(); + } + + go_timeout_ = node->now(); + last_ask_waypoint_time_ = node->now(); + } + + bool MoveMission::Update() + { + if (!nav_->HasArrived()) + { + auto now = node_->now(); + double elapsed_total = (now - go_timeout_).seconds(); + double elapsed_since_last_ask = (now - last_ask_waypoint_time_).seconds(); + + if (elapsed_total > MIN_ASK_WAYPOINT && elapsed_since_last_ask > ASK_WAYPOINT_INTERVAL) + { + nav_->AskWaypoint(); + last_ask_waypoint_time_ = now; + return false; + } + + if (elapsed_total > GO_TIMEOUT) + { + RCLCPP_WARN(node_->get_logger(), "Go Timeout"); + return true; + } + + return false; + } + + return true; + } +} \ No newline at end of file diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index efe1cab..1a69efe 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -4,19 +4,17 @@ namespace Modelec { TakeMission::TakeMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor, - BaseAction::Side side) - : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + const std::shared_ptr& action_executor, BaseAction::Side side) : + Mission(MissionStatus::READY), ActionMission(action_executor), MoveMission(nav), + side_(side) { } - void TakeMission::Start(rclcpp::Node::SharedPtr node) + void TakeMission::Start(const rclcpp::Node::SharedPtr& node) { - node_ = node; - - go_timeout_ = node_->now(); - deploy_time_ = node_->now(); - last_ask_waypoint_time_ = node_->now(); + ActionMission::Start(node); + MoveMission::Start(node); + MinTimeMission::Start(node); status_ = MissionStatus::RUNNING; @@ -31,37 +29,11 @@ namespace Modelec { steps_.push(DONE); } - void TakeMission::Update() + bool TakeMission::Update() { - if (!action_executor_->IsActionDone()) + if (!ActionMission::Update() || !MoveMission::Update() || !MinTimeMission::Update()) { - return; - } - - if (!nav_->HasArrived()) - { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) - { - nav_->AskWaypoint(); - last_ask_waypoint_time_ = node_->now(); - return; - } - if ((node_->now() - go_timeout_).seconds() < 10) - { - return; - } - } - - if (min_time_.has_value()) - { - if ((node_->now() - min_time_.value()).seconds() < 0.1) - { - return; - } - else - { - min_time_.reset(); - } + return false; } auto step_ = steps_.front(); @@ -85,6 +57,8 @@ namespace Modelec { auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); + // TODO the robot do not go to the exact position i would like it to go to the exact so he can go forward after to take the box in a good way + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) @@ -92,7 +66,7 @@ namespace Modelec { if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - break; + return false; } } } @@ -105,7 +79,7 @@ namespace Modelec { if (action_executor_->box_obstacles_[side_] == nullptr) { status_ = MissionStatus::FAILED; - break; + return false; } auto pos = action_executor_->box_obstacles_[side_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); @@ -114,7 +88,7 @@ namespace Modelec { if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - break; + return false; } go_timeout_ = node_->now(); @@ -137,7 +111,6 @@ namespace Modelec { case UP: { action_executor_->Up(side_); - action_executor_->LookOn(side_); deploy_time_ = node_->now(); } break; @@ -146,7 +119,7 @@ namespace Modelec { if (action_executor_->box_obstacles_[side_] == nullptr) { status_ = MissionStatus::FAILED; - break; + return false; } nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[side_]->GetId()); @@ -159,15 +132,12 @@ namespace Modelec { default: break; } + + return true; } void TakeMission::Clear() { } - MissionStatus TakeMission::GetStatus() const - { - return status_; - } - } diff --git a/src/modelec_strat/src/missions/thermo_mission.cpp b/src/modelec_strat/src/missions/thermo_mission.cpp index 3492dcd..b0fd51e 100644 --- a/src/modelec_strat/src/missions/thermo_mission.cpp +++ b/src/modelec_strat/src/missions/thermo_mission.cpp @@ -7,18 +7,15 @@ namespace Modelec { bool ThermoMission::IsThermoDone = false; ThermoMission::ThermoMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor) - : status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + const std::shared_ptr& action_executor) : + Mission(MissionStatus::READY), ActionMission(action_executor), MoveMission(nav) { } - void ThermoMission::Start(rclcpp::Node::SharedPtr node) + void ThermoMission::Start(const rclcpp::Node::SharedPtr& node) { - node_ = node; - - go_timeout_ = node_->now(); - deploy_time_ = node_->now(); - last_ask_waypoint_time_ = node_->now(); + ActionMission::Start(node); + MoveMission::Start(node); status_ = MissionStatus::RUNNING; @@ -35,37 +32,11 @@ namespace Modelec { steps_.push(DONE); } - void ThermoMission::Update() + bool ThermoMission::Update() { - if (!action_executor_->IsActionDone()) + if (!ActionMission::Update() || !MoveMission::Update()) { - return; - } - - if (!nav_->HasArrived()) - { - if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) - { - nav_->AskWaypoint(); - last_ask_waypoint_time_ = node_->now(); - return; - } - if ((node_->now() - go_timeout_).seconds() < 10) - { - return; - } - } - - if (min_time_.has_value()) - { - if ((node_->now() - min_time_.value()).seconds() < 0.1) - { - return; - } - else - { - min_time_.reset(); - } + return false; } auto step_ = steps_.front(); @@ -80,7 +51,7 @@ namespace Modelec { if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - break; + return false; } go_timeout_ = node_->now(); @@ -93,7 +64,7 @@ namespace Modelec { if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - break; + return false; } go_timeout_ = node_->now(); @@ -101,7 +72,6 @@ namespace Modelec { break; case ACTIVATE_THERMO: { - RCLCPP_INFO(node_->get_logger(), "Activating thermo"); action_executor_->ActivateThermo(nav_->GetTeamId() == NavigationHelper::YELLOW ? BaseAction::RIGHT : BaseAction::LEFT, true); deploy_time_ = node_->now(); } @@ -113,13 +83,12 @@ namespace Modelec { if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; - break; + return false; } } break; case DEACTIVATE_THERMO: { - RCLCPP_INFO(node_->get_logger(), "Deactivating thermo"); action_executor_->ActivateThermo(nav_->GetTeamId() == NavigationHelper::YELLOW ? BaseAction::RIGHT : BaseAction::LEFT, false); deploy_time_ = node_->now(); } @@ -135,15 +104,12 @@ namespace Modelec { default: break; } + + return true; } void ThermoMission::Clear() { } - MissionStatus ThermoMission::GetStatus() const - { - return status_; - } - } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index cb54df1..79d643b 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -1,6 +1,7 @@ #include #include #include +#include namespace Modelec { @@ -9,41 +10,9 @@ namespace Modelec NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node) { - node_->declare_parameter("factor_close_enemy", -0.5); - node_->declare_parameter("factor.theta", 20.0); - node_->declare_parameter("enemy.detection.min_emergency_distance_mm", 390); - - factor_close_enemy_ = node_->get_parameter("factor_close_enemy").as_double(); - factor_theta_ = node_->get_parameter("factor.theta").as_double(); - enemy_emergency_distance_ = node_->get_parameter("enemy.detection.min_emergency_distance_mm").as_int(); - - node_->declare_parameter("home.yellow.x", 0); - node_->declare_parameter("home.yellow.y", 0); - node_->declare_parameter("home.yellow.theta", 0.0); - node_->declare_parameter("home.blue.x", 0); - node_->declare_parameter("home.blue.y", 0); - node_->declare_parameter("home.blue.theta", 0.0); - - node_->declare_parameter("thermo.yellow.start.x", 0); - node_->declare_parameter("thermo.yellow.start.y", 0); - node_->declare_parameter("thermo.yellow.start.theta", 0.0); - node_->declare_parameter("thermo.yellow.finish.x", 0); - node_->declare_parameter("thermo.yellow.finish.y", 0); - node_->declare_parameter("thermo.yellow.finish.theta", 0.0); - - node_->declare_parameter("thermo.blue.start.x", 0); - node_->declare_parameter("thermo.blue.start.y", 0); - node_->declare_parameter("thermo.blue.start.theta", 0.0); - node_->declare_parameter("thermo.blue.finish.x", 0); - node_->declare_parameter("thermo.blue.finish.y", 0); - node_->declare_parameter("thermo.blue.finish.theta", 0.0); - - node_->declare_parameter("spawn.yellow_top.x", 0); - node_->declare_parameter("spawn.yellow_top.y", 0); - node_->declare_parameter("spawn.yellow_top.theta", 0.0); - node_->declare_parameter("spawn.blue_top.x", 0); - node_->declare_parameter("spawn.blue_top.y", 0); - node_->declare_parameter("spawn.blue_top.theta", 0.0); + factor_close_enemy_ = Config::get("config.factor.close_enemy", 1.0f); + factor_theta_ = Config::get("config.factor.theta", 1.0f); + enemy_emergency_distance_ = Config::get("config.enemy.detection.min_emergency_distance_mm", 300); pathfinding_ = std::make_shared(node); @@ -90,13 +59,6 @@ namespace Modelec start_odo_pub_ = node_->create_publisher("odometry/start", 10); - std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + - "/data/deposite_zone.xml"; - if (!LoadDepositeZoneFromXML(deposite_zone_path)) - { - RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML"); - } - spawn_pub_ = node_->create_publisher("nav/spawn", 10); ask_spawn_srv_ = node->create_service( @@ -130,6 +92,8 @@ namespace Modelec odo_ask_waypoint_pub_ = node_->create_publisher( "odometry/ask_active_waypoint", 30); + + LoadDepositeZoneFromXML(); } void NavigationHelper::ReInit() @@ -333,7 +297,7 @@ namespace Modelec { double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x); - if (Point::angleDiff(angle, (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) + if (std::abs(Point::angleDiff(angle, (current_pos_->theta + (front ? 0 : M_PI)))) > M_PI / 4) { Rotate(angle); return true; @@ -505,32 +469,14 @@ namespace Modelec return current_pos_; } - bool NavigationHelper::LoadDepositeZoneFromXML(const std::string& filename) + void NavigationHelper::LoadDepositeZoneFromXML() { - tinyxml2::XMLDocument doc; - if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) + for (size_t i = 0; i < Config::count("Map.DepositeZone"); ++i) { - RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str()); - return false; + deposite_zones_.push_back(std::make_shared(i)); } - tinyxml2::XMLElement* root = doc.FirstChildElement("Map"); - if (!root) - { - RCLCPP_ERROR(node_->get_logger(), "No root element in file"); - return false; - } - - for (tinyxml2::XMLElement* elem = root->FirstChildElement("DepositeZone"); - elem != nullptr; - elem = elem->NextSiblingElement("DepositeZone")) - { - std::shared_ptr obs = std::make_shared(elem); - deposite_zones_[obs->GetId()] = obs; - } - - RCLCPP_INFO(node_->get_logger(), "Loaded %zu zone from XML", deposite_zones_.size()); - return true; + RCLCPP_INFO(node_->get_logger(), "Loaded %zu deposite zones from XML", deposite_zones_.size()); } std::shared_ptr NavigationHelper::GetClosestDepositeZone( @@ -541,10 +487,10 @@ namespace Modelec auto posPoint = Point(pos->x, pos->y, pos->theta); auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta); - for (const auto& [id, zone] : deposite_zones_) + for (const auto& zone : deposite_zones_) { if (blacklistedId.end() == std::find( - blacklistedId.begin(), blacklistedId.end(), id) + blacklistedId.begin(), blacklistedId.end(), zone->GetId()) && (!only_free || !zone->Validate())) { auto zonePoint = zone->GetPosition(); @@ -567,29 +513,29 @@ namespace Modelec { PosMsg::SharedPtr home = std::make_shared(); - std::string prefix = (team_id_ == YELLOW) ? "home.yellow" : "home.blue"; + std::string prefix = (team_id_ == YELLOW) ? "config.home.yellow" : "config.home.blue"; - home->x = node_->get_parameter(prefix + ".x").as_int(); - home->y = node_->get_parameter(prefix + ".y").as_int(); - home->theta = node_->get_parameter(prefix + ".theta").as_double(); + home->x = Config::get(prefix + "@x"); + home->y = Config::get(prefix + "@y"); + home->theta = Config::get(prefix + "@theta"); return home; } std::array NavigationHelper::GetThermoPositions() { - std::string prefix = (team_id_ == YELLOW) ? "thermo.yellow" : "thermo.blue"; + std::string prefix = (team_id_ == YELLOW) ? "config.thermo.yellow" : "config.thermo.blue"; Point start( - node_->get_parameter(prefix + ".start.x").as_int(), - node_->get_parameter(prefix + ".start.y").as_int(), - node_->get_parameter(prefix + ".start.theta").as_double() + Config::get(prefix + ".start@x"), + Config::get(prefix + ".start@y"), + Config::get(prefix + ".start@theta") ); Point finish( - node_->get_parameter(prefix + ".finish.x").as_int(), - node_->get_parameter(prefix + ".finish.y").as_int(), - node_->get_parameter(prefix + ".finish.theta").as_double() + Config::get(prefix + ".finish@x"), + Config::get(prefix + ".finish@y"), + Config::get(prefix + ".finish@theta") ); return {start, finish}; @@ -653,13 +599,13 @@ namespace Modelec pathfinding_->OnEnemyPositionLongTime(msg); Point enemy_pos(msg->x, msg->y, msg->theta); - for (auto& [id, zone] : deposite_zones_) + for (auto& zone : deposite_zones_) { auto zonePos = zone->GetPosition(); if (Point::distance(enemy_pos, zonePos) < (zone->GetWidth() / 2) + pathfinding_->enemy_margin_mm_) { std::shared_ptr obs = std::make_shared( - id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone"); + zone->GetId(), zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone"); pathfinding_->AddObstacle(obs); } } @@ -804,15 +750,15 @@ namespace Modelec void NavigationHelper::SetupSpawn() { spawn_yellow_["top"] = Point( - node_->get_parameter("spawn.yellow_top.x").as_int(), - node_->get_parameter("spawn.yellow_top.y").as_int(), - node_->get_parameter("spawn.yellow_top.theta").as_double() + Config::get("config.spawn.yellow.top@x"), + Config::get("config.spawn.yellow.top@y"), + Config::get("config.spawn.yellow.top@theta") ); spawn_blue_["top"] = Point( - node_->get_parameter("spawn.blue_top.x").as_int(), - node_->get_parameter("spawn.blue_top.y").as_int(), - node_->get_parameter("spawn.blue_top.theta").as_double() + Config::get("config.spawn.blue.top@x"), + Config::get("config.spawn.blue.top@y"), + Config::get("config.spawn.blue.top@theta") ); } } \ No newline at end of file diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index 7e19e5f..2ab29de 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -9,15 +9,17 @@ namespace Modelec { } - BoxObstacle::BoxObstacle(tinyxml2::XMLElement* obstacleElem) - : Obstacle(obstacleElem) + BoxObstacle::BoxObstacle(int id) : Obstacle(id) { - possible_angles_.push_back(theta_); - for (auto elem = obstacleElem->FirstChildElement("possible-angle"); - elem; - elem = elem->NextSiblingElement("possible-angle")) + if (Config::count("Obstacles.Obstacle[" + std::to_string(id) + "].possible-angle") > 0) { - possible_angles_.push_back(elem->DoubleAttribute("theta")); + possible_angles_ = Config::getArray("Obstacles.Obstacle[" + std::to_string(id) + "].possible-angle", [](const std::string& base) + { + return Config::get(base + "@theta"); + }); + possible_angles_.push_back(theta_); + } else { + possible_angles_ = {theta_}; } } diff --git a/src/modelec_strat/src/obstacle/obstacle.cpp b/src/modelec_strat/src/obstacle/obstacle.cpp index 575e524..464be93 100644 --- a/src/modelec_strat/src/obstacle/obstacle.cpp +++ b/src/modelec_strat/src/obstacle/obstacle.cpp @@ -1,5 +1,7 @@ #include +#include "modelec_utils/config.hpp" + namespace Modelec { Obstacle::Obstacle(int id, int x, int y, double theta, int w, int h, const std::string& type) @@ -7,28 +9,22 @@ namespace Modelec { } - 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->QueryDoubleAttribute("theta", &theta_) != 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), theta_(msg.theta), type_("unknown") { } + Obstacle::Obstacle(int id) + { + id_ = Config::get("Obstacles.Obstacle[" + std::to_string(id) + "]@id", id); + x_ = Config::get("Obstacles.Obstacle[" + std::to_string(id) + "]@x"); + y_ = Config::get("Obstacles.Obstacle[" + std::to_string(id) + "]@y"); + theta_ = Config::get("Obstacles.Obstacle[" + std::to_string(id) + "]@theta"); + w_ = Config::get("Obstacles.Obstacle[" + std::to_string(id) + "]@width"); + h_ = Config::get("Obstacles.Obstacle[" + std::to_string(id) + "]@height"); + type_ = Config::get("Obstacles.Obstacle[" + std::to_string(id) + "]@type", "box"); + } + modelec_interfaces::msg::Obstacle Obstacle::toMsg() const { modelec_interfaces::msg::Obstacle msg; @@ -62,7 +58,7 @@ namespace Modelec int Obstacle::GetHeight() const { return h_; } - const std::string& Obstacle::Type() const + const std::string& Obstacle::GetType() const { return type_; } Point Obstacle::GetPosition() const @@ -118,4 +114,14 @@ namespace Modelec { isAtObjective = atObjective; } + + bool Obstacle::ShouldTakeCountInPathfinding() const + { + return takeCountInPathfinding; + } + + void Obstacle::SetTakeCountInPathfinding(bool takeCount) + { + takeCountInPathfinding = takeCount; + } } diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp index 052a567..4b4d188 100644 --- a/src/modelec_strat/src/pami_manager.cpp +++ b/src/modelec_strat/src/pami_manager.cpp @@ -5,92 +5,6 @@ namespace Modelec { PamiManger::PamiManger() : Node("pami_manager") { - this->declare_parameter("time_to_put_zone", 80); - this->declare_parameter("time_to_remove_top_pot", 70); - - time_to_put_zone_ = this->get_parameter("time_to_put_zone").as_int(); - time_to_remove_top_pot_ = this->get_parameter("time_to_remove_top_pot").as_int(); - - score_to_add_ = 0; - - 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) - { - /*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_add_ = create_wall_timer( - second_goal - now, - [this]() - { - for (const auto& obs : zones_) - { - add_obs_pub_->publish(obs.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); - - reset_strat_sub_ = create_subscription( - "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr) - { - /*if (timer_add_) - { - timer_add_->cancel(); - }*/ - /*if (timer_remove_) - { - timer_remove_->cancel(); - }*/ - }); - } - - 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; - } - - for (tinyxml2::XMLElement* elem = root->FirstChildElement("zone"); - elem; - elem = elem->NextSiblingElement("zone")) - { - zones_.push_back(Obstacle(elem)); - } - - RCLCPP_INFO(get_logger(), "Loaded zone obstacles from XML"); - return true; } } diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 9775ca8..f077bfb 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -1,5 +1,6 @@ #include #include +#include namespace Modelec { struct AStarNode { @@ -22,39 +23,22 @@ namespace Modelec { } Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr &node) : node_(node) { - node_->declare_parameter("map.size.map_width_mm", 3000); - node_->declare_parameter("map.size.map_height_mm", 2000); - node_->declare_parameter("map.size.grid_width", 300); - node_->declare_parameter("map.size.grid_height", 200); - map_width_mm_ = node_->get_parameter("map.size.map_width_mm").as_int(); - map_height_mm_ = node_->get_parameter("map.size.map_height_mm").as_int(); - grid_width_ = node_->get_parameter("map.size.grid_width").as_int(); - grid_height_ = node_->get_parameter("map.size.grid_height").as_int(); + map_width_mm_ = Config::get("config.map.size.map_width_mm", 3000); + map_height_mm_ = Config::get("config.map.size.map_height_mm", 2000); + grid_width_ = Config::get("config.map.size.grid_width", 300); + grid_height_ = Config::get("config.map.size.grid_height", 200); - node_->declare_parameter("robot.size.length_mm", 300); - node_->declare_parameter("robot.size.width_mm", 300); - node_->declare_parameter("robot.size.margin_mm", 100); + robot_length_mm_ = Config::get("config.robot.size.length_mm", 300); + robot_width_mm_ = Config::get("config.robot.size.width_mm", 200); + margin_mm_ = Config::get("config.robot.size.margin_mm", 50); - robot_length_mm_ = node_->get_parameter("robot.size.length_mm").as_int(); - robot_width_mm_ = node_->get_parameter("robot.size.width_mm").as_int(); - margin_mm_ = node_->get_parameter("robot.size.margin_mm").as_int(); + enemy_length_mm_ = Config::get("config.enemy.size.length_mm", 300); + enemy_width_mm_ = Config::get("config.enemy.size.width_mm", 300); + enemy_margin_mm_ = Config::get("config.enemy.size.margin_mm", 0); + factor_close_enemy_ = Config::get("config.factor.close_enemy", 1.0f); - node_->declare_parameter("enemy.size.length_mm", 300); - node_->declare_parameter("enemy.size.width_mm", 300); - node_->declare_parameter("enemy.size.margin_mm", 50); - node_->declare_parameter("enemy.factor_close_enemy", -0.5); - - enemy_length_mm_ = node_->get_parameter("enemy.size.length_mm").as_int(); - enemy_width_mm_ = node_->get_parameter("enemy.size.width_mm").as_int(); - enemy_margin_mm_ = node_->get_parameter("enemy.size.margin_mm").as_int(); - factor_close_enemy_ = node_->get_parameter("enemy.factor_close_enemy").as_double(); - - std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + - "/data/obstacles.xml"; - if (!LoadObstaclesFromXML(obstacles_path)) { - RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML"); - } + LoadObstaclesFromXML(); obstacle_add_sub_ = node_->create_subscription( "obstacle/add", 10, @@ -201,7 +185,11 @@ namespace Modelec { // 2. Fill obstacles with inflation // TODO some bug exist with the inflate - for (const auto &[id, obstacle]: obstacle_map_) { + for (const auto &obstacle: obstacles_) { + if (!obstacle->ShouldTakeCountInPathfinding()) + { + continue; + } float cx = obstacle->GetX(); float cy = obstacle->GetY(); float width = obstacle->GetWidth() + inflate_x * 2 * cell_size_mm_x; @@ -286,10 +274,10 @@ namespace Modelec { if (!TestCollision(start_x, start_y, collisionMask) || !TestCollision(goal_x, goal_y, collisionMask)) { if (!TestCollision(start_x, start_y, collisionMask)) { - RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle"); + RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle x=%d y=%d mask=%d | %d", start_x, start_y, collisionMask, grid_[start_y][start_x]); return {grid_[start_y][start_x], waypoints}; } else { - RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle"); + RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle x=%d y=%d mask=%d | %d", goal_x, goal_y, collisionMask, grid_[goal_y][goal_x]); return {grid_[goal_y][goal_x], waypoints}; } } @@ -505,11 +493,13 @@ namespace Modelec { } std::shared_ptr Pathfinding::GetObstacle(int id) const { - return obstacle_map_.at(id); + return obstacles_.at(id); } void Pathfinding::RemoveObstacle(int id) { - obstacle_map_.erase(id); + obstacles_.erase(std::remove_if(obstacles_.begin(), obstacles_.end(), + [id](const std::shared_ptr &obs) { return obs->GetId() == id; }), + obstacles_.end()); modelec_interfaces::msg::Obstacle msg; msg.id = id; @@ -517,7 +507,7 @@ namespace Modelec { } void Pathfinding::AddObstacle(const std::shared_ptr &obstacle) { - obstacle_map_[obstacle->GetId()] = obstacle; + obstacles_.push_back(obstacle); modelec_interfaces::msg::Obstacle msg = obstacle->toMsg(); obstacle_add_pub_->publish(msg); } @@ -542,7 +532,7 @@ namespace Modelec { void Pathfinding::HandleAskObstacleRequest(const std::shared_ptr, const std::shared_ptr) { - for (auto &[index, obs]: obstacle_map_) { + for (auto &obs: obstacles_) { obstacle_add_pub_->publish(obs->toMsg()); } } @@ -554,7 +544,7 @@ namespace Modelec { void Pathfinding::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { Point enemyPos(msg->x, msg->y, msg->theta); - for (auto &[index, obs]: obstacle_map_) { + for (auto &obs: obstacles_) { if (auto column = std::dynamic_pointer_cast(obs)) { if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->GetWidth() / 2) + enemy_margin_mm_) { @@ -568,41 +558,29 @@ namespace Modelec { if (y < 0 || y >= static_cast(grid_.size()) || x < 0 || x >= static_cast(grid_[y].size())) { RCLCPP_WARN(node_->get_logger(), "TestCollision: access out of bounds x=%d y=%d", x, y); - return false; // ou true, selon ce que tu veux (false = pas de collision) + return false; } return (grid_[y][x] & collisionMask) && !(grid_[y][x] & ~collisionMask); } - 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; + void Pathfinding::LoadObstaclesFromXML() { + auto obs = Config::getArray("Obstacles.Obstacle", [](const std::string& prefix) + { + return Config::get(prefix + "@type", "box"); + }); + for (size_t i = 0; i < obs.size(); ++i) + { + if (obs[i] == "box") + { + obstacles_.push_back(std::make_shared(i)); + } else + { + obstacles_.push_back(std::make_shared(i)); + } } - 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")) { - std::shared_ptr obs = std::make_shared(obstacleElem); - obstacle_map_[obs->GetId()] = obs; - } - - for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Box"); - obstacleElem != nullptr; - obstacleElem = obstacleElem->NextSiblingElement("Box")) { - std::shared_ptr obs = std::make_shared(obstacleElem); - obstacle_map_[obs->GetId()] = obs; - } - - RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size()); - return true; + RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacles_.size()); } Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos &pos, int index, bool isLast) { diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 6f85f5c..be9562e 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -15,15 +15,28 @@ namespace Modelec StratFMS::StratFMS() : Node("start_fms") { - this->declare_parameter("static_strat", false); - this->declare_parameter("factor.obs", 1.0); - this->declare_parameter("factor.thermo", 0.5); - this->declare_parameter("timer_period_ms", 100); + std::string data_dir = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/"; + if (!Config::load(data_dir + "/config.xml")) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/config.xml").c_str()); + } + if (!Config::load(data_dir + "/obstacles.xml")) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/obstacles.xml").c_str()); + } + if (!Config::load(data_dir + "/deposite_zone.xml")) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/deposite_zone.xml").c_str()); + } + if (!Config::load(data_dir + "/action.xml")) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", (data_dir + "/action.xml").c_str()); + } - static_strat_ = this->get_parameter("static_strat").as_bool(); - factor_obs_ = this->get_parameter("factor.obs").as_double(); - factor_thermo_ = this->get_parameter("factor.thermo").as_double(); - timer_period_ms_ = this->get_parameter("timer_period_ms").as_int(); + static_strat_ = Config::get("config.static_strat.enabled", false); + factor_obs_ = Config::get("config.factor.obs", 1.0); + factor_thermo_ = Config::get("config.factor.thermo", 1.0); + timer_period_ms_ = Config::get("config.timer.strat.ms", 100); tir_sub_ = create_subscription( "/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr) @@ -153,8 +166,6 @@ namespace Modelec game_action_sequence_.push(State::TAKE_MISSION); game_action_sequence_.push(State::TAKE_MISSION); game_action_sequence_.push(State::FREE_MISSION); - game_action_sequence_.push(State::FREE_MISSION); - game_action_sequence_.push(State::THERMO_MISSION); Transition(State::WAIT_START, "System ready"); } diff --git a/src/modelec_utils/include/modelec_utils/config.hpp b/src/modelec_utils/include/modelec_utils/config.hpp index f391257..263bf42 100644 --- a/src/modelec_utils/include/modelec_utils/config.hpp +++ b/src/modelec_utils/include/modelec_utils/config.hpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace Modelec { @@ -17,19 +18,33 @@ namespace Modelec template using BuilderFunc = std::function; + template + using MapBuilderFunc = std::function(const std::string& base_key)>; + static bool load(const std::string& filepath); template - static T get(const std::string& key, const T& default_value = T()); + static T get(const std::string& key, const T& default_value = T(), bool is_optional = false); template static std::vector getArray(const std::string& prefix, BuilderFunc builder = [](const std::string& base) { return get(base); }); + template + static std::unordered_map getMap( + const std::string& prefix, + MapBuilderFunc builder); + static size_t count(const std::string& prefix); + static void clear(const std::string& prefix); + + static bool has(const std::string& prefix); + static void printAll(); + static void print(const std::string& prefix); + private: static void parseNode(tinyxml2::XMLElement* element, const std::string& key); @@ -37,25 +52,47 @@ namespace Modelec }; template - T Config::get(const std::string& key, const T& default_value) { + T Config::get(const std::string& key, const T& default_value, bool is_optional) { auto it = values_.find(key); - if (it == values_.end()) return default_value; + if (it == values_.end()) + { + if (!is_optional) + { + std::cerr << "Config key not found: " << key << std::endl; + } + return default_value; + } std::istringstream iss(it->second); T result; - if (!(iss >> result)) return default_value; + if (!(iss >> result)) + { + if (!is_optional) + { + std::cerr << "Config key has invalid format: " << key << " = " << it->second << std::endl; + } + return default_value; + } return result; } template<> - inline std::string Config::get(const std::string& key, const std::string& default_value) { + inline std::string Config::get(const std::string& key, const std::string& default_value, bool is_optional) { auto it = values_.find(key); - return it != values_.end() ? it->second : default_value; + if (it == values_.end()) + { + if (is_optional) + { + std::cerr << "Config key not found: " << key << std::endl; + } + return default_value; + } + return it->second; } template<> - inline bool Config::get(const std::string& key, const bool& default_value) { - auto str = get(key, default_value ? "true" : "false"); + inline bool Config::get(const std::string& key, const bool& default_value, bool is_optional) { + auto str = get(key, default_value ? "true" : "false", is_optional); return str == "true" || str == "1"; } @@ -65,13 +102,49 @@ namespace Modelec { std::vector result; - size_t n = Config::count(prefix); + size_t n = count(prefix); result.reserve(n); + if (n > 0) + { + if (n == 1 && !has(prefix + "[0]")) + { + result.emplace_back(builder(prefix)); + } else + { + for (size_t i = 0; i < n; ++i) + { + std::string base = prefix + "[" + std::to_string(i) + "]"; + result.emplace_back(builder(base)); + } + } + } else + { + std::cerr << "Config array not found: " << prefix << std::endl; + } + + return result; + } + + template + std::unordered_map Config::getMap( + const std::string& prefix, + MapBuilderFunc builder) + { + std::unordered_map result; + + size_t n = count(prefix); + if (n == 0) + { + std::cerr << "Config map not found: " << prefix << std::endl; + return result; + } + for (size_t i = 0; i < n; ++i) { std::string base = prefix + "[" + std::to_string(i) + "]"; - result.emplace_back(builder(base)); + auto [key, value] = builder(base); + result.emplace(key, value); } return result; diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index 083c419..d86eb97 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -1,7 +1,7 @@ #pragma once -#define CLOSE_DISTANCE 155 -#define BASE_DISTANCE 310 +#define CLOSE_DISTANCE 170 +#define BASE_DISTANCE 305 namespace Modelec { diff --git a/src/modelec_utils/src/config.cpp b/src/modelec_utils/src/config.cpp index c566b52..32da8d6 100644 --- a/src/modelec_utils/src/config.cpp +++ b/src/modelec_utils/src/config.cpp @@ -1,4 +1,3 @@ -#include #include namespace Modelec @@ -40,14 +39,49 @@ namespace Modelec } } - return found ? max_index + 1 : 0; + return found ? max_index + 1 : has(prefix) ? 1 : 0; + } + + void Config::clear(const std::string& prefix) + { + for (auto it = values_.begin(); it != values_.end();) + { + if (it->first.rfind(prefix, 0) == 0) + { + it = values_.erase(it); + } + else + { + ++it; + } + } + } + + bool Config::has(const std::string& prefix) + { + return std::any_of(values_.begin(), values_.end(), [&prefix](const auto& pair) { + return pair.first.rfind(prefix, 0) == 0; + }); } void Config::printAll() { + std::cerr << "Config values:" << std::endl; for (const auto& [key, value] : values_) { - std::cout << key << " = " << value << std::endl; + std::cerr << " " << key << " = " << value << std::endl; + } + } + + void Config::print(const std::string& prefix) + { + std::cerr << "Config values with prefix '" << prefix << "':" << std::endl; + for (const auto& [key, value] : values_) + { + if (key.rfind(prefix, 0) == 0) + { + std::cerr << " " << key << " = " << value << std::endl; + } } } @@ -60,20 +94,17 @@ namespace Modelec } } - // Store attributes for (auto* attr = element->FirstAttribute(); attr; attr = attr->Next()) { values_[key + "@" + attr->Name()] = attr->Value(); } - // Count children by name std::unordered_map child_count; for (auto* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) { child_count[child->Name()]++; } - // Index children std::unordered_map child_index; for (auto* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) diff --git a/src/modelec_utils/test/config.test.cpp b/src/modelec_utils/test/config.test.cpp index 608eea3..b366295 100644 --- a/src/modelec_utils/test/config.test.cpp +++ b/src/modelec_utils/test/config.test.cpp @@ -34,6 +34,9 @@ TEST(ConfigTest, LoadValidXML) // Deep nested node EXPECT_EQ(Modelec::Config::get("root.nested.deep", ""), "hello"); + + EXPECT_TRUE(Modelec::Config::has("root.child1")); + EXPECT_FALSE(Modelec::Config::has("root.child11T1435TRERGFSRVSPKRMGHJNIUSRJOGFH")); } TEST(ConfigTest, LoadInvalidXML) @@ -113,4 +116,33 @@ TEST(ConfigTest, GetArray) EXPECT_EQ(array2[1].attr, "c"); EXPECT_EQ(array2[2].value, 3); EXPECT_EQ(array2[2].attr, "d"); +} + +TEST(ConfigTest, Clear) +{ + // Create temporary XML file + const std::string filepath = "test_array_config.xml"; + + std::ofstream file(filepath); + file << + "" + " 1" + " 2" + " 3" + " 3" + ""; + file.close(); + + EXPECT_TRUE(Modelec::Config::load(filepath)); + + EXPECT_TRUE(Modelec::Config::has("root.test")); + Modelec::Config::clear("root.test"); + EXPECT_FALSE(Modelec::Config::has("root.test")); + + EXPECT_EQ(Modelec::Config::count("root.item"), 3); + Modelec::Config::clear("root.item"); + EXPECT_EQ(Modelec::Config::count("root.item"), 0); + + Modelec::Config::clear(""); + EXPECT_FALSE(Modelec::Config::has("root")); } \ No newline at end of file diff --git a/start_cam_ros2.sh b/start_cam_ros2.sh new file mode 100755 index 0000000..a343aa9 --- /dev/null +++ b/start_cam_ros2.sh @@ -0,0 +1,12 @@ +#!/bin/bash +set -e + +source /opt/ros/jazzy/setup.bash +source /home/modelec/Modelec-ROS2/install/setup.bash + +export RCL_LOG_LEVEL=info +#export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +#export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml +#export ROS_DOMAIN_ID=128 + +exec ros2 run modelec_com color_detector diff --git a/start_ros2.sh b/start_ros2.sh index 5305644..02c2ca2 100755 --- a/start_ros2.sh +++ b/start_ros2.sh @@ -6,7 +6,7 @@ source /home/modelec/Modelec-ROS2/install/setup.bash export RCL_LOG_LEVEL=info #export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml +#export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml #export ROS_DOMAIN_ID=128 -exec ros2 launch modelec_core modelec.launch.py with_color_detector:=false "$@" +exec ros2 launch modelec_core modelec.launch.py "$@" diff --git a/start_ros2.zsh b/start_ros2.zsh new file mode 100755 index 0000000..4b6b599 --- /dev/null +++ b/start_ros2.zsh @@ -0,0 +1,9 @@ +#!/bin/bash +set -e + +source /opt/ros/jazzy/setup.zsh +source ~/Modelec-ROS2/install/setup.zsh + +export RCL_LOG_LEVEL=info + +exec ros2 launch modelec_core modelec.launch.py "$@"