From 084b70e91e707ec383c838f3a73679c47d50172b Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 7 Apr 2025 14:16:21 -0400 Subject: [PATCH] PATCH EVEYTHINGS + add start button --- .../modelec/multiple_serial_listener.hpp | 72 ++++--- .../include/modelec/pcb_odo_interface.h | 35 +++- src/modelec/launch/test.modelec.launch.yml | 6 +- src/modelec/src/multiple_serial_listener.cpp | 185 +++++++++--------- src/modelec/src/pcb_odo_interface.cpp | 145 +++++++++++--- .../include/modelec_gui/modelec_gui.hpp | 5 +- src/modelec_gui/src/main.cpp | 50 +++-- src/modelec_gui/src/modelec_gui.cpp | 70 +++++-- src/modelec_interface/CMakeLists.txt | 3 + .../msg/Odometry/OdometryStart.msg | 1 + src/modelec_interface/msg/WriteSerial.msg | 2 + .../srv/Odometry/OdometryStart.srv | 3 + 12 files changed, 377 insertions(+), 200 deletions(-) create mode 100644 src/modelec_interface/msg/Odometry/OdometryStart.msg create mode 100644 src/modelec_interface/msg/WriteSerial.msg create mode 100644 src/modelec_interface/srv/Odometry/OdometryStart.srv diff --git a/src/modelec/include/modelec/multiple_serial_listener.hpp b/src/modelec/include/modelec/multiple_serial_listener.hpp index a7b1f49..8e473d6 100644 --- a/src/modelec/include/modelec/multiple_serial_listener.hpp +++ b/src/modelec/include/modelec/multiple_serial_listener.hpp @@ -4,12 +4,49 @@ #include #include #include +#include +#include +#include +#include #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 read_buffer_; + std::deque 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::SharedPtr publisher_; + rclcpp::Subscription::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::SharedPtr publisher_; - rclcpp::Subscription::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> serial_listeners; boost::asio::io_service io; - rclcpp::TimerBase::SharedPtr timer; rclcpp::Service::SharedPtr add_serial_listener_service_; - void add_serial_listener(const std::shared_ptr request, std::shared_ptr response); - - void readData(const SerialListener& listener); + void add_serial_listener( + const std::shared_ptr request, + std::shared_ptr response); OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector ¶meters); diff --git a/src/modelec/include/modelec/pcb_odo_interface.h b/src/modelec/include/modelec/pcb_odo_interface.h index 2924e1d..7cabe24 100644 --- a/src/modelec/include/modelec/pcb_odo_interface.h +++ b/src/modelec/include/modelec/pcb_odo_interface.h @@ -3,8 +3,8 @@ #include #include -#include #include +#include #include #include @@ -12,17 +12,24 @@ #include #include #include +#include #include #include #include +#include +#include namespace Modelec { class PCBOdoInterface : public rclcpp::Node { public: PCBOdoInterface(); - ~PCBOdoInterface() override = default; + + rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; + std::shared_ptr pcb_executor_; + std::thread pcb_executor_thread_; + ~PCBOdoInterface() override; struct OdometryData { long x; @@ -42,17 +49,17 @@ private: rclcpp::Publisher::SharedPtr odo_waypoint_reach_publisher_; rclcpp::Subscription::SharedPtr odo_add_waypoint_subscriber_; - - void AddWaypointCallback(const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const; - rclcpp::Subscription::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::SharedPtr get_tof_service_; rclcpp::Service::SharedPtr get_speed_service_; rclcpp::Service::SharedPtr get_position_service_; + rclcpp::Service::SharedPtr set_start_service_; + // Promises and mutexes to synchronize service responses asynchronously std::queue> tof_promises_; std::mutex tof_mutex_; @@ -62,6 +69,10 @@ private: std::queue> pos_promises_; std::mutex pos_mutex_; + std::queue> start_promises_; + std::mutex start_mutex_; + + // Service handlers using async wait with promises void HandleGetTof(const std::shared_ptr request, std::shared_ptr response); @@ -69,24 +80,26 @@ private: std::shared_ptr response); void HandleGetPosition(const std::shared_ptr request, - std::shared_ptr response); + std::shared_ptr response); + void HandleGetStart(const std::shared_ptr request, + std::shared_ptr 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& data = {}) const; void GetData(const std::string& elem, const std::vector& data = {}) const; - void SendOrder(const std::string &elem, const std::vector &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 diff --git a/src/modelec/launch/test.modelec.launch.yml b/src/modelec/launch/test.modelec.launch.yml index f7e57fa..c0d5f71 100644 --- a/src/modelec/launch/test.modelec.launch.yml +++ b/src/modelec/launch/test.modelec.launch.yml @@ -11,6 +11,6 @@ launch: name: 'pcb_odo_interface' - node: - pkg: 'modelec_gui' - exec: "modelec_gui" - name: "modelec_gui" \ No newline at end of file + pkg: 'modelec_gui' + exec: "modelec_gui" + name: "modelec_gui" diff --git a/src/modelec/src/multiple_serial_listener.cpp b/src/modelec/src/multiple_serial_listener.cpp index 1f05be1..e0a11d0 100644 --- a/src/modelec/src/multiple_serial_listener.cpp +++ b/src/modelec/src/multiple_serial_listener.cpp @@ -1,26 +1,102 @@ -#include +#include "modelec/multiple_serial_listener.hpp" +#include +#include +#include +#include +#include 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 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 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( "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& parameters) - { - return onParameterChange(parameters); - }); } MultipleSerialListener::~MultipleSerialListener() @@ -54,12 +130,10 @@ namespace Modelec return; } - listener->publisher_ = create_publisher("raw_data/" + listener->name_, 10); + listener->publisher_ = create_publisher("raw_data/" + request->name, 10); listener->subscriber_ = create_subscription( - "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 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()); - } } } diff --git a/src/modelec/src/pcb_odo_interface.cpp b/src/modelec/src/pcb_odo_interface.cpp index f0ab82a..3f135cf 100644 --- a/src/modelec/src/pcb_odo_interface.cpp +++ b/src/modelec/src/pcb_odo_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec auto request = std::make_shared(); request->name = "pcb_odo"; request->bauds = 115200; - request->serial_port = "/dev/pts/6"; // TODO : check the real serial port + request->serial_port = "/dev/pts/4"; // TODO : check the real serial port auto client = this->create_client("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( - 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(); + 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(res->subscriber, 10); } @@ -87,10 +98,23 @@ namespace Modelec get_position_service_ = create_service( "odometry/position", std::bind(&PCBOdoInterface::HandleGetPosition, this, std::placeholders::_1, std::placeholders::_2)); + + set_start_service_ = create_service( + "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 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 request, - std::shared_ptr response) + void PCBOdoInterface::HandleGetTof( + const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); + { std::lock_guard 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, - std::shared_ptr response) + void PCBOdoInterface::HandleGetSpeed( + const std::shared_ptr, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); + { std::lock_guard 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 promise; auto future = promise.get_future(); + { std::lock_guard 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 request, + std::shared_ptr response) + { + std::promise promise; + auto future = promise.get_future(); + + { + std::lock_guard 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 lock(tof_mutex_); - if (!tof_promises_.empty()) - { - std::promise 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 lock(speed_mutex_); - if (!speed_promises_.empty()) - { - std::promise 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 lock(pos_mutex_); - if (!pos_promises_.empty()) - { - std::promise 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 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()); rclcpp::shutdown(); return 0; +}*/ + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + // 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; } diff --git a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp index 6a78003..3913172 100644 --- a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp +++ b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp @@ -9,17 +9,19 @@ #include #include #include +#include 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::SharedPtr client_; + rclcpp::Client::SharedPtr client_start_; void PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg); }; diff --git a/src/modelec_gui/src/main.cpp b/src/modelec_gui/src/main.cpp index a2bbc3c..12df1ba 100644 --- a/src/modelec_gui/src/main.cpp +++ b/src/modelec_gui/src/main.cpp @@ -1,24 +1,40 @@ +#include +#include +#include #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(); - 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(); - 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; -} \ No newline at end of file + // 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 +} diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index d1e595c..e2c7f63 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -1,8 +1,30 @@ #include "modelec_gui/modelec_gui.hpp" +#include +#include +#include -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(); + request->start = true; + client_start_->async_send_request(request, [this](rclcpp::Client::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(); - 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::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( "/odometry/position", 10, std::bind(&ROS2QtGUI::PositionCallback, this, std::placeholders::_1)); + // Set up service client client_ = node_->create_client("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("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..."); } } diff --git a/src/modelec_interface/CMakeLists.txt b/src/modelec_interface/CMakeLists.txt index 15abe1e..ec221c6 100644 --- a/src/modelec_interface/CMakeLists.txt +++ b/src/modelec_interface/CMakeLists.txt @@ -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" diff --git a/src/modelec_interface/msg/Odometry/OdometryStart.msg b/src/modelec_interface/msg/Odometry/OdometryStart.msg new file mode 100644 index 0000000..24197e4 --- /dev/null +++ b/src/modelec_interface/msg/Odometry/OdometryStart.msg @@ -0,0 +1 @@ +bool start \ No newline at end of file diff --git a/src/modelec_interface/msg/WriteSerial.msg b/src/modelec_interface/msg/WriteSerial.msg new file mode 100644 index 0000000..589dd1d --- /dev/null +++ b/src/modelec_interface/msg/WriteSerial.msg @@ -0,0 +1,2 @@ +string name +string data \ No newline at end of file diff --git a/src/modelec_interface/srv/Odometry/OdometryStart.srv b/src/modelec_interface/srv/Odometry/OdometryStart.srv new file mode 100644 index 0000000..8c8f6fe --- /dev/null +++ b/src/modelec_interface/srv/Odometry/OdometryStart.srv @@ -0,0 +1,3 @@ +bool start +--- +bool success \ No newline at end of file