modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-19 00:57:37 +01:00
Code Issues Packages Projects Releases Wiki Activity

rework pcb interface + begin of Qt6

Browse Source
This commit is contained in:
acki
2025-04-06 20:39:03 -04:00
parent 368b70963b
commit e208863016
9 changed files with 379 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/modelec/launch/test.modelec.launch.yml
View File

@@ -5,12 +5,12 @@ launch:
exec: "serial_listener"
name: "serial_listener"
- node:
pkg: 'modelec'
exec: 'usb_arduino_logic_processor'
name: 'usb_arduino_logic_processor'
- node:
pkg: 'modelec'
exec: 'pcb_odo_interface'
name: 'pcb_odo_interface'
- node:
pkg: 'modelec_gui'
exec: "modelec_gui"
name: "modelec_gui"

205
src/modelec/src/gamecontroller_listener.cpp
View File

@@ -2,7 +2,8 @@
#include "modelec/utils.hpp"
#include <modelec_interface/srv/add_serial_listener.hpp>
namespace Modelec {
namespace Modelec
{
ControllerListener::ControllerListener() : Node("controller_listener")
{
// Subscribe to the 'joy' topic
@@ -12,58 +13,85 @@ namespace Modelec {
servo_publisher_ = this->create_publisher<ServoMode>("arm_control", 10);
// Service to create a new serial listener
// Service to create a new serial listener
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
request->name = "odometry";
request->bauds = 115200;
request->serial_port = "/dev/ttyACM0";
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
while (!client->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) {
if (!result.get()->success) {
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
} else {
RCLCPP_INFO(this->get_logger(), "Added serial listener");
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (auto res = result.get())
{
if (res->success)
{
RCLCPP_INFO(this->get_logger(), "Serial listener created");
odometry_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
clear_pca_publisher_ = this->create_publisher<std_msgs::msg::Empty>("clear_pca9685", 10);
pca9685_publisher_ = this->create_publisher<modelec_interface::msg::PCA9685Servo>(
"servo_control", 10);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to create serial listener");
}
}
} else {
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to get result from service");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
odometry_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
clear_pca_publisher_ = this->create_publisher<std_msgs::msg::Empty>("clear_pca9685", 10);
pca9685_publisher_ = this->create_publisher<modelec_interface::msg::PCA9685Servo>("servo_control", 10);
client_ = this->create_client<modelec_interface::srv::AddServoMotor>("add_servo");
// ensure the server is up
while (!client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
for (auto servo : solarPannelServos) {
for (auto servo : solarPannelServos)
{
auto request = std::make_shared<modelec_interface::srv::AddServoMotor::Request>();
request->pin = servo.pin;
auto res = client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) == rclcpp::FutureReturnCode::SUCCESS) {
if (!res.get()->success) {
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (!res.get()->success)
{
RCLCPP_ERROR(this->get_logger(), "Failed to add servo on pin %d", servo.pin);
} else {
}
else
{
RCLCPP_INFO(this->get_logger(), "Added servo on pin %d", servo.pin);
}
} else {
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
}
@@ -79,96 +107,132 @@ namespace Modelec {
void ControllerListener::CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg)
{
if (msg->buttons[2] == 1) {
if (button_2_was_pressed) {
if (msg->buttons[2] == 1)
{
if (button_2_was_pressed)
{
return;
}
auto message = ServoMode();
message.pin = 0;
if (pinces[0] == ServoMode::PINCE_CLOSED) {
if (pinces[0] == ServoMode::PINCE_CLOSED)
{
pinces[0] = ServoMode::PINCE_OPEN;
} else {
}
else
{
pinces[0] = ServoMode::PINCE_CLOSED;
}
message.mode = pinces[0];
servo_publisher_->publish(message);
button_2_was_pressed = true;
} else {
}
else
{
button_2_was_pressed = false;
}
if (msg->buttons[3] == 1) {
if (button_3_was_pressed) {
if (msg->buttons[3] == 1)
{
if (button_3_was_pressed)
{
return;
}
auto message = ServoMode();
message.pin = 1;
if (pinces[1] == ServoMode::PINCE_CLOSED) {
if (pinces[1] == ServoMode::PINCE_CLOSED)
{
pinces[1] = ServoMode::PINCE_OPEN;
} else {
}
else
{
pinces[1] = ServoMode::PINCE_CLOSED;
}
message.mode = pinces[1];
servo_publisher_->publish(message);
button_3_was_pressed = true;
} else {
}
else
{
button_3_was_pressed = false;
}
if (msg->buttons[1] == 1) {
if (button_1_was_pressed) {
if (msg->buttons[1] == 1)
{
if (button_1_was_pressed)
{
return;
}
auto message = ServoMode();
message.pin = 2;
if (pinces[2] == ServoMode::PINCE_CLOSED) {
if (pinces[2] == ServoMode::PINCE_CLOSED)
{
pinces[2] = ServoMode::PINCE_OPEN;
} else {
}
else
{
pinces[2] = ServoMode::PINCE_CLOSED;
}
message.mode = pinces[2];
servo_publisher_->publish(message);
button_1_was_pressed = true;
} else {
}
else
{
button_1_was_pressed = false;
}
// arm
if (msg->buttons[0] == 1) {
if (button_0_was_pressed) {
if (msg->buttons[0] == 1)
{
if (button_0_was_pressed)
{
return;
}
auto message = ServoMode();
if (arm == ServoMode::ARM_BOTTOM) {
if (arm == ServoMode::ARM_BOTTOM)
{
arm = ServoMode::ARM_TOP;
} else {
}
else
{
arm = ServoMode::ARM_BOTTOM;
}
message.mode = arm;
message.is_arm = true;
servo_publisher_->publish(message);
button_0_was_pressed = true;
} else {
}
else
{
button_0_was_pressed = false;
}
if (msg->buttons[4]) {
if (button_4_was_pressed) {
if (msg->buttons[4])
{
if (button_4_was_pressed)
{
return;
}
auto message = std_msgs::msg::String();
message.data = "W";
odometry_publisher_->publish(message);
button_4_was_pressed = true;
} else {
}
else
{
button_4_was_pressed = false;
}
if (msg->buttons[5]) {
if (button_5_was_pressed) {
if (msg->buttons[5])
{
if (button_5_was_pressed)
{
return;
}
clear_pca_publisher_->publish(std_msgs::msg::Empty());
button_5_was_pressed = true;
} else {
}
else
{
button_5_was_pressed = false;
}
}
@@ -177,33 +241,45 @@ namespace Modelec {
{
auto message = std_msgs::msg::String();
int speed = 0;
if (msg->axes[1] < 0.1 && msg->axes[1] > -0.1) {
if (msg->axes[1] < 0.1 && msg->axes[1] > -0.1)
{
speed = 0;
} else {
}
else
{
speed = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[1]), -1.0f, 1.0f, -310.0f, 310.0f));
}
if (speed != last_speed) {
if (speed != last_speed)
{
message.data = "V " + std::to_string(speed);
odometry_publisher_->publish(message);
last_speed = speed;
}
int rotation = 0;
if (msg->axes[3] < 0.1 && msg->axes[3] > -0.1) {
if (msg->axes[3] < 0.1 && msg->axes[3] > -0.1)
{
rotation = 0;
} else {
rotation = static_cast<int>(Modelec::mapValue(static_cast<float>(-msg->axes[3]), -1.0f, 1.0f, -310.0f, 310.0f));
}
else
{
rotation = static_cast<int>(Modelec::mapValue(static_cast<float>(-msg->axes[3]), -1.0f, 1.0f, -310.0f,
310.0f));
}
if (rotation != last_rotation) {
if (rotation != last_rotation)
{
message.data = "R " + std::to_string(rotation);
odometry_publisher_->publish(message);
last_rotation = rotation;
}
if (msg->axes[2] != last_solar_1_angle) {
int solarPannelAngle = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[2]), -1.0f, 1.0f, solarPannelServos[0].startAngle, solarPannelServos[0].endAngle));
if (msg->axes[2] != last_solar_1_angle)
{
int solarPannelAngle = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[2]), -1.0f, 1.0f,
solarPannelServos[0].startAngle,
solarPannelServos[0].endAngle));
auto solarPannelAngleMessage = modelec_interface::msg::PCA9685Servo();
solarPannelAngleMessage.pin = solarPannelServos[0].pin;
solarPannelAngleMessage.angle = solarPannelAngle;
@@ -211,8 +287,11 @@ namespace Modelec {
last_solar_1_angle = msg->axes[2];
}
if (msg->axes[5] != last_solar_2_angle) {
int solarPannelAngle = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[5]), -1.0f, 1.0f, solarPannelServos[1].startAngle, solarPannelServos[1].endAngle));
if (msg->axes[5] != last_solar_2_angle)
{
int solarPannelAngle = static_cast<int>(Modelec::mapValue(static_cast<float>(msg->axes[5]), -1.0f, 1.0f,
solarPannelServos[1].startAngle,
solarPannelServos[1].endAngle));
auto solarPannelAngleMessage = modelec_interface::msg::PCA9685Servo();
solarPannelAngleMessage.pin = solarPannelServos[1].pin;
solarPannelAngleMessage.angle = solarPannelAngle;
@@ -222,7 +301,7 @@ namespace Modelec {
}
}
int main(int argc, char **argv)
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::ControllerListener>());

110
src/modelec/src/multiple_serial_listener.cpp
View File

@@ -1,45 +1,63 @@
#include <modelec/multiple_serial_listener.hpp>
namespace Modelec {
MultipleSerialListener::MultipleSerialListener() : Node("multiple_serial_listener"), io() {
add_serial_listener_service_ = create_service<modelec_interface::srv::AddSerialListener>("add_serial_listener", std::bind(&MultipleSerialListener::add_serial_listener, this, std::placeholders::_1, std::placeholders::_2));
timer = create_wall_timer(std::chrono::milliseconds(READ_REFRESH_RATE), [this]() {
for (auto &listener : serial_listeners) {
namespace Modelec
{
MultipleSerialListener::MultipleSerialListener() : Node("multiple_serial_listener"), io()
{
add_serial_listener_service_ = create_service<modelec_interface::srv::AddSerialListener>(
"add_serial_listener", std::bind(&MultipleSerialListener::add_serial_listener, this, std::placeholders::_1,
std::placeholders::_2));
timer = create_wall_timer(std::chrono::milliseconds(READ_REFRESH_RATE), [this]()
{
for (auto& listener : serial_listeners)
{
if (listener.second->IsOk())
listener.second->read();
}
});
parameter_callback_handle_ = this->add_on_set_parameters_callback(
[this](const std::vector<rclcpp::Parameter> &parameters) {
[this](const std::vector<rclcpp::Parameter>& parameters)
{
return onParameterChange(parameters);
});
}
MultipleSerialListener::~MultipleSerialListener() {
for (auto &listener : serial_listeners) {
MultipleSerialListener::~MultipleSerialListener()
{
for (auto& listener : serial_listeners)
{
listener.second->port_.close();
}
}
void MultipleSerialListener::add_serial_listener(const std::shared_ptr<modelec_interface::srv::AddSerialListener::Request> request, std::shared_ptr<modelec_interface::srv::AddSerialListener::Response> response) {
if (serial_listeners.find(request->name) != serial_listeners.end()) {
void MultipleSerialListener::add_serial_listener(
const std::shared_ptr<modelec_interface::srv::AddSerialListener::Request> request,
std::shared_ptr<modelec_interface::srv::AddSerialListener::Response> response)
{
if (serial_listeners.find(request->name) != serial_listeners.end())
{
response->success = true;
response->publisher = serial_listeners[request->name]->publisher_->get_topic_name();
response->subscriber = serial_listeners[request->name]->subscriber_->get_topic_name();
return;
}
auto listener = std::make_shared<SerialListener>(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN, io);
RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Adding serial listener: %s", request->name.c_str());
if (!listener->IsOk()) {
auto listener = std::make_shared<SerialListener>(request->name, request->bauds, request->serial_port,
MAX_MESSAGE_LEN, io);
if (!listener->IsOk())
{
response->success = false;
return;
}
listener->publisher_ = create_publisher<std_msgs::msg::String>("raw_data/" + listener->name_, 10);
listener->subscriber_ = create_subscription<std_msgs::msg::String>(
"send_to_serial/" + listener->name_, 10, [listener](std_msgs::msg::String::SharedPtr msg) {
listener->subscriber_ = create_subscription<std_msgs::msg::String>(
"send_to_serial/" + listener->name_, 10, [listener](std_msgs::msg::String::SharedPtr msg)
{
listener->write(msg->data);
});
@@ -50,12 +68,17 @@ namespace Modelec {
response->subscriber = listener->subscriber_->get_topic_name();
}
rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange(const std::vector<rclcpp::Parameter> &parameters) {
rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange(
const std::vector<rclcpp::Parameter>& parameters)
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto &parameter : parameters) {
if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == "max_message_len") {
for (const auto& parameter : parameters)
{
if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() ==
"max_message_len")
{
updateConnection();
}
}
@@ -63,64 +86,83 @@ namespace Modelec {
return result;
}
void MultipleSerialListener::updateConnection() {
for (auto &listener : serial_listeners) {
void MultipleSerialListener::updateConnection()
{
for (auto& listener : serial_listeners)
{
listener.second->SetMaxMessageLen(get_parameter("max_message_len").as_int());
}
read_refresh_rate_ = get_parameter("read_refresh_rate").as_int();
timer->cancel();
timer = create_wall_timer(std::chrono::milliseconds(read_refresh_rate_), [this]() {
for (auto &listener : serial_listeners) {
timer = create_wall_timer(std::chrono::milliseconds(read_refresh_rate_), [this]()
{
for (auto& listener : serial_listeners)
{
listener.second->read();
}
});
}
MultipleSerialListener::SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port, int max_message_len, boost::asio::io_service& io)
MultipleSerialListener::SerialListener::SerialListener(const std::string& name, int bauds,
const std::string& serial_port, int max_message_len,
boost::asio::io_service& io)
: bauds_(bauds), serial_port_(serial_port), max_message_len_(max_message_len), io_(io), name_(name), port_(io)
{
try {
try
{
port_.open(serial_port_);
port_.set_option(boost::asio::serial_port_base::baud_rate(bauds_));
status_ = true;
} catch (boost::system::system_error &e) {
}
catch (boost::system::system_error& e)
{
RCLCPP_ERROR(rclcpp::get_logger("MultipleSerialListener"), "Failed to open serial port: %s", e.what());
status_ = false;
return;
}
}
void MultipleSerialListener::SerialListener::read() {
void MultipleSerialListener::SerialListener::read()
{
if (!status_) return;
std::vector<char> data(max_message_len_);
try {
try
{
// Attempt to read data from the serial port
size_t len = port_.read_some(boost::asio::buffer(data.data(), max_message_len_));
if (len > 0) {
if (len > 0)
{
// Prepare and publish the message
auto msg = std_msgs::msg::String();
msg.data = std::string(data.begin(), data.begin() + len);
publisher_->publish(msg);
}
} catch (boost::system::system_error &e) {
}
catch (boost::system::system_error& e)
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Read error: %s", e.what());
}
}
void MultipleSerialListener::SerialListener::write(const std::string& data) {
try {
void MultipleSerialListener::SerialListener::write(const std::string& data)
{
try
{
boost::asio::write(port_, boost::asio::buffer(data));
} catch (boost::system::system_error &e) {
}
catch (boost::system::system_error& e)
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Write error: %s", e.what());
}
}
}
int main(int argc, char **argv) {
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::MultipleSerialListener>());
rclcpp::shutdown();

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

@@ -24,24 +24,32 @@ namespace Modelec
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (!result.get()->success)
if (auto res = result.get())
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
if (res->success)
{
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_subscriber_ = this->create_subscription<std_msgs::msg::String>(
result.get()->publisher, 10,
std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
}
}
else
{
RCLCPP_INFO(this->get_logger(), "Added serial listener");
RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
result.get()->publisher, 10,
std::bind(&PCBAlimInterface::PCBCallback, this, std::placeholders::_1));
}
void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
@@ -57,7 +65,7 @@ namespace Modelec
}
void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem, const std::string& data,
const std::string& value)
const std::string& value)
{
std::string command = order + ";" + elem;
if (!data.empty())

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

@@ -10,7 +10,7 @@ namespace Modelec
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
request->name = "pcb_odo";
request->bauds = 115200;
request->serial_port = "/dev/serial1"; // TODO : check the real serial port
request->serial_port = "/dev/pts/6"; // TODO : check the real serial port
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{
@@ -25,13 +25,29 @@ namespace Modelec
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (!result.get()->success)
if (auto res = result.get())
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
if (res->success)
{
RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str());
RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str());
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
res->publisher, 10, [this](std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
}
}
else
{
RCLCPP_INFO(this->get_logger(), "Added serial listener");
RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
}
}
else
@@ -39,11 +55,6 @@ namespace Modelec
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
result.get()->publisher, 10,
std::bind(&PCBOdoInterface::PCBCallback, this, std::placeholders::_1));
odo_pos_publisher_ = this->create_publisher<modelec_interface::msg::OdometryPos>(
"odometry/position", 10);
@@ -54,14 +65,14 @@ namespace Modelec
"odometry/tof", 10);
odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interface::msg::OdometryWaypointReach>(
"odometry/waypoint-reach", 10);
"odometry/waypoint_reach", 10);
odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryAddWaypoint>(
"odometry/add-waypoint", 10,
"odometry/add_waypoint", 10,
std::bind(&PCBOdoInterface::AddWaypointCallback, this, std::placeholders::_1));
odo_set_pos_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryPos>(
"odometry/set-position", 10,
"odometry/set_position", 10,
std::bind(&PCBOdoInterface::SetPosCallback, this, std::placeholders::_1));
// Services
@@ -101,7 +112,7 @@ namespace Modelec
message.theta = theta;
odo_pos_publisher_->publish(message);
ResolvePositionRequest({ x, y, theta });
ResolvePositionRequest({x, y, theta});
}
else if (tokens[1] == "SPEED")
{
@@ -115,7 +126,7 @@ namespace Modelec
message.theta = theta;
odo_speed_publisher_->publish(message);
ResolveSpeedRequest({ x, y, theta });
ResolveSpeedRequest({x, y, theta});
}
else if (tokens[1] == "DIST")
{
@@ -175,7 +186,7 @@ namespace Modelec
}
void PCBOdoInterface::HandleGetSpeed(const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request>,
std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response)
std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response)
{
std::promise<OdometryData> promise;
auto future = promise.get_future();
@@ -252,10 +263,6 @@ namespace Modelec
pos_promises_.pop();
promise.set_value(position);
}
else
{
RCLCPP_WARN(this->get_logger(), "Received Position response but no promise waiting");
}
}
void PCBOdoInterface::SendToPCB(const std::string& data) const
@@ -266,7 +273,7 @@ namespace Modelec
}
void PCBOdoInterface::SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data) const
const std::vector<std::string>& data) const
{
std::string command = order + ";" + elem;
for (const auto& d : data)
@@ -299,7 +306,7 @@ namespace Modelec
void PCBOdoInterface::GetToF(const int& tof) const
{
GetData("DIST", { std::to_string(tof) });
GetData("DIST", {std::to_string(tof)});
}
void PCBOdoInterface::SetRobotPos(const modelec_interface::msg::OdometryPos::SharedPtr msg) const
@@ -324,7 +331,8 @@ namespace Modelec
AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta);
}
void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y, const long theta) const
void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y,
const long theta) const
{
std::vector<std::string> data = {
std::to_string(index),

2
src/modelec_gui/CMakeLists.txt
View File

@@ -12,6 +12,7 @@ set(CMAKE_AUTOMOC ON)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(modelec_interface REQUIRED)
find_package(Qt6 COMPONENTS
Core
Gui
@@ -23,6 +24,7 @@ add_executable(modelec_gui src/modelec_gui.cpp src/main.cpp include/modelec_gui/
ament_target_dependencies(modelec_gui
rclcpp
std_msgs
modelec_interface
)
target_link_libraries(modelec_gui
Qt6::Core

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

@@ -7,20 +7,31 @@
#include <QVBoxLayout>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <modelec_interface/msg/odometry_pos.hpp>
#include <modelec_interface/srv/odometry_speed.hpp>
class ROS2QtGUI : public QWidget {
Q_OBJECT
public:
explicit ROS2QtGUI(std::shared_ptr<rclcpp::Node> node, QWidget *parent = nullptr);
explicit ROS2QtGUI(QWidget *parent = nullptr);
~ROS2QtGUI() override; // Explicitly declare destructor
private slots:
void onSendClicked();
rclcpp::Node::SharedPtr get_node() const { return node_; }
private:
QLineEdit *textBox_;
QPushButton *sendButton_;
std::shared_ptr<rclcpp::Node> node_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
QLineEdit *xBox_, *yBox_, *thetaBox_;
QVBoxLayout *mainLayout_;
QHBoxLayout *posLayout_;
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_;
void PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg);
};

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

@@ -1,17 +1,24 @@
#include "modelec_gui/modelec_gui.hpp"
int main(int argc, char *argv[]) {
// Initialize ROS 2
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>("ros2_qt_gui_node");
// Initialize Qt
QApplication app(argc, argv);
ROS2QtGUI gui(node);
gui.setWindowTitle("ROS 2 Qt GUI");
gui.resize(400, 200);
gui.show();
// Run the Qt event loop
return app.exec();
// Create the ROS2QtGUI instance, which is the Qt GUI and ROS node
auto gui = std::make_shared<ROS2QtGUI>();
gui->show();
// Create and start the ROS 2 executor in a separate thread
auto exec = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
exec->add_node(gui->get_node()); // Ensure this is the correct way to add your node
std::thread spin_thread([exec]() { exec->spin(); });
int result = app.exec();
// Shutdown ROS 2 and join the thread
exec->cancel();
spin_thread.join();
rclcpp::shutdown();
return result;
}

88
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -1,25 +1,81 @@
#include "modelec_gui/modelec_gui.hpp"
ROS2QtGUI::ROS2QtGUI(std::shared_ptr<rclcpp::Node> node, QWidget *parent)
: QWidget(parent), node_(node) {
publisher_ = node_->create_publisher<std_msgs::msg::String>("ui_topic", 10);
ROS2QtGUI::ROS2QtGUI(QWidget *parent)
: QWidget(parent), node_(rclcpp::Node::make_shared("qt_gui_node")) {
QVBoxLayout *layout = new QVBoxLayout(this);
textBox_ = new QLineEdit(this);
sendButton_ = new QPushButton("Send Message", this);
xBox_ = new QLineEdit("x: ?");
yBox_ = new QLineEdit("y: ?");
thetaBox_ = new QLineEdit("theta: ?");
xBox_->setReadOnly(true);
yBox_->setReadOnly(true);
thetaBox_->setReadOnly(true);
layout->addWidget(textBox_);
layout->addWidget(sendButton_);
setLayout(layout);
posLayout_ = new QHBoxLayout;
posLayout_->addWidget(xBox_);
posLayout_->addWidget(yBox_);
posLayout_->addWidget(thetaBox_);
connect(sendButton_, &QPushButton::clicked, this, &ROS2QtGUI::onSendClicked);
askSpeed_ = new QPushButton("Ask speed");
connect(askSpeed_, &QPushButton::clicked, this, [this]() {
auto request = std::make_shared<modelec_interface::srv::OdometrySpeed::Request>();
auto future = client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(node_, future) !=
rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(node_->get_logger(), "Failed to call service");
return;
}
if (auto response = future.get()) {
xSpeedBox_->setText(QString("x speed: %1").arg(response->x));
ySpeedBox_->setText(QString("y speed: %1").arg(response->y));
thetaSpeedBox_->setText(QString("theta speed: %1").arg(response->theta));
} else {
RCLCPP_ERROR(node_->get_logger(), "Failed to call service");
}
});
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_->addLayout(posLayout_);
mainLayout_->addWidget(askSpeed_);
mainLayout_->addLayout(speedLayout_);
setLayout(mainLayout_);
sub_ = node_->create_subscription<modelec_interface::msg::OdometryPos>(
"/odometry/position", 10,
std::bind(&ROS2QtGUI::PositionCallback, this, std::placeholders::_1));
client_ = node_->create_client<modelec_interface::srv::OdometrySpeed>("odometry/speed");
// ensure the server is up
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...");
}
}
ROS2QtGUI::~ROS2QtGUI() = default;
void ROS2QtGUI::onSendClicked() {
std_msgs::msg::String message;
message.data = textBox_->text().toStdString();
publisher_->publish(message);
RCLCPP_INFO(node_->get_logger(), "Published: '%s'", message.data.c_str());
}
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));
});
}