add lidar_filter_node

CMakeLists.txt

@@ -228,3 +228,11 @@ add_dependencies(costmap_cleaner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX
 target_link_libraries(costmap_cleaner
   ${catkin_LIBRARIES}
 )
+
+add_executable(lidar_filter_node
+  src/lidar_filter_node.cpp
+)
+add_dependencies(lidar_filter_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(lidar_filter_node
+  ${catkin_LIBRARIES}
+)

README.md

@@ -16,13 +16,18 @@ Youtube: [《代价地图清除》](https://www.youtube.com/watch?v=giHf_PY4EmY)
 cd ~/catkin_ws/src/
 git clone https://github.com/6-robot/jie_ware.git
 ```
+国内镜像：
+```
+cd ~/catkin_ws/src/
+git clone https://gitee.com/s-robot/jie_ware.git
+```
 2. 编译
 ```
 cd ~/catkin_ws
 catkin_make
 ```
 ## 激光定位
-1. 修改导航Launch文件，用如下内容替换AMCL节点
+1. 修改导航Launch文件，用如下内容替换AMCL节点：
 ```
 <node pkg="jie_ware" type="lidar_loc" name="lidar_loc" >
     <param name="base_frame" value="base_footprint" />
@@ -36,8 +41,22 @@ catkin_make
 roslaunch jie_ware lidar_loc_test.launch 
 ```
 ## 代价地图清除
-1. 修改导航Launch文件，添加如下内容
+1. 修改导航Launch文件，添加如下内容：
 ```
 <node pkg="jie_ware" type="costmap_cleaner" name="costmap_cleaner" />
 ```
 2. 运行修改后的Launch文件，正常设置机器人的估计位置即可。
+## 激光雷达滤波
+1. 修改导航Launch文件，添加如下内容：
+```
+<node pkg="jie_ware" type="lidar_filter_node" name="lidar_filter_node" />
+```
+2. 修改代价地图参数文件 costmap_common_params.yaml ，将激光雷达数据话题 topic 从 scan 修改为 scan_filtered 。
+```
+  observation_sources: scan
+  
+  scan:
+    data_type: LaserScan
+    topic: scan_filtered
+```
+3. 运行修改后的Launch文件，正常进行导航。

package.xml

@@ -13,7 +13,7 @@
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>BSD</license>
+  <license>GPL</license>
 
 
   <!-- Url tags are optional, but mutiple are allowed, one per tag -->

src/lidar_filter_node.cpp

@@ -0,0 +1,154 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2020-2025, Waterplus http://www.6-robot.com
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the WaterPlus nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+/* @author Zhang Wanjie                                             */
+
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <limits> 
+#include <cmath> 
+
+class CLidarFilter
+{
+public:
+    CLidarFilter(ros::NodeHandle& nh, ros::NodeHandle& pnh); 
+private:
+     ros::NodeHandle n_; 
+     ros::Publisher scan_pub_;
+     ros::Subscriber scan_sub_;
+     std::string source_topic_name_;
+     std::string pub_topic_name_;
+     double outlier_threshold_;
+
+     void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
+};
+
+CLidarFilter::CLidarFilter(ros::NodeHandle& nh, ros::NodeHandle& pnh) : n_(nh)
+{
+    // 从launch文件获取参数
+    pnh.param<std::string>("source_topic", source_topic_name_, "/scan");        // 原始数据话题
+    pnh.param<std::string>("pub_topic", pub_topic_name_, "/scan_filtered");     // 滤波后的数据话题
+    pnh.param<double>("outlier_threshold", outlier_threshold_, 0.1);            // 用于离群点判断的阈值
+
+    scan_pub_ = n_.advertise<sensor_msgs::LaserScan>(pub_topic_name_, 10);
+    scan_sub_ = n_.subscribe<sensor_msgs::LaserScan>(source_topic_name_, 10, &CLidarFilter::lidarCallback, this);
+}
+
+void CLidarFilter::lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
+{
+    int nRanges = scan->ranges.size();
+
+    // 如果点数太少，无法进行近邻比较，直接发布原始数据
+    if (nRanges < 3) 
+    {
+        scan_pub_.publish(scan);
+        return;
+    }
+
+    sensor_msgs::LaserScan new_scan;
+
+    // 拷贝源数据
+    new_scan.header = scan->header; 
+    new_scan.angle_min = scan->angle_min;
+    new_scan.angle_max = scan->angle_max;
+    new_scan.angle_increment = scan->angle_increment;
+    new_scan.time_increment = scan->time_increment;
+    new_scan.scan_time = scan->scan_time; 
+    new_scan.range_min = scan->range_min; 
+    new_scan.range_max = scan->range_max;
+    new_scan.ranges = scan->ranges; 
+    if (!scan->intensities.empty()) 
+    {
+        new_scan.intensities = scan->intensities; 
+    }
+	
+    // 对new_scan中的离群点进行剔除
+    // 遍历备份数据 new_scan.ranges，从第二个点到倒数第二个点
+    // 因为第一个点没有前一个点，最后一个点没有后一个点
+    for (int i = 1; i < nRanges - 1; ++i)
+    {
+        float prev_range = new_scan.ranges[i-1];
+        float current_range = new_scan.ranges[i];
+        float next_range = new_scan.ranges[i+1];
+
+        // 检查当前点是否有效 (在min_range和max_range之间，且不是inf/nan)
+        // 使用 new_scan 的 range_min 和 range_max 进行有效性判断
+        bool current_valid = std::isfinite(current_range) && 
+                             current_range >= new_scan.range_min && 
+                             current_range <= new_scan.range_max;
+
+        if (!current_valid) 
+        {
+            continue; // 当前点本身无效，跳过
+        }
+
+        // 检查前后邻居点是否有效
+        bool prev_valid = std::isfinite(prev_range) &&
+                          prev_range >= new_scan.range_min &&
+                          prev_range <= new_scan.range_max;
+        
+        bool next_valid = std::isfinite(next_range) &&
+                          next_range >= new_scan.range_min &&
+                          next_range <= new_scan.range_max;
+
+        // 只有当当前点和其前后两个邻居点都有效时，才进行离群判断
+        if (prev_valid && next_valid) {
+            if (std::abs(current_range - prev_range) > outlier_threshold_ &&
+                std::abs(current_range - next_range) > outlier_threshold_)
+            {
+                // 判定为离群点，将其无效化
+                new_scan.ranges[i] = std::numeric_limits<float>::infinity();
+                // 如果有强度信息，将其对应强度设为0
+                if (!new_scan.intensities.empty() && i < new_scan.intensities.size()) 
+                {
+                    new_scan.intensities[i] = 0.0f; 
+                }
+            }
+        }
+    }
+
+    scan_pub_.publish(new_scan);
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc,argv,"lidar_filter_node");
+
+    ros::NodeHandle nh; 
+    ros::NodeHandle pnh("~"); 
+
+    CLidarFilter lidar_filter(nh, pnh);
+    
+    ros::spin();
+    return 0; 
+}