diff --git a/src/mqtt_report/src/mqtt_report.cpp b/src/mqtt_report/src/mqtt_report.cpp index 9e9783a..f5ad316 100644 --- a/src/mqtt_report/src/mqtt_report.cpp +++ b/src/mqtt_report/src/mqtt_report.cpp @@ -38,6 +38,8 @@ void Msg_Handler(const mc::msg::CanFrame::SharedPtr msg) float gear_ratio = 32.0; float wheel_radius = 0.15; // 轮胎半径,单位米 float vehicle_speed_kmh = (2 * 3.1416 * wheel_radius * (motorspeed / gear_ratio) * 60.0) / 1000.0; + // 保留两位小数 + vehicle_speed_kmh = std::round(vehicle_speed_kmh * 100.0f) / 100.0f; info_report.speed = vehicle_speed_kmh; break; } @@ -256,12 +258,14 @@ int main(int argc, char **argv) if (!load_config(config)) { - std::cerr << "Failed to load MQTT config." << std::endl; + RCLCPP_ERROR(rclcpp::get_logger("main"), "Failed to load MQTT config."); return 1; } auto node = std::make_shared(config); + RCLCPP_INFO(node->get_logger(), "MqttReport node started."); + rclcpp::spin(node); rclcpp::shutdown(); return 0;