laser_joint_processor_unittest.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
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 the Willow Garage 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 #include <gtest/gtest.h>
00036 #include <laser_joint_processor/laser_joint_processor.h>
00037 
00038 using namespace std;
00039 using namespace laser_joint_processor;
00040 
00041 static const float eps = 1e-6;
00042 
00043 class LaserJointProcessor_EasyTests : public ::testing::Test
00044 {
00045 protected:
00046   virtual void SetUp()
00047   {
00048     bool success;
00049 
00050     // Set up joints of interest
00051     vector<string> names;
00052     names.push_back("JointA");
00053     names.push_back("JointB");
00054     processor_.setJointNames(names);
00055     processor_.setCacheSize(1000);
00056 
00057     // Populate the cache
00058     for (unsigned int i=0; i<20; i++)
00059     {
00060       sensor_msgs::JointStatePtr joint_state(new sensor_msgs::JointState);
00061       joint_state->header.stamp = ros::Time(100+i, 0);
00062       joint_state->name.push_back("JointA");
00063       joint_state->name.push_back("JointB");
00064       joint_state->name.push_back("JointC");
00065       joint_state->position.push_back(i*1.0);
00066       joint_state->position.push_back(i*10.0);
00067       joint_state->position.push_back(i*20.0);
00068       joint_state->velocity.resize(3);
00069       joint_state->effort.resize(3);
00070 
00071       success = processor_.addJointState(joint_state);
00072       ASSERT_TRUE(success);
00073     }
00074 
00075     // Build The Snapshot
00076     snapshot_.angle_min = 10.0;
00077     snapshot_.angle_increment = 1.0;
00078     snapshot_.time_increment  = .5;
00079     snapshot_.readings_per_scan = 4;
00080     snapshot_.num_scans = 3;
00081     snapshot_.ranges.resize(12);
00082     for (unsigned int i=0; i<snapshot_.ranges.size(); i++)
00083       snapshot_.ranges[i] = i*100;
00084     snapshot_.scan_start.resize(snapshot_.num_scans);
00085     snapshot_.scan_start[0] = ros::Time(105);
00086     snapshot_.scan_start[1] = ros::Time(110);
00087     snapshot_.scan_start[2] = ros::Time(115.1);
00088 
00089     // Build Feature Vector
00090     features_.image_points.resize(P);
00091 
00092     // Boundaries
00093     features_.image_points[0] = Point(0,0);
00094     features_.image_points[1] = Point(3,0);
00095     features_.image_points[2] = Point(0,2);
00096     features_.image_points[3] = Point(3,2);
00097 
00098     features_.image_points[4] = Point(2.5, .5);
00099     features_.image_points[5] = Point(2,    1);
00100     features_.image_points[6] = Point(1.25,  1.5);
00101   }
00102 
00103   void testProcessing()
00104   {
00105     bool success;
00106     // Run the processing step
00107     success = processor_.processLaserData(snapshot_, features_, result_, ros::Duration(2,0));
00108     EXPECT_TRUE(success);
00109   }
00110 
00111   geometry_msgs::Point Point(float x, float y)
00112   {
00113     geometry_msgs::Point point;
00114     point.x = x;
00115     point.y = y;
00116     return point;
00117   }
00118 
00119   static const unsigned int P = 7;
00120   calibration_msgs::JointStateCalibrationPattern result_;
00121   calibration_msgs::CalibrationPattern features_;
00122   calibration_msgs::DenseLaserSnapshot snapshot_;
00123   LaserJointProcessor processor_;
00124 };
00125 
00126 TEST_F(LaserJointProcessor_EasyTests, earlyTest)
00127 {
00128   EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
00129   EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
00130   snapshot_.scan_start[2] = ros::Time(300);
00131   EXPECT_TRUE(processor_.isSnapshotEarly(snapshot_));
00132   EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
00133 }
00134 
00135 TEST_F(LaserJointProcessor_EasyTests, lateTest)
00136 {
00137   EXPECT_FALSE(processor_.isSnapshotLate(snapshot_));
00138   EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
00139   snapshot_.scan_start[0] = ros::Time(10);
00140   EXPECT_TRUE(processor_.isSnapshotLate(snapshot_));
00141   EXPECT_FALSE(processor_.isSnapshotEarly(snapshot_));
00142 }
00143 
00144 /*
00145  * Joint 1:
00146  *  5.00   5.50   6.00   6.50
00147  * 10.00  10.50  11.00  11.50
00148  * 15.10  15.60  16.10  16.60
00149  *
00150  * Joint 2:
00151  *  50.00   55.00   60.00   65.00
00152  * 100.00  105.00  110.00  115.00
00153  * 151.00  156.00  161.00  166.00
00154  */
00155 
00156 TEST_F(LaserJointProcessor_EasyTests, jointTest)
00157 {
00158   testProcessing();
00159 
00160   ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
00161   for (unsigned int i=0; i<result_.joint_points.size(); i++)
00162   {
00163     ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
00164     ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
00165     EXPECT_EQ(result_.joint_points[i].name[0], "JointA");
00166     EXPECT_EQ(result_.joint_points[i].name[1], "JointB");
00167   }
00168 
00169   // point #0 = (0,0)
00170   // t=105
00171   EXPECT_NEAR(result_.joint_points[0].position[0],  5.0, eps);
00172   EXPECT_NEAR(result_.joint_points[0].position[1], 50.0, eps);
00173 
00174   // point #1 = (3,0)
00175   // t=1.5
00176   EXPECT_NEAR(result_.joint_points[1].position[0],  6.5, eps);
00177   EXPECT_NEAR(result_.joint_points[1].position[1],   65, eps);
00178 
00179   // point #2 = (0,2)
00180   // t=115.1
00181   EXPECT_NEAR(result_.joint_points[2].position[0],  15.1, eps);
00182   EXPECT_NEAR(result_.joint_points[2].position[1],   151, eps);
00183 
00184   // point #3 = (3,2)
00185   // t=116.6
00186   EXPECT_NEAR(result_.joint_points[3].position[0],  16.6, eps);
00187   EXPECT_NEAR(result_.joint_points[3].position[1],   166, eps);
00188 
00189   // point #4 = (2.5, .5)
00190   EXPECT_NEAR(result_.joint_points[4].position[0],  8.75, eps);
00191   EXPECT_NEAR(result_.joint_points[4].position[1],  87.5, eps);
00192 
00193   // point #5 = (2, 1)
00194   EXPECT_NEAR(result_.joint_points[5].position[0],   11, eps);
00195   EXPECT_NEAR(result_.joint_points[5].position[1],  110, eps);
00196 
00197   // point #6 = (1.25, 1.5)
00198   EXPECT_NEAR(result_.joint_points[6].position[0],  13.175, eps);
00199   EXPECT_NEAR(result_.joint_points[6].position[1],  131.75, eps);
00200 }
00201 
00202 TEST_F(LaserJointProcessor_EasyTests, anglesTest)
00203 {
00204   testProcessing();
00205 
00206   ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
00207   for (unsigned int i=0; i<P; i++)
00208   {
00209     ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
00210     ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
00211     EXPECT_EQ(result_.joint_points[i].name[2], "laser_angle_joint");
00212   }
00213 
00214   EXPECT_NEAR(result_.joint_points[0].position[2], 10.0, eps);
00215   EXPECT_NEAR(result_.joint_points[1].position[2], 13.0, eps);
00216   EXPECT_NEAR(result_.joint_points[2].position[2], 10.0, eps);
00217   EXPECT_NEAR(result_.joint_points[3].position[2], 13.0, eps);
00218   EXPECT_NEAR(result_.joint_points[4].position[2], 12.5, eps);
00219   EXPECT_NEAR(result_.joint_points[5].position[2], 12.0, eps);
00220   EXPECT_NEAR(result_.joint_points[6].position[2], 11.25, eps);
00221 }
00222 
00223 TEST_F(LaserJointProcessor_EasyTests, rangesTest)
00224 {
00225   testProcessing();
00226 
00227   ASSERT_EQ(result_.joint_points.size(), (unsigned int) P);
00228   for (unsigned int i=0; i<P; i++)
00229   {
00230     ASSERT_EQ(result_.joint_points[i].name.size(), (unsigned int) 4);
00231     ASSERT_EQ(result_.joint_points[i].position.size(), (unsigned int) 4);
00232     EXPECT_EQ(result_.joint_points[i].name[3], "laser_range_joint");
00233   }
00234 
00235   EXPECT_NEAR(result_.joint_points[0].position[3],    0.0, eps);
00236   EXPECT_NEAR(result_.joint_points[1].position[3],  300.0, eps);
00237   EXPECT_NEAR(result_.joint_points[2].position[3],  800.0, eps);
00238   EXPECT_NEAR(result_.joint_points[3].position[3], 1100.0, eps);
00239   EXPECT_NEAR(result_.joint_points[4].position[3],  450.0, eps);
00240   EXPECT_NEAR(result_.joint_points[5].position[3],  600.0, eps);
00241   EXPECT_NEAR(result_.joint_points[6].position[3],  725.0, eps);
00242 }
00243 
00244 int main(int argc, char **argv){
00245   testing::InitGoogleTest(&argc, argv);
00246   return RUN_ALL_TESTS();
00247 }


laser_joint_processor
Author(s): Vijay Pradeep
autogenerated on Fri Aug 28 2015 12:06:26