From a1a33c2889fb45b8043d29b1e87afde9379c780a Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Sun, 19 Oct 2025 21:51:10 -0400 Subject: [PATCH 01/21] started work --- .../include/rj_utils/latency_benchmarking.hpp | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 src/rj_utils/include/rj_utils/latency_benchmarking.hpp diff --git a/src/rj_utils/include/rj_utils/latency_benchmarking.hpp b/src/rj_utils/include/rj_utils/latency_benchmarking.hpp new file mode 100644 index 0000000000..5c99f65520 --- /dev/null +++ b/src/rj_utils/include/rj_utils/latency_benchmarking.hpp @@ -0,0 +1,47 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace latency_benchmarking { + +class Registry { +public: + // Singleton Pattern + static Registry& instance() { + static Registry R; + return R; + } + + void record(const char* label, uint64_t ns) { + + } + + void dump() { + + } + + void clear() {} + +private: + // Constructor and creates callback that will dump latency statistics at program end + Registry() { + std::atexit([] {Registry::instance.dump();}); + } + + const time_t now = std::time(nullptr); + std::tm* localTime = std::localtime(&now); + std::string path_ = "../log/latency_" + localTime->tm_year + 1900 + "_" + localTime->tm_mon + 1 + "_" + localTime->tm_mday + + +} + + +} // namespace latency_benchmarking \ No newline at end of file From e04d73e3f8ed08a74a66e8230c04f0692dfc936d Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 21 Oct 2025 21:51:56 -0400 Subject: [PATCH 02/21] theoretically finished benchmarking --- install/setup.bash | 2 +- install/setup.zsh | 2 +- .../include/rj_utils/latency_benchmarking.hpp | 58 +++++++++++++------ 3 files changed, 42 insertions(+), 20 deletions(-) diff --git a/install/setup.bash b/install/setup.bash index da36e7c200..10ea0f7c07 100644 --- a/install/setup.bash +++ b/install/setup.bash @@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null _colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_bash_source_script \ No newline at end of file +unset _colcon_prefix_chain_bash_source_script diff --git a/install/setup.zsh b/install/setup.zsh index a1ba46044b..54799fde6f 100644 --- a/install/setup.zsh +++ b/install/setup.zsh @@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && p _colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_zsh_source_script \ No newline at end of file +unset _colcon_prefix_chain_zsh_source_script diff --git a/src/rj_utils/include/rj_utils/latency_benchmarking.hpp b/src/rj_utils/include/rj_utils/latency_benchmarking.hpp index 5c99f65520..b5eee13853 100644 --- a/src/rj_utils/include/rj_utils/latency_benchmarking.hpp +++ b/src/rj_utils/include/rj_utils/latency_benchmarking.hpp @@ -1,14 +1,12 @@ -#pragma once -#include -#include -#include -#include -#include -#include #include #include -#include #include +#include +#include +#include +#include + +#include namespace latency_benchmarking { @@ -20,28 +18,52 @@ class Registry { return R; } - void record(const char* label, uint64_t ns) { - + void record(std::string label, uint64_t time) { + registry_[label].push_back(time); } void dump() { - + std::ofstream output_file; + output_file.open(path_); + for (auto& p : registry_) { + output_file << p.first << " "; + for (uint64_t e : p.second) { + output_file << e << " "; + } + output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) / p.second.size(); + output_file << "\n"; + } + output_file.close(); } - void clear() {} + void clear() { + registry_.clear(); + } private: // Constructor and creates callback that will dump latency statistics at program end - Registry() { - std::atexit([] {Registry::instance.dump();}); + Registry() {} + ~Registry() { + Registry::instance().dump(); } - const time_t now = std::time(nullptr); - std::tm* localTime = std::localtime(&now); - std::string path_ = "../log/latency_" + localTime->tm_year + 1900 + "_" + localTime->tm_mon + 1 + "_" + localTime->tm_mday + std::string path_ = "../../log/latency.txt"; + std::unordered_map> registry_; +}; +class Timer{ -} +public: + Timer(std::string label) : label_(label), start_(std::chrono::steady_clock::now()) {} + ~Timer() { + uint64_t time = static_cast(std::chrono:: + duration_cast(std::chrono::steady_clock::now() - start_).count()); + Registry::instance().record(label_, time); + } +private: +const std::chrono::steady_clock::time_point start_; +std::string label_; +}; } // namespace latency_benchmarking \ No newline at end of file From 163a7df60d0b47ff4adbb3bf3bac8c4d4c901011 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Sun, 26 Oct 2025 21:45:55 -0400 Subject: [PATCH 03/21] i'm confused --- .../planners/rotate_path_planner.hpp | 2 + .../src/planners/rotate_path_planner.cpp | 1 + src/rj_ui/src/main.cpp | 5 +- src/rj_utils/CMakeLists.txt | 1 + .../include/rj_utils/latency_benchmarking.hpp | 78 ++++++++++++------- src/rj_utils/src/latency_benchmarking.cpp | 2 + 6 files changed, 58 insertions(+), 31 deletions(-) create mode 100644 src/rj_utils/src/latency_benchmarking.cpp diff --git a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp index 1f39d9c137..93bd0bb454 100644 --- a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp +++ b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp @@ -17,6 +17,8 @@ #include "rj_planning/primitives/trapezoidal_motion.hpp" #include "rj_planning/primitives/velocity_profiling.hpp" +#include "rj_utils/latency_benchmarking.hpp" + namespace planning { /** * Path planner that only rotates the robot about a point given. diff --git a/src/rj_planning/src/planners/rotate_path_planner.cpp b/src/rj_planning/src/planners/rotate_path_planner.cpp index 956a44b4a2..933a322100 100644 --- a/src/rj_planning/src/planners/rotate_path_planner.cpp +++ b/src/rj_planning/src/planners/rotate_path_planner.cpp @@ -35,6 +35,7 @@ void RotatePathPlanner::update_state() { bool RotatePathPlanner::is_done() const { return current_state_ == END; } Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { + latency_benchmarking::Timer timer("rotate_kick_pivot", request.shell_id); const RobotInstant& start_instant = request.start; const auto& linear_constraints = request.constraints.mot; const auto& rotation_constraints = request.constraints.rot; diff --git a/src/rj_ui/src/main.cpp b/src/rj_ui/src/main.cpp index 1e139f20b4..39bd788419 100644 --- a/src/rj_ui/src/main.cpp +++ b/src/rj_ui/src/main.cpp @@ -18,6 +18,7 @@ #include "rj_ui/main_window.hpp" #include "rj_ui/style_sheet_manager.hpp" +#include "rj_utils/latency_benchmarking.hpp" using namespace std; @@ -43,7 +44,9 @@ void usage(const char* prog) { int main(int argc, char* argv[]) { printf("Starting Soccer...\n"); - + latency_benchmarking::Registry registery; + // registery.lol(); + printf("stupidest thing ive ever witnessed"); // register our signal handler signal(SIGINT, signal_handler); diff --git a/src/rj_utils/CMakeLists.txt b/src/rj_utils/CMakeLists.txt index ce21151753..8d332cf901 100644 --- a/src/rj_utils/CMakeLists.txt +++ b/src/rj_utils/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(Qt5 COMPONENTS Core Widgets REQUIRED) add_library(${PROJECT_NAME} SHARED src/conversions.cpp src/logging.cpp + src/latency_benchmarking.cpp ) ament_target_dependencies(${PROJECT_NAME} diff --git a/src/rj_utils/include/rj_utils/latency_benchmarking.hpp b/src/rj_utils/include/rj_utils/latency_benchmarking.hpp index b5eee13853..5ca3849d5f 100644 --- a/src/rj_utils/include/rj_utils/latency_benchmarking.hpp +++ b/src/rj_utils/include/rj_utils/latency_benchmarking.hpp @@ -1,10 +1,12 @@ #include #include +#include #include #include #include #include #include +#include #include @@ -12,57 +14,73 @@ namespace latency_benchmarking { class Registry { public: - // Singleton Pattern - static Registry& instance() { - static Registry R; - return R; - } + // // Singleton Pattern + // static Registry& instance() { + // static Registry R; + // return R; + // } - void record(std::string label, uint64_t time) { - registry_[label].push_back(time); - } + // void record(std::string label, uint64_t time, int8_t robot_id) { + // registry_.at(robot_id)[label].push_back(time); + // } - void dump() { - std::ofstream output_file; - output_file.open(path_); - for (auto& p : registry_) { - output_file << p.first << " "; - for (uint64_t e : p.second) { - output_file << e << " "; - } - output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) / p.second.size(); - output_file << "\n"; - } - output_file.close(); - } + // void dump() { + // SPDLOG_INFO("testing testing testing"); + // std::ofstream output_file; + // output_file.open(path_); + // for (int i = 0; i < 6; i++) { + // output_file << "Robot " << i << "\n"; + // for (auto& p : registry_[i]) { + // output_file << p.first << " "; + // for (uint64_t e : p.second) { + // output_file << e << ", "; + // } + // output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) / p.second.size(); + // output_file << "\n"; + // } + // } + // output_file.close(); + // } - void clear() { - registry_.clear(); - } + // void clear() { + // registry_.clear(); + // } -private: +// private: // Constructor and creates callback that will dump latency statistics at program end - Registry() {} + Registry() { + SPDLOG_INFO("TESTING TESTING TESTING"); + printf("TESTING FROM PRINTFFFF"); + } ~Registry() { - Registry::instance().dump(); + // Registry::instance().dump(); + SPDLOG_INFO("DESTRUCTOR HAS BEEN CALLED TESTING"); } + // void lol() { + // printf("lol"); + // } + std::string path_ = "../../log/latency.txt"; - std::unordered_map> registry_; + std::array>, 6> registry_; }; class Timer{ public: - Timer(std::string label) : label_(label), start_(std::chrono::steady_clock::now()) {} + Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { + SPDLOG_INFO("HEY DONT LOOK AT ME"); + } ~Timer() { + SPDLOG_INFO("hey look at me"); uint64_t time = static_cast(std::chrono:: duration_cast(std::chrono::steady_clock::now() - start_).count()); - Registry::instance().record(label_, time); + // Registry::instance().record(label_, time, robot_id_); } private: const std::chrono::steady_clock::time_point start_; std::string label_; +int8_t robot_id_; }; diff --git a/src/rj_utils/src/latency_benchmarking.cpp b/src/rj_utils/src/latency_benchmarking.cpp new file mode 100644 index 0000000000..361fe6dfc0 --- /dev/null +++ b/src/rj_utils/src/latency_benchmarking.cpp @@ -0,0 +1,2 @@ +#include "rj_utils/latency_benchmarking.hpp" + From 0367db813b21e2f55f9f6faa239d84f9546ae22f Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 28 Oct 2025 21:24:54 -0400 Subject: [PATCH 04/21] created a new node for rj_benchmarking --- launch/soccer.launch.py | 12 +++++ src/rj_benchmarking/CMakeLists.txt | 53 +++++++++++++++++++ .../rj_benchmarking}/latency_benchmarking.hpp | 0 .../include/rj_benchmarking/registry.hpp | 14 +++++ .../include/rj_benchmarking/timer.hpp | 0 src/rj_benchmarking/package.xml | 20 +++++++ src/rj_benchmarking/src/main.cpp | 11 ++++ src/rj_benchmarking/src/registry.cpp | 13 +++++ src/rj_benchmarking/src/timer.cpp | 0 src/rj_utils/CMakeLists.txt | 1 - src/rj_utils/src/latency_benchmarking.cpp | 2 - 11 files changed, 123 insertions(+), 3 deletions(-) create mode 100644 src/rj_benchmarking/CMakeLists.txt rename src/{rj_utils/include/rj_utils => rj_benchmarking/include/rj_benchmarking}/latency_benchmarking.hpp (100%) create mode 100644 src/rj_benchmarking/include/rj_benchmarking/registry.hpp create mode 100644 src/rj_benchmarking/include/rj_benchmarking/timer.hpp create mode 100644 src/rj_benchmarking/package.xml create mode 100644 src/rj_benchmarking/src/main.cpp create mode 100644 src/rj_benchmarking/src/registry.cpp create mode 100644 src/rj_benchmarking/src/timer.cpp delete mode 100644 src/rj_utils/src/latency_benchmarking.cpp diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index f8f992d0f8..9dcb50fa80 100644 --- a/launch/soccer.launch.py +++ b/launch/soccer.launch.py @@ -109,6 +109,13 @@ def generate_launch_description(): # # Note the order doesn't matter here: ROS nodes launch in some # random order (there are Executors to change that) + Node( + package="rj_benchmarking", + executable="rj_benchmarking_node", + output="screen", + parameters=[param_config_filepath], + on_exit=Shutdown() + ), Node( package="rj_vision_receiver", executable="rj_vision_receiver_node", @@ -238,5 +245,10 @@ def generate_launch_description(): parameters=[param_config_filepath], on_exit=Shutdown(), ) + # Node( + # package="rj_benchmarking", + # executable="benchmarking_node", + + # ) ] ) diff --git a/src/rj_benchmarking/CMakeLists.txt b/src/rj_benchmarking/CMakeLists.txt new file mode 100644 index 0000000000..ab67c611cb --- /dev/null +++ b/src/rj_benchmarking/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(rj_benchmarking) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(spdlog REQUIRED) +find_package(fmt REQUIRED) + +set(BENCHMARKING_DEPS + fmt + rclcpp + spdlog +) + +set(BENCHMARKING_LIBS + fmt + spdlog +) + +set(BENCHMARKING_SRC + src/registry.cpp +) + +add_executable(${PROJECT_NAME}_node + ${BENCHMARKING_SRC} + src/main.cpp +) + +target_link_libraries(${PROJECT_NAME}_node PUBLIC ${BENCHMARKING_LIBS}) + +target_include_directories(${PROJECT_NAME}_node + PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME}_node PUBLIC ${BENCHMARKING_DEPS}) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/src/rj_utils/include/rj_utils/latency_benchmarking.hpp b/src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp similarity index 100% rename from src/rj_utils/include/rj_utils/latency_benchmarking.hpp rename to src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp new file mode 100644 index 0000000000..ee284a4eb1 --- /dev/null +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -0,0 +1,14 @@ +#include +#include + +namespace benchmarking { + class Registry : public rclcpp::Node { + public: + Registry(); + ~Registry(); + + private: + std::string path_ = "../../log/latency.txt"; + std::array>, 6> registry_; + }; +} \ No newline at end of file diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/rj_benchmarking/package.xml b/src/rj_benchmarking/package.xml new file mode 100644 index 0000000000..441029f447 --- /dev/null +++ b/src/rj_benchmarking/package.xml @@ -0,0 +1,20 @@ + + + + rj_benchmarking + 0.0.0 + Node for Benchmarking + Yuvraj Dhadwal + TODO: License declaration + + rclcpp + rclcpp_components + spdlog + fmt + + ament_cmake + + + ament_cmake + + diff --git a/src/rj_benchmarking/src/main.cpp b/src/rj_benchmarking/src/main.cpp new file mode 100644 index 0000000000..1f6cec73ef --- /dev/null +++ b/src/rj_benchmarking/src/main.cpp @@ -0,0 +1,11 @@ +#include + +#include + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp new file mode 100644 index 0000000000..b3282e389b --- /dev/null +++ b/src/rj_benchmarking/src/registry.cpp @@ -0,0 +1,13 @@ +#include "rj_benchmarking/registry.hpp" + +namespace benchmarking { + Registry::Registry() : rclcpp::Node{"rj_benchmarking"} + { + SPDLOG_INFO("TESTING: Registry Built"); + } + + Registry::~Registry() + { + SPDLOG_INFO("TESTING: Registry Destroyed"); + } +} \ No newline at end of file diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/rj_utils/CMakeLists.txt b/src/rj_utils/CMakeLists.txt index 8d332cf901..ce21151753 100644 --- a/src/rj_utils/CMakeLists.txt +++ b/src/rj_utils/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(Qt5 COMPONENTS Core Widgets REQUIRED) add_library(${PROJECT_NAME} SHARED src/conversions.cpp src/logging.cpp - src/latency_benchmarking.cpp ) ament_target_dependencies(${PROJECT_NAME} diff --git a/src/rj_utils/src/latency_benchmarking.cpp b/src/rj_utils/src/latency_benchmarking.cpp deleted file mode 100644 index 361fe6dfc0..0000000000 --- a/src/rj_utils/src/latency_benchmarking.cpp +++ /dev/null @@ -1,2 +0,0 @@ -#include "rj_utils/latency_benchmarking.hpp" - From f4ea17a3a9ec58d509f286415bf82528bb2a7cd8 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 28 Oct 2025 21:59:00 -0400 Subject: [PATCH 05/21] started working on registry within the node, there is an issue with multiple Registrys being created? possibly cpp rule of 5 --- src/rj_benchmarking/CMakeLists.txt | 1 + .../include/rj_benchmarking/benchmarking.hpp | 14 +++++++ .../include/rj_benchmarking/registry.hpp | 41 ++++++++++++++----- .../include/rj_benchmarking/timer.hpp | 17 ++++++++ src/rj_benchmarking/src/benchmarking.cpp | 15 +++++++ src/rj_benchmarking/src/main.cpp | 4 +- src/rj_benchmarking/src/registry.cpp | 29 +++++++++++-- 7 files changed, 104 insertions(+), 17 deletions(-) create mode 100644 src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp create mode 100644 src/rj_benchmarking/src/benchmarking.cpp diff --git a/src/rj_benchmarking/CMakeLists.txt b/src/rj_benchmarking/CMakeLists.txt index ab67c611cb..34a123fb06 100644 --- a/src/rj_benchmarking/CMakeLists.txt +++ b/src/rj_benchmarking/CMakeLists.txt @@ -22,6 +22,7 @@ set(BENCHMARKING_LIBS ) set(BENCHMARKING_SRC + src/benchmarking.cpp src/registry.cpp ) diff --git a/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp b/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp new file mode 100644 index 0000000000..380b2fe972 --- /dev/null +++ b/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp @@ -0,0 +1,14 @@ +#include +#include + +namespace benchmarking { + class Benchmarking : public rclcpp::Node { + public: + Benchmarking(); + ~Benchmarking(); + + private: + // std::string path_ = "../../log/latency.txt"; + // std::array>, 6> registry_; + }; +} \ No newline at end of file diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index ee284a4eb1..3d41fe6a06 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -1,14 +1,33 @@ +#include +#include +#include +#include +#include +#include #include -#include + +#include namespace benchmarking { - class Registry : public rclcpp::Node { - public: - Registry(); - ~Registry(); - - private: - std::string path_ = "../../log/latency.txt"; - std::array>, 6> registry_; - }; -} \ No newline at end of file + +class Registry { +public: + // Singleton Pattern + static Registry& instance() { + static Registry R; + return R; + } + + void record(std::string label, uint64_t time, int8_t robot_id); + + void dump(); + + // Constructor and creates callback that will dump latency statistics at program end + Registry(); + ~Registry(); + +private: + std::string path_ = "../../log/latency.txt"; + std::array>, 6> registry_; +}; +} // namespace benchmarking \ No newline at end of file diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp index e69de29bb2..aa99c8410c 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -0,0 +1,17 @@ +class Timer{ + +public: + Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { + SPDLOG_INFO("HEY DONT LOOK AT ME"); + } + ~Timer() { + SPDLOG_INFO("hey look at me"); + uint64_t time = static_cast(std::chrono:: + duration_cast(std::chrono::steady_clock::now() - start_).count()); + // Registry::instance().record(label_, time, robot_id_); + } +private: +const std::chrono::steady_clock::time_point start_; +std::string label_; +int8_t robot_id_; +}; \ No newline at end of file diff --git a/src/rj_benchmarking/src/benchmarking.cpp b/src/rj_benchmarking/src/benchmarking.cpp new file mode 100644 index 0000000000..6efc907d5e --- /dev/null +++ b/src/rj_benchmarking/src/benchmarking.cpp @@ -0,0 +1,15 @@ +#include "rj_benchmarking/benchmarking.hpp" +#include "rj_benchmarking/registry.hpp" + +namespace benchmarking { + Benchmarking::Benchmarking() : rclcpp::Node{"rj_benchmarking"} + { + SPDLOG_INFO("TESTING: Benchmarking Built"); + Registry(); + } + + Benchmarking::~Benchmarking() + { + SPDLOG_INFO("TESTING: Benchmarking Destroyed"); + } +} \ No newline at end of file diff --git a/src/rj_benchmarking/src/main.cpp b/src/rj_benchmarking/src/main.cpp index 1f6cec73ef..0b8842b7ef 100644 --- a/src/rj_benchmarking/src/main.cpp +++ b/src/rj_benchmarking/src/main.cpp @@ -1,11 +1,11 @@ #include -#include +#include int main(int argc, char* argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index b3282e389b..11a4ddbf9f 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -1,13 +1,34 @@ #include "rj_benchmarking/registry.hpp" namespace benchmarking { - Registry::Registry() : rclcpp::Node{"rj_benchmarking"} - { + Registry::Registry() { SPDLOG_INFO("TESTING: Registry Built"); } - Registry::~Registry() - { + Registry::~Registry() { + Registry::instance().dump(); SPDLOG_INFO("TESTING: Registry Destroyed"); } + + void Registry::record(std::string label, uint64_t time, int8_t robot_id) { + registry_.at(robot_id)[label].push_back(time); + } + + void Registry::dump() { + SPDLOG_INFO("TESTING: Dump Called"); + std::ofstream output_file; + output_file.open(path_); + for (int i = 0; i < 6; i++) { + output_file << "Robot " << i << "\n"; + for (auto& p : registry_[i]) { + output_file << p.first << " "; + for (uint64_t e : p.second) { + output_file << e << ", "; + } + output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) / p.second.size(); + output_file << "\n"; + } + } + output_file.close(); + } } \ No newline at end of file From 1a61c6a8aa4821a5f494443857818e453cb79769 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Sun, 2 Nov 2025 21:35:57 -0500 Subject: [PATCH 06/21] aaaaaaalind --- src/rj_benchmarking/CMakeLists.txt | 3 +- .../include/rj_benchmarking/benchmarking.hpp | 15 ++---- .../include/rj_benchmarking/registry.hpp | 29 +++++++---- .../include/rj_benchmarking/timer.hpp | 27 +++++----- src/rj_benchmarking/src/benchmarking.cpp | 20 ++++---- src/rj_benchmarking/src/main.cpp | 14 +++-- src/rj_benchmarking/src/registry.cpp | 51 +++++++++---------- src/rj_benchmarking/src/timer.cpp | 13 +++++ src/rj_ui/package.xml | 1 + src/rj_ui/src/main.cpp | 5 +- 10 files changed, 100 insertions(+), 78 deletions(-) diff --git a/src/rj_benchmarking/CMakeLists.txt b/src/rj_benchmarking/CMakeLists.txt index 34a123fb06..a5592c201c 100644 --- a/src/rj_benchmarking/CMakeLists.txt +++ b/src/rj_benchmarking/CMakeLists.txt @@ -24,6 +24,7 @@ set(BENCHMARKING_LIBS set(BENCHMARKING_SRC src/benchmarking.cpp src/registry.cpp + src/timer.cpp ) add_executable(${PROJECT_NAME}_node @@ -51,4 +52,4 @@ install( DESTINATION lib/${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() diff --git a/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp b/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp index 380b2fe972..954b659220 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp @@ -1,14 +1,9 @@ #include #include -namespace benchmarking { - class Benchmarking : public rclcpp::Node { - public: - Benchmarking(); - ~Benchmarking(); +class Benchmarking : public rclcpp::Node { - private: - // std::string path_ = "../../log/latency.txt"; - // std::array>, 6> registry_; - }; -} \ No newline at end of file +public: + Benchmarking(); + ~Benchmarking(); +}; diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index 3d41fe6a06..17b5364012 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -4,30 +4,39 @@ #include #include #include -#include - #include -namespace benchmarking { +#include + class Registry { public: // Singleton Pattern - static Registry& instance() { - static Registry R; - return R; + static Registry* getInstance() { + if (instance == nullptr) { + instance = new Registry(); + } + + return instance; } void record(std::string label, uint64_t time, int8_t robot_id); void dump(); - // Constructor and creates callback that will dump latency statistics at program end - Registry(); ~Registry(); private: - std::string path_ = "../../log/latency.txt"; + static Registry* instance; + + // Private Constructor + Registry(); + + // Delete Copy Constructor and Assignment + Registry(const Registry& other) = delete; + Registry& operator=(const Registry& other) = delete; + + + std::string path_ = "log/latency.txt"; std::array>, 6> registry_; }; -} // namespace benchmarking \ No newline at end of file diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp index aa99c8410c..631b92e0ad 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -1,17 +1,18 @@ +#include +#include + +#include + +#include + class Timer{ public: - Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { - SPDLOG_INFO("HEY DONT LOOK AT ME"); - } - ~Timer() { - SPDLOG_INFO("hey look at me"); - uint64_t time = static_cast(std::chrono:: - duration_cast(std::chrono::steady_clock::now() - start_).count()); - // Registry::instance().record(label_, time, robot_id_); - } + Timer(std::string label, std::int8_t robot_id); + ~Timer(); + private: -const std::chrono::steady_clock::time_point start_; -std::string label_; -int8_t robot_id_; -}; \ No newline at end of file + const std::chrono::steady_clock::time_point start_; + std::string label_; + std::int8_t robot_id_; +}; diff --git a/src/rj_benchmarking/src/benchmarking.cpp b/src/rj_benchmarking/src/benchmarking.cpp index 6efc907d5e..5b7782b8e4 100644 --- a/src/rj_benchmarking/src/benchmarking.cpp +++ b/src/rj_benchmarking/src/benchmarking.cpp @@ -1,15 +1,13 @@ #include "rj_benchmarking/benchmarking.hpp" #include "rj_benchmarking/registry.hpp" -namespace benchmarking { - Benchmarking::Benchmarking() : rclcpp::Node{"rj_benchmarking"} - { - SPDLOG_INFO("TESTING: Benchmarking Built"); - Registry(); - } +Benchmarking::Benchmarking() : rclcpp::Node{"rj_benchmarking"} +{ + SPDLOG_INFO("TESTING: Benchmarking Built"); + Registry::getInstance(); +} - Benchmarking::~Benchmarking() - { - SPDLOG_INFO("TESTING: Benchmarking Destroyed"); - } -} \ No newline at end of file +Benchmarking::~Benchmarking() +{ + SPDLOG_INFO("TESTING: Benchmarking Destroyed"); +} diff --git a/src/rj_benchmarking/src/main.cpp b/src/rj_benchmarking/src/main.cpp index 0b8842b7ef..3f4eb745b0 100644 --- a/src/rj_benchmarking/src/main.cpp +++ b/src/rj_benchmarking/src/main.cpp @@ -1,11 +1,19 @@ #include -#include +#include "rj_benchmarking/benchmarking.hpp" +#include "rj_benchmarking/registry.hpp" +#include "rj_benchmarking/timer.hpp" + + +Registry* Registry::instance = nullptr; int main(int argc, char* argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); + { + Timer t("lol", 1); + } rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index 11a4ddbf9f..bccb6cbb20 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -1,34 +1,33 @@ #include "rj_benchmarking/registry.hpp" -namespace benchmarking { - Registry::Registry() { - SPDLOG_INFO("TESTING: Registry Built"); - } - Registry::~Registry() { - Registry::instance().dump(); - SPDLOG_INFO("TESTING: Registry Destroyed"); - } +Registry::Registry() { + SPDLOG_INFO("TESTING: Registry Built"); +} - void Registry::record(std::string label, uint64_t time, int8_t robot_id) { - registry_.at(robot_id)[label].push_back(time); - } +Registry::~Registry() { + Registry::getInstance()->dump(); + SPDLOG_INFO("TESTING: Registry Destroyed"); +} + +void Registry::record(std::string label, uint64_t time, int8_t robot_id) { + registry_.at(robot_id)[label].push_back(time); +} - void Registry::dump() { - SPDLOG_INFO("TESTING: Dump Called"); - std::ofstream output_file; - output_file.open(path_); - for (int i = 0; i < 6; i++) { - output_file << "Robot " << i << "\n"; - for (auto& p : registry_[i]) { - output_file << p.first << " "; - for (uint64_t e : p.second) { - output_file << e << ", "; - } - output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) / p.second.size(); - output_file << "\n"; +void Registry::dump() { + SPDLOG_INFO("TESTING: Dump Called"); + std::ofstream output_file; + output_file.open(path_); + for (int i = 0; i < 6; i++) { + output_file << "Robot " << i << "\n"; + for (auto& p : registry_[i]) { + output_file << p.first << " "; + for (uint64_t e : p.second) { + output_file << e << ", "; } + output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) / p.second.size(); + output_file << "\n"; } - output_file.close(); } -} \ No newline at end of file + output_file.close(); +} diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index e69de29bb2..07c27483c8 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -0,0 +1,13 @@ +#include "rj_benchmarking/timer.hpp" +#include "rj_benchmarking/registry.hpp" + +Timer::Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { + SPDLOG_INFO("HEY DONT LOOK AT ME"); +} + +Timer::~Timer() { + SPDLOG_INFO("hey look at me"); + uint64_t time = static_cast(std::chrono:: + duration_cast(std::chrono::steady_clock::now() - start_).count()); + Registry::getInstance()->record(label_, time, robot_id_); +} diff --git a/src/rj_ui/package.xml b/src/rj_ui/package.xml index 7c3b251d16..d9a52abd16 100644 --- a/src/rj_ui/package.xml +++ b/src/rj_ui/package.xml @@ -30,6 +30,7 @@ rj_referee rj_config_client rj_topic_utils + ament_cmake diff --git a/src/rj_ui/src/main.cpp b/src/rj_ui/src/main.cpp index 39bd788419..b67e3f063f 100644 --- a/src/rj_ui/src/main.cpp +++ b/src/rj_ui/src/main.cpp @@ -18,7 +18,7 @@ #include "rj_ui/main_window.hpp" #include "rj_ui/style_sheet_manager.hpp" -#include "rj_utils/latency_benchmarking.hpp" +// #include using namespace std; @@ -44,9 +44,6 @@ void usage(const char* prog) { int main(int argc, char* argv[]) { printf("Starting Soccer...\n"); - latency_benchmarking::Registry registery; - // registery.lol(); - printf("stupidest thing ive ever witnessed"); // register our signal handler signal(SIGINT, signal_handler); From 4988c4b44dc7907667e2413bf39565a83cab8b23 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Sun, 2 Nov 2025 21:52:46 -0500 Subject: [PATCH 07/21] end of day work --- src/rj_benchmarking/src/benchmarking.cpp | 1 + src/rj_benchmarking/src/timer.cpp | 4 ++-- src/rj_planning/CMakeLists.txt | 2 ++ .../include/rj_planning/planners/rotate_path_planner.hpp | 4 ++-- src/rj_planning/package.xml | 1 + src/rj_planning/src/planners/rotate_path_planner.cpp | 2 +- 6 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/rj_benchmarking/src/benchmarking.cpp b/src/rj_benchmarking/src/benchmarking.cpp index 5b7782b8e4..af756aa963 100644 --- a/src/rj_benchmarking/src/benchmarking.cpp +++ b/src/rj_benchmarking/src/benchmarking.cpp @@ -9,5 +9,6 @@ Benchmarking::Benchmarking() : rclcpp::Node{"rj_benchmarking"} Benchmarking::~Benchmarking() { + delete Registry::getInstance(); SPDLOG_INFO("TESTING: Benchmarking Destroyed"); } diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index 07c27483c8..115fe9b5d2 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -2,11 +2,11 @@ #include "rj_benchmarking/registry.hpp" Timer::Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { - SPDLOG_INFO("HEY DONT LOOK AT ME"); + SPDLOG_INFO("Testing: Timer Created " + label); } Timer::~Timer() { - SPDLOG_INFO("hey look at me"); + SPDLOG_INFO("Testing: Timer Destroyed"); uint64_t time = static_cast(std::chrono:: duration_cast(std::chrono::steady_clock::now() - start_).count()); Registry::getInstance()->record(label_, time, robot_id_); diff --git a/src/rj_planning/CMakeLists.txt b/src/rj_planning/CMakeLists.txt index 2796055865..8446be892a 100644 --- a/src/rj_planning/CMakeLists.txt +++ b/src/rj_planning/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(rj_convert REQUIRED) find_package(rj_msgs REQUIRED) find_package(rj_param_utils REQUIRED) find_package(rj_rrt REQUIRED) +find_package(rj_benchmarking REQUIRED) set(RJ_PLANNING_DEPS rclcpp @@ -39,6 +40,7 @@ set(RJ_PLANNING_DEPS rj_msgs rj_param_utils rj_rrt + rj_benchmarking Flann ) diff --git a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp index 93bd0bb454..f226e6355b 100644 --- a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp +++ b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp @@ -17,7 +17,7 @@ #include "rj_planning/primitives/trapezoidal_motion.hpp" #include "rj_planning/primitives/velocity_profiling.hpp" -#include "rj_utils/latency_benchmarking.hpp" +#include namespace planning { /** @@ -65,4 +65,4 @@ class RotatePathPlanner : public PathPlanner { double isDoneAngleChangeThresh{1.0}; }; -} // namespace planning \ No newline at end of file +} // namespace planning diff --git a/src/rj_planning/package.xml b/src/rj_planning/package.xml index 98af846728..147dd07784 100644 --- a/src/rj_planning/package.xml +++ b/src/rj_planning/package.xml @@ -21,6 +21,7 @@ rj_msgs rj_param_utils rj_rrt + rj_benchmarking ament_cmake diff --git a/src/rj_planning/src/planners/rotate_path_planner.cpp b/src/rj_planning/src/planners/rotate_path_planner.cpp index 933a322100..e24ea336c5 100644 --- a/src/rj_planning/src/planners/rotate_path_planner.cpp +++ b/src/rj_planning/src/planners/rotate_path_planner.cpp @@ -35,7 +35,7 @@ void RotatePathPlanner::update_state() { bool RotatePathPlanner::is_done() const { return current_state_ == END; } Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { - latency_benchmarking::Timer timer("rotate_kick_pivot", request.shell_id); + Timer timer("rotate_kick_pivot", request.shell_id); const RobotInstant& start_instant = request.start; const auto& linear_constraints = request.constraints.mot; const auto& rotation_constraints = request.constraints.rot; From b799876474bb3d5b4509e8121110f9a0e3b242d9 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Sun, 9 Nov 2025 15:40:04 -0500 Subject: [PATCH 08/21] starting work to transform to pub/sub model --- src/rj_benchmarking/CMakeLists.txt | 58 ++++++++++++++++++- .../include/rj_benchmarking/benchmarking.hpp | 5 +- .../rj_benchmarking/latency_benchmarking.hpp | 4 +- .../include/rj_benchmarking/registry.hpp | 42 +++++++++----- .../include/rj_benchmarking/timer.hpp | 5 +- src/rj_benchmarking/src/main.cpp | 4 +- src/rj_benchmarking/src/registry.cpp | 42 +++++++++----- src/rj_benchmarking/src/timer.cpp | 16 +++-- src/rj_msgs/CMakeLists.txt | 3 + src/rj_msgs/benchmarking/Latency.msg | 3 + .../planners/rotate_path_planner.hpp | 3 +- 11 files changed, 141 insertions(+), 44 deletions(-) create mode 100644 src/rj_msgs/benchmarking/Latency.msg diff --git a/src/rj_benchmarking/CMakeLists.txt b/src/rj_benchmarking/CMakeLists.txt index a5592c201c..5fd505e3c7 100644 --- a/src/rj_benchmarking/CMakeLists.txt +++ b/src/rj_benchmarking/CMakeLists.txt @@ -5,15 +5,23 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +# Find dependencies +find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rj_msgs REQUIRED) set(BENCHMARKING_DEPS fmt rclcpp spdlog + rj_msgs ) set(BENCHMARKING_LIBS @@ -27,29 +35,73 @@ set(BENCHMARKING_SRC src/timer.cpp ) +# RJ Benchmarking Library +add_library(${PROJECT_NAME} SHARED + ${BENCHMARKING_SRC} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + ${BENCHMARKING_LIBS} +) + +ament_target_dependencies(${PROJECT_NAME} + PUBLIC + ${BENCHMARKING_DEPS} +) + +# Benchmarking Node add_executable(${PROJECT_NAME}_node ${BENCHMARKING_SRC} src/main.cpp ) -target_link_libraries(${PROJECT_NAME}_node PUBLIC ${BENCHMARKING_LIBS}) - target_include_directories(${PROJECT_NAME}_node PUBLIC $ $ ) -ament_target_dependencies(${PROJECT_NAME}_node PUBLIC ${BENCHMARKING_DEPS}) +target_link_libraries(${PROJECT_NAME}_node + PUBLIC + ${BENCHMARKING_LIBS} +) + +ament_target_dependencies(${PROJECT_NAME}_node + PUBLIC + ${BENCHMARKING_DEPS} +) install( DIRECTORY include/ DESTINATION include ) +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + install( TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME} ) +ament_export_dependencies( + ${BENCHMARKING_DEPS} +) + +ament_export_include_directories(include) + +ament_export_targets(${PROJECT_NAME}) ament_package() diff --git a/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp b/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp index 954b659220..6ffbefda6c 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp @@ -1,7 +1,10 @@ +#pragma once + #include #include -class Benchmarking : public rclcpp::Node { +class Benchmarking : public rclcpp::Node +{ public: Benchmarking(); diff --git a/src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp b/src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp index 5ca3849d5f..484766c0e5 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include #include @@ -84,4 +86,4 @@ int8_t robot_id_; }; -} // namespace latency_benchmarking \ No newline at end of file +} // namespace latency_benchmarking diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index 17b5364012..a9076412a0 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include #include @@ -7,36 +9,46 @@ #include #include +#include +#include -class Registry { +class Registry : public rclcpp::Node +{ public: // Singleton Pattern - static Registry* getInstance() { - if (instance == nullptr) { - instance = new Registry(); - } - - return instance; - } + // static Registry* getInstance() + // { + // if (instance == nullptr) + // { + // instance = new Registry(); + // } - void record(std::string label, uint64_t time, int8_t robot_id); + // return instance; + // } - void dump(); + // void record(std::string label, uint64_t time, int8_t robot_id); + // void dump(); + Registry(); ~Registry(); private: - static Registry* instance; + // static Registry* instance; // Private Constructor - Registry(); + // Registry(); // Delete Copy Constructor and Assignment - Registry(const Registry& other) = delete; - Registry& operator=(const Registry& other) = delete; + // Registry(const Registry& other) = delete; + // Registry& operator=(const Registry& other) = delete; + void topic_callback(const rj_msgs::benchmarking::Latency &msg); + void dump(); - std::string path_ = "log/latency.txt"; + std::string path_ { "log/latency.txt" }; + + // registry[label][robot_id] -> latency sampling std::array>, 6> registry_; + rclcpp::Subscription::SharedPtr subscription_; }; diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp index 631b92e0ad..dd88e2c8d5 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include @@ -5,7 +7,8 @@ #include -class Timer{ +class Timer +{ public: Timer(std::string label, std::int8_t robot_id); diff --git a/src/rj_benchmarking/src/main.cpp b/src/rj_benchmarking/src/main.cpp index 3f4eb745b0..d31b9654db 100644 --- a/src/rj_benchmarking/src/main.cpp +++ b/src/rj_benchmarking/src/main.cpp @@ -1,6 +1,6 @@ #include -#include "rj_benchmarking/benchmarking.hpp" +// #include "rj_benchmarking/benchmarking.hpp" #include "rj_benchmarking/registry.hpp" #include "rj_benchmarking/timer.hpp" @@ -10,7 +10,7 @@ Registry* Registry::instance = nullptr; int main(int argc, char* argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); { Timer t("lol", 1); } diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index bccb6cbb20..22386417b1 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -1,32 +1,48 @@ #include "rj_benchmarking/registry.hpp" - -Registry::Registry() { +Registry::Registry() : rclcpp::Node{"rj_benchmarking"} +{ SPDLOG_INFO("TESTING: Registry Built"); + subscription_ = this->create_subscription( + "/registry", 100, std::bind(&Registry::topic_callback, this, + std::placeholders::_1)); } -Registry::~Registry() { - Registry::getInstance()->dump(); +Registry::~Registry() +{ + dump(); SPDLOG_INFO("TESTING: Registry Destroyed"); } -void Registry::record(std::string label, uint64_t time, int8_t robot_id) { - registry_.at(robot_id)[label].push_back(time); +// void Registry::record(std::string label, uint64_t time, int8_t robot_id) +// { +// registry_.at(robot_id)[label].push_back(time); +// } + +void Registry::topic_callback(const rj_msgs::benchmarking::Latency &msg) +{ + registry_.at(msg->label)[msg->robot_id].push_back(msg->duration_ns); } -void Registry::dump() { + +void Registry::dump() +{ SPDLOG_INFO("TESTING: Dump Called"); std::ofstream output_file; output_file.open(path_); - for (int i = 0; i < 6; i++) { - output_file << "Robot " << i << "\n"; - for (auto& p : registry_[i]) { + for (int i = 0; i < 6; i++) + { + output_file << "Robot " << i << '\n'; + for (auto& p : registry_[i]) + { output_file << p.first << " "; - for (uint64_t e : p.second) { + for (uint64_t e : p.second) + { output_file << e << ", "; } - output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) / p.second.size(); - output_file << "\n"; + output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) + / p.second.size(); + output_file << '\n'; } } output_file.close(); diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index 115fe9b5d2..5e6796c855 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -1,13 +1,17 @@ #include "rj_benchmarking/timer.hpp" #include "rj_benchmarking/registry.hpp" -Timer::Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { +Timer::Timer(std::string label, int8_t robot_id) : label_(label), + robot_id_(robot_id), start_(std::chrono::steady_clock::now()) +{ SPDLOG_INFO("Testing: Timer Created " + label); } -Timer::~Timer() { - SPDLOG_INFO("Testing: Timer Destroyed"); - uint64_t time = static_cast(std::chrono:: - duration_cast(std::chrono::steady_clock::now() - start_).count()); - Registry::getInstance()->record(label_, time, robot_id_); +Timer::~Timer() +{ + SPDLOG_INFO("Testing: Timer Destroyed"); + uint64_t time = static_cast(std::chrono:: + duration_cast(std::chrono::steady_clock::now() + - start_).count()); + Registry::getInstance()->record(label_, time, robot_id_); } diff --git a/src/rj_msgs/CMakeLists.txt b/src/rj_msgs/CMakeLists.txt index 8d7117d82f..2029b84cac 100644 --- a/src/rj_msgs/CMakeLists.txt +++ b/src/rj_msgs/CMakeLists.txt @@ -17,6 +17,9 @@ find_package(builtin_interfaces REQUIRED) rosidl_generate_interfaces( ${PROJECT_NAME} + # Benchmarking + benchmarking/Latency.msg + # Actions action/RobotMove.action diff --git a/src/rj_msgs/benchmarking/Latency.msg b/src/rj_msgs/benchmarking/Latency.msg new file mode 100644 index 0000000000..9f6dc46d86 --- /dev/null +++ b/src/rj_msgs/benchmarking/Latency.msg @@ -0,0 +1,3 @@ +string label +uint64 duration_ns +int8 robot_id diff --git a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp index f226e6355b..d55988eaa0 100644 --- a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp +++ b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include "rj_planning/planners/path_planner.hpp" #include "rj_planning/planners/path_target_path_planner.hpp" @@ -17,8 +18,6 @@ #include "rj_planning/primitives/trapezoidal_motion.hpp" #include "rj_planning/primitives/velocity_profiling.hpp" -#include - namespace planning { /** * Path planner that only rotates the robot about a point given. From 2e3cb1766f1716761d4bc2f8d81bf89f30e04ba7 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Sun, 9 Nov 2025 21:16:24 -0500 Subject: [PATCH 09/21] its working! we are now measuring latency for pivots! --- src/rj_benchmarking/CMakeLists.txt | 2 +- .../include/rj_benchmarking/benchmarking.hpp | 12 --- .../rj_benchmarking/latency_benchmarking.hpp | 89 ------------------- .../include/rj_benchmarking/registry.hpp | 8 +- .../rj_benchmarking/registry_publisher.hpp | 37 ++++++++ .../include/rj_benchmarking/timer.hpp | 4 + src/rj_benchmarking/src/benchmarking.cpp | 14 --- src/rj_benchmarking/src/main.cpp | 12 ++- src/rj_benchmarking/src/registry.cpp | 6 +- .../src/registry_publisher.cpp | 21 +++++ src/rj_benchmarking/src/timer.cpp | 8 +- src/rj_msgs/CMakeLists.txt | 5 +- src/rj_msgs/{benchmarking => msg}/Latency.msg | 0 13 files changed, 82 insertions(+), 136 deletions(-) delete mode 100644 src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp delete mode 100644 src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp create mode 100644 src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp delete mode 100644 src/rj_benchmarking/src/benchmarking.cpp create mode 100644 src/rj_benchmarking/src/registry_publisher.cpp rename src/rj_msgs/{benchmarking => msg}/Latency.msg (100%) diff --git a/src/rj_benchmarking/CMakeLists.txt b/src/rj_benchmarking/CMakeLists.txt index 5fd505e3c7..abb8a15697 100644 --- a/src/rj_benchmarking/CMakeLists.txt +++ b/src/rj_benchmarking/CMakeLists.txt @@ -30,9 +30,9 @@ set(BENCHMARKING_LIBS ) set(BENCHMARKING_SRC - src/benchmarking.cpp src/registry.cpp src/timer.cpp + src/registry_publisher.cpp ) # RJ Benchmarking Library diff --git a/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp b/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp deleted file mode 100644 index 6ffbefda6c..0000000000 --- a/src/rj_benchmarking/include/rj_benchmarking/benchmarking.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include -#include - -class Benchmarking : public rclcpp::Node -{ - -public: - Benchmarking(); - ~Benchmarking(); -}; diff --git a/src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp b/src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp deleted file mode 100644 index 484766c0e5..0000000000 --- a/src/rj_benchmarking/include/rj_benchmarking/latency_benchmarking.hpp +++ /dev/null @@ -1,89 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace latency_benchmarking { - -class Registry { -public: - // // Singleton Pattern - // static Registry& instance() { - // static Registry R; - // return R; - // } - - // void record(std::string label, uint64_t time, int8_t robot_id) { - // registry_.at(robot_id)[label].push_back(time); - // } - - // void dump() { - // SPDLOG_INFO("testing testing testing"); - // std::ofstream output_file; - // output_file.open(path_); - // for (int i = 0; i < 6; i++) { - // output_file << "Robot " << i << "\n"; - // for (auto& p : registry_[i]) { - // output_file << p.first << " "; - // for (uint64_t e : p.second) { - // output_file << e << ", "; - // } - // output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) / p.second.size(); - // output_file << "\n"; - // } - // } - // output_file.close(); - // } - - // void clear() { - // registry_.clear(); - // } - -// private: - // Constructor and creates callback that will dump latency statistics at program end - Registry() { - SPDLOG_INFO("TESTING TESTING TESTING"); - printf("TESTING FROM PRINTFFFF"); - } - ~Registry() { - // Registry::instance().dump(); - SPDLOG_INFO("DESTRUCTOR HAS BEEN CALLED TESTING"); - } - - // void lol() { - // printf("lol"); - // } - - std::string path_ = "../../log/latency.txt"; - std::array>, 6> registry_; -}; - -class Timer{ - -public: - Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { - SPDLOG_INFO("HEY DONT LOOK AT ME"); - } - ~Timer() { - SPDLOG_INFO("hey look at me"); - uint64_t time = static_cast(std::chrono:: - duration_cast(std::chrono::steady_clock::now() - start_).count()); - // Registry::instance().record(label_, time, robot_id_); - } -private: -const std::chrono::steady_clock::time_point start_; -std::string label_; -int8_t robot_id_; -}; - - -} // namespace latency_benchmarking diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index a9076412a0..d613980b6a 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -10,7 +10,7 @@ #include #include -#include +#include class Registry : public rclcpp::Node @@ -43,12 +43,12 @@ class Registry : public rclcpp::Node // Registry(const Registry& other) = delete; // Registry& operator=(const Registry& other) = delete; - void topic_callback(const rj_msgs::benchmarking::Latency &msg); + void topic_callback(const rj_msgs::msg::Latency &msg); void dump(); std::string path_ { "log/latency.txt" }; - // registry[label][robot_id] -> latency sampling + // registry[robot_id][label] -> latency sampling std::array>, 6> registry_; - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_; }; diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp new file mode 100644 index 0000000000..622f029fbc --- /dev/null +++ b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include +#include + +class RegistryPublisher +{ +public: + // Singleton pattern + static RegistryPublisher* getInstance() + { + if (instance == nullptr) + { + instance = new RegistryPublisher(); + } + + return instance; + } + + void publish(std::string label, std::int8_t robot_id, uint64_t time); + +private: + static RegistryPublisher* instance; + + // Private Constructor + RegistryPublisher(); + + // Delete Copy Constructor and Assignment + RegistryPublisher(const RegistryPublisher& other) = delete; + RegistryPublisher& operator=(const RegistryPublisher& other) = delete; + + std::shared_ptr node_ = std::make_shared( + "rj_benchmarking_publisher"); + rclcpp::Publisher::SharedPtr publisher_ + = node_->create_publisher("/registry", 100); + +}; diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp index dd88e2c8d5..eb38091ddd 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -6,6 +6,10 @@ #include #include +#include +#include + +#include "rj_benchmarking/registry_publisher.hpp" class Timer { diff --git a/src/rj_benchmarking/src/benchmarking.cpp b/src/rj_benchmarking/src/benchmarking.cpp deleted file mode 100644 index af756aa963..0000000000 --- a/src/rj_benchmarking/src/benchmarking.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "rj_benchmarking/benchmarking.hpp" -#include "rj_benchmarking/registry.hpp" - -Benchmarking::Benchmarking() : rclcpp::Node{"rj_benchmarking"} -{ - SPDLOG_INFO("TESTING: Benchmarking Built"); - Registry::getInstance(); -} - -Benchmarking::~Benchmarking() -{ - delete Registry::getInstance(); - SPDLOG_INFO("TESTING: Benchmarking Destroyed"); -} diff --git a/src/rj_benchmarking/src/main.cpp b/src/rj_benchmarking/src/main.cpp index d31b9654db..73da805e03 100644 --- a/src/rj_benchmarking/src/main.cpp +++ b/src/rj_benchmarking/src/main.cpp @@ -2,18 +2,16 @@ // #include "rj_benchmarking/benchmarking.hpp" #include "rj_benchmarking/registry.hpp" -#include "rj_benchmarking/timer.hpp" - - -Registry* Registry::instance = nullptr; +// #include "rj_benchmarking/timer.hpp" +#include "rj_benchmarking/registry_publisher.hpp" int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); - { - Timer t("lol", 1); - } + // { + // Timer t("lol", 1); + // } rclcpp::shutdown(); return 0; } diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index 22386417b1..9daeb980b1 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -3,7 +3,7 @@ Registry::Registry() : rclcpp::Node{"rj_benchmarking"} { SPDLOG_INFO("TESTING: Registry Built"); - subscription_ = this->create_subscription( + subscription_ = this->create_subscription( "/registry", 100, std::bind(&Registry::topic_callback, this, std::placeholders::_1)); } @@ -19,9 +19,9 @@ Registry::~Registry() // registry_.at(robot_id)[label].push_back(time); // } -void Registry::topic_callback(const rj_msgs::benchmarking::Latency &msg) +void Registry::topic_callback(const rj_msgs::msg::Latency &msg) { - registry_.at(msg->label)[msg->robot_id].push_back(msg->duration_ns); + registry_.at(msg.robot_id)[msg.label].push_back(msg.duration_ns); } diff --git a/src/rj_benchmarking/src/registry_publisher.cpp b/src/rj_benchmarking/src/registry_publisher.cpp new file mode 100644 index 0000000000..3a344b60ae --- /dev/null +++ b/src/rj_benchmarking/src/registry_publisher.cpp @@ -0,0 +1,21 @@ +#include "rj_benchmarking/registry_publisher.hpp" + +RegistryPublisher* RegistryPublisher::instance = nullptr; + +RegistryPublisher::RegistryPublisher() +{ + printf("Testing"); +} + +void RegistryPublisher::publish(std::string label, std::int8_t robot_id, uint64_t time) +{ + // idk what type this should be im just following tutorial code + auto message = rj_msgs::msg::Latency(); + + message.label = label; + message.robot_id = robot_id; + message.duration_ns = time; + + // Registry::getInstance()->record(label_, time, robot_id_); + publisher_->publish(message); +} diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index 5e6796c855..ee3a566713 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -1,10 +1,11 @@ #include "rj_benchmarking/timer.hpp" -#include "rj_benchmarking/registry.hpp" +// #include "rj_benchmarking/registry.hpp" Timer::Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { - SPDLOG_INFO("Testing: Timer Created " + label); + SPDLOG_INFO("Testing: Timer Created " + label); + // publisher_ = this->create_publisher("/registry", 100); } Timer::~Timer() @@ -13,5 +14,6 @@ Timer::~Timer() uint64_t time = static_cast(std::chrono:: duration_cast(std::chrono::steady_clock::now() - start_).count()); - Registry::getInstance()->record(label_, time, robot_id_); + + RegistryPublisher::getInstance()->publish(label_, robot_id_, time); } diff --git a/src/rj_msgs/CMakeLists.txt b/src/rj_msgs/CMakeLists.txt index 2029b84cac..c92c6d1ab2 100644 --- a/src/rj_msgs/CMakeLists.txt +++ b/src/rj_msgs/CMakeLists.txt @@ -16,9 +16,7 @@ find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) rosidl_generate_interfaces( - ${PROJECT_NAME} - # Benchmarking - benchmarking/Latency.msg + ${PROJECT_NAME} # Actions action/RobotMove.action @@ -38,6 +36,7 @@ rosidl_generate_interfaces( msg/ManipulatorSetpoint.msg msg/MatchState.msg msg/MotionSetpoint.msg + msg/Latency.msg msg/LinearMotionInstant.msg msg/MotionCommand.msg diff --git a/src/rj_msgs/benchmarking/Latency.msg b/src/rj_msgs/msg/Latency.msg similarity index 100% rename from src/rj_msgs/benchmarking/Latency.msg rename to src/rj_msgs/msg/Latency.msg From e7ab8769fc9bc272d84255f5f332b0d601ef5d0b Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Sun, 9 Nov 2025 21:30:03 -0500 Subject: [PATCH 10/21] added benchmarking as a library import to all packages --- src/rj_common/CMakeLists.txt | 2 ++ src/rj_config_client/CMakeLists.txt | 3 +++ src/rj_config_server/CMakeLists.txt | 2 ++ src/rj_control/CMakeLists.txt | 4 ++++ src/rj_geometry/CMakeLists.txt | 3 +++ src/rj_joystick/CMakeLists.txt | 2 ++ src/rj_optimization/CMakeLists.txt | 3 +++ src/rj_radio/CMakeLists.txt | 3 +++ src/rj_referee/CMakeLists.txt | 4 ++++ src/rj_strategy/CMakeLists.txt | 3 +++ src/rj_ui/CMakeLists.txt | 4 ++++ src/rj_utils/CMakeLists.txt | 4 +++- src/rj_vision_filter/CMakeLists.txt | 3 +++ src/rj_vision_receiver/CMakeLists.txt | 2 ++ 14 files changed, 41 insertions(+), 1 deletion(-) diff --git a/src/rj_common/CMakeLists.txt b/src/rj_common/CMakeLists.txt index c2996bf5a3..e650d924d8 100644 --- a/src/rj_common/CMakeLists.txt +++ b/src/rj_common/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rj_geometry REQUIRED) find_package(rj_param_utils REQUIRED) find_package(rj_utils REQUIRED) find_package(rj_drawing_msgs REQUIRED) +find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/debug_drawer.cpp @@ -51,6 +52,7 @@ ament_target_dependencies(${PROJECT_NAME} rj_param_utils rj_utils rj_drawing_msgs + rj_benchmarking ) target_link_libraries(${PROJECT_NAME} diff --git a/src/rj_config_client/CMakeLists.txt b/src/rj_config_client/CMakeLists.txt index 057a8d82bb..63ec4f3899 100644 --- a/src/rj_config_client/CMakeLists.txt +++ b/src/rj_config_client/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(fmt REQUIRED) find_package(rj_constants REQUIRED) find_package(rj_common REQUIRED) find_package(rj_utils REQUIRED) +find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/config_client.cpp @@ -53,6 +54,7 @@ ament_target_dependencies(${PROJECT_NAME} rj_constants rj_common rj_utils + rj_benchmarking ) ament_target_dependencies(${PROJECT_NAME}_node @@ -63,6 +65,7 @@ ament_target_dependencies(${PROJECT_NAME}_node rj_constants rj_common rj_utils + rj_benchmarking ) install( diff --git a/src/rj_config_server/CMakeLists.txt b/src/rj_config_server/CMakeLists.txt index b4c86280d4..485979db87 100644 --- a/src/rj_config_server/CMakeLists.txt +++ b/src/rj_config_server/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(fmt REQUIRED) find_package(rj_constants REQUIRED) find_package(rj_common REQUIRED) find_package(rj_utils REQUIRED) +find_package(rj_benchmarking REQUIRED) add_executable(${PROJECT_NAME}_node src/config_server.cpp @@ -41,6 +42,7 @@ ament_target_dependencies(${PROJECT_NAME}_node rj_constants rj_common rj_utils + rj_benchmarking ) install( diff --git a/src/rj_control/CMakeLists.txt b/src/rj_control/CMakeLists.txt index 665d7bf528..d5b8217f84 100644 --- a/src/rj_control/CMakeLists.txt +++ b/src/rj_control/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(rj_common REQUIRED) find_package(rj_geometry REQUIRED) find_package(rj_planning REQUIRED) find_package(rj_convert REQUIRED) +find_package(rj_benchmarking REQUIRED) add_executable(motion_control_node src/motion_control_node.cpp @@ -73,6 +74,7 @@ ament_target_dependencies(motion_control_node rj_geometry rj_planning rj_convert + rj_benchmarking ) ament_target_dependencies(${PROJECT_NAME} @@ -89,6 +91,7 @@ ament_target_dependencies(${PROJECT_NAME} rj_geometry rj_planning rj_convert + rj_benchmarking ) install( @@ -121,6 +124,7 @@ ament_export_dependencies( rj_geometry rj_planning rj_convert + rj_benchmarking ) if(BUILD_TESTING) diff --git a/src/rj_geometry/CMakeLists.txt b/src/rj_geometry/CMakeLists.txt index 88c05e8d8f..84bfd5907e 100644 --- a/src/rj_geometry/CMakeLists.txt +++ b/src/rj_geometry/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(rj_convert REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(rj_geometry_msgs REQUIRED) +find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/arc.cpp @@ -49,6 +50,7 @@ ament_target_dependencies(${PROJECT_NAME} rj_constants rj_convert rj_geometry_msgs + rj_benchmarking ) target_link_libraries(${PROJECT_NAME} @@ -81,6 +83,7 @@ ament_export_dependencies( rj_constants rj_convert rj_geometry_msgs + rj_benchmarking ) if(BUILD_TESTING) diff --git a/src/rj_joystick/CMakeLists.txt b/src/rj_joystick/CMakeLists.txt index e2a02acad1..74eb5f0321 100644 --- a/src/rj_joystick/CMakeLists.txt +++ b/src/rj_joystick/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(rj_common REQUIRED) find_package(rj_constants REQUIRED) find_package(rj_msgs REQUIRED) find_package(rj_param_utils REQUIRED) +find_package(rj_benchmarking REQUIRED) # Find sdl2 include(FindPkgConfig) @@ -54,6 +55,7 @@ ament_target_dependencies(manual_control_node rj_msgs rj_param_utils rclcpp_components + rj_benchmarking ) install( diff --git a/src/rj_optimization/CMakeLists.txt b/src/rj_optimization/CMakeLists.txt index 64fed90da3..cf32707c4a 100644 --- a/src/rj_optimization/CMakeLists.txt +++ b/src/rj_optimization/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rj_common REQUIRED) find_package(rj_geometry REQUIRED) +find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/gradient_ascent_1d.cpp @@ -46,6 +47,7 @@ ament_target_dependencies(${PROJECT_NAME} PythonLibs rj_common rj_geometry + rj_benchmarking ) install( @@ -68,6 +70,7 @@ ament_export_dependencies( PythonLibs rj_common rj_geometry + rj_benchmarking ) if(BUILD_TESTING) diff --git a/src/rj_radio/CMakeLists.txt b/src/rj_radio/CMakeLists.txt index 0f601c0f52..a28c3561d1 100644 --- a/src/rj_radio/CMakeLists.txt +++ b/src/rj_radio/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(rj_strategy REQUIRED) find_package(rj_geometry REQUIRED) find_package(rj_planning REQUIRED) find_package(rj_constants REQUIRED) +find_package(rj_benchmarking REQUIRED) add_executable(network_radio_node src/network_radio.cpp @@ -79,6 +80,7 @@ ament_target_dependencies(network_radio_node rj_geometry rj_planning rj_constants + rj_benchmarking ) ament_target_dependencies(sim_radio_node @@ -97,6 +99,7 @@ ament_target_dependencies(sim_radio_node rj_geometry rj_planning rj_constants + rj_benchmarking ) install( diff --git a/src/rj_referee/CMakeLists.txt b/src/rj_referee/CMakeLists.txt index f0b5d080bf..80ee036925 100644 --- a/src/rj_referee/CMakeLists.txt +++ b/src/rj_referee/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rj_utils REQUIRED) find_package(rj_msgs REQUIRED) find_package(rj_protos REQUIRED) find_package(rj_config_client REQUIRED) +find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/referee_base.cpp @@ -94,6 +95,7 @@ ament_target_dependencies(${PROJECT_NAME} rj_msgs rj_protos rj_config_client + rj_benchmarking ) ament_target_dependencies(external_referee_node @@ -110,6 +112,7 @@ ament_target_dependencies(external_referee_node rj_msgs rj_protos rj_config_client + rj_benchmarking ) ament_target_dependencies(internal_referee_node @@ -126,6 +129,7 @@ ament_target_dependencies(internal_referee_node rj_msgs rj_protos rj_config_client + rj_benchmarking ) install( diff --git a/src/rj_strategy/CMakeLists.txt b/src/rj_strategy/CMakeLists.txt index b00d47d05a..2346ab6a99 100644 --- a/src/rj_strategy/CMakeLists.txt +++ b/src/rj_strategy/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(rj_common REQUIRED) find_package(rj_geometry REQUIRED) find_package(rj_param_utils REQUIRED) find_package(rj_utils REQUIRED) +find_package(rj_benchmarking REQUIRED) set(RJ_STRATEGY_DEPS rclcpp @@ -36,6 +37,7 @@ set(RJ_STRATEGY_DEPS rj_geometry rj_param_utils rj_utils + rj_benchmarking ) set(RJ_STRATEGY_SRC @@ -163,6 +165,7 @@ ament_export_dependencies( rj_geometry rj_param_utils rj_utils + rj_benchmarking ) ament_export_targets(${PROJECT_NAME}) diff --git a/src/rj_ui/CMakeLists.txt b/src/rj_ui/CMakeLists.txt index 3ea82ab1bf..5ffd7066d3 100644 --- a/src/rj_ui/CMakeLists.txt +++ b/src/rj_ui/CMakeLists.txt @@ -34,6 +34,7 @@ find_package(rj_strategy REQUIRED) find_package(rj_referee REQUIRED) find_package(rj_config_client REQUIRED) find_package(rj_topic_utils REQUIRED) +find_package(rj_benchmarking REQUIRED) # Enable Qt's automatic processing set(CMAKE_AUTOMOC ON) # Handle Q_OBJECT in headers automatically @@ -189,6 +190,7 @@ ament_target_dependencies(${PROJECT_NAME}_node rj_referee rj_config_client rj_topic_utils + rj_benchmarking ) ament_target_dependencies(log_viewer_node @@ -215,6 +217,7 @@ ament_target_dependencies(log_viewer_node rj_referee rj_config_client rj_topic_utils + rj_benchmarking ) ament_target_dependencies(${PROJECT_NAME} @@ -241,6 +244,7 @@ ament_target_dependencies(${PROJECT_NAME} rj_referee rj_config_client rj_topic_utils + rj_benchmarking ) install( diff --git a/src/rj_utils/CMakeLists.txt b/src/rj_utils/CMakeLists.txt index ce21151753..5bf335dd25 100644 --- a/src/rj_utils/CMakeLists.txt +++ b/src/rj_utils/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rj_msgs REQUIRED) find_package(rj_convert REQUIRED) find_package(rj_protos REQUIRED) find_package(Qt5 COMPONENTS Core Widgets REQUIRED) +find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/conversions.cpp @@ -27,6 +28,7 @@ ament_target_dependencies(${PROJECT_NAME} rj_msgs rj_convert rj_protos + rj_benchmarking ) target_link_libraries(${PROJECT_NAME} @@ -42,7 +44,7 @@ target_include_directories(${PROJECT_NAME} $ ) -ament_export_dependencies(rclcpp rj_msgs rj_convert rj_protos) +ament_export_dependencies(rclcpp rj_msgs rj_convert rj_protos rj_benchmarking) install( DIRECTORY include/ diff --git a/src/rj_vision_filter/CMakeLists.txt b/src/rj_vision_filter/CMakeLists.txt index 14c74b1c2e..fa18ba47fd 100644 --- a/src/rj_vision_filter/CMakeLists.txt +++ b/src/rj_vision_filter/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(rclcpp_components REQUIRED) find_package(rj_config_client REQUIRED) find_package(rj_topic_utils REQUIRED) find_package(rj_constants REQUIRED) +find_package(rj_benchmarking REQUIRED) add_executable(${PROJECT_NAME}_node src/main.cpp @@ -110,6 +111,7 @@ ament_target_dependencies(${PROJECT_NAME}_node rj_config_client rj_topic_utils rj_constants + rj_benchmarking ) ament_target_dependencies(${PROJECT_NAME} @@ -128,6 +130,7 @@ ament_target_dependencies(${PROJECT_NAME} rj_config_client rj_topic_utils rj_constants + rj_benchmarking ) install( diff --git a/src/rj_vision_receiver/CMakeLists.txt b/src/rj_vision_receiver/CMakeLists.txt index 751526ae52..be273cfc49 100644 --- a/src/rj_vision_receiver/CMakeLists.txt +++ b/src/rj_vision_receiver/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(rj_utils REQUIRED) find_package(rj_constants REQUIRED) find_package(rj_protos REQUIRED) find_package(rj_config_client REQUIRED) +find_package(rj_benchmarking REQUIRED) set(VISION_RECEIVER_DEPS rclcpp @@ -31,6 +32,7 @@ set(VISION_RECEIVER_DEPS rj_constants rj_protos rj_config_client + rj_benchmarking ) set(VISION_RECEIVER_LIBS From 5d38c15232f2b1b16a5ae2fa33dbb6a34b9aa0f7 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 11 Nov 2025 19:59:35 -0500 Subject: [PATCH 11/21] linting --- .../include/rj_benchmarking/registry.hpp | 40 +++++-------------- .../rj_benchmarking/registry_publisher.hpp | 3 +- .../include/rj_benchmarking/timer.hpp | 17 ++++---- src/rj_benchmarking/src/main.cpp | 9 +---- src/rj_benchmarking/src/registry.cpp | 17 +++++--- .../src/registry_publisher.cpp | 1 - src/rj_benchmarking/src/timer.cpp | 2 - 7 files changed, 34 insertions(+), 55 deletions(-) diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index d613980b6a..a3dff6e221 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -1,5 +1,10 @@ #pragma once +#include + +#include +#include + #include #include #include @@ -7,48 +12,23 @@ #include #include #include - -#include -#include -#include - +#include +#include +#include class Registry : public rclcpp::Node { public: - // Singleton Pattern - // static Registry* getInstance() - // { - // if (instance == nullptr) - // { - // instance = new Registry(); - // } - - // return instance; - // } - - // void record(std::string label, uint64_t time, int8_t robot_id); - - // void dump(); Registry(); ~Registry(); private: - // static Registry* instance; - - // Private Constructor - // Registry(); - - // Delete Copy Constructor and Assignment - // Registry(const Registry& other) = delete; - // Registry& operator=(const Registry& other) = delete; - void topic_callback(const rj_msgs::msg::Latency &msg); void dump(); std::string path_ { "log/latency.txt" }; // registry[robot_id][label] -> latency sampling - std::array>, 6> registry_; - rclcpp::Subscription::SharedPtr subscription_; + std::array>, 6> registry_{}; + rclcpp::Subscription::SharedPtr subscription_{}; }; diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp index 622f029fbc..9737d8f989 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp @@ -1,8 +1,9 @@ #pragma once -#include #include +#include + class RegistryPublisher { public: diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp index eb38091ddd..af959585f5 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -1,15 +1,14 @@ #pragma once -#include -#include +#include "rj_benchmarking/registry_publisher.hpp" +#include +#include #include +#include +#include #include -#include -#include - -#include "rj_benchmarking/registry_publisher.hpp" class Timer { @@ -19,7 +18,7 @@ class Timer ~Timer(); private: - const std::chrono::steady_clock::time_point start_; - std::string label_; - std::int8_t robot_id_; + const std::chrono::steady_clock::time_point start_{}; + std::string label_{}; + std::int8_t robot_id_{}; }; diff --git a/src/rj_benchmarking/src/main.cpp b/src/rj_benchmarking/src/main.cpp index 73da805e03..f01378fbb1 100644 --- a/src/rj_benchmarking/src/main.cpp +++ b/src/rj_benchmarking/src/main.cpp @@ -1,17 +1,12 @@ -#include - -// #include "rj_benchmarking/benchmarking.hpp" #include "rj_benchmarking/registry.hpp" -// #include "rj_benchmarking/timer.hpp" #include "rj_benchmarking/registry_publisher.hpp" +#include + int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); - // { - // Timer t("lol", 1); - // } rclcpp::shutdown(); return 0; } diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index 9daeb980b1..c903d4a6da 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -14,11 +14,6 @@ Registry::~Registry() SPDLOG_INFO("TESTING: Registry Destroyed"); } -// void Registry::record(std::string label, uint64_t time, int8_t robot_id) -// { -// registry_.at(robot_id)[label].push_back(time); -// } - void Registry::topic_callback(const rj_msgs::msg::Latency &msg) { registry_.at(msg.robot_id)[msg.label].push_back(msg.duration_ns); @@ -28,6 +23,18 @@ void Registry::topic_callback(const rj_msgs::msg::Latency &msg) void Registry::dump() { SPDLOG_INFO("TESTING: Dump Called"); + + /* + 1. Make LatencyLogs Directory if not already there + 2. Make latency_curr-date_curr-time folder + 3. Make csv for Robot 1 + 3a. first row is labels + 4. Make csvs for all robots + */ + + std::filesystem::create_directories("./latency"); + + std::filesystem::create_directories("./latency/") std::ofstream output_file; output_file.open(path_); for (int i = 0; i < 6; i++) diff --git a/src/rj_benchmarking/src/registry_publisher.cpp b/src/rj_benchmarking/src/registry_publisher.cpp index 3a344b60ae..16c39154bb 100644 --- a/src/rj_benchmarking/src/registry_publisher.cpp +++ b/src/rj_benchmarking/src/registry_publisher.cpp @@ -16,6 +16,5 @@ void RegistryPublisher::publish(std::string label, std::int8_t robot_id, uint64_ message.robot_id = robot_id; message.duration_ns = time; - // Registry::getInstance()->record(label_, time, robot_id_); publisher_->publish(message); } diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index ee3a566713..c940af1ae6 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -1,11 +1,9 @@ #include "rj_benchmarking/timer.hpp" -// #include "rj_benchmarking/registry.hpp" Timer::Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { SPDLOG_INFO("Testing: Timer Created " + label); - // publisher_ = this->create_publisher("/registry", 100); } Timer::~Timer() From 94d891838e77a42b417bf30ae10ddd36c5926e96 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 11 Nov 2025 21:56:33 -0500 Subject: [PATCH 12/21] added new flush mechanism so that latency files are created and easy to use for later data analysis --- .gitignore | 3 +- .../include/rj_benchmarking/registry.hpp | 5 +- src/rj_benchmarking/src/registry.cpp | 72 ++++++++++++++----- .../src/registry_publisher.cpp | 2 +- src/rj_benchmarking/src/timer.cpp | 4 +- .../planners/goalie_idle_path_planner.hpp | 1 + .../src/planners/goalie_idle_path_planner.cpp | 1 + .../agent/position/solo_offense.hpp | 1 + .../src/agent/position/solo_offense.cpp | 5 ++ 9 files changed, 71 insertions(+), 23 deletions(-) diff --git a/.gitignore b/.gitignore index 622f92e0f5..d2307f68af 100644 --- a/.gitignore +++ b/.gitignore @@ -55,4 +55,5 @@ compile_commands.json # Colcon Build Things build/ install/ -log/ \ No newline at end of file +log/ +latency/ diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index a3dff6e221..039ff0e8d6 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -13,8 +13,8 @@ #include #include #include -#include #include +#include class Registry : public rclcpp::Node { @@ -26,9 +26,10 @@ class Registry : public rclcpp::Node void topic_callback(const rj_msgs::msg::Latency &msg); void dump(); - std::string path_ { "log/latency.txt" }; + std::string get_curr_datetime(); // registry[robot_id][label] -> latency sampling std::array>, 6> registry_{}; rclcpp::Subscription::SharedPtr subscription_{}; + int max_rows_{}; }; diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index c903d4a6da..a2463ba134 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -2,7 +2,7 @@ Registry::Registry() : rclcpp::Node{"rj_benchmarking"} { - SPDLOG_INFO("TESTING: Registry Built"); + // SPDLOG_INFO("TESTING: Registry Built"); subscription_ = this->create_subscription( "/registry", 100, std::bind(&Registry::topic_callback, this, std::placeholders::_1)); @@ -11,18 +11,19 @@ Registry::Registry() : rclcpp::Node{"rj_benchmarking"} Registry::~Registry() { dump(); - SPDLOG_INFO("TESTING: Registry Destroyed"); + // SPDLOG_INFO("TESTING: Registry Destroyed"); } void Registry::topic_callback(const rj_msgs::msg::Latency &msg) { registry_.at(msg.robot_id)[msg.label].push_back(msg.duration_ns); + max_rows_ = std::max(max_rows_, static_cast(registry_.at(msg.robot_id)[msg.label].size())); } void Registry::dump() { - SPDLOG_INFO("TESTING: Dump Called"); + // SPDLOG_INFO("TESTING: Dump Called"); /* 1. Make LatencyLogs Directory if not already there @@ -31,26 +32,63 @@ void Registry::dump() 3a. first row is labels 4. Make csvs for all robots */ + std::string base_path{ "./latency" }; + std::filesystem::create_directories(base_path); + base_path += "/session_"; + base_path += get_curr_datetime(); + std::filesystem::create_directories(base_path); - std::filesystem::create_directories("./latency"); - - std::filesystem::create_directories("./latency/") - std::ofstream output_file; - output_file.open(path_); + // std::ofstream output_file; + // output_file.open(path_); for (int i = 0; i < 6; i++) { - output_file << "Robot " << i << '\n'; - for (auto& p : registry_[i]) + std::stringstream ss; + ss << "/robot_" << i << ".csv"; + std::ofstream robot_csv{ base_path + ss.str() }; + + for (int row = -1; row < max_rows_; row++) { - output_file << p.first << " "; - for (uint64_t e : p.second) + if (row == -1) + { + for (std::pair> labels : registry_.at(i)) + { + robot_csv << labels.first << ','; + } + + robot_csv << '\n'; + } + + for (std::pair> labels : registry_.at(i)) { - output_file << e << ", "; + robot_csv << labels.second[row] << ','; } - output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) - / p.second.size(); - output_file << '\n'; + + robot_csv << '\n'; } + + + // output_file << "Robot " << i << '\n'; + // for (auto& p : registry_[i]) + // { + // output_file << p.first << " "; + // for (uint64_t e : p.second) + // { + // output_file << e << ", "; + // } + // output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) + // / p.second.size(); + // output_file << '\n'; + // } } - output_file.close(); + // output_file.close(); +} + +std::string Registry::get_curr_datetime() +{ + std::time_t time = std::time({}); + char timeString[std::size("yyyy-mm-ddThh:mm:ssZ")]; + std::strftime(std::data(timeString), std::size(timeString), + "%FT%TZ", std::localtime(&time)); + std::string out{ timeString }; + return out; } diff --git a/src/rj_benchmarking/src/registry_publisher.cpp b/src/rj_benchmarking/src/registry_publisher.cpp index 16c39154bb..529cdddc80 100644 --- a/src/rj_benchmarking/src/registry_publisher.cpp +++ b/src/rj_benchmarking/src/registry_publisher.cpp @@ -4,7 +4,7 @@ RegistryPublisher* RegistryPublisher::instance = nullptr; RegistryPublisher::RegistryPublisher() { - printf("Testing"); + // printf("Testing"); } void RegistryPublisher::publish(std::string label, std::int8_t robot_id, uint64_t time) diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index c940af1ae6..852af4f2f0 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -3,12 +3,12 @@ Timer::Timer(std::string label, int8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { - SPDLOG_INFO("Testing: Timer Created " + label); + // SPDLOG_INFO("Testing: Timer Created " + label); } Timer::~Timer() { - SPDLOG_INFO("Testing: Timer Destroyed"); + // SPDLOG_INFO("Testing: Timer Destroyed"); uint64_t time = static_cast(std::chrono:: duration_cast(std::chrono::steady_clock::now() - start_).count()); diff --git a/src/rj_planning/include/rj_planning/planners/goalie_idle_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/goalie_idle_path_planner.hpp index 6e34ef4efb..50fb941ba1 100644 --- a/src/rj_planning/include/rj_planning/planners/goalie_idle_path_planner.hpp +++ b/src/rj_planning/include/rj_planning/planners/goalie_idle_path_planner.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "rj_planning/planners/path_planner.hpp" #include "rj_planning/planners/path_target_path_planner.hpp" diff --git a/src/rj_planning/src/planners/goalie_idle_path_planner.cpp b/src/rj_planning/src/planners/goalie_idle_path_planner.cpp index 00bc04edcc..3d837d4ec3 100644 --- a/src/rj_planning/src/planners/goalie_idle_path_planner.cpp +++ b/src/rj_planning/src/planners/goalie_idle_path_planner.cpp @@ -3,6 +3,7 @@ namespace planning { Trajectory GoalieIdlePathPlanner::plan(const PlanRequest& plan_request) { + // Timer timer("goalie_idle_path_planner", plan_request.shell_id); // lots of this is duplicated from PathTargetPathPlanner, because there's not // an easy way to convert from one PlanRequest to another diff --git a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp index a158820edc..899326e9bd 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "rj_strategy/agent/position.hpp" diff --git a/src/rj_strategy/src/agent/position/solo_offense.cpp b/src/rj_strategy/src/agent/position/solo_offense.cpp index 3154df7bb9..91fde44bb6 100644 --- a/src/rj_strategy/src/agent/position/solo_offense.cpp +++ b/src/rj_strategy/src/agent/position/solo_offense.cpp @@ -9,6 +9,7 @@ SoloOffense::SoloOffense(const Position& other) : Position{other} { SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {} std::optional SoloOffense::derived_get_task(RobotIntent intent) { + Timer timer("solo_offense: derived_get_task", robot_id_); // Get next state, and if different, reset clock State new_state = next_state(); // if (new_state != current_state_) { @@ -25,6 +26,7 @@ std::string SoloOffense::get_current_state() { } SoloOffense::State SoloOffense::next_state() { + Timer timer("solo_offense: next_state", robot_id_); // handle transitions between current state double closest_dist = std::numeric_limits::infinity(); auto current_point = last_world_state_->ball.position; @@ -77,6 +79,7 @@ SoloOffense::State SoloOffense::next_state() { } std::optional SoloOffense::state_to_task(RobotIntent intent) { + Timer timer("solo_offense: state_to_task", robot_id_); switch (current_state_) { case MARKER: { auto marker_target_pos = @@ -132,6 +135,7 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { } rj_geometry::Point SoloOffense::calculate_best_shot() const { + Timer timer("solo_offense: calculate_best_shot", robot_id_); // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); double goal_width = field_dimensions_.goal_width(); // 1.0 meters @@ -157,6 +161,7 @@ rj_geometry::Point SoloOffense::calculate_best_shot() const { double SoloOffense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const { + Timer timer("solo_offense: distance_from_their_robots", robot_id_); rj_geometry::Point vec = head - tail; auto& their_robots = this->last_world_state_->their_robots; From c0e130389ec20aa599cfaf263e99e4e60af13564 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Sun, 16 Nov 2025 20:03:38 -0500 Subject: [PATCH 13/21] removed timer calls in other code --- .../rj_planning/planners/goalie_idle_path_planner.hpp | 1 - .../include/rj_planning/planners/rotate_path_planner.hpp | 1 - src/rj_planning/src/planners/goalie_idle_path_planner.cpp | 1 - src/rj_planning/src/planners/rotate_path_planner.cpp | 1 - .../include/rj_strategy/agent/position/solo_offense.hpp | 1 - src/rj_strategy/src/agent/position/solo_offense.cpp | 5 ----- src/rj_ui/src/main.cpp | 1 - 7 files changed, 11 deletions(-) diff --git a/src/rj_planning/include/rj_planning/planners/goalie_idle_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/goalie_idle_path_planner.hpp index 50fb941ba1..6e34ef4efb 100644 --- a/src/rj_planning/include/rj_planning/planners/goalie_idle_path_planner.hpp +++ b/src/rj_planning/include/rj_planning/planners/goalie_idle_path_planner.hpp @@ -6,7 +6,6 @@ #include #include #include -#include #include "rj_planning/planners/path_planner.hpp" #include "rj_planning/planners/path_target_path_planner.hpp" diff --git a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp index d55988eaa0..7b30472cb0 100644 --- a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp +++ b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp @@ -9,7 +9,6 @@ #include #include #include -#include #include "rj_planning/planners/path_planner.hpp" #include "rj_planning/planners/path_target_path_planner.hpp" diff --git a/src/rj_planning/src/planners/goalie_idle_path_planner.cpp b/src/rj_planning/src/planners/goalie_idle_path_planner.cpp index 3d837d4ec3..00bc04edcc 100644 --- a/src/rj_planning/src/planners/goalie_idle_path_planner.cpp +++ b/src/rj_planning/src/planners/goalie_idle_path_planner.cpp @@ -3,7 +3,6 @@ namespace planning { Trajectory GoalieIdlePathPlanner::plan(const PlanRequest& plan_request) { - // Timer timer("goalie_idle_path_planner", plan_request.shell_id); // lots of this is duplicated from PathTargetPathPlanner, because there's not // an easy way to convert from one PlanRequest to another diff --git a/src/rj_planning/src/planners/rotate_path_planner.cpp b/src/rj_planning/src/planners/rotate_path_planner.cpp index e24ea336c5..956a44b4a2 100644 --- a/src/rj_planning/src/planners/rotate_path_planner.cpp +++ b/src/rj_planning/src/planners/rotate_path_planner.cpp @@ -35,7 +35,6 @@ void RotatePathPlanner::update_state() { bool RotatePathPlanner::is_done() const { return current_state_ == END; } Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { - Timer timer("rotate_kick_pivot", request.shell_id); const RobotInstant& start_instant = request.start; const auto& linear_constraints = request.constraints.mot; const auto& rotation_constraints = request.constraints.rot; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp index 899326e9bd..a158820edc 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/solo_offense.hpp @@ -15,7 +15,6 @@ #include #include #include -#include #include "rj_strategy/agent/position.hpp" diff --git a/src/rj_strategy/src/agent/position/solo_offense.cpp b/src/rj_strategy/src/agent/position/solo_offense.cpp index 91fde44bb6..3154df7bb9 100644 --- a/src/rj_strategy/src/agent/position/solo_offense.cpp +++ b/src/rj_strategy/src/agent/position/solo_offense.cpp @@ -9,7 +9,6 @@ SoloOffense::SoloOffense(const Position& other) : Position{other} { SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {} std::optional SoloOffense::derived_get_task(RobotIntent intent) { - Timer timer("solo_offense: derived_get_task", robot_id_); // Get next state, and if different, reset clock State new_state = next_state(); // if (new_state != current_state_) { @@ -26,7 +25,6 @@ std::string SoloOffense::get_current_state() { } SoloOffense::State SoloOffense::next_state() { - Timer timer("solo_offense: next_state", robot_id_); // handle transitions between current state double closest_dist = std::numeric_limits::infinity(); auto current_point = last_world_state_->ball.position; @@ -79,7 +77,6 @@ SoloOffense::State SoloOffense::next_state() { } std::optional SoloOffense::state_to_task(RobotIntent intent) { - Timer timer("solo_offense: state_to_task", robot_id_); switch (current_state_) { case MARKER: { auto marker_target_pos = @@ -135,7 +132,6 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { } rj_geometry::Point SoloOffense::calculate_best_shot() const { - Timer timer("solo_offense: calculate_best_shot", robot_id_); // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); double goal_width = field_dimensions_.goal_width(); // 1.0 meters @@ -161,7 +157,6 @@ rj_geometry::Point SoloOffense::calculate_best_shot() const { double SoloOffense::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point head) const { - Timer timer("solo_offense: distance_from_their_robots", robot_id_); rj_geometry::Point vec = head - tail; auto& their_robots = this->last_world_state_->their_robots; diff --git a/src/rj_ui/src/main.cpp b/src/rj_ui/src/main.cpp index b67e3f063f..8005eac23e 100644 --- a/src/rj_ui/src/main.cpp +++ b/src/rj_ui/src/main.cpp @@ -18,7 +18,6 @@ #include "rj_ui/main_window.hpp" #include "rj_ui/style_sheet_manager.hpp" -// #include using namespace std; From d4c529288bbeaed0da00c28cb4e9a740e4a338b7 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 16 Nov 2025 21:12:16 -0500 Subject: [PATCH 14/21] Fix Code Style On feature/latency_benchmarking (#2462) automated style fixes Co-authored-by: yuvrajdhadwal --- .../include/rj_benchmarking/registry.hpp | 29 ++++++------ .../rj_benchmarking/registry_publisher.hpp | 22 ++++----- .../include/rj_benchmarking/timer.hpp | 23 +++++---- src/rj_benchmarking/src/main.cpp | 7 ++- src/rj_benchmarking/src/registry.cpp | 47 +++++++------------ .../src/registry_publisher.cpp | 8 ++-- src/rj_benchmarking/src/timer.cpp | 16 +++---- 7 files changed, 64 insertions(+), 88 deletions(-) diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index 039ff0e8d6..0481548c6c 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -1,33 +1,32 @@ #pragma once -#include - -#include -#include - -#include -#include #include -#include -#include -#include #include -#include #include +#include +#include #include +#include +#include +#include +#include -class Registry : public rclcpp::Node -{ +#include +#include + +#include + +class Registry : public rclcpp::Node { public: Registry(); ~Registry(); private: - void topic_callback(const rj_msgs::msg::Latency &msg); + void topic_callback(const rj_msgs::msg::Latency& msg); void dump(); std::string get_curr_datetime(); - + // registry[robot_id][label] -> latency sampling std::array>, 6> registry_{}; rclcpp::Subscription::SharedPtr subscription_{}; diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp index 9737d8f989..7dab498153 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp @@ -1,17 +1,14 @@ #pragma once -#include - #include -class RegistryPublisher -{ +#include + +class RegistryPublisher { public: // Singleton pattern - static RegistryPublisher* getInstance() - { - if (instance == nullptr) - { + static RegistryPublisher* getInstance() { + if (instance == nullptr) { instance = new RegistryPublisher(); } @@ -30,9 +27,8 @@ class RegistryPublisher RegistryPublisher(const RegistryPublisher& other) = delete; RegistryPublisher& operator=(const RegistryPublisher& other) = delete; - std::shared_ptr node_ = std::make_shared( - "rj_benchmarking_publisher"); - rclcpp::Publisher::SharedPtr publisher_ - = node_->create_publisher("/registry", 100); - + std::shared_ptr node_ = + std::make_shared("rj_benchmarking_publisher"); + rclcpp::Publisher::SharedPtr publisher_ = + node_->create_publisher("/registry", 100); }; diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp index af959585f5..efe09b89bf 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -1,24 +1,23 @@ #pragma once -#include "rj_benchmarking/registry_publisher.hpp" -#include +#include +#include +#include #include #include -#include -#include -#include +#include -class Timer -{ +#include "rj_benchmarking/registry_publisher.hpp" +class Timer { public: - Timer(std::string label, std::int8_t robot_id); - ~Timer(); + Timer(std::string label, std::int8_t robot_id); + ~Timer(); private: - const std::chrono::steady_clock::time_point start_{}; - std::string label_{}; - std::int8_t robot_id_{}; + const std::chrono::steady_clock::time_point start_{}; + std::string label_{}; + std::int8_t robot_id_{}; }; diff --git a/src/rj_benchmarking/src/main.cpp b/src/rj_benchmarking/src/main.cpp index f01378fbb1..6087e90474 100644 --- a/src/rj_benchmarking/src/main.cpp +++ b/src/rj_benchmarking/src/main.cpp @@ -1,10 +1,9 @@ +#include + #include "rj_benchmarking/registry.hpp" #include "rj_benchmarking/registry_publisher.hpp" -#include - -int main(int argc, char* argv[]) -{ +int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index a2463ba134..2592430594 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -1,28 +1,22 @@ #include "rj_benchmarking/registry.hpp" -Registry::Registry() : rclcpp::Node{"rj_benchmarking"} -{ +Registry::Registry() : rclcpp::Node{"rj_benchmarking"} { // SPDLOG_INFO("TESTING: Registry Built"); subscription_ = this->create_subscription( - "/registry", 100, std::bind(&Registry::topic_callback, this, - std::placeholders::_1)); + "/registry", 100, std::bind(&Registry::topic_callback, this, std::placeholders::_1)); } -Registry::~Registry() -{ +Registry::~Registry() { dump(); // SPDLOG_INFO("TESTING: Registry Destroyed"); } -void Registry::topic_callback(const rj_msgs::msg::Latency &msg) -{ +void Registry::topic_callback(const rj_msgs::msg::Latency& msg) { registry_.at(msg.robot_id)[msg.label].push_back(msg.duration_ns); max_rows_ = std::max(max_rows_, static_cast(registry_.at(msg.robot_id)[msg.label].size())); } - -void Registry::dump() -{ +void Registry::dump() { // SPDLOG_INFO("TESTING: Dump Called"); /* @@ -32,7 +26,7 @@ void Registry::dump() 3a. first row is labels 4. Make csvs for all robots */ - std::string base_path{ "./latency" }; + std::string base_path{"./latency"}; std::filesystem::create_directories(base_path); base_path += "/session_"; base_path += get_curr_datetime(); @@ -40,33 +34,27 @@ void Registry::dump() // std::ofstream output_file; // output_file.open(path_); - for (int i = 0; i < 6; i++) - { + for (int i = 0; i < 6; i++) { std::stringstream ss; ss << "/robot_" << i << ".csv"; - std::ofstream robot_csv{ base_path + ss.str() }; + std::ofstream robot_csv{base_path + ss.str()}; - for (int row = -1; row < max_rows_; row++) - { - if (row == -1) - { - for (std::pair> labels : registry_.at(i)) - { + for (int row = -1; row < max_rows_; row++) { + if (row == -1) { + for (std::pair> labels : registry_.at(i)) { robot_csv << labels.first << ','; } robot_csv << '\n'; } - for (std::pair> labels : registry_.at(i)) - { + for (std::pair> labels : registry_.at(i)) { robot_csv << labels.second[row] << ','; } robot_csv << '\n'; } - // output_file << "Robot " << i << '\n'; // for (auto& p : registry_[i]) // { @@ -76,19 +64,18 @@ void Registry::dump() // output_file << e << ", "; // } // output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) - // / p.second.size(); + // / + // p.second.size(); // output_file << '\n'; // } } // output_file.close(); } -std::string Registry::get_curr_datetime() -{ +std::string Registry::get_curr_datetime() { std::time_t time = std::time({}); char timeString[std::size("yyyy-mm-ddThh:mm:ssZ")]; - std::strftime(std::data(timeString), std::size(timeString), - "%FT%TZ", std::localtime(&time)); - std::string out{ timeString }; + std::strftime(std::data(timeString), std::size(timeString), "%FT%TZ", std::localtime(&time)); + std::string out{timeString}; return out; } diff --git a/src/rj_benchmarking/src/registry_publisher.cpp b/src/rj_benchmarking/src/registry_publisher.cpp index 529cdddc80..ea2475b0b2 100644 --- a/src/rj_benchmarking/src/registry_publisher.cpp +++ b/src/rj_benchmarking/src/registry_publisher.cpp @@ -2,16 +2,14 @@ RegistryPublisher* RegistryPublisher::instance = nullptr; -RegistryPublisher::RegistryPublisher() -{ +RegistryPublisher::RegistryPublisher() { // printf("Testing"); } -void RegistryPublisher::publish(std::string label, std::int8_t robot_id, uint64_t time) -{ +void RegistryPublisher::publish(std::string label, std::int8_t robot_id, uint64_t time) { // idk what type this should be im just following tutorial code auto message = rj_msgs::msg::Latency(); - + message.label = label; message.robot_id = robot_id; message.duration_ns = time; diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index 852af4f2f0..748ada4a37 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -1,17 +1,15 @@ #include "rj_benchmarking/timer.hpp" -Timer::Timer(std::string label, int8_t robot_id) : label_(label), - robot_id_(robot_id), start_(std::chrono::steady_clock::now()) -{ +Timer::Timer(std::string label, int8_t robot_id) + : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { // SPDLOG_INFO("Testing: Timer Created " + label); } -Timer::~Timer() -{ +Timer::~Timer() { // SPDLOG_INFO("Testing: Timer Destroyed"); - uint64_t time = static_cast(std::chrono:: - duration_cast(std::chrono::steady_clock::now() - - start_).count()); - + uint64_t time = static_cast(std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_) + .count()); + RegistryPublisher::getInstance()->publish(label_, robot_id_, time); } From f4bc21fbe21c66926d83153e7bf69ae91ce95b93 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 18 Nov 2025 00:51:48 -0500 Subject: [PATCH 15/21] fixed pr comments --- launch/soccer.launch.py | 5 - src/rj_benchmarking/CMakeLists.txt | 2 + .../include/rj_benchmarking/registry.hpp | 8 +- .../rj_benchmarking/registry_publisher.hpp | 11 +-- .../include/rj_benchmarking/timer.hpp | 8 +- src/rj_benchmarking/src/registry.cpp | 96 ++++++++----------- .../src/registry_publisher.cpp | 9 +- src/rj_benchmarking/src/timer.cpp | 5 +- src/rj_common/CMakeLists.txt | 2 - src/rj_config_client/CMakeLists.txt | 3 - src/rj_config_server/CMakeLists.txt | 2 - src/rj_control/CMakeLists.txt | 4 - src/rj_geometry/CMakeLists.txt | 3 - src/rj_joystick/CMakeLists.txt | 2 - src/rj_optimization/CMakeLists.txt | 3 - src/rj_planning/CMakeLists.txt | 2 - src/rj_planning/package.xml | 1 - src/rj_radio/CMakeLists.txt | 3 - src/rj_referee/CMakeLists.txt | 4 - src/rj_strategy/CMakeLists.txt | 3 - src/rj_ui/CMakeLists.txt | 4 - src/rj_ui/package.xml | 1 - 22 files changed, 60 insertions(+), 121 deletions(-) diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index 9dcb50fa80..b2093f7f5d 100644 --- a/launch/soccer.launch.py +++ b/launch/soccer.launch.py @@ -245,10 +245,5 @@ def generate_launch_description(): parameters=[param_config_filepath], on_exit=Shutdown(), ) - # Node( - # package="rj_benchmarking", - # executable="benchmarking_node", - - # ) ] ) diff --git a/src/rj_benchmarking/CMakeLists.txt b/src/rj_benchmarking/CMakeLists.txt index abb8a15697..3270372829 100644 --- a/src/rj_benchmarking/CMakeLists.txt +++ b/src/rj_benchmarking/CMakeLists.txt @@ -16,12 +16,14 @@ find_package(fmt REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(rj_msgs REQUIRED) +find_package(rj_constants REQUIRED) set(BENCHMARKING_DEPS fmt rclcpp spdlog rj_msgs + rj_constants ) set(BENCHMARKING_LIBS diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index 0481548c6c..9aafce3db6 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -1,11 +1,13 @@ #pragma once #include +#include #include #include #include #include #include +#include #include #include #include @@ -15,6 +17,7 @@ #include #include +#include class Registry : public rclcpp::Node { public: @@ -23,12 +26,11 @@ class Registry : public rclcpp::Node { private: void topic_callback(const rj_msgs::msg::Latency& msg); - void dump(); std::string get_curr_datetime(); // registry[robot_id][label] -> latency sampling - std::array>, 6> registry_{}; + std::array>, kNumShells> registry_{}; rclcpp::Subscription::SharedPtr subscription_{}; - int max_rows_{}; + size_t max_rows_{}; }; diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp index 7dab498153..eed0b89832 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp @@ -7,21 +7,18 @@ class RegistryPublisher { public: // Singleton pattern - static RegistryPublisher* getInstance() { - if (instance == nullptr) { - instance = new RegistryPublisher(); - } - + static std::shared_ptr getInstance() { + static std::shared_ptr instance{new RegistryPublisher()}; return instance; } - void publish(std::string label, std::int8_t robot_id, uint64_t time); + void publish(const std::string& label, uint8_t robot_id, uint64_t time); private: static RegistryPublisher* instance; // Private Constructor - RegistryPublisher(); + RegistryPublisher() = default; // Delete Copy Constructor and Assignment RegistryPublisher(const RegistryPublisher& other) = delete; diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp index efe09b89bf..6dbad0200a 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -13,11 +13,11 @@ class Timer { public: - Timer(std::string label, std::int8_t robot_id); + Timer(const std::string& label, uint8_t robot_id); ~Timer(); private: - const std::chrono::steady_clock::time_point start_{}; - std::string label_{}; - std::int8_t robot_id_{}; + std::string label_; + std::uint8_t robot_id_; + const std::chrono::steady_clock::time_point start_; }; diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index 2592430594..65a971ac08 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -1,24 +1,11 @@ #include "rj_benchmarking/registry.hpp" Registry::Registry() : rclcpp::Node{"rj_benchmarking"} { - // SPDLOG_INFO("TESTING: Registry Built"); subscription_ = this->create_subscription( "/registry", 100, std::bind(&Registry::topic_callback, this, std::placeholders::_1)); } Registry::~Registry() { - dump(); - // SPDLOG_INFO("TESTING: Registry Destroyed"); -} - -void Registry::topic_callback(const rj_msgs::msg::Latency& msg) { - registry_.at(msg.robot_id)[msg.label].push_back(msg.duration_ns); - max_rows_ = std::max(max_rows_, static_cast(registry_.at(msg.robot_id)[msg.label].size())); -} - -void Registry::dump() { - // SPDLOG_INFO("TESTING: Dump Called"); - /* 1. Make LatencyLogs Directory if not already there 2. Make latency_curr-date_curr-time folder @@ -26,56 +13,57 @@ void Registry::dump() { 3a. first row is labels 4. Make csvs for all robots */ - std::string base_path{"./latency"}; - std::filesystem::create_directories(base_path); - base_path += "/session_"; - base_path += get_curr_datetime(); - std::filesystem::create_directories(base_path); - // std::ofstream output_file; - // output_file.open(path_); - for (int i = 0; i < 6; i++) { - std::stringstream ss; - ss << "/robot_" << i << ".csv"; - std::ofstream robot_csv{base_path + ss.str()}; + if (max_rows_ != 0) + { + std::string base_path{"./latency"}; + std::filesystem::create_directories(base_path); + base_path += "/session_"; + base_path += get_curr_datetime(); + std::filesystem::create_directories(base_path); - for (int row = -1; row < max_rows_; row++) { - if (row == -1) { - for (std::pair> labels : registry_.at(i)) { - robot_csv << labels.first << ','; - } - - robot_csv << '\n'; + for (size_t i = 0; i < kNumShells; i++) { + if (registry_[i].empty()) { + continue; } - for (std::pair> labels : registry_.at(i)) { - robot_csv << labels.second[row] << ','; - } + std::string ss{}; + ss += "/robot_"; + ss += std::to_string(i); + ss += ".csv"; + std::ofstream robot_csv{base_path + ss}; + // Print out labels + for (const auto& [label, timestamps] : registry_[i]) { + robot_csv << label << ','; + } robot_csv << '\n'; - } - // output_file << "Robot " << i << '\n'; - // for (auto& p : registry_[i]) - // { - // output_file << p.first << " "; - // for (uint64_t e : p.second) - // { - // output_file << e << ", "; - // } - // output_file << "AVG: " << std::accumulate(p.second.begin(), p.second.end(), 0) - // / - // p.second.size(); - // output_file << '\n'; - // } + // Print out data row by row + for (size_t row = 0; row < max_rows_; ++row) { + for (const auto& [label, timestamps] : registry_[i]) { + robot_csv << timestamps[row] << ','; + } + + robot_csv << '\n'; + } + } } - // output_file.close(); +} + +void Registry::topic_callback(const rj_msgs::msg::Latency& msg) { + registry_[msg.robot_id][msg.label].push_back(msg.duration_ns); + max_rows_ = std::max(max_rows_, static_cast(registry_[msg.robot_id][msg.label].size())); } std::string Registry::get_curr_datetime() { - std::time_t time = std::time({}); - char timeString[std::size("yyyy-mm-ddThh:mm:ssZ")]; - std::strftime(std::data(timeString), std::size(timeString), "%FT%TZ", std::localtime(&time)); - std::string out{timeString}; - return out; + auto now = std::chrono::system_clock::now(); + std::time_t now_time = std::chrono::system_clock::to_time_t(now); + std::tm tm_buf{}; + + localtime_r(&now_time, &tm_buf); + + std::ostringstream ss; + ss << std::put_time(&tm_buf, "%Y-%m-%d_%H:%M:%S"); + return ss.str(); } diff --git a/src/rj_benchmarking/src/registry_publisher.cpp b/src/rj_benchmarking/src/registry_publisher.cpp index ea2475b0b2..fba5151c43 100644 --- a/src/rj_benchmarking/src/registry_publisher.cpp +++ b/src/rj_benchmarking/src/registry_publisher.cpp @@ -2,13 +2,8 @@ RegistryPublisher* RegistryPublisher::instance = nullptr; -RegistryPublisher::RegistryPublisher() { - // printf("Testing"); -} - -void RegistryPublisher::publish(std::string label, std::int8_t robot_id, uint64_t time) { - // idk what type this should be im just following tutorial code - auto message = rj_msgs::msg::Latency(); +void RegistryPublisher::publish(const std::string& label, u_int8_t robot_id, uint64_t time) { + rj_msgs::msg::Latency message = rj_msgs::msg::Latency(); message.label = label; message.robot_id = robot_id; diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index 748ada4a37..c3181a2b32 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -1,15 +1,12 @@ #include "rj_benchmarking/timer.hpp" -Timer::Timer(std::string label, int8_t robot_id) +Timer::Timer(const std::string& label, uint8_t robot_id) : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { - // SPDLOG_INFO("Testing: Timer Created " + label); } Timer::~Timer() { - // SPDLOG_INFO("Testing: Timer Destroyed"); uint64_t time = static_cast(std::chrono::duration_cast( std::chrono::steady_clock::now() - start_) .count()); - RegistryPublisher::getInstance()->publish(label_, robot_id_, time); } diff --git a/src/rj_common/CMakeLists.txt b/src/rj_common/CMakeLists.txt index e650d924d8..c2996bf5a3 100644 --- a/src/rj_common/CMakeLists.txt +++ b/src/rj_common/CMakeLists.txt @@ -22,7 +22,6 @@ find_package(rj_geometry REQUIRED) find_package(rj_param_utils REQUIRED) find_package(rj_utils REQUIRED) find_package(rj_drawing_msgs REQUIRED) -find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/debug_drawer.cpp @@ -52,7 +51,6 @@ ament_target_dependencies(${PROJECT_NAME} rj_param_utils rj_utils rj_drawing_msgs - rj_benchmarking ) target_link_libraries(${PROJECT_NAME} diff --git a/src/rj_config_client/CMakeLists.txt b/src/rj_config_client/CMakeLists.txt index 63ec4f3899..057a8d82bb 100644 --- a/src/rj_config_client/CMakeLists.txt +++ b/src/rj_config_client/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(fmt REQUIRED) find_package(rj_constants REQUIRED) find_package(rj_common REQUIRED) find_package(rj_utils REQUIRED) -find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/config_client.cpp @@ -54,7 +53,6 @@ ament_target_dependencies(${PROJECT_NAME} rj_constants rj_common rj_utils - rj_benchmarking ) ament_target_dependencies(${PROJECT_NAME}_node @@ -65,7 +63,6 @@ ament_target_dependencies(${PROJECT_NAME}_node rj_constants rj_common rj_utils - rj_benchmarking ) install( diff --git a/src/rj_config_server/CMakeLists.txt b/src/rj_config_server/CMakeLists.txt index 485979db87..b4c86280d4 100644 --- a/src/rj_config_server/CMakeLists.txt +++ b/src/rj_config_server/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(fmt REQUIRED) find_package(rj_constants REQUIRED) find_package(rj_common REQUIRED) find_package(rj_utils REQUIRED) -find_package(rj_benchmarking REQUIRED) add_executable(${PROJECT_NAME}_node src/config_server.cpp @@ -42,7 +41,6 @@ ament_target_dependencies(${PROJECT_NAME}_node rj_constants rj_common rj_utils - rj_benchmarking ) install( diff --git a/src/rj_control/CMakeLists.txt b/src/rj_control/CMakeLists.txt index d5b8217f84..665d7bf528 100644 --- a/src/rj_control/CMakeLists.txt +++ b/src/rj_control/CMakeLists.txt @@ -24,7 +24,6 @@ find_package(rj_common REQUIRED) find_package(rj_geometry REQUIRED) find_package(rj_planning REQUIRED) find_package(rj_convert REQUIRED) -find_package(rj_benchmarking REQUIRED) add_executable(motion_control_node src/motion_control_node.cpp @@ -74,7 +73,6 @@ ament_target_dependencies(motion_control_node rj_geometry rj_planning rj_convert - rj_benchmarking ) ament_target_dependencies(${PROJECT_NAME} @@ -91,7 +89,6 @@ ament_target_dependencies(${PROJECT_NAME} rj_geometry rj_planning rj_convert - rj_benchmarking ) install( @@ -124,7 +121,6 @@ ament_export_dependencies( rj_geometry rj_planning rj_convert - rj_benchmarking ) if(BUILD_TESTING) diff --git a/src/rj_geometry/CMakeLists.txt b/src/rj_geometry/CMakeLists.txt index 84bfd5907e..88c05e8d8f 100644 --- a/src/rj_geometry/CMakeLists.txt +++ b/src/rj_geometry/CMakeLists.txt @@ -20,7 +20,6 @@ find_package(rj_convert REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(rj_geometry_msgs REQUIRED) -find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/arc.cpp @@ -50,7 +49,6 @@ ament_target_dependencies(${PROJECT_NAME} rj_constants rj_convert rj_geometry_msgs - rj_benchmarking ) target_link_libraries(${PROJECT_NAME} @@ -83,7 +81,6 @@ ament_export_dependencies( rj_constants rj_convert rj_geometry_msgs - rj_benchmarking ) if(BUILD_TESTING) diff --git a/src/rj_joystick/CMakeLists.txt b/src/rj_joystick/CMakeLists.txt index 74eb5f0321..e2a02acad1 100644 --- a/src/rj_joystick/CMakeLists.txt +++ b/src/rj_joystick/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(rj_common REQUIRED) find_package(rj_constants REQUIRED) find_package(rj_msgs REQUIRED) find_package(rj_param_utils REQUIRED) -find_package(rj_benchmarking REQUIRED) # Find sdl2 include(FindPkgConfig) @@ -55,7 +54,6 @@ ament_target_dependencies(manual_control_node rj_msgs rj_param_utils rclcpp_components - rj_benchmarking ) install( diff --git a/src/rj_optimization/CMakeLists.txt b/src/rj_optimization/CMakeLists.txt index cf32707c4a..64fed90da3 100644 --- a/src/rj_optimization/CMakeLists.txt +++ b/src/rj_optimization/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rj_common REQUIRED) find_package(rj_geometry REQUIRED) -find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/gradient_ascent_1d.cpp @@ -47,7 +46,6 @@ ament_target_dependencies(${PROJECT_NAME} PythonLibs rj_common rj_geometry - rj_benchmarking ) install( @@ -70,7 +68,6 @@ ament_export_dependencies( PythonLibs rj_common rj_geometry - rj_benchmarking ) if(BUILD_TESTING) diff --git a/src/rj_planning/CMakeLists.txt b/src/rj_planning/CMakeLists.txt index 8446be892a..2796055865 100644 --- a/src/rj_planning/CMakeLists.txt +++ b/src/rj_planning/CMakeLists.txt @@ -25,7 +25,6 @@ find_package(rj_convert REQUIRED) find_package(rj_msgs REQUIRED) find_package(rj_param_utils REQUIRED) find_package(rj_rrt REQUIRED) -find_package(rj_benchmarking REQUIRED) set(RJ_PLANNING_DEPS rclcpp @@ -40,7 +39,6 @@ set(RJ_PLANNING_DEPS rj_msgs rj_param_utils rj_rrt - rj_benchmarking Flann ) diff --git a/src/rj_planning/package.xml b/src/rj_planning/package.xml index 147dd07784..98af846728 100644 --- a/src/rj_planning/package.xml +++ b/src/rj_planning/package.xml @@ -21,7 +21,6 @@ rj_msgs rj_param_utils rj_rrt - rj_benchmarking ament_cmake diff --git a/src/rj_radio/CMakeLists.txt b/src/rj_radio/CMakeLists.txt index a28c3561d1..0f601c0f52 100644 --- a/src/rj_radio/CMakeLists.txt +++ b/src/rj_radio/CMakeLists.txt @@ -26,7 +26,6 @@ find_package(rj_strategy REQUIRED) find_package(rj_geometry REQUIRED) find_package(rj_planning REQUIRED) find_package(rj_constants REQUIRED) -find_package(rj_benchmarking REQUIRED) add_executable(network_radio_node src/network_radio.cpp @@ -80,7 +79,6 @@ ament_target_dependencies(network_radio_node rj_geometry rj_planning rj_constants - rj_benchmarking ) ament_target_dependencies(sim_radio_node @@ -99,7 +97,6 @@ ament_target_dependencies(sim_radio_node rj_geometry rj_planning rj_constants - rj_benchmarking ) install( diff --git a/src/rj_referee/CMakeLists.txt b/src/rj_referee/CMakeLists.txt index 80ee036925..f0b5d080bf 100644 --- a/src/rj_referee/CMakeLists.txt +++ b/src/rj_referee/CMakeLists.txt @@ -22,7 +22,6 @@ find_package(rj_utils REQUIRED) find_package(rj_msgs REQUIRED) find_package(rj_protos REQUIRED) find_package(rj_config_client REQUIRED) -find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/referee_base.cpp @@ -95,7 +94,6 @@ ament_target_dependencies(${PROJECT_NAME} rj_msgs rj_protos rj_config_client - rj_benchmarking ) ament_target_dependencies(external_referee_node @@ -112,7 +110,6 @@ ament_target_dependencies(external_referee_node rj_msgs rj_protos rj_config_client - rj_benchmarking ) ament_target_dependencies(internal_referee_node @@ -129,7 +126,6 @@ ament_target_dependencies(internal_referee_node rj_msgs rj_protos rj_config_client - rj_benchmarking ) install( diff --git a/src/rj_strategy/CMakeLists.txt b/src/rj_strategy/CMakeLists.txt index 2346ab6a99..b00d47d05a 100644 --- a/src/rj_strategy/CMakeLists.txt +++ b/src/rj_strategy/CMakeLists.txt @@ -22,7 +22,6 @@ find_package(rj_common REQUIRED) find_package(rj_geometry REQUIRED) find_package(rj_param_utils REQUIRED) find_package(rj_utils REQUIRED) -find_package(rj_benchmarking REQUIRED) set(RJ_STRATEGY_DEPS rclcpp @@ -37,7 +36,6 @@ set(RJ_STRATEGY_DEPS rj_geometry rj_param_utils rj_utils - rj_benchmarking ) set(RJ_STRATEGY_SRC @@ -165,7 +163,6 @@ ament_export_dependencies( rj_geometry rj_param_utils rj_utils - rj_benchmarking ) ament_export_targets(${PROJECT_NAME}) diff --git a/src/rj_ui/CMakeLists.txt b/src/rj_ui/CMakeLists.txt index 5ffd7066d3..3ea82ab1bf 100644 --- a/src/rj_ui/CMakeLists.txt +++ b/src/rj_ui/CMakeLists.txt @@ -34,7 +34,6 @@ find_package(rj_strategy REQUIRED) find_package(rj_referee REQUIRED) find_package(rj_config_client REQUIRED) find_package(rj_topic_utils REQUIRED) -find_package(rj_benchmarking REQUIRED) # Enable Qt's automatic processing set(CMAKE_AUTOMOC ON) # Handle Q_OBJECT in headers automatically @@ -190,7 +189,6 @@ ament_target_dependencies(${PROJECT_NAME}_node rj_referee rj_config_client rj_topic_utils - rj_benchmarking ) ament_target_dependencies(log_viewer_node @@ -217,7 +215,6 @@ ament_target_dependencies(log_viewer_node rj_referee rj_config_client rj_topic_utils - rj_benchmarking ) ament_target_dependencies(${PROJECT_NAME} @@ -244,7 +241,6 @@ ament_target_dependencies(${PROJECT_NAME} rj_referee rj_config_client rj_topic_utils - rj_benchmarking ) install( diff --git a/src/rj_ui/package.xml b/src/rj_ui/package.xml index d9a52abd16..7c3b251d16 100644 --- a/src/rj_ui/package.xml +++ b/src/rj_ui/package.xml @@ -30,7 +30,6 @@ rj_referee rj_config_client rj_topic_utils - ament_cmake From 787ea2c7f106d4a01872fe838cc20400d582b85e Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal <146681831+yuvrajdhadwal@users.noreply.github.com> Date: Tue, 18 Nov 2025 00:53:15 -0500 Subject: [PATCH 16/21] Fix missing newline at end of rotate_path_planner.hpp From c4cfa5e7a3b33c12301b2c470fffb70a30061e90 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 18 Nov 2025 00:56:07 -0500 Subject: [PATCH 17/21] whoops missed some --- src/rj_ui/src/main.cpp | 1 + src/rj_utils/CMakeLists.txt | 4 +--- src/rj_vision_filter/CMakeLists.txt | 3 --- src/rj_vision_receiver/CMakeLists.txt | 2 -- 4 files changed, 2 insertions(+), 8 deletions(-) diff --git a/src/rj_ui/src/main.cpp b/src/rj_ui/src/main.cpp index 8005eac23e..1e139f20b4 100644 --- a/src/rj_ui/src/main.cpp +++ b/src/rj_ui/src/main.cpp @@ -43,6 +43,7 @@ void usage(const char* prog) { int main(int argc, char* argv[]) { printf("Starting Soccer...\n"); + // register our signal handler signal(SIGINT, signal_handler); diff --git a/src/rj_utils/CMakeLists.txt b/src/rj_utils/CMakeLists.txt index 5bf335dd25..ce21151753 100644 --- a/src/rj_utils/CMakeLists.txt +++ b/src/rj_utils/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(rj_msgs REQUIRED) find_package(rj_convert REQUIRED) find_package(rj_protos REQUIRED) find_package(Qt5 COMPONENTS Core Widgets REQUIRED) -find_package(rj_benchmarking REQUIRED) add_library(${PROJECT_NAME} SHARED src/conversions.cpp @@ -28,7 +27,6 @@ ament_target_dependencies(${PROJECT_NAME} rj_msgs rj_convert rj_protos - rj_benchmarking ) target_link_libraries(${PROJECT_NAME} @@ -44,7 +42,7 @@ target_include_directories(${PROJECT_NAME} $ ) -ament_export_dependencies(rclcpp rj_msgs rj_convert rj_protos rj_benchmarking) +ament_export_dependencies(rclcpp rj_msgs rj_convert rj_protos) install( DIRECTORY include/ diff --git a/src/rj_vision_filter/CMakeLists.txt b/src/rj_vision_filter/CMakeLists.txt index fa18ba47fd..14c74b1c2e 100644 --- a/src/rj_vision_filter/CMakeLists.txt +++ b/src/rj_vision_filter/CMakeLists.txt @@ -24,7 +24,6 @@ find_package(rclcpp_components REQUIRED) find_package(rj_config_client REQUIRED) find_package(rj_topic_utils REQUIRED) find_package(rj_constants REQUIRED) -find_package(rj_benchmarking REQUIRED) add_executable(${PROJECT_NAME}_node src/main.cpp @@ -111,7 +110,6 @@ ament_target_dependencies(${PROJECT_NAME}_node rj_config_client rj_topic_utils rj_constants - rj_benchmarking ) ament_target_dependencies(${PROJECT_NAME} @@ -130,7 +128,6 @@ ament_target_dependencies(${PROJECT_NAME} rj_config_client rj_topic_utils rj_constants - rj_benchmarking ) install( diff --git a/src/rj_vision_receiver/CMakeLists.txt b/src/rj_vision_receiver/CMakeLists.txt index be273cfc49..751526ae52 100644 --- a/src/rj_vision_receiver/CMakeLists.txt +++ b/src/rj_vision_receiver/CMakeLists.txt @@ -20,7 +20,6 @@ find_package(rj_utils REQUIRED) find_package(rj_constants REQUIRED) find_package(rj_protos REQUIRED) find_package(rj_config_client REQUIRED) -find_package(rj_benchmarking REQUIRED) set(VISION_RECEIVER_DEPS rclcpp @@ -32,7 +31,6 @@ set(VISION_RECEIVER_DEPS rj_constants rj_protos rj_config_client - rj_benchmarking ) set(VISION_RECEIVER_LIBS From 0f76cf1f57d4f8c4ef7a3332e8a3e919015651a1 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 18 Nov 2025 01:00:44 -0500 Subject: [PATCH 18/21] more cleanup --- src/rj_msgs/CMakeLists.txt | 3 +-- .../include/rj_planning/planners/rotate_path_planner.hpp | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/rj_msgs/CMakeLists.txt b/src/rj_msgs/CMakeLists.txt index c92c6d1ab2..a358f1706c 100644 --- a/src/rj_msgs/CMakeLists.txt +++ b/src/rj_msgs/CMakeLists.txt @@ -16,8 +16,7 @@ find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) rosidl_generate_interfaces( - ${PROJECT_NAME} - + ${PROJECT_NAME} # Actions action/RobotMove.action diff --git a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp index 7b30472cb0..1f39d9c137 100644 --- a/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp +++ b/src/rj_planning/include/rj_planning/planners/rotate_path_planner.hpp @@ -63,4 +63,4 @@ class RotatePathPlanner : public PathPlanner { double isDoneAngleChangeThresh{1.0}; }; -} // namespace planning +} // namespace planning \ No newline at end of file From 652e3dbc7038098a42d6501083d87dc3ab1e58d2 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 18 Nov 2025 01:04:43 -0500 Subject: [PATCH 19/21] Fix Code Style On feature/latency_benchmarking (#2464) automated style fixes Co-authored-by: yuvrajdhadwal --- src/rj_benchmarking/include/rj_benchmarking/registry.hpp | 4 ++-- src/rj_benchmarking/src/registry.cpp | 5 ++--- src/rj_benchmarking/src/timer.cpp | 3 +-- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp index 9aafce3db6..8da8ecd07b 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/registry.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -6,8 +6,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -16,8 +16,8 @@ #include #include -#include #include +#include class Registry : public rclcpp::Node { public: diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp index 65a971ac08..e38dcbe6ff 100644 --- a/src/rj_benchmarking/src/registry.cpp +++ b/src/rj_benchmarking/src/registry.cpp @@ -14,8 +14,7 @@ Registry::~Registry() { 4. Make csvs for all robots */ - if (max_rows_ != 0) - { + if (max_rows_ != 0) { std::string base_path{"./latency"}; std::filesystem::create_directories(base_path); base_path += "/session_"; @@ -35,7 +34,7 @@ Registry::~Registry() { // Print out labels for (const auto& [label, timestamps] : registry_[i]) { - robot_csv << label << ','; + robot_csv << label << ','; } robot_csv << '\n'; diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index c3181a2b32..0bc5aa0976 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -1,8 +1,7 @@ #include "rj_benchmarking/timer.hpp" Timer::Timer(const std::string& label, uint8_t robot_id) - : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) { -} + : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) {} Timer::~Timer() { uint64_t time = static_cast(std::chrono::duration_cast( From f85c032ef94bb52a0c99b2f5f9e8298da2f313fa Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Tue, 18 Nov 2025 20:01:18 -0500 Subject: [PATCH 20/21] added package dependencies --- src/rj_benchmarking/package.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/rj_benchmarking/package.xml b/src/rj_benchmarking/package.xml index 441029f447..4a81761ae0 100644 --- a/src/rj_benchmarking/package.xml +++ b/src/rj_benchmarking/package.xml @@ -11,6 +11,10 @@ rclcpp_components spdlog fmt + rosidl_default_generators + std_msgs + rj_msgs + rj_constants ament_cmake From c045a293e73d2390ff43bb52186d508bc3cb5943 Mon Sep 17 00:00:00 2001 From: Yuvraj Dhadwal Date: Wed, 19 Nov 2025 00:33:48 -0500 Subject: [PATCH 21/21] switched to ros2 timer so that it matches sim time rather than real time --- .../include/rj_benchmarking/timer.hpp | 12 +++++++++--- src/rj_benchmarking/src/timer.cpp | 9 ++++----- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp index 6dbad0200a..797af15d36 100644 --- a/src/rj_benchmarking/include/rj_benchmarking/timer.hpp +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -11,13 +11,19 @@ #include "rj_benchmarking/registry_publisher.hpp" +// Lazy static ros_clock +static rclcpp::Clock& ros_clock() { + static rclcpp::Clock clock(RCL_STEADY_TIME); + return clock; +} + class Timer { public: Timer(const std::string& label, uint8_t robot_id); ~Timer(); private: - std::string label_; - std::uint8_t robot_id_; - const std::chrono::steady_clock::time_point start_; + const std::string label_; + const std::uint8_t robot_id_; + const rclcpp::Time start_; }; diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp index 0bc5aa0976..277b051561 100644 --- a/src/rj_benchmarking/src/timer.cpp +++ b/src/rj_benchmarking/src/timer.cpp @@ -1,11 +1,10 @@ #include "rj_benchmarking/timer.hpp" Timer::Timer(const std::string& label, uint8_t robot_id) - : label_(label), robot_id_(robot_id), start_(std::chrono::steady_clock::now()) {} + : label_(label), robot_id_(robot_id), start_(ros_clock().now()) {} Timer::~Timer() { - uint64_t time = static_cast(std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_) - .count()); - RegistryPublisher::getInstance()->publish(label_, robot_id_, time); + rclcpp::Duration duration = ros_clock().now() - start_; + uint64_t time_ns = static_cast(duration.nanoseconds()); + RegistryPublisher::getInstance()->publish(label_, robot_id_, time_ns); }