22 #include "visualization_thread.h" 24 #include <navgraph/constraints/constraint_repo.h> 25 #include <navgraph/constraints/polygon_edge_constraint.h> 26 #include <navgraph/constraints/polygon_node_constraint.h> 27 #include <navgraph/navgraph.h> 30 #include <utils/math/angle.h> 31 #include <utils/math/coord.h> 40 typedef std::multimap<std::string, std::string> ConnMap;
54 vispub_ =
rosnode->advertise<visualization_msgs::MarkerArray>(
"visualization_marker_array",
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");
67 cfg_cost_scale_max_ -= 1.0;
69 last_id_num_ = constraints_last_id_num_ = 0;
76 visualization_msgs::MarkerArray m;
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";
85 m.markers.push_back(delop);
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";
93 delop.action = visualization_msgs::Marker::DELETE;
94 m.markers.push_back(delop);
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";
102 delop.action = visualization_msgs::Marker::DELETE;
103 m.markers.push_back(delop);
119 plan_to_ = plan_from_ =
"";
138 traversal_ = traversal;
139 plan_to_ = plan_from_ =
"";
148 plan_to_ = plan_from_ =
"";
159 if (plan_from_ != from || plan_to_ != to) {
179 NavGraphVisualizationThread::edge_cost_factor(
180 std::list<std::tuple<std::string, std::string, std::string, float>> &costs,
181 const std::string & from,
182 const std::string & to,
183 std::string & constraint_name)
185 for (
const std::tuple<std::string, std::string, std::string, float> &c : costs) {
186 if ((std::get<0>(c) == from && std::get<1>(c) == to)
187 || (std::get<0>(c) == to && std::get<1>(c) == from)) {
188 constraint_name = std::get<2>(c);
189 return std::get<3>(c);
197 NavGraphVisualizationThread::add_circle_markers(visualization_msgs::MarkerArray &m,
202 unsigned int arc_length,
209 for (
unsigned int a = 0; a < 360; a += 2 * arc_length) {
210 visualization_msgs::Marker arc;
211 arc.header.frame_id = cfg_global_frame_;
212 arc.header.stamp = ros::Time::now();
215 arc.type = visualization_msgs::Marker::LINE_STRIP;
216 arc.action = visualization_msgs::Marker::ADD;
217 arc.scale.x = arc.scale.y = arc.scale.z = line_width;
222 arc.lifetime = ros::Duration(0, 0);
223 arc.points.resize(arc_length);
224 for (
unsigned int j = 0; j < arc_length; ++j) {
225 float circ_x = 0, circ_y = 0;
227 arc.points[j].x = center_x + circ_x;
228 arc.points[j].y = center_y + circ_y;
229 arc.points[j].z = 0.;
231 m.markers.push_back(arc);
236 NavGraphVisualizationThread::publish()
242 std::vector<fawkes::NavGraphNode> nodes = graph_->
nodes();
243 std::vector<fawkes::NavGraphEdge> edges = graph_->
edges();
247 std::map<std::string, fawkes::NavGraphNode> nodes_map;
249 nodes_map[n.name()] = n;
253 std::map<std::string, std::string> bl_nodes = crepo_->
blocks(nodes);
254 std::map<std::pair<std::string, std::string>, std::string> bl_edges = crepo_->
blocks(edges);
255 std::list<std::tuple<std::string, std::string, std::string, float>> edge_cfs =
260 size_t constraints_id_num = 0;
262 visualization_msgs::MarkerArray m;
264 #if ROS_VERSION_MINIMUM(1, 10, 0) 266 visualization_msgs::Marker delop;
267 delop.header.frame_id = cfg_global_frame_;
268 delop.header.stamp = ros::Time::now();
269 delop.ns =
"navgraph-constraints";
272 m.markers.push_back(delop);
276 visualization_msgs::Marker lines;
277 lines.header.frame_id = cfg_global_frame_;
278 lines.header.stamp = ros::Time::now();
279 lines.ns =
"navgraph";
281 lines.type = visualization_msgs::Marker::LINE_LIST;
282 lines.action = visualization_msgs::Marker::ADD;
284 lines.color.g = lines.color.b = 0.f;
286 lines.scale.x = 0.02;
287 lines.lifetime = ros::Duration(0, 0);
289 visualization_msgs::Marker plan_lines;
290 plan_lines.header.frame_id = cfg_global_frame_;
291 plan_lines.header.stamp = ros::Time::now();
292 plan_lines.ns =
"navgraph";
293 plan_lines.id = id_num++;
294 plan_lines.type = visualization_msgs::Marker::LINE_LIST;
295 plan_lines.action = visualization_msgs::Marker::ADD;
296 plan_lines.color.r = 1.0;
297 plan_lines.color.g = plan_lines.color.b = 0.f;
298 plan_lines.color.a = 1.0;
299 plan_lines.scale.x = 0.035;
300 plan_lines.lifetime = ros::Duration(0, 0);
302 visualization_msgs::Marker blocked_lines;
303 blocked_lines.header.frame_id = cfg_global_frame_;
304 blocked_lines.header.stamp = ros::Time::now();
305 blocked_lines.ns =
"navgraph";
306 blocked_lines.id = id_num++;
307 blocked_lines.type = visualization_msgs::Marker::LINE_LIST;
308 blocked_lines.action = visualization_msgs::Marker::ADD;
309 blocked_lines.color.r = blocked_lines.color.g = blocked_lines.color.b = 0.5;
310 blocked_lines.color.a = 1.0;
311 blocked_lines.scale.x = 0.02;
312 blocked_lines.lifetime = ros::Duration(0, 0);
314 visualization_msgs::Marker cur_line;
315 cur_line.header.frame_id = cfg_global_frame_;
316 cur_line.header.stamp = ros::Time::now();
317 cur_line.ns =
"navgraph";
318 cur_line.id = id_num++;
319 cur_line.type = visualization_msgs::Marker::LINE_LIST;
320 cur_line.action = visualization_msgs::Marker::ADD;
321 cur_line.color.g = 1.f;
322 cur_line.color.r = cur_line.color.b = 0.f;
323 cur_line.color.a = 1.0;
324 cur_line.scale.x = 0.05;
325 cur_line.lifetime = ros::Duration(0, 0);
327 for (
size_t i = 0; i < nodes.size(); ++i) {
328 bool is_in_plan = traversal_ && traversal_.
path().
contains(nodes[i]);
330 traversal_ && (traversal_.
path().
size() >= 1) && (traversal_.
path().
goal() == nodes[i]);
332 bool is_active = (plan_to_ == nodes[i].name());
334 visualization_msgs::Marker sphere;
335 sphere.header.frame_id = cfg_global_frame_;
336 sphere.header.stamp = ros::Time::now();
337 sphere.ns =
"navgraph";
338 sphere.id = id_num++;
339 sphere.type = visualization_msgs::Marker::SPHERE;
340 sphere.action = visualization_msgs::Marker::ADD;
341 sphere.pose.position.x = nodes[i].x();
342 sphere.pose.position.y = nodes[i].y();
343 sphere.pose.position.z = 0.;
344 sphere.pose.orientation.w = 1.;
345 sphere.scale.y = 0.05;
346 sphere.scale.z = 0.05;
348 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.1;
350 sphere.color.r = 0.f;
351 sphere.color.g = 1.f;
353 sphere.color.r = 1.f;
354 sphere.color.g = 0.f;
356 sphere.color.b = 0.f;
357 }
else if (bl_nodes.find(nodes[i].name()) != bl_nodes.end()) {
358 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.05;
359 sphere.color.r = sphere.color.g = sphere.color.b = 0.5;
361 visualization_msgs::Marker text;
362 text.header.frame_id = cfg_global_frame_;
363 text.header.stamp = ros::Time::now();
364 text.ns =
"navgraph-constraints";
365 text.id = constraints_id_num++;
366 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
367 text.action = visualization_msgs::Marker::ADD;
368 text.pose.position.x = nodes[i].x();
369 text.pose.position.y = nodes[i].y();
370 text.pose.position.z = 0.3;
371 text.pose.orientation.w = 1.;
374 text.color.g = text.color.b = 0.f;
376 text.lifetime = ros::Duration(0, 0);
377 text.text = bl_nodes[nodes[i].name()];
378 m.markers.push_back(text);
381 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.05;
382 sphere.color.r = 0.5;
383 sphere.color.b = 0.f;
385 sphere.color.a = 1.0;
386 sphere.lifetime = ros::Duration(0, 0);
387 m.markers.push_back(sphere);
390 float target_tolerance = 0.;
391 if (nodes[i].has_property(
"target_tolerance")) {
392 target_tolerance = nodes[i].property_as_float(
"target_tolerance");
393 }
else if (default_props.find(
"target_tolerance") != default_props.end()) {
394 target_tolerance = StringConversions::to_float(default_props[
"target_tolerance"]);
396 if (target_tolerance > 0.) {
397 add_circle_markers(m,
406 is_active ? sphere.color.a : 0.4);
408 }
else if (is_active) {
409 float travel_tolerance = 0.;
410 if (nodes[i].has_property(
"travel_tolerance")) {
411 travel_tolerance = nodes[i].property_as_float(
"travel_tolerance");
412 }
else if (default_props.find(
"travel_tolerance") != default_props.end()) {
413 travel_tolerance = StringConversions::to_float(default_props[
"travel_tolerance"]);
415 if (travel_tolerance > 0.) {
416 add_circle_markers(m,
430 float shortcut_tolerance = 0.;
431 if (nodes[i].has_property(
"shortcut_tolerance")) {
432 shortcut_tolerance = nodes[i].property_as_float(
"shortcut_tolerance");
433 }
else if (default_props.find(
"shortcut_tolerance") != default_props.end()) {
434 shortcut_tolerance = StringConversions::to_float(default_props[
"shortcut_tolerance"]);
436 if (shortcut_tolerance > 0.) {
437 add_circle_markers(m,
450 if (nodes[i].has_property(
"orientation")) {
451 float ori = nodes[i].property_as_float(
"orientation");
453 visualization_msgs::Marker arrow;
454 arrow.header.frame_id = cfg_global_frame_;
455 arrow.header.stamp = ros::Time::now();
456 arrow.ns =
"navgraph";
458 arrow.type = visualization_msgs::Marker::ARROW;
459 arrow.action = visualization_msgs::Marker::ADD;
460 arrow.pose.position.x = nodes[i].x();
461 arrow.pose.position.y = nodes[i].y();
462 arrow.pose.position.z = 0.;
463 tf::Quaternion q = tf::create_quaternion_from_yaw(ori);
464 arrow.pose.orientation.x = q.x();
465 arrow.pose.orientation.y = q.y();
466 arrow.pose.orientation.z = q.z();
467 arrow.pose.orientation.w = q.w();
468 arrow.scale.x = 0.08;
469 arrow.scale.y = 0.02;
470 arrow.scale.z = 0.02;
473 arrow.color.r = arrow.color.g = 1.f;
483 arrow.lifetime = ros::Duration(0, 0);
484 m.markers.push_back(arrow);
487 visualization_msgs::Marker text;
488 text.header.frame_id = cfg_global_frame_;
489 text.header.stamp = ros::Time::now();
490 text.ns =
"navgraph";
492 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
493 text.action = visualization_msgs::Marker::ADD;
494 text.pose.position.x = nodes[i].x();
495 text.pose.position.y = nodes[i].y();
496 text.pose.position.z = 0.08;
497 text.pose.orientation.w = 1.;
499 text.color.r = text.color.g = text.color.b = 1.0f;
501 text.lifetime = ros::Duration(0, 0);
502 text.text = nodes[i].name();
503 m.markers.push_back(text);
506 if (traversal_ && !traversal_.
path().
empty()
511 visualization_msgs::Marker sphere;
512 sphere.header.frame_id = cfg_global_frame_;
513 sphere.header.stamp = ros::Time::now();
514 sphere.ns =
"navgraph";
515 sphere.id = id_num++;
516 sphere.type = visualization_msgs::Marker::SPHERE;
517 sphere.action = visualization_msgs::Marker::ADD;
518 sphere.pose.position.x = target_node.
x();
519 sphere.pose.position.y = target_node.
y();
520 sphere.pose.position.z = 0.;
521 sphere.pose.orientation.w = 1.;
522 sphere.scale.y = 0.05;
523 sphere.scale.z = 0.05;
524 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.1;
526 sphere.color.g = 0.5f;
527 sphere.color.b = 0.f;
528 sphere.color.a = 1.0;
529 sphere.lifetime = ros::Duration(0, 0);
530 m.markers.push_back(sphere);
532 visualization_msgs::Marker text;
533 text.header.frame_id = cfg_global_frame_;
534 text.header.stamp = ros::Time::now();
535 text.ns =
"navgraph";
537 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
538 text.action = visualization_msgs::Marker::ADD;
539 text.pose.position.x = target_node.
x();
540 text.pose.position.y = target_node.
y();
541 text.pose.position.z = 0.08;
542 text.pose.orientation.w = 1.;
544 text.color.r = text.color.g = text.color.b = 1.0f;
546 text.lifetime = ros::Duration(0, 0);
547 text.text =
"Free Target";
548 m.markers.push_back(text);
552 visualization_msgs::Marker ori_arrow;
553 ori_arrow.header.frame_id = cfg_global_frame_;
554 ori_arrow.header.stamp = ros::Time::now();
555 ori_arrow.ns =
"navgraph";
556 ori_arrow.id = id_num++;
557 ori_arrow.type = visualization_msgs::Marker::ARROW;
558 ori_arrow.action = visualization_msgs::Marker::ADD;
559 ori_arrow.pose.position.x = target_node.
x();
560 ori_arrow.pose.position.y = target_node.
y();
561 ori_arrow.pose.position.z = 0.;
562 tf::Quaternion q = tf::create_quaternion_from_yaw(ori);
563 ori_arrow.pose.orientation.x = q.x();
564 ori_arrow.pose.orientation.y = q.y();
565 ori_arrow.pose.orientation.z = q.z();
566 ori_arrow.pose.orientation.w = q.w();
567 ori_arrow.scale.x = 0.08;
568 ori_arrow.scale.y = 0.02;
569 ori_arrow.scale.z = 0.02;
570 ori_arrow.color.r = 1.f;
571 ori_arrow.color.g = 0.5f;
572 ori_arrow.color.b = 0.f;
573 ori_arrow.color.a = 1.0;
574 ori_arrow.lifetime = ros::Duration(0, 0);
575 m.markers.push_back(ori_arrow);
578 float target_tolerance = 0.;
581 }
else if (default_props.find(
"target_tolerance") != default_props.end()) {
582 target_tolerance = StringConversions::to_float(default_props[
"target_tolerance"]);
584 if (target_tolerance > 0.) {
585 add_circle_markers(m,
594 (traversal_.
last()) ? sphere.color.a : 0.5);
597 float shortcut_tolerance = 0.;
600 }
else if (default_props.find(
"shortcut_tolerance") != default_props.end()) {
601 shortcut_tolerance = StringConversions::to_float(default_props[
"shortcut_tolerance"]);
603 if (shortcut_tolerance > 0.) {
604 add_circle_markers(m,
619 geometry_msgs::Point p1;
620 p1.x = last_graph_node.
x();
621 p1.y = last_graph_node.
y();
624 geometry_msgs::Point p2;
625 p2.x = target_node.
x();
626 p2.y = target_node.
y();
629 visualization_msgs::Marker arrow;
630 arrow.header.frame_id = cfg_global_frame_;
631 arrow.header.stamp = ros::Time::now();
632 arrow.ns =
"navgraph";
634 arrow.type = visualization_msgs::Marker::ARROW;
635 arrow.action = visualization_msgs::Marker::ADD;
637 arrow.lifetime = ros::Duration(0, 0);
638 arrow.points.push_back(p1);
639 arrow.points.push_back(p2);
641 if (plan_to_ == target_node.
name()) {
643 arrow.color.r = arrow.color.g = 1.f;
650 arrow.color.g = 0.5f;
652 arrow.scale.x = 0.07;
655 m.markers.push_back(arrow);
659 for (
size_t i = 0; i < edges.size(); ++i) {
661 if (nodes_map.find(edge.
from()) != nodes_map.end()
662 && nodes_map.find(edge.
to()) != nodes_map.end()) {
666 geometry_msgs::Point p1;
671 geometry_msgs::Point p2;
676 std::string cost_cstr_name;
677 float cost_factor = edge_cost_factor(edge_cfs, from.
name(), to.
name(), cost_cstr_name);
680 visualization_msgs::Marker arrow;
681 arrow.header.frame_id = cfg_global_frame_;
682 arrow.header.stamp = ros::Time::now();
683 arrow.ns =
"navgraph";
685 arrow.type = visualization_msgs::Marker::ARROW;
686 arrow.action = visualization_msgs::Marker::ADD;
688 arrow.lifetime = ros::Duration(0, 0);
689 arrow.points.push_back(p1);
690 arrow.points.push_back(p2);
692 if (plan_from_ == from.
name() && plan_to_ == to.
name()) {
695 arrow.color.r = arrow.color.b = 0.f;
699 bool in_plan =
false;
701 for (
size_t p = 0; p < traversal_.
path().
nodes().size(); ++p) {
703 && (p < traversal_.
path().
nodes().size() - 1
704 && traversal_.
path().
nodes()[p + 1] == to)) {
714 if (cost_factor >= 1.00001) {
715 arrow.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_);
720 arrow.scale.x = 0.07;
722 }
else if (bl_nodes.find(from.
name()) != bl_nodes.end()
723 || bl_nodes.find(to.
name()) != bl_nodes.end()
724 || bl_edges.find(std::make_pair(to.
name(), from.
name())) != bl_edges.end()
725 || bl_edges.find(std::make_pair(from.
name(), to.
name())) != bl_edges.end()) {
726 arrow.color.r = arrow.color.g = arrow.color.b = 0.5;
727 arrow.scale.x = 0.04;
728 arrow.scale.y = 0.15;
730 tf::Vector3 p1v(p1.x, p1.y, p1.z);
731 tf::Vector3 p2v(p2.x, p2.y, p2.z);
733 tf::Vector3 p = p1v + (p2v - p1v) * 0.5;
735 std::string text_s =
"";
737 std::map<std::pair<std::string, std::string>, std::string>::iterator e =
738 bl_edges.find(std::make_pair(to.
name(), from.
name()));
739 if (e != bl_edges.end()) {
742 e = bl_edges.find(std::make_pair(from.
name(), to.
name()));
743 if (e != bl_edges.end()) {
748 visualization_msgs::Marker text;
749 text.header.frame_id = cfg_global_frame_;
750 text.header.stamp = ros::Time::now();
751 text.ns =
"navgraph-constraints";
752 text.id = constraints_id_num++;
753 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
754 text.action = visualization_msgs::Marker::ADD;
755 text.pose.position.x = p[0];
756 text.pose.position.y = p[1];
757 text.pose.position.z = 0.3;
758 text.pose.orientation.w = 1.;
761 text.color.g = text.color.b = 0.f;
763 text.lifetime = ros::Duration(0, 0);
765 m.markers.push_back(text);
769 arrow.color.r = 0.66666;
770 if (cost_factor >= 1.00001) {
771 arrow.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_) * 0.66666;
776 arrow.scale.x = 0.04;
777 arrow.scale.y = 0.15;
780 m.markers.push_back(arrow);
782 if ((plan_to_ == from.
name() && plan_from_ == to.
name())
783 || (plan_from_ == from.
name() && plan_to_ == to.
name())) {
785 cur_line.points.push_back(p1);
786 cur_line.points.push_back(p2);
788 bool in_plan =
false;
790 for (
size_t p = 0; p < traversal_.
path().
nodes().size(); ++p) {
792 && ((p > 0 && traversal_.
path().
nodes()[p - 1] == to)
793 || (p < traversal_.
path().
nodes().size() - 1
794 && traversal_.
path().
nodes()[p + 1] == to))) {
803 if (cost_factor >= 1.00001) {
804 visualization_msgs::Marker line;
805 line.header.frame_id = cfg_global_frame_;
806 line.header.stamp = ros::Time::now();
807 line.ns =
"navgraph";
809 line.type = visualization_msgs::Marker::LINE_STRIP;
810 line.action = visualization_msgs::Marker::ADD;
812 line.lifetime = ros::Duration(0, 0);
813 line.points.push_back(p1);
814 line.points.push_back(p2);
816 line.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_);
818 line.scale.x = 0.035;
819 m.markers.push_back(line);
821 plan_lines.points.push_back(p1);
822 plan_lines.points.push_back(p2);
824 }
else if (bl_nodes.find(from.
name()) != bl_nodes.end()
825 || bl_nodes.find(to.
name()) != bl_nodes.end()) {
826 blocked_lines.points.push_back(p1);
827 blocked_lines.points.push_back(p2);
829 }
else if (bl_edges.find(std::make_pair(to.
name(), from.
name())) != bl_edges.end()
830 || bl_edges.find(std::make_pair(from.
name(), to.
name())) != bl_edges.end()) {
831 blocked_lines.points.push_back(p1);
832 blocked_lines.points.push_back(p2);
834 tf::Vector3 p1v(p1.x, p1.y, p1.z);
835 tf::Vector3 p2v(p2.x, p2.y, p2.z);
837 tf::Vector3 p = p1v + (p2v - p1v) * 0.5;
839 std::string text_s =
"";
841 std::map<std::pair<std::string, std::string>, std::string>::iterator e =
842 bl_edges.find(std::make_pair(to.
name(), from.
name()));
843 if (e != bl_edges.end()) {
846 e = bl_edges.find(std::make_pair(from.
name(), to.
name()));
847 if (e != bl_edges.end()) {
852 visualization_msgs::Marker text;
853 text.header.frame_id = cfg_global_frame_;
854 text.header.stamp = ros::Time::now();
855 text.ns =
"navgraph-constraints";
856 text.id = constraints_id_num++;
857 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
858 text.action = visualization_msgs::Marker::ADD;
859 text.pose.position.x = p[0];
860 text.pose.position.y = p[1];
861 text.pose.position.z = 0.3;
862 text.pose.orientation.w = 1.;
865 text.color.g = text.color.b = 0.f;
867 text.lifetime = ros::Duration(0, 0);
869 m.markers.push_back(text);
872 if (cost_factor >= 1.00001) {
873 visualization_msgs::Marker line;
874 line.header.frame_id = cfg_global_frame_;
875 line.header.stamp = ros::Time::now();
876 line.ns =
"navgraph";
878 line.type = visualization_msgs::Marker::LINE_STRIP;
879 line.action = visualization_msgs::Marker::ADD;
881 line.lifetime = ros::Duration(0, 0);
882 line.points.push_back(p1);
883 line.points.push_back(p2);
884 line.color.r = 0.66666;
885 line.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_) * 0.66666;
888 m.markers.push_back(line);
890 lines.points.push_back(p1);
891 lines.points.push_back(p2);
899 m.markers.push_back(lines);
900 m.markers.push_back(plan_lines);
901 m.markers.push_back(blocked_lines);
902 m.markers.push_back(cur_line);
907 std::list<const NavGraphPolygonConstraint *> poly_constraints;
909 std::for_each(node_constraints.begin(),
910 node_constraints.end(),
913 dynamic_cast<const NavGraphPolygonNodeConstraint *>(c);
915 poly_constraints.push_back(pc);
919 std::for_each(edge_constraints.begin(),
920 edge_constraints.end(),
923 dynamic_cast<const NavGraphPolygonEdgeConstraint *>(c);
925 poly_constraints.push_back(pc);
931 for (
auto const &p : polygons) {
932 visualization_msgs::Marker polc_lines;
933 polc_lines.header.frame_id = cfg_global_frame_;
934 polc_lines.header.stamp = ros::Time::now();
935 polc_lines.ns =
"navgraph-constraints";
936 polc_lines.id = constraints_id_num++;
937 polc_lines.type = visualization_msgs::Marker::LINE_STRIP;
938 polc_lines.action = visualization_msgs::Marker::ADD;
939 polc_lines.color.r = polc_lines.color.g = 1.0;
940 polc_lines.color.b = 0.f;
941 polc_lines.color.a = 1.0;
942 polc_lines.scale.x = 0.02;
943 polc_lines.lifetime = ros::Duration(0, 0);
945 polc_lines.points.resize(p.second.size());
946 for (
size_t i = 0; i < p.second.size(); ++i) {
947 polc_lines.points[i].x = p.second[i].x;
948 polc_lines.points[i].y = p.second[i].y;
949 polc_lines.points[i].z = 0.;
952 m.markers.push_back(polc_lines);
957 #if !ROS_VERSION_MINIMUM(1, 10, 0) 958 for (
size_t i = id_num; i < last_id_num_; ++i) {
959 visualization_msgs::Marker delop;
960 delop.header.frame_id = cfg_global_frame_;
961 delop.header.stamp = ros::Time::now();
962 delop.ns =
"navgraph";
964 delop.action = visualization_msgs::Marker::DELETE;
965 m.markers.push_back(delop);
968 for (
size_t i = constraints_id_num; i < constraints_last_id_num_; ++i) {
969 visualization_msgs::Marker delop;
970 delop.header.frame_id = cfg_global_frame_;
971 delop.header.stamp = ros::Time::now();
972 delop.ns =
"navgraph-constraints";
974 delop.action = visualization_msgs::Marker::DELETE;
975 m.markers.push_back(delop);
979 last_id_num_ = id_num;
980 constraints_last_id_num_ = constraints_id_num;
virtual void finalize()
Finalize the thread.
const std::string & from() const
Get edge originating node name.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
size_t size() const
Get size of path.
NavGraphVisualizationThread()
Constructor.
Constraint that can be queried to check if an edge is blocked.
Constraint that blocks nodes inside a polygon.
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.
const std::map< std::string, std::string > & default_properties() const
Get all default properties.
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::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
bool is_directed() const
Check if edge is directed.
void unlock() const
Unlock object mutex.
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.
Thread class encapsulation of pthreads.
const NodeConstraintList & node_constraints() const
Get a list of registered node constraints.
Constraint that blocks nodes within and edges touching a polygon.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
std::map< PolygonHandle, Polygon > PolygonMap
Map for accessing all polygons at once with their handles.
LockPtr<> is a reference-counting shared lockable smartpointer.
bool last() const
Check if the current node is the last node in the path.
std::vector< fawkes::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge constraints.
void wakeup()
Wake up thread.
Base class for exceptions in Fawkes.
const std::string & to() const
Get edge target node name.
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.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
Constraint that blocks nodes within and edges touching a polygon.
Sub-class representing a navgraph path traversal.
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
float y() const
Get Y coordinate in global frame.
void set_coalesce_wakeups(bool coalesce=true)
Set wakeup coalescing.
bool empty() const
Check if path is empty.
virtual void init()
Initialize the thread.
float x() const
Get X coordinate in global frame.
void invalidate()
Invalidate this traversal.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
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.
Configuration * config
This is the Configuration member used to access the configuration.
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.
const PolygonMap & polygons() const
Get reference to the map of polygons.
void lock() const
Lock access to the encapsulated object.
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.