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

strat v0.2.2 :

Browse Source 
- start of a dynamic strat
- enemy isn't part of the strat for the moment (next update)
This commit is contained in:
acki
2025-05-03 17:20:12 -04:00
parent 1818adc0b5
commit 4bd1a1438c
27 changed files with 516 additions and 52 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/9' # Modify this to your serial port (e.g., 'COM3' on Windows)
serial_port = '/dev/pts/12' # Modify this to your serial port (e.g., 'COM3' on Windows)
baud_rate = 115200 # Modify this to your baud rate
# Open the serial connection

2
simulated_pcb/odo.py
View File

@@ -4,7 +4,7 @@ import math
import threading
class SimulatedPCB:
def __init__(self, port='/dev/pts/8', baud=115200):
def __init__(self, port='/dev/pts/6', baud=115200):
self.ser = serial.Serial(port, baud, timeout=1)
self.running = True
self.start = False

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/10"; // TODO : check the real serial port
request->serial_port = "/dev/pts/13"; // 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/9"; // TODO : check the real serial port
request->serial_port = "/dev/pts/7"; // 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)))
{

11
src/modelec_gui/include/modelec_gui/pages/map_page.hpp
View File

@@ -18,8 +18,10 @@
#include <modelec_interfaces/srv/map_size.hpp>
#include <modelec_interfaces/msg/obstacle.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/strat_state.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int64.hpp>
namespace ModelecGUI {
class MapPage : public QWidget
@@ -61,6 +63,9 @@ namespace ModelecGUI {
QPushButton* tirette_button_;
QLabel* timer_label_;
QLabel* score_label_;
modelec_interfaces::msg::OdometryPos robotPos;
std::vector<QPoint> qpoints;
//std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
@@ -97,5 +102,11 @@ namespace ModelecGUI {
modelec_interfaces::msg::OdometryPos enemy_pos_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_pub_;
rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr strat_start_sub_;
bool isGameStarted_ = false;
long int start_time_ = 0;
rclcpp::Subscription<modelec_interfaces::msg::StratState>::SharedPtr strat_state_sub_;
};
}

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

@@ -17,9 +17,20 @@ namespace ModelecGUI
tirette_button_ = new QPushButton("Tirette", this);
timer_label_ = new QLabel("00", this);
timer_label_->setAlignment(Qt::AlignCenter);
timer_label_->setFont(QFont("Arial", 24));
timer_label_->setStyleSheet("QLabel { color: white; }");
score_label_ = new QLabel("Score: 0", this);
score_label_->setAlignment(Qt::AlignCenter);
score_label_->setFont(QFont("Arial", 24));
score_label_->setStyleSheet("QLabel { color: white; }");
h_layout = new QHBoxLayout(this);
h_layout->addStretch();
h_layout->addWidget(score_label_);
h_layout->addWidget(tirette_button_);
h_layout->addWidget(timer_label_);
h_layout->addStretch();
v_layout->addLayout(h_layout);
@@ -39,6 +50,8 @@ namespace ModelecGUI
enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 100,
[this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
if (lastWapointWasEnd)
@@ -75,14 +88,27 @@ namespace ModelecGUI
obstacle_.erase(msg->id);
});
go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("nav/go_to", 10);
enemy_pos_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryPos>("enemy/position", 10);
tirette_pub_ = node_->create_publisher<std_msgs::msg::Bool>("tirette", 10);
strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("/strat/start_time", 10,
[this](const std_msgs::msg::Int64::SharedPtr msg){
isGameStarted_ = true;
start_time_ = msg->data;
});
strat_state_sub_ = node_->create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
[this](const modelec_interfaces::msg::StratState::SharedPtr msg){
if (msg->state == modelec_interfaces::msg::StratState::STOP)
{
RCLCPP_INFO(node_->get_logger(), "Game stop");
isGameStarted_ = false;
}
});
connect(tirette_button_, &QPushButton::clicked, this, [this]() {
std_msgs::msg::Bool msg;
msg.data = true;
@@ -145,6 +171,15 @@ namespace ModelecGUI
{
QWidget::paintEvent(paint_event);
if (isGameStarted_)
{
auto now = std::chrono::system_clock::now().time_since_epoch();
auto start = std::chrono::nanoseconds(start_time_);
auto elapsed = now - start;
auto elapsed_s = std::chrono::duration_cast<std::chrono::seconds>(elapsed).count();
timer_label_->setText(QString::number(elapsed_s));
}
QPainter painter(this);
renderer->render(&painter, rect()); // Scales SVG to widget size
painter.save();

2
src/modelec_strat/CMakeLists.txt
View File

@@ -15,7 +15,7 @@ find_package(ament_index_cpp REQUIRED)
find_package(modelec_interfaces REQUIRED)
find_package(modelec_utils REQUIRED)
add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp src/obstacle.cpp)
add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/mission_manager.cpp src/missions/prepare_concert_mission.cpp src/missions/promotion_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.cpp src/deposite_zone.cpp)
ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp)
target_link_libraries(strat_fsm modelec_utils::utils modelec_utils::config)
target_include_directories(strat_fsm PUBLIC

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

@@ -16,7 +16,7 @@
<size>
<width_mm>300</width_mm>
<length_mm>300</length_mm>
<margin_mm>0</margin_mm>
<margin_mm>300</margin_mm>
</size>
</robot>

63
src/modelec_strat/data/deposite_zone.xml Normal file
View File

@@ -0,0 +1,63 @@
<Map>
<!-- YELLOW ZONE -->
<DepositeZone id="0" team="0" max_pot="1">
<Pos x="775" y="75" theta="1.5708" />
<PotPos>
<Pos x="775" y="75" theta="1.5708" />
</PotPos>
</DepositeZone>
<DepositeZone id="1" team="0" max_pot="2">
<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" />
</PotPos>
</DepositeZone>
<DepositeZone id="2" team="0" max_pot="1">
<Pos x="2775" y="75" theta="1.5708" />
<PotPos>
<Pos x="2775" y="75" theta="1.5708" />
</PotPos>
</DepositeZone>
<DepositeZone id="3" team="0" max_pot="2">
<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" />
</PotPos>
</DepositeZone>
<!-- BLUE ZONE -->
<DepositeZone id="10" team="1" max_pot="1">
<Pos x="2225" y="75" theta="1.5708" />
<PotPos>
<Pos x="2225" y="75" theta="1.5708" />
</PotPos>
</DepositeZone>
<DepositeZone id="11" team="1" max_pot="2">
<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" />
</PotPos>
</DepositeZone>
<DepositeZone id="12" team="1" max_pot="1">
<Pos x="225" y="75" theta="1.5708" />
<PotPos>
<Pos x="225" y="75" theta="1.5708" />
</PotPos>
</DepositeZone>
<DepositeZone id="13" team="1" max_pot="2">
<Pos x="225" y="875" theta="0" />
<PotPos>
<Pos x="125" y="875" theta="0" />
<Pos x="300" y="875" theta="0" />
</PotPos>
</DepositeZone>
</Map>

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

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

33
src/modelec_strat/include/modelec_strat/deposite_zone.hpp Normal file
View File

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

7
src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp
View File

@@ -2,6 +2,7 @@
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <modelec_strat/obstacle/column.hpp>
namespace Modelec {
@@ -19,9 +20,11 @@ namespace Modelec {
GO_TO_COLUMN,
GO_CLOSE_TO_COLUMN,
TAKE_COLUMN,
GO_BACK,
GO_TO_PLATFORM,
GO_CLOSE_TO_PLATFORM,
PLACE_PLATFORM,
GO_BACK_2,
DONE
} step_;
@@ -29,7 +32,9 @@ namespace Modelec {
std::shared_ptr<NavigationHelper> nav_;
rclcpp::Node::SharedPtr node_;
Obstacle column_;
std::shared_ptr<ColumnObstacle> column_;
std::shared_ptr<DepositeZone> closestDepoZone_;
Point closestDepoZonePoint_;
};
}

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

@@ -5,6 +5,7 @@
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include "deposite_zone.hpp"
#include "pathfinding.hpp"
namespace Modelec {
@@ -37,6 +38,12 @@ namespace Modelec {
WaypointListMsg FindPath(const PosMsg::SharedPtr &goal, bool isClose = false);
PosMsg::SharedPtr GetCurrentPos() const;
bool LoadDepositeZoneFromXML(const std::string &filename);
std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId);
protected:
void OnWaypointReach(const WaypointReachMsg::SharedPtr msg);
@@ -51,6 +58,8 @@ namespace Modelec {
PosMsg::SharedPtr current_pos_;
std::map<int, std::shared_ptr<DepositeZone>> deposite_zones_;
rclcpp::Subscription<WaypointReachMsg>::SharedPtr waypoint_reach_sub_;
rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;

23
src/modelec_strat/include/modelec_strat/obstacle/column.hpp Normal file
View File

@@ -0,0 +1,23 @@
#pragma once
#include "obstacle.hpp"
namespace Modelec
{
class ColumnObstacle : public Obstacle
{
public:
ColumnObstacle() = default;
ColumnObstacle(const ColumnObstacle&) = default;
ColumnObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type);
ColumnObstacle(tinyxml2::XMLElement * obstacleElem);
ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg);
bool IsAtObjective() const;
void SetAtObjective(bool atObjective);
protected:
bool isAtObjective = false;
};
}

15
src/modelec_strat/include/modelec_strat/obstacle.hpp → src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp
View File

@@ -6,6 +6,8 @@
#include <tinyxml2.h>
#include <modelec_utils/point.hpp>
namespace Modelec {
class Obstacle {
public:
@@ -15,7 +17,9 @@ namespace Modelec {
Obstacle(const modelec_interfaces::msg::Obstacle& msg);
Obstacle(const Obstacle& other) = default;
modelec_interfaces::msg::Obstacle toMsg() const;
virtual ~Obstacle() = default;
virtual modelec_interfaces::msg::Obstacle toMsg() const;
int id() const { return id_; }
int x() const { return x_; }
@@ -24,6 +28,7 @@ namespace Modelec {
int width() const { return w_; }
int height() const { return h_; }
const std::string& type() const { return type_; }
Point position() const { return Point(x_, y_, theta_); }
void setId(int id) { id_ = id; }
void setX(int x) { x_ = x; }
@@ -39,11 +44,17 @@ namespace Modelec {
theta_ = theta;
}
void setPosition(const Point& p) {
x_ = p.x;
y_ = p.y;
theta_ = p.theta;
}
void setSize(int w, int h) {
w_ = w;
h_ = h;
}
private:
protected:
int id_, x_, y_, w_, h_;
double theta_;
std::string type_;

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

@@ -14,7 +14,9 @@
#include <modelec_interfaces/srv/map_size.hpp>
#include <modelec_interfaces/msg/obstacle.hpp>
#include "obstacle.hpp"
#include <modelec_strat/obstacle/obstacle.hpp>
#include "obstacle/column.hpp"
namespace Modelec {
@@ -53,11 +55,17 @@ namespace Modelec {
void SetCurrentPos(const PosMsg::SharedPtr& pos);
Obstacle GetObstacle(int id) const;
std::shared_ptr<Obstacle> GetObstacle(int id) const;
void RemoveObstacle(int id);
void AddObstacle(const Obstacle& obstacle);
void AddObstacle(const std::shared_ptr<Obstacle>& obstacle);
template <typename T,
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);
protected:
void HandleMapRequest(
@@ -93,7 +101,7 @@ namespace Modelec {
int margin_mm_ = 0;
std::map<int, Obstacle> obstacle_map_;
std::map<int, std::shared_ptr<Obstacle>> obstacle_map_;
PosMsg::SharedPtr current_start_;
PosMsg::SharedPtr current_goal_;
@@ -116,4 +124,23 @@ namespace Modelec {
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_obstacle_srv_;
};
template <typename T, typename>
std::shared_ptr<T> Pathfinding::GetClosestObstacle(const PosMsg::SharedPtr& pos) const
{
std::shared_ptr<T> 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<T>(obstacle.second)) {
auto dist = Point::distance(robotPos, obs->position());
if (dist < distance) {
distance = dist;
closest_obstacle = obs;
}
}
}
return closest_obstacle;
}
}

2
src/modelec_strat/include/modelec_strat/strat_fms.hpp
View File

@@ -11,6 +11,7 @@
#include "missions/go_home_mission.hpp"
#include "std_msgs/msg/bool.hpp"
#include <std_msgs/msg/int64.hpp>
#include "modelec_interfaces/msg/strat_state.hpp"
@@ -62,5 +63,6 @@ namespace Modelec
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tirette_sub_;
rclcpp::Publisher<modelec_interfaces::msg::StratState>::SharedPtr state_pub_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr start_time_pub_;
};
}

33
src/modelec_strat/src/deposite_zone.cpp Normal file
View File

@@ -0,0 +1,33 @@
#include <modelec_strat/deposite_zone.hpp>
#include <rclcpp/logging.hpp>
namespace Modelec
{
DepositeZone::DepositeZone(tinyxml2::XMLElement* obstacleElem)
{
obstacleElem->QueryIntAttribute("id", &id_);
obstacleElem->QueryIntAttribute("team", &team_);
obstacleElem->QueryIntAttribute("max_pot", &max_pot_);
auto posElem = obstacleElem->FirstChildElement("Pos");
if (posElem)
{
posElem->QueryIntAttribute("x", &position_.x);
posElem->QueryIntAttribute("y", &position_.y);
posElem->QueryDoubleAttribute("theta", &position_.theta);
}
auto posPotList = obstacleElem->FirstChildElement("PotPos");
if (posPotList)
{
for (auto elemPos = posPotList->FirstChildElement("Pos"); elemPos; elemPos = elemPos->NextSiblingElement("Pos"))
{
Point pos;
elemPos->QueryIntAttribute("x", &pos.x);
elemPos->QueryIntAttribute("y", &pos.y);
elemPos->QueryDoubleAttribute("theta", &pos.theta);
pot_queue_.emplace(pos);
}
}
}
}

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

@@ -1,4 +1,5 @@
#include <modelec_strat/missions/prepare_concert_mission.hpp>
#include <modelec_strat/obstacle/column.hpp>
namespace Modelec
{
@@ -9,11 +10,20 @@ namespace Modelec
void PrepareConcertMission::start(rclcpp::Node::SharedPtr node)
{
column_ = nav_->getPathfinding()->GetObstacle(14);
if (auto col = nav_->getPathfinding()->GetClosestColumn(nav_->GetCurrentPos()))
{
column_ = col;
} else
{
status_ = MissionStatus::FAILED;
return;
}
node_ = node;
nav_->GoTo(775, 480, -M_PI_2);
auto pos = column_->position().GetTakeBasePosition();
nav_->GoTo(pos.x, pos.y, pos.theta);
status_ = MissionStatus::RUNNING;
}
@@ -27,26 +37,40 @@ namespace Modelec
switch (step_)
{
case GO_TO_COLUMN:
nav_->GoTo(775, 460, -M_PI_2, true);
{
auto pos = column_->position().GetTakeClosePosition();
nav_->GoTo(pos.x, pos.y, pos.theta, true);
}
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:
{
nav_->GoTo(2500, 875, 0);
auto pos = column_->position().GetTakeBasePosition();
nav_->GoTo(pos.x, pos.y, pos.theta, true);
}
step_ = GO_BACK;
break;
case GO_BACK:
{
closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0);
closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos();
auto p = closestDepoZonePoint_.GetTakeBasePosition();
nav_->GoTo(p.x, p.y, p.theta);
}
step_ = GO_TO_PLATFORM;
break;
case GO_TO_PLATFORM:
{
nav_->GoTo(2700, 875, 0, true);
auto p = closestDepoZonePoint_.GetTakeClosePosition();
nav_->GoTo(p.x, p.y, p.theta, true);
}
step_ = GO_CLOSE_TO_PLATFORM;
@@ -54,9 +78,10 @@ namespace Modelec
case GO_CLOSE_TO_PLATFORM:
{
column_.setX(2925);
column_.setY(875);
column_.setTheta(0);
column_->setX(closestDepoZonePoint_.x);
column_->setY(closestDepoZonePoint_.y);
column_->setTheta(closestDepoZonePoint_.theta);
column_->SetAtObjective(true);
nav_->getPathfinding()->AddObstacle(column_);
}
@@ -64,6 +89,15 @@ namespace Modelec
break;
case PLACE_PLATFORM:
{
auto p = closestDepoZonePoint_.GetTakeBasePosition();
nav_->GoTo(p.x, p.y, p.theta, true);
}
step_ = GO_BACK_2;
break;
case GO_BACK_2:
step_ = DONE;
status_ = MissionStatus::DONE;
break;

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

@@ -1,5 +1,6 @@
#include <modelec_strat/navigation_helper.hpp>
#include <utility>
#include <ament_index_cpp/get_package_share_directory.hpp>
namespace Modelec {
NavigationHelper::NavigationHelper()
@@ -28,6 +29,12 @@ namespace Modelec {
"nav/go_to", 10, [this](const PosMsg::SharedPtr msg) {
GoTo(msg);
});
std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml";
if (!LoadDepositeZoneFromXML(deposite_zone_path))
{
RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
}
}
rclcpp::Node::SharedPtr NavigationHelper::getNode() const
@@ -187,6 +194,60 @@ namespace Modelec {
return pathfinding_->FindPath(current_pos_, goal, isClose);
}
PosMsg::SharedPtr NavigationHelper::GetCurrentPos() const
{
return current_pos_;
}
bool NavigationHelper::LoadDepositeZoneFromXML(const std::string& filename)
{
tinyxml2::XMLDocument doc;
if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS)
{
RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str());
return false;
}
tinyxml2::XMLElement* root = doc.FirstChildElement("Map");
if (!root)
{
RCLCPP_ERROR(node_->get_logger(), "No <Obstacles> root element in file");
return false;
}
for (tinyxml2::XMLElement* elem = root->FirstChildElement("DepositeZone");
elem != nullptr;
elem = elem->NextSiblingElement("DepositeZone"))
{
std::shared_ptr<DepositeZone> obs = std::make_shared<DepositeZone>(elem);
deposite_zones_[obs->GetId()] = obs;
}
RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", deposite_zones_.size());
return true;
}
std::shared_ptr<DepositeZone> NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId)
{
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_)
{
if (zone.second->GetTeam() == teamId)
{
double distance = Point::distance(posPoint, zone.second->GetPosition());
if (distance < min_distance)
{
min_distance = distance;
closest_zone = zone.second;
}
}
}
return closest_zone;
}
void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg)
{
for (auto & waypoint : waypoints_)

29
src/modelec_strat/src/obstacle/column.cpp Normal file
View File

@@ -0,0 +1,29 @@
#include <modelec_strat/obstacle/column.hpp>
namespace Modelec
{
ColumnObstacle::ColumnObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type)
: Obstacle(id, x, y, theta, w, h, type)
{
}
ColumnObstacle::ColumnObstacle(tinyxml2::XMLElement* obstacleElem)
: Obstacle(obstacleElem)
{
}
ColumnObstacle::ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg) :
Obstacle(msg)
{
}
bool ColumnObstacle::IsAtObjective() const
{
return isAtObjective;
}
void ColumnObstacle::SetAtObjective(bool atObjective)
{
isAtObjective = atObjective;
}
}

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

@@ -1,4 +1,4 @@
#include <modelec_strat/obstacle.hpp>
#include <modelec_strat/obstacle/obstacle.hpp>
namespace Modelec
{

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

@@ -1,5 +1,6 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_strat/pathfinding.hpp>
#include <modelec_strat/obstacle/column.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec {
@@ -54,7 +55,7 @@ namespace Modelec {
"obstacle/add", 10,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
RCLCPP_INFO(node_->get_logger(), "Obstacle add request received");
AddObstacle(Obstacle(*msg));
AddObstacle(std::make_shared<Obstacle>(*msg));
});
obstacle_add_pub_ = node_->create_publisher<modelec_interfaces::msg::Obstacle>(
@@ -152,8 +153,8 @@ namespace Modelec {
const float cell_size_mm_y = map_height_mm_ / grid_height_;
// Robot dimensions
const int inflate_x = isClose ? 0 : std::ceil((robot_width_mm_ + margin_mm_ / 2.0f) / cell_size_mm_x);
const int inflate_y = isClose ? 0 : std::ceil((robot_length_mm_ + margin_mm_ / 2.0f) / cell_size_mm_y);
const int inflate_x = isClose ? 0 : std::ceil(((robot_width_mm_ + margin_mm_) / 2.0f) / cell_size_mm_x);
const int inflate_y = isClose ? 0 : std::ceil(((robot_length_mm_ + margin_mm_) / 2.0f) / cell_size_mm_y);
// 1. Build fresh empty grid
grid_.clear();
@@ -206,11 +207,11 @@ namespace Modelec {
// 2. Fill obstacles with inflation
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 theta = M_PI_2 - obstacle.theta();
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 theta = M_PI_2 - obstacle->theta();
float dx = width / 2.0f;
float dy = height / 2.0f;
@@ -496,7 +497,7 @@ namespace Modelec {
current_start_ = pos;
}
Obstacle Pathfinding::GetObstacle(int id) const
std::shared_ptr<Obstacle> Pathfinding::GetObstacle(int id) const
{
return obstacle_map_.at(id);
}
@@ -510,13 +511,35 @@ namespace Modelec {
obstacle_remove_pub_->publish(msg);
}
void Pathfinding::AddObstacle(const Obstacle& obstacle)
void Pathfinding::AddObstacle(const std::shared_ptr<Obstacle>& obstacle)
{
obstacle_map_[obstacle.id()] = obstacle;
modelec_interfaces::msg::Obstacle msg = obstacle.toMsg();
obstacle_map_[obstacle->id()] = obstacle;
modelec_interfaces::msg::Obstacle msg = obstacle->toMsg();
obstacle_add_pub_->publish(msg);
}
std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos)
{
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())
{
auto dist = Point::distance(robotPos, obs->position());
if (dist < distance) {
distance = dist;
closest_obstacle = obs;
}
}
}
}
return closest_obstacle;
}
void Pathfinding::HandleMapRequest(const std::shared_ptr<modelec_interfaces::srv::Map::Request>,
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response)
{
@@ -544,7 +567,7 @@ namespace Modelec {
{
for (auto & [index, obs] : obstacle_map_)
{
obstacle_add_pub_->publish(obs.toMsg());
obstacle_add_pub_->publish(obs->toMsg());
}
}
@@ -582,8 +605,16 @@ namespace Modelec {
obstacleElem != nullptr;
obstacleElem = obstacleElem->NextSiblingElement("Obstacle"))
{
Obstacle obs(obstacleElem);
obstacle_map_[obs.id()] = obs;
std::shared_ptr<Obstacle> obs = std::make_shared<Obstacle>(obstacleElem);
obstacle_map_[obs->id()] = obs;
}
for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Gradin");
obstacleElem != nullptr;
obstacleElem = obstacleElem->NextSiblingElement("Gradin"))
{
std::shared_ptr<ColumnObstacle> obs = std::make_shared<ColumnObstacle>(obstacleElem);
obstacle_map_[obs->id()] = obs;
}
RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size());

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

@@ -18,6 +18,8 @@ namespace Modelec
state_pub_ = create_publisher<modelec_interfaces::msg::StratState>("/strat/state", 10);
start_time_pub_ = create_publisher<std_msgs::msg::Int64>("/strat/start_time", 10);
std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
if (!Config::load(config_path))
{
@@ -66,6 +68,13 @@ namespace Modelec
if (started_)
{
match_start_time_ = now;
std_msgs::msg::Int64 msg;
msg.data = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::system_clock::now().time_since_epoch())
.count();
start_time_pub_->publish(msg);
transition(State::SELECT_MISSION, "Match started");
}
break;
@@ -78,7 +87,7 @@ namespace Modelec
{
transition(State::DO_PROMOTION, "Start promotion");
}
else if (elapsed.seconds() < 20)
else if (elapsed.seconds() < 80)
{
transition(State::DO_PREPARE_CONCERT, "Proceed to concert");
}

1
src/modelec_utils/CMakeLists.txt
View File

@@ -25,6 +25,7 @@ target_link_libraries(config PUBLIC tinyxml2::tinyxml2)
# === modelec::utils ===
add_library(utils
src/utils.cpp
src/point.cpp
)
target_include_directories(utils PUBLIC

18
src/modelec_utils/include/modelec_utils/point.hpp Normal file
View File

@@ -0,0 +1,18 @@
#pragma once
namespace Modelec
{
struct Point {
int x;
int y;
double theta;
Point() : x(0), y(0), theta(0) {}
Point(int x, int y, double theta) : x(x), y(y), theta(theta) {}
static double distance(const Point& p1, const Point& p2);
Point GetTakeBasePosition();
Point GetTakeClosePosition();
};
}

29
src/modelec_utils/src/point.cpp Normal file
View File

@@ -0,0 +1,29 @@
#include <cmath>
#include <modelec_utils/point.hpp>
namespace Modelec
{
double Point::distance(const Point& p1, const Point& p2)
{
return sqrt(std::pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}
Point Point::GetTakeBasePosition()
{
Point pos;
pos.x = x + 400 * cos(theta);
pos.y = y + 400 * sin(theta);
pos.theta = theta + M_PI;
return pos;
}
Point Point::GetTakeClosePosition()
{
Point pos;
pos.x = x + 210 * cos(theta);
pos.y = y + 210 * sin(theta);
pos.theta = theta + M_PI;
return pos;
}
}