任务介绍：
1: 分别发布高斯投影下的GPS消息和UTM下的GPS消息；
2：订阅GPS消息，并将激光检测到的平面的位置和角度通过自定义消息类型发布出去；
任务1：发布转换后的两种GPS
在/src下创建的gps.cpp文件（包含main()的文件）中：
需要为发布和订阅添加头文件：
```
#include "ros/ros.h" //使用ROS必须包含的头文件
#include <geometry_msgs/PoseStamped.h> //发布的GPS消息类型属于geometry_msgs/PoseStamped
```
其中的geometry_msgs/PoseStamped.h可以在ROS官网中查到
这里先附上GPS转到高斯克吕格投影下和UTM坐标系下的函数：
高斯克吕格投影：
```
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椭球参数
//ProjNo = trunc((longitude - 1.5) / 3) + 1;//计算度带号
ProjNo = 40;
longitude0 = ProjNo * ZoneWide;//计算中央经线
longitude0 = longitude0 * iPI;
latitude0 = 0;
longitude1 = longitude * iPI; //经度转换为弧度
latitude1 = latitude * iPI; //纬度转换为弧度
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度带内的相对平面坐标
Y0 = 0;
xval = xval + X0; yval = yval + Y0;
X = xval;
Y = yval;
}
```
UTM坐标系：
```
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 ; //经度转换为弧度
latitude1 = latitude * iPI ; //纬度转换为弧度
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函数内添加以下内容：
```
ros::init(argc,argv,"gps_pub"); //初始化ROS，节点名称为gps_pub
ros::NodeHandle n; //ROS系统通讯
ros::Publisher gps_utm_puber = n.advertise<geometry_msgs::PoseStamped>("/gps_pose_utm", 1); //定义 要publish的消息类型和消息名称，1为消息缓冲的数量
ros::Publisher gps_gauss_puber = n.advertise<geometry_msgs::PoseStamped>("/gps_pose_gauss", 1);
geometry_msgs::PoseStamped geo_utm_msg; //定义了 geometry_msgs::PoseStamped的对象 geo_utm_msg
geometry_msgs::PoseStamped geo_gauss_msg;
```
主函数分别调用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);//发布高斯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); //发布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表示要添加一个可执行文件，${PROJECT_NAME}_node为这个可执行文件的名字，src/gps.cpp，src/kvaserToDBC.cpp和src/interpreter.cpp为编译这个工程的源文件位置；
target_link_libraries表示我们要将可执行文件链接到的库，因为原始的GPS是通过can发送过来的，所以用到了canlib和kvadblib
至此，编译，运行之后即实现了ROS下两种类型GPS的发布。
网友评论