marker_display.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
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 the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * 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 THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <sstream>
31 
32 #include <tf/transform_listener.h>
33 
36 #include "rviz/display_context.h"
37 #include "rviz/frame_manager.h"
45 
47 
48 namespace rviz
49 {
51 
52 MarkerDisplay::MarkerDisplay() : Display(), tf_filter_(nullptr)
53 {
55  "Marker Topic", "visualization_marker",
56  QString::fromStdString(ros::message_traits::datatype<visualization_msgs::Marker>()),
57  "visualization_msgs::Marker topic to subscribe to. <topic>_array will also"
58  " automatically be subscribed with type visualization_msgs::MarkerArray.",
59  this, SLOT(updateTopic()));
60 
62  new IntProperty("Queue Size", 100,
63  "Advanced: set the size of the incoming Marker message queue. Increasing this is"
64  " useful if your incoming TF data is delayed significantly from your Marker data, "
65  "but it can greatly increase memory usage if the messages are big.",
66  this, SLOT(updateQueueSize()));
68 
69  namespaces_category_ = new Property("Namespaces", QVariant(), "", this);
70 }
71 
73 {
74 // TODO(wjwwood): remove this and use tf2 interface instead
75 #ifndef _WIN32
76 #pragma GCC diagnostic push
77 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
78 #endif
79 
80  auto tf_client = context_->getTFClient();
81 
82 #ifndef _WIN32
83 #pragma GCC diagnostic pop
84 #endif
86  *tf_client, fixed_frame_.toStdString(), queue_size_property_->getInt(), update_nh_);
87 
91 
93 }
94 
96 {
97  if (initialized())
98  {
99  unsubscribe();
100 
101  clearMarkers();
102 
103  delete tf_filter_;
104  }
105 }
106 
107 void MarkerDisplay::load(const Config& config)
108 {
109  Display::load(config);
110 
111  Config c = config.mapGetChild("Namespaces");
112  for (Config::MapIterator iter = c.mapIterator(); iter.isValid(); iter.advance())
113  {
114  QString key = iter.currentKey();
115  const Config& child = iter.currentChild();
116  namespace_config_enabled_state_[key] = child.getValue().toBool();
117  }
118 }
119 
121 {
122  markers_.clear();
123  markers_with_expiration_.clear();
124  frame_locked_markers_.clear();
125  if (tf_filter_) // also clear messages in pipeline
126  tf_filter_->clear();
128  namespaces_.clear();
129 }
130 
132 {
133  subscribe();
134 }
135 
137 {
138  unsubscribe();
139  reset();
140 }
141 
143 {
145  subscribe();
146 }
147 
149 {
150  onDisable();
151  onEnable();
152 }
153 
155 {
156  if (!isEnabled())
157  {
158  return;
159  }
160 
161  std::string marker_topic = marker_topic_property_->getTopicStd();
162  if (!marker_topic.empty())
163  {
165  sub_.unsubscribe();
166 
167  try
168  {
170  array_sub_ = update_nh_.subscribe(marker_topic + "_array", queue_size_property_->getInt(),
172  setStatus(StatusProperty::Ok, "Topic", "OK");
173  }
174  catch (ros::Exception& e)
175  {
176  setStatus(StatusProperty::Error, "Topic", QString("Error subscribing: ") + e.what());
177  }
178  }
179 }
180 
182 {
183  sub_.unsubscribe();
185 }
186 
188 {
189  deleteMarkerStatus(id);
191 }
192 
194 {
195  M_IDToMarker::iterator it = markers_.find(id);
196  if (it != markers_.end())
197  {
198  markers_with_expiration_.erase(it->second);
199  frame_locked_markers_.erase(it->second);
200  markers_.erase(it);
201  }
202 }
203 
204 void MarkerDisplay::deleteMarkersInNamespace(const std::string& ns)
205 {
206  std::vector<MarkerID> to_delete;
207 
208  // TODO: this is inefficient, should store every in-use id per namespace and lookup by that
209  M_IDToMarker::iterator marker_it = markers_.begin();
210  M_IDToMarker::iterator marker_end = markers_.end();
211  for (; marker_it != marker_end; ++marker_it)
212  {
213  if (marker_it->first.first == ns)
214  {
215  to_delete.push_back(marker_it->first);
216  }
217  }
218 
219  {
220  std::vector<MarkerID>::iterator it = to_delete.begin();
221  std::vector<MarkerID>::iterator end = to_delete.end();
222  for (; it != end; ++it)
223  {
224  deleteMarker(*it);
225  }
226  }
227 }
228 
230 {
231  std::vector<MarkerID> to_delete;
232  M_IDToMarker::iterator marker_it = markers_.begin();
233  for (; marker_it != markers_.end(); ++marker_it)
234  {
235  to_delete.push_back(marker_it->first);
236  }
237 
238  for (std::vector<MarkerID>::iterator it = to_delete.begin(); it != to_delete.end(); ++it)
239  {
240  deleteMarker(*it);
241  }
242 }
243 
244 void MarkerDisplay::setMarkerStatus(MarkerID id, StatusLevel level, const std::string& text)
245 {
246  std::stringstream ss;
247  ss << id.first << "/" << id.second;
248  std::string marker_name = ss.str();
249  setStatusStd(level, marker_name, text);
250 }
251 
253 {
254  std::stringstream ss;
255  ss << id.first << "/" << id.second;
256  std::string marker_name = ss.str();
257  deleteStatusStd(marker_name);
258 }
259 
260 void MarkerDisplay::incomingMarkerArray(const visualization_msgs::MarkerArray::ConstPtr& array)
261 {
262  checkMarkerArrayMsg(*array, this);
263  for (const visualization_msgs::Marker& marker : array->markers)
264  {
265  tf_filter_->add(visualization_msgs::Marker::Ptr(new visualization_msgs::Marker(marker)));
266  }
267 }
268 
269 void MarkerDisplay::incomingMarker(const visualization_msgs::Marker::ConstPtr& marker)
270 {
271  boost::mutex::scoped_lock lock(queue_mutex_);
272  message_queue_.push_back(marker);
273 }
274 
277 {
278  visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
279  if (marker->action == visualization_msgs::Marker::DELETE ||
280  marker->action == visualization_msgs::Marker::DELETEALL)
281  {
282  return this->processMessage(marker);
283  }
284  std::string authority = marker_evt.getPublisherName();
285 // TODO(wjwwood): remove this and use tf2 interface instead
286 #ifndef _WIN32
287 #pragma GCC diagnostic push
288 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
289 #endif
290 
291  std::string error = context_->getFrameManager()->discoverFailureReason(
292  marker->header.frame_id, marker->header.stamp, authority, reason);
293 
294 #ifndef _WIN32
295 #pragma GCC diagnostic pop
296 #endif
297  setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
298 }
299 
300 void MarkerDisplay::processMessage(const visualization_msgs::Marker::ConstPtr& message)
301 {
302  switch (message->action)
303  {
304  case visualization_msgs::Marker::ADD:
305  if (checkMarkerMsg(*message, this))
306  processAdd(message);
307  else
308  // delete marker, but keep marker status (generated by checkMarkerMsg)
309  deleteMarkerInternal(MarkerID(message->ns, message->id));
310  break;
311 
312  case visualization_msgs::Marker::DELETE:
313  processDelete(message);
314  break;
315 
316  case visualization_msgs::Marker::DELETEALL:
318  break;
319 
320  default:
321  ROS_ERROR("Unknown marker action: %d\n", message->action);
322  }
323 }
324 
325 void MarkerDisplay::processAdd(const visualization_msgs::Marker::ConstPtr& message)
326 {
327  QString namespace_name = QString::fromStdString(message->ns);
328  M_Namespace::iterator ns_it = namespaces_.find(namespace_name);
329  if (ns_it == namespaces_.end())
330  {
331  ns_it = namespaces_.insert(namespace_name,
332  new MarkerNamespace(namespace_name, namespaces_category_, this));
333 
334  // Adding a new namespace, determine if it's configured to be disabled
335  if (namespace_config_enabled_state_.count(namespace_name) > 0 &&
336  !namespace_config_enabled_state_[namespace_name])
337  {
338  ns_it.value()->setValue(false); // Disable the namespace
339  }
340  }
341 
342  if (!ns_it.value()->isEnabled())
343  {
344  return;
345  }
346 
347  bool create = true;
348  MarkerBasePtr marker;
349 
350  M_IDToMarker::iterator it = markers_.find(MarkerID(message->ns, message->id));
351  if (it != markers_.end())
352  {
353  marker = it->second;
354  markers_with_expiration_.erase(marker);
355  if (message->type == marker->getMessage()->type)
356  {
357  create = false;
358  }
359  else
360  {
361  deleteMarkerInternal(it->first);
362  }
363  }
364 
365  if (create)
366  {
367  marker.reset(createMarker(message->type, this, context_, scene_node_));
368  if (marker)
369  {
370  markers_.insert(std::make_pair(MarkerID(message->ns, message->id), marker));
371  }
372  }
373 
374  if (marker)
375  {
376  marker->setMessage(message);
377 
378  if (message->lifetime.toSec() > 0.0001f)
379  {
380  markers_with_expiration_.insert(marker);
381  }
382 
383  if (message->frame_locked)
384  {
385  frame_locked_markers_.insert(marker);
386  }
387 
389  }
390 }
391 
392 void MarkerDisplay::processDelete(const visualization_msgs::Marker::ConstPtr& message)
393 {
394  deleteMarker(MarkerID(message->ns, message->id));
395 
397 }
398 
399 void MarkerDisplay::update(float /*wall_dt*/, float /*ros_dt*/)
400 {
401  V_MarkerMessage local_queue;
402 
403  {
404  boost::mutex::scoped_lock lock(queue_mutex_);
405 
406  local_queue.swap(message_queue_);
407  }
408 
409  if (!local_queue.empty())
410  {
411  V_MarkerMessage::iterator message_it = local_queue.begin();
412  V_MarkerMessage::iterator message_end = local_queue.end();
413  for (; message_it != message_end; ++message_it)
414  {
415  visualization_msgs::Marker::ConstPtr& marker = *message_it;
416 
417  processMessage(marker);
418  }
419  }
420 
421  {
422  S_MarkerBase::iterator it = markers_with_expiration_.begin();
423  S_MarkerBase::iterator end = markers_with_expiration_.end();
424  for (; it != end;)
425  {
426  MarkerBasePtr marker = *it;
427  if (marker->expired())
428  {
429  ++it;
430  deleteMarker(marker->getID());
431  }
432  else
433  {
434  ++it;
435  }
436  }
437  }
438 
439  {
440  S_MarkerBase::iterator it = frame_locked_markers_.begin();
441  S_MarkerBase::iterator end = frame_locked_markers_.end();
442  for (; it != end; ++it)
443  {
444  MarkerBasePtr marker = *it;
445  marker->updateFrameLocked();
446  }
447  }
448 }
449 
451 {
452  tf_filter_->setTargetFrame(fixed_frame_.toStdString());
453 
454  clearMarkers();
455 }
456 
458 {
459  Display::reset();
460  clearMarkers();
461 }
462 
463 void MarkerDisplay::setTopic(const QString& topic, const QString& /*datatype*/)
464 {
466 }
467 
469 // MarkerNamespace
470 
471 MarkerNamespace::MarkerNamespace(const QString& name, Property* parent_property, MarkerDisplay* owner)
472  : BoolProperty(name, true, "Enable/disable all markers in this namespace.", parent_property)
473  , owner_(owner)
474 {
475  // Can't do this connect in chained constructor above because at
476  // that point it doesn't really know that "this" is a
477  // MarkerNamespace*, so the signal doesn't get connected.
478  connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged()));
479 }
480 
482 {
483  if (!isEnabled())
484  {
485  owner_->deleteMarkersInNamespace(getName().toStdString());
486  }
487 
488  // Update the configuration that stores the enabled state of all markers
490 }
491 
492 } // namespace rviz
493 
const boost::shared_ptr< ConstMessage > & getConstMessage() const
void setStatusStd(StatusProperty::Level level, const std::string &name, const std::string &text)
Show status level and text, using a std::string. Convenience function which converts std::string to Q...
Definition: display.h:163
void processMessage(const visualization_msgs::Marker::ConstPtr &message)
Processes a marker message.
void deleteStatusStd(const std::string &name)
Delete the status entry with the given std::string name. This is thread-safe.
Definition: display.h:172
virtual void setString(const QString &str)
void processAdd(const visualization_msgs::Marker::ConstPtr &message)
Processes an "Add" marker message.
void connectInput(F &f)
void onDisable() override
Derived classes override this to do the actual work of disabling themselves.
std::string discoverFailureReason(const std::string &frame_id, const ros::Time &stamp, const std::string &caller_id, tf::FilterFailureReason reason)
Create a description of a transform problem.
void processDelete(const visualization_msgs::Marker::ConstPtr &message)
Processes a "Delete" marker message.
S_MarkerBase frame_locked_markers_
void setMarkerStatus(MarkerID id, StatusLevel level, const std::string &text)
RosTopicProperty * marker_topic_property_
virtual tf::TransformListener * getTFClient() const =0
Convenience function: returns getFrameManager()->getTFClient().
MarkerNamespace(const QString &name, Property *parent_property, MarkerDisplay *owner)
QVariant getValue() const
If this config object is valid and is a Value type, this returns its value. Otherwise it returns an i...
Definition: config.cpp:324
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
DisplayContext * context_
This DisplayContext pointer is the main connection a Display has into the rest of rviz...
Definition: display.h:287
void changed()
Emitted by setValue() just after the value has changed.
void add(const MEvent &evt)
std::vector< visualization_msgs::Marker::ConstPtr > V_MarkerMessage
M_Namespace namespaces_
MarkerDisplay * owner_
ros::NodeHandle update_nh_
A NodeHandle whose CallbackQueue is run from the main GUI thread (the "update" thread).
Definition: display.h:300
A single element of a property tree, with a name, value, description, and possibly children...
Definition: property.h:100
IntProperty * queue_size_property_
boost::mutex queue_mutex_
void reset() override
Called to tell the display to clear its state.
void incomingMarker(const visualization_msgs::Marker::ConstPtr &marker)
ROS callback notifying us of a new marker.
bool checkMarkerMsg(const visualization_msgs::Marker &marker, MarkerDisplay *owner)
void load(const Config &config) override
Load the settings for this display from the given Config node, which must be a map.
Definition: display.cpp:228
void setMin(int min)
bool isValid()
Return true if the iterator currently points to a valid entry, false if not.
Definition: config.cpp:397
Config mapGetChild(const QString &key) const
If the referenced Node is a Map and it has a child with the given key, return a reference to the chil...
Definition: config.cpp:212
void deleteMarkerInternal(MarkerID id)
virtual void unsubscribe()
Unsubscribes from the "visualization_marker" "visualization_marker_array" topics. ...
std::string getTopicStd() const
Property specialized to provide max/min enforcement for integers.
Definition: int_property.h:37
Ogre::SceneNode * scene_node_
The Ogre::SceneNode to hold all 3D scene elements shown by this Display.
Definition: display.h:295
M_EnabledState namespace_config_enabled_state_
void deleteMarker(MarkerID id)
Configuration data storage class.
Definition: config.h:124
Property * namespaces_category_
void incomingMarkerArray(const visualization_msgs::MarkerArray::ConstPtr &array)
Process a MarkerArray message.
QString fixed_frame_
A convenience variable equal to context_->getFixedFrame().
Definition: display.h:312
virtual void setQueueSize(uint32_t new_queue_size)
MarkerBase * createMarker(int marker_type, MarkerDisplay *owner, DisplayContext *context, Ogre::SceneNode *parent_node)
void deleteMarkersInNamespace(const std::string &ns)
Delete all the markers within the given namespace.
std::pair< std::string, int32_t > MarkerID
void update(float wall_dt, float ros_dt) override
Called periodically by the visualization manager.
bool checkMarkerArrayMsg(const visualization_msgs::MarkerArray &array, MarkerDisplay *owner)
M_IDToMarker markers_
Map of marker id to the marker info structure.
virtual void removeChildren(int start_index=0, int count=-1)
Remove and delete some or all child Properties. Does not change the value of this Property...
Definition: property.cpp:104
virtual void reset()
Called to tell the display to clear its state.
Definition: display.cpp:287
V_MarkerMessage message_queue_
in our update() function
void deleteMarkerStatus(MarkerID id)
virtual QString getName() const
Return the name of this Property as a QString.
Definition: property.cpp:164
Iterator class for looping over all entries in a Map type Config Node.
Definition: config.h:288
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
virtual FrameManager * getFrameManager() const =0
Return the FrameManager instance.
Property(const QString &name=QString(), const QVariant default_value=QVariant(), const QString &description=QString(), Property *parent=nullptr, const char *changed_slot=nullptr, QObject *receiver=nullptr)
Constructor.
Definition: property.cpp:58
void onInitialize() override
Override this function to do subclass-specific initialization.
ros::Subscriber array_sub_
message_filters::Subscriber< visualization_msgs::Marker > sub_
void setTopic(const QString &topic, const QString &datatype) override
Set the ROS topic to listen to for this display.
friend class MarkerNamespace
tf::MessageFilter< visualization_msgs::Marker > * tf_filter_
void fixedFrameChanged() override
Called by setFixedFrame(). Override to respond to changes to fixed_frame_.
void failedMarker(const ros::MessageEvent< visualization_msgs::Marker > &marker_evt, tf::FilterFailureReason reason)
void onEnable() override
Derived classes override this to do the actual work of enabling themselves.
S_MarkerBase markers_with_expiration_
virtual int getInt() const
Return the internal property value as an integer.
Definition: int_property.h:74
virtual void subscribe()
Subscribes to the "visualization_marker" and "visualization_marker_array" topics. ...
virtual void queueRender()=0
Queues a render. Multiple calls before a render happens will only cause a single render.
bool isEnabled() const
Return true if this Display is enabled, false if not.
Definition: display.cpp:268
void load(const Config &config) override
Load the value of this property and/or its children from the given Config reference.
void deleteAllMarkers()
Delete all known markers to this plugin, regardless of id or namespace.
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
Property specialized to provide getter for booleans.
Definition: bool_property.h:38
void setTargetFrame(const std::string &target_frame)
MapIterator mapIterator() const
Return a new iterator for looping over key/value pairs.
Definition: config.cpp:356
bool initialized() const
Returns true if the display has been initialized.
Definition: display.h:275
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
Displays "markers" sent in by other ROS nodes on the "visualization_marker" topic.
#define ROS_ERROR(...)
void clearMarkers()
Removes all the markers.
const std::string & getPublisherName() const
virtual void setStatus(StatusProperty::Level level, const QString &name, const QString &text)
Show status level and text. This is thread-safe.
Definition: display.cpp:175
Connection registerCallback(const C &callback)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Fri Aug 12 2022 02:06:09