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

huge UI update

Browse Source
This commit is contained in:
acki
2025-05-20 00:14:41 -04:00
parent 3d23967050
commit 4a4087a977
21 changed files with 335 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/modelec_com/src/pcb_action_interface.cpp
View File

@@ -387,10 +387,12 @@ namespace Modelec
void PCBActionInterface::SendToPCB(const std::string& data) const
{
RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: '%s'", data.c_str());
auto message = std_msgs::msg::String();
message.data = data;
pcb_publisher_->publish(message);
if (pcb_publisher_)
{
auto message = std_msgs::msg::String();
message.data = data;
pcb_publisher_->publish(message);
}
}
void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem,

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

@@ -419,9 +419,12 @@ namespace Modelec
void PCBAlimInterface::SendToPCB(const std::string& data) const
{
auto message = std_msgs::msg::String();
message.data = data;
pcb_publisher_->publish(message);
if (pcb_publisher_)
{
auto message = std_msgs::msg::String();
message.data = data;
pcb_publisher_->publish(message);
}
}
void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem,

BIN
src/modelec_gui/img/logo/ISEN-Nantes-fond-blanc.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 44 KiB

76
src/modelec_gui/img/logo/ISEN-Nantes-fond-blanc.svg Normal file
View File

@@ -0,0 +1,76 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<svg
version="1.0"
width="1007.1511pt"
height="627.71661pt"
viewBox="0 0 1007.1511 627.71661"
preserveAspectRatio="xMidYMid"
id="svg6"
sodipodi:docname="ISEN-Nantes-fond-blanc.svg"
inkscape:version="1.4 (86a8ad7, 2024-10-11)"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns="http://www.w3.org/2000/svg"
xmlns:svg="http://www.w3.org/2000/svg">
<defs
id="defs6" />
<sodipodi:namedview
id="namedview6"
pagecolor="#ffffff"
bordercolor="#000000"
borderopacity="0.25"
inkscape:showpageshadow="2"
inkscape:pageopacity="0.0"
inkscape:pagecheckerboard="0"
inkscape:deskcolor="#d1d1d1"
inkscape:document-units="pt"
inkscape:zoom="0.36604644"
inkscape:cx="891.96332"
inkscape:cy="562.77012"
inkscape:window-width="1440"
inkscape:window-height="788"
inkscape:window-x="-6"
inkscape:window-y="-6"
inkscape:window-maximized="1"
inkscape:current-layer="svg6">
<inkscape:page
x="0"
y="0"
width="1007.1511"
height="627.71661"
id="page6"
margin="0 2.0488932 0 0"
bleed="0" />
</sodipodi:namedview>
<g
transform="matrix(0.1,0,0,-0.1,-139.72874,775.78779)"
fill="#000000"
stroke="none"
id="g6"
style="fill:#ff0000">
<path
d="m 3859,7351 c -339,-63 -603,-258 -749,-554 -77,-155 -103,-268 -104,-452 -1,-180 20,-276 89,-423 57,-120 113,-198 211,-293 177,-171 384,-263 645,-288 156,-15 254,-57 358,-155 241,-228 252,-555 26,-781 -121,-121 -244,-171 -404,-162 -163,8 -285,72 -396,207 -68,83 -117,117 -157,107 -32,-9 -384,-262 -398,-287 -32,-60 13,-151 146,-290 219,-229 504,-350 824,-350 262,0 491,77 701,234 287,216 459,560 459,916 0,432 -251,837 -639,1030 -143,72 -238,96 -503,128 -103,13 -177,50 -254,126 -99,99 -139,215 -122,351 17,129 112,257 237,318 63,30 72,32 181,32 103,0 121,-3 170,-26 31,-14 83,-51 117,-82 65,-61 91,-69 133,-40 14,9 103,77 198,151 115,89 174,142 178,158 10,40 -14,79 -93,155 -131,124 -276,204 -451,250 -86,23 -324,34 -403,20 z"
id="path1"
style="fill:#ff0000" />
<path
d="m 1872,7273 c -18,-9 -41,-30 -50,-47 -16,-29 -17,-152 -20,-1706 l -2,-1675 22,-45 c 13,-25 38,-55 57,-67 32,-22 44,-23 203,-23 191,0 226,10 263,72 20,33 20,51 20,1718 0,1612 -1,1687 -18,1722 -31,61 -59,68 -264,68 -144,0 -184,-4 -211,-17 z"
id="path2"
style="fill:#ff0000" />
<path
d="m 5696,7267 c -25,-14 -48,-39 -63,-66 l -23,-44 V 5501 c 0,-1117 4,-1668 11,-1693 5,-21 21,-50 34,-65 l 23,-28 1014,-3 1015,-2 33,22 c 67,45 75,73 75,258 0,182 -7,208 -68,254 -28,21 -36,21 -751,26 -777,5 -733,2 -778,55 -35,42 -39,90 -36,471 l 3,370 25,36 c 52,77 13,72 700,78 l 615,5 34,24 c 59,42 71,85 71,256 0,137 -2,155 -22,195 -42,84 -11,80 -693,80 h -601 l -44,23 c -30,14 -53,36 -68,62 -22,40 -22,43 -20,372 3,319 4,333 24,360 11,15 36,38 54,51 l 33,22 h 707 707 l 33,23 c 62,41 74,75 78,226 6,202 -16,268 -101,300 -19,8 -339,11 -1005,11 h -977 z"
id="path3"
style="fill:#ff0000" />
<path
d="m 8462,7273 c -18,-9 -41,-30 -50,-47 -16,-29 -17,-152 -20,-1706 l -2,-1675 22,-45 c 13,-25 38,-55 57,-67 32,-22 44,-23 203,-23 149,0 173,2 208,20 80,41 74,-44 80,1133 l 5,1049 24,19 c 29,24 56,18 91,-19 14,-15 345,-484 735,-1042 407,-581 732,-1037 762,-1066 82,-82 116,-94 267,-94 140,0 165,7 210,60 l 26,32 v 1697 1698 l -23,34 c -37,55 -54,59 -250,59 -201,0 -234,-8 -264,-62 -17,-31 -18,-93 -23,-1089 -4,-875 -7,-1059 -19,-1072 -19,-24 -55,-21 -85,6 -14,12 -354,488 -756,1057 -402,569 -750,1054 -773,1078 -71,74 -96,82 -254,82 -107,0 -145,-4 -171,-17 z"
id="path4"
style="fill:#ff0000" />
<path
d="m 1802,2398 3,-503 h 2370 2370 l 3,503 2,502 H 4175 1800 Z m 4266,371 c 58,-12 123,-62 142,-109 16,-38 12,-42 -68,-68 -58,-20 -63,-20 -69,-4 -20,56 -84,83 -147,62 -42,-13 -66,-43 -66,-81 0,-37 20,-51 153,-107 65,-28 133,-62 152,-77 117,-88 93,-271 -45,-349 -48,-27 -66,-31 -153,-34 -93,-4 -101,-3 -162,27 -48,24 -71,43 -94,78 -17,26 -31,52 -31,58 0,10 141,71 144,63 33,-73 39,-80 73,-95 57,-23 121,-13 155,26 61,68 25,115 -132,175 -117,45 -168,78 -200,131 -31,50 -26,155 9,207 61,89 197,128 339,97 z M 2514,2545 c 65,-118 130,-244 145,-279 l 27,-65 -4,280 -5,279 h 77 76 v -375 -375 h -103 -103 l -128,241 c -71,132 -136,260 -145,283 -15,41 -16,31 -13,-240 l 4,-284 h -76 -76 v 375 375 h 103 103 z m 1007,-145 c 71,-195 132,-363 135,-372 5,-16 -3,-18 -79,-18 h -85 l -32,90 -32,90 h -148 -148 l -33,-90 -32,-90 h -78 c -48,0 -79,4 -79,10 0,6 62,175 137,376 l 137,365 104,-3 104,-3 z m 531,155 c 60,-110 125,-234 144,-275 l 33,-75 1,278 v 277 h 75 75 l -2,-372 -3,-373 -102,-3 c -97,-3 -103,-2 -116,20 -29,45 -244,454 -260,493 -15,38 -16,29 -16,-237 l -1,-278 h -75 -75 v 375 376 l 106,-3 105,-3 z m 968,140 v -65 h -100 -100 v -310 -310 h -80 -80 v 310 309 l -102,3 -103,3 -3,63 -3,62 h 286 285 z m 570,5 v -60 l -162,-2 -163,-3 -3,-87 -3,-88 h 156 155 v -60 -59 l -152,-3 -153,-3 -3,-97 -3,-98 h 171 170 v -65 -65 h -250 -250 v 375 375 h 245 245 z"
id="path5"
style="fill:#ff0000" />
<path
d="m 3275,2595 c -5,-16 -30,-87 -56,-157 l -47,-128 h 109 109 l -49,139 c -27,76 -51,147 -53,157 -4,15 -7,12 -13,-11 z"
id="path6"
style="fill:#ff0000" />
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 5.3 KiB

BIN
src/modelec_gui/img/logo/ISEN-Nantes.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 22 KiB

BIN
src/modelec_gui/img/logo/modelec.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 135 KiB

BIN
src/modelec_gui/img/wood.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.5 KiB

21
src/modelec_gui/include/modelec_gui/pages/home_page.hpp
View File

@@ -9,11 +9,15 @@
#include <std_msgs/msg/int8.hpp>
#include <std_msgs/msg/empty.hpp>
#include <std_srvs/srv/empty.hpp>
#include <modelec_interfaces/msg/spawn.hpp>
namespace ModelecGUI
{
class HomePage : public QWidget
{
private:
Q_OBJECT
public:
@@ -21,11 +25,6 @@ namespace ModelecGUI
void Init();
public slots:
void onYellowButtonClicked();
void onBlueButtonClicked();
signals:
void TeamChoose();
@@ -34,15 +33,13 @@ namespace ModelecGUI
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr team_pub_;
rclcpp::Publisher<modelec_interfaces::msg::Spawn>::SharedPtr spawn_pub_;
rclcpp::Subscription<modelec_interfaces::msg::Spawn>::SharedPtr spawn_sub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr reset_strat_pub_;
QVBoxLayout* v_layout_;
QHBoxLayout* h_layout_;
QPushButton* blue_button_;
QPushButton* yellow_button_;
rclcpp::Client<std_srvs::srv::Empty>::SharedPtr ask_spawn_client_;
QSvgRenderer* renderer_;
std::vector<QPushButton*> spawn_buttons_;
};
}

5
src/modelec_gui/resource.qrc
View File

@@ -3,5 +3,10 @@
<qresource prefix="/">
<file>img/playmat/2025_FINAL.svg</file>
<file>img/playmat/grid_v1.svg</file>
<file>img/logo/ISEN-Nantes.png</file>
<file>img/logo/ISEN-Nantes-fond-blanc.svg</file>
<file>img/logo/ISEN-Nantes-fond-blanc.png</file>
<file>img/logo/modelec.png</file>
<file>img/wood.jpg</file>
</qresource>
</RCC>

4
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -10,6 +10,8 @@ namespace ModelecGUI {
: QMainWindow(parent), node_(std::move(node)), stackedWidget(new QStackedWidget(this)) {
// Add pages to stack
resize(1200, 800);
home_page_ = new HomePage(get_node(), this);
odo_page_ = new OdoPage(get_node(), this);
test_map_page_ = new TestMapPage(get_node(), this);
@@ -23,8 +25,6 @@ namespace ModelecGUI {
setupMenu();
resize(1200, 800);
connect(home_page_, &HomePage::TeamChoose, this, [this]()
{
stackedWidget->setCurrentIndex(3);

83
src/modelec_gui/src/pages/home_page.cpp
View File

@@ -2,6 +2,9 @@
#include <modelec_gui/pages/home_page.hpp>
#include <QVBoxLayout>
#include <modelec_interfaces/msg/detail/servo_mode__builder.hpp>
#include "../../../modelec_utils/include/modelec_utils/config.hpp"
namespace ModelecGUI
{
@@ -9,38 +12,54 @@ namespace ModelecGUI
: QWidget(parent), node_(node),
renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this))
{
yellow_button_ = new QPushButton("Yellow", this);
blue_button_ = new QPushButton("Blue", this);
spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("/strat/spawn", 10);
yellow_button_->setStyleSheet(
"background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;");
blue_button_->setStyleSheet(
"background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;");
auto w = Modelec::Config::get<int>("config.spawn.width_mm");
auto h = Modelec::Config::get<int>("config.spawn.height_mm");
yellow_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
blue_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
spawn_sub_ = node_->create_subscription<modelec_interfaces::msg::Spawn>("/nav/spawn", 10,
[this, w, h](const modelec_interfaces::msg::Spawn::SharedPtr msg)
{
auto ratioX = 1200 / 3000.0f;
auto ratioY = 800 / 2000.0f;
h_layout_ = new QHBoxLayout();
h_layout_->setContentsMargins(0, 0, 0, 0);
h_layout_->setSpacing(0);
h_layout_->addWidget(yellow_button_);
h_layout_->addWidget(blue_button_);
h_layout_->setStretch(0, 1);
h_layout_->setStretch(1, 1);
auto* button = new QPushButton(this);
spawn_buttons_.push_back(button);
v_layout_ = new QVBoxLayout();
v_layout_->setContentsMargins(0, 0, 0, 0);
v_layout_->setSpacing(0);
v_layout_->addLayout(h_layout_, 1);
button->setText(msg->team_id == 0 ? "Yellow" : "Blue");
button->setStyleSheet(
msg->team_id == 0
? "background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;"
: "background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;"
);
setLayout(v_layout_);
button->move(
static_cast<int>(msg->pose.x * ratioX - (w * ratioX) / 2),
static_cast<int>(800 - msg->pose.y * ratioY - (h * ratioY) / 2)
);
team_pub_ = node_->create_publisher<std_msgs::msg::Int8>("/strat/team", 10);
button->setFixedSize(w * ratioX, h * ratioY);
button->show();
connect(button, &QPushButton::clicked, this, [this, msg]()
{
modelec_interfaces::msg::Spawn team_msg;
team_msg.team_id = msg->team_id;
team_msg.name = msg->name;
spawn_pub_->publish(team_msg);
emit TeamChoose();
});
});
reset_strat_pub_ = node_->create_publisher<std_msgs::msg::Empty>("/strat/reset", 10);
connect(yellow_button_, &QPushButton::clicked, this, &HomePage::onYellowButtonClicked);
connect(blue_button_, &QPushButton::clicked, this, &HomePage::onBlueButtonClicked);
ask_spawn_client_ = node_->create_client<std_srvs::srv::Empty>("/nav/ask_spawn");
ask_spawn_client_->wait_for_service();
auto ask_spawn_request_ = std::make_shared<std_srvs::srv::Empty::Request>();
auto res = ask_spawn_client_->async_send_request(ask_spawn_request_);
rclcpp::spin_until_future_complete(node_->get_node_base_interface(), res);
}
void HomePage::Init()
@@ -48,24 +67,6 @@ namespace ModelecGUI
reset_strat_pub_->publish(std_msgs::msg::Empty());
}
void HomePage::onYellowButtonClicked()
{
std_msgs::msg::Int8 msg;
msg.data = 0;
team_pub_->publish(msg);
emit TeamChoose();
}
void HomePage::onBlueButtonClicked()
{
std_msgs::msg::Int8 msg;
msg.data = 1;
team_pub_->publish(msg);
emit TeamChoose();
}
void HomePage::paintEvent(QPaintEvent* paint_event)
{
QWidget::paintEvent(paint_event);

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

@@ -18,20 +18,22 @@ namespace ModelecGUI
v_layout = new QVBoxLayout(this);
timer_label_ = new QLabel("00", this);
timer_label_ = new QLabel("00 s", this);
timer_label_->setAlignment(Qt::AlignCenter);
timer_label_->setFont(QFont("Arial", 24));
timer_label_->setStyleSheet("QLabel { color: white; }");
timer_label_->setFont(QFont("Arial", 26));
timer_label_->setStyleSheet("QLabel { color: black; }");
score_label_ = new QLabel("Score: 0", this);
score_label_->setAlignment(Qt::AlignCenter);
score_label_->setFont(QFont("Arial", 24));
score_label_->setStyleSheet("QLabel { color: white; }");
score_label_->setFont(QFont("Arial", 26));
score_label_->setStyleSheet("QLabel { color: black; }");
h_layout = new QHBoxLayout(this);
h_layout->addStretch();
h_layout->addStretch();
h_layout->addWidget(score_label_);
h_layout->addStretch();
h_layout->addStretch();
h_layout->addStretch();
h_layout->addWidget(timer_label_);
h_layout->addStretch();
h_layout->addStretch();
@@ -213,7 +215,7 @@ namespace ModelecGUI
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));
timer_label_->setText(QString::number(elapsed_s) + " s");
}
QPainter painter(this);
@@ -255,7 +257,45 @@ namespace ModelecGUI
QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_);
painter.translate(obsPoint);
painter.rotate(90 - obs.theta * (180.0 / M_PI));
painter.setBrush(Qt::black);
if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN)
{
QPixmap texture(":/img/wood.jpg");
painter.setBrush(QBrush(texture));
}
else if (obs.id == 2)
{
QPixmap texture(":/img/logo/ISEN-Nantes.png");
texture = texture.scaled(obs.width * ratioBetweenMapAndWidgetX_,
obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio);
QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height());
QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
-(obs.height * ratioBetweenMapAndWidgetY_ / 2),
obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
painter.setBrush(Qt::white);
painter.setPen(Qt::NoPen);
painter.drawRect(toDraw);
painter.drawPixmap(imageRect.topLeft(), texture);
painter.restore();
continue;
}
else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE)
{
painter.setBrush(Qt::white);
painter.setPen(Qt::NoPen);
}
else
{
painter.setBrush(Qt::red);
painter.setOpacity(0.5);
painter.setPen(QPen(Qt::red, 5));
}
QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
-(obs.height * ratioBetweenMapAndWidgetY_ / 2),
@@ -269,7 +309,7 @@ namespace ModelecGUI
// -- Draw enemy position --
if (hasEnemy)
{
painter.setBrush(Qt::darkBlue);
painter.setBrush(Qt::red);
painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_,
height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_,
enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_);
@@ -280,11 +320,20 @@ namespace ModelecGUI
painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_);
painter.rotate(90 - robotPos.theta * (180.0 / M_PI));
QPixmap texture(":/img/logo/modelec.png");
texture = texture.scaled(robot_width_ * ratioBetweenMapAndWidgetX_ * 0.9,
robot_length_ * ratioBetweenMapAndWidgetY_ * 0.9, Qt::KeepAspectRatio);
QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height());
QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2),
robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_);
painter.setBrush(Qt::red);
painter.setPen(Qt::black);
robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_);
painter.setBrush(Qt::white);
painter.setPen(QPen(Qt::black, 5));
painter.drawRect(rect);
painter.drawPixmap(imageRect.topLeft(), texture);
}
void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg)

4
src/modelec_interfaces/CMakeLists.txt
View File

@@ -8,6 +8,7 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
@@ -23,6 +24,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Strat/StratState.msg"
"msg/Map/Map.msg"
"msg/Map/Obstacle.msg"
"msg/Map/Spawn.msg"
"msg/PCA9685Servo.msg"
"msg/ServoMode.msg"
"msg/Solenoid.msg"
@@ -50,7 +52,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/AddButton.srv"
"srv/Button.srv"
"srv/AddSerialListener.srv"
DEPENDENCIES std_msgs
DEPENDENCIES std_msgs geometry_msgs
)
if(BUILD_TESTING)

5
src/modelec_interfaces/msg/Map/Obstacle.msg
View File

@@ -1,6 +1,11 @@
string ESTRADE="estrade"
string GRADIN="gradin"
string PAMI_START="pami-start"
int32 id
int32 x
int32 y
float32 theta
int32 width
int32 height
string type

7
src/modelec_interfaces/msg/Map/Spawn.msg Normal file
View File

@@ -0,0 +1,7 @@
string TOP="top"
string BOTTOM="bottom"
string SIDE="side"
geometry_msgs/Pose2D pose
int32 team_id
string name

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

@@ -36,14 +36,24 @@
<yellow x="350" y="1600" theta="-1.5708"/>
</home>
<spawn>
<blue x="1775" y="300" theta="1.5708"/>
<yellow x="1225" y="300" theta="1.5708"/>
<yellow>
<top x="375" y="1775" theta="-1.5708" />
<side x="2775" y="875" theta="3.1415" />
<bottom x="1225" y="300" theta="1.5708" />
</yellow>
<blue>
<top x="2625" y="1775" theta="-1.5708" />
<side x="225" y="875" theta="0" />
<bottom x="1775" y="300" theta="1.5708" />
</blue>
<width_mm>450</width_mm>
<height_mm>450</height_mm>
</spawn>
<usb>
<pcb>
<pcb_odo>
<name>pcb_odo</name>
<port>/dev/pts/8</port>
<port>/dev/pts/4</port>
<baud_rate>115200</baud_rate>
</pcb_odo>
<pcb_alim>
@@ -53,7 +63,7 @@
</pcb_alim>
<pcb_action>
<name>pcb_action</name>
<port>/dev/pts/15</port>
<port>/dev/pts/9</port>
<baud_rate>115200</baud_rate>
</pcb_action>
</pcb>

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

@@ -5,6 +5,8 @@
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_go_to.hpp>
#include <modelec_interfaces/msg/spawn.hpp>
#include <std_srvs/srv/empty.hpp>
#include "deposite_zone.hpp"
#include "pathfinding.hpp"
@@ -88,7 +90,7 @@ namespace Modelec
void SetTeamId(int id);
void SetSpawn();
void SetSpawn(const std::string& name);
Point GetSpawnYellow() const;
Point GetSpawnBlue() const;
@@ -99,6 +101,8 @@ namespace Modelec
void OnPos(const PosMsg::SharedPtr msg);
void SetupSpawn();
struct
{
PosMsg::SharedPtr goal;
@@ -112,8 +116,9 @@ namespace Modelec
std::shared_ptr<Pathfinding> pathfinding_;
int team_id_ = YELLOW;
Point spawn_yellow_;
Point spawn_blue_;
std::map<std::string, Point> spawn_yellow_;
std::map<std::string, Point> spawn_blue_;
Point spawn_;
float factor_close_enemy_ = 0;
@@ -137,5 +142,9 @@ namespace Modelec
bool await_rotate_ = false;
std::vector<Waypoint> send_back_waypoints_;
rclcpp::Publisher<modelec_interfaces::msg::Spawn>::SharedPtr spawn_pub_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_spawn_srv_;
};
}

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

@@ -14,6 +14,7 @@
#include <std_msgs/msg/int8.hpp>
#include <modelec_interfaces/msg/strat_state.hpp>
#include <modelec_interfaces/msg/spawn.hpp>
#include <modelec_interfaces/srv/odometry_start.hpp>
#include <std_msgs/msg/empty.hpp>
@@ -64,6 +65,7 @@ namespace Modelec
rclcpp::Publisher<modelec_interfaces::msg::StratState>::SharedPtr state_pub_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr start_time_pub_;
rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr team_id_sub_;
rclcpp::Subscription<modelec_interfaces::msg::Spawn>::SharedPtr spawn_id_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;

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

@@ -16,13 +16,7 @@ namespace Modelec
factor_close_enemy_ = Config::get<float>("config.enemy.factor_close_enemy", -0.5f);
spawn_yellow_.x = Config::get<int>("config.spawn.yellow@x", 0);
spawn_yellow_.y = Config::get<int>("config.spawn.yellow@y", 0);
spawn_yellow_.theta = Config::get<double>("config.spawn.yellow@theta", 0);
spawn_blue_.x = Config::get<int>("config.spawn.blue@x", 0);
spawn_blue_.y = Config::get<int>("config.spawn.blue@y", 0);
spawn_blue_.theta = Config::get<double>("config.spawn.blue@theta", 0.0f);
SetupSpawn();
waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
"odometry/waypoint_reach", 10,
@@ -68,6 +62,37 @@ namespace Modelec
{
RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
}
spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("nav/spawn", 10);
ask_spawn_srv_ = node->create_service<std_srvs::srv::Empty>(
"/nav/ask_spawn", [this](const std_srvs::srv::Empty::Request::SharedPtr,
const std_srvs::srv::Empty::Response::SharedPtr)
{
for (auto& ys : spawn_yellow_)
{
auto s = modelec_interfaces::msg::Spawn();
s.team_id = YELLOW;
s.name = ys.first;
s.pose.x = ys.second.x;
s.pose.y = ys.second.y;
s.pose.theta = ys.second.theta;
spawn_pub_->publish(s);
}
for (auto& bs : spawn_blue_)
{
auto s = modelec_interfaces::msg::Spawn();
s.team_id = BLUE;
s.name = bs.first;
s.pose.x = bs.second.x;
s.pose.y = bs.second.y;
s.pose.theta = bs.second.theta;
spawn_pub_->publish(s);
}
});
}
rclcpp::Node::SharedPtr NavigationHelper::GetNode() const
@@ -553,43 +578,25 @@ namespace Modelec
team_id_ = id;
}
void NavigationHelper::SetSpawn()
void NavigationHelper::SetSpawn(const std::string& name)
{
switch (team_id_)
{
case YELLOW:
SetPos(spawn_yellow_);
SetPos(spawn_yellow_[name]);
break;
case BLUE:
SetPos(spawn_blue_);
SetPos(spawn_blue_[name]);
break;
default:
SetPos(spawn_yellow_);
SetPos(spawn_yellow_[name]);
break;
}
}
Point NavigationHelper::GetSpawnYellow() const
{
return spawn_yellow_;
}
Point NavigationHelper::GetSpawnBlue() const
{
return spawn_blue_;
}
Point NavigationHelper::GetSpawn() const
{
switch (team_id_)
{
case YELLOW:
return spawn_yellow_;
case BLUE:
return spawn_blue_;
default:
return spawn_yellow_;
}
return spawn_;
}
void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg)
@@ -620,4 +627,43 @@ namespace Modelec
current_pos_ = msg;
pathfinding_->SetCurrentPos(msg);
}
void NavigationHelper::SetupSpawn()
{
spawn_yellow_["top"] = Point(
Config::get<int>("config.spawn.yellow.top@x"),
Config::get<int>("config.spawn.yellow.top@y"),
Config::get<double>("config.spawn.yellow.top@theta")
);
spawn_yellow_["side"] = Point(
Config::get<int>("config.spawn.yellow.side@x"),
Config::get<int>("config.spawn.yellow.side@y"),
Config::get<double>("config.spawn.yellow.side@theta")
);
spawn_yellow_["bottom"] = Point(
Config::get<int>("config.spawn.yellow.bottom@x"),
Config::get<int>("config.spawn.yellow.bottom@y"),
Config::get<double>("config.spawn.yellow.bottom@theta")
);
spawn_blue_["top"] = Point(
Config::get<int>("config.spawn.blue.top@x"),
Config::get<int>("config.spawn.blue.top@y"),
Config::get<double>("config.spawn.blue.top@theta")
);
spawn_blue_["side"] = Point(
Config::get<int>("config.spawn.blue.side@x"),
Config::get<int>("config.spawn.blue.side@y"),
Config::get<double>("config.spawn.blue.side@theta")
);
spawn_blue_["bottom"] = Point(
Config::get<int>("config.spawn.blue.bottom@x"),
Config::get<int>("config.spawn.blue.bottom@y"),
Config::get<double>("config.spawn.blue.bottom@theta")
);
}
}

1
src/modelec_strat/src/obstacle/obstacle.cpp
View File

@@ -39,6 +39,7 @@ namespace Modelec
msg.width = w_;
msg.height = h_;
msg.theta = theta_;
msg.type = type_;
return msg;
}

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

@@ -23,10 +23,17 @@ namespace Modelec
team_id_sub_ = create_subscription<std_msgs::msg::Int8>(
"/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg)
{
setup_ = true;
team_id_ = static_cast<int>(msg->data);
nav_->SetTeamId(team_id_);
nav_->SetSpawn();
});
spawn_id_sub_ = create_subscription<modelec_interfaces::msg::Spawn>(
"/strat/spawn", 10, [this](const modelec_interfaces::msg::Spawn::SharedPtr msg)
{
setup_ = true;
team_id_ = msg->team_id;
nav_->SetTeamId(team_id_);
nav_->SetSpawn(msg->name);
});
reset_strat_sub_ = create_subscription<std_msgs::msg::Empty>(