Added parameter transforms/require_all_reachable.

README.md

@@ -157,7 +157,10 @@ The basic workings of this filter are done via the [`filters::FilterBase` API](h
     how old scans you still want to process.
 - `transforms/timeout/unreachable` (`float`, default `0.2 s`)
 
-    How long to wait while getting unreachable TF.     
+    How long to wait while getting unreachable TF.
+- `transforms/require_all_reachable` (`bool`, default `false`)
+
+   If true, the filter won't publish anything until all transforms are reachable.
 - `bounding_sphere/compute` (`bool`, default `false`)
 
     Whether to compute and publish bounding sphere.

include/robot_body_filter/RobotBodyFilter.h

@@ -248,6 +248,9 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase<T> {
   //! Timeout for unreachable transforms.
   ros::Duration unreachableTransformTimeout;
 
+  //! Whether to process data when there are some unreachable frames.
+  bool requireAllFramesReachable;
+
   //! A mutex that has to be locked in order to work with shapesToLinks or tfBuffer.
   std::shared_ptr<std::mutex> modelMutex;
 

include/robot_body_filter/TfFramesWatchdog.h

@@ -72,6 +72,12 @@ class TFFramesWatchdog {
    */
   bool isReachable(const std::string& frame) const;
 
+  /**
+   * \brief Return whether all monitored frames are reachable.
+   * \return Whether all monitored frames are reachable.
+   */
+  bool areAllFramesReachable() const;
+
   /**
    * \brief Looks for a transform if it is marked reachable. Returns immediately
    *        for transforms marked unreachable.

src/RobotBodyFilter.cpp

@@ -76,6 +76,7 @@ bool RobotBodyFilter<T>::configure() {
   const bool doShadowTest = this->getParamVerbose("filter/do_shadow_test", true);
   this->reachableTransformTimeout = this->getParamVerbose("transforms/timeout/reachable", ros::Duration(0.1), "s");
   this->unreachableTransformTimeout = this->getParamVerbose("transforms/timeout/unreachable", ros::Duration(0.2), "s");
+  this->requireAllFramesReachable = this->getParamVerbose("transforms/require_all_reachable", false);
   this->publishNoBoundingSpherePointcloud = this->getParamVerbose("bounding_sphere/publish_cut_out_pointcloud", false);
   this->publishNoBoundingBoxPointcloud = this->getParamVerbose("bounding_box/publish_cut_out_pointcloud", false);
   this->publishNoOrientedBoundingBoxPointcloud = this->getParamVerbose("oriented_bounding_box/publish_cut_out_pointcloud", false);
@@ -434,6 +435,18 @@ bool RobotBodyFilterLaserScan::update(const LaserScan &inputScan, LaserScan &fil
     return false;
   }
 
+  if (!this->tfFramesWatchdog->isReachable(this->sensorFrame))
+  {
+    ROS_DEBUG("RobotBodyFilter: Throwing away scan since sensor frame is unreachable.");
+    return false;
+  }
+
+  if (this->requireAllFramesReachable && !this->tfFramesWatchdog->areAllFramesReachable())
+  {
+    ROS_DEBUG("RobotBodyFilter: Throwing away scan since not all frames are reachable.");
+    return false;
+  }
+
   const clock_t stopwatchOverall = clock();
 
   // tf2 doesn't like frames starting with slash
@@ -593,6 +606,18 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::PointCloud2 &inputClo
     return false;
   }
 
+  if (!this->tfFramesWatchdog->isReachable(this->sensorFrame))
+  {
+    ROS_DEBUG("RobotBodyFilter: Throwing away scan since sensor frame is unreachable.");
+    return false;
+  }
+
+  if (this->requireAllFramesReachable && !this->tfFramesWatchdog->areAllFramesReachable())
+  {
+    ROS_DEBUG("RobotBodyFilter: Throwing away scan since not all frames are reachable.");
+    return false;
+  }
+
   bool hasStampsField = false;
   bool hasVpXField = false, hasVpYField = false, hasVpZField = false;
   for (const auto& field : inputCloud.fields) {

src/TFFramesWatchdog.cpp

@@ -168,4 +168,10 @@ void TFFramesWatchdog::markUnreachable(const std::string &frame)
   this->reachableFrames.erase(frame);
 }
 
+bool TFFramesWatchdog::areAllFramesReachable() const
+{
+  std::lock_guard<std::mutex> guard(this->framesMutex);
+  return this->reachableFrames.size() == this->monitoredFrames.size();
+}
+
 }