apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add tolerance parameter

Browse Source
This commit is contained in:
Paul Bovbel 2015-01-18 11:43:24 -05:00 committed by Paul Bovbel
parent b9e0b5d213
commit fb5c47982c
3 changed files with 15 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h
View File

@ -70,6 +70,8 @@ namespace pointcloud_to_laserscan
virtual void onInit(); virtual void onInit();
void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg); void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
tf2_ros::filter_failure_reasons::FilterFailureReason reason);
void connectCb(); void connectCb();
@ -87,6 +89,7 @@ namespace pointcloud_to_laserscan
// ROS Parameters // ROS Parameters
unsigned int input_queue_size_; unsigned int input_queue_size_;
std::string target_frame_; std::string target_frame_;
double tolerance_;
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_; double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
bool use_inf_; bool use_inf_;
}; };

1
pointcloud_to_laserscan/launch/sample_node.launch
View File

@ -16,6 +16,7 @@
<remap from="scan" to="$(arg camera)/scan"/> <remap from="scan" to="$(arg camera)/scan"/>
<rosparam> <rosparam>
target_frame: base_link # Leave empty for no transform target_frame: base_link # Leave empty for no transform
transform_tolerance: 1.0
min_height: 0.0 min_height: 0.0
max_height: 1.0 max_height: 1.0

13
pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp
View File

@ -58,6 +58,7 @@ namespace pointcloud_to_laserscan
private_nh_ = getMTPrivateNodeHandle(); private_nh_ = getMTPrivateNodeHandle();
private_nh_.param<std::string>("target_frame", target_frame_, ""); private_nh_.param<std::string>("target_frame", target_frame_, "");
private_nh_.param<double>("tolerance", tolerance_, 0.0);
private_nh_.param<double>("min_height", min_height_, 0.0); private_nh_.param<double>("min_height", min_height_, 0.0);
private_nh_.param<double>("max_height", max_height_, 1.0); private_nh_.param<double>("max_height", max_height_, 1.0);
@ -91,23 +92,31 @@ namespace pointcloud_to_laserscan
{ {
boost::mutex::scoped_lock lock(connect_mutex_); boost::mutex::scoped_lock lock(connect_mutex_);
if (!sub_ && pub_.getNumSubscribers() > 0) { if (!sub_ && pub_.getNumSubscribers() > 0) {
NODELET_DEBUG("Got a subscriber to laserscan, starting subscriber to point cloud"); NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud");
sub_.reset(new FilteredSub(nh_, "cloud_in", input_queue_size_)); sub_.reset(new FilteredSub(nh_, "cloud_in", input_queue_size_));
if(!target_frame_.empty()) if(!target_frame_.empty())
{ {
message_filter_.reset(new MessageFilter(*sub_, tf2_, target_frame_, input_queue_size_, nh_)); message_filter_.reset(new MessageFilter(*sub_, tf2_, target_frame_, input_queue_size_, nh_));
message_filter_->setTolerance(ros::Duration(tolerance_));
message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2));
}else{ }else{
sub_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1)); sub_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
} }
} }
} }
void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
tf2_ros::filter_failure_reasons::FilterFailureReason reason){
NODELET_WARN_STREAM_THROTTLE(3.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to "
<< message_filter_->getTargetFramesString() << " with tolerance " << tolerance_);
}
void PointCloudToLaserScanNodelet::disconnectCb() void PointCloudToLaserScanNodelet::disconnectCb()
{ {
boost::mutex::scoped_lock lock(connect_mutex_); boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() == 0) { if (pub_.getNumSubscribers() == 0) {
NODELET_DEBUG("No subscibers to laserscan, shutting down subscriber to point cloud"); NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud");
if(!target_frame_.empty()) if(!target_frame_.empty())
{ {
message_filter_.reset(); message_filter_.reset();