diff --git a/src/autonomy/route/src/route_node.cpp b/src/autonomy/route/src/route_node.cpp index 6d03528..729ac14 100644 --- a/src/autonomy/route/src/route_node.cpp +++ b/src/autonomy/route/src/route_node.cpp @@ -16,6 +16,7 @@ #include "rclcpp/rclcpp.hpp" #include "sweeper_interfaces/msg/rtk.hpp" #include "sweeper_interfaces/msg/sub.hpp" +#include "sweeper_interfaces/msg/vehicle_identity.hpp" std::mutex collect_mutex; // 互斥锁,用于保护 isCollecting 变量 @@ -113,6 +114,11 @@ class route_node : public rclcpp::Node msg_subscribe_ = this->create_subscription( "rtk_message", 10, std::bind(&route_node::msg_callback, this, std::placeholders::_1)); + + // identity(transient_local,确保晚启动也能拿到) + identity_sub_ = this->create_subscription( + "/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(), + std::bind(&route_node::identityCallback, this, std::placeholders::_1)); } private: @@ -170,9 +176,17 @@ class route_node : public rclcpp::Node } } } + void identityCallback(const sweeper_interfaces::msg::VehicleIdentity::SharedPtr msg) + { + vid = msg->vid; + + LOG_INFO("Identity: VID=%s", msg->vid.c_str()); + } + // 声明订阅者 rclcpp::Subscription::SharedPtr sub_subscribe_; rclcpp::Subscription::SharedPtr msg_subscribe_; + rclcpp::Subscription::SharedPtr identity_sub_; }; double ntzx_GPS_length(double lonti1, double lati1, double lonti2, double lati2) @@ -229,9 +243,7 @@ void init_main() } if (reader.parse(in, root)) { - vid = root["mqtt"]["vid"].asString(); upload_URL = root["mqtt"]["upload_url"].asString(); - LOG_INFO("vid:%s", vid.c_str()); } in.close(); // 关闭文件流 }