fix(planning, control): reuse stamp of subscribed topic to measure co…

…mponent latency (#10201)

* fix(behavior_velocity_planner): reuse timestamp of recieved path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(behavior_path_planner): check timestamp first in timer driven callback

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(trajectory_follower_node): check timestamp first in timer driven callback

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(vehicle_cmd_gate): reuse timestamp of recieved path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

control/autoware_trajectory_follower_node/src/controller_node.cpp

@@ -204,6 +204,9 @@ boost::optional<trajectory_follower::InputData> 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);

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -569,13 +569,15 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
       filtered_control.longitudinal = createLongitudinalStopControlCmd();
     } else {
       filtered_control = createStopControlCmd();
+      filtered_control.stamp = commands.control.stamp;
     }
   }
 
   // Check if command filtering option is enable
   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;

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<PathWithLaneId>(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();

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;