Merge pull request #758 from tier4/sync-upstream

chore: sync upstream

control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml

@@ -5,6 +5,7 @@
     use_emergency_handling: false
     check_external_emergency_heartbeat: false
     use_start_request: false
+    enable_cmd_limit_filter: true
     external_emergency_stop_heartbeat_timeout: 0.0
     stop_hold_acceleration: -1.5
     emergency_acceleration: -2.4

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -150,6 +150,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   moderate_stop_service_acceleration_ =
     declare_parameter<double>("moderate_stop_service_acceleration");
   stop_check_duration_ = declare_parameter<double>("stop_check_duration");
+  enable_cmd_limit_filter_ = declare_parameter<bool>("enable_cmd_limit_filter");
 
   // Vehicle Parameter
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -411,9 +412,11 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
     filtered_commands.control.longitudinal.acceleration = stop_hold_acceleration_;
   }
 
-  // Apply limit filtering
-  filtered_commands.control = filterControlCommand(filtered_commands.control);
-
+  // Check if command filtering option is enable
+  if (enable_cmd_limit_filter_) {
+    // Apply limit filtering
+    filtered_commands.control = filterControlCommand(filtered_commands.control);
+  }
   // tmp: Publish vehicle emergency status
   VehicleEmergencyStamped vehicle_cmd_emergency;
   vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_);

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -164,6 +164,7 @@ class VehicleCmdGate : public rclcpp::Node
   double stop_hold_acceleration_;
   double emergency_acceleration_;
   double moderate_stop_service_acceleration_;
+  bool enable_cmd_limit_filter_;
 
   // Service
   rclcpp::Service<EngageSrv>::SharedPtr srv_engage_;

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -294,6 +294,7 @@
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
+      <arg name="priority_mode" value="0"/>
       <arg name="output/object" value="$(var merger/output/objects)"/>
       <arg name="data_association_matrix_path" value="$(var object_recognition_detection_object_merger_data_association_matrix_param_path)"/>
       <arg name="distance_threshold_list_path" value="$(var object_recognition_detection_object_merger_distance_threshold_list_path)"/>

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -141,6 +141,7 @@
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="temporary_merged_objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
+      <arg name="priority_mode" value="0"/>
       <arg name="output/object" value="$(var merger/output/objects)"/>
       <arg name="data_association_matrix_path" value="$(var object_recognition_detection_object_merger_data_association_matrix_param_path)"/>
       <arg name="distance_threshold_list_path" value="$(var object_recognition_detection_object_merger_distance_threshold_list_path)"/>

localization/ekf_localizer/README.md

@@ -18,6 +18,7 @@ This package includes the following features:
 - **Automatic estimation of yaw bias** prevents modeling errors caused by sensor mounting angle errors, which can improve estimation accuracy.
 - **Mahalanobis distance gate** enables probabilistic outlier detection to determine which inputs should be used or ignored.
 - **Smooth update**, the Kalman Filter measurement update is typically performed when a measurement is obtained, but it can cause large changes in the estimated value, especially for low-frequency measurements. Since the algorithm can consider the measurement time, the measurement data can be divided into multiple pieces and integrated smoothly while maintaining consistency (see the following figure).
+- **Calculation of vertical correction amount from pitch** mitigates localization instability on slopes. For example, when going uphill, it behaves as if it is buried in the ground (see the left side of the "Calculate delta from pitch" figure) because EKF only considers 3DoF(x,y,yaw). Therefore, EKF corrects the z-coordinate according to the formula (see the right side of the "Calculate delta from pitch" figure).
 
 <p align="center">
 <img src="./media/ekf_delay_comp.png" width="800">
@@ -27,6 +28,10 @@ This package includes the following features:
   <img src="./media/ekf_smooth_update.png" width="800">
 </p>
 
+<p align="center">
+  <img src="./media/calculation_delta_from_pitch.png" width="800">
+</p>
+
 ## Launch
 
 The `ekf_localizer` starts with the default parameters with the following command.

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -446,7 +446,15 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
     poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps);
 
   ekf_.updateWithDelay(y, C, R, delay_step);
-  updateSimple1DFilters(pose, params_.pose_smoothing_steps);
+
+  // Considering change of z value due to measurement pose delay
+  const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);
+  const double dz_delay = current_ekf_twist_.twist.linear.x * delay_time * std::sin(-rpy.y);
+  geometry_msgs::msg::PoseWithCovarianceStamped pose_with_z_delay;
+  pose_with_z_delay = pose;
+  pose_with_z_delay.pose.pose.position.z += dz_delay;
+
+  updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps);
 
   // debug
   const Eigen::MatrixXd X_result = ekf_.getLatestX();

perception/ground_segmentation/CMakeLists.txt

@@ -75,7 +75,7 @@ find_library(YAML_CPP_LIBRARY
             PATHS ${YAML_CPP_LIBRARY_DIRS})
 
 link_directories(${YAML_CPP_LIBRARY_DIRS})
-
+# cspell:ignore DHAVE, YAMLCPP
 if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
   add_definitions(-DHAVE_NEW_YAMLCPP)
 endif()

planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml

@@ -31,7 +31,8 @@
       # object recognition
       object_recognition:
         use_object_recognition: true
-        object_recognition_collision_check_margin: 1.0
+        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
 
       # pull over
       pull_over:

planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md

@@ -150,10 +150,11 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio
 
 #### Parameters for object recognition based collision check
 
-| Name                                      | Unit | Type   | Description                                                | Default value |
-| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ |
-| 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.      | 1.0           |
+| Name                                                         | Unit | Type   | Description                                                | Default value |
+| :----------------------------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | ---------------------------------------------------------------------------------------------------------- |
+| 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 |                                                            | 1.0           | 　maximum value when adding longitudinal distance margin for collision check considering stopping distance |
 
 ## **Goal Search**
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -53,7 +53,8 @@ struct PullOutStatus
   PlannerType planner_type{PlannerType::NONE};
   PathWithLaneId backward_path{};
   lanelet::ConstLanelets pull_out_lanes{};
-  bool is_safe{false};
+  bool is_safe_static_objects{false};   // current path is safe against static objects
+  bool is_safe_dynamic_objects{false};  // current path is safe against dynamic objects
   bool back_finished{false};
   Pose pull_out_start_pose{};
 

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp

@@ -68,6 +68,7 @@ struct GoalPlannerParameters
   // object recognition
   bool use_object_recognition;
   double object_recognition_collision_check_margin;
+  double object_recognition_collision_check_max_extra_stopping_margin;
 
   // pull over general params
   double pull_over_velocity;

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -101,6 +101,15 @@ bool checkCollision(
   const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration,
   const double rear_object_deceleration, CollisionCheckDebug & debug);
 
+/**
+ * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
+ * objects.
+ * @return Has collision or not
+ */
+bool checkCollisionWithExtraStoppingMargin(
+  const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
+  const double base_to_front, const double base_to_rear, const double width,
+  const double maximum_deceleration, const double margin, const double max_stopping_margin);
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -164,6 +164,16 @@ double getDistanceBetweenPredictedPathAndObject(
   const PredictedObject & object, const PredictedPath & path, const double start_time,
   const double end_time, const double resolution);
 
+/**
+ * @brief Check collision between ego path footprints with extra longitudinal stopping margin and
+ * objects.
+ * @return Has collision or not
+ */
+bool checkCollisionWithExtraStoppingMargin(
+  const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects,
+  const double base_to_front, const double base_to_rear, const double width,
+  const double maximum_deceleration, const double margin, const double max_stopping_margin);
+
 /**
  * @brief Check collision between ego path footprints and objects.
  * @return Has collision or not

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

@@ -101,15 +101,20 @@ std::pair<double, double> projectObstacleVelocityToTrajectory(
   const std::vector<PathPointWithLaneId> & path_points, const PredictedObject & object)
 {
   const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose;
-  const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;
+  const double obj_vel_norm = std::hypot(
+    object.kinematics.initial_twist_with_covariance.twist.linear.x,
+    object.kinematics.initial_twist_with_covariance.twist.linear.y);
 
   const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position);
 
-  const double obj_yaw = tf2::getYaw(obj_pose.orientation);
+  const double obj_vel_yaw = std::atan2(
+    object.kinematics.initial_twist_with_covariance.twist.linear.y,
+    object.kinematics.initial_twist_with_covariance.twist.linear.x);
   const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation);
 
   return std::make_pair(
-    obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw));
+    obj_vel_norm * std::cos(obj_vel_yaw - path_yaw),
+    obj_vel_norm * std::sin(obj_vel_yaw - path_yaw));
 }
 
 double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape)
@@ -365,7 +370,9 @@ void DynamicAvoidanceModule::updateTargetObjects()
   for (const auto & predicted_object : predicted_objects) {
     const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id);
     const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
-    const double obj_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x;
+    const double obj_vel_norm = std::hypot(
+      predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
+      predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y);
     const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid);
 
     // 1.a. check label
@@ -393,7 +400,7 @@ void DynamicAvoidanceModule::updateTargetObjects()
                                              ? parameters_->min_overtaking_crossing_object_vel
                                              : parameters_->min_oncoming_crossing_object_vel;
     const bool is_crossing_object_to_ignore =
-      min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path;
+      min_crossing_object_vel < obj_vel_norm && is_obstacle_crossing_path;
     if (is_crossing_object_to_ignore) {
       RCLCPP_INFO_EXPRESSION(
         getLogger(), parameters_->enable_debug_info,

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -553,7 +553,9 @@ void GoalPlannerModule::selectSafePullOverPath()
     }
 
     // check if path is valid and safe
-    if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) {
+    if (
+      !hasEnoughDistance(pull_over_path) ||
+      checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) {
       continue;
     }
 
@@ -1178,9 +1180,14 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
   }
 
   if (parameters_->use_object_recognition) {
-    if (utils::checkCollisionBetweenPathFootprintsAndObjects(
-          vehicle_footprint_, path, *(planner_data_->dynamic_object),
-          parameters_->object_recognition_collision_check_margin)) {
+    const auto common_parameters = planner_data_->parameters;
+    const double base_link2front = common_parameters.base_link2front;
+    const double base_link2rear = common_parameters.base_link2rear;
+    const double vehicle_width = common_parameters.vehicle_width;
+    if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin(
+          path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width,
+          parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin,
+          parameters_->object_recognition_collision_check_max_extra_stopping_margin)) {
       return true;
     }
   }

planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp

@@ -88,6 +88,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
     p.use_object_recognition = node->declare_parameter<bool>(ns + "use_object_recognition");
     p.object_recognition_collision_check_margin =
       node->declare_parameter<double>(ns + "object_recognition_collision_check_margin");
+    p.object_recognition_collision_check_max_extra_stopping_margin =
+      node->declare_parameter<double>(
+        ns + "object_recognition_collision_check_max_extra_stopping_margin");
   }
 
   // pull over general params

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -336,7 +336,8 @@ std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_arra
     debug_msg.is_front = debug_data.is_front;
     debug_msg.relative_distance = debug_data.relative_to_ego;
     debug_msg.failed_reason = debug_data.failed_reason;
-    debug_msg.velocity = debug_data.object_twist.linear.x;
+    debug_msg.velocity =
+      std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y);
     debug_msg_array.lane_change_info.push_back(debug_msg);
   }
   lane_change_debug_msg_array_ = debug_msg_array;

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -677,7 +677,9 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   LaneChangeTargetObjectIndices filtered_obj_indices;
   for (size_t i = 0; i < objects.objects.size(); ++i) {
     const auto & object = objects.objects.at(i);
-    const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x;
+    const auto & obj_velocity_norm = std::hypot(
+      object.kinematics.initial_twist_with_covariance.twist.linear.x,
+      object.kinematics.initial_twist_with_covariance.twist.linear.y);
     const auto extended_object =
       utils::lane_change::transform(object, common_parameters, *lane_change_parameters_);
 
@@ -700,7 +702,7 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     }
 
     // ignore static object that are behind the ego vehicle
-    if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) {
+    if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) {
       continue;
     }
 
@@ -972,7 +974,16 @@ bool NormalLaneChange::getLaneChangePaths(
       const lanelet::BasicPoint2d lc_start_point(
         prepare_segment.points.back().point.pose.position.x,
         prepare_segment.points.back().point.pose.position.y);
-      if (!boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d)) {
+
+      const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength(
+        target_lanes, 0, std::numeric_limits<double>::max());
+      const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon();
+
+      const auto is_valid_start_point =
+        boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) ||
+        boost::geometry::covered_by(lc_start_point, target_lane_poly_2d);
+
+      if (!is_valid_start_point) {
         // lane changing points are not inside of the target preferred lanes or its neighbors
         continue;
       }