void CrioReceiver::handleDiagnosticsPacket(CRIODiagnosticsPacket packet) {
    ros::Time current_time = ros::Time::now();
    CRIODiagnosticsPacket swapped_packet = swapDiagnosticsPacket(packet);
    diagnostics_info_ = swapped_packet;
    std_msgs::Bool msg;
    msg.data = !diagnostics_info_.eStopTriggered;
    estop_pub_.publish(msg);
	
	cwru_msgs::PowerState power_msg;
	power_msg.header.stamp = current_time;
	power_msg.header.frame_id = "crio";
	power_msg.battery_voltage = diagnostics_info_.VMonitor_24V_mV / 1000.0;
	power_msg.v13_8_voltage = diagnostics_info_.VMonitor_13V_mV / 1000.0;
	power_msg.motor_voltage = diagnostics_info_.VMonitor_eStop_mV / 1000.0;
	power_msg.cRIO_voltage = diagnostics_info_.VMonitor_cRIO_mV / 1000.0;
	power_pub_.publish(power_msg);
	
    cwru_msgs::cRIOSensors sensor_msg;
    sensor_msg.header.stamp = current_time;
    sensor_msg.header.frame_id = "crio";
    sensor_msg.left_wheel_encoder = diagnostics_info_.LWheelTicks;
    sensor_msg.right_wheel_encoder = diagnostics_info_.RWheelTicks;
    sensor_msg.left_motor_encoder = diagnostics_info_.LMotorTicks;
    sensor_msg.right_motor_encoder = diagnostics_info_.RMotorTicks;
    sensor_msg.yaw_rate = diagnostics_info_.YawRate_mV;
    sensor_msg.yaw_temp = diagnostics_info_.YawTemp_mV;
    sensor_msg.yaw_ref = diagnostics_info_.YawRef_mV;
    sensor_pub_.publish(sensor_msg);
  }
  void CrioReceiver::handlePosePacket(CRIOPosePacket packet) {
    ros::Time current_time = ros::Time::now();
    CRIOPosePacket swapped_packet = swapPosePacket(packet);
    if (push_casters_) {
      swapped_packet.x = -swapped_packet.x;
      swapped_packet.y = -swapped_packet.y;
      swapped_packet.theta = swapped_packet.theta + M_PI;
      swapped_packet.vel = -swapped_packet.vel;
    }
    pose_packet_ = swapped_packet;
    cwru_msgs::Pose p, p2;
    p.x = swapped_packet.x;
    p.y = swapped_packet.y;
    p.theta = swapped_packet.theta;
    p.vel = swapped_packet.vel;
    p.omega = swapped_packet.omega;
    ROS_DEBUG("Yaw bias: %f", swapped_packet.yaw_bias);
    p.x_var = swapped_packet.x_variance;
    p.y_var = swapped_packet.y_variance;
    p.theta_var = swapped_packet.theta_variance;
    p.vel_var = swapped_packet.vel_variance;
    p.omega_var = swapped_packet.omega_variance;
    ROS_DEBUG("Yaw bias variance: %f", swapped_packet.yaw_bias_variance);
    p.header.frame_id = "crio";
    p.header.stamp = current_time;
    p2 = p;
    p2.y = -p2.y;
    p2.theta = -p2.theta;
    p2.omega = -p2.omega;
    p2.header.frame_id = "flipped_crio";
    pose_pub_.publish(p);
    flipped_pose_pub_.publish(p2);

    cwru_msgs::Sonar ping;
    ping.header.stamp = current_time;
    handleSonarPing(ping, swapped_packet.sonar_ping_1, std::string("sonar_1_link"), sonar1_pub_);
    handleSonarPing(ping, swapped_packet.sonar_ping_2, std::string("sonar_2_link"), sonar2_pub_);
    handleSonarPing(ping, swapped_packet.sonar_ping_3, std::string("sonar_3_link"), sonar3_pub_);
    handleSonarPing(ping, swapped_packet.sonar_ping_4, std::string("sonar_4_link"), sonar4_pub_);
    handleSonarPing(ping, swapped_packet.sonar_ping_5, std::string("sonar_5_link"), sonar5_pub_);
    ROS_DEBUG("Handled a Pose Packet");
  }
  void CrioReceiver::handleDiagnosticsPacket(CRIODiagnosticsPacket packet) {
    ros::Time current_time = ros::Time::now();
    CRIODiagnosticsPacket swapped_packet = swapDiagnosticsPacket(packet);
    diagnostics_info_ = swapped_packet;
    std_msgs::Bool msg;
    msg.data = diagnostics_info_.eStopTriggered;
    estop_pub_.publish(msg);

    cwru_base::cRIOSensors sensor_msg;
    sensor_msg.header.stamp = current_time;
    sensor_msg.header.frame_id = "crio";
    sensor_msg.left_wheel_encoder = diagnostics_info_.LWheelTicks;
    sensor_msg.right_wheel_encoder = diagnostics_info_.RWheelTicks;
    sensor_msg.left_motor_encoder = diagnostics_info_.LMotorTicks;
    sensor_msg.right_motor_encoder = diagnostics_info_.RMotorTicks;
    sensor_msg.yaw_rate = diagnostics_info_.YawRate_mV;
    sensor_msg.yaw_temp = diagnostics_info_.YawTemp_mV;
    sensor_msg.yaw_ref = diagnostics_info_.YawRef_mV;
    sensor_pub_.publish(sensor_msg);
  }
  void MCDriverNode::publishEncoderPacket(const encoder_packet_t& packet)
  {
    minicrusher_msgs::EncoderArray encoder_array;
    minicrusher_msgs::Encoder encoder;

    encoder.header.stamp = ros::Time::now();
    encoder.header.frame_id = "left_front_encoder";
    encoder.distance = packet.positions_ticks[0] * lf_enc_m_per_tick_;
    encoder_array.encoders.push_back(encoder);
    lf_encoder_pub_.publish(encoder);

    encoder.header.frame_id = "left_middle_encoder";
    encoder.distance = packet.positions_ticks[2] * lm_enc_m_per_tick_;
    encoder_array.encoders.push_back(encoder);
    lm_encoder_pub_.publish(encoder);

    encoder.header.frame_id = "left_back_encoder";
    encoder.distance = packet.positions_ticks[4] * lb_enc_m_per_tick_;
    encoder_array.encoders.push_back(encoder);
    lb_encoder_pub_.publish(encoder);

    encoder.header.frame_id = "right_front_encoder";
    encoder.distance = packet.positions_ticks[1] * rf_enc_m_per_tick_;
    encoder_array.encoders.push_back(encoder);
    rf_encoder_pub_.publish(encoder);

    encoder.header.frame_id = "right_middle_encoder";
    encoder.distance = packet.positions_ticks[3] * rm_enc_m_per_tick_;
    encoder_array.encoders.push_back(encoder);
    rm_encoder_pub_.publish(encoder);

    encoder.header.frame_id = "right_back_encoder";
    encoder.distance = packet.positions_ticks[5] * rb_enc_m_per_tick_;
    encoder_array.encoders.push_back(encoder);
    rb_encoder_pub_.publish(encoder);

    encoder_array_pub_.publish(encoder_array);
  }