1030
This commit is contained in:
parent
be514b6d83
commit
8a056fa841
118
gps_load_now.txt
118
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
|
||||
|
||||
68
routes/gps_load_1761367443087.txt
Normal file
68
routes/gps_load_1761367443087.txt
Normal 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
96
routes/gps_load_1761367875670.txt
Normal 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
|
||||
@ -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<int>(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);
|
||||
|
||||
@ -43,14 +43,12 @@ public:
|
||||
{
|
||||
std::lock_guard<std::mutex> 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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user