waypoint.h

Go to the documentation of this file.

#ifndef MPL_WAYPOINT_H
#define MPL_WAYPOINT_H
#include <bitset>
#include <boost/functional/hash.hpp>
#include <iostream>

#include "control.h"
#include "data_type.h"

template <int Dim>
struct Waypoint {
  Waypoint() : control(Control::NONE) {}
  Waypoint(Control::Control c) : control(c) {}

  Vecf<Dim> pos{Vecf<Dim>::Zero()};  
  Vecf<Dim> vel{Vecf<Dim>::Zero()};  
  Vecf<Dim> acc{Vecf<Dim>::Zero()};  
  Vecf<Dim> jrk{Vecf<Dim>::Zero()};  
  decimal_t yaw{0};                  
  decimal_t t{0};  

  union {
    struct {
      bool use_pos : 1;  
      bool use_vel : 1;  
      bool use_acc : 1;  
      bool use_jrk : 1;  
      bool use_yaw : 1;  
    };
    Control::Control control : 5;  
  };

  bool enable_t{false};  

  void print(std::string str = "") const {
    if (!str.empty()) std::cout << str << std::endl;
    if (use_pos) std::cout << "pos: " << pos.transpose() << std::endl;
    if (use_vel) std::cout << "vel: " << vel.transpose() << std::endl;
    if (use_acc) std::cout << "acc: " << acc.transpose() << std::endl;
    if (use_jrk) std::cout << "jrk: " << jrk.transpose() << std::endl;
    if (use_yaw) std::cout << "yaw: " << yaw << std::endl;
    if (enable_t) std::cout << " t: " << t << std::endl;

    if (control == Control::VEL)
      std::cout << "use vel!" << std::endl;
    else if (control == Control::ACC)
      std::cout << "use acc!" << std::endl;
    else if (control == Control::JRK)
      std::cout << "use jrk!" << std::endl;
    else if (control == Control::SNP)
      std::cout << "use snp!" << std::endl;
    else if (control == Control::VELxYAW)
      std::cout << "use vel & yaw!" << std::endl;
    else if (control == Control::ACCxYAW)
      std::cout << "use acc & yaw!" << std::endl;
    else if (control == Control::JRKxYAW)
      std::cout << "use jrk & yaw!" << std::endl;
    else if (control == Control::SNPxYAW)
      std::cout << "use snp & yaw!" << std::endl;
    else
      std::cout << "use null!" << std::endl;
  }
};

template <int Dim>
std::size_t hash_value(const Waypoint<Dim>& key) {
  std::size_t val = 0;
  for (int i = 0; i < Dim; i++) {
    if (key.use_pos) {
      int id = std::round(key.pos(i) / 0.01);
      boost::hash_combine(val, id);
    }
    if (key.use_vel) {
      int id = std::round(key.vel(i) / 0.1);
      boost::hash_combine(val, id);
    }
    if (key.use_acc) {
      int id = std::round(key.acc(i) / 0.1);
      boost::hash_combine(val, id);
    }
    if (key.use_jrk) {
      int id = std::round(key.jrk(i) / 0.1);
      boost::hash_combine(val, id);
    }
  }

  if (key.use_yaw) {
    int id = std::round(key.yaw / 0.1);
    boost::hash_combine(val, id);
  }

  if (key.enable_t) {
    int id = std::round(key.t / 0.1);
    boost::hash_combine(val, id);
  }

  return val;
}

template <int Dim>
bool operator==(const Waypoint<Dim>& l, const Waypoint<Dim>& r) {
  return hash_value(l) == hash_value(r);
}

template <int Dim>
bool operator!=(const Waypoint<Dim>& l, const Waypoint<Dim>& r) {
  return hash_value(l) != hash_value(r);
}

typedef Waypoint<2> Waypoint2D;

typedef Waypoint<3> Waypoint3D;

#endif