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 
53  bool initialize()
54  {
55  visual_tools_.reset(new rviz_visual_tools::RvizVisualTools("base", "/rviz_visual_tools"));
56 
57  // Allow time to publish messages
58  ROS_INFO_STREAM_NAMED("test", "Waiting 4 seconds to start test...");
59  return true;
60  }
61 
62  bool testIsometry3d(const std::string& id, const Eigen::Isometry3d& expect, const Eigen::Isometry3d& actual)
63  {
64  static const double EPSILON = 0.000001;
65  EXPECT_GT(EPSILON, fabs(expect.translation().x() - actual.translation().x()))
66  << id << " Translation x - expect: " << expect.translation().x() << " actual: " << actual.translation().x();
67  EXPECT_GT(EPSILON, fabs(expect.translation().y() - actual.translation().y()))
68  << id << " Translation y - expect: " << expect.translation().y() << " actual: " << actual.translation().y();
69  EXPECT_GT(EPSILON, fabs(expect.translation().z() - actual.translation().z()))
70  << id << " Translation z - expect: " << expect.translation().z() << " actual: " << actual.translation().z();
71 
72  Eigen::Quaterniond q1(expect.rotation());
73  Eigen::Quaterniond q2(actual.rotation());
74  EXPECT_GT(EPSILON, fabs(q1.x() - q2.x())) << id << " Quaternion x - expect: " << q1.x() << " actual: " << q2.x();
75  EXPECT_GT(EPSILON, fabs(q1.y() - q2.y())) << id << " Quaternion y - expect: " << q1.y() << " actual: " << q2.y();
76  EXPECT_GT(EPSILON, fabs(q1.z() - q2.z())) << id << " Quaternion z - expect: " << q1.z() << " actual: " << q2.z();
77  EXPECT_GT(EPSILON, fabs(q1.w() - q2.w())) << id << " Quaternion w - expect: " << q1.w() << " actual: " << q2.w();
78 
79  return true;
80  }
81 
82  bool testVector(const std::string& id, const std::vector<double>& expect, const std::vector<double>& actual)
83  {
84  EXPECT_EQ(expect.size(), actual.size()) << id << " Unequal vector sizes";
85 
86  static const double EPSILON = 0.000001;
87  for (std::size_t i = 0; i < expect.size(); ++i)
88  {
89  EXPECT_GT(EPSILON, fabs(expect[i] - actual[i])) << "Section " << id << ", Element " << i
90  << ", Expect: " << expect[i] << ", Actual: " << actual[i];
91  }
92 
93  return true;
94  }
95 
96  // A shared node handle
97  // ros::NodeHandle nh_;
98 
99  // For visualizing things in rviz
100  rviz_visual_tools::RvizVisualToolsPtr visual_tools_;
101 
102 }; // class
103 
104 /* Create instance of test class ---------------------------------------------------------- */
105 RVTTest base;
106 
107 /* Run tests ------------------------------------------------------------------------------ */
108 TEST(RVTTest, initialize)
109 {
110  ASSERT_TRUE(base.initialize());
111 }
112 
113 // Test rpy conversion
114 TEST(RVTTest, test_rpy_conversions)
115 {
116  // Identity conversions with RPY
117  Eigen::Isometry3d expected_affine = Eigen::Isometry3d::Identity();
118  std::vector<double> xyzrpy;
119  base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
120  std::vector<double> expected_vector;
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  expected_vector.push_back(0);
126  expected_vector.push_back(0);
127  EXPECT_TRUE(base.testVector("Identity: ", expected_vector, xyzrpy));
128 
129  // Identity conversion back to Eigen
130  Eigen::Isometry3d expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
131  EXPECT_TRUE(base.testIsometry3d("Identity convert back", expected_affine, expected_affine2));
132 
133  // -------------------------------------------------------------------
134  // Translation conversions to RPY
135  expected_affine.translation().x() = 1;
136  expected_affine.translation().y() = 2;
137  expected_affine.translation().z() = 3;
138  base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
139  expected_vector[0] = 1;
140  expected_vector[1] = 2;
141  expected_vector[2] = 3;
142  EXPECT_TRUE(base.testVector("123: ", expected_vector, xyzrpy));
143 
144  // Translation convertion back to Eigen
145  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
146  EXPECT_TRUE(base.testIsometry3d("123 convert back", expected_affine, expected_affine2));
147 
148  // Translation convertion back to Eigen via long function
149  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
150  xyzrpy[5], rviz_visual_tools::XYZ);
151  EXPECT_TRUE(base.testIsometry3d("123 convert back long", expected_affine, expected_affine2));
152 
153  // Translation convertion back to Eigen via NEW long function
154  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
155  xyzrpy[5], rviz_visual_tools::XYZ);
156  EXPECT_TRUE(base.testIsometry3d("123 convert back new long", expected_affine, expected_affine2));
157 
158  // -------------------------------------------------------------------
159  // Rotation conversions to RPY
160  expected_affine = expected_affine * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()) *
161  Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitZ()) *
162  Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX());
163  base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
164 
165  // Rotation convertion back to Eigen
166  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy, rviz_visual_tools::XYZ);
167  EXPECT_TRUE(base.testIsometry3d("123 convert back", expected_affine, expected_affine2));
168 
169  // Rotation convertion back to Eigen via long function
170  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
171  xyzrpy[5], rviz_visual_tools::XYZ);
172  EXPECT_TRUE(base.testIsometry3d("123 convert back long", expected_affine, expected_affine2));
173 
174  // Rotation convertion back to Eigen via NEW long function
175  expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3], xyzrpy[4],
176  xyzrpy[5], rviz_visual_tools::XYZ);
177  EXPECT_TRUE(base.testIsometry3d("123 convert back new long", expected_affine, expected_affine2));
178 }
179 
180 TEST(RVTTest, default_arguments)
181 {
182  // Check for correct number of parameters and correct size of path
183  std::vector<geometry_msgs::Point> path1;
184  EXPECT_FALSE(base.visual_tools_->publishPath(path1, rviz_visual_tools::RED, rviz_visual_tools::MEDIUM));
185  path1.resize(1);
186  EXPECT_FALSE(base.visual_tools_->publishPath(path1, rviz_visual_tools::RED, rviz_visual_tools::MEDIUM));
187  path1.resize(2);
188  EXPECT_TRUE(base.visual_tools_->publishPath(path1, rviz_visual_tools::RED, rviz_visual_tools::MEDIUM));
189 
190  EigenSTL::vector_Isometry3d path2;
191  EXPECT_FALSE(base.visual_tools_->publishPath(path2, rviz_visual_tools::GREEN, rviz_visual_tools::MEDIUM));
192  path2.resize(1);
193  EXPECT_FALSE(base.visual_tools_->publishPath(path2, rviz_visual_tools::RED, rviz_visual_tools::MEDIUM));
194  path2.resize(2);
195  EXPECT_TRUE(base.visual_tools_->publishPath(path2, rviz_visual_tools::RED, rviz_visual_tools::MEDIUM));
196 
197  EigenSTL::vector_Vector3d path3;
198  EXPECT_FALSE(base.visual_tools_->publishPath(path3, rviz_visual_tools::BLUE, rviz_visual_tools::MEDIUM));
199  path3.resize(1);
200  EXPECT_FALSE(base.visual_tools_->publishPath(path3, rviz_visual_tools::RED, rviz_visual_tools::MEDIUM));
201  path3.resize(2);
202  EXPECT_TRUE(base.visual_tools_->publishPath(path3, rviz_visual_tools::RED, rviz_visual_tools::MEDIUM));
203 }
204 
205 /* Main ------------------------------------------------------------------------------------- */
206 int main(int argc, char** argv)
207 {
208  testing::InitGoogleTest(&argc, argv);
209  ros::init(argc, argv, "rviz_visual_tools_tests");
210  return RUN_ALL_TESTS();
211 }
212 
213 /*
214 reminders:
215 EXPECT_FALSE(robot_state.hasFixedLinks());
216 EXPECT_EQ(robot_state.getFixedLinksCount(), 0);
217 EXPECT_TRUE(robot_state.getPrimaryFixedLink() == NULL);
218 EXPECT_GT(robot_state.getFixedLinksMode(), 0);
219 EXPECT_LT( fabs(vars[0] - 0), EPSILON) << "Virtual joint in wrong position " << vars[0];
220 */
main
int main(int argc, char **argv)
Definition: rvt_test.cpp:206
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
RVTTest::testIsometry3d
bool testIsometry3d(const std::string &id, const Eigen::Isometry3d &expect, const Eigen::Isometry3d &actual)
Definition: rvt_test.cpp:62
base
RVTTest base
Definition: rvt_test.cpp:105
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:184
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
rviz_visual_tools::GREEN
Definition: rviz_visual_tools.h:99
rviz_visual_tools::XYZ
Definition: rviz_visual_tools.h:133
rviz_visual_tools::RvizVisualToolsPtr
std::shared_ptr< RvizVisualTools > RvizVisualToolsPtr
Definition: rviz_visual_tools.h:1112
RVTTest::initialize
bool initialize()
Definition: rvt_test.cpp:53
rviz_visual_tools::RED
Definition: rviz_visual_tools.h:104
RVTTest::visual_tools_
rviz_visual_tools::RvizVisualToolsPtr visual_tools_
Definition: rvt_test.cpp:100
rviz_visual_tools::BLUE
Definition: rviz_visual_tools.h:95
TEST
TEST(RVTTest, initialize)
Definition: rvt_test.cpp:108
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:82
rviz_visual_tools::MEDIUM
Definition: rviz_visual_tools.h:123
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d

rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Mon Feb 28 2022 23:43:06