occupancy_grid_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2018, Eurecat
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Eurecat nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #include <GL/glew.h>
31 
33 #include <GL/glut.h>
34 
35 // C++ standard libraries
36 #include <cstdio>
37 #include <vector>
38 
39 // QT libraries
40 #include <QGLWidget>
41 #include <QPalette>
42 
44 
45 // Declare plugin
48 
49 namespace mapviz_plugins
50 {
51  const int CHANNELS = 4;
52 
53  typedef std::array<uchar, 256*4> Palette;
54 
55  Palette makeMapPalette()
56  {
57  Palette palette;
58  uchar* palette_ptr = palette.data();
59  // Standard gray map palette values
60  for( int i = 0; i <= 100; i++ )
61  {
62  uchar v = 255 - (255 * i) / 100;
63  *palette_ptr++ = v; // red
64  *palette_ptr++ = v; // green
65  *palette_ptr++ = v; // blue
66  *palette_ptr++ = 255; // alpha
67  }
68  // illegal positive values in green
69  for( int i = 101; i <= 127; i++ )
70  {
71  *palette_ptr++ = 0; // red
72  *palette_ptr++ = 255; // green
73  *palette_ptr++ = 0; // blue
74  *palette_ptr++ = 255; // alpha
75  }
76  // illegal negative (char) values in shades of red/yellow
77  for( int i = 128; i <= 254; i++ )
78  {
79  *palette_ptr++ = 255; // red
80  *palette_ptr++ = (255*(i-128))/(254-128); // green
81  *palette_ptr++ = 0; // blue
82  *palette_ptr++ = 255; // alpha
83  }
84  // legal -1 value is tasteful blueish greenish grayish color
85  *palette_ptr++ = 0x70; // red
86  *palette_ptr++ = 0x89; // green
87  *palette_ptr++ = 0x86; // blue
88  *palette_ptr++ = 160; // alpha
89 
90  return palette;
91  }
92 
94  {
95  Palette palette;
96  uchar* palette_ptr = palette.data();
97 
98  // zero values have alpha=0
99  *palette_ptr++ = 0; // red
100  *palette_ptr++ = 0; // green
101  *palette_ptr++ = 0; // blue
102  *palette_ptr++ = 0; // alpha
103 
104  // Blue to red spectrum for most normal cost values
105  for( int i = 1; i <= 98; i++ )
106  {
107  uchar v = (255 * i) / 100;
108  *palette_ptr++ = v; // red
109  *palette_ptr++ = 0; // green
110  *palette_ptr++ = 255 - v; // blue
111  *palette_ptr++ = 255; // alpha
112  }
113  // inscribed obstacle values (99) in cyan
114  *palette_ptr++ = 0; // red
115  *palette_ptr++ = 255; // green
116  *palette_ptr++ = 255; // blue
117  *palette_ptr++ = 255; // alpha
118  // lethal obstacle values (100) in purple
119  *palette_ptr++ = 255; // red
120  *palette_ptr++ = 0; // green
121  *palette_ptr++ = 255; // blue
122  *palette_ptr++ = 255; // alpha
123  // illegal positive values in green
124  for( int i = 101; i <= 127; i++ )
125  {
126  *palette_ptr++ = 0; // red
127  *palette_ptr++ = 255; // green
128  *palette_ptr++ = 0; // blue
129  *palette_ptr++ = 255; // alpha
130  }
131  // illegal negative (char) values in shades of red/yellow
132  for( int i = 128; i <= 254; i++ )
133  {
134  *palette_ptr++ = 255; // red
135  *palette_ptr++ = (255*(i-128))/(254-128); // green
136  *palette_ptr++ = 0; // blue
137  *palette_ptr++ = 255; // alpha
138  }
139  // legal -1 value is tasteful blueish greenish grayish color
140  *palette_ptr++ = 0x70; // red
141  *palette_ptr++ = 0x89; // green
142  *palette_ptr++ = 0x86; // blue
143  *palette_ptr++ = 160; // alpha
144 
145  return palette;
146  }
147 
148 
149 
151  config_widget_(new QWidget()),
152  transformed_(false),
153  texture_id_(0),
154  map_palette_( makeMapPalette() ),
155  costmap_palette_( makeCostmapPalette() )
156  {
157  ui_.setupUi(config_widget_);
158 
159  // Set background white
160  QPalette p(config_widget_->palette());
161  p.setColor(QPalette::Background, Qt::white);
162  config_widget_->setPalette(p);
163 
164  // Set status text red
165  QPalette p3(ui_.status->palette());
166  p3.setColor(QPalette::Text, Qt::red);
167  ui_.status->setPalette(p3);
168 
169  QObject::connect(ui_.select_grid, SIGNAL(clicked()), this, SLOT(SelectTopicGrid()));
170 
171  QObject::connect(ui_.topic_grid, SIGNAL(textEdited(const QString&)), this, SLOT(TopicGridEdited()));
172 
173  QObject::connect(this, SIGNAL(TargetFrameChanged(std::string)), this, SLOT(FrameChanged(std::string)));
174 
175  QObject::connect(ui_.checkbox_update, SIGNAL(toggled(bool)), this, SLOT(upgradeCheckBoxToggled(bool)));
176 
177  QObject::connect(ui_.color_scheme, SIGNAL(currentTextChanged(const QString &)), this, SLOT(colorSchemeUpdated(const QString &)));
178 
179  PrintWarning("waiting for first message");
180  }
181 
183  {
184  Shutdown();
185  }
186 
188  {
189  }
190 
192  {
193  if (icon_)
194  {
195  QPixmap icon(16, 16);
196  icon.fill(Qt::transparent);
197 
198  QPainter painter(&icon);
199  painter.setRenderHint(QPainter::Antialiasing, true);
200 
201  QPen pen(Qt::black);
202 
203  pen.setWidth(2);
204  pen.setCapStyle(Qt::SquareCap);
205  painter.setPen(pen);
206 
207  painter.drawLine(2, 2, 14, 2);
208  painter.drawLine(2, 2, 2, 14);
209  painter.drawLine(14, 2, 14, 14);
210  painter.drawLine(2, 14, 14, 14);
211  painter.drawLine(8, 2, 8, 14);
212  painter.drawLine(2, 8, 14, 8);
213 
214  icon_->SetPixmap(icon);
215  }
216  }
217 
219  {
220  transformed_ = false;
221  }
222 
224  {
225  ros::master::TopicInfo topic = mapviz::SelectTopicDialog::selectTopic("nav_msgs/OccupancyGrid");
226  if (!topic.name.empty())
227  {
228  QString str = QString::fromStdString(topic.name);
229  ui_.topic_grid->setText( str);
230  TopicGridEdited();
231  }
232  }
233 
234 
236  {
237  const std::string topic = ui_.topic_grid->text().trimmed().toStdString();
238 
239  initialized_ = false;
240  grid_.reset();
241  raw_buffer_.clear();
242 
245 
246  if (!topic.empty())
247  {
249  if( ui_.checkbox_update)
250  {
251  update_sub_ = node_.subscribe(topic+ "_updates", 10, &OccupancyGridPlugin::CallbackUpdate, this);
252  }
253  ROS_INFO("Subscribing to %s", topic.c_str());
254  }
255  }
256 
258  {
259  const std::string topic = ui_.topic_grid->text().trimmed().toStdString();
261 
262  if( ui_.checkbox_update)
263  {
264  update_sub_ = node_.subscribe(topic+ "_updates", 10, &OccupancyGridPlugin::CallbackUpdate, this);
265  }
266  }
267 
269  {
270 
271  if( grid_ && raw_buffer_.size()>0)
272  {
273  const size_t width = grid_->info.width;
274  const size_t height = grid_->info.height;
275  const Palette& palette = (ui_.color_scheme->currentText() == "map") ? map_palette_ : costmap_palette_;
276 
277  for (size_t row = 0; row < height; row++)
278  {
279  for (size_t col = 0; col < width; col++)
280  {
281  size_t index = (col + row * texture_size_);
282  uchar color = raw_buffer_[index];
283  memcpy( &color_buffer_[index*CHANNELS], &palette[color*CHANNELS], CHANNELS);
284  }
285  }
286  updateTexture();
287  }
288  }
289 
290  void OccupancyGridPlugin::PrintError(const std::string& message)
291  {
292  PrintErrorHelper(ui_.status, message);
293  }
294 
295  void OccupancyGridPlugin::PrintInfo(const std::string& message)
296  {
297  PrintInfoHelper(ui_.status, message);
298  }
299 
300  void OccupancyGridPlugin::PrintWarning(const std::string& message)
301  {
302  PrintWarningHelper(ui_.status, message);
303  }
304 
305  QWidget* OccupancyGridPlugin::GetConfigWidget(QWidget* parent)
306  {
307  config_widget_->setParent(parent);
308 
309  return config_widget_;
310  }
311 
312  bool OccupancyGridPlugin::Initialize(QGLWidget* canvas)
313  {
314  canvas_ = canvas;
315  DrawIcon();
316  return true;
317  }
318 
320  {
321  if (texture_id_ != -1)
322  {
323  glDeleteTextures(1, &texture_id_);
324  }
325 
326  // Get a new texture id.
327  glGenTextures(1, &texture_id_);
328 
329  // Bind the texture with the correct size and null memory.
330  glBindTexture(GL_TEXTURE_2D, texture_id_);
331  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
332 
333  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
334  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
335 
336  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
337  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
338 
339  glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
340 
341  glTexImage2D(
342  GL_TEXTURE_2D,
343  0,
344  GL_RGBA,
347  0,
348  GL_RGBA,
349  GL_UNSIGNED_BYTE,
350  color_buffer_.data());
351 
352  glBindTexture(GL_TEXTURE_2D, 0);
353  glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
354 
355  }
356 
357 
358  void OccupancyGridPlugin::Callback(const nav_msgs::OccupancyGridConstPtr& msg)
359  {
360  grid_ = msg;
361  const int width = grid_->info.width;
362  const int height = grid_->info.height;
363  initialized_ = true;
364  source_frame_ = grid_->header.frame_id;
366  if ( !transformed_ )
367  {
368  PrintError("No transform between " + source_frame_ + " and " + target_frame_);
369  }
370 
371  int32_t max_dimension = std::max(height, width);
372 
373  texture_size_ = 2;
374  while (texture_size_ < max_dimension){
376  }
377 
378  const Palette& palette = (ui_.color_scheme->currentText() == "map") ? map_palette_ : costmap_palette_;
379 
381  color_buffer_.resize(texture_size_*texture_size_*CHANNELS, 0);
382 
383  for (size_t row = 0; row < height; row++)
384  {
385  for (size_t col = 0; col < width; col++)
386  {
387  size_t index_src = (col + row*width);
388  size_t index_dst = (col + row*texture_size_);
389  uchar color = static_cast<uchar>( grid_->data[ index_src ] );
390  raw_buffer_[index_dst] = color;
391  memcpy( &color_buffer_[index_dst*CHANNELS], &palette[color*CHANNELS], CHANNELS);
392  }
393  }
394 
395  texture_x_ = static_cast<float>(width) / static_cast<float>(texture_size_);
396  texture_y_ = static_cast<float>(height) / static_cast<float>(texture_size_);
397 
398  updateTexture();
399  PrintInfo("Map received");
400  }
401 
402  void OccupancyGridPlugin::CallbackUpdate(const map_msgs::OccupancyGridUpdateConstPtr &msg)
403  {
404  PrintInfo("Update Received");
405 
406  if( initialized_ )
407  {
408  const Palette& palette = (ui_.color_scheme->currentText() == "map") ? map_palette_ : costmap_palette_;
409 
410  for (size_t row = 0; row < msg->height; row++)
411  {
412  for (size_t col = 0; col < msg->width; col++)
413  {
414  size_t index_src = (col + row * msg->width);
415  size_t index_dst = ( (col + msg->x) + (row + msg->y)*texture_size_);
416  uchar color = static_cast<uchar>( msg->data[ index_src ] );
417  raw_buffer_[index_dst] = color;
418  memcpy( &color_buffer_[index_dst*CHANNELS], &palette[color*CHANNELS], CHANNELS);
419  }
420  }
421  updateTexture();
422  }
423  }
424 
425  void OccupancyGridPlugin::Draw(double x, double y, double scale)
426  {
427  glPushMatrix();
428 
429  if( grid_ && transformed_)
430  {
431  double resolution = grid_->info.resolution;
432  glTranslatef( transform_.GetOrigin().getX(),
433  transform_.GetOrigin().getY(),
434  0.0);
435 
436  const double RAD_TO_DEG = 180.0 / M_PI;
437 
438  tfScalar yaw, pitch, roll;
440  mat.getEulerYPR(yaw, pitch, roll);
441 
442  glRotatef(pitch * RAD_TO_DEG, 0, 1, 0);
443  glRotatef(roll * RAD_TO_DEG, 1, 0, 0);
444  glRotatef(yaw * RAD_TO_DEG, 0, 0, 1);
445 
446  glTranslatef( grid_->info.origin.position.x,
447  grid_->info.origin.position.y,
448  0.0);
449 
450  glScalef( resolution, resolution, 1.0);
451 
452  float width = static_cast<float>(grid_->info.width);
453  float height = static_cast<float>(grid_->info.height);
454 
455  glEnable(GL_TEXTURE_2D);
456  glBindTexture(GL_TEXTURE_2D, texture_id_);
457  glBegin(GL_TRIANGLES);
458 
459  glColor4f(1.0f, 1.0f, 1.0f, ui_.alpha->value() );
460 
461  glTexCoord2d(0, 0);
462  glVertex2d(0, 0);
463  glTexCoord2d(texture_x_, 0);
464  glVertex2d(width, 0);
465  glTexCoord2d(texture_x_, texture_y_);
466  glVertex2d(width, height);
467 
468  glTexCoord2d(0, 0);
469  glVertex2d(0, 0);
470  glTexCoord2d(texture_x_, texture_y_);
471  glVertex2d(width, height);
472  glTexCoord2d(0, texture_y_);
473  glVertex2d(0, height);
474 
475  glEnd();
476 
477  glBindTexture(GL_TEXTURE_2D, 0);
478  glDisable(GL_TEXTURE_2D);
479  }
480  glPopMatrix();
481  }
482 
484  {
485  if( !initialized_ ) return;
487  if ( grid_ )
488  {
489  if( GetTransform( source_frame_, ros::Time(0), transform) )
490  {
491  transformed_ = true;
492  transform_ = transform;
493  }
494  }
495  if ( !transformed_ )
496  {
497  PrintError("No transform between " + source_frame_ + " and " + target_frame_);
498  }
499  }
500 
501  void OccupancyGridPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
502  {
503  if (node["topic"])
504  {
505  std::string topic;
506  node["topic"] >> topic;
507  ui_.topic_grid->setText(QString::fromStdString(topic));
508  }
509 
510  if (node["update"])
511  {
512  bool checked;
513  node["update"] >> checked;
514  ui_.checkbox_update->setChecked( checked );
515  }
516 
517  if (node["alpha"])
518  {
519  double alpha;
520  node["alpha"] >> alpha;
521  ui_.alpha->setValue(alpha);
522  }
523 
524  if (node["scheme"])
525  {
526  std::string scheme;
527  node["scheme"] >> scheme;
528  int index = ui_.color_scheme->findText(QString::fromStdString(scheme), Qt::MatchExactly);
529  if (index >= 0)
530  {
531  ui_.color_scheme->setCurrentIndex(index);
532  }
533  colorSchemeUpdated(QString::fromStdString(scheme));
534  }
535 
536  TopicGridEdited();
537  }
538 
539  void OccupancyGridPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
540  {
541  emitter << YAML::Key << "alpha" << YAML::Value << ui_.alpha->value();
542  emitter << YAML::Key << "topic" << YAML::Value << ui_.topic_grid->text().toStdString();
543  emitter << YAML::Key << "update" << YAML::Value << ui_.checkbox_update->isChecked();
544  emitter << YAML::Key << "scheme" << YAML::Value << ui_.color_scheme->currentText().toStdString();
545  }
546 }
547 
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform, bool use_latest_transforms=true)
void PrintWarning(const std::string &message)
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
IconWidget * icon_
ros::NodeHandle node_
tf::Vector3 GetOrigin() const
f
double tfScalar
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
nav_msgs::OccupancyGridConstPtr grid_
std::string target_frame_
void PrintError(const std::string &message)
void getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) const
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
std::string source_frame_
void SetPixmap(QPixmap pixmap)
swri_transform_util::Transform transform_
std::array< uchar, 256 *4 > Palette
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
void LoadConfig(const YAML::Node &node, const std::string &path)
void Callback(const nav_msgs::OccupancyGridConstPtr &msg)
#define ROS_INFO(...)
tf::Quaternion GetOrientation() const
void PrintInfo(const std::string &message)
void TargetFrameChanged(const std::string &target_frame)
QWidget * GetConfigWidget(QWidget *parent)
void Draw(double x, double y, double scale)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void CallbackUpdate(const map_msgs::OccupancyGridUpdateConstPtr &msg)


mapviz_plugins
Author(s): Marc Alban
autogenerated on Fri Dec 16 2022 03:59:33