22 #include "clusters_distance_cost_constraint.h" 23 #include "navgraph_clusters_thread.h" 25 #include <core/exception.h> 47 float cost_min,
float cost_max,
float dist_min,
float dist_max)
53 cost_span_ = cost_max_ - cost_min_;
56 dist_span_ = dist_max_ - dist_min_;
59 if (cost_min_ > cost_max_) {
60 throw Exception(
"Cost min must be less or equal to max");
62 if (dist_min_ > dist_max_) {
63 throw Exception(
"Dist min must be less or equal to max");
87 std::string to_n = to.name();
88 std::string from_n = from.name();
90 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>::iterator bl=
91 std::find_if(blocked_.begin(), blocked_.end(),
92 [&to_n, &from_n](std::tuple<std::string, std::string, Eigen::Vector2f> &b) {
94 (to_n == std::get<0>(b) && from_n == std::get<1>(b)) ||
95 (to_n == std::get<1>(b) && from_n == std::get<0>(b));
98 if (bl != blocked_.end()) {
99 float distance = (pose_ - std::get<2>(*bl)).norm();
100 if (distance <= dist_min_) {
102 }
else if (distance >= dist_max_) {
105 return cost_max_ - (((distance - dist_min_) / dist_span_) * cost_span_);
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Fawkes library namespace.
bool robot_pose(Eigen::Vector2f &pose)
Determine current robot pose.
virtual ~NavGraphClustersDistanceCostConstraint()
Virtual empty destructor.
Constraint that can be queried for an edge cost factor.
Block navgraph paths based on laser clusters.
Base class for exceptions in Fawkes.
NavGraphClustersDistanceCostConstraint(const char *name, NavGraphClustersThread *parent, float cost_min, float cost_max, float dist_min, float dist_max)
Constructor.
std::list< std::tuple< std::string, std::string, Eigen::Vector2f > > blocked_edges_centroids()
Get a list of edges close to a clusters and its centroid considered blocked.
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Get cost factor for given edge.