Go to the documentation of this file.00001
00002
00003
00004
00005
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
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
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
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
00047 for (vector<wire_msgs::ObjectState>::const_iterator it_obj = msg->objects.begin(); it_obj != msg->objects.end(); ++it_obj) {
00048
00049
00050 wire_msgs::ObjectState obj = *it_obj;
00051 ROS_INFO(" Object:");
00052
00053
00054 for (vector<wire_msgs::Property>::const_iterator it_prop = obj.properties.begin(); it_prop != obj.properties.end(); ++it_prop) {
00055
00056
00057 pbl::PDF* pdf = pbl::msgToPDF(it_prop->pdf);
00058
00059 if (pdf) {
00060
00062
00063
00064 if (it_prop->attribute == "class_label") {
00065 string label = "";
00066 pdf->getExpectedValue(label);
00067
00068
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
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
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
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
00123 ros::Subscriber sub_obj = nh.subscribe("/world_state", 1000, &worldStateCallback);
00124
00125 ros::spin();
00126 return 0;
00127 }