TrajectoryMetric.h

Go to the documentation of this file.

#pragma once
 
#include <ompl/control/PathControl.h>
#include <ompl/geometric/PathGeometric.h>
 
#include "steer_functions/Steering.h"
#include "utils/PlannerUtils.hpp"
 
template <class METRIC, bool MoreIsBetter = false>
class TMetric {
 public:
  static constexpr double ComparisonTolerance = 1e-6;
 
  static double evaluate(const ompl::geometric::PathGeometric &trajectory,
                         double dt = 0.1) {
    return METRIC::evaluateMetric(trajectory, dt);
  }
 
  static double evaluate(const ompl::control::PathControl &trajectory,
                         double dt = 0.1) {
    return METRIC::evaluateMetric(trajectory, dt);
  }
 
  static int compare(const ompl::geometric::PathGeometric &a,
                     const ompl::geometric::PathGeometric &b, double dt = 0.1) {
    double va = evaluate(a, dt), vb = evaluate(b, dt);
    if (std::abs(va - vb) < ComparisonTolerance) return 0;
    if (MoreIsBetter && va > vb) return 1;
    return -1;
  }
 
  static int compare(const ompl::control::PathControl &a,
                     const ompl::control::PathControl &b, double dt = 0.1) {
    double va = evaluate(a, dt), vb = evaluate(b, dt);
    if (std::abs(va - vb) < ComparisonTolerance) return 0;
    if (MoreIsBetter && va > vb) return 1;
    return -1;
  }
};