example mission

This commit is contained in:
acki
2025-05-28 18:41:16 -04:00
parent 6c592afe2b
commit 20f037a1f9
3 changed files with 9 additions and 5 deletions

View File

@@ -5,22 +5,22 @@
<Obstacle id="3" x="2150" y="1900" theta="1.5708" width="400" height="200" type="estrade"/>
<!-- Gradin TOP TO BOTTOM LEFT -->
<Gradin id="10" x="825" y="1725" theta="-1.5708" width="400" height="100" type="gradin" />
<!--<Gradin id="10" x="825" y="1725" theta="-1.5708" width="400" height="100" type="gradin" />
<Gradin id="11" x="75" y="1325" theta="0" width="400" height="100" type="gradin"/>
<Gradin id="12" x="1100" y="950" theta="1.5708" width="400" height="100" type="gradin" >
<possible-angle theta="-1.5708" />
</Gradin>
<Gradin id="13" x="75" y="400" theta="0" width="400" height="100" type="gradin"/>
<Gradin id="14" x="775" y="250" theta="1.5708" width="400" height="100" type="gradin"/>
<Gradin id="14" x="775" y="250" theta="1.5708" width="400" height="100" type="gradin"/>-->
<!-- Gradin TOP TO BOTTOM RIGHT -->
<Gradin id="20" x="2175" y="1725" theta="-1.5708" width="400" height="100" type="gradin" />
<!--<Gradin id="20" x="2175" y="1725" theta="-1.5708" width="400" height="100" type="gradin" />
<Gradin id="21" x="2925" y="1325" theta="3.1415" width="400" height="100" type="gradin"/>
<Gradin id="22" x="1900" y="950" theta="1.5708" width="400" height="100" type="gradin">
<possible-angle theta="-1.5708" />
</Gradin>
<Gradin id="23" x="2925" y="400" theta="3.1415" width="400" height="100" type="gradin"/>
<Gradin id="24" x="2225" y="250" theta="1.5708" width="400" height="100" type="gradin"/>
<Gradin id="24" x="2225" y="250" theta="1.5708" width="400" height="100" type="gradin"/>-->
<!-- PAMI Start -->
<Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>

View File

@@ -55,7 +55,7 @@ namespace Modelec
case GO_TO_FRONT:
{
nav_->GoTo(spawn_.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 150, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
nav_->GoTo(spawn_.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 600, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
step_ = GO_FORWARD;
}

View File

@@ -580,6 +580,10 @@ namespace Modelec
bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg)
{
if (!current_pos_) {
return false;
}
auto curr = Waypoint(*current_pos_, -1, false);
std::vector<Waypoint> waypointsList;
waypointsList.push_back(curr);