 /*
  * Publish imu data to ros.
  */
-void doPublishRawImu() {
+void doPublishRawImu(void) {
   ros::Time current_time = ros_nh.now();
   imu.read();
   mag.read();

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

// Position of robot
geometry_msgs::Pose2D pose;

char ros_odom_header_frame_id[] = "/odom";
char ros_odom_child_frame_id[] = "/base_link";

char debug_str[80];
std_msgs::String ros_debug_msg;

ros::Publisher ros_bumper_debug_pub("bumper_debug", &ros_debug_msg);
ros::Publisher ros_encoder_debug_pub("encoder_debug", &ros_debug_msg);
victoria_debug_msgs::TeensyDebug teensy_debug_msg;
ros::Publisher ros_teensy_debug_pub("teensy_debug", &teensy_debug_msg);

// Debug blink
// Blink duration is in milliseconds.
const int BLINK_DURATION(500);
int blink_state = LOW;
  
void setup() {
  // Register ros publishers
  ros_nh.advertise(ros_raw_odom_pub);
  ros_nh.advertise(ros_raw_imu_pub);
  ros_nh.advertise(ros_bumper_debug_pub);
  ros_nh.advertise(ros_encoder_debug_pub);
  ros_nh.advertise(ros_teensy_debug_pub);

  // Register ros subscribers

  ros_raw_imu_msg.magnetometer = convertMagnetometer(mag);

  ros_raw_imu_pub.publish(&ros_raw_imu_msg);
}

/*
 * 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_raw_imu_pub.publish(&ros_bumper_left_msg);
  ros_raw_imu_pub.publish(&ros_bumper_right_msg);
}

/*
