@@ -64,7 +64,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
64
64
diagnostics_warn_extrapolation_ = declare_parameter<double >(" diagnostics_warn_extrapolation_" );
65
65
diagnostics_error_extrapolation_ = declare_parameter<double >(" diagnostics_error_extrapolation_" );
66
66
67
-
68
67
declare_parameter (" selected_input_channels" , std::vector<std::string>());
69
68
std::vector<std::string> selected_input_channels =
70
69
get_parameter (" selected_input_channels" ).as_string_array ();
@@ -194,13 +193,12 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
194
193
}
195
194
196
195
// Debugger
197
- debugger_ = std::make_unique<TrackerDebugger>(*this , world_frame_id_,input_channels_config_);
196
+ debugger_ = std::make_unique<TrackerDebugger>(*this , world_frame_id_, input_channels_config_);
198
197
199
198
published_time_publisher_ = std::make_unique<autoware_utils::PublishedTimePublisher>(this );
200
199
// Diagnostics
201
- diagnostics_interface_ptr_ =
202
- std::make_unique<autoware_utils::DiagnosticsInterface>(this , " multi_object_tracker_delay_compensation" );
203
-
200
+ diagnostics_interface_ptr_ = std::make_unique<autoware_utils::DiagnosticsInterface>(
201
+ this , " multi_object_tracker_delay_compensation" );
204
202
}
205
203
206
204
void MultiObjectTracker::onTrigger ()
@@ -235,8 +233,8 @@ void MultiObjectTracker::onTimer()
235
233
// Get minimum prediction time delta from all trackers
236
234
const double min_extrapolation_time = (current_time - last_updated_time_).seconds ();
237
235
238
-
239
- std::cout<< " MultiObjectTracker::onTimer: min_prediction_delta: " <<min_extrapolation_time<< std::endl;
236
+ std::cout << " MultiObjectTracker::onTimer: min_prediction_delta: " << min_extrapolation_time
237
+ << std::endl;
240
238
241
239
// ensure minimum interval: room for the next process(prediction)
242
240
const double minimum_publish_interval = publisher_period_ * minimum_publish_interval_ratio;
@@ -258,22 +256,19 @@ void MultiObjectTracker::onTimer()
258
256
259
257
if (min_extrapolation_time > diagnostics_error_extrapolation_) {
260
258
std::stringstream message;
261
- message << " min_extrapolation_time exceeds error threshold ("
259
+ message << " min_extrapolation_time exceeds error threshold ("
262
260
<< diagnostics_error_extrapolation_ << " ), current value: " << min_extrapolation_time;
263
261
diagnostics_interface_ptr_->update_level_and_message (
264
262
diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str ());
265
- }
266
- else if (min_extrapolation_time > diagnostics_warn_extrapolation_) {
263
+ } else if (min_extrapolation_time > diagnostics_warn_extrapolation_) {
267
264
std::stringstream message;
268
- message << " min_extrapolation_time exceeds warning threshold ("
265
+ message << " min_extrapolation_time exceeds warning threshold ("
269
266
<< diagnostics_warn_extrapolation_ << " ), current value: " << min_extrapolation_time;
270
267
diagnostics_interface_ptr_->update_level_and_message (
271
268
diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str ());
272
- }
273
- else {
274
- diagnostics_interface_ptr_->update_level_and_message (
275
- diagnostic_msgs::msg::DiagnosticStatus::OK, " OK" );
276
-
269
+ } else {
270
+ diagnostics_interface_ptr_->update_level_and_message (
271
+ diagnostic_msgs::msg::DiagnosticStatus::OK, " OK" );
277
272
}
278
273
diagnostics_interface_ptr_->publish (current_time);
279
274
0 commit comments