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::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);
}
