Add lidar_z_threshold node and update relevant CMake files

Added a new subdirectory in rr_evgp called lidar_z_threshold and
within it a new node named lidar_z_threshold.cpp. The node checks
for a specific z value and tolerance value as parameters to threshold
the velodyne_points topic. This is the start of a resolution to issue #357.

rr_evgp/CMakeLists.txt

@@ -37,3 +37,4 @@ include_directories(
 
 add_subdirectory(src/costmap_plugins)
 add_subdirectory(src/UniformCostSearch)
+add_subdirectory(src/lidar_z_threshold)

rr_evgp/src/lidar_z_threshold/CMakeLists.txt

@@ -0,0 +1,3 @@
+add_executable(lidar_z_threshold lidar_z_threshold.cpp)
+target_link_libraries(lidar_z_threshold ${catkin_LIBRARIES})
+add_dependencies(lidar_z_threshold ${catkin_EXPORTED_TARGETS})

rr_evgp/src/lidar_z_threshold/lidar_z_threshold.cpp

@@ -0,0 +1,37 @@
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+ros::Publisher pub;
+float z;
+float ep;
+
+void lidar_callback(const sensor_msgs::PointCloud2ConstPtr& inputPointer) {
+
+    sensor_msgs::PointCloud2 thresholded;
+    thresholded = *inputPointer;
+
+    for (sensor_msgs::PointCloud2Iterator<float> i(thresholded, "x"); i != i.end(); ++i) {
+        if (std::abs(z - i[2]) > ep) {
+            i[2] = std::numeric_limits<float>::quiet_NaN();
+        }
+    }
+
+    pub.publish(thresholded);
+
+}
+
+int main(int argc, char** argv) {
+
+    ros::init(argc, argv, "lidar_z_threshold");
+    ros::NodeHandle nh;
+    ros::Subscriber sub = nh.subscribe("/velodyne_points", 1, lidar_callback);
+    pub = nh.advertise<sensor_msgs::PointCloud2>("z_points", 1);
+
+    while (ros::ok()) {
+        nh.param("lidar_threshold_z_value", z, (float) 0);
+        nh.param("lidar_threshold_ep_value", ep, (float) 10);
+        ros::spinOnce();
+    }
+
+}