Fix time computation for velocity estimator; removed synced GT publis…

…hing - evaluate should subscribe to original GT; latest observation time is now given by landmark observations to prevent cases where target is not seen

include/pfuclt_omni_dataset/pfuclt_particles.h

@@ -549,9 +549,11 @@ class ParticleFilter
    */
   inline void saveLandmarkObservation(const uint robotNumber,
                                       const uint landmarkNumber,
-                                      const LandmarkObservation obs)
+                                      const LandmarkObservation obs,
+                                      ros::Time stamp)
   {
     bufLandmarkObservations_[robotNumber][landmarkNumber] = obs;
+    latestObservationTime_ = stamp;
   }
 
   /**
@@ -585,7 +587,6 @@ class ParticleFilter
                                     ros::Time stamp)
   {
     bufTargetObservations_[robotNumber] = obs;
-    latestObservationTime_ = stamp;
   }
 
   /**
@@ -649,9 +650,8 @@ class PFPublisher : public ParticleFilter
 
 private:
   ros::Subscriber GT_sub_;
-  ros::Publisher estimatePublisher_,
-      particlePublisher_, syncedGTPublisher_, targetEstimatePublisher_,
-      targetGTPublisher_, targetParticlePublisher_;
+  ros::Publisher estimatePublisher_, particlePublisher_,
+      targetEstimatePublisher_, targetGTPublisher_, targetParticlePublisher_;
   std::vector<ros::Publisher> particleStdPublishers_;
   std::vector<ros::Publisher> robotGTPublishers_;
   std::vector<ros::Publisher> robotEstimatePublishers_;

src/pfuclt_omni_dataset.cpp

@@ -355,7 +355,8 @@ void Robot::landmarkDataCallback(
                   pow(cos(obs.phi), 2) *
                       (pow(obs.d, 2) * obs.covPP + obs.covDD * obs.covPP);
 
-      pf_->saveLandmarkObservation(robotNumber_, i, obs);
+      pf_->saveLandmarkObservation(robotNumber_, i, obs,
+                                   landmarkData->header.stamp);
     }
   }
 

src/pfuclt_particles.cpp

@@ -172,6 +172,9 @@ void ParticleFilter::fuseRobots()
 {
   *iteration_oss << "fuseRobots() -> ";
 
+  // Save the latest observation time to be used when publishing
+  savedLatestObservationTime_ = latestObservationTime_;
+
   // Keeps track of number of landmarks seen for each robot
   std::vector<uint> landmarksSeen(nRobots_, 0);
 
@@ -325,8 +328,6 @@ void ParticleFilter::fuseTarget()
     return;
   }
   // If program is here, at least one robot saw the ball
-  // Save the latest observation time to be used when publishing
-  savedLatestObservationTime_ = latestObservationTime_;
 
   // Instance variables to be worked in the loops
   pdata_t maxTargetSubParticleWeight, totalWeight;
@@ -874,15 +875,11 @@ PFPublisher::PFPublisher(struct ParticleFilter::PFinitData& data,
   // Prepare particle message
   resize_particles(nParticles_);
 
-  // Subscribe and advertise the republishing of GT data, time synced with the
-  // state publisher
+  // Subscribe to GT data
   GT_sub_ = nh_.subscribe<read_omni_dataset::LRMGTData>(
       "/gtData_4robotExp", 10,
       boost::bind(&PFPublisher::gtDataCallback, this, _1));
 
-  syncedGTPublisher_ = nh_.advertise<read_omni_dataset::LRMGTData>(
-      "/gtData_synced_pfuclt_estimate", 1000);
-
   // Other publishers
   estimatePublisher_ =
       nh_.advertise<read_omni_dataset::Estimate>("/pfuclt_estimate", 100);
@@ -1152,9 +1149,6 @@ void PFPublisher::publishTargetObservations()
 
 void PFPublisher::publishGTData()
 {
-  // Publish custom format
-  syncedGTPublisher_.publish(msg_GT_);
-
   geometry_msgs::PointStamped gtPoint;
   gtPoint.header.stamp = savedLatestObservationTime_;
   gtPoint.header.frame_id = "world";
@@ -1253,7 +1247,7 @@ void ParticleFilter::State::targetVelocityEstimator_s::insertZeros()
     const double lastPos = posVecs[velType].back();
 
     // Insert the same position at a new time
-    timeVecs[velType].push_back(ros::Time::now().toNSec() * 1e9 - timeInit);
+    timeVecs[velType].push_back(ros::Time::now().toNSec() * 1e-9 - timeInit);
     posVecs[velType].push_back(lastPos);
   }
 }