Fawkes API  Fawkes Development Version
clusters_distance_cost_constraint.cpp
1 /***************************************************************************
2  * clusters_distance_cost_constraint.cpp - distance-based cost factor for
3  * blocked edges
4  *
5  * Created: Sat Jul 19 21:17:25 2014
6  * Copyright 2014 Tim Niemueller
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "clusters_distance_cost_constraint.h"
23 #include "navgraph_clusters_thread.h"
24 
25 #include <core/exception.h>
26 
27 using namespace fawkes;
28 
29 /** @class NavGraphClustersDistanceCostConstraint "clusters_distance_cost_constraint.h"
30  * Constraint apply linearly scaled costs based on the distance.
31  * @author Tim Niemueller
32  */
33 
34 /** Constructor.
35  * @param name constraint name
36  * @param parent parent to call for blocked edges
37  * @param cost_min minimum cost for centroids in range
38  * @param cost_max maximum cost for close centroids
39  * @param dist_min minimum distance, for cost scaling, anything closer will be
40  * assigned cost_max
41  * @param dist_max maximum distance for costs, anything farther will be
42  * assigned 1.0.
43  */
45  const char *name,
46  NavGraphClustersThread *parent,
47  float cost_min, float cost_max, float dist_min, float dist_max)
49 {
50  parent_ = parent;
51  cost_min_ = cost_min;
52  cost_max_ = cost_max;
53  cost_span_ = cost_max_ - cost_min_;
54  dist_min_ = dist_min;
55  dist_max_ = dist_max;
56  dist_span_ = dist_max_ - dist_min_;
57  valid_ = false;
58 
59  if (cost_min_ > cost_max_) {
60  throw Exception("Cost min must be less or equal to max");
61  }
62  if (dist_min_ > dist_max_) {
63  throw Exception("Dist min must be less or equal to max");
64  }
65 }
66 
67 /** Virtual empty destructor. */
69 {
70 }
71 
72 
73 bool
75 {
76  blocked_ = parent_->blocked_edges_centroids();
77  valid_ = parent_->robot_pose(pose_);
78  return valid_;
79 }
80 
81 
82 float
84  const fawkes::NavGraphNode &to) throw()
85 {
86  if (valid_) {
87  std::string to_n = to.name();
88  std::string from_n = from.name();
89 
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) {
93  return
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));
96  });
97 
98  if (bl != blocked_.end()) {
99  float distance = (pose_ - std::get<2>(*bl)).norm();
100  if (distance <= dist_min_) {
101  return cost_max_;
102  } else if (distance >= dist_max_) {
103  return 1.0;
104  } else {
105  return cost_max_ - (((distance - dist_min_) / dist_span_) * cost_span_);
106  }
107  }
108  }
109 
110  return 1.0;
111 }
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:62
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.
Definition: exception.h:36
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.
Topological graph node.
Definition: navgraph_node.h:38