diff --git a/carma-messenger-core/traffic_incident/include/traffic_incident_node.hpp b/carma-messenger-core/traffic_incident/include/traffic_incident_node.hpp index ecb46179..743fd902 100644 --- a/carma-messenger-core/traffic_incident/include/traffic_incident_node.hpp +++ b/carma-messenger-core/traffic_incident/include/traffic_incident_node.hpp @@ -86,6 +86,7 @@ class TrafficIncidentNode : public carma_ros2_utils::CarmaLifecycleNode double down_track_; double up_track_; double min_gap_; + static constexpr double epsilon_ = 1e-6; }; } // namespace traffic diff --git a/carma-messenger-core/traffic_incident/src/traffic_incident_node.cpp b/carma-messenger-core/traffic_incident/src/traffic_incident_node.cpp index 7432aa4e..596ac978 100644 --- a/carma-messenger-core/traffic_incident/src/traffic_incident_node.cpp +++ b/carma-messenger-core/traffic_incident/src/traffic_incident_node.cpp @@ -150,7 +150,7 @@ bool TrafficIncidentNode::stopTrafficBroadcastCallback( void TrafficIncidentNode::spin_callback(void) { if ( - traffic_worker_.getDownTrack() > 0 || traffic_worker_.getUpTrack() > 0 || traffic_worker_.getAdvisorySpeed() > 0) { + std::fabs(traffic_worker_.getDownTrack()) > epsilon_ || std::fabs(traffic_worker_.getUpTrack() > epsilon_) || std::fabs(traffic_worker_.getAdvisorySpeed() > epsilon_)) { // construct local mobilityOperation msg carma_v2x_msgs::msg::MobilityOperation traffic_mobility_msg = traffic_worker_.mobilityMessageGenerator(traffic_worker_.getPinPoint());