Fawkes API Fawkes Development Version
navgraph_clusters_thread.cpp
1/***************************************************************************
2 * navgraph_clusters_thread.cpp - block paths based on laser clusters
3 *
4 * Created: Sun Jul 13 15:30:08 2014
5 * Copyright 2014 Tim Niemueller [www.niemueller.de]
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 "navgraph_clusters_thread.h"
22
23#include "clusters_block_constraint.h"
24#include "clusters_distance_cost_constraint.h"
25#include "clusters_static_cost_constraint.h"
26
27#include <core/threading/mutex_locker.h>
28#include <interfaces/Position3DInterface.h>
29#include <navgraph/constraints/constraint_repo.h>
30#include <navgraph/navgraph.h>
31#include <tf/utils.h>
32
33#include <Eigen/StdVector>
34#include <algorithm>
35
36using namespace fawkes;
37
38using std::find;
39using std::make_tuple;
40
41/** @class NavGraphClustersThread "navgraph_clusters_thread.h"
42 * Block navgraph paths based on laser clusters.
43 * @author Tim Niemueller
44 */
45
46/** Constructor. */
48: Thread("NavGraphClustersThread", Thread::OPMODE_WAITFORWAKEUP),
49 BlackBoardInterfaceListener("NavGraphClustersThread")
50{
51}
52
53/** Destructor. */
55{
56}
57
58void
60{
61 cfg_iface_prefix_ = config->get_string("/navgraph-clusters/interface-prefix");
62 cfg_close_threshold_ = config->get_float("/navgraph-clusters/close-threshold");
63 cfg_fixed_frame_ = config->get_string("/frames/fixed");
64 cfg_base_frame_ = config->get_string("/frames/base");
65 cfg_min_vishistory_ = config->get_int("/navgraph-clusters/min-visibility-history");
66 cfg_mode_ = config->get_string("/navgraph-clusters/constraint-mode");
67
68 std::string pattern = cfg_iface_prefix_ + "*";
69
70 cluster_ifs_ = blackboard->open_multiple_for_reading<Position3DInterface>(pattern.c_str());
71
72 for (Position3DInterface *pif : cluster_ifs_) {
75 }
77
78 bbio_add_observed_create("Position3DInterface", pattern.c_str());
80
81 edge_constraint_ = NULL;
82 edge_cost_constraint_ = NULL;
83 if (cfg_mode_ == "block") {
84 edge_constraint_ = new NavGraphClustersBlockConstraint("clusters", this);
85 navgraph->constraint_repo()->register_constraint(edge_constraint_);
86 } else if (cfg_mode_ == "static-cost") {
87 float cost_factor = config->get_float("/navgraph-clusters/static-cost/cost-factor");
88 edge_cost_constraint_ = new NavGraphClustersStaticCostConstraint("clusters", this, cost_factor);
89 navgraph->constraint_repo()->register_constraint(edge_cost_constraint_);
90 } else if (cfg_mode_ == "distance-cost") {
91 float cost_min = config->get_float("/navgraph-clusters/distance-cost/cost-min");
92 float cost_max = config->get_float("/navgraph-clusters/distance-cost/cost-max");
93 float dist_min = config->get_float("/navgraph-clusters/distance-cost/dist-min");
94 float dist_max = config->get_float("/navgraph-clusters/distance-cost/dist-max");
95 edge_cost_constraint_ = new NavGraphClustersDistanceCostConstraint(
96 "clusters", this, cost_min, cost_max, dist_min, dist_max);
97 navgraph->constraint_repo()->register_constraint(edge_cost_constraint_);
98 } else {
99 throw Exception("Unknown constraint mode '%s'", cfg_mode_.c_str());
100 }
101}
102
103void
105{
106 if (edge_constraint_) {
107 navgraph->constraint_repo()->unregister_constraint(edge_constraint_->name());
108 delete edge_constraint_;
109 }
110
111 if (edge_cost_constraint_) {
112 navgraph->constraint_repo()->unregister_constraint(edge_cost_constraint_->name());
113 delete edge_cost_constraint_;
114 }
115
118
119 for (Position3DInterface *pif : cluster_ifs_) {
120 blackboard->close(pif);
121 }
122 cluster_ifs_.clear();
123}
124
125void
127{
128}
129
130void
131NavGraphClustersThread::bb_interface_created(const char *type, const char *id) noexcept
132{
134 try {
135 pif = blackboard->open_for_reading<Position3DInterface>(id);
136 } catch (Exception &e) {
137 // ignored
138 logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what_no_backtrace());
139 return;
140 }
141
142 try {
143 bbil_add_reader_interface(pif);
144 bbil_add_writer_interface(pif);
145 blackboard->update_listener(this);
146 } catch (Exception &e) {
147 logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
148 try {
149 bbil_remove_reader_interface(pif);
150 bbil_remove_writer_interface(pif);
151 blackboard->update_listener(this);
152 blackboard->close(pif);
153 } catch (Exception &e) {
154 logger->log_error(
155 name(), "Failed to deregister %s:%s during error recovery: %s", type, id, e.what());
156 }
157 return;
158 }
159
160 cluster_ifs_.push_back_locked(pif);
161}
162
163void
164NavGraphClustersThread::bb_interface_writer_removed(fawkes::Interface *interface,
165 fawkes::Uuid instance_serial) noexcept
166{
167 conditional_close(interface);
168}
169
170void
171NavGraphClustersThread::bb_interface_reader_removed(fawkes::Interface *interface,
172 fawkes::Uuid instance_serial) noexcept
173{
174 conditional_close(interface);
175}
176
177void
178NavGraphClustersThread::conditional_close(Interface *interface) noexcept
179{
180 Position3DInterface *pif = dynamic_cast<Position3DInterface *>(interface);
181
182 bool close = false;
183 MutexLocker lock(cluster_ifs_.mutex());
184
186 std::find(cluster_ifs_.begin(), cluster_ifs_.end(), pif);
187
188 if (c != cluster_ifs_.end() && (!interface->has_writer() && (interface->num_readers() == 1))) {
189 // It's only us
190 logger->log_info(name(), "Last on %s, closing", interface->uid());
191 close = true;
192 cluster_ifs_.erase(c);
193 }
194
195 lock.unlock();
196
197 if (close) {
198 std::string uid = interface->uid();
199 try {
200 bbil_remove_reader_interface(interface);
201 bbil_remove_writer_interface(interface);
202 blackboard->update_listener(this);
203 blackboard->close(interface);
204 } catch (Exception &e) {
205 logger->log_error(name(), "Failed to unregister or close %s: %s", uid.c_str(), e.what());
206 }
207 }
208}
209
210/** Get a list of edges close to a clusters considered blocked.
211 * @return list of pairs of blocked edges' start and end name.
212 */
213std::list<std::pair<std::string, std::string>>
215{
216 std::list<std::pair<std::string, std::string>> blocked;
217 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>> blocked_c =
219
220 std::for_each(blocked_c.begin(),
221 blocked_c.end(),
222 [&blocked](std::tuple<std::string, std::string, Eigen::Vector2f> &b) {
223 blocked.push_back(std::make_pair(std::get<0>(b), std::get<1>(b)));
224 });
225
226 return blocked;
227}
228
229/** Get a list of edges close to a clusters and its centroid considered blocked.
230 * @return list of tuples of blocked edges' start and end name and the centroid
231 * of the object close to the edge.
232 */
233std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>
235{
236 MutexLocker lock(cluster_ifs_.mutex());
237 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>> blocked;
238
239 const std::vector<NavGraphEdge> &graph_edges = navgraph->edges();
240
241 for (Position3DInterface *pif : cluster_ifs_) {
242 pif->read();
243 if (pif->visibility_history() >= cfg_min_vishistory_) {
244 try {
245 // Should use *pif->timestamp(), but the way things are timed we
246 // would always run into an extrapolation exception
247 Eigen::Vector2f centroid(fixed_frame_pose(
248 pif->frame(), fawkes::Time(0, 0), pif->translation(0), pif->translation(1)));
249
250 for (const NavGraphEdge &edge : graph_edges) {
251 const Eigen::Vector2f origin(edge.from_node().x(), edge.from_node().y());
252 const Eigen::Vector2f target(edge.to_node().x(), edge.to_node().y());
253 const Eigen::Vector2f direction(target - origin);
254 const Eigen::Vector2f direction_norm = direction.normalized();
255 const Eigen::Vector2f diff = centroid - origin;
256 const float t = direction.dot(diff) / direction.squaredNorm();
257
258 if (t >= 0.0 && t <= 1.0) {
259 // projection of the centroid onto the edge is within the line segment
260 float distance = (diff - direction_norm.dot(diff) * direction_norm).norm();
261 if (distance < cfg_close_threshold_) {
262 blocked.push_back(make_tuple(edge.from(), edge.to(), centroid));
263 }
264 }
265 }
266 } catch (Exception &e) {
267 //logger->log_info(name(), "Failed to transform %s, ignoring", pif->uid());
268 }
269 }
270 }
271 blocked.sort([](const std::tuple<std::string, std::string, Eigen::Vector2f> &a,
272 const std::tuple<std::string, std::string, Eigen::Vector2f> &b) {
273 return (std::get<0>(a) < std::get<0>(b)
274 || (std::get<0>(a) == std::get<0>(b) && std::get<1>(a) < std::get<1>(b)));
275 });
276 blocked.unique();
277
278 return blocked;
279}
280
281Eigen::Vector2f
282NavGraphClustersThread::fixed_frame_pose(std::string frame,
283 const fawkes::Time &timestamp,
284 float x,
285 float y)
286{
287 if (frame == cfg_fixed_frame_) {
288 return Eigen::Vector2f(x, y);
289 } else {
290 //logger->log_debug(name(), "Transforming %s -> %s", frame.c_str(),
291 // cfg_fixed_frame_.c_str());
293 tf::Stamped<tf::Point> input(tf::Point(x, y, 0), timestamp, frame);
294 try {
295 tf_listener->transform_point(cfg_fixed_frame_, input, tpose);
296 //logger->log_debug(name(), "Transformed %s -> %s: (%f,%f) -> (%f,%f)", frame.c_str(),
297 // cfg_fixed_frame_.c_str(), x, y, tpose.x(), tpose.y());
298 return Eigen::Vector2f(tpose.x(), tpose.y());
299
300 } catch (Exception &e) {
301 //logger->log_warn(name(),
302 // "Failed to transform cluster pose: %s", e.what_no_backtrace());
303 throw;
304 }
305 }
306}
307
308/** Determine current robot pose.
309 * @param pose upon returning true contains the current pose of the robot
310 * @return true if the pose could be determined, false otherwise
311 */
312bool
313NavGraphClustersThread::robot_pose(Eigen::Vector2f &pose) noexcept
314{
316 tf::Stamped<tf::Point> input(tf::Point(0, 0, 0), fawkes::Time(0, 0), cfg_base_frame_);
317 try {
318 tf_listener->transform_point(cfg_fixed_frame_, input, tpose);
319 pose[0] = tpose.x();
320 pose[1] = tpose.y();
321 return true;
322 } catch (Exception &e) {
323 return false;
324 }
325}
Constraint to block edges close to clusters.
Constraint apply linearly scaled costs based on the distance.
Constraint apply a static cost factor to blocked edges.
std::list< std::tuple< std::string, std::string, Eigen::Vector2f > > blocked_edges_centroids() noexcept
Get a list of edges close to a clusters and its centroid considered blocked.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
bool robot_pose(Eigen::Vector2f &pose) noexcept
Determine current robot pose.
std::list< std::pair< std::string, std::string > > blocked_edges() noexcept
Get a list of edges close to a clusters considered blocked.
virtual ~NavGraphClustersThread()
Destructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*") noexcept
Add interface creation type to watch list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:240
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:197
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:225
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
List with a lock.
Definition: lock_list.h:45
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
Definition: lock_list.h:172
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:237
Mutex locking helper.
Definition: mutex_locker.h:34
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:44
std::string name()
Get name of constraint.
std::string name()
Get name of constraint.
Topological graph edge.
Definition: navgraph_edge.h:38
Position3DInterface Fawkes BlackBoard Interface.
int32_t visibility_history() const
Get visibility_history value.
char * frame() const
Get frame value.
double * translation() const
Get translation value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
A convenience class for universally unique identifiers (UUIDs).
Definition: uuid.h:29
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
void transform_point(const std::string &target_frame, const Stamped< Point > &stamped_in, Stamped< Point > &stamped_out) const
Transform a stamped point into the target frame.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59