00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00044 #include <laser_geometry/laser_geometry.h>
00045
00046
00047 #include <tf/transform_listener.h>
00048 #include "tf/message_filter.h"
00049 #include "message_filters/subscriber.h"
00050
00051
00052 #include "filters/filter_chain.h"
00053 #include <pcl_ros/transforms.h>
00054
00057 class ScanToCloudFilterChain
00058 {
00059 public:
00060
00061
00062 laser_geometry::LaserProjection projector_;
00063
00064 double laser_max_range_;
00065 int window_;
00066
00067 bool high_fidelity_;
00068 std::string target_frame_;
00069 std::string scan_topic_, cloud_topic_;
00070
00071 ros::NodeHandle nh;
00072 ros::NodeHandle private_nh;
00073
00074
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::PointCloud2> cloud_filter_chain_;
00082 filters::FilterChain<sensor_msgs::LaserScan> scan_filter_chain_;
00083 ros::Publisher cloud_pub_;
00084
00085
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 incident_angle_correction_;
00097
00099 ScanToCloudFilterChain () : laser_max_range_ (DBL_MAX), private_nh("~"), filter_(tf_, "", 50),
00100 cloud_filter_chain_("sensor_msgs::PointCloud2"), 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
00107 using_default_target_frame_deprecated_ = !private_nh.hasParam("target_frame");
00108
00109
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("incident_angle_correction", incident_angle_correction_, 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::PointCloud2> (cloud_topic_, 10);
00139 else
00140 cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2> ("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
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
00199
00200 sensor_msgs::LaserScan filtered_scan;
00201 scan_filter_chain_.update (*scan_msg, filtered_scan);
00202
00203
00204 sensor_msgs::PointCloud2 tmp_cloud;
00205 sensor_msgs::PointCloud2 scan_cloud;
00206
00207
00208
00209 if(incident_angle_correction_)
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
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
00235 }
00236 }
00237 else
00238 {
00239 projector_.projectLaser (filtered_scan, tmp_cloud, laser_max_range_, mask);
00240 pcl_ros::transformPointCloud(target_frame_, tmp_cloud, scan_cloud, tf_);
00241 }
00242
00243 sensor_msgs::PointCloud2 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