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 <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/NavSatStatus.h>
#include <sensor_msgs/NavSatFix.h>
#include <gps_common/conversions.h>
#include <nav_msgs/Odometry.h>
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<double, 36> 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<std::string>("frame_id", frame_id, "");
priv_node.param<std::string>("child_frame_id", child_frame_id, "");
priv_node.param<double>("rot_covariance", rot_cov, 99999.0);
priv_node.param<bool>("append_zone", append_zone, false);
odom_pub = node.advertise<nav_msgs::Odometry>("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 <cmath>
#include <cstdio>
#include <cstdlib>
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
或者，創建launch文件utm_odometry_node.launch：
<launch>
<node name="gps_conv" pkg="gps_common" type="utm_odometry_node">
<remap from="odom" to="vo"/>
<remap from="fix" to="/gps/fix" />
<param name="rot_covariance" value="99999" />
<param name="frame_id" value="base_footprint" />
</node>
</launch>
注意：參數根據自己的需求自定義。
然後運行：
roslaunch gps_common utm_odometry_node.launch
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:
<launch>
<!--
fix_translator translates to and from NatSatFix
and GPSFix messages.
If you want to translate from NavSatFix to GPSFix,
you have to modify the first two remap lines.
If you want to translate from GPSFix to NavSatFix,
you have to uncomment and modify the last two remap
lines.
Only adjust topic names after "to=" in each remap line.
-->
<node name="fix_translator" pkg="gps_common" type="fix_translator">
<!-- Translate from NavSatFix to GPSFix //-->
<remap from="/navsat_fix_in" to="/fix"/>
<remap from="/gps_fix_out" to="/gps_fix"/>
<!-- Translate from GPSFix to NavSatFix //-->
<!--
<remap from="/gps_fix_in" to="/YOUR_GPSFIX_TOPIC"/>
<remap from="/navsat_fix_out" to="/YOUR_NAVSATFIX_TOPIC"/>
//-->
</node>
</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