Merge pull request autowarefoundation#593 from tier4/sync-upstream

chore: sync upstream

.github/CODEOWNERS

@@ -179,6 +179,7 @@ sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp @autowarefoundation/
 simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners
 simulator/fault_injection/** keisuke.shima@tier4.jp @autowarefoundation/autoware-global-codeowners
 simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners
+system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai @autowarefoundation/autoware-global-codeowners
 system/bluetooth_monitor/** fumihito.ito@tier4.jp @autowarefoundation/autoware-global-codeowners
 system/component_state_monitor/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners
 system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners

common/component_interface_specs/include/component_interface_specs/planning.hpp

@@ -75,6 +75,24 @@ struct Route
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
 };
 
+struct NormalRoute
+{
+  using Message = autoware_planning_msgs::msg::LaneletRoute;
+  static constexpr char name[] = "/planning/mission_planning/normal_route";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+struct MrmRoute
+{
+  using Message = autoware_planning_msgs::msg::LaneletRoute;
+  static constexpr char name[] = "/planning/mission_planning/mrm_route";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
 struct Trajectory
 {
   using Message = autoware_auto_planning_msgs::msg::Trajectory;

perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp

@@ -63,6 +63,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   lanelet::ConstLanelets getIntersectedLanelets(
     const LinearRing2d &, const lanelet::ConstLanelets &);
   bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
+  geometry_msgs::msg::Polygon setFootprint(
+    const autoware_auto_perception_msgs::msg::DetectedObject &);
 };
 
 }  // namespace object_lanelet_filter

perception/detected_object_validation/object-lanelet-filter.md

@@ -2,7 +2,7 @@
 
 ## Purpose
 
-The `object_lanelet_filter` is a node that filters detected object by using vector map.  
+The `object_lanelet_filter` is a node that filters detected object by using vector map.
 The objects only inside of the vector map will be published.
 
 ## Inner-workings / Algorithms
@@ -39,7 +39,7 @@ The objects only inside of the vector map will be published.
 
 ## Assumptions / Known limits
 
-Filtering is performed based on the shape polygon of the object.
+The lanelet filter is performed based on the shape polygon and bounding box of the objects.
 
 ## (Optional) Error detection and handling
 

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -89,7 +89,7 @@ void ObjectLaneletFilterNode::objectCallback(
 
   int index = 0;
   for (const auto & object : transformed_objects.objects) {
-    const auto & footprint = object.shape.footprint;
+    const auto footprint = setFootprint(object);
     const auto & label = object.classification.front().label;
     if (filter_target_.isTarget(label)) {
       Polygon2d polygon;
@@ -110,13 +110,39 @@ void ObjectLaneletFilterNode::objectCallback(
   object_pub_->publish(output_object_msg);
 }
 
+geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint(
+  const autoware_auto_perception_msgs::msg::DetectedObject & detected_object)
+{
+  geometry_msgs::msg::Polygon footprint;
+  if (detected_object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
+    const auto object_size = detected_object.shape.dimensions;
+    const double x_front = object_size.x / 2.0;
+    const double x_rear = -object_size.x / 2.0;
+    const double y_left = object_size.y / 2.0;
+    const double y_right = -object_size.y / 2.0;
+
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_front).y(y_left).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_front).y(y_right).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_rear).y(y_right).z(0.0));
+    footprint.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(x_rear).y(y_left).z(0.0));
+  } else {
+    footprint = detected_object.shape.footprint;
+  }
+  return footprint;
+}
+
 LinearRing2d ObjectLaneletFilterNode::getConvexHull(
   const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects)
 {
   MultiPoint2d candidate_points;
   for (const auto & object : detected_objects.objects) {
     const auto & pos = object.kinematics.pose_with_covariance.pose.position;
-    for (const auto & p : object.shape.footprint.points) {
+    const auto footprint = setFootprint(object);
+    for (const auto & p : footprint.points) {
       candidate_points.emplace_back(p.x + pos.x, p.y + pos.y);
     }
   }

planning/behavior_path_planner/config/start_planner/start_planner.param.yaml

@@ -12,7 +12,7 @@
       minimum_shift_pull_out_distance: 0.0
       deceleration_interval: 15.0
       lateral_jerk: 0.5
-      lateral_acceleration_sampling_num: 1
+      lateral_acceleration_sampling_num: 3
       minimum_lateral_acc: 0.15
       maximum_lateral_acc: 0.5
       # geometric pull out

planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md

@@ -2,7 +2,17 @@
 
 ## Purpose / Role
 
-Pull out from the shoulder lane without colliding with objects.
+The Start Planner module is designed to generate a path from the current ego position to the driving lane, avoiding static obstacles and implementing safety checks against dynamic obstacles. (Note: The feature of safety checks against dynamic obstacles is currently a work in progress.)
+This module is activated when a new route is received.
+
+Use cases are as follows
+
+- start smoothly from the current ego position to centerline.
+  - ![case1](../image/start_from_road_lane.drawio.svg)
+- pull out from the side of the road lane to centerline.
+  - ![case2](../image/start_from_road_side.drawio.svg)
+- pull out from the shoulder lane to the road lane centerline.
+  - ![case3](../image/start_from_road_shoulder.drawio.svg)
 
 ## Design
 
@@ -58,31 +68,37 @@ PullOutPath --o PullOutPlannerBase
 | collision_check_margin            | [m]   | double | Obstacle collision check margin                                      | 1.0           |
 | collision_check_distance_from_end | [m]   | double | collision check distance from end point. currently only for pull out | 15.0          |
 
-## **Safe check with obstacles in shoulder lane**
+## Safety check with static obstacles
 
 1. Calculate ego-vehicle's footprint on pull out path between from current position to pull out end point. (Illustrated by blue frame)
-2. Calculate object's polygon which is located in shoulder lane
+2. Calculate object's polygon
 3. If a distance between the footprint and the polygon is lower than the threshold (default: `1.0 m`), that is judged as a unsafe path
 
 ![pull_out_collision_check](../image/pull_out_collision_check.drawio.svg)
 
-### **Path Generation**
+## Safety check with dynamic obstacles
+
+WIP
+
+## **Path Generation**
 
 There are two path generation methods.
 
-#### **shift pull out**
+### **shift pull out**
+
+This is the most basic method of starting path planning and is used on road lanes and shoulder lanes when there is no particular obstruction.
 
 Pull out distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values, and the one that generates a safe path is selected.
 
-- Generate the shoulder lane centerline and shift it to the current position.
+- Generate the road lane centerline and shift it to the current position.
 - In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials)
 - Combine this path with center line of road lane
 
 ![shift_pull_out](../image/shift_pull_out.drawio.svg)
 
 [shift pull out video](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4)
 
-##### parameters for shift pull out
+#### parameters for shift pull out
 
 | Name                            | Unit   | Type   | Description                                                                                                          | Default value |
 | :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
@@ -93,7 +109,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral
 | minimum_lateral_jerk            | [m/s3] | double | minimum lateral jerk                                                                                                 | 0.1           |
 | minimum_shift_pull_out_distance | [m]    | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0           |
 
-#### **geometric pull out**
+### **geometric pull out**
 
 Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot.
 See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm.
@@ -102,7 +118,7 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474
 
 [geometric pull out video](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4)
 
-##### parameters for geometric pull out
+#### parameters for geometric pull out
 
 | Name                        | Unit  | Type   | Description                                                                                                                                                | Default value |
 | :-------------------------- | :---- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
@@ -113,15 +129,15 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474
 | lane_departure_margin       | [m]   | double | margin of deviation to lane right                                                                                                                          | 0.2           |
 | pull_out_max_steer_angle    | [rad] | double | maximum steer angle for path generation                                                                                                                    | 0.26          |
 
-### **backward pull out start point search**
+## **backward pull out start point search**
 
 If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`).
 
 ![pull_out_after_back](../image/pull_out_after_back.drawio.svg)
 
 [pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4)
 
-#### **parameters for backward pull out start point search**
+### **parameters for backward pull out start point search**
 
 | Name                          | Unit | Type   | Description                                                                                                                                                          | Default value  |
 | :---------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- |

planning/mission_planner/src/mission_planner/mission_planner.cpp

@@ -75,7 +75,6 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   plugin_loader_("mission_planner", "mission_planner::PlannerPlugin"),
   tf_buffer_(get_clock()),
   tf_listener_(tf_buffer_),
-  original_route_(nullptr),
   normal_route_(nullptr)
 {
   map_frame_ = declare_parameter<std::string>("map_frame");
@@ -101,6 +100,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   const auto adaptor = component_interface_utils::NodeAdaptor(this);
   adaptor.init_pub(pub_state_);
   adaptor.init_pub(pub_route_);
+  adaptor.init_pub(pub_normal_route_);
+  adaptor.init_pub(pub_mrm_route_);
   adaptor.init_srv(srv_clear_route_, this, &MissionPlanner::on_clear_route);
   adaptor.init_srv(srv_set_route_, this, &MissionPlanner::on_set_route);
   adaptor.init_srv(srv_set_route_points_, this, &MissionPlanner::on_set_route_points);
@@ -163,6 +164,7 @@ void MissionPlanner::change_route(const LaneletRoute & route)
 
   arrival_checker_.set_goal(goal);
   pub_route_->publish(route);
+  pub_normal_route_->publish(route);
   pub_marker_->publish(planner_->visualize(route));
   planner_->updateRoute(route);
 
@@ -289,7 +291,6 @@ void MissionPlanner::on_set_route(
   // Update route.
   change_route(route);
   change_state(RouteState::Message::SET);
-  original_route_ = std::make_shared<LaneletRoute>(route);
   res->status.success = true;
 }
 
@@ -325,7 +326,6 @@ void MissionPlanner::on_set_route_points(
   // Update route.
   change_route(route);
   change_state(RouteState::Message::SET);
-  original_route_ = std::make_shared<LaneletRoute>(route);
   res->status.success = true;
 }
 
@@ -431,7 +431,7 @@ void MissionPlanner::on_change_route(
 
   // check route safety
   if (checkRerouteSafety(*normal_route_, new_route)) {
-    // sucess to reroute
+    // success to reroute
     change_route(new_route);
     res->status.success = true;
     change_state(RouteState::Message::SET);
@@ -485,7 +485,7 @@ void MissionPlanner::on_change_route_points(
 
   // check route safety
   if (checkRerouteSafety(*normal_route_, new_route)) {
-    // sucess to reroute
+    // success to reroute
     change_route(new_route);
     res->status.success = true;
     change_state(RouteState::Message::SET);

planning/mission_planner/src/mission_planner/mission_planner.hpp

@@ -51,6 +51,8 @@ using SetRoute = planning_interface::SetRoute;
 using ChangeRoutePoints = planning_interface::ChangeRoutePoints;
 using ChangeRoute = planning_interface::ChangeRoute;
 using Route = planning_interface::Route;
+using NormalRoute = planning_interface::NormalRoute;
+using MrmRoute = planning_interface::MrmRoute;
 using RouteState = planning_interface::RouteState;
 using Odometry = nav_msgs::msg::Odometry;
 
@@ -89,6 +91,8 @@ class MissionPlanner : public rclcpp::Node
   RouteState::Message state_;
   component_interface_utils::Publisher<RouteState>::SharedPtr pub_state_;
   component_interface_utils::Publisher<Route>::SharedPtr pub_route_;
+  component_interface_utils::Publisher<NormalRoute>::SharedPtr pub_normal_route_;
+  component_interface_utils::Publisher<MrmRoute>::SharedPtr pub_mrm_route_;
   void change_state(RouteState::Message::_state_type state);
 
   component_interface_utils::Service<ClearRoute>::SharedPtr srv_clear_route_;
@@ -131,7 +135,6 @@ class MissionPlanner : public rclcpp::Node
   double minimum_reroute_length_{30.0};
   bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route);
 
-  std::shared_ptr<LaneletRoute> original_route_{nullptr};
   std::shared_ptr<LaneletRoute> normal_route_{nullptr};
 };
 

system/autoware_auto_msgs_adapter/package.xml

@@ -4,9 +4,14 @@
   <name>autoware_auto_msgs_adapter</name>
   <version>1.0.0</version>
   <description>Converts an autoware_msgs message to autoware_auto_msgs version and publishes it.</description>
+
   <maintainer email="mfc@leodrive.ai">M. Fatih Cırıt</maintainer>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
+
   <license>Apache License 2.0</license>
 
+  <author email="mfc@leodrive.ai">M. Fatih Cırıt</author>
+
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>