Pulls out reading from URDF file to tf buffer. (#199)

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -125,6 +125,16 @@ google_library(time_conversion
     time_conversion.h
 )
 
+google_library(urdf_reader
+  USES_CARTOGRAPHER
+  SRCS
+    urdf_reader.cc
+  HDRS
+    urdf_reader.h
+  DEPENDS
+    msg_conversion
+)
+
 google_test(configuration_files_test
   USES_CARTOGRAPHER
   SRCS
@@ -150,6 +160,7 @@ google_binary(cartographer_assets_writer
   DEPENDS
     msg_conversion
     time_conversion
+    urdf_reader
 )
 
 google_binary(cartographer_node

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -26,6 +26,7 @@
 #include "cartographer/transform/transform_interpolation_buffer.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/time_conversion.h"
+#include "cartographer_ros/urdf_reader.h"
 #include "gflags/gflags.h"
 #include "glog/logging.h"
 #include "ros/ros.h"
@@ -56,34 +57,6 @@ namespace {
 
 namespace carto = ::cartographer;
 
-void ReadStaticTransformsFromUrdf(const string& urdf_filename,
-                                  ::tf2_ros::Buffer* const buffer) {
-  urdf::Model model;
-  CHECK(model.initFile(urdf_filename));
-#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
-  std::vector<urdf::LinkSharedPtr> links;
-#else
-  std::vector<boost::shared_ptr<urdf::Link>> links;
-#endif
-  model.getLinks(links);
-  for (const auto& link : links) {
-    if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) {
-      continue;
-    }
-
-    const urdf::Pose& pose =
-        link->parent_joint->parent_to_joint_origin_transform;
-    geometry_msgs::TransformStamped transform;
-    transform.transform = ToGeometryMsgTransform(carto::transform::Rigid3d(
-        Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z),
-        Eigen::Quaterniond(pose.rotation.w, pose.rotation.x, pose.rotation.y,
-                           pose.rotation.z)));
-    transform.child_frame_id = link->name;
-    transform.header.frame_id = link->getParent()->name;
-    buffer->setTransform(transform, "urdf", true /* is_static */);
-  }
-}
-
 void Run(const string& trajectory_filename, const string& bag_filename,
          const string& configuration_directory,
          const string& configuration_basename, const string& urdf_filename) {

cartographer_ros/cartographer_ros/urdf_reader.cc

@@ -0,0 +1,56 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_ros/urdf_reader.h"
+
+#include <string>
+#include <vector>
+
+#include "cartographer_ros/msg_conversion.h"
+#include "urdf/model.h"
+
+namespace cartographer_ros {
+
+void ReadStaticTransformsFromUrdf(const string& urdf_filename,
+                                  tf2_ros::Buffer* const buffer) {
+  urdf::Model model;
+  CHECK(model.initFile(urdf_filename));
+#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
+  std::vector<urdf::LinkSharedPtr> links;
+#else
+  std::vector<boost::shared_ptr<urdf::Link> > links;
+#endif
+  model.getLinks(links);
+  for (const auto& link : links) {
+    if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) {
+      continue;
+    }
+
+    const urdf::Pose& pose =
+        link->parent_joint->parent_to_joint_origin_transform;
+    geometry_msgs::TransformStamped transform;
+    transform.transform =
+        ToGeometryMsgTransform(cartographer::transform::Rigid3d(
+            Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z),
+            Eigen::Quaterniond(pose.rotation.w, pose.rotation.x,
+                               pose.rotation.y, pose.rotation.z)));
+    transform.child_frame_id = link->name;
+    transform.header.frame_id = link->getParent()->name;
+    buffer->setTransform(transform, "urdf", true /* is_static */);
+  }
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/urdf_reader.h

@@ -0,0 +1,30 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_ROS_URDF_READER_H_
+#define CARTOGRAPHER_ROS_URDF_READER_H_
+
+#include "cartographer/common/port.h"
+#include "tf2_ros/buffer.h"
+
+namespace cartographer_ros {
+
+void ReadStaticTransformsFromUrdf(const string& urdf_filename,
+                                  tf2_ros::Buffer* buffer);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_URDF_READER_H_