add opt. sensor_model expected_rate and per_second

octomap_server/include/octomap_server/CallbackRate.h

@@ -0,0 +1,86 @@
+#ifndef OCTOMAP_SERVER_CALLBACK_RATE_H
+#define OCTOMAP_SERVER_CALLBACK_RATE_H
+
+#include <cmath>
+
+#include <octomap/octomap_utils.h>
+#include <ros/time.h>
+
+namespace octomap_server {
+
+class CallbackRate
+{
+public:
+  CallbackRate()
+  {
+    reset();
+  }
+
+  void reset()
+  {
+    n_ = 0;
+  }
+
+  void update()
+  {
+    ros::Time now = ros::Time::now();
+    if (n_ == 0)
+    {
+      last_time_ = now;
+    }
+    else
+    {
+      double delta_t = (now - last_time_).toSec();
+      if (n_ == 1)
+      {
+        delta_t_ema_ = delta_t;
+      }
+      else
+      {
+        if (delta_t > delta_t_invalid_factor_ * delta_t_ema_)
+        {
+          reset();
+          last_time_ = now;
+        }
+        else
+        {
+          delta_t_ema_ = delta_t * ema_alpha_ + delta_t_ema_ * (1.0 - ema_alpha_);
+        }
+      }
+    }
+    ++n_;
+  }
+
+  bool valid()
+  {
+    return n_ >= min_n_ && delta_t_ema_ > 0.0;
+  }
+
+  double getCurrentRate()
+  {
+    return 1.0 / delta_t_ema_;
+  }
+
+  bool rateIsApproximately(double expected_rate)
+  {
+    return valid() && std::abs(getCurrentRate() - expected_rate) < expected_rate * approximate_epsilon_;
+  }
+
+protected:
+  ros::Time last_time_;
+  size_t n_;
+  // Current delta_t exponential moving average
+  double delta_t_ema_;
+  // Alpha to use when calculating the exponential moving average
+  double ema_alpha_ = 0.05;
+  // Minimum number of samples before considering moving average valid.
+  double min_n_ = 20;
+  // If a delta_t of more than the factor times the current delta_t is encountered the EMA is reset.
+  double delta_t_invalid_factor_ = 5.0;
+  // What factor of the expected rate will be considered approximately equal to the tracked rate.
+  double approximate_epsilon_ = .2;
+};
+
+}  // namespace octomap_server
+
+#endif  // OCTOMAP_SERVER_CALLBACK_RATE_H

octomap_server/include/octomap_server/OctomapServer.h

@@ -76,6 +76,7 @@
 #endif
 
 #include <octomap_server/types.h>
+#include <octomap_server/CallbackRate.h>
 #include <octomap_server/PointCloudSynchronizer.h>
 #include <octomap_server/OcTreeStampedWithExpiry.h>
 #include <octomap_server/SensorUpdateKeyMap.h>
@@ -154,6 +155,7 @@ class OctomapServer {
 
     int callback_id = m_callbackCounts.size();
     m_callbackCounts.push_back(0);
+    m_callbackRates.push_back(CallbackRate());
     std::vector<std::string> callback_topics;
     callback_topics.resize(4);
     callback_topics[0] = ground_topic;
@@ -334,6 +336,7 @@ class OctomapServer {
   bool m_latchedTopics;
   int m_callbackSkipCount;
   std::vector<unsigned int> m_callbackCounts;
+  std::vector<CallbackRate> m_callbackRates;
   std::vector<std::vector<std::string>> m_callbackTopics;
   bool m_trackFreeSpace;
   bool m_publishFreeSpace;
@@ -347,6 +350,7 @@ class OctomapServer {
   ros::Time m_publish3DMapUpdateLastTime;
   double m_publish2DPeriod;
   ros::Time m_publish2DLastTime;
+  double m_expectedSensorRate;
 
   double m_res;
   unsigned m_treeDepth;

octomap_server/src/OctomapServer.cpp

@@ -64,6 +64,7 @@ OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeH
   m_publish3DMapLastTime(ros::Time::now()),
   m_publish2DLastTime(ros::Time::now()),
   m_publish3DMapUpdateLastTime(ros::Time::now()),
+  m_expectedSensorRate(0.0),
   m_newFullSub(false),
   m_newBinarySub(false),
   m_res(0.05),
@@ -148,6 +149,19 @@ OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeH
   m_nh_private.param("sensor_model/miss", probMiss, 0.4);
   m_nh_private.param("sensor_model/min", thresMin, 0.12);
   m_nh_private.param("sensor_model/max", thresMax, 0.97);
+  m_nh_private.param("sensor_model/expected_rate", m_expectedSensorRate, 0.0);
+  if (m_expectedSensorRate > 0.0)
+  {
+    double hit_per_second;
+    double miss_per_second;
+    // Make the defaults based on the original sensor model parameters in case
+    // the new per second parameters are not set.
+    m_nh_private.param("sensor_model/hit_per_second", hit_per_second, octomap::probability(octomap::logodds(probHit) * m_expectedSensorRate));
+    m_nh_private.param("sensor_model/miss_per_second", miss_per_second, octomap::probability(octomap::logodds(probMiss) * m_expectedSensorRate));
+    // Convert seconds to hits using the expected sensor rate
+    probHit = octomap::probability(octomap::logodds(hit_per_second) / m_expectedSensorRate);
+    probMiss = octomap::probability(octomap::logodds(miss_per_second) / m_expectedSensorRate);
+  }
   m_nh_private.param("compress_map", m_compressMap, m_compressMap);
   m_nh_private.param("compress_period", m_compressPeriod, m_compressPeriod);
   m_nh_private.param("incremental_2D_projection", m_incrementalUpdate, m_incrementalUpdate);
@@ -613,6 +627,18 @@ void OctomapServer::insertSegmentedCloudCallback(
     const std::string& sensor_origin_frame_id,
     unsigned int callback_id)
 {
+  if (m_expectedSensorRate > 0.0)
+  {
+    // Estimate the callback rate for reporting if the rate does not match m_expectedSensorRate
+    m_callbackRates[callback_id].update();
+    if (m_callbackRates[callback_id].valid() &&
+        !m_callbackRates[callback_id].rateIsApproximately(m_expectedSensorRate))
+    {
+      ROS_WARN_STREAM_THROTTLE(10.0, "Sensor for frame " << sensor_origin_frame_id <<
+          " not updating at configured rate. Expected rate: " << m_expectedSensorRate <<
+          ", actual update rate: " << m_callbackRates[callback_id].getCurrentRate());
+    }
+  }
   if (m_callbackCounts[callback_id] < m_callbackSkipCount)
   {
     // skip the callback until we are at the skip count