00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "wm_thread.h"
00024 #include "net_thread.h"
00025
00026 #include "fusers/single_copy.h"
00027 #include "fusers/multi_copy.h"
00028 #include "fusers/objpos_average.h"
00029
00030 #include <netcomm/worldinfo/transceiver.h>
00031 #include <utils/system/pathparser.h>
00032 #include <geometry/hom_point.h>
00033 #include <geometry/hom_vector.h>
00034
00035 #include <interfaces/GameStateInterface.h>
00036 #include <interfaces/ObjectPositionInterface.h>
00037
00038 #include <cmath>
00039 #include <cstring>
00040
00041 using namespace std;
00042 using namespace fawkes;
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 WorldModelThread::WorldModelThread(WorldModelNetworkThread* net_thread)
00054 : Thread("WorldModelThread", Thread::OPMODE_WAITFORWAKEUP),
00055 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
00056 {
00057 __net_thread = net_thread;
00058
00059 __wi_send_enabled = false;
00060 __wi_send_interval = 20;
00061 __wi_send_counter = 1;
00062
00063 __wi_send_pose = NULL;
00064 __wi_send_ball = NULL;
00065
00066
00067 __wi_send_interval = 15;
00068 __wi_send_counter = 1;
00069 }
00070
00071
00072
00073 WorldModelThread::~WorldModelThread()
00074 {
00075 }
00076
00077 void
00078 WorldModelThread::init()
00079 {
00080 try {
00081 __cfg_confspace = config->get_string("/worldmodel/confspace");
00082
00083 logger->log_debug("WorldModelThread", "Config space: %s", __cfg_confspace.c_str());
00084
00085 std::string prefix = "/worldmodel/interfaces/" + __cfg_confspace + "/";
00086 std::list<std::string> combos;
00087
00088 Configuration::ValueIterator *vi = config->search(prefix.c_str());
00089 while (vi->next()) {
00090 if (strcmp(vi->type(), "string") == 0) {
00091 PathParser pp(vi->path());
00092 if ( pp.size() > 1 ) {
00093 combos.push_back(pp[pp.size() - 2]);
00094 }
00095 }
00096 }
00097 combos.sort();
00098 combos.unique();
00099
00100 for (std::list<std::string>::iterator i = combos.begin(); i != combos.end(); ++i) {
00101 std::string type = config->get_string((prefix + *i + "/type").c_str());
00102 std::string from_id = config->get_string((prefix + *i + "/from_id").c_str());
00103 std::string to_id = config->get_string((prefix + *i + "/to_id").c_str());
00104 std::string method = config->get_string((prefix + *i + "/method").c_str());
00105
00106 if (method == "copy") {
00107 if (from_id.find_first_of("*?[") == std::string::npos) {
00108 logger->log_debug(name(), "Instantiating SingleCopyFuser for %s -> %s (type: %s)",
00109 from_id.c_str(), to_id.c_str(), type.c_str());
00110 WorldModelSingleCopyFuser *fuser = new WorldModelSingleCopyFuser(blackboard, type.c_str(),
00111 from_id.c_str(), to_id.c_str());
00112 __fusers.push_back(fuser);
00113 } else {
00114 logger->log_debug(name(), "Instantiating MultiCopyFuser for %s -> %s (type: %s)",
00115 from_id.c_str(), to_id.c_str(), type.c_str());
00116 WorldModelMultiCopyFuser *fuser = new WorldModelMultiCopyFuser(blackboard, type.c_str(),
00117 from_id.c_str(), to_id.c_str());
00118 __fusers.push_back(fuser);
00119 }
00120 } else if (method == "average") {
00121
00122 if (type != "ObjectPositionInterface") {
00123 throw Exception("Can only average interfaces of type ObjectPositionInterface");
00124 }
00125 logger->log_debug(name(), "Instantiating ObjPosAverageFuser for %s -> %s (type: %s)",
00126 from_id.c_str(), to_id.c_str(), type.c_str());
00127 WorldModelObjPosAverageFuser *fuser = new WorldModelObjPosAverageFuser(logger, blackboard,
00128 from_id.c_str(), to_id.c_str());
00129 __fusers.push_back(fuser);
00130
00131 } else {
00132 throw Exception("Unknown fuse method '%s', for interface %s -> %s (type %s)",
00133 method.c_str(), from_id.c_str(), to_id.c_str(), type.c_str());
00134 }
00135 }
00136
00137 } catch (Exception &e) {
00138 e.print_trace();
00139 }
00140
00141 __wi_send_enabled = false;
00142 try {
00143 std::string prefix = "/worldmodel/wi_send/" + __cfg_confspace + "/";
00144 __wi_send_enabled = config->get_bool((prefix + "enable_send").c_str());
00145
00146 if (__wi_send_enabled) {
00147 logger->log_debug(name(), "Sending worldinfo messages enabled");
00148
00149 std::string pose_id = config->get_string((prefix + "pose_id").c_str());
00150 std::string ball_id = config->get_string((prefix + "ball_id").c_str());
00151
00152 logger->log_debug(name(), "Obtaining pose worldinfo data from interface %s.",
00153 pose_id.c_str());
00154 logger->log_debug(name(), "Obtaining ball worldinfo data from interface %s.",
00155 ball_id.c_str());
00156
00157 __wi_send_pose = blackboard->open_for_reading<ObjectPositionInterface>(pose_id.c_str());
00158 __wi_send_ball = blackboard->open_for_reading<ObjectPositionInterface>(ball_id.c_str());
00159
00160 } else {
00161 logger->log_debug(name(), "Sending worldinfo messages disabled");
00162 }
00163
00164 } catch (Exception& e) {
00165 if ( __wi_send_enabled) {
00166 throw;
00167 } else {
00168 logger->log_debug(name(), "Sending worldinfo messages disabled (enable not set)");
00169 }
00170 }
00171
00172 }
00173
00174
00175
00176
00177
00178
00179 void
00180 WorldModelThread::init_failure_cleanup()
00181 {
00182 }
00183
00184
00185 void
00186 WorldModelThread::finalize()
00187 {
00188 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
00189 delete *__fit;
00190 }
00191 __fusers.clear();
00192
00193 if (__wi_send_enabled) {
00194 try {
00195 blackboard->close(__wi_send_pose);
00196 blackboard->close(__wi_send_ball);
00197 } catch (Exception& e) {
00198 e.print_trace();
00199 }
00200 }
00201 }
00202
00203
00204 void
00205 WorldModelThread::loop()
00206 {
00207 for (__fit = __fusers.begin(); __fit != __fusers.end(); ++__fit) {
00208 (*__fit)->fuse();
00209 }
00210
00211
00212 if ( 0 != (__wi_send_counter % __wi_send_interval) ) {
00213 ++__wi_send_counter;
00214 return;
00215 }
00216
00217 __wi_send_counter = 1;
00218
00219 WorldInfoTransceiver* transceiver = __net_thread->get_transceiver();
00220
00221 if (__wi_send_enabled) {
00222 __wi_send_pose->read();
00223 __wi_send_ball->read();
00224
00225 bool do_send = false;
00226
00227
00228 HomPoint pos;
00229 pos.x( __wi_send_pose->world_x() );
00230 pos.y( __wi_send_pose->world_y() );
00231 float yaw = __wi_send_pose->world_z();
00232 if (__wi_send_pose->has_writer()) {
00233 do_send = true;
00234 transceiver->set_pose(pos.x(), pos.y(), yaw,
00235 __wi_send_pose->world_xyz_covariance() );
00236 transceiver->set_velocity(__wi_send_pose->world_x_velocity(),
00237 __wi_send_pose->world_y_velocity(),
00238 __wi_send_pose->world_z_velocity(),
00239 __wi_send_pose->world_xyz_velocity_covariance());
00240
00241
00242 if (__wi_send_ball->has_writer() && __wi_send_ball->is_valid()) {
00243 if (__wi_send_ball->flags() & ObjectPositionInterface::FLAG_HAS_WORLD) {
00244 transceiver->set_glob_ball_pos(__wi_send_ball->world_x(),
00245 __wi_send_ball->world_y(),
00246 __wi_send_ball->world_z(),
00247 __wi_send_ball->world_xyz_covariance() );
00248 } else {
00249
00250 HomVector relative_ball;
00251 relative_ball.x( __wi_send_ball->relative_x() );
00252 relative_ball.y( __wi_send_ball->relative_y() );
00253 relative_ball.rotate_z( yaw );
00254 HomPoint global_ball = pos + relative_ball;
00255
00256 transceiver->set_glob_ball_pos(global_ball.x(), global_ball.y(), 0.0,
00257 __wi_send_ball->dbs_covariance() );
00258 }
00259 transceiver->set_glob_ball_visible(__wi_send_ball->is_visible(),
00260 __wi_send_ball->visibility_history());
00261
00262
00263
00264
00265
00266
00267 }
00268 }
00269
00270 if (do_send) {
00271 transceiver->send();
00272 }
00273 }
00274 }
