Merge branch 'main' into feature/change_in_use_of_rectified_pointcloud

build_depends.humble.repos

@@ -29,3 +29,7 @@ repositories:
     type: git
     url: https://github.com/tier4/ndt_omp.git
     version: tier4/main
+  universe/external/morai_msgs:
+    type: git
+    url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
+    version: main

build_depends.repos

@@ -29,3 +29,7 @@ repositories:
     type: git
     url: https://github.com/tier4/ndt_omp.git
     version: tier4/main
+  universe/external/morai_msgs:
+    type: git
+    url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
+    version: main

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml

@@ -15,6 +15,7 @@
       min_predicted_path_confidence: 0.05
       collision_start_margin_time: 5.0 # [s]
       collision_end_margin_time: 2.0 # [s]
+      use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
 
     merge_from_private_road:
       stop_duration_sec: 1.0

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -763,6 +763,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::alignUsingMonteCar
     [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; });
 
   geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg;
+  result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp;
   result_pose_with_cov_msg.header.frame_id = map_frame_;
   result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose;
   // ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg);

perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml

@@ -13,7 +13,7 @@
   <arg name="param_file" default="$(find-pkg-share lidar_apollo_instance_segmentation)/config/$(var base_name).param.yaml"/>
 
   <arg name="target_frame" default="base_link"/>
-  <arg name="z_offset" default="-2"/>
+  <arg name="z_offset" default="-2.0"/>
 
   <node pkg="lidar_apollo_instance_segmentation" exec="lidar_apollo_instance_segmentation_node" name="lidar_apollo_instance_segmentation" output="screen">
     <remap from="input/pointcloud" to="$(var input/pointcloud)"/>

perception/lidar_apollo_instance_segmentation/src/detector.cpp

@@ -38,7 +38,7 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node *
   use_intensity_feature = node_->declare_parameter("use_intensity_feature", true);
   use_constant_feature = node_->declare_parameter("use_constant_feature", true);
   target_frame_ = node_->declare_parameter("target_frame", "base_link");
-  z_offset_ = node_->declare_parameter("z_offset", 2);
+  z_offset_ = node_->declare_parameter<float>("z_offset", -2.0);
 
   // load weight file
   std::ifstream fs(engine_file);

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -160,7 +160,7 @@ class MapBasedPredictionNode : public rclcpp::Node
     const lanelet::BasicPoint2d & search_point);
   float calculateLocalLikelihood(
     const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const;
-  static double getObjectYaw(const TrackedObject & object);
+  void updateObjectData(TrackedObject & object);
 
   void updateObjectsHistory(
     const std_msgs::msg::Header & header, const TrackedObject & object,

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -17,6 +17,8 @@
 #include <interpolation/linear_interpolation.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+
 #include <boost/geometry.hpp>
 #include <boost/geometry/geometries/polygon.hpp>
 
@@ -385,6 +387,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
       label == ObjectClassification::CAR || label == ObjectClassification::BUS ||
       label == ObjectClassification::TRAILER || label == ObjectClassification::MOTORCYCLE ||
       label == ObjectClassification::TRUCK) {
+      // Update object yaw and velocity
+      updateObjectData(transformed_object);
+
       // Get Closest Lanelet
       const auto current_lanelets = getCurrentLanelets(transformed_object);
 
@@ -522,7 +527,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     const auto entry_point = getCrosswalkEntryPoint(crossing_crosswalk.get());
 
     if (hasPotentialToReach(
-          object, entry_point.first, prediction_time_horizon_,
+          object, entry_point.first, std::numeric_limits<double>::max(),
           min_velocity_for_map_based_prediction_)) {
       PredictedPath predicted_path =
         path_generator_->generatePathToTargetPoint(object, entry_point.first);
@@ -531,7 +536,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     }
 
     if (hasPotentialToReach(
-          object, entry_point.second, prediction_time_horizon_,
+          object, entry_point.second, std::numeric_limits<double>::max(),
           min_velocity_for_map_based_prediction_)) {
       PredictedPath predicted_path =
         path_generator_->generatePathToTargetPoint(object, entry_point.second);
@@ -540,11 +545,16 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     }
 
   } else if (withinRoadLanelet(object, lanelet_map_ptr_)) {
-    for (const auto & crosswalk : crosswalks_) {
-      const auto entry_point = getCrosswalkEntryPoint(crosswalk);
+    lanelet::ConstLanelet closest_crosswalk{};
+    const auto & obj_pose = object.kinematics.pose_with_covariance.pose;
+    const auto found_closest_crosswalk =
+      lanelet::utils::query::getClosestLanelet(crosswalks_, obj_pose, &closest_crosswalk);
+
+    if (found_closest_crosswalk) {
+      const auto entry_point = getCrosswalkEntryPoint(closest_crosswalk);
 
       if (hasPotentialToReach(
-            object, entry_point.first, prediction_time_horizon_,
+            object, entry_point.first, prediction_time_horizon_ * 2.0,
             min_velocity_for_map_based_prediction_)) {
         PredictedPath predicted_path =
           path_generator_->generatePathToTargetPoint(object, entry_point.first);
@@ -553,7 +563,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
       }
 
       if (hasPotentialToReach(
-            object, entry_point.second, prediction_time_horizon_,
+            object, entry_point.second, prediction_time_horizon_ * 2.0,
             min_velocity_for_map_based_prediction_)) {
         PredictedPath predicted_path =
           path_generator_->generatePathToTargetPoint(object, entry_point.second);
@@ -605,17 +615,41 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   return predicted_object;
 }
 
-double MapBasedPredictionNode::getObjectYaw(const TrackedObject & object)
+void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
 {
-  if (object.kinematics.orientation_availability) {
-    return tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
+  if (
+    object.kinematics.orientation_availability ==
+    autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) {
+    return;
   }
 
+  // Compute yaw angle from the velocity and position of the object
   const auto & object_pose = object.kinematics.pose_with_covariance.pose;
   const auto & object_twist = object.kinematics.twist_with_covariance.twist;
   const auto future_object_pose = tier4_autoware_utils::calcOffsetPose(
     object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0);
-  return tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position);
+
+  if (object.kinematics.twist_with_covariance.twist.linear.x < 0.0) {
+    if (
+      object.kinematics.orientation_availability ==
+      autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) {
+      const auto original_yaw =
+        tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
+      // flip the angle
+      object.kinematics.pose_with_covariance.pose.orientation =
+        tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::pi + original_yaw);
+    } else {
+      const auto updated_object_yaw =
+        tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position);
+
+      object.kinematics.pose_with_covariance.pose.orientation =
+        tier4_autoware_utils::createQuaternionFromYaw(updated_object_yaw);
+    }
+
+    object.kinematics.twist_with_covariance.twist.linear.x *= -1.0;
+  }
+
+  return;
 }
 
 void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time)
@@ -726,7 +760,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition(
   }
 
   // Step3. Calculate the angle difference between the lane angle and obstacle angle
-  const double object_yaw = getObjectYaw(object);
+  const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
   const double lane_yaw = lanelet::utils::getLaneletAngle(
     lanelet.second, object.kinematics.pose_with_covariance.pose.position);
   const double delta_yaw = object_yaw - lane_yaw;
@@ -753,7 +787,7 @@ float MapBasedPredictionNode::calculateLocalLikelihood(
   const auto & obj_point = object.kinematics.pose_with_covariance.pose.position;
 
   // compute yaw difference between the object and lane
-  const double obj_yaw = getObjectYaw(object);
+  const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
   const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point);
   const double delta_yaw = obj_yaw - lane_yaw;
   const double abs_norm_delta_yaw = std::fabs(tier4_autoware_utils::normalizeRadian(delta_yaw));
@@ -793,7 +827,7 @@ void MapBasedPredictionNode::updateObjectsHistory(
   single_object_data.current_lanelets = current_lanelets;
   single_object_data.future_possible_lanelets = current_lanelets;
   single_object_data.pose = object.kinematics.pose_with_covariance.pose;
-  const double object_yaw = getObjectYaw(object);
+  const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
   single_object_data.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(object_yaw);
   single_object_data.time_delay = std::fabs((this->get_clock()->now() - header.stamp).seconds());
   single_object_data.twist = object.kinematics.twist_with_covariance.twist;

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -475,8 +475,6 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
   const auto & lat_collision_margin = parameters_.lateral_collision_margin;
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
   const auto & road_shoulder_safety_margin = parameters_.road_shoulder_safety_margin;
-  const auto max_allowable_lateral_distance = lat_collision_safety_buffer + lat_collision_margin +
-                                              vehicle_width - road_shoulder_safety_margin;
 
   const auto avoid_margin =
     lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width;
@@ -493,13 +491,16 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
         avoidance_debug_msg_array.push_back(avoidance_debug_msg);
       };
 
+    const auto max_allowable_lateral_distance =
+      o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width;
+
     avoidance_debug_msg.object_id = getUuidStr(o);
     avoidance_debug_msg.longitudinal_distance = o.longitudinal;
     avoidance_debug_msg.lateral_distance_from_centerline = o.lateral;
     avoidance_debug_msg.to_furthest_linestring_distance = o.to_road_shoulder_distance;
     avoidance_debug_msg.max_shift_length = max_allowable_lateral_distance;
 
-    if (!(o.to_road_shoulder_distance > max_allowable_lateral_distance)) {
+    if (max_allowable_lateral_distance <= avoid_margin) {
       avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN);
       continue;
     }

planning/behavior_velocity_planner/config/intersection.param.yaml

@@ -15,6 +15,7 @@
       min_predicted_path_confidence: 0.05
       collision_start_margin_time: 5.0 # [s]
       collision_end_margin_time: 2.0 # [s]
+      use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck
 
     merge_from_private_road:
       stop_duration_sec: 1.0

planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp

@@ -122,6 +122,7 @@ class IntersectionModule : public SceneModuleInterface
     double external_input_timeout;       //! used to disable external input
     double collision_start_margin_time;  //! start margin time to check collision
     double collision_end_margin_time;    //! end margin time to check collision
+    bool use_stuck_stopline;  //! stopline generate before the intersection lanelet when is_stuck.
   };
 
   IntersectionModule(

planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp

@@ -80,6 +80,21 @@ bool generateStopLine(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx,
   int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger);
 
+/**
+ * @brief If use_stuck_stopline is true, a stop line is generated before the intersection.
+ * @param input_path      input path
+ * @param output_path     output path
+ * @param stuck_stop_line_idx   generated stuck stop line index
+ * @param pass_judge_line_idx  generated pass judge line index
+ * @return false when generation failed
+ */
+bool generateStopLineBeforeIntersection(
+  const int lane_id, lanelet::LaneletMapConstPtr lanelet_map_ptr,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  autoware_auto_planning_msgs::msg::PathWithLaneId * output_path, int * stuck_stop_line_idx,
+  int * pass_judge_line_idx, const rclcpp::Logger logger);
+
 /**
  * @brief Calculate first path index that is in the polygon.
  * @param path     target path

planning/behavior_velocity_planner/intersection-design.md

@@ -7,7 +7,7 @@ This module is designed for rule-based intersection velocity decision that is ea
 
 In addition, the external users / modules (e.g. remote operation) to can intervene the STOP/GO decision for the vehicle behavior. The override interface is expected to be used, for example, for remote intervention in emergency situations or gathering information on operator decisions during development.
 
-![brief](./docs/intersection/intersection.svg)
+![brief](./docs/intersection/intersection.drawio.svg)
 
 ### Activation Timing
 

planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp

@@ -229,11 +229,13 @@ PathPointWithLaneId getBackwardPointFromBasePoint(
   PathPointWithLaneId output;
   const double dx = p_to.point.pose.position.x - p_from.point.pose.position.x;
   const double dy = p_to.point.pose.position.y - p_from.point.pose.position.y;
-  const double norm = std::hypot(dx, dy);
+  const double dz = p_to.point.pose.position.z - p_from.point.pose.position.z;
+  const double norm = std::hypot(dx, dy, dz);
 
   output = p_base;
   output.point.pose.position.x += backward_length * dx / norm;
   output.point.pose.position.y += backward_length * dy / norm;
+  output.point.pose.position.z += backward_length * dz / norm;
 
   return output;
 }
@@ -638,7 +640,12 @@ void CrosswalkModule::insertDecelPoint(
   setDistance(stop_point_distance);
 
   debug_data_.first_stop_pose = stop_point.second.point.pose;
-  debug_data_.stop_poses.push_back(stop_point.second.point.pose);
+
+  if (std::abs(target_velocity) < 1e-3) {
+    debug_data_.stop_poses.push_back(stop_point.second.point.pose);
+  } else {
+    debug_data_.slow_poses.push_back(stop_point.second.point.pose);
+  }
 }
 
 float CrosswalkModule::calcTargetVelocity(

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

@@ -50,6 +50,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0);
   ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0);
   ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0);
+  ip.use_stuck_stopline = node.declare_parameter(ns + ".use_stuck_stopline", true);
 }
 
 MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node)

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -188,9 +188,18 @@ bool IntersectionModule::modifyPathVelocity(
     path->points.at(stop_line_idx).point.pose.position));
 
   if (!isActivated()) {
-    const double v = 0.0;
+    constexpr double v = 0.0;
+    if (planner_param_.use_stuck_stopline && is_stuck) {
+      int stuck_stop_line_idx = -1;
+      int stuck_pass_judge_line_idx = -1;
+      if (util::generateStopLineBeforeIntersection(
+            lane_id_, lanelet_map_ptr, planner_data_, *path, path, &stuck_stop_line_idx,
+            &stuck_pass_judge_line_idx, logger_.get_child("util"))) {
+        stop_line_idx = stuck_stop_line_idx;
+        pass_judge_line_idx = stuck_pass_judge_line_idx;
+      }
+    }
     util::setVelocityFrom(stop_line_idx, v, path);
-
     debug_data_.stop_required = true;
     debug_data_.stop_wall_pose = util::getAheadPose(stop_line_idx, base_link2front, *path);
     debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose;
@@ -272,7 +281,6 @@ bool IntersectionModule::checkCollision(
     const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics);
     if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) {
       target_objects.objects.push_back(object);
-      break;
     }
   }