From 91055f6f40c5c38a3428460ff039e0a440a1337a Mon Sep 17 00:00:00 2001
From: blabla-my <1042244638@qq.com>
Date: Thu, 8 Jun 2023 19:36:52 -0400
Subject: [PATCH] Block rollouts that cut through of drivable areas.

---
 .../include/op_planner/MappingHelpers.h       |  1 +
 op_planner/include/op_planner/RoadNetwork.h   |  2 ++
 .../include/op_planner/TrajectoryEvaluator.h  |  4 ++++
 .../include/op_planner/VectorMapLoader.h      |  2 +-
 op_planner/src/MappingHelpers.cpp             | 12 +++++++++++
 op_planner/src/PlanningHelpers.cpp            |  1 +
 op_planner/src/TrajectoryEvaluator.cpp        | 20 ++++++++++++++++++-
 7 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/op_planner/include/op_planner/MappingHelpers.h b/op_planner/include/op_planner/MappingHelpers.h
index 0940dc3..7b110ef 100755
--- a/op_planner/include/op_planner/MappingHelpers.h
+++ b/op_planner/include/op_planner/MappingHelpers.h
@@ -66,6 +66,7 @@ class MappingHelpers {
 	static WayPoint* GetLastWaypoint(RoadNetwork& map);
 	static void FindAdjacentLanes(RoadNetwork& map);
 	static void FindAdjacentLanesV2(RoadNetwork& map, const double& min_d = 1.2, const double& max_d = 3.5);
+	static bool PointInMap(RoadNetwork& map,const WayPoint& pos);
 	/**
 	 *
 	 *
diff --git a/op_planner/include/op_planner/RoadNetwork.h b/op_planner/include/op_planner/RoadNetwork.h
index 831ca5f..6ebc6fb 100755
--- a/op_planner/include/op_planner/RoadNetwork.h
+++ b/op_planner/include/op_planner/RoadNetwork.h
@@ -1273,6 +1273,7 @@ class PlanningParams
 	bool 	enableTrafficLightBehavior;
 	bool 	enableStopSignBehavior;
 	bool 	enableTimeOutAvoidance;
+	bool 	enableBlockingRolloutsOutOfMap;
 	double 	avoidanceTimeOut;
 
 	bool 	enabTrajectoryVelocities;
@@ -1319,6 +1320,7 @@ class PlanningParams
 		enableLaneChange = false;
 		enableStopSignBehavior = false;
 		enabTrajectoryVelocities = false;
+		enableBlockingRolloutsOutOfMap = false;
 		minIndicationDistance = 15;
 
 		enableTimeOutAvoidance = false;
diff --git a/op_planner/include/op_planner/TrajectoryEvaluator.h b/op_planner/include/op_planner/TrajectoryEvaluator.h
index f668149..23f93b3 100755
--- a/op_planner/include/op_planner/TrajectoryEvaluator.h
+++ b/op_planner/include/op_planner/TrajectoryEvaluator.h
@@ -7,6 +7,7 @@
 #define TRAJECTORY_EVALUATOR_H_
 
 #include "PlanningHelpers.h"
+#include "MappingHelpers.h"
 #include "PlannerCommonDef.h"
 
 namespace PlannerHNS
@@ -67,6 +68,7 @@ class TrajectoryEvaluator
   }
 
 public:
+  RoadNetwork m_Map;
   std::vector<WayPoint> all_contour_points_;
   std::vector<WayPoint> all_trajectories_points_;
   std::vector<WayPoint> collision_points_;
@@ -99,6 +101,8 @@ class TrajectoryEvaluator
 
   void calculateDistanceCosts(const PlanningParams& params, const double& c_lateral_d, const std::vector<std::vector<WayPoint> >& roll_outs, const std::vector<WayPoint>& contour_points, const std::vector<WayPoint>& trajectory_points, std::vector<TrajectoryCost>& trajectory_costs, std::vector<WayPoint>& collision_points);
 
+  void blockTrajectoryOutOfRoad(const std::vector<std::vector<WayPoint> >& roll_outs, std::vector<TrajectoryCost>& trajectory_costs);
+
   TrajectoryCost findBestTrajectory(const PlanningParams& params, const int& prev_curr_index, const bool& b_keep_curr, std::vector<TrajectoryCost> trajectory_costs);
 
 };
diff --git a/op_planner/include/op_planner/VectorMapLoader.h b/op_planner/include/op_planner/VectorMapLoader.h
index 9e97f5b..bfb1d11 100644
--- a/op_planner/include/op_planner/VectorMapLoader.h
+++ b/op_planner/include/op_planner/VectorMapLoader.h
@@ -21,7 +21,7 @@ namespace PlannerHNS {
 
 class VectorMapLoader {
 public:
-	VectorMapLoader(int map_version = 1, bool enable_lane_change = false, bool load_curb = false, bool load_lines = false, bool load_wayarea = false);
+	VectorMapLoader(int map_version = 1, bool enable_lane_change = false, bool load_curb = false, bool load_lines = false, bool load_wayarea = true);
 	virtual ~VectorMapLoader();
 
 	/**
diff --git a/op_planner/src/MappingHelpers.cpp b/op_planner/src/MappingHelpers.cpp
index d9519aa..eee8656 100755
--- a/op_planner/src/MappingHelpers.cpp
+++ b/op_planner/src/MappingHelpers.cpp
@@ -420,6 +420,18 @@ Lane* MappingHelpers::GetClosestLaneFromMap(const WayPoint& pos, RoadNetwork& ma
 	return pCloseLane;
 }
 
+bool MappingHelpers::PointInMap(RoadNetwork& map,const WayPoint& pos)
+{
+	for(unsigned int i=0; i < map.boundaries.size(); i++)
+	{
+		if ( PlanningHelpers::PointInsidePolygon(map.boundaries.at(i).points, pos) )
+		{
+			return true;
+		}
+	}
+	return false;
+}
+
 WayPoint MappingHelpers::GetFirstWaypoint(RoadNetwork& map)
 {
 	for(unsigned int j=0; j< map.roadSegments.size(); j ++)
diff --git a/op_planner/src/PlanningHelpers.cpp b/op_planner/src/PlanningHelpers.cpp
index a1c36fd..45126b1 100755
--- a/op_planner/src/PlanningHelpers.cpp
+++ b/op_planner/src/PlanningHelpers.cpp
@@ -3952,6 +3952,7 @@ void PlanningHelpers::InitializeSafetyPolygon(const PlannerHNS::WayPoint& curr_s
   car_border.points.push_back(top_left_car);
 }
 
+
 int PlanningHelpers::PointInsidePolygon(const std::vector<GPSPoint>& points,const GPSPoint& p)
 {
         int counter = 0;
diff --git a/op_planner/src/TrajectoryEvaluator.cpp b/op_planner/src/TrajectoryEvaluator.cpp
index 10a97a9..e46d52a 100755
--- a/op_planner/src/TrajectoryEvaluator.cpp
+++ b/op_planner/src/TrajectoryEvaluator.cpp
@@ -56,7 +56,7 @@ TrajectoryCost TrajectoryEvaluator::doOneStep(const std::vector<std::vector<WayP
 	best_trajectory.lane_index = 0;
 
 	if(roll_outs.size() == 0)
-	{
+        {
 		std::cout << " ### Zero generated rollouts, But They should = " << params.rollOutNumber + 1 << std::endl;
 		return best_trajectory;
 	}
@@ -87,6 +87,9 @@ TrajectoryCost TrajectoryEvaluator::doOneStep(const std::vector<std::vector<WayP
 
   collision_points_.clear();
   calculateDistanceCosts(params, critical_lateral_distance, local_roll_outs_, all_contour_points_, all_trajectories_points_, trajectory_costs_, collision_points_);
+  if(params.enableBlockingRolloutsOutOfMap == true){
+    blockTrajectoryOutOfRoad(local_roll_outs_, trajectory_costs_);
+  }
 //  collision_points_.clear();
 //  collision_points_.insert(collision_points_.begin(), all_contour_points_.begin(), all_contour_points_.end());
 //  collision_points_.insert(collision_points_.begin(), all_trajectories_points_.begin(), all_trajectories_points_.end());
@@ -364,6 +367,21 @@ void TrajectoryEvaluator::initializeCosts(const std::vector<std::vector<WayPoint
   }
 }
 
+void TrajectoryEvaluator::blockTrajectoryOutOfRoad(const std::vector<std::vector<WayPoint> >& roll_outs, std::vector<TrajectoryCost>& trajectory_costs)
+{
+  for(unsigned int i=0; i<roll_outs.size(); i++)
+  {
+    for(unsigned int j=0; j<roll_outs.at(i).size(); j++)
+    {
+      if( ! MappingHelpers::PointInMap(m_Map,roll_outs.at(i).at(j)) )
+      {
+        trajectory_costs.at(i).bBlocked = true;
+        break;
+      }
+    }
+  }
+}
+
 void TrajectoryEvaluator::initializeSafetyPolygon(const WayPoint& curr_state, const CAR_BASIC_INFO& car_info,
                                                   const VehicleState& vehicle_state, const double& c_lateral_d,
                                                   const double& c_long_front_d, const double& c_long_back_d,