任务2：
订阅GPS消息，并将激光检测到的平面的位置和角度通过自定义消息类型发布出去；
这个任务可分为两个：订阅ROS标准类型的GPS消息；定义并发布自己的消息
1 .订阅ROS标准类型的GPS消息
符合ROS标准的gps消息类型为：sensor_msgs::NavSatFix （http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.htm）
Screenshot from 2020-04-20 21-17-54.png
图片中File这一行说明需要在头文件中#include <sensor_msgs/NavSatFix.h>；
（查看Compact Message Definition可以看到：消息包含的各数据成员以及它们的数据类型）
在接收信息的cpp中写一个gps_callback回调函数
void gps_callback(const sensor_msgs::NavSatFix& gps_data)
{
bool receive_gps = false;
if(fout_data_gps)
{
fout_data_gps << gps_data.header.stamp <<" " << gps_data.longitude <<" " << gps_data.latitude <<std::endl;
}
if(receive_gps)
{
return;
}
receive_gps = true;
}
并在main()中添加订阅：
ros::Subscriber gps_sub = nh.subscribe("/gps_data", 5, &gps_callback);
此时，同样需要在CMakeLists.txt里添加订阅消息的执行文件并链接库：
add_executable(plane_fitting src/demo.cpp )
这里的plane_fitting为可执行文件名，其中demo.cpp是订阅消息所在的cpp文件。
target_link_libraries(plane_fitting
${catkin_LIBRARIES}
)
add_executable(plane_fitting src/demo.cpp src/kalman_filter.cpp
src/lib/interpreter.cc src/lib/kvaserToDBC.cc )
2. 定义并发布自己的消息
- 在当前package下创建msg文件夹，并在该文件夹下创建一个新的文件（这里我想定义的消息为articulated_angle，为方便文件名也一样为articulated_angle.msg）
由于该消息是激光检测到的平面上的点的x值（激光雷达坐标正前方距离），查阅http://wiki.ros.org/msg网页中的Built-in types，C++ double型对应的ROS下Primitive Type 是float64，故在articulated_angle.msg文件中按照：成员类型 自定义成员名字的格式输入如下内容：
float64 distance
float64 distance2
- 在msg文件写好后，需要让package知道我们新定义了消息类型，在CMakeList.txt中添加三个内容：
(1)
find_package(catkin REQUIRED COMPONENTS
roscpp
sensor_msgs
#serial
message_generation
rospy
std_msgs
)
自定义消息类型，需要添加message_generation；
（2）接着添加上面我们在msg文件夹下创建的文件：
add_message_files(
FILES
articulated_angle.msg
)
（3）添加依赖std_msgs，因为上面定义的距离distance消息属于ROS基础消息类型
generate_messages(
DEPENDENCIES
std_msgs
)
至此，CMakeList.txt的修改告一段落。
- 打开package的package.xml文件：
找到message_generation相关的将注释去掉，改为如下：
<build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>message_runtime</exec_depend>
保存并关闭。
- 在cpp文件的main()中：
添加#include <plane_fitting/articulated_angle.h>其中plane_fitting是package名，articulated_angle即为msg文件名；
pub中advertise函数后边添加<plane_fitting::articulated_angle>，并订阅话题名/articulated_angle；
ros::Publisher pub = nh.advertise<plane_fitting::articulated_angle>("/articulated_angle", 1000);
定义类型为plane_fitting::articulated_angle的articulated消息：
plane_fitting::articulated_angle articulated;
并且将计算所得的两平面距离值分别赋值给msg文件中的distance和distance2，并将其用pub.publish发布出来，如下内容：
while (ros::ok())
{
plane_fitting::articulated_angle articulated;
// articulated.angle = attitude_angle;
articulated.distance = z_filtered[0];
articulated.distance2 = z_filtered2[0];
pub.publish(articulated);
ros::spinOnce();
loop_rate.sleep();
++count;
std::cout << " C O U N T " << count << std::endl;
}
return 0;
ros::spin();
先这样，待日后业务有一定提升，再来修改......
