00001 #include "wire/WorldModelROS.h"
00002
00003 #include "wire/core/ClassModel.h"
00004 #include "wire/core/Property.h"
00005 #include "wire/core/Evidence.h"
00006 #include "wire/core/EvidenceSet.h"
00007 #include "wire/storage/SemanticObject.h"
00008 #include "wire/util/ObjectModelParser.h"
00009
00010 #include "wire/logic/Hypothesis.h"
00011 #include "wire/logic/HypothesesTree.h"
00012
00013 #include "boost/thread.hpp"
00014
00015 using namespace std;
00016 using namespace mhf;
00017
00018 WorldModelROS::WorldModelROS(tf::TransformListener* tf_listener)
00019 : loop_rate_(20), world_model_(0), tf_listener_(tf_listener), is_tf_owner_(false), last_update_duration(0),
00020 max_update_duration(0), world_model_frame_id_("/map"), output_frame_id_("/map"), max_num_hyps_(100), min_prob_ratio_(1e-10),
00021 last_update_(0) {
00022 initialize();
00023 }
00024
00025 WorldModelROS::~WorldModelROS() {
00026
00027
00028 if (is_tf_owner_) {
00029 delete tf_listener_;
00030 }
00031
00032
00033 delete world_model_;
00034
00035
00036 for (list<ros::Subscriber>::iterator it = subs_evidence_.begin(); it != subs_evidence_.end(); ++it) {
00037 it->shutdown();
00038 }
00039 }
00040
00041 bool WorldModelROS::initialize() {
00042
00043 ros::NodeHandle n("~");
00044
00045
00046 n.getParam("world_model_frame", world_model_frame_id_);
00047 n.getParam("output_frame", output_frame_id_);
00048 n.getParam("max_num_hypotheses", max_num_hyps_);
00049 n.getParam("min_probability_ratio", min_prob_ratio_);
00050
00051 string object_models_filename = "";
00052 n.getParam("knowledge_filename", object_models_filename);
00053
00054 if (object_models_filename == "") {
00055 ROS_ERROR("Parameter 'knowledge_filename' not set.");
00056 return false;
00057 }
00058
00059
00060
00061 ObjectModelParser parser(object_models_filename);
00062 if (!parser.parse(KnowledgeDatabase::getInstance())) {
00063
00064 ROS_ERROR_STREAM("While parsing '" << object_models_filename << "': " << endl << endl << parser.getErrorMessage());
00065 return false;
00066 }
00067
00068
00069 if (!tf_listener_) {
00070 tf_listener_ = new tf::TransformListener();
00071 is_tf_owner_ = true;
00072 }
00073
00074
00075 srv_reset_ = n.advertiseService("reset", &WorldModelROS::resetWorldModel, this);
00076
00077
00078 pub_wm_ = n.advertise<wire_msgs::WorldState>("/world_state", 10);
00079
00080
00081 world_model_ = new HypothesisTree(max_num_hyps_, min_prob_ratio_);
00082
00083 return true;
00084 }
00085
00086 void WorldModelROS::registerEvidenceTopic(const std::string& topic_name) {
00087 ros::NodeHandle n;
00088 subs_evidence_.push_back(n.subscribe(topic_name, 100, &WorldModelROS::evidenceCallback, this));
00089 }
00090
00091 void WorldModelROS::startThreaded() {
00092 processing_thread_ = boost::thread(boost::bind(&WorldModelROS::start, this));
00093 }
00094
00095 void WorldModelROS::start() {
00096 ros::Rate r(loop_rate_);
00097
00098 int count = 0;
00099 while(ros::ok()) {
00100 ros::spinOnce();
00101 processEvidence(r.expectedCycleTime());
00102 publish();
00103 ++count;
00104 if (count == 15) {
00105
00106 count = 0;
00107 }
00108 r.sleep();
00109 }
00110
00111 ros::spinOnce();
00112 }
00113
00114 bool WorldModelROS::objectToMsg(const SemanticObject& obj, wire_msgs::ObjectState& msg) const {
00115 msg.ID = obj.getID();
00116
00117 const map<Attribute, Property*>& properties = obj.getPropertyMap();
00118
00119 for(map<Attribute, Property*>::const_iterator it_prop = properties.begin(); it_prop != properties.end(); ++it_prop) {
00120 Property* prop = it_prop->second;
00121
00122 wire_msgs::Property prop_msg;
00123 prop_msg.attribute = AttributeConv::attribute_str(it_prop->first);
00124 pbl::PDFtoMsg(prop->getValue(), prop_msg.pdf);
00125 msg.properties.push_back(prop_msg);
00126 }
00127
00128 return true;
00129 }
00130
00131 bool WorldModelROS::hypothesisToMsg(const Hypothesis& hyp, wire_msgs::WorldState& msg) const {
00132 ros::Time time = ros::Time::now();
00133
00134 msg.header.frame_id = world_model_frame_id_;
00135 msg.header.stamp = time;
00136
00137 for(list<SemanticObject*>::const_iterator it = hyp.getObjects().begin(); it != hyp.getObjects().end(); ++it) {
00138
00139 SemanticObject* obj_clone = (*it)->clone();
00140 obj_clone->propagate(time.toSec());
00141
00142 wire_msgs::ObjectState obj_msg;
00143 if (objectToMsg(*obj_clone, obj_msg)) {
00144 msg.objects.push_back(obj_msg);
00145 }
00146
00147 delete obj_clone;
00148
00149 }
00150
00151 return true;
00152 }
00153
00154 bool WorldModelROS::transformPosition(const pbl::PDF& pdf_in, const string& frame_in, pbl::Gaussian& pdf_out) const {
00155 const pbl::Gaussian* gauss = pbl::PDFtoGaussian(pdf_in);
00156
00157 if (!gauss) {
00158 ROS_ERROR("Position evidence is not a gaussian!");
00159 return false;
00160 }
00161
00162 const arma::vec& pos = gauss->getMean();
00163 tf::Stamped<tf::Point> pos_stamped(tf::Point(pos(0), pos(1), pos(2)), ros::Time(), frame_in);
00164
00165 try{
00166 tf::Stamped<tf::Point> pos_stamped_world;
00167 tf_listener_->transformPoint(world_model_frame_id_, pos_stamped, pos_stamped_world);
00168
00169 pbl::Vector3 pos_world(pos_stamped_world.getX(), pos_stamped_world.getY(), pos_stamped_world.getZ());
00170 pdf_out.setMean(pos_world);
00171
00172
00173 pdf_out.setCovariance(gauss->getCovariance());
00174
00175 } catch (tf::TransformException& ex){
00176 ROS_ERROR("[WORLD_MODEL] %s",ex.what());
00177 return false;
00178 }
00179 return true;
00180 }
00181
00182 bool WorldModelROS::transformOrientation(const pbl::PDF& pdf_in, const string& frame_in, pbl::Gaussian& pdf_out) const {
00183 const pbl::Gaussian* gauss = pbl::PDFtoGaussian(pdf_in);
00184
00185 if (!gauss) {
00186 ROS_ERROR("Orientation evidence is not a gaussian!");
00187 return false;
00188 }
00189
00190 const arma::vec& ori = gauss->getMean();
00191 tf::Stamped<tf::Quaternion> ori_stamped(tf::Quaternion(ori(0), ori(1), ori(2), ori(3)), ros::Time(), frame_in);
00192
00193 try{
00194 tf::Stamped<tf::Quaternion> ori_stamped_world;
00195 tf_listener_->transformQuaternion(world_model_frame_id_, ori_stamped, ori_stamped_world);
00196
00197 pbl::Vector4 ori_world(ori_stamped_world.getX(), ori_stamped_world.getY(), ori_stamped_world.getZ(), ori_stamped_world.getW());
00198 pdf_out.setMean(ori_world);
00199
00200
00201 pdf_out.setCovariance(gauss->getCovariance());
00202
00203 } catch (tf::TransformException& ex){
00204 ROS_ERROR("[WORLD MODEL] %s",ex.what());
00205 return false;
00206 }
00207 return true;
00208 }
00209
00210 void WorldModelROS::evidenceCallback(const wire_msgs::WorldEvidence::ConstPtr& world_evidence_msg) {
00211 evidence_buffer_.push_back(*world_evidence_msg);
00212 }
00213
00214 void WorldModelROS::processEvidence(const ros::Duration max_duration) {
00215
00216 ros::Time start_time = ros::Time::now();
00217
00218 while(!evidence_buffer_.empty() && ros::Time::now() - start_time < max_duration) {
00219
00220 ros::Time time_before_update = ros::Time::now();
00221
00222 processEvidence(evidence_buffer_.back());
00223
00224 last_update_duration = (ros::Time::now().toSec() - time_before_update.toSec());
00225 max_update_duration = max(max_update_duration, last_update_duration);
00226
00227 evidence_buffer_.pop_back();
00228 }
00229 }
00230
00231 void WorldModelROS::processEvidence(const wire_msgs::WorldEvidence& world_evidence_msg) {
00232 ros::Time current_time = ros::Time::now();
00233
00234 if (current_time < last_update_) {
00235 ROS_WARN("Saw a negative time change of %f seconds; resetting the world model.", (current_time - last_update_).toSec());
00236 delete world_model_;
00237 world_model_ = new HypothesisTree(max_num_hyps_, min_prob_ratio_);
00238 }
00239 last_update_ = current_time;
00240
00241
00242 warnings_.str("");
00243
00244 EvidenceSet evidence_set;
00245 list<Evidence*> measurements_mem;
00246
00247 const vector<wire_msgs::ObjectEvidence>& object_evidence = world_evidence_msg.object_evidence;
00248 for(vector<wire_msgs::ObjectEvidence>::const_iterator it_ev = object_evidence.begin(); it_ev != object_evidence.end(); ++it_ev) {
00249 const wire_msgs::ObjectEvidence& evidence = (*it_ev);
00250
00251
00252 Evidence* meas = new Evidence(current_time.toSec());
00253
00254 measurements_mem.push_back(meas);
00255
00256 bool position_ok = true;
00257
00258 for(vector<wire_msgs::Property>::const_iterator it_prop = evidence.properties.begin(); it_prop != evidence.properties.end(); ++it_prop ) {
00259 const wire_msgs::Property& prop = *it_prop;
00260
00261 pbl::PDF* pdf = pbl::msgToPDF(prop.pdf);
00262
00263 if (pdf) {
00264 if (prop.attribute == "position") {
00265 pbl::Gaussian pos_pdf(3);
00266 if (!transformPosition(*pdf, world_evidence_msg.header.frame_id, pos_pdf)) {
00267
00268 position_ok = false;
00269 break;
00270 } else {
00271 meas->addProperty(AttributeConv::attribute(prop.attribute), pos_pdf);
00272 }
00273 } else if (prop.attribute == "orientation") {
00274 pbl::Gaussian ori_pdf(4);
00275 if (!transformOrientation(*pdf, world_evidence_msg.header.frame_id, ori_pdf)) {
00276 meas->addProperty(AttributeConv::attribute(prop.attribute), ori_pdf);
00277 }
00278 } else {
00279 meas->addProperty(AttributeConv::attribute(prop.attribute), *pdf);
00280 }
00281 delete pdf;
00282 } else {
00283 ROS_ERROR_STREAM("For attribute '" << prop.attribute << "': malformed pdf: " << prop.pdf);
00284 }
00285 }
00286
00287 if (position_ok) {
00288 evidence_set.add(meas);
00289 } else {
00290 ROS_ERROR("Unable to transform position.");
00291 }
00292
00293 }
00294
00295 world_model_->addEvidence(evidence_set);
00296
00297 for(list<Evidence*>::iterator it = measurements_mem.begin(); it != measurements_mem.end(); ++it) {
00298 delete (*it);
00299 }
00300 }
00301
00302 bool WorldModelROS::resetWorldModel(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) {
00303 delete world_model_;
00304 world_model_ = new HypothesisTree(max_num_hyps_, min_prob_ratio_);
00305 return true;
00306 }
00307
00308 void WorldModelROS::publish() const {
00309 wire_msgs::WorldState map_world_msg;
00310 hypothesisToMsg(world_model_->getMAPHypothesis(), map_world_msg);
00311
00312
00313 pub_wm_.publish(map_world_msg);
00314
00315 }
00316
00317 const list<SemanticObject*>& WorldModelROS::getMAPObjects() const {
00318 return world_model_->getMAPObjects();
00319 }
00320
00321 void WorldModelROS::showStatistics() const {
00322 printf("***** %f *****\n", ros::Time::now().toSec());
00323 world_model_->showStatistics();
00324 cout << "Num MAP objects: " << world_model_->getMAPObjects().size() << endl;
00325 cout << "Last update: " << last_update_duration << " seconds" << endl;
00326 cout << "Max update: " << max_update_duration << " seconds" << endl;
00327 cout << "Evidence buffer size: " << evidence_buffer_.size() << endl;
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342 if (!warnings_.str().empty()) {
00343 ROS_WARN_STREAM(warnings_.str());
00344 }
00345 }