From 913286bd2d76087a7f3a19b056e4fa7ee567b6df Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 30 Apr 2025 22:42:56 -0400 Subject: [PATCH] =?UTF-8?q?STRAT=20v2=20:=20-=20d=C3=A9but=20de=20strat=20?= =?UTF-8?q?r=C3=A9el?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modelec_core/CMakeLists.txt | 9 -- .../include/modelec/move_controller.hpp | 21 --- src/modelec_core/src/move_controller.cpp | 29 ---- .../include/modelec_gui/pages/map_page.hpp | 4 + src/modelec_gui/src/pages/map_page.cpp | 50 +++--- src/modelec_interfaces/CMakeLists.txt | 1 + .../msg/Strat/StratState.msg | 12 ++ .../modelec_sensors/lidar_controller.hpp | 4 +- .../modelec_sensors/tirette_controller.hpp | 9 +- src/modelec_sensors/src/lidar_controller.cpp | 1 + .../src/tirette_controller.cpp | 28 ++-- src/modelec_strat/CMakeLists.txt | 12 +- .../include/modelec_strat/enemy_manager.hpp | 35 ++++ .../missions/go_home_mission.hpp | 24 +++ .../modelec_strat/missions/mission_base.hpp | 25 +++ .../missions/prepare_concert_mission.hpp | 31 ++++ .../missions/promotion_mission.hpp | 33 ++++ .../include/modelec_strat/pathfinding.hpp | 42 +++-- .../include/modelec_strat/strat_fms.hpp | 46 ++++-- src/modelec_strat/src/enemy_manager.cpp | 150 ++++++++++++++++++ src/modelec_strat/src/mission_manager.cpp | 13 ++ .../src/missions/go_home_mission.cpp | 30 ++++ .../src/missions/prepare_concert_mission.cpp | 52 ++++++ .../src/missions/promotion_mission.cpp | 24 +++ src/modelec_strat/src/navigation_helper.cpp | 2 + src/modelec_strat/src/pathfinding.cpp | 124 ++++++++++++++- src/modelec_strat/src/strat_fms.cpp | 124 ++++++++++++++- 27 files changed, 793 insertions(+), 142 deletions(-) delete mode 100644 src/modelec_core/include/modelec/move_controller.hpp delete mode 100644 src/modelec_core/src/move_controller.cpp create mode 100644 src/modelec_interfaces/msg/Strat/StratState.msg create mode 100644 src/modelec_strat/include/modelec_strat/enemy_manager.hpp create mode 100644 src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp create mode 100644 src/modelec_strat/include/modelec_strat/missions/mission_base.hpp create mode 100644 src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp create mode 100644 src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp create mode 100644 src/modelec_strat/src/enemy_manager.cpp create mode 100644 src/modelec_strat/src/missions/go_home_mission.cpp create mode 100644 src/modelec_strat/src/missions/prepare_concert_mission.cpp create mode 100644 src/modelec_strat/src/missions/promotion_mission.cpp diff --git a/src/modelec_core/CMakeLists.txt b/src/modelec_core/CMakeLists.txt index ea4c91a..f0dc3cb 100644 --- a/src/modelec_core/CMakeLists.txt +++ b/src/modelec_core/CMakeLists.txt @@ -26,14 +26,6 @@ target_include_directories(gamecontroller_listener PUBLIC $ ) -add_executable(move_controller src/move_controller.cpp) -ament_target_dependencies(move_controller rclcpp std_msgs sensor_msgs modelec_interfaces) -target_link_libraries(move_controller modelec_utils::modelec_utils) -target_include_directories(move_controller PUBLIC - $ - $ -) - add_executable(solenoid_controller src/solenoid_controller.cpp) ament_target_dependencies(solenoid_controller rclcpp modelec_interfaces) target_link_libraries(solenoid_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils) @@ -77,7 +69,6 @@ endif() # Install targets install(TARGETS gamecontroller_listener - move_controller solenoid_controller arm_controller button_gpio_controller diff --git a/src/modelec_core/include/modelec/move_controller.hpp b/src/modelec_core/include/modelec/move_controller.hpp deleted file mode 100644 index 2a49a1e..0000000 --- a/src/modelec_core/include/modelec/move_controller.hpp +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include -#include - -#define REFRESH_RATE 100 - -namespace Modelec { - class MoveController : public rclcpp::Node { - public: - MoveController(); - - private: - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - - float x = 0, y = 0, theta = 0; - - void PublishPosition(); - }; -} \ No newline at end of file diff --git a/src/modelec_core/src/move_controller.cpp b/src/modelec_core/src/move_controller.cpp deleted file mode 100644 index e74dc0a..0000000 --- a/src/modelec_core/src/move_controller.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include - -namespace Modelec { - MoveController::MoveController() : Node("move_controller") - { - // Initialize the publisher - publisher_ = this->create_publisher("robot_position", 10); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(REFRESH_RATE), - std::bind(&MoveController::PublishPosition, this) - ); - } - - void MoveController::PublishPosition() - { - std_msgs::msg::String msg; - msg.data = std::to_string(x) + ";" + std::to_string(y) + ";" + std::to_string(theta); - publisher_->publish(msg); - } -} - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file 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 74289b0..b0645f4 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -69,5 +69,9 @@ namespace ModelecGUI { rclcpp::Client::SharedPtr map_client_; rclcpp::Subscription::SharedPtr obstacle_sub_; rclcpp::Client::SharedPtr ask_map_obstacle_client_; + + + modelec_interfaces::msg::OdometryPos enemy_pos_; + rclcpp::Publisher::SharedPtr enemy_pos_pub_; }; } diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index ad4759c..7b9be0d 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -22,6 +22,10 @@ namespace ModelecGUI qpoints = {}; points = {}; + enemy_pos_.x = 1775; + enemy_pos_.y = 200; + enemy_pos_.theta = 3.1415/2; + add_waypoint_sub_ = node_->create_subscription("odometry/add_waypoint", 100, [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) { if (lastWapointWasEnd) @@ -57,6 +61,8 @@ namespace ModelecGUI go_to_pub_ = node_->create_publisher("nav/go_to", 10); + enemy_pos_pub_ = node_->create_publisher("enemy/position", 10); + // client to nav/map map_client_ = node_->create_client("nav/map_size"); while (!map_client_->wait_for_service(std::chrono::seconds(1))) { @@ -123,7 +129,6 @@ namespace ModelecGUI QPainter painter(this); renderer->render(&painter, rect()); // Scales SVG to widget size - painter.setRenderHint(QPainter::Antialiasing); // --- Draw lines --- @@ -160,6 +165,9 @@ namespace ModelecGUI painter.setBrush(Qt::black); painter.drawRect(obs.x * ratioBetweenMapAndWidget, height() - (obs.y + obs.height) * ratioBetweenMapAndWidget, obs.width * ratioBetweenMapAndWidget, obs.height * ratioBetweenMapAndWidget); } + + painter.setBrush(Qt::red); + painter.drawRect((enemy_pos_.x - 150.0f) * ratioBetweenMapAndWidget, height() - (enemy_pos_.y + 150.0f) * ratioBetweenMapAndWidget, 300.0f*ratioBetweenMapAndWidget, 300.0f*ratioBetweenMapAndWidget); } } @@ -167,37 +175,23 @@ namespace ModelecGUI { QWidget::mousePressEvent(event); - modelec_interfaces::msg::OdometryPos msg; - msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000); - msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000); - msg.theta = 0; - - go_to_pub_->publish(msg); - - /* - qpoints.push_back(event->pos()); - - modelec_interfaces::msg::OdometryAddWaypoint msg; - msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000); - msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000); - msg.is_end = false; - msg.id = points.size(); - - if (points.empty()) + if (event->button() == Qt::LeftButton) { - QPointF vec = QPoint(msg.x, msg.y) - robotPosPoint; - msg.theta = std::atan2(vec.y(), vec.x()); + modelec_interfaces::msg::OdometryPos msg; + msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000); + msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000); + msg.theta = 0; + + go_to_pub_->publish(msg); } - else + else if (event->button() == Qt::RightButton) { - auto lastPoint = points.back(); - QPointF vec = QPoint(msg.x, msg.y) - QPoint(lastPoint.x, lastPoint.y); - msg.theta = std::atan2(vec.y(), vec.x()); + enemy_pos_.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000); + enemy_pos_.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000); + enemy_pos_.theta = 0; + + enemy_pos_pub_->publish(enemy_pos_); } - - points.push_back(msg); - - update();*/ } void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) diff --git a/src/modelec_interfaces/CMakeLists.txt b/src/modelec_interfaces/CMakeLists.txt index 139425c..fd0a699 100644 --- a/src/modelec_interfaces/CMakeLists.txt +++ b/src/modelec_interfaces/CMakeLists.txt @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Odometry/OdometryAddWaypoint.msg" "msg/Odometry/OdometryStart.msg" "msg/Odometry/PID/OdometryPid.msg" + "msg/Strat/StratState.msg" "msg/Map/Map.msg" "msg/Map/Obstacle.msg" "msg/PCA9685Servo.msg" diff --git a/src/modelec_interfaces/msg/Strat/StratState.msg b/src/modelec_interfaces/msg/Strat/StratState.msg new file mode 100644 index 0000000..f81a2e1 --- /dev/null +++ b/src/modelec_interfaces/msg/Strat/StratState.msg @@ -0,0 +1,12 @@ +int32 INIT=0 +int32 WAIT_START=1 +int32 SELECT_MISSION=2 + +int32 DO_PREPARE_CONCERT=3 +int32 DO_PROMOTION=4 + +int32 DO_GO_HOME=5 +int32 STOP=6 + +int32 state +string reason \ No newline at end of file diff --git a/src/modelec_sensors/include/modelec_sensors/lidar_controller.hpp b/src/modelec_sensors/include/modelec_sensors/lidar_controller.hpp index afeee04..5e187b3 100644 --- a/src/modelec_sensors/include/modelec_sensors/lidar_controller.hpp +++ b/src/modelec_sensors/include/modelec_sensors/lidar_controller.hpp @@ -5,9 +5,9 @@ #include namespace Modelec { - class LidarController : public rclcpp::Node { - private: + // TODO - DEPRECATED + class LidarController : public rclcpp::Node { public: LidarController(); private: diff --git a/src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp b/src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp index 1c0835d..7f1a09c 100644 --- a/src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp +++ b/src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp @@ -16,10 +16,11 @@ namespace Modelec { TiretteController(); private: - rclcpp::Service::SharedPtr service; - rclcpp::TimerBase::SharedPtr timer; - rclcpp::Publisher::SharedPtr publisher; - bool tirette_state; + rclcpp::Service::SharedPtr service_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_change_; + int tirette_state_; void check_tirette(const std::shared_ptr request, std::shared_ptr response); }; diff --git a/src/modelec_sensors/src/lidar_controller.cpp b/src/modelec_sensors/src/lidar_controller.cpp index c74b927..fb7f5d0 100644 --- a/src/modelec_sensors/src/lidar_controller.cpp +++ b/src/modelec_sensors/src/lidar_controller.cpp @@ -1,6 +1,7 @@ #include namespace Modelec { + // TODO - DEPRECATED LidarController::LidarController() : Node("lidar_controller") { subscription_ = this->create_subscription( "scan", 10, [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) { diff --git a/src/modelec_sensors/src/tirette_controller.cpp b/src/modelec_sensors/src/tirette_controller.cpp index bd97b92..2524238 100644 --- a/src/modelec_sensors/src/tirette_controller.cpp +++ b/src/modelec_sensors/src/tirette_controller.cpp @@ -14,33 +14,37 @@ namespace Modelec { pullUpDnControl(GPIO_TIRETTE, PUD_UP); - tirette_state = false; + tirette_state_ = 0; // Initialize the service - service = this->create_service( + service_ = this->create_service( "tirette", std::bind(&TiretteController::check_tirette, this, std::placeholders::_1, std::placeholders::_2)); // Initialize the publisher - publisher = this->create_publisher("tirette_state", 10); + publisher_ = this->create_publisher("tirette", 10); + publisher_change_ = this->create_publisher("tirette/continue", 10); // Initialize the timer - timer = this->create_wall_timer(std::chrono::milliseconds(REFRESH_RATE), [this]() { - // TODO : change that to publish in continue (so change the main program) - bool lastState = tirette_state; - tirette_state = digitalRead(GPIO_TIRETTE) == LOW; + timer_ = this->create_wall_timer(std::chrono::milliseconds(REFRESH_RATE), [this]() { + // TODO : CHECK THAT + bool lastState = tirette_state_; + tirette_state_ = digitalRead(GPIO_TIRETTE); - if (lastState == LOW && tirette_state == HIGH) { - auto msg = std_msgs::msg::Bool(); - msg.data = tirette_state; - publisher->publish(msg); + auto msg = std_msgs::msg::Bool(); + msg.data = tirette_state_; + + if (lastState == LOW && tirette_state_ == HIGH) { + publisher_->publish(msg); } + + publisher_change_->publish(msg); }); } void TiretteController::check_tirette(const std::shared_ptr, std::shared_ptr response) { - response->state = tirette_state; + response->state = tirette_state_; } } diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 04a00c2..00fa44e 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -7,6 +7,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tinyxml2 REQUIRED) find_package(rclcpp REQUIRED) @@ -14,7 +15,7 @@ find_package(rclcpp REQUIRED) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) -add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp) +add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp) ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces) target_link_libraries(strat_fsm modelec_utils::modelec_utils tinyxml2::tinyxml2) target_include_directories(strat_fsm PUBLIC @@ -22,6 +23,14 @@ target_include_directories(strat_fsm PUBLIC $ ) +add_executable(enemy_manager src/enemy_manager.cpp) +ament_target_dependencies(enemy_manager rclcpp std_msgs sensor_msgs std_srvs modelec_interfaces) +target_link_libraries(enemy_manager modelec_utils::modelec_utils) +target_include_directories(enemy_manager PUBLIC + $ + $ +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) @@ -31,6 +40,7 @@ endif() install(TARGETS strat_fsm + enemy_manager DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp new file mode 100644 index 0000000..ce6c148 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include + +#include +#include + +namespace Modelec +{ + class EnemyManager : public rclcpp::Node + { + public: + EnemyManager(); + + protected: + void OnCurrentPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + + void OnLidarData(const sensor_msgs::msg::LaserScan::SharedPtr msg); + + void TimerCallback(); + + private: + + rclcpp::Subscription::SharedPtr current_pos_sub_; + rclcpp::Subscription::SharedPtr laser_scan_sub_; + rclcpp::Publisher::SharedPtr enemy_pos_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + modelec_interfaces::msg::OdometryPos current_pos_; + + modelec_interfaces::msg::OdometryPos last_enemy_pos_; + bool enemy_initialized_ = false; + rclcpp::Time last_publish_time_; + }; +} diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp new file mode 100644 index 0000000..6be662f --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include + +#include "mission_base.hpp" + +namespace Modelec +{ + class GoHomeMission : public Mission + { + public: + GoHomeMission(const std::shared_ptr& nav); + + void start(rclcpp::Node::SharedPtr node) override; + void update() override; + MissionStatus getStatus() const override; + std::string name() const override { return "GoHome"; } + + private: + MissionStatus status_; + std::shared_ptr nav_; + rclcpp::Node::SharedPtr node_; + }; +} diff --git a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp new file mode 100644 index 0000000..0a76550 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +namespace Modelec +{ + enum class MissionStatus + { + READY, + RUNNING, + DONE, + FAILED + }; + + class Mission + { + public: + virtual ~Mission() = default; + virtual void start(rclcpp::Node::SharedPtr node) = 0; + virtual void update() = 0; + virtual MissionStatus getStatus() const = 0; + virtual std::string name() const = 0; + }; +} diff --git a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp new file mode 100644 index 0000000..94f210f --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include + +namespace Modelec { + + class PrepareConcertMission : public Mission { + public: + PrepareConcertMission(const std::shared_ptr& nav); + + void start(rclcpp::Node::SharedPtr node) override; + void update() override; + MissionStatus getStatus() const override; + std::string name() const override { return "PrepareConcert"; } + + private: + enum Step { + GO_TO_COLUMN, + TAKE_COLUMN, + GO_TO_PLATFORM, + PLACE_PLATFORM, + DONE + } step_; + + MissionStatus status_; + std::shared_ptr nav_; + rclcpp::Node::SharedPtr node_; + }; + +} diff --git a/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp new file mode 100644 index 0000000..4f44a15 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include + +#include "mission_base.hpp" + +namespace Modelec +{ + /* + * Banderole mission + * + */ + class PromotionMission : public Mission + { + public: + PromotionMission(const std::shared_ptr& nav); + + void start(rclcpp::Node::SharedPtr node) override; + void update() override; + MissionStatus getStatus() const override; + std::string name() const override { return "Promotion"; } + + enum Step { + GO_TO_FRONT, + DEPLOY_BANNER, + DONE + } step_; + + MissionStatus status_; + std::shared_ptr nav_; + rclcpp::Node::SharedPtr node_; + }; +} diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index 7ad608c..ea686de 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -34,13 +34,6 @@ namespace Modelec { WaypointMsg toMsg() const; }; - /* - * - Pathfinding DStar Lite - * - * - * - */ - class Pathfinding { public: Pathfinding(); @@ -54,6 +47,10 @@ namespace Modelec { bool LoadObstaclesFromXML(const std::string& filename); + //void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal); + + void SetCurrentPos(const PosMsg::SharedPtr& pos); + protected: void HandleMapRequest( const std::shared_ptr request, @@ -67,25 +64,38 @@ namespace Modelec { const std::shared_ptr request, const std::shared_ptr response); - private: - int robot_width_mm_; - int robot_length_mm_; + void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); - int grid_width_; - int grid_height_; + //bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos& enemy_pos); + //void Replan(); + + private: + rclcpp::Node::SharedPtr node_; std::vector> grid_; + int grid_width_ = 0; + int grid_height_ = 0; + int map_width_mm_ = 0; + int map_height_mm_ = 0; + float robot_width_mm_ = 0; + float robot_length_mm_ = 0; std::map obstacle_map_; - rclcpp::Node::SharedPtr node_; + PosMsg::SharedPtr current_start_; + PosMsg::SharedPtr current_goal_; + WaypointListMsg current_waypoints_; + + modelec_interfaces::msg::OdometryPos last_enemy_pos_; + bool has_enemy_pos_ = false; + + rclcpp::Subscription::SharedPtr enemy_pos_sub_; + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr waypoint_pub_; rclcpp::Service::SharedPtr map_srv_; rclcpp::Service::SharedPtr map_size_srv_; rclcpp::Service::SharedPtr ask_obstacle_srv_; - - rclcpp::Publisher::SharedPtr map_pub_; - }; } diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 0fcf12c..902784a 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -5,9 +5,29 @@ #include "action_executor.hpp" #include "mission_manager.hpp" #include "navigation_helper.hpp" +#include "missions/mission_base.hpp" +#include "missions/promotion_mission.hpp" +#include "missions/prepare_concert_mission.hpp" +#include "missions/go_home_mission.hpp" + +#include "std_msgs/msg/bool.hpp" + +#include "modelec_interfaces/msg/strat_state.hpp" namespace Modelec { + enum class State + { + INIT, + WAIT_START, + SELECT_MISSION, + + DO_PREPARE_CONCERT, + DO_PROMOTION, + + DO_GO_HOME, + STOP + }; /* * TODO @@ -15,15 +35,6 @@ namespace Modelec * - setup des missions * - gestion des interruptions (robot adverses / obstacles) * - scoring des missions - * - * - * - * - Banderole - * - * - * - * - * - Gradin */ class StratFMS : public rclcpp::Node @@ -33,12 +44,23 @@ namespace Modelec void Init(); - private: - // rclcpp::TimerBase::SharedPtr timer_; + protected: + void transition(State next, const std::string& reason); - std::unique_ptr nav_; + void update(); + + private: + State state_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time match_start_time_; + bool started_ = false; + std::unique_ptr current_mission_; + + std::shared_ptr nav_; std::unique_ptr mission_manager_; std::unique_ptr action_executor_; + rclcpp::Subscription::SharedPtr tirette_sub_; + rclcpp::Publisher::SharedPtr state_pub_; }; } diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp new file mode 100644 index 0000000..97a6172 --- /dev/null +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -0,0 +1,150 @@ +#include +#include + +namespace Modelec +{ + EnemyManager::EnemyManager() : Node("enemy_manager") + { + current_pos_sub_ = this->create_subscription( + "odometry/position", 10, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + OnCurrentPos(msg); + }); + + laser_scan_sub_ = this->create_subscription( + "scan", 10, + [this](const sensor_msgs::msg::LaserScan::SharedPtr msg) { + OnLidarData(msg); + }); + + enemy_pos_pub_ = this->create_publisher( + "enemy/position", 10); + + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&EnemyManager::TimerCallback, this) + ); + + last_publish_time_ = this->now(); + } + + void EnemyManager::OnCurrentPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + current_pos_ = *msg; + } + + void EnemyManager::OnLidarData(const sensor_msgs::msg::LaserScan::SharedPtr msg) + { + if (std::isnan(current_pos_.x) || std::isnan(current_pos_.y)) + { + RCLCPP_WARN(this->get_logger(), "Current robot position unknown, cannot compute enemy position"); + return; + } + + const double robot_x = current_pos_.x; + const double robot_y = current_pos_.y; + const double robot_theta = current_pos_.theta; + + double angle = msg->angle_min; + + double min_distance = std::numeric_limits::max(); + double best_x = 0.0; + double best_y = 0.0; + + for (size_t i = 0; i < msg->ranges.size(); ++i) + { + float range = msg->ranges[i]; + + if (std::isnan(range) || range < msg->range_min || range > msg->range_max) + { + angle += msg->angle_increment; + continue; + } + + // Convert to local robot frame + double x_local = range * std::cos(angle) * 1000.0; // meters -> mm + double y_local = range * std::sin(angle) * 1000.0; // meters -> mm + + // Rotate + translate into global frame + double x_global = robot_x + (x_local * std::cos(robot_theta) - y_local * std::sin(robot_theta)); + double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta)); + + // Ignore points outside of the table + if (x_global < 0 || x_global > 3000 || y_global < 0 || y_global > 2000) + { + angle += msg->angle_increment; + continue; + } + + if (range < min_distance) + { + min_distance = range; + best_x = x_global; + best_y = y_global; + } + + angle += msg->angle_increment; + } + + if (min_distance < std::numeric_limits::max()) + { + modelec_interfaces::msg::OdometryPos enemy_pos; + enemy_pos.x = best_x; + enemy_pos.y = best_y; + enemy_pos.theta = 0.0; + + bool need_publish = false; + + if (!enemy_initialized_) + { + need_publish = true; + enemy_initialized_ = true; + } + else + { + float dx = enemy_pos.x - last_enemy_pos_.x; + float dy = enemy_pos.y - last_enemy_pos_.y; + float distance_squared = dx * dx + dy * dy; + + constexpr float min_move_threshold_mm = 50.0f; + if (distance_squared > min_move_threshold_mm * min_move_threshold_mm) + { + need_publish = true; + } + } + + if (need_publish) + { + last_enemy_pos_ = enemy_pos; + last_publish_time_ = this->now(); + enemy_pos_pub_->publish(enemy_pos); + RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%ld, y=%ld", enemy_pos.x, enemy_pos.y); + } + } + else + { + RCLCPP_INFO(this->get_logger(), "No enemy detected in Lidar scan"); + } + } + + void EnemyManager::TimerCallback() + { + if (!enemy_initialized_) + return; + + rclcpp::Duration duration_since_last = this->now() - last_publish_time_; + if (duration_since_last.seconds() >= 2.0) + { + enemy_pos_pub_->publish(last_enemy_pos_); + last_publish_time_ = this->now(); + RCLCPP_INFO(this->get_logger(), "Periodic refresh of enemy position"); + } + } +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/modelec_strat/src/mission_manager.cpp b/src/modelec_strat/src/mission_manager.cpp index 1094808..cb43054 100644 --- a/src/modelec_strat/src/mission_manager.cpp +++ b/src/modelec_strat/src/mission_manager.cpp @@ -2,6 +2,19 @@ namespace Modelec { + enum class State { + INIT, // Initialisation des capteurs, timers, etc. + WAIT_START, // Attente du feu vert + SELECT_MISSION, // Choix intelligent de la prochaine mission + + RETURN_HOME, // Retour zone de fin + FAILSAFE, // En cas d'erreur bloquante + STOP, // Fin du match + + GO_TO_OBJECTIVE, // Navigation vers la cible + EXECUTE_MISSION, // Exécution de l'action + }; + MissionManager::MissionManager() { } diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp new file mode 100644 index 0000000..542c087 --- /dev/null +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -0,0 +1,30 @@ +#include + +namespace Modelec +{ + GoHomeMission::GoHomeMission(const std::shared_ptr& nav) : status_(MissionStatus::READY), nav_(nav) + { + } + + void GoHomeMission::start(rclcpp::Node::SharedPtr node) + { + node_ = node; + + // go home + + status_ = MissionStatus::RUNNING; + } + + void GoHomeMission::update() + { + if (nav_->HasArrived()) + { + status_ = MissionStatus::DONE; + } + } + + MissionStatus GoHomeMission::getStatus() const + { + return status_; + } +} diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp new file mode 100644 index 0000000..fd32746 --- /dev/null +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -0,0 +1,52 @@ +#include + +namespace Modelec +{ + PrepareConcertMission::PrepareConcertMission(const std::shared_ptr& nav) : step_(GO_TO_COLUMN), status_(MissionStatus::READY), nav_(nav) + { + } + + void PrepareConcertMission::start(rclcpp::Node::SharedPtr node) + { + node_ = node; + + + + status_ = MissionStatus::RUNNING; + } + + void PrepareConcertMission::update() + { + if (status_ != MissionStatus::RUNNING) return; + + if (!nav_->HasArrived()) return; + + switch (step_) + { + case GO_TO_COLUMN: + step_ = TAKE_COLUMN; + break; + + case TAKE_COLUMN: + step_ = GO_TO_PLATFORM; + break; + + case GO_TO_PLATFORM: + step_ = PLACE_PLATFORM; + break; + + case PLACE_PLATFORM: + step_ = DONE; + status_ = MissionStatus::DONE; + break; + + default: + break; + } + } + + MissionStatus PrepareConcertMission::getStatus() const + { + return status_; + } +} diff --git a/src/modelec_strat/src/missions/promotion_mission.cpp b/src/modelec_strat/src/missions/promotion_mission.cpp new file mode 100644 index 0000000..76f6829 --- /dev/null +++ b/src/modelec_strat/src/missions/promotion_mission.cpp @@ -0,0 +1,24 @@ +#include + +namespace Modelec +{ + PromotionMission::PromotionMission(const std::shared_ptr& nav) : step_(GO_TO_FRONT), status_(MissionStatus::READY), nav_(nav) + { + } + + void PromotionMission::start(rclcpp::Node::SharedPtr node) + { + node_ = node; + + status_ = MissionStatus::RUNNING; + } + + void PromotionMission::update() + { + } + + MissionStatus PromotionMission::getStatus() const + { + return status_; + } +} diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 6ba6c5a..cd2556e 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -157,6 +157,7 @@ namespace Modelec { void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal) { SendWaypoint(FindPath(goal)); + //pathfinding_->SetStartAndGoal(current_pos_, goal); } WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal) @@ -178,6 +179,7 @@ namespace Modelec { void NavigationHelper::OnPos(const PosMsg::SharedPtr msg) { current_pos_ = msg; + pathfinding_->SetCurrentPos(msg); } } diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index bc14cc3..8286d33 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -27,6 +27,9 @@ namespace Modelec { Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr& node) : node_(node) { + map_width_mm_ = 3000.0f; + map_height_mm_ = 2000.0f; + robot_length_mm_ = 300.0f; robot_width_mm_ = 300.0f; @@ -65,6 +68,14 @@ namespace Modelec { HandleMapSizeRequest(request, response); }); + enemy_pos_sub_ = node_->create_subscription( + "enemy/position", 10, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + OnEnemyPosition(msg); + }); + + waypoint_pub_ = node_->create_publisher( + "odometry/add_waypoint", 100); } rclcpp::Node::SharedPtr Pathfinding::getNode() const @@ -72,15 +83,47 @@ namespace Modelec { return node_; } + /*bool Pathfinding::EnemyOnPath(const modelec_interfaces::msg::OdometryPos& enemy_pos) + { + int ex = enemy_pos.x; + int ey = enemy_pos.y; + + for (const auto& wp : current_waypoints_) + { + int wx = wp.x; + int wy = wp.y; + + if (std::abs(wx - ex) + std::abs(wy - ey) <= 300) + return true; + } + return false; + }*/ + + /*void Pathfinding::Replan() + { + if (!current_start_ || !current_goal_) + return; + + current_waypoints_ = FindPath(current_start_, current_goal_); + + for (const auto& wp : current_waypoints_) + { + waypoint_pub_->publish(wp); + } + }*/ + WaypointListMsg Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal) { + if (!start || !goal) + { + RCLCPP_WARN(node_->get_logger(), "Start or Goal position is null"); + return WaypointListMsg(); + } + WaypointListMsg waypoints; - // Constantes de ta map - constexpr float map_width_mm = 3000.0f; // mm - constexpr float map_height_mm = 2000.0f; // mm - const float cell_size_mm_x = map_width_mm / grid_width_; - const float cell_size_mm_y = map_height_mm / grid_height_; + const float cell_size_mm_x = map_width_mm_ / grid_width_; + const float cell_size_mm_y = map_height_mm_ / grid_height_; // Robot dimensions const int inflate_x = std::ceil((robot_width_mm_ / 2.0f) / cell_size_mm_x); @@ -94,6 +137,46 @@ namespace Modelec { row.assign(grid_width_, 0); // 0 = free } + if (has_enemy_pos_) + { + int ex = (last_enemy_pos_.x / cell_size_mm_x) - inflate_x; + int ey = (grid_height_ - 1) - ((last_enemy_pos_.y / cell_size_mm_y) - inflate_y); + + const int inflate_enemy_x = std::ceil(150.0 / cell_size_mm_x + inflate_x); + const int inflate_enemy_y = std::ceil(150.0 / cell_size_mm_y + inflate_y); + + for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y) + { + for (int x = ex - inflate_enemy_x; x <= ex + inflate_enemy_x; ++x) + { + if (x >= 0 && y >= 0 && x < grid_width_ && y < grid_height_) + { + grid_[y][x] = 1; + } + } + } + } + + // Bord gauche et droit + for (int y = 0; y < grid_height_; ++y) + { + for (int x = 0; x < inflate_x; ++x) + { + grid_[y][x] = 1; // bord gauche + grid_[y][grid_width_ - 1 - x] = 1; // bord droit + } + } + + // Bord haut et bas + for (int x = 0; x < grid_width_; ++x) + { + for (int y = 0; y < inflate_y; ++y) + { + grid_[y][x] = 1; // bord bas + grid_[grid_height_ - 1 - y][x] = 1; // bord haut + } + } + // 2. Fill obstacles with inflation for (const auto& [id, obstacle] : obstacle_map_) { @@ -353,6 +436,23 @@ namespace Modelec { return true; } + /*void Pathfinding::SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal) + { + current_start_ = start; + current_goal_ = goal; + current_waypoints_ = FindPath(start, goal); + + for (const auto& wp : current_waypoints_) + { + waypoint_pub_->publish(wp); + } + }*/ + + void Pathfinding::SetCurrentPos(const PosMsg::SharedPtr& pos) + { + current_start_ = pos; + } + void Pathfinding::HandleMapRequest(const std::shared_ptr, const std::shared_ptr response) { @@ -384,6 +484,20 @@ namespace Modelec { } } + void Pathfinding::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) + { + last_enemy_pos_ = *msg; + has_enemy_pos_ = true; + + RCLCPP_INFO(node_->get_logger(), "Enemy position updated: x=%ld, y=%ld", msg->x, msg->y); + + /*if (EnemyOnPath(last_enemy_pos_)) + { + RCLCPP_INFO(node_->get_logger(), "Enemy is blocking the path, replanning..."); + Replan(); + }*/ + } + Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast) { id = index; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index bc5baa4..ca17649 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -2,20 +2,138 @@ namespace Modelec { - StratFMS::StratFMS() : Node("start_fms") + + StratFMS::StratFMS() : Node("start_fms"), state_(State::INIT) { + tirette_sub_ = create_subscription( + "/tirette", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) + { + if (!started_ && msg->data) + { + started_ = true; + } + }); + + state_pub_ = create_publisher("/strat/state", 10); } void StratFMS::Init() { - nav_ = std::make_unique(shared_from_this()); + nav_ = std::make_shared(shared_from_this()); mission_manager_ = std::make_unique(shared_from_this()); action_executor_ = std::make_unique(shared_from_this()); RCLCPP_INFO(this->get_logger(), "StratFMS fully initialized"); + + timer_ = create_wall_timer(std::chrono::milliseconds(100), [this] + { + update(); + }); + } + + void StratFMS::transition(State next, const std::string& reason) + { + RCLCPP_INFO(get_logger(), "Transition %d -> %d: %s", static_cast(state_), static_cast(next), + reason.c_str()); + state_ = next; + modelec_interfaces::msg::StratState msg; + msg.state = static_cast(state_); + msg.reason = reason; + state_pub_->publish(msg); + } + + void StratFMS::update() + { + auto now = this->now(); + + switch (state_) + { + case State::INIT: + RCLCPP_INFO_ONCE(get_logger(), "State: INIT"); + transition(State::WAIT_START, "System ready"); + break; + + case State::WAIT_START: + RCLCPP_INFO_ONCE(get_logger(), "State: WAIT_START"); + if (started_) + { + match_start_time_ = now; + transition(State::SELECT_MISSION, "Match started"); + } + break; + + case State::SELECT_MISSION: + { + auto elapsed = now - match_start_time_; + // select mission in a good way there. + if (elapsed.seconds() < 10) + { + transition(State::DO_PROMOTION, "Start by preparing the concert"); + } + else if (elapsed.seconds() < 30) + { + transition(State::DO_PROMOTION, "Proceed to promotion"); + } + else if (elapsed.seconds() >= 100.0) + { + transition(State::DO_GO_HOME, "Cleanup and finish match"); + } + else + { + transition(State::STOP, "Nothing more to do"); + } + break; + } + + case State::DO_PREPARE_CONCERT: + if (!current_mission_) + { + current_mission_ = std::make_unique(nav_); + current_mission_->start(shared_from_this()); + } + current_mission_->update(); + if (current_mission_->getStatus() == MissionStatus::DONE) + { + current_mission_.reset(); + transition(State::SELECT_MISSION, "PrepareConcert finished"); + } + break; + + case State::DO_PROMOTION: + if (!current_mission_) + { + current_mission_ = std::make_unique(nav_); + current_mission_->start(shared_from_this()); + } + current_mission_->update(); + if (current_mission_->getStatus() == MissionStatus::DONE) + { + current_mission_.reset(); + transition(State::SELECT_MISSION, "Promotion finished"); + } + break; + + case State::DO_GO_HOME: + if (!current_mission_) + { + current_mission_ = std::make_unique(nav_); + current_mission_->start(shared_from_this()); + } + current_mission_->update(); + if (current_mission_->getStatus() == MissionStatus::DONE) + { + current_mission_.reset(); + transition(State::STOP, "Cleanup done"); + } + break; + + case State::STOP: + RCLCPP_INFO_ONCE(get_logger(), "State: STOP - Match finished"); + break; + } } } -int main(int argc, char **argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared();