Merge branch 'main' into chore/object_recognition_utils_maintainer

Author: scepter914

.cppcheck_suppressions

@@ -2,13 +2,9 @@
 
 checkersReport
 constParameterReference
-constVariableReference
-// cspell: ignore cstyle
-cstyleCast
 funcArgNamesDifferent
 functionConst
 functionStatic
-invalidPointerCast
 knownConditionTrueFalse
 missingInclude
 missingIncludeSystem

.github/CODEOWNERS

@@ -70,6 +70,14 @@ jobs:
           restore-keys: |
             ccache-main-${{ runner.arch }}-${{ matrix.rosdistro }}-
 
+      # Limit ccache size only for CUDA builds to avoid running out of disk space
+      - name: Limit ccache size
+        if: ${{ matrix.container-suffix == '-cuda' }}
+        run: |
+          rm -f "${CCACHE_DIR}/ccache.conf"
+          echo -e "# Set maximum cache size\nmax_size = 1MB" >> "${CCACHE_DIR}/ccache.conf"
+        shell: bash
+
       - name: Show ccache stats before build
         run: du -sh ${CCACHE_DIR} && ccache -s
         shell: bash

.github/workflows/build-and-test-differential.yaml

@@ -58,7 +58,7 @@ jobs:
         run: |
           filtered_paths=""
           for dir in ${{ steps.get-full-paths.outputs.full-paths }}; do
-            if [ -d "$dir" ] && find "$dir" -name "*.cpp" -o -name "*.hpp" | grep -q .; then
+            if [ -d "$dir" ] && find "$dir" -name "*.cpp" | grep -q .; then
               filtered_paths="$filtered_paths $dir"
             fi
           done

.github/workflows/cppcheck-differential.yaml

@@ -991,34 +991,51 @@ calcCurvature<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
  * curvature calculation
  */
 template <class T>
-std::vector<std::pair<double, double>> calcCurvatureAndArcLength(const T & points)
+std::vector<std::pair<double, std::pair<double, double>>> calcCurvatureAndSegmentLength(
+  const T & points)
 {
-  // Note that arclength is for the segment, not the sum.
-  std::vector<std::pair<double, double>> curvature_arc_length_vec;
-  curvature_arc_length_vec.emplace_back(0.0, 0.0);
+  // segment length is pair of segment length between {p1, p2} and {p2, p3}
+  std::vector<std::pair<double, std::pair<double, double>>> curvature_and_segment_length_vec;
+  curvature_and_segment_length_vec.reserve(points.size());
+  curvature_and_segment_length_vec.emplace_back(0.0, std::make_pair(0.0, 0.0));
   for (size_t i = 1; i < points.size() - 1; ++i) {
     const auto p1 = autoware::universe_utils::getPoint(points.at(i - 1));
     const auto p2 = autoware::universe_utils::getPoint(points.at(i));
     const auto p3 = autoware::universe_utils::getPoint(points.at(i + 1));
     const double curvature = autoware::universe_utils::calcCurvature(p1, p2, p3);
-    const double arc_length =
-      autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)) +
-      autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1));
-    curvature_arc_length_vec.emplace_back(curvature, arc_length);
+
+    // The first point has only the next point, so put the distance to that point.
+    // In other words, assign the first segment length at the second point to the
+    // second_segment_length at the first point.
+    if (i == 1) {
+      curvature_and_segment_length_vec.at(0).second.second =
+        autoware::universe_utils::calcDistance2d(p1, p2);
+    }
+
+    // The second_segment_length of the previous point and the first segment length of the current
+    // point are equal.
+    const std::pair<double, double> arc_length{
+      curvature_and_segment_length_vec.back().second.second,
+      autoware::universe_utils::calcDistance2d(p2, p3)};
+
+    curvature_and_segment_length_vec.emplace_back(curvature, arc_length);
   }
-  curvature_arc_length_vec.emplace_back(0.0, 0.0);
 
-  return curvature_arc_length_vec;
+  // set to the last point
+  curvature_and_segment_length_vec.emplace_back(
+    0.0, std::make_pair(curvature_and_segment_length_vec.back().second.second, 0.0));
+
+  return curvature_and_segment_length_vec;
 }
 
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
   const std::vector<autoware_planning_msgs::msg::PathPoint> & points);
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
   const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> & points);
-extern template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
+extern template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 /**

common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp

@@ -238,14 +238,14 @@ calcCurvature<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 //
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::PathPoint>>(
   const std::vector<autoware_planning_msgs::msg::PathPoint> & points);
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<tier4_planning_msgs::msg::PathPointWithLaneId>>(
   const std::vector<tier4_planning_msgs::msg::PathPointWithLaneId> & points);
-template std::vector<std::pair<double, double>>
-calcCurvatureAndArcLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
+template std::vector<std::pair<double, std::pair<double, double>>>
+calcCurvatureAndSegmentLength<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>>(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & points);
 
 //

common/autoware_motion_utils/src/trajectory/trajectory.cpp

@@ -103,8 +103,7 @@ private Q_SLOTS:
     turn_signals_sub_;
   rclcpp::Subscription<autoware_vehicle_msgs::msg::HazardLightsReport>::SharedPtr
     hazard_lights_sub_;
-  rclcpp::Subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>::SharedPtr
-    traffic_sub_;
+  rclcpp::Subscription<autoware_perception_msgs::msg::TrafficLightGroup>::SharedPtr traffic_sub_;
   rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr speed_limit_sub_;
 
   std::mutex property_mutex_;
@@ -118,7 +117,7 @@ private Q_SLOTS:
     const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg);
   void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
   void updateTrafficLightData(
-    const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg);
+    const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg);
   void drawWidget(QImage & hud);
 };
 }  // namespace autoware_overlay_rviz_plugin

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp

@@ -25,7 +25,6 @@
 
 #include <autoware_perception_msgs/msg/traffic_light_element.hpp>
 #include <autoware_perception_msgs/msg/traffic_light_group.hpp>
-#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
 
 #include <OgreColourValue.h>
 #include <OgreMaterial.h>
@@ -40,8 +39,8 @@ class TrafficDisplay
   TrafficDisplay();
   void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect);
   void updateTrafficLightData(
-    const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg);
-  autoware_perception_msgs::msg::TrafficLightGroupArray current_traffic_;
+    const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg);
+  autoware_perception_msgs::msg::TrafficLightGroup current_traffic_;
 
 private:
   QImage traffic_light_image_;

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp

@@ -112,9 +112,10 @@ void SignalDisplay::onInitialize()
   speed_limit_topic_property_->initialize(rviz_ros_node);
 
   traffic_topic_property_ = std::make_unique<rviz_common::properties::RosTopicProperty>(
-    "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals",
-    "autoware_perception_msgs/msgs/msg/TrafficLightGroupArray", "Topic for Traffic Light Data",
-    this, SLOT(topic_updated_traffic()));
+    "Traffic Topic",
+    "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal",
+    "autoware_perception_msgs/msgs/msg/TrafficLightGroup", "Topic for Traffic Light Data", this,
+    SLOT(topic_updated_traffic()));
   traffic_topic_property_->initialize(rviz_ros_node);
 }
 
@@ -192,7 +193,7 @@ void SignalDisplay::onDisable()
 }
 
 void SignalDisplay::updateTrafficLightData(
-  const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg)
+  const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(property_mutex_);
 
@@ -458,14 +459,13 @@ void SignalDisplay::topic_updated_traffic()
   // resubscribe to the topic
   traffic_sub_.reset();
   auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
-  traffic_sub_ =
-    rviz_ros_node->get_raw_node()
-      ->create_subscription<autoware_perception_msgs::msg::TrafficLightGroupArray>(
-        traffic_topic_property_->getTopicStd(),
-        rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-        [this](const autoware_perception_msgs::msg::TrafficLightGroupArray::SharedPtr msg) {
-          updateTrafficLightData(msg);
-        });
+  traffic_sub_ = rviz_ros_node->get_raw_node()
+                   ->create_subscription<autoware_perception_msgs::msg::TrafficLightGroup>(
+                     traffic_topic_property_->getTopicStd(),
+                     rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+                     [this](const autoware_perception_msgs::msg::TrafficLightGroup::SharedPtr msg) {
+                       updateTrafficLightData(msg);
+                     });
 }
 
 }  // namespace autoware_overlay_rviz_plugin

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp

@@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay()
 }
 
 void TrafficDisplay::updateTrafficLightData(
-  const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg)
+  const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg)
 {
   current_traffic_ = *msg;
 }
@@ -68,8 +68,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
     backgroundRect.top() + circleRect.height() / 2));
   painter.drawEllipse(circleRect);
 
-  if (!current_traffic_.traffic_light_groups.empty()) {
-    switch (current_traffic_.traffic_light_groups[0].elements[0].color) {
+  if (!current_traffic_.elements.empty()) {
+    switch (current_traffic_.elements[0].color) {
       case 1:
         painter.setBrush(QBrush(tl_red_));
         painter.drawEllipse(circleRect);

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp

@@ -54,6 +54,23 @@ enum ReturnType : uint8_t {
   DUAL_ONLY,
 };
 
+struct PointXYZIRC
+{
+  float x{0.0F};
+  float y{0.0F};
+  float z{0.0F};
+  std::uint8_t intensity{0U};
+  std::uint8_t return_type{0U};
+  std::uint16_t channel{0U};
+
+  friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept
+  {
+    return float_eq<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
+           float_eq<float>(p1.z, p2.z) && p1.intensity == p2.intensity &&
+           p1.return_type == p2.return_type && p1.channel == p2.channel;
+  }
+};
+
 struct PointXYZIRADRT
 {
   float x{0.0F};
@@ -75,25 +92,97 @@ struct PointXYZIRADRT
   }
 };
 
-enum class PointIndex { X, Y, Z, Intensity, Ring, Azimuth, Distance, ReturnType, TimeStamp };
+struct PointXYZIRCAEDT
+{
+  float x{0.0F};
+  float y{0.0F};
+  float z{0.0F};
+  std::uint8_t intensity{0U};
+  std::uint8_t return_type{0U};
+  std::uint16_t channel{0U};
+  float azimuth{0.0F};
+  float elevation{0.0F};
+  float distance{0.0F};
+  std::uint32_t time_stamp{0U};
+
+  friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept
+  {
+    return float_eq<float>(p1.x, p2.x) && float_eq<float>(p1.y, p2.y) &&
+           float_eq<float>(p1.z, p2.z) && p1.intensity == p2.intensity &&
+           p1.return_type == p2.return_type && p1.channel == p2.channel &&
+           float_eq<float>(p1.azimuth, p2.azimuth) && float_eq<float>(p1.distance, p2.distance) &&
+           p1.time_stamp == p2.time_stamp;
+  }
+};
+
+enum class PointXYZIIndex { X, Y, Z, Intensity };
+enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel };
+enum class PointXYZIRADRTIndex {
+  X,
+  Y,
+  Z,
+  Intensity,
+  Ring,
+  Azimuth,
+  Distance,
+  ReturnType,
+  TimeStamp
+};
+enum class PointXYZIRCAEDTIndex {
+  X,
+  Y,
+  Z,
+  Intensity,
+  ReturnType,
+  Channel,
+  Azimuth,
+  Elevation,
+  Distance,
+  TimeStamp
+};
 
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth);
+LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type);
 LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp);
 
+LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel);
+
+using PointXYZIRCGenerator = std::tuple<
+  point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
+  point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
+  field_return_type_generator, field_channel_generator>;
+
 using PointXYZIRADRTGenerator = std::tuple<
   point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
   point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
   point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator,
   field_return_type_generator, field_time_stamp_generator>;
 
+using PointXYZIRCAEDTGenerator = std::tuple<
+  point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator,
+  point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator,
+  field_return_type_generator, field_channel_generator, field_azimuth_generator,
+  field_elevation_generator, field_distance_generator, field_time_stamp_generator>;
+
 }  // namespace autoware_point_types
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  autoware_point_types::PointXYZIRC,
+  (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
+    std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel))
+
 POINT_CLOUD_REGISTER_POINT_STRUCT(
   autoware_point_types::PointXYZIRADRT,
   (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(
     float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)(
     double, time_stamp, time_stamp))
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  autoware_point_types::PointXYZIRCAEDT,
+  (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)(
+    std::uint8_t, return_type,
+    return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)(
+    float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp))
 #endif  // AUTOWARE_POINT_TYPES__TYPES_HPP_

common/autoware_point_types/include/autoware_point_types/types.hpp

@@ -36,6 +36,15 @@ TEST(PointEquality, PointXYZIRADRT)
   EXPECT_EQ(pt0, pt1);
 }
 
+TEST(PointEquality, PointXYZIRCAEDT)
+{
+  using autoware_point_types::PointXYZIRCAEDT;
+
+  PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
+  PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
+  EXPECT_EQ(pt0, pt1);
+}
+
 TEST(PointEquality, FloatEq)
 {
   // test template

common/autoware_point_types/test/test_point_types.cpp

@@ -14,6 +14,8 @@ ament_auto_add_library(autoware_universe_utils SHARED
   src/geometry/geometry.cpp
   src/geometry/pose_deviation.cpp
   src/geometry/boost_polygon_utils.cpp
+  src/geometry/random_convex_polygon.cpp
+  src/geometry/gjk_2d.cpp
   src/math/sin_table.cpp
   src/math/trigonometry.cpp
   src/ros/msg_operation.cpp