任務(wù)介紹：
1:?? 分別發(fā)布高斯投影下的GPS消息和UTM下的GPS消息留拾；
2：訂閱GPS消息末早，并將激光檢測(cè)到的平面的位置和角度通過自定義消息類型發(fā)布出去幽钢；
任務(wù)1：發(fā)布轉(zhuǎn)換后的兩種GPS
在/src下創(chuàng)建的gps.cpp文件（包含main()的文件）中：
需要為發(fā)布和訂閱添加頭文件：
```
#include "ros/ros.h"??? //使用ROS必須包含的頭文件
#include <geometry_msgs/PoseStamped.h> //發(fā)布的GPS消息類型屬于geometry_msgs/PoseStamped
```
其中的geometry_msgs/PoseStamped.h可以在ROS官網(wǎng)中查到
這里先附上GPS轉(zhuǎn)到高斯克呂格投影下和UTM坐標(biāo)系下的函數(shù)：
??????? 高斯克呂格投影：
```
void GaussProjCal(double longitude, double latitude, double &X, double &Y)
{
??? int ProjNo = 0; int ZoneWide;???? //帶寬
??? double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
??? double a, f, e2, ee, NN, T, C, A, M, iPI;
??? iPI = 0.0174532925199433;
??? ZoneWide = 3;//wgs84? 3度帶
??? a = 6378137;
??? f = 1 / 298.2572236; //WGS84橢球參數(shù)
??? //ProjNo = trunc((longitude - 1.5) / 3) + 1;//計(jì)算度帶號(hào)
??? ProjNo = 40;
??? longitude0 = ProjNo * ZoneWide;//計(jì)算中央經(jīng)線
??? longitude0 = longitude0 * iPI;
??? latitude0 = 0;
??? longitude1 = longitude * iPI; //經(jīng)度轉(zhuǎn)換為弧度
??? latitude1 = latitude * iPI; //緯度轉(zhuǎn)換為弧度
??? e2 = 2 * f - f*f;
??? ee = e2 * (1.0 - e2);
??? NN = a / sqrt(1.0 - e2 * sin(latitude1) * sin(latitude1));
??? T = tan(latitude1) * tan(latitude1);
??? C = ee * cos(latitude1) * cos(latitude1);
??? A = (longitude1 - longitude0) * cos(latitude1);
??? M = a * ((1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 *e2 / 256) * latitude1
?????????? - (3 * e2 / 8 + 3 * e2 * e2 / 32 + 45 * e2 * e2 * e2 / 1024) * sin(2 * latitude1)
?????????? + (15 * e2 *e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1)
?????????? - (35 * e2 * e2 * e2 / 3072) * sin(6 * latitude1));
??? xval = NN * (A + (1 - T + C) * A * A * A / 6
?????????????????? + (5 - 18 * T + T*T + 72 * C - 58 * ee)* A * A * A * A * A / 120);
??? yval = M + NN * tan(latitude1) * (A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24
???????????? + (61 - 58 * T + T * T + 600 * C - 330 * ee)* A * A * A * A * A * A / 720);
??? X0 = /*1000000L * ProjNo*/ + 500000L;//3度帶內(nèi)的相對(duì)平面坐標(biāo)
??? Y0 = 0;
??? xval = xval + X0; yval = yval + Y0;
??? X = xval;
??? Y = yval;
}
```
??????? UTM坐標(biāo)系：
```
void GC2UTM(const double longitude_, const double latitude_, const double height_ellipsoid_,
double *utm_x, double *utm_y, double *utm_z)
{
int ProjNo = 0;
int ZoneWide = 0;
double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
double a, f, e2, ee, NN, T, C, A, M, iPI;
iPI = 0.017453293;//=DEGREE_TO_RADIAN;
ZoneWide = 3; //3 DEGREE width
a = 6378137;
f = 1 / 298.257223563;//earth ellipse constant
double longitude = longitude_;
double latitude = latitude_;
double altitude = height_ellipsoid_;
ProjNo = (int)(longitude / ZoneWide) ;
longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
longitude0 = longitude0 * iPI ;
latitude0? = 0;
longitude1 = longitude * iPI ; //經(jīng)度轉(zhuǎn)換為弧度
latitude1 = latitude * iPI ; //緯度轉(zhuǎn)換為弧度
e2 = 2 * f - f * f;
ee = e2 * (1.0 - e2);
NN = a / sqrt(1.0 - e2 * sin(latitude1) * sin(latitude1));
T = tan(latitude1) * tan(latitude1);
C = ee * cos(latitude1) * cos(latitude1);
A = (longitude1 - longitude0) * cos(latitude1);
M = a * ((1 - e2 / 4 - 3 * e2 * e2 / 64 - 5 * e2 * e2 * e2 / 256) * latitude1 -
(3 * e2 / 8 + 3 * e2 * e2 / 32 + 45 * e2 * e2 * e2 / 1024) * sin(2 * latitude1)+
(15 * e2 * e2 / 256 + 45 * e2 * e2 * e2 / 1024) * sin(4 * latitude1) -
(35 * e2 * e2 * e2 / 3072) * sin(6 * latitude1));
xval = NN * (A + (1 - T + C) * A * A * A /6 +
(5 - 18 * T + T * T + 72 * C - 58 * ee) *? A * A * A * A * A / 120);
yval = M + NN * tan(latitude1) * (A * A/2 + (5 - T + 9 * C + 4 *? C* C) * A * A * A * A / 24
+(61 - 58 * T + T * T + 600 * C - 330 * ee) * A * A * A * A * A * A / 720);
X0 = 1000000L * (ProjNo + 1) + 500000L;
Y0 = 0;
xval = xval + X0;
yval = yval + Y0;
*utm_x = xval;
*utm_y = yval;
*utm_z = altitude;
}
```
接下來在main函數(shù)內(nèi)添加以下內(nèi)容：
```
??? ros::init(argc,argv,"gps_pub"); //初始化ROS，節(jié)點(diǎn)名稱為gps_pub
??? ros::NodeHandle n;???????????????? //ROS系統(tǒng)通訊
??? ros::Publisher gps_utm_puber = n.advertise<geometry_msgs::PoseStamped>("/gps_pose_utm", 1);?? //定義 要publish的消息類型和消息名稱谈竿，1為消息緩沖的數(shù)量?
??? ros::Publisher gps_gauss_puber = n.advertise<geometry_msgs::PoseStamped>("/gps_pose_gauss", 1);?
??? geometry_msgs::PoseStamped geo_utm_msg; //定義了 geometry_msgs::PoseStamped的對(duì)象 geo_utm_msg
??? geometry_msgs::PoseStamped geo_gauss_msg;?
```
主函數(shù)分別調(diào)用GaussProjCal(lon, lat, x, y)和GC2UTM(lon, lat, hei, utm_x, utm_y, utm_z)：
```
??????????????????? GaussProjCal(lon, lat, x, y);
??????????????????? gauss_msg.lon = x;
??????????????????? gauss_msg.lat = y;
??????????????????? gauss_msg.height = 0;
??????????????????? gauss_msg.yaw = gpsimu.yaw;
??????????????????? gauss_msg.roll = gpsimu.roll;
??????????????????? gauss_msg.pitch = gpsimu.pitch;
??????????????????? double hei = gpsimu.height;
??????????????????? geo_gauss_msg.header.stamp = ros::Time::now();
??????????????????? geo_gauss_msg.pose.position.x = x - origin_E;
??????????????????? geo_gauss_msg.pose.position.y = y - origin_N;
??????????????????? geo_gauss_msg.pose.position.z = 0;
??????????????????? Eigen::AngleAxisd rollAngle(0, Eigen::Vector3d::UnitX());
??????????????????? Eigen::AngleAxisd pitchAngle(0, Eigen::Vector3d::UnitY());
??????????????????? Eigen::AngleAxisd yawAngle( M_PI /2.0 - gpsimu.yaw / 180.0 * M_PI, Eigen::Vector3d::UnitZ());
??????????????????? Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
??????????????????? geo_gauss_msg.pose.orientation.w = q.w();
??????????????????? geo_gauss_msg.pose.orientation.x = q.x();
??????????????????? geo_gauss_msg.pose.orientation.y = q.y();
??????????????????? geo_gauss_msg.pose.orientation.z = q.z();
??????????????????? gps_gauss_puber.publish(geo_gauss_msg);//發(fā)布高斯GPS
??????????????????? GC2UTM(lon, lat, hei, utm_x, utm_y, utm_z);
??????????????????? geo_utm_msg.header.stamp = ros::Time::now();
??????????????????? geo_utm_msg.pose.position.x = *utm_x - origin_E;?
??????????????????? geo_utm_msg.pose.position.y = *utm_y - origin_N;
??????????????????? geo_utm_msg.pose.position.z = 0;
??????????????????? geo_utm_msg.pose.orientation.w = q.w();
??????????????????? geo_utm_msg.pose.orientation.x = q.x();
??????????????????? geo_utm_msg.pose.orientation.y = q.y();
??????????????????? geo_utm_msg.pose.orientation.z = q.z();
??????????????????? gps_utm_puber.publish(geo_utm_msg); //發(fā)布UTM_GPS
```
在CMakeLists.txt中找到## Declare a C++ executable团驱，在下邊添加
```
add_executable(${PROJECT_NAME}_node
??? src/gps.cpp
??? src/kvaserToDBC.cpp
??? src/interpreter.cpp)
target_link_libraries(${PROJECT_NAME}_node
?? ${catkin_LIBRARIES}
?? canlib
?? kvadblib
?)
```
add_executable表示要添加一個(gè)可執(zhí)行文件，${PROJECT_NAME}_node為這個(gè)可執(zhí)行文件的名字空凸，src/gps.cpp嚎花，src/kvaserToDBC.cpp和src/interpreter.cpp為編譯這個(gè)工程的源文件位置；
target_link_libraries表示我們要將可執(zhí)行文件鏈接到的庫呀洲，因?yàn)樵嫉腉PS是通過can發(fā)送過來的紊选，所以用到了canlib和kvadblib
至此，編譯道逗，運(yùn)行之后即實(shí)現(xiàn)了ROS下兩種類型GPS的發(fā)布兵罢。
