Updates to publish robot model

include/robot_state_publisher/joint_state_listener.h

@@ -63,7 +63,7 @@ class JointStateListener
    */
   JointStateListener(
     rclcpp::Node::SharedPtr node, const KDL::Tree & tree, const MimicMap & m,
-    const urdf::Model & model = urdf::Model());
+    const std::string & urdf_xml, const urdf::Model & model = urdf::Model());
 
   /// Destructor
   ~JointStateListener();

include/robot_state_publisher/robot_state_publisher.h

@@ -48,6 +48,8 @@
 #include "tf2_ros/transform_broadcaster.h"
 #include "urdf/model.h"
 
+#include "std_msgs/msg/string.hpp"
+
 #include "rclcpp/rclcpp.hpp"
 
 namespace robot_state_publisher
@@ -71,7 +73,7 @@ class RobotStatePublisher
    */
   RobotStatePublisher(
     rclcpp::Node::SharedPtr node_handle, const KDL::Tree & tree,
-    const urdf::Model & model);
+    const urdf::Model & model, const std::string & model_xml);
 
   /// Destructor
   virtual ~RobotStatePublisher() {}
@@ -94,6 +96,9 @@ class RobotStatePublisher
   const urdf::Model & model_;
   tf2_ros::TransformBroadcaster tf_broadcaster_;
   tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
+  std_msgs::msg::String model_xml_;
+  bool description_published_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr description_pub_;
 };
 
 }  // namespace robot_state_publisher

src/joint_state_listener.cpp

@@ -35,9 +35,10 @@
 /* Author: Wim Meeussen */
 
 #include <chrono>
-#include <string>
+#include <fstream>
 #include <map>
 #include <memory>
+#include <string>
 #include <utility>
 
 #include "kdl/tree.hpp"
@@ -56,9 +57,9 @@ namespace robot_state_publisher
 
 JointStateListener::JointStateListener(
   rclcpp::Node::SharedPtr node, const KDL::Tree & tree,
-  const MimicMap & m, const urdf::Model & model)
+  const MimicMap & m, const std::string & urdf_xml, const urdf::Model & model)
 : node_(node),
-  state_publisher_(node, tree, model),
+  state_publisher_(node, tree, model, urdf_xml),
   mimic_(m)
 {
   /*
@@ -162,6 +163,23 @@ void JointStateListener::callbackJointState(const sensor_msgs::msg::JointState::
 }
 }  // namespace robot_state_publisher
 
+bool read_urdf_xml(const std::string & filename, std::string & xml_string)
+{
+  std::fstream xml_file(filename.c_str(), std::fstream::in);
+  if (xml_file.is_open()) {
+    while (xml_file.good()) {
+      std::string line;
+      std::getline(xml_file, line);
+      xml_string += (line + "\n");
+    }
+    xml_file.close();
+    return true;
+  } else {
+    fprintf(stderr, "Could not open file [%s] for parsing.\n", filename.c_str());
+    return false;
+  }
+}
+
 // ----------------------------------
 // ----- MAIN -----------------------
 // ----------------------------------
@@ -200,8 +218,15 @@ int main(int argc, char ** argv)
     fprintf(stderr, "got segment %s\n", segment.first.c_str());
   }
 
+  // Read the URDF XML data (this should always succeed)
+  std::string urdf_xml;
+  if (!read_urdf_xml(argv[1], urdf_xml)) {
+    fprintf(stderr, "failed to read urdf xml '%s'\n", argv[1]);
+    return -1;
+  }
+
   auto node = std::make_shared<rclcpp::Node>("robot_state_publisher");
-  robot_state_publisher::JointStateListener state_publisher(node, tree, mimic, model);
+  robot_state_publisher::JointStateListener state_publisher(node, tree, mimic, urdf_xml, model);
 
   rclcpp::spin(node);
   return 0;

src/robot_state_publisher.cpp

@@ -71,13 +71,24 @@ geometry_msgs::msg::TransformStamped kdlToTransform(const KDL::Frame & k)
 RobotStatePublisher::RobotStatePublisher(
   rclcpp::Node::SharedPtr node_handle,
   const KDL::Tree & tree,
-  const urdf::Model & model)
+  const urdf::Model & model,
+  const std::string & model_xml)
 : model_(model),
   tf_broadcaster_(node_handle),
-  static_tf_broadcaster_(node_handle)
+  static_tf_broadcaster_(node_handle),
+  description_published_(false)
 {
   // walk the tree and add segments to segments_
   addChildren(tree.getRootSegment());
+
+  // Creates a latched topic
+  rmw_qos_profile_t qos = rmw_qos_profile_default;
+  qos.depth = 1;
+  qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+
+  model_xml_.data = model_xml;
+  description_pub_ = node_handle->create_publisher<std_msgs::msg::String>(
+    "robot_description", qos);
 }
 
 // add children to correct maps
@@ -133,6 +144,12 @@ void RobotStatePublisher::publishTransforms(
     }
   }
   tf_broadcaster_.sendTransform(tf_transforms);
+
+  // Publish the robot description, as necessary
+  if (!description_published_) {
+    description_pub_->publish(model_xml_);
+    description_published_ = true;
+  }
 }
 
 // publish fixed transforms