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/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/launch/soccer.launch.py b/launch/soccer.launch.py index f8f992d0f8..b2093f7f5d 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", diff --git a/src/rj_benchmarking/CMakeLists.txt b/src/rj_benchmarking/CMakeLists.txt new file mode 100644 index 0000000000..3270372829 --- /dev/null +++ b/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 + $ + $ +) + +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 + $ + $ +) + +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/registry.hpp b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp new file mode 100644 index 0000000000..8da8ecd07b --- /dev/null +++ b/src/rj_benchmarking/include/rj_benchmarking/registry.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +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>, kNumShells> registry_{}; + rclcpp::Subscription::SharedPtr subscription_{}; + 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 new file mode 100644 index 0000000000..eed0b89832 --- /dev/null +++ b/src/rj_benchmarking/include/rj_benchmarking/registry_publisher.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include + +#include + +class RegistryPublisher { +public: + // Singleton pattern + static std::shared_ptr getInstance() { + static std::shared_ptr 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 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 new file mode 100644 index 0000000000..797af15d36 --- /dev/null +++ b/src/rj_benchmarking/include/rj_benchmarking/timer.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include +#include + +#include +#include + +#include + +#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_; +}; diff --git a/src/rj_benchmarking/package.xml b/src/rj_benchmarking/package.xml new file mode 100644 index 0000000000..4a81761ae0 --- /dev/null +++ b/src/rj_benchmarking/package.xml @@ -0,0 +1,24 @@ + + + + rj_benchmarking + 0.0.0 + Node for Benchmarking + Yuvraj Dhadwal + TODO: License declaration + + rclcpp + rclcpp_components + spdlog + fmt + rosidl_default_generators + std_msgs + rj_msgs + rj_constants + + 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..6087e90474 --- /dev/null +++ b/src/rj_benchmarking/src/main.cpp @@ -0,0 +1,11 @@ +#include + +#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()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/rj_benchmarking/src/registry.cpp b/src/rj_benchmarking/src/registry.cpp new file mode 100644 index 0000000000..e38dcbe6ff --- /dev/null +++ b/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( + "/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(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(); +} diff --git a/src/rj_benchmarking/src/registry_publisher.cpp b/src/rj_benchmarking/src/registry_publisher.cpp new file mode 100644 index 0000000000..fba5151c43 --- /dev/null +++ b/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); +} diff --git a/src/rj_benchmarking/src/timer.cpp b/src/rj_benchmarking/src/timer.cpp new file mode 100644 index 0000000000..277b051561 --- /dev/null +++ b/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(duration.nanoseconds()); + RegistryPublisher::getInstance()->publish(label_, robot_id_, time_ns); +} diff --git a/src/rj_msgs/CMakeLists.txt b/src/rj_msgs/CMakeLists.txt index 8d7117d82f..a358f1706c 100644 --- a/src/rj_msgs/CMakeLists.txt +++ b/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 diff --git a/src/rj_msgs/msg/Latency.msg b/src/rj_msgs/msg/Latency.msg new file mode 100644 index 0000000000..9f6dc46d86 --- /dev/null +++ b/src/rj_msgs/msg/Latency.msg @@ -0,0 +1,3 @@ +string label +uint64 duration_ns +int8 robot_id