RobotModelDisplay: display error when mesh fails to load

ros-visualization · Mar 18, 2021 · e1f18d0 · e1f18d0
1 parent 671d1f5
commit e1f18d0
Show file tree

Hide file tree

Showing 6 changed files with 37 additions and 10 deletions.
diff --git a/src/rviz/default_plugin/robot_model_display.cpp b/src/rviz/default_plugin/robot_model_display.cpp
@@ -99,6 +99,8 @@ void RobotModelDisplay::onInitialize()
 {
   robot_ = new Robot(scene_node_, context_, "Robot: " + getName().toStdString(), this);
 
+  connect(robot_, &Robot::linkMeshLoadingFailed, this, &RobotModelDisplay::onLinkMeshLoadingFailed);
+
   updateVisualVisible();
   updateCollisionVisible();
   updateAlpha();
@@ -113,7 +115,12 @@ void RobotModelDisplay::updateAlpha()
 void RobotModelDisplay::updateRobotDescription()
 {
   if (isEnabled())
-    load();
+      load();
+}
+
+void RobotModelDisplay::onLinkMeshLoadingFailed(const QString& linkName, const QString& details)
+{
+  setStatus(StatusLevel::Error, linkName, details);
 }
 
 void RobotModelDisplay::updateVisualVisible()

diff --git a/src/rviz/default_plugin/robot_model_display.h b/src/rviz/default_plugin/robot_model_display.h
@@ -81,6 +81,7 @@ private Q_SLOTS:
   void updateTfPrefix();
   void updateAlpha();
   void updateRobotDescription();
+  void onLinkMeshLoadingFailed(const QString& linkName, const QString& details);
 
 protected:
   /** @brief Loads a URDF from the ros-param named by our

diff --git a/src/rviz/robot/robot.cpp b/src/rviz/robot/robot.cpp
@@ -222,7 +222,10 @@ RobotLink* Robot::LinkFactory::createLink(Robot* robot,
                                           bool visual,
                                           bool collision)
 {
-  return new RobotLink(robot, link, parent_joint_name, visual, collision);
+  RobotLink* l = new RobotLink(robot, link, parent_joint_name);
+  connect(l, &RobotLink::meshLoadingFailed, robot, &Robot::linkMeshLoadingFailed);
+  l->init(link, visual, collision);
+  return l;
 }
 
 RobotJoint* Robot::LinkFactory::createJoint(Robot* robot, const urdf::JointConstSharedPtr& joint)

diff --git a/src/rviz/robot/robot.h b/src/rviz/robot/robot.h
@@ -242,6 +242,8 @@ class Robot : public QObject
 
   // set joint checkboxes and All Links Enabled checkbox based on current link enables.
   void calculateJointCheckboxes();
+Q_SIGNALS:
+  void linkMeshLoadingFailed(const QString& linkName, const QString& details);
 
 private Q_SLOTS:
   void changedLinkTreeStyle();

diff --git a/src/rviz/robot/robot_link.cpp b/src/rviz/robot/robot_link.cpp
@@ -154,9 +154,7 @@ void RobotLinkSelectionHandler::postRenderPass(uint32_t /*pass*/)
 
 RobotLink::RobotLink(Robot* robot,
                      const urdf::LinkConstSharedPtr& link,
-                     const std::string& parent_joint_name,
-                     bool visual,
-                     bool collision)
+                     const std::string& parent_joint_name)
   : robot_(robot)
   , scene_manager_(robot->getDisplayContext()->getSceneManager())
   , context_(robot->getDisplayContext())
@@ -203,7 +201,12 @@ RobotLink::RobotLink(Robot* robot,
 
   visual_node_ = robot_->getVisualNode()->createChildSceneNode();
   collision_node_ = robot_->getCollisionNode()->createChildSceneNode();
+}
 
+void RobotLink::init(const urdf::LinkConstSharedPtr& link,
+                     bool visual,
+                     bool collision)
+{
   // create material for coloring links
   color_material_ =
       Ogre::MaterialPtr(new Ogre::Material(nullptr, "robot link color material", 0, ROS_PACKAGE_NAME));
@@ -599,20 +602,28 @@ void RobotLink::createEntityForGeometryElement(const urdf::LinkConstSharedPtr& l
 
     try
     {
-      if (!loadMeshFromResource(model_name).isNull())
+      if (loadMeshFromResource(model_name).isNull())
       {
-        entity = scene_manager_->createEntity(ss.str(), model_name);
+        Q_EMIT meshLoadingFailed(QString(name_.c_str()), QString("Failed loading mesh '%1'")
+                                 .arg(model_name.c_str()));
+        return;
       }
+      entity = scene_manager_->createEntity(ss.str(), model_name);
+
     }
     catch (Ogre::InvalidParametersException& e)
     {
       ROS_ERROR("Could not convert mesh resource '%s' for link '%s'. It might be an empty mesh: %s",
                 model_name.c_str(), link->name.c_str(), e.what());
+      Q_EMIT meshLoadingFailed(QString(name_.c_str()), QString("Could not convert mesh resource '%1'. It might be an empty mesh: %3")
+                               .arg(model_name.c_str()).arg(e.what()));
     }
     catch (Ogre::Exception& e)
     {
       ROS_ERROR("Could not load model '%s' for link '%s': %s", model_name.c_str(), link->name.c_str(),
                 e.what());
+      Q_EMIT meshLoadingFailed(QString(name_.c_str()), QString("Could not load model '%s': %s").arg(model_name.c_str())
+                               .arg(e.what()));
     }
     break;
   }

diff --git a/src/rviz/robot/robot_link.h b/src/rviz/robot/robot_link.h
@@ -87,9 +87,7 @@ class RobotLink : public QObject
 public:
   RobotLink(Robot* robot,
             const urdf::LinkConstSharedPtr& link,
-            const std::string& parent_joint_name,
-            bool visual,
-            bool collision);
+            const std::string& parent_joint_name);
   ~RobotLink() override;
 
   virtual void setRobotAlpha(float a);
@@ -168,6 +166,11 @@ class RobotLink : public QObject
   // expand all sub properties
   void expandDetails(bool expand);
 
+  void init(const urdf::LinkConstSharedPtr& link, bool visual, bool collision);
+
+Q_SIGNALS:
+  void meshLoadingFailed(const QString& name, const QString& details);
+
 public Q_SLOTS:
   /** @brief Update the visibility of the link elements: visual mesh, collision mesh, trail, and axes.
    *