Merge branch 'autowarefoundation:main' into chore/rework_param_map_lo…

…ader
autowarefoundation · Jan 30, 2024 · f593330 · f593330
2 parents 3693fa8 + 25bc636
commit f593330
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 2 deletions.
diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
@@ -90,8 +90,9 @@ std::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::getPCDMetadata(
 {
   std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
   if (pcd_paths.size() != 1) {
-    if (!fs::exists(pcd_metadata_path)) {
-      throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path);
+    while (!fs::exists(pcd_metadata_path)) {
+      RCLCPP_ERROR_STREAM(get_logger(), "PCD metadata file not found: " << pcd_metadata_path);
+      std::this_thread::sleep_for(std::chrono::seconds(1));
     }
 
     pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path);

diff --git a/...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp
@@ -493,6 +493,7 @@ class GoalPlannerModule : public SceneModuleInterface
   std::optional<BehaviorModuleOutput> last_previous_module_output_{};
   bool hasPreviousModulePathShapeChanged() const;
   bool hasDeviatedFromLastPreviousModulePath() const;
+  bool hasDeviatedFromCurrentPreviousModulePath() const;
 
   // timer for generating pull over path candidates in a separate thread
   void onTimer();

diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -161,6 +161,13 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const
            planner_data_->self_odometry->pose.pose.position)) > 0.3;
 }
 
+bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const
+{
+  return std::abs(motion_utils::calcLateralOffset(
+           getPreviousModuleOutput().path.points,
+           planner_data_->self_odometry->pose.pose.position)) > 0.3;
+}
+
 // generate pull over candidate paths
 void GoalPlannerModule::onTimer()
 {
@@ -179,6 +186,10 @@ void GoalPlannerModule::onTimer()
 
   // check if new pull over path candidates are needed to be generated
   const bool need_update = std::invoke([&]() {
+    if (hasDeviatedFromCurrentPreviousModulePath()) {
+      RCLCPP_ERROR(getLogger(), "has deviated from current previous module path");
+      return false;
+    }
     if (thread_safe_data_.get_pull_over_path_candidates().empty()) {
       return true;
     }