perf(behavior_velocity_planner): remove pose.orientation from occlusi…

…on_spot (autowarefoundation#2591)

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

@@ -183,15 +183,18 @@ struct PossibleCollisionInfo
 struct DebugData
 {
   double z;
+  double baselink_to_front;
   std::string road_type = "";
   std::string detection_type = "";
   Polygons2d detection_area_polygons;
   std::vector<lanelet::BasicPolygon2d> close_partition;
   std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
   std::vector<PossibleCollisionInfo> possible_collisions;
   std::vector<geometry_msgs::msg::Point> occlusion_points;
+  std::vector<geometry_msgs::msg::Pose> debug_poses;
   void resetData()
   {
+    debug_poses.clear();
     close_partition.clear();
     detection_area_polygons.clear();
     parked_vehicle_point.clear();

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp

@@ -28,7 +28,7 @@ namespace occlusion_spot_utils
 {
 void applySafeVelocityConsideringPossibleCollision(
   PathWithLaneId * inout_path, std::vector<PossibleCollisionInfo> & possible_collisions,
-  const PlannerParam & param);
+  std::vector<geometry_msgs::msg::Pose> & debug_poses, const PlannerParam & param);
 
 /**
  * @param: v: ego velocity config

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp

@@ -219,13 +219,12 @@ MarkerArray OcclusionSpotModule::createVirtualWallMarkerArray()
 
   MarkerArray wall_marker;
   std::string module_name = "occlusion_spot";
-  if (!debug_data_.possible_collisions.empty()) {
-    for (size_t id = 0; id < debug_data_.possible_collisions.size(); id++) {
-      const auto & pose = debug_data_.possible_collisions.at(id).intersection_pose;
-      appendMarkerArray(
-        motion_utils::createSlowDownVirtualWallMarker(pose, module_name, current_time, id),
-        &wall_marker, current_time);
-    }
+  for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) {
+    const auto p_front =
+      calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0);
+    appendMarkerArray(
+      motion_utils::createSlowDownVirtualWallMarker(p_front, module_name, current_time, id),
+      &wall_marker, current_time);
   }
   return wall_marker;
 }

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -283,16 +283,13 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot(
   const ArcCoordinates & arc_coord_occlusion_with_offset,
   const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param)
 {
-  auto calcPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) {
-    const auto & ll = pl.centerline2d();
+  auto calcPosition = [](const ConstLineString2d & ll, const ArcCoordinates & arc) {
     BasicPoint2d bp = fromArcCoordinates(ll, arc);
-    Pose pose;
-    pose.position.x = bp[0];
-    pose.position.y = bp[1];
-    pose.position.z = 0.0;
-    pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
-      lanelet::utils::getLaneletAngle(pl, pose.position));
-    return pose;
+    Point position;
+    position.x = bp[0];
+    position.y = bp[1];
+    position.z = 0.0;
+    return position;
   };
   /**
    * @brief calculate obstacle collision intersection from arc coordinate info.
@@ -316,11 +313,13 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot(
   pc.arc_lane_dist_at_collision = {distance_to_stop, arc_coord_occlusion_with_offset.distance};
   pc.obstacle_info.safe_motion = sm;
   pc.obstacle_info.ttv = ttv;
-  pc.obstacle_info.position = calcPose(path_lanelet, arc_coord_occlusion).position;
+
+  const auto & ll = path_lanelet.centerline2d();
+  pc.obstacle_info.position = calcPosition(ll, arc_coord_occlusion);
   pc.obstacle_info.max_velocity = param.pedestrian_vel;
-  pc.collision_pose = calcPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0});
-  pc.collision_with_margin.pose = calcPose(path_lanelet, {distance_to_stop, 0.0});
-  pc.intersection_pose = calcPose(path_lanelet, {arc_coord_occlusion.length, 0.0});
+  pc.collision_pose.position = calcPosition(ll, {arc_coord_occlusion_with_offset.length, 0.0});
+  pc.collision_with_margin.pose.position = calcPosition(ll, {distance_to_stop, 0.0});
+  pc.intersection_pose.position = calcPosition(ll, {arc_coord_occlusion.length, 0.0});
   return pc;
 }
 

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp

@@ -25,7 +25,7 @@ namespace occlusion_spot_utils
 {
 void applySafeVelocityConsideringPossibleCollision(
   PathWithLaneId * inout_path, std::vector<PossibleCollisionInfo> & possible_collisions,
-  const PlannerParam & param)
+  std::vector<geometry_msgs::msg::Pose> & debug_poses, const PlannerParam & param)
 {
   // return nullptr or too few points
   if (!inout_path || inout_path->points.size() < 2) {
@@ -60,7 +60,9 @@ void applySafeVelocityConsideringPossibleCollision(
     safe_velocity = std::max(safe_velocity, v_min);
     possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity;
     const auto & pose = possible_collision.collision_with_margin.pose;
-    planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity);
+    const auto & decel_pose =
+      planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity);
+    if (decel_pose) debug_poses.push_back(decel_pose.get());
   }
 }
 

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

@@ -183,7 +183,9 @@ bool OcclusionSpotModule::modifyPathVelocity(
   // Note: Consider offset from path start to ego here
   utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego);
   // apply safe velocity using ebs and pbs deceleration
-  utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_);
+  utils::applySafeVelocityConsideringPossibleCollision(
+    path, possible_collisions, debug_data_.debug_poses, param_);
+  debug_data_.baselink_to_front = param_.baselink_to_front;
   // these debug topics needs computation resource
   debug_data_.z = path->points.front().point.pose.position.z;
   debug_data_.possible_collisions = possible_collisions;