fix bug : 上报卡顿
This commit is contained in:
parent
d963cff057
commit
80711d985c
@ -72,7 +72,7 @@ int main(int argc, char** argv)
|
||||
auto node = rclcpp::Node::make_shared("can_driver_node");
|
||||
LOG_INFO("Starting mc package...");
|
||||
|
||||
auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
|
||||
auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 100);
|
||||
auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);
|
||||
auto vid_sub = node->create_subscription<sweeperMsg::VehicleIdentity>("/vehicle/identity", rclcpp::QoS(1).transient_local().reliable(), vehicleIdentityCallback);
|
||||
|
||||
|
||||
@ -70,6 +70,9 @@ class MQTTClientWrapper
|
||||
|
||||
bool publish(const std::string& topic, const std::string& payload, int qos = 0, bool retained = false)
|
||||
{
|
||||
// 快速检查连接状态,避免不必要的锁竞争
|
||||
if (!connected_) return false;
|
||||
|
||||
std::lock_guard<std::mutex> lock(mtx_);
|
||||
if (!client_ || !connected_) return false;
|
||||
|
||||
@ -195,6 +198,10 @@ class MQTTClientWrapper
|
||||
{
|
||||
if (!connected_)
|
||||
{
|
||||
// 尝试连接前先释放锁,给 publish 线程机会
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
if (stop_flag_) break;
|
||||
|
||||
std::lock_guard<std::mutex> lock(mtx_);
|
||||
doConnect();
|
||||
}
|
||||
|
||||
@ -37,7 +37,7 @@ class VehicleReportNode : public rclcpp::Node
|
||||
{
|
||||
// CAN
|
||||
subscription_ = this->create_subscription<sweeperMsg::CanFrame>(
|
||||
"can_data", 10, std::bind(&VehicleReportNode::topic_callback, this, std::placeholders::_1));
|
||||
"can_data", 100, std::bind(&VehicleReportNode::topic_callback, this, std::placeholders::_1));
|
||||
|
||||
// Identity
|
||||
identity_sub_ = this->create_subscription<VehicleIdentity>(
|
||||
|
||||
Loading…
Reference in New Issue
Block a user