scan_to_cloud_filter_chain.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008 Radu Bogdan Rusu <rusu@cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
00028  *
00029  */
00030 
00031 /*
00032 \author Radu Bogdan Rusu <rusu@cs.tum.edu>
00033 
00034 
00035  */
00036 
00037 #include <ros/ros.h>
00038 #include <sensor_msgs/PointCloud2.h>
00039 #include <sensor_msgs/LaserScan.h>
00040 
00041 #include <float.h>
00042 
00043 // Laser projection
00044 #include <laser_geometry/laser_geometry.h>
00045 
00046 // TF
00047 #include <tf/transform_listener.h>
00048 #include "tf/message_filter.h"
00049 #include "message_filters/subscriber.h"
00050 
00051 //Filters
00052 #include "filters/filter_chain.h"
00053 
00056 class ScanToCloudFilterChain
00057 {
00058 public:
00059 
00060   // ROS related
00061   laser_geometry::LaserProjection projector_; // Used to project laser scans
00062 
00063   double laser_max_range_;           // Used in laser scan projection
00064   int window_;
00065     
00066   bool high_fidelity_;                    // High fidelity (interpolating time across scan)
00067   std::string target_frame_;                   // Target frame for high fidelity result
00068   std::string scan_topic_, cloud_topic_;
00069 
00070   ros::NodeHandle nh;
00071   ros::NodeHandle private_nh;
00072     
00073   // TF
00074   tf::TransformListener tf_;
00075 
00076   message_filters::Subscriber<sensor_msgs::LaserScan> sub_;
00077   tf::MessageFilter<sensor_msgs::LaserScan> filter_;
00078 
00079   double tf_tolerance_;
00080   filters::FilterChain<sensor_msgs::PointCloud2> cloud_filter_chain_;
00081   filters::FilterChain<sensor_msgs::LaserScan> scan_filter_chain_;
00082   ros::Publisher cloud_pub_;
00083 
00084   // Timer for displaying deprecation warnings
00085   ros::Timer deprecation_timer_;
00086   bool  using_scan_topic_deprecated_;
00087   bool  using_cloud_topic_deprecated_;
00088   bool  using_default_target_frame_deprecated_;
00089   bool  using_laser_max_range_deprecated_;
00090   bool  using_filter_window_deprecated_;
00091   bool  using_scan_filters_deprecated_;
00092   bool  using_cloud_filters_deprecated_;
00093   bool  using_scan_filters_wrong_deprecated_;
00094   bool  using_cloud_filters_wrong_deprecated_;
00095   bool  incident_angle_correction_;
00096 
00098   ScanToCloudFilterChain () : laser_max_range_ (DBL_MAX), private_nh("~"), filter_(tf_, "", 50),
00099                    cloud_filter_chain_("sensor_msgs::PointCloud2"), scan_filter_chain_("sensor_msgs::LaserScan")
00100   {
00101     private_nh.param("high_fidelity", high_fidelity_, false);
00102     private_nh.param("notifier_tolerance", tf_tolerance_, 0.03);
00103     private_nh.param("target_frame", target_frame_, std::string ("base_link"));
00104 
00105     // DEPRECATED with default value
00106     using_default_target_frame_deprecated_ = !private_nh.hasParam("target_frame");
00107 
00108     // DEPRECATED
00109     using_scan_topic_deprecated_ = private_nh.hasParam("scan_topic");
00110     using_cloud_topic_deprecated_ = private_nh.hasParam("cloud_topic");
00111     using_laser_max_range_deprecated_ = private_nh.hasParam("laser_max_range");
00112     using_filter_window_deprecated_ = private_nh.hasParam("filter_window");
00113     using_cloud_filters_deprecated_ = private_nh.hasParam("cloud_filters/filter_chain");
00114     using_scan_filters_deprecated_ = private_nh.hasParam("scan_filters/filter_chain");
00115     using_cloud_filters_wrong_deprecated_ = private_nh.hasParam("cloud_filters/cloud_filter_chain");
00116     using_scan_filters_wrong_deprecated_ = private_nh.hasParam("scan_filters/scan_filter_chain");
00117 
00118 
00119     private_nh.param("filter_window", window_, 2);
00120     private_nh.param("laser_max_range", laser_max_range_, DBL_MAX);
00121     private_nh.param("scan_topic", scan_topic_, std::string("tilt_scan"));
00122     private_nh.param("cloud_topic", cloud_topic_, std::string("tilt_laser_cloud_filtered"));
00123     private_nh.param("incident_angle_correction", incident_angle_correction_, true);
00124 
00125     filter_.setTargetFrame(target_frame_);
00126     filter_.registerCallback(boost::bind(&ScanToCloudFilterChain::scanCallback, this, _1));
00127     filter_.setTolerance(ros::Duration(tf_tolerance_));
00128 
00129     if (using_scan_topic_deprecated_)
00130       sub_.subscribe(nh, scan_topic_, 50);
00131     else
00132       sub_.subscribe(nh, "scan", 50);
00133 
00134     filter_.connectInput(sub_);
00135 
00136     if (using_cloud_topic_deprecated_)
00137       cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2> (cloud_topic_, 10);
00138     else
00139       cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2> ("cloud_filtered", 10);
00140 
00141     std::string cloud_filter_xml;
00142 
00143     if (using_cloud_filters_deprecated_)
00144       cloud_filter_chain_.configure("cloud_filters/filter_chain", private_nh);
00145     else if (using_cloud_filters_wrong_deprecated_)
00146       cloud_filter_chain_.configure("cloud_filters/cloud_filter_chain", private_nh);
00147     else
00148       cloud_filter_chain_.configure("cloud_filter_chain", private_nh);
00149 
00150     if (using_scan_filters_deprecated_)
00151       scan_filter_chain_.configure("scan_filter/filter_chain", private_nh);
00152     else if (using_scan_filters_wrong_deprecated_)
00153       scan_filter_chain_.configure("scan_filters/scan_filter_chain", private_nh);
00154     else
00155       scan_filter_chain_.configure("scan_filter_chain", private_nh);
00156 
00157     deprecation_timer_ = nh.createTimer(ros::Duration(5.0), boost::bind(&ScanToCloudFilterChain::deprecation_warn, this, _1));
00158   }
00159 
00160   // We use a deprecation warning on a timer to avoid warnings getting lost in the noise
00161   void deprecation_warn(const ros::TimerEvent& e)
00162   {
00163     if (using_scan_topic_deprecated_)
00164       ROS_WARN("Use of '~scan_topic' parameter in scan_to_cloud_filter_chain has been deprecated.");
00165 
00166     if (using_cloud_topic_deprecated_)
00167       ROS_WARN("Use of '~cloud_topic' parameter in scan_to_cloud_filter_chain has been deprecated.");
00168 
00169     if (using_laser_max_range_deprecated_)
00170       ROS_WARN("Use of '~laser_max_range' parameter in scan_to_cloud_filter_chain has been deprecated.");
00171 
00172     if (using_filter_window_deprecated_)
00173       ROS_WARN("Use of '~filter_window' parameter in scan_to_cloud_filter_chain has been deprecated.");
00174 
00175     if (using_default_target_frame_deprecated_)
00176       ROS_WARN("Use of default '~target_frame' parameter in scan_to_cloud_filter_chain has been deprecated.  Default currently set to 'base_link' please set explicitly as appropriate.");
00177 
00178     if (using_cloud_filters_deprecated_)
00179       ROS_WARN("Use of '~cloud_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated.  Replace with '~cloud_filter_chain'");
00180 
00181     if (using_scan_filters_deprecated_)
00182       ROS_WARN("Use of '~scan_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated.  Replace with '~scan_filter_chain'");
00183 
00184     if (using_cloud_filters_wrong_deprecated_)
00185       ROS_WARN("Use of '~cloud_filters/cloud_filter_chain' parameter in scan_to_cloud_filter_chain is incorrect.  Please Replace with '~cloud_filter_chain'");
00186 
00187     if (using_scan_filters_wrong_deprecated_)
00188       ROS_WARN("Use of '~scan_filters/scan_filter_chain' parameter in scan_to_scan_filter_chain is incorrect.  Please Replace with '~scan_filter_chain'");
00189 
00190   }
00191 
00192 
00194   void
00195   scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00196   {
00197     //    sensor_msgs::LaserScan scan_msg = *scan_in;
00198 
00199     sensor_msgs::LaserScan filtered_scan;
00200     scan_filter_chain_.update (*scan_msg, filtered_scan);
00201 
00202     // Project laser into point cloud
00203     sensor_msgs::PointCloud2 scan_cloud;
00204 
00205     //\TODO CLEAN UP HACK 
00206     // This is a trial at correcting for incident angles.  It makes many assumptions that do not generalise
00207     if(incident_angle_correction_)
00208     {
00209       for (unsigned int i = 0; i < filtered_scan.ranges.size(); i++)
00210       {
00211         double angle = filtered_scan.angle_min + i * filtered_scan.angle_increment;
00212         filtered_scan.ranges[i] = filtered_scan.ranges[i] + 0.03 * exp(-fabs(sin(angle)));
00213       }
00214     }
00215 
00216     // Transform into a PointCloud message
00217     int mask = laser_geometry::channel_option::Intensity |
00218       laser_geometry::channel_option::Distance |
00219       laser_geometry::channel_option::Index |
00220       laser_geometry::channel_option::Timestamp;
00221       
00222     if (high_fidelity_)
00223     {
00224       try
00225       {
00226         projector_.transformLaserScanToPointCloud (target_frame_, filtered_scan, scan_cloud, tf_, mask);
00227       }
00228       catch (tf::TransformException &ex)
00229       {
00230         ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", target_frame_.c_str (), ex.what ());
00231         return;
00232         //projector_.projectLaser (filtered_scan, scan_cloud, laser_max_range_, preservative_, mask);
00233       }
00234     }
00235     else
00236     {
00237       projector_.transformLaserScanToPointCloud(target_frame_, filtered_scan, scan_cloud, tf_, laser_max_range_, mask);
00238     }
00239       
00240     sensor_msgs::PointCloud2 filtered_cloud;
00241     cloud_filter_chain_.update (scan_cloud, filtered_cloud);
00242 
00243     cloud_pub_.publish(filtered_cloud);
00244   }
00245 
00246 } ;
00247 
00248 
00249 
00250 int
00251 main (int argc, char** argv)
00252 {
00253   ros::init (argc, argv, "scan_to_cloud_filter_chain");
00254   ros::NodeHandle nh;
00255   ScanToCloudFilterChain f;
00256 
00257   ros::spin();
00258 
00259   return (0);
00260 }
00261 
00262 


laser_filters
Author(s): Tully Foote
autogenerated on Sat Sep 9 2017 02:57:38