Fix nan values into /odom topic when can bus is down

Signed-off-by: Gregory Mermet <g.mermet@ez-wheel.com>

Author: GMezWheel

src/diff_drive_controller/DiffDriveController.cpp

@@ -389,15 +389,15 @@ namespace ezw {
             if (ERROR_NONE != err_l) {
                 ROS_ERROR(
                     "Failed to get the NMT state for left motor, EZW_ERR: SMCService : "
-                    "Controller::getPDSState() return error code : %d",
+                    "Controller::getNMTState() return error code : %d",
                     (int)err_l);
             }
 
             err_r = m_right_controller.getNMTState(nmt_state_r);
             if (ERROR_NONE != err_r) {
                 ROS_ERROR(
                     "Failed to get the NMT state for right motor, EZW_ERR: SMCService : "
-                    "Controller::getPDSState() return error code : %d",
+                    "Controller::getNMTState() return error code : %d",
                     (int)err_r);
             }
 
@@ -533,6 +533,12 @@ namespace ezw {
                 return;
             }
 
+            if (m_left_timestamp_prev_us == left_timestamp_us || m_right_timestamp_prev_us == right_timestamp_us) {
+                // Nothing to do
+                // Values have not changed (avoid "nan" values in /odom topic)
+                return;
+            }
+
             // Encoder difference between t and t-1
             double d_dist_left_m = static_cast<double>(left_dist_now_mm - m_dist_left_prev_mm) / 1000.0;
             double d_dist_right_m = static_cast<double>(right_dist_now_mm - m_dist_right_prev_mm) / 1000.0;
@@ -911,6 +917,7 @@ namespace ezw {
                     "Error reading SAFEIN_1 control word from left motor, EZW_ERR: SMCService : "
                     "Controller::getSafetyControlWord() return error code : %d",
                     (int)err);
+                return;
             }
             bool safein1_l[6];
             safein1_l[0] = safety_control_word.safety_function_0;
@@ -926,6 +933,7 @@ namespace ezw {
                     "Error reading SAFEIN_1 control word from right motor, EZW_ERR: SMCService : "
                     "Controller::getSafetyControlWord() return error code : %d",
                     (int)err);
+                return;
             }
             bool safein1_r[6];
             safein1_r[0] = safety_control_word.safety_function_0;