modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-03-18 21:50:36 +01:00
Code Issues Packages Projects Releases Wiki Activity

UI waypoint

Browse Source
This commit is contained in:
acki
2025-04-22 16:47:53 -04:00
parent 690f0191e1
commit c40892d038
16 changed files with 282 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/modelec_com/include/modelec_com/multiple_serial_listener.hpp
View File

@@ -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);
};

2
src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
View File

@@ -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;

28
src/modelec_com/src/multiple_serial_listener.cpp
View File

@@ -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(

3
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/12"; // TODO : check the real serial port
request->serial_port = "/dev/pts/11"; // 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)))
{
@@ -427,6 +427,7 @@ namespace Modelec
{
command += ";" + d;
}
command += "\n";
SendToPCB(command);
}

7
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/7"; // TODO : check the real serial port
request->serial_port = "/dev/pts/6"; // 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)))
{
@@ -84,7 +84,7 @@ namespace Modelec
"odometry/get_pid", 10);
odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
"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<std::string> data = {
std::to_string(index),

2
src/modelec_core/launch/test.modelec.launch.yml
View File

@@ -21,6 +21,6 @@ launch:
name: "modelec_gui"
- node:
pkg: 'modelec'
pkg: 'modelec_core'
exec: 'speed_result'
name: 'speed_result'

14
src/modelec_gui/CMakeLists.txt
View File

@@ -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

5
src/modelec_gui/include/modelec_gui/modelec_gui.hpp
View File

@@ -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_;
};
}

54
src/modelec_gui/include/modelec_gui/pages/map_page.hpp Normal file
View File

@@ -0,0 +1,54 @@
#pragma once
#include <QWidget>
#include <QLabel>
#include <QPainter>
#include <QPushButton>
#include <QSvgRenderer>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
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<QPoint> qpoints;
std::vector<modelec_interfaces::msg::OdometryAddWaypoint> points;
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_publisher_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_subscriber_;
};
}

7
src/modelec_gui/resource.qrc Normal file
View File

@@ -0,0 +1,7 @@
<!DOCTYPE RCC>
<RCC version="1.0">
<qresource prefix="/">
<file>img/playmat/2025_FINAL.svg</file>
<file>img/playmat/grid_v1.svg</file>
</qresource>
</RCC>

33
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -3,6 +3,7 @@
#include <QMenuBar>
#include <utility>
#include <modelec_gui/pages/home_page.hpp>
#include <modelec_gui/pages/map_page.hpp>
#include <modelec_gui/pages/test_page.hpp>
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<MapPage *>(stackedWidget->currentWidget());
if (map_page) {
map_page->setPlaymatMap();
}
});
connect(playmat_grid_, &QAction::triggered, this, [this]() {
auto map_page = dynamic_cast<MapPage *>(stackedWidget->currentWidget());
if (map_page) {
map_page->setPlaymatGrid();
}
});
connect(exit_action_, &QAction::triggered, this, [this]() {
close();
});

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

@@ -1,5 +1,6 @@
#include <modelec_gui/pages/home_page.hpp>
#include <modelec_gui/pages/test_page.hpp>
#include <QVBoxLayout>
namespace ModelecGUI
{

130
src/modelec_gui/src/pages/map_page.cpp Normal file
View File

@@ -0,0 +1,130 @@
#include <modelec_gui/pages/map_page.hpp>
#include <modelec_utils/utils.hpp>
#include <QMouseEvent>
#include <utility>
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<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 30);
// lambda
odometry_subscriber_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/get_position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
robotPosPoint.setX(Modelec::mapValue(static_cast<int>(msg->x), 0, 3000, 0, width()));
robotPosPoint.setY(height() - Modelec::mapValue(static_cast<int>(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();
}
}

2
src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg
View File

@@ -2,4 +2,4 @@ uint8 id
bool is_end
int64 x
int64 y
int64 theta
float64 theta

2
src/modelec_interfaces/msg/Odometry/OdometryPos.msg
View File

@@ -1,3 +1,3 @@
int64 x
int64 y
int64 theta
float64 theta

2
src/modelec_interfaces/srv/Odometry/OdometryAddWaypoint.srv
View File

@@ -2,6 +2,6 @@ uint8 id
bool is_end
int64 x
int64 y
int64 theta
float64 theta
---
bool success