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