add catkin/ros/points_filter_nan, for delete nan point in pointcloud.

src/ros/catkin/src/points_filter_nan/CMakeLists.txt

@@ -0,0 +1,86 @@
+cmake_minimum_required(VERSION 2.8.11)
+project(points_filter_nan)
+
+
+message(STATUS  "Enter points_filter_nan")
+
+
+
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  pluginlib
+  sensor_msgs
+  pcl_ros
+  pcl_conversions
+)
+find_package(Boost REQUIRED)
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTORCC ON)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX)
+    set(CMAKE_CXX_FLAGS "-fPIC ${CMAKE_CXX_FLAGS}")
+    message(STATUS "optional:-fPIC")   
+endif(CMAKE_COMPILER_IS_GNUCXX)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+#find_package(Qt5Core  REQUIRED)
+#qt5_wrap_cpp(MOC src/qt_ros_test.h)
+#qt5_wrap_ui(UIC src/qt_ros_test.ui)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+##   INCLUDE_DIRS include
+#  LIBRARIES client_plugin
+   CATKIN_DEPENDS 
+    roscpp std_msgs 
+    sensor_msgs pluginlib
+    pcl_ros pcl_conversions
+  DEPENDS
+    Boost
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+  ${Boost_INCLUDE_DIR}
+  ${autoware_msgs_INCLUDE_DIRS}
+)
+
+## Declare a C++ executable
+add_executable(${PROJECT_NAME}_node src/main.cpp src  )
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES})
+

src/ros/catkin/src/points_filter_nan/package.xml

@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package>
+  <name>points_filter_nan</name>
+  <version>0.0.0</version>
+  <description>The points_filter_nan package for filter nan points in pointcloud</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="yuchuli@catarc.ac.cn">yuchuli</maintainer>
+
+
+  <!-- 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>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/client_plugin</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+  <build_depend>pluginlib</build_depend>
+  <run_depend>pluginlib</run_depend>
+
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+  <build_depend>pcl_ros</build_depend>
+  <run_depend>pcl_ros</run_depend>
+
+  <build_depend>pcl_conversions</build_depend>
+  <run_depend>pcl_conversions</run_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/catkin/src/points_filter_nan/src/main.cpp

@@ -0,0 +1,83 @@
+#include "ros/ros.h"
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <thread>
+
+
+static std::string _point_sensor_topic = "/points_raw_sensor";
+static std::string _point_topic = "/points_raw";
+
+ros::Publisher  point_cloud_pub;
+ros::Subscriber points_sub;
+
+
+
+static void points_callback(const sensor_msgs::PointCloud2::ConstPtr &input)
+{
+    pcl::PointCloud<pcl::PointXYZI> point_cloud;
+
+    ros::Time current_scan_time = input->header.stamp;
+    static ros::Time previous_scan_time = current_scan_time;
+
+    pcl::fromROSMsg(*input, point_cloud);
+
+    int pointsize = point_cloud.width;
+    int i;
+    int nskip = 0;
+
+        pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_filter(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    point_cloud_filter->header = point_cloud.header;
+    for(i=0;i<pointsize;i++)
+    {
+        double x,y,z;
+        x = point_cloud.at(i).x;
+        y = point_cloud.at(i).y;
+        z = point_cloud.at(i).z;
+        if(std::isnan(x) ||(std::isnan(y))||std::isnan(z))
+        {
+            nskip++;
+            continue;
+        }
+        if((fabs(x)>10000)||(fabs(y)>10000)||(fabs(z)>10000))
+        {
+            nskip++;
+            continue;
+        }
+        point_cloud_filter->points.push_back(point_cloud.at(i));
+        ++point_cloud_filter->width;
+    }
+
+    std::cout<<" skip nan point count: "<<nskip<<" out point count: "<<point_cloud_filter->width<<std::endl;
+     point_cloud_pub.publish(point_cloud);
+}
+
+
+int main(int argc, char *argv[])
+{
+	ros::init(argc, argv, "points_filter_nan");
+//	ros::NodeHandle n;
+    ros::NodeHandle private_nh("~");
+	ros::Rate loop_rate(10);
+
+	    
+    private_nh.getParam("points_topic",_point_topic);
+    private_nh.getParam("sensor_topic",_point_sensor_topic);
+    std::cout<<"_point_topic is "<<_point_topic<<std::endl;
+	std::cout<<"_sensor_topic : "<<_point_sensor_topic<<std::endl;
+
+    point_cloud_pub = private_nh.advertise<sensor_msgs::PointCloud2>(
+                _point_topic, 10);
+
+    points_sub = private_nh.subscribe(_point_sensor_topic, 1, points_callback);
+
+	while (ros::ok())
+	{
+        ros::spinOnce();
+	 	loop_rate.sleep();
+	}
+    
+}