diff --git a/src/urg_node2.cpp b/src/urg_node2.cpp index c39fed4..59adddd 100644 --- a/src/urg_node2.cpp +++ b/src/urg_node2.cpp @@ -484,6 +484,7 @@ void UrgNode2::scan_thread() RCLCPP_WARN(get_logger(), "Could not get multi echo scan."); error_count_++; total_error_count_++; + prev_time = system_clock.now(); device_status_ = urg_sensor_status(&urg_); sensor_status_ = urg_sensor_state(&urg_); is_stable_ = urg_is_stable(&urg_); @@ -499,6 +500,7 @@ void UrgNode2::scan_thread() RCLCPP_WARN(get_logger(), "Could not get single echo scan."); error_count_++; total_error_count_++; + prev_time = system_clock.now(); device_status_ = urg_sensor_status(&urg_); sensor_status_ = urg_sensor_state(&urg_); is_stable_ = urg_is_stable(&urg_); @@ -514,10 +516,8 @@ void UrgNode2::scan_thread() break; } else { // エラーカウントのリセット - rclcpp::Time current_time = system_clock.now(); - rclcpp::Duration period = current_time - prev_time; + rclcpp::Duration period = system_clock.now() - prev_time; if (period.seconds() >= error_reset_period_) { - prev_time = current_time; error_count_ = 0; } }