lse_xsens_mti: /home/laradmin/lar/sensors/imu/lse_xsens

00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2010, ISR University of Coimbra.
00006 *  Copyright (c) 2011-2012, INRIA, CNRS, all rights reserved
00007 *  All rights reserved.
00008 *
00009 *  Redistribution and use in source and binary forms, with or without
00010 *  modification, are permitted provided that the following conditions
00011 *  are met:
00012 *
00013 *   * Redistributions of source code must retain the above copyright
00014 *     notice, this list of conditions and the following disclaimer.
00015 *   * Redistributions in binary form must reproduce the above
00016 *     copyright notice, this list of conditions and the following
00017 *     disclaimer in the documentation and/or other materials provided
00018 *     with the distribution.
00019 *   * Neither the name of the ISR University of Coimbra nor the names of its
00020 *     contributors may be used to endorse or promote products derived
00021 *     from this software without specific prior written permission.
00022 *
00023 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 *  POSSIBILITY OF SUCH DAMAGE.
00035 *
00036 * Author: Gonçalo Cabrita
00037 * Notes: Original code in Cocoa from 07/08/2009, went C++ on 10/11/2010
00038 *
00039 * Author: Nicolas Vignard on 2 MAY 2012
00040 * Notes: Add the ability to control the Mti-G
00041 *********************************************************************/
00042 #include <cereal_port/CerealPort.h>
00043 #include "MTMessage.h"
00044 #include <tf/transform_broadcaster.h>
00045 #include <tf/transform_listener.h>
00046 #include <nav_msgs/Odometry.h>
00047 #include <gps_common/conversions.h>
00048 #include <sensor_msgs/Imu.h>
00049 #include <sensor_msgs/NavSatFix.h>
00050 #include <sensor_msgs/NavSatStatus.h>
00051 
00052 #include <string>
00053 #include <vector>
00054 
00055 #define PRE 0xFA
00056 #define BID 0xFF
00057 
00058 namespace Xsens
00059 {
00060 static const int GPS_PVT_DATA_OFFSET = 44;
00061 static const unsigned char SELF_TEST = 0x01;  
00062 static const unsigned char XKF_VALID = 0x02; 
00063 static const unsigned char GPS_FIX   = 0x04; 
00064 static const int ONE_BYTE = 1;
00065 static const std::string IMU_FRAME_ID = "/base_imu";
00066 static const std::string BASE_LINK_FRAME_ID = "/base_link";
00067 static const std::string ODOMETRY_FRAME_ID = "/odom";
00068 
00069 
00070 typedef enum _Scenario
00071 {
00072     General = 1,
00073     Automotive = 2,
00074     Aerospace = 3,
00075     Human = 4,
00076     Human_large_accelerations = 5,
00077     Machine = 6,
00078     Machine_nomagfield = 7,
00079     Marine_MTIMTX = 8,
00080     General_nobaro = 9,
00081     Aerospace_nobaro = 10,
00082     Automotive_nobaro = 11,
00083     Marine_MTIG = 17,
00084 }Scenario;
00085 
00086 class MTi
00087 {
00088 public:
00089     MTi();
00090     ~MTi();
00091 
00092     typedef struct _outputMode
00093     {
00094         bool temperatureData;
00095         bool calibratedData;
00096         bool orientationData;
00097         bool auxiliaryData;
00098         bool positionData;
00099         bool velocityData;
00100         bool statusData;
00101         bool rawGPSData;
00102         bool rawInertialData;
00103 
00104     } outputMode;
00105 
00106     typedef struct _outputSettings
00107     {
00108         bool timeStamp;
00109         MTOrientationMode orientationMode;
00110         bool enableAcceleration;
00111         bool enableRateOfTurn;
00112         bool enableMagnetometer;
00113         bool velocityModeNED;
00114 
00115     } outputSettings;
00116 
00117     typedef struct _Position
00118     {
00119         float x;
00120         float y;
00121         float z;
00122 
00123     } Position;
00124 
00125     bool setSettings(outputMode mode, outputSettings settings, Scenario scenario, const std::string& rosNamespace, const std::string& frameID, const Position& GPSLeverArm, int timeout);
00126 
00127     bool openPort(char * name, int baudrate);
00128     bool closePort();
00129 
00130     void getDeviceID();
00131 
00132     void makeMessage(MTMessageIdentifier mid, std::vector<unsigned char> * data, std::vector<unsigned char> * message);
00133     void addMessageToQueue(MTMessageIdentifier messageID, std::vector<unsigned char> * data, MTMessageIdentifier ack);
00134     bool waitForQueueToFinish(int timeout);
00135     nav_msgs::Odometry fillOdometryMessage(const tf::TransformListener& listener, tf::TransformBroadcaster& odom_broadcaster, const ros::Time& now);
00136     sensor_msgs::Imu fillImuMessage(const ros::Time& now);
00137     sensor_msgs::NavSatFix fillNavFixMessage(const ros::Time& now);
00138 
00139     //void resetOrientation();
00140 
00141     float accelerometer_x() const { return accX; }
00142     float accelerometer_y() const { return accY; }
00143     float accelerometer_z() const { return accZ; }
00144     float gyroscope_x() const { return gyrX; }
00145     float gyroscope_y() const { return gyrY; }
00146     float gyroscope_z() const { return gyrZ; }
00147     float compass_x() const { return magX; }
00148     float compass_y() const { return magY; }
00149     float compass_z() const { return magZ; }
00150     float temperature() const { return mTemperature; }
00151     float quaternion_x() const {return q1; }
00152     float quaternion_y() const { return q2; }
00153     float quaternion_z() const { return q3; }
00154     float quaternion_w() const { return q0; }
00155     float roll() { return eroll; }
00156     float pitch() { return epitch; }
00157     float yaw() { return eyaw; }
00158 
00159     float altitude() const { return mAltitude; }
00160     float longitude() const { return mLongitude; }
00161     float latitude() const { return mLatitude; }
00162 
00163     float velocity_x() const { return mVelocityX;}
00164     float velocity_y() const { return mVelocityY; }
00165     float velocity_z() const { return mVelocityZ; }
00166     float velocityNorth() const { return mVelocityNorth;}
00167     float velocityEast() const { return mVelocityEast; }
00168     float velocityDown() const { return mVelocityDown; }
00169     bool GPSFix() const  {return (mStatus & GPS_FIX);}
00170     uint32_t horizontalAccuracy() const {return mHorizontalAccuracy;}
00171     uint32_t verticalAccuracy() const {return mVerticalAccuracy;}
00172 
00173 private:
00174     // Serial Port variables
00175     cereal::CerealPort serial_port;
00176 
00177     // OutputMode
00178     std::vector<unsigned char> outputModeData;
00179     outputMode output_mode;
00180 
00181     // OutputSettings
00182     std::vector<unsigned char> outputSettingsData;
00183     outputSettings output_settings;
00184 
00185     //Scenario
00186     std::vector<unsigned char> mScenarioData;
00187     Scenario mScenario;
00188 
00189     std::string mFrameID;
00190     std::string mRosNamespace;
00191 
00192 
00193     // To manage incoming packages
00194     std::vector<unsigned char> package;
00195     int packageLength;
00196     bool packageInTransit;
00197     bool packageIsExtended;
00198     int packageIndex;
00199 
00200     bool ConfigState;
00201 
00202     float yawCompensation;
00203 
00204     int numOfBytes;
00205     uint32_t  mDeviceID;
00206 
00207 
00208     Position mInitialPosition;
00209 
00210     tf::TransformListener listener;
00211      geometry_msgs::PoseStamped source_pose;
00212      geometry_msgs::PoseStamped target_pose;
00213 
00214     // ******* MTi Data *******
00215     float accX, accY, accZ;
00216     float gyrX, gyrY, gyrZ;
00217     float magX, magY, magZ;
00218     float mTemperature;
00219     float q0, q1, q2, q3;
00220     float eroll, epitch, eyaw;
00221     unsigned int ts;
00222 
00223     float mAltitude, mLongitude, mLatitude;
00224     float mVelocityNorth, mVelocityEast, mVelocityDown;
00225     float mVelocityX, mVelocityY, mVelocityZ;
00226     unsigned char mStatus;
00227     uint32_t mHorizontalAccuracy, mVerticalAccuracy;
00228     // ************************
00229 
00230     // Message queue
00231     std::vector<MTMessage> queue;
00232     MTMessageIdentifier queueAck;
00233     bool queueIsRunning;
00234     bool queueIsWaiting;
00235 
00236     void resetPackage();
00237     void resetGPSValues();
00238     void manageQueue();
00239 
00240     bool serialPortSendData(std::vector<unsigned char> * data);
00241     void serialPortReadData(char * data, int length);
00242     void manageIncomingData(std::vector<unsigned char> * data, bool dataIsExtended);
00243     bool isMtiG();
00244     bool isSelfTestCompleted();
00245     void fillQuaternionWithOutputSettings(double& x, double& y, double& z, double& w );
00246 };
00247 }
00248 
00249 // EOF
00250
/home/laradmin/lar/sensors/imu/lse_xsens_mti/src/MTi/MTi.h
