robot_state_publisher.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Wim Meeussen */
36 
37 #include <kdl/frames_io.hpp>
38 #include <geometry_msgs/TransformStamped.h>
39 #include <tf2_kdl/tf2_kdl.h>
40 
42 
43 using namespace std;
44 using namespace ros;
45 
46 namespace robot_state_publisher {
47 
48 RobotStatePublisher::RobotStatePublisher(const KDL::Tree& tree, const urdf::Model& model)
49  : model_(model)
50 {
51  // walk the tree and add segments to segments_
53 }
54 
55 // add children to correct maps
56 void RobotStatePublisher::addChildren(const KDL::SegmentMap::const_iterator segment)
57 {
58  const std::string& root = GetTreeElementSegment(segment->second).getName();
59 
60  const std::vector<KDL::SegmentMap::const_iterator>& children = GetTreeElementChildren(segment->second);
61  for (unsigned int i=0; i<children.size(); i++) {
62  const KDL::Segment& child = GetTreeElementSegment(children[i]->second);
63  SegmentPair s(GetTreeElementSegment(children[i]->second), root, child.getName());
64  if (child.getJoint().getType() == KDL::Joint::None) {
65  if (model_.getJoint(child.getJoint().getName()) && model_.getJoint(child.getJoint().getName())->type == urdf::Joint::FLOATING) {
66  ROS_INFO("Floating joint. Not adding segment from %s to %s. This TF can not be published based on joint_states info", root.c_str(), child.getName().c_str());
67  }
68  else {
69  segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
70  ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
71  }
72  }
73  else {
74  segments_.insert(make_pair(child.getJoint().getName(), s));
75  ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
76  }
77  addChildren(children[i]);
78  }
79 }
80 
81 
82 // publish moving transforms
83 void RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time, const std::string& tf_prefix)
84 {
85  ROS_DEBUG("Publishing transforms for moving joints");
86  std::vector<geometry_msgs::TransformStamped> tf_transforms;
87 
88  // loop over all joints
89  for (map<string, double>::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++) {
90  std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jnt->first);
91  if (seg != segments_.end()) {
92  geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(jnt->second));
93  tf_transform.header.stamp = time;
94  tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root);
95  tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip);
96  tf_transforms.push_back(tf_transform);
97  }
98  else {
99  ROS_WARN_THROTTLE(10, "Joint state with name: \"%s\" was received but not found in URDF", jnt->first.c_str());
100  }
101  }
102  tf_broadcaster_.sendTransform(tf_transforms);
103 }
104 
105 // publish fixed transforms
106 void RobotStatePublisher::publishFixedTransforms(const std::string& tf_prefix, bool use_tf_static)
107 {
108  ROS_DEBUG("Publishing transforms for fixed joints");
109  std::vector<geometry_msgs::TransformStamped> tf_transforms;
110  geometry_msgs::TransformStamped tf_transform;
111 
112  // loop over all fixed segments
113  for (map<string, SegmentPair>::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++) {
114  geometry_msgs::TransformStamped tf_transform = tf2::kdlToTransform(seg->second.segment.pose(0));
115  tf_transform.header.stamp = ros::Time::now();
116  if (!use_tf_static) {
117  tf_transform.header.stamp += ros::Duration(0.5);
118  }
119  tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root);
120  tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip);
121  tf_transforms.push_back(tf_transform);
122  }
123  if (use_tf_static) {
124  static_tf_broadcaster_.sendTransform(tf_transforms);
125  }
126  else {
127  tf_broadcaster_.sendTransform(tf_transforms);
128  }
129 }
130 
131 }
#define GetTreeElementSegment(tree_element)
virtual void addChildren(const KDL::SegmentMap::const_iterator segment)
#define ROS_WARN_THROTTLE(rate,...)
XmlRpcServer s
virtual void publishFixedTransforms(const std::string &tf_prefix, bool use_tf_static=false)
#define GetTreeElementChildren(tree_element)
std::map< std::string, SegmentPair > segments_fixed_
std::string resolve(const std::string &prefix, const std::string &frame_name)
const std::string & getName() const
const std::string & getName() const
tf2_ros::TransformBroadcaster tf_broadcaster_
virtual void publishTransforms(const std::map< std::string, double > &joint_positions, const ros::Time &time, const std::string &tf_prefix)
std::map< std::string, SegmentPair > segments_
#define ROS_INFO(...)
const Joint & getJoint() const
void sendTransform(const geometry_msgs::TransformStamped &transform)
tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_
SegmentMap::const_iterator getRootSegment() const
geometry_msgs::TransformStamped kdlToTransform(const KDL::Frame &k)
static Time now()
void sendTransform(const geometry_msgs::TransformStamped &transform)
const JointType & getType() const
#define ROS_DEBUG(...)


robot_state_publisher
Author(s): Ioan Sucan , Jackie Kay , Wim Meeussen
autogenerated on Tue Sep 3 2019 03:44:13