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