diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index f5096c8..ca076c4 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/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 diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index a3653b9..6a8eda0 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/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 links; -#else - std::vector> 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) { diff --git a/cartographer_ros/cartographer_ros/urdf_reader.cc b/cartographer_ros/cartographer_ros/urdf_reader.cc new file mode 100644 index 0000000..07ab385 --- /dev/null +++ b/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 +#include + +#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 links; +#else + std::vector > 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 diff --git a/cartographer_ros/cartographer_ros/urdf_reader.h b/cartographer_ros/cartographer_ros/urdf_reader.h new file mode 100644 index 0000000..0642870 --- /dev/null +++ b/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_