MRPT  1.9.9
FrameTransformer_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <gtest/gtest.h>
11 #include <mrpt/poses/CPose2D.h>
12 #include <mrpt/poses/CPose3D.h>
14 #include <Eigen/Dense>
15 
16 template <int DIM>
18 {
19  using namespace mrpt::poses;
20 
22 
23  // Pub:
24  const auto real_A2B = (typename FrameTransformer<DIM>::pose_t)(A2B_);
25  tf.sendTransform("A", "B", real_A2B);
26 
27  // Read:
28  {
29  typename FrameTransformer<DIM>::pose_t A2B_looked_up;
30  const auto ret = tf.lookupTransform("B", "A", A2B_looked_up);
31  EXPECT_EQ(ret, mrpt::poses::LKUP_GOOD);
32  EXPECT_NEAR(
33  .0,
34  (real_A2B.asVectorVal() - A2B_looked_up.asVectorVal())
35  .array()
36  .abs()
37  .sum(),
38  1e-6);
39  }
40 }
41 
42 TEST(FrameTransformer, SimplePublishAndLookup)
43 {
44  const mrpt::poses::CPose2D test_A2B(5.0, 2.0, 0.3);
45 
46  run_tf_test1<2>(test_A2B);
47  run_tf_test1<3>(test_A2B);
48 }
TEST(FrameTransformer, SimplePublishAndLookup)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
See docs in FrameTransformerInterface.
void sendTransform(const std::string &parent_frame, const std::string &child_frame, const typename base_t::pose_t &child_wrt_parent, const mrpt::system::TTimeStamp &timestamp=mrpt::system::now()) override
void run_tf_test1(const mrpt::poses::CPose2D &A2B_)
FrameLookUpStatus lookupTransform(const std::string &target_frame, const std::string &source_frame, typename base_t::light_type &child_wrt_parent, const mrpt::system::TTimeStamp query_time=INVALID_TIMESTAMP, const double timeout_secs=.0) override
Queries the current pose of target_frame wrt ("as seen from") source_frame.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019