22 #include "visualization_thread.h"
24 #include "aspect/blocked_timing.h"
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>
32 #include <utils/math/angle.h>
33 #include <utils/math/coord.h>
42 typedef std::multimap<std::string, std::string> ConnMap;
56 vispub_ =
rosnode->advertise<visualization_msgs::MarkerArray>(
"visualization_marker_array",
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");
69 cfg_cost_scale_max_ -= 1.0;
71 last_id_num_ = constraints_last_id_num_ = 0;
78 visualization_msgs::MarkerArray m;
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";
87 m.markers.push_back(delop);
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";
95 delop.action = visualization_msgs::Marker::DELETE;
96 m.markers.push_back(delop);
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";
104 delop.action = visualization_msgs::Marker::DELETE;
105 m.markers.push_back(delop);
121 plan_to_ = plan_from_ =
"";
140 traversal_ = traversal;
141 plan_to_ = plan_from_ =
"";
150 plan_to_ = plan_from_ =
"";
161 if (plan_from_ != from || plan_to_ != to) {
181 NavGraphVisualizationThread::edge_cost_factor(
182 std::list<std::tuple<std::string, std::string, std::string, float>> &costs,
183 const std::string & from,
184 const std::string & to,
185 std::string & constraint_name)
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);
199 NavGraphVisualizationThread::add_circle_markers(visualization_msgs::MarkerArray &m,
204 unsigned int arc_length,
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();
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;
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;
229 arc.points[j].x = center_x + circ_x;
230 arc.points[j].y = center_y + circ_y;
231 arc.points[j].z = 0.;
233 m.markers.push_back(arc);
238 NavGraphVisualizationThread::regenerate()
244 std::vector<fawkes::NavGraphNode> nodes = graph_->
nodes();
245 std::vector<fawkes::NavGraphEdge> edges = graph_->
edges();
249 std::map<std::string, fawkes::NavGraphNode> nodes_map;
251 nodes_map[n.name()] = n;
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 =
262 size_t constraints_id_num = 0;
264 visualization_msgs::MarkerArray m;
266 #if ROS_VERSION_MINIMUM(1, 10, 0)
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";
274 m.markers.push_back(delop);
278 visualization_msgs::Marker lines;
279 lines.header.frame_id = cfg_global_frame_;
280 lines.header.stamp = ros::Time::now();
281 lines.ns =
"navgraph";
283 lines.type = visualization_msgs::Marker::LINE_LIST;
284 lines.action = visualization_msgs::Marker::ADD;
286 lines.color.g = lines.color.b = 0.f;
288 lines.scale.x = 0.02;
289 lines.lifetime = ros::Duration(0, 0);
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);
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);
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);
329 for (
size_t i = 0; i < nodes.size(); ++i) {
330 bool is_in_plan = traversal_ && traversal_.
path().
contains(nodes[i]);
332 traversal_ && (traversal_.
path().
size() >= 1) && (traversal_.
path().
goal() == nodes[i]);
334 bool is_active = (plan_to_ == nodes[i].name());
336 std::string ns =
"navgraph";
337 if (nodes[i].has_property(
"group")) {
338 ns +=
"-" + nodes[i].property(
"group");
341 visualization_msgs::Marker sphere;
342 sphere.header.frame_id = cfg_global_frame_;
343 sphere.header.stamp = ros::Time::now();
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;
355 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.1;
357 sphere.color.r = 0.f;
358 sphere.color.g = 1.f;
360 sphere.color.r = 1.f;
361 sphere.color.g = 0.f;
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;
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.;
381 text.color.g = text.color.b = 0.f;
383 text.lifetime = ros::Duration(0, 0);
384 text.text = bl_nodes[nodes[i].name()];
385 m.markers.push_back(text);
388 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.05;
389 sphere.color.r = 0.5;
390 sphere.color.b = 0.f;
392 sphere.color.a = 1.0;
393 sphere.lifetime = ros::Duration(0, 0);
394 m.markers.push_back(sphere);
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"]);
403 if (target_tolerance > 0.) {
404 add_circle_markers(m,
413 is_active ? sphere.color.a : 0.4);
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"]);
422 if (travel_tolerance > 0.) {
423 add_circle_markers(m,
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"]);
443 if (shortcut_tolerance > 0.) {
444 add_circle_markers(m,
457 if (nodes[i].has_property(
"orientation")) {
458 float ori = nodes[i].property_as_float(
"orientation");
460 visualization_msgs::Marker arrow;
461 arrow.header.frame_id = cfg_global_frame_;
462 arrow.header.stamp = ros::Time::now();
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;
480 arrow.color.r = arrow.color.g = 1.f;
490 arrow.lifetime = ros::Duration(0, 0);
491 m.markers.push_back(arrow);
494 visualization_msgs::Marker text;
495 text.header.frame_id = cfg_global_frame_;
496 text.header.stamp = ros::Time::now();
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.;
506 text.color.r = text.color.g = text.color.b = 1.0f;
508 text.lifetime = ros::Duration(0, 0);
509 text.text = nodes[i].name();
510 m.markers.push_back(text);
513 if (traversal_ && !traversal_.
path().
empty()
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;
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);
539 visualization_msgs::Marker text;
540 text.header.frame_id = cfg_global_frame_;
541 text.header.stamp = ros::Time::now();
542 text.ns =
"navgraph";
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.;
551 text.color.r = text.color.g = text.color.b = 1.0f;
553 text.lifetime = ros::Duration(0, 0);
554 text.text =
"Free Target";
555 m.markers.push_back(text);
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);
585 float target_tolerance = 0.;
588 }
else if (default_props.find(
"target_tolerance") != default_props.end()) {
589 target_tolerance = StringConversions::to_float(default_props[
"target_tolerance"]);
591 if (target_tolerance > 0.) {
592 add_circle_markers(m,
601 (traversal_.
last()) ? sphere.color.a : 0.5);
604 float shortcut_tolerance = 0.;
607 }
else if (default_props.find(
"shortcut_tolerance") != default_props.end()) {
608 shortcut_tolerance = StringConversions::to_float(default_props[
"shortcut_tolerance"]);
610 if (shortcut_tolerance > 0.) {
611 add_circle_markers(m,
626 geometry_msgs::Point p1;
627 p1.x = last_graph_node.
x();
628 p1.y = last_graph_node.
y();
631 geometry_msgs::Point p2;
632 p2.x = target_node.
x();
633 p2.y = target_node.
y();
636 visualization_msgs::Marker arrow;
637 arrow.header.frame_id = cfg_global_frame_;
638 arrow.header.stamp = ros::Time::now();
639 arrow.ns =
"navgraph";
641 arrow.type = visualization_msgs::Marker::ARROW;
642 arrow.action = visualization_msgs::Marker::ADD;
644 arrow.lifetime = ros::Duration(0, 0);
645 arrow.points.push_back(p1);
646 arrow.points.push_back(p2);
648 if (plan_to_ == target_node.
name()) {
650 arrow.color.r = arrow.color.g = 1.f;
657 arrow.color.g = 0.5f;
659 arrow.scale.x = 0.07;
662 m.markers.push_back(arrow);
666 for (
size_t i = 0; i < edges.size(); ++i) {
668 if (nodes_map.find(edge.
from()) != nodes_map.end()
669 && nodes_map.find(edge.
to()) != nodes_map.end()) {
673 geometry_msgs::Point p1;
678 geometry_msgs::Point p2;
683 std::string cost_cstr_name;
684 float cost_factor = edge_cost_factor(edge_cfs, from.
name(), to.
name(), cost_cstr_name);
687 visualization_msgs::Marker arrow;
688 arrow.header.frame_id = cfg_global_frame_;
689 arrow.header.stamp = ros::Time::now();
690 arrow.ns =
"navgraph";
692 arrow.type = visualization_msgs::Marker::ARROW;
693 arrow.action = visualization_msgs::Marker::ADD;
695 arrow.lifetime = ros::Duration(0, 0);
696 arrow.points.push_back(p1);
697 arrow.points.push_back(p2);
699 if (plan_from_ == from.
name() && plan_to_ == to.
name()) {
702 arrow.color.r = arrow.color.b = 0.f;
706 bool in_plan =
false;
708 for (
size_t p = 0; p < traversal_.
path().nodes().size(); ++p) {
710 && (p < traversal_.
path().
nodes().size() - 1
711 && traversal_.
path().
nodes()[p + 1] == to)) {
721 if (cost_factor >= 1.00001) {
722 arrow.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_);
727 arrow.scale.x = 0.07;
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;
735 arrow.scale.y = 0.15;
737 tf::Vector3 p1v(p1.x, p1.y, p1.z);
738 tf::Vector3 p2v(p2.x, p2.y, p2.z);
740 tf::Vector3 p = p1v + (p2v - p1v) * 0.5;
742 std::string text_s =
"";
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()) {
749 e = bl_edges.find(std::make_pair(from.
name(), to.
name()));
750 if (e != bl_edges.end()) {
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.;
768 text.color.g = text.color.b = 0.f;
770 text.lifetime = ros::Duration(0, 0);
772 m.markers.push_back(text);
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;
783 arrow.scale.x = 0.04;
784 arrow.scale.y = 0.15;
787 m.markers.push_back(arrow);
789 if ((plan_to_ == from.
name() && plan_from_ == to.
name())
790 || (plan_from_ == from.
name() && plan_to_ == to.
name())) {
792 cur_line.points.push_back(p1);
793 cur_line.points.push_back(p2);
795 bool in_plan =
false;
797 for (
size_t p = 0; p < traversal_.
path().nodes().size(); ++p) {
799 && ((p > 0 && traversal_.
path().
nodes()[p - 1] == to)
800 || (p < traversal_.
path().
nodes().size() - 1
801 && traversal_.
path().
nodes()[p + 1] == to))) {
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";
816 line.type = visualization_msgs::Marker::LINE_STRIP;
817 line.action = visualization_msgs::Marker::ADD;
819 line.lifetime = ros::Duration(0, 0);
820 line.points.push_back(p1);
821 line.points.push_back(p2);
823 line.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_);
825 line.scale.x = 0.035;
826 m.markers.push_back(line);
828 plan_lines.points.push_back(p1);
829 plan_lines.points.push_back(p2);
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);
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);
841 tf::Vector3 p1v(p1.x, p1.y, p1.z);
842 tf::Vector3 p2v(p2.x, p2.y, p2.z);
844 tf::Vector3 p = p1v + (p2v - p1v) * 0.5;
846 std::string text_s =
"";
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()) {
853 e = bl_edges.find(std::make_pair(from.
name(), to.
name()));
854 if (e != bl_edges.end()) {
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.;
872 text.color.g = text.color.b = 0.f;
874 text.lifetime = ros::Duration(0, 0);
876 m.markers.push_back(text);
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";
885 line.type = visualization_msgs::Marker::LINE_STRIP;
886 line.action = visualization_msgs::Marker::ADD;
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;
895 m.markers.push_back(line);
897 lines.points.push_back(p1);
898 lines.points.push_back(p2);
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);
914 std::list<const NavGraphPolygonConstraint *> poly_constraints;
916 std::for_each(node_constraints.begin(),
917 node_constraints.end(),
919 const NavGraphPolygonNodeConstraint *pc =
920 dynamic_cast<const NavGraphPolygonNodeConstraint *>(c);
922 poly_constraints.push_back(pc);
926 std::for_each(edge_constraints.begin(),
927 edge_constraints.end(),
929 const NavGraphPolygonEdgeConstraint *pc =
930 dynamic_cast<const NavGraphPolygonEdgeConstraint *>(c);
932 poly_constraints.push_back(pc);
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);
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.;
959 m.markers.push_back(polc_lines);
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";
971 delop.action = visualization_msgs::Marker::DELETE;
972 m.markers.push_back(delop);
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";
981 delop.action = visualization_msgs::Marker::DELETE;
982 m.markers.push_back(delop);
986 last_id_num_ = id_num;
987 constraints_last_id_num_ = constraints_id_num;
993 NavGraphVisualizationThread::publish()
995 vispub_.publish(markers_);