/*
* World state callback. Creates a marker for each detection.
*/
void worldStateCallback(const wire_msgs::WorldState::ConstPtr& msg) {
// Iterate over world state objects
for (vector<wire_msgs::ObjectState>::const_iterator it = msg->objects.begin(); it != msg->objects.end(); ++it) {
// Get current object
const wire_msgs::ObjectState& obj = *it;
// Create marker
visualization_msgs::MarkerArray markers_msg;
world_state_visualizer_.createMarkers(msg->header, obj.ID, obj.properties, markers_msg.markers, marker_frame_);
// Publish marker
world_state_marker_pub_.publish(markers_msg);
}
}
/*
* World evidence callback. Creates a marker for each of the world model objects
*/
void worldEvidenceCallback(const wire_msgs::WorldEvidence::ConstPtr& msg) {
// ID for marker administration
long ID = 0;
// Iterate over all elements of evidence
for (vector<wire_msgs::ObjectEvidence>::const_iterator it = msg->object_evidence.begin(); it != msg->object_evidence.end(); ++it) {
// Get current evidence
const wire_msgs::ObjectEvidence& obj_ev = *it;
// Create marker
visualization_msgs::MarkerArray markers_msg;
world_evidence_visualizer_.createMarkers(msg->header, ID++, obj_ev.properties, markers_msg.markers, marker_frame_);
// Publish marker
world_evidence_marker_pub_.publish(markers_msg);
}
}
