11 four_wheel_steering_msgs::FourWheelSteering cmd_vel;
13 cmd_vel.front_steering_angle = 0.0;
14 cmd_vel.rear_steering_angle = 0.0;
18 nav_msgs::Odometry old_odom = getLastOdom();
23 double travel_time = 5.0;
26 nav_msgs::Odometry new_odom = getLastOdom();
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;
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;
36 EXPECT_LT(fabs(dz),
EPS);
39 double roll_old, pitch_old, yaw_old;
40 double 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);
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);
61 four_wheel_steering_msgs::FourWheelSteering cmd_vel;
63 cmd_vel.front_steering_angle = 0.0;
64 cmd_vel.rear_steering_angle = 0.0;
68 nav_msgs::Odometry old_odom = getLastOdom();
71 cmd_vel.front_steering_angle = M_PI/8.0;
72 cmd_vel.rear_steering_angle = cmd_vel.front_steering_angle;
75 double travel_time = 5.0;
78 nav_msgs::Odometry new_odom = getLastOdom();
80 double actual_travel_time = (new_odom.header.stamp - old_odom.header.stamp).toSec();
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);
92 double roll_old, pitch_old, yaw_old;
93 double 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);
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);
114 four_wheel_steering_msgs::FourWheelSteering cmd_vel;
116 cmd_vel.front_steering_angle = 0.0;
117 cmd_vel.rear_steering_angle = 0.0;
118 publish_4ws(cmd_vel);
121 nav_msgs::Odometry old_odom = getLastOdom();
123 cmd_vel.speed = M_PI/2.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);
133 nav_msgs::Odometry new_odom = getLastOdom();
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);
143 double roll_old, pitch_old, yaw_old;
144 double roll_new, pitch_new, yaw_new;
147 EXPECT_LT(fabs(roll_new - roll_old),
EPS);
148 EXPECT_LT(fabs(pitch_new - pitch_old),
EPS);
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);
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);
166 four_wheel_steering_msgs::FourWheelSteering cmd_vel;
168 cmd_vel.front_steering_angle = 0.0;
169 cmd_vel.rear_steering_angle = 0.0;
170 publish_4ws(cmd_vel);
173 nav_msgs::Odometry old_odom = getLastOdom();
175 cmd_vel.speed = M_PI/2.0;
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);
186 nav_msgs::Odometry new_odom = getLastOdom();
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);
196 double roll_old, pitch_old, yaw_old;
197 double roll_new, pitch_new, yaw_new;
200 EXPECT_LT(fabs(roll_new - roll_old),
EPS);
201 EXPECT_LT(fabs(pitch_new - pitch_old),
EPS);
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);
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);
225 int main(
int argc,
char** argv)
227 testing::InitGoogleTest(&argc, argv);
228 ros::init(argc, argv,
"four_wheel_steering_4ws_cmd_test");
233 int ret = RUN_ALL_TESTS();
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TEST_F(FourWheelSteeringControllerTest, testForward)
tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion &quat)
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
const double POSITION_TOLERANCE