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

PATCH EVEYTHINGS + add start button

Browse Source
This commit is contained in:
acki
2025-04-07 14:16:21 -04:00
parent e208863016
commit 084b70e91e
12 changed files with 377 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
src/modelec/include/modelec/multiple_serial_listener.hpp
View File

@@ -4,12 +4,49 @@
#include <boost/asio.hpp>
#include <std_msgs/msg/string.hpp>
#include <modelec_interface/srv/add_serial_listener.hpp>
#include <modelec_interface/msg/write_serial.hpp>
#include <deque>
#include <thread>
#include <mutex>
#define MAX_MESSAGE_LEN 1048
#define READ_REFRESH_RATE 100 //ms
namespace Modelec
{
class SerialListener {
private:
bool status_;
int bauds_;
std::string serial_port_;
int max_message_len_;
boost::asio::io_service& io_;
std::vector<char> read_buffer_;
std::deque<std::string> write_queue_;
std::mutex write_mutex_;
std::thread io_thread_;
void start_async_read();
void start_async_write();
public:
std::string name_;
boost::asio::serial_port port_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
SerialListener(const std::string& name, int bauds, const std::string& serial_port,
int max_message_len, boost::asio::io_service& io);
~SerialListener();
void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; }
bool IsOk() const { return status_; }
void write(std_msgs::msg::String::SharedPtr msg);
};
class MultipleSerialListener : public rclcpp::Node
{
public:
@@ -17,43 +54,16 @@ public:
~MultipleSerialListener() override;
private:
int default_max_message_len_ = MAX_MESSAGE_LEN;
int read_refresh_rate_ = READ_REFRESH_RATE;
class SerialListener {
private:
bool status_;
int bauds_;
std::string serial_port_;
int max_message_len_;
boost::asio::streambuf read_buf_;
boost::asio::io_service& io_;
public:
std::string name_;
boost::asio::serial_port port_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
SerialListener(const std::string& name, int bauds, const std::string& serial_port, int max_message_len, boost::asio::io_service& io);
void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; }
bool IsOk() const { return status_; }
void read();
void write(const std::string& data);
};
std::map<std::string, std::shared_ptr<SerialListener>> serial_listeners;
boost::asio::io_service io;
rclcpp::TimerBase::SharedPtr timer;
rclcpp::Service<modelec_interface::srv::AddSerialListener>::SharedPtr add_serial_listener_service_;
void add_serial_listener(const std::shared_ptr<modelec_interface::srv::AddSerialListener::Request> request, std::shared_ptr<modelec_interface::srv::AddSerialListener::Response> response);
void readData(const SerialListener& listener);
void add_serial_listener(
const std::shared_ptr<modelec_interface::srv::AddSerialListener::Request> request,
std::shared_ptr<modelec_interface::srv::AddSerialListener::Response> response);
OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter> &parameters);

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

@@ -3,8 +3,8 @@
#include <rclcpp/rclcpp.hpp>
#include <queue>
#include <future>
#include <mutex>
#include <future>
#include <std_msgs/msg/string.hpp>
#include <modelec_interface/msg/odometry_pos.hpp>
@@ -12,17 +12,24 @@
#include <modelec_interface/msg/odometry_to_f.hpp>
#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/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>
namespace Modelec {
class PCBOdoInterface : public rclcpp::Node {
public:
PCBOdoInterface();
~PCBOdoInterface() override = default;
rclcpp::CallbackGroup::SharedPtr pcb_callback_group_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
std::thread pcb_executor_thread_;
~PCBOdoInterface() override;
struct OdometryData {
long x;
@@ -42,17 +49,17 @@ private:
rclcpp::Publisher<modelec_interface::msg::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_publisher_;
rclcpp::Subscription<modelec_interface::msg::OdometryAddWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
void AddWaypointCallback(const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const;
rclcpp::Subscription<modelec_interface::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
void AddWaypointCallback(const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const;
void SetPosCallback(const modelec_interface::msg::OdometryPos::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_;
// Promises and mutexes to synchronize service responses asynchronously
std::queue<std::promise<long>> tof_promises_;
std::mutex tof_mutex_;
@@ -62,6 +69,10 @@ private:
std::queue<std::promise<OdometryData>> pos_promises_;
std::mutex pos_mutex_;
std::queue<std::promise<bool>> start_promises_;
std::mutex start_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);
@@ -69,24 +80,26 @@ private:
std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response);
void HandleGetPosition(const std::shared_ptr<modelec_interface::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryPosition::Response> response);
std::shared_ptr<modelec_interface::srv::OdometryPosition::Response> response);
void HandleGetStart(const std::shared_ptr<modelec_interface::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryStart::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);
public:
void SendToPCB(const std::string &data) const;
void SendToPCB(const std::string& order, const std::string& elem, const std::vector<std::string>& data = {}) const;
void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendOrder(const std::string &elem, const std::vector<std::string> &data = {}) const;
void GetPos() const;
void GetSpeed() const;
void GetToF(const int &tof) const;
void SetRobotPos(const modelec_interface::msg::OdometryPos::SharedPtr msg) const;
@@ -95,6 +108,8 @@ public:
void AddWaypoint(modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const;
void AddWaypoint(int index, bool IsStopPoint, long x, long y, long theta) const;
void SetStart(const modelec_interface::msg::OdometryStart::SharedPtr msg) const;
void SetStart(bool start) const;
};
} // Modelec
} // namespace Modelec

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

@@ -11,6 +11,6 @@ launch:
name: 'pcb_odo_interface'
- node:
pkg: 'modelec_gui'
exec: "modelec_gui"
name: "modelec_gui"
pkg: 'modelec_gui'
exec: "modelec_gui"
name: "modelec_gui"

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

@@ -1,26 +1,102 @@
#include <modelec/multiple_serial_listener.hpp>
#include "modelec/multiple_serial_listener.hpp"
#include <boost/system/error_code.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/asio.hpp>
#include <chrono>
#include <thread>
namespace Modelec
{
MultipleSerialListener::MultipleSerialListener() : Node("multiple_serial_listener"), io()
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
{
port_.open(serial_port_);
port_.set_option(boost::asio::serial_port_base::baud_rate(bauds_));
status_ = true;
}
catch (boost::system::system_error& e)
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Failed to open serial port: %s", e.what());
status_ = false;
return;
}
read_buffer_.resize(max_message_len_);
start_async_read();
io_thread_ = std::thread([this]() {
try {
io_.run();
} catch (const std::exception& e) {
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "IO thread exception: %s", e.what());
}
});
}
SerialListener::~SerialListener()
{
if (port_.is_open()) port_.close();
io_.stop();
if (io_thread_.joinable()) io_thread_.join();
}
void SerialListener::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);
start_async_read(); // continue reading
} else {
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async read error: %s", ec.message().c_str());
}
});
}
void SerialListener::write(std_msgs::msg::String::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(write_mutex_);
bool write_in_progress = !write_queue_.empty();
write_queue_.push_back(msg->data);
if (!write_in_progress) {
start_async_write();
}
}
void SerialListener::start_async_write()
{
if (write_queue_.empty()) return;
boost::asio::async_write(
port_,
boost::asio::buffer(write_queue_.front()),
[this](const boost::system::error_code& ec, std::size_t /*length*/) {
std::lock_guard<std::mutex> lock(write_mutex_);
if (!ec) {
write_queue_.pop_front();
if (!write_queue_.empty()) {
start_async_write(); // continue writing
}
} else {
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async write error: %s", ec.message().c_str());
}
});
}
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)
{
return onParameterChange(parameters);
});
}
MultipleSerialListener::~MultipleSerialListener()
@@ -54,12 +130,10 @@ namespace Modelec
return;
}
listener->publisher_ = create_publisher<std_msgs::msg::String>("raw_data/" + listener->name_, 10);
listener->publisher_ = create_publisher<std_msgs::msg::String>("raw_data/" + request->name, 10);
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);
});
"send_to_serial/" + request->name, rclcpp::QoS(10).reliability(rclcpp::ReliabilityPolicy::BestEffort),
std::bind(&SerialListener::write, listener.get(), std::placeholders::_1));
serial_listeners.insert({request->name, listener});
@@ -76,8 +150,7 @@ namespace Modelec
for (const auto& parameter : parameters)
{
if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() ==
"max_message_len")
if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == "max_message_len")
{
updateConnection();
}
@@ -92,72 +165,6 @@ namespace Modelec
{
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)
{
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)
: bauds_(bauds), serial_port_(serial_port), max_message_len_(max_message_len), io_(io), name_(name), port_(io)
{
try
{
port_.open(serial_port_);
port_.set_option(boost::asio::serial_port_base::baud_rate(bauds_));
status_ = true;
}
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()
{
if (!status_) return;
std::vector<char> data(max_message_len_);
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)
{
// 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)
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Read error: %s", e.what());
}
}
void MultipleSerialListener::SerialListener::write(const std::string& data)
{
try
{
boost::asio::write(port_, boost::asio::buffer(data));
}
catch (boost::system::system_error& e)
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Write error: %s", e.what());
}
}
}

145
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/pts/6"; // TODO : check the real serial port
request->serial_port = "/dev/pts/4"; // 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)))
{
@@ -32,11 +32,22 @@ 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_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>(
res->publisher, 10, [this](std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
});
res->publisher, 10,
std::bind(&PCBOdoInterface::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>(res->subscriber, 10);
}
@@ -87,10 +98,23 @@ namespace Modelec
get_position_service_ = create_service<modelec_interface::srv::OdometryPosition>(
"odometry/position",
std::bind(&PCBOdoInterface::HandleGetPosition, this, std::placeholders::_1, std::placeholders::_2));
set_start_service_ = create_service<modelec_interface::srv::OdometryStart>(
"odometry/start",
std::bind(&PCBOdoInterface::HandleGetStart, this, std::placeholders::_1, std::placeholders::_2));
}
PCBOdoInterface::~PCBOdoInterface()
{
pcb_executor_->cancel();
if (pcb_executor_thread_.joinable()) {
pcb_executor_thread_.join();
}
}
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<std::string> tokens = split(msg->data, ';');
if (tokens.size() < 2)
{
@@ -112,6 +136,7 @@ namespace Modelec
message.theta = theta;
odo_pos_publisher_->publish(message);
ResolvePositionRequest({x, y, theta});
}
else if (tokens[1] == "SPEED")
@@ -152,11 +177,19 @@ namespace Modelec
}
else if (tokens[0] == "OK")
{
RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str());
if (tokens[1] == "START")
{
bool start = std::stoi(tokens[2]);
ResolveStartRequest(start);
}
else
{
RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str());
}
}
else if (tokens[0] == "KO")
{
RCLCPP_ERROR(this->get_logger(), "PCB error: %s", msg->data.c_str());
RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg->data.c_str());
}
}
@@ -170,11 +203,13 @@ namespace Modelec
SetRobotPos(msg);
}
void PCBOdoInterface::HandleGetTof(const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response)
void PCBOdoInterface::HandleGetTof(
const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response)
{
std::promise<long> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(tof_mutex_);
tof_promises_.push(std::move(promise));
@@ -185,11 +220,13 @@ namespace Modelec
response->distance = future.get();
}
void PCBOdoInterface::HandleGetSpeed(const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request>,
std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response)
void PCBOdoInterface::HandleGetSpeed(
const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request>,
std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response)
{
std::promise<OdometryData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(speed_mutex_);
speed_promises_.push(std::move(promise));
@@ -198,7 +235,6 @@ namespace Modelec
GetSpeed();
OdometryData result = future.get();
response->x = result.x;
response->y = result.y;
response->theta = result.theta;
@@ -210,6 +246,7 @@ namespace Modelec
{
std::promise<OdometryData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(pos_mutex_);
pos_promises_.push(std::move(promise));
@@ -218,50 +255,71 @@ namespace Modelec
GetPos();
OdometryData result = future.get();
response->x = result.x;
response->y = result.y;
response->theta = result.theta;
}
void PCBOdoInterface::HandleGetStart(const std::shared_ptr<modelec_interface::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryStart::Response> response)
{
std::promise<bool> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(start_mutex_);
start_promises_.push(std::move(promise));
}
SetStart(request->start);
response->success = future.get();
}
void PCBOdoInterface::ResolveToFRequest(const long distance)
{
std::lock_guard<std::mutex> lock(tof_mutex_);
if (!tof_promises_.empty())
{
std::promise<long> promise = std::move(tof_promises_.front());
if (!tof_promises_.empty()) {
auto promise = std::move(tof_promises_.front());
tof_promises_.pop();
promise.set_value(distance);
}
else
{
RCLCPP_WARN(this->get_logger(), "Received ToF response but no promise waiting");
} else {
RCLCPP_DEBUG(get_logger(), "No pending ToF request to resolve.");
}
}
void PCBOdoInterface::ResolveSpeedRequest(const OdometryData& speed)
{
std::lock_guard<std::mutex> lock(speed_mutex_);
if (!speed_promises_.empty())
{
std::promise<OdometryData> promise = std::move(speed_promises_.front());
if (!speed_promises_.empty()) {
auto promise = std::move(speed_promises_.front());
speed_promises_.pop();
promise.set_value(speed);
}
else
{
RCLCPP_WARN(this->get_logger(), "Received Speed response but no promise waiting");
} else {
RCLCPP_DEBUG(get_logger(), "No pending Speed request to resolve.");
}
}
void PCBOdoInterface::ResolvePositionRequest(const OdometryData& position)
{
std::lock_guard<std::mutex> lock(pos_mutex_);
if (!pos_promises_.empty())
{
std::promise<OdometryData> promise = std::move(pos_promises_.front());
if (!pos_promises_.empty()) {
auto promise = std::move(pos_promises_.front());
pos_promises_.pop();
promise.set_value(position);
} else {
RCLCPP_DEBUG(get_logger(), "No pending Position request to resolve.");
}
}
void PCBOdoInterface::ResolveStartRequest(bool start)
{
std::lock_guard<std::mutex> lock(start_mutex_);
if (!start_promises_.empty()) {
auto promise = std::move(start_promises_.front());
start_promises_.pop();
promise.set_value(start);
} else {
RCLCPP_DEBUG(get_logger(), "No pending Start request to resolve.");
}
}
@@ -344,13 +402,38 @@ namespace Modelec
SendOrder("WAYPOINT", data);
}
void PCBOdoInterface::SetStart(const modelec_interface::msg::OdometryStart::SharedPtr msg) const
{
SetStart(msg->start);
}
void PCBOdoInterface::SetStart(bool start) const
{
SendOrder("START", {std::to_string(start)});
}
} // Modelec
int main(int argc, char** argv)
/*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);
auto node = std::make_shared<Modelec::PCBOdoInterface>();
// Increase number of threads explicitly!
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 4 /* or more threads! */);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}

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

@@ -9,17 +9,19 @@
#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(QWidget *parent = nullptr);
explicit ROS2QtGUI(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
~ROS2QtGUI() override; // Explicitly declare destructor
rclcpp::Node::SharedPtr get_node() const { return node_; }
private:
QPushButton* startButton_;
QLineEdit *xBox_, *yBox_, *thetaBox_;
QVBoxLayout *mainLayout_;
QHBoxLayout *posLayout_;
@@ -32,6 +34,7 @@ private:
// client
rclcpp::Client<modelec_interface::srv::OdometrySpeed>::SharedPtr client_;
rclcpp::Client<modelec_interface::srv::OdometryStart>::SharedPtr client_start_;
void PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg);
};

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

@@ -1,24 +1,40 @@
#include <rclcpp/rclcpp.hpp>
#include <QApplication>
#include <QThread>
#include "modelec_gui/modelec_gui.hpp"
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
QApplication app(argc, argv);
int main(int argc, char **argv)
{
// Initialize the Qt application
QApplication app(argc, argv);
// Create the ROS2QtGUI instance, which is the Qt GUI and ROS node
auto gui = std::make_shared<ROS2QtGUI>();
gui->show();
// Initialize ROS 2
rclcpp::init(argc, argv);
// 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
// Create the node only once
auto node = rclcpp::Node::make_shared("qt_gui_node");
std::thread spin_thread([exec]() { exec->spin(); });
// Create the GUI and pass the node to it
ROS2QtGUI window(node); // Pass the node to the GUI component
window.show();
int result = app.exec();
// Create an executor for ROS 2 to manage the node
rclcpp::executors::MultiThreadedExecutor executor;
// Shutdown ROS 2 and join the thread
exec->cancel();
spin_thread.join();
rclcpp::shutdown();
return result;
}
// Add the node to the executor once
executor.add_node(node);
// Run ROS 2 in a separate thread
std::thread ros_thread([&executor]() {
executor.spin(); // Execute the node's callbacks
});
// Start the Qt application event loop
int ret = app.exec();
// Ensure the ROS 2 executor thread ends correctly
ros_thread.join();
rclcpp::shutdown(); // Shutdown ROS 2
return ret; // Return the application result
}

70
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -1,8 +1,30 @@
#include "modelec_gui/modelec_gui.hpp"
#include <rclcpp/rclcpp.hpp>
#include <QMetaObject>
#include <utility>
ROS2QtGUI::ROS2QtGUI(QWidget *parent)
: QWidget(parent), node_(rclcpp::Node::make_shared("qt_gui_node")) {
ROS2QtGUI::ROS2QtGUI(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: ?");
@@ -17,22 +39,21 @@ ROS2QtGUI::ROS2QtGUI(QWidget *parent)
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>();
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");
}
// 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: ?");
@@ -48,24 +69,37 @@ ROS2QtGUI::ROS2QtGUI(QWidget *parent)
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");
// ensure the server is up
// 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...");
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...");
}
}

3
src/modelec_interface/CMakeLists.txt
View File

@@ -16,6 +16,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Odometry/OdometryToF.msg"
"msg/Odometry/OdometryWaypointReach.msg"
"msg/Odometry/OdometryAddWaypoint.msg"
"msg/Odometry/OdometryStart.msg"
"msg/WriteSerial.msg"
"msg/PCA9685Servo.msg"
"msg/ServoMode.msg"
"msg/Solenoid.msg"
@@ -23,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"srv/Odometry/OdometryPosition.srv"
"srv/Odometry/OdometrySpeed.srv"
"srv/Odometry/OdometryToF.srv"
"srv/Odometry/OdometryStart.srv"
"srv/AddServoMotor.srv"
"srv/AddSolenoid.srv"
"srv/Tirette.srv"

1
src/modelec_interface/msg/Odometry/OdometryStart.msg Normal file
View File

@@ -0,0 +1 @@
bool start

2
src/modelec_interface/msg/WriteSerial.msg Normal file
View File

@@ -0,0 +1,2 @@
string name
string data

3
src/modelec_interface/srv/Odometry/OdometryStart.srv Normal file
View File

@@ -0,0 +1,3 @@
bool start
---
bool success