modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-19 00:57:37 +01:00
Code Issues Packages Projects Releases Wiki Activity

handle a bug where the robot can't go to the zone

Browse Source
This commit is contained in:
acki
2025-05-05 21:54:51 -04:00
parent ccef10fb43
commit f79bd549f5
2 changed files with 44 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/modelec_gui/src/pages/map_page.cpp
View File

@@ -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)

52
src/modelec_strat/src/missions/prepare_concert_mission.cpp
View File

@@ -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<int> 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;