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();
}
