11 geometry_msgs::Twist cmd_vel;
12 cmd_vel.linear.x = 0.0;
13 cmd_vel.angular.z = 0.0;
17 nav_msgs::Odometry old_odom = getLastOdom();
19 cmd_vel.linear.x = 0.1;
22 double travel_time = 5.0;
25 nav_msgs::Odometry new_odom = getLastOdom();
27 const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;
29 const double expected_distance = cmd_vel.linear.x * actual_elapsed_time.
toSec();
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.linear.x,
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 geometry_msgs::Twist cmd_vel;
62 cmd_vel.linear.x = 0.0;
63 cmd_vel.angular.z = 0.0;
67 nav_msgs::Odometry old_odom = getLastOdom();
69 cmd_vel.linear.x = M_PI/2.0;
70 cmd_vel.angular.z = M_PI/10.0;
75 nav_msgs::Odometry new_odom = getLastOdom();
78 const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
79 const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
80 const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
81 EXPECT_NEAR(sqrt(dx*dx + dy*dy), 2*cmd_vel.linear.x/cmd_vel.angular.z,
POSITION_TOLERANCE);
82 EXPECT_LT(fabs(dz),
EPS);
85 double roll_old, pitch_old, yaw_old;
86 double roll_new, pitch_new, yaw_new;
89 EXPECT_LT(fabs(roll_new - roll_old),
EPS);
90 EXPECT_LT(fabs(pitch_new - pitch_old),
EPS);
93 EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x,
EPS);
94 EXPECT_LT(fabs(new_odom.twist.twist.linear.y),
EPS);
95 EXPECT_LT(fabs(new_odom.twist.twist.linear.z),
EPS);
97 EXPECT_LT(fabs(new_odom.twist.twist.angular.x),
EPS);
98 EXPECT_LT(fabs(new_odom.twist.twist.angular.y),
EPS);
99 EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), cmd_vel.angular.z,
EPS);
114 int main(
int argc,
char** argv)
116 testing::InitGoogleTest(&argc, argv);
117 ros::init(argc, argv,
"four_wheel_steering_twist_cmd_test");
122 int ret = RUN_ALL_TESTS();
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion &quat)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
ROSCPP_DECL void shutdown()
TEST_F(FourWheelSteeringControllerTest, testForward)
const double ORIENTATION_TOLERANCE
const double POSITION_TOLERANCE