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" 26 #include <core/threading/mutex_locker.h> 27 #include <navgraph/navgraph.h> 29 #include <interfaces/Position3DInterface.h> 30 #include <navgraph/constraints/constraint_repo.h> 32 #include <Eigen/StdVector> 38 using std::make_tuple;
47 :
Thread(
"NavGraphClustersThread",
Thread::OPMODE_WAITFORWAKEUP),
60 cfg_iface_prefix_ =
config->
get_string(
"/navgraph-clusters/interface-prefix");
61 cfg_close_threshold_ =
config->
get_float(
"/navgraph-clusters/close-threshold");
64 cfg_min_vishistory_ =
config->
get_int(
"/navgraph-clusters/min-visibility-history");
67 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");
88 edge_cost_constraint_ =
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_ =
98 cost_min, cost_max, dist_min, dist_max);
99 navgraph->constraint_repo()->register_constraint(edge_cost_constraint_);
101 throw Exception(
"Unknown constraint mode '%s'", cfg_mode_.c_str());
108 if (edge_constraint_) {
109 navgraph->constraint_repo()->unregister_constraint(edge_constraint_->
name());
110 delete edge_constraint_;
113 if (edge_cost_constraint_) {
114 navgraph->constraint_repo()->unregister_constraint(edge_cost_constraint_->
name());
115 delete edge_cost_constraint_;
124 cluster_ifs_.clear();
133 NavGraphClustersThread::bb_interface_created(
const char *type,
const char *
id)
throw()
167 NavGraphClustersThread::bb_interface_writer_removed(
fawkes::Interface *interface,
168 unsigned int instance_serial)
throw()
170 conditional_close(interface);
174 NavGraphClustersThread::bb_interface_reader_removed(
fawkes::Interface *interface,
175 unsigned int instance_serial)
throw()
177 conditional_close(interface);
181 NavGraphClustersThread::conditional_close(
Interface *interface)
throw()
189 std::find(cluster_ifs_.begin(), cluster_ifs_.end(), pif);
191 if (c != cluster_ifs_.end() &&
197 cluster_ifs_.erase(c);
203 std::string uid = interface->
uid();
211 uid.c_str(), e.
what());
220 std::list<std::pair<std::string, std::string>>
223 std::list<std::pair<std::string, std::string>> blocked;
224 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>> blocked_c =
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)));
240 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>>
244 std::list<std::tuple<std::string, std::string, Eigen::Vector2f>> blocked;
246 const std::vector<NavGraphEdge> &graph_edges =
navgraph->edges();
250 if (pif->visibility_history() >= cfg_min_vishistory_) {
254 Eigen::Vector2f centroid(fixed_frame_pose(pif->frame(),
fawkes::Time(0,0),
255 pif->translation(0), pif->translation(1)));
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();
265 if (t >= 0.0 && t <= 1.0) {
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));
278 blocked.sort([](
const std::tuple<std::string, std::string, Eigen::Vector2f> &a,
279 const std::tuple<std::string, std::string, Eigen::Vector2f> &b) {
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)));
291 NavGraphClustersThread::fixed_frame_pose(std::string frame,
const fawkes::Time ×tamp,
294 if (frame == cfg_fixed_frame_) {
295 return Eigen::Vector2f(x, y);
305 return Eigen::Vector2f(tpose.x(), tpose.y());
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
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.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
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.
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.
Logger * logger
This is the Logger member used to access the logger.
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.
virtual void unlock() const
Unlock list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
std::string name()
Get name of constraint.
bool has_writer() const
Check if there is a writer for the interface.
Constraint apply linearly scaled costs based on the distance.
const char * name() const
Get name of thread.
const char * uid() const
Get unique identifier of interface.
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).
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.
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.
unsigned int num_readers() const
Get the number of readers.
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.
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.
virtual void close(Interface *interface)=0
Close interface.