30 #include <gtest/gtest.h> 36 #include <geometry_msgs/TransformStamped.h> 46 for (
unsigned int i = 0; i < 1000; i++)
48 int pseudo_rand = std::floor(i * M_PI);
49 values.push_back(( pseudo_rand % 100)/50.0 - 1.0);
57 if (
values.size() == 0)
throw std::runtime_error(
"you need to call seed_rand first");
71 stor.
rotation_.setValue(0.0, 0.0, 0.0, 1.0);
76 unsigned int runs = 100;
83 for ( uint64_t i = 1; i < runs ; i++ )
91 for ( uint64_t i = 1; i < runs ; i++ )
103 unsigned int runs = 100;
110 for (
int i = runs -1; i >= 0 ; i-- )
117 for ( uint64_t i = 1; i < runs ; i++ )
127 #if 0 // jfaust: this doesn't seem to actually be testing random insertion? 134 double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
135 std::vector<double>
values (my_vals, my_vals +
sizeof(my_vals)/
sizeof(
double));
136 unsigned int runs = values.size();
140 for ( uint64_t i = 0; i <runs ; i++ )
143 std::stringstream ss;
145 stor.header.frame_id = ss.str();
151 for ( uint64_t i = 1; i < runs ; i++ )
157 std::stringstream ss;
159 EXPECT_EQ(stor.header.frame_id, ss.str());
174 for ( uint64_t i = 1; i < runs ; i++ )
186 for ( uint64_t i = 1; i < runs ; i++ )
217 std::vector<double> xvalues(2);
218 std::vector<double> yvalues(2);
219 std::vector<double> zvalues(2);
221 uint64_t offset = 200;
226 for ( uint64_t i = 1; i < runs ; i++ )
241 for (
int pos = 0; pos < 100 ; pos ++)
243 uint64_t time = offset + pos;
253 EXPECT_EQ(time, time_out);
254 EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (
double)pos/100.0, x_out, epsilon);
255 EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (
double)pos/100.0, y_out, epsilon);
256 EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (
double)pos/100.0, z_out, epsilon);
270 uint64_t offset = 555;
275 std::vector<double> xvalues(2);
276 std::vector<double> yvalues(2);
277 std::vector<double> zvalues(2);
294 for (
int pos = 0; pos < 100 ; pos ++)
300 EXPECT_NEAR(xvalues[0], x_out, epsilon);
301 EXPECT_NEAR(yvalues[0], y_out, epsilon);
302 EXPECT_NEAR(zvalues[0], z_out, epsilon);
315 for (uint64_t i = 0 ; i < runs ; i++)
324 EXPECT_NEAR(q3.
angle(q1), q2.
angle(q3), 1e-5);
337 std::vector<double> yawvalues(2);
338 std::vector<double> pitchvalues(2);
339 std::vector<double> rollvalues(2);
340 uint64_t offset = 200;
342 std::vector<tf2::Quaternion> quats(2);
347 for ( uint64_t i = 1; i < runs ; i++ )
353 pitchvalues[
step] = 0;
354 rollvalues[
step] = 0;
355 quats[
step].setRPY(yawvalues[
step], pitchvalues[step], rollvalues[step]);
362 for (
int pos = 0; pos < 100 ; pos ++)
364 uint64_t time = offset + pos;
373 EXPECT_EQ(time, time_out);
374 EXPECT_NEAR(0,
angle(ground_truth, quat), epsilon);
405 EXPECT_TRUE(!std::isnan(stor.
rotation_.x()));
406 EXPECT_TRUE(!std::isnan(stor.
rotation_.y()));
407 EXPECT_TRUE(!std::isnan(stor.
rotation_.z()));
408 EXPECT_TRUE(!std::isnan(stor.
rotation_.w()));
411 int main(
int argc,
char **argv){
412 testing::InitGoogleTest(&argc, argv);
413 return RUN_ALL_TESTS();
void setIdentity(geometry_msgs::Transform &tx)
TEST(TimeCache, Repeatability)
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
Return the ***half*** angle between two quaternions.
Time & fromNSec(uint64_t t)
std::vector< double > values
virtual bool insertData(const TransformStorage &new_data)
Insert data into the cache.
int main(int argc, char **argv)
Quaternion slerp(const Quaternion &q, const tf2Scalar &t) const
Return the quaternion which is the result of Spherical Linear Interpolation between this and the othe...
void setEuler(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
Set the quaternion using Euler angles.
virtual void clearList()
Clear the list of stored values.
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
tf2Scalar angle(const Quaternion &q) const
Return the ***half*** angle between this quaternion and the other.
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
TF2SIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tf2Scalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
virtual bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)
Virtual methods.