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

« back to all changes in this revision

Viewing changes to csiro_asl_drivers/novatelINS/include/novatelINS/NovatelInterface.h

  • 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
#ifndef NOVATEL_INTERFACE_H
 
16
#define NOVATEL_INTERFACE_H
 
17
 
 
18
/*** Includes ***/
 
19
/* ROS stuff */
 
20
#include <ros/ros.h>
 
21
#include <ros/node_handle.h>
 
22
#include <ros/master.h>
 
23
#include <ros/time.h>
 
24
#include <ros/console.h>
 
25
/* Other ROS package stuff */
 
26
#include <sensor_msgs/NavSatFix.h>
 
27
#include <nav_msgs/Odometry.h>
 
28
#include <sensor_msgs/Imu.h>
 
29
#include <tf/transform_broadcaster.h>
 
30
 
 
31
/* Other CSIRO stuff */
 
32
#include <small/Rotation3D.hh> // TODO: replace SMALL with ROS tf library or bullet?
 
33
//#include <Linearmath/btQuaternion.h>
 
34
 
 
35
/* ACFR Gearbox stuff */
 
36
#include <gbxutilacfr/exceptions.h>
 
37
#include <gbxnovatelacfr/driver.h>
 
38
 
 
39
/*** Definitions ***/
 
40
/* magnetic field defines for initializing azimuth from an imu3dmg */
 
41
#define MAG_STD_DEFAULT 3.0
 
42
#define MAG_AVG_SIZE 20
 
43
/* comms */
 
44
#define DEFAULT_BAUD 115200 // 200
 
45
#define DEFAULT_PORT "/dev/ttyS0"
 
46
#define DEFAULT_ALIGNMENT_METHOD "static" // Other option is "kinematic"
 
47
#define DEG2RAD(x) ((x)*M_PI/180.0)
 
48
 
 
49
/* namespace modifications */
 
50
namespace gna = gbxnovatelacfr;
 
51
using namespace std;
 
52
 
 
53
class NovatelInterfaceROS
 
54
{
 
55
        protected:
 
56
        ros::Publisher _pub_GPSFix, _pub_Odometry;
 
57
        ros::Subscriber _sub_imu3dmg; // magnetometer is used for inital alignment calibration if alignment method is static
 
58
        tf::TransformBroadcaster _br_tf, _br_tf_gps, _br_tf_gpsoffset;
 
59
        
 
60
        boost::shared_ptr<ros::NodeHandle> _node, _node_ns;
 
61
 
 
62
        sensor_msgs::NavSatFix _GPSFixData;
 
63
        nav_msgs::Odometry _OdometryData;
 
64
        sensor_msgs::Imu _imu3dmgData;
 
65
        tf::Transform _tf_transform, _tf_gps_transform, _tf_gpsoffset_transform;
 
66
                
 
67
                std::string alignType;
 
68
                std::string port;
 
69
                std::string cntlframe_id, gpsframe_id, baseframe_id;
 
70
                int baud;
 
71
                double imuOffset[3]; // position of IMU (JAUS)
 
72
                double gpsOffset[3]; // position of GPS antenna (JAUS)
 
73
                bool align;
 
74
 
 
75
                std::auto_ptr<gna::Config> config;
 
76
                std::auto_ptr<gna::Driver> driver;
 
77
                
 
78
                double imuToGpsOffset[3];
 
79
                
 
80
                double magaz, magazstd; // azimuth according to magnetometer, standard deviation in the azimuth estimate from the magnetometer
 
81
                int num_mag_avg;
 
82
                double mag[2], magbias[2]; // magnetometer reading (X,Y), bias in the magnetometer reading
 
83
                
 
84
                double tfoffsetX,  tfoffsetY, tfoffsetZ; // offsets for TF
 
85
 
 
86
                int jaus2novatel_frame(std::vector<double> *vectorArg);
 
87
                int getInitAzimuth(std::vector<double> *vectorArg);
 
88
                void getLocalPos(double latitude, double longitude, double height, double *X, double *Y, double *Z);
 
89
                
 
90
                SMALL::Rotation3D rot3D, rot3Dgps; // internal tracking of vehicle orientation
 
91
                
 
92
        public:         
 
93
                NovatelInterfaceROS();
 
94
                ~NovatelInterfaceROS();
 
95
                void imu3dmgCallback(const sensor_msgs::Imu::ConstPtr& data);
 
96
                int setupROS();
 
97
                int reset();
 
98
                int setupDriver();
 
99
                int start(); 
 
100
                int read_and_process_data();
 
101
                int setup();
 
102
                        
 
103
};
 
104
                
 
105
#endif // NOVATEL_INTERFACE_H