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