four_wheel_steering_4ws_cmd_test.cpp
Go to the documentation of this file.
1 #include "test_common.h"
3 
4 // TEST CASES
6 {
7  // wait for ROS
8  waitForController();
9 
10  // zero everything before test
11  four_wheel_steering_msgs::FourWheelSteering cmd_vel;
12  cmd_vel.speed = 0.0;
13  cmd_vel.front_steering_angle = 0.0;
14  cmd_vel.rear_steering_angle = 0.0;
15  publish_4ws(cmd_vel);
16  ros::Duration(0.1).sleep();
17  // get initial odom
18  nav_msgs::Odometry old_odom = getLastOdom();
19  // send a velocity command of 0.1 m/s
20  cmd_vel.speed = 0.1;
21  publish_4ws(cmd_vel);
22  // wait for Xs
23  double travel_time = 5.0;
24  ros::Duration(travel_time).sleep();
25 
26  nav_msgs::Odometry new_odom = getLastOdom();
27 
28  const double actual_travel_time = (new_odom.header.stamp - old_odom.header.stamp).toSec();
29  const double expected_distance = cmd_vel.speed * actual_travel_time;
30 
31  // check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
32  const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
33  const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
34  const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
35  EXPECT_NEAR(sqrt(dx*dx + dy*dy), expected_distance, POSITION_TOLERANCE);
36  EXPECT_LT(fabs(dz), EPS);
37 
38  // convert to rpy and test that way
39  double roll_old, pitch_old, yaw_old;
40  double roll_new, pitch_new, yaw_new;
41  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
42  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
43  EXPECT_LT(fabs(roll_new - roll_old), EPS);
44  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
45  EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
46  EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.speed, EPS);
47  EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
48  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
49 
50  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
51  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
52  EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
53 }
54 
56 {
57  // wait for ROS
58  waitForController();
59 
60  // zero everything before test
61  four_wheel_steering_msgs::FourWheelSteering cmd_vel;
62  cmd_vel.speed = 0.0;
63  cmd_vel.front_steering_angle = 0.0;
64  cmd_vel.rear_steering_angle = 0.0;
65  publish_4ws(cmd_vel);
66  ros::Duration(0.1).sleep();
67  // get initial odom
68  nav_msgs::Odometry old_odom = getLastOdom();
69  // send a velocityand steering command
70  cmd_vel.speed = 0.2;
71  cmd_vel.front_steering_angle = M_PI/8.0;
72  cmd_vel.rear_steering_angle = cmd_vel.front_steering_angle;
73  publish_4ws(cmd_vel);
74  // wait for Xs
75  double travel_time = 5.0;
76  ros::Duration(travel_time).sleep();
77 
78  nav_msgs::Odometry new_odom = getLastOdom();
79 
80  double actual_travel_time = (new_odom.header.stamp - old_odom.header.stamp).toSec();
81 
82  // check if the robot traveled 5 meter in XY plane, changes in z should be ~~0
83  const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
84  const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
85  const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
86  EXPECT_NEAR(sqrt(dx*dx + dy*dy), cmd_vel.speed*actual_travel_time, POSITION_TOLERANCE);
87  EXPECT_NEAR(dx, cmd_vel.speed*actual_travel_time*cos(cmd_vel.front_steering_angle), POSITION_TOLERANCE);
88  EXPECT_NEAR(dy, cmd_vel.speed*actual_travel_time*sin(cmd_vel.front_steering_angle), POSITION_TOLERANCE);
89  EXPECT_LT(fabs(dz), EPS);
90 
91  // convert to rpy and test that way
92  double roll_old, pitch_old, yaw_old;
93  double roll_new, pitch_new, yaw_new;
94  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
95  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
96  EXPECT_LT(fabs(roll_new - roll_old), EPS);
97  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
98  EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
99  EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.speed*cos(cmd_vel.front_steering_angle), EPS);
100  EXPECT_NEAR(fabs(new_odom.twist.twist.linear.y), cmd_vel.speed*sin(cmd_vel.front_steering_angle), EPS);
101  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
102 
103  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
104  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
105  EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
106 }
107 
109 {
110  // wait for ROS
111  waitForController();
112 
113  // zero everything before test
114  four_wheel_steering_msgs::FourWheelSteering cmd_vel;
115  cmd_vel.speed = 0.0;
116  cmd_vel.front_steering_angle = 0.0;
117  cmd_vel.rear_steering_angle = 0.0;
118  publish_4ws(cmd_vel);
119  ros::Duration(0.1).sleep();
120  // get initial odom
121  nav_msgs::Odometry old_odom = getLastOdom();
122  // send a velocity command
123  cmd_vel.speed = M_PI/2.0;
124  // send steering for angular speed of M_PI/10.0
125  double cmd_angular = M_PI/10.0;
127  cmd_vel.front_steering_angle = 0.18776;
128  cmd_vel.rear_steering_angle = -0.18776;
129  publish_4ws(cmd_vel);
130  // wait for 10s to make a half turn
131  ros::Duration(10.0).sleep();
132 
133  nav_msgs::Odometry new_odom = getLastOdom();
134 
135  // check if the robot traveled 20 meter in XY plane, changes in z should be ~~0
136  const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
137  const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
138  const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
139  EXPECT_NEAR(sqrt(dx*dx + dy*dy), fabs(2*cmd_vel.speed/(cmd_angular)), POSITION_TOLERANCE);
140  EXPECT_LT(fabs(dz), EPS);
141 
142  // convert to rpy and test that way
143  double roll_old, pitch_old, yaw_old;
144  double roll_new, pitch_new, yaw_new;
145  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
146  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
147  EXPECT_LT(fabs(roll_new - roll_old), EPS);
148  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
149  EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
150 
151  EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.speed, EPS);
152  EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
153  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
154 
155  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
156  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
157  EXPECT_NEAR(new_odom.twist.twist.angular.z, cmd_angular, EPS);
158 }
159 
161 {
162  // wait for ROS
163  waitForController();
164 
165  // zero everything before test
166  four_wheel_steering_msgs::FourWheelSteering cmd_vel;
167  cmd_vel.speed = 0.0;
168  cmd_vel.front_steering_angle = 0.0;
169  cmd_vel.rear_steering_angle = 0.0;
170  publish_4ws(cmd_vel);
171  ros::Duration(0.1).sleep();
172  // get initial odom
173  nav_msgs::Odometry old_odom = getLastOdom();
174  // send a velocity command
175  cmd_vel.speed = M_PI/2.0;
176  // send steering for angular speed
177  double cmd_angular = -M_PI/10.0;
180  cmd_vel.front_steering_angle = -0.21655;
181  cmd_vel.rear_steering_angle = 0.15866;
182  publish_4ws(cmd_vel);
183  // wait for 10s to make a half turn
184  ros::Duration(10.0).sleep();
185 
186  nav_msgs::Odometry new_odom = getLastOdom();
187 
188  // check if the robot traveled 20 meter in XY plane, changes in z should be ~~0
189  const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
190  const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
191  const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
192  EXPECT_NEAR(sqrt(dx*dx + dy*dy), fabs(2*cmd_vel.speed/(cmd_angular)), POSITION_TOLERANCE);
193  EXPECT_LT(fabs(dz), EPS);
194 
195  // convert to rpy and test that way
196  double roll_old, pitch_old, yaw_old;
197  double roll_new, pitch_new, yaw_new;
198  tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
199  tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
200  EXPECT_LT(fabs(roll_new - roll_old), EPS);
201  EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
202  EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE);
203 
204  EXPECT_NEAR(sqrt(pow(new_odom.twist.twist.linear.x,2)+pow(new_odom.twist.twist.linear.y,2)),
205  fabs(cmd_vel.speed), EPS);
206  EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
207 
208  EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
209  EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
210  EXPECT_NEAR(new_odom.twist.twist.angular.z, cmd_angular, EPS);
211 }
212 
214 {
215  // wait for ROS
216  waitForController();
217 
218  // set up tf listener
219  tf::TransformListener listener;
220  ros::Duration(2.0).sleep();
221  // check the odom frame exist
222  EXPECT_TRUE(listener.frameExists("odom"));
223 }
224 
225 int main(int argc, char** argv)
226 {
227  testing::InitGoogleTest(&argc, argv);
228  ros::init(argc, argv, "four_wheel_steering_4ws_cmd_test");
229 
231  spinner.start();
232  //ros::Duration(0.5).sleep();
233  int ret = RUN_ALL_TESTS();
234  spinner.stop();
235  ros::shutdown();
236  return ret;
237 }
bool sleep() const
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(FourWheelSteeringControllerTest, testForward)
void spinner()
tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion &quat)
Definition: test_common.h:130
int main(int argc, char **argv)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
ROSCPP_DECL void shutdown()
const double ORIENTATION_TOLERANCE
Definition: test_common.h:49
bool frameExists(const std::string &frame_id_str) const
const double POSITION_TOLERANCE
Definition: test_common.h:45


four_wheel_steering_controller
Author(s): Vincent Rousseau
autogenerated on Thu Apr 11 2019 03:08:25