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

repatch

Browse Source
This commit is contained in:
acki
2025-04-07 16:04:49 -04:00
parent 084b70e91e
commit 66be4987c1
17 changed files with 561 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
TODO.md
View File

@@ -1 +1,2 @@
- rework position system with that https://docs.ros2.org/latest/api/geometry_msgs/index-msg.html
- rework position system with that https://docs.ros2.org/latest/api/geometry_msgs/index-msg.html
- API (check for the middleware already implemented or fill the API package)

5
src/modelec/include/modelec/pcb_alim_interface.hpp
View File

@@ -9,6 +9,11 @@ class PCBAlimInterface : public rclcpp::Node
{
public:
PCBAlimInterface();
rclcpp::CallbackGroup::SharedPtr pcb_callback_group_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
std::thread pcb_executor_thread_;
~PCBAlimInterface() override;
private:

33
src/modelec/include/modelec/pcb_odo_interface.h
View File

@@ -13,12 +13,15 @@
#include <modelec_interface/msg/odometry_waypoint_reach.hpp>
#include <modelec_interface/msg/odometry_add_waypoint.hpp>
#include <modelec_interface/msg/odometry_start.hpp>
#include <modelec_interface/msg/odometry_pid.hpp>
#include <modelec_interface/srv/odometry_position.hpp>
#include <modelec_interface/srv/odometry_speed.hpp>
#include <modelec_interface/srv/odometry_to_f.hpp>
#include <modelec_interface/srv/add_serial_listener.hpp>
#include <modelec_interface/srv/odometry_start.hpp>
#include <modelec_interface/srv/odometry_get_pid.hpp>
#include <modelec_interface/srv/odometry_set_pid.hpp>
namespace Modelec {
@@ -37,6 +40,13 @@ public:
long theta;
};
struct PIDData
{
float p;
float i;
float d;
};
private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr pcb_subscriber_;
@@ -47,17 +57,22 @@ private:
rclcpp::Publisher<modelec_interface::msg::OdometrySpeed>::SharedPtr odo_speed_publisher_;
rclcpp::Publisher<modelec_interface::msg::OdometryToF>::SharedPtr odo_tof_publisher_;
rclcpp::Publisher<modelec_interface::msg::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_publisher_;
rclcpp::Publisher<modelec_interface::msg::OdometryPid>::SharedPtr odo_pid_publisher_;
rclcpp::Subscription<modelec_interface::msg::OdometryAddWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
rclcpp::Subscription<modelec_interface::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
rclcpp::Subscription<modelec_interface::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
void AddWaypointCallback(const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const;
void SetPosCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg) const;
void SetPIDCallback(const modelec_interface::msg::OdometryPid::SharedPtr msg) const;
rclcpp::Service<modelec_interface::srv::OdometryToF>::SharedPtr get_tof_service_;
rclcpp::Service<modelec_interface::srv::OdometrySpeed>::SharedPtr get_speed_service_;
rclcpp::Service<modelec_interface::srv::OdometryPosition>::SharedPtr get_position_service_;
rclcpp::Service<modelec_interface::srv::OdometryStart>::SharedPtr set_start_service_;
rclcpp::Service<modelec_interface::srv::OdometryGetPid>::SharedPtr get_pid_service_;
rclcpp::Service<modelec_interface::srv::OdometrySetPid>::SharedPtr set_pid_service_;
// Promises and mutexes to synchronize service responses asynchronously
std::queue<std::promise<long>> tof_promises_;
@@ -72,6 +87,12 @@ private:
std::queue<std::promise<bool>> start_promises_;
std::mutex start_mutex_;
std::queue<std::promise<PIDData>> get_pid_promises_;
std::mutex get_pid_mutex_;
std::queue<std::promise<bool>> set_pid_promises_;
std::mutex set_pid_mutex_;
// Service handlers using async wait with promises
void HandleGetTof(const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response);
@@ -85,11 +106,19 @@ private:
void HandleGetStart(const std::shared_ptr<modelec_interface::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryStart::Response> response);
void HandleGetPID(const std::shared_ptr<modelec_interface::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryGetPid::Response> response);
void HandleSetPID(const std::shared_ptr<modelec_interface::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interface::srv::OdometrySetPid::Response> response);
// Resolving methods called by subscriber callback
void ResolveToFRequest(long distance);
void ResolveSpeedRequest(const OdometryData& speed);
void ResolvePositionRequest(const OdometryData& position);
void ResolveStartRequest(bool start);
void ResolveGetPIDRequest(const PIDData& pid);
void ResolveSetPIDRequest(bool success);
public:
void SendToPCB(const std::string &data) const;
@@ -110,6 +139,10 @@ public:
void SetStart(const modelec_interface::msg::OdometryStart::SharedPtr msg) const;
void SetStart(bool start) const;
void GetPID() const;
void SetPID(const modelec_interface::msg::OdometryPid::SharedPtr msg) const;
void SetPID(float p, float i, float d) const;
};
} // namespace Modelec

36
src/modelec/src/pcb_alim_interface.cpp
View File

@@ -31,10 +31,23 @@ namespace Modelec
RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str());
RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str());
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;
options.callback_group = pcb_callback_group_;
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
result.get()->publisher, 10,
std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1));
std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1), options);
pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
pcb_executor_thread_ = std::thread([this]() {
pcb_executor_->spin();
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
}
else
{
@@ -52,6 +65,14 @@ namespace Modelec
}
}
PCBAlimInterface::~PCBAlimInterface()
{
pcb_executor_->cancel();
if (pcb_executor_thread_.joinable()) {
pcb_executor_thread_.join();
}
}
void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
{
RCLCPP_INFO(this->get_logger(), "Received message: '%s'", msg->data.c_str());
@@ -205,10 +226,17 @@ namespace Modelec
}
} // namespace Modelec
int main(int argc, char** argv)
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::PCBAlimInterface>());
auto node = std::make_shared<Modelec::PCBAlimInterface>();
// Increase number of threads explicitly!
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 2 /* or more threads! */);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}

130
src/modelec/src/pcb_odo_interface.cpp
View File

@@ -67,7 +67,7 @@ namespace Modelec
}
odo_pos_publisher_ = this->create_publisher<modelec_interface::msg::OdometryPos>(
"odometry/position", 10);
"odometry/get_position", 10);
odo_speed_publisher_ = this->create_publisher<modelec_interface::msg::OdometrySpeed>(
"odometry/speed", 10);
@@ -78,6 +78,9 @@ namespace Modelec
odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interface::msg::OdometryWaypointReach>(
"odometry/waypoint_reach", 10);
odo_pid_publisher_ = this->create_publisher<modelec_interface::msg::OdometryPid>(
"odometry/get_pid", 10);
odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryAddWaypoint>(
"odometry/add_waypoint", 10,
std::bind(&PCBOdoInterface::AddWaypointCallback, this, std::placeholders::_1));
@@ -86,6 +89,10 @@ namespace Modelec
"odometry/set_position", 10,
std::bind(&PCBOdoInterface::SetPosCallback, this, std::placeholders::_1));
odo_set_pid_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryPid>(
"odometry/set_pid", 10,
std::bind(&PCBOdoInterface::SetPIDCallback, this, std::placeholders::_1));
// Services
get_tof_service_ = create_service<modelec_interface::srv::OdometryToF>(
"odometry/tof",
@@ -102,6 +109,14 @@ namespace Modelec
set_start_service_ = create_service<modelec_interface::srv::OdometryStart>(
"odometry/start",
std::bind(&PCBOdoInterface::HandleGetStart, this, std::placeholders::_1, std::placeholders::_2));
get_pid_service_ = create_service<modelec_interface::srv::OdometryGetPid>(
"odometry/get_pid",
std::bind(&PCBOdoInterface::HandleGetPID, this, std::placeholders::_1, std::placeholders::_2));
set_pid_service_ = create_service<modelec_interface::srv::OdometrySetPid>(
"odometry/set_pid",
std::bind(&PCBOdoInterface::HandleSetPID, this, std::placeholders::_1, std::placeholders::_2));
}
PCBOdoInterface::~PCBOdoInterface()
@@ -174,6 +189,20 @@ namespace Modelec
odo_waypoint_reach_publisher_->publish(message);
}
else if (tokens[1] == "PID")
{
float p = std::stof(tokens[2]);
float i = std::stof(tokens[3]);
float d = std::stof(tokens[4]);
auto message = modelec_interface::msg::OdometryPid();
message.p = p;
message.i = i;
message.d = d;
odo_pid_publisher_->publish(message);
ResolveGetPIDRequest({p, i, d});
}
}
else if (tokens[0] == "OK")
{
@@ -203,6 +232,11 @@ namespace Modelec
SetRobotPos(msg);
}
void PCBOdoInterface::SetPIDCallback(const modelec_interface::msg::OdometryPid::SharedPtr msg) const
{
SetPID(msg);
}
void PCBOdoInterface::HandleGetTof(
const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response)
@@ -275,6 +309,43 @@ namespace Modelec
response->success = future.get();
}
void PCBOdoInterface::HandleGetPID(const std::shared_ptr<modelec_interface::srv::OdometryGetPid::Request>,
std::shared_ptr<modelec_interface::srv::OdometryGetPid::Response> response)
{
std::promise<PIDData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(get_pid_mutex_);
get_pid_promises_.push(std::move(promise));
}
GetPID();
PIDData result = future.get();
response->p = result.p;
response->i = result.i;
response->d = result.d;
}
void PCBOdoInterface::HandleSetPID(const std::shared_ptr<modelec_interface::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interface::srv::OdometrySetPid::Response> response)
{
std::promise<bool> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(set_pid_mutex_);
set_pid_promises_.push(std::move(promise));
}
SetPID(request->p, request->i, request->d);
bool result = future.get();
response->success = result;
}
void PCBOdoInterface::ResolveToFRequest(const long distance)
{
std::lock_guard<std::mutex> lock(tof_mutex_);
@@ -323,6 +394,30 @@ namespace Modelec
}
}
void PCBOdoInterface::ResolveGetPIDRequest(const PIDData& pid)
{
std::lock_guard<std::mutex> lock(get_pid_mutex_);
if (!get_pid_promises_.empty()) {
auto promise = std::move(get_pid_promises_.front());
get_pid_promises_.pop();
promise.set_value(pid);
} else {
RCLCPP_DEBUG(get_logger(), "No pending PID request to resolve.");
}
}
void PCBOdoInterface::ResolveSetPIDRequest(bool success)
{
std::lock_guard<std::mutex> lock(set_pid_mutex_);
if (!set_pid_promises_.empty()) {
auto promise = std::move(set_pid_promises_.front());
set_pid_promises_.pop();
promise.set_value(success);
} else {
RCLCPP_DEBUG(get_logger(), "No pending Set PID request to resolve.");
}
}
void PCBOdoInterface::SendToPCB(const std::string& data) const
{
auto message = std_msgs::msg::String();
@@ -412,17 +507,30 @@ namespace Modelec
{
SendOrder("START", {std::to_string(start)});
}
void PCBOdoInterface::GetPID() const
{
GetData("PID");
}
void PCBOdoInterface::SetPID(const modelec_interface::msg::OdometryPid::SharedPtr msg) const
{
SetPID(msg->p, msg->i, msg->d);
}
void PCBOdoInterface::SetPID(float p, float i, float d) const
{
std::vector<std::string> data = {
std::to_string(p),
std::to_string(i),
std::to_string(d)
};
SendOrder("PID", data);
}
} // Modelec
/*int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::PCBOdoInterface>());
rclcpp::shutdown();
return 0;
}*/
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
@@ -430,7 +538,7 @@ int main(int argc, char **argv)
// Increase number of threads explicitly!
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 4 /* or more threads! */);
rclcpp::ExecutorOptions(), 2 /* or more threads! */);
executor.add_node(node);
executor.spin();

31
src/modelec_gui/CMakeLists.txt
View File

@@ -2,9 +2,9 @@ cmake_minimum_required(VERSION 3.8)
project(modelec_gui)
# Enable all warnings
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif ()
set(CMAKE_AUTOMOC ON)
@@ -20,11 +20,20 @@ find_package(Qt6 COMPONENTS
REQUIRED)
# Add the executable
add_executable(modelec_gui src/modelec_gui.cpp src/main.cpp include/modelec_gui/modelec_gui.hpp)
add_executable(modelec_gui
src/main.cpp
src/modelec_gui.cpp
src/pages/test_page.cpp
src/pages/home_page.cpp
include/modelec_gui/modelec_gui.hpp
include/modelec_gui/pages/test_page.hpp
include/modelec_gui/pages/home_page.hpp
)
ament_target_dependencies(modelec_gui
rclcpp
std_msgs
modelec_interface
rclcpp
std_msgs
modelec_interface
)
target_link_libraries(modelec_gui
Qt6::Core
@@ -33,18 +42,18 @@ target_link_libraries(modelec_gui
)
target_include_directories(modelec_gui PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Install targets
install(TARGETS modelec_gui
DESTINATION lib/${PROJECT_NAME}
DESTINATION lib/${PROJECT_NAME}
)
# Install include files
install(DIRECTORY include/
DESTINATION include/
DESTINATION include/
)
# Package finalization

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

@@ -1,40 +1,32 @@
#pragma once
#include <QApplication>
#include <QWidget>
#include <QPushButton>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QStackedWidget>
#include <QMainWindow>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <modelec_interface/msg/odometry_pos.hpp>
#include <modelec_interface/srv/odometry_speed.hpp>
#include <modelec_interface/srv/odometry_start.hpp>
class ROS2QtGUI : public QWidget {
Q_OBJECT
public:
explicit ROS2QtGUI(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
~ROS2QtGUI() override; // Explicitly declare destructor
namespace ModelecGUI
{
class ROS2QtGUI : public QMainWindow {
Q_OBJECT
rclcpp::Node::SharedPtr get_node() const { return node_; }
public:
explicit ROS2QtGUI(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
~ROS2QtGUI() override; // Explicitly declare destructor
private:
QPushButton* startButton_;
QLineEdit *xBox_, *yBox_, *thetaBox_;
QVBoxLayout *mainLayout_;
QHBoxLayout *posLayout_;
QPushButton *askSpeed_;
QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_;
QHBoxLayout *speedLayout_;
rclcpp::Node::SharedPtr get_node() const { return node_; }
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<modelec_interface::msg::OdometryPos>::SharedPtr sub_;
protected:
// client
rclcpp::Client<modelec_interface::srv::OdometrySpeed>::SharedPtr client_;
rclcpp::Client<modelec_interface::srv::OdometryStart>::SharedPtr client_start_;
rclcpp::Node::SharedPtr node_;
void PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg);
};
QStackedWidget *stackedWidget;
void setupMenu();
QAction* home_action_;
QAction* test_action_;
QAction* exit_action_;
};
}

16
src/modelec_gui/include/modelec_gui/pages/home_page.hpp Normal file
View File

@@ -0,0 +1,16 @@
#pragma once
#include <QWidget>
#include <QLabel>
namespace ModelecGUI {
class HomePage : public QWidget
{
Q_OBJECT
public:
HomePage(QWidget *parent = nullptr);
protected:
QLayout* m_layout;
};
}

56
src/modelec_gui/include/modelec_gui/pages/test_page.hpp Normal file
View File

@@ -0,0 +1,56 @@
#pragma once
#include <QWidget>
#include <QPushButton>
#include <QLineEdit>
#include <QVBoxLayout>
#include <rclcpp/rclcpp.hpp>
#include <modelec_interface/msg/odometry_pos.hpp>
#include <modelec_interface/srv/odometry_speed.hpp>
#include <modelec_interface/srv/odometry_start.hpp>
#include <modelec_interface/srv/odometry_get_pid.hpp>
#include <modelec_interface/srv/odometry_set_pid.hpp>
namespace ModelecGUI {
class TestPage : public QWidget
{
Q_OBJECT
public:
TestPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
~TestPage() override;
rclcpp::Node::SharedPtr get_node() const { return node_; }
private:
QVBoxLayout *mainLayout_;
QPushButton* startButton_;
QLineEdit *xBox_, *yBox_, *thetaBox_;
QHBoxLayout *posLayout_;
QPushButton *askPID_;
QLineEdit *pPIDBox_, *iPIDBox_, *dPIDBox_;
QHBoxLayout *pidLayout_;
QPushButton *setPID_;
QPushButton *askSpeed_;
QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_;
QHBoxLayout *speedLayout_;
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<modelec_interface::msg::OdometryPos>::SharedPtr sub_;
// client
rclcpp::Client<modelec_interface::srv::OdometrySpeed>::SharedPtr client_;
rclcpp::Client<modelec_interface::srv::OdometryStart>::SharedPtr client_start_;
rclcpp::Client<modelec_interface::srv::OdometryGetPid>::SharedPtr client_get_pid_;
rclcpp::Client<modelec_interface::srv::OdometrySetPid>::SharedPtr client_set_pid_;
void PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg);
};
} // namespace Modelec

2
src/modelec_gui/src/main.cpp
View File

@@ -15,7 +15,7 @@ int main(int argc, char **argv)
auto node = rclcpp::Node::make_shared("qt_gui_node");
// Create the GUI and pass the node to it
ROS2QtGUI window(node); // Pass the node to the GUI component
ModelecGUI::ROS2QtGUI window(node); // Pass the node to the GUI component
window.show();
// Create an executor for ROS 2 to manage the node

137
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -1,115 +1,52 @@
#include "modelec_gui/modelec_gui.hpp"
#include <rclcpp/rclcpp.hpp>
#include <QMetaObject>
#include <QMenuBar>
#include <utility>
#include <modelec_gui/pages/home_page.hpp>
#include <modelec_gui/pages/test_page.hpp>
ROS2QtGUI::ROS2QtGUI(rclcpp::Node::SharedPtr node, QWidget *parent)
: QWidget(parent), node_(std::move(node)) {
namespace ModelecGUI {
startButton_ = new QPushButton("Start");
connect(startButton_, &QPushButton::clicked, this, [this]() {
// Create a request for the speed service
RCLCPP_INFO(node_->get_logger(), "Start button clicked.");
auto request = std::make_shared<modelec_interface::srv::OdometryStart::Request>();
request->start = true;
client_start_->async_send_request(request, [this](rclcpp::Client<modelec_interface::srv::OdometryStart>::SharedFuture response) {
if (response.get()->success)
{
RCLCPP_INFO(node_->get_logger(), "Start command sent successfully.");
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to send start command.");
}
});
});
ROS2QtGUI::ROS2QtGUI(rclcpp::Node::SharedPtr node, QWidget *parent)
: QMainWindow(parent), node_(std::move(node)), stackedWidget(new QStackedWidget(this)) {
// Initialize the UI components
xBox_ = new QLineEdit("x: ?");
yBox_ = new QLineEdit("y: ?");
thetaBox_ = new QLineEdit("theta: ?");
xBox_->setReadOnly(true);
yBox_->setReadOnly(true);
thetaBox_->setReadOnly(true);
// Add pages to stack
stackedWidget->addWidget(new HomePage(this));
stackedWidget->addWidget(new TestPage(get_node(), this));
setCentralWidget(stackedWidget);
posLayout_ = new QHBoxLayout;
posLayout_->addWidget(xBox_);
posLayout_->addWidget(yBox_);
posLayout_->addWidget(thetaBox_);
setupMenu();
askSpeed_ = new QPushButton("Ask speed");
connect(askSpeed_, &QPushButton::clicked, this, [this]() {
// Create a request for the speed service
auto request = std::make_shared<modelec_interface::srv::OdometrySpeed::Request>();
// Make the asynchronous service call
client_->async_send_request(request, [this](rclcpp::Client<modelec_interface::srv::OdometrySpeed>::SharedFuture response) {
if (auto res = response.get()) {
QMetaObject::invokeMethod(this, [this, res]() {
xSpeedBox_->setText(QString("x: %1").arg(res->x));
ySpeedBox_->setText(QString("y: %1").arg(res->y));
thetaSpeedBox_->setText(QString("theta: %1").arg(res->theta));
});
} else {
RCLCPP_ERROR(node_->get_logger(), "Failed to get response for speed request.");
}
});
});
xSpeedBox_ = new QLineEdit("x speed: ?");
ySpeedBox_ = new QLineEdit("y speed: ?");
thetaSpeedBox_ = new QLineEdit("theta speed: ?");
xSpeedBox_->setReadOnly(true);
ySpeedBox_->setReadOnly(true);
thetaSpeedBox_->setReadOnly(true);
speedLayout_ = new QHBoxLayout;
speedLayout_->addWidget(xSpeedBox_);
speedLayout_->addWidget(ySpeedBox_);
speedLayout_->addWidget(thetaSpeedBox_);
mainLayout_ = new QVBoxLayout(this);
mainLayout_->addWidget(startButton_);
mainLayout_->addLayout(posLayout_);
mainLayout_->addWidget(askSpeed_);
mainLayout_->addLayout(speedLayout_);
setLayout(mainLayout_);
// Set up subscription
sub_ = node_->create_subscription<modelec_interface::msg::OdometryPos>(
"/odometry/position", 10,
std::bind(&ROS2QtGUI::PositionCallback, this, std::placeholders::_1));
// Set up service client
client_ = node_->create_client<modelec_interface::srv::OdometrySpeed>("odometry/speed");
// Wait for the service to be available
while (!client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again...");
resize(800, 600);
}
client_start_ = node_->create_client<modelec_interface::srv::OdometryStart>("odometry/start");
void ROS2QtGUI::setupMenu() {
QMenuBar *menuBar = this->menuBar();
while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again...");
QMenu *optionsMenu = menuBar->addMenu("Options");
home_action_ = new QAction("Home", this);
test_action_ = new QAction("Test", this);
exit_action_ = new QAction("Exit", this);
optionsMenu->addAction(home_action_);
optionsMenu->addAction(test_action_);
optionsMenu->addSeparator();
optionsMenu->addAction(exit_action_);
connect(home_action_, &QAction::triggered, this, [this]() {
stackedWidget->setCurrentIndex(0);
});
connect(test_action_, &QAction::triggered, this, [this]() {
stackedWidget->setCurrentIndex(1);
});
connect(exit_action_, &QAction::triggered, this, [this]() {
close();
});
}
}
ROS2QtGUI::~ROS2QtGUI() = default;
ROS2QtGUI::~ROS2QtGUI() = default;
void ROS2QtGUI::PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg)
{
QMetaObject::invokeMethod(this, [this, msg]() {
xBox_->setText(QString("x: %1").arg(msg->x));
yBox_->setText(QString("y: %1").arg(msg->y));
thetaBox_->setText(QString("theta: %1").arg(msg->theta));
});
}

16
src/modelec_gui/src/pages/home_page.cpp Normal file
View File

@@ -0,0 +1,16 @@
#include <modelec_gui/pages/home_page.hpp>
#include <modelec_gui/pages/test_page.hpp>
namespace ModelecGUI
{
HomePage::HomePage(QWidget* parent) : QWidget(parent)
{
// label with default text
m_layout = new QVBoxLayout(this);
m_layout->addWidget(new QLabel("Welcome to Modelec GUI!"));
this->setLayout(m_layout);
}
}

188
src/modelec_gui/src/pages/test_page.cpp Normal file
View File

@@ -0,0 +1,188 @@
#include <modelec_gui/pages/test_page.hpp>
#include <utility>
#include <boost/asio/connect.hpp>
namespace ModelecGUI
{
TestPage::TestPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(std::move(node))
{
startButton_ = new QPushButton("Start");
connect(startButton_, &QPushButton::clicked, this, [this]() {
// Create a request for the speed service
RCLCPP_INFO(node_->get_logger(), "Start button clicked.");
auto request = std::make_shared<modelec_interface::srv::OdometryStart::Request>();
request->start = true;
client_start_->async_send_request(request, [this](rclcpp::Client<modelec_interface::srv::OdometryStart>::SharedFuture response) {
if (response.get()->success)
{
RCLCPP_INFO(node_->get_logger(), "Start command sent successfully.");
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to send start command.");
}
});
});
// Initialize the UI components
xBox_ = new QLineEdit("x: ?");
yBox_ = new QLineEdit("y: ?");
thetaBox_ = new QLineEdit("theta: ?");
xBox_->setReadOnly(true);
yBox_->setReadOnly(true);
thetaBox_->setReadOnly(true);
posLayout_ = new QHBoxLayout;
posLayout_->addWidget(xBox_);
posLayout_->addWidget(yBox_);
posLayout_->addWidget(thetaBox_);
askPID_ = new QPushButton("Ask PID");
connect(askPID_, &QPushButton::clicked, this, [this]() {
// Create a request for the PID service
auto request = std::make_shared<modelec_interface::srv::OdometryGetPid::Request>();
// Make the asynchronous service call
client_get_pid_->async_send_request(request, [this](rclcpp::Client<modelec_interface::srv::OdometryGetPid>::SharedFuture response) {
if (auto res = response.get()) {
QMetaObject::invokeMethod(this, [this, res]() {
pPIDBox_->setText(QString("%1").arg(res->p));
iPIDBox_->setText(QString("%1").arg(res->i));
dPIDBox_->setText(QString("%1").arg(res->d));
});
} else {
RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request.");
}
});
});
pPIDBox_ = new QLineEdit("");
iPIDBox_ = new QLineEdit("");
dPIDBox_ = new QLineEdit("");
pidLayout_ = new QHBoxLayout;
pidLayout_->addWidget(pPIDBox_);
pidLayout_->addWidget(iPIDBox_);
pidLayout_->addWidget(dPIDBox_);
setPID_ = new QPushButton("Set PID");
connect(setPID_, &QPushButton::clicked, this, [this]() {
// Create a request for the PID service
auto request = std::make_shared<modelec_interface::srv::OdometrySetPid::Request>();
request->p = pPIDBox_->text().toFloat();
request->i = iPIDBox_->text().toFloat();
request->d = dPIDBox_->text().toFloat();
// Make the asynchronous service call
client_set_pid_->async_send_request(request, [this](rclcpp::Client<modelec_interface::srv::OdometrySetPid>::SharedFuture response) {
if (response.get()->success)
{
RCLCPP_INFO(node_->get_logger(), "Set PID command sent successfully.");
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to send set PID command.");
}
});
});
askSpeed_ = new QPushButton("Ask speed");
connect(askSpeed_, &QPushButton::clicked, this, [this]() {
// Create a request for the speed service
auto request = std::make_shared<modelec_interface::srv::OdometrySpeed::Request>();
// Make the asynchronous service call
client_->async_send_request(request, [this](rclcpp::Client<modelec_interface::srv::OdometrySpeed>::SharedFuture response) {
if (auto res = response.get()) {
QMetaObject::invokeMethod(this, [this, res]() {
xSpeedBox_->setText(QString("x: %1").arg(res->x));
ySpeedBox_->setText(QString("y: %1").arg(res->y));
thetaSpeedBox_->setText(QString("theta: %1").arg(res->theta));
});
} else {
RCLCPP_ERROR(node_->get_logger(), "Failed to get response for speed request.");
}
});
});
xSpeedBox_ = new QLineEdit("x speed: ?");
ySpeedBox_ = new QLineEdit("y speed: ?");
thetaSpeedBox_ = new QLineEdit("theta speed: ?");
xSpeedBox_->setReadOnly(true);
ySpeedBox_->setReadOnly(true);
thetaSpeedBox_->setReadOnly(true);
speedLayout_ = new QHBoxLayout;
speedLayout_->addWidget(xSpeedBox_);
speedLayout_->addWidget(ySpeedBox_);
speedLayout_->addWidget(thetaSpeedBox_);
mainLayout_ = new QVBoxLayout(this);
mainLayout_->addWidget(startButton_);
mainLayout_->addLayout(posLayout_);
mainLayout_->addWidget(askPID_);
mainLayout_->addLayout(pidLayout_);
mainLayout_->addWidget(setPID_);
mainLayout_->addWidget(askSpeed_);
mainLayout_->addLayout(speedLayout_);
setLayout(mainLayout_);
// Set up subscription
sub_ = node_->create_subscription<modelec_interface::msg::OdometryPos>(
"/odometry/get_position", 10,
std::bind(&TestPage::PositionCallback, this, std::placeholders::_1));
// Set up service client
client_ = node_->create_client<modelec_interface::srv::OdometrySpeed>("odometry/speed");
// Wait for the service to be available
while (!client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again...");
}
client_start_ = node_->create_client<modelec_interface::srv::OdometryStart>("odometry/start");
while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again...");
}
client_get_pid_ = node_->create_client<modelec_interface::srv::OdometryGetPid>("odometry/get_pid");
while (!client_get_pid_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again...");
}
client_set_pid_ = node_->create_client<modelec_interface::srv::OdometrySetPid>("odometry/set_pid");
while (!client_set_pid_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again...");
}
}
TestPage::~TestPage() = default;
void TestPage::PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg)
{
QMetaObject::invokeMethod(this, [this, msg]() {
xBox_->setText(QString("x: %1").arg(msg->x));
yBox_->setText(QString("y: %1").arg(msg->y));
thetaBox_->setText(QString("theta: %1").arg(msg->theta));
});
}
}

3
src/modelec_interface/CMakeLists.txt
View File

@@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Odometry/OdometryWaypointReach.msg"
"msg/Odometry/OdometryAddWaypoint.msg"
"msg/Odometry/OdometryStart.msg"
"msg/Odometry/PID/OdometryPid.msg"
"msg/WriteSerial.msg"
"msg/PCA9685Servo.msg"
"msg/ServoMode.msg"
@@ -26,6 +27,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/Odometry/OdometrySpeed.srv"
"srv/Odometry/OdometryToF.srv"
"srv/Odometry/OdometryStart.srv"
"srv/Odometry/PID/OdometryGetPid.srv"
"srv/Odometry/PID/OdometrySetPid.srv"
"srv/AddServoMotor.srv"
"srv/AddSolenoid.srv"
"srv/Tirette.srv"

3
src/modelec_interface/msg/Odometry/PID/OdometryPid.msg Normal file
View File

@@ -0,0 +1,3 @@
float32 p
float32 i
float32 d

4
src/modelec_interface/srv/Odometry/PID/OdometryGetPid.srv Normal file
View File

@@ -0,0 +1,4 @@
---
float32 p
float32 i
float32 d

5
src/modelec_interface/srv/Odometry/PID/OdometrySetPid.srv Normal file
View File

@@ -0,0 +1,5 @@
float32 p
float32 i
float32 d
---
bool success