dummy_evidence_publisher.cpp
Go to the documentation of this file.
00001 /*
00002  * dummy_evidence_publisher.cpp
00003  *
00004  *  Created on: Mar 2, 2012
00005  *      Author: sdries
00006  */
00007 
00008 /*
00009  * msg_converter.cpp
00010  *
00011  *  Created on: Dec 14, 2011
00012  *      Author: Jos Elfring, Sjoerd van den Dries
00013  */
00014 
00015 #include <ros/ros.h>
00016 
00017 #include "wire_msgs/WorldEvidence.h"
00018 #include "wire_msgs/ObjectEvidence.h"
00019 
00020 #include "problib/PDF.h"
00021 
00022 #include "problib/conversions.h"
00023 
00024 #include <map>
00025 
00026 using namespace std;
00027 
00028 ros::Publisher EVIDENCE_PUB;                             // publisher
00029 
00030 ros::Time time_last_cycle_;
00031 
00032 double x_person = 0;
00033 double x_vel_person = 0.5;
00034 
00035 void addEvidence(wire_msgs::WorldEvidence& world_evidence, double x, double y, double z, const string& class_label) {
00036         wire_msgs::ObjectEvidence obj_evidence;
00037 
00038     obj_evidence.certainty = 1.0;
00039 
00040         // setting position property
00041     wire_msgs::Property posProp;
00042         posProp.attribute = "position";
00043         pbl::PDFtoMsg(pbl::Gaussian(pbl::Vector3(x, y, z), pbl::Matrix3(0.001, 0.001, 0.001)), posProp.pdf);
00044         obj_evidence.properties.push_back(posProp);
00045 
00046         // setting class label property
00047     wire_msgs::Property classProp;
00048         classProp.attribute = "class_label";
00049         pbl::PMF classPMF;
00050         classPMF.setProbability(class_label, 0.2);
00051         pbl::PDFtoMsg(classPMF, classProp.pdf);
00052         obj_evidence.properties.push_back(classProp);
00053 
00054         // Add to array
00055         world_evidence.object_evidence.push_back(obj_evidence);
00056 }
00057 
00058 
00059 void generateEvidence() {
00060         ros::Time time_now = ros::Time::now();
00061         double dt = (time_now - time_last_cycle_).toSec();
00062 
00063         x_person += dt * x_vel_person;
00064 
00065         wire_msgs::WorldEvidence world_evidence;
00066         world_evidence.header.stamp = ros::Time::now();
00067         world_evidence.header.frame_id = "/map";
00068 
00069         addEvidence(world_evidence, 5, 4, 3, "body");
00070         addEvidence(world_evidence, 5, 4, 4, "face");
00071         addEvidence(world_evidence, 1, 2, 3, "cup");
00072         addEvidence(world_evidence, 3, 2.5, 1, "cup");
00073         addEvidence(world_evidence, 2, 2, 3, "test_class");
00074         //addEvidence(world_evidence, x_person, 2, 1+x_person, "cup", "");
00075 
00076         // Publish results
00077         EVIDENCE_PUB.publish(world_evidence);
00078 
00079         time_last_cycle_ = time_now;
00080 }
00081 
00085 int main(int argc, char **argv) {
00086 
00087         // Initialize ros and create node handle
00088         ros::init(argc,argv,"dummy_evidence_publisher");
00089         ros::NodeHandle nh;
00090 
00091         // Subscriber/publisher
00092         EVIDENCE_PUB = nh.advertise<wire_msgs::WorldEvidence>("/world_evidence", 100);
00093 
00094         ros::Rate r(20);
00095 
00096         time_last_cycle_ = ros::Time::now();
00097         while (ros::ok()) {
00098                 generateEvidence();
00099                 r.sleep();
00100         }
00101 
00102         return 0;
00103 }


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