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

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

@ -180,49 +180,52 @@ class RemoteCtrlNode : public rclcpp::Node
void publishMcCtrlMsg() 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"); LOG_WARN("[REMOTE] NOT AUTHORIZED - will NOT publish message");
return; return;
} }
// 已授权但超时：完全不发
if (!alive)
{
LOG_WARN("[REMOTE] REMOTE ALIVE=FALSE - will NOT publish message");
return;
}
sweeperMsg::McCtrl msg; sweeperMsg::McCtrl msg;
{ {
std::lock_guard<std::mutex> lock(state_mtx_); 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;
}
// 正常远控 // 正常远控
LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message"); else
msg.gear = desired_.gear; {
msg.brake = desired_.brake; LOG_INFO("[REMOTE] REMOTE ALIVE=TRUE - sending normal control message");
msg.rpm = desired_.rpm; msg.gear = desired_.gear;
msg.angle = desired_.angle; msg.brake = desired_.brake;
msg.angle_speed = desired_.angle_speed; msg.rpm = desired_.rpm;
msg.sweep = desired_.sweep; msg.angle = desired_.angle;
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, 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); msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep);
pub_->publish(msg); pub_->publish(msg);
LOG_DEBUG("[REMOTE] Message published successfully");
} }
void onMqttMessage(const std::string& payload) void onMqttMessage(const std::string& payload)
{ {
std::lock_guard<std::mutex> lock(state_mtx_);
try try
{ {
auto j = json::parse(payload); auto j = json::parse(payload);
@ -251,19 +254,16 @@ class RemoteCtrlNode : public rclcpp::Node
{ {
int mode = value.get<int>(); int mode = value.get<int>();
if (mode == 3)
{ {
std::lock_guard<std::mutex> lock(state_mtx_); remote_authorized_.store(true, std::memory_order_release);
if (mode == 3) remote_alive_.store(true, std::memory_order_relaxed);
{ }
remote_authorized_.store(true, std::memory_order_release); else if (mode == 0)
remote_alive_.store(true, std::memory_order_relaxed); {
} remote_authorized_.store(false, std::memory_order_release);
else if (mode == 0) remote_alive_.store(false, std::memory_order_relaxed);
{ desired_ = {};
remote_authorized_.store(false, std::memory_order_release);
remote_alive_.store(false, std::memory_order_relaxed);
desired_ = {};
}
} }
publishMcCtrlMsg(); publishMcCtrlMsg();
@ -291,89 +291,83 @@ class RemoteCtrlNode : public rclcpp::Node
LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str()); LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
// ===================================================== // =====================================================
// 处理具体指令 // drive 指令
// ===================================================== // =====================================================
if (cmd == "drive")
{ {
std::lock_guard<std::mutex> lock(state_mtx_); int throttle = 0;
// ===================================================== int brake = 1;
// drive 指令 int steer_raw = 0;
// =====================================================
if (cmd == "drive") if (value.is_string())
{ {
int throttle = 0; sscanf(value.get<std::string>().c_str(), "%d,%d,%d", &throttle, &brake, &steer_raw);
int brake = 1;
int steer_raw = 0;
if (value.is_string())
{
sscanf(value.get<std::string>().c_str(), "%d,%d,%d", &throttle, &brake, &steer_raw);
}
else
{
LOG_WARN("[REMOTE] drive value is not string");
return;
}
LOG_DEBUG("[REMOTE] drive: throttle=%d brake=%d steer=%d", throttle, brake, steer_raw);
if (brake > 0)
{
desired_.brake = 1;
desired_.rpm = 0;
desired_.angle = 0.0f;
}
else
{
desired_.brake = 0;
desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U;
float target_angle = (static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f;
float delta = target_angle - current_feedback_angle_;
float angle_speed_deg = std::fabs(delta) / DELTA_T;
float motor_rpm = angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
desired_.angle_speed = std::clamp<uint16_t>(static_cast<uint16_t>(motor_rpm), 120, 1500);
desired_.angle = target_angle;
}
}
// =====================================================
// gear 指令
// =====================================================
else if (cmd == "gear")
{
int gear = value.get<int>();
LOG_DEBUG("[REMOTE] gear command: %d", gear);
switch (gear)
{
case 0:
desired_.gear = 0; // N
break;
case 1:
desired_.gear = 2; // D
break;
case 2:
desired_.gear = 1; // R
break;
default:
desired_.gear = 0;
break;
}
}
// =====================================================
// sweepCtrl 指令
// =====================================================
else if (cmd == "sweepCtrl")
{
int val = value.get<int>();
desired_.sweep = (val != 0);
LOG_DEBUG("[REMOTE] sweepCtrl: %d", val);
} }
else else
{ {
LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str()); LOG_WARN("[REMOTE] drive value is not string");
return; return;
} }
LOG_DEBUG("[REMOTE] drive: throttle=%d brake=%d steer=%d", throttle, brake, steer_raw);
if (brake > 0)
{
desired_.brake = 1;
desired_.rpm = 0;
desired_.angle = 0.0f;
}
else
{
desired_.brake = 0;
desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U;
float target_angle = (static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f;
float delta = target_angle - current_feedback_angle_;
float angle_speed_deg = std::fabs(delta) / DELTA_T;
float motor_rpm = angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO;
desired_.angle_speed = std::clamp<uint16_t>(static_cast<uint16_t>(motor_rpm), 120, 1500);
desired_.angle = target_angle;
}
}
// =====================================================
// gear 指令
// =====================================================
else if (cmd == "gear")
{
int gear = value.get<int>();
LOG_DEBUG("[REMOTE] gear command: %d", gear);
switch (gear)
{
case 0:
desired_.gear = 0; // N
break;
case 1:
desired_.gear = 2; // D
break;
case 2:
desired_.gear = 1; // R
break;
default:
desired_.gear = 0;
break;
}
}
// =====================================================
// sweepCtrl 指令
// =====================================================
else if (cmd == "sweepCtrl")
{
int val = value.get<int>();
desired_.sweep = (val != 0);
LOG_DEBUG("[REMOTE] sweepCtrl: %d", val);
}
else
{
LOG_WARN("[REMOTE] Unknown command: %s", cmd.c_str());
return;
} }
publishMcCtrlMsg(); publishMcCtrlMsg();