23 #include "objpos_majority.h"
30 #include <core/threading/mutex_locker.h>
31 #include <blackboard/blackboard.h>
32 #include <logging/logger.h>
73 const std::string& own_id,
74 const std::string& foreign_id_pattern,
75 const std::string& output_id,
76 float self_confidence_radius)
78 blackboard_(blackboard),
80 output_id_(output_id),
81 self_confidence_radius_(self_confidence_radius)
88 foreign_id_pattern.c_str());
89 for (std::list<Opi*>::const_iterator it = input_ifs.begin();
90 it != input_ifs.end(); ++it) {
92 std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert(opi);
94 blackboard->
close(opi);
97 if (own_if_ != NULL) {
98 std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert(own_if_);
100 blackboard->
close(own_if_);
101 own_if_ = *ret.first;
107 if (iter != input_ifs_.end()) {
109 blackboard->
close(opi);
110 input_ifs_.erase(iter);
113 for (OpiSet::const_iterator it = input_ifs_.begin();
114 it != input_ifs_.end(); ++it) {
115 blackboard->
close(*it);
118 if (output_if_ != NULL) {
119 blackboard->
close(output_if_);
128 foreign_id_pattern.c_str());
139 for (OpiSet::const_iterator it = input_ifs_.begin();
140 it != input_ifs_.end(); ++it) {
141 blackboard_->
close(*it);
146 if (output_if_ != NULL) {
147 blackboard_->
close(output_if_);
154 const char *
id)
throw()
156 if (output_id_ ==
id) {
161 from_if = blackboard_->open_for_reading<
Opi>(id);
162 std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert_locked(from_if);
164 blackboard_->close(from_if);
166 Opi* inserted_if = *ret.first;
167 if (own_if_ == NULL && own_id_ == std::string(inserted_if->
id())) {
168 own_if_ = inserted_if;
171 if (from_if != NULL) {
172 blackboard_->close(from_if);
179 WorldModelObjPosMajorityFuser::length(
float x,
float y,
float z)
181 return sqrt(x*x + y*y + z*z);
185 WorldModelObjPosMajorityFuser::rel_length(
const Opi* opi)
187 return length(opi->relative_x(), opi->relative_y(), opi->relative_z());
191 WorldModelObjPosMajorityFuser::world_object_dist(
const Opi* from,
const Opi* to)
193 return length(to->world_x() - from->world_x(),
194 to->world_y() - from->world_y(),
195 to->world_z() - from->world_z());
199 WorldModelObjPosMajorityFuser::same_contents(
const OpiBucket& left,
200 const OpiBucket& right)
202 if (left.size() != right.size()) {
206 std::set<OpiWrapper> rightSet(right.begin(), right.end());
207 for (OpiBucket::const_iterator it = left.begin();
208 it != left.end(); ++it) {
210 if (rightSet.find(opi) == rightSet.end()) {
220 if (own_if_ != NULL) {
224 if (own_if_ != NULL &&
229 rel_length(own_if_) <= self_confidence_radius_) ||
231 own_if_->
distance() <= self_confidence_radius_))) {
239 for (OpiSet::const_iterator it = input_ifs_.begin();
240 it != input_ifs_.end(); ++it) {
242 if (opi != own_if_) {
251 for (OpiSet::const_iterator it = input_ifs_.begin();
252 it != input_ifs_.end(); ++it) {
261 bucket.push_back(opi);
262 for (OpiSet::const_iterator jt = input_ifs_.begin();
263 jt != input_ifs_.end(); ++jt) {
264 Opi* candidate = *jt;
265 if (candidate != opi &&
267 world_object_dist(opi, candidate) <= GROUP_RADIUS) {
268 bucket.push_back(candidate);
271 buckets.push_back(bucket);
277 bool unambiguous =
false;
278 for (OpiBuckets::const_iterator it = buckets.begin();
279 it != buckets.end(); ++it) {
280 const OpiBucket& bucket = *it;
281 if (majority.size() <= bucket.size()) {
282 if (majority.size() < bucket.size()) {
286 unambiguous = unambiguous && same_contents(majority, bucket);
290 if (majority.size() > 0 && unambiguous) {
301 WorldModelObjPosMajorityFuser::check()
303 unsigned int base_flags = 0;
305 unsigned int object_type = 0;
306 bool object_type_warned =
false;
307 bool flags_read =
false;
309 for (OpiSet::const_iterator it = input_ifs_.begin();
310 it != input_ifs_.end(); ++it) {
312 if (!opi->has_writer()) {
315 if (!opi->is_valid()) {
318 if (object_type != 0 && opi->object_type() != object_type &&
319 !object_type_warned) {
320 logger_->
log_warn(
"WMObjPosAvgFus",
"Object types of input interfaces "
321 "for %s disagree, %s has %u, expected was %u",
322 output_id_.c_str(), opi->uid(), opi->object_type(),
324 object_type_warned =
true;
326 object_type = opi->object_type();
330 unsigned int iflags = opi->flags()
334 if (iflags != base_flags) {
335 logger_->
log_warn(
"WMObjPosAvgFus",
"Interface flags for %s "
336 "disagree. Exected %x, got %x", base_flags,
340 base_flags = opi->flags()
342 & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_CARTESIAN)
350 WorldModelObjPosMajorityFuser::copy_own_if()
363 WorldModelObjPosMajorityFuser::average(
const OpiBucket& input_ifs)
365 unsigned int flags = 0;
366 unsigned int world_num_inputs = 0;
367 unsigned int extent_num_inputs = 0;
368 unsigned int euler_num_inputs = 0;
369 unsigned int worldvel_num_inputs = 0;
370 unsigned int relcart_num_inputs = 0;
371 unsigned int relpolar_num_inputs = 0;
377 float bearing = 0.0f;
380 float world_x = 0.0f;
381 float world_y = 0.0f;
382 float world_z = 0.0f;
384 float relative_x = 0.0f;
385 float relative_y = 0.0f;
386 float relative_z = 0.0f;
388 float extent_x = 0.0f;
389 float extent_y = 0.0f;
390 float extent_z = 0.0f;
392 float world_x_velocity = 0.0f;
393 float world_y_velocity = 0.0f;
394 float world_z_velocity = 0.0f;
396 float relative_x_velocity = 0.0f;
397 float relative_y_velocity = 0.0f;
398 float relative_z_velocity = 0.0f;
402 int vishistory_min = 0;
403 int vishistory_max = 0;
404 bool have_world =
false, have_relative =
false;
406 for (OpiBucket::const_iterator it = input_ifs.begin();
407 it != input_ifs.end(); ++it) {
409 if (!opi->has_writer()) {
413 if (!opi->is_valid()) {
417 if (opi->is_visible()) {
422 world_x += opi->world_x();
423 world_y += opi->world_y();
424 world_z += opi->world_z();
425 world_num_inputs += 1;
429 pitch += opi->pitch();
432 euler_num_inputs += 1;
436 world_x_velocity += opi->world_x_velocity();
437 world_y_velocity += opi->world_y_velocity();
438 world_z_velocity += opi->world_z_velocity();
440 worldvel_num_inputs += 1;
445 have_relative =
true;
449 relative_x += opi->relative_x();
450 relative_y += opi->relative_y();
451 relative_z += opi->relative_z();
452 relative_x_velocity += opi->relative_x_velocity();
453 relative_y_velocity += opi->relative_y_velocity();
454 relative_z_velocity += opi->relative_z_velocity();
455 relcart_num_inputs += 1;
459 have_relative =
true;
463 distance += opi->distance();
464 bearing += opi->bearing();
465 slope += opi->slope();
466 relpolar_num_inputs += 1;
470 extent_x += opi->extent_x();
471 extent_y += opi->extent_y();
472 extent_z += opi->extent_z();
474 extent_num_inputs += 1;
477 if (opi->visibility_history() > vishistory_max) {
478 vishistory_max = opi->visibility_history();
481 if (opi->visibility_history() < vishistory_min) {
482 vishistory_min = opi->visibility_history();
487 if (world_num_inputs > 0) {
488 output_if_->
set_world_x(world_x / world_num_inputs);
489 output_if_->
set_world_y(world_y / world_num_inputs);
490 output_if_->
set_world_z(world_z / world_num_inputs);
492 if (euler_num_inputs > 0) {
493 output_if_->
set_roll(roll / euler_num_inputs);
494 output_if_->
set_pitch(pitch / euler_num_inputs);
495 output_if_->
set_yaw(yaw / euler_num_inputs);
497 if (worldvel_num_inputs > 0) {
503 if (extent_num_inputs > 0) {
508 if (relcart_num_inputs > 0) {
516 if (relpolar_num_inputs > 0) {
517 output_if_->
set_distance(distance / (
float)relpolar_num_inputs);
518 output_if_->
set_bearing(bearing / (
float)relpolar_num_inputs);
519 output_if_->
set_slope(slope / (
float)relpolar_num_inputs);
522 visible = have_world || have_relative;
void set_world_y_velocity(const float new_world_y_velocity)
Set world_y_velocity value.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
void set_world_z_velocity(const float new_world_z_velocity)
Set world_z_velocity value.
~WorldModelObjPosMajorityFuser()
Destructor.
void set_world_x_velocity(const float new_world_x_velocity)
Set world_x_velocity value.
void set_relative_y_velocity(const float new_relative_y_velocity)
Set relative_y_velocity value.
void unlock() const
Unlock list.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
void set_valid(const bool new_valid)
Set valid value.
ObjectPositionInterface Fawkes BlackBoard Interface.
static const uint32_t FLAG_HAS_EULER_ANGLES
FLAG_HAS_EULER_ANGLES constant.
void set_world_z(const float new_world_z)
Set world_z value.
void set_extent_y(const float new_extent_y)
Set extent_y value.
void set_flags(const uint32_t new_flags)
Set flags value.
uint32_t flags() const
Get flags value.
void set_distance(const float new_distance)
Set distance value.
void set_roll(const float new_roll)
Set roll value.
void set_relative_y(const float new_relative_y)
Set relative_y value.
static const uint32_t FLAG_HAS_WORLD
FLAG_HAS_WORLD constant.
void write()
Write from local copy into BlackBoard memory.
void lock() const
Lock list.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
bool has_writer() const
Check if there is a writer for the interface.
const char * id() const
Get identifier of interface.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
static const uint32_t FLAG_HAS_RELATIVE_POLAR
FLAG_HAS_RELATIVE_POLAR constant.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Base class for exceptions in Fawkes.
void set_relative_x_velocity(const float new_relative_x_velocity)
Set relative_x_velocity value.
bool is_visible() const
Get visible value.
void read()
Read from BlackBoard into local copy.
void set_relative_z_velocity(const float new_relative_z_velocity)
Set relative_z_velocity value.
static const uint32_t FLAG_HAS_EXTENT
FLAG_HAS_EXTENT constant.
void set_pitch(const float new_pitch)
Set pitch value.
void set_extent_z(const float new_extent_z)
Set extent_z value.
void set_extent_x(const float new_extent_x)
Set extent_x value.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
float distance() const
Get distance value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_visible(const bool new_visible)
Set visible value.
void set_bearing(const float new_bearing)
Set bearing value.
static const uint32_t FLAG_HAS_RELATIVE_CARTESIAN
FLAG_HAS_RELATIVE_CARTESIAN constant.
void print_trace()
Prints trace to stderr.
void set_slope(const float new_slope)
Set slope value.
void set_world_x(const float new_world_x)
Set world_x value.
void set_relative_x(const float new_relative_x)
Set relative_x value.
WorldModelObjPosMajorityFuser(fawkes::Logger *logger, fawkes::BlackBoard *blackboard, const std::string &own_id, const std::string &foreign_id_pattern, const std::string &output_id, float self_confidence_radius)
Constructor.
bool is_valid() const
Get valid value.
static const uint32_t FLAG_HAS_WORLD_VELOCITY
FLAG_HAS_WORLD_VELOCITY constant.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
std::set< OpiWrapper, LessKey >::iterator iterator
Iterator.
virtual void fuse()
The single function that makes fusers work.
The BlackBoard abstract class.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*")=0
Open multiple interfaces for reading.
void set_yaw(const float new_yaw)
Set yaw value.
virtual void copy_values(const Interface *other)
Copy values from other interface.
void set_relative_z(const float new_relative_z)
Set relative_z value.
void set_world_y(const float new_world_y)
Set world_y value.
virtual void close(Interface *interface)=0
Close interface.