void OverlayMenuDisplay::openingAnimation()
{
ROS_DEBUG("openningAnimation");
prepareOverlay();
int current_width = animation_t_ / animate_duration * overlay_->getTextureWidth();
int current_height = animation_t_ / animate_duration * overlay_->getTextureHeight();
{
ScopedPixelBuffer buffer = overlay_->getBuffer();
QColor bg_color(0, 0, 0, 255.0 / 2.0);
QColor transparent(0, 0, 0, 0.0);
QImage Hud = buffer.getQImage(*overlay_);
for (int i = 0; i < overlay_->getTextureWidth(); i++) {
for (int j = 0; j < overlay_->getTextureHeight(); j++) {
if (i > (overlay_->getTextureWidth() - current_width) / 2.0 &&
i < overlay_->getTextureWidth() - (overlay_->getTextureWidth() - current_width) / 2.0 &&
j > (overlay_->getTextureHeight() - current_height) / 2.0 &&
j < overlay_->getTextureHeight() - (overlay_->getTextureHeight() - current_height) / 2.0) {
Hud.setPixel(i, j, bg_color.rgba());
}
else {
Hud.setPixel(i, j, transparent.rgba());
}
}
}
}
overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
int window_width = context_->getViewManager()->getRenderPanel()->width();
int window_height = context_->getViewManager()->getRenderPanel()->height();
double window_left = (window_width - (int)overlay_->getTextureWidth()) / 2.0;
double window_top = (window_height - (int)overlay_->getTextureHeight()) / 2.0;
overlay_->setPosition(window_left, window_top);
current_menu_ = next_menu_;
}
void OverlayImageDisplay::redraw()
{
cv_bridge::CvImagePtr cv_ptr;
try
{
if (msg_->width == 0 || msg_->height == 0) {
// image width/height and texture width/height should be same
// but they are not when input image width/height is 0
return;
}
else if (msg_->encoding == sensor_msgs::image_encodings::RGBA8 ||
msg_->encoding == sensor_msgs::image_encodings::BGRA8) {
cv_ptr = cv_bridge::toCvCopy(msg_, sensor_msgs::image_encodings::RGBA8);
cv::Mat mat = cv_ptr->image;
ScopedPixelBuffer buffer = overlay_->getBuffer();
QImage Hud = buffer.getQImage(*overlay_);
for (int i = 0; i < overlay_->getTextureWidth(); i++) {
for (int j = 0; j < overlay_->getTextureHeight(); j++) {
QColor color(mat.data[j * mat.step + i * mat.elemSize() + 0],
mat.data[j * mat.step + i * mat.elemSize() + 1],
mat.data[j * mat.step + i * mat.elemSize() + 2],
mat.data[j * mat.step + i * mat.elemSize() + 3]);
Hud.setPixel(i, j, color.rgba());
}
}
}
else {
cv_ptr = cv_bridge::toCvCopy(msg_, sensor_msgs::image_encodings::RGB8);
cv::Mat mat = cv_ptr->image;
ScopedPixelBuffer buffer = overlay_->getBuffer();
QImage Hud = buffer.getQImage(*overlay_);
for (int i = 0; i < overlay_->getTextureWidth(); i++) {
for (int j = 0; j < overlay_->getTextureHeight(); j++) {
QColor color(mat.data[j * mat.step + i * mat.elemSize() + 0],
mat.data[j * mat.step + i * mat.elemSize() + 1],
mat.data[j * mat.step + i * mat.elemSize() + 2],
alpha_ * 255.0);
Hud.setPixel(i, j, color.rgba());
}
}
}
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
}
}
void GISCircleVisualizer::update(float wall_dt, float ros_dt)
{
ros::WallTime now = ros::WallTime::now();
std::string text = text_ + " ";
{
ScopedPixelBuffer buffer = texture_object_->getBuffer();
QColor transparent(0, 0, 0, 0);
QColor foreground = rviz::ogreToQt(color_);
QColor white(255, 255, 255, color_.a * 255);
QImage Hud = buffer.getQImage(128, 128, transparent);
double line_width = 5;
double inner_line_width = 10;
double l = 128;
//double cx = l / 2 - line_width / 4.0;
double cx = l / 2;
//double cy = l / 2 - line_width / 4.0;
double cy = l / 2;
double r = 48;
double inner_r = 40;
double mouse_r = 30;
double mouse_cy_offset = 5;
QPainter painter( &Hud );
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setPen(QPen(foreground, line_width, Qt::SolidLine));
painter.setBrush(white);
painter.drawEllipse(line_width / 2.0, line_width / 2.0,
l - line_width, l - line_width);
double offset_rate = fmod(now.toSec(), 10) / 10.0;
double theta_offset = offset_rate * M_PI * 2.0;
for (size_t ci = 0; ci < text.length(); ci++) {
double theta = M_PI * 2.0 / text.length() * ci + theta_offset;
painter.save();
QFont font("DejaVu Sans Mono");
font.setPointSize(8);
font.setBold(false);
painter.setFont(font);
painter.translate(cx + r * cos(theta),
cy + r * sin(theta));
painter.rotate(theta / M_PI * 180 + 90);
painter.drawText(0, 0, text.substr(ci, 1).c_str());
painter.restore();
}
painter.setPen(QPen(foreground, inner_line_width, Qt::SolidLine));
painter.setBrush(transparent);
painter.drawEllipse(cx - inner_r, cy - inner_r,
inner_r * 2, inner_r * 2);
double mouse_c_x = cx;
double mouse_c_y = cy - mouse_cy_offset;
double start_angle = -25 * M_PI / 180;
double end_angle = -125 * M_PI / 180;
painter.setPen(QPen(foreground, line_width, Qt::SolidLine));
painter.drawChord(mouse_c_x - mouse_r, mouse_c_y - mouse_r,
2.0 * mouse_r, 2.0 * mouse_r,
start_angle * 180 / M_PI * 16,
end_angle * 180 / M_PI * 16);
painter.end();
}
}
void OverlayMenuDisplay::redraw()
{
ROS_DEBUG("redraw");
prepareOverlay();
{
ScopedPixelBuffer buffer = overlay_->getBuffer();
QColor bg_color(0, 0, 0, 255.0 / 2.0);
QColor fg_color(25, 255, 240, 255.0);
QImage Hud = buffer.getQImage(*overlay_, bg_color);
QPainter painter( &Hud );
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setPen(QPen(fg_color, 1, Qt::SolidLine));
painter.setFont(font());
int line_height = fontMetrics().height();
int w = drawAreaWidth(next_menu_);
painter.drawText(menu_padding_x, menu_padding_y,
w, line_height,
Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
next_menu_->title.c_str());
for (size_t i = 0; i < next_menu_->menus.size(); i++) {
std::string menu = getMenuString(next_menu_, i);
painter.drawText(menu_padding_x, line_height * ( 1 + i ) + menu_padding_y + menu_last_padding_y,
w, line_height,
Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
menu.c_str());
}
if (next_menu_->current_index <= next_menu_->menus.size()) {
// draw '>'
painter.drawText(menu_padding_x - fontMetrics().width(">") * 2,
line_height * ( 1 + next_menu_->current_index ) + menu_padding_y + menu_last_padding_y,
w, line_height,
Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
">");
}
// draw line
int texture_width = overlay_->getTextureWidth();
int texture_height = overlay_->getTextureHeight();
painter.drawLine(menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
painter.drawLine(texture_width - menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
texture_width - menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
painter.drawLine(menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
texture_width - menu_padding_x / 2, menu_last_padding_y / 2 + line_height);
painter.drawLine(menu_padding_x / 2, texture_height - menu_last_padding_y / 2,
texture_width - menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
painter.end();
current_menu_ = next_menu_;
}
overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
int window_width = context_->getViewManager()->getRenderPanel()->width();
int window_height = context_->getViewManager()->getRenderPanel()->height();
double window_left = (window_width - (int)overlay_->getTextureWidth()) / 2.0;
double window_top = (window_height - (int)overlay_->getTextureHeight()) / 2.0;
overlay_->setPosition(window_left, window_top);
}
void OverlayTextDisplay::update(float wall_dt, float ros_dt)
{
if (!require_update_texture_) {
return;
}
if (!isEnabled()) {
return;
}
if (!overlay_) {
return;
}
overlay_->updateTextureSize(texture_width_, texture_height_);
{
ScopedPixelBuffer buffer = overlay_->getBuffer();
QImage Hud = buffer.getQImage(*overlay_, bg_color_);
QPainter painter( &Hud );
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setPen(QPen(fg_color_, line_width_ || 1, Qt::SolidLine));
uint16_t w = overlay_->getTextureWidth();
uint16_t h = overlay_->getTextureHeight();
// font
if (text_size_ != 0) {
//QFont font = painter.font();
QFont font(font_.length() > 0 ? font_.c_str(): "Arial");
font.setPointSize(text_size_);
font.setBold(true);
painter.setFont(font);
}
if (text_.length() > 0) {
// painter.drawText(0, 0, w, h,
// Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
// text_.c_str());
std::string color_wrapped_text
= (boost::format("<span style=\"color: rgba(%2%, %3%, %4%, %5%)\">%1%</span>")
% text_ % fg_color_.red() % fg_color_.green() % fg_color_.blue() %
fg_color_.alpha()).str();
QStaticText static_text(
boost::algorithm::replace_all_copy(color_wrapped_text, "\n", "<br >").c_str());
static_text.setTextWidth(w);
painter.drawStaticText(0, 0, static_text);
}
painter.end();
}
overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
require_update_texture_ = false;
}
void PictogramObject::update(float wall_dt, float ros_dt)
{
if (text_.empty()) {
// not yet setted
return;
}
// update position and orientation
if (!context_) {
return;
}
updatePose(wall_dt);
if (!need_to_update_) {
return;
}
need_to_update_ = false;
ScopedPixelBuffer buffer = texture_object_->getBuffer();
QColor transparent(255, 255, 255, 0);
QImage Hud = buffer.getQImage(128, 128, transparent); // should change according to size
QPainter painter( &Hud );
painter.setRenderHint(QPainter::Antialiasing, true);
QColor foreground = rviz::ogreToQt(color_);
painter.setPen(QPen(foreground, 5, Qt::SolidLine));
if (isCharacterSupported(text_)) {
QFont font = getFont(text_);
QString pictogram_text = lookupPictogramText(text_);
if (isEntypo(text_)) {
font.setPointSize(100);
}
else if (isFontAwesome(text_)) {
font.setPointSize(45);
}
painter.setFont(font);
painter.drawText(0, 0, 128, 128,
Qt::AlignHCenter | Qt::AlignVCenter,
pictogram_text);
painter.end();
}
else {
ROS_WARN("%s is not supported", text_.c_str());
}
}
void OverlayTextDisplay::update(float wall_dt, float ros_dt)
{
if (!require_update_texture_) {
return;
}
if (!isEnabled()) {
return;
}
if (!overlay_) {
return;
}
overlay_->updateTextureSize(texture_width_, texture_height_);
{
ScopedPixelBuffer buffer = overlay_->getBuffer();
QImage Hud = buffer.getQImage(*overlay_, bg_color_);
QPainter painter( &Hud );
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setPen(QPen(fg_color_, line_width_ || 1, Qt::SolidLine));
uint16_t w = overlay_->getTextureWidth();
uint16_t h = overlay_->getTextureHeight();
// font
if (text_size_ != 0) {
//QFont font = painter.font();
QFont font(font_.length() > 0 ? font_.c_str(): "Arial");
font.setPointSize(text_size_);
font.setBold(true);
painter.setFont(font);
}
if (text_.length() > 0) {
//painter.drawText(0, 0, w, h, Qt::TextWordWrap | Qt::AlignLeft,
painter.drawText(0, 0, w, h,
Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
text_.c_str());
}
painter.end();
}
overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
require_update_texture_ = false;
}
void PieChartDisplay::drawPlot(double val)
{
QColor fg_color(fg_color_);
if (auto_color_change_) {
double r
= std::min(1.0, fabs((val - min_value_) / (max_value_ - min_value_)));
fg_color.setRed((max_color_.red() - fg_color_.red()) * r
+ fg_color_.red());
fg_color.setGreen((max_color_.green() - fg_color_.green()) * r
+ fg_color_.green());
fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r
+ fg_color_.blue());
}
QColor fg_color2(fg_color);
QColor bg_color(bg_color_);
QColor text_color(text_color_);
fg_color.setAlpha(fg_alpha_);
fg_color2.setAlpha(fg_alpha2_);
bg_color.setAlpha(bg_alpha_);
text_color.setAlpha(text_alpha_);
int width = overlay_->getTextureWidth();
int height = overlay_->getTextureHeight();
{
ScopedPixelBuffer buffer = overlay_->getBuffer();
QImage Hud = buffer.getQImage(*overlay_, bg_color);
QPainter painter( &Hud );
painter.setRenderHint(QPainter::Antialiasing, true);
const int outer_line_width = 5;
const int value_line_width = 10;
const int value_indicator_line_width = 2;
const int value_padding = 5;
const int value_aabb_offset
= outer_line_width + value_padding + value_line_width / 2;
painter.setPen(QPen(fg_color, outer_line_width, Qt::SolidLine));
painter.drawEllipse(outer_line_width / 2, outer_line_width / 2,
width - outer_line_width ,
height - outer_line_width - caption_offset_);
painter.setPen(QPen(fg_color2, value_indicator_line_width, Qt::SolidLine));
painter.drawEllipse(value_aabb_offset, value_aabb_offset,
width - value_aabb_offset * 2,
height - value_aabb_offset * 2 - caption_offset_);
const double ratio = (val - min_value_) / (max_value_ - min_value_);
const double ratio_angle = ratio * 360.0;
const double start_angle_offset = -90;
painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
painter.drawArc(QRectF(value_aabb_offset, value_aabb_offset,
width - value_aabb_offset * 2,
height - value_aabb_offset * 2 - caption_offset_),
start_angle_offset * 16 ,
ratio_angle * 16);
QFont font = painter.font();
font.setPointSize(text_size_);
font.setBold(true);
painter.setFont(font);
painter.setPen(QPen(text_color, value_line_width, Qt::SolidLine));
std::ostringstream s;
s << std::fixed << std::setprecision(2) << val;
painter.drawText(0, 0, width, height - caption_offset_,
Qt::AlignCenter | Qt::AlignVCenter,
s.str().c_str());
// caption
if (show_caption_) {
painter.drawText(0, height - caption_offset_, width, caption_offset_,
Qt::AlignCenter | Qt::AlignVCenter,
getName());
}
// done
painter.end();
// Unlock the pixel buffer
}
}
void Plotter2DDisplay::drawPlot()
{
QColor fg_color(fg_color_);
QColor bg_color(bg_color_);
fg_color.setAlpha(fg_alpha_);
bg_color.setAlpha(bg_alpha_);
if (auto_color_change_) {
double r
= std::min(std::max((buffer_[buffer_.size() - 1] - min_value_) / (max_value_ - min_value_),
0.0), 1.0);
if (r > 0.3) {
double r2 = (r - 0.3) / 0.7;
fg_color.setRed((max_color_.red() - fg_color_.red()) * r2
+ fg_color_.red());
fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2
+ fg_color_.green());
fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2
+ fg_color_.blue());
}
}
{
ScopedPixelBuffer buffer = overlay_->getBuffer();
QImage Hud = buffer.getQImage(*overlay_);
// initilize by the background color
for (int i = 0; i < overlay_->getTextureWidth(); i++) {
for (int j = 0; j < overlay_->getTextureHeight(); j++) {
Hud.setPixel(i, j, bg_color.rgba());
}
}
// paste in HUD speedometer. I resize the image and offset it by 8 pixels from
// the bottom left edge of the render window
QPainter painter( &Hud );
painter.setRenderHint(QPainter::Antialiasing, true);
painter.setPen(QPen(fg_color, line_width_, Qt::SolidLine));
uint16_t w = overlay_->getTextureWidth();
uint16_t h = overlay_->getTextureHeight() - caption_offset_;
double margined_max_value = max_value_ + (max_value_ - min_value_) / 2;
double margined_min_value = min_value_ - (max_value_ - min_value_) / 2;
for (size_t i = 1; i < buffer_length_; i++) {
double v_prev = (margined_max_value - buffer_[i - 1]) / (margined_max_value - margined_min_value);
double v = (margined_max_value - buffer_[i]) / (margined_max_value - margined_min_value);
double u_prev = (i - 1) / (float)buffer_length_;
double u = i / (float)buffer_length_;
// chop within 0 ~ 1
v_prev = std::max(std::min(v_prev, 1.0), 0.0);
u_prev = std::max(std::min(u_prev, 1.0), 0.0);
v = std::max(std::min(v, 1.0), 0.0);
u = std::max(std::min(u, 1.0), 0.0);
uint16_t x_prev = (int)(u_prev * w);
uint16_t x = (int)(u * w);
uint16_t y_prev = (int)(v_prev * h);
uint16_t y = (int)(v * h);
painter.drawLine(x_prev, y_prev, x, y);
}
// draw border
if (show_border_) {
painter.drawLine(0, 0, 0, h);
painter.drawLine(0, h, w, h);
painter.drawLine(w, h, w, 0);
painter.drawLine(w, 0, 0, 0);
}
// draw caption
if (show_caption_) {
QFont font = painter.font();
font.setPointSize(text_size_);
font.setBold(true);
painter.setFont(font);
painter.drawText(0, h, w, caption_offset_,
Qt::AlignCenter | Qt::AlignVCenter,
getName());
}
if (show_value_) {
QFont font = painter.font();
font.setPointSize(w / 4);
font.setBold(true);
painter.setFont(font);
std::ostringstream ss;
ss << std::fixed << std::setprecision(2) << buffer_[buffer_.size() - 1];
painter.drawText(0, 0, w, h,
Qt::AlignCenter | Qt::AlignVCenter,
ss.str().c_str());
}
// done
painter.end();
}
}