TEST_F(DiffDriveControllerTest, testLinearVelocityLimits)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// zero everything before test
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
ros::Duration(2.0).sleep();
// get initial odom
nav_msgs::Odometry old_odom = getLastOdom();
// send a big command
cmd_vel.linear.x = 10.0;
publish(cmd_vel);
// wait for a while
ros::Duration(5.0).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot speed is now 1.0 m.s-1, the limit
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 1.0 + VELOCITY_TOLERANCE);
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
cmd_vel.linear.x = 0.0;
publish(cmd_vel);
}
TEST_F(DiffDriveControllerTest, testOdomTopic)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// get an odom message
nav_msgs::Odometry odom_msg = getLastOdom();
// check its frame_id
ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "new_odom");
}
TEST_F(DiffDriveControllerTest, testNewOdomFrame)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// set up tf listener
tf::TransformListener listener;
ros::Duration(2.0).sleep();
// check the new_odom frame does exist
EXPECT_TRUE(listener.frameExists("new_odom"));
}
