occupancy_grid_plugin.cpp

Go to the documentation of this file.

// *****************************************************************************
//
// Copyright (c) 2018, Eurecat
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Eurecat nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************
 
#include <GL/glew.h>
 
#include <mapviz_plugins/occupancy_grid_plugin.h>
#include <GL/glut.h>
 
// C++ standard libraries
#include <cstdio>
#include <vector>
 
// QT libraries
#include <QGLWidget>
#include <QPalette>
 
#include <mapviz/select_topic_dialog.h>
 
// Declare plugin
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mapviz_plugins::OccupancyGridPlugin, mapviz::MapvizPlugin)
 
namespace mapviz_plugins
{
  const int CHANNELS = 4;
 
  typedef std::array<uchar, 256*4> Palette;
 
  Palette makeMapPalette()
  {
    Palette palette;
    uchar* palette_ptr = palette.data();
    // Standard gray map palette values
    for( int i = 0; i <= 100; i++ )
    {
      uchar v = 255 - (255 * i) / 100;
      *palette_ptr++ = v; // red
      *palette_ptr++ = v; // green
      *palette_ptr++ = v; // blue
      *palette_ptr++ = 255; // alpha
    }
    // illegal positive values in green
    for( int i = 101; i <= 127; i++ )
    {
      *palette_ptr++ = 0; // red
      *palette_ptr++ = 255; // green
      *palette_ptr++ = 0; // blue
      *palette_ptr++ = 255; // alpha
    }
    // illegal negative (char) values in shades of red/yellow
    for( int i = 128; i <= 254; i++ )
    {
      *palette_ptr++ = 255; // red
      *palette_ptr++ = (255*(i-128))/(254-128); // green
      *palette_ptr++ = 0; // blue
      *palette_ptr++ = 255; // alpha
    }
    // legal -1 value is tasteful blueish greenish grayish color
    *palette_ptr++ = 0x70; // red
    *palette_ptr++ = 0x89; // green
    *palette_ptr++ = 0x86; // blue
    *palette_ptr++ = 160; // alpha
 
    return palette;
  }
 
  Palette makeCostmapPalette()
  {
    Palette palette;
    uchar* palette_ptr = palette.data();
 
    // zero values have alpha=0
    *palette_ptr++ = 0; // red
    *palette_ptr++ = 0; // green
    *palette_ptr++ = 0; // blue
    *palette_ptr++ = 0; // alpha
 
    // Blue to red spectrum for most normal cost values
    for( int i = 1; i <= 98; i++ )
    {
      uchar v = (255 * i) / 100;
      *palette_ptr++ = v; // red
      *palette_ptr++ = 0; // green
      *palette_ptr++ = 255 - v; // blue
      *palette_ptr++ = 255; // alpha
    }
    // inscribed obstacle values (99) in cyan
    *palette_ptr++ = 0; // red
    *palette_ptr++ = 255; // green
    *palette_ptr++ = 255; // blue
    *palette_ptr++ = 255; // alpha
    // lethal obstacle values (100) in purple
    *palette_ptr++ = 255; // red
    *palette_ptr++ = 0; // green
    *palette_ptr++ = 255; // blue
    *palette_ptr++ = 255; // alpha
    // illegal positive values in green
    for( int i = 101; i <= 127; i++ )
    {
      *palette_ptr++ = 0; // red
      *palette_ptr++ = 255; // green
      *palette_ptr++ = 0; // blue
      *palette_ptr++ = 255; // alpha
    }
    // illegal negative (char) values in shades of red/yellow
    for( int i = 128; i <= 254; i++ )
    {
      *palette_ptr++ = 255; // red
      *palette_ptr++ = (255*(i-128))/(254-128); // green
      *palette_ptr++ = 0; // blue
      *palette_ptr++ = 255; // alpha
    }
    // legal -1 value is tasteful blueish greenish grayish color
    *palette_ptr++ = 0x70; // red
    *palette_ptr++ = 0x89; // green
    *palette_ptr++ = 0x86; // blue
    *palette_ptr++ = 160; // alpha
 
    return palette;
  }
 
 
 
  OccupancyGridPlugin::OccupancyGridPlugin() :
    config_widget_(new QWidget()),
    transformed_(false),
    texture_id_(0),
    map_palette_( makeMapPalette() ),
    costmap_palette_( makeCostmapPalette() )
  {
    ui_.setupUi(config_widget_);
 
    // Set background white
    QPalette p(config_widget_->palette());
    p.setColor(QPalette::Background, Qt::white);
    config_widget_->setPalette(p);
 
    // Set status text red
    QPalette p3(ui_.status->palette());
    p3.setColor(QPalette::Text, Qt::red);
    ui_.status->setPalette(p3);
 
    QObject::connect(ui_.select_grid, SIGNAL(clicked()), this, SLOT(SelectTopicGrid()));
 
    QObject::connect(ui_.topic_grid, SIGNAL(textEdited(const QString&)), this, SLOT(TopicGridEdited()));
 
    QObject::connect(this, SIGNAL(TargetFrameChanged(std::string)), this, SLOT(FrameChanged(std::string)));
 
    QObject::connect(ui_.checkbox_update, SIGNAL(toggled(bool)), this, SLOT(upgradeCheckBoxToggled(bool)));
 
    QObject::connect(ui_.color_scheme, SIGNAL(currentTextChanged(const QString &)), this, SLOT(colorSchemeUpdated(const QString &)));
 
    PrintWarning("waiting for first message");
  }
 
  OccupancyGridPlugin::~OccupancyGridPlugin()
  {
    Shutdown();
  }
 
  void OccupancyGridPlugin::Shutdown()
  {
  }
 
  void OccupancyGridPlugin::DrawIcon()
  {
    if (icon_)
    {
      QPixmap icon(16, 16);
      icon.fill(Qt::transparent);
      
      QPainter painter(&icon);
      painter.setRenderHint(QPainter::Antialiasing, true);
      
      QPen pen(Qt::black);
      
      pen.setWidth(2);
      pen.setCapStyle(Qt::SquareCap);
      painter.setPen(pen);
 
      painter.drawLine(2, 2, 14, 2);
      painter.drawLine(2, 2, 2, 14);
      painter.drawLine(14, 2, 14, 14);
      painter.drawLine(2, 14, 14, 14);
      painter.drawLine(8, 2, 8, 14);
      painter.drawLine(2, 8, 14, 8);
      
      icon_->SetPixmap(icon);
    }
  }
 
  void OccupancyGridPlugin::FrameChanged(std::string)
  {
    transformed_ = false;
  }
 
  void OccupancyGridPlugin::SelectTopicGrid()
  {
    ros::master::TopicInfo topic = mapviz::SelectTopicDialog::selectTopic("nav_msgs/OccupancyGrid");
    if (!topic.name.empty())
    {
      QString str = QString::fromStdString(topic.name);
      ui_.topic_grid->setText( str);
      TopicGridEdited();
    }
  }
 
 
  void OccupancyGridPlugin::TopicGridEdited()
  {
    const std::string topic = ui_.topic_grid->text().trimmed().toStdString();
 
    initialized_ = false;
    grid_.reset();
    raw_buffer_.clear();
 
    grid_sub_.shutdown();
    update_sub_.shutdown();
 
    if (!topic.empty())
    {
      grid_sub_   = node_.subscribe(topic, 10, &OccupancyGridPlugin::Callback, this);
      if( ui_.checkbox_update)
      {
        update_sub_ = node_.subscribe(topic+ "_updates", 10, &OccupancyGridPlugin::CallbackUpdate, this);
      }
      ROS_INFO("Subscribing to %s", topic.c_str());
    }
  }
 
  void OccupancyGridPlugin::upgradeCheckBoxToggled(bool)
  {
    const std::string topic = ui_.topic_grid->text().trimmed().toStdString();
    update_sub_.shutdown();
 
    if( ui_.checkbox_update)
    {
      update_sub_ = node_.subscribe(topic+ "_updates", 10, &OccupancyGridPlugin::CallbackUpdate, this);
    }
  }
 
  void OccupancyGridPlugin::colorSchemeUpdated(const QString &)
  {
 
    if( grid_ && raw_buffer_.size()>0)
    {
      const size_t width  = grid_->info.width;
      const size_t height = grid_->info.height;
      const Palette& palette = (ui_.color_scheme->currentText() == "map") ?  map_palette_ : costmap_palette_;
 
      for (size_t row = 0; row < height;  row++)
      {
        for (size_t col = 0; col < width; col++)
        {
          size_t index = (col + row * texture_size_);
          uchar color = raw_buffer_[index];
          memcpy( &color_buffer_[index*CHANNELS], &palette[color*CHANNELS], CHANNELS);
        }
      }
      updateTexture();
    }
  }
 
  void OccupancyGridPlugin::PrintError(const std::string& message)
  {
    PrintErrorHelper(ui_.status, message);
  }
 
  void OccupancyGridPlugin::PrintInfo(const std::string& message)
  {
    PrintInfoHelper(ui_.status, message);
  }
 
  void OccupancyGridPlugin::PrintWarning(const std::string& message)
  {
    PrintWarningHelper(ui_.status, message);
  }
 
  QWidget* OccupancyGridPlugin::GetConfigWidget(QWidget* parent)
  {
    config_widget_->setParent(parent);
 
    return config_widget_;
  }
 
  bool OccupancyGridPlugin::Initialize(QGLWidget* canvas)
  {
    canvas_ = canvas;
    DrawIcon();
    return true;
  }
 
  void OccupancyGridPlugin::updateTexture()
  {
    if (texture_id_ != -1)
    {
      glDeleteTextures(1, &texture_id_);
    }
 
    // Get a new texture id.
    glGenTextures(1, &texture_id_);
 
    // Bind the texture with the correct size and null memory.
    glBindTexture(GL_TEXTURE_2D, texture_id_);
    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
 
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 
    glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
 
    glTexImage2D(
          GL_TEXTURE_2D,
          0,
          GL_RGBA,
          texture_size_,
          texture_size_,
          0,
          GL_RGBA,
          GL_UNSIGNED_BYTE,
          color_buffer_.data());
 
    glBindTexture(GL_TEXTURE_2D, 0);
    glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
 
  }
 
 
  void OccupancyGridPlugin::Callback(const nav_msgs::OccupancyGridConstPtr& msg)
  {
    grid_ = msg;
    const int width  = grid_->info.width;
    const int height = grid_->info.height;
    initialized_ = true;
    source_frame_ = grid_->header.frame_id;
    transformed_ = GetTransform( source_frame_, grid_->header.stamp, transform_);
    if ( !transformed_ )
    {
      PrintError("No transform between " + source_frame_ + " and " + target_frame_);
    }
 
    int32_t max_dimension = std::max(height, width);
 
    texture_size_ = 2;
    while (texture_size_ < max_dimension){
      texture_size_ = texture_size_ << 1;
    }
 
    const Palette& palette = (ui_.color_scheme->currentText() == "map") ?  map_palette_ : costmap_palette_;
 
    raw_buffer_.resize(texture_size_*texture_size_, 0);
    color_buffer_.resize(texture_size_*texture_size_*CHANNELS, 0);
 
    for (size_t row = 0; row < height; row++)
    {
      for (size_t col = 0; col < width; col++)
      {
        size_t index_src = (col + row*width);
        size_t index_dst = (col + row*texture_size_);
        uchar color = static_cast<uchar>( grid_->data[ index_src ] );
        raw_buffer_[index_dst] = color;
        memcpy( &color_buffer_[index_dst*CHANNELS], &palette[color*CHANNELS], CHANNELS);
      }
    }
 
    texture_x_ = static_cast<float>(width) / static_cast<float>(texture_size_);
    texture_y_ = static_cast<float>(height) / static_cast<float>(texture_size_);
 
    updateTexture();
    PrintInfo("Map received");
  }
 
  void OccupancyGridPlugin::CallbackUpdate(const map_msgs::OccupancyGridUpdateConstPtr &msg)
  {
    PrintInfo("Update Received");
 
    if( initialized_ )
    {
      const Palette& palette = (ui_.color_scheme->currentText() == "map") ?  map_palette_ : costmap_palette_;
 
      for (size_t row = 0; row < msg->height; row++)
      {
        for (size_t col = 0; col < msg->width; col++)
        {
          size_t index_src = (col + row * msg->width);
          size_t index_dst = ( (col + msg->x) + (row + msg->y)*texture_size_);
          uchar color = static_cast<uchar>( msg->data[ index_src ] );
          raw_buffer_[index_dst] = color;
          memcpy( &color_buffer_[index_dst*CHANNELS], &palette[color*CHANNELS], CHANNELS);
        }
      }
      updateTexture();
    }
  }
 
  void OccupancyGridPlugin::Draw(double x, double y, double scale)
  {
    glPushMatrix();
 
    if( grid_ && transformed_)
    {
      double resolution = grid_->info.resolution;
      glTranslatef( transform_.GetOrigin().getX(),
                    transform_.GetOrigin().getY(),
                    0.0);
 
      const double RAD_TO_DEG = 180.0 / M_PI;
 
      tfScalar yaw, pitch, roll;
      tf::Matrix3x3 mat( transform_.GetOrientation() );
      mat.getEulerYPR(yaw, pitch, roll);
 
      glRotatef(pitch * RAD_TO_DEG, 0, 1, 0);
      glRotatef(roll  * RAD_TO_DEG, 1, 0, 0);
      glRotatef(yaw   * RAD_TO_DEG, 0, 0, 1);
 
      glTranslatef( grid_->info.origin.position.x,
                    grid_->info.origin.position.y,
                    0.0);
 
      glScalef( resolution, resolution, 1.0);
 
      float width  = static_cast<float>(grid_->info.width);
      float height = static_cast<float>(grid_->info.height);
 
      glEnable(GL_TEXTURE_2D);
      glBindTexture(GL_TEXTURE_2D, texture_id_);
      glBegin(GL_TRIANGLES);
 
      glColor4f(1.0f, 1.0f, 1.0f, ui_.alpha->value() );
 
      glTexCoord2d(0, 0);
      glVertex2d(0, 0);
      glTexCoord2d(texture_x_, 0);
      glVertex2d(width, 0);
      glTexCoord2d(texture_x_, texture_y_);
      glVertex2d(width, height);
 
      glTexCoord2d(0, 0);
      glVertex2d(0, 0);
      glTexCoord2d(texture_x_, texture_y_);
      glVertex2d(width, height);
      glTexCoord2d(0, texture_y_);
      glVertex2d(0, height);
 
      glEnd();
 
      glBindTexture(GL_TEXTURE_2D, 0);
      glDisable(GL_TEXTURE_2D);
    }
    glPopMatrix();
  }
 
  void OccupancyGridPlugin::Transform()
  {
    if( !initialized_ ) return;
    swri_transform_util::Transform transform;
    if ( grid_ )
    {
      if( GetTransform( source_frame_, ros::Time(0), transform) )
      {
        transformed_ = true;
        transform_ = transform;
      }
    }
    if ( !transformed_ )
    {
      PrintError("No transform between " + source_frame_ + " and " + target_frame_);
    }
  }
 
  void OccupancyGridPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
  {
    if (node["topic"])
    {
      std::string topic;
      node["topic"] >> topic;
      ui_.topic_grid->setText(QString::fromStdString(topic));
    }
 
    if (node["update"])
    {
      bool checked;
      node["update"] >> checked;
      ui_.checkbox_update->setChecked( checked );
    }
 
    if (node["alpha"])
    {
      double alpha;
      node["alpha"] >> alpha;
      ui_.alpha->setValue(alpha);
    }
 
    if (node["scheme"])
    {
      std::string scheme;
      node["scheme"] >> scheme;
      int index = ui_.color_scheme->findText(QString::fromStdString(scheme), Qt::MatchExactly);
      if (index >= 0)
      {
        ui_.color_scheme->setCurrentIndex(index);
      }
      colorSchemeUpdated(QString::fromStdString(scheme));
    }
 
    TopicGridEdited();
  }
 
  void OccupancyGridPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
  {
    emitter << YAML::Key << "alpha"  << YAML::Value << ui_.alpha->value();
    emitter << YAML::Key << "topic"  << YAML::Value << ui_.topic_grid->text().toStdString();
    emitter << YAML::Key << "update" << YAML::Value << ui_.checkbox_update->isChecked();
    emitter << YAML::Key << "scheme" << YAML::Value << ui_.color_scheme->currentText().toStdString();
  }
}