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

start to be very nice :)

Browse Source
This commit is contained in:
acki
2025-05-04 00:26:13 -04:00
parent 4bd1a1438c
commit e22b745b02
18 changed files with 322 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
simulated_pcb/alim.py
View File

@@ -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/11' # Modify this to your serial port (e.g., 'COM3' on Windows)
baud_rate = 115200 # Modify this to your baud rate
# Open the serial connection

10
simulated_pcb/odo.py
View File

@@ -4,7 +4,7 @@ import math
import threading
class SimulatedPCB:
def __init__(self, port='/dev/pts/6', baud=115200):
def __init__(self, port='/dev/pts/9', baud=115200):
self.ser = serial.Serial(port, baud, timeout=1)
self.running = True
self.start = False
@@ -45,7 +45,7 @@ class SimulatedPCB:
angle_diff = self.normalize_angle(wp['theta'] - self.theta)
if wp['type'] == 1:
if distance < 5.0 and abs(angle_diff) < 0.05:
if distance < 5.0 and abs(angle_diff) < 0.15:
self.vx = 0.0
self.vy = 0.0
self.vtheta = 0.0
@@ -54,12 +54,12 @@ class SimulatedPCB:
self.waypoint_order.pop(0)
del self.waypoints[wp['id']]
else:
speed = min(200.0, distance * 1.5)
speed = min(300.0, distance * 3)
self.vx = speed * math.cos(angle)
self.vy = speed * math.sin(angle)
self.vtheta = max(-1.0, min(1.0, angle_diff))
self.vtheta = max(-9.0, min(9.0, angle_diff * 2))
else:
speed = min(300.0, distance * 2)
speed = min(600.0, distance * 4)
self.vx = speed * math.cos(angle)
self.vy = speed * math.sin(angle)
self.vtheta = 0.0

2
src/modelec_com/src/pcb_alim_interface.cpp
View File

@@ -10,7 +10,7 @@ namespace Modelec
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = "pcb_alim";
request->bauds = 115200;
request->serial_port = "/dev/pts/13"; // TODO : check the real serial port
request->serial_port = "/dev/pts/12"; // TODO : check the real serial port
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{

2
src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -10,7 +10,7 @@ namespace Modelec
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = "pcb_odo";
request->bauds = 115200;
request->serial_port = "/dev/pts/7"; // TODO : check the real serial port
request->serial_port = "/dev/pts/10"; // TODO : check the real serial port
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{

3
src/modelec_strat/data/config.xml
View File

@@ -3,7 +3,6 @@
<size>
<width_mm>300</width_mm>
<length_mm>300</length_mm>
<margin_mm>50</margin_mm>
</size>
<detection>
@@ -16,7 +15,7 @@
<size>
<width_mm>300</width_mm>
<length_mm>300</length_mm>
<margin_mm>300</margin_mm>
<margin_mm>100</margin_mm>
</size>
</robot>

28
src/modelec_strat/data/deposite_zone.xml
View File

@@ -7,11 +7,12 @@
</PotPos>
</DepositeZone>
<DepositeZone id="1" team="0" max_pot="2">
<DepositeZone id="1" team="0" max_pot="3">
<Pos x="1225" y="225" theta="1.5708" />
<PotPos>
<Pos x="1225" y="100" theta="1.5708" />
<Pos x="1225" y="250" theta="1.5708" />
<Pos x="1225" y="75" theta="1.5708" />
<Pos x="1225" y="225" theta="1.5708" />
<Pos x="1225" y="400" theta="1.5708" />
</PotPos>
</DepositeZone>
@@ -22,11 +23,12 @@
</PotPos>
</DepositeZone>
<DepositeZone id="3" team="0" max_pot="2">
<DepositeZone id="3" team="0" max_pot="3">
<Pos x="2775" y="875" theta="3.1415" />
<PotPos>
<Pos x="2875" y="875" theta="3.1415" />
<Pos x="2700" y="875" theta="3.1415" />
<Pos x="2925" y="875" theta="3.1415" />
<Pos x="2775" y="875" theta="3.1415" />
<Pos x="2600" y="875" theta="3.1415" />
</PotPos>
</DepositeZone>
@@ -38,11 +40,12 @@
</PotPos>
</DepositeZone>
<DepositeZone id="11" team="1" max_pot="2">
<DepositeZone id="11" team="1" max_pot="3">
<Pos x="1775" y="225" theta="1.5708" />
<PotPos>
<Pos x="1775" y="100" theta="1.5708" />
<Pos x="1775" y="250" theta="1.5708" />
<Pos x="1775" y="75" theta="1.5708" />
<Pos x="1775" y="225" theta="1.5708" />
<Pos x="1775" y="400" theta="1.5708" />
</PotPos>
</DepositeZone>
@@ -53,11 +56,12 @@
</PotPos>
</DepositeZone>
<DepositeZone id="13" team="1" max_pot="2">
<DepositeZone id="13" team="1" max_pot="3">
<Pos x="225" y="875" theta="0" />
<PotPos>
<Pos x="125" y="875" theta="0" />
<Pos x="300" y="875" theta="0" />
<Pos x="75" y="875" theta="0" />
<Pos x="225" y="875" theta="0" />
<Pos x="400" y="875" theta="0" />
</PotPos>
</DepositeZone>
</Map>

12
src/modelec_strat/data/obstacles.xml
View File

@@ -5,16 +5,20 @@
<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"/>
<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 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"/>
<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"/>

25
src/modelec_strat/include/modelec_strat/deposite_zone.hpp
View File

@@ -1,29 +1,44 @@
#pragma once
#include <limits>
#include <queue>
#include <tinyxml2.h>
#include <modelec_utils/point.hpp>
namespace Modelec {
namespace Modelec
{
class DepositeZone
{
public:
DepositeZone(tinyxml2::XMLElement * obstacleElem);
DepositeZone(tinyxml2::XMLElement* obstacleElem);
Point GetPosition() const { return position_; }
Point GetNextPotPos()
{
if (pot_queue_.empty())
return Point();
auto elem = pot_queue_.front();
return Point(std::numeric_limits<int>::min(), std::numeric_limits<int>::min(), 0);
return pot_queue_.front();
}
Point ValidNextPotPos()
{
if (pot_queue_.empty())
return Point(std::numeric_limits<int>::min(), std::numeric_limits<int>::min(), 0);
Point p = pot_queue_.front();
pot_queue_.pop();
return elem;
return p;
}
int GetTeam() const { return team_; }
int GetId() const { return id_; }
int GetMaxPot() const { return max_pot_; }
int RemainingPotPos() const
{
return pot_queue_.size();
}
protected:
int id_, team_, max_pot_;
Point position_;

16
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

@@ -16,9 +16,9 @@ namespace Modelec {
NavigationHelper(const rclcpp::Node::SharedPtr& node);
rclcpp::Node::SharedPtr getNode() const;
rclcpp::Node::SharedPtr GetNode() const;
std::shared_ptr<Pathfinding> getPathfinding() const;
std::shared_ptr<Pathfinding> GetPathfinding() const;
void SendWaypoint() const;
void SendWaypoint(const std::vector<WaypointMsg> &waypoints) const;
@@ -33,16 +33,20 @@ namespace Modelec {
bool HasArrived() const;
void GoTo(const PosMsg::SharedPtr &goal, bool isClose = false);
void GoTo(int x, int y, double theta, bool isClose = false);
int GoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
int GoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE);
WaypointListMsg FindPath(const PosMsg::SharedPtr &goal, bool isClose = false);
int CanGoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
int CanGoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE);
std::pair<int, WaypointListMsg> FindPath(const PosMsg::SharedPtr& goal, bool isClose = false,
int collisionMask = Pathfinding::FREE);
PosMsg::SharedPtr GetCurrentPos() const;
bool LoadDepositeZoneFromXML(const std::string &filename);
std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId);
std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId, const std::vector<int>& blacklistedId = {});
protected:
void OnWaypointReach(const WaypointReachMsg::SharedPtr msg);

8
src/modelec_strat/include/modelec_strat/obstacle/column.hpp
View File

@@ -1,6 +1,7 @@
#pragma once
#include "obstacle.hpp"
#include <modelec_interfaces/msg/odometry_pos.hpp>
namespace Modelec
{
@@ -17,7 +18,14 @@ namespace Modelec
bool IsAtObjective() const;
void SetAtObjective(bool atObjective);
Point GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const;
Point GetOptimizedGetPos(const Point& currentPos) const;
std::vector<Point> GetAllPossiblePositions() const;
protected:
bool isAtObjective = false;
std::vector<double> possible_angles_;
};
}

17
src/modelec_strat/include/modelec_strat/pathfinding.hpp
View File

@@ -40,14 +40,23 @@ namespace Modelec {
class Pathfinding {
public:
enum
{
FREE = 1 << 0,
WALL = 1 << 1,
OBSTACLE = 1 << 2,
ENEMY = 1 << 3,
};
Pathfinding();
Pathfinding(const rclcpp::Node::SharedPtr& node);
rclcpp::Node::SharedPtr getNode() const;
WaypointListMsg FindPath(const PosMsg::SharedPtr& start,
const PosMsg::SharedPtr& goal, bool isClose = false);
std::pair<int, WaypointListMsg> FindPath(const PosMsg::SharedPtr& start,
const PosMsg::SharedPtr& goal, bool isClose = false,
int collisionMask = FREE);
//void SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal);
@@ -65,7 +74,7 @@ namespace Modelec {
typename = std::enable_if_t<std::is_base_of<Obstacle, T>::value>>
std::shared_ptr<T> GetClosestObstacle(const PosMsg::SharedPtr& pos) const;
std::shared_ptr<ColumnObstacle> GetClosestColumn(const PosMsg::SharedPtr& pos);
std::shared_ptr<ColumnObstacle> GetClosestColumn(const PosMsg::SharedPtr& pos, const std::vector<int>& blacklistedId = {});
protected:
void HandleMapRequest(
@@ -85,6 +94,8 @@ namespace Modelec {
//bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos& enemy_pos);
//void Replan();
bool TestCollision(int x, int y, int collisionMask = FREE);
private:
rclcpp::Node::SharedPtr node_;

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

@@ -10,7 +10,7 @@ namespace Modelec
void PrepareConcertMission::start(rclcpp::Node::SharedPtr node)
{
if (auto col = nav_->getPathfinding()->GetClosestColumn(nav_->GetCurrentPos()))
if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos()))
{
column_ = col;
} else
@@ -21,9 +21,9 @@ namespace Modelec
node_ = node;
auto pos = column_->position().GetTakeBasePosition();
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
nav_->GoTo(pos.x, pos.y, pos.theta);
auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL);
status_ = MissionStatus::RUNNING;
}
@@ -38,31 +38,64 @@ namespace Modelec
{
case GO_TO_COLUMN:
{
auto pos = column_->position().GetTakeClosePosition();
nav_->GoTo(pos.x, pos.y, pos.theta, true);
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
nav_->GoTo(pos.x, pos.y, pos.theta, true, Pathfinding::FREE | Pathfinding::WALL);
}
step_ = GO_CLOSE_TO_COLUMN;
break;
case GO_CLOSE_TO_COLUMN:
nav_->getPathfinding()->RemoveObstacle(column_->id());
nav_->GetPathfinding()->RemoveObstacle(column_->id());
step_ = TAKE_COLUMN;
break;
case TAKE_COLUMN:
{
auto pos = column_->position().GetTakeBasePosition();
nav_->GoTo(pos.x, pos.y, pos.theta, true);
// TODO
closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0);
if (!closestDepoZone_)
{
status_ = MissionStatus::FAILED;
return;
}
closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos();
auto p = closestDepoZonePoint_.GetTakeBasePosition();
auto res = nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL);
if (res != Pathfinding::FREE)
{
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
nav_->GoTo(pos.x, pos.y, pos.theta, true, Pathfinding::FREE | Pathfinding::WALL);
step_ = GO_BACK;
}
else
{
nav_->GoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL);
closestDepoZone_->ValidNextPotPos();
step_ = GO_TO_PLATFORM;
}
}
step_ = GO_BACK;
break;
case GO_BACK:
{
closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0);
closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos();
if (!closestDepoZone_)
{
status_ = MissionStatus::FAILED;
return;
}
closestDepoZonePoint_ = closestDepoZone_->ValidNextPotPos();
auto p = closestDepoZonePoint_.GetTakeBasePosition();
nav_->GoTo(p.x, p.y, p.theta);
if (nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE)
{
nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL);
}
else
{
nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL);
}
}
step_ = GO_TO_PLATFORM;
@@ -82,7 +115,7 @@ namespace Modelec
column_->setY(closestDepoZonePoint_.y);
column_->setTheta(closestDepoZonePoint_.theta);
column_->SetAtObjective(true);
nav_->getPathfinding()->AddObstacle(column_);
nav_->GetPathfinding()->AddObstacle(column_);
}
step_ = PLACE_PLATFORM;
@@ -91,7 +124,10 @@ namespace Modelec
case PLACE_PLATFORM:
{
auto p = closestDepoZonePoint_.GetTakeBasePosition();
nav_->GoTo(p.x, p.y, p.theta, true);
if (nav_->CanGoTo(p.x, p.y, p.theta) != Pathfinding::FREE)
{
nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL);
}
}
step_ = GO_BACK_2;

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

@@ -27,7 +27,7 @@ namespace Modelec {
go_to_sub_ = node_->create_subscription<PosMsg>(
"nav/go_to", 10, [this](const PosMsg::SharedPtr msg) {
GoTo(msg);
GoTo(msg, false, Pathfinding::FREE | Pathfinding::WALL);
});
std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml";
@@ -37,12 +37,12 @@ namespace Modelec {
}
}
rclcpp::Node::SharedPtr NavigationHelper::getNode() const
rclcpp::Node::SharedPtr NavigationHelper::GetNode() const
{
return node_;
}
std::shared_ptr<Pathfinding> NavigationHelper::getPathfinding() const
std::shared_ptr<Pathfinding> NavigationHelper::GetPathfinding() const
{
return pathfinding_;
}
@@ -166,32 +166,54 @@ namespace Modelec {
return waypoints_.back().reached;
}
void NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose)
int NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask)
{
auto [res, wl] = FindPath(goal, isClose, collisionMask);
if (wl.empty() || res != Pathfinding::FREE)
{
return res;
}
waypoints_.clear();
auto wml = FindPath(goal, isClose);
for (auto & w : wml)
for (auto & w : wl)
{
waypoints_.emplace_back(w);
}
SendWaypoint();
return res;
}
void NavigationHelper::GoTo(int x, int y, double theta, bool isClose)
int NavigationHelper::GoTo(int x, int y, double theta, bool isClose, int collisionMask)
{
PosMsg::SharedPtr goal = std::make_shared<PosMsg>();
goal->x = x;
goal->y = y;
goal->theta = theta;
GoTo(goal, isClose);
return GoTo(goal, isClose, collisionMask);
}
WaypointListMsg NavigationHelper::FindPath(const PosMsg::SharedPtr& goal, bool isClose)
int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask)
{
return pathfinding_->FindPath(current_pos_, goal, isClose);
return FindPath(goal, isClose, collisionMask).first;
}
int NavigationHelper::CanGoTo(int x, int y, double theta, bool isClose, int collisionMask)
{
PosMsg::SharedPtr goal = std::make_shared<PosMsg>();
goal->x = x;
goal->y = y;
goal->theta = theta;
return CanGoTo(goal, isClose, collisionMask);
}
std::pair<int, WaypointListMsg> NavigationHelper::FindPath(const PosMsg::SharedPtr& goal, bool isClose,
int collisionMask)
{
return pathfinding_->FindPath(current_pos_, goal, isClose, collisionMask);
}
PosMsg::SharedPtr NavigationHelper::GetCurrentPos() const
@@ -227,20 +249,20 @@ namespace Modelec {
return true;
}
std::shared_ptr<DepositeZone> NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId)
std::shared_ptr<DepositeZone> NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId, const std::vector<int>& blacklistedId)
{
std::shared_ptr<DepositeZone> closest_zone = nullptr;
double min_distance = std::numeric_limits<double>::max();
auto posPoint = Point(pos->x, pos->y, pos->theta);
for (const auto& zone : deposite_zones_)
for (const auto& [id, zone] : deposite_zones_)
{
if (zone.second->GetTeam() == teamId)
if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find(blacklistedId.begin(), blacklistedId.end(), id))
{
double distance = Point::distance(posPoint, zone.second->GetPosition());
double distance = Point::distance(posPoint, zone->GetPosition());
if (distance < min_distance)
{
min_distance = distance;
closest_zone = zone.second;
closest_zone = zone;
}
}
}

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

@@ -10,6 +10,13 @@ namespace Modelec
ColumnObstacle::ColumnObstacle(tinyxml2::XMLElement* obstacleElem)
: Obstacle(obstacleElem)
{
possible_angles_.push_back(theta_);
for (auto elem = obstacleElem->FirstChildElement("possible-angle");
elem;
elem = elem->NextSiblingElement("possible-angle"))
{
possible_angles_.push_back(elem->DoubleAttribute("theta"));
}
}
ColumnObstacle::ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg) :
@@ -26,4 +33,38 @@ namespace Modelec
{
isAtObjective = atObjective;
}
Point ColumnObstacle::GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const
{
Point p = Point(msg->x, msg->y, msg->theta);
return GetOptimizedGetPos(p);
}
Point ColumnObstacle::GetOptimizedGetPos(const Point& currentPos) const
{
auto distance = std::numeric_limits<double>::max();
double optimizedAngle = 0;
for (const auto& angle : possible_angles_)
{
auto newPos = position().GetTakePosition(400, angle);
double dist = Point::distance(currentPos, newPos);
if (dist < distance)
{
distance = dist;
optimizedAngle = angle;
}
}
return Point(x_, y_, optimizedAngle);
}
std::vector<Point> ColumnObstacle::GetAllPossiblePositions() const
{
std::vector<Point> positions;
for (const auto & angle : possible_angles_)
{
positions.push_back(Point(x_, y_, angle));
}
return positions;
}
}

134
src/modelec_strat/src/pathfinding.cpp
View File

@@ -3,13 +3,13 @@
#include <modelec_strat/obstacle/column.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec {
namespace Modelec
{
struct AStarNode
{
int x, y;
double g = std::numeric_limits<double>::infinity(); // Cost from start
double f = std::numeric_limits<double>::infinity(); // g + heuristic
double g = std::numeric_limits<double>::infinity(); // Cost from start
double f = std::numeric_limits<double>::infinity(); // g + heuristic
int parent_x = -1;
int parent_y = -1;
@@ -21,7 +21,7 @@ namespace Modelec {
double heuristic(int x1, int y1, int x2, int y2)
{
return std::hypot(x1 - x2, y1 - y2); // Euclidean distance
return std::hypot(x1 - x2, y1 - y2); // Euclidean distance
}
Pathfinding::Pathfinding()
@@ -45,7 +45,8 @@ namespace Modelec {
grid_width_ = Config::get<int>("config.map.size.grid_width", 300);
grid_height_ = Config::get<int>("config.map.size.grid_height", 200);
std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/obstacles.xml";
std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
"/data/obstacles.xml";
if (!LoadObstaclesFromXML(obstacles_path))
{
RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
@@ -53,7 +54,8 @@ namespace Modelec {
obstacle_add_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
"obstacle/add", 10,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
RCLCPP_INFO(node_->get_logger(), "Obstacle add request received");
AddObstacle(std::make_shared<Obstacle>(*msg));
});
@@ -63,7 +65,8 @@ namespace Modelec {
obstacle_remove_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
"obstacle/remove", 10,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
RCLCPP_INFO(node_->get_logger(), "Obstacle remove request received");
RemoveObstacle(msg->id);
});
@@ -74,7 +77,8 @@ namespace Modelec {
ask_obstacle_srv_ = node_->create_service<std_srvs::srv::Empty>(
"nav/ask_map_obstacle",
[this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
const std::shared_ptr<std_srvs::srv::Empty::Response> response) {
const std::shared_ptr<std_srvs::srv::Empty::Response> response)
{
RCLCPP_INFO(node_->get_logger(), "Ask obstacle request received");
HandleAskObstacleRequest(request, response);
});
@@ -82,7 +86,8 @@ namespace Modelec {
map_srv_ = node_->create_service<modelec_interfaces::srv::Map>(
"nav/map",
[this](const std::shared_ptr<modelec_interfaces::srv::Map::Request> request,
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response) {
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response)
{
RCLCPP_INFO(node_->get_logger(), "Map request received");
HandleMapRequest(request, response);
});
@@ -90,14 +95,16 @@ namespace Modelec {
map_size_srv_ = node_->create_service<modelec_interfaces::srv::MapSize>(
"nav/map_size",
[this](const std::shared_ptr<modelec_interfaces::srv::MapSize::Request> request,
const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response) {
const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response)
{
RCLCPP_INFO(node_->get_logger(), "Map size request received");
HandleMapSizeRequest(request, response);
});
enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"enemy/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
OnEnemyPosition(msg);
});
@@ -139,12 +146,13 @@ namespace Modelec {
}
}*/
WaypointListMsg Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal, bool isClose)
std::pair<int, WaypointListMsg> Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal,
bool isClose, int collisionMask)
{
if (!start || !goal)
{
RCLCPP_WARN(node_->get_logger(), "Start or Goal position is null");
return WaypointListMsg();
return {-3, WaypointListMsg()};
}
WaypointListMsg waypoints;
@@ -161,7 +169,7 @@ namespace Modelec {
grid_.resize(grid_height_);
for (auto& row : grid_)
{
row.assign(grid_width_, 0); // 0 = free
row.assign(grid_width_, FREE);
}
if (has_enemy_pos_)
@@ -169,8 +177,10 @@ namespace Modelec {
int ex = (last_enemy_pos_.x / cell_size_mm_x);
int ey = ((map_height_mm_ - last_enemy_pos_.y) / cell_size_mm_y);
const int inflate_enemy_x = std::ceil((enemy_margin_mm_ + (enemy_width_mm_/2)) / cell_size_mm_x) + inflate_x;
const int inflate_enemy_y = std::ceil((enemy_margin_mm_ + (enemy_length_mm_/2)) / cell_size_mm_y) + inflate_y;
const int inflate_enemy_x = std::ceil((enemy_margin_mm_ + (enemy_width_mm_ / 2)) / cell_size_mm_x) +
inflate_x;
const int inflate_enemy_y = std::ceil((enemy_margin_mm_ + (enemy_length_mm_ / 2)) / cell_size_mm_y) +
inflate_y;
for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y)
{
@@ -178,7 +188,7 @@ namespace Modelec {
{
if (x >= 0 && y >= 0 && x < grid_width_ && y < grid_height_)
{
grid_[y][x] = 1;
grid_[y][x] |= ENEMY;
}
}
}
@@ -189,8 +199,8 @@ namespace Modelec {
{
for (int x = 0; x < inflate_x; ++x)
{
grid_[y][x] = 1; // bord gauche
grid_[y][grid_width_ - 1 - x] = 1; // bord droit
grid_[y][x] |= WALL;
grid_[y][grid_width_ - 1 - x] |= WALL;
}
}
@@ -199,18 +209,19 @@ namespace Modelec {
{
for (int y = 0; y < inflate_y; ++y)
{
grid_[y][x] = 1; // bord bas
grid_[grid_height_ - 1 - y][x] = 1; // bord haut
grid_[y][x] |= WALL;
grid_[grid_height_ - 1 - y][x] |= WALL;
}
}
// 2. Fill obstacles with inflation
// TODO some bug exist with the inflate
for (const auto& [id, obstacle] : obstacle_map_)
{
float cx = obstacle->x();
float cy = obstacle->y();
float width = obstacle->width() + inflate_x * cell_size_mm_x;
float height = obstacle->height() + inflate_y * cell_size_mm_y;
float width = obstacle->width() + inflate_x * 2 * cell_size_mm_x;
float height = obstacle->height() + inflate_y * 2 * cell_size_mm_y;
float theta = M_PI_2 - obstacle->theta();
float dx = width / 2.0f;
@@ -244,9 +255,9 @@ namespace Modelec {
}
int x_start = std::max(0, (int)(min_x / cell_size_mm_x));
int x_end = std::min(grid_width_ - 1, (int)(max_x / cell_size_mm_x));
int x_end = std::min(grid_width_ - 1, (int)(max_x / cell_size_mm_x));
int y_start = std::max(0, (int)(min_y / cell_size_mm_y));
int y_end = std::min(grid_height_ - 1, (int)(max_y / cell_size_mm_y));
int y_end = std::min(grid_height_ - 1, (int)(max_y / cell_size_mm_y));
// Mark cells that fall inside rotated rectangle
for (int y = y_start; y <= y_end; ++y)
@@ -269,7 +280,7 @@ namespace Modelec {
int gy = (grid_height_ - 1) - y;
if (x >= 0 && gy >= 0 && x < grid_width_ && gy < grid_height_)
{
grid_[gy][x] = 1;
grid_[gy][x] |= OBSTACLE;
}
}
}
@@ -287,34 +298,37 @@ namespace Modelec {
goal_x >= grid_width_ || goal_y >= grid_height_)
{
RCLCPP_WARN(node_->get_logger(), "Start or Goal out of bounds");
return waypoints;
return {-2, waypoints};
}
if (grid_[start_y][start_x] == 1 || grid_[goal_y][goal_x] == 1)
if (!TestCollision(start_x, start_y, collisionMask) || !TestCollision(goal_x, goal_y, collisionMask))
{
if (grid_[start_y][start_x] == 1)
if (!TestCollision(start_x, start_y, collisionMask))
{
RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle");
return {grid_[start_y][start_x], waypoints};
}
else
{
RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle");
return {grid_[goal_y][goal_x], waypoints};
}
return waypoints;
}
// 4. A* algorithm
std::priority_queue<AStarNode, std::vector<AStarNode>, std::greater<AStarNode>> open_list;
std::unordered_map<int64_t, AStarNode> all_nodes;
auto hash = [](int x, int y) {
auto hash = [](int x, int y)
{
return (int64_t)x << 32 | (uint32_t)y;
};
auto neighbors = [](int x, int y) {
auto neighbors = [](int x, int y)
{
return std::vector<std::pair<int, int>>{
{x+1, y}, {x-1, y}, {x, y+1}, {x, y-1},
{x+1, y+1}, {x-1, y+1}, {x+1, y-1}, {x-1, y-1} // diagonals
{x + 1, y}, {x - 1, y}, {x, y + 1}, {x, y - 1},
{x + 1, y + 1}, {x - 1, y + 1}, {x + 1, y - 1}, {x - 1, y - 1} // diagonals
};
};
@@ -343,7 +357,7 @@ namespace Modelec {
if (nx < 0 || ny < 0 || ny >= (int)grid_.size() || nx >= (int)grid_[0].size())
continue;
if (grid_[ny][nx] == 1)
if (!TestCollision(nx, ny, collisionMask))
continue;
double tentative_g = current.g + heuristic(current.x, current.y, nx, ny);
@@ -367,7 +381,7 @@ namespace Modelec {
if (!path_found)
{
RCLCPP_WARN(node_->get_logger(), "No path found");
return waypoints;
return {-1, waypoints};
}
// 5. Reconstruct path
@@ -411,7 +425,7 @@ namespace Modelec {
while (true)
{
if (grid_[y][x] == 1)
if (!TestCollision(x, y, collisionMask))
{
clear = false;
break;
@@ -477,7 +491,7 @@ namespace Modelec {
waypoints.back().is_end = true;
}
return waypoints;
return {FREE, waypoints};
}
/*void Pathfinding::SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal)
@@ -518,20 +532,28 @@ namespace Modelec {
obstacle_add_pub_->publish(msg);
}
std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos)
std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos,
const std::vector<int>& blacklistedId)
{
std::shared_ptr<ColumnObstacle> closest_obstacle = nullptr;
auto robotPos = Point(pos->x, pos->y, pos->theta);
float distance = std::numeric_limits<float>::max();
for (const auto& obstacle : obstacle_map_) {
if (auto obs = std::dynamic_pointer_cast<ColumnObstacle>(obstacle.second)) {
if (!obs->IsAtObjective())
for (const auto& [id, obstacle] : obstacle_map_)
{
if (auto obs = std::dynamic_pointer_cast<ColumnObstacle>(obstacle))
{
if (!obs->IsAtObjective() && std::find(blacklistedId.begin(), blacklistedId.end(), obs->id()) ==
blacklistedId.end())
{
auto dist = Point::distance(robotPos, obs->position());
if (dist < distance) {
distance = dist;
closest_obstacle = obs;
for (auto possiblePos : obs->GetAllPossiblePositions())
{
auto dist = Point::distance(robotPos, possiblePos);
if (dist < distance)
{
distance = dist;
closest_obstacle = obs;
}
}
}
}
@@ -556,16 +578,16 @@ namespace Modelec {
}
void Pathfinding::HandleMapSizeRequest(const std::shared_ptr<modelec_interfaces::srv::MapSize::Request>,
const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response)
const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response)
{
response->width = grid_width_;
response->height = grid_height_;
}
void Pathfinding::HandleAskObstacleRequest(const std::shared_ptr<std_srvs::srv::Empty::Request>,
const std::shared_ptr<std_srvs::srv::Empty::Response>)
const std::shared_ptr<std_srvs::srv::Empty::Response>)
{
for (auto & [index, obs] : obstacle_map_)
for (auto& [index, obs] : obstacle_map_)
{
obstacle_add_pub_->publish(obs->toMsg());
}
@@ -585,6 +607,18 @@ namespace Modelec {
}*/
}
bool Pathfinding::TestCollision(int x, int y, int collisionMask)
{
if (y < 0 || y >= static_cast<int>(grid_.size()) ||
x < 0 || x >= static_cast<int>(grid_[y].size()))
{
RCLCPP_WARN(node_->get_logger(), "TestCollision: access out of bounds x=%d y=%d", x, y);
return false; // ou true, selon ce que tu veux (false = pas de collision)
}
return (grid_[y][x] & collisionMask) && !(grid_[y][x] & ~collisionMask);
}
bool Pathfinding::LoadObstaclesFromXML(const std::string& filename)
{
tinyxml2::XMLDocument doc;

11
src/modelec_strat/src/strat_fms.cpp
View File

@@ -64,7 +64,6 @@ namespace Modelec
break;
case State::WAIT_START:
RCLCPP_INFO_ONCE(get_logger(), "State: WAIT_START");
if (started_)
{
match_start_time_ = now;
@@ -110,6 +109,11 @@ namespace Modelec
current_mission_.reset();
transition(State::SELECT_MISSION, "PrepareConcert finished");
}
else if (current_mission_->getStatus() == MissionStatus::FAILED)
{
current_mission_.reset();
transition(State::SELECT_MISSION, "PrepareConcert failed");
}
break;
case State::DO_PROMOTION:
@@ -124,6 +128,11 @@ namespace Modelec
current_mission_.reset();
transition(State::SELECT_MISSION, "Promotion finished");
}
else if (current_mission_->getStatus() == MissionStatus::FAILED)
{
current_mission_.reset();
transition(State::SELECT_MISSION, "Promotion failed");
}
break;
case State::DO_GO_HOME:

6
src/modelec_utils/include/modelec_utils/point.hpp
View File

@@ -12,7 +12,9 @@ namespace Modelec
static double distance(const Point& p1, const Point& p2);
Point GetTakeBasePosition();
Point GetTakeClosePosition();
Point GetTakePosition(int distance, double angle) const;
Point GetTakeBasePosition() const;
Point GetTakeClosePosition() const;
};
}

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

@@ -8,22 +8,23 @@ namespace Modelec
return sqrt(std::pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}
Point Point::GetTakeBasePosition()
Point Point::GetTakePosition(int distance, double angle) const
{
Point pos;
pos.x = x + 400 * cos(theta);
pos.y = y + 400 * sin(theta);
pos.theta = theta + M_PI;
pos.x = x + distance * cos(angle);
pos.y = y + distance * sin(angle);
pos.theta = angle + M_PI;
return pos;
}
Point Point::GetTakeClosePosition()
Point Point::GetTakeBasePosition() const
{
Point pos;
pos.x = x + 210 * cos(theta);
pos.y = y + 210 * sin(theta);
pos.theta = theta + M_PI;
return pos;
return GetTakePosition(300, theta);
}
Point Point::GetTakeClosePosition() const
{
return GetTakePosition(210, theta);
}
}