 #include <std_msgs/String.h>
 #include <victoria_debug_msgs/TeensyDebug.h>
 #include <victoria_nav_msgs/Odom2DRaw.h>
-#include <victoria_sensor_msgs/ContactState1D.h>
+#include <victoria_sensor_msgs/DistanceDisplacement1D.h>
 #include <victoria_sensor_msgs/IMURaw.h>
 
 // Teensy pin definitions

 * Publish bumper data to ros.
 */
void doPublishBumpers(void) {
  // TODO(mwomack): Get readings from robot
  static const int MIN_RAW_LEFT_BUMPER(650);
  static const int MAX_RAW_LEFT_BUMPER(450);
  static const double RANGE_RAW_LEFT_BUMPER(MAX_RAW_LEFT_BUMPER - MIN_RAW_LEFT_BUMPER);
  static const double MAX_FORCE_LEFT_BUMPER(1.0);
  static const int MIN_RAW_RIGHT_BUMPER(650);
  static const int MAX_RAW_RIGHT_BUMPER(450);
  static const double RANGE_RAW_RIGHT_BUMPER(MAX_RAW_RIGHT_BUMPER - MIN_RAW_RIGHT_BUMPER);
  static const double MAX_FORCE_RIGHT_BUMPER(1.0);
  
  ros::Time current_time = ros_nh.now();

  int rawBumperLeft = analogRead(BUMPER_LEFT_PIN);
  int rawBumperRight = analogRead(BUMPER_RIGHT_PIN);

  double bumperLeft = MAX_FORCE_LEFT_BUMPER 
                        * ((rawBumperLeft - MIN_RAW_LEFT_BUMPER) / RANGE_RAW_LEFT_BUMPER);
  double bumperRight = MAX_FORCE_RIGHT_BUMPER 
                        * ((rawBumperRight - MIN_RAW_RIGHT_BUMPER) / RANGE_RAW_RIGHT_BUMPER);

  ros_bumper_left_msg.header.stamp = current_time;
  ros_bumper_left_msg.min_value = 0.0;
  ros_bumper_left_msg.max_value = MAX_FORCE_LEFT_BUMPER;
  ros_bumper_left_msg.value = bumperLeft;
  
  ros_bumper_right_msg.header.stamp = current_time;
  ros_bumper_right_msg.min_value = 0.0;
  ros_bumper_right_msg.max_value = MAX_FORCE_RIGHT_BUMPER;
  ros_bumper_right_msg.value = bumperRight;

  ros_bumper_left_pub.publish(&ros_bumper_left_msg);
  ros_bumper_right_pub.publish(&ros_bumper_right_msg);
}

/*

ros::Publisher ros_raw_imu_pub("imu_raw", &ros_raw_imu_msg);

// Bumper publishers
victoria_sensor_msgs::DistanceDisplacement1D ros_bumper_left_msg;
victoria_sensor_msgs::DistanceDisplacement1D ros_bumper_right_msg;
ros::Publisher ros_bumper_left_pub("bumper_left", &ros_bumper_left_msg);
ros::Publisher ros_bumper_right_pub("bumper_right", &ros_bumper_right_msg);

