@@ -244,20 +244,22 @@ void LidarCenterPointNode::pointCloudCallback(
244
244
245
245
// Check the timestamp of consecutively delayed processing
246
246
// If the node is consistently delayed, publish an error diagnostic message
247
- void LidarCenterPointNode::diagnosticsTimerCallback () {
247
+ void LidarCenterPointNode::diagnosticsTimerCallback ()
248
+ {
248
249
// skip if the node has not performed inference yet
249
250
if (last_normal_processing_timestamp_) {
250
251
diagnostics_processing_delay_->clear ();
251
252
252
253
const rclcpp::Time timestamp_now = this ->get_clock ()->now ();
253
254
const double delayed_state_duration =
254
- std::chrono::duration<double , std::milli>(
255
- std::chrono::nanoseconds (
256
- (timestamp_now - last_normal_processing_timestamp_.value ()).nanoseconds ()))
257
- .count ();
255
+ std::chrono::duration<double , std::milli>(
256
+ std::chrono::nanoseconds (
257
+ (timestamp_now - last_normal_processing_timestamp_.value ()).nanoseconds ()))
258
+ .count ();
258
259
259
260
if (delayed_state_duration > max_acceptable_consecutive_delay_ms_) {
260
- diagnostics_processing_delay_->add_key_value (" is_processing_time_ms_in_expected_range" , false );
261
+ diagnostics_processing_delay_->add_key_value (
262
+ " is_processing_time_ms_in_expected_range" , false );
261
263
diagnostics_processing_delay_->update_level_and_message (
262
264
diagnostic_msgs::msg::DiagnosticStatus::ERROR,
263
265
" CenterPoint processing delay has exceeded the acceptable limit continuously." );
0 commit comments