WorldModelROS.cpp
Go to the documentation of this file.
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     // delete tf listener
00028     if (is_tf_owner_) {
00029         delete tf_listener_;
00030     }
00031 
00032     // delete multiple hypothesis filter_
00033     delete world_model_;
00034 
00035     // shutdown subscribers
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     // get node handle
00043     ros::NodeHandle n("~");
00044 
00045     // set parameters
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     // Parse object models
00060     // ObjectModelParser parser(object_models_filename);
00061     ObjectModelParser parser(object_models_filename);
00062     if (!parser.parse(KnowledgeDatabase::getInstance())) {
00063         // parsing failed
00064         ROS_ERROR_STREAM("While parsing '" << object_models_filename << "': " << endl << endl << parser.getErrorMessage());
00065         return false;
00066     }
00067 
00068     // create tf listener
00069     if (!tf_listener_) {
00070         tf_listener_ = new tf::TransformListener();
00071         is_tf_owner_ = true;
00072     }
00073 
00074     // Service for resetting the world model
00075     srv_reset_ = n.advertiseService("reset", &WorldModelROS::resetWorldModel, this);
00076 
00077     // Publishers
00078     pub_wm_ = n.advertise<wire_msgs::WorldState>("/world_state", 10);
00079 
00080     // initialize the filter
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             //showStatistics();
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         // todo: also transform covariance
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         // todo: also transform covariance
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     // reset the warnings stringstream
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         //Evidence* meas = new Evidence(world_evidence_msg->header.stamp.toSec(), evidence.certainty, evidence.negative);
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                         // position is a necessary property. If the transform was not successful, abort and don't use the evidence
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     } // end iteration over object evidence list
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     // Publish results
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     const list<Hypothesis*>& hyp_list = world_model_->getHypotheses();
00331     for(list<Hypothesis* >::const_iterator it_hyp = hyp_list.begin(); it_hyp != hyp_list.end(); ++it_hyp) {
00332 
00333         const list<SemanticObject*>& objs = (*it_hyp)->getObjects();
00334 
00335         double hyp_prob = (*it_hyp)->getProbability();
00336 
00337         cout << "Hyp P = " << hyp_prob << ": " << objs.size() << " object(s)" << endl;
00338 
00339     }
00340     */
00341 
00342     if (!warnings_.str().empty()) {
00343         ROS_WARN_STREAM(warnings_.str());
00344     }
00345 }


wire_core
Author(s): Sjoerd van den Dries, Jos Elfring
autogenerated on Tue Jan 7 2014 11:43:19