Merge pull request #925 from tier4/beta-to-tier4-main-sync

chore: sync beta branch beta/v0.13.0 with tier4/main

.github/CODEOWNERS

@@ -21,11 +21,11 @@ common/goal_distance_calculator/** [email protected]
 common/grid_map_utils/** [email protected]
 common/interpolation/** [email protected] [email protected] [email protected]
 common/kalman_filter/** [email protected] [email protected] [email protected]
-common/motion_utils/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+common/motion_utils/** [email protected] [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] [email protected] [email protected]
-common/perception_utils/** [email protected] [email protected] [email protected] [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]
@@ -34,7 +34,7 @@ common/tensorrt_common/** [email protected] [email protected] manato.
 common/tier4_adapi_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_api_utils/** [email protected] [email protected] [email protected]
 common/tier4_automatic_goal_rviz_plugin/** [email protected] [email protected]
-common/tier4_autoware_utils/** [email protected] [email protected] [email protected]
+common/tier4_autoware_utils/** [email protected] [email protected] [email protected] [email protected]
 common/tier4_calibration_rviz_plugin/** [email protected]
 common/tier4_control_rviz_plugin/** [email protected]
 common/tier4_datetime_rviz_plugin/** [email protected] [email protected] [email protected]
@@ -46,6 +46,8 @@ common/tier4_planning_rviz_plugin/** [email protected] yukihiro.saito@ti
 common/tier4_screen_capture_rviz_plugin/** [email protected]
 common/tier4_simulated_clock_rviz_plugin/** [email protected]
 common/tier4_state_rviz_plugin/** [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]
@@ -85,7 +87,7 @@ launch/tier4_vehicle_launch/** [email protected]
 localization/ar_tag_based_localizer/** [email protected] [email protected]
 localization/ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/gyro_odometer/** [email protected] [email protected]
-localization/localization_error_monitor/** [email protected] [email protected]
+localization/localization_error_monitor/** [email protected] [email protected] [email protected]
 localization/ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected]
 localization/pose2twist/** [email protected] [email protected]
 localization/pose_initializer/** [email protected] [email protected] [email protected]
@@ -102,8 +104,9 @@ map/map_projection_loader/** [email protected] [email protected] yam
 map/map_tf_generator/** [email protected]
 map/util/lanelet2_map_preprocessor/** [email protected]
 perception/bytetrack/** [email protected]
+perception/cluster_merger/** [email protected] [email protected] [email protected]
 perception/compare_map_segmentation/** [email protected] [email protected] [email protected]
-perception/crosswalk_traffic_light_estimator/** [email protected]
+perception/crosswalk_traffic_light_estimator/** [email protected] [email protected] [email protected]
 perception/detected_object_feature_remover/** [email protected]
 perception/detected_object_validation/** [email protected] [email protected]
 perception/detection_by_tracker/** [email protected] [email protected]
@@ -118,7 +121,7 @@ perception/lidar_apollo_segmentation_tvm/** [email protected] xinyu.wang@
 perception/lidar_apollo_segmentation_tvm_nodes/** [email protected] [email protected]
 perception/lidar_centerpoint/** [email protected] [email protected]
 perception/lidar_centerpoint_tvm/** [email protected] [email protected]
-perception/map_based_prediction/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+perception/map_based_prediction/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 perception/multi_object_tracker/** [email protected] [email protected]
 perception/object_merger/** [email protected] [email protected]
 perception/object_range_splitter/** [email protected]
@@ -137,10 +140,10 @@ perception/tensorrt_yolo/** [email protected]
 perception/tensorrt_yolox/** [email protected] [email protected] [email protected]
 perception/traffic_light_arbiter/** [email protected] [email protected]
 perception/traffic_light_classifier/** [email protected] [email protected]
-perception/traffic_light_fine_detector/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp
+perception/traffic_light_fine_detector/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
 perception/traffic_light_map_based_detector/** [email protected]
-perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp
-perception/traffic_light_occlusion_predictor/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp
+perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
+perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
 perception/traffic_light_ssd_fine_detector/** [email protected]
 perception/traffic_light_visualization/** [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]
@@ -173,8 +176,8 @@ planning/obstacle_velocity_limiter/** [email protected]
 planning/path_smoother/** [email protected] [email protected]
 planning/planning_debug_tools/** [email protected] [email protected]
 planning/planning_test_utils/** [email protected] [email protected]
-planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp
-planning/route_handler/** [email protected] [email protected] [email protected] [email protected] [email protected]
+planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp
+planning/route_handler/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/rtc_auto_mode_manager/** [email protected] [email protected]
 planning/rtc_interface/** [email protected] [email protected]
 planning/rtc_replayer/** [email protected] [email protected]
@@ -185,7 +188,7 @@ planning/sampling_based_planner/sampler_common/** [email protected]
 planning/scenario_selector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/static_centerline_optimizer/** [email protected] [email protected]
 planning/surround_obstacle_checker/** [email protected]
-sensing/gnss_poser/** [email protected]
+sensing/gnss_poser/** [email protected] [email protected]
 sensing/image_diagnostics/** [email protected]
 sensing/image_transport_decompressor/** [email protected]
 sensing/imu_corrector/** [email protected] [email protected] [email protected]

common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp

@@ -45,56 +45,6 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
 
 namespace tf2
 {
-
-/*************/
-/** Point32 **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The point to transform, as a Point32 message.
- * \param t_out The transformed point, as a Point32 message.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
-  const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
-  t_out.x = static_cast<float>(v_out[0]);
-  t_out.y = static_cast<float>(v_out[1]);
-  t_out.z = static_cast<float>(v_out[2]);
-}
-
-/*************/
-/** Polygon **/
-/*************/
-
-/** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The polygon to transform.
- * \param t_out The transformed polygon.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
-  const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  // Don't call the Point32 doTransform to avoid doing this conversion every time
-  const auto kdl_frame = gmTransformToKDL(transform);
-  // We don't use std::back_inserter to allow aliasing between t_in and t_out
-  t_out.points.resize(t_in.points.size());
-  for (size_t i = 0; i < t_in.points.size(); ++i) {
-    const KDL::Vector v_out =
-      kdl_frame * KDL::Vector(t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
-    t_out.points[i].x = static_cast<float>(v_out[0]);
-    t_out.points[i].y = static_cast<float>(v_out[1]);
-    t_out.points[i].z = static_cast<float>(v_out[2]);
-  }
-}
-
 /******************/
 /** Quaternion32 **/
 /******************/

common/geography_utils/src/lanelet2_projector.cpp

@@ -15,6 +15,7 @@
 #include <GeographicLib/Geoid.hpp>
 #include <geography_utils/lanelet2_projector.hpp>
 #include <lanelet2_extension/projection/mgrs_projector.hpp>
+#include <lanelet2_extension/projection/transverse_mercator_projector.hpp>
 
 #include <lanelet2_projection/UTM.h>
 
@@ -30,14 +31,23 @@ std::unique_ptr<lanelet::Projector> get_lanelet2_projector(const MapProjectorInf
     lanelet::Origin origin{position};
     lanelet::projection::UtmProjector projector{origin};
     return std::make_unique<lanelet::projection::UtmProjector>(projector);
+
   } else if (projector_info.projector_type == MapProjectorInfo::MGRS) {
     lanelet::projection::MGRSProjector projector{};
     return std::make_unique<lanelet::projection::MGRSProjector>(projector);
-  } else {
-    const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type +
-                                  ". Currently supported types: MGRS, and LocalCartesianUTM";
-    throw std::invalid_argument(error_msg);
+
+  } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) {
+    lanelet::GPSPoint position{
+      projector_info.map_origin.latitude, projector_info.map_origin.longitude,
+      projector_info.map_origin.altitude};
+    lanelet::Origin origin{position};
+    lanelet::projection::TransverseMercatorProjector projector{origin};
+    return std::make_unique<lanelet::projection::TransverseMercatorProjector>(projector);
   }
+  const std::string error_msg =
+    "Invalid map projector type: " + projector_info.projector_type +
+    ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator";
+  throw std::invalid_argument(error_msg);
 }
 
 }  // namespace geography_utils

common/motion_utils/CMakeLists.txt

@@ -7,13 +7,14 @@ autoware_package()
 find_package(Boost REQUIRED)
 
 ament_auto_add_library(motion_utils SHARED
-  src/motion_utils.cpp
   src/distance/distance.cpp
   src/marker/marker_helper.cpp
   src/marker/virtual_wall_marker_creator.cpp
   src/resample/resample.cpp
+  src/trajectory/trajectory.cpp
   src/trajectory/interpolation.cpp
   src/trajectory/path_with_lane_id.cpp
+  src/trajectory/tmp_conversion.cpp
   src/vehicle/vehicle_state_checker.cpp
 )
 

common/motion_utils/README.md

@@ -109,3 +109,9 @@ const size_t ego_nearest_seg_idx = findFirstNearestSegmentIndex(points, ego_pose
 const size_t dyn_obj_nearest_seg_idx = findFirstNearestSegmentIndex(points, dyn_obj_pose, dyn_obj_nearest_dist_threshold);
 const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_nearest_seg_idx, dyn_obj_pose, dyn_obj_nearest_seg_idx);
 ```
+
+## For developers
+
+Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time.
+
+`motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing.

common/motion_utils/include/motion_utils/marker/marker_helper.hpp

@@ -15,29 +15,30 @@
 #ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_
 
-#include "motion_utils/resample/resample_utils.hpp"
+#include <rclcpp/time.hpp>
 
 #include <visualization_msgs/msg/marker_array.hpp>
 
-#include <functional>
 #include <string>
-#include <vector>
 
 namespace motion_utils
 {
 using geometry_msgs::msg::Pose;
 
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);

common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp

@@ -15,14 +15,14 @@
 #ifndef MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
 #define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_
 
-#include "motion_utils/marker/marker_helper.hpp"
+#include <rclcpp/time.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <string>
 #include <unordered_map>
 #include <vector>
-
 namespace motion_utils
 {
 
@@ -36,6 +36,7 @@ struct VirtualWall
   std::string ns{};
   VirtualWallType style = stop;
   double longitudinal_offset{};
+  bool is_driving_forward{true};
 };
 typedef std::vector<VirtualWall> VirtualWalls;
 
@@ -52,7 +53,7 @@ class VirtualWallMarkerCreator
   using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
     const geometry_msgs::msg::Pose & pose, const std::string & module_name,
     const rclcpp::Time & now, const int32_t id, const double longitudinal_offset,
-    const std::string & ns_prefix)>;
+    const std::string & ns_prefix, const bool is_driving_forward)>;
 
   VirtualWalls virtual_walls;
   std::unordered_map<std::string, MarkerCount> marker_count_per_namespace;

common/motion_utils/include/motion_utils/resample/resample.hpp

@@ -15,17 +15,6 @@
 #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
 #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
 
-#include "interpolation/interpolation_utils.hpp"
-#include "interpolation/linear_interpolation.hpp"
-#include "interpolation/spline_interpolation.hpp"
-#include "interpolation/zero_order_hold.hpp"
-#include "motion_utils/trajectory/trajectory.hpp"
-#include "tier4_autoware_utils/geometry/geometry.hpp"
-#include "tier4_autoware_utils/geometry/pose_deviation.hpp"
-#include "tier4_autoware_utils/math/constants.hpp"
-
-#include <rclcpp/rclcpp.hpp>
-
 #include "autoware_auto_planning_msgs/msg/path.hpp"
 #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"

common/motion_utils/include/motion_utils/resample/resample_utils.hpp

@@ -18,7 +18,6 @@
 #include <motion_utils/constants.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
-#include <tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp>
 
 #include <vector>