Harmonise failed transform behaviour (#292)

* Add convenience method for setting failed transform + minor cleanup

* Harmonise transform behaviour over many displays

* Delete default value for pointcloud

* Fix camera_display_visual_test

rviz_common/include/rviz_common/display.hpp

@@ -166,7 +166,7 @@ class RVIZ_COMMON_PUBLIC Display : public rviz_common::properties::BoolProperty
    * This is thread-safe.
    *
    * Every Display has a StatusList to indicate how it is doing.  The
-   * StatusList has StatusPropertychildren indicating the status of
+   * StatusList has StatusProperty children indicating the status of
    * various subcomponents of the Display.  Each child of the status
    * has a level, a name, and descriptive text.  The top-level
    * StatusList has a level which is set to the worst of all the
@@ -193,6 +193,21 @@ class RVIZ_COMMON_PUBLIC Display : public rviz_common::properties::BoolProperty
     const std::string & name,
     const std::string & text);
 
+  /// Convenience: Show and log missing transform
+  /**
+   * Convenience function which
+   * @param frame frame with missing transform to fixed_frame
+   * @param additional_info additional info included in the error_message which then reads "Could
+   * not transform <additional_info> from [<fixed_frame>] to [<frame>].
+   */
+  void
+  setMissingTransformToFixedFrame(
+    const std::string & frame, const std::string & additional_info = "");
+
+  /// Convenience: Set Transform ok
+  void
+  setTransformOk();
+
   /// Delete the status entry with the given name.
   /**
    * This is thread-safe.

rviz_common/src/rviz_common/display.cpp

@@ -48,6 +48,7 @@
 #include "rviz_rendering/apply_visibility_bits.hpp"
 
 #include "rviz_common/display_context.hpp"
+#include "rviz_common/logging.hpp"
 #include "rviz_common/panel_dock_widget.hpp"
 #include "rviz_common/properties/property_tree_model.hpp"
 #include "rviz_common/properties/status_list.hpp"
@@ -57,13 +58,13 @@ namespace rviz_common
 {
 
 Display::Display()
-: context_(0),
-  scene_node_(NULL),
-  status_(0),
+: context_(nullptr),
+  scene_node_(nullptr),
+  status_(nullptr),
   initialized_(false),
   visibility_bits_(0xFFFFFFFF),
-  associated_widget_(NULL),
-  associated_widget_panel_(NULL)
+  associated_widget_(nullptr),
+  associated_widget_panel_(nullptr)
 {
   // Needed for timeSignal (see header) to work across threads
   qRegisterMetaType<rclcpp::Time>();
@@ -215,6 +216,21 @@ void Display::setStatusStd(
   setStatus(level, QString::fromStdString(name), QString::fromStdString(text));
 }
 
+void Display::setMissingTransformToFixedFrame(
+  const std::string & frame, const std::string & additional_info)
+{
+  std::string error_string =
+    "Could not transform " + (additional_info.empty() ? "from [" : additional_info + " from [") +
+    frame + "] to [" + fixed_frame_.toStdString() + "]";
+  setStatusStd(properties::StatusProperty::Error, "Transform", error_string);
+  RVIZ_COMMON_LOG_DEBUG(error_string);
+}
+
+void Display::setTransformOk()
+{
+  setStatusStd(properties::StatusProperty::Ok, "Transform", "Ok");
+}
+
 void Display::deleteStatusStd(const std::string & name)
 {
   deleteStatus(QString::fromStdString(name));
@@ -397,11 +413,11 @@ void Display::setAssociatedWidget(QWidget * widget)
       connect(associated_widget_panel_, SIGNAL(closed()), this, SLOT(disable()));
       associated_widget_panel_->setIcon(getIcon());
     } else {
-      associated_widget_panel_ = NULL;
+      associated_widget_panel_ = nullptr;
       associated_widget_->setWindowTitle(getName());
     }
   } else {
-    associated_widget_panel_ = NULL;
+    associated_widget_panel_ = nullptr;
   }
 }
 
@@ -418,11 +434,7 @@ PanelDockWidget * Display::getAssociatedWidgetPanel()
 void Display::associatedPanelVisibilityChange(bool visible)
 {
   // if something external makes the panel visible, make sure we're enabled
-  if (visible) {
-    setEnabled(true);
-  } else {
-    setEnabled(false);
-  }
+  setEnabled(visible);
 }
 
 void Display::setIcon(const QIcon & icon)

rviz_default_plugins/src/rviz_default_plugins/displays/camera/camera_display.cpp

@@ -382,8 +382,13 @@ bool CameraDisplay::updateCamera()
 
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
-  context_->getFrameManager()->getTransform(
-    image->header.frame_id, image->header.stamp, position, orientation);
+  if (!context_->getFrameManager()->getTransform(
+      image->header.frame_id, image->header.stamp, position, orientation))
+  {
+    setMissingTransformToFixedFrame(image->header.frame_id);
+    return false;
+  }
+  setTransformOk();
 
   // convert vision (Z-forward) frame to ogre frame (Z-out)
   orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X);

rviz_default_plugins/src/rviz_default_plugins/displays/grid/grid_display.cpp

@@ -145,15 +145,9 @@ void GridDisplay::update(float dt, float ros_dt)
   if (context_->getFrameManager()->getTransform(frame, position, orientation)) {
     scene_node_->setPosition(position);
     scene_node_->setOrientation(orientation);
-    setStatus(StatusProperty::Ok, "Transform", "Transform OK");
+    setTransformOk();
   } else {
-    std::string error;
-    if (context_->getFrameManager()->transformHasProblems(frame, error)) {
-      setStatus(StatusProperty::Error, "Transform", QString::fromStdString(error));
-    } else {
-      setStatus(StatusProperty::Error, "Transform",
-        "Could not transform from [" + qframe + "] to [" + fixed_frame_ + "]");
-    }
+    setMissingTransformToFixedFrame(qframe.toStdString());
   }
 }
 

rviz_default_plugins/src/rviz_default_plugins/displays/path/path_display.cpp

@@ -416,12 +416,10 @@ void PathDisplay::processMessage(nav_msgs::msg::Path::ConstSharedPtr msg)
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
   if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) {
-    rviz_common::UniformStringStream ss;
-    ss << "Error transforming from frame '" << msg->header.frame_id << "' to frame '" <<
-      qPrintable(fixed_frame_) << "'";
-    setStatusStd(rviz_common::properties::StatusProperty::Error, "Message", ss.str());
+    setMissingTransformToFixedFrame(msg->header.frame_id);
     return;
   }
+  setTransformOk();
 
   Ogre::Matrix4 transform(orientation);
   transform.setTrans(position);

rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud2_display.cpp

@@ -57,7 +57,6 @@ PointCloud2Display::PointCloud2Display()
 void PointCloud2Display::onInitialize()
 {
   RTDClass::onInitialize();
-  topic_property_->setValue("pointcloud2");
   point_cloud_common_->initialize(context_, scene_node_);
 }
 

rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp

@@ -576,13 +576,10 @@ bool PointCloudCommon::transformCloud(const CloudInfoPtr & cloud_info, bool upda
         cloud_info->position_,
         cloud_info->orientation_))
     {
-      std::stringstream ss;
-      ss << "Failed to transform from frame [" << cloud_info->message_->header.frame_id << "] to "
-        "frame [" << context_->getFrameManager()->getFixedFrame() << "]";
-      display_->setStatusStd(
-        rviz_common::properties::StatusProperty::Error, message_status_name_, ss.str());
+      display_->setMissingTransformToFixedFrame(cloud_info->message_->header.frame_id);
       return false;
     }
+    display_->setTransformOk();
   }
   // Remove outdated error message
   display_->deleteStatusStd(message_status_name_);

rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp

@@ -53,7 +53,6 @@ PointCloudDisplay::PointCloudDisplay()
 void PointCloudDisplay::onInitialize()
 {
   RTDClass::onInitialize();
-  topic_property_->setValue("pointcloud");
   point_cloud_common_->initialize(context_, scene_node_);
 }
 

rviz_default_plugins/src/rviz_default_plugins/displays/polygon/polygon_display.cpp

@@ -106,9 +106,10 @@ void PolygonDisplay::processMessage(geometry_msgs::msg::PolygonStamped::ConstSha
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
   if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) {
-    RVIZ_COMMON_LOG_DEBUG_STREAM("Error transforming from frame '" <<
-      msg->header.frame_id.c_str() << "' to frame '" << qPrintable(fixed_frame_) << "'");
+    setMissingTransformToFixedFrame(msg->header.frame_id);
+    return;
   }
+  setTransformOk();
 
   scene_node_->setPosition(position);
   scene_node_->setOrientation(orientation);

rviz_default_plugins/src/rviz_default_plugins/displays/pose/pose_display.cpp

@@ -199,11 +199,10 @@ void PoseDisplay::processMessage(geometry_msgs::msg::PoseStamped::ConstSharedPtr
   if (!context_->getFrameManager()->transform(message->header, message->pose, position,
     orientation))
   {
-    RVIZ_COMMON_LOG_ERROR_STREAM("Error transforming pose '" << qPrintable(getName()) <<
-      "' from frame '" << message->header.frame_id.c_str() << "' to frame '" <<
-      qPrintable(fixed_frame_));
+    setMissingTransformToFixedFrame(message->header.frame_id, "pose " + getNameStd());
     return;
   }
+  setTransformOk();
 
   pose_valid_ = true;
   updateShapeVisibility();

rviz_default_plugins/src/rviz_default_plugins/displays/pose_array/pose_array_display.cpp

@@ -217,11 +217,11 @@ bool PoseArrayDisplay::setTransform(std_msgs::msg::Header const & header)
   Ogre::Vector3 position;
   Ogre::Quaternion orientation;
   if (!context_->getFrameManager()->getTransform(header, position, orientation)) {
-    RVIZ_COMMON_LOG_ERROR_STREAM(
-      "Error transforming pose '" << qPrintable(getName()) << "' from frame '" <<
-        header.frame_id.c_str() << "' to frame '" << qPrintable(fixed_frame_) << "'");
+    setMissingTransformToFixedFrame(header.frame_id, "pose " + getNameStd());
     return false;
   }
+  setTransformOk();
+
   scene_node_->setPosition(position);
   scene_node_->setOrientation(orientation);
   return true;

rviz_default_plugins/test/rviz_default_plugins/displays/camera/camera_display_visual_test.cpp

@@ -46,7 +46,7 @@ TEST_F(VisualTestFixture, test_camera_display_with_published_image) {
   auto points = {nodes::createPoint(0, 0, 10)};
   std::vector<PublisherWithFrame> publishers = {
     PublisherWithFrame(std::make_shared<nodes::CameraInfoPublisher>(), "image"),
-    PublisherWithFrame(std::make_shared<nodes::ImagePublisher>(), "image"),
+    PublisherWithFrame(std::make_shared<nodes::ImagePublisher>(), "image_frame"),
     PublisherWithFrame(std::make_shared<nodes::PointCloudPublisher>(points), "pointcloud_frame")
   };
   auto cam_publisher = std::make_unique<VisualTestPublisher>(publishers);
@@ -59,6 +59,7 @@ TEST_F(VisualTestFixture, test_camera_display_with_published_image) {
   camera_display->collapse();
 
   auto pointcloud_display = addDisplay<PointCloudDisplayPageObject>();
+  pointcloud_display->setTopic("/pointcloud");
   pointcloud_display->setStyle("Spheres");
   pointcloud_display->setSizeMeters(11);
   pointcloud_display->setColor(0, 0, 255);