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

some patch

Browse Source
This commit is contained in:
acki
2025-05-28 08:04:38 -04:00
parent 03ecb3acff
commit 6a22e0c44a
4 changed files with 19 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -48,6 +48,7 @@ namespace Modelec
auto res = nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL);
if (res != Pathfinding::FREE)
{
RCLCPP_WARN(node_->get_logger(), "Cannot go to column %d, trying another one", column_->GetId());
blacklistId.push_back(column_->GetId());
}
else
@@ -110,14 +111,13 @@ namespace Modelec
return;
}
// TODO : make the mission finished if the pos is not valid
closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos();
auto p = closestDepoZonePoint_.GetTakeBasePosition();
auto res = nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL);
if (res != Pathfinding::FREE)
{
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::WALL);
nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
step_ = GO_BACK;
}
else
@@ -209,7 +209,7 @@ namespace Modelec
auto p = closestDepoZonePoint_.GetTakeBasePosition();
if (nav_->CanGoTo(p.x, p.y, p.theta) != Pathfinding::FREE)
{
nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL);
nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
}
}

18
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -481,11 +481,19 @@ namespace Modelec
PosMsg::SharedPtr NavigationHelper::GetHomePosition()
{
PosMsg::SharedPtr home = std::make_shared<PosMsg>();
home->x = spawn_.x;
home->y = spawn_.y;
home->theta = spawn_.theta;
return home;
}
if (team_id_ == YELLOW)
{
home->x = Config::get<int>("config.home.yellow@x", 0);
home->y = Config::get<int>("config.home.yellow@y", 0);
home->theta = Config::get<double>("config.home.yellow@theta", 0);
}
else
{
home->x = Config::get<int>("config.home.blue@x", 0);
home->y = Config::get<int>("config.home.blue@y", 0);
home->theta = Config::get<double>("config.home.blue@theta", 0);
}
return home; }
void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{

2
src/modelec_strat/src/obstacle/column.cpp
View File

@@ -46,7 +46,7 @@ namespace Modelec
double optimizedAngle = 0;
for (const auto& angle : possible_angles_)
{
auto newPos = GetPosition().GetTakePosition(400, angle);
auto newPos = GetPosition().GetTakePosition(310, angle);
double dist = Point::distance(currentPos, newPos);
if (dist < distance)
{

4
src/modelec_utils/src/point.cpp
View File

@@ -24,11 +24,11 @@ namespace Modelec
Point Point::GetTakeBasePosition() const
{
return GetTakePosition(320, theta);
return GetTakePosition(290, theta);
}
Point Point::GetTakeClosePosition() const
{
return GetTakePosition(210, theta);
return GetTakePosition(130, theta);
}
}