Merge pull request #1107 from tier4/sync-awf-latest

chore: sync awf-latest

planning/behavior_path_goal_planner_module/README.md

@@ -155,6 +155,7 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio
 | use_object_recognition                                       | [-]  | bool   | flag whether to use object recognition for collision check                                               | true          |
 | object_recognition_collision_check_margin                    | [m]  | double | margin to calculate ego-vehicle cells from footprint.                                                    | 0.6           |
 | object_recognition_collision_check_max_extra_stopping_margin | [m]  | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0           |
+| detection_bound_offset                                       | [m]  | double | expand pull over lane with this offset to make detection area for collision check of path generation     | 15.0          |
 
 ## **Goal Search**
 

planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml

@@ -38,6 +38,7 @@
         object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker
         object_recognition_collision_check_max_extra_stopping_margin: 1.0
         th_moving_object_velocity: 1.0
+        detection_bound_offset: 15.0
 
       # pull over
       pull_over:

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -254,6 +254,29 @@ struct PreviousPullOverData
   bool has_decided_path{false};
 };
 
+// store stop_pose_ pointer with reason string
+struct PoseWithString
+{
+  std::optional<Pose> * pose;
+  std::string string;
+
+  explicit PoseWithString(std::optional<Pose> * shared_pose) : pose(shared_pose), string("") {}
+
+  void set(const Pose & new_pose, const std::string & new_string)
+  {
+    *pose = new_pose;
+    string = new_string;
+  }
+
+  void set(const std::string & new_string) { string = new_string; }
+
+  void clear()
+  {
+    pose->reset();
+    string = "";
+  }
+};
+
 class GoalPlannerModule : public SceneModuleInterface
 {
 public:
@@ -385,6 +408,7 @@ class GoalPlannerModule : public SceneModuleInterface
 
   // debug
   mutable GoalPlannerDebugData debug_data_;
+  mutable PoseWithString debug_stop_pose_with_info_;
 
   // collision check
   void initializeOccupancyGridMap();

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp

@@ -74,6 +74,7 @@ struct GoalPlannerParameters
   double object_recognition_collision_check_margin{0.0};
   double object_recognition_collision_check_max_extra_stopping_margin{0.0};
   double th_moving_object_velocity{0.0};
+  double detection_bound_offset{0.0};
 
   // pull over general params
   double pull_over_minimum_request_length{0.0};

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -61,7 +61,8 @@ GoalPlannerModule::GoalPlannerModule(
 : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{parameters},
   vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()},
-  thread_safe_data_{mutex_, clock_}
+  thread_safe_data_{mutex_, clock_},
+  debug_stop_pose_with_info_{&stop_pose_}
 {
   LaneDepartureChecker lane_departure_checker{};
   lane_departure_checker.setVehicleInfo(vehicle_info_);
@@ -275,6 +276,10 @@ void GoalPlannerModule::onTimer()
 
 void GoalPlannerModule::onFreespaceParkingTimer()
 {
+  if (getCurrentStatus() == ModuleStatus::IDLE) {
+    return;
+  }
+
   if (!planner_data_) {
     return;
   }
@@ -841,10 +846,13 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const
   } else {
     // not_safe -> not_safe: use previous stop path
     output.path = *prev_data_.stop_path;
-    // stop_pose_ is removed in manager every loop, so need to set every loop.
-    stop_pose_ = utils::getFirstStopPoseFromPath(output.path);
     RCLCPP_WARN_THROTTLE(
       getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path");
+    // stop_pose_ is removed in manager every loop, so need to set every loop.
+    const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path);
+    if (stop_pose_opt) {
+      debug_stop_pose_with_info_.set(stop_pose_opt.value(), std::string("keep previous stop pose"));
+    }
   }
 }
 
@@ -859,6 +867,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output
         parameters_->maximum_jerk_for_stop);
     if (stop_path) {
       output.path = *stop_path;
+      debug_stop_pose_with_info_.set(std::string("feasible stop after approval"));
       RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path");
     } else {
       output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
@@ -870,7 +879,11 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output
     // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path
     output.path = *prev_data_.stop_path_after_approval;
     // stop_pose_ is removed in manager every loop, so need to set every loop.
-    stop_pose_ = utils::getFirstStopPoseFromPath(output.path);
+    const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path);
+    if (stop_pose_opt) {
+      debug_stop_pose_with_info_.set(
+        stop_pose_opt.value(), std::string("keep feasible stop after approval"));
+    }
     RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path");
   }
 }
@@ -1014,6 +1027,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate()
     return output;
   }
 
+  setDebugData();
+
   return output;
 }
 
@@ -1238,37 +1253,47 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const
   //     (In the case of the curve lane, the position is not aligned due to the
   //     difference between the outer and inner sides)
   // 4. feasible stop
-  const auto stop_pose = std::invoke([&]() -> std::optional<Pose> {
-    if (thread_safe_data_.foundPullOverPath()) {
-      return thread_safe_data_.get_pull_over_path()->start_pose;
-    }
-    if (thread_safe_data_.get_closest_start_pose()) {
-      return thread_safe_data_.get_closest_start_pose().value();
-    }
-    if (!decel_pose) {
-      return std::nullopt;
-    }
-    return decel_pose.value();
-  });
-  if (!stop_pose) {
-    return generateFeasibleStopPath();
+  const auto stop_pose_with_info =
+    std::invoke([&]() -> std::optional<std::pair<Pose, std::string>> {
+      if (thread_safe_data_.foundPullOverPath()) {
+        return std::make_pair(
+          thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose");
+      }
+      if (thread_safe_data_.get_closest_start_pose()) {
+        return std::make_pair(
+          thread_safe_data_.get_closest_start_pose().value(), "stop at closest start pose");
+      }
+      if (!decel_pose) {
+        return std::nullopt;
+      }
+      return std::make_pair(decel_pose.value(), "stop at search start pose");
+    });
+  if (!stop_pose_with_info) {
+    const auto feasible_stop_path = generateFeasibleStopPath();
+    // override stop pose info debug string
+    debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose"));
+    return feasible_stop_path;
   }
+  const Pose stop_pose = stop_pose_with_info->first;
 
   // if stop pose is closer than min_stop_distance, stop as soon as possible
-  const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, *stop_pose);
+  const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, stop_pose);
   const auto min_stop_distance = calcFeasibleDecelDistance(
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0);
   const double eps_vel = 0.01;
   const bool is_stopped = std::abs(current_vel) < eps_vel;
   const double buffer = is_stopped ? stop_distance_buffer_ : 0.0;
   if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) {
-    return generateFeasibleStopPath();
+    const auto feasible_stop_path = generateFeasibleStopPath();
+    debug_stop_pose_with_info_.set(
+      std::string("feasible stop: stop pose is closer than min_stop_distance"));
+    return feasible_stop_path;
   }
 
   // slow down for turn signal, insert stop point to stop_pose
   auto stop_path = extended_prev_path;
-  decelerateForTurnSignal(*stop_pose, stop_path);
-  stop_pose_ = *stop_pose;  // for debug wall marker
+  decelerateForTurnSignal(stop_pose, stop_path);
+  debug_stop_pose_with_info_.set(stop_pose, stop_pose_with_info->second);
 
   // slow down before the search area.
   if (decel_pose) {
@@ -1306,7 +1331,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const
   const auto stop_idx =
     motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points);
   if (stop_idx) {
-    stop_pose_ = stop_path.points.at(*stop_idx).point.pose;
+    debug_stop_pose_with_info_.set(stop_path.points.at(*stop_idx).point.pose, "feasible stop");
   }
 
   return stop_path;
@@ -1487,9 +1512,16 @@ bool GoalPlannerModule::checkObjectsCollision(
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length);
+
+  const auto expanded_pull_over_lanes =
+    left_side_parking_ ? lanelet::utils::getExpandedLanelets(
+                           pull_over_lanes, parameters_->detection_bound_offset, 0.0)
+                       : lanelet::utils::getExpandedLanelets(
+                           pull_over_lanes, 0.0, parameters_->detection_bound_offset);
+
   const auto [pull_over_lane_objects, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      *(planner_data_->dynamic_object), pull_over_lanes,
+      *(planner_data_->dynamic_object), expanded_pull_over_lanes,
       utils::path_safety_checker::isPolygonOverlapLanelet);
   const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
     pull_over_lane_objects, parameters_->th_moving_object_velocity);
@@ -2080,6 +2112,20 @@ void GoalPlannerModule::setDebugData()
     planner_type_marker_array.markers.push_back(marker);
     add(planner_type_marker_array);
   }
+
+  // Visualize stop pose info
+  if (debug_stop_pose_with_info_.pose->has_value()) {
+    visualization_msgs::msg::MarkerArray stop_pose_marker_array{};
+    const auto color = createMarkerColor(1.0, 1.0, 1.0, 0.99);
+    auto marker = createDefaultMarker(
+      header.frame_id, header.stamp, "stop_pose_info", 0,
+      visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 0.5), color);
+    marker.pose = debug_stop_pose_with_info_.pose->value();
+    marker.text = debug_stop_pose_with_info_.string;
+    stop_pose_marker_array.markers.push_back(marker);
+    add(stop_pose_marker_array);
+    add(createPoseMarkerArray(marker.pose, "stop_pose", 1.0, 1.0, 1.0, 0.9));
+  }
 }
 
 void GoalPlannerModule::printParkingPositionError() const

planning/behavior_path_goal_planner_module/src/manager.cpp

@@ -101,6 +101,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
       node->declare_parameter<double>(
         ns + "object_recognition_collision_check_max_extra_stopping_margin");
     p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
+    p.detection_bound_offset = node->declare_parameter<double>(ns + "detection_bound_offset");
   }
 
   // pull over general params

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -86,6 +86,10 @@ StartPlannerModule::StartPlannerModule(
 
 void StartPlannerModule::onFreespacePlannerTimer()
 {
+  if (getCurrentStatus() == ModuleStatus::IDLE) {
+    return;
+  }
+
   if (!planner_data_) {
     return;
   }
@@ -96,7 +100,11 @@ void StartPlannerModule::onFreespacePlannerTimer()
 
   const bool is_new_costmap =
     (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0;
-  if (isStuck() && is_new_costmap) {
+  if (!is_new_costmap) {
+    return;
+  }
+
+  if (isStuck()) {
     planFreespacePath();
   }
 }

planning/planning_validator/include/planning_validator/planning_validator.hpp

@@ -64,6 +64,7 @@ class PlanningValidator : public rclcpp::Node
 
   void onTrajectory(const Trajectory::ConstSharedPtr msg);
 
+  bool checkValidSize(const Trajectory & trajectory) const;
   bool checkValidFiniteValue(const Trajectory & trajectory);
   bool checkValidInterval(const Trajectory & trajectory);
   bool checkValidRelativeAngle(const Trajectory & trajectory);
@@ -116,7 +117,7 @@ class PlanningValidator : public rclcpp::Node
 
   vehicle_info_util::VehicleInfo vehicle_info_;
 
-  bool isAllValid(const PlanningValidatorStatus & status);
+  bool isAllValid(const PlanningValidatorStatus & status) const;
 
   Trajectory::ConstSharedPtr current_trajectory_;
   Trajectory::ConstSharedPtr previous_published_trajectory_;

planning/planning_validator/msg/PlanningValidatorStatus.msg

@@ -1,6 +1,7 @@
 builtin_interfaces/Time stamp
 
 # states
+bool is_valid_size
 bool is_valid_finite_value
 bool is_valid_interval
 bool is_valid_relative_angle

planning/planning_validator/src/planning_validator.cpp

@@ -113,6 +113,9 @@ void PlanningValidator::setupDiag()
   d->setHardwareID("planning_validator");
 
   std::string ns = "trajectory_validation_";
+  d->add(ns + "size", [&](auto & stat) {
+    setStatus(stat, validation_status_.is_valid_size, "invalid trajectory size is found");
+  });
   d->add(ns + "finite", [&](auto & stat) {
     setStatus(stat, validation_status_.is_valid_finite_value, "infinite value is found");
   });
@@ -251,14 +254,26 @@ void PlanningValidator::publishDebugInfo()
 
 void PlanningValidator::validate(const Trajectory & trajectory)
 {
-  if (trajectory.points.size() < 2) {
-    RCLCPP_ERROR(get_logger(), "trajectory size is less than 2. Cannot validate.");
-    return;
-  }
-
   auto & s = validation_status_;
 
+  const auto terminateValidation = [&](const auto & ss) {
+    RCLCPP_ERROR_STREAM(get_logger(), ss);
+    s.invalid_count += 1;
+  };
+
+  s.is_valid_size = checkValidSize(trajectory);
+  if (!s.is_valid_size) {
+    return terminateValidation(
+      "trajectory has invalid point size (" + std::to_string(trajectory.points.size()) +
+      "). Stop validation process, raise an error.");
+  }
+
   s.is_valid_finite_value = checkValidFiniteValue(trajectory);
+  if (!s.is_valid_finite_value) {
+    return terminateValidation(
+      "trajectory has invalid value (NaN, Inf, etc). Stop validation process, raise an error.");
+  }
+
   s.is_valid_interval = checkValidInterval(trajectory);
   s.is_valid_lateral_acc = checkValidLateralAcceleration(trajectory);
   s.is_valid_longitudinal_max_acc = checkValidMaxLongitudinalAcceleration(trajectory);
@@ -279,6 +294,11 @@ void PlanningValidator::validate(const Trajectory & trajectory)
   s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
 }
 
+bool PlanningValidator::checkValidSize(const Trajectory & trajectory) const
+{
+  return trajectory.points.size() >= 2;
+}
+
 bool PlanningValidator::checkValidFiniteValue(const Trajectory & trajectory)
 {
   for (const auto & p : trajectory.points) {
@@ -427,12 +447,13 @@ bool PlanningValidator::checkValidDistanceDeviation(const Trajectory & trajector
   return true;
 }
 
-bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s)
+bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
 {
-  return s.is_valid_finite_value && s.is_valid_interval && s.is_valid_relative_angle &&
-         s.is_valid_curvature && s.is_valid_lateral_acc && s.is_valid_longitudinal_max_acc &&
-         s.is_valid_longitudinal_min_acc && s.is_valid_steering && s.is_valid_steering_rate &&
-         s.is_valid_velocity_deviation && s.is_valid_distance_deviation;
+  return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval &&
+         s.is_valid_relative_angle && s.is_valid_curvature && s.is_valid_lateral_acc &&
+         s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc &&
+         s.is_valid_steering && s.is_valid_steering_rate && s.is_valid_velocity_deviation &&
+         s.is_valid_distance_deviation;
 }
 
 void PlanningValidator::displayStatus()
@@ -447,6 +468,7 @@ void PlanningValidator::displayStatus()
 
   const auto & s = validation_status_;
 
+  warn(s.is_valid_size, "planning trajectory size is invalid, too small.");
   warn(s.is_valid_curvature, "planning trajectory curvature is too large!!");
   warn(s.is_valid_distance_deviation, "planning trajectory is too far from ego!!");
   warn(s.is_valid_finite_value, "planning trajectory has invalid value!!");