get_world_state.cpp
Go to the documentation of this file.
00001 /*
00002  * get_world_state.cpp
00003  *
00004  *  Created on: Nov 13, 2012
00005  *      Author: jelfring
00006  */
00007 
00008 #include "ros/ros.h"
00009 #include "wire_msgs/ObjectState.h"
00010 #include "wire_msgs/WorldState.h"
00011 
00012 #include "problib/conversions.h"
00013 
00014 #include <vector>
00015 
00016 using namespace std;
00017 
00018 
00019 // Function that extract the Gaussian with the highest weight from a mixture of Gaussians and/or a uniform distribution
00020 const pbl::Gaussian* getBestGaussianFromMixture(const pbl::Mixture& mix, double min_weight = 0) {
00021         const pbl::Gaussian* G_best = 0;
00022         double w_best = min_weight;
00023 
00024         // Iterate over all components
00025         for(int i = 0; i < mix.components(); ++i) {
00026                 const pbl::PDF& pdf = mix.getComponent(i);
00027                 const pbl::Gaussian* G = pbl::PDFtoGaussian(pdf);
00028                 double w = mix.getWeight(i);
00029 
00030                 // Get Gaussian with the highest associated weight
00031                 if (G && w > w_best) {
00032                         G_best = G;
00033                         w_best = w;
00034                 }
00035         }
00036         return G_best;
00037 }
00038 
00039 
00040 
00041 void worldStateCallback(const wire_msgs::WorldState::ConstPtr& msg) {
00042 
00043         ROS_INFO("Received world state with %d objects", msg->objects.size());
00044 
00045 
00046         // Iterate over world model objects
00047         for (vector<wire_msgs::ObjectState>::const_iterator it_obj = msg->objects.begin(); it_obj != msg->objects.end(); ++it_obj) {
00048 
00049                 // Get object from the object array
00050                 wire_msgs::ObjectState obj = *it_obj;
00051                 ROS_INFO(" Object:");
00052 
00053                 // Iterate over all properties
00054                 for (vector<wire_msgs::Property>::const_iterator it_prop = obj.properties.begin(); it_prop != obj.properties.end(); ++it_prop) {
00055 
00056                         // Get the pdf of the current property
00057                         pbl::PDF* pdf = pbl::msgToPDF(it_prop->pdf);
00058 
00059                         if (pdf) {
00060 
00062 
00063                                 // Object class
00064                                 if (it_prop->attribute == "class_label") {
00065                                         string label = "";
00066                                         pdf->getExpectedValue(label);
00067 
00068                                         // Get probability
00069                                         const pbl::PMF* pmf = pbl::PDFtoPMF(*pdf);
00070                                         double p = pmf->getProbability(label);
00071                                         ROS_INFO(" - class %s with probability %f", label.c_str(), p);
00072                                 }
00073                                 // Position
00074                                 else if (it_prop->attribute == "position") {
00075                                         const pbl::Mixture* pos_pdf = pbl::PDFtoMixture(*pdf);
00076                                         if (pos_pdf) {
00077                                                 const pbl::Gaussian* pos_gauss = getBestGaussianFromMixture(*pos_pdf);
00078                                                 if (pos_gauss) {
00079                                                         const pbl::Vector& pos = pos_gauss->getMean();
00080                                                         ROS_INFO(" - position: (%f,%f,%f)", pos(0), pos(1), pos(2));
00081                                                         ROS_INFO(" - diagonal position cov: (%f,%f,%f)",
00082                                                                         pos_gauss->getCovariance()(0, 0), pos_gauss->getCovariance()(1, 1), pos_gauss->getCovariance()(2, 2));
00083 
00084                                                 }
00085                                         } else {
00086                                                 ROS_INFO(" - position: object position unknown (uniform distribution)");
00087                                         }
00088                                 }
00089                                 // Orientation
00090                                 else if (it_prop->attribute == "orientation") {
00091                                         const pbl::Mixture* orientation_pdf = pbl::PDFtoMixture(*pdf);
00092                                         if (orientation_pdf) {
00093                                                 const pbl::Gaussian* orientation_gauss = getBestGaussianFromMixture(*orientation_pdf);
00094                                                 if (orientation_gauss) {
00095                                                         const pbl::Vector& ori = orientation_gauss->getMean();
00096                                                         ROS_INFO(" - orientation: (%f,%f,%f,%f)", ori(0), ori(1), ori(2), ori(3));
00097                                                 }
00098                                         } else {
00099                                                 ROS_INFO(" - orientation: object orientation unknown (uniform distribution)");
00100                                         }
00101                                 }
00102                                 // Color
00103                                 else if (it_prop->attribute == "color") {
00104                                         string color = "";
00105                                         pdf->getExpectedValue(color);
00106                                         ROS_INFO(" - color: %s", color.c_str());
00107                                 }
00108 
00109                                 delete pdf;
00110                         }
00111 
00112                 }
00113 
00114         }
00115 
00116 }
00117 
00118 int main(int argc, char **argv) {
00119         ros::init(argc, argv, "get_world_state");
00120         ros::NodeHandle nh;
00121 
00122         // Subscribe to the world model
00123         ros::Subscriber sub_obj = nh.subscribe("/world_state", 1000, &worldStateCallback);
00124 
00125         ros::spin();
00126         return 0;
00127 }


wire_tutorials
Author(s): Jos Elfring
autogenerated on Tue Jan 7 2014 11:43:45