30 #include <gtest/gtest.h> 35 #include <geometry_msgs/TransformStamped.h> 45 for (
unsigned int i = 0; i < 1000; i++)
47 int pseudo_rand = std::floor(i * M_PI);
48 values.push_back(( pseudo_rand % 100)/50.0 - 1.0);
56 if (
values.size() == 0)
throw std::runtime_error(
"you need to call seed_rand first");
70 stor.
rotation_.setValue(0.0, 0.0, 0.0, 1.0);
75 unsigned int runs = 100;
82 for ( uint64_t i = 1; i < runs ; i++ )
90 for ( uint64_t i = 1; i < runs ; i++ )
102 unsigned int runs = 100;
109 for (
int i = runs -1; i >= 0 ; i-- )
116 for ( uint64_t i = 1; i < runs ; i++ )
126 #if 0 // jfaust: this doesn't seem to actually be testing random insertion? 133 double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
134 std::vector<double>
values (my_vals, my_vals +
sizeof(my_vals)/
sizeof(
double));
135 unsigned int runs = values.size();
139 for ( uint64_t i = 0; i <runs ; i++ )
142 std::stringstream ss;
144 stor.header.frame_id = ss.str();
150 for ( uint64_t i = 1; i < runs ; i++ )
156 std::stringstream ss;
158 EXPECT_EQ(stor.header.frame_id, ss.str());
173 for ( uint64_t i = 1; i < runs ; i++ )
185 for ( uint64_t i = 1; i < runs ; i++ )
216 std::vector<double> xvalues(2);
217 std::vector<double> yvalues(2);
218 std::vector<double> zvalues(2);
220 uint64_t offset = 200;
225 for ( uint64_t i = 1; i < runs ; i++ )
240 for (
int pos = 0; pos < 100 ; pos ++)
242 uint64_t time = offset + pos;
252 EXPECT_EQ(time, time_out);
253 EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (
double)pos/100.0, x_out, epsilon);
254 EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (
double)pos/100.0, y_out, epsilon);
255 EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (
double)pos/100.0, z_out, epsilon);
269 uint64_t offset = 555;
274 std::vector<double> xvalues(2);
275 std::vector<double> yvalues(2);
276 std::vector<double> zvalues(2);
293 for (
int pos = 0; pos < 100 ; pos ++)
299 EXPECT_NEAR(xvalues[0], x_out, epsilon);
300 EXPECT_NEAR(yvalues[0], y_out, epsilon);
301 EXPECT_NEAR(zvalues[0], z_out, epsilon);
314 for (uint64_t i = 0 ; i < runs ; i++)
323 EXPECT_NEAR(q3.
angle(q1), q2.
angle(q3), 1e-5);
336 std::vector<double> yawvalues(2);
337 std::vector<double> pitchvalues(2);
338 std::vector<double> rollvalues(2);
339 uint64_t offset = 200;
341 std::vector<tf2::Quaternion> quats(2);
346 for ( uint64_t i = 1; i < runs ; i++ )
352 pitchvalues[
step] = 0;
353 rollvalues[
step] = 0;
354 quats[
step].setRPY(yawvalues[
step], pitchvalues[step], rollvalues[step]);
361 for (
int pos = 0; pos < 100 ; pos ++)
363 uint64_t time = offset + pos;
372 EXPECT_EQ(time, time_out);
373 EXPECT_NEAR(0,
angle(ground_truth, quat), epsilon);
404 EXPECT_TRUE(!std::isnan(stor.
rotation_.x()));
405 EXPECT_TRUE(!std::isnan(stor.
rotation_.y()));
406 EXPECT_TRUE(!std::isnan(stor.
rotation_.z()));
407 EXPECT_TRUE(!std::isnan(stor.
rotation_.w()));
410 int main(
int argc,
char **argv){
411 testing::InitGoogleTest(&argc, argv);
412 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
tf2Scalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
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...
virtual bool insertData(const TransformStorage &new_data)
Insert data into the cache.
int main(int argc, char **argv)
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...
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.