Merge pull request #773 from tier4/sync-upstream

chore: sync upstream

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -346,9 +346,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   {
     std_msgs::msg::ColorRGBA sample_color;
     sample_color.r = 1.0;
-    sample_color.g = 0.0;
-    sample_color.b = 1.0;
-    colors.push_back(sample_color);  // magenta
+    sample_color.g = 0.65;
+    sample_color.b = 0.0;
+    colors.push_back(sample_color);  // orange
+    sample_color.r = 1.0;
+    sample_color.g = 1.0;
+    sample_color.b = 0.0;
+    colors.push_back(sample_color);  // yellow
     sample_color.r = 0.69;
     sample_color.g = 1.0;
     sample_color.b = 0.18;
@@ -361,22 +365,18 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     sample_color.g = 1.0;
     sample_color.b = 0.0;
     colors.push_back(sample_color);  // chartreuse green
-    sample_color.r = 0.12;
-    sample_color.g = 0.56;
-    sample_color.b = 1.0;
-    colors.push_back(sample_color);  // dodger blue
     sample_color.r = 0.0;
     sample_color.g = 1.0;
     sample_color.b = 1.0;
     colors.push_back(sample_color);  // cyan
-    sample_color.r = 0.54;
-    sample_color.g = 0.168;
-    sample_color.b = 0.886;
-    colors.push_back(sample_color);  // blueviolet
-    sample_color.r = 0.0;
-    sample_color.g = 1.0;
-    sample_color.b = 0.5;
-    colors.push_back(sample_color);  // spring green
+    sample_color.r = 0.53;
+    sample_color.g = 0.81;
+    sample_color.b = 0.98;
+    colors.push_back(sample_color);  // light skyblue
+    sample_color.r = 1.0;
+    sample_color.g = 0.41;
+    sample_color.b = 0.71;
+    colors.push_back(sample_color);  // hot pink
   }
 
   double get_line_width() { return m_line_width_property.getFloat(); }

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -64,8 +64,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr(
   marker_ptr->color = path_confidence_color;
   marker_ptr->pose.position = predicted_path.path.back().position;
   marker_ptr->text = std::to_string(predicted_path.confidence);
-  marker_ptr->color.a = std::max(
-    static_cast<double>(std::min(static_cast<double>(predicted_path.confidence), 0.999)), 0.5);
+  marker_ptr->color.a = 0.5;
   return marker_ptr;
 }
 
@@ -81,9 +80,8 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr(
   marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
   marker_ptr->pose = initPose();
   marker_ptr->color = predicted_path_color;
-  marker_ptr->color.a = std::max(
-    static_cast<double>(std::min(static_cast<double>(predicted_path.confidence), 0.999)), 0.5);
-  marker_ptr->scale.x = 0.03 * marker_ptr->color.a;
+  marker_ptr->color.a = 0.6;
+  marker_ptr->scale.x = 0.015;
   calc_path_line_list(predicted_path, marker_ptr->points, is_simple);
   for (size_t k = 0; k < marker_ptr->points.size(); ++k) {
     marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0;

common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp

@@ -150,7 +150,11 @@ void ConsoleMeterDisplay::update(float wall_dt, float ros_dt)
   font.setBold(true);
   painter.setFont(font);
   std::ostringstream velocity_ss;
-  velocity_ss << std::fixed << std::setprecision(2) << linear_x * 3.6 << "km/h";
+  if (last_msg_ptr_) {
+    velocity_ss << std::fixed << std::setprecision(2) << linear_x * 3.6 << "km/h";
+  } else {
+    velocity_ss << "Not received";
+  }
   painter.drawText(
     0, std::min(property_value_height_offset_->getInt(), h - 1), w,
     std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter,

common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp

@@ -153,7 +153,11 @@ void SteeringAngleDisplay::update(float wall_dt, float ros_dt)
   font.setBold(true);
   painter.setFont(font);
   std::ostringstream steering_angle_ss;
-  steering_angle_ss << std::fixed << std::setprecision(1) << steering * 180.0 / M_PI << "deg";
+  if (last_msg_ptr_) {
+    steering_angle_ss << std::fixed << std::setprecision(1) << steering * 180.0 / M_PI << "deg";
+  } else {
+    steering_angle_ss << "Not received";
+  }
   painter.drawText(
     0, std::min(property_value_height_offset_->getInt(), h - 1), w,
     std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignCenter | Qt::AlignVCenter,

launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml

@@ -1,7 +1,18 @@
 <launch>
-  <group>
-    <include file="$(find-pkg-share mission_planner)/launch/mission_planner.launch.xml"/>
-  </group>
+  <arg name="modified_goal_topic_name" default="/planning/scenario_planning/modified_goal"/>
+  <arg name="map_topic_name" default="/map/vector_map"/>
+  <arg name="visualization_topic_name" default="/planning/mission_planning/route_marker"/>
+  <arg name="mission_planner_param_path" default="$(find-pkg-share mission_planner)/config/mission_planner.param.yaml"/>
+
+  <node_container pkg="rclcpp_components" exec="component_container" name="mission_planner_container" namespace="">
+    <composable_node pkg="mission_planner" plugin="mission_planner::MissionPlanner" name="mission_planner" namespace="">
+      <param from="$(var mission_planner_param_path)"/>
+      <remap from="input/modified_goal" to="$(var modified_goal_topic_name)"/>
+      <remap from="input/vector_map" to="$(var map_topic_name)"/>
+      <remap from="debug/route_marker" to="$(var visualization_topic_name)"/>
+    </composable_node>
+    <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+  </node_container>
 
   <group>
     <include file="$(find-pkg-share mission_planner)/launch/goal_pose_visualizer.launch.xml"/>

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -24,15 +24,25 @@
     </group>
     <!-- motion velocity smoother -->
     <group>
-      <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
-      <set_remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
-      <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
-        <arg name="velocity_smoother_type" value="$(var velocity_smoother_type)"/>
-        <arg name="common_param_path" value="$(var common_param_path)"/>
-        <arg name="nearest_search_param_path" value="$(var nearest_search_param_path)"/>
-        <arg name="param_path" value="$(var motion_velocity_smoother_param_path)"/>
-        <arg name="velocity_smoother_param_path" value="$(var velocity_smoother_type_param_path)"/>
-      </include>
+      <node_container pkg="rclcpp_components" exec="component_container" name="motion_velocity_smoother_container" namespace="">
+        <composable_node pkg="motion_velocity_smoother" plugin="motion_velocity_smoother::MotionVelocitySmootherNode" name="motion_velocity_smoother" namespace="">
+          <param name="algorithm_type" value="$(var velocity_smoother_type)"/>
+          <param from="$(var common_param_path)"/>
+          <param from="$(var nearest_search_param_path)"/>
+          <param from="$(var motion_velocity_smoother_param_path)"/>
+          <param from="$(var velocity_smoother_type_param_path)"/>
+
+          <param name="publish_debug_trajs" value="false"/>
+          <remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
+          <remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
+
+          <remap from="~/input/external_velocity_limit_mps" to="/planning/scenario_planning/max_velocity"/>
+          <remap from="~/input/acceleration" to="/localization/acceleration"/>
+          <remap from="~/input/operation_mode_state" to="/system/operation_mode/state"/>
+          <remap from="~/output/current_velocity_limit_mps" to="/planning/scenario_planning/current_max_velocity"/>
+        </composable_node>
+        <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
+      </node_container>
     </group>
   </group>
 

map/map_projection_loader/README.md

@@ -25,7 +25,7 @@ sample-map-rosbag
 
 ```yaml
 # map_projector_info.yaml
-type: "Local"
+type: "local"
 ```
 
 ### Using MGRS

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -11,7 +11,10 @@
     sigma_yaw_angle_deg: 5.0 #[angle degree]
     object_buffer_time_length: 2.0 #[s]
     history_time_length: 1.0 #[s]
+    # parameter for shoulder lane prediction
+    prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
 
+    # parameters for lc prediction
     lane_change_detection:
       method: "lat_diff_distance" # choose from "lat_diff_distance" or "time_to_change_lane"
       time_to_change_lane:

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -639,33 +639,31 @@ lanelet::Lanelets getLeftOppositeLanelets(
 MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options)
 : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0)
 {
-  enable_delay_compensation_ = declare_parameter("enable_delay_compensation", true);
-  prediction_time_horizon_ = declare_parameter("prediction_time_horizon", 10.0);
-  prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time", 0.5);
+  enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
+  prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
+  prediction_sampling_time_interval_ = declare_parameter<double>("prediction_sampling_delta_time");
   min_velocity_for_map_based_prediction_ =
-    declare_parameter("min_velocity_for_map_based_prediction", 1.0);
-  min_crosswalk_user_velocity_ = declare_parameter("min_crosswalk_user_velocity", 1.0);
+    declare_parameter<double>("min_velocity_for_map_based_prediction");
+  min_crosswalk_user_velocity_ = declare_parameter<double>("min_crosswalk_user_velocity");
   dist_threshold_for_searching_lanelet_ =
-    declare_parameter("dist_threshold_for_searching_lanelet", 3.0);
+    declare_parameter<double>("dist_threshold_for_searching_lanelet");
   delta_yaw_threshold_for_searching_lanelet_ =
-    declare_parameter("delta_yaw_threshold_for_searching_lanelet", 0.785);
-  sigma_lateral_offset_ = declare_parameter("sigma_lateral_offset", 0.5);
-  sigma_yaw_angle_deg_ = declare_parameter("sigma_yaw_angle_deg", 5.0);
-  object_buffer_time_length_ = declare_parameter("object_buffer_time_length", 2.0);
-  history_time_length_ = declare_parameter("history_time_length", 1.0);
+    declare_parameter<double>("delta_yaw_threshold_for_searching_lanelet");
+  sigma_lateral_offset_ = declare_parameter<double>("sigma_lateral_offset");
+  sigma_yaw_angle_deg_ = declare_parameter<double>("sigma_yaw_angle_deg");
+  object_buffer_time_length_ = declare_parameter<double>("object_buffer_time_length");
+  history_time_length_ = declare_parameter<double>("history_time_length");
   {  // lane change detection
     lane_change_detection_method_ = declare_parameter<std::string>("lane_change_detection.method");
 
     // lane change detection by time_to_change_lane
-    dist_threshold_to_bound_ = declare_parameter(
-      "lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection",
-      1.0);  // 1m
-    time_threshold_to_bound_ = declare_parameter(
-      "lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection",
-      5.0);  // 5 sec
-    cutoff_freq_of_velocity_lpf_ = declare_parameter(
-      "lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_detection",
-      0.1);  // 0.1Hz
+    dist_threshold_to_bound_ = declare_parameter<double>(
+      "lane_change_detection.time_to_change_lane.dist_threshold_for_lane_change_detection");  // 1m
+    time_threshold_to_bound_ = declare_parameter<double>(
+      "lane_change_detection.time_to_change_lane.time_threshold_for_lane_change_detection");
+    cutoff_freq_of_velocity_lpf_ = declare_parameter<double>(
+      "lane_change_detection.time_to_change_lane.cutoff_freq_of_velocity_for_lane_change_"
+      "detection");
 
     // lane change detection by lat_diff_distance
     dist_ratio_threshold_to_left_bound_ = declare_parameter<double>(
@@ -680,11 +678,11 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     num_continuous_state_transition_ =
       declare_parameter<int>("lane_change_detection.num_continuous_state_transition");
   }
-  reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5);
+  reference_path_resolution_ = declare_parameter<double>("reference_path_resolution");
   /* prediction path will disabled when the estimated path length exceeds lanelet length. This
    * parameter control the estimated path length = vx * th * (rate)  */
   prediction_time_horizon_rate_for_validate_lane_length_ =
-    declare_parameter("prediction_time_horizon_rate_for_validate_lane_length", 0.8);
+    declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");
 
   path_generator_ = std::make_shared<PathGenerator>(
     prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_);

perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp

@@ -113,7 +113,7 @@ class MultiCameraFusion : public rclcpp::Node
   /*
   the mapping from traffic light id (instance id) to regulatory element id (group id)
   */
-  std::map<lanelet::Id, lanelet::Id> traffic_light_id_to_regulatory_ele_id_;
+  std::map<lanelet::Id, std::vector<lanelet::Id>> traffic_light_id_to_regulatory_ele_id_;
   /*
   save record arrays by increasing timestamp order.
   use multiset in case there are multiple cameras publishing images at exactly the same time

perception/traffic_light_multi_camera_fusion/src/node.cpp

@@ -216,7 +216,7 @@ void MultiCameraFusion::mapCallback(
 
     auto lights = tl->trafficLights();
     for (const auto & light : lights) {
-      traffic_light_id_to_regulatory_ele_id_[light.id()] = tl->id();
+      traffic_light_id_to_regulatory_ele_id_[light.id()].emplace_back(tl->id());
     }
   }
 }
@@ -302,11 +302,15 @@ void MultiCameraFusion::groupFusion(
     if (traffic_light_id_to_regulatory_ele_id_.count(roi_id) == 0) {
       RCLCPP_WARN_STREAM(
         get_logger(), "Found Traffic Light Id = " << roi_id << " which is not defined in Map");
-    } else {
-      /*
-      keep the best record for every regulatory element id
-      */
-      IdType reg_ele_id = traffic_light_id_to_regulatory_ele_id_[p.second.roi.traffic_light_id];
+      continue;
+    }
+
+    /*
+    keep the best record for every regulatory element id
+    */
+    const auto reg_ele_id_vec =
+      traffic_light_id_to_regulatory_ele_id_[p.second.roi.traffic_light_id];
+    for (const auto & reg_ele_id : reg_ele_id_vec) {
       if (
         grouped_record_map.count(reg_ele_id) == 0 ||
         ::compareRecord(p.second, grouped_record_map[reg_ele_id]) >= 0) {

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

@@ -142,7 +142,8 @@
         time_resolution: 0.5                             # [s]
         time_horizon: 10.0                               # [s]
         safety_check_backward_distance: 100.0            # [m]
-        safety_check_hysteresis_factor: 2.0              # [-]
+        hysteresis_factor_expand_rate: 2.0               # [-]
+        hysteresis_factor_safe_count: 10                 # [-]
         # rss parameters
         expected_front_deceleration: -1.0                # [m/ss]
         expected_rear_deceleration: -1.0                 # [m/ss]

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

@@ -131,6 +131,10 @@ MarkerArray createObjectsMarkerArray(
 MarkerArray createDrivableLanesMarkerArray(
   const std::vector<DrivableLanes> & drivable_lanes, std::string && ns);
 
+MarkerArray createPredictedPathMarkerArray(
+  const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info,
+  std::string && ns, const int32_t & id, const float & r, const float & g, const float & b);
+
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -556,6 +556,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   bool arrived_path_end_{false};
 
+  bool safe_{true};
+
   std::shared_ptr<AvoidanceParameters> parameters_;
 
   helper::avoidance::AvoidanceHelper helper_;
@@ -576,6 +578,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   ObjectDataArray registered_objects_;
 
+  mutable size_t safe_count_{0};
+
   mutable ObjectDataArray ego_stopped_objects_;
 
   mutable ObjectDataArray stopped_objects_;

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

@@ -194,7 +194,8 @@ struct AvoidanceParameters
   double safety_check_backward_distance;
 
   // transit hysteresis (unsafe to safe)
-  double safety_check_hysteresis_factor;
+  size_t hysteresis_factor_safe_count;
+  double hysteresis_factor_expand_rate;
 
   // keep target velocity in yield maneuver
   double yield_velocity;

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp

@@ -55,24 +55,37 @@ PredictedObjects filterObjects(
   const std::shared_ptr<ObjectsFilteringParams> & params);
 
 /**
- * @brief Filter objects based on their velocity.
+ * @brief Filters objects based on their velocity.
  *
- * @param objects The predicted objects to filter.
- * @param lim_v Velocity limit for filtering.
- * @return PredictedObjects The filtered objects.
+ * Depending on the remove_above_threshold parameter, this function either removes objects with
+ * velocities above the given threshold or only keeps those objects. It uses the helper function
+ * filterObjectsByVelocity() to do the actual filtering.
+ *
+ * @param objects The objects to be filtered.
+ * @param velocity_threshold The velocity threshold for the filtering.
+ * @param remove_above_threshold If true, objects with velocities above the threshold are removed.
+ *                               If false, only objects with velocities above the threshold are
+ * kept.
+ * @return A new collection of objects that have been filtered according to the rules.
  */
-PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v);
+PredictedObjects filterObjectsByVelocity(
+  const PredictedObjects & objects, const double velocity_threshold,
+  const bool remove_above_threshold = true);
 
 /**
- * @brief Filter objects based on a velocity range.
+ * @brief Helper function to filter objects based on their velocity.
  *
- * @param objects The predicted objects to filter.
- * @param min_v Minimum velocity for filtering.
- * @param max_v Maximum velocity for filtering.
- * @return PredictedObjects The filtered objects.
+ * This function iterates over all objects and calculates their velocity norm. If the velocity norm
+ * is within the velocity_threshold and max_velocity range, the object is added to a new collection.
+ * This new collection is then returned.
+ *
+ * @param objects The objects to be filtered.
+ * @param velocity_threshold The minimum velocity for an object to be included in the output.
+ * @param max_velocity The maximum velocity for an object to be included in the output.
+ * @return A new collection of objects that have been filtered according to the rules.
  */
 PredictedObjects filterObjectsByVelocity(
-  const PredictedObjects & objects, double min_v, double max_v);
+  const PredictedObjects & objects, double velocity_threshold, double max_velocity);
 
 /**
  * @brief Filter objects based on their position relative to a current_pose.

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -63,6 +63,9 @@ Polygon2d createExtendedPolygon(
   const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin,
   CollisionCheckDebug & debug);
 
+PredictedPath convertToPredictedPath(
+  const std::vector<PoseWithVelocityStamped> & path, const double time_resolution);
+
 double calcRssDistance(
   const double front_object_velocity, const double rear_object_velocity,
   const RSSparams & rss_params);
@@ -98,7 +101,7 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  CollisionCheckDebug & debug);
+  const double hysteresis_factor, CollisionCheckDebug & debug);
 
 /**
  * @brief Check collision between ego path footprints with extra longitudinal stopping margin and