badai-nguyen
diff --git a/‎planning/obstacle_avoidance_planner/CMakeLists.txt
+55 b/‎planning/obstacle_avoidance_planner/CMakeLists.txt
+55
diff --git a/‎planning/obstacle_avoidance_planner/README.md
+384 b/‎planning/obstacle_avoidance_planner/README.md
+384
diff --git a/‎planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml
+99 b/‎planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml
+99
diff --git a/‎planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug.hpp
+105 b/‎planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/debug.hpp
+105
@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.5)
+project(obstacle_avoidance_planner)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
+
+ament_auto_add_library(obstacle_avoidance_planner SHARED
+  src/vehicle_model/vehicle_model_interface.cpp
+  src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
+  src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
+  src/util.cpp
+  src/debug.cpp
+  src/eb_path_optimizer.cpp
+  src/mpt_optimizer.cpp
+  src/process_cv.cpp
+  src/node.cpp
+)
+
+target_include_directories(obstacle_avoidance_planner
+  PUBLIC
+    ${OpenCV_INCLUDE_DIRS}
+)
+
+target_link_libraries(obstacle_avoidance_planner
+  ${OpenCV_LIBRARIES}
+)
+
+rclcpp_components_register_node(obstacle_avoidance_planner
+  PLUGIN "ObstacleAvoidancePlanner"
+  EXECUTABLE obstacle_avoidance_planner_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+    config
+    scripts
+)
@@ -0,0 +1,99 @@
+/**:
+  ros__parameters:
+    # trajectory total/fixing length
+    trajectory_length: 200 # total trajectory length[m]
+    forward_fixing_distance: 20.0 # forward fixing length from base_link[m]
+    backward_fixing_distance: 5.0 # backward fixing length from base_link[m]
+
+    # clearance(distance) when generating trajectory
+    clearance_from_road: 0.2 # clearance from road boundary[m]
+    clearance_from_object: 1.0 # clearance from object[m]
+    min_object_clearance_for_joint: 3.2 # minimum clearance from object[m] when judging is_including_only_smooth_range
+    extra_desired_clearance_from_road: 0.0 # extra desired clearance from road
+
+    # clearance for unique points
+    clearance_for_straight_line: 0.05 # minimum optimizing range around straight points
+    clearance_for_joint: 0.1 # minimum optimizing range around joint points
+    clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing
+    range_for_extend_joint: 0.1 # minimum optimizing range around joint points for extending
+    clearance_from_object_for_straight: 10.0 # minimum clearance from object when judging straight line
+
+    # avoiding param
+    max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects[m/s]
+    max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects[m/s]
+    center_line_width: 1.7 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. center line width around path points used for judging that object is required to avoid or not
+    acceleration_for_non_deceleration_range: 1.0 # assumed acceleration for calculating non deceleration range
+    avoiding_object_type: # avoiding object class
+      unknown: true
+      car: true
+      truck: true
+      bus: true
+      bicycle: true
+      motorbike: true
+      pedestrian: true
+      animal: true
+
+    # solving quadratic programming
+    qp_max_iteration: 10000 # max iteration when solving QP
+    qp_eps_abs: 1.0e-8 # eps abs when solving OSQP
+    qp_eps_rel: 1.0e-11 # eps rel when solving OSQP
+    qp_eps_abs_for_extending: 1.0e-8 # eps abs when solving OSQP for extending
+    qp_eps_rel_for_extending: 1.0e-9 # eps rel when solving OSQP for extending
+    qp_eps_abs_for_visualizing: 1.0e-6 # eps abs when solving OSQP for visualizing
+    qp_eps_rel_for_visualizing: 1.0e-8 # eps rel when solving OSQP for visualizing
+
+    # constrain space
+    is_getting_constraints_close2path_points: true # generate trajectory closer to path points
+    max_x_constrain_search_range: 0.0 # maximum x search range in local coordinate
+    coef_x_constrain_search_resolution: 1.0 # coef for fine sampling when exploring x direction
+    coef_y_constrain_search_resolution: 0.5 # coef for fine sampling when exploring y direction
+    keep_space_shape_x: 0.2 # keep space for x direction from base_link[m]
+    keep_space_shape_y: 0.1 # replaced with /vehicle_info/vehicle_width when is_using_vehicle_config is true. keep space for y direction from base_link[m]
+    max_lon_space_for_driveable_constraint: 0.5 # maximum lon(x) space when optimizing point[m]
+
+    is_publishing_clearance_map: true # publish clearance map as nav_msgs::OccupancyGrid
+    is_publishing_area_with_objects: true # publish occupancy map as nav_msgs::OccupancyGrid
+    enable_avoidance: true # enable avoidance function
+    is_using_vehicle_config: true # use vehicle config
+    num_sampling_points: 100 # number of optimizing points
+    num_joint_buffer_points: 3 # number of joint buffer points
+    num_joint_buffer_points_for_extending: 6 # number of joint buffer points for extending
+    num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx
+    num_fix_points_for_extending: 50 # number of fixing points when extending
+    delta_arc_length_for_optimization: 1.0 # delta arc length when optimizing[m]
+    delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory[m]
+    delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point[m]
+    delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0. delta yaw thres for closest point
+    delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point
+
+    # mpt param
+    # vehicle param for mpt
+    max_steer_deg: 40.0 # max steering angle [deg]
+    steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s]
+
+    # trajectory param for mpt
+    num_curvature_sampling_points: 5 # number of sampling points when calculating curvature
+    delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt[m]
+    forward_fixing_mpt_distance: 10 # forward fixing distance for MPT
+
+    # optimization param for mpt
+    is_hard_fixing_terminal_point: false # hard fixing terminal point
+    base_point_weight: 2000.0 # slack weight for lateral error around base_link
+    top_point_weight: 1000.0 # slack weight for lateral error around vehicle-top point
+    mid_point_weight: 1000.0 # slack weight for lateral error around the middle point
+    lat_error_weight: 10.0 # weight for lateral error
+    yaw_error_weight: 0.0 # weight for yaw error
+    steer_input_weight: 0.01 # weight for steering input
+    steer_rate_weight: 1.0 # weight for steering rate
+    steer_acc_weight: 0.000001 # weight for steering acceleration
+    terminal_lat_error_weight: 0.0 # weight for lateral error at terminal point
+    terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point
+    terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point
+    terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point
+    zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero
+
+    # replanning & trimming trajectory param outside algorithm
+    min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m]
+    min_delta_time_sec_for_replan: 1.0 # minimum delta time for replan[second]
+    max_dist_for_extending_end_point: 5.0 # minimum delta dist thres for extending last point[m]
+    distance_for_path_shape_change_detection: 2.0 # minimum delta dist thres for detecting path shape change
@@ -0,0 +1,105 @@
+// Copyright 2020 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_AVOIDANCE_PLANNER__DEBUG_HPP_
+#define OBSTACLE_AVOIDANCE_PLANNER__DEBUG_HPP_
+
+#include <opencv2/core.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <string>
+#include <vector>
+
+struct ConstrainRectangle;
+struct Bounds;
+struct DebugData;
+struct VehicleParam;
+
+namespace util
+{
+struct Footprint;
+}
+
+visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
+  const DebugData & debug_data,
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & optimized_points,
+  const VehicleParam & vehicle_param);
+
+geometry_msgs::msg::Pose getVirtualWallPose(
+  const geometry_msgs::msg::Pose & target_pose, const VehicleParam & vehicle_param);
+
+visualization_msgs::msg::MarkerArray getDebugPointsMarkers(
+  const std::vector<geometry_msgs::msg::Point> & interpolated_points,
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & optimized_points,
+  const std::vector<geometry_msgs::msg::Point> & straight_points,
+  const std::vector<geometry_msgs::msg::Pose> & fixed_points,
+  const std::vector<geometry_msgs::msg::Pose> & non_fixed_points);
+
+visualization_msgs::msg::MarkerArray getDebugConstrainMarkers(
+  const std::vector<ConstrainRectangle> & constrain_ranges, const std::string & ns);
+
+visualization_msgs::msg::MarkerArray getObjectsMarkerArray(
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
+  const std::string & ns, const double r, const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getRectanglesMarkerArray(
+  const std::vector<util::Footprint> & rects, const std::string & ns, const double r,
+  const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray(
+  const std::vector<util::Footprint> & rects, const std::string & ns, const double r,
+  const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getPointsMarkerArray(
+  const std::vector<geometry_msgs::msg::Pose> & points, const std::string & ns, const double r,
+  const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getPointsMarkerArray(
+  const std::vector<geometry_msgs::msg::Point> & points, const std::string & ns, const double r,
+  const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getPointsTextMarkerArray(
+  const std::vector<geometry_msgs::msg::Pose> & points, const std::string & ns, const double r,
+  const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getPointsTextMarkerArray(
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & points,
+  const std::string & ns, const double r, const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getBaseBoundsLineMarkerArray(
+  const std::vector<Bounds> & bounds, const std::vector<geometry_msgs::msg::Pose> & candidate_p0,
+  const std::string & ns, const double r, const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getTopBoundsLineMarkerArray(
+  const std::vector<Bounds> & bounds, const std::vector<geometry_msgs::msg::Pose> & candidate_p1,
+  const std::string & ns, const double r, const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getMidBoundsLineMarkerArray(
+  const std::vector<Bounds> & bounds, const std::vector<geometry_msgs::msg::Pose> & candidate_top,
+  const std::string & ns, const double r, const double g, const double b);
+
+visualization_msgs::msg::MarkerArray getVirtualWallMarkerArray(
+  const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g,
+  const double b);
+
+visualization_msgs::msg::MarkerArray getVirtualWallTextMarkerArray(
+  const geometry_msgs::msg::Pose & pose, const std::string & ns, const double r, const double g,
+  const double b);
+
+nav_msgs::msg::OccupancyGrid getDebugCostmap(
+  const cv::Mat & clearance_map, const nav_msgs::msg::OccupancyGrid & occupancy_grid);
+#endif  // OBSTACLE_AVOIDANCE_PLANNER__DEBUG_HPP_