clean the code

AbangLZU · Sep 24, 2021 · 109c313 · 109c313
1 parent 76e9d55
commit 109c313
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 41 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(ndt_localizer)
 
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 11)
 
 find_package(catkin REQUIRED COMPONENTS
         roscpp
@@ -50,13 +50,13 @@ include_directories(include ${catkin_INCLUDE_DIRS})
 include_directories(include ${catkin_INCLUDE_DIRS})
 SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}")
 
-add_executable(voxel_grid_filter nodes/voxel_grid_filter.cpp)
+add_executable(voxel_grid_filter nodes/points_downsampler.cpp)
 
 add_dependencies(voxel_grid_filter ${catkin_EXPORTED_TARGETS})
 
 target_link_libraries(voxel_grid_filter ${catkin_LIBRARIES})
 
-add_executable(map_loader nodes/mapLoader.cpp)
+add_executable(map_loader nodes/map_loader.cpp)
 target_link_libraries(map_loader ${catkin_LIBRARIES} ${PCL_LIBRARIES})
 
 add_executable(ndt_localizer_node nodes/ndt.cpp)

diff --git a/include/mapLoader.h → include/map_loader.h b/include/mapLoader.h → include/map_loader.h
diff --git a/include/points_downsampler.h b/include/points_downsampler.h
diff --git a/nodes/mapLoader.cpp → nodes/map_loader.cpp b/nodes/mapLoader.cpp → nodes/map_loader.cpp
@@ -1,4 +1,4 @@
-#include "mapLoader.h"
+#include "map_loader.h"
 
 MapLoader::MapLoader(ros::NodeHandle &nh){
     std::string pcd_file_path, map_topic;

diff --git a/nodes/voxel_grid_filter.cpp → nodes/points_downsampler.cpp b/nodes/voxel_grid_filter.cpp → nodes/points_downsampler.cpp
@@ -5,7 +5,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/filters/voxel_grid.h>
 
-#include "points_downsampler.h"
+// #include "points_downsampler.h"
 
 #define MAX_MEASUREMENT_RANGE 120.0
 
@@ -20,6 +20,32 @@ static std::string filename;
 
 static std::string POINTS_TOPIC;
 
+static pcl::PointCloud<pcl::PointXYZ> removePointsByRange(pcl::PointCloud<pcl::PointXYZ> scan, double min_range, double max_range)
+{
+  pcl::PointCloud<pcl::PointXYZ> narrowed_scan;
+  narrowed_scan.header = scan.header;
+
+  if( min_range>=max_range ) {
+    ROS_ERROR_ONCE("min_range>=max_range @(%lf, %lf)", min_range, max_range );
+    return scan;
+  }
+
+  double square_min_range = min_range * min_range;
+  double square_max_range = max_range * max_range;
+
+  for(pcl::PointCloud<pcl::PointXYZ>::const_iterator iter = scan.begin(); iter != scan.end(); ++iter)
+  {
+    const pcl::PointXYZ &p = *iter;
+    double square_distance = p.x * p.x + p.y * p.y;
+
+    if(square_min_range <= square_distance && square_distance <= square_max_range){
+      narrowed_scan.points.push_back(p);
+    }
+  }
+
+  return narrowed_scan;
+}
+
 static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
 {
   pcl::PointCloud<pcl::PointXYZ> scan;