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_);
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Fawkes library namespace.
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.
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.