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

---
 .../include/op_trajectory_evaluator_core.h            |  2 ++
 op_local_planner/launch/op_common_params.launch       |  2 ++
 .../op_trajectory_evaluator_core.cpp                  | 11 +++++++++--
 3 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/op_local_planner/include/op_trajectory_evaluator_core.h b/op_local_planner/include/op_trajectory_evaluator_core.h
index 7557f70d..1a588448 100644
--- a/op_local_planner/include/op_trajectory_evaluator_core.h
+++ b/op_local_planner/include/op_trajectory_evaluator_core.h
@@ -32,6 +32,7 @@
 #include "op_planner/TrajectoryEvaluator.h"
 #include "op_ros_helpers/ROSVelocityHandler.h"
 #include "op_ros_helpers/op_ParamsHandler.h"
+#include "op_ros_helpers/ROSMapHandler.h"
 
 namespace TrajectoryEvaluatorNS
 {
@@ -75,6 +76,7 @@ class TrajectoryEvalCore
   	PlannerHNS::PlanningParams m_PlanningParams;
   	PlannerHNS::PlanningParams m_ModPlanningParams;
   	PlannerHNS::CAR_BASIC_INFO m_CarInfo;
+	PlannerHNS::MapHandler m_MapHandler;
 
   	PlannerHNS::BehaviorState m_CurrentBehavior;
   	double m_AdditionalFollowDistance;
diff --git a/op_local_planner/launch/op_common_params.launch b/op_local_planner/launch/op_common_params.launch
index eae84276..f1409789 100644
--- a/op_local_planner/launch/op_common_params.launch
+++ b/op_local_planner/launch/op_common_params.launch
@@ -29,6 +29,7 @@
 	<arg name="enableTrafficLightBehavior" 	default="false" />
 	<arg name="enableStopSignBehavior" 		default="false" />	
 	<arg name="enableLaneChange" 			default="false" />	
+	<arg name="enableBlockingRolloutsOutOfMap"	default="false" />	
 	
 	<!-- Vehicle Info, shared with controller and simulation -->
 	<arg name="height" 						default="1.47"  />
@@ -92,6 +93,7 @@
 		<param name="enableTrafficLightBehavior" value="$(arg enableTrafficLightBehavior)" />
 		<param name="enableStopSignBehavior" 	value="$(arg enableStopSignBehavior)" />	
 		<param name="enableLaneChange" 			value="$(arg enableLaneChange)" />
+		<param name="enableBlockingRolloutsOutOfMap" value="$(arg enableBlockingRolloutsOutOfMap)" />
 		
 		<param name="height" 					value="$(arg height)"  />		
 		<param name="front_length" 				value="$(arg front_length)"  />
diff --git a/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp b/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp
index ac621b70..b8c7265b 100644
--- a/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp
+++ b/op_local_planner/nodes/op_trajectory_evaluator/op_trajectory_evaluator_core.cpp
@@ -58,6 +58,9 @@ TrajectoryEvalCore::TrajectoryEvalCore()
 
 	m_TrajectoryCostsCalculator.SetEvalParams(m_EvaluationParams);
 	PlannerHNS::ROSHelpers::InitCollisionPointsMarkers(500, m_CollisionsDummy);
+
+	m_MapHandler.InitMapHandler(nh, "/op_common_params/mapSource",
+			"/op_common_params/mapFileName", "/op_common_params/lanelet2_origin");
 }
 
 TrajectoryEvalCore::~TrajectoryEvalCore()
@@ -105,6 +108,7 @@ void TrajectoryEvalCore::UpdatePlanningParams(ros::NodeHandle& _nh)
 	_nh.getParam("/op_common_params/minDistanceToAvoid", m_PlanningParams.minDistanceToAvoid);
 	_nh.getParam("/op_common_params/maxDistanceToAvoid", m_PlanningParams.maxDistanceToAvoid);
 	_nh.getParam("/op_common_params/speedProfileFactor", m_PlanningParams.speedProfileFactor);
+	_nh.getParam("/op_common_params/enableBlockingRolloutsOutOfMap", m_PlanningParams.enableBlockingRolloutsOutOfMap);
 
 	_nh.getParam("/op_common_params/enableLaneChange", m_PlanningParams.enableLaneChange);
 	if(!m_PlanningParams.enableLaneChange)
@@ -231,7 +235,7 @@ void TrajectoryEvalCore::callbackGetGlobalPlannerPath(const autoware_msgs::LaneA
 
 void TrajectoryEvalCore::callbackGetLocalPlannerPath(const autoware_msgs::LaneArrayConstPtr& msg)
 {
-	if(msg->lanes.size() != (m_PlanningParams.rollOutNumber + 1))
+	if(msg->lanes.size() != m_GlobalPaths.size()*(m_PlanningParams.rollOutNumber + 1))
 	{
 		std::cout << " ### Waiting until nodes are synchronized, Generated rollouts = " << msg->lanes.size() << ", But They should = " << m_PlanningParams.rollOutNumber + 1 << std::endl;
 		return;
@@ -434,7 +438,10 @@ void TrajectoryEvalCore::MainLoop()
 	while (ros::ok())
 	{
 		ros::spinOnce();
-
+		if( ! m_MapHandler.IsMapLoaded())
+		{
+			m_MapHandler.LoadMap(m_TrajectoryCostsCalculator.m_Map, m_PlanningParams.enableLaneChange);
+		}
 
 		if(bNewCurrentPos)
 		{