最完整的毫米波雷达ROS开源包解析及二次开发（三）

  主要目的：进行原作者ROS开发包下的滤波功能开发，注意，小编只增加了滤波功能，其它功能可以按照我的流程进行开发，原理都是一样的。本文采用自顶向下的源码添加方式进行二次开发，建议二次开发前，先读一下前面的文章：

1. 源码部署
2. 源码解读
3. 源码github地址（已经完成测试，觉得可以的coders们可以star）
   

  下面正式开始介绍近期的二次开发工作，通过阅读本博客，你可以掌握：源码的深层运行机制解读以及进行二次开发的流程及二次开发需要注意的事项，废话不多说，正文开始。

  我们的目的，就是在原作者代码的基础上，进行滤波功能的添加，主要是通过新建一个ros提供的服务set_filter_lifetime_service_，来通过ROS调控滤波功能的运行参数：例如点云生存的最大时间（max_lifetime），并且为实现此目的，修改添加一系列底层代码接口。

毫米波雷达点云可视化

滤波源码数据结构

  我们要设置雷达的某项运行参数，那就首先要了解：雷达是如何与底层通信的，雷达如何完成上层接口开发的等基本问题，只有了解了这些，我们才能说我们了解了这个源码，而不是简单部署着玩。从了解到进行二次开发，我们需要对整体架构有了解：包括数据流向，类的继承关系等基本问题。编者主要想实现雷达的滤波功能，所以我需要在知道了源码的架构数据流向基础上，对雷达的参数设置部分做详细的了解。

 大陆雷达进行参数设置的一个基本数据流向是：

1.首先，ROS会发布一个service(服务)面向我们用户，我们通过set这个service可以设置雷达的运行参数：比如雷达的最大检测距离，最大功率，编者这里要设置滤波功能。
2.通过service的callback函数，更新雷达基类radar_cfg的滤波变量max_lifetime（这个变量在类中声明，是一个全局变量）。
3. 将雷达更新后的参数通过socket_can通信包发布到雷达中去，由此完成雷达参数的设置。

 首先需要了解的是：大陆雷达所提供滤波功能的通信格式，这是我们所有工作的基础，因为上层ROS接口的开发必然涉及到下层的通信，本源码对所有通信格式都进行了格式化，方便后面其他开发者进行二次开发。可以看到，与雷达通信的数据中，滤波是需要设计很多参数的，大概是这么一个流程：
4. 一是设定滤波项目（下图的FilterCfg_Index进行设置）：对时间、距离、RCS等参数单独进行设定，编者需要对点云的生存时间进行设定，因此进行选择lifetime项目，即FilterCfg_Index = 0x6。
5. 设定参数更新的有效性，类型等基本参数（filtercfg_type filtercfg_valid）。
6. 设定滤波项目的项目参数：例如lifetime项目的最大最小生存时间(filtercfg_min_distance)。

 根据格式，编者的滤波结构如下：

//filter index决定了采用那种滤波模式，如filter_index = 0x1则16位开始的滤波是进行距离滤波
//filter union function
namespace radar_filter_cfg{
  typedef union radar_filter_cfg{
    struct{
      uint64_t Reserved1 : 1;
      uint64_t FilterCfg_Valid : 1;
      uint64_t FilterCfg_Active : 1; 
      uint64_t FilterCfg_Index : 4; 
      uint64_t FilterCfg_Type : 1;
      uint64_t FilterCfg_Min_Lifetime1 : 4;
      uint64_t Reserved2 : 4;
      uint64_t FilterCfg_Min_Lifetime2 : 8;
      uint64_t Filter_Max_Lifetime1 : 4;
      uint64_t Reserved3 : 4;
      uint64_t FilterCfg_Max_Lifetime2 : 8;
    } data = {};
    uint8_t raw_data[5];
  }radar_filter_cfg;
 //雷达lifetime项目的消息结构体
  class RadarFilterCfg{
    public:
    RadarFilterCfg();

    ~RadarFilterCfg();
    //todo
    bool set_filter_min_lifetime(uint64_t max_life_time, bool valid = true);
    
    radar_filter_cfg *get_radar_filter_cfg();

    private:
    radar_filter_cfg radar_filter_cfg_msg; // this variable will be used for message definition

  };
//雷达lifetime滤波项目的基本类与方法
}

 通过以上，我们根据官方文档，定义了滤波项目中lifetime滤波的基本类与消息通信格式。

滤波的ROS类方法接口设计

//以下为radar_cfg_ros.hpp文件中的修改
class RadarCfgROS {
 public:
  RadarCfgROS(ros::NodeHandle &nh, ARS_40X_CAN *ars_40X_can);

  ~RadarCfgROS();

  bool set_max_distance(
      MaxDistance::Request &req,
      MaxDistance::Response & /*res*/);
   //此处省略一段
  bool set_filter_lifetime(//作为服务的callback函数处理用户的request
      RadarFilter::Request &req,
      RadarFilter::Response & /*res*/);

 private:
  ARS_40X_CAN *ars_40X_can_;
//建立饼初始化雷达两个配置类
  radar_cfg::RadarCfg *radar_cfg_;
  radar_filter_cfg::RadarFilterCfg *radar_filter_cfg_;

  ros::ServiceServer set_max_distance_service_;
  //此处省略一段
  ros::ServiceServer set_rcs_threshold_service_;
  //这是我们添加的ros设置滤波时长的服务
  ros::ServiceServer set_filter_lifetime_service_;
};
}

 以上为radar_cfg_ros中我们需要添加的服务名称：set_filter_lifetime_service_与对应的服务callback函数：set_filter_lifetime（响应函数）。

 下面就是对应cpp文件中的声明：

//以下为radar_cfg_ros.cpp文件中的声明
  set_filter_lifetime_service_ = 
      nh.advertiseService("set_filter_min_lifetime", &RadarCfgROS::set_filter_lifetime, this);
    }

//这里是在radar_cfg_ros.cpp中的声明
bool RadarCfgROS::set_filter_lifetime(
    RadarFilter::Request &req,
    RadarFilter::Response & /*res*/) {
  if (!radar_filter_cfg_->set_filter_min_lifetime(static_cast<uint64_t>(req.FilterLifetime))) {//修改filter的raw_data值
    return false;
  }
  ars_40X_can_->send_radar_data(can_messages::FilterCfg);//传入带有帧头的数据，即在上一句raw_data值已更新得值
	Distance::Request &req,
    MaxDistance::Response & /*res*/) {
  if (!radar_cfg_->set_max_distance(static_cast<uint64_t>(req.max_distance))) {//src文件中,radar_cfg_是之前radar_config.cpp定义的类指针，以此传入其中
    return false;
  }
  ars_40X_can_->send_radar_data(can_messages::RadarCfg);
  return true;
}

通过上面声明，我们已经完成了雷达ROS的服务接口设计，下面需要对底层接口进行增删改查。

底层基本类接口设计

 底层的接口主要包括：

1. 基类中的综合类ARS_40X_CAN：也就是将各个基类进行综合的基类，ros也是从这里进行继承，其中不仅包含了读取以及写数据的方法，还包括了各类通信的数据格式等重要部分，可以说读懂了这个类，底层你就基本上掌握了框架。
2. 雷达的设置基类radar_filter_cfg：这个类主要是设定底层的通信数据格式，这里如果是设置滤波，那就是滤波message的基本格式以及对应的基本方法。

ars_40x_can.cpp

  radar_filter_cfg::RadarFilterCfg *get_radar_filter_cfg();//这里是在hpp文件中的定义
  
  radar_filter_cfg::RadarFilterCfg *ARS_40X_CAN::get_radar_filter_cfg() {
  	return &radar_filter_cfg_;//这里是在cpp文件中的声明，这个函数的意义在于返回类的指针，在ros类方便进行初始化，调用基类中的方法
}

radar_filter_cfg::RadarFilterCfg radar_filter_cfg_;//这个在hpp文件中，先定义基类，方便上面的函数进行返回类指针用

bool ARS_40X_CAN::send_radar_data(uint32_t frame_id) {
  switch (frame_id) {
    case RadarCfg://要修改雷达的滤波器信息，就要从这里入手
      can_.write(frame_id, 8, radar_cfg_.get_radar_cfg()->raw_data);//这里的数字8的意思是：数据长度
      break;
    case SpeedInformation:
      can_.write(frame_id,
                 2,
                 speed_information_.get_speed_information()->raw_data);
      break;
    case YawRateInformation:
      can_.write(frame_id,
                 2,
                 yaw_rate_information_.get_yaw_rate_information()->raw_data);
      break;
     //can_是socket_can包提供的一个类，用来与雷达进行通信，这里就是要添加的滤波器，frame_id就是滤波器的帧头,0x202告诉雷达我要调整的是滤波中的设置
    case FilterCfg:
    can_.write(
      frame_id,
      5,
      radar_filter_cfg_.get_radar_filter_cfg()->raw_data
    );
    break;

radar_cfg.cpp & radar_cfg.hpp

radar_cfg.hpp

/filter index决定了采用那种滤波模式，如filter_index = 0x1则16位开始的滤波是进行距离滤波
//filter union function
namespace radar_filter_cfg{
  typedef union radar_filter_cfg{
    struct{
      uint64_t Reserved1 : 1;
      uint64_t FilterCfg_Valid : 1;
      uint64_t FilterCfg_Active : 1; 
      uint64_t FilterCfg_Index : 4; 
      uint64_t FilterCfg_Type : 1;
      uint64_t FilterCfg_Min_Lifetime1 : 4;
      uint64_t Reserved2 : 4;
      uint64_t FilterCfg_Min_Lifetime2 : 8;
      uint64_t Filter_Max_Lifetime1 : 4;
      uint64_t Reserved3 : 4;
      uint64_t FilterCfg_Max_Lifetime2 : 8;
    } data = {};
    uint8_t raw_data[5];
  }radar_filter_cfg;

  class RadarFilterCfg{
    public:
    RadarFilterCfg();

    ~RadarFilterCfg();
    //todo
    //bool set_filter_max_lifetime(uint64_t min_life_time, bool valid = true);

    bool set_filter_min_lifetime(uint64_t max_life_time, bool valid = true);

    //bool set_filter_filtercfg_type(uint64_t filter_type, bool valid = true);
    
    radar_filter_cfg *get_radar_filter_cfg();

    private:
    radar_filter_cfg radar_filter_cfg_msg; // this variable will be used for message definition

  };

}

radar_cfg.cpp

  RadarFilterCfg::RadarFilterCfg(){
  //new added
  radar_filter_cfg_msg.data.FilterCfg_Min_Lifetime1 = 2;
  radar_filter_cfg_msg.data.FilterCfg_Min_Lifetime2 = 0;
  radar_filter_cfg_msg.data.FilterCfg_Type = 0;
  radar_filter_cfg_msg.data.FilterCfg_Index = 0x6;//life time模式
  }
  RadarFilterCfg::~RadarFilterCfg(){

  }
  bool RadarFilterCfg::set_filter_min_lifetime(uint64_t min_life_time, bool valid){
    //TODO
    if(min_life_time < 1 || min_life_time > 10){
      return false;
    }
    radar_filter_cfg_msg.data.FilterCfg_Min_Lifetime1 = min_life_time >> 8;//转为64位，方便进行数据发送，因为can帧的数据为64位
    radar_filter_cfg_msg.data.FilterCfg_Min_Lifetime2 = min_life_time & 0xff;//转为64位，方便进行数据发送，因为can帧的数据为64位
    radar_filter_cfg_msg.data.FilterCfg_Valid = static_cast<uint64_t>(valid);
    return true;
  }
  radar_filter_cfg *RadarFilterCfg::get_radar_filter_cfg() {
  return &radar_filter_cfg_msg;
  }

 以上，完成了基本的ROS接口及底层接口的程序源码基本修改，但是仍然需要调整一些文件，才能达到最后的效果。

开发时的注意事项

srv文件的添加

srv文件需要添加，因为服务增加了一项RadarFilter.srv。

Cmakelis.txt文件的修改

  add_service_files(
      FILES
      MaxDistance.srv
      OutputType.srv
      RadarPower.srv
      RCSThreshold.srv
      SensorID.srv
      SortIndex.srv
      #经过上面的修改，这里是我们需要添加的
      RadarFilter.srv
  )

radar_cfg_ros.hpp中引用库的修改

#include "ars_40X/ars_40X_can.hpp"
#include "ars_40X/MaxDistance.h"
#include "ars_40X/OutputType.h"
#include "ars_40X/RadarPower.h"
#include "ars_40X/RCSThreshold.h"
#include "ars_40X/SensorID.h"
#include "ars_40X/SortIndex.h"
#include "ars_40X/RadarFilter.h"//这里是我们需要添加的，因为srv生成一个.h文件，我们需要引用这个文件来初始化:例如RadarFilter::Request &req

总结与下一步工作 ：TODO

 经过滤波，效果在object模式下有了很大的改善，表现在闪电（不稳定的噪声点）减少。
 在点云模式下，仍然存在闪点问题，无法对目标的形状进行建模。因此标定有一定的困难。
 在cluster模式，也就是点云模式下，雷达没有提供对应的滤波功能，因此我们如果要进行多传感器的融合的标定问题，不能依靠硬件进行滤波了，所以编者下一步要进行雷达的点云软件层面的滤波，初步考虑使用卡尔曼滤波方法进行滤波。