Merge branch 'main' into feat/pose_initilizer/add_method_type

Author: YamatoAndo

.github/CODEOWNERS

@@ -156,6 +156,7 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon
 perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
 perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp
 planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai
+planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
 planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
 planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
 planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp
@@ -208,7 +209,6 @@ planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp
 planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp
 planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp
 planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
-planning/static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
 planning/surround_obstacle_checker/** satoshi.ota@tier4.jp
 sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp

common/component_interface_tools/CMakeLists.txt

@@ -3,5 +3,14 @@ project(component_interface_tools)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
-ament_auto_add_executable(service_log_checker src/service_log_checker.cpp)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/service_log_checker.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "ServiceLogChecker"
+  EXECUTABLE service_log_checker_node
+)
+
 ament_auto_package(INSTALL_TO_SHARE launch)

common/component_interface_tools/launch/service_log_checker.launch.xml

@@ -1,3 +1,3 @@
 <launch>
-  <node pkg="component_interface_tools" exec="service_log_checker" name="service_log_checker"/>
+  <node pkg="component_interface_tools" exec="service_log_checker_node"/>
 </launch>

common/component_interface_tools/package.xml

@@ -13,6 +13,7 @@
   <depend>diagnostic_updater</depend>
   <depend>fmt</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>tier4_system_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
 

common/component_interface_tools/src/service_log_checker.cpp

@@ -22,7 +22,8 @@
 #define FMT_HEADER_ONLY
 #include <fmt/format.h>
 
-ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this)
+ServiceLogChecker::ServiceLogChecker(const rclcpp::NodeOptions & options)
+: Node("service_log_checker", options), diagnostics_(this)
 {
   sub_ = create_subscription<ServiceLog>(
     "/service_log", 50, std::bind(&ServiceLogChecker::on_service_log, this, std::placeholders::_1));
@@ -98,13 +99,5 @@ void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusW
   }
 }
 
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  rclcpp::executors::SingleThreadedExecutor executor;
-  auto node = std::make_shared<ServiceLogChecker>();
-  executor.add_node(node);
-  executor.spin();
-  executor.remove_node(node);
-  rclcpp::shutdown();
-}
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(ServiceLogChecker)

common/component_interface_tools/src/service_log_checker.hpp

@@ -26,7 +26,7 @@
 class ServiceLogChecker : public rclcpp::Node
 {
 public:
-  ServiceLogChecker();
+  explicit ServiceLogChecker(const rclcpp::NodeOptions & options);
 
 private:
   using ServiceLog = tier4_system_msgs::msg::ServiceLog;

common/motion_utils/CMakeLists.txt

@@ -7,15 +7,7 @@ autoware_package()
 find_package(Boost REQUIRED)
 
 ament_auto_add_library(motion_utils SHARED
-  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/conversion.cpp
-  src/vehicle/vehicle_state_checker.cpp
+  DIRECTORY src
 )
 
 if(BUILD_TESTING)

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp

@@ -1,5 +1,5 @@
 
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-2024 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
-#define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
+#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
+#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
 
 #include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
@@ -25,35 +25,31 @@
 #include <string>
 #include <vector>
 
-namespace behavior_velocity_planner
+namespace motion_utils
 {
-
 using autoware_adapi_v1_msgs::msg::PlanningBehavior;
 using autoware_adapi_v1_msgs::msg::VelocityFactor;
-using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
-using geometry_msgs::msg::Pose;
 using VelocityFactorBehavior = VelocityFactor::_behavior_type;
 using VelocityFactorStatus = VelocityFactor::_status_type;
+using geometry_msgs::msg::Pose;
 
 class VelocityFactorInterface
 {
 public:
-  VelocityFactorInterface() { behavior_ = VelocityFactor::UNKNOWN; }
-
-  VelocityFactor get() const { return velocity_factor_; }
-  void init(const VelocityFactorBehavior behavior) { behavior_ = behavior; }
+  [[nodiscard]] VelocityFactor get() const { return velocity_factor_; }
+  void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; }
   void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; }
 
   void set(
     const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
     const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
-    const std::string detail = "");
+    const std::string & detail = "");
 
 private:
-  VelocityFactorBehavior behavior_;
-  VelocityFactor velocity_factor_;
+  VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};
+  VelocityFactor velocity_factor_{};
 };
 
-}  // namespace behavior_velocity_planner
+}  // namespace motion_utils
 
-#endif  // BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
+#endif  // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_

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

@@ -27,7 +27,11 @@ namespace resample_utils
 {
 constexpr double close_s_threshold = 1e-6;
 
-#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl;
+static inline rclcpp::Logger get_logger()
+{
+  constexpr const char * logger{"motion_utils.resample_utils"};
+  return rclcpp::get_logger(logger);
+}
 
 template <class T>
 bool validate_size(const T & points)
@@ -62,27 +66,27 @@ bool validate_arguments(const T & input_points, const std::vector<double> & resa
 {
   // Check size of the arguments
   if (!validate_size(input_points)) {
-    log_error("[resample_utils] invalid argument: The number of input points is less than 2");
+    RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2");
     tier4_autoware_utils::print_backtrace();
     return false;
   }
   if (!validate_size(resampling_intervals)) {
-    log_error(
-      "[resample_utils] invalid argument: The number of resampling intervals is less than 2");
+    RCLCPP_DEBUG(
+      get_logger(), "invalid argument: The number of resampling intervals is less than 2");
     tier4_autoware_utils::print_backtrace();
     return false;
   }
 
   // Check resampling range
   if (!validate_resampling_range(input_points, resampling_intervals)) {
-    log_error("[resample_utils] invalid argument: resampling interval is longer than input points");
+    RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points");
     tier4_autoware_utils::print_backtrace();
     return false;
   }
 
   // Check duplication
   if (!validate_points_duplication(input_points)) {
-    log_error("[resample_utils] invalid argument: input points has some duplicated points");
+    RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points");
     tier4_autoware_utils::print_backtrace();
     return false;
   }
@@ -95,23 +99,23 @@ bool validate_arguments(const T & input_points, const double resampling_interval
 {
   // Check size of the arguments
   if (!validate_size(input_points)) {
-    log_error("[resample_utils] invalid argument: The number of input points is less than 2");
+    RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2");
     tier4_autoware_utils::print_backtrace();
     return false;
   }
 
   // check resampling interval
   if (resampling_interval < motion_utils::overlap_threshold) {
-    log_error(
-      "[resample_utils] invalid argument: resampling interval is less than " +
-      std::to_string(motion_utils::overlap_threshold));
+    RCLCPP_DEBUG(
+      get_logger(), "invalid argument: resampling interval is less than %f",
+      motion_utils::overlap_threshold);
     tier4_autoware_utils::print_backtrace();
     return false;
   }
 
   // Check duplication
   if (!validate_points_duplication(input_points)) {
-    log_error("[resample_utils] invalid argument: input points has some duplicated points");
+    RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points");
     tier4_autoware_utils::print_backtrace();
     return false;
   }