kdl_msg.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, Willow Garage, Inc.
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 
31 
32 namespace tf {
33 
34  void pointMsgToKDL(const geometry_msgs::Point &m, KDL::Vector &k)
35  {
36  k[0] = m.x;
37  k[1] = m.y;
38  k[2] = m.z;
39  }
40 
41  void pointKDLToMsg(const KDL::Vector &k, geometry_msgs::Point &m)
42  {
43  m.x = k[0];
44  m.y = k[1];
45  m.z = k[2];
46  }
47 
48  void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
49  {
50  k.p[0] = m.position.x;
51  k.p[1] = m.position.y;
52  k.p[2] = m.position.z;
53 
54  k.M = KDL::Rotation::Quaternion( m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w);
55  }
56 
57  void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
58  {
59  m.position.x = k.p[0];
60  m.position.y = k.p[1];
61  m.position.z = k.p[2];
62 
63  k.M.GetQuaternion(m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w);
64  }
65 
66  void quaternionMsgToKDL(const geometry_msgs::Quaternion &m, KDL::Rotation &k)
67  {
68  k = KDL::Rotation::Quaternion(m.x, m.y, m.z, m.w);
69  }
70 
71  void quaternionKDLToMsg(const KDL::Rotation &k, geometry_msgs::Quaternion &m)
72  {
73  k.GetQuaternion(m.x, m.y, m.z, m.w);
74  }
75 
76  void transformMsgToKDL(const geometry_msgs::Transform &m, KDL::Frame &k)
77  {
78  k.p[0] = m.translation.x;
79  k.p[1] = m.translation.y;
80  k.p[2] = m.translation.z;
81 
82  k.M = KDL::Rotation::Quaternion( m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w);
83  }
84 
85  void transformKDLToMsg(const KDL::Frame &k, geometry_msgs::Transform &m)
86  {
87  m.translation.x = k.p[0];
88  m.translation.y = k.p[1];
89  m.translation.z = k.p[2];
90 
91  k.M.GetQuaternion(m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w);
92  }
93 
94  void twistKDLToMsg(const KDL::Twist &t, geometry_msgs::Twist &m)
95  {
96  m.linear.x = t.vel[0];
97  m.linear.y = t.vel[1];
98  m.linear.z = t.vel[2];
99  m.angular.x = t.rot[0];
100  m.angular.y = t.rot[1];
101  m.angular.z = t.rot[2];
102  }
103 
104  void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &t)
105  {
106  t.vel[0] = m.linear.x;
107  t.vel[1] = m.linear.y;
108  t.vel[2] = m.linear.z;
109  t.rot[0] = m.angular.x;
110  t.rot[1] = m.angular.y;
111  t.rot[2] = m.angular.z;
112  }
113 
114  void vectorMsgToKDL(const geometry_msgs::Vector3 &m, KDL::Vector &k)
115  {
116  k[0] = m.x;
117  k[1] = m.y;
118  k[2] = m.z;
119  }
120 
121  void vectorKDLToMsg(const KDL::Vector &k, geometry_msgs::Vector3 &m)
122  {
123  m.x = k[0];
124  m.y = k[1];
125  m.z = k[2];
126  }
127 
128  void wrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &k)
129  {
130  k[0] = m.force.x;
131  k[1] = m.force.y;
132  k[2] = m.force.z;
133  k[3] = m.torque.x;
134  k[4] = m.torque.y;
135  k[5] = m.torque.z;
136  }
137 
138  void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m)
139  {
140  m.force.x = k[0];
141  m.force.y = k[1];
142  m.force.z = k[2];
143  m.torque.x = k[3];
144  m.torque.y = k[4];
145  m.torque.z = k[5];
146  }
147 
148 
150 
152 void PoseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k) { poseMsgToKDL(m, k);}
153 
155 void PoseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m) { poseKDLToMsg(k, m);}
156 
158 void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k) {twistMsgToKDL(m, k);};
159 
161 void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m){twistKDLToMsg(k, m);};
162 
163 
164 } // namespace tf
165 
void vectorKDLToMsg(const KDL::Vector &k, geometry_msgs::Vector3 &m)
Converts a KDL Vector into a Vector3 message.
Definition: kdl_msg.cpp:121
double y() const
double x() const
ROS_DEPRECATED void PoseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
Converts a KDL Frame into a Pose message.
Definition: kdl_msg.cpp:155
Vector vel
void transformKDLToMsg(const KDL::Frame &k, geometry_msgs::Transform &m)
Converts a KDL Frame into a Transform message.
Definition: kdl_msg.cpp:85
ROS_DEPRECATED void PoseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
Converts a Pose message into a KDL Frame.
Definition: kdl_msg.cpp:152
static Rotation Quaternion(double x, double y, double z, double w)
void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k)
Converts a Twist message into a KDL Twist.
Definition: kdl_msg.cpp:104
Definition: kdl_msg.h:46
Vector torque
Rotation M
void quaternionMsgToKDL(const geometry_msgs::Quaternion &m, KDL::Rotation &k)
Converts a quaternion message into a KDL Rotation.
Definition: kdl_msg.cpp:66
Vector rot
void transformMsgToKDL(const geometry_msgs::Transform &m, KDL::Frame &k)
Converts a Transform message into a KDL Frame.
Definition: kdl_msg.cpp:76
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
Converts a Pose message into a KDL Frame.
Definition: kdl_msg.cpp:48
void twistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m)
Converts a KDL Twist into a Twist message.
Definition: kdl_msg.cpp:94
Vector force
void wrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &k)
Converts a Wrench message into a KDL Wrench.
Definition: kdl_msg.cpp:128
void quaternionKDLToMsg(const KDL::Rotation &k, geometry_msgs::Quaternion &m)
Converts a KDL Rotation into a message quaternion.
Definition: kdl_msg.cpp:71
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
Converts a KDL Frame into a Pose message.
Definition: kdl_msg.cpp:57
void pointKDLToMsg(const KDL::Vector &k, geometry_msgs::Point &m)
Converts a KDL Vector into a geometry_msgs Vector3.
Definition: kdl_msg.cpp:41
void pointMsgToKDL(const geometry_msgs::Point &m, KDL::Vector &k)
Conversion functions from/to the corresponding KDL and geometry_msgs types.
Definition: kdl_msg.cpp:34
void GetQuaternion(double &x, double &y, double &z, double &w) const
double z() const
void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m)
Converts a KDL Wrench into a Wrench message.
Definition: kdl_msg.cpp:138
ROS_DEPRECATED void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k)
Converts a Twist message into a KDL Twist.
Definition: kdl_msg.cpp:158
void vectorMsgToKDL(const geometry_msgs::Vector3 &m, KDL::Vector &k)
Converts a Vector3 message into a KDL Vector.
Definition: kdl_msg.cpp:114
ROS_DEPRECATED void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m)
Converts a KDL Twist into a Twist message.
Definition: kdl_msg.cpp:161


kdl_conversions
Author(s): Adam Leeper
autogenerated on Mon Feb 28 2022 22:26:18