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>
41 #include "pr2_mechanism_model/joint_calibration_simulator.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 
62 TEST_F(TestParser, test)
63 {
64  ASSERT_TRUE(g_argc == 2);
65 
66  urdf::Model robot;
67  ASSERT_TRUE(robot.initFile(g_argv[1]));
68 
69 #if URDFDOM_1_0_0_API
70  urdf::JointConstSharedPtr jnt_cont1 = robot.getJoint("continuous1");
71  urdf::JointConstSharedPtr jnt_cont2 = robot.getJoint("continuous2");
72  urdf::JointConstSharedPtr jnt_rev = robot.getJoint("revolute");
73 #else
74  boost::shared_ptr<const urdf::Joint> jnt_cont1 = robot.getJoint("continuous1");
75  boost::shared_ptr<const urdf::Joint> jnt_cont2 = robot.getJoint("continuous2");
76  boost::shared_ptr<const urdf::Joint> jnt_rev = robot.getJoint("revolute");
77 #endif
78  ASSERT_TRUE(jnt_cont1 != NULL);
79  ASSERT_TRUE(jnt_cont2 != NULL);
80  ASSERT_TRUE(jnt_rev != NULL);
81 
82  // test cont1
83  pr2_mechanism_model::JointState js;
84  pr2_hardware_interface::Actuator as;
85 
86  js.joint_ = jnt_cont1;
87  js.position_ = 0.01;
88  pr2_mechanism_model::JointCalibrationSimulator jnt_sim_1;
89  jnt_sim_1.simulateJointCalibration(&js,&as);
90  ASSERT_FALSE(as.state_.calibration_reading_);
91  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
92  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
93 
94  js.position_ = 1.0;
95  as.state_.position_ = 10;
96  jnt_sim_1.simulateJointCalibration(&js,&as);
97  ASSERT_FALSE(as.state_.calibration_reading_);
98  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
99  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
100 
101  js.position_ = 1.6;
102  as.state_.position_ = 16;
103  jnt_sim_1.simulateJointCalibration(&js,&as);
104  ASSERT_TRUE(as.state_.calibration_reading_);
105  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
106  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
107  ASSERT_EQ(as.state_.last_calibration_rising_edge_, 10);
108 
109  js.position_ = 1.5001;
110  as.state_.position_ = 150;
111  jnt_sim_1.simulateJointCalibration(&js,&as);
112  js.position_ = 1.4;
113  as.state_.position_ = 14;
114  jnt_sim_1.simulateJointCalibration(&js,&as);
115  ASSERT_FALSE(as.state_.calibration_reading_);
116  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
117  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
118  ASSERT_EQ(as.state_.last_calibration_rising_edge_, 150);
119 
120  js.position_ = 0.01;
121  as.state_.position_ = 999;
122  jnt_sim_1.simulateJointCalibration(&js,&as);
123  js.position_ = -0.1;
124  as.state_.position_ = -1;
125  jnt_sim_1.simulateJointCalibration(&js,&as);
126  ASSERT_TRUE(as.state_.calibration_reading_);
127  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
128  ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
129  ASSERT_EQ(as.state_.last_calibration_falling_edge_, 999);
130 
131  // test cont2
132  as.state_.calibration_rising_edge_valid_ = false;
133  as.state_.calibration_falling_edge_valid_ = false;
134  js.joint_ = jnt_cont2;
135  js.position_ = 0.1;
136  pr2_mechanism_model::JointCalibrationSimulator jnt_sim_2;
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.0;
143  jnt_sim_2.simulateJointCalibration(&js,&as);
144  ASSERT_TRUE(as.state_.calibration_reading_);
145  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
146  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
147 
148  js.position_ = 1.4;
149  as.state_.position_ = 15;
150  jnt_sim_2.simulateJointCalibration(&js,&as);
151  js.position_ = 1.6;
152  as.state_.position_ = 16;
153  jnt_sim_2.simulateJointCalibration(&js,&as);
154  ASSERT_FALSE(as.state_.calibration_reading_);
155  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
156  ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
157  ASSERT_EQ(as.state_.last_calibration_falling_edge_, 15);
158 
159  js.position_ = 1.6;
160  as.state_.position_ = 15;
161  jnt_sim_2.simulateJointCalibration(&js,&as);
162  js.position_ = 1.4;
163  as.state_.position_ = 14;
164  jnt_sim_2.simulateJointCalibration(&js,&as);
165  ASSERT_TRUE(as.state_.calibration_reading_);
166  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
167  ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
168  ASSERT_EQ(as.state_.last_calibration_falling_edge_, 15);
169 
170  js.position_ = -0.1;
171  as.state_.position_ = 0;
172  jnt_sim_2.simulateJointCalibration(&js,&as);
173  js.position_ = 0.1;
174  as.state_.position_ = -1;
175  jnt_sim_2.simulateJointCalibration(&js,&as);
176  ASSERT_TRUE(as.state_.calibration_reading_);
177  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
178  ASSERT_TRUE(as.state_.calibration_falling_edge_valid_);
179  ASSERT_EQ(as.state_.last_calibration_rising_edge_, 0);
180 
181  // test rev
182  as.state_.calibration_rising_edge_valid_ = false;
183  as.state_.calibration_falling_edge_valid_ = false;
184  js.joint_ = jnt_rev;
185  js.position_ = 0.01;
186  pr2_mechanism_model::JointCalibrationSimulator jnt_sim_rev;
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.04;
193  jnt_sim_rev.simulateJointCalibration(&js,&as);
194  ASSERT_FALSE(as.state_.calibration_reading_);
195  ASSERT_FALSE(as.state_.calibration_rising_edge_valid_);
196  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
197 
198  js.position_ = 0.5;
199  as.state_.position_ = 5;
200  jnt_sim_rev.simulateJointCalibration(&js,&as);
201  js.position_ = 1.6;
202  as.state_.position_ = 16;
203  jnt_sim_rev.simulateJointCalibration(&js,&as);
204  ASSERT_TRUE(as.state_.calibration_reading_);
205  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
206  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
207  ASSERT_EQ(as.state_.last_calibration_rising_edge_, 5);
208 
209  js.position_ = 10.0;
210  as.state_.position_ = 100.0;
211  jnt_sim_rev.simulateJointCalibration(&js,&as);
212  js.position_ = -10.0;
213  as.state_.position_ = -100;
214  jnt_sim_rev.simulateJointCalibration(&js,&as);
215  ASSERT_FALSE(as.state_.calibration_reading_);
216  ASSERT_TRUE(as.state_.calibration_rising_edge_valid_);
217  ASSERT_FALSE(as.state_.calibration_falling_edge_valid_);
218 
219 
220  SUCCEED();
221 }
222 
223 
224 
225 
226 int main(int argc, char** argv)
227 {
228  testing::InitGoogleTest(&argc, argv);
229  ros::init(argc, argv, "test_joint_calibration_simulator");
230  g_argc = argc;
231  g_argv = argv;
232  return RUN_ALL_TESTS();
233 }
g_argc
int g_argc
Definition: test_joint_calibration_simulator.cpp:45
pr2_mechanism_model::JointState
Definition: joint.h:103
boost::shared_ptr< const urdf::Joint >
TEST_F
TEST_F(TestParser, test)
Definition: test_joint_calibration_simulator.cpp:62
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
pr2_mechanism_model
Definition: chain.h:41
ros.h
g_argv
char ** g_argv
Definition: test_joint_calibration_simulator.cpp:46
pr2_hardware_interface::Actuator::state_
ActuatorState state_
urdf::Model
pr2_mechanism_model::JointState::position_
double position_
The joint position in radians or meters (read-only variable)
Definition: joint.h:120
pr2_mechanism_model::JointState::joint_
boost::shared_ptr< const urdf::Joint > joint_
A pointer to the corresponding urdf::Joint from the urdf::Model.
Definition: joint.h:116
test
pr2_hardware_interface::ActuatorState::calibration_falling_edge_valid_
bool calibration_falling_edge_valid_
urdf::Model::initFile
URDF_EXPORT bool initFile(const std::string &filename)
main
int main(int argc, char **argv)
Definition: test_joint_calibration_simulator.cpp:226
model.h
pr2_mechanism_model::JointCalibrationSimulator::simulateJointCalibration
void simulateJointCalibration(pr2_mechanism_model::JointState *, pr2_hardware_interface::Actuator *)
Definition: joint_calibration_simulator.cpp:91
pr2_hardware_interface::ActuatorState::position_
double position_
pr2_mechanism_model::JointCalibrationSimulator
Definition: joint_calibration_simulator.h:42
pr2_hardware_interface::ActuatorState::calibration_rising_edge_valid_
bool calibration_rising_edge_valid_
TestParser
Definition: test_joint_calibration_simulator.cpp:48
pr2_hardware_interface::ActuatorState::calibration_reading_
bool calibration_reading_
pr2_hardware_interface::ActuatorState::last_calibration_rising_edge_
double last_calibration_rising_edge_
pr2_hardware_interface::ActuatorState::last_calibration_falling_edge_
double last_calibration_falling_edge_
pr2_hardware_interface::Actuator
joint_calibration_simulator.h

pr2_mechanism_model
Author(s): Eric Berger berger@willowgarage.com, Stuart Glaser, Wim Meeussen
autogenerated on Mon Mar 6 2023 03:49:17