diff --git a/.gitignore b/.gitignore index aabb6791..468f66c7 100644 --- a/.gitignore +++ b/.gitignore @@ -106,4 +106,5 @@ AMENT_IGNORE .vscode .cache -.idea \ No newline at end of file +.idea +cam \ No newline at end of file diff --git a/joy.ros2_launch_marcel.desktop b/Desktop/joy.ros2_launch_marcel.desktop similarity index 100% rename from joy.ros2_launch_marcel.desktop rename to Desktop/joy.ros2_launch_marcel.desktop diff --git a/Desktop/no-gui.ros2_launch_marcel.desktop b/Desktop/no-gui.ros2_launch_marcel.desktop new file mode 100644 index 00000000..23aa910c --- /dev/null +++ b/Desktop/no-gui.ros2_launch_marcel.desktop @@ -0,0 +1,8 @@ +[Desktop Entry] +Type=Application +Name=No GUI +Comment=Lance le système ROS 2 Sans GUI +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_gui:=false +Icon=utilities-terminal +Terminal=true +Categories=Utility; diff --git a/no_lidar.joy.ros2_launch_marcel.desktop b/Desktop/no_lidar.joy.ros2_launch_marcel.desktop similarity index 88% rename from no_lidar.joy.ros2_launch_marcel.desktop rename to Desktop/no_lidar.joy.ros2_launch_marcel.desktop index e323a0db..34ba8775 100644 --- a/no_lidar.joy.ros2_launch_marcel.desktop +++ b/Desktop/no_lidar.joy.ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=Joy no Lidar Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_enemy_manager:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/no_lidar.ros2_launch_marcel.desktop b/Desktop/no_lidar.ros2_launch_marcel.desktop similarity index 82% rename from no_lidar.ros2_launch_marcel.desktop rename to Desktop/no_lidar.ros2_launch_marcel.desktop index ff3a9d26..2847fafc 100644 --- a/no_lidar.ros2_launch_marcel.desktop +++ b/Desktop/no_lidar.ros2_launch_marcel.desktop @@ -2,9 +2,7 @@ Type=Application Name=No Lidar Comment=Lance le système ROS 2 avec GUI - -Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false - +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false with_enemy_manager:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/ros2_launch_marcel.desktop b/Desktop/ros2_launch_marcel.desktop similarity index 100% rename from ros2_launch_marcel.desktop rename to Desktop/ros2_launch_marcel.desktop diff --git a/TODO.md b/TODO.md deleted file mode 100644 index a5d8687d..00000000 --- a/TODO.md +++ /dev/null @@ -1,4 +0,0 @@ -- API (check for the middleware already implemented or fill the API package) - - -- Check every value and after that update the max TIME mission \ No newline at end of file diff --git a/build.rasp.sh b/build.rasp.sh index 517e5500..92cfa84a 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_EXPORT_COMPILE_COMMANDS=ON # -DCMAKE_BUILD_TYPE=Debug +MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential # --cmake-args -DCMAKE_BUILD_TYPE=Debug source install/setup.bash diff --git a/build.sh b/build.sh index 947ccfff..54fc2eb6 100755 --- a/build.sh +++ b/build.sh @@ -1,4 +1,4 @@ #!/bin/bash -colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON +colcon build --symlink-install --cmake-args # -DCMAKE_EXPORT_COMPILE_COMMANDS=ON source install/setup.bash diff --git a/fastdds_setup.xml b/fastdds_setup.xml index bc22a6e0..42dde2c7 100644 --- a/fastdds_setup.xml +++ b/fastdds_setup.xml @@ -28,7 +28,7 @@ -
100.73.69.106
+
100.91.204.77
 diff --git a/install.sh b/install.sh index 3e7cbef7..7cffb6b8 100755 --- a/install.sh +++ b/install.sh @@ -27,9 +27,9 @@ git submodule init git submodule update echo "source /opt/ros/jazzy/setup.bash -source ~/Modelec-ROS/install/setup.bash +source ~/Modelec-ROS2/install/setup.bash export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS/fastdds_setup.xml +export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS2/fastdds_setup.xml export ROS_DOMAIN_ID=128" >> ~/.bashrc source ~/.bashrc @@ -39,9 +39,10 @@ source src/rplidar_ros/scripts/create_udev_rules.sh cd ../.. -cp ./*.desktop ~/Desktop +cp ./Desktop/*.desktop ~/Desktop chmod +x ~/Desktop/*.desktop gio set ~/Desktop/no_lidar.joy.ros2_launch_marcel.desktop "metadata::trusted" true 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 \ No newline at end of file +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 diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index 6aefd4ef..2be80eae 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -125,6 +125,9 @@ def handle_message(self, msg): elif msg.startswith("SET;START;"): self.start = msg.split(';')[2] == '1' + if not self.start: + self.waypoints.clear() + self.waypoint_queue.clear() self.ser.write(b"OK;START;1\n") # --- MULTI WAYPOINT (clears previous) --- diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index 5e16b00c..329f175a 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -10,10 +10,13 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(ament_index_cpp REQUIRED) find_package(fmt) +find_package(OpenCV REQUIRED) + find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) @@ -35,10 +38,35 @@ target_include_directories(pcb_action_interface PUBLIC $ ) +# Color Detector Node +add_executable(color_detector + src/color_detector.cpp +) + +ament_target_dependencies(color_detector + rclcpp + std_msgs + std_srvs + modelec_interfaces + ament_index_cpp +) + +target_link_libraries(color_detector + ${OpenCV_LIBS} + modelec_utils::utils + modelec_utils::config +) + +target_include_directories(color_detector PUBLIC + $ + $ +) + # Install targets install(TARGETS pcb_odo_interface pcb_action_interface + color_detector DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/modelec_com/include/modelec_com/color_detector.hpp b/src/modelec_com/include/modelec_com/color_detector.hpp new file mode 100644 index 00000000..8c8c8fcc --- /dev/null +++ b/src/modelec_com/include/modelec_com/color_detector.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace Modelec +{ + struct ColorSetting + { + std::string name; + double h_min; + double h_max; + }; + + class ColorDetector : public rclcpp::Node + { + public: + ColorDetector(); + + ~ColorDetector() override; + private: + bool processSnapshot(std::vector& colors, std::string& error); + + void onRequest( + const std::shared_ptr request, + std::shared_ptr response); + + std::vector classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const; + + std::string classify(const cv::Vec3d& hsv_roi) const; + + std::string generateImagePath() const; + + rclcpp::Service::SharedPtr service_; + + rclcpp::Subscription::SharedPtr ask_sub_; + rclcpp::Publisher::SharedPtr color_pub_; + + std::vector rois_; + + std::string link_; + bool save_to_file_ = true; + std::string save_directory_ = "./"; + bool enable_ = false; + bool headless_ = true; + + std::vector color_configs_; + }; +} diff --git a/src/modelec_com/include/modelec_com/serial_listener.hpp b/src/modelec_com/include/modelec_com/serial_listener.hpp index 5078f526..530e27b8 100644 --- a/src/modelec_com/include/modelec_com/serial_listener.hpp +++ b/src/modelec_com/include/modelec_com/serial_listener.hpp @@ -30,7 +30,6 @@ namespace Modelec void start_async_write(); public: - std::string name_; boost::asio::serial_port port_; rclcpp::Publisher::SharedPtr publisher_; @@ -38,13 +37,13 @@ namespace Modelec SerialListener(); - SerialListener(const std::string& name, int bauds, const std::string& serial_port, + SerialListener(int bauds, const std::string& serial_port, int max_message_len); virtual ~SerialListener(); void close(); - void open(const std::string& name, int bauds, const std::string& serial_port, + void open(int bauds, const std::string& serial_port, int max_message_len); void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; } diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp new file mode 100644 index 00000000..80615d80 --- /dev/null +++ b/src/modelec_com/src/color_detector.cpp @@ -0,0 +1,238 @@ +#include +#include +#include + +namespace Modelec +{ + ColorDetector::ColorDetector() + : 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); + + 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(); + + 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]) + }); + } + + if (!enable_) + { + RCLCPP_INFO(get_logger(), "Camera disabled by config"); + rclcpp::shutdown(); + return; + } + + service_ = create_service( + "action/detect_color", + std::bind(&ColorDetector::onRequest, this, + std::placeholders::_1, + std::placeholders::_2)); + + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.reliable(); + + ask_sub_ = create_subscription( + "action/detect_color/ask", qos, + [this](const std_msgs::msg::Empty::SharedPtr) + { + std_msgs::msg::String res; + std::vector colors; + std::string error; + + if (!processSnapshot(colors, error)) + { + res.data = "0|" + error; + } + else + { + res.data = "1|" + join(colors, ";"); + } + + color_pub_->publish(res); + }); + + color_pub_ = create_publisher("action/detect_color/res", qos); + + if (!headless_) + { + cv::namedWindow("color_detector", cv::WINDOW_NORMAL); + } + + RCLCPP_INFO(get_logger(), "Color detector service ready"); + } + + ColorDetector::~ColorDetector() + { + if (!headless_) + { + cv::destroyAllWindows(); + } + } + + 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; + + if (frame.empty()) + { + RCLCPP_WARN(get_logger(), "Captured empty frame from camera"); + error = "Empty frame"; + return false; + } + + cv::Mat hsv; + cv::GaussianBlur(frame, frame, cv::Size(5, 5), 0); + cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); + + colors = classifyROIs(hsv, frame); + + if (!headless_) + { + cv::imshow("color_detector", frame); + cv::waitKey(1); + } + + if (save_to_file_) + { + std::string path = save_directory_ + generateImagePath(); + cv::imwrite(path, frame); + RCLCPP_DEBUG(get_logger(), "Saved snapshot to %s", path.c_str()); + } + + cap.release(); + + return true; + } + + void ColorDetector::onRequest( + const std::shared_ptr, + std::shared_ptr response) + { + std::vector colors; + std::string error; + + if (!processSnapshot(colors, error)) + { + response->success = false; + response->message = error; + return; + } + + response->success = true; + response->message = join(colors, ";"); + } + + std::vector ColorDetector::classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const + { + std::vector results; + + for (auto r : rois_) + { + cv::Rect roi = r & cv::Rect(0, 0, hsv.cols, hsv.rows); + cv::Scalar mean = cv::mean(hsv(roi)); + + std::string color = classify(cv::Vec3d(mean[0], mean[1], mean[2])); + + RCLCPP_DEBUG(get_logger(), "ROI at (%d, %d, %d, %d) has mean HSV (%.2f, %.2f, %.2f) classified as %s", + roi.x, roi.y, roi.width, roi.height, + mean[0], mean[1], mean[2], + color.c_str()); + + results.push_back(color); + + if (save_to_file_) + { + cv::rectangle(debug_img, roi, {0, 255, 0}, 1); + cv::putText( + debug_img, + color, + roi.tl() + cv::Point(0, -5), + cv::FONT_HERSHEY_SIMPLEX, + 0.4, + {0, 255, 0}, + 1); + } + } + + return results; + } + + std::string ColorDetector::classify(const cv::Vec3d& hsv_roi) const + { + double h = hsv_roi[0]; + + for (const auto& color_config : color_configs_) + { + if (h >= color_config.h_min && h <= color_config.h_max) + { + return color_config.name; + } + } + + return "unknown"; + } + + std::string ColorDetector::generateImagePath() const + { + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + std::stringstream ss; + ss << "snapshot_" + << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S") + << ".png"; + return ss.str(); + } +} + + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 0c3ce6d6..7f6b17e9 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -1,6 +1,4 @@ #include -#include -#include #include #include @@ -11,12 +9,11 @@ namespace Modelec // Service to create a new serial listener declare_parameter("serial_port", "/dev/USB_ACTION"); declare_parameter("baudrate", 115200); - declare_parameter("name", "pcb_action"); - auto request = std::make_shared(); - request->name = get_parameter("name").as_string(); - request->bauds = get_parameter("baudrate").as_int(); - request->serial_port = get_parameter("serial_port").as_string(); + auto serial_port = get_parameter("serial_port").as_string(); + auto baudrate = get_parameter("baudrate").as_int(); + + RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate); asc_get_sub_ = this->create_subscription( "action/get/asc", 10, @@ -233,7 +230,7 @@ namespace Modelec }); - this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); + this->open(baudrate, serial_port, MAX_MESSAGE_LEN); // TODO : check for real value there /*asc_value_mapper_ = { @@ -262,15 +259,15 @@ namespace Modelec {5, 0} };*/ - /*servo_value_ = { - {0, 2.95}, - {1, 0.93}, - {2, 0}, - {3, 3}, - {4, 0.8}, - {5, 0.8}, - {6, 0.8}, - {7, 0.8}, + servo_value_ = { + {0, 2.93}, + {1, 0.91}, + {2, 3.05}, + {3, 0.3}, + {4, 1}, + {5, 1}, + {6, 1}, + {7, 1}, }; std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";"; @@ -282,7 +279,7 @@ namespace Modelec data += "\n"; - SendToPCB(data);*/ + SendToPCB(data); /*relay_value_ = { {1, false}, diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 8c79f74e..138a3c63 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -1,7 +1,5 @@ #include #include -#include -#include namespace Modelec { @@ -9,13 +7,11 @@ namespace Modelec { declare_parameter("serial_port", "/dev/USB_ODO"); declare_parameter("baudrate", 115200); - declare_parameter("name", "pcb_odo"); - // Service to create a new serial listener - auto request = std::make_shared(); - request->name = get_parameter("name").as_string(); - request->bauds = get_parameter("baudrate").as_int(); - request->serial_port = get_parameter("serial_port").as_string(); + auto serial_port = get_parameter("serial_port").as_string(); + auto baudrate = get_parameter("baudrate").as_int(); + + RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate); odo_pos_publisher_ = this->create_publisher( "odometry/position", 10); @@ -102,17 +98,14 @@ namespace Modelec "odometry/start", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) { - if (msg->data != start_odo_) - { - start_odo_ = msg->data; - SendOrder("START", {std::to_string(msg->data)}); - } + start_odo_ = msg->data; + SendOrder("START", {std::to_string(msg->data)}); }); - this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); + this->open(baudrate, serial_port, MAX_MESSAGE_LEN); SetPID("THETA", 14, 0, 0); - SetPID("POS", 10, 0, 0); + SetPID("POS", 5, 0, 0); SetPID("LEFT", 5, 0, 0); SetPID("RIGHT", 5, 0, 0); } @@ -179,7 +172,7 @@ namespace Modelec { if (tokens[2] == "REACH") { - // RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); + RCLCPP_DEBUG(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); int id = std::stoi(tokens[3]); diff --git a/src/modelec_com/src/serial_listener.cpp b/src/modelec_com/src/serial_listener.cpp index 29a8c6ea..cb789624 100644 --- a/src/modelec_com/src/serial_listener.cpp +++ b/src/modelec_com/src/serial_listener.cpp @@ -7,10 +7,10 @@ namespace Modelec { } - SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port, + SerialListener::SerialListener(int bauds, const std::string& serial_port, int max_message_len) : io_(), port_(io_) { - open(name, bauds, serial_port, max_message_len); + open(bauds, serial_port, max_message_len); } SerialListener::~SerialListener() @@ -32,9 +32,8 @@ namespace Modelec } } - void SerialListener::open(const std::string& name, int bauds, const std::string& serial_port, + void SerialListener::open(int bauds, const std::string& serial_port, int max_message_len) { - this->name_ = name; this->bauds_ = bauds; this->serial_port_ = serial_port; this->max_message_len_ = max_message_len; diff --git a/src/modelec_core/config/config.yml b/src/modelec_core/config/config.yml new file mode 100644 index 00000000..c4d62953 --- /dev/null +++ b/src/modelec_core/config/config.yml @@ -0,0 +1,134 @@ +enemy_manager: + ros__parameters: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 50 + detection: + min_move_threshold_mm: 50 + refresh_rate: 3 + max_stationary_time_s: 0.5 + min_emergency_distance_mm: 300 + margin_detection_table_mm: 50 + factor_close_enemy: -2 + +pami_manager: + ros__parameters: + time_to_put_zone: 77 + time_to_remove_top_pot: 65 + +modelec_gui: + ros__parameters: + robot: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 100 + map: + size: + grid_width: 600 + grid_height: 400 + map_width_mm: 3000 + map_height_mm: 2000 + enemy: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 50 + +strat_fsm: + ros__parameters: + robot: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 100 + + enemy: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 50 + + map: + size: + grid_width: 600 + grid_height: 400 + map_width_mm: 3000 + map_height_mm: 2000 + + spawn: + yellow_top: + x: 350 + y: 1700 + theta: -1.5708 + blue_top: + x: 2650 + y: 1700 + theta: -1.5708 + width_mm: 450 + height_mm: 450 + + home: + blue: + x: 2650 + y: 1700 + theta: -1.5708 + yellow: + x: 350 + y: 1700 + theta: -1.5708 + + static_strat: false + factor: + theta: 20.0 + obs: 0.6 + thermo: 0.7 + timer_period_ms: 20 + + thermo: + yellow: + start: + x: 200 + y: 175 + theta: 0.0 + finish: + x: 700 + y: 175 + theta: 0.0 + blue: + start: + x: 2800 + y: 175 + theta: 3.14 + finish: + x: 2300 + y: 175 + theta: 3.14 + + mission_score: + go_home: 10 + +color_detector: + ros__parameters: + save_to_file: + enabled: true + path: "./cam/" + enabled: true + # link: "/dev/video0" + link: "http://192.168.1.21:8080/video" + headless: true + rois: [300, 700, 50, 50, 700, 700, 50, 50, 1200, 700, 50, 50, 1500, 700, 50, 50] + colors: + blue: [90, 130] + yellow: [20, 40] + +pcb_odo_interface: + ros__parameters: + serial_port: '/dev/USB_ODO' + baudrate: 115200 + +pcb_action_interface: + ros__parameters: + serial_port: '/dev/USB_ACTION' + baudrate: 115200 diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 62a073db..72c9fb12 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -1,36 +1,70 @@ +import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown, RegisterEventHandler, TimerAction, LogInfo +from launch.actions import ( + DeclareLaunchArgument, + OpaqueFunction, + Shutdown, + RegisterEventHandler, + TimerAction, + LogInfo, +) from launch.event_handlers import OnProcessExit from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node - +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - # Declare launch arguments - with_gui_arg = DeclareLaunchArgument('with_gui', default_value='true', description='Launch GUI?') - with_rplidar_arg = DeclareLaunchArgument('with_rplidar', default_value='true', description='Launch RPLIDAR?') - with_com_arg = DeclareLaunchArgument('with_com', default_value='true', description='Launch COM nodes?') - with_strat_arg = DeclareLaunchArgument('with_strat', default_value='true', description='Launch strategy nodes?') - with_joy_arg = DeclareLaunchArgument('with_joy', default_value='true', description='Launch joystick node?') + pkg_name = 'modelec_core' + # ------------------------------------------------- + # Launch arguments + # ------------------------------------------------- + with_gui_arg = DeclareLaunchArgument( + 'with_gui', default_value='true', description='Launch GUI?' + ) + with_rplidar_arg = DeclareLaunchArgument( + 'with_rplidar', default_value='true', description='Launch RPLIDAR?' + ) + with_com_arg = DeclareLaunchArgument( + 'with_com', default_value='true', description='Launch COM nodes?' + ) + with_strat_arg = DeclareLaunchArgument( + 'with_strat', default_value='true', description='Launch strategy nodes (except enemy_manager)?' + ) + with_enemy_manager_arg = DeclareLaunchArgument( + 'with_enemy_manager', + default_value='true', + description='Launch enemy_manager node?', + ) + with_joy_arg = DeclareLaunchArgument( + 'with_joy', default_value='true', description='Launch joystick node?' + ) + with_color_detector_arg = DeclareLaunchArgument( + 'with_color_detector', default_value='true', description='Launch color detector node?' + ) channel_type = LaunchConfiguration('channel_type', default='serial') serial_port = LaunchConfiguration('serial_port', default='/dev/rplidar') serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') frame_id = LaunchConfiguration('frame_id', default='laser') inverted = LaunchConfiguration('inverted', default='false') - angle_compensate = LaunchConfiguration('angle_compensate', default='true') - scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + 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' + ) - # ---------------------------- - # Infinite LIDAR restart logic - # ---------------------------- + # ------------------------------------------------- + # RPLIDAR with auto-restart + # ------------------------------------------------- def create_lidar_with_restart(): - """Create a lidar node and its auto-restart handler.""" lidar_node = Node( package='rplidar_ros', executable='rplidar_node', name='rplidar_node', - parameters=[{ + parameters=[param_file, { 'channel_type': channel_type, 'serial_port': serial_port, 'serial_baudrate': serial_baudrate, @@ -39,20 +73,22 @@ def create_lidar_with_restart(): 'angle_compensate': angle_compensate, 'scan_mode': scan_mode, }], - output='screen' ) - # Instead of recursion at definition time, we delay the re-creation using a lambda restart_handler = RegisterEventHandler( OnProcessExit( target_action=lidar_node, on_exit=[ LogInfo(msg='[Launch] RPLIDAR crashed — restarting in 2s...'), TimerAction( - period=0.3, - actions=[OpaqueFunction(function=lambda *_: create_lidar_with_restart())] - ) - ] + period=2.0, + actions=[ + OpaqueFunction( + function=lambda *_: create_lidar_with_restart() + ) + ], + ), + ], ) ) @@ -60,37 +96,38 @@ def create_lidar_with_restart(): def launch_rplidar_restart_if_needed(context, *args, **kwargs): if context.launch_configurations.get('with_rplidar') == 'true': - # Start after 3 seconds return [ TimerAction( period=3.0, - actions=create_lidar_with_restart() + actions=create_lidar_with_restart(), ) ] return [] - # ---------------------------- - # GUI node - # ---------------------------- + # ------------------------------------------------- + # GUI + # ------------------------------------------------- def launch_gui(context, *args, **kwargs): if context.launch_configurations.get('with_gui') == 'true': gui = Node( package='modelec_gui', executable='modelec_gui', - name='modelec_gui' + name='modelec_gui', + parameters=[param_file], ) + shutdown = RegisterEventHandler( OnProcessExit( target_action=gui, - on_exit=[Shutdown()] + on_exit=[Shutdown()], ) ) return [gui, shutdown] return [] - # ---------------------------- + # ------------------------------------------------- # COM nodes - # ---------------------------- + # ------------------------------------------------- def launch_com(context, *args, **kwargs): if context.launch_configurations.get('with_com') == 'true': return [ @@ -98,74 +135,108 @@ def launch_com(context, *args, **kwargs): package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface', - parameters=[{ - 'serial_port': "/dev/USB_ODO", - 'baudrate': 115200, - 'name': "pcb_odo", - }] + parameters=[param_file], ), Node( package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface', - parameters=[{ - 'serial_port': "/dev/USB_ACTION", - 'baudrate': 115200, - 'name': "pcb_action", - }] + parameters=[param_file], ), ] return [] - # ---------------------------- - # Strategy nodes - # ---------------------------- + # ------------------------------------------------- + # Strategy nodes (WITHOUT enemy_manager) + # ------------------------------------------------- def launch_strat(context, *args, **kwargs): if context.launch_configurations.get('with_strat') == 'true': return [ - Node(package='modelec_strat', executable='strat_fsm', name='strat_fsm'), - Node(package='modelec_strat', executable='pami_manager', name='pami_manager'), - Node(package='modelec_strat', executable='enemy_manager', name='enemy_manager'), + Node( + 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) + # ------------------------------------------------- + def launch_enemy_manager(context, *args, **kwargs): + if context.launch_configurations.get('with_enemy_manager') == 'true': + return [ + Node( + package='modelec_strat', + executable='enemy_manager', + name='enemy_manager', + parameters=[param_file], + ) ] return [] - # ---------------------------- - # Joy node - # ---------------------------- + # ------------------------------------------------- + # Joystick + # ------------------------------------------------- def launch_joy(context, *args, **kwargs): - if context.launch_configurations.get('with_joy', 'true') == 'true': - joy = Node( - package='joy', - executable='joy_node', - name='joy_node', - output='screen' - ) - return [joy] + if context.launch_configurations.get('with_joy') == 'true': + return [ + Node( + package='joy', + executable='joy_node', + name='joy_node', + parameters=[param_file], + ) + ] + return [] + + def launch_color_detector(context, *args, **kwargs): + if context.launch_configurations.get('with_color_detector') == 'true': + return [ + Node( + package='modelec_com', + executable='color_detector', + name='color_detector', + parameters=[param_file], + ) + ] return [] - # ---------------------------- - # Final launch description - # ---------------------------- + # ------------------------------------------------- + # Final LaunchDescription + # ------------------------------------------------- return LaunchDescription([ - DeclareLaunchArgument('channel_type', default_value=channel_type, description='Specifying channel type of lidar'), - DeclareLaunchArgument('serial_port', default_value=serial_port, description='Specifying usb port to connected lidar'), - DeclareLaunchArgument('serial_baudrate', default_value=serial_baudrate, description='Specifying usb port baudrate to connected lidar'), - DeclareLaunchArgument('frame_id', default_value=frame_id, description='Specifying frame_id of lidar'), - DeclareLaunchArgument('inverted', default_value=inverted, description='Specifying whether or not to invert scan data'), - DeclareLaunchArgument('angle_compensate', default_value=angle_compensate, description='Specifying whether or not to enable angle_compensate of scan data'), - DeclareLaunchArgument('scan_mode', default_value=scan_mode, description='Specifying scan mode of lidar'), + DeclareLaunchArgument('channel_type', default_value=channel_type), + DeclareLaunchArgument('serial_port', default_value=serial_port), + DeclareLaunchArgument('serial_baudrate', default_value=serial_baudrate), + DeclareLaunchArgument('frame_id', default_value=frame_id), + DeclareLaunchArgument('inverted', default_value=inverted), + DeclareLaunchArgument('angle_compensate', default_value=angle_compensate), + DeclareLaunchArgument('scan_mode', default_value=scan_mode), with_gui_arg, with_rplidar_arg, with_com_arg, with_strat_arg, + with_enemy_manager_arg, with_joy_arg, + with_color_detector_arg, OpaqueFunction(function=launch_rplidar_restart_if_needed), OpaqueFunction(function=launch_gui), OpaqueFunction(function=launch_com), OpaqueFunction(function=launch_strat), + OpaqueFunction(function=launch_enemy_manager), OpaqueFunction(function=launch_joy), + OpaqueFunction(function=launch_color_detector), ]) -# to run in debug : , prefix=['xterm -e gdb -ex run --args'] +# prefix = ['xterm -e gdb -ex run --args'] \ No newline at end of file diff --git a/src/modelec_gui/include/modelec_gui/pages/action_page.hpp b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp index 1d8361f0..83e6f6c7 100644 --- a/src/modelec_gui/include/modelec_gui/pages/action_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp @@ -55,12 +55,8 @@ namespace ModelecGUI *deploy_action1_front_servo3_button_, *deploy_action1_front_servo4_button_; - bool deploy_action1_front_servo1_state_, - deploy_action1_front_servo2_state_, - deploy_action1_front_servo3_state_, - deploy_action1_front_servo4_state_; - QPushButton* deploy_action1_front_down_button_; + QPushButton* deploy_action1_front_rotate_button_; // ---- Action1 back ---- QVBoxLayout* deploy_action1_back_layout_; @@ -73,12 +69,8 @@ namespace ModelecGUI *deploy_action1_back_servo3_button_, *deploy_action1_back_servo4_button_; - bool deploy_action1_back_servo1_state_, - deploy_action1_back_servo2_state_, - deploy_action1_back_servo3_state_, - deploy_action1_back_servo4_state_; - QPushButton* deploy_action1_back_down_button_; + QPushButton* deploy_action1_back_rotate_button_; // ---- Action2 ---- QVBoxLayout* deploy_action2_layout_; diff --git a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp index ddc4ce04..79fac1da 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include @@ -40,15 +41,19 @@ namespace ModelecGUI { protected: void paintEvent(QPaintEvent*) override; - void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); - void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg); void resizeEvent(QResizeEvent* event) override; + void updateBackgroundCache(); + + void updateObstaclesCache(); + + void updateWaypointsCache(); + rclcpp::TimerBase::SharedPtr reset_timer_; - QSvgRenderer* renderer; + QSvgRenderer* renderer_; QVBoxLayout* v_layout; QHBoxLayout* h_layout; @@ -97,5 +102,17 @@ namespace ModelecGUI { long int start_time_ = 0; rclcpp::Subscription::SharedPtr strat_state_sub_; + + QPixmap background_cache_; + QPixmap obstacles_cache_; + QPixmap waypoints_cache_; + + bool bg_dirty_ = true; + bool obstacles_dirty_ = true; + bool waypoints_dirty_ = true; + + QPixmap robot_texture_; + QPixmap top_texture_; + QPixmap obs_texture_; }; } diff --git a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp index 6e038058..e0a2c2bd 100644 --- a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp @@ -54,7 +54,13 @@ namespace ModelecGUI void resizeEvent(QResizeEvent* event) override; - QSvgRenderer* renderer; + void updateBackgroundCache(); + + void updateObstaclesCache(); + + void updateWaypointsCache(); + + QSvgRenderer* renderer_; modelec_interfaces::msg::OdometryPos robotPos; std::vector qpoints; @@ -95,5 +101,18 @@ namespace ModelecGUI bool hasEnemy = false; rclcpp::Publisher::SharedPtr enemy_pos_pub_; rclcpp::Subscription::SharedPtr enemy_pos_sub_; + + QPixmap background_cache_; + QPixmap obstacles_cache_; + QPixmap waypoints_cache_; + + bool bg_dirty_ = true; + bool obstacles_dirty_ = true; + bool waypoints_dirty_ = true; + + QPixmap robot_texture_; + QPixmap top_texture_; + QPixmap obs_texture_; + }; } diff --git a/src/modelec_gui/src/main.cpp b/src/modelec_gui/src/main.cpp index 2e45856e..6015fdb4 100644 --- a/src/modelec_gui/src/main.cpp +++ b/src/modelec_gui/src/main.cpp @@ -4,7 +4,6 @@ #include "modelec_gui/modelec_gui.hpp" #include -#include int main(int argc, char **argv) { @@ -12,14 +11,17 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Modelec::Config::load(config_path)) - { - RCLCPP_ERROR(rclcpp::get_logger("modelec_gui"), "Failed to load configuration file: %s", config_path.c_str()); - } - 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); + ModelecGUI::ROS2QtGUI window(node); window.show(); diff --git a/src/modelec_gui/src/pages/action_page.cpp b/src/modelec_gui/src/pages/action_page.cpp index b420e2c8..3c0c88d5 100644 --- a/src/modelec_gui/src/pages/action_page.cpp +++ b/src/modelec_gui/src/pages/action_page.cpp @@ -49,6 +49,7 @@ namespace ModelecGUI deploy_action1_front_up_button_ = new QPushButton("Up"); deploy_action1_front_down_button_ = new QPushButton("Down"); + deploy_action1_front_rotate_button_ = new QPushButton("DownRota"); connect(deploy_action1_front_up_button_, &QPushButton::clicked, this, [this]() @@ -62,7 +63,15 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1"; + action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "0"; + action_exec_pub_->publish(action_exec); + }); + + connect(deploy_action1_front_rotate_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -76,12 +85,9 @@ namespace ModelecGUI connect(deploy_action1_front_servo1_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_front_servo1_state_ = !deploy_action1_front_servo1_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_front_servo1_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -89,12 +95,9 @@ namespace ModelecGUI connect(deploy_action1_front_servo2_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_front_servo2_state_ = !deploy_action1_front_servo2_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_front_servo2_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "2"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -102,12 +105,9 @@ namespace ModelecGUI connect(deploy_action1_front_servo3_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_front_servo3_state_ = !deploy_action1_front_servo3_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_front_servo3_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "3"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "2" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -115,12 +115,9 @@ namespace ModelecGUI connect(deploy_action1_front_servo4_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_front_servo4_state_ = !deploy_action1_front_servo4_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_front_servo4_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "4"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "3" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -134,12 +131,14 @@ namespace ModelecGUI deploy_action1_front_layout_->addWidget(deploy_action1_front_up_button_); deploy_action1_front_layout_->addLayout(deploy_action1_front_servos_layout_); deploy_action1_front_layout_->addWidget(deploy_action1_front_down_button_); + deploy_action1_front_layout_->addWidget(deploy_action1_front_rotate_button_); deploy_action1_back_layout_ = new QVBoxLayout; deploy_action1_back_label_ = new QLabel("Back Action 1"); deploy_action1_back_up_button_ = new QPushButton("Up"); deploy_action1_back_down_button_ = new QPushButton("Down"); + deploy_action1_back_rotate_button_ = new QPushButton("DownRota"); connect(deploy_action1_back_up_button_, &QPushButton::clicked, this, [this]() @@ -153,7 +152,15 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0"; + action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "0"; + action_exec_pub_->publish(action_exec); + }); + + connect(deploy_action1_back_rotate_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -167,12 +174,9 @@ namespace ModelecGUI connect(deploy_action1_back_servo1_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_back_servo1_state_ = !deploy_action1_back_servo1_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_back_servo1_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); @@ -180,12 +184,9 @@ namespace ModelecGUI connect(deploy_action1_back_servo2_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_back_servo2_state_ = !deploy_action1_back_servo2_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_back_servo2_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "2"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); @@ -193,12 +194,9 @@ namespace ModelecGUI connect(deploy_action1_back_servo3_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_back_servo3_state_ = !deploy_action1_back_servo3_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_back_servo3_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "3"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "2" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); @@ -206,12 +204,9 @@ namespace ModelecGUI connect(deploy_action1_back_servo4_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_back_servo4_state_ = !deploy_action1_back_servo4_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_back_servo4_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "4"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "3" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); @@ -225,6 +220,7 @@ namespace ModelecGUI deploy_action1_back_layout_->addWidget(deploy_action1_back_up_button_); deploy_action1_back_layout_->addLayout(deploy_action1_back_servos_layout_); deploy_action1_back_layout_->addWidget(deploy_action1_back_down_button_); + deploy_action1_back_layout_->addWidget(deploy_action1_back_rotate_button_); deploy_action1_layout_->addLayout(deploy_action1_front_layout_); deploy_action1_layout_->addLayout(deploy_action1_back_layout_); @@ -238,7 +234,7 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::THERMO_DEPLOY; + action_exec.action = ActionExec::THERMO + ActionExec::DELIMITER + "-1" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -246,7 +242,7 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::THERMO_UNDEPLOY; + action_exec.action = ActionExec::THERMO + ActionExec::DELIMITER + "-1" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index 5bc00ac6..c9fbfb2e 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -2,7 +2,6 @@ #include #include -#include namespace ModelecGUI { diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 9a2fe1f8..7ff6f4c5 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -3,15 +3,18 @@ #include #include -#include #include namespace ModelecGUI { MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), - renderer(new QSvgRenderer( + renderer_(new QSvgRenderer( QString(":/img/playmat/2026_FINAL.svg"), - this)), node_(node) + this)), + node_(node), + robot_texture_(":/img/logo/modelec.png"), + top_texture_(":/img/logo/ISEN-Nantes.png"), + obs_texture_(":/img/wood.jpg") { ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; @@ -45,11 +48,14 @@ namespace ModelecGUI qpoints = {}; - robot_length_ = Modelec::Config::get("config.robot.size.length_mm", 200); - robot_width_ = Modelec::Config::get("config.robot.size.width_mm", 300); + map_height_ = node_->get_parameter("map.size.map_height_mm").as_int(); + map_width_ = node_->get_parameter("map.size.map_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); + robot_length_ = node_->get_parameter("robot.size.length_mm").as_int(); + robot_width_ = node_->get_parameter("robot.size.width_mm").as_int(); + + enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int(); + enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int(); add_waypoint_sub_ = node_->create_subscription( "odometry/add_waypoint", 100, @@ -71,6 +77,8 @@ namespace ModelecGUI qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_)); + + waypoints_dirty_ = true; update(); }); @@ -90,6 +98,7 @@ namespace ModelecGUI height() - point.y * ratioBetweenMapAndWidgetY_)); } + waypoints_dirty_ = true; update(); }); @@ -113,6 +122,8 @@ namespace ModelecGUI [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { OnObstacleReceived(msg); + obstacles_dirty_ = true; + update(); }); obstacle_removed_sub_ = node_->create_subscription( @@ -120,6 +131,8 @@ namespace ModelecGUI [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { obstacle_.erase(msg->id); + obstacles_dirty_ = true; + update(); }); enemy_pos_sub_ = node_->create_subscription("enemy/position", 10, @@ -185,6 +198,14 @@ namespace ModelecGUI auto result2 = ask_map_obstacle_client_->async_send_request(std::make_shared()); rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2); + + auto timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, [this]() { + if (!isGameStarted_) return; + auto elapsed = (std::chrono::system_clock::now().time_since_epoch().count() - start_time_) / 1e9; + timer_label_->setText(QString("%1 s").arg(elapsed)); + }); + timer->start(1000); } void MapPage::AskMap() @@ -228,143 +249,160 @@ namespace ModelecGUI { QWidget::paintEvent(paint_event); - if (isGameStarted_) + QPainter painter(this); + + if (bg_dirty_) updateBackgroundCache(); + painter.drawPixmap(0, 0, background_cache_); + + if (show_obstacle_) { - auto now = std::chrono::system_clock::now().time_since_epoch(); - auto start = std::chrono::nanoseconds(start_time_); - auto elapsed = now - start; - auto elapsed_s = std::chrono::duration_cast(elapsed).count(); - timer_label_->setText(QString::number(elapsed_s) + " s"); + if (obstacles_dirty_) updateObstaclesCache(); + painter.drawPixmap(0, 0, obstacles_cache_); } - QPainter painter(this); - renderer->render(&painter, rect()); // Scales SVG to widget size + if (waypoints_dirty_) updateWaypointsCache(); + painter.drawPixmap(0, 0, waypoints_cache_); + + // --- robot --- painter.save(); + painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_); + painter.rotate(90 - robotPos.theta * 180.0 / M_PI); - painter.setRenderHint(QPainter::Antialiasing); + QRect r(-(robot_width_*ratioBetweenMapAndWidgetX_)/2, -(robot_length_*ratioBetweenMapAndWidgetY_)/2, + (robot_width_*ratioBetweenMapAndWidgetX_), (robot_length_*ratioBetweenMapAndWidgetY_)); - // --- Draw lines --- - painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide - for (size_t i = 0; i + 1 < qpoints.size(); ++i) + painter.drawPixmap(r, robot_texture_); + painter.restore(); + + // --- Enemy --- + if (hasEnemy) { - painter.drawLine(qpoints[i], qpoints[i + 1]); + painter.setBrush(Qt::red); + painter.drawRect( + (enemy_pos_.x - enemy_width_/2) * ratioBetweenMapAndWidgetX_, + height() - (enemy_pos_.y + enemy_length_/2) * ratioBetweenMapAndWidgetY_, + enemy_width_ * ratioBetweenMapAndWidgetX_, + enemy_length_ * ratioBetweenMapAndWidgetY_); } + } - painter.setPen(Qt::NoPen); - - // --- Draw colored points --- - const int radius = 5; + void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) + { + obstacle_[msg->id] = *msg; + obstacles_dirty_ = true; + update(); + } - for (size_t i = 0; i < qpoints.size(); ++i) - { - if (i == qpoints.size() - 1) - painter.setBrush(Qt::blue); // Last = blue - else - painter.setBrush(Qt::red); // Middle = red + void MapPage::resizeEvent(QResizeEvent* event) + { + QWidget::resizeEvent(event); - painter.drawEllipse(qpoints[i], radius, radius); - } + ratioBetweenMapAndWidgetX_ = width() / 3000.0f; + ratioBetweenMapAndWidgetY_ = height() / 2000.0f; - painter.restore(); + bg_dirty_ = true; + obstacles_dirty_ = true; + waypoints_dirty_ = true; + update(); + } - if (show_obstacle_) - { - for (auto& [index, obs] : obstacle_) - { - painter.save(); + void MapPage::updateBackgroundCache() + { + background_cache_ = QPixmap(size()); + background_cache_.fill(Qt::transparent); - QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_); - painter.translate(obsPoint); - painter.rotate(90 - obs.theta * (180.0 / M_PI)); + QPainter p(&background_cache_); + renderer_->render(&p, rect()); + bg_dirty_ = false; + } - if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN) - { - QPixmap texture(":/img/wood.jpg"); - painter.setBrush(QBrush(texture)); - } - else if (obs.id == 2) - { - QPixmap texture(":/img/logo/ISEN-Nantes.png"); - texture = texture.scaled(obs.width * ratioBetweenMapAndWidgetX_, - obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio); + void MapPage::updateObstaclesCache() + { + obstacles_cache_ = QPixmap(size()); + obstacles_cache_.fill(Qt::transparent); - QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height()); + QPainter painter(&obstacles_cache_); + painter.setRenderHint(QPainter::Antialiasing); - QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), - -(obs.height * ratioBetweenMapAndWidgetY_ / 2), - obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); + for (auto& [index, obs] : obstacle_) + { + painter.save(); - painter.setBrush(Qt::white); - painter.setPen(Qt::NoPen); - painter.drawRect(toDraw); + QPoint pos(obs.x * ratioBetweenMapAndWidgetX_, + height() - obs.y * ratioBetweenMapAndWidgetY_); + painter.translate(pos); + painter.rotate(90 - obs.theta * 180.0 / M_PI); - painter.drawPixmap(imageRect.topLeft(), texture); + if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN) + { + painter.setBrush(QBrush(obs_texture_)); + } + else if (obs.id == 2) + { - painter.restore(); + auto texture = top_texture_.scaled(obs.width * ratioBetweenMapAndWidgetX_, + obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio); - continue; - } - else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE) - { - painter.setBrush(Qt::white); - painter.setPen(Qt::NoPen); - } - else - { - painter.setBrush(Qt::red); - painter.setOpacity(0.5); - painter.setPen(QPen(Qt::red, 5)); - } + QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height()); QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), - -(obs.height * ratioBetweenMapAndWidgetY_ / 2), - obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); + painter.setBrush(Qt::white); + painter.setPen(Qt::NoPen); painter.drawRect(toDraw); + painter.drawPixmap(imageRect.topLeft(), texture); + painter.restore(); - } - // -- Draw enemy position -- - if (hasEnemy) + continue; + } + else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE) + { + painter.setBrush(Qt::white); + painter.setPen(Qt::NoPen); + } + else { painter.setBrush(Qt::red); - painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, - height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, - enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_); + painter.setOpacity(0.5); + painter.setPen(QPen(Qt::red, 5)); } - } - - // -- Draw robot position -- - painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_); - painter.rotate(90 - robotPos.theta * (180.0 / M_PI)); - - QPixmap texture(":/img/logo/modelec.png"); - texture = texture.scaled(robot_width_ * ratioBetweenMapAndWidgetX_ * 0.9, - robot_length_ * ratioBetweenMapAndWidgetY_ * 0.9, Qt::KeepAspectRatio); - QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height()); + QRect r(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, + obs.height * ratioBetweenMapAndWidgetY_); - QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2), - robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_); + painter.drawRect(r); - painter.setBrush(Qt::white); - painter.setPen(QPen(Qt::black, 5)); - painter.drawRect(rect); + painter.restore(); + } - painter.drawPixmap(imageRect.topLeft(), texture); + obstacles_dirty_ = false; } - void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) + void MapPage::updateWaypointsCache() { - obstacle_[msg->id] = *msg; - } + waypoints_cache_ = QPixmap(size()); + waypoints_cache_.fill(Qt::transparent); - void MapPage::resizeEvent(QResizeEvent* event) - { - QWidget::resizeEvent(event); + QPainter painter(&waypoints_cache_); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(QPen(Qt::red, 2)); - ratioBetweenMapAndWidgetX_ = width() / 3000.0f; - ratioBetweenMapAndWidgetY_ = height() / 2000.0f; + for (size_t i = 0; i + 1 < qpoints.size(); ++i) + painter.drawLine(qpoints[i], qpoints[i + 1]); + + painter.setPen(Qt::NoPen); + painter.setBrush(Qt::red); + + for (auto& p : qpoints) + painter.drawEllipse(p, 5, 5); + + waypoints_dirty_ = false; } } diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index 8d09f26a..a31af6ba 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -3,27 +3,33 @@ #include #include -#include #include namespace ModelecGUI { TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), - renderer(new QSvgRenderer( + renderer_(new QSvgRenderer( QString( ":/img/playmat/2026_FINAL.svg"), - this)), node_(node) + this)), + node_(node), + robot_texture_(":/img/logo/modelec.png"), + top_texture_(":/img/logo/ISEN-Nantes.png"), + obs_texture_(":/img/wood.jpg") { ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; qpoints = {}; - robot_length_ = Modelec::Config::get("config.robot.size.length_mm", 200); - robot_width_ = Modelec::Config::get("config.robot.size.width_mm", 300); + map_height_ = node_->get_parameter("map.size.map_height_mm").as_int(); + map_width_ = node_->get_parameter("map.size.map_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); + robot_length_ = node_->get_parameter("robot.size.length_mm").as_int(); + robot_width_ = node_->get_parameter("robot.size.width_mm").as_int(); + + enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int(); + enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int(); add_waypoint_sub_ = node_->create_subscription( "odometry/add_waypoint", 100, @@ -45,6 +51,8 @@ namespace ModelecGUI qpoints.emplace_back(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_); + + waypoints_dirty_ = true; update(); }); @@ -64,6 +72,7 @@ namespace ModelecGUI height() - point.y * ratioBetweenMapAndWidgetY_); } + waypoints_dirty_ = true; update(); }); @@ -85,6 +94,8 @@ namespace ModelecGUI [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { obstacle_.erase(msg->id); + obstacles_dirty_ = true; + update(); }); go_to_pub_ = node_->create_publisher("nav/go_to", 10); @@ -142,13 +153,15 @@ namespace ModelecGUI void TestMapPage::setPlaymatGrid() { - renderer->load(QString(":/img/playmat/grid_v1.svg")); + renderer_->load(QString(":/img/playmat/grid_v1.svg")); + bg_dirty_ = true; update(); } void TestMapPage::setPlaymatMap() { - renderer->load(QString(":/img/playmat/2026_FINAL.svg")); + renderer_->load(QString(":/img/playmat/2026_FINAL.svg")); + bg_dirty_ = true; update(); } @@ -173,73 +186,41 @@ namespace ModelecGUI QWidget::paintEvent(paint_event); QPainter painter(this); - renderer->render(&painter, rect()); // Scales SVG to widget size - painter.save(); - painter.setRenderHint(QPainter::Antialiasing); + if (bg_dirty_) updateBackgroundCache(); + painter.drawPixmap(0, 0, background_cache_); - // --- Draw lines --- - painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide - for (size_t i = 0; i + 1 < qpoints.size(); ++i) + if (show_obstacle_) { - painter.drawLine(qpoints[i], qpoints[i + 1]); + if (obstacles_dirty_) updateObstaclesCache(); + painter.drawPixmap(0, 0, obstacles_cache_); } - painter.setPen(Qt::NoPen); + if (waypoints_dirty_) updateWaypointsCache(); + painter.drawPixmap(0, 0, waypoints_cache_); - // --- Draw colored points --- - const int radius = 5; - - for (size_t i = 0; i < qpoints.size(); ++i) - { - if (i == qpoints.size() - 1) - painter.setBrush(Qt::blue); // Last = blue - else - painter.setBrush(Qt::red); // Middle = red + // --- robot --- + painter.save(); + painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_); + painter.rotate(90 - robotPos.theta * 180.0 / M_PI); - painter.drawEllipse(qpoints[i], radius, radius); - } + QRect r(-(robot_width_*ratioBetweenMapAndWidgetX_)/2, -(robot_length_*ratioBetweenMapAndWidgetY_)/2, + (robot_width_*ratioBetweenMapAndWidgetX_), (robot_length_*ratioBetweenMapAndWidgetY_)); + painter.drawPixmap(r, robot_texture_); painter.restore(); - if (show_obstacle_) + // --- Enemy --- + if (hasEnemy) { - for (auto& [index, obs] : obstacle_) - { - painter.save(); - - QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_); - painter.translate(obsPoint); - painter.rotate(90 - obs.theta * (180.0 / M_PI)); - painter.setBrush(Qt::black); - - QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), - -(obs.height * ratioBetweenMapAndWidgetY_ / 2), - obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); - - painter.drawRect(toDraw); - - painter.restore(); - } - - // -- Draw enemy position -- - if (hasEnemy) - { - painter.setBrush(Qt::red); - painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, - height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, - enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_); - } + painter.setBrush(Qt::red); + painter.drawRect( + (enemy_pos_.x - enemy_width_/2) * ratioBetweenMapAndWidgetX_, + height() - (enemy_pos_.y + enemy_length_/2) * ratioBetweenMapAndWidgetY_, + enemy_width_ * ratioBetweenMapAndWidgetX_, + enemy_length_ * ratioBetweenMapAndWidgetY_); } - - // -- Draw robot position -- - painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_); - painter.rotate(90 - robotPos.theta * (180.0 / M_PI)); - - QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2), - robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_); - painter.setBrush(Qt::green); - painter.drawRect(rect); } void TestMapPage::mousePressEvent(QMouseEvent* event) @@ -279,6 +260,8 @@ namespace ModelecGUI void TestMapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) { obstacle_.emplace(msg->id, *msg); + obstacles_dirty_ = true; + update(); } void TestMapPage::resizeEvent(QResizeEvent* event) @@ -287,5 +270,109 @@ namespace ModelecGUI ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; + + bg_dirty_ = true; + obstacles_dirty_ = true; + waypoints_dirty_ = true; + update(); + } + + void TestMapPage::updateBackgroundCache() + { + background_cache_ = QPixmap(size()); + background_cache_.fill(Qt::transparent); + + QPainter p(&background_cache_); + renderer_->render(&p, rect()); + bg_dirty_ = false; + } + + void TestMapPage::updateObstaclesCache() + { + obstacles_cache_ = QPixmap(size()); + obstacles_cache_.fill(Qt::transparent); + + QPainter painter(&obstacles_cache_); + painter.setRenderHint(QPainter::Antialiasing); + + for (auto& [index, obs] : obstacle_) + { + painter.save(); + + QPoint pos(obs.x * ratioBetweenMapAndWidgetX_, + height() - obs.y * ratioBetweenMapAndWidgetY_); + painter.translate(pos); + painter.rotate(90 - obs.theta * 180.0 / M_PI); + + if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN) + { + painter.setBrush(QBrush(obs_texture_)); + } + else if (obs.id == 2) + { + + auto texture = top_texture_.scaled(obs.width * ratioBetweenMapAndWidgetX_, + obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio); + + QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height()); + + QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); + + painter.setBrush(Qt::white); + painter.setPen(Qt::NoPen); + painter.drawRect(toDraw); + + painter.drawPixmap(imageRect.topLeft(), texture); + + painter.restore(); + + continue; + } + else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE) + { + painter.setBrush(Qt::white); + painter.setPen(Qt::NoPen); + } + else + { + painter.setBrush(Qt::red); + painter.setOpacity(0.5); + painter.setPen(QPen(Qt::red, 5)); + } + + QRect r(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, + obs.height * ratioBetweenMapAndWidgetY_); + + painter.drawRect(r); + + painter.restore(); + } + + obstacles_dirty_ = false; + } + + void TestMapPage::updateWaypointsCache() + { + waypoints_cache_ = QPixmap(size()); + waypoints_cache_.fill(Qt::transparent); + + QPainter painter(&waypoints_cache_); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(QPen(Qt::red, 2)); + + for (size_t i = 0; i + 1 < qpoints.size(); ++i) + painter.drawLine(qpoints[i], qpoints[i + 1]); + + painter.setPen(Qt::NoPen); + painter.setBrush(Qt::red); + + for (auto& p : qpoints) + painter.drawEllipse(p, 5, 5); + + waypoints_dirty_ = false; } } diff --git a/src/modelec_interfaces/msg/Action/ActionExec.msg b/src/modelec_interfaces/msg/Action/ActionExec.msg index 24367e6e..c9f91f2f 100644 --- a/src/modelec_interfaces/msg/Action/ActionExec.msg +++ b/src/modelec_interfaces/msg/Action/ActionExec.msg @@ -3,10 +3,13 @@ string DELIMITER = ";" # Action string UP = "UP" string DOWN = "DOWN" +string TOGGLE_ARM = "TOGGLE_ARM" +string ROTATE_ARM = "ROTATE_ARM" string TAKE = "TAKE" string FREE = "FREE" -string THERMO_DEPLOY = "THERMO_DEPLOY" -string THERMO_UNDEPLOY = "THERMO_UNDEPLOY" +string TOGGLE_SERVO = "TOGGLE_SERVO" +string THERMO = "THERMO" +string LOOK_ON = "LOOK_ON" string MAX_DEPLOY = "MAX_DEPLOY" # Step @@ -15,8 +18,11 @@ int32 UP_STEP = 1 int32 DOWN_STEP = 2 int32 TAKE_STEP = 3 int32 FREE_STEP = 4 -int32 THERMO_DEPLOY_STEP = 5 -int32 THERMO_UNDEPLOY_STEP = 6 +int32 THERMO_STEP = 5 +int32 TOGGLE_SERVO_STEP = 7 +int32 TOGGLE_ARM_STEP = 8 +int32 ROTATE_ARM_STEP = 9 +int32 LOOK_ON_STEP = 10 int32 MAX_DEPLOY_STEP = 99 int32[] steps diff --git a/src/modelec_interfaces/msg/Strat/StratState.msg b/src/modelec_interfaces/msg/Strat/StratState.msg index 89c45a02..0d4c8aec 100644 --- a/src/modelec_interfaces/msg/Strat/StratState.msg +++ b/src/modelec_interfaces/msg/Strat/StratState.msg @@ -5,6 +5,7 @@ int32 SELECT_GAME_ACTION=3 int32 TAKE_MISSION=10 int32 FREE_MISSION=11 +int32 THERMO_MISSION=12 int32 DO_GO_HOME=20 int32 STOP=21 diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index b51d79a4..7db4dee0 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -25,10 +25,14 @@ set(strat_fsm_sources src/action/down_action.cpp 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/obstacle/obstacle.cpp src/obstacle/box.cpp diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 307e602a..738bc916 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -12,7 +12,7 @@ 300 50 - -0.3 + -2 @@ -36,8 +36,8 @@ - - + + @@ -49,27 +49,47 @@ 450 450 - - 300 - 5 - - 20 - 10 - - 4 - 8 - 16 - - - 5 - 5 - 10 - 77 65 - true + false + + 20 + 0.6 + 0.7 + + 20 + + + + + + + + + + + + + true + ./cam/ + + true + + http://192.168.1.21:8080/video + true + + + + + + + + + + + \ No newline at end of file diff --git a/src/modelec_strat/data/obstacles.xml b/src/modelec_strat/data/obstacles.xml index 151de310..d4ccf6db 100644 --- a/src/modelec_strat/data/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -1,8 +1,6 @@ - - - + 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 cb4b1032..944f34ad 100644 --- a/src/modelec_strat/include/modelec_strat/action/base_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/base_action.hpp @@ -25,11 +25,14 @@ namespace Modelec class BaseAction { public: - enum Front + enum Side { FRONT = 1, BACK = 0, BOTH = -1, + LEFT = 2, + RIGHT = 3, + CENTER = 4, }; using Ptr = std::shared_ptr; @@ -55,6 +58,8 @@ namespace Modelec const std::string& action_name, const std::shared_ptr& action_executor); + virtual void End() = 0; + protected: std::shared_ptr action_executor_; 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 814e01ec..0884b7aa 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -9,16 +9,21 @@ namespace Modelec { public: DownAction(const std::shared_ptr& action_executor); - DownAction(const std::shared_ptr& action_executor, Front front); + DownAction(const std::shared_ptr& action_executor, Side side, bool inverted = false); void Next() override; void Init(const std::vector& params) override; - void SetFront(Front front); + void SetSide(Side side); + void SetInverted(bool inverted); + + void End() override; inline static const std::string Name = ActionExec::DOWN; private: - Front front_; + Side side_; + + bool inverted_; std::queue steps_; 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 3a4532d1..fcfb79ee 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -9,20 +9,22 @@ namespace Modelec { public: FreeAction(const std::shared_ptr& action_executor); - FreeAction(const std::shared_ptr& action_executor, Front front, int n); - FreeAction(const std::shared_ptr& action_executor, std::pair servo); - FreeAction(const std::shared_ptr& action_executor, std::vector> servos); + FreeAction(const std::shared_ptr& action_executor, Side side, int n); + FreeAction(const std::shared_ptr& action_executor, std::pair servo); + FreeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, Front front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Side side); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); + + void End() override; inline static const std::string Name = ActionExec::FREE; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; 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 new file mode 100644 index 00000000..0534cbf9 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/action/look_on_action.hpp @@ -0,0 +1,38 @@ +#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 68cd7a66..0ad3de6a 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -9,20 +9,22 @@ namespace Modelec { public: TakeAction(const std::shared_ptr& action_executor); - TakeAction(const std::shared_ptr& action_executor, Front front, int n); - TakeAction(const std::shared_ptr& action_executor, std::pair servo); - TakeAction(const std::shared_ptr& action_executor, std::vector> servos); + TakeAction(const std::shared_ptr& action_executor, Side side, int n); + TakeAction(const std::shared_ptr& action_executor, std::pair servo); + TakeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, Front front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Side side); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); + + void End() override; inline static const std::string Name = ActionExec::TAKE; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp b/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp new file mode 100644 index 00000000..9cf1ac63 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include + +namespace Modelec +{ + class ThermoAction : public BaseAction + { + public: + ThermoAction(const std::shared_ptr& action_executor); + ThermoAction(const std::shared_ptr& action_executor, Side side, bool deploy); + + void Next() override; + void Init(const std::vector& params) override; + void SetSide(Side side); + void SetDeploy(bool deploy); + + void End() override; + + inline static const std::string Name = ActionExec::THERMO; + + private: + Side side_ = BOTH; + bool deploy_ = true; + + 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/toggle_servo_action.hpp b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp new file mode 100644 index 00000000..76c84840 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include + +namespace Modelec +{ + class ToggleServoAction : public BaseAction + { + public: + ToggleServoAction(const std::shared_ptr& action_executor); + ToggleServoAction(const std::shared_ptr& action_executor, Side side, int n); + ToggleServoAction(const std::shared_ptr& action_executor, std::pair servo); + ToggleServoAction(const std::shared_ptr& action_executor, std::vector> servos); + + void Next() override; + void Init(const std::vector& params) override; + void AddServo(int id, Side side); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); + + void End() override; + + inline static const std::string Name = ActionExec::TOGGLE_SERVO; + + private: + std::vector> servos_; + + 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/up_action.hpp b/src/modelec_strat/include/modelec_strat/action/up_action.hpp index 95b8ea8f..46b980d0 100644 --- a/src/modelec_strat/include/modelec_strat/action/up_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/up_action.hpp @@ -9,16 +9,18 @@ namespace Modelec { public: UPAction(const std::shared_ptr& action_executor); - UPAction(const std::shared_ptr& action_executor, Front front); + UPAction(const std::shared_ptr& action_executor, Side side); void Next() override; void Init(const std::vector& params) override; - void SetFront(Front front); + void SetSide(Side side); + + void End() override; inline static const std::string Name = ActionExec::UP; private: - Front front_; + Side side_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 6186b018..becc6fcc 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -8,8 +8,12 @@ #include #include +#include +#include +#include #include "action/base_action.hpp" +#include "missions/thermo_mission.hpp" #include "obstacle/box.hpp" namespace Modelec @@ -31,25 +35,66 @@ namespace Modelec void ReInit(); - void Down(BaseAction::Front front, bool force = false); + void Down(BaseAction::Side side, bool force = false, bool inverted = false); - void Up(BaseAction::Front front, bool force = false); + void Up(BaseAction::Side side, bool force = false); - void Take(const std::vector>& servos, bool force = false); + void ToggleArm(BaseAction::Side side, bool force = false); - void Free(const std::vector>& servos, bool force = false); + void RotateArm(BaseAction::Side side, bool force = false, bool rotated = false); + + void Take(const std::vector>& servos); + + void Free(const std::vector>& servos); + + void ToggleServo(const std::vector>& servos); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg); + 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_; + std::array servo_pos_; + + struct ArmState + { + bool down; + bool rotated; + }; + std::array arm_pos_; + + std::array servo_value_ = { + 2.91, + 0.95, + 3.2, + 0, + 1, + 1, + 1, + 1, + }; + + std::map thermo_state_ = { + {BaseAction::LEFT, false}, + {BaseAction::RIGHT, false}, + }; + + BaseAction::Side cam_side_ = BaseAction::Side::CENTER; + + bool looking_on_front_ = true; + bool IsEmpty() const; bool IsFull() const; - bool HasBox(BaseAction::Front front) const; + bool HasBox(BaseAction::Side side) const; bool HasFrontBox() const; @@ -57,6 +102,10 @@ namespace Modelec bool HasOneBox() const; + void SendPoint(int point) const; + + void AskColor() const; + protected: rclcpp::Publisher::SharedPtr asc_move_pub_; rclcpp::Publisher::SharedPtr servo_move_pub_; @@ -68,10 +117,17 @@ namespace Modelec rclcpp::Subscription::SharedPtr relay_move_res_sub_; rclcpp::Subscription::SharedPtr servo_timed_move_res_sub_; + rclcpp::Client::SharedPtr ask_color_client_; + rclcpp::Subscription::SharedPtr action_exec_sub_; rclcpp::Subscription::SharedPtr joy_sub_; - std::shared_ptr action_; + rclcpp::Publisher::SharedPtr score_pub_; + + rclcpp::Publisher::SharedPtr ask_pub_; + rclcpp::Subscription::SharedPtr color_sub_; + + std::queue> action_; bool action_done_ = true; int step_running_ = 0; @@ -84,6 +140,9 @@ namespace Modelec int current_step_; + float last_left_trig = 1.0f; + float last_right_trig = 1.0f; + private: rclcpp::Node::SharedPtr node_; }; 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 0bdd862c..783eb3a7 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -9,7 +9,7 @@ namespace Modelec { public: FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, - BaseAction::Front front = BaseAction::FRONT); + BaseAction::Side side = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -21,14 +21,17 @@ namespace Modelec { enum Step { GO_TO_FREE, + CHECK_BOX, DOWN, - FREE, + FREE_FIRST, + ROTATE_ARM, + FREE_OTHER, UP, GO_BACK, DONE, }; - BaseAction::Front front_; + BaseAction::Side side_; MissionStatus status_; std::shared_ptr nav_; 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 eba12063..494cc6d8 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -9,7 +9,7 @@ namespace Modelec { public: TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, - BaseAction::Front front = BaseAction::FRONT); + BaseAction::Side side = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -28,7 +28,7 @@ namespace Modelec { DONE, }; - BaseAction::Front front_; + BaseAction::Side side_; std::shared_ptr closestBox; MissionStatus status_; diff --git a/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp new file mode 100644 index 00000000..b2d90eaa --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include + +namespace Modelec { + class ThermoMission : public Mission { + public: + ThermoMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor); + + void Start(rclcpp::Node::SharedPtr node) override; + void Update() override; + void Clear() override; + MissionStatus GetStatus() const override; + std::string GetName() const override { return "Thermo"; } + + static bool IsThermoDone; + + private: + enum Step + { + GO_TO_THERMO, + GO_TO_THERMO_CLOSE, + ACTIVATE_THERMO, + GO_TO_10, + DEACTIVATE_THERMO, + DONE, + }; + + 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 e7e01018..8ca9a912 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -19,7 +19,7 @@ namespace Modelec class NavigationHelper { public: - enum + enum Team { YELLOW = 0, BLUE = 1, @@ -35,7 +35,7 @@ namespace Modelec std::shared_ptr GetPathfinding() const; - int GetTeamId() const; + Team GetTeamId() const; void Update(); @@ -95,6 +95,7 @@ namespace Modelec std::shared_ptr GetClosestObstacle(const PosMsg::SharedPtr& pos) const; PosMsg::SharedPtr GetHomePosition(); + std::array GetThermoPositions(); void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); @@ -107,7 +108,7 @@ namespace Modelec bool Replan(bool force = false); - void SetTeamId(int id); + void SetTeamId(Team id); void SetSpawn(const std::string& name); @@ -134,12 +135,13 @@ namespace Modelec std::shared_ptr pathfinding_; - int team_id_ = YELLOW; + Team team_id_ = YELLOW; std::map spawn_yellow_; std::map spawn_blue_; Point spawn_; float factor_close_enemy_ = 0; + float factor_theta_ = 0; int enemy_emergency_distance_ = 0; @@ -184,7 +186,7 @@ namespace Modelec std::shared_ptr closest_obstacle = nullptr; auto robotPos = Point(pos->x, pos->y, pos->theta); auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta); - float distance = std::numeric_limits::max(); + double score = std::numeric_limits::max(); for (const auto& obstacle : GetPathfinding()->GetObstacles()) { @@ -192,17 +194,15 @@ namespace Modelec { if (!obs->IsAtObjective()) { - auto dist = Point::distance(robotPos, obs->GetPosition()); + auto obsPoint = obs->GetPosition(); + double distance = Point::distance(robotPos, obsPoint); + double enemy_distance = Point::distance(enemyPos, obsPoint); + double theta = std::abs(Point::angleDiff(robotPos, obsPoint)); - if (has_enemy_) + double s = distance + (enemy_distance * factor_close_enemy_ * has_enemy_) + theta * factor_theta_; + if (s < score) { - auto enemyDist = Point::distance(enemyPos, obs->GetPosition()); - dist *= (1.0f + factor_close_enemy_ * std::exp(-enemyDist / enemy_emergency_distance_)); - } - - if (dist < distance) - { - distance = dist; + score = s; closest_obstacle = obs; } } diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 18f5f8a3..4cb70a2a 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -8,6 +8,12 @@ namespace Modelec class BoxObstacle : public Obstacle { public: + enum Team + { + YELLOW = 0, + BLUE = 1, + }; + BoxObstacle() = default; BoxObstacle(const BoxObstacle&) = default; @@ -20,8 +26,24 @@ namespace Modelec std::vector GetAllPossiblePositions() const; + void SetColor(size_t index, Team team); + Team GetColor(size_t index) const; + std::array GetColors() const; + + std::vector GetSide(Team team) const; + + void ParseColor(const std::string& colorStr); + void ParseColor(const std::vector& colorVec); + protected: std::vector possible_angles_; + + std::array colors_ = { + BLUE, + BLUE, + BLUE, + BLUE + }; }; } diff --git a/src/modelec_strat/include/modelec_strat/pami_manager.hpp b/src/modelec_strat/include/modelec_strat/pami_manager.hpp index 2dee3368..c46aa344 100644 --- a/src/modelec_strat/include/modelec_strat/pami_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp @@ -2,7 +2,6 @@ #include -#include #include #include @@ -29,11 +28,6 @@ namespace Modelec int score_to_add_ = 0; - int score_goupie_ = 0; - int score_superstar_ = 0; - int score_all_party_ = 0; - int score_free_zone_ = 0; - rclcpp::TimerBase::SharedPtr timer_add_; // rclcpp::TimerBase::SharedPtr timer_remove_; diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 29fb2443..a7938a64 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -28,6 +28,7 @@ namespace Modelec TAKE_MISSION = modelec_interfaces::msg::StratState::TAKE_MISSION, FREE_MISSION = modelec_interfaces::msg::StratState::FREE_MISSION, + THERMO_MISSION = modelec_interfaces::msg::StratState::THERMO_MISSION, DO_GO_HOME = modelec_interfaces::msg::StratState::DO_GO_HOME, STOP = modelec_interfaces::msg::StratState::STOP, @@ -64,6 +65,10 @@ namespace Modelec std::queue game_action_sequence_; bool static_strat_ = false; + float factor_obs_; + float factor_thermo_; + int timer_period_ms_ = 100; + std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 18602b9d..f8a93a32 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -2,15 +2,16 @@ #include "modelec_strat/action_executor.hpp" -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) +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); } -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Front front) : DownAction(action_executor) +Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Side side, bool inverted) : DownAction(action_executor) { - front_ = front; + side_ = side; + inverted_ = inverted; } void Modelec::DownAction::Next() @@ -29,43 +30,43 @@ void Modelec::DownAction::Next() case ActionExec::DOWN_STEP: { ActionServoTimedArray msg; - msg.items.resize(front_ == BOTH ? 8 : 4); + msg.items.resize(side_ == BOTH ? 8 : 4); - if (front_ == FRONT || front_ == BOTH) + if (side_ == FRONT || side_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 1.95; - msg.items[0].end_angle = 3; - msg.items[0].duration_s = 1; + 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 = 1.9; - msg.items[1].end_angle = 0.85; - msg.items[1].duration_s = 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.3; - msg.items[2].end_angle = 0; - msg.items[2].duration_s = 0.5; + 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.7; - msg.items[3].end_angle = 2.9; - msg.items[3].duration_s = 0.5; + 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 (front_ == BACK || front_ == BOTH) + if (side_ == BACK || side_ == BOTH) { - int i = front_ == BOTH ? 4 : 0; + 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 = 1; + 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 = 1; + msg.items[i+1].duration_s = 0.5; msg.items[i+2].id = 10; msg.items[i+2].start_angle = 0; @@ -95,11 +96,35 @@ void Modelec::DownAction::Init(const std::vector& params) { if (!params.empty()) { - SetFront(static_cast(std::stoi(params[1]))); + SetSide(static_cast(std::stoi(params[1]))); + SetInverted(params.size() >= 3 ? (params[2] == "1" || params[2] == "true") : false); } } -void Modelec::DownAction::SetFront(Front front) +void Modelec::DownAction::SetSide(Side side) { - front_ = front; + side_ = side; +} + +void Modelec::DownAction::SetInverted(bool inverted) +{ + inverted_ = inverted; +} + +void Modelec::DownAction::End() +{ + if (side_ == BOTH) + { + action_executor_->arm_pos_[FRONT].down = true; + action_executor_->arm_pos_[BACK].down = true; + + action_executor_->arm_pos_[FRONT].rotated = inverted_; + action_executor_->arm_pos_[BACK].rotated = inverted_; + } + else + { + action_executor_->arm_pos_[side_].down = true; + + action_executor_->arm_pos_[side_].rotated = inverted_; + } } diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 52fe4031..30cb1a43 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -8,17 +8,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, Front front, int n) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, Side side, int n) : FreeAction(action_executor) { - AddServo(n, front); + AddServo(n, side); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) { AddServo(servo.first, servo.second); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) { AddServos(servos); } @@ -45,8 +45,8 @@ 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 = servos_[i].second ? 3 : 0; - msg.items[i].end_angle = servos_[i].second ? 1 : 0; + msg.items[i].start_angle = 3; + msg.items[i].end_angle = 1; msg.items[i].duration_s = 0.5; } @@ -70,23 +70,32 @@ void Modelec::FreeAction::Init(const std::vector& params) for (size_t i = 1; i < params.size(); i += 2) { int id = std::stoi(params[i]); - bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front ? FRONT : BACK); + bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true; + AddServo(id, side ? FRONT : BACK); } } } -void Modelec::FreeAction::AddServo(int id, Front front) +void Modelec::FreeAction::AddServo(int id, Side side) { - servos_.emplace_back(id, front); + servos_.emplace_back(id, side); } -void Modelec::FreeAction::AddServo(std::pair servo) +void Modelec::FreeAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::FreeAction::AddServos(const std::vector>& servos) +void Modelec::FreeAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } + +void Modelec::FreeAction::End() +{ + for (auto servo : servos_) + { + auto index = servo.first + (servo.second == FRONT ? 0 : 4); + action_executor_->servo_pos_[index] = false; + } +} diff --git a/src/modelec_strat/src/action/look_on_action.cpp b/src/modelec_strat/src/action/look_on_action.cpp new file mode 100644 index 00000000..5663bd9e --- /dev/null +++ b/src/modelec_strat/src/action/look_on_action.cpp @@ -0,0 +1,69 @@ +#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 25849a6c..91752e5c 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -8,17 +8,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, Front front, int n) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, Side side, int n) : TakeAction(action_executor) { - AddServo(n, front); + AddServo(n, side); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) { AddServo(servo); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) { AddServos(servos); } @@ -45,8 +45,8 @@ 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 = servos_[i].second ? 1 : 0; - msg.items[i].end_angle = servos_[i].second ? 3 : 0; + msg.items[i].start_angle = 0; + msg.items[i].end_angle = 3; msg.items[i].duration_s = 0.5; } @@ -70,23 +70,32 @@ void Modelec::TakeAction::Init(const std::vector& params) for (size_t i = 1; i < params.size(); i += 2) { int id = std::stoi(params[i]); - bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front ? FRONT : BACK); + bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true; + AddServo(id, side ? FRONT : BACK); } } } -void Modelec::TakeAction::AddServo(int id, Front front) +void Modelec::TakeAction::AddServo(int id, Side side) { - servos_.emplace_back(id, front); + servos_.emplace_back(id, side); } -void Modelec::TakeAction::AddServo(std::pair servo) +void Modelec::TakeAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::TakeAction::AddServos(const std::vector>& servos) +void Modelec::TakeAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } + +void Modelec::TakeAction::End() +{ + for (auto servo : servos_) + { + auto index = servo.first + (servo.second == FRONT ? 0 : 4); + action_executor_->servo_pos_[index] = true; + } +} diff --git a/src/modelec_strat/src/action/thermo_action.cpp b/src/modelec_strat/src/action/thermo_action.cpp new file mode 100644 index 00000000..3a395bcd --- /dev/null +++ b/src/modelec_strat/src/action/thermo_action.cpp @@ -0,0 +1,87 @@ +#include + +#include "modelec_strat/action_executor.hpp" + +Modelec::ThermoAction::ThermoAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) +{ + steps_.push(ActionExec::THERMO_STEP); + steps_.push(ActionExec::DONE_STEP); +} + +Modelec::ThermoAction::ThermoAction(const std::shared_ptr& action_executor, Side side, bool deploy) : ThermoAction(action_executor) +{ + side_ = side; + deploy_ = deploy; +} + +void Modelec::ThermoAction::Next() +{ + if (steps_.empty()) + { + done_ = true; + return; + } + + auto step = steps_.front(); + steps_.pop(); + + switch (step) + { + case ActionExec::THERMO_STEP: + { + 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 (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; + } + + action_executor_->MoveServoTimed(msg); + } + break; + case ActionExec::DONE_STEP: + { + done_ = true; + } + break; + default: + break; + } +} + +void Modelec::ThermoAction::Init(const std::vector& params) +{ + if (!params.empty()) + { + SetSide(static_cast(std::stoi(params[0]))); + SetDeploy(params[1] == "1" || params[1] == "true"); + } +} + +void Modelec::ThermoAction::SetSide(Side side) +{ + side_ = side; +} + +void Modelec::ThermoAction::SetDeploy(bool deploy) +{ + deploy_ = deploy; +} + +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 new file mode 100644 index 00000000..03778a32 --- /dev/null +++ b/src/modelec_strat/src/action/toggle_servo_action.cpp @@ -0,0 +1,101 @@ +#include + +#include "modelec_strat/action_executor.hpp" + +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) +{ + steps_.push(ActionExec::TOGGLE_SERVO_STEP); + steps_.push(ActionExec::DONE_STEP); +} + +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, Side side, int n) : ToggleServoAction(action_executor) +{ + AddServo(n, side); +} + +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, std::pair servo) : ToggleServoAction(action_executor) +{ + AddServo(servo.first, servo.second); +} + +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, std::vector> servos) : ToggleServoAction(action_executor) +{ + AddServos(servos); +} + +void Modelec::ToggleServoAction::Next() +{ + if (steps_.empty()) + { + done_ = true; + return; + } + + auto step = steps_.front(); + steps_.pop(); + + switch (step) + { + case ActionExec::TOGGLE_SERVO_STEP: + { + modelec_interfaces::msg::ActionServoTimedArray msg; + + msg.items.resize(servos_.size()); + + 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; + } + + action_executor_->MoveServoTimed(msg); + } + break; + case ActionExec::DONE_STEP: + { + done_ = true; + } + break; + default: + break; + } +} + +void Modelec::ToggleServoAction::Init(const std::vector& params) +{ + if (params.size() >= 2) + { + for (size_t i = 1; i < params.size(); i += 2) + { + int id = std::stoi(params[i]); + bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true; + AddServo(id, side ? FRONT : BACK); + } + } +} + +void Modelec::ToggleServoAction::AddServo(int id, Side side) +{ + servos_.emplace_back(id, side); +} + +void Modelec::ToggleServoAction::AddServo(std::pair servo) +{ + servos_.emplace_back(servo); +} + +void Modelec::ToggleServoAction::AddServos(const std::vector>& servos) +{ + servos_.insert(servos_.end(), servos.begin(), servos.end()); +} + +void Modelec::ToggleServoAction::End() +{ + for (auto servo : servos_) + { + auto index = servo.first + (servo.second == FRONT ? 0 : 4); + action_executor_->servo_pos_[index] = !action_executor_->servo_pos_[index]; + } +} diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 0907bbab..bef70952 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -8,9 +8,9 @@ Modelec::UPAction::UPAction(const std::shared_ptr& action_execut steps_.push(ActionExec::DONE_STEP); } -Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, Front front) : UPAction(action_executor) +Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, Side side) : UPAction(action_executor) { - front_ = front; + side_ = side; } void Modelec::UPAction::Next() @@ -29,53 +29,53 @@ void Modelec::UPAction::Next() case ActionExec::UP_STEP: { ActionServoTimedArray msg; - msg.items.resize(front_ == BOTH ? 8 : 4); + msg.items.resize(side_ == BOTH ? 8 : 4); - if (front_ == FRONT || front_ == BOTH) + if (side_ == FRONT || side_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 3; - msg.items[0].end_angle = 1.95; - msg.items[0].duration_s = 1; + 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.85; - msg.items[1].end_angle = 1.9; - msg.items[1].duration_s = 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 = 0; - msg.items[2].end_angle = 0.3; - msg.items[2].duration_s = 1; + 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 = 2.9; - msg.items[3].end_angle = 2.7; - msg.items[3].duration_s = 1; + 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 (front_ == BACK || front_ == BOTH) { - int i = front_ == BOTH ? 4 : 0; + 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 = 1; + 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 = 1; + 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 = 1; + 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 = 1; + msg.items[i+3].duration_s = 0.5; } action_executor_->MoveServoTimed(msg); @@ -95,11 +95,24 @@ void Modelec::UPAction::Init(const std::vector& params) { if (!params.empty()) { - SetFront(static_cast(std::stoi(params[1]))); + SetSide(static_cast(std::stoi(params[1]))); } } -void Modelec::UPAction::SetFront(Front front) +void Modelec::UPAction::SetSide(Side side) { - front_ = front; + side_ = side; +} + +void Modelec::UPAction::End() +{ + if (side_ == BOTH) + { + action_executor_->arm_pos_[FRONT].down = false; + action_executor_->arm_pos_[BACK].down = false; + } + else + { + action_executor_->arm_pos_[side_].down = false; + } } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index d41a7cc7..c927d76e 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -4,6 +4,9 @@ #include "modelec_strat/action/up_action.hpp" #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" namespace Modelec @@ -26,12 +29,12 @@ namespace Modelec }); servo_move_res_sub_ = node_->create_subscription( - "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr) + "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr msg) { - // BUG - // if ServoTimed is called this one will trigger so step_running_ will be decremented at the beginning of the Timed one - // step_running_ -= msg->items.size(); - // Update(); + for (const auto& item : msg->items) + { + servo_value_[item.id] = item.angle; + } }); relay_move_res_sub_ = node_->create_subscription( @@ -46,7 +49,7 @@ namespace Modelec { auto token = split(msg->action, ActionExec::DELIMITER[0]); - if (token.size() == 0) + if (token.empty()) { RCLCPP_WARN( node_->get_logger(), @@ -54,10 +57,10 @@ namespace Modelec return; } - action_ = BaseAction::CreateAction( + auto action = BaseAction::CreateAction( token[0], shared_from_this()); - if (action_ == nullptr) + if (action == nullptr) { RCLCPP_WARN( node_->get_logger(), @@ -65,7 +68,8 @@ namespace Modelec msg->action.c_str()); return; } - action_->Init(token); + action->Init(token); + action_.push(action); action_done_ = false; step_running_ = 0; Update(); @@ -84,26 +88,150 @@ namespace Modelec "/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() >= 5) + if (msg->buttons.size() >= 15) { if (msg->buttons[0] == 1) // A button { - Down(BaseAction::BOTH); + if (action_done_) + { + ToggleServo({{0, BaseAction::FRONT}}); + } } else if (msg->buttons[1] == 1) // B button { - Up(BaseAction::BOTH); + if (action_done_) + { + ToggleServo({{1, BaseAction::FRONT}}); + } } else if (msg->buttons[3] == 1) // X button { - Take({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); + if (action_done_) + { + ToggleServo({{3, BaseAction::FRONT}}); + } } else if (msg->buttons[4] == 1) // Y button { - Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); + if (action_done_) + { + ToggleServo({{2, BaseAction::FRONT}}); + } + } + else if (msg->buttons[6] == 1) // L1 button + { + if (action_done_) + { + RotateArm(BaseAction::BACK, false, !arm_pos_[BaseAction::BACK].rotated); + } + } + else if (msg->buttons[7] == 1) // R1 button + { + if (action_done_) + { + RotateArm(BaseAction::FRONT, false, !arm_pos_[BaseAction::FRONT].rotated); + } + } + else if (msg->buttons[14] == 1) // LT button + { + ActivateThermo(BaseAction::Side::LEFT, !thermo_state_[BaseAction::Side::LEFT]); + } + else if (msg->buttons[15] == 1) // LR button + { + ActivateThermo(BaseAction::Side::RIGHT, !thermo_state_[BaseAction::Side::RIGHT]); + } + } + if (msg->axes.size() == 8) + { + auto left_trig = msg->axes[4]; + auto right_trig = msg->axes[5]; + + if (left_trig == 1 && last_left_trig == -1) // left trigger pressed + { + if (action_done_) + { + ToggleArm(BaseAction::BACK); + } + last_left_trig = left_trig; + } else if (left_trig == -1 && last_left_trig == 1) // left trigger released + { + last_left_trig = left_trig; + } + + if (right_trig == 1 && last_left_trig == -1) // right trigger pressed + { + if (action_done_) + { + ToggleArm(BaseAction::FRONT); + } + last_right_trig = right_trig; + } else if (right_trig == -1 && last_right_trig == 1) // right trigger released + { + last_right_trig = right_trig; + } + + auto btn_horizontal = msg->axes[6]; + auto btn_vertical = msg->axes[7]; + + if (btn_horizontal == 1.0) // left + { + if (action_done_) + { + ToggleServo({{3, BaseAction::BACK}}); + } + } + else if (btn_horizontal == -1.0) // right + { + if (action_done_) + { + ToggleServo({{1, BaseAction::BACK}}); + } + } + if (btn_vertical == 1.0) // up + { + if (action_done_) + { + ToggleServo({{0, BaseAction::BACK}}); + } + } + else if (btn_vertical == -1.0) // down + { + if (action_done_) + { + ToggleServo({{2, BaseAction::BACK}}); + } } } }); + + score_pub_ = node_->create_publisher("/strat/score", 10); + + ask_color_client_ = node_->create_client("action/detect_color"); + + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.reliable(); + + color_sub_ = node_->create_subscription( + "/action/detect_color/res", qos, [this](const std_msgs::msg::String::SharedPtr msg) + { + auto res = split(msg->data, '|'); + if (res.size() < 2 || res[0] != "1") + { + RCLCPP_WARN(node_->get_logger(), "Color detection failed: %s", msg->data.c_str()); + return; + } + + 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]); + } else { + RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side"); + } + }); + + ask_pub_ = node_->create_publisher( + "/action/detect_color/ask", qos); } rclcpp::Node::SharedPtr ActionExecutor::GetNode() const @@ -118,27 +246,42 @@ namespace Modelec void ActionExecutor::Update() { - if (action_ != nullptr && !action_->IsDone() && step_running_ <= 0) + auto action = action_.front(); + if (action != nullptr && !action->IsDone() && step_running_ <= 0) { RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Update: Executing next step of action, step_running_=%d", step_running_); - action_->Next(); - if (action_->IsDone()) + action->Next(); + if (action->IsDone()) { - action_done_ = true; - action_ = nullptr; + ActionFinished(action); + action_.pop(); + if (action_.empty()) + { + action_done_ = true; + } else + { + Update(); + } } } - else if (action_ != nullptr && action_->IsDone()) + else if (action != nullptr && action->IsDone()) { RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Update: Action is done, step_running_=%d", step_running_); - action_done_ = true; - action_ = nullptr; + ActionFinished(action); + + action_.pop(); + if (action_.empty()) + { + action_done_ = true; + } else { + Update(); + } } } @@ -146,13 +289,22 @@ namespace Modelec { action_done_ = true; step_running_ = 0; - action_ = nullptr; + + std::queue> empty; + std::swap(action_, empty); } - void ActionExecutor::Down(BaseAction::Front front, bool force) { - if (action_done_ || force) + void ActionExecutor::Down(BaseAction::Side front, bool force, bool inverted) { + if (!arm_pos_[front].down || force) { - action_ = std::make_shared(shared_from_this(), front); + RCLCPP_DEBUG( + node_->get_logger(), + "ActionExecutor Down called for front=%d, force=%d", + static_cast(front), + static_cast(force)); + + auto action = std::make_shared(shared_from_this(), front, inverted); + action_.push(action); if (action_done_) { step_running_ = 0; @@ -163,10 +315,17 @@ namespace Modelec } } - void ActionExecutor::Up(BaseAction::Front front, bool force) { - if (action_done_ || force) + void ActionExecutor::Up(BaseAction::Side side, bool force) { + if ((arm_pos_[side].down) || force) { - action_ = std::make_shared(shared_from_this(), front); + RCLCPP_DEBUG( + node_->get_logger(), + "ActionExecutor Up called for side=%d, force=%d", + static_cast(side), + static_cast(force)); + + auto action = std::make_shared(shared_from_this(), side); + action_.push(action); if (action_done_) { step_running_ = 0; @@ -177,24 +336,50 @@ namespace Modelec } } - void ActionExecutor::Take(const std::vector>& servos, bool force) { - if (action_done_ || force) + void ActionExecutor::ToggleArm(BaseAction::Side side, bool force) + { + RCLCPP_DEBUG( + node_->get_logger(), + "ActionExecutor ToggleArm called for side=%d, force=%d", + static_cast(side), + static_cast(force)); + + if (side == BaseAction::BOTH) { - action_ = std::make_shared(shared_from_this(), servos); - if (action_done_) - { - step_running_ = 0; - } - action_done_ = false; + ToggleArm(BaseAction::FRONT, force); + ToggleArm(BaseAction::BACK, force); + return; + } - Update(); + if (arm_pos_[side].down) + { + Up(side, force); + } + else + { + Down(side, force, arm_pos_[side].rotated); } } - void ActionExecutor::Free(const std::vector>& servos, bool force) { - if (action_done_ || force) + void ActionExecutor::RotateArm(BaseAction::Side side, bool force, bool rotated) + { + if ((arm_pos_[side].rotated != rotated || !arm_pos_[side].down) || force) { - action_ = std::make_shared(shared_from_this(), servos); + RCLCPP_DEBUG( + node_->get_logger(), + "ActionExecutor RotateArm called for side=%d, force=%d, rotated=%d", + static_cast(side), + static_cast(force), + static_cast(rotated)); + + if (arm_pos_[side].down) + { + auto action = std::make_shared(shared_from_this(), side); + action_.push(action); + } + + auto action = std::make_shared(shared_from_this(), side, rotated); + action_.push(action); if (action_done_) { step_running_ = 0; @@ -205,6 +390,58 @@ namespace Modelec } } + void ActionExecutor::Take(const std::vector>& servos) { + RCLCPP_DEBUG( + node_->get_logger(), + "ActionExecutor Take called for %d servos", + static_cast(servos.size())); + + auto action = std::make_shared(shared_from_this(), servos); + action_.push(action); + if (action_done_) + { + step_running_ = 0; + } + action_done_ = false; + + Update(); + } + + void ActionExecutor::Free(const std::vector>& servos) { + RCLCPP_DEBUG( + node_->get_logger(), + "ActionExecutor Free called for %d servos", + static_cast(servos.size())); + + auto action = std::make_shared(shared_from_this(), servos); + action_.push(action); + if (action_done_) + { + step_running_ = 0; + } + action_done_ = false; + + Update(); + } + + void ActionExecutor::ToggleServo(const std::vector>& servos) + { + RCLCPP_DEBUG( + node_->get_logger(), + "ActionExecutor ToggleServo called for %d servos", + static_cast(servos.size())); + + auto action = std::make_shared(shared_from_this(), servos); + action_.push(action); + if (action_done_) + { + step_running_ = 0; + } + action_done_ = false; + + Update(); + } + void ActionExecutor::MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg) { servo_timed_move_pub_->publish(msg); @@ -223,6 +460,46 @@ namespace Modelec step_running_ += msg.items.size(); } + void ActionExecutor::ActivateThermo(BaseAction::Side side, bool deploy, bool force) + { + if (thermo_state_[side] != deploy || force) + { + auto action = std::make_shared(shared_from_this(), side, deploy); + action_.push(action); + if (action_done_) + { + step_running_ = 0; + } + action_done_ = false; + + Update(); + } + } + + 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( + node_->get_logger(), + "ActionExecutor ActionFinished called for action"); + action->End(); + } + bool ActionExecutor::IsEmpty() const { return box_obstacles_[0] == nullptr && box_obstacles_[1] == nullptr; @@ -233,9 +510,9 @@ namespace Modelec return box_obstacles_[0] != nullptr && box_obstacles_[1] != nullptr; } - bool ActionExecutor::HasBox(BaseAction::Front front) const + bool ActionExecutor::HasBox(BaseAction::Side side) const { - return box_obstacles_[front] != nullptr; + return box_obstacles_[side] != nullptr; } bool ActionExecutor::HasFrontBox() const @@ -252,4 +529,37 @@ namespace Modelec { return HasFrontBox() != HasBackBox(); } + + void ActionExecutor::SendPoint(const int point) const + { + std_msgs::msg::Int64 msg; + msg.data = point; + score_pub_->publish(msg); + } + + 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/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 47b80d3f..8f325c45 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -7,28 +7,29 @@ namespace Modelec { EnemyManager::EnemyManager() : Node("enemy_manager") { - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Config::load(config_path)) - { - RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); - } - - min_move_threshold_mm_ = Config::get("config.enemy.detection.min_move_threshold_mm", 50); - - refresh_rate_s_ = Config::get("config.enemy.detection.refresh_rate", 1.0); + 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); - max_stationary_time_s_ = Config::get("config.enemy.detection.max_stationary_time_s", 10.0); + this->declare_parameter("map.size.map_width_mm", 3000.0); + this->declare_parameter("map.size.map_height_mm", 2000.0); - map_width_ = Config::get("config.map.size.map_width_mm", 3000.0); - map_height_ = Config::get("config.map.size.map_height_mm", 2000.0); + this->declare_parameter("robot.size.width_mm", 500.0); + this->declare_parameter("robot.size.length_mm", 500.0); - margin_detection_table_ = Config::get("config.enemy.detection.margin_detection_table_mm", 50.0); + 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(); - robot_width_ = Config::get("config.robot.size.width_mm", 500.0); - robot_length_ = Config::get("config.robot.size.length_mm", 500.0); - robot_radius_ = std::max(robot_width_, robot_length_) * 0.4; + map_width_ = this->get_parameter("map.size.map_width_mm").as_double(); + map_height_ = this->get_parameter("map.size.map_height_mm").as_double(); - min_emergency_distance_ = Config::get("config.enemy.detection.min_emergency_distance_mm", 500.0f); + robot_width_ = this->get_parameter("robot.size.width_mm").as_double(); + robot_length_ = this->get_parameter("robot.size.length_mm").as_double(); current_pos_sub_ = this->create_subscription( "odometry/position", 10, @@ -50,7 +51,7 @@ namespace Modelec close_enemy_pos_pub_ = this->create_publisher( "enemy/position/emergency", 10); - state_sub_ = create_subscription("/strat/state", 10, + state_sub_ = create_subscription("strat/state", 10, [this]( const modelec_interfaces::msg::StratState::SharedPtr @@ -69,7 +70,7 @@ namespace Modelec }); enemy_long_time_pub_ = this->create_publisher( - "/enemy/long_time", 10); + "enemy/long_time", 10); start_sub_ = this->create_subscription( "lidar/start", 10, @@ -80,7 +81,7 @@ namespace Modelec }); timer_ = this->create_wall_timer( - std::chrono::nanoseconds(static_cast(refresh_rate_s_ )), + std::chrono::milliseconds(static_cast(refresh_rate_s_ * 1000.0)), [this]() { TimerCallback(); diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 10aa9b24..e53cbdf0 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -5,8 +5,8 @@ namespace Modelec { FreeMission::FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, - BaseAction::Front front) - : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + BaseAction::Side side) + : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -24,8 +24,11 @@ namespace Modelec { std::swap(steps_, empty); steps_.push(GO_TO_FREE); + steps_.push(CHECK_BOX); steps_.push(DOWN); - steps_.push(FREE); + steps_.push(FREE_FIRST); + steps_.push(ROTATE_ARM); + steps_.push(FREE_OTHER); steps_.push(UP); steps_.push(GO_BACK); steps_.push(DONE); @@ -82,64 +85,101 @@ namespace Modelec { return; } - auto dist = std::clamp(Point::distance(Point(currPos->x, currPos->y, currPos->theta), - nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0); - auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); - auto pos = depoPoint.GetTakePosition(dist); - - RCLCPP_INFO( - node_->get_logger(), - "FreeMission: position (%.2d, %.2d) with distance %.2f", - pos.x, pos.y, dist); + auto pos = depoPoint.GetTakePosition(200.0); - pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; return; } } + action_executor_->LookOn(BaseAction::Side::CENTER); + go_timeout_ = node_->now(); } + break; + case CHECK_BOX: + { + auto obs = action_executor_->box_obstacles_[side_]; + + auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::BLUE : BoxObstacle::YELLOW); + + RCLCPP_DEBUG(node_->get_logger(), "Box on side %d has %d boxes of his color side", static_cast(side_), static_cast(vect.size())); + + if (vect.size() == 4) + { + std::queue empty; + std::swap(steps_, empty); + + steps_.push(DOWN); + steps_.push(FREE_FIRST); + steps_.push(UP); + steps_.push(GO_BACK); + steps_.push(DONE); + } else if (vect.empty()) + { + std::queue empty; + std::swap(steps_, empty); + + steps_.push(ROTATE_ARM); + steps_.push(FREE_OTHER); + steps_.push(UP); + steps_.push(GO_BACK); + steps_.push(DONE); + } + } + break; case DOWN: { - action_executor_->Down(front_); + action_executor_->Down(side_); deploy_time_ = node_->now(); } break; - case FREE: + case FREE_FIRST: { - action_executor_->Free({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); - deploy_time_ = node_->now(); + auto obs = action_executor_->box_obstacles_[side_]; - auto obs = action_executor_->box_obstacles_[front_]; - action_executor_->box_obstacles_[front_] = nullptr; + auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::BLUE : BoxObstacle::YELLOW); - auto pos = nav_->GetCurrentPos(); + auto servo = std::vector>(); + for (auto s : vect) + { + servo.push_back({s, side_}); + } - obs->SetPosition( - pos->x + 200 * cos(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)), - pos->y + 200 * sin(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)), - pos->theta); + action_executor_->Free(servo); - obs->SetAtObjective(true); + deploy_time_ = node_->now(); - nav_->GetPathfinding()->AddObstacle(obs); + min_time_ = node_->now() + rclcpp::Duration::from_seconds(1); + } + break; + case ROTATE_ARM: + { + action_executor_->RotateArm(side_, false, true); + deploy_time_ = node_->now(); + } + break; + case FREE_OTHER: + { + action_executor_->Free({{0, side_}, {1, side_}, {2, side_}, {3, side_}}); + deploy_time_ = node_->now(); - min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); + min_time_ = node_->now() + rclcpp::Duration::from_seconds(1); } break; case UP: { - action_executor_->Up(front_); + action_executor_->Up(side_); deploy_time_ = node_->now(); } break; @@ -150,7 +190,7 @@ namespace Modelec { auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); auto pos = depoPoint.GetTakePosition(300); - pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE) { @@ -164,6 +204,20 @@ namespace Modelec { break; case DONE: { + auto obs = action_executor_->box_obstacles_[side_]; + action_executor_->box_obstacles_[side_] = nullptr; + + 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)), + pos->theta); + + obs->SetAtObjective(true); + + nav_->GetPathfinding()->AddObstacle(obs); + target_deposite_zone_->Validate(true); } diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 8e8026e7..9b11d740 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -13,7 +13,9 @@ namespace Modelec { node_ = node; - mission_score_ = Config::get("config.mission_score.go_home", 0); + node->declare_parameter("mission_score.go_home", 0); + + mission_score_ = node->get_parameter("mission_score.go_home").as_int(); score_pub_ = node_->create_publisher("/strat/score", 10); diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 36470080..efe1cab9 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -5,8 +5,8 @@ namespace Modelec { TakeMission::TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, - BaseAction::Front front) - : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + BaseAction::Side side) + : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -80,16 +80,16 @@ namespace Modelec { break; } - action_executor_->box_obstacles_[front_] = closestBox; + action_executor_->box_obstacles_[side_] = closestBox; auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI); + pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); - if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; break; @@ -102,16 +102,16 @@ namespace Modelec { break; case GO_TO_TAKE_CLOSE: { - if (action_executor_->box_obstacles_[front_] == nullptr) + if (action_executor_->box_obstacles_[side_] == nullptr) { status_ = MissionStatus::FAILED; break; } - auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); - pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + auto pos = action_executor_->box_obstacles_[side_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; break; @@ -122,32 +122,36 @@ namespace Modelec { break; case DOWN: { - action_executor_->Down(front_); + action_executor_->RotateArm(side_, false, false); + deploy_time_ = node_->now(); } break; case TAKE: { - action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); + action_executor_->Take({{0, side_}, {1, side_}, {2, side_}, {3, side_}}); deploy_time_ = node_->now(); min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); } break; case UP: { - action_executor_->Up(front_); + action_executor_->Up(side_); + action_executor_->LookOn(side_); deploy_time_ = node_->now(); } break; case DONE: { - if (action_executor_->box_obstacles_[front_] == nullptr) + if (action_executor_->box_obstacles_[side_] == nullptr) { status_ = MissionStatus::FAILED; break; } - nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId()); + nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[side_]->GetId()); + + action_executor_->AskColor(); } status_ = MissionStatus::DONE; diff --git a/src/modelec_strat/src/missions/thermo_mission.cpp b/src/modelec_strat/src/missions/thermo_mission.cpp new file mode 100644 index 00000000..3492dcd6 --- /dev/null +++ b/src/modelec_strat/src/missions/thermo_mission.cpp @@ -0,0 +1,149 @@ +#include + +#include "modelec_strat/action/base_action.hpp" + +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) + { + } + + void ThermoMission::Start(rclcpp::Node::SharedPtr node) + { + node_ = node; + + go_timeout_ = node_->now(); + deploy_time_ = node_->now(); + last_ask_waypoint_time_ = node_->now(); + + status_ = MissionStatus::RUNNING; + + thermo_positions_ = nav_->GetThermoPositions(); + + std::queue empty; + std::swap(steps_, empty); + + steps_.push(GO_TO_THERMO); + steps_.push(GO_TO_THERMO_CLOSE); + steps_.push(ACTIVATE_THERMO); + steps_.push(GO_TO_10); + steps_.push(DEACTIVATE_THERMO); + steps_.push(DONE); + } + + void ThermoMission::Update() + { + if (!action_executor_->IsActionDone()) + { + 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(); + } + } + + auto step_ = steps_.front(); + steps_.pop(); + + switch (step_) + { + case GO_TO_THERMO: + { + auto start = thermo_positions_[0].GetTakePosition(CLOSE_DISTANCE, M_PI_2); + + if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } + + go_timeout_ = node_->now(); + } + break; + case GO_TO_THERMO_CLOSE: + { + auto start = thermo_positions_[0]; + + if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } + + go_timeout_ = node_->now(); + } + 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(); + } + break; + case GO_TO_10: + { + auto pos = thermo_positions_[1]; + + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } + } + 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(); + } + break; + case DONE: + { + action_executor_->SendPoint(10); + IsThermoDone = true; + } + + status_ = MissionStatus::DONE; + break; + default: + break; + } + } + + 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 c219c29d..cb54df16 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -1,7 +1,6 @@ #include #include #include -#include namespace Modelec { @@ -10,11 +9,43 @@ namespace Modelec NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node) { - pathfinding_ = std::make_shared(node); - - factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); + 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); - enemy_emergency_distance_ = Config::get("config.enemy.detection.min_emergency_distance_mm", 390); + pathfinding_ = std::make_shared(node); SetupSpawn(); @@ -116,7 +147,7 @@ namespace Modelec return pathfinding_; } - int NavigationHelper::GetTeamId() const + NavigationHelper::Team NavigationHelper::GetTeamId() const { return team_id_; } @@ -302,7 +333,7 @@ namespace Modelec { double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x); - if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) + if (Point::angleDiff(angle, (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) { Rotate(angle); return true; @@ -518,9 +549,9 @@ namespace Modelec { auto zonePoint = zone->GetPosition(); double distance = Point::distance(posPoint, zonePoint); - double enemy_distance = Point::distance(enemyPos, zone->GetPosition()); + double enemy_distance = Point::distance(enemyPos, zonePoint); double theta = std::abs(Point::angleDiff(posPoint, zonePoint)); - double s = distance + enemy_distance * factor_close_enemy_ + theta * 2; + double s = distance + (enemy_distance * factor_close_enemy_ * has_enemy_) + theta * factor_theta_; if (s < score) { score = s; @@ -535,21 +566,35 @@ namespace Modelec PosMsg::SharedPtr NavigationHelper::GetHomePosition() { PosMsg::SharedPtr home = std::make_shared(); - if (team_id_ == YELLOW) - { - home->x = Config::get("config.home.yellow@x", 0); - home->y = Config::get("config.home.yellow@y", 0); - home->theta = Config::get("config.home.yellow@theta", 0); - } - else - { - home->x = Config::get("config.home.blue@x", 0); - home->y = Config::get("config.home.blue@y", 0); - home->theta = Config::get("config.home.blue@theta", 0); - } + + std::string prefix = (team_id_ == YELLOW) ? "home.yellow" : "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(); + return home; } + std::array NavigationHelper::GetThermoPositions() + { + std::string prefix = (team_id_ == YELLOW) ? "thermo.yellow" : "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() + ); + + 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() + ); + + return {start, finish}; + } + void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { if (!has_enemy_) has_enemy_ = true; @@ -691,7 +736,7 @@ namespace Modelec return true; } - void NavigationHelper::SetTeamId(int id) + void NavigationHelper::SetTeamId(Team id) { team_id_ = id; } @@ -722,6 +767,7 @@ namespace Modelec void NavigationHelper::AskWaypoint() { + RCLCPP_DEBUG(node_->get_logger(), "Asking for active waypoint..."); std_msgs::msg::Empty msg; odo_ask_waypoint_pub_->publish(msg); } @@ -758,15 +804,15 @@ namespace Modelec void NavigationHelper::SetupSpawn() { spawn_yellow_["top"] = Point( - Config::get("config.spawn.yellow.top@x"), - Config::get("config.spawn.yellow.top@y"), - Config::get("config.spawn.yellow.top@theta") + 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() ); spawn_blue_["top"] = Point( - Config::get("config.spawn.blue.top@x"), - Config::get("config.spawn.blue.top@y"), - Config::get("config.spawn.blue.top@theta") + 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() ); } } \ 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 fdae6b27..7e19e5f6 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -1,5 +1,7 @@ #include +#include "modelec_utils/utils.hpp" + namespace Modelec { BoxObstacle::BoxObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type) @@ -57,4 +59,70 @@ namespace Modelec } return positions; } + + void BoxObstacle::SetColor(size_t index, Team team) + { + if (index < colors_.size()) + { + colors_[index] = team; + } + } + + BoxObstacle::Team BoxObstacle::GetColor(size_t index) const + { + if (index < colors_.size()) + { + return colors_[index]; + } + return YELLOW; + } + + std::array BoxObstacle::GetColors() const + { + return colors_; + } + + std::vector BoxObstacle::GetSide(Team team) const + { + std::vector sideColors; + for (size_t i = 0; i < colors_.size(); ++i) + { + if (colors_[i] == team) + { + sideColors.push_back(i); + } + } + return sideColors; + } + + void BoxObstacle::ParseColor(const std::string& colorStr) + { + std::vector tokens = split(colorStr, ';'); + for (size_t i = 0; i < tokens.size() && i < colors_.size(); ++i) + { + if (tokens[i] == "yellow") + { + colors_[i] = YELLOW; + } + else if (tokens[i] == "blue") + { + colors_[i] = BLUE; + } + } + } + + void BoxObstacle::ParseColor(const std::vector& colorVec) + { + for (size_t i = 0; i < colorVec.size() && i < colors_.size(); ++i) + { + if (colorVec[i] == "yellow") + { + colors_[i] = YELLOW; + } + else if (colorVec[i] == "blue") + { + colors_[i] = BLUE; + } + } + } } diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp index 7b134420..052a5676 100644 --- a/src/modelec_strat/src/pami_manager.cpp +++ b/src/modelec_strat/src/pami_manager.cpp @@ -5,23 +5,13 @@ namespace Modelec { PamiManger::PamiManger() : Node("pami_manager") { - std::string config_file = ament_index_cpp::get_package_share_directory("modelec_strat") + - "/data/config.xml"; + this->declare_parameter("time_to_put_zone", 80); + this->declare_parameter("time_to_remove_top_pot", 70); - if (!Config::load(config_file)) - { - RCLCPP_ERROR(get_logger(), "Failed to load configuration"); - } - - time_to_put_zone_ = Config::get("config.pami.time_to_put_zone", 80); - time_to_remove_top_pot_ = Config::get("config.pami.time_to_remove_top_pot", 70); + 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_goupie_ = Config::get("config.mission_score.pami.goupie"); - score_superstar_ = Config::get("config.mission_score.pami.superstar"); - score_all_party_ = Config::get("config.mission_score.pami.all_party"); - score_free_zone_ = 0; - - score_to_add_ = 0; //score_goupie_ + score_superstar_ + /*score_all_party_ +*/ score_free_zone_; + score_to_add_ = 0; std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/pami_zone.xml"; @@ -31,9 +21,9 @@ namespace Modelec } start_time_sub_ = create_subscription( - "/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr msg) + "/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr) { - auto now = std::chrono::system_clock::now().time_since_epoch(); + /*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_); @@ -51,7 +41,7 @@ namespace Modelec } timer_add_->cancel(); - }); + });*/ }); add_obs_pub_ = create_publisher( @@ -65,10 +55,10 @@ namespace Modelec reset_strat_sub_ = create_subscription( "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr) { - if (timer_add_) + /*if (timer_add_) { timer_add_->cancel(); - } + }*/ /*if (timer_remove_) { timer_remove_->cancel(); diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 923ab722..9775ca85 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -1,6 +1,5 @@ #include #include -#include namespace Modelec { struct AStarNode { @@ -23,22 +22,33 @@ namespace Modelec { } Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr &node) : node_(node) { - map_width_mm_ = Config::get("config.map.size.map_width_mm", 3000); - map_height_mm_ = Config::get("config.map.size.map_height_mm", 2000); - - robot_length_mm_ = Config::get("config.robot.size.length_mm", 300); - robot_width_mm_ = Config::get("config.robot.size.width_mm", 300); - margin_mm_ = Config::get("config.robot.size.margin_mm", 100); - - 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", 50); - - factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); - - grid_width_ = Config::get("config.map.size.grid_width", 300); - grid_height_ = Config::get("config.map.size.grid_height", 200); + 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(); + + 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_ = 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(); + + 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"; @@ -262,7 +272,7 @@ namespace Modelec { const int goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y); // log the x and y in the real format - RCLCPP_INFO(node_->get_logger(), "Start: (%d, %d), Goal: (%d, %d)", start_x * (int) cell_size_mm_x, + RCLCPP_DEBUG(node_->get_logger(), "Start: (%d, %d), Goal: (%d, %d)", start_x * (int) cell_size_mm_x, (grid_height_ - 1 - start_y) * (int) cell_size_mm_y, goal_x * (int) cell_size_mm_x, (grid_height_ - 1 - goal_y) * (int) cell_size_mm_y); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index c981a8f5..6f85f5cf 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "modelec_strat/action/base_action.hpp" @@ -14,6 +15,16 @@ 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); + + 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(); + tir_sub_ = create_subscription( "/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr) { @@ -30,8 +41,7 @@ namespace Modelec team_id_sub_ = create_subscription( "/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg) { - team_id_ = static_cast(msg->data); - nav_->SetTeamId(team_id_); + nav_->SetTeamId(static_cast(msg->data)); }); spawn_id_sub_ = create_subscription( @@ -39,7 +49,7 @@ namespace Modelec { team_selected_ = true; team_id_ = msg->team_id; - nav_->SetTeamId(team_id_); + nav_->SetTeamId(static_cast(team_id_)); nav_->SetSpawn(msg->name); }); @@ -59,16 +69,6 @@ namespace Modelec "/action/tir/arm/set", 10); start_odo_pub_ = create_publisher("/odometry/start", 10); - - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Config::load(config_path)) - { - RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); - } - - game_action_sequence_.push(State::TAKE_MISSION); - game_action_sequence_.push(State::FREE_MISSION); - static_strat_ = Config::get("config.static_strat", false); } void StratFMS::Init() @@ -106,7 +106,7 @@ namespace Modelec current_mission_.reset(); match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - timer_ = create_wall_timer(std::chrono::milliseconds(100), [this] + timer_ = create_wall_timer(std::chrono::milliseconds(timer_period_ms_), [this] { Update(); }); @@ -142,11 +142,19 @@ namespace Modelec arm_msg.data = true; tir_arm_set_pub_->publish(arm_msg); - action_executor_->Up(BaseAction::Front::BOTH, true); + action_executor_->Up(BaseAction::Side::BOTH, true); action_executor_->Free({ {0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}, {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK}, - }, true); + }); + + auto empty_queue_ = std::queue(); + std::swap(game_action_sequence_, empty_queue_); + 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"); } @@ -170,18 +178,18 @@ namespace Modelec case State::SELECT_MISSION: { - auto elapsed = now - match_start_time_; + auto duration = (now - match_start_time_).seconds(); - if (elapsed.seconds() >= 100) + if (duration >= 100) { Transition(State::STOP, "All missions done"); } - else if (elapsed.seconds() > 60 && !action_executor_->IsEmpty()) + else if (duration > 60 && !action_executor_->IsEmpty() && duration < 90) { Transition(State::FREE_MISSION, "No Time left, freeing boxes"); } - else if (elapsed.seconds() < 80) + else if (duration < 80) { Transition(State::SELECT_GAME_ACTION, "Selecting a game action"); } @@ -208,59 +216,69 @@ namespace Modelec return; } + auto pos = nav_->GetCurrentPos(); + auto closestBox = nav_->GetClosestObstacle(pos); + auto closestDeposite = nav_->GetClosestDepositeZone(pos); - // TODO : If close to border, do the side mission (thermometre) + auto thermoPos = nav_->GetThermoPositions()[0]; - if (action_executor_->IsFull()) - { - RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); - Transition(State::FREE_MISSION, "Selecting FREE mission"); - } - else if (action_executor_->IsEmpty()) + double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta), + closestBox->GetPosition()) * factor_obs_; + double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta), + closestDeposite->GetPosition()); + double distToThermo = Point::distance(Point(pos->x, pos->y, pos->theta), + thermoPos) * factor_thermo_; + + if (distToThermo < distToBox && distToThermo < distToDeposite && !ThermoMission::IsThermoDone) { - RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission"); - Transition(State::TAKE_MISSION, "Selecting TAKE mission"); - } - else + RCLCPP_INFO(get_logger(), "Choosing THERMO mission (dist to thermo: %.2f < dist to box: %.2f, dist to deposite: %.2f)", + distToThermo, distToBox, distToDeposite); + Transition(State::THERMO_MISSION, "Selecting THERMO mission"); + } else { - auto pos = nav_->GetCurrentPos(); - auto closestBox = nav_->GetClosestObstacle(pos); - auto closestDeposite = nav_->GetClosestDepositeZone(pos); - - if (closestBox && closestDeposite) + if (action_executor_->IsFull()) + { + RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + else if (action_executor_->IsEmpty()) + { + RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission"); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); + } + else { - double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta), - closestBox->GetPosition()); - double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta), - closestDeposite->GetPosition()); - if (distToBox < distToDeposite) + if (closestBox && closestDeposite) { - RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)", - distToBox, distToDeposite); + if (distToBox < distToDeposite) + { + RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)", + distToBox, distToDeposite); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); + } + else + { + RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)", + distToDeposite, distToBox); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + } + else if (closestBox) + { + RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission"); Transition(State::TAKE_MISSION, "Selecting TAKE mission"); } - else + else if (closestDeposite) { - RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)", - distToDeposite, distToBox); + RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission"); Transition(State::FREE_MISSION, "Selecting FREE mission"); } - } - else if (closestBox) - { - RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission"); - Transition(State::TAKE_MISSION, "Selecting TAKE mission"); - } - else if (closestDeposite) - { - RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission"); - Transition(State::FREE_MISSION, "Selecting FREE mission"); - } - else - { - RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!"); - Transition(State::STOP, "No missions available"); + else + { + RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!"); + Transition(State::STOP, "No missions available"); + } } } } @@ -339,6 +357,27 @@ namespace Modelec } break; + case State::THERMO_MISSION: + { + if (!current_mission_) + { + current_mission_ = std::make_unique(nav_, action_executor_); + current_mission_->Start(shared_from_this()); + } + current_mission_->Update(); + if (current_mission_->GetStatus() == MissionStatus::DONE) + { + current_mission_.reset(); + Transition(State::SELECT_MISSION, "Thermo done"); + } + else if (current_mission_->GetStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + RCLCPP_ERROR(get_logger(), "Thermo mission failed!"); + Transition(State::SELECT_MISSION, "Thermo mission failed"); + } + } + break; case State::DO_GO_HOME: if (!current_mission_) { diff --git a/src/modelec_utils/include/modelec_utils/config.hpp b/src/modelec_utils/include/modelec_utils/config.hpp index 8aba5472..f3912574 100644 --- a/src/modelec_utils/include/modelec_utils/config.hpp +++ b/src/modelec_utils/include/modelec_utils/config.hpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include namespace Modelec { @@ -11,13 +13,25 @@ namespace Modelec { public: + + template + using BuilderFunc = std::function; + static bool load(const std::string& filepath); template static T get(const std::string& key, const T& default_value = T()); + template + static std::vector getArray(const std::string& prefix, + BuilderFunc builder = [](const std::string& base) { return get(base); }); + + static size_t count(const std::string& prefix); + + static void printAll(); + private: - static void parseNode(tinyxml2::XMLElement* element, const std::string& prefix); + static void parseNode(tinyxml2::XMLElement* element, const std::string& key); static inline std::unordered_map values_; }; @@ -44,4 +58,22 @@ namespace Modelec auto str = get(key, default_value ? "true" : "false"); return str == "true" || str == "1"; } + + template + std::vector Config::getArray(const std::string& prefix, + BuilderFunc builder) + { + std::vector result; + + size_t n = Config::count(prefix); + result.reserve(n); + + for (size_t i = 0; i < n; ++i) + { + std::string base = prefix + "[" + std::to_string(i) + "]"; + result.emplace_back(builder(base)); + } + + return result; + } } diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index 90850ac1..083c419c 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 165 -#define BASE_DISTANCE 290 +#define CLOSE_DISTANCE 155 +#define BASE_DISTANCE 310 namespace Modelec { @@ -15,9 +15,15 @@ namespace Modelec static double distance(const Point& p1, const Point& p2); static double angleDiff(const Point& p1, const Point& p2); + static double angleDiff(const double& p1, const double& p2); double distance(const Point& p2) const; double angleDiff(const Point& p2) const; + double angleDiff(const double& p2) const; + + void normalizeAngle(); + + static double normalizeAngle(double angle); [[nodiscard]] Point GetTakePosition(int distance, double angle) const; [[nodiscard]] Point GetTakePosition(int distance) const; diff --git a/src/modelec_utils/src/config.cpp b/src/modelec_utils/src/config.cpp index 50441977..c566b529 100644 --- a/src/modelec_utils/src/config.cpp +++ b/src/modelec_utils/src/config.cpp @@ -1,3 +1,4 @@ +#include #include namespace Modelec @@ -15,27 +16,77 @@ namespace Modelec return false; } - parseNode(root, ""); + parseNode(root, root->Name()); return true; } - void Config::parseNode(tinyxml2::XMLElement* element, const std::string& prefix) { - std::string key_prefix = prefix.empty() ? element->Name() : prefix + "." + element->Name(); + size_t Config::count(const std::string& prefix) + { + size_t max_index = 0; + bool found = false; + + for (const auto& [key, _] : values_) + { + if (key.rfind(prefix + "[", 0) == 0) + { + auto start = key.find('[', prefix.size()); + auto end = key.find(']', start); + if (start != std::string::npos && end != std::string::npos) + { + size_t index = std::stoul(key.substr(start + 1, end - start - 1)); + max_index = std::max(max_index, index); + found = true; + } + } + } + + return found ? max_index + 1 : 0; + } + + void Config::printAll() + { + for (const auto& [key, value] : values_) + { + std::cout << key << " = " << value << std::endl; + } + } - const char* text = element->GetText(); - if (text && std::string(text).find_first_not_of(" \n\t") != std::string::npos) { - values_[key_prefix] = text; + void Config::parseNode(tinyxml2::XMLElement* element, const std::string& key) { + if (const char* text = element->GetText()) + { + if (std::string(text).find_first_not_of(" \n\t") != std::string::npos) + { + values_[key] = text; + } + } + + // Store attributes + for (auto* attr = element->FirstAttribute(); attr; attr = attr->Next()) + { + values_[key + "@" + attr->Name()] = attr->Value(); } - const tinyxml2::XMLAttribute* attr = element->FirstAttribute(); - while (attr) { - std::string attr_key = key_prefix + "@" + attr->Name(); - values_[attr_key] = attr->Value(); - attr = attr->Next(); + // Count children by name + std::unordered_map child_count; + for (auto* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) + { + child_count[child->Name()]++; } - for (tinyxml2::XMLElement* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) { - parseNode(child, key_prefix); + // Index children + std::unordered_map child_index; + + for (auto* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) + { + std::string child_key = key + "." + child->Name(); + + if (child_count[child->Name()] > 1) + { + int index = child_index[child->Name()]++; + child_key += "[" + std::to_string(index) + "]"; + } + + parseNode(child, child_key); } } } \ No newline at end of file diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index 95807742..61f3e270 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -9,12 +9,27 @@ namespace Modelec } double Point::angleDiff(const Point &p1, const Point &p2) { - double diff = std::fmod(p1.theta - p2.theta + M_PI, 2 * M_PI); + return angleDiff(p1.theta, p2.theta); + } + + double Point::angleDiff(const double& p1, const double& p2) + { + double diff = std::fmod(p1 - p2 + M_PI, 2 * M_PI); if (diff < 0) diff += 2 * M_PI; return diff - M_PI; } + double Point::normalizeAngle(double angle) + { + double newAngle = std::fmod(angle + M_PI, 2.0 * M_PI); + if (newAngle < 0) { + newAngle += 2.0 * M_PI; + } + + return newAngle - M_PI; + } + double Point::distance(const Point& p2) const { return distance(*this, p2); @@ -25,6 +40,16 @@ namespace Modelec return angleDiff(*this, p2); } + double Point::angleDiff(const double& p2) const + { + return angleDiff(theta, p2); + } + + void Point::normalizeAngle() + { + theta = normalizeAngle(theta); + } + Point Point::GetTakePosition(int distance, double angle) const { Point pos; diff --git a/src/modelec_utils/test/config.test.cpp b/src/modelec_utils/test/config.test.cpp index f856ca3d..608eea3e 100644 --- a/src/modelec_utils/test/config.test.cpp +++ b/src/modelec_utils/test/config.test.cpp @@ -48,3 +48,69 @@ TEST(ConfigTest, DefaultValues) EXPECT_EQ(Modelec::Config::get("missing.string", "default"), "default"); EXPECT_FALSE(Modelec::Config::get("missing.bool", false)); } + +TEST(ConfigTest, CountArray) +{ + // Create temporary XML file + const std::string filepath = "test_array_config.xml"; + + std::ofstream file(filepath); + file << + "" + " 1" + " 2" + " 3" + ""; + file.close(); + + EXPECT_TRUE(Modelec::Config::load(filepath)); + + EXPECT_EQ(Modelec::Config::count("root.item"), 3); +} + +TEST(ConfigTest, GetArray) +{ + // Create temporary XML file + const std::string filepath = "test_array_config.xml"; + + std::ofstream file(filepath); + file << + "" + " 1" + " 2" + " 3" + ""; + file.close(); + + struct Item + { + int value; + std::string attr; + }; + + EXPECT_TRUE(Modelec::Config::load(filepath)); + + auto array = Modelec::Config::getArray("root.item"); + + EXPECT_EQ(array.size(), 3); + EXPECT_EQ(array[0], 1); + EXPECT_EQ(array[1], 2); + EXPECT_EQ(array[2], 3); + + auto array2 = Modelec::Config::getArray("root.item", + [](const std::string& base) + { + return Item{ + Modelec::Config::get(base, 0), + Modelec::Config::get(base + "@a", "") + }; + }); + + EXPECT_EQ(array2.size(), 3); + EXPECT_EQ(array2[0].value, 1); + EXPECT_EQ(array2[0].attr, "b"); + EXPECT_EQ(array2[1].value, 2); + EXPECT_EQ(array2[1].attr, "c"); + EXPECT_EQ(array2[2].value, 3); + EXPECT_EQ(array2[2].attr, "d"); +} \ No newline at end of file diff --git a/src/modelec_utils/test/point.test.cpp b/src/modelec_utils/test/point.test.cpp index c57f3944..6b7e79da 100644 --- a/src/modelec_utils/test/point.test.cpp +++ b/src/modelec_utils/test/point.test.cpp @@ -33,13 +33,13 @@ TEST(PointTest, GetTakePositionDefaultAngle) { TEST(PointTest, GetTakeBasePosition) { Modelec::Point p(0, 0, 0); Modelec::Point base = p.GetTakeBasePosition(); - EXPECT_EQ(base.x, 290); + EXPECT_EQ(base.x, BASE_DISTANCE); EXPECT_EQ(base.y, 0); } TEST(PointTest, GetTakeClosePosition) { Modelec::Point p(0, 0, 0); Modelec::Point close = p.GetTakeClosePosition(); - EXPECT_EQ(close.x, 165); + EXPECT_EQ(close.x, CLOSE_DISTANCE); EXPECT_EQ(close.y, 0); } diff --git a/start_ros2.sh b/start_ros2.sh index 25ed44d7..5305644e 100755 --- a/start_ros2.sh +++ b/start_ros2.sh @@ -5,8 +5,8 @@ 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 RMW_IMPLEMENTATION=rmw_fastrtps_cpp export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml -export ROS_DOMAIN_ID=128 +#export ROS_DOMAIN_ID=128 -exec ros2 launch modelec_core modelec.launch.py "$@" +exec ros2 launch modelec_core modelec.launch.py with_color_detector:=false "$@"