diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt
index dd0ebd3626f47..757a3b7ecad34 100644
--- a/planning/behavior_path_planner/CMakeLists.txt
+++ b/planning/behavior_path_planner/CMakeLists.txt
@@ -12,7 +12,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utilities.cpp
   src/path_utilities.cpp
   src/debug_utilities.cpp
-  src/path_shifter/path_shifter.cpp
   src/turn_signal_decider.cpp
   src/scene_module/scene_module_bt_node_interface.cpp
   src/scene_module/side_shift/side_shift_module.cpp
@@ -28,6 +27,8 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/scene_module/pull_out/pull_out_module.cpp
   src/scene_module/pull_out/util.cpp
   src/scene_module/utils/geometric_parallel_parking.cpp
+  src/scene_module/utils/occupancy_grid_based_collision_detector.cpp
+  src/scene_module/utils/path_shifter.cpp
 )
 
 target_include_directories(behavior_path_planner_node SYSTEM PUBLIC
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp
index 8edf9e8941cd9..3ecdbefbb2ad8 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/debug_utilities.hpp
@@ -14,8 +14,8 @@
 #ifndef BEHAVIOR_PATH_PLANNER__DEBUG_UTILITIES_HPP_
 #define BEHAVIOR_PATH_PLANNER__DEBUG_UTILITIES_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp
index c59f3c238b90e..7b7cc3b186f03 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp
@@ -16,7 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__PATH_UTILITIES_HPP_
 
 #include <behavior_path_planner/parameters.hpp>
-#include <behavior_path_planner/path_shifter/path_shifter.hpp>
+#include <behavior_path_planner/scene_module/utils/path_shifter.hpp>
 #include <opencv2/opencv.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
index d92567de42463..c5a0a0f78b63e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -15,9 +15,9 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
index 1411ce8ae3aff..790417222651e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
@@ -15,7 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp
index 69ef193d5ee71..ba2b1a4570c94 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp
@@ -16,8 +16,8 @@
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__DEBUG_HPP_
 
 #include "behavior_path_planner/debug_utilities.hpp"
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp
index b86c874a7f17d..fad9860e830f9 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp
@@ -15,7 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
index 67698d1022255..f47c191edfef5 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp
@@ -15,9 +15,9 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_MODULE_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
@@ -35,7 +35,6 @@
 
 namespace behavior_path_planner
 {
-
 struct PullOutParameters
 {
   double min_stop_distance;
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp
index 6d05db2aa52a7..5b32ff64babba 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp
@@ -15,7 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PATH_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OUT__PULL_OUT_PATH_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
index 0121a4286dcbc..455fdf0c2ab79 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp
@@ -15,9 +15,9 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_MODULE_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/scene_module/pull_over/pull_over_path.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp
index 01c866ca20512..d0ff86570a6d0 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp
@@ -15,7 +15,7 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PATH_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__PULL_OVER__PULL_OVER_PATH_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
index 6202f680ce8f1..30356c78a6b1e 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp
@@ -15,8 +15,8 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <route_handler/route_handler.hpp>
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp
new file mode 100644
index 0000000000000..41e7714156ebc
--- /dev/null
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp
@@ -0,0 +1,152 @@
+// Copyright 2022 TIER IV, Inc. All rights reserved.
+//
+// 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 BEHAVIOR_PATH_PLANNER__SCENE_MODULE__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
+
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <vector>
+
+namespace behavior_path_planner
+{
+int discretizeAngle(const double theta, const int theta_size);
+
+struct IndexXYT
+{
+  int x;
+  int y;
+  int theta;
+};
+
+struct IndexXY
+{
+  int x;
+  int y;
+};
+
+IndexXYT pose2index(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local,
+  const int theta_size);
+
+geometry_msgs::msg::Pose index2pose(
+  const nav_msgs::msg::OccupancyGrid & costmap, const IndexXYT & index, const int theta_size);
+
+geometry_msgs::msg::Pose global2local(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_global);
+
+geometry_msgs::msg::Pose local2global(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local);
+
+struct VehicleShape
+{
+  double length;     // X [m]
+  double width;      // Y [m]
+  double base2back;  // base_link to rear [m]
+};
+
+struct OccupancyGridMapParam
+{
+  // robot configs
+  VehicleShape vehicle_shape;
+
+  // costmap configs
+  int theta_size;          // discretized angle table size [-]
+  int obstacle_threshold;  // obstacle threshold on grid [-]
+};
+
+struct PlannerWaypoint
+{
+  geometry_msgs::msg::PoseStamped pose;
+  bool is_back = false;
+};
+
+struct PlannerWaypoints
+{
+  std_msgs::msg::Header header;
+  std::vector<PlannerWaypoint> waypoints;
+};
+
+class OccupancyGridBasedCollisionDetector
+{
+public:
+  OccupancyGridBasedCollisionDetector() {}
+  void setParam(const OccupancyGridMapParam & param) { param_ = param; };
+  OccupancyGridMapParam getParam() const { return param_; };
+  void setMap(const nav_msgs::msg::OccupancyGrid & costmap);
+  nav_msgs::msg::OccupancyGrid getMap() const { return costmap_; };
+  void setVehicleShape(const VehicleShape & vehicle_shape) { param_.vehicle_shape = vehicle_shape; }
+  bool hasObstacleOnPath(
+    const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const;
+  bool hasObstacleOnPath(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const bool check_out_of_range) const;
+  const PlannerWaypoints & getWaypoints() const { return waypoints_; }
+  bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const;
+  virtual ~OccupancyGridBasedCollisionDetector() {}
+
+protected:
+  void computeCollisionIndexes(int theta_index, std::vector<IndexXY> & indexes);
+  inline bool isOutOfRange(const IndexXYT & index) const
+  {
+    if (index.x < 0 || static_cast<int>(costmap_.info.width) <= index.x) {
+      return true;
+    }
+    if (index.y < 0 || static_cast<int>(costmap_.info.height) <= index.y) {
+      return true;
+    }
+    return false;
+  }
+  inline bool isObs(const IndexXYT & index) const
+  {
+    // NOTE: Accessing by .at() instead makes 1.2 times slower here.
+    // Also, boundary check is already done in isOutOfRange before calling this function.
+    // So, basically .at() is not necessary.
+    return is_obstacle_table_[index.y][index.x];
+  }
+
+  OccupancyGridMapParam param_;
+
+  // costmap as occupancy grid
+  nav_msgs::msg::OccupancyGrid costmap_;
+
+  // collision indexes cache
+  std::vector<std::vector<IndexXY>> coll_indexes_table_;
+
+  // is_obstacle's table
+  std::vector<std::vector<bool>> is_obstacle_table_;
+
+  // pose in costmap frame
+  geometry_msgs::msg::Pose start_pose_;
+  geometry_msgs::msg::Pose goal_pose_;
+
+  // result path
+  PlannerWaypoints waypoints_;
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp
similarity index 97%
rename from planning/behavior_path_planner/include/behavior_path_planner/path_shifter/path_shifter.hpp
rename to planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp
index c420e203c0b64..bb984e7601769 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/path_shifter/path_shifter.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__PATH_SHIFTER__PATH_SHIFTER_HPP_
-#define BEHAVIOR_PATH_PLANNER__PATH_SHIFTER__PATH_SHIFTER_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__UTILS__PATH_SHIFTER_HPP_
+#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__UTILS__PATH_SHIFTER_HPP_
 
 #include "behavior_path_planner/parameters.hpp"
 
@@ -242,4 +242,4 @@ class PathShifter
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__PATH_SHIFTER__PATH_SHIFTER_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__UTILS__PATH_SHIFTER_HPP_
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
index c32c636c453f1..2ba50833ec166 100644
--- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
+++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
@@ -14,7 +14,7 @@
 
 #include "behavior_path_planner/scene_module/lane_change/util.hpp"
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
index 24f10cfefcaf7..91855a313520f 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
@@ -15,10 +15,10 @@
 #include "behavior_path_planner/scene_module/pull_out/pull_out_module.hpp"
 
 #include "behavior_path_planner/behavior_path_planner_node.hpp"
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/scene_module/avoidance/debug.hpp"
 #include "behavior_path_planner/scene_module/pull_out/util.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/util/create_vehicle_footprint.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp
index 5e4735510086e..5991c6f60e713 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_out/util.cpp
@@ -14,8 +14,8 @@
 
 #include "behavior_path_planner/scene_module/pull_out/util.hpp"
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/util/create_vehicle_footprint.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
index 7a44c8c9ccbdb..4f88218a39fb5 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp
@@ -15,10 +15,10 @@
 #include "behavior_path_planner/scene_module/pull_over/pull_over_module.hpp"
 
 #include "behavior_path_planner/behavior_path_planner_node.hpp"
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/scene_module/avoidance/debug.hpp"
 #include "behavior_path_planner/scene_module/pull_over/util.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp
index 371d88251fc00..262f34d4ec1c3 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp
@@ -14,8 +14,8 @@
 
 #include "behavior_path_planner/scene_module/pull_over/util.hpp"
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
diff --git a/planning/behavior_path_planner/src/scene_module/utils/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/src/scene_module/utils/occupancy_grid_based_collision_detector.cpp
new file mode 100644
index 0000000000000..1e59014c4cc28
--- /dev/null
+++ b/planning/behavior_path_planner/src/scene_module/utils/occupancy_grid_based_collision_detector.cpp
@@ -0,0 +1,206 @@
+// Copyright 2022 TIER IV, Inc. All rights reserved.
+//
+// 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.
+
+#include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp"
+
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+
+#include <vector>
+
+namespace behavior_path_planner
+{
+using tier4_autoware_utils::createQuaternionFromYaw;
+using tier4_autoware_utils::normalizeRadian;
+using tier4_autoware_utils::transformPose;
+
+int discretizeAngle(const double theta, const int theta_size)
+{
+  const double one_angle_range = 2.0 * M_PI / theta_size;
+  return static_cast<int>(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size;
+}
+
+IndexXYT pose2index(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local,
+  const int theta_size)
+{
+  const int index_x = pose_local.position.x / costmap.info.resolution;
+  const int index_y = pose_local.position.y / costmap.info.resolution;
+  const int index_theta = discretizeAngle(tf2::getYaw(pose_local.orientation), theta_size);
+  return {index_x, index_y, index_theta};
+}
+
+geometry_msgs::msg::Pose index2pose(
+  const nav_msgs::msg::OccupancyGrid & costmap, const IndexXYT & index, const int theta_size)
+{
+  geometry_msgs::msg::Pose pose_local;
+
+  pose_local.position.x = index.x * costmap.info.resolution;
+  pose_local.position.y = index.y * costmap.info.resolution;
+
+  const double one_angle_range = 2.0 * M_PI / theta_size;
+  const double yaw = index.theta * one_angle_range;
+  pose_local.orientation = createQuaternionFromYaw(yaw);
+
+  return pose_local;
+}
+
+geometry_msgs::msg::Pose global2local(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_global)
+{
+  tf2::Transform tf_origin;
+  tf2::convert(costmap.info.origin, tf_origin);
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.transform = tf2::toMsg(tf_origin.inverse());
+
+  return transformPose(pose_global, transform);
+}
+
+geometry_msgs::msg::Pose local2global(
+  const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local)
+{
+  tf2::Transform tf_origin;
+  tf2::convert(costmap.info.origin, tf_origin);
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.transform = tf2::toMsg(tf_origin);
+
+  return transformPose(pose_local, transform);
+}
+
+void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
+{
+  costmap_ = costmap;
+  const auto height = costmap_.info.height;
+  const auto width = costmap_.info.width;
+
+  // Initialize status
+  std::vector<std::vector<bool>> is_obstacle_table;
+  is_obstacle_table.resize(height);
+  for (uint32_t i = 0; i < height; i++) {
+    is_obstacle_table.at(i).resize(width);
+    for (uint32_t j = 0; j < width; j++) {
+      const int cost = costmap_.data[i * width + j];
+
+      if (cost < 0 || param_.obstacle_threshold <= cost) {
+        is_obstacle_table[i][j] = true;
+      }
+    }
+  }
+  is_obstacle_table_ = is_obstacle_table;
+
+  // construct collision indexes table
+  coll_indexes_table_.clear();
+  for (int i = 0; i < param_.theta_size; i++) {
+    std::vector<IndexXY> indexes_2d;
+    computeCollisionIndexes(i, indexes_2d);
+    coll_indexes_table_.push_back(indexes_2d);
+  }
+}
+
+void OccupancyGridBasedCollisionDetector::computeCollisionIndexes(
+  int theta_index, std::vector<IndexXY> & indexes_2d)
+{
+  IndexXYT base_index{0, 0, theta_index};
+  const VehicleShape & vehicle_shape = param_.vehicle_shape;
+
+  // Define the robot as rectangle
+  const double back = -1.0 * vehicle_shape.base2back;
+  const double front = vehicle_shape.length - vehicle_shape.base2back;
+  const double right = -1.0 * vehicle_shape.width / 2.0;
+  const double left = vehicle_shape.width / 2.0;
+
+  const auto base_pose = index2pose(costmap_, base_index, param_.theta_size);
+  const auto base_theta = tf2::getYaw(base_pose.orientation);
+
+  // Convert each point to index and check if the node is Obstacle
+  const auto addIndex2d = [&](const double x, const double y) {
+    // Calculate offset in rotated frame
+    const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y;
+    const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y;
+
+    geometry_msgs::msg::Pose pose_local;
+    pose_local.position.x = base_pose.position.x + offset_x;
+    pose_local.position.y = base_pose.position.y + offset_y;
+
+    const auto index = pose2index(costmap_, pose_local, param_.theta_size);
+    const auto index_2d = IndexXY{index.x, index.y};
+    indexes_2d.push_back(index_2d);
+  };
+
+  for (double x = back; x <= front; x += costmap_.info.resolution / 2) {
+    for (double y = right; y <= left; y += costmap_.info.resolution / 2) {
+      addIndex2d(x, y);
+    }
+    addIndex2d(x, left);
+  }
+  for (double y = right; y <= left; y += costmap_.info.resolution / 2) {
+    addIndex2d(front, y);
+  }
+  addIndex2d(front, left);
+}
+
+bool OccupancyGridBasedCollisionDetector::detectCollision(
+  const IndexXYT & base_index, const bool check_out_of_range) const
+{
+  const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta];
+  for (const auto & coll_index_2d : coll_indexes_2d) {
+    int idx_theta = 0;  // whatever. Yaw is nothing to do with collision detection between grids.
+    IndexXYT coll_index{coll_index_2d.x, coll_index_2d.y, idx_theta};
+    // must slide to current base position
+    coll_index.x += base_index.x;
+    coll_index.y += base_index.y;
+    if (check_out_of_range) {
+      if (isOutOfRange(coll_index) || isObs(coll_index)) return true;
+    } else {
+      if (isOutOfRange(coll_index)) return false;
+      if (isObs(coll_index)) return true;
+    }
+  }
+  return false;
+}
+
+bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath(
+  const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const
+{
+  for (const auto & pose : path.poses) {
+    const auto pose_local = global2local(costmap_, pose);
+    const auto index = pose2index(costmap_, pose_local, param_.theta_size);
+
+    if (detectCollision(index, check_out_of_range)) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const bool check_out_of_range) const
+{
+  for (const auto & p : path.points) {
+    const auto pose_local = global2local(costmap_, p.point.pose);
+    const auto index = pose2index(costmap_, pose_local, param_.theta_size);
+
+    if (detectCollision(index, check_out_of_range)) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+}  // namespace behavior_path_planner
diff --git a/planning/behavior_path_planner/src/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp
similarity index 99%
rename from planning/behavior_path_planner/src/path_shifter/path_shifter.cpp
rename to planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp
index 780c0842d6617..b3cfddf50fbc9 100644
--- a/planning/behavior_path_planner/src/path_shifter/path_shifter.cpp
+++ b/planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/utilities.hpp"