Skip to content

Commit

Permalink
add validation with absolute function
Browse files Browse the repository at this point in the history
Signed-off-by: MasatoSaeki <[email protected]>
  • Loading branch information
MasatoSaeki committed Dec 24, 2024
1 parent ab2e428 commit 982643f
Showing 1 changed file with 3 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP
latest_perception_msg_ = *msg;

if (
(rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds() >
std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds()) >
external_time_tolerance_) {
latest_external_msg_.traffic_light_groups.clear();
}
Expand All @@ -130,7 +130,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP

void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg)
{
if ((this->now() - rclcpp::Time(msg->stamp)).seconds() > external_delay_tolerance_) {
if (std::abs((this->now() - rclcpp::Time(msg->stamp)).seconds()) > external_delay_tolerance_) {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 5000, "Received outdated V2X traffic signal messages");
return;
Expand All @@ -139,7 +139,7 @@ void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr
latest_external_msg_ = *msg;

if (
(rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() >
std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds()) >
perception_time_tolerance_) {
latest_perception_msg_.traffic_light_groups.clear();
}
Expand Down

0 comments on commit 982643f

Please sign in to comment.