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_);
virtual bool compute(void) noexcept
Perform compuations before graph search and to indicate re-planning.
virtual ~NavGraphClustersDistanceCostConstraint()
Virtual empty destructor.
NavGraphClustersDistanceCostConstraint(const char *name, NavGraphClustersThread *parent, float cost_min, float cost_max, float dist_min, float dist_max)
Constructor.
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to) noexcept
Get cost factor for given edge.
Block navgraph paths based on laser clusters.
Base class for exceptions in Fawkes.
Constraint that can be queried for an edge cost factor.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.