Error counts continuously being reset

[richardw347]

Hello,

We've been using the urg_node2 on our robots with the UAM-05LP scanner. Our robots are using ROS Humble. The problem we experienced was when the laser was disconnected/powered down temporarily. The driver gets stuck in a loop where the error_count_ never reaches the error_limit_ because when an error occurs the driver is updating the diagnostic status:

urg_node2/src/urg_node2.cpp

Lines 498 to 505 in 6632d1c

	} else {
	RCLCPP_WARN(get_logger(), "Could not get single echo scan.");
	error_count_++;
	total_error_count_++;
	device_status_ = urg_sensor_status(&urg_);
	sensor_status_ = urg_sensor_state(&urg_);
	is_stable_ = urg_is_stable(&urg_);
	}

Which can take ~1-2 seconds each time. This is causing the error_counts to be reset before they reach the limit because of this block:

urg_node2/src/urg_node2.cpp

Lines 517 to 522 in 6632d1c

	rclcpp::Time current_time = system_clock.now();
	rclcpp::Duration period = current_time - prev_time;
	if (period.seconds() >= error_reset_period_) {
	prev_time = current_time;
	error_count_ = 0;
	}

Semantically the purpose of the above block seems to be to reset the error counts if we haven't seen an error in a while but what's actually happening is the error counts are always being reset after the error_reset_period.

See the log from below (with an added log statement every time the error counts are reset).

This PR moves the setting of prev_time to error block. This fixes the error reset count loop and has been tested with our UAM's and works well. I've added it to the multi-echo branch but don't have a multi-echo scanner to test, given they are identical blocks it should work the same.