feat(obstacle_velocity_planner): add obstacle_velocity_planner from .iv's development

planning/obstacle_velocity_planner/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(obstacle_velocity_planner)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Eigen3 REQUIRED)
+
+ament_auto_add_library(obstacle_velocity_planner_core SHARED
+  src/node.cpp
+  src/resample.cpp
+  src/box2d.cpp
+  src/velocity_optimizer.cpp
+)
+
+rclcpp_components_register_node(obstacle_velocity_planner_core
+  PLUGIN "ObstacleVelocityPlanner"
+  EXECUTABLE obstacle_velocity_planner
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)
+
+install(PROGRAMS
+  scripts/trajectory_visualizer.py
+  DESTINATION lib/${PROJECT_NAME}
+)

planning/obstacle_velocity_planner/config/obstacle_velocity_planner.param.yaml

@@ -0,0 +1,49 @@
+/**:
+  ros__parameters:
+    resampling_s_interval: 2.0
+    dense_resampling_time_interval: 0.1
+    sparse_resampling_time_interval: 0.5
+    dense_time_horizon: 5.0
+    max_time_horizon: 15.0
+    max_trajectory_length: 200.0
+    max_accel:  1.0
+    min_accel: -1.0
+    max_jerk:  1.0
+    min_jerk: -0.5
+    min_object_accel: -1.0
+    initial_velocity_margin: 0.1 #[m/s]
+
+    # Parameters for safe distance
+    safe_distance_margin: 5.0
+    t_dangerous: 0.5
+    t_idling: 2.0
+
+    # Parameters for collision detection
+    collision_time_threshold: 10.0
+    object_zero_velocity_threshold: 3.0 #[m/s]
+    object_low_velocity_threshold: 3.0 #[m/s]
+    external_velocity_limit: 20.0 #[m/s]
+    delta_yaw_threshold_of_object_and_ego: 90.0 # [deg]
+    delta_yaw_threshold_of_nearest_index: 60.0 # [deg]
+    collision_duration_threshold_of_object_and_ego: 1.0 # [s]
+
+    # Switch for each functions
+    enable_adaptive_cruise: true
+    use_object_acceleration: false
+    use_hd_map: true
+
+    # For initial Motion
+    replan_vel_deviation: 5.0         # velocity deviation to replan initial velocity [m/s]
+    engage_velocity: 0.25              # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
+    engage_acceleration: 0.1           # engage acceleration [m/ss] (use this acceleration when engagement)
+    engage_exit_ratio: 0.5             # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
+    stop_dist_to_prohibit_engage: 0.5  # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
+
+    # Weights for optimization
+    max_s_weight: 100.0
+    max_v_weight: 1.0
+    over_s_safety_weight:  1000000.0
+    over_s_ideal_weight:  50.0
+    over_v_weight:  500000.0
+    over_a_weight:  5000.0
+    over_j_weight:  10000.0

planning/obstacle_velocity_planner/include/obstacle_velocity_planner/box2d.hpp

@@ -0,0 +1,135 @@
+// 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 OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_
+
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <cmath>
+#include <limits>
+#include <vector>
+
+class Box2d
+{
+public:
+  Box2d(const geometry_msgs::msg::Pose & center_pose, const double length, const double width);
+
+  bool hasOverlap(const Box2d & box) const;
+
+  void initCorners();
+
+  /**
+   * @brief Getter of the center of the box
+   * @return The center of the box
+   */
+  const geometry_msgs::msg::Point & center() const { return center_; }
+
+  /**
+   * @brief Getter of the x-coordinate of the center of the box
+   * @return The x-coordinate of the center of the box
+   */
+  double getCenterX() const { return center_.x; }
+
+  /**
+   * @brief Getter of the y-coordinate of the center of the box
+   * @return The y-coordinate of the center of the box
+   */
+  double getCenterY() const { return center_.y; }
+
+  /**
+   * @brief Getter of the length
+   * @return The length of the heading-axis
+   */
+  double length() const { return length_; }
+
+  /**
+   * @brief Getter of the width
+   * @return The width of the box taken perpendicularly to the heading
+   */
+  double width() const { return width_; }
+
+  /**
+   * @brief Getter of half the length
+   * @return Half the length of the heading-axis
+   */
+  double getHalfLength() const { return half_length_; }
+
+  /**
+   * @brief Getter of half the width
+   * @return Half the width of the box taken perpendicularly to the heading
+   */
+  double getHalfWidth() const { return half_width_; }
+
+  /**
+   * @brief Getter of the heading
+   * @return The counter-clockwise angle between the x-axis and the heading-axis
+   */
+  double heading() const { return heading_; }
+
+  /**
+   * @brief Getter of the cosine of the heading
+   * @return The cosine of the heading
+   */
+  double getCosHeading() const { return cos_heading_; }
+
+  /**
+   * @brief Getter of the sine of the heading
+   * @return The sine of the heading
+   */
+  double getSinHeading() const { return sin_heading_; }
+
+  /**
+   * @brief Getter of the area of the box
+   * @return The product of its length and width
+   */
+  double area() const { return length_ * width_; }
+
+  /**
+   * @brief Getter of the size of the diagonal of the box
+   * @return The diagonal size of the box
+   */
+  double diagonal() const { return std::hypot(length_, width_); }
+
+  /**
+   * @brief Getter of the corners of the box
+   * @param corners The vector where the corners are listed
+   */
+  std::vector<geometry_msgs::msg::Point> getAllCorners() const { return corners_; }
+
+  double getMaxX() const { return max_x_; }
+  double getMinX() const { return min_x_; }
+  double getMaxY() const { return max_y_; }
+  double getMinY() const { return min_y_; }
+
+private:
+  geometry_msgs::msg::Point center_;
+  double length_ = 0.0;
+  double width_ = 0.0;
+  double half_length_ = 0.0;
+  double half_width_ = 0.0;
+  double heading_ = 0.0;
+  double cos_heading_ = 1.0;
+  double sin_heading_ = 0.0;
+
+  std::vector<geometry_msgs::msg::Point> corners_;
+
+  double max_x_ = std::numeric_limits<double>::lowest();
+  double min_x_ = std::numeric_limits<double>::max();
+  double max_y_ = std::numeric_limits<double>::lowest();
+  double min_y_ = std::numeric_limits<double>::max();
+};
+
+#endif  // OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_

planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/s_boundary.hpp

@@ -0,0 +1,33 @@
+// 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 OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_
+
+#include <limits>
+#include <vector>
+
+class SBoundary
+{
+public:
+  SBoundary(const double _max_s, const double _min_s) : max_s(_max_s), min_s(_min_s) {}
+  SBoundary() : max_s(std::numeric_limits<double>::max()), min_s(0.0) {}
+
+  double max_s = std::numeric_limits<double>::max();
+  double min_s = 0.0;
+  bool is_object = false;
+};
+
+using SBoundaries = std::vector<SBoundary>;
+
+#endif  // OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_

planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common/st_point.hpp

@@ -0,0 +1,31 @@
+// 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 OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_
+#define OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_
+
+#include <vector>
+
+class STPoint
+{
+public:
+  STPoint(const double _s, const double _t) : s(_s), t(_t) {}
+  STPoint() : s(0.0), t(0.0) {}
+
+  double s;
+  double t;
+};
+
+using STPoints = std::vector<STPoint>;
+
+#endif  // OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_