22 #include "clusters_distance_cost_constraint.h"
24 #include "navgraph_clusters_thread.h"
26 #include <core/exception.h>
57 cost_span_ = cost_max_ - cost_min_;
60 dist_span_ = dist_max_ - dist_min_;
63 if (cost_min_ > cost_max_) {
64 throw Exception(
"Cost min must be less or equal to max");
66 if (dist_min_ > dist_max_) {
67 throw Exception(
"Dist min must be less or equal to max");
79 blocked_ = parent_->blocked_edges_centroids();
80 valid_ = parent_->robot_pose(pose_);
89 std::string to_n = to.name();
90 std::string from_n = from.name();
92 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>::iterator bl =
93 std::find_if(blocked_.begin(),
95 [&to_n, &from_n](std::tuple<std::string, std::string, Eigen::Vector2f> &b) {
96 return (to_n == std::get<0>(b) && from_n == std::get<1>(b))
97 || (to_n == std::get<1>(b) && from_n == std::get<0>(b));
100 if (bl != blocked_.end()) {
101 float distance = (pose_ - std::get<2>(*bl)).norm();
107 return cost_max_ - (((
distance - dist_min_) / dist_span_) * cost_span_);