1 #include <gtest/gtest.h> 20 for (
double t = 0.1; t <= 6; t += 0.1)
23 else if (t < 2) y += 1;
24 else if (t < 3) x -= 1;
25 else if (t < 4) y -= 1;
26 else if (t < 5) z += 1;
35 tw_.header.frame_id =
"bar";
36 tw_.twist.linear.x = 1;
37 tw_.twist.linear.y = 2;
38 tw_.twist.linear.z = 3;
39 tw_.twist.angular.x = 0;
40 tw_.twist.angular.y = 0;
41 tw_.twist.angular.z = 0;
66 geometry_msgs::TwistStamped
tw_;
80 for (
double t = 0.1; t < 6; t += 0.1)
83 else if (t < 2) x -= .1;
84 else if (t < 3) y += .1;
85 else if (t < 4) y -= .1;
86 else if (t < 5) z += .1;
96 tw_x.header.frame_id =
"bar";
97 tw_x.twist.angular.x = 1;
100 tw_y.header.frame_id =
"bar";
101 tw_y.twist.angular.y = 1;
104 tw_z.header.frame_id =
"bar";
105 tw_z.twist.angular.z = 1;
128 geometry_msgs::TwistStamped tw_x, tw_y,
tw_z;
133 geometry_msgs::TwistStamped tw;
134 geometry_msgs::TwistStamped tw_in =tw_;
136 std::vector<std::string> offset_frames;
138 offset_frames.push_back(
"foo");
139 offset_frames.push_back(
"stationary_offset_child");
140 offset_frames.push_back(
"stationary_offset_parent");
143 for (std::vector<std::string>::iterator it = offset_frames.begin(); it != offset_frames.end(); ++it)
148 tf_.transformTwist(*it, tw_in, tw);
149 EXPECT_FLOAT_EQ(11.0, tw.twist.linear.x);
150 EXPECT_FLOAT_EQ(2.0 , tw.twist.linear.y);
151 EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
152 EXPECT_FLOAT_EQ(0.0, tw.twist.angular.x);
153 EXPECT_FLOAT_EQ(0.0, tw.twist.angular.y);
154 EXPECT_FLOAT_EQ(0.0, tw.twist.angular.z);
157 tf_.transformTwist(*it, tw_in, tw);
158 EXPECT_FLOAT_EQ(1.0, tw.twist.linear.x);
159 EXPECT_FLOAT_EQ(12.0, tw.twist.linear.y);
160 EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
161 EXPECT_FLOAT_EQ(0.0, tw.twist.angular.x);
162 EXPECT_FLOAT_EQ(0.0, tw.twist.angular.y);
163 EXPECT_FLOAT_EQ(0.0, tw.twist.angular.z);
166 tf_.transformTwist(*it, tw_in, tw);
167 EXPECT_FLOAT_EQ(-9.0, tw.twist.linear.x);
168 EXPECT_FLOAT_EQ(2.0, tw.twist.linear.y);
169 EXPECT_FLOAT_EQ(3.0, tw.twist.linear.z);
170 EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
171 EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
172 EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
175 tf_.transformTwist(*it, tw_in, tw);
176 EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
177 EXPECT_FLOAT_EQ(tw.twist.linear.y, -8.0);
178 EXPECT_FLOAT_EQ(tw.twist.linear.z, 3.0);
179 EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
180 EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
181 EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
184 tf_.transformTwist(*it, tw_in, tw);
185 EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
186 EXPECT_FLOAT_EQ(tw.twist.linear.y, 2.0);
187 EXPECT_FLOAT_EQ(tw.twist.linear.z, 13.0);
188 EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
189 EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
190 EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
193 tf_.transformTwist(*it, tw_in, tw);
194 EXPECT_FLOAT_EQ(tw.twist.linear.x, 1.0);
195 EXPECT_FLOAT_EQ(tw.twist.linear.y, 2.0);
196 EXPECT_FLOAT_EQ(tw.twist.linear.z, -7.0);
197 EXPECT_FLOAT_EQ(tw.twist.angular.x, 0.0);
198 EXPECT_FLOAT_EQ(tw.twist.angular.y, 0.0);
199 EXPECT_FLOAT_EQ(tw.twist.angular.z, 0.0);
203 EXPECT_STREQ(
"", ex.what());
211 geometry_msgs::TwistStamped tw;
212 geometry_msgs::TwistStamped tw_in;
219 tf_.transformTwist(
"foo", tw_in, tw);
220 EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
221 EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
222 EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
223 EXPECT_NEAR(tw.twist.angular.x, 2.0, epsilon);
224 EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
225 EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
230 tf_.transformTwist(
"foo", tw_in, tw);
231 EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
232 EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
233 EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
234 EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
235 EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
236 EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
241 tf_.transformTwist(
"foo", tw_in, tw);
242 EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
243 EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
244 EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
245 EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
246 EXPECT_NEAR(tw.twist.angular.y, 2.0, epsilon);
247 EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
252 tf_.transformTwist(
"foo", tw_in, tw);
253 EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
254 EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
255 EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
256 EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
257 EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
258 EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
263 tf_.transformTwist(
"foo", tw_in, tw);
264 EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
265 EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
266 EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
267 EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
268 EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
269 EXPECT_NEAR(tw.twist.angular.z, 2.0, epsilon);
274 tf_.transformTwist(
"foo", tw_in, tw);
275 EXPECT_NEAR(tw.twist.linear.x, 0.0, epsilon);
276 EXPECT_NEAR(tw.twist.linear.y, 0.0, epsilon);
277 EXPECT_NEAR(tw.twist.linear.z, 0.0, epsilon);
278 EXPECT_NEAR(tw.twist.angular.x, 0.0, epsilon);
279 EXPECT_NEAR(tw.twist.angular.y, 0.0, epsilon);
280 EXPECT_NEAR(tw.twist.angular.z, 0.0, epsilon);
284 EXPECT_STREQ(
"", ex.what());
409 int main(
int argc,
char **argv){
410 testing::InitGoogleTest(&argc, argv);
411 ros::init(argc, argv,
"testing_transform_twist");
412 return RUN_ALL_TESTS();
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
construct a Quaternion from Fixed angles
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...