diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 1d5c7b44dcc45..b62ee6c8aba39 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -316,16 +316,18 @@ void ScenarioSelectorNode::onParkingTrajectory( void ScenarioSelectorNode::publishTrajectory( const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { - const auto now = this->now(); - const auto delay_sec = (now - msg->header.stamp).seconds(); - if (delay_sec <= th_max_message_delay_sec_) { - pub_trajectory_->publish(*msg); - } else { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "trajectory is delayed: scenario = %s, delay = %f, th_max_message_delay = %f", - current_scenario_.c_str(), delay_sec, th_max_message_delay_sec_); - } + auto msg_sent = *msg; + msg_sent.header.stamp = this->now(); + pub_trajectory_->publish(msg_sent); + + // const auto delay_sec = (now - msg->header.stamp).seconds(); + // if (delay_sec <= th_max_message_delay_sec_) { + // } else { + // RCLCPP_WARN_THROTTLE( + // this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + // "trajectory is delayed: scenario = %s, delay = %f, th_max_message_delay = %f", + // current_scenario_.c_str(), delay_sec, th_max_message_delay_sec_); + // } } ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_options)