From 84b74311892c7977e27372802054377c85d2c9cc Mon Sep 17 00:00:00 2001 From: aquintan Date: Fri, 17 Apr 2026 11:25:19 +0200 Subject: [PATCH 01/11] WebGUI cpp common interface --- common_interfaces_cpp/CMakeLists.txt | 16 +- .../common_interfaces_cpp/hal/camera.hpp | 38 +-- .../webgui/MeasuringThreadingGUI.hpp | 62 +++++ common_interfaces_cpp/package.xml | 3 + common_interfaces_cpp/src/hal/camera.cpp | 19 +- .../src/webgui/MeasuringThreadingGUI.cpp | 241 ++++++++++++++++++ 6 files changed, 341 insertions(+), 38 deletions(-) create mode 100644 common_interfaces_cpp/include/common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp create mode 100644 common_interfaces_cpp/src/webgui/MeasuringThreadingGUI.cpp diff --git a/common_interfaces_cpp/CMakeLists.txt b/common_interfaces_cpp/CMakeLists.txt index 4c5b1a3e4..e4c2c9545 100644 --- a/common_interfaces_cpp/CMakeLists.txt +++ b/common_interfaces_cpp/CMakeLists.txt @@ -17,7 +17,6 @@ set(ros2_dependencies ros_gz_interfaces ) -# Find ament_cmake (required for the build system itself) find_package(ament_cmake REQUIRED) # Loop through the list to find all specified ROS 2 packages automatically @@ -28,8 +27,13 @@ endforeach() # Find OpenCV explicitly (Required by Camera and Classnet for cv::Mat and cv::dnn) find_package(OpenCV REQUIRED) +# Find external C++ libraries for the WebGUI +find_package(Boost REQUIRED COMPONENTS system thread) +find_package(nlohmann_json REQUIRED) + # Create the shared library from the source files matching the new structure add_library(${PROJECT_NAME} SHARED + # HAL src/hal/bumper.cpp src/hal/camera.cpp src/hal/classnet.cpp @@ -38,6 +42,8 @@ add_library(${PROJECT_NAME} SHARED src/hal/motors.cpp src/hal/odometry.cpp src/hal/sim_time.cpp + # WebGUI + src/webgui/MeasuringThreadingGUI.cpp ) # Specify the include directories so the compiler finds the .hpp files @@ -45,11 +51,17 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $ ${OpenCV_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) # Link all listed ROS 2 dependencies and OpenCV to the library ament_target_dependencies(${PROJECT_NAME} ${ros2_dependencies}) -target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} + Boost::system + Boost::thread + nlohmann_json::nlohmann_json +) # Install the include directory so other packages can do #include install(DIRECTORY include/ diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/camera.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/camera.hpp index 86c553328..6d3cf65cc 100644 --- a/common_interfaces_cpp/include/common_interfaces_cpp/hal/camera.hpp +++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/camera.hpp @@ -3,64 +3,42 @@ #include #include +#include #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" #include -#include -/* ### AUXILIARY FUNCTIONS ### */ - -const int MAXRANGE = 8; // max length received from imageD +const int MAXRANGE = 8; const int MINRANGE = 0; -/* Represents a camera image with metadata and pixel data. */ class Image { public: Image(); - int height; // Image height [pixels] - int width; // Image width [pixels] - double timeStamp; // Time stamp [s] */ - std::string format; // Image format string (RGB8, BGR,...) - cv::Mat data; // The image data itself + int height; + int width; + double timeStamp; + std::string format; + cv::Mat data; std::string to_string() const; }; -// Declaración de función auxiliar implícita en tu código Python cv::Mat depthToRGB8(const cv::Mat& gray_img_buff, const std::string& encoding); -/* - * Convert a ROS Image message to a JdeRobot Image object. - * - * Args: - * img: ROS sensor_msgs/Image message. - * - * Returns: - * Image object with BGR data, or None if the message is empty. - */ std::shared_ptr imageMsg2Image(const sensor_msgs::msg::Image& img); -/* ### HAL INTERFACE ### */ - -/* ROS2 node that subscribes to a camera topic and stores the latest image. */ class CameraNode : public rclcpp::Node { public: CameraNode(const std::string& topic); - /* - * Return the latest camera image. - * - * Returns: - * Image object with BGR data, or None if no image has been received. - */ std::shared_ptr getImage() const; private: - /* Store the latest image message received from the topic. */ void listener_callback(const sensor_msgs::msg::Image::SharedPtr msg); rclcpp::Subscription::SharedPtr sub_; sensor_msgs::msg::Image last_img_; + mutable std::mutex img_mutex_; }; #endif \ No newline at end of file diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp new file mode 100644 index 000000000..ff92ecb0e --- /dev/null +++ b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp @@ -0,0 +1,62 @@ +#ifndef MEASURING_THREADING_GUI_HPP_ +#define MEASURING_THREADING_GUI_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class MeasuringThreadingGUI { +public: + MeasuringThreadingGUI(const std::string& host = "127.0.0.1", const std::string& port = "2303", double freq = 30.0, const std::string& world_name = "default"); + virtual ~MeasuringThreadingGUI(); + + void start(); + void send_to_client(const std::string& msg); + + void gui_in_thread(const std::string& message); + void trigger_update(); + void set_session(std::shared_ptr session); + +protected: + virtual void update_gui() = 0; + + std::string host_; + std::string port_; + std::string world_name_; + double out_period_; + + bool ack_; + bool ack_frontend_; + std::mutex ack_lock_; + + double ideal_cycle_; + std::atomic real_time_factor_; + std::atomic iteration_counter_; + std::atomic fps_; + std::atomic lat_; + + std::atomic running_; + +private: + void get_real_time_factor(); + void measure_and_send_frequency(); + + boost::asio::io_context ioc_; + std::thread ioc_thread_; + std::thread rtf_thread_; + std::thread freq_thread_; + + std::shared_ptr session_; + std::mutex session_mutex_; +}; + +#endif \ No newline at end of file diff --git a/common_interfaces_cpp/package.xml b/common_interfaces_cpp/package.xml index 2645085fc..ecdf9eda5 100644 --- a/common_interfaces_cpp/package.xml +++ b/common_interfaces_cpp/package.xml @@ -13,6 +13,9 @@ cv_bridge opencv + boost + nlohmann-json-dev + nav_msgs sensor_msgs geometry_msgs diff --git a/common_interfaces_cpp/src/hal/camera.cpp b/common_interfaces_cpp/src/hal/camera.cpp index 93f498e1a..8c85ec74d 100644 --- a/common_interfaces_cpp/src/hal/camera.cpp +++ b/common_interfaces_cpp/src/hal/camera.cpp @@ -1,15 +1,16 @@ #include "common_interfaces_cpp/hal/camera.hpp" #include #include +#include const double PI = M_PI; Image::Image() { - height = 480; // Image height [pixels] - width = 640; // Image width [pixels] - timeStamp = 0; // Time stamp [s] */ - format = ""; // Image format string (RGB8, BGR,...) - data = cv::Mat::zeros(height, width, CV_8UC3); // The image data itself + height = 480; + width = 640; + timeStamp = 0; + format = ""; + data = cv::Mat::zeros(height, width, CV_8UC3); } std::string Image::to_string() const { @@ -62,9 +63,15 @@ CameraNode::CameraNode(const std::string& topic) } void CameraNode::listener_callback(const sensor_msgs::msg::Image::SharedPtr msg) { + std::lock_guard lock(img_mutex_); last_img_ = *msg; } std::shared_ptr CameraNode::getImage() const { - return imageMsg2Image(last_img_); + sensor_msgs::msg::Image img_copy; + { + std::lock_guard lock(img_mutex_); + img_copy = last_img_; + } + return imageMsg2Image(img_copy); } \ No newline at end of file diff --git a/common_interfaces_cpp/src/webgui/MeasuringThreadingGUI.cpp b/common_interfaces_cpp/src/webgui/MeasuringThreadingGUI.cpp new file mode 100644 index 000000000..ff2e282a7 --- /dev/null +++ b/common_interfaces_cpp/src/webgui/MeasuringThreadingGUI.cpp @@ -0,0 +1,241 @@ +#include "common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp" +#include +#include +#include +#include +#include + +namespace beast = boost::beast; +namespace websocket = beast::websocket; +namespace net = boost::asio; +using tcp = boost::asio::ip::tcp; + +class WSSession : public std::enable_shared_from_this { + tcp::resolver resolver_; + websocket::stream ws_; + beast::flat_buffer buffer_; + MeasuringThreadingGUI* gui_; + std::string host_; + std::string port_; + net::steady_timer timer_; + std::vector write_queue_; + double out_period_; + +public: + explicit WSSession(net::io_context& ioc, MeasuringThreadingGUI* gui, const std::string& host, const std::string& port, double out_period) + : resolver_(net::make_strand(ioc)), ws_(net::make_strand(ioc)), gui_(gui), host_(host), port_(port), timer_(net::make_strand(ioc)), out_period_(out_period) {} + + void run() { + resolver_.async_resolve(host_, port_, beast::bind_front_handler(&WSSession::on_resolve, shared_from_this())); + } + + void on_resolve(beast::error_code ec, tcp::resolver::results_type results) { + if (ec) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + run(); + return; + } + beast::get_lowest_layer(ws_).expires_after(std::chrono::seconds(30)); + beast::get_lowest_layer(ws_).async_connect(results, beast::bind_front_handler(&WSSession::on_connect, shared_from_this())); + } + + void on_connect(beast::error_code ec, tcp::resolver::results_type::endpoint_type ep) { + if (ec) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + run(); + return; + } + beast::get_lowest_layer(ws_).expires_never(); + ws_.set_option(websocket::stream_base::timeout::suggested(beast::role_type::client)); + ws_.read_message_max(1024 * 1024 * 10); + ws_.auto_fragment(false); + std::string h = host_ + ":" + std::to_string(ep.port()); + ws_.async_handshake(h, "/", beast::bind_front_handler(&WSSession::on_handshake, shared_from_this())); + } + + void on_handshake(beast::error_code ec) { + if (ec) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + run(); + return; + } + gui_->set_session(shared_from_this()); + do_read(); + + timer_.expires_after(std::chrono::milliseconds(static_cast(out_period_ * 1000))); + timer_.async_wait(beast::bind_front_handler(&WSSession::on_timer, shared_from_this())); + } + + void do_read() { + ws_.async_read(buffer_, beast::bind_front_handler(&WSSession::on_read, shared_from_this())); + } + + void on_read(beast::error_code ec, std::size_t) { + if (ec) { + gui_->set_session(nullptr); + std::this_thread::sleep_for(std::chrono::seconds(1)); + run(); + return; + } + std::string msg = beast::buffers_to_string(buffer_.data()); + buffer_.consume(buffer_.size()); + gui_->gui_in_thread(msg); + do_read(); + } + + void on_timer(beast::error_code ec) { + if (ec) return; + + gui_->trigger_update(); + + timer_.expires_after(std::chrono::milliseconds(static_cast(out_period_ * 1000))); + timer_.async_wait(beast::bind_front_handler(&WSSession::on_timer, shared_from_this())); + } + + void send(const std::string& msg) { + net::post(ws_.get_executor(), beast::bind_front_handler(&WSSession::do_send, shared_from_this(), msg)); + } + + void do_send(const std::string& msg) { + bool writing = !write_queue_.empty(); + write_queue_.push_back(msg); + if (!writing) { + ws_.async_write(net::buffer(write_queue_.front()), beast::bind_front_handler(&WSSession::on_write, shared_from_this())); + } + } + + void on_write(beast::error_code ec, std::size_t) { + if (ec) { + gui_->set_session(nullptr); + std::this_thread::sleep_for(std::chrono::seconds(1)); + run(); + return; + } + write_queue_.erase(write_queue_.begin()); + if (!write_queue_.empty()) { + ws_.async_write(net::buffer(write_queue_.front()), beast::bind_front_handler(&WSSession::on_write, shared_from_this())); + } + } + + void close() { + if (ws_.is_open()) { + beast::error_code ec; + ws_.close(websocket::close_code::normal, ec); + } + } +}; + +MeasuringThreadingGUI::MeasuringThreadingGUI(const std::string& host, const std::string& port, double freq, const std::string& world_name) + : host_(host), port_(port), world_name_(world_name), out_period_(1.0 / freq), + ack_(true), ack_frontend_(false), ideal_cycle_(80.0), real_time_factor_(0.0), + iteration_counter_(0), fps_(-1), lat_(-1), running_(true), session_(nullptr) {} + +MeasuringThreadingGUI::~MeasuringThreadingGUI() { + running_ = false; + + if (session_) { + std::static_pointer_cast(session_)->close(); + } + ioc_.stop(); + + if (ioc_thread_.joinable()) ioc_thread_.join(); + if (rtf_thread_.joinable()) rtf_thread_.join(); + if (freq_thread_.joinable()) freq_thread_.join(); +} + +void MeasuringThreadingGUI::start() { + rtf_thread_ = std::thread(&MeasuringThreadingGUI::get_real_time_factor, this); + freq_thread_ = std::thread(&MeasuringThreadingGUI::measure_and_send_frequency, this); + + auto session = std::make_shared(ioc_, this, host_, port_, out_period_); + session->run(); + ioc_thread_ = std::thread([this]() { ioc_.run(); }); +} + +void MeasuringThreadingGUI::set_session(std::shared_ptr session) { + std::lock_guard lock(session_mutex_); + session_ = session; +} + +void MeasuringThreadingGUI::gui_in_thread(const std::string& message) { + std::lock_guard lock(ack_lock_); + if (message.find("ack") != std::string::npos) { + ack_ = true; + } else if (message.find("start") != std::string::npos) { + ack_frontend_ = true; + } +} + +void MeasuringThreadingGUI::trigger_update() { + iteration_counter_++; + bool should_update = false; + { + std::lock_guard lock(ack_lock_); + if (ack_frontend_ && ack_) { + should_update = true; + ack_ = false; + } + } + + if (should_update) { + update_gui(); + } +} + +void MeasuringThreadingGUI::send_to_client(const std::string& msg) { + std::lock_guard lock(session_mutex_); + if (session_) { + std::static_pointer_cast(session_)->send(msg); + } +} + +void MeasuringThreadingGUI::get_real_time_factor() { + while (running_) { + std::this_thread::sleep_for(std::chrono::seconds(2)); + std::string cmd = "gz topic -e -t /world/" + world_name_ + "/stats --num 1 2>/dev/null"; + FILE* pipe = popen(cmd.c_str(), "r"); + if (!pipe) continue; + + char buffer[128]; + std::string result; + while (fgets(buffer, sizeof(buffer), pipe) != nullptr) { + result += buffer; + } + pclose(pipe); + + std::regex r("real_time_factor:\\s*([0-9\\.]+)"); + std::smatch match; + if (std::regex_search(result, match, r)) { + double rtf = std::stod(match[1].str()); + real_time_factor_ = std::round(rtf * 1000.0) / 1000.0; + } + } +} + +void MeasuringThreadingGUI::measure_and_send_frequency() { + auto previous_time = std::chrono::steady_clock::now(); + + while (running_) { + std::this_thread::sleep_for(std::chrono::seconds(2)); + auto current_time = std::chrono::steady_clock::now(); + std::chrono::duration dt = current_time - previous_time; + previous_time = current_time; + + int iters = iteration_counter_.exchange(0); + double measured_cycle = (iters > 0) ? (dt.count() / iters) : 0.0; + double brain_frequency = (measured_cycle != 0.0) ? (1000.0 / measured_cycle) : 0.0; + brain_frequency = std::round(brain_frequency * 10.0) / 10.0; + + double gui_frequency = std::round((1000.0 / ideal_cycle_) * 10.0) / 10.0; + + nlohmann::json freq_msg = { + {"brain", brain_frequency}, + {"gui", gui_frequency}, + {"rtf", real_time_factor_.load()}, + {"fps", fps_.load()}, + {"lat", lat_.load()} + }; + + send_to_client(freq_msg.dump()); + } +} \ No newline at end of file From 2cf86c27cabe334490f0e8085ca6c8dd7bfdbb17 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 20 Apr 2026 09:13:23 +0200 Subject: [PATCH 02/11] new webgui common interface for C++ exercises --- common_interfaces_cpp/CMakeLists.txt | 4 +- .../webgui/MeasuringThreadingGUI.hpp | 62 ----- .../webgui/WebGUIBridge.hpp | 81 ++++++ .../src/webgui/MeasuringThreadingGUI.cpp | 241 ------------------ .../src/webgui/WebGUIBridge.cpp | 144 +++++++++++ 5 files changed, 227 insertions(+), 305 deletions(-) delete mode 100644 common_interfaces_cpp/include/common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp create mode 100644 common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp delete mode 100644 common_interfaces_cpp/src/webgui/MeasuringThreadingGUI.cpp create mode 100644 common_interfaces_cpp/src/webgui/WebGUIBridge.cpp diff --git a/common_interfaces_cpp/CMakeLists.txt b/common_interfaces_cpp/CMakeLists.txt index e4c2c9545..c246c0aa6 100644 --- a/common_interfaces_cpp/CMakeLists.txt +++ b/common_interfaces_cpp/CMakeLists.txt @@ -43,7 +43,7 @@ add_library(${PROJECT_NAME} SHARED src/hal/odometry.cpp src/hal/sim_time.cpp # WebGUI - src/webgui/MeasuringThreadingGUI.cpp + src/webgui/WebGUIBridge.cpp ) # Specify the include directories so the compiler finds the .hpp files @@ -79,6 +79,6 @@ install(TARGETS ${PROJECT_NAME} # Export include directories, targets, and dependencies ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${ros2_dependencies}) +ament_export_dependencies(${ros2_dependencies} OpenCV nlohmann_json Boost) ament_package() \ No newline at end of file diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp deleted file mode 100644 index ff92ecb0e..000000000 --- a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp +++ /dev/null @@ -1,62 +0,0 @@ -#ifndef MEASURING_THREADING_GUI_HPP_ -#define MEASURING_THREADING_GUI_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class MeasuringThreadingGUI { -public: - MeasuringThreadingGUI(const std::string& host = "127.0.0.1", const std::string& port = "2303", double freq = 30.0, const std::string& world_name = "default"); - virtual ~MeasuringThreadingGUI(); - - void start(); - void send_to_client(const std::string& msg); - - void gui_in_thread(const std::string& message); - void trigger_update(); - void set_session(std::shared_ptr session); - -protected: - virtual void update_gui() = 0; - - std::string host_; - std::string port_; - std::string world_name_; - double out_period_; - - bool ack_; - bool ack_frontend_; - std::mutex ack_lock_; - - double ideal_cycle_; - std::atomic real_time_factor_; - std::atomic iteration_counter_; - std::atomic fps_; - std::atomic lat_; - - std::atomic running_; - -private: - void get_real_time_factor(); - void measure_and_send_frequency(); - - boost::asio::io_context ioc_; - std::thread ioc_thread_; - std::thread rtf_thread_; - std::thread freq_thread_; - - std::shared_ptr session_; - std::mutex session_mutex_; -}; - -#endif \ No newline at end of file diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp new file mode 100644 index 000000000..10fc3df13 --- /dev/null +++ b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp @@ -0,0 +1,81 @@ +#ifndef WEBUI_BRIDGE_HPP_ +#define WEBUI_BRIDGE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace beast = boost::beast; +namespace websocket = beast::websocket; +namespace net = boost::asio; +using tcp = net::ip::tcp; +using json = nlohmann::json; + +class WebSocketSession : public std::enable_shared_from_this +{ +public: + explicit WebSocketSession(net::io_context& ioc); + void run(const std::string& host, const std::string& port); + void send(const std::string& msg); + void set_message_callback(std::function cb); + +private: + void on_resolve(beast::error_code ec, tcp::resolver::results_type results); + void on_connect(beast::error_code ec, tcp::resolver::results_type::endpoint_type ep); + void on_handshake(beast::error_code ec); + void on_write(beast::error_code ec, std::size_t bytes_transferred); + void on_read(beast::error_code ec, std::size_t bytes_transferred); + + tcp::resolver resolver_; + websocket::stream ws_; + beast::flat_buffer buffer_; + std::string host_; + std::vector write_queue_; + std::mutex queue_mutex_; + bool is_writing_; + std::function message_callback_; +}; + +class BaseWebGUI : public rclcpp::Node +{ +public: + BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq); + virtual ~BaseWebGUI(); + + virtual void process_message(const std::string& msg); + virtual json update_gui() = 0; + + void send_to_client(const std::string& msg); + +protected: + double real_time_factor_; + bool ack_frontend_; + bool ack_; + std::mutex ack_lock_; + double brain_freq_; + double gui_freq_; + +private: + void performance_callback(const gazebo_msgs::msg::PerformanceMetrics::SharedPtr msg); + void gui_timer_callback(); + + net::io_context ioc_; + std::shared_ptr ws_session_; + std::thread ioc_thread_; + + rclcpp::Subscription::SharedPtr perf_sub_; + rclcpp::TimerBase::SharedPtr gui_timer_; +}; + +#endif // WEBUI_BRIDGE_HPP_ \ No newline at end of file diff --git a/common_interfaces_cpp/src/webgui/MeasuringThreadingGUI.cpp b/common_interfaces_cpp/src/webgui/MeasuringThreadingGUI.cpp deleted file mode 100644 index ff2e282a7..000000000 --- a/common_interfaces_cpp/src/webgui/MeasuringThreadingGUI.cpp +++ /dev/null @@ -1,241 +0,0 @@ -#include "common_interfaces_cpp/webgui/MeasuringThreadingGUI.hpp" -#include -#include -#include -#include -#include - -namespace beast = boost::beast; -namespace websocket = beast::websocket; -namespace net = boost::asio; -using tcp = boost::asio::ip::tcp; - -class WSSession : public std::enable_shared_from_this { - tcp::resolver resolver_; - websocket::stream ws_; - beast::flat_buffer buffer_; - MeasuringThreadingGUI* gui_; - std::string host_; - std::string port_; - net::steady_timer timer_; - std::vector write_queue_; - double out_period_; - -public: - explicit WSSession(net::io_context& ioc, MeasuringThreadingGUI* gui, const std::string& host, const std::string& port, double out_period) - : resolver_(net::make_strand(ioc)), ws_(net::make_strand(ioc)), gui_(gui), host_(host), port_(port), timer_(net::make_strand(ioc)), out_period_(out_period) {} - - void run() { - resolver_.async_resolve(host_, port_, beast::bind_front_handler(&WSSession::on_resolve, shared_from_this())); - } - - void on_resolve(beast::error_code ec, tcp::resolver::results_type results) { - if (ec) { - std::this_thread::sleep_for(std::chrono::seconds(1)); - run(); - return; - } - beast::get_lowest_layer(ws_).expires_after(std::chrono::seconds(30)); - beast::get_lowest_layer(ws_).async_connect(results, beast::bind_front_handler(&WSSession::on_connect, shared_from_this())); - } - - void on_connect(beast::error_code ec, tcp::resolver::results_type::endpoint_type ep) { - if (ec) { - std::this_thread::sleep_for(std::chrono::seconds(1)); - run(); - return; - } - beast::get_lowest_layer(ws_).expires_never(); - ws_.set_option(websocket::stream_base::timeout::suggested(beast::role_type::client)); - ws_.read_message_max(1024 * 1024 * 10); - ws_.auto_fragment(false); - std::string h = host_ + ":" + std::to_string(ep.port()); - ws_.async_handshake(h, "/", beast::bind_front_handler(&WSSession::on_handshake, shared_from_this())); - } - - void on_handshake(beast::error_code ec) { - if (ec) { - std::this_thread::sleep_for(std::chrono::seconds(1)); - run(); - return; - } - gui_->set_session(shared_from_this()); - do_read(); - - timer_.expires_after(std::chrono::milliseconds(static_cast(out_period_ * 1000))); - timer_.async_wait(beast::bind_front_handler(&WSSession::on_timer, shared_from_this())); - } - - void do_read() { - ws_.async_read(buffer_, beast::bind_front_handler(&WSSession::on_read, shared_from_this())); - } - - void on_read(beast::error_code ec, std::size_t) { - if (ec) { - gui_->set_session(nullptr); - std::this_thread::sleep_for(std::chrono::seconds(1)); - run(); - return; - } - std::string msg = beast::buffers_to_string(buffer_.data()); - buffer_.consume(buffer_.size()); - gui_->gui_in_thread(msg); - do_read(); - } - - void on_timer(beast::error_code ec) { - if (ec) return; - - gui_->trigger_update(); - - timer_.expires_after(std::chrono::milliseconds(static_cast(out_period_ * 1000))); - timer_.async_wait(beast::bind_front_handler(&WSSession::on_timer, shared_from_this())); - } - - void send(const std::string& msg) { - net::post(ws_.get_executor(), beast::bind_front_handler(&WSSession::do_send, shared_from_this(), msg)); - } - - void do_send(const std::string& msg) { - bool writing = !write_queue_.empty(); - write_queue_.push_back(msg); - if (!writing) { - ws_.async_write(net::buffer(write_queue_.front()), beast::bind_front_handler(&WSSession::on_write, shared_from_this())); - } - } - - void on_write(beast::error_code ec, std::size_t) { - if (ec) { - gui_->set_session(nullptr); - std::this_thread::sleep_for(std::chrono::seconds(1)); - run(); - return; - } - write_queue_.erase(write_queue_.begin()); - if (!write_queue_.empty()) { - ws_.async_write(net::buffer(write_queue_.front()), beast::bind_front_handler(&WSSession::on_write, shared_from_this())); - } - } - - void close() { - if (ws_.is_open()) { - beast::error_code ec; - ws_.close(websocket::close_code::normal, ec); - } - } -}; - -MeasuringThreadingGUI::MeasuringThreadingGUI(const std::string& host, const std::string& port, double freq, const std::string& world_name) - : host_(host), port_(port), world_name_(world_name), out_period_(1.0 / freq), - ack_(true), ack_frontend_(false), ideal_cycle_(80.0), real_time_factor_(0.0), - iteration_counter_(0), fps_(-1), lat_(-1), running_(true), session_(nullptr) {} - -MeasuringThreadingGUI::~MeasuringThreadingGUI() { - running_ = false; - - if (session_) { - std::static_pointer_cast(session_)->close(); - } - ioc_.stop(); - - if (ioc_thread_.joinable()) ioc_thread_.join(); - if (rtf_thread_.joinable()) rtf_thread_.join(); - if (freq_thread_.joinable()) freq_thread_.join(); -} - -void MeasuringThreadingGUI::start() { - rtf_thread_ = std::thread(&MeasuringThreadingGUI::get_real_time_factor, this); - freq_thread_ = std::thread(&MeasuringThreadingGUI::measure_and_send_frequency, this); - - auto session = std::make_shared(ioc_, this, host_, port_, out_period_); - session->run(); - ioc_thread_ = std::thread([this]() { ioc_.run(); }); -} - -void MeasuringThreadingGUI::set_session(std::shared_ptr session) { - std::lock_guard lock(session_mutex_); - session_ = session; -} - -void MeasuringThreadingGUI::gui_in_thread(const std::string& message) { - std::lock_guard lock(ack_lock_); - if (message.find("ack") != std::string::npos) { - ack_ = true; - } else if (message.find("start") != std::string::npos) { - ack_frontend_ = true; - } -} - -void MeasuringThreadingGUI::trigger_update() { - iteration_counter_++; - bool should_update = false; - { - std::lock_guard lock(ack_lock_); - if (ack_frontend_ && ack_) { - should_update = true; - ack_ = false; - } - } - - if (should_update) { - update_gui(); - } -} - -void MeasuringThreadingGUI::send_to_client(const std::string& msg) { - std::lock_guard lock(session_mutex_); - if (session_) { - std::static_pointer_cast(session_)->send(msg); - } -} - -void MeasuringThreadingGUI::get_real_time_factor() { - while (running_) { - std::this_thread::sleep_for(std::chrono::seconds(2)); - std::string cmd = "gz topic -e -t /world/" + world_name_ + "/stats --num 1 2>/dev/null"; - FILE* pipe = popen(cmd.c_str(), "r"); - if (!pipe) continue; - - char buffer[128]; - std::string result; - while (fgets(buffer, sizeof(buffer), pipe) != nullptr) { - result += buffer; - } - pclose(pipe); - - std::regex r("real_time_factor:\\s*([0-9\\.]+)"); - std::smatch match; - if (std::regex_search(result, match, r)) { - double rtf = std::stod(match[1].str()); - real_time_factor_ = std::round(rtf * 1000.0) / 1000.0; - } - } -} - -void MeasuringThreadingGUI::measure_and_send_frequency() { - auto previous_time = std::chrono::steady_clock::now(); - - while (running_) { - std::this_thread::sleep_for(std::chrono::seconds(2)); - auto current_time = std::chrono::steady_clock::now(); - std::chrono::duration dt = current_time - previous_time; - previous_time = current_time; - - int iters = iteration_counter_.exchange(0); - double measured_cycle = (iters > 0) ? (dt.count() / iters) : 0.0; - double brain_frequency = (measured_cycle != 0.0) ? (1000.0 / measured_cycle) : 0.0; - brain_frequency = std::round(brain_frequency * 10.0) / 10.0; - - double gui_frequency = std::round((1000.0 / ideal_cycle_) * 10.0) / 10.0; - - nlohmann::json freq_msg = { - {"brain", brain_frequency}, - {"gui", gui_frequency}, - {"rtf", real_time_factor_.load()}, - {"fps", fps_.load()}, - {"lat", lat_.load()} - }; - - send_to_client(freq_msg.dump()); - } -} \ No newline at end of file diff --git a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp new file mode 100644 index 000000000..a32573ad2 --- /dev/null +++ b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp @@ -0,0 +1,144 @@ +#include "common_interfaces_cpp/webgui/WebGUIBridge.hpp" + +WebSocketSession::WebSocketSession(net::io_context& ioc) + : resolver_(net::make_strand(ioc)), ws_(net::make_strand(ioc)), is_writing_(false) {} + +void WebSocketSession::run(const std::string& host, const std::string& port) +{ + host_ = host; + buffer_.max_size(1024 * 1024 * 10); + resolver_.async_resolve(host, port, beast::bind_front_handler(&WebSocketSession::on_resolve, shared_from_this())); +} + +void WebSocketSession::set_message_callback(std::function cb) +{ + message_callback_ = cb; +} + +void WebSocketSession::send(const std::string& msg) +{ + std::lock_guard lock(queue_mutex_); + write_queue_.push_back(msg); + if (!is_writing_) { + is_writing_ = true; + ws_.async_write(net::buffer(write_queue_.front()), beast::bind_front_handler(&WebSocketSession::on_write, shared_from_this())); + } +} + +void WebSocketSession::on_resolve(beast::error_code ec, tcp::resolver::results_type results) +{ + if (ec) return; + beast::get_lowest_layer(ws_).expires_after(std::chrono::seconds(30)); + beast::get_lowest_layer(ws_).async_connect(results, beast::bind_front_handler(&WebSocketSession::on_connect, shared_from_this())); +} + +void WebSocketSession::on_connect(beast::error_code ec, tcp::resolver::results_type::endpoint_type ep) +{ + if (ec) return; + beast::get_lowest_layer(ws_).expires_never(); + ws_.set_option(websocket::stream_base::timeout::suggested(beast::role_type::client)); + ws_.read_message_max(1024 * 1024 * 10); + ws_.auto_fragment(false); + host_ += ':' + std::to_string(ep.port()); + ws_.async_handshake(host_, "/", beast::bind_front_handler(&WebSocketSession::on_handshake, shared_from_this())); +} + +void WebSocketSession::on_handshake(beast::error_code ec) +{ + if (ec) return; + ws_.async_read(buffer_, beast::bind_front_handler(&WebSocketSession::on_read, shared_from_this())); +} + +void WebSocketSession::on_write(beast::error_code ec, std::size_t) +{ + std::lock_guard lock(queue_mutex_); + write_queue_.erase(write_queue_.begin()); + if (ec) { + is_writing_ = false; + return; + } + if (!write_queue_.empty()) { + ws_.async_write(net::buffer(write_queue_.front()), beast::bind_front_handler(&WebSocketSession::on_write, shared_from_this())); + } else { + is_writing_ = false; + } +} + +void WebSocketSession::on_read(beast::error_code ec, std::size_t) +{ + if (ec) return; + std::string msg = beast::buffers_to_string(buffer_.data()); + buffer_.consume(buffer_.size()); + + if (message_callback_) { + message_callback_(msg); + } + + ws_.async_read(buffer_, beast::bind_front_handler(&WebSocketSession::on_read, shared_from_this())); +} + +BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq) + : Node(node_name), real_time_factor_(0.0), ack_frontend_(false), ack_(true), brain_freq_(0.0), gui_freq_(freq) +{ + ws_session_ = std::make_shared(ioc_); + ws_session_->set_message_callback(std::bind(&BaseWebGUI::process_message, this, std::placeholders::_1)); + ws_session_->run(host, port); + + ioc_thread_ = std::thread([this]() { ioc_.run(); }); + + perf_sub_ = this->create_subscription( + "/performance_metrics", 10, std::bind(&BaseWebGUI::performance_callback, this, std::placeholders::_1)); + + auto period = std::chrono::duration(1.0 / freq); + gui_timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&BaseWebGUI::gui_timer_callback, this)); +} + +BaseWebGUI::~BaseWebGUI() +{ + ioc_.stop(); + if (ioc_thread_.joinable()) { + ioc_thread_.join(); + } +} + +void BaseWebGUI::process_message(const std::string& msg) +{ + if (msg.find("ack") != std::string::npos) { + std::lock_guard lock(ack_lock_); + ack_ = true; + } else if (msg.find("start") != std::string::npos) { + std::lock_guard lock(ack_lock_); + ack_frontend_ = true; + } +} + +void BaseWebGUI::send_to_client(const std::string& msg) +{ + if (ws_session_) { + ws_session_->send(msg); + } +} + +void BaseWebGUI::performance_callback(const gazebo_msgs::msg::PerformanceMetrics::SharedPtr msg) +{ + real_time_factor_ = msg->real_time_factor; +} + +void BaseWebGUI::gui_timer_callback() +{ + std::lock_guard lock(ack_lock_); + if (ack_frontend_ && ack_) { + json payload = update_gui(); + + payload["rtf"] = real_time_factor_; + payload["brain"] = brain_freq_; + payload["gui"] = gui_freq_; + payload["fps"] = -1.0; + payload["lat"] = -1.0; + + send_to_client(payload.dump()); + ack_ = false; + } +} \ No newline at end of file From 3305b224a98d615d70899c6a8addec7dd4bd8453 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 20 Apr 2026 10:41:40 +0200 Subject: [PATCH 03/11] add virtual method for get nodes --- .../include/common_interfaces_cpp/webgui/WebGUIBridge.hpp | 1 + common_interfaces_cpp/src/webgui/WebGUIBridge.cpp | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp index 10fc3df13..47b000aa2 100644 --- a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp +++ b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp @@ -55,6 +55,7 @@ class BaseWebGUI : public rclcpp::Node virtual void process_message(const std::string& msg); virtual json update_gui() = 0; + virtual std::vector get_nodes(); void send_to_client(const std::string& msg); diff --git a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp index a32573ad2..b33ce8e62 100644 --- a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp +++ b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp @@ -114,6 +114,11 @@ void BaseWebGUI::process_message(const std::string& msg) } } +std::vector BaseWebGUI::get_nodes() +{ + return {shared_from_this()}; +} + void BaseWebGUI::send_to_client(const std::string& msg) { if (ws_session_) { From 4047847ec8c910ddbdc7e44c595da117eea5a5cf Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 20 Apr 2026 12:21:58 +0200 Subject: [PATCH 04/11] performance metrics --- common_interfaces_cpp/src/webgui/WebGUIBridge.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp index b33ce8e62..da36ed2c4 100644 --- a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp +++ b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp @@ -81,13 +81,20 @@ BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, co : Node(node_name), real_time_factor_(0.0), ack_frontend_(false), ack_(true), brain_freq_(0.0), gui_freq_(freq) { ws_session_ = std::make_shared(ioc_); - ws_session_->set_message_callback(std::bind(&BaseWebGUI::process_message, this, std::placeholders::_1)); + + ws_session_->set_message_callback([this](const std::string& msg) { + this->process_message(msg); + }); + ws_session_->run(host, port); ioc_thread_ = std::thread([this]() { ioc_.run(); }); perf_sub_ = this->create_subscription( - "/performance_metrics", 10, std::bind(&BaseWebGUI::performance_callback, this, std::placeholders::_1)); + "/performance_metrics", + rclcpp::SensorDataQoS(), + std::bind(&BaseWebGUI::performance_callback, this, std::placeholders::_1) + ); auto period = std::chrono::duration(1.0 / freq); gui_timer_ = this->create_wall_timer( From 104a318f59a6ec048c989c9121c92d3cb9390b55 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 20 Apr 2026 13:40:52 +0200 Subject: [PATCH 05/11] use gz api to get RTF --- common_interfaces_cpp/CMakeLists.txt | 29 ++++++++++--------- .../webgui/WebGUIBridge.hpp | 9 +++--- .../src/webgui/WebGUIBridge.cpp | 18 +++++------- 3 files changed, 28 insertions(+), 28 deletions(-) diff --git a/common_interfaces_cpp/CMakeLists.txt b/common_interfaces_cpp/CMakeLists.txt index c246c0aa6..8c35ded93 100644 --- a/common_interfaces_cpp/CMakeLists.txt +++ b/common_interfaces_cpp/CMakeLists.txt @@ -5,7 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Define a list of ROS 2 dependencies for easy modularity set(ros2_dependencies rclcpp nav_msgs @@ -19,21 +18,17 @@ set(ros2_dependencies find_package(ament_cmake REQUIRED) -# Loop through the list to find all specified ROS 2 packages automatically foreach(dependency IN LISTS ros2_dependencies) find_package(${dependency} REQUIRED) endforeach() -# Find OpenCV explicitly (Required by Camera and Classnet for cv::Mat and cv::dnn) find_package(OpenCV REQUIRED) - -# Find external C++ libraries for the WebGUI find_package(Boost REQUIRED COMPONENTS system thread) find_package(nlohmann_json REQUIRED) +find_package(gz-transport13 REQUIRED) +find_package(gz-msgs10 REQUIRED) -# Create the shared library from the source files matching the new structure add_library(${PROJECT_NAME} SHARED - # HAL src/hal/bumper.cpp src/hal/camera.cpp src/hal/classnet.cpp @@ -42,33 +37,33 @@ add_library(${PROJECT_NAME} SHARED src/hal/motors.cpp src/hal/odometry.cpp src/hal/sim_time.cpp - # WebGUI src/webgui/WebGUIBridge.cpp ) -# Specify the include directories so the compiler finds the .hpp files target_include_directories(${PROJECT_NAME} PUBLIC $ $ ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} + ${GZ-TRANSPORT_INCLUDE_DIRS} + ${GZ-MSGS_INCLUDE_DIRS} ) -# Link all listed ROS 2 dependencies and OpenCV to the library ament_target_dependencies(${PROJECT_NAME} ${ros2_dependencies}) + target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} Boost::system Boost::thread nlohmann_json::nlohmann_json + gz-transport13::core + gz-msgs10::gz-msgs10 ) -# Install the include directory so other packages can do #include install(DIRECTORY include/ DESTINATION include ) -# Install the library and export the target for downstream usage install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib @@ -76,9 +71,15 @@ install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin ) -# Export include directories, targets, and dependencies ament_export_include_directories(include) ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${ros2_dependencies} OpenCV nlohmann_json Boost) +ament_export_dependencies( + ${ros2_dependencies} + OpenCV + nlohmann_json + Boost + gz-transport13 + gz-msgs10 +) ament_package() \ No newline at end of file diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp index 47b000aa2..c78fa1718 100644 --- a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp +++ b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp @@ -8,7 +8,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -68,15 +69,15 @@ class BaseWebGUI : public rclcpp::Node double gui_freq_; private: - void performance_callback(const gazebo_msgs::msg::PerformanceMetrics::SharedPtr msg); void gui_timer_callback(); + void gz_stats_callback(const gz::msgs::WorldStatistics &msg); net::io_context ioc_; std::shared_ptr ws_session_; std::thread ioc_thread_; - rclcpp::Subscription::SharedPtr perf_sub_; + gz::transport::Node gz_node_; rclcpp::TimerBase::SharedPtr gui_timer_; }; -#endif // WEBUI_BRIDGE_HPP_ \ No newline at end of file +#endif \ No newline at end of file diff --git a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp index da36ed2c4..f9ba8bb7e 100644 --- a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp +++ b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp @@ -90,11 +90,7 @@ BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, co ioc_thread_ = std::thread([this]() { ioc_.run(); }); - perf_sub_ = this->create_subscription( - "/performance_metrics", - rclcpp::SensorDataQoS(), - std::bind(&BaseWebGUI::performance_callback, this, std::placeholders::_1) - ); + gz_node_.subscribe("/world/default/stats", &BaseWebGUI::gz_stats_callback, this); auto period = std::chrono::duration(1.0 / freq); gui_timer_ = this->create_wall_timer( @@ -102,6 +98,13 @@ BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, co std::bind(&BaseWebGUI::gui_timer_callback, this)); } +void BaseWebGUI::gz_stats_callback(const gz::msgs::WorldStatistics &msg) +{ + if (msg.has_real_time_factor()) { + real_time_factor_ = msg.real_time_factor(); + } +} + BaseWebGUI::~BaseWebGUI() { ioc_.stop(); @@ -133,11 +136,6 @@ void BaseWebGUI::send_to_client(const std::string& msg) } } -void BaseWebGUI::performance_callback(const gazebo_msgs::msg::PerformanceMetrics::SharedPtr msg) -{ - real_time_factor_ = msg->real_time_factor; -} - void BaseWebGUI::gui_timer_callback() { std::lock_guard lock(ack_lock_); From 53991b74ce669f48c745e8cc1f96b350a5f356a4 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 20 Apr 2026 13:45:36 +0200 Subject: [PATCH 06/11] update package.xml for gazebo deps --- common_interfaces_cpp/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/common_interfaces_cpp/package.xml b/common_interfaces_cpp/package.xml index ecdf9eda5..2798f2d02 100644 --- a/common_interfaces_cpp/package.xml +++ b/common_interfaces_cpp/package.xml @@ -12,7 +12,6 @@ rclcpp cv_bridge opencv - boost nlohmann-json-dev @@ -23,6 +22,9 @@ rosgraph_msgs ros_gz_interfaces + gz-transport13 + gz-msgs10 + ament_cmake From 20125bde2137b4bdd43bc88d5f76b925f3028af5 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 20 Apr 2026 13:51:16 +0200 Subject: [PATCH 07/11] link gz libraries --- common_interfaces_cpp/CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/common_interfaces_cpp/CMakeLists.txt b/common_interfaces_cpp/CMakeLists.txt index 8c35ded93..64e3067fc 100644 --- a/common_interfaces_cpp/CMakeLists.txt +++ b/common_interfaces_cpp/CMakeLists.txt @@ -25,6 +25,7 @@ endforeach() find_package(OpenCV REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread) find_package(nlohmann_json REQUIRED) + find_package(gz-transport13 REQUIRED) find_package(gz-msgs10 REQUIRED) @@ -45,8 +46,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} - ${GZ-TRANSPORT_INCLUDE_DIRS} - ${GZ-MSGS_INCLUDE_DIRS} ) ament_target_dependencies(${PROJECT_NAME} ${ros2_dependencies}) @@ -60,9 +59,7 @@ target_link_libraries(${PROJECT_NAME} gz-msgs10::gz-msgs10 ) -install(DIRECTORY include/ - DESTINATION include -) +install(DIRECTORY include/ DESTINATION include) install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} From ac233abf05816ae031675e100aab084f50b62bdb Mon Sep 17 00:00:00 2001 From: aquintan Date: Tue, 21 Apr 2026 08:24:56 +0200 Subject: [PATCH 08/11] remove gazebo api dependencies, methods and variables. RTF will be calculated via command execution and parsing in a thread --- common_interfaces_cpp/CMakeLists.txt | 9 +----- .../webgui/WebGUIBridge.hpp | 9 +++--- common_interfaces_cpp/package.xml | 3 -- .../src/webgui/WebGUIBridge.cpp | 29 +++++++++++++------ 4 files changed, 26 insertions(+), 24 deletions(-) diff --git a/common_interfaces_cpp/CMakeLists.txt b/common_interfaces_cpp/CMakeLists.txt index 64e3067fc..eb77fb199 100644 --- a/common_interfaces_cpp/CMakeLists.txt +++ b/common_interfaces_cpp/CMakeLists.txt @@ -26,9 +26,6 @@ find_package(OpenCV REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread) find_package(nlohmann_json REQUIRED) -find_package(gz-transport13 REQUIRED) -find_package(gz-msgs10 REQUIRED) - add_library(${PROJECT_NAME} SHARED src/hal/bumper.cpp src/hal/camera.cpp @@ -55,8 +52,6 @@ target_link_libraries(${PROJECT_NAME} Boost::system Boost::thread nlohmann_json::nlohmann_json - gz-transport13::core - gz-msgs10::gz-msgs10 ) install(DIRECTORY include/ DESTINATION include) @@ -74,9 +69,7 @@ ament_export_dependencies( ${ros2_dependencies} OpenCV nlohmann_json - Boost - gz-transport13 - gz-msgs10 + Boost ) ament_package() \ No newline at end of file diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp index c78fa1718..e95882b52 100644 --- a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp +++ b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp @@ -8,14 +8,14 @@ #include #include #include -#include -#include #include #include #include #include #include #include +#include +#include namespace beast = boost::beast; namespace websocket = beast::websocket; @@ -70,13 +70,14 @@ class BaseWebGUI : public rclcpp::Node private: void gui_timer_callback(); - void gz_stats_callback(const gz::msgs::WorldStatistics &msg); + void rtf_worker(); net::io_context ioc_; std::shared_ptr ws_session_; std::thread ioc_thread_; + std::thread rtf_thread_; + bool running_; - gz::transport::Node gz_node_; rclcpp::TimerBase::SharedPtr gui_timer_; }; diff --git a/common_interfaces_cpp/package.xml b/common_interfaces_cpp/package.xml index 2798f2d02..6624743e3 100644 --- a/common_interfaces_cpp/package.xml +++ b/common_interfaces_cpp/package.xml @@ -22,9 +22,6 @@ rosgraph_msgs ros_gz_interfaces - gz-transport13 - gz-msgs10 - ament_cmake diff --git a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp index f9ba8bb7e..6dfc16d89 100644 --- a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp +++ b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp @@ -78,7 +78,7 @@ void WebSocketSession::on_read(beast::error_code ec, std::size_t) } BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq) - : Node(node_name), real_time_factor_(0.0), ack_frontend_(false), ack_(true), brain_freq_(0.0), gui_freq_(freq) + : Node(node_name), real_time_factor_(0.0), ack_frontend_(false), ack_(true), brain_freq_(0.0), gui_freq_(freq), running_(true) { ws_session_ = std::make_shared(ioc_); @@ -89,8 +89,7 @@ BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, co ws_session_->run(host, port); ioc_thread_ = std::thread([this]() { ioc_.run(); }); - - gz_node_.subscribe("/world/default/stats", &BaseWebGUI::gz_stats_callback, this); + rtf_thread_ = std::thread(&BaseWebGUI::rtf_worker, this); auto period = std::chrono::duration(1.0 / freq); gui_timer_ = this->create_wall_timer( @@ -98,19 +97,31 @@ BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, co std::bind(&BaseWebGUI::gui_timer_callback, this)); } -void BaseWebGUI::gz_stats_callback(const gz::msgs::WorldStatistics &msg) +void BaseWebGUI::rtf_worker() { - if (msg.has_real_time_factor()) { - real_time_factor_ = msg.real_time_factor(); + FILE* pipe = popen("gz topic -e -t /world/default/stats", "r"); + if (!pipe) return; + + char buffer[512]; + while (running_ && fgets(buffer, sizeof(buffer), pipe)) { + std::string line(buffer); + size_t pos = line.find("real_time_factor:"); + if (pos != std::string::npos) { + try { + std::string val_str = line.substr(pos + 17); + real_time_factor_ = std::stod(val_str); + } catch (...) {} + } } + pclose(pipe); } BaseWebGUI::~BaseWebGUI() { + running_ = false; ioc_.stop(); - if (ioc_thread_.joinable()) { - ioc_thread_.join(); - } + if (ioc_thread_.joinable()) ioc_thread_.join(); + if (rtf_thread_.joinable()) rtf_thread_.join(); } void BaseWebGUI::process_message(const std::string& msg) From 15696c05954b8e06f7f739cc07ad3848b31231b9 Mon Sep 17 00:00:00 2001 From: aquintan Date: Tue, 21 Apr 2026 08:29:04 +0200 Subject: [PATCH 09/11] add stats topics to allow changing the topic name for getting the RTF --- .../include/common_interfaces_cpp/webgui/WebGUIBridge.hpp | 4 ++-- common_interfaces_cpp/src/webgui/WebGUIBridge.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp index e95882b52..232b68024 100644 --- a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp +++ b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp @@ -15,7 +15,6 @@ #include #include #include -#include namespace beast = boost::beast; namespace websocket = beast::websocket; @@ -51,7 +50,7 @@ class WebSocketSession : public std::enable_shared_from_this class BaseWebGUI : public rclcpp::Node { public: - BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq); + BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq, const std::string& stats_topic = "/world/default/stats"); virtual ~BaseWebGUI(); virtual void process_message(const std::string& msg); @@ -67,6 +66,7 @@ class BaseWebGUI : public rclcpp::Node std::mutex ack_lock_; double brain_freq_; double gui_freq_; + std::string stats_topic_; private: void gui_timer_callback(); diff --git a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp index 6dfc16d89..2551085ff 100644 --- a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp +++ b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp @@ -77,8 +77,8 @@ void WebSocketSession::on_read(beast::error_code ec, std::size_t) ws_.async_read(buffer_, beast::bind_front_handler(&WebSocketSession::on_read, shared_from_this())); } -BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq) - : Node(node_name), real_time_factor_(0.0), ack_frontend_(false), ack_(true), brain_freq_(0.0), gui_freq_(freq), running_(true) +BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq, const std::string& stats_topic) + : Node(node_name), real_time_factor_(0.0), ack_frontend_(false), ack_(true), brain_freq_(0.0), gui_freq_(freq), stats_topic_(stats_topic), running_(true) { ws_session_ = std::make_shared(ioc_); @@ -99,7 +99,8 @@ BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, co void BaseWebGUI::rtf_worker() { - FILE* pipe = popen("gz topic -e -t /world/default/stats", "r"); + std::string command = "gz topic -e -t " + stats_topic_; + FILE* pipe = popen(command.c_str(), "r"); if (!pipe) return; char buffer[512]; From 222f84724192a580cf08a885bdd456cb964003f7 Mon Sep 17 00:00:00 2001 From: aquintan Date: Tue, 21 Apr 2026 09:08:56 +0200 Subject: [PATCH 10/11] get rtf --- .../webgui/WebGUIBridge.hpp | 12 ++- .../src/webgui/WebGUIBridge.cpp | 81 +++++++++++-------- 2 files changed, 57 insertions(+), 36 deletions(-) diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp index 232b68024..6567d5888 100644 --- a/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp +++ b/common_interfaces_cpp/include/common_interfaces_cpp/webgui/WebGUIBridge.hpp @@ -15,6 +15,8 @@ #include #include #include +#include +#include namespace beast = boost::beast; namespace websocket = beast::websocket; @@ -50,7 +52,7 @@ class WebSocketSession : public std::enable_shared_from_this class BaseWebGUI : public rclcpp::Node { public: - BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq, const std::string& stats_topic = "/world/default/stats"); + BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq, const std::string& stats_topic = "/stats"); virtual ~BaseWebGUI(); virtual void process_message(const std::string& msg); @@ -70,15 +72,17 @@ class BaseWebGUI : public rclcpp::Node private: void gui_timer_callback(); - void rtf_worker(); + void stats_timer_callback(); net::io_context ioc_; std::shared_ptr ws_session_; std::thread ioc_thread_; - std::thread rtf_thread_; - bool running_; rclcpp::TimerBase::SharedPtr gui_timer_; + rclcpp::TimerBase::SharedPtr stats_timer_; + + std::atomic iteration_counter_; + std::chrono::time_point last_freq_update_; }; #endif \ No newline at end of file diff --git a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp index 2551085ff..4ce42ef4f 100644 --- a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp +++ b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp @@ -1,4 +1,5 @@ #include "common_interfaces_cpp/webgui/WebGUIBridge.hpp" +#include WebSocketSession::WebSocketSession(net::io_context& ioc) : resolver_(net::make_strand(ioc)), ws_(net::make_strand(ioc)), is_writing_(false) {} @@ -78,51 +79,32 @@ void WebSocketSession::on_read(beast::error_code ec, std::size_t) } BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, const std::string& port, double freq, const std::string& stats_topic) - : Node(node_name), real_time_factor_(0.0), ack_frontend_(false), ack_(true), brain_freq_(0.0), gui_freq_(freq), stats_topic_(stats_topic), running_(true) + : Node(node_name), real_time_factor_(0.0), ack_frontend_(false), ack_(true), brain_freq_(0.0), gui_freq_(freq), stats_topic_(stats_topic), iteration_counter_(0) { ws_session_ = std::make_shared(ioc_); - ws_session_->set_message_callback([this](const std::string& msg) { this->process_message(msg); }); - ws_session_->run(host, port); ioc_thread_ = std::thread([this]() { ioc_.run(); }); - rtf_thread_ = std::thread(&BaseWebGUI::rtf_worker, this); + + last_freq_update_ = std::chrono::steady_clock::now(); auto period = std::chrono::duration(1.0 / freq); gui_timer_ = this->create_wall_timer( std::chrono::duration_cast(period), std::bind(&BaseWebGUI::gui_timer_callback, this)); -} -void BaseWebGUI::rtf_worker() -{ - std::string command = "gz topic -e -t " + stats_topic_; - FILE* pipe = popen(command.c_str(), "r"); - if (!pipe) return; - - char buffer[512]; - while (running_ && fgets(buffer, sizeof(buffer), pipe)) { - std::string line(buffer); - size_t pos = line.find("real_time_factor:"); - if (pos != std::string::npos) { - try { - std::string val_str = line.substr(pos + 17); - real_time_factor_ = std::stod(val_str); - } catch (...) {} - } - } - pclose(pipe); + stats_timer_ = this->create_wall_timer( + std::chrono::seconds(2), + std::bind(&BaseWebGUI::stats_timer_callback, this)); } BaseWebGUI::~BaseWebGUI() { - running_ = false; ioc_.stop(); if (ioc_thread_.joinable()) ioc_thread_.join(); - if (rtf_thread_.joinable()) rtf_thread_.join(); } void BaseWebGUI::process_message(const std::string& msg) @@ -148,18 +130,53 @@ void BaseWebGUI::send_to_client(const std::string& msg) } } +void BaseWebGUI::stats_timer_callback() +{ + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - last_freq_update_).count(); + + if (elapsed > 0) { + brain_freq_ = static_cast(iteration_counter_) / elapsed; + } + iteration_counter_ = 0; + last_freq_update_ = now; + + std::string cmd = "gz topic -e -t " + stats_topic_ + " -n 1"; + FILE* pipe = popen(cmd.c_str(), "r"); + if (pipe) { + char buffer[1024]; + while (fgets(buffer, sizeof(buffer), pipe)) { + std::string line(buffer); + if (line.find("real_time_factor:") != std::string::npos) { + try { + size_t start = line.find(":") + 1; + std::string val = line.substr(start); + val.erase(std::remove(val.begin(), val.end(), ' '), val.end()); + val.erase(std::remove(val.begin(), val.end(), '\n'), val.end()); + real_time_factor_ = std::stod(val); + } catch (...) {} + break; + } + } + pclose(pipe); + } + + json payload; + payload["brain"] = std::round(brain_freq_ * 10.0) / 10.0; + payload["gui"] = gui_freq_; + payload["rtf"] = real_time_factor_; + payload["fps"] = -1.0; + payload["lat"] = -1.0; + + send_to_client(payload.dump()); +} + void BaseWebGUI::gui_timer_callback() { + iteration_counter_++; std::lock_guard lock(ack_lock_); if (ack_frontend_ && ack_) { json payload = update_gui(); - - payload["rtf"] = real_time_factor_; - payload["brain"] = brain_freq_; - payload["gui"] = gui_freq_; - payload["fps"] = -1.0; - payload["lat"] = -1.0; - send_to_client(payload.dump()); ack_ = false; } From 5017e88e088667d2aee5b123668b51a490d04a47 Mon Sep 17 00:00:00 2001 From: aquintan Date: Tue, 21 Apr 2026 09:51:25 +0200 Subject: [PATCH 11/11] update rtf often + simplify decimals --- common_interfaces_cpp/src/webgui/WebGUIBridge.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp index 4ce42ef4f..efdf5e9dc 100644 --- a/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp +++ b/common_interfaces_cpp/src/webgui/WebGUIBridge.cpp @@ -97,7 +97,7 @@ BaseWebGUI::BaseWebGUI(const std::string& node_name, const std::string& host, co std::bind(&BaseWebGUI::gui_timer_callback, this)); stats_timer_ = this->create_wall_timer( - std::chrono::seconds(2), + std::chrono::milliseconds(250), std::bind(&BaseWebGUI::stats_timer_callback, this)); } @@ -133,10 +133,10 @@ void BaseWebGUI::send_to_client(const std::string& msg) void BaseWebGUI::stats_timer_callback() { auto now = std::chrono::steady_clock::now(); - auto elapsed = std::chrono::duration_cast(now - last_freq_update_).count(); + std::chrono::duration elapsed = now - last_freq_update_; - if (elapsed > 0) { - brain_freq_ = static_cast(iteration_counter_) / elapsed; + if (elapsed.count() > 0.0) { + brain_freq_ = static_cast(iteration_counter_) / elapsed.count(); } iteration_counter_ = 0; last_freq_update_ = now; @@ -153,7 +153,9 @@ void BaseWebGUI::stats_timer_callback() std::string val = line.substr(start); val.erase(std::remove(val.begin(), val.end(), ' '), val.end()); val.erase(std::remove(val.begin(), val.end(), '\n'), val.end()); - real_time_factor_ = std::stod(val); + + double raw_val = std::stod(val); + real_time_factor_ = std::round(raw_val * 100.0) / 100.0; } catch (...) {} break; }