diff --git a/diff_drive_controller/test/diff_drive_controller_nan.test b/diff_drive_controller/test/diff_drive_controller_nan.test
index 892e0bd59..a836a6551 100644
--- a/diff_drive_controller/test/diff_drive_controller_nan.test
+++ b/diff_drive_controller/test/diff_drive_controller_nan.test
@@ -2,6 +2,11 @@
   <!-- Load common test stuff -->
   <include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
 
+  <rosparam>
+    diffbot_controller:
+      publish_cmd: True
+  </rosparam>
+
   <!-- Controller test -->
   <test test-name="diff_drive_nan_test"
         pkg="diff_drive_controller"
@@ -9,5 +14,6 @@
         time-limit="80.0">
     <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
     <remap from="odom" to="diffbot_controller/odom" />
+    <remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
   </test>
 </launch>
diff --git a/diff_drive_controller/test/diff_drive_nan_test.cpp b/diff_drive_controller/test/diff_drive_nan_test.cpp
index f622728f5..cca671184 100644
--- a/diff_drive_controller/test/diff_drive_nan_test.cpp
+++ b/diff_drive_controller/test/diff_drive_nan_test.cpp
@@ -99,8 +99,8 @@ TEST_F(DiffDriveControllerTest, testNaNCmd)
     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);
+    geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
+    ROS_INFO_STREAM("cmd_vel_out.twist.linear.x "<<odom_msg.twist.linear.x);
     ros::Duration(0.1).sleep();
   }
 
@@ -109,6 +109,10 @@ TEST_F(DiffDriveControllerTest, testNaNCmd)
   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);
+
+  geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
+
+  EXPECT_EQ(fabs(odom_msg.twist.linear.x), 0);
 }
 
 int main(int argc, char** argv)