Merge pull request #5 from ipa-svn/indigo-devel

New cleanedup verison of pointcloud to laserscan including roi filter and frame publisher

Author: Benjamin Maidel

CMakeLists.txt

@@ -6,23 +6,34 @@ find_package(catkin REQUIRED COMPONENTS
   nodelet
   roscpp
   sensor_msgs
+  tf
   tf2
   tf2_ros
   tf2_sensor_msgs
   tf2_geometry_msgs
+  pcl_ros
 )
 
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES pointcloud_to_laserscan ipa_pointcloud_to_laserscan
-  CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros tf2_sensor_msgs tf2_geometry_msgs
+  LIBRARIES pointcloud_to_laserscan ipa_pointcloud_to_laserscan roi_outlier_removal
+  CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros tf2_sensor_msgs tf2_geometry_msgs pcl_ros
 )
 
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
 )
 
+add_library(frame_publisher src/frame_publisher.cpp)
+target_link_libraries(frame_publisher ${catkin_LIBRARIES})
+
+add_executable(frame_publisher_node src/frame_publisher_node.cpp)
+target_link_libraries(frame_publisher_node frame_publisher ${catkin_LIBRARIES})
+
+add_library(roi_outlier_removal src/roi_outlier_removal_nodelet.cpp)
+target_link_libraries(roi_outlier_removal ${catkin_LIBRARIES})
+
 add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_nodelet.cpp)
 target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES})
 

README

@@ -0,0 +1,33 @@
+
+This package contains the original pointcloud to laserscan from Paul Bovbel and a new improved version from Fraunhofer IPA.
+The improvements are in form of strongly reduced computational time for the laserscan creation as described in detail below,
+and some added functionality also presented in detail below.
+
+The procedure for converting the pointcloud to a laserscan follows the following principle roughly:
+  1. Make sure the transformation between the original frame (frame of the pointcloud) and the goal frame (frame of the scan) is available
+  2. Find the points that are within the specified region
+  3. Translate the relevant point to scan coordinates
+  4. Update scan if range of new point is shorter than current range for the same angle
+
+
+
+The two versions of pointcloud to laserscan (pointcloud_to_laserscan_nodelet and ipa_pointcloud_to_laserscan_nodelet) differs in the following ways:
+ * The IPA version converts the borders for the point selection to the original coordinates and does so avoid further processing of points that are outside the selected borders. Thsi savers a lot of computational time compared to the original version where the complete pointcloud was transformed.
+
+ * The ipa version if not based on MessageFilters. This choise was made due to the problem that the message filter stops generating cloud callbacks completely after an arbitrary amout of time (mostely after at least 30 min oparation). We have not had time to investigate the problem further but the effect is removed by not using the Message filter interface. 
+
+ * The ipa version applies an additional outlier filter to the created scan. The filter is configured and called directly in the cloud callback and can be configured and activated/deactivated over parameter cofiguartions. This filter was implemented becaue of the noise clusters in the kinnect pointcloud. More about the filter below.
+
+Using the ipa version without the outlier filter activated should result in exactly the same scans as the original version (only faster). We have however not had time to test this extensively enough to assure correctness of this statement.
+
+
+
+The scan_outlier_removal_filter removes noise clusters with much smaller range than the surrounding points. The filter can be configured with the following three parameters cluster_break_distance, max_noise_cluster_size, max_noise_cluster_distance. 
+This filter was constructed to handle noise clusters detected in the kinnect pointcloud. Those noise clusters are in form of false clusters of 15-35 points sporadically occuring at distances of up to a couple of meters in fromt of the sensor. They seems to be caused by reflection phenomenas in the environment. Finding and removing the clusters in the pointcloud was taking to much computational time. Standard statistical filters not applicabe du to the relatively large cluster size. The constructed filter removes the effect in the constructed scan so that this can be used for freespace generation and navigation.
+
+
+The roi_outlier_removal_nodelet creates a new pointcloud only containing the points that are within the specified region of intereset. The region of interest can be specified in any frame as long as the transformation to the pointcloud frame is known by the tf tree. The reduced pointcloud is given in the same frame as the original pointcloud. The point selection algorithm is the same as used within the ipa pointcloud to laserscan nodelet. This filter should be moved out of this package in the future since this functionality is not directly related to the cloud to scan functionality. For not it is a useful tool if one want to use external pointcloud filters befor constructing the scan, but want to save resources by only filtering a reduced pointcloud. 
+
+A useful method for getting a decent scan out of a noisy pointcloud is: roi_outlier_removal -> statistical pcl filter -> ipa_pointcloud_to_laserscan (using outlier_remval_filer).
+
+The frame_publisher publishes a new frame spatialy aligned with the specified base_frame but with the specified rotations from the rotation_frame. This node should not be inside this package.

include/pointcloud_to_laserscan/frame_publisher.h

@@ -0,0 +1,71 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Fraunhofer IPA.
+ *  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 copyright holder 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,
+ *  INCIDENTAL, 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: Sofie Nilsson
+ */
+
+#ifndef FRAME_PUB_H
+#define FRAME_PUB_H
+
+#include <ros/ros.h>
+
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+
+class FramePublisher
+{
+public:
+  bool initialize();
+
+private:
+  void frameBroadcastCallback(const ros::TimerEvent& event);
+
+private:
+  ros::NodeHandle nh_, priv_nh_;
+  tf::TransformListener tf_listener_;
+  tf::TransformBroadcaster tf_broadcaster_;
+
+  ros::Timer frame_broadcast_timer_;
+  double update_rate_;
+  std::string base_frame_;
+  std::string rotation_frame_;
+  std::string target_frame_;
+  bool rot_z_, rot_x_, rot_y_;
+};
+
+#endif  // FRAME_PUB_H

include/pointcloud_to_laserscan/ipa_pointcloud_to_laserscan_nodelet.h

@@ -2,6 +2,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  Copyright (c) 2015, Fraunhofer IPA.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +15,7 @@
  *     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 Willow Garage, Inc. nor the names of its
+ *   * Neither the name of the copyright holders nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -33,7 +34,7 @@
  *
  *
  */
-
+ 
 /*
  * Author: Paul Bovbel
  * Author: Sofie Nilsson
@@ -49,7 +50,8 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/message_filter.h"
-#include "message_filters/subscriber.h"
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include "sensor_msgs/PointCloud2.h"
 #include <pointcloud_to_laserscan/scan_outlier_removal_filter.h>
 
@@ -72,21 +74,17 @@ namespace pointcloud_to_laserscan
     virtual void onInit();
 
     void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
-    void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
-        tf2_ros::filter_failure_reasons::FilterFailureReason reason);
-
-    void connectCb();
 
-    void disconnectCb();
+    void convert_pointcloud_to_laserscan(const sensor_msgs::PointCloud2ConstPtr &cloud, sensor_msgs::LaserScan &output, const tf2::Transform &T, const double range_min );
 
     ros::NodeHandle nh_, private_nh_;
     ros::Publisher pub_;
     boost::mutex connect_mutex_;
 
     boost::shared_ptr<tf2_ros::Buffer> tf2_;
     boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
-    message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
-    boost::shared_ptr<MessageFilter> message_filter_;
+
+    ros::Subscriber sub_;
 
     scan_outlier_filter::ScanOutlierRemovalFilter outlier_filter_;
     // ROS Parameters

include/pointcloud_to_laserscan/roi_outlier_removal.h

@@ -0,0 +1,103 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016, Fraunhofer IPA.
+ *  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 copyright holder 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,
+ *  INCIDENTAL, 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: Sofie Nilsson
+ */
+
+#ifndef ROI_OUTLIER_REMOVAL_NODELET
+#define ROI_OUTLIER_REMOVAL_NODELET
+
+#include "ros/ros.h"
+#include "boost/thread/mutex.hpp"
+
+#include "nodelet/nodelet.h"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/message_filter.h"
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include "sensor_msgs/PointCloud2.h"
+
+// includes for pcl filtering
+#include <pcl_ros/point_cloud.h>
+
+namespace pointcloud_to_laserscan
+{
+  typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
+/**
+* The ROI outlier removal removes all points outside the specified region of interest (ROI) and publishes the remaining points into a new pointcloud.
+* The roi is defined as a z value range, angle range in the xy plane, and a max and min range in the xyy plane, for a chosen frame.
+* The filter converts the roi borders to to the original pointcloud frame in order to not have to waist time on transforming points 
+* that are not to be used later on.
+* The points within the roi are transformed to and published in the same frame as the borders were given.
+* The filter is relevant to the pointcloud to laserscan transformation since it creates a possibility to apply additional statistical filtering
+* to a reduced pointcloud before creating the scan. The loclation of this filter within the pointcloud to laserscan package is however questionable.
+*/
+  class RoiOutlierRemovalNodelet : public nodelet::Nodelet
+  {
+
+  public:
+    RoiOutlierRemovalNodelet();
+    void configure_roi_settings();
+
+  private:
+    virtual void onInit();
+
+    void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
+
+    void reduce_point_cloud_to_roi(const pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud,  
+                                  pcl::PointCloud<pcl::PointXYZ>::Ptr reduced_pcl_cloud,
+                                  const tf2::Transform &T);
+
+    ros::NodeHandle nh_, private_nh_;
+    ros::Publisher pub_;
+
+    boost::shared_ptr<tf2_ros::Buffer> tf2_;
+    boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+
+    ros::Subscriber sub_;
+
+    // ROS Parameters
+    unsigned int input_queue_size_;
+    std::string roi_def_frame_;
+    double tolerance_;
+    double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, range_min_, range_max_;    
+  };
+
+}  // roi_outlier_removal
+
+#endif  // ROI_OUTLIER_REMOVAL_NODELET

include/pointcloud_to_laserscan/scan_outlier_removal_filter.h

@@ -2,7 +2,7 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2010-2012, Fraunhofer IPA
+ *  Copyright (c) 2016, Fraunhofer IPA.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -15,7 +15,7 @@
  *     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 Willow Garage, Inc. nor the names of its
+ *   * Neither the name of the copyright holder nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -39,31 +39,37 @@
  * Author: Sofie Nilsson
  */
 
+/* 
+ * The scan_outlier_removal_filter removes noise clusters with much smaller range than the surrounding points. 
+ * The filter can be configured with the following three parameters cluster_break_distance, max_noise_cluster_size, 
+ * max_noise_cluster_distance. 
+ */
+
 #ifndef IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER
 #define IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER
 
 #include <sensor_msgs/LaserScan.h>
 
-namespace scan_outlier_filter
-{
-	class ScanOutlierRemovalFilter
-	{
-	private:
-		bool filter_configured_;
-		double cluster_break_distance_; 
-		int max_noise_cluster_size_; 
-		double max_noise_cluster_distance_;
+ namespace scan_outlier_filter
+ {
+   class ScanOutlierRemovalFilter
+   {
+   private:
+    bool filter_configured_;
+    double cluster_break_distance_; 
+    int max_noise_cluster_size_; 
+    double max_noise_cluster_distance_;
 
-	public:
-		ScanOutlierRemovalFilter(): 
-			filter_configured_(false), 
-			cluster_break_distance_(0.0), 
-			max_noise_cluster_size_(0), 
-			max_noise_cluster_distance_(0.0)
-			{};
-		void configure(const double cluster_break_distance, const int max_noise_cluster_size, const double max_noise_cluster_distance);
+  public:
+    ScanOutlierRemovalFilter(): 
+    filter_configured_(false), 
+    cluster_break_distance_(0.0), 
+    max_noise_cluster_size_(0), 
+    max_noise_cluster_distance_(0.0)
+    {};
+    void configure(const double cluster_break_distance, const int max_noise_cluster_size, const double max_noise_cluster_distance);
 
-		void remove_outliers(sensor_msgs::LaserScan &scan);
-	};
+    void remove_outliers(sensor_msgs::LaserScan &scan);
+  };
 }
 #endif //IPA_POINTCLOUD_TO_LASERSCAN_SCAN_OUTLIER_REMOVAL_FILTER

nodelets.xml

@@ -21,3 +21,15 @@
   </class>
 
 </library>
+
+<library path="lib/libroi_outlier_removal">
+
+  <class name="pointcloud_to_laserscan/RoiOutlierRemovalNodelet"
+   type="pointcloud_to_laserscan::RoiOutlierRemovalNodelet"
+   base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelet to remove all points not witin the defined field of view. The remaining points are published to a new pointcloud.
+    </description>
+  </class>
+
+</library>