diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index a02ab010e8cea..b26a2630ed994 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -361,12 +361,14 @@ template T VehicleCmdGate::getContinuousTopic( const std::shared_ptr & prev_topic, const T & current_topic, const std::string & topic_name) { - if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() > 0.0) { + if ((rclcpp::Time(current_topic.stamp) - rclcpp::Time(prev_topic->stamp)).seconds() >= 0.0) { return current_topic; } else { - RCLCPP_INFO( - get_logger(), - "The operation mode is changed, but the %s is not received yet:", topic_name.c_str()); + if (topic_name != "") { + RCLCPP_INFO( + get_logger(), + "The operation mode is changed, but the %s is not received yet:", topic_name.c_str()); + } return *prev_topic; } } diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 3f74be72be9dc..279bc6036bd34 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -240,7 +240,8 @@ class VehicleCmdGate : public rclcpp::Node template T getContinuousTopic( - const std::shared_ptr & prev_topic, const T & current_topic, const std::string & topic_name); + const std::shared_ptr & prev_topic, const T & current_topic, + const std::string & topic_name = ""); // Algorithm Control prev_control_cmd_;