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",

src/rj_benchmarking/CMakeLists.txt

@@ -0,0 +1,109 @@
+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)
+find_package(rj_constants REQUIRED)
+
+set(BENCHMARKING_DEPS
+    fmt
+    rclcpp
+    spdlog
+    rj_msgs
+    rj_constants
+)
+
+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,36 @@
+#pragma once
+
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ctime>
+#include <filesystem>
+#include <fstream>
+#include <iomanip>
+#include <iterator>
+#include <numeric>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <spdlog/spdlog.h>
+
+#include <rj_constants/constants.hpp>
+#include <rj_msgs/msg/latency.hpp>
+
+class Registry : public rclcpp::Node {
+public:
+    Registry();
+    ~Registry();
+
+private:
+    void topic_callback(const rj_msgs::msg::Latency& msg);
+
+    std::string get_curr_datetime();
+
+    // registry[robot_id][label] -> latency sampling
+    std::array<std::unordered_map<std::string, std::vector<uint64_t>>, kNumShells> registry_{};
+    rclcpp::Subscription<rj_msgs::msg::Latency>::SharedPtr subscription_{};
+    size_t max_rows_{};
+};

src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <rj_msgs/msg/latency.hpp>
+
+class RegistryPublisher {
+public:
+    // Singleton pattern
+    static std::shared_ptr<RegistryPublisher> getInstance() {
+        static std::shared_ptr<RegistryPublisher> instance{new RegistryPublisher()};
+        return instance;
+    }
+
+    void publish(const std::string& label, uint8_t robot_id, uint64_t time);
+
+private:
+    static RegistryPublisher* instance;
+
+    // Private Constructor
+    RegistryPublisher() = default;
+
+    // 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,29 @@
+#pragma once
+
+#include <chrono>
+#include <cstdint>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <spdlog/spdlog.h>
+
+#include <rj_msgs/msg/latency.hpp>
+
+#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:
+    const std::string label_;
+    const std::uint8_t robot_id_;
+    const rclcpp::Time start_;
+};

src/rj_benchmarking/package.xml

@@ -0,0 +1,24 @@
+<?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>
+  <depend>rosidl_default_generators</depend>
+  <depend>std_msgs</depend>
+  <depend>rj_msgs</depend>
+  <depend>rj_constants</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,11 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include "rj_benchmarking/registry.hpp"
+#include "rj_benchmarking/registry_publisher.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,68 @@
+#include "rj_benchmarking/registry.hpp"
+
+Registry::Registry() : rclcpp::Node{"rj_benchmarking"} {
+    subscription_ = this->create_subscription<rj_msgs::msg::Latency>(
+        "/registry", 100, std::bind(&Registry::topic_callback, this, std::placeholders::_1));
+}
+
+Registry::~Registry() {
+    /*
+        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
+    */
+
+    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 (size_t i = 0; i < kNumShells; i++) {
+            if (registry_[i].empty()) {
+                continue;
+            }
+
+            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';
+
+            // 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';
+            }
+        }
+    }
+}
+
+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<size_t>(registry_[msg.robot_id][msg.label].size()));
+}
+
+std::string Registry::get_curr_datetime() {
+    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();
+}

src/rj_benchmarking/src/registry_publisher.cpp

@@ -0,0 +1,13 @@
+#include "rj_benchmarking/registry_publisher.hpp"
+
+RegistryPublisher* RegistryPublisher::instance = nullptr;
+
+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;
+    message.duration_ns = time;
+
+    publisher_->publish(message);
+}

src/rj_benchmarking/src/timer.cpp

@@ -0,0 +1,10 @@
+#include "rj_benchmarking/timer.hpp"
+
+Timer::Timer(const std::string& label, uint8_t robot_id)
+    : label_(label), robot_id_(robot_id), start_(ros_clock().now()) {}
+
+Timer::~Timer() {
+    rclcpp::Duration duration = ros_clock().now() - start_;
+    uint64_t time_ns = static_cast<uint64_t>(duration.nanoseconds());
+    RegistryPublisher::getInstance()->publish(label_, robot_id_, time_ns);
+}

src/rj_msgs/CMakeLists.txt

@@ -35,6 +35,7 @@ rosidl_generate_interfaces(
   msg/ManipulatorSetpoint.msg
   msg/MatchState.msg
   msg/MotionSetpoint.msg
+  msg/Latency.msg
 
   msg/LinearMotionInstant.msg
   msg/MotionCommand.msg

src/rj_msgs/msg/Latency.msg

@@ -0,0 +1,3 @@
+string label
+uint64 duration_ns
+int8 robot_id