Merge branch 'main' into chore/test/compare_map_filter

autowarefoundation · Sep 24, 2024 · a6a6aac · a6a6aac
2 parents 9ddcf8a + 81e7aba
commit a6a6aac
Show file tree

Hide file tree

Showing 395 changed files with 3,619 additions and 5,505 deletions.
diff --git a/.cspell.json b/.cspell.json
@@ -4,5 +4,5 @@
     "planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/**"
   ],
   "ignoreRegExpList": [],
-  "words": ["dltype", "tvmgen", "fromarray"]
+  "words": ["dltype", "tvmgen", "fromarray", "soblin"]
 }
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -10,9 +10,11 @@ common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodr
 common/autoware_path_distance_calculator/** [email protected]
 common/autoware_perception_rviz_plugin/** [email protected] [email protected] [email protected] [email protected] [email protected]
 common/autoware_point_types/** [email protected] [email protected]
+common/autoware_signal_processing/** [email protected] [email protected] [email protected] [email protected] [email protected]
 common/autoware_test_utils/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_testing/** [email protected] [email protected] [email protected] [email protected]
 common/autoware_universe_utils/** [email protected] [email protected] [email protected]
+common/autoware_vehicle_info_utils/** [email protected] [email protected] [email protected]
 common/bag_time_manager_rviz_plugin/** [email protected]
 common/component_interface_specs/** [email protected] [email protected]
 common/component_interface_tools/** [email protected]
@@ -26,7 +28,6 @@ common/object_recognition_utils/** [email protected] takayuki.murooka@tier
 common/osqp_interface/** [email protected] [email protected] [email protected] [email protected]
 common/polar_grid/** [email protected]
 common/qp_interface/** [email protected] [email protected] [email protected] [email protected]
-common/signal_processing/** [email protected] [email protected] [email protected] [email protected] [email protected]
 common/tensorrt_common/** [email protected] [email protected]
 common/tier4_adapi_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_api_utils/** [email protected]
@@ -76,6 +77,7 @@ launch/tier4_sensing_launch/** [email protected]
 launch/tier4_simulator_launch/** [email protected] [email protected] [email protected] [email protected] [email protected]
 launch/tier4_system_launch/** [email protected] [email protected]
 launch/tier4_vehicle_launch/** [email protected]
+localization/autoware_ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_geo_pose_projector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_gyro_odometer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -89,9 +91,8 @@ localization/autoware_pose_initializer/** [email protected] isamu.takagi@tie
 localization/autoware_pose_instability_detector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_stop_filter/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_twist2accel/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-localization/ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-localization/localization_util/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-localization/ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+localization/autoware_localization_util/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+localization/autoware_ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_common/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_image_processing/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_monitor/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -101,7 +102,6 @@ map/autoware_map_height_fitter/** [email protected] [email protected] ke
 map/autoware_map_projection_loader/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 map/autoware_map_tf_generator/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 map/map_loader/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-map/util/lanelet2_map_preprocessor/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 perception/autoware_bytetrack/** [email protected] [email protected]
 perception/autoware_cluster_merger/** [email protected] [email protected] [email protected]
 perception/autoware_compare_map_segmentation/** [email protected] [email protected] [email protected]
@@ -231,13 +231,11 @@ system/diagnostic_graph_utils/** [email protected]
 system/dummy_diag_publisher/** [email protected] [email protected]
 system/dummy_infrastructure/** [email protected]
 system/duplicated_node_checker/** [email protected] [email protected] [email protected]
-system/emergency_handler/** [email protected] [email protected] [email protected]
 system/hazard_status_converter/** [email protected]
 system/mrm_comfortable_stop_operator/** [email protected] [email protected]
 system/mrm_emergency_stop_operator/** [email protected] [email protected]
 system/mrm_handler/** [email protected] [email protected] [email protected]
 system/system_diagnostic_monitor/** [email protected]
-system/system_error_monitor/** [email protected]
 system/system_monitor/** [email protected] [email protected]
 system/topic_state_monitor/** [email protected]
 system/velodyne_monitor/** [email protected]
@@ -246,6 +244,5 @@ vehicle/autoware_accel_brake_map_calibrator/** [email protected] taiki.tana
 vehicle/autoware_external_cmd_converter/** [email protected] [email protected]
 vehicle/autoware_raw_vehicle_cmd_converter/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 vehicle/autoware_steer_offset_estimator/** [email protected]
-vehicle/autoware_vehicle_info_utils/** [email protected] [email protected] [email protected]
 
 ### Copied from .github/CODEOWNERS-manual ###
diff --git a/.github/actions/build-and-test-differential/action.yaml b/.github/actions/build-and-test-differential/action.yaml
@@ -57,8 +57,10 @@ runs:
         restore-keys: |
           ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-
 
-    - name: Show ccache stats before build
-      run: du -sh ${CCACHE_DIR} && ccache -s
+    - name: Show ccache stats before build and reset stats
+      run: |
+        du -sh ${CCACHE_DIR} && ccache -s
+        ccache --zero-stats
       shell: bash
 
     - name: Export CUDA state as a variable for adding to cache key

diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml
@@ -69,8 +69,10 @@ jobs:
           echo -e "# Set maximum cache size\nmax_size = 600MB" >> "${CCACHE_DIR}/ccache.conf"
         shell: bash
 
-      - name: Show ccache stats before build
-        run: du -sh ${CCACHE_DIR} && ccache -s
+      - name: Show ccache stats before build and reset stats
+        run: |
+          du -sh ${CCACHE_DIR} && ccache -s
+          ccache --zero-stats
         shell: bash
 
       - name: Export CUDA state as a variable for adding to cache key

diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml
@@ -71,7 +71,7 @@ jobs:
         id: cppcheck
         run: |
           echo "Running Cppcheck on modified packages: ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }}"
-          cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions --inline-suppr ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }} 2> cppcheck-report.txt
+          cppcheck --enable=all --inconclusive --check-level=exhaustive -D'PLUGINLIB_EXPORT_CLASS(class_type, base_class)=' -Dslots= -DQ_SLOTS= --error-exitcode=1 --suppressions-list=.cppcheck_suppressions --inline-suppr ${{ steps.filter-paths-no-cpp-files.outputs.filtered-full-paths }} 2> cppcheck-report.txt
         shell: bash
 
       - name: Setup Problem Matchers for cppcheck

diff --git a/.github/workflows/cppcheck-weekly.yaml b/.github/workflows/cppcheck-weekly.yaml
@@ -23,7 +23,7 @@ jobs:
         continue-on-error: true
         id: cppcheck
         run: |
-          cppcheck --enable=all --inconclusive --check-level=exhaustive --suppress=*:*/test/* --error-exitcode=1 --xml --inline-suppr . 2> cppcheck-report.xml
+          cppcheck --enable=all --inconclusive --check-level=exhaustive -D'PLUGINLIB_EXPORT_CLASS(class_type, base_class)=' -Dslots= -DQ_SLOTS= --suppress=*:*/test/* --error-exitcode=1 --xml --inline-suppr . 2> cppcheck-report.xml
         shell: bash
 
       - name: Count errors by error ID and severity

diff --git a/common/interpolation/CMakeLists.txt → common/autoware_interpolation/CMakeLists.txt b/common/interpolation/CMakeLists.txt → common/autoware_interpolation/CMakeLists.txt
@@ -1,10 +1,10 @@
 cmake_minimum_required(VERSION 3.14)
-project(interpolation)
+project(autoware_interpolation)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(interpolation SHARED
+ament_auto_add_library(autoware_interpolation SHARED
   src/linear_interpolation.cpp
   src/spline_interpolation.cpp
   src/spline_interpolation_points_2d.cpp
@@ -17,7 +17,7 @@ if(BUILD_TESTING)
   ament_add_ros_isolated_gtest(test_interpolation ${test_files})
 
   target_link_libraries(test_interpolation
-    interpolation
+    autoware_interpolation
   )
 endif()
 

diff --git a/common/interpolation/README.md → common/autoware_interpolation/README.md b/common/interpolation/README.md → common/autoware_interpolation/README.md
diff --git a/...ude/interpolation/interpolation_utils.hpp → ...are/interpolation/interpolation_utils.hpp b/...ude/interpolation/interpolation_utils.hpp → ...are/interpolation/interpolation_utils.hpp
@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_
-#define INTERPOLATION__INTERPOLATION_UTILS_HPP_
+#ifndef AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
+#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
 
 #include <algorithm>
 #include <array>
 #include <stdexcept>
 #include <vector>
 
-namespace interpolation_utils
+namespace autoware::interpolation
 {
 inline bool isIncreasing(const std::vector<double> & x)
 {
@@ -109,6 +109,6 @@ void validateKeysAndValues(
     throw std::invalid_argument("The size of base_keys and base_values are not the same.");
   }
 }
-}  // namespace interpolation_utils
+}  // namespace autoware::interpolation
 
-#endif  // INTERPOLATION__INTERPOLATION_UTILS_HPP_
+#endif  // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
diff --git a/...de/interpolation/linear_interpolation.hpp → ...re/interpolation/linear_interpolation.hpp b/...de/interpolation/linear_interpolation.hpp → ...re/interpolation/linear_interpolation.hpp
@@ -12,14 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef INTERPOLATION__LINEAR_INTERPOLATION_HPP_
-#define INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+#ifndef AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+#define AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
 
-#include "interpolation/interpolation_utils.hpp"
+#include "autoware/interpolation/interpolation_utils.hpp"
 
 #include <vector>
 
-namespace interpolation
+namespace autoware::interpolation
 {
 double lerp(const double src_val, const double dst_val, const double ratio);
 
@@ -30,7 +30,6 @@ std::vector<double> lerp(
 double lerp(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const double query_key);
+}  // namespace autoware::interpolation
 
-}  // namespace interpolation
-
-#endif  // INTERPOLATION__LINEAR_INTERPOLATION_HPP_
+#endif  // AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
diff --git a/...lation/spherical_linear_interpolation.hpp → ...lation/spherical_linear_interpolation.hpp b/...lation/spherical_linear_interpolation.hpp → ...lation/spherical_linear_interpolation.hpp
@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
-#define INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+#ifndef AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+#define AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
 
-#include "interpolation/interpolation_utils.hpp"
+#include "autoware/interpolation/interpolation_utils.hpp"
 
 #include <geometry_msgs/msg/quaternion.hpp>
 
@@ -29,7 +29,7 @@
 
 #include <vector>
 
-namespace interpolation
+namespace autoware::interpolation
 {
 geometry_msgs::msg::Quaternion slerp(
   const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat,
@@ -43,6 +43,6 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
 geometry_msgs::msg::Quaternion lerpOrientation(
   const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
   const double ratio);
-}  // namespace interpolation
+}  // namespace autoware::interpolation
 
-#endif  // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
+#endif  // AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
diff --git a/...de/interpolation/spline_interpolation.hpp → ...re/interpolation/spline_interpolation.hpp b/...de/interpolation/spline_interpolation.hpp → ...re/interpolation/spline_interpolation.hpp
@@ -12,47 +12,29 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_
-#define INTERPOLATION__SPLINE_INTERPOLATION_HPP_
+#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_
+#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_
 
+#include "autoware/interpolation/interpolation_utils.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
-#include "interpolation/interpolation_utils.hpp"
+
+#include <Eigen/Core>
 
 #include <algorithm>
 #include <cmath>
 #include <iostream>
 #include <numeric>
 #include <vector>
 
-namespace interpolation
-{
-// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1)
-struct MultiSplineCoef
+namespace autoware::interpolation
 {
-  MultiSplineCoef() = default;
-
-  explicit MultiSplineCoef(const size_t num_spline)
-  {
-    a.resize(num_spline);
-    b.resize(num_spline);
-    c.resize(num_spline);
-    d.resize(num_spline);
-  }
-
-  std::vector<double> a;
-  std::vector<double> b;
-  std::vector<double> c;
-  std::vector<double> d;
-};
-
 // static spline interpolation functions
 std::vector<double> spline(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys);
 std::vector<double> splineByAkima(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys);
-}  // namespace interpolation
 
 // non-static 1-dimensional spline interpolation
 //
@@ -98,11 +80,18 @@ class SplineInterpolation
   size_t getSize() const { return base_keys_.size(); }
 
 private:
+  Eigen::VectorXd a_;
+  Eigen::VectorXd b_;
+  Eigen::VectorXd c_;
+  Eigen::VectorXd d_;
+
   std::vector<double> base_keys_;
-  interpolation::MultiSplineCoef multi_spline_coef_;
 
   void calcSplineCoefficients(
     const std::vector<double> & base_keys, const std::vector<double> & base_values);
+
+  Eigen::Index get_index(const double & key) const;
 };
+}  // namespace autoware::interpolation
 
-#endif  // INTERPOLATION__SPLINE_INTERPOLATION_HPP_
+#endif  // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_
diff --git a/...lation/spline_interpolation_points_2d.hpp → ...lation/spline_interpolation_points_2d.hpp b/...lation/spline_interpolation_points_2d.hpp → ...lation/spline_interpolation_points_2d.hpp
@@ -12,19 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
-#define INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
+#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
+#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
 
-#include "interpolation/spline_interpolation.hpp"
+#include "autoware/interpolation/spline_interpolation.hpp"
 
 #include <vector>
 
-namespace interpolation
+namespace autoware::interpolation
 {
 
 template <typename T>
 std::vector<double> splineYawFromPoints(const std::vector<T> & points);
-}  // namespace interpolation
 
 // non-static points spline interpolation
 // NOTE: We can calculate yaw from the x and y by interpolation derivatives.
@@ -85,5 +84,6 @@ class SplineInterpolationPoints2d
 
   std::vector<double> base_s_vec_;
 };
+}  // namespace autoware::interpolation
 
-#endif  // INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
+#endif  // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
diff --git a/...include/interpolation/zero_order_hold.hpp → ...utoware/interpolation/zero_order_hold.hpp b/...include/interpolation/zero_order_hold.hpp → ...utoware/interpolation/zero_order_hold.hpp
@@ -12,21 +12,21 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef INTERPOLATION__ZERO_ORDER_HOLD_HPP_
-#define INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+#ifndef AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+#define AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_
 
-#include "interpolation/interpolation_utils.hpp"
+#include "autoware/interpolation/interpolation_utils.hpp"
 
 #include <vector>
 
-namespace interpolation
+namespace autoware::interpolation
 {
 inline std::vector<size_t> calc_closest_segment_indices(
   const std::vector<double> & base_keys, const std::vector<double> & query_keys,
   const double overlap_threshold = 1e-3)
 {
   // throw exception for invalid arguments
-  const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys);
+  const auto validated_query_keys = validateKeys(base_keys, query_keys);
 
   std::vector<size_t> closest_segment_indices(validated_query_keys.size());
   size_t closest_segment_idx = 0;
@@ -58,7 +58,7 @@ std::vector<T> zero_order_hold(
   const std::vector<size_t> & closest_segment_indices)
 {
   // throw exception for invalid arguments
-  interpolation_utils::validateKeysAndValues(base_keys, base_values);
+  validateKeysAndValues(base_keys, base_values);
 
   std::vector<T> query_values(closest_segment_indices.size());
   for (size_t i = 0; i < closest_segment_indices.size(); ++i) {
@@ -76,6 +76,6 @@ std::vector<T> zero_order_hold(
   return zero_order_hold(
     base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold));
 }
-}  // namespace interpolation
+}  // namespace autoware::interpolation
 
-#endif  // INTERPOLATION__ZERO_ORDER_HOLD_HPP_
+#endif  // AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_