Merge branch 'autowarefoundation:main' into refactor/radar_threshold_filter_src_parameter

Author: scepter914

.github/CODEOWNERS

@@ -493,7 +493,7 @@ namespace
 //!
 inline LogStreamConsumer LOG_VERBOSE(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE) << "[TRT] ";
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE);
 }
 
 //!
@@ -505,7 +505,7 @@ inline LogStreamConsumer LOG_VERBOSE(const Logger & logger)
 //!
 inline LogStreamConsumer LOG_INFO(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO) << "[TRT] ";
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO);
 }
 
 //!
@@ -517,7 +517,7 @@ inline LogStreamConsumer LOG_INFO(const Logger & logger)
 //!
 inline LogStreamConsumer LOG_WARN(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING) << "[TRT] ";
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING);
 }
 
 //!
@@ -529,7 +529,7 @@ inline LogStreamConsumer LOG_WARN(const Logger & logger)
 //!
 inline LogStreamConsumer LOG_ERROR(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR) << "[TRT] ";
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR);
 }
 
 //!
@@ -543,7 +543,7 @@ inline LogStreamConsumer LOG_ERROR(const Logger & logger)
 //!
 inline LogStreamConsumer LOG_FATAL(const Logger & logger)
 {
-  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR) << "[TRT] ";
+  return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR);
 }
 
 }  // anonymous namespace

common/tensorrt_common/include/tensorrt_common/logger.hpp

@@ -8,6 +8,7 @@
   <!-- Parameter files -->
   <arg name="pointcloud_map_loader_param_path"/>
   <arg name="lanelet2_map_loader_param_path"/>
+  <arg name="map_projection_loader_param_path"/>
 
   <!-- whether use intra-process -->
   <arg name="use_intra_process" default="false"/>
@@ -59,6 +60,7 @@
     </node>
 
     <include file="$(find-pkg-share map_projection_loader)/launch/map_projection_loader.launch.xml">
+      <arg name="param_path" value="$(var map_projection_loader_param_path)"/>
       <arg name="map_projector_info_path" value="$(var map_projector_info_path)"/>
       <arg name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
     </include>

launch/tier4_map_launch/launch/map.launch.xml

@@ -74,6 +74,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
   // load map from file
   const auto map = load_map(lanelet2_filename, *msg);
   if (!map) {
+    RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published.");
     return;
   }
 
@@ -87,6 +88,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
   pub_map_bin_ =
     create_publisher<HADMapBin>("output/lanelet2_map", rclcpp::QoS{1}.transient_local());
   pub_map_bin_->publish(map_bin_msg);
+  RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published.");
 }
 
 lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
@@ -105,6 +107,12 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
     const lanelet::projection::LocalProjector projector;
     const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
 
+    if (!errors.empty()) {
+      for (const auto & error : errors) {
+        RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
+      }
+    }
+
     // overwrite local_x, local_y
     for (lanelet::Point3d point : map->pointLayer) {
       if (point.hasAttribute("local_x")) {

map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -90,9 +90,8 @@ std::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::getPCDMetadata(
 {
   std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
   if (pcd_paths.size() != 1) {
-    while (!fs::exists(pcd_metadata_path)) {
-      RCLCPP_ERROR_STREAM(get_logger(), "PCD metadata file not found: " << pcd_metadata_path);
-      std::this_thread::sleep_for(std::chrono::seconds(1));
+    if (!fs::exists(pcd_metadata_path)) {
+      throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path);
     }
 
     pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path);

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -59,4 +59,5 @@ endif()
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

map/map_projection_loader/CMakeLists.txt

@@ -82,11 +82,10 @@ map_origin:
 
 ## Published Topics
 
-- ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information
+- `~/map_projector_info` (tier4_map_msgs/MapProjectorInfo) : This topic shows the definition of map projector information
 
 ## Parameters
 
-| Name                    | Type        | Description                                                                      |
-| :---------------------- | :---------- | :------------------------------------------------------------------------------- |
-| map_projector_info_path | std::string | A path to map_projector_info.yaml (used by default)                              |
-| lanelet2_map_path       | std::string | A path to lanelet2 map (used only when `map_projector_info_path` does not exist) |
+Note that these parameters are assumed to be passed from launch arguments, and it is not recommended to directly write them in `map_projection_loader.param.yaml`.
+
+{{ json_to_markdown("map/map_projection_loader/schema/map_projection_loader.schema.json") }}

map/map_projection_loader/README.md

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    map_projector_info_path: $(var map_projector_info_path)
+    lanelet2_map_path: $(var lanelet2_map_path)

map/map_projection_loader/config/map_projection_loader.param.yaml

@@ -1,9 +1,10 @@
 <launch>
+  <arg name="param_path" default="$(find-pkg-share map_projection_loader)/config/map_projection_loader.param.yaml"/>
+
   <arg name="map_projector_info_path" description="Path to the yaml file"/>
   <arg name="lanelet2_map_path" description="Path to the lanelet2 map file"/>
 
   <node pkg="map_projection_loader" exec="map_projection_loader" name="map_projection_loader" output="screen">
-    <param name="map_projector_info_path" value="$(var map_projector_info_path)"/>
-    <param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
+    <param from="$(var param_path)" allow_substs="true"/>
   </node>
 </launch>

map/map_projection_loader/launch/map_projection_loader.launch.xml

@@ -0,0 +1,38 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for map_projection_loader",
+  "type": "object",
+  "definitions": {
+    "map_projection_loader": {
+      "type": "object",
+      "properties": {
+        "map_projector_info_path": {
+          "type": "string",
+          "description": "The path where map_projector_info.yaml is located",
+          "default": "$(var map_projector_info_path)"
+        },
+        "lanelet2_map_path": {
+          "type": "string",
+          "description": "The path where the lanelet2 map file (.osm) is located",
+          "default": "$(var lanelet2_map_path)"
+        }
+      },
+      "required": ["map_projector_info_path", "lanelet2_map_path"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/map_projection_loader"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

map/map_projection_loader/schema/map_projection_loader.schema.json

@@ -19,6 +19,7 @@
     use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not
     speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value
     acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds
+    use_crosswalk_signal: true
     # parameter for shoulder lane prediction
     prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
 

perception/map_based_prediction/config/map_based_prediction.param.yaml

@@ -29,6 +29,7 @@
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
@@ -42,6 +43,7 @@
 #include <algorithm>
 #include <deque>
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -107,6 +109,9 @@ using autoware_auto_perception_msgs::msg::TrackedObject;
 using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
 using autoware_auto_perception_msgs::msg::TrackedObjects;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using autoware_perception_msgs::msg::TrafficSignal;
+using autoware_perception_msgs::msg::TrafficSignalArray;
+using autoware_perception_msgs::msg::TrafficSignalElement;
 using tier4_autoware_utils::StopWatch;
 using tier4_debug_msgs::msg::StringStamped;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
@@ -122,6 +127,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   rclcpp::Publisher<StringStamped>::SharedPtr pub_calculation_time_;
   rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
+  rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_signals_;
 
   // Object History
   std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
@@ -131,6 +137,8 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::shared_ptr<lanelet::routing::RoutingGraph> routing_graph_ptr_;
   std::shared_ptr<lanelet::traffic_rules::TrafficRules> traffic_rules_ptr_;
 
+  std::unordered_map<lanelet::Id, TrafficSignal> traffic_signal_id_map_;
+
   // parameter update
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
   rcl_interfaces::msg::SetParametersResult onParam(
@@ -181,11 +189,14 @@ class MapBasedPredictionNode : public rclcpp::Node
   double speed_limit_multiplier_;
   double acceleration_exponential_half_life_;
 
+  bool use_crosswalk_signal_;
+
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
 
   // Member Functions
   void mapCallback(const HADMapBin::ConstSharedPtr msg);
+  void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);
   void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects);
 
   bool doesPathCrossAnyFence(const PredictedPath & predicted_path);
@@ -249,6 +260,8 @@ class MapBasedPredictionNode : public rclcpp::Node
     const LaneletsData & lanelets_data);
   bool isDuplicated(
     const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths);
+  std::optional<lanelet::Id> getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet);
+  std::optional<TrafficSignalElement> getTrafficSignalElement(const lanelet::Id & id);
 
   visualization_msgs::msg::Marker getDebugMarker(
     const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num);

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -3,12 +3,14 @@
   <arg name="param_path" default="$(find-pkg-share map_based_prediction)/config/map_based_prediction.param.yaml"/>
 
   <arg name="vector_map_topic" default="/map/vector_map"/>
+  <arg name="traffic_signals_topic" default="/perception/traffic_light_recognition/traffic_signals"/>
   <arg name="output_topic" default="objects"/>
   <arg name="input_topic" default="/perception/object_recognition/tracking/objects"/>
 
   <node pkg="map_based_prediction" exec="map_based_prediction" name="map_based_prediction" output="screen">
     <param from="$(var param_path)"/>
     <remap from="/vector_map" to="$(var vector_map_topic)"/>
+    <remap from="/traffic_signals" to="$(var traffic_signals_topic)"/>
     <remap from="~/output/objects" to="$(var output_topic)"/>
     <remap from="~/input/objects" to="$(var input_topic)"/>
   </node>

perception/map_based_prediction/launch/map_based_prediction.launch.xml

@@ -16,6 +16,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_perception_msgs</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
   <depend>libgoogle-glog-dev</depend>

perception/map_based_prediction/package.xml

@@ -788,6 +788,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   acceleration_exponential_half_life_ =
     declare_parameter<double>("acceleration_exponential_half_life");
 
+  use_crosswalk_signal_ = declare_parameter<bool>("use_crosswalk_signal");
+
   path_generator_ = std::make_shared<PathGenerator>(
     prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
     min_crosswalk_user_velocity_);
@@ -801,6 +803,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   sub_map_ = this->create_subscription<HADMapBin>(
     "/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1));
+  sub_traffic_signals_ = this->create_subscription<TrafficSignalArray>(
+    "/traffic_signals", 1,
+    std::bind(&MapBasedPredictionNode::trafficSignalsCallback, this, std::placeholders::_1));
 
   pub_objects_ = this->create_publisher<PredictedObjects>("~/output/objects", rclcpp::QoS{1});
   pub_debug_markers_ =
@@ -872,6 +877,14 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg)
   crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end());
 }
 
+void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg)
+{
+  traffic_signal_id_map_.clear();
+  for (const auto & signal : msg->signals) {
+    traffic_signal_id_map_[signal.traffic_signal_id] = signal;
+  }
+}
+
 void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects)
 {
   stop_watch_.tic();
@@ -1218,6 +1231,18 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   }
   // try to find the edge points for all crosswalks and generate path to the crosswalk edge
   for (const auto & crosswalk : crosswalks_) {
+    const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk);
+    if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) {
+      const auto signal_color = [&] {
+        const auto elem_opt = getTrafficSignalElement(crosswalk_signal_id_opt.value());
+        return elem_opt ? elem_opt.value().color : TrafficSignalElement::UNKNOWN;
+      }();
+
+      if (signal_color == TrafficSignalElement::RED) {
+        continue;
+      }
+    }
+
     const auto edge_points = getCrosswalkEdgePoints(crosswalk);
 
     const auto reachable_first = hasPotentialToReach(
@@ -2211,6 +2236,38 @@ bool MapBasedPredictionNode::isDuplicated(
 
   return false;
 }
+
+std::optional<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
+  const lanelet::ConstLanelet & way_lanelet)
+{
+  const auto traffic_light_reg_elems =
+    way_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
+  if (traffic_light_reg_elems.empty()) {
+    return std::nullopt;
+  } else if (traffic_light_reg_elems.size() > 1) {
+    RCLCPP_ERROR(
+      get_logger(),
+      "[Map Based Prediction]: "
+      "Multiple regulatory elements as TrafficLight are defined to one lanelet object.");
+  }
+  return traffic_light_reg_elems.front()->id();
+}
+
+std::optional<TrafficSignalElement> MapBasedPredictionNode::getTrafficSignalElement(
+  const lanelet::Id & id)
+{
+  if (traffic_signal_id_map_.count(id) != 0) {
+    const auto & signal_elements = traffic_signal_id_map_.at(id).elements;
+    if (signal_elements.size() > 1) {
+      RCLCPP_ERROR(
+        get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received.");
+    } else if (!signal_elements.empty()) {
+      return signal_elements.front();
+    }
+  }
+  return std::nullopt;
+}
+
 }  // namespace map_based_prediction
 
 #include <rclcpp_components/register_node_macro.hpp>