23 #include "wm_thread.h"
24 #include "net_thread.h"
26 #include "fusers/single_copy.h"
27 #include "fusers/multi_copy.h"
28 #include "fusers/objpos_average.h"
29 #include "fusers/objpos_majority.h"
31 #include <netcomm/worldinfo/transceiver.h>
32 #include <utils/system/pathparser.h>
33 #include <geometry/hom_point.h>
34 #include <geometry/hom_vector.h>
36 #include <interfaces/GameStateInterface.h>
37 #include <interfaces/ObjectPositionInterface.h>
55 :
Thread(
"WorldModelThread",
Thread::OPMODE_WAITFORWAKEUP),
58 __net_thread = net_thread;
60 __wi_send_enabled =
false;
61 __wi_send_interval = 20;
62 __wi_send_counter = 1;
64 __wi_send_pose = NULL;
65 __wi_send_ball = NULL;
68 __wi_send_interval = 15;
69 __wi_send_counter = 1;
84 logger->
log_debug(
"WorldModelThread",
"Config space: %s", __cfg_confspace.c_str());
86 std::string prefix =
"/worldmodel/interfaces/" + __cfg_confspace +
"/";
87 std::list<std::string> combos;
91 if (strcmp(vi->
type(),
"string") == 0) {
93 if ( pp.size() > 1 ) {
94 combos.push_back(pp[pp.size() - 2]);
101 for (std::list<std::string>::iterator i = combos.begin(); i != combos.end(); ++i) {
103 std::string from_id =
config->
get_string((prefix + *i +
"/from_id").c_str());
107 if (method ==
"copy") {
108 if (from_id.find_first_of(
"*?[") == std::string::npos) {
110 from_id.c_str(), to_id.c_str(), type.c_str());
112 from_id.c_str(), to_id.c_str());
113 __fusers.push_back(fuser);
116 from_id.c_str(), to_id.c_str(), type.c_str());
118 from_id.c_str(), to_id.c_str());
119 __fusers.push_back(fuser);
121 }
else if (method ==
"average") {
123 if (type !=
"ObjectPositionInterface") {
124 throw Exception(
"Can only average interfaces of type ObjectPositionInterface");
127 from_id.c_str(), to_id.c_str(), type.c_str());
129 from_id.c_str(), to_id.c_str());
130 __fusers.push_back(fuser);
131 }
else if (method ==
"majority") {
133 if (type !=
"ObjectPositionInterface") {
134 throw Exception(
"Can only average interfaces of type ObjectPositionInterface");
137 float self_confidence_radius =
config->
get_float((prefix + *i +
"/confidence_radius").c_str());
139 from_id.c_str(), to_id.c_str(), type.c_str());
144 self_confidence_radius);
145 __fusers.push_back(fuser);
147 throw Exception(
"Unknown fuse method '%s', for interface %s -> %s (type %s)",
148 method.c_str(), from_id.c_str(), to_id.c_str(), type.c_str());
156 __wi_send_enabled =
false;
158 std::string prefix =
"/worldmodel/wi_send/" + __cfg_confspace +
"/";
159 __wi_send_enabled =
config->
get_bool((prefix +
"enable_send").c_str());
161 if (__wi_send_enabled) {
180 if ( __wi_send_enabled) {
203 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
208 if (__wi_send_enabled) {
222 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
227 if ( 0 != (__wi_send_counter % __wi_send_interval) ) {
232 __wi_send_counter = 1;
236 if (__wi_send_enabled) {
237 __wi_send_pose->
read();
238 __wi_send_ball->
read();
240 bool do_send =
false;
246 float yaw = __wi_send_pose->
yaw();
258 if (__wi_send_ball->
flags() & ObjectPositionInterface::FLAG_HAS_WORLD) {
269 HomPoint global_ball = pos + relative_ball;
float relative_y() const
Get relative_y value.
void send()
Send information.
float * world_xyz_velocity_covariance() const
Get world_xyz_velocity_covariance value.
Class to send and receive world information.
virtual float y() const
RO-getter for y.
float relative_x() const
Get relative_x value.
void init_failure_cleanup()
Clean up when init failed.
ObjectPositionInterface Fawkes BlackBoard Interface.
virtual const char * type() const =0
Type of value.
void set_glob_ball_pos(float x, float y, float z, float *covariance)
Set global ball position.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void loop()
Code to execute in the thread.
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.
virtual void finalize()
Finalize the thread.
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual ~WorldModelThread()
Destructor.
float yaw() const
Get yaw value.
virtual bool next()=0
Check if there is another element and advance to this if possible.
Thread class encapsulation of pthreads.
Multi interface copy fuser.
Logger * logger
This is the Logger member used to access the logger.
float world_x() const
Get world_x value.
Single interface copy fuser.
bool has_writer() const
Check if there is a writer for the interface.
float world_y_velocity() const
Get world_y_velocity value.
ObjectPositionInterface majority fuser.
Thread aspect to use blocked timing.
fawkes::WorldInfoTransceiver * get_transceiver()
Access the WI transceiver.
void set_pose(float x, float y, float theta, float *covariance)
Set global pose of robot.
Base class for exceptions in Fawkes.
float * dbs_covariance() const
Get dbs_covariance value.
bool is_visible() const
Get visible value.
void read()
Read from BlackBoard into local copy.
WorldModelThread(WorldModelNetworkThread *net_thread)
Constructor.
float world_z_velocity() const
Get world_z_velocity value.
virtual void init()
Initialize the thread.
ObjectPositionModel average fuser.
virtual const char * path() const =0
Path of value.
const char * name() const
Get name of thread.
int32_t visibility_history() const
Get visibility_history value.
void print_trace()
Prints trace to stderr.
virtual HomCoord & rotate_z(float rad)
Convenience function to rotate the HomCoord around the z-axis.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float world_y() const
Get world_y value.
bool is_valid() const
Get valid value.
Iterator interface to iterate over config values.
float * world_xyz_covariance() const
Get world_xyz_covariance value.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
void set_glob_ball_visible(bool visible, int visibility_history)
Set ball visibility for the global ball.
Network thread of worldmodel plugin.
virtual float x() const
RO-getter for x.
void set_velocity(float vel_x, float vel_y, float vel_theta, float *covariance)
Set velocity of the robot.
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.