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 
36 using namespace fawkes;
37 
38 using std::find;
39 using 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 
58 void
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 
103 void
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 
125 void
127 {
128 }
129 
130 void
131 NavGraphClustersThread::bb_interface_created(const char *type, const char *id) throw()
132 {
133  Position3DInterface *pif;
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 
163 void
164 NavGraphClustersThread::bb_interface_writer_removed(fawkes::Interface *interface,
165  unsigned int instance_serial) throw()
166 {
167  conditional_close(interface);
168 }
169 
170 void
171 NavGraphClustersThread::bb_interface_reader_removed(fawkes::Interface *interface,
172  unsigned int instance_serial) throw()
173 {
174  conditional_close(interface);
175 }
176 
177 void
178 NavGraphClustersThread::conditional_close(Interface *interface) throw()
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  */
213 std::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  */
233 std::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 
281 Eigen::Vector2f
282 NavGraphClustersThread::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  */
312 bool
313 NavGraphClustersThread::robot_pose(Eigen::Vector2f &pose) throw()
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 }
fawkes::MultiLogger::log_error
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:237
NavGraphClustersDistanceCostConstraint
Constraint apply linearly scaled costs based on the distance.
Definition: clusters_distance_cost_constraint.h:35
fawkes::Position3DInterface::visibility_history
int32_t visibility_history() const
Get visibility_history value.
Definition: Position3DInterface.cpp:113
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
NavGraphClustersThread::blocked_edges_centroids
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.
Definition: navgraph_clusters_thread.cpp:234
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
fawkes::Position3DInterface::frame
char * frame() const
Get frame value.
Definition: Position3DInterface.cpp:75
fawkes::NavGraphEdge
Topological graph edge.
Definition: navgraph_edge.h:38
NavGraphClustersThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navgraph_clusters_thread.cpp:104
fawkes::MutexLocker
Mutex locking helper.
Definition: mutex_locker.h:34
fawkes::LockList
List with a lock.
Definition: lock_list.h:45
fawkes::BlackBoard::unregister_listener
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
fawkes::BlackBoardInterfaceListener
BlackBoard interface listener.
Definition: interface_listener.h:42
fawkes::Configuration::get_int
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
fawkes::BlackBoardInterfaceObserver::bbio_add_observed_create
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
Definition: interface_observer.cpp:119
fawkes::tf::Stamped
Wrapper class to add time stamp and frame ID to base types.
Definition: types.h:130
fawkes::Position3DInterface::translation
double * translation() const
Get translation value.
Definition: Position3DInterface.cpp:214
fawkes::LockList::mutex
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
Definition: lock_list.h:172
fawkes::MultiLogger::log_warn
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
NavGraphClustersThread::init
virtual void init()
Initialize the thread.
Definition: navgraph_clusters_thread.cpp:59
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
NavGraphClustersBlockConstraint
Constraint to block edges close to clusters.
Definition: clusters_block_constraint.h:32
fawkes
Fawkes library namespace.
fawkes::distance
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59
fawkes::BlackBoard::register_observer
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:225
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
NavGraphClustersThread::blocked_edges
std::list< std::pair< std::string, std::string > > blocked_edges()
Get a list of edges close to a clusters considered blocked.
Definition: navgraph_clusters_thread.cpp:214
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::TransformAspect::tf_listener
tf::Transformer * tf_listener
This is the transform listener which saves transforms published by other threads in the system.
Definition: tf.h:67
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
fawkes::NavGraphEdgeCostConstraint::name
std::string name()
Get name of constraint.
Definition: edge_cost_constraint.cpp:79
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoard::unregister_observer
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:240
fawkes::Position3DInterface
Position3DInterface Fawkes BlackBoard Interface.
Definition: Position3DInterface.h:34
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::NavGraphAspect::navgraph
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
Definition: navgraph.h:44
fawkes::tf::Transformer::transform_point
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.
Definition: transformer.cpp:410
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
fawkes::BlackBoardInterfaceListener::bbil_add_reader_interface
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
Definition: interface_listener.cpp:258
NavGraphClustersStaticCostConstraint
Constraint apply a static cost factor to blocked edges.
Definition: clusters_static_cost_constraint.h:32
fawkes::BlackBoard::open_multiple_for_reading
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.
NavGraphClustersThread::robot_pose
bool robot_pose(Eigen::Vector2f &pose)
Determine current robot pose.
Definition: navgraph_clusters_thread.cpp:313
NavGraphClustersThread::NavGraphClustersThread
NavGraphClustersThread()
Constructor.
Definition: navgraph_clusters_thread.cpp:47
fawkes::BlackBoard::update_listener
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:197
fawkes::BlackBoardInterfaceListener::bbil_add_writer_interface
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
Definition: interface_listener.cpp:270
NavGraphClustersThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navgraph_clusters_thread.cpp:126
NavGraphClustersThread::~NavGraphClustersThread
virtual ~NavGraphClustersThread()
Destructor.
Definition: navgraph_clusters_thread.cpp:54
fawkes::NavGraphEdgeConstraint::name
std::string name()
Get name of constraint.
Definition: edge_constraint.cpp:77
fawkes::MultiLogger::log_info
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36