PR #2245: Fix deferred robot model loading

moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.h

@@ -68,6 +68,7 @@ class RobotStateDisplay : public rviz::Display
   RobotStateDisplay();
   ~RobotStateDisplay() override;
 
+  void load(const rviz::Config& config) override;
   void update(float wall_dt, float ros_dt) override;
   void reset() override;
 
@@ -127,7 +128,6 @@ private Q_SLOTS:
   robot_state::RobotStatePtr robot_state_;
   std::map<std::string, std_msgs::ColorRGBA> highlights_;
   bool update_state_;
-  bool load_robot_model_;  // for delayed robot initialization
 
   rviz::StringProperty* robot_description_property_;
   rviz::StringProperty* root_link_name_property_;

moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp

@@ -58,7 +58,7 @@ namespace moveit_rviz_plugin
 // ******************************************************************************************
 // Base class contructor
 // ******************************************************************************************
-RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false), load_robot_model_(false)
+RobotStateDisplay::RobotStateDisplay() : Display(), update_state_(false)
 {
   robot_description_property_ =
       new rviz::StringProperty("Robot Description", "robot_description",
@@ -119,8 +119,8 @@ void RobotStateDisplay::reset()
   robot_->clear();
   rdf_loader_.reset();
   Display::reset();
-
-  loadRobotModel();
+  if (isEnabled())
+    onEnable();
 }
 
 void RobotStateDisplay::changedAllLinks()
@@ -292,6 +292,8 @@ void RobotStateDisplay::changedRobotStateTopic()
   if (static_cast<bool>(robot_state_))
     robot_state_->setToDefaultValues();
   update_state_ = true;
+  robot_->setVisible(false);
+  setStatus(rviz::StatusProperty::Warn, "RobotState", "No msg received");
 
   robot_state_subscriber_ = root_nh_.subscribe(robot_state_topic_property_->getStdString(), 10,
                                                &RobotStateDisplay::newRobotStateCallback, this);
@@ -306,15 +308,17 @@ void RobotStateDisplay::newRobotStateCallback(const moveit_msgs::DisplayRobotSta
   // possibly use TF to construct a robot_state::Transforms object to pass in to the conversion function?
   try
   {
-    robot_state::robotStateMsgToRobotState(state_msg->state, *robot_state_);
+    moveit::core::robotStateMsgToRobotState(state_msg->state, *robot_state_);
     setRobotHighlights(state_msg->highlight_links);
     setStatus(rviz::StatusProperty::Ok, "RobotState", "");
+    robot_->setVisible(true);
   }
   catch (const moveit::Exception& e)
   {
     robot_state_->setToDefaultValues();
     setRobotHighlights(moveit_msgs::DisplayRobotState::_highlight_links_type());
     setStatus(rviz::StatusProperty::Error, "RobotState", e.what());
+    robot_->setVisible(false);
     return;
   }
   update_state_ = true;
@@ -353,9 +357,7 @@ void RobotStateDisplay::unsetLinkColor(rviz::Robot* robot, const std::string& li
 // ******************************************************************************************
 void RobotStateDisplay::loadRobotModel()
 {
-  load_robot_model_ = false;
-  if (!rdf_loader_)
-    rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString()));
+  rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString()));
 
   if (rdf_loader_->getURDF())
   {
@@ -369,22 +371,31 @@ void RobotStateDisplay::loadRobotModel()
     root_link_name_property_->setStdString(getRobotModel()->getRootLinkName());
     root_link_name_property_->blockSignals(old_state);
     update_state_ = true;
-    setStatus(rviz::StatusProperty::Ok, "RobotState", "Planning Model Loaded Successfully");
+    setStatus(rviz::StatusProperty::Ok, "RobotModel", "Loaded successfully");
 
     changedEnableVisualVisible();
     changedEnableCollisionVisible();
-    robot_->setVisible(true);
   }
   else
-    setStatus(rviz::StatusProperty::Error, "RobotState", "No Planning Model Loaded");
+    setStatus(rviz::StatusProperty::Error, "RobotModel", "Loading failed");
 
   highlights_.clear();
 }
 
+void RobotStateDisplay::load(const rviz::Config& config)
+{
+  // This property needs to be loaded in onEnable() below, which is triggered
+  // in the beginning of Display::load() before the other property would be available
+  robot_description_property_->load(config.mapGetChild("Robot Description"));
+  Display::load(config);
+}
+
 void RobotStateDisplay::onEnable()
 {
   Display::onEnable();
-  load_robot_model_ = true;  // allow loading of robot model in update()
+  if (!rdf_loader_)
+    loadRobotModel();
+  changedRobotStateTopic();
   calculateOffsetPosition();
 }
 
@@ -402,13 +413,6 @@ void RobotStateDisplay::onDisable()
 void RobotStateDisplay::update(float wall_dt, float ros_dt)
 {
   Display::update(wall_dt, ros_dt);
-
-  if (load_robot_model_)
-  {
-    loadRobotModel();
-    changedRobotStateTopic();
-  }
-
   calculateOffsetPosition();
   if (robot_ && update_state_ && robot_state_)
   {

moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.h

@@ -66,6 +66,7 @@ class TrajectoryDisplay : public rviz::Display
 
   void loadRobotModel();
 
+  void load(const rviz::Config& config) override;
   void update(float wall_dt, float ros_dt) override;
   void reset() override;
 
@@ -89,7 +90,6 @@ private Q_SLOTS:
   rdf_loader::RDFLoaderPtr rdf_loader_;
   robot_model::RobotModelConstPtr robot_model_;
   robot_state::RobotStatePtr robot_state_;
-  bool load_robot_model_;  // for delayed robot initialization
 
   // Properties
   rviz::StringProperty* robot_description_property_;

moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp

@@ -41,7 +41,7 @@
 
 namespace moveit_rviz_plugin
 {
-TrajectoryDisplay::TrajectoryDisplay() : Display(), load_robot_model_(false)
+TrajectoryDisplay::TrajectoryDisplay() : Display()
 {
   // The robot description property is only needed when using the trajectory playback standalone (not within motion
   // planning plugin)
@@ -64,7 +64,6 @@ void TrajectoryDisplay::onInitialize()
 
 void TrajectoryDisplay::loadRobotModel()
 {
-  load_robot_model_ = false;
   rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString()));
 
   if (!rdf_loader_->getURDF())
@@ -81,7 +80,6 @@ void TrajectoryDisplay::loadRobotModel()
 
   // Send to child class
   trajectory_visual_->onRobotModelLoaded(robot_model_);
-  trajectory_visual_->onEnable();
 }
 
 void TrajectoryDisplay::reset()
@@ -91,10 +89,20 @@ void TrajectoryDisplay::reset()
   trajectory_visual_->reset();
 }
 
+void TrajectoryDisplay::load(const rviz::Config& config)
+{
+  // This property needs to be loaded in onEnable() below, which is triggered
+  // in the beginning of Display::load() before the other property would be available
+  robot_description_property_->load(config.mapGetChild("Robot Description"));
+  Display::load(config);
+}
+
 void TrajectoryDisplay::onEnable()
 {
   Display::onEnable();
-  load_robot_model_ = true;  // allow loading of robot model in update()
+  if (!rdf_loader_)
+    loadRobotModel();
+  trajectory_visual_->onEnable();
 }
 
 void TrajectoryDisplay::onDisable()
@@ -106,10 +114,6 @@ void TrajectoryDisplay::onDisable()
 void TrajectoryDisplay::update(float wall_dt, float ros_dt)
 {
   Display::update(wall_dt, ros_dt);
-
-  if (load_robot_model_)
-    loadRobotModel();
-
   trajectory_visual_->update(wall_dt, ros_dt);
 }