Scripts for managing and processing logs parameterized

README.md

@@ -35,9 +35,18 @@ rosinstall -n . srpb/srpb/srpb.rosinstall
 
 ## Usage
 
+### Setup
+
 A log file is saved once the goal is reached by the `srpb_move_base` node. Renewing the goal pose before reaching the previous one does not cause the files to be divided into parts.
 
-TBD...
+### Typical workflow
+
+If one intends to collect a bunch of logs, this is the correct workflow:
+
+* instead of running the typical `move_base`, run the `srpb_move_base` node that aggregates the `srpb_logger` modules,
+* after each trial, run the `scripts/copy_logs.sh` to copy the logs related to the newest trial into a separate directory,
+* if one wishes to be extra safe, one might want to evaluate the newest logs at this point with `scripts/evaluate_from_dir.sh <PATH TO THE DIR WITH LOGS COPIED INTO>` OR (typically) once all trials were completed, run the `scripts/evaluate_all_dirs.sh <PATH TO THE MAIN DIR WITH LOGS GROUPED INTO DIRS>`,
+* now an Excel sheet can be created with `python3 create_excel_from_results.py` script.
 
 ## Acknowledgments
 

srpb_evaluation/CMakeLists.txt

@@ -43,7 +43,7 @@ include_directories(
 add_library(${PROJECT_NAME}_lib
     # libs
     include/${PROJECT_NAME}/metric.h
-    include/${PROJECT_NAME}/metric_gaussian.h
+    include/${PROJECT_NAME}/metric_statistics.h
     include/${PROJECT_NAME}/rewinder.h
     include/${PROJECT_NAME}/utils.h
     # specific metrics
@@ -63,7 +63,7 @@ add_library(${PROJECT_NAME}_lib
     include/${PROJECT_NAME}/metrics/personal_space_instrusion.h
     include/${PROJECT_NAME}/metrics/velocity_smoothness.h
     # sources
-    src/metric_gaussian.cpp
+    src/metric_statistics.cpp
     src/rewinder.cpp
     src/utils.cpp
 )
@@ -95,8 +95,8 @@ if (CATKIN_ENABLE_TESTING)
     if(TARGET test_utils)
         target_link_libraries(test_utils ${PROJECT_NAME}_lib)
     endif()
-    catkin_add_gtest(test_metric_gaussian test/test_metric_gaussian.cpp)
-    if(TARGET test_metric_gaussian)
-        target_link_libraries(test_metric_gaussian ${PROJECT_NAME}_lib)
+    catkin_add_gtest(test_metric_statistics test/test_metric_statistics.cpp)
+    if(TARGET test_metric_statistics)
+        target_link_libraries(test_metric_statistics ${PROJECT_NAME}_lib)
     endif()
 endif()

srpb_evaluation/include/srpb_evaluation/metric_statistics.h

@@ -0,0 +1,66 @@
+#pragma once
+
+#include "srpb_evaluation/metric.h"
+
+namespace srpb {
+namespace evaluation {
+
+class MetricStatistics: public Metric {
+public:
+	MetricStatistics(
+      const std::vector<std::pair<double, logger::RobotData>>& robot_data
+  ): Metric(robot_data) {}
+
+  MetricStatistics(
+      const std::vector<std::pair<double, logger::RobotData>>& robot_data,
+      const std::vector<std::pair<double, people_msgs_utils::Person>>& people_data
+  ): Metric(robot_data, people_data) {}
+
+  MetricStatistics(
+      const std::vector<std::pair<double, logger::RobotData>>& robot_data,
+      const std::vector<std::pair<double, people_msgs_utils::Person>>& people_data,
+      const std::vector<std::pair<double, people_msgs_utils::Group>>& groups_data
+  ): Metric(robot_data, people_data, groups_data) {}
+
+  /// Returns minimum value obtained throughout the scenario
+  virtual double getValueMin() const = 0;
+
+  /// Returns maximum value obtained throughout the scenario
+  virtual double getValueMax() const = 0;
+
+  /// Returns the percentage of violations obtained throughout the scenario (considering the threshold value)
+  virtual double getViolations() const = 0;
+
+  /// Prints results
+  virtual void printResults() const = 0;
+
+  /**
+   * @brief Computes min, max, normalized metrics and counts the number of violations (above the certain threshold)
+   *
+   * @param timed_values vector of pairs with, first, a timestamp, and second, a container with values
+   * @param violation_threshold values bigger than that will be considered as violations
+   * @param violation_above_threshold selects whether the violation is counted when the value is bigger than
+   * the threshold (true, default) or when the violation is less than the threshold (false).
+   * This is specific to a certain metric (whether computes "cost" or "reward").
+   * @param max_method set to true (default) so the max element is used to normalize metrics;
+   * false means averaging over all values/occurrences in a current time step
+   *
+   * @return std::tuple<double, double, double, double> tuple with scores:
+   * 1) min value,
+   * 2) max value,
+   * 3) a metric value normalized according to the duration/execution time,
+   * 4) timing-corrected percentage of the threshold value violations
+   */
+  static std::tuple<double, double, double, double> calculateStatistics(
+    std::vector<std::pair<double, std::vector<double>>> timed_values,
+    double violation_threshold,
+    bool violation_above_threshold = true,
+    bool max_method = true
+  );
+
+protected:
+  virtual void compute() = 0;
+};
+
+} // namespace evaluation
+} // namespace srpb

srpb_evaluation/include/srpb_evaluation/metrics/formation_space_instrusion.h

@@ -1,14 +1,14 @@
 #pragma once
 
-#include "srpb_evaluation/metric_gaussian.h"
+#include "srpb_evaluation/metric_statistics.h"
 
 #include <social_nav_utils/formation_space_intrusion.h>
 
 namespace srpb {
 namespace evaluation {
 
 /// Similar to personal space intrusion but related to the group space
-class FormationSpaceIntrusion: public MetricGaussian {
+class FormationSpaceIntrusion: public MetricStatistics {
 public:
   FormationSpaceIntrusion(
     const std::vector<std::pair<double, logger::RobotData>>& robot_data,
@@ -17,9 +17,13 @@ class FormationSpaceIntrusion: public MetricGaussian {
     double group_space_threshold,
     bool max_method = true
   ):
-    MetricGaussian(robot_data, people_data, groups_data),
+    MetricStatistics(robot_data, people_data, groups_data),
     group_space_threshold_(group_space_threshold),
-    max_method_(max_method)
+    max_method_(max_method),
+    intrusion_min_(0.0),
+    intrusion_max_(0.0),
+    intrusion_total_(0.0),
+    violations_percentage_(0.0)
   {
     if (people_data.empty() || groups_data.empty()) {
       return;
@@ -118,12 +122,13 @@ class FormationSpaceIntrusion: public MetricGaussian {
 
     rewinder_.perform();
 
+    // Gaussian is computed as a "cost"; hence, "violation_above_threshold" is hard-coded to true
     std::tie(
       intrusion_min_,
       intrusion_max_,
       intrusion_total_,
       violations_percentage_
-    ) = MetricGaussian::calculateGaussianStatistics(timed_gaussians, group_space_threshold_, max_method_);
+    ) = MetricStatistics::calculateStatistics(timed_gaussians, group_space_threshold_, true, max_method_);
   }
 };
 

srpb_evaluation/include/srpb_evaluation/metrics/goal_reached.h

@@ -30,37 +30,42 @@ class GoalReached: public Metric {
 
   void printResults() const override {
     printf(
-      "Goal reached = %d [bool] (tolerance violations: position %d, orientation %d)\n",
+      "Goal reached = %d [bool] "
+      "(tolerance violations: position %d [offset %7.4f m], orientation %d [offset %7.4f rad])\n",
       static_cast<int>(!tolerance_xy_violated_ && !tolerance_yaw_violated_),
       static_cast<int>(tolerance_xy_violated_),
-      static_cast<int>(tolerance_yaw_violated_)
+      offset_xy_,
+      static_cast<int>(tolerance_yaw_violated_),
+      offset_yaw_
     );
   }
 
 protected:
   double tolerance_xy_;
   double tolerance_yaw_;
+  double offset_xy_;
+  double offset_yaw_;
   bool tolerance_xy_violated_;
   bool tolerance_yaw_violated_;
 
   void compute() override {
     rewinder_.setHandlerLastTimestamp(
       [&]() {
-        double dist_to_goal_xy = std::hypot(
+        offset_xy_ = std::hypot(
           rewinder_.getRobotCurr().getPositionX() - rewinder_.getRobotCurr().getGoalPositionX(),
           rewinder_.getRobotCurr().getPositionY() - rewinder_.getRobotCurr().getGoalPositionY()
         );
 
-        double dist_to_goal_yaw = std::abs(
+        offset_yaw_ = std::abs(
           angles::shortest_angular_distance(
             rewinder_.getRobotCurr().getOrientationYaw(),
             rewinder_.getRobotCurr().getGoalOrientationYaw()
           )
         );
 
         // save results
-        tolerance_xy_violated_ = dist_to_goal_xy > tolerance_xy_;
-        tolerance_yaw_violated_ = std::abs(dist_to_goal_yaw) > std::abs(tolerance_yaw_);
+        tolerance_xy_violated_ = offset_xy_ > tolerance_xy_;
+        tolerance_yaw_violated_ = std::abs(offset_yaw_) > std::abs(tolerance_yaw_);
       }
     );
     rewinder_.perform();

srpb_evaluation/include/srpb_evaluation/metrics/heading_change_smoothness.h

@@ -28,6 +28,11 @@ class HeadingChangeSmoothness: public Metric {
     rewinder_.setHandlerNextTimestamp(
       [&]() {
         double dt = rewinder_.getTimestampNext() - rewinder_.getTimestampCurr();
+        // if we attempt to divide by 0 (due to possibly insufficient sampling rate of the logger), then skip this one;
+        // however, this sample will still be included in the final result
+        if (dt <= 0.0) {
+          return;
+        }
         double dtheta = rewinder_.getRobotNext().getVelocityTheta() - rewinder_.getRobotCurr().getVelocityTheta();
         hsm += (std::abs(dtheta) / dt);
       }

srpb_evaluation/include/srpb_evaluation/metrics/heading_direction_disturbance.h

@@ -1,14 +1,14 @@
 #pragma once
 
-#include "srpb_evaluation/metric_gaussian.h"
+#include "srpb_evaluation/metric_statistics.h"
 
 #include <social_nav_utils/heading_direction_disturbance.h>
 
 namespace srpb {
 namespace evaluation {
 
 /// Related to velocity and direction of the robot movement towards person
-class HeadingDirectionDisturbance: public MetricGaussian {
+class HeadingDirectionDisturbance: public MetricStatistics {
 public:
   HeadingDirectionDisturbance(
     const std::vector<std::pair<double, logger::RobotData>>& robot_data,
@@ -20,13 +20,17 @@ class HeadingDirectionDisturbance: public MetricGaussian {
     double robot_max_speed = social_nav_utils::HeadingDirectionDisturbance::MAX_SPEED_DEFAULT,
     bool max_method = true
   ):
-    MetricGaussian(robot_data, people_data),
+    MetricStatistics(robot_data, people_data),
     disturbance_threshold_(disturbance_threshold),
     person_occupancy_radius_(person_occupancy_radius),
     person_fov_(person_fov),
     robot_circumradius_(robot_circumradius),
     robot_max_speed_(robot_max_speed),
-    max_method_(max_method)
+    max_method_(max_method),
+    disturbance_min_(0.0),
+    disturbance_max_(0.0),
+    disturbance_total_(0.0),
+    violations_percentage_(0.0)
   {
     if (people_data.empty()) {
       return;
@@ -127,12 +131,13 @@ class HeadingDirectionDisturbance: public MetricGaussian {
     );
     rewinder_.perform();
 
+    // Gaussian is computed as a "cost"; hence, "violation_above_threshold" is hard-coded to true
     std::tie(
       disturbance_min_,
       disturbance_max_,
       disturbance_total_,
       violations_percentage_
-    ) = MetricGaussian::calculateGaussianStatistics(timed_disturbances, disturbance_threshold_, max_method_);
+    ) = MetricStatistics::calculateStatistics(timed_disturbances, disturbance_threshold_, true, max_method_);
   }
 };