move occ and path_shifter to utils

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Jul 7, 2022 · 319e157 · 319e157
1 parent 5957d4d
commit 319e157
Show file tree

Hide file tree

Showing 21 changed files with 32 additions and 32 deletions.
diff --git 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
@@ -27,7 +26,8 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/scene_module/pull_over/util.cpp
   src/scene_module/pull_out/pull_out_module.cpp
   src/scene_module/pull_out/util.cpp
-  src/occupancy_grid_map/occupancy_grid_map.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
@@ -14,7 +14,7 @@
 #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/scene_module/utils/path_shifter.hpp"
 #include "behavior_path_planner/path_utilities.hpp"
 #include "behavior_path_planner/utilities.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
@@ -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/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_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/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/...th_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/...ning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/...ning/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/..._path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/..._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/...vior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/...vior_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/...havior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_path.hpp b/...havior_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/...or_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/...or_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/...vior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_path.hpp b/...vior_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/..._path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/..._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/...occupancy_grid_map/occupancy_grid_map.hpp → ...cupancy_grid_based_collision_detector.hpp b/...occupancy_grid_map/occupancy_grid_map.hpp → ...cupancy_grid_based_collision_detector.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_PATH_PLANNER__OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_
-#define BEHAVIOR_PATH_PLANNER__OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_
+#ifndef BEHAVIOR_PATH_PLANNER__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
+#define BEHAVIOR_PATH_PLANNER__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
 
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
@@ -85,10 +85,10 @@ struct PlannerWaypoints
   std::vector<PlannerWaypoint> waypoints;
 };
 
-class OccupancyGridMap
+class OccupancyGridBasedCollisionDetector
 {
 public:
-  OccupancyGridMap() {}
+  OccupancyGridBasedCollisionDetector() {}
   void setParam(const OccupancyGridMapParam & param) { param_ = param; };
   OccupancyGridMapParam getParam() const { return param_; };
   void setMap(const nav_msgs::msg::OccupancyGrid & costmap);
@@ -101,7 +101,7 @@ class OccupancyGridMap
     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 ~OccupancyGridMap() {}
+  virtual ~OccupancyGridBasedCollisionDetector() {}
 
 protected:
   void computeCollisionIndexes(int theta_index, std::vector<IndexXY> & indexes);
@@ -144,4 +144,4 @@ class OccupancyGridMap
 
 }  // namespace behavior_path_planner
 
-#endif  // BEHAVIOR_PATH_PLANNER__OCCUPANCY_GRID_MAP__OCCUPANCY_GRID_MAP_HPP_
+#endif  // BEHAVIOR_PATH_PLANNER__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_
diff --git a/...ath_planner/path_shifter/path_shifter.hpp → ...anner/scene_module/utils/path_shifter.hpp b/...ath_planner/path_shifter/path_shifter.hpp → ...anner/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
@@ -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
@@ -15,7 +15,7 @@
 #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/scene_module/utils/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"

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
@@ -14,7 +14,7 @@
 
 #include "behavior_path_planner/scene_module/pull_out/util.hpp"
 
-#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/util/create_vehicle_footprint.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
@@ -15,7 +15,7 @@
 #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/scene_module/utils/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"

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
@@ -14,7 +14,7 @@
 
 #include "behavior_path_planner/scene_module/pull_over/util.hpp"
 
-#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 <lanelet2_extension/utility/query.hpp>

diff --git a/...occupancy_grid_map/occupancy_grid_map.cpp → ...cupancy_grid_based_collision_detector.cpp b/...occupancy_grid_map/occupancy_grid_map.cpp → ...cupancy_grid_based_collision_detector.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "behavior_path_planner/occupancy_grid_map/occupancy_grid_map.hpp"
+#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>
@@ -80,7 +80,7 @@ geometry_msgs::msg::Pose local2global(
   return transformPose(pose_local, transform);
 }
 
-void OccupancyGridMap::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
+void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
 {
   costmap_ = costmap;
   const auto height = costmap_.info.height;
@@ -110,7 +110,8 @@ void OccupancyGridMap::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
   }
 }
 
-void OccupancyGridMap::computeCollisionIndexes(int theta_index, std::vector<IndexXY> & 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;
@@ -151,7 +152,7 @@ void OccupancyGridMap::computeCollisionIndexes(int theta_index, std::vector<Inde
   addIndex2d(front, left);
 }
 
-bool OccupancyGridMap::detectCollision(
+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];
@@ -171,7 +172,7 @@ bool OccupancyGridMap::detectCollision(
   return false;
 }
 
-bool OccupancyGridMap::hasObstacleOnPath(
+bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath(
   const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const
 {
   for (const auto & pose : path.poses) {
@@ -186,7 +187,7 @@ bool OccupancyGridMap::hasObstacleOnPath(
   return false;
 }
 
-bool OccupancyGridMap::hasObstacleOnPath(
+bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const bool check_out_of_range) const
 {

diff --git a/...planner/src/path_shifter/path_shifter.cpp → ...r/src/scene_module/utils/path_shifter.cpp b/...planner/src/path_shifter/path_shifter.cpp → ...r/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"