 #include <victoria_debug_msgs/TeensyDebug.h>
 #include <victoria_nav_msgs/Odom2DRaw.h>
 #include <victoria_sensor_msgs/DistanceDisplacement1D.h>
+#include <victoria_sensor_msgs/EncoderCount.h>
 #include <victoria_sensor_msgs/IMURaw.h>
 
 // Teensy pin definitions

#include <LIS3MDL.h>

// Other includes
#include <Timer.h>
#include "RosParamHelper.h"


  ros_raw_odom_msg.child_frame_id = "/base_link";
  ros_bumper_left_msg.header.frame_id = "/bumper_left";
  ros_bumper_right_msg.header.frame_id = "/bumper_right";
  ros_encoder_left_msg.header.frame_id = "/encoder_left";
  ros_encoder_right_msg.header.frame_id = "/encoder_right";
  last_cmd_vel_time = current_time;
  cmd_vel_timeout_exceeded = false;
  last_encoder_read_time = current_time;
