From 09c05a0f80ed82e8a159986c90ef6093621cf633 Mon Sep 17 00:00:00 2001
From: Vincent Rousseau <vincent.rousseau@irstea.fr>
Date: Mon, 3 Jul 2017 14:38:50 +0200
Subject: [PATCH] Add test but it always succeeded

---
 .../test/diff_drive_nan_test.cpp              | 33 +++++++++++++++++++
 1 file changed, 33 insertions(+)

diff --git a/diff_drive_controller/test/diff_drive_nan_test.cpp b/diff_drive_controller/test/diff_drive_nan_test.cpp
index 2f06e7eb1..f622728f5 100644
--- a/diff_drive_controller/test/diff_drive_nan_test.cpp
+++ b/diff_drive_controller/test/diff_drive_nan_test.cpp
@@ -78,6 +78,39 @@ TEST_F(DiffDriveControllerTest, testNaN)
   EXPECT_NE(std::isnan(odom.pose.pose.orientation.w), true);
 }
 
+TEST_F(DiffDriveControllerTest, testNaNCmd)
+{
+  // 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(0.1).sleep();
+
+  // send NaN
+  for(int i=0; i<100;++i)
+  {
+    geometry_msgs::Twist cmd_vel;
+    cmd_vel.linear.x = NAN;
+    cmd_vel.angular.z = 0.0;
+    publish(cmd_vel);
+    nav_msgs::Odometry odom = getLastOdom();
+    ROS_INFO_STREAM("odom.twist.twist.linear.x "<<odom.twist.twist.linear.x);
+    ros::Duration(0.1).sleep();
+  }
+
+  nav_msgs::Odometry odom = getLastOdom();
+  ROS_INFO_STREAM("odom.twist.twist.linear.x "<<odom.twist.twist.linear.x);
+  EXPECT_EQ(odom.twist.twist.linear.x, 0.0);
+  EXPECT_EQ(odom.pose.pose.position.x, 0.0);
+  EXPECT_EQ(odom.pose.pose.position.y, 0.0);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);