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