From c9e94d70ddcb020623aa51ae8fc31fd1d1b9effb Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Sat, 9 Sep 2023 14:14:14 +0900
Subject: [PATCH] feat(surround_obstacle_checker): make parameters more tunable
 (#4925)

* feat(surround_obstacle_checker): make parameters more tunable

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update param

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 .../surround_obstacle_checker.param.yaml      |  59 +++++-
 .../debug_marker.hpp                          |  24 ++-
 .../surround_obstacle_checker/node.hpp        |  25 ++-
 .../src/debug_marker.cpp                      |  86 +++++----
 .../surround_obstacle_checker/src/node.cpp    | 180 ++++++++++++++----
 5 files changed, 282 insertions(+), 92 deletions(-)

diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml
index 10b5c2814e2a8..5ec10572ffe83 100644
--- a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml
+++ b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml
@@ -1,12 +1,61 @@
 /**:
   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]
+    # surround_check_*_distance: if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m]
+    # surround_check_hysteresis_distance: if no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m]
+    pointcloud:
+      enable_check: false
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.5
+      surround_check_back_distance: 0.5
+    unknown:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.5
+      surround_check_back_distance: 0.5
+    car:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    truck:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    bus:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    trailer:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    motorcycle:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.0
+      surround_check_back_distance: 0.5
+    bicycle:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.5
+      surround_check_back_distance: 0.5
+    pedestrian:
+      enable_check: true
+      surround_check_front_distance: 0.5
+      surround_check_side_distance: 0.5
+      surround_check_back_distance: 0.5
+
+    surround_check_hysteresis_distance: 0.3
+
     state_clear_time: 2.0
 
+    # ego stop state
+    stop_state_ego_speed: 0.1 #[m/s]
+
     # debug
     publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets
+    debug_footprint_label: "car"
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
index bd0fe48bbda28..590f026942801 100644
--- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp
+++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp
@@ -16,6 +16,7 @@
 #define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <geometry_msgs/msg/polygon_stamped.hpp>
@@ -38,6 +39,7 @@ using geometry_msgs::msg::PolygonStamped;
 using tier4_planning_msgs::msg::StopFactor;
 using tier4_planning_msgs::msg::StopReason;
 using tier4_planning_msgs::msg::StopReasonArray;
+using vehicle_info_util::VehicleInfo;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
@@ -52,15 +54,19 @@ class SurroundObstacleCheckerDebugNode
 {
 public:
   explicit SurroundObstacleCheckerDebugNode(
-    const Polygon2d & ego_polygon, const double base_link2front,
-    const double & surround_check_distance, const double & surround_check_recover_distance,
-    const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock,
-    rclcpp::Node & node);
+    const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front,
+    const std::string & object_label, const double & surround_check_front_distance,
+    const double & surround_check_side_distance, const double & surround_check_back_distance,
+    const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose,
+    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();
   void publishFootprints();
+  void updateFootprintMargin(
+    const std::string & object_label, const double front_distance, const double side_distance,
+    const double back_distance);
 
 private:
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_virtual_wall_pub_;
@@ -72,10 +78,13 @@ class SurroundObstacleCheckerDebugNode
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_offset_pub_;
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_recover_offset_pub_;
 
-  Polygon2d ego_polygon_;
+  vehicle_info_util::VehicleInfo vehicle_info_;
   double base_link2front_;
-  double surround_check_distance_;
-  double surround_check_recover_distance_;
+  std::string object_label_;
+  double surround_check_front_distance_;
+  double surround_check_side_distance_;
+  double surround_check_back_distance_;
+  double surround_check_hysteresis_distance_;
   geometry_msgs::msg::Pose self_pose_;
 
   MarkerArray makeVirtualWallMarker();
@@ -83,7 +92,6 @@ class SurroundObstacleCheckerDebugNode
   StopReasonArray makeStopReasonArray();
   VelocityFactorArray makeVelocityFactorArray();
 
-  Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset);
   PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z);
 
   std::shared_ptr<geometry_msgs::msg::Point> stop_obstacle_point_ptr_;
diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
index de3e4bf120b13..a7c177f202173 100644
--- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
+++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp
@@ -38,6 +38,7 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -62,18 +63,27 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
 
   struct NodeParam
   {
-    bool use_pointcloud;
-    bool use_dynamic_object;
     bool is_surround_obstacle;
-    // For preventing chattering,
-    // surround_check_recover_distance_ must be  bigger than surround_check_distance_
-    double surround_check_recover_distance;
-    double surround_check_distance;
+    std::unordered_map<int, bool> enable_check_map;
+    std::unordered_map<int, double> surround_check_front_distance_map;
+    std::unordered_map<int, double> surround_check_side_distance_map;
+    std::unordered_map<int, double> surround_check_back_distance_map;
+    bool pointcloud_enable_check;
+    double pointcloud_surround_check_front_distance;
+    double pointcloud_surround_check_side_distance;
+    double pointcloud_surround_check_back_distance;
+    double surround_check_hysteresis_distance;
     double state_clear_time;
     bool publish_debug_footprints;
+    std::string debug_footprint_label;
   };
 
 private:
+  rcl_interfaces::msg::SetParametersResult onParam(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  std::array<double, 3> getCheckDistances(const std::string & str_label) const;
+
   void onTimer();
 
   void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
@@ -107,6 +117,9 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
   rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr pub_clear_velocity_limit_;
   rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
 
+  // parameter callback result
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
   // stop checker
   std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
 
diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp
index c30f778584fd7..79c2300be3941 100644
--- a/planning/surround_obstacle_checker/src/debug_marker.cpp
+++ b/planning/surround_obstacle_checker/src/debug_marker.cpp
@@ -26,6 +26,29 @@
 
 namespace surround_obstacle_checker
 {
+namespace
+{
+Polygon2d createSelfPolygon(
+  const VehicleInfo & vehicle_info, const double front_margin = 0.0, const double side_margin = 0.0,
+  const double rear_margin = 0.0)
+{
+  const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin;
+  const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin;
+  const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin;
+  const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin;
+
+  Polygon2d ego_polygon;
+
+  ego_polygon.outer().push_back(Point2d(front_m, width_left_m));
+  ego_polygon.outer().push_back(Point2d(front_m, width_right_m));
+  ego_polygon.outer().push_back(Point2d(rear_m, width_right_m));
+  ego_polygon.outer().push_back(Point2d(rear_m, width_left_m));
+
+  bg::correct(ego_polygon);
+
+  return ego_polygon;
+}
+}  // namespace
 
 using motion_utils::createStopVirtualWallMarker;
 using tier4_autoware_utils::appendMarkerArray;
@@ -36,14 +59,18 @@ using tier4_autoware_utils::createMarkerScale;
 using tier4_autoware_utils::createPoint;
 
 SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
-  const Polygon2d & ego_polygon, const double base_link2front,
-  const double & surround_check_distance, const double & surround_check_recover_distance,
-  const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock,
-  rclcpp::Node & node)
-: ego_polygon_(ego_polygon),
+  const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front,
+  const std::string & object_label, const double & surround_check_front_distance,
+  const double & surround_check_side_distance, const double & surround_check_back_distance,
+  const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose,
+  const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node)
+: vehicle_info_(vehicle_info),
   base_link2front_(base_link2front),
-  surround_check_distance_(surround_check_distance),
-  surround_check_recover_distance_(surround_check_recover_distance),
+  object_label_(object_label),
+  surround_check_front_distance_(surround_check_front_distance),
+  surround_check_side_distance_(surround_check_side_distance),
+  surround_check_back_distance_(surround_check_back_distance),
+  surround_check_hysteresis_distance_(surround_check_hysteresis_distance),
   self_pose_(self_pose),
   clock_(clock)
 {
@@ -86,20 +113,25 @@ bool SurroundObstacleCheckerDebugNode::pushObstaclePoint(
 
 void SurroundObstacleCheckerDebugNode::publishFootprints()
 {
+  const auto ego_polygon = createSelfPolygon(vehicle_info_);
+
   /* publish vehicle footprint polygon */
-  const auto footprint = boostPolygonToPolygonStamped(ego_polygon_, self_pose_.position.z);
+  const auto footprint = boostPolygonToPolygonStamped(ego_polygon, self_pose_.position.z);
   vehicle_footprint_pub_->publish(footprint);
 
   /* publish vehicle footprint polygon with offset */
-  const auto polygon_with_offset =
-    createSelfPolygonWithOffset(ego_polygon_, surround_check_distance_);
+  const auto polygon_with_offset = createSelfPolygon(
+    vehicle_info_, surround_check_front_distance_, surround_check_side_distance_,
+    surround_check_back_distance_);
   const auto footprint_with_offset =
     boostPolygonToPolygonStamped(polygon_with_offset, self_pose_.position.z);
   vehicle_footprint_offset_pub_->publish(footprint_with_offset);
 
   /* publish vehicle footprint polygon with recover offset */
-  const auto polygon_with_recover_offset =
-    createSelfPolygonWithOffset(ego_polygon_, surround_check_recover_distance_);
+  const auto polygon_with_recover_offset = createSelfPolygon(
+    vehicle_info_, surround_check_front_distance_ + surround_check_hysteresis_distance_,
+    surround_check_side_distance_ + surround_check_hysteresis_distance_,
+    surround_check_back_distance_ + surround_check_hysteresis_distance_);
   const auto footprint_with_recover_offset =
     boostPolygonToPolygonStamped(polygon_with_recover_offset, self_pose_.position.z);
   vehicle_footprint_recover_offset_pub_->publish(footprint_with_recover_offset);
@@ -206,26 +238,6 @@ VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray()
   return velocity_factor_array;
 }
 
-Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset(
-  const Polygon2d & base_polygon, const double & offset)
-{
-  typedef double coordinate_type;
-  const double buffer_distance = offset;
-  const int points_per_circle = 36;
-  boost::geometry::strategy::buffer::distance_symmetric<coordinate_type> distance_strategy(
-    buffer_distance);
-  boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle);
-  boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle);
-  boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle);
-  boost::geometry::strategy::buffer::side_straight side_strategy;
-  boost::geometry::model::multi_polygon<Polygon2d> result;
-  // Create the buffer of a multi polygon
-  boost::geometry::buffer(
-    base_polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy,
-    circle_strategy);
-  return result.front();
-}
-
 PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped(
   const Polygon2d & boost_polygon, const double & z)
 {
@@ -244,4 +256,14 @@ PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped(
   return polygon_stamped;
 }
 
+void SurroundObstacleCheckerDebugNode::updateFootprintMargin(
+  const std::string & object_label, const double front_distance, const double side_distance,
+  const double back_distance)
+{
+  object_label_ = object_label;
+  surround_check_front_distance_ = front_distance;
+  surround_check_side_distance_ = side_distance;
+  surround_check_back_distance_ = back_distance;
+}
+
 }  // namespace surround_obstacle_checker
diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp
index 17a47be8c0596..4ea3e81d65411 100644
--- a/planning/surround_obstacle_checker/src/node.cpp
+++ b/planning/surround_obstacle_checker/src/node.cpp
@@ -47,6 +47,7 @@ namespace surround_obstacle_checker
 namespace bg = boost::geometry;
 using Point2d = bg::model::d2::point_xy<double>;
 using Polygon2d = bg::model::polygon<Point2d>;
+using autoware_auto_perception_msgs::msg::ObjectClassification;
 using tier4_autoware_utils::createPoint;
 using tier4_autoware_utils::pose2transform;
 
@@ -120,12 +121,14 @@ Polygon2d createObjPolygon(
   return createObjPolygon(pose, polygon);
 }
 
-Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info)
+Polygon2d createSelfPolygon(
+  const VehicleInfo & vehicle_info, const double front_margin, const double side_margin,
+  const double rear_margin)
 {
-  const double & front_m = vehicle_info.max_longitudinal_offset_m;
-  const double & width_left_m = vehicle_info.max_lateral_offset_m;
-  const double & width_right_m = vehicle_info.min_lateral_offset_m;
-  const double & rear_m = vehicle_info.min_longitudinal_offset_m;
+  const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin;
+  const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin;
+  const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin;
+  const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin;
 
   Polygon2d ego_polygon;
 
@@ -146,13 +149,42 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio
   // Parameters
   {
     auto & p = node_param_;
-    p.use_pointcloud = this->declare_parameter<bool>("use_pointcloud");
-    p.use_dynamic_object = this->declare_parameter<bool>("use_dynamic_object");
-    p.surround_check_distance = this->declare_parameter<double>("surround_check_distance");
-    p.surround_check_recover_distance =
-      this->declare_parameter<double>("surround_check_recover_distance");
+
+    // for object label
+    std::unordered_map<std::string, int> label_map{
+      {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR},
+      {"truck", ObjectClassification::TRUCK},     {"bus", ObjectClassification::BUS},
+      {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE},
+      {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}};
+    for (const auto & label_pair : label_map) {
+      p.enable_check_map.emplace(
+        label_pair.second, this->declare_parameter<bool>(label_pair.first + ".enable_check"));
+      p.surround_check_front_distance_map.emplace(
+        label_pair.second,
+        this->declare_parameter<double>(label_pair.first + ".surround_check_front_distance"));
+      p.surround_check_side_distance_map.emplace(
+        label_pair.second,
+        this->declare_parameter<double>(label_pair.first + ".surround_check_side_distance"));
+      p.surround_check_back_distance_map.emplace(
+        label_pair.second,
+        this->declare_parameter<double>(label_pair.first + ".surround_check_back_distance"));
+    }
+
+    // for pointcloud
+    p.pointcloud_enable_check = this->declare_parameter<bool>("pointcloud.enable_check");
+    p.pointcloud_surround_check_front_distance =
+      this->declare_parameter<double>("pointcloud.surround_check_front_distance");
+    p.pointcloud_surround_check_side_distance =
+      this->declare_parameter<double>("pointcloud.surround_check_side_distance");
+    p.pointcloud_surround_check_back_distance =
+      this->declare_parameter<double>("pointcloud.surround_check_back_distance");
+
+    p.surround_check_hysteresis_distance =
+      this->declare_parameter<double>("surround_check_hysteresis_distance");
+
     p.state_clear_time = this->declare_parameter<double>("state_clear_time");
     p.publish_debug_footprints = this->declare_parameter<bool>("publish_debug_footprints");
+    p.debug_footprint_label = this->declare_parameter<std::string>("debug_footprint_label");
   }
 
   vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo();
@@ -176,6 +208,10 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio
     "~/input/odometry", 1,
     std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1));
 
+  // Parameter callback
+  set_param_res_ = this->add_on_set_parameters_callback(
+    std::bind(&SurroundObstacleCheckerNode::onParam, this, std::placeholders::_1));
+
   using std::chrono_literals::operator""ms;
   timer_ = rclcpp::create_timer(
     this, get_clock(), 100ms, std::bind(&SurroundObstacleCheckerNode::onTimer, this));
@@ -184,15 +220,55 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio
   vehicle_stop_checker_ = std::make_unique<VehicleStopChecker>(this);
 
   // Debug
-  auto const self_polygon = createSelfPolygon(vehicle_info_);
   odometry_ptr_ = std::make_shared<nav_msgs::msg::Odometry>();
 
+  const auto check_distances = getCheckDistances(node_param_.debug_footprint_label);
   debug_ptr_ = std::make_shared<SurroundObstacleCheckerDebugNode>(
-    self_polygon, vehicle_info_.max_longitudinal_offset_m, node_param_.surround_check_distance,
-    node_param_.surround_check_recover_distance, odometry_ptr_->pose.pose, this->get_clock(),
+    vehicle_info_, vehicle_info_.max_longitudinal_offset_m, node_param_.debug_footprint_label,
+    check_distances.at(0), check_distances.at(1), check_distances.at(2),
+    node_param_.surround_check_hysteresis_distance, odometry_ptr_->pose.pose, this->get_clock(),
     *this);
 }
 
+std::array<double, 3> SurroundObstacleCheckerNode::getCheckDistances(
+  const std::string & str_label) const
+{
+  if (str_label == "pointcloud") {
+    return {
+      node_param_.pointcloud_surround_check_front_distance,
+      node_param_.pointcloud_surround_check_side_distance,
+      node_param_.pointcloud_surround_check_back_distance};
+  }
+
+  std::unordered_map<std::string, int> label_map{
+    {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR},
+    {"truck", ObjectClassification::TRUCK},     {"bus", ObjectClassification::BUS},
+    {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE},
+    {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}};
+
+  const int int_label = label_map.at(str_label);
+  return {
+    node_param_.surround_check_front_distance_map.at(int_label),
+    node_param_.surround_check_side_distance_map.at(int_label),
+    node_param_.surround_check_back_distance_map.at(int_label)};
+}
+
+rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  tier4_autoware_utils::updateParam<std::string>(
+    parameters, "debug_footprint_label", node_param_.debug_footprint_label);
+  const auto check_distances = getCheckDistances(node_param_.debug_footprint_label);
+  debug_ptr_->updateFootprintMargin(
+    node_param_.debug_footprint_label, check_distances.at(0), check_distances.at(1),
+    check_distances.at(2));
+
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+  return result;
+}
+
 void SurroundObstacleCheckerNode::onTimer()
 {
   if (!odometry_ptr_) {
@@ -205,13 +281,22 @@ void SurroundObstacleCheckerNode::onTimer()
     debug_ptr_->publishFootprints();
   }
 
-  if (node_param_.use_pointcloud && !pointcloud_ptr_) {
+  if (node_param_.pointcloud_enable_check && !pointcloud_ptr_) {
     RCLCPP_INFO_THROTTLE(
       this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info...");
     return;
   }
 
-  if (node_param_.use_dynamic_object && !object_ptr_) {
+  const bool use_dynamic_object =
+    node_param_.enable_check_map.at(ObjectClassification::UNKNOWN) ||
+    node_param_.enable_check_map.at(ObjectClassification::CAR) ||
+    node_param_.enable_check_map.at(ObjectClassification::TRUCK) ||
+    node_param_.enable_check_map.at(ObjectClassification::BUS) ||
+    node_param_.enable_check_map.at(ObjectClassification::TRAILER) ||
+    node_param_.enable_check_map.at(ObjectClassification::MOTORCYCLE) ||
+    node_param_.enable_check_map.at(ObjectClassification::BICYCLE) ||
+    node_param_.enable_check_map.at(ObjectClassification::PEDESTRIAN);
+  if (use_dynamic_object && !object_ptr_) {
     RCLCPP_INFO_THROTTLE(
       this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info...");
     return;
@@ -220,11 +305,11 @@ void SurroundObstacleCheckerNode::onTimer()
   const auto nearest_obstacle = getNearestObstacle();
   const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped();
 
+  constexpr double epsilon = 1e-3;
   switch (state_) {
     case State::PASS: {
       const auto is_obstacle_found =
-        !nearest_obstacle ? false
-                          : nearest_obstacle.get().first < node_param_.surround_check_distance;
+        !nearest_obstacle ? false : nearest_obstacle.get().first < epsilon;
 
       if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) {
         break;
@@ -250,7 +335,7 @@ void SurroundObstacleCheckerNode::onTimer()
       const auto is_obstacle_found =
         !nearest_obstacle
           ? false
-          : nearest_obstacle.get().first < node_param_.surround_check_recover_distance;
+          : nearest_obstacle.get().first < node_param_.surround_check_hysteresis_distance;
 
       if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) {
         break;
@@ -304,17 +389,8 @@ void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::Cons
 
 boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacle() const
 {
-  boost::optional<Obstacle> nearest_pointcloud{boost::none};
-  boost::optional<Obstacle> nearest_object{boost::none};
-
-  if (node_param_.use_pointcloud) {
-    nearest_pointcloud = getNearestObstacleByPointCloud();
-  }
-
-  if (node_param_.use_dynamic_object) {
-    nearest_object = getNearestObstacleByDynamicObject();
-  }
-
+  const auto nearest_pointcloud = getNearestObstacleByPointCloud();
+  const auto nearest_object = getNearestObstacleByDynamicObject();
   if (!nearest_pointcloud && !nearest_object) {
     return {};
   }
@@ -333,15 +409,13 @@ boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacle() cons
 
 boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const
 {
-  if (pointcloud_ptr_->data.empty()) {
+  if (!node_param_.pointcloud_enable_check || pointcloud_ptr_->data.empty()) {
     return boost::none;
   }
+
   const auto transform_stamped =
     getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5);
 
-  geometry_msgs::msg::Point nearest_point;
-  auto minimum_distance = std::numeric_limits<double>::max();
-
   if (!transform_stamped) {
     return {};
   }
@@ -352,8 +426,14 @@ boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByPoint
   tier4_autoware_utils::transformPointCloud(
     transformed_pointcloud, transformed_pointcloud, isometry);
 
-  const auto ego_polygon = createSelfPolygon(vehicle_info_);
+  const double front_margin = node_param_.pointcloud_surround_check_front_distance;
+  const double side_margin = node_param_.pointcloud_surround_check_side_distance;
+  const double back_margin = node_param_.pointcloud_surround_check_back_distance;
+  const auto ego_polygon = createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin);
 
+  geometry_msgs::msg::Point nearest_point;
+  double minimum_distance = std::numeric_limits<double>::max();
+  bool was_minimum_distance_updated = false;
   for (const auto & p : transformed_pointcloud) {
     Point2d boost_point(p.x, p.y);
 
@@ -362,10 +442,14 @@ boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByPoint
     if (distance_to_object < minimum_distance) {
       nearest_point = createPoint(p.x, p.y, p.z);
       minimum_distance = distance_to_object;
+      was_minimum_distance_updated = true;
     }
   }
 
-  return std::make_pair(minimum_distance, nearest_point);
+  if (was_minimum_distance_updated) {
+    return std::make_pair(minimum_distance, nearest_point);
+  }
+  return boost::none;
 }
 
 boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const
@@ -373,9 +457,6 @@ boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByDynam
   const auto transform_stamped =
     getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5);
 
-  geometry_msgs::msg::Point nearest_point;
-  auto minimum_distance = std::numeric_limits<double>::max();
-
   if (!transform_stamped) {
     return {};
   }
@@ -383,10 +464,23 @@ boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByDynam
   tf2::Transform tf_src2target;
   tf2::fromMsg(transform_stamped.get().transform, tf_src2target);
 
-  const auto ego_polygon = createSelfPolygon(vehicle_info_);
-
+  // TODO(murooka) check computation cost
+  geometry_msgs::msg::Point nearest_point;
+  double minimum_distance = std::numeric_limits<double>::max();
+  bool was_minimum_distance_updated = false;
   for (const auto & object : object_ptr_->objects) {
     const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
+    const int label = object.classification.front().label;
+
+    if (!node_param_.enable_check_map.at(label)) {
+      continue;
+    }
+
+    const double front_margin = node_param_.surround_check_front_distance_map.at(label);
+    const double side_margin = node_param_.surround_check_side_distance_map.at(label);
+    const double back_margin = node_param_.surround_check_back_distance_map.at(label);
+    const auto ego_polygon =
+      createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin);
 
     tf2::Transform tf_src2object;
     tf2::fromMsg(object_pose, tf_src2object);
@@ -404,10 +498,14 @@ boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByDynam
     if (distance_to_object < minimum_distance) {
       nearest_point = object_pose.position;
       minimum_distance = distance_to_object;
+      was_minimum_distance_updated = true;
     }
   }
 
-  return std::make_pair(minimum_distance, nearest_point);
+  if (was_minimum_distance_updated) {
+    return std::make_pair(minimum_distance, nearest_point);
+  }
+  return boost::none;
 }
 
 boost::optional<geometry_msgs::msg::TransformStamped> SurroundObstacleCheckerNode::getTransform(