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

Compare commits

base: lyq:ae9b7c1704bccf1a465c25faf23278a4f4074578
Branches Tags
lyq:master
lyq:humble
..
compare: lyq:098e67d7983b5a410cef0982c52f186b8e75011e
Branches Tags
lyq:humble
lyq:master

No commits in common. "ae9b7c1704bccf1a465c25faf23278a4f4074578" and "098e67d7983b5a410cef0982c52f186b8e75011e" have entirely different histories.
ae9b7c1704 ... 098e67d798

3 changed files with 337 additions and 623 deletions
Show Stats Expand all files Collapse all files

454
gps_load_now.txt
View File

@ -1,292 +1,308 @@
32.0309031720
120.91507456700
358.903015
32.0309270710
120.91508380700
174.522003
0.000000
32.0309122700
120.91507465100
1.824000
32.0309177630
120.91508508700
170.716995
0.000000
32.0309218300
120.91507536900
5.379000
32.0309083630
120.91508691600
167.014008
0.000000
32.0309309300
120.91507646200
9.029000
32.0308991500
120.91509013100
158.218994
0.000000
32.0309399620
120.91507767700
8.762000
32.0308909680
120.91509463700
151.005997
0.000000
32.0309491970
120.91507969400
12.384000
32.0308825810
120.91510033500
147.225006
0.000000
32.0309588180
120.91508216100
13.471000
32.0308741660
120.91510631000
156.934006
0.000000
32.0309675640
120.91508480500
17.896000
32.0308652190
120.91510999400
166.878998
0.000000
32.0309766450
120.91508880400
22.101999
32.0308561900
120.91511247100
165.128998
0.000000
32.0309848880
120.91509322200
26.379999
32.0308470540
120.91511526000
164.408005
0.000000
32.0309930520
120.91509823000
30.480000
32.0308384050
120.91511818800
165.382996
0.000000
32.0310012620
120.91510437600
33.999001
32.0308288240
120.91512052600
167.072998
0.000000
32.0310087320
120.91511028400
34.645000
32.0308193630
120.91512325900
164.526993
0.000000
32.0310163110
120.91511626500
36.090000
32.0308099890
120.91512646200
160.466995
0.000000
32.0310240180
120.91512326500
37.971001
32.0308016850
120.91513058700
151.772003
0.000000
32.0310317340
120.91513052000
39.084000
32.0307940750
120.91513631900
140.869995
0.000000
32.0310391000
120.91513778100
41.595001
32.0307872200
120.91514318900
132.024994
0.000000
32.0310459690
120.91514589500
51.403000
32.0307816190
120.91515177200
123.033997
0.000000
32.0310509040
120.91515476500
65.014999
32.0307767850
120.91516175300
111.536003
0.000000
32.0310540470
120.91516559800
83.711998
32.0307737970
120.91517181200
104.806999
0.000000
32.0310540770
120.91517649900
98.914001
32.0307716670
120.91518215100
100.636002
0.000000
32.0310514290
120.91518685500
114.102997
32.0307703680
120.91519282400
89.806000
0.000000
32.0310469800
120.91519624200
130.358994
32.0307709200
120.91520367800
81.512001
0.000000
32.0310399570
120.91520416900
143.240005
32.0307728420
120.91521413600
69.341003
0.000000
32.0310318070
120.91521042600
154.128006
32.0307771010
120.91522443200
52.085999
0.000000
32.0310236210
120.91521480300
156.352997
32.0307837600
120.91523281900
41.723999
0.000000
32.0310153520
120.91521894700
156.727997
32.0307914160
120.91523974900
31.273001
0.000000
32.0310068890
120.91522295900
162.268997
32.0307994650
120.91524466500
21.080000
0.000000
32.0309976280
120.91522578700
167.934006
32.0308081020
120.91524794900
11.581000
0.000000
32.0309887430
120.91522755900
171.403000
32.0308169840
120.91524990900
6.804000
0.000000
32.0309798350
120.91522898000
172.026993
32.0308266650
120.91525087500
2.044000
0.000000
32.0309701240
120.91523061000
173.535004
32.0308363640
120.91525102500
357.092987
0.000000
32.0309608820
120.91523161700
174.781998
32.0308454740
120.91525076200
0.180000
0.000000
32.0309511450
120.91523278100
173.095001
32.0308545400
120.91525048300
358.631012
0.000000
32.0309414590
120.91523408100
172.738007
32.0308642580
120.91525064500
1.479000
0.000000
32.0309317890
120.91523561300
172.759995
32.0308733660
120.91525116000
3.574000
0.000000
32.0309220880
120.91523714100
171.729996
32.0308823780
120.91525174100
1.308000
0.000000
32.0309124620
120.91523885800
171.533005
32.0308915670
120.91525213700
3.472000
0.000000
32.0309035530
120.91524062100
170.462006
32.0309013400
120.91525273600
1.245000
0.000000
32.0308939330
120.91524243500
173.511993
32.0309103910
120.91525290600
1.489000
0.000000
32.0308849020
120.91524366900
174.072998
32.0309201180
120.91525361800
1.429000
0.000000
32.0308758540
120.91524473900
174.837006
32.0309291640
120.91525371800
359.424011
0.000000
32.0308668250
120.91524554200
175.477005
32.0309388660
120.91525320500
356.106995
0.000000
32.0308571550
120.91524639000
176.475998
32.0309478980
120.91525251700
352.549988
0.000000
32.0308481530
120.91524702000
181.354996
32.0309576240
120.91525108400
350.079010
0.000000
32.0308390220
120.91524675200
181.582001
32.0309665940
120.91524933800
352.509003
0.000000
32.0308293150
120.91524646800
181.154007
32.0309755100
120.91524791200
351.528015
0.000000
32.0308195650
120.91524630200
181.212997
32.0309844990
120.91524603700
348.252014
0.000000
32.0308105960
120.91524531800
190.882996
32.0309940620
120.91524366200
346.699005
0.000000
32.0308017000
120.91524309800
196.507996
32.0310034720
120.91524080400
344.282990
0.000000
32.0307927000
120.91523864000
215.287994
32.0310120850
120.91523778100
340.821014
0.000000
32.0307854930
120.91523117700
234.037994
32.0310205650
120.91523415800
335.459015
0.000000
32.0307810700
120.91522120300
251.423996
32.0310285570
120.91522924400
325.936005
0.000000
32.0307787600
120.91521000500
261.298004
32.0310358980
120.91522309900
322.156006
0.000000
32.0307778120
120.91519940900
266.550995
32.0310433430
120.91521577300
312.940002
0.000000
32.0307773010
120.91518860600
267.316986
32.0310496090
120.91520717900
303.739990
0.000000
32.0307770810
120.91517787700
274.162994
32.0310549300
120.91519746200
298.476013
0.000000
32.0307783650
120.91516734800
287.984009
32.0310588260
120.91518787300
291.746002
0.000000
32.0307823320
120.91515692700
303.756989
32.0310619800
120.91517768500
284.023010
0.000000
32.0307878050
120.91514836000
312.296997
32.0310634770
120.91516693600
275.184998
0.000000
32.0307945510
120.91514116000
327.027008
32.0310638860
120.91515611400
264.816010
0.000000
32.0308022350
120.91513533100
327.855988
32.0310623280
120.91514564900
253.266998
0.000000
32.0308099090
120.91512965100
327.330994
32.0310587730
120.91513498100
240.110001
0.000000
32.0308180980
120.91512338900
326.914001
32.0310535320
120.91512525600
233.132004
0.000000
32.0308265120
120.91511760300
333.226013
32.0310479220
120.91511694700
226.000000
0.000000
32.0308350690
120.91511249800
332.450012
32.0310409290
120.91510900500
220.576004
0.000000
32.0308436950
120.91510715800
331.259003
32.0310339060
120.91510237400
214.026001
0.000000
32.0308516230
120.91510207100
330.828003
32.0310256190
120.91509629900
203.143997
0.000000
32.0308596170
120.91509688700
329.868988
32.0310169390
120.91509284600
191.787994
0.000000
32.0308675160
120.91509126400
328.059998
32.0310074340
120.91509122200
179.445999
0.000000
32.0308754470
120.91508499700
326.908997
32.0309977060
120.91509169800
172.662994
0.000000
32.0308839230
120.91507948100
331.130005
32.0309887870
120.91509335300
167.395004
0.000000
32.0308927370
120.91507461700
346.996002
32.0309799010
120.91509565100
168.294998
0.000000
32.0309702280
120.91509780900
172.145996
0.000000
32.0309611820
120.91509877900
176.160995
0.000000
32.0309514570
120.91509948100
176.261993
0.000000
32.0309423130
120.91510028200
174.684998
0.000000

296
routes/gps_load_1769059102539.txt
View File

@ -1,296 +0,0 @@
32.0309031720
120.91507456700
358.903015
0.000000
32.0309122700
120.91507465100
1.824000
0.000000
32.0309218300
120.91507536900
5.379000
0.000000
32.0309309300
120.91507646200
9.029000
0.000000
32.0309399620
120.91507767700
8.762000
0.000000
32.0309491970
120.91507969400
12.384000
0.000000
32.0309588180
120.91508216100
13.471000
0.000000
32.0309675640
120.91508480500
17.896000
0.000000
32.0309766450
120.91508880400
22.101999
0.000000
32.0309848880
120.91509322200
26.379999
0.000000
32.0309930520
120.91509823000
30.480000
0.000000
32.0310012620
120.91510437600
33.999001
0.000000
32.0310087320
120.91511028400
34.645000
0.000000
32.0310163110
120.91511626500
36.090000
0.000000
32.0310240180
120.91512326500
37.971001
0.000000
32.0310317340
120.91513052000
39.084000
0.000000
32.0310391000
120.91513778100
41.595001
0.000000
32.0310459690
120.91514589500
51.403000
0.000000
32.0310509040
120.91515476500
65.014999
0.000000
32.0310540470
120.91516559800
83.711998
0.000000
32.0310540770
120.91517649900
98.914001
0.000000
32.0310514290
120.91518685500
114.102997
0.000000
32.0310469800
120.91519624200
130.358994
0.000000
32.0310399570
120.91520416900
143.240005
0.000000
32.0310318070
120.91521042600
154.128006
0.000000
32.0310236210
120.91521480300
156.352997
0.000000
32.0310153520
120.91521894700
156.727997
0.000000
32.0310068890
120.91522295900
162.268997
0.000000
32.0309976280
120.91522578700
167.934006
0.000000
32.0309887430
120.91522755900
171.403000
0.000000
32.0309798350
120.91522898000
172.026993
0.000000
32.0309701240
120.91523061000
173.535004
0.000000
32.0309608820
120.91523161700
174.781998
0.000000
32.0309511450
120.91523278100
173.095001
0.000000
32.0309414590
120.91523408100
172.738007
0.000000
32.0309317890
120.91523561300
172.759995
0.000000
32.0309220880
120.91523714100
171.729996
0.000000
32.0309124620
120.91523885800
171.533005
0.000000
32.0309035530
120.91524062100
170.462006
0.000000
32.0308939330
120.91524243500
173.511993
0.000000
32.0308849020
120.91524366900
174.072998
0.000000
32.0308758540
120.91524473900
174.837006
0.000000
32.0308668250
120.91524554200
175.477005
0.000000
32.0308571550
120.91524639000
176.475998
0.000000
32.0308481530
120.91524702000
181.354996
0.000000
32.0308390220
120.91524675200
181.582001
0.000000
32.0308293150
120.91524646800
181.154007
0.000000
32.0308195650
120.91524630200
181.212997
0.000000
32.0308105960
120.91524531800
190.882996
0.000000
32.0308017000
120.91524309800
196.507996
0.000000
32.0307927000
120.91523864000
215.287994
0.000000
32.0307854930
120.91523117700
234.037994
0.000000
32.0307810700
120.91522120300
251.423996
0.000000
32.0307787600
120.91521000500
261.298004
0.000000
32.0307778120
120.91519940900
266.550995
0.000000
32.0307773010
120.91518860600
267.316986
0.000000
32.0307770810
120.91517787700
274.162994
0.000000
32.0307783650
120.91516734800
287.984009
0.000000
32.0307823320
120.91515692700
303.756989
0.000000
32.0307878050
120.91514836000
312.296997
0.000000
32.0307945510
120.91514116000
327.027008
0.000000
32.0308022350
120.91513533100
327.855988
0.000000
32.0308099090
120.91512965100
327.330994
0.000000
32.0308180980
120.91512338900
326.914001
0.000000
32.0308265120
120.91511760300
333.226013
0.000000
32.0308350690
120.91511249800
332.450012
0.000000
32.0308436950
120.91510715800
331.259003
0.000000
32.0308516230
120.91510207100
330.828003
0.000000
32.0308596170
120.91509688700
329.868988
0.000000
32.0308675160
120.91509126400
328.059998
0.000000
32.0308754470
120.91508499700
326.908997
0.000000
32.0308839230
120.91507948100
331.130005
0.000000
32.0308927370
120.91507461700
346.996002
0.000000
32.0309016030
120.91507282800
355.095001
0.000000

40
src/control/remote_ctrl/src/remote_ctrl_node.cpp
View File

@ -180,31 +180,32 @@ class RemoteCtrlNode : public rclcpp::Node
void publishMcCtrlMsg()
{
// 调试日志：显示当前授权状态
bool authorized = remote_authorized_.load(std::memory_order_acquire);
bool alive = remote_alive_.load(std::memory_order_relaxed);
LOG_DEBUG("[REMOTE] publishMcCtrlMsg: authorized=%d, alive=%d", authorized, alive);
// 未授权时：完全不发
if (!authorized)
if (!remote_authorized_.load(std::memory_order_acquire))
{
LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message");
return;
}
// 已授权但超时：完全不发
if (!alive)
{
LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message");
return;
}
sweeperMsg::McCtrl msg;
{
std::lock_guard<std::mutex> lock(state_mtx_);
// 已授权但超时：安全停车状态
if (!remote_alive_.load(std::memory_order_relaxed))
{
LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - sending safe-stop message");
msg.gear = desired_.gear;
msg.brake = 1;
msg.rpm = 0;
msg.angle = 0.0f;
msg.angle_speed = 120;
msg.sweep = false;
}
// 正常远控
else
{
LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message");
msg.gear = desired_.gear;
msg.brake = desired_.brake;
@ -213,16 +214,18 @@ class RemoteCtrlNode : public rclcpp::Node
msg.angle_speed = desired_.angle_speed;
msg.sweep = desired_.sweep;
}
}
LOG_INFO("[REMOTE] Publishing McCtrl: gear=%d brake=%d rpm=%d angle=%.1f angle_speed=%u sweep=%d", msg.gear,
msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep);
pub_->publish(msg);
LOG_DEBUG("[REMOTE] Message published successfully");
}
void onMqttMessage(const std::string& payload)
{
std::lock_guard<std::mutex> lock(state_mtx_);
try
{
auto j = json::parse(payload);
@ -251,8 +254,6 @@ class RemoteCtrlNode : public rclcpp::Node
{
int mode = value.get<int>();
{
std::lock_guard<std::mutex> lock(state_mtx_);
if (mode == 3)
{
remote_authorized_.store(true, std::memory_order_release);
@ -264,7 +265,6 @@ class RemoteCtrlNode : public rclcpp::Node
remote_alive_.store(false, std::memory_order_relaxed);
desired_ = {};
}
}
publishMcCtrlMsg();
return;
@ -290,11 +290,6 @@ class RemoteCtrlNode : public rclcpp::Node
LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
// =====================================================
// 处理具体指令
// =====================================================
{
std::lock_guard<std::mutex> lock(state_mtx_);
// =====================================================
// drive 指令
// =====================================================
@ -374,7 +369,6 @@ class RemoteCtrlNode : public rclcpp::Node
LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str());
return;
}
}
publishMcCtrlMsg();
}