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
35using 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
42typedef 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
53void
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
75void
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 */
116void
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 */
128void
130{
131 crepo_ = crepo;
132}
133
134/** Set current path.
135 * @param traversal currently active path traversal
136 */
137void
139{
140 traversal_ = traversal;
141 plan_to_ = plan_from_ = "";
142 regenerate();
143}
144
145/** Reset the current plan. */
146void
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 */
158void
159NavGraphVisualizationThread::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
168void
170{
171 regenerate();
172}
173
174void
176{
177 publish();
178}
179
180float
181NavGraphVisualizationThread::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
198void
199NavGraphVisualizationThread::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
237void
238NavGraphVisualizationThread::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
992void
993NavGraphVisualizationThread::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.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
Definition: navgraph_path.h:64
void invalidate()
Invalidate this traversal.
size_t remaining() const
Get the number of remaining nodes in path traversal.
bool empty() const
Check if path is empty.
size_t size() const
Get size of path.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the 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