diff --git a/control/autoware_trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp index fab0f329f0d95..bed8616340ce7 100644 --- a/control/autoware_trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -204,6 +204,9 @@ boost::optional Controller::createInputData(rclc void Controller::callbackTimerControl() { + autoware_control_msgs::msg::Control out; + out.stamp = this->now(); + // 1. create input data const auto input_data = createInputData(*get_clock()); if (!input_data) { @@ -239,8 +242,6 @@ void Controller::callbackTimerControl() if (isTimeOut(lon_out, lat_out)) return; // 5. publish control command - autoware_control_msgs::msg::Control out; - out.stamp = this->now(); out.lateral = lat_out.control_cmd; out.longitudinal = lon_out.control_cmd; control_cmd_pub_->publish(out); 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 2ca8234274802..8a41313f95e26 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -569,6 +569,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) filtered_control.longitudinal = createLongitudinalStopControlCmd(); } else { filtered_control = createStopControlCmd(); + filtered_control.stamp = commands.control.stamp; } } @@ -576,6 +577,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) if (enable_cmd_limit_filter_) { // Apply limit filtering filtered_control = filterControlCommand(filtered_control); + filtered_control.stamp = commands.control.stamp; } // tmp: Publish vehicle emergency status VehicleEmergencyStamped vehicle_cmd_emergency; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index ef95c4fdbe90d..b9c998f7d7bdc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -300,6 +300,8 @@ bool BehaviorPathPlannerNode::isDataReady() void BehaviorPathPlannerNode::run() { + const auto stamp = this->now(); + takeData(); if (!isDataReady()) { @@ -372,6 +374,7 @@ void BehaviorPathPlannerNode::run() // path handling const auto path = getPath(output, planner_data_, planner_manager_); + path->header.stamp = stamp; // update planner data planner_data_->prev_output_path = path; @@ -714,7 +717,6 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( auto path = !output.path.points.empty() ? std::make_shared(output.path) : planner_data->prev_output_path; path->header = planner_data->route_handler->getRouteHeader(); - path->header.stamp = this->now(); PathWithLaneId connected_path; const auto module_status_ptr_vec = planner_manager->getSceneModuleStatus(); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index c80c27eeacbd5..709d5813a6e00 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -344,7 +344,7 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( "Backward path is NOT supported. just converting path_with_lane_id to path"); output_path_msg = to_path(*input_path_msg); output_path_msg.header.frame_id = "map"; - output_path_msg.header.stamp = this->now(); + output_path_msg.header.stamp = input_path_msg->header.stamp; output_path_msg.left_bound = input_path_msg->left_bound; output_path_msg.right_bound = input_path_msg->right_bound; return output_path_msg; @@ -366,7 +366,7 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( output_path_msg = autoware::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; - output_path_msg.header.stamp = this->now(); + output_path_msg.header.stamp = input_path_msg->header.stamp; // TODO(someone): This must be updated in each scene module, but copy from input message for now. output_path_msg.left_bound = input_path_msg->left_bound;