feat: resource check

.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/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_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) {

sensing/imu_corrector/CMakeLists.txt

@@ -14,19 +14,42 @@ endif()
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
+ament_auto_add_library(gyro_bias_estimation_module SHARED
+  src/gyro_bias_estimation_module.cpp
+)
+
 ament_auto_add_library(imu_corrector_node SHARED
   src/imu_corrector_core.cpp
-  include/imu_corrector/imu_corrector_core.hpp
 )
 
+ament_auto_add_library(gyro_bias_estimator_node SHARED
+  src/gyro_bias_estimator.cpp
+)
+
+target_link_libraries(gyro_bias_estimator_node gyro_bias_estimation_module)
+
 rclcpp_components_register_node(imu_corrector_node
   PLUGIN "imu_corrector::ImuCorrector"
   EXECUTABLE imu_corrector
 )
 
+rclcpp_components_register_node(gyro_bias_estimator_node
+  PLUGIN "imu_corrector::GyroBiasEstimator"
+  EXECUTABLE gyro_bias_estimator
+)
+
+function(add_testcase filepath)
+  get_filename_component(filename ${filepath} NAME)
+  string(REGEX REPLACE ".cpp" "" test_name ${filename})
+  ament_add_gmock(${test_name} ${filepath})
+  target_link_libraries("${test_name}" gyro_bias_estimation_module imu_corrector_node gyro_bias_estimator_node)
+  ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+endfunction()
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+  add_testcase(test/test_gyro_bias_estimation_module.cpp)
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE