32 #include "sensor_msgs/LaserScan.h" 55 sensor_msgs::LaserScan
msg_;
66 scan_sub_(nh_,
"scan", 50),
69 filter_chain_(
"sensor_msgs::LaserScan")
73 using_filter_chain_deprecated_ = private_nh_.
hasParam(
"filter_chain");
75 if (using_filter_chain_deprecated_)
76 filter_chain_.
configure(
"filter_chain", private_nh_);
78 filter_chain_.
configure(
"scan_filter_chain", private_nh_);
80 std::string tf_message_filter_target_frame;
82 if (private_nh_.
hasParam(
"tf_message_filter_target_frame"))
84 private_nh_.
getParam(
"tf_message_filter_target_frame", tf_message_filter_target_frame);
86 private_nh_.
param(
"tf_message_filter_tolerance", tf_filter_tolerance_, 0.03);
103 output_pub_ = nh_.
advertise<sensor_msgs::LaserScan>(
"scan_filtered", 1000);
121 if (using_filter_chain_deprecated_)
122 ROS_WARN(
"Use of '~filter_chain' parameter in scan_to_scan_filter_chain has been deprecated. Please replace with '~scan_filter_chain'.");
126 void callback(
const sensor_msgs::LaserScan::ConstPtr& msg_in)
129 if (filter_chain_.
update(*msg_in, msg_))
134 ROS_ERROR_THROTTLE(1,
"Filtering the scan from time %i.%i failed.", msg_in->header.stamp.sec, msg_in->header.stamp.nsec);
139 int main(
int argc,
char **argv)
141 ros::init(argc, argv,
"scan_to_scan_filter_chain");
bool using_filter_chain_deprecated_
int main(int argc, char **argv)
ros::Publisher output_pub_
void publish(const boost::shared_ptr< M > &message) const
message_filters::Subscriber< sensor_msgs::LaserScan > scan_sub_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf::MessageFilter< sensor_msgs::LaserScan > * tf_filter_
ros::Timer deprecation_timer_
double tf_filter_tolerance_
ROSCPP_DECL void spin(Spinner &spinner)
sensor_msgs::LaserScan msg_
#define ROS_ERROR_THROTTLE(rate,...)
filters::FilterChain< sensor_msgs::LaserScan > filter_chain_
void deprecation_warn(const ros::TimerEvent &e)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle private_nh_
bool getParam(const std::string &key, std::string &s) const
void callback(const sensor_msgs::LaserScan::ConstPtr &msg_in)
bool update(const T &data_in, T &data_out)
bool configure(std::string param_name, ros::NodeHandle node=ros::NodeHandle())
void setTargetFrame(const std::string &target_frame)
tf::TransformListener * tf_
bool hasParam(const std::string &key) const
void setTolerance(const ros::Duration &tolerance)
Connection registerCallback(const C &callback)