Allowed ignoring extrapolation TF warnings.

Author: peci1

include/robot_model_renderer/c_api.h

@@ -219,14 +219,16 @@ int robot_model_renderer_sensorMsgsEncodingToOgrePixelFormat(const char* encodin
  * \param[in] model The robot model to render (string version of the URDF file).
  * \param[out] errorMessagesAllocator Allocator for the errors encountered during construction of this class.
  * \param[in] config Configuration of this class.
+ * \param[in] warnExtrapolation Whether to warn on TF extrapolation errors.
  * \return A handle to the constructed class instance. Nullptr if the construction failed.
  * \note Delete this instance by calling robot_model_renderer_deleteRobotModelRenderer() when no longer needed.
  */
 robot_model_renderer_RobotModelRendererHandle robot_model_renderer_createRobotModelRenderer(
   cras_allocator_t logMessagesAllocator,
   const char* model,
   cras_allocator_t errorMessagesAllocator,
-  robot_model_renderer_RobotModelRendererConfig config);
+  robot_model_renderer_RobotModelRendererConfig config,
+  bool warnExtrapolation);
 
 /**
  * \brief Delete the instance created by robot_model_renderer_createRobotModelRenderer().

include/robot_model_renderer/robot/tf_link_updater.hpp

@@ -25,7 +25,7 @@ class TFLinkUpdater : public LinkUpdater, public cras::HasLogger
 {
 public:
   explicit TFLinkUpdater(const cras::LogHelperPtr& log, tf2::BufferCore* tf,
-    const std::string& fixed_frame = {}, const std::string& tf_prefix = {});
+    const std::string& fixed_frame = {}, const std::string& tf_prefix = {}, bool warn_extrapolation = true);
 
   void setFixedFrame(const std::string& fixedFrame);
 
@@ -37,14 +37,15 @@ class TFLinkUpdater : public LinkUpdater, public cras::HasLogger
   tf2::BufferCore* tf_;
   std::string fixed_frame_;
   std::string tf_prefix_;
+  bool warn_extrapolation_;
 };
 
 class TFROSLinkUpdater : public TFLinkUpdater
 {
 public:
   explicit TFROSLinkUpdater(const cras::LogHelperPtr& log, const std::shared_ptr<cras::InterruptibleTFBuffer>& tf,
     const std::string& fixed_frame = {}, const std::string& tf_prefix = {},
-    const ros::Duration& timeout = ros::Duration(0.01));
+    const ros::Duration& timeout = ros::Duration(0.01), bool warn_extrapolation = true);
 
   virtual void setTimeoutStart(const ros::Time& time);
 

src/c_api.cpp

@@ -185,7 +185,7 @@ class LogGuard
 
 robot_model_renderer_RobotModelRendererHandle robot_model_renderer_createRobotModelRenderer(
   cras_allocator_t logMessagesAllocator, const char* model, cras_allocator_t errorMessagesAllocator,
-  robot_model_renderer_RobotModelRendererConfig config)
+  robot_model_renderer_RobotModelRendererConfig config, const bool warnExtrapolation)
 {
   urdf::Model robotModel;
   try
@@ -213,7 +213,8 @@ robot_model_renderer_RobotModelRendererHandle robot_model_renderer_createRobotMo
   {
     LogGuard logGuard(handle);
 
-    handle->linkUpdater = std::make_shared<robot_model_renderer::TFLinkUpdater>(handle->log, &handle->tf);
+    handle->linkUpdater = std::make_shared<robot_model_renderer::TFLinkUpdater>(
+      handle->log, &handle->tf, "", "", warnExtrapolation);
 
     handle->renderer = std::make_unique<robot_model_renderer::RobotModelRenderer>(
       handle->log, robotModel, handle->linkUpdater.get(), errors, config_cpp);

src/robot/tf_link_updater.cpp

@@ -30,15 +30,16 @@ std::string concat(const std::string& prefix, const std::string& frame)
 }
 
 TFLinkUpdater::TFLinkUpdater(const cras::LogHelperPtr& log, tf2::BufferCore* tf,
-  const std::string& fixed_frame, const std::string& tf_prefix)
-  : cras::HasLogger(log), tf_(tf), fixed_frame_(fixed_frame), tf_prefix_(tf_prefix)
+  const std::string& fixed_frame, const std::string& tf_prefix, const bool warn_extrapolation)
+  : cras::HasLogger(log), tf_(tf), fixed_frame_(fixed_frame), tf_prefix_(tf_prefix),
+    warn_extrapolation_(warn_extrapolation)
 {
 }
 
 TFROSLinkUpdater::TFROSLinkUpdater(const cras::LogHelperPtr& log,
   const std::shared_ptr<cras::InterruptibleTFBuffer>& tf, const std::string& fixed_frame, const std::string& tf_prefix,
-  const ros::Duration& timeout)
-  : TFLinkUpdater(log, &tf->getRawBuffer(), fixed_frame, tf_prefix), timeout_(timeout), tf_ros_(tf)
+  const ros::Duration& timeout, const bool warn_extrapolation)
+  : TFLinkUpdater(log, &tf->getRawBuffer(), fixed_frame, tf_prefix, warn_extrapolation), tf_ros_(tf), timeout_(timeout)
 {
 }
 
@@ -71,8 +72,11 @@ cras::expected<void, LinkUpdateError> TFLinkUpdater::getLinkTransforms(
   }
   catch (const tf2::ExtrapolationException& e)
   {
-    CRAS_WARN_STREAM_THROTTLE_NAMED(1.0, "link_updater",
-      "No transform from [" << link_name_prefixed << "] to [" << fixed_frame_ << "]: " << e.what());
+    if (warn_extrapolation_)
+    {
+      CRAS_WARN_STREAM_THROTTLE_NAMED(1.0, "link_updater",
+        "No transform from [" << link_name_prefixed << "] to [" << fixed_frame_ << "]: " << e.what());
+    }
     error.error = e.what();
     error.name = link_name_prefixed;
     error.maySucceedLater = true;

src/robot_model_renderer/cras_bag_tools_plugin.py

@@ -113,7 +113,7 @@ def set_bag(self, bag):
         if model is None:
             raise RuntimeError("RenderCameraMask requires robot URDF either as a file or ROS parameter.")
 
-        self.renderer = RobotModelRenderer(model, self._config, self._encoding)
+        self.renderer = RobotModelRenderer(model, self._config, self._encoding, warnExtrapolation=False)
 
     def filter(self, topic, msg, stamp, header):
         if self.renderer is None:
@@ -124,8 +124,8 @@ def filter(self, topic, msg, stamp, header):
             if stamp - s < self.tf_timeout:
                 to_try.append((m, s, h))
             else:
-                print("Failed transforming robot for mask rendering at time %s: %r" % (m.header.stamp, e),
-                      file=sys.stderr)
+                print("Failed transforming robot for mask rendering at time %i.%09i: %r" % (
+                    m.header.stamp.secs, m.header.stamp.nsecs, e), file=sys.stderr)
         self._failed_msgs = []
 
         if topic.lstrip('/') == 'tf':
@@ -148,7 +148,8 @@ def filter(self, topic, msg, stamp, header):
                 header = fix_connection_header(copy.copy(header), self.mask_topic, Image._type, Image._md5sum, Image)
                 result.append((self.mask_topic, img, stamp, header))
             else:
-                print("Failed rendering robot mask for time %s: %r" % (msg.header.stamp, err), file=sys.stderr)
+                print("Failed rendering robot mask for time %i.%09i: %r" % (
+                    msg.header.stamp.secs, msg.header.stamp.nsecs, err), file=sys.stderr)
 
         return result
 

src/robot_model_renderer/renderer.py

@@ -33,7 +33,7 @@ def _get_library():
 
         __lib.robot_model_renderer_createRobotModelRenderer.restype = robot_model_renderer_RobotModelRendererHandle
         __lib.robot_model_renderer_createRobotModelRenderer.argtypes = [
-            Allocator.ALLOCATOR, c_char_p, Allocator.ALLOCATOR, RobotModelRendererConfig,
+            Allocator.ALLOCATOR, c_char_p, Allocator.ALLOCATOR, RobotModelRendererConfig, c_bool,
         ]
 
         __lib.robot_model_renderer_deleteRobotModelRenderer.restype = None
@@ -85,13 +85,14 @@ def sensorMsgsEncodingToOgrePixelFormat(encoding):
 class RobotModelRenderer(object):
     """Renderer of robot model from URDF."""
 
-    def __init__(self, model, config, imageEncoding=None):
+    def __init__(self, model, config, imageEncoding=None, warnExtrapolation=True):
         """Construct the renderer.
 
         :param str model: The URDF model to load.
         :param RobotModelRendererConfig config: Configuration of this class.
         :param imageEncoding: Encoding of the rendered image (one of sensor_msgs/image_encodings.h).
         :type imageEncoding: str or none
+        :param bool warnExtrapolation: If true, warn on TF extrapolation errors.
         :throws RuntimeError: If construction failed.
         """
         self._log_alloc = LogMessagesAllocator()
@@ -104,7 +105,7 @@ def __init__(self, model, config, imageEncoding=None):
 
         error_alloc = StringAllocator()
         self._handle = _get_library().robot_model_renderer_createRobotModelRenderer(
-            self._log_alloc_func, model.encode("utf-8"), error_alloc.get_cfunc(), config)
+            self._log_alloc_func, model.encode("utf-8"), error_alloc.get_cfunc(), config, warnExtrapolation)
 
         self._camera_info = None
         self._flush_log_messages()