/// 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)); } }