~csiro-asl/csiro-asl-ros-drivers/trunk

« back to all changes in this revision

Viewing changes to csiro_asl_drivers/novatelINS/src/NovatelInterface.cpp

  • Committer: Nick Hillier
  • Date: 2011-08-03 03:16:26 UTC
  • Revision ID: nick.hillier@csiro.au-20110803031626-3ekp14jxlbda9bzf
Initial add of ROS wrappers for novatel INS around the Orca driver.

Show diffs side-by-side

added added

removed removed

Lines of Context:
 
1
/*
 
2
 * Copyright 2009-2011, CSIRO.
 
3
 * Nick Hillier and Polly Alexander
 
4
 */
 
5
/*
 
6
 * This program is distributed in the hope that it will be useful,
 
7
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 
8
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 
9
 * GNU Lesser General Public License for more details.
 
10
 * 
 
11
 * You should have received a copy of the GNU Lesser General Public License
 
12
 * along with this program.  If not, see <http://www.gnu.org/licenses/>. 
 
13
 */
 
14
 
 
15
#include <novatelINS/NovatelInterface.h>
 
16
#include <novatelINS/gps_convert.h>
 
17
 
 
18
 
 
19
// TODO: replace sleep() with ros::Time equivalent
 
20
 
 
21
NovatelInterfaceROS::NovatelInterfaceROS() 
 
22
{
 
23
        ;
 
24
}
 
25
 
 
26
NovatelInterfaceROS::~NovatelInterfaceROS() 
 
27
{
 
28
        ;
 
29
}
 
30
 
 
31
void NovatelInterfaceROS::getLocalPos(double latitude, double longitude, double height, double *X, double *Y, double *Z){
 
32
        // pose is JAUS-like Northings/Eastings/Height
 
33
        double easting, northing;
 
34
        int zone;
 
35
        // TODO: The latlong2eastnorth() function needs review. Not confident it is correct.
 
36
        latlong2eastnorth(latitude, longitude, &easting, &northing, &zone);
 
37
        
 
38
        double tf_x, tf_y, tf_z;
 
39
        tf_x = northing - tfoffsetX;
 
40
        tf_y = easting - tfoffsetY;
 
41
        tf_z = height - tfoffsetZ;
 
42
        
 
43
        *(X) = tf_x;
 
44
        *(Y) = tf_y;
 
45
        *(Z) = tf_z;
 
46
}
 
47
 
 
48
 
 
49
void NovatelInterfaceROS::imu3dmgCallback(const sensor_msgs::Imu::ConstPtr& data){
 
50
        _imu3dmgData = *data;
 
51
        
 
52
        // TODO: ROS driver for the 3DM-GX? does not support magnetic field data at this time
 
53
        //mag[0] += _imu3dmgData.field[0];
 
54
        mag[0] += 1;
 
55
        //mag[1] += _imu3dmgData.field[1];
 
56
        mag[1] += 1;
 
57
        num_mag_avg++;
 
58
        
 
59
    return;
 
60
}
 
61
 
 
62
int NovatelInterfaceROS::setupROS() 
 
63
{    
 
64
    // TODO: publish as a transform to /tf
 
65
    
 
66
    _node.reset(new ros::NodeHandle()); 
 
67
    _node_ns.reset(new ros::NodeHandle("~/config"));    
 
68
        _node_ns->param("port", port, (std::string)DEFAULT_PORT);
 
69
        _node_ns->param("baud", baud, DEFAULT_BAUD);
 
70
        _node_ns->param("imuOffsetX", imuOffset[0], 0.);
 
71
        _node_ns->param("imuOffsetY", imuOffset[1], 0.);
 
72
        _node_ns->param("imuOffsetZ", imuOffset[2], 0.);
 
73
        _node_ns->param("gpsOffsetX", gpsOffset[0], 0.);
 
74
        _node_ns->param("gpsOffsetY", gpsOffset[1], 0.);
 
75
        _node_ns->param("gpsOffsetZ", gpsOffset[2], 0.);
 
76
        _node_ns->param("alignType", alignType, (std::string)DEFAULT_ALIGNMENT_METHOD);
 
77
        _node_ns->param("magbiasX", magbias[0], 0.);
 
78
        _node_ns->param("magbiasY", magbias[1], 0.);
 
79
        _node_ns->param("mazazstd", magazstd, MAG_STD_DEFAULT);
 
80
        _node_ns->param("tfOffsetX", tfoffsetX, 0.);
 
81
        _node_ns->param("tfOffsetY", tfoffsetY, 0.);
 
82
        _node_ns->param("tfOffsetZ", tfoffsetZ, 0.);
 
83
        _node_ns->param("basel_frame_id", baseframe_id, (std::string)"Novatelworld");
 
84
        _node_ns->param("control_frame_id", cntlframe_id, (std::string)"NovatelINS");
 
85
        _node_ns->param("GPS_frame_id", gpsframe_id, (std::string)"NovatelGPS");
 
86
    _pub_GPSFix = _node->advertise<sensor_msgs::NavSatFix>("Novatel/NavSatFix", 100);
 
87
    _pub_Odometry = _node->advertise<nav_msgs::Odometry>("Novatel/odom", 100);
 
88
    reset();
 
89
    
 
90
        return 0;
 
91
}
 
92
 
 
93
int NovatelInterfaceROS::reset() {
 
94
    _GPSFixData.header.seq = 0;
 
95
    _GPSFixData.header.frame_id = gpsframe_id;
 
96
    _GPSFixData.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
 
97
    _OdometryData.header.seq = 0;
 
98
    _OdometryData.header.frame_id = cntlframe_id;
 
99
    _OdometryData.child_frame_id = baseframe_id;
 
100
    return 0;
 
101
}
 
102
 
 
103
int NovatelInterfaceROS::setupDriver()
 
104
{
 
105
    ROS_DEBUG("Baud: %d\n", baud);
 
106
        ROS_DEBUG("Port: %s\n", port.c_str());
 
107
    ROS_DEBUG("imu offset: %f,%f,%f\n", imuOffset[0], imuOffset[1], imuOffset[2]);
 
108
    ROS_DEBUG("gps offset: %f,%f,%f\n", gpsOffset[0], gpsOffset[1], gpsOffset[2]);
 
109
        
 
110
        // Vector from Bobcat origin to Novatel INS unit
 
111
        vector<double> insOffset(imuOffset, imuOffset + sizeof(imuOffset)/sizeof(*imuOffset));
 
112
        ROS_DEBUG("imuOffset (JAUS)= %f %f %f\n", insOffset[0], insOffset[1], insOffset[2]);
 
113
        jaus2novatel_frame(&insOffset); // Transform into Novatel frame convention
 
114
    ROS_DEBUG("imuOffset (novatel) = %f %f %f\n", insOffset[0], insOffset[1], insOffset[2]);
 
115
 
 
116
        // Vector from Vehicle origin to GPS antenna
 
117
        vector<double> originToGps(gpsOffset, gpsOffset + sizeof(gpsOffset)/sizeof(*gpsOffset));
 
118
        ROS_DEBUG("gpsOffset (JAUS) = %f %f %f", originToGps[0], originToGps[1], originToGps[2]);
 
119
        jaus2novatel_frame(&originToGps); // Transform into Novatel frame convention
 
120
    ROS_DEBUG("gpsOffset (novatel) = %f %f %f", originToGps[0], originToGps[1], originToGps[2]);
 
121
    
 
122
        // Vector from Novatel INS unit to GPS antenna
 
123
        vector<double> imuToGps(3,0.0); //empty vector with 3 elements
 
124
        for (int i = 0; i < 3; i++)
 
125
        {
 
126
                imuToGps[i] = originToGps[i] - insOffset[i];
 
127
        }
 
128
        ROS_DEBUG("imuToGps (novatel) = %f %f %f", imuToGps[0], imuToGps[1], imuToGps[2]);
 
129
 
 
130
        auto_ptr<gna::SimpleConfig > simpleCfg;
 
131
        simpleCfg.reset( new gna::SimpleConfig(port, baud, "IMU_HG1700_AG62" , imuToGps));
 
132
        config.reset( new gna::Config(*simpleCfg.get()) );
 
133
 
 
134
        // set up a couple of extra parameters
 
135
        config->enableInsPhaseUpdate_ = true;
 
136
        config->enableRTK_ = true;
 
137
        config->dtInsPva_ = 0.1;
 
138
        config->dtInsCov_ = 0.1; //!cov
 
139
        vector<double > fullUncertainty(3, 0.0);
 
140
        fullUncertainty[0] = 0.02;
 
141
        fullUncertainty[1] = 0.02;
 
142
        fullUncertainty[2] = 0.02;
 
143
        config->imuToGpsOffsetUncertainty_ = fullUncertainty;
 
144
        config->enableInsOffset_ = true;
 
145
        config->insOffset_ = insOffset; // read from config file
 
146
        config->enableVehicleBodyRotation_ = true;
 
147
        vector<double > bodyRot(3,0.0);
 
148
        bodyRot[0] = -2.6;
 
149
        bodyRot[1] = 0.8;
 
150
        bodyRot[2] = 0.0;
 
151
        config->vehicleBodyRotation_ = bodyRot;
 
152
        vector<double > bodyRotUncert(3, 0.0);
 
153
        bodyRotUncert[0] = 0.5; 
 
154
        bodyRotUncert[1] = 0.6;
 
155
        bodyRotUncert[2] = 0.6;
 
156
        config->vehicleBodyRotationUncertainty_ = bodyRotUncert;
 
157
    
 
158
    if ((alignType != "kinematic") && (alignType != "static")){
 
159
        ROS_WARN("Unknown alignment type, defaulting to %s\n",DEFAULT_ALIGNMENT_METHOD);
 
160
        alignType = DEFAULT_ALIGNMENT_METHOD;
 
161
    }
 
162
    if (alignType == "kinematic") {
 
163
        align = false;
 
164
    } else {// alignType == "static"
 
165
        align = true;
 
166
    }
 
167
 
 
168
    ROS_DEBUG("Alignment type: %s (%s)\n", alignType.c_str(), (align)?"true (static)":"false (kinematic)");
 
169
 
 
170
        vector<double > initAzimuth(2, 0.0);    
 
171
        if (align) {
 
172
                // static       
 
173
                // initAzimuth[0] = -90.0;
 
174
                // initAzimuth[1] = 10.0;
 
175
                ROS_INFO("Novatel static align start");
 
176
                getInitAzimuth(&initAzimuth);
 
177
                ROS_INFO("Novatel static align done");
 
178
                config->initAzimuth_ = initAzimuth;
 
179
                config->enableSetInitAzimuth_ = true;
 
180
        } else {
 
181
                // kinematic
 
182
                config->enableSetInitAzimuth_ = false;
 
183
        }
 
184
 
 
185
        //check that it makes sense
 
186
    int err = 0;
 
187
        if(!config->isValid()){
 
188
        err = -1;
 
189
        ROS_ERROR("NovatelInterfaceROS::setupDriver: Invalid config");
 
190
        }
 
191
    
 
192
        ROS_DEBUG("Config is: %s", config->toString().c_str());
 
193
    
 
194
        return err;
 
195
}
 
196
 
 
197
int NovatelInterfaceROS::start()
 
198
{
 
199
        //create the driver; this gets things rolling (opens the serial port, sets up the device)
 
200
        int errorCnt = 0;
 
201
        do{
 
202
                try{
 
203
                        driver.reset( new gna::Driver( *(config.get()) ) );
 
204
                }
 
205
                catch(std::exception &e){
 
206
                        ROS_WARN("%s: Waiting 5 secs before re-trying", e.what());
 
207
                        //std::cout << e.what() << "\nWaiting 5 sec before re-trying.\n\n";
 
208
                        sleep(5);
 
209
                }
 
210
                catch(...){
 
211
                        ROS_WARN( "caught unknown exception!\nWaiting 5 sec before re-trying.\n\n");
 
212
                        sleep(5);
 
213
                }
 
214
        }while(0 == driver.get() && errorCnt++<1); // try up to 5 times 
 
215
        
 
216
        //check that we succeeded
 
217
        int err = 0;
 
218
        if(0 == driver.get()){
 
219
                ROS_ERROR("Failed to set up driver!\n");
 
220
                err = -1;
 
221
        }
 
222
        return err;
 
223
}
 
224
 
 
225
int NovatelInterfaceROS::read_and_process_data()
 
226
{
 
227
        auto_ptr<gna::GenericData > generic;
 
228
        try{
 
229
                generic = driver->read();
 
230
        }
 
231
        catch( const std::exception& e ){
 
232
                int ins_driver_read_exception_caught = 0;
 
233
                assert(ins_driver_read_exception_caught);
 
234
                ROS_ERROR("%s: NovatelInterfaceROS::read_and_process_data: exception cought on driver read", e.what());
 
235
                return -1; 
 
236
        }
 
237
        //figure out what we've got
 
238
        switch(generic->type()){
 
239
                case gna::InsPva:
 
240
                {
 
241
                        gna::InsPvaData *data = dynamic_cast<gna::InsPvaData *>(generic.get());
 
242
                        if (0 == data) {
 
243
                                ROS_ERROR("InsPva data structure empty");
 
244
                                return -1;
 
245
                        }
 
246
                        
 
247
                        // Set the timestamp 
 
248
                        // TODO: should this be the GPS time or ROS time?
 
249
                        ros::Time::Time timestamp((int)data->timeStampSec, (int)(data->timeStampUSec*1e3)); // sec, nsec WALL TIME from driver - call to gettimeofday()
 
250
                        //ros::Time::Time timestamp(,)); // sec, nsec GPS TIME
 
251
                        
 
252
                        // TODO: should the header completion be outside the switch, at the end with the publish?
 
253
                        _OdometryData.header.seq ++; 
 
254
                        _OdometryData.header.stamp = timestamp; 
 
255
                        _OdometryData.header.stamp = timestamp; 
 
256
                        
 
257
                        // fill in the standard ROS messages
 
258
                        double tf_x, tf_y, tf_z;
 
259
                        getLocalPos((double)data->latitude, (double)data->longitude, -(double)data->height, &tf_x, &tf_y, &tf_z);
 
260
                        
 
261
                        _tf_transform.setOrigin(tf::Vector3(tf_x, tf_y, tf_z));
 
262
                        
 
263
                        _OdometryData.pose.pose.position.x = tf_x;
 
264
                        _OdometryData.pose.pose.position.y = tf_y;
 
265
                        _OdometryData.pose.pose.position.z = tf_z;
 
266
                        
 
267
                        // twist is in the vehicle frame
 
268
                        //btQuaternion q;
 
269
                        //q.setEulerYPR(-(double)data->azimuth,(double)data->pitch,(double)data->roll); // TODO: Is this right? fixed or relative axis?
 
270
                        double roll, pitch, yaw;
 
271
                        roll = (double)DEG2RAD(data->roll);
 
272
                        pitch = (double)DEG2RAD(data->pitch);
 
273
                        yaw = (double)DEG2RAD(-data->azimuth);
 
274
                        rot3D.setRollPitchYawRad(roll, pitch, yaw); // set using Rodriguez's formula as relative axis
 
275
                        double quatX = rot3D.getQuaternion()[1];
 
276
                        double quatY = rot3D.getQuaternion()[2];
 
277
                        double quatZ = rot3D.getQuaternion()[3];
 
278
                        double quatW = rot3D.getQuaternion()[0];
 
279
                        
 
280
                        _tf_transform.setRotation( tf::Quaternion(quatX, quatY, quatZ, quatW) );
 
281
                        
 
282
                        _OdometryData.pose.pose.orientation.x = quatX;
 
283
                        _OdometryData.pose.pose.orientation.y = quatY;
 
284
                        _OdometryData.pose.pose.orientation.z = quatZ;
 
285
                        _OdometryData.pose.pose.orientation.w = quatW;
 
286
                        SMALL::Vector3D velNEU, velBody;
 
287
                        velNEU = (double)data->northVelocity, (double)data->eastVelocity, -(double)data->upVelocity;
 
288
                        velBody = rot3D.rotate(velNEU);
 
289
                        _OdometryData.twist.twist.linear.x = velBody[0];
 
290
                        _OdometryData.twist.twist.linear.y = velBody[1];
 
291
                        _OdometryData.twist.twist.linear.z = velBody[2];
 
292
                        // Novatel unit doesn't give us angular rates
 
293
                        // TODO: difference the headings to get the rates
 
294
                        _OdometryData.twist.twist.angular.x = 0.0;
 
295
                        _OdometryData.twist.twist.angular.y = 0.0;
 
296
                        _OdometryData.twist.twist.angular.z = 0.0;
 
297
                        // TODO: fill in a custom Novatel ROS msg with the remaining guff
 
298
                        
 
299
                        // clear the statusMessage string, overwrite all elements with 0:
 
300
                        //memset(insPvaData.statusMessage, 0, MAX_STATUS_MESSAGE_LENGTH);
 
301
                        
 
302
                        //insPvaData.gpsWeekNr          = (int)data->gpsWeekNr   ; 
 
303
                        //insPvaData.secIntoWeek                = (double)data->secIntoWeek      ;
 
304
 
 
305
                        //insPvaData.statusMessageType = (int)data->statusMessageType;
 
306
                        //sprintf(insPvaData.statusMessage, "%s", data->statusMessage.c_str()) ;
 
307
 
 
308
                        ROS_DEBUG("insPvaData: %s", data->toString().c_str());
 
309
                        ROS_DEBUG("got inspva\n");
 
310
                        
 
311
                        break;
 
312
                }
 
313
                
 
314
                //!cov:start
 
315
                case gna::InsCov:
 
316
                {
 
317
                        gna::InsCovData *data = dynamic_cast<gna::InsCovData *>(generic.get());
 
318
                        if (0 == data) {
 
319
                                ROS_ERROR("InsCov data structure empty");
 
320
                                return -1;
 
321
                        }
 
322
                        
 
323
                        // Set the timestamp 
 
324
                        // TODO: should this be the GPS time or ROS time?
 
325
                        //ros::Time::Time timestamp((int)data->timeStampSec, (int)(data->timeStampUSec*1e3)); // sec, nsec WALL TIME from driver - call to gettimeofday()
 
326
                        //ros::Time::Time timestamp(,)); // sec, nsec GPS TIME
 
327
                        
 
328
                        // fill in the standard ROS message
 
329
                        _OdometryData.pose.covariance[0] = (double)data->posxx;
 
330
                        _OdometryData.pose.covariance[1] = (double)data->posxy;
 
331
                        _OdometryData.pose.covariance[2] = (double)data->posxz;
 
332
                        _OdometryData.pose.covariance[6] = (double)data->posyx;
 
333
                        _OdometryData.pose.covariance[7] = (double)data->posyy;
 
334
                        _OdometryData.pose.covariance[8] = (double)data->posyz;
 
335
                        _OdometryData.pose.covariance[12] = (double)data->poszx;
 
336
                        _OdometryData.pose.covariance[13] = (double)data->poszy;
 
337
                        _OdometryData.pose.covariance[14] = (double)data->poszz;
 
338
                        
 
339
                        _OdometryData.pose.covariance[21] = (double)data->attxx;
 
340
                        _OdometryData.pose.covariance[22] = (double)data->attxy;
 
341
                        _OdometryData.pose.covariance[23] = (double)data->attxz;
 
342
                        _OdometryData.pose.covariance[27] = (double)data->attyx;
 
343
                        _OdometryData.pose.covariance[28] = (double)data->attyy;
 
344
                        _OdometryData.pose.covariance[29] = (double)data->attyz;
 
345
                        _OdometryData.pose.covariance[33] = (double)data->attzx;
 
346
                        _OdometryData.pose.covariance[34] = (double)data->attzy;
 
347
                        _OdometryData.pose.covariance[35] = (double)data->attzz;
 
348
 
 
349
                        // Convert the velocity covar to the vehicle frame
 
350
                        SMALL::Matrix33 velcovar, velcovarVeh;
 
351
                        velcovar =  (double)data->velxx, (double)data->velxy, (double)data->velxz,
 
352
                                                (double)data->velyx, (double)data->velyy, (double)data->velyz,
 
353
                                                (double)data->velzx, (double)data->velzy, (double)data->velzz;
 
354
                        //velcovarVeh = rot3D.i().getRotationMatrix()* velcovar * rot3D.i().getRotationMatrix().transposed(); // TODO : is this the same as rot3D.i().getRotationMatrix()* velcovar * rot3D.getRotationMatrix() ???
 
355
                        velcovarVeh = rot3D.getRotationMatrix()* velcovar * rot3D.getRotationMatrix().transposed();
 
356
                        
 
357
                        _OdometryData.twist.covariance[0] = velcovarVeh(1,1);
 
358
                        _OdometryData.twist.covariance[1] = velcovarVeh(1,2);
 
359
                        _OdometryData.twist.covariance[2] = velcovarVeh(1,3);
 
360
                        _OdometryData.twist.covariance[6] = velcovarVeh(2,1);
 
361
                        _OdometryData.twist.covariance[7] = velcovarVeh(2,2);
 
362
                        _OdometryData.twist.covariance[8] = velcovarVeh(2,3);
 
363
                        _OdometryData.twist.covariance[12] =velcovarVeh(3,1);
 
364
                        _OdometryData.twist.covariance[13] =velcovarVeh(3,2);
 
365
                        _OdometryData.twist.covariance[14] =velcovarVeh(3,3);
 
366
 
 
367
                        //insCovData.gpsWeekNr          = (int)data->gpsWeekNr; 
 
368
                        //insPvaData.secIntoWeek                = (double)data->secIntoWeek;
 
369
                
 
370
                        ROS_DEBUG("insCovData: %s", data->toString().c_str());
 
371
                
 
372
                        ROS_DEBUG("got inscov\n");
 
373
                
 
374
                        break;
 
375
                }
 
376
                //!cov:end              
 
377
                
 
378
                case gna::BestGpsPos:
 
379
                {
 
380
                        gna::BestGpsPosData *data = dynamic_cast<gna::BestGpsPosData *>(generic.get());
 
381
                        if (0 == data) {
 
382
                                ROS_ERROR("BestGpsPos returned data == 0");
 
383
                                return -1;
 
384
                        }
 
385
                        
 
386
                        // Set the timestamp 
 
387
                        // TODO: should this be the GPS time or ROS time?
 
388
                        ros::Time::Time timestamp((int)data->timeStampSec, (int)(data->timeStampUSec*1e3)); // sec, nsec WALL TIME from driver - call to gettimeofday()
 
389
                        //ros::Time::Time timestamp(,)); // sec, nsec GPS TIME
 
390
                        
 
391
                        _GPSFixData.header.seq ++; 
 
392
                        _GPSFixData.header.stamp = timestamp; 
 
393
 
 
394
                        // fill in the standard ROS message
 
395
                        _GPSFixData.latitude = data->latitude;
 
396
                        _GPSFixData.longitude = data->longitude;
 
397
                        _GPSFixData.altitude = data->heightAMSL;
 
398
                        _GPSFixData.position_covariance[0] = data->sigmaLatitude*data->sigmaLatitude;
 
399
                        _GPSFixData.position_covariance[4] = data->sigmaLongitude*data->sigmaLongitude;
 
400
                        _GPSFixData.position_covariance[8] = data->sigmaHeight*data->sigmaHeight;
 
401
                        _GPSFixData.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
 
402
                        
 
403
                        // TODO: make sense of the conversion to the NavSatStatus Solution Status
 
404
                        ROS_WARN_ONCE("NavSatStatus Solution Status not properly implemented, read with caution!");
 
405
                        //switch (data->positionType) {
 
406
                        //      case :
 
407
                        //              _GPSFixData.status.status = sensor_msgs::NavSatFix.STATUS_GBAS_FIX; // ground based augmentation
 
408
                        //      default:
 
409
                        //              _GPSFixData.status.status = sensor_msgs::NavSatFix.STATUS_FIX; // standard sat fix
 
410
                        //}
 
411
                        if (data->numL1RangesRTK > 0) {
 
412
                                _GPSFixData.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
 
413
                        } else if (data->numL1Ranges > 4) {
 
414
                                _GPSFixData.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
 
415
                        } else {
 
416
                                _GPSFixData.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
 
417
                        }
 
418
                        
 
419
                        double tf_x, tf_y, tf_z;                        
 
420
                        getLocalPos(_GPSFixData.latitude, _GPSFixData.longitude, _GPSFixData.altitude, &tf_x, &tf_y, &tf_z);
 
421
                        
 
422
                        tf_z += data->undulation;
 
423
                        _tf_gps_transform.setOrigin(tf::Vector3(tf_x, tf_y, tf_z));
 
424
                        
 
425
                        _tf_gpsoffset_transform.setOrigin(tf::Vector3(gpsOffset[0], gpsOffset[1], gpsOffset[2])); // Should this be "originToGps[]" or gpsOffset?
 
426
                        _tf_gpsoffset_transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
 
427
                        
 
428
                        // TODO: fill in a custom Novatel ROS msg with the remaining guff                       
 
429
                                                
 
430
                        //gpsBestPosData.gpsWeekNr                      =data->gpsWeekNr        ;
 
431
                        //gpsBestPosData.msIntoWeek                     =data->msIntoWeek       ;
 
432
                        //gpsBestPosData.solutionStatus                 =data->solutionStatus   ;
 
433
                        //gpsBestPosData.positionType                   =data->positionType     ;
 
434
 
 
435
                        //gpsBestPosData.undulation                     =data->undulation       ;
 
436
                        //gpsBestPosData.datumId                        =data->datumId          ;
 
437
                        
 
438
                        //memcpy(gpsBestPosData.baseStationId, data->baseStationId, sizeof(gpsBestPosData.baseStationId));
 
439
                        //gpsBestPosData.diffAge                        =data->diffAge          ;  
 
440
                        //gpsBestPosData.solutionAge                    =data->solutionAge      ; 
 
441
                        //gpsBestPosData.numObservations                =data->numObservations  ;
 
442
                        //gpsBestPosData.numL1Ranges                    =data->numL1Ranges      ;
 
443
                        //gpsBestPosData.numL1RangesRTK                 =data->numL1RangesRTK   ;
 
444
                        //gpsBestPosData.numL2RangesRTK                 =data->numL2RangesRTK   ;
 
445
                        //gpsBestPosData.statusMessageType      =data->statusMessageType ;
 
446
                        //sprintf(gpsBestPosData.statusMessage, "%s", data->statusMessage.c_str()) ;
 
447
                        //gpsBestPosData.timeStampSec           =data->timeStampSec      ;
 
448
                        //gpsBestPosData.timeStampUSec          =data->timeStampUSec     ;
 
449
 
 
450
                        ROS_DEBUG("bestGpsPosData: %s", data->toString().c_str());
 
451
                
 
452
                        ROS_DEBUG("got bestGpsPos\n");
 
453
                        
 
454
                        break;
 
455
                }
 
456
                case gna::BestGpsVel:
 
457
                {
 
458
                        gna::BestGpsVelData *data = dynamic_cast<gna::BestGpsVelData *>(generic.get());
 
459
                        if (0 == data) {
 
460
                                ROS_ERROR("BestGpsVel returned data == 0");
 
461
                                return -1;
 
462
                        }
 
463
                        ROS_WARN_ONCE("Handling for GPS velocity data not yet implemented");
 
464
                        // Set the timestamp 
 
465
                        // TODO: should this be the GPS time or ROS time?
 
466
                        ros::Time::Time timestamp((int)data->timeStampSec, (int)(data->timeStampUSec*1e3)); // sec, nsec WALL TIME from driver - call to gettimeofday()
 
467
                        //ros::Time::Time timestamp(,)); // sec, nsec GPS TIME
 
468
                        double GPS_yaw = DEG2RAD(data->trackOverGround);
 
469
                        
 
470
                        rot3Dgps.setRollPitchYawRad(0., 0., GPS_yaw); // no roll or pitch info from the GPS
 
471
                        double quatX = rot3Dgps.getQuaternion()[1];
 
472
                        double quatY = rot3Dgps.getQuaternion()[2];
 
473
                        double quatZ = rot3Dgps.getQuaternion()[3];
 
474
                        double quatW = rot3Dgps.getQuaternion()[0];
 
475
                        
 
476
                        _tf_gps_transform.setRotation( tf::Quaternion(quatX, quatY, quatZ, quatW) );
 
477
 
 
478
                        //gpsBestVelData.gpsWeekNr              = data->gpsWeekNr; 
 
479
                        //gpsBestVelData.msIntoWeek             = data->msIntoWeek;
 
480
                        //gpsBestVelData.solutionStatus         = data->solutionStatus;
 
481
                        //gpsBestVelData.velocityType           = data->velocityType; 
 
482
                        //gpsBestVelData.latency                = data->latency;               
 
483
                        //gpsBestVelData.diffAge                = data->diffAge;              
 
484
                        //gpsBestVelData.horizontalSpeed        = data->horizontalSpeed;     
 
485
                        //gpsBestVelData.trackOverGround        = data->trackOverGround;    
 
486
                        //gpsBestVelData.verticalSpeed          = data->verticalSpeed;     
 
487
                        //gpsBestVelData.statusMessageType = data->statusMessageType;     
 
488
                        //sprintf(gpsBestVelData.statusMessage, "%s", data->statusMessage.c_str()) ;
 
489
                        //gpsBestVelData.timeStampSec           = data->timeStampSec; 
 
490
                        //gpsBestVelData.timeStampUSec          = data->timeStampUSec;
 
491
 
 
492
                        ROS_DEBUG("bestGpsVelData: %s", data->toString().c_str());
 
493
                
 
494
                        ROS_DEBUG("got bestGpsVel\n");
 
495
                        
 
496
                        break;
 
497
                }
 
498
                case gna::RawImu:
 
499
                {
 
500
                        //gna::RawImuData *data = dynamic_cast<gna::RawImuData *>(generic.get());
 
501
 
 
502
                        //if (0 == data) {
 
503
                        //      ROS_ERROR("RawImu returned data == 0");
 
504
                        //      return -1;
 
505
                        //}
 
506
                        //
 
507
                        //// Set the timestamp 
 
508
                        //// TODO: should this be the GPS time or ROS time?
 
509
                        //ros::Time::Time timestamp((int)data->timeStampSec, (int)(data->timeStampUSec*1e3)); // sec, nsec WALL TIME from driver - call to gettimeofday()
 
510
                        ////ros::Time::Time timestamp(,)); // sec, nsec GPS TIME
 
511
                        //
 
512
                        //rawIMUData.gpsWeekNr       = data->gpsWeekNr; 
 
513
                        //rawIMUData.secIntoWeek     = data->secIntoWeek;
 
514
                        //rawIMUData.zDeltaV         = data->zDeltaV;  
 
515
                        //rawIMUData.yDeltaV         = data->yDeltaV;
 
516
                        //rawIMUData.xDeltaV         = data->xDeltaV; 
 
517
                        //rawIMUData.zDeltaAng       = data->zDeltaAng;
 
518
                        //rawIMUData.yDeltaAng       = data->yDeltaAng;
 
519
                        //rawIMUData.xDeltaAng       = data->xDeltaAng;
 
520
                        //rawIMUData.statusMessageType = data->statusMessageType;
 
521
                        //sprintf(rawIMUData.statusMessage, "%s", data->statusMessage.c_str()) ;
 
522
                        //rawIMUData.timeStampSec    = data->timeStampSec;
 
523
                        //rawIMUData.timeStampUSec   = data->timeStampUSec; 
 
524
                        //
 
525
                        //if (!rawIMU.t_writefrom(rawIMUData)) {
 
526
                        //      ROS_WARN("rawIMU.t_writefrom returned false");
 
527
                        //}
 
528
            //
 
529
                        //
 
530
                        //ROS_DEBUG("rawIMU Data: %s", data->toString().c_str());
 
531
                        ROS_WARN_ONCE("Handling for raw IMU data not yet implemented");
 
532
                        ROS_DEBUG("got rawIMU \n");
 
533
                        
 
534
                        break;
 
535
                }
 
536
                default:
 
537
                {
 
538
                        ROS_WARN("Got unknown message\n");
 
539
                        return -1;
 
540
                        break;
 
541
                }
 
542
        }
 
543
        _pub_GPSFix.publish(_GPSFixData);
 
544
        _pub_Odometry.publish(_OdometryData);
 
545
        _br_tf.sendTransform(tf::StampedTransform(_tf_transform, _OdometryData.header.stamp, baseframe_id, cntlframe_id)); // INS control point
 
546
        _br_tf_gps.sendTransform(tf::StampedTransform(_tf_gps_transform, _GPSFixData.header.stamp, baseframe_id, gpsframe_id)); // GPS antennae
 
547
        _br_tf_gpsoffset.sendTransform(tf::StampedTransform(_tf_gpsoffset_transform, _GPSFixData.header.stamp, gpsframe_id, (baseframe_id + (std::string)"_gps"))); // GPS control point
 
548
        
 
549
        return 0;
 
550
}
 
551
 
 
552
/* This funtion transforms a vector from a JAUS compliant coordinate frame to
 
553
 * the Novatel convention used by the INS unit. */
 
554
int NovatelInterfaceROS::jaus2novatel_frame(vector<double> *vectorArg)
 
555
{
 
556
        double temp = vectorArg->at(0);
 
557
        vectorArg->at(0) = vectorArg->at(1); // x(Novatel) = y(Jaus)
 
558
        vectorArg->at(1) = temp; // y(Novatel) = x(Jaus)
 
559
        vectorArg->at(2) = - vectorArg->at(2); // z(Novatel) = - z(Jaus)
 
560
        return 0;
 
561
}
 
562
 
 
563
int NovatelInterfaceROS::getInitAzimuth(vector<double> *vectorArg)
 
564
{
 
565
        ROS_WARN("Magnetic field static alignment is not currently implemented.");
 
566
        // TODO: allow static alignment in any direction by using a magnetometer
 
567
        // Code below and in the imu3dmgCallback() is how to do it.
 
568
        
 
569
        mag[0] = 0.0; mag[1] = 0.0;
 
570
        num_mag_avg = 0;
 
571
        _sub_imu3dmg = _node->subscribe("imu3DMG", 1, &NovatelInterfaceROS::imu3dmgCallback, this);
 
572
        
 
573
        
 
574
        ROS_WARN("Skipping magnetic calibration!");
 
575
        num_mag_avg = MAG_AVG_SIZE; // TODO: remove this line when using imu for magnetic heading determination
 
576
        
 
577
        
 
578
        
 
579
        
 
580
        ROS_DEBUG("Microstrain ave start");     
 
581
        while (num_mag_avg < MAG_AVG_SIZE) { // the call back increments num_mag_avg
 
582
                // do nothing
 
583
                usleep(100000);
 
584
        }
 
585
        num_mag_avg--;
 
586
        mag[0] /= num_mag_avg; mag[1] /= num_mag_avg; 
 
587
        mag[0] -= magbias[0]; mag[1] -= magbias[1];
 
588
        // TODO: calculate the standard deviation somewhere too.
 
589
        ROS_DEBUG("Microstrain ave done");
 
590
        
 
591
        magaz = atan2(mag[1], mag[0])*180/M_PI;
 
592
        if (magaz < 0) {
 
593
                magaz += 360.0;
 
594
        };
 
595
        vectorArg->at(0) = magaz;
 
596
        vectorArg->at(1) = magazstd;
 
597
        
 
598
        ROS_WARN("Setting static alignment to North, if the novatel is not facing North, then you need to re-orient and re-start the driver.");
 
599
        vectorArg->at(0) = 0.0; // North
 
600
        vectorArg->at(1) = magazstd;
 
601
        
 
602
        return num_mag_avg; // number of data points averaged over
 
603
}
 
604
 
 
605
int NovatelInterfaceROS::setup() {
 
606
        if(setupROS() != 0) {
 
607
                ROS_ERROR("setupROS: Failed");
 
608
                return -1;
 
609
        }       
 
610
        ROS_INFO("setupROS done");
 
611
        if(setupDriver() != 0) {
 
612
                ROS_ERROR("setupDriver: Failed");
 
613
                return -1;
 
614
        }
 
615
        ROS_INFO("setupDriver done");
 
616
        return 0;
 
617
}
 
618
 
 
619
int main(int argc, char * argv[])
 
620
{
 
621
        ros::init(argc, argv, "novatel");
 
622
        sleep(2.);
 
623
        NovatelInterfaceROS novInt;     
 
624
        
 
625
        if (novInt.setup() != 0) {
 
626
                ROS_FATAL("Driver setup: Failed");
 
627
                exit(-1);
 
628
        }
 
629
        ROS_INFO("Driver setup done");
 
630
        if (novInt.start() != 0) {
 
631
                ROS_FATAL("startDriver: Failed");
 
632
                exit(-1);
 
633
        }
 
634
        ROS_INFO("Driver started");
 
635
        while(ros::ok()) {
 
636
                if (novInt.read_and_process_data() != 0) {
 
637
                        ROS_ERROR("read_and_process_data: Failed");
 
638
                }
 
639
                ros::spinOnce();
 
640
        }
 
641
        ROS_INFO("Exiting...");
 
642
        return 0;
 
643
}