fix robot state publisher #14

Signed-off-by: Karsten Knese <karsten@openrobotics.org>
ros2 · Feb 25, 2019 · 314150c · 314150c
1 parent a8eea33
commit 314150c
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 5 deletions.
diff --git a/...default_plugins/include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp b/...default_plugins/include/rviz_default_plugins/displays/robot_model/robot_model_display.hpp
@@ -115,7 +115,9 @@ private Q_SLOTS:
    * "Robot Description" property, iterates through the links, and
    * loads any necessary models. */
   virtual void load_urdf();
-  virtual void load_urdf_from(const std::string & filepath);
+  virtual void load_urdf_from_file(const std::string & filepath);
+  virtual void load_urdf_from_string(const std::string & robot_description);
+  void display_urdf_content();
   void updateRobot();
 
   void processMessage(std_msgs::msg::String::ConstSharedPtr msg) override;

diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/robot_model/robot_model_display.cpp
@@ -120,6 +120,10 @@ RobotModelDisplay::RobotModelDisplay()
   topic_property_->setName("Description Topic");
 
   unreliable_property_->setHidden(true);
+  rmw_qos_profile_t qos = rmw_qos_profile_default;
+  qos.depth = 1;
+  qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  qos_profile = qos;
 
   tf_prefix_property_ = new StringProperty("TF Prefix", "",
       "Robot Model normally assumes the link name is the same as the tf frame name. "
@@ -206,13 +210,13 @@ void RobotModelDisplay::load_urdf()
   if (description_source_property_->getOptionInt() == DescriptionSource::FILE &&
     !description_file_property_->getString().isEmpty())
   {
-    load_urdf_from(description_file_property_->getStdString());
+    load_urdf_from_file(description_file_property_->getStdString());
   } else {
     clear();
   }
 }
 
-void RobotModelDisplay::load_urdf_from(const std::string & filepath)
+void RobotModelDisplay::load_urdf_from_file(const std::string & filepath)
 {
   std::string content;
   QFile urdf_file(QString::fromStdString(filepath));
@@ -232,6 +236,17 @@ void RobotModelDisplay::load_urdf_from(const std::string & filepath)
 
   robot_description_ = content;
 
+  display_urdf_content();
+}
+
+void RobotModelDisplay::load_urdf_from_string(const std::string & robot_description)
+{
+  robot_description_ = robot_description;
+  display_urdf_content();
+}
+
+void RobotModelDisplay::display_urdf_content()
+{
   TiXmlDocument doc;
   doc.Parse(robot_description_.c_str());
   if (!doc.RootElement() ) {
@@ -316,8 +331,7 @@ void RobotModelDisplay::reset()
 
 void RobotModelDisplay::processMessage(std_msgs::msg::String::ConstSharedPtr msg)
 {
-  std::string filepath = msg->data;
-  load_urdf_from(filepath);
+  load_urdf_from_string(msg->data);
 }
 
 }  // namespace displays