change force lane change path color

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
autowarefoundation · Aug 1, 2022 · 3c129ed · 3c129ed
1 parent ac9ff7c
commit 3c129ed
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -168,6 +168,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
   void publishDebugMarker(const std::vector<MarkerArray> & debug_markers);
+
+  /**
+   * @brief check path if it is unsafe or forced
+   */
+  bool isForcedCandidatePath() const;
 };
 }  // namespace behavior_path_planner
 

diff --git a/...ath_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp b/...ath_planner/include/behavior_path_planner/scene_module/scene_module_bt_node_interface.hpp
@@ -29,6 +29,7 @@ struct SceneModuleStatus
   explicit SceneModuleStatus(const std::string & n) : module_name(n) {}
   std::string module_name;  // TODO(Horibe) should be const
   bool is_requested{false};
+  bool is_execution_ready{false};
   bool is_waiting_approval{false};
   BT::NodeStatus status{BT::NodeStatus::IDLE};
 };

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -552,18 +552,19 @@ void BehaviorPathPlannerNode::run()
 
   // path handling
   const auto path = getPath(output, planner_data);
-  const auto path_candidate = getPathCandidate(output, planner_data);
 
   // update planner data
   planner_data_->prev_output_path = path;
   mutex_pd_.unlock();
 
   PathWithLaneId clipped_path;
-  if (skipSmoothGoalConnection(bt_manager_->getModulesStatus())) {
+  const auto module_status_ptr_vec = bt_manager_->getModulesStatus();
+  if (skipSmoothGoalConnection(module_status_ptr_vec)) {
     clipped_path = *path;
   } else {
     clipped_path = modifyPathForSmoothGoalConnection(*path);
   }
+
   clipPathLength(clipped_path);
   if (!clipped_path.points.empty()) {
     path_publisher_->publish(clipped_path);
@@ -572,6 +573,7 @@ void BehaviorPathPlannerNode::run()
       get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");
   }
 
+  const auto path_candidate = getPathCandidate(output, planner_data);
   path_candidate_publisher_->publish(util::toPath(*path_candidate));
 
   // debug_path_publisher_->publish(util::toPath(path));
@@ -628,6 +630,13 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
 {
   auto path_candidate =
     bt_output.path_candidate ? bt_output.path_candidate : std::make_shared<PathWithLaneId>();
+
+  if (isForcedCandidatePath()) {
+    for (auto & path_point : path_candidate->points) {
+      path_point.point.longitudinal_velocity_mps = 1.0;
+    }
+  }
+
   path_candidate->header = planner_data->route_handler->getRouteHeader();
   path_candidate->header.stamp = this->now();
   RCLCPP_DEBUG(
@@ -636,6 +645,26 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
   return path_candidate;
 }
 
+bool BehaviorPathPlannerNode::isForcedCandidatePath() const
+{
+  const auto & module_status_ptr_vec = bt_manager_->getModulesStatus();
+  for (const auto & module_status_ptr : module_status_ptr_vec) {
+    if (!module_status_ptr) {
+      continue;
+    }
+    if (module_status_ptr->module_name != "LaneChange") {
+      continue;
+    }
+    const auto & is_waiting_approval = module_status_ptr->is_waiting_approval;
+    const auto & is_execution_ready = module_status_ptr->is_execution_ready;
+    if (is_waiting_approval && !is_execution_ready) {
+      return true;
+    }
+    break;
+  }
+  return false;
+}
+
 bool BehaviorPathPlannerNode::skipSmoothGoalConnection(
   const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const
 {

diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp
@@ -45,6 +45,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick()
 
   scene_module_->onEntry();
   module_status_->is_waiting_approval = scene_module_->isWaitingApproval();
+  module_status_->is_execution_ready = scene_module_->isExecutionReady();
 
   const bool is_lane_following = scene_module_->name() == "LaneFollowing";
 
@@ -59,6 +60,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick()
         RCLCPP_ERROR_STREAM(scene_module_->getLogger(), "setOutput() failed : " << res.error());
       }
       module_status_->is_waiting_approval = scene_module_->isWaitingApproval();
+      module_status_->is_execution_ready = scene_module_->isExecutionReady();
     } catch (const std::exception & e) {
       RCLCPP_ERROR_STREAM(
         scene_module_->getLogger(), "behavior module has failed with exception: " << e.what());
@@ -81,6 +83,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick()
       // for data output
       module_status_->status = current_status;
       module_status_->is_waiting_approval = scene_module_->isWaitingApproval();
+      module_status_->is_execution_ready = scene_module_->isExecutionReady();
     } catch (const std::exception & e) {
       RCLCPP_ERROR_STREAM(
         scene_module_->getLogger(), "behavior module has failed with exception: " << e.what());