rvt_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, PickNik Consulting
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of PickNik Consulting nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman
36  Desc: Test for Rviz Visual tools
37 */
38 
39 // C++
40 #include <string>
41 #include <vector>
42 
43 #include <gtest/gtest.h>
44 #include <ros/ros.h>
45 
46 // For visualizing things in rviz
47 #include <rviz_visual_tools/rviz_visual_tools.h>
48 
49 class RVTTest
50 {
51 public:
52  // A shared node handle
53  // ros::NodeHandle nh_;
54 
55  // For visualizing things in rviz
56  rviz_visual_tools::RvizVisualToolsPtr visual_tools_;
57 
58  bool initialize()
59  {
60  visual_tools_.reset(new rviz_visual_tools::RvizVisualTools("base", "/rviz_visual_tools"));
61 
62  // Allow time to publish messages
63  ROS_INFO_STREAM_NAMED("test", "Waiting 4 seconds to start test...");
64  return true;
65  }
66 
67  bool testAffine3d(const std::string& id, const Eigen::Affine3d& expect, const Eigen::Affine3d& actual)
68  {
69  static const double EPSILON = 0.000001;
70  EXPECT_GT(EPSILON, fabs(expect.translation().x() - actual.translation().x()))
71  << id << " Translation x - expect: " << expect.translation().x() << " actual: " << actual.translation().x();
72  EXPECT_GT(EPSILON, fabs(expect.translation().y() - actual.translation().y()))
73  << id << " Translation y - expect: " << expect.translation().y() << " actual: " << actual.translation().y();
74  EXPECT_GT(EPSILON, fabs(expect.translation().z() - actual.translation().z()))
75  << id << " Translation z - expect: " << expect.translation().z() << " actual: " << actual.translation().z();
76 
77  Eigen::Quaterniond q1(expect.rotation());
78  Eigen::Quaterniond q2(actual.rotation());
79  EXPECT_GT(EPSILON, fabs(q1.x() - q2.x())) << id << " Quaternion x - expect: " << q1.x() << " actual: " << q2.x();
80  EXPECT_GT(EPSILON, fabs(q1.y() - q2.y())) << id << " Quaternion y - expect: " << q1.y() << " actual: " << q2.y();
81  EXPECT_GT(EPSILON, fabs(q1.z() - q2.z())) << id << " Quaternion z - expect: " << q1.z() << " actual: " << q2.z();
82  EXPECT_GT(EPSILON, fabs(q1.w() - q2.w())) << id << " Quaternion w - expect: " << q1.w() << " actual: " << q2.w();
83 
84  return true;
85  }
86 
87  bool testVector(const std::string& id, const std::vector<double>& expect, const std::vector<double>& actual)
88  {
89  EXPECT_EQ(expect.size(), actual.size()) << id << " Unequal vector sizes";
90 
91  static const double EPSILON = 0.000001;
92  for (std::size_t i = 0; i < expect.size(); ++i)
93  {
94  EXPECT_GT(EPSILON, fabs(expect[i] - actual[i])) << "Section " << id << ", Element " << i
95  << ", Expect: " << expect[i] << ", Actual: " << actual[i];
96  }
97 
98  return true;
99  }
100 }; // class
101 
102 /* Create instance of test class ---------------------------------------------------------- */
103 RVTTest base;
104 
105 /* Run tests ------------------------------------------------------------------------------ */
106 TEST(RVTTest, initialize)
107 {
108  ASSERT_TRUE(base.initialize());
109 }
110 
111 // Test rpy conversion
112 TEST(RVTTest, test_rpy_conversions)
113 {
114  // Identity conversions with RPY
115  Eigen::Affine3d expected_affine = Eigen::Affine3d::Identity();
116  std::vector<double> xyzrpy;
117  base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
118  std::vector<double> expected_vector;
119  expected_vector.push_back(0);
120  expected_vector.push_back(0);
121  expected_vector.push_back(0);
122  expected_vector.push_back(0);
123  expected_vector.push_back(0);
124  expected_vector.push_back(0);
125  EXPECT_TRUE(base.testVector("Identity: ", expected_vector, xyzrpy));
126 
127  // Identity conversion back to Eigen
128  Eigen::Affine3d expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
129  EXPECT_TRUE(base.testAffine3d("Identity convert back", expected_affine, expected_affine2));
130 
131  // -------------------------------------------------------------------
132  // Translation conversions to RPY
133  expected_affine.translation().x() = 1;
134  expected_affine.translation().y() = 2;
135  expected_affine.translation().z() = 3;
136  base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
137  expected_vector[0] = 1;
138  expected_vector[1] = 2;
139  expected_vector[2] = 3;
140  EXPECT_TRUE(base.testVector("123: ", expected_vector, xyzrpy));
141 
142  // Translation convertion back to Eigen
143  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
144  EXPECT_TRUE(base.testAffine3d("123 convert back", expected_affine, expected_affine2));
145 
146  // Translation convertion back to Eigen via long function
147  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
148  xyzrpy[5], rviz_visual_tools::XYZ);
149  EXPECT_TRUE(base.testAffine3d("123 convert back long", expected_affine, expected_affine2));
150 
151  // Translation convertion back to Eigen via NEW long function
152  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
153  xyzrpy[5], rviz_visual_tools::XYZ);
154  EXPECT_TRUE(base.testAffine3d("123 convert back new long", expected_affine, expected_affine2));
155 
156  // -------------------------------------------------------------------
157  // Rotation conversions to RPY
158  expected_affine = expected_affine * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()) *
159  Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitZ()) *
160  Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX());
161  base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
162 
163  // Rotation convertion back to Eigen
164  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
165  EXPECT_TRUE(base.testAffine3d("123 convert back", expected_affine, expected_affine2));
166 
167  // Rotation convertion back to Eigen via long function
168  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
169  xyzrpy[5], rviz_visual_tools::XYZ);
170  EXPECT_TRUE(base.testAffine3d("123 convert back long", expected_affine, expected_affine2));
171 
172  // Rotation convertion back to Eigen via NEW long function
173  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
174  xyzrpy[5], rviz_visual_tools::XYZ);
175  EXPECT_TRUE(base.testAffine3d("123 convert back new long", expected_affine, expected_affine2));
176 }
177 
178 /* Main ------------------------------------------------------------------------------------- */
179 int main(int argc, char** argv)
180 {
181  testing::InitGoogleTest(&argc, argv);
182  ros::init(argc, argv, "rviz_visual_tools_tests");
183  return RUN_ALL_TESTS();
184 }
185 
186 /*
187 reminders:
188 EXPECT_FALSE(robot_state.hasFixedLinks());
189 EXPECT_EQ(robot_state.getFixedLinksCount(), 0);
190 EXPECT_TRUE(robot_state.getPrimaryFixedLink() == NULL);
191 EXPECT_GT(robot_state.getFixedLinksMode(), 0);
192 EXPECT_LT( fabs(vars[0] - 0), EPSILON) << "Virtual joint in wrong position " << vars[0];
193 */
main
int main(int argc, char **argv)
Definition: rvt_test.cpp:179
base
RVTTest base
Definition: rvt_test.cpp:103
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
rviz_visual_tools::RvizVisualTools
Definition: rviz_visual_tools.h:173
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
rviz_visual_tools::XYZ
Definition: rviz_visual_tools.h:122
rviz_visual_tools::RvizVisualToolsPtr
std::shared_ptr< RvizVisualTools > RvizVisualToolsPtr
Definition: rviz_visual_tools.h:1129
RVTTest::initialize
bool initialize()
Definition: rvt_test.cpp:58
RVTTest::visual_tools_
rviz_visual_tools::RvizVisualToolsPtr visual_tools_
Definition: rvt_test.cpp:56
TEST
TEST(RVTTest, initialize)
Definition: rvt_test.cpp:106
rviz_visual_tools.h
RVTTest::testVector
bool testVector(const std::string &id, const std::vector< double > &expect, const std::vector< double > &actual)
Definition: rvt_test.cpp:87
RVTTest::testAffine3d
bool testAffine3d(const std::string &id, const Eigen::Affine3d &expect, const Eigen::Affine3d &actual)
Definition: rvt_test.cpp:67

rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Mon Feb 25 2019 03:54:12