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,
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
50bool
52{
53 blocked_ = parent_->blocked_edges();
54 return true;
55}
56
57float
59 const fawkes::NavGraphNode &to) noexcept
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}
virtual bool compute(void) noexcept
Perform compuations before graph search and to indicate re-planning.
virtual ~NavGraphClustersStaticCostConstraint()
Virtual empty destructor.
NavGraphClustersStaticCostConstraint(const char *name, NavGraphClustersThread *parent, float cost_factor)
Constructor.
virtual float cost_factor(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to) noexcept
Get cost factor for given edge.
Block navgraph paths based on laser clusters.
Topological graph node.
Definition: navgraph_node.h:36