Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
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;
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
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
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
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
00075
00076
00077 EVIDENCE_PUB.publish(world_evidence);
00078
00079 time_last_cycle_ = time_now;
00080 }
00081
00085 int main(int argc, char **argv) {
00086
00087
00088 ros::init(argc,argv,"dummy_evidence_publisher");
00089 ros::NodeHandle nh;
00090
00091
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 }