@@ -112,11 +112,12 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
112
112
113
113
// diagnostics parameters
114
114
max_allowed_processing_time_ = declare_parameter<float >(" max_allowed_processing_time" );
115
- max_acceptable_consecutive_delay_ms_ = declare_parameter<int >(" max_acceptable_consecutive_delay_ms" );
115
+ max_acceptable_consecutive_delay_ms_ =
116
+ declare_parameter<int >(" max_acceptable_consecutive_delay_ms" );
116
117
117
118
diagnostics_timer_ = this ->create_wall_timer (
118
- std::chrono::milliseconds (100 ),
119
- std::bind (&LidarCenterPointNode::diagnosticTimerCallback, this ));
119
+ std::chrono::milliseconds (100 ),
120
+ std::bind (&LidarCenterPointNode::diagnosticTimerCallback, this ));
120
121
121
122
pointcloud_sub_ = this ->create_subscription <sensor_msgs::msg::PointCloud2>(
122
123
" ~/input/pointcloud" , rclcpp::SensorDataQoS{}.keep_last (1 ),
@@ -237,12 +238,13 @@ void LidarCenterPointNode::pointCloudCallback(
237
238
238
239
// Check the timestamp of the processing that has started consecutively delaying,
239
240
// and if the node is delayed constantly, publish the error diagnostic message
240
- void LidarCenterPointNode::diagnosticTimerCallback () {
241
+ void LidarCenterPointNode::diagnosticTimerCallback ()
242
+ {
241
243
const double delayed_state_duration =
242
- std::chrono::duration<double , std::milli>(
243
- std::chrono::nanoseconds (
244
- (this ->get_clock ()->now () - started_delayed_timestamp_).nanoseconds ()))
245
- .count ();
244
+ std::chrono::duration<double , std::milli>(
245
+ std::chrono::nanoseconds (
246
+ (this ->get_clock ()->now () - started_delayed_timestamp_).nanoseconds ()))
247
+ .count ();
246
248
247
249
if (delayed_state_duration > max_acceptable_consecutive_delay_ms_) {
248
250
diagnostics_interface_ptr_->add_key_value (" is_processing_time_ms_in_expected_range" , false );
0 commit comments