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
28using 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,
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
76bool
78{
79 blocked_ = parent_->blocked_edges_centroids();
80 valid_ = parent_->robot_pose(pose_);
81 return valid_;
82}
83
84float
86 const fawkes::NavGraphNode &to) noexcept
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 bool compute(void) noexcept
Perform compuations before graph search and to indicate re-planning.
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 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.
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