Skip to content

Commit 9b8bb68

Browse files
Copilotmarc-hanheide
Copilot
and
marc-hanheide
committed
fix: Race 3 detection/recovery, code review issues, and data_received tracking
Co-authored-by: marc-hanheide <1153084+marc-hanheide@users.noreply.github.com>
1 parent e3e8441 commit 9b8bb68

7 files changed

Lines changed: 151 additions & 37 deletions

File tree

‎src/call_back/livox_lidar_callback.cpp‎

Lines changed: 41 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -108,7 +108,9 @@ void LivoxLidarCallback::LidarInfoChangeCallback(const uint32_t handle,
108108
// process, silently breaking the other driver instance that owns it.
109109
// Log once per foreign IP to avoid flooding the console on every
110110
// periodic SDK re-discovery event.
111+
static std::mutex s_foreign_mutex;
111112
static std::unordered_set<uint32_t> s_logged_foreign;
113+
std::lock_guard<std::mutex> lock(s_foreign_mutex);
112114
if (s_logged_foreign.insert(handle).second) {
113115
LIVOX_WARN("[%s] ignoring lidar — not in this instance's config"
114116
" (likely owned by another driver process)",
@@ -175,8 +177,11 @@ void LivoxLidarCallback::LidarInfoChangeCallback(const uint32_t handle,
175177
LIVOX_INFO("[%s] no config commands needed, state -> Sampling",
176178
IpNumToString(handle).c_str());
177179
} else {
178-
LIVOX_INFO("[%s] waiting for %u config ack(s) before Sampling",
179-
IpNumToString(handle).c_str(), pending_bits);
180+
// Count set bits using Brian Kernighan's algorithm (each iteration clears the lowest set bit).
181+
uint32_t pending_count = 0;
182+
for (uint32_t b = pending_bits; b != 0; b &= (b - 1)) { ++pending_count; }
183+
LIVOX_INFO("[%s] waiting for %u config ack(s) before Sampling (pending mask=0x%x)",
184+
IpNumToString(handle).c_str(), pending_count, pending_bits);
180185
}
181186
} // free lock for set_bits
182187

@@ -205,9 +210,13 @@ void LivoxLidarCallback::WorkModeChangedCallback(livox_status status,
205210
void *client_data) {
206211
if (status != kLivoxLidarStatusSuccess) {
207212
// Track per-handle retry count to surface persistent failures.
213+
static std::mutex s_retry_mutex;
208214
static std::unordered_map<uint32_t, int> s_retry_count;
209-
int& retries = s_retry_count[handle];
210-
++retries;
215+
int retries;
216+
{
217+
std::lock_guard<std::mutex> lock(s_retry_mutex);
218+
retries = ++s_retry_count[handle];
219+
}
211220
LIVOX_WARN("[%s] work mode change failed (status=%d), retry #%d",
212221
IpNumToString(handle).c_str(), static_cast<int>(status), retries);
213222
std::this_thread::sleep_for(std::chrono::seconds(1));
@@ -245,8 +254,13 @@ void LivoxLidarCallback::SetDataTypeCallback(livox_status status, uint32_t handl
245254
LivoxLidarCallback::SetDataTypeCallback, client_data);
246255
LIVOX_WARN("[%s] set data type timed out, retrying...", IpNumToString(handle).c_str());
247256
} else {
248-
LIVOX_ERROR("[%s] set data type failed: ret_code=%d error_key=%d",
249-
IpNumToString(handle).c_str(), response->ret_code, response->error_key);
257+
if (response) {
258+
LIVOX_ERROR("[%s] set data type failed: ret_code=%d error_key=%d",
259+
IpNumToString(handle).c_str(), response->ret_code, response->error_key);
260+
} else {
261+
LIVOX_ERROR("[%s] set data type failed: status=%d (no response payload)",
262+
IpNumToString(handle).c_str(), static_cast<int>(status));
263+
}
250264
}
251265
return;
252266
}
@@ -278,8 +292,13 @@ void LivoxLidarCallback::SetPatternModeCallback(livox_status status, uint32_t ha
278292
LivoxLidarCallback::SetPatternModeCallback, client_data);
279293
LIVOX_WARN("[%s] set scan pattern timed out, retrying...", IpNumToString(handle).c_str());
280294
} else {
281-
LIVOX_ERROR("[%s] set scan pattern failed: ret_code=%d error_key=%d",
282-
IpNumToString(handle).c_str(), response->ret_code, response->error_key);
295+
if (response) {
296+
LIVOX_ERROR("[%s] set scan pattern failed: ret_code=%d error_key=%d",
297+
IpNumToString(handle).c_str(), response->ret_code, response->error_key);
298+
} else {
299+
LIVOX_ERROR("[%s] set scan pattern failed: status=%d (no response payload)",
300+
IpNumToString(handle).c_str(), static_cast<int>(status));
301+
}
283302
}
284303
return;
285304
}
@@ -311,8 +330,13 @@ void LivoxLidarCallback::SetBlindSpotCallback(livox_status status, uint32_t hand
311330
LivoxLidarCallback::SetBlindSpotCallback, client_data);
312331
LIVOX_WARN("[%s] set blind spot timed out, retrying...", IpNumToString(handle).c_str());
313332
} else {
314-
LIVOX_ERROR("[%s] set blind spot failed: ret_code=%d error_key=%d",
315-
IpNumToString(handle).c_str(), response->ret_code, response->error_key);
333+
if (response) {
334+
LIVOX_ERROR("[%s] set blind spot failed: ret_code=%d error_key=%d",
335+
IpNumToString(handle).c_str(), response->ret_code, response->error_key);
336+
} else {
337+
LIVOX_ERROR("[%s] set blind spot failed: status=%d (no response payload)",
338+
IpNumToString(handle).c_str(), static_cast<int>(status));
339+
}
316340
}
317341
return;
318342
}
@@ -344,8 +368,13 @@ void LivoxLidarCallback::SetDualEmitCallback(livox_status status, uint32_t handl
344368
LivoxLidarCallback::SetDualEmitCallback, client_data);
345369
LIVOX_WARN("[%s] set dual emit timed out, retrying...", IpNumToString(handle).c_str());
346370
} else {
347-
LIVOX_ERROR("[%s] set dual emit failed: ret_code=%d error_key=%d",
348-
IpNumToString(handle).c_str(), response->ret_code, response->error_key);
371+
if (response) {
372+
LIVOX_ERROR("[%s] set dual emit failed: ret_code=%d error_key=%d",
373+
IpNumToString(handle).c_str(), response->ret_code, response->error_key);
374+
} else {
375+
LIVOX_ERROR("[%s] set dual emit failed: status=%d (no response payload)",
376+
IpNumToString(handle).c_str(), static_cast<int>(status));
377+
}
349378
}
350379
return;
351380
}

‎src/comm/comm.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -284,6 +284,10 @@ typedef struct {
284284
// immediately visible across threads without relying on volatile, which only
285285
// prevents compiler optimisation and not CPU reordering.
286286
std::atomic<LidarConnectState> connect_state;
287+
// Set to true the first time a point cloud or IMU packet arrives from this
288+
// lidar. Used by the reconnect watchdog to detect when the lidar is in
289+
// Sampling state but no data is flowing (Race 3 symptom).
290+
std::atomic<bool> data_received{false};
287291
// DeviceInfo info;
288292

289293
LidarDataQueue data;

‎src/comm/pub_handler.cpp‎

Lines changed: 37 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,11 @@ void PubHandler::SetPointCloudsCallback(PointCloudsCallback cb, void* client_dat
100100
pub_client_data_ = client_data;
101101
points_callback_ = cb;
102102
lidar_listen_id_ = LivoxLidarAddPointCloudObserver(OnLivoxLidarPointCloudCallback, this);
103+
if (lidar_listen_id_ == 0) {
104+
LIVOX_ERROR("LivoxLidarAddPointCloudObserver failed — no point cloud data will be received");
105+
} else {
106+
LIVOX_INFO("point cloud observer registered (listen_id=%u)", lidar_listen_id_);
107+
}
103108
}
104109

105110
void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type,
@@ -109,25 +114,45 @@ void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t d
109114
return;
110115
}
111116

117+
// Log the first packet received from each handle to confirm data is flowing.
118+
// This MUST be before the AllowHandle check so we can detect Race 3: if the
119+
// front driver's SDK is receiving the back lidar's data (and silently dropping
120+
// it), this log will appear in the front driver's output, revealing that the
121+
// back lidar's data stream is being routed to the wrong process.
122+
{
123+
static std::unordered_set<uint32_t> s_first_packet_seen;
124+
static std::mutex s_first_packet_mutex;
125+
std::lock_guard<std::mutex> lock(s_first_packet_mutex);
126+
if (s_first_packet_seen.insert(handle).second) {
127+
LIVOX_INFO("[%s] first packet received (dev_type=%u, data_type=%u)",
128+
IpNumToString(handle).c_str(), dev_type, data->data_type);
129+
}
130+
}
131+
112132
// Drop packets from lidars not explicitly configured for this driver instance.
113133
// This prevents cross-talk when two driver nodes run on the same host and the
114134
// SDK observer fires for all discovered lidars regardless of which node owns them.
115135
{
116136
std::lock_guard<std::mutex> lock(self->allowed_handles_mutex_);
117137
if (!self->allowed_handles_.empty() &&
118138
self->allowed_handles_.find(handle) == self->allowed_handles_.end()) {
119-
return; // foreign lidar — silently ignore
120-
}
121-
}
122-
123-
// Log the first packet received from each lidar to confirm data is flowing.
124-
{
125-
static std::unordered_set<uint32_t> s_first_packet_seen;
126-
static std::mutex s_first_packet_mutex;
127-
std::lock_guard<std::mutex> lock(s_first_packet_mutex);
128-
if (s_first_packet_seen.insert(handle).second) {
129-
LIVOX_INFO("[%s] first point cloud packet received (dev_type=%u, data_type=%u)",
130-
IpNumToString(handle).c_str(), dev_type, data->data_type);
139+
// Throttled warning: if this fires in the FRONT driver for the back
140+
// lidar's IP (or vice versa), it confirms Race 3 is occurring — the
141+
// SDK routed the wrong lidar's data stream to this process.
142+
static std::unordered_map<uint32_t, std::chrono::steady_clock::time_point> s_drop_warn_last;
143+
static std::mutex s_drop_warn_mutex;
144+
{
145+
std::lock_guard<std::mutex> wlock(s_drop_warn_mutex);
146+
auto now = std::chrono::steady_clock::now();
147+
auto& last = s_drop_warn_last[handle];
148+
if (now - last > std::chrono::seconds(10)) {
149+
last = now;
150+
LIVOX_WARN("[%s] dropping packet — handle not in this driver's"
151+
" allowed set (Race 3: SDK data mis-routing?)",
152+
IpNumToString(handle).c_str());
153+
}
154+
}
155+
return; // foreign lidar — drop
131156
}
132157
}
133158

‎src/driver_node.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,8 @@ class DriverNode final : public rclcpp::Node {
7272
std::promise<void> exit_signal_;
7373
/** One-shot timer: fires if no lidar connects within connect_timeout_s. */
7474
rclcpp::TimerBase::SharedPtr watchdog_timer_;
75+
/** Periodic timer: re-asserts work mode for Sampling lidars with no data yet (Race 3 recovery). */
76+
rclcpp::TimerBase::SharedPtr reconnect_timer_;
7577
};
7678
#endif
7779

‎src/lddc.cpp‎

Lines changed: 5 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -40,9 +40,6 @@
4040

4141
namespace livox_ros {
4242

43-
// Minimum interval between "not yet in Sampling state" warnings per lidar (ns).
44-
static constexpr int64_t kSamplingStateWarnIntervalNs = 10LL * 1000000000LL;
45-
4643
/** Lidar Data Distribute Control--------------------------------------------*/
4744
#ifdef BUILDING_ROS1
4845
Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
@@ -133,18 +130,14 @@ void Lddc::DistributePointCloudData(void) {
133130
for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
134131
uint32_t lidar_id = i;
135132
LidarDevice *lidar = &lds_->lidars_[lidar_id];
136-
LidarDataQueue *p_queue = &lidar->data;
137-
if (p_queue == nullptr) {
138-
continue;
139-
}
140133
if (kConnectStateSampling != lidar->connect_state.load(std::memory_order_acquire)) {
141134
// Emit a throttled warning so the user can see which lidar is stuck
142135
// in the init state and not yet publishing data.
143136
if (lidar->handle != 0) {
144-
static std::unordered_map<uint32_t, int64_t> s_last_warn_ns;
145-
auto now = std::chrono::steady_clock::now().time_since_epoch().count();
146-
auto& last = s_last_warn_ns[lidar->handle];
147-
if (now - last > kSamplingStateWarnIntervalNs) { // warn at most once per 10 s
137+
static std::unordered_map<uint32_t, std::chrono::steady_clock::time_point> s_last_warn;
138+
auto now = std::chrono::steady_clock::now();
139+
auto& last = s_last_warn[lidar->handle];
140+
if (now - last > std::chrono::seconds(10)) {
148141
last = now;
149142
LIVOX_WARN("[%s] not yet in Sampling state (connect_state=%d),"
150143
" point cloud will not be published",
@@ -172,8 +165,7 @@ void Lddc::DistributeImuData(void) {
172165
for (uint32_t i = 0; i < lds_->lidar_count_; i++) {
173166
uint32_t lidar_id = i;
174167
LidarDevice *lidar = &lds_->lidars_[lidar_id];
175-
LidarImuDataQueue *p_queue = &lidar->imu_data;
176-
if ((kConnectStateSampling != lidar->connect_state.load(std::memory_order_acquire)) || (p_queue == nullptr)) {
168+
if (kConnectStateSampling != lidar->connect_state.load(std::memory_order_acquire)) {
177169
continue;
178170
}
179171
PollingLidarImuData(lidar_id, lidar);

‎src/lds.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) {
6161

6262
lidar->data_src = data_src;
6363
lidar->connect_state.store(kConnectStateOff, std::memory_order_release);
64+
lidar->data_received.store(false, std::memory_order_release);
6465
}
6566

6667
void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {
@@ -183,6 +184,7 @@ void Lds::PushLidarData(PointPacket* lidar_data, const uint8_t index, const uint
183184

184185
if (!QueueIsFull(queue)) {
185186
QueuePushAny(queue, (uint8_t *)lidar_data, base_time);
187+
p_lidar->data_received.store(true, std::memory_order_release);
186188
if (!QueueIsEmpty(queue)) {
187189
if (pcd_semaphore_.GetCount() <= 0) {
188190
pcd_semaphore_.Signal();

‎src/livox_ros_driver2.cpp‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -209,6 +209,66 @@ DriverNode::DriverNode(const rclcpp::NodeOptions & node_options)
209209
}).detach();
210210
}
211211
});
212+
213+
// Race-3 recovery timer: fires every 3 s and re-sends SetLivoxLidarWorkMode
214+
// + EnableLivoxLidarImuData for any lidar that has reached kConnectStateSampling
215+
// but has not yet delivered any data.
216+
//
217+
// Root cause of Race 3: when the second driver instance starts, the Livox
218+
// SDK it initialises discovers ALL lidars on the network and sends each one
219+
// a "host registration" packet (host IP + data ports from the config file).
220+
// If the first driver's host_net_info entry is not correctly scoped to only
221+
// its own lidar IP, this registration overwrites the second driver's, causing
222+
// the lidar to send its data stream to the first driver's ports where it is
223+
// silently dropped by the AllowHandle filter.
224+
//
225+
// Mitigation: re-sending SetLivoxLidarWorkMode from THIS driver's cmd_data_port
226+
// causes the lidar to update its "active host" to the sender of this command.
227+
// Subsequent data will then be routed to this driver's data ports. The timer
228+
// cancels itself once all configured lidars have confirmed data flow.
229+
reconnect_timer_ = this->create_wall_timer(
230+
std::chrono::seconds(3),
231+
[this, lds]() {
232+
bool all_flowing = true;
233+
for (uint32_t i = 0; i < lds->lidar_count_; i++) {
234+
LidarDevice& dev = lds->lidars_[i];
235+
if (dev.lidar_type == 0) continue; // slot not in use
236+
if (dev.connect_state.load(std::memory_order_acquire)
237+
!= kConnectStateSampling) continue; // not ready yet
238+
if (dev.data_received.load(std::memory_order_acquire)) continue; // flowing ✓
239+
// Lidar is in Sampling state but no data has arrived yet.
240+
// Re-assert ownership by re-sending work mode from this driver.
241+
RCLCPP_WARN(this->get_logger(),
242+
"[%s] in Sampling state but no data received — Race 3 suspected."
243+
" Re-asserting work mode to reclaim data stream.",
244+
IpNumToString(dev.handle).c_str());
245+
SetLivoxLidarWorkMode(dev.handle, kLivoxLidarNormal,
246+
[](livox_status status, uint32_t handle,
247+
LivoxLidarAsyncControlResponse*, void* /*ctx*/) {
248+
if (status == kLivoxLidarStatusSuccess) {
249+
LIVOX_INFO("[%s] reconnect work-mode refresh OK",
250+
IpNumToString(handle).c_str());
251+
} else {
252+
LIVOX_WARN("[%s] reconnect work-mode refresh failed (status=%d)",
253+
IpNumToString(handle).c_str(), static_cast<int>(status));
254+
}
255+
}, nullptr);
256+
// Re-send IMU enable so the lidar re-registers its IMU data destination.
257+
EnableLivoxLidarImuData(dev.handle,
258+
[](livox_status status, uint32_t handle,
259+
LivoxLidarAsyncControlResponse*, void*) {
260+
if (status != kLivoxLidarStatusSuccess) {
261+
LIVOX_WARN("[%s] reconnect IMU re-enable failed (status=%d)",
262+
IpNumToString(handle).c_str(), static_cast<int>(status));
263+
}
264+
}, nullptr);
265+
all_flowing = false;
266+
}
267+
if (all_flowing) {
268+
reconnect_timer_->cancel();
269+
DRIVER_INFO(*this, "All lidars are flowing; reconnect timer cancelled.");
270+
}
271+
});
212272
} else {
213273
DRIVER_ERROR(*this, "Init lds lidar fail!");
214274
}

0 commit comments

Comments
 (0)