feat add surround obstacle checker package (autowarefoundation#42)

* release v0.4.0 * fix bug(return true in getPose) (autowarefoundation#686) * Feature/stop reason (autowarefoundation#712) * add stop reason msg * add mock of stop resaon publisher * change namespace of stop reason * update stop reason msg * add toRosPoint * implement stop reason publisher of blind stop * implement stop reason publisher of crosswalk * implement stop reason publisher of intersection * implement stop reason publisher of stop line * implement stop reason publisher of trafficlight * implement stop reason publisher of detection area * fix bug * remove unnecessary process * add remained stop factor * clean code * fix bug * not punlish stop reason if array size is 0 * add stop reason to stuck object in intersection * add stop factor of obstacle stop planner * add stop reason of surround_obstacle checker * Apply review Signed-off-by: Kenji Miyake <[email protected]> * fix message type * delete unused message from cmake * remove stopReasonStamped * change topic name of stop reasons Co-authored-by: Kenji Miyake <[email protected]> * Fix/stop reason (autowarefoundation#724) * input stop reason of traffic light * add comment * add empty traffic light handling * change calculation method of traffic light position * avoid 0 position output * Add surround obstacle state (autowarefoundation#785) * Cleanup code Signed-off-by: Kenji Miyake <[email protected]> * Add state to surround_obstacle_checker Signed-off-by: Kenji Miyake <[email protected]> * Guard pushObstaclePoint for StopFactor Signed-off-by: Kenji Miyake <[email protected]> * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <[email protected]> * Revert "remove ROS1 packages temporarily" This reverts commit 623dc7f7b59bb12639c5ca768f87b8e2d24c19b7. Signed-off-by: mitsudome-r <[email protected]> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <[email protected]> * Rename launch files to launch.xml (autowarefoundation#28) * Surround obstacle checker (autowarefoundation#64) * Port surround_obstacle_checker to ROS2 * Reviewer comment * Review comment * Fix launch files (autowarefoundation#122) * [surround_obstacle_checker] add parameter and arguments to launch file Signed-off-by: mitsudome-r <[email protected]> * [obstacle_stop_planner] modify launch file to remap trajectory from argument Signed-off-by: mitsudome-r <[email protected]> * [obstacle_avoidance_planner] modify launch file to remap topics from arguments Signed-off-by: mitsudome-r <[email protected]> * [motion_velocity_optimizer] modify launch file to enable remapping from argument Signed-off-by: mitsudome-r <[email protected]> * Convert calls of Duration to Duration::from_seconds where appropriate (autowarefoundation#131) * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Fix lints in surround_obstacle_checker package (autowarefoundation#130) * Replace waitForTransform by lookupTransform in surround_obstacle_checker (autowarefoundation#212) * Fix erroneous waitForTransform in surround_obstacle_checker * Line length fix * Ros2 v0.8.0 surround obstacle checker (autowarefoundation#328) * Rename ROS-related .yaml to .param.yaml (autowarefoundation#352) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake <[email protected]> * Remove prefix 'default_' of yaml files Signed-off-by: Kenji Miyake <[email protected]> * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake <[email protected]> * Rename diagnostic_aggregator's param files Signed-off-by: Kenji Miyake <[email protected]> * Fix overlooked parameters Signed-off-by: Kenji Miyake <[email protected]> * rename vehicle_info_param to vehicle_param_file (autowarefoundation#353) Signed-off-by: Kosuke Murakami <[email protected]> * Sensor data qos (autowarefoundation#407) * Use sensor data qos for pointcloud preprocessor Signed-off-by: Autoware <[email protected]> * Use sensor data qos for pointcloud Signed-off-by: Autoware <[email protected]> * Fix lint Signed-off-by: wep21 <[email protected]> * Use sensor data qos for livox tag filter and vector map filter Signed-off-by: wep21 <[email protected]> * Fix lint Signed-off-by: wep21 <[email protected]> Co-authored-by: Autoware <[email protected]> * Ros2 fix topic name part1 (autowarefoundation#408) * Fix topic name of lane_departure_checker debug Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of mpc_follower debug Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of velocity_controller debug Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of motion_velocity_optimizer debug Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of lane_change_planner debug Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of behavior_velocity_planner debug Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of obstacle_avoidance_planner debug Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of motion_velocity_optimizer Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of lane_departure_checker Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of mpc_follower Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of behavior_velocity_planner Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of velocity_controller Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of lane_change_planner Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of obstacle_avoidance_planner Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of obstacle_stop_planner Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of freespace_planner Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of surround_obstacle_checker Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of costmap_generator Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of emergency_handler Signed-off-by: Takagi, Isamu <[email protected]> * Fix lint errors Signed-off-by: Takagi, Isamu <[email protected]> * Fix typo Signed-off-by: Takagi, Isamu <[email protected]> * add use_sim-time option (autowarefoundation#454) * Fix rolling build errors (autowarefoundation#1225) * Add missing include files Signed-off-by: Kenji Miyake <[email protected]> * Replace rclcpp::Duration Signed-off-by: Kenji Miyake <[email protected]> * Use reference for exceptions Signed-off-by: Kenji Miyake <[email protected]> * Use from_seconds Signed-off-by: Kenji Miyake <[email protected]> * Sync public repo (autowarefoundation#1228) * [simple_planning_simulator] add readme (autowarefoundation#424) * add readme of simple_planning_simulator Signed-off-by: Takamasa Horibe <[email protected]> * Update simulator/simple_planning_simulator/README.md * set transit_margin_time to intersect. planner (autowarefoundation#460) * Fix pose2twist (autowarefoundation#462) Signed-off-by: Takagi, Isamu <[email protected]> * Ros2 vehicle info param server (autowarefoundation#447) * add vehicle_info_param_server * update vehicle info * apply format * fix bug * skip unnecessary search * delete vehicle param file * fix bug * Ros2 fix topic name part2 (autowarefoundation#425) * Fix topic name of traffic_light_classifier Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of traffic_light_visualization Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <[email protected]> * Fix topic name of traffic_light_map_based_detector Signed-off-by: Takagi, Isamu <[email protected]> * Fix lint traffic_light_recognition Signed-off-by: Takagi, Isamu <[email protected]> * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <[email protected]> * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu <[email protected]> * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu <[email protected]> * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <[email protected]> * Fix issues in hdd_reader (autowarefoundation#466) * Fix some issues detected by Coverity Scan and Clang-Tidy * Update launch command * Add more `close(new_sock)` * Simplify the definitions of struct * fix: re-construct laneletMapLayer for reindex RTree (autowarefoundation#463) * Rviz overlay render fix (autowarefoundation#461) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski <[email protected]> * super class now back to MFD Signed-off-by: Adam Dabrowski <[email protected]> * uncrustified Signed-off-by: Adam Dabrowski <[email protected]> * acquire data in mutex Signed-off-by: Adam Dabrowski <[email protected]> * back to RTD as superclass Signed-off-by: Adam Dabrowski <[email protected]> * Rviz overlay render in update (autowarefoundation#465) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski <[email protected]> * super class now back to MFD Signed-off-by: Adam Dabrowski <[email protected]> * uncrustified Signed-off-by: Adam Dabrowski <[email protected]> * acquire data in mutex Signed-off-by: Adam Dabrowski <[email protected]> * removed unnecessary includes and some dead code Signed-off-by: Adam Dabrowski <[email protected]> * Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass Signed-off-by: Adam Dabrowski <[email protected]> * restored RTD superclass Signed-off-by: Adam Dabrowski <[email protected]> Co-authored-by: Takamasa Horibe <[email protected]> Co-authored-by: tkimura4 <[email protected]> Co-authored-by: Takagi, Isamu <[email protected]> Co-authored-by: Kazuki Miyahara <[email protected]> Co-authored-by: Makoto Tokunaga <[email protected]> Co-authored-by: Adam Dąbrowski <[email protected]> * Unify Apache-2.0 license name (autowarefoundation#1242) * Make planning modules components (autowarefoundation#1263) Signed-off-by: wep21 <[email protected]> * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <[email protected]> * Refactor vehicle info util (autowarefoundation#1305) * Update license Signed-off-by: Kenji Miyake <[email protected]> * Refactor vehicle_info_util Signed-off-by: Kenji Miyake <[email protected]> * Rename and split files Signed-off-by: Kenji Miyake <[email protected]> * Fix interfaces Signed-off-by: Kenji Miyake <[email protected]> * Fix bug and add error handling Signed-off-by: Kenji Miyake <[email protected]> * Add "// namespace" Signed-off-by: Kenji Miyake <[email protected]> * Add missing include Signed-off-by: Kenji Miyake <[email protected]> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <[email protected]> * Fix mistake Signed-off-by: Kenji Miyake <[email protected]> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <[email protected]> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <[email protected]> Co-authored-by: Hiroki OTA <[email protected]> * Add surround_obstacle_checker document (autowarefoundation#1895) * Add surround_obstacle_checker document * Remove unnecessary unit * Fix stop condition * Fix japanese doc and add english doc * Fix typo * Add detail information Co-authored-by: taikitanaka3 <[email protected]> * Fix figure files and explainations * Add known limits Co-authored-by: taikitanaka3 <[email protected]> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <[email protected]> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <[email protected]> * Apply Black Signed-off-by: Kenji Miyake <[email protected]> * Apply clang-format Signed-off-by: Kenji Miyake <[email protected]> * Fix build errors Signed-off-by: Kenji Miyake <[email protected]> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <[email protected]> * Apply clang-format Signed-off-by: Kenji Miyake <[email protected]> * Fix build errors Signed-off-by: Kenji Miyake <[email protected]> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <[email protected]> * port surround obstacle checker (autowarefoundation#493) * change trajectory msg * change dynamicobject to predictedobject * run pre-commit * delete colcon ignore * deal with non polygon shape * replace twist with odometry * rename to README.md (autowarefoundation#550) * rename to README.md * dealt with new auto_msgs format Co-authored-by: Takayuki Murooka <[email protected]> * [apply_predicted_obj_type] adapt to autoware auto msgs (autowarefoundation#564) * fix obj shape * fix obj shape * fix goal pose * rename topic name twist -> odometry (autowarefoundation#568) Co-authored-by: Takayuki Murooka <[email protected]> * update iv_msgs -> auto_msgs in planning readme (autowarefoundation#576) * update iv_msgs -> auto_msgs in planning readme * minor change * some fix * some fix Co-authored-by: Takayuki Murooka <[email protected]> * Auto/fix surround obstacle checker (autowarefoundation#684) * change trajectory to trajectorypoints in function * add header to output * use output_trajectory_points Co-authored-by: tomoya.kimura <[email protected]> * Fix no ground pointcloud topic name (autowarefoundation#733) Signed-off-by: j4tfwm6z <[email protected]> Co-authored-by: j4tfwm6z <[email protected]> * fix/rename segmentation namespace (autowarefoundation#742) * rename segmentation directory * fix namespace: system stack * fix namespace: planning * fix namespace: control stack * fix namespace: perception stack * fix readme * ci(pre-commit): autofix Co-authored-by: mitsudome-r <[email protected]> Co-authored-by: tkimura4 <[email protected]> Co-authored-by: Kenji Miyake <[email protected]> Co-authored-by: Kenji Miyake <[email protected]> Co-authored-by: Nikolai Morin <[email protected]> Co-authored-by: Ryohsuke Mitsudome <[email protected]> Co-authored-by: Daisuke Nishimatsu <[email protected]> Co-authored-by: Kosuke Murakami <[email protected]> Co-authored-by: Autoware <[email protected]> Co-authored-by: Takagi, Isamu <[email protected]> Co-authored-by: Takamasa Horibe <[email protected]> Co-authored-by: Kazuki Miyahara <[email protected]> Co-authored-by: Makoto Tokunaga <[email protected]> Co-authored-by: Adam Dąbrowski <[email protected]> Co-authored-by: Hiroki OTA <[email protected]> Co-authored-by: Fumiya Watanabe <[email protected]> Co-authored-by: Keisuke Shima <[email protected]> Co-authored-by: Sugatyon <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]> Co-authored-by: j4tfwm6z <[email protected]> Co-authored-by: Satoshi OTA <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
badai-nguyen · Dec 3, 2021 · f4d472c · f4d472c
1 parent 021b448
commit f4d472c
Show file tree

Hide file tree

Showing 13 changed files with 1,176 additions and 0 deletions.
diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt
@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.5)
+project(surround_obstacle_checker)
+
+### Compile options
+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)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED COMPONENTS common)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/debug_marker.cpp
+  src/node.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "SurroundObstacleCheckerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)
diff --git a/planning/surround_obstacle_checker/README.md b/planning/surround_obstacle_checker/README.md
@@ -0,0 +1,82 @@
+# Surround Obstacle Checker
+
+## Purpose
+
+`surround_obstacle_checker` is a module to prevent moving if any obstacles is near stopping ego vehicle.
+This module runs only when ego vehicle is stopping.
+
+## Inner-workings / Algorithms
+
+### Flow chart
+
+![surround_obstacle_checker_flow](./media/surround_obstacle_checker_flow.svg)
+
+![check_distance](./media/check_distance.drawio.svg)
+
+### Algorithms
+
+### Check data
+
+Check that `surround_obstacle_checker` receives no ground pointcloud, dynamic objects and current velocity data.
+
+### Get distance to nearest object
+
+Calculate distance between ego vehicle and the nearest object.
+In this function, it calculates the minimum distance between the polygon of ego vehicle and all points in pointclouds and the polygons of dynamic objects.
+
+### Stop requirement
+
+If it satisfies all following conditions, it plans stopping.
+
+- Ego vehicle is stopped
+- It satisfies any following conditions
+  1. The distance to nearest obstacle satisfies following conditions
+     - If state is `State::PASS`, the distance is less than `surround_check_distance`
+     - If state is `State::STOP`, the distance is less than `surround_check_recover_distance`
+  2. If it does not satisfies the condition in 1, elapsed time from the time it satisfies the condition in 1 is less than `state_clear_time`
+
+### States
+
+To prevent chattering, `surround_obstacle_checker` manages two states.
+As mentioned in stop condition section, it prevents chattering by changing threshold to find surround obstacle depending on the states.
+
+- `State::PASS` : Stop planning is released
+- `State::STOP` ：While stop planning
+
+## Inputs / Outputs
+
+### Input
+
+| Name                                           | Type                                                   | Description                                                        |
+| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ |
+| `~/input/trajectory`                           | `autoware_auto_planning_msgs::msg::Trajectory`         | Reference trajectory                                               |
+| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2`                        | Pointcloud of obstacles which the ego-vehicle should stop or avoid |
+| `/perception/object_recognition/objects`       | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects                                                    |
+| `/localization/kinematic_state`                | `nav_msgs::msg::Odometry`                              | Current twist                                                      |
+| `/tf`                                          | `tf2_msgs::msg::TFMessage`                             | TF                                                                 |
+| `/tf_static`                                   | `tf2_msgs::msg::TFMessage`                             | TF static                                                          |
+
+### Output
+
+| Name                       | Type                                           | Description              |
+| -------------------------- | ---------------------------------------------- | ------------------------ |
+| `~/output/trajectory`      | `autoware_auto_planning_msgs/Trajectory`       | Modified trajectory      |
+| `~/output/no_start_reason` | `diagnostic_msgs::msg::DiagnosticStatus`       | No start reason          |
+| `~/output/stop_reasons`    | `autoware_planning_msgs::msg::StopReasonArray` | Stop reasons             |
+| `~/debug/marker`           | `visualization_msgs::msg::MarkerArray`         | Marker for visualization |
+
+## Parameters
+
+| Name                              | Type     | Description                                                                            | Default value |
+| :-------------------------------- | :------- | :------------------------------------------------------------------------------------- | :------------ |
+| `use_pointcloud`                  | `bool`   | Use pointcloud as obstacle check                                                       | `true`        |
+| `use_dynamic_object`              | `bool`   | Use dynamic object as obstacle check                                                   | `true`        |
+| `surround_check_distance`         | `double` | If objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]  | 0.5           |
+| `surround_check_recover_distance` | `double` | If no object exists in this distance, transit to "non-surrounding-obstacle" status [m] | 0.8           |
+| `state_clear_time`                | `double` | Threshold to clear stop state [s]                                                      | 2.0           |
+| `stop_state_ego_speed`            | `double` | Threshold to check ego vehicle stopped [m/s]                                           | 0.1           |
+
+## Assumptions / Known limits
+
+To perform stop planning, it is necessary to get obstacle pointclouds data.
+Hence, it does not plan stopping if the obstacle is in blind spot.
diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml
@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    # obstacle check
+    use_pointcloud: true # use pointcloud as obstacle check
+    use_dynamic_object: true # use dynamic object as obstacle check
+    surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]
+    surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m]
+    state_clear_time: 2.0
+
+    # ego stop state
+    stop_state_ego_speed: 0.1 #[m/s]
diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp
@@ -0,0 +1,53 @@
+// 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 SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_
+#define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_planning_msgs/msg/stop_reason_array.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <memory>
+#include <string>
+
+enum class PoseType : int8_t { NoStart = 0 };
+enum class PointType : int8_t { NoStart = 0 };
+class SurroundObstacleCheckerDebugNode
+{
+public:
+  explicit SurroundObstacleCheckerDebugNode(
+    const double base_link2front, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node);
+
+  bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type);
+  bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type);
+  void publish();
+
+private:
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
+  rclcpp::Publisher<autoware_planning_msgs::msg::StopReasonArray>::SharedPtr stop_reason_pub_;
+  double base_link2front_;
+
+  visualization_msgs::msg::MarkerArray makeVisualizationMarker();
+  autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray();
+
+  std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;
+  std::shared_ptr<geometry_msgs::msg::Pose> stop_pose_ptr_;
+  rclcpp::Clock::SharedPtr clock_;
+};
+
+#endif  // SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_
diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
@@ -0,0 +1,128 @@
+// 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 SURROUND_OBSTACLE_CHECKER__NODE_HPP_
+#define SURROUND_OBSTACLE_CHECKER__NODE_HPP_
+
+#include "autoware_utils/trajectory/tmp_conversion.hpp"
+#include "surround_obstacle_checker/debug_marker.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <diagnostic_msgs/msg/key_value.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <boost/assert.hpp>
+#include <boost/assign/list_of.hpp>
+#include <boost/format.hpp>
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/linestring.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <tf2/utils.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+using Point2d = boost::geometry::model::d2::point_xy<double>;
+using Polygon2d =
+  boost::geometry::model::polygon<Point2d, false, false>;  // counter-clockwise, open
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
+enum class State { PASS, STOP };
+
+class SurroundObstacleCheckerNode : public rclcpp::Node
+{
+public:
+  explicit SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void pathCallback(const Trajectory::ConstSharedPtr input_msg);
+  void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg);
+  void dynamicObjectCallback(
+    const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_msg);
+  void currentVelocityCallback(const nav_msgs::msg::Odometry::ConstSharedPtr input_msg);
+  void insertStopVelocity(const size_t closest_idx, TrajectoryPoints * traj);
+  bool convertPose(
+    const geometry_msgs::msg::Pose & pose, const std::string & source, const std::string & target,
+    const rclcpp::Time & time, geometry_msgs::msg::Pose & conv_pose);
+  bool getPose(
+    const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose);
+  void getNearestObstacle(double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
+  void getNearestObstacleByPointCloud(
+    double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
+  void getNearestObstacleByDynamicObject(
+    double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point);
+  bool isObstacleFound(const double min_dist_to_obj);
+  bool isStopRequired(const bool is_obstacle_found, const bool is_stopped);
+  size_t getClosestIdx(const TrajectoryPoints & traj, const geometry_msgs::msg::Pose current_pose);
+  bool checkStop(const TrajectoryPoint & closest_point);
+  Polygon2d createSelfPolygon();
+  Polygon2d createObjPolygon(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size);
+  Polygon2d createObjPolygon(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint);
+  diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag(
+    const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose);
+  std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose);
+
+  /*
+   * ROS
+   */
+  // publisher and subscriber
+  rclcpp::Subscription<Trajectory>::SharedPtr path_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr
+    dynamic_object_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr current_velocity_sub_;
+  rclcpp::Publisher<Trajectory>::SharedPtr path_pub_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr stop_reason_diag_pub_;
+  std::shared_ptr<SurroundObstacleCheckerDebugNode> debug_ptr_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  // parameter
+  nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_;
+  sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_;
+  autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr_;
+  vehicle_info_util::VehicleInfo vehicle_info_;
+  Polygon2d self_poly_;
+  bool use_pointcloud_;
+  bool use_dynamic_object_;
+  double surround_check_distance_;
+  // For preventing chattering,
+  // surround_check_recover_distance_ must be  bigger than surround_check_distance_
+  double surround_check_recover_distance_;
+  double state_clear_time_;
+  double stop_state_ego_speed_;
+  bool is_surround_obstacle_;
+
+  // State Machine
+  State state_ = State::PASS;
+  std::shared_ptr<const rclcpp::Time> last_obstacle_found_time_;
+};
+
+#endif  // SURROUND_OBSTACLE_CHECKER__NODE_HPP_
diff --git a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml
@@ -0,0 +1,21 @@
+<launch>
+  <arg name="param_path" default="$(find-pkg-share surround_obstacle_checker)/config/surround_obstacle_checker.param.yaml" />
+
+  <arg name="input_trajectory" default="input/trajectory" />
+  <arg name="input_objects" default="/perception/object_recognition/objects" />
+  <arg name="input_odometry" default="/localization/kinematic_state" />
+  <arg name="input_pointcloud" default="/perception/obstacle_segmentation/pointcloud" />
+  <arg name="output_trajectory" default="output/trajectory" />
+
+  <node pkg="surround_obstacle_checker" exec="surround_obstacle_checker_node" name="surround_obstacle_checker" output="screen">
+    <param from="$(var param_path)" />
+    <remap from="~/output/no_start_reason" to="/planning/scenario_planning/status/no_start_reason" />
+    <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons" />
+    <remap from="~/output/trajectory" to="$(var output_trajectory)" />
+    <remap from="~/input/pointcloud" to="$(var input_pointcloud)" />
+    <remap from="~/input/objects" to="$(var input_objects)" />
+    <remap from="~/input/odometry" to="$(var input_odometry)" />
+    <remap from="~/input/trajectory" to="$(var input_trajectory)" />
+  </node>
+
+</launch>
diff --git a/planning/surround_obstacle_checker/media/check_distance.drawio.svg b/planning/surround_obstacle_checker/media/check_distance.drawio.svg