$search
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/PointCloud.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 00054 00057 class ScanToCloudFilterChain 00058 { 00059 public: 00060 00061 // ROS related 00062 laser_geometry::LaserProjection projector_; // Used to project laser scans 00063 00064 double laser_max_range_; // Used in laser scan projection 00065 int window_; 00066 00067 bool high_fidelity_; // High fidelity (interpolating time across scan) 00068 std::string target_frame_; // Target frame for high fidelity result 00069 std::string scan_topic_, cloud_topic_; 00070 00071 ros::NodeHandle nh; 00072 ros::NodeHandle private_nh; 00073 00074 // TF 00075 tf::TransformListener tf_; 00076 00077 message_filters::Subscriber<sensor_msgs::LaserScan> sub_; 00078 tf::MessageFilter<sensor_msgs::LaserScan> filter_; 00079 00080 double tf_tolerance_; 00081 filters::FilterChain<sensor_msgs::PointCloud> cloud_filter_chain_; 00082 filters::FilterChain<sensor_msgs::LaserScan> scan_filter_chain_; 00083 ros::Publisher cloud_pub_; 00084 00085 // Timer for displaying deprecation warnings 00086 ros::Timer deprecation_timer_; 00087 bool using_scan_topic_deprecated_; 00088 bool using_cloud_topic_deprecated_; 00089 bool using_default_target_frame_deprecated_; 00090 bool using_laser_max_range_deprecated_; 00091 bool using_filter_window_deprecated_; 00092 bool using_scan_filters_deprecated_; 00093 bool using_cloud_filters_deprecated_; 00094 bool using_scan_filters_wrong_deprecated_; 00095 bool using_cloud_filters_wrong_deprecated_; 00096 bool use_hack_; 00097 00099 ScanToCloudFilterChain () : laser_max_range_ (DBL_MAX), private_nh("~"), filter_(tf_, "", 50), 00100 cloud_filter_chain_("sensor_msgs::PointCloud"), scan_filter_chain_("sensor_msgs::LaserScan") 00101 { 00102 private_nh.param("high_fidelity", high_fidelity_, false); 00103 private_nh.param("notifier_tolerance", tf_tolerance_, 0.03); 00104 private_nh.param("target_frame", target_frame_, std::string ("base_link")); 00105 00106 // DEPRECATED with default value 00107 using_default_target_frame_deprecated_ = !private_nh.hasParam("target_frame"); 00108 00109 // DEPRECATED 00110 using_scan_topic_deprecated_ = private_nh.hasParam("scan_topic"); 00111 using_cloud_topic_deprecated_ = private_nh.hasParam("cloud_topic"); 00112 using_laser_max_range_deprecated_ = private_nh.hasParam("laser_max_range"); 00113 using_filter_window_deprecated_ = private_nh.hasParam("filter_window"); 00114 using_cloud_filters_deprecated_ = private_nh.hasParam("cloud_filters/filter_chain"); 00115 using_scan_filters_deprecated_ = private_nh.hasParam("scan_filters/filter_chain"); 00116 using_cloud_filters_wrong_deprecated_ = private_nh.hasParam("cloud_filters/cloud_filter_chain"); 00117 using_scan_filters_wrong_deprecated_ = private_nh.hasParam("scan_filters/scan_filter_chain"); 00118 00119 00120 private_nh.param("filter_window", window_, 2); 00121 private_nh.param("laser_max_range", laser_max_range_, DBL_MAX); 00122 private_nh.param("scan_topic", scan_topic_, std::string("tilt_scan")); 00123 private_nh.param("cloud_topic", cloud_topic_, std::string("tilt_laser_cloud_filtered")); 00124 private_nh.param("use_hack", use_hack_, true); 00125 00126 filter_.setTargetFrame(target_frame_); 00127 filter_.registerCallback(boost::bind(&ScanToCloudFilterChain::scanCallback, this, _1)); 00128 filter_.setTolerance(ros::Duration(tf_tolerance_)); 00129 00130 if (using_scan_topic_deprecated_) 00131 sub_.subscribe(nh, scan_topic_, 50); 00132 else 00133 sub_.subscribe(nh, "scan", 50); 00134 00135 filter_.connectInput(sub_); 00136 00137 if (using_cloud_topic_deprecated_) 00138 cloud_pub_ = nh.advertise<sensor_msgs::PointCloud> (cloud_topic_, 10); 00139 else 00140 cloud_pub_ = nh.advertise<sensor_msgs::PointCloud> ("cloud_filtered", 10); 00141 00142 std::string cloud_filter_xml; 00143 00144 if (using_cloud_filters_deprecated_) 00145 cloud_filter_chain_.configure("cloud_filters/filter_chain", private_nh); 00146 else if (using_cloud_filters_wrong_deprecated_) 00147 cloud_filter_chain_.configure("cloud_filters/cloud_filter_chain", private_nh); 00148 else 00149 cloud_filter_chain_.configure("cloud_filter_chain", private_nh); 00150 00151 if (using_scan_filters_deprecated_) 00152 scan_filter_chain_.configure("scan_filter/filter_chain", private_nh); 00153 else if (using_scan_filters_wrong_deprecated_) 00154 scan_filter_chain_.configure("scan_filters/scan_filter_chain", private_nh); 00155 else 00156 scan_filter_chain_.configure("scan_filter_chain", private_nh); 00157 00158 deprecation_timer_ = nh.createTimer(ros::Duration(5.0), boost::bind(&ScanToCloudFilterChain::deprecation_warn, this, _1)); 00159 } 00160 00161 // We use a deprecation warning on a timer to avoid warnings getting lost in the noise 00162 void deprecation_warn(const ros::TimerEvent& e) 00163 { 00164 if (using_scan_topic_deprecated_) 00165 ROS_WARN("Use of '~scan_topic' parameter in scan_to_cloud_filter_chain has been deprecated."); 00166 00167 if (using_cloud_topic_deprecated_) 00168 ROS_WARN("Use of '~cloud_topic' parameter in scan_to_cloud_filter_chain has been deprecated."); 00169 00170 if (using_laser_max_range_deprecated_) 00171 ROS_WARN("Use of '~laser_max_range' parameter in scan_to_cloud_filter_chain has been deprecated."); 00172 00173 if (using_filter_window_deprecated_) 00174 ROS_WARN("Use of '~filter_window' parameter in scan_to_cloud_filter_chain has been deprecated."); 00175 00176 if (using_default_target_frame_deprecated_) 00177 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."); 00178 00179 if (using_cloud_filters_deprecated_) 00180 ROS_WARN("Use of '~cloud_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated. Replace with '~cloud_filter_chain'"); 00181 00182 if (using_scan_filters_deprecated_) 00183 ROS_WARN("Use of '~scan_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated. Replace with '~scan_filter_chain'"); 00184 00185 if (using_cloud_filters_wrong_deprecated_) 00186 ROS_WARN("Use of '~cloud_filters/cloud_filter_chain' parameter in scan_to_cloud_filter_chain is incorrect. Please Replace with '~cloud_filter_chain'"); 00187 00188 if (using_scan_filters_wrong_deprecated_) 00189 ROS_WARN("Use of '~scan_filters/scan_filter_chain' parameter in scan_to_scan_filter_chain is incorrect. Please Replace with '~scan_filter_chain'"); 00190 00191 } 00192 00193 00195 void 00196 scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg) 00197 { 00198 // sensor_msgs::LaserScan scan_msg = *scan_in; 00199 00200 sensor_msgs::LaserScan filtered_scan; 00201 scan_filter_chain_.update (*scan_msg, filtered_scan); 00202 00203 // Project laser into point cloud 00204 sensor_msgs::PointCloud tmp_cloud; 00205 sensor_msgs::PointCloud scan_cloud; 00206 00207 //\TODO CLEAN UP HACK 00208 // This is a trial at correcting for incident angles. It makes many assumptions that do not generalise 00209 if(use_hack_) 00210 { 00211 for (unsigned int i = 0; i < filtered_scan.ranges.size(); i++) 00212 { 00213 double angle = filtered_scan.angle_min + i * filtered_scan.angle_increment; 00214 filtered_scan.ranges[i] = filtered_scan.ranges[i] + 0.03 * exp(-fabs(sin(angle))); 00215 }; 00216 } 00217 00218 // Transform into a PointCloud message 00219 int mask = laser_geometry::channel_option::Intensity | 00220 laser_geometry::channel_option::Distance | 00221 laser_geometry::channel_option::Index | 00222 laser_geometry::channel_option::Timestamp; 00223 00224 if (high_fidelity_) 00225 { 00226 try 00227 { 00228 projector_.transformLaserScanToPointCloud (target_frame_, filtered_scan, scan_cloud, tf_, mask); 00229 } 00230 catch (tf::TransformException &ex) 00231 { 00232 ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", target_frame_.c_str (), ex.what ()); 00233 return; 00234 //projector_.projectLaser (filtered_scan, scan_cloud, laser_max_range_, preservative_, mask); 00235 } 00236 } 00237 else 00238 { 00239 projector_.projectLaser (filtered_scan, tmp_cloud, laser_max_range_, mask); 00240 tf_.transformPointCloud(target_frame_, tmp_cloud, scan_cloud); 00241 } 00242 00243 sensor_msgs::PointCloud filtered_cloud; 00244 cloud_filter_chain_.update (scan_cloud, filtered_cloud); 00245 00246 cloud_pub_.publish(filtered_cloud); 00247 } 00248 00249 } ; 00250 00251 00252 00253 int 00254 main (int argc, char** argv) 00255 { 00256 ros::init (argc, argv, "scan_to_cloud_filter_chain"); 00257 ros::NodeHandle nh; 00258 ScanToCloudFilterChain f; 00259 00260 ros::spin(); 00261 00262 return (0); 00263 } 00264 00265