Merge pull request #726 from tier4/sync-upstream

chore: sync upstream

control/lane_departure_checker/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The lane_departure_checker package</description>
   <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
+  <maintainer email="[email protected]">Makoto Kurihara</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -23,6 +23,7 @@
 #include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
@@ -31,6 +32,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <tier4_debug_msgs/msg/string_stamped.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
@@ -106,6 +108,7 @@ using autoware_auto_perception_msgs::msg::TrackedObject;
 using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
 using autoware_auto_perception_msgs::msg::TrackedObjects;
 using tier4_autoware_utils::StopWatch;
+using tier4_debug_msgs::msg::StringStamped;
 
 class MapBasedPredictionNode : public rclcpp::Node
 {
@@ -116,6 +119,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   // ROS Publisher and Subscriber
   rclcpp::Publisher<PredictedObjects>::SharedPtr pub_objects_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_markers_;
+  rclcpp::Publisher<StringStamped>::SharedPtr pub_calculation_time_;
   rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
 
@@ -179,8 +183,7 @@ class MapBasedPredictionNode : public rclcpp::Node
 
   LaneletsData getCurrentLanelets(const TrackedObject & object);
   bool checkCloseLaneletCondition(
-    const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object,
-    const lanelet::BasicPoint2d & search_point);
+    const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object);
   float calculateLocalLikelihood(
     const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const;
   void updateObjectData(TrackedObject & object);

perception/map_based_prediction/package.xml

@@ -23,6 +23,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>unique_identifier_msgs</depend>
   <depend>visualization_msgs</depend>
 

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -40,6 +40,8 @@
 namespace map_based_prediction
 {
 
+namespace
+{
 /**
  * @brief First order Low pass filtering
  *
@@ -586,6 +588,15 @@ ObjectClassification::_label_type changeLabelForPrediction(
   }
 }
 
+StringStamped createStringStamped(const rclcpp::Time & now, const double data)
+{
+  StringStamped msg;
+  msg.stamp = now;
+  msg.data = std::to_string(data);
+  return msg;
+}
+}  // namespace
+
 MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options)
 : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0)
 {
@@ -649,6 +660,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   pub_objects_ = this->create_publisher<PredictedObjects>("objects", rclcpp::QoS{1});
   pub_debug_markers_ =
     this->create_publisher<visualization_msgs::msg::MarkerArray>("maneuver", rclcpp::QoS{1});
+  pub_calculation_time_ = create_publisher<StringStamped>("~/debug/calculation_time", 1);
 }
 
 PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics(
@@ -691,6 +703,7 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg)
 
 void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects)
 {
+  stop_watch_.tic();
   // Guard for map pointer and frame transformation
   if (!lanelet_map_ptr_) {
     return;
@@ -869,6 +882,8 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
   // Publish Results
   pub_objects_->publish(output);
   pub_debug_markers_->publish(debug_markers);
+  const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc());
+  pub_calculation_time_->publish(calculation_time_msg);
 }
 
 PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
@@ -1076,39 +1091,48 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob
     return {};
   }
 
-  LaneletsData closest_lanelets;
+  LaneletsData object_lanelets;
+  std::optional<std::pair<double, lanelet::Lanelet>> closest_lanelet{std::nullopt};
   for (const auto & lanelet : surrounding_lanelets) {
     // Check if the close lanelets meet the necessary condition for start lanelets and
-    // Check if similar lanelet is inside the closest lanelet
-    if (
-      !checkCloseLaneletCondition(lanelet, object, search_point) ||
-      isDuplicated(lanelet, closest_lanelets)) {
+    // Check if similar lanelet is inside the object lanelet
+    if (!checkCloseLaneletCondition(lanelet, object) || isDuplicated(lanelet, object_lanelets)) {
       continue;
     }
 
-    LaneletData closest_lanelet;
-    closest_lanelet.lanelet = lanelet.second;
-    closest_lanelet.probability = calculateLocalLikelihood(lanelet.second, object);
-    closest_lanelets.push_back(closest_lanelet);
+    // Memorize closest lanelet
+    // NOTE: The object may be outside the lanelet.
+    if (!closest_lanelet || lanelet.first < closest_lanelet->first) {
+      closest_lanelet = lanelet;
+    }
+
+    // Check if the obstacle is inside of this lanelet
+    constexpr double epsilon = 1e-3;
+    if (lanelet.first < epsilon) {
+      const auto object_lanelet =
+        LaneletData{lanelet.second, calculateLocalLikelihood(lanelet.second, object)};
+      object_lanelets.push_back(object_lanelet);
+    }
   }
 
-  return closest_lanelets;
+  if (!object_lanelets.empty()) {
+    return object_lanelets;
+  }
+  if (closest_lanelet) {
+    return LaneletsData{LaneletData{
+      closest_lanelet->second, calculateLocalLikelihood(closest_lanelet->second, object)}};
+  }
+  return LaneletsData{};
 }
 
 bool MapBasedPredictionNode::checkCloseLaneletCondition(
-  const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object,
-  const lanelet::BasicPoint2d & search_point)
+  const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object)
 {
   // Step1. If we only have one point in the centerline, we will ignore the lanelet
   if (lanelet.second.centerline().size() <= 1) {
     return false;
   }
 
-  // Step2. Check if the obstacle is inside of this lanelet
-  if (!lanelet::geometry::inside(lanelet.second, search_point)) {
-    return false;
-  }
-
   // If the object is in the objects history, we check if the target lanelet is
   // inside the current lanelets id or following lanelets
   const std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
@@ -1124,15 +1148,15 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition(
     }
   }
 
-  // Step3. Calculate the angle difference between the lane angle and obstacle angle
+  // Step2. Calculate the angle difference between the lane angle and obstacle angle
   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;
   const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);
   const double abs_norm_delta = std::fabs(normalized_delta_yaw);
 
-  // Step4. Check if the closest lanelet is valid, and add all
+  // Step3. Check if the closest lanelet is valid, and add all
   // of the lanelets that are below max_dist and max_delta_yaw
   const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x;
   const bool is_yaw_reversed =

perception/traffic_light_multi_camera_fusion/README.md

@@ -9,12 +9,13 @@
 
 ## Input topics
 
-For every camera, the following three topics are subscribed:  
-| Name | Type | |
-| ---------------------------------------| -------------------------------------------------------|----------------------------------------------------|
-| `~/<camera_namespace>/camera_info` | sensor_msgs::CameraInfo |camera info from traffic_light_map_based_detector |
-| `~/<camera_namespace>/rois` | tier4_perception_msgs::TrafficLightRoiArray |detection roi from traffic_light_fine_detector |
-| `~/<camera_namespace>/traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray |classification result from traffic_light_classifier |
+For every camera, the following three topics are subscribed:
+
+| Name                                   | Type                                           | Description                                         |
+| -------------------------------------- | ---------------------------------------------- | --------------------------------------------------- |
+| `~/<camera_namespace>/camera_info`     | sensor_msgs::CameraInfo                        | camera info from traffic_light_map_based_detector   |
+| `~/<camera_namespace>/rois`            | tier4_perception_msgs::TrafficLightRoiArray    | detection roi from traffic_light_fine_detector      |
+| `~/<camera_namespace>/traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray | classification result from traffic_light_classifier |
 
 You don't need to configure these topics manually. Just provide the `camera_namespaces` parameter and the node will automatically extract the `<camera_namespace>` and create the subscribers.
 

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

@@ -143,6 +143,10 @@ class NormalLaneChange : public LaneChangeBase
     const double front_decel, const double rear_decel,
     std::unordered_map<std::string, CollisionCheckDebug> & debug_data) const;
 
+  LaneChangeTargetObjectIndices filterObject(
+    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+    const lanelet::ConstLanelets & target_backward_lanes) const;
+
   rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
 };
 }  // namespace behavior_path_planner

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

@@ -186,12 +186,6 @@ boost::optional<size_t> getLeadingStaticObjectIdx(
 std::optional<lanelet::BasicPolygon2d> createPolygon(
   const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);
 
-LaneChangeTargetObjectIndices filterObject(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes,
-  const Pose & current_pose, const RouteHandler & route_handler,
-  const LaneChangeParameters & lane_change_parameters);
-
 ExtendedPredictedObject transform(
   const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
   const LaneChangeParameters & lane_change_parameters);

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

@@ -604,9 +604,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
     route_handler, target_lanes, current_pose, backward_length);
 
   // filter objects to get target object index
-  const auto target_obj_index = utils::lane_change::filterObject(
-    objects, current_lanes, target_lanes, target_backward_lanes, current_pose, route_handler,
-    *lane_change_parameters_);
+  const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes);
 
   LaneChangeTargetObjects target_objects;
   target_objects.current_lane.reserve(target_obj_index.current_lane.size());
@@ -637,6 +635,117 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
   return target_objects;
 }
 
+LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+  const lanelet::ConstLanelets & target_backward_lanes) const
+{
+  const auto current_pose = getEgoPose();
+  const auto current_vel = getEgoVelocity();
+  const auto & route_handler = *getRouteHandler();
+  const auto & common_parameters = planner_data_->parameters;
+  const auto & objects = *planner_data_->dynamic_object;
+  const auto & object_check_min_road_shoulder_width =
+    lane_change_parameters_->object_check_min_road_shoulder_width;
+  const auto & object_shiftable_ratio_threshold =
+    lane_change_parameters_->object_shiftable_ratio_threshold;
+
+  // Guard
+  if (objects.objects.empty()) {
+    return {};
+  }
+
+  // Get path
+  const auto path =
+    route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto target_path =
+    route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits<double>::max());
+
+  const auto current_polygon =
+    utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto target_polygon =
+    utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto target_backward_polygon = utils::lane_change::createPolygon(
+    target_backward_lanes, 0.0, std::numeric_limits<double>::max());
+
+  // minimum distance to lane changing start point
+  const double t_prepare = common_parameters.lane_change_prepare_duration;
+  const double a_min = lane_change_parameters_->min_longitudinal_acc;
+  const double min_dist_to_lane_changing_start = std::max(
+    current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare);
+
+  LaneChangeTargetObjectIndices filtered_obj_indices;
+  for (size_t i = 0; i < objects.objects.size(); ++i) {
+    const auto & object = objects.objects.at(i);
+    const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x;
+    const auto extended_object =
+      utils::lane_change::transform(object, common_parameters, *lane_change_parameters_);
+
+    // ignore specific object types
+    if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) {
+      continue;
+    }
+
+    const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
+
+    // calc distance from the current ego position
+    double max_dist_ego_to_obj = std::numeric_limits<double>::lowest();
+    double min_dist_ego_to_obj = std::numeric_limits<double>::max();
+    for (const auto & polygon_p : obj_polygon.outer()) {
+      const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
+      const double dist_ego_to_obj =
+        motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p);
+      max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj);
+      min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj);
+    }
+
+    // ignore static object that are behind the ego vehicle
+    if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) {
+      continue;
+    }
+
+    // check if the object intersects with target lanes
+    if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
+      // ignore static parked object that are in front of the ego vehicle in target lanes
+      bool is_parked_object = utils::lane_change::isParkedObject(
+        target_path, route_handler, extended_object, object_check_min_road_shoulder_width,
+        object_shiftable_ratio_threshold);
+      if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) {
+        continue;
+      }
+
+      filtered_obj_indices.target_lane.push_back(i);
+      continue;
+    }
+
+    // check if the object intersects with target backward lanes
+    if (
+      target_backward_polygon &&
+      boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) {
+      filtered_obj_indices.target_lane.push_back(i);
+      continue;
+    }
+
+    // check if the object intersects with current lanes
+    if (
+      current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) &&
+      max_dist_ego_to_obj >= 0.0) {
+      // check only the objects that are in front of the ego vehicle
+      filtered_obj_indices.current_lane.push_back(i);
+      continue;
+    }
+
+    // ignore all objects that are behind the ego vehicle and not on the current and target
+    // lanes
+    if (max_dist_ego_to_obj < 0.0) {
+      continue;
+    }
+
+    filtered_obj_indices.other_lane.push_back(i);
+  }
+
+  return filtered_obj_indices;
+}
+
 PathWithLaneId NormalLaneChange::getTargetSegment(
   const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose,
   const double target_lane_length, const double lane_changing_length,

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -185,7 +185,7 @@ void SideShiftModule::updateData()
     }
   }
 
-  if (current_state_ != ModuleStatus::RUNNING) {
+  if (current_state_ != ModuleStatus::RUNNING && current_state_ != ModuleStatus::IDLE) {
     return;
   }