Fawkes API  Fawkes Development Version
visualization_thread.cpp
1 
2 /***************************************************************************
3  * visualization_thread.cpp - Visualization pathplan via rviz
4  *
5  * Created: Fri Nov 11 21:25:46 2011
6  * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "visualization_thread.h"
23 
24 #include "aspect/blocked_timing.h"
25 
26 #include <navgraph/constraints/constraint_repo.h>
27 #include <navgraph/constraints/polygon_edge_constraint.h>
28 #include <navgraph/constraints/polygon_node_constraint.h>
29 #include <navgraph/navgraph.h>
30 #include <ros/ros.h>
31 #include <tf/types.h>
32 #include <utils/math/angle.h>
33 #include <utils/math/coord.h>
34 
35 using namespace fawkes;
36 
37 /** @class NavGraphVisualizationThread "visualization_thread.h"
38  * Send Marker messages to rviz to show navgraph info.
39  * @author Tim Niemueller
40  */
41 
42 typedef std::multimap<std::string, std::string> ConnMap;
43 
44 /** Constructor. */
46 : fawkes::Thread("NavGraphVisualizationThread", Thread::OPMODE_WAITFORWAKEUP),
47  fawkes::BlockedTimingAspect(fawkes::BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
48 {
49  graph_ = NULL;
50  crepo_ = NULL;
51 }
52 
53 void
55 {
56  vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>("visualization_marker_array",
57  100,
58  /* latching */ true);
59 
60  cfg_global_frame_ = config->get_string("/frames/fixed");
61 
62  cfg_cost_scale_max_ = config->get_float("/navgraph/visualization/cost_scale_max");
63  if (cfg_cost_scale_max_ < 1.0) {
64  throw Exception("Visualization cost max scale must greater or equal to 1.0");
65  }
66 
67  // subtract one because 1.0 is the minimum value where we want the
68  // resulting value to be zero.
69  cfg_cost_scale_max_ -= 1.0;
70 
71  last_id_num_ = constraints_last_id_num_ = 0;
72  publish();
73 }
74 
75 void
77 {
78  visualization_msgs::MarkerArray m;
79 
80 #if ROS_VERSION_MINIMUM(1, 10, 0)
81  visualization_msgs::Marker delop;
82  delop.header.frame_id = cfg_global_frame_;
83  delop.header.stamp = ros::Time::now();
84  delop.ns = "navgraph-constraints";
85  delop.id = 0;
86  delop.action = 3; // visualization_msgs::Marker::DELETEALL;
87  m.markers.push_back(delop);
88 #else
89  for (size_t i = 0; i < last_id_num_; ++i) {
90  visualization_msgs::Marker delop;
91  delop.header.frame_id = cfg_global_frame_;
92  delop.header.stamp = ros::Time::now();
93  delop.ns = "navgraph";
94  delop.id = i;
95  delop.action = visualization_msgs::Marker::DELETE;
96  m.markers.push_back(delop);
97  }
98  for (size_t i = 0; i < constraints_last_id_num_; ++i) {
99  visualization_msgs::Marker delop;
100  delop.header.frame_id = cfg_global_frame_;
101  delop.header.stamp = ros::Time::now();
102  delop.ns = "navgraph-constraints";
103  delop.id = i;
104  delop.action = visualization_msgs::Marker::DELETE;
105  m.markers.push_back(delop);
106  }
107 #endif
108  vispub_.publish(m);
109  usleep(500000); // needs some time to actually send
110  vispub_.shutdown();
111 }
112 
113 /** Set the graph.
114  * @param graph graph to use
115  */
116 void
118 {
119  graph_ = graph;
120  traversal_.invalidate();
121  plan_to_ = plan_from_ = "";
122  regenerate();
123 }
124 
125 /** Set the constraint repo.
126  * @param crepo constraint repo
127  */
128 void
130 {
131  crepo_ = crepo;
132 }
133 
134 /** Set current path.
135  * @param traversal currently active path traversal
136  */
137 void
139 {
140  traversal_ = traversal;
141  plan_to_ = plan_from_ = "";
142  regenerate();
143 }
144 
145 /** Reset the current plan. */
146 void
148 {
149  traversal_.invalidate();
150  plan_to_ = plan_from_ = "";
151  regenerate();
152 }
153 
154 /** Set the currently executed edge of the plan.
155  * @param from node name of the originating node
156  * @param to node name of the target node
157  */
158 void
159 NavGraphVisualizationThread::set_current_edge(const std::string &from, const std::string &to)
160 {
161  if (plan_from_ != from || plan_to_ != to) {
162  plan_from_ = from;
163  plan_to_ = to;
164  wakeup();
165  }
166 }
167 
168 void
170 {
171  regenerate();
172 }
173 
174 void
176 {
177  publish();
178 }
179 
180 float
181 NavGraphVisualizationThread::edge_cost_factor(
182  std::list<std::tuple<std::string, std::string, std::string, float>> &costs,
183  const std::string & from,
184  const std::string & to,
185  std::string & constraint_name)
186 {
187  for (const std::tuple<std::string, std::string, std::string, float> &c : costs) {
188  if ((std::get<0>(c) == from && std::get<1>(c) == to)
189  || (std::get<0>(c) == to && std::get<1>(c) == from)) {
190  constraint_name = std::get<2>(c);
191  return std::get<3>(c);
192  }
193  }
194 
195  return 0.;
196 }
197 
198 void
199 NavGraphVisualizationThread::add_circle_markers(visualization_msgs::MarkerArray &m,
200  size_t & id_num,
201  float center_x,
202  float center_y,
203  float radius,
204  unsigned int arc_length,
205  float r,
206  float g,
207  float b,
208  float alpha,
209  float line_width)
210 {
211  for (unsigned int a = 0; a < 360; a += 2 * arc_length) {
212  visualization_msgs::Marker arc;
213  arc.header.frame_id = cfg_global_frame_;
214  arc.header.stamp = ros::Time::now();
215  arc.ns = "navgraph";
216  arc.id = id_num++;
217  arc.type = visualization_msgs::Marker::LINE_STRIP;
218  arc.action = visualization_msgs::Marker::ADD;
219  arc.scale.x = arc.scale.y = arc.scale.z = line_width;
220  arc.color.r = r;
221  arc.color.g = g;
222  arc.color.b = b;
223  arc.color.a = alpha;
224  arc.lifetime = ros::Duration(0, 0);
225  arc.points.resize(arc_length);
226  for (unsigned int j = 0; j < arc_length; ++j) {
227  float circ_x = 0, circ_y = 0;
228  polar2cart2d(deg2rad(a + j), radius, &circ_x, &circ_y);
229  arc.points[j].x = center_x + circ_x;
230  arc.points[j].y = center_y + circ_y;
231  arc.points[j].z = 0.;
232  }
233  m.markers.push_back(arc);
234  }
235 }
236 
237 void
238 NavGraphVisualizationThread::regenerate()
239 {
240  if (!graph_)
241  return;
242 
243  graph_.lock();
244  std::vector<fawkes::NavGraphNode> nodes = graph_->nodes();
245  std::vector<fawkes::NavGraphEdge> edges = graph_->edges();
246  std::map<std::string, std::string> default_props = graph_->default_properties();
247  graph_.unlock();
248 
249  std::map<std::string, fawkes::NavGraphNode> nodes_map;
250  for (const fawkes::NavGraphNode &n : nodes) {
251  nodes_map[n.name()] = n;
252  }
253 
254  crepo_.lock();
255  std::map<std::string, std::string> bl_nodes = crepo_->blocks(nodes);
256  std::map<std::pair<std::string, std::string>, std::string> bl_edges = crepo_->blocks(edges);
257  std::list<std::tuple<std::string, std::string, std::string, float>> edge_cfs =
258  crepo_->cost_factor(edges);
259  crepo_.unlock();
260 
261  size_t id_num = 0;
262  size_t constraints_id_num = 0;
263 
264  visualization_msgs::MarkerArray m;
265 
266 #if ROS_VERSION_MINIMUM(1, 10, 0)
267  {
268  visualization_msgs::Marker delop;
269  delop.header.frame_id = cfg_global_frame_;
270  delop.header.stamp = ros::Time::now();
271  delop.ns = "navgraph-constraints";
272  delop.id = 0;
273  delop.action = 3; // visualization_msgs::Marker::DELETEALL;
274  m.markers.push_back(delop);
275  }
276 #endif
277 
278  visualization_msgs::Marker lines;
279  lines.header.frame_id = cfg_global_frame_;
280  lines.header.stamp = ros::Time::now();
281  lines.ns = "navgraph";
282  lines.id = id_num++;
283  lines.type = visualization_msgs::Marker::LINE_LIST;
284  lines.action = visualization_msgs::Marker::ADD;
285  lines.color.r = 0.5;
286  lines.color.g = lines.color.b = 0.f;
287  lines.color.a = 1.0;
288  lines.scale.x = 0.02;
289  lines.lifetime = ros::Duration(0, 0);
290 
291  visualization_msgs::Marker plan_lines;
292  plan_lines.header.frame_id = cfg_global_frame_;
293  plan_lines.header.stamp = ros::Time::now();
294  plan_lines.ns = "navgraph";
295  plan_lines.id = id_num++;
296  plan_lines.type = visualization_msgs::Marker::LINE_LIST;
297  plan_lines.action = visualization_msgs::Marker::ADD;
298  plan_lines.color.r = 1.0;
299  plan_lines.color.g = plan_lines.color.b = 0.f;
300  plan_lines.color.a = 1.0;
301  plan_lines.scale.x = 0.035;
302  plan_lines.lifetime = ros::Duration(0, 0);
303 
304  visualization_msgs::Marker blocked_lines;
305  blocked_lines.header.frame_id = cfg_global_frame_;
306  blocked_lines.header.stamp = ros::Time::now();
307  blocked_lines.ns = "navgraph";
308  blocked_lines.id = id_num++;
309  blocked_lines.type = visualization_msgs::Marker::LINE_LIST;
310  blocked_lines.action = visualization_msgs::Marker::ADD;
311  blocked_lines.color.r = blocked_lines.color.g = blocked_lines.color.b = 0.5;
312  blocked_lines.color.a = 1.0;
313  blocked_lines.scale.x = 0.02;
314  blocked_lines.lifetime = ros::Duration(0, 0);
315 
316  visualization_msgs::Marker cur_line;
317  cur_line.header.frame_id = cfg_global_frame_;
318  cur_line.header.stamp = ros::Time::now();
319  cur_line.ns = "navgraph";
320  cur_line.id = id_num++;
321  cur_line.type = visualization_msgs::Marker::LINE_LIST;
322  cur_line.action = visualization_msgs::Marker::ADD;
323  cur_line.color.g = 1.f;
324  cur_line.color.r = cur_line.color.b = 0.f;
325  cur_line.color.a = 1.0;
326  cur_line.scale.x = 0.05;
327  cur_line.lifetime = ros::Duration(0, 0);
328 
329  for (size_t i = 0; i < nodes.size(); ++i) {
330  bool is_in_plan = traversal_ && traversal_.path().contains(nodes[i]);
331  bool is_last =
332  traversal_ && (traversal_.path().size() >= 1) && (traversal_.path().goal() == nodes[i]);
333  //bool is_next = (plan_.size() >= 2) && (plan_[1].name() == nodes[i].name());
334  bool is_active = (plan_to_ == nodes[i].name());
335 
336  std::string ns = "navgraph";
337  if (nodes[i].has_property("group")) {
338  ns += "-" + nodes[i].property("group");
339  }
340 
341  visualization_msgs::Marker sphere;
342  sphere.header.frame_id = cfg_global_frame_;
343  sphere.header.stamp = ros::Time::now();
344  sphere.ns = ns;
345  sphere.id = id_num++;
346  sphere.type = visualization_msgs::Marker::SPHERE;
347  sphere.action = visualization_msgs::Marker::ADD;
348  sphere.pose.position.x = nodes[i].x();
349  sphere.pose.position.y = nodes[i].y();
350  sphere.pose.position.z = 0.;
351  sphere.pose.orientation.w = 1.;
352  sphere.scale.y = 0.05;
353  sphere.scale.z = 0.05;
354  if (is_in_plan) {
355  sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.1;
356  if (is_last) {
357  sphere.color.r = 0.f;
358  sphere.color.g = 1.f;
359  } else {
360  sphere.color.r = 1.f;
361  sphere.color.g = 0.f;
362  }
363  sphere.color.b = 0.f;
364  } else if (bl_nodes.find(nodes[i].name()) != bl_nodes.end()) {
365  sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.05;
366  sphere.color.r = sphere.color.g = sphere.color.b = 0.5;
367 
368  visualization_msgs::Marker text;
369  text.header.frame_id = cfg_global_frame_;
370  text.header.stamp = ros::Time::now();
371  text.ns = "navgraph-constraints";
372  text.id = constraints_id_num++;
373  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
374  text.action = visualization_msgs::Marker::ADD;
375  text.pose.position.x = nodes[i].x();
376  text.pose.position.y = nodes[i].y();
377  text.pose.position.z = 0.3;
378  text.pose.orientation.w = 1.;
379  text.scale.z = 0.12;
380  text.color.r = 1.0;
381  text.color.g = text.color.b = 0.f;
382  text.color.a = 1.0;
383  text.lifetime = ros::Duration(0, 0);
384  text.text = bl_nodes[nodes[i].name()];
385  m.markers.push_back(text);
386 
387  } else {
388  sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.05;
389  sphere.color.r = 0.5;
390  sphere.color.b = 0.f;
391  }
392  sphere.color.a = 1.0;
393  sphere.lifetime = ros::Duration(0, 0);
394  m.markers.push_back(sphere);
395 
396  if (is_last) {
397  float target_tolerance = 0.;
398  if (nodes[i].has_property("target_tolerance")) {
399  target_tolerance = nodes[i].property_as_float("target_tolerance");
400  } else if (default_props.find("target_tolerance") != default_props.end()) {
401  target_tolerance = StringConversions::to_float(default_props["target_tolerance"]);
402  }
403  if (target_tolerance > 0.) {
404  add_circle_markers(m,
405  id_num,
406  nodes[i].x(),
407  nodes[i].y(),
408  target_tolerance,
409  5,
410  sphere.color.r,
411  sphere.color.g,
412  sphere.color.b,
413  is_active ? sphere.color.a : 0.4);
414  }
415  } else if (is_active) {
416  float travel_tolerance = 0.;
417  if (nodes[i].has_property("travel_tolerance")) {
418  travel_tolerance = nodes[i].property_as_float("travel_tolerance");
419  } else if (default_props.find("travel_tolerance") != default_props.end()) {
420  travel_tolerance = StringConversions::to_float(default_props["travel_tolerance"]);
421  }
422  if (travel_tolerance > 0.) {
423  add_circle_markers(m,
424  id_num,
425  nodes[i].x(),
426  nodes[i].y(),
427  travel_tolerance,
428  10,
429  sphere.color.r,
430  sphere.color.g,
431  sphere.color.b,
432  sphere.color.a);
433  }
434  }
435 
436  if (is_in_plan) {
437  float shortcut_tolerance = 0.;
438  if (nodes[i].has_property("shortcut_tolerance")) {
439  shortcut_tolerance = nodes[i].property_as_float("shortcut_tolerance");
440  } else if (default_props.find("shortcut_tolerance") != default_props.end()) {
441  shortcut_tolerance = StringConversions::to_float(default_props["shortcut_tolerance"]);
442  }
443  if (shortcut_tolerance > 0.) {
444  add_circle_markers(m,
445  id_num,
446  nodes[i].x(),
447  nodes[i].y(),
448  shortcut_tolerance,
449  30,
450  sphere.color.r,
451  sphere.color.g,
452  sphere.color.b,
453  0.3);
454  }
455  }
456 
457  if (nodes[i].has_property("orientation")) {
458  float ori = nodes[i].property_as_float("orientation");
459  //logger->log_debug(name(), "Node %s has orientation %f", nodes[i].name().c_str(), ori);
460  visualization_msgs::Marker arrow;
461  arrow.header.frame_id = cfg_global_frame_;
462  arrow.header.stamp = ros::Time::now();
463  arrow.ns = ns;
464  arrow.id = id_num++;
465  arrow.type = visualization_msgs::Marker::ARROW;
466  arrow.action = visualization_msgs::Marker::ADD;
467  arrow.pose.position.x = nodes[i].x();
468  arrow.pose.position.y = nodes[i].y();
469  arrow.pose.position.z = 0.;
470  tf::Quaternion q = tf::create_quaternion_from_yaw(ori);
471  arrow.pose.orientation.x = q.x();
472  arrow.pose.orientation.y = q.y();
473  arrow.pose.orientation.z = q.z();
474  arrow.pose.orientation.w = q.w();
475  arrow.scale.x = 0.08;
476  arrow.scale.y = 0.02;
477  arrow.scale.z = 0.02;
478  if (is_in_plan) {
479  if (is_last) {
480  arrow.color.r = arrow.color.g = 1.f;
481  } else {
482  arrow.color.r = 1.f;
483  arrow.color.g = 0.f;
484  }
485  } else {
486  arrow.color.r = 0.5;
487  }
488  arrow.color.b = 0.f;
489  arrow.color.a = 1.0;
490  arrow.lifetime = ros::Duration(0, 0);
491  m.markers.push_back(arrow);
492  }
493 
494  visualization_msgs::Marker text;
495  text.header.frame_id = cfg_global_frame_;
496  text.header.stamp = ros::Time::now();
497  text.ns = ns;
498  text.id = id_num++;
499  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
500  text.action = visualization_msgs::Marker::ADD;
501  text.pose.position.x = nodes[i].x();
502  text.pose.position.y = nodes[i].y();
503  text.pose.position.z = 0.08;
504  text.pose.orientation.w = 1.;
505  text.scale.z = 0.15; // 15cm high
506  text.color.r = text.color.g = text.color.b = 1.0f;
507  text.color.a = 1.0;
508  text.lifetime = ros::Duration(0, 0);
509  text.text = nodes[i].name();
510  m.markers.push_back(text);
511  }
512 
513  if (traversal_ && !traversal_.path().empty()
514  && traversal_.path().goal().name() == "free-target") {
515  const NavGraphNode &target_node = traversal_.path().goal();
516 
517  // we are traveling to a free target
518  visualization_msgs::Marker sphere;
519  sphere.header.frame_id = cfg_global_frame_;
520  sphere.header.stamp = ros::Time::now();
521  sphere.ns = "navgraph";
522  sphere.id = id_num++;
523  sphere.type = visualization_msgs::Marker::SPHERE;
524  sphere.action = visualization_msgs::Marker::ADD;
525  sphere.pose.position.x = target_node.x();
526  sphere.pose.position.y = target_node.y();
527  sphere.pose.position.z = 0.;
528  sphere.pose.orientation.w = 1.;
529  sphere.scale.y = 0.05;
530  sphere.scale.z = 0.05;
531  sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.1;
532  sphere.color.r = 1.;
533  sphere.color.g = 0.5f;
534  sphere.color.b = 0.f;
535  sphere.color.a = 1.0;
536  sphere.lifetime = ros::Duration(0, 0);
537  m.markers.push_back(sphere);
538 
539  visualization_msgs::Marker text;
540  text.header.frame_id = cfg_global_frame_;
541  text.header.stamp = ros::Time::now();
542  text.ns = "navgraph";
543  text.id = id_num++;
544  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
545  text.action = visualization_msgs::Marker::ADD;
546  text.pose.position.x = target_node.x();
547  text.pose.position.y = target_node.y();
548  text.pose.position.z = 0.08;
549  text.pose.orientation.w = 1.;
550  text.scale.z = 0.15; // 15cm high
551  text.color.r = text.color.g = text.color.b = 1.0f;
552  text.color.a = 1.0;
553  text.lifetime = ros::Duration(0, 0);
554  text.text = "Free Target";
555  m.markers.push_back(text);
556 
557  if (target_node.has_property("orientation")) {
558  float ori = target_node.property_as_float("orientation");
559  visualization_msgs::Marker ori_arrow;
560  ori_arrow.header.frame_id = cfg_global_frame_;
561  ori_arrow.header.stamp = ros::Time::now();
562  ori_arrow.ns = "navgraph";
563  ori_arrow.id = id_num++;
564  ori_arrow.type = visualization_msgs::Marker::ARROW;
565  ori_arrow.action = visualization_msgs::Marker::ADD;
566  ori_arrow.pose.position.x = target_node.x();
567  ori_arrow.pose.position.y = target_node.y();
568  ori_arrow.pose.position.z = 0.;
569  tf::Quaternion q = tf::create_quaternion_from_yaw(ori);
570  ori_arrow.pose.orientation.x = q.x();
571  ori_arrow.pose.orientation.y = q.y();
572  ori_arrow.pose.orientation.z = q.z();
573  ori_arrow.pose.orientation.w = q.w();
574  ori_arrow.scale.x = 0.08;
575  ori_arrow.scale.y = 0.02;
576  ori_arrow.scale.z = 0.02;
577  ori_arrow.color.r = 1.f;
578  ori_arrow.color.g = 0.5f;
579  ori_arrow.color.b = 0.f;
580  ori_arrow.color.a = 1.0;
581  ori_arrow.lifetime = ros::Duration(0, 0);
582  m.markers.push_back(ori_arrow);
583  }
584 
585  float target_tolerance = 0.;
586  if (traversal_.path().goal().has_property("target_tolerance")) {
587  target_tolerance = traversal_.path().goal().property_as_float("target_tolerance");
588  } else if (default_props.find("target_tolerance") != default_props.end()) {
589  target_tolerance = StringConversions::to_float(default_props["target_tolerance"]);
590  }
591  if (target_tolerance > 0.) {
592  add_circle_markers(m,
593  id_num,
594  traversal_.path().goal().x(),
595  traversal_.path().goal().y(),
596  target_tolerance,
597  10,
598  sphere.color.r,
599  sphere.color.g,
600  sphere.color.b,
601  (traversal_.last()) ? sphere.color.a : 0.5);
602  }
603 
604  float shortcut_tolerance = 0.;
605  if (traversal_.path().goal().has_property("shortcut_tolerance")) {
606  shortcut_tolerance = traversal_.path().goal().property_as_float("shortcut_tolerance");
607  } else if (default_props.find("shortcut_tolerance") != default_props.end()) {
608  shortcut_tolerance = StringConversions::to_float(default_props["shortcut_tolerance"]);
609  }
610  if (shortcut_tolerance > 0.) {
611  add_circle_markers(m,
612  id_num,
613  traversal_.path().goal().x(),
614  traversal_.path().goal().y(),
615  shortcut_tolerance,
616  30,
617  sphere.color.r,
618  sphere.color.g,
619  sphere.color.b,
620  0.3);
621  }
622 
623  if (traversal_.remaining() >= 2) {
624  const NavGraphNode &last_graph_node = traversal_.path().nodes()[traversal_.path().size() - 2];
625 
626  geometry_msgs::Point p1;
627  p1.x = last_graph_node.x();
628  p1.y = last_graph_node.y();
629  p1.z = 0.;
630 
631  geometry_msgs::Point p2;
632  p2.x = target_node.x();
633  p2.y = target_node.y();
634  p2.z = 0.;
635 
636  visualization_msgs::Marker arrow;
637  arrow.header.frame_id = cfg_global_frame_;
638  arrow.header.stamp = ros::Time::now();
639  arrow.ns = "navgraph";
640  arrow.id = id_num++;
641  arrow.type = visualization_msgs::Marker::ARROW;
642  arrow.action = visualization_msgs::Marker::ADD;
643  arrow.color.a = 1.0;
644  arrow.lifetime = ros::Duration(0, 0);
645  arrow.points.push_back(p1);
646  arrow.points.push_back(p2);
647 
648  if (plan_to_ == target_node.name()) {
649  // it's the current line
650  arrow.color.r = arrow.color.g = 1.f;
651  arrow.color.b = 0.f;
652  arrow.scale.x = 0.1; // shaft radius
653  arrow.scale.y = 0.3; // head radius
654  } else {
655  // it's in the current plan
656  arrow.color.r = 1.0;
657  arrow.color.g = 0.5f;
658  arrow.color.b = 0.f;
659  arrow.scale.x = 0.07; // shaft radius
660  arrow.scale.y = 0.2; // head radius
661  }
662  m.markers.push_back(arrow);
663  }
664  }
665 
666  for (size_t i = 0; i < edges.size(); ++i) {
667  NavGraphEdge &edge = edges[i];
668  if (nodes_map.find(edge.from()) != nodes_map.end()
669  && nodes_map.find(edge.to()) != nodes_map.end()) {
670  NavGraphNode from = nodes_map[edge.from()];
671  NavGraphNode to = nodes_map[edge.to()];
672 
673  geometry_msgs::Point p1;
674  p1.x = from.x();
675  p1.y = from.y();
676  p1.z = 0.;
677 
678  geometry_msgs::Point p2;
679  p2.x = to.x();
680  p2.y = to.y();
681  p2.z = 0.;
682 
683  std::string cost_cstr_name;
684  float cost_factor = edge_cost_factor(edge_cfs, from.name(), to.name(), cost_cstr_name);
685 
686  if (edge.is_directed()) {
687  visualization_msgs::Marker arrow;
688  arrow.header.frame_id = cfg_global_frame_;
689  arrow.header.stamp = ros::Time::now();
690  arrow.ns = "navgraph";
691  arrow.id = id_num++;
692  arrow.type = visualization_msgs::Marker::ARROW;
693  arrow.action = visualization_msgs::Marker::ADD;
694  arrow.color.a = 1.0;
695  arrow.lifetime = ros::Duration(0, 0);
696  arrow.points.push_back(p1);
697  arrow.points.push_back(p2);
698 
699  if (plan_from_ == from.name() && plan_to_ == to.name()) {
700  // it's the current line
701  arrow.color.g = 1.f;
702  arrow.color.r = arrow.color.b = 0.f;
703  arrow.scale.x = 0.1; // shaft radius
704  arrow.scale.y = 0.3; // head radius
705  } else {
706  bool in_plan = false;
707  if (traversal_) {
708  for (size_t p = 0; p < traversal_.path().nodes().size(); ++p) {
709  if (traversal_.path().nodes()[p] == from
710  && (p < traversal_.path().nodes().size() - 1
711  && traversal_.path().nodes()[p + 1] == to)) {
712  in_plan = true;
713  break;
714  }
715  }
716  }
717 
718  if (in_plan) {
719  // it's in the current plan
720  arrow.color.r = 1.0;
721  if (cost_factor >= 1.00001) {
722  arrow.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_);
723  } else {
724  arrow.color.g = 0.f;
725  }
726  arrow.color.b = 0.f;
727  arrow.scale.x = 0.07; // shaft radius
728  arrow.scale.y = 0.2; // head radius
729  } else if (bl_nodes.find(from.name()) != bl_nodes.end()
730  || bl_nodes.find(to.name()) != bl_nodes.end()
731  || bl_edges.find(std::make_pair(to.name(), from.name())) != bl_edges.end()
732  || bl_edges.find(std::make_pair(from.name(), to.name())) != bl_edges.end()) {
733  arrow.color.r = arrow.color.g = arrow.color.b = 0.5;
734  arrow.scale.x = 0.04; // shaft radius
735  arrow.scale.y = 0.15; // head radius
736 
737  tf::Vector3 p1v(p1.x, p1.y, p1.z);
738  tf::Vector3 p2v(p2.x, p2.y, p2.z);
739 
740  tf::Vector3 p = p1v + (p2v - p1v) * 0.5;
741 
742  std::string text_s = "";
743 
744  std::map<std::pair<std::string, std::string>, std::string>::iterator e =
745  bl_edges.find(std::make_pair(to.name(), from.name()));
746  if (e != bl_edges.end()) {
747  text_s = e->second;
748  } else {
749  e = bl_edges.find(std::make_pair(from.name(), to.name()));
750  if (e != bl_edges.end()) {
751  text_s = e->second;
752  }
753  }
754 
755  visualization_msgs::Marker text;
756  text.header.frame_id = cfg_global_frame_;
757  text.header.stamp = ros::Time::now();
758  text.ns = "navgraph-constraints";
759  text.id = constraints_id_num++;
760  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
761  text.action = visualization_msgs::Marker::ADD;
762  text.pose.position.x = p[0];
763  text.pose.position.y = p[1];
764  text.pose.position.z = 0.3;
765  text.pose.orientation.w = 1.;
766  text.scale.z = 0.12;
767  text.color.r = 1.0;
768  text.color.g = text.color.b = 0.f;
769  text.color.a = 1.0;
770  text.lifetime = ros::Duration(0, 0);
771  text.text = text_s;
772  m.markers.push_back(text);
773 
774  } else {
775  // regular
776  arrow.color.r = 0.66666;
777  if (cost_factor >= 1.00001) {
778  arrow.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_) * 0.66666;
779  } else {
780  arrow.color.g = 0.f;
781  }
782  arrow.color.b = 0.f;
783  arrow.scale.x = 0.04; // shaft radius
784  arrow.scale.y = 0.15; // head radius
785  }
786  }
787  m.markers.push_back(arrow);
788  } else {
789  if ((plan_to_ == from.name() && plan_from_ == to.name())
790  || (plan_from_ == from.name() && plan_to_ == to.name())) {
791  // it's the current line
792  cur_line.points.push_back(p1);
793  cur_line.points.push_back(p2);
794  } else {
795  bool in_plan = false;
796  if (traversal_) {
797  for (size_t p = 0; p < traversal_.path().nodes().size(); ++p) {
798  if (traversal_.path().nodes()[p] == from
799  && ((p > 0 && traversal_.path().nodes()[p - 1] == to)
800  || (p < traversal_.path().nodes().size() - 1
801  && traversal_.path().nodes()[p + 1] == to))) {
802  in_plan = true;
803  break;
804  }
805  }
806  }
807 
808  if (in_plan) {
809  // it's in the current plan
810  if (cost_factor >= 1.00001) {
811  visualization_msgs::Marker line;
812  line.header.frame_id = cfg_global_frame_;
813  line.header.stamp = ros::Time::now();
814  line.ns = "navgraph";
815  line.id = id_num++;
816  line.type = visualization_msgs::Marker::LINE_STRIP;
817  line.action = visualization_msgs::Marker::ADD;
818  line.color.a = 1.0;
819  line.lifetime = ros::Duration(0, 0);
820  line.points.push_back(p1);
821  line.points.push_back(p2);
822  line.color.r = 1.f;
823  line.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_);
824  line.color.b = 0.f;
825  line.scale.x = 0.035;
826  m.markers.push_back(line);
827  } else {
828  plan_lines.points.push_back(p1);
829  plan_lines.points.push_back(p2);
830  }
831  } else if (bl_nodes.find(from.name()) != bl_nodes.end()
832  || bl_nodes.find(to.name()) != bl_nodes.end()) {
833  blocked_lines.points.push_back(p1);
834  blocked_lines.points.push_back(p2);
835 
836  } else if (bl_edges.find(std::make_pair(to.name(), from.name())) != bl_edges.end()
837  || bl_edges.find(std::make_pair(from.name(), to.name())) != bl_edges.end()) {
838  blocked_lines.points.push_back(p1);
839  blocked_lines.points.push_back(p2);
840 
841  tf::Vector3 p1v(p1.x, p1.y, p1.z);
842  tf::Vector3 p2v(p2.x, p2.y, p2.z);
843 
844  tf::Vector3 p = p1v + (p2v - p1v) * 0.5;
845 
846  std::string text_s = "";
847 
848  std::map<std::pair<std::string, std::string>, std::string>::iterator e =
849  bl_edges.find(std::make_pair(to.name(), from.name()));
850  if (e != bl_edges.end()) {
851  text_s = e->second;
852  } else {
853  e = bl_edges.find(std::make_pair(from.name(), to.name()));
854  if (e != bl_edges.end()) {
855  text_s = e->second;
856  }
857  }
858 
859  visualization_msgs::Marker text;
860  text.header.frame_id = cfg_global_frame_;
861  text.header.stamp = ros::Time::now();
862  text.ns = "navgraph-constraints";
863  text.id = constraints_id_num++;
864  text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
865  text.action = visualization_msgs::Marker::ADD;
866  text.pose.position.x = p[0];
867  text.pose.position.y = p[1];
868  text.pose.position.z = 0.3;
869  text.pose.orientation.w = 1.;
870  text.scale.z = 0.12;
871  text.color.r = 1.0;
872  text.color.g = text.color.b = 0.f;
873  text.color.a = 1.0;
874  text.lifetime = ros::Duration(0, 0);
875  text.text = text_s;
876  m.markers.push_back(text);
877 
878  } else {
879  if (cost_factor >= 1.00001) {
880  visualization_msgs::Marker line;
881  line.header.frame_id = cfg_global_frame_;
882  line.header.stamp = ros::Time::now();
883  line.ns = "navgraph";
884  line.id = id_num++;
885  line.type = visualization_msgs::Marker::LINE_STRIP;
886  line.action = visualization_msgs::Marker::ADD;
887  line.color.a = 1.0;
888  line.lifetime = ros::Duration(0, 0);
889  line.points.push_back(p1);
890  line.points.push_back(p2);
891  line.color.r = 0.66666;
892  line.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_) * 0.66666;
893  line.color.b = 0.f;
894  line.scale.x = 0.02;
895  m.markers.push_back(line);
896  } else {
897  lines.points.push_back(p1);
898  lines.points.push_back(p2);
899  }
900  }
901  }
902  }
903  }
904  }
905 
906  m.markers.push_back(lines);
907  m.markers.push_back(plan_lines);
908  m.markers.push_back(blocked_lines);
909  m.markers.push_back(cur_line);
910 
911  crepo_.lock();
912  const NavGraphConstraintRepo::NodeConstraintList &node_constraints = crepo_->node_constraints();
913  const NavGraphConstraintRepo::EdgeConstraintList &edge_constraints = crepo_->edge_constraints();
914  std::list<const NavGraphPolygonConstraint *> poly_constraints;
915 
916  std::for_each(node_constraints.begin(),
917  node_constraints.end(),
918  [&poly_constraints](const NavGraphNodeConstraint *c) {
919  const NavGraphPolygonNodeConstraint *pc =
920  dynamic_cast<const NavGraphPolygonNodeConstraint *>(c);
921  if (pc) {
922  poly_constraints.push_back(pc);
923  }
924  });
925 
926  std::for_each(edge_constraints.begin(),
927  edge_constraints.end(),
928  [&poly_constraints](const NavGraphEdgeConstraint *c) {
929  const NavGraphPolygonEdgeConstraint *pc =
930  dynamic_cast<const NavGraphPolygonEdgeConstraint *>(c);
931  if (pc) {
932  poly_constraints.push_back(pc);
933  }
934  });
935 
936  for (const NavGraphPolygonConstraint *pc : poly_constraints) {
937  const NavGraphPolygonConstraint::PolygonMap &polygons = pc->polygons();
938  for (auto const &p : polygons) {
939  visualization_msgs::Marker polc_lines;
940  polc_lines.header.frame_id = cfg_global_frame_;
941  polc_lines.header.stamp = ros::Time::now();
942  polc_lines.ns = "navgraph-constraints";
943  polc_lines.id = constraints_id_num++;
944  polc_lines.type = visualization_msgs::Marker::LINE_STRIP;
945  polc_lines.action = visualization_msgs::Marker::ADD;
946  polc_lines.color.r = polc_lines.color.g = 1.0;
947  polc_lines.color.b = 0.f;
948  polc_lines.color.a = 1.0;
949  polc_lines.scale.x = 0.02;
950  polc_lines.lifetime = ros::Duration(0, 0);
951 
952  polc_lines.points.resize(p.second.size());
953  for (size_t i = 0; i < p.second.size(); ++i) {
954  polc_lines.points[i].x = p.second[i].x;
955  polc_lines.points[i].y = p.second[i].y;
956  polc_lines.points[i].z = 0.;
957  }
958 
959  m.markers.push_back(polc_lines);
960  }
961  }
962  crepo_.unlock();
963 
964 #if !ROS_VERSION_MINIMUM(1, 10, 0)
965  for (size_t i = id_num; i < last_id_num_; ++i) {
966  visualization_msgs::Marker delop;
967  delop.header.frame_id = cfg_global_frame_;
968  delop.header.stamp = ros::Time::now();
969  delop.ns = "navgraph";
970  delop.id = i;
971  delop.action = visualization_msgs::Marker::DELETE;
972  m.markers.push_back(delop);
973  }
974 
975  for (size_t i = constraints_id_num; i < constraints_last_id_num_; ++i) {
976  visualization_msgs::Marker delop;
977  delop.header.frame_id = cfg_global_frame_;
978  delop.header.stamp = ros::Time::now();
979  delop.ns = "navgraph-constraints";
980  delop.id = i;
981  delop.action = visualization_msgs::Marker::DELETE;
982  m.markers.push_back(delop);
983  }
984 #endif
985 
986  last_id_num_ = id_num;
987  constraints_last_id_num_ = constraints_id_num;
988 
989  markers_ = m;
990 }
991 
992 void
993 NavGraphVisualizationThread::publish()
994 {
995  vispub_.publish(markers_);
996 }
void set_constraint_repo(fawkes::LockPtr< fawkes::NavGraphConstraintRepo > &crepo)
Set the constraint repo.
virtual void loop()
Code to execute in the thread.
virtual void graph_changed() noexcept
Function called if the graph has been changed.
virtual void finalize()
Finalize the thread.
void set_current_edge(const std::string &from, const std::string &to)
Set the currently executed edge of the plan.
void set_graph(fawkes::LockPtr< fawkes::NavGraph > &graph)
Set the graph.
virtual void init()
Initialize the thread.
void set_traversal(fawkes::NavGraphPath::Traversal &traversal)
Set current path.
void reset_plan()
Reset the current plan.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
LockPtr<> is a reference-counting shared lockable smartpointer.
Definition: lockptr.h:55
void lock() const
Lock access to the encapsulated object.
Definition: lockptr.h:257
void unlock() const
Unlock object mutex.
Definition: lockptr.h:273
const EdgeConstraintList & edge_constraints() const
Get a list of registered edge constraints.
std::vector< fawkes::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
std::vector< fawkes::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge constraints.
const NodeConstraintList & node_constraints() const
Get a list of registered node constraints.
float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Get the highest increasing cost factor for an edge.
NavGraphNodeConstraint * blocks(const fawkes::NavGraphNode &node)
Check if any constraint in the repo blocks the node.
Constraint that can be queried to check if an edge is blocked.
Topological graph edge.
Definition: navgraph_edge.h:38
bool is_directed() const
Check if edge is directed.
const std::string & to() const
Get edge target node name.
Definition: navgraph_edge.h:62
const std::string & from() const
Get edge originating node name.
Definition: navgraph_edge.h:54
Constraint that can be queried to check if a node is blocked.
Topological graph node.
Definition: navgraph_node.h:36
float x() const
Get X coordinate in global frame.
Definition: navgraph_node.h:58
const std::string & name() const
Get name of node.
Definition: navgraph_node.h:50
float property_as_float(const std::string &prop) const
Get property converted to float.
bool has_property(const std::string &property) const
Check if node has specified property.
float y() const
Get Y coordinate in global frame.
Definition: navgraph_node.h:66
Sub-class representing a navgraph path traversal.
Definition: navgraph_path.h:40
bool last() const
Check if the current node is the last node in the path.
void invalidate()
Invalidate this traversal.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
Definition: navgraph_path.h:64
size_t remaining() const
Get the number of remaining nodes in path traversal.
bool empty() const
Check if path is empty.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
size_t size() const
Get size of path.
bool contains(const NavGraphNode &node) const
Check if the path contains a given node.
const NavGraphNode & goal() const
Get goal of path.
Constraint that blocks nodes within and edges touching a polygon.
std::map< PolygonHandle, Polygon > PolygonMap
Map for accessing all polygons at once with their handles.
const std::map< std::string, std::string > & default_properties() const
Get all default properties.
Definition: navgraph.cpp:759
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
Definition: navgraph.cpp:133
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
Definition: navgraph.cpp:142
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
void wakeup()
Wake up thread.
Definition: thread.cpp:995
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
Definition: coord.h:72