Merge branch 'beta/v0.3.19' into feat/localization/pose_instability_d…

…etector/calculate_at_msg_callback

.pre-commit-config.yaml

@@ -35,8 +35,9 @@ repos:
       - id: yamllint
 
   - repo: https://github.com/tier4/pre-commit-hooks-ros
-    rev: v0.6.0
+    rev: v0.8.0
     hooks:
+      - id: flake8-ros
       - id: prettier-xacro
       - id: prettier-launch-xml
       - id: prettier-package-xml
@@ -65,22 +66,6 @@ repos:
       - id: black
         args: [--line-length=100]
 
-  - repo: https://github.com/PyCQA/flake8
-    rev: 4.0.1
-    hooks:
-      - id: flake8
-        additional_dependencies:
-          [
-            flake8-blind-except,
-            flake8-builtins,
-            flake8-class-newline,
-            flake8-comprehensions,
-            flake8-deprecated,
-            flake8-docstrings,
-            flake8-import-order,
-            flake8-quotes,
-          ]
-
   - repo: https://github.com/pre-commit/mirrors-clang-format
     rev: v13.0.1
     hooks:

common/tier4_debug_tools/package.xml

@@ -17,8 +17,8 @@
   <depend>tier4_debug_msgs</depend>
 
   <exec_depend>launch_ros</exec_depend>
-  <exec_depend>rclpy</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-rtree</exec_depend>
+  <exec_depend>rclpy</exec_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

perception/elevation_map_loader/src/elevation_map_loader_node.cpp

@@ -102,7 +102,8 @@ void ElevationMapLoaderNode::publish()
     try {
       is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag(
         *data_manager_.elevation_map_path_, "elevation_map", elevation_map_);
-    } catch (rosbag2_storage_plugins::SqliteException & e) {
+    } catch (const std::runtime_error & e) {
+      RCLCPP_ERROR(this->get_logger(), e.what());
       is_bag_loaded = false;
     }
     if (!is_bag_loaded) {

planning/behavior_path_planner/src/utilities.cpp

@@ -26,29 +26,6 @@
 #include <string>
 #include <vector>
 
-namespace tier4_autoware_utils
-{
-template <class T>
-double calcLateralOffset(
-  const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx)
-{
-  validateNonEmpty(points);
-
-  const auto p_front = getPoint(points.at(seg_idx));
-  const auto p_back = getPoint(points.at(seg_idx + 1));
-
-  const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0};
-  const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0};
-
-  if (segment_vec.norm() == 0.0) {
-    throw std::runtime_error("Same points are given.");
-  }
-
-  const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec);
-  return cross_vec(2) / segment_vec.norm();
-}
-}  // namespace tier4_autoware_utils
-
 namespace drivable_area_utils
 {
 double quantize(const double val, const double resolution)

planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp

@@ -62,7 +62,8 @@ class DetectionAreaModule : public SceneModuleInterface
 
 public:
   DetectionAreaModule(
-    const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem,
+    const int64_t module_id, const size_t lane_id,
+    const lanelet::autoware::DetectionArea & detection_area_reg_elem,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
     const rclcpp::Clock::SharedPtr clock);
 
@@ -106,6 +107,8 @@ class DetectionAreaModule : public SceneModuleInterface
 
   // Debug
   DebugData debug_data_;
+
+  int64_t lane_id_;
 };
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp

@@ -28,22 +28,24 @@ namespace behavior_velocity_planner
 {
 namespace
 {
-std::vector<lanelet::DetectionAreaConstPtr> getDetectionAreaRegElemsOnPath(
+using DetectionAreaWithLaneId = std::pair<lanelet::DetectionAreaConstPtr, int64_t>;
+
+std::vector<DetectionAreaWithLaneId> getDetectionAreaRegElemsOnPath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const lanelet::LaneletMapPtr lanelet_map)
 {
-  std::vector<lanelet::DetectionAreaConstPtr> detection_area_reg_elems;
+  std::vector<DetectionAreaWithLaneId> detection_areas_with_lane_id;
 
   for (const auto & p : path.points) {
     const auto lane_id = p.lane_ids.at(0);
     const auto ll = lanelet_map->laneletLayer.get(lane_id);
     const auto detection_areas = ll.regulatoryElementsAs<const lanelet::autoware::DetectionArea>();
     for (const auto & detection_area : detection_areas) {
-      detection_area_reg_elems.push_back(detection_area);
+      detection_areas_with_lane_id.push_back(std::make_pair(detection_area, lane_id));
     }
   }
 
-  return detection_area_reg_elems;
+  return detection_areas_with_lane_id;
 }
 
 std::set<int64_t> getDetectionAreaIdSetOnPath(
@@ -52,7 +54,7 @@ std::set<int64_t> getDetectionAreaIdSetOnPath(
 {
   std::set<int64_t> detection_area_id_set;
   for (const auto & detection_area : getDetectionAreaRegElemsOnPath(path, lanelet_map)) {
-    detection_area_id_set.insert(detection_area->id());
+    detection_area_id_set.insert(detection_area.first->id());
   }
   return detection_area_id_set;
 }
@@ -72,14 +74,15 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node)
 void DetectionAreaModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  for (const auto & detection_area :
+  for (const auto & detection_area_with_lane_id :
        getDetectionAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
     // Use lanelet_id to unregister module when the route is changed
-    const auto module_id = detection_area->id();
+    const auto module_id = detection_area_with_lane_id.first->id();
+    const auto lane_id = detection_area_with_lane_id.second;
     if (!isModuleRegistered(module_id)) {
       registerModule(std::make_shared<DetectionAreaModule>(
-        module_id, *detection_area, planner_param_, logger_.get_child("detection_area_module"),
-        clock_));
+        module_id, lane_id, *(detection_area_with_lane_id.first), planner_param_,
+        logger_.get_child("detection_area_module"), clock_));
     }
   }
 }

planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp

@@ -69,9 +69,13 @@ boost::optional<Point2d> getNearestCollisionPoint(
 }
 
 boost::optional<PathIndexWithPoint2d> findCollisionSegment(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line)
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
+  const SearchRangeIndex & search_index)
 {
-  for (size_t i = 0; i < path.points.size() - 1; ++i) {
+  const auto min_search_index = std::max(static_cast<size_t>(0), search_index.min_idx);
+  const auto max_search_index = std::min(search_index.max_idx, path.points.size() - 1);
+
+  for (size_t i = min_search_index; i < max_search_index; ++i) {
     const auto & p1 = path.points.at(i).point.pose.position;      // Point before collision point
     const auto & p2 = path.points.at(i + 1).point.pose.position;  // Point after collision point
 
@@ -186,13 +190,15 @@ geometry_msgs::msg::Pose calcTargetPose(
 }  // namespace
 
 DetectionAreaModule::DetectionAreaModule(
-  const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem,
+  const int64_t module_id, const size_t lane_id,
+  const lanelet::autoware::DetectionArea & detection_area_reg_elem,
   const PlannerParam & planner_param, const rclcpp::Logger logger,
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   detection_area_reg_elem_(detection_area_reg_elem),
   state_(State::GO),
-  planner_param_(planner_param)
+  planner_param_(planner_param),
+  lane_id_(lane_id)
 {
 }
 
@@ -351,8 +357,13 @@ bool DetectionAreaModule::isOverLine(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const
 {
-  return tier4_autoware_utils::calcSignedArcLength(
-           path.points, self_pose.position, line_pose.position) < 0;
+  const PointWithSearchRangeIndex src_point_with_search_range_index =
+    planning_utils::findFirstNearSearchRangeIndex(path.points, self_pose.position);
+  const PointWithSearchRangeIndex dst_point_with_search_range_index = {
+    line_pose.position, planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_)};
+
+  return planning_utils::calcSignedArcLengthWithSearchIndex(
+           path.points, src_point_with_search_range_index, dst_point_with_search_range_index) < 0;
 }
 
 bool DetectionAreaModule::hasEnoughBrakingDistance(
@@ -392,8 +403,10 @@ boost::optional<PathIndexWithPose> DetectionAreaModule::createTargetPoint(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
   const double margin) const
 {
+  const auto dst_search_range = planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_);
+
   // Find collision segment
-  const auto collision_segment = findCollisionSegment(path, stop_line);
+  const auto collision_segment = findCollisionSegment(path, stop_line, dst_search_range);
   if (!collision_segment) {
     // No collision
     return {};

planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp

@@ -12,6 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <scene_module/virtual_traffic_light/manager.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
 
@@ -81,11 +84,28 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
 void VirtualTrafficLightModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
+  tier4_autoware_utils::LineString2d ego_path_linestring;
+  for (const auto & path_point : path.points) {
+    ego_path_linestring.push_back(
+      tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d());
+  }
+
   for (const auto & m : getRegElemMapOnPath<VirtualTrafficLight>(
          path, planner_data_->route_handler_->getLaneletMapPtr())) {
+    const auto stop_line_opt = m.first->getStopLine();
+    if (!stop_line_opt) {
+      RCLCPP_FATAL(
+        logger_, "No stop line at virtual_traffic_light_reg_elem_id = %ld, please fix the map!",
+        m.first->id());
+      continue;
+    }
+
     // Use lanelet_id to unregister module when the route is changed
     const auto module_id = m.second.id();
-    if (!isModuleRegistered(module_id)) {
+    if (
+      !isModuleRegistered(module_id) &&
+      boost::geometry::intersects(
+        ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) {
       registerModule(std::make_shared<VirtualTrafficLightModule>(
         module_id, *m.first, m.second, planner_param_,
         logger_.get_child("virtual_traffic_light_module"), clock_));

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -53,6 +53,10 @@ template <typename T>
 boost::optional<geometry_msgs::msg::Pose> lerpPose(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return {};
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =
@@ -94,6 +98,10 @@ template <typename T>
 double lerpTwistX(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return 0.0;
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =
@@ -116,6 +124,10 @@ template <typename T>
 double lerpPoseZ(
   const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx)
 {
+  if (points.size() < 2) {
+    return 0.0;
+  }
+
   constexpr double epsilon = 1e-6;
 
   const double closest_to_target_dist =

planning/obstacle_avoidance_planner/src/node.cpp

@@ -1093,6 +1093,9 @@ void ObstacleAvoidancePlanner::calcVelocity(
   const std::vector<autoware_auto_planning_msgs::msg::PathPoint> & path_points,
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & traj_points) const
 {
+  if (path_points.size() < 2) {
+    return;
+  }
   for (size_t i = 0; i < traj_points.size(); i++) {
     const size_t nearest_seg_idx = [&]() {
       const auto opt_seg_idx = tier4_autoware_utils::findNearestSegmentIndex(

planning/obstacle_avoidance_planner/src/utils.cpp

@@ -343,26 +343,29 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw);
 
   // spline interpolation
-  const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
-  const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
-  const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s);
-
-  for (size_t i = 0; i < interpolated_x.size(); i++) {
-    if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
-      return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
+  try {
+    const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
+    const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
+    const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s);
+
+    for (size_t i = 0; i < interpolated_x.size(); i++) {
+      if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
+        return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+      }
     }
-  }
 
-  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
-  for (size_t i = 0; i < interpolated_x.size(); i++) {
-    autoware_auto_planning_msgs::msg::TrajectoryPoint point;
-    point.pose.position.x = interpolated_x[i];
-    point.pose.position.y = interpolated_y[i];
-    point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
-      tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
-    interpolated_points.push_back(point);
+    for (size_t i = 0; i < interpolated_x.size(); i++) {
+      autoware_auto_planning_msgs::msg::TrajectoryPoint point;
+      point.pose.position.x = interpolated_x[i];
+      point.pose.position.y = interpolated_y[i];
+      point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
+        tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
+      interpolated_points.push_back(point);
+    }
+  } catch (const std::invalid_argument & e) {
+    return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }
-
   return interpolated_points;
 }
 

planning/obstacle_stop_planner/src/node.cpp

@@ -753,6 +753,10 @@ void ObstacleStopPlannerNode::insertVelocity(
   const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info,
   const double current_acc, const double current_vel, const StopParam & stop_param)
 {
+  if (output.size() < 3) {
+    return;
+  }
+
   if (planner_data.stop_require) {
     // insert stop point
     const auto traj_end_idx = output.size() - 1;

planning/planning_error_monitor/README.md

@@ -47,7 +47,7 @@ This function checks if the relative angle at point1 generated from point2 and 3
 | :------------------------ | :------- | :------------------------------------ | :------------ |
 | `error_interval`          | `double` | Error Interval Distance Threshold [m] | 100.0         |
 | `error_curvature`         | `double` | Error Curvature Threshold             | 1.0           |
-| `error_sharp_angle`       | `double` | Error Sharp Angle Threshold           | $\pi$/4       |
+| `error_sharp_angle`       | `double` | Error Sharp Angle Threshold           | 2.0           |
 | `ignore_too_close_points` | `double` | Ignore Too Close Distance Threshold   | 0.005         |
 
 ## Visualization

planning/planning_error_monitor/launch/planning_error_monitor.launch.xml

@@ -9,7 +9,7 @@
     <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
     <param name="error_interval" value="100.0"/>
     <param name="error_curvature" value="2.0"/>
-    <param name="error_sharp_angle" value="0.785398"/>
+    <param name="error_sharp_angle" value="2.0"/>
     <param name="ignore_too_close_points" value="0.01"/>
   </node>
 </launch>

planning/route_handler/src/route_handler.cpp

@@ -121,7 +121,7 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path)
       continue;
     }
 
-    constexpr double min_dist = 0.001;
+    constexpr double min_dist = 0.1;
     if (
       tier4_autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) <
       min_dist) {