marker_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
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 Southwest Research Institute® (SwRI®) 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 
31 
33 
35 
36 #include <boost/algorithm/string.hpp>
37 
38 // Declare plugin
41 
42 namespace mapviz_plugins
43 {
44 #define IS_INSTANCE(msg, type) \
45  (msg->getDataType() == ros::message_traits::datatype<type>())
46 
47  MarkerPlugin::MarkerPlugin() :
48  config_widget_(new QWidget()),
49  connected_(false)
50  {
51  ui_.setupUi(config_widget_);
52 
53  // Set background white
54  QPalette p(config_widget_->palette());
55  p.setColor(QPalette::Background, Qt::white);
56  config_widget_->setPalette(p);
57 
58  // Set status text red
59  QPalette p3(ui_.status->palette());
60  p3.setColor(QPalette::Text, Qt::red);
61  ui_.status->setPalette(p3);
62 
63  QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic()));
64  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited()));
65  QObject::connect(ui_.clear, SIGNAL(clicked()), this, SLOT(ClearHistory()));
66 
67  startTimer(1000);
68  }
69 
71  {
72  }
73 
75  {
76  ROS_DEBUG("MarkerPlugin::ClearHistory()");
77  markers_.clear();
78  marker_visible_.clear();
79  ui_.nsList->clear();
80  }
81 
83  {
85  "visualization_msgs/Marker",
86  "visualization_msgs/MarkerArray");
87 
88  if (topic.name.empty())
89  {
90  return;
91  }
92 
93  ui_.topic->setText(QString::fromStdString(topic.name));
94  TopicEdited();
95  }
96 
98  {
99  std::string topic = ui_.topic->text().trimmed().toStdString();
100  if (topic != topic_)
101  {
102  initialized_ = false;
103  markers_.clear();
104  marker_visible_.clear();
105  ui_.nsList->clear();
106  has_message_ = false;
107  PrintWarning("No messages received.");
108 
110  connected_ = false;
111 
112  topic_ = topic;
113  if (!topic.empty())
114  {
116  topic_, 100, &MarkerPlugin::handleMessage, this);
117 
118  ROS_INFO("Subscribing to %s", topic_.c_str());
119  }
120  }
121  }
122 
124  {
125  connected_ = true;
126  if (IS_INSTANCE(msg, visualization_msgs::Marker))
127  {
128  handleMarker(*(msg->instantiate<visualization_msgs::Marker>()));
129  }
130  else if (IS_INSTANCE(msg, visualization_msgs::MarkerArray))
131  {
132  handleMarkerArray(*(msg->instantiate<visualization_msgs::MarkerArray>()));
133  }
134  else
135  {
136  PrintError("Unknown message type: " + msg->getDataType());
137  }
138  }
139 
140  void MarkerPlugin::handleMarker(const visualization_msgs::Marker &marker)
141  {
142  if (marker.type == visualization_msgs::Marker::ARROW &&
143  marker.points.size() == 1)
144  {
145  // Arrow markers must have either 0 or >1 points; exactly one point is
146  // invalid. If we get one with 1 point, assume it's corrupt and ignore it.
147  return;
148  }
149  if (!has_message_)
150  {
151  initialized_ = true;
152  has_message_ = true;
153  }
154 
155  // Note that unlike some plugins, this one does not store nor rely on the
156  // source_frame_ member variable. This one can potentially store many
157  // messages with different source frames, so we need to store and transform
158  // them individually.
159 
160  if (marker.action == visualization_msgs::Marker::ADD)
161  {
162  MarkerData& markerData = markers_[std::make_pair(marker.ns, marker.id)];
163  markerData.points.clear(); // clear marker points
164  markerData.text.clear(); // clear marker text
165  markerData.stamp = marker.header.stamp;
166  markerData.display_type = marker.type;
167  markerData.color = {marker.color.r, marker.color.g, marker.color.b, marker.color.a};
168  markerData.scale_x = static_cast<float>(marker.scale.x);
169  markerData.scale_y = static_cast<float>(marker.scale.y);
170  markerData.scale_z = static_cast<float>(marker.scale.z);
171  markerData.transformed = true;
172  markerData.source_frame = marker.header.frame_id;
173 
174  if (marker_visible_.emplace(marker.ns, true).second)
175  {
176  QString name_string(marker.ns.c_str());
177  auto* item = new QListWidgetItem(name_string, ui_.nsList);
178  item->setFlags(item->flags() | Qt::ItemIsUserCheckable);
179  item->setCheckState(Qt::Unchecked);
180  item->setCheckState(Qt::Checked);
181  //item->setData(Qt::StatusTipRole, layer_string);
182  }
183 
184 
185  // Since orientation was not implemented, many markers publish
186  // invalid all-zero orientations, so we need to check for this
187  // and provide a default identity transform.
188  tf::Quaternion orientation(0.0, 0.0, 0.0, 1.0);
189  if (marker.pose.orientation.x ||
190  marker.pose.orientation.y ||
191  marker.pose.orientation.z ||
192  marker.pose.orientation.w)
193  {
194  orientation = tf::Quaternion(marker.pose.orientation.x,
195  marker.pose.orientation.y,
196  marker.pose.orientation.z,
197  marker.pose.orientation.w);
198  }
199 
202  orientation,
203  tf::Vector3(marker.pose.position.x,
204  marker.pose.position.y,
205  marker.pose.position.z)));
206 
207  markerData.points.clear();
208  markerData.text = std::string();
209 
211  if (!GetTransform(markerData.source_frame, marker.header.stamp, transform))
212  {
213  markerData.transformed = false;
214  PrintError("No transform between " + markerData.source_frame + " and " + target_frame_);
215  }
216 
217  // Handle lifetime parameter
218  ros::Duration lifetime = marker.lifetime;
219  if (lifetime.isZero())
220  {
221  markerData.expire_time = ros::TIME_MAX;
222  }
223  else
224  {
225  // Temporarily add 5 seconds to fix some existing markers.
226  markerData.expire_time = ros::Time::now() + lifetime + ros::Duration(5);
227  }
228 
229  if (markerData.display_type == visualization_msgs::Marker::ARROW)
230  {
231  StampedPoint point;
232  point.color = markerData.color;
233  point.orientation = orientation;
234 
235  if (marker.points.empty())
236  {
237  // If the "points" array is empty, we'll use the pose as the base of
238  // the arrow and scale its size based on the scale_x value.
239  point.point = markerData.local_transform * tf::Point(0.0, 0.0, 0.0);
240  point.arrow_point = markerData.local_transform * tf::Point(1.0, 0.0, 0.0);
241  }
242  else
243  {
244  // Otherwise the "points" array should have exactly two values, the
245  // start and end of the arrow.
246  point.point = markerData.local_transform * tf::Point(marker.points[0].x, marker.points[0].y, marker.points[0].z);
247  point.arrow_point = markerData.local_transform * tf::Point(marker.points[1].x, marker.points[1].y, marker.points[1].z);
248  }
249 
250  markerData.points.push_back(point);
251 
252  if (!marker.points.empty())
253  {
254  // The point we just pushed back has both the start and end of the
255  // arrow, so the point we're pushing here is useless; we use it later
256  // only to indicate whether the original message had two points or not.
257  markerData.points.push_back(StampedPoint());
258  }
259 
260  transformArrow(markerData, transform);
261  }
262  else if (markerData.display_type == visualization_msgs::Marker::CYLINDER ||
263  markerData.display_type == visualization_msgs::Marker::SPHERE ||
264  markerData.display_type == visualization_msgs::Marker::TEXT_VIEW_FACING)
265  {
266  StampedPoint point;
267  point.point = tf::Point(0.0, 0.0, 0.0);
268  point.transformed_point = transform * (markerData.local_transform * point.point);
269  point.color = markerData.color;
270  markerData.points.push_back(point);
271  markerData.text = marker.text;
272  }
273  else if (markerData.display_type == visualization_msgs::Marker::CUBE)
274  {
275  StampedPoint point;
276  point.color = markerData.color;
277 
278  point.point = tf::Point(marker.scale.x / 2, marker.scale.y / 2, 0.0);
279  point.transformed_point = transform * (markerData.local_transform * point.point);
280  markerData.points.push_back(point);
281 
282  point.point = tf::Point(-marker.scale.x / 2, marker.scale.y / 2, 0.0);
283  point.transformed_point = transform * (markerData.local_transform * point.point);
284  markerData.points.push_back(point);
285 
286  point.point = tf::Point(-marker.scale.x / 2, -marker.scale.y / 2, 0.0);
287  point.transformed_point = transform * (markerData.local_transform * point.point);
288  markerData.points.push_back(point);
289 
290  point.point = tf::Point(marker.scale.x / 2, -marker.scale.y / 2, 0.0);
291  point.transformed_point = transform * (markerData.local_transform * point.point);
292  markerData.points.push_back(point);
293  }
294  else if (markerData.display_type == visualization_msgs::Marker::LINE_STRIP ||
295  markerData.display_type == visualization_msgs::Marker::LINE_LIST ||
296  markerData.display_type == visualization_msgs::Marker::CUBE_LIST ||
297  markerData.display_type == visualization_msgs::Marker::SPHERE_LIST ||
298  markerData.display_type == visualization_msgs::Marker::POINTS ||
299  markerData.display_type == visualization_msgs::Marker::TRIANGLE_LIST)
300  {
301  markerData.points.reserve(marker.points.size());
302  StampedPoint point;
303  for (unsigned int i = 0; i < marker.points.size(); i++)
304  {
305  point.point = tf::Point(marker.points[i].x, marker.points[i].y, marker.points[i].z);
306  point.transformed_point = transform * (markerData.local_transform * point.point);
307 
308  if (i < marker.colors.size())
309  {
310  point.color = {marker.colors[i].r, marker.colors[i].g, marker.colors[i].b, marker.colors[i].a};
311  }
312  else
313  {
314  point.color = markerData.color;
315  }
316 
317  markerData.points.push_back(point);
318  }
319  }
320  else
321  {
322  ROS_WARN_ONCE("Unsupported marker type: %d", markerData.display_type);
323  }
324  }
325  else if (marker.action == visualization_msgs::Marker::DELETE)
326  {
327  markers_.erase(std::make_pair(marker.ns, marker.id));
328  }
329  else if (marker.action == 3) // The DELETEALL enum doesn't exist in Indigo
330  {
331  markers_.clear();
332  }
333  }
334 
343  const swri_transform_util::Transform& transform)
344  {
345  // The first point in the markerData.points array always represents the
346  // base of the arrow.
347  StampedPoint& point = markerData.points.front();
348  tf::Point arrowOffset;
349  if (markerData.points.size() == 1)
350  {
351  // If the markerData only has a single point, that means its "point" is
352  // the base of the arrow and the arrow's angle and length are determined
353  // by the orientation and scale_x.
354  point.transformed_point = transform * (markerData.local_transform * point.point);
355  tf::Transform arrow_tf(tf::Transform(
356  transform.GetOrientation()) * point.orientation,
357  tf::Point(0.0, 0.0, 0.0));
359  arrow_tf * point.arrow_point * markerData.scale_x;
360  arrowOffset = tf::Point(0.25, 0.0, 0.0);
361  }
362  else
363  {
364  // If the markerData has two points, that means that the start and end points
365  // of the arrow were explicitly specified in the original message, so the
366  // length and angle are determined by them.
367  point.transformed_point = transform * point.point;
368  point.transformed_arrow_point = transform * point.arrow_point;
369  // Also, in this mode, scale_y is the diameter of the arrow's head.
370  arrowOffset = tf::Point(0.25 * markerData.scale_y, 0.0, 0.0);
371  }
372 
373  tf::Vector3 pointDiff = point.transformed_arrow_point - point.transformed_point;
374  double angle = std::atan2(pointDiff.getY(), pointDiff.getX());
375 
376  tf::Transform left_tf(tf::createQuaternionFromRPY(0, 0, M_PI*0.75 + angle));
377  tf::Transform right_tf(tf::createQuaternionFromRPY(0, 0, -M_PI*0.75 + angle));
378 
379  point.transformed_arrow_left = point.transformed_arrow_point + left_tf * arrowOffset;
380  point.transformed_arrow_right = point.transformed_arrow_point + right_tf * arrowOffset;
381  }
382 
383  void MarkerPlugin::handleMarkerArray(const visualization_msgs::MarkerArray &markers)
384  {
385  for (unsigned int i = 0; i < markers.markers.size(); i++)
386  {
387  handleMarker(markers.markers[i]);
388  }
389  }
390 
391  void MarkerPlugin::PrintError(const std::string& message)
392  {
393  PrintErrorHelper(ui_.status, message);
394  }
395 
396  void MarkerPlugin::PrintInfo(const std::string& message)
397  {
398  PrintInfoHelper(ui_.status, message);
399  }
400 
401  void MarkerPlugin::PrintWarning(const std::string& message)
402  {
403  PrintWarningHelper(ui_.status, message);
404  }
405 
406  QWidget* MarkerPlugin::GetConfigWidget(QWidget* parent)
407  {
408  config_widget_->setParent(parent);
409 
410  return config_widget_;
411  }
412 
413  bool MarkerPlugin::Initialize(QGLWidget* canvas)
414  {
415  canvas_ = canvas;
416 
417  return true;
418  }
419 
420  void MarkerPlugin::Draw(double x, double y, double scale)
421  {
422  for (size_t i = 0; i < ui_.nsList->count(); i++)
423  {
424  if (ui_.nsList->item(i)->checkState() == Qt::Checked)
425  {
426  marker_visible_[ui_.nsList->item(i)->text().toStdString()] = true;
427  }
428  else
429  {
430  marker_visible_[ui_.nsList->item(i)->text().toStdString()] = false;
431  }
432  }
433 
434  ros::Time now = ros::Time::now();
435 
436  auto markerIter = markers_.begin();
437  while (markerIter != markers_.end())
438  {
439  MarkerData& marker = markerIter->second;
440 
441  if (!(marker.expire_time > now)) {
442  PrintInfo("OK");
443  markerIter = markers_.erase(markerIter);
444  continue;
445  }
446 
447  if (!marker.transformed) {
448  markerIter++;
449  continue;
450  }
451 
452  if (!marker_visible_[markerIter->first.first])
453  {
454  markerIter++;
455  continue;
456  }
457 
458  glColor4f(marker.color.r, marker.color.g, marker.color.b, marker.color.a);
459 
460  if (marker.display_type == visualization_msgs::Marker::ARROW) {
461  if (marker.points.size() == 1) {
462  // If the marker only has one point, use scale_y as the arrow width.
463  glLineWidth(marker.scale_y);
464  }
465  else {
466  // If the marker has both start and end points explicitly specified, use
467  // scale_x as the shaft diameter.
468  glLineWidth(marker.scale_x);
469  }
470  glBegin(GL_LINES);
471 
472  for (const auto &point : marker.points) {
473  glColor4f(point.color.r, point.color.g, point.color.b, point.color.a);
474 
475  glVertex2d(
476  point.transformed_point.getX(),
477  point.transformed_point.getY());
478  glVertex2d(
479  point.transformed_arrow_point.getX(),
480  point.transformed_arrow_point.getY());
481  glVertex2d(
482  point.transformed_arrow_point.getX(),
483  point.transformed_arrow_point.getY());
484  glVertex2d(
485  point.transformed_arrow_left.getX(),
486  point.transformed_arrow_left.getY());
487  glVertex2d(
488  point.transformed_arrow_point.getX(),
489  point.transformed_arrow_point.getY());
490  glVertex2d(
491  point.transformed_arrow_right.getX(),
492  point.transformed_arrow_right.getY());
493  }
494 
495  glEnd();
496  }
497  else if (marker.display_type == visualization_msgs::Marker::LINE_STRIP) {
498  glLineWidth(std::max(1.0f, marker.scale_x));
499  glBegin(GL_LINE_STRIP);
500 
501  for (const auto &point : marker.points) {
502  glColor4f(point.color.r, point.color.g, point.color.b, point.color.a);
503 
504  glVertex2d(
505  point.transformed_point.getX(),
506  point.transformed_point.getY());
507  }
508 
509  glEnd();
510  }
511  else if (marker.display_type == visualization_msgs::Marker::LINE_LIST) {
512  glLineWidth(std::max(1.0f, marker.scale_x));
513  glBegin(GL_LINES);
514 
515  for (const auto &point : marker.points) {
516  glColor4f(point.color.r, point.color.g, point.color.b, point.color.a);
517 
518  glVertex2d(
519  point.transformed_point.getX(),
520  point.transformed_point.getY());
521  }
522 
523  glEnd();
524  }
525  else if (marker.display_type == visualization_msgs::Marker::POINTS) {
526  glPointSize(std::max(1.0f, marker.scale_x));
527  glBegin(GL_POINTS);
528 
529  for (const auto &point : marker.points) {
530  glColor4f(point.color.r, point.color.g, point.color.b, point.color.a);
531 
532  glVertex2d(
533  point.transformed_point.getX(),
534  point.transformed_point.getY());
535  }
536 
537  glEnd();
538  }
539  else if (marker.display_type == visualization_msgs::Marker::TRIANGLE_LIST) {
540  glBegin(GL_TRIANGLES);
541 
542  for (const auto &point : marker.points) {
543  glColor4f(point.color.r, point.color.g, point.color.b, point.color.a);
544 
545  glVertex2d(
546  point.transformed_point.getX(),
547  point.transformed_point.getY());
548  }
549 
550  glEnd();
551  }
552  else if (marker.display_type == visualization_msgs::Marker::CYLINDER ||
553  marker.display_type == visualization_msgs::Marker::SPHERE ||
554  marker.display_type == visualization_msgs::Marker::SPHERE_LIST) {
555  for (const auto &point : marker.points) {
556  glColor4f(point.color.r, point.color.g, point.color.b, point.color.a);
557 
558  glBegin(GL_TRIANGLE_FAN);
559 
560 
561  double marker_x = point.transformed_point.getX();
562  double marker_y = point.transformed_point.getY();
563 
564  glVertex2d(marker_x, marker_y);
565 
566  for (int32_t i = 0; i <= 360; i += 10) {
567  double radians = static_cast<double>(i) * static_cast<double>(swri_math_util::_deg_2_rad);
568  // Spheres may be specified w/ only one scale value
569  if (marker.scale_y == 0.0) {
570  marker.scale_y = marker.scale_x;
571  }
572  glVertex2d(
573  marker_x + std::sin(radians) * marker.scale_x,
574  marker_y + std::cos(radians) * marker.scale_y);
575  }
576 
577  glEnd();
578  }
579  }
580  else if (marker.display_type == visualization_msgs::Marker::CUBE ||
581  marker.display_type == visualization_msgs::Marker::CUBE_LIST) {
582  glBegin(GL_TRIANGLE_FAN);
583  for (const auto &point : marker.points) {
584  glColor4f(point.color.r, point.color.g, point.color.b, point.color.a);
585 
586  glVertex2d(point.transformed_point.getX(), point.transformed_point.getY());
587  }
588  glEnd();
589  }
590 
591  markerIter++;
592  PrintInfo("OK");
593  }
594  }
595 
596  void MarkerPlugin::Paint(QPainter* painter, double x, double y, double scale)
597  {
598  for (size_t i = 0; i < ui_.nsList->count(); i++)
599  {
600  if (ui_.nsList->item(i)->checkState() == Qt::Checked)
601  {
602  marker_visible_[ui_.nsList->item(i)->text().toStdString()] = true;
603  }
604  else
605  {
606  marker_visible_[ui_.nsList->item(i)->text().toStdString()] = false;
607  }
608  }
609 
610  // Most of the marker drawing is done using OpenGL commands, but text labels
611  // are rendered using a QPainter. This is intended primarily as an example
612  // of how the QPainter works.
613  ros::Time now = ros::Time::now();
614 
615  // We don't want the text to be rotated or scaled, but we do want it to be
616  // translated appropriately. So, we save off the current world transform
617  // and reset it; when we actually draw the text, we'll manually translate
618  // it to the right place.
619  QTransform tf = painter->worldTransform();
620  QFont font("Helvetica", 10);
621  painter->setFont(font);
622  painter->save();
623  painter->resetTransform();
624 
625  for (auto markerIter = markers_.begin(); markerIter != markers_.end(); ++markerIter)
626  {
627  MarkerData& marker = markerIter->second;
628 
629  if (marker.display_type != visualization_msgs::Marker::TEXT_VIEW_FACING ||
630  marker.expire_time <= now ||
631  !marker.transformed)
632  {
633  continue;
634  }
635 
636  if (!marker_visible_[markerIter->first.first])
637  {
638  continue;
639  }
640 
641  QPen pen(QBrush(QColor::fromRgbF(marker.color.r, marker.color.g,
642  marker.color.b, marker.color.a)), 1);
643  painter->setPen(pen);
644 
645  StampedPoint& rosPoint = marker.points.front();
646  QPointF point = tf.map(QPointF(rosPoint.transformed_point.x(),
647  rosPoint.transformed_point.y()));
648 
649  auto text = QString::fromStdString(marker.text);
650  // Get bounding rectangle
651  QRectF rect(point, QSizeF(10,10));
652  rect = painter->boundingRect(rect, Qt::AlignLeft | Qt::AlignHCenter, text);
653  painter->drawText(rect, text);
654 
655  PrintInfo("OK");
656  }
657 
658  painter->restore();
659  }
660 
662  {
663  for (auto markerIter = markers_.begin(); markerIter != markers_.end(); ++markerIter)
664  {
665  MarkerData& marker = markerIter->second;
666 
668  if (GetTransform(marker.source_frame, marker.stamp, transform))
669  {
670  marker.transformed = true;
671 
672  if (marker.display_type == visualization_msgs::Marker::ARROW)
673  {
674  // Points for the ARROW marker type are stored a bit differently
675  // than other types, so they have their own special transform case.
676  transformArrow(marker, transform);
677  }
678  else
679  {
680  for (auto &point : marker.points)
681  {
682  point.transformed_point = transform * (marker.local_transform * point.point);
683  }
684  }
685  }
686  else
687  {
688  marker.transformed = false;
689  }
690  }
691  }
692 
693  void MarkerPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
694  {
695  if (node["topic"])
696  {
697  std::string topic;
698  node["topic"] >> topic;
699  ui_.topic->setText(boost::trim_copy(topic).c_str());
700 
701  TopicEdited();
702  }
703  }
704 
705  void MarkerPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
706  {
707  emitter << YAML::Key << "topic" << YAML::Value << boost::trim_copy(ui_.topic->text().toStdString());
708  }
709 
710  void MarkerPlugin::timerEvent(QTimerEvent *event)
711  {
712  bool new_connected = (marker_sub_.getNumPublishers() > 0);
713  if (connected_ && !new_connected)
714  {
716  if (!topic_.empty())
717  {
719  topic_, 100, &MarkerPlugin::handleMessage, this);
720  }
721  }
722  connected_ = new_connected;
723  }
724 }
725 
bool GetTransform(const ros::Time &stamp, swri_transform_util::Transform &transform, bool use_latest_transforms=true)
static ros::master::TopicInfo selectTopic(const std::string &datatype, QWidget *parent=0)
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
static const long double _deg_2_rad
ros::NodeHandle node_
void transform(Route &route, const swri_transform_util::Transform &transform, const std::string &target_frame)
f
std::unordered_map< std::string, bool, MarkerNsHash > marker_visible_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void handleMessage(const topic_tools::ShapeShifter::ConstPtr &msg)
void handleMarker(const visualization_msgs::Marker &marker)
void Draw(double x, double y, double scale)
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)
tf::Quaternion GetOrientation() const
TFSIMD_FORCE_INLINE const tfScalar & getY() const
std::string target_frame_
tf::Vector3 Point
void PrintError(const std::string &message)
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
QWidget * GetConfigWidget(QWidget *parent)
void transformArrow(MarkerData &markerData, const swri_transform_util::Transform &transform)
void LoadConfig(const YAML::Node &node, const std::string &path)
#define IS_INSTANCE(msg, type)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void PrintWarning(const std::string &message)
#define ROS_WARN_ONCE(...)
#define ROS_INFO(...)
uint32_t getNumPublishers() const
TFSIMD_FORCE_INLINE const tfScalar & y() const
ROSTIME_DECL const Time TIME_MAX
void timerEvent(QTimerEvent *)
void Paint(QPainter *painter, double x, double y, double scale)
swri_transform_util::Transform local_transform
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static Time now()
void handleMarkerArray(const visualization_msgs::MarkerArray &markers)
std::unordered_map< MarkerId, MarkerData, MarkerIdHash > markers_
void PrintInfo(const std::string &message)
std::vector< StampedPoint > points
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool Initialize(QGLWidget *canvas)
#define ROS_DEBUG(...)


mapviz_plugins
Author(s): Marc Alban
autogenerated on Fri Mar 19 2021 02:44:32