TransformBroadcaster.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016, Bielefeld University, CITEC
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Author: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
30  */
31 
32 #include "TransformBroadcaster.h"
33 
34 TransformBroadcaster::TransformBroadcaster(const QString &parent_frame, const QString &child_frame, QObject *parent) :
35  QObject(parent), valid_(false), enabled_(false)
36 {
37  setPosition(0,0,0);
38  setQuaternion(0,0,0,1);
39 
40  setParentFrame(parent_frame);
41  setChildFrame(child_frame);
42 
43  enabled_ = true;
44  check(); send();
45 }
46 
47 const geometry_msgs::TransformStamped &TransformBroadcaster::value() const
48 {
49  return msg_;
50 }
51 
52 void TransformBroadcaster::setValue(const geometry_msgs::TransformStamped &tf)
53 {
54  msg_ = tf;
55  check(); send();
56 }
57 
58 void TransformBroadcaster::setPose(const geometry_msgs::Pose &pose)
59 {
60  bool old = enabled_;
61  enabled_ = false;
62 
63  const geometry_msgs::Point &p = pose.position;
64  setPosition(p.x, p.y, p.z);
65 
66  const geometry_msgs::Quaternion &q = pose.orientation;
67  setQuaternion(q.x, q.y, q.z, q.w);
68 
69  enabled_ = old;
70  send();
71 }
72 
74 {
75  return enabled_;
76 }
77 
79 {
80  enabled_ = bEnabled;
81  check(); send();
82 }
83 
85 {
86  setEnabled(!bDisabled);
87 }
88 
89 void TransformBroadcaster::setParentFrame(const QString &frame)
90 {
91  msg_.header.frame_id = frame.toStdString();
92  check(); send();
93 }
94 
95 void TransformBroadcaster::setChildFrame(const QString &frame)
96 {
97  msg_.child_frame_id = frame.toStdString();
98  check(); send();
99 }
100 
101 void TransformBroadcaster::setPosition(const Eigen::Vector3d &p)
102 {
103  setPosition(p.x(), p.y(), p.z());
104 }
105 
106 void TransformBroadcaster::setQuaternion(const Eigen::Quaterniond &q)
107 {
108  setQuaternion(q.x(), q.y(), q.z(), q.w());
109 }
110 
111 void TransformBroadcaster::setPosition(double x, double y, double z)
112 {
113  msg_.transform.translation.x = x;
114  msg_.transform.translation.y = y;
115  msg_.transform.translation.z = z;
116  send();
117 }
118 
119 void TransformBroadcaster::setQuaternion(double x, double y, double z, double w)
120 {
121  msg_.transform.rotation.x = x;
122  msg_.transform.rotation.y = y;
123  msg_.transform.rotation.z = z;
124  msg_.transform.rotation.w = w;
125  send();
126 }
127 
129 {
130  if (enabled_ && valid_) {
131  msg_.header.stamp = ros::Time::now();
132  ++msg_.header.seq;
134  ros::spinOnce();
135  }
136 }
137 
139 {
140  valid_ =
141  !msg_.header.frame_id.empty() &&
142  !msg_.child_frame_id.empty() &&
143  msg_.header.frame_id != msg_.child_frame_id;
144 }
void setDisabled(bool bDisabled=true)
const geometry_msgs::TransformStamped & value() const
TransformBroadcaster(const QString &parent_frame="", const QString &child_frame="", QObject *parent=0)
void setQuaternion(const Eigen::Quaterniond &q)
void setChildFrame(const QString &frame)
geometry_msgs::TransformStamped msg_
void setParentFrame(const QString &frame)
void setEnabled(bool bEnabled=true)
void setPosition(const Eigen::Vector3d &p)
static Time now()
void setPose(const geometry_msgs::Pose &pose)
void setValue(const geometry_msgs::TransformStamped &tf)
void sendTransform(const geometry_msgs::TransformStamped &transform)
ROSCPP_DECL void spinOnce()
tf2_ros::StaticTransformBroadcaster broadcaster_


agni_tf_tools
Author(s): Robert Haschke
autogenerated on Fri Jun 7 2019 22:04:59