test_joint_calibration_simulator.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Wim Meeussen */
36 
37 #include <string>
38 #include <gtest/gtest.h>
39 #include <ros/ros.h>
40 #include <urdf/model.h>
42 
43 using namespace pr2_mechanism_model;
44 
45 int g_argc;
46 char** g_argv;
47 
48 class TestParser : public testing::Test
49 {
50 public:
51 
53  TestParser() {}
54 
56  ~TestParser() {}
57 };
58 
59 
60 
61 
63 {
64  ASSERT_TRUE(g_argc == 2);
65 
66  urdf::Model robot;
67  ASSERT_TRUE(robot.initFile(g_argv[1]));
68 
69  boost::shared_ptr<const urdf::Joint> jnt_cont1 = robot.getJoint("continuous1");
70  boost::shared_ptr<const urdf::Joint> jnt_cont2 = robot.getJoint("continuous2");
71  boost::shared_ptr<const urdf::Joint> jnt_rev = robot.getJoint("revolute");
72  ASSERT_TRUE(jnt_cont1 != NULL);
73  ASSERT_TRUE(jnt_cont2 != NULL);
74  ASSERT_TRUE(jnt_rev != NULL);
75 
76  // test cont1
79 
80  js.joint_ = jnt_cont1;
81  js.position_ = 0.01;
83  jnt_sim_1.simulateJointCalibration(&js,&as);
84  ASSERT_FALSE(as.state_.calibration_reading_);
85  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
86  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
87 
88  js.position_ = 1.0;
89  as.state_.position_ = 10;
90  jnt_sim_1.simulateJointCalibration(&js,&as);
91  ASSERT_FALSE(as.state_.calibration_reading_);
92  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
93  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
94 
95  js.position_ = 1.6;
96  as.state_.position_ = 16;
97  jnt_sim_1.simulateJointCalibration(&js,&as);
98  ASSERT_TRUE(as.state_.calibration_reading_);
99  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
100  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
101  ASSERT_EQ(as.state_.last_calibration_rising_edge_, 10);
102 
103  js.position_ = 1.5001;
104  as.state_.position_ = 150;
105  jnt_sim_1.simulateJointCalibration(&js,&as);
106  js.position_ = 1.4;
107  as.state_.position_ = 14;
108  jnt_sim_1.simulateJointCalibration(&js,&as);
109  ASSERT_FALSE(as.state_.calibration_reading_);
110  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
111  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
112  ASSERT_EQ(as.state_.last_calibration_rising_edge_, 150);
113 
114  js.position_ = 0.01;
115  as.state_.position_ = 999;
116  jnt_sim_1.simulateJointCalibration(&js,&as);
117  js.position_ = -0.1;
118  as.state_.position_ = -1;
119  jnt_sim_1.simulateJointCalibration(&js,&as);
120  ASSERT_TRUE(as.state_.calibration_reading_);
121  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
122  ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
123  ASSERT_EQ(as.state_.last_calibration_falling_edge_, 999);
124 
125  // test cont2
128  js.joint_ = jnt_cont2;
129  js.position_ = 0.1;
131  jnt_sim_2.simulateJointCalibration(&js,&as);
132  ASSERT_TRUE(as.state_.calibration_reading_);
133  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
134  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
135 
136  js.position_ = 1.0;
137  jnt_sim_2.simulateJointCalibration(&js,&as);
138  ASSERT_TRUE(as.state_.calibration_reading_);
139  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
140  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
141 
142  js.position_ = 1.4;
143  as.state_.position_ = 15;
144  jnt_sim_2.simulateJointCalibration(&js,&as);
145  js.position_ = 1.6;
146  as.state_.position_ = 16;
147  jnt_sim_2.simulateJointCalibration(&js,&as);
148  ASSERT_FALSE(as.state_.calibration_reading_);
149  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
150  ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
151  ASSERT_EQ(as.state_.last_calibration_falling_edge_, 15);
152 
153  js.position_ = 1.6;
154  as.state_.position_ = 15;
155  jnt_sim_2.simulateJointCalibration(&js,&as);
156  js.position_ = 1.4;
157  as.state_.position_ = 14;
158  jnt_sim_2.simulateJointCalibration(&js,&as);
159  ASSERT_TRUE(as.state_.calibration_reading_);
160  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
161  ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
162  ASSERT_EQ(as.state_.last_calibration_falling_edge_, 15);
163 
164  js.position_ = -0.1;
165  as.state_.position_ = 0;
166  jnt_sim_2.simulateJointCalibration(&js,&as);
167  js.position_ = 0.1;
168  as.state_.position_ = -1;
169  jnt_sim_2.simulateJointCalibration(&js,&as);
170  ASSERT_TRUE(as.state_.calibration_reading_);
171  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
172  ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
173  ASSERT_EQ(as.state_.last_calibration_rising_edge_, 0);
174 
175  // test rev
178  js.joint_ = jnt_rev;
179  js.position_ = 0.01;
181  jnt_sim_rev.simulateJointCalibration(&js,&as);
182  ASSERT_FALSE(as.state_.calibration_reading_);
183  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
184  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
185 
186  js.position_ = 0.04;
187  jnt_sim_rev.simulateJointCalibration(&js,&as);
188  ASSERT_FALSE(as.state_.calibration_reading_);
189  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
190  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
191 
192  js.position_ = 0.5;
193  as.state_.position_ = 5;
194  jnt_sim_rev.simulateJointCalibration(&js,&as);
195  js.position_ = 1.6;
196  as.state_.position_ = 16;
197  jnt_sim_rev.simulateJointCalibration(&js,&as);
198  ASSERT_TRUE(as.state_.calibration_reading_);
199  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
200  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
201  ASSERT_EQ(as.state_.last_calibration_rising_edge_, 5);
202 
203  js.position_ = 10.0;
204  as.state_.position_ = 100.0;
205  jnt_sim_rev.simulateJointCalibration(&js,&as);
206  js.position_ = -10.0;
207  as.state_.position_ = -100;
208  jnt_sim_rev.simulateJointCalibration(&js,&as);
209  ASSERT_FALSE(as.state_.calibration_reading_);
210  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
211  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
212 
213 
214  SUCCEED();
215 }
216 
217 
218 
219 
220 int main(int argc, char** argv)
221 {
222  testing::InitGoogleTest(&argc, argv);
223  ros::init(argc, argv, "test_joint_calibration_simulator");
224  g_argc = argc;
225  g_argv = argv;
226  return RUN_ALL_TESTS();
227 }
URDF_EXPORT bool initFile(const std::string &filename)
void simulateJointCalibration(pr2_mechanism_model::JointState *, pr2_hardware_interface::Actuator *)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< const urdf::Joint > joint_
A pointer to the corresponding urdf::Joint from the urdf::Model.
Definition: joint.h:81
double position_
The joint position in radians or meters (read-only variable)
Definition: joint.h:84
int main(int argc, char **argv)
TEST_F(TestParser, test)


pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Jun 10 2019 14:19:04