pr2_mechanism_model: test_joint_calibration

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 /* Author: Wim Meeussen */
00036 
00037 #include <string>
00038 #include <gtest/gtest.h>
00039 #include <ros/ros.h>
00040 #include <urdf/model.h>
00041 #include "pr2_mechanism_model/joint_calibration_simulator.h"
00042 
00043 using namespace pr2_mechanism_model;
00044 
00045 int g_argc;
00046 char** g_argv;
00047 
00048 class TestParser : public testing::Test
00049 {
00050 public:
00051 
00053   TestParser() {}
00054 
00056   ~TestParser() {}
00057 };
00058 
00059 
00060 
00061 
00062 TEST_F(TestParser, test)
00063 {
00064   ASSERT_TRUE(g_argc == 2);
00065 
00066   urdf::Model robot;
00067   ASSERT_TRUE(robot.initFile(g_argv[1]));
00068 
00069   boost::shared_ptr<const urdf::Joint> jnt_cont1 = robot.getJoint("continuous1");
00070   boost::shared_ptr<const urdf::Joint> jnt_cont2 = robot.getJoint("continuous2");
00071   boost::shared_ptr<const urdf::Joint> jnt_rev  = robot.getJoint("revolute");
00072   ASSERT_TRUE(jnt_cont1 != NULL);
00073   ASSERT_TRUE(jnt_cont2 != NULL);
00074   ASSERT_TRUE(jnt_rev != NULL);
00075 
00076   // test cont1
00077   pr2_mechanism_model::JointState js;
00078   pr2_hardware_interface::Actuator as;
00079 
00080   js.joint_ = jnt_cont1;
00081   js.position_ = 0.01;
00082   pr2_mechanism_model::JointCalibrationSimulator jnt_sim_1;
00083   jnt_sim_1.simulateJointCalibration(&js,&as);
00084   ASSERT_FALSE(as.state_.calibration_reading_);
00085   ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
00086   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00087 
00088   js.position_ = 1.0;
00089   as.state_.position_ = 10;
00090   jnt_sim_1.simulateJointCalibration(&js,&as);
00091   ASSERT_FALSE(as.state_.calibration_reading_);
00092   ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
00093   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00094 
00095   js.position_ = 1.6;
00096   as.state_.position_ = 16;
00097   jnt_sim_1.simulateJointCalibration(&js,&as);
00098   ASSERT_TRUE(as.state_.calibration_reading_);
00099   ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
00100   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00101   ASSERT_EQ(as.state_.last_calibration_rising_edge_, 10);
00102 
00103   js.position_ = 1.5001;
00104   as.state_.position_ = 150;
00105   jnt_sim_1.simulateJointCalibration(&js,&as);
00106   js.position_ = 1.4;
00107   as.state_.position_ = 14;
00108   jnt_sim_1.simulateJointCalibration(&js,&as);
00109   ASSERT_FALSE(as.state_.calibration_reading_);
00110   ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
00111   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00112   ASSERT_EQ(as.state_.last_calibration_rising_edge_, 150);
00113 
00114   js.position_ = 0.01;
00115   as.state_.position_ = 999;
00116   jnt_sim_1.simulateJointCalibration(&js,&as);
00117   js.position_ = -0.1;
00118   as.state_.position_ = -1;
00119   jnt_sim_1.simulateJointCalibration(&js,&as);
00120   ASSERT_TRUE(as.state_.calibration_reading_);
00121   ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
00122   ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
00123   ASSERT_EQ(as.state_.last_calibration_falling_edge_, 999);
00124 
00125   // test cont2
00126   as.state_.calibration_rising_edge_valid_ = false;
00127   as.state_.calibration_falling_edge_valid_ = false;
00128   js.joint_ = jnt_cont2;
00129   js.position_ = 0.1;
00130   pr2_mechanism_model::JointCalibrationSimulator jnt_sim_2;
00131   jnt_sim_2.simulateJointCalibration(&js,&as);
00132   ASSERT_TRUE(as.state_.calibration_reading_);
00133   ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
00134   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00135 
00136   js.position_ = 1.0;
00137   jnt_sim_2.simulateJointCalibration(&js,&as);
00138   ASSERT_TRUE(as.state_.calibration_reading_);
00139   ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
00140   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00141 
00142   js.position_ = 1.4;
00143   as.state_.position_ = 15;
00144   jnt_sim_2.simulateJointCalibration(&js,&as);
00145   js.position_ = 1.6;
00146   as.state_.position_ = 16;
00147   jnt_sim_2.simulateJointCalibration(&js,&as);
00148   ASSERT_FALSE(as.state_.calibration_reading_);
00149   ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
00150   ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
00151   ASSERT_EQ(as.state_.last_calibration_falling_edge_, 15);
00152 
00153   js.position_ = 1.6;
00154   as.state_.position_ = 15;
00155   jnt_sim_2.simulateJointCalibration(&js,&as);
00156   js.position_ = 1.4;
00157   as.state_.position_ = 14;
00158   jnt_sim_2.simulateJointCalibration(&js,&as);
00159   ASSERT_TRUE(as.state_.calibration_reading_);
00160   ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
00161   ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
00162   ASSERT_EQ(as.state_.last_calibration_falling_edge_, 15);  
00163 
00164   js.position_ = -0.1;
00165   as.state_.position_ = 0;
00166   jnt_sim_2.simulateJointCalibration(&js,&as);
00167   js.position_ = 0.1;
00168   as.state_.position_ = -1;
00169   jnt_sim_2.simulateJointCalibration(&js,&as);
00170   ASSERT_TRUE(as.state_.calibration_reading_);
00171   ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
00172   ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
00173   ASSERT_EQ(as.state_.last_calibration_rising_edge_, 0);  
00174 
00175   // test rev
00176   as.state_.calibration_rising_edge_valid_ = false;
00177   as.state_.calibration_falling_edge_valid_ = false;
00178   js.joint_ = jnt_rev;
00179   js.position_ = 0.01;
00180   pr2_mechanism_model::JointCalibrationSimulator jnt_sim_rev;
00181   jnt_sim_rev.simulateJointCalibration(&js,&as);
00182   ASSERT_FALSE(as.state_.calibration_reading_);
00183   ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
00184   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00185 
00186   js.position_ = 0.04;
00187   jnt_sim_rev.simulateJointCalibration(&js,&as);
00188   ASSERT_FALSE(as.state_.calibration_reading_);
00189   ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
00190   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00191 
00192   js.position_ = 0.5;
00193   as.state_.position_ = 5;
00194   jnt_sim_rev.simulateJointCalibration(&js,&as);
00195   js.position_ = 1.6;
00196   as.state_.position_ = 16;
00197   jnt_sim_rev.simulateJointCalibration(&js,&as);
00198   ASSERT_TRUE(as.state_.calibration_reading_);
00199   ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
00200   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00201   ASSERT_EQ(as.state_.last_calibration_rising_edge_, 5);
00202 
00203   js.position_ = 10.0;
00204   as.state_.position_ = 100.0;
00205   jnt_sim_rev.simulateJointCalibration(&js,&as);
00206   js.position_ = -10.0;
00207   as.state_.position_ = -100;
00208   jnt_sim_rev.simulateJointCalibration(&js,&as);
00209   ASSERT_FALSE(as.state_.calibration_reading_);
00210   ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
00211   ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
00212 
00213 
00214   SUCCEED();
00215 }
00216 
00217 
00218 
00219 
00220 int main(int argc, char** argv)
00221 {
00222   testing::InitGoogleTest(&argc, argv);
00223   ros::init(argc, argv, "test_joint_calibration_simulator");
00224   g_argc = argc;
00225   g_argv = argv;
00226   return RUN_ALL_TESTS();
00227 }
test_joint_calibration_simulator.cpp
