Compare commits
4 Commits
466a615378
...
098e67d798
| Author | SHA1 | Date | |
|---|---|---|---|
| 098e67d798 | |||
| 4e6b05eb3f | |||
| 8edaa28967 | |||
| 886b6b6de3 |
632
routes/gps_load_1768981254899.txt
Normal file
632
routes/gps_load_1768981254899.txt
Normal file
@ -0,0 +1,632 @@
|
||||
32.0309059220
|
||||
120.91507453600
|
||||
17.753000
|
||||
0.000000
|
||||
32.0309148270
|
||||
120.91507776600
|
||||
17.525999
|
||||
0.000000
|
||||
32.0309236310
|
||||
120.91508057300
|
||||
17.218000
|
||||
0.000000
|
||||
32.0309327390
|
||||
120.91508386700
|
||||
17.608000
|
||||
0.000000
|
||||
32.0309416330
|
||||
120.91508699800
|
||||
17.511000
|
||||
0.000000
|
||||
32.0309504650
|
||||
120.91509028100
|
||||
18.521000
|
||||
0.000000
|
||||
32.0309595190
|
||||
120.91509363200
|
||||
17.833000
|
||||
0.000000
|
||||
32.0309684230
|
||||
120.91509729700
|
||||
20.240999
|
||||
0.000000
|
||||
32.0309774410
|
||||
120.91510135000
|
||||
21.204000
|
||||
0.000000
|
||||
32.0309859680
|
||||
120.91510527100
|
||||
21.841999
|
||||
0.000000
|
||||
32.0309943390
|
||||
120.91510941700
|
||||
23.318001
|
||||
0.000000
|
||||
32.0310032190
|
||||
120.91511407400
|
||||
27.270000
|
||||
0.000000
|
||||
32.0310116700
|
||||
120.91511970300
|
||||
30.927000
|
||||
0.000000
|
||||
32.0310201800
|
||||
120.91512568400
|
||||
31.695000
|
||||
0.000000
|
||||
32.0310284370
|
||||
120.91513180500
|
||||
32.013000
|
||||
0.000000
|
||||
32.0310366160
|
||||
120.91513795800
|
||||
33.563999
|
||||
0.000000
|
||||
32.0310447580
|
||||
120.91514428000
|
||||
34.560001
|
||||
0.000000
|
||||
32.0310521640
|
||||
120.91515038300
|
||||
35.237000
|
||||
0.000000
|
||||
32.0310601020
|
||||
120.91515715000
|
||||
36.064999
|
||||
0.000000
|
||||
32.0310674000
|
||||
120.91516351500
|
||||
38.910999
|
||||
0.000000
|
||||
32.0310741240
|
||||
120.91517079600
|
||||
43.208000
|
||||
0.000000
|
||||
32.0310807950
|
||||
120.91517838600
|
||||
43.783001
|
||||
0.000000
|
||||
32.0310872180
|
||||
120.91518587100
|
||||
44.694000
|
||||
0.000000
|
||||
32.0310936010
|
||||
120.91519343700
|
||||
45.299999
|
||||
0.000000
|
||||
32.0311007320
|
||||
120.91520131400
|
||||
38.423000
|
||||
0.000000
|
||||
32.0311083860
|
||||
120.91520835100
|
||||
38.085999
|
||||
0.000000
|
||||
32.0311161270
|
||||
120.91521541400
|
||||
37.209000
|
||||
0.000000
|
||||
32.0311232950
|
||||
120.91522199300
|
||||
37.362000
|
||||
0.000000
|
||||
32.0311310550
|
||||
120.91522896000
|
||||
35.877998
|
||||
0.000000
|
||||
32.0311382810
|
||||
120.91523533600
|
||||
35.492001
|
||||
0.000000
|
||||
32.0311461850
|
||||
120.91524203800
|
||||
35.508999
|
||||
0.000000
|
||||
32.0311535190
|
||||
120.91524821200
|
||||
38.588001
|
||||
0.000000
|
||||
32.0311604720
|
||||
120.91525511500
|
||||
40.641998
|
||||
0.000000
|
||||
32.0311671790
|
||||
120.91526228800
|
||||
43.164001
|
||||
0.000000
|
||||
32.0311739140
|
||||
120.91527055200
|
||||
46.932999
|
||||
0.000000
|
||||
32.0311802090
|
||||
120.91527889700
|
||||
49.226002
|
||||
0.000000
|
||||
32.0311864270
|
||||
120.91528781800
|
||||
51.997002
|
||||
0.000000
|
||||
32.0311919750
|
||||
120.91529620400
|
||||
53.888000
|
||||
0.000000
|
||||
32.0311970510
|
||||
120.91530500100
|
||||
58.710999
|
||||
0.000000
|
||||
32.0312017590
|
||||
120.91531489700
|
||||
64.824997
|
||||
0.000000
|
||||
32.0312055030
|
||||
120.91532550700
|
||||
70.234001
|
||||
0.000000
|
||||
32.0312085640
|
||||
120.91533638200
|
||||
74.036003
|
||||
0.000000
|
||||
32.0312109030
|
||||
120.91534742100
|
||||
78.997002
|
||||
0.000000
|
||||
32.0312123390
|
||||
120.91535796700
|
||||
83.719002
|
||||
0.000000
|
||||
32.0312131810
|
||||
120.91536929700
|
||||
86.478996
|
||||
0.000000
|
||||
32.0312136050
|
||||
120.91538077100
|
||||
88.044998
|
||||
0.000000
|
||||
32.0312137320
|
||||
120.91539148000
|
||||
89.156998
|
||||
0.000000
|
||||
32.0312138820
|
||||
120.91540299500
|
||||
88.689003
|
||||
0.000000
|
||||
32.0312141080
|
||||
120.91541366300
|
||||
87.801003
|
||||
0.000000
|
||||
32.0312143330
|
||||
120.91542426200
|
||||
87.367996
|
||||
0.000000
|
||||
32.0312147380
|
||||
120.91543489800
|
||||
87.220001
|
||||
0.000000
|
||||
32.0312150710
|
||||
120.91544615700
|
||||
88.532997
|
||||
0.000000
|
||||
32.0312151700
|
||||
120.91545780100
|
||||
89.377998
|
||||
0.000000
|
||||
32.0312153040
|
||||
120.91546932300
|
||||
87.929001
|
||||
0.000000
|
||||
32.0312155460
|
||||
120.91547994300
|
||||
87.074997
|
||||
0.000000
|
||||
32.0312160640
|
||||
120.91549141800
|
||||
86.186996
|
||||
0.000000
|
||||
32.0312166750
|
||||
120.91550289300
|
||||
85.649002
|
||||
0.000000
|
||||
32.0312173150
|
||||
120.91551347800
|
||||
86.931999
|
||||
0.000000
|
||||
32.0312177400
|
||||
120.91552505000
|
||||
86.444000
|
||||
0.000000
|
||||
32.0312183350
|
||||
120.91553650500
|
||||
85.163002
|
||||
0.000000
|
||||
32.0312191160
|
||||
120.91554711300
|
||||
86.652000
|
||||
0.000000
|
||||
32.0312195390
|
||||
120.91555856800
|
||||
86.635002
|
||||
0.000000
|
||||
32.0312200820
|
||||
120.91557008600
|
||||
86.950996
|
||||
0.000000
|
||||
32.0312206390
|
||||
120.91558155400
|
||||
86.192001
|
||||
0.000000
|
||||
32.0312212560
|
||||
120.91559215900
|
||||
86.212997
|
||||
0.000000
|
||||
32.0312218250
|
||||
120.91560365700
|
||||
86.130997
|
||||
0.000000
|
||||
32.0312224570
|
||||
120.91561511900
|
||||
86.133003
|
||||
0.000000
|
||||
32.0312230080
|
||||
120.91562663800
|
||||
87.842003
|
||||
0.000000
|
||||
32.0312232550
|
||||
120.91563816300
|
||||
89.364998
|
||||
0.000000
|
||||
32.0312233280
|
||||
120.91564881200
|
||||
89.200996
|
||||
0.000000
|
||||
32.0312234440
|
||||
120.91566023400
|
||||
88.639999
|
||||
0.000000
|
||||
32.0312236340
|
||||
120.91567083200
|
||||
87.695000
|
||||
0.000000
|
||||
32.0312240270
|
||||
120.91568233400
|
||||
87.403000
|
||||
0.000000
|
||||
32.0312243780
|
||||
120.91569294900
|
||||
87.195000
|
||||
0.000000
|
||||
32.0312248570
|
||||
120.91570442700
|
||||
85.874001
|
||||
0.000000
|
||||
32.0312254770
|
||||
120.91571499700
|
||||
86.217003
|
||||
0.000000
|
||||
32.0312261490
|
||||
120.91572650800
|
||||
85.580002
|
||||
0.000000
|
||||
32.0312267750
|
||||
120.91573709100
|
||||
85.873001
|
||||
0.000000
|
||||
32.0312274030
|
||||
120.91574857800
|
||||
86.735001
|
||||
0.000000
|
||||
32.0312278540
|
||||
120.91576005600
|
||||
87.473000
|
||||
0.000000
|
||||
32.0312282120
|
||||
120.91577064300
|
||||
87.811996
|
||||
0.000000
|
||||
32.0312284600
|
||||
120.91578219900
|
||||
88.080002
|
||||
0.000000
|
||||
32.0312287020
|
||||
120.91579362400
|
||||
88.499001
|
||||
0.000000
|
||||
32.0312288700
|
||||
120.91580507900
|
||||
88.637001
|
||||
0.000000
|
||||
32.0312291280
|
||||
120.91581668800
|
||||
88.600998
|
||||
0.000000
|
||||
32.0312295050
|
||||
120.91582823300
|
||||
86.709000
|
||||
0.000000
|
||||
32.0312298770
|
||||
120.91583972000
|
||||
86.411003
|
||||
0.000000
|
||||
32.0312304810
|
||||
120.91585115000
|
||||
85.708000
|
||||
0.000000
|
||||
32.0312311600
|
||||
120.91586174700
|
||||
87.033997
|
||||
0.000000
|
||||
32.0312313880
|
||||
120.91587237100
|
||||
89.141998
|
||||
0.000000
|
||||
32.0312313680
|
||||
120.91588313400
|
||||
85.974998
|
||||
0.000000
|
||||
32.0312322530
|
||||
120.91589430000
|
||||
84.378998
|
||||
0.000000
|
||||
32.0312331030
|
||||
120.91590550400
|
||||
85.447998
|
||||
0.000000
|
||||
32.0312337200
|
||||
120.91591623100
|
||||
86.781998
|
||||
0.000000
|
||||
32.0312341560
|
||||
120.91592685600
|
||||
86.746002
|
||||
0.000000
|
||||
32.0312347330
|
||||
120.91593756900
|
||||
86.496002
|
||||
0.000000
|
||||
32.0312353150
|
||||
120.91594891900
|
||||
86.263000
|
||||
0.000000
|
||||
32.0312359390
|
||||
120.91595981500
|
||||
85.277000
|
||||
0.000000
|
||||
32.0312366530
|
||||
120.91597135300
|
||||
84.977997
|
||||
0.000000
|
||||
32.0312375340
|
||||
120.91598281000
|
||||
84.515999
|
||||
0.000000
|
||||
32.0312384120
|
||||
120.91599336000
|
||||
85.305000
|
||||
0.000000
|
||||
32.0312389410
|
||||
120.91600396900
|
||||
86.956001
|
||||
0.000000
|
||||
32.0312393590
|
||||
120.91601538600
|
||||
87.115997
|
||||
0.000000
|
||||
32.0312398990
|
||||
120.91602678000
|
||||
86.516998
|
||||
0.000000
|
||||
32.0312404200
|
||||
120.91603735900
|
||||
85.605003
|
||||
0.000000
|
||||
32.0312410700
|
||||
120.91604866000
|
||||
85.514999
|
||||
0.000000
|
||||
32.0312418440
|
||||
120.91605932200
|
||||
84.931000
|
||||
0.000000
|
||||
32.0312425240
|
||||
120.91607070600
|
||||
86.876999
|
||||
0.000000
|
||||
32.0312429500
|
||||
120.91608222000
|
||||
87.763000
|
||||
0.000000
|
||||
32.0312431370
|
||||
120.91609374700
|
||||
87.617996
|
||||
0.000000
|
||||
32.0312436760
|
||||
120.91610519700
|
||||
86.650002
|
||||
0.000000
|
||||
32.0312441960
|
||||
120.91611580500
|
||||
86.314003
|
||||
0.000000
|
||||
32.0312448280
|
||||
120.91612729300
|
||||
85.889999
|
||||
0.000000
|
||||
32.0312455840
|
||||
120.91613880200
|
||||
85.323997
|
||||
0.000000
|
||||
32.0312463040
|
||||
120.91615025300
|
||||
85.443001
|
||||
0.000000
|
||||
32.0312469720
|
||||
120.91616173100
|
||||
87.385002
|
||||
0.000000
|
||||
32.0312472400
|
||||
120.91617322400
|
||||
87.737000
|
||||
0.000000
|
||||
32.0312476780
|
||||
120.91618465800
|
||||
86.783997
|
||||
0.000000
|
||||
32.0312482210
|
||||
120.91619618400
|
||||
86.132004
|
||||
0.000000
|
||||
32.0312488670
|
||||
120.91620773300
|
||||
85.084000
|
||||
0.000000
|
||||
32.0312495330
|
||||
120.91621909100
|
||||
84.349998
|
||||
0.000000
|
||||
32.0312503240
|
||||
120.91622970500
|
||||
86.352997
|
||||
0.000000
|
||||
32.0312508580
|
||||
120.91624118300
|
||||
87.614998
|
||||
0.000000
|
||||
32.0312511870
|
||||
120.91625269600
|
||||
87.379997
|
||||
0.000000
|
||||
32.0312516130
|
||||
120.91626412400
|
||||
87.073997
|
||||
0.000000
|
||||
32.0312521140
|
||||
120.91627474700
|
||||
87.023003
|
||||
0.000000
|
||||
32.0312526240
|
||||
120.91628616900
|
||||
86.621002
|
||||
0.000000
|
||||
32.0312531820
|
||||
120.91629766400
|
||||
86.105003
|
||||
0.000000
|
||||
32.0312538090
|
||||
120.91630916400
|
||||
85.706001
|
||||
0.000000
|
||||
32.0312543880
|
||||
120.91632068200
|
||||
87.724998
|
||||
0.000000
|
||||
32.0312546460
|
||||
120.91633132800
|
||||
88.665001
|
||||
0.000000
|
||||
32.0312547180
|
||||
120.91634203600
|
||||
88.679001
|
||||
0.000000
|
||||
32.0312549880
|
||||
120.91635345100
|
||||
87.136002
|
||||
0.000000
|
||||
32.0312553720
|
||||
120.91636404200
|
||||
87.433998
|
||||
0.000000
|
||||
32.0312558060
|
||||
120.91637468500
|
||||
86.809998
|
||||
0.000000
|
||||
32.0312563790
|
||||
120.91638608600
|
||||
85.931000
|
||||
0.000000
|
||||
32.0312570560
|
||||
120.91639749100
|
||||
85.924004
|
||||
0.000000
|
||||
32.0312575200
|
||||
120.91640896300
|
||||
85.889999
|
||||
0.000000
|
||||
32.0312581010
|
||||
120.91642048400
|
||||
87.275002
|
||||
0.000000
|
||||
32.0312584050
|
||||
120.91643203800
|
||||
88.722000
|
||||
0.000000
|
||||
32.0312585420
|
||||
120.91644358800
|
||||
87.987000
|
||||
0.000000
|
||||
32.0312588670
|
||||
120.91645506400
|
||||
87.643997
|
||||
0.000000
|
||||
32.0312593360
|
||||
120.91646658100
|
||||
87.431000
|
||||
0.000000
|
||||
32.0312598290
|
||||
120.91647806800
|
||||
86.720001
|
||||
0.000000
|
||||
32.0312603720
|
||||
120.91648865300
|
||||
85.902000
|
||||
0.000000
|
||||
32.0312610920
|
||||
120.91649982200
|
||||
85.807999
|
||||
0.000000
|
||||
32.0312616930
|
||||
120.91651066200
|
||||
85.612999
|
||||
0.000000
|
||||
32.0312624190
|
||||
120.91652205000
|
||||
85.580002
|
||||
0.000000
|
||||
32.0312628980
|
||||
120.91653348800
|
||||
87.941002
|
||||
0.000000
|
||||
32.0312631520
|
||||
120.91654499600
|
||||
88.758003
|
||||
0.000000
|
||||
32.0312632690
|
||||
120.91655563300
|
||||
88.308998
|
||||
0.000000
|
||||
32.0312635020
|
||||
120.91656705500
|
||||
87.642998
|
||||
0.000000
|
||||
32.0312639160
|
||||
120.91657851900
|
||||
86.695999
|
||||
0.000000
|
||||
32.0312644430
|
||||
120.91659004000
|
||||
86.541000
|
||||
0.000000
|
||||
32.0312651230
|
||||
120.91660065700
|
||||
85.900002
|
||||
0.000000
|
||||
32.0312657190
|
||||
120.91661207400
|
||||
87.696999
|
||||
0.000000
|
||||
32.0312659540
|
||||
120.91662364000
|
||||
89.132004
|
||||
0.000000
|
||||
32.0312662160
|
||||
120.91663460400
|
||||
74.685997
|
||||
0.000000
|
||||
2932
routes/gps_load_1768981796615.txt
Normal file
2932
routes/gps_load_1768981796615.txt
Normal file
File diff suppressed because it is too large
Load Diff
@ -60,11 +60,14 @@ bool upload_file(string filename)
|
||||
curl = curl_easy_init();
|
||||
struct curl_httppost* post = NULL;
|
||||
struct curl_httppost* last = NULL;
|
||||
|
||||
// 只计算一次MD5值
|
||||
string md5_value = calculate_md5(filename);
|
||||
|
||||
curl_formadd(&post, &last, CURLFORM_PTRNAME, "vid", CURLFORM_PTRCONTENTS, vid.c_str(),
|
||||
CURLFORM_END); // form-data key(path) 和 value(device_cover)
|
||||
curl_formadd(&post, &last, CURLFORM_COPYNAME, "file", CURLFORM_FILE, filename.c_str(), CURLFORM_END);
|
||||
curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, calculate_md5(filename).c_str(),
|
||||
CURLFORM_END);
|
||||
curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, md5_value.c_str(), CURLFORM_END);
|
||||
curl_easy_setopt(curl, CURLOPT_URL, upload_URL.c_str());
|
||||
curl_easy_setopt(curl, CURLOPT_HTTPPOST, post);
|
||||
ret = curl_easy_perform(curl);
|
||||
|
||||
@ -62,7 +62,7 @@ void SubNode::timerCallback()
|
||||
mc.angle_speed = 120;
|
||||
}
|
||||
|
||||
pub_mc_->publish(mc);
|
||||
// pub_mc_->publish(mc);
|
||||
}
|
||||
|
||||
void SubNode::identityCallback(const sweeper_interfaces::msg::VehicleIdentity::SharedPtr msg)
|
||||
|
||||
@ -91,8 +91,7 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
try_subscribe_ctrl_topic();
|
||||
});
|
||||
|
||||
// 主动连一次，后面内部 loop 线程也会自动尝试重连
|
||||
mqtt_->connect();
|
||||
// MQTTClientWrapper内部已有reconnectLoop线程自动处理连接和重连，无需主动调用connect()
|
||||
|
||||
identity_sub_ = this->create_subscription<sweeper_interfaces::msg::VehicleIdentity>(
|
||||
"/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(),
|
||||
@ -119,7 +118,7 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
|
||||
LOG_INFO("RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), client_id.c_str());
|
||||
|
||||
// ===== 看门狗 & 定时发布 =====
|
||||
// ===== 看门狗 & 消息发布 =====
|
||||
last_msg_time_ = std::chrono::steady_clock::now();
|
||||
|
||||
// 看门狗：800ms 内没收到新 remote 指令则停控
|
||||
@ -142,43 +141,6 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
}
|
||||
});
|
||||
|
||||
// 周期发布到 /remote_mc_ctrl
|
||||
timer_ = this->create_wall_timer(20ms, // 50Hz
|
||||
[this]()
|
||||
{
|
||||
// 未授权远控：完全不发
|
||||
if (!remote_authorized_.load(std::memory_order_acquire)) return;
|
||||
|
||||
sweeperMsg::McCtrl msg;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mtx_);
|
||||
|
||||
// 已授权但超时：安全停车态
|
||||
if (!remote_alive_.load(std::memory_order_relaxed))
|
||||
{
|
||||
msg.gear = desired_.gear; // 通常保持挡位
|
||||
msg.brake = 1;
|
||||
msg.rpm = 0;
|
||||
msg.angle = 0.0f;
|
||||
msg.angle_speed = 120;
|
||||
msg.sweep = false;
|
||||
}
|
||||
// 正常远控
|
||||
else
|
||||
{
|
||||
msg.gear = desired_.gear;
|
||||
msg.brake = desired_.brake;
|
||||
msg.rpm = desired_.rpm;
|
||||
msg.angle = desired_.angle;
|
||||
msg.angle_speed = desired_.angle_speed;
|
||||
msg.sweep = desired_.sweep;
|
||||
}
|
||||
}
|
||||
|
||||
pub_->publish(msg);
|
||||
});
|
||||
|
||||
// CAN 反馈订阅，用于计算当前转向角
|
||||
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
|
||||
"can_data", 10,
|
||||
@ -216,36 +178,77 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
mqtt_->subscribe(ctrl_topic_, 1);
|
||||
}
|
||||
|
||||
void publishMcCtrlMsg()
|
||||
{
|
||||
// 未授权时：完全不发
|
||||
if (!remote_authorized_.load(std::memory_order_acquire))
|
||||
{
|
||||
LOG_WARN("[REMOTE] NOT AUTHORIZED - 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;
|
||||
msg.rpm = desired_.rpm;
|
||||
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,
|
||||
msg.brake, msg.rpm, msg.angle, msg.angle_speed, msg.sweep);
|
||||
|
||||
pub_->publish(msg);
|
||||
}
|
||||
|
||||
void onMqttMessage(const std::string& payload)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(state_mtx_);
|
||||
|
||||
// =====================================================
|
||||
// 1. MQTT 消息到达：刷新活性 & 判断是否从超时中恢复
|
||||
// =====================================================
|
||||
bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
|
||||
if (was_dead)
|
||||
{
|
||||
LOG_INFO("[REMOTE] control recovered");
|
||||
}
|
||||
|
||||
last_msg_time_ = std::chrono::steady_clock::now();
|
||||
|
||||
LOG_DEBUG("[REMOTE] recv MQTT payload: %s", payload.c_str());
|
||||
|
||||
try
|
||||
{
|
||||
auto j = json::parse(payload);
|
||||
if (!j.contains("data")) return;
|
||||
if (!j.contains("data"))
|
||||
{
|
||||
LOG_WARN("[REMOTE] Invalid JSON: missing 'data' field, payload=%s", payload.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
const auto& data = j["data"];
|
||||
if (!data.contains("command") || !data.contains("value")) return;
|
||||
if (!data.contains("command") || !data.contains("value"))
|
||||
{
|
||||
LOG_WARN("[REMOTE] Invalid JSON: missing 'command' or 'value' field");
|
||||
return;
|
||||
}
|
||||
|
||||
const std::string cmd = data["command"].get<std::string>();
|
||||
const auto& value = data["value"];
|
||||
|
||||
LOG_INFO("[REMOTE] Received command: %s", cmd.c_str());
|
||||
|
||||
// =====================================================
|
||||
// 2. mode 指令：唯一的“授权 / 取消授权”入口
|
||||
// mode 指令：唯一的授权入口
|
||||
// =====================================================
|
||||
if (cmd == "mode")
|
||||
{
|
||||
@ -253,35 +256,42 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
|
||||
if (mode == 3)
|
||||
{
|
||||
// 进入远控授权态（锁存）
|
||||
remote_authorized_.store(true, std::memory_order_release);
|
||||
remote_alive_.store(true, std::memory_order_relaxed);
|
||||
|
||||
LOG_INFO("[REMOTE] authorized (mode=3)");
|
||||
}
|
||||
else if (mode == 0)
|
||||
{
|
||||
// 退出远控授权态（锁存）
|
||||
remote_authorized_.store(false, std::memory_order_release);
|
||||
remote_alive_.store(false, std::memory_order_relaxed);
|
||||
|
||||
// 明确清空目标状态
|
||||
desired_ = {};
|
||||
|
||||
LOG_INFO("[REMOTE] deauthorized (mode=0)");
|
||||
}
|
||||
|
||||
// mode 指令处理完直接返回
|
||||
publishMcCtrlMsg();
|
||||
return;
|
||||
}
|
||||
|
||||
// =====================================================
|
||||
// 3. 非 mode 指令：只有在“已授权”状态下才处理
|
||||
// 非 mode 指令：检查授权
|
||||
// =====================================================
|
||||
if (!remote_authorized_.load(std::memory_order_acquire)) return;
|
||||
if (!remote_authorized_.load(std::memory_order_acquire))
|
||||
{
|
||||
LOG_WARN("[REMOTE] Command '%s' received but NOT AUTHORIZED - will not process", cmd.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
// 刷新活性
|
||||
bool was_dead = !remote_alive_.exchange(true, std::memory_order_relaxed);
|
||||
if (was_dead)
|
||||
{
|
||||
LOG_INFO("[REMOTE] ✓ control recovered");
|
||||
}
|
||||
|
||||
last_msg_time_ = std::chrono::steady_clock::now();
|
||||
|
||||
LOG_DEBUG("[REMOTE] Processing command: %s", cmd.c_str());
|
||||
|
||||
// =====================================================
|
||||
// 4. drive：油门 / 刹车 / 转向
|
||||
// drive 指令
|
||||
// =====================================================
|
||||
if (cmd == "drive")
|
||||
{
|
||||
@ -295,12 +305,14 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
}
|
||||
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;
|
||||
@ -308,28 +320,25 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
else
|
||||
{
|
||||
desired_.brake = 0;
|
||||
|
||||
// 油门映射：0~65535 -> 0~6000 rpm
|
||||
desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U;
|
||||
|
||||
// 转向映射：0~65535 -> -50~+50 度
|
||||
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;
|
||||
}
|
||||
}
|
||||
// =====================================================
|
||||
// 5. gear：挡位
|
||||
// gear 指令
|
||||
// =====================================================
|
||||
else if (cmd == "gear")
|
||||
{
|
||||
int gear = value.get<int>();
|
||||
LOG_DEBUG("[REMOTE] gear command: %d", gear);
|
||||
|
||||
switch (gear)
|
||||
{
|
||||
case 0:
|
||||
@ -347,16 +356,26 @@ class RemoteCtrlNode : public rclcpp::Node
|
||||
}
|
||||
}
|
||||
// =====================================================
|
||||
// 6. sweepCtrl：清扫开关
|
||||
// sweepCtrl 指令
|
||||
// =====================================================
|
||||
else if (cmd == "sweepCtrl")
|
||||
{
|
||||
desired_.sweep = (value.get<int>() != 0);
|
||||
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();
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
LOG_WARN("[REMOTE] JSON parse error: %s", e.what());
|
||||
LOG_ERROR("[REMOTE] Exception in onMqttMessage: %s, payload=%s", e.what(), payload.c_str());
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user