chore: sync upstream

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp

@@ -34,6 +34,7 @@ using Box2d = boost::geometry::model::box<Point2d>;
 using LineString2d = boost::geometry::model::linestring<Point2d>;
 using LinearRing2d = boost::geometry::model::ring<Point2d>;
 using Polygon2d = boost::geometry::model::polygon<Point2d>;
+using Line2d = boost::geometry::model::linestring<Point2d>;
 using MultiPoint2d = boost::geometry::model::multi_point<Point2d>;
 using MultiLineString2d = boost::geometry::model::multi_linestring<LineString2d>;
 using MultiPolygon2d = boost::geometry::model::multi_polygon<Polygon2d>;

localization/pose_initializer/src/pose_initializer/gnss_module.cpp

@@ -43,7 +43,10 @@ geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose()
       Initialize::Service::Response::ERROR_GNSS, "The GNSS pose is out of date.");
   }
 
-  auto result = *pose_;
-  result.pose.pose.position = fitter_.fit(result.pose.pose.position, result.header.frame_id);
-  return result;
+  PoseWithCovarianceStamped pose = *pose_;
+  const auto fitted = fitter_.fit(pose.pose.pose.position, pose.header.frame_id);
+  if (fitted) {
+    pose.pose.pose.position = fitted.value();
+  }
+  return pose;
 }

map/map_height_fitter/CMakeLists.txt

@@ -10,4 +10,8 @@ ament_auto_add_library(map_height_fitter SHARED
 )
 target_link_libraries(map_height_fitter ${PCL_LIBRARIES})
 
-ament_auto_package()
+ament_auto_add_executable(node
+  src/node.cpp
+)
+
+ament_auto_package(INSTALL_TO_SHARE launch)

map/map_height_fitter/include/map_height_fitter/map_height_fitter.hpp

@@ -20,6 +20,7 @@
 #include <geometry_msgs/msg/point.hpp>
 
 #include <memory>
+#include <optional>
 #include <string>
 
 namespace map_height_fitter
@@ -32,7 +33,7 @@ class MapHeightFitter final
 public:
   MapHeightFitter(rclcpp::Node * node);
   ~MapHeightFitter();
-  Point fit(const Point & position, const std::string & frame);
+  std::optional<Point> fit(const Point & position, const std::string & frame);
 
 private:
   struct Impl;

map/map_height_fitter/launch/map_height_fitter.launch.xml

@@ -0,0 +1,10 @@
+<launch>
+  <group>
+    <push-ros-namespace namespace="map"/>
+    <node pkg="map_height_fitter" exec="node" name="map_height_fitter">
+      <param name="map_loader_name" value="/map/pointcloud_map_loader"/>
+      <remap from="~/pointcloud_map" to="/map/pointcloud_map"/>
+      <remap from="~/partial_map_load" to="/map/get_partial_pointcloud_map"/>
+    </node>
+  </group>
+</launch>

map/map_height_fitter/package.xml

@@ -21,6 +21,7 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_localization_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

map/map_height_fitter/src/map_height_fitter.cpp

@@ -32,9 +32,9 @@ struct MapHeightFitter::Impl
 
   explicit Impl(rclcpp::Node * node);
   void on_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
-  void get_partial_point_cloud_map(const Point & point);
+  bool get_partial_point_cloud_map(const Point & point);
   double get_ground_height(const tf2::Vector3 & point) const;
-  Point fit(const Point & position, const std::string & frame);
+  std::optional<Point> fit(const Point & position, const std::string & frame);
 
   tf2::BufferCore tf2_buffer_;
   tf2_ros::TransformListener tf2_listener_;
@@ -83,17 +83,17 @@ void MapHeightFitter::Impl::on_map(const sensor_msgs::msg::PointCloud2::ConstSha
   pcl::fromROSMsg(*msg, *map_cloud_);
 }
 
-void MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
+bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
 {
   const auto logger = node_->get_logger();
 
   if (!cli_map_) {
     RCLCPP_WARN_STREAM(logger, "Partial map loading in pointcloud_map_loader is not enabled");
-    return;
+    return false;
   }
   if (!cli_map_->service_is_ready()) {
     RCLCPP_WARN_STREAM(logger, "Partial map loading in pointcloud_map_loader is not ready");
-    return;
+    return false;
   }
 
   const auto req = std::make_shared<autoware_map_msgs::srv::GetPartialPointCloudMap::Request>();
@@ -107,7 +107,7 @@ void MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
   while (status != std::future_status::ready) {
     RCLCPP_INFO(logger, "waiting response");
     if (!rclcpp::ok()) {
-      return;
+      return false;
     }
     status = future.wait_for(std::chrono::seconds(1));
   }
@@ -131,6 +131,7 @@ void MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
   map_frame_ = res->header.frame_id;
   map_cloud_ = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::fromROSMsg(pcd_msg, *map_cloud_);
+  return true;
 }
 
 double MapHeightFitter::Impl::get_ground_height(const tf2::Vector3 & point) const
@@ -162,32 +163,37 @@ double MapHeightFitter::Impl::get_ground_height(const tf2::Vector3 & point) cons
   return std::isfinite(height) ? height : point.getZ();
 }
 
-Point MapHeightFitter::Impl::fit(const Point & position, const std::string & frame)
+std::optional<Point> MapHeightFitter::Impl::fit(const Point & position, const std::string & frame)
 {
   const auto logger = node_->get_logger();
   tf2::Vector3 point(position.x, position.y, position.z);
 
-  RCLCPP_INFO(logger, "map fit1: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
+  RCLCPP_INFO(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
 
   if (cli_map_) {
-    map_cloud_.reset();
-    get_partial_point_cloud_map(position);
+    if (!get_partial_point_cloud_map(position)) {
+      return std::nullopt;
+    }
   }
 
-  if (map_cloud_) {
-    try {
-      const auto stamped = tf2_buffer_.lookupTransform(map_frame_, frame, tf2::TimePointZero);
-      tf2::Transform transform{tf2::Quaternion{}, tf2::Vector3{}};
-      tf2::fromMsg(stamped.transform, transform);
-      point = transform * point;
-      point.setZ(get_ground_height(point));
-      point = transform.inverse() * point;
-    } catch (tf2::TransformException & exception) {
-      RCLCPP_WARN_STREAM(logger, "failed to lookup transform: " << exception.what());
-    }
+  if (!map_cloud_) {
+    RCLCPP_WARN_STREAM(logger, "point cloud map is not ready");
+    return std::nullopt;
+  }
+
+  try {
+    const auto stamped = tf2_buffer_.lookupTransform(map_frame_, frame, tf2::TimePointZero);
+    tf2::Transform transform{tf2::Quaternion{}, tf2::Vector3{}};
+    tf2::fromMsg(stamped.transform, transform);
+    point = transform * point;
+    point.setZ(get_ground_height(point));
+    point = transform.inverse() * point;
+  } catch (tf2::TransformException & exception) {
+    RCLCPP_WARN_STREAM(logger, "failed to lookup transform: " << exception.what());
+    return std::nullopt;
   }
 
-  RCLCPP_INFO(logger, "map fit2: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
+  RCLCPP_INFO(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
 
   Point result;
   result.x = point.getX();
@@ -205,7 +211,7 @@ MapHeightFitter::~MapHeightFitter()
 {
 }
 
-Point MapHeightFitter::fit(const Point & position, const std::string & frame)
+std::optional<Point> MapHeightFitter::fit(const Point & position, const std::string & frame)
 {
   return impl_->fit(position, frame);
 }

map/map_height_fitter/src/node.cpp

@@ -0,0 +1,58 @@
+// Copyright 2023 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "map_height_fitter/map_height_fitter.hpp"
+
+#include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>
+
+#include <memory>
+
+using tier4_localization_msgs::srv::PoseWithCovarianceStamped;
+
+class MapHeightFitterNode : public rclcpp::Node
+{
+public:
+  MapHeightFitterNode() : Node("map_height_fitter"), fitter_(this)
+  {
+    const auto on_service = [this](
+                              const PoseWithCovarianceStamped::Request::SharedPtr req,
+                              const PoseWithCovarianceStamped::Response::SharedPtr res) {
+      const auto & pose = req->pose_with_covariance;
+      const auto fitted = fitter_.fit(pose.pose.pose.position, pose.header.frame_id);
+      if (fitted) {
+        res->pose_with_covariance = req->pose_with_covariance;
+        res->pose_with_covariance.pose.pose.position = fitted.value();
+        res->success = true;
+      } else {
+        res->success = false;
+      }
+    };
+    srv_ = create_service<PoseWithCovarianceStamped>("~/service", on_service);
+  }
+
+private:
+  map_height_fitter::MapHeightFitter fitter_;
+  rclcpp::Service<PoseWithCovarianceStamped>::SharedPtr srv_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::executors::MultiThreadedExecutor executor;
+  auto node = std::make_shared<MapHeightFitterNode>();
+  executor.add_node(node);
+  executor.spin();
+  executor.remove_node(node);
+  rclcpp::shutdown();
+}

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -127,6 +127,7 @@
         safety_check_idling_time: 1.5                    # [s]
         safety_check_accel_for_rss: 2.5                  # [m/ss]
         safety_check_hysteresis_factor: 2.0              # [-]
+        safety_check_ego_offset: 1.0                     # [m]
 
       # For avoidance maneuver
       avoidance:
@@ -144,6 +145,8 @@
           min_avoidance_distance: 10.0                  # [m]
           min_nominal_avoidance_speed: 7.0              # [m/s]
           min_sharp_avoidance_speed: 1.0                # [m/s]
+          min_slow_down_speed: 1.38                     # [m/s]
+          buf_slow_down_speed: 0.56                     # [m/s]
 
       # For yield maneuver
       yield:
@@ -153,6 +156,7 @@
       stop:
         min_distance: 10.0                              # [m]
         max_distance: 20.0                              # [m]
+        stop_buffer: 1.0                                # [m]
 
       constraints:
         # vehicle slows down under longitudinal constraints

planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md

@@ -655,13 +655,14 @@ namespace: `avoidance.target_filtering.`
 
 namespace: `avoidance.safety_check.`
 
-| Name                           | Unit   | Type   | Description                                                                       | Default value |
-| :----------------------------- | ------ | ------ | --------------------------------------------------------------------------------- | ------------- |
-| safety_check_backward_distance | [m]    | double | Backward distance to search the dynamic objects.                                  | 50.0          |
-| safety_check_time_horizon      | [s]    | double | Time horizon to check lateral/longitudinal margin is enough or not.               | 10.0          |
-| safety_check_idling_time       | [t]    | double | Time delay constant that be use for longitudinal margin calculation based on RSS. | 1.5           |
-| safety_check_accel_for_rss     | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS.     | 2.5           |
-| safety_check_hysteresis_factor | [-]    | double | Hysteresis factor that be used for chattering prevention.                         | 2.0           |
+| Name                           | Unit   | Type   | Description                                                                                                | Default value |
+| :----------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- |
+| safety_check_backward_distance | [m]    | double | Backward distance to search the dynamic objects.                                                           | 50.0          |
+| safety_check_time_horizon      | [s]    | double | Time horizon to check lateral/longitudinal margin is enough or not.                                        | 10.0          |
+| safety_check_idling_time       | [t]    | double | Time delay constant that be use for longitudinal margin calculation based on RSS.                          | 1.5           |
+| safety_check_accel_for_rss     | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS.                              | 2.5           |
+| safety_check_hysteresis_factor | [-]    | double | Hysteresis factor that be used for chattering prevention.                                                  | 2.0           |
+| safety_check_ego_offset        | [m]    | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0           |
 
 ### Avoidance maneuver parameters
 
@@ -677,14 +678,16 @@ namespace: `avoidance.avoidance.lateral.`
 
 namespace: `avoidance.avoidance.longitudinal.`
 
-| Name                                 | Unit  | Type   | Description                                                                                            | Default value |
-| :----------------------------------- | :---- | :----- | :----------------------------------------------------------------------------------------------------- | :------------ |
-| prepare_time                         | [s]   | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change.          | 2.0           |
-| longitudinal_collision_safety_buffer | [s]   | double | Longitudinal collision buffer between target object and shift line.                                    | 0.0           |
-| min_prepare_distance                 | [m]   | double | Minimum distance for "prepare_time" x "ego_speed".                                                     | 1.0           |
-| min_avoidance_distance               | [m]   | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low) | 10.0          |
-| min_nominal_avoidance_speed          | [m/s] | double | Minimum speed for jerk calculation in a nominal situation (\*1).                                       | 7.0           |
-| min_sharp_avoidance_speed            | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1).                                         | 1.0           |
+| Name                                 | Unit  | Type   | Description                                                                                                                                                                                                                                                                                                                                                       | Default value  |
+| :----------------------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- |
+| prepare_time                         | [s]   | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change.                                                                                                                                                                                                                                                                     | 2.0            |
+| longitudinal_collision_safety_buffer | [s]   | double | Longitudinal collision buffer between target object and shift line.                                                                                                                                                                                                                                                                                               | 0.0            |
+| min_prepare_distance                 | [m]   | double | Minimum distance for "prepare_time" x "ego_speed".                                                                                                                                                                                                                                                                                                                | 1.0            |
+| min_avoidance_distance               | [m]   | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low)                                                                                                                                                                                                                                                            | 10.0           |
+| min_nominal_avoidance_speed          | [m/s] | double | Minimum speed for jerk calculation in a nominal situation (\*1).                                                                                                                                                                                                                                                                                                  | 7.0            |
+| min_sharp_avoidance_speed            | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1).                                                                                                                                                                                                                                                                                                    | 1.0            |
+| min_slow_down_speed                  | [m/s] | double | Minimum slow speed for avoidance prepare section.                                                                                                                                                                                                                                                                                                                 | 1.38 (5km/h)   |
+| buf_slow_down_speed                  | [m/s] | double | Buffer for controller tracking error. Basically, vehicle always cannot follow velocity profile precisely. Therefore, the module inserts lower speed than target speed that satisfies conditions to avoid object within accel/jerk constraints so that the avoidance path always can be output even if the current speed is a little bit higher than target speed. | 0.57 (2.0km/h) |
 
 ### Yield maneuver parameters
 
@@ -702,6 +705,7 @@ namespace: `avoidance.stop.`
 | :----------- | :--- | :----- | :---------------------------------------------------------------------------------------------------- | :------------ |
 | min_distance | [m]  | double | Minimum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 10.0          |
 | max_distance | [m]  | double | Maximum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 20.0          |
+| stop_buffer  | [m]  | double | Buffer distance in the situation where avoidance maneuver is not approved or in yield maneuver.       | 1.0           |
 
 ### Constraints parameters
 

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -187,6 +187,9 @@ struct AvoidanceParameters
   // transit hysteresis (unsafe to safe)
   double safety_check_hysteresis_factor;
 
+  // don't output new candidate path if the offset between ego and path is larger than this.
+  double safety_check_ego_offset;
+
   // keep target velocity in yield maneuver
   double yield_velocity;
 
@@ -196,6 +199,9 @@ struct AvoidanceParameters
   // maximum stop distance
   double stop_max_distance;
 
+  // stop buffer
+  double stop_buffer;
+
   // start avoidance after this time to avoid sudden path change
   double prepare_time;
 
@@ -216,6 +222,12 @@ struct AvoidanceParameters
   // distance for avoidance. Need a sharp avoidance path to avoid the object.
   double min_sharp_avoidance_speed;
 
+  // minimum slow down speed
+  double min_slow_down_speed;
+
+  // slow down speed buffer
+  double buf_slow_down_speed;
+
   // The margin is configured so that the generated avoidance trajectory does not come near to the
   // road shoulder.
   double road_shoulder_safety_margin{1.0};