novatel计算odom--GPS坐标与UTM坐标转换

保证你的novatel的driver是在ros-drivers上的驱动

1 简介


1.1 消息


gps_common定义了两个通用消息,供GPS驱动程序输出:gps_common/GPSFix和gps_common/GPSStatus。

在大多数情况下,这些消息应同时发布,并带有相同的时间戳。

1.2 utm_odometry_node节点


utm_odometry_node将经纬度坐标转换为UTM坐标。

1.3 订阅的话题


fix (sensor_msgs/NavSatFix):GPS测量和状态

1.4 发布的话题


odom (nav_msgs/Odometry):UTM编码的位置

1.5 参数


~rot_covariance (double, default: 99999):指定旋转测量的方差(以米为单位)
~frame_id (string, default: Copy frame_id from fix message): Frame to specify in header of outgoing Odometry message
~child_frame_id (string): Child frame to specify in header of outgoing Odometry message


2 安装


mkdir -p ~/gps_ws/src
cd ~/gps_ws/src
git clone https://github.com/swri-robotics/gps_umd.git
cd ..
catkin_make

报错:

CMake Error at /usr/share/cmake-3.5/Modules/FindPkgConfig.cmake:367 (message):
A required package was not found
Call Stack (most recent call first):
/usr/share/cmake-3.5/Modules/FindPkgConfig.cmake:532 (_pkg_check_modules_internal)
gps_umd/gpsd_client/CMakeLists.txt:15 (pkg_check_modules)

– Configuring incomplete, errors occurred!

解决办法:

sudo apt-get install libgps-dev

最后,重新编译:

catkin_make

3 GPS坐标与UTM坐标的转换

先写这个,个人认为用得比较多。

3.1 GPS坐标转换为UTM坐标

源文件utm_odometry_node.cpp:

/*
 * Translates sensor_msgs/NavSat{Fix,Status} into nav_msgs/Odometry using UTM
 */

#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace gps_common;

static ros::Publisher odom_pub;
std::string frame_id, child_frame_id;
double rot_cov;
bool append_zone = false;

void callback(const sensor_msgs::NavSatFixConstPtr& fix) {
  if (fix->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
    ROS_DEBUG_THROTTLE(60,"No fix.");
    return;
  }

  if (fix->header.stamp == ros::Time(0)) {
    return;
  }

  double northing, easting;
  std::string zone;

  LLtoUTM(fix->latitude, fix->longitude, northing, easting, zone);

  if (odom_pub) {
    nav_msgs::Odometry odom;
    odom.header.stamp = fix->header.stamp;

    if (frame_id.empty()) {
      if(append_zone) {
        odom.header.frame_id = fix->header.frame_id + "/utm_" + zone;
      } else {
        odom.header.frame_id = fix->header.frame_id;
      }
    } else {
      if(append_zone) {
        odom.header.frame_id = frame_id + "/utm_" + zone;
      } else {
        odom.header.frame_id = frame_id;
      }
    }

    odom.child_frame_id = child_frame_id;

    odom.pose.pose.position.x = easting;
    odom.pose.pose.position.y = northing;
    odom.pose.pose.position.z = fix->altitude;
    
    odom.pose.pose.orientation.x = 0;
    odom.pose.pose.orientation.y = 0;
    odom.pose.pose.orientation.z = 0;
    odom.pose.pose.orientation.w = 1;
    
    // Use ENU covariance to build XYZRPY covariance
    boost::array covariance = {{
      fix->position_covariance[0],
      fix->position_covariance[1],
      fix->position_covariance[2],
      0, 0, 0,
      fix->position_covariance[3],
      fix->position_covariance[4],
      fix->position_covariance[5],
      0, 0, 0,
      fix->position_covariance[6],
      fix->position_covariance[7],
      fix->position_covariance[8],
      0, 0, 0,
      0, 0, 0, rot_cov, 0, 0,
      0, 0, 0, 0, rot_cov, 0,
      0, 0, 0, 0, 0, rot_cov
    }};

    odom.pose.covariance = covariance;

    odom_pub.publish(odom);
  }
}

int main (int argc, char **argv) {
  ros::init(argc, argv, "utm_odometry_node");
  ros::NodeHandle node;
  ros::NodeHandle priv_node("~");

  priv_node.param("frame_id", frame_id, "");
  priv_node.param("child_frame_id", child_frame_id, "");
  priv_node.param("rot_covariance", rot_cov, 99999.0);
  priv_node.param("append_zone", append_zone, false);

  odom_pub = node.advertise("odom", 10);

  ros::Subscriber fix_sub = node.subscribe("fix", 10, callback);

  ros::spin();
}

坐标转换函数所在的头文件conversion.h:

/* Taken from utexas-art-ros-pkg:art_vehicle/applanix */

/*
 * Conversions between coordinate systems.
 *
 * Includes LatLong<->UTM.
 */

#ifndef _UTM_H
#define _UTM_H

/**  @file

     @brief Universal Transverse Mercator transforms.

     Functions to convert (spherical) latitude and longitude to and
     from (Euclidean) UTM coordinates.

     @author Chuck Gantz- [email protected]

 */

#include 
#include 
#include 

namespace gps_common
{

const double RADIANS_PER_DEGREE = M_PI/180.0;
const double DEGREES_PER_RADIAN = 180.0/M_PI;

// WGS84 Parameters
const double WGS84_A = 6378137.0;		// major axis
const double WGS84_B = 6356752.31424518;	// minor axis
const double WGS84_F = 0.0033528107;		// ellipsoid flattening
const double WGS84_E = 0.0818191908;		// first eccentricity
const double WGS84_EP = 0.0820944379;		// second eccentricity

// UTM Parameters
const double UTM_K0 = 0.9996;			// scale factor
const double UTM_FE = 500000.0;		// false easting
const double UTM_FN_N = 0.0;			// false northing on north hemisphere
const double UTM_FN_S = 10000000.0;		// false northing on south hemisphere
const double UTM_E2 = (WGS84_E*WGS84_E);	// e^2
const double UTM_E4 = (UTM_E2*UTM_E2);		// e^4
const double UTM_E6 = (UTM_E4*UTM_E2);		// e^6
const double UTM_EP2 = (UTM_E2/(1-UTM_E2));	// e'^2

/**
 * Utility function to convert geodetic to UTM position
 *
 * Units in are floating point degrees (sign for east/west)
 *
 * Units out are meters
 */
static inline void UTM(double lat, double lon, double *x, double *y)
{
  // constants
  const static double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);
  const static double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);
  const static double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);
  const static double m3 = -(35*UTM_E6/3072);

  // compute the central meridian
  int cm = ((lon >= 0.0)
	    ? ((int)lon - ((int)lon)%6 + 3)
	    : ((int)lon - ((int)lon)%6 - 3));

  // convert degrees into radians
  double rlat = lat * RADIANS_PER_DEGREE;
  double rlon = lon * RADIANS_PER_DEGREE;
  double rlon0 = cm * RADIANS_PER_DEGREE;

  // compute trigonometric functions
  double slat = sin(rlat);
  double clat = cos(rlat);
  double tlat = tan(rlat);

  // decide the false northing at origin
  double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;

  double T = tlat * tlat;
  double C = UTM_EP2 * clat * clat;
  double A = (rlon - rlon0) * clat;
  double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)
			+ m2*sin(4*rlat) + m3*sin(6*rlat));
  double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);

  // compute the easting-northing coordinates
  *x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A,3)/6
			      + (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A,5)/120);
  *y = fn + UTM_K0 * (M + V * tlat * (A*A/2
				      + (5-T+9*C+4*C*C)*pow(A,4)/24
				      + ((61-58*T+T*T+600*C-330*UTM_EP2)
					 * pow(A,6)/720)));

  return;
}

/**
 * Determine the correct UTM letter designator for the
 * given latitude
 *
 * @returns 'Z' if latitude is outside the UTM limits of 84N to 80S
 *
 * Written by Chuck Gantz- [email protected]
 */
static inline char UTMLetterDesignator(double Lat)
{
	char LetterDesignator;

	if     ((84 >= Lat) && (Lat >= 72))  LetterDesignator = 'X';
	else if ((72 > Lat) && (Lat >= 64))  LetterDesignator = 'W';
	else if ((64 > Lat) && (Lat >= 56))  LetterDesignator = 'V';
	else if ((56 > Lat) && (Lat >= 48))  LetterDesignator = 'U';
	else if ((48 > Lat) && (Lat >= 40))  LetterDesignator = 'T';
	else if ((40 > Lat) && (Lat >= 32))  LetterDesignator = 'S';
	else if ((32 > Lat) && (Lat >= 24))  LetterDesignator = 'R';
	else if ((24 > Lat) && (Lat >= 16))  LetterDesignator = 'Q';
	else if ((16 > Lat) && (Lat >= 8))   LetterDesignator = 'P';
	else if (( 8 > Lat) && (Lat >= 0))   LetterDesignator = 'N';
	else if (( 0 > Lat) && (Lat >= -8))  LetterDesignator = 'M';
	else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
	else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
	else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
	else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
	else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
	else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
	else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
	else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
	else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
        // 'Z' is an error flag, the Latitude is outside the UTM limits
	else LetterDesignator = 'Z';
	return LetterDesignator;
}

/**
 * Convert lat/long to UTM coords.  Equations from USGS Bulletin 1532
 *
 * East Longitudes are positive, West longitudes are negative.
 * North latitudes are positive, South latitudes are negative
 * Lat and Long are in fractional degrees
 *
 * Written by Chuck Gantz- [email protected]
 */
static inline void LLtoUTM(const double Lat, const double Long,
                           double &UTMNorthing, double &UTMEasting,
                           char* UTMZone)
{
	double a = WGS84_A;
	double eccSquared = UTM_E2;
	double k0 = UTM_K0;

	double LongOrigin;
	double eccPrimeSquared;
	double N, T, C, A, M;

        //Make sure the longitude is between -180.00 .. 179.9
	double LongTemp = (Long+180)-int((Long+180)/360)*360-180;

	double LatRad = Lat*RADIANS_PER_DEGREE;
	double LongRad = LongTemp*RADIANS_PER_DEGREE;
	double LongOriginRad;
	int    ZoneNumber;

	ZoneNumber = int((LongTemp + 180)/6) + 1;

	if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
		ZoneNumber = 32;

        // Special zones for Svalbard
	if( Lat >= 72.0 && Lat < 84.0 )
	{
	  if(      LongTemp >= 0.0  && LongTemp <  9.0 ) ZoneNumber = 31;
	  else if( LongTemp >= 9.0  && LongTemp < 21.0 ) ZoneNumber = 33;
	  else if( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
	  else if( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
	 }
        // +3 puts origin in middle of zone
	LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
	LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;

	//compute the UTM Zone from the latitude and longitude
	snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));

	eccPrimeSquared = (eccSquared)/(1-eccSquared);

	N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
	T = tan(LatRad)*tan(LatRad);
	C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
	A = cos(LatRad)*(LongRad-LongOriginRad);

	M = a*((1	- eccSquared/4		- 3*eccSquared*eccSquared/64	- 5*eccSquared*eccSquared*eccSquared/256)*LatRad
				- (3*eccSquared/8	+ 3*eccSquared*eccSquared/32	+ 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
									+ (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
									- (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));

	UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6
					+ (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
					+ 500000.0);

	UTMNorthing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
				 + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
	if(Lat < 0)
		UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere
}

static inline void LLtoUTM(const double Lat, const double Long,
                           double &UTMNorthing, double &UTMEasting,
                           std::string &UTMZone) {
  char zone_buf[] = {0, 0, 0, 0};

  LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, zone_buf);

  UTMZone = zone_buf;
}

/**
 * Converts UTM coords to lat/long.  Equations from USGS Bulletin 1532
 *
 * East Longitudes are positive, West longitudes are negative.
 * North latitudes are positive, South latitudes are negative
 * Lat and Long are in fractional degrees.
 *
 * Written by Chuck Gantz- [email protected]
 */
static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
                           const char* UTMZone, double& Lat,  double& Long )
{
	double k0 = UTM_K0;
	double a = WGS84_A;
	double eccSquared = UTM_E2;
	double eccPrimeSquared;
	double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
	double N1, T1, C1, R1, D, M;
	double LongOrigin;
	double mu, phi1Rad;
	double x, y;
	int ZoneNumber;
	char* ZoneLetter;

	x = UTMEasting - 500000.0; //remove 500,000 meter offset for longitude
	y = UTMNorthing;

	ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
	if((*ZoneLetter - 'N') < 0)
	{
		y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere
	}

	LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;  //+3 puts origin in middle of zone

	eccPrimeSquared = (eccSquared)/(1-eccSquared);

	M = y / k0;
	mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));

	phi1Rad = mu	+ (3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
				+ (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
				+(151*e1*e1*e1/96)*sin(6*mu);

	N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
	T1 = tan(phi1Rad)*tan(phi1Rad);
	C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
	R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
	D = x/(N1*k0);

	Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
					+(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
	Lat = Lat * DEGREES_PER_RADIAN;

	Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
					*D*D*D*D*D/120)/cos(phi1Rad);
	Long = LongOrigin + Long * DEGREES_PER_RADIAN;

}

static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
    std::string UTMZone, double& Lat, double& Long) {
  UTMtoLL(UTMNorthing, UTMEasting, UTMZone.c_str(), Lat, Long);
}
} // end namespace UTM

#endif // _UTM_H

 

运行:

rosrun gps_common utm_odometry_node

3.2 UTM坐标转换为GPS坐标

代码在~/gps_ws/src/gps_umd/gps_common/src/utm_odometry_to_navsatfix_node.cpp,这里就不贴了。

注:代码写得标准又易读,值得学习,这也是我在这里贴代码的原因。

 

4 sensor_msgs/NavSatFix与gps_common/GPSFix的转换


sensor_msgs/NavSatFix的内容见:http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html

gps_common/GPSFix的路径为~/tingjiandan/gps_ws/src/gps_umd/gps_common/msg/GPSFix.msg,描述如下:

# A more complete GPS fix to supplement sensor_msgs/NavSatFix.

主函数fix_translator.py:

#!/usr/bin/env python

# Translates from NavSatFix to GPSFix and back

import rospy
from sensor_msgs.msg import NavSatFix
from gps_common.msg import GPSFix
import gps_common.gps_message_converter as converter

navsat_pub = rospy.Publisher('navsat_fix_out', NavSatFix, queue_size=10)
gps_pub = rospy.Publisher('gps_fix_out', GPSFix, queue_size=10)

def navsat_callback(navsat_msg):
    gps_msg = converter.navsatfix_to_gpsfix(navsat_msg)
    gps_pub.publish(gps_msg)

# Translates from GPSFix to NavSatFix.
# As GPSFix can store much more information than NavSatFix, 
# a lot of this additional information might get lost.
def gps_callback(gps_msg):
    navsat_msg = converter.gpsfix_to_navsatfix(gps_msg)
    navsat_pub.publish(navsat_msg)

if __name__ == '__main__':
    rospy.init_node('fix_translator', anonymous=True)
    navsat_sub = rospy.Subscriber("navsat_fix_in", NavSatFix, navsat_callback)
    gps_sub = rospy.Subscriber("gps_fix_in", GPSFix, gps_callback)
    rospy.spin()



消息转换函数gps_message_converter.py:

from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import NavSatStatus
from gps_common.msg import GPSFix
from gps_common.msg import GPSStatus

def navsatfix_to_gpsfix(navsat_msg):
    # Convert sensor_msgs/NavSatFix messages to gps_common/GPSFix messages
    gpsfix_msg = GPSFix()
    gpsfix_msg.header = navsat_msg.header
    gpsfix_msg.status.status = navsat_msg.status.status

    gpsfix_msg.status.motion_source = GPSStatus.SOURCE_NONE
    gpsfix_msg.status.orientation_source = GPSStatus.SOURCE_NONE
    gpsfix_msg.status.position_source = GPSStatus.SOURCE_NONE
    if ((navsat_msg.status.service & NavSatStatus.SERVICE_GPS) or
            (navsat_msg.status.service & NavSatStatus.SERVICE_GLONASS) or
            (navsat_msg.status.service & NavSatStatus.SERVICE_GALILEO)):
        gpsfix_msg.status.motion_source = \
            gpsfix_msg.status.motion_source | GPSStatus.SOURCE_GPS
        gpsfix_msg.status.orientation_source = \
            gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_GPS
        gpsfix_msg.status.position_source = \
            gpsfix_msg.status.position_source | GPSStatus.SOURCE_GPS

    if navsat_msg.status.service & NavSatStatus.SERVICE_COMPASS:
        gpsfix_msg.status.orientation_source = \
            gpsfix_msg.status.orientation_source | GPSStatus.SOURCE_MAGNETIC

    gpsfix_msg.latitude = navsat_msg.latitude
    gpsfix_msg.longitude = navsat_msg.longitude
    gpsfix_msg.altitude = navsat_msg.altitude
    gpsfix_msg.position_covariance = navsat_msg.position_covariance
    gpsfix_msg.position_covariance_type = navsat_msg.position_covariance_type

    return gpsfix_msg

def gpsfix_to_navsatfix(gpsfix_msg):
    # Convert gps_common/GPSFix messages to sensor_msgs/NavSatFix messages
    navsat_msg = NavSatFix()
    navsat_msg.header = gpsfix_msg.header

    # Caution: GPSFix has defined some additional status constants, which are
    # not defined in NavSatFix.
    navsat_msg.status.status = gpsfix_msg.status.status

    navsat_msg.status.service = 0
    if gpsfix_msg.status.position_source & GPSStatus.SOURCE_GPS:
        navsat_msg.status.service = \
            navsat_msg.status.service | NavSatStatus.SERVICE_GPS
    if gpsfix_msg.status.orientation_source & GPSStatus.SOURCE_MAGNETIC:
        navsat_msg.status.service = \
            navsat_msg.status.service | NavSatStatus.SERVICE_COMPASS

    navsat_msg.latitude = gpsfix_msg.latitude
    navsat_msg.longitude = gpsfix_msg.longitude
    navsat_msg.altitude = gpsfix_msg.altitude
    navsat_msg.position_covariance = gpsfix_msg.position_covariance
    navsat_msg.position_covariance_type = gpsfix_msg.position_covariance_type

    return navsat_msg


launch文件fix_translator.launch:





  
    
             
      

    
     
  



注释写得很详细,就不翻译了。

运行:

roslaunch gps_common fix_translator.launch


注:ROS支持Python,这个项目同时使用了C++和Python,值得学习一下哦。

5 参考


[1] Github: http://wiki.ros.org/gps_common
[2] ROS wiki: http://wiki.ros.org/gps_common
 

你可能感兴趣的:(【SLAM探索】)