joint_states_settler: joint_states_settler: joint_states_deflater

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 "joint_states_settler/joint_states_deflater.h"
00037 
00038 using namespace std;
00039 using namespace joint_states_settler;
00040 using namespace sensor_msgs;
00041 
00042 static const double eps = 1e-10;
00043 
00044 TEST(JointStatesDeflator, easy1)
00045 {
00046   JointStatesDeflater deflater;
00047 
00048   vector<string> joint_names;
00049   joint_names.resize(2);
00050   joint_names[0] = "A";
00051   joint_names[1] = "C";
00052   deflater.setDeflationJointNames(joint_names);
00053 
00054   JointStatePtr joint_states;
00055   DeflatedJointStates deflated_msg;
00056   JointState pruned;
00057 
00058   // ***********************************************
00059 
00060   joint_states.reset(new JointState);
00061   joint_states->name.resize(3);
00062   joint_states->position.resize(3);
00063   joint_states->velocity.resize(3);
00064   joint_states->effort.resize(3);
00065   joint_states->name[0] = "A";
00066   joint_states->name[1] = "B";
00067   joint_states->name[2] = "C";
00068   joint_states->position[0] = 1.0;
00069   joint_states->position[1] = 2.0;
00070   joint_states->position[2] = 3.0;
00071 
00072   deflater.deflate(joint_states, deflated_msg);
00073   deflater.prune(*joint_states, pruned);
00074 
00075   ASSERT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
00076   EXPECT_NEAR(deflated_msg.channels_[0], 1.0, eps);
00077   EXPECT_NEAR(deflated_msg.channels_[1], 3.0, eps);
00078 
00079   ASSERT_EQ(pruned.name.size(), (unsigned int) 2);
00080   ASSERT_EQ(pruned.position.size(), (unsigned int) 2);
00081   EXPECT_EQ(pruned.name[0], "A");
00082   EXPECT_EQ(pruned.name[1], "C");
00083   EXPECT_NEAR(pruned.position[0], 1.0, eps);
00084   EXPECT_NEAR(pruned.position[1], 3.0, eps);
00085 
00086   // ***********************************************
00087 
00088   joint_states.reset(new JointState);
00089   joint_states->name.resize(3);
00090   joint_states->position.resize(3);
00091   joint_states->velocity.resize(3);
00092   joint_states->effort.resize(3);
00093   joint_states->name[0] = "C";
00094   joint_states->name[1] = "A";
00095   joint_states->name[2] = "B";
00096   joint_states->position[0] = 4.0;
00097   joint_states->position[1] = 5.0;
00098   joint_states->position[2] = 6.0;
00099 
00100   deflater.deflate(joint_states, deflated_msg);
00101   deflater.prune(*joint_states, pruned);
00102 
00103   EXPECT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
00104   EXPECT_NEAR(deflated_msg.channels_[0], 5.0, eps);
00105   EXPECT_NEAR(deflated_msg.channels_[1], 4.0, eps);
00106 
00107   ASSERT_EQ(pruned.name.size(), (unsigned int) 2);
00108   ASSERT_EQ(pruned.position.size(), (unsigned int) 2);
00109   EXPECT_EQ(pruned.name[0], "A");
00110   EXPECT_EQ(pruned.name[1], "C");
00111   EXPECT_NEAR(pruned.position[0], 5.0, eps);
00112   EXPECT_NEAR(pruned.position[1], 4.0, eps);
00113 
00114   // ***********************************************
00115 
00116   joint_states.reset(new JointState);
00117   joint_states->name.resize(4);
00118   joint_states->position.resize(4);
00119   joint_states->velocity.resize(4);
00120   joint_states->effort.resize(4);
00121   joint_states->name[0] = "D";
00122   joint_states->name[1] = "C";
00123   joint_states->name[2] = "B";
00124   joint_states->name[3] = "A";
00125   joint_states->position[0] = 7.0;
00126   joint_states->position[1] = 8.0;
00127   joint_states->position[2] = 9.0;
00128   joint_states->position[3] = 10.0;
00129 
00130   deflater.deflate(joint_states, deflated_msg);
00131   deflater.prune(*joint_states, pruned);
00132 
00133   EXPECT_EQ(deflated_msg.channels_.size(), (unsigned int) 2);
00134   EXPECT_NEAR(deflated_msg.channels_[0], 10.0, eps);
00135   EXPECT_NEAR(deflated_msg.channels_[1], 8.0, eps);
00136 
00137   ASSERT_EQ(pruned.name.size(), (unsigned int) 2);
00138   ASSERT_EQ(pruned.position.size(), (unsigned int) 2);
00139   EXPECT_EQ(pruned.name[0], "A");
00140   EXPECT_EQ(pruned.name[1], "C");
00141   EXPECT_NEAR(pruned.position[0], 10.0, eps);
00142   EXPECT_NEAR(pruned.position[1],  8.0, eps);
00143 }
00144 
00145 
00146 int main(int argc, char **argv){
00147   testing::InitGoogleTest(&argc, argv);
00148   return RUN_ALL_TESTS();
00149 }
joint_states_deflater_unittest.cpp
