position.h
Go to the documentation of this file.
00001 /*
00002  *  Description:  System-wide position class definitions
00003  *
00004  *  Copyright (C) 2009 Austin Robot Technology
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  *
00008  *  $Id: position.h 1161 2011-03-26 02:10:49Z jack.oquin $
00009  */
00010 
00011 #ifndef _POSITION_H
00012 #define _POSITION_H
00013 
00021 #include <angles/angles.h>
00022 
00023 namespace Position
00024 {
00026   class Pose3D
00027   {
00028   public:
00030     double x;
00032     double y;
00034     double z;
00036     double roll;
00038     double pitch;
00040     double yaw;
00041 
00042     Pose3D(void):
00043       x(0.0), y(0.0), z(0.0), roll(0.0), pitch(0.0), yaw(0.0)
00044     {}
00045     Pose3D(const Pose3D &copy):
00046       x(copy.x), y(copy.y), z(copy.z),
00047       roll(copy.roll), pitch(copy.pitch), yaw(copy.yaw)
00048     {}
00049     Pose3D(double _x, double _y, double _z,
00050            double _roll, double _pitch, double _yaw):
00051       x(_x), y(_y), z(_z), roll(_roll), pitch(_pitch), yaw(_yaw)
00052     {}
00053 
00054     // some useful operators, define more if needed
00055     bool operator==(const Pose3D &that) const
00056     {
00057       return (this->x == that.x
00058               && this->y == that.y
00059               && this->z == that.z
00060               && this->roll == that.roll
00061               && this->pitch == that.pitch
00062               && this->yaw == that.yaw);
00063     }
00064     bool operator!=(const Pose3D &that) const
00065     {
00066       return (this->x != that.x
00067               || this->y != that.y
00068               || this->z != that.z
00069               || this->roll != that.roll
00070               || this->pitch != that.pitch
00071               || this->yaw != that.yaw);
00072     }
00073     Pose3D operator+(const Pose3D &that) const
00074     {
00075       return Pose3D(this->x + that.x,
00076                     this->y + that.y,
00077                     this->z + that.z,
00078                     angles::normalize_angle(this->roll + that.roll),
00079                     angles::normalize_angle(this->pitch + that.pitch),
00080                     angles::normalize_angle(this->yaw + that.yaw));
00081     }
00082     Pose3D operator-(const Pose3D &that) const
00083     {
00084       return Pose3D(this->x - that.x,
00085                     this->y - that.y,
00086                     this->z - that.z,
00087                     angles::normalize_angle(this->roll - that.roll),
00088                     angles::normalize_angle(this->pitch - that.pitch),
00089                     angles::normalize_angle(this->yaw - that.yaw));
00090     }
00091   };
00092 
00094   class Position3D
00095   {
00096   public:
00098     Pose3D pos;
00100     Pose3D vel;
00101   };
00102 }
00103 
00104 #endif // _POSITION_H


applanix
Author(s): Jack O'Quin, Patrick Beeson, Alberto Alonso
autogenerated on Fri Jan 3 2014 11:09:02