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_);
void set_constraint_repo(fawkes::LockPtr< fawkes::NavGraphConstraintRepo > &crepo)
Set the constraint repo.
virtual void graph_changed()
Function called if the graph has been changed.
NavGraphVisualizationThread()
Constructor.
virtual void loop()
Code to execute in the thread.
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.
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.
LockPtr<> is a reference-counting shared lockable smartpointer.
void lock() const
Lock access to the encapsulated object.
void unlock() const
Unlock object mutex.
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.
bool is_directed() const
Check if edge is directed.
const std::string & to() const
Get edge target node name.
const std::string & from() const
Get edge originating node name.
Constraint that can be queried to check if a node is blocked.
float x() const
Get X coordinate in global frame.
const std::string & name() const
Get name of node.
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.
Sub-class representing a navgraph path traversal.
bool last() const
Check if the current node is the last node in the path.
void invalidate()
Invalidate this traversal.
const NavGraphPath & path() const
Get parent path the traversal belongs to.
size_t remaining() const
Get the number of remaining nodes in path traversal.
bool empty() const
Check if path is empty.
const std::vector< NavGraphNode > & nodes() const
Get nodes along the path.
size_t size() const
Get size of 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.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
const std::vector< NavGraphEdge > & edges() const
Get edges of the graph.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Thread class encapsulation of pthreads.
void wakeup()
Wake up thread.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.