23 #include "objpos_average.h" 25 #include <core/threading/mutex_locker.h> 26 #include <blackboard/blackboard.h> 27 #include <logging/logger.h> 28 #include <interfaces/ObjectPositionInterface.h> 51 const char *from_id_pattern,
55 __blackboard = blackboard;
67 if (__to_id == (*i)->id()) {
68 blackboard->
close(*i);
75 blackboard->
close(*i);
78 blackboard->
close(__output_if);
82 bbio_add_observed_create(
"ObjectPositionInterface", from_id_pattern);
90 __blackboard->unregister_observer(
this);
93 for (__iii = __input_ifs.begin(); __iii != __input_ifs.end(); ++__iii) {
94 __blackboard->close(*__iii);
99 __blackboard->close(__output_if);
106 if (__to_id ==
id)
return;
113 __input_ifs.push_back_locked(from_if);
115 if (from_if != NULL) {
116 __blackboard->close(from_if);
128 unsigned int flags = 0;
129 unsigned int base_flags = 0;
130 unsigned int world_num_inputs=0, extent_num_inputs=0, euler_num_inputs = 0,
131 worldvel_num_inputs = 0, relcart_num_inputs = 0, relpolar_num_inputs = 0;
132 float roll = 0, pitch = 0, yaw = 0,
distance = 0, bearing = 0, slope = 0,
133 world_x = 0, world_y = 0, world_z = 0,
134 relative_x = 0, relative_y = 0, relative_z = 0,
135 extent_x = 0, extent_y = 0, extent_z = 0,
136 world_x_velocity = 0, world_y_velocity = 0, world_z_velocity = 0,
137 relative_x_velocity = 0, relative_y_velocity = 0, relative_z_velocity = 0;
138 bool valid =
true, visible =
true;
139 int vishistory_min = 0, vishistory_max = 0;
140 unsigned int object_type = 0;
141 bool object_type_warned =
false;
142 bool flags_read =
false;
143 bool have_world =
false, have_relative =
false;
145 for (__iii = __input_ifs.begin(); __iii != __input_ifs.end(); ++__iii) {
150 if ( (object_type != 0) && (iface->
object_type() != object_type) &&
151 ! object_type_warned) {
152 __logger->log_warn(
"WMObjPosAvgFus",
"Object types of input interfaces " 153 "for %s disagree, %s has %u, expected was %u",
156 object_type_warned =
true;
162 unsigned int iflags = iface->
flags()
166 if (iflags != base_flags) {
167 __logger->log_warn(
"WMObjPosAvgFus",
"Interface flags for %s " 168 "disagree. Exected %x, got %x", base_flags, iflags);
171 base_flags = iface->
flags()
186 world_num_inputs += 1;
189 roll += iface->
roll();
190 pitch += iface->
pitch();
193 euler_num_inputs += 1;
201 worldvel_num_inputs += 1;
206 have_relative =
true;
216 relcart_num_inputs += 1;
220 have_relative =
true;
226 slope += iface->
slope();
227 relpolar_num_inputs += 1;
235 extent_num_inputs += 1;
250 if ( world_num_inputs > 0 ) {
251 __output_if->set_world_x(world_x / (
float)world_num_inputs);
252 __output_if->set_world_y(world_y / (
float)world_num_inputs);
253 __output_if->set_world_z(world_z / (
float)world_num_inputs);
255 if ( euler_num_inputs > 0 ) {
256 __output_if->set_roll(roll / (
float)euler_num_inputs);
257 __output_if->set_pitch(pitch / (
float)euler_num_inputs);
258 __output_if->set_yaw(yaw / (
float)euler_num_inputs);
260 if ( worldvel_num_inputs > 0) {
261 __output_if->set_world_x_velocity(world_x_velocity / (
float)worldvel_num_inputs);
262 __output_if->set_world_y_velocity(world_y_velocity / (
float)worldvel_num_inputs);
263 __output_if->set_world_z_velocity(world_z_velocity / (
float)worldvel_num_inputs);
266 if ( extent_num_inputs > 0 ) {
267 __output_if->set_extent_x(extent_x / (
float)extent_num_inputs);
268 __output_if->set_extent_y(extent_y / (
float)extent_num_inputs);
269 __output_if->set_extent_z(extent_z / (
float)extent_num_inputs);
271 if ( relcart_num_inputs > 0) {
272 __output_if->set_relative_x(relative_x / (
float)relcart_num_inputs);
273 __output_if->set_relative_y(relative_y / (
float)relcart_num_inputs);
274 __output_if->set_relative_z(relative_z / (
float)relcart_num_inputs);
275 __output_if->set_relative_x_velocity(relative_x_velocity / (
float)relcart_num_inputs);
276 __output_if->set_relative_y_velocity(relative_y_velocity / (
float)relcart_num_inputs);
277 __output_if->set_relative_z_velocity(relative_z_velocity / (
float)relcart_num_inputs);
279 if ( relpolar_num_inputs > 0) {
280 __output_if->set_distance(
distance / (
float)relpolar_num_inputs);
281 __output_if->set_bearing(bearing / (
float)relpolar_num_inputs);
282 __output_if->set_slope(slope / (
float)relpolar_num_inputs);
285 visible = have_world || have_relative;
287 __output_if->set_flags(flags);
288 __output_if->set_valid(valid);
289 __output_if->set_visible(visible);
290 __output_if->set_visibility_history(visible ? vishistory_max : vishistory_min);
292 __output_if->write();
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
float relative_y() const
Get relative_y value.
float relative_x_velocity() const
Get relative_x_velocity value.
uint32_t object_type() const
Get object_type value.
WorldModelObjPosAverageFuser(fawkes::Logger *logger, fawkes::BlackBoard *blackboard, const char *from_id_pattern, const char *to_id)
Constructor.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
float relative_x() const
Get relative_x value.
ObjectPositionInterface Fawkes BlackBoard Interface.
static const uint32_t FLAG_HAS_EULER_ANGLES
FLAG_HAS_EULER_ANGLES constant.
Fawkes library namespace.
uint32_t flags() const
Get flags value.
float world_x_velocity() const
Get world_x_velocity value.
float world_z() const
Get world_z value.
float yaw() const
Get yaw value.
float slope() const
Get slope value.
static const uint32_t FLAG_HAS_WORLD
FLAG_HAS_WORLD constant.
float bearing() const
Get bearing value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
float extent_x() const
Get extent_x value.
float world_x() const
Get world_x value.
const char * uid() const
Get unique identifier of interface.
~WorldModelObjPosAverageFuser()
Destructor.
float relative_z_velocity() const
Get relative_z_velocity value.
bool has_writer() const
Check if there is a writer for the interface.
float world_y_velocity() const
Get world_y_velocity value.
static const uint32_t FLAG_HAS_RELATIVE_POLAR
FLAG_HAS_RELATIVE_POLAR constant.
float relative_z() const
Get relative_z value.
Base class for exceptions in Fawkes.
float pitch() const
Get pitch value.
bool is_visible() const
Get visible value.
void read()
Read from BlackBoard into local copy.
static const uint32_t FLAG_HAS_EXTENT
FLAG_HAS_EXTENT constant.
float roll() const
Get roll value.
float world_z_velocity() const
Get world_z_velocity value.
float extent_z() const
Get extent_z value.
float distance() const
Get distance value.
static const uint32_t FLAG_HAS_RELATIVE_CARTESIAN
FLAG_HAS_RELATIVE_CARTESIAN constant.
int32_t visibility_history() const
Get visibility_history value.
void print_trace()
Prints trace to stderr.
float extent_y() const
Get extent_y value.
float world_y() const
Get world_y value.
bool is_valid() const
Get valid value.
static const uint32_t FLAG_HAS_WORLD_VELOCITY
FLAG_HAS_WORLD_VELOCITY constant.
virtual void fuse()
The single function that makes fusers work.
The BlackBoard abstract class.
float relative_y_velocity() const
Get relative_y_velocity value.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*")=0
Open multiple interfaces for reading.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
virtual void close(Interface *interface)=0
Close interface.