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