WorldModelROS.h
Go to the documentation of this file.
00001 #ifndef WORLDMODELROS_H_
00002 #define WORLDMODELROS_H_
00003 
00004 #include "wire/core/datatypes.h"
00005 
00006 // ros
00007 #include "ros/ros.h"
00008 
00009 #include "wire_msgs/WorldEvidence.h"
00010 #include "wire_msgs/ObjectEvidence.h"
00011 
00012 #include "wire_msgs/WorldState.h"
00013 #include "wire_msgs/ObjectState.h"
00014 
00015 // services
00016 #include "std_srvs/Empty.h"  // for reset 'button'
00017 
00018 #include "problib/conversions.h"
00019 
00020 // transform listener
00021 #include "tf/transform_listener.h"
00022 
00023 #include "wire/storage/KnowledgeDatabase.h"
00024 
00025 namespace mhf {
00026 
00027 class HypothesisTree;
00028 class Hypothesis;
00029 class KnowledgeProbModel;
00030 class PropertySet;
00031 class ClassModel;
00032 class SemanticObject;
00033 
00034 class WorldModelROS {
00035 
00036 public:
00037 
00038     WorldModelROS(tf::TransformListener* tf_listener = 0);
00039 
00040     virtual ~WorldModelROS();
00041 
00042     void registerEvidenceTopic(const std::string& topic_name);
00043 
00044     void publish() const;
00045 
00046     void showStatistics() const;
00047 
00048     void processEvidence(const ros::Duration max_duration);
00049 
00050     void processEvidence(const wire_msgs::WorldEvidence& world_evidence_msg);
00051 
00052     void start();
00053 
00054     void startThreaded();
00055 
00056     const std::list<SemanticObject*>& getMAPObjects() const;
00057 
00058 protected:
00059 
00060     boost::thread processing_thread_;
00061 
00062     double loop_rate_;
00063 
00064     // evidence buffer
00065     std::list<wire_msgs::WorldEvidence> evidence_buffer_;
00066 
00067     // multiple hypothesis filter_
00068     mhf::HypothesisTree* world_model_;
00069 
00070     // transform listener
00071     tf::TransformListener* tf_listener_;
00072     bool is_tf_owner_;
00073 
00074     // world model publishers
00075     ros::Publisher pub_wm_;
00076 
00077     // evidence subscriber
00078     std::list<ros::Subscriber> subs_evidence_;
00079 
00080     ros::ServiceServer srv_reset_;
00081 
00082     // computation time needed for last tree update
00083     double last_update_duration;
00084     double max_update_duration;
00085 
00086     // frame in which objects are tracked and stored in world model
00087     std::string world_model_frame_id_;
00088 
00089     // frame in which objects are published
00090     std::string output_frame_id_;
00091 
00092     std::stringstream warnings_;
00093 
00094     int max_num_hyps_;
00095 
00096     double min_prob_ratio_;
00097 
00098     ros::Time last_update_;
00099 
00100     bool initialize();
00101 
00102     void initializeMHF();
00103 
00104     bool objectToMsg(const SemanticObject& obj, wire_msgs::ObjectState& msg) const;
00105 
00106     bool hypothesisToMsg(const mhf::Hypothesis& hyp, wire_msgs::WorldState& msg) const;
00107 
00108     void printWorldObjects(const mhf::Hypothesis& hyp) const;
00109 
00110     bool transformPosition(const pbl::PDF& pdf_in, const std::string& frame_in, pbl::Gaussian& pdf_out) const;
00111 
00112     bool transformOrientation(const pbl::PDF& pdf_in, const std::string& frame_in, pbl::Gaussian& pdf_out) const;
00113 
00114     void evidenceCallback(const wire_msgs::WorldEvidence::ConstPtr& world_evidence_msg);
00115 
00116     bool resetWorldModel(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00117 
00118     void shutdown();
00119 
00120 };
00121 
00122 }
00123 
00124 #endif /* WORLDMODELROS_H_ */


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