fix(tier4_autoware_utils): impl class inheritance

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_state_checker.hpp

@@ -28,6 +28,8 @@ namespace tier4_autoware_utils
 {
 
 using autoware_auto_planning_msgs::msg::Trajectory;
+using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::Odometry;
 
 class VehicleStopChecker
 {
@@ -38,46 +40,35 @@ class VehicleStopChecker
 
   rclcpp::Logger getLogger() { return logger_; }
 
-private:
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
-
+protected:
+  rclcpp::Subscription<Odometry>::SharedPtr sub_odom_;
   rclcpp::Clock::SharedPtr clock_;
   rclcpp::Logger logger_;
 
-  std::deque<geometry_msgs::msg::TwistStamped> twist_buffer_;
+  Odometry::SharedPtr odometry_ptr_;
+
+  std::deque<TwistStamped> twist_buffer_;
 
+private:
   static constexpr double velocity_buffer_time_sec = 10.0;
 
-  void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void onOdom(const Odometry::SharedPtr msg);
 };
 
-class VehicleArrivalChecker
+class VehicleArrivalChecker : public VehicleStopChecker
 {
 public:
   explicit VehicleArrivalChecker(rclcpp::Node * node);
 
-  bool isVehicleStopped(const double stop_duration) const;
-
   bool isVehicleStoppedAtStopPoint(const double stop_duration) const;
 
-  rclcpp::Logger getLogger() { return logger_; }
-
 private:
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
-  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+  static constexpr double th_arrived_distance_m = 1.0;
 
-  rclcpp::Clock::SharedPtr clock_;
-  rclcpp::Logger logger_;
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
 
-  nav_msgs::msg::Odometry::SharedPtr odometry_ptr_;
   Trajectory::SharedPtr trajectory_ptr_;
 
-  std::deque<geometry_msgs::msg::TwistStamped> twist_buffer_;
-
-  static constexpr double velocity_buffer_time_sec = 10.0;
-  static constexpr double th_arrived_distance_m = 1.0;
-
-  void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
   void onTrajectory(const Trajectory::SharedPtr msg);
 };
 }  // namespace tier4_autoware_utils

common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp

@@ -25,7 +25,7 @@ VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node)
 {
   using std::placeholders::_1;
 
-  sub_odom_ = node->create_subscription<nav_msgs::msg::Odometry>(
+  sub_odom_ = node->create_subscription<Odometry>(
     "/localization/kinematic_state", rclcpp::QoS(1),
     std::bind(&VehicleStopChecker::onOdom, this, _1));
 }
@@ -64,9 +64,11 @@ bool VehicleStopChecker::isVehicleStopped(const double stop_duration = 0.0) cons
   return true;
 }
 
-void VehicleStopChecker::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
+void VehicleStopChecker::onOdom(const Odometry::SharedPtr msg)
 {
-  auto current_velocity = std::make_shared<geometry_msgs::msg::TwistStamped>();
+  odometry_ptr_ = msg;
+
+  auto current_velocity = std::make_shared<TwistStamped>();
   current_velocity->header = msg->header;
   current_velocity->twist = msg->twist.twist;
 
@@ -87,54 +89,15 @@ void VehicleStopChecker::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
   }
 }
 
-VehicleArrivalChecker::VehicleArrivalChecker(rclcpp::Node * node)
-: clock_(node->get_clock()), logger_(node->get_logger())
+VehicleArrivalChecker::VehicleArrivalChecker(rclcpp::Node * node) : VehicleStopChecker(node)
 {
   using std::placeholders::_1;
 
-  sub_odom_ = node->create_subscription<nav_msgs::msg::Odometry>(
-    "/localization/kinematic_state", rclcpp::QoS(1),
-    std::bind(&VehicleArrivalChecker::onOdom, this, _1));
-
   sub_trajectory_ = node->create_subscription<Trajectory>(
     "/planning/scenario_planning/trajectory", rclcpp::QoS(1),
     std::bind(&VehicleArrivalChecker::onTrajectory, this, _1));
 }
 
-bool VehicleArrivalChecker::isVehicleStopped(const double stop_duration = 0.0) const
-{
-  if (twist_buffer_.empty()) {
-    return false;
-  }
-
-  const auto now = clock_->now();
-  const auto time_buffer = now - twist_buffer_.back().header.stamp;
-  if (time_buffer.seconds() < stop_duration) {
-    return false;
-  }
-
-  // Get velocities within stop_duration
-  std::vector<double> vs;
-  for (const auto & velocity : twist_buffer_) {
-    vs.push_back(velocity.twist.linear.x);
-
-    const auto time_diff = now - velocity.header.stamp;
-    if (time_diff.seconds() >= stop_duration) {
-      break;
-    }
-  }
-
-  // Check all velocities
-  constexpr double stop_velocity = 1e-3;
-  for (const auto & v : vs) {
-    if (v >= stop_velocity) {
-      return false;
-    }
-  }
-
-  return true;
-}
-
 bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const
 {
   if (!odometry_ptr_ || !trajectory_ptr_) {
@@ -156,30 +119,5 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati
          th_arrived_distance_m;
 }
 
-void VehicleArrivalChecker::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
-{
-  odometry_ptr_ = msg;
-
-  auto current_velocity = std::make_shared<geometry_msgs::msg::TwistStamped>();
-  current_velocity->header = msg->header;
-  current_velocity->twist = msg->twist.twist;
-
-  twist_buffer_.push_front(*current_velocity);
-
-  const auto now = clock_->now();
-  while (true) {
-    // Check oldest data time
-    const auto time_diff = now - twist_buffer_.back().header.stamp;
-
-    // Finish when oldest data is newer than threshold
-    if (time_diff.seconds() <= velocity_buffer_time_sec) {
-      break;
-    }
-
-    // Remove old data
-    twist_buffer_.pop_back();
-  }
-}
-
 void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) { trajectory_ptr_ = msg; }
 }  // namespace tier4_autoware_utils