feat(autoware_motion_velocity_planner_common): port to core repo

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_motion_velocity_planner_common)
+
+find_package(autoware_cmake REQUIRED)
+
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME}_lib SHARED
+  DIRECTORY src
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
+    test/test_collision_checker.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    gtest_main
+    ${PROJECT_NAME}_lib
+  )
+  target_include_directories(test_${PROJECT_NAME} PRIVATE src)
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  include
+)

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/README.md

@@ -0,0 +1,59 @@
+# Motion Velocity Planner Common
+
+This package provides common utilities and data structures for the motion velocity planner in the Autoware system. It contains tools for geometric calculations, trajectory processing, and velocity planning results.
+
+## Overview
+
+Package motion velocity planner is responsible for generating velocity profiles for autonomous vehicles based on the current trajectory and environment. This package `autoware_motion_velocity_planner_common` contains essential utilities and structures that support the planning process, including:
+
+- Geometric operations for polygons and trajectories.
+- General utilities for trajectory processing and visualization.
+- Data structures for storing velocity planning results.
+
+## Design
+
+### Key Components
+
+1. **Polygon Utilities (`polygon_utils.hpp`)**  
+   This component provides functions for handling geometric polygons related to motion planning. It includes:
+
+   - Collision detection between trajectories and obstacles.
+   - Creation of polygons representing the vehicle's trajectory at different time steps.
+   - Geometric calculations using Boost Geometry.
+
+2. **General Utilities (`utils.hpp`)**  
+   This component provides various utility functions, including:
+
+   - Conversion between point representations (e.g., `pcl::PointXYZ` to `geometry_msgs::msg::Point`).
+   - Distance calculations between trajectory points and obstacles.
+   - Functions for concatenating vectors and processing trajectories.
+   - Visualization tools for creating markers.
+
+3. **Velocity Planning Results (`velocity_planning_result.hpp`)**  
+   This component defines data structures for storing the results of velocity planning, including:
+   - `SlowdownInterval`: Represents a segment where the vehicle should slow down, with specified start and end points and velocity.
+   - `VelocityPlanningResult`: Contains a collection of stop points, slowdown intervals, and optional velocity limits and clear commands.
+
+## Usage
+
+### Including the Package
+
+To use this package in your project, you need to include it in your CMakeLists.txt:
+
+```cmake
+find_package(autoware_motion_velocity_planner_common REQUIRED)
+```
+
+### Example Usage
+
+Here's a simple example demonstrating the use of the utility functions:
+
+```cpp
+#include "autoware/motion_velocity_planner_common/utils.hpp"
+
+const auto decimated_traj_points = autoware::motion_velocity_planner::utils::decimate_trajectory_points_from_ego(
+    traj_points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold,
+    p.decimate_trajectory_step_length, 0.0);
+```
+
+this example is from autoware_universe/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp

@@ -0,0 +1,69 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_
+#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_
+
+#include <autoware_utils/geometry/boost_geometry.hpp>
+
+#include <boost/geometry/algorithms/envelope.hpp>
+#include <boost/geometry/geometry.hpp>
+#include <boost/geometry/index/rtree.hpp>
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace autoware::motion_velocity_planner
+{
+namespace bgi = boost::geometry::index;
+using RtreeNode = std::pair<autoware_utils::Box2d, size_t>;
+using Rtree = bgi::rtree<RtreeNode, bgi::rstar<16>>;
+
+/// @brief collision as a trajectory index and corresponding collision points
+struct Collision
+{
+  size_t trajectory_index{};
+  autoware_utils::MultiPoint2d collision_points;
+};
+
+/// @brief collision checker for a trajectory as a sequence of 2D footprints
+class CollisionChecker
+{
+  const autoware_utils::MultiPolygon2d trajectory_footprints_;
+  std::shared_ptr<Rtree> rtree_;
+
+public:
+  /// @brief construct the collisions checker
+  /// @param trajectory_footprints footprints of the trajectory to be used for collision checking
+  /// @details internally, the rtree is built with the packing algorithm
+  explicit CollisionChecker(autoware_utils::MultiPolygon2d trajectory_footprints);
+
+  /// @brief efficiently calculate collisions with a geometry
+  /// @tparam Geometry boost geometry type
+  /// @param geometry geometry to check for collisions
+  /// @return collisions between the trajectory footprints and the input geometry
+  template <class Geometry>
+  [[nodiscard]] std::vector<Collision> get_collisions(const Geometry & geometry) const;
+
+  /// @brief direct access to the rtree storing the polygon footprints
+  /// @return rtree of the polygon footprints
+  [[nodiscard]] std::shared_ptr<const Rtree> get_rtree() const { return rtree_; }
+
+  /// @brief get the size of the trajectory used by this collision checker
+  [[nodiscard]] size_t trajectory_size() const { return trajectory_footprints_.size(); }
+};
+}  // namespace autoware::motion_velocity_planner
+
+#endif  // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp

@@ -0,0 +1,194 @@
+// Copyright 2024 Autoware Foundation
+//
+// 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_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_
+#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_
+
+#include <autoware/motion_utils/distance/distance.hpp>
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/motion_velocity_planner_common/collision_checker.hpp>
+#include <autoware/route_handler/route_handler.hpp>
+#include <autoware/velocity_smoother/smoother/smoother_base.hpp>
+#include <autoware_utils/geometry/boost_polygon_utils.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+
+#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_perception_msgs/msg/traffic_light_group.hpp>
+#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/header.hpp>
+
+#include <lanelet2_core/Forward.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <map>
+#include <memory>
+#include <optional>
+#include <vector>
+
+namespace autoware::motion_velocity_planner
+{
+using autoware_planning_msgs::msg::TrajectoryPoint;
+
+struct TrafficSignalStamped
+{
+  builtin_interfaces::msg::Time stamp;
+  autoware_perception_msgs::msg::TrafficLightGroup signal;
+};
+
+struct StopPoint
+{
+  double ego_trajectory_arc_length{};  // [m] arc length along the ego trajectory
+  geometry_msgs::msg::Pose
+    ego_stop_pose;                       // intersection between the trajectory and a map stop line
+  lanelet::BasicLineString2d stop_line;  // stop line from the map
+};
+struct TrajectoryPolygonCollisionCheck
+{
+  double decimate_trajectory_step_length;
+  double goal_extended_trajectory_length;
+  bool enable_to_consider_current_pose;
+  double time_to_convergence;
+};
+
+struct PlannerData
+{
+  explicit PlannerData(rclcpp::Node & node)
+  : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo())
+  {
+  }
+
+  class Object
+  {
+  public:
+    Object() = default;
+    explicit Object(const autoware_perception_msgs::msg::PredictedObject & arg_predicted_object)
+    : predicted_object(arg_predicted_object)
+    {
+    }
+
+    autoware_perception_msgs::msg::PredictedObject predicted_object;
+
+    double get_dist_to_traj_poly(
+      const std::vector<autoware_utils::Polygon2d> & decimated_traj_polys) const;
+    double get_dist_to_traj_lateral(const std::vector<TrajectoryPoint> & traj_points) const;
+    double get_dist_from_ego_longitudinal(
+      const std::vector<TrajectoryPoint> & traj_points,
+      const geometry_msgs::msg::Point & ego_pos) const;
+    double get_lon_vel_relative_to_traj(const std::vector<TrajectoryPoint> & traj_points) const;
+    double get_lat_vel_relative_to_traj(const std::vector<TrajectoryPoint> & traj_points) const;
+    geometry_msgs::msg::Pose get_predicted_pose(
+      const rclcpp::Time & current_stamp, const rclcpp::Time & predicted_object_stamp) const;
+
+  private:
+    void calc_vel_relative_to_traj(const std::vector<TrajectoryPoint> & traj_points) const;
+
+    mutable std::optional<double> dist_to_traj_poly{std::nullopt};
+    mutable std::optional<double> dist_to_traj_lateral{std::nullopt};
+    mutable std::optional<double> dist_from_ego_longitudinal{std::nullopt};
+    mutable std::optional<double> lon_vel_relative_to_traj{std::nullopt};
+    mutable std::optional<double> lat_vel_relative_to_traj{std::nullopt};
+    mutable std::optional<geometry_msgs::msg::Pose> predicted_pose;
+  };
+
+  struct Pointcloud
+  {
+  public:
+    Pointcloud() = default;
+    explicit Pointcloud(const pcl::PointCloud<pcl::PointXYZ> & arg_pointcloud)
+    : pointcloud(arg_pointcloud)
+    {
+    }
+
+    pcl::PointCloud<pcl::PointXYZ> pointcloud;
+
+  private:
+    // NOTE: clustered result will be added.
+  };
+
+  void process_predicted_objects(
+    const autoware_perception_msgs::msg::PredictedObjects & predicted_objects);
+
+  // msgs from callbacks that are used for data-ready
+  nav_msgs::msg::Odometry current_odometry;
+  geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration;
+  std_msgs::msg::Header predicted_objects_header;
+  std::vector<std::shared_ptr<Object>> objects;
+  Pointcloud no_ground_pointcloud;
+  nav_msgs::msg::OccupancyGrid occupancy_grid;
+  std::shared_ptr<route_handler::RouteHandler> route_handler;
+
+  // nearest search
+  double ego_nearest_dist_threshold{};
+  double ego_nearest_yaw_threshold{};
+
+  TrajectoryPolygonCollisionCheck trajectory_polygon_collision_check{};
+
+  // other internal data
+  // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the
+  // last observed infomation for UNKNOWN
+  std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_raw_;
+  std::map<lanelet::Id, TrafficSignalStamped> traffic_light_id_map_last_observed_;
+
+  // velocity smoother
+  std::shared_ptr<autoware::velocity_smoother::SmootherBase> velocity_smoother_;
+  // parameters
+  autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+
+  bool is_driving_forward{true};
+
+  /**
+   *@fn
+   *@brief queries the traffic signal information of given Id. if keep_last_observation = true,
+   *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation
+   */
+  [[nodiscard]] std::optional<TrafficSignalStamped> get_traffic_signal(
+    const lanelet::Id id, const bool keep_last_observation = false) const;
+
+  /// @brief calculate possible stop points along the current trajectory where it intersects with
+  /// stop lines
+  /// @param [in] trajectory ego trajectory
+  /// @return stop points taken from the map
+  [[nodiscard]] std::vector<StopPoint> calculate_map_stop_points(
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory) const;
+
+  /// @brief calculate the minimum distance needed by ego to decelerate to the given velocity
+  /// @param [in] target_velocity [m/s] target velocity
+  /// @return [m] distance needed to reach the target velocity
+  [[nodiscard]] std::optional<double> calculate_min_deceleration_distance(
+    const double target_velocity) const;
+
+  size_t find_index(
+    const std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & pose) const
+  {
+    return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
+      traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+  }
+
+  size_t find_segment_index(
+    const std::vector<TrajectoryPoint> & traj_points, const geometry_msgs::msg::Pose & pose) const
+  {
+    return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      traj_points, pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold);
+  }
+};
+}  // namespace autoware::motion_velocity_planner
+
+#endif  // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_