feat: porting autoware_motion_utils from universe to core

common/autoware_motion_utils/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_motion_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Boost REQUIRED)
+
+ament_auto_add_library(autoware_motion_utils SHARED
+  DIRECTORY src
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_autoware_motion_utils ${test_files})
+
+  target_link_libraries(test_autoware_motion_utils
+    autoware_motion_utils
+  )
+endif()
+
+ament_auto_package()

common/autoware_motion_utils/README.md

@@ -0,0 +1,104 @@
+# Motion Utils package
+
+## Definition of terms
+
+### Segment
+
+`Segment` in Autoware is the line segment between two successive points as follows.
+
+![segment](./media/segment.svg){: style="width:600px"}
+
+The nearest segment index and nearest point index to a certain position is not always th same.
+Therefore, we prepare two different utility functions to calculate a nearest index for points and segments.
+
+## Nearest index search
+
+In this section, the nearest index and nearest segment index search is explained.
+
+We have the same functions for the nearest index search and nearest segment index search.
+Taking for the example the nearest index search, we have two types of functions.
+
+The first function finds the nearest index with distance and yaw thresholds.
+
+```cpp
+template <class T>
+size_t findFirstNearestIndexWithSoftConstraints(
+  const T & points, const geometry_msgs::msg::Pose & pose,
+  const double dist_threshold = std::numeric_limits<double>::max(),
+  const double yaw_threshold = std::numeric_limits<double>::max());
+```
+
+This function finds the first local solution within thresholds.
+The reason to find the first local one is to deal with some edge cases explained in the next subsection.
+
+There are default parameters for thresholds arguments so that you can decide which thresholds to pass to the function.
+
+1. When both the distance and yaw thresholds are given.
+   - First, try to find the nearest index with both the distance and yaw thresholds.
+   - If not found, try to find again with only the distance threshold.
+   - If not found, find without any thresholds.
+2. When only distance are given.
+   - First, try to find the nearest index the distance threshold.
+   - If not found, find without any thresholds.
+3. When no thresholds are given.
+   - Find the nearest index.
+
+The second function finds the nearest index in the lane whose id is `lane_id`.
+
+```cpp
+size_t findNearestIndexFromLaneId(
+  const autoware_internal_planning_msgs::msg::PathWithLaneId & path,
+  const geometry_msgs::msg::Point & pos, const int64_t lane_id);
+```
+
+### Application to various object
+
+Many node packages often calculate the nearest index of objects.
+We will explain the recommended method to calculate it.
+
+#### Nearest index for the ego
+
+Assuming that the path length before the ego is short enough, we expect to find the correct nearest index in the following edge cases by `findFirstNearestIndexWithSoftConstraints` with both distance and yaw thresholds.
+Blue circles describes the distance threshold from the base link position and two blue lines describe the yaw threshold against the base link orientation.
+Among points in these cases, the correct nearest point which is red can be found.
+
+![ego_nearest_search](./media/ego_nearest_search.svg)
+
+Therefore, the implementation is as follows.
+
+```cpp
+const size_t ego_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+const size_t ego_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, ego_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+```
+
+#### Nearest index for dynamic objects
+
+For the ego nearest index, the orientation is considered in addition to the position since the ego is supposed to follow the points.
+However, for the dynamic objects (e.g., predicted object), sometimes its orientation may be different from the points order, e.g. the dynamic object driving backward although the ego is driving forward.
+
+Therefore, the yaw threshold should not be considered for the dynamic object.
+The implementation is as follows.
+
+```cpp
+const size_t dynamic_obj_nearest_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold);
+const size_t dynamic_obj_nearest_seg_idx = findFirstNearestIndexWithSoftConstraints(points, dynamic_obj_pose, dynamic_obj_nearest_dist_threshold);
+```
+
+#### Nearest index for traffic objects
+
+In lanelet maps, traffic objects belong to the specific lane.
+With this specific lane's id, the correct nearest index can be found.
+
+The implementation is as follows.
+
+```cpp
+// first extract `lane_id` which the traffic object belong to.
+const size_t traffic_obj_nearest_idx = findNearestIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id);
+const size_t traffic_obj_nearest_seg_idx = findNearestSegmentIndexFromLaneId(path_with_lane_id, traffic_obj_pos, lane_id);
+```
+
+## For developers
+
+Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time.
+
+`autoware_motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing.

common/autoware_motion_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 <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_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 <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::sampleFunc()
+{
+  ...
+
+  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/autoware_motion_utils/include/autoware/motion_utils/constants.hpp

@@ -0,0 +1,23 @@
+// 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 AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_
+#define AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_
+
+namespace autoware::motion_utils
+{
+constexpr double overlap_threshold = 0.1;
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__CONSTANTS_HPP_

common/autoware_motion_utils/include/autoware/motion_utils/distance/distance.hpp

@@ -0,0 +1,33 @@
+// Copyright 2023 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 AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_
+#define AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <optional>
+#include <tuple>
+#include <vector>
+
+namespace autoware::motion_utils
+{
+std::optional<double> calcDecelDistWithJerkAndAccConstraints(
+  const double current_vel, const double target_vel, const double current_acc, const double acc_min,
+  const double jerk_acc, const double jerk_dec);
+
+}  // namespace autoware::motion_utils
+
+#endif  // AUTOWARE__MOTION_UTILS__DISTANCE__DISTANCE_HPP_