urdf_reader.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <string>
20 #include <vector>
21 
23 #include "urdf/model.h"
24 
25 namespace cartographer_ros {
26 
27 std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
28  const std::string& urdf_filename, tf2_ros::Buffer* const tf_buffer) {
29  urdf::Model model;
30  CHECK(model.initFile(urdf_filename));
31 #if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
32  std::vector<urdf::LinkSharedPtr> links;
33 #else
34  std::vector<boost::shared_ptr<urdf::Link> > links;
35 #endif
36  model.getLinks(links);
37  std::vector<geometry_msgs::TransformStamped> transforms;
38  for (const auto& link : links) {
39  if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) {
40  continue;
41  }
42 
43  const urdf::Pose& pose =
44  link->parent_joint->parent_to_joint_origin_transform;
45  geometry_msgs::TransformStamped transform;
46  transform.transform =
48  Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z),
49  Eigen::Quaterniond(pose.rotation.w, pose.rotation.x,
50  pose.rotation.y, pose.rotation.z)));
51  transform.child_frame_id = link->name;
52  transform.header.frame_id = link->getParent()->name;
53  tf_buffer->setTransform(transform, "urdf", true /* is_static */);
54  transforms.push_back(transform);
55  }
56  return transforms;
57 }
58 
59 } // namespace cartographer_ros
URDF_EXPORT bool initFile(const std::string &filename)
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d &rigid3d)
transform::Rigid3d pose
std::vector< geometry_msgs::TransformStamped > ReadStaticTransformsFromUrdf(const std::string &urdf_filename, tf2_ros::Buffer *const tf_buffer)
Definition: urdf_reader.cc:27


cartographer_ros
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:06:05