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/PointCloud.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
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::PointCloud> 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
00098 ScanToCloudFilterChain () : laser_max_range_ (DBL_MAX), private_nh("~"), filter_(tf_, "", 50),
00099 cloud_filter_chain_("sensor_msgs::PointCloud"), 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
00106 using_default_target_frame_deprecated_ = !private_nh.hasParam("target_frame");
00107
00108
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
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::PointCloud> (cloud_topic_, 10);
00138 else
00139 cloud_pub_ = nh.advertise<sensor_msgs::PointCloud> ("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
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
00198
00199 sensor_msgs::LaserScan filtered_scan;
00200 scan_filter_chain_.update (*scan_msg, filtered_scan);
00201
00202
00203 sensor_msgs::PointCloud tmp_cloud;
00204 sensor_msgs::PointCloud scan_cloud;
00205
00206
00207
00208 for (unsigned int i = 0; i < filtered_scan.ranges.size(); i++)
00209 {
00210 double angle = filtered_scan.angle_min + i * filtered_scan.angle_increment;
00211 filtered_scan.ranges[i] = filtered_scan.ranges[i] + 0.03 * exp(-fabs(sin(angle)));
00212
00213 };
00214
00215
00216 int mask = laser_geometry::channel_option::Intensity |
00217 laser_geometry::channel_option::Distance |
00218 laser_geometry::channel_option::Index |
00219 laser_geometry::channel_option::Timestamp;
00220
00221 if (high_fidelity_)
00222 {
00223 try
00224 {
00225 projector_.transformLaserScanToPointCloud (target_frame_, filtered_scan, scan_cloud, tf_, mask);
00226 }
00227 catch (tf::TransformException &ex)
00228 {
00229 ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", target_frame_.c_str (), ex.what ());
00230 return;
00231
00232 }
00233 }
00234 else
00235 {
00236 projector_.projectLaser (filtered_scan, tmp_cloud, laser_max_range_, mask);
00237 tf_.transformPointCloud(target_frame_, tmp_cloud, scan_cloud);
00238 }
00239
00240 sensor_msgs::PointCloud 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