Estimate is now sent in one unique custom message with more informati…

…on for evaluation of performance

include/pfuclt_omni_dataset/pfuclt_particles.h

@@ -15,6 +15,7 @@
 
 #include <read_omni_dataset/RobotState.h>
 #include <read_omni_dataset/LRMGTData.h>
+#include <read_omni_dataset/Estimate.h>
 #include <pfuclt_omni_dataset/particle.h>
 #include <pfuclt_omni_dataset/particles.h>
 
@@ -306,12 +307,13 @@ class ParticleFilter
   std::vector<TargetObservation> bufTargetObservations_;
   TimeEval targetIterationTime_, odometryTime_;
   ros::WallTime iterationEvalTime_;
-  ros::WallDuration maxDeltaIteration_;
+  ros::WallDuration deltaIteration_, maxDeltaIteration_;
   ros::WallDuration durationSum;
   uint16_t numberIterations;
   struct State state_;
   ros::Publisher velPublisher_;
   ros::Time latestObservationTime_, savedLatestObservationTime_;
+  bool converged_;
 
   /**
    * @brief copyParticle - copies a whole particle from one particle set to
@@ -647,7 +649,7 @@ class PFPublisher : public ParticleFilter
 
 private:
   ros::Subscriber GT_sub_;
-  ros::Publisher robotStatePublisher_, targetStatePublisher_,
+  ros::Publisher estimatePublisher_,
       particlePublisher_, syncedGTPublisher_, targetEstimatePublisher_,
       targetGTPublisher_, targetParticlePublisher_;
   std::vector<ros::Publisher> particleStdPublishers_;
@@ -657,8 +659,7 @@ class PFPublisher : public ParticleFilter
 
   read_omni_dataset::LRMGTData msg_GT_;
   pfuclt_omni_dataset::particles msg_particles_;
-  read_omni_dataset::RobotState msg_state_;
-  read_omni_dataset::BallData msg_target_;
+  read_omni_dataset::Estimate msg_estimate_;
 
   std::vector<tf2_ros::TransformBroadcaster> robotBroadcasters;
 
@@ -667,6 +668,7 @@ class PFPublisher : public ParticleFilter
   void publishParticles();
   void publishRobotStates();
   void publishTargetState();
+  void publishEstimate();
   void publishGTData();
   void publishTargetObservations();
 

src/pfuclt_particles.cpp

@@ -518,9 +518,12 @@ void ParticleFilter::resample()
     // Print iteration and state information
     *iteration_oss << "FAIL! -> ";
 
+    converged_ = false;
     return;
   }
 
+  converged_ = true;
+
   // All resamplers use normalized weights
   for (uint p = 0; p < nParticles_; ++p)
     particles_[O_WEIGHT][p] = particles_[O_WEIGHT][p] / weightSum;
@@ -829,16 +832,15 @@ void ParticleFilter::predict(const uint robotNumber, const Odometry odom,
     ROS_INFO("(WALL TIME) Odometry analyzed with = %fms",
              1e3 * odometryTime_.diff);
 
-    ros::WallDuration deltaIteration =
-        ros::WallTime::now() - iterationEvalTime_;
-    if (deltaIteration > maxDeltaIteration_)
-      maxDeltaIteration_ = deltaIteration;
+    deltaIteration_ = ros::WallTime::now() - iterationEvalTime_;
+    if (deltaIteration_ > maxDeltaIteration_)
+      maxDeltaIteration_ = deltaIteration_;
 
-    durationSum += deltaIteration;
+    durationSum += deltaIteration_;
     numberIterations++;
 
     ROS_INFO_STREAM("(WALL TIME) Iteration time: "
-                    << 1e-6 * deltaIteration.toNSec() << "ms ::: Worst case: "
+                    << 1e-6 * deltaIteration_.toNSec() << "ms ::: Worst case: "
                     << 1e-6 * maxDeltaIteration_.toNSec() << "ms ::: Average: "
                     << 1e-6 * (durationSum.toNSec() / numberIterations) << "s");
 #endif
@@ -882,10 +884,8 @@ PFPublisher::PFPublisher(struct ParticleFilter::PFinitData& data,
       "/gtData_synced_pfuclt_estimate", 1000);
 
   // Other publishers
-  robotStatePublisher_ =
-      nh_.advertise<read_omni_dataset::RobotState>("/pfuclt_omni_poses", 1000);
-  targetStatePublisher_ = nh_.advertise<read_omni_dataset::BallData>(
-      "/pfuclt_orangeBallState", 1000);
+  estimatePublisher_ =
+      nh_.advertise<read_omni_dataset::Estimate>("/pfuclt_estimate", 100);
   particlePublisher_ =
       nh_.advertise<pfuclt_omni_dataset::particles>("/pfuclt_particles", 10);
 
@@ -916,18 +916,17 @@ PFPublisher::PFPublisher(struct ParticleFilter::PFinitData& data,
     robotEstimatePublishers_[r] = nh_.advertise<geometry_msgs::PoseStamped>(
         "/" + robotName.str() + "/estimatedPose", 1000);
 
+    // build estimate msg
+    msg_estimate_.robotEstimates.push_back(geometry_msgs::Pose());
+    msg_estimate_.targetVisibility.push_back(false);
+
 // ground truth publisher, in the simulation package we have PoseStamped
 #ifndef USE_NEWER_READ_OMNI_PACKAGE
     robotGTPublishers_[r] = nh_.advertise<geometry_msgs::PointStamped>(
         "/" + robotName.str() + "/gtPose", 1000);
 #else
     robotGTPublishers_[r] = nh_.advertise<geometry_msgs::PoseStamped>(
         "/" + robotName.str() + "/gtPose", 1000);
-
-    // add a new pose to the pose message for each robot, with base frame
-    geometry_msgs::PoseStamped ps;
-    ps.header.frame_id = "world";
-    msg_state_.robotPose.push_back(ps);
 #endif
   }
 
@@ -1007,14 +1006,11 @@ void PFPublisher::publishRobotStates()
     std::ostringstream robotName;
     robotName << "omni" << r + 1;
 
-    msg_state_.header.stamp = savedLatestObservationTime_;
+    msg_estimate_.header.stamp = savedLatestObservationTime_;
 
     ParticleFilter::State::robotState_s& pfState = state_.robots[r];
-#ifdef USE_NEWER_READ_OMNI_PACKAGE
-    geometry_msgs::Pose& rosState = msg_state_.robotPose[r].pose;
-#else
-    geometry_msgs::Pose& rosState = msg_state_.robotPose[r].pose.pose;
-#endif
+    geometry_msgs::Pose& rosState = msg_estimate_.robotEstimates[r];
+
     // Create from Euler angles
     tf2::Quaternion tf2q(tf2::Vector3(0, 0, 1), pfState.pose[O_THETA]);
     tf2::Transform tf2t(tf2q, tf2::Vector3(pfState.pose[O_X], pfState.pose[O_Y],
@@ -1041,22 +1037,22 @@ void PFPublisher::publishRobotStates()
     robotEstimatePublishers_[r].publish(estPose);
 #endif
   }
-
-  robotStatePublisher_.publish(msg_state_);
 }
 
 void PFPublisher::publishTargetState()
 {
-  msg_target_.header.stamp = savedLatestObservationTime_;
-  msg_target_.header.frame_id = "world";
+  msg_estimate_.targetEstimate.header.frame_id = "world";
 
   // Our custom message type
-  msg_target_.x = state_.target.pos[O_TX];
-  msg_target_.y = state_.target.pos[O_TY];
-  msg_target_.z = state_.target.pos[O_TZ];
-  msg_target_.found = state_.target.seen;
+  msg_estimate_.targetEstimate.x = state_.target.pos[O_TX];
+  msg_estimate_.targetEstimate.y = state_.target.pos[O_TY];
+  msg_estimate_.targetEstimate.z = state_.target.pos[O_TZ];
+  msg_estimate_.targetEstimate.found = state_.target.seen;
 
-  targetStatePublisher_.publish(msg_target_);
+  for (uint r = 0; r < nRobots_; ++r)
+  {
+    msg_estimate_.targetVisibility[r] = bufTargetObservations_[r].found;
+  }
 
 #ifdef BROADCAST_TF_AND_POSES
   // Publish as a standard pose msg using the previous TF
@@ -1071,6 +1067,16 @@ void PFPublisher::publishTargetState()
 #endif
 }
 
+void PFPublisher::publishEstimate()
+{
+  // msg_estimate_ has been built in other methods (publishRobotStates and
+  // publishTargetState)
+  msg_estimate_.computationTime = deltaIteration_.toNSec() * 1e-9;
+  msg_estimate_.converged = converged_;
+
+  estimatePublisher_.publish(msg_estimate_);
+}
+
 void PFPublisher::publishTargetObservations()
 {
   static std::vector<bool> previouslyPublished(nRobots_, false);
@@ -1211,10 +1217,6 @@ void PFPublisher::nextIteration()
   // Call the base class method
   ParticleFilter::nextIteration();
 
-  // Time stamps using ros time
-  msg_state_.header.stamp = msg_GT_.header.stamp = msg_target_.header.stamp =
-      ros::Time::now();
-
   // Publish the particles first
   publishParticles();
 
@@ -1224,6 +1226,9 @@ void PFPublisher::nextIteration()
   // Publish target state
   publishTargetState();
 
+  // Publish estimate
+  publishEstimate();
+
   // Publish robot-to-target lines
   publishTargetObservations();