38 #include <sensor_msgs/PointCloud2.h> 39 #include <sensor_msgs/LaserScan.h> 99 cloud_filter_chain_(
"sensor_msgs::PointCloud2"), scan_filter_chain_(
"sensor_msgs::LaserScan")
101 private_nh.
param(
"high_fidelity", high_fidelity_,
false);
102 private_nh.
param(
"notifier_tolerance", tf_tolerance_, 0.03);
103 private_nh.
param(
"target_frame", target_frame_, std::string (
"base_link"));
106 using_default_target_frame_deprecated_ = !private_nh.
hasParam(
"target_frame");
109 using_scan_topic_deprecated_ = private_nh.
hasParam(
"scan_topic");
110 using_cloud_topic_deprecated_ = private_nh.
hasParam(
"cloud_topic");
111 using_laser_max_range_deprecated_ = private_nh.
hasParam(
"laser_max_range");
112 using_filter_window_deprecated_ = private_nh.
hasParam(
"filter_window");
113 using_cloud_filters_deprecated_ = private_nh.
hasParam(
"cloud_filters/filter_chain");
114 using_scan_filters_deprecated_ = private_nh.
hasParam(
"scan_filters/filter_chain");
115 using_cloud_filters_wrong_deprecated_ = private_nh.
hasParam(
"cloud_filters/cloud_filter_chain");
116 using_scan_filters_wrong_deprecated_ = private_nh.
hasParam(
"scan_filters/scan_filter_chain");
119 private_nh.
param(
"filter_window", window_, 2);
120 private_nh.
param(
"laser_max_range", laser_max_range_, DBL_MAX);
121 private_nh.
param(
"scan_topic", scan_topic_, std::string(
"tilt_scan"));
122 private_nh.
param(
"cloud_topic", cloud_topic_, std::string(
"tilt_laser_cloud_filtered"));
123 private_nh.
param(
"incident_angle_correction", incident_angle_correction_,
true);
129 if (using_scan_topic_deprecated_)
136 if (using_cloud_topic_deprecated_)
139 cloud_pub_ = nh.
advertise<sensor_msgs::PointCloud2> (
"cloud_filtered", 10);
141 std::string cloud_filter_xml;
143 if (using_cloud_filters_deprecated_)
144 cloud_filter_chain_.
configure(
"cloud_filters/filter_chain", private_nh);
145 else if (using_cloud_filters_wrong_deprecated_)
146 cloud_filter_chain_.
configure(
"cloud_filters/cloud_filter_chain", private_nh);
148 cloud_filter_chain_.
configure(
"cloud_filter_chain", private_nh);
150 if (using_scan_filters_deprecated_)
151 scan_filter_chain_.
configure(
"scan_filter/filter_chain", private_nh);
152 else if (using_scan_filters_wrong_deprecated_)
153 scan_filter_chain_.
configure(
"scan_filters/scan_filter_chain", private_nh);
155 scan_filter_chain_.
configure(
"scan_filter_chain", private_nh);
163 if (using_scan_topic_deprecated_)
164 ROS_WARN(
"Use of '~scan_topic' parameter in scan_to_cloud_filter_chain has been deprecated.");
166 if (using_cloud_topic_deprecated_)
167 ROS_WARN(
"Use of '~cloud_topic' parameter in scan_to_cloud_filter_chain has been deprecated.");
169 if (using_laser_max_range_deprecated_)
170 ROS_WARN(
"Use of '~laser_max_range' parameter in scan_to_cloud_filter_chain has been deprecated.");
172 if (using_filter_window_deprecated_)
173 ROS_WARN(
"Use of '~filter_window' parameter in scan_to_cloud_filter_chain has been deprecated.");
175 if (using_default_target_frame_deprecated_)
176 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.");
178 if (using_cloud_filters_deprecated_)
179 ROS_WARN(
"Use of '~cloud_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated. Replace with '~cloud_filter_chain'");
181 if (using_scan_filters_deprecated_)
182 ROS_WARN(
"Use of '~scan_filters/filter_chain' parameter in scan_to_cloud_filter_chain has been deprecated. Replace with '~scan_filter_chain'");
184 if (using_cloud_filters_wrong_deprecated_)
185 ROS_WARN(
"Use of '~cloud_filters/cloud_filter_chain' parameter in scan_to_cloud_filter_chain is incorrect. Please Replace with '~cloud_filter_chain'");
187 if (using_scan_filters_wrong_deprecated_)
188 ROS_WARN(
"Use of '~scan_filters/scan_filter_chain' parameter in scan_to_scan_filter_chain is incorrect. Please Replace with '~scan_filter_chain'");
199 sensor_msgs::LaserScan filtered_scan;
200 scan_filter_chain_.
update (*scan_msg, filtered_scan);
203 sensor_msgs::PointCloud2 scan_cloud;
207 if(incident_angle_correction_)
209 for (
unsigned int i = 0; i < filtered_scan.ranges.size(); i++)
211 double angle = filtered_scan.angle_min + i * filtered_scan.angle_increment;
212 filtered_scan.ranges[i] = filtered_scan.ranges[i] + 0.03 * exp(-fabs(sin(angle)));
230 ROS_WARN(
"High fidelity enabled, but TF returned a transform exception to frame %s: %s", target_frame_.c_str (), ex.what ());
240 sensor_msgs::PointCloud2 filtered_cloud;
241 cloud_filter_chain_.
update (scan_cloud, filtered_cloud);
243 cloud_pub_.
publish(filtered_cloud);
253 ros::init (argc, argv,
"scan_to_cloud_filter_chain");
bool using_scan_filters_deprecated_
bool using_default_target_frame_deprecated_
bool using_scan_topic_deprecated_
tf::MessageFilter< sensor_msgs::LaserScan > filter_
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
ROSCPP_DECL void spin(Spinner &spinner)
filters::FilterChain< sensor_msgs::LaserScan > scan_filter_chain_
bool using_cloud_filters_wrong_deprecated_
bool incident_angle_correction_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
bool using_scan_filters_wrong_deprecated_
std::string target_frame_
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void deprecation_warn(const ros::TimerEvent &e)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
filters::FilterChain< sensor_msgs::PointCloud2 > cloud_filter_chain_
bool using_laser_max_range_deprecated_
ros::Publisher cloud_pub_
message_filters::Subscriber< sensor_msgs::LaserScan > sub_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
bool using_cloud_filters_deprecated_
bool using_cloud_topic_deprecated_
bool update(const T &data_in, T &data_out)
bool configure(std::string param_name, ros::NodeHandle node=ros::NodeHandle())
ros::Timer deprecation_timer_
void setTargetFrame(const std::string &target_frame)
tf::TransformListener tf_
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options=channel_option::Default)
bool hasParam(const std::string &key) const
bool using_filter_window_deprecated_
laser_geometry::LaserProjection projector_
void setTolerance(const ros::Duration &tolerance)
ros::NodeHandle private_nh
int main(int argc, char **argv)
Connection registerCallback(const C &callback)