odometry.cc File Reference

vehicle position and velocity monitoring More...

#include <ros/ros.h>
#include <angles/angles.h>
#include <algorithm>
#include <cmath>
#include <stdio.h>
#include <string>
#include <vector>
#include <ostream>
#include "ros/serialization.h"
#include "ros/builtin_message_traits.h"
#include "ros/message.h"
#include "ros/time.h"
#include "ros/message_operations.h"
#include "geometry_msgs/Vector3.h"
#include "std_msgs/Header.h"
#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/Pose.h"
#include <iostream>
#include <iomanip>
#include <sstream>
#include <map>
#include <stdexcept>
#include <list>
#include <boost/thread/mutex.hpp>
#include "geometry_msgs/Point.h"
#include "geometry_msgs/PoseStamped.h"
#include "btMatrix3x3.h"
#include "ros/console.h"
#include "tf/exceptions.h"
#include "LinearMath/btTransform.h"
#include <boost/signals.hpp>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TransformStamped.h"
#include <tf/tf.h>
#include <stdlib.h>
#include <math.h>
#include <sys/time.h>
#include <time.h>
#include "applanix.h"
#include <angles/angles.h>

Include dependency graph for odometry.cc:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Defines
#define	NODE   "applanix"
Functions
void	displayHelp ()
bool	getNewData (applanix_data_t *adata)
bool	getOdom (Position::Position3D *odom_pos3d, ros::Time *odom_time, ros::Publisher *gps_pub)
int	getParameters (int argc, char *argv[])
void	getShifter (const art_msgs::Shifter::ConstPtr &shifterIn)
bool	GlobalToLocal (Position::Pose3D *current)
int	main (int argc, char **argv)
void	publishGPS (const applanix_data_t &adata, double utm_e, double utm_n, const char *zone, ros::Publisher *gps_pub)
void	putPose (const Position::Position3D *odom_pos3d, const ros::Time *odom_time, tf::TransformBroadcaster *odom_broad, ros::Publisher *odom_pub, ros::Publisher *imu_pub)

---

Detailed Description

vehicle position and velocity monitoring

ROS odometry driver for the Applanix Position and Orientation System for Land Vehicles (POS-LV).

The odometry driver publishes its best estimate of the vehicle's location, velocity and yaw rate. It collects data from the Applanix Position and Orientation System for Land Vehicles (POS-LV) which provides differential GPS and accurate inertial navigation.

Publishes

• odom (nav_msgs/Odometry): Current estimate of vehicle position and velocity in three dimensions, including roll, pitch, and yaw. All data are in the /odom frame of reference.

• imu (sensor_msgs/Imu): Current inertial measurements from the Applanix.

• gps (art_msgs/GpsInfo): Current GPS status from the Applanix.

• tf: broadcast transform from vehicle frame to odom frame.

Subscribes

• shifter/state: current transmission gear

Author:

Jack O'Quin, Patrick Beeson

Definition in file odometry.cc.

---

Define Documentation

#define NODE   "applanix"

Definition at line 59 of file odometry.cc.

---

Function Documentation

void displayHelp

(

)

Definition at line 370 of file odometry.cc.

bool getNewData

(

applanix_data_t *

adata

)

Get new Applanix data.

Returns:

true if new data available.

Updates *adata if possible.

Definition at line 143 of file odometry.cc.

bool getOdom	(	Position::Position3D *	odom_pos3d,
		ros::Time *	odom_time,
		ros::Publisher *	gps_pub
	)

Get any new odometry data available

Updates odometry information, which will be published if valid packets available and this is not the first call.

Publishes GPS information, if new data received.

Parameters:

	odom_pos3d	-> updated position, if new data
	odom_time	-> updated time when new data received
	gps_pub	-> GpsInfo ROS topic publisher

Returns:

true if odometry should be published

Definition at line 231 of file odometry.cc.

int getParameters	(	int	argc,
		char *	argv[]
	)

get command line and ROS parameters

Returns:

0 if successful

Definition at line 392 of file odometry.cc.

void getShifter

(

const art_msgs::Shifter::ConstPtr &

shifterIn

)

subscriber callback for current shifter position data

Definition at line 291 of file odometry.cc.

bool GlobalToLocal

(

Position::Pose3D *

current

)

Global to local coordinate transform.

Translate current pose from UTM meters (northing, easting) to local coordinates by subtracting initial pose.

returns: true if initial pose, false otherwise

Definition at line 86 of file odometry.cc.

int main	(	int	argc,
		char **	argv
	)

main program

Definition at line 441 of file odometry.cc.

void publishGPS	(	const applanix_data_t &	adata,
		double	utm_e,
		double	utm_n,
		const char *	zone,
		ros::Publisher *	gps_pub
	)

Publish GpsInfo message.

Todo:

unpack Applanix grp2 and grp3 data to complete other fields

Definition at line 187 of file odometry.cc.

void putPose	(	const Position::Position3D *	odom_pos3d,
		const ros::Time *	odom_time,
		tf::TransformBroadcaster *	odom_broad,
		ros::Publisher *	odom_pub,
		ros::Publisher *	imu_pub
	)

Publish the current 3D Pose and accelerations

Todo:

figure covariances of Pose and Twist

Todo:

figure covariances of IMU data

Definition at line 300 of file odometry.cc.