lyq/sweeper_whu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1030

Browse Source
This commit is contained in:
lyq 2025-10-30 10:15:05 +08:00
parent be514b6d83
commit 8a056fa841
6 changed files with 262 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
gps_load_now.txt
View File

@ -1,44 +1,96 @@
32.0311442390 32.0311468570
120.91525759600 120.91525671300
358.131012 358.096008
0.000000 0.000000
32.0311534830 32.0311563730
120.91525806000 120.91525624100
10.571000 354.798004
0.000000 0.000000
32.0311624300 32.0311655490
120.91526160300 120.91525493600
30.403999 347.576996
0.000000 0.000000
32.0311698350 32.0311743100
120.91526811100 120.91525172600
42.812000 332.431000
0.000000 0.000000
32.0311764320 32.0311818630
120.91527574000 120.91524556200
45.012001 313.209991
0.000000 0.000000
32.0311827500 32.0311872560
120.91528336200 120.91523692500
45.269001 291.302002
0.000000 0.000000
32.0311892690 32.0311895010
120.91529116800 120.91522644400
46.345001 273.058014
0.000000 0.000000
32.0311954980 32.0311897810
120.91529957900 120.91521522700
54.297001 268.415009
0.000000 0.000000
32.0312002620 32.0311893590
120.91530907900 120.91520456100
66.992996 264.153992
0.000000 0.000000
32.0312030070 32.0311880140
120.91531955100 120.91519349600
80.920998 256.733002
0.000000 0.000000
32.0312029590 32.0311854800
120.91530925500 120.91518299200
78.594002 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 0.000000

68
routes/gps_load_1761367443087.txt Normal file
View File

@ -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

96
routes/gps_load_1761367875670.txt Normal file
View File

@ -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

8
src/fu/src/fu_node.cpp
View File

@ -220,7 +220,7 @@ private:
int reliability = 0; // RTK信号质量 1行 0不行 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]，负数表示左转，正数表示右转 int publish_angle = 0; // [-66.0,66.0]，负数表示左转，正数表示右转
// 车体矩形边界（栅格坐标） // 车体矩形边界（栅格坐标）
@ -718,7 +718,7 @@ private:
float target_angle = calculate_the_angle(x, y); 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<int>(target_angle); publish_angle = static_cast<int>(target_angle);
// 安全检查：未检测到车体位置 // 安全检查：未检测到车体位置
@ -892,7 +892,7 @@ private:
else else
{ {
// 距离足够，可以减速接近 // 距离足够，可以减速接近
publish_speed = std::max(800, publish_speed / 2); publish_speed = std::max(500, publish_speed / 2);
RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格", RCLCPP_INFO(this->get_logger(), "障碍物较宽，减速接近中，距离=%d格",
obstacle_analysis_.obstacle_distance); obstacle_analysis_.obstacle_distance);
} }
@ -1601,7 +1601,7 @@ private:
double return_time = (current_time - avoid_ctx_.last_state_change_time).seconds(); 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); float target_angle = calculate_the_angle(x, y);

29
src/mqtt_report/include/mqtt_report/mqtt_client.hpp
View File

@ -43,14 +43,12 @@ public:
{ {
std::lock_guard<std::mutex> lock(mtx_); std::lock_guard<std::mutex> lock(mtx_);
if (!client_) if (!client_)
{
std::cerr << "MQTT client not initialized" << std::endl;
return false; return false;
}
MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer; MQTTClient_connectOptions conn_opts = MQTTClient_connectOptions_initializer;
conn_opts.keepAliveInterval = 20; conn_opts.keepAliveInterval = 20;
conn_opts.cleansession = 1; conn_opts.cleansession = 1;
conn_opts.connectTimeout = 5; // ✅ 限制阻塞时间
if (!username_.empty()) if (!username_.empty())
{ {
@ -59,10 +57,11 @@ public:
} }
int rc; int rc;
int retry_count = 0; for (int retry_count = 0; retry_count < max_retries; ++retry_count)
while (retry_count < max_retries)
{ {
std::cout << "Connecting to MQTT (" << retry_count + 1 << "/" << max_retries << ")..." << std::endl;
rc = MQTTClient_connect(client_, &conn_opts); rc = MQTTClient_connect(client_, &conn_opts);
if (rc == MQTTCLIENT_SUCCESS) if (rc == MQTTCLIENT_SUCCESS)
{ {
is_connected_ = true; is_connected_ = true;
@ -70,20 +69,8 @@ public:
return true; return true;
} }
retry_count++; std::cerr << "Connect failed rc=" << rc << ", wait " << reconnect_interval_ms_ << " ms" << std::endl;
std::cerr << "Failed to connect (attempt " << retry_count << "/" << max_retries std::this_thread::sleep_for(std::chrono::milliseconds(reconnect_interval_ms_));
<< "), 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_));
}
} }
is_connected_ = false; 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) if (rc != MQTTCLIENT_SUCCESS)
{ {
std::cerr << "Failed to wait for completion, return code " << rc << std::endl; std::cerr << "Failed to wait for completion, return code " << rc << std::endl;
is_connected_ = false; // 确认失败标记为断开 // is_connected_ = false; // 确认失败标记为断开
return false; return false;
} }

2
src/pl/src/pl.cpp
View File

@ -267,7 +267,7 @@ void PL_ProcThread()
x = x_cm_tmp; x = x_cm_tmp;
y = y_cm_tmp; y = y_cm_tmp;
pl_speed = 1500; pl_speed = 800;
task_status = TaskStatus::RUNNING; task_status = TaskStatus::RUNNING;
} }