Merge branch 'main' into fix/fix_sensing_docs

Author: vividf

perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp

@@ -193,9 +193,15 @@ TrtClassifier::TrtClassifier(
 
 TrtClassifier::~TrtClassifier()
 {
-  if (m_cuda) {
-    if (h_img_) CHECK_CUDA_ERROR(cudaFreeHost(h_img_));
-    if (d_img_) CHECK_CUDA_ERROR(cudaFree(d_img_));
+  try {
+    if (m_cuda) {
+      if (h_img_) CHECK_CUDA_ERROR(cudaFreeHost(h_img_));
+      if (d_img_) CHECK_CUDA_ERROR(cudaFree(d_img_));
+    }
+  } catch (const std::exception & e) {
+    std::cerr << "Exception in TrtClassifier destructor: " << e.what() << std::endl;
+  } catch (...) {
+    std::cerr << "Unknown exception in TrtClassifier destructor" << std::endl;
   }
 }
 

perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp

@@ -341,16 +341,22 @@ TrtYoloX::TrtYoloX(
 
 TrtYoloX::~TrtYoloX()
 {
-  if (use_gpu_preprocess_) {
-    if (image_buf_h_) {
-      image_buf_h_.reset();
-    }
-    if (image_buf_d_) {
-      image_buf_d_.reset();
-    }
-    if (argmax_buf_d_) {
-      argmax_buf_d_.reset();
+  try {
+    if (use_gpu_preprocess_) {
+      if (image_buf_h_) {
+        image_buf_h_.reset();
+      }
+      if (image_buf_d_) {
+        image_buf_d_.reset();
+      }
+      if (argmax_buf_d_) {
+        argmax_buf_d_.reset();
+      }
     }
+  } catch (const std::exception & e) {
+    std::cerr << "Exception in TrtYoloX destructor: " << e.what() << std::endl;
+  } catch (...) {
+    std::cerr << "Unknown exception in TrtYoloX destructor" << std::endl;
   }
 }
 

perception/autoware_traffic_light_arbiter/README.md

@@ -43,7 +43,8 @@ The table below outlines how the matching process determines the output based on
 
 | Name                        | Type   | Default Value | Description                                                                                                                                                                    |
 | --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| `external_time_tolerance`   | double | 5.0           | The duration in seconds an external message is considered valid for merging                                                                                                    |
-| `perception_time_tolerance` | double | 1.0           | The duration in seconds a perception message is considered valid for merging                                                                                                   |
+| `external_delay_tolerance`  | double | 5.0           | The duration in seconds an external message is considered valid for merging in comparison with current time                                                                    |
+| `external_time_tolerance`   | double | 5.0           | The duration in seconds an external message is considered valid for merging in comparison with a timestamp of perception message                                               |
+| `perception_time_tolerance` | double | 1.0           | The duration in seconds a perception message is considered valid for merging in comparison with a timestamp of external message                                                |
 | `external_priority`         | bool   | false         | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria                                               |
 | `enable_signal_matching`    | bool   | false         | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical |

perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml

@@ -1,5 +1,6 @@
 /**:
   ros__parameters:
+    external_delay_tolerance: 5.0
     external_time_tolerance: 5.0
     perception_time_tolerance: 1.0
     external_priority: false

perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp

@@ -51,6 +51,7 @@ class TrafficLightArbiter : public rclcpp::Node
 
   std::unique_ptr<std::unordered_set<lanelet::Id>> map_regulatory_elements_set_;
 
+  double external_delay_tolerance_;
   double external_time_tolerance_;
   double perception_time_tolerance_;
   bool external_priority_;

perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp

@@ -20,6 +20,7 @@
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
+#include <algorithm>
 #include <map>
 #include <memory>
 #include <tuple>
@@ -70,6 +71,7 @@ namespace autoware
 TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options)
 : Node("traffic_light_arbiter", options)
 {
+  external_delay_tolerance_ = this->declare_parameter<double>("external_delay_tolerance", 5.0);
   external_time_tolerance_ = this->declare_parameter<double>("external_time_tolerance", 5.0);
   perception_time_tolerance_ = this->declare_parameter<double>("perception_time_tolerance", 1.0);
   external_priority_ = this->declare_parameter<bool>("external_priority", false);
@@ -119,7 +121,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP
   latest_perception_msg_ = *msg;
 
   if (
-    (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds() >
+    std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds()) >
     external_time_tolerance_) {
     latest_external_msg_.traffic_light_groups.clear();
   }
@@ -129,10 +131,16 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP
 
 void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg)
 {
+  if (std::abs((this->now() - rclcpp::Time(msg->stamp)).seconds()) > external_delay_tolerance_) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Received outdated V2X traffic signal messages");
+    return;
+  }
+
   latest_external_msg_ = *msg;
 
   if (
-    (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() >
+    std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds()) >
     perception_time_tolerance_) {
     latest_perception_msg_.traffic_light_groups.clear();
   }
@@ -229,6 +237,13 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim
   }
 
   pub_->publish(output_signals_msg);
+
+  const auto latest_time =
+    std::max(rclcpp::Time(latest_perception_msg_.stamp), rclcpp::Time(latest_external_msg_.stamp));
+  if (rclcpp::Time(output_signals_msg.stamp) < latest_time) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Published traffic signal messages are not latest");
+  }
 }
 }  // namespace autoware
 

perception/autoware_traffic_light_arbiter/test/test_node.cpp

@@ -196,6 +196,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyPerceptionMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  perception_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(
@@ -229,6 +232,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyExternalMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  external_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(
@@ -268,6 +274,10 @@ TEST(TrafficLightArbiterTest, testTrafficSignalBothMsg)
   };
   test_manager->set_subscriber<TrafficSignalArray>(output_topic, callback);
 
+  // stamp preparation
+  external_msg.stamp = test_target_node->now();
+  perception_msg.stamp = test_target_node->now();
+
   test_manager->test_pub_msg<LaneletMapBin>(
     test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local());
   test_manager->test_pub_msg<TrafficSignalArray>(

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp

@@ -35,7 +35,7 @@ using autoware::behavior_path_planner::lane_change::Debug;
 using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using visualization_msgs::msg::MarkerArray;
 MarkerArray showAllValidLaneChangePath(
-  const std::vector<LaneChangePath> & lanes, std::string && ns);
+  const std::vector<LaneChangePath> & lane_change_paths, std::string && ns);
 MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp

@@ -38,34 +38,74 @@ using autoware::universe_utils::createDefaultMarker;
 using autoware::universe_utils::createMarkerScale;
 using geometry_msgs::msg::Point;
 
-MarkerArray showAllValidLaneChangePath(const std::vector<LaneChangePath> & lanes, std::string && ns)
+MarkerArray showAllValidLaneChangePath(
+  const std::vector<LaneChangePath> & lane_change_paths, std::string && ns)
 {
-  if (lanes.empty()) {
+  if (lane_change_paths.empty()) {
     return MarkerArray{};
   }
 
   MarkerArray marker_array;
-  int32_t id{0};
   const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()};
 
   const auto colors = colors::colors_list();
-  const auto loop_size = std::min(lanes.size(), colors.size());
+  const auto loop_size = std::min(lane_change_paths.size(), colors.size());
   marker_array.markers.reserve(loop_size);
 
+  const auto info_prep_to_string =
+    [](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
+    std::ostringstream ss_text;
+    ss_text << std::setprecision(3) << "vel: " << info.velocity.prepare
+            << "[m/s] | lon_acc: " << info.longitudinal_acceleration.prepare
+            << "[m/s2] | t: " << info.duration.prepare << "[s] | L: " << info.length.prepare
+            << "[m]";
+    return ss_text.str();
+  };
+
+  const auto info_lc_to_string =
+    [](const autoware::behavior_path_planner::lane_change::Info & info) -> std::string {
+    std::ostringstream ss_text;
+    ss_text << std::setprecision(3) << "vel: " << info.velocity.lane_changing
+            << "[m/s] | lon_acc: " << info.longitudinal_acceleration.lane_changing
+            << "[m/s2] | lat_acc: " << info.lateral_acceleration
+            << "[m/s2] | t: " << info.duration.lane_changing
+            << "[s] | L: " << info.length.lane_changing << "[m]";
+    return ss_text.str();
+  };
+
   for (std::size_t idx = 0; idx < loop_size; ++idx) {
-    if (lanes.at(idx).path.points.empty()) {
+    int32_t id{0};
+    const auto & lc_path = lane_change_paths.at(idx);
+    if (lc_path.path.points.empty()) {
       continue;
     }
-
+    std::string ns_with_idx = ns + "[" + std::to_string(idx) + "]";
     const auto & color = colors.at(idx);
-    Marker marker = createDefaultMarker(
-      "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color);
-    marker.points.reserve(lanes.at(idx).path.points.size());
+    const auto & points = lc_path.path.points;
+    auto marker = createDefaultMarker(
+      "map", current_time, ns_with_idx, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0),
+      color);
+    marker.points.reserve(points.size());
 
-    for (const auto & point : lanes.at(idx).path.points) {
+    for (const auto & point : points) {
       marker.points.push_back(point.point.pose.position);
     }
 
+    auto text_marker = createDefaultMarker(
+      "map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.1, 0.1, 0.8), colors::yellow());
+    const auto prep_idx = points.size() / 4;
+    text_marker.pose = points.at(prep_idx).point.pose;
+    text_marker.pose.position.z += 2.0;
+    text_marker.text = info_prep_to_string(lc_path.info);
+    marker_array.markers.push_back(text_marker);
+
+    const auto lc_idx = points.size() / 2;
+    text_marker.id = ++id;
+    text_marker.pose = points.at(lc_idx).point.pose;
+    text_marker.text = info_lc_to_string(lc_path.info);
+    marker_array.markers.push_back(text_marker);
+
     marker_array.markers.push_back(marker);
   }
   return marker_array;

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -233,11 +233,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
 
   std::for_each(
     data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) {
-      if (!not_use_adjacent_lane) {
-        data.drivable_lanes.push_back(
-          utils::static_obstacle_avoidance::generateExpandedDrivableLanes(
-            lanelet, planner_data_, parameters_));
-      } else if (red_signal_lane_itr->id() != lanelet.id()) {
+      if (!not_use_adjacent_lane || red_signal_lane_itr->id() != lanelet.id()) {
         data.drivable_lanes.push_back(
           utils::static_obstacle_avoidance::generateExpandedDrivableLanes(
             lanelet, planner_data_, parameters_));

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp

@@ -264,9 +264,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
   AvoidOutlines outlines;
   for (auto & o : data.target_objects) {
     if (!o.avoid_margin.has_value()) {
-      if (!data.red_signal_lane.has_value()) {
-        o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;
-      } else if (data.red_signal_lane.value().id() == o.overhang_lanelet.id()) {
+      if (data.red_signal_lane.value().id() == o.overhang_lanelet.id()) {
         o.info = ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY;
       } else {
         o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;

sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md

@@ -55,16 +55,9 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml
   - `hesai`: (x: 90 degrees, y: 0 degrees)
   - `others`: (x: 0 degrees, y: 90 degrees) and (x: 270 degrees, y: 0 degrees)
 
-<table>
-  <tr>
-    <td><img src="./image/velodyne.drawio.png" alt="velodyne azimuth coordinate"></td>
-    <td><img src="./image/hesai.drawio.png" alt="hesai azimuth coordinate"></td>
-   </tr>
-   <tr>
-    <td><p style="text-align: center;">Velodyne azimuth coordinate</p></td>
-    <td><p style="text-align: center;">Hesai azimuth coordinate</p></td>
-  </tr>
-</table>
+| ![Velodyne Azimuth Coordinate](./image/velodyne.drawio.png) | ![Hesai Azimuth Coordinate](./image/hesai.drawio.png) |
+| :---------------------------------------------------------: | :---------------------------------------------------: |
+|               **Velodyne azimuth coordinate**               |             **Hesai azimuth coordinate**              |
 
 ## References/External links
 

system/autoware_default_adapi/src/utils/route_conversion.cpp

@@ -115,13 +115,13 @@ ExternalState convert_state(const InternalState & internal)
   const auto convert = [](InternalState::_state_type state) {
     switch(state) {
       // TODO(Takagi, Isamu): Add adapi state.
-      case InternalState::INITIALIZING: return ExternalState::UNSET;
+      case InternalState::INITIALIZING: return ExternalState::UNSET; // NOLINT
       case InternalState::UNSET:        return ExternalState::UNSET;
       case InternalState::ROUTING:      return ExternalState::UNSET;
       case InternalState::SET:          return ExternalState::SET;
       case InternalState::REROUTING:    return ExternalState::CHANGING;
       case InternalState::ARRIVED:      return ExternalState::ARRIVED;
-      case InternalState::ABORTED:      return ExternalState::SET;
+      case InternalState::ABORTED:      return ExternalState::SET;  // NOLINT
       case InternalState::INTERRUPTED:  return ExternalState::SET;
       default:                          return ExternalState::UNKNOWN;
     }

system/diagnostic_graph_aggregator/test/src/test2.cpp

@@ -29,7 +29,6 @@ using namespace diagnostic_graph_aggregator;  // NOLINT(build/namespaces)
 
 using diagnostic_msgs::msg::DiagnosticArray;
 using diagnostic_msgs::msg::DiagnosticStatus;
-using tier4_system_msgs::msg::DiagnosticGraph;
 
 constexpr auto OK = DiagnosticStatus::OK;
 constexpr auto WARN = DiagnosticStatus::WARN;