diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 5442659..ea6106c 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -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, diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index ce0c2a6..fd9e231 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -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, diff --git a/src/modelec_gui/img/logo/ISEN-Nantes-fond-blanc.png b/src/modelec_gui/img/logo/ISEN-Nantes-fond-blanc.png new file mode 100644 index 0000000..d42aad3 Binary files /dev/null and b/src/modelec_gui/img/logo/ISEN-Nantes-fond-blanc.png differ diff --git a/src/modelec_gui/img/logo/ISEN-Nantes-fond-blanc.svg b/src/modelec_gui/img/logo/ISEN-Nantes-fond-blanc.svg new file mode 100644 index 0000000..030637a --- /dev/null +++ b/src/modelec_gui/img/logo/ISEN-Nantes-fond-blanc.svg @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + diff --git a/src/modelec_gui/img/logo/ISEN-Nantes.png b/src/modelec_gui/img/logo/ISEN-Nantes.png new file mode 100644 index 0000000..51d9fa3 Binary files /dev/null and b/src/modelec_gui/img/logo/ISEN-Nantes.png differ diff --git a/src/modelec_gui/img/logo/modelec.png b/src/modelec_gui/img/logo/modelec.png new file mode 100644 index 0000000..e0cd0b4 Binary files /dev/null and b/src/modelec_gui/img/logo/modelec.png differ diff --git a/src/modelec_gui/img/wood.jpg b/src/modelec_gui/img/wood.jpg new file mode 100644 index 0000000..ac58eeb Binary files /dev/null and b/src/modelec_gui/img/wood.jpg differ diff --git a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp index d2e822a..1f423dd 100644 --- a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp @@ -9,11 +9,15 @@ #include #include +#include + +#include 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::SharedPtr team_pub_; + rclcpp::Publisher::SharedPtr spawn_pub_; + rclcpp::Subscription::SharedPtr spawn_sub_; rclcpp::Publisher::SharedPtr reset_strat_pub_; - - QVBoxLayout* v_layout_; - QHBoxLayout* h_layout_; - - QPushButton* blue_button_; - QPushButton* yellow_button_; + rclcpp::Client::SharedPtr ask_spawn_client_; QSvgRenderer* renderer_; + + std::vector spawn_buttons_; }; } diff --git a/src/modelec_gui/resource.qrc b/src/modelec_gui/resource.qrc index b5ed637..5e3f88a 100644 --- a/src/modelec_gui/resource.qrc +++ b/src/modelec_gui/resource.qrc @@ -3,5 +3,10 @@ img/playmat/2025_FINAL.svg img/playmat/grid_v1.svg + img/logo/ISEN-Nantes.png + img/logo/ISEN-Nantes-fond-blanc.svg + img/logo/ISEN-Nantes-fond-blanc.png + img/logo/modelec.png + img/wood.jpg \ No newline at end of file diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index 4936e19..5d95401 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -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); diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index 3c9ed8b..5da77c1 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -2,6 +2,9 @@ #include #include +#include + +#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("/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("config.spawn.width_mm"); + auto h = Modelec::Config::get("config.spawn.height_mm"); - yellow_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - blue_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + spawn_sub_ = node_->create_subscription("/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(msg->pose.x * ratioX - (w * ratioX) / 2), + static_cast(800 - msg->pose.y * ratioY - (h * ratioY) / 2) + ); - team_pub_ = node_->create_publisher("/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("/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("/nav/ask_spawn"); + ask_spawn_client_->wait_for_service(); + auto ask_spawn_request_ = std::make_shared(); + 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); diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index bb4d4d0..944f072 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -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(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) diff --git a/src/modelec_interfaces/CMakeLists.txt b/src/modelec_interfaces/CMakeLists.txt index 6ef854f..ae08d0a 100644 --- a/src/modelec_interfaces/CMakeLists.txt +++ b/src/modelec_interfaces/CMakeLists.txt @@ -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) diff --git a/src/modelec_interfaces/msg/Map/Obstacle.msg b/src/modelec_interfaces/msg/Map/Obstacle.msg index 0200577..e524d88 100644 --- a/src/modelec_interfaces/msg/Map/Obstacle.msg +++ b/src/modelec_interfaces/msg/Map/Obstacle.msg @@ -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 \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Map/Spawn.msg b/src/modelec_interfaces/msg/Map/Spawn.msg new file mode 100644 index 0000000..55355f7 --- /dev/null +++ b/src/modelec_interfaces/msg/Map/Spawn.msg @@ -0,0 +1,7 @@ +string TOP="top" +string BOTTOM="bottom" +string SIDE="side" + +geometry_msgs/Pose2D pose +int32 team_id +string name diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index aa908d6..e40418b 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -36,14 +36,24 @@ - - + + + + + + + + + + + 450 + 450 pcb_odo - /dev/pts/8 + /dev/pts/4 115200 @@ -53,7 +63,7 @@ pcb_action - /dev/pts/15 + /dev/pts/9 115200 diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index e21cf4a..1c3f749 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -5,6 +5,8 @@ #include #include #include +#include +#include #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_; int team_id_ = YELLOW; - Point spawn_yellow_; - Point spawn_blue_; + std::map spawn_yellow_; + std::map spawn_blue_; + Point spawn_; float factor_close_enemy_ = 0; @@ -137,5 +142,9 @@ namespace Modelec bool await_rotate_ = false; std::vector send_back_waypoints_; + + rclcpp::Publisher::SharedPtr spawn_pub_; + rclcpp::Service::SharedPtr ask_spawn_srv_; + }; } diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index f8f3148..bc2c05b 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -64,6 +65,7 @@ namespace Modelec rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Publisher::SharedPtr start_time_pub_; rclcpp::Subscription::SharedPtr team_id_sub_; + rclcpp::Subscription::SharedPtr spawn_id_sub_; rclcpp::Subscription::SharedPtr reset_strat_sub_; rclcpp::Client::SharedPtr client_start_; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index b794d8d..7f48c56 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -16,13 +16,7 @@ namespace Modelec factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); - spawn_yellow_.x = Config::get("config.spawn.yellow@x", 0); - spawn_yellow_.y = Config::get("config.spawn.yellow@y", 0); - spawn_yellow_.theta = Config::get("config.spawn.yellow@theta", 0); - - spawn_blue_.x = Config::get("config.spawn.blue@x", 0); - spawn_blue_.y = Config::get("config.spawn.blue@y", 0); - spawn_blue_.theta = Config::get("config.spawn.blue@theta", 0.0f); + SetupSpawn(); waypoint_reach_sub_ = node_->create_subscription( "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("nav/spawn", 10); + + ask_spawn_srv_ = node->create_service( + "/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("config.spawn.yellow.top@x"), + Config::get("config.spawn.yellow.top@y"), + Config::get("config.spawn.yellow.top@theta") + ); + + spawn_yellow_["side"] = Point( + Config::get("config.spawn.yellow.side@x"), + Config::get("config.spawn.yellow.side@y"), + Config::get("config.spawn.yellow.side@theta") + ); + + spawn_yellow_["bottom"] = Point( + Config::get("config.spawn.yellow.bottom@x"), + Config::get("config.spawn.yellow.bottom@y"), + Config::get("config.spawn.yellow.bottom@theta") + ); + + spawn_blue_["top"] = Point( + Config::get("config.spawn.blue.top@x"), + Config::get("config.spawn.blue.top@y"), + Config::get("config.spawn.blue.top@theta") + ); + + spawn_blue_["side"] = Point( + Config::get("config.spawn.blue.side@x"), + Config::get("config.spawn.blue.side@y"), + Config::get("config.spawn.blue.side@theta") + ); + + spawn_blue_["bottom"] = Point( + Config::get("config.spawn.blue.bottom@x"), + Config::get("config.spawn.blue.bottom@y"), + Config::get("config.spawn.blue.bottom@theta") + ); + } } diff --git a/src/modelec_strat/src/obstacle/obstacle.cpp b/src/modelec_strat/src/obstacle/obstacle.cpp index 839a2e0..5fbdb1f 100644 --- a/src/modelec_strat/src/obstacle/obstacle.cpp +++ b/src/modelec_strat/src/obstacle/obstacle.cpp @@ -39,6 +39,7 @@ namespace Modelec msg.width = w_; msg.height = h_; msg.theta = theta_; + msg.type = type_; return msg; } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 6f5ba9b..ee68ea3 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -23,10 +23,17 @@ namespace Modelec team_id_sub_ = create_subscription( "/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg) { - setup_ = true; team_id_ = static_cast(msg->data); nav_->SetTeamId(team_id_); - nav_->SetSpawn(); + }); + + spawn_id_sub_ = create_subscription( + "/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(