Fawkes API  Fawkes Development Version
clusters_static_cost_constraint.cpp
1 /***************************************************************************
2  * clusters_static_cost_constraint.cpp - static cost factor for blocked edges
3  *
4  * Created: Fri Jul 18 22:39:30 2014
5  * Copyright 2014 Tim Niemueller
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "clusters_static_cost_constraint.h"
22 #include "navgraph_clusters_thread.h"
23 
24 /** @class NavGraphClustersStaticCostConstraint "clusters_static_cost_constraint.h"
25  * Constraint apply a static cost factor to blocked edges.
26  * @author Tim Niemueller
27  */
28 
29 /** Constructor.
30  * @param name constraint name
31  * @param parent parent to call for blocked edges
32  * @param cost_factor cost factor to return for blocked edges
33  */
35  const char *name,
36  NavGraphClustersThread *parent,
37  float cost_factor)
38 : NavGraphEdgeCostConstraint(name)
39 {
40  parent_ = parent;
41  cost_factor_ = cost_factor;
42 }
43 
44 /** Virtual empty destructor. */
46 {
47 }
48 
49 
50 bool
52 {
53  blocked_ = parent_->blocked_edges();
54  return true;
55 }
56 
57 
58 float
60  const fawkes::NavGraphNode &to) throw()
61 {
62  std::string to_n = to.name();
63  std::string from_n = from.name();
64  if ((find(blocked_.begin(), blocked_.end(), make_pair(from_n, to_n)) != blocked_.end()) ||
65  (find(blocked_.begin(), blocked_.end(), make_pair(to_n, from_n)) != blocked_.end()) )
66  {
67  return cost_factor_;
68  }
69 
70  return 1.0;
71 }
NavGraphClustersStaticCostConstraint(const char *name, NavGraphClustersThread *parent, float cost_factor)
Constructor.
std::list< std::pair< std::string, std::string > > blocked_edges()
Get a list of edges close to a clusters considered blocked.
virtual ~NavGraphClustersStaticCostConstraint()
Virtual empty destructor.
Block navgraph paths based on laser clusters.
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
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.