diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 589b89d..ceccd73 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -271,14 +271,11 @@ namespace ModelecGUI { QWidget::mouseMoveEvent(event); - if (event->button() == Qt::RightButton) - { - 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_.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_); - } + enemy_pos_pub_->publish(enemy_pos_); } void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index 9104b9c..7067387 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -76,7 +76,6 @@ namespace Modelec break; case TAKE_COLUMN: { - // TODO closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId()); if (!closestDepoZone_) { @@ -104,23 +103,52 @@ namespace Modelec break; case GO_BACK: { - closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId()); - if (!closestDepoZone_) + + bool canGo = false; + std::vector blacklistId = {}; + int timeout = 0; + + while (!canGo && timeout < 3) + { + if (auto zone = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId(), blacklistId)) + { + closestDepoZone_ = zone; + closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos(); + auto p = closestDepoZonePoint_.GetTakeBasePosition(); + + if (nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE) + { + if (nav_->GoToRotateFirst(p, true, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE) + { + blacklistId.push_back(closestDepoZone_->GetId()); + } + else + { + canGo = true; + } + } + else + { + nav_->GoToRotateFirst(p, false, Pathfinding::FREE | Pathfinding::WALL); + canGo = true; + } + } + else + { + status_ = MissionStatus::FAILED; + return; + } + + timeout++; + } + + if (!canGo) { status_ = MissionStatus::FAILED; return; } closestDepoZonePoint_ = closestDepoZone_->ValidNextPotPos(); - auto p = closestDepoZonePoint_.GetTakeBasePosition(); - if (nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE) - { - nav_->GoToRotateFirst(p, true, Pathfinding::FREE | Pathfinding::WALL); - } - else - { - nav_->GoToRotateFirst(p, false, Pathfinding::FREE | Pathfinding::WALL); - } } step_ = GO_TO_PLATFORM;