Merge branch 'main' into test/image_projection_based_fusion/add_calc_…

…iou_tests

.github/CODEOWNERS

@@ -21,35 +21,25 @@ common/goal_distance_calculator/** [email protected]
 common/grid_map_utils/** [email protected]
 common/interpolation/** [email protected] [email protected]
 common/kalman_filter/** [email protected] [email protected] [email protected]
-common/mission_planner_rviz_plugin/** [email protected]
 common/motion_utils/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 common/object_recognition_utils/** [email protected] [email protected] [email protected] [email protected]
 common/osqp_interface/** [email protected] [email protected] [email protected] [email protected]
 common/path_distance_calculator/** [email protected]
 common/perception_utils/** [email protected] [email protected] [email protected]
 common/polar_grid/** [email protected]
 common/qp_interface/** [email protected] [email protected] [email protected] [email protected]
-common/rtc_manager_rviz_plugin/** [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]
-common/tier4_automatic_goal_rviz_plugin/** [email protected] [email protected] [email protected] [email protected]
 common/tier4_autoware_utils/** [email protected] [email protected] [email protected]
-common/tier4_calibration_rviz_plugin/** [email protected]
 common/tier4_camera_view_rviz_plugin/** [email protected] [email protected]
-common/tier4_control_rviz_plugin/** [email protected]
 common/tier4_datetime_rviz_plugin/** [email protected]
-common/tier4_debug_rviz_plugin/** [email protected]
 common/tier4_localization_rviz_plugin/** [email protected] [email protected]
-common/tier4_logging_level_configure_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_perception_rviz_plugin/** [email protected]
 common/tier4_planning_rviz_plugin/** [email protected] [email protected]
-common/tier4_screen_capture_rviz_plugin/** [email protected] [email protected] [email protected]
-common/tier4_simulated_clock_rviz_plugin/** [email protected]
-common/tier4_state_rviz_plugin/** [email protected] [email protected]
+common/tier4_state_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_system_rviz_plugin/** [email protected]
-common/tier4_target_object_type_rviz_plugin/** [email protected]
 common/tier4_traffic_light_rviz_plugin/** [email protected]
 common/tier4_vehicle_rviz_plugin/** [email protected]
 common/time_utils/** [email protected] [email protected] [email protected]
@@ -156,13 +146,14 @@ perception/traffic_light_map_based_detector/** [email protected] tao.zhong
 perception/traffic_light_multi_camera_fusion/** [email protected] [email protected]
 perception/traffic_light_occlusion_predictor/** [email protected] [email protected]
 perception/traffic_light_visualization/** [email protected] [email protected]
+planning/autoware_behavior_path_external_request_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+planning/autoware_behavior_velocity_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/autoware_planning_test_manager/** [email protected] [email protected]
 planning/autoware_remaining_distance_time_calculator/** [email protected]
 planning/autoware_static_centerline_generator/** [email protected] [email protected]
 planning/behavior_path_avoidance_by_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_avoidance_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_dynamic_avoidance_module/** [email protected] [email protected] [email protected] [email protected]
-planning/behavior_path_external_request_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_goal_planner_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -179,7 +170,6 @@ planning/behavior_velocity_no_drivable_lane_module/** [email protected]
 planning/behavior_velocity_no_stopping_area_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_occlusion_spot_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_out_of_lane_module/** [email protected] [email protected] [email protected] [email protected]
-planning/behavior_velocity_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_planner_common/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_run_out_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_speed_bump_module/** [email protected] [email protected] [email protected]

common/.pages

@@ -0,0 +1,62 @@
+nav:
+  - 'Introduction': common
+  - 'Testing Libraries':
+    - 'autoware_testing': common/autoware_testing/design/autoware_testing-design
+    - 'fake_test_node': common/fake_test_node/design/fake_test_node-design
+  - 'Common Libraries':
+    - 'autoware_auto_common':
+      - 'comparisons': common/autoware_auto_common/design/comparisons
+    - 'autoware_auto_geometry':
+      - 'interval': common/autoware_auto_geometry/design/interval
+      - 'polygon intersection 2d': common/autoware_auto_geometry/design/polygon_intersection_2d-design
+      - 'spatial hash': common/autoware_auto_geometry/design/spatial-hash-design
+    - 'autoware_auto_tf2': common/autoware_auto_tf2/design/autoware-auto-tf2-design
+    - 'autoware_point_types': common/autoware_point_types
+    - 'Cuda Utils': common/cuda_utils
+    - 'Geography Utils': common/geography_utils
+    - 'Global Parameter Loader': common/global_parameter_loader/Readme
+    - 'Glog Component': common/glog_component
+    - 'grid_map_utils': common/grid_map_utils
+    - 'interpolation': common/interpolation
+    - 'Kalman Filter': common/kalman_filter
+    - 'Motion Utils': common/motion_utils
+    - 'Vehicle Utils': common/motion_utils/docs/vehicle/vehicle
+    - 'Object Recognition Utils': common/object_recognition_utils
+    - 'OSQP Interface': common/osqp_interface/design/osqp_interface-design
+    - 'Perception Utils': common/perception_utils
+    - 'QP Interface': common/qp_interface/design/qp_interface-design
+    - 'Signal Processing':
+      - 'Introduction': common/signal_processing
+      - 'Butterworth Filter': common/signal_processing/documentation/ButterworthFilter
+    - 'TensorRT Common': common/tensorrt_common
+    - 'tier4_autoware_utils': common/tier4_autoware_utils
+    - 'traffic_light_utils': common/traffic_light_utils
+    - 'TVM Utility':
+      - 'Introduction': common/tvm_utility
+      - 'YOLOv2 Tiny Example': common/tvm_utility/tvm-utility-yolo-v2-tiny-tests
+  - 'RVIZ2 Plugins':
+    - 'autoware_auto_perception_rviz_plugin': common/autoware_auto_perception_rviz_plugin
+    - 'autoware_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin
+    - 'autoware_mission_details_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin
+    - 'bag_time_manager_rviz_plugin': common/bag_time_manager_rviz_plugin
+    - 'polar_grid': common/polar_grid/Readme
+    - 'tier4_adapi_rviz_plugin': common/tier4_adapi_rviz_plugin
+    - 'tier4_api_utils': common/tier4_api_utils
+    - 'tier4_camera_view_rviz_plugin': common/tier4_camera_view_rviz_plugin
+    - 'tier4_datetime_rviz_plugin': common/tier4_datetime_rviz_plugin
+    - 'tier4_localization_rviz_plugin': common/tier4_localization_rviz_plugin
+    - 'tier4_perception_rviz_plugin': common/tier4_perception_rviz_plugin
+    - 'tier4_planning_rviz_plugin': common/tier4_planning_rviz_plugin
+    - 'tier4_state_rviz_plugin': common/tier4_state_rviz_plugin
+    - 'tier4_system_rviz_plugin': common/tier4_system_rviz_plugin
+    - 'tier4_traffic_light_rviz_plugin': common/tier4_traffic_light_rviz_plugin
+    - 'tier4_vehicle_rviz_plugin': common/tier4_vehicle_rviz_plugin
+    - 'traffic_light_recognition_marker_publisher': common/traffic_light_recognition_marker_publisher/Readme
+  - 'Node':
+    - 'Goal Distance Calculator': common/goal_distance_calculator/Readme
+    - 'Path Distance Calculator': common/path_distance_calculator/Readme
+  - 'Others':
+    - 'autoware_ad_api_specs': common/autoware_ad_api_specs
+    - 'component_interface_specs': common/component_interface_specs
+    - 'component_interface_tools': common/component_interface_tools
+    - 'component_interface_utils': common/component_interface_utils

common/README.md

@@ -0,0 +1,16 @@
+# Common
+
+## Getting Started
+
+The Autoware.Universe Common folder consists of common and testing libraries that are used by other Autoware components, as well as useful plugins for visualization in RVIZ2.
+
+!!! note
+
+    In addition to the ones listed in this folder, users can also have a look at some of the add-ons in the `autoware_tools/common` documentation [page](https://autowarefoundation.github.io/autoware_tools/main/common/mission_planner_rviz_plugin/).
+
+## Highlights
+
+Some of the commonly used libraries are:
+
+1. `tier4_autoware_utils`
+2. `motion_utils`

common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp

@@ -0,0 +1,36 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
+#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/heartbeat.hpp>
+
+namespace autoware_ad_api::system
+{
+
+struct Heartbeat
+{
+  using Message = autoware_adapi_v1_msgs::msg::Heartbeat;
+  static constexpr char name[] = "/api/system/heartbeat";
+  static constexpr size_t depth = 10;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::system
+
+#endif  // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_

common/autoware_point_types/README.md

@@ -0,0 +1 @@
+# Autoware Point Types

common/component_interface_specs/include/component_interface_specs/localization.hpp

@@ -18,16 +18,16 @@
 #include <rclcpp/qos.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
-#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_localization_msgs/srv/initialize_localization.hpp>
 
 namespace localization_interface
 {
 
 struct Initialize
 {
-  using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization;
+  using Service = tier4_localization_msgs::srv::InitializeLocalization;
   static constexpr char name[] = "/localization/initialize";
 };
 

common/component_interface_specs/package.xml

@@ -21,6 +21,7 @@
   <depend>rclcpp</depend>
   <depend>rosidl_runtime_cpp</depend>
   <depend>tier4_control_msgs</depend>
+  <depend>tier4_localization_msgs</depend>
   <depend>tier4_map_msgs</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>tier4_system_msgs</depend>

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -1921,20 +1921,21 @@ insertOrientation<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>
  * radians (default: M_PI_2)
  */
 template <class T>
-void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
+void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
 {
-  for (size_t i = 1; i < points.size();) {
-    const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
-    const auto p2 = tier4_autoware_utils::getPose(points.at(i));
+  for (auto itr = points.begin(); std::next(itr) != points.end();) {
+    const auto p1 = tier4_autoware_utils::getPose(*itr);
+    const auto p2 = tier4_autoware_utils::getPose(*std::next(itr));
     const double yaw1 = tf2::getYaw(p1.orientation);
     const double yaw2 = tf2::getYaw(p2.orientation);
 
     if (
       max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) ||
       !tier4_autoware_utils::isDrivingForward(p1, p2)) {
-      points.erase(points.begin() + i);
+      points.erase(std::next(itr));
+      return;
     } else {
-      ++i;
+      itr++;
     }
   }
 }

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -5337,10 +5337,10 @@ TEST(trajectory, cropPoints)
   }
 }
 
-TEST(Trajectory, removeInvalidOrientationPoints)
+TEST(Trajectory, removeFirstInvalidOrientationPoints)
 {
   using motion_utils::insertOrientation;
-  using motion_utils::removeInvalidOrientationPoints;
+  using motion_utils::removeFirstInvalidOrientationPoints;
 
   const double max_yaw_diff = M_PI_2;
 
@@ -5351,7 +5351,7 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     auto modified_traj = traj;
     insertOrientation(modified_traj.points, true);
     modifyTrajectory(modified_traj);
-    removeInvalidOrientationPoints(modified_traj.points, max_yaw_diff);
+    removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff);
     EXPECT_EQ(modified_traj.points.size(), expected_size);
     for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) {
       EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i));
@@ -5374,7 +5374,7 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     [&](Trajectory & t) {
       auto invalid_point = t.points.back();
       invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
+        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4));
       t.points.push_back(invalid_point);
     },
     traj.points.size());
@@ -5385,21 +5385,10 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     [&](Trajectory & t) {
       auto invalid_point = t.points[4];
       invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
+        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4));
       t.points.insert(t.points.begin() + 4, invalid_point);
     },
     traj.points.size());
-
-  // invalid point at the beginning
-  testRemoveInvalidOrientationPoints(
-    traj,
-    [&](Trajectory & t) {
-      auto invalid_point = t.points.front();
-      invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
-      t.points.insert(t.points.begin(), invalid_point);
-    },
-    1);  // expected size is 1 since only the first point remains
 }
 
 TEST(trajectory, calcYawDeviation)

common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp

@@ -15,13 +15,17 @@
 #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 
+#include <utility>
+
 namespace tier4_autoware_utils
 {
 
 float sin(float radian);
 
 float cos(float radian);
 
+std::pair<float, float> sin_and_cos(float radian);
+
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

common/tier4_autoware_utils/src/math/trigonometry.cpp

@@ -49,4 +49,27 @@ float cos(float radian)
   return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f);
 }
 
+std::pair<float, float> sin_and_cos(float radian)
+{
+  constexpr float tmp =
+    (180.f / static_cast<float>(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f);
+  const float degree = radian * tmp;
+  size_t idx =
+    (static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
+    discrete_arcs_num_360;
+
+  if (idx < discrete_arcs_num_90) {
+    return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
+  } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) {
+    idx = 2 * discrete_arcs_num_90 - idx;
+    return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
+  } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) {
+    idx = idx - 2 * discrete_arcs_num_90;
+    return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
+  } else {  // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90
+    idx = 4 * discrete_arcs_num_90 - idx;
+    return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
+  }
+}
+
 }  // namespace tier4_autoware_utils

common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp

@@ -40,3 +40,13 @@ TEST(trigonometry, cos)
         tier4_autoware_utils::cos(x * static_cast<float>(i))) < 10e-5);
   }
 }
+
+TEST(trigonometry, sin_and_cos)
+{
+  float x = 4.f * tier4_autoware_utils::pi / 128.f;
+  for (int i = 0; i < 128; i++) {
+    const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast<float>(i));
+    EXPECT_TRUE(std::abs(std::sin(x * static_cast<float>(i)) - sin_and_cos.first) < 10e-7);
+    EXPECT_TRUE(std::abs(std::cos(x * static_cast<float>(i)) - sin_and_cos.second) < 10e-7);
+  }
+}