feat(tier4_autoware_utils): add vehicle state checker (tier4#896)

* feat(tier4_autoware_utils): add vehicle state checker

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

* fix(tier4_autoware_utils): use absolute value

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

* feat(tier4_autoware_utils): divide into two classies

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

* test(tier4_autoware_utils): add unit test for vehicle_state checker

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

* fix(tier4_autoware_utils): impl class inheritance

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

* docs(tier4_autoware_utils): add vehicle_state_checker document

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

* fix(tier4_autoware_utils): into same loop

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

* fix(tier4_autoware_utils): fix variables name

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

* fix(tier4_autoware_utils): remove redundant codes

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

common/tier4_autoware_utils/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(Boost REQUIRED)
 ament_auto_add_library(tier4_autoware_utils SHARED
   src/tier4_autoware_utils.cpp
   src/planning/planning_marker_helper.cpp
+  src/vehicle/vehicle_state_checker.cpp
 )
 
 if(BUILD_TESTING)

common/tier4_autoware_utils/docs/vehicle/vehicle.md

@@ -0,0 +1,169 @@
+# vehicle utils
+
+Vehicle utils provides a convenient library used to check vehicle status.
+
+## Feature
+
+The library contains following classes.
+
+### vehicle_stop_checker
+
+This class check whether the vehicle is stopped or not based on localization result.
+
+#### Subscribed Topics
+
+| Name                            | Type                      | Description      |
+| ------------------------------- | ------------------------- | ---------------- |
+| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry |
+
+#### Parameters
+
+| Name                       | Type   | Default Value | Explanation                 |
+| -------------------------- | ------ | ------------- | --------------------------- |
+| `velocity_buffer_time_sec` | double | 10.0          | odometry buffering time [s] |
+
+#### Member functions
+
+```c++
+bool isVehicleStopped(const double stop_duration)
+```
+
+- Check simply whether the vehicle is stopped based on the localization result.
+- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity.
+
+#### Example Usage
+
+Necessary includes:
+
+```c++
+#include <tier4_autoware_utils/vehicle/vehicle_state_checker.hpp>
+```
+
+1.Create a checker instance.
+
+```c++
+class SampleNode : public rclcpp::Node
+{
+public:
+  SampleNode() : Node("sample_node")
+  {
+    vehicle_stop_checker_ = std::make_unique<VehicleStopChecker>(this);
+  }
+
+  std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
+
+  bool sampleFunc();
+
+  ...
+}
+```
+
+2.Check the vehicle state.
+
+```c++
+
+bool SampleNode::sampleFunc()
+{
+  ...
+
+  const auto result_1 = vehicle_stop_checker_->isVehicleStopped();
+
+  ...
+
+  const auto result_2 = vehicle_stop_checker_->isVehicleStopped(3.0);
+
+  ...
+}
+
+```
+
+### vehicle_arrival_checker
+
+This class check whether the vehicle arrive at stop point based on localization and planning result.
+
+#### Subscribed Topics
+
+| Name                                     | Type                                           | Description      |
+| ---------------------------------------- | ---------------------------------------------- | ---------------- |
+| `/localization/kinematic_state`          | `nav_msgs::msg::Odometry`                      | vehicle odometry |
+| `/planning/scenario_planning/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | trajectory       |
+
+#### Parameters
+
+| Name                       | Type   | Default Value | Explanation                                                            |
+| -------------------------- | ------ | ------------- | ---------------------------------------------------------------------- |
+| `velocity_buffer_time_sec` | double | 10.0          | odometry buffering time [s]                                            |
+| `th_arrived_distance_m`    | double | 1.0           | threshold distance to check if vehicle has arrived at target point [m] |
+
+#### Member functions
+
+```c++
+bool isVehicleStopped(const double stop_duration)
+```
+
+- Check simply whether the vehicle is stopped based on the localization result.
+- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity.
+
+```c++
+bool isVehicleStoppedAtStopPoint(const double stop_duration)
+```
+
+- Check whether the vehicle is stopped at stop point based on the localization and planning result.
+- Returns `true` if the vehicle is not only stopped but also arrived at stop point.
+
+#### Example Usage
+
+Necessary includes:
+
+```c++
+#include <tier4_autoware_utils/vehicle/vehicle_state_checker.hpp>
+```
+
+1.Create a checker instance.
+
+```c++
+class SampleNode : public rclcpp::Node
+{
+public:
+  SampleNode() : Node("sample_node")
+  {
+    vehicle_arrival_checker_ = std::make_unique<VehicleArrivalChecker>(this);
+  }
+
+  std::unique_ptr<VehicleArrivalChecker> vehicle_arrival_checker_;
+
+  bool sampleFunc();
+
+  ...
+}
+```
+
+2.Check the vehicle state.
+
+```c++
+
+bool SampleNode::sampleFunc1()
+{
+  ...
+
+  const auto result_1 = vehicle_arrival_checker_->isVehicleStopped();
+
+  ...
+
+  const auto result_2 = vehicle_arrival_checker_->isVehicleStopped(3.0);
+
+  ...
+
+  const auto result_3 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint();
+
+  ...
+
+  const auto result_4 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(3.0);
+
+  ...
+}
+```
+
+## Assumptions / Known limits
+
+`vehicle_stop_checker` and `vehicle_arrival_checker` cannot check whether the vehicle is stopped more than `velocity_buffer_time_sec` second.

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

@@ -0,0 +1,76 @@
+// Copyright 2022 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.
+// 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 TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_
+#define TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <deque>
+#include <memory>
+
+namespace tier4_autoware_utils
+{
+
+using autoware_auto_planning_msgs::msg::Trajectory;
+using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::Odometry;
+
+class VehicleStopChecker
+{
+public:
+  explicit VehicleStopChecker(rclcpp::Node * node);
+
+  bool isVehicleStopped(const double stop_duration) const;
+
+  rclcpp::Logger getLogger() { return logger_; }
+
+protected:
+  rclcpp::Subscription<Odometry>::SharedPtr sub_odom_;
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Logger logger_;
+
+  Odometry::SharedPtr odometry_ptr_;
+
+  std::deque<TwistStamped> twist_buffer_;
+
+private:
+  static constexpr double velocity_buffer_time_sec = 10.0;
+
+  void onOdom(const Odometry::SharedPtr msg);
+};
+
+class VehicleArrivalChecker : public VehicleStopChecker
+{
+public:
+  explicit VehicleArrivalChecker(rclcpp::Node * node);
+
+  bool isVehicleStoppedAtStopPoint(const double stop_duration) const;
+
+private:
+  static constexpr double th_arrived_distance_m = 1.0;
+
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+
+  Trajectory::SharedPtr trajectory_ptr_;
+
+  void onTrajectory(const Trajectory::SharedPtr msg);
+};
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_

common/tier4_autoware_utils/package.xml

@@ -13,6 +13,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>

common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp

@@ -0,0 +1,118 @@
+// Copyright 2022 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.
+// 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.
+
+#include "tier4_autoware_utils/vehicle/vehicle_state_checker.hpp"
+
+#include "tier4_autoware_utils/trajectory/trajectory.hpp"
+
+#include <string>
+
+namespace tier4_autoware_utils
+{
+VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node)
+: clock_(node->get_clock()), logger_(node->get_logger())
+{
+  using std::placeholders::_1;
+
+  sub_odom_ = node->create_subscription<Odometry>(
+    "/localization/kinematic_state", rclcpp::QoS(1),
+    std::bind(&VehicleStopChecker::onOdom, this, _1));
+}
+
+bool VehicleStopChecker::isVehicleStopped(const double stop_duration = 0.0) const
+{
+  if (twist_buffer_.empty()) {
+    return false;
+  }
+
+  constexpr double stop_velocity = 1e-3;
+  const auto now = clock_->now();
+
+  const auto time_buffer_back = now - twist_buffer_.back().header.stamp;
+  if (time_buffer_back.seconds() < stop_duration) {
+    return false;
+  }
+
+  // Get velocities within stop_duration
+  for (const auto & velocity : twist_buffer_) {
+    if (stop_velocity <= velocity.twist.linear.x) {
+      return false;
+    }
+
+    const auto time_diff = now - velocity.header.stamp;
+    if (time_diff.seconds() >= stop_duration) {
+      break;
+    }
+  }
+
+  return true;
+}
+
+void VehicleStopChecker::onOdom(const Odometry::SharedPtr msg)
+{
+  odometry_ptr_ = msg;
+
+  auto current_velocity = std::make_shared<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();
+  }
+}
+
+VehicleArrivalChecker::VehicleArrivalChecker(rclcpp::Node * node) : VehicleStopChecker(node)
+{
+  using std::placeholders::_1;
+
+  sub_trajectory_ = node->create_subscription<Trajectory>(
+    "/planning/scenario_planning/trajectory", rclcpp::QoS(1),
+    std::bind(&VehicleArrivalChecker::onTrajectory, this, _1));
+}
+
+bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const
+{
+  if (!odometry_ptr_ || !trajectory_ptr_) {
+    return false;
+  }
+
+  if (!isVehicleStopped(stop_duration)) {
+    return false;
+  }
+
+  const auto & p = odometry_ptr_->pose.pose.position;
+  const auto idx = searchZeroVelocityIndex(trajectory_ptr_->points);
+
+  if (!idx) {
+    return false;
+  }
+
+  return std::abs(calcSignedArcLength(trajectory_ptr_->points, p, idx.get())) <
+         th_arrived_distance_m;
+}
+
+void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) { trajectory_ptr_ = msg; }
+}  // namespace tier4_autoware_utils

common/tier4_autoware_utils/test/src/test_autoware_utils.cpp

@@ -19,5 +19,8 @@
 int main(int argc, char * argv[])
 {
   testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+  rclcpp::init(argc, argv);
+  bool result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
 }