laser_filters源码整体分析

laser_filters 也用了挺久，感觉还是蛮好用的，最近看了下源码（ laser_filters包为kinetic-devel， filters包为hydro-devel），在这里写个流水账。

laser_filters

总的来说 laser_filters包 调用了 filters包，下面来详细阅读下代码。

<launch>
<node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter">
      <rosparam command="load" file="$(find laser_filters)/examples/speckle_filter_example.yaml" />
</node>
</launch>

scan_filter_chain:
- name: speckle_filter
  type: laser_filters/LaserScanSpeckleFilter
  params:
    # Select which filter type to use.
    # 0: Range based filtering (distance between consecutive points)
    # 1: Euclidean filtering based on radius outlier search
    filter_type: 0

    # Only ranges smaller than this range are taken into account
    max_range: 2.0

    # filter_type[0] (Distance): max distance between consecutive points
    # filter_type[1] (RadiusOutlier): max distance between points
    max_range_difference: 0.1

    # filter_type[0] (Distance): Number of consecutive ranges that will be tested for max_distance
    # filter_type[1] (RadiusOutlier): Minimum number of neighbors
    filter_window: 2

先看launch文件，启动了 scan_to_scan_filter_chain 并导入了 yaml文件。 scan_to_scan_filter_chain 可执行文件是由 scan_to_scan_filter_chain.cpp 生成的，scan_to_scan_filter_chain.cpp 中使用了 class scan_to_scan_filter_chain ， class scan_to_scan_filter_chain 中又使用了 filters::FilterChain<sensor_msgs::LaserScan> filter_chain_; 。在类初始化时使用了 filter_chain_.configure("scan_filter_chain", private_nh_); 配置 filter_chain_ ，在激光数据到来时会使用 filter_chain_.update(*msg_in, msg_) 滤波。
下面再继续看 filters包 中的 filter_chain 。

filters

filters包 使用了ROS的插件机制，这个插件机制实现还是挺神奇的，以后有空可以详细了解下，开始阅读代码filter_chain.h。

bool configure(std::string param_name, ros::NodeHandle node = ros::NodeHandle())
{
  XmlRpc::XmlRpcValue config;
  if(node.getParam(param_name + "/filter_chain", config))
  {
    std::string resolved_name = node.resolveName(param_name).c_str();
    ROS_WARN("Filter chains no longer check implicit nested 'filter_chain' parameter.  This node is configured to look directly at '%s'.  Please move your chain description from '%s/filter_chain' to '%s'", resolved_name.c_str(), resolved_name.c_str(), resolved_name.c_str());
  }
  else if(!node.getParam(param_name, config))
  {
    ROS_DEBUG("Could not load the filter chain configuration from parameter %s, are you sure it was pushed to the parameter server? Assuming that you meant to leave it empty.", param_name.c_str());
    configured_ = true;
    return true;
  }
  return this->configure(config, node.getNamespace());
}

filter_chain_.configure("scan_filter_chain", private_nh_); 传参为"scan_filter_chain"，再根据上文中的yaml文件，所以在 else if(!node.getParam(param_name, config)) 这里可以获取到参数服务器上的数据，最后又执行了这个 this->configure(config, node.getNamespace())。

bool configure(XmlRpc::XmlRpcValue& config, const std::string& filter_ns)
{
  ......//省略了检查数据的部分，前面检查数据结构，后面检查name不能重复，然后把type存起来

  bool result = true;
  for (int i = 0; i < config.size(); ++i)
  {
    boost::shared_ptr<filters::FilterBase<T> > p(loader_.createInstance(config[i]["type"]));
    if (p.get() == NULL)
      return false;
    result = result &&  p.get()->configure(config[i]);    
    reference_pointers_.push_back(p);
    std::string type = config[i]["type"];
    std::string name = config[i]["name"];
    ROS_DEBUG("%s: Configured %s:%s filter at %p\n", filter_ns.c_str(), type.c_str(),
              name.c_str(),  p.get());
  }
  
  if (result == true)
  {
    configured_ = true;
  }
  return result;
};

这里就是将所有的type（滤波插件）实例化后，再配置一遍，params也都用上了。
还用到一个 XmlRpc::XmlRpcValue ，这个是跟ros的底层通信有关，参考中列了一些链接。
最后 bool update(const T& data_in, T& data_out) 就是根据配置依次滤波。
还有个需要注意下，只用到了 类FilterBase（所有的滤波插件都继承了该类），类MultiChannelFilterBase 和 类MultiChannelFilterChain 都没用到，不要看错了。（不要问为什么会有这个提醒o(╥﹏╥)o）

参考

laser_filters wiki
filters wiki
ros_comm
一文搞懂XML、Json、Protobuf序列化协议
探索ROS中的XML

标签：laser,name,chain,scan,filter,源码,filters,type
来源： https://blog.csdn.net/shamozhizhoutx/article/details/122659502