feat(autoware_velocity_smoother): porting from universe to core, auto…

planning/autoware_velocity_smoother/README.ja.md

@@ -1,12 +1,12 @@
 # Velocity Smoother
 
-## Purpose
+## Overview
 
 `autoware_velocity_smoother`は目標軌道上の各点における望ましい車速を計画して出力するモジュールである。
 このモジュールは、速度の最大化と乗り心地の良さを両立するために、事前に指定された制限速度、制限加速度および制限躍度の範囲で車速を計画する。
 加速度や躍度の制限を与えることは車速の変化を滑らかにすることに対応するため、このモジュールを`autoware_velocity_smoother`と呼んでいる。
 
-## Inner-workings / Algorithms
+## Design
 
 ### Flow chart
 
@@ -237,14 +237,8 @@ Example:
 - 参照経路に設定されている制限速度を指定した減速度やジャークで達成不可能な場合、可能な範囲で速度、加速度、ジャークの逸脱量を抑えながら減速
 - 各逸脱量の重視の度合いはパラメータにより指定
 
-## (Optional) Error detection and handling
-
-## (Optional) Performance characterization
-
-## (Optional) References/External links
+## References/External links
 
 [1] B. Stellato, et al., "OSQP: an operator splitting solver for quadratic programs", Mathematical Programming Computation, 2020, [10.1007/s12532-020-00179-2](https://link.springer.com/article/10.1007/s12532-020-00179-2).
 
 [2] Y. Zhang, et al., "Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization", Sensors, vol. 18, no. 7, p. 2185, 2018, [10.3390/s18072185](https://doi.org/10.3390/s18072185)
-
-## (Optional) Future extensions / Unimplemented parts

planning/autoware_velocity_smoother/README.md

@@ -1,12 +1,12 @@
 # Velocity Smoother
 
-## Purpose
+## Overview
 
 `autoware_velocity_smoother` outputs a desired velocity profile on a reference trajectory.
 This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality.
 We call this module `autoware_velocity_smoother` because the limitations of the acceleration and the jerk means the smoothness of the velocity profile.
 
-## Inner-workings / Algorithms
+## Design
 
 ### Flow chart
 
@@ -262,14 +262,8 @@ Example:
 - If the velocity limit set in the reference path cannot be achieved by the designated constraints of the deceleration and the jerk, decelerate while suppressing the velocity, the acceleration and the jerk deviation as much as possible
 - The importance of the deviations is set in the config file
 
-## (Optional) Error detection and handling
-
-## (Optional) Performance characterization
-
-## (Optional) References/External links
+## References/External links
 
 [1] B. Stellato, et al., "OSQP: an operator splitting solver for quadratic programs", Mathematical Programming Computation, 2020, [10.1007/s12532-020-00179-2](https://link.springer.com/article/10.1007/s12532-020-00179-2).
 
 [2] Y. Zhang, et al., "Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization", Sensors, vol. 18, no. 7, p. 2185, 2018, [10.3390/s18072185](https://doi.org/10.3390/s18072185)
-
-## (Optional) Future extensions / Unimplemented parts

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp

@@ -18,26 +18,26 @@
 #include "autoware/motion_utils/trajectory/conversion.hpp"
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/osqp_interface/osqp_interface.hpp"
-#include "autoware/universe_utils/geometry/geometry.hpp"
-#include "autoware/universe_utils/math/unit_conversion.hpp"
-#include "autoware/universe_utils/ros/diagnostics_interface.hpp"
-#include "autoware/universe_utils/ros/logger_level_configure.hpp"
-#include "autoware/universe_utils/ros/polling_subscriber.hpp"
-#include "autoware/universe_utils/ros/self_pose_listener.hpp"
-#include "autoware/universe_utils/system/stop_watch.hpp"
-#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/resample.hpp"
 #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp"
 #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp"
 #include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
 #include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
 #include "autoware/velocity_smoother/trajectory_utils.hpp"
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/math/unit_conversion.hpp"
+#include "autoware_utils/ros/diagnostics_interface.hpp"
+#include "autoware_utils/ros/logger_level_configure.hpp"
+#include "autoware_utils/ros/polling_subscriber.hpp"
+#include "autoware_utils/ros/self_pose_listener.hpp"
+#include "autoware_utils/system/stop_watch.hpp"
+#include "autoware_utils/system/time_keeper.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/utils.h"
 #include "tf2_ros/transform_listener.h"
 
-#include <autoware/universe_utils/ros/published_time_publisher.hpp>
+#include <autoware_utils/ros/published_time_publisher.hpp>
 
 #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
 #include "autoware_internal_debug_msgs/msg/float32_stamped.hpp"
@@ -61,10 +61,10 @@ namespace autoware::velocity_smoother
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
-using autoware::universe_utils::DiagnosticsInterface;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_internal_debug_msgs::msg::Float32Stamped;
 using autoware_internal_debug_msgs::msg::Float64Stamped;
+using autoware_utils::DiagnosticsInterface;
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::PoseStamped;
@@ -90,14 +90,14 @@ class VelocitySmootherNode : public rclcpp::Node
   rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_;
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;
   rclcpp::Subscription<Trajectory>::SharedPtr sub_current_trajectory_;
-  autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_current_odometry_{
+  autoware_utils::InterProcessPollingSubscriber<Odometry> sub_current_odometry_{
     this, "/localization/kinematic_state"};
-  autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>
+  autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>
     sub_current_acceleration_{this, "~/input/acceleration"};
-  autoware::universe_utils::InterProcessPollingSubscriber<
-    VelocityLimit, universe_utils::polling_policy::Newest>
+  autoware_utils::InterProcessPollingSubscriber<
+    VelocityLimit, autoware_utils::polling_policy::Newest>
     sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"};
-  autoware::universe_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
+  autoware_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
     this, "~/input/operation_mode_state"};
 
   Odometry::ConstSharedPtr current_odometry_ptr_;  // current odometry
@@ -255,7 +255,7 @@ class VelocitySmootherNode : public rclcpp::Node
   void initCommonParam();
 
   // debug
-  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
+  autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
   std::shared_ptr<rclcpp::Time> prev_time_;
   double prev_acc_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr pub_dist_to_stopline_;
@@ -270,8 +270,7 @@ class VelocitySmootherNode : public rclcpp::Node
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_jerk_;
   rclcpp::Publisher<Float64Stamped>::SharedPtr debug_calculation_time_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_max_velocity_;
-  rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
-    debug_processing_time_detail_;
+  rclcpp::Publisher<autoware_utils::ProcessingTimeDetail>::SharedPtr debug_processing_time_detail_;
 
   // For Jerk Filtered Algorithm Debug
   rclcpp::Publisher<Trajectory>::SharedPtr pub_forward_filtered_trajectory_;
@@ -285,10 +284,10 @@ class VelocitySmootherNode : public rclcpp::Node
   void flipVelocity(TrajectoryPoints & points) const;
   void publishStopWatchTime();
 
-  std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
-  std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
+  std::unique_ptr<autoware_utils::LoggerLevelConfigure> logger_configure_;
+  std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
-  mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};
+  mutable std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_{nullptr};
 
   std::unique_ptr<DiagnosticsInterface> diagnostics_interface_{nullptr};
 };

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp

@@ -16,9 +16,10 @@
 #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_  // NOLINT
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
-#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/system/time_keeper.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tf2/utils.h"
 
@@ -68,8 +69,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
   };
 
   explicit AnalyticalJerkConstrainedSmoother(
-    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper =
-                           std::make_shared<autoware::universe_utils::TimeKeeper>());
+    rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper =
+                           std::make_shared<autoware_utils::TimeKeeper>());
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp

@@ -17,9 +17,9 @@
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/qp_interface/qp_interface.hpp"
-#include "autoware/universe_utils/geometry/geometry.hpp"
-#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/system/time_keeper.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
@@ -43,7 +43,7 @@ class JerkFilteredSmoother : public SmootherBase
   };
 
   explicit JerkFilteredSmoother(
-    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
+    rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp

@@ -17,9 +17,9 @@
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/osqp_interface/osqp_interface.hpp"
-#include "autoware/universe_utils/geometry/geometry.hpp"
-#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/system/time_keeper.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
@@ -41,7 +41,7 @@ class L2PseudoJerkSmoother : public SmootherBase
   };
 
   explicit L2PseudoJerkSmoother(
-    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
+    rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp

@@ -17,9 +17,9 @@
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/osqp_interface/osqp_interface.hpp"
-#include "autoware/universe_utils/geometry/geometry.hpp"
-#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
+#include "autoware_utils/geometry/geometry.hpp"
+#include "autoware_utils/system/time_keeper.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
@@ -41,7 +41,7 @@ class LinfPseudoJerkSmoother : public SmootherBase
   };
 
   explicit LinfPseudoJerkSmoother(
-    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
+    rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp

@@ -15,8 +15,8 @@
 #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
 #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
 
-#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/resample.hpp"
+#include "autoware_utils/system/time_keeper.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
@@ -57,7 +57,7 @@ class SmootherBase
   };
 
   explicit SmootherBase(
-    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
+    rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper);
   virtual ~SmootherBase() = default;
   virtual bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
@@ -91,7 +91,7 @@ class SmootherBase
 
 protected:
   BaseParam base_param_;
-  mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};
+  mutable std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_{nullptr};
 };
 }  // namespace autoware::velocity_smoother
 

planning/autoware_velocity_smoother/package.xml

@@ -10,6 +10,7 @@
   <maintainer email="makoto.kurihara@tier4.jp">Makoto Kurihara</maintainer>
   <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <maintainer email="go.sakayori@tier4.jp">Go Sakayori</maintainer>
+  <maintainer email="lxg19892021@gmail.com">Xingang Liu</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="takamasa.horibe@tier4.jp">Takamasa Horibe</author>
@@ -28,7 +29,7 @@
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_planning_test_manager</depend>
   <depend>autoware_qp_interface</depend>
-  <depend>autoware_universe_utils</depend>
+  <depend>autoware_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>