eigen.h
Go to the documentation of this file.
00001 
00026 #ifndef GRIZZLY_MSGS_EIGEN_H
00027 #define GRIZZLY_MSGS_EIGEN_H
00028 
00029 #include "grizzly_msgs/Drive.h"
00030 #include <Eigen/Core>
00031 #include <stdexcept>
00032 #include <string>
00033 
00034 typedef Eigen::Vector4f VectorDrive;
00035 static const float default_stationary_threshold(0.001);
00036 
00037 namespace grizzly_msgs
00038 {
00039 
00040 namespace Drives
00041 {
00042   enum {
00043     FrontLeft = 0,
00044     FrontRight = 1,
00045     RearLeft = 2,
00046     RearRight = 3
00047   };
00048 }
00049 
00050 static inline VectorDrive vectorFromDriveMsg(const Drive& msg)
00051 {
00052   return VectorDrive(
00053       msg.front_left,
00054       msg.front_right,
00055       msg.rear_left,
00056       msg.rear_right);
00057 }
00058 
00059 static inline void fillDriveMsgFromVector(const VectorDrive& vec, Drive* msg)
00060 {
00061   msg->front_left = vec[Drives::FrontLeft];
00062   msg->front_right = vec[Drives::FrontRight];
00063   msg->rear_left = vec[Drives::RearLeft];
00064   msg->rear_right = vec[Drives::RearRight];
00065 }
00066 
00067 static inline Drive driveMsgFromVector(const VectorDrive& vec)
00068 {
00069   Drive msg;
00070   fillDriveMsgFromVector(vec, &msg);
00071   return msg;
00072 }
00073 
00074 static inline std::string nameFromDriveIndex(VectorDrive::Index field)
00075 {
00076   switch(field)
00077   {
00078     case Drives::FrontLeft: return "front-left";
00079     case Drives::FrontRight: return "front-right";
00080     case Drives::RearLeft: return "rear-left";
00081     case Drives::RearRight: return "rear-right";
00082     default:
00083       throw std::out_of_range("Passed field number not in range 0..3");
00084   }
00085 }
00086 
00087 static inline bool isStationary(const VectorDrive& vec, float threshold=default_stationary_threshold)
00088 { 
00089   return vec.cwiseAbs().maxCoeff() < threshold;
00090 }
00091 
00092 static inline bool isStationary(const Drive& msg, float threshold=default_stationary_threshold)
00093 {
00094   return isStationary(vectorFromDriveMsg(msg), threshold);
00095 }
00096 
00097 }
00098 
00099 #endif


grizzly_msgs
Author(s):
autogenerated on Fri Aug 28 2015 10:55:26