diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 8de80ac..308e254 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -258,7 +258,7 @@ namespace Modelec void PCBActionInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg) { RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str()); - std::vector tokens = split(msg->data, ';'); + std::vector tokens = split(trim(msg->data), ';'); if (tokens.size() < 3) { @@ -274,6 +274,7 @@ namespace Modelec modelec_interfaces::msg::ActionAscPos asc_msg; asc_msg.pos = asc_state_; + asc_msg.value = asc_value_mapper_[asc_state_]; asc_msg.success = true; asc_get_res_pub_->publish(asc_msg); } @@ -286,6 +287,7 @@ namespace Modelec modelec_interfaces::msg::ActionServoPos servo_msg; servo_msg.id = servo_id; servo_msg.pos = v; + servo_msg.angle = servo_pos_mapper_[servo_id][v]; servo_msg.success = true; servo_get_res_pub_->publish(servo_msg); } diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index ca6b65a..33e7f0c 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -146,7 +146,7 @@ namespace Modelec void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg) { RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str()); - std::vector tokens = split(msg->data, ';'); + std::vector tokens = split(trim(msg->data), ';'); if (tokens.size() < 2) { RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str()); diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 4572fde..6a6e22f 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -187,7 +187,7 @@ namespace Modelec void PCBOdoInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg) { RCLCPP_DEBUG(this->get_logger(), "Received from PCB: %s", msg->data.c_str()); - std::vector tokens = split(msg->data, ';'); + std::vector tokens = split(trim(msg->data), ';'); if (tokens.size() < 2) { RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str()); diff --git a/src/modelec_gui/CMakeLists.txt b/src/modelec_gui/CMakeLists.txt index 0549eeb..1128147 100644 --- a/src/modelec_gui/CMakeLists.txt +++ b/src/modelec_gui/CMakeLists.txt @@ -37,12 +37,18 @@ add_executable(modelec_gui src/pages/home_page.cpp src/pages/test_map_page.cpp src/pages/map_page.cpp + src/pages/action_page.cpp + src/pages/alim_page.cpp + src/widget/action_widget.cpp include/modelec_gui/modelec_gui.hpp include/modelec_gui/pages/odo_page.hpp include/modelec_gui/pages/home_page.hpp include/modelec_gui/pages/test_map_page.hpp include/modelec_gui/pages/map_page.hpp + include/modelec_gui/pages/action_page.hpp + include/modelec_gui/pages/alim_page.hpp + include/modelec_gui/widget/action_widget.hpp ) ament_target_dependencies(modelec_gui diff --git a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp index 2e48db1..3336b87 100644 --- a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp +++ b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp @@ -8,6 +8,9 @@ #include #include #include +#include +#include + namespace ModelecGUI { @@ -31,10 +34,14 @@ namespace ModelecGUI TestMapPage* test_map_page_; OdoPage* odo_page_; MapPage* map_page_; + ActionPage* action_page_; + AlimPage* alim_page_; QAction* home_action_; QAction* odo_action_; QAction* map_action_; + QAction* action_action_; + QAction* alim_action_; QAction* exit_action_; QMenu* playmat_map_menu_; diff --git a/src/modelec_gui/include/modelec_gui/pages/action_page.hpp b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp new file mode 100644 index 0000000..38147b5 --- /dev/null +++ b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace ModelecGUI +{ + class ActionPage : public QWidget + { + Q_OBJECT + + public: + + using ActionAscPos = modelec_interfaces::msg::ActionAscPos; + using ActionServoPos = modelec_interfaces::msg::ActionServoPos; + using ActionRelayState = modelec_interfaces::msg::ActionRelayState; + + ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); + ~ActionPage() override; + + rclcpp::Node::SharedPtr get_node() const { return node_; } + + private: + + QVBoxLayout* layout_; + QHBoxLayout* asc_layout_; + + ActionWidget* asc_action_; + bool waiting_for_move_asc_; + + QGridLayout* servo_layout_; + + ActionWidget* servo0_action_; + ActionWidget* servo1_action_; + ActionWidget* servo2_action_; + ActionWidget* servo3_action_; + ActionWidget* servo4_action_; + ActionWidget* servo5_action_; + ActionWidget* servo6_action_; + ActionWidget* servo7_action_; + ActionWidget* servo8_action_; + std::vector servo_actions_; + std::vector waiting_for_move_servo_; + + QHBoxLayout* relay_layout_; + + QPushButton* relay_top_button_; + QPushButton* relay_bottom_button_; + std::vector relay_buttons_; + std::vector relay_values_; + + rclcpp::Node::SharedPtr node_; + + rclcpp::Subscription::SharedPtr asc_get_sub_; + rclcpp::Publisher::SharedPtr asc_set_pub_; + rclcpp::Subscription::SharedPtr asc_set_res_sub_; + rclcpp::Publisher::SharedPtr asc_move_pub_; + rclcpp::Subscription::SharedPtr asc_move_res_sub_; + + rclcpp::Subscription::SharedPtr servo_get_sub_; + rclcpp::Publisher::SharedPtr servo_set_pub_; + rclcpp::Subscription::SharedPtr servo_set_res_sub_; + rclcpp::Publisher::SharedPtr servo_move_pub_; + rclcpp::Subscription::SharedPtr servo_move_res_sub_; + + rclcpp::Subscription::SharedPtr relay_get_sub_; + rclcpp::Publisher::SharedPtr relay_move_pub_; + rclcpp::Subscription::SharedPtr relay_move_res_sub_; + }; +} // namespace Modelec diff --git a/src/modelec_gui/include/modelec_gui/pages/alim_page.hpp b/src/modelec_gui/include/modelec_gui/pages/alim_page.hpp new file mode 100644 index 0000000..14da727 --- /dev/null +++ b/src/modelec_gui/include/modelec_gui/pages/alim_page.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace ModelecGUI +{ + class AlimPage : public QWidget + { + Q_OBJECT + + public: + AlimPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); + ~AlimPage() override; + + rclcpp::Node::SharedPtr get_node() const { return node_; } + + private: + + rclcpp::Node::SharedPtr node_; + }; +} // namespace Modelec diff --git a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp index 2160d77..3bbe451 100644 --- a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp @@ -55,4 +55,4 @@ namespace ModelecGUI void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); }; -} // namespace Modelec +} // namespace Modelec GUI diff --git a/src/modelec_gui/include/modelec_gui/widget/action_widget.hpp b/src/modelec_gui/include/modelec_gui/widget/action_widget.hpp new file mode 100644 index 0000000..a54dd4c --- /dev/null +++ b/src/modelec_gui/include/modelec_gui/widget/action_widget.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include +#include + +namespace ModelecGUI +{ + class ActionWidget : public QWidget + { + Q_OBJECT + public: + ActionWidget(QWidget *parent = 0); + + ~ActionWidget(); + + void SetButtonText(const QString& text); + void SetSpinBoxValue(double value); + void SetSpinBoxRange(double min, double max); + void SetSpinBoxStep(double step); + + double GetSpinBoxValue(); + + protected slots: + void OnButtonClicked(); + + signals: + void ButtonClicked(double value); + + private: + + QHBoxLayout* layout_; + QDoubleSpinBox* spinBox_; + QPushButton* pushButton_; + }; +} \ 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 5d95401..8bb770c 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -16,11 +16,15 @@ namespace ModelecGUI { odo_page_ = new OdoPage(get_node(), this); test_map_page_ = new TestMapPage(get_node(), this); map_page_ = new MapPage(get_node(), this); + action_page_ = new ActionPage(get_node(), this); + alim_page_ = new AlimPage(get_node(), this); stackedWidget->addWidget(home_page_); stackedWidget->addWidget(odo_page_); stackedWidget->addWidget(test_map_page_); stackedWidget->addWidget(map_page_); + stackedWidget->addWidget(action_page_); + stackedWidget->addWidget(alim_page_); setCentralWidget(stackedWidget); setupMenu(); @@ -38,6 +42,8 @@ namespace ModelecGUI { home_action_ = new QAction("Home", this); odo_action_ = new QAction("Odometrie", this); + action_action_ = new QAction("Action", this); + alim_action_ = new QAction("Alim", this); map_action_ = new QAction("Map", this); playmat_map_menu_ = new QMenu("playmat"); @@ -50,6 +56,8 @@ namespace ModelecGUI { optionsMenu->addAction(home_action_); optionsMenu->addAction(odo_action_); + optionsMenu->addAction(action_action_); + optionsMenu->addAction(alim_action_); optionsMenu->addSeparator(); optionsMenu->addAction(map_action_); optionsMenu->addMenu(playmat_map_menu_); @@ -69,6 +77,14 @@ namespace ModelecGUI { stackedWidget->setCurrentIndex(1); }); + connect(action_action_, &QAction::triggered, this, [this]() { + stackedWidget->setCurrentIndex(4); + }); + + connect(alim_action_, &QAction::triggered, this, [this]() { + stackedWidget->setCurrentIndex(5); + }); + connect(map_action_, &QAction::triggered, this, [this]() { stackedWidget->setCurrentIndex(2); }); diff --git a/src/modelec_gui/src/pages/action_page.cpp b/src/modelec_gui/src/pages/action_page.cpp new file mode 100644 index 0000000..bd5daae --- /dev/null +++ b/src/modelec_gui/src/pages/action_page.cpp @@ -0,0 +1,254 @@ +#include + +namespace ModelecGUI +{ + ActionPage::ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent) : + QWidget(parent), + node_(node) + { + layout_ = new QVBoxLayout(this); + setLayout(layout_); + setWindowTitle("Action Page"); + + asc_layout_ = new QHBoxLayout; + + asc_action_ = new ActionWidget(this); + asc_action_->SetSpinBoxRange(0, 1000); + asc_action_->SetSpinBoxStep(1); + asc_action_->SetSpinBoxValue(0); + asc_action_->SetButtonText("Send ASC"); + + waiting_for_move_asc_ = false; + + connect(asc_action_, &ActionWidget::ButtonClicked, this, + [this](double value) + { + ActionAscPos asc_move; + asc_move.value = value; + asc_move.pos = 128; + asc_set_pub_->publish(asc_move); + asc_action_->setDisabled(true); + + waiting_for_move_asc_ = true; + }); + + asc_layout_->addWidget(asc_action_); + + servo_layout_ = new QGridLayout; + + servo0_action_ = new ActionWidget(this); + servo0_action_->SetSpinBoxRange(0, 180); + servo0_action_->SetSpinBoxStep(.1); + servo0_action_->SetSpinBoxValue(90); + servo0_action_->SetButtonText("Servo 0"); + + servo1_action_ = new ActionWidget(this); + servo1_action_->SetSpinBoxRange(0, 180); + servo1_action_->SetSpinBoxStep(.1); + servo1_action_->SetSpinBoxValue(90); + servo1_action_->SetButtonText("Servo 1"); + + servo2_action_ = new ActionWidget(this); + servo2_action_->SetSpinBoxRange(0, 180); + servo2_action_->SetSpinBoxStep(.1); + servo2_action_->SetSpinBoxValue(90); + servo2_action_->SetButtonText("Servo 2"); + + servo3_action_ = new ActionWidget(this); + servo3_action_->SetSpinBoxRange(0, 180); + servo3_action_->SetSpinBoxStep(.1); + servo3_action_->SetSpinBoxValue(90); + servo3_action_->SetButtonText("Servo 3"); + + servo4_action_ = new ActionWidget(this); + servo4_action_->SetSpinBoxRange(0, 180); + servo4_action_->SetSpinBoxStep(.1); + servo4_action_->SetSpinBoxValue(90); + servo4_action_->SetButtonText("Servo 4"); + + servo5_action_ = new ActionWidget(this); + servo5_action_->SetSpinBoxRange(0, 180); + servo5_action_->SetSpinBoxStep(.1); + servo5_action_->SetSpinBoxValue(90); + servo5_action_->SetButtonText("Servo 5"); + + servo6_action_ = new ActionWidget(this); + servo6_action_->SetSpinBoxRange(0, 180); + servo6_action_->SetSpinBoxStep(.1); + servo6_action_->SetSpinBoxValue(90); + servo6_action_->SetButtonText("Servo 6"); + + servo7_action_ = new ActionWidget(this); + servo7_action_->SetSpinBoxRange(0, 180); + servo7_action_->SetSpinBoxStep(.1); + servo7_action_->SetSpinBoxValue(90); + servo7_action_->SetButtonText("Servo 7"); + + servo8_action_ = new ActionWidget(this); + servo8_action_->SetSpinBoxRange(0, 180); + servo8_action_->SetSpinBoxStep(.1); + servo8_action_->SetSpinBoxValue(90); + servo8_action_->SetButtonText("Servo 8"); + + servo_layout_->addWidget(servo0_action_, 0, 0); + servo_layout_->addWidget(servo1_action_, 0, 1); + servo_layout_->addWidget(servo2_action_, 0, 2); + servo_layout_->addWidget(servo3_action_, 1, 0); + servo_layout_->addWidget(servo4_action_, 1, 1); + servo_layout_->addWidget(servo5_action_, 1, 2); + servo_layout_->addWidget(servo6_action_, 2, 0); + servo_layout_->addWidget(servo7_action_, 2, 1); + servo_layout_->addWidget(servo8_action_, 2, 2); + + servo_actions_.push_back(servo0_action_); + servo_actions_.push_back(servo1_action_); + servo_actions_.push_back(servo2_action_); + servo_actions_.push_back(servo3_action_); + servo_actions_.push_back(servo4_action_); + servo_actions_.push_back(servo5_action_); + servo_actions_.push_back(servo6_action_); + servo_actions_.push_back(servo7_action_); + servo_actions_.push_back(servo8_action_); + waiting_for_move_servo_ = std::vector(servo_actions_.size(), false); + + for (size_t i = 0; i < servo_actions_.size(); ++i) + { + connect(servo_actions_[i], &ActionWidget::ButtonClicked, this, + [this, i](double value) + { + ActionServoPos servo_move; + servo_move.id = i; + servo_move.pos = 128; + servo_move.angle = value; + servo_set_pub_->publish(servo_move); + servo_actions_[i]->setDisabled(true); + + waiting_for_move_servo_[i] = true; + }); + } + + relay_layout_ = new QHBoxLayout; + + relay_top_button_ = new QPushButton(this); + relay_top_button_->setText("Toggle Relay Top"); + + relay_bottom_button_ = new QPushButton(this); + relay_bottom_button_->setText("Toggle Relay Bottom"); + + relay_layout_->addWidget(relay_top_button_); + relay_layout_->addWidget(relay_bottom_button_); + + relay_buttons_.push_back(relay_top_button_); + relay_buttons_.push_back(relay_bottom_button_); + relay_values_ = {false, false}; + + for (size_t i = 0; i < relay_buttons_.size(); ++i) + { + connect(relay_buttons_[i], &QPushButton::clicked, this, + [this, i]() + { + ActionRelayState relay_state; + relay_state.id = i; + relay_state.state = !relay_values_[i]; + relay_move_pub_->publish(relay_state); + relay_buttons_[i]->setDisabled(true); + }); + } + + layout_->addLayout(asc_layout_); + layout_->addLayout(servo_layout_); + layout_->addLayout(relay_layout_); + + asc_get_sub_ = node_->create_subscription( + "action/get/asc", 10, [this](const ActionAscPos::SharedPtr msg) + { + asc_action_->SetSpinBoxValue(msg->value); + }); + + asc_set_pub_ = node_->create_publisher( + "action/set/asc", 10); + + asc_set_res_sub_ = node_->create_subscription( + "action/set/asc/res", 10, [this](const ActionAscPos::SharedPtr msg) + { + if (waiting_for_move_asc_) + { + ActionAscPos asc_move; + asc_move.pos = msg->pos; + asc_move_pub_->publish(asc_move); + waiting_for_move_asc_ = false; + } + }); + + asc_move_pub_ = node_->create_publisher( + "action/move/asc", 10); + + asc_move_res_sub_ = node_->create_subscription( + "action/move/asc/res", 10, [this](const ActionAscPos::SharedPtr) + { + asc_action_->setDisabled(false); + }); + + servo_get_sub_ = node_->create_subscription( + "action/get/servo", 10, [this](const ActionServoPos::SharedPtr msg) + { + if (static_cast(servo_actions_.size()) > msg->id && servo_actions_[msg->id] != nullptr) + { + servo_actions_[msg->id]->SetSpinBoxValue(msg->angle); + } + }); + + servo_set_pub_ = node_->create_publisher( + "action/set/servo", 10); + + servo_set_res_sub_ = node_->create_subscription( + "action/set/servo/res", 10, [this](const ActionServoPos::SharedPtr msg) + { + if (waiting_for_move_servo_[msg->id]) + { + ActionServoPos servo_move; + servo_move.id = msg->id; + servo_move.pos = msg->pos; + servo_move_pub_->publish(servo_move); + waiting_for_move_servo_[msg->id] = false; + } + }); + + servo_move_pub_ = node_->create_publisher( + "action/move/servo", 10); + + servo_move_res_sub_ = node_->create_subscription( + "action/move/servo/res", 10, [this](const ActionServoPos::SharedPtr msg) + { + if (static_cast(servo_actions_.size()) > msg->id && servo_actions_[msg->id] != nullptr) + { + servo_actions_[msg->id]->setDisabled(false); + } + }); + + relay_get_sub_ = node_->create_subscription( + "action/get/relay", 10, [this](const ActionRelayState::SharedPtr msg) + { + if (static_cast(relay_values_.size()) > msg->id) + { + relay_values_[msg->id] = msg->state; + } + }); + + relay_move_pub_ = node_->create_publisher( + "action/move/relay", 10); + + relay_move_res_sub_ = node_->create_subscription( + "action/move/relay/res", 10, [this](const ActionRelayState::SharedPtr msg) + { + if (static_cast(relay_buttons_.size()) > msg->id && relay_buttons_[msg->id] != nullptr && static_cast(relay_values_.size()) > msg->id) + { + relay_buttons_[msg->id]->setDisabled(false); + relay_values_[msg->id] = msg->state; + } + }); + } + + ActionPage::~ActionPage() + = default; +} diff --git a/src/modelec_gui/src/pages/alim_page.cpp b/src/modelec_gui/src/pages/alim_page.cpp new file mode 100644 index 0000000..d28aa64 --- /dev/null +++ b/src/modelec_gui/src/pages/alim_page.cpp @@ -0,0 +1,13 @@ +#include + +namespace ModelecGUI +{ + AlimPage::AlimPage(rclcpp::Node::SharedPtr node, QWidget* parent) : + QWidget(parent), + node_(node) + { + } + + AlimPage::~AlimPage() + = default; +} diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 944f072..9dba091 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -27,7 +27,7 @@ namespace ModelecGUI score_label_->setFont(QFont("Arial", 26)); score_label_->setStyleSheet("QLabel { color: black; }"); - h_layout = new QHBoxLayout(this); + h_layout = new QHBoxLayout(); h_layout->addStretch(); h_layout->addStretch(); h_layout->addWidget(score_label_); diff --git a/src/modelec_gui/src/widget/action_widget.cpp b/src/modelec_gui/src/widget/action_widget.cpp new file mode 100644 index 0000000..81dc947 --- /dev/null +++ b/src/modelec_gui/src/widget/action_widget.cpp @@ -0,0 +1,69 @@ +#include + +namespace ModelecGUI +{ + ActionWidget::ActionWidget(QWidget* parent) : QWidget(parent) + { + layout_ = new QHBoxLayout(this); + setLayout(layout_); + + spinBox_ = new QDoubleSpinBox(this); + pushButton_ = new QPushButton(this); + connect(pushButton_, &QPushButton::clicked, this, &ActionWidget::OnButtonClicked); + + layout_->addWidget(spinBox_); + layout_->addWidget(pushButton_); + + layout_->setStretch(0, 4); + layout_->setStretch(1, 1); + } + + ActionWidget::~ActionWidget() + { + delete layout_; + delete spinBox_; + delete pushButton_; + } + + void ActionWidget::SetButtonText(const QString& text) + { + if (pushButton_) + { + pushButton_->setText(text); + } + } + + void ActionWidget::SetSpinBoxValue(double value) + { + if (spinBox_) + { + spinBox_->setValue(value); + } + } + + void ActionWidget::SetSpinBoxRange(double min, double max) + { + if (spinBox_) + { + spinBox_->setRange(min, max); + } + } + + void ActionWidget::SetSpinBoxStep(double step) + { + if (spinBox_) + { + spinBox_->setSingleStep(step); + } + } + + double ActionWidget::GetSpinBoxValue() + { + return spinBox_ ? spinBox_->value() : 0.0; + } + + void ActionWidget::OnButtonClicked() + { + emit ButtonClicked(GetSpinBoxValue()); + } +} diff --git a/src/modelec_interfaces/msg/Action/ActionAscPos.msg b/src/modelec_interfaces/msg/Action/ActionAscPos.msg index 4973630..e329493 100644 --- a/src/modelec_interfaces/msg/Action/ActionAscPos.msg +++ b/src/modelec_interfaces/msg/Action/ActionAscPos.msg @@ -1,3 +1,3 @@ int32 pos -int32 value +float64 value bool success diff --git a/src/modelec_interfaces/msg/Action/ActionServoPos.msg b/src/modelec_interfaces/msg/Action/ActionServoPos.msg index 84b7339..5a45d99 100644 --- a/src/modelec_interfaces/msg/Action/ActionServoPos.msg +++ b/src/modelec_interfaces/msg/Action/ActionServoPos.msg @@ -1,4 +1,4 @@ int32 id int32 pos -float32 angle +float64 angle bool success \ No newline at end of file diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index e40418b..6e6571d 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -53,17 +53,17 @@ pcb_odo - /dev/pts/4 + /tmp/USB_ODO 115200 pcb_alim - /dev/pts/12 + /tmp/USB_ALIM 115200 pcb_action - /dev/pts/9 + /tmp/USB_ACTION 115200 diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 4913c10..668d326 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -59,6 +59,7 @@ namespace Modelec rclcpp::Time match_start_time_; bool started_ = false; bool setup_ = false; + bool team_selected_ = false; std::unique_ptr current_mission_; int team_id_ = 0; bool is_banner_done_ = false; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index ec46161..ae19eac 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -30,6 +30,7 @@ namespace Modelec spawn_id_sub_ = create_subscription( "/strat/spawn", 10, [this](const modelec_interfaces::msg::Spawn::SharedPtr msg) { + team_selected_ = true; team_id_ = msg->team_id; nav_->SetTeamId(team_id_); nav_->SetSpawn(msg->name); @@ -103,6 +104,7 @@ namespace Modelec state_ = State::INIT; started_ = false; + team_selected_ = false; current_mission_.reset(); match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); @@ -130,7 +132,7 @@ namespace Modelec switch (state_) { case State::INIT: - if (setup_) + if (setup_ && team_selected_) { Transition(State::WAIT_START, "System ready"); } diff --git a/src/modelec_utils/include/modelec_utils/utils.hpp b/src/modelec_utils/include/modelec_utils/utils.hpp index 44657e2..39699d8 100644 --- a/src/modelec_utils/include/modelec_utils/utils.hpp +++ b/src/modelec_utils/include/modelec_utils/utils.hpp @@ -21,4 +21,6 @@ namespace Modelec { T mapValue(T v, T v_min, T v_max, T v_min_prime, T v_max_prime) { return v_min_prime + (((v - v_min) * (v_max_prime - v_min_prime)) / (v_max - v_min)); } + + std::string trim(const std::string &s); } diff --git a/src/modelec_utils/src/utils.cpp b/src/modelec_utils/src/utils.cpp index b3824b8..f7c5c50 100644 --- a/src/modelec_utils/src/utils.cpp +++ b/src/modelec_utils/src/utils.cpp @@ -38,4 +38,14 @@ namespace Modelec { { return s.find(substring) != std::string::npos; } + + std::string trim(const std::string& s) + { + size_t start = s.find_first_not_of(" \t\n\r\f\v"); + if (start == std::string::npos) + return ""; + + size_t end = s.find_last_not_of(" \t\n\r\f\v"); + return s.substr(start, end - start + 1); + } }; \ No newline at end of file