Skip to content

Commit

Permalink
Added range checks for pcl. Changed panel init
Browse files Browse the repository at this point in the history
  • Loading branch information
@tysik
tysik committed Jul 21, 2017
1 parent 9dd4deb commit 3dc9b1a
Show file tree
Hide file tree
Showing 4 changed files with 108 additions and 66 deletions.
11 changes: 6 additions & 5 deletions include/obstacle_detector/panels/scans_merger_panel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
6 changes: 2 additions & 4 deletions include/obstacle_detector/scans_merger.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
47 changes: 33 additions & 14 deletions src/panels/scans_merger_panel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,7 +201,7 @@ void ScansMergerPanel::verifyInputs() {
try { p_max_y_range_ = boost::lexical_cast<double>(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() {
Expand All @@ -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<int>("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<bool>("active", p_active_, true);
nh_local_.param<bool>("publish_scan", p_publish_scan_, true);
nh_local_.param<bool>("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<double>("min_scanner_range", p_min_scanner_range_, 0.05);
nh_local_.param<double>("max_scanner_range", p_max_scanner_range_, 10.0);
nh_local_.param<double>("max_x_range", p_max_x_range_, 10.0);
nh_local_.param<double>("min_x_range", p_min_x_range_, -10.0);
nh_local_.param<double>("max_y_range", p_max_y_range_, 10.0);
nh_local_.param<double>("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<string>("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() {
Expand Down Expand Up @@ -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() {
Expand Down
110 changes: 67 additions & 43 deletions src/scans_merger.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,15 +63,14 @@ bool ScansMerger::updateParams(std_srvs::Empty::Request &req, std_srvs::Empty::R

nh_local_.param<double>("min_scanner_range", p_min_scanner_range_, 0.05);
nh_local_.param<double>("max_scanner_range", p_max_scanner_range_, 10.0);
nh_local_.param<double>("max_x_range", p_max_x_range_, 10.0);

nh_local_.param<double>("min_x_range", p_min_x_range_, -10.0);
nh_local_.param<double>("max_y_range", p_max_y_range_, 10.0);
nh_local_.param<double>("max_x_range", p_max_x_range_, 10.0);
nh_local_.param<double>("min_y_range", p_min_y_range_, -10.0);
nh_local_.param<double>("max_y_range", p_max_y_range_, 10.0);

nh_local_.param<string>("fixed_frame_id", p_fixed_frame_id_, "map");
nh_local_.param<string>("target_frame_id", p_target_frame_id_, "scanner_base");
nh_local_.param<string>("front_scan_frame_id", p_front_scan_frame_id_, "front_scanner");
nh_local_.param<string>("rear_scan_frame_id", p_rear_scan_frame_id_, "rear_scanner");
nh_local_.param<string>("target_frame_id", p_target_frame_id_, "robot");

if (p_active_ != prev_active) {
if (p_active_) {
Expand Down Expand Up @@ -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<float> ranges;
vector<geometry_msgs::Point32> 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<float> 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<int>(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<int>(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<int>(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<int>(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);
}
Expand All @@ -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);
}
Expand Down

0 comments on commit 3dc9b1a

Please sign in to comment.