Feature/latency benchmarking

.gitignore

@@ -55,4 +55,5 @@ compile_commands.json
 # Colcon Build Things
 build/
 install/
-log/
+log/
+latency/

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
+unset _colcon_prefix_chain_bash_source_script

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
+unset _colcon_prefix_chain_zsh_source_script

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",
+
+            # )
         ]
     )

src/rj_benchmarking/CMakeLists.txt

@@ -0,0 +1,107 @@
+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()
+
+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
+    fmt
+    spdlog
+)
+
+set(BENCHMARKING_SRC
+    src/registry.cpp
+    src/timer.cpp
+    src/registry_publisher.cpp
+)
+
+# RJ Benchmarking Library
+add_library(${PROJECT_NAME} SHARED
+    ${BENCHMARKING_SRC}
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+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_include_directories(${PROJECT_NAME}_node
+    PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+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()

src/rj_benchmarking/include/rj_benchmarking/registry.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <rj_msgs/msg/latency.hpp>
+
+#include <spdlog/spdlog.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+#include <unordered_map>
+#include <array>
+#include <vector>
+#include <numeric>
+#include <fstream>
+#include <cstdint>
+#include <filesystem>
+#include <ctime>
+#include <iterator>
+
+class Registry : public rclcpp::Node
+{
+public:
+    Registry();
+    ~Registry();
+
+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<std::unordered_map<std::string, std::vector<uint64_t>>, 6> registry_{};
+    rclcpp::Subscription<rj_msgs::msg::Latency>::SharedPtr subscription_{};
+    int max_rows_{};
+};

src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <rj_msgs/msg/latency.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+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<rclcpp::Node> node_ = std::make_shared<rclcpp::Node>(
+                                                                "rj_benchmarking_publisher");
+    rclcpp::Publisher<rj_msgs::msg::Latency>::SharedPtr publisher_
+        = node_->create_publisher<rj_msgs::msg::Latency>("/registry", 100);
+
+};

src/rj_benchmarking/include/rj_benchmarking/timer.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "rj_benchmarking/registry_publisher.hpp"
+#include <rj_msgs/msg/latency.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+#include <spdlog/spdlog.h>
+
+#include <string>
+#include <chrono>
+#include <cstdint>
+
+class Timer
+{
+
+public:
+  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_{};
+};

src/rj_benchmarking/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rj_benchmarking</name>
+  <version>0.0.0</version>
+  <description>Node for Benchmarking</description>
+  <maintainer email="[email protected]">Yuvraj Dhadwal</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>spdlog</depend>
+  <depend>fmt</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rj_benchmarking/src/main.cpp

@@ -0,0 +1,12 @@
+#include "rj_benchmarking/registry.hpp"
+#include "rj_benchmarking/registry_publisher.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+int main(int argc, char* argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<Registry>());
+    rclcpp::shutdown();
+    return 0;
+}

src/rj_benchmarking/src/registry.cpp

@@ -0,0 +1,94 @@
+#include "rj_benchmarking/registry.hpp"
+
+Registry::Registry() : rclcpp::Node{"rj_benchmarking"}
+{
+    // SPDLOG_INFO("TESTING: Registry Built");
+    subscription_ = this->create_subscription<rj_msgs::msg::Latency>(
+            "/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<int>(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
+        3. Make csv for Robot 1
+            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() };
+
+        for (int row = -1; row < max_rows_; row++)
+        {
+            if (row == -1)
+            {
+                for (std::pair<std::string, std::vector<uint64_t>> labels : registry_.at(i))
+                {
+                    robot_csv << labels.first << ',';
+                }
+
+                robot_csv << '\n';
+            }
+
+            for (std::pair<std::string, std::vector<uint64_t>> labels : registry_.at(i))
+            {
+                robot_csv << labels.second[row] << ',';
+            }
+
+            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();
+}
+
+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;
+}