Merge branch 'beta/x2_gen2/v0.29.0' into cherry-pick/aeb-latest-to-v4…

….0.0

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -556,14 +556,14 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
 
   // Check engage
   if (!is_engaged_) {
-    filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
+    filtered_commands.control = createStopControlCmd();
   }
 
   // Check pause. Place this check after all other checks as it needs the final output.
   adapi_pause_->update(filtered_commands.control);
   if (adapi_pause_->is_paused()) {
     if (is_engaged_) {
-      filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
+      filtered_commands.control = createStopControlCmd();
     } else {
       filtered_commands.control = createStopControlCmd();
     }

launch/tier4_system_launch/launch/system.launch.xml

@@ -22,6 +22,7 @@
 
   <arg name="launch_system_monitor" default="true" description="launch system monitor"/>
   <arg name="launch_dummy_diag_publisher" description="launch dummy diag publisher"/>
+  <arg name="launch_system_recover_operator" default="false" description="launch recover operator"/>
   <arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
   <arg name="sensor_model" description="sensor model name"/>
 
@@ -142,4 +143,9 @@
       <arg name="launch_rqt_runtime_monitor_err" value="$(var launch_rqt_runtime_monitor_err)"/>
     </include>
   </group>
+
+  <!-- System Recover Operator -->
+  <group if="$(var launch_system_recover_operator)">
+    <node pkg="diagnostic_graph_utils" exec="recovery_node" name="recovery"/>
+  </group>
 </launch>

perception/image_projection_based_fusion/src/fusion_node.cpp

@@ -54,8 +54,8 @@ FusionNode<TargetMsg3D, ObjType, Msg2D>::FusionNode(
   }
   if (rois_number_ > 8) {
     RCLCPP_WARN(
-      this->get_logger(), "maximum rois_number is 8. current rois_number is %zu", rois_number_);
-    rois_number_ = 8;
+      this->get_logger(),
+      "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number_);
   }
 
   // Set parameters

perception/lidar_centerpoint/include/lidar_centerpoint/postprocess/non_maximum_suppression.hpp

@@ -66,14 +66,14 @@ class NonMaximumSuppression
   std::vector<DetectedObject> apply(const std::vector<DetectedObject> &);
 
 private:
-  bool isTargetLabel(const std::uint8_t);
-
   bool isTargetPairObject(const DetectedObject &, const DetectedObject &);
 
   Eigen::MatrixXd generateIoUMatrix(const std::vector<DetectedObject> &);
 
   NMSParams params_{};
   std::vector<bool> target_class_mask_{};
+
+  double search_distance_2d_sq_{};
 };
 
 }  // namespace centerpoint

perception/lidar_centerpoint/lib/centerpoint_trt.cpp

@@ -115,15 +115,28 @@ bool CenterPointTRT::detect(
     cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_));
 
   if (!preprocess(input_pointcloud_msg, tf_buffer)) {
-    RCLCPP_WARN_STREAM(
-      rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect.");
+    RCLCPP_WARN(rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect.");
     return false;
   }
 
   inference();
 
   postProcess(det_boxes3d);
 
+  // Check the actual number of pillars after inference to avoid unnecessary synchronization.
+  unsigned int num_pillars = 0;
+  CHECK_CUDA_ERROR(
+    cudaMemcpy(&num_pillars, num_voxels_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost));
+
+  if (num_pillars >= config_.max_voxel_size_) {
+    rclcpp::Clock clock{RCL_ROS_TIME};
+    RCLCPP_WARN_THROTTLE(
+      rclcpp::get_logger("lidar_centerpoint"), clock, 1000,
+      "The actual number of pillars (%u) exceeds its maximum value (%zu). "
+      "Please considering increasing it since it may limit the detection performance.",
+      num_pillars, config_.max_voxel_size_);
+  }
+
   return true;
 }
 

perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp

@@ -19,38 +19,32 @@
 #include "object_recognition_utils/object_recognition_utils.hpp"
 namespace centerpoint
 {
+using Label = autoware_perception_msgs::msg::ObjectClassification;
 
 void NonMaximumSuppression::setParameters(const NMSParams & params)
 {
   assert(params.search_distance_2d_ >= 0.0);
   assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0);
 
   params_ = params;
+  search_distance_2d_sq_ = params.search_distance_2d_ * params.search_distance_2d_;
   target_class_mask_ = classNamesToBooleanMask(params.target_class_names_);
 }
 
-bool NonMaximumSuppression::isTargetLabel(const uint8_t label)
-{
-  if (label >= target_class_mask_.size()) {
-    return false;
-  }
-  return target_class_mask_.at(label);
-}
-
 bool NonMaximumSuppression::isTargetPairObject(
   const DetectedObject & object1, const DetectedObject & object2)
 {
   const auto label1 = object_recognition_utils::getHighestProbLabel(object1.classification);
   const auto label2 = object_recognition_utils::getHighestProbLabel(object2.classification);
 
-  if (isTargetLabel(label1) && isTargetLabel(label2)) {
-    return true;
+  // if labels are not the same, and one of them is pedestrian, do not suppress
+  if (label1 != label2 && (label1 == Label::PEDESTRIAN || label2 == Label::PEDESTRIAN)) {
+    return false;
   }
 
-  const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_;
   const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d(
     object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2));
-  return sqr_dist_2d <= search_sqr_dist_2d;
+  return sqr_dist_2d <= search_distance_2d_sq_;
 }
 
 Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md

@@ -107,7 +107,7 @@ If the target path contains a goal, modify the points of the path so that the pa
 - Route is set with `allow_goal_modification=true` .
   - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service.
   - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz.
-- ego-vehicle is in the same lane as the goal.
+- The terminal point of the current path is in the same lane sequence as the goal. If goal is on the road shoulder, then it is in the adjacent road lane sequence.
 
 <img src="https://user-images.githubusercontent.com/39142679/237929950-989ca6c3-d48c-4bb5-81e5-e8d6a38911aa.png" width="600">
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -266,6 +266,7 @@ struct GoalPlannerDebugData
   FreespacePlannerDebugData freespace_planner{};
   std::vector<Polygon2d> ego_polygons_expanded{};
   lanelet::ConstLanelet expanded_pull_over_lane_between_ego{};
+  Polygon2d objects_extraction_polygon{};
 };
 
 struct LastApprovalData

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp

@@ -31,13 +31,12 @@ class GoalSearcher : public GoalSearcherBase
 public:
   GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);
 
-  GoalCandidates search(
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) override;
+  GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) override;
   void update(
     GoalCandidates & goal_candidates,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const override;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const override;
 
   // todo(kosuke55): Functions for this specific use should not be in the interface,
   // so it is better to consider interface design when we implement other goal searchers.
@@ -47,7 +46,8 @@ class GoalSearcher : public GoalSearcherBase
   bool isSafeGoalWithMarginScaleFactor(
     const GoalCandidate & goal_candidate, const double margin_scale_factor,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const override;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const override;
 
 private:
   void countObjectsToAvoid(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp

@@ -53,13 +53,12 @@ class GoalSearcherBase
   const Pose & getReferenceGoal() const { return reference_goal_pose_; }
 
   MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
-  virtual GoalCandidates search(
-    const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) = 0;
+  virtual GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) = 0;
   virtual void update(
     [[maybe_unused]] GoalCandidates & goal_candidates,
     [[maybe_unused]] const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data) const
+    [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data,
+    [[maybe_unused]] const PredictedObjects & objects) const
   {
     return;
   }
@@ -69,7 +68,8 @@ class GoalSearcherBase
   virtual bool isSafeGoalWithMarginScaleFactor(
     const GoalCandidate & goal_candidate, const double margin_scale_factor,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const std::shared_ptr<const PlannerData> & planner_data) const = 0;
+    const std::shared_ptr<const PlannerData> & planner_data,
+    const PredictedObjects & objects) const = 0;
 
 protected:
   GoalPlannerParameters parameters_{};

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/shift_pull_over.hpp

@@ -43,7 +43,7 @@ class ShiftPullOver : public PullOverPlannerBase
   std::optional<PathWithLaneId> cropPrevModulePath(
     const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const;
   std::optional<PullOverPath> generatePullOverPath(
-    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
     const Pose & goal_pose, const double lateral_jerk) const;
   static double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double after_shifted_arc_length, const double dr);

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -62,6 +62,19 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
   const geometry_msgs::msg::Pose ego_pose,
   const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset,
   const double inner_road_offset);
+
+/*
+ * @brief generate polygon to extract objects
+ * @param pull_over_lanes pull over lanes
+ * @param left_side left side or right side
+ * @param outer_offset outer offset from pull over lane boundary
+ * @param inner_offset inner offset from pull over lane boundary
+ * @return polygon to extract objects
+ */
+std::optional<Polygon2d> generateObjectExtractionPolygon(
+  const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, const double outer_offset,
+  const double inner_offset);
+
 PredictedObjects extractObjectsInExpandedPullOverLanes(
   const RouteHandler & route_handler, const bool left_side, const double backward_distance,
   const double forward_distance, double bound_offset, const PredictedObjects & objects);
@@ -82,12 +95,31 @@ bool isReferencePath(
 std::optional<PathWithLaneId> cropPath(const PathWithLaneId & path, const Pose & end_pose);
 PathWithLaneId cropForwardPoints(
   const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length);
+
+/**
+ * @brief extend target_path by extend_length
+ * @param target_path original target path to extend
+ * @param reference_path reference path to extend
+ * @param extend_length length to extend
+ * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of
+ *                                       target_path has zero velocity
+ * @return extended path
+ */
 PathWithLaneId extendPath(
-  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
-  const double extend_length);
+  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
+  const double extend_length, const bool remove_connected_zero_velocity);
+/**
+ * @brief extend target_path to extend_pose
+ * @param target_path original target path to extend
+ * @param reference_path reference path to extend
+ * @param extend_pose pose to extend
+ * @param remove_connected_zero_velocity flag to remove zero velocity if the last point of
+ *                                       target_path has zero velocity
+ * @return extended path
+ */
 PathWithLaneId extendPath(
-  const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path,
-  const Pose & extend_pose);
+  const PathWithLaneId & target_path, const PathWithLaneId & reference_path,
+  const Pose & extend_pose, const bool remove_connected_zero_velocity);
 
 std::vector<Polygon2d> createPathFootPrints(
   const PathWithLaneId & path, const double base_to_front, const double base_to_rear,
@@ -109,6 +141,23 @@ MarkerArray createLaneletPolygonMarkerArray(
 MarkerArray createNumObjectsToAvoidTextsMarkerArray(
   const GoalCandidates & goal_candidates, std::string && ns,
   const std_msgs::msg::ColorRGBA & color);
+
+/**
+ * @brief combine two points
+ * @param points lane points
+ * @param points_next next lane points
+ * @return combined points
+ */
+lanelet::Points3d combineLanePoints(
+  const lanelet::Points3d & points, const lanelet::Points3d & points_next);
+/** @brief Create a lanelet that represents the departure check area.
+ * @param [in] pull_over_lanes Lanelets that the vehicle will pull over to.
+ * @param [in] route_handler RouteHandler object.
+ * @return Lanelet that goal footprints should be inside.
+ */
+lanelet::Lanelet createDepartureCheckLanelet(
+  const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler,
+  const bool left_side_parking);
 }  // namespace autoware::behavior_path_planner::goal_planner_utils
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp

@@ -51,7 +51,6 @@ std::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
   if (road_lanes.empty() || pull_over_lanes.empty()) {
     return {};
   }
-  const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes);
 
   const auto & p = parallel_parking_parameters_;
   const double max_steer_angle =
@@ -65,10 +64,12 @@ std::optional<PullOverPath> GeometricPullOver::plan(const Pose & goal_pose)
     return {};
   }
 
+  const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet(
+    pull_over_lanes, *planner_data_->route_handler, left_side_parking_);
   const auto arc_path = planner_.getArcPath();
 
   // check lane departure with road and shoulder lanes
-  if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {};
+  if (lane_departure_checker_.checkPathWillLeaveLane({departure_check_lane}, arc_path)) return {};
 
   PullOverPath pull_over_path{};
   pull_over_path.type = getPlannerType();