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 
24 #include "navgraph_clusters_thread.h"
25 
26 #include <core/exception.h>
27 
28 using namespace fawkes;
29 
30 /** @class NavGraphClustersDistanceCostConstraint "clusters_distance_cost_constraint.h"
31  * Constraint apply linearly scaled costs based on the distance.
32  * @author Tim Niemueller
33  */
34 
35 /** Constructor.
36  * @param name constraint name
37  * @param parent parent to call for blocked edges
38  * @param cost_min minimum cost for centroids in range
39  * @param cost_max maximum cost for close centroids
40  * @param dist_min minimum distance, for cost scaling, anything closer will be
41  * assigned cost_max
42  * @param dist_max maximum distance for costs, anything farther will be
43  * assigned 1.0.
44  */
46  const char * name,
47  NavGraphClustersThread *parent,
48  float cost_min,
49  float cost_max,
50  float dist_min,
51  float dist_max)
53 {
54  parent_ = parent;
55  cost_min_ = cost_min;
56  cost_max_ = cost_max;
57  cost_span_ = cost_max_ - cost_min_;
58  dist_min_ = dist_min;
59  dist_max_ = dist_max;
60  dist_span_ = dist_max_ - dist_min_;
61  valid_ = false;
62 
63  if (cost_min_ > cost_max_) {
64  throw Exception("Cost min must be less or equal to max");
65  }
66  if (dist_min_ > dist_max_) {
67  throw Exception("Dist min must be less or equal to max");
68  }
69 }
70 
71 /** Virtual empty destructor. */
73 {
74 }
75 
76 bool
78 {
79  blocked_ = parent_->blocked_edges_centroids();
80  valid_ = parent_->robot_pose(pose_);
81  return valid_;
82 }
83 
84 float
86  const fawkes::NavGraphNode &to) throw()
87 {
88  if (valid_) {
89  std::string to_n = to.name();
90  std::string from_n = from.name();
91 
92  std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>::iterator bl =
93  std::find_if(blocked_.begin(),
94  blocked_.end(),
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));
98  });
99 
100  if (bl != blocked_.end()) {
101  float distance = (pose_ - std::get<2>(*bl)).norm();
102  if (distance <= dist_min_) {
103  return cost_max_;
104  } else if (distance >= dist_max_) {
105  return 1.0;
106  } else {
107  return cost_max_ - (((distance - dist_min_) / dist_span_) * cost_span_);
108  }
109  }
110  }
111 
112  return 1.0;
113 }
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Get cost factor for given edge.
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 bool compute(void)
Perform compuations before graph search and to indicate re-planning.
Block navgraph paths based on laser clusters.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Constraint that can be queried for an edge cost factor.
Topological graph node.
Definition: navgraph_node.h:36
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59