diff --git a/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp b/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp index d28439e..538f7d4 100644 --- a/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp +++ b/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp @@ -40,9 +40,13 @@ public: ~SerialListener(); + void close(); + void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; } bool IsOk() const { return status_; } + void SetOk() { status_ = true; } + void write(std_msgs::msg::String::SharedPtr msg); }; diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index 01967dd..25027f5 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -145,7 +145,7 @@ public: void SetRobotPos(long x, long y, long theta) const; void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const; - void AddWaypoint(int index, bool IsStopPoint, long x, long y, long theta) const; + void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta) const; void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const; void SetStart(bool start) const; diff --git a/src/modelec_com/src/multiple_serial_listener.cpp b/src/modelec_com/src/multiple_serial_listener.cpp index caa4964..66aede7 100644 --- a/src/modelec_com/src/multiple_serial_listener.cpp +++ b/src/modelec_com/src/multiple_serial_listener.cpp @@ -38,20 +38,36 @@ namespace Modelec SerialListener::~SerialListener() { - if (port_.is_open()) port_.close(); - io_.stop(); - if (io_thread_.joinable()) io_thread_.join(); + if (status_) + { + close(); + } + } + + void SerialListener::close() + { + if (status_) { + if (port_.is_open()) port_.close(); + io_.stop(); + if (io_thread_.joinable()) io_thread_.join(); + status_ = false; + } } void SerialListener::start_async_read() { + if (!status_) start_async_read(); + port_.async_read_some( boost::asio::buffer(read_buffer_), [this](const boost::system::error_code& ec, std::size_t bytes_transferred) { if (!ec && bytes_transferred > 0) { auto msg = std_msgs::msg::String(); msg.data = std::string(read_buffer_.begin(), read_buffer_.begin() + bytes_transferred); - publisher_->publish(msg); + if (publisher_) + { + publisher_->publish(msg); + } start_async_read(); // continue reading } else { @@ -103,7 +119,7 @@ namespace Modelec { for (auto& listener : serial_listeners) { - listener.second->port_.close(); + listener.second->close(); } } @@ -140,6 +156,8 @@ namespace Modelec response->success = true; response->publisher = listener->publisher_->get_topic_name(); response->subscriber = listener->subscriber_->get_topic_name(); + + RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Serial listener %s fully created", request->name.c_str()); } rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange( diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 1e16200..281a1ae 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_alim"; request->bauds = 115200; - request->serial_port = "/dev/pts/12"; // TODO : check the real serial port + request->serial_port = "/dev/pts/11"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { @@ -427,6 +427,7 @@ namespace Modelec { command += ";" + d; } + command += "\n"; SendToPCB(command); } diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index bcf69a6..eaf4235 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_odo"; request->bauds = 115200; - request->serial_port = "/dev/pts/7"; // TODO : check the real serial port + request->serial_port = "/dev/pts/6"; // TODO : check the real serial port auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { @@ -84,7 +84,7 @@ namespace Modelec "odometry/get_pid", 10); odo_add_waypoint_subscriber_ = this->create_subscription( - "odometry/add_waypoint", 10, + "odometry/add_waypoint", 30, [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) { AddWaypointCallback(msg); @@ -534,6 +534,7 @@ namespace Modelec { command += ";" + d; } + command += "\n"; SendToPCB(command); } @@ -586,7 +587,7 @@ namespace Modelec } void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y, - const long theta) const + const double theta) const { std::vector data = { std::to_string(index), diff --git a/src/modelec_core/launch/test.modelec.launch.yml b/src/modelec_core/launch/test.modelec.launch.yml index af31724..b122144 100644 --- a/src/modelec_core/launch/test.modelec.launch.yml +++ b/src/modelec_core/launch/test.modelec.launch.yml @@ -21,6 +21,6 @@ launch: name: "modelec_gui" - node: - pkg: 'modelec' + pkg: 'modelec_core' exec: 'speed_result' name: 'speed_result' diff --git a/src/modelec_gui/CMakeLists.txt b/src/modelec_gui/CMakeLists.txt index 90172f6..9944a4a 100644 --- a/src/modelec_gui/CMakeLists.txt +++ b/src/modelec_gui/CMakeLists.txt @@ -7,28 +7,40 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif () set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) +set(CMAKE_AUTOUIC ON) # Find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(modelec_interfaces REQUIRED) +find_package(modelec_utils COMPONENTS + modelec_utils + REQUIRED +) + find_package(Qt6 COMPONENTS Core Gui Widgets + Svg REQUIRED) # Add the executable add_executable(modelec_gui + resource.qrc + src/main.cpp src/modelec_gui.cpp src/pages/test_page.cpp src/pages/home_page.cpp + src/pages/map_page.cpp include/modelec_gui/modelec_gui.hpp include/modelec_gui/pages/test_page.hpp include/modelec_gui/pages/home_page.hpp + include/modelec_gui/pages/map_page.hpp ) ament_target_dependencies(modelec_gui rclcpp @@ -39,6 +51,8 @@ target_link_libraries(modelec_gui Qt6::Core Qt6::Gui Qt6::Widgets + Qt6::Svg + modelec_utils::modelec_utils ) target_include_directories(modelec_gui PUBLIC diff --git a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp index 9b02866..9bc80f3 100644 --- a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp +++ b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp @@ -26,7 +26,12 @@ namespace ModelecGUI QAction* home_action_; QAction* test_action_; + QAction* map_action_; QAction* exit_action_; + QMenu* playmat_map_menu_; + QAction* playmat_map_; + QAction* playmat_grid_; + }; } diff --git a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp new file mode 100644 index 0000000..aceecc4 --- /dev/null +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace ModelecGUI { + class MapPage : public QWidget + { + Q_OBJECT + public: + MapPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr); + + rclcpp::Node::SharedPtr get_node() const { return node_; } + + void setPlaymatGrid(); + + void setPlaymatMap(); + + protected: + void paintEvent(QPaintEvent*) override; + + void mousePressEvent(QMouseEvent* event) override; + + void onStartButtonClicked(); + + void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); + + QSvgRenderer* renderer; + + QPushButton* startButton; + + QVBoxLayout* v_layout; + QHBoxLayout* h_layout; + + QPoint robotPosPoint = QPoint(0, 0); + std::vector qpoints; + std::vector points; + + rclcpp::Node::SharedPtr node_; + + rclcpp::Publisher::SharedPtr add_waypoint_publisher_; + rclcpp::Subscription::SharedPtr odometry_subscriber_; + }; +} diff --git a/src/modelec_gui/resource.qrc b/src/modelec_gui/resource.qrc new file mode 100644 index 0000000..b5ed637 --- /dev/null +++ b/src/modelec_gui/resource.qrc @@ -0,0 +1,7 @@ + + + + img/playmat/2025_FINAL.svg + img/playmat/grid_v1.svg + + \ 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 ea599e3..638a631 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include namespace ModelecGUI { @@ -13,6 +14,7 @@ namespace ModelecGUI { // Add pages to stack stackedWidget->addWidget(new HomePage(this)); stackedWidget->addWidget(new TestPage(get_node(), this)); + stackedWidget->addWidget(new MapPage(get_node(), this)); setCentralWidget(stackedWidget); setupMenu(); @@ -23,15 +25,26 @@ namespace ModelecGUI { void ROS2QtGUI::setupMenu() { QMenuBar *menuBar = this->menuBar(); - QMenu *optionsMenu = menuBar->addMenu("Options"); + QMenu *optionsMenu = menuBar->addMenu("View"); home_action_ = new QAction("Home", this); test_action_ = new QAction("Test", this); + map_action_ = new QAction("Map", this); + + playmat_map_menu_ = new QMenu("playmat"); + playmat_map_ = new QAction("Map", this); + playmat_grid_ = new QAction("Grid", this); + exit_action_ = new QAction("Exit", this); optionsMenu->addAction(home_action_); optionsMenu->addAction(test_action_); optionsMenu->addSeparator(); + optionsMenu->addAction(map_action_); + optionsMenu->addMenu(playmat_map_menu_); + playmat_map_menu_->addAction(playmat_map_); + playmat_map_menu_->addAction(playmat_grid_); + optionsMenu->addSeparator(); optionsMenu->addAction(exit_action_); connect(home_action_, &QAction::triggered, this, [this]() { @@ -42,6 +55,24 @@ namespace ModelecGUI { stackedWidget->setCurrentIndex(1); }); + connect(map_action_, &QAction::triggered, this, [this]() { + stackedWidget->setCurrentIndex(2); + }); + + connect(playmat_map_, &QAction::triggered, this, [this]() { + auto map_page = dynamic_cast(stackedWidget->currentWidget()); + if (map_page) { + map_page->setPlaymatMap(); + } + }); + + connect(playmat_grid_, &QAction::triggered, this, [this]() { + auto map_page = dynamic_cast(stackedWidget->currentWidget()); + if (map_page) { + map_page->setPlaymatGrid(); + } + }); + connect(exit_action_, &QAction::triggered, this, [this]() { close(); }); diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index 9f379be..b4dc7d2 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -1,5 +1,6 @@ #include -#include + +#include namespace ModelecGUI { diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp new file mode 100644 index 0000000..cca8596 --- /dev/null +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -0,0 +1,130 @@ +#include +#include + +#include +#include + +namespace ModelecGUI +{ + MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/grid_v1.svg"), this)), node_(std::move(node)) + { + startButton = new QPushButton("Start", this); + + v_layout = new QVBoxLayout(this); + v_layout->addStretch(); + + h_layout = new QHBoxLayout(this); + h_layout->addStretch(); + h_layout->addWidget(startButton); + h_layout->addStretch(); + + v_layout->addLayout(h_layout); + + this->setLayout(v_layout); + + connect(startButton, &QPushButton::clicked, this, &MapPage::onStartButtonClicked); + + qpoints = {}; + points = {}; + + add_waypoint_publisher_ = node_->create_publisher("odometry/add_waypoint", 30); + + // lambda + odometry_subscriber_ = node_->create_subscription("odometry/get_position", 10, + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { + robotPosPoint.setX(Modelec::mapValue(static_cast(msg->x), 0, 3000, 0, width())); + robotPosPoint.setY(height() - Modelec::mapValue(static_cast(msg->y), 0, 2000, 0, height())); + update(); + }); + } + + void MapPage::setPlaymatGrid() + { + renderer->load(QString(":/img/playmat/grid_v1.svg")); + update(); + } + + void MapPage::setPlaymatMap() + { + renderer->load(QString(":/img/playmat/2025_FINAL.svg")); + update(); + } + + void MapPage::paintEvent(QPaintEvent* paint_event) + { + QWidget::paintEvent(paint_event); + + QPainter painter(this); + renderer->render(&painter, rect()); // Scales SVG to widget size + + painter.setRenderHint(QPainter::Antialiasing); + + // --- Draw lines --- + painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide + if (!points.empty()) + painter.drawLine(robotPosPoint, qpoints[0]); + + for (size_t i = 0; i + 1 < points.size(); ++i) { + painter.drawLine(qpoints[i], qpoints[i + 1]); + } + + // --- Draw colored points --- + const int radius = 5; + + painter.setBrush(Qt::green); + painter.setPen(Qt::NoPen); + painter.drawEllipse(robotPosPoint, radius, radius); // Robot position + + for (size_t i = 0; i < points.size(); ++i) { + if (i == points.size() - 1) + painter.setBrush(Qt::blue); // Last = blue + else + painter.setBrush(Qt::red); // Middle = red + + painter.setPen(Qt::NoPen); + painter.drawEllipse(qpoints[i], radius, radius); + } + } + + void MapPage::mousePressEvent(QMouseEvent* event) + { + QWidget::mousePressEvent(event); + + qpoints.push_back(event->pos()); + + modelec_interfaces::msg::OdometryAddWaypoint msg; + msg.x = Modelec::mapValue(event->pos().x(), 0, width(), 0, 3000); + msg.y = 2000 - Modelec::mapValue(event->pos().y(), 0, height(), 0, 2000); + msg.is_end = false; + msg.id = points.size(); + + if (points.empty()) + { + QPointF vec = QPoint(msg.x, msg.y) - robotPosPoint; + msg.theta = std::atan2(vec.y(), vec.x()); + } + else + { + auto lastPoint = points.back(); + QPointF vec = QPoint(msg.x, msg.y) - QPoint(lastPoint.x, lastPoint.y); + msg.theta = std::atan2(vec.y(), vec.x()); + } + + points.push_back(msg); + + update(); + } + + void MapPage::onStartButtonClicked() + { + points.back().is_end = true; + + for (const auto& point : points) { + add_waypoint_publisher_->publish(point); + } + + qpoints.clear(); + points.clear(); + update(); + } +} diff --git a/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg b/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg index 3febaf0..e939edd 100644 --- a/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg +++ b/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg @@ -2,4 +2,4 @@ uint8 id bool is_end int64 x int64 y -int64 theta \ No newline at end of file +float64 theta \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Odometry/OdometryPos.msg b/src/modelec_interfaces/msg/Odometry/OdometryPos.msg index ecf3ac5..2659cab 100644 --- a/src/modelec_interfaces/msg/Odometry/OdometryPos.msg +++ b/src/modelec_interfaces/msg/Odometry/OdometryPos.msg @@ -1,3 +1,3 @@ int64 x int64 y -int64 theta +float64 theta diff --git a/src/modelec_interfaces/srv/Odometry/OdometryAddWaypoint.srv b/src/modelec_interfaces/srv/Odometry/OdometryAddWaypoint.srv index 525de05..91dcdf5 100644 --- a/src/modelec_interfaces/srv/Odometry/OdometryAddWaypoint.srv +++ b/src/modelec_interfaces/srv/Odometry/OdometryAddWaypoint.srv @@ -2,6 +2,6 @@ uint8 id bool is_end int64 x int64 y -int64 theta +float64 theta --- bool success \ No newline at end of file