39 nav_msgs::Odometry old_odom = getLastOdom();
43 nav_msgs::Odometry new_odom = getLastOdom();
45 const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
46 const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
47 const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
49 EXPECT_LT(fabs(dz),
EPS);
52 double roll_old, pitch_old, yaw_old;
53 double roll_new, pitch_new, yaw_new;
56 EXPECT_LT(fabs(roll_new - roll_old),
EPS);
57 EXPECT_LT(fabs(pitch_new - pitch_old),
EPS);
58 EXPECT_LT(fabs(yaw_new - yaw_old),
EPS);
61 int main(
int argc,
char** argv)
63 testing::InitGoogleTest(&argc, argv);
64 ros::init(argc, argv,
"diff_drive_multiple_publishers_test");
68 int ret = RUN_ALL_TESTS();
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
int main(int argc, char **argv)
TEST_F(DiffDriveControllerTest, breakWithMultiplePublishers)
ROSCPP_DECL void shutdown()
const double POSITION_TOLERANCE