From 3dc9b1a394523d71678154ca85a5535558edd4d3 Mon Sep 17 00:00:00 2001 From: Mateusz Przybyla Date: Fri, 21 Jul 2017 16:17:30 +0200 Subject: [PATCH] Added range checks for pcl. Changed panel init --- .../panels/scans_merger_panel.h | 11 +- include/obstacle_detector/scans_merger.h | 6 +- src/panels/scans_merger_panel.cpp | 47 +++++--- src/scans_merger.cpp | 110 +++++++++++------- 4 files changed, 108 insertions(+), 66 deletions(-) diff --git a/include/obstacle_detector/panels/scans_merger_panel.h b/include/obstacle_detector/panels/scans_merger_panel.h index 5d3448d..6a94d4b 100644 --- a/include/obstacle_detector/panels/scans_merger_panel.h +++ b/include/obstacle_detector/panels/scans_merger_panel.h @@ -92,20 +92,21 @@ private Q_SLOTS: ros::ServiceClient params_cli_; // Parameters - int p_ranges_num_; - bool p_active_; bool p_publish_scan_; bool p_publish_pcl_; + int p_ranges_num_; + double p_min_scanner_range_; double p_max_scanner_range_; - double p_max_x_range_; + double p_min_x_range_; - double p_max_y_range_; + double p_max_x_range_; double p_min_y_range_; + double p_max_y_range_; - std::string p_frame_id_; + std::string p_target_frame_id_; }; } // namespace obstacle_detector diff --git a/include/obstacle_detector/scans_merger.h b/include/obstacle_detector/scans_merger.h index d22887b..a81e376 100644 --- a/include/obstacle_detector/scans_merger.h +++ b/include/obstacle_detector/scans_merger.h @@ -90,15 +90,13 @@ class ScansMerger double p_min_scanner_range_; double p_max_scanner_range_; - double p_max_x_range_; double p_min_x_range_; - double p_max_y_range_; + double p_max_x_range_; double p_min_y_range_; + double p_max_y_range_; std::string p_fixed_frame_id_; std::string p_target_frame_id_; - std::string p_front_scan_frame_id_; - std::string p_rear_scan_frame_id_; }; } // namespace obstacle_detector diff --git a/src/panels/scans_merger_panel.cpp b/src/panels/scans_merger_panel.cpp index bdd0915..652b9ab 100644 --- a/src/panels/scans_merger_panel.cpp +++ b/src/panels/scans_merger_panel.cpp @@ -201,7 +201,7 @@ void ScansMergerPanel::verifyInputs() { try { p_max_y_range_ = boost::lexical_cast(y_max_input_->text().toStdString()); } catch(boost::bad_lexical_cast &) { p_max_y_range_ = 0.0; y_max_input_->setText("0.0"); } - p_frame_id_ = frame_id_input_->text().toStdString(); + p_target_frame_id_ = frame_id_input_->text().toStdString(); } void ScansMergerPanel::setParams() { @@ -218,24 +218,42 @@ void ScansMergerPanel::setParams() { nh_local_.setParam("max_y_range", p_max_y_range_); nh_local_.setParam("min_y_range", p_min_y_range_); - nh_local_.setParam("frame_id", p_frame_id_); + nh_local_.setParam("target_frame_id", p_target_frame_id_); } void ScansMergerPanel::getParams() { - nh_local_.param("ranges_num", p_ranges_num_, 1000); + if (!nh_local_.getParam("active", p_active_)) + p_active_ = false; + + if (!nh_local_.getParam("publish_scan", p_publish_scan_)) + p_publish_scan_ = false; + + if (!nh_local_.getParam("publish_pcl", p_publish_pcl_)) + p_publish_pcl_ = false; + + if (!nh_local_.getParam("ranges_num", p_ranges_num_)) + p_ranges_num_ = 0; + + if (!nh_local_.getParam("min_scanner_range", p_min_scanner_range_)) + p_min_scanner_range_ = 0.0; - nh_local_.param("active", p_active_, true); - nh_local_.param("publish_scan", p_publish_scan_, true); - nh_local_.param("publish_pcl", p_publish_pcl_, true); + if (!nh_local_.getParam("max_scanner_range", p_max_scanner_range_)) + p_max_scanner_range_ = 0.0; - nh_local_.param("min_scanner_range", p_min_scanner_range_, 0.05); - nh_local_.param("max_scanner_range", p_max_scanner_range_, 10.0); - nh_local_.param("max_x_range", p_max_x_range_, 10.0); - nh_local_.param("min_x_range", p_min_x_range_, -10.0); - nh_local_.param("max_y_range", p_max_y_range_, 10.0); - nh_local_.param("min_y_range", p_min_y_range_, -10.0); + if (!nh_local_.getParam("max_x_range", p_max_x_range_)) + p_max_x_range_ = 0.0; - nh_local_.param("frame_id", p_frame_id_, "scanner_base"); + if (!nh_local_.getParam("min_x_range", p_min_x_range_)) + p_min_x_range_ = 0.0; + + if (!nh_local_.getParam("max_y_range", p_max_y_range_)) + p_max_y_range_ = 0.0; + + if (!nh_local_.getParam("min_y_range", p_min_y_range_)) + p_min_y_range_ = 0.0; + + if (!nh_local_.getParam("target_frame_id", p_target_frame_id_)) + p_target_frame_id_ = std::string("-"); } void ScansMergerPanel::evaluateParams() { @@ -263,7 +281,8 @@ void ScansMergerPanel::evaluateParams() { x_max_input_->setText(QString::number(p_max_x_range_)); y_min_input_->setText(QString::number(p_min_y_range_)); y_max_input_->setText(QString::number(p_max_y_range_)); - frame_id_input_->setText(QString::fromStdString(p_frame_id_)); + + frame_id_input_->setText(QString::fromStdString(p_target_frame_id_)); } void ScansMergerPanel::notifyParamsUpdate() { diff --git a/src/scans_merger.cpp b/src/scans_merger.cpp index 4739235..fc15d08 100644 --- a/src/scans_merger.cpp +++ b/src/scans_merger.cpp @@ -63,15 +63,14 @@ bool ScansMerger::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::R nh_local_.param("min_scanner_range", p_min_scanner_range_, 0.05); nh_local_.param("max_scanner_range", p_max_scanner_range_, 10.0); - nh_local_.param("max_x_range", p_max_x_range_, 10.0); + nh_local_.param("min_x_range", p_min_x_range_, -10.0); - nh_local_.param("max_y_range", p_max_y_range_, 10.0); + nh_local_.param("max_x_range", p_max_x_range_, 10.0); nh_local_.param("min_y_range", p_min_y_range_, -10.0); + nh_local_.param("max_y_range", p_max_y_range_, 10.0); nh_local_.param("fixed_frame_id", p_fixed_frame_id_, "map"); - nh_local_.param("target_frame_id", p_target_frame_id_, "scanner_base"); - nh_local_.param("front_scan_frame_id", p_front_scan_frame_id_, "front_scanner"); - nh_local_.param("rear_scan_frame_id", p_rear_scan_frame_id_, "rear_scanner"); + nh_local_.param("target_frame_id", p_target_frame_id_, "robot"); if (p_active_ != prev_active) { if (p_active_) { @@ -126,57 +125,84 @@ void ScansMerger::rearScanCallback(const sensor_msgs::LaserScan::ConstPtr rear_s void ScansMerger::publishMessages() { ros::Time now = ros::Time::now(); - if (!tf_ls_.waitForTransform(p_target_frame_id_, now, front_pcl_.header.frame_id, front_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(1.0))) - return; - - if (!tf_ls_.waitForTransform(p_target_frame_id_, now, rear_pcl_.header.frame_id, rear_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(1.0))) - return; - + vector ranges; + vector points; sensor_msgs::PointCloud new_front_pcl, new_rear_pcl; - tf_ls_.transformPointCloud(p_target_frame_id_, now, front_pcl_, p_fixed_frame_id_, new_front_pcl); - tf_ls_.transformPointCloud(p_target_frame_id_, now, rear_pcl_, p_fixed_frame_id_, new_rear_pcl); - if (p_publish_scan_) { - sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan); + ranges.assign(p_ranges_num_, nanf("")); // Assign nan values - scan_msg->header.frame_id = p_target_frame_id_; - scan_msg->header.stamp = now; - scan_msg->angle_min = -M_PI; - scan_msg->angle_max = M_PI; - scan_msg->angle_increment = 2.0 * M_PI / (p_ranges_num_ - 1); - scan_msg->time_increment = 0.0; - scan_msg->scan_time = 0.1; - scan_msg->range_min = p_min_scanner_range_; - scan_msg->range_max = p_max_scanner_range_; + if (!front_scan_error_) { + if (!tf_ls_.waitForTransform(p_target_frame_id_, now, front_pcl_.header.frame_id, front_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(1.0))) + return; - vector ranges; - ranges.assign(p_ranges_num_, 2.0 * p_max_scanner_range_); + tf_ls_.transformPointCloud(p_target_frame_id_, now, front_pcl_, p_fixed_frame_id_, new_front_pcl); for (auto& point : new_front_pcl.points) { - float angle = atan2(point.y, point.x); - float range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); + if (point.x > p_min_x_range_ && point.x < p_max_x_range_ && + point.y > p_min_y_range_ && point.y < p_max_y_range_) { + + double range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); - int idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); + if (range > p_min_scanner_range_ && range < p_max_scanner_range_) { + if (p_publish_pcl_) { + points.push_back(point); + } - if (ranges[idx] > range) - ranges[idx] = range; + if (p_publish_scan_) { + double angle = atan2(point.y, point.x); + + size_t idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); + if (isnan(ranges[idx]) || range < ranges[idx]) + ranges[idx] = range; + } + } + } } + } + + + if (!rear_scan_error_) { + if (!tf_ls_.waitForTransform(p_target_frame_id_, now, rear_pcl_.header.frame_id, rear_pcl_.header.stamp, p_fixed_frame_id_, ros::Duration(1.0))) + return; + + tf_ls_.transformPointCloud(p_target_frame_id_, now, rear_pcl_, p_fixed_frame_id_, new_rear_pcl); for (auto& point : new_rear_pcl.points) { - float angle = atan2(point.y, point.x); - float range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); + if (point.x > p_min_x_range_ && point.x < p_max_x_range_ && + point.y > p_min_y_range_ && point.y < p_max_y_range_) { - int idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); + double range = sqrt(pow(point.x, 2.0) + pow(point.y, 2.0)); - if (ranges[idx] > range) - ranges[idx] = range; + if (range > p_min_scanner_range_ && range < p_max_scanner_range_) { + if (p_publish_pcl_) { + points.push_back(point); + } + + if (p_publish_scan_) { + double angle = atan2(point.y, point.x); + + size_t idx = static_cast(p_ranges_num_ * (angle + M_PI) / (2.0 * M_PI)); + if (isnan(ranges[idx]) || range < ranges[idx]) + ranges[idx] = range; + } + } + } } + } - for (int jdx = 0; jdx < ranges.size(); ++jdx) - if (ranges[jdx] < p_min_scanner_range_ || ranges[jdx] > p_max_scanner_range_) - ranges[jdx] = nan(""); + if (p_publish_scan_) { + sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan); - scan_msg->ranges = ranges; + scan_msg->header.frame_id = p_target_frame_id_; + scan_msg->header.stamp = now; + scan_msg->angle_min = -M_PI; + scan_msg->angle_max = M_PI; + scan_msg->angle_increment = 2.0 * M_PI / (p_ranges_num_ - 1); + scan_msg->time_increment = 0.0; + scan_msg->scan_time = 0.1; + scan_msg->range_min = p_min_scanner_range_; + scan_msg->range_max = p_max_scanner_range_; + scan_msg->ranges.assign(ranges.begin(), ranges.end()); scan_pub_.publish(scan_msg); } @@ -186,9 +212,7 @@ void ScansMerger::publishMessages() { pcl_msg->header.frame_id = p_target_frame_id_; pcl_msg->header.stamp = now; - - pcl_msg->points.insert(pcl_msg->points.end(), new_front_pcl.points.begin(), new_front_pcl.points.end()); - pcl_msg->points.insert(pcl_msg->points.end(), new_rear_pcl.points.begin(), new_rear_pcl.points.end()); + pcl_msg->points.assign(points.begin(), points.end()); pcl_pub_.publish(pcl_msg); }