diff --git a/gps_load_now.txt b/gps_load_now.txt index d5ec464..f0bb720 100644 --- a/gps_load_now.txt +++ b/gps_load_now.txt @@ -1,44 +1,96 @@ -32.0311442390 -120.91525759600 -358.131012 +32.0311468570 +120.91525671300 +358.096008 0.000000 -32.0311534830 -120.91525806000 -10.571000 +32.0311563730 +120.91525624100 +354.798004 0.000000 -32.0311624300 -120.91526160300 -30.403999 +32.0311655490 +120.91525493600 +347.576996 0.000000 -32.0311698350 -120.91526811100 -42.812000 +32.0311743100 +120.91525172600 +332.431000 0.000000 -32.0311764320 -120.91527574000 -45.012001 +32.0311818630 +120.91524556200 +313.209991 0.000000 -32.0311827500 -120.91528336200 -45.269001 +32.0311872560 +120.91523692500 +291.302002 0.000000 -32.0311892690 -120.91529116800 -46.345001 +32.0311895010 +120.91522644400 +273.058014 0.000000 -32.0311954980 -120.91529957900 -54.297001 +32.0311897810 +120.91521522700 +268.415009 0.000000 -32.0312002620 -120.91530907900 -66.992996 +32.0311893590 +120.91520456100 +264.153992 0.000000 -32.0312030070 -120.91531955100 -80.920998 +32.0311880140 +120.91519349600 +256.733002 0.000000 -32.0312029590 -120.91530925500 -78.594002 +32.0311854800 +120.91518299200 +247.852005 +0.000000 +32.0311813510 +120.91517328200 +235.455994 +0.000000 +32.0311754410 +120.91516488300 +219.582001 +0.000000 +32.0311677300 +120.91515878500 +201.425995 +0.000000 +32.0311588910 +120.91515602500 +183.102005 +0.000000 +32.0311498140 +120.91515571600 +178.761002 +0.000000 +32.0311408050 +120.91515620700 +172.496002 +0.000000 +32.0311313060 +120.91515857400 +158.169998 +0.000000 +32.0311232530 +120.91516330100 +142.091995 +0.000000 +32.0311168570 +120.91517109100 +123.244003 +0.000000 +32.0311126890 +120.91518064100 +104.664001 +0.000000 +32.0311106850 +120.91519129000 +97.190002 +0.000000 +32.0311099930 +120.91520212400 +89.829002 +0.000000 +32.0311102590 +120.91521279400 +85.438004 0.000000 diff --git a/routes/gps_load_1761367443087.txt b/routes/gps_load_1761367443087.txt new file mode 100644 index 0000000..87d4932 --- /dev/null +++ b/routes/gps_load_1761367443087.txt @@ -0,0 +1,68 @@ +32.0311017640 +120.91525757800 +352.238007 +0.000000 +32.0311108500 +120.91525573000 +349.462006 +0.000000 +32.0311198770 +120.91525398500 +348.104004 +0.000000 +32.0311290760 +120.91525266500 +356.882996 +0.000000 +32.0311383690 +120.91525232600 +1.130000 +0.000000 +32.0311478450 +120.91525275400 +2.373000 +0.000000 +32.0311569190 +120.91525367400 +9.031000 +0.000000 +32.0311658220 +120.91525588000 +16.469999 +0.000000 +32.0311748260 +120.91525988700 +25.517000 +0.000000 +32.0311828180 +120.91526533400 +36.137001 +0.000000 +32.0311902300 +120.91527240700 +45.061001 +0.000000 +32.0311962480 +120.91528038700 +55.247002 +0.000000 +32.0312010270 +120.91528964500 +64.734001 +0.000000 +32.0312043320 +120.91530032000 +77.070000 +0.000000 +32.0312057240 +120.91531159000 +82.500999 +0.000000 +32.0312068290 +120.91532278200 +83.804001 +0.000000 +32.0312077120 +120.91533411500 +87.883003 +0.000000 diff --git a/routes/gps_load_1761367875670.txt b/routes/gps_load_1761367875670.txt new file mode 100644 index 0000000..f0bb720 --- /dev/null +++ b/routes/gps_load_1761367875670.txt @@ -0,0 +1,96 @@ +32.0311468570 +120.91525671300 +358.096008 +0.000000 +32.0311563730 +120.91525624100 +354.798004 +0.000000 +32.0311655490 +120.91525493600 +347.576996 +0.000000 +32.0311743100 +120.91525172600 +332.431000 +0.000000 +32.0311818630 +120.91524556200 +313.209991 +0.000000 +32.0311872560 +120.91523692500 +291.302002 +0.000000 +32.0311895010 +120.91522644400 +273.058014 +0.000000 +32.0311897810 +120.91521522700 +268.415009 +0.000000 +32.0311893590 +120.91520456100 +264.153992 +0.000000 +32.0311880140 +120.91519349600 +256.733002 +0.000000 +32.0311854800 +120.91518299200 +247.852005 +0.000000 +32.0311813510 +120.91517328200 +235.455994 +0.000000 +32.0311754410 +120.91516488300 +219.582001 +0.000000 +32.0311677300 +120.91515878500 +201.425995 +0.000000 +32.0311588910 +120.91515602500 +183.102005 +0.000000 +32.0311498140 +120.91515571600 +178.761002 +0.000000 +32.0311408050 +120.91515620700 +172.496002 +0.000000 +32.0311313060 +120.91515857400 +158.169998 +0.000000 +32.0311232530 +120.91516330100 +142.091995 +0.000000 +32.0311168570 +120.91517109100 +123.244003 +0.000000 +32.0311126890 +120.91518064100 +104.664001 +0.000000 +32.0311106850 +120.91519129000 +97.190002 +0.000000 +32.0311099930 +120.91520212400 +89.829002 +0.000000 +32.0311102590 +120.91521279400 +85.438004 +0.000000 diff --git a/src/fu/src/fu_node.cpp b/src/fu/src/fu_node.cpp index 7a3791d..ae8b267 100644 --- a/src/fu/src/fu_node.cpp +++ b/src/fu/src/fu_node.cpp @@ -220,7 +220,7 @@ private: int reliability = 0; // RTK信号质量 1行 0不行 // 发布消息的参数 - int publish_speed = 0; // 转速 0-2000 + int publish_speed = 0; // 转速 0-1000 int publish_angle = 0; // [-66.0,66.0]，负数表示左转，正数表示右转 // 车体矩形边界（栅格坐标） @@ -718,7 +718,7 @@ private: float target_angle = calculate_the_angle(x, y); // 初始化控制参数 - publish_speed = std::max(800, std::min(speed, 1500)); // 限制速度范围 + publish_speed = std::max(500, std::min(speed, 1000)); // 限制速度范围 publish_angle = static_cast(target_angle); // 安全检查：未检测到车体位置 @@ -892,7 +892,7 @@ private: else { // 距离足够，可以减速接近 - publish_speed = std::max(800, publish_speed / 2); + publish_speed = std::max(500, publish_speed / 2); RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格", obstacle_analysis_.obstacle_distance); } @@ -1601,7 +1601,7 @@ private: double return_time = (current_time - avoid_ctx_.last_state_change_time).seconds(); // 恢复正常速度 - publish_speed = std::min(1500, speed); + publish_speed = std::min(1000, speed); // 计算目标角度 float target_angle = calculate_the_angle(x, y); diff --git a/src/mqtt_report/include/mqtt_report/mqtt_client.hpp b/src/mqtt_report/include/mqtt_report/mqtt_client.hpp index 1d4d00d..97023d1 100644 --- a/src/mqtt_report/include/mqtt_report/mqtt_client.hpp +++ b/src/mqtt_report/include/mqtt_report/mqtt_client.hpp @@ -43,14 +43,12 @@ public: { std::lock_guard lock(mtx_); if (!client_) - { - std::cerr << "MQTT client not initialized" << std::endl; return false; - } MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; conn_opts.keepAliveInterval = 20; conn_opts.cleansession = 1; + conn_opts.connectTimeout = 5; // ✅ 限制阻塞时间 if (!username_.empty()) { @@ -59,10 +57,11 @@ public: } int rc; - int retry_count = 0; - while (retry_count < max_retries) + for (int retry_count = 0; retry_count < max_retries; ++retry_count) { + std::cout << "Connecting to MQTT (" << retry_count + 1 << "/" << max_retries << ")..." << std::endl; rc = MQTTClient_connect(client_, &conn_opts); + if (rc == MQTTCLIENT_SUCCESS) { is_connected_ = true; @@ -70,20 +69,8 @@ public: return true; } - retry_count++; - std::cerr << "Failed to connect (attempt " << retry_count << "/" << max_retries - << "), return code " << rc << std::endl; - - if (retry_count < max_retries) - { - // 等待重连间隔前，检查是否需要退出程序 - if (!rclcpp::ok()) - { - std::cerr << "Program exiting, stop reconnection" << std::endl; - return false; - } - std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_)); - } + std::cerr << "Connect failed rc=" << rc << ", wait " << reconnect_interval_ms_ << " ms" << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_)); } is_connected_ = false; @@ -146,11 +133,11 @@ public: } // 等待发布完成 - rc = MQTTClient_waitForCompletion(client_, token, 1000L); // 超时1秒 + rc = MQTTClient_waitForCompletion(client_, token, 10000L); // 超时10秒 if (rc != MQTTCLIENT_SUCCESS) { std::cerr << "Failed to wait for completion, return code " << rc << std::endl; - is_connected_ = false; // 确认失败标记为断开 + // is_connected_ = false; // 确认失败标记为断开 return false; } diff --git a/src/pl/src/pl.cpp b/src/pl/src/pl.cpp index 16f6bff..e3718db 100644 --- a/src/pl/src/pl.cpp +++ b/src/pl/src/pl.cpp @@ -267,7 +267,7 @@ void PL_ProcThread() x = x_cm_tmp; y = y_cm_tmp; - pl_speed = 1500; + pl_speed = 800; task_status = TaskStatus::RUNNING; }