TAMSVIZ
Visualization and annotation tool for ROS
interactive.cpp
1 // TAMSVIZ
2 // (c) 2020 Philipp Ruppel
3 
4 #include "interactive.h"
5 
6 #include "../core/interaction.h"
7 #include "../core/profiler.h"
8 #include "../core/workspace.h"
9 
10 #include <eigen_conversions/eigen_msg.h>
11 
12 #include <interactive_markers/tools.h>
13 
14 InteractiveMarkerControl::InteractiveMarkerControl(
15  const visualization_msgs::InteractiveMarkerControl &message,
16  const std::shared_ptr<InteractiveMarkerParameters> &params,
17  InteractiveMarker *parent)
18  : _parent(parent), _params(params) {
19  update(message);
20 }
21 
22 void InteractiveMarkerControl::update(
23  const visualization_msgs::InteractiveMarkerControl &message) {
24  std::lock_guard<std::mutex> lock(_mutex);
25  PROFILER();
26  tf::quaternionMsgToEigen(message.orientation, _orientation);
27  if (_orientation.norm() == 0) {
28  _orientation = Eigen::Quaterniond::Identity();
29  } else {
30  _orientation.normalize();
31  }
32  _name = message.name;
33  _interaction_mode = message.interaction_mode;
34  _markers.resize(message.markers.size());
35  for (size_t i = 0; i < message.markers.size(); i++) {
36  if (!_markers.at(i)) {
37  _markers.at(i) = create<VisualizationMarker>(message.markers[i]);
38  } else {
39  _markers.at(i)->update(message.markers[i]);
40  }
41  }
42 }
43 
44 bool InteractiveMarkerControl::interact(const Interaction &interaction) {
45  std::lock_guard<std::mutex> lock(_mutex);
46  PROFILER();
47  if (pick(interaction.id)) {
48  LOG_DEBUG("interactive marker interaction mode " << _interaction_mode);
49  if (_interaction_mode != 0) {
50  Eigen::Affine3d p = _parent->pose();
51  Eigen::Affine3d control_pose =
52  _parent->renderPose() * Eigen::AngleAxisd(_orientation);
53  Eigen::Matrix3d control_orientation = control_pose.linear();
54  switch (_interaction_mode) {
55  case visualization_msgs::InteractiveMarkerControl::MOVE_3D: {
56  p.translation() +=
57  _parent->framePose().linear().inverse() *
58  (interaction.current.point - interaction.previous.point);
59  break;
60  }
61  case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE: {
62  p.translation() += _parent->framePose().linear().inverse() *
63  interaction.projectPlane(control_orientation *
64  Eigen::Vector3d::UnitX());
65  break;
66  }
67  case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS: {
68  p.translation() += _parent->framePose().linear().inverse() *
69  interaction.projectToAxis(control_orientation *
70  Eigen::Vector3d::UnitX());
71  break;
72  }
73  case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
74  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE: {
75  Eigen::Vector3d a = control_pose.inverse() *
76  interaction.previous.projectPlane(
77  control_pose.translation(),
78  control_orientation * Eigen::Vector3d::UnitX());
79  Eigen::Vector3d b = control_pose.inverse() *
80  interaction.current.projectPlane(
81  control_pose.translation(),
82  control_orientation * Eigen::Vector3d::UnitX());
83  Eigen::Matrix3d rotation =
84  Eigen::AngleAxisd(
85  std::atan2(a.y(), a.z()) - std::atan2(b.y(), b.z()),
86  Eigen::AngleAxisd(_orientation) * Eigen::Vector3d::UnitX())
87  .matrix();
88  p.linear() = (p.linear() * rotation).eval();
89  if (_interaction_mode ==
90  visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE) {
91  p.translation() +=
92  _parent->framePose().linear().inverse() *
93  (control_orientation * (b.normalized() * (b.norm() - a.norm())));
94  }
95  break;
96  }
97  case visualization_msgs::InteractiveMarkerControl::ROTATE_3D:
98  case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D: {
99  Eigen::Vector3d a =
100  _parent->renderPose().inverse() * interaction.previous.point;
101  Eigen::Vector3d b =
102  _parent->renderPose().inverse() * interaction.current.point;
103  Eigen::Quaterniond quat = Eigen::Quaterniond::Identity();
104  quat.setFromTwoVectors(a, b);
105  p *= Eigen::AngleAxisd(quat);
106  if (_interaction_mode ==
107  visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D) {
108  p.translation() += _parent->framePose().linear().inverse() *
109  (_parent->renderPose().linear() *
110  (b.normalized() * (b.norm() - a.norm())));
111  }
112  break;
113  }
114  }
115  _parent->pose(p);
116  {
117  visualization_msgs::InteractiveMarkerFeedback feedback;
118  feedback.marker_name = parentInteractiveMarker()->markerName();
119  feedback.control_name = controlName();
120  if (interaction.pressed) {
121  feedback.event_type =
122  visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN;
123  } else if (interaction.finished) {
124  feedback.event_type =
125  visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP;
126  } else {
127  feedback.event_type =
128  visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE;
129  }
130  feedback.header.frame_id = _parent->frame().name();
131  feedback.client_id = ros::this_node::getNamespace();
132  tf::pointEigenToMsg(_parent->framePose().inverse() *
133  interaction.current.point,
134  feedback.mouse_point);
135  feedback.mouse_point_valid = true;
136  tf::poseEigenToMsg(_parent->pose(), feedback.pose);
137  parentInteractiveMarker()->parentInteractiveMarkerArray()->feedback(
138  feedback);
139  }
140  return true;
141  }
142  }
143  return false;
144 }
145 
146 InteractiveMarker::InteractiveMarker(
147  const visualization_msgs::InteractiveMarker &message,
148  const std::shared_ptr<InteractiveMarkerParameters> &params,
149  InteractiveMarkerArray *parent)
150  : _parent(parent), _params(params) {
151  _text = create<TextRenderer>(_params->description_material);
152  update(message);
153 }
154 
155 void InteractiveMarker::update(const visualization_msgs::InteractiveMarker &m) {
156  PROFILER();
157  auto marker = m;
158  interactive_markers::autoComplete(marker, false);
159  std::lock_guard<std::mutex> lock(_mutex);
160  if (_dragged) {
161  return;
162  }
163  _scale = m.scale;
164  _text->text(marker.description);
165  {
166  Eigen::Affine3d p = Eigen::Affine3d::Identity();
167  tf::poseMsgToEigen(marker.pose, p);
168  pose(p);
169  }
170  _name = m.name;
171  frame(marker.header.frame_id);
172  _controls.resize(marker.controls.size());
173  for (size_t i = 0; i < marker.controls.size(); i++) {
174  auto &m = _controls.at(i);
175  if (!m) {
176  // LOG_DEBUG("creating interactive marker control");
177  m = create<InteractiveMarkerControl>(marker.controls.at(i), _params,
178  this);
179  } else {
180  m->update(marker.controls.at(i));
181  }
182  }
183 }
184 
185 void InteractiveMarker::renderSync(const RenderSyncContext &context) {
186  SceneNode::renderSync(context);
187  _text->size(_scale * _params->description_size);
188  _text->offset(_params->description_offset * _scale);
189  _text->visible(_params->show_descriptions && _params->description_size > 0);
190 }
191 
192 bool InteractiveMarker::interact(const Interaction &interaction) {
193  std::lock_guard<std::mutex> lock(_mutex);
194  PROFILER();
195  bool x = SceneNode::interact(interaction);
196  if (x) {
197  _dragged = !interaction.finished;
198  }
199  return x;
200 }
201 
202 void InteractiveMarker::update(
203  const visualization_msgs::InteractiveMarkerPose &message) {
204  std::lock_guard<std::mutex> lock(_mutex);
205  PROFILER();
206  if (_dragged) {
207  return;
208  }
209  frame(message.header.frame_id);
210  Eigen::Affine3d p = Eigen::Affine3d::Identity();
211  tf::poseMsgToEigen(message.pose, p);
212  pose(p);
213 }
214 
215 InteractiveMarkerArray::InteractiveMarkerArray(
216  const std::shared_ptr<InteractiveMarkerParameters> &params)
217  : _params(params) {}
218 
219 void InteractiveMarkerArray::renderSync(const RenderSyncContext &context) {
220  SceneNode::renderSync(context);
221 }
222 
223 void InteractiveMarkerArray::init(
224  const visualization_msgs::InteractiveMarkerInit &init) {
225  std::lock_guard<std::mutex> lock(_mutex);
226  PROFILER();
227  auto previous_markers = _markers;
228  _markers.clear();
229  for (auto &marker : init.markers) {
230  auto &m = _markers[marker.name];
231  m = previous_markers[marker.name];
232  if (!m) {
233  m = create<InteractiveMarker>(marker, _params, this);
234  } else {
235  m->update(marker);
236  }
237  }
238 }
239 
240 void InteractiveMarkerArray::update(
241  const visualization_msgs::InteractiveMarkerUpdate &update) {
242  std::lock_guard<std::mutex> lock(_mutex);
243  PROFILER();
244  for (auto &erase : update.erases) {
245  _markers.erase(erase);
246  }
247  for (auto &pose : update.poses) {
248  auto iter = _markers.find(pose.name);
249  if (iter != _markers.end()) {
250  iter->second->update(pose);
251  }
252  }
253  for (auto &marker : update.markers) {
254  auto &m = _markers[marker.name];
255  if (!m) {
256  m = create<InteractiveMarker>(marker, _params, this);
257  } else {
258  m->update(marker);
259  }
260  }
261 }
262 
263 std::shared_ptr<InteractiveMarker>
264 InteractiveMarkerArray::marker(const std::string &name) {
265  auto iter = _markers.find(name);
266  if (iter != _markers.end()) {
267  return iter->second;
268  } else {
269  return nullptr;
270  }
271 }
272 
273 InteractiveMarkerDisplayBase::InteractiveMarkerDisplayBase() {
274  _params = std::make_shared<InteractiveMarkerParameters>();
275  _params->description_material = std::make_shared<Material>();
276  _markers = node()->create<InteractiveMarkerArray>(_params);
277 }
278 
279 void InteractiveMarkerDisplayBase::renderSync(
280  const RenderSyncContext &context) {
281  MeshDisplayBase::renderSync(context);
282 }
283 
284 InteractiveMarkerDisplay::InteractiveMarkerDisplay() {
285  _publisher = std::make_shared<
286  Publisher<visualization_msgs::InteractiveMarkerFeedback>>();
287  auto *pub_ptr = _publisher.get();
288  _markers->feedback.connect(
289  _publisher,
290  [pub_ptr](const visualization_msgs::InteractiveMarkerFeedback &feedback) {
291  pub_ptr->publish(feedback);
292  });
293 }
294 
295 void InteractiveMarkerDisplay::renderSync(const RenderSyncContext &context) {
296  _params->show_descriptions = showDescriptions();
297  _params->description_size = descriptionSize();
298  _params->description_offset = descriptionOffset();
299  _params->description_material->color(descriptionColor());
300  _params->description_material->opacity(descriptionOpacity());
301  if (_watcher.changed(topicNamespace())) {
302  auto markers = _markers;
303  _update_subscriber = std::make_shared<
304  Subscriber<visualization_msgs::InteractiveMarkerUpdate>>(
305  topicNamespace() + "/update", markers,
306  [markers](const std::shared_ptr<
307  const visualization_msgs::InteractiveMarkerUpdate> &update) {
308  if (update) {
309  // LOG_DEBUG("interactive marker update");
310  markers->update(*update);
311  }
312  });
313  _init_subscriber =
314  std::make_shared<Subscriber<visualization_msgs::InteractiveMarkerInit>>(
315  topicNamespace() + "/update_full", markers,
316  [markers](const std::shared_ptr<
317  const visualization_msgs::InteractiveMarkerInit> &init) {
318  if (init) {
319  // LOG_DEBUG("interactive marker init");
320  markers->init(*init);
321  }
322  });
323  _publisher->topic(topicNamespace() + "/feedback");
324  }
325  InteractiveMarkerDisplayBase::renderSync(context);
326 }
327 
328 std::vector<std::string> InteractiveMarkerDisplay::listTopicNamespaces() {
329  std::string postfix = "/update";
330  std::vector<std::string> all = TopicManager::instance()->listTopics(
331  ros::message_traits::DataType<
332  visualization_msgs::InteractiveMarkerUpdate>::value());
333  std::vector<std::string> ret;
334  for (auto &n : all) {
335  if (n.size() > postfix.size()) {
336  if (n.substr(n.size() - postfix.size()) == postfix) {
337  ret.push_back(n.substr(0, n.size() - postfix.size()));
338  }
339  }
340  }
341  return ret;
342 }
343 
344 InteractivePoseDisplayBase::InteractivePoseDisplayBase() {}
345 
346 void InteractivePoseDisplayBase::refresh() {
347  InteractiveMarkerDisplayBase::refresh();
348  if (!_publish_thread_data) {
349  auto publish_thread_data = _publish_thread_data =
350  std::make_shared<PublishThreadData>();
351  std::weak_ptr<InteractivePoseDisplayBase> _this =
352  std::dynamic_pointer_cast<InteractivePoseDisplayBase>(
353  shared_from_this());
354  std::thread([_this, publish_thread_data]() {
355  LOG_DEBUG("start publisher thread");
356  auto t = std::chrono::steady_clock::now();
357  while (true) {
358  t += std::chrono::seconds(1);
359  {
360  std::unique_lock<std::mutex> lock(
361  publish_thread_data->_publish_mutex);
362  while (true) {
363  if (publish_thread_data->_stop_flag) {
364  LOG_DEBUG("exit publisher thread");
365  return;
366  }
367  if (std::chrono::steady_clock::now() >= t) {
368  break;
369  }
370  publish_thread_data->_stop_condition.wait_until(lock, t);
371  }
372  }
373  if (auto me = _this.lock()) {
374  LockScope ws;
375  me->publish();
376  }
377  }
378  LOG_DEBUG("exit publisher thread");
379  })
380  .detach();
381  }
382 }
383 
384 InteractivePoseDisplayBase::~InteractivePoseDisplayBase() {
385  if (_publish_thread_data) {
386  {
387  std::unique_lock<std::mutex> lock(_publish_thread_data->_publish_mutex);
388  _publish_thread_data->_stop_flag = true;
389  _publish_thread_data->_stop_condition.notify_all();
390  }
391  _publish_thread_data = nullptr;
392  }
393 }
394 
395 void InteractivePoseDisplayBase::publish() {
396  if (auto m = _markers->marker("")) {
397  auto pose = m->pose();
398  pose.linear().col(0).normalize();
399  pose.linear().col(1).normalize();
400  pose.linear().col(2).normalize();
401  publish(frame().empty()
402  ? LockScope()->document()->display()->transformer->root()
403  : frame().name(),
404  Eigen::Isometry3d(pose.matrix()));
405  }
406 }
407 
408 void InteractivePoseDisplayBase::renderSync(const RenderSyncContext &context) {
409  if (auto m = _markers->marker("")) {
410  m->frame(frame());
411  {
412  auto p = transform().toIsometry3d();
413  p.linear() *= scale();
414  m->pose(p);
415  }
416  }
417  InteractiveMarkerDisplayBase::renderSync(context);
418  publish();
419 }
420 
421 bool InteractivePoseDisplayBase::interact(const Interaction &interaction) {
422  if (InteractiveMarkerDisplayBase::interact(interaction)) {
423  if (auto m = _markers->marker("")) {
424  if (interaction.finished) {
425  ActionScope ws("Move handle");
426  transform().fromIsometry3d(Eigen::Isometry3d(m->pose().matrix()));
427  ws->modified();
428  } else {
429  LockScope ws;
430  transform().fromIsometry3d(Eigen::Isometry3d(m->pose().matrix()));
431  ws->modified();
432  }
433  }
434  return true;
435  } else {
436  return false;
437  }
438 }
439 
440 visualization_msgs::InteractiveMarker
441 InteractiveMarkerDisplayBase::makePoseMarker() {
442  visualization_msgs::InteractiveMarker marker;
443  {
444  visualization_msgs::InteractiveMarkerControl control;
445  control.interaction_mode =
446  visualization_msgs::InteractiveMarkerControl::MOVE_3D;
447  control.always_visible = true;
448  {
449  visualization_msgs::Marker marker;
450  marker.type = visualization_msgs::Marker::CUBE;
451  marker.scale.x = 0.45;
452  marker.scale.y = 0.45;
453  marker.scale.z = 0.45;
454  marker.color.r = 0.5;
455  marker.color.g = 0.5;
456  marker.color.b = 0.5;
457  marker.color.a = 1.0;
458  control.markers.push_back(marker);
459  }
460  marker.controls.push_back(control);
461  }
462  {
463  visualization_msgs::InteractiveMarkerControl control;
464  control.orientation.w = 1;
465  control.orientation.x = 1;
466  control.orientation.y = 0;
467  control.orientation.z = 0;
468  control.name = "rotate_x";
469  control.interaction_mode =
470  visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
471  marker.controls.push_back(control);
472  control.name = "move_x";
473  control.interaction_mode =
474  visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
475  marker.controls.push_back(control);
476  }
477  {
478  visualization_msgs::InteractiveMarkerControl control;
479  control.orientation.w = 1;
480  control.orientation.x = 0;
481  control.orientation.y = 0;
482  control.orientation.z = 1;
483  control.name = "rotate_y";
484  control.interaction_mode =
485  visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
486  marker.controls.push_back(control);
487  control.name = "move_y";
488  control.interaction_mode =
489  visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
490  marker.controls.push_back(control);
491  }
492  {
493  visualization_msgs::InteractiveMarkerControl control;
494  control.orientation.w = 1;
495  control.orientation.x = 0;
496  control.orientation.y = 1;
497  control.orientation.z = 0;
498  control.name = "rotate_z";
499  control.interaction_mode =
500  visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
501  marker.controls.push_back(control);
502  control.name = "move_z";
503  control.interaction_mode =
504  visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
505  marker.controls.push_back(control);
506  }
507  return marker;
508 }
509 
510 visualization_msgs::InteractiveMarker
511 InteractiveMarkerDisplayBase::makeRotationMarker() {
512  visualization_msgs::InteractiveMarker marker;
513  {
514  visualization_msgs::InteractiveMarkerControl control;
515  control.interaction_mode =
516  visualization_msgs::InteractiveMarkerControl::ROTATE_3D;
517  control.always_visible = true;
518  {
519  visualization_msgs::Marker marker;
520  marker.type = visualization_msgs::Marker::CUBE;
521  marker.scale.x = 0.45;
522  marker.scale.y = 0.45;
523  marker.scale.z = 0.45;
524  marker.color.r = 0.5;
525  marker.color.g = 0.5;
526  marker.color.b = 0.5;
527  marker.color.a = 1.0;
528  control.markers.push_back(marker);
529  }
530  marker.controls.push_back(control);
531  }
532  {
533  visualization_msgs::InteractiveMarkerControl control;
534  control.orientation.w = 1;
535  control.orientation.x = 1;
536  control.orientation.y = 0;
537  control.orientation.z = 0;
538  control.name = "rotate_x";
539  control.interaction_mode =
540  visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
541  marker.controls.push_back(control);
542  }
543  {
544  visualization_msgs::InteractiveMarkerControl control;
545  control.orientation.w = 1;
546  control.orientation.x = 0;
547  control.orientation.y = 0;
548  control.orientation.z = 1;
549  control.name = "rotate_y";
550  control.interaction_mode =
551  visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
552  marker.controls.push_back(control);
553  }
554  {
555  visualization_msgs::InteractiveMarkerControl control;
556  control.orientation.w = 1;
557  control.orientation.x = 0;
558  control.orientation.y = 1;
559  control.orientation.z = 0;
560  control.name = "rotate_z";
561  control.interaction_mode =
562  visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
563  marker.controls.push_back(control);
564  }
565  return marker;
566 }
567 
568 visualization_msgs::InteractiveMarker
569 InteractiveMarkerDisplayBase::makePointMarker() {
570  visualization_msgs::InteractiveMarker marker;
571  {
572  visualization_msgs::InteractiveMarkerControl control;
573  control.interaction_mode =
574  visualization_msgs::InteractiveMarkerControl::MOVE_3D;
575  control.always_visible = true;
576  {
577  visualization_msgs::Marker marker;
578  marker.type = visualization_msgs::Marker::CUBE;
579  marker.scale.x = 0.45;
580  marker.scale.y = 0.45;
581  marker.scale.z = 0.45;
582  marker.color.r = 0.5;
583  marker.color.g = 0.5;
584  marker.color.b = 0.5;
585  marker.color.a = 1.0;
586  control.markers.push_back(marker);
587  }
588  marker.controls.push_back(control);
589  }
590  {
591  visualization_msgs::InteractiveMarkerControl control;
592  control.orientation.w = 1;
593  control.orientation.x = 1;
594  control.orientation.y = 0;
595  control.orientation.z = 0;
596  control.name = "move_x";
597  control.interaction_mode =
598  visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
599  marker.controls.push_back(control);
600  }
601  {
602  visualization_msgs::InteractiveMarkerControl control;
603  control.orientation.w = 1;
604  control.orientation.x = 0;
605  control.orientation.y = 0;
606  control.orientation.z = 1;
607  control.name = "move_y";
608  control.interaction_mode =
609  visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
610  marker.controls.push_back(control);
611  }
612  {
613  visualization_msgs::InteractiveMarkerControl control;
614  control.orientation.w = 1;
615  control.orientation.x = 0;
616  control.orientation.y = 1;
617  control.orientation.z = 0;
618  control.name = "move_z";
619  control.interaction_mode =
620  visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
621  marker.controls.push_back(control);
622  }
623  return marker;
624 }
625 
626 TransformPublisherDisplay::TransformPublisherDisplay() {
627  visualization_msgs::InteractiveMarkerInit init;
628  init.markers.push_back(makePoseMarker());
629  _markers->init(init);
630 }
631 
632 void TransformPublisherDisplay::publish(const std::string &frame,
633  const Eigen::Isometry3d &pose) {
634  if (_publish_watcher.changed(childFrame(), frame, poseToArray(pose),
635  publisherClock())) {
636  tf2_msgs::TFMessage m;
637  m.transforms.emplace_back();
638  m.transforms.back().header.frame_id = frame;
639  m.transforms.back().header.stamp = ros::Time::now();
640  m.transforms.back().child_frame_id = childFrame();
641  tf::transformEigenToMsg(pose, m.transforms.back().transform);
642  _publisher.publish(m);
643  }
644 }
Generated by   doxygen 1.8.11