diff --git a/src/rviz/default_plugin/robot_model_display.cpp b/src/rviz/default_plugin/robot_model_display.cpp index a71425b76a..716a2acd8a 100644 --- 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 index 90bbe3d6f1..8c5aee9839 100644 --- 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 index dddb4b702a..ffd1e1a993 100644 --- 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 index d5b5255a4a..97a1b0edcb 100644 --- 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 index 2d12448c9f..2a153cfab2 100644 --- 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 index 6d83708cc4..0cc359aeaf 100644 --- 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. *