eigen.h

Go to the documentation of this file.

#ifndef GRIZZLY_MSGS_EIGEN_H
#define GRIZZLY_MSGS_EIGEN_H

#include "grizzly_msgs/Drive.h"
#include <Eigen/Core>
#include <stdexcept>
#include <string>

typedef Eigen::Vector4f VectorDrive;
static const float default_stationary_threshold(0.001);

namespace grizzly_msgs
{

namespace Drives
{
  enum {
    FrontLeft = 0,
    FrontRight = 1,
    RearLeft = 2,
    RearRight = 3
  };
}

static inline VectorDrive vectorFromDriveMsg(const Drive& msg)
{
  return VectorDrive(
      msg.front_left,
      msg.front_right,
      msg.rear_left,
      msg.rear_right);
}

static inline void fillDriveMsgFromVector(const VectorDrive& vec, Drive* msg)
{
  msg->front_left = vec[Drives::FrontLeft];
  msg->front_right = vec[Drives::FrontRight];
  msg->rear_left = vec[Drives::RearLeft];
  msg->rear_right = vec[Drives::RearRight];
}

static inline Drive driveMsgFromVector(const VectorDrive& vec)
{
  Drive msg;
  fillDriveMsgFromVector(vec, &msg);
  return msg;
}

static inline std::string nameFromDriveIndex(VectorDrive::Index field)
{
  switch(field)
  {
    case Drives::FrontLeft: return "front_left";
    case Drives::FrontRight: return "front_right";
    case Drives::RearLeft: return "rear_left";
    case Drives::RearRight: return "rear_right";
    default:
      throw std::out_of_range("Passed field number not in range 0..3");
  }
}

static inline bool isStationary(const VectorDrive& vec, float threshold=default_stationary_threshold)
{ 
  return vec.cwiseAbs().maxCoeff() < threshold;
}

static inline bool isStationary(const Drive& msg, float threshold=default_stationary_threshold)
{
  return isStationary(vectorFromDriveMsg(msg), threshold);
}

}

#endif