Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
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
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 }