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

chore: sync upstream

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

@@ -230,7 +230,7 @@
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="$(var merger/input/objects)"/>
       <arg name="input/object1" value="clustering/camera_lidar_fusion/objects"/>
-      <arg name="output/object" value="camera_lidar_fusion/objects"/>
+      <arg name="output/object" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
       <arg name="priority_mode" value="0"/>
     </include>
   </group>
@@ -239,7 +239,7 @@
     <let name="merger/output/objects" value="objects_before_filter" if="$(var use_object_filter)"/>
     <let name="merger/output/objects" value="$(var output/objects)" unless="$(var use_object_filter)"/>
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
-      <arg name="input/object0" value="camera_lidar_fusion/objects"/>
+      <arg name="input/object0" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
       <arg name="output/object" value="$(var merger/output/objects)"/>
     </include>

perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp

@@ -44,7 +44,8 @@ BigVehicleTracker::BigVehicleTracker(
 : Tracker(time, object.classification),
   logger_(rclcpp::get_logger("BigVehicleTracker")),
   last_update_time_(time),
-  z_(object.kinematics.pose_with_covariance.pose.position.z)
+  z_(object.kinematics.pose_with_covariance.pose.position.z),
+  tracking_offset_(Eigen::Vector2d::Zero())
 {
   object_ = object;
 

perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

@@ -44,7 +44,8 @@ NormalVehicleTracker::NormalVehicleTracker(
 : Tracker(time, object.classification),
   logger_(rclcpp::get_logger("NormalVehicleTracker")),
   last_update_time_(time),
-  z_(object.kinematics.pose_with_covariance.pose.position.z)
+  z_(object.kinematics.pose_with_covariance.pose.position.z),
+  tracking_offset_(Eigen::Vector2d::Zero())
 {
   object_ = object;
 

perception/radar_fusion_to_detected_object/README.md

@@ -56,10 +56,10 @@ ros2 launch radar_fusion_to_detected_object radar_object_to_detected_object.laun
 
 ### Input
 
-| Name                    | Type                                                 | Description                                                            |
-| ----------------------- | ---------------------------------------------------- | ---------------------------------------------------------------------- |
-| `~/input/objects`       | autoware_auto_perception_msgs/msg/DetectedObject.msg | 3D detected objects.                                                   |
-| `~/input/radar_objects` | autoware_auto_perception_msgs/msg/TrackedObjects.msg | Radar objects. Note that frame_id need to be same as `~/input/objects` |
+| Name                    | Type                                                  | Description                                                            |
+| ----------------------- | ----------------------------------------------------- | ---------------------------------------------------------------------- |
+| `~/input/objects`       | autoware_auto_perception_msgs/msg/DetectedObject.msg  | 3D detected objects.                                                   |
+| `~/input/radar_objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. Note that frame_id need to be same as `~/input/objects` |
 
 ### Output
 

perception/radar_fusion_to_detected_object/include/radar_object_fusion_to_detected_object/radar_object_fusion_to_detected_object_node.hpp

@@ -23,7 +23,6 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
-#include "autoware_auto_perception_msgs/msg/tracked_objects.hpp"
 
 #include <chrono>
 #include <memory>
@@ -34,8 +33,6 @@ namespace radar_fusion_to_detected_object
 {
 using autoware_auto_perception_msgs::msg::DetectedObject;
 using autoware_auto_perception_msgs::msg::DetectedObjects;
-using autoware_auto_perception_msgs::msg::TrackedObject;
-using autoware_auto_perception_msgs::msg::TrackedObjects;
 
 class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node
 {
@@ -50,22 +47,22 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node
 private:
   // Subscriber
   message_filters::Subscriber<DetectedObjects> sub_object_{};
-  message_filters::Subscriber<TrackedObjects> sub_radar_{};
+  message_filters::Subscriber<DetectedObjects> sub_radar_{};
 
   using SyncPolicy =
-    message_filters::sync_policies::ApproximateTime<DetectedObjects, TrackedObjects>;
+    message_filters::sync_policies::ApproximateTime<DetectedObjects, DetectedObjects>;
   using Sync = message_filters::Synchronizer<SyncPolicy>;
   typename std::shared_ptr<Sync> sync_ptr_;
 
   // Callback
   void onData(
     const DetectedObjects::ConstSharedPtr object_msg,
-    const TrackedObjects::ConstSharedPtr radar_msg);
+    const DetectedObjects::ConstSharedPtr radar_msg);
   bool isDataReady();
 
   // Data Buffer
   DetectedObjects::ConstSharedPtr detected_objects_{};
-  TrackedObjects::ConstSharedPtr radar_objects_{};
+  DetectedObjects::ConstSharedPtr radar_objects_{};
 
   // Publisher
   rclcpp::Publisher<DetectedObjects>::SharedPtr pub_objects_{};
@@ -87,7 +84,7 @@ class RadarObjectFusionToDetectedObjectNode : public rclcpp::Node
 
   // Lapper
   RadarFusionToDetectedObject::RadarInput setRadarInput(
-    const TrackedObject & radar_object, const std_msgs::msg::Header & header_);
+    const DetectedObject & radar_object, const std_msgs::msg::Header & header_);
 };
 
 }  // namespace radar_fusion_to_detected_object

perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp

@@ -51,7 +51,6 @@ namespace radar_fusion_to_detected_object
 {
 using autoware_auto_perception_msgs::msg::DetectedObject;
 using autoware_auto_perception_msgs::msg::DetectedObjects;
-using autoware_auto_perception_msgs::msg::TrackedObjects;
 
 RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode(
   const rclcpp::NodeOptions & node_options)
@@ -181,7 +180,7 @@ bool RadarObjectFusionToDetectedObjectNode::isDataReady()
 }
 
 void RadarObjectFusionToDetectedObjectNode::onData(
-  const DetectedObjects::ConstSharedPtr object_msg, const TrackedObjects::ConstSharedPtr radar_msg)
+  const DetectedObjects::ConstSharedPtr object_msg, const DetectedObjects::ConstSharedPtr radar_msg)
 {
   detected_objects_ = object_msg;
   radar_objects_ = radar_msg;
@@ -211,7 +210,7 @@ void RadarObjectFusionToDetectedObjectNode::onData(
 }
 
 RadarFusionToDetectedObject::RadarInput RadarObjectFusionToDetectedObjectNode::setRadarInput(
-  const TrackedObject & radar_object, const std_msgs::msg::Header & header_)
+  const DetectedObject & radar_object, const std_msgs::msg::Header & header_)
 {
   RadarFusionToDetectedObject::RadarInput output{};
   output.pose_with_covariance = radar_object.kinematics.pose_with_covariance;

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

@@ -18,6 +18,7 @@
       enable_safety_check: true
       enable_yield_maneuver: true
       disable_path_update: false
+      use_hatched_road_markings: false
 
       # for debug
       publish_debug_marker: false

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -89,6 +89,7 @@ struct DrivableAreaInfo
   };
   std::vector<DrivableLanes> drivable_lanes;
   std::vector<Obstacle> obstacles;  // obstacles to extract from the drivable area
+  bool enable_expanding_hatched_road_markings{false};
 
   // temporary only for pull over's freespace planning
   double drivable_margin{0.0};

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -60,6 +60,7 @@ class LaneChangeBase
     prev_module_reference_path_ = std::make_shared<PathWithLaneId>();
     prev_module_path_ = std::make_shared<PathWithLaneId>();
     prev_drivable_area_info_ = std::make_shared<DrivableAreaInfo>();
+    prev_turn_signal_info_ = std::make_shared<TurnSignalInfo>();
   }
 
   LaneChangeBase(const LaneChangeBase &) = delete;
@@ -107,6 +108,11 @@ class LaneChangeBase
     }
   }
 
+  virtual void setPreviousTurnSignalInfo(const TurnSignalInfo & prev_turn_signal_info)
+  {
+    *prev_turn_signal_info_ = prev_turn_signal_info;
+  }
+
   virtual void updateSpecialData() {}
 
   const LaneChangeStatus & getLaneChangeStatus() const { return status_; }
@@ -232,6 +238,7 @@ class LaneChangeBase
   std::shared_ptr<PathWithLaneId> prev_module_reference_path_{};
   std::shared_ptr<PathWithLaneId> prev_module_path_{};
   std::shared_ptr<DrivableAreaInfo> prev_drivable_area_info_{};
+  std::shared_ptr<TurnSignalInfo> prev_turn_signal_info_{};
 
   PathWithLaneId prev_approved_path_{};
 

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

@@ -94,6 +94,9 @@ struct AvoidanceParameters
   // disable path update
   bool disable_path_update{false};
 
+  // use hatched road markings for avoidance
+  bool use_hatched_road_markings{false};
+
   // constrains
   bool use_constraints_for_decel{false};
 

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

@@ -229,13 +229,18 @@ boost::optional<lanelet::ConstLanelet> getLeftLanelet(
 std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & current_lanes);
 std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes);
+std::vector<geometry_msgs::msg::Point> calcBound(
+  const std::shared_ptr<RouteHandler> route_handler,
+  const std::vector<DrivableLanes> & drivable_lanes, const bool enable_expanding_polygon,
+  const bool is_left);
 
 boost::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
 std::vector<DrivableLanes> cutOverlappedLanes(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes);
 
 void generateDrivableArea(
-  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double vehicle_length,
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
+  const bool enable_expanding_polygon, const double vehicle_length,
   const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
 
 void generateDrivableArea(
@@ -386,8 +391,17 @@ std::vector<DrivableLanes> combineDrivableLanes(
   const std::vector<DrivableLanes> & original_drivable_lanes_vec,
   const std::vector<DrivableLanes> & new_drivable_lanes_vec);
 
+DrivableAreaInfo combineDrivableAreaInfo(
+  const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);
+
 void extractObstaclesFromDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & obstacles);
+
+void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left);
+
+std::optional<lanelet::Polygon3d> getPolygonByPoint(
+  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstPoint3d & point,
+  const std::string & polygon_name);
 }  // namespace behavior_path_planner::utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -525,6 +525,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
     p.enable_safety_check = declare_parameter<bool>(ns + "enable_safety_check");
     p.enable_yield_maneuver = declare_parameter<bool>(ns + "enable_yield_maneuver");
     p.disable_path_update = declare_parameter<bool>(ns + "disable_path_update");
+    p.use_hatched_road_markings = declare_parameter<bool>(ns + "use_hatched_road_markings");
     p.publish_debug_marker = declare_parameter<bool>(ns + "publish_debug_marker");
     p.print_debug_info = declare_parameter<bool>(ns + "print_debug_info");
   }

planning/behavior_path_planner/src/planner_manager.cpp

@@ -129,23 +129,22 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
 void PlannerManager::generateCombinedDrivableArea(
   BehaviorModuleOutput & output, const std::shared_ptr<PlannerData> & data) const
 {
+  const auto & di = output.drivable_area_info;
   constexpr double epsilon = 1e-3;
-  if (epsilon < std::abs(output.drivable_area_info.drivable_margin)) {
+
+  if (epsilon < std::abs(di.drivable_margin)) {
     // for single free space pull over
     const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points);
     const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true;
 
     utils::generateDrivableArea(
-      *output.path, data->parameters.vehicle_length, output.drivable_area_info.drivable_margin,
-      is_driving_forward);
-  } else if (output.drivable_area_info.is_already_expanded) {
+      *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward);
+  } else if (di.is_already_expanded) {
     // for single side shift
     utils::generateDrivableArea(
-      *output.path, output.drivable_area_info.drivable_lanes, data->parameters.vehicle_length,
-      data);
+      *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data);
   } else {
-    const auto shorten_lanes =
-      utils::cutOverlappedLanes(*output.path, output.drivable_area_info.drivable_lanes);
+    const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes);
 
     const auto & dp = data->drivable_area_expansion_parameters;
     const auto expanded_lanes = utils::expandLanelets(
@@ -154,11 +153,12 @@ void PlannerManager::generateCombinedDrivableArea(
 
     // for other modules where multiple modules may be launched
     utils::generateDrivableArea(
-      *output.path, expanded_lanes, data->parameters.vehicle_length, data);
+      *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings,
+      data->parameters.vehicle_length, data);
   }
 
   // extract obstacles from drivable area
-  utils::extractObstaclesFromDrivableArea(*output.path, output.drivable_area_info.obstacles);
+  utils::extractObstaclesFromDrivableArea(*output.path, di.obstacles);
 }
 
 std::vector<SceneModulePtr> PlannerManager::getRequestModules(

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

@@ -2523,18 +2523,25 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
   }
 
   {  // for new architecture
+    DrivableAreaInfo current_drivable_area_info;
     // generate drivable lanes
-    output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
-      getPreviousModuleOutput().drivable_area_info.drivable_lanes, drivable_lanes);
+    current_drivable_area_info.drivable_lanes = drivable_lanes;
     // generate obstacle polygons
-    output.drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea(
-      avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
+    current_drivable_area_info.obstacles =
+      utils::avoidance::generateObstaclePolygonsForDrivableArea(
+        avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
+    // expand hatched road markings
+    current_drivable_area_info.enable_expanding_hatched_road_markings =
+      parameters_->use_hatched_road_markings;
+
+    output.drivable_area_info = utils::combineDrivableAreaInfo(
+      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
   }
 
   {  // for old architecture
     // NOTE: Obstacles to avoid are not extracted from the drivable area with an old architecture.
     utils::generateDrivableArea(
-      *output.path, drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+      *output.path, drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_);
   }
 }
 
@@ -2783,7 +2790,18 @@ BehaviorModuleOutput AvoidanceModule::plan()
   }
 
   BehaviorModuleOutput output;
-  output.turn_signal_info = calcTurnSignalInfo(avoidance_path);
+
+  // turn signal info
+  {
+    const auto original_signal = getPreviousModuleOutput().turn_signal_info;
+    const auto new_signal = calcTurnSignalInfo(avoidance_path);
+    const auto current_seg_idx = planner_data_->findEgoSegmentIndex(avoidance_path.path.points);
+    output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+      avoidance_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
+      planner_data_->parameters.ego_nearest_dist_threshold,
+      planner_data_->parameters.ego_nearest_yaw_threshold);
+  }
+
   // sparse resampling for computational cost
   {
     avoidance_path.path =

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

@@ -639,7 +639,8 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
       utils::clipPathLength(path, ego_idx, planner_data_->parameters);
       const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes);
       utils::generateDrivableArea(
-        path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+        path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length,
+        planner_data_);
     }
   }
 
@@ -691,8 +692,11 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
       planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
   } else {
     const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*output.path, status_.lanes);
-    output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
-      getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes);
+
+    DrivableAreaInfo current_drivable_area_info;
+    current_drivable_area_info.drivable_lanes = target_drivable_lanes;
+    output.drivable_area_info = utils::combineDrivableAreaInfo(
+      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
   }
 
   // return to lane parking if it is possible
@@ -818,8 +822,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification(
       planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
   } else {
     const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, status_.lanes);
-    out.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
-      getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes);
+
+    DrivableAreaInfo current_drivable_area_info;
+    current_drivable_area_info.drivable_lanes = target_drivable_lanes;
+    out.drivable_area_info = utils::combineDrivableAreaInfo(
+      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
   }
 
   return out;
@@ -918,7 +925,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath()
   const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes);
   const auto target_drivable_lanes = getNonOverlappingExpandedLanes(reference_path, drivable_lanes);
   utils::generateDrivableArea(
-    reference_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_);
+    reference_path, target_drivable_lanes, false, common_parameters.vehicle_length, planner_data_);
 
   return reference_path;
 }
@@ -953,7 +960,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath()
   const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes);
   const auto target_drivable_lanes = getNonOverlappingExpandedLanes(stop_path, drivable_lanes);
   utils::generateDrivableArea(
-    stop_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_);
+    stop_path, target_drivable_lanes, false, common_parameters.vehicle_length, planner_data_);
 
   return stop_path;
 }

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

@@ -131,6 +131,7 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   }
 
   module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
+  module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
   auto output = module_type_->generateOutput();
   path_reference_ = output.reference_path;
   *prev_approved_path_ = *getPreviousModuleOutput().path;