Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Feeding detections to the world model

Description: This tutorial shows the input message type of the wire stack. It in addition explains how any detector can be adapted to publish its results to the world model in the appropriate format.

Tutorial Level: INTERMEDIATE

Next Tutorial: Using the estimated world state

Goal

This tutorial will show how the world model can be fed with evidence.

Approach

Wire expects the evidence to be of message type wire_msgs/WorldEvidence. Converting detections to the appropriate format can be done using problib in at most a few lines of code. A detailed example available in the src folder of the wire_tutorials package shows how this is done.

   1 #include <ros/ros.h>
   2 
   3 #include "wire_msgs/WorldEvidence.h"
   4 #include "wire_msgs/ObjectEvidence.h"
   5 
   6 #include "problib/conversions.h"
   7 
   8 // Publisher used to send evidence to world model
   9 ros::Publisher world_evidence_publisher_;
  10 
  11 
  12 void addEvidence(wire_msgs::WorldEvidence& world_evidence, double x, double y, double z, const std::string& class_label, const std::string& color) {
  13         wire_msgs::ObjectEvidence obj_evidence;
  14 
  15         // Set the continuous position property
  16         wire_msgs::Property posProp;
  17         posProp.attribute = "position";
  18 
  19         // Set position (x,y,z), set the covariance matrix as 0.005*identity_matrix
  20         pbl::PDFtoMsg(pbl::Gaussian(pbl::Vector3(x, y, z), pbl::Matrix3(0.0005, 0.0005, 0.0005)), posProp.pdf);
  21         obj_evidence.properties.push_back(posProp);
  22 
  23         // Set the discrete class label property
  24         wire_msgs::Property classProp;
  25         classProp.attribute = "class_label";
  26         pbl::PMF classPMF;
  27 
  28         // Probability of the class label is 0.7
  29         classPMF.setProbability(class_label, 0.7);
  30         pbl::PDFtoMsg(classPMF, classProp.pdf);
  31         obj_evidence.properties.push_back(classProp);
  32 
  33         // Set the discrete color property with a probability of 0.9
  34         wire_msgs::Property colorProp;
  35         colorProp.attribute = "color";
  36         pbl::PMF colorPMF;
  37 
  38         // The probability of the detected color is 0.9
  39         colorPMF.setProbability(color, 0.9);
  40         pbl::PDFtoMsg(colorPMF, colorProp.pdf);
  41         obj_evidence.properties.push_back(colorProp);
  42 
  43         // Add all properties to the array
  44         world_evidence.object_evidence.push_back(obj_evidence);
  45 }
  46 
  47 
  48 void generateEvidence() {
  49 
  50         // Create world evidence message
  51         wire_msgs::WorldEvidence world_evidence;
  52 
  53         // Set header
  54         world_evidence.header.stamp = ros::Time::now();
  55         world_evidence.header.frame_id = "/map";
  56 
  57         // Add evidence
  58         addEvidence(world_evidence, 2, 2.2, 3, "mug", "red");
  59 
  60         // Publish results
  61         world_evidence_publisher_.publish(world_evidence);
  62         ROS_INFO("Published world evidence with size %d", world_evidence.object_evidence.size());
  63 
  64 }
  65 
  66 /**
  67  * Main
  68  */
  69 int main(int argc, char **argv) {
  70 
  71         // Initialize ros and create node handle
  72         ros::init(argc,argv,"generate_evidence");
  73         ros::NodeHandle nh;
  74 
  75         // Publisher
  76         world_evidence_publisher_ = nh.advertise<wire_msgs::WorldEvidence>("/world_evidence", 100);
  77 
  78         // Publish with 3 Hz
  79         ros::Rate r(3.0);
  80 
  81         while (ros::ok()) {
  82                 generateEvidence();
  83                 r.sleep();
  84         }
  85 
  86         return 0;
  87 }

The code explained

   1 #include <ros/ros.h>
   2 
   3 #include "wire_msgs/WorldEvidence.h"
   4 #include "wire_msgs/ObjectEvidence.h"
   5 

Here the required headers are included. wire_msgs/WorldEvidence messages are sent to wire_core and contain wire_msgs/ObjectEvidence messages.

   6 #include "problib/conversions.h"
   7 

Problib is used to simplify creating and converting detections.

   8 // Publisher used to send evidence to world model
   9 ros::Publisher world_evidence_publisher_;

The global publisher used for publishing the data to the world model in this example node.

  12 void addEvidence(wire_msgs::WorldEvidence& world_evidence, double x, double y, double z, const std::string& class_label, const std::string& color) {
  13         wire_msgs::ObjectEvidence obj_evidence;

This function adds object evidence to the world evidence message. wire_msgs/WorldEvidence is a collection of wire_msgs/ObjectEvidence messages. wire_msgs/ObjectEvidence is a vector of wire_msgs/Property messages, each property represents one measured object attribute. In its current implementation, different properties are estimated independently of each other.

  15         // Set the continuous position property
  16         wire_msgs::Property posProp;
  17         posProp.attribute = "position";

Here the property 'position' is created.

  19         // Set position (x,y,z), set the covariance matrix as 0.005*identity_matrix
  20         pbl::PDFtoMsg(pbl::Gaussian(pbl::Vector3(x, y, z), pbl::Matrix3(0.0005, 0.0005, 0.0005)), posProp.pdf);

The position property is defined by means of a Gaussian with a mean of (x,y,z) and a covariance matrix that equals 0.0005 times an identity matrix with size three.

  21         obj_evidence.properties.push_back(posProp);

The position property is added to the object evidence message.

  23         // Set the discrete class label property
  24         wire_msgs::Property classProp;
  25         classProp.attribute = "class_label";

Now the property 'class_label' is created.

  26         pbl::PMF classPMF;

This time a probability mass function is used to represent the evidence.

  29         classPMF.setProbability(class_label, 0.7);

The class label given by the user is set with a probability of 0.7. In a real world application this is the result of some object detection algorithm.

  33         // Set the discrete color property with a probability of 0.9
  34         wire_msgs::Property colorProp;
  35         colorProp.attribute = "color";
  36         pbl::PMF colorPMF;
  37 
  38         // The probability of the detected color is 0.9
  39         colorPMF.setProbability(color, 0.9);
  40         pbl::PDFtoMsg(colorPMF, colorProp.pdf);
  41         obj_evidence.properties.push_back(colorProp);

The property color is added in the same manner as the class label, this time with a probability of 0.9.

  44         world_evidence.object_evidence.push_back(obj_evidence);

The collection of properties for this object is added to the world evidence.

  48 void generateEvidence() {

A function that generates evidence.

  50         // Create world evidence message
  51         wire_msgs::WorldEvidence world_evidence;

Create a message of the appropriate type.

  53         // Set header
  54         world_evidence.header.stamp = ros::Time::now();
  55         world_evidence.header.frame_id = "/map";

Set the header. Detections can be made with respect to any frame, the world model will transform evidence if needed.

  57         // Add evidence
  58         addEvidence(world_evidence, 2, 2.2, 3, "mug", "red");

Add a detection of a red mug to the world evidence.

  60         // Publish results
  61         world_evidence_publisher_.publish(world_evidence);
  62         ROS_INFO("Published world evidence with size %d", world_evidence.object_evidence.size());

Publish the resulting world_evidence message to the world model and inform the user.

  75         // Publisher
  76         world_evidence_publisher_ = nh.advertise<wire_msgs::WorldEvidence>("/world_evidence", 100);

World evidence is published over the /world_evidence topic.

Reproducing the result

Make cloned and compiled wire (this includes the example node explained above):

$ git clone https://github.com/tue-robotics/wire.git
$ catkin_make

Start a ros core

$ roscore

Now start the node that was explained above:

$ rosrun wire_tutorials generate_evidence

which creates dummy detections. The following output should appear:

[ INFO] [1359389963.780913354]: Published world evidence with size 1
[ INFO] [1359389964.114297651]: Published world evidence with size 1
[ INFO] [1359389964.447664760]: Published world evidence with size 1

Visualizing the wire world model input or output can be done as explained in this tutorial:

$ roslaunch wire_tutorials rviz_wire_kinetic.launch

Wiki: wire/Tutorials/Feeding detections to the world model (last edited 2019-06-03 11:28:02 by JosElfring)