/// Display stuff
void publishStuff(bool all = true){
if (pubMarker.getNumSubscribers()>0){
// Publish Landmarks
pubMarker.publish(odometry.getMapMarkers());
pubMarker.publish(odometry.getMapObservations());
// Publish Track (ie all path and poses estimated)
pubMarker.publish(odometry.getTrackLineMarker());
}
if (pubTrack.getNumSubscribers()>0){
pubTrack.publish(odometry.getTrackPoseMarker());
}
if (all){
// Publish TF for each KF
ROS_INFO("NOD = Publishing TF for each KF");
const Frame::Ptrs& kfs = odometry.getKeyFrames();
//const Frame::Ptr kf = kfs[0];
for(uint i=0; i<kfs.size(); ++i){
pubTF.sendTransform(kfs[i]->getStampedTransform());
}
}
// Publish TF for current frame
const Frame::Ptr f = odometry.getLastFrame();
if (f->poseEstimated()){
pubTF.sendTransform(f->getStampedTransform());
pubTF.sendTransform(f->getStampedTransform(true));
}
}
