21 #include "navgraph_clusters_thread.h" 23 #include "clusters_block_constraint.h" 24 #include "clusters_distance_cost_constraint.h" 25 #include "clusters_static_cost_constraint.h" 27 #include <core/threading/mutex_locker.h> 28 #include <interfaces/Position3DInterface.h> 29 #include <navgraph/constraints/constraint_repo.h> 30 #include <navgraph/navgraph.h> 33 #include <Eigen/StdVector> 39 using std::make_tuple;
48 :
Thread(
"NavGraphClustersThread",
Thread::OPMODE_WAITFORWAKEUP),
61 cfg_iface_prefix_ =
config->
get_string(
"/navgraph-clusters/interface-prefix");
62 cfg_close_threshold_ =
config->
get_float(
"/navgraph-clusters/close-threshold");
65 cfg_min_vishistory_ =
config->
get_int(
"/navgraph-clusters/min-visibility-history");
68 std::string pattern = cfg_iface_prefix_ +
"*";
81 edge_constraint_ = NULL;
82 edge_cost_constraint_ = NULL;
83 if (cfg_mode_ ==
"block") {
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");
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");
96 "clusters",
this, cost_min, cost_max, dist_min, dist_max);
97 navgraph->constraint_repo()->register_constraint(edge_cost_constraint_);
99 throw Exception(
"Unknown constraint mode '%s'", cfg_mode_.c_str());
106 if (edge_constraint_) {
107 navgraph->constraint_repo()->unregister_constraint(edge_constraint_->
name());
108 delete edge_constraint_;
111 if (edge_cost_constraint_) {
112 navgraph->constraint_repo()->unregister_constraint(edge_cost_constraint_->
name());
113 delete edge_cost_constraint_;
122 cluster_ifs_.clear();
131 NavGraphClustersThread::bb_interface_created(
const char *type,
const char *
id)
throw()
143 bbil_add_reader_interface(pif);
144 bbil_add_writer_interface(pif);
147 logger->
log_warn(name(),
"Failed to register for %s:%s: %s", type,
id, e.
what());
149 bbil_remove_reader_interface(pif);
150 bbil_remove_writer_interface(pif);
152 blackboard->
close(pif);
155 name(),
"Failed to deregister %s:%s during error recovery: %s", type,
id, e.
what());
160 cluster_ifs_.push_back_locked(pif);
164 NavGraphClustersThread::bb_interface_writer_removed(
fawkes::Interface *interface,
165 unsigned int instance_serial)
throw()
167 conditional_close(interface);
171 NavGraphClustersThread::bb_interface_reader_removed(
fawkes::Interface *interface,
172 unsigned int instance_serial)
throw()
174 conditional_close(interface);
178 NavGraphClustersThread::conditional_close(
Interface *interface)
throw()
186 std::find(cluster_ifs_.begin(), cluster_ifs_.end(), pif);
188 if (c != cluster_ifs_.end() && (!interface->has_writer() && (interface->num_readers() == 1))) {
190 logger->
log_info(name(),
"Last on %s, closing", interface->uid());
192 cluster_ifs_.erase(c);
198 std::string uid = interface->uid();
200 bbil_remove_reader_interface(interface);
201 bbil_remove_writer_interface(interface);
203 blackboard->
close(interface);
205 logger->
log_error(name(),
"Failed to unregister or close %s: %s", uid.c_str(), e.
what());
213 std::list<std::pair<std::string, std::string>>
216 std::list<std::pair<std::string, std::string>> blocked;
217 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>> blocked_c =
220 std::for_each(blocked_c.begin(),
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)));
233 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>
237 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>> blocked;
239 const std::vector<NavGraphEdge> &graph_edges =
navgraph->edges();
247 Eigen::Vector2f centroid(fixed_frame_pose(
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();
258 if (t >= 0.0 && t <= 1.0) {
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));
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)));
282 NavGraphClustersThread::fixed_frame_pose(std::string frame,
287 if (frame == cfg_fixed_frame_) {
288 return Eigen::Vector2f(x, y);
298 return Eigen::Vector2f(tpose.x(), tpose.y());
318 tf_listener->transform_point(cfg_fixed_frame_, input, tpose);
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Constraint to block edges close to clusters.
char * frame() const
Get frame value.
virtual void init()
Initialize the thread.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
virtual void log_error(const char *component, const char *format,...)
Log error message.
Fawkes library namespace.
bool robot_pose(Eigen::Vector2f &pose)
Determine current robot pose.
NavGraphClustersThread()
Constructor.
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.
virtual const char * what() const
Get primary string.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Base class for all Fawkes BlackBoard interfaces.
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
virtual void loop()
Code to execute in the thread.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
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.
virtual ~NavGraphClustersThread()
Destructor.
void read()
Read from BlackBoard into local copy.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
std::string name()
Get name of constraint.
Constraint apply linearly scaled costs based on the distance.
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).
double * translation() const
Get translation value.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Wrapper class to add time stamp and frame ID to base types.
virtual void finalize()
Finalize the thread.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
int32_t visibility_history() const
Get visibility_history value.
Configuration * config
This is the Configuration member used to access the configuration.
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.
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.
virtual void close(Interface *interface)=0
Close interface.