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

Compare commits

base: lyq:098e67d7983b5a410cef0982c52f186b8e75011e
Branches Tags
lyq:master
lyq:humble
..
compare: lyq:466a6153781b4598875ec951a7bc1f4857051be0
Branches Tags
lyq:humble
lyq:master

No commits in common. "098e67d7983b5a410cef0982c52f186b8e75011e" and "466a6153781b4598875ec951a7bc1f4857051be0" have entirely different histories.
098e67d798 ... 466a615378

5 changed files with 81 additions and 3667 deletions
Show Stats Expand all files Collapse all files

632
routes/gps_load_1768981254899.txt
View File

@ -1,632 +0,0 @@
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
View File

File diff suppressed because it is too large Load Diff

7
src/autonomy/route/src/route_node.cpp
View File

@ -60,14 +60,11 @@ bool upload_file(string filename)
curl = curl_easy_init(); curl = curl_easy_init();
struct curl_httppost* post = NULL; struct curl_httppost* post = NULL;
struct curl_httppost* last = 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(), curl_formadd(&post, &last, CURLFORM_PTRNAME, "vid", CURLFORM_PTRCONTENTS, vid.c_str(),
CURLFORM_END); // form-data key(path) 和 value(device_cover) 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, "file", CURLFORM_FILE, filename.c_str(), CURLFORM_END);
curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, md5_value.c_str(), CURLFORM_END); curl_formadd(&post, &last, CURLFORM_COPYNAME, "md5", CURLFORM_COPYCONTENTS, calculate_md5(filename).c_str(),
CURLFORM_END);
curl_easy_setopt(curl, CURLOPT_URL, upload_URL.c_str()); curl_easy_setopt(curl, CURLOPT_URL, upload_URL.c_str());
curl_easy_setopt(curl, CURLOPT_HTTPPOST, post); curl_easy_setopt(curl, CURLOPT_HTTPPOST, post);
ret = curl_easy_perform(curl); ret = curl_easy_perform(curl);

2
src/communication/sub/src/sub_node.cpp
View File

@ -62,7 +62,7 @@ void SubNode::timerCallback()
mc.angle_speed = 120; mc.angle_speed = 120;
} }
// pub_mc_->publish(mc); pub_mc_->publish(mc);
} }
void SubNode::identityCallback(const sweeper_interfaces::msg::VehicleIdentity::SharedPtr msg) void SubNode::identityCallback(const sweeper_interfaces::msg::VehicleIdentity::SharedPtr msg)

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

@ -91,7 +91,8 @@ class RemoteCtrlNode : public rclcpp::Node
try_subscribe_ctrl_topic(); try_subscribe_ctrl_topic();
}); });
// MQTTClientWrapper内部已有reconnectLoop线程自动处理连接和重连，无需主动调用connect() // 主动连一次，后面内部 loop 线程也会自动尝试重连
mqtt_->connect();
identity_sub_ = this->create_subscription<sweeper_interfaces::msg::VehicleIdentity>( identity_sub_ = this->create_subscription<sweeper_interfaces::msg::VehicleIdentity>(
"/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(), "/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(),
@ -118,7 +119,7 @@ class RemoteCtrlNode : public rclcpp::Node
LOG_INFO("RemoteCtrlNode started mqtt=%s client_id=%s", uri.str().c_str(), client_id.c_str()); 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(); last_msg_time_ = std::chrono::steady_clock::now();
// 看门狗：800ms 内没收到新 remote 指令则停控 // 看门狗：800ms 内没收到新 remote 指令则停控
@ -141,6 +142,43 @@ 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 反馈订阅，用于计算当前转向角
can_sub_ = this->create_subscription<sweeperMsg::CanFrame>( can_sub_ = this->create_subscription<sweeperMsg::CanFrame>(
"can_data", 10, "can_data", 10,
@ -178,77 +216,36 @@ class RemoteCtrlNode : public rclcpp::Node
mqtt_->subscribe(ctrl_topic_, 1); 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) void onMqttMessage(const std::string& payload)
{ {
std::lock_guard<std::mutex> lock(state_mtx_); 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 try
{ {
auto j = json::parse(payload); auto j = json::parse(payload);
if (!j.contains("data")) if (!j.contains("data")) return;
{
LOG_WARN("[REMOTE] Invalid JSON: missing 'data' field, payload=%s", payload.c_str());
return;
}
const auto& data = j["data"]; const auto& data = j["data"];
if (!data.contains("command") || !data.contains("value")) if (!data.contains("command") || !data.contains("value")) return;
{
LOG_WARN("[REMOTE] Invalid JSON: missing 'command' or 'value' field");
return;
}
const std::string cmd = data["command"].get<std::string>(); const std::string cmd = data["command"].get<std::string>();
const auto& value = data["value"]; const auto& value = data["value"];
LOG_INFO("[REMOTE] Received command: %s", cmd.c_str());
// ===================================================== // =====================================================
// mode 指令：唯一的授权入口 // 2. mode 指令：唯一的“授权 / 取消授权”入口
// ===================================================== // =====================================================
if (cmd == "mode") if (cmd == "mode")
{ {
@ -256,42 +253,35 @@ class RemoteCtrlNode : public rclcpp::Node
if (mode == 3) if (mode == 3)
{ {
// 进入远控授权态（锁存）
remote_authorized_.store(true, std::memory_order_release); remote_authorized_.store(true, std::memory_order_release);
remote_alive_.store(true, std::memory_order_relaxed); remote_alive_.store(true, std::memory_order_relaxed);
LOG_INFO("[REMOTE] authorized (mode=3)");
} }
else if (mode == 0) else if (mode == 0)
{ {
// 退出远控授权态（锁存）
remote_authorized_.store(false, std::memory_order_release); remote_authorized_.store(false, std::memory_order_release);
remote_alive_.store(false, std::memory_order_relaxed); remote_alive_.store(false, std::memory_order_relaxed);
// 明确清空目标状态
desired_ = {}; desired_ = {};
LOG_INFO("[REMOTE] deauthorized (mode=0)");
} }
publishMcCtrlMsg(); // mode 指令处理完直接返回
return; return;
} }
// ===================================================== // =====================================================
// 非 mode 指令：检查授权 // 3. 非 mode 指令：只有在“已授权”状态下才处理
// ===================================================== // =====================================================
if (!remote_authorized_.load(std::memory_order_acquire)) if (!remote_authorized_.load(std::memory_order_acquire)) return;
{
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());
// ===================================================== // =====================================================
// drive 指令 // 4. drive：油门 / 刹车 / 转向
// ===================================================== // =====================================================
if (cmd == "drive") if (cmd == "drive")
{ {
@ -305,14 +295,12 @@ class RemoteCtrlNode : public rclcpp::Node
} }
else else
{ {
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) if (brake > 0)
{ {
// 刹车优先
desired_.brake = 1; desired_.brake = 1;
desired_.rpm = 0; desired_.rpm = 0;
desired_.angle = 0.0f; desired_.angle = 0.0f;
@ -320,25 +308,28 @@ class RemoteCtrlNode : public rclcpp::Node
else else
{ {
desired_.brake = 0; desired_.brake = 0;
// 油门映射：0~65535 -> 0~6000 rpm
desired_.rpm = (static_cast<uint32_t>(throttle) * 6000U) / 65535U; 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 target_angle = (static_cast<float>(steer_raw) * 100.0f / 65535.0f) - 50.0f;
float delta = target_angle - current_feedback_angle_; float delta = target_angle - current_feedback_angle_;
float angle_speed_deg = std::fabs(delta) / DELTA_T; float angle_speed_deg = std::fabs(delta) / DELTA_T;
float motor_rpm = angle_speed_deg * DEG_PER_SEC_TO_RPM * GEAR_RATIO; 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_speed = std::clamp<uint16_t>(static_cast<uint16_t>(motor_rpm), 120, 1500);
desired_.angle = target_angle; desired_.angle = target_angle;
} }
} }
// ===================================================== // =====================================================
// gear 指令 // 5. gear：挡位
// ===================================================== // =====================================================
else if (cmd == "gear") else if (cmd == "gear")
{ {
int gear = value.get<int>(); int gear = value.get<int>();
LOG_DEBUG("[REMOTE] gear command: %d", gear);
switch (gear) switch (gear)
{ {
case 0: case 0:
@ -356,26 +347,16 @@ class RemoteCtrlNode : public rclcpp::Node
} }
} }
// ===================================================== // =====================================================
// sweepCtrl 指令 // 6. sweepCtrl：清扫开关
// ===================================================== // =====================================================
else if (cmd == "sweepCtrl") else if (cmd == "sweepCtrl")
{ {
int val = value.get<int>(); desired_.sweep = (value.get<int>() != 0);
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) catch (const std::exception& e)
{ {
LOG_ERROR("[REMOTE] Exception in onMqttMessage: %s, payload=%s", e.what(), payload.c_str()); LOG_WARN("[REMOTE] JSON parse error: %s", e.what());
return;
} }
} }