fix(behavior_velocity): visualization and interpolation (#351)

* fix(behavior_velocity): fix duplicated polygon

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): Add paths used by the occluded spot module to the debug visualization of BehaviorVelocityPlanner

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(behavior_velocity): add visualize marker and fix angle of last point

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(behavior_velocity): fix occlusion spot last angle after interpolation

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): better readability

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Author: taikitanaka3

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

@@ -48,6 +48,8 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface
     std::string road_type = "private";
     std::vector<lanelet::BasicPolygon2d> sidewalks;
     std::vector<occlusion_spot_utils::PossibleCollisionInfo> possible_collisions;
+    PathWithLaneId path_raw;
+    PathWithLaneId interp_path;
   };
 
   OcclusionSpotInPrivateModule(

planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp

@@ -129,9 +129,9 @@ bool splineInterpolate(
     output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
   }
   if (output->points.size() > 1) {
-    size_t l = resampled_s.size();
+    size_t l = output->points.size();
     output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation;
-    output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation;
+    output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation;
   }
   return true;
 }

planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp

@@ -102,9 +102,9 @@ bool splineInterpolate(
     output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
   }
   if (output->points.size() > 1) {
-    size_t l = resampled_s.size();
+    size_t l = output->points.size();
     output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation;
-    output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation;
+    output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation;
   }
   return true;
 }

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

@@ -149,7 +149,7 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
   debug_marker.action = visualization_msgs::msg::Marker::ADD;
   debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z);
   debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0);
-  debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.05, 0.05, 0.05);
+  debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1);
   debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3);
   debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1);
   debug_marker.ns = "sidewalk";
@@ -160,10 +160,41 @@ visualization_msgs::msg::MarkerArray makePolygonMarker(
       debug_marker.points.push_back(point);
     }
     debug_markers.markers.push_back(debug_marker);
+    debug_marker.id++;
+    debug_marker.points.clear();
   }
   return debug_markers;
 }
 
+visualization_msgs::msg::MarkerArray createPathMarkerArray(
+  const PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const double r,
+  const double g, const double b)
+{
+  visualization_msgs::msg::MarkerArray msg;
+  int32_t uid = planning_utils::bitShift(lane_id);
+  int32_t i = 0;
+  for (const auto & p : path.points) {
+    visualization_msgs::msg::Marker marker{};
+    marker.header.frame_id = "map";
+    marker.ns = ns;
+    marker.id = uid + i++;
+    marker.lifetime = rclcpp::Duration::from_seconds(0.3);
+    marker.type = visualization_msgs::msg::Marker::ARROW;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.pose = p.point.pose;
+    marker.scale = createMarkerScale(0.6, 0.3, 0.3);
+    if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) {
+      // if p.lane_ids has lane_id
+      marker.color = createMarkerColor(r, g, b, 0.999);
+    } else {
+      marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999);
+    }
+    msg.markers.push_back(marker);
+  }
+
+  return msg;
+}
+
 template <class T>
 visualization_msgs::msg::MarkerArray createMarkers(
   T & debug_data, [[maybe_unused]] const int64_t module_id_)
@@ -222,6 +253,12 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMa
   appendMarkerArray(createMarkers(debug_data_, module_id_), current_time, &debug_marker_array);
   appendMarkerArray(
     makePolygonMarker(debug_data_.sidewalks, debug_data_.z), current_time, &debug_marker_array);
+  appendMarkerArray(
+    createPathMarkerArray(debug_data_.path_raw, "path_raw", 0, 0.0, 1.0, 1.0), current_time,
+    &debug_marker_array);
+  appendMarkerArray(
+    createPathMarkerArray(debug_data_.interp_path, "path_interp", 0, 0.0, 1.0, 1.0), current_time,
+    &debug_marker_array);
   return debug_marker_array;
 }
 }  // namespace behavior_velocity_planner

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

@@ -123,9 +123,9 @@ bool splineInterpolate(
     output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
   }
   if (output->points.size() > 1) {
-    size_t l = resampled_s.size();
+    size_t l = output->points.size();
     output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation;
-    output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation;
+    output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation;
   }
   return true;
 }

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

@@ -68,6 +68,7 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity(
   utils::clipPathByLength(*path, clipped_path, max_range);
   PathWithLaneId interp_path;
   utils::splineInterpolate(clipped_path, 1.0, &interp_path, logger_);
+  debug_data_.interp_path = interp_path;
   int closest_idx = -1;
   if (!planning_utils::calcClosestIndex<PathWithLaneId>(
         interp_path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) {
@@ -103,6 +104,7 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity(
   utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_);
   debug_data_.z = path->points.front().point.pose.position.z;
   debug_data_.possible_collisions = possible_collisions;
+  debug_data_.path_raw = *path;
   return true;
 }