@@ -215,7 +215,7 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(
215
215
preprocess (*output_msg);
216
216
217
217
int64_t timestamp_nsec =
218
- (*output_msg).header .stamp .sec * ( int64_t ) 1e9 + (*output_msg).header .stamp .nanosec ;
218
+ (*output_msg).header .stamp .sec * static_cast < int64_t >( 1e9 ) + (*output_msg).header .stamp .nanosec ;
219
219
220
220
// if matching rois exist, fuseOnSingle
221
221
// please ask maintainers before parallelize this loop because debugger is not thread safe
@@ -232,14 +232,17 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(
232
232
std::list<int64_t > outdate_stamps;
233
233
234
234
for (const auto & [k, v] : cached_roi_msgs_.at (roi_i)) {
235
- int64_t new_stamp = timestamp_nsec + input_offset_ms_.at (roi_i) * ( int64_t ) 1e6 ;
236
- int64_t interval = abs (int64_t (k) - new_stamp);
235
+ int64_t new_stamp = timestamp_nsec + input_offset_ms_.at (roi_i) * static_cast < int64_t >( 1e6 ) ;
236
+ int64_t interval = abs (static_cast < int64_t > (k) - new_stamp);
237
237
238
- if (interval <= min_interval && interval <= match_threshold_ms_ * (int64_t )1e6 ) {
238
+ if (
239
+ interval <= min_interval && interval <= match_threshold_ms_ * static_cast <int64_t >(1e6 )) {
239
240
min_interval = interval;
240
241
matched_stamp = k;
241
- } else if (int64_t (k) < new_stamp && interval > match_threshold_ms_ * (int64_t )1e6 ) {
242
- outdate_stamps.push_back (int64_t (k));
242
+ } else if (
243
+ static_cast <int64_t >(k) < new_stamp &&
244
+ interval > match_threshold_ms_ * static_cast <int64_t >(1e6 )) {
245
+ outdate_stamps.push_back (static_cast <int64_t >(k));
243
246
}
244
247
}
245
248
@@ -293,7 +296,7 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(
293
296
processing_time_ms = 0 ;
294
297
}
295
298
} else {
296
- cached_msg_.first = int64_t ( timestamp_nsec) ;
299
+ cached_msg_.first = timestamp_nsec;
297
300
cached_msg_.second = output_msg;
298
301
processing_time_ms = stop_watch_ptr_->toc (" processing_time" , true );
299
302
}
@@ -305,15 +308,16 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::roiCallback(
305
308
{
306
309
stop_watch_ptr_->toc (" processing_time" , true );
307
310
308
- int64_t timestamp_nsec =
309
- (*input_roi_msg). header . stamp . sec * ( int64_t ) 1e9 + (*input_roi_msg).header .stamp .nanosec ;
311
+ int64_t timestamp_nsec = (*input_roi_msg). header . stamp . sec * static_cast < int64_t >( 1e9 ) +
312
+ (*input_roi_msg).header .stamp .nanosec ;
310
313
311
314
// if cached Msg exist, try to match
312
315
if (cached_msg_.second != nullptr ) {
313
- int64_t new_stamp = cached_msg_.first + input_offset_ms_.at (roi_i) * ( int64_t ) 1e6 ;
316
+ int64_t new_stamp = cached_msg_.first + input_offset_ms_.at (roi_i) * static_cast < int64_t >( 1e6 ) ;
314
317
int64_t interval = abs (timestamp_nsec - new_stamp);
315
318
316
- if (interval < match_threshold_ms_ * (int64_t )1e6 && is_fused_.at (roi_i) == false ) {
319
+ if (
320
+ interval < match_threshold_ms_ * static_cast <int64_t >(1e6 ) && is_fused_.at (roi_i) == false ) {
317
321
if (camera_info_map_.find (roi_i) == camera_info_map_.end ()) {
318
322
RCLCPP_WARN_THROTTLE (
319
323
this ->get_logger (), *this ->get_clock (), 5000 , " no camera info. id is %zu" , roi_i);
0 commit comments