Asoragna/sync from mauro

composition_benchmark/apps/base_publisher_main.cpp

@@ -51,7 +51,7 @@ int main(int argc, char ** argv)
     performance_test::SpinType::SPIN);
 
   system->add_nodes(nodes);
-  system->spin(MAX_HOURS, false, false);
+  system->spin(MAX_HOURS, false);
 
   rclcpp::shutdown();
 }

composition_benchmark/apps/base_subscriber_main.cpp

@@ -47,7 +47,7 @@ int main(int argc, char ** argv)
     performance_test::SpinType::SPIN);
 
   system->add_nodes(nodes);
-  system->spin(MAX_HOURS, false, false);
+  system->spin(MAX_HOURS, false);
 
   rclcpp::shutdown();
 }

composition_benchmark/apps/composable_pub_sub_main.cpp

@@ -74,7 +74,7 @@ int main(int argc, char ** argv)
     spin_type);
 
   system->add_nodes(nodes);
-  system->spin(MAX_HOURS, false, false);
+  system->spin(MAX_HOURS, false);
 
   rclcpp::shutdown();
 }

composition_benchmark/apps/multi-producer-node.cpp

@@ -71,7 +71,7 @@ int main(int argc, char ** argv)
     performance_test::SpinType::SPIN);
 
   system->add_node(nodes);
-  system->spin(std::chrono::seconds(20), false, false);
+  system->spin(std::chrono::seconds(20), false);
 
   ru_logger.stop();
 

irobot_benchmark/CMakeLists.txt

@@ -37,7 +37,8 @@ set(TOPOLOGY_FILES
 
 install(FILES
   ${TOPOLOGY_FILES}
-  DESTINATION lib/${PROJECT_NAME}/topology)
+  DESTINATION lib/${PROJECT_NAME}/topology
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

irobot_benchmark/src/irobot_benchmark.cpp

@@ -46,14 +46,19 @@ parse_options(int argc, char ** argv)
 
 static
 std::string
-create_result_directory(const std::string & topology_json)
+create_result_directory(const std::string & topology_json, const std::string & result_dir)
 {
-  const size_t last_slash =
-    topology_json.find_last_of("/");
-  const std::string topology_basename =
-    topology_json.substr(last_slash + 1, topology_json.length());
-  const std::string result_dir_name =
-    topology_basename.substr(0, topology_basename.length() - 5) + "_log";
+  std::string result_dir_name;
+
+  if (!result_dir.empty()) {
+    result_dir_name = result_dir;
+  } else {
+    size_t last_slash = topology_json.find_last_of("/");
+    std::string topology_basename =
+      topology_json.substr(last_slash + 1, topology_json.length());
+    result_dir_name =
+      topology_basename.substr(0, topology_basename.length() - 5) + "_log";
+  }
 
   const std::string make_dir_cmd = "mkdir -p " + result_dir_name;
   const int ret = system(make_dir_cmd.c_str());
@@ -136,11 +141,10 @@ int main(int argc, char ** argv)
   std::string topology_json = options.topology_json_list[process_index];
 
   // Create results dir based on the topology name
-  std::string result_dir_name = create_result_directory(topology_json);
+  std::string result_dir_name = create_result_directory(topology_json, options.result_folder_name);
   // Define output paths
   std::string resources_output_path = result_dir_name + "/resources.txt";
   std::string events_output_path = result_dir_name + "/events.txt";
-  std::string latency_all_output_path = result_dir_name + "/latency_all.txt";
   std::string latency_total_output_path = result_dir_name + "/latency_total.txt";
   std::string metadata_output_path = result_dir_name + "/metadata.txt";
 
@@ -159,13 +163,13 @@ int main(int argc, char ** argv)
   // Create ROS 2 system manager
   auto ros2_system = create_ros2_system(options, events_output_path);
   ros2_system->add_nodes(nodes_vec);
+  ros2_system->set_latency_callback(ru_logger);
 
   // now the system is complete and we can make it spin for the requested duration
   bool wait_for_discovery = true;
   ros2_system->spin(
     std::chrono::seconds(options.duration_sec),
-    wait_for_discovery,
-    options.name_threads);
+    wait_for_discovery);
 
   // terminate the experiment
   ru_logger.stop();
@@ -180,7 +184,7 @@ int main(int argc, char ** argv)
   }
 
   std::cout << std::endl;
-  ros2_system->save_latency_all_stats(latency_all_output_path);
+  ros2_system->save_latency_all_stats(result_dir_name);
   ros2_system->save_latency_total_stats(latency_total_output_path);
 
   // Parent process: wait for children to exit and print system stats
@@ -189,6 +193,6 @@ int main(int argc, char ** argv)
       waitpid(getpid() + 1, &pid, 0);
     }
     std::cout << "System total:" << std::endl;
-    ros2_system->print_aggregate_stats(options.topology_json_list);
+    ros2_system->print_aggregate_stats(options.topology_json_list, result_dir_name);
   }
 }

irobot_interfaces_plugin/CMakeLists.txt

@@ -26,10 +26,13 @@ set( CUSTOM_MSGS
   "msg/Stamped9Float32.msg"
   "msg/Stamped12Float32.msg"
   "msg/Stamped10b.msg"
+  "msg/Stamped50b.msg"
   "msg/Stamped100b.msg"
   "msg/Stamped250b.msg"
   "msg/Stamped1kb.msg"
+  "msg/Stamped5kb.msg"
   "msg/Stamped10kb.msg"
+  "msg/Stamped25kb.msg"
   "msg/Stamped50kb.msg"
   "msg/Stamped100kb.msg"
   "msg/Stamped250kb.msg"
@@ -44,15 +47,26 @@ set( CUSTOM_MSGS
 
 set ( CUSTOM_SRVS
   "srv/Stamped10b.srv"
+  "srv/Stamped100kb.srv"
+  "srv/Stamped1mb.srv"
+  "srv/Stamped4mb.srv"
+)
+
+set( CUSTOM_ACTIONS
+  "action/Stamped10b.action"
+  "action/Stamped100kb.action"
+  "action/Stamped1mb.action"
+  "action/Stamped4mb.action"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   ${CUSTOM_MSGS}
   ${CUSTOM_SRVS}
+  ${CUSTOM_ACTIONS}
   DEPENDENCIES std_msgs performance_test_msgs
 )
 
-generate_factory_plugin("${CUSTOM_MSGS}" "${CUSTOM_SRVS}")
+generate_factory_plugin("${CUSTOM_MSGS}" "${CUSTOM_SRVS}" "${CUSTOM_ACTIONS}")
 
 ament_export_dependencies(rosidl_default_runtime)
 

irobot_interfaces_plugin/action/Stamped100kb.action

@@ -0,0 +1,11 @@
+#goal definition
+performance_test_msgs/PerformanceHeader header
+byte[102400] data
+---
+#result definition
+performance_test_msgs/PerformanceHeader header
+byte[102400] data
+---
+#feedback
+performance_test_msgs/PerformanceHeader header
+byte[102400] data

irobot_interfaces_plugin/action/Stamped10b.action

@@ -0,0 +1,11 @@
+#goal definition
+performance_test_msgs/PerformanceHeader header
+byte[10] data
+---
+#result definition
+performance_test_msgs/PerformanceHeader header
+byte[10] data
+---
+#feedback
+performance_test_msgs/PerformanceHeader header
+byte[10] data

irobot_interfaces_plugin/action/Stamped1mb.action

@@ -0,0 +1,11 @@
+#goal definition
+performance_test_msgs/PerformanceHeader header
+byte[1048576] data
+---
+#result definition
+performance_test_msgs/PerformanceHeader header
+byte[1048576] data
+---
+#feedback
+performance_test_msgs/PerformanceHeader header
+byte[1048576] data

irobot_interfaces_plugin/action/Stamped4mb.action

@@ -0,0 +1,11 @@
+#goal definition
+performance_test_msgs/PerformanceHeader header
+byte[4194304] data
+---
+#result definition
+performance_test_msgs/PerformanceHeader header
+byte[4194304] data
+---
+#feedback
+performance_test_msgs/PerformanceHeader header
+byte[4194304] data

irobot_interfaces_plugin/msg/Stamped25kb.msg

@@ -0,0 +1,2 @@
+performance_test_msgs/PerformanceHeader header
+byte[25000] data

irobot_interfaces_plugin/msg/Stamped50b.msg

@@ -0,0 +1,2 @@
+performance_test_msgs/PerformanceHeader header
+byte[50] data

irobot_interfaces_plugin/msg/Stamped5kb.msg

@@ -0,0 +1,2 @@
+performance_test_msgs/PerformanceHeader header
+byte[5000] data

irobot_interfaces_plugin/srv/Stamped100kb.srv

@@ -0,0 +1,5 @@
+performance_test_msgs/PerformanceHeader header
+byte[102400] data
+---
+performance_test_msgs/PerformanceHeader header
+byte[102400] data

irobot_interfaces_plugin/srv/Stamped1mb.srv

@@ -0,0 +1,5 @@
+performance_test_msgs/PerformanceHeader header
+byte[1048576] data
+---
+performance_test_msgs/PerformanceHeader header
+byte[1048576] data

irobot_interfaces_plugin/srv/Stamped4mb.srv

@@ -0,0 +1,5 @@
+performance_test_msgs/PerformanceHeader header
+byte[4194304] data
+---
+performance_test_msgs/PerformanceHeader header
+byte[4194304] data

performance_metrics/include/performance_metrics/events_logger.hpp

@@ -33,7 +33,8 @@ class EventsLogger
     late_message,
     too_late_message,
     lost_messages,
-    service_unavailable
+    service_unavailable,
+    action_unavailable
   };
 
   struct Event

performance_metrics/include/performance_metrics/resource_usage_logger.hpp

@@ -16,6 +16,7 @@
 #include <atomic>
 #include <chrono>
 #include <fstream>
+#include <functional>
 #include <iostream>
 #include <string>
 #include <thread>
@@ -35,6 +36,7 @@ class ResourceUsageLogger
     uint64_t mem_mmap_KB = 0;
     uint64_t mem_max_rss_KB = 0;
     uint64_t mem_virtual_KB = 0;
+    uint64_t latency_us = 0;
   };
 
   ResourceUsageLogger() = delete;
@@ -45,6 +47,8 @@ class ResourceUsageLogger
 
   void start(std::chrono::milliseconds period = std::chrono::milliseconds(1000));
 
+  void set_get_latency_callback(std::function<uint64_t()> get_latency_fn);
+
   void stop();
 
   void print_resource_usage();
@@ -75,6 +79,7 @@ class ResourceUsageLogger
   std::chrono::time_point<std::chrono::steady_clock> m_t1_real_start;
   pid_t m_pid;
   int m_pagesize;
+  std::function<uint64_t()> m_get_average_latency_func;
 
   // the following values are used for comparing different plots using the python scripts
   bool m_has_system_info {false};

performance_metrics/include/performance_metrics/stat_logger.hpp

@@ -39,6 +39,9 @@ void log_trackers_latency_total_stats(
   const std::vector<Tracker> & trackers,
   const bool csv_out = false);
 
+uint64_t get_trackers_avg_latency(
+  const std::vector<const Tracker *> & trackers);
+
 template<typename T>
 void stream_out(
   const bool csv_out,

performance_metrics/include/performance_metrics/tracker.hpp

@@ -76,6 +76,14 @@ class Tracker
 
   Stat<uint64_t> stat() const {return m_stat;}
 
+  uint64_t delta_received() const {return m_delta_received_messages;}
+
+  void reset_delta_received() const {m_delta_received_messages = 0;}
+
+  Stat<uint64_t> delta_stat() const {return m_delta_stat;}
+
+  void reset_delta_stat() const {m_delta_stat = Stat<uint64_t>();}
+
   double throughput() const;
 
   void set_frequency(float f) {m_frequency = f;}
@@ -99,6 +107,10 @@ class Tracker
   std::string m_topic_srv_name;
   Options m_tracking_options;
 
+  // TODO: fix this `mutable` hack.
+  // We should allow for shared ownership of the trackers
+  mutable Stat<uint64_t> m_delta_stat;
+  mutable uint64_t m_delta_received_messages = 0;
   uint64_t m_last_latency = 0;
   uint64_t m_lost_messages = 0;
   uint64_t m_received_messages = 0;

performance_metrics/src/resource_usage_logger.cpp

@@ -75,6 +75,12 @@ void ResourceUsageLogger::start(std::chrono::milliseconds period)
     });
 }
 
+void ResourceUsageLogger::set_get_latency_callback(
+  std::function<uint64_t()> get_average_latency)
+{
+  m_get_average_latency_func = get_average_latency;
+}
+
 void ResourceUsageLogger::stop()
 {
   bool is_logging = m_is_logging.exchange(false);
@@ -155,6 +161,11 @@ void ResourceUsageLogger::_get()
   } else {
     m_resources.mem_virtual_KB = -1;
   }
+
+  // Call the latency callback to get the average latency in microseconds
+  if (m_get_average_latency_func) {
+    m_resources.latency_us = m_get_average_latency_func();
+  }
 }
 
 template<typename T>
@@ -176,6 +187,7 @@ void ResourceUsageLogger::_print_header(std::ostream & stream)
 {
   _stream_out(stream, "time_ms");
   _stream_out(stream, "cpu_perc", m_narrow_space);
+  _stream_out(stream, "latency_us", m_wide_space);
   _stream_out(stream, "arena_KB");
   _stream_out(stream, "in_use_KB");
   _stream_out(stream, "mmap_KB");
@@ -197,6 +209,7 @@ void ResourceUsageLogger::_print(std::ostream & stream)
 
   _stream_out(stream, std::round(m_resources.elasped_ms), m_wide_space, prec);
   _stream_out(stream, m_resources.cpu_usage, m_narrow_space);
+  _stream_out(stream, m_resources.latency_us, m_wide_space);
   _stream_out(stream, m_resources.mem_arena_KB);
   _stream_out(stream, m_resources.mem_in_use_KB);
   _stream_out(stream, m_resources.mem_mmap_KB);

performance_metrics/src/stat_logger.cpp

@@ -166,4 +166,24 @@ void log_trackers_latency_total_stats(
     average_latency, stream, csv_out);
 }
 
+uint64_t get_trackers_avg_latency(const std::vector<const Tracker *> & trackers)
+{
+  uint64_t total_received = 0;
+  double total_latency = 0;
+
+  for (const auto & tracker : trackers) {
+    total_received += tracker->delta_received();
+    total_latency += tracker->delta_received() * tracker->delta_stat().mean();
+    // Reset values after read
+    tracker->reset_delta_received();
+    tracker->reset_delta_stat();
+  }
+
+  if (total_received) {
+    return std::round(total_latency / total_received);
+  } else {
+    return 0;
+  }
+}
+
 }  // namespace performance_metrics