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>
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();
131NavGraphClustersThread::bb_interface_created(
const char *type,
const char *
id)
noexcept
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);
167 conditional_close(interface);
174 conditional_close(interface);
178NavGraphClustersThread::conditional_close(
Interface *interface)
noexcept
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());
213std::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)));
233std::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)));
282NavGraphClustersThread::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);
Constraint to block edges close to clusters.
Constraint apply linearly scaled costs based on the distance.
std::list< std::tuple< std::string, std::string, Eigen::Vector2f > > blocked_edges_centroids() noexcept
Get a list of edges close to a clusters and its centroid considered blocked.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
bool robot_pose(Eigen::Vector2f &pose) noexcept
Determine current robot pose.
std::list< std::pair< std::string, std::string > > blocked_edges() noexcept
Get a list of edges close to a clusters considered blocked.
virtual ~NavGraphClustersThread()
Destructor.
NavGraphClustersThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
BlackBoard interface listener.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*") noexcept
Add interface creation type to watch list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
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.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void close(Interface *interface)=0
Close interface.
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.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Base class for all Fawkes BlackBoard interfaces.
void read()
Read from BlackBoard into local copy.
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void log_error(const char *component, const char *format,...)
Log error message.
fawkes::LockPtr< NavGraph > navgraph
NavGraph instance shared in framework.
std::string name()
Get name of constraint.
std::string name()
Get name of constraint.
Position3DInterface Fawkes BlackBoard Interface.
int32_t visibility_history() const
Get visibility_history value.
char * frame() const
Get frame value.
double * translation() const
Get translation value.
Thread class encapsulation of pthreads.
A class for handling time.
A convenience class for universally unique identifiers (UUIDs).
Wrapper class to add time stamp and frame ID to base types.
Fawkes library namespace.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.