rvt_test.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, PickNik LLC
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PickNik LLC nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036    Desc:   Test for Rviz Visual tools
00037 */
00038 
00039 // C++
00040 #include <string>
00041 #include <vector>
00042 
00043 #include <ros/ros.h>
00044 #include <gtest/gtest.h>
00045 
00046 // For visualizing things in rviz
00047 #include <rviz_visual_tools/rviz_visual_tools.h>
00048 
00049 class RVTTest
00050 {
00051 public:
00052   // A shared node handle
00053   // ros::NodeHandle nh_;
00054 
00055   // For visualizing things in rviz
00056   rviz_visual_tools::RvizVisualToolsPtr visual_tools_;
00057 
00058   bool initialize()
00059   {
00060     visual_tools_.reset(new rviz_visual_tools::RvizVisualTools("base", "/rviz_visual_tools"));
00061 
00062     // Allow time to publish messages
00063     ROS_INFO_STREAM_NAMED("test", "Waiting 4 seconds to start test...");
00064     return true;
00065   }
00066 
00067   bool testAffine3d(const std::string& id, const Eigen::Affine3d& expect, const Eigen::Affine3d& actual)
00068   {
00069     static const double EPSILON = 0.000001;
00070     EXPECT_GT(EPSILON, fabs(expect.translation().x() - actual.translation().x()))
00071         << id << " Translation x - expect: " << expect.translation().x() << " actual: " << actual.translation().x();
00072     EXPECT_GT(EPSILON, fabs(expect.translation().y() - actual.translation().y()))
00073         << id << " Translation y - expect: " << expect.translation().y() << " actual: " << actual.translation().y();
00074     EXPECT_GT(EPSILON, fabs(expect.translation().z() - actual.translation().z()))
00075         << id << " Translation z - expect: " << expect.translation().z() << " actual: " << actual.translation().z();
00076 
00077     Eigen::Quaterniond q1(expect.rotation());
00078     Eigen::Quaterniond q2(actual.rotation());
00079     EXPECT_GT(EPSILON, fabs(q1.x() - q2.x())) << id << " Quaternion x - expect: " << q1.x() << " actual: " << q2.x();
00080     EXPECT_GT(EPSILON, fabs(q1.y() - q2.y())) << id << " Quaternion y - expect: " << q1.y() << " actual: " << q2.y();
00081     EXPECT_GT(EPSILON, fabs(q1.z() - q2.z())) << id << " Quaternion z - expect: " << q1.z() << " actual: " << q2.z();
00082     EXPECT_GT(EPSILON, fabs(q1.w() - q2.w())) << id << " Quaternion w - expect: " << q1.w() << " actual: " << q2.w();
00083 
00084     return true;
00085   }
00086 
00087   bool testVector(const std::string& id, const std::vector<double>& expect, const std::vector<double>& actual)
00088   {
00089     EXPECT_EQ(expect.size(), actual.size()) << id << " Unequal vector sizes";
00090 
00091     static const double EPSILON = 0.000001;
00092     for (std::size_t i = 0; i < expect.size(); ++i)
00093     {
00094       EXPECT_GT(EPSILON, fabs(expect[i] - actual[i])) << "Section " << id << ", Element " << i
00095                                                       << ", Expect: " << expect[i] << ", Actual: " << actual[i];
00096     }
00097 
00098     return true;
00099   }
00100 };  // class
00101 
00102 /* Create instance of test class ---------------------------------------------------------- */
00103 RVTTest base;
00104 
00105 /* Run tests ------------------------------------------------------------------------------ */
00106 TEST(RVTTest, initialize) { ASSERT_TRUE(base.initialize()); }
00107 
00108 // Test rpy conversion
00109 TEST(RVTTest, test_rpy_conversions)
00110 {
00111   // Identity conversions with RPY
00112   Eigen::Affine3d expected_affine = Eigen::Affine3d::Identity();
00113   std::vector<double> xyzrpy;
00114   base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
00115   std::vector<double> expected_vector;
00116   expected_vector.push_back(0);
00117   expected_vector.push_back(0);
00118   expected_vector.push_back(0);
00119   expected_vector.push_back(0);
00120   expected_vector.push_back(0);
00121   expected_vector.push_back(0);
00122   EXPECT_TRUE(base.testVector("Identity: ", expected_vector, xyzrpy));
00123 
00124   // Identity conversion back to Eigen
00125   Eigen::Affine3d expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy);
00126   EXPECT_TRUE(base.testAffine3d("Identity convert back", expected_affine, expected_affine2));
00127 
00128   // Translation conversions to RPY
00129   expected_affine.translation().x() = 1;
00130   expected_affine.translation().y() = 2;
00131   expected_affine.translation().z() = 3;
00132   base.visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
00133   expected_vector[0] = 1;
00134   expected_vector[1] = 2;
00135   expected_vector[2] = 3;
00136   EXPECT_TRUE(base.testVector("123: ", expected_vector, xyzrpy));
00137 
00138   // Translation convertion back to Eigen
00139   expected_affine2 = base.visual_tools_->convertFromXYZRPY(xyzrpy);
00140   EXPECT_TRUE(base.testAffine3d("123 convert back", expected_affine, expected_affine2));
00141 
00142   // Rotation conversions to RPY
00143   expected_vector[3] = 1;
00144   expected_vector[4] = 1.5;
00145   expected_vector[5] = 2;
00146 
00147   expected_affine2 = base.visual_tools_->convertFromXYZRPY(expected_vector);
00148   base.visual_tools_->convertToXYZRPY(expected_affine2, xyzrpy);
00149   EXPECT_TRUE(base.testVector("Rotation", expected_vector, xyzrpy));
00150 }
00151 
00152 /* Main  ------------------------------------------------------------------------------------- */
00153 int main(int argc, char** argv)
00154 {
00155   testing::InitGoogleTest(&argc, argv);
00156   ros::init(argc, argv, "rviz_visual_tools_tests");
00157   return RUN_ALL_TESTS();
00158 }
00159 
00160 /*
00161 reminders:
00162 EXPECT_FALSE(robot_state.hasFixedLinks());
00163 EXPECT_EQ(robot_state.getFixedLinksCount(), 0);
00164 EXPECT_TRUE(robot_state.getPrimaryFixedLink() == NULL);
00165 EXPECT_GT(robot_state.getFixedLinksMode(), 0);
00166 EXPECT_LT( fabs(vars[0] - 0), EPSILON) << "Virtual joint in wrong position " << vars[0];
00167 */


rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Thu Jun 6 2019 19:01:23