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 
23 #include "navgraph_clusters_thread.h"
24 
25 /** @class NavGraphClustersStaticCostConstraint "clusters_static_cost_constraint.h"
26  * Constraint apply a static cost factor to blocked edges.
27  * @author Tim Niemueller
28  */
29 
30 /** Constructor.
31  * @param name constraint name
32  * @param parent parent to call for blocked edges
33  * @param cost_factor cost factor to return for blocked edges
34  */
36  const char * name,
37  NavGraphClustersThread *parent,
38  float cost_factor)
39 : NavGraphEdgeCostConstraint(name)
40 {
41  parent_ = parent;
42  cost_factor_ = cost_factor;
43 }
44 
45 /** Virtual empty destructor. */
47 {
48 }
49 
50 bool
52 {
53  blocked_ = parent_->blocked_edges();
54  return true;
55 }
56 
57 float
59  const fawkes::NavGraphNode &to) throw()
60 {
61  std::string to_n = to.name();
62  std::string from_n = from.name();
63  if ((find(blocked_.begin(), blocked_.end(), make_pair(from_n, to_n)) != blocked_.end())
64  || (find(blocked_.begin(), blocked_.end(), make_pair(to_n, from_n)) != blocked_.end())) {
65  return cost_factor_;
66  }
67 
68  return 1.0;
69 }
fawkes::NavGraphNode
Topological graph node.
Definition: navgraph_node.h:36
NavGraphClustersStaticCostConstraint::cost_factor
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to)
Get cost factor for given edge.
Definition: clusters_static_cost_constraint.cpp:58
NavGraphClustersStaticCostConstraint::compute
virtual bool compute(void)
Perform compuations before graph search and to indicate re-planning.
Definition: clusters_static_cost_constraint.cpp:51
NavGraphClustersStaticCostConstraint::~NavGraphClustersStaticCostConstraint
virtual ~NavGraphClustersStaticCostConstraint()
Virtual empty destructor.
Definition: clusters_static_cost_constraint.cpp:46
NavGraphClustersStaticCostConstraint::NavGraphClustersStaticCostConstraint
NavGraphClustersStaticCostConstraint(const char *name, NavGraphClustersThread *parent, float cost_factor)
Constructor.
Definition: clusters_static_cost_constraint.cpp:35
NavGraphClustersThread
Block navgraph paths based on laser clusters.
Definition: navgraph_clusters_thread.h:56