Added VehicleSpec

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Author: SakodaShintaro

planning/autoware_diffusion_planner/include/autoware/diffusion_planner/diffusion_planner_core.hpp

@@ -67,6 +67,22 @@ using utils::NormalizationMap;
 using InputDataMap = std::unordered_map<std::string, std::vector<float>>;
 using AgentPoses = std::vector<std::vector<std::vector<Eigen::Matrix4d>>>;
 
+struct VehicleSpec
+{
+  double wheel_base;
+  double vehicle_length;
+  double vehicle_width;
+  double base_link_to_center;
+
+  explicit VehicleSpec(const VehicleInfo & info)
+  : wheel_base(info.wheel_base_m),
+    vehicle_length(info.front_overhang_m + info.wheel_base_m + info.rear_overhang_m),
+    vehicle_width(info.left_overhang_m + info.wheel_tread_m + info.right_overhang_m),
+    base_link_to_center((info.front_overhang_m + info.wheel_base_m - info.rear_overhang_m) / 2.0)
+  {
+  }
+};
+
 struct PlannerOutput
 {
   Trajectory trajectory;
@@ -254,7 +270,7 @@ class DiffusionPlannerCore
 private:
   // Parameters
   DiffusionPlannerParams params_;
-  VehicleInfo vehicle_info_;
+  VehicleSpec vehicle_spec_;
   NormalizationMap normalization_map_;
 
   // Inference engine

planning/autoware_diffusion_planner/src/diffusion_planner_core.cpp

@@ -39,7 +39,7 @@ namespace autoware::diffusion_planner
 DiffusionPlannerCore::DiffusionPlannerCore(
   const DiffusionPlannerParams & params, const VehicleInfo & vehicle_info)
 : params_(params),
-  vehicle_info_(vehicle_info),
+  vehicle_spec_(vehicle_info),
   turn_indicator_manager_(
     rclcpp::Duration::from_seconds(params.turn_indicator_hold_duration),
     params.turn_indicator_keep_offset)
@@ -98,7 +98,7 @@ std::optional<FrameContext> DiffusionPlannerCore::create_frame_context(
   Odometry kinematic_state = *ego_kinematic_state;
   if (params_.shift_x) {
     kinematic_state.pose.pose =
-      utils::shift_x(kinematic_state.pose.pose, vehicle_info_.wheel_base_m / 2.0);
+      utils::shift_x(kinematic_state.pose.pose, vehicle_spec_.base_link_to_center);
   }
 
   // Get transforms
@@ -155,7 +155,7 @@ InputDataMap DiffusionPlannerCore::create_input_data(const FrameContext & frame_
   const geometry_msgs::msg::Pose & pose_center =
     params_.shift_x
       ? utils::shift_x(
-          frame_context.ego_kinematic_state.pose.pose, vehicle_info_.wheel_base_m / 2.0)
+          frame_context.ego_kinematic_state.pose.pose, vehicle_spec_.base_link_to_center)
       : frame_context.ego_kinematic_state.pose.pose;
   const Eigen::Matrix4d ego_to_map_transform = utils::pose_to_matrix4d(pose_center);
   const Eigen::Matrix4d map_to_ego_transform = utils::inverse(ego_to_map_transform);
@@ -174,7 +174,7 @@ InputDataMap DiffusionPlannerCore::create_input_data(const FrameContext & frame_
   {
     const auto ego_current_state = preprocess::create_ego_current_state(
       frame_context.ego_kinematic_state, frame_context.ego_acceleration,
-      static_cast<float>(vehicle_info_.wheel_base_m));
+      static_cast<float>(vehicle_spec_.wheel_base));
     input_data_map["ego_current_state"] =
       utils::replicate_for_batch(ego_current_state, params_.batch_size);
   }
@@ -257,12 +257,10 @@ InputDataMap DiffusionPlannerCore::create_input_data(const FrameContext & frame_
 
   // ego shape
   {
-    const float wheel_base = static_cast<float>(vehicle_info_.wheel_base_m);
-    const float vehicle_length = static_cast<float>(
-      vehicle_info_.front_overhang_m + vehicle_info_.wheel_base_m + vehicle_info_.rear_overhang_m);
-    const float vehicle_width = static_cast<float>(
-      vehicle_info_.left_overhang_m + vehicle_info_.wheel_tread_m + vehicle_info_.right_overhang_m);
-    std::vector<float> single_ego_shape = {wheel_base, vehicle_length, vehicle_width};
+    const std::vector<float> single_ego_shape = {
+      static_cast<float>(vehicle_spec_.wheel_base),
+      static_cast<float>(vehicle_spec_.vehicle_length),
+      static_cast<float>(vehicle_spec_.vehicle_width)};
     input_data_map["ego_shape"] = utils::replicate_for_batch(single_ego_shape, params_.batch_size);
   }
 
@@ -313,8 +311,7 @@ PlannerOutput DiffusionPlannerCore::create_planner_output(
 
     if (params_.shift_x) {
       for (auto & point : trajectory.points) {
-        // TODO(sakoda): front and rear overhang should be considered for more accurate shifting.
-        point.pose = utils::shift_x(point.pose, -vehicle_info_.wheel_base_m / 2.0);
+        point.pose = utils::shift_x(point.pose, -vehicle_spec_.base_link_to_center);
       }
     }
 
@@ -369,7 +366,7 @@ DiffusionPlannerCore::get_first_traffic_light_on_route(const FrameContext & fram
   const geometry_msgs::msg::Pose & pose_center =
     params_.shift_x
       ? utils::shift_x(
-          frame_context.ego_kinematic_state.pose.pose, vehicle_info_.wheel_base_m / 2.0)
+          frame_context.ego_kinematic_state.pose.pose, vehicle_spec_.base_link_to_center)
       : frame_context.ego_kinematic_state.pose.pose;
 
   const double center_x = pose_center.position.x;