Merge branch 'main' into feature/change_in_use_of_rectified_pointcloud

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/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/scene_intersection.cpp

@@ -272,7 +272,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;
     }
   }
 

planning/motion_velocity_smoother/README.md

@@ -30,9 +30,11 @@ This function is used to approach near the obstacle or improve the accuracy of s
 #### Apply lateral acceleration limit
 
 It applies the velocity limit to decelerate at the curve.
-It calculate the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`.
+It calculates the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`.
 The velocity limit is set as not to fall under `min_curve_velocity`.
 
+Note: velocity limit that requests larger than `nominal.jerk` is not applied. In other words, even if a sharp curve is planned just in front of the ego, no deceleration is performed.
+
 #### Resample trajectory
 
 It resamples the points on the reference trajectory with designated time interval.

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -157,7 +157,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & input) const;
 
-  bool smoothVelocity(const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const;
+  bool smoothVelocity(
+    const TrajectoryPoints & input, const size_t input_closest,
+    TrajectoryPoints & traj_smoothed) const;
 
   std::tuple<double, double, InitializeType> calcInitialMotion(
     const TrajectoryPoints & input_traj, const size_t input_closest,

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp

@@ -75,7 +75,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
 
   boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
-    const TrajectoryPoints & input) const override;
+    const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+    [[maybe_unused]] const double a0,
+    [[maybe_unused]] const bool enable_smooth_limit) const override;
 
   void setParam(const Param & param);
   Param getParam() const;

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp

@@ -60,7 +60,9 @@ class SmootherBase
     const TrajectoryPoints & input, const double v_current, const int closest_id) const = 0;
 
   virtual boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
-    const TrajectoryPoints & input) const;
+    const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0,
+    [[maybe_unused]] const double a0 = 0.0,
+    [[maybe_unused]] const bool enable_smooth_limit = false) const;
 
   double getMaxAccel() const;
   double getMinDecel() const;

planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp

@@ -85,6 +85,10 @@ boost::optional<TrajectoryPoints> applyDecelFilterWithJerkConstraint(
 boost::optional<std::tuple<double, double, double, double>> updateStateWithJerkConstraint(
   const double v0, const double a0, const std::map<double, double> & jerk_profile, const double t);
 
+std::vector<double> calcVelocityProfileWithConstantJerkAndAccelerationLimit(
+  const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk,
+  const double acc_max, const double acc_min);
+
 }  // namespace trajectory_utils
 }  // namespace motion_velocity_smoother
 

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -478,7 +478,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
   }
 
   // Smoothing velocity
-  if (!smoothVelocity(*traj_extracted, output)) {
+  if (!smoothVelocity(*traj_extracted, *traj_extracted_closest, output)) {
     return prev_output_;
   }
 
@@ -493,10 +493,18 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
 }
 
 bool MotionVelocitySmootherNode::smoothVelocity(
-  const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const
+  const TrajectoryPoints & input, const size_t input_closest,
+  TrajectoryPoints & traj_smoothed) const
 {
+  // Calculate initial motion for smoothing
+  double initial_vel{};
+  double initial_acc{};
+  InitializeType type{};
+  std::tie(initial_vel, initial_acc, type) = calcInitialMotion(input, input_closest, prev_output_);
+
   // Lateral acceleration limit
-  const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input);
+  const auto traj_lateral_acc_filtered =
+    smoother_->applyLateralAccelerationFilter(input, initial_vel, initial_acc, true);
   if (!traj_lateral_acc_filtered) {
     return false;
   }
@@ -505,13 +513,25 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   const auto traj_pre_resampled_closest = motion_utils::findNearestIndex(
     *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
     node_param_.delta_yaw_threshold);
+  if (!traj_pre_resampled_closest) {
+    RCLCPP_WARN(
+      get_logger(), "Cannot find closest waypoint for traj_lateral_acc_filtered trajectory");
+    return false;
+  }
   auto traj_resampled = smoother_->resampleTrajectory(
     *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x,
     *traj_pre_resampled_closest);
   if (!traj_resampled) {
     RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization");
     return false;
   }
+  const auto traj_resampled_closest = motion_utils::findNearestIndex(
+    *traj_resampled, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+  if (!traj_resampled_closest) {
+    RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory");
+    return false;
+  }
 
   // Set 0[m/s] in the terminal point
   if (!traj_resampled->empty()) {
@@ -521,20 +541,6 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   // Publish Closest Resample Trajectory Velocity
   publishClosestVelocity(*traj_resampled, current_pose_ptr_->pose, debug_closest_max_velocity_);
 
-  // Calculate initial motion for smoothing
-  double initial_vel{};
-  double initial_acc{};
-  InitializeType type{};
-  const auto traj_resampled_closest = motion_utils::findNearestIndex(
-    *traj_resampled, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
-    node_param_.delta_yaw_threshold);
-  if (!traj_resampled_closest) {
-    RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory");
-    return false;
-  }
-  std::tie(initial_vel, initial_acc, type) =
-    calcInitialMotion(*traj_resampled, *traj_resampled_closest, prev_output_);
-
   // Clip trajectory from closest point
   TrajectoryPoints clipped;
   clipped.insert(

planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp

@@ -273,7 +273,8 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTra
 }
 
 boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter(
-  const TrajectoryPoints & input) const
+  const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+  [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const
 {
   if (input.empty()) {
     return boost::none;