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