feat: resource check

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

sensing/imu_corrector/README.md

@@ -1,6 +1,6 @@
 # imu_corrector
 
-## Purpose
+## imu_corrector
 
 `imu_corrector_node` is a node that correct imu data.
 
@@ -10,8 +10,6 @@
 <!-- TODO(TIER IV): Make this repository public or change the link. -->
 <!-- Use the value estimated by [deviation_estimator](https://github.com/tier4/calibration_tools/tree/main/localization/deviation_estimation_tools) as the parameters for this node. -->
 
-## Inputs / Outputs
-
 ### Input
 
 | Name     | Type                    | Description  |
@@ -24,9 +22,7 @@
 | --------- | ----------------------- | ------------------ |
 | `~output` | `sensor_msgs::msg::Imu` | corrected imu data |
 
-## Parameters
-
-### Core Parameters
+### Parameters
 
 | Name                         | Type   | Description                           |
 | ---------------------------- | ------ | ------------------------------------- |
@@ -37,12 +33,33 @@
 | `angular_velocity_stddev_yy` | double | pitch rate standard deviation [rad/s] |
 | `angular_velocity_stddev_zz` | double | yaw rate standard deviation [rad/s]   |
 
-## Assumptions / Known limits
+## gyro_bias_estimator
+
+`gyro_bias_validator` is a node that validates the bias of the gyroscope. It subscribes to the `sensor_msgs::msg::Imu` topic and validate if the bias of the gyroscope is within the specified range.
 
-## (Optional) Error detection and handling
+Note that the node calculates bias from the gyroscope data by averaging the data only when the vehicle is stopped.
 
-## (Optional) Performance characterization
+### Input
+
+| Name              | Type                                             | Description      |
+| ----------------- | ------------------------------------------------ | ---------------- |
+| `~/input/imu_raw` | `sensor_msgs::msg::Imu`                          | **raw** imu data |
+| `~/input/twist`   | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity |
 
-## (Optional) References/External links
+### Output
 
-## (Optional) Future extensions / Unimplemented parts
+| Name                 | Type                                 | Description                   |
+| -------------------- | ------------------------------------ | ----------------------------- |
+| `~/output/gyro_bias` | `geometry_msgs::msg::Vector3Stamped` | bias of the gyroscope [rad/s] |
+
+### Parameters
+
+| Name                        | Type   | Description                                                                   |
+| --------------------------- | ------ | ----------------------------------------------------------------------------- |
+| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s]                                          |
+| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s]                                            |
+| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s]                                              |
+| `gyro_bias_threshold`       | double | threshold of the bias of the gyroscope [rad/s]                                |
+| `velocity_threshold`        | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] |
+| `timestamp_threshold`       | double | threshold of the timestamp diff between IMU and twist [s]                     |
+| `data_num_threshold`        | int    | number of data used to calculate bias                                         |

sensing/imu_corrector/config/gyro_bias_estimator.param.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    gyro_bias_threshold: 0.0015 # [rad/s]
+    velocity_threshold: 0.0 # [m/s]
+    timestamp_threshold: 0.1 # [s]
+    data_num_threshold: 200 # [num]

sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input_imu_raw" default="imu_raw"/>
+  <arg name="input_twist" default="twist"/>
+  <arg name="output_gyro_bias" default="gyro_bias"/>
+  <arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share imu_corrector)/config/gyro_bias_estimator.param.yaml"/>
+  <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>
+
+  <node pkg="imu_corrector" exec="gyro_bias_estimator" name="gyro_bias_estimator" output="screen">
+    <remap from="~/input/imu_raw" to="$(var input_imu_raw)"/>
+    <remap from="~/input/twist" to="$(var input_twist)"/>
+    <remap from="~/output/gyro_bias" to="$(var output_gyro_bias)"/>
+    <param from="$(var gyro_bias_estimator_param_file)"/>
+    <param from="$(var imu_corrector_param_file)"/>
+  </node>
+</launch>