tmp scenario_selector header update

autowarefoundation · Dec 28, 2023 · 09d7b81 · 09d7b81
1 parent 4702e8e
commit 09d7b81
Showing 1 changed file with 12 additions and 10 deletions.
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
@@ -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)