22 #include "visualization_thread.h" 24 #include <navgraph/navgraph.h> 25 #include <navgraph/constraints/constraint_repo.h> 26 #include <navgraph/constraints/polygon_node_constraint.h> 27 #include <navgraph/constraints/polygon_edge_constraint.h> 29 #include <utils/math/angle.h> 30 #include <utils/math/coord.h> 41 typedef std::multimap<std::string, std::string> ConnMap;
56 vispub_ =
rosnode->advertise<visualization_msgs::MarkerArray>(
"visualization_marker_array", 100,
true);
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);
120 plan_to_ = plan_from_ =
"";
139 traversal_ = traversal;
140 plan_to_ = plan_from_ =
"";
149 plan_to_ = plan_from_ =
"";
161 if (plan_from_ != from || plan_to_ != to) {
184 NavGraphVisualizationThread::edge_cost_factor(
185 std::list<std::tuple<std::string, std::string, std::string, float>> &costs,
186 const std::string &from,
const std::string &to, std::string &constraint_name)
188 for (
const std::tuple<std::string, std::string, std::string, float> &c : costs) {
189 if ((std::get<0>(c) == from && std::get<1>(c) == to) ||
190 (std::get<0>(c) == to && std::get<1>(c) == from))
192 constraint_name = std::get<2>(c);
193 return std::get<3>(c);
201 NavGraphVisualizationThread::add_circle_markers(visualization_msgs::MarkerArray &m,
size_t &id_num,
202 float center_x,
float center_y,
float radius,
203 unsigned int arc_length,
204 float r,
float g,
float b,
float alpha,
float line_width)
206 for (
unsigned int a = 0; a < 360; a += 2 * arc_length) {
207 visualization_msgs::Marker arc;
208 arc.header.frame_id = cfg_global_frame_;
209 arc.header.stamp = ros::Time::now();
212 arc.type = visualization_msgs::Marker::LINE_STRIP;
213 arc.action = visualization_msgs::Marker::ADD;
214 arc.scale.x = arc.scale.y = arc.scale.z = line_width;
219 arc.lifetime = ros::Duration(0, 0);
220 arc.points.resize(arc_length);
221 for (
unsigned int j = 0; j < arc_length; ++j) {
222 float circ_x = 0, circ_y = 0;
224 arc.points[j].x = center_x + circ_x;
225 arc.points[j].y = center_y + circ_y;
226 arc.points[j].z = 0.;
228 m.markers.push_back(arc);
233 NavGraphVisualizationThread::publish()
235 if (! graph_)
return;
238 std::vector<fawkes::NavGraphNode> nodes = graph_->
nodes();
239 std::vector<fawkes::NavGraphEdge> edges = graph_->
edges();
243 std::map<std::string, fawkes::NavGraphNode> nodes_map;
245 nodes_map[n.name()] = n;
249 std::map<std::string, std::string> bl_nodes = crepo_->
blocks(nodes);
250 std::map<std::pair<std::string, std::string>, std::string> bl_edges =
252 std::list<std::tuple<std::string, std::string, std::string, float>> edge_cfs =
257 size_t constraints_id_num = 0;
259 visualization_msgs::MarkerArray m;
261 #if ROS_VERSION_MINIMUM(1,10,0) 263 visualization_msgs::Marker delop;
264 delop.header.frame_id = cfg_global_frame_;
265 delop.header.stamp = ros::Time::now();
266 delop.ns =
"navgraph-constraints";
269 m.markers.push_back(delop);
273 visualization_msgs::Marker lines;
274 lines.header.frame_id = cfg_global_frame_;
275 lines.header.stamp = ros::Time::now();
276 lines.ns =
"navgraph";
278 lines.type = visualization_msgs::Marker::LINE_LIST;
279 lines.action = visualization_msgs::Marker::ADD;
281 lines.color.g = lines.color.b = 0.f;
283 lines.scale.x = 0.02;
284 lines.lifetime = ros::Duration(0, 0);
286 visualization_msgs::Marker plan_lines;
287 plan_lines.header.frame_id = cfg_global_frame_;
288 plan_lines.header.stamp = ros::Time::now();
289 plan_lines.ns =
"navgraph";
290 plan_lines.id = id_num++;
291 plan_lines.type = visualization_msgs::Marker::LINE_LIST;
292 plan_lines.action = visualization_msgs::Marker::ADD;
293 plan_lines.color.r = 1.0;
294 plan_lines.color.g = plan_lines.color.b = 0.f;
295 plan_lines.color.a = 1.0;
296 plan_lines.scale.x = 0.035;
297 plan_lines.lifetime = ros::Duration(0, 0);
299 visualization_msgs::Marker blocked_lines;
300 blocked_lines.header.frame_id = cfg_global_frame_;
301 blocked_lines.header.stamp = ros::Time::now();
302 blocked_lines.ns =
"navgraph";
303 blocked_lines.id = id_num++;
304 blocked_lines.type = visualization_msgs::Marker::LINE_LIST;
305 blocked_lines.action = visualization_msgs::Marker::ADD;
306 blocked_lines.color.r = blocked_lines.color.g = blocked_lines.color.b = 0.5;
307 blocked_lines.color.a = 1.0;
308 blocked_lines.scale.x = 0.02;
309 blocked_lines.lifetime = ros::Duration(0, 0);
311 visualization_msgs::Marker cur_line;
312 cur_line.header.frame_id = cfg_global_frame_;
313 cur_line.header.stamp = ros::Time::now();
314 cur_line.ns =
"navgraph";
315 cur_line.id = id_num++;
316 cur_line.type = visualization_msgs::Marker::LINE_LIST;
317 cur_line.action = visualization_msgs::Marker::ADD;
318 cur_line.color.g = 1.f;
319 cur_line.color.r = cur_line.color.b = 0.f;
320 cur_line.color.a = 1.0;
321 cur_line.scale.x = 0.05;
322 cur_line.lifetime = ros::Duration(0, 0);
325 for (
size_t i = 0; i < nodes.size(); ++i) {
326 bool is_in_plan = traversal_ && traversal_.
path().
contains(nodes[i]);
327 bool is_last = traversal_ &&
330 bool is_active = (plan_to_ == nodes[i].name());
332 visualization_msgs::Marker sphere;
333 sphere.header.frame_id = cfg_global_frame_;
334 sphere.header.stamp = ros::Time::now();
335 sphere.ns =
"navgraph";
336 sphere.id = id_num++;
337 sphere.type = visualization_msgs::Marker::SPHERE;
338 sphere.action = visualization_msgs::Marker::ADD;
339 sphere.pose.position.x = nodes[i].x();
340 sphere.pose.position.y = nodes[i].y();
341 sphere.pose.position.z = 0.;
342 sphere.pose.orientation.w = 1.;
343 sphere.scale.y = 0.05;
344 sphere.scale.z = 0.05;
346 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.1;
348 sphere.color.r = 0.f;
349 sphere.color.g = 1.f;
351 sphere.color.r = 1.f;
352 sphere.color.g = 0.f;
354 sphere.color.b = 0.f;
355 }
else if (bl_nodes.find(nodes[i].name()) != bl_nodes.end()) {
356 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.05;
357 sphere.color.r = sphere.color.g = sphere.color.b = 0.5;
359 visualization_msgs::Marker text;
360 text.header.frame_id = cfg_global_frame_;
361 text.header.stamp = ros::Time::now();
362 text.ns =
"navgraph-constraints";
363 text.id = constraints_id_num++;
364 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
365 text.action = visualization_msgs::Marker::ADD;
366 text.pose.position.x = nodes[i].x();
367 text.pose.position.y = nodes[i].y();
368 text.pose.position.z = 0.3;
369 text.pose.orientation.w = 1.;
372 text.color.g = text.color.b = 0.f;
374 text.lifetime = ros::Duration(0, 0);
375 text.text = bl_nodes[nodes[i].name()];
376 m.markers.push_back(text);
379 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.05;
380 sphere.color.r = 0.5;
381 sphere.color.b = 0.f;
383 sphere.color.a = 1.0;
384 sphere.lifetime = ros::Duration(0, 0);
385 m.markers.push_back(sphere);
388 float target_tolerance = 0.;
389 if (nodes[i].has_property(
"target_tolerance")) {
390 target_tolerance = nodes[i].property_as_float(
"target_tolerance");
391 }
else if (default_props.find(
"target_tolerance") != default_props.end()) {
392 target_tolerance = StringConversions::to_float(default_props[
"target_tolerance"]);
394 if (target_tolerance > 0.) {
395 add_circle_markers(m, id_num, nodes[i].x(), nodes[i].y(), target_tolerance, 5,
396 sphere.color.r, sphere.color.g, sphere.color.b,
397 is_active ? sphere.color.a : 0.4);
399 }
else if (is_active) {
400 float travel_tolerance = 0.;
401 if (nodes[i].has_property(
"travel_tolerance")) {
402 travel_tolerance = nodes[i].property_as_float(
"travel_tolerance");
403 }
else if (default_props.find(
"travel_tolerance") != default_props.end()) {
404 travel_tolerance = StringConversions::to_float(default_props[
"travel_tolerance"]);
406 if (travel_tolerance > 0.) {
407 add_circle_markers(m, id_num, nodes[i].x(), nodes[i].y(), travel_tolerance, 10,
408 sphere.color.r, sphere.color.g, sphere.color.b, sphere.color.a);
413 float shortcut_tolerance = 0.;
414 if (nodes[i].has_property(
"shortcut_tolerance")) {
415 shortcut_tolerance = nodes[i].property_as_float(
"shortcut_tolerance");
416 }
else if (default_props.find(
"shortcut_tolerance") != default_props.end()) {
417 shortcut_tolerance = StringConversions::to_float(default_props[
"shortcut_tolerance"]);
419 if (shortcut_tolerance > 0.) {
420 add_circle_markers(m, id_num, nodes[i].x(), nodes[i].y(), shortcut_tolerance, 30,
421 sphere.color.r, sphere.color.g, sphere.color.b, 0.3);
425 if (nodes[i].has_property(
"orientation")) {
426 float ori = nodes[i].property_as_float(
"orientation");
428 visualization_msgs::Marker arrow;
429 arrow.header.frame_id = cfg_global_frame_;
430 arrow.header.stamp = ros::Time::now();
431 arrow.ns =
"navgraph";
433 arrow.type = visualization_msgs::Marker::ARROW;
434 arrow.action = visualization_msgs::Marker::ADD;
435 arrow.pose.position.x = nodes[i].x();
436 arrow.pose.position.y = nodes[i].y();
437 arrow.pose.position.z = 0.;
438 tf::Quaternion q = tf::create_quaternion_from_yaw(ori);
439 arrow.pose.orientation.x = q.x();
440 arrow.pose.orientation.y = q.y();
441 arrow.pose.orientation.z = q.z();
442 arrow.pose.orientation.w = q.w();
443 arrow.scale.x = 0.08;
444 arrow.scale.y = 0.02;
445 arrow.scale.z = 0.02;
448 arrow.color.r = arrow.color.g = 1.f;
458 arrow.lifetime = ros::Duration(0, 0);
459 m.markers.push_back(arrow);
463 visualization_msgs::Marker text;
464 text.header.frame_id = cfg_global_frame_;
465 text.header.stamp = ros::Time::now();
466 text.ns =
"navgraph";
468 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
469 text.action = visualization_msgs::Marker::ADD;
470 text.pose.position.x = nodes[i].x();
471 text.pose.position.y = nodes[i].y();
472 text.pose.position.z = 0.08;
473 text.pose.orientation.w = 1.;
475 text.color.r = text.color.g = text.color.b = 1.0f;
477 text.lifetime = ros::Duration(0, 0);
478 text.text = nodes[i].name();
479 m.markers.push_back(text);
489 visualization_msgs::Marker sphere;
490 sphere.header.frame_id = cfg_global_frame_;
491 sphere.header.stamp = ros::Time::now();
492 sphere.ns =
"navgraph";
493 sphere.id = id_num++;
494 sphere.type = visualization_msgs::Marker::SPHERE;
495 sphere.action = visualization_msgs::Marker::ADD;
496 sphere.pose.position.x = target_node.
x();
497 sphere.pose.position.y = target_node.
y();
498 sphere.pose.position.z = 0.;
499 sphere.pose.orientation.w = 1.;
500 sphere.scale.y = 0.05;
501 sphere.scale.z = 0.05;
502 sphere.scale.x = sphere.scale.y = sphere.scale.z = 0.1;
504 sphere.color.g = 0.5f;
505 sphere.color.b = 0.f;
506 sphere.color.a = 1.0;
507 sphere.lifetime = ros::Duration(0, 0);
508 m.markers.push_back(sphere);
510 visualization_msgs::Marker text;
511 text.header.frame_id = cfg_global_frame_;
512 text.header.stamp = ros::Time::now();
513 text.ns =
"navgraph";
515 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
516 text.action = visualization_msgs::Marker::ADD;
517 text.pose.position.x = target_node.
x();
518 text.pose.position.y = target_node.
y();
519 text.pose.position.z = 0.08;
520 text.pose.orientation.w = 1.;
522 text.color.r = text.color.g = text.color.b = 1.0f;
524 text.lifetime = ros::Duration(0, 0);
525 text.text =
"Free Target";
526 m.markers.push_back(text);
530 visualization_msgs::Marker ori_arrow;
531 ori_arrow.header.frame_id = cfg_global_frame_;
532 ori_arrow.header.stamp = ros::Time::now();
533 ori_arrow.ns =
"navgraph";
534 ori_arrow.id = id_num++;
535 ori_arrow.type = visualization_msgs::Marker::ARROW;
536 ori_arrow.action = visualization_msgs::Marker::ADD;
537 ori_arrow.pose.position.x = target_node.
x();
538 ori_arrow.pose.position.y = target_node.
y();
539 ori_arrow.pose.position.z = 0.;
540 tf::Quaternion q = tf::create_quaternion_from_yaw(ori);
541 ori_arrow.pose.orientation.x = q.x();
542 ori_arrow.pose.orientation.y = q.y();
543 ori_arrow.pose.orientation.z = q.z();
544 ori_arrow.pose.orientation.w = q.w();
545 ori_arrow.scale.x = 0.08;
546 ori_arrow.scale.y = 0.02;
547 ori_arrow.scale.z = 0.02;
548 ori_arrow.color.r = 1.f;
549 ori_arrow.color.g = 0.5f;
550 ori_arrow.color.b = 0.f;
551 ori_arrow.color.a = 1.0;
552 ori_arrow.lifetime = ros::Duration(0, 0);
553 m.markers.push_back(ori_arrow);
556 float target_tolerance = 0.;
559 }
else if (default_props.find(
"target_tolerance") != default_props.end()) {
560 target_tolerance = StringConversions::to_float(default_props[
"target_tolerance"]);
562 if (target_tolerance > 0.) {
563 add_circle_markers(m, id_num, traversal_.
path().
goal().
x(), traversal_.
path().
goal().
y(),
564 target_tolerance, 10,
565 sphere.color.r, sphere.color.g, sphere.color.b,
566 (traversal_.
last()) ? sphere.color.a : 0.5);
569 float shortcut_tolerance = 0.;
572 }
else if (default_props.find(
"shortcut_tolerance") != default_props.end()) {
573 shortcut_tolerance = StringConversions::to_float(default_props[
"shortcut_tolerance"]);
575 if (shortcut_tolerance > 0.) {
576 add_circle_markers(m, id_num, traversal_.
path().
goal().
x(), traversal_.
path().
goal().
y(),
577 shortcut_tolerance, 30,
578 sphere.color.r, sphere.color.g, sphere.color.b, 0.3);
585 geometry_msgs::Point p1;
586 p1.x = last_graph_node.
x();
587 p1.y = last_graph_node.
y();
590 geometry_msgs::Point p2;
591 p2.x = target_node.
x();
592 p2.y = target_node.
y();
595 visualization_msgs::Marker arrow;
596 arrow.header.frame_id = cfg_global_frame_;
597 arrow.header.stamp = ros::Time::now();
598 arrow.ns =
"navgraph";
600 arrow.type = visualization_msgs::Marker::ARROW;
601 arrow.action = visualization_msgs::Marker::ADD;
603 arrow.lifetime = ros::Duration(0, 0);
604 arrow.points.push_back(p1);
605 arrow.points.push_back(p2);
608 if (plan_to_ == target_node.
name())
611 arrow.color.r = arrow.color.g = 1.f;
618 arrow.color.g = 0.5f;
620 arrow.scale.x = 0.07;
623 m.markers.push_back(arrow);
628 for (
size_t i = 0; i < edges.size(); ++i) {
630 if (nodes_map.find(edge.
from()) != nodes_map.end() &&
631 nodes_map.find(edge.
to()) != nodes_map.end())
636 geometry_msgs::Point p1;
641 geometry_msgs::Point p2;
646 std::string cost_cstr_name;
647 float cost_factor = edge_cost_factor(edge_cfs, from.
name(), to.
name(), cost_cstr_name);
650 visualization_msgs::Marker arrow;
651 arrow.header.frame_id = cfg_global_frame_;
652 arrow.header.stamp = ros::Time::now();
653 arrow.ns =
"navgraph";
655 arrow.type = visualization_msgs::Marker::ARROW;
656 arrow.action = visualization_msgs::Marker::ADD;
658 arrow.lifetime = ros::Duration(0, 0);
659 arrow.points.push_back(p1);
660 arrow.points.push_back(p2);
662 if (plan_from_ == from.
name() && plan_to_ == to.
name())
666 arrow.color.r = arrow.color.b = 0.f;
670 bool in_plan =
false;
672 for (
size_t p = 0; p < traversal_.
path().
nodes().size(); ++p) {
673 if (traversal_.
path().
nodes()[p] == from &&
685 if (cost_factor >= 1.00001) {
686 arrow.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_);
691 arrow.scale.x = 0.07;
693 }
else if (bl_nodes.find(from.
name()) != bl_nodes.end() ||
694 bl_nodes.find(to.
name()) != bl_nodes.end() ||
695 bl_edges.find(std::make_pair(to.
name(), from.
name())) != bl_edges.end() ||
696 bl_edges.find(std::make_pair(from.
name(), to.
name())) != bl_edges.end())
698 arrow.color.r = arrow.color.g = arrow.color.b = 0.5;
699 arrow.scale.x = 0.04;
700 arrow.scale.y = 0.15;
702 tf::Vector3 p1v(p1.x, p1.y, p1.z);
703 tf::Vector3 p2v(p2.x, p2.y, p2.z);
705 tf::Vector3 p = p1v + (p2v - p1v) * 0.5;
707 std::string text_s =
"";
709 std::map<std::pair<std::string, std::string>, std::string>::iterator e =
710 bl_edges.find(std::make_pair(to.
name(), from.
name()));
711 if (e != bl_edges.end()) {
714 e = bl_edges.find(std::make_pair(from.
name(), to.
name()));
715 if (e != bl_edges.end()) {
720 visualization_msgs::Marker text;
721 text.header.frame_id = cfg_global_frame_;
722 text.header.stamp = ros::Time::now();
723 text.ns =
"navgraph-constraints";
724 text.id = constraints_id_num++;
725 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
726 text.action = visualization_msgs::Marker::ADD;
727 text.pose.position.x = p[0];
728 text.pose.position.y = p[1];
729 text.pose.position.z = 0.3;
730 text.pose.orientation.w = 1.;
733 text.color.g = text.color.b = 0.f;
735 text.lifetime = ros::Duration(0, 0);
737 m.markers.push_back(text);
741 arrow.color.r = 0.66666;
742 if (cost_factor >= 1.00001) {
744 std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_) * 0.66666;
749 arrow.scale.x = 0.04;
750 arrow.scale.y = 0.15;
753 m.markers.push_back(arrow);
755 if ( (plan_to_ == from.
name() && plan_from_ == to.
name()) ||
756 (plan_from_ == from.
name() && plan_to_ == to.
name()) )
759 cur_line.points.push_back(p1);
760 cur_line.points.push_back(p2);
762 bool in_plan =
false;
764 for (
size_t p = 0; p < traversal_.
path().
nodes().size(); ++p) {
765 if (traversal_.
path().
nodes()[p] == from &&
766 ((p > 0 && traversal_.
path().
nodes()[p-1] == to) ||
767 (p < traversal_.
path().
nodes().size() - 1 &&
778 if (cost_factor >= 1.00001) {
779 visualization_msgs::Marker line;
780 line.header.frame_id = cfg_global_frame_;
781 line.header.stamp = ros::Time::now();
782 line.ns =
"navgraph";
784 line.type = visualization_msgs::Marker::LINE_STRIP;
785 line.action = visualization_msgs::Marker::ADD;
787 line.lifetime = ros::Duration(0, 0);
788 line.points.push_back(p1);
789 line.points.push_back(p2);
791 line.color.g = std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_);
793 line.scale.x = 0.035;
794 m.markers.push_back(line);
796 plan_lines.points.push_back(p1);
797 plan_lines.points.push_back(p2);
799 }
else if (bl_nodes.find(from.
name()) != bl_nodes.end() ||
800 bl_nodes.find(to.
name()) != bl_nodes.end())
802 blocked_lines.points.push_back(p1);
803 blocked_lines.points.push_back(p2);
805 }
else if (bl_edges.find(std::make_pair(to.
name(), from.
name())) != bl_edges.end() ||
806 bl_edges.find(std::make_pair(from.
name(), to.
name())) != bl_edges.end())
808 blocked_lines.points.push_back(p1);
809 blocked_lines.points.push_back(p2);
811 tf::Vector3 p1v(p1.x, p1.y, p1.z);
812 tf::Vector3 p2v(p2.x, p2.y, p2.z);
814 tf::Vector3 p = p1v + (p2v - p1v) * 0.5;
816 std::string text_s =
"";
818 std::map<std::pair<std::string, std::string>, std::string>::iterator e =
819 bl_edges.find(std::make_pair(to.
name(), from.
name()));
820 if (e != bl_edges.end()) {
823 e = bl_edges.find(std::make_pair(from.
name(), to.
name()));
824 if (e != bl_edges.end()) {
829 visualization_msgs::Marker text;
830 text.header.frame_id = cfg_global_frame_;
831 text.header.stamp = ros::Time::now();
832 text.ns =
"navgraph-constraints";
833 text.id = constraints_id_num++;
834 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
835 text.action = visualization_msgs::Marker::ADD;
836 text.pose.position.x = p[0];
837 text.pose.position.y = p[1];
838 text.pose.position.z = 0.3;
839 text.pose.orientation.w = 1.;
842 text.color.g = text.color.b = 0.f;
844 text.lifetime = ros::Duration(0, 0);
846 m.markers.push_back(text);
849 if (cost_factor >= 1.00001) {
850 visualization_msgs::Marker line;
851 line.header.frame_id = cfg_global_frame_;
852 line.header.stamp = ros::Time::now();
853 line.ns =
"navgraph";
855 line.type = visualization_msgs::Marker::LINE_STRIP;
856 line.action = visualization_msgs::Marker::ADD;
858 line.lifetime = ros::Duration(0, 0);
859 line.points.push_back(p1);
860 line.points.push_back(p2);
861 line.color.r = 0.66666;
863 std::min(1.0, (cost_factor - 1.0) / cfg_cost_scale_max_) * 0.66666;
866 m.markers.push_back(line);
868 lines.points.push_back(p1);
869 lines.points.push_back(p2);
877 m.markers.push_back(lines);
878 m.markers.push_back(plan_lines);
879 m.markers.push_back(blocked_lines);
880 m.markers.push_back(cur_line);
887 std::list<const NavGraphPolygonConstraint *> poly_constraints;
889 std::for_each(node_constraints.begin(), node_constraints.end(),
894 poly_constraints.push_back(pc);
898 std::for_each(edge_constraints.begin(), edge_constraints.end(),
903 poly_constraints.push_back(pc);
909 for (
auto const &p : polygons) {
910 visualization_msgs::Marker polc_lines;
911 polc_lines.header.frame_id = cfg_global_frame_;
912 polc_lines.header.stamp = ros::Time::now();
913 polc_lines.ns =
"navgraph-constraints";
914 polc_lines.id = constraints_id_num++;
915 polc_lines.type = visualization_msgs::Marker::LINE_STRIP;
916 polc_lines.action = visualization_msgs::Marker::ADD;
917 polc_lines.color.r = polc_lines.color.g = 1.0;
918 polc_lines.color.b = 0.f;
919 polc_lines.color.a = 1.0;
920 polc_lines.scale.x = 0.02;
921 polc_lines.lifetime = ros::Duration(0, 0);
923 polc_lines.points.resize(p.second.size());
924 for (
size_t i = 0; i < p.second.size(); ++i) {
925 polc_lines.points[i].x = p.second[i].x;
926 polc_lines.points[i].y = p.second[i].y;
927 polc_lines.points[i].z = 0.;
930 m.markers.push_back(polc_lines);
936 #if ! ROS_VERSION_MINIMUM(1,10,0) 937 for (
size_t i = id_num; i < last_id_num_; ++i) {
938 visualization_msgs::Marker delop;
939 delop.header.frame_id = cfg_global_frame_;
940 delop.header.stamp = ros::Time::now();
941 delop.ns =
"navgraph";
943 delop.action = visualization_msgs::Marker::DELETE;
944 m.markers.push_back(delop);
947 for (
size_t i = constraints_id_num; i < constraints_last_id_num_; ++i) {
948 visualization_msgs::Marker delop;
949 delop.header.frame_id = cfg_global_frame_;
950 delop.header.stamp = ros::Time::now();
951 delop.ns =
"navgraph-constraints";
953 delop.action = visualization_msgs::Marker::DELETE;
954 m.markers.push_back(delop);
958 last_id_num_ = id_num;
959 constraints_last_id_num_ = constraints_id_num;
virtual void finalize()
Finalize the thread.
const std::string & from() const
Get edge originating node name.
std::vector< fawkes::NavGraphNodeConstraint * > NodeConstraintList
List of navgraph node constraints.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
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_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::NavGraphEdgeConstraint * > EdgeConstraintList
List of navgraph edge 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.
void set_current_edge(std::string from, std::string to)
Set the currently executed edge of the plan.
const std::vector< NavGraphNode > & nodes() const
Get nodes of the graph.
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.
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.
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.