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);