任务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();

先这样，待日后业务有一定提升，再来修改......