ackermann_steering_controller_test.cpp
Go to the documentation of this file.
1 // Copyright (C) 2013, PAL Robotics S.L.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright notice,
7 // this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of PAL Robotics, Inc. nor the names of its
12 // contributors may be used to endorse or promote products derived from
13 // this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 // POSSIBILITY OF SUCH DAMAGE.
27 
30 
31 #include "../common/include/test_common.h"
32 #include <tf/transform_listener.h>
33 
34 // TEST CASES
36 {
37  // wait for ROS
38  while(!isControllerAlive())
39  {
40  ros::Duration(0.1).sleep();
41  }
42  // zero everything before test
43  geometry_msgs::Twist cmd_vel;
44  cmd_vel.linear.x = 0.0;
45  cmd_vel.angular.z = 0.0;
46  publish(cmd_vel);
47  ros::Duration(0.1).sleep();
48  // get initial odom
49  nav_msgs::Odometry old_odom = getLastOdom();
50  // send a velocity command of 0.1 m/s
51  cmd_vel.linear.x = 0.1;
52  publish(cmd_vel);
53  // wait for 10s
54  ros::Duration(10.0).sleep();
55 
56  nav_msgs::Odometry new_odom = getLastOdom();
57 
58  // check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
59  const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
60  const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
61  const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
62  EXPECT_NEAR(sqrt(dx*dx + dy*dy), 1.0, POSITION_TOLERANCE);
63  EXPECT_LT(fabs(dz), EPS);
64 
65  // convert to rpy and test that way
66  double roll_old, pitch_old, yaw_old;
67  double roll_new, pitch_new, yaw_new;
68  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
69  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
70  EXPECT_LT(fabs(roll_new - roll_old), EPS);
71  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
72  EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
73  EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
74  EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
75  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
76 
77  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
78  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
79  EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
80 }
81 
83 {
84  // wait for ROS
85  while(!isControllerAlive())
86  {
87  ros::Duration(0.1).sleep();
88  }
89  // zero everything before test
90  geometry_msgs::Twist cmd_vel;
91  cmd_vel.linear.x = 0.0;
92  cmd_vel.angular.z = 0.0;
93  publish(cmd_vel);
94  ros::Duration(0.1).sleep();
95  // get initial odom
96  nav_msgs::Odometry old_odom = getLastOdom();
97  // send a velocity command
98  cmd_vel.angular.z = M_PI/10.0;
99  // send linear command too
100  // because sending only angular command doesn't actuate wheels for steer drive mechanism
101  cmd_vel.linear.x = 0.1;
102  publish(cmd_vel);
103  // wait for 10s
104  ros::Duration(10.0).sleep();
105 
106  nav_msgs::Odometry new_odom = getLastOdom();
107 
108  // check if the robot rotated PI around z, changes in x should be ~~0 and in y should be y_answer
109  double x_answer = 0.0;
110  double y_answer = 2.0 * cmd_vel.linear.x / cmd_vel.angular.z; // R = v/w, D = 2R
111  EXPECT_NEAR(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), x_answer, POSITION_TOLERANCE);
112  EXPECT_NEAR(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), y_answer, POSITION_TOLERANCE);
113  EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS);
114 
115  // convert to rpy and test that way
116  double roll_old, pitch_old, yaw_old;
117  double roll_new, pitch_new, yaw_new;
118  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
119  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
120  EXPECT_LT(fabs(roll_new - roll_old), EPS);
121  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
122  EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
123 
124  EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.1, VELOCITY_TOLERANCE);
125  EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
126  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
127 
128  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
129  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
130  EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), M_PI/10.0, EPS);
131 }
132 
134 {
135  // wait for ROS
136  while(!isControllerAlive())
137  {
138  ros::Duration(0.1).sleep();
139  }
140  // set up tf listener
141  tf::TransformListener listener;
142  ros::Duration(2.0).sleep();
143  // check the odom frame exist
144  EXPECT_TRUE(listener.frameExists("odom"));
145 }
146 
147 int main(int argc, char** argv)
148 {
149  testing::InitGoogleTest(&argc, argv);
150  ros::init(argc, argv, "ackermann_steering_controller_test");
151 
153  spinner.start();
154  //ros::Duration(0.5).sleep();
155  int ret = RUN_ALL_TESTS();
156  spinner.stop();
157  ros::shutdown();
158  return ret;
159 }
bool sleep() const
const double VELOCITY_TOLERANCE
Definition: test_common.h:46
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const double EPS
Definition: test_common.h:44
TEST_F(AckermannSteeringControllerTest, testForward)
void spinner()
tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion &quat)
Definition: test_common.h:95
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
int main(int argc, char **argv)
ROSCPP_DECL void shutdown()
const double ORIENTATION_TOLERANCE
Definition: test_common.h:50
bool frameExists(const std::string &frame_id_str) const
const double POSITION_TOLERANCE
Definition: test_common.h:45


ackermann_steering_controller
Author(s): Masaru Morita
autogenerated on Thu Apr 11 2019 03:08:13