object_plugin.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2020, 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 
48  config_widget_(new QWidget()),
49  connected_(false)
50  {
51  ui_.setupUi(config_widget_);
52 
53  ui_.color->setColor(Qt::red);
54 
55  // Set background white
56  QPalette p(config_widget_->palette());
57  p.setColor(QPalette::Background, Qt::white);
58  config_widget_->setPalette(p);
59 
60  // Set status text red
61  QPalette p3(ui_.status->palette());
62  p3.setColor(QPalette::Text, Qt::red);
63  ui_.status->setPalette(p3);
64 
65  QObject::connect(ui_.selecttopic, SIGNAL(clicked()), this, SLOT(SelectTopic()));
66  QObject::connect(ui_.topic, SIGNAL(editingFinished()), this, SLOT(TopicEdited()));
67  QObject::connect(ui_.color, SIGNAL(colorEdited(const QColor&)), this,
68  SLOT(SetColor(const QColor&)));
69 
70  startTimer(1000);
71  }
72 
74  {
75  }
76 
77  void ObjectPlugin::SetColor(const QColor& color)
78  {
79  color_ = color;
80  }
81 
83  {
84  objects_.clear();
85  }
86 
88  {
90  "marti_nav_msgs/TrackedObjectArray",
91  "marti_nav_msgs/ObstacleArray");
92 
93  if (topic.name.empty())
94  {
95  return;
96  }
97 
98  ui_.topic->setText(QString::fromStdString(topic.name));
99  TopicEdited();
100  }
101 
103  {
104  std::string topic = ui_.topic->text().trimmed().toStdString();
105  if (topic != topic_)
106  {
107  initialized_ = false;
108  objects_.clear();
109  has_message_ = false;
110  PrintWarning("No messages received.");
111 
113  connected_ = false;
114 
115  topic_ = topic;
116  if (!topic.empty())
117  {
119  topic_, 100, &ObjectPlugin::handleMessage, this);
120 
121  ROS_INFO("Subscribing to %s", topic_.c_str());
122  }
123  }
124  }
125 
127  {
128  connected_ = true;
129  if (IS_INSTANCE(msg, marti_nav_msgs::TrackedObjectArray))
130  {
131  objects_.clear();
132 
133  auto objs = msg->instantiate<marti_nav_msgs::TrackedObjectArray>();
134  objects_.reserve(objs->objects.size());
135  for (const auto& obj: objs->objects)
136  {
137  handleTrack(obj);
138  }
139  }
140  else if (IS_INSTANCE(msg, marti_nav_msgs::ObstacleArray))
141  {
142  objects_.clear();
143 
144  auto objs = msg->instantiate<marti_nav_msgs::ObstacleArray>();
145  objects_.reserve(objs->obstacles.size());
146  for (const auto& obj: objs->obstacles)
147  {
148  handleObstacle(obj, objs->header);
149  }
150  }
151  else
152  {
153  PrintError("Unknown message type: " + msg->getDataType());
154  }
155  }
156 
157  void ObjectPlugin::handleObstacle(const marti_nav_msgs::Obstacle& obj, const std_msgs::Header& header)
158  {
159  if (!has_message_)
160  {
161  initialized_ = true;
162  has_message_ = true;
163  }
164 
165  // Since orientation was not implemented, many markers publish
166  // invalid all-zero orientations, so we need to check for this
167  // and provide a default identity transform.
168  tf::Quaternion orientation(0.0, 0.0, 0.0, 1.0);
169  if (obj.pose.orientation.x ||
170  obj.pose.orientation.y ||
171  obj.pose.orientation.z ||
172  obj.pose.orientation.w)
173  {
174  orientation = tf::Quaternion(obj.pose.orientation.x,
175  obj.pose.orientation.y,
176  obj.pose.orientation.z,
177  obj.pose.orientation.w);
178  }
179 
183  orientation,
184  tf::Vector3(obj.pose.position.x,
185  obj.pose.position.y,
186  obj.pose.position.z)));
187  data.source_frame = header.frame_id;
188  data.id = obj.id;
189  data.stamp = header.stamp;
190  data.transformed = false;
191  data.active = true;
192 
194  if (GetTransform(data.source_frame, data.stamp, transform))
195  {
196  data.transformed = true;
197  }
198 
199  data.polygon.reserve(obj.polygon.size()+1);
200  for (auto& point: obj.polygon)
201  {
202  tf::Vector3 pt(point.x, point.y, point.z);
203  pt = data.local_transform*pt;
204  tf::Vector3 transformed = transform *pt;
205  data.polygon.push_back({pt, transformed});
206  }
207  if (data.polygon.size())
208  {
209  data.polygon.push_back(data.polygon.front());
210  }
211  objects_.push_back(data);
212  }
213 
214  void ObjectPlugin::handleTrack(const marti_nav_msgs::TrackedObject &obj)
215  {
216  if (!has_message_)
217  {
218  initialized_ = true;
219  has_message_ = true;
220  }
221 
222  // Since orientation was not implemented, many markers publish
223  // invalid all-zero orientations, so we need to check for this
224  // and provide a default identity transform.
225  tf::Quaternion orientation(0.0, 0.0, 0.0, 1.0);
226  if (obj.pose.pose.orientation.x ||
227  obj.pose.pose.orientation.y ||
228  obj.pose.pose.orientation.z ||
229  obj.pose.pose.orientation.w)
230  {
231  orientation = tf::Quaternion(obj.pose.pose.orientation.x,
232  obj.pose.pose.orientation.y,
233  obj.pose.pose.orientation.z,
234  obj.pose.pose.orientation.w);
235  }
236 
240  orientation,
241  tf::Vector3(obj.pose.pose.position.x,
242  obj.pose.pose.position.y,
243  obj.pose.pose.position.z)));
244  data.stamp = obj.header.stamp;
245  data.source_frame = obj.header.frame_id;
246  data.id = std::to_string(obj.id);
247  data.transformed = false;
248  data.active = obj.active;
249 
251  if (GetTransform(data.source_frame, data.stamp, transform))
252  {
253  data.transformed = true;
254  }
255 
256  data.polygon.reserve(obj.polygon.size()+1);
257  for (auto& point: obj.polygon)
258  {
259  tf::Vector3 pt(point.x, point.y, point.z);
260  pt = data.local_transform*pt;
261  tf::Vector3 transformed = transform *pt;
262  data.polygon.push_back({pt, transformed});
263  }
264  if (data.polygon.size())
265  {
266  data.polygon.push_back(data.polygon.front());
267  }
268  objects_.push_back(data);
269  }
270 
271  void ObjectPlugin::PrintError(const std::string& message)
272  {
273  PrintErrorHelper(ui_.status, message);
274  }
275 
276  void ObjectPlugin::PrintInfo(const std::string& message)
277  {
278  PrintInfoHelper(ui_.status, message);
279  }
280 
281  void ObjectPlugin::PrintWarning(const std::string& message)
282  {
283  PrintWarningHelper(ui_.status, message);
284  }
285 
286  QWidget* ObjectPlugin::GetConfigWidget(QWidget* parent)
287  {
288  config_widget_->setParent(parent);
289 
290  return config_widget_;
291  }
292 
293  bool ObjectPlugin::Initialize(QGLWidget* canvas)
294  {
295  canvas_ = canvas;
296  SetColor(ui_.color->color());
297 
298  return true;
299  }
300 
301  void ObjectPlugin::Draw(double x, double y, double scale)
302  {
303  for (const auto& obj: objects_)
304  {
305  if (!obj.transformed)
306  {
307  continue;
308  }
309 
310  if (!obj.active && !ui_.show_inactive->isChecked())
311  {
312  continue;
313  }
314 
315  glColor4f(color_.redF(), color_.greenF(), color_.blueF(), 1.0);
316 
317  glLineWidth(3.0);
318  glBegin(GL_LINE_STRIP);
319 
320  for (const auto &point : obj.polygon) {
321  glVertex2d(
322  point.transformed_point.getX(),
323  point.transformed_point.getY());
324  }
325 
326  glEnd();
327 
328  PrintInfo("OK");
329  }
330  }
331 
332  void ObjectPlugin::Paint(QPainter* painter, double x, double y, double scale)
333  {
334  if (!ui_.show_ids->isChecked())
335  {
336  return;
337  }
338 
339  // Most of the marker drawing is done using OpenGL commands, but text labels
340  // are rendered using a QPainter. This is intended primarily as an example
341  // of how the QPainter works.
342  ros::Time now = ros::Time::now();
343 
344  // We don't want the text to be rotated or scaled, but we do want it to be
345  // translated appropriately. So, we save off the current world transform
346  // and reset it; when we actually draw the text, we'll manually translate
347  // it to the right place.
348  QTransform tf = painter->worldTransform();
349  QFont font("Helvetica", 10);
350  painter->setFont(font);
351  painter->save();
352  painter->resetTransform();
353 
354  for (const auto& obj: objects_)
355  {
356  if (!obj.transformed)
357  {
358  continue;
359  }
360 
361  QPen pen(QBrush(QColor::fromRgbF(0, 0, 0, 1)), 1);
362  painter->setPen(pen);
363 
364  const StampedPoint& rosPoint = obj.polygon.front();
365  QPointF point = tf.map(QPointF(rosPoint.transformed_point.x(),
366  rosPoint.transformed_point.y()));
367 
368  auto text = QString::fromStdString(obj.id);
369  // Get bounding rectangle
370  QRectF rect(point, QSizeF(10,10));
371  rect = painter->boundingRect(rect, Qt::AlignLeft | Qt::AlignHCenter, text);
372  painter->drawText(rect, text);
373 
374  PrintInfo("OK");
375  }
376 
377  painter->restore();
378  }
379 
381  {
382  for (auto& obj: objects_)
383  {
385  if (GetTransform(obj.source_frame, obj.stamp, transform))
386  {
387  obj.transformed = true;
388 
389  for (auto &point : obj.polygon)
390  {
391  point.transformed_point = transform * point.point;
392  }
393  }
394  else
395  {
396  obj.transformed = false;
397  }
398  }
399  }
400 
401  void ObjectPlugin::LoadConfig(const YAML::Node& node, const std::string& path)
402  {
403  if (node["topic"])
404  {
405  std::string topic;
406  node["topic"] >> topic;
407  ui_.topic->setText(boost::trim_copy(topic).c_str());
408 
409  TopicEdited();
410  }
411 
412  if (node["color"])
413  {
414  std::string color;
415  node["color"] >> color;
416  QColor qcolor(color.c_str());
417  SetColor(qcolor);
418  ui_.color->setColor(qcolor);
419  }
420 
421  if (node["show_ids"])
422  {
423  bool checked;
424  node["show_ids"] >> checked;
425  ui_.show_ids->setChecked( checked );
426  }
427 
428  if (node["show_inactive"])
429  {
430  bool checked;
431  node["show_inactive"] >> checked;
432  ui_.show_inactive->setChecked( checked );
433  }
434  }
435 
436  void ObjectPlugin::SaveConfig(YAML::Emitter& emitter, const std::string& path)
437  {
438  emitter << YAML::Key << "topic" << YAML::Value << boost::trim_copy(ui_.topic->text().toStdString());
439 
440  emitter << YAML::Key << "color" << YAML::Value
441  << ui_.color->color().name().toStdString();
442 
443  emitter << YAML::Key << "show_ids" << YAML::Value << ui_.show_ids->isChecked();
444 
445  emitter << YAML::Key << "show_inactive" << YAML::Value << ui_.show_inactive->isChecked();
446  }
447 
448  void ObjectPlugin::timerEvent(QTimerEvent *event)
449  {
450  bool new_connected = (object_sub_.getNumPublishers() > 0);
451  if (connected_ && !new_connected)
452  {
454  if (!topic_.empty())
455  {
457  topic_, 100, &ObjectPlugin::handleMessage, this);
458  }
459  }
460  connected_ = new_connected;
461  }
462 }
463 
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)
void timerEvent(QTimerEvent *)
ros::NodeHandle node_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::vector< ObjectData > objects_
void Draw(double x, double y, double scale)
void handleMessage(const topic_tools::ShapeShifter::ConstPtr &msg)
bool Initialize(QGLWidget *canvas)
uint32_t getNumPublishers() const
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void Paint(QPainter *painter, double x, double y, double scale)
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
data
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
void PrintWarning(const std::string &message)
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
QWidget * GetConfigWidget(QWidget *parent)
void SetColor(const QColor &color)
std::vector< StampedPoint > polygon
void LoadConfig(const YAML::Node &node, const std::string &path)
#define ROS_INFO(...)
void handleObstacle(const marti_nav_msgs::Obstacle &obj, const std_msgs::Header &header)
void handleTrack(const marti_nav_msgs::TrackedObject &obj)
void PrintInfo(const std::string &message)
void transform(marti_nav_msgs::Path &path, const swri_transform_util::Transform &transform, const std::string &target_frame)
static Time now()
swri_transform_util::Transform local_transform
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void PrintError(const std::string &message)
#define IS_INSTANCE(msg, type)


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