void LaserScanPlugin::DrawIcon() { if (icon_) { QPixmap icon(16, 16); icon.fill(Qt::transparent); QPainter painter(&icon); painter.setRenderHint(QPainter::Antialiasing, true); QPen pen; pen.setWidth(4); pen.setCapStyle(Qt::RoundCap); pen.setColor(InterpolateColors(min_color_, max_color_, 0.2)); painter.setPen(pen); painter.drawPoint(2, 13); pen.setColor(InterpolateColors(min_color_, max_color_, 0.6)); painter.setPen(pen); painter.drawPoint(4, 6); pen.setColor(InterpolateColors(min_color_, max_color_, 0.4)); painter.setPen(pen); painter.drawPoint(12, 9); pen.setColor(InterpolateColors(min_color_, max_color_, 0.8)); painter.setPen(pen); painter.drawPoint(13, 2); icon_->SetPixmap(icon); } }
void LaserScanPlugin::UpdateColors() { for (size_t i = 0; i < scans_.size(); i++) { for (size_t j = 0; j < scans_[i].points.size(); j++) { StampedPoint& point = scans_[i].points[j]; double weight = std::min(1.0, std::max( 0.0, (point.intensity - min_intensity_) / max_intensity_)); point.color = InterpolateColors(min_color_, max_color_, weight); } } }
void do_SetupGlobals(struct work_item *p) { canvas_width = p->u.setup_canvas_width; /* Initialize the mask so that all pixels recompute every time */ memset(mask, 0, sizeof(mask)); InterpolateColors(); }
void LaserScanPlugin::laserScanCallback(const sensor_msgs::LaserScanConstPtr msg) { if (!has_message_) { source_frame_ = msg->header.frame_id; initialized_ = true; has_message_ = true; } Scan scan; scan.stamp = msg->header.stamp; scan.transformed = true; scan.points.reserve(msg->ranges.size()); scan.has_intensity = msg->ranges.size() == msg->intensities.size(); double angle, x, y; for (size_t i = 0; i < msg->ranges.size(); i++) { // Discard the point if it's out of range if( msg->ranges[i] > msg->range_max || msg->ranges[i] < msg->range_min) { continue; } StampedPoint point; angle = msg->angle_min + msg->angle_increment * i; x = cos(angle) * msg->ranges[i]; y = sin(angle) * msg->ranges[i]; point.point = tf::Point(x, y, 0.0f); if (scan.has_intensity) { point.intensity = msg->intensities[i]; double weight = std::min(1.0, std::max( 0.0, (point.intensity - min_intensity_) / max_intensity_)); point.color = InterpolateColors(min_color_, max_color_, weight); } else { point.color = min_color_; } scan.points.push_back(point); } transform_util::Transform transform; if (GetTransform(msg->header.stamp, transform)) { for (size_t i = 0; i < scan.points.size(); i++) { scan.points[i].transformed_point = transform * scan.points[i].point; } } else { scan.transformed = false; PrintError("No transform between " + source_frame_ + " and " + target_frame_); } scans_.push_back(scan); if (buffer_size_ > 0) { while (static_cast<int>(scans_.size()) > buffer_size_) { scans_.pop_front(); } } canvas_->update(); }