diff --git a/simulated_pcb/alim.py b/simulated_pcb/alim.py index 2a22ad1..9884deb 100644 --- a/simulated_pcb/alim.py +++ b/simulated_pcb/alim.py @@ -4,7 +4,7 @@ import time import serial # Set the parameters for the serial connection -serial_port = '/dev/pts/12' # Modify this to your serial port (e.g., 'COM3' on Windows) +serial_port = '/dev/pts/9' # Modify this to your serial port (e.g., 'COM3' on Windows) baud_rate = 115200 # Modify this to your baud rate # Open the serial connection diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 58e2502..8d4e0ca 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_alim"; request->bauds = 115200; - request->serial_port = "/dev/pts/13"; // TODO : check the real serial port + request->serial_port = "/dev/pts/10"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index fbce313..c040e9d 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -101,5 +101,7 @@ namespace Modelec { rclcpp::Subscription::SharedPtr pos_sub_; rclcpp::Subscription::SharedPtr enemy_pos_sub_; + + modelec_interfaces::msg::OdometryPos last_enemy_pos_; }; } \ No newline at end of file diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index 6f6962b..2f4d64d 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -10,6 +10,7 @@ namespace Modelec void PrepareConcertMission::start(rclcpp::Node::SharedPtr node) { + node_ = node; if (!nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId())) { @@ -17,24 +18,35 @@ namespace Modelec return; } - if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos())) + int timeout = 0; + std::vector blacklistId = {}; + bool canGo = false; + + while (!canGo && timeout < 10) { - column_ = col; - } else - { - status_ = MissionStatus::FINISH_ALL; - return; + if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos(), blacklistId)) + { + column_ = col; + + auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); + + auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL); + if (res != Pathfinding::FREE) + { + blacklistId.push_back(column_->id()); + } + else + { + canGo = true; + } + } + + timeout++; } - node_ = node; - - auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - - auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL); - if (res != Pathfinding::FREE) + if (!canGo) { - RCLCPP_WARN(node_->get_logger(), "Can't go to column %d", column_->id()); - status_ = MissionStatus::FAILED; + status_ = MissionStatus::FINISH_ALL; return; } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 3d6feda..d4baa26 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -277,17 +277,22 @@ namespace Modelec { std::shared_ptr NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId, const std::vector& blacklistedId) { + // TODO : score std::shared_ptr closest_zone = nullptr; - double min_distance = std::numeric_limits::max(); + double score = std::numeric_limits::max(); auto posPoint = Point(pos->x, pos->y, pos->theta); + auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta); + for (const auto& [id, zone] : deposite_zones_) { if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find(blacklistedId.begin(), blacklistedId.end(), id)) { double distance = Point::distance(posPoint, zone->GetPosition()); - if (distance < min_distance) + double enemy_distance = Point::distance(enemyPos, zone->GetPosition()); + double s = distance * 2 + enemy_distance; + if (s < score) { - min_distance = distance; + score = s; closest_zone = zone; } } @@ -318,6 +323,7 @@ namespace Modelec { void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { pathfinding_->OnEnemyPosition(msg); + last_enemy_pos_ = *msg; if (EnemyOnPath(*msg)) { diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index dac7b2e..8157976 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -539,9 +539,11 @@ namespace Modelec std::shared_ptr Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos, const std::vector& blacklistedId) { + // TODO score (count dist and dist with enemy) std::shared_ptr closest_obstacle = nullptr; auto robotPos = Point(pos->x, pos->y, pos->theta); - float distance = std::numeric_limits::max(); + auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta); + float score = std::numeric_limits::max(); for (const auto& [id, obstacle] : obstacle_map_) { @@ -553,9 +555,13 @@ namespace Modelec for (auto possiblePos : obs->GetAllPossiblePositions()) { auto dist = Point::distance(robotPos, possiblePos); - if (dist < distance) + auto distEnemy = Point::distance(enemyPos, possiblePos); + + auto s = dist * 2 + distEnemy; + + if (s < score) { - distance = dist; + score = s; closest_obstacle = obs; } }