add op_plan lib in src1.

src/ros/catkin/src/pilottoros/src/main.cpp

@@ -20,6 +20,7 @@
 #include <geometry_msgs/TwistStamped.h>
 
 #include "autoware_msgs/Lane.h"
+#include "autoware_msgs/DetectedObjectArray.h"
 
 #include "rawpic.pb.h"
 #include "odom.pb.h"
@@ -53,6 +54,8 @@ ros::Publisher waypoints_pub;
 ros::Publisher pose_pub;
 ros::Publisher twist_pub;
 ros::Publisher localizer_pub;
+ros::Publisher pub_objects_;
+
 
 
 static iv::map::tracemap gtracemap;
@@ -321,6 +324,50 @@ void ThreadCalcNearPoint()
     }
 }
 
+#include "tf/transform_datatypes.h"
+void ThreadTestObjects()
+{
+    while(1)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        autoware_msgs::DetectedObjectArray  xobjectsarray;
+
+        autoware_msgs::DetectedObject xobject;
+        autoware_msgs::DetectedObject * pobject = NULL;
+        pobject = &xobject;
+
+        pobject->score = 1.0;
+        pobject->label = "car";
+
+
+         //   object.header = in_header;
+        pobject->valid = true;
+        pobject->pose_reliable = true;
+
+        pobject->pose.position.x = 10;
+        pobject->pose.position.y = 1;
+        pobject->pose.position.z = 0;
+
+// Trained this way
+         float yaw = 0;//detections[i * OUTPUT_NUM_BOX_FEATURE_ + 6];
+//  yaw += M_PI/2;
+// yaw = std::atan2(std::sin(yaw), std::cos(yaw));
+        geometry_msgs::Quaternion q = tf::createQuaternionMsgFromYaw(-yaw);
+        pobject->pose.orientation = q;
+
+
+// Again: Trained this way
+        pobject->dimensions.x = 4.6;
+        pobject->dimensions.y = 1.9;
+        pobject->dimensions.z = 1.5;
+
+        pobject->velocity.linear.x = 0;
+        xobjectsarray.objects.push_back(xobject);
+        pub_objects_.publish(xobjectsarray);
+
+    }
+}
+
 int main(int argc, char *argv[])
 {
 	ros::init(argc, argv, "pilottoros");
@@ -367,6 +414,8 @@ int main(int argc, char *argv[])
         twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
         localizer_pub = private_nh.advertise<geometry_msgs::PoseStamped>("/localizer_pose",10);
     }
+
+    pub_objects_ = private_nh.advertise<autoware_msgs::DetectedObjectArray>("/tracked_objects", 1);
 	
 
 
@@ -386,7 +435,8 @@ int main(int argc, char *argv[])
 
     void * pway = iv::modulecomm::RegisterRecv(_waypoints_msgname.data(),ListenWayPointsMap);
 
-
+    void * threadtestobject = new std::thread(ThreadTestObjects);
+    (void )threadtestobject;
 
 ros::spin();
 /*

src1/planning/op_planner/BehaviorPrediction.cpp

@@ -0,0 +1,762 @@
+
+/// \file  BehaviorPrediction.cpp
+/// \brief Predict detected vehicles's possible trajectories, these trajectories extracted from the vector map.
+/// \author Hatem Darweesh
+/// \date Jul 6, 2017
+
+
+
+#include "op_planner/BehaviorPrediction.h"
+#include "op_planner/MappingHelpers.h"
+#include "op_planner/PlanningHelpers.h"
+#include "op_planner/MatrixOperations.h"
+
+
+namespace PlannerHNS
+{
+
+BehaviorPrediction::BehaviorPrediction()
+{
+  m_MaxLaneDetectionDistance = 0.5;
+  m_PredictionDistance = 20.0;
+  m_bGenerateBranches = false;
+  m_bUseFixedPrediction = true;
+  m_bStepByStep = false;
+  m_bCanDecide = true;
+  m_bParticleFilter = false;
+  UtilityHNS::UtilityH::GetTickCount(m_GenerationTimer);
+  UtilityHNS::UtilityH::GetTickCount(m_ResamplingTimer);
+  m_bFirstMove = true;
+  m_bDebugOut = false;
+}
+
+BehaviorPrediction::~BehaviorPrediction()
+{
+}
+
+void BehaviorPrediction::FilterObservations(const std::vector<DetectedObject>& obj_list, RoadNetwork& map, std::vector<DetectedObject>& filtered_list)
+{
+  for(unsigned int i=0; i < obj_list.size(); i++)
+  {
+    if(obj_list.at(i).t == SIDEWALK || obj_list.at(i).center.v < 1.0)
+      continue;
+
+    bool bFound = false;
+    int found_index = 0;
+    for(unsigned int ip=0; ip < filtered_list.size(); ip++)
+    {
+      if(filtered_list.at(ip).id == obj_list.at(i).id)
+      {
+        found_index = ip;
+        bFound = true;
+        break;
+      }
+    }
+
+    if(bFound)
+      filtered_list.at(found_index) = obj_list.at(i);
+    else
+      filtered_list.push_back(obj_list.at(i));
+  }
+
+  for(int ip=0; ip < (int)filtered_list.size(); ip++)
+  {
+    //check for cleaning
+    bool bRevFound = false;
+    for(unsigned int ic=0; ic < obj_list.size(); ic++)
+    {
+      if(filtered_list.at(ip).id == obj_list.at(ic).id)
+      {
+        bRevFound = true;
+        break;
+      }
+    }
+
+    if(!bRevFound)
+    {
+      filtered_list.erase(filtered_list.begin()+ip);
+      ip--;
+    }
+  }
+}
+
+void BehaviorPrediction::DoOneStep(const std::vector<DetectedObject>& obj_list, const WayPoint& currPose, const double& minSpeed, const double& maxDeceleration, RoadNetwork& map)
+{
+  if(!m_bUseFixedPrediction && maxDeceleration !=0)
+    m_PredictionDistance = -pow(currPose.v, 2)/(maxDeceleration);
+
+  ExtractTrajectoriesFromMap(obj_list, map, m_ParticleInfo_II);
+  CalculateCollisionTimes(minSpeed);
+
+  if(m_bParticleFilter)
+  {
+    ParticleFilterSteps(m_ParticleInfo_II);
+  }
+}
+
+void BehaviorPrediction::CalculateCollisionTimes(const double& minSpeed)
+{
+  for(unsigned int i=0; i < m_ParticleInfo_II.size(); i++)
+  {
+    for(unsigned int j=0; j < m_ParticleInfo_II.at(i)->obj.predTrajectories.size(); j++)
+    {
+      PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(m_ParticleInfo_II.at(i)->obj.predTrajectories.at(j), m_ParticleInfo_II.at(i)->obj.center, minSpeed, m_PredictionDistance);
+//      PlannerHNS::PlanningHelpers::CalcAngleAndCost(m_PredictedObjects.at(i).predTrajectories.at(j));
+    }
+  }
+}
+
+void BehaviorPrediction::ExtractTrajectoriesFromMap(const std::vector<DetectedObject>& curr_obj_list,RoadNetwork& map, std::vector<ObjParticles*>& old_obj_list)
+{
+  PlannerH planner;
+  m_temp_list_ii.clear();
+
+  std::vector<ObjParticles*> delete_me_list = old_obj_list;
+
+  for(unsigned int i=0; i < curr_obj_list.size(); i++)
+  {
+    bool bMatch = false;
+    for(unsigned int ip=0; ip < old_obj_list.size(); ip++)
+    {
+      if(old_obj_list.at(ip)->obj.id == curr_obj_list.at(i).id)
+      {
+        bool bFound = false;
+        for(unsigned int k=0; k < m_temp_list_ii.size(); k++)
+        {
+          if(m_temp_list_ii.at(k) == old_obj_list.at(ip))
+          {
+            bFound = true;
+            break;
+          }
+        }
+
+        if(!bFound)
+        {
+          old_obj_list.at(ip)->obj = curr_obj_list.at(i);
+          m_temp_list_ii.push_back(old_obj_list.at(ip));
+        }
+
+        DeleteFromList(delete_me_list, old_obj_list.at(ip));
+
+        old_obj_list.erase(old_obj_list.begin()+ip);
+        bMatch = true;
+        break;
+      }
+    }
+
+    if(!bMatch)
+    {
+      ObjParticles* pNewObj = new  ObjParticles();
+      pNewObj->obj = curr_obj_list.at(i);
+      m_temp_list_ii.push_back(pNewObj);
+    }
+  }
+
+  DeleteTheRest(delete_me_list);
+  old_obj_list.clear();
+  old_obj_list = m_temp_list_ii;
+
+  //m_PredictedObjects.clear();
+  for(unsigned int ip=0; ip < old_obj_list.size(); ip++)
+  {
+    PredictCurrentTrajectory(map, old_obj_list.at(ip));
+    //m_PredictedObjects.push_back(old_obj_list.at(ip)->obj);
+    old_obj_list.at(ip)->MatchTrajectories();
+  }
+
+}
+
+void BehaviorPrediction::CalPredictionTimeForObject(ObjParticles* pCarPart)
+{
+  if(pCarPart->obj.center.v > 0 )
+    pCarPart->m_PredictionTime = (MIN_PREDICTION_TIME + 1.5*pCarPart->obj.center.v) / pCarPart->obj.center.v;
+  else
+    pCarPart->m_PredictionTime = MIN_PREDICTION_TIME;
+}
+
+void BehaviorPrediction::PredictCurrentTrajectory(RoadNetwork& map, ObjParticles* pCarPart)
+{
+  pCarPart->obj.predTrajectories.clear();
+  PlannerH planner;
+  if(pCarPart->obj.bDirection && pCarPart->obj.bVelocity)
+  {
+    PlannerHNS::WayPoint fake_pose = pCarPart->obj.center;
+    pCarPart->obj.pClosestWaypoints = MappingHelpers::GetClosestWaypointsListFromMap(fake_pose, map, m_MaxLaneDetectionDistance, pCarPart->obj.bDirection);
+    planner.PredictTrajectoriesUsingDP(fake_pose, pCarPart->obj.pClosestWaypoints, m_PredictionDistance, pCarPart->obj.predTrajectories, m_bGenerateBranches, pCarPart->obj.bDirection);
+  }
+  else
+  {
+    bool bLocalDirectionSearch = false;
+    pCarPart->obj.pClosestWaypoints = MappingHelpers::GetClosestWaypointsListFromMap(pCarPart->obj.center, map, m_MaxLaneDetectionDistance, pCarPart->obj.bDirection);
+    if(pCarPart->obj.pClosestWaypoints.size()>0)
+    {
+      pCarPart->obj.center.pos.a = pCarPart->obj.pClosestWaypoints.at(0)->pos.a;
+      bLocalDirectionSearch = true;
+    }
+    planner.PredictTrajectoriesUsingDP(pCarPart->obj.center, pCarPart->obj.pClosestWaypoints, m_PredictionDistance, pCarPart->obj.predTrajectories, m_bGenerateBranches, pCarPart->obj.bDirection);
+  }
+
+
+  for(unsigned int t = 0; t < pCarPart->obj.predTrajectories.size(); t ++)
+  {
+//    std::ostringstream path_name;
+//    path_name << "/home/hatem/autoware_openplanner_logs/TempPredLog/";
+//    path_name << t ;
+//    path_name << "_";
+//    PlanningHelpers::GenerateRecommendedSpeed(pCarPart->obj.predTrajectories.at(t), 10, 1.0);
+//    PlannerHNS::PlanningHelpers::WritePathToFile(path_name.str(), pCarPart->obj.predTrajectories.at(t));
+    if(pCarPart->obj.predTrajectories.at(t).size() > 0)
+      pCarPart->obj.predTrajectories.at(t).at(0).collisionCost = 0;
+  }
+}
+
+void BehaviorPrediction::ParticleFilterSteps(std::vector<ObjParticles*>& part_info)
+{
+  for(unsigned int i=0; i < part_info.size(); i++)
+  {
+    SamplesFreshParticles(part_info.at(i));
+    CollectParticles(part_info.at(i));
+    MoveParticles(part_info.at(i));
+    CalculateWeights(part_info.at(i));
+    RemoveWeakParticles(part_info.at(i));
+    CalculateAveragesAndProbabilities(part_info.at(i));
+    FindBest(part_info.at(i));
+  }
+}
+
+int BehaviorPrediction::FromIndicatorToNumber(const PlannerHNS::LIGHT_INDICATOR& indi)
+{
+  if(indi == PlannerHNS::INDICATOR_NONE)
+    return 0;
+  else if(indi == PlannerHNS::INDICATOR_LEFT)
+  {
+    return 1;
+  }
+  else if(indi == PlannerHNS::INDICATOR_RIGHT)
+    return 2;
+  else if(indi == PlannerHNS::INDICATOR_BOTH)
+    return 3;
+  else
+    return 0;
+}
+
+PlannerHNS::LIGHT_INDICATOR BehaviorPrediction::FromNumbertoIndicator(const int& num)
+{
+  if(num == 0)
+    return PlannerHNS::INDICATOR_NONE;
+  else if(num == 1)
+    return PlannerHNS::INDICATOR_LEFT;
+  else if(num == 2)
+    return PlannerHNS::INDICATOR_RIGHT;
+  else if(num == 3)
+    return PlannerHNS::INDICATOR_BOTH;
+  else
+    return PlannerHNS::INDICATOR_NONE;
+}
+
+double BehaviorPrediction::CalcIndicatorWeight(PlannerHNS::LIGHT_INDICATOR p_ind, PlannerHNS::LIGHT_INDICATOR obj_ind)
+{
+  if((obj_ind == PlannerHNS::INDICATOR_LEFT || obj_ind == PlannerHNS::INDICATOR_RIGHT || obj_ind == PlannerHNS::INDICATOR_NONE) && p_ind == obj_ind)
+    return 0.99;
+  else
+    return 0.01;
+}
+
+double BehaviorPrediction::CalcAccelerationWeight(int p_acl, int obj_acl)
+{
+  if((p_acl > 0 && obj_acl > 0) || (p_acl < 0 && obj_acl < 0))
+    return 0.99;
+  else
+    return 0.01;
+}
+
+void BehaviorPrediction::CalOnePartWeight(ObjParticles* pParts,Particle& p)
+{
+  if(p.bDeleted) return;
+
+  //p.pose_w = exp(-(0.5*pow((p.pose.pos.x - pParts->obj.center.pos.x),2)/(2*MEASURE_POSE_ERROR*MEASURE_POSE_ERROR)+ pow((p.pose.pos.y - pParts->obj.center.pos.y),2)/(2*MEASURE_POSE_ERROR*MEASURE_POSE_ERROR)));
+  p.pose_w = 1.0/hypot(0.5*(p.pose.pos.y - pParts->obj.center.pos.y), 0.5*(p.pose.pos.x - pParts->obj.center.pos.x));
+  //p.dir_w  = exp(-(pow(fabs(UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(p.pose.pos.a,  pParts->obj.center.pos.a)),2)/(2*MEASURE_ANGLE_ERROR*MEASURE_ANGLE_ERROR)));
+  p.dir_w  = M_PI_2 - fabs(UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(p.pose.pos.a,  pParts->obj.center.pos.a));
+  p.vel_w  = exp(-(pow((p.vel - pParts->obj.center.v),2)/(2*MEASURE_VEL_ERROR*MEASURE_VEL_ERROR)));
+  //p.vel_w = fabs(p.vel - pParts->obj.center.v);
+  p.ind_w  = CalcIndicatorWeight(FromNumbertoIndicator(p.indicator), pParts->obj.indicator_state);
+  p.ind_w  -= p.ind_w*MEASURE_IND_ERROR;
+  p.acl_w = CalcAccelerationWeight(p.acc, pParts->obj.acceleration_desc);
+
+  //std::cout << p.beh << "|" << p.vel_w <<"|" <<p.vel << "|" << pParts->obj.center.v;
+
+  pParts->pose_w_t += p.pose_w;
+  pParts->dir_w_t += p.dir_w;
+  pParts->vel_w_t += p.vel_w;
+  pParts->ind_w_t += p.ind_w;
+  pParts->acl_w_t += p.acl_w;
+
+  if(p.pose_w > pParts->pose_w_max)
+    pParts->pose_w_max = p.pose_w;
+  if(p.dir_w > pParts->dir_w_max)
+    pParts->dir_w_max = p.dir_w;
+  if(p.vel_w > pParts->vel_w_max)
+    pParts->vel_w_max = p.vel_w;
+  if(p.ind_w > pParts->ind_w_max)
+    pParts->ind_w_max = p.ind_w;
+  if(p.acl_w > pParts->acl_w_max)
+    pParts->acl_w_max = p.acl_w;
+
+  if(p.pose_w < pParts->pose_w_min)
+    pParts->pose_w_min = p.pose_w;
+  if(p.dir_w < pParts->dir_w_min)
+    pParts->dir_w_min = p.dir_w;
+  if(p.vel_w < pParts->vel_w_min)
+    pParts->vel_w_min = p.vel_w;
+  if(p.ind_w < pParts->ind_w_min)
+    pParts->ind_w_min = p.ind_w;
+  if(p.acl_w < pParts->acl_w_min)
+    pParts->acl_w_min = p.acl_w;
+
+  p.w_raw = p.pose_w*POSE_FACTOR + p.dir_w*DIRECTION_FACTOR + p.vel_w*VELOCITY_FACTOR + p.ind_w*INDICATOR_FACTOR + p.acl_w*ACCELERATE_FACTOR;
+
+  if(p.w_raw >= pParts->max_w_raw)
+    pParts->max_w_raw = p.w_raw;
+
+  if(p.w_raw <= pParts->min_w_raw)
+    pParts->min_w_raw = p.w_raw;
+}
+
+void BehaviorPrediction::NormalizeOnePartWeight(ObjParticles* pParts,Particle& p)
+{
+  if(p.bDeleted) return;
+
+  double pose_diff  = pParts->pose_w_max-pParts->pose_w_min;
+  double dir_diff = pParts->dir_w_max-pParts->dir_w_min;
+  double vel_diff = pParts->vel_w_max-pParts->vel_w_min;
+  double ind_diff = pParts->ind_w_max-pParts->ind_w_min;
+  double acl_diff = pParts->acl_w_max-pParts->acl_w_min;
+
+  double epsilon = 0.05;
+
+  if(fabs(pose_diff) > epsilon)
+    p.pose_w = p.pose_w/pose_diff;
+  else
+    p.pose_w = 0;
+
+  if(p.pose_w > 1.0 ) p.pose_w = 1.0;
+
+  if(fabs(dir_diff) > epsilon)
+    p.dir_w = (p.dir_w - pParts->dir_w_min)/dir_diff;
+  else
+    p.dir_w = 0;
+
+  if(p.dir_w > 1.0 ) p.dir_w = 1.0;
+
+  if(fabs(vel_diff) > epsilon)
+    p.vel_w = (p.vel_w-pParts->vel_w_min)/vel_diff;
+  else
+    p.vel_w = 0;
+
+  if(p.vel_w > 1.0) p.vel_w = 1.0;
+
+  if(fabs(ind_diff) > epsilon)
+    p.ind_w = (p.ind_w - pParts->ind_w_min)/ind_diff;
+  else
+    p.ind_w = 0;
+
+  if(p.ind_w > 1.0) p.ind_w = 1.0;
+
+  if(fabs(acl_diff) > epsilon)
+    p.acl_w = (p.acl_w - pParts->acl_w_min)/acl_diff;
+  else
+    p.acl_w = 0;
+
+  if(p.acl_w > 1.0) p.acl_w = 1.0;
+
+  p.w = p.pose_w*POSE_FACTOR + p.dir_w*DIRECTION_FACTOR + p.vel_w*VELOCITY_FACTOR + p.ind_w*INDICATOR_FACTOR + p.acl_w*ACCELERATE_FACTOR;
+  //p.w = p.pose_w*0.1 + p.vel_w*0.2 + p.dir_w * 0.2 + p.ind_w + 0.5;
+
+  if(p.w >= pParts->max_w)
+    pParts->max_w = p.w;
+
+  if(p.w <= pParts->min_w)
+    pParts->min_w = p.w;
+
+    pParts->all_w += p.w;
+}
+
+void BehaviorPrediction::CalculateWeights(ObjParticles* pParts)
+{
+  pParts->all_w = 0;
+  pParts->pose_w_t = 0;
+  pParts->dir_w_t = 0;
+  pParts->vel_w_t = 0;
+  pParts->acl_w_t = 0;
+
+  pParts->pose_w_max = -99999999;
+  pParts->dir_w_max = -99999999;
+  pParts->vel_w_max = -99999999;
+  pParts->acl_w_max = -99999999;
+
+  pParts->pose_w_min = 99999999;
+  pParts->dir_w_min = 99999999;
+  pParts->vel_w_min = 99999999;
+  pParts->acl_w_min = 99999999;
+
+  pParts->max_w_raw = DBL_MIN;
+  pParts->min_w_raw = DBL_MAX;
+
+  //std::cout << "Acceleration Diff: ";
+  for(unsigned int i = 0 ; i < pParts->m_AllParticles.size(); i++)
+  {
+    //  std::cout << "(" << pParts->m_AllParticles.at(i)->pTraj->index <<",";
+      CalOnePartWeight(pParts, *pParts->m_AllParticles.at(i));
+      //std::cout << ")";
+  }
+
+  //std::cout << "Befor Normalize: Max: " <<  pParts->acl_w_max << ", Min: " << pParts->acl_w_min << std::endl;
+  //std::cout << std::endl;
+
+  //if((pParts->m_TrajectoryTracker.size() > 1 && pParts->min_w_raw < 0.5) || pParts->max_w_raw == 0 || fabs(pParts->max_w_raw - pParts->min_w_raw) < 0.1 )
+  if((pParts->max_w_raw == 0 || fabs(pParts->max_w_raw - pParts->min_w_raw) < 0.1 || pParts->min_w_raw > 0.5) && pParts->m_TrajectoryTracker.size() > 1)
+    m_bCanDecide = false;
+  else
+    m_bCanDecide = true;
+
+  //Normalize
+  pParts->max_w = -9999999;
+  pParts->min_w = 9999999;
+  pParts->all_w = 0;
+
+  for(unsigned int i = 0 ; i < pParts->m_AllParticles.size(); i++)
+  {
+    NormalizeOnePartWeight(pParts, *pParts->m_AllParticles.at(i));
+  }
+}
+
+void BehaviorPrediction::CalculateAveragesAndProbabilities(ObjParticles* pParts)
+{
+  for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
+  {
+    pParts->m_TrajectoryTracker.at(t)->CalcAverages();
+    pParts->m_TrajectoryTracker.at(t)->CalcProbabilities();
+  }
+}
+
+void BehaviorPrediction::RemoveWeakParticles(ObjParticles* pParts)
+{
+
+  double critical_val = pParts->min_w + (pParts->max_w - pParts->min_w)*KEEP_PERCENTAGE;
+//  std::cout << "Behavior Weights ------------------------------------------------ " << std::endl;
+//  std::cout << "Max Raw: " << pParts->max_w_raw << ", Min Raw: " << pParts->min_w_raw << std::endl;
+//  std::cout << "Keep Percent Value: " << critical_val <<  std::endl;
+//
+//  if(pParts->obj.acceleration > 0 )
+//    std::cout << "Accelerate vrooom vroom : " << std::endl;
+//  else if(pParts->obj.acceleration  < 0 )
+//    std::cout << "Brake Eeeeee Eeeeeeee: " << std::endl;
+
+  for(int i =0 ; i < pParts->m_AllParticles.size(); i++)
+  {
+    //also delete far particle
+    double d = hypot(pParts->obj.center.pos.y - pParts->m_AllParticles.at(i)->pose.pos.y, pParts->obj.center.pos.x - pParts->m_AllParticles.at(i)->pose.pos.x);
+
+    if(pParts->m_AllParticles.at(i)->w < critical_val || d > m_PredictionDistance)
+    {
+      pParts->m_AllParticles.at(i)->pTraj->DeleteParticle(*(pParts->m_AllParticles.at(i)), pParts->m_AllParticles.at(i)->original_index);
+      pParts->m_AllParticles.erase(pParts->m_AllParticles.begin()+i);
+      i--;
+    }
+  }
+}
+
+void BehaviorPrediction::FindBest(ObjParticles* pParts)
+{
+  if(pParts->m_TrajectoryTracker.size() > 0)
+  {
+    pParts->best_beh_track = pParts->m_TrajectoryTracker.at(0);
+    pParts->i_best_track = 0;
+  }
+
+  for(unsigned int t = 1; t < pParts->m_TrajectoryTracker.size(); t++)
+  {
+    if(pParts->m_TrajectoryTracker.at(t)->best_p > pParts->best_beh_track->best_p)
+    {
+      pParts->best_beh_track = pParts->m_TrajectoryTracker.at(t);
+      pParts->i_best_track = t;
+    }
+  }
+
+
+  if(m_bDebugOut )
+  {
+    std::cout << "Behavior Prob ------------------------------------------------ : " << pParts->m_TrajectoryTracker.size() << std::endl;
+    std::cout << "Raw  Weights: Max: " <<  pParts->max_w_raw << ", Min: " << pParts->min_w_raw << std::endl;
+    std::cout << "Norm Weights: Max: " <<  pParts->max_w << ", Min: " << pParts->min_w << std::endl;
+  }
+
+  for(unsigned int t=0; t < pParts->obj.predTrajectories.size() ; t++)
+  {
+    if(pParts->obj.predTrajectories.at(t).size()>0)
+    {
+      pParts->obj.predTrajectories.at(t).at(0).collisionCost = 0.25;
+    }
+  }
+
+  if(m_bCanDecide && pParts->best_beh_track != nullptr)
+  {
+    std::string str_beh = "Unknown";
+    if(pParts->best_beh_track->best_beh == BEH_STOPPING_STATE)
+      str_beh = "Stopping";
+    else if(pParts->best_beh_track->best_beh == BEH_FORWARD_STATE)
+      str_beh = "Forward";
+
+    if(m_bDebugOut )
+      std::cout << "Trajectory (" << pParts->i_best_track << "), P: " << pParts->best_beh_track->best_p << " , Beh (" << pParts->best_beh_track->best_beh << ", " << str_beh << ")" << std::endl;
+
+    if(pParts->best_beh_track->index < pParts->obj.predTrajectories.size())
+      pParts->obj.predTrajectories.at(pParts->best_beh_track->index).at(0).collisionCost = 1;
+
+  }
+  else
+  {
+    if(m_bDebugOut )
+      std::cout << "Trajectory (" << -1 << "), P: " << 0 << " , Beh (" << -1 << ", " << "Can't Decide" << ")" << std::endl;
+  }
+
+  if(m_bDebugOut )
+  {
+    for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size() ; t++)
+    {
+      if(pParts->m_TrajectoryTracker.at(t)->nAliveForward > 0)
+        std::cout << t << ", Forward Particles:" << pParts->m_TrajectoryTracker.at(t)->nAliveForward << std::endl;
+      if(pParts->m_TrajectoryTracker.at(t)->nAliveStop > 0)
+        std::cout << t << ", Stoping Particles:" << pParts->m_TrajectoryTracker.at(t)->nAliveStop << std::endl;
+    }
+
+    std::cout << "------------------------------------------------ --------------" << std::endl<< std::endl;
+  }
+}
+
+void BehaviorPrediction::SamplesFreshParticles(ObjParticles* pParts)
+{
+  timespec _time;
+  UtilityHNS::UtilityH::GetTickCount(_time);
+  srand(_time.tv_nsec);
+
+  ENG eng(_time.tv_nsec);
+  NormalDIST dist_x(0, MOTION_POSE_ERROR);
+  VariatGEN gen_x(eng, dist_x);
+  NormalDIST vel(MOTION_VEL_ERROR, MOTION_VEL_ERROR);
+  VariatGEN gen_v(eng, vel);
+  NormalDIST ang(0, MOTION_ANGLE_ERROR);
+  VariatGEN gen_a(eng, ang);
+//  NormalDIST acl(0, MEASURE_ACL_ERROR);
+//  VariatGEN gen_acl(eng, acl);
+
+  Particle p;
+  p.pose = pParts->obj.center;
+  p.vel = 0;
+  p.acc = 0;
+  p.indicator = 0;
+  bool bRegenerate = true;
+
+  if(UtilityHNS::UtilityH::GetTimeDiffNow(m_GenerationTimer) > 2)
+  {
+    UtilityHNS::UtilityH::GetTickCount(m_GenerationTimer);
+    bRegenerate = true;
+  }
+
+//  for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
+//  {
+//    PlanningHelpers::FixPathDensity(pParts->m_TrajectoryTracker.at(t)->trajectory, 0.5);
+//    PlanningHelpers::CalcAngleAndCost(pParts->m_TrajectoryTracker.at(t)->trajectory);
+//  }
+
+  for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
+  {
+    RelativeInfo info;
+    PlanningHelpers::GetRelativeInfo(pParts->m_TrajectoryTracker.at(t)->trajectory, pParts->obj.center, info);
+    unsigned int point_index = 0;
+    p.pose = PlanningHelpers::GetFollowPointOnTrajectory(pParts->m_TrajectoryTracker.at(t)->trajectory, info, PREDICTION_DISTANCE_PERCENTAGE*m_PredictionDistance, point_index);
+
+    if(pParts->m_TrajectoryTracker.at(t)->beh == PlannerHNS::BEH_FORWARD_STATE && pParts->m_TrajectoryTracker.at(t)->nAliveForward < BEH_PARTICLES_NUM)
+    {
+      p.beh = PlannerHNS::BEH_FORWARD_STATE;
+      int nPs = BEH_PARTICLES_NUM - pParts->m_TrajectoryTracker.at(t)->nAliveForward;
+
+      for(unsigned int i=0; i < nPs; i++)
+      {
+        Particle p_new = p;
+        p_new.pose.pos.x += gen_x();
+        p_new.pose.pos.y += gen_x();
+        p_new.pose.pos.a += gen_a();
+        p_new.vel = pParts->obj.center.v + fabs(gen_v());
+        p_new.pose.v = p_new.vel;
+        pParts->m_TrajectoryTracker.at(t)->InsertNewParticle(p_new);
+      }
+    }
+
+    if(ENABLE_STOP_BEHAVIOR_GEN == 1 && pParts->m_TrajectoryTracker.at(t)->nAliveStop <   BEH_PARTICLES_NUM)
+    {
+      p.beh = PlannerHNS::BEH_STOPPING_STATE;
+      int nPs = BEH_PARTICLES_NUM - pParts->m_TrajectoryTracker.at(t)->nAliveStop;
+
+      for(unsigned int i=0; i < nPs; i++)
+      {
+        Particle p_new = p;
+        p_new.pose.pos.x += gen_x();
+        p_new.pose.pos.y += gen_x();
+        p_new.pose.pos.a += gen_a();
+        p_new.vel = 0;
+        p_new.pose.v = pParts->obj.center.v + fabs(gen_v());
+        pParts->m_TrajectoryTracker.at(t)->InsertNewParticle(p_new);
+      }
+    }
+  }
+}
+
+void BehaviorPrediction::CollectParticles(ObjParticles* pParts)
+{
+  pParts->m_AllParticles.clear();
+  for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
+  {
+    for(unsigned int i=0; i < pParts->m_TrajectoryTracker.at(t)->m_ForwardPart.size(); i++)
+    {
+      pParts->m_TrajectoryTracker.at(t)->m_ForwardPart.at(i).original_index = i;
+      pParts->m_AllParticles.push_back(&pParts->m_TrajectoryTracker.at(t)->m_ForwardPart.at(i));
+    }
+
+    for(unsigned int i=0; i < pParts->m_TrajectoryTracker.at(t)->m_StopPart.size(); i++)
+    {
+      pParts->m_TrajectoryTracker.at(t)->m_StopPart.at(i).original_index = i;
+      pParts->m_AllParticles.push_back(&pParts->m_TrajectoryTracker.at(t)->m_StopPart.at(i));
+    }
+  }
+}
+
+void BehaviorPrediction::MoveParticles(ObjParticles* pParts)
+{
+  double dt = 0.01;
+  if(m_bStepByStep)
+  {
+    dt = 0.08;
+  }
+  else
+  {
+    dt = UtilityHNS::UtilityH::GetTimeDiffNow(m_ResamplingTimer);
+    UtilityHNS::UtilityH::GetTickCount(m_ResamplingTimer);
+    if(m_bFirstMove)
+    {
+      m_bFirstMove  = false;
+      return;
+    }
+  }
+
+  PlannerHNS::BehaviorState curr_behavior;
+  PlannerHNS::ParticleInfo curr_part_info;
+  PlannerHNS::VehicleState control_u;
+  PassiveDecisionMaker decision_make;
+  PlannerHNS::CAR_BASIC_INFO carInfo;
+  carInfo.width = pParts->obj.w;
+  carInfo.length = pParts->obj.l;
+  carInfo.max_acceleration = 3.0;
+  carInfo.max_deceleration = -3;
+  carInfo.max_speed_forward = MAX_PREDICTION_SPEED;
+  carInfo.min_speed_forward = 0;
+  carInfo.max_steer_angle = 0.4;
+  carInfo.min_steer_angle = -0.4;
+  carInfo.turning_radius = 7.2;
+  carInfo.wheel_base = carInfo.length*0.75;
+
+  for(unsigned int t=0; t < pParts->m_TrajectoryTracker.size(); t++)
+  {
+    PlanningHelpers::GenerateRecommendedSpeed(pParts->m_TrajectoryTracker.at(t)->trajectory, carInfo.max_speed_forward, 1.0);
+  }
+
+//  std::cout << "Motion Status------ " << std::endl;
+//  if(pParts->obj.acceleration_desc == 1)
+//    std::cout << "Acceleration: " << pParts->obj.acceleration_raw << ", Accelerate: " << pParts->obj.acceleration_desc << std::endl;
+//  else if(pParts->obj.acceleration_desc == -1)
+//    std::cout << "Acceleration: " << pParts->obj.acceleration_raw << ", Braking   : " << pParts->obj.acceleration_desc << std::endl;
+//  else
+//    std::cout << "Acceleration: " << pParts->obj.acceleration_raw << ", Cruising  : " << pParts->obj.acceleration_desc << std::endl;
+
+  for(unsigned int i=0; i < pParts->m_AllParticles.size(); i++)
+  {
+    Particle* p = pParts->m_AllParticles.at(i);
+
+    if(USE_OPEN_PLANNER_MOVE == 0)
+      {
+      p->pose.v = pParts->obj.center.v;
+      curr_part_info = decision_make.MoveStepSimple(dt, p->pose, p->pTraj->trajectory,carInfo);
+      if(p->prev_time_diff > ACCELERATION_CALC_TIME)
+      {
+        p->acc_raw = (curr_part_info.vel - p->vel_prev_big)/p->prev_time_diff;
+        p->vel_prev_big = curr_part_info.vel;
+        p->prev_time_diff = 0;
+      }
+      else
+      {
+        p->prev_time_diff += dt;
+      }
+
+      if(fabs(p->acc_raw) < ACCELERATION_DECISION_VALUE)
+        p->acc = 0;
+      else if(p->acc_raw > ACCELERATION_DECISION_VALUE)
+        p->acc = 1;
+      else if(p->acc_raw < -ACCELERATION_DECISION_VALUE)
+        p->acc = -1;
+
+      p->indicator = FromIndicatorToNumber(curr_part_info.indicator);
+
+//      if(curr_behavior.state == PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_YIELDING_STATE)
+//        p->vel += 1;
+//      else if(p->beh == PlannerHNS::BEH_YIELDING_STATE)
+//        p->vel = p->vel/2.0;
+//      else if(curr_behavior.state != PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_STOPPING_STATE)
+//        p->vel += 1;
+//      else if(p->beh == PlannerHNS::BEH_STOPPING_STATE)
+//        p->vel = 0;
+
+//      if(curr_behavior.state != PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_STOPPING_STATE)
+//        p->vel += 1;
+      if(p->beh == PlannerHNS::BEH_STOPPING_STATE)
+      {
+        p->vel = 0;
+        if(p->acc == 0)
+          p->acc = -1;
+        else if(p->acc == 1)
+          p->acc = 0;
+      }
+
+      }
+    else
+      {
+      curr_behavior = decision_make.MoveStep(dt, p->pose, p->pTraj->trajectory,carInfo);
+      p->acc = UtilityHNS::UtilityH::GetSign(curr_behavior.maxVelocity - p->vel_prev_big);
+      p->vel = curr_behavior.maxVelocity;
+      if(fabs(p->vel - p->vel_prev_big) > 0.5)
+        p->vel_prev_big = p->vel;
+      p->indicator = FromIndicatorToNumber(curr_behavior.indicator);
+//      if(p->indicator == 0)
+//        std::cout << ", Off";
+//      else if(p->indicator == 1)
+//        std::cout << ", Left";
+//      else if(p->indicator == 2)
+//        std::cout << ", Right";
+
+      if(curr_behavior.state == PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_YIELDING_STATE)
+        p->vel += 1;
+      else if(p->beh == PlannerHNS::BEH_YIELDING_STATE)
+        p->vel = p->vel/2.0;
+      else if(curr_behavior.state != PlannerHNS::STOPPING_STATE && p->beh == PlannerHNS::BEH_STOPPING_STATE)
+        p->vel += 1;
+      else if(p->beh == PlannerHNS::BEH_STOPPING_STATE)
+      {
+        p->vel = 0;
+      }
+
+      }
+  }
+  //std::cout << "End Motion Status ------ " << std::endl;
+}
+
+} /* namespace PlannerHNS */

src1/planning/op_planner/BehaviorStateMachine.cpp

@@ -0,0 +1,518 @@
+
+/// \file BehaviorStateMachine.cpp
+/// \author Hatem Darweesh
+/// \brief OpenPlanner's state machine implementation for different driving behaviors
+/// \date Jun 19, 2016
+
+#include "op_planner/BehaviorStateMachine.h"
+#include "op_utility/UtilityH.h"
+#include <iostream>
+
+using namespace UtilityHNS;
+
+
+namespace PlannerHNS
+{
+
+BehaviorStateMachine::BehaviorStateMachine(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* nextState)
+{
+  m_Behavior = INITIAL_STATE;
+
+  m_currentStopSignID    = -1;
+  m_currentTrafficLightID  = -1;
+  decisionMakingTime    = 0.0;
+  decisionMakingCount    = 1;
+  m_zero_velocity     = 0.1;
+
+  if(!pPreCalcVal)
+    m_pCalculatedValues = new PreCalculatedConditions();
+  else
+    m_pCalculatedValues = pPreCalcVal;
+
+  if(!pParams)
+    m_pParams = new PlanningParams;
+  else
+    m_pParams = pParams;
+
+  if(nextState)
+    pNextStates.push_back(nextState);
+
+  pNextStates.push_back(this);
+
+  Init();
+}
+
+void BehaviorStateMachine::InsertNextState(BehaviorStateMachine* nextState)
+{
+  if(nextState)
+    pNextStates.push_back(nextState);
+}
+
+void BehaviorStateMachine::UpdateLogCount(BehaviorStateMachine* pState)
+{
+  if(!pState) return;
+
+  bool bFound = false;
+  for(unsigned int i = 0; i < m_BehaviorsLog.size(); i++)
+  {
+    if(m_BehaviorsLog.at(i).first->m_Behavior == pState->m_Behavior)
+    {
+      m_BehaviorsLog.at(i).second++;
+      bFound = true;
+      break;
+    }
+  }
+
+  if(!bFound)
+  {
+    m_BehaviorsLog.push_back(std::make_pair(pState, 1));
+  }
+}
+
+BehaviorStateMachine* BehaviorStateMachine::FindBestState(int nMinCount)
+{
+  for(unsigned int i = 0; i < m_BehaviorsLog.size(); i++)
+  {
+    if(m_BehaviorsLog.at(i).second >= nMinCount)
+    {
+      //std::cout << "Found Next Beh: " << m_BehaviorsLog.at(i).first->m_Behavior << ", Count: " << m_BehaviorsLog.at(i).second  << ", LogSize: " << m_BehaviorsLog.size() << std::endl;
+      return m_BehaviorsLog.at(i).first;
+    }
+  }
+
+  return nullptr;
+}
+
+BehaviorStateMachine* BehaviorStateMachine::FindBehaviorState(const STATE_TYPE& behavior)
+{
+  for(unsigned int i = 0 ; i < pNextStates.size(); i++)
+  {
+    BehaviorStateMachine* pState = pNextStates.at(i);
+    if(pState && behavior == pState->m_Behavior )
+    {
+      //UpdateLogCount(pState);
+      //pState = FindBestState(decisionMakingCount);
+
+      if(pState == 0) return this;
+
+      m_BehaviorsLog.clear();
+      pState->ResetTimer();
+      return pState;
+    }
+  }
+
+  return nullptr;
+}
+
+void BehaviorStateMachine::Init()
+{
+  UtilityH::GetTickCount(m_StateTimer);
+}
+
+void BehaviorStateMachine::ResetTimer()
+{
+  UtilityH::GetTickCount(m_StateTimer);
+}
+
+BehaviorStateMachine::~BehaviorStateMachine()
+{
+}
+
+BehaviorStateMachine* ForwardState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this; //return this behavior only , without reset
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->currentGoalID != pCParams->prevGoalID)
+    return FindBehaviorState(GOAL_STATE);
+
+  else if(m_pParams->enableSwerving
+      && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid
+      && !pCParams->bFullyBlock
+      && (pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory || pCParams->iCurrSafeLane != pCParams->iPrevSafeLane)
+      )
+    return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
+
+  else if(m_pParams->enableTrafficLightBehavior
+      && pCParams->currentTrafficLightID > 0
+      && pCParams->bTrafficIsRed
+      && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
+    return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+
+  else if(m_pParams->enableStopSignBehavior
+      && pCParams->currentStopSignID > 0
+      && pCParams->currentStopSignID != pCParams->prevStopSignID)
+      return FindBehaviorState(STOP_SIGN_STOP_STATE);
+
+  else if(m_pParams->enableFollowing
+      && pCParams->bFullyBlock)
+      return FindBehaviorState(FOLLOW_STATE);
+
+//  else if(pCParams->distanceToNext <= m_pParams->maxDistanceToAvoid)
+//    return FindBehaviorState(STOPPING_STATE);
+
+  else
+  {
+    if(pCParams->iCurrSafeTrajectory == pCParams->iCentralTrajectory
+        && pCParams->iPrevSafeTrajectory != pCParams->iCurrSafeTrajectory)
+      pCParams->bRePlan = true;
+
+    return FindBehaviorState(this->m_Behavior); // return and reset
+  }
+}
+
+BehaviorStateMachine* MissionAccomplishedState::GetNextState()
+{
+  return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* StopState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->distanceToNext > m_pParams->maxDistanceToAvoid)
+    return FindBehaviorState(FORWARD_STATE);
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* TrafficLightStopState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(!pCParams->bTrafficIsRed)
+  {
+    pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
+    return FindBehaviorState(FORWARD_STATE);
+  }
+
+  else if(pCParams->bTrafficIsRed && pCParams->currentVelocity <= m_zero_velocity)
+      return FindBehaviorState(TRAFFIC_LIGHT_WAIT_STATE);
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* TrafficLightWaitState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(!pCParams->bTrafficIsRed)
+  {
+    pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
+    return FindBehaviorState(FORWARD_STATE);
+  }
+
+  else if(pCParams->currentVelocity > m_zero_velocity)
+    return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+
+}
+
+BehaviorStateMachine* StopSignStopState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  //std::cout << "From Stop Beh D: " << pCParams->distanceToStop() << ", Prev LineID: " << pCParams->prevStopSignID << ", Curr SignID: " << pCParams->currentStopSignID << std::endl;
+
+  if(pCParams->currentVelocity < m_zero_velocity)
+    return FindBehaviorState(STOP_SIGN_WAIT_STATE);
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* StopSignWaitState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  //std::cout << "From Wait Beh D: " << pCParams->distanceToStop() << ", Prev LineID: " << pCParams->prevStopSignID << ", Curr SignID: " << pCParams->currentStopSignID << std::endl;
+
+  pCParams->prevStopSignID = pCParams->currentStopSignID;
+
+  return FindBehaviorState(FORWARD_STATE);
+}
+
+BehaviorStateMachine* WaitState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  //PreCalculatedConditions* pCParams = GetCalcParams();
+
+  return FindBehaviorState(FORWARD_STATE);
+}
+
+BehaviorStateMachine* InitState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->bOutsideControl == 1)
+  {
+    pCParams->prevGoalID = pCParams->currentGoalID;
+    return FindBehaviorState(FORWARD_STATE);
+  }
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* FollowState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+      return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  //std::cout << "Following State >> followDistance: " << pCParams->distanceToNext << ", followSpeed: " << pCParams->velocityOfNext << std::endl;
+
+  if(m_pParams->enableTrafficLightBehavior
+        && pCParams->currentTrafficLightID > 0
+        && pCParams->bTrafficIsRed
+        && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
+      return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+
+  else if(m_pParams->enableStopSignBehavior
+      && pCParams->currentStopSignID > 0
+      && pCParams->currentStopSignID != pCParams->prevStopSignID)
+      return FindBehaviorState(STOP_SIGN_STOP_STATE);
+
+  else if(pCParams->currentGoalID != pCParams->prevGoalID
+      || !pCParams->bFullyBlock)
+    return FindBehaviorState(FORWARD_STATE);
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* SwerveState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->distanceToNext > 0
+        && pCParams->distanceToNext < m_pParams->minDistanceToAvoid
+        && !pCParams->bFullyBlock
+        && pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
+    return FindBehaviorState(this->m_Behavior);
+
+  else
+    return FindBehaviorState(FORWARD_STATE);
+}
+
+BehaviorStateMachine* GoalState::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->currentGoalID == -1)
+    return FindBehaviorState(FINISH_STATE);
+
+  else if(pCParams->currentGoalID != pCParams->prevGoalID)
+  {
+    pCParams->prevGoalID = pCParams->currentGoalID;
+    return FindBehaviorState(FORWARD_STATE);
+  }
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* ForwardStateII::GetNextState()
+{
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->currentGoalID != pCParams->prevGoalID)
+    return FindBehaviorState(GOAL_STATE);
+
+  else if(m_pParams->enableTrafficLightBehavior
+        && pCParams->currentTrafficLightID > 0
+        && pCParams->bTrafficIsRed
+        && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
+      return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+
+  else if(m_pParams->enableStopSignBehavior
+      && pCParams->currentStopSignID > 0
+      && pCParams->currentStopSignID != pCParams->prevStopSignID)
+    return FindBehaviorState(STOP_SIGN_STOP_STATE);
+
+  else if(m_pParams->enableFollowing && pCParams->bFullyBlock)
+    return FindBehaviorState(FOLLOW_STATE);
+
+  else if(m_pParams->enableSwerving
+      && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid
+      && !pCParams->bFullyBlock
+      && pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
+    return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
+
+  else
+    return FindBehaviorState(this->m_Behavior);
+}
+
+BehaviorStateMachine* FollowStateII::GetNextState()
+{
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->currentGoalID != pCParams->prevGoalID)
+    return FindBehaviorState(GOAL_STATE);
+
+  else if(m_pParams->enableTrafficLightBehavior
+        && pCParams->currentTrafficLightID > 0
+        && pCParams->bTrafficIsRed
+        && pCParams->currentTrafficLightID != pCParams->prevTrafficLightID)
+      return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+
+  else if(m_pParams->enableStopSignBehavior
+      && pCParams->currentStopSignID > 0
+      && pCParams->currentStopSignID != pCParams->prevStopSignID)
+    return FindBehaviorState(STOP_SIGN_STOP_STATE);
+
+  else if(m_pParams->enableSwerving
+      && pCParams->distanceToNext <= m_pParams->minDistanceToAvoid
+      && !pCParams->bFullyBlock
+      && pCParams->iCurrSafeTrajectory != pCParams->iPrevSafeTrajectory)
+    return FindBehaviorState(OBSTACLE_AVOIDANCE_STATE);
+
+  else if(!pCParams->bFullyBlock)
+    return FindBehaviorState(FORWARD_STATE);
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* SwerveStateII::GetNextState()
+{
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  pCParams->iPrevSafeTrajectory = pCParams->iCurrSafeTrajectory;
+  pCParams->bRePlan = true;
+
+  return FindBehaviorState(FORWARD_STATE);
+}
+
+BehaviorStateMachine* InitStateII::GetNextState()
+{
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->currentGoalID > 0)
+    return FindBehaviorState(FORWARD_STATE);
+  else
+    return FindBehaviorState(this->m_Behavior);
+}
+
+BehaviorStateMachine* GoalStateII::GetNextState()
+{
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->currentGoalID == -1)
+    return FindBehaviorState(FINISH_STATE);
+
+  else
+  {
+    pCParams->prevGoalID = pCParams->currentGoalID;
+    return FindBehaviorState(FORWARD_STATE);
+  }
+}
+
+BehaviorStateMachine* MissionAccomplishedStateII::GetNextState()
+{
+  return FindBehaviorState(this->m_Behavior);
+}
+
+BehaviorStateMachine* StopSignStopStateII::GetNextState()
+{
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  if(pCParams->currentGoalID != pCParams->prevGoalID)
+    return FindBehaviorState(GOAL_STATE);
+
+  else if(pCParams->currentVelocity < m_zero_velocity)
+    return FindBehaviorState(STOP_SIGN_WAIT_STATE);
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+}
+
+BehaviorStateMachine* StopSignWaitStateII::GetNextState()
+{
+  if(UtilityH::GetTimeDiffNow(m_StateTimer) < decisionMakingTime)
+    return this;
+
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  pCParams->prevStopSignID = pCParams->currentStopSignID;
+
+  return FindBehaviorState(FORWARD_STATE);
+}
+
+BehaviorStateMachine* TrafficLightStopStateII::GetNextState()
+{
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  //std::cout << "Stopping for trafficLight "  << std::endl;
+  if(!pCParams->bTrafficIsRed)
+  {
+    //std::cout << "Color Changed Stopping for trafficLight "  << std::endl;
+    pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
+    return FindBehaviorState(FORWARD_STATE);
+  }
+
+  else if(pCParams->bTrafficIsRed && pCParams->currentVelocity <= m_zero_velocity)
+  {
+    //std::cout << "Velocity Changed Stopping for trafficLight ("  <<pCParams->currentVelocity << ", " << m_zero_velocity << ")" <<  std::endl;
+    return FindBehaviorState(TRAFFIC_LIGHT_WAIT_STATE);
+  }
+
+  else
+  {
+    return FindBehaviorState(this->m_Behavior); // return and reset
+  }
+}
+
+BehaviorStateMachine* TrafficLightWaitStateII::GetNextState()
+{
+  PreCalculatedConditions* pCParams = GetCalcParams();
+
+  //std::cout << "Wait for trafficLight "  << std::endl;
+
+  if(!pCParams->bTrafficIsRed)
+  {
+    pCParams->prevTrafficLightID = pCParams->currentTrafficLightID;
+    return FindBehaviorState(FORWARD_STATE);
+  }
+
+//  else if(pCParams->currentVelocity > m_zero_velocity)
+//    return FindBehaviorState(TRAFFIC_LIGHT_STOP_STATE);
+
+  else
+    return FindBehaviorState(this->m_Behavior); // return and reset
+
+}
+
+} /* namespace PlannerHNS */

src1/planning/op_planner/DecisionMaker.cpp

@@ -0,0 +1,489 @@
+
+/// \file DecisionMaker.cpp
+/// \brief Initialize behaviors state machine, and calculate required parameters for the state machine transition conditions
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+
+#include "op_planner/DecisionMaker.h"
+#include "op_utility/UtilityH.h"
+#include "op_planner/PlanningHelpers.h"
+#include "op_planner/MappingHelpers.h"
+#include "op_planner/MatrixOperations.h"
+
+
+namespace PlannerHNS
+{
+
+DecisionMaker::DecisionMaker()
+{
+  m_iCurrentTotalPathId = 0;
+  pLane = 0;
+  m_pCurrentBehaviorState = 0;
+  m_pGoToGoalState = 0;
+  m_pStopState= 0;
+  m_pWaitState= 0;
+  m_pMissionCompleteState= 0;
+  m_pAvoidObstacleState = 0;
+  m_pTrafficLightStopState = 0;
+  m_pTrafficLightWaitState = 0;
+  m_pStopSignStopState = 0;
+  m_pStopSignWaitState = 0;
+  m_pFollowState = 0;
+  m_MaxLaneSearchDistance = 3.0;
+  m_pStopState = 0;
+  m_pMissionCompleteState = 0;
+  m_pGoalState = 0;
+  m_pGoToGoalState = 0;
+  m_pWaitState = 0;
+  m_pInitState = 0;
+  m_pFollowState = 0;
+  m_pAvoidObstacleState = 0;
+}
+
+DecisionMaker::~DecisionMaker()
+{
+  delete m_pStopState;
+  delete m_pMissionCompleteState;
+  delete m_pGoalState;
+  delete m_pGoToGoalState;
+  delete m_pWaitState;
+  delete m_pInitState;
+  delete m_pFollowState;
+  delete m_pAvoidObstacleState;
+  delete m_pTrafficLightStopState;
+  delete m_pTrafficLightWaitState;
+  delete m_pStopSignWaitState;
+  delete m_pStopSignStopState;
+}
+
+void DecisionMaker::Init(const ControllerParams& ctrlParams, const PlannerHNS::PlanningParams& params,const CAR_BASIC_INFO& carInfo)
+   {
+     m_CarInfo = carInfo;
+     m_ControlParams = ctrlParams;
+     m_params = params;
+
+     m_pidVelocity.Init(0.01, 0.004, 0.01);
+    m_pidVelocity.Setlimit(m_params.maxSpeed, 0);
+
+    m_pidStopping.Init(0.05, 0.05, 0.1);
+    m_pidStopping.Setlimit(m_params.horizonDistance, 0);
+
+    m_pidFollowing.Init(0.05, 0.05, 0.01);
+    m_pidFollowing.Setlimit(m_params.minFollowingDistance, 0);
+
+    InitBehaviorStates();
+
+    if(m_pCurrentBehaviorState)
+      m_pCurrentBehaviorState->SetBehaviorsParams(&m_params);
+   }
+
+void DecisionMaker::InitBehaviorStates()
+{
+
+  m_pStopState         = new StopState(&m_params, 0, 0);
+  m_pMissionCompleteState   = new MissionAccomplishedStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), 0);
+  m_pGoalState        = new GoalStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pMissionCompleteState);
+  m_pGoToGoalState       = new ForwardStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoalState);
+  m_pInitState         = new InitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+
+  m_pFollowState        = new FollowStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pAvoidObstacleState    = new SwerveStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pStopSignWaitState    = new StopSignWaitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pStopSignStopState    = new StopSignStopStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pStopSignWaitState);
+
+  m_pTrafficLightWaitState  = new TrafficLightWaitStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pTrafficLightStopState  = new TrafficLightStopStateII(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+
+  m_pGoToGoalState->InsertNextState(m_pAvoidObstacleState);
+  m_pGoToGoalState->InsertNextState(m_pStopSignStopState);
+  m_pGoToGoalState->InsertNextState(m_pTrafficLightStopState);
+  m_pGoToGoalState->InsertNextState(m_pFollowState);
+  m_pGoToGoalState->decisionMakingCount = 0;//m_params.nReliableCount;
+
+  m_pGoalState->InsertNextState(m_pGoToGoalState);
+
+  m_pStopSignWaitState->decisionMakingTime = m_params.stopSignStopTime;
+  m_pStopSignWaitState->InsertNextState(m_pStopSignStopState);
+  m_pStopSignWaitState->InsertNextState(m_pGoalState);
+
+
+  m_pTrafficLightStopState->InsertNextState(m_pTrafficLightWaitState);
+
+  m_pTrafficLightWaitState->InsertNextState(m_pTrafficLightStopState);
+  m_pTrafficLightWaitState->InsertNextState(m_pGoalState);
+
+  m_pFollowState->InsertNextState(m_pAvoidObstacleState);
+  m_pFollowState->InsertNextState(m_pStopSignStopState);
+  m_pFollowState->InsertNextState(m_pTrafficLightStopState);
+  m_pFollowState->InsertNextState(m_pGoalState);
+  m_pFollowState->decisionMakingCount = 0;//m_params.nReliableCount;
+
+  m_pInitState->decisionMakingCount = 0;//m_params.nReliableCount;
+
+  m_pCurrentBehaviorState = m_pInitState;
+}
+
+ bool DecisionMaker::GetNextTrafficLight(const int& prevTrafficLightId, const std::vector<PlannerHNS::TrafficLight>& trafficLights, PlannerHNS::TrafficLight& trafficL)
+ {
+   for(unsigned int i = 0; i < trafficLights.size(); i++)
+   {
+     double d = hypot(trafficLights.at(i).pos.y - state.pos.y, trafficLights.at(i).pos.x - state.pos.x);
+     if(d <= trafficLights.at(i).stoppingDistance)
+     {
+       double a_diff = UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(UtilityHNS::UtilityH::FixNegativeAngle(trafficLights.at(i).pos.a) , UtilityHNS::UtilityH::FixNegativeAngle(state.pos.a));
+
+       if(a_diff < M_PI_2 && trafficLights.at(i).id != prevTrafficLightId)
+       {
+         //std::cout << "Detected Light, ID = " << trafficLights.at(i).id << ", Distance = " << d << ", Angle = " << trafficLights.at(i).pos.a*RAD2DEG << ", Car Heading = " << state.pos.a*RAD2DEG << ", Diff = " << a_diff*RAD2DEG << std::endl;
+         trafficL = trafficLights.at(i);
+         return true;
+       }
+     }
+   }
+
+   return false;
+ }
+
+ void DecisionMaker::CalculateImportantParameterForDecisionMaking(const PlannerHNS::VehicleState& car_state,
+     const int& goalID, const bool& bEmergencyStop, const std::vector<TrafficLight>& detectedLights,
+     const TrajectoryCost& bestTrajectory)
+ {
+   if(m_TotalPath.size() == 0) return;
+
+   PreCalculatedConditions* pValues = m_pCurrentBehaviorState->GetCalcParams();
+
+   if(m_CarInfo.max_deceleration != 0)
+     pValues->minStoppingDistance = -pow(car_state.speed, 2)/(m_CarInfo.max_deceleration);
+
+   pValues->iCentralTrajectory    = m_pCurrentBehaviorState->m_pParams->rollOutNumber/2;
+
+  if(pValues->iPrevSafeTrajectory < 0)
+    pValues->iPrevSafeTrajectory = pValues->iCentralTrajectory;
+
+   pValues->stoppingDistances.clear();
+   pValues->currentVelocity     = car_state.speed;
+   pValues->bTrafficIsRed       = false;
+   pValues->currentTrafficLightID   = -1;
+   pValues->bFullyBlock       = false;
+
+   pValues->distanceToNext = bestTrajectory.closest_obj_distance;
+   pValues->velocityOfNext = bestTrajectory.closest_obj_velocity;
+
+   if(bestTrajectory.index >=0 &&  bestTrajectory.index < (int)m_RollOuts.size())
+     pValues->iCurrSafeTrajectory = bestTrajectory.index;
+   else
+     pValues->iCurrSafeTrajectory = pValues->iCentralTrajectory;
+
+  pValues->bFullyBlock = bestTrajectory.bBlocked;
+
+   if(bestTrajectory.lane_index >=0)
+     pValues->iCurrSafeLane = bestTrajectory.lane_index;
+   else
+   {
+     PlannerHNS::RelativeInfo info;
+     PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_TotalPath, state, m_params.rollOutDensity*m_params.rollOutNumber/2.0 + 0.1, info);
+     pValues->iCurrSafeLane = info.iGlobalPath;
+   }
+
+   double critical_long_front_distance =  m_CarInfo.wheel_base/2.0 + m_CarInfo.length/2.0 + m_params.verticalSafetyDistance;
+
+  if(ReachEndOfGlobalPath(pValues->minStoppingDistance + critical_long_front_distance, pValues->iCurrSafeLane))
+    pValues->currentGoalID = -1;
+  else
+    pValues->currentGoalID = goalID;
+
+   m_iCurrentTotalPathId = pValues->iCurrSafeLane;
+
+   int stopLineID = -1;
+   int stopSignID = -1;
+   int trafficLightID = -1;
+   double distanceToClosestStopLine = 0;
+   bool bGreenTrafficLight = true;
+
+
+    distanceToClosestStopLine = PlanningHelpers::GetDistanceToClosestStopLineAndCheck(m_TotalPath.at(pValues->iCurrSafeLane), state, m_params.giveUpDistance, stopLineID, stopSignID, trafficLightID) - critical_long_front_distance;
+
+    //std::cout << "StopLineID" << stopLineID << ", StopSignID: " << stopSignID << ", TrafficLightID: " << trafficLightID << ", Distance: " << distanceToClosestStopLine << ", MinStopDistance: " << pValues->minStoppingDistance << std::endl;
+
+   if(distanceToClosestStopLine > m_params.giveUpDistance && distanceToClosestStopLine < (pValues->minStoppingDistance + 1.0))
+   {
+     if(m_pCurrentBehaviorState->m_pParams->enableTrafficLightBehavior)
+     {
+       pValues->currentTrafficLightID = trafficLightID;
+       //std::cout << "Detected Traffic Light: " << trafficLightID << std::endl;
+       for(unsigned int i=0; i< detectedLights.size(); i++)
+       {
+         if(detectedLights.at(i).id == trafficLightID)
+           bGreenTrafficLight = (detectedLights.at(i).lightState == GREEN_LIGHT);
+       }
+     }
+
+     if(m_pCurrentBehaviorState->m_pParams->enableStopSignBehavior)
+       pValues->currentStopSignID = stopSignID;
+
+    pValues->stoppingDistances.push_back(distanceToClosestStopLine);
+    //std::cout << "LP => D: " << pValues->distanceToStop() << ", PrevSignID: " << pValues->prevTrafficLightID << ", CurrSignID: " << pValues->currentTrafficLightID << ", Green: " << bGreenTrafficLight << endl;
+   }
+
+
+   //std::cout << "Distance To Closest: " << distanceToClosestStopLine << ", Stop LineID: " << stopLineID << ", Stop SignID: " << stopSignID << ", TFID: " << trafficLightID << std::endl;
+
+   pValues->bTrafficIsRed = !bGreenTrafficLight;
+
+   if(bEmergencyStop)
+  {
+    pValues->bFullyBlock = true;
+    pValues->distanceToNext = 1;
+    pValues->velocityOfNext = 0;
+  }
+   //cout << "Distances: " << pValues->stoppingDistances.size() << ", Distance To Stop : " << pValues->distanceToStop << endl;
+ }
+
+ void DecisionMaker::UpdateCurrentLane(const double& search_distance)
+ {
+   PlannerHNS::Lane* pMapLane = 0;
+  PlannerHNS::Lane* pPathLane = 0;
+  pPathLane = MappingHelpers::GetLaneFromPath(state, m_TotalPath.at(m_iCurrentTotalPathId));
+  if(!pPathLane)
+  {
+    std::cout << "Performance Alert: Can't Find Lane Information in Global Path, Searching the Map :( " << std::endl;
+    pMapLane  = MappingHelpers::GetClosestLaneFromMap(state, m_Map, search_distance);
+  }
+
+  if(pPathLane)
+    pLane = pPathLane;
+  else if(pMapLane)
+    pLane = pMapLane;
+  else
+    pLane = 0;
+ }
+
+ bool DecisionMaker::ReachEndOfGlobalPath(const double& min_distance, const int& iGlobalPathIndex)
+ {
+   if(m_TotalPath.size()==0) return false;
+
+   PlannerHNS::RelativeInfo info;
+   PlanningHelpers::GetRelativeInfo(m_TotalPath.at(iGlobalPathIndex), state, info);
+
+   double d = 0;
+   for(unsigned int i = info.iFront; i < m_TotalPath.at(iGlobalPathIndex).size()-1; i++)
+   {
+     d+= hypot(m_TotalPath.at(iGlobalPathIndex).at(i+1).pos.y - m_TotalPath.at(iGlobalPathIndex).at(i).pos.y, m_TotalPath.at(iGlobalPathIndex).at(i+1).pos.x - m_TotalPath.at(iGlobalPathIndex).at(i).pos.x);
+     if(d > min_distance)
+       return false;
+   }
+
+   return true;
+ }
+
+ void DecisionMaker::SetNewGlobalPath(const std::vector<std::vector<WayPoint> >& globalPath)
+ {
+   if(m_pCurrentBehaviorState)
+   {
+     m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath = true;
+     m_TotalOriginalPath = globalPath;
+   }
+ }
+
+ bool DecisionMaker::SelectSafeTrajectory()
+ {
+   bool bNewTrajectory = false;
+   PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
+
+   if(!preCalcPrams || m_RollOuts.size() == 0) return bNewTrajectory;
+
+  int currIndex = PlannerHNS::PlanningHelpers::GetClosestNextPointIndexFast(m_Path, state);
+  int index_limit = 0;
+  if(index_limit<=0)
+    index_limit =  m_Path.size()/2.0;
+  if(currIndex > index_limit
+      || preCalcPrams->bRePlan
+      || preCalcPrams->bNewGlobalPath)
+  {
+    std::cout << "New Local Plan !! " << currIndex << ", "<< preCalcPrams->bRePlan << ", " << preCalcPrams->bNewGlobalPath  << ", " <<  m_TotalOriginalPath.at(0).size() << ", PrevLocal: " << m_Path.size();
+    m_Path = m_RollOuts.at(preCalcPrams->iCurrSafeTrajectory);
+    std::cout << ", NewLocal: " << m_Path.size() << std::endl;
+
+    preCalcPrams->bNewGlobalPath = false;
+    preCalcPrams->bRePlan = false;
+    bNewTrajectory = true;
+  }
+
+  return bNewTrajectory;
+ }
+
+ PlannerHNS::BehaviorState DecisionMaker::GenerateBehaviorState(const PlannerHNS::VehicleState& vehicleState)
+ {
+  PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
+
+  m_pCurrentBehaviorState = m_pCurrentBehaviorState->GetNextState();
+  if(m_pCurrentBehaviorState==0)
+    m_pCurrentBehaviorState = m_pInitState;
+
+  PlannerHNS::BehaviorState currentBehavior;
+
+  currentBehavior.state = m_pCurrentBehaviorState->m_Behavior;
+  currentBehavior.followDistance = preCalcPrams->distanceToNext;
+
+  currentBehavior.minVelocity    = 0;
+  currentBehavior.stopDistance   = preCalcPrams->distanceToStop();
+  currentBehavior.followVelocity   = preCalcPrams->velocityOfNext;
+  if(preCalcPrams->iPrevSafeTrajectory<0 || preCalcPrams->iPrevSafeTrajectory >= m_RollOuts.size())
+    currentBehavior.iTrajectory    = preCalcPrams->iCurrSafeTrajectory;
+  else
+    currentBehavior.iTrajectory    = preCalcPrams->iPrevSafeTrajectory;
+
+  double average_braking_distance = -pow(vehicleState.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
+
+  if(average_braking_distance  < m_params.minIndicationDistance)
+    average_braking_distance = m_params.minIndicationDistance;
+
+  currentBehavior.indicator = PlanningHelpers::GetIndicatorsFromPath(m_Path, state, average_braking_distance );
+
+  return currentBehavior;
+ }
+
+ double DecisionMaker::UpdateVelocityDirectlyToTrajectory(const BehaviorState& beh, const VehicleState& CurrStatus, const double& dt)
+ {
+  if(m_TotalOriginalPath.size() ==0 ) return 0;
+
+  RelativeInfo info, total_info;
+  PlanningHelpers::GetRelativeInfo(m_TotalOriginalPath.at(m_iCurrentTotalPathId), state, total_info);
+  PlanningHelpers::GetRelativeInfo(m_Path, state, info);
+  double average_braking_distance = -pow(CurrStatus.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
+  double max_velocity  = PlannerHNS::PlanningHelpers::GetVelocityAhead(m_TotalOriginalPath.at(m_iCurrentTotalPathId), total_info, total_info.iBack, average_braking_distance);
+
+  unsigned int point_index = 0;
+  double critical_long_front_distance = m_CarInfo.length/2.0;
+
+  if(beh.state == TRAFFIC_LIGHT_STOP_STATE || beh.state == STOP_SIGN_STOP_STATE)
+  {
+    PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.stopDistance - critical_long_front_distance, point_index);
+
+    double e = -beh.stopDistance;
+    double desiredVelocity = m_pidStopping.getPID(e);
+
+//    std::cout << "Stopping : e=" << e << ", desiredPID=" << desiredVelocity << ", PID: " << m_pidStopping.ToString() << std::endl;
+
+    if(desiredVelocity > max_velocity)
+      desiredVelocity = max_velocity;
+    else if(desiredVelocity < m_params.minSpeed)
+      desiredVelocity = 0;
+
+    for(unsigned int i =  0; i < m_Path.size(); i++)
+      m_Path.at(i).v = desiredVelocity;
+
+    return desiredVelocity;
+  }
+  else if(beh.state == FOLLOW_STATE)
+  {
+
+    double deceleration_critical = 0;
+    double inv_time = 2.0*((beh.followDistance- (critical_long_front_distance+m_params.additionalBrakingDistance))-CurrStatus.speed);
+    if(inv_time <= 0)
+      deceleration_critical = m_CarInfo.max_deceleration;
+    else
+      deceleration_critical = CurrStatus.speed*CurrStatus.speed/inv_time;
+
+    if(deceleration_critical > 0) deceleration_critical = -deceleration_critical;
+    if(deceleration_critical < - m_CarInfo.max_acceleration) deceleration_critical = - m_CarInfo.max_acceleration;
+
+    double desiredVelocity = (deceleration_critical * dt) + CurrStatus.speed;
+
+    if(desiredVelocity > m_params.maxSpeed)
+      desiredVelocity = m_params.maxSpeed;
+
+    if((desiredVelocity < 0.1 && desiredVelocity > -0.1) || beh.followDistance <= 0) //use only effective velocities
+      desiredVelocity = 0;
+
+    //std::cout << "Acc: V: " << desiredVelocity << ", Accel: " << deceleration_critical<< std::endl;
+
+    for(unsigned int i = 0; i < m_Path.size(); i++)
+      m_Path.at(i).v = desiredVelocity;
+
+    return desiredVelocity;
+
+  }
+  else if(beh.state == FORWARD_STATE || beh.state == OBSTACLE_AVOIDANCE_STATE )
+  {
+    double target_velocity = max_velocity;
+    bool bSlowBecauseChange=false;
+    if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+    {
+      target_velocity*=0.5;
+      bSlowBecauseChange = true;
+    }
+
+    double e = target_velocity - CurrStatus.speed;
+    double desiredVelocity = m_pidVelocity.getPID(e);
+
+    if(desiredVelocity>max_velocity)
+      desiredVelocity = max_velocity;
+    else if(desiredVelocity < m_params.minSpeed)
+      desiredVelocity = 0;
+
+    for(unsigned int i = 0; i < m_Path.size(); i++)
+      m_Path.at(i).v = desiredVelocity;
+
+    //std::cout << "Target Velocity: " << desiredVelocity << ", Change Slowdown: " << bSlowBecauseChange  << std::endl;
+
+    return desiredVelocity;
+  }
+  else if(beh.state == STOP_SIGN_WAIT_STATE || beh.state == TRAFFIC_LIGHT_WAIT_STATE)
+  {
+    double target_velocity = 0;
+    for(unsigned int i = 0; i < m_Path.size(); i++)
+      m_Path.at(i).v = target_velocity;
+
+    return target_velocity;
+  }
+  else
+  {
+    double target_velocity = 0;
+    for(unsigned int i = 0; i < m_Path.size(); i++)
+      m_Path.at(i).v = target_velocity;
+
+    return target_velocity;
+  }
+
+  return max_velocity;
+ }
+
+ PlannerHNS::BehaviorState DecisionMaker::DoOneStep(
+    const double& dt,
+    const PlannerHNS::WayPoint currPose,
+    const PlannerHNS::VehicleState& vehicleState,
+    const int& goalID,
+    const std::vector<TrafficLight>& trafficLight,
+    const TrajectoryCost& tc,
+    const bool& bEmergencyStop)
+{
+   PlannerHNS::BehaviorState beh;
+   state = currPose;
+   m_TotalPath.clear();
+  for(unsigned int i = 0; i < m_TotalOriginalPath.size(); i++)
+  {
+    t_centerTrajectorySmoothed.clear();
+    PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_TotalOriginalPath.at(i), state, m_params.horizonDistance ,  m_params.pathDensity , t_centerTrajectorySmoothed);
+    m_TotalPath.push_back(t_centerTrajectorySmoothed);
+  }
+
+  if(m_TotalPath.size()==0) return beh;
+
+  UpdateCurrentLane(m_MaxLaneSearchDistance);
+
+  CalculateImportantParameterForDecisionMaking(vehicleState, goalID, bEmergencyStop, trafficLight, tc);
+
+  beh = GenerateBehaviorState(vehicleState);
+
+  beh.bNewPlan = SelectSafeTrajectory();
+
+  beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);
+
+  //std::cout << "Eval_i: " << tc.index << ", Curr_i: " <<  m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << ", Prev_i: " << m_pCurrentBehaviorState->GetCalcParams()->iPrevSafeTrajectory << std::endl;
+
+  return beh;
+ }
+
+} /* namespace PlannerHNS */

src1/planning/op_planner/LocalPlannerH.cpp

@@ -0,0 +1,797 @@
+/// \file LocalPlannerH.cpp
+/// \brief OpenPlanner's local planing functions combines in one process, used in simulation vehicle and OpenPlanner old implementation like dp_planner node.
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+#include "op_planner/LocalPlannerH.h"
+#include "op_utility/UtilityH.h"
+#include "op_planner/PlanningHelpers.h"
+#include "op_planner/MappingHelpers.h"
+#include "op_planner/MatrixOperations.h"
+#include "op_planner/PlannerH.h"
+
+using namespace UtilityHNS;
+
+namespace PlannerHNS
+{
+
+LocalPlannerH::LocalPlannerH()
+{
+
+  m_PrevBrakingWayPoint = 0;
+  m_iSafeTrajectory = 0;
+  m_iCurrentTotalPathId = 0;
+  pLane = 0;
+  m_CurrentVelocity =  m_CurrentVelocityD =0;
+  m_CurrentSteering = m_CurrentSteeringD =0;
+  m_CurrentShift     =  m_CurrentShiftD = SHIFT_POS_NN;
+  m_CurrentAccSteerAngle = m_CurrentAccVelocity = 0;
+  m_pCurrentBehaviorState = 0;
+  m_pGoToGoalState = 0;
+  m_pStopState= 0;
+  m_pWaitState= 0;
+  m_pMissionCompleteState= 0;
+  m_pAvoidObstacleState = 0;
+  m_pTrafficLightStopState = 0;
+  m_pTrafficLightWaitState = 0;
+  m_pStopSignStopState = 0;
+  m_pStopSignWaitState = 0;
+  m_pFollowState = 0;
+  m_SimulationSteeringDelayFactor = 0.1;
+  UtilityH::GetTickCount(m_SteerDelayTimer);
+  m_PredictionTime = 0;
+
+  InitBehaviorStates();
+}
+
+LocalPlannerH::~LocalPlannerH()
+{
+  delete m_pStopState;
+  delete m_pMissionCompleteState ;
+  delete m_pGoalState      ;
+  delete m_pGoToGoalState     ;
+  delete m_pWaitState       ;
+  delete m_pInitState       ;
+  delete m_pFollowState      ;
+  delete m_pAvoidObstacleState  ;
+  delete m_pTrafficLightStopState;
+  delete m_pTrafficLightWaitState;
+  delete m_pStopSignWaitState  ;
+  delete m_pStopSignStopState;
+}
+
+void LocalPlannerH::Init(const ControllerParams& ctrlParams, const PlannerHNS::PlanningParams& params,const CAR_BASIC_INFO& carInfo)
+   {
+
+     m_CarInfo = carInfo;
+     m_ControlParams = ctrlParams;
+     m_CurrentVelocity =  m_CurrentVelocityD =0;
+     m_CurrentSteering = m_CurrentSteeringD =0;
+     m_CurrentShift     =  m_CurrentShiftD = SHIFT_POS_NN;
+     m_CurrentAccSteerAngle = m_CurrentAccVelocity = 0;
+     m_params = params;
+     m_InitialFollowingDistance = m_params.minFollowingDistance;
+
+     m_pidVelocity.Init(0.005, 0.005, 0.05);
+    m_pidVelocity.Setlimit(m_params.maxSpeed, 0);
+
+    m_pidStopping.Init(0.1, 0.05, 0.1);
+    m_pidStopping.Setlimit(m_params.horizonDistance, 0);
+
+    if(m_pCurrentBehaviorState)
+      m_pCurrentBehaviorState->SetBehaviorsParams(&m_params);
+
+   }
+
+void LocalPlannerH::InitBehaviorStates()
+{
+
+  m_pStopState         = new StopState(&m_params, 0, 0);
+  m_pMissionCompleteState   = new MissionAccomplishedState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), 0);
+  m_pGoalState        = new GoalState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pMissionCompleteState);
+  m_pGoToGoalState       = new ForwardState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoalState);
+  m_pWaitState         = new WaitState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pInitState         = new InitState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pFollowState        = new FollowState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pAvoidObstacleState    = new SwerveState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pTrafficLightStopState  = new TrafficLightStopState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pTrafficLightWaitState  = new TrafficLightWaitState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pStopSignWaitState    = new StopSignWaitState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pGoToGoalState);
+  m_pStopSignStopState    = new StopSignStopState(m_pStopState->m_pParams, m_pStopState->GetCalcParams(), m_pStopSignWaitState);
+
+  m_pGoToGoalState->InsertNextState(m_pStopState);
+  m_pGoToGoalState->InsertNextState(m_pWaitState);
+  m_pGoToGoalState->InsertNextState(m_pFollowState);
+  m_pGoToGoalState->InsertNextState(m_pAvoidObstacleState);
+  m_pGoToGoalState->InsertNextState(m_pTrafficLightStopState);
+  m_pGoToGoalState->InsertNextState(m_pStopSignStopState);
+
+  m_pStopState->InsertNextState(m_pGoToGoalState);
+
+  m_pTrafficLightStopState->InsertNextState(m_pTrafficLightWaitState);
+  m_pTrafficLightWaitState->InsertNextState(m_pTrafficLightStopState);
+
+  m_pStopSignWaitState->decisionMakingTime = 5.0;
+  m_pStopSignWaitState->InsertNextState(m_pStopSignStopState);
+
+  m_pFollowState->InsertNextState(m_pStopState);
+  m_pFollowState->InsertNextState(m_pTrafficLightStopState);
+  m_pFollowState->InsertNextState(m_pStopSignStopState);
+
+  m_pAvoidObstacleState->decisionMakingTime = 0.1;
+
+  m_pCurrentBehaviorState = m_pInitState;
+}
+
+void LocalPlannerH::InitPolygons()
+{
+  double l2 = m_CarInfo.length/2.0;
+  double w2 = m_CarInfo.width/2.0;
+
+  m_CarShapePolygon.push_back(GPSPoint(-w2, -l2, 0,0));
+  m_CarShapePolygon.push_back(GPSPoint(w2, -l2, 0,0));
+  m_CarShapePolygon.push_back(GPSPoint(w2, l2, 0,0));
+  m_CarShapePolygon.push_back(GPSPoint(-w2, l2, 0,0));
+}
+
+void LocalPlannerH::ReInitializePlanner(const WayPoint& start_pose)
+{
+
+  m_pidVelocity.Init(0.005, 0.005, 0.05);
+  m_pidVelocity.Setlimit(m_params.maxSpeed, 0);
+
+  m_pidStopping.Init(0.1, 0.05, 0.1);
+  m_pidStopping.Setlimit(m_params.horizonDistance, 0);
+
+  m_PrevBrakingWayPoint = 0;
+  m_iSafeTrajectory = 0;
+  m_iCurrentTotalPathId = 0;
+  m_CurrentVelocity =  m_CurrentVelocityD =0;
+  m_CurrentSteering = m_CurrentSteeringD =0;
+  m_CurrentShift     =  m_CurrentShiftD = SHIFT_POS_NN;
+  m_CurrentAccSteerAngle = m_CurrentAccVelocity = 0;
+
+  m_pCurrentBehaviorState = m_pFollowState;
+  m_TotalPath.clear();
+  m_OriginalLocalPath.clear();
+  m_TotalOriginalPath.clear();
+  m_Path.clear();
+  m_RollOuts.clear();
+  m_pCurrentBehaviorState->m_Behavior = PlannerHNS::FORWARD_STATE;
+  m_pCurrentBehaviorState->GetCalcParams()->bOutsideControl = 1;
+  FirstLocalizeMe(start_pose);
+  LocalizeMe(0);
+}
+
+ void LocalPlannerH::FirstLocalizeMe(const WayPoint& initCarPos)
+ {
+  pLane = initCarPos.pLane;
+  state = initCarPos;
+  m_OdometryState.pos.a = initCarPos.pos.a;
+  m_OdometryState.pos.x = initCarPos.pos.x + (m_CarInfo.wheel_base/2.0 * cos(initCarPos.pos.a));
+  m_OdometryState.pos.y = initCarPos.pos.y + (m_CarInfo.wheel_base/2.0 * sin(initCarPos.pos.a));
+ }
+
+ void LocalPlannerH::LocalizeMe(const double& dt)
+{
+  //calculate the new x, y ,
+   WayPoint currPose = state;
+
+  if(m_CurrentShift == SHIFT_POS_DD)
+  {
+    m_OdometryState.pos.x   +=  m_CurrentVelocity * dt * cos(currPose.pos.a);
+    m_OdometryState.pos.y   +=  m_CurrentVelocity * dt * sin(currPose.pos.a);
+    m_OdometryState.pos.a   +=  m_CurrentVelocity * dt * tan(m_CurrentSteering)  / m_CarInfo.wheel_base;
+
+  }
+  else if(m_CurrentShift == SHIFT_POS_RR )
+  {
+    m_OdometryState.pos.x   +=  -m_CurrentVelocity * dt * cos(currPose.pos.a);
+    m_OdometryState.pos.y   +=  -m_CurrentVelocity * dt * sin(currPose.pos.a);
+    m_OdometryState.pos.a   +=  -m_CurrentVelocity * dt * tan(m_CurrentSteering);
+  }
+
+  m_OdometryState.pos.a = atan2(sin(m_OdometryState.pos.a), cos(m_OdometryState.pos.a));
+  m_OdometryState.pos.a = UtilityH::FixNegativeAngle(m_OdometryState.pos.a);
+
+  state.pos.a = m_OdometryState.pos.a;
+  state.pos.x = m_OdometryState.pos.x   - (m_CurrentVelocity*dt* (m_CarInfo.wheel_base) * cos (m_OdometryState.pos.a));
+  state.pos.y = m_OdometryState.pos.y   - (m_CurrentVelocity*dt* (m_CarInfo.wheel_base/2.0) * sin (m_OdometryState.pos.a));
+}
+
+ void LocalPlannerH::UpdateState(const PlannerHNS::VehicleState& state, const bool& bUseDelay)
+  {
+   if(!bUseDelay)
+   {
+     m_CurrentSteering   = m_CurrentSteeringD;
+    // std::cout << " No Delay " << std::endl;
+   }
+   else
+   {
+     double currSteerDeg = RAD2DEG * m_CurrentSteering;
+     double desiredSteerDeg = RAD2DEG * m_CurrentSteeringD;
+
+     double mFact = UtilityH::GetMomentumScaleFactor(state.speed);
+     double diff = desiredSteerDeg - currSteerDeg;
+     double diffSign = UtilityH::GetSign(diff);
+     double inc = 1.0*diffSign;
+     if(fabs(diff) < 1.0 )
+       inc = diff;
+
+//     std::cout << "Delay: " << m_SimulationSteeringDelayFactor
+//         << ", Fact: " << mFact
+//         << ", Diff: " << diff
+//         << ", inc: " << inc << std::endl;
+     if(UtilityH::GetTimeDiffNow(m_SteerDelayTimer) > m_SimulationSteeringDelayFactor*mFact)
+     {
+       UtilityH::GetTickCount(m_SteerDelayTimer);
+       currSteerDeg += inc;
+     }
+
+     m_CurrentSteering = DEG2RAD * currSteerDeg;
+   }
+
+   m_CurrentShift   = m_CurrentShiftD;
+   m_CurrentVelocity = m_CurrentVelocityD;
+  }
+
+ void LocalPlannerH::AddAndTransformContourPoints(const PlannerHNS::DetectedObject& obj, std::vector<PlannerHNS::WayPoint>& contourPoints)
+ {
+   contourPoints.clear();
+   WayPoint  p, p_center = obj.center;
+   p_center.pos.a += M_PI_2;
+   for(unsigned int i=0; i< obj.contour.size(); i++)
+   {
+     p.pos = obj.contour.at(i);
+     //TransformPoint(p_center, p.pos);
+     contourPoints.push_back(p);
+   }
+
+   contourPoints.push_back(obj.center);
+ }
+
+ void LocalPlannerH::TransformPoint(const PlannerHNS::WayPoint& refPose, PlannerHNS::GPSPoint& p)
+ {
+   PlannerHNS::Mat3 rotationMat(refPose.pos.a);
+   PlannerHNS::Mat3 translationMat(refPose.pos.x, refPose.pos.y);
+  p = rotationMat*p;
+  p = translationMat*p;
+ }
+
+ bool LocalPlannerH::GetNextTrafficLight(const int& prevTrafficLightId, const std::vector<PlannerHNS::TrafficLight>& trafficLights, PlannerHNS::TrafficLight& trafficL)
+ {
+   for(unsigned int i = 0; i < trafficLights.size(); i++)
+   {
+     double d = hypot(trafficLights.at(i).pos.y - state.pos.y, trafficLights.at(i).pos.x - state.pos.x);
+     if(d <= trafficLights.at(i).stoppingDistance)
+     {
+       double a_diff = UtilityH::AngleBetweenTwoAnglesPositive(UtilityH::FixNegativeAngle(trafficLights.at(i).pos.a) , UtilityH::FixNegativeAngle(state.pos.a));
+
+       if(a_diff < M_PI_2 && trafficLights.at(i).id != prevTrafficLightId)
+       {
+         //std::cout << "Detected Light, ID = " << trafficLights.at(i).id << ", Distance = " << d << ", Angle = " << trafficLights.at(i).pos.a*RAD2DEG << ", Car Heading = " << state.pos.a*RAD2DEG << ", Diff = " << a_diff*RAD2DEG << std::endl;
+         trafficL = trafficLights.at(i);
+         return true;
+       }
+     }
+   }
+
+   return false;
+ }
+
+ void LocalPlannerH::CalculateImportantParameterForDecisionMaking(const PlannerHNS::VehicleState& car_state,
+     const int& goalID, const bool& bEmergencyStop, const vector<TrafficLight>& detectedLights,
+     const TrajectoryCost& bestTrajectory)
+ {
+   PreCalculatedConditions* pValues = m_pCurrentBehaviorState->GetCalcParams();
+
+   double critical_long_front_distance =  m_CarInfo.wheel_base/2.0 + m_CarInfo.length/2.0 + m_params.verticalSafetyDistance;
+
+   pValues->minStoppingDistance = -pow(car_state.speed, 2)/(m_CarInfo.max_deceleration);
+
+   pValues->iCentralTrajectory    = m_pCurrentBehaviorState->m_pParams->rollOutNumber/2;
+
+   if(pValues->iCurrSafeTrajectory < 0)
+       pValues->iCurrSafeTrajectory = pValues->iCentralTrajectory;
+
+  if(pValues->iPrevSafeTrajectory < 0)
+    pValues->iPrevSafeTrajectory = pValues->iCentralTrajectory;
+
+   pValues->stoppingDistances.clear();
+   pValues->currentVelocity     = car_state.speed;
+   pValues->bTrafficIsRed       = false;
+   pValues->currentTrafficLightID   = -1;
+   pValues->bRePlan         = false;
+   pValues->bFullyBlock       = false;
+
+
+   pValues->distanceToNext = bestTrajectory.closest_obj_distance;
+   pValues->velocityOfNext = bestTrajectory.closest_obj_velocity;
+
+   if(pValues->distanceToNext > m_params.minDistanceToAvoid)
+     pValues->iCurrSafeTrajectory = pValues->iCentralTrajectory;
+   else if(bestTrajectory.index>=0)
+     pValues->iCurrSafeTrajectory = bestTrajectory.index;
+
+  pValues->bFullyBlock = bestTrajectory.bBlocked;
+
+   if(bestTrajectory.lane_index >=0)
+     pValues->iCurrSafeLane = bestTrajectory.lane_index;
+   else
+   {
+     PlannerHNS::RelativeInfo info;
+     PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_TotalPath, state, m_params.rollOutDensity*m_params.rollOutNumber/2.0 + 0.1, info);
+     pValues->iCurrSafeLane = info.iGlobalPath;
+   }
+
+
+  if(NoWayTest(pValues->minStoppingDistance, pValues->iCurrSafeLane))
+    pValues->currentGoalID = -1;
+  else
+    pValues->currentGoalID = goalID;
+
+   m_iSafeTrajectory = pValues->iCurrSafeTrajectory;
+   m_iCurrentTotalPathId = pValues->iCurrSafeLane;
+
+   int stopLineID = -1;
+   int stopSignID = -1;
+   int trafficLightID = -1;
+   double distanceToClosestStopLine = 0;
+   bool bGreenTrafficLight = true;
+
+   if(m_TotalPath.size()>0)
+     distanceToClosestStopLine = PlanningHelpers::GetDistanceToClosestStopLineAndCheck(m_TotalPath.at(pValues->iCurrSafeLane), state, 0, stopLineID, stopSignID, trafficLightID) - critical_long_front_distance;
+
+   if(distanceToClosestStopLine > 0 && distanceToClosestStopLine < pValues->minStoppingDistance)
+   {
+     if(m_pCurrentBehaviorState->m_pParams->enableTrafficLightBehavior)
+     {
+       pValues->currentTrafficLightID = trafficLightID;
+       //cout << "Detected Traffic Light: " << trafficLightID << endl;
+       for(unsigned int i=0; i< detectedLights.size(); i++)
+       {
+         if(detectedLights.at(i).id == trafficLightID)
+           bGreenTrafficLight = (detectedLights.at(i).lightState == GREEN_LIGHT);
+       }
+     }
+
+     if(m_pCurrentBehaviorState->m_pParams->enableStopSignBehavior)
+       pValues->currentStopSignID = stopSignID;
+
+    pValues->stoppingDistances.push_back(distanceToClosestStopLine);
+    //std::cout << "LP => D: " << pValues->distanceToStop() << ", PrevSignID: " << pValues->prevTrafficLightID << ", CurrSignID: " << pValues->currentTrafficLightID << ", Green: " << bGreenTrafficLight << endl;
+   }
+
+
+//   cout << "Distance To Closest: " << distanceToClosestStopLine << ", Stop LineID: " << stopLineID << ", Stop SignID: " << stopSignID << ", TFID: " << trafficLightID << endl;
+
+   pValues->bTrafficIsRed = !bGreenTrafficLight;
+
+   if(bEmergencyStop)
+  {
+    pValues->bFullyBlock = true;
+    pValues->distanceToNext = 1;
+    pValues->velocityOfNext = 0;
+  }
+   //cout << "Distances: " << pValues->stoppingDistances.size() << ", Distance To Stop : " << pValues->distanceToStop << endl;
+ }
+
+ void LocalPlannerH::UpdateCurrentLane(PlannerHNS::RoadNetwork& map, const double& search_distance)
+ {
+   PlannerHNS::Lane* pMapLane = 0;
+  PlannerHNS::Lane* pPathLane = 0;
+  pPathLane = MappingHelpers::GetLaneFromPath(state, m_Path);
+  if(!pPathLane)
+  {
+    cout << "Performance Alert: Can't Find Lane Information in Global Path, Searching the Map :( " << endl;
+    pMapLane  = MappingHelpers::GetClosestLaneFromMap(state, map, search_distance);
+
+  }
+
+  if(pPathLane)
+    pLane = pPathLane;
+  else if(pMapLane)
+    pLane = pMapLane;
+  else
+    pLane = 0;
+ }
+
+ void LocalPlannerH::SimulateOdoPosition(const double& dt, const PlannerHNS::VehicleState& vehicleState)
+ {
+  SetSimulatedTargetOdometryReadings(vehicleState.speed, vehicleState.steer, vehicleState.shift);
+  UpdateState(vehicleState, false);
+  LocalizeMe(dt);
+ }
+
+ bool LocalPlannerH::NoWayTest(const double& min_distance, const int& iGlobalPathIndex)
+ {
+   if(m_TotalPath.size()==0) return false;
+
+   PlannerHNS::RelativeInfo info;
+   PlanningHelpers::GetRelativeInfo(m_TotalPath.at(iGlobalPathIndex), state, info);
+
+   double d = 0;
+   for(unsigned int i = info.iFront; i < m_TotalPath.at(iGlobalPathIndex).size()-1; i++)
+   {
+     d+= hypot(m_TotalPath.at(iGlobalPathIndex).at(i+1).pos.y - m_TotalPath.at(iGlobalPathIndex).at(i).pos.y, m_TotalPath.at(iGlobalPathIndex).at(i+1).pos.x - m_TotalPath.at(iGlobalPathIndex).at(i).pos.x);
+     if(d > min_distance)
+       return false;
+   }
+
+   return true;
+ }
+
+ bool LocalPlannerH::SelectSafeTrajectoryAndSpeedProfile(const PlannerHNS::VehicleState& vehicleState)
+ {
+   PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
+
+  bool bNewTrajectory = false;
+
+  if(m_TotalPath.size()>0)
+  {
+    int currIndex = PlannerHNS::PlanningHelpers::GetClosestNextPointIndexFast(m_Path, state);
+    int index_limit = 0;//m_Path.size() - 20;
+    if(index_limit<=0)
+      index_limit =  m_Path.size()/2.0;
+    if(m_RollOuts.size() == 0
+        || currIndex > index_limit
+        || m_pCurrentBehaviorState->GetCalcParams()->bRePlan
+        || m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath
+        || m_pCurrentBehaviorState->m_Behavior == OBSTACLE_AVOIDANCE_STATE)
+    {
+      PlannerHNS::PlannerH planner;
+      planner.GenerateRunoffTrajectory(m_TotalPath, state,
+          m_pCurrentBehaviorState->m_pParams->enableLaneChange,
+          vehicleState.speed,
+          m_pCurrentBehaviorState->m_pParams->microPlanDistance,
+          m_pCurrentBehaviorState->m_pParams->maxSpeed,
+          m_pCurrentBehaviorState->m_pParams->minSpeed,
+          m_pCurrentBehaviorState->m_pParams->carTipMargin,
+          m_pCurrentBehaviorState->m_pParams->rollInMargin,
+          m_pCurrentBehaviorState->m_pParams->rollInSpeedFactor,
+          m_pCurrentBehaviorState->m_pParams->pathDensity,
+          m_pCurrentBehaviorState->m_pParams->rollOutDensity,
+          m_pCurrentBehaviorState->m_pParams->rollOutNumber,
+          m_pCurrentBehaviorState->m_pParams->smoothingDataWeight,
+          m_pCurrentBehaviorState->m_pParams->smoothingSmoothWeight,
+          m_pCurrentBehaviorState->m_pParams->smoothingToleranceError,
+          m_pCurrentBehaviorState->m_pParams->speedProfileFactor,
+          m_pCurrentBehaviorState->m_pParams->enableHeadingSmoothing,
+          preCalcPrams->iCurrSafeLane , preCalcPrams->iCurrSafeTrajectory,
+          m_RollOuts, m_SampledPoints);
+
+      m_pCurrentBehaviorState->GetCalcParams()->bRePlan = false;
+      m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath = false;
+
+      if(m_pCurrentBehaviorState->m_Behavior == OBSTACLE_AVOIDANCE_STATE)
+        preCalcPrams->iPrevSafeTrajectory = preCalcPrams->iCurrSafeTrajectory;
+      else
+        preCalcPrams->iPrevSafeTrajectory = preCalcPrams->iCentralTrajectory;
+
+      preCalcPrams->iPrevSafeLane = preCalcPrams->iCurrSafeLane;
+
+      if(preCalcPrams->iPrevSafeLane >= 0
+          && preCalcPrams->iPrevSafeLane < (int)m_RollOuts.size()
+          && preCalcPrams->iPrevSafeTrajectory >= 0
+          && preCalcPrams->iPrevSafeTrajectory < (int)m_RollOuts.at(preCalcPrams->iPrevSafeLane).size())
+      {
+        m_Path = m_RollOuts.at(preCalcPrams->iPrevSafeLane).at(preCalcPrams->iPrevSafeTrajectory);
+        m_OriginalLocalPath = m_TotalPath.at(m_iCurrentTotalPathId);
+        bNewTrajectory = true;
+      }
+    }
+  }
+
+  return bNewTrajectory;
+ }
+
+ PlannerHNS::BehaviorState LocalPlannerH::GenerateBehaviorState(const PlannerHNS::VehicleState& vehicleState)
+ {
+  PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
+
+  m_pCurrentBehaviorState = m_pCurrentBehaviorState->GetNextState();
+  if(m_pCurrentBehaviorState==0)
+    m_pCurrentBehaviorState = m_pInitState;
+
+  PlannerHNS::BehaviorState currentBehavior;
+
+  currentBehavior.state = m_pCurrentBehaviorState->m_Behavior;
+  currentBehavior.followDistance = preCalcPrams->distanceToNext;
+
+
+  currentBehavior.minVelocity    = 0;
+  currentBehavior.stopDistance   = preCalcPrams->distanceToStop();
+  currentBehavior.followVelocity   = preCalcPrams->velocityOfNext;
+
+  double average_braking_distance = -pow(vehicleState.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
+
+  if(average_braking_distance  < 15)
+    average_braking_distance = 15;
+
+  currentBehavior.indicator = PlanningHelpers::GetIndicatorsFromPath(m_Path, state, average_braking_distance );
+
+  return currentBehavior;
+ }
+
+// double LocalPlannerH::UpdateVelocityDirectlyToTrajectory(const BehaviorState& beh, const VehicleState& CurrStatus, const double& dt)
+// {
+//  RelativeInfo info, total_info;
+//  PlanningHelpers::GetRelativeInfo(m_TotalPath.at(m_iCurrentTotalPathId), state, total_info);
+//  PlanningHelpers::GetRelativeInfo(m_Path, state, info);
+//  double average_braking_distance = -pow(CurrStatus.speed, 2)/(m_CarInfo.max_deceleration);
+//  double max_velocity  = PlannerHNS::PlanningHelpers::GetVelocityAhead(m_TotalPath.at(m_iCurrentTotalPathId), total_info, average_braking_distance);
+//
+//  unsigned int point_index = 0;
+//  double critical_long_front_distance = 2.0;
+//
+//  if(m_Path.size() <= 5)
+//  {
+//    double target_velocity = 0;
+//    for(unsigned int i = 0; i < m_Path.size(); i++)
+//      m_Path.at(i).v = target_velocity;
+//  }
+//  else if(beh.state == TRAFFIC_LIGHT_STOP_STATE || beh.state == STOP_SIGN_STOP_STATE || beh.state == STOP_SIGN_WAIT_STATE || beh.state == TRAFFIC_LIGHT_WAIT_STATE)
+//  {
+//    PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.stopDistance - critical_long_front_distance, point_index);
+//
+//    double inc = CurrStatus.speed;
+//    int iRange = point_index - info.iBack;
+//    if(iRange > 0)
+//      inc = inc / (double)iRange;
+//    else
+//      inc = 0;
+//
+//    double target_velocity = CurrStatus.speed - inc;
+//    for(unsigned int i =  info.iBack; i < point_index; i++)
+//    {
+//       if(i < m_Path.size() && i >= 0)
+//         m_Path.at(i).v = target_velocity;
+//       target_velocity -= inc;
+//    }
+//  }
+//  else if(beh.state == FOLLOW_STATE)
+//  {
+//    double targe_acceleration = -pow(CurrStatus.speed, 2)/(2.0*(beh.followDistance - critical_long_front_distance));
+//    if(targe_acceleration <= 0 &&  targe_acceleration > m_CarInfo.max_deceleration/2.0)
+//    {
+//      double target_velocity = (targe_acceleration * dt) + CurrStatus.speed;
+//      for(unsigned int i = 0; i < m_Path.size(); i++)
+//      {
+//        if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory == m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+//          m_Path.at(i).v = target_velocity;
+//        else
+//          m_Path.at(i).v = target_velocity*AVOIDANCE_SPEED_FACTOR;
+//      }
+//
+//      //cout << "Accelerate -> Target V: " << target_velocity << ", Brake D: " <<  average_braking_distance << ", Acceleration: " << targe_acceleration << endl;
+//    }
+//    else
+//    {
+//      WayPoint pursuite_point = PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.followDistance - critical_long_front_distance, point_index);
+//      double inc = CurrStatus.speed;
+//      int iRange = point_index - info.iBack;
+//
+//      if(iRange > 0)
+//        inc = inc / (double)iRange;
+//      else
+//        inc = 0;
+//
+//      double target_velocity = CurrStatus.speed - inc;
+//      for(unsigned int i =  info.iBack; i < point_index; i++)
+//      {
+//         if(i < m_Path.size() && i >= 0)
+//         {
+//           target_velocity = target_velocity < 0 ? 0 : target_velocity;
+//           if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory == m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+//             m_Path.at(i).v = target_velocity;
+//           else
+//             m_Path.at(i).v = target_velocity*AVOIDANCE_SPEED_FACTOR;
+//         }
+//
+//         target_velocity -= inc;
+//      }
+//
+//      //cout << "Decelerate -> Target V: " << target_velocity << ", Brake D: " <<  average_braking_distance << ", Start I" << info.iBack << endl;
+//    }
+//
+//  }
+//  else if(beh.state == FORWARD_STATE || beh.state == OBSTACLE_AVOIDANCE_STATE )
+//  {
+//    double target_velocity = max_velocity;
+//
+//    for(unsigned int i = 0; i < m_Path.size(); i++)
+//    {
+//      if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory == m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+//        m_Path.at(i).v = target_velocity;
+//      else
+//        m_Path.at(i).v = target_velocity*AVOIDANCE_SPEED_FACTOR;
+//    }
+//  }
+//  else
+//  {
+//    double target_velocity = 0;
+//    for(unsigned int i = 0; i < m_Path.size(); i++)
+//      m_Path.at(i).v = target_velocity;
+//  }
+//
+//  return max_velocity;
+// }
+
+ double LocalPlannerH::UpdateVelocityDirectlyToTrajectory(const BehaviorState& beh, const VehicleState& CurrStatus, const double& dt)
+ {
+  if(m_TotalOriginalPath.size() ==0 ) return 0;
+
+  RelativeInfo info, total_info;
+  PlanningHelpers::GetRelativeInfo(m_TotalOriginalPath.at(m_iCurrentTotalPathId), state, total_info);
+  PlanningHelpers::GetRelativeInfo(m_Path, state, info);
+  double average_braking_distance = -pow(CurrStatus.speed, 2)/(m_CarInfo.max_deceleration) + m_params.additionalBrakingDistance;
+  double max_velocity  = PlannerHNS::PlanningHelpers::GetVelocityAhead(m_TotalOriginalPath.at(m_iCurrentTotalPathId), total_info, m_PrevBrakingWayPoint, average_braking_distance);
+
+  unsigned int point_index = 0;
+  double critical_long_front_distance = 2.0;
+
+  if(m_Path.size() <= 5)
+  {
+    double target_velocity = 0;
+    for(unsigned int i = 0; i < m_Path.size(); i++)
+      m_Path.at(i).v = target_velocity;
+  }
+  else if(beh.state == TRAFFIC_LIGHT_STOP_STATE || beh.state == STOP_SIGN_STOP_STATE || beh.state == STOP_SIGN_WAIT_STATE || beh.state == TRAFFIC_LIGHT_WAIT_STATE)
+  {
+    PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.stopDistance - critical_long_front_distance, point_index);
+
+    double e = -beh.stopDistance;
+    double desiredVelocity = m_pidStopping.getPID(e);
+
+    for(unsigned int i =  info.iBack; i < point_index; i++)
+    {
+       if(i < m_Path.size() && i >= 0)
+         m_Path.at(i).v = desiredVelocity;
+    }
+
+    return desiredVelocity;
+  }
+  else if(beh.state == FOLLOW_STATE)
+  {
+    double targe_acceleration = -pow(CurrStatus.speed, 2)/(2.0*(beh.followDistance - critical_long_front_distance));
+    if(targe_acceleration <= 0 &&  targe_acceleration > m_CarInfo.max_deceleration/2.0)
+    {
+      double target_velocity = (targe_acceleration * dt) + CurrStatus.speed;
+
+      double e = target_velocity - CurrStatus.speed;
+      double desiredVelocity = m_pidVelocity.getPID(e);
+
+      for(unsigned int i = info.iBack; i < m_Path.size(); i++)
+      {
+        if(i < m_Path.size() && i >= 0)
+          m_Path.at(i).v = desiredVelocity;
+      }
+
+      return desiredVelocity;
+      //cout << "Accelerate -> Target V: " << target_velocity << ", Brake D: " <<  average_braking_distance << ", Acceleration: " << targe_acceleration << endl;
+    }
+    else
+    {
+      WayPoint pursuite_point = PlanningHelpers::GetFollowPointOnTrajectory(m_Path, info, beh.followDistance - critical_long_front_distance, point_index);
+
+      double e = beh.followDistance - m_params.minFollowingDistance;
+      double desiredVelocity = m_pidStopping.getPID(e);
+
+      for(unsigned int i =  info.iBack; i < point_index; i++)
+      {
+        if(i < m_Path.size() && i >= 0)
+          m_Path.at(i).v = desiredVelocity;
+      }
+
+      return desiredVelocity;
+      //cout << "Decelerate -> Target V: " << target_velocity << ", Brake D: " <<  average_braking_distance << ", Start I" << info.iBack << endl;
+    }
+
+  }
+  else if(beh.state == FORWARD_STATE || beh.state == OBSTACLE_AVOIDANCE_STATE )
+  {
+    double target_velocity = max_velocity;
+    if(m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory != m_pCurrentBehaviorState->GetCalcParams()->iCentralTrajectory)
+      target_velocity*=AVOIDANCE_SPEED_FACTOR;
+
+    double e = target_velocity - CurrStatus.speed;
+    double desiredVelocity = m_pidVelocity.getPID(e);
+
+    for(unsigned int i = info.iBack; i < m_Path.size(); i++)
+    {
+      m_Path.at(i).v = desiredVelocity;
+    }
+
+    return desiredVelocity;
+  }
+  else
+  {
+    double target_velocity = 0;
+    for(unsigned int i = info.iBack; i < m_Path.size(); i++)
+      m_Path.at(i).v = target_velocity;
+
+    return target_velocity;
+  }
+
+  return max_velocity;
+ }
+
+ void LocalPlannerH::ExtractHorizonAndCalculateRecommendedSpeed()
+ {
+   if(m_pCurrentBehaviorState->GetCalcParams()->bNewGlobalPath && m_TotalOriginalPath.size() > 0)
+    {
+       m_PrevBrakingWayPoint = 0;
+       PlanningHelpers::FixPathDensity(m_TotalOriginalPath.at(m_iCurrentTotalPathId), m_pCurrentBehaviorState->m_pParams->pathDensity);
+       PlanningHelpers::SmoothPath(m_TotalOriginalPath.at(m_iCurrentTotalPathId), 0.49, 0.25, 0.05);
+       
+      PlanningHelpers::GenerateRecommendedSpeed(m_TotalOriginalPath.at(m_iCurrentTotalPathId), m_CarInfo.max_speed_forward, m_pCurrentBehaviorState->m_pParams->speedProfileFactor);
+      m_TotalOriginalPath.at(m_iCurrentTotalPathId).at(m_TotalOriginalPath.at(m_iCurrentTotalPathId).size()-1).v = 0;
+
+    }
+
+     m_TotalPath.clear();
+
+     for(unsigned int i = 0; i < m_TotalOriginalPath.size(); i++)
+    {
+      vector<WayPoint> centerTrajectorySmoothed;
+      PlanningHelpers::ExtractPartFromPointToDistanceFast(m_TotalOriginalPath.at(i), state,
+          m_pCurrentBehaviorState->m_pParams->horizonDistance ,
+          m_pCurrentBehaviorState->m_pParams->pathDensity ,
+          centerTrajectorySmoothed,
+          m_pCurrentBehaviorState->m_pParams->smoothingDataWeight,
+          m_pCurrentBehaviorState->m_pParams->smoothingSmoothWeight,
+          m_pCurrentBehaviorState->m_pParams->smoothingToleranceError);
+
+      m_TotalPath.push_back(centerTrajectorySmoothed);
+    }
+ }
+
+ PlannerHNS::BehaviorState LocalPlannerH::DoOneStep(
+     const double& dt,
+    const PlannerHNS::VehicleState& vehicleState,
+    const std::vector<PlannerHNS::DetectedObject>& obj_list,
+    const int& goalID, PlannerHNS::RoadNetwork& map  ,
+    const bool& bEmergencyStop,
+    const std::vector<TrafficLight>& trafficLight,
+    const bool& bLive)
+{
+   PlannerHNS::BehaviorState beh;
+
+   m_params.minFollowingDistance = m_InitialFollowingDistance + vehicleState.speed*1.5;
+
+   if(!bLive)
+     SimulateOdoPosition(dt, vehicleState);
+
+  UpdateCurrentLane(map, 3.0);
+
+
+  ExtractHorizonAndCalculateRecommendedSpeed();
+
+
+  m_PredictedTrajectoryObstacles = obj_list;
+
+  timespec t;
+  UtilityH::GetTickCount(t);
+  TrajectoryCost tc = m_TrajectoryCostsCalculatotor.DoOneStep(m_RollOuts, m_TotalPath, state,
+      m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory, m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeLane, *m_pCurrentBehaviorState->m_pParams,
+      m_CarInfo,vehicleState, m_PredictedTrajectoryObstacles);
+  m_CostCalculationTime = UtilityH::GetTimeDiffNow(t);
+
+
+  UtilityH::GetTickCount(t);
+  CalculateImportantParameterForDecisionMaking(vehicleState, goalID, bEmergencyStop, trafficLight, tc);
+
+  beh = GenerateBehaviorState(vehicleState);
+  m_BehaviorGenTime = UtilityH::GetTimeDiffNow(t);
+
+  UtilityH::GetTickCount(t);
+  beh.bNewPlan = SelectSafeTrajectoryAndSpeedProfile(vehicleState);
+
+  m_RollOutsGenerationTime = UtilityH::GetTimeDiffNow(t);
+
+  beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);
+
+  return beh;
+ }
+
+} /* namespace PlannerHNS */

src1/planning/op_planner/MappingHelpers.cpp

@@ -0,0 +1,3445 @@
+
+/// \file MappingHelpers.cpp
+/// \brief Helper functions for mapping operation such as (load and initialize vector maps , convert map from one format to another, .. )
+/// \author Hatem Darweesh
+/// \date Jul 2, 2016
+
+
+
+#include "op_planner/MappingHelpers.h"
+#include "op_planner/MatrixOperations.h"
+#include "op_planner/PlanningHelpers.h"
+#include <float.h>
+
+#include "math.h"
+#include <fstream>
+
+using namespace UtilityHNS;
+using namespace std;
+#define RIGHT_INITIAL_TURNS_COST 0
+#define LEFT_INITIAL_TURNS_COST 0
+#define DEBUG_MAP_PARSING 0
+#define DEFAULT_REF_VELOCITY 60 //km/h
+
+namespace PlannerHNS
+{
+
+int MappingHelpers::g_max_point_id = 0;
+int MappingHelpers::g_max_lane_id = 0;
+int MappingHelpers::g_max_stop_line_id = 0;
+int MappingHelpers::g_max_traffic_light_id = 0;
+double MappingHelpers::m_USING_VER_ZERO = 0;
+
+MappingHelpers::MappingHelpers() {
+}
+
+MappingHelpers::~MappingHelpers() {
+}
+
+GPSPoint MappingHelpers::GetTransformationOrigin(const int& bToyotaCityMap)
+{
+//  if(bToyotaCityMap == 1)
+//    return GPSPoint(-3700, 99427, -88,0); //toyota city
+//  else if(bToyotaCityMap == 2)
+//    return GPSPoint(14805.945, 84680.211, -39.59, 0); // for moriyama map
+//  else
+    return GPSPoint();
+  //return GPSPoint(18221.1, 93546.1, -36.19, 0);
+}
+
+Lane* MappingHelpers::GetLaneById(const int& id,RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      if(map.roadSegments.at(rs).Lanes.at(i).id == id)
+        return &map.roadSegments.at(rs).Lanes.at(i);
+    }
+  }
+
+  return nullptr;
+}
+
+int MappingHelpers::GetLaneIdByWaypointId(const int& id,std::vector<Lane>& lanes)
+{
+  for(unsigned int in_l= 0; in_l < lanes.size(); in_l++)
+  {
+    for(unsigned int in_p = 0; in_p<lanes.at(in_l).points.size(); in_p++)
+    {
+      if(id == lanes.at(in_l).points.at(in_p).id)
+      {
+        return lanes.at(in_l).points.at(in_p).laneId;
+      }
+    }
+  }
+
+  return 0;
+}
+
+int MappingHelpers::ReplaceMyID(int& id,const std::vector<std::pair<int,int> >& rep_list)
+{
+  for(unsigned int i=0; i < rep_list.size(); i++)
+  {
+    if(rep_list.at(i).first == id)
+    {
+      id = rep_list.at(i).second;
+      return id;
+    }
+  }
+
+  return -1;
+}
+
+void MappingHelpers::ConstructRoadNetworkFromROSMessage(const std::vector<UtilityHNS::AisanLanesFileReader::AisanLane>& lanes_data,
+    const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+    const std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine>& dt_data,
+    const std::vector<UtilityHNS::AisanIntersectionFileReader::AisanIntersection>& intersection_data,
+    const std::vector<UtilityHNS::AisanAreasFileReader::AisanArea>& area_data,
+    const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+    const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
+    const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
+    const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
+    const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
+    const std::vector<UtilityHNS::AisanRoadEdgeFileReader::AisanRoadEdge>& roadedge_data,
+    const std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea>& wayarea_data,
+    const std::vector<UtilityHNS::AisanCrossWalkFileReader::AisanCrossWalk>& crosswalk_data,
+    const std::vector<UtilityHNS::AisanNodesFileReader::AisanNode>& nodes_data,
+    const std::vector<UtilityHNS::AisanDataConnFileReader::DataConn>& conn_data,
+    const GPSPoint& origin, RoadNetwork& map, const bool& bSpecialFlag,
+    const bool& bFindLaneChangeLanes, const bool& bFindCurbsAndWayArea)
+{
+  vector<Lane> roadLanes;
+  Lane lane_obj;
+  int laneIDSeq = 0;
+  WayPoint prevWayPoint;
+  UtilityHNS::AisanLanesFileReader::AisanLane prev_lane_point;
+  UtilityHNS::AisanLanesFileReader::AisanLane curr_lane_point;
+  UtilityHNS::AisanLanesFileReader::AisanLane next_lane_point;
+  vector<pair<int,int> > id_replace_list;
+
+  for(unsigned int l= 0; l < lanes_data.size(); l++)
+  {
+    curr_lane_point = lanes_data.at(l);
+    curr_lane_point.originalMapID = -1;
+
+    if(l+1 < lanes_data.size())
+    {
+      next_lane_point = lanes_data.at(l+1);
+      if(curr_lane_point.FLID == next_lane_point.LnID && curr_lane_point.DID == next_lane_point.DID)
+      {
+        next_lane_point.BLID = curr_lane_point.BLID;
+        if(next_lane_point.LaneDir == 'F')
+          next_lane_point.LaneDir = curr_lane_point.LaneDir;
+
+        if(curr_lane_point.BLID2 != 0)
+        {
+          if(next_lane_point.BLID2 == 0)  next_lane_point.BLID2 = curr_lane_point.BLID2;
+          else if(next_lane_point.BLID3 == 0)  next_lane_point.BLID3 = curr_lane_point.BLID2;
+          else if(next_lane_point.BLID4 == 0)  next_lane_point.BLID4 = curr_lane_point.BLID2;
+        }
+
+        if(curr_lane_point.BLID3 != 0)
+        {
+          if(next_lane_point.BLID2 == 0)  next_lane_point.BLID2 = curr_lane_point.BLID3;
+          else if(next_lane_point.BLID3 == 0)  next_lane_point.BLID3 = curr_lane_point.BLID3;
+          else if(next_lane_point.BLID4 == 0)  next_lane_point.BLID4 = curr_lane_point.BLID3;
+        }
+
+        if(curr_lane_point.BLID3 != 0)
+        {
+          if(next_lane_point.BLID2 == 0)  next_lane_point.BLID2 = curr_lane_point.BLID4;
+          else if(next_lane_point.BLID3 == 0)  next_lane_point.BLID3 = curr_lane_point.BLID4;
+          else if(next_lane_point.BLID4 == 0)  next_lane_point.BLID4 = curr_lane_point.BLID4;
+        }
+
+        if(curr_lane_point.FLID2 != 0)
+        {
+          if(next_lane_point.FLID2 == 0)  next_lane_point.FLID2 = curr_lane_point.FLID2;
+          else if(next_lane_point.FLID3 == 0)  next_lane_point.FLID3 = curr_lane_point.FLID2;
+          else if(next_lane_point.FLID4 == 0)  next_lane_point.FLID4 = curr_lane_point.FLID2;
+        }
+
+        if(curr_lane_point.FLID3 != 0)
+        {
+          if(next_lane_point.FLID2 == 0)  next_lane_point.FLID2 = curr_lane_point.FLID3;
+          else if(next_lane_point.FLID3 == 0)  next_lane_point.FLID3 = curr_lane_point.FLID3;
+          else if(next_lane_point.FLID4 == 0)  next_lane_point.FLID4 = curr_lane_point.FLID3;
+        }
+
+        if(curr_lane_point.FLID3 != 0)
+        {
+          if(next_lane_point.FLID2 == 0)  next_lane_point.FLID2 = curr_lane_point.FLID4;
+          else if(next_lane_point.FLID3 == 0)  next_lane_point.FLID3 = curr_lane_point.FLID4;
+          else if(next_lane_point.FLID4 == 0)  next_lane_point.FLID4 = curr_lane_point.FLID4;
+        }
+
+        if(prev_lane_point.FLID == curr_lane_point.LnID)
+          prev_lane_point.FLID = next_lane_point.LnID;
+
+        id_replace_list.push_back(make_pair(curr_lane_point.LnID, next_lane_point.LnID));
+        int originalMapID = curr_lane_point.LnID;
+        curr_lane_point = next_lane_point;
+        curr_lane_point.originalMapID = originalMapID;
+        l++;
+      }
+    }
+
+    if(curr_lane_point.LnID != prev_lane_point.FLID)
+    {
+      if(laneIDSeq != 0) //first lane
+      {
+        lane_obj.toIds.push_back(prev_lane_point.FLID);
+        roadLanes.push_back(lane_obj);
+//        if(lane_obj.points.size() <= 1)
+//          prev_FLID = 0;
+      }
+
+      laneIDSeq++;
+      lane_obj = Lane();
+      lane_obj.speed = curr_lane_point.LimitVel;
+      lane_obj.id = curr_lane_point.LnID;
+      lane_obj.fromIds.push_back(curr_lane_point.BLID);
+      lane_obj.roadId = laneIDSeq;
+    }
+
+    WayPoint wp;
+    bool bFound = GetWayPoint(curr_lane_point.LnID, lane_obj.id, curr_lane_point.RefVel,curr_lane_point.DID,
+        dt_data, points_data,origin, wp);
+
+    wp.originalMapID = curr_lane_point.originalMapID;
+
+    if(curr_lane_point.LaneDir == 'L')
+    {
+      wp.actionCost.push_back(make_pair(LEFT_TURN_ACTION, LEFT_INITIAL_TURNS_COST));
+      //std::cout << " Left Lane : " << curr_lane_point.LnID << std::endl ;
+    }
+    else  if(curr_lane_point.LaneDir == 'R')
+    {
+      wp.actionCost.push_back(make_pair(RIGHT_TURN_ACTION, RIGHT_INITIAL_TURNS_COST));
+      //std::cout << " Right Lane : " << curr_lane_point.LnID << std::endl ;
+    }
+    else
+    {
+      wp.actionCost.push_back(make_pair(FORWARD_ACTION, 0));
+    }
+
+    wp.fromIds.push_back(curr_lane_point.BLID);
+    wp.toIds.push_back(curr_lane_point.FLID);
+
+    //if(curr_lane_point.JCT > 0)
+    if(curr_lane_point.FLID2 > 0)
+    {
+      lane_obj.toIds.push_back(curr_lane_point.FLID2);
+      wp.toIds.push_back(curr_lane_point.FLID2);
+    }
+    if(curr_lane_point.FLID3 > 0)
+    {
+      lane_obj.toIds.push_back(curr_lane_point.FLID3);
+      wp.toIds.push_back(curr_lane_point.FLID3);
+    }
+    if(curr_lane_point.FLID4 > 0)
+    {
+      lane_obj.toIds.push_back(curr_lane_point.FLID4);
+      wp.toIds.push_back(curr_lane_point.FLID4);
+    }
+
+    if(curr_lane_point.BLID2 > 0)
+    {
+      lane_obj.fromIds.push_back(curr_lane_point.BLID2);
+      wp.fromIds.push_back(curr_lane_point.BLID2);
+    }
+    if(curr_lane_point.BLID3 > 0)
+    {
+      lane_obj.fromIds.push_back(curr_lane_point.BLID3);
+      wp.fromIds.push_back(curr_lane_point.BLID3);
+    }
+    if(curr_lane_point.BLID4 > 0)
+    {
+      lane_obj.fromIds.push_back(curr_lane_point.BLID4);
+      wp.fromIds.push_back(curr_lane_point.BLID4);
+    }
+
+    //if(prev_lane_point.DID == curr_lane_point.DID && curr_lane_point.LnID == prev_lane_point.FLID)
+//    if(prevWayPoint.pos.x == wp.pos.x && prevWayPoint.pos.y == wp.pos.y)
+//    {
+//      //if((prev_lane_point.FLID2 != 0 && curr_lane_point.FLID2 != 0) || (prev_lane_point.FLID3 != 0 && curr_lane_point.FLID3 != 0) || (prev_lane_point.FLID4 != 0 && curr_lane_point.FLID4 != 0))
+//      {
+//        cout << "Prev WP, LnID: " << prev_lane_point.LnID << ",BLID: " << prev_lane_point.BLID << ",FLID: " << prev_lane_point.FLID << ",DID: " << prev_lane_point.DID
+//            << ", Begin: " << prev_lane_point.BLID2 << "," << prev_lane_point.BLID3 << "," << prev_lane_point.BLID4
+//            << ", End: " << prev_lane_point.FLID2 << "," << prev_lane_point.FLID3 << "," << prev_lane_point.FLID4 << ": " << prev_lane_point.LaneDir <<   endl;
+//        cout << "Curr WP, LnID: " << curr_lane_point.LnID << ",BLID: " << curr_lane_point.BLID << ",FLID: " << curr_lane_point.FLID << ",DID: " << curr_lane_point.DID
+//            << ", Begin: " << curr_lane_point.BLID2 <<  "," << curr_lane_point.BLID3 <<  "," << curr_lane_point.BLID4
+//            << ", End: " << curr_lane_point.FLID2 <<  "," <<curr_lane_point.FLID3 <<  "," << curr_lane_point.FLID4 <<   ": " << curr_lane_point.LaneDir << endl << endl;
+//      }
+//    }
+
+    if(bFound)
+    {
+      lane_obj.points.push_back(wp);
+      prevWayPoint = wp;
+    }
+    else
+      cout << " Strange ! point is not in the map !! " << endl;
+
+    prev_lane_point = curr_lane_point;
+  }
+
+  //delete first two lanes !!!!! Don't know why , you don't know why ! , these two line cost you a lot .. ! why why , works for toyota map , but not with moriyama
+  if(bSpecialFlag)
+  {
+    if(roadLanes.size() > 0)
+      roadLanes.erase(roadLanes.begin()+0);
+    if(roadLanes.size() > 0)
+      roadLanes.erase(roadLanes.begin()+0);
+  }
+
+  roadLanes.push_back(lane_obj);
+
+  for(unsigned int i =0; i < roadLanes.size(); i++)
+  {
+    Lane* pL = &roadLanes.at(i);
+    ReplaceMyID(pL->id, id_replace_list);
+
+    for(unsigned int j = 0 ; j < pL->fromIds.size(); j++)
+    {
+      int id = ReplaceMyID(pL->fromIds.at(j), id_replace_list);
+      if(id != -1)
+        pL->fromIds.at(j) = id;
+    }
+
+    for(unsigned int j = 0 ; j < pL->toIds.size(); j++)
+    {
+      int id = ReplaceMyID(pL->toIds.at(j), id_replace_list);
+      if(id != -1)
+        pL->toIds.at(j) = id;
+    }
+
+    for(unsigned int j = 0 ; j < pL->points.size(); j++)
+    {
+      ReplaceMyID(pL->points.at(j).id, id_replace_list);
+      ReplaceMyID(pL->points.at(j).laneId, id_replace_list);
+
+      for(unsigned int jj = 0 ; jj < pL->points.at(j).fromIds.size(); jj++)
+      {
+        int id = ReplaceMyID(pL->points.at(j).fromIds.at(jj), id_replace_list);
+        if(id != -1)
+          pL->points.at(j).fromIds.at(jj) = id;
+      }
+
+      for(unsigned int jj = 0 ; jj < pL->points.at(j).toIds.size(); jj++)
+      {
+        int id = ReplaceMyID(pL->points.at(j).toIds.at(jj), id_replace_list);
+        if(id != -1)
+          pL->points.at(j).toIds.at(jj) = id;
+      }
+    }
+  }
+
+  //Link Lanes and lane's waypoints by pointers
+  //For each lane, the previous code set the fromId as the id of the last waypoint of the previos lane.
+  //here we fix that by finding from each fromID the corresponding point and replace the fromId by the LaneID associated with that point.
+  for(unsigned int l= 0; l < roadLanes.size(); l++)
+  {
+    for(unsigned int fp = 0; fp< roadLanes.at(l).fromIds.size(); fp++)
+    {
+      roadLanes.at(l).fromIds.at(fp) = GetLaneIdByWaypointId(roadLanes.at(l).fromIds.at(fp), roadLanes);
+    }
+
+    for(unsigned int tp = 0; tp< roadLanes.at(l).toIds.size(); tp++)
+    {
+      roadLanes.at(l).toIds.at(tp) = GetLaneIdByWaypointId(roadLanes.at(l).toIds.at(tp), roadLanes);
+    }
+
+    double sum_a = 0;
+    for(unsigned int j = 0 ; j < roadLanes.at(l).points.size(); j++)
+    {
+      sum_a += roadLanes.at(l).points.at(j).pos.a;
+    }
+    roadLanes.at(l).dir = sum_a/(double)roadLanes.at(l).points.size();
+  }
+
+  //map has one road segment
+  RoadSegment roadSegment1;
+  roadSegment1.id = 1;
+  roadSegment1.Lanes = roadLanes;
+  map.roadSegments.push_back(roadSegment1);
+
+  //Link Lanes and lane's waypoints by pointers
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    //Link Lanes
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      for(unsigned int j = 0 ; j < pL->fromIds.size(); j++)
+      {
+        for(unsigned int l= 0; l < map.roadSegments.at(rs).Lanes.size(); l++)
+        {
+          if(map.roadSegments.at(rs).Lanes.at(l).id == pL->fromIds.at(j))
+          {
+            pL->fromLanes.push_back(&map.roadSegments.at(rs).Lanes.at(l));
+          }
+        }
+      }
+
+      for(unsigned int j = 0 ; j < pL->toIds.size(); j++)
+      {
+        for(unsigned int l= 0; l < map.roadSegments.at(rs).Lanes.size(); l++)
+        {
+          if(map.roadSegments.at(rs).Lanes.at(l).id == pL->toIds.at(j))
+          {
+            pL->toLanes.push_back(&map.roadSegments.at(rs).Lanes.at(l));
+          }
+        }
+      }
+
+      for(unsigned int j = 0 ; j < pL->points.size(); j++)
+      {
+        pL->points.at(j).pLane  = pL;
+      }
+    }
+  }
+
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      for(unsigned int j = 0 ; j < pL->points.size(); j++)
+      {
+          if(pL->points.at(j).actionCost.size() > 0)
+            {
+          if(pL->points.at(j).actionCost.at(0).first == LEFT_TURN_ACTION)
+            {
+              AssignActionCostToLane(pL, LEFT_TURN_ACTION, LEFT_INITIAL_TURNS_COST);
+              break;
+            }
+          else if(pL->points.at(j).actionCost.at(0).first == RIGHT_TURN_ACTION)
+            {
+              AssignActionCostToLane(pL, RIGHT_TURN_ACTION, RIGHT_INITIAL_TURNS_COST);
+            break;
+
+            }
+            }
+      }
+    }
+  }
+
+  if(bFindLaneChangeLanes)
+  {
+    cout << " >> Extract Lane Change Information... " << endl;
+    FindAdjacentLanes(map);
+  }
+
+  //Extract Signals and StopLines
+  // Signals
+  ExtractSignalData(signal_data, vector_data, points_data, origin, map);
+
+
+  //Stop Lines
+  ExtractStopLinesData(stop_line_data, line_data, points_data, origin, map);
+
+
+  //Link waypoints
+  LinkMissingBranchingWayPoints(map);
+
+  //Link StopLines and Traffic Lights
+  LinkTrafficLightsAndStopLines(map);
+  //LinkTrafficLightsAndStopLinesConData(conn_data, id_replace_list, map);
+
+  if(bFindCurbsAndWayArea)
+  {
+    //Curbs
+    ExtractCurbData(curb_data, line_data, points_data, origin, map);
+
+    //Wayarea
+    ExtractWayArea(area_data, wayarea_data, line_data, points_data, origin, map);
+  }
+
+  //Fix angle for lanes
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      PlannerHNS::PlanningHelpers::FixAngleOnly(pL->points);
+    }
+  }
+
+  cout << "Map loaded from data with " << roadLanes.size()  << " lanes" << endl;
+}
+
+void MappingHelpers::AssignActionCostToLane(Lane* pL, ACTION_TYPE action, double cost)
+{
+  for(unsigned int j = 0 ; j < pL->points.size(); j++)
+  {
+      pL->points.at(j).actionCost.clear();
+      pL->points.at(j).actionCost.push_back(make_pair(action, cost));
+  }
+}
+
+WayPoint* MappingHelpers::FindWaypoint(const int& id, RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int p= 0; p < map.roadSegments.at(rs).Lanes.at(i).points.size(); p++)
+      {
+        if(map.roadSegments.at(rs).Lanes.at(i).points.at(p).id == id)
+          return &map.roadSegments.at(rs).Lanes.at(i).points.at(p);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+WayPoint* MappingHelpers::FindWaypointV2(const int& id, const int& l_id, RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pLane = &map.roadSegments.at(rs).Lanes.at(i);
+      if(pLane ->id != l_id)
+      {
+        for(unsigned int p= 0; p < pLane->points.size(); p++)
+        {
+          if(pLane->points.at(p).id == id)
+            return &pLane->points.at(p);
+        }
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+void MappingHelpers::ConstructRoadNetworkFromDataFiles(const std::string vectoMapPath, RoadNetwork& map, const bool& bZeroOrigin)
+{
+  /**
+   * Exporting the center lines
+   */
+  string laneLinesDetails = vectoMapPath + "point.csv";
+  string center_lines_info = vectoMapPath + "dtlane.csv";
+  string lane_info = vectoMapPath + "lane.csv";
+  string node_info = vectoMapPath + "node.csv";
+  string area_info = vectoMapPath + "area.csv";
+  string line_info = vectoMapPath + "line.csv";
+  string signal_info = vectoMapPath + "signaldata.csv";
+  string stop_line_info = vectoMapPath + "stopline.csv";
+  string vector_info = vectoMapPath + "vector.csv";
+  string curb_info = vectoMapPath + "curb.csv";
+  string roadedge_info = vectoMapPath + "roadedge.csv";
+  string wayarea_info = vectoMapPath + "wayarea.csv";
+  string crosswalk_info = vectoMapPath + "crosswalk.csv";
+  string conn_info = vectoMapPath + "dataconnection.csv";
+  string intersection_info = vectoMapPath + "intersection.csv";
+
+  cout << " >> Loading vector map data files ... " << endl;
+  AisanCenterLinesFileReader  center_lanes(center_lines_info);
+  AisanLanesFileReader lanes(lane_info);
+  AisanPointsFileReader points(laneLinesDetails);
+  AisanNodesFileReader nodes(node_info);
+  AisanLinesFileReader lines(line_info);
+  AisanStopLineFileReader stop_line(stop_line_info);
+  AisanSignalFileReader signal(signal_info);
+  AisanVectorFileReader vec(vector_info);
+  AisanCurbFileReader curb(curb_info);
+  AisanRoadEdgeFileReader roadedge(roadedge_info);
+  AisanDataConnFileReader conn(conn_info);
+  AisanAreasFileReader areas(area_info);
+  AisanWayareaFileReader way_area(wayarea_info);
+  AisanCrossWalkFileReader cross_walk(crosswalk_info);
+
+
+  vector<AisanIntersectionFileReader::AisanIntersection> intersection_data;
+  vector<AisanNodesFileReader::AisanNode> nodes_data;
+  vector<AisanLanesFileReader::AisanLane> lanes_data;
+  vector<AisanPointsFileReader::AisanPoints> points_data;
+  vector<AisanCenterLinesFileReader::AisanCenterLine> dt_data;
+  vector<AisanLinesFileReader::AisanLine> line_data;
+  vector<AisanStopLineFileReader::AisanStopLine> stop_line_data;
+  vector<AisanSignalFileReader::AisanSignal> signal_data;
+  vector<AisanVectorFileReader::AisanVector> vector_data;
+  vector<AisanCurbFileReader::AisanCurb> curb_data;
+  vector<AisanRoadEdgeFileReader::AisanRoadEdge> roadedge_data;
+  vector<AisanAreasFileReader::AisanArea> area_data;
+  vector<AisanWayareaFileReader::AisanWayarea> way_area_data;
+  vector<AisanCrossWalkFileReader::AisanCrossWalk> crosswalk_data;
+  vector<AisanDataConnFileReader::DataConn> conn_data;
+
+
+  nodes.ReadAllData(nodes_data);
+  lanes.ReadAllData(lanes_data);
+  points.ReadAllData(points_data);
+  center_lanes.ReadAllData(dt_data);
+  lines.ReadAllData(line_data);
+  stop_line.ReadAllData(stop_line_data);
+  signal.ReadAllData(signal_data);
+  vec.ReadAllData(vector_data);
+  curb.ReadAllData(curb_data);
+  roadedge.ReadAllData(roadedge_data);
+  areas.ReadAllData(area_data);
+  way_area.ReadAllData(way_area_data);
+  cross_walk.ReadAllData(crosswalk_data);
+  conn.ReadAllData(conn_data);
+
+  if(points_data.size() == 0)
+  {
+    std::cout << std::endl << "## Alert Can't Read Points Data from vector map files in path: " << vectoMapPath << std::endl;
+    return;
+  }
+
+  //Special Condtion to be able to pars old data structures
+  int bSpecialMap = 0;
+
+  // use this to transform data to origin (0,0,0)
+  if(nodes_data.size() > 0 && bSpecialMap == 0)
+  {
+    ConstructRoadNetworkFromROSMessageV2(lanes_data, points_data, dt_data, intersection_data, area_data,
+        line_data, stop_line_data, signal_data, vector_data, curb_data, roadedge_data,
+        way_area_data, crosswalk_data, nodes_data, conn_data, &lanes, &points, &nodes, &lines,
+        GetTransformationOrigin(0), map, false);
+  }
+  else
+  {
+    ConstructRoadNetworkFromROSMessage(lanes_data, points_data, dt_data, intersection_data, area_data,
+            line_data, stop_line_data, signal_data, vector_data, curb_data, roadedge_data,
+            way_area_data, crosswalk_data, nodes_data, conn_data,
+            GetTransformationOrigin(0), map, bSpecialMap == 1);
+  }
+
+  WayPoint origin = GetFirstWaypoint(map);
+  cout << origin.pos.ToString() ;
+}
+
+bool MappingHelpers::GetWayPoint(const int& id, const int& laneID,const double& refVel, const int& did,
+    const std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine>& dtpoints,
+    const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points,
+    const GPSPoint& origin, WayPoint& way_point)
+{
+
+  for(unsigned int dtp =0; dtp < dtpoints.size(); dtp++)
+  {
+    if(dtpoints.at(dtp).DID == did)
+    {
+      for(unsigned int p =0; p < points.size(); p++)
+      {
+        if(dtpoints.at(dtp).PID == points.at(p).PID)
+        {
+          WayPoint wp;
+          wp.id = id;
+          wp.laneId = laneID;
+          wp.v = refVel;
+//          double integ_part = points.at(p).L;
+//          double deg = trunc(points.at(p).L);
+//          double min = trunc((points.at(p).L - deg) * 100.0) / 60.0;
+//          double sec = modf((points.at(p).L - deg) * 100.0, &integ_part)/36.0;
+//          double L =  deg + min + sec;
+//
+//          deg = trunc(points.at(p).B);
+//          min = trunc((points.at(p).B - deg) * 100.0) / 60.0;
+//          sec = modf((points.at(p).B - deg) * 100.0, &integ_part)/36.0;
+//          double B = deg + min + sec;
+
+          wp.pos = GPSPoint(points.at(p).Ly + origin.x, points.at(p).Bx + origin.y, points.at(p).H + origin.z, dtpoints.at(dtp).Dir);
+
+          wp.pos.lat = points.at(p).L;
+          wp.pos.lon = points.at(p).B;
+          wp.pos.alt = points.at(p).H;
+          wp.pos.dir = dtpoints.at(dtp).Dir;
+          wp.iOriginalIndex = p;
+
+          way_point = wp;
+          return 1;
+        }
+      }
+    }
+  }
+
+  return false;
+}
+
+void MappingHelpers::LoadKML(const std::string& kmlFile, RoadNetwork& map)
+{
+  //First, Get the main element
+  TiXmlElement* pHeadElem = 0;
+  TiXmlElement* pElem = 0;
+
+  ifstream f(kmlFile.c_str());
+  if(!f.good())
+  {
+    cout << "Can't Open KML Map File: (" << kmlFile << ")" << endl;
+    return;
+  }
+
+  std::cout << " >> Loading KML Map file ... " << std::endl;
+
+  TiXmlDocument doc(kmlFile);
+  try
+  {
+    doc.LoadFile();
+  }
+  catch(exception& e)
+  {
+    cout << "KML Custom Reader Error, Can't Load .kml File, path is: "<<  kmlFile << endl;
+    cout << e.what() << endl;
+    return;
+  }
+
+
+  std::cout << " >> Reading Data from KML map file ... " << std::endl;
+
+  pElem = doc.FirstChildElement();
+  pHeadElem = GetHeadElement(pElem);
+
+  std::cout << " >> Load Lanes from KML file .. " << std::endl;
+  vector<Lane> laneLinksList = GetLanesList(pHeadElem);
+
+  map.roadSegments.clear();
+  map.roadSegments = GetRoadSegmentsList(pHeadElem);
+
+  std::cout << " >> Load Traffic lights from KML file .. " << std::endl;
+  vector<TrafficLight> trafficLights = GetTrafficLightsList(pHeadElem);
+
+  std::cout << " >> Load Stop lines from KML file .. " << std::endl;
+  vector<StopLine> stopLines = GetStopLinesList(pHeadElem);
+
+  std::cout << " >> Load Signes from KML file .. " << std::endl;
+  vector<TrafficSign> signs = GetTrafficSignsList(pHeadElem);
+
+  std::cout << " >> Load Crossings from KML file .. " << std::endl;
+  vector<Crossing> crossings = GetCrossingsList(pHeadElem);
+
+  std::cout << " >> Load Markings from KML file .. " << std::endl;
+  vector<Marking> markings = GetMarkingsList(pHeadElem);
+
+  std::cout << " >> Load Road boundaries from KML file .. " << std::endl;
+  vector<Boundary> boundaries = GetBoundariesList(pHeadElem);
+
+  std::cout << " >> Load Curbs from KML file .. " << std::endl;
+  vector<Curb> curbs = GetCurbsList(pHeadElem);
+
+  map.signs.clear();
+  map.signs = signs;
+
+  map.crossings.clear();
+  map.crossings = crossings;
+
+  map.markings.clear();
+  map.markings = markings;
+
+  map.boundaries.clear();
+  map.boundaries = boundaries;
+
+  map.curbs.clear();
+  map.curbs = curbs;
+
+  //Fill the relations
+  for(unsigned int i= 0; i<map.roadSegments.size(); i++ )
+  {
+    for(unsigned int j=0; j < laneLinksList.size(); j++)
+    {
+      PlanningHelpers::CalcAngleAndCost(laneLinksList.at(j).points);
+      map.roadSegments.at(i).Lanes.push_back(laneLinksList.at(j));
+    }
+  }
+
+  cout << " >> Link lanes and waypoints with pointers ... " << endl;
+  //Link Lanes and lane's waypoints by pointers
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    //Link Lanes
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      for(unsigned int j = 0 ; j < pL->fromIds.size(); j++)
+      {
+        for(unsigned int l= 0; l < map.roadSegments.at(rs).Lanes.size(); l++)
+        {
+          if(map.roadSegments.at(rs).Lanes.at(l).id == pL->fromIds.at(j))
+          {
+            pL->fromLanes.push_back(&map.roadSegments.at(rs).Lanes.at(l));
+          }
+        }
+      }
+
+      for(unsigned int j = 0 ; j < pL->toIds.size(); j++)
+      {
+        for(unsigned int l= 0; l < map.roadSegments.at(rs).Lanes.size(); l++)
+        {
+          if(map.roadSegments.at(rs).Lanes.at(l).id == pL->toIds.at(j))
+          {
+            pL->toLanes.push_back(&map.roadSegments.at(rs).Lanes.at(l));
+          }
+        }
+      }
+
+      for(unsigned int j = 0 ; j < pL->points.size(); j++)
+      {
+        pL->points.at(j).pLane  = pL;
+      }
+    }
+  }
+
+  //Link waypoints
+  cout << " >> Link missing branches and waypoints... " << endl;
+  LinkMissingBranchingWayPointsV2(map);
+
+  cout << " >> Link Lane change semantics ... " << endl;
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int p= 0; p < map.roadSegments.at(rs).Lanes.at(i).points.size(); p++)
+      {
+        WayPoint* pWP = &map.roadSegments.at(rs).Lanes.at(i).points.at(p);
+        if(pWP->pLeft == 0 && pWP->LeftPointId > 0)
+        {
+          pWP->pLeft = FindWaypointV2(pWP->LeftPointId, pWP->laneId, map);
+
+          if(pWP->pLeft != nullptr)
+          {
+            pWP->LeftLnId = pWP->pLeft->laneId;
+            pWP->pLane->pLeftLane = pWP->pLeft->pLane;
+
+            if(pWP->pLeft->RightPointId == pWP->id)
+            {
+              pWP->pLeft->pRight = pWP;
+              pWP->pLeft->RightLnId = pWP->laneId;
+              pWP->pLeft->pLane->pRightLane = pWP->pLane;
+            }
+          }
+        }
+
+        if(pWP->pRight == 0 && pWP->RightPointId > 0)
+        {
+          pWP->pRight = FindWaypointV2(pWP->RightPointId, pWP->laneId, map);
+
+          if(pWP->pRight != nullptr)
+          {
+            pWP->RightLnId = pWP->pRight->laneId;
+            pWP->pLane->pRightLane = pWP->pRight->pLane;
+
+            if(pWP->pRight->LeftPointId == pWP->id)
+            {
+              pWP->pRight->pLeft = pWP;
+              pWP->pRight->LeftLnId = pWP->laneId;
+              pWP->pRight->pLane->pLeftLane = pWP->pLane;
+            }
+          }
+        }
+      }
+    }
+  }
+
+  map.stopLines = stopLines;
+  map.trafficLights = trafficLights;
+
+  //Link waypoints && StopLines
+  cout << " >> Link Stop lines and Traffic lights ... " << endl;
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int itl = 0; itl < map.trafficLights.size(); itl++)
+      {
+        if(map.trafficLights.at(itl).CheckLane(map.roadSegments.at(rs).Lanes.at(i).id))
+        {
+          map.trafficLights.at(itl).pLanes.push_back(&map.roadSegments.at(rs).Lanes.at(i));
+          map.roadSegments.at(rs).Lanes.at(i).trafficlights.push_back(map.trafficLights.at(itl));
+        }
+      }
+
+      for(unsigned int isl = 0; isl < map.stopLines.size(); isl++)
+      {
+        if(map.stopLines.at(isl).laneId == map.roadSegments.at(rs).Lanes.at(i).id)
+        {
+          map.stopLines.at(isl).pLane = &map.roadSegments.at(rs).Lanes.at(i);
+          map.roadSegments.at(rs).Lanes.at(i).stopLines.push_back(map.stopLines.at(isl));
+          WayPoint wp((map.stopLines.at(isl).points.at(0).x+map.stopLines.at(isl).points.at(1).x)/2.0, (map.stopLines.at(isl).points.at(0).y+map.stopLines.at(isl).points.at(1).y)/2.0, (map.stopLines.at(isl).points.at(0).z+map.stopLines.at(isl).points.at(1).z)/2.0, (map.stopLines.at(isl).points.at(0).a+map.stopLines.at(isl).points.at(1).a)/2.0);
+          map.roadSegments.at(rs).Lanes.at(i).points.at(PlanningHelpers::GetClosestNextPointIndexFast(map.roadSegments.at(rs).Lanes.at(i).points, wp)).stopLineID = map.stopLines.at(isl).id;
+        }
+      }
+    }
+  }
+
+  cout << " >> Find Max IDs ... " << endl;
+  GetMapMaxIds(map);
+
+  cout << "Map loaded from kml file with (" << laneLinksList.size()  << ") lanes, First Point ( " << GetFirstWaypoint(map).pos.ToString() << ")"<< endl;
+
+}
+
+TiXmlElement* MappingHelpers::GetHeadElement(TiXmlElement* pMainElem)
+{
+  TiXmlElement* pElem = pMainElem;
+  if(pElem)
+    pElem = pElem->FirstChildElement("Folder");
+  if(pElem && pElem->FirstChildElement("Folder"))
+    pElem = pElem->FirstChildElement("Folder");
+  if(pElem && pElem->FirstChildElement("Document"))
+    pElem = pElem->FirstChildElement("Document");
+
+  if(!pElem)
+  {
+    return nullptr;
+  }
+  return pElem;
+}
+
+TiXmlElement* MappingHelpers::GetDataFolder(const string& folderName, TiXmlElement* pMainElem)
+{
+  if(!pMainElem) return nullptr;
+
+  TiXmlElement* pElem = pMainElem->FirstChildElement("Folder");
+
+  string folderID="";
+  for(; pElem; pElem=pElem->NextSiblingElement())
+  {
+    folderID="";
+    if(pElem->FirstChildElement("name")->GetText()) //Map Name
+      folderID = pElem->FirstChildElement("name")->GetText();
+    if(folderID.compare(folderName)==0)
+      return pElem;
+  }
+  return nullptr;
+}
+
+WayPoint* MappingHelpers::GetClosestWaypointFromMap(const WayPoint& pos, RoadNetwork& map, const bool bDirectionBased)
+{
+  double distance_to_nearest_lane = 1;
+  Lane* pLane = 0;
+  while(distance_to_nearest_lane < 100 && pLane == 0)
+  {
+    pLane = GetClosestLaneFromMap(pos, map, distance_to_nearest_lane, bDirectionBased);
+    distance_to_nearest_lane += 1;
+  }
+
+  if(!pLane) return nullptr;
+
+  int closest_index = PlanningHelpers::GetClosestNextPointIndexFast(pLane->points, pos);
+
+  return &pLane->points.at(closest_index);
+}
+
+vector<WayPoint*> MappingHelpers::GetClosestWaypointsListFromMap(const WayPoint& pos, RoadNetwork& map, const double& distance, const bool bDirectionBased)
+{
+  vector<Lane*> pLanes = GetClosestLanesListFromMap(pos, map, distance, bDirectionBased);
+
+  vector<WayPoint*> waypoints_list;
+
+  if(pLanes.size() == 0) return waypoints_list;
+
+  for(unsigned int i = 0; i < pLanes.size(); i++)
+  {
+    if(pLanes.at(i))
+    {
+      int closest_index = PlanningHelpers::GetClosestNextPointIndexFast(pLanes.at(i)->points, pos);
+      waypoints_list.push_back(&pLanes.at(i)->points.at(closest_index));
+    }
+  }
+
+  return waypoints_list;
+}
+
+WayPoint* MappingHelpers::GetClosestBackWaypointFromMap(const WayPoint& pos, RoadNetwork& map)
+{
+  double distance_to_nearest_lane = 1;
+  Lane* pLane = 0;
+  while(distance_to_nearest_lane < 100 && pLane == 0)
+  {
+    pLane = GetClosestLaneFromMap(pos, map, distance_to_nearest_lane);
+    distance_to_nearest_lane += 1;
+  }
+
+  if(!pLane) return nullptr;
+
+  int closest_index = PlanningHelpers::GetClosestNextPointIndexFast(pLane->points, pos);
+
+  if(closest_index>2)
+    return &pLane->points.at(closest_index-3);
+  else if(closest_index>1)
+    return &pLane->points.at(closest_index-2);
+  else if(closest_index>0)
+    return &pLane->points.at(closest_index-1);
+  else
+    return &pLane->points.at(closest_index);
+}
+
+std::vector<Lane*> MappingHelpers::GetClosestLanesFast(const WayPoint& center, RoadNetwork& map, const double& distance)
+{
+  vector<Lane*> lanesList;
+  for(unsigned int j=0; j< map.roadSegments.size(); j ++)
+  {
+    for(unsigned int k=0; k< map.roadSegments.at(j).Lanes.size(); k ++)
+    {
+      Lane* pL = &map.roadSegments.at(j).Lanes.at(k);
+      int index = PlanningHelpers::GetClosestNextPointIndexFast(pL->points, center);
+
+      if(index < 0 || index >= pL->points.size()) continue;
+
+      double d = hypot(pL->points.at(index).pos.y - center.pos.y, pL->points.at(index).pos.x - center.pos.x);
+      if(d <= distance)
+        lanesList.push_back(pL);
+    }
+  }
+
+  return lanesList;
+}
+
+Lane* MappingHelpers::GetClosestLaneFromMap(const WayPoint& pos, RoadNetwork& map, const double& distance, const bool bDirectionBased)
+{
+  vector<pair<double, Lane*> > laneLinksList;
+  double d = 0;
+  double min_d = DBL_MAX;
+  //std::cout<<"road segmentt size: "<<map.roadSegments.size()<<std::endl;
+  for(unsigned int j=0; j< map.roadSegments.size(); j ++)
+  {
+    for(unsigned int k=0; k< map.roadSegments.at(j).Lanes.size(); k ++)
+    {
+      //Lane* pLane = &pEdge->lanes.at(k);
+       d = 0;
+      min_d = DBL_MAX;
+      for(unsigned int pindex=0; pindex< map.roadSegments.at(j).Lanes.at(k).points.size(); pindex ++)
+      {
+
+        d = distance2points(map.roadSegments.at(j).Lanes.at(k).points.at(pindex).pos, pos.pos);
+        if(d < min_d)
+          min_d = d;
+      }
+
+      if(min_d < distance)
+        laneLinksList.push_back(make_pair(min_d, &map.roadSegments.at(j).Lanes.at(k)));
+    }
+  }
+
+  if(laneLinksList.size() == 0) return nullptr;
+
+  min_d = DBL_MAX;
+  Lane* closest_lane = 0;
+  for(unsigned int i = 0; i < laneLinksList.size(); i++)
+  {
+    RelativeInfo info;
+    PlanningHelpers::GetRelativeInfo(laneLinksList.at(i).second->points, pos, info);
+
+    if(info.perp_distance == 0 && laneLinksList.at(i).first != 0)
+      continue;
+
+    if(bDirectionBased && fabs(info.perp_distance) < min_d && fabs(info.angle_diff) < 45)
+    {
+      min_d = fabs(info.perp_distance);
+      closest_lane = laneLinksList.at(i).second;
+    }
+    else if(!bDirectionBased && fabs(info.perp_distance) < min_d)
+    {
+      min_d = fabs(info.perp_distance);
+      closest_lane = laneLinksList.at(i).second;
+    }
+  }
+
+  //std::cout<<" min d : "<<min_d<<std::endl;;
+  return closest_lane;
+}
+
+vector<Lane*> MappingHelpers::GetClosestLanesListFromMap(const WayPoint& pos, RoadNetwork& map, const double& distance, const bool bDirectionBased)
+{
+  vector<pair<double, Lane*> > laneLinksList;
+  double d = 0;
+  double min_d = DBL_MAX;
+  for(unsigned int j=0; j< map.roadSegments.size(); j ++)
+  {
+    for(unsigned int k=0; k< map.roadSegments.at(j).Lanes.size(); k ++)
+    {
+      //Lane* pLane = &pEdge->lanes.at(k);
+       d = 0;
+      min_d = DBL_MAX;
+      for(unsigned int pindex=0; pindex< map.roadSegments.at(j).Lanes.at(k).points.size(); pindex ++)
+      {
+
+        d = distance2points(map.roadSegments.at(j).Lanes.at(k).points.at(pindex).pos, pos.pos);
+        if(d < min_d)
+          min_d = d;
+      }
+
+      if(min_d < distance)
+        laneLinksList.push_back(make_pair(min_d, &map.roadSegments.at(j).Lanes.at(k)));
+    }
+  }
+
+  vector<Lane*> closest_lanes;
+  if(laneLinksList.size() == 0) return closest_lanes;
+
+
+  for(unsigned int i = 0; i < laneLinksList.size(); i++)
+  {
+    RelativeInfo info;
+    PlanningHelpers::GetRelativeInfo(laneLinksList.at(i).second->points, pos, info);
+
+    if(info.perp_distance == 0 && laneLinksList.at(i).first != 0)
+      continue;
+
+    if(bDirectionBased && fabs(info.perp_distance) < distance && fabs(info.angle_diff) < 30)
+    {
+      closest_lanes.push_back(laneLinksList.at(i).second);
+    }
+    else if(!bDirectionBased && fabs(info.perp_distance) < distance)
+    {
+      closest_lanes.push_back(laneLinksList.at(i).second);
+    }
+  }
+
+  return closest_lanes;
+}
+
+Lane* MappingHelpers::GetClosestLaneFromMapDirectionBased(const WayPoint& pos, RoadNetwork& map, const double& distance)
+{
+  vector<pair<double, WayPoint*> > laneLinksList;
+  double d = 0;
+  double min_d = DBL_MAX;
+  int min_i = 0;
+  for(unsigned int j=0; j< map.roadSegments.size(); j ++)
+  {
+    for(unsigned int k=0; k< map.roadSegments.at(j).Lanes.size(); k ++)
+    {
+      //Lane* pLane = &pEdge->lanes.at(k);
+      d = 0;
+      min_d = DBL_MAX;
+      for(unsigned int pindex=0; pindex< map.roadSegments.at(j).Lanes.at(k).points.size(); pindex ++)
+      {
+
+        d = distance2points(map.roadSegments.at(j).Lanes.at(k).points.at(pindex).pos, pos.pos);
+        if(d < min_d)
+        {
+          min_d = d;
+          min_i = pindex;
+        }
+      }
+
+      if(min_d < distance)
+        laneLinksList.push_back(make_pair(min_d, &map.roadSegments.at(j).Lanes.at(k).points.at(min_i)));
+    }
+  }
+
+  if(laneLinksList.size() == 0) return nullptr;
+
+  min_d = DBL_MAX;
+  Lane* closest_lane = 0;
+  double a_diff = 0;
+  for(unsigned int i = 0; i < laneLinksList.size(); i++)
+  {
+    RelativeInfo info;
+    PlanningHelpers::GetRelativeInfo(laneLinksList.at(i).second->pLane->points, pos, info);
+    if(info.perp_distance == 0 && laneLinksList.at(i).first != 0)
+      continue;
+
+    a_diff = UtilityH::AngleBetweenTwoAnglesPositive(laneLinksList.at(i).second->pos.a, pos.pos.a);
+
+    if(fabs(info.perp_distance)<min_d && a_diff <= M_PI_4)
+    {
+      min_d = fabs(info.perp_distance);
+      closest_lane = laneLinksList.at(i).second->pLane;
+    }
+  }
+
+  return closest_lane;
+}
+
+
+ std::vector<Lane*> MappingHelpers::GetClosestMultipleLanesFromMap(const WayPoint& pos, RoadNetwork& map, const double& distance)
+{
+  vector<Lane*> lanesList;
+  double d = 0;
+  double a_diff = 0;
+  for(unsigned int j=0; j< map.roadSegments.size(); j ++)
+  {
+    for(unsigned int k=0; k< map.roadSegments.at(j).Lanes.size(); k ++)
+    {
+      for(unsigned int pindex=0; pindex< map.roadSegments.at(j).Lanes.at(k).points.size(); pindex ++)
+      {
+        d = distance2points(map.roadSegments.at(j).Lanes.at(k).points.at(pindex).pos, pos.pos);
+        a_diff = UtilityH::AngleBetweenTwoAnglesPositive(map.roadSegments.at(j).Lanes.at(k).points.at(pindex).pos.a, pos.pos.a);
+
+        if(d <= distance && a_diff <= M_PI_4)
+        {
+          bool bLaneExist = false;
+          for(unsigned int il = 0; il < lanesList.size(); il++)
+          {
+            if(lanesList.at(il)->id == map.roadSegments.at(j).Lanes.at(k).id)
+            {
+              bLaneExist = true;
+              break;
+            }
+          }
+
+          if(!bLaneExist)
+            lanesList.push_back(&map.roadSegments.at(j).Lanes.at(k));
+
+          break;
+        }
+      }
+    }
+  }
+
+  return lanesList;
+}
+
+WayPoint MappingHelpers::GetFirstWaypoint(RoadNetwork& map)
+{
+  for(unsigned int j=0; j< map.roadSegments.size(); j ++)
+  {
+    for(unsigned int k=0; k< map.roadSegments.at(j).Lanes.size(); k ++)
+    {
+      for(unsigned int pindex=0; pindex< map.roadSegments.at(j).Lanes.at(k).points.size(); pindex ++)
+      {
+        WayPoint fp =  map.roadSegments.at(j).Lanes.at(k).points.at(pindex);
+        return fp;
+      }
+    }
+  }
+
+  return WayPoint();
+}
+
+WayPoint* MappingHelpers::GetLastWaypoint(RoadNetwork& map)
+{
+  if(map.roadSegments.size() > 0 && map.roadSegments.at(map.roadSegments.size()-1).Lanes.size() > 0)
+  {
+    std::vector<Lane>* lanes = &map.roadSegments.at(map.roadSegments.size()-1).Lanes;
+    if(lanes->at(lanes->size()-1).points.size() > 0)
+      return &lanes->at(lanes->size()-1).points.at(lanes->at(lanes->size()-1).points.size()-1);
+  }
+
+  return nullptr;
+}
+
+void MappingHelpers::GetUniqueNextLanes(const Lane* l,  const vector<Lane*>& traversed_lanes, vector<Lane*>& lanes_list)
+{
+  if(!l) return;
+
+  for(unsigned int i=0; i< l->toLanes.size(); i++)
+  {
+    bool bFound = false;
+    for(unsigned int j = 0; j < traversed_lanes.size(); j++)
+    if(l->toLanes.at(i)->id == traversed_lanes.at(j)->id)
+    {
+      bFound = true;
+      break;
+    }
+
+    if(!bFound)
+      lanes_list.push_back(l->toLanes.at(i));
+  }
+}
+
+Lane* MappingHelpers::GetLaneFromPath(const WayPoint& currPos, const std::vector<WayPoint>& currPath)
+{
+  if(currPath.size() < 1) return nullptr;
+
+  int closest_index = PlanningHelpers::GetClosestNextPointIndexFast(currPath, currPos);
+
+  return currPath.at(closest_index).pLane;
+}
+
+std::vector<Curb> MappingHelpers::GetCurbsList(TiXmlElement* pElem)
+{
+  vector<Curb> cList;
+  TiXmlElement* pCurbs = GetDataFolder("CurbsLines", pElem);
+  if(!pCurbs)
+    return cList;
+
+  TiXmlElement* pE = pCurbs->FirstChildElement("Placemark");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml = pE->FirstChildElement("name");
+
+    if(pNameXml)
+    {
+      tfID = pNameXml->GetText();
+      Curb c;
+      c.id = GetIDsFromPrefix(tfID, "BID", "LnID").at(0);
+      c.laneId = GetIDsFromPrefix(tfID, "LnID", "RdID").at(0);
+      c.roadId = GetIDsFromPrefix(tfID, "RdID", "").at(0);
+
+      TiXmlElement* pPoints = pE->FirstChildElement("LineString")->FirstChildElement("coordinates");
+      c.points = GetPointsData(pPoints);
+
+      cList.push_back(c);
+    }
+  }
+
+  return cList;
+}
+
+std::vector<Boundary> MappingHelpers::GetBoundariesList(TiXmlElement* pElem)
+{
+  vector<Boundary> bList;
+  TiXmlElement* pBoundaries = GetDataFolder("Boundaries", pElem);
+  if(!pBoundaries)
+    return bList;
+
+  TiXmlElement* pE = pBoundaries->FirstChildElement("Placemark");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml = pE->FirstChildElement("name");
+
+    if(pNameXml)
+    {
+      tfID = pNameXml->GetText();
+      Boundary b;
+      b.id = GetIDsFromPrefix(tfID, "BID", "RdID").at(0);
+      b.roadId = GetIDsFromPrefix(tfID, "RdID", "").at(0);
+
+      TiXmlElement* pPoints = pE->FirstChildElement("LineString")->FirstChildElement("coordinates");
+      b.points = GetPointsData(pPoints);
+
+      bList.push_back(b);
+    }
+  }
+
+  return bList;
+}
+
+std::vector<Marking> MappingHelpers::GetMarkingsList(TiXmlElement* pElem)
+{
+  vector<Marking> mList;
+  TiXmlElement* pMarkings= GetDataFolder("Markings", pElem);
+  if(!pMarkings)
+    return mList;
+
+  TiXmlElement* pE = pMarkings->FirstChildElement("Placemark");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml =pE->FirstChildElement("name");
+
+    if(pNameXml)
+    {
+      tfID = pNameXml->GetText();
+      Marking m;
+      m.id = GetIDsFromPrefix(tfID, "MID", "LnID").at(0);
+      m.laneId = GetIDsFromPrefix(tfID, "LnID", "RdID").at(0);
+      m.roadId = GetIDsFromPrefix(tfID, "RdID", "").at(0);
+
+      TiXmlElement* pPoints = pE->FirstChildElement("LineString")->FirstChildElement("coordinates");
+
+      m.points = GetPointsData(pPoints);
+
+      if(m.points.size() > 0)
+      {
+        //first item is the center of the marking
+        m.center = m.points.at(0);
+        m.points.erase(m.points.begin()+0);
+      }
+
+      mList.push_back(m);
+    }
+  }
+
+  return mList;
+}
+
+std::vector<Crossing> MappingHelpers::GetCrossingsList(TiXmlElement* pElem)
+{
+  vector<Crossing> crossList;
+  TiXmlElement* pCrossings= GetDataFolder("Crossings", pElem);
+
+  if(!pCrossings)
+    return crossList;
+
+  TiXmlElement* pE = pCrossings->FirstChildElement("Placemark");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml =pE->FirstChildElement("name");
+
+    if(pNameXml)
+    {
+      tfID = pNameXml->GetText();
+      Crossing cross;
+      cross.id = GetIDsFromPrefix(tfID, "CRID", "RdID").at(0);
+      cross.roadId = GetIDsFromPrefix(tfID, "RdID", "").at(0);
+
+      TiXmlElement* pPoints = pE->FirstChildElement("LineString")->FirstChildElement("coordinates");
+      cross.points = GetPointsData(pPoints);
+
+      crossList.push_back(cross);
+    }
+  }
+
+  return crossList;
+}
+
+std::vector<TrafficSign> MappingHelpers::GetTrafficSignsList(TiXmlElement* pElem)
+{
+  vector<TrafficSign> tsList;
+  TiXmlElement* pSigns = GetDataFolder("TrafficSigns", pElem);
+
+  if(!pSigns)
+    return tsList;
+
+  TiXmlElement* pE = pSigns->FirstChildElement("Placemark");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml =pE->FirstChildElement("name");
+
+    if(pNameXml)
+    {
+      tfID = pNameXml->GetText();
+
+        TrafficSign ts;
+      ts.id = GetIDsFromPrefix(tfID, "TSID", "LnID").at(0);
+      ts.laneId = GetIDsFromPrefix(tfID, "LnID", "RdID").at(0);
+      ts.roadId = GetIDsFromPrefix(tfID, "RdID", "Type").at(0);
+      int iType = GetIDsFromPrefix(tfID, "Type", "").at(0);
+      switch(iType)
+      {
+      case 0:
+        ts.signType = UNKNOWN_SIGN;
+        break;
+      case 1:
+        ts.signType = STOP_SIGN;
+        break;
+      case 2:
+        ts.signType = MAX_SPEED_SIGN;
+        break;
+      case 3:
+        ts.signType = MIN_SPEED_SIGN;
+        break;
+      default:
+        ts.signType = STOP_SIGN;
+        break;
+      }
+
+
+      TiXmlElement* pPoints = pE->FirstChildElement("LineString")->FirstChildElement("coordinates");
+      ts.pos = GetPointsData(pPoints).at(0);
+
+      tsList.push_back(ts);
+    }
+  }
+
+  return tsList;
+}
+
+std::vector<StopLine> MappingHelpers::GetStopLinesList(TiXmlElement* pElem)
+{
+  vector<StopLine> slList;
+  TiXmlElement* pStopLines = GetDataFolder("StopLines", pElem);
+
+  if(!pStopLines)
+    return slList;
+
+  TiXmlElement* pE = pStopLines->FirstChildElement("Placemark");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml =pE->FirstChildElement("name");
+
+    if(pNameXml)
+    {
+      tfID = pNameXml->GetText();
+
+      StopLine sl;
+      sl.id = GetIDsFromPrefix(tfID, "SLID", "LnID").at(0);
+      sl.laneId = GetIDsFromPrefix(tfID, "LnID", "TSID").at(0);
+      sl.stopSignID = GetIDsFromPrefix(tfID, "TSID", "TLID").at(0);
+      sl.trafficLightID = GetIDsFromPrefix(tfID, "TLID", "").at(0);
+
+
+      TiXmlElement* pPoints = pE->FirstChildElement("LineString")->FirstChildElement("coordinates");
+      sl.points = GetPointsData(pPoints);
+
+      slList.push_back(sl);
+    }
+  }
+
+  return slList;
+}
+
+std::vector<TrafficLight> MappingHelpers::GetTrafficLightsList(TiXmlElement* pElem)
+{
+  vector<TrafficLight> tlList;
+  TiXmlElement* pLightsLines = GetDataFolder("TrafficLights", pElem);
+
+  if(!pLightsLines)
+    return tlList;
+
+  TiXmlElement* pE = pLightsLines->FirstChildElement("Placemark");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml =pE->FirstChildElement("name");
+
+    if(pNameXml)
+    {
+      tfID = pNameXml->GetText();
+
+      TrafficLight tl;
+      tl.id = GetIDsFromPrefix(tfID, "TLID", "LnID").at(0);
+      tl.laneIds = GetIDsFromPrefix(tfID, "LnID", "");
+
+      TiXmlElement* pPoints = pE->FirstChildElement("Point")->FirstChildElement("coordinates");
+      tl.pos = GetPointsData(pPoints).at(0);
+
+      tlList.push_back(tl);
+    }
+  }
+
+  return tlList;
+}
+
+vector<Lane> MappingHelpers::GetLanesList(TiXmlElement* pElem)
+{
+  vector<Lane> llList;
+  TiXmlElement* pLaneLinks = GetDataFolder("Lanes", pElem);
+
+  if(!pLaneLinks)
+    return llList;
+
+  TiXmlElement* pE = pLaneLinks->FirstChildElement("Folder");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml =pE->FirstChildElement("description");
+    if(pNameXml)
+    {
+      tfID = pNameXml->GetText();
+
+      Lane ll;
+      ll.id = GetIDsFromPrefix(tfID, "LID", "RSID").at(0);
+      ll.roadId = GetIDsFromPrefix(tfID, "RSID", "NUM").at(0);
+      ll.num = GetIDsFromPrefix(tfID, "NUM", "From").at(0);
+      ll.fromIds = GetIDsFromPrefix(tfID, "From", "To");
+      ll.toIds = GetIDsFromPrefix(tfID, "To", "Vel");
+      ll.speed = GetIDsFromPrefix(tfID, "Vel", "").at(0);
+  if(m_USING_VER_ZERO == 1)
+      ll.points = GetCenterLaneDataVer0(pE, ll.id);
+  else
+      ll.points = GetCenterLaneData(pE, ll.id);
+
+      llList.push_back(ll);
+    }
+  }
+
+  return llList;
+}
+
+vector<RoadSegment> MappingHelpers::GetRoadSegmentsList(TiXmlElement* pElem)
+{
+  vector<RoadSegment> rlList;
+  TiXmlElement* pRoadLinks = GetDataFolder("RoadSegments", pElem);
+
+  if(!pRoadLinks)
+    return rlList;
+
+  TiXmlElement* pE = pRoadLinks->FirstChildElement("Placemark");
+  for( ; pE; pE=pE->NextSiblingElement())
+  {
+    string tfID;
+    TiXmlElement* pNameXml =pE->FirstChildElement("description");
+    if(pNameXml)
+      tfID = pNameXml->GetText();
+
+    RoadSegment rl;
+    rl.id = GetIDsFromPrefix(tfID, "RSID", "").at(0);
+    rlList.push_back(rl);
+  }
+
+  return rlList;
+}
+
+std::vector<GPSPoint> MappingHelpers::GetPointsData(TiXmlElement* pElem)
+{
+  std::vector<GPSPoint> points;
+  if(pElem)
+  {
+    string coordinate_list;
+    if(!pElem->NoChildren())
+      coordinate_list = pElem->GetText();
+
+    istringstream str_stream(coordinate_list);
+    string token, temp;
+
+    while(getline(str_stream, token, ' '))
+    {
+      string lat, lon, alt;
+      double numLat=0, numLon=0, numAlt=0;
+
+      istringstream ss(token);
+
+      getline(ss, lat, ',');
+      getline(ss, lon, ',');
+      getline(ss, alt, ',');
+
+      numLat = atof(lat.c_str());
+      numLon = atof(lon.c_str());
+      numAlt = atof(alt.c_str());
+
+      GPSPoint p;
+
+      p.x = p.lat = numLat;
+      p.y = p.lon = numLon;
+      p.z = p.alt = numAlt;
+      points.push_back(p);
+    }
+  }
+
+  return points;
+}
+
+vector<WayPoint> MappingHelpers::GetCenterLaneData(TiXmlElement* pElem, const int& currLaneID)
+{
+  vector<WayPoint> gps_points;
+
+  TiXmlElement* pV = pElem->FirstChildElement("Placemark");
+
+  if(pV)
+   pV = pV->FirstChildElement("LineString");
+
+  if(pV)
+    pV = pV->FirstChildElement("coordinates");
+
+  if(pV)
+  {
+    string coordinate_list;
+    if(!pV->NoChildren())
+      coordinate_list = pV->GetText();
+
+    istringstream str_stream(coordinate_list);
+    string token, temp;
+
+
+    while(getline(str_stream, token, ' '))
+    {
+      string lat, lon, alt;
+      double numLat=0, numLon=0, numAlt=0;
+
+      istringstream ss(token);
+
+      getline(ss, lat, ',');
+      getline(ss, lon, ',');
+      getline(ss, alt, ',');
+
+      numLat = atof(lat.c_str());
+      numLon = atof(lon.c_str());
+      numAlt = atof(alt.c_str());
+
+      WayPoint wp;
+
+      wp.pos.x = wp.pos.lat = numLat;
+      wp.pos.y = wp.pos.lon = numLon;
+      wp.pos.z = wp.pos.alt = numAlt;
+
+      wp.laneId = currLaneID;
+      gps_points.push_back(wp);
+    }
+
+    TiXmlElement* pInfoEl =pElem->FirstChildElement("Folder")->FirstChildElement("description");
+    string additional_info;
+    if(pInfoEl)
+      additional_info = pInfoEl->GetText();
+    additional_info.insert(additional_info.begin(), ',');
+    additional_info.erase(additional_info.end()-1);
+    vector<string> add_info_list = SplitString(additional_info, ",");
+    if(gps_points.size() == add_info_list.size())
+    {
+      for(unsigned int i=0; i< gps_points.size(); i++)
+      {
+        gps_points.at(i).id =  GetIDsFromPrefix(add_info_list.at(i), "WPID", "AC").at(0);
+        gps_points.at(i).actionCost.push_back(GetActionPairFromPrefix(add_info_list.at(i), "AC", "From"));
+        gps_points.at(i).fromIds =  GetIDsFromPrefix(add_info_list.at(i), "From", "To");
+        gps_points.at(i).toIds =  GetIDsFromPrefix(add_info_list.at(i), "To", "Lid");
+
+        vector<int> ids = GetIDsFromPrefix(add_info_list.at(i), "Lid", "Rid");
+        if(ids.size() > 0)
+          gps_points.at(i).LeftPointId =  ids.at(0);
+
+        ids = GetIDsFromPrefix(add_info_list.at(i), "Rid", "Vel");
+        if(ids.size() > 0)
+          gps_points.at(i).RightPointId =  ids.at(0);
+
+        vector<double> dnums = GetDoubleFromPrefix(add_info_list.at(i), "Vel", "Dir");
+        if(dnums.size() > 0)
+          gps_points.at(i).v =  dnums.at(0);
+
+        dnums = GetDoubleFromPrefix(add_info_list.at(i), "Dir", "");
+        if(dnums.size() > 0)
+          gps_points.at(i).pos.a = gps_points.at(i).pos.dir =  dnums.at(0);
+      }
+    }
+  }
+
+  return gps_points;
+}
+
+vector<WayPoint> MappingHelpers::GetCenterLaneDataVer0(TiXmlElement* pElem, const int& currLaneID)
+{
+  vector<WayPoint> gps_points;
+
+  TiXmlElement* pV = pElem->FirstChildElement("Placemark");
+
+  if(pV)
+   pV = pV->FirstChildElement("LineString");
+
+  if(pV)
+    pV = pV->FirstChildElement("coordinates");
+
+  if(pV)
+  {
+    string coordinate_list;
+    if(!pV->NoChildren())
+      coordinate_list = pV->GetText();
+
+    istringstream str_stream(coordinate_list);
+    string token, temp;
+
+
+    while(getline(str_stream, token, ' '))
+    {
+      string lat, lon, alt;
+      double numLat=0, numLon=0, numAlt=0;
+
+      istringstream ss(token);
+
+      getline(ss, lat, ',');
+      getline(ss, lon, ',');
+      getline(ss, alt, ',');
+
+      numLat = atof(lat.c_str());
+      numLon = atof(lon.c_str());
+      numAlt = atof(alt.c_str());
+
+      WayPoint wp;
+
+      wp.pos.x = wp.pos.lat = numLat;
+      wp.pos.y = wp.pos.lon = numLon;
+      wp.pos.z = wp.pos.alt = numAlt;
+
+      wp.laneId = currLaneID;
+      gps_points.push_back(wp);
+    }
+
+    TiXmlElement* pInfoEl =pElem->FirstChildElement("Folder")->FirstChildElement("description");
+    string additional_info;
+    if(pInfoEl)
+      additional_info = pInfoEl->GetText();
+    additional_info.insert(additional_info.begin(), ',');
+    additional_info.erase(additional_info.end()-1);
+    vector<string> add_info_list = SplitString(additional_info, ",");
+    if(gps_points.size() == add_info_list.size())
+    {
+      for(unsigned int i=0; i< gps_points.size(); i++)
+      {
+        gps_points.at(i).id =  GetIDsFromPrefix(add_info_list.at(i), "WPID", "C").at(0);
+        gps_points.at(i).fromIds =  GetIDsFromPrefix(add_info_list.at(i), "From", "To");
+        gps_points.at(i).toIds =  GetIDsFromPrefix(add_info_list.at(i), "To", "Vel");
+        gps_points.at(i).v =  GetDoubleFromPrefix(add_info_list.at(i), "Vel", "Dir").at(0);
+        gps_points.at(i).pos.a = gps_points.at(i).pos.dir =  GetDoubleFromPrefix(add_info_list.at(i), "Dir", "").at(0);
+//        if(currLaneID == 11115)
+//        {
+//
+//          pair<ACTION_TYPE, double> act_cost;
+//            act_cost.first = FORWARD_ACTION;
+//            act_cost.second = 100;
+//          gps_points.at(i).actionCost.push_back(act_cost);
+//        }
+      }
+    }
+  }
+
+  return gps_points;
+}
+
+vector<int> MappingHelpers::GetIDsFromPrefix(const string& str, const string& prefix, const string& postfix)
+{
+  int index1 = str.find(prefix)+prefix.size();
+  int index2 = str.find(postfix, index1);
+  if(index2 < 0  || postfix.size() ==0)
+    index2 = str.size();
+
+  string str_ids = str.substr(index1, index2-index1);
+
+  vector<int> ids;
+  vector<string> idstr = SplitString(str_ids, "_");
+
+  for(unsigned  int i=0; i< idstr.size(); i++ )
+  {
+    if(idstr.at(i).size()>0)
+    {
+      int num = atoi(idstr.at(i).c_str());
+      //if(num>-1)
+        ids.push_back(num);
+    }
+  }
+
+  return ids;
+}
+
+vector<double> MappingHelpers::GetDoubleFromPrefix(const string& str, const string& prefix, const string& postfix)
+{
+  int index1 = str.find(prefix)+prefix.size();
+  int index2 = str.find(postfix, index1);
+  if(index2 < 0  || postfix.size() ==0)
+    index2 = str.size();
+
+  string str_ids = str.substr(index1, index2-index1);
+
+  vector<double> ids;
+  vector<string> idstr = SplitString(str_ids, "_");
+
+  for(unsigned  int i=0; i< idstr.size(); i++ )
+  {
+    if(idstr.at(i).size()>0)
+    {
+      double num = atof(idstr.at(i).c_str());
+      //if(num>-1)
+        ids.push_back(num);
+    }
+  }
+
+  return ids;
+}
+
+pair<ACTION_TYPE, double> MappingHelpers::GetActionPairFromPrefix(const string& str, const string& prefix, const string& postfix)
+{
+  int index1 = str.find(prefix)+prefix.size();
+  int index2 = str.find(postfix, index1);
+  if(index2<0  || postfix.size() ==0)
+    index2 = str.size();
+
+  string str_ids = str.substr(index1, index2-index1);
+
+  pair<ACTION_TYPE, double> act_cost;
+  act_cost.first = FORWARD_ACTION;
+  act_cost.second = 0;
+
+  vector<string> idstr = SplitString(str_ids, "_");
+
+  if(idstr.size() >= 2)
+  {
+    if(idstr.at(0).size() > 0 && idstr.at(0).at(0) == 'L')
+      act_cost.first = LEFT_TURN_ACTION;
+    else if(idstr.at(0).size() > 0 && idstr.at(0).at(0) == 'R')
+      act_cost.first = RIGHT_TURN_ACTION;
+    if(idstr.at(1).size() > 0)
+      act_cost.second = atof(idstr.at(1).c_str());
+  }
+
+  return act_cost;
+}
+
+vector<string> MappingHelpers::SplitString(const string& str, const string& token)
+{
+  vector<string> str_parts;
+  int iFirstPart = str.find(token);
+
+  while(iFirstPart >= 0)
+  {
+    iFirstPart++;
+    int iSecondPart = str.find(token, iFirstPart);
+    if(iSecondPart>0)
+    {
+      str_parts.push_back(str.substr(iFirstPart,iSecondPart - iFirstPart));
+    }
+    else
+    {
+      str_parts.push_back(str.substr(iFirstPart,str.size() - iFirstPart));
+    }
+
+    iFirstPart = iSecondPart;
+  }
+
+  return str_parts;
+}
+
+void MappingHelpers::FindAdjacentLanes(RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    //Link Lanes
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      //Link left and right lanes
+      for(unsigned int rs_2 = 0; rs_2 < map.roadSegments.size(); rs_2++)
+      {
+        for(unsigned int i2 =0; i2 < map.roadSegments.at(rs_2).Lanes.size(); i2++)
+        {
+          int iCenter1 = pL->points.size()/2;
+          WayPoint wp_1 = pL->points.at(iCenter1);
+          int iCenter2 = PlanningHelpers::GetClosestNextPointIndexFast(map.roadSegments.at(rs_2).Lanes.at(i2).points, wp_1 );
+          WayPoint closest_p = map.roadSegments.at(rs_2).Lanes.at(i2).points.at(iCenter2);
+          double mid_a1 = wp_1.pos.a;
+          double mid_a2 = closest_p.pos.a;
+          double angle_diff = UtilityH::AngleBetweenTwoAnglesPositive(mid_a1, mid_a2);
+          double distance = distance2points(wp_1.pos, closest_p.pos);
+
+          if(pL->id != map.roadSegments.at(rs_2).Lanes.at(i2).id && angle_diff < 0.05 && distance < 3.5 && distance > 2.5)
+          {
+            double perp_distance = DBL_MAX;
+            if(pL->points.size() > 2 && map.roadSegments.at(rs_2).Lanes.at(i2).points.size()>2)
+            {
+              RelativeInfo info;
+              PlanningHelpers::GetRelativeInfo(pL->points, closest_p, info);
+              perp_distance = info.perp_distance;
+              //perp_distance = PlanningHelpers::GetPerpDistanceToVectorSimple(pL->points.at(iCenter1-1), pL->points.at(iCenter1+1), closest_p);
+            }
+
+            if(perp_distance > 1.0 && perp_distance < 10.0)
+            {
+              pL->pRightLane = &map.roadSegments.at(rs_2).Lanes.at(i2);
+              for(unsigned int i_internal = 0; i_internal< pL->points.size(); i_internal++)
+              {
+                if(i_internal<map.roadSegments.at(rs_2).Lanes.at(i2).points.size())
+                {
+                  pL->points.at(i_internal).RightPointId = map.roadSegments.at(rs_2).Lanes.at(i2).id;
+                  pL->points.at(i_internal).pRight = &map.roadSegments.at(rs_2).Lanes.at(i2).points.at(i_internal);
+//                  map.roadSegments.at(rs_2).Lanes.at(i2).points.at(i_internal).pLeft = &pL->points.at(i_internal);
+                }
+              }
+            }
+            else if(perp_distance < -1.0 && perp_distance > -10.0)
+            {
+              pL->pLeftLane = &map.roadSegments.at(rs_2).Lanes.at(i2);
+              for(unsigned int i_internal = 0; i_internal< pL->points.size(); i_internal++)
+              {
+                if(i_internal<map.roadSegments.at(rs_2).Lanes.at(i2).points.size())
+                {
+                  pL->points.at(i_internal).LeftPointId = map.roadSegments.at(rs_2).Lanes.at(i2).id;
+                  pL->points.at(i_internal).pLeft = &map.roadSegments.at(rs_2).Lanes.at(i2).points.at(i_internal);
+//                  map.roadSegments.at(rs_2).Lanes.at(i2).points.at(i_internal).pRight = &pL->points.at(i_internal);
+                }
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
+void MappingHelpers::ExtractSignalData(const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
+      const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+      const GPSPoint& origin, RoadNetwork& map)
+{
+  for(unsigned int is=0; is< signal_data.size(); is++)
+  {
+    if(signal_data.at(is).Type == 2)
+    {
+      TrafficLight tl;
+      tl.id = signal_data.at(is).ID;
+      tl.linkID = signal_data.at(is).LinkID;
+      tl.stoppingDistance = 0;
+
+      for(unsigned int iv = 0; iv < vector_data.size(); iv++)
+      {
+        if(signal_data.at(is).VID == vector_data.at(iv).VID)
+        {
+          for(unsigned int ip = 0; ip < points_data.size(); ip++)
+          {
+            if(vector_data.at(iv).PID == points_data.at(ip).PID)
+            {
+              tl.pos = GPSPoint(points_data.at(ip).Ly + origin.x, points_data.at(ip).Bx + origin.y, points_data.at(ip).H + origin.z, vector_data.at(iv).Hang*DEG2RAD);
+              break;
+            }
+          }
+        }
+      }
+      map.trafficLights.push_back(tl);
+      if(tl.id > g_max_traffic_light_id)
+        g_max_traffic_light_id = tl.id;
+    }
+  }
+}
+
+void MappingHelpers::ExtractStopLinesData(const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
+      const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+      const GPSPoint& origin, RoadNetwork& map)
+{
+  for(unsigned int ist=0; ist < stop_line_data.size(); ist++)
+    {
+    StopLine sl;
+    sl.linkID = stop_line_data.at(ist).LinkID;
+    sl.id = stop_line_data.at(ist).ID;
+    if(stop_line_data.at(ist).TLID>0)
+      sl.trafficLightID = stop_line_data.at(ist).TLID;
+    else
+      sl.stopSignID = 100+ist;
+
+    for(unsigned int il=0; il < line_data.size(); il++)
+    {
+      if(stop_line_data.at(ist).LID == line_data.at(il).LID)
+      {
+        int s_id = line_data.at(il).BPID;
+        int e_id = line_data.at(il).FPID;
+        for(unsigned int ip = 0; ip < points_data.size(); ip++)
+        {
+          if(points_data.at(ip).PID == s_id || points_data.at(ip).PID == e_id)
+          {
+            sl.points.push_back(GPSPoint(points_data.at(ip).Ly + origin.x, points_data.at(ip).Bx + origin.y, points_data.at(ip).H + origin.z, 0));
+          }
+        }
+      }
+    }
+    map.stopLines.push_back(sl);
+    if(sl.id > g_max_stop_line_id)
+      g_max_stop_line_id = sl.id;
+  }
+}
+
+void MappingHelpers::ExtractCurbData(const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
+        const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+        const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+        const GPSPoint& origin, RoadNetwork& map)
+{
+  for(unsigned int ic=0; ic < curb_data.size(); ic++)
+    {
+      Curb c;
+      c.id = curb_data.at(ic).ID;
+
+      for(unsigned int il=0; il < line_data.size(); il++)
+      {
+        if(curb_data.at(ic).LID == line_data.at(il).LID)
+        {
+          int s_id = line_data.at(il).BPID;
+          //int e_id = line_data.at(il).FPID;
+          for(unsigned int ip = 0; ip < points_data.size(); ip++)
+          {
+            if(points_data.at(ip).PID == s_id)
+            {
+              c.points.push_back(GPSPoint(points_data.at(ip).Ly + origin.x, points_data.at(ip).Bx + origin.y, points_data.at(ip).H + origin.z, 0));
+              WayPoint wp;
+              wp.pos = c.points.at(0);
+              Lane* pLane = GetClosestLaneFromMap(wp, map, 5);
+              if(pLane)
+              {
+                c.laneId = pLane->id;
+                c.pLane = pLane;
+              }
+            }
+          }
+        }
+      }
+      map.curbs.push_back(c);
+    }
+}
+
+void MappingHelpers::ExtractWayArea(const std::vector<UtilityHNS::AisanAreasFileReader::AisanArea>& area_data,
+    const std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea>& wayarea_data,
+      const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+      const GPSPoint& origin, RoadNetwork& map)
+{
+  for(unsigned int iw=0; iw < wayarea_data.size(); iw ++)
+  {
+    Boundary bound;
+    bound.id = wayarea_data.at(iw).ID;
+
+    for(unsigned int ia=0; ia < area_data.size(); ia ++)
+    {
+      if(wayarea_data.at(iw).AID == area_data.at(ia).AID)
+      {
+        int s_id = area_data.at(ia).SLID;
+        int e_id = area_data.at(ia).ELID;
+
+        for(unsigned int il=0; il< line_data.size(); il++)
+        {
+          if(line_data.at(il).LID >= s_id && line_data.at(il).LID <= e_id)
+          {
+            for(unsigned int ip=0; ip < points_data.size(); ip++)
+            {
+              if(points_data.at(ip).PID == line_data.at(il).BPID)
+              {
+                GPSPoint p(points_data.at(ip).Ly + origin.x, points_data.at(ip).Bx + origin.y, points_data.at(ip).H + origin.z, 0);
+                bound.points.push_back(p);
+              }
+            }
+          }
+        }
+      }
+    }
+
+    map.boundaries.push_back(bound);
+  }
+}
+
+void MappingHelpers::LinkMissingBranchingWayPoints(RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int p= 0; p < map.roadSegments.at(rs).Lanes.at(i).points.size(); p++)
+      {
+        WayPoint* pWP = &map.roadSegments.at(rs).Lanes.at(i).points.at(p);
+        for(unsigned int j = 0 ; j < pWP->toIds.size(); j++)
+        {
+          pWP->pFronts.push_back(FindWaypoint(pWP->toIds.at(j), map));
+        }
+      }
+    }
+  }
+}
+
+void MappingHelpers::LinkMissingBranchingWayPointsV2(RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pLane = &map.roadSegments.at(rs).Lanes.at(i);
+      for(unsigned int p= 0; p < pLane->points.size(); p++)
+      {
+        WayPoint* pWP = &pLane->points.at(p);
+
+        if(p+1 == pLane->points.size()) // Last Point in Lane
+        {
+          for(unsigned int j = 0 ; j < pLane->toLanes.size(); j++)
+          {
+            //cout << "Link, Next Lane: " << pWP->laneId << ", WP: " << pWP->id << " To WP: " << pWP->toIds.at(j) << endl;
+            pWP->pFronts.push_back(&pLane->toLanes.at(j)->points.at(0));
+          }
+        }
+        else
+        {
+          if(pWP->toIds.size() > 1)
+          {
+            cout << "Error Error Erro ! Lane: " << pWP->laneId << ", Point: " << pWP->originalMapID << endl;
+          }
+          else
+          {
+          //  cout << "Link, Same Lane: " << pWP->laneId << ", WP: " << pWP->id << " To WP: " << map.roadSegments.at(rs).Lanes.at(i).points.at(p+1).id << endl;
+            pWP->pFronts.push_back(&pLane->points.at(p+1));
+          }
+        }
+      }
+    }
+  }
+}
+
+void MappingHelpers::LinkTrafficLightsAndStopLines(RoadNetwork& map)
+{
+
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int p= 0; p < map.roadSegments.at(rs).Lanes.at(i).points.size(); p++)
+      {
+        for(unsigned int isl = 0; isl < map.stopLines.size(); isl++)
+        {
+          WayPoint* pWP = &map.roadSegments.at(rs).Lanes.at(i).points.at(p);
+
+          if(map.stopLines.at(isl).linkID == pWP->id)
+          {
+            map.stopLines.at(isl).laneId = pWP->laneId;
+            map.stopLines.at(isl).pLane = pWP->pLane;
+            map.roadSegments.at(rs).Lanes.at(i).stopLines.push_back(map.stopLines.at(isl));
+
+            pWP->stopLineID = map.stopLines.at(isl).id;
+
+            for(unsigned int itl = 0; itl < map.trafficLights.size(); itl++)
+            {
+              if(map.trafficLights.at(itl).id == map.stopLines.at(isl).trafficLightID)
+              {
+                map.trafficLights.at(itl).laneIds.push_back(pWP->laneId);
+                map.trafficLights.at(itl).pLanes.push_back(pWP->pLane);
+              }
+            }
+            break;
+          }
+        }
+      }
+    }
+  }
+
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int itl = 0; itl < map.trafficLights.size(); itl++)
+      {
+        for(unsigned int p= 0; p < map.roadSegments.at(rs).Lanes.at(i).points.size(); p++)
+        {
+          WayPoint* pWP = &map.roadSegments.at(rs).Lanes.at(i).points.at(p);
+          if(map.trafficLights.at(itl).linkID == pWP->id)
+          {
+            map.roadSegments.at(rs).Lanes.at(i).trafficlights.push_back(map.trafficLights.at(itl));
+            break;
+          }
+        }
+      }
+    }
+  }
+}
+
+void MappingHelpers::LinkTrafficLightsAndStopLinesConData(const std::vector<UtilityHNS::AisanDataConnFileReader::DataConn>& conn_data,
+    const std::vector<std::pair<int,int> >& id_replace_list, RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+
+      for(unsigned int ic = 0; ic < conn_data.size(); ic++)
+      {
+        UtilityHNS::AisanDataConnFileReader::DataConn data_conn = conn_data.at(ic);
+        ReplaceMyID(data_conn.LID , id_replace_list);
+
+        if(map.roadSegments.at(rs).Lanes.at(i).id == data_conn.LID)
+        {
+          for(unsigned int itl = 0; itl < map.trafficLights.size(); itl++)
+          {
+            if(map.trafficLights.at(itl).id == data_conn.SID)
+            {
+              map.trafficLights.at(itl).laneIds.push_back(map.roadSegments.at(rs).Lanes.at(i).id);
+              map.trafficLights.at(itl).pLanes.push_back(&map.roadSegments.at(rs).Lanes.at(i));
+              map.roadSegments.at(rs).Lanes.at(i).trafficlights.push_back(map.trafficLights.at(itl));
+            }
+          }
+
+          for(unsigned int isl = 0; isl < map.stopLines.size(); isl++)
+          {
+            if(map.stopLines.at(isl).id == data_conn.SLID)
+            {
+              map.stopLines.at(isl).laneId = map.roadSegments.at(rs).Lanes.at(i).id;
+              map.stopLines.at(isl).pLane = &map.roadSegments.at(rs).Lanes.at(i);
+              map.stopLines.at(isl).trafficLightID = data_conn.SID;
+              map.stopLines.at(isl).stopSignID = data_conn.SSID;
+              map.roadSegments.at(rs).Lanes.at(i).stopLines.push_back(map.stopLines.at(isl));
+              WayPoint wp((map.stopLines.at(isl).points.at(0).x+map.stopLines.at(isl).points.at(1).x)/2.0, (map.stopLines.at(isl).points.at(0).y+map.stopLines.at(isl).points.at(1).y)/2.0, (map.stopLines.at(isl).points.at(0).z+map.stopLines.at(isl).points.at(1).z)/2.0, (map.stopLines.at(isl).points.at(0).a+map.stopLines.at(isl).points.at(1).a)/2.0);
+              map.roadSegments.at(rs).Lanes.at(i).points.at(PlanningHelpers::GetClosestNextPointIndexFast(map.roadSegments.at(rs).Lanes.at(i).points, wp)).stopLineID = map.stopLines.at(isl).id;
+            }
+          }
+        }
+      }
+
+    }
+  }
+}
+
+void MappingHelpers::UpdateMapWithOccupancyGrid(OccupancyToGridMap& map_info, const std::vector<int>& data, RoadNetwork& map, std::vector<WayPoint*>& updated_list)
+{
+  PlannerHNS::Mat3 rotationMat(- map_info.center.pos.a);
+  PlannerHNS::Mat3 translationMat(-map_info.center.pos.x, -map_info.center.pos.y);
+  updated_list.clear();
+
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int p= 0; p < map.roadSegments.at(rs).Lanes.at(i).points.size(); p++)
+      {
+        WayPoint* pWP = &map.roadSegments.at(rs).Lanes.at(i).points.at(p);
+
+        GPSPoint relative_point = pWP->pos;
+        relative_point = translationMat * relative_point;
+        relative_point = rotationMat *relative_point;
+
+        int cell_value = 0;
+        if(map_info.GetCellIndexFromPoint(relative_point, data, cell_value) == true)
+        {
+          if(cell_value == 0)
+          {
+            bool bFound = false;
+            for(unsigned int i_action=0; i_action < pWP->actionCost.size(); i_action++)
+            {
+              if(pWP->actionCost.at(i_action).first == FORWARD_ACTION)
+              {
+                pWP->actionCost.at(i_action).second = 100;
+                bFound = true;
+              }
+            }
+
+            if(!bFound)
+              pWP->actionCost.push_back(make_pair(FORWARD_ACTION, 100));
+
+            updated_list.push_back(pWP);
+          }
+        }
+      }
+    }
+  }
+}
+
+void MappingHelpers::ConstructRoadNetworkFromROSMessageV2(const std::vector<UtilityHNS::AisanLanesFileReader::AisanLane>& lanes_data,
+    const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+    const std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine>& dt_data,
+    const std::vector<UtilityHNS::AisanIntersectionFileReader::AisanIntersection>& intersection_data,
+    const std::vector<UtilityHNS::AisanAreasFileReader::AisanArea>& area_data,
+    const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+    const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
+    const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
+    const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
+    const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
+    const std::vector<UtilityHNS::AisanRoadEdgeFileReader::AisanRoadEdge>& roadedge_data,
+    const std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea>& wayarea_data,
+    const std::vector<UtilityHNS::AisanCrossWalkFileReader::AisanCrossWalk>& crosswalk_data,
+    const std::vector<UtilityHNS::AisanNodesFileReader::AisanNode>& nodes_data,
+    const std::vector<UtilityHNS::AisanDataConnFileReader::DataConn>& conn_data,
+    UtilityHNS::AisanLanesFileReader* pLaneData,
+    UtilityHNS::AisanPointsFileReader* pPointsData,
+    UtilityHNS::AisanNodesFileReader* pNodesData,
+    UtilityHNS::AisanLinesFileReader* pLinedata,
+    const GPSPoint& origin, RoadNetwork& map, const bool& bSpecialFlag,
+    const bool& bFindLaneChangeLanes, const bool& bFindCurbsAndWayArea)
+{
+  vector<Lane> roadLanes;
+
+  for(unsigned int i=0; i< pLaneData->m_data_list.size(); i++)
+  {
+    if(pLaneData->m_data_list.at(i).LnID > g_max_lane_id)
+      g_max_lane_id = pLaneData->m_data_list.at(i).LnID;
+  }
+
+  cout << " >> Extracting Lanes ... " << endl;
+  CreateLanes(pLaneData, pPointsData, pNodesData, roadLanes);
+
+  cout << " >> Fix Waypoints errors ... " << endl;
+  FixTwoPointsLanes(roadLanes);
+  FixRedundantPointsLanes(roadLanes);
+
+  ConnectLanes(pLaneData, roadLanes);
+
+  cout << " >> Create Missing lane connections ... " << endl;
+  FixUnconnectedLanes(roadLanes);
+  ////FixTwoPointsLanes(roadLanes);
+
+  //map has one road segment
+  RoadSegment roadSegment1;
+  roadSegment1.id = 1;
+  roadSegment1.Lanes = roadLanes;
+  map.roadSegments.push_back(roadSegment1);
+
+  //Fix angle for lanes
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      PlannerHNS::PlanningHelpers::FixAngleOnly(pL->points);
+    }
+  }
+
+  //Link Lanes and lane's waypoints by pointers
+  cout << " >> Link lanes and waypoints with pointers ... " << endl;
+  LinkLanesPointers(map);
+
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      for(unsigned int j = 0 ; j < pL->points.size(); j++)
+      {
+          if(pL->points.at(j).actionCost.size() > 0)
+        {
+          if(pL->points.at(j).actionCost.at(0).first == LEFT_TURN_ACTION)
+          {
+            AssignActionCostToLane(pL, LEFT_TURN_ACTION, LEFT_INITIAL_TURNS_COST);
+            break;
+          }
+          else if(pL->points.at(j).actionCost.at(0).first == RIGHT_TURN_ACTION)
+          {
+            AssignActionCostToLane(pL, RIGHT_TURN_ACTION, RIGHT_INITIAL_TURNS_COST);
+          break;
+
+          }
+          }
+      }
+    }
+  }
+
+  if(bFindLaneChangeLanes)
+  {
+    cout << " >> Extract Lane Change Information... " << endl;
+    FindAdjacentLanesV2(map);
+  }
+
+  //Extract Signals and StopLines
+  cout << " >> Extract Signal data ... " << endl;
+  ExtractSignalDataV2(signal_data, vector_data, pPointsData, origin, map);
+
+  //Stop Lines
+  cout << " >> Extract Stop lines data ... " << endl;
+  ExtractStopLinesDataV2(stop_line_data, pLinedata, pPointsData, origin, map);
+
+  if(bFindCurbsAndWayArea)
+  {
+    //Curbs
+    cout << " >> Extract curbs data ... " << endl;
+    ExtractCurbDataV2(curb_data, pLinedata, pPointsData, origin, map);
+
+    //Wayarea
+    cout << " >> Extract wayarea data ... " << endl;
+    ExtractWayArea(area_data, wayarea_data, line_data, points_data, origin, map);
+  }
+
+  //Link waypoints
+  cout << " >> Link missing branches and waypoints... " << endl;
+  LinkMissingBranchingWayPointsV2(map);
+
+  //Link StopLines and Traffic Lights
+  cout << " >> Link StopLines and Traffic Lights ... " << endl;
+  LinkTrafficLightsAndStopLinesV2(map);
+//  //LinkTrafficLightsAndStopLinesConData(conn_data, id_replace_list, map);
+
+  cout << " >> Map loaded from data with " << roadLanes.size()  << " lanes" << endl;
+}
+
+bool MappingHelpers::GetPointFromDataList(UtilityHNS::AisanPointsFileReader* pPointsData,const int& pid, WayPoint& out_wp)
+{
+  if(pPointsData == nullptr) return false;
+
+  AisanPointsFileReader::AisanPoints* pP =  pPointsData->GetDataRowById(pid);
+
+  if(pP!=nullptr)
+  {
+    out_wp.id = pP->PID;
+    out_wp.pos.x = pP->Ly;
+    out_wp.pos.y = pP->Bx;
+    out_wp.pos.z = pP->H;
+    return true;
+  }
+
+  return false;
+}
+
+int MappingHelpers::GetBeginPointIdFromLaneNo(UtilityHNS::AisanLanesFileReader* pLaneData,
+    UtilityHNS::AisanPointsFileReader* pPointsData,
+    UtilityHNS::AisanNodesFileReader* pNodesData,const int& LnID)
+{
+  if(pLaneData == nullptr) return false;
+  UtilityHNS::AisanLanesFileReader::AisanLane* pL = nullptr;
+  UtilityHNS::AisanPointsFileReader::AisanPoints* pP = nullptr;
+  UtilityHNS::AisanNodesFileReader::AisanNode* pN = nullptr;
+
+  pL = pLaneData->GetDataRowById(LnID);
+  if(pL!=nullptr)
+  {
+    return pL->FNID;
+  }
+
+  return 0;
+}
+
+int MappingHelpers::GetEndPointIdFromLaneNo(UtilityHNS::AisanLanesFileReader* pLaneData,
+    UtilityHNS::AisanPointsFileReader* pPointsData,
+    UtilityHNS::AisanNodesFileReader* pNodesData,const int& LnID)
+{
+  UtilityHNS::AisanLanesFileReader::AisanLane* pL = nullptr;
+  UtilityHNS::AisanPointsFileReader::AisanPoints* pP = nullptr;
+  UtilityHNS::AisanNodesFileReader::AisanNode* pN = nullptr;
+
+  pL = pLaneData->GetDataRowById(LnID);
+  if(pL!=nullptr)
+  {
+    return pL->BNID;
+  }
+
+  return 0;
+}
+
+bool MappingHelpers::IsStartLanePoint(UtilityHNS::AisanLanesFileReader* pLaneData, UtilityHNS::AisanLanesFileReader::AisanLane* pL)
+{
+  if(pL->JCT > 0 || pL->BLID == 0)
+    return true;
+
+  if(pL->BLID2 != 0 || pL->BLID3 != 0 || pL->BLID4 != 0)
+    return true;
+
+  UtilityHNS::AisanLanesFileReader::AisanLane* pPrevL = pLaneData->GetDataRowById(pL->BLID);
+  if(pPrevL == nullptr || pPrevL->FLID2 > 0 || pPrevL->FLID3 > 0 || pPrevL->FLID4 > 0 || pPrevL->JCT > 0)
+    return true;
+
+  return false;
+}
+
+bool MappingHelpers::IsEndLanePoint(UtilityHNS::AisanLanesFileReader* pLaneData, UtilityHNS::AisanLanesFileReader::AisanLane* pL)
+{
+  if(pL->FLID2 > 0 || pL->FLID3 > 0 || pL->FLID4 > 0)
+    return true;
+
+  UtilityHNS::AisanLanesFileReader::AisanLane* pNextL = pLaneData->GetDataRowById(pL->FLID);
+
+  return IsStartLanePoint(pLaneData, pNextL);
+}
+
+void MappingHelpers::GetLanesStartPoints(UtilityHNS::AisanLanesFileReader* pLaneData,
+        std::vector<int>& m_LanesStartIds)
+{
+  m_LanesStartIds.clear();
+  UtilityHNS::AisanLanesFileReader::AisanLane* pL = nullptr;
+  UtilityHNS::AisanLanesFileReader::AisanLane* pPrevL = nullptr;
+  for(unsigned int il=0; il < pLaneData->m_data_list.size(); il++)
+  {
+    pL = &pLaneData->m_data_list.at(il);
+
+    if(IsStartLanePoint(pLaneData, pL) == true)
+    {
+      m_LanesStartIds.push_back(pL->LnID);
+    }
+
+    if(DEBUG_MAP_PARSING)
+    {
+      if(IsStartLanePoint(pLaneData, pL) && IsEndLanePoint(pLaneData, pL))
+        cout << " :( :( :( Start And End in the same time !! " << pL->LnID << endl;
+    }
+  }
+}
+
+void MappingHelpers::ConnectLanes(UtilityHNS::AisanLanesFileReader* pLaneData, std::vector<PlannerHNS::Lane>& lanes)
+{
+  for(unsigned int il = 0; il < lanes.size(); il++)
+  {
+    WayPoint fp = lanes.at(il).points.at(0);
+    UtilityHNS::AisanLanesFileReader::AisanLane* pFirstL = pLaneData->GetDataRowById(fp.originalMapID);
+    if(pFirstL!=nullptr)
+    {
+      if(pFirstL->BLID > 0)
+        lanes.at(il).fromIds.push_back(pFirstL->BLID);
+      if(pFirstL->BLID2 > 0)
+        lanes.at(il).fromIds.push_back(pFirstL->BLID2);
+      if(pFirstL->BLID3 > 0)
+        lanes.at(il).fromIds.push_back(pFirstL->BLID3);
+      if(pFirstL->BLID4 > 0)
+        lanes.at(il).fromIds.push_back(pFirstL->BLID4);
+    }
+
+    WayPoint ep = lanes.at(il).points.at(lanes.at(il).points.size()-1);
+    UtilityHNS::AisanLanesFileReader::AisanLane* pEndL = pLaneData->GetDataRowById(ep.originalMapID);
+    if(pEndL!=nullptr)
+    {
+      if(pEndL->FLID > 0)
+        lanes.at(il).toIds.push_back(pEndL->FLID);
+      if(pEndL->FLID2 > 0)
+        lanes.at(il).toIds.push_back(pEndL->FLID2);
+      if(pEndL->FLID3 > 0)
+        lanes.at(il).toIds.push_back(pEndL->FLID3);
+      if(pEndL->FLID4 > 0)
+        lanes.at(il).toIds.push_back(pEndL->FLID4);
+    }
+  }
+}
+
+void MappingHelpers::CreateLanes(UtilityHNS::AisanLanesFileReader* pLaneData,
+        UtilityHNS::AisanPointsFileReader* pPointsData,
+        UtilityHNS::AisanNodesFileReader* pNodesData,
+        std::vector<PlannerHNS::Lane>& out_lanes)
+{
+
+  out_lanes.clear();
+  std::vector<int> start_lines;
+  GetLanesStartPoints(pLaneData, start_lines);
+  for(unsigned int l =0; l < start_lines.size(); l++)
+  {
+    Lane _lane;
+    GetLanePoints(pLaneData, pPointsData, pNodesData, start_lines.at(l), _lane);
+    out_lanes.push_back(_lane);
+  }
+}
+
+void MappingHelpers::GetLanePoints(UtilityHNS::AisanLanesFileReader* pLaneData,
+      UtilityHNS::AisanPointsFileReader* pPointsData,
+      UtilityHNS::AisanNodesFileReader* pNodesData, int lnID,
+      PlannerHNS::Lane& out_lane)
+{
+  int next_lnid = lnID;
+  bool bStart = false;
+  bool bLast = false;
+  int _rID = 0;
+  out_lane.points.clear();
+  UtilityHNS::AisanLanesFileReader::AisanLane* pL = nullptr;
+  out_lane.id = lnID;
+  out_lane.speed = 0;
+
+  while(!bStart)
+  {
+    pL = pLaneData->GetDataRowById(next_lnid);
+    if(pL == nullptr)
+    {
+      cout << "## Error, Can't find lane from ID: " << next_lnid <<endl;
+      break;
+    }
+
+    next_lnid = pL->FLID;
+    if(next_lnid == 0)
+      bStart = true;
+    else
+      bStart = IsStartLanePoint(pLaneData, pLaneData->GetDataRowById(next_lnid));
+
+//    if(_lnid == 1267 ) //|| _lnid == 1268 || _lnid == 1269 || _lnid == 958)
+//      out_lane.id = lnID;
+
+    if(out_lane.points.size() == 0)
+    {
+      WayPoint wp1, wp2;
+      GetPointFromDataList(pPointsData, pNodesData->GetDataRowById(pL->BNID)->PID, wp1);
+      wp1.v = pL->RefVel == 0 ? DEFAULT_REF_VELOCITY : pL->RefVel;
+
+      wp1.id = pL->BNID;
+
+      if(pL->BLID != 0)
+      {
+        _rID = GetBeginPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->BLID);
+        if(_rID > 0)
+          wp1.fromIds.push_back(_rID);
+      }
+      if(pL->BLID2 != 0)
+      {
+        _rID = GetBeginPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->BLID2);
+        if(_rID > 0)
+          wp1.fromIds.push_back(_rID);
+      }
+      if(pL->BLID3 != 0)
+      {
+        _rID = GetBeginPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->BLID3);
+        if(_rID > 0)
+          wp1.fromIds.push_back(_rID);
+      }
+      if(pL->BLID4 != 0)
+      {
+        _rID = GetBeginPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->BLID4);
+        if(_rID > 0)
+          wp1.fromIds.push_back(_rID);
+      }
+
+      GetPointFromDataList(pPointsData, pNodesData->GetDataRowById(pL->FNID)->PID, wp2);
+      wp2.v = pL->RefVel == 0 ? DEFAULT_REF_VELOCITY : pL->RefVel;
+      wp2.id = pL->FNID;
+
+      if(bStart && pL->FLID != 0)
+      {
+        _rID = GetEndPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->FLID);
+        if(_rID > 0)
+          wp2.toIds.push_back(_rID);
+      }
+      if(pL->FLID2 != 0)
+      {
+        _rID = GetEndPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->FLID2);
+        if(_rID > 0)
+          wp2.toIds.push_back(_rID);
+      }
+      if(pL->FLID3 != 0)
+      {
+        _rID = GetEndPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->FLID3);
+        if(_rID > 0)
+          wp2.toIds.push_back(_rID);
+      }
+      if(pL->FLID4 != 0)
+      {
+        _rID = GetEndPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->FLID4);
+        if(_rID > 0)
+          wp2.toIds.push_back(_rID);
+      }
+
+      if(pL->LaneDir == 'L')
+      {
+        wp1.actionCost.push_back(make_pair(LEFT_TURN_ACTION, LEFT_INITIAL_TURNS_COST));
+        wp2.actionCost.push_back(make_pair(LEFT_TURN_ACTION, LEFT_INITIAL_TURNS_COST));
+      }
+      else  if(pL->LaneDir == 'R')
+      {
+        wp1.actionCost.push_back(make_pair(RIGHT_TURN_ACTION, RIGHT_INITIAL_TURNS_COST));
+        wp2.actionCost.push_back(make_pair(RIGHT_TURN_ACTION, RIGHT_INITIAL_TURNS_COST));
+      }
+      else
+      {
+        wp1.actionCost.push_back(make_pair(FORWARD_ACTION, 0));
+        wp2.actionCost.push_back(make_pair(FORWARD_ACTION, 0));
+      }
+
+      wp1.originalMapID = pL->LnID;
+      wp2.originalMapID = pL->LnID;
+
+      wp1.laneId = lnID;
+      wp2.laneId = lnID;
+
+      wp1.toIds.push_back(wp2.id);
+      wp2.fromIds.push_back(wp1.id);
+      out_lane.points.push_back(wp1);
+      out_lane.points.push_back(wp2);
+      out_lane.speed = pL->RefVel == 0 ? DEFAULT_REF_VELOCITY : pL->RefVel;
+    }
+    else
+    {
+      WayPoint wp;
+      GetPointFromDataList(pPointsData, pNodesData->GetDataRowById(pL->FNID)->PID, wp);
+      wp.v = pL->RefVel == 0 ? DEFAULT_REF_VELOCITY : pL->RefVel;
+      wp.id = pL->FNID;
+
+      out_lane.points.at(out_lane.points.size()-1).toIds.push_back(wp.id);
+      wp.fromIds.push_back(out_lane.points.at(out_lane.points.size()-1).id);
+
+      if(bStart && pL->FLID != 0)
+      {
+        _rID = GetEndPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->FLID);
+        if(_rID > 0)
+          wp.toIds.push_back(_rID);
+      }
+      if(pL->FLID2 != 0)
+      {
+        _rID = GetEndPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->FLID2);
+        if(_rID > 0)
+          wp.toIds.push_back(_rID);
+      }
+      if(pL->FLID3 != 0)
+      {
+        _rID = GetEndPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->FLID3);
+        if(_rID > 0)
+          wp.toIds.push_back(_rID);
+      }
+      if(pL->FLID4 != 0)
+      {
+        _rID = GetEndPointIdFromLaneNo(pLaneData, pPointsData, pNodesData, pL->FLID4);
+        if(_rID > 0)
+          wp.toIds.push_back(_rID);
+      }
+
+      if(pL->LaneDir == 'L')
+      {
+        wp.actionCost.push_back(make_pair(LEFT_TURN_ACTION, LEFT_INITIAL_TURNS_COST));
+      }
+      else  if(pL->LaneDir == 'R')
+      {
+        wp.actionCost.push_back(make_pair(RIGHT_TURN_ACTION, RIGHT_INITIAL_TURNS_COST));
+      }
+      else
+      {
+        wp.actionCost.push_back(make_pair(FORWARD_ACTION, 0));
+      }
+
+      wp.originalMapID = pL->LnID;
+      wp.laneId = lnID;
+
+      out_lane.points.push_back(wp);
+    }
+
+//    if(next_lnid == 0)
+//      break;
+  }
+}
+
+void MappingHelpers::FixRedundantPointsLanes(std::vector<Lane>& lanes)
+{
+  //Fix Redundant point for two points in a row
+  for(unsigned int il=0; il < lanes.size(); il ++)
+  {
+    for(int ip = 1; ip < lanes.at(il).points.size(); ip++)
+    {
+      WayPoint* p1 = &lanes.at(il).points.at(ip-1);
+      WayPoint* p2 = &lanes.at(il).points.at(ip);
+      WayPoint* p3 = nullptr;
+      if(ip+1 < lanes.at(il).points.size())
+        p3 = &lanes.at(il).points.at(ip+1);
+
+      double d = hypot(p2->pos.y-p1->pos.y, p2->pos.x-p1->pos.x);
+      if(d == 0)
+      {
+        p1->toIds = p2->toIds;
+        p1->originalMapID = p2->originalMapID;
+        if(p3 != nullptr)
+          p3->fromIds = p2->fromIds;
+
+        lanes.at(il).points.erase(lanes.at(il).points.begin()+ip);
+        ip--;
+
+        if(DEBUG_MAP_PARSING)
+          cout << "Fixed Redundant Points for Lane:" << lanes.at(il).id << ", Current: " << ip << ", Size: " << lanes.at(il).points.size() << endl;
+      }
+    }
+  }
+}
+
+void MappingHelpers::FixTwoPointsLane(Lane& l)
+{
+  if(l.points.size() == 2)
+  {
+    g_max_point_id++;
+    WayPoint wp = l.points.at(0);
+    wp.id = g_max_point_id;
+    wp.fromIds.clear();
+    wp.fromIds.push_back(l.points.at(0).id);
+    wp.toIds.clear();
+    wp.toIds.push_back(l.points.at(1).id);
+
+    l.points.at(0).toIds.clear();
+    l.points.at(0).toIds.push_back(wp.id);
+
+    l.points.at(1).fromIds.clear();
+    l.points.at(1).fromIds.push_back(wp.id);
+
+    wp.pos.x = (l.points.at(0).pos.x + l.points.at(1).pos.x)/2.0;
+    wp.pos.y = (l.points.at(0).pos.y + l.points.at(1).pos.y)/2.0;
+    wp.pos.z = (l.points.at(0).pos.z + l.points.at(1).pos.z)/2.0;
+
+    l.points.insert(l.points.begin()+1, wp);
+  }
+  else if(l.points.size() < 2)
+  {
+    cout << "## WOW Lane " <<  l.id << " With Size (" << l.points.size() << ") " << endl;
+  }
+}
+
+void MappingHelpers::FixTwoPointsLanes(std::vector<Lane>& lanes)
+{
+  for(unsigned int il=0; il < lanes.size(); il ++)
+  {
+    for(unsigned int ip = 0; ip < lanes.at(il).points.size(); ip++)
+    {
+      if(lanes.at(il).points.at(ip).id > g_max_point_id)
+      {
+        g_max_point_id = lanes.at(il).points.at(ip).id;
+      }
+    }
+  }
+
+  for(unsigned int il=0; il < lanes.size(); il ++)
+  {
+    FixTwoPointsLane(lanes.at(il));
+    PlannerHNS::PlanningHelpers::CalcAngleAndCost(lanes.at(il).points);
+  }
+}
+
+void MappingHelpers::InsertWayPointToBackOfLane(const WayPoint& wp, Lane& lane, int& global_id)
+{
+  WayPoint* pFirst = &lane.points.at(0);
+  pFirst->pos = wp.pos;
+
+//  WayPoint f_wp = *pFirst;
+//  f_wp.pos = wp.pos;
+//
+//  //Give old first new ID
+//  global_id++;
+//  pFirst->id = global_id;
+//
+//  //link ids
+//  f_wp.toIds.clear(); //only see front
+//  f_wp.toIds.push_back(pFirst->id);
+//
+//  pFirst->fromIds.clear();
+//  pFirst->fromIds.push_back(f_wp.id);
+//
+//  if(lane.points.size() > 1)
+//  {
+//    lane.points.at(1).fromIds.clear();
+//    lane.points.at(1).fromIds.push_back(pFirst->id);
+//  }
+//
+//  lane.points.insert(lane.points.begin(), f_wp);
+}
+
+void MappingHelpers::InsertWayPointToFrontOfLane(const WayPoint& wp, Lane& lane, int& global_id)
+{
+  WayPoint* pLast = &lane.points.at(lane.points.size()-1);
+  pLast->pos = wp.pos;
+
+//  WayPoint l_wp = *pLast;
+//  l_wp.pos = wp.pos;
+//
+//  //Give old first new ID
+//  global_id++;
+//  pLast->id = global_id;
+//
+//  //link ids
+//  l_wp.fromIds.clear(); //only see front
+//  l_wp.fromIds.push_back(pLast->id);
+//
+//  pLast->toIds.clear();
+//  pLast->toIds.push_back(l_wp.id);
+//
+//  if(lane.points.size() > 1)
+//  {
+//    lane.points.at(lane.points.size()-2).toIds.clear();
+//    lane.points.at(lane.points.size()-2).toIds.push_back(pLast->id);
+//  }
+//
+//  lane.points.push_back(l_wp);
+}
+
+void MappingHelpers::FixUnconnectedLanes(std::vector<Lane>& lanes)
+{
+  std::vector<Lane> sp_lanes = lanes;
+  bool bAtleastOneChange = false;
+  //Find before lanes
+  for(unsigned int il=0; il < lanes.size(); il ++)
+  {
+    if(lanes.at(il).fromIds.size() == 0)
+    {
+      double closest_d = DBL_MAX;
+      Lane* pL = nullptr;
+      Lane* pFL = nullptr;
+      for(int l=0; l < sp_lanes.size(); l ++)
+      {
+        if(lanes.at(il).id == sp_lanes.at(l).id)
+        {
+          pFL = &sp_lanes.at(l);
+          break;
+        }
+      }
+
+      PlannerHNS::RelativeInfo closest_info;
+      int closest_index = -1;
+      for(int l=0; l < sp_lanes.size(); l ++)
+      {
+        if(pFL->id != sp_lanes.at(l).id )
+        {
+          PlannerHNS::RelativeInfo info;
+          WayPoint lastofother = sp_lanes.at(l).points.at(sp_lanes.at(l).points.size()-1);
+          PlanningHelpers::GetRelativeInfoLimited(sp_lanes.at(l).points, pFL->points.at(0), info, 0);
+          double back_distance = hypot(lastofother.pos.y - pFL->points.at(0).pos.y, lastofother.pos.x - pFL->points.at(0).pos.x);
+          bool bCloseFromBack = false;
+          if((info.bAfter == true && back_distance < 15.0) || info.bAfter == false)
+            bCloseFromBack = true;
+
+
+          if(fabs(info.perp_distance) < 2 && fabs(info.perp_distance) < closest_d && info.bBefore == false && bCloseFromBack)
+          {
+            closest_d = fabs(info.perp_distance);
+            pL = &sp_lanes.at(l);
+            closest_info = info;
+            closest_info.angle_diff = back_distance;
+            closest_index = l;
+          }
+        }
+      }
+
+      if(pL != nullptr && pFL != nullptr)
+      {
+        if(closest_info.iFront == pL->points.size()-1)
+        {
+          pL->toIds.push_back(pFL->id);
+          pL->points.at(closest_info.iFront).toIds.push_back(pFL->points.at(0).id);
+
+          pFL->points.at(0).fromIds.push_back(pL->points.at(closest_info.iFront).id);
+          pFL->fromIds.push_back(pL->id);
+          bAtleastOneChange = true;
+          if(DEBUG_MAP_PARSING)
+          {
+            cout << "Closest Next Lane For: " << pFL->id << " , Is:" << pL->id << ", Distance=" << fabs(closest_info.perp_distance) << ", Size: " << pL->points.size() << ", back_index: " << closest_info.iBack <<", front_index: " << closest_info.iFront << ", Direct: " << closest_info.angle_diff << endl;
+            cout << "Don't Split , Perfect !" << endl;
+          }
+        }
+        else
+        {
+           // split from previous point
+          if(DEBUG_MAP_PARSING)
+            cout << "Closest Next Lane For: " << pFL->id << " , Is:" << pL->id << ", Distance=" << fabs(closest_info.perp_distance) << ", Size: " << pL->points.size() << ", back_index: " << closest_info.iBack <<", front_index: " << closest_info.iFront << ", Direct: " << closest_info.angle_diff << endl;
+
+          Lane front_half, back_half;
+          front_half.points.insert(front_half.points.begin(), pL->points.begin()+closest_info.iFront, pL->points.end());
+          front_half.toIds = pL->toIds;
+          front_half.fromIds.push_back(pL->id);
+          front_half.id = front_half.points.at(0).originalMapID;
+          front_half.areaId = pL->areaId;
+          front_half.dir = pL->dir;
+          front_half.num = pL->num;
+          front_half.roadId = pL->roadId;
+          front_half.speed = pL->speed;
+          front_half.type = pL->type;
+          front_half.width = pL->width;
+
+          back_half.points.insert(back_half.points.begin(), pL->points.begin(), pL->points.begin()+closest_info.iFront);
+          back_half.toIds.clear();
+          back_half.toIds.push_back(front_half.id);
+          back_half.toIds.push_back(pFL->id);
+          back_half.fromIds = pL->fromIds;
+          back_half.id = pL->id;
+          back_half.areaId = pL->areaId;
+          back_half.dir = pL->dir;
+          back_half.num = pL->num;
+          back_half.roadId = pL->roadId;
+          back_half.speed = pL->speed;
+          back_half.type = pL->type;
+          back_half.width = pL->width;
+
+          WayPoint* last_from_back =  &back_half.points.at(back_half.points.size()-1);
+          WayPoint* first_from_front =  &pFL->points.at(0);
+
+          last_from_back->toIds.push_back(first_from_front->id);
+          first_from_front->fromIds.push_back(last_from_back->id);
+
+          if(front_half.points.size() > 1 && back_half.points.size() > 1)
+          {
+            if(DEBUG_MAP_PARSING)
+              cout << "Split this one Nicely! first_half_size: " << front_half.points.size() << ", second_hald_size: " << back_half.points.size() << endl;
+
+            pFL->fromIds.push_back(back_half.id);
+
+            if(closest_index >= 0)
+              sp_lanes.erase(sp_lanes.begin()+closest_index);
+            else
+              cout << "## Alert Alert Alert !!!! " << endl;
+
+            // add perp point to lane points
+            InsertWayPointToBackOfLane(closest_info.perp_point, front_half, g_max_point_id);
+            InsertWayPointToFrontOfLane(closest_info.perp_point, back_half, g_max_point_id);
+
+            sp_lanes.push_back(front_half);
+            sp_lanes.push_back(back_half);
+            bAtleastOneChange = true;
+          }
+          else
+          {
+            if(DEBUG_MAP_PARSING)
+              cout << "=> Can't Split this one :( !" << endl;
+          }
+        }
+      }
+      else
+      {
+        if(DEBUG_MAP_PARSING)
+          cout << "=> Can't find Before Lanes For:  " << lanes.at(il).id  << endl;
+      }
+    }
+  }
+
+  lanes = sp_lanes;
+
+  //Find to lanes
+
+  for(unsigned int il=0; il < lanes.size(); il ++)
+  {
+    if(lanes.at(il).toIds.size() == 0)
+    {
+      double closest_d = DBL_MAX;
+      Lane* pL = nullptr;
+      Lane* pBL = nullptr;
+      for(int l=0; l < sp_lanes.size(); l ++)
+      {
+        if(lanes.at(il).id == sp_lanes.at(l).id)
+        {
+          pBL = &sp_lanes.at(l);
+          break;
+        }
+      }
+
+      PlannerHNS::RelativeInfo closest_info;
+      int closest_index = -1;
+      for(int l=0; l < sp_lanes.size(); l ++)
+      {
+        if(pBL->id != sp_lanes.at(l).id )
+        {
+          PlannerHNS::RelativeInfo info;
+          WayPoint firstofother = sp_lanes.at(l).points.at(0);
+          WayPoint last_point = pBL->points.at(pBL->points.size()-1);
+          PlanningHelpers::GetRelativeInfoLimited(sp_lanes.at(l).points, last_point, info, 0);
+          double front_distance = hypot(firstofother.pos.y - last_point.pos.y, firstofother.pos.x - last_point.pos.x);
+          bool bCloseFromFront = false;
+          if((info.bBefore == true && front_distance < 15.0) || info.bBefore == false)
+            bCloseFromFront = true;
+
+          if(fabs(info.perp_distance) < 2 && fabs(info.perp_distance) < closest_d && info.bAfter == false && bCloseFromFront)
+          {
+            closest_d = fabs(info.perp_distance);
+            pL = &sp_lanes.at(l);
+            closest_info = info;
+            closest_info.angle_diff = front_distance;
+            closest_index = l;
+          }
+        }
+      }
+
+      if(pL != nullptr && pBL != nullptr)
+      {
+        if(closest_info.iFront == 0)
+        {
+          pL->fromIds.push_back(pBL->id);
+          pL->points.at(closest_info.iFront).fromIds.push_back(pBL->points.at(pBL->points.size()-1).id);
+
+          pBL->points.at(pBL->points.size()-1).toIds.push_back(pL->points.at(closest_info.iFront).id);
+          pBL->toIds.push_back(pL->id);
+
+          bAtleastOneChange = true;
+          if(DEBUG_MAP_PARSING)
+          {
+            cout << "Closest Back Lane For: " << pBL->id << " , Is:" << pL->id << ", Distance=" << fabs(closest_info.perp_distance) << ", Size: " << pL->points.size() << ", back_index: " << closest_info.iBack <<", front_index: " << closest_info.iFront << ", Direct: " << closest_info.angle_diff << endl;
+            cout << "Don't Split , Perfect !" << endl;
+          }
+        }
+        else
+        {
+           // split from previous point
+          if(DEBUG_MAP_PARSING)
+            cout << "Closest Back Lane For: " << pBL->id << " , Is:" << pL->id << ", Distance=" << fabs(closest_info.perp_distance) << ", Size: " << pL->points.size() << ", back_index: " << closest_info.iBack <<", front_index: " << closest_info.iFront << ", Direct: " << closest_info.angle_diff << endl;
+
+          Lane front_half, back_half;
+          front_half.points.insert(front_half.points.begin(), pL->points.begin()+closest_info.iFront, pL->points.end());
+          front_half.toIds = pL->toIds;
+          front_half.fromIds.push_back(pL->id);
+          front_half.fromIds.push_back(pBL->id);
+          front_half.id = front_half.points.at(0).originalMapID;
+          front_half.areaId = pL->areaId;
+          front_half.dir = pL->dir;
+          front_half.num = pL->num;
+          front_half.roadId = pL->roadId;
+          front_half.speed = pL->speed;
+          front_half.type = pL->type;
+          front_half.width = pL->width;
+
+          back_half.points.insert(back_half.points.begin(), pL->points.begin(), pL->points.begin()+closest_info.iFront);
+          back_half.toIds.push_back(front_half.id);
+          back_half.fromIds = pL->fromIds;
+          back_half.id = pL->id;
+          back_half.areaId = pL->areaId;
+          back_half.dir = pL->dir;
+          back_half.num = pL->num;
+          back_half.roadId = pL->roadId;
+          back_half.speed = pL->speed;
+          back_half.type = pL->type;
+          back_half.width = pL->width;
+
+          WayPoint* first_from_next =  &front_half.points.at(0);
+          WayPoint* last_from_front =  &pBL->points.at(pBL->points.size()-1);
+
+          first_from_next->fromIds.push_back(last_from_front->id);
+          last_from_front->toIds.push_back(first_from_next->id);
+
+          if(front_half.points.size() > 1 && back_half.points.size() > 1)
+          {
+            if(DEBUG_MAP_PARSING)
+              cout << "Split this one Nicely! first_half_size: " << front_half.points.size() << ", second_hald_size: " << back_half.points.size() << endl;
+
+            pBL->toIds.push_back(front_half.id);
+
+            if(closest_index >= 0)
+              sp_lanes.erase(sp_lanes.begin()+closest_index);
+            else
+              cout << "## Alert Alert Alert !!!! " << endl;
+
+            // add perp point to lane points
+            InsertWayPointToBackOfLane(closest_info.perp_point, front_half, g_max_point_id);
+            InsertWayPointToFrontOfLane(closest_info.perp_point, back_half, g_max_point_id);
+
+            sp_lanes.push_back(front_half);
+            sp_lanes.push_back(back_half);
+            bAtleastOneChange = true;
+          }
+          else
+          {
+            if(DEBUG_MAP_PARSING)
+              cout << "=> Can't Split this one :( !" << endl;
+          }
+        }
+      }
+      else
+      {
+        if(DEBUG_MAP_PARSING)
+          cout << "=> Can't find After Lanes For:  " << lanes.at(il).id  << endl;
+      }
+    }
+  }
+
+  lanes = sp_lanes;
+}
+
+void MappingHelpers::LinkLanesPointers(PlannerHNS::RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    //Link Lanes
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      for(unsigned int j = 0 ; j < pL->fromIds.size(); j++)
+      {
+        for(unsigned int l= 0; l < map.roadSegments.at(rs).Lanes.size(); l++)
+        {
+          if(map.roadSegments.at(rs).Lanes.at(l).id == pL->fromIds.at(j))
+          {
+            pL->fromLanes.push_back(&map.roadSegments.at(rs).Lanes.at(l));
+          }
+        }
+      }
+
+      for(unsigned int j = 0 ; j < pL->toIds.size(); j++)
+      {
+        for(unsigned int l= 0; l < map.roadSegments.at(rs).Lanes.size(); l++)
+        {
+          if(map.roadSegments.at(rs).Lanes.at(l).id == pL->toIds.at(j))
+          {
+            pL->toLanes.push_back(&map.roadSegments.at(rs).Lanes.at(l));
+          }
+        }
+      }
+
+      for(unsigned int j = 0 ; j < pL->points.size(); j++)
+      {
+        pL->points.at(j).pLane  = pL;
+      }
+    }
+  }
+}
+
+void MappingHelpers::ExtractCurbDataV2(const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
+        UtilityHNS::AisanLinesFileReader* pLinedata,
+        UtilityHNS::AisanPointsFileReader* pPointsData,
+        const GPSPoint& origin, RoadNetwork& map)
+{
+  for(unsigned int ic=0; ic < curb_data.size(); ic++)
+  {
+    Curb c;
+    c.id = curb_data.at(ic).ID;
+
+    for(unsigned int il=0; il < pLinedata->m_data_list.size() ; il++)
+    {
+      if(curb_data.at(ic).LID == pLinedata->m_data_list.at(il).LID)
+      {
+        int s_id = pLinedata->m_data_list.at(il).BPID;
+        if(s_id == 0)
+          s_id = pLinedata->m_data_list.at(il).FPID;
+
+        AisanPointsFileReader::AisanPoints* pP = pPointsData->GetDataRowById(s_id);
+        if(pP != nullptr)
+        {
+          c.points.push_back(GPSPoint(pP->Ly + origin.x, pP->Bx + origin.y, pP->H + origin.z, 0));
+          WayPoint wp;
+          wp.pos = c.points.at(0);
+          Lane* pLane = GetClosestLaneFromMap(wp, map, 5);
+          if(pLane != nullptr)
+          {
+            c.laneId = pLane->id;
+            c.pLane = pLane;
+          }
+        }
+      }
+    }
+    map.curbs.push_back(c);
+  }
+}
+
+void MappingHelpers::ExtractSignalDataV2(const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
+      const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
+      UtilityHNS::AisanPointsFileReader* pPointData,
+      const GPSPoint& origin, RoadNetwork& map)
+{
+  for(unsigned int is=0; is< signal_data.size(); is++)
+  {
+    //if(signal_data.at(is).Type == 2)
+    {
+      TrafficLight tl;
+      tl.id = signal_data.at(is).ID;
+      tl.linkID = signal_data.at(is).LinkID;
+      tl.stoppingDistance = 0;
+
+      for(unsigned int iv = 0; iv < vector_data.size(); iv++)
+      {
+        if(signal_data.at(is).VID == vector_data.at(iv).VID)
+        {
+          AisanPointsFileReader::AisanPoints* pP =  pPointData->GetDataRowById(vector_data.at(iv).PID);
+          if(pP != nullptr)
+          {
+            tl.pos = GPSPoint(pP->Ly + origin.x, pP->Bx + origin.y, pP->H + origin.z, vector_data.at(iv).Hang*DEG2RAD);
+          }
+        }
+      }
+      map.trafficLights.push_back(tl);
+      if(tl.id > g_max_traffic_light_id)
+        g_max_traffic_light_id = tl.id;
+    }
+  }
+}
+
+void MappingHelpers::ExtractStopLinesDataV2(const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
+      UtilityHNS::AisanLinesFileReader* pLineData,
+      UtilityHNS::AisanPointsFileReader* pPointData,
+      const GPSPoint& origin, RoadNetwork& map)
+{
+  for(unsigned int ist=0; ist < stop_line_data.size(); ist++)
+  {
+    StopLine sl;
+    sl.linkID = stop_line_data.at(ist).LinkID;
+    sl.id = stop_line_data.at(ist).ID;
+    if(stop_line_data.at(ist).TLID>0)
+      sl.trafficLightID = stop_line_data.at(ist).TLID;
+    else
+      sl.stopSignID = 100+ist;
+
+    AisanLinesFileReader::AisanLine* pLine = pLineData->GetDataRowById(stop_line_data.at(ist).LID);
+    if(pLine != nullptr)
+    {
+      UtilityHNS::AisanPointsFileReader::AisanPoints* pStart = pPointData->GetDataRowById(pLine->BPID);
+      UtilityHNS::AisanPointsFileReader::AisanPoints* pEnd = pPointData->GetDataRowById(pLine->FPID);
+      if(pStart != nullptr)
+        sl.points.push_back(GPSPoint(pStart->Ly + origin.x, pStart->Bx + origin.y, pStart->H + origin.z, 0));
+
+      if(pEnd != nullptr)
+        sl.points.push_back(GPSPoint(pEnd->Ly + origin.x, pEnd->Bx + origin.y, pEnd->H + origin.z, 0));
+    }
+
+    map.stopLines.push_back(sl);
+    if(sl.id > g_max_stop_line_id)
+      g_max_stop_line_id = sl.id;
+  }
+}
+
+void MappingHelpers::LinkTrafficLightsAndStopLinesV2(RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int p= 0; p < map.roadSegments.at(rs).Lanes.at(i).points.size(); p++)
+      {
+        WayPoint* pWP = &map.roadSegments.at(rs).Lanes.at(i).points.at(p);
+
+        for(unsigned int isl = 0; isl < map.stopLines.size(); isl++)
+        {
+          if(map.stopLines.at(isl).linkID == pWP->originalMapID)
+          {
+            map.stopLines.at(isl).laneId = pWP->laneId;
+            map.stopLines.at(isl).pLane = pWP->pLane;
+            map.roadSegments.at(rs).Lanes.at(i).stopLines.push_back(map.stopLines.at(isl));
+
+            pWP->stopLineID = map.stopLines.at(isl).id;
+
+            for(unsigned int itl = 0; itl < map.trafficLights.size(); itl++)
+            {
+              if(map.trafficLights.at(itl).id == map.stopLines.at(isl).trafficLightID)
+              {
+                map.trafficLights.at(itl).laneIds.push_back(pWP->laneId);
+                map.trafficLights.at(itl).pLanes.push_back(pWP->pLane);
+              }
+            }
+            break;
+          }
+        }
+      }
+    }
+  }
+
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      for(unsigned int itl = 0; itl < map.trafficLights.size(); itl++)
+      {
+        for(unsigned int p= 0; p < map.roadSegments.at(rs).Lanes.at(i).points.size(); p++)
+        {
+          WayPoint* pWP = &map.roadSegments.at(rs).Lanes.at(i).points.at(p);
+          if(map.trafficLights.at(itl).linkID == pWP->originalMapID)
+          {
+            map.roadSegments.at(rs).Lanes.at(i).trafficlights.push_back(map.trafficLights.at(itl));
+            break;
+          }
+        }
+      }
+    }
+  }
+}
+
+void MappingHelpers::FindAdjacentLanesV2(RoadNetwork& map)
+{
+  bool bTestDebug = false;
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      for(unsigned int i2 =0; i2 < map.roadSegments.at(rs).Lanes.size(); i2++)
+      {
+        Lane* pL2 = &map.roadSegments.at(rs).Lanes.at(i2);
+
+        if(pL->id == pL2->id) continue;
+
+
+        if(pL->id == 1683)
+          bTestDebug = true;
+
+        for(unsigned int p=0; p < pL->points.size(); p++)
+        {
+          WayPoint* pWP = &pL->points.at(p);
+          RelativeInfo info;
+          PlanningHelpers::GetRelativeInfoLimited(pL2->points, *pWP, info);
+
+          if(!info.bAfter && !info.bBefore && fabs(info.perp_distance) > 1.2 && fabs(info.perp_distance) < 3.5 && UtilityH::AngleBetweenTwoAnglesPositive(info.perp_point.pos.a, pWP->pos.a) < 0.06)
+          {
+            WayPoint* pWP2 = &pL2->points.at(info.iFront);
+            if(info.perp_distance < 0)
+            {
+              if(pWP->pRight == 0)
+              {
+                pWP->pRight = pWP2;
+                pWP->RightPointId = pWP2->id;
+                pWP->RightLnId = pL2->id;
+                pL->pRightLane = pL2;
+
+              }
+
+              if(pWP2->pLeft == 0)
+              {
+                pWP2->pLeft = pWP;
+                pWP2->LeftPointId = pWP->id;
+                pWP2->LeftLnId = pL->id;
+                pL2->pLeftLane = pL;
+              }
+            }
+            else
+            {
+              if(pWP->pLeft == 0)
+              {
+                pWP->pLeft = pWP2;
+                pWP->LeftPointId = pWP2->id;
+                pWP->LeftLnId = pL2->id;
+                pL->pLeftLane = pL2;
+              }
+
+              if(pWP2->pRight == 0)
+              {
+                pWP2->pRight = pWP->pLeft;
+                pWP2->RightPointId = pWP->id;
+                pWP2->RightLnId = pL->id;
+                pL2->pRightLane = pL;
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
+void MappingHelpers::GetMapMaxIds(PlannerHNS::RoadNetwork& map)
+{
+  for(unsigned int rs = 0; rs < map.roadSegments.size(); rs++)
+  {
+    for(unsigned int i =0; i < map.roadSegments.at(rs).Lanes.size(); i++)
+    {
+      Lane* pL = &map.roadSegments.at(rs).Lanes.at(i);
+      if(pL->id > g_max_lane_id)
+        g_max_lane_id = pL->id;
+
+      for(unsigned int j = 0 ; j < pL->points.size(); j++)
+      {
+        if(pL->points.at(j).id > g_max_point_id)
+        {
+          g_max_point_id = pL->points.at(j).id;
+        }
+      }
+    }
+  }
+
+  for(unsigned int i=0; i < map.stopLines.size(); i++)
+  {
+    if(map.stopLines.at(i).id > g_max_stop_line_id)
+      g_max_stop_line_id = map.stopLines.at(i).id;
+  }
+
+  for(unsigned int i=0; i < map.trafficLights.size(); i++)
+  {
+    if(map.trafficLights.at(i).id > g_max_traffic_light_id)
+      g_max_traffic_light_id = map.trafficLights.at(i).id;
+  }
+}
+
+
+} /* namespace PlannerHNS */

src1/planning/op_planner/MatrixOperations.cpp

@@ -0,0 +1,10 @@
+/// \file MatrixOperations.cpp
+/// \brief Simple matrix operations
+/// \author Hatem Darweesh
+/// \date Jun 19, 2016
+
+#include "op_planner/MatrixOperations.h"
+
+namespace PlannerHNS {
+
+} /* namespace PlannerHNS */

src1/planning/op_planner/PassiveDecisionMaker.cpp

@@ -0,0 +1,153 @@
+
+/// \file  PassiveDecisionMaker.cpp
+/// \brief Decision Maker for surrounding vehicle, to be integrated with particle filter for intention prediction
+/// \author Hatem Darweesh
+/// \date Jan 10, 2018
+
+#include "op_planner/PassiveDecisionMaker.h"
+#include "op_utility/UtilityH.h"
+#include "op_planner/PlanningHelpers.h"
+#include "op_planner/MappingHelpers.h"
+#include "op_planner/MatrixOperations.h"
+
+
+namespace PlannerHNS
+{
+
+PassiveDecisionMaker::PassiveDecisionMaker()
+{
+}
+
+PassiveDecisionMaker& PassiveDecisionMaker::operator=(const PassiveDecisionMaker& obj)
+{
+  return *this;
+}
+
+PassiveDecisionMaker::PassiveDecisionMaker(const PassiveDecisionMaker& obj)
+{
+}
+
+PassiveDecisionMaker::~PassiveDecisionMaker()
+{
+}
+
+ double PassiveDecisionMaker::GetVelocity(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo, const RelativeInfo& info)
+ {
+  double average_braking_distance = -pow(currPose.v, 2)/(carInfo.max_deceleration) + 1.0;
+  int prev_index = 0;
+  double velocity = PlannerHNS::PlanningHelpers::GetVelocityAhead(path, info, prev_index, average_braking_distance);
+  if(velocity > carInfo.max_speed_forward)
+    velocity = carInfo.max_speed_forward;
+  else if(velocity < carInfo.min_speed_forward)
+    velocity = carInfo.min_speed_forward;
+
+  return velocity;
+ }
+
+ double PassiveDecisionMaker::GetSteerAngle(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const RelativeInfo& info)
+  {
+     unsigned int point_index = 0;
+     PlannerHNS::WayPoint pursuite_point = PlanningHelpers::GetFollowPointOnTrajectory(path, info, 2, point_index);
+
+     double current_a = UtilityHNS::UtilityH::SplitPositiveAngle(currPose.pos.a);
+     double target_a = atan2(pursuite_point.pos.y - currPose.pos.y, pursuite_point.pos.x - currPose.pos.x);
+     double e =  UtilityHNS::UtilityH::SplitPositiveAngle(target_a - current_a);
+     double before_lowpass = e;//m_pidSteer.getPID(e);
+     //std::cout << "CurrA: " << current_a << ", targetA: " << target_a << ", e: " << e << std::endl;
+     return before_lowpass;
+
+  }
+
+ bool PassiveDecisionMaker::CheckForStopLine(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo)
+ {
+   double minStoppingDistance = -pow(currPose.v, 2)/(carInfo.max_deceleration);
+   double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0;
+
+  int stopLineID = -1;
+  int stopSignID = -1;
+  int trafficLightID = -1;
+  double distanceToClosestStopLine = 0;
+
+  distanceToClosestStopLine = PlanningHelpers::GetDistanceToClosestStopLineAndCheck(path, currPose, 0, stopLineID, stopSignID, trafficLightID) - critical_long_front_distance;
+
+  if(distanceToClosestStopLine > -2 && distanceToClosestStopLine < minStoppingDistance)
+  {
+    return true;
+  }
+
+  return false;
+ }
+
+ PlannerHNS::BehaviorState PassiveDecisionMaker::MoveStep(const double& dt, PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo)
+ {
+   PlannerHNS::BehaviorState beh;
+   if(path.size() == 0) return beh;
+
+   RelativeInfo info;
+   PlanningHelpers::GetRelativeInfo(path, currPose, info);
+
+   bool bStopLine = CheckForStopLine(currPose, path, carInfo);
+   if(bStopLine)
+     beh.state = PlannerHNS::STOPPING_STATE;
+   else
+     beh.state = PlannerHNS::FORWARD_STATE;
+
+   double average_braking_distance = -pow(currPose.v, 2)/(carInfo.max_deceleration) + 1.0;
+
+  if(average_braking_distance  < 10)
+    average_braking_distance = 10;
+
+  beh.indicator = PlanningHelpers::GetIndicatorsFromPath(path, currPose, average_braking_distance);
+
+  currPose.v = beh.maxVelocity = GetVelocity(currPose, path, carInfo, info);
+
+  double steer = GetSteerAngle(currPose, path, info);
+
+  currPose.pos.x += currPose.v * dt * cos(currPose.pos.a);
+  currPose.pos.y += currPose.v * dt * sin(currPose.pos.a);
+  currPose.pos.a += currPose.v * dt * tan(steer)  / carInfo.wheel_base;
+
+  return beh;
+
+ }
+
+ PlannerHNS::ParticleInfo PassiveDecisionMaker::MoveStepSimple(const double& dt, PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo)
+  {
+    PlannerHNS::ParticleInfo beh;
+    if(path.size() == 0) return beh;
+
+    RelativeInfo info;
+    PlanningHelpers::GetRelativeInfo(path, currPose, info);
+
+    bool bStopLine = CheckForStopLine(currPose, path, carInfo);
+    if(bStopLine)
+      beh.state = PlannerHNS::STOPPING_STATE;
+    else
+      beh.state = PlannerHNS::FORWARD_STATE;
+
+    double average_braking_distance = -pow(currPose.v, 2)/(carInfo.max_deceleration) + 15.0;
+
+
+   PlannerHNS::WayPoint startPose = path.at(0);
+   beh.indicator = PlanningHelpers::GetIndicatorsFromPath(path, startPose, average_braking_distance);
+
+
+   double speed = 0;
+   if(info.iFront < path.size())
+   {
+     beh.vel = path.at(info.iFront).v;
+   }
+   else
+     beh.vel = 0;
+
+   double steer = GetSteerAngle(currPose, path, info);
+
+   currPose.pos.x += currPose.v * dt * cos(currPose.pos.a);
+   currPose.pos.y += currPose.v * dt * sin(currPose.pos.a);
+   currPose.pos.a += currPose.v * dt * tan(steer)  / carInfo.wheel_base;
+
+   return beh;
+
+  }
+
+} /* namespace PlannerHNS */

src1/planning/op_planner/PlannerH.cpp

@@ -0,0 +1,495 @@
+/// \file PlannerH.cpp
+/// \brief Main functions for path generation (global and local)
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+#include "op_planner/PlannerH.h"
+#include "op_planner/PlanningHelpers.h"
+#include "op_planner/MappingHelpers.h"
+#include <iostream>
+
+using namespace std;
+using namespace UtilityHNS;
+
+namespace PlannerHNS
+{
+PlannerH::PlannerH()
+{
+  //m_Params = params;
+}
+
+PlannerH::~PlannerH()
+{
+}
+
+void PlannerH::GenerateRunoffTrajectory(const std::vector<std::vector<WayPoint> >& referencePaths,const WayPoint& carPos, const bool& bEnableLaneChange, const double& speed, const double& microPlanDistance,
+    const double& maxSpeed,const double& minSpeed, const double&  carTipMargin, const double& rollInMargin,
+    const double& rollInSpeedFactor, const double& pathDensity, const double& rollOutDensity,
+    const int& rollOutNumber, const double& SmoothDataWeight, const double& SmoothWeight,
+    const double& SmoothTolerance, const double& speedProfileFactor, const bool& bHeadingSmooth,
+    const int& iCurrGlobalPath, const int& iCurrLocalTraj,
+    std::vector<std::vector<std::vector<WayPoint> > >& rollOutsPaths,
+    std::vector<WayPoint>& sampledPoints_debug)
+{
+
+  if(referencePaths.size()==0) return;
+  if(microPlanDistance <=0 ) return;
+  rollOutsPaths.clear();
+
+  sampledPoints_debug.clear(); //for visualization only
+
+  for(unsigned int i = 0; i < referencePaths.size(); i++)
+  {
+    std::vector<std::vector<WayPoint> > local_rollOutPaths;
+    int s_index = 0, e_index = 0;
+    vector<double> e_distances;
+    if(referencePaths.at(i).size()>0)
+    {
+      PlanningHelpers::CalculateRollInTrajectories(carPos, speed, referencePaths.at(i), s_index, e_index, e_distances,
+          local_rollOutPaths, microPlanDistance, maxSpeed, carTipMargin, rollInMargin,
+          rollInSpeedFactor, pathDensity, rollOutDensity,rollOutNumber,
+          SmoothDataWeight, SmoothWeight, SmoothTolerance, bHeadingSmooth, sampledPoints_debug);
+    }
+    else
+    {
+      for(int j=0; j< rollOutNumber+1; j++)
+      {
+        local_rollOutPaths.push_back(vector<WayPoint>());
+      }
+    }
+
+    rollOutsPaths.push_back(local_rollOutPaths);
+  }
+}
+
+double PlannerH::PlanUsingDPRandom(const WayPoint& start,
+    const double& maxPlanningDistance,
+    RoadNetwork& map,
+    std::vector<std::vector<WayPoint> >& paths)
+{
+  PlannerHNS::WayPoint* pStart = PlannerHNS::MappingHelpers::GetClosestBackWaypointFromMap(start, map);
+
+  if(!pStart)
+  {
+    GPSPoint sp = start.pos;
+    cout << endl << "Error: PlannerH -> Can't Find Global Waypoint Nodes in the Map for Start (" <<  sp.ToString() << ")" << endl;
+    return 0;
+  }
+
+  if(!pStart->pLane)
+  {
+    cout << endl << "Error: PlannerH -> Null Lane, Start (" << pStart->pLane << ")" << endl;
+    return 0;
+  }
+
+  RelativeInfo start_info;
+  PlanningHelpers::GetRelativeInfo(pStart->pLane->points, start, start_info);
+
+  if(start_info.perp_distance > START_POINT_MAX_DISTANCE)
+  {
+    GPSPoint sp = start.pos;
+    cout << endl << "Error: PlannerH -> Start Distance to Lane is: " << start_info.perp_distance
+        << ", Pose: " << sp.ToString() << ", LanePose:" << start_info.perp_point.pos.ToString()
+        << ", LaneID: " << pStart->pLane->id << " -> Check origin and vector map. " << endl;
+    return 0;
+  }
+
+  vector<WayPoint*> local_cell_to_delete;
+  WayPoint* pLaneCell = 0;
+  pLaneCell =  PlanningHelpers::BuildPlanningSearchTreeStraight(pStart, maxPlanningDistance, local_cell_to_delete);
+
+  if(!pLaneCell)
+  {
+    cout << endl << "PlannerH -> Plan (B) Failed, Sorry we Don't have plan (C) This is the END." << endl;
+    return 0;
+  }
+
+
+  vector<WayPoint> path;
+  vector<vector<WayPoint> > tempCurrentForwardPathss;
+  const std::vector<int> globalPath;
+  PlanningHelpers::TraversePathTreeBackwards(pLaneCell, pStart, globalPath, path, tempCurrentForwardPathss);
+  cout << endl <<"Info: PlannerH -> Plan (B) Path With Size (" << (int)path.size() << "), MultiPaths No(" << paths.size() << ") Extraction Time : " << endl;
+
+  //PlanningHelpers::CreateManualBranch(path, 0, FORWARD_RIGHT_DIR);
+  //cout << "Right Branch Created with Size: " << path.size()  << endl;
+  //PlanningHelpers::CreateManualBranch(path, 0, FORWARD_LEFT_DIR);
+  paths.push_back(path);
+
+  if(path.size()<2)
+  {
+    cout << endl << "Err: PlannerH -> Invalid Path, Car Should Stop." << endl;
+    if(pLaneCell)
+      DeleteWaypoints(local_cell_to_delete);
+    return 0 ;
+  }
+
+  if(pLaneCell)
+    DeleteWaypoints(local_cell_to_delete);
+
+  double totalPlanningDistance = path.at(path.size()-1).cost;
+  return totalPlanningDistance;
+}
+
+double PlannerH::PlanUsingDP(const WayPoint& start,
+    const WayPoint& goalPos,
+    const double& maxPlanningDistance,
+    const bool bEnableLaneChange,
+    const std::vector<int>& globalPath,
+    RoadNetwork& map,
+    std::vector<std::vector<WayPoint> >& paths, vector<WayPoint*>* all_cell_to_delete)
+{
+  PlannerHNS::WayPoint* pStart = PlannerHNS::MappingHelpers::GetClosestWaypointFromMap(start, map);
+  PlannerHNS::WayPoint* pGoal = PlannerHNS::MappingHelpers::GetClosestWaypointFromMap(goalPos, map);
+  bool bEnableGoalBranching = false;
+
+std::cout<<" Finding Near"<<std::endl;
+  if(!pStart ||  !pGoal)
+  {
+    GPSPoint sp = start.pos;
+    GPSPoint gp = goalPos.pos;
+    cout << endl << "Error: PlannerH -> Can't Find Global Waypoint Nodes in the Map for Start (" <<  sp.ToString() << ") and Goal (" << gp.ToString() << ")" << endl;
+    return 0;
+  }
+
+  std::cout<<"Found Near."<<std::endl;
+
+  if(!pStart->pLane || !pGoal->pLane)
+  {
+    cout << endl << "Error: PlannerH -> Null Lane, Start (" << pStart->pLane << ") and Goal (" << pGoal->pLane << ")" << endl;
+    return 0;
+  }
+
+  RelativeInfo start_info, goal_info;
+  PlanningHelpers::GetRelativeInfo(pStart->pLane->points, start, start_info);
+  PlanningHelpers::GetRelativeInfo(pGoal->pLane->points, goalPos, goal_info);
+
+  vector<WayPoint> start_path, goal_path;
+
+  if(fabs(start_info.perp_distance) > START_POINT_MAX_DISTANCE)
+  {
+    GPSPoint sp = start.pos;
+    cout << endl << "Error: PlannerH -> Start Distance to Lane is: " << start_info.perp_distance
+        << ", Pose: " << sp.ToString() << ", LanePose:" << start_info.perp_point.pos.ToString()
+        << ", LaneID: " << pStart->pLane->id << " -> Check origin and vector map. " << endl;
+    return 0;
+  }
+
+  if(fabs(goal_info.perp_distance) > GOAL_POINT_MAX_DISTANCE)
+  {
+    if(fabs(start_info.perp_distance) > 20)
+    {
+      GPSPoint gp = goalPos.pos;
+      cout << endl << "Error: PlannerH -> Goal Distance to Lane is: " << goal_info.perp_distance
+          << ", Pose: " << gp.ToString() << ", LanePose:" << goal_info.perp_point.pos.ToString()
+          << ", LaneID: " << pGoal->pLane->id << " -> Check origin and vector map. " << endl;
+      return 0;
+    }
+    else
+    {
+      WayPoint wp = *pGoal;
+      wp.pos.x = (goalPos.pos.x+pGoal->pos.x)/2.0;
+      wp.pos.y = (goalPos.pos.y+pGoal->pos.y)/2.0;
+      goal_path.push_back(wp);
+      goal_path.push_back(goalPos);
+    }
+  }
+
+  vector<WayPoint*> local_cell_to_delete;
+  WayPoint* pLaneCell = 0;
+  char bPlan = 'A';
+
+  if(all_cell_to_delete)
+    pLaneCell =  PlanningHelpers::BuildPlanningSearchTreeV2(pStart, *pGoal, globalPath, maxPlanningDistance,bEnableLaneChange, *all_cell_to_delete);
+  else
+    pLaneCell =  PlanningHelpers::BuildPlanningSearchTreeV2(pStart, *pGoal, globalPath, maxPlanningDistance,bEnableLaneChange, local_cell_to_delete);
+
+  if(!pLaneCell)
+  {
+    bPlan = 'B';
+    cout << endl << "PlannerH -> Plan (A) Failed, Trying Plan (B)." << endl;
+
+    if(all_cell_to_delete)
+      pLaneCell =  PlanningHelpers::BuildPlanningSearchTreeStraight(pStart, BACKUP_STRAIGHT_PLAN_DISTANCE, *all_cell_to_delete);
+    else
+      pLaneCell =  PlanningHelpers::BuildPlanningSearchTreeStraight(pStart, BACKUP_STRAIGHT_PLAN_DISTANCE, local_cell_to_delete);
+
+    if(!pLaneCell)
+    {
+      bPlan = 'Z';
+      cout << endl << "PlannerH -> Plan (B) Failed, Sorry we Don't have plan (C) This is the END." << endl;
+      return 0;
+    }
+  }
+
+  vector<WayPoint> path;
+  vector<vector<WayPoint> > tempCurrentForwardPathss;
+  PlanningHelpers::TraversePathTreeBackwards(pLaneCell, pStart, globalPath, path, tempCurrentForwardPathss);
+  if(path.size()==0) return 0;
+
+  paths.clear();
+
+  if(bPlan == 'A')
+  {
+    PlanningHelpers::ExtractPlanAlernatives(path, paths);
+  }
+  else if (bPlan == 'B')
+  {
+    paths.push_back(path);
+  }
+
+  //attach start path to beginning of all paths, but goal path to only the path connected to the goal path.
+  for(unsigned int i=0; i< paths.size(); i++ )
+  {
+    paths.at(i).insert(paths.at(i).begin(), start_path.begin(), start_path.end());
+    if(paths.at(i).size() > 0)
+    {
+      //if(hypot(paths.at(i).at(paths.at(i).size()-1).pos.y-goal_info.perp_point.pos.y, paths.at(i).at(paths.at(i).size()-1).pos.x-goal_info.perp_point.pos.x) < 1.5)
+      {
+
+        if(paths.at(i).size() > 0 && goal_path.size() > 0)
+        {
+          goal_path.insert(goal_path.begin(), paths.at(i).end()-5, paths.at(i).end());
+          PlanningHelpers::SmoothPath(goal_path, 0.25, 0.25);
+          PlanningHelpers::FixPathDensity(goal_path, 0.75);
+          PlanningHelpers::SmoothPath(goal_path, 0.25, 0.35);
+          paths.at(i).erase(paths.at(i).end()-5, paths.at(i).end());
+          paths.at(i).insert(paths.at(i).end(), goal_path.begin(), goal_path.end());
+        }
+      }
+    }
+  }
+
+  cout << endl <<"Info: PlannerH -> Plan (" << bPlan << ") Path With Size (" << (int)path.size() << "), MultiPaths No(" << paths.size() << ") Extraction Time : " << endl;
+
+
+  if(path.size()<2)
+  {
+    cout << endl << "Err: PlannerH -> Invalid Path, Car Should Stop." << endl;
+    if(pLaneCell && !all_cell_to_delete)
+      DeleteWaypoints(local_cell_to_delete);
+    return 0 ;
+  }
+
+  if(pLaneCell && !all_cell_to_delete)
+    DeleteWaypoints(local_cell_to_delete);
+
+  double totalPlanningDistance = path.at(path.size()-1).cost;
+  return totalPlanningDistance;
+}
+
+double PlannerH::PredictPlanUsingDP(PlannerHNS::Lane* l, const WayPoint& start, const double& maxPlanningDistance, std::vector<std::vector<WayPoint> >& paths)
+{
+  if(!l)
+  {
+    cout <<endl<< "Err: PredictPlanUsingDP, PlannerH -> Null Lane !!" << endl;
+    return 0;
+  }
+
+  WayPoint carPos = start;
+  vector<vector<WayPoint> > tempCurrentForwardPathss;
+  vector<WayPoint*> all_cell_to_delete;
+  vector<int> globalPath;
+
+  RelativeInfo info;
+  PlanningHelpers::GetRelativeInfo(l->points, carPos, info);
+  WayPoint closest_p = l->points.at(info.iBack);
+  WayPoint* pStartWP = &l->points.at(info.iBack);
+
+  if(distance2points(closest_p.pos, carPos.pos) > 8)
+  {
+    cout <<endl<< "Err: PredictiveDP PlannerH -> Distance to Lane is: " << distance2points(closest_p.pos, carPos.pos)
+         << ", Pose: " << carPos.pos.ToString() << ", LanePose:" << closest_p.pos.ToString()
+         << ", LaneID: " << l->id << " -> Check origin and vector map. " << endl;
+    return 0;
+  }
+
+  vector<WayPoint*> pLaneCells;
+  int nPaths =  PlanningHelpers::PredictiveDP(pStartWP, maxPlanningDistance, all_cell_to_delete, pLaneCells);
+
+  if(nPaths==0)
+  {
+    cout <<endl<< "Err PlannerH -> Null Tree Head." << endl;
+    return 0;
+  }
+
+  double totalPlanDistance  = 0;
+  for(unsigned int i = 0; i< pLaneCells.size(); i++)
+  {
+    std::vector<WayPoint> path;
+    PlanningHelpers::TraversePathTreeBackwards(pLaneCells.at(i), pStartWP, globalPath, path, tempCurrentForwardPathss);
+    if(path.size()>0)
+      totalPlanDistance+= path.at(path.size()-1).cost;
+
+    PlanningHelpers::FixPathDensity(path, 0.5);
+    PlanningHelpers::SmoothPath(path, 0.3 , 0.3,0.1);
+    PlanningHelpers::CalcAngleAndCost(path);
+
+    paths.push_back(path);
+  }
+
+  DeleteWaypoints(all_cell_to_delete);
+
+  return totalPlanDistance;
+}
+
+double PlannerH::PredictTrajectoriesUsingDP(const WayPoint& startPose, std::vector<WayPoint*> closestWPs, const double& maxPlanningDistance, std::vector<std::vector<WayPoint> >& paths, const bool& bFindBranches , const bool bDirectionBased, const bool pathDensity)
+{
+  vector<vector<WayPoint> > tempCurrentForwardPathss;
+  vector<WayPoint*> all_cell_to_delete;
+  vector<int> globalPath;
+
+  vector<WayPoint*> pLaneCells;
+  vector<int> unique_lanes;
+  std::vector<WayPoint> path;
+  for(unsigned int j = 0 ; j < closestWPs.size(); j++)
+  {
+    pLaneCells.clear();
+    int nPaths =  PlanningHelpers::PredictiveIgnorIdsDP(closestWPs.at(j), maxPlanningDistance, all_cell_to_delete, pLaneCells, unique_lanes);
+    for(unsigned int i = 0; i< pLaneCells.size(); i++)
+    {
+      path.clear();
+      PlanningHelpers::TraversePathTreeBackwards(pLaneCells.at(i), closestWPs.at(j), globalPath, path, tempCurrentForwardPathss);
+
+      for(unsigned int k = 0; k< path.size(); k++)
+      {
+        bool bFoundLaneID = false;
+        for(unsigned int l_id = 0 ; l_id < unique_lanes.size(); l_id++)
+        {
+          if(path.at(k).laneId == unique_lanes.at(l_id))
+          {
+            bFoundLaneID = true;
+            break;
+          }
+        }
+
+        if(!bFoundLaneID)
+          unique_lanes.push_back(path.at(k).laneId);
+      }
+
+      if(path.size()>0)
+      {
+        path.insert(path.begin(), startPose);
+        if(!bDirectionBased)
+          path.at(0).pos.a = path.at(1).pos.a;
+
+        path.at(0).beh_state = path.at(1).beh_state = PlannerHNS::BEH_FORWARD_STATE;
+        path.at(0).laneId = path.at(1).laneId;
+
+        PlanningHelpers::FixPathDensity(path, pathDensity);
+        PlanningHelpers::SmoothPath(path,0.4,0.3,0.1);
+        PlanningHelpers::CalcAngleAndCost(path);
+        paths.push_back(path);
+      }
+    }
+  }
+
+  if(bDirectionBased && bFindBranches)
+  {
+    WayPoint p1, p2;
+    if(paths.size()> 0 && paths.at(0).size() > 0)
+      p2 = p1 = paths.at(0).at(0);
+    else
+      p2 = p1 = startPose;
+
+    double branch_length = maxPlanningDistance*0.5;
+
+    p2.pos.y = p1.pos.y + branch_length*0.4*sin(p1.pos.a);
+    p2.pos.x = p1.pos.x + branch_length*0.4*cos(p1.pos.a);
+
+    vector<WayPoint> l_branch;
+    vector<WayPoint> r_branch;
+
+    PlanningHelpers::CreateManualBranchFromTwoPoints(p1, p2, branch_length, FORWARD_RIGHT_DIR,r_branch);
+    PlanningHelpers::CreateManualBranchFromTwoPoints(p1, p2, branch_length, FORWARD_LEFT_DIR, l_branch);
+
+    PlanningHelpers::FixPathDensity(l_branch, pathDensity);
+    PlanningHelpers::SmoothPath(l_branch,0.4,0.3,0.1);
+    PlanningHelpers::CalcAngleAndCost(l_branch);
+
+    PlanningHelpers::FixPathDensity(r_branch, pathDensity);
+    PlanningHelpers::SmoothPath(r_branch,0.4,0.3,0.1);
+    PlanningHelpers::CalcAngleAndCost(r_branch);
+
+    paths.push_back(l_branch);
+    paths.push_back(r_branch);
+  }
+
+  DeleteWaypoints(all_cell_to_delete);
+
+  return paths.size();
+}
+
+double PlannerH::PredictPlanUsingDP(const WayPoint& startPose, WayPoint* closestWP, const double& maxPlanningDistance, std::vector<std::vector<WayPoint> >& paths, const bool& bFindBranches)
+{
+  if(!closestWP || !closestWP->pLane)
+  {
+    cout <<endl<< "Err: PredictPlanUsingDP, PlannerH -> Null Lane !!" << endl;
+    return 0;
+  }
+
+  vector<vector<WayPoint> > tempCurrentForwardPathss;
+  vector<WayPoint*> all_cell_to_delete;
+  vector<int> globalPath;
+
+  vector<WayPoint*> pLaneCells;
+  int nPaths =  PlanningHelpers::PredictiveDP(closestWP, maxPlanningDistance, all_cell_to_delete, pLaneCells);
+
+  if(nPaths==0)
+  {
+    cout <<endl<< "Err PlannerH -> Null Tree Head." << endl;
+    return 0;
+  }
+
+  double totalPlanDistance  = 0;
+  for(unsigned int i = 0; i< pLaneCells.size(); i++)
+  {
+    std::vector<WayPoint> path;
+    PlanningHelpers::TraversePathTreeBackwards(pLaneCells.at(i), closestWP, globalPath, path, tempCurrentForwardPathss);
+    if(path.size()>0)
+    {
+      totalPlanDistance+= path.at(path.size()-1).cost;
+      path.insert(path.begin(), startPose);
+      //path.at(0).pos.a = path.at(1).pos.a;;
+    }
+
+
+    PlanningHelpers::FixPathDensity(path, 0.5);
+    PlanningHelpers::SmoothPath(path, 0.3 , 0.3,0.1);
+    paths.push_back(path);
+
+    if(bFindBranches)
+    {
+      int max_branch_index = path.size() > 5 ? 5 : path.size();
+      vector<WayPoint> l_branch;
+      vector<WayPoint> r_branch;
+      l_branch.insert(l_branch.begin(), path.begin(), path.begin()+5);
+      r_branch.insert(r_branch.begin(), path.begin(), path.begin()+5);
+
+      PlanningHelpers::CreateManualBranch(r_branch, 0, FORWARD_RIGHT_DIR);
+      PlanningHelpers::CreateManualBranch(l_branch, 0, FORWARD_LEFT_DIR);
+
+      paths.push_back(l_branch);
+      paths.push_back(r_branch);
+    }
+  }
+
+  DeleteWaypoints(all_cell_to_delete);
+
+  return totalPlanDistance;
+}
+
+void PlannerH::DeleteWaypoints(vector<WayPoint*>& wps)
+{
+  for(unsigned int i=0; i<wps.size(); i++)
+  {
+    if(wps.at(i))
+    {
+      delete wps.at(i);
+      wps.at(i) = 0;
+    }
+  }
+  wps.clear();
+}
+
+}

src1/planning/op_planner/PlanningHelpers.cpp

@@ -0,0 +1,2801 @@
+
+/// \file PlanningHelpers.cpp
+/// \brief Helper functions for planning algorithms
+/// \author Hatem Darweesh
+/// \date Jun 16, 2016
+
+
+#include "op_planner/PlanningHelpers.h"
+#include "op_planner/MatrixOperations.h"
+#include <string>
+#include <float.h>
+
+using namespace UtilityHNS;
+using namespace std;
+
+namespace PlannerHNS
+{
+
+std::vector<std::pair<GPSPoint, GPSPoint> > PlanningHelpers::m_TestingClosestPoint;
+
+PlanningHelpers::PlanningHelpers()
+{
+}
+
+PlanningHelpers::~PlanningHelpers()
+{
+}
+
+bool PlanningHelpers::GetRelativeInfoRange(const std::vector<std::vector<WayPoint> >& trajectories, const WayPoint& p,const double& searchDistance, RelativeInfo& info)
+{
+  if(trajectories.size() == 0) return false;
+
+  vector<RelativeInfo> infos;
+  for(unsigned int i=0; i < trajectories.size(); i++)
+  {
+    RelativeInfo info_item;
+    GetRelativeInfo(trajectories.at(i), p, info_item);
+    double angle_diff = UtilityH::AngleBetweenTwoAnglesPositive(info_item.perp_point.pos.a, p.pos.a)*RAD2DEG;
+    if(angle_diff < 75)
+    {
+      info_item.iGlobalPath = i;
+      infos.push_back(info_item);
+    }
+  }
+
+  if(infos.size() == 0)
+    return false;
+  else if(infos.size() == 1)
+  {
+    info = infos.at(0);
+    return true;
+  }
+
+  double minCost = DBL_MAX;
+  int min_index = 0;
+
+  for(unsigned int i=0 ; i< infos.size(); i++)
+  {
+    if(searchDistance > 0)
+    {
+      double laneChangeCost = trajectories.at(infos.at(i).iGlobalPath).at(infos.at(i).iFront).laneChangeCost;
+      if(fabs(infos.at(i).perp_distance) < searchDistance && laneChangeCost < minCost)
+      {
+        min_index = i;
+        minCost = laneChangeCost;
+      }
+    }
+    else
+    {
+      if(fabs(infos.at(i).perp_distance) < minCost)
+      {
+        min_index = i;
+        minCost = infos.at(i).perp_distance;
+      }
+    }
+  }
+
+  info = infos.at(min_index);
+
+  return true;
+}
+
+
+bool PlanningHelpers::GetRelativeInfo(const std::vector<WayPoint>& trajectory, const WayPoint& p, RelativeInfo& info, const int& prevIndex )
+{
+  if(trajectory.size() < 2) return false;
+
+  WayPoint p0, p1;
+  if(trajectory.size()==2)
+  {
+    p0 = trajectory.at(0);
+    p1 = WayPoint((trajectory.at(0).pos.x+trajectory.at(1).pos.x)/2.0,
+            (trajectory.at(0).pos.y+trajectory.at(1).pos.y)/2.0,
+            (trajectory.at(0).pos.z+trajectory.at(1).pos.z)/2.0, trajectory.at(0).pos.a);
+    info.iFront = 1;
+    info.iBack = 0;
+  }
+  else
+  {
+    info.iFront = GetClosestNextPointIndexFast(trajectory, p, prevIndex);
+//    WayPoint p2 = p;
+//    int old_index = GetClosestNextPointIndex(trajectory, p2, prevIndex);
+//    if(old_index != info.iFront)
+//      cout << " Alert Alert !!!! fast: " << info.iFront << ", slow: " << old_index  << endl;
+
+    if(info.iFront > 0)
+      info.iBack = info.iFront -1;
+    else
+      info.iBack = 0;
+
+    if(info.iFront == 0)
+    {
+      p0 = trajectory.at(info.iFront);
+      p1 = trajectory.at(info.iFront+1);
+    }
+    else if(info.iFront > 0 && info.iFront < trajectory.size()-1)
+    {
+      p0 = trajectory.at(info.iFront-1);
+      p1 = trajectory.at(info.iFront);
+    }
+    else
+    {
+      p0 = trajectory.at(info.iFront-1);
+      p1 = WayPoint((p0.pos.x+trajectory.at(info.iFront).pos.x)/2.0, (p0.pos.y+trajectory.at(info.iFront).pos.y)/2.0, (p0.pos.z+trajectory.at(info.iFront).pos.z)/2.0, p0.pos.a);
+    }
+  }
+
+  WayPoint prevWP = p0;
+  Mat3 rotationMat(-p1.pos.a);
+  Mat3 translationMat(-p.pos.x, -p.pos.y);
+  Mat3 invRotationMat(p1.pos.a);
+  Mat3 invTranslationMat(p.pos.x, p.pos.y);
+
+  p0.pos = translationMat*p0.pos;
+  p0.pos = rotationMat*p0.pos;
+
+  p1.pos = translationMat*p1.pos;
+  p1.pos = rotationMat*p1.pos;
+
+  double m = (p1.pos.y-p0.pos.y)/(p1.pos.x-p0.pos.x);
+  info.perp_distance = p1.pos.y - m*p1.pos.x; // solve for x = 0
+
+  if(std::isnan(info.perp_distance) || std::isinf(info.perp_distance)) info.perp_distance = 0;
+
+  info.to_front_distance = fabs(p1.pos.x); // distance on the x axes
+
+
+  info.perp_point = p1;
+  info.perp_point.pos.x = 0; // on the same y axis of the car
+  info.perp_point.pos.y = info.perp_distance; //perp distance between the car and the trajectory
+
+  info.perp_point.pos = invRotationMat  * info.perp_point.pos;
+  info.perp_point.pos = invTranslationMat  * info.perp_point.pos;
+
+  info.from_back_distance = hypot(info.perp_point.pos.y - prevWP.pos.y, info.perp_point.pos.x - prevWP.pos.x);
+
+  info.angle_diff = UtilityH::AngleBetweenTwoAnglesPositive(p1.pos.a, p.pos.a)*RAD2DEG;
+
+  return true;
+}
+
+bool PlanningHelpers::GetRelativeInfoLimited(const std::vector<WayPoint>& trajectory, const WayPoint& p, RelativeInfo& info, const int& prevIndex )
+{
+  if(trajectory.size() < 2) return false;
+
+  WayPoint p0, p1;
+
+  if(trajectory.size()==2)
+  {
+    vector<WayPoint> _trajectory;
+    p0 = trajectory.at(0);
+    p1 = p0;
+    p1 = WayPoint((trajectory.at(0).pos.x+trajectory.at(1).pos.x)/2.0,
+            (trajectory.at(0).pos.y+trajectory.at(1).pos.y)/2.0,
+            (trajectory.at(0).pos.z+trajectory.at(1).pos.z)/2.0, trajectory.at(0).pos.a);
+    _trajectory.push_back(p0);
+    _trajectory.push_back(p1);
+    _trajectory.push_back(trajectory.at(1));
+
+    info.iFront = GetClosestNextPointIndexFast(_trajectory, p, prevIndex);
+    if(info.iFront > 0)
+      info.iBack = info.iFront -1;
+    else
+      info.iBack = 0;
+
+    if(info.iFront == 0)
+    {
+      p0 = _trajectory.at(info.iFront);
+      p1 = _trajectory.at(info.iFront+1);
+    }
+    else if(info.iFront > 0 && info.iFront < _trajectory.size()-1)
+    {
+      p0 = _trajectory.at(info.iFront-1);
+      p1 = _trajectory.at(info.iFront);
+    }
+    else
+    {
+      p0 = _trajectory.at(info.iFront-1);
+      p1 = WayPoint((p0.pos.x+_trajectory.at(info.iFront).pos.x)/2.0, (p0.pos.y+_trajectory.at(info.iFront).pos.y)/2.0, (p0.pos.z+_trajectory.at(info.iFront).pos.z)/2.0, p0.pos.a);
+    }
+
+    WayPoint prevWP = p0;
+    Mat3 rotationMat(-p1.pos.a);
+    Mat3 translationMat(-p.pos.x, -p.pos.y);
+    Mat3 invRotationMat(p1.pos.a);
+    Mat3 invTranslationMat(p.pos.x, p.pos.y);
+
+    p0.pos = translationMat*p0.pos;
+    p0.pos = rotationMat*p0.pos;
+
+    p1.pos = translationMat*p1.pos;
+    p1.pos = rotationMat*p1.pos;
+
+    double m = (p1.pos.y-p0.pos.y)/(p1.pos.x-p0.pos.x);
+    info.perp_distance = p1.pos.y - m*p1.pos.x; // solve for x = 0
+
+    if(std::isnan(info.perp_distance) || std::isinf(info.perp_distance)) info.perp_distance = 0;
+
+    info.to_front_distance = fabs(p1.pos.x); // distance on the x axes
+
+    info.perp_point = p1;
+    info.perp_point.pos.x = 0; // on the same y axis of the car
+    info.perp_point.pos.y = info.perp_distance; //perp distance between the car and the _trajectory
+
+    info.perp_point.pos = invRotationMat  * info.perp_point.pos;
+    info.perp_point.pos = invTranslationMat  * info.perp_point.pos;
+
+    info.from_back_distance = hypot(info.perp_point.pos.y - prevWP.pos.y, info.perp_point.pos.x - prevWP.pos.x);
+
+    info.angle_diff = UtilityH::AngleBetweenTwoAnglesPositive(p1.pos.a, p.pos.a)*RAD2DEG;
+
+    info.bAfter = false;
+    info.bBefore = false;
+
+    if(info.iFront == 0)
+    {
+      info.bBefore = true;
+    }
+    else if(info.iFront == _trajectory.size()-1)
+    {
+      int s = _trajectory.size();
+      double angle_befor_last = UtilityH::FixNegativeAngle(atan2(_trajectory.at(s-2).pos.y - _trajectory.at(s-1).pos.y, _trajectory.at(s-2).pos.x - _trajectory.at(s-1).pos.x));
+      double angle_from_perp = UtilityH::FixNegativeAngle(atan2(info.perp_point.pos.y - _trajectory.at(s-1).pos.y, info.perp_point.pos.x - _trajectory.at(s-1).pos.x));
+      double diff_last_perp = UtilityH::AngleBetweenTwoAnglesPositive(angle_befor_last, angle_from_perp);
+      info.after_angle = diff_last_perp;
+      if(diff_last_perp > M_PI_2)
+      {
+        info.bAfter = true;
+      }
+
+    }
+  }
+  else
+  {
+    info.iFront = GetClosestNextPointIndexFast(trajectory, p, prevIndex);
+    if(info.iFront > 0)
+      info.iBack = info.iFront -1;
+    else
+      info.iBack = 0;
+
+    if(info.iFront == 0)
+    {
+      p0 = trajectory.at(info.iFront);
+      p1 = trajectory.at(info.iFront+1);
+    }
+    else if(info.iFront > 0 && info.iFront < trajectory.size()-1)
+    {
+      p0 = trajectory.at(info.iFront-1);
+      p1 = trajectory.at(info.iFront);
+    }
+    else
+    {
+      p0 = trajectory.at(info.iFront-1);
+      p1 = WayPoint((p0.pos.x+trajectory.at(info.iFront).pos.x)/2.0, (p0.pos.y+trajectory.at(info.iFront).pos.y)/2.0, (p0.pos.z+trajectory.at(info.iFront).pos.z)/2.0, p0.pos.a);
+    }
+
+    WayPoint prevWP = p0;
+    Mat3 rotationMat(-p1.pos.a);
+    Mat3 translationMat(-p.pos.x, -p.pos.y);
+    Mat3 invRotationMat(p1.pos.a);
+    Mat3 invTranslationMat(p.pos.x, p.pos.y);
+
+    p0.pos = translationMat*p0.pos;
+    p0.pos = rotationMat*p0.pos;
+
+    p1.pos = translationMat*p1.pos;
+    p1.pos = rotationMat*p1.pos;
+
+    double m = (p1.pos.y-p0.pos.y)/(p1.pos.x-p0.pos.x);
+    info.perp_distance = p1.pos.y - m*p1.pos.x; // solve for x = 0
+
+    if(std::isnan(info.perp_distance) || std::isinf(info.perp_distance)) info.perp_distance = 0;
+
+    info.to_front_distance = fabs(p1.pos.x); // distance on the x axes
+
+    info.perp_point = p1;
+    info.perp_point.pos.x = 0; // on the same y axis of the car
+    info.perp_point.pos.y = info.perp_distance; //perp distance between the car and the trajectory
+
+    info.perp_point.pos = invRotationMat  * info.perp_point.pos;
+    info.perp_point.pos = invTranslationMat  * info.perp_point.pos;
+
+    info.from_back_distance = hypot(info.perp_point.pos.y - prevWP.pos.y, info.perp_point.pos.x - prevWP.pos.x);
+
+    info.angle_diff = UtilityH::AngleBetweenTwoAnglesPositive(p1.pos.a, p.pos.a)*RAD2DEG;
+
+    info.bAfter = false;
+    info.bBefore = false;
+
+    if(info.iFront == 0)
+    {
+      info.bBefore = true;
+    }
+    else if(info.iFront == trajectory.size()-1)
+    {
+      int s = trajectory.size();
+      double angle_befor_last = UtilityH::FixNegativeAngle(atan2(trajectory.at(s-2).pos.y - trajectory.at(s-1).pos.y, trajectory.at(s-2).pos.x - trajectory.at(s-1).pos.x));
+      double angle_from_perp = UtilityH::FixNegativeAngle(atan2(info.perp_point.pos.y - trajectory.at(s-1).pos.y, info.perp_point.pos.x - trajectory.at(s-1).pos.x));
+      double diff_last_perp = UtilityH::AngleBetweenTwoAnglesPositive(angle_befor_last, angle_from_perp);
+      info.after_angle = diff_last_perp;
+      if(diff_last_perp > M_PI_2)
+      {
+        info.bAfter = true;
+      }
+
+    }
+  }
+
+  return true;
+}
+
+bool PlanningHelpers::GetThreePointsInfo(const WayPoint& p0, const WayPoint& p1, const WayPoint& p2, WayPoint& perp_p, double& long_d, double lat_d)
+{
+
+  if(p0.pos.x == p1.pos.x && p0.pos.y == p1.pos.y) return false;
+  if(p0.pos.x == p2.pos.x && p0.pos.y == p2.pos.y) return false;
+  if(p1.pos.x == p2.pos.x && p1.pos.y == p2.pos.y) return false;
+
+  perp_p = p1;
+  WayPoint first_p = p0;
+  double angle_x = atan2(p1.pos.y-p0.pos.y, p1.pos.x-p0.pos.x);
+
+  Mat3 rotationMat(-angle_x);
+  Mat3 translationMat(-p2.pos.x, -p2.pos.y);
+  Mat3 invRotationMat(angle_x);
+  Mat3 invTranslationMat(p2.pos.x, p2.pos.y);
+
+  first_p.pos = translationMat*first_p.pos;
+  first_p.pos = rotationMat*first_p.pos;
+
+  perp_p.pos = translationMat*perp_p.pos;
+  perp_p.pos = rotationMat*perp_p.pos;
+
+  if(perp_p.pos.x-first_p.pos.x == 0) return false;
+
+  double m = (perp_p.pos.y-first_p.pos.y)/(perp_p.pos.x-first_p.pos.x);
+  lat_d = perp_p.pos.y - m*perp_p.pos.x; // solve for x = 0
+
+  if(std::isnan(lat_d) || std::isinf(lat_d)) return false;
+
+  if(perp_p.pos.x < 0)
+    return false;
+
+  perp_p.pos.x = 0; // on the same y axis of the car
+  perp_p.pos.y = lat_d; //perp distance between the car and the trajectory
+
+  perp_p.pos = invRotationMat  * perp_p.pos;
+  perp_p.pos = invTranslationMat  * perp_p.pos;
+
+  long_d = hypot(perp_p.pos.y - first_p.pos.y, perp_p.pos.x - first_p.pos.x);
+
+  return true;
+}
+
+WayPoint PlanningHelpers::GetFollowPointOnTrajectory(const std::vector<WayPoint>& trajectory, const RelativeInfo& init_p, const double& distance, unsigned int& point_index)
+{
+  WayPoint follow_point;
+
+  if(trajectory.size()==0) return follow_point;
+
+  //condition 1, if far behind the first point on the trajectory
+  int local_i = init_p.iFront;
+
+  if(init_p.iBack == 0 && init_p.iBack == init_p.iFront && init_p.from_back_distance > distance)
+  {
+    follow_point = trajectory.at(init_p.iFront);
+    follow_point.pos.x = init_p.perp_point.pos.x + distance * cos(follow_point.pos.a);
+    follow_point.pos.y = init_p.perp_point.pos.y + distance * sin(follow_point.pos.a);
+  }
+  //condition 2, if far after the last point on the trajectory
+  else if(init_p.iFront == (int)trajectory.size() - 1)
+  {
+    follow_point = trajectory.at(init_p.iFront);
+    follow_point.pos.x = init_p.perp_point.pos.x + distance * cos(follow_point.pos.a);
+    follow_point.pos.y = init_p.perp_point.pos.y + distance * sin(follow_point.pos.a);
+  }
+  else
+  {
+    double d = init_p.to_front_distance;
+    while(local_i < (int)trajectory.size()-1 && d < distance)
+    {
+      local_i++;
+      d += hypot(trajectory.at(local_i).pos.y - trajectory.at(local_i-1).pos.y, trajectory.at(local_i).pos.x - trajectory.at(local_i-1).pos.x);
+    }
+
+    double d_part = distance - d;
+
+    follow_point = trajectory.at(local_i);
+    follow_point.pos.x = follow_point.pos.x + d_part * cos(follow_point.pos.a);
+    follow_point.pos.y = follow_point.pos.y + d_part * sin(follow_point.pos.a);
+  }
+
+  point_index = local_i;
+
+  return follow_point;
+}
+
+double PlanningHelpers::GetExactDistanceOnTrajectory(const std::vector<WayPoint>& trajectory, const RelativeInfo& p1,const RelativeInfo& p2)
+{
+  if(trajectory.size() == 0) return 0;
+
+  if(p2.iFront == p1.iFront && p2.iBack == p1.iBack)
+  {
+    return p2.to_front_distance - p1.to_front_distance;
+  }
+  else if(p2.iBack >= p1.iFront)
+  {
+    double d_on_path = p1.to_front_distance + p2.from_back_distance;
+    for(int i = p1.iFront; i < p2.iBack; i++)
+      d_on_path += hypot(trajectory.at(i+1).pos.y - trajectory.at(i).pos.y, trajectory.at(i+1).pos.x - trajectory.at(i).pos.x);
+
+    return d_on_path;
+  }
+  else if(p2.iFront <= p1.iBack)
+  {
+    double d_on_path = p1.from_back_distance + p2.to_front_distance;
+    for(int i = p2.iFront; i < p1.iBack; i++)
+      d_on_path += hypot(trajectory.at(i+1).pos.y - trajectory.at(i).pos.y, trajectory.at(i+1).pos.x - trajectory.at(i).pos.x);
+
+    return -d_on_path;
+  }
+  else
+  {
+    return 0;
+  }
+}
+
+int PlanningHelpers::GetClosestNextPointIndex_obsolete(const vector<WayPoint>& trajectory, const WayPoint& p,const int& prevIndex )
+{
+  if(trajectory.size() == 0 || prevIndex < 0) return 0;
+
+  double d = 0, minD = DBL_MAX;
+  int min_index  = prevIndex;
+
+  for(unsigned int i=prevIndex; i< trajectory.size(); i++)
+  {
+    d  = distance2pointsSqr(trajectory.at(i).pos, p.pos);
+    if(d < minD)
+    {
+      min_index = i;
+      minD = d;
+    }
+  }
+
+//  cout << "Slow=> Start: " << 0;
+//  cout << ", End: " << trajectory.size();
+//  cout << ", Minimum Before: " << min_index;
+
+  if(min_index < (int)trajectory.size()-2)
+  {
+    GPSPoint curr, next;
+    curr = trajectory.at(min_index).pos;
+    next = trajectory.at(min_index+1).pos;
+    GPSPoint v_1(p.pos.x - curr.x   ,p.pos.y - curr.y,0,0);
+    double norm1 = pointNorm(v_1);
+    GPSPoint v_2(next.x - curr.x,next.y - curr.y,0,0);
+    double norm2 = pointNorm(v_2);
+    double dot_pro = v_1.x*v_2.x + v_1.y*v_2.y;
+    double a = UtilityH::FixNegativeAngle(acos(dot_pro/(norm1*norm2)));
+    if(a <= M_PI_2)
+      min_index = min_index+1;
+  }
+
+//  cout << ", Minimum After: " << min_index << ", Big O: " << trajectory.size() << endl;
+
+  return min_index;
+}
+
+int PlanningHelpers::GetClosestNextPointIndexFastV2(const vector<WayPoint>& trajectory, const WayPoint& p,const int& prevIndex)
+{
+
+  int size = (int)trajectory.size();
+
+    if(size < 2 || prevIndex < 0) return 0;
+
+    double d = 0, minD = DBL_MAX;
+
+
+    double resolution = hypot(trajectory[1].pos.y -trajectory[0].pos.y , trajectory[1].pos.x -trajectory[0].pos.x);
+    double d_to_zero = hypot(p.pos.y -trajectory[0].pos.y , p.pos.x - trajectory[0].pos.x);
+    double d_to_size = hypot(trajectory[size-1].pos.y - p.pos.y , trajectory[size-1].pos.x - p.pos.x);
+
+    int iStart = d_to_zero / resolution;
+    WayPoint perp_p;
+    double lat_d = 0;
+    double long_d = 0;
+
+    if(iStart > 0 && iStart < size-1 &&  GetThreePointsInfo(trajectory.at(0), trajectory.at(iStart), p, perp_p, long_d, lat_d))
+    {
+  //    m_TestingClosestPoint.push_back(make_pair(trajectory.at(0).pos, p.pos));
+  //    m_TestingClosestPoint.push_back(make_pair(trajectory.at(iStart).pos, p.pos));
+      iStart = long_d / resolution;
+    }
+
+    if(iStart>0)
+      iStart--;
+
+    int iEnd = size - (d_to_size / resolution);
+
+    if(iEnd >= 0 && iEnd < size-2 &&  GetThreePointsInfo(trajectory.at(size-1), trajectory.at(iEnd), p, perp_p, long_d, lat_d))
+    {
+  //    m_TestingClosestPoint.push_back(make_pair(trajectory.at(size-1).pos, p.pos));
+  //    m_TestingClosestPoint.push_back(make_pair(trajectory.at(iEnd).pos, p.pos));
+      iEnd = size - (long_d / resolution);
+    }
+
+    if(iEnd < size-1)
+      iEnd++;
+
+  //  cout << "Fast=>";
+  //  cout << " Start: " << iStart;
+  //  cout << ", End: " << iEnd;
+
+    double d_from_start = d_to_zero;
+    if(iStart < size)
+      d_from_start = hypot(trajectory[iStart].pos.y - p.pos.y , trajectory[iStart].pos.x - p.pos.x);
+
+    double d_from_end = d_to_size;
+    if(iEnd >= 0)
+      d_from_end = hypot(trajectory[iEnd].pos.y - p.pos.y , trajectory[iEnd].pos.x - p.pos.x);
+
+    if(iStart >= size && iEnd < 0)
+    {
+      if(d_to_zero < d_to_size)
+      {
+        iStart = 0;
+        iEnd = size/2 -1;
+      }
+      else
+      {
+        iStart = size/2;
+        iEnd = size - 1;
+      }
+    }
+    else
+    {
+      if(iStart >=size || (d_from_start > d_to_zero))
+        iStart = 0;
+
+      if(iEnd < 0 || (d_from_end > d_to_size))
+        iEnd = size-1;
+    }
+
+    if(iStart > iEnd)
+      iEnd = size-1;
+
+    int min_index  =  iStart;
+
+    int ncout = 0;
+    for(int i=iStart; i<= iEnd; i++)
+    {
+      d  = distance2pointsSqr(trajectory[i].pos, p.pos);
+      if(d < minD)
+      {
+        min_index = i;
+        minD = d;
+      }
+      ncout++;
+    }
+
+  //  cout << ", Minimum Before: " << min_index;
+
+    if(min_index < size-2)
+    {
+      GPSPoint curr, next;
+      curr = trajectory[min_index].pos;
+      next = trajectory[min_index+1].pos;
+      GPSPoint v_1(p.pos.x - curr.x   ,p.pos.y - curr.y,0,0);
+      double norm1 = pointNorm(v_1);
+      GPSPoint v_2(next.x - curr.x,next.y - curr.y,0,0);
+      double norm2 = pointNorm(v_2);
+      double dot_pro = v_1.x*v_2.x + v_1.y*v_2.y;
+      double a = UtilityH::FixNegativeAngle(acos(dot_pro/(norm1*norm2)));
+      if(a <= M_PI_2)
+        min_index = min_index+1;
+    }
+
+  //  m_TestingClosestPoint.push_back(make_pair(trajectory.at(min_index).pos, p.pos));
+
+  //  cout << ", Minimum After: " << min_index << ", Big O: " << ncout << endl;
+  //  cout << "d_zero: " << d_to_zero << ", d_start: " << d_from_start << endl;
+  //  cout << "d_size: " << d_to_size << ", d_end: " << d_from_end << endl;
+    return min_index;
+
+
+}
+
+int PlanningHelpers::GetClosestNextPointIndexFast(const vector<WayPoint>& trajectory, const WayPoint& p,const int& prevIndex )
+{
+  int size = (int)trajectory.size();
+
+    if(size < 2 || prevIndex < 0) return 0;
+
+    double d = 0, minD = DBL_MAX;
+    int min_index  = prevIndex;
+    int iStart = prevIndex;
+    int iEnd = size;
+    double resolution = hypot(trajectory[1].pos.y -trajectory[0].pos.y , trajectory[1].pos.x -trajectory[0].pos.x);
+
+    //divide every 5 meters
+    int skip_factor = 5;
+    if(resolution > skip_factor)
+      resolution = skip_factor;
+
+
+    int skip = 1;
+    if(resolution > 0)
+      skip = skip_factor/resolution;
+
+    for(int i=0; i< size; i+=skip)
+    {
+      if(i+skip/2 < size)
+        d  = (distance2pointsSqr(trajectory[i].pos, p.pos) + distance2pointsSqr(trajectory[i+skip/2].pos, p.pos))/2.0;
+      else
+        d  = distance2pointsSqr(trajectory[i].pos, p.pos);
+      if(d < minD)
+      {
+        iStart = i-skip;
+        iEnd = i+skip;
+        minD = d;
+        min_index = i;
+      }
+    }
+
+    if((size - skip/2 - 1) > 0)
+      d  = (distance2pointsSqr(trajectory[size-1].pos, p.pos) + distance2pointsSqr(trajectory[size - skip/2 -1 ].pos, p.pos))/2.0;
+    else
+      d  = distance2pointsSqr(trajectory[size-1].pos, p.pos);
+
+    if(d < minD)
+    {
+      iStart = size-skip;
+      iEnd = size+skip;
+      minD = d;
+      min_index = size-1;
+    }
+
+    if(iStart < 0) iStart = 0;
+    if(iEnd >= size) iEnd = size -1;
+
+    for(int i=iStart; i< iEnd; i++)
+    {
+      d  = distance2pointsSqr(trajectory[i].pos, p.pos);
+      if(d < minD)
+      {
+        min_index = i;
+        minD = d;
+      }
+    }
+
+    if(min_index < size-1)
+    {
+      GPSPoint curr, next;
+      curr = trajectory[min_index].pos;
+      next = trajectory[min_index+1].pos;
+      GPSPoint v_1(p.pos.x - curr.x   ,p.pos.y - curr.y,0,0);
+      double norm1 = pointNorm(v_1);
+      GPSPoint v_2(next.x - curr.x,next.y - curr.y,0,0);
+      double norm2 = pointNorm(v_2);
+      double dot_pro = v_1.x*v_2.x + v_1.y*v_2.y;
+      double a = UtilityH::FixNegativeAngle(acos(dot_pro/(norm1*norm2)));
+      if(a <= M_PI_2)
+        min_index = min_index+1;
+    }
+
+    //m_TestingClosestPoint.push_back(make_pair(trajectory.at(min_index).pos, p.pos));
+
+    return min_index;
+}
+
+int PlanningHelpers::GetClosestNextPointIndexDirectionFast(const vector<WayPoint>& trajectory, const WayPoint& p,const int& prevIndex )
+{
+  int size = (int)trajectory.size();
+
+  if(size < 2 || prevIndex < 0) return 0;
+
+  double d = 0, minD = DBL_MAX;
+  int min_index  = prevIndex;
+
+  for(unsigned int i=prevIndex; i< size; i++)
+  {
+    d  = distance2pointsSqr(trajectory[i].pos, p.pos);
+    double angle_diff = UtilityH::AngleBetweenTwoAnglesPositive(trajectory[i].pos.a, p.pos.a)*RAD2DEG;
+
+    if(d < minD && angle_diff < 45)
+    {
+      min_index = i;
+      minD = d;
+    }
+  }
+
+  if(min_index < (int)trajectory.size()-2)
+  {
+    GPSPoint curr, next;
+    curr = trajectory.at(min_index).pos;
+    next = trajectory.at(min_index+1).pos;
+    GPSPoint v_1(p.pos.x - curr.x   ,p.pos.y - curr.y,0,0);
+    double norm1 = pointNorm(v_1);
+    GPSPoint v_2(next.x - curr.x,next.y - curr.y,0,0);
+    double norm2 = pointNorm(v_2);
+    double dot_pro = v_1.x*v_2.x + v_1.y*v_2.y;
+    double a = UtilityH::FixNegativeAngle(acos(dot_pro/(norm1*norm2)));
+    if(a <= M_PI_2)
+      min_index = min_index+1;
+  }
+
+  return min_index;
+}
+
+int PlanningHelpers::GetClosestPointIndex_obsolete(const vector<WayPoint>& trajectory, const WayPoint& p,const int& prevIndex )
+{
+  if(trajectory.size() == 0 || prevIndex < 0) return 0;
+
+  double d = 0, minD = DBL_MAX;
+  int min_index  = prevIndex;
+
+  for(unsigned int i=prevIndex; i< trajectory.size(); i++)
+  {
+    d  = distance2pointsSqr(trajectory.at(i).pos, p.pos);
+    if(d < minD)
+    {
+      min_index = i;
+      minD = d;
+    }
+  }
+
+  return min_index;
+}
+
+WayPoint PlanningHelpers::GetPerpendicularOnTrajectory_obsolete(const vector<WayPoint>& trajectory, const WayPoint& p, double& distance, const int& prevIndex )
+{
+  if(trajectory.size() < 2) return p;
+
+  WayPoint p0, p1, p2, perp;
+  if(trajectory.size()==2)
+  {
+    p0 = trajectory.at(0);
+    p1 = WayPoint((trajectory.at(0).pos.x+trajectory.at(1).pos.x)/2.0,
+            (trajectory.at(0).pos.y+trajectory.at(1).pos.y)/2.0,
+            (trajectory.at(0).pos.z+trajectory.at(1).pos.z)/2.0, trajectory.at(0).pos.a);
+    p2 = trajectory.at(1);
+  }
+  else
+  {
+    int next_index = GetClosestNextPointIndex_obsolete(trajectory, p, prevIndex);
+
+    if(next_index == 0)
+    {
+      p0 = trajectory[next_index];
+      p1 = trajectory[next_index+1];
+      p2 = trajectory[next_index+2];
+    }
+    else if(next_index > 0 && next_index < trajectory.size()-1)
+    {
+      p0 = trajectory[next_index-1];
+      p1 = trajectory[next_index];
+      p2 = trajectory[next_index+1];
+    }
+    else
+    {
+      p0 = trajectory[next_index-1];
+      p2 = trajectory[next_index];
+
+      p1 = WayPoint((p0.pos.x+p2.pos.x)/2.0, (p0.pos.y+p2.pos.y)/2.0, (p0.pos.z+p2.pos.z)/2.0, p0.pos.a);
+
+    }
+  }
+
+  Mat3 rotationMat(-p1.pos.a);
+  Mat3 translationMat(-p.pos.x, -p.pos.y);
+  Mat3 invRotationMat(p1.pos.a);
+  Mat3 invTranslationMat(p.pos.x, p.pos.y);
+
+  p0.pos = translationMat*p0.pos;
+  p0.pos = rotationMat*p0.pos;
+
+  p1.pos = translationMat*p1.pos;
+  p1.pos = rotationMat*p1.pos;
+
+  p2.pos = translationMat*p2.pos;
+  p2.pos= rotationMat*p2.pos;
+
+  double m = (p1.pos.y-p0.pos.y)/(p1.pos.x-p0.pos.x);
+  double d = p1.pos.y - m*p1.pos.x; // solve for x = 0
+  distance = p1.pos.x; // distance on the x axes
+
+  perp = p1;
+  perp.pos.x = 0; // on the same y axis of the car
+  perp.pos.y = d; //perp distance between the car and the trajectory
+
+  perp.pos = invRotationMat  * perp.pos;
+  perp.pos = invTranslationMat  * perp.pos;
+
+  return perp;
+}
+
+double PlanningHelpers::GetPerpDistanceToTrajectorySimple_obsolete(const vector<WayPoint>& trajectory, const WayPoint& p,const int& prevIndex)
+{
+
+  if(trajectory.size() < 2)
+    return 0;
+
+  WayPoint p0, p1, p2;
+  int next_index = 0;
+  if(trajectory.size()==2)
+  {
+    p0 = trajectory.at(0);
+    p2 = trajectory.at(1);
+    p1 = WayPoint((p0.pos.x+p2.pos.x)/2.0, (p0.pos.y+p2.pos.y)/2.0, (p0.pos.z+p2.pos.z)/2.0, p0.pos.a);
+
+  }
+  else
+  {
+    next_index = GetClosestNextPointIndexFast(trajectory, p, prevIndex);
+    if(next_index == 0)
+    {
+      p0 = trajectory[next_index];
+      p1 = trajectory[next_index+1];
+      p2 = trajectory[next_index+2];
+    }
+    else if(next_index > 0 && next_index < trajectory.size()-1)
+    {
+      p0 = trajectory[next_index-1];
+      p1 = trajectory[next_index];
+      p2 = trajectory[next_index+1];
+    }
+    else
+    {
+      p0 = trajectory[next_index-1];
+      p2 = trajectory[next_index];
+
+      p1 = WayPoint((p0.pos.x+p2.pos.x)/2.0, (p0.pos.y+p2.pos.y)/2.0, (p0.pos.z+p2.pos.z)/2.0, p0.pos.a);
+
+    }
+
+  }
+
+
+  Mat3 rotationMat(-p1.pos.a);
+  Mat3 translationMat(-p.pos.x, -p.pos.y);
+
+  p0.pos = translationMat*p0.pos;
+  p0.pos = rotationMat*p0.pos;
+
+  p1.pos = translationMat*p1.pos;
+  p1.pos = rotationMat*p1.pos;
+
+  p2.pos = translationMat*p2.pos;
+  p2.pos = rotationMat*p2.pos;
+
+  double m = (p1.pos.y-p0.pos.y)/(p1.pos.x-p0.pos.x);
+  double d = p1.pos.y - m*p1.pos.x;
+
+  if(std::isnan(d) || std::isinf(d))
+  {
+    //assert(false);
+    d = 0;
+  }
+
+  return d;
+}
+
+double PlanningHelpers::GetPerpDistanceToVectorSimple_obsolete(const WayPoint& point1, const WayPoint& point2, const WayPoint& pose)
+{
+  WayPoint p1 = point1, p2 = point2;
+  Mat3 rotationMat(-p1.pos.a);
+  Mat3 translationMat(-pose.pos.x, -pose.pos.y);
+
+  p1.pos = translationMat*p1.pos;
+  p1.pos = rotationMat*p1.pos;
+
+  p2.pos = translationMat*p2.pos;
+  p2.pos = rotationMat*p2.pos;
+
+  double m = (p2.pos.y-p1.pos.y)/(p2.pos.x-p1.pos.x);
+  double d = p2.pos.y - m*p2.pos.x;
+
+  if(std::isnan(d) || std::isinf(d))
+  {
+    //assert(false);
+    d = 0;
+  }
+
+  return d;
+}
+
+WayPoint PlanningHelpers::GetNextPointOnTrajectory_obsolete(const vector<WayPoint>& trajectory, const double& distance, const int& currIndex)
+{
+  assert(trajectory.size()>0);
+
+  int local_currIndex = currIndex;
+
+  if(local_currIndex < 0 || local_currIndex >= trajectory.size())
+    return trajectory.at(0);
+
+  WayPoint p1 = trajectory.at(local_currIndex);
+  WayPoint p2;
+
+  double d = 0;
+  while(local_currIndex < (trajectory.size()-1) && d < distance)
+  {
+    local_currIndex++;
+    p2 = p1;
+    p1 = trajectory.at(local_currIndex);
+    d += distance2points(p1.pos, p2.pos);
+  }
+
+  if(local_currIndex >= trajectory.size()-1)
+    return p1;
+
+  double distance_diff = distance -  d;
+
+  p2 = trajectory.at(local_currIndex);
+  p1 = trajectory.at(local_currIndex+1);
+
+  GPSPoint uv(p1.pos.x - p2.pos.x, p1.pos.y - p2.pos.y ,0,0);
+  double v_norm = pointNorm(uv);
+
+  assert(v_norm != 0);
+
+  uv.x = (uv.x / v_norm) * distance_diff;
+  uv.y = (uv.y / v_norm) * distance_diff;
+
+  double ydiff = p1.pos.y-p2.pos.y;
+  double xdiff = p1.pos.x-p2.pos.x;
+  double a =  atan2(ydiff,xdiff);
+
+  WayPoint abs_waypoint = p2;
+
+  abs_waypoint.pos.x = p2.pos.x + uv.x;
+  abs_waypoint.pos.y = p2.pos.y + uv.y;
+  abs_waypoint.pos.a = a;
+
+  return abs_waypoint;
+}
+
+double PlanningHelpers::GetDistanceOnTrajectory_obsolete(const std::vector<WayPoint>& path, const int& start_index, const WayPoint& p)
+{
+
+  int end_point_index = GetClosestPointIndex_obsolete(path, p);
+  if(end_point_index > 0)
+    end_point_index--;
+
+  double padding_distance = distance2points(path.at(end_point_index).pos, p.pos);
+
+  double d_on_path = 0;
+  if(end_point_index >= start_index)
+  {
+    for(unsigned int i = start_index; i < end_point_index; i++)
+      d_on_path += distance2points(path.at(i).pos, path.at(i+1).pos);
+
+    d_on_path += padding_distance;
+  }
+  else
+  {
+    for(unsigned int i = start_index; i > end_point_index; i--)
+      d_on_path -= distance2points(path.at(i).pos, path.at(i-1).pos);
+  }
+
+  return d_on_path;
+}
+
+bool PlanningHelpers::CompareTrajectories(const std::vector<WayPoint>& path1, const std::vector<WayPoint>& path2)
+{
+  if(path1.size() != path2.size())
+    return false;
+
+  for(unsigned int i=0; i< path1.size(); i++)
+  {
+    if(path1.at(i).v != path2.at(i).v || path1.at(i).pos.x != path2.at(i).pos.x || path1.at(i).pos.y != path2.at(i).pos.y || path1.at(i).pos.alt != path2.at(i).pos.alt || path1.at(i).pos.lon != path2.at(i).pos.lon)
+      return false;
+  }
+
+  return true;
+}
+
+double PlanningHelpers::GetDistanceToClosestStopLineAndCheck(const std::vector<WayPoint>& path, const WayPoint& p, const double& giveUpDistance, int& stopLineID, int& stopSignID, int& trafficLightID, const int& prevIndex)
+{
+  trafficLightID = stopSignID = stopLineID = -1;
+
+  RelativeInfo info;
+  GetRelativeInfo(path, p, info, prevIndex);
+
+  for(unsigned int i=info.iBack; i<path.size(); i++)
+  {
+    if(path.at(i).stopLineID > 0 && path.at(i).pLane)
+    {
+
+      for(unsigned int j = 0; j < path.at(i).pLane->stopLines.size(); j++)
+      {
+
+        if(path.at(i).pLane->stopLines.at(j).id == path.at(i).stopLineID)
+        {
+          stopLineID = path.at(i).stopLineID;
+
+          RelativeInfo stop_info;
+          WayPoint stopLineWP ;
+          stopLineWP.pos = path.at(i).pLane->stopLines.at(j).points.at(0);
+          GetRelativeInfo(path, stopLineWP, stop_info);
+          double localDistance = GetExactDistanceOnTrajectory(path, info, stop_info);
+
+          if(localDistance > giveUpDistance)
+          {
+            stopSignID = path.at(i).pLane->stopLines.at(j).stopSignID;
+            trafficLightID = path.at(i).pLane->stopLines.at(j).trafficLightID;
+            return localDistance;
+          }
+        }
+      }
+    }
+  }
+
+  return -1;
+}
+
+void PlanningHelpers::CreateManualBranchFromTwoPoints(WayPoint& p1,WayPoint& p2 , const double& distance, const DIRECTION_TYPE& direction, std::vector<WayPoint>& path)
+{
+  WayPoint endWP, midWP;
+
+  double branch_angle = 0;
+  if(direction == FORWARD_RIGHT_DIR)
+  {
+    branch_angle = p1.pos.a-M_PI_2;
+  }
+  else if(direction == FORWARD_LEFT_DIR)
+  {
+    branch_angle = p1.pos.a+M_PI_2;
+  }
+  endWP.pos.y = p2.pos.y + distance*sin(branch_angle);
+  endWP.pos.x = p2.pos.x + distance*cos(branch_angle);
+
+  midWP = p2;
+  midWP.pos.x = (p1.pos.x+p2.pos.x)/2.0;
+  midWP.pos.y = (p1.pos.y+p2.pos.y)/2.0;
+  endWP.bDir = midWP.bDir = direction;
+
+  path.clear();
+  path.push_back(p1);
+  path.push_back(p2);
+  path.push_back(endWP);
+
+  //PlanningHelpers::SmoothPath(path, 0.4, 0.1);
+  PlanningHelpers::FixPathDensity(path, 1);
+  PlanningHelpers::SmoothPath(path, 0.4, 0.25);
+  PlanningHelpers::FixPathDensity(path, 0.5);
+  PlanningHelpers::SmoothPath(path, 0.25, 0.4);
+
+  for(unsigned int i=0; i < path.size(); i++)
+  {
+    if(direction == FORWARD_LEFT_DIR)
+    {
+      path.at(i).state = INITIAL_STATE;
+      path.at(i).beh_state = BEH_BRANCH_LEFT_STATE;
+      path.at(i).laneId = -2;
+    }
+    if(direction == FORWARD_RIGHT_DIR)
+    {
+      path.at(i).state = INITIAL_STATE;
+      path.at(i).beh_state = BEH_BRANCH_RIGHT_STATE;
+      path.at(i).laneId = -3;
+    }
+  }
+}
+
+void PlanningHelpers::CreateManualBranch(std::vector<WayPoint>& path, const int& degree, const DIRECTION_TYPE& direction)
+{
+  if(path.size() < 5) return;
+
+  //start branch point
+  WayPoint branch_start = path.at(path.size()-5);
+  WayPoint last_wp = path.at(path.size()-1);
+
+
+  WayPoint endWP;
+  vector<WayPoint> goal_path;
+  double branch_angle = 0;
+  if(direction == FORWARD_RIGHT_DIR)
+  {
+    branch_angle = last_wp.pos.a-M_PI_2;
+  }
+  else if(direction == FORWARD_LEFT_DIR)
+  {
+    branch_angle = last_wp.pos.a+M_PI_2;
+  }
+  endWP.pos.y = last_wp.pos.y + 10*sin(branch_angle);
+  endWP.pos.x = last_wp.pos.x + 10*cos(branch_angle);
+
+  WayPoint wp = last_wp;
+  wp.pos.x = (last_wp.pos.x+endWP.pos.x)/2.0;
+  wp.pos.y = (last_wp.pos.y+endWP.pos.y)/2.0;
+  endWP.bDir = wp.bDir = direction;
+  goal_path.push_back(wp);
+  goal_path.push_back(endWP);
+
+  goal_path.insert(goal_path.begin(), path.end()-5, path.end());
+  PlanningHelpers::SmoothPath(goal_path, 0.25, 0.25);
+  PlanningHelpers::FixPathDensity(goal_path, 0.75);
+  PlanningHelpers::SmoothPath(goal_path, 0.25, 0.35);
+
+  path.erase(path.end()-5, path.end());
+  path.insert(path.end(), goal_path.begin(), goal_path.end());
+
+  PlanningHelpers::CalcAngleAndCost(path);
+
+  for(unsigned int i=0; i < path.size(); i++)
+  {
+    if(direction == FORWARD_LEFT_DIR)
+    {
+      path.at(i).state = INITIAL_STATE;
+      path.at(i).beh_state = BEH_BRANCH_LEFT_STATE;
+    }
+    if(direction == FORWARD_RIGHT_DIR)
+    {
+      path.at(i).state = INITIAL_STATE;
+      path.at(i).beh_state = BEH_BRANCH_RIGHT_STATE;
+    }
+  }
+
+
+}
+
+void PlanningHelpers::FixPathDensity(vector<WayPoint>& path, const double& distanceDensity)
+{
+  if(path.size() == 0 || distanceDensity==0) return;
+
+  double d = 0, a = 0;
+  double margin = distanceDensity*0.01;
+  double remaining = 0;
+  int nPoints = 0;
+  vector<WayPoint> fixedPath;
+  fixedPath.push_back(path.at(0));
+  for(unsigned int si = 0, ei=1; ei < path.size(); )
+  {
+    d += hypot(path.at(ei).pos.x- path.at(ei-1).pos.x, path.at(ei).pos.y- path.at(ei-1).pos.y) + remaining;
+    a = atan2(path.at(ei).pos.y - path.at(si).pos.y, path.at(ei).pos.x - path.at(si).pos.x);
+
+    if(d < distanceDensity - margin ) // skip
+    {
+      ei++;
+      remaining = 0;
+    }
+    else if(d > (distanceDensity +  margin)) // skip
+    {
+      WayPoint pm = path.at(si);
+      nPoints = d  / distanceDensity;
+      for(int k = 0; k < nPoints; k++)
+      {
+        pm.pos.x = pm.pos.x + distanceDensity * cos(a);
+        pm.pos.y = pm.pos.y + distanceDensity * sin(a);
+        fixedPath.push_back(pm);
+      }
+      remaining = d - nPoints*distanceDensity;
+      si++;
+      path.at(si).pos = pm.pos;
+      d = 0;
+      ei++;
+    }
+    else
+    {
+      d = 0;
+      remaining = 0;
+      fixedPath.push_back(path.at(ei));
+      ei++;
+      si = ei - 1;
+    }
+  }
+
+  path = fixedPath;
+}
+
+void PlanningHelpers::SmoothPath(vector<WayPoint>& path, double weight_data,
+    double weight_smooth, double tolerance)
+{
+
+  if (path.size() <= 2 )
+  {
+    //cout << "Can't Smooth Path, Path_in Size=" << path.size() << endl;
+    return;
+  }
+
+  const vector<WayPoint>& path_in = path;
+  vector<WayPoint> smoothPath_out =  path_in;
+
+  double change = tolerance;
+  double xtemp, ytemp;
+  int nIterations = 0;
+
+  int size = path_in.size();
+
+  while (change >= tolerance)
+  {
+    change = 0.0;
+    for (int i = 1; i < size - 1; i++)
+    {
+//      if (smoothPath_out[i].pos.a != smoothPath_out[i - 1].pos.a)
+//        continue;
+
+      xtemp = smoothPath_out[i].pos.x;
+      ytemp = smoothPath_out[i].pos.y;
+
+      smoothPath_out[i].pos.x += weight_data
+          * (path_in[i].pos.x - smoothPath_out[i].pos.x);
+      smoothPath_out[i].pos.y += weight_data
+          * (path_in[i].pos.y - smoothPath_out[i].pos.y);
+
+      smoothPath_out[i].pos.x += weight_smooth
+          * (smoothPath_out[i - 1].pos.x + smoothPath_out[i + 1].pos.x
+              - (2.0 * smoothPath_out[i].pos.x));
+      smoothPath_out[i].pos.y += weight_smooth
+          * (smoothPath_out[i - 1].pos.y + smoothPath_out[i + 1].pos.y
+              - (2.0 * smoothPath_out[i].pos.y));
+
+      change += fabs(xtemp - smoothPath_out[i].pos.x);
+      change += fabs(ytemp - smoothPath_out[i].pos.y);
+
+    }
+    nIterations++;
+  }
+
+  path = smoothPath_out;
+}
+
+void PlanningHelpers::PredictConstantTimeCostForTrajectory(std::vector<PlannerHNS::WayPoint>& path, const PlannerHNS::WayPoint& currPose, const double& minVelocity, const double& minDist)
+{
+  if(path.size() == 0) return;
+
+  for(unsigned int i = 0 ; i < path.size(); i++)
+    path.at(i).timeCost = -1;
+
+  if(currPose.v == 0 || currPose.v < minVelocity) return;
+
+  RelativeInfo info;
+  PlanningHelpers::GetRelativeInfo(path, currPose, info);
+
+  double total_distance = 0;
+  double accum_time = 0;
+
+  path.at(info.iFront).timeCost = 0;
+  if(info.iFront == 0 ) info.iFront++;
+
+  for(unsigned int i=info.iFront; i<path.size(); i++)
+  {
+    total_distance += hypot(path.at(i).pos.x- path.at(i-1).pos.x,path.at(i).pos.y- path.at(i-1).pos.y);
+    accum_time = total_distance/currPose.v;
+    path.at(i).timeCost = accum_time;
+  }
+}
+
+void PlanningHelpers::FixAngleOnly(std::vector<WayPoint>& path)
+{
+  if(path.size() <= 2) return;
+
+  path[0].pos.a = UtilityH::FixNegativeAngle(atan2(path[1].pos.y - path[0].pos.y, path[1].pos.x - path[0].pos.x ));
+
+  for(int j = 1; j < path.size()-1; j++)
+    path[j].pos.a     = UtilityH::FixNegativeAngle(atan2(path[j+1].pos.y - path[j].pos.y, path[j+1].pos.x - path[j].pos.x ));
+
+  int j = (int)path.size()-1;
+
+  path[j].pos.a = path[j-1].pos.a;
+
+  for(int j = 0; j < path.size()-1; j++)
+  {
+    if(path.at(j).pos.x == path.at(j+1).pos.x && path.at(j).pos.y == path.at(j+1).pos.y)
+      path.at(j).pos.a = path.at(j+1).pos.a;
+  }
+}
+
+double PlanningHelpers::CalcAngleAndCost(vector<WayPoint>& path, const double& lastCost, const bool& bSmooth)
+{
+  if(path.size() < 2) return 0;
+  if(path.size() == 2)
+  {
+    path[0].pos.a = UtilityH::FixNegativeAngle(atan2(path[1].pos.y - path[0].pos.y, path[1].pos.x - path[0].pos.x ));
+    path[0].cost = lastCost;
+    path[1].pos.a = path[0].pos.a;
+    path[1].cost = path[0].cost +  distance2points(path[0].pos, path[1].pos);
+    return path[1].cost;
+  }
+
+  path[0].pos.a = UtilityH::FixNegativeAngle(atan2(path[1].pos.y - path[0].pos.y, path[1].pos.x - path[0].pos.x ));
+  path[0].cost = lastCost;
+
+  for(int j = 1; j < path.size()-1; j++)
+  {
+    path[j].pos.a     = UtilityH::FixNegativeAngle(atan2(path[j+1].pos.y - path[j].pos.y, path[j+1].pos.x - path[j].pos.x ));
+    path[j].cost   = path[j-1].cost +  distance2points(path[j-1].pos, path[j].pos);
+  }
+
+  int j = (int)path.size()-1;
+
+  path[j].pos.a     = path[j-1].pos.a;
+  path[j].cost   = path[j-1].cost + distance2points(path[j-1].pos, path[j].pos);
+
+  for(int j = 0; j < path.size()-1; j++)
+  {
+    if(path.at(j).pos.x == path.at(j+1).pos.x && path.at(j).pos.y == path.at(j+1).pos.y)
+      path.at(j).pos.a = path.at(j+1).pos.a;
+  }
+
+  return path[j].cost;
+}
+
+double PlanningHelpers::CalcAngleAndCostAndCurvatureAnd2D(vector<WayPoint>& path, const double& lastCost)
+{
+  if(path.size() < 2) return -1;
+
+  path[0].pos.a   = atan2(path[1].pos.y - path[0].pos.y, path[1].pos.x - path[0].pos.x );
+  path[0].cost   = lastCost;
+
+  double k = 0;
+  GPSPoint center;
+
+  for(unsigned int j = 1; j < path.size()-1; j++)
+  {
+    k =  CalcCircle(path[j-1].pos,path[j].pos, path[j+1].pos, center);
+    if(k > 150.0 || std::isnan(k))
+      k = 150.0;
+
+    if(k<1.0)
+      path[j].cost = 0;
+    else
+      path[j].cost = 1.0-1.0/k;
+
+    path[j].pos.a   = atan2(path[j+1].pos.y - path[j].pos.y, path[j+1].pos.x - path[j].pos.x );
+  }
+  unsigned int j = path.size()-1;
+
+  path[0].cost    = path[1].cost;
+  path[j].cost   = path[j-1].cost;
+  path[j].pos.a   = path[j-1].pos.a;
+  path[j].cost   = path[j-1].cost ;
+
+  return path[j].cost;
+}
+
+double PlanningHelpers::CalcCircle(const GPSPoint& pt1, const GPSPoint& pt2, const GPSPoint& pt3, GPSPoint& center)
+{
+  double yDelta_a= pt2.y - pt1.y;
+  double xDelta_a= pt2.x - pt1.x;
+  double yDelta_b= pt3.y - pt2.y;
+  double xDelta_b= pt3.x - pt2.x;
+
+  if (fabs(xDelta_a) <= 0.000000000001 && fabs(yDelta_b) <= 0.000000000001)
+  {
+    center.x= 0.5*(pt2.x + pt3.x);
+    center.y= 0.5*(pt1.y + pt2.y);
+    return distance2points(center,pt1);
+  }
+
+  double aSlope=yDelta_a/xDelta_a;
+  double bSlope=yDelta_b/xDelta_b;
+  if (fabs(aSlope-bSlope) <= 0.000000000001)
+  {
+    return 100000;
+  }
+
+  center.x= (aSlope*bSlope*(pt1.y - pt3.y) + bSlope*(pt1.x + pt2 .x) - aSlope*(pt2.x+pt3.x) )/(2.0* (bSlope-aSlope) );
+  center.y = -1.0*(center.x - (pt1.x+pt2.x)/2.0)/aSlope +  (pt1.y+pt2.y)/2.0;
+
+  return  distance2points(center,pt1);
+}
+
+void PlanningHelpers::ExtractPartFromPointToDistance(const vector<WayPoint>& originalPath, const WayPoint& pos, const double& minDistance,
+    const double& pathDensity, vector<WayPoint>& extractedPath, const double& SmoothDataWeight, const double& SmoothWeight, const double& SmoothTolerance)
+{
+  extractedPath.clear();
+  unsigned int close_index = GetClosestNextPointIndexDirectionFast(originalPath, pos);
+//  int i_slow = GetClosestNextPointIndexDirection(originalPath, pos);
+//  if(close_index != i_slow)
+//    cout << "Aler Alert !!! fast: " << close_index << ", slow: " << i_slow  << endl;
+  //vector<WayPoint> tempPath;
+  double d_limit = 0;
+  if(close_index >= 2) close_index -=2;
+  else close_index = 0;
+
+  for(unsigned int i=close_index; i< originalPath.size(); i++)
+  {
+    extractedPath.push_back(originalPath.at(i));
+
+    if(i>0)
+      d_limit += hypot(originalPath.at(i).pos.y - originalPath.at(i-1).pos.y, originalPath.at(i).pos.x - originalPath.at(i-1).pos.x);
+
+    if(d_limit > minDistance)
+      break;
+  }
+
+  if(extractedPath.size() < 2)
+  {
+    cout << endl << "### Planner Z . Extracted Rollout Path is too Small, Size = " << extractedPath.size() << endl;
+    return;
+  }
+
+  FixPathDensity(extractedPath, pathDensity);
+  SmoothPath(extractedPath, SmoothDataWeight, SmoothWeight , SmoothTolerance);
+  CalcAngleAndCost(extractedPath);
+
+  //extractedPath = tempPath;
+  //tempPath.clear();
+  //TestQuadraticSpline(extractedPath, tempPath);
+}
+
+void PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(const vector<WayPoint>& originalPath, const WayPoint& pos, const double& minDistance,
+    const double& pathDensity, vector<WayPoint>& extractedPath)
+{
+  if(originalPath.size() < 2 ) return;
+
+  extractedPath.clear();
+
+  int close_index = GetClosestNextPointIndexDirectionFast(originalPath, pos);
+  double d = 0;
+
+  if(close_index + 1 >= originalPath.size())
+    close_index = originalPath.size() - 2;
+
+  for(int i=close_index; i >=  0; i--)
+  {
+    extractedPath.insert(extractedPath.begin(),  originalPath.at(i));
+    if(i < originalPath.size())
+      d += hypot(originalPath.at(i).pos.y - originalPath.at(i+1).pos.y, originalPath.at(i).pos.x - originalPath.at(i+1).pos.x);
+    if(d > 10)
+      break;
+  }
+
+  //extractedPath.push_back(info.perp_point);
+  d = 0;
+  for(int i=close_index+1; i < (int)originalPath.size(); i++)
+  {
+    extractedPath.push_back(originalPath.at(i));
+    if(i > 0)
+      d += hypot(originalPath.at(i).pos.y - originalPath.at(i-1).pos.y, originalPath.at(i).pos.x - originalPath.at(i-1).pos.x);
+    if(d > minDistance)
+      break;
+  }
+
+  if(extractedPath.size() < 2)
+  {
+    cout << endl << "### Planner Z . Extracted Rollout Path is too Small, Size = " << extractedPath.size() << endl;
+    return;
+  }
+
+  FixPathDensity(extractedPath, pathDensity);
+  CalcAngleAndCost(extractedPath);
+}
+
+void PlanningHelpers::ExtractPartFromPointToDistanceFast(const vector<WayPoint>& originalPath, const WayPoint& pos, const double& minDistance,
+    const double& pathDensity, vector<WayPoint>& extractedPath, const double& SmoothDataWeight, const double& SmoothWeight, const double& SmoothTolerance)
+{
+  extractedPath.clear();
+  RelativeInfo info;
+  GetRelativeInfo(originalPath, pos, info);
+  double d = 0;
+  if(info.iBack > 0)
+    info.iBack--;
+
+  for(int i=info.iBack; i >=  0; i--)
+  {
+    extractedPath.insert(extractedPath.begin(),  originalPath.at(i));
+    if(i < originalPath.size())
+      d += hypot(originalPath.at(i).pos.y - originalPath.at(i+1).pos.y, originalPath.at(i).pos.x - originalPath.at(i+1).pos.x);
+    if(d > 10)
+      break;
+  }
+
+  //extractedPath.push_back(info.perp_point);
+  d = 0;
+  for(int i=info.iBack+1; i < (int)originalPath.size(); i++)
+  {
+    extractedPath.push_back(originalPath.at(i));
+    if(i > 0)
+      d += hypot(originalPath.at(i).pos.y - originalPath.at(i-1).pos.y, originalPath.at(i).pos.x - originalPath.at(i-1).pos.x);
+    if(d > minDistance)
+      break;
+  }
+
+  if(extractedPath.size() < 2)
+  {
+    cout << endl << "### Planner Z . Extracted Rollout Path is too Small, Size = " << extractedPath.size() << endl;
+    return;
+  }
+
+  FixPathDensity(extractedPath, pathDensity);
+  CalcAngleAndCost(extractedPath);
+}
+
+void PlanningHelpers::CalculateRollInTrajectories(const WayPoint& carPos, const double& speed, const vector<WayPoint>& originalCenter, int& start_index,
+    int& end_index, vector<double>& end_laterals ,
+    vector<vector<WayPoint> >& rollInPaths, const double& max_roll_distance,
+    const double& maxSpeed, const double&  carTipMargin, const double& rollInMargin,
+    const double& rollInSpeedFactor, const double& pathDensity, const double& rollOutDensity,
+    const int& rollOutNumber, const double& SmoothDataWeight, const double& SmoothWeight,
+    const double& SmoothTolerance, const bool& bHeadingSmooth,
+    std::vector<WayPoint>& sampledPoints)
+{
+  WayPoint p;
+  double dummyd = 0;
+
+  int iLimitIndex = (carTipMargin/0.3)/pathDensity;
+  if(iLimitIndex >= originalCenter.size())
+    iLimitIndex = originalCenter.size() - 1;
+
+  //Get Closest Index
+  RelativeInfo info;
+  GetRelativeInfo(originalCenter, carPos, info);
+  double remaining_distance = 0;
+  int close_index = info.iBack;
+  for(unsigned int i=close_index; i< originalCenter.size()-1; i++)
+    {
+    if(i>0)
+      remaining_distance += distance2points(originalCenter[i].pos, originalCenter[i+1].pos);
+    }
+
+  double initial_roll_in_distance = info.perp_distance ; //GetPerpDistanceToTrajectorySimple(originalCenter, carPos, close_index);
+
+
+  vector<WayPoint> RollOutStratPath;
+  ///***   Smoothing From Car Heading Section ***///
+//  if(bHeadingSmooth)
+//  {
+//    unsigned int num_of_strait_points = carTipMargin / pathDensity;
+//    int closest_for_each_iteration = 0;
+//    WayPoint np = GetPerpendicularOnTrajectory_obsolete(originalCenter, carPos, dummyd, closest_for_each_iteration);
+//    np.pos = carPos.pos;
+//
+//    RollOutStratPath.push_back(np);
+//    for(unsigned int i = 0; i < num_of_strait_points; i++)
+//    {
+//      p = RollOutStratPath.at(i);
+//      p.pos.x = p.pos.x +  pathDensity*cos(p.pos.a);
+//      p.pos.y = p.pos.y +  pathDensity*sin(p.pos.a);
+//      np = GetPerpendicularOnTrajectory_obsolete(originalCenter, p, dummyd, closest_for_each_iteration);
+//      np.pos = p.pos;
+//      RollOutStratPath.push_back(np);
+//    }
+//
+//    initial_roll_in_distance = GetPerpDistanceToTrajectorySimple_obsolete(originalCenter, RollOutStratPath.at(RollOutStratPath.size()-1), close_index);
+//  }
+  ///***   -------------------------------- ***///
+
+
+  //printf("\n Lateral Distance: %f" , initial_roll_in_distance);
+
+  //calculate the starting index
+  double d_limit = 0;
+  unsigned int far_index = close_index;
+
+  //calculate end index
+  double start_distance = rollInSpeedFactor*speed+rollInMargin;
+  if(start_distance > remaining_distance)
+    start_distance = remaining_distance;
+
+  d_limit = 0;
+  for(unsigned int i=close_index; i< originalCenter.size(); i++)
+    {
+      if(i>0)
+        d_limit += distance2points(originalCenter[i].pos, originalCenter[i-1].pos);
+
+      if(d_limit >= start_distance)
+      {
+        far_index = i;
+        break;
+      }
+    }
+
+  int centralTrajectoryIndex = rollOutNumber/2;
+  vector<double> end_distance_list;
+  for(int i=0; i< rollOutNumber+1; i++)
+    {
+      double end_roll_in_distance = rollOutDensity*(i - centralTrajectoryIndex);
+      end_distance_list.push_back(end_roll_in_distance);
+    }
+
+  start_index = close_index;
+  end_index = far_index;
+  end_laterals = end_distance_list;
+
+  //calculate the actual calculation starting index
+  d_limit = 0;
+  unsigned int smoothing_start_index = start_index;
+  unsigned int smoothing_end_index = end_index;
+
+  for(unsigned int i=smoothing_start_index; i< originalCenter.size(); i++)
+  {
+    if(i > 0)
+      d_limit += distance2points(originalCenter[i].pos, originalCenter[i-1].pos);
+    if(d_limit > carTipMargin)
+      break;
+
+    smoothing_start_index++;
+  }
+
+  d_limit = 0;
+  for(unsigned int i=end_index; i< originalCenter.size(); i++)
+  {
+    if(i > 0)
+      d_limit += distance2points(originalCenter[i].pos, originalCenter[i-1].pos);
+    if(d_limit > carTipMargin)
+      break;
+
+    smoothing_end_index++;
+  }
+
+  int nSteps = end_index - smoothing_start_index;
+
+
+  vector<double> inc_list;
+  rollInPaths.clear();
+  vector<double> inc_list_inc;
+  for(int i=0; i< rollOutNumber+1; i++)
+  {
+    double diff = end_laterals.at(i)-initial_roll_in_distance;
+    inc_list.push_back(diff/(double)nSteps);
+    rollInPaths.push_back(vector<WayPoint>());
+    inc_list_inc.push_back(0);
+  }
+
+
+
+  vector<vector<WayPoint> > execluded_from_smoothing;
+  for(unsigned int i=0; i< rollOutNumber+1 ; i++)
+    execluded_from_smoothing.push_back(vector<WayPoint>());
+
+
+
+  //Insert First strait points within the tip of the car range
+  for(unsigned int j = start_index; j < smoothing_start_index; j++)
+  {
+    p = originalCenter.at(j);
+    double original_speed = p.v;
+    for(unsigned int i=0; i< rollOutNumber+1 ; i++)
+    {
+      p.pos.x = originalCenter.at(j).pos.x -  initial_roll_in_distance*cos(p.pos.a + M_PI_2);
+      p.pos.y = originalCenter.at(j).pos.y -  initial_roll_in_distance*sin(p.pos.a + M_PI_2);
+      if(i!=centralTrajectoryIndex)
+        p.v = original_speed * LANE_CHANGE_SPEED_FACTOR;
+      else
+        p.v = original_speed ;
+
+      if(j < iLimitIndex)
+        execluded_from_smoothing.at(i).push_back(p);
+      else
+        rollInPaths.at(i).push_back(p);
+
+      sampledPoints.push_back(p);
+    }
+  }
+
+  for(unsigned int j = smoothing_start_index; j < end_index; j++)
+    {
+      p = originalCenter.at(j);
+      double original_speed = p.v;
+      for(unsigned int i=0; i< rollOutNumber+1 ; i++)
+      {
+        inc_list_inc[i] += inc_list[i];
+        double d = inc_list_inc[i];
+        p.pos.x = originalCenter.at(j).pos.x -  initial_roll_in_distance*cos(p.pos.a + M_PI_2) - d*cos(p.pos.a+ M_PI_2);
+        p.pos.y = originalCenter.at(j).pos.y -  initial_roll_in_distance*sin(p.pos.a + M_PI_2) - d*sin(p.pos.a+ M_PI_2);
+        if(i!=centralTrajectoryIndex)
+          p.v = original_speed * LANE_CHANGE_SPEED_FACTOR;
+        else
+          p.v = original_speed ;
+
+        rollInPaths.at(i).push_back(p);
+
+        sampledPoints.push_back(p);
+      }
+    }
+
+  //Insert last strait points to make better smoothing
+  for(unsigned int j = end_index; j < smoothing_end_index; j++)
+  {
+    p = originalCenter.at(j);
+    double original_speed = p.v;
+    for(unsigned int i=0; i< rollOutNumber+1 ; i++)
+    {
+      double d = end_laterals.at(i);
+      p.pos.x  = originalCenter.at(j).pos.x - d*cos(p.pos.a + M_PI_2);
+      p.pos.y  = originalCenter.at(j).pos.y - d*sin(p.pos.a + M_PI_2);
+      if(i!=centralTrajectoryIndex)
+        p.v = original_speed * LANE_CHANGE_SPEED_FACTOR;
+      else
+        p.v = original_speed ;
+      rollInPaths.at(i).push_back(p);
+
+      sampledPoints.push_back(p);
+    }
+  }
+
+  for(unsigned int i=0; i< rollOutNumber+1 ; i++)
+    rollInPaths.at(i).insert(rollInPaths.at(i).begin(), execluded_from_smoothing.at(i).begin(), execluded_from_smoothing.at(i).end());
+
+  ///***   Smoothing From Car Heading Section ***///
+//  if(bHeadingSmooth)
+//  {
+//    for(unsigned int i=0; i< rollOutNumber+1 ; i++)
+//    {
+//      unsigned int cut_index = GetClosestNextPointIndex(rollInPaths.at(i), RollOutStratPath.at(RollOutStratPath.size()-1));
+//      rollInPaths.at(i).erase(rollInPaths.at(i).begin(), rollInPaths.at(i).begin()+cut_index);
+//      rollInPaths.at(i).insert(rollInPaths.at(i).begin(), RollOutStratPath.begin(), RollOutStratPath.end());
+//    }
+//  }
+  ///***   -------------------------------- ***///
+
+
+
+  d_limit = 0;
+  for(unsigned int j = smoothing_end_index; j < originalCenter.size(); j++)
+    {
+    if(j > 0)
+      d_limit += distance2points(originalCenter.at(j).pos, originalCenter.at(j-1).pos);
+
+    if(d_limit > max_roll_distance)
+      break;
+
+      p = originalCenter.at(j);
+      double original_speed = p.v;
+      for(unsigned int i=0; i< rollInPaths.size() ; i++)
+      {
+        double d = end_laterals.at(i);
+        p.pos.x  = originalCenter.at(j).pos.x - d*cos(p.pos.a + M_PI_2);
+        p.pos.y  = originalCenter.at(j).pos.y - d*sin(p.pos.a + M_PI_2);
+
+        if(i!=centralTrajectoryIndex)
+          p.v = original_speed * LANE_CHANGE_SPEED_FACTOR;
+        else
+          p.v = original_speed ;
+
+        rollInPaths.at(i).push_back(p);
+
+        sampledPoints.push_back(p);
+      }
+    }
+
+  for(unsigned int i=0; i< rollOutNumber+1 ; i++)
+  {
+    SmoothPath(rollInPaths.at(i), SmoothDataWeight, SmoothWeight, SmoothTolerance);
+  }
+
+//  for(unsigned int i=0; i< rollInPaths.size(); i++)
+//    CalcAngleAndCost(rollInPaths.at(i));
+}
+
+bool PlanningHelpers::FindInList(const std::vector<int>& list,const int& x)
+{
+  for(unsigned int i = 0 ; i < list.size(); i++)
+  {
+    if(list.at(i) == x)
+      return true;
+  }
+  return false;
+}
+
+void PlanningHelpers::RemoveWithValue(std::vector<int>& list,const int& x)
+{
+  for(unsigned int i = 0 ; i < list.size(); i++)
+  {
+    if(list.at(i) == x)
+    {
+      list.erase(list.begin()+i);
+    }
+  }
+}
+
+std::vector<int> PlanningHelpers::GetUniqueLeftRightIds(const std::vector<WayPoint>& path)
+{
+   vector<int> sideLanes;
+  for(unsigned int iwp = 0; iwp < path.size(); iwp++)
+   {
+     if(path.at(iwp).LeftPointId>0)
+     {
+       bool bFound = false;
+       for(unsigned int is = 0 ; is < sideLanes.size(); is++)
+       {
+         if(sideLanes.at(is) == path.at(iwp).LeftPointId)
+         {
+           bFound = true;
+           break;
+         }
+       }
+
+       if(!bFound)
+         sideLanes.push_back(path.at(iwp).LeftPointId);
+     }
+
+     if(path.at(iwp).RightPointId>0)
+     {
+       bool bFound = false;
+       for(unsigned int is = 0 ; is < sideLanes.size(); is++)
+       {
+         if(sideLanes.at(is) == path.at(iwp).RightPointId)
+         {
+           bFound = true;
+           break;
+         }
+       }
+
+       if(!bFound)
+         sideLanes.push_back(path.at(iwp).RightPointId);
+     }
+
+     //RemoveWithValue(sideLanes, path.at(iwp).laneId);
+   }
+  return sideLanes;
+}
+
+void PlanningHelpers::SmoothSpeedProfiles(vector<WayPoint>& path_in, double weight_data, double weight_smooth, double tolerance  )
+{
+
+  if (path_in.size() <= 1)
+    return;
+  vector<WayPoint> newpath = path_in;
+
+  double change = tolerance;
+  double xtemp;
+  int nIterations = 0;
+  int size = newpath.size();
+
+  while (change >= tolerance)
+  {
+    change = 0.0;
+    for (int i = 1; i < size -1; i++)
+    {
+      xtemp = newpath[i].v;
+      newpath[i].v += weight_data * (path_in[i].v - newpath[i].v);
+      newpath[i].v += weight_smooth * (newpath[i - 1].v + newpath[i + 1].v - (2.0 * newpath[i].v));
+      change += fabs(xtemp - newpath[i].v);
+
+    }
+    nIterations++;
+  }
+
+  path_in = newpath;
+}
+
+void PlanningHelpers::SmoothCurvatureProfiles(vector<WayPoint>& path_in, double weight_data, double weight_smooth, double tolerance)
+{
+  if (path_in.size() <= 1)
+      return;
+  vector<WayPoint> newpath = path_in;
+
+  double change = tolerance;
+  double xtemp;
+  int nIterations = 0;
+  int size = newpath.size();
+
+  while (change >= tolerance)
+  {
+    change = 0.0;
+    for (int i = 1; i < size -1; i++)
+    {
+      xtemp = newpath[i].cost;
+      newpath[i].cost += weight_data * (path_in[i].cost - newpath[i].cost);
+      newpath[i].cost += weight_smooth * (newpath[i - 1].cost + newpath[i + 1].cost - (2.0 * newpath[i].cost));
+      change += fabs(xtemp - newpath[i].cost);
+
+    }
+    nIterations++;
+  }
+  path_in = newpath;
+}
+
+void PlanningHelpers::SmoothWayPointsDirections(vector<WayPoint>& path_in, double weight_data, double weight_smooth, double tolerance  )
+{
+
+  if (path_in.size() <= 1)
+    return;
+
+  vector<WayPoint> newpath = path_in;
+
+  double change = tolerance;
+  double xtemp;
+  int nIterations = 0;
+  int size = newpath.size();
+
+  while (change >= tolerance)
+  {
+    change = 0.0;
+    for (int i = 1; i < size -1; i++)
+    {
+      xtemp = newpath[i].pos.a;
+      newpath[i].pos.a += weight_data * (path_in[i].pos.a - newpath[i].pos.a);
+      newpath[i].pos.a += weight_smooth * (newpath[i - 1].pos.a + newpath[i + 1].pos.a - (2.0 * newpath[i].pos.a));
+      change += fabs(xtemp - newpath[i].pos.a);
+
+    }
+    nIterations++;
+  }
+  path_in = newpath;
+}
+
+void PlanningHelpers::SmoothGlobalPathSpeed(vector<WayPoint>& path)
+{
+  CalcAngleAndCostAndCurvatureAnd2D(path);
+  SmoothSpeedProfiles(path, 0.45,0.25, 0.01);
+}
+
+void PlanningHelpers::GenerateRecommendedSpeed(vector<WayPoint>& path, const double& max_speed, const double& speedProfileFactor)
+{
+  CalcAngleAndCostAndCurvatureAnd2D(path);
+  SmoothCurvatureProfiles(path, 0.4, 0.3, 0.01);
+  double v = 0;
+
+  for(unsigned int i = 0 ; i < path.size(); i++)
+  {
+    double k_ratio = path.at(i).cost*10.0;
+    double local_max = (path.at(i).v >= 0 && max_speed > path.at(i).v) ? path.at(i).v : max_speed;
+
+    if(k_ratio >= 9.5)
+      v = local_max;
+    else if(k_ratio <= 8.5)
+      v = 1.0*speedProfileFactor;
+    else
+    {
+      k_ratio = k_ratio - 8.5;
+      v = (local_max - 1.0) * k_ratio + 1.0;
+      v = v*speedProfileFactor;
+    }
+
+    if(v > local_max)
+      path.at(i).v = local_max;
+    else
+      path.at(i).v = v;
+
+  }
+
+  SmoothSpeedProfiles(path, 0.4,0.3, 0.01);
+}
+
+WayPoint* PlanningHelpers::BuildPlanningSearchTreeV2(WayPoint* pStart,
+    const WayPoint& goalPos,
+    const vector<int>& globalPath,
+    const double& DistanceLimit,
+    const bool& bEnableLaneChange,
+    vector<WayPoint*>& all_cells_to_delete)
+{
+  if(!pStart) return NULL;
+
+  vector<pair<WayPoint*, WayPoint*> >nextLeafToTrace;
+
+  WayPoint* pZero = 0;
+  WayPoint* wp    = new WayPoint();
+  *wp = *pStart;
+  nextLeafToTrace.push_back(make_pair(pZero, wp));
+  all_cells_to_delete.push_back(wp);
+
+  double     distance     = 0;
+  double     before_change_distance  = 0;
+  WayPoint*   pGoalCell     = 0;
+  double     nCounter     = 0;
+
+
+  while(nextLeafToTrace.size()>0)
+  {
+    nCounter++;
+
+    unsigned int min_cost_index = 0;
+    double min_cost = DBL_MAX;
+
+    for(unsigned int i=0; i < nextLeafToTrace.size(); i++)
+    {
+      if(nextLeafToTrace.at(i).second->cost < min_cost)
+      {
+        min_cost = nextLeafToTrace.at(i).second->cost;
+        min_cost_index = i;
+      }
+    }
+
+    WayPoint* pH   = nextLeafToTrace.at(min_cost_index).second;
+
+    assert(pH != 0);
+
+    nextLeafToTrace.erase(nextLeafToTrace.begin()+min_cost_index);
+
+    double distance_to_goal = distance2points(pH->pos, goalPos.pos);
+    double angle_to_goal = UtilityH::AngleBetweenTwoAnglesPositive(UtilityH::FixNegativeAngle(pH->pos.a), UtilityH::FixNegativeAngle(goalPos.pos.a));
+    if( distance_to_goal <= 0.1 && angle_to_goal < M_PI_4)
+    {
+      cout << "Goal Found, LaneID: " << pH->laneId <<", Distance : " << distance_to_goal << ", Angle: " << angle_to_goal*RAD2DEG << endl;
+      pGoalCell = pH;
+      break;
+    }
+    else
+    {
+
+      if(pH->pLeft && !CheckLaneExits(all_cells_to_delete, pH->pLeft->pLane) && !CheckNodeExits(all_cells_to_delete, pH->pLeft) && bEnableLaneChange && before_change_distance > LANE_CHANGE_MIN_DISTANCE)
+      {
+        wp = new WayPoint();
+        *wp = *pH->pLeft;
+        double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);
+        distance += d;
+        before_change_distance = -LANE_CHANGE_MIN_DISTANCE*3;
+
+        for(unsigned int a = 0; a < wp->actionCost.size(); a++)
+        {
+          //if(wp->actionCost.at(a).first == LEFT_TURN_ACTION)
+            d += wp->actionCost.at(a).second;
+        }
+
+        wp->cost = pH->cost + d;
+        wp->pRight = pH;
+        wp->pLeft = 0;
+
+        nextLeafToTrace.push_back(make_pair(pH, wp));
+        all_cells_to_delete.push_back(wp);
+      }
+
+      if(pH->pRight && !CheckLaneExits(all_cells_to_delete, pH->pRight->pLane) && !CheckNodeExits(all_cells_to_delete, pH->pRight) && bEnableLaneChange && before_change_distance > LANE_CHANGE_MIN_DISTANCE)
+      {
+        wp = new WayPoint();
+        *wp = *pH->pRight;
+        double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);
+        distance += d;
+        before_change_distance = -LANE_CHANGE_MIN_DISTANCE*3;
+
+        for(unsigned int a = 0; a < wp->actionCost.size(); a++)
+        {
+          //if(wp->actionCost.at(a).first == RIGHT_TURN_ACTION)
+            d += wp->actionCost.at(a).second;
+        }
+
+        wp->cost = pH->cost + d ;
+        wp->pLeft = pH;
+        wp->pRight = 0;
+        nextLeafToTrace.push_back(make_pair(pH, wp));
+        all_cells_to_delete.push_back(wp);
+      }
+
+      for(unsigned int i =0; i< pH->pFronts.size(); i++)
+      {
+        if(CheckLaneIdExits(globalPath, pH->pLane) && pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i)))
+        {
+          wp = new WayPoint();
+          *wp = *pH->pFronts.at(i);
+
+          double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);
+          distance += d;
+          before_change_distance += d;
+
+          for(unsigned int a = 0; a < wp->actionCost.size(); a++)
+          {
+            //if(wp->actionCost.at(a).first == FORWARD_ACTION)
+              d += wp->actionCost.at(a).second;
+          }
+
+          wp->cost = pH->cost + d;
+          wp->pBacks.push_back(pH);
+
+          nextLeafToTrace.push_back(make_pair(pH, wp));
+          all_cells_to_delete.push_back(wp);
+        }
+      }
+    }
+
+    if(distance > DistanceLimit && globalPath.size()==0)
+    {
+      //if(!pGoalCell)
+      cout << "Goal Not Found, LaneID: " << pH->laneId <<", Distance : " << distance << endl;
+      pGoalCell = pH;
+      break;
+    }
+
+    //pGoalCell = pH;
+  }
+
+  while(nextLeafToTrace.size()!=0)
+    nextLeafToTrace.pop_back();
+  //closed_nodes.clear();
+
+  return pGoalCell;
+}
+
+WayPoint* PlanningHelpers::BuildPlanningSearchTreeStraight(WayPoint* pStart,
+    const double& DistanceLimit,
+    vector<WayPoint*>& all_cells_to_delete)
+{
+  if(!pStart) return NULL;
+
+  vector<pair<WayPoint*, WayPoint*> >nextLeafToTrace;
+
+  WayPoint* pZero = 0;
+  WayPoint* wp    = new WayPoint();
+  *wp = *pStart;
+  wp->cost = 0;
+  nextLeafToTrace.push_back(make_pair(pZero, wp));
+  all_cells_to_delete.push_back(wp);
+
+  double     distance     = 0;
+  WayPoint*   pGoalCell     = 0;
+  double     nCounter     = 0;
+
+  while(nextLeafToTrace.size()>0)
+  {
+    nCounter++;
+
+    unsigned int min_cost_index = 0;
+    double min_cost = DBL_MAX;
+
+    for(unsigned int i=0; i < nextLeafToTrace.size(); i++)
+    {
+      if(nextLeafToTrace.at(i).second->cost < min_cost)
+      {
+        min_cost = nextLeafToTrace.at(i).second->cost;
+        min_cost_index = i;
+      }
+    }
+
+    WayPoint* pH   = nextLeafToTrace.at(min_cost_index).second;
+    assert(pH != 0);
+
+    nextLeafToTrace.erase(nextLeafToTrace.begin()+min_cost_index);
+
+    for(unsigned int i =0; i< pH->pFronts.size(); i++)
+    {
+      if(pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i)))
+      {
+        wp = new WayPoint();
+        *wp = *pH->pFronts.at(i);
+
+        double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);
+        distance += d;
+
+//        for(unsigned int a = 0; a < wp->actionCost.size(); a++)
+//        {
+//          //if(wp->actionCost.at(a).first == FORWARD_ACTION)
+//            d += wp->actionCost.at(a).second;
+//        }
+
+        wp->cost = pH->cost + d;
+        wp->pBacks.push_back(pH);
+        if(wp->cost < DistanceLimit)
+        {
+          nextLeafToTrace.push_back(make_pair(pH, wp));
+          all_cells_to_delete.push_back(wp);
+        }
+        else
+          delete wp;
+      }
+    }
+
+//    if(pH->pLeft && !CheckLaneExits(all_cells_to_delete, pH->pLeft->pLane))
+//    {
+//      wp = new WayPoint();
+//      *wp = *pH->pLeft;
+//      double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);
+//
+//      for(unsigned int a = 0; a < wp->actionCost.size(); a++)
+//      {
+//        //if(wp->actionCost.at(a).first == LEFT_TURN_ACTION)
+//          d += wp->actionCost.at(a).second;
+//      }
+//
+//      wp->cost = pH->cost + d + LANE_CHANGE_COST;
+//      wp->pRight = pH;
+//      wp->pRight = 0;
+//
+//      nextLeafToTrace.push_back(make_pair(pH, wp));
+//      all_cells_to_delete.push_back(wp);
+//    }
+//
+//    if(pH->pRight && !CheckLaneExits(all_cells_to_delete, pH->pRight->pLane))
+//    {
+//      wp = new WayPoint();
+//      *wp = *pH->pRight;
+//      double d = hypot(wp->pos.y - pH->pos.y, wp->pos.x - pH->pos.x);;
+//
+//      for(unsigned int a = 0; a < wp->actionCost.size(); a++)
+//      {
+//        //if(wp->actionCost.at(a).first == RIGHT_TURN_ACTION)
+//          d += wp->actionCost.at(a).second;
+//      }
+//
+//      wp->cost = pH->cost + d + LANE_CHANGE_COST;
+//      wp->pLeft = pH;
+//      wp->pRight = 0;
+//      nextLeafToTrace.push_back(make_pair(pH, wp));
+//      all_cells_to_delete.push_back(wp);
+//    }
+
+    pGoalCell = pH;
+  }
+
+  while(nextLeafToTrace.size()!=0)
+    nextLeafToTrace.pop_back();
+
+  return pGoalCell;
+}
+
+int PlanningHelpers::PredictiveIgnorIdsDP(WayPoint* pStart, const double& DistanceLimit,
+    vector<WayPoint*>& all_cells_to_delete,vector<WayPoint*>& end_waypoints, std::vector<int>& lanes_ids)
+{
+  if(!pStart) return 0;
+
+    vector<pair<WayPoint*, WayPoint*> >nextLeafToTrace;
+
+    WayPoint* pZero = 0;
+    WayPoint* wp    = new WayPoint();
+    *wp = *pStart;
+    wp->cost = 0;
+    wp->pLeft = 0;
+    wp->pRight = 0;
+    nextLeafToTrace.push_back(make_pair(pZero, wp));
+    all_cells_to_delete.push_back(wp);
+
+    double     distance     = 0;
+    end_waypoints.clear();
+    double     nCounter     = 0;
+
+    while(nextLeafToTrace.size()>0)
+    {
+      nCounter++;
+
+      WayPoint* pH   = nextLeafToTrace.at(0).second;
+
+      assert(pH != 0);
+
+      nextLeafToTrace.erase(nextLeafToTrace.begin()+0);
+
+      for(unsigned int i =0; i< pH->pFronts.size(); i++)
+      {
+        if(pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i)))
+        {
+          if(pH->cost < DistanceLimit)
+          {
+            wp = new WayPoint();
+            *wp = *pH->pFronts.at(i);
+
+            double d = distance2points(wp->pos, pH->pos);
+            distance += d;
+            wp->cost = pH->cost + d;
+            wp->pBacks.push_back(pH);
+            wp->pLeft = 0;
+            wp->pRight = 0;
+
+            bool bFoundLane = false;
+            for(unsigned int k = 0 ; k < lanes_ids.size(); k++)
+            {
+              if(wp->laneId == lanes_ids.at(k))
+              {
+                bFoundLane = true;
+                break;
+              }
+            }
+
+            if(!bFoundLane)
+              nextLeafToTrace.push_back(make_pair(pH, wp));
+            all_cells_to_delete.push_back(wp);
+          }
+          else
+          {
+            end_waypoints.push_back(pH);
+          }
+        }
+      }
+    }
+
+    while(nextLeafToTrace.size()!=0)
+      nextLeafToTrace.pop_back();
+    //closed_nodes.clear();
+
+    return end_waypoints.size();
+}
+
+int PlanningHelpers::PredictiveDP(WayPoint* pStart, const double& DistanceLimit,
+    vector<WayPoint*>& all_cells_to_delete,vector<WayPoint*>& end_waypoints)
+{
+  if(!pStart) return 0;
+
+  vector<pair<WayPoint*, WayPoint*> >nextLeafToTrace;
+
+  WayPoint* pZero = 0;
+  WayPoint* wp    = new WayPoint();
+  *wp = *pStart;
+  wp->pLeft = 0;
+  wp->pRight = 0;
+  nextLeafToTrace.push_back(make_pair(pZero, wp));
+  all_cells_to_delete.push_back(wp);
+
+  double     distance     = 0;
+  end_waypoints.clear();
+  double     nCounter     = 0;
+
+  while(nextLeafToTrace.size()>0)
+  {
+    nCounter++;
+
+    WayPoint* pH   = nextLeafToTrace.at(0).second;
+
+    assert(pH != 0);
+
+    nextLeafToTrace.erase(nextLeafToTrace.begin()+0);
+
+    for(unsigned int i =0; i< pH->pFronts.size(); i++)
+    {
+      if(pH->pFronts.at(i) && !CheckNodeExits(all_cells_to_delete, pH->pFronts.at(i)))
+      {
+        if(pH->cost < DistanceLimit)
+        {
+          wp = new WayPoint();
+          *wp = *pH->pFronts.at(i);
+
+          double d = distance2points(wp->pos, pH->pos);
+          distance += d;
+          wp->cost = pH->cost + d;
+          wp->pBacks.push_back(pH);
+          wp->pLeft = 0;
+          wp->pRight = 0;
+
+          nextLeafToTrace.push_back(make_pair(pH, wp));
+          all_cells_to_delete.push_back(wp);
+        }
+        else
+        {
+          end_waypoints.push_back(pH);
+        }
+      }
+    }
+  }
+
+  while(nextLeafToTrace.size()!=0)
+    nextLeafToTrace.pop_back();
+  //closed_nodes.clear();
+
+  return end_waypoints.size();
+}
+
+bool PlanningHelpers::CheckLaneIdExits(const std::vector<int>& lanes, const Lane* pL)
+{
+  if(lanes.size()==0) return true;
+
+  for(unsigned int i=0; i< lanes.size(); i++)
+  {
+    if(lanes.at(i) == pL->id)
+      return true;
+  }
+
+  return false;
+}
+
+WayPoint* PlanningHelpers::CheckLaneExits(const vector<WayPoint*>& nodes, const Lane* pL)
+{
+  if(nodes.size()==0) return nullptr;
+
+  for(unsigned int i=0; i< nodes.size(); i++)
+  {
+    if(nodes.at(i)->pLane == pL)
+      return nodes.at(i);
+  }
+
+  return nullptr;
+}
+
+WayPoint* PlanningHelpers::CheckNodeExits(const vector<WayPoint*>& nodes, const WayPoint* pL)
+{
+  if(nodes.size()==0) return nullptr;
+
+  for(unsigned int i=0; i< nodes.size(); i++)
+  {
+    if(nodes.at(i)->laneId == pL->laneId && nodes.at(i)->id == pL->id)
+      return nodes.at(i);
+  }
+
+  return nullptr;
+}
+
+WayPoint* PlanningHelpers::CreateLaneHeadCell(Lane* pLane, WayPoint* pLeft, WayPoint* pRight,
+    WayPoint* pBack)
+{
+  if(!pLane) return nullptr;
+  if(pLane->points.size()==0) return nullptr;
+
+  WayPoint* c = new WayPoint;
+  c->pLane     = pLane;
+  c->pos       = pLane->points.at(0).pos;
+  c->v      = pLane->speed;
+  c->laneId      = pLane->id;
+  c->pLeft     = pLeft;
+  if(pLeft)
+    c->cost    = pLeft->cost;
+
+  c->pRight    = pRight;
+  if(pRight)
+    c->cost = pRight->cost;
+
+  if(pBack)
+  {
+    pBack->pFronts.push_back(c);
+    c->pBacks.push_back(pBack);
+    c->cost = pBack->cost + distance2points(c->pos, pBack->pos);
+
+    for(unsigned int i=0; i< c->pBacks.size(); i++)
+    {
+        if(c->pBacks.at(i)->cost < c->cost)
+          c->cost = c->pBacks.at(i)->cost;
+    }
+  }
+  return c;
+}
+
+double PlanningHelpers::GetLanePoints(Lane* l, const WayPoint& prevWayPointIndex,
+    const double& minDistance , const double& prevCost, vector<WayPoint>& points)
+{
+  if(l == NULL || minDistance<=0) return 0;
+
+  int index = 0;
+  WayPoint  p1, p2;
+  WayPoint idx;
+
+  p2 = p1 = l->points.at(index);
+  p1.pLane = l;
+  p1.cost = prevCost;
+  p2.cost = p1.cost + distance2points(p1.pos, p2.pos);
+
+  points.push_back(p1);
+
+  for(unsigned int i=index+1; i<l->points.size(); i++)
+  {
+
+    p2 = l->points.at(i);
+    p2.pLane = l;
+    p2.cost = p1.cost + distance2points(p1.pos, p2.pos);
+    points.push_back(p2);
+
+    if(p2.cost >= minDistance)
+        break;
+    p1 = p2;
+  }
+  return p2.cost;
+}
+
+WayPoint* PlanningHelpers::GetMinCostCell(const vector<WayPoint*>& cells, const vector<int>& globalPathIds)
+{
+  if(cells.size() == 1)
+  {
+//    for(unsigned int j = 0; j < cells.at(0)->actionCost.size(); j++)
+//      cout << "Cost (" << cells.at(0)->laneId << ") of going : " << cells.at(0)->actionCost.at(j).first << ", is : " << cells.at(0)->actionCost.at(j).second << endl;
+    return cells.at(0);
+  }
+
+  WayPoint* pC = cells.at(0); //cost is distance
+  for(unsigned int i=1; i < cells.size(); i++)
+  {
+    bool bFound = false;
+    if(globalPathIds.size()==0)
+      bFound = true;
+
+    int iLaneID = cells.at(i)->id;
+    for(unsigned int j=0; j < globalPathIds.size(); j++)
+    {
+      if(globalPathIds.at(j) == iLaneID)
+      {
+        bFound = true;
+        break;
+      }
+    }
+
+//    for(unsigned int j = 0; j < cells.at(0)->actionCost.size(); j++)
+//      cout << "Cost ("<< i <<") of going : " << cells.at(0)->actionCost.at(j).first << ", is : " << cells.at(0)->actionCost.at(j).second << endl;
+
+
+    if(cells.at(i)->cost < pC->cost && bFound == true)
+      pC = cells.at(i);
+  }
+
+
+  return pC;
+}
+
+void PlanningHelpers::ExtractPlanAlernatives(const std::vector<WayPoint>& singlePath, std::vector<std::vector<WayPoint> >& allPaths)
+{
+  if(singlePath.size() == 0)
+    return;
+
+  allPaths.clear();
+  std::vector<WayPoint> path;
+  path.push_back(singlePath.at(0));
+  double skip_distance = 8;
+  double d = 0;
+  bool bStartSkip = false;
+  for(unsigned int i= 1; i < singlePath.size(); i++)
+  {
+    if(singlePath.at(i).bDir != FORWARD_DIR && singlePath.at(i).pLane && singlePath.at(i).pFronts.size() > 0)
+    {
+
+      bStartSkip = true;
+      WayPoint start_point = singlePath.at(i-1);
+
+      cout << "Current Velocity = " << start_point.v << endl;
+
+      RelativeInfo start_info;
+      PlanningHelpers::GetRelativeInfo(start_point.pLane->points, start_point, start_info);
+      vector<WayPoint*> local_cell_to_delete;
+      PlannerHNS::WayPoint* pStart = &start_point.pLane->points.at(start_info.iFront);
+      WayPoint* pLaneCell =  PlanningHelpers::BuildPlanningSearchTreeStraight(pStart, BACKUP_STRAIGHT_PLAN_DISTANCE, local_cell_to_delete);
+      if(pLaneCell)
+      {
+        vector<WayPoint> straight_path;
+        vector<vector<WayPoint> > tempCurrentForwardPathss;
+        vector<int> globalPathIds;
+        PlanningHelpers::TraversePathTreeBackwards(pLaneCell, pStart, globalPathIds, straight_path, tempCurrentForwardPathss);
+        if(straight_path.size() > 2)
+        {
+          straight_path.insert(straight_path.begin(), path.begin(), path.end());
+          for(unsigned int ic = 0; ic < straight_path.size(); ic++)
+            straight_path.at(ic).laneChangeCost = 1;
+          allPaths.push_back(straight_path);
+        }
+      }
+    }
+
+    if(bStartSkip)
+    {
+      d += hypot(singlePath.at(i).pos.y - singlePath.at(i-1).pos.y, singlePath.at(i).pos.x - singlePath.at(i-1).pos.x);
+      if(d > skip_distance)
+      {
+        d = 0;
+        bStartSkip = false;
+      }
+    }
+
+    if(!bStartSkip)
+      path.push_back(singlePath.at(i));
+  }
+
+  allPaths.push_back(path);
+}
+
+void PlanningHelpers::TraversePathTreeBackwards(WayPoint* pHead, WayPoint* pStartWP,const vector<int>& globalPathIds,
+    vector<WayPoint>& localPath, std::vector<std::vector<WayPoint> >& localPaths)
+{
+  if(pHead != NULL && pHead->id != pStartWP->id)
+  {
+    if(pHead->pBacks.size()>0)
+    {
+      localPaths.push_back(localPath);
+      TraversePathTreeBackwards(GetMinCostCell(pHead->pBacks, globalPathIds),pStartWP, globalPathIds, localPath, localPaths);
+      pHead->bDir = FORWARD_DIR;
+      localPath.push_back(*pHead);
+    }
+    else if(pHead->pLeft && pHead->cost > 0)
+    {
+      //vector<Vector2D> forward_path;
+      //TravesePathTreeForwards(pHead->pLeft, forward_path, FORWARD_RIGHT);
+      //localPaths.push_back(forward_path);
+      cout << "Global Lane Change  Right " << endl;
+      TraversePathTreeBackwards(pHead->pLeft,pStartWP, globalPathIds, localPath, localPaths);
+      pHead->bDir = FORWARD_RIGHT_DIR;
+      localPath.push_back(*pHead);
+    }
+    else if(pHead->pRight && pHead->cost > 0)
+    {
+      //vector<Vector2D> forward_path;
+      //TravesePathTreeForwards(pHead->pRight, forward_path, FORWARD_LEFT);
+      //localPaths.push_back(forward_path);
+
+      cout << "Global Lane Change  Left " << endl;
+      TraversePathTreeBackwards(pHead->pRight,pStartWP, globalPathIds, localPath, localPaths);
+      pHead->bDir = FORWARD_LEFT_DIR;
+      localPath.push_back(*pHead);
+    }
+//    else
+//      cout << "Err: PlannerZ -> NULL Back Pointer " << pHead;
+  }
+  else
+    assert(pHead);
+}
+
+ACTION_TYPE PlanningHelpers::GetBranchingDirection(WayPoint& currWP, WayPoint& nextWP)
+{
+  ACTION_TYPE t = FORWARD_ACTION;
+
+//  //first Get the average of the next 3 waypoint directions
+//  double angle = 0;
+//  if(nextWP.pLane->id == 487)
+//    angle = 11;
+//
+//  int counter = 0;
+//  angle = 0;
+//
+//  for(unsigned int i=0; i < nextWP.pLane->points.size() && counter < 10; i++, counter++)
+//  {
+//    angle += nextWP.pLane->points.at(i).pos.a;
+//  }
+//  angle = angle / counter;
+//
+//  //Get Circular angle for correct subtraction
+//  double circle_angle = UtilityH::GetCircularAngle(currWP.pos.a, angle);
+//
+//  if( currWP.pos.a - circle_angle > (7.5*DEG2RAD))
+//  {
+//    t = RIGHT_TURN_ACTION;
+//    cout << "Right Lane, Average Angle = " << angle*RAD2DEG << ", Circle Angle = " << circle_angle*RAD2DEG << ", currAngle = " << currWP.pos.a*RAD2DEG << endl;
+//  }
+//  else if( currWP.pos.a - circle_angle < (-7.5*DEG2RAD))
+//  {
+//    t = LEFT_TURN_ACTION;
+//    cout << "Left Lane, Average Angle = " << angle*RAD2DEG << ", Circle Angle = " << circle_angle*RAD2DEG << ", currAngle = " << currWP.pos.a*RAD2DEG << endl;
+//  }
+
+  return t;
+}
+
+void PlanningHelpers::CalcContourPointsForDetectedObjects(const WayPoint& currPose, vector<DetectedObject>& obj_list, const double& filterDistance)
+{
+  vector<DetectedObject> res_list;
+  for(unsigned int i = 0; i < obj_list.size(); i++)
+  {
+    GPSPoint center = obj_list.at(i).center.pos;
+    double distance = distance2points(center, currPose.pos);
+
+    if(distance < filterDistance)
+    {
+      DetectedObject obj = obj_list.at(i);
+
+      Mat3 rotationMat(center.a);
+      Mat3 translationMat(center.x, center.y);
+      double w2 = obj.w/2.0;
+      double h2 = obj.l/2.0;
+      double z = center.z + obj.h/2.0;
+
+      GPSPoint left_bottom(-w2, -h2, z,0);
+      GPSPoint right_bottom(w2,-h2, z,0);
+      GPSPoint right_top(w2,h2, z,0);
+      GPSPoint left_top(-w2,h2, z,0);
+
+      left_bottom   = rotationMat * left_bottom;
+      right_bottom   = rotationMat * right_bottom;
+      right_top     = rotationMat * right_top;
+      left_top     = rotationMat * left_top;
+
+      left_bottom   = translationMat * left_bottom;
+      right_bottom   = translationMat * right_bottom;
+      right_top     = translationMat * right_top;
+      left_top     = translationMat * left_top;
+
+      obj.contour.clear();
+      obj.contour.push_back(left_bottom);
+      obj.contour.push_back(right_bottom);
+      obj.contour.push_back(right_top);
+      obj.contour.push_back(left_top);
+
+      res_list.push_back(obj);
+    }
+  }
+
+  obj_list = res_list;
+}
+
+double PlanningHelpers::GetVelocityAhead(const std::vector<WayPoint>& path, const RelativeInfo& info, int& prev_index, const double& reasonable_brake_distance)
+{
+  if(path.size()==0) return 0;
+
+
+  double min_v = path.at(info.iBack).v;
+  double d = info.to_front_distance;
+
+  int local_i = info.iFront;
+  while(local_i < path.size()-1 && d < reasonable_brake_distance)
+  {
+    local_i++;
+    d += hypot(path.at(local_i).pos.y - path.at(local_i-1).pos.y, path.at(local_i).pos.x - path.at(local_i-1).pos.x);
+    if(path.at(local_i).v < min_v)
+      min_v = path.at(local_i).v;
+  }
+
+  if(local_i < prev_index && prev_index < path.size())
+  {
+    min_v = path.at(prev_index).v;
+  }
+  else
+    prev_index = local_i;
+
+  return min_v;
+}
+
+void PlanningHelpers::WritePathToFile(const string& fileName, const vector<WayPoint>& path)
+{
+  DataRW  dataFile;
+  ostringstream str_header;
+  str_header << "laneID" << "," << "wpID"  << "," "x" << "," << "y" << "," << "a"<<","<< "cost" << "," << "Speed" << "," ;
+  vector<string> dataList;
+   for(unsigned int i=0; i<path.size(); i++)
+   {
+     ostringstream strwp;
+     strwp << path.at(i).laneId << "," << path.at(i).id <<","<<path.at(i).pos.x<<","<< path.at(i).pos.y
+         <<","<< path.at(i).pos.a << "," << path.at(i).cost << "," << path.at(i).v << ",";
+     dataList.push_back(strwp.str());
+   }
+
+   dataFile.WriteLogData("", fileName, str_header.str(), dataList);
+}
+
+LIGHT_INDICATOR PlanningHelpers::GetIndicatorsFromPath(const std::vector<WayPoint>& path, const WayPoint& pose,  const double& seachDistance)
+{
+  if(path.size() < 2)
+    return INDICATOR_NONE;
+
+  LIGHT_INDICATOR ind = INDICATOR_NONE;
+  RelativeInfo info;
+  PlanningHelpers::GetRelativeInfo(path, pose, info);
+
+  if(info.perp_point.actionCost.size() > 0)
+  {
+    if(info.perp_point.actionCost.at(0).first == LEFT_TURN_ACTION)
+      ind = INDICATOR_LEFT;
+    else if(info.perp_point.actionCost.at(0).first == RIGHT_TURN_ACTION)
+      ind = INDICATOR_RIGHT;
+  }
+
+  double total_d = 0;
+  for(unsigned int i=info.iFront; i < path.size()-2; i++)
+  {
+
+    total_d+= hypot(path.at(i+1).pos.y - path.at(i).pos.y, path.at(i+1).pos.x - path.at(i).pos.x);
+    if(path.at(i).actionCost.size() > 0)
+    {
+      if(path.at(i).actionCost.at(0).first == LEFT_TURN_ACTION)
+        return INDICATOR_LEFT;
+      else if(path.at(i).actionCost.at(0).first == RIGHT_TURN_ACTION)
+        return INDICATOR_RIGHT;
+    }
+
+    if(total_d > seachDistance)
+      break;
+  }
+
+  return ind;
+}
+
+PlannerHNS::WayPoint PlanningHelpers::GetRealCenter(const PlannerHNS::WayPoint& currState, const double& wheel_base)
+{
+  PlannerHNS::WayPoint pose_center = currState;
+  PlannerHNS::Mat3 rotationMat(-currState.pos.a);
+  PlannerHNS::Mat3 translationMat(-currState.pos.x, -currState.pos.y);
+
+  PlannerHNS::Mat3 rotationMatInv(currState.pos.a);
+  PlannerHNS::Mat3 translationMatInv(currState.pos.x, currState.pos.y);
+
+  pose_center.pos = translationMat*pose_center.pos;
+  pose_center.pos = rotationMat*pose_center.pos;
+
+  pose_center.pos.x += wheel_base/3.0;
+
+  pose_center.pos = rotationMatInv*pose_center.pos;
+  pose_center.pos = translationMatInv*pose_center.pos;
+
+  return pose_center;
+}
+
+double PlanningHelpers::frunge ( double x )
+{
+  double fx;
+
+  fx = 1.0 / ( 1.0 + 25.0 * x * x );
+
+  return fx;
+}
+
+double PlanningHelpers::fprunge ( double x )
+{
+  double bot;
+  double fx;
+
+  bot = 1.0 + 25.0 * x * x;
+  fx = -50.0 * x / ( bot * bot );
+
+  return fx;
+}
+
+double PlanningHelpers::fpprunge ( double x )
+{
+  double bot;
+  double fx;
+
+  bot = 1.0 + 25.0 * x * x;
+  fx = ( -50.0 + 3750.0 * x * x ) / ( bot * bot * bot );
+
+  return fx;
+}
+
+
+} /* namespace PlannerHNS */

src1/planning/op_planner/Readme.md

@@ -0,0 +1 @@
+include vector_map_msgs copy from ros.

src1/planning/op_planner/SimuDecisionMaker.cpp

@@ -0,0 +1,250 @@
+/// \file  SimuDecisionMaker.cpp
+/// \brief Decision Maker for Simulated Vehicles
+/// \author Hatem Darweesh
+/// \date Jan 10, 2018
+
+#include "op_planner/SimuDecisionMaker.h"
+#include "op_utility/UtilityH.h"
+#include "op_planner/PlanningHelpers.h"
+#include "op_planner/MappingHelpers.h"
+#include "op_planner/MatrixOperations.h"
+#include "op_planner/PlannerH.h"
+
+
+namespace PlannerHNS
+{
+
+SimuDecisionMaker::SimuDecisionMaker()
+{
+  m_CurrentVelocity =  m_CurrentVelocityD =0;
+  m_CurrentSteering = m_CurrentSteeringD =0;
+  m_CurrentShift     =  m_CurrentShiftD = PlannerHNS::SHIFT_POS_NN;
+  m_CurrentAccSteerAngle = m_CurrentAccVelocity = 0;
+
+  m_SimulationSteeringDelayFactor = 0.1;
+  UtilityHNS::UtilityH::GetTickCount(m_SteerDelayTimer);
+}
+
+SimuDecisionMaker::~SimuDecisionMaker()
+{
+}
+
+void SimuDecisionMaker::ReInitializePlanner(const WayPoint& start_pose)
+{
+  m_pidVelocity.Init(0.01, 0.004, 0.01);
+  m_pidVelocity.Setlimit(m_params.maxSpeed, 0);
+
+  m_pidStopping.Init(0.05, 0.05, 0.1);
+  m_pidStopping.Setlimit(m_params.horizonDistance, 0);
+
+  m_pidFollowing.Init(0.05, 0.05, 0.01);
+  m_pidFollowing.Setlimit(m_params.minFollowingDistance, 0);
+
+  m_iCurrentTotalPathId = 0;
+  m_CurrentVelocity =  m_CurrentVelocityD =0;
+  m_CurrentSteering = m_CurrentSteeringD =0;
+  m_CurrentShift     =  m_CurrentShiftD = SHIFT_POS_NN;
+  m_CurrentAccSteerAngle = m_CurrentAccVelocity = 0;
+
+  m_pCurrentBehaviorState = m_pFollowState;
+  m_TotalPath.clear();
+  m_TotalOriginalPath.clear();
+  m_Path.clear();
+  m_RollOuts.clear();
+  m_pCurrentBehaviorState->m_Behavior = PlannerHNS::FORWARD_STATE;
+  FirstLocalizeMe(start_pose);
+  LocalizeMe(0);
+}
+
+void SimuDecisionMaker::SetSimulatedTargetOdometryReadings(const double& velocity_d, const double& steering_d, const SHIFT_POS& shift_d)
+{
+  m_CurrentVelocityD = velocity_d;
+  m_CurrentSteeringD = steering_d;
+  m_CurrentShiftD = shift_d;
+}
+
+void SimuDecisionMaker::FirstLocalizeMe(const WayPoint& initCarPos)
+ {
+  pLane = initCarPos.pLane;
+  state = initCarPos;
+  m_OdometryState.pos.a = initCarPos.pos.a;
+  m_OdometryState.pos.x = initCarPos.pos.x + (m_CarInfo.wheel_base/2.0 * cos(initCarPos.pos.a));
+  m_OdometryState.pos.y = initCarPos.pos.y + (m_CarInfo.wheel_base/2.0 * sin(initCarPos.pos.a));
+ }
+
+ void SimuDecisionMaker::LocalizeMe(const double& dt)
+{
+  //calculate the new x, y ,
+   WayPoint currPose = state;
+
+  if(m_CurrentShift == SHIFT_POS_DD)
+  {
+    m_OdometryState.pos.x   +=  m_CurrentVelocity * dt * cos(currPose.pos.a);
+    m_OdometryState.pos.y   +=  m_CurrentVelocity * dt * sin(currPose.pos.a);
+    m_OdometryState.pos.a   +=  m_CurrentVelocity * dt * tan(m_CurrentSteering)  / m_CarInfo.wheel_base;
+
+  }
+  else if(m_CurrentShift == SHIFT_POS_RR )
+  {
+    m_OdometryState.pos.x   +=  -m_CurrentVelocity * dt * cos(currPose.pos.a);
+    m_OdometryState.pos.y   +=  -m_CurrentVelocity * dt * sin(currPose.pos.a);
+    m_OdometryState.pos.a   +=  -m_CurrentVelocity * dt * tan(m_CurrentSteering);
+  }
+
+  m_OdometryState.pos.a = atan2(sin(m_OdometryState.pos.a), cos(m_OdometryState.pos.a));
+  m_OdometryState.pos.a = UtilityHNS::UtilityH::FixNegativeAngle(m_OdometryState.pos.a);
+
+  state.pos.a = m_OdometryState.pos.a;
+  state.pos.x = m_OdometryState.pos.x   - (m_CurrentVelocity*dt* (m_CarInfo.wheel_base) * cos (m_OdometryState.pos.a));
+  state.pos.y = m_OdometryState.pos.y   - (m_CurrentVelocity*dt* (m_CarInfo.wheel_base/2.0) * sin (m_OdometryState.pos.a));
+}
+
+ void SimuDecisionMaker::UpdateState(const PlannerHNS::VehicleState& state, const bool& bUseDelay)
+  {
+   if(!bUseDelay)
+   {
+     m_CurrentSteering   = m_CurrentSteeringD;
+   }
+   else
+   {
+     double currSteerDeg = RAD2DEG * m_CurrentSteering;
+     double desiredSteerDeg = RAD2DEG * m_CurrentSteeringD;
+
+     double mFact = UtilityHNS::UtilityH::GetMomentumScaleFactor(state.speed);
+     double diff = desiredSteerDeg - currSteerDeg;
+     double diffSign = UtilityHNS::UtilityH::GetSign(diff);
+     double inc = 1.0*diffSign;
+     if(fabs(diff) < 1.0 )
+       inc = diff;
+
+     if(UtilityHNS::UtilityH::GetTimeDiffNow(m_SteerDelayTimer) > m_SimulationSteeringDelayFactor*mFact)
+     {
+       UtilityHNS::UtilityH::GetTickCount(m_SteerDelayTimer);
+       currSteerDeg += inc;
+     }
+
+     m_CurrentSteering = DEG2RAD * currSteerDeg;
+   }
+
+   m_CurrentShift   = m_CurrentShiftD;
+   m_CurrentVelocity = m_CurrentVelocityD;
+  }
+
+ void SimuDecisionMaker::GenerateLocalRollOuts()
+ {
+  std::vector<PlannerHNS::WayPoint> sampledPoints_debug;
+  std::vector<std::vector<std::vector<PlannerHNS::WayPoint> > > _roll_outs;
+  PlannerHNS::PlannerH _planner;
+  _planner.GenerateRunoffTrajectory(m_TotalPath, state,
+            m_params.enableLaneChange,
+            state.v,
+            m_params.microPlanDistance,
+            m_params.maxSpeed,
+            m_params.minSpeed,
+            m_params.carTipMargin,
+            m_params.rollInMargin,
+            m_params.rollInSpeedFactor,
+            m_params.pathDensity,
+            m_params.rollOutDensity,
+            m_params.rollOutNumber,
+            m_params.smoothingDataWeight,
+            m_params.smoothingSmoothWeight,
+            m_params.smoothingToleranceError,
+            m_params.speedProfileFactor,
+            m_params.enableHeadingSmoothing,
+            -1 , -1,
+            _roll_outs, sampledPoints_debug);
+
+  if(_roll_outs.size()>0)
+    m_RollOuts.clear();
+  for(unsigned int i=0; i < _roll_outs.size(); i++)
+  {
+    for(unsigned int j=0; j < _roll_outs.at(i).size(); j++)
+    {
+      PlannerHNS::PlanningHelpers::PredictConstantTimeCostForTrajectory(_roll_outs.at(i).at(j), state, m_params.minSpeed, m_params.microPlanDistance);
+      m_RollOuts.push_back(_roll_outs.at(i).at(j));
+    }
+  }
+ }
+
+ bool SimuDecisionMaker::SelectSafeTrajectory()
+ {
+   bool bNewTrajectory = false;
+   PlannerHNS::PreCalculatedConditions *preCalcPrams = m_pCurrentBehaviorState->GetCalcParams();
+
+   if(!preCalcPrams || m_TotalPath.size()==0) return bNewTrajectory;
+
+  int currIndex = PlannerHNS::PlanningHelpers::GetClosestNextPointIndexFast(m_Path, state);
+  int index_limit = 0;
+  if(index_limit<=0)
+    index_limit =  m_Path.size()/2.0;
+  if(currIndex > index_limit
+      || preCalcPrams->bRePlan
+      || preCalcPrams->bNewGlobalPath)
+  {
+    GenerateLocalRollOuts();
+    if(m_RollOuts.size() <= preCalcPrams->iCurrSafeTrajectory)
+      return false;
+
+    std::cout << "New Local Plan !! " << currIndex << ", "<< preCalcPrams->bRePlan << ", " << preCalcPrams->bNewGlobalPath  << ", " <<  m_TotalOriginalPath.at(0).size() << ", PrevLocal: " << m_Path.size();
+    std::cout << ", NewLocal: " << m_Path.size() << std::endl;
+
+    m_Path = m_RollOuts.at(preCalcPrams->iCurrSafeTrajectory);
+    preCalcPrams->bNewGlobalPath = false;
+    preCalcPrams->bRePlan = false;
+    bNewTrajectory = true;
+  }
+
+  return bNewTrajectory;
+ }
+
+ PlannerHNS::VehicleState SimuDecisionMaker::LocalizeStep(const double& dt, const PlannerHNS::VehicleState& desiredStatus)
+ {
+  SetSimulatedTargetOdometryReadings(desiredStatus.speed, desiredStatus.steer, desiredStatus.shift);
+  UpdateState(desiredStatus, false);
+  LocalizeMe(dt);
+  PlannerHNS::VehicleState currStatus;
+  currStatus.shift = desiredStatus.shift;
+  currStatus.steer = m_CurrentSteering;
+  currStatus.speed = m_CurrentVelocity;
+  return currStatus;
+ }
+
+ PlannerHNS::BehaviorState SimuDecisionMaker::DoOneStep(const double& dt,
+    const PlannerHNS::VehicleState& vehicleState,
+    const int& goalID,
+    const std::vector<TrafficLight>& trafficLight,
+    const std::vector<PlannerHNS::DetectedObject>& objects,
+    const bool& bEmergencyStop)
+{
+   PlannerHNS::BehaviorState beh;
+   state.v = vehicleState.speed;
+   m_TotalPath.clear();
+  for(unsigned int i = 0; i < m_TotalOriginalPath.size(); i++)
+  {
+    t_centerTrajectorySmoothed.clear();
+    PlannerHNS::PlanningHelpers::ExtractPartFromPointToDistanceDirectionFast(m_TotalOriginalPath.at(i), state, m_params.horizonDistance ,  m_params.pathDensity , t_centerTrajectorySmoothed);
+    m_TotalPath.push_back(t_centerTrajectorySmoothed);
+  }
+
+  if(m_TotalPath.size()==0) return beh;
+
+  UpdateCurrentLane(m_MaxLaneSearchDistance);
+
+  PlannerHNS::TrajectoryCost tc = m_TrajectoryCostsCalculator.DoOneStepStatic(m_RollOuts, m_TotalPath.at(m_iCurrentTotalPathId), state,  m_params, m_CarInfo, vehicleState, objects);
+
+  //std::cout << "Detected Objects Distance: " << tc.closest_obj_distance << ", N RollOuts: " << m_RollOuts.size() << std::endl;
+
+  CalculateImportantParameterForDecisionMaking(vehicleState, goalID, bEmergencyStop, trafficLight, tc);
+
+  beh = GenerateBehaviorState(vehicleState);
+
+  beh.bNewPlan = SelectSafeTrajectory();
+
+  beh.maxVelocity = UpdateVelocityDirectlyToTrajectory(beh, vehicleState, dt);
+
+  //std::cout << "Eval_i: " << tc.index << ", Curr_i: " <<  m_pCurrentBehaviorState->GetCalcParams()->iCurrSafeTrajectory << ", Prev_i: " << m_pCurrentBehaviorState->GetCalcParams()->iPrevSafeTrajectory << std::endl;
+
+  return beh;
+ }
+}

src1/planning/op_planner/TrajectoryCosts.cpp

@@ -0,0 +1,390 @@
+/// \file TrajectoryCosts.cpp
+/// \brief Calculate collision costs for roll out trajectory for free trajectory evaluation for dp_planner
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+#include "op_planner/TrajectoryCosts.h"
+#include "op_planner/MatrixOperations.h"
+#include "float.h"
+
+namespace PlannerHNS
+{
+
+TrajectoryCosts::TrajectoryCosts()
+{
+  m_PrevCostIndex = -1;
+  m_WeightPriority = 0.125;
+  m_WeightTransition = 0.13;
+  m_WeightLong = 1.0;
+  m_WeightLat = 1.0;
+  m_WeightLaneChange = 1.0;
+  m_LateralSkipDistance = 10;
+}
+
+TrajectoryCosts::~TrajectoryCosts()
+{
+}
+
+TrajectoryCost TrajectoryCosts::DoOneStep(const vector<vector<vector<WayPoint> > >& rollOuts,
+    const vector<vector<WayPoint> >& totalPaths, const WayPoint& currState, const int& currIndex,
+    const int& currLaneIndex,
+    const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
+    const std::vector<PlannerHNS::DetectedObject>& obj_list)
+{
+  TrajectoryCost bestTrajectory;
+  bestTrajectory.bBlocked = true;
+  bestTrajectory.closest_obj_distance = params.horizonDistance;
+  bestTrajectory.closest_obj_velocity = 0;
+  bestTrajectory.index = -1;
+
+  if(!ValidateRollOutsInput(rollOuts) || rollOuts.size() != totalPaths.size()) return bestTrajectory;
+
+  if(m_PrevCostIndex == -1)
+    m_PrevCostIndex = params.rollOutNumber/2;
+
+  m_TrajectoryCosts.clear();
+
+  for(unsigned int il = 0; il < rollOuts.size(); il++)
+  {
+    if(rollOuts.at(il).size()>0 && rollOuts.at(il).at(0).size()>0)
+    {
+      vector<TrajectoryCost> costs = CalculatePriorityAndLaneChangeCosts(rollOuts.at(il), il, params);
+      m_TrajectoryCosts.insert(m_TrajectoryCosts.end(), costs.begin(), costs.end());
+    }
+  }
+
+  CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
+
+
+  WayPoint p;
+  m_AllContourPoints.clear();
+  for(unsigned int io=0; io<obj_list.size(); io++)
+  {
+    for(unsigned int icon=0; icon < obj_list.at(io).contour.size(); icon++)
+    {
+      p.pos = obj_list.at(io).contour.at(icon);
+      p.v = obj_list.at(io).center.v;
+      p.id = io;
+      p.cost = sqrt(obj_list.at(io).w*obj_list.at(io).w + obj_list.at(io).l*obj_list.at(io).l);
+      m_AllContourPoints.push_back(p);
+    }
+  }
+
+  CalculateLateralAndLongitudinalCosts(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);
+
+  NormalizeCosts(m_TrajectoryCosts);
+
+  int smallestIndex = -1;
+  double smallestCost = DBL_MAX;
+  double smallestDistance = DBL_MAX;
+  double velo_of_next = 0;
+
+  //cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
+  for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
+  {
+    //cout << m_TrajectoryCosts.at(ic).ToString();
+    if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
+    {
+      smallestCost = m_TrajectoryCosts.at(ic).cost;
+      smallestIndex = ic;
+    }
+
+    if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
+    {
+      smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
+      velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+    }
+  }
+
+  //cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;
+
+  //All is blocked !
+  if(smallestIndex == -1 && m_PrevCostIndex < (int)m_TrajectoryCosts.size())
+  {
+    bestTrajectory.bBlocked = true;
+    bestTrajectory.lane_index = currLaneIndex;
+    bestTrajectory.index = currIndex;
+    bestTrajectory.closest_obj_distance = smallestDistance;
+    bestTrajectory.closest_obj_velocity = velo_of_next;
+    //bestTrajectory.index = smallestIndex;
+  }
+  else if(smallestIndex >= 0)
+  {
+    bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
+    //bestTrajectory.index = smallestIndex;
+  }
+
+//  cout << "smallestI: " <<  smallestIndex << ", C_Size: " << m_TrajectoryCosts.size()
+//      << ", LaneI: " << bestTrajectory.lane_index << "TrajI: " << bestTrajectory.index
+//      << ", prevSmalI: " << m_PrevCostIndex << ", distance: " << bestTrajectory.closest_obj_distance
+//      << ", Blocked: " << bestTrajectory.bBlocked
+//      << endl;
+
+  m_PrevCostIndex = smallestIndex;
+
+  return bestTrajectory;
+}
+
+void TrajectoryCosts::CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost>& trajectoryCosts,
+    const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths,
+    const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params,
+    const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState)
+{
+  double critical_lateral_distance =  carInfo.width/2.0 + params.horizontalSafetyDistancel;
+  double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance;
+  double critical_long_back_distance =  carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0;
+  int iCostIndex = 0;
+
+  PlannerHNS::Mat3 invRotationMat(currState.pos.a-M_PI_2);
+  PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
+
+  double corner_slide_distance = critical_lateral_distance/2.0;
+  double ratio_to_angle = corner_slide_distance/carInfo.max_steer_angle;
+  double slide_distance = vehicleState.steer * ratio_to_angle;
+
+  GPSPoint bottom_left(-critical_lateral_distance ,-critical_long_back_distance,  currState.pos.z, 0);
+  GPSPoint bottom_right(critical_lateral_distance, -critical_long_back_distance,  currState.pos.z, 0);
+
+  GPSPoint top_right_car(critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0,  currState.pos.z, 0);
+  GPSPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0);
+
+  GPSPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance,  currState.pos.z, 0);
+  GPSPoint top_left(-critical_lateral_distance - slide_distance , critical_long_front_distance, currState.pos.z, 0);
+
+  bottom_left = invRotationMat*bottom_left;
+  bottom_left = invTranslationMat*bottom_left;
+
+  top_right = invRotationMat*top_right;
+  top_right = invTranslationMat*top_right;
+
+  bottom_right = invRotationMat*bottom_right;
+  bottom_right = invTranslationMat*bottom_right;
+
+  top_left = invRotationMat*top_left;
+  top_left = invTranslationMat*top_left;
+
+  top_right_car = invRotationMat*top_right_car;
+  top_right_car = invTranslationMat*top_right_car;
+
+  top_left_car = invRotationMat*top_left_car;
+  top_left_car = invTranslationMat*top_left_car;
+
+  m_SafetyBorder.points.clear();
+  m_SafetyBorder.points.push_back(bottom_left) ;
+  m_SafetyBorder.points.push_back(bottom_right) ;
+  m_SafetyBorder.points.push_back(top_right_car) ;
+  m_SafetyBorder.points.push_back(top_right) ;
+  m_SafetyBorder.points.push_back(top_left) ;
+  m_SafetyBorder.points.push_back(top_left_car) ;
+
+  for(unsigned int il=0; il < rollOuts.size(); il++)
+  {
+    if(rollOuts.at(il).size() > 0 && rollOuts.at(il).at(0).size()>0)
+    {
+      RelativeInfo car_info;
+      PlanningHelpers::GetRelativeInfo(totalPaths.at(il), currState, car_info);
+
+
+      for(unsigned int it=0; it< rollOuts.at(il).size(); it++)
+      {
+        int skip_id = -1;
+        for(unsigned int icon = 0; icon < contourPoints.size(); icon++)
+        {
+          if(skip_id == contourPoints.at(icon).id)
+            continue;
+
+          RelativeInfo obj_info;
+          PlanningHelpers::GetRelativeInfo(totalPaths.at(il), contourPoints.at(icon), obj_info);
+          double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths.at(il), car_info, obj_info);
+          if(obj_info.iFront == 0 && longitudinalDist > 0)
+            longitudinalDist = -longitudinalDist;
+
+          double direct_distance = hypot(obj_info.perp_point.pos.y-contourPoints.at(icon).pos.y, obj_info.perp_point.pos.x-contourPoints.at(icon).pos.x);
+          if(contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance+contourPoints.at(icon).cost))
+          {
+            skip_id = contourPoints.at(icon).id;
+            continue;
+          }
+
+          double close_in_percentage = 1;
+//          close_in_percentage = ((longitudinalDist- critical_long_front_distance)/params.rollInMargin)*4.0;
+//
+//          if(close_in_percentage <= 0 || close_in_percentage > 1) close_in_percentage = 1;
+
+          double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center;
+
+          if(close_in_percentage < 1)
+            distance_from_center = distance_from_center - distance_from_center * (1.0-close_in_percentage);
+
+          double lateralDist = fabs(obj_info.perp_distance - distance_from_center);
+
+          if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance)
+          {
+            continue;
+          }
+
+          longitudinalDist = longitudinalDist - critical_long_front_distance;
+
+          if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true)
+            trajectoryCosts.at(iCostIndex).bBlocked = true;
+
+          if(lateralDist <= critical_lateral_distance
+              && longitudinalDist >= -carInfo.length/1.5
+              && longitudinalDist < params.minFollowingDistance)
+            trajectoryCosts.at(iCostIndex).bBlocked = true;
+
+
+          if(lateralDist != 0)
+            trajectoryCosts.at(iCostIndex).lateral_cost += 1.0/lateralDist;
+
+          if(longitudinalDist != 0)
+            trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0/fabs(longitudinalDist);
+
+
+          if(longitudinalDist >= -critical_long_front_distance && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance)
+          {
+            trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
+            trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
+          }
+        }
+
+        iCostIndex++;
+      }
+    }
+  }
+}
+
+void TrajectoryCosts::NormalizeCosts(vector<TrajectoryCost>& trajectoryCosts)
+{
+  //Normalize costs
+  double totalPriorities = 0;
+  double totalChange = 0;
+  double totalLateralCosts = 0;
+  double totalLongitudinalCosts = 0;
+  double transitionCosts = 0;
+
+  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
+  {
+    totalPriorities += trajectoryCosts.at(ic).priority_cost;
+    transitionCosts += trajectoryCosts.at(ic).transition_cost;
+  }
+
+  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
+  {
+    totalChange += trajectoryCosts.at(ic).lane_change_cost;
+    totalLateralCosts += trajectoryCosts.at(ic).lateral_cost;
+    totalLongitudinalCosts += trajectoryCosts.at(ic).longitudinal_cost;
+  }
+
+//  cout << "------ Normalizing Step " << endl;
+  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
+  {
+    if(totalPriorities != 0)
+      trajectoryCosts.at(ic).priority_cost = trajectoryCosts.at(ic).priority_cost / totalPriorities;
+    else
+      trajectoryCosts.at(ic).priority_cost = 0;
+
+    if(transitionCosts != 0)
+      trajectoryCosts.at(ic).transition_cost = trajectoryCosts.at(ic).transition_cost / transitionCosts;
+    else
+      trajectoryCosts.at(ic).transition_cost = 0;
+
+    if(totalChange != 0)
+      trajectoryCosts.at(ic).lane_change_cost = trajectoryCosts.at(ic).lane_change_cost / totalChange;
+    else
+      trajectoryCosts.at(ic).lane_change_cost = 0;
+
+    if(totalLateralCosts != 0)
+      trajectoryCosts.at(ic).lateral_cost = trajectoryCosts.at(ic).lateral_cost / totalLateralCosts;
+    else
+      trajectoryCosts.at(ic).lateral_cost = 0;
+
+    if(totalLongitudinalCosts != 0)
+      trajectoryCosts.at(ic).longitudinal_cost = trajectoryCosts.at(ic).longitudinal_cost / totalLongitudinalCosts;
+    else
+      trajectoryCosts.at(ic).longitudinal_cost = 0;
+
+    trajectoryCosts.at(ic).priority_cost = m_WeightPriority*trajectoryCosts.at(ic).priority_cost;
+    trajectoryCosts.at(ic).transition_cost = m_WeightTransition*trajectoryCosts.at(ic).transition_cost;
+    trajectoryCosts.at(ic).lane_change_cost = m_WeightLaneChange*trajectoryCosts.at(ic).lane_change_cost;
+    trajectoryCosts.at(ic).lateral_cost = m_WeightLat*trajectoryCosts.at(ic).lateral_cost;
+    trajectoryCosts.at(ic).longitudinal_cost = m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost;
+
+
+    trajectoryCosts.at(ic).cost = m_WeightPriority*trajectoryCosts.at(ic).priority_cost/5.0 +
+        m_WeightLaneChange*trajectoryCosts.at(ic).lane_change_cost/5.0 +
+        m_WeightLat*trajectoryCosts.at(ic).lateral_cost/5.0 +
+        m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost/5.0 +
+        m_WeightTransition*trajectoryCosts.at(ic).transition_cost/5.0;
+
+//    cout << "Index: " << ic
+//            << ", Priority: " << trajectoryCosts.at(ic).priority_cost
+//            << ", Transition: " << trajectoryCosts.at(ic).transition_cost
+//            << ", Lat: " << trajectoryCosts.at(ic).lateral_cost
+//            << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost
+//            << ", Change: " << trajectoryCosts.at(ic).lane_change_cost
+//            << ", Avg: " << trajectoryCosts.at(ic).cost
+//            << endl;
+  }
+
+//  cout << "------------------------ " << endl;
+}
+
+vector<TrajectoryCost> TrajectoryCosts::CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint> >& laneRollOuts,
+    const int& lane_index, const PlanningParams& params)
+{
+  vector<TrajectoryCost> costs;
+  TrajectoryCost tc;
+  int centralIndex = params.rollOutNumber/2;
+
+  tc.lane_index = lane_index;
+  for(unsigned int it=0; it< laneRollOuts.size(); it++)
+  {
+    tc.index = it;
+    tc.relative_index = it - centralIndex;
+    tc.distance_from_center = params.rollOutDensity*tc.relative_index;
+    tc.priority_cost = fabs(tc.distance_from_center);
+    tc.closest_obj_distance = params.horizonDistance;
+    tc.lane_change_cost = laneRollOuts.at(it).at(0).laneChangeCost;
+
+//    if(laneRollOuts.at(it).at(0).bDir == FORWARD_LEFT_DIR || laneRollOuts.at(it).at(0).bDir == FORWARD_RIGHT_DIR)
+//      tc.lane_change_cost = 1;
+//    else if(laneRollOuts.at(it).at(0).bDir == BACKWARD_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_RIGHT_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_LEFT_DIR)
+//      tc.lane_change_cost = 2;
+//    else
+//      tc.lane_change_cost = 0;
+
+    costs.push_back(tc);
+  }
+
+  return costs;
+}
+
+void TrajectoryCosts::CalculateTransitionCosts(vector<TrajectoryCost>& trajectoryCosts, const int& currTrajectoryIndex, const PlanningParams& params)
+{
+  for(int ic = 0; ic< trajectoryCosts.size(); ic++)
+  {
+    trajectoryCosts.at(ic).transition_cost = fabs(params.rollOutDensity * (ic - currTrajectoryIndex));
+  }
+}
+
+/**
+ * @brief Validate input, each trajectory must have at least 1 way point
+ * @param rollOuts
+ * @return true if data isvalid for cost calculation
+ */
+bool TrajectoryCosts::ValidateRollOutsInput(const vector<vector<vector<WayPoint> > >& rollOuts)
+{
+  if(rollOuts.size()==0)
+    return false;
+
+  for(unsigned int il = 0; il < rollOuts.size(); il++)
+  {
+    if(rollOuts.at(il).size() == 0)
+      return false;
+  }
+
+  return true;
+}
+
+}

src1/planning/op_planner/TrajectoryDynamicCosts.cpp

@@ -0,0 +1,917 @@
+
+/// \file TrajectoryDynamicCosts.cpp
+/// \brief Calculate collision costs for roll out trajectory for free trajectory evaluation for OpenPlanner local planner version 1.5+
+/// \author Hatem Darweesh
+/// \date Jan 14, 2018
+
+#include "op_planner/TrajectoryDynamicCosts.h"
+#include "op_planner/MatrixOperations.h"
+#include "float.h"
+
+namespace PlannerHNS
+{
+
+
+TrajectoryDynamicCosts::TrajectoryDynamicCosts()
+{
+  m_PrevCostIndex = -1;
+  //m_WeightPriority = 0.125;
+  //m_WeightTransition = 0.13;
+  m_WeightLong = 1.0;
+  m_WeightLat = 1.2;
+  m_WeightLaneChange = 0.0;
+  m_LateralSkipDistance = 50;
+
+
+  m_CollisionTimeDiff = 6.0; //seconds
+  m_PrevIndex = -1;
+  m_WeightPriority = 0.9;
+  m_WeightTransition = 0.9;
+}
+
+TrajectoryDynamicCosts::~TrajectoryDynamicCosts()
+{
+}
+
+TrajectoryCost TrajectoryDynamicCosts::DoOneStepDynamic(const vector<vector<WayPoint> >& rollOuts,
+    const vector<WayPoint>& totalPaths, const WayPoint& currState,
+    const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
+    const std::vector<PlannerHNS::DetectedObject>& obj_list, const int& iCurrentIndex)
+{
+  TrajectoryCost bestTrajectory;
+  bestTrajectory.bBlocked = true;
+  bestTrajectory.closest_obj_distance = params.horizonDistance;
+  bestTrajectory.closest_obj_velocity = 0;
+  bestTrajectory.index = -1;
+
+  double critical_lateral_distance   =  carInfo.width/2.0 + params.horizontalSafetyDistancel;
+  double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance;
+  double critical_long_back_distance   =  carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0;
+
+  int currIndex = -1;
+  if(iCurrentIndex >=0 && iCurrentIndex < rollOuts.size())
+    currIndex  = iCurrentIndex;
+  else
+    currIndex = GetCurrentRollOutIndex(totalPaths, currState, params);
+
+  InitializeCosts(rollOuts, params);
+
+  InitializeSafetyPolygon(currState, carInfo, vehicleState, critical_lateral_distance, critical_long_front_distance, critical_long_back_distance);
+
+  CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
+
+  CalculateLateralAndLongitudinalCostsDynamic(obj_list, rollOuts, totalPaths,  currState, params, carInfo, vehicleState, critical_lateral_distance, critical_long_front_distance, critical_long_back_distance);
+
+  NormalizeCosts(m_TrajectoryCosts);
+
+  int smallestIndex = -1;
+  double smallestCost = DBL_MAX;
+  double smallestDistance = DBL_MAX;
+  double velo_of_next = 0;
+  bool bAllFree = true;
+
+  //cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
+  for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
+  {
+    //cout << m_TrajectoryCosts.at(ic).ToString();
+    if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
+    {
+      smallestCost = m_TrajectoryCosts.at(ic).cost;
+      smallestIndex = ic;
+    }
+
+    if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
+    {
+      smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
+      velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+    }
+
+    if(m_TrajectoryCosts.at(ic).bBlocked)
+      bAllFree = false;
+  }
+  //cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;
+
+  if(bAllFree && smallestIndex >=0)
+    smallestIndex = params.rollOutNumber/2;
+
+
+  if(smallestIndex == -1)
+  {
+    bestTrajectory.bBlocked = true;
+    bestTrajectory.lane_index = 0;
+    bestTrajectory.index = m_PrevCostIndex;
+    bestTrajectory.closest_obj_distance = smallestDistance;
+    bestTrajectory.closest_obj_velocity = velo_of_next;
+  }
+  else if(smallestIndex >= 0)
+  {
+    bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
+  }
+
+  m_PrevIndex = currIndex;
+
+  //std::cout << "Current Selected Index : " << bestTrajectory.index << std::endl;
+  return bestTrajectory;
+}
+
+TrajectoryCost TrajectoryDynamicCosts::DoOneStepStatic(const vector<vector<WayPoint> >& rollOuts,
+    const vector<WayPoint>& totalPaths, const WayPoint& currState,
+    const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
+    const std::vector<PlannerHNS::DetectedObject>& obj_list, const int& iCurrentIndex)
+{
+  TrajectoryCost bestTrajectory;
+  bestTrajectory.bBlocked = true;
+  bestTrajectory.closest_obj_distance = params.horizonDistance;
+  bestTrajectory.closest_obj_velocity = 0;
+  bestTrajectory.index = -1;
+
+  RelativeInfo obj_info;
+  PlanningHelpers::GetRelativeInfo(totalPaths, currState, obj_info);
+  int currIndex = params.rollOutNumber/2 + floor(obj_info.perp_distance/params.rollOutDensity);
+  //std::cout <<  "Current Index: " << currIndex << std::endl;
+  if(currIndex < 0)
+    currIndex = 0;
+  else if(currIndex > params.rollOutNumber)
+    currIndex = params.rollOutNumber;
+
+  m_TrajectoryCosts.clear();
+  if(rollOuts.size()>0)
+  {
+    TrajectoryCost tc;
+    int centralIndex = params.rollOutNumber/2;
+    tc.lane_index = 0;
+    for(unsigned int it=0; it< rollOuts.size(); it++)
+    {
+      tc.index = it;
+      tc.relative_index = it - centralIndex;
+      tc.distance_from_center = params.rollOutDensity*tc.relative_index;
+      tc.priority_cost = fabs(tc.distance_from_center);
+      tc.closest_obj_distance = params.horizonDistance;
+      if(rollOuts.at(it).size() > 0)
+          tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;
+      m_TrajectoryCosts.push_back(tc);
+    }
+  }
+
+  CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
+
+  WayPoint p;
+  m_AllContourPoints.clear();
+  for(unsigned int io=0; io<obj_list.size(); io++)
+  {
+    for(unsigned int icon=0; icon < obj_list.at(io).contour.size(); icon++)
+    {
+      p.pos = obj_list.at(io).contour.at(icon);
+      p.v = obj_list.at(io).center.v;
+      p.id = io;
+      p.cost = sqrt(obj_list.at(io).w*obj_list.at(io).w + obj_list.at(io).l*obj_list.at(io).l);
+      m_AllContourPoints.push_back(p);
+    }
+  }
+
+  CalculateLateralAndLongitudinalCostsStatic(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);
+
+  NormalizeCosts(m_TrajectoryCosts);
+
+  int smallestIndex = -1;
+  double smallestCost = DBL_MAX;
+  double smallestDistance = DBL_MAX;
+  double velo_of_next = 0;
+
+  //cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
+  for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
+  {
+    //cout << m_TrajectoryCosts.at(ic).ToString();
+    if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
+    {
+      smallestCost = m_TrajectoryCosts.at(ic).cost;
+      smallestIndex = ic;
+    }
+
+    if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
+    {
+      smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
+      velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+    }
+  }
+  //cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;
+
+  if(smallestIndex == -1)
+  {
+    bestTrajectory.bBlocked = true;
+    bestTrajectory.lane_index = 0;
+    bestTrajectory.index = m_PrevCostIndex;
+    bestTrajectory.closest_obj_distance = smallestDistance;
+    bestTrajectory.closest_obj_velocity = velo_of_next;
+  }
+  else if(smallestIndex >= 0)
+  {
+    bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
+  }
+
+  m_PrevIndex = currIndex;
+  return bestTrajectory;
+}
+
+TrajectoryCost TrajectoryDynamicCosts::DoOneStep(const vector<vector<vector<WayPoint> > >& rollOuts,
+    const vector<vector<WayPoint> >& totalPaths, const WayPoint& currState, const int& currIndex,
+    const int& currLaneIndex,
+    const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
+    const std::vector<PlannerHNS::DetectedObject>& obj_list)
+{
+  TrajectoryCost bestTrajectory;
+  bestTrajectory.bBlocked = true;
+  bestTrajectory.closest_obj_distance = params.horizonDistance;
+  bestTrajectory.closest_obj_velocity = 0;
+  bestTrajectory.index = -1;
+
+  if(!ValidateRollOutsInput(rollOuts) || rollOuts.size() != totalPaths.size()) return bestTrajectory;
+
+  if(m_PrevCostIndex == -1)
+    m_PrevCostIndex = params.rollOutNumber/2;
+
+  m_TrajectoryCosts.clear();
+
+  for(unsigned int il = 0; il < rollOuts.size(); il++)
+  {
+    if(rollOuts.at(il).size()>0 && rollOuts.at(il).at(0).size()>0)
+    {
+      vector<TrajectoryCost> costs = CalculatePriorityAndLaneChangeCosts(rollOuts.at(il), il, params);
+      m_TrajectoryCosts.insert(m_TrajectoryCosts.end(), costs.begin(), costs.end());
+    }
+  }
+
+  CalculateTransitionCosts(m_TrajectoryCosts, currIndex, params);
+
+
+  WayPoint p;
+  m_AllContourPoints.clear();
+  for(unsigned int io=0; io<obj_list.size(); io++)
+  {
+    for(unsigned int icon=0; icon < obj_list.at(io).contour.size(); icon++)
+    {
+      p.pos = obj_list.at(io).contour.at(icon);
+      p.v = obj_list.at(io).center.v;
+      p.id = io;
+      p.cost = sqrt(obj_list.at(io).w*obj_list.at(io).w + obj_list.at(io).l*obj_list.at(io).l);
+      m_AllContourPoints.push_back(p);
+    }
+  }
+
+  CalculateLateralAndLongitudinalCosts(m_TrajectoryCosts, rollOuts, totalPaths, currState, m_AllContourPoints, params, carInfo, vehicleState);
+
+  NormalizeCosts(m_TrajectoryCosts);
+
+  int smallestIndex = -1;
+  double smallestCost = DBL_MAX;
+  double smallestDistance = DBL_MAX;
+  double velo_of_next = 0;
+
+  //cout << "Trajectory Costs Log : CurrIndex: " << currIndex << " --------------------- " << endl;
+  for(unsigned int ic = 0; ic < m_TrajectoryCosts.size(); ic++)
+  {
+    //cout << m_TrajectoryCosts.at(ic).ToString();
+    if(!m_TrajectoryCosts.at(ic).bBlocked && m_TrajectoryCosts.at(ic).cost < smallestCost)
+    {
+      smallestCost = m_TrajectoryCosts.at(ic).cost;
+      smallestIndex = ic;
+    }
+
+    if(m_TrajectoryCosts.at(ic).closest_obj_distance < smallestDistance)
+    {
+      smallestDistance = m_TrajectoryCosts.at(ic).closest_obj_distance;
+      velo_of_next = m_TrajectoryCosts.at(ic).closest_obj_velocity;
+    }
+  }
+
+  //cout << "Smallest Distance: " <<  smallestDistance << "------------------------------------------------------------- " << endl;
+
+  //All is blocked !
+  if(smallestIndex == -1 && m_PrevCostIndex < (int)m_TrajectoryCosts.size())
+  {
+    bestTrajectory.bBlocked = true;
+    bestTrajectory.lane_index = currLaneIndex;
+    bestTrajectory.index = currIndex;
+    bestTrajectory.closest_obj_distance = smallestDistance;
+    bestTrajectory.closest_obj_velocity = velo_of_next;
+    //bestTrajectory.index = smallestIndex;
+  }
+  else if(smallestIndex >= 0)
+  {
+    bestTrajectory = m_TrajectoryCosts.at(smallestIndex);
+    //bestTrajectory.index = smallestIndex;
+  }
+
+//  cout << "smallestI: " <<  smallestIndex << ", C_Size: " << m_TrajectoryCosts.size()
+//      << ", LaneI: " << bestTrajectory.lane_index << "TrajI: " << bestTrajectory.index
+//      << ", prevSmalI: " << m_PrevCostIndex << ", distance: " << bestTrajectory.closest_obj_distance
+//      << ", Blocked: " << bestTrajectory.bBlocked
+//      << endl;
+
+  m_PrevCostIndex = smallestIndex;
+
+  return bestTrajectory;
+}
+
+void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsStatic(vector<TrajectoryCost>& trajectoryCosts,
+    const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
+    const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params,
+    const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState)
+{
+  double critical_lateral_distance =  carInfo.width/2.0 + params.horizontalSafetyDistancel;
+  double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance;
+  double critical_long_back_distance =  carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0;
+
+  PlannerHNS::Mat3 invRotationMat(currState.pos.a-M_PI_2);
+  PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
+
+  double corner_slide_distance = critical_lateral_distance/2.0;
+  double ratio_to_angle = corner_slide_distance/carInfo.max_steer_angle;
+  double slide_distance = vehicleState.steer * ratio_to_angle;
+
+  GPSPoint bottom_left(-critical_lateral_distance ,-critical_long_back_distance,  currState.pos.z, 0);
+  GPSPoint bottom_right(critical_lateral_distance, -critical_long_back_distance,  currState.pos.z, 0);
+
+  GPSPoint top_right_car(critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0,  currState.pos.z, 0);
+  GPSPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0);
+
+  GPSPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance,  currState.pos.z, 0);
+  GPSPoint top_left(-critical_lateral_distance - slide_distance , critical_long_front_distance, currState.pos.z, 0);
+
+  bottom_left = invRotationMat*bottom_left;
+  bottom_left = invTranslationMat*bottom_left;
+
+  top_right = invRotationMat*top_right;
+  top_right = invTranslationMat*top_right;
+
+  bottom_right = invRotationMat*bottom_right;
+  bottom_right = invTranslationMat*bottom_right;
+
+  top_left = invRotationMat*top_left;
+  top_left = invTranslationMat*top_left;
+
+  top_right_car = invRotationMat*top_right_car;
+  top_right_car = invTranslationMat*top_right_car;
+
+  top_left_car = invRotationMat*top_left_car;
+  top_left_car = invTranslationMat*top_left_car;
+
+  m_SafetyBorder.points.clear();
+  m_SafetyBorder.points.push_back(bottom_left) ;
+  m_SafetyBorder.points.push_back(bottom_right) ;
+  m_SafetyBorder.points.push_back(top_right_car) ;
+  m_SafetyBorder.points.push_back(top_right) ;
+  m_SafetyBorder.points.push_back(top_left) ;
+  m_SafetyBorder.points.push_back(top_left_car) ;
+
+  int iCostIndex = 0;
+  if(rollOuts.size() > 0 && rollOuts.at(0).size()>0)
+  {
+    RelativeInfo car_info;
+    PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
+
+
+    for(unsigned int it=0; it< rollOuts.size(); it++)
+    {
+      int skip_id = -1;
+      for(unsigned int icon = 0; icon < contourPoints.size(); icon++)
+      {
+        if(skip_id == contourPoints.at(icon).id)
+          continue;
+
+        RelativeInfo obj_info;
+        PlanningHelpers::GetRelativeInfo(totalPaths, contourPoints.at(icon), obj_info);
+        double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, obj_info);
+        if(obj_info.iFront == 0 && longitudinalDist > 0)
+          longitudinalDist = -longitudinalDist;
+
+        double direct_distance = hypot(obj_info.perp_point.pos.y-contourPoints.at(icon).pos.y, obj_info.perp_point.pos.x-contourPoints.at(icon).pos.x);
+        if(contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance+contourPoints.at(icon).cost))
+        {
+          skip_id = contourPoints.at(icon).id;
+          continue;
+        }
+
+        double close_in_percentage = 1;
+//          close_in_percentage = ((longitudinalDist- critical_long_front_distance)/params.rollInMargin)*4.0;
+//
+//          if(close_in_percentage <= 0 || close_in_percentage > 1) close_in_percentage = 1;
+
+        double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center;
+
+        if(close_in_percentage < 1)
+          distance_from_center = distance_from_center - distance_from_center * (1.0-close_in_percentage);
+
+        double lateralDist = fabs(obj_info.perp_distance - distance_from_center);
+
+        if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance)
+        {
+          continue;
+        }
+
+        longitudinalDist = longitudinalDist - critical_long_front_distance;
+
+        if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true)
+          trajectoryCosts.at(iCostIndex).bBlocked = true;
+
+        if(lateralDist <= critical_lateral_distance
+            && longitudinalDist >= -carInfo.length/1.5
+            && longitudinalDist < params.minFollowingDistance)
+          trajectoryCosts.at(iCostIndex).bBlocked = true;
+
+
+        if(lateralDist != 0)
+          trajectoryCosts.at(iCostIndex).lateral_cost += 1.0/lateralDist;
+
+        if(longitudinalDist != 0)
+          trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0/fabs(longitudinalDist);
+
+
+        if(longitudinalDist >= -critical_long_front_distance && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance)
+        {
+          trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
+          trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
+        }
+      }
+
+      iCostIndex++;
+    }
+  }
+}
+
+void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost>& trajectoryCosts,
+    const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths,
+    const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params,
+    const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState)
+{
+  double critical_lateral_distance =  carInfo.width/2.0 + params.horizontalSafetyDistancel;
+  double critical_long_front_distance =  carInfo.wheel_base/2.0 + carInfo.length/2.0 + params.verticalSafetyDistance;
+  double critical_long_back_distance =  carInfo.length/2.0 + params.verticalSafetyDistance - carInfo.wheel_base/2.0;
+  int iCostIndex = 0;
+
+  PlannerHNS::Mat3 invRotationMat(currState.pos.a-M_PI_2);
+  PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
+
+  double corner_slide_distance = critical_lateral_distance/2.0;
+  double ratio_to_angle = corner_slide_distance/carInfo.max_steer_angle;
+  double slide_distance = vehicleState.steer * ratio_to_angle;
+
+  GPSPoint bottom_left(-critical_lateral_distance ,-critical_long_back_distance,  currState.pos.z, 0);
+  GPSPoint bottom_right(critical_lateral_distance, -critical_long_back_distance,  currState.pos.z, 0);
+
+  GPSPoint top_right_car(critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0,  currState.pos.z, 0);
+  GPSPoint top_left_car(-critical_lateral_distance, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0);
+
+  GPSPoint top_right(critical_lateral_distance - slide_distance, critical_long_front_distance,  currState.pos.z, 0);
+  GPSPoint top_left(-critical_lateral_distance - slide_distance , critical_long_front_distance, currState.pos.z, 0);
+
+  bottom_left = invRotationMat*bottom_left;
+  bottom_left = invTranslationMat*bottom_left;
+
+  top_right = invRotationMat*top_right;
+  top_right = invTranslationMat*top_right;
+
+  bottom_right = invRotationMat*bottom_right;
+  bottom_right = invTranslationMat*bottom_right;
+
+  top_left = invRotationMat*top_left;
+  top_left = invTranslationMat*top_left;
+
+  top_right_car = invRotationMat*top_right_car;
+  top_right_car = invTranslationMat*top_right_car;
+
+  top_left_car = invRotationMat*top_left_car;
+  top_left_car = invTranslationMat*top_left_car;
+
+  m_SafetyBorder.points.clear();
+  m_SafetyBorder.points.push_back(bottom_left) ;
+  m_SafetyBorder.points.push_back(bottom_right) ;
+  m_SafetyBorder.points.push_back(top_right_car) ;
+  m_SafetyBorder.points.push_back(top_right) ;
+  m_SafetyBorder.points.push_back(top_left) ;
+  m_SafetyBorder.points.push_back(top_left_car) ;
+
+  for(unsigned int il=0; il < rollOuts.size(); il++)
+  {
+    if(rollOuts.at(il).size() > 0 && rollOuts.at(il).at(0).size()>0)
+    {
+      RelativeInfo car_info;
+      PlanningHelpers::GetRelativeInfo(totalPaths.at(il), currState, car_info);
+
+
+      for(unsigned int it=0; it< rollOuts.at(il).size(); it++)
+      {
+        int skip_id = -1;
+        for(unsigned int icon = 0; icon < contourPoints.size(); icon++)
+        {
+          if(skip_id == contourPoints.at(icon).id)
+            continue;
+
+          RelativeInfo obj_info;
+          PlanningHelpers::GetRelativeInfo(totalPaths.at(il), contourPoints.at(icon), obj_info);
+          double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths.at(il), car_info, obj_info);
+          if(obj_info.iFront == 0 && longitudinalDist > 0)
+            longitudinalDist = -longitudinalDist;
+
+          double direct_distance = hypot(obj_info.perp_point.pos.y-contourPoints.at(icon).pos.y, obj_info.perp_point.pos.x-contourPoints.at(icon).pos.x);
+          if(contourPoints.at(icon).v < params.minSpeed && direct_distance > (m_LateralSkipDistance+contourPoints.at(icon).cost))
+          {
+            skip_id = contourPoints.at(icon).id;
+            continue;
+          }
+
+          double close_in_percentage = 1;
+//          close_in_percentage = ((longitudinalDist- critical_long_front_distance)/params.rollInMargin)*4.0;
+//
+//          if(close_in_percentage <= 0 || close_in_percentage > 1) close_in_percentage = 1;
+
+          double distance_from_center = trajectoryCosts.at(iCostIndex).distance_from_center;
+
+          if(close_in_percentage < 1)
+            distance_from_center = distance_from_center - distance_from_center * (1.0-close_in_percentage);
+
+          double lateralDist = fabs(obj_info.perp_distance - distance_from_center);
+
+          if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || lateralDist > m_LateralSkipDistance)
+          {
+            continue;
+          }
+
+          longitudinalDist = longitudinalDist - critical_long_front_distance;
+
+          if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, contourPoints.at(icon).pos) == true)
+            trajectoryCosts.at(iCostIndex).bBlocked = true;
+
+          if(lateralDist <= critical_lateral_distance
+              && longitudinalDist >= -carInfo.length/1.5
+              && longitudinalDist < params.minFollowingDistance)
+            trajectoryCosts.at(iCostIndex).bBlocked = true;
+
+
+          if(lateralDist != 0)
+            trajectoryCosts.at(iCostIndex).lateral_cost += 1.0/lateralDist;
+
+          if(longitudinalDist != 0)
+            trajectoryCosts.at(iCostIndex).longitudinal_cost += 1.0/fabs(longitudinalDist);
+
+
+          if(longitudinalDist >= -critical_long_front_distance && longitudinalDist < trajectoryCosts.at(iCostIndex).closest_obj_distance)
+          {
+            trajectoryCosts.at(iCostIndex).closest_obj_distance = longitudinalDist;
+            trajectoryCosts.at(iCostIndex).closest_obj_velocity = contourPoints.at(icon).v;
+          }
+        }
+
+        iCostIndex++;
+      }
+    }
+  }
+}
+
+void TrajectoryDynamicCosts::NormalizeCosts(vector<TrajectoryCost>& trajectoryCosts)
+{
+  //Normalize costs
+  double totalPriorities = 0;
+  double totalChange = 0;
+  double totalLateralCosts = 0;
+  double totalLongitudinalCosts = 0;
+  double transitionCosts = 0;
+
+  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
+  {
+    totalPriorities += trajectoryCosts.at(ic).priority_cost;
+    transitionCosts += trajectoryCosts.at(ic).transition_cost;
+  }
+
+  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
+  {
+    totalChange += trajectoryCosts.at(ic).lane_change_cost;
+    totalLateralCosts += trajectoryCosts.at(ic).lateral_cost;
+    totalLongitudinalCosts += trajectoryCosts.at(ic).longitudinal_cost;
+  }
+
+//  cout << "------ Normalizing Step " << endl;
+  for(unsigned int ic = 0; ic< trajectoryCosts.size(); ic++)
+  {
+    if(totalPriorities != 0)
+      trajectoryCosts.at(ic).priority_cost = trajectoryCosts.at(ic).priority_cost / totalPriorities;
+    else
+      trajectoryCosts.at(ic).priority_cost = 0;
+
+    if(transitionCosts != 0)
+      trajectoryCosts.at(ic).transition_cost = trajectoryCosts.at(ic).transition_cost / transitionCosts;
+    else
+      trajectoryCosts.at(ic).transition_cost = 0;
+
+    if(totalChange != 0)
+      trajectoryCosts.at(ic).lane_change_cost = trajectoryCosts.at(ic).lane_change_cost / totalChange;
+    else
+      trajectoryCosts.at(ic).lane_change_cost = 0;
+
+    if(totalLateralCosts != 0)
+      trajectoryCosts.at(ic).lateral_cost = trajectoryCosts.at(ic).lateral_cost / totalLateralCosts;
+    else
+      trajectoryCosts.at(ic).lateral_cost = 0;
+
+    if(totalLongitudinalCosts != 0)
+      trajectoryCosts.at(ic).longitudinal_cost = trajectoryCosts.at(ic).longitudinal_cost / totalLongitudinalCosts;
+    else
+      trajectoryCosts.at(ic).longitudinal_cost = 0;
+
+    trajectoryCosts.at(ic).cost = (m_WeightPriority*trajectoryCosts.at(ic).priority_cost + m_WeightTransition*trajectoryCosts.at(ic).transition_cost + m_WeightLat*trajectoryCosts.at(ic).lateral_cost + m_WeightLong*trajectoryCosts.at(ic).longitudinal_cost)/4.0;
+
+//    cout << "Index: " << ic
+//            << ", Priority: " << trajectoryCosts.at(ic).priority_cost
+//            << ", Transition: " << trajectoryCosts.at(ic).transition_cost
+//            << ", Lat: " << trajectoryCosts.at(ic).lateral_cost
+//            << ", Long: " << trajectoryCosts.at(ic).longitudinal_cost
+//            << ", Change: " << trajectoryCosts.at(ic).lane_change_cost
+//            << ", Avg: " << trajectoryCosts.at(ic).cost
+//            << endl;
+  }
+
+//  cout << "------------------------ " << endl;
+}
+
+vector<TrajectoryCost> TrajectoryDynamicCosts::CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint> >& laneRollOuts,
+    const int& lane_index, const PlanningParams& params)
+{
+  vector<TrajectoryCost> costs;
+  TrajectoryCost tc;
+  int centralIndex = params.rollOutNumber/2;
+
+  tc.lane_index = lane_index;
+  for(unsigned int it=0; it< laneRollOuts.size(); it++)
+  {
+    tc.index = it;
+    tc.relative_index = it - centralIndex;
+    tc.distance_from_center = params.rollOutDensity*tc.relative_index;
+    tc.priority_cost = fabs(tc.distance_from_center);
+    tc.closest_obj_distance = params.horizonDistance;
+    tc.lane_change_cost = laneRollOuts.at(it).at(0).laneChangeCost;
+
+//    if(laneRollOuts.at(it).at(0).bDir == FORWARD_LEFT_DIR || laneRollOuts.at(it).at(0).bDir == FORWARD_RIGHT_DIR)
+//      tc.lane_change_cost = 1;
+//    else if(laneRollOuts.at(it).at(0).bDir == BACKWARD_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_RIGHT_DIR || laneRollOuts.at(it).at(0).bDir == BACKWARD_LEFT_DIR)
+//      tc.lane_change_cost = 2;
+//    else
+//      tc.lane_change_cost = 0;
+
+    costs.push_back(tc);
+  }
+
+  return costs;
+}
+
+void TrajectoryDynamicCosts::CalculateTransitionCosts(vector<TrajectoryCost>& trajectoryCosts, const int& currTrajectoryIndex, const PlanningParams& params)
+{
+  for(int ic = 0; ic< trajectoryCosts.size(); ic++)
+  {
+    trajectoryCosts.at(ic).transition_cost = fabs(params.rollOutDensity * (ic - currTrajectoryIndex));
+  }
+}
+
+/**
+ * @brief Validate input, each trajectory must have at least 1 way point
+ * @param rollOuts
+ * @return true if data isvalid for cost calculation
+ */
+bool TrajectoryDynamicCosts::ValidateRollOutsInput(const vector<vector<vector<WayPoint> > >& rollOuts)
+{
+  if(rollOuts.size()==0)
+    return false;
+
+  for(unsigned int il = 0; il < rollOuts.size(); il++)
+  {
+    if(rollOuts.at(il).size() == 0)
+      return false;
+  }
+
+  return true;
+}
+
+void TrajectoryDynamicCosts::CalculateIntersectionVelocities(const std::vector<PlannerHNS::WayPoint>& path, const PlannerHNS::DetectedObject& obj, const WayPoint& currPose, const CAR_BASIC_INFO& carInfo, const double& c_lateral_d, WayPoint& collisionPoint, TrajectoryCost& trajectoryCosts)
+{
+  trajectoryCosts.bBlocked = false;
+  int closest_path_i = path.size();
+  for(unsigned int k = 0; k < obj.predTrajectories.size(); k++)
+  {
+    for(unsigned int j = 0; j < obj.predTrajectories.at(k).size(); j++)
+    {
+      for(unsigned int i = 0; i < path.size(); i++)
+      {
+        //if(path.at(i).timeCost > -1)
+        {
+          double collision_distance = hypot(path.at(i).pos.x-obj.predTrajectories.at(k).at(j).pos.x, path.at(i).pos.y-obj.predTrajectories.at(k).at(j).pos.y);
+          double collision_t = fabs(path.at(i).timeCost - obj.predTrajectories.at(k).at(j).timeCost);
+
+          //if(collision_distance <= c_lateral_d && i < closest_path_i && collision_t < m_CollisionTimeDiff)
+          if(collision_distance <= c_lateral_d && i < closest_path_i)
+          {
+
+            closest_path_i = i;
+            double a = UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(path.at(i).pos.a, obj.predTrajectories.at(k).at(j).pos.a)/M_PI;
+            if(a < 0.25 && (currPose.v - obj.center.v) > 0)
+              trajectoryCosts.closest_obj_velocity = (currPose.v - obj.center.v);
+            else
+              trajectoryCosts.closest_obj_velocity = currPose.v;
+
+            collisionPoint = path.at(i);
+            collisionPoint.collisionCost = collision_t;
+            collisionPoint.cost = collision_distance;
+            trajectoryCosts.bBlocked = true;
+          }
+        }
+      }
+    }
+  }
+}
+
+int TrajectoryDynamicCosts::GetCurrentRollOutIndex(const std::vector<WayPoint>& path, const WayPoint& currState, const PlanningParams& params)
+{
+  RelativeInfo obj_info;
+  PlanningHelpers::GetRelativeInfo(path, currState, obj_info);
+  int currIndex = params.rollOutNumber/2 + floor(obj_info.perp_distance/params.rollOutDensity);
+  if(currIndex < 0)
+    currIndex = 0;
+  else if(currIndex > params.rollOutNumber)
+    currIndex = params.rollOutNumber;
+
+  return currIndex;
+}
+
+void TrajectoryDynamicCosts::InitializeCosts(const vector<vector<WayPoint> >& rollOuts, const PlanningParams& params)
+{
+  m_TrajectoryCosts.clear();
+  if(rollOuts.size()>0)
+  {
+    TrajectoryCost tc;
+    int centralIndex = params.rollOutNumber/2;
+    tc.lane_index = 0;
+    for(unsigned int it=0; it< rollOuts.size(); it++)
+    {
+      tc.index = it;
+      tc.relative_index = it - centralIndex;
+      tc.distance_from_center = params.rollOutDensity*tc.relative_index;
+      tc.priority_cost = fabs(tc.distance_from_center);
+      tc.closest_obj_distance = params.horizonDistance;
+      if(rollOuts.at(it).size() > 0)
+        tc.lane_change_cost = rollOuts.at(it).at(0).laneChangeCost;
+      m_TrajectoryCosts.push_back(tc);
+    }
+  }
+}
+
+void TrajectoryDynamicCosts::InitializeSafetyPolygon(const WayPoint& currState, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState, const double& c_lateral_d, const double& c_long_front_d, const double& c_long_back_d)
+{
+  PlannerHNS::Mat3 invRotationMat(currState.pos.a-M_PI_2);
+  PlannerHNS::Mat3 invTranslationMat(currState.pos.x, currState.pos.y);
+
+  double corner_slide_distance = c_lateral_d/2.0;
+  double ratio_to_angle = corner_slide_distance/carInfo.max_steer_angle;
+  double slide_distance = vehicleState.steer * ratio_to_angle;
+
+  GPSPoint bottom_left(-c_lateral_d ,-c_long_back_d,  currState.pos.z, 0);
+  GPSPoint bottom_right(c_lateral_d, -c_long_back_d,  currState.pos.z, 0);
+
+  GPSPoint top_right_car(c_lateral_d, carInfo.wheel_base/3.0 + carInfo.length/3.0,  currState.pos.z, 0);
+  GPSPoint top_left_car(-c_lateral_d, carInfo.wheel_base/3.0 + carInfo.length/3.0, currState.pos.z, 0);
+
+  GPSPoint top_right(c_lateral_d - slide_distance, c_long_front_d,  currState.pos.z, 0);
+  GPSPoint top_left(-c_lateral_d - slide_distance , c_long_front_d, currState.pos.z, 0);
+
+  bottom_left = invRotationMat*bottom_left;
+  bottom_left = invTranslationMat*bottom_left;
+
+  top_right = invRotationMat*top_right;
+  top_right = invTranslationMat*top_right;
+
+  bottom_right = invRotationMat*bottom_right;
+  bottom_right = invTranslationMat*bottom_right;
+
+  top_left = invRotationMat*top_left;
+  top_left = invTranslationMat*top_left;
+
+  top_right_car = invRotationMat*top_right_car;
+  top_right_car = invTranslationMat*top_right_car;
+
+  top_left_car = invRotationMat*top_left_car;
+  top_left_car = invTranslationMat*top_left_car;
+
+  m_SafetyBorder.points.clear();
+  m_SafetyBorder.points.push_back(bottom_left) ;
+  m_SafetyBorder.points.push_back(bottom_right) ;
+  m_SafetyBorder.points.push_back(top_right_car) ;
+  m_SafetyBorder.points.push_back(top_right) ;
+  m_SafetyBorder.points.push_back(top_left) ;
+  m_SafetyBorder.points.push_back(top_left_car) ;
+}
+
+void TrajectoryDynamicCosts::CalculateLateralAndLongitudinalCostsDynamic(const std::vector<PlannerHNS::DetectedObject>& obj_list, const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
+    const WayPoint& currState, const PlanningParams& params, const CAR_BASIC_INFO& carInfo,
+    const VehicleState& vehicleState, const double& c_lateral_d, const double& c_long_front_d, const double& c_long_back_d )
+{
+
+  RelativeInfo car_info;
+  PlanningHelpers::GetRelativeInfo(totalPaths, currState, car_info);
+  m_CollisionPoints.clear();
+
+  for(unsigned int i=0; i < obj_list.size(); i++)
+  {
+    if(obj_list.at(i).label.compare("curb") == 0)
+    {
+      double d = hypot(obj_list.at(i).center.pos.y - currState.pos.y ,  obj_list.at(i).center.pos.x - currState.pos.x);
+      if(d > params.minFollowingDistance + c_lateral_d)
+        continue;
+    }
+
+    if(obj_list.at(i).bVelocity && obj_list.at(i).predTrajectories.size() > 0) // dynamic
+    {
+
+      for(unsigned int ir=0; ir < rollOuts.size(); ir++)
+      {
+        WayPoint collisionPoint;
+        TrajectoryCost trajectoryCosts;
+        CalculateIntersectionVelocities(rollOuts.at(ir), obj_list.at(i), currState, carInfo, c_lateral_d, collisionPoint,trajectoryCosts);
+        if(trajectoryCosts.bBlocked)
+        {
+          RelativeInfo col_info;
+          PlanningHelpers::GetRelativeInfo(totalPaths, collisionPoint, col_info);
+          double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, col_info);
+
+          if(col_info.iFront == 0 && longitudinalDist > 0)
+            longitudinalDist = -longitudinalDist;
+
+          if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance || fabs(longitudinalDist) < carInfo.width/2.0)
+            continue;
+
+          //std::cout << "LongDistance: " << longitudinalDist << std::endl;
+
+          if(longitudinalDist >= -c_long_front_d && longitudinalDist < m_TrajectoryCosts.at(ir).closest_obj_distance)
+            m_TrajectoryCosts.at(ir).closest_obj_distance = longitudinalDist;
+
+          m_TrajectoryCosts.at(ir).closest_obj_velocity = trajectoryCosts.closest_obj_velocity;
+          m_TrajectoryCosts.at(ir).bBlocked = true;
+
+          m_CollisionPoints.push_back(collisionPoint);
+        }
+      }
+    }
+    else
+    {
+      RelativeInfo obj_info;
+      WayPoint corner_p;
+      for(unsigned int icon = 0; icon < obj_list.at(i).contour.size(); icon++)
+      {
+        if(m_SafetyBorder.PointInsidePolygon(m_SafetyBorder, obj_list.at(i).contour.at(icon)) == true)
+        {
+          for(unsigned int it=0; it< rollOuts.size(); it++)
+            m_TrajectoryCosts.at(it).bBlocked = true;
+
+          return;
+        }
+
+        corner_p.pos = obj_list.at(i).contour.at(icon);
+        PlanningHelpers::GetRelativeInfo(totalPaths, corner_p, obj_info);
+        double longitudinalDist = PlanningHelpers::GetExactDistanceOnTrajectory(totalPaths, car_info, obj_info);
+        if(obj_info.iFront == 0 && longitudinalDist > 0)
+          longitudinalDist = -longitudinalDist;
+
+
+        if(longitudinalDist < -carInfo.length || longitudinalDist > params.minFollowingDistance)
+          continue;
+
+        longitudinalDist = longitudinalDist - c_long_front_d;
+
+        for(unsigned int it=0; it< rollOuts.size(); it++)
+        {
+          double lateralDist = fabs(obj_info.perp_distance - m_TrajectoryCosts.at(it).distance_from_center);
+
+          if(lateralDist > m_LateralSkipDistance)
+            continue;
+
+          if(lateralDist <= c_lateral_d && longitudinalDist > -carInfo.length && longitudinalDist < params.minFollowingDistance)
+          {
+            m_TrajectoryCosts.at(it).bBlocked = true;
+            m_CollisionPoints.push_back(obj_info.perp_point);
+          }
+
+          if(lateralDist != 0)
+            m_TrajectoryCosts.at(it).lateral_cost += 1.0/lateralDist;
+
+          if(longitudinalDist != 0)
+            m_TrajectoryCosts.at(it).longitudinal_cost += 1.0/fabs(longitudinalDist);
+
+          if(longitudinalDist >= -c_long_front_d && longitudinalDist < m_TrajectoryCosts.at(it).closest_obj_distance)
+          {
+            m_TrajectoryCosts.at(it).closest_obj_distance = longitudinalDist;
+            m_TrajectoryCosts.at(it).closest_obj_velocity = obj_list.at(i).center.v;
+          }
+        }
+      }
+
+    }
+  }
+}
+
+}

src1/planning/op_planner/include/op_planner/BehaviorPrediction.h

@@ -0,0 +1,882 @@
+
+/// \file  BehaviorPrediction.h
+/// \brief Predict detected vehicles's possible trajectories, these trajectories extracted from the vector map.
+/// \author Hatem Darweesh
+/// \date Jul 6, 2017
+
+
+#ifndef BEHAVIORPREDICTION_H_
+#define BEHAVIORPREDICTION_H_
+
+#include <boost/random.hpp>
+#include <boost/math/distributions/normal.hpp>
+
+#include "PlannerH.h"
+#include "op_utility/UtilityH.h"
+#include "PassiveDecisionMaker.h"
+
+namespace PlannerHNS
+{
+
+#define MOTION_POSE_ERROR 0.2 // 50 cm pose error
+#define MOTION_ANGLE_ERROR 0.01 // 0.05 rad angle error
+#define MOTION_VEL_ERROR 0.2
+#define MEASURE_POSE_ERROR 0.1
+#define MEASURE_ANGLE_ERROR 0.01
+#define MEASURE_VEL_ERROR 0.1
+#define MEASURE_IND_ERROR 0.1
+
+#define PREDICTION_DISTANCE_PERCENTAGE 0.25
+
+#define BEH_PARTICLES_NUM 25
+#define BEH_MIN_PARTICLE_NUM 0
+#define KEEP_PERCENTAGE 0.85
+
+#define MAX_PREDICTION_SPEED 10.0
+
+#define POSE_FACTOR 0.1
+#define DIRECTION_FACTOR 0.1
+#define VELOCITY_FACTOR 0.2
+#define ACCELERATE_FACTOR 0.1
+#define INDICATOR_FACTOR 0.5
+
+#define FIXED_PLANNING_DISTANCE 10
+
+#define MIN_PREDICTION_TIME 5
+#define USE_OPEN_PLANNER_MOVE 0
+
+
+#define ACCELERATION_CALC_TIME 0.25
+#define ACCELERATION_DECISION_VALUE 0.5
+
+#define ENABLE_STOP_BEHAVIOR_GEN 1
+
+typedef boost::mt19937 ENG;
+typedef boost::normal_distribution<double> NormalDIST;
+typedef boost::variate_generator<ENG, NormalDIST> VariatGEN;
+
+class TrajectoryTracker;
+
+class Particle
+{
+public:
+  bool bDeleted;
+  BEH_STATE_TYPE beh; //[Stop, Yielding, Forward, Branching]
+  double vel; //[0 -> Stop,1 -> moving]
+  double vel_prev_big;
+  double prev_time_diff;
+  int acc; //[-1 ->Slowing, 0, Stopping, 1 -> accelerating]
+  double acc_raw;
+  int indicator; //[0 -> No, 1 -> Left, 2 -> Right , 3 -> both]
+  WayPoint pose;
+  bool bStopLine;
+  double w;
+  double w_raw;
+  double pose_w;
+  double dir_w;
+  double vel_w;
+  double acl_w;
+  double ind_w;
+  TrajectoryTracker* pTraj;
+  int original_index;
+
+  Particle()
+  {
+    prev_time_diff = 0;
+    vel_prev_big = 0;
+    original_index = 0;
+    bStopLine = false;
+    bDeleted = false;
+    pTraj = nullptr;
+    w = 0;
+    w_raw = 0;
+    pose_w = 0;
+    dir_w = 0;
+    vel_w = 0;
+    acl_w = 0;
+    ind_w = 0;
+    beh = BEH_STOPPING_STATE;
+    vel = 0;
+    acc = 0;
+    acc_raw = 0;
+    indicator = 0;
+  }
+};
+
+class TrajectoryTracker
+{
+public:
+  unsigned int index;
+  BEH_STATE_TYPE beh;
+  BEH_STATE_TYPE best_beh;
+  double best_p;
+  std::vector<int> ids;
+  std::vector<int> path_ids;
+  WayPoint path_last_pose;
+  double rms_error;
+  std::vector<WayPoint> trajectory;
+
+  std::vector<Particle> m_ForwardPart;
+  std::vector<Particle> m_StopPart;
+  std::vector<Particle> m_YieldPart;
+  std::vector<Particle> m_LeftPart;
+  std::vector<Particle> m_RightPart;
+  BehaviorState m_CurrBehavior;
+
+  int nAliveStop;
+  int nAliveYield;
+  int nAliveForward;
+  int nAliveLeft;
+  int nAliveRight;
+
+  double pStop;
+  double pYield;
+  double pForward;
+  double pLeft;
+  double pRight;
+
+  double w_avg_forward;
+  double w_avg_stop;
+  double w_avg_yield;
+  double w_avg_left;
+  double w_avg_right;
+
+  PassiveDecisionMaker m_SinglePathDecisionMaker;
+
+  TrajectoryTracker()
+  {
+    beh = BEH_STOPPING_STATE;
+    rms_error = 0;
+    index = 0;
+    nAliveStop = 0;
+    nAliveYield = 0;
+    nAliveForward = 0;
+    nAliveLeft = 0;
+    nAliveRight = 0;
+
+    pStop  = 0;
+    pYield = 0;
+    pForward = 0;
+    pLeft = 0;
+    pRight = 0;
+    best_beh = PlannerHNS::BEH_STOPPING_STATE;
+    best_p = 0;
+
+    w_avg_forward = 0;
+    w_avg_stop = 0;
+    w_avg_yield = 0;
+    w_avg_left = 0;
+    w_avg_right = 0;
+  }
+
+  virtual ~TrajectoryTracker()
+  {
+  }
+
+  void InitDecision()
+  {
+  }
+
+  TrajectoryTracker(const TrajectoryTracker& obj)
+  {
+
+    rms_error = 0;
+    ids = obj.ids;
+    path_ids = obj.path_ids;
+    path_last_pose = obj.path_last_pose;
+    beh = obj.beh;
+    index = obj.index;
+    trajectory = obj.trajectory;
+    nAliveStop = obj.nAliveStop;
+    nAliveYield = obj.nAliveYield;
+    nAliveForward = obj.nAliveForward;
+    nAliveLeft = obj.nAliveLeft;
+    nAliveRight = obj.nAliveRight;
+
+    pStop  = obj.pStop;
+    pYield = obj.pYield;
+    pForward = obj.pForward;
+    pLeft = obj.pLeft;
+    pRight = obj.pRight;
+    best_beh = obj.best_beh;
+    best_p = obj.best_p;
+    m_SinglePathDecisionMaker = obj.m_SinglePathDecisionMaker;
+
+    m_ForwardPart = obj.m_ForwardPart;
+    m_StopPart = obj.m_StopPart;
+    m_YieldPart = obj.m_YieldPart;
+    m_LeftPart = obj.m_LeftPart;
+    m_RightPart = obj.m_RightPart;
+    m_CurrBehavior = obj.m_CurrBehavior;
+
+    w_avg_forward = obj.w_avg_forward;
+    w_avg_stop = obj.w_avg_stop;
+    w_avg_yield = obj.w_avg_yield;
+    w_avg_left = obj.w_avg_left;
+    w_avg_right = obj.w_avg_right;
+  }
+
+  TrajectoryTracker(std::vector<PlannerHNS::WayPoint>& path, const unsigned int& _index)
+  {
+
+    if(path.size()>0)
+    {
+      beh = path.at(0).beh_state;
+      //std::cout << "New Path: Beh: " << beh << ", index: " << _index << ", LaneID_0: " << path.at(0).laneId << ", LaneID_1: "<< path.at(1).laneId << std::endl;
+    }
+
+    index = _index;
+    trajectory = path;
+    int prev_id = -10;
+    int curr_id = -10;
+    ids.clear();
+    path_ids.clear();
+    for(unsigned int i = 0; i < path.size(); i++)
+    {
+      curr_id = path.at(i).laneId;
+      path_ids.push_back(curr_id);
+
+      if(curr_id != prev_id)
+      {
+        ids.push_back(curr_id);
+        prev_id = curr_id;
+      }
+    }
+
+    path_last_pose = path.at(path.size()-1);
+
+    nAliveStop = 0;
+    nAliveYield = 0;
+    nAliveForward = 0;
+    nAliveLeft = 0;
+    nAliveRight = 0;
+
+    pStop  = 0;
+    pYield = 0;
+    pForward = 0;
+    pLeft = 0;
+    pRight = 0;
+    best_beh = PlannerHNS::BEH_STOPPING_STATE;
+    best_p = 0;
+
+    InitDecision();
+  }
+
+  void UpdatePathAndIndex(std::vector<PlannerHNS::WayPoint>& _path, const unsigned int& _index)
+  {
+    if(_path.size() == 0) return;
+
+    beh = _path.at(0).beh_state;
+    index = _index;
+    trajectory = _path;
+    int prev_id = -10;
+    int curr_id = -10;
+    ids.clear();
+    path_ids.clear();
+    for(unsigned int i = 0; i < _path.size(); i++)
+    {
+      curr_id = _path.at(i).laneId;
+      path_ids.push_back(curr_id);
+      if(curr_id != prev_id)
+      {
+        ids.push_back(curr_id);
+        prev_id = curr_id;
+      }
+    }
+
+    path_last_pose = _path.at(_path.size()-1);
+  }
+
+  double CalcMatchingPercentage(const std::vector<PlannerHNS::WayPoint>& _path)
+  {
+    if(_path.size() == 0) return 0;
+
+    if(beh != _path.at(0).beh_state) return 0;
+
+    int nCount = 0, nIds = 0;
+    int prev_id = -10;
+    int curr_id = -10;
+    std::vector<int> _ids;
+
+    for(unsigned int i = 0; i < _path.size(); i++)
+    {
+      curr_id = _path.at(i).laneId;
+      if(i < path_ids.size())
+      {
+        nCount++;
+        if(curr_id == path_ids.at(i))
+          nIds++;
+      }
+
+      if(curr_id != prev_id)
+      {
+        _ids.push_back(curr_id);
+        prev_id = curr_id;
+      }
+    }
+
+    int nEqualities = 0;
+    for(unsigned int i=0; i < _ids.size(); i++)
+    {
+      for(unsigned int j=0; j < ids.size(); j++)
+      {
+        if(_ids.at(i) == ids.at(j))
+        {
+          nEqualities++;
+          break;
+        }
+      }
+    }
+
+    double rms_val = 0;
+        for(unsigned int i=0; i < _path.size(); i++)
+          {
+      if(i < trajectory.size())
+        {
+          rms_val += hypot(_path.at(i).pos.y - trajectory.at(i).pos.y, _path.at(i).pos.y - trajectory.at(i).pos.y);
+        }
+          }
+
+        rms_error = rms_val;
+
+    if(rms_error < 5.0)
+      return 1;
+
+    if(_ids.size() == ids.size() && ids.size() == nEqualities && rms_error < 5.0) // perfect match
+      return 1;
+
+    WayPoint curr_last_pose = _path.at(_path.size()-1);
+    double nMatch = (double)nIds/(double)nCount;
+    double _d = hypot(path_last_pose.pos.y-curr_last_pose.pos.y, path_last_pose.pos.x-curr_last_pose.pos.x);
+
+    double dCost = _d/FIXED_PLANNING_DISTANCE;
+    if(dCost > 1.0)
+      dCost = 1.0;
+    double dMatch = 1.0 - dCost;
+
+    double _a_diff = UtilityHNS::UtilityH::AngleBetweenTwoAnglesPositive(path_last_pose.pos.a, curr_last_pose.pos.a);
+    double aCost = _a_diff/M_PI;
+    if(aCost > 1.0)
+      aCost = 1.0;
+    double aMatch = 1.0 - aCost;
+
+    double totalMatch = (nMatch + dMatch + aMatch)/3.0;
+
+    return totalMatch;
+  }
+
+  void InsertNewParticle(const Particle& p)
+  {
+    if(p.beh == PlannerHNS::BEH_STOPPING_STATE && nAliveStop < BEH_PARTICLES_NUM)
+    {
+      m_StopPart.push_back(p);
+      m_StopPart.at(m_StopPart.size()-1).pTraj = this;
+      nAliveStop++;
+    }
+    else if(p.beh == PlannerHNS::BEH_YIELDING_STATE && nAliveYield < BEH_PARTICLES_NUM)
+    {
+      m_YieldPart.push_back(p);
+      m_YieldPart.at(m_YieldPart.size()-1).pTraj = this;
+      nAliveYield++;
+    }
+    else if(p.beh == PlannerHNS::BEH_FORWARD_STATE && nAliveForward < BEH_PARTICLES_NUM)
+    {
+      m_ForwardPart.push_back(p);
+      m_ForwardPart.at(m_ForwardPart.size()-1).pTraj = this;
+      nAliveForward++;
+    }
+    else if(p.beh == PlannerHNS::BEH_BRANCH_LEFT_STATE && nAliveLeft < BEH_PARTICLES_NUM)
+    {
+      m_LeftPart.push_back(p);
+      m_LeftPart.at(m_LeftPart.size()-1).pTraj = this;
+      nAliveLeft++;
+    }
+    else if(p.beh == PlannerHNS::BEH_BRANCH_RIGHT_STATE && nAliveRight < BEH_PARTICLES_NUM)
+    {
+      m_RightPart.push_back(p);
+      m_RightPart.at(m_RightPart.size()-1).pTraj = this;
+      nAliveRight++;
+    }
+  }
+
+  void DeleteParticle(const Particle& p, const int& _i)
+  {
+    if(p.beh == PlannerHNS::BEH_STOPPING_STATE && nAliveStop > BEH_MIN_PARTICLE_NUM)
+    {
+      m_StopPart.erase(m_StopPart.begin()+_i);
+      nAliveStop--;
+    }
+    else if(p.beh == PlannerHNS::BEH_YIELDING_STATE && nAliveYield > BEH_MIN_PARTICLE_NUM)
+    {
+      m_YieldPart.erase(m_YieldPart.begin()+_i);
+      nAliveYield--;
+    }
+    else if(p.beh == PlannerHNS::BEH_FORWARD_STATE && nAliveForward > BEH_MIN_PARTICLE_NUM)
+    {
+      m_ForwardPart.erase(m_ForwardPart.begin()+_i);
+      nAliveForward--;
+    }
+    else if(p.beh == PlannerHNS::BEH_BRANCH_LEFT_STATE && nAliveLeft > BEH_MIN_PARTICLE_NUM)
+    {
+      m_LeftPart.erase(m_LeftPart.begin()+_i);
+      nAliveLeft--;
+    }
+    else if(p.beh == PlannerHNS::BEH_BRANCH_RIGHT_STATE && nAliveRight > BEH_MIN_PARTICLE_NUM)
+    {
+      m_RightPart.erase(m_RightPart.begin()+_i);
+      nAliveRight--;
+    }
+  }
+
+  void CalcAverages()
+  {
+    w_avg_forward = 0;
+    double avg_sum = 0;
+    for(unsigned int i = 0; i < m_ForwardPart.size(); i++)
+    {
+      avg_sum += m_ForwardPart.at(i).w;
+    }
+    if(m_ForwardPart.size() > 0)
+      w_avg_forward = avg_sum/(double)m_ForwardPart.size();
+
+    w_avg_left = 0;
+    avg_sum = 0;
+    for(unsigned int i = 0; i < m_LeftPart.size(); i++)
+    {
+      avg_sum += m_LeftPart.at(i).w;
+    }
+    if(m_LeftPart.size() > 0)
+      w_avg_left = avg_sum/(double)m_LeftPart.size();
+
+    w_avg_right = 0;
+    avg_sum = 0;
+    for(unsigned int i = 0; i < m_RightPart.size(); i++)
+    {
+      avg_sum += m_RightPart.at(i).w;
+    }
+    if(m_RightPart.size() > 0)
+      w_avg_right = avg_sum/(double)m_RightPart.size();
+
+    w_avg_stop = 0;
+    avg_sum = 0;
+    for(unsigned int i = 0; i < m_StopPart.size(); i++)
+    {
+      avg_sum += m_StopPart.at(i).w;
+    }
+    if(m_StopPart.size() > 0)
+      w_avg_stop = avg_sum/(double)m_StopPart.size();
+
+    w_avg_yield = 0;
+    avg_sum = 0;
+    for(unsigned int i = 0; i < m_YieldPart.size(); i++)
+    {
+      avg_sum += m_YieldPart.at(i).w;
+    }
+    if(m_YieldPart.size() > 0)
+      w_avg_yield = avg_sum/(double)m_YieldPart.size();
+  }
+
+  void CalcProbabilities()
+  {
+    best_beh = PlannerHNS::BEH_STOPPING_STATE;
+    pStop  = (double)nAliveStop/(double)BEH_PARTICLES_NUM;
+    best_p = pStop;
+
+    pYield = (double)nAliveYield/(double)BEH_PARTICLES_NUM;
+    if(pYield > best_p)
+    {
+      best_p = pYield;
+      best_beh = PlannerHNS::BEH_YIELDING_STATE;
+    }
+
+    pForward = (double)nAliveForward/(double)BEH_PARTICLES_NUM;
+    if(pForward > best_p)
+    {
+      best_p = pForward;
+      best_beh = PlannerHNS::BEH_FORWARD_STATE;
+    }
+
+    pLeft = (double)nAliveLeft/(double)BEH_PARTICLES_NUM;
+    if(pLeft > best_p)
+    {
+      best_p = pLeft;
+      best_beh = PlannerHNS::BEH_BRANCH_LEFT_STATE;
+    }
+
+    pRight = (double)nAliveRight/(double)BEH_PARTICLES_NUM;
+    if(pRight > best_p)
+    {
+      best_p = pRight;
+      best_beh = PlannerHNS::BEH_BRANCH_RIGHT_STATE;
+    }
+  }
+};
+
+class ObjParticles
+{
+public:
+  DetectedObject obj;
+  std::vector<TrajectoryTracker*> m_TrajectoryTracker;
+  std::vector<TrajectoryTracker*> m_TrajectoryTracker_temp;
+
+  std::vector<Particle*> m_AllParticles;
+
+  TrajectoryTracker* best_beh_track;
+  int i_best_track;
+
+  PlannerHNS::BehaviorState m_beh;
+  double m_PredictionTime;
+
+  double all_w;
+  double max_w;
+  double min_w;
+  double max_w_raw;
+  double min_w_raw;
+  double avg_w;
+  double pose_w_t;
+  double dir_w_t;
+  double vel_w_t;
+  double acl_w_t;
+  double ind_w_t;
+
+  double pose_w_max;
+  double dir_w_max;
+  double vel_w_max;
+  double acl_w_max;
+  double ind_w_max;
+
+  double pose_w_min;
+  double dir_w_min;
+  double vel_w_min;
+  double acl_w_min;
+  double ind_w_min;
+
+  int n_stop;
+  int n_yield;
+  int n_left_branch;
+  int n_right_branch;
+
+  double p_stop;
+  double p_yield;
+  double p_left_branch;
+  double p_right_branch;
+
+  virtual ~ObjParticles()
+  {
+    DeleteTheRest(m_TrajectoryTracker);
+    m_TrajectoryTracker_temp.clear();
+  }
+
+  ObjParticles()
+  {
+    m_PredictionTime = 0;
+    best_beh_track = nullptr;
+    i_best_track = -1;
+    all_w = 0;
+    pose_w_t = 0;
+    dir_w_t = 0;
+    vel_w_t = 0;
+    acl_w_t = 0;
+    ind_w_t = 0;
+    max_w = DBL_MIN;
+    min_w = DBL_MAX;
+    max_w_raw = DBL_MIN;
+    min_w_raw = DBL_MAX;
+    avg_w = 0;
+
+    pose_w_max = -999999;
+    dir_w_max=-999999;
+    vel_w_max=-999999;
+    acl_w_max=-999999;
+    ind_w_max=-999999;
+
+    pose_w_min=999999;
+    dir_w_min=999999;
+    vel_w_min=999999;
+    acl_w_min=999999;
+    ind_w_min=999999;
+
+    n_stop = 0;
+    n_yield = 0;
+    n_left_branch = 0;
+    n_right_branch = 0;
+
+    p_stop = 0;
+    p_yield = 0;
+    p_left_branch = 0;
+    p_right_branch = 0;
+  }
+
+//  void CalculateProbabilities()
+//  {
+//    for(unsigned int i = 0; i < m_TrajectoryTracker.size(); i++)
+//    {
+//      m_TrajectoryTracker.at(i)->CalcProbabilities();
+//    }
+//
+//    if(m_TrajectoryTracker.size() > 0)
+//    {
+//      best_beh_track = m_TrajectoryTracker.at(0);
+//      i_best_track = 0;
+//    }
+//
+//    for(unsigned int i = 1; i < m_TrajectoryTracker.size(); i++)
+//    {
+//      if(m_TrajectoryTracker.at(i)->best_p > best_beh_track->best_p)
+//      {
+//        best_beh_track = m_TrajectoryTracker.at(i);
+//        i_best_track = i;
+//      }
+//    }
+//  }
+
+  class LLP
+  {
+  public:
+    int new_index;
+    double match_percent;
+    TrajectoryTracker* pTrack;
+
+    LLP()
+    {
+      new_index = -1;
+      match_percent = -1;
+      pTrack = 0;
+
+    }
+  };
+
+  void DeleteFromList(std::vector<TrajectoryTracker*>& delete_me_track, const TrajectoryTracker* track)
+  {
+    for(unsigned int k = 0; k < delete_me_track.size(); k++)
+    {
+      if(delete_me_track.at(k) == track)
+      {
+        delete_me_track.erase(delete_me_track.begin()+k);
+        return;
+      }
+    }
+  }
+
+  void DeleteTheRest(std::vector<TrajectoryTracker*>& delete_me_track)
+  {
+    for(unsigned int k = 0; k < delete_me_track.size(); k++)
+    {
+      delete delete_me_track.at(k);
+    }
+
+    delete_me_track.clear();
+  }
+
+  static bool IsBeggerPercentage(const LLP& p1, const LLP& p2)
+  {
+    return p1.match_percent > p2.match_percent;
+  }
+
+  void MatchWithMax(std::vector<LLP>& matching_list, std::vector<TrajectoryTracker*>& delete_me_track, std::vector<TrajectoryTracker*>& check_list)
+  {
+    if(matching_list.size() == 0 ) return;
+
+    std::sort(matching_list.begin(), matching_list.end(), IsBeggerPercentage);
+
+    while(matching_list.size()>0)
+    {
+      LLP f = matching_list.at(0);
+      f.pTrack->UpdatePathAndIndex(obj.predTrajectories.at(f.new_index), f.new_index);
+
+      bool bFound = false;
+      for(unsigned int k=0; k < check_list.size(); k++)
+      {
+        if(check_list.at(k) == f.pTrack)
+        {
+          bFound = true;
+          break;
+        }
+      }
+
+      if(!bFound)
+        check_list.push_back(f.pTrack);
+
+      DeleteFromList(delete_me_track, f.pTrack);
+      for(int i=0; i < matching_list.size(); i++)
+      {
+        if(matching_list.at(i).new_index == f.new_index || matching_list.at(i).pTrack == f.pTrack)
+        {
+          matching_list.erase(matching_list.begin()+i);
+          i--;
+        }
+      }
+    }
+  }
+
+  void MatchTrajectories()
+  {
+    m_TrajectoryTracker_temp.clear();
+    std::vector<LLP> matching_list;
+    std::vector<TrajectoryTracker*> delete_me_track = m_TrajectoryTracker;
+    for(unsigned int t = 0; t < obj.predTrajectories.size();t++)
+    {
+      bool bMatched = false;
+      LLP match_item;
+      match_item.new_index = t;
+
+      for(int i = 0; i < m_TrajectoryTracker.size(); i++)
+      {
+          TrajectoryTracker* pTracker = m_TrajectoryTracker.at(i);
+
+        double vMatch = pTracker->CalcMatchingPercentage(obj.predTrajectories.at(t));
+        if(vMatch == 1.0) // perfect match
+        {
+            pTracker->UpdatePathAndIndex(obj.predTrajectories.at(t), t);
+            bool bFound = false;
+            for(unsigned int k=0; k < m_TrajectoryTracker_temp.size(); k++)
+            {
+              if(m_TrajectoryTracker_temp.at(k) == pTracker)
+              {
+                bFound = true;
+                break;
+              }
+            }
+
+          if(!bFound)
+            m_TrajectoryTracker_temp.push_back(pTracker);
+
+          DeleteFromList(delete_me_track, pTracker);
+
+          for(unsigned int k=0; k < matching_list.size(); k++)
+          {
+            if(matching_list.at(k).pTrack == pTracker)
+            {
+              matching_list.erase(matching_list.begin()+k);
+              break;
+            }
+          }
+
+          m_TrajectoryTracker.erase(m_TrajectoryTracker.begin()+i);
+          bMatched = true;
+          i--;
+          break;
+        }
+        else if(vMatch > 0.5) // any matching less than 50%, the trajectory will be considered new
+        {
+          bMatched = true;
+          match_item.match_percent = vMatch;
+          match_item.pTrack = pTracker;
+          matching_list.push_back(match_item);
+        }
+      }
+
+      if(!bMatched)
+      {
+        m_TrajectoryTracker_temp.push_back(new TrajectoryTracker(obj.predTrajectories.at(t), t));
+      }
+    }
+
+    MatchWithMax(matching_list,delete_me_track, m_TrajectoryTracker_temp);
+    m_TrajectoryTracker.clear();
+    DeleteTheRest(delete_me_track);
+    m_TrajectoryTracker = m_TrajectoryTracker_temp;
+  }
+};
+
+class BehaviorPrediction
+{
+public:
+  BehaviorPrediction();
+  virtual ~BehaviorPrediction();
+  void DoOneStep(const std::vector<DetectedObject>& obj_list, const WayPoint& currPose, const double& minSpeed, const double& maxDeceleration, RoadNetwork& map);
+
+public:
+  std::vector<PassiveDecisionMaker*> m_d_makers;
+  double m_MaxLaneDetectionDistance;
+  double m_PredictionDistance;
+  bool m_bGenerateBranches;
+  bool m_bUseFixedPrediction;
+  bool m_bStepByStep;
+  bool m_bParticleFilter;
+  //std::vector<DetectedObject> m_PredictedObjects;
+  //std::vector<DetectedObject*> m_PredictedObjectsII;
+
+  std::vector<ObjParticles> m_temp_list;
+  std::vector<ObjParticles> m_ParticleInfo;
+
+  std::vector<ObjParticles*> m_temp_list_ii;
+  std::vector<ObjParticles*> m_ParticleInfo_II;
+
+  struct timespec m_GenerationTimer;
+  timespec m_ResamplingTimer;
+
+  bool m_bCanDecide;
+  bool m_bFirstMove;
+  bool m_bDebugOut;
+
+
+protected:
+  //int GetTrajectoryPredictedDirection(const std::vector<WayPoint>& path, const PlannerHNS::WayPoint& pose, const double& pred_distance);
+  int FromIndicatorToNumber(const PlannerHNS::LIGHT_INDICATOR& ind);
+  PlannerHNS::LIGHT_INDICATOR FromNumbertoIndicator(const int& num);
+  double CalcIndicatorWeight(PlannerHNS::LIGHT_INDICATOR p_ind, PlannerHNS::LIGHT_INDICATOR obj_ind);
+  double CalcAccelerationWeight(int p_acl, int obj_acl);
+
+  void CalPredictionTimeForObject(ObjParticles* pCarPart);
+  void PredictCurrentTrajectory(RoadNetwork& map, ObjParticles* pCarPart);
+  void FilterObservations(const std::vector<DetectedObject>& obj_list, RoadNetwork& map, std::vector<DetectedObject>& filtered_list);
+  void ExtractTrajectoriesFromMap(const std::vector<DetectedObject>& obj_list, RoadNetwork& map, std::vector<ObjParticles*>& old_list);
+  void CalculateCollisionTimes(const double& minSpeed);
+
+  void ParticleFilterSteps(std::vector<ObjParticles*>& part_info);
+
+  void SamplesFreshParticles(ObjParticles* pParts);
+  void MoveParticles(ObjParticles* parts);
+  void CalculateWeights(ObjParticles* pParts);
+
+  void CalOnePartWeight(ObjParticles* pParts,Particle& p);
+  void NormalizeOnePartWeight(ObjParticles* pParts,Particle& p);
+
+  void CollectParticles(ObjParticles* pParts);
+
+  void RemoveWeakParticles(ObjParticles* pParts);
+  void FindBest(ObjParticles* pParts);
+  void CalculateAveragesAndProbabilities(ObjParticles* pParts);
+
+  static bool sort_weights(const Particle* p1, const Particle* p2)
+  {
+    return p1->w > p2->w;
+  }
+
+  static bool sort_trajectories(const std::pair<int, double>& p1, const std::pair<int, double>& p2)
+  {
+    return p1.second > p2.second;
+  }
+
+
+public:
+  //move to CPP later
+  void DeleteFromList(std::vector<ObjParticles*>& delete_me, const ObjParticles* pElement)
+  {
+    for(unsigned int k = 0; k < delete_me.size(); k++)
+    {
+      if(delete_me.at(k) == pElement)
+      {
+        delete_me.erase(delete_me.begin()+k);
+        return;
+      }
+    }
+  }
+
+  void DeleteTheRest(std::vector<ObjParticles*>& delete_me)
+  {
+    for(unsigned int k = 0; k < delete_me.size(); k++)
+    {
+      delete delete_me.at(k);
+    }
+
+    delete_me.clear();
+  }
+};
+
+
+
+} /* namespace PlannerHNS */
+
+#endif /* BEHAVIORPREDICTION_H_ */

src1/planning/op_planner/include/op_planner/BehaviorStateMachine.h

@@ -0,0 +1,277 @@
+
+/// \file BehaviorStateMachine.h
+/// \author Hatem Darweesh
+/// \brief OpenPlanner's state machine implementation for different driving behaviors
+/// \date Jun 19, 2016
+
+
+#ifndef BEHAVIORSTATEMACHINE_H_
+#define BEHAVIORSTATEMACHINE_H_
+
+#include "RoadNetwork.h"
+#include <sstream>
+
+namespace PlannerHNS
+{
+class BehaviorStateMachine
+{
+public:
+  virtual BehaviorStateMachine* GetNextState() = 0;
+  virtual void Init();
+  virtual void ResetTimer();
+  virtual void InsertNextState(BehaviorStateMachine* nextState);
+  BehaviorStateMachine(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* nextState);
+  virtual ~BehaviorStateMachine() ;
+
+  STATE_TYPE m_Behavior;
+  int m_currentStopSignID  ;
+  int m_currentTrafficLightID ;
+  double decisionMakingTime;
+  int decisionMakingCount;
+  double m_zero_velocity;
+
+  PreCalculatedConditions* GetCalcParams()
+  {
+    if(!m_pCalculatedValues)
+        m_pCalculatedValues = new PreCalculatedConditions();
+
+    return m_pCalculatedValues;
+  }
+
+  void SetBehaviorsParams(PlanningParams* pParams)
+  {
+    if(!pParams)
+      m_pParams = new PlanningParams;
+    else
+      m_pParams = pParams;
+  }
+
+
+  PreCalculatedConditions* m_pCalculatedValues;
+  PlanningParams* m_pParams;
+  timespec m_StateTimer;
+  std::vector<BehaviorStateMachine*> pNextStates;
+
+  BehaviorStateMachine* FindBehaviorState(const STATE_TYPE& behavior);
+  void UpdateLogCount(BehaviorStateMachine* pState);
+  BehaviorStateMachine* FindBestState(int nMinCount);
+
+private:
+  std::vector<std::pair<BehaviorStateMachine*, int> > m_BehaviorsLog;
+};
+
+class ForwardState : public BehaviorStateMachine
+{
+public:
+  ForwardState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = FORWARD_STATE;}
+  virtual ~ForwardState(){}
+  virtual BehaviorStateMachine* GetNextState();
+};
+
+class ForwardStateII : public BehaviorStateMachine
+{
+public:
+  ForwardStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = FORWARD_STATE;}
+  virtual ~ForwardStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+};
+
+class MissionAccomplishedState : public BehaviorStateMachine
+{
+public:
+  MissionAccomplishedState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = FINISH_STATE;}
+  virtual ~MissionAccomplishedState(){}
+  virtual BehaviorStateMachine* GetNextState();
+};
+
+class MissionAccomplishedStateII : public BehaviorStateMachine
+{
+public:
+  MissionAccomplishedStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = FINISH_STATE;}
+  virtual ~MissionAccomplishedStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+};
+
+class FollowState : public BehaviorStateMachine
+{
+public:
+  FollowState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = FOLLOW_STATE;}
+  virtual ~FollowState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class FollowStateII : public BehaviorStateMachine
+{
+public:
+  FollowStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = FOLLOW_STATE;}
+  virtual ~FollowStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class SwerveState : public BehaviorStateMachine
+{
+public:
+  SwerveState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = OBSTACLE_AVOIDANCE_STATE;}
+  virtual ~SwerveState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class SwerveStateII : public BehaviorStateMachine
+{
+public:
+  SwerveStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = OBSTACLE_AVOIDANCE_STATE;}
+  virtual ~SwerveStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class StopState : public BehaviorStateMachine
+{
+public:
+  StopState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = STOPPING_STATE;}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class TrafficLightStopState : public BehaviorStateMachine
+{
+public:
+  TrafficLightStopState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = TRAFFIC_LIGHT_STOP_STATE;}
+  virtual ~TrafficLightStopState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class TrafficLightWaitState : public BehaviorStateMachine
+{
+public:
+  TrafficLightWaitState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = TRAFFIC_LIGHT_WAIT_STATE;}
+  virtual ~TrafficLightWaitState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class StopSignStopState : public BehaviorStateMachine
+{
+public:
+  StopSignStopState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = STOP_SIGN_STOP_STATE;}
+  virtual ~StopSignStopState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class StopSignStopStateII : public BehaviorStateMachine
+{
+public:
+  StopSignStopStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = STOP_SIGN_STOP_STATE;}
+  virtual ~StopSignStopStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class StopSignWaitState : public BehaviorStateMachine
+{
+public:
+  StopSignWaitState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = STOP_SIGN_WAIT_STATE;}
+  virtual ~StopSignWaitState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class StopSignWaitStateII : public BehaviorStateMachine
+{
+public:
+  StopSignWaitStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = STOP_SIGN_WAIT_STATE;}
+  virtual ~StopSignWaitStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class WaitState : public BehaviorStateMachine
+{
+public:
+  WaitState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = WAITING_STATE;}
+  virtual ~WaitState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class InitState : public BehaviorStateMachine
+{
+public:
+  InitState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = INITIAL_STATE;}
+  virtual ~InitState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class InitStateII : public BehaviorStateMachine
+{
+public:
+  InitStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = INITIAL_STATE;}
+  virtual ~InitStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+};
+
+class GoalState : public BehaviorStateMachine
+{
+public:
+  GoalState(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = GOAL_STATE;}
+  virtual ~GoalState(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class GoalStateII : public BehaviorStateMachine
+{
+public:
+  GoalStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = GOAL_STATE;}
+  virtual ~GoalStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+
+};
+
+class TrafficLightStopStateII : public BehaviorStateMachine
+{
+public:
+  TrafficLightStopStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = TRAFFIC_LIGHT_STOP_STATE;}
+  virtual ~TrafficLightStopStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+};
+
+class TrafficLightWaitStateII : public BehaviorStateMachine
+{
+public:
+  TrafficLightWaitStateII(PlanningParams* pParams, PreCalculatedConditions* pPreCalcVal, BehaviorStateMachine* pNextState)
+  : BehaviorStateMachine(pParams, pPreCalcVal, pNextState){m_Behavior = TRAFFIC_LIGHT_WAIT_STATE;}
+  virtual ~TrafficLightWaitStateII(){}
+  virtual BehaviorStateMachine* GetNextState();
+};
+
+} /* namespace PlannerHNS */
+
+#endif /* BEHAVIORSTATEMACHINE_H_ */

src1/planning/op_planner/include/op_planner/DecisionMaker.h

@@ -0,0 +1,92 @@
+
+/// \file DecisionMaker.h
+/// \brief Initialize behaviors state machine, and calculate required parameters for the state machine transition conditions
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+
+#ifndef BEHAVIOR_DECISION_MAKER
+#define BEHAVIOR_DECISION_MAKER
+
+#include "op_planner/BehaviorStateMachine.h"
+#include "op_planner/PlannerCommonDef.h"
+#include "op_planner/RoadNetwork.h"
+
+namespace PlannerHNS
+{
+
+class DecisionMaker
+{
+public:
+  WayPoint state;
+  CAR_BASIC_INFO m_CarInfo;
+  ControllerParams m_ControlParams;
+  std::vector<WayPoint> m_Path;
+  PlannerHNS::RoadNetwork m_Map;
+
+  double m_MaxLaneSearchDistance;
+  int m_iCurrentTotalPathId;
+  std::vector<std::vector<WayPoint> > m_RollOuts;
+  Lane* pLane;
+
+  BehaviorStateMachine*     m_pCurrentBehaviorState;
+  StopState*           m_pStopState;
+  WaitState*           m_pWaitState;
+  SwerveStateII*        m_pAvoidObstacleState;
+  TrafficLightStopStateII*  m_pTrafficLightStopState;
+  TrafficLightWaitStateII*  m_pTrafficLightWaitState;
+
+  ForwardStateII *       m_pGoToGoalState;;
+  InitStateII*         m_pInitState;
+  MissionAccomplishedStateII*  m_pMissionCompleteState;
+  GoalStateII*        m_pGoalState;
+  FollowStateII*        m_pFollowState;
+  StopSignStopStateII*       m_pStopSignStopState;
+  StopSignWaitStateII*       m_pStopSignWaitState;
+
+  void InitBehaviorStates();
+
+  //For Simulation
+  UtilityHNS::PIDController   m_pidVelocity;
+  UtilityHNS::PIDController   m_pidStopping;
+  UtilityHNS::PIDController   m_pidFollowing;
+
+public:
+
+  DecisionMaker();
+  virtual ~DecisionMaker();
+  void Init(const ControllerParams& ctrlParams, const PlanningParams& params, const CAR_BASIC_INFO& carInfo);
+  void CalculateImportantParameterForDecisionMaking(const VehicleState& car_state,
+      const int& goalID, const bool& bEmergencyStop, const std::vector<TrafficLight>& detectedLights,
+      const TrajectoryCost& bestTrajectory);
+  void SetNewGlobalPath(const std::vector<std::vector<WayPoint> >& globalPath);
+
+  BehaviorState DoOneStep(
+      const double& dt,
+      const PlannerHNS::WayPoint currPose,
+      const PlannerHNS::VehicleState& vehicleState,
+      const int& goalID,
+      const std::vector<TrafficLight>& trafficLight,
+      const TrajectoryCost& tc,
+      const bool& bEmergencyStop);
+
+protected:
+  bool GetNextTrafficLight(const int& prevTrafficLightId, const std::vector<TrafficLight>& trafficLights, TrafficLight& trafficL);
+  void UpdateCurrentLane(const double& search_distance);
+  bool SelectSafeTrajectory();
+  BehaviorState GenerateBehaviorState(const VehicleState& vehicleState);
+  double UpdateVelocityDirectlyToTrajectory(const BehaviorState& beh, const VehicleState& CurrStatus, const double& dt);
+  bool ReachEndOfGlobalPath(const double& min_distance, const int& iGlobalPathIndex);
+
+
+
+  std::vector<PlannerHNS::WayPoint> t_centerTrajectorySmoothed;
+  std::vector<std::vector<WayPoint> > m_TotalOriginalPath;
+  std::vector<std::vector<WayPoint> > m_TotalPath;
+  PlannerHNS::PlanningParams m_params;
+
+};
+
+} /* namespace PlannerHNS */
+
+#endif /* BEHAVIOR_DECISION_MAKER */

src1/planning/op_planner/include/op_planner/LocalPlannerH.h

@@ -0,0 +1,168 @@
+
+/// \file LocalPlannerH.h
+/// \brief OpenPlanner's local planing functions combines in one process, used in simulation vehicle and OpenPlanner old implementation like dp_planner node.
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+
+#ifndef LOCALPLANNERH_H_
+#define LOCALPLANNERH_H_
+
+#include "BehaviorStateMachine.h"
+#include "PlannerCommonDef.h"
+#include "RoadNetwork.h"
+#include "TrajectoryCosts.h"
+
+#define AVOIDANCE_SPEED_FACTOR 0.75
+namespace PlannerHNS
+{
+
+class LocalPlannerH
+{
+public:
+  WayPoint state;
+  CAR_BASIC_INFO m_CarInfo;
+  ControllerParams m_ControlParams;
+  std::vector<GPSPoint> m_CarShapePolygon;
+  std::vector<WayPoint> m_Path;
+  std::vector<WayPoint> m_OriginalLocalPath;
+  std::vector<std::vector<WayPoint> > m_TotalPath;
+  std::vector<std::vector<WayPoint> > m_TotalOriginalPath;
+  std::vector<DetectedObject> m_PredictedTrajectoryObstacles;
+  int m_iCurrentTotalPathId;
+  int m_iSafeTrajectory;
+  double m_InitialFollowingDistance;
+//  int m_iGlobalPathPrevID;
+//  std::vector<std::vector<WayPoint> > m_PredictedPath;
+  std::vector<std::vector<std::vector<WayPoint> > > m_RollOuts;
+  std::string carId;
+  Lane* pLane;
+  double m_SimulationSteeringDelayFactor; //second , time that every degree change in the steering wheel takes
+  timespec m_SteerDelayTimer;
+  double m_PredictionTime;
+  double m_CostCalculationTime;
+  double m_BehaviorGenTime;
+  double m_RollOutsGenerationTime;
+  int m_PrevBrakingWayPoint;
+
+  BehaviorStateMachine*     m_pCurrentBehaviorState;
+  ForwardState *         m_pGoToGoalState;
+  StopState*           m_pStopState;
+  WaitState*           m_pWaitState;
+  InitState*           m_pInitState;
+  MissionAccomplishedState*  m_pMissionCompleteState;
+  GoalState*          m_pGoalState;
+  FollowState*        m_pFollowState;
+  SwerveState*        m_pAvoidObstacleState;
+  TrafficLightStopState*    m_pTrafficLightStopState;
+  TrafficLightWaitState*    m_pTrafficLightWaitState;
+  StopSignStopState*       m_pStopSignStopState;
+  StopSignWaitState*       m_pStopSignWaitState;
+
+  TrajectoryCosts m_TrajectoryCostsCalculatotor;
+
+  //for debugging
+
+  std::vector<WayPoint> m_SampledPoints;
+
+  void InitBehaviorStates();
+
+  void ReInitializePlanner(const WayPoint& start_pose);
+
+  void SetSimulatedTargetOdometryReadings(const double& velocity_d, const double& steering_d, const SHIFT_POS& shift_d)
+  {
+    m_CurrentVelocityD = velocity_d;
+    m_CurrentSteeringD = steering_d;
+    m_CurrentShiftD = shift_d;
+  }
+
+  double GetSimulatedVelocity()
+  {
+    return m_CurrentVelocity;
+  }
+
+  double GetSimulatedSteering()
+  {
+    return m_CurrentSteering;
+  }
+
+  double GetSimulatedShift()
+  {
+    return m_CurrentShift;
+  }
+
+
+  //For Simulation
+  WayPoint m_OdometryState;
+  double m_CurrentVelocity, m_CurrentVelocityD; //meter/second
+  double m_CurrentSteering, m_CurrentSteeringD; //radians
+  SHIFT_POS m_CurrentShift , m_CurrentShiftD;
+
+  double m_CurrentAccSteerAngle; //degrees steer wheel range
+  double m_CurrentAccVelocity; // kilometer/hour
+  //std::vector<TrafficLight> m_TrafficLights;
+
+  UtilityHNS::PIDController   m_pidVelocity;
+  UtilityHNS::PIDController   m_pidStopping;
+
+public:
+
+  LocalPlannerH();
+  virtual ~LocalPlannerH();
+  void Init(const ControllerParams& ctrlParams, const PlanningParams& params, const CAR_BASIC_INFO& carInfo);
+  void InitPolygons();
+  void FirstLocalizeMe(const WayPoint& initCarPos);
+  void LocalizeMe(const double& dt); // in seconds
+  void UpdateState(const VehicleState& state, const bool& bUseDelay = false);
+  void CalculateImportantParameterForDecisionMaking(const VehicleState& car_state,
+      const int& goalID, const bool& bEmergencyStop, const vector<TrafficLight>& detectedLights,
+      const TrajectoryCost& bestTrajectory);
+
+  BehaviorState DoOneStep(
+      const double& dt,
+      const VehicleState& state,
+      const std::vector<DetectedObject>& obj_list,
+      const int& goalID,
+      RoadNetwork& map,
+      const bool& bEmergencyStop,
+      const std::vector<TrafficLight>& trafficLight,
+      const bool& bLive = false);
+
+  void SimulateOdoPosition(const double& dt, const VehicleState& vehicleState);
+
+private:
+
+  //Obstacle avoidance functionalities
+//  bool CalculateObstacleCosts(RoadNetwork& map, const VehicleState& vstatus, const std::vector<DetectedObject>& obj_list);
+//
+//  double PredictTimeCostForTrajectory(std::vector<WayPoint>& path,
+//      const VehicleState& vstatus,
+//      const WayPoint& currState);
+//
+//  void PredictObstacleTrajectory(RoadNetwork& map,
+//      const WayPoint& pos,
+//      const double& predTime,
+//      std::vector<std::vector<WayPoint> >& paths);
+//
+//  bool CalculateIntersectionVelocities(std::vector<WayPoint>& path,
+//      std::vector<std::vector<WayPoint> >& predctedPath,
+//      const DetectedObject& obj);
+
+  bool GetNextTrafficLight(const int& prevTrafficLightId, const std::vector<TrafficLight>& trafficLights, TrafficLight& trafficL);
+  void UpdateCurrentLane(RoadNetwork& map, const double& search_distance);
+  bool SelectSafeTrajectoryAndSpeedProfile(const VehicleState& vehicleState);
+  BehaviorState GenerateBehaviorState(const VehicleState& vehicleState);
+  void TransformPoint(const WayPoint& refPose, GPSPoint& p);
+  void AddAndTransformContourPoints(const DetectedObject& obj, std::vector<WayPoint>& contourPoints);
+  double UpdateVelocityDirectlyToTrajectory(const BehaviorState& beh, const VehicleState& CurrStatus, const double& dt);
+
+  void ExtractHorizonAndCalculateRecommendedSpeed();
+
+  bool NoWayTest(const double& min_distance, const int& iGlobalPathIndex);
+
+  PlannerHNS::PlanningParams m_params;
+};
+
+} /* namespace PlannerHNS */
+
+#endif /* LOCALPLANNERH_H_ */

src1/planning/op_planner/include/op_planner/MappingHelpers.h

@@ -0,0 +1,227 @@
+
+/// \file MappingHelpers.h
+/// \brief Helper functions for mapping operation such as (load and initialize vector maps , convert map from one format to another, .. )
+/// \author Hatem Darweesh
+/// \date Jul 2, 2016
+
+
+
+#ifndef MAPPINGHELPERS_H_
+#define MAPPINGHELPERS_H_
+
+#include <math.h>
+#include "RoadNetwork.h"
+#include "op_utility/UtilityH.h"
+#include "op_utility/DataRW.h"
+#include "tinyxml.h"
+
+
+namespace PlannerHNS {
+
+
+class MappingHelpers {
+public:
+  MappingHelpers();
+  virtual ~MappingHelpers();
+
+  static void ConstructRoadNetworkFromROSMessage(const std::vector<UtilityHNS::AisanLanesFileReader::AisanLane>& lanes_data,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+      const std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine>& dt_data,
+      const std::vector<UtilityHNS::AisanIntersectionFileReader::AisanIntersection>& intersection_data,
+      const std::vector<UtilityHNS::AisanAreasFileReader::AisanArea>& area_data,
+      const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+      const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
+      const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
+      const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
+      const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
+      const std::vector<UtilityHNS::AisanRoadEdgeFileReader::AisanRoadEdge>& roadedge_data,
+      const std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea>& wayarea_data,
+      const std::vector<UtilityHNS::AisanCrossWalkFileReader::AisanCrossWalk>& crosswalk_data,
+      const std::vector<UtilityHNS::AisanNodesFileReader::AisanNode>& nodes_data,
+      const std::vector<UtilityHNS::AisanDataConnFileReader::DataConn>& conn_data,
+      const GPSPoint& origin, RoadNetwork& map, const bool& bSpecialFlag = false,
+      const bool& bFindLaneChangeLanes = false,
+      const bool& bFindCurbsAndWayArea = false);
+
+  static void ConstructRoadNetworkFromROSMessageV2(const std::vector<UtilityHNS::AisanLanesFileReader::AisanLane>& lanes_data,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+        const std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine>& dt_data,
+        const std::vector<UtilityHNS::AisanIntersectionFileReader::AisanIntersection>& intersection_data,
+        const std::vector<UtilityHNS::AisanAreasFileReader::AisanArea>& area_data,
+        const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+        const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
+        const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
+        const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
+        const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
+        const std::vector<UtilityHNS::AisanRoadEdgeFileReader::AisanRoadEdge>& roadedge_data,
+        const std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea>& wayarea_data,
+        const std::vector<UtilityHNS::AisanCrossWalkFileReader::AisanCrossWalk>& crosswalk_data,
+        const std::vector<UtilityHNS::AisanNodesFileReader::AisanNode>& nodes_data,
+        const std::vector<UtilityHNS::AisanDataConnFileReader::DataConn>& conn_data,
+        UtilityHNS::AisanLanesFileReader* pLaneData,
+        UtilityHNS::AisanPointsFileReader* pPointsData,
+        UtilityHNS::AisanNodesFileReader* pNodesData,
+        UtilityHNS::AisanLinesFileReader* pLinedata,
+        const GPSPoint& origin, RoadNetwork& map, const bool& bSpecialFlag = false,
+        const bool& bFindLaneChangeLanes = false,
+        const bool& bFindCurbsAndWayArea = false);
+
+  static void ConstructRoadNetworkFromDataFiles(const std::string vectoMapPath, RoadNetwork& map, const bool& bZeroOrigin = false);
+
+  static void UpdateMapWithOccupancyGrid(OccupancyToGridMap& map_info, const std::vector<int>& data, RoadNetwork& map, std::vector<WayPoint*>& updated_list);
+
+  static bool GetWayPoint(const int& id, const int& laneID,const double& refVel, const int& did,
+      const std::vector<UtilityHNS::AisanCenterLinesFileReader::AisanCenterLine>& dtpoints,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points,
+      const GPSPoint& origin, WayPoint& way_point);
+
+  static void LoadKML(const std::string& kmlMap, RoadNetwork& map);
+
+  static TiXmlElement* GetHeadElement(TiXmlElement* pMainElem);
+  static TiXmlElement* GetDataFolder(const std::string& folderName, TiXmlElement* pMainElem);
+
+
+  static Lane* GetClosestLaneFromMap(const WayPoint& pos, RoadNetwork& map, const double& distance = 5.0, const bool bDirectionBased = true);
+  static std::vector<Lane*> GetClosestLanesListFromMap(const WayPoint& pos, RoadNetwork& map, const double& distance = 2.0, const bool bDirectionBased = true);
+  static Lane* GetClosestLaneFromMapDirectionBased(const WayPoint& pos, RoadNetwork& map, const double& distance = 5.0);
+  static std::vector<Lane*> GetClosestMultipleLanesFromMap(const WayPoint& pos, RoadNetwork& map, const double& distance = 5.0);
+  static WayPoint* GetClosestWaypointFromMap(const WayPoint& pos, RoadNetwork& map, const bool bDirectionBased = true);
+  static std::vector<Lane*> GetClosestLanesFast(const WayPoint& pos, RoadNetwork& map, const double& distance = 10.0);
+
+  static std::vector<WayPoint*> GetClosestWaypointsListFromMap(const WayPoint& center, RoadNetwork& map, const double& distance = 2.0, const bool bDirectionBased = true);
+
+  static WayPoint* GetClosestBackWaypointFromMap(const WayPoint& pos, RoadNetwork& map);
+  static WayPoint GetFirstWaypoint(RoadNetwork& map);
+  static WayPoint* GetLastWaypoint(RoadNetwork& map);
+  static void FindAdjacentLanes(RoadNetwork& map);
+  static void FindAdjacentLanesV2(RoadNetwork& map);
+  static void ExtractSignalData(const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
+      const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+      const GPSPoint& origin, RoadNetwork& map);
+
+  static void ExtractSignalDataV2(const std::vector<UtilityHNS::AisanSignalFileReader::AisanSignal>& signal_data,
+        const std::vector<UtilityHNS::AisanVectorFileReader::AisanVector>& vector_data,
+        UtilityHNS::AisanPointsFileReader* pPointsData,
+        const GPSPoint& origin, RoadNetwork& map);
+
+  static void ExtractStopLinesData(const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
+      const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+      const GPSPoint& origin, RoadNetwork& map);
+
+  static void ExtractStopLinesDataV2(const std::vector<UtilityHNS::AisanStopLineFileReader::AisanStopLine>& stop_line_data,
+        UtilityHNS::AisanLinesFileReader* pLineData,
+        UtilityHNS::AisanPointsFileReader* pPointData,
+        const GPSPoint& origin, RoadNetwork& map);
+
+  static void ExtractCurbData(const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
+        const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+        const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+        const GPSPoint& origin, RoadNetwork& map);
+
+  static void ExtractCurbDataV2(const std::vector<UtilityHNS::AisanCurbFileReader::AisanCurb>& curb_data,
+          UtilityHNS::AisanLinesFileReader* pLinedata,
+          UtilityHNS::AisanPointsFileReader* pPointsData,
+          const GPSPoint& origin, RoadNetwork& map);
+
+  static void ExtractWayArea(const std::vector<UtilityHNS::AisanAreasFileReader::AisanArea>& area_data,
+      const std::vector<UtilityHNS::AisanWayareaFileReader::AisanWayarea>& wayarea_data,
+      const std::vector<UtilityHNS::AisanLinesFileReader::AisanLine>& line_data,
+      const std::vector<UtilityHNS::AisanPointsFileReader::AisanPoints>& points_data,
+      const GPSPoint& origin, RoadNetwork& map);
+
+  static void LinkMissingBranchingWayPoints(RoadNetwork& map);
+  static void LinkMissingBranchingWayPointsV2(RoadNetwork& map);
+  static void LinkTrafficLightsAndStopLinesConData(const std::vector<UtilityHNS::AisanDataConnFileReader::DataConn>& conn_data,
+      const std::vector<std::pair<int,int> >& id_replace_list, RoadNetwork& map);
+
+  static void LinkTrafficLightsAndStopLines(RoadNetwork& map);
+
+  static void LinkTrafficLightsAndStopLinesV2(RoadNetwork& map);
+
+  static void GetUniqueNextLanes(const Lane* l,  const std::vector<Lane*>& traversed_lanes, std::vector<Lane*>& lanes_list);
+
+  static GPSPoint GetTransformationOrigin(const int& bToyotaCityMap = 0);
+
+  static Lane* GetLaneFromPath(const WayPoint& currPos, const std::vector<WayPoint>& currPath);
+  static Lane* GetLaneById(const int& id,RoadNetwork& map);
+  static int GetLaneIdByWaypointId(const int& id,std::vector<Lane>& lanes);
+
+  static WayPoint* FindWaypoint(const int& id, RoadNetwork& map);
+  static WayPoint* FindWaypointV2(const int& id, const int& l_id, RoadNetwork& map);
+
+  static std::vector<Curb> GetCurbsList(TiXmlElement* pElem);
+  static std::vector<Boundary> GetBoundariesList(TiXmlElement* pElem);
+  static std::vector<Marking> GetMarkingsList(TiXmlElement* pElem);
+  static std::vector<Crossing> GetCrossingsList(TiXmlElement* pElem);
+  static std::vector<TrafficSign> GetTrafficSignsList(TiXmlElement* pElem);
+  static std::vector<TrafficLight> GetTrafficLightsList(TiXmlElement* pElem);
+  static std::vector<StopLine> GetStopLinesList(TiXmlElement* pElem);
+  static std::vector<Lane> GetLanesList(TiXmlElement* pElem);
+  static std::vector<RoadSegment> GetRoadSegmentsList(TiXmlElement* pElem);
+  static std::vector<int> GetIDsFromPrefix(const std::string& str, const std::string& prefix, const std::string& postfix);
+  static std::vector<double> GetDoubleFromPrefix(const std::string& str, const std::string& prefix, const std::string& postfix);
+  static std::pair<ACTION_TYPE, double> GetActionPairFromPrefix(const std::string& str, const std::string& prefix, const std::string& postfix);
+  static std::vector<WayPoint> GetCenterLaneData(TiXmlElement* pElem, const int& currLaneID);
+  static std::vector<WayPoint> GetCenterLaneDataVer0(TiXmlElement* pElem, const int& currLaneID);
+  static std::vector<GPSPoint> GetPointsData(TiXmlElement* pElem);
+  static std::vector<std::string> SplitString(const std::string& str, const std::string& token);
+
+  //static void CreateKmlFromLocalizationPathFile(const std::string& pathFileName,const double& maxLaneDistance, const double& density,const std::vector<TrafficLight>& trafficLights, const std::vector<GPSPoint>& stopLines);
+
+  static void AssignActionCostToLane(Lane* pL, ACTION_TYPE action, double cost);
+
+  static int ReplaceMyID(int& id, const std::vector<std::pair<int,int> >& rep_list);
+
+  static void GetLanesStartPoints(UtilityHNS::AisanLanesFileReader* pLaneData,
+        std::vector<int>& m_LanesStartIds);
+
+  static void GetLanePoints(UtilityHNS::AisanLanesFileReader* pLaneData,
+        UtilityHNS::AisanPointsFileReader* pPointsData,
+        UtilityHNS::AisanNodesFileReader* pNodesData, int lnID,
+        PlannerHNS::Lane& out_lane);
+
+  static void CreateLanes(UtilityHNS::AisanLanesFileReader* pLaneData,
+      UtilityHNS::AisanPointsFileReader* pPointsData,
+      UtilityHNS::AisanNodesFileReader* pNodesData,
+      std::vector<PlannerHNS::Lane>& out_lanes);
+
+  static void ConnectLanes(UtilityHNS::AisanLanesFileReader* pLaneData,
+      std::vector<PlannerHNS::Lane>& lanes);
+
+  static bool GetPointFromDataList(UtilityHNS::AisanPointsFileReader* pPointsData,const int& pid, WayPoint& out_wp);
+
+  static int GetBeginPointIdFromLaneNo(UtilityHNS::AisanLanesFileReader* pLaneData,
+      UtilityHNS::AisanPointsFileReader* pPointsData,
+      UtilityHNS::AisanNodesFileReader* pNodesData, const int& LnID);
+  static int GetEndPointIdFromLaneNo(UtilityHNS::AisanLanesFileReader* pLaneData,
+      UtilityHNS::AisanPointsFileReader* pPointsData,
+      UtilityHNS::AisanNodesFileReader* pNodesData,const int& LnID);
+
+  static bool IsStartLanePoint(UtilityHNS::AisanLanesFileReader* pLaneData, UtilityHNS::AisanLanesFileReader::AisanLane* pL);
+  static bool IsEndLanePoint(UtilityHNS::AisanLanesFileReader* pLaneData, UtilityHNS::AisanLanesFileReader::AisanLane* pL);
+
+  static void FixRedundantPointsLanes(std::vector<Lane>& lanes);
+  static void FixTwoPointsLanes(std::vector<Lane>& lanes);
+  static void FixTwoPointsLane(Lane& lanes);
+  static void FixUnconnectedLanes(std::vector<Lane>& lanes);
+  static void InsertWayPointToBackOfLane(const WayPoint& wp, Lane& lane, int& global_id);
+  static void InsertWayPointToFrontOfLane(const WayPoint& wp, Lane& lane, int& global_id);
+
+  static void LinkLanesPointers(PlannerHNS::RoadNetwork& map);
+
+  static void GetMapMaxIds(PlannerHNS::RoadNetwork& map);
+
+  static double m_USING_VER_ZERO;
+
+  static int g_max_point_id;
+  static int g_max_lane_id;
+  static int g_max_stop_line_id;
+  static int g_max_traffic_light_id ;
+
+};
+
+} /* namespace PlannerHNS */
+
+#endif /* MAPPINGHELPERS_H_ */

src1/planning/op_planner/include/op_planner/MatrixOperations.h

@@ -0,0 +1,79 @@
+
+/// \file MatrixOperations.h
+/// \brief Simple matrix operations
+/// \author Hatem Darweesh
+/// \date Jun 19, 2016
+
+
+#ifndef MATRIXOPERATIONS_H_
+#define MATRIXOPERATIONS_H_
+
+#include "RoadNetwork.h"
+#include <math.h>
+
+
+namespace PlannerHNS {
+
+
+class Mat3
+{
+  double m[3][3];
+
+public:
+  Mat3()
+  {
+    //initialize Identity by default
+    for(int i=0;i<3;i++)
+      for(int j=0;j<3;j++)
+        m[i][j] = 0;
+
+    m[0][0] = m[1][1] = m[2][2] = 1;
+  }
+
+  Mat3(double transX, double transY, bool mirrorX, bool mirrorY )
+  {
+    m[0][0] = (mirrorX == true ) ? -1 : 1; m[0][1] =  0; m[0][2] =  transX;
+    m[1][0] = 0; m[1][1] =  (mirrorY==true) ? -1 : 1; m[1][2] =  transY;
+    m[2][0] = 0; m[2][1] =  0; m[2][2] =  1;
+  }
+
+  Mat3(double transX, double transY)
+  {
+    m[0][0] = 1; m[0][1] =  0; m[0][2] =  transX;
+    m[1][0] = 0; m[1][1] =  1; m[1][2] =  transY;
+    m[2][0] = 0; m[2][1] =  0; m[2][2] =  1;
+  }
+
+  Mat3(double rotation_angle)
+  {
+    double c = cos(rotation_angle);
+    double s = sin(rotation_angle);
+    m[0][0] = c; m[0][1] = -s; m[0][2] =  0;
+    m[1][0] = s; m[1][1] =  c; m[1][2] =  0;
+    m[2][0] = 0; m[2][1] =  0; m[2][2] =  1;
+  }
+
+  Mat3(GPSPoint rotationCenter)
+  {
+    double c = cos(rotationCenter.a);
+    double s = sin(rotationCenter.a);
+    double u = rotationCenter.x;
+    double v = rotationCenter.y;
+    m[0][0] = c; m[0][1] = -s; m[0][2] = -u*c + v*s + u;
+    m[1][0] = s; m[1][1] =  c; m[1][2] = -u*s - v*c + v;
+    m[2][0] = 0; m[2][1] =  0; m[2][2] =  1;
+  }
+
+
+  GPSPoint operator * (GPSPoint v)
+  {
+    GPSPoint _v = v;
+    v.x = m[0][0]*_v.x + m[0][1]*_v.y + m[0][2]*1;
+    v.y = m[1][0]*_v.x + m[1][1]*_v.y + m[1][2]*1;
+    return v;
+  }
+};
+
+} /* namespace PlannerHNS */
+
+#endif /* MATRIXOPERATIONS_H_ */

src1/planning/op_planner/include/op_planner/PassiveDecisionMaker.h

@@ -0,0 +1,37 @@
+/*
+ * CarState.h
+ *
+ *  Created on: Dec 14, 2016
+ *      Author: hatem
+ */
+
+#ifndef PASSIVE_BEHAVIOR_DECISION_MAKER
+#define PASSIVE_BEHAVIOR_DECISION_MAKER
+
+#include "BehaviorStateMachine.h"
+#include "PlannerCommonDef.h"
+#include "RoadNetwork.h"
+
+namespace PlannerHNS
+{
+
+class PassiveDecisionMaker
+{
+public:
+  PassiveDecisionMaker();
+  PassiveDecisionMaker(const PassiveDecisionMaker& obj);
+  PassiveDecisionMaker& operator=(const PassiveDecisionMaker& obj);
+  virtual ~PassiveDecisionMaker();
+  PlannerHNS::BehaviorState MoveStep(const double& dt, PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo);
+  PlannerHNS::ParticleInfo MoveStepSimple(const double& dt, PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo);
+
+private:
+  double GetVelocity(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo, const RelativeInfo& info);
+  double GetSteerAngle(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const RelativeInfo& info);
+  bool CheckForStopLine(PlannerHNS::WayPoint& currPose, const std::vector<WayPoint>& path, const CAR_BASIC_INFO& carInfo);
+
+};
+
+} /* namespace PlannerHNS */
+
+#endif /* PASSIVE_BEHAVIOR_DECISION_MAKER */

src1/planning/op_planner/include/op_planner/PlannerCommonDef.h

@@ -0,0 +1,165 @@
+
+/// \file PlannerCommonDef.h
+/// \brief Definition file for control related data types
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+
+#ifndef PLANNERCOMMONDEF_H_
+#define PLANNERCOMMONDEF_H_
+
+#include <math.h>
+#include <string>
+
+namespace PlannerHNS
+{
+
+enum MAP_SOURCE_TYPE
+{
+  MAP_AUTOWARE,
+  MAP_FOLDER,
+  MAP_KML_FILE,
+  MAP_ONE_CSV_FILE,
+  MAP_LANES_CSV_FILES
+};
+
+enum CAR_TYPE
+{
+  Mv2Car, //!< Mv2Car
+  PHVCar, //!< PHVCar
+  HVCar,  //!< HVCar
+  RoboCar,//!< RoboCar
+  SimulationCar
+};
+
+class PID_CONST
+{
+public:
+  double kP;
+  double kI;
+  double kD;
+
+  PID_CONST()
+  {
+    kP = kI = kD = 0;
+  }
+
+  PID_CONST(const double& p, const double& i, const double& d)
+  {
+    kP = p;
+    kI = i;
+    kD = d;
+  }
+};
+
+class ControllerParams
+{
+public:
+  double SimulationSteeringDelay;
+  double SteeringDelay;
+  double minPursuiteDistance;
+  PID_CONST Steering_Gain;
+  PID_CONST Velocity_Gain;
+  double Acceleration;
+  double Deceleration;
+  double FollowDistance;
+  double LowpassSteerCutoff;
+  double maxAccel;
+  double maxDecel;
+
+
+  ControllerParams()
+  {
+    SimulationSteeringDelay = 0.0;
+    SteeringDelay     = 0.8;
+    Acceleration    = 0.5;
+    Deceleration    = -0.8;
+    FollowDistance    = 8.0;
+    LowpassSteerCutoff  = 5.0;
+    maxAccel      = 0.9;
+    minPursuiteDistance = 2.0;
+    maxDecel       = -1.5;
+  }
+};
+
+class CAR_BASIC_INFO
+{
+public:
+  CAR_TYPE model;
+
+  double turning_radius;
+  double wheel_base;
+  double max_speed_forward;
+  double min_speed_forward;
+  double max_speed_backword;
+  double max_steer_value;
+  double min_steer_value;
+  double max_brake_value;
+  double min_brake_value;
+  double max_steer_angle;
+  double min_steer_angle;
+  double length;
+  double width;
+  double max_acceleration;
+  double max_deceleration;
+
+  CAR_BASIC_INFO()
+  {
+    model         = SimulationCar;
+    turning_radius     = 5.2;
+    wheel_base      = 2.7;
+    max_speed_forward    = 3.0;
+    min_speed_forward    = 0.0;
+    max_speed_backword  = 1.0;
+    max_steer_value    = 660;
+    min_steer_value    = -660;
+    max_brake_value    = 0;
+    min_brake_value    = 0;
+    max_steer_angle    = 0.42;
+    min_steer_angle    = 0.42;
+    length        = 4.3;
+    width          = 1.82;
+    max_acceleration    = 1.5; // m/s2
+    max_deceleration    = -1.5; // 1/3 G
+  }
+
+  double CalcMaxSteeringAngle()
+  {
+    return  max_steer_angle;//asin(wheel_base/turning_radius);
+  }
+
+  double BoundSpeed(double s)
+  {
+  if(s > 0 && s > max_speed_forward)
+    return max_speed_forward;
+  if(s < 0 && s < max_speed_backword)
+    return max_speed_backword;
+  return s;
+  }
+
+  double BoundSteerAngle(double a)
+  {
+  if(a > max_steer_angle)
+    return max_steer_angle;
+  if(a < min_steer_angle)
+    return min_steer_angle;
+
+  return a;
+  }
+
+  double BoundSteerValue(double v)
+  {
+    if(v >= max_steer_value)
+    return max_steer_value;
+  if(v <= min_steer_value)
+    return min_steer_value;
+
+  return v;
+  }
+
+};
+
+
+} /* namespace PlannerHNS */
+
+#endif /* PLANNERCOMMONDEF_H_ */

src1/planning/op_planner/include/op_planner/PlannerH.h

@@ -0,0 +1,56 @@
+
+/// \file PlannerH.h
+/// \brief Main functions for path generation (global and local)
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+
+#define START_POINT_MAX_DISTANCE 8 // meters
+#define GOAL_POINT_MAX_DISTANCE 8 // meters
+#define LANE_CHANGE_SMOOTH_FACTOR_DISTANCE 8 // meters
+
+#include "RoadNetwork.h"
+
+namespace PlannerHNS
+{
+
+enum PLANDIRECTION {MOVE_FORWARD_ONLY, MOVE_BACKWARD_ONLY,   MOVE_FREE};
+enum HeuristicConstrains {EUCLIDEAN, NEIGBORHOOD,DIRECTION };
+
+class PlannerH
+{
+public:
+  PlannerH();
+  virtual ~PlannerH();
+
+  void GenerateRunoffTrajectory(const std::vector<std::vector<WayPoint> >& referencePaths, const WayPoint& carPos, const bool& bEnableLaneChange, const double& speed, const double& microPlanDistance,
+        const double& maxSpeed,const double& minSpeed, const double&  carTipMargin, const double& rollInMargin,
+        const double& rollInSpeedFactor, const double& pathDensity, const double& rollOutDensity,
+        const int& rollOutNumber, const double& SmoothDataWeight, const double& SmoothWeight,
+        const double& SmoothTolerance, const double& speedProfileFactor, const bool& bHeadingSmooth,
+        const int& iCurrGlobalPath, const int& iCurrLocalTraj,
+        std::vector<std::vector<std::vector<WayPoint> > >& rollOutsPaths,
+        std::vector<WayPoint>& sampledPoints);
+
+  double PlanUsingDP(const WayPoint& carPos,const WayPoint& goalPos,
+      const double& maxPlanningDistance, const bool bEnableLaneChange, const std::vector<int>& globalPath,
+      RoadNetwork& map, std::vector<std::vector<WayPoint> >& paths, std::vector<WayPoint*>* all_cell_to_delete = 0);
+
+   double PlanUsingDPRandom(const WayPoint& start,
+        const double& maxPlanningDistance,
+        RoadNetwork& map,
+        std::vector<std::vector<WayPoint> >& paths);
+
+  double PredictPlanUsingDP(Lane* lane, const WayPoint& carPos, const double& maxPlanningDistance,
+      std::vector<std::vector<WayPoint> >& paths);
+
+  double PredictPlanUsingDP(const WayPoint& startPose, WayPoint* closestWP, const double& maxPlanningDistance, std::vector<std::vector<WayPoint> >& paths, const bool& bFindBranches = true);
+
+  double PredictTrajectoriesUsingDP(const WayPoint& startPose, std::vector<WayPoint*> closestWPs, const double& maxPlanningDistance, std::vector<std::vector<WayPoint> >& paths, const bool& bFindBranches = true, const bool bDirectionBased = false, const bool pathDensity = 1.0);
+
+  void DeleteWaypoints(std::vector<WayPoint*>& wps);
+};
+
+}
+
+

src1/planning/op_planner/include/op_planner/PlanningHelpers.h

@@ -0,0 +1,195 @@
+
+/// \file PlanningHelpers.h
+/// \brief Helper functions for planning algorithms
+/// \author Hatem Darweesh
+/// \date Jun 16, 2016
+
+
+#ifndef PLANNINGHELPERS_H_
+#define PLANNINGHELPERS_H_
+
+#include "RoadNetwork.h"
+#include "op_utility/UtilityH.h"
+#include "op_utility/DataRW.h"
+#include "tinyxml.h"
+
+
+namespace PlannerHNS {
+
+#define distance2points(from , to) sqrt(pow(to.x - from.x, 2) + pow(to.y - from.y, 2))
+#define distance2pointsSqr(from , to) pow(to.x - from.x, 2) + pow(to.y - from.y, 2)
+#define pointNorm(v) sqrt(v.x*v.x + v.y*v.y)
+#define angle2points(from , to) atan2(to.y - from.y, to.x - from.x )
+#define LANE_CHANGE_SPEED_FACTOR 0.5
+#define LANE_CHANGE_COST 3.0 // meters
+#define BACKUP_STRAIGHT_PLAN_DISTANCE 75 //meters
+#define LANE_CHANGE_MIN_DISTANCE 5
+
+class PlanningHelpers
+{
+
+public:
+  static std::vector<std::pair<GPSPoint, GPSPoint> > m_TestingClosestPoint;
+
+public:
+  PlanningHelpers();
+  virtual ~PlanningHelpers();
+
+  static bool GetRelativeInfo(const std::vector<WayPoint>& trajectory, const WayPoint& p, RelativeInfo& info, const int& prevIndex = 0);
+
+  static bool GetRelativeInfoRange(const std::vector<std::vector<WayPoint> >& trajectories, const WayPoint& p, const double& searchDistance, RelativeInfo& info);
+
+  static bool GetRelativeInfoLimited(const std::vector<WayPoint>& trajectory, const WayPoint& p, RelativeInfo& info, const int& prevIndex = 0);
+
+  static WayPoint GetFollowPointOnTrajectory(const std::vector<WayPoint>& trajectory, const RelativeInfo& init_p, const double& distance, unsigned int& point_index);
+
+  static double GetExactDistanceOnTrajectory(const std::vector<WayPoint>& trajectory, const RelativeInfo& p1,const RelativeInfo& p2);
+
+  static int GetClosestNextPointIndex_obsolete(const std::vector<WayPoint>& trajectory, const WayPoint& p, const int& prevIndex = 0);
+
+  static int GetClosestNextPointIndexFast(const std::vector<WayPoint>& trajectory, const WayPoint& p, const int& prevIndex = 0);
+
+  static int GetClosestNextPointIndexFastV2(const std::vector<WayPoint>& trajectory, const WayPoint& p, const int& prevIndex = 0);
+
+  static int GetClosestNextPointIndexDirectionFast(const std::vector<WayPoint>& trajectory, const WayPoint& p, const int& prevIndex = 0);
+
+  static int GetClosestNextPointIndexDirectionFastV2(const std::vector<WayPoint>& trajectory, const WayPoint& p, const int& prevIndex = 0);
+
+  static int GetClosestPointIndex_obsolete(const std::vector<WayPoint>& trajectory, const WayPoint& p,const int& prevIndex = 0 );
+
+  static WayPoint GetPerpendicularOnTrajectory_obsolete(const std::vector<WayPoint>& trajectory, const WayPoint& p, double& distance, const int& prevIndex = 0);  static double GetPerpDistanceToTrajectorySimple_obsolete(const std::vector<WayPoint>& trajectory, const WayPoint& p, const int& prevIndex = 0);
+
+  static double GetPerpDistanceToVectorSimple_obsolete(const WayPoint& p1, const WayPoint& p2, const WayPoint& pose);
+
+  static WayPoint GetNextPointOnTrajectory_obsolete(const std::vector<WayPoint>& trajectory, const double& distance, const int& currIndex = 0);
+
+  static double GetDistanceOnTrajectory_obsolete(const std::vector<WayPoint>& path, const int& start_index, const WayPoint& p);
+
+  static void CreateManualBranch(std::vector<WayPoint>& path, const int& degree, const DIRECTION_TYPE& direction);
+
+  static void CreateManualBranchFromTwoPoints(WayPoint& p1,WayPoint& p2 , const double& distance, const DIRECTION_TYPE& direction, std::vector<WayPoint>& path);
+
+  static void FixPathDensity(std::vector<WayPoint>& path, const double& distanceDensity);
+
+  static void SmoothPath(std::vector<WayPoint>& path, double weight_data =0.25,double weight_smooth = 0.25,double tolerance = 0.01);
+
+  static double CalcCircle(const GPSPoint& pt1, const GPSPoint& pt2, const GPSPoint& pt3, GPSPoint& center);
+
+  static void FixAngleOnly(std::vector<WayPoint>& path);
+
+  static double CalcAngleAndCost(std::vector<WayPoint>& path, const double& lastCost = 0, const bool& bSmooth = true );
+
+  //static double CalcAngleAndCostSimple(std::vector<WayPoint>& path, const double& lastCost = 0);
+
+  static double CalcAngleAndCostAndCurvatureAnd2D(std::vector<WayPoint>& path, const double& lastCost = 0);
+
+  static void PredictConstantTimeCostForTrajectory(std::vector<PlannerHNS::WayPoint>& path, const PlannerHNS::WayPoint& currPose, const double& minVelocity, const double& minDist);
+
+  static double GetAccurateDistanceOnTrajectory(std::vector<WayPoint>& path, const int& start_index, const WayPoint& p);
+
+  static void ExtractPartFromPointToDistance(const std::vector<WayPoint>& originalPath, const WayPoint& pos, const double& minDistance,
+      const double& pathDensity, std::vector<WayPoint>& extractedPath, const double& SmoothDataWeight, const double& SmoothWeight, const double& SmoothTolerance);
+
+  static void ExtractPartFromPointToDistanceFast(const std::vector<WayPoint>& originalPath, const WayPoint& pos, const double& minDistance,
+        const double& pathDensity, std::vector<WayPoint>& extractedPath, const double& SmoothDataWeight, const double& SmoothWeight, const double& SmoothTolerance);
+
+  static void ExtractPartFromPointToDistanceDirectionFast(const std::vector<WayPoint>& originalPath, const WayPoint& pos, const double& minDistance,
+      const double& pathDensity, std::vector<WayPoint>& extractedPath);
+
+  static void CalculateRollInTrajectories(const WayPoint& carPos, const double& speed, const std::vector<WayPoint>& originalCenter, int& start_index,
+      int& end_index, std::vector<double>& end_laterals ,
+      std::vector<std::vector<WayPoint> >& rollInPaths, const double& max_roll_distance,
+      const double& maxSpeed, const double&  carTipMargin, const double& rollInMargin,
+      const double& rollInSpeedFactor, const double& pathDensity, const double& rollOutDensity,
+      const int& rollOutNumber, const double& SmoothDataWeight, const double& SmoothWeight,
+      const double& SmoothTolerance, const bool& bHeadingSmooth,
+      std::vector<WayPoint>& sampledPoints);
+
+  static void SmoothSpeedProfiles(std::vector<WayPoint>& path_in, double weight_data, double weight_smooth, double tolerance  = 0.1);
+
+  static void SmoothCurvatureProfiles(std::vector<WayPoint>& path_in, double weight_data, double weight_smooth, double tolerance = 0.1);
+
+  static void SmoothWayPointsDirections(std::vector<WayPoint>& path_in, double weight_data, double weight_smooth, double tolerance  = 0.1);
+
+  static void SmoothGlobalPathSpeed(std::vector<WayPoint>& path);
+
+  static void GenerateRecommendedSpeed(std::vector<WayPoint>& path, const double& max_speed, const double& speedProfileFactor);
+
+//  static WayPoint* BuildPlanningSearchTree(Lane* l, const WayPoint& prevWayPointIndex,
+//      const WayPoint& startPos, const WayPoint& goalPos,
+//      const std::vector<int>& globalPath, const double& DistanceLimit,
+//      int& nMaxLeftBranches, int& nMaxRightBranches,
+//      std::vector<WayPoint*>& all_cells_to_delete );
+
+  static WayPoint* BuildPlanningSearchTreeV2(WayPoint* pStart,
+      const WayPoint& goalPos,
+      const std::vector<int>& globalPath, const double& DistanceLimit,
+      const bool& bEnableLaneChange,
+      std::vector<WayPoint*>& all_cells_to_delete );
+
+  static WayPoint* BuildPlanningSearchTreeStraight(WayPoint* pStart,
+      const double& DistanceLimit,
+      std::vector<WayPoint*>& all_cells_to_delete );
+
+  static int PredictiveDP(WayPoint* pStart, const double& DistanceLimit,
+      std::vector<WayPoint*>& all_cells_to_delete, std::vector<WayPoint*>& end_waypoints);
+
+  static int PredictiveIgnorIdsDP(WayPoint* pStart, const double& DistanceLimit,
+        std::vector<WayPoint*>& all_cells_to_delete, std::vector<WayPoint*>& end_waypoints, std::vector<int>& lanes_ids);
+
+  static bool CheckLaneIdExits(const std::vector<int>& lanes, const Lane* pL);
+
+  static WayPoint* CheckLaneExits(const std::vector<WayPoint*>& nodes, const Lane* pL);
+
+  static WayPoint* CheckNodeExits(const std::vector<WayPoint*>& nodes, const WayPoint* pL);
+
+  static WayPoint* CreateLaneHeadCell(Lane* pLane, WayPoint* pLeft, WayPoint* pRight,
+      WayPoint* pBack);
+
+  static double GetLanePoints(Lane* l, const WayPoint& prevWayPointIndex,
+      const double& minDistance , const double& prevCost, std::vector<WayPoint>& points);
+
+  static WayPoint* GetMinCostCell(const std::vector<WayPoint*>& cells, const std::vector<int>& globalPathIds);
+
+  static void TraversePathTreeBackwards(WayPoint* pHead, WayPoint* pStartWP, const std::vector<int>& globalPathIds,
+      std::vector<WayPoint>& localPath, std::vector<std::vector<WayPoint> >& localPaths);
+
+  static void ExtractPlanAlernatives(const std::vector<WayPoint>& singlePath, std::vector<std::vector<WayPoint> >& allPaths);
+
+  static std::vector<int> GetUniqueLeftRightIds(const std::vector<WayPoint>& path);
+
+  static bool FindInList(const std::vector<int>& list,const int& x);
+
+  static void RemoveWithValue(std::vector<int>& list,const int& x);
+
+  static ACTION_TYPE GetBranchingDirection(WayPoint& currWP, WayPoint& nextWP);
+
+  static void CalcContourPointsForDetectedObjects(const WayPoint& currPose, std::vector<DetectedObject>& obj_list, const double& filterDistance = 100);
+
+  static double GetVelocityAhead(const std::vector<WayPoint>& path, const RelativeInfo& info,int& prev_index, const double& reasonable_brake_distance);
+
+  static bool CompareTrajectories(const std::vector<WayPoint>& path1, const std::vector<WayPoint>& path2);
+
+  static double GetDistanceToClosestStopLineAndCheck(const std::vector<WayPoint>& path, const WayPoint& p, const double& giveUpDistance, int& stopLineID,int& stopSignID, int& trafficLightID, const int& prevIndex = 0);
+
+  static bool GetThreePointsInfo(const WayPoint& p0, const WayPoint& p1, const WayPoint& p2, WayPoint& perp_p, double& long_d, double lat_d);
+
+  static void WritePathToFile(const std::string& fileName, const std::vector<WayPoint>& path);
+
+  static LIGHT_INDICATOR GetIndicatorsFromPath(const std::vector<WayPoint>& path, const WayPoint& pose, const double& seachDistance);
+
+  static PlannerHNS::WayPoint GetRealCenter(const PlannerHNS::WayPoint& currState, const double& wheel_base);
+
+  static void TestQuadraticSpline(const std::vector<WayPoint>& center_line, std::vector<WayPoint>& path);
+
+  static double frunge ( double x );
+
+  static double fprunge ( double x );
+
+  static double fpprunge ( double x );
+
+};
+
+} /* namespace PlannerHNS */
+
+#endif /* PLANNINGHELPERS_H_ */

src1/planning/op_planner/include/op_planner/RoadNetwork.h

@@ -0,0 +1,1344 @@
+
+/// \file RoadNetwork.h
+/// \brief Definition of OpenPlanner's data types
+/// \author Hatem Darweesh
+/// \date May 19, 2016
+
+
+#ifndef ROADNETWORK_H_
+#define ROADNETWORK_H_
+
+#include <string>
+#include <vector>
+#include <sstream>
+#include "op_utility/UtilityH.h"
+
+#define OPENPLANNER_ENABLE_LOGS
+
+namespace PlannerHNS
+{
+
+
+enum DIRECTION_TYPE {  FORWARD_DIR, FORWARD_LEFT_DIR, FORWARD_RIGHT_DIR,
+  BACKWARD_DIR, BACKWARD_LEFT_DIR, BACKWARD_RIGHT_DIR, STANDSTILL_DIR};
+
+enum OBSTACLE_TYPE {SIDEWALK, TREE, CAR, TRUCK, HOUSE, PEDESTRIAN, CYCLIST, GENERAL_OBSTACLE};
+
+enum DRIVABLE_TYPE {DIRT, TARMAC, PARKINGAREA, INDOOR, GENERAL_AREA};
+
+enum GLOBAL_STATE_TYPE {G_WAITING_STATE, G_PLANING_STATE, G_FORWARD_STATE, G_BRANCHING_STATE, G_FINISH_STATE};
+
+enum STATE_TYPE {INITIAL_STATE, WAITING_STATE, FORWARD_STATE, STOPPING_STATE, EMERGENCY_STATE,
+  TRAFFIC_LIGHT_STOP_STATE,TRAFFIC_LIGHT_WAIT_STATE, STOP_SIGN_STOP_STATE, STOP_SIGN_WAIT_STATE, FOLLOW_STATE, LANE_CHANGE_STATE, OBSTACLE_AVOIDANCE_STATE, GOAL_STATE, FINISH_STATE, YIELDING_STATE, BRANCH_LEFT_STATE, BRANCH_RIGHT_STATE};
+
+enum LIGHT_INDICATOR {INDICATOR_LEFT, INDICATOR_RIGHT, INDICATOR_BOTH , INDICATOR_NONE};
+
+enum SHIFT_POS {SHIFT_POS_PP = 0x60, SHIFT_POS_RR = 0x40, SHIFT_POS_NN = 0x20,
+  SHIFT_POS_DD = 0x10, SHIFT_POS_BB = 0xA0, SHIFT_POS_SS = 0x0f, SHIFT_POS_UU = 0xff };
+
+enum ACTION_TYPE {FORWARD_ACTION, BACKWARD_ACTION, STOP_ACTION, LEFT_TURN_ACTION,
+  RIGHT_TURN_ACTION, U_TURN_ACTION, SWERVE_ACTION, OVERTACK_ACTION, START_ACTION, SLOWDOWN_ACTION, CHANGE_DESTINATION, WAITING_ACTION, DESTINATION_REACHED,  UNKOWN_ACTION};
+
+enum BEH_STATE_TYPE {BEH_FORWARD_STATE=0,BEH_STOPPING_STATE=1, BEH_BRANCH_LEFT_STATE=2, BEH_BRANCH_RIGHT_STATE=3, BEH_YIELDING_STATE=4, BEH_ACCELERATING_STATE=5, BEH_SLOWDOWN_STATE=6};
+
+enum SEGMENT_TYPE {NORMAL_ROAD_SEG, INTERSECTION_ROAD_SEG, UTURN_ROAD_SEG, EXIT_ROAD_SEG, MERGE_ROAD_SEG, HIGHWAY_ROAD_SEG};
+enum RoadSegmentType {NORMAL_ROAD, INTERSECTION_ROAD, UTURN_ROAD, EXIT_ROAD, MERGE_ROAD, HIGHWAY_ROAD};
+
+enum MARKING_TYPE {UNKNOWN_MARK, TEXT_MARK, AF_MARK, AL_MARK, AR_MARK, AFL_MARK, AFR_MARK, ALR_MARK, UTURN_MARK, NOUTURN_MARK};
+
+enum TrafficSignTypes {UNKNOWN_SIGN, STOP_SIGN, MAX_SPEED_SIGN, MIN_SPEED_SIGN};
+
+class Lane;
+class TrafficLight;
+class RoadSegment;
+
+class ObjTimeStamp
+{
+public:
+  timespec tStamp;
+
+  ObjTimeStamp()
+  {
+    tStamp.tv_nsec = 0;
+    tStamp.tv_sec = 0;
+  }
+};
+
+//class POINT2D
+//{
+//public:
+//    double x;
+//    double y;
+//    double z;
+//    POINT2D()
+//    {
+//      x=0;y=0;z=0;
+//    }
+//    POINT2D(double px, double py, double pz = 0)
+//    {
+//      x = px;
+//      y = py;
+//      z = pz;
+//    }
+//};
+
+class GPSPoint
+{
+public:
+  double lat, x;
+  double lon, y;
+  double alt, z;
+  double dir, a;
+
+  GPSPoint()
+  {
+    lat = x = 0;
+    lon = y = 0;
+    alt = z = 0;
+    dir = a = 0;
+  }
+
+  GPSPoint(const double& x, const double& y, const double& z, const double& a)
+  {
+    this->x = x;
+    this->y = y;
+    this->z = z;
+    this->a = a;
+
+    lat = 0;
+    lon = 0;
+    alt = 0;
+    dir = 0;
+  }
+
+  std::string ToString()
+  {
+    std::stringstream str;
+    str.precision(12);
+    str << "X:" << x << ", Y:" << y << ", Z:" << z << ", A:" << a << std::endl;
+    str << "Lon:" << lon << ", Lat:" << lat << ", Alt:" << alt << ", Dir:" << dir << std::endl;
+    return str.str();
+  }
+};
+
+class RECTANGLE
+
+{
+public:
+  GPSPoint bottom_left;
+  GPSPoint top_right;
+  double width;
+  double length;
+  bool bObstacle;
+
+
+  inline bool PointInRect(GPSPoint p)
+  {
+    return p.x >= bottom_left.x && p.x <= top_right.x && p.y >= bottom_left.y && p.y <= top_right.y;
+  }
+
+  inline bool PointInsideRect(GPSPoint p)
+  {
+    return p.x > bottom_left.x && p.x < top_right.x && p.y > bottom_left.y && p.y < top_right.y;
+  }
+
+  inline bool HitTest(GPSPoint p)
+  {
+    return PointInRect(p) && bObstacle;
+  }
+
+  RECTANGLE()
+  {
+    width=0;
+    length = 0;
+    bObstacle = true;
+  }
+
+  virtual ~RECTANGLE(){}
+};
+
+class PolygonShape
+{
+public:
+  std::vector<GPSPoint> points;
+
+  inline int PointInsidePolygon(const PolygonShape& polygon,const GPSPoint& p)
+  {
+    int counter = 0;
+      int i;
+      double xinters;
+      GPSPoint p1,p2;
+      int N = polygon.points.size();
+      if(N <=0 ) return -1;
+
+      p1 = polygon.points.at(0);
+      for (i=1;i<=N;i++)
+      {
+        p2 = polygon.points.at(i % N);
+
+        if (p.y > MIN(p1.y,p2.y))
+        {
+          if (p.y <= MAX(p1.y,p2.y))
+          {
+            if (p.x <= MAX(p1.x,p2.x))
+            {
+              if (p1.y != p2.y)
+              {
+                xinters = (p.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x;
+                if (p1.x == p2.x || p.x <= xinters)
+                  counter++;
+              }
+            }
+          }
+        }
+        p1 = p2;
+      }
+
+      if (counter % 2 == 0)
+        return 0;
+      else
+        return 1;
+  }
+};
+
+class MapItem
+{
+public:
+  int id;
+  GPSPoint sp; //start point
+  GPSPoint ep; // end point
+  GPSPoint center;
+  double c; //curvature
+  double w; //width
+  double l; //length
+  std::string fileName; //
+  std::vector<GPSPoint> polygon;
+
+
+  MapItem(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length, std::string objName)
+  {
+    id = ID;
+    sp = start;
+    ep = end;
+    c = curvature;
+    w = width;
+    l = length;
+    fileName = objName;
+
+  }
+
+  MapItem()
+  {
+    id = 0; c = 0; w = 0; l = 0;
+  }
+
+  virtual ~MapItem(){}
+
+  MapItem(const MapItem & cmi)
+  {
+        id = cmi.id;
+        sp = cmi.sp;
+        ep = cmi.ep;
+        c = cmi.c;
+        w = cmi.w;
+        l = cmi.l;
+        fileName = cmi.fileName;
+  }
+  MapItem &operator=(const MapItem &cmi)
+  {
+    this->id = cmi.id;
+      this->sp = cmi.sp;
+      this->ep = cmi.ep;
+      this->c = cmi.c;
+      this->w = cmi.w;
+      this->l = cmi.l;
+      this->fileName = cmi.fileName;
+      return *this;
+  }
+
+  virtual int operator==(const MapItem &mi) const
+    {
+      return this->id == mi.id;
+    }
+};
+
+class Obstacle : public MapItem
+{
+  public:
+    OBSTACLE_TYPE t;
+
+    Obstacle(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length,OBSTACLE_TYPE type, std::string fileName ) : MapItem(ID, start, end, curvature, width, length, fileName)
+  {
+      t = type;
+  }
+    virtual ~Obstacle()
+    {
+    }
+
+    Obstacle() : MapItem()
+       {
+      t = SIDEWALK;
+       }
+
+    Obstacle(const Obstacle& ob) : MapItem(ob)
+      {
+        t = ob.t;
+      }
+
+    Obstacle& operator=(const Obstacle& ob)
+      {
+      this->id = ob.id;
+      this->sp = ob.sp;
+      this->ep = ob.ep;
+      this->c = ob.c;
+      this->w = ob.w;
+      this->l = ob.l;
+      this->t = ob.t;
+      this->fileName = ob.fileName;
+      return *this;
+      }
+
+      virtual int operator==(const Obstacle &ob) const
+          {
+            return this->id == ob.id && this->t == ob.t;
+          }
+};
+
+class DrivableArea : public MapItem
+{
+public:
+  DRIVABLE_TYPE t; // drivable area type
+
+  DrivableArea(int ID, GPSPoint start, GPSPoint end, double curvature, double width, double length,DRIVABLE_TYPE type, std::string fileName ) : MapItem( ID, start, end, curvature, width, length, fileName)
+  {
+    t = type;
+  }
+
+  virtual ~DrivableArea()
+  {
+
+  }
+
+  DrivableArea() : MapItem()
+    {
+      t = PARKINGAREA;
+    }
+
+  DrivableArea(const DrivableArea& da) : MapItem(da)
+  {
+    t = da.t;
+  }
+
+  DrivableArea& operator=(const DrivableArea& da)
+  {
+    this->id = da.id;
+    this->sp = da.sp;
+    this->ep = da.ep;
+    this->c = da.c;
+    this->w = da.w;
+    this->l = da.l;
+    this->t = da.t;
+    this->fileName = da.fileName;
+    return *this;
+  }
+
+  virtual int operator==(const DrivableArea &da) const
+      {
+        return this->id == da.id && this->t == da.t;
+      }
+
+};
+
+class Rotation
+{
+public:
+  double x;
+  double y;
+  double z;
+  double w;
+
+  Rotation()
+  {
+    x = 0;
+    y = 0;
+    z = 0;
+    w = 0;
+  }
+};
+
+class WayPoint
+{
+public:
+  GPSPoint  pos;
+  Rotation   rot;
+  double    v;
+  double    cost;
+  double    timeCost;
+  double    totalReward;
+  double    collisionCost;
+  double     laneChangeCost;
+  int     laneId;
+  int     id;
+  int     LeftPointId;
+  int     RightPointId;
+  int     LeftLnId;
+  int     RightLnId;
+  int     stopLineID;
+  DIRECTION_TYPE bDir;
+  STATE_TYPE  state;
+  BEH_STATE_TYPE beh_state;
+  int     iOriginalIndex;
+
+  Lane* pLane;
+  WayPoint* pLeft;
+  WayPoint* pRight;
+  std::vector<int>   toIds;
+  std::vector<int>   fromIds;
+  std::vector<WayPoint*> pFronts;
+  std::vector<WayPoint*> pBacks;
+  std::vector<std::pair<ACTION_TYPE, double> > actionCost;
+
+  int      originalMapID;
+  int      gid;
+
+  WayPoint()
+  {
+    id = 0;
+    v = 0;
+    cost = 0;
+    laneId = -1;
+    pLane  = 0;
+    pLeft = 0;
+    pRight = 0;
+    bDir = FORWARD_DIR;
+    LeftPointId = 0;
+    RightPointId = 0;
+    LeftLnId = 0;
+    RightLnId = 0;
+    timeCost = 0;
+    totalReward = 0;
+    collisionCost = 0;
+    laneChangeCost = 0;
+    stopLineID = -1;
+    state = INITIAL_STATE;
+    beh_state = BEH_STOPPING_STATE;
+    iOriginalIndex = 0;
+
+    gid = 0;
+    originalMapID = -1;
+  }
+
+  WayPoint(const double& x, const double& y, const double& z, const double& a)
+  {
+    pos.x = x;
+    pos.y = y;
+    pos.z = z;
+    pos.a = a;
+
+    id = 0;
+    v = 0;
+    cost = 0;
+    laneId = -1;
+    pLane  = 0;
+    pLeft = 0;
+    pRight = 0;
+    bDir = FORWARD_DIR;
+    LeftPointId = 0;
+    RightPointId = 0;
+    LeftLnId = 0;
+    RightLnId = 0;
+    timeCost = 0;
+    totalReward = 0;
+    collisionCost = 0;
+    laneChangeCost = 0;
+    stopLineID = -1;
+    iOriginalIndex = 0;
+    state = INITIAL_STATE;
+    beh_state = BEH_STOPPING_STATE;
+
+    gid = 0;
+    originalMapID = -1;
+  }
+};
+
+class RelativeInfo
+{
+public:
+  double perp_distance;
+  double to_front_distance; //negative
+  double from_back_distance;
+  int iFront;
+  int iBack;
+  int iGlobalPath;
+  WayPoint perp_point;
+  double angle_diff; // degrees
+  bool bBefore;
+  bool bAfter;
+  double after_angle;
+
+  RelativeInfo()
+  {
+    after_angle = 0;
+    bBefore = false;
+    bAfter = false;
+    perp_distance = 0;
+    to_front_distance = 0;
+    from_back_distance = 0;
+    iFront = 0;
+    iBack = 0;
+    iGlobalPath = 0;
+    angle_diff = 0;
+  }
+};
+
+class Boundary //represent wayarea in vector map
+{
+public:
+  int id;
+  int roadId;
+  std::vector<GPSPoint> points;
+  RoadSegment* pSegment;
+
+  Boundary()
+  {
+    id    = 0;
+    roadId =0;
+    pSegment = nullptr;
+  }
+};
+
+class Curb
+{
+public:
+  int id;
+  int laneId;
+  int roadId;
+  std::vector<GPSPoint> points;
+  Lane* pLane;
+
+  Curb()
+  {
+    id    = 0;
+    laneId =0;
+    roadId =0;
+    pLane = 0;
+  }
+};
+
+class Crossing
+{
+public:
+  int id;
+  int roadId;
+  std::vector<GPSPoint> points;
+  RoadSegment* pSegment;
+
+  Crossing()
+  {
+    id    = 0;
+    roadId =0;
+    pSegment = nullptr;
+  }
+};
+
+class StopLine
+{
+public:
+  int id;
+  int laneId;
+  int roadId;
+  int trafficLightID;
+  int stopSignID;
+  std::vector<GPSPoint> points;
+  Lane* pLane;
+  int linkID;
+
+  StopLine()
+  {
+    id    = 0;
+    laneId =0;
+    roadId =0;
+    pLane = 0;
+    trafficLightID = -1;
+    stopSignID = -1;
+    linkID = 0;
+  }
+};
+
+class WaitingLine
+{
+public:
+  int id;
+  int laneId;
+  int roadId;
+  std::vector<GPSPoint> points;
+  Lane* pLane;
+
+  WaitingLine()
+  {
+    id    = 0;
+    laneId =0;
+    roadId =0;
+    pLane = 0;
+  }
+};
+
+class TrafficSign
+{
+public:
+  int id;
+  int laneId;
+  int roadId;
+
+  GPSPoint pos;
+  TrafficSignTypes signType;
+  double value;
+  double fromValue;
+  double toValue;
+  std::string strValue;
+  timespec timeValue;
+  timespec fromTimeValue;
+  timespec toTimeValue;
+
+  Lane* pLane;
+
+  TrafficSign()
+  {
+    id        = 0;
+    laneId     = 0;
+    roadId    = 0;
+    signType    = UNKNOWN_SIGN;
+    value    = 0;
+    fromValue  = 0;
+    toValue    = 0;
+//    timeValue  = 0;
+//    fromTimeValue = 0;
+//    toTimeValue  = 0;
+    pLane     = 0;
+  }
+};
+
+enum TrafficLightState {UNKNOWN_LIGHT, RED_LIGHT, GREEN_LIGHT, YELLOW_LIGHT, LEFT_GREEN, FORWARD_GREEN, RIGHT_GREEN, FLASH_YELLOW, FLASH_RED};
+
+class TrafficLight
+{
+public:
+  int id;
+  GPSPoint pos;
+  TrafficLightState lightState;
+  double stoppingDistance;
+  std::vector<int> laneIds;
+  std::vector<Lane*> pLanes;
+  int linkID;
+
+  TrafficLight()
+  {
+    stoppingDistance = 2;
+    id       = 0;
+    lightState  = GREEN_LIGHT;
+    linkID     = 0;
+  }
+
+  bool CheckLane(const int& laneId)
+  {
+    for(unsigned int i=0; i < laneIds.size(); i++)
+    {
+      if(laneId == laneIds.at(i))
+        return true;
+    }
+    return false;
+  }
+};
+
+class Marking
+{
+public:
+  int id;
+  int laneId;
+  int roadId;
+  MARKING_TYPE  mark_type;
+  GPSPoint center;
+  std::vector<GPSPoint> points;
+  Lane* pLane;
+
+  Marking()
+  {
+    id = 0;
+    laneId = 0;
+    roadId = 0;
+    mark_type = UNKNOWN_MARK;
+    pLane = nullptr;
+  }
+};
+
+class RoadSegment
+{
+public:
+  int id;
+
+  SEGMENT_TYPE   roadType;
+  Boundary  boundary;
+  Crossing  start_crossing;
+  Crossing  finish_crossing;
+  double     avgWidth;
+  std::vector<int> fromIds;
+  std::vector<int> toIds;
+  std::vector<Lane> Lanes;
+
+
+  std::vector<RoadSegment*> fromLanes;
+  std::vector<RoadSegment*> toLanes;
+
+  RoadSegment()
+  {
+    id = 0;
+    avgWidth = 0;
+    roadType = NORMAL_ROAD_SEG;
+  }
+
+
+};
+
+enum LaneType{NORMAL_LANE, MERGE_LANE, EXIT_LANE, BUS_LANE, BUS_STOP_LANE, EMERGENCY_LANE};
+
+class Lane
+{
+public:
+  int id;
+  int roadId;
+  int areaId;
+  int fromAreaId;
+  int toAreaId;
+  std::vector<int> fromIds;
+  std::vector<int> toIds;
+  int num; //lane number in the road segment from left to right
+  double speed;
+  double length;
+  double dir;
+  LaneType type;
+  double width;
+  std::vector<WayPoint> points;
+  std::vector<TrafficLight> trafficlights;
+  std::vector<StopLine> stopLines;
+  WaitingLine waitingLine;
+
+  std::vector<Lane*> fromLanes;
+  std::vector<Lane*> toLanes;
+  Lane* pLeftLane;
+  Lane* pRightLane;
+
+  RoadSegment * pRoad;
+
+  Lane()
+  {
+    id     = 0;
+    num    = 0;
+    speed   = 0;
+    length   = 0;
+    dir    = 0;
+    type   = NORMAL_LANE;
+    width   = 0;
+    pLeftLane = 0;
+    pRightLane = 0;
+    pRoad  = 0;
+    roadId = 0;
+    areaId = 0;
+    fromAreaId = 0;
+    toAreaId = 0;
+  }
+
+};
+
+class RoadNetwork
+{
+public:
+  std::vector<RoadSegment> roadSegments;
+  std::vector<TrafficLight> trafficLights;
+  std::vector<StopLine> stopLines;
+  std::vector<Curb> curbs;
+  std::vector<Boundary> boundaries;
+  std::vector<Crossing> crossings;
+  std::vector<Marking> markings;
+  std::vector<TrafficSign> signs;
+};
+
+class VehicleState : public ObjTimeStamp
+{
+public:
+  double speed;
+  double steer;
+  SHIFT_POS shift;
+
+  VehicleState()
+  {
+    speed = 0;
+    steer = 0;
+    shift = SHIFT_POS_NN;
+  }
+
+};
+
+class BehaviorState
+{
+public:
+  STATE_TYPE state;
+  double maxVelocity;
+  double minVelocity;
+  double stopDistance;
+  double followVelocity;
+  double followDistance;
+  LIGHT_INDICATOR indicator;
+  bool bNewPlan;
+  int iTrajectory;
+
+
+  BehaviorState()
+  {
+    state = INITIAL_STATE;
+    maxVelocity = 0;
+    minVelocity = 0;
+    stopDistance = 0;
+    followVelocity = 0;
+    followDistance = 0;
+    indicator  = INDICATOR_NONE;
+    bNewPlan = false;
+    iTrajectory = -1;
+
+  }
+
+};
+
+class DetectedObject
+{
+public:
+  int id;
+  std::string label;
+  OBSTACLE_TYPE t;
+  WayPoint center;
+  WayPoint predicted_center;
+  WayPoint noisy_center;
+  STATE_TYPE predicted_behavior;
+  std::vector<WayPoint> centers_list;
+  std::vector<GPSPoint> contour;
+  std::vector<std::vector<WayPoint> > predTrajectories;
+  std::vector<WayPoint*> pClosestWaypoints;
+  double w;
+  double l;
+  double h;
+  double distance_to_center;
+
+  double actual_speed;
+  double actual_yaw;
+
+  bool bDirection;
+  bool bVelocity;
+  int acceleration;
+
+  int acceleration_desc;
+  double acceleration_raw;
+
+  LIGHT_INDICATOR indicator_state;
+
+  int originalID;
+  BEH_STATE_TYPE behavior_state;
+
+  DetectedObject()
+  {
+    bDirection = false;
+    bVelocity = false;
+    acceleration = 0;
+    acceleration_desc = 0;
+    acceleration_raw = 0.0;
+    id = 0;
+    w = 0;
+    l = 0;
+    h = 0;
+    t = GENERAL_OBSTACLE;
+    distance_to_center = 0;
+    predicted_behavior = INITIAL_STATE;
+    actual_speed = 0;
+    actual_yaw = 0;
+
+    acceleration_desc = 0;
+    acceleration_raw = 0.0;
+    indicator_state = INDICATOR_NONE;
+
+    originalID = -1;
+    behavior_state = BEH_STOPPING_STATE;
+  }
+
+};
+
+class PlanningParams
+{
+public:
+  double   maxSpeed;
+  double   minSpeed;
+  double   planningDistance;
+  double   microPlanDistance;
+  double   carTipMargin;
+  double   rollInMargin;
+  double   rollInSpeedFactor;
+  double   pathDensity;
+  double   rollOutDensity;
+  int   rollOutNumber;
+  double   horizonDistance;
+  double   minFollowingDistance; //should be bigger than Distance to follow
+  double   minDistanceToAvoid; // should be smaller than minFollowingDistance and larger than maxDistanceToAvoid
+  double  maxDistanceToAvoid; // should be smaller than minDistanceToAvoid
+  double   speedProfileFactor;
+  double   smoothingDataWeight;
+  double   smoothingSmoothWeight;
+  double   smoothingToleranceError;
+
+  double stopSignStopTime;
+
+  double additionalBrakingDistance;
+  double verticalSafetyDistance;
+  double horizontalSafetyDistancel;
+
+  double giveUpDistance;
+
+  int nReliableCount;
+
+  bool   enableLaneChange;
+  bool   enableSwerving;
+  bool   enableFollowing;
+  bool   enableHeadingSmoothing;
+  bool   enableTrafficLightBehavior;
+  bool   enableStopSignBehavior;
+
+  bool   enabTrajectoryVelocities;
+  double minIndicationDistance;
+
+  PlanningParams()
+  {
+    maxSpeed             = 3;
+    minSpeed             = 0;
+    planningDistance         = 10000;
+    microPlanDistance         = 30;
+    carTipMargin          = 4.0;
+    rollInMargin          = 12.0;
+    rollInSpeedFactor        = 0.25;
+    pathDensity            = 0.25;
+    rollOutDensity          = 0.5;
+    rollOutNumber          = 4;
+    horizonDistance          = 120;
+    minFollowingDistance      = 35;
+    minDistanceToAvoid        = 15;
+    maxDistanceToAvoid        = 5;
+    speedProfileFactor        = 1.0;
+    smoothingDataWeight        = 0.47;
+    smoothingSmoothWeight      = 0.2;
+    smoothingToleranceError      = 0.05;
+
+    stopSignStopTime         = 2.0;
+
+    additionalBrakingDistance    = 10.0;
+    verticalSafetyDistance       = 0.0;
+    horizontalSafetyDistancel    = 0.0;
+
+    giveUpDistance          = -4;
+    nReliableCount          = 2;
+
+    enableHeadingSmoothing      = false;
+    enableSwerving           = false;
+    enableFollowing          = false;
+    enableTrafficLightBehavior    = false;
+    enableLaneChange         = false;
+    enableStopSignBehavior      = false;
+    enabTrajectoryVelocities     = false;
+    minIndicationDistance      = 15;
+  }
+};
+
+class HMIPreCalculatedConditions
+{
+public:
+
+  HMIPreCalculatedConditions()
+  {
+
+  }
+};
+
+class PreCalculatedConditions
+{
+public:
+  //-------------------------------------------//
+  //Global Goals
+  int         currentGoalID;
+  int         prevGoalID;
+  //-------------------------------------------//
+  //Following
+  double         distanceToNext;
+  double        velocityOfNext;
+  //-------------------------------------------//
+  //For Lane Change
+  int         iPrevSafeLane;
+  int         iCurrSafeLane;
+  double        distanceToGoBack;
+  double         timeToGoBack;
+  double         distanceToChangeLane;
+  double        timeToChangeLane;
+  int         currentLaneID;
+  int         originalLaneID;
+  int         targetLaneID;
+  bool         bUpcomingLeft;
+  bool         bUpcomingRight;
+  bool        bCanChangeLane;
+  bool        bTargetLaneSafe;
+  //-------------------------------------------//
+  //Traffic Lights & Stop Sign
+  int         currentStopSignID;
+  int         prevStopSignID;
+  int         currentTrafficLightID;
+  int         prevTrafficLightID;
+  bool         bTrafficIsRed; //On , off status
+  //-------------------------------------------//
+  //Swerving
+  int         iPrevSafeTrajectory;
+  int         iCurrSafeTrajectory;
+  int         iCentralTrajectory;
+  bool        bFullyBlock;
+  LIGHT_INDICATOR   indicator;
+
+  //-------------------------------------------//
+  //General
+  bool         bNewGlobalPath;
+  bool         bRePlan;
+  double         currentVelocity;
+  double        minStoppingDistance; //comfortably
+  int         bOutsideControl; // 0 waiting, 1 start, 2 Green Traffic Light, 3 Red Traffic Light, 5 Emergency Stop
+  bool        bGreenOutsideControl;
+  std::vector<double> stoppingDistances;
+
+
+  double distanceToStop()
+  {
+    if(stoppingDistances.size()==0) return 0;
+    double minS = stoppingDistances.at(0);
+    for(unsigned int i=0; i< stoppingDistances.size(); i++)
+    {
+      if(stoppingDistances.at(i) < minS)
+        minS = stoppingDistances.at(i);
+    }
+    return minS;
+  }
+
+  PreCalculatedConditions()
+  {
+    currentGoalID       = 0;
+    prevGoalID        = -1;
+    currentVelocity     = 0;
+    minStoppingDistance    = 1;
+    bOutsideControl      = 0;
+    bGreenOutsideControl  = false;
+    //distance to stop
+    distanceToNext      = -1;
+    velocityOfNext      = 0;
+    currentStopSignID    = -1;
+    prevStopSignID      = -1;
+    currentTrafficLightID  = -1;
+    prevTrafficLightID    = -1;
+    bTrafficIsRed      = false;
+    iCurrSafeTrajectory    = -1;
+    bFullyBlock        = false;
+
+    iPrevSafeTrajectory    = -1;
+    iCentralTrajectory    = -1;
+    bRePlan          = false;
+    bNewGlobalPath      = false;
+
+    bCanChangeLane      = false;
+    distanceToGoBack    = 0;
+    timeToGoBack      = 0;
+    distanceToChangeLane  = 0;
+    timeToChangeLane    = 0;
+    bTargetLaneSafe      = true;
+    bUpcomingLeft      = false;
+    bUpcomingRight      = false;
+    targetLaneID      = -1;
+    currentLaneID      = -1;
+    originalLaneID      = -1;
+    iCurrSafeLane       = -1;
+    iPrevSafeLane      = -1;
+
+    indicator         = INDICATOR_NONE;
+  }
+
+  virtual ~PreCalculatedConditions(){}
+
+  std::string ToStringHeader()
+  {
+    return "Time:General>>:currentVelocity:distanceToStop:minStoppingDistance:bStartBehaviorGenerator:bGoalReached:"
+        "Following>>:velocityOfNext:distanceToNext:"
+        "TrafficLight>>:currentTrafficLightID:bTrafficIsRed:"
+        "Swerving>>:iSafeTrajectory:bFullyBlock:";
+  }
+
+  std::string ToString(STATE_TYPE beh)
+  {
+    std::string str = "Unknown";
+    switch(beh)
+    {
+    case PlannerHNS::INITIAL_STATE:
+      str = "Init";
+      break;
+    case PlannerHNS::WAITING_STATE:
+      str = "Waiting";
+      break;
+    case PlannerHNS::FORWARD_STATE:
+      str = "Forward";
+      break;
+    case PlannerHNS::STOPPING_STATE:
+      str = "Stop";
+      break;
+    case PlannerHNS::FINISH_STATE:
+      str = "End";
+      break;
+    case PlannerHNS::FOLLOW_STATE:
+      str = "Follow";
+      break;
+    case PlannerHNS::OBSTACLE_AVOIDANCE_STATE:
+      str = "Swerving";
+      break;
+    case PlannerHNS::TRAFFIC_LIGHT_STOP_STATE:
+      str = "Light Stop";
+      break;
+    case PlannerHNS::TRAFFIC_LIGHT_WAIT_STATE:
+      str = "Light Wait";
+      break;
+    case PlannerHNS::STOP_SIGN_STOP_STATE:
+      str = "Sign Stop";
+      break;
+    case PlannerHNS::STOP_SIGN_WAIT_STATE:
+      str = "Sign Wait";
+      break;
+    default:
+      str = "Unknown";
+      break;
+    }
+
+    return str;
+  }
+};
+
+class TrajectoryCost
+{
+public:
+  int index;
+  int relative_index;
+  double closest_obj_velocity;
+  double distance_from_center;
+  double priority_cost; //0 to 1
+  double transition_cost; // 0 to 1
+  double closest_obj_cost; // 0 to 1
+  double cost;
+  double closest_obj_distance;
+
+  int lane_index;
+  double lane_change_cost;
+  double lateral_cost;
+  double longitudinal_cost;
+  bool bBlocked;
+  std::vector<std::pair<int, double> > lateral_costs;
+
+
+  TrajectoryCost()
+  {
+    lane_index = -1;
+    index = -1;
+    relative_index = -100;
+    closest_obj_velocity = 0;
+    priority_cost = 0;
+    transition_cost = 0;
+    closest_obj_cost = 0;
+    distance_from_center = 0;
+    cost = 0;
+    closest_obj_distance = -1;
+    lane_change_cost = 0;
+    lateral_cost = 0;
+    longitudinal_cost = 0;
+    bBlocked = false;
+  }
+
+  std::string ToString()
+  {
+    std::ostringstream str;
+    str.precision(4);
+    str << "LI   : " << lane_index;
+    str << ", In : " << relative_index;
+    str << ", Co : " << cost;
+    str << ", Pr : " << priority_cost;
+    str << ", Tr : " << transition_cost;
+    str << ", La : " << lateral_cost;
+    str << ", Lo : " << longitudinal_cost;
+    str << ", Ln : " << lane_change_cost;
+    str << ", Bl : " << bBlocked;
+    str << "\n";
+    for (unsigned int i=0; i<lateral_costs.size(); i++ )
+    {
+      str << " - (" << lateral_costs.at(i).first << ", " << lateral_costs.at(i).second << ")";
+    }
+
+    return str.str();
+
+  }
+};
+
+class OccupancyToGridMap
+{
+public:
+  int width;
+  int length;
+  double res;
+  WayPoint center;
+
+  OccupancyToGridMap(const int& _width, const int& _length, const double& _res, const WayPoint& _center)
+  {
+    width = _width;
+    length = _length;
+    res = _res;
+    center = _center;
+  }
+
+  OccupancyToGridMap()
+  {
+    width = 0;
+    length  = 0;
+    res = 0.0;
+  }
+
+  bool GetCellIndexFromPoint(const GPSPoint& p, const std::vector<int>& data, int& _cell)
+  {
+    int col = floor(p.x / res);
+    int row = floor(p.y / res);
+
+    int index = -1;
+    if(row >= 0 && row < length && col >=0 && col < width)
+    {
+      index = get2dIndex(row,col);
+
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        //printf("Cell Info: P(%f,%f) , D(%f,%f), G(%d,%d), index = %d \n", p.x, p.y, p.x-center.pos.x, p.y-center.pos.y, col, row , index);
+        return true;
+      }
+    }
+
+    if(row+1 >= 0 && row+1 < length && col >=0 && col < width)
+    {
+      index = get2dIndex(row+1,col);
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        return true;
+      }
+    }
+
+    if(row >= 0 && row < length && col+1 >=0 && col+1 < width)
+    {
+      index = get2dIndex(row,col+1);
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        return true;
+      }
+    }
+
+    if(row-1 >= 0 && row-1 < length && col >=0 && col < width)
+    {
+      index = get2dIndex(row-1,col);
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        return true;
+      }
+    }
+
+    if(row >= 0 && row < length && col-1 >=0 && col-1 < width)
+    {
+      index = get2dIndex(row,col-1);
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        return true;
+      }
+    }
+
+    if(row+1 >= 0 && row+1 < length && col+1 >=0 && col+1 < width)
+    {
+      index = get2dIndex(row+1,col+1);
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        return true;
+      }
+    }
+
+    if(row-1 >= 0 && row-1 < length && col-1 >=0 && col-1 < width)
+    {
+      index = get2dIndex(row-1,col-1);
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        return true;
+      }
+    }
+
+    if(row-1 >= 0 && row-1 < length && col+1 >=0 && col+1 < width)
+    {
+      index = get2dIndex(row-1,col+1);
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        return true;
+      }
+    }
+
+    if(row+1 >= 0 && row+1 < length && col-1 >=0 && col-1 < width)
+    {
+      index = get2dIndex(row+1,col-1);
+      if(index >= 0 && index < (int)data.size())
+      {
+        _cell = data.at((unsigned int)index);
+        return true;
+      }
+    }
+
+    //printf("Error Getting Cell with Info: P(%f,%f) , C(%d,%d), index = %d \n", p.x, p.y, row, col, index);
+    return false;
+  }
+private:
+
+  int get2dIndex(const int& r,const int& c)
+  {
+    return ((r*width) + c);
+  }
+
+};
+
+class ParticleInfo
+{
+public:
+  double vel;
+  int acl; //slow down -1 braking , 0 cruising , 1 accelerating
+  PlannerHNS::LIGHT_INDICATOR indicator;
+  PlannerHNS::STATE_TYPE state;
+
+  ParticleInfo()
+  {
+    vel = 0;
+    acl = 0;
+    indicator = PlannerHNS::INDICATOR_NONE;
+    state = PlannerHNS::FORWARD_STATE;
+  }
+};
+
+}
+
+
+#endif /* ROADNETWORK_H_ */
+

src1/planning/op_planner/include/op_planner/SimuDecisionMaker.h

@@ -0,0 +1,57 @@
+/// \file  SimuDecisionMaker.h
+/// \brief Decision Maker for Simulated Vehicles
+/// \author Hatem Darweesh
+/// \date Jan 10, 2018
+
+
+#ifndef SIMUDECISIONMAKER_H_
+#define SIMUDECISIONMAKER_H_
+
+#include "DecisionMaker.h"
+#include "TrajectoryDynamicCosts.h"
+
+namespace PlannerHNS
+{
+
+
+class SimuDecisionMaker: public PlannerHNS::DecisionMaker
+{
+public:
+  SimuDecisionMaker();
+  virtual ~SimuDecisionMaker();
+
+  //For Simulation
+  std::vector<PlannerHNS::GPSPoint> m_CarShapePolygon;
+  PlannerHNS::WayPoint m_OdometryState;
+  double m_CurrentVelocity, m_CurrentVelocityD; //meter/second
+  double m_CurrentSteering, m_CurrentSteeringD; //radians
+  PlannerHNS::SHIFT_POS m_CurrentShift , m_CurrentShiftD;
+
+  double m_CurrentAccSteerAngle; //degrees steer wheel range
+  double m_CurrentAccVelocity; // kilometer/hour
+  double m_SimulationSteeringDelayFactor; //second , time that every degree change in the steering wheel takes
+  timespec m_SteerDelayTimer;
+  PlannerHNS::TrajectoryDynamicCosts m_TrajectoryCostsCalculator;
+
+  void ReInitializePlanner(const WayPoint& start_pose);
+  void InitPolygons();
+  void SetSimulatedTargetOdometryReadings(const double& velocity_d, const double& steering_d, const SHIFT_POS& shift_d);
+  void FirstLocalizeMe(const PlannerHNS::WayPoint& initCarPos);
+  void LocalizeMe(const double& dt); // in seconds
+  void UpdateState(const PlannerHNS::VehicleState& state, const bool& bUseDelay = false);
+  void GenerateLocalRollOuts();
+
+  bool SelectSafeTrajectory();
+  PlannerHNS::VehicleState LocalizeStep(const double& dt, const PlannerHNS::VehicleState& desiredStatus);
+
+  PlannerHNS::BehaviorState DoOneStep(const double& dt,
+      const PlannerHNS::VehicleState& vehicleState,
+      const int& goalID,
+      const std::vector<TrafficLight>& trafficLight,
+      const std::vector<PlannerHNS::DetectedObject>& objects,
+      const bool& bEmergencyStop);
+};
+
+}
+
+#endif /* SIMUDECISIONMAKER_H_ */

src1/planning/op_planner/include/op_planner/TrajectoryCosts.h

@@ -0,0 +1,56 @@
+
+/// \file TrajectoryCosts.h
+/// \brief Calculate collision costs for roll out trajectory for free trajectory evaluation for dp_planner
+/// \author Hatem Darweesh
+/// \date Dec 14, 2016
+
+
+#ifndef TRAJECTORYCOSTS_H_
+#define TRAJECTORYCOSTS_H_
+
+#include "RoadNetwork.h"
+#include "PlannerCommonDef.h"
+#include "PlanningHelpers.h"
+
+using namespace std;
+
+namespace PlannerHNS
+{
+
+class TrajectoryCosts
+{
+public:
+  TrajectoryCosts();
+  virtual ~TrajectoryCosts();
+
+  TrajectoryCost DoOneStep(const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths,
+      const WayPoint& currState, const int& currTrajectoryIndex, const int& currLaneIndex, const PlanningParams& params,
+      const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState, const std::vector<PlannerHNS::DetectedObject>& obj_list);
+
+public:
+  int m_PrevCostIndex;
+  vector<TrajectoryCost> m_TrajectoryCosts;
+  PlanningParams m_Params;
+  PolygonShape m_SafetyBorder;
+  vector<WayPoint> m_AllContourPoints;
+  vector<WayPoint> m_CollisionPoints;
+  double m_WeightPriority;
+  double m_WeightTransition;
+  double m_WeightLong;
+  double m_WeightLat;
+  double m_WeightLaneChange;
+  double m_LateralSkipDistance;
+
+
+
+private:
+  bool ValidateRollOutsInput(const vector<vector<vector<WayPoint> > >& rollOuts);
+  vector<TrajectoryCost> CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint> >& laneRollOuts, const int& lane_index, const PlanningParams& params);
+  void NormalizeCosts(vector<TrajectoryCost>& trajectoryCosts);
+  void CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost>& trajectoryCosts, const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths, const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState);
+  void CalculateTransitionCosts(vector<TrajectoryCost>& trajectoryCosts, const int& currTrajectoryIndex, const PlanningParams& params);
+};
+
+}
+
+#endif /* TRAJECTORYCOSTS_H_ */

src1/planning/op_planner/include/op_planner/TrajectoryDynamicCosts.h

@@ -0,0 +1,76 @@
+
+/// \file TrajectoryDynamicCosts.h
+/// \brief Calculate collision costs for roll out trajectory for free trajectory evaluation for OpenPlanner local planner version 1.5+
+/// \author Hatem Darweesh
+/// \date Jan 14, 2018
+
+
+#ifndef TRAJECTORYDYNAMICCOSTS_H_
+#define TRAJECTORYDYNAMICCOSTS_H_
+
+#include "RoadNetwork.h"
+#include "PlannerCommonDef.h"
+#include "PlanningHelpers.h"
+
+using namespace std;
+
+namespace PlannerHNS
+{
+
+class TrajectoryDynamicCosts
+{
+public:
+  TrajectoryDynamicCosts();
+  virtual ~TrajectoryDynamicCosts();
+
+  TrajectoryCost DoOneStep(const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths,
+      const WayPoint& currState, const int& currTrajectoryIndex, const int& currLaneIndex, const PlanningParams& params,
+      const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState, const std::vector<PlannerHNS::DetectedObject>& obj_list);
+
+  TrajectoryCost DoOneStepStatic(const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
+      const WayPoint& currState, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
+      const std::vector<PlannerHNS::DetectedObject>& obj_list, const int& iCurrentIndex = -1);
+
+  TrajectoryCost DoOneStepDynamic(const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
+      const WayPoint& currState, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState,
+      const std::vector<PlannerHNS::DetectedObject>& obj_list, const int& iCurrentIndex = -1);
+
+public:
+  int m_PrevCostIndex;
+  int m_PrevIndex;
+  vector<TrajectoryCost> m_TrajectoryCosts;
+  PlanningParams m_Params;
+  PolygonShape m_SafetyBorder;
+  vector<WayPoint> m_AllContourPoints;
+  vector<WayPoint> m_CollisionPoints;
+  double m_WeightPriority;
+  double m_WeightTransition;
+  double m_WeightLong;
+  double m_WeightLat;
+  double m_WeightLaneChange;
+  double m_LateralSkipDistance;
+  double m_CollisionTimeDiff;
+
+
+
+private:
+  bool ValidateRollOutsInput(const vector<vector<vector<WayPoint> > >& rollOuts);
+  vector<TrajectoryCost> CalculatePriorityAndLaneChangeCosts(const vector<vector<WayPoint> >& laneRollOuts, const int& lane_index, const PlanningParams& params);
+  void NormalizeCosts(vector<TrajectoryCost>& trajectoryCosts);
+  void CalculateLateralAndLongitudinalCosts(vector<TrajectoryCost>& trajectoryCosts, const vector<vector<vector<WayPoint> > >& rollOuts, const vector<vector<WayPoint> >& totalPaths, const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState);
+  void CalculateLateralAndLongitudinalCostsStatic(vector<TrajectoryCost>& trajectoryCosts, const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths, const WayPoint& currState, const vector<WayPoint>& contourPoints, const PlanningParams& params, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState);
+  void CalculateTransitionCosts(vector<TrajectoryCost>& trajectoryCosts, const int& currTrajectoryIndex, const PlanningParams& params);
+  
+  void CalculateIntersectionVelocities(const std::vector<WayPoint>& path, const DetectedObject& obj, const WayPoint& currPose, const CAR_BASIC_INFO& carInfo, const double& c_lateral_d, WayPoint& collisionPoint, TrajectoryCost& trajectoryCosts);
+  int GetCurrentRollOutIndex(const std::vector<WayPoint>& path, const WayPoint& currState, const PlanningParams& params);
+  void InitializeCosts(const vector<vector<WayPoint> >& rollOuts, const PlanningParams& params);
+  void InitializeSafetyPolygon(const WayPoint& currState, const CAR_BASIC_INFO& carInfo, const VehicleState& vehicleState, const double& c_lateral_d, const double& c_long_front_d, const double& c_long_back_d);
+  void CalculateLateralAndLongitudinalCostsDynamic(const std::vector<PlannerHNS::DetectedObject>& obj_list, const vector<vector<WayPoint> >& rollOuts, const vector<WayPoint>& totalPaths,
+      const WayPoint& currState, const PlanningParams& params, const CAR_BASIC_INFO& carInfo,
+      const VehicleState& vehicleState, const double& c_lateral_d, const double& c_long_front_d, const double& c_long_back_d );
+
+};
+
+}
+
+#endif /* TRAJECTORYDYNAMICCOSTS_H_ */

src1/planning/op_planner/op_planner.pro

@@ -0,0 +1,49 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2020-03-05T21:39:06
+#
+#-------------------------------------------------
+
+QT       -= gui
+
+
+TARGET = op_planner
+TEMPLATE = lib
+
+
+CONFIG += plugin
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    BehaviorPrediction.cpp \
+    BehaviorStateMachine.cpp \
+    DecisionMaker.cpp \
+    LocalPlannerH.cpp \
+    MappingHelpers.cpp \
+    MatrixOperations.cpp \
+    PassiveDecisionMaker.cpp \
+    PlannerH.cpp \
+    PlanningHelpers.cpp \
+    SimuDecisionMaker.cpp \
+    TrajectoryCosts.cpp \
+    TrajectoryDynamicCosts.cpp
+        
+HEADERS +=
+        
+
+INCLUDEPATH += /opt/ros/melodic/include
+INCLUDEPATH += $$PWD/../include
+INCLUDEPATH += $$PWD/include
+
+INCLUDEPATH += $$PWD/../op_utility/include
+

src1/planning/op_utility/DataRW.cpp

@@ -0,0 +1,2075 @@
+
+/// \file DataRW.cpp
+/// \brief File operations for loading vector map files, loading kml map files and writing log .csv files
+/// \author Hatem Darweesh
+/// \date Jun 23, 2016
+
+#include "op_utility/DataRW.h"
+#include <stdlib.h>
+#include <tinyxml.h>
+#include <sys/stat.h>
+#include "op_utility/UtilityH.h"
+
+
+using namespace std;
+
+namespace UtilityHNS
+{
+
+std::string DataRW::LoggingMainfolderName   = "/autoware_openplanner_logs/";
+std::string DataRW::ControlLogFolderName   = "ControlLogs/";
+std::string DataRW::GlobalPathLogFolderName = "GlobalPathLogs/";
+std::string DataRW::PathLogFolderName     = "TrajectoriesLogs/";
+std::string DataRW::StatesLogFolderName   = "BehaviorsLogs/";
+std::string DataRW::SimulationFolderName   = "SimulationData/";
+std::string DataRW::KmlMapsFolderName     = "KmlMaps/";
+std::string DataRW::PredictionFolderName   = "PredictionResults/";
+std::string DataRW::TrackingFolderName     = "TrackingLogs/";
+
+
+DataRW::DataRW()
+{
+}
+
+DataRW::~DataRW()
+{
+}
+
+void DataRW::CreateLoggingFolder()
+{
+  std::string main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName;
+  int dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+  if (-1 == dir_err)
+      cout << "Can't Create OpenPlanner Log Path!n" << endl;
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + DataRW::ControlLogFolderName;
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + DataRW::GlobalPathLogFolderName;
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + DataRW::PathLogFolderName;
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + DataRW::StatesLogFolderName;
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + DataRW::SimulationFolderName;
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + DataRW::PredictionFolderName;
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + DataRW::TrackingFolderName;
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + "SimulatedCar1";
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + "SimulatedCar2";
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + "SimulatedCar3";
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + "SimulatedCar4";
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+
+  main_folder = UtilityH::GetHomeDirectory() + DataRW::LoggingMainfolderName + "SimulatedCar5";
+  dir_err = mkdir(main_folder.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+}
+
+void DataRW::WriteLogData(const std::string& logFolder, const std::string& logTitle, const std::string& header, const std::vector<std::string>& logData)
+{
+  if(logData.size() < 2)
+    return;
+
+  ostringstream fileName;
+  fileName << logFolder;
+  fileName << logTitle;
+  fileName << UtilityH::GetFilePrefixHourMinuteSeconds();
+  fileName << ".csv";
+
+  std::ofstream f(fileName.str().c_str());
+
+  if(f.is_open())
+  {
+    if(header.size() > 0)
+      f << header << "\r\n";
+    for(unsigned int i = 0 ; i < logData.size(); i++)
+      f << logData.at(i) << "\r\n";
+  }
+
+  f.close();
+}
+
+void DataRW::WriteKMLFile(const string& fileName, const vector<string>& gps_list)
+{
+  TiXmlDocument kmldoc(UtilityH::GetHomeDirectory()+DataRW::KmlMapsFolderName + "KmlTemplate.kml");
+
+  bool bkmlFileLoaded =  kmldoc.LoadFile();
+
+  assert(bkmlFileLoaded== true);
+
+  TiXmlElement* pElem = kmldoc.FirstChildElement();
+
+  if(!pElem)
+  {
+    printf("\n Empty KML File !");
+    return;
+  }
+
+  TiXmlElement* pV=0;
+  TiXmlHandle hKmlFile(pElem);
+
+  //pV = hKmlFile.FirstChild("Folder").FirstChild("Folder").FirstChild("Document").FirstChild("Placemark").FirstChild("LineString").FirstChild("coordinates").Element();
+  pV = hKmlFile.FirstChild("Folder").FirstChild("Document").FirstChild("Placemark").FirstChild("LineString").FirstChild("coordinates").Element();
+  if(!pV)
+    pV = hKmlFile.FirstChild( "Placemark" ).FirstChild("LineString").FirstChild("coordinates").Element();
+
+  if(pV)
+  {
+      ostringstream val;
+      val.precision(18);
+
+      for(unsigned int i =0; i < gps_list.size(); i++)
+      {
+        val << gps_list[i] <<  " ";
+      }
+
+      TiXmlText * text = new TiXmlText( val.str() );
+      pV->LinkEndChild(text);
+  }
+
+  kmldoc.SaveFile(fileName);
+}
+
+void DataRW::WriteKMLFile(const string& fileName, const vector<vector<string> >& gps_list)
+  {
+    TiXmlDocument kmldoc(UtilityH::GetHomeDirectory()+DataRW::KmlMapsFolderName + "KmlTemplate.kml");
+
+      bool bkmlFileLoaded =  kmldoc.LoadFile();
+
+      assert(bkmlFileLoaded== true);
+
+      TiXmlElement* pElem = kmldoc.FirstChildElement();
+
+      if(!pElem)
+      {
+        printf("\n Empty KML File !");
+        return;
+      }
+
+      TiXmlNode* pV=0;
+      TiXmlNode* pPlaceMarkNode=0;
+      TiXmlElement* pDocument=0;
+      TiXmlHandle hKmlFile(pElem);
+
+      //pV = hKmlFile.FirstChild("Folder").FirstChild("Folder").FirstChild("Document").FirstChild("Placemark").FirstChild("LineString").FirstChild("coordinates").Element();
+
+      pDocument = hKmlFile.FirstChild("Folder").FirstChild("Document").Element();
+      pPlaceMarkNode = hKmlFile.FirstChild("Folder").FirstChild("Document").FirstChild("Placemark").Node();
+
+      if(!pDocument)
+      {
+        pDocument = hKmlFile.Element();
+        pPlaceMarkNode = hKmlFile.FirstChild( "Placemark" ).Node();
+      }
+
+
+
+//      pV = hKmlFile.FirstChild("Folder").FirstChild("Document").FirstChild("Placemark").FirstChild("LineString").FirstChild("coordinates").Element();
+//      if(!pV)
+//        pV = hKmlFile.FirstChild( "Placemark" ).FirstChild("LineString").FirstChild("coordinates").Element();
+
+
+      if(pDocument)
+      {
+        for(unsigned int l = 0; l < gps_list.size(); l++)
+        {
+
+          pV = pPlaceMarkNode->Clone();
+          TiXmlElement* pElement = pV->FirstChild("LineString")->FirstChild("coordinates")->ToElement();
+
+          ostringstream val;
+        val.precision(18);
+
+        for(unsigned int i =0; i < gps_list[l].size(); i++)
+        {
+          val << gps_list[l][i] <<  " ";
+        }
+
+        TiXmlText * text = new TiXmlText( val.str() );
+        pElement->LinkEndChild(text);
+
+        pDocument->InsertEndChild(*pV);
+
+        }
+
+      }
+
+      kmldoc.SaveFile(fileName);
+  }
+
+SimpleReaderBase::SimpleReaderBase(const string& fileName, const int& nHeaders,const char& separator,
+      const int& iDataTitles, const int& nVariablesForOneObject ,
+      const int& nLineHeaders, const string& headerRepeatKey)
+{
+  if(fileName.compare("d") != 0)
+  {
+    m_pFile = new ifstream(fileName.c_str(), ios::in);
+    if(!m_pFile->is_open())
+    {
+      printf("\n Can't Open Map File !, %s", fileName.c_str());
+      return;
+    }
+
+  m_nHeders = nHeaders;
+  m_iDataTitles = iDataTitles;
+  m_nVarPerObj = nVariablesForOneObject;
+  m_HeaderRepeatKey = headerRepeatKey;
+  m_nLineHeaders = nLineHeaders;
+  m_Separator = separator;
+  m_pFile->precision(16);
+
+  ReadHeaders();
+  }
+}
+
+SimpleReaderBase::~SimpleReaderBase()
+{
+  if(m_pFile->is_open())
+    m_pFile->close();
+}
+
+bool SimpleReaderBase::ReadSingleLine(vector<vector<string> >& line)
+{
+  if(!m_pFile->is_open() || m_pFile->eof()) return false;
+
+  string strLine, innerToken;
+  line.clear();
+  getline(*m_pFile, strLine);
+  istringstream str_stream(strLine);
+
+  vector<string> header;
+  vector<string> obj_part;
+
+  if(m_nVarPerObj == 0)
+  {
+    while(getline(str_stream, innerToken, m_Separator))
+    {
+      obj_part.push_back(innerToken);
+    }
+
+    line.push_back(obj_part);
+    return true;
+  }
+  else
+  {
+    int iCounter = 0;
+    while(iCounter < m_nLineHeaders && getline(str_stream, innerToken, m_Separator))
+    {
+      header.push_back(innerToken);
+      iCounter++;
+    }
+    obj_part.insert(obj_part.begin(), header.begin(), header.end());
+
+    iCounter = 1;
+
+    while(getline(str_stream, innerToken, m_Separator))
+    {
+      obj_part.push_back(innerToken);
+      if(iCounter == m_nVarPerObj)
+      {
+        line.push_back(obj_part);
+        obj_part.clear();
+
+        iCounter = 0;
+        obj_part.insert(obj_part.begin(), header.begin(), header.end());
+
+      }
+      iCounter++;
+    }
+  }
+
+  return true;
+}
+
+int SimpleReaderBase::ReadAllData()
+{
+  if(!m_pFile->is_open()) return 0;
+
+  m_AllData.clear();
+  vector<vector<string> > singleLine;
+  while(!m_pFile->eof())
+  {
+    ReadSingleLine(singleLine);
+    m_AllData.push_back(singleLine);
+  }
+
+  return m_AllData.size();
+}
+
+void SimpleReaderBase::ReadHeaders()
+{
+  if(!m_pFile->is_open()) return;
+
+  string strLine;
+  int iCounter = 0;
+  m_RawHeaders.clear();
+  while(!m_pFile->eof() && iCounter < m_nHeders)
+  {
+    getline(*m_pFile, strLine);
+    m_RawHeaders.push_back(strLine);
+    if(iCounter == m_iDataTitles)
+      ParseDataTitles(strLine);
+    iCounter++;
+  }
+}
+
+void SimpleReaderBase::ParseDataTitles(const string& header)
+{
+  if(header.size()==0) return;
+
+  string innerToken;
+  istringstream str_stream(header);
+  m_DataTitlesHeader.clear();
+  while(getline(str_stream, innerToken, m_Separator))
+  {
+    if(innerToken.compare(m_HeaderRepeatKey)!=0)
+      m_DataTitlesHeader.push_back(innerToken);
+  }
+}
+
+bool GPSDataReader::ReadNextLine(GPSBasicData& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 5) return false;
+
+    data.lat = strtod(lineData.at(0).at(2).c_str(), NULL);
+    data.lon = strtod(lineData.at(0).at(3).c_str(), NULL);
+    data.alt = strtod(lineData.at(0).at(4).c_str(), NULL);
+    data.distance = strtod(lineData.at(0).at(5).c_str(), NULL);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int GPSDataReader::ReadAllData(vector<GPSBasicData>& data_list)
+{
+  data_list.clear();
+  GPSBasicData data;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+bool SimulationFileReader::ReadNextLine(SimulationPoint& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 6) return false;
+
+    data.x = strtod(lineData.at(0).at(0).c_str(), NULL);
+    data.y = strtod(lineData.at(0).at(1).c_str(), NULL);
+    data.z = strtod(lineData.at(0).at(2).c_str(), NULL);
+    data.a = strtod(lineData.at(0).at(3).c_str(), NULL);
+    data.c = strtod(lineData.at(0).at(4).c_str(), NULL);
+    data.v = strtod(lineData.at(0).at(5).c_str(), NULL);
+    data.name = lineData.at(0).at(6);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int SimulationFileReader::ReadAllData(SimulationData& data_list)
+{
+  data_list.simuCars.clear();
+  SimulationPoint data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    if(count == 0)
+      data_list.startPoint = data;
+    else if(count == 1)
+      data_list.goalPoint = data;
+    else
+      data_list.simuCars.push_back(data);
+
+    count++;
+  }
+
+  return count;
+}
+
+bool LocalizationPathReader::ReadNextLine(LocalizationWayPoint& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 5) return false;
+
+    //data.t = strtod(lineData.at(0).at(0).c_str(), NULL);
+    data.x = strtod(lineData.at(0).at(0).c_str(), NULL);
+    data.y = strtod(lineData.at(0).at(1).c_str(), NULL);
+    data.z = strtod(lineData.at(0).at(2).c_str(), NULL);
+    data.a = strtod(lineData.at(0).at(3).c_str(), NULL);
+    data.v = strtod(lineData.at(0).at(4).c_str(), NULL);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int LocalizationPathReader::ReadAllData(vector<LocalizationWayPoint>& data_list)
+{
+  data_list.clear();
+  LocalizationWayPoint data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//Nodes
+
+AisanNodesFileReader::AisanNodesFileReader(const vector_map_msgs::NodeArray& _nodes) : SimpleReaderBase("d", 1)
+{
+  if(_nodes.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  //TODO Fix PID and NID problem
+
+  m_data_list.clear();
+  AisanNode data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _nodes.data.size(); i++)
+  {
+    ParseNextLine(_nodes.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.NID < m_min_id)
+      m_min_id = data.NID;
+
+    if(data.NID > max_id)
+      max_id = data.NID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).NID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanNodesFileReader::ParseNextLine(const vector_map_msgs::Node& _rec, AisanNode& data)
+{
+  data.NID = _rec.nid;
+  data.PID = _rec.pid;
+}
+
+AisanNodesFileReader::AisanNode* AisanNodesFileReader::GetDataRowById(int _nid)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _nid-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).NID == _nid)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanNodesFileReader::ReadNextLine(AisanNode& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 2) return false;
+
+    data.NID = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.PID = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanNodesFileReader::ReadAllData(vector<AisanNode>& data_list)
+{
+  m_data_list.clear();
+  AisanNode data;
+  //double logTime = 0;
+  int max_id = std::numeric_limits<int>::min();
+  while(ReadNextLine(data))
+  {
+    m_data_list.push_back(data);
+    if(data.NID < m_min_id)
+      m_min_id = data.NID;
+
+    if(data.NID > max_id)
+      max_id = data.NID;
+  }
+
+  m_data_map.resize(max_id - m_min_id + 2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).NID-m_min_id) = &m_data_list.at(i);
+  }
+
+  data_list = m_data_list;
+  return m_data_list.size();
+}
+
+//Points
+
+AisanPointsFileReader::AisanPointsFileReader(const vector_map_msgs::PointArray& _points) : SimpleReaderBase("d", 1)
+{
+  if(_points.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanPoints data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _points.data.size(); i++)
+  {
+    ParseNextLine(_points.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.PID < m_min_id)
+      m_min_id = data.PID;
+
+    if(data.PID > max_id)
+      max_id = data.PID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).PID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanPointsFileReader::ParseNextLine(const vector_map_msgs::Point& _rec, AisanPoints& data)
+{
+  data.B = _rec.b;
+  data.Bx = _rec.bx;
+  data.H = _rec.h;
+  data.L = _rec.l;
+  data.Ly = _rec.ly;
+  data.MCODE1 = _rec.mcode1;
+  data.MCODE2 = _rec.mcode2;
+  data.MCODE3 = _rec.mcode3;
+  data.PID = _rec.pid;
+  data.Ref = _rec.ref;
+}
+
+AisanPointsFileReader::AisanPoints* AisanPointsFileReader::GetDataRowById(int _pid)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _pid-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).PID == _pid)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanPointsFileReader::ReadNextLine(AisanPoints& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 10) return false;
+
+    data.PID = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.B = strtod(lineData.at(0)[1].c_str(), NULL);
+    data.L = strtod(lineData.at(0)[2].c_str(), NULL);
+    data.H = strtod(lineData.at(0)[3].c_str(), NULL);
+
+    data.Bx = strtod(lineData.at(0)[4].c_str(), NULL);
+    data.Ly = strtod(lineData.at(0)[5].c_str(), NULL);
+    data.Ref = strtol(lineData.at(0).at(6).c_str(), NULL, 10);
+    data.MCODE1 = strtol(lineData.at(0).at(7).c_str(), NULL, 10);
+    data.MCODE2 = strtol(lineData.at(0).at(8).c_str(), NULL, 10);
+    data.MCODE3 = strtol(lineData.at(0).at(9).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanPointsFileReader::ReadAllData(vector<AisanPoints>& data_list)
+{
+  m_data_list.clear();
+  AisanPoints data;
+  //double logTime = 0;
+  int max_id = std::numeric_limits<int>::min();
+  while(ReadNextLine(data))
+  {
+    m_data_list.push_back(data);
+    if(data.PID < m_min_id)
+      m_min_id = data.PID;
+
+    if(data.PID > max_id)
+      max_id = data.PID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).PID-m_min_id) = &m_data_list.at(i);
+  }
+
+  data_list = m_data_list;
+  return m_data_list.size();
+}
+
+// Lines
+
+AisanLinesFileReader::AisanLinesFileReader(const vector_map_msgs::LineArray& _nodes) : SimpleReaderBase("d", 1)
+{
+  if(_nodes.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanLine data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _nodes.data.size(); i++)
+  {
+    ParseNextLine(_nodes.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.LID < m_min_id)
+      m_min_id = data.LID;
+
+    if(data.LID > max_id)
+      max_id = data.LID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).LID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanLinesFileReader::ParseNextLine(const vector_map_msgs::Line& _rec, AisanLine& data)
+{
+  data.BLID = _rec.blid;
+  data.BPID = _rec.bpid;
+  data.FLID = _rec.flid;
+  data.FPID = _rec.fpid;
+  data.LID = _rec.lid;
+}
+
+AisanLinesFileReader::AisanLine* AisanLinesFileReader::GetDataRowById(int _lid)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _lid-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).LID == _lid)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanLinesFileReader::ReadNextLine(AisanLine& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 5) return false;
+
+    data.LID = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.BPID = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.FPID = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+    data.BLID = strtol(lineData.at(0).at(3).c_str(), NULL, 10);
+    data.FLID = strtol(lineData.at(0).at(4).c_str(), NULL, 10);
+
+    return true;
+  }
+  else
+    return false;
+}
+
+int AisanLinesFileReader::ReadAllData(vector<AisanLine>& data_list)
+{
+  data_list.clear();
+  AisanLine data;
+  //double logTime = 0;
+
+  int max_id = std::numeric_limits<int>::min();
+  while(ReadNextLine(data))
+  {
+    m_data_list.push_back(data);
+    if(data.LID < m_min_id)
+      m_min_id = data.LID;
+
+    if(data.LID > max_id)
+      max_id = data.LID;
+  }
+
+  m_data_map.resize(max_id - m_min_id + 2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).LID-m_min_id) = &m_data_list.at(i);
+  }
+
+  data_list = m_data_list;
+  return m_data_list.size();
+}
+
+//dt Lanes (center lines)
+
+AisanCenterLinesFileReader::AisanCenterLinesFileReader(const vector_map_msgs::DTLaneArray& _Lines) : SimpleReaderBase("d", 1)
+{
+  if(_Lines.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanCenterLine data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _Lines.data.size(); i++)
+  {
+    ParseNextLine(_Lines.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.DID < m_min_id)
+      m_min_id = data.DID;
+
+    if(data.DID > max_id)
+      max_id = data.DID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).DID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanCenterLinesFileReader::ParseNextLine(const vector_map_msgs::DTLane& _rec, AisanCenterLine& data)
+{
+  data.Apara = _rec.apara;
+  data.DID = _rec.did;
+  data.Dir = _rec.dir;
+  data.Dist = _rec.dist;
+  data.LW = _rec.lw;
+  data.PID = _rec.pid;
+  data.RW = _rec.rw;
+  data.cant = _rec.cant;
+  data.r = _rec.r;
+  data.slope = _rec.slope;
+}
+
+AisanCenterLinesFileReader::AisanCenterLine* AisanCenterLinesFileReader::GetDataRowById(int _did)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _did-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).DID == _did)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanCenterLinesFileReader::ReadNextLine(AisanCenterLine& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 10) return false;
+
+    data.DID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.Dist   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.PID   = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+
+    data.Dir   = strtod(lineData.at(0)[3].c_str(), NULL);
+    data.Apara   = strtod(lineData.at(0)[4].c_str(), NULL);
+    data.r     = strtod(lineData.at(0)[5].c_str(), NULL);
+    data.slope   = strtod(lineData.at(0)[6].c_str(), NULL);
+    data.cant   = strtod(lineData.at(0)[7].c_str(), NULL);
+    data.LW   = strtod(lineData.at(0)[8].c_str(), NULL);
+    data.RW   = strtod(lineData.at(0)[9].c_str(), NULL);
+
+    return true;
+  }
+  else
+    return false;
+}
+
+int AisanCenterLinesFileReader::ReadAllData(vector<AisanCenterLine>& data_list)
+{
+  data_list.clear();
+  AisanCenterLine data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//Lane
+
+AisanLanesFileReader::AisanLanesFileReader(const vector_map_msgs::LaneArray& _lanes) : SimpleReaderBase("d", 1)
+{
+  if(_lanes.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanLane data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _lanes.data.size(); i++)
+  {
+    ParseNextLine(_lanes.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.LnID < m_min_id)
+      m_min_id = data.LnID;
+
+    if(data.LnID > max_id)
+      max_id = data.LnID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).LnID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanLanesFileReader::ParseNextLine(const vector_map_msgs::Lane& _rec, AisanLane& data)
+{
+  data.BLID = _rec.blid;
+  data.BLID2 = _rec.blid2;
+  data.BLID3 = _rec.blid3;
+  data.BLID4 = _rec.blid4;
+  data.BNID = _rec.bnid;
+  data.ClossID = _rec.clossid;
+  data.DID = _rec.did;
+  data.FLID = _rec.flid;
+  data.FLID2 = _rec.flid2;
+  data.FLID3 = _rec.flid3;
+  data.FLID4 = _rec.flid4;
+
+  data.FNID = _rec.fnid;
+  data.JCT = _rec.jct;
+  data.LCnt = _rec.lcnt;
+  data.LaneChgFG = _rec.lanecfgfg;
+  //data.LaneDir = _rec.;
+  data.LaneType = _rec.lanetype;
+  //data.LeftLaneId = _rec.;
+  data.LimitVel = _rec.limitvel;
+  data.LinkWAID = _rec.linkwaid;
+  data.LnID = _rec.lnid;
+  data.Lno = _rec.lno;
+  data.RefVel = _rec.refvel;
+  //data.RightLaneId = _rec.;
+  data.RoadSecID = _rec.roadsecid;
+  data.Span = _rec.span;
+  //data.originalMapID = _rec.;
+
+}
+
+AisanLanesFileReader::AisanLane* AisanLanesFileReader::GetDataRowById(int _lnid)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _lnid-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).LnID == _lnid)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanLanesFileReader::ReadNextLine(AisanLane& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size() == 0) return false;
+    if(lineData.at(0).size() < 17) return false;
+
+    data.LnID    = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.DID    = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.BLID    = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+    data.FLID    = strtol(lineData.at(0).at(3).c_str(), NULL, 10);
+    data.BNID     = strtol(lineData.at(0).at(4).c_str(), NULL, 10);
+    data.FNID    = strtol(lineData.at(0).at(5).c_str(), NULL, 10);
+    data.JCT    = strtol(lineData.at(0).at(6).c_str(), NULL, 10);
+    data.BLID2     = strtol(lineData.at(0).at(7).c_str(), NULL, 10);
+    data.BLID3    = strtol(lineData.at(0).at(8).c_str(), NULL, 10);
+    data.BLID4    = strtol(lineData.at(0).at(9).c_str(), NULL, 10);
+    data.FLID2     = strtol(lineData.at(0).at(10).c_str(), NULL, 10);
+    data.FLID3    = strtol(lineData.at(0).at(11).c_str(), NULL, 10);
+    data.FLID4    = strtol(lineData.at(0).at(12).c_str(), NULL, 10);
+    data.ClossID   = strtol(lineData.at(0).at(13).c_str(), NULL, 10);
+    data.Span     = strtod(lineData.at(0).at(14).c_str(), NULL);
+    data.LCnt     = strtol(lineData.at(0).at(15).c_str(), NULL, 10);
+    data.Lno      = strtol(lineData.at(0).at(16).c_str(), NULL, 10);
+
+
+    if(lineData.at(0).size() < 23) return true;
+
+    data.LaneType  = strtol(lineData.at(0).at(17).c_str(), NULL, 10);
+    data.LimitVel  = strtol(lineData.at(0).at(18).c_str(), NULL, 10);
+    data.RefVel     = strtol(lineData.at(0).at(19).c_str(), NULL, 10);
+    data.RoadSecID  = strtol(lineData.at(0).at(20).c_str(), NULL, 10);
+    data.LaneChgFG   = strtol(lineData.at(0).at(21).c_str(), NULL, 10);
+    data.LinkWAID  = strtol(lineData.at(0).at(22).c_str(), NULL, 10);
+
+
+    if(lineData.at(0).size() > 23)
+    {
+      string str_dir = lineData.at(0).at(23);
+      if(str_dir.size() > 0)
+        data.LaneDir   = str_dir.at(0);
+      else
+        data.LaneDir    = 'F';
+    }
+
+//    data.LeftLaneId  = 0;
+//    data.RightLaneId = 0;
+//    data.LeftLaneId   = strtol(lineData.at(0).at(24).c_str(), NULL, 10);
+//    data.RightLaneId   = strtol(lineData.at(0).at(25).c_str(), NULL, 10);
+
+
+    return true;
+  }
+  else
+    return false;
+}
+
+int AisanLanesFileReader::ReadAllData(vector<AisanLane>& data_list)
+{
+  data_list.clear();
+  AisanLane data;
+  //double logTime = 0;
+  int max_id = std::numeric_limits<int>::min();
+
+  while(ReadNextLine(data))
+  {
+    m_data_list.push_back(data);
+    if(data.LnID < m_min_id)
+      m_min_id = data.LnID;
+
+    if(data.LnID > max_id)
+      max_id = data.LnID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).LnID-m_min_id) = &m_data_list.at(i);
+  }
+
+  data_list = m_data_list;
+
+  return m_data_list.size();
+}
+
+//Area
+
+AisanAreasFileReader::AisanAreasFileReader(const vector_map_msgs::AreaArray& _areas) : SimpleReaderBase("d", 1)
+{
+  if(_areas.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanArea data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _areas.data.size(); i++)
+  {
+    ParseNextLine(_areas.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.AID < m_min_id)
+      m_min_id = data.AID;
+
+    if(data.AID > max_id)
+      max_id = data.AID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).AID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanAreasFileReader::ParseNextLine(const vector_map_msgs::Area& _rec, AisanArea& data)
+{
+  data.AID = _rec.aid;
+  data.ELID = _rec.elid;
+  data.SLID = _rec.slid;
+}
+
+AisanAreasFileReader::AisanArea* AisanAreasFileReader::GetDataRowById(int _aid)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _aid-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).AID == _aid)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanAreasFileReader::ReadNextLine(AisanArea& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 3) return false;
+
+    data.AID = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.SLID = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.ELID = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanAreasFileReader::ReadAllData(vector<AisanArea>& data_list)
+{
+  data_list.clear();
+  AisanArea data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//Intersection
+
+AisanIntersectionFileReader::AisanIntersectionFileReader(const vector_map_msgs::CrossRoadArray& _inters) : SimpleReaderBase("d", 1)
+{
+  if(_inters.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanIntersection data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _inters.data.size(); i++)
+  {
+    ParseNextLine(_inters.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.ID < m_min_id)
+      m_min_id = data.ID;
+
+    if(data.ID > max_id)
+      max_id = data.ID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).ID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanIntersectionFileReader::ParseNextLine(const vector_map_msgs::CrossRoad& _rec, AisanIntersection& data)
+{
+  data.AID = _rec.aid;
+  data.ID = _rec.id;
+  data.LinkID = _rec.linkid;
+}
+
+AisanIntersectionFileReader::AisanIntersection* AisanIntersectionFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).ID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanIntersectionFileReader::ReadNextLine(AisanIntersection& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 3) return false;
+
+    data.ID = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.AID = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.LinkID = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanIntersectionFileReader::ReadAllData(vector<AisanIntersection>& data_list)
+{
+  data_list.clear();
+  AisanIntersection data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//StopLine
+
+AisanStopLineFileReader::AisanStopLineFileReader(const vector_map_msgs::StopLineArray& _stopLines) : SimpleReaderBase("d", 1)
+{
+  if(_stopLines.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanStopLine data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _stopLines.data.size(); i++)
+  {
+    ParseNextLine(_stopLines.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.ID < m_min_id)
+      m_min_id = data.ID;
+
+    if(data.ID > max_id)
+      max_id = data.ID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).ID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanStopLineFileReader::ParseNextLine(const vector_map_msgs::StopLine& _rec, AisanStopLine& data)
+{
+  data.ID = _rec.id;
+  data.LID = _rec.lid;
+  data.LinkID = _rec.linkid;
+  data.SignID = _rec.signid;
+  data.TLID = _rec.tlid;
+}
+
+AisanStopLineFileReader::AisanStopLine* AisanStopLineFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).ID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanStopLineFileReader::ReadNextLine(AisanStopLine& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 5) return false;
+
+    data.ID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.LID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.TLID   = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+    data.SignID = strtol(lineData.at(0).at(3).c_str(), NULL, 10);
+    data.LinkID = strtol(lineData.at(0).at(4).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanStopLineFileReader::ReadAllData(vector<AisanStopLine>& data_list)
+{
+  data_list.clear();
+  AisanStopLine data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//RoadSign
+
+AisanRoadSignFileReader::AisanRoadSignFileReader(const vector_map_msgs::RoadSignArray& _signs) : SimpleReaderBase("d", 1)
+{
+  if(_signs.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanRoadSign data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _signs.data.size(); i++)
+  {
+    ParseNextLine(_signs.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.ID < m_min_id)
+      m_min_id = data.ID;
+
+    if(data.ID > max_id)
+      max_id = data.ID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).ID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanRoadSignFileReader::ParseNextLine(const vector_map_msgs::RoadSign& _rec, AisanRoadSign& data)
+{
+  data.ID = _rec.id;
+  data.LinkID = _rec.linkid;
+  data.PLID = _rec.plid;
+  data.Type = _rec.type;
+  data.VID = _rec.vid;
+}
+
+AisanRoadSignFileReader::AisanRoadSign* AisanRoadSignFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).ID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanRoadSignFileReader::ReadNextLine(AisanRoadSign& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 5) return false;
+
+    data.ID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.VID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.PLID   = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+    data.Type   = strtol(lineData.at(0).at(3).c_str(), NULL, 10);
+    data.LinkID = strtol(lineData.at(0).at(4).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanRoadSignFileReader::ReadAllData(vector<AisanRoadSign>& data_list)
+{
+  data_list.clear();
+  AisanRoadSign data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//Signal
+
+AisanSignalFileReader::AisanSignalFileReader(const vector_map_msgs::SignalArray& _signal) : SimpleReaderBase("d", 1)
+{
+  if(_signal.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanSignal data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _signal.data.size(); i++)
+  {
+    ParseNextLine(_signal.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.ID < m_min_id)
+      m_min_id = data.ID;
+
+    if(data.ID > max_id)
+      max_id = data.ID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).ID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanSignalFileReader::ParseNextLine(const vector_map_msgs::Signal& _rec, AisanSignal& data)
+{
+  data.ID = _rec.id;
+  data.LinkID = _rec.linkid;
+  data.PLID = _rec.plid;
+  data.Type = _rec.type;
+  data.VID = _rec.vid;
+}
+
+AisanSignalFileReader::AisanSignal* AisanSignalFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).ID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanSignalFileReader::ReadNextLine(AisanSignal& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 5) return false;
+
+    data.ID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.VID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.PLID   = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+    data.Type   = strtol(lineData.at(0).at(3).c_str(), NULL, 10);
+    data.LinkID = strtol(lineData.at(0).at(4).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanSignalFileReader::ReadAllData(vector<AisanSignal>& data_list)
+{
+  data_list.clear();
+  AisanSignal data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+
+  return count;
+}
+
+//Vector
+
+AisanVectorFileReader::AisanVectorFileReader(const vector_map_msgs::VectorArray& _vectors) : SimpleReaderBase("d", 1)
+{
+  if(_vectors.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanVector data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _vectors.data.size(); i++)
+  {
+    ParseNextLine(_vectors.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.VID < m_min_id)
+      m_min_id = data.VID;
+
+    if(data.VID > max_id)
+      max_id = data.VID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).VID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanVectorFileReader::ParseNextLine(const vector_map_msgs::Vector& _rec, AisanVector& data)
+{
+  data.Hang = _rec.hang;
+  data.PID = _rec.pid;
+  data.VID = _rec.vid;
+  data.Vang = _rec.vang;
+}
+
+AisanVectorFileReader::AisanVector* AisanVectorFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).VID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanVectorFileReader::ReadNextLine(AisanVector& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 4) return false;
+
+    data.VID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.PID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.Hang   = strtod(lineData.at(0).at(2).c_str(), NULL);
+    data.Vang   = strtod(lineData.at(0).at(3).c_str(), NULL);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanVectorFileReader::ReadAllData(vector<AisanVector>& data_list)
+{
+  data_list.clear();
+  AisanVector data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//Curb
+
+AisanCurbFileReader::AisanCurbFileReader(const vector_map_msgs::CurbArray& _curbs) : SimpleReaderBase("d", 1)
+{
+  if(_curbs.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanCurb data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _curbs.data.size(); i++)
+  {
+    ParseNextLine(_curbs.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.ID < m_min_id)
+      m_min_id = data.ID;
+
+    if(data.ID > max_id)
+      max_id = data.ID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).ID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanCurbFileReader::ParseNextLine(const vector_map_msgs::Curb& _rec, AisanCurb& data)
+{
+  data.Height = _rec.height;
+  data.ID = _rec.id;
+  data.LID = _rec.lid;
+  data.LinkID = _rec.linkid;
+  data.Width = _rec.width;
+  data.dir = _rec.dir;
+}
+
+AisanCurbFileReader::AisanCurb* AisanCurbFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).ID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanCurbFileReader::ReadNextLine(AisanCurb& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 6) return false;
+
+    data.ID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.LID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.Height = strtod(lineData.at(0).at(2).c_str(), NULL);
+    data.Width   = strtod(lineData.at(0).at(3).c_str(), NULL);
+    data.dir   = strtol(lineData.at(0).at(4).c_str(), NULL, 10);
+    data.LinkID = strtol(lineData.at(0).at(5).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanCurbFileReader::ReadAllData(vector<AisanCurb>& data_list)
+{
+  data_list.clear();
+  AisanCurb data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+// RoadEdge
+
+AisanRoadEdgeFileReader::AisanRoadEdgeFileReader(const vector_map_msgs::RoadEdgeArray& _edges) : SimpleReaderBase("d", 1)
+{
+  if(_edges.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanRoadEdge data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _edges.data.size(); i++)
+  {
+    ParseNextLine(_edges.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.ID < m_min_id)
+      m_min_id = data.ID;
+
+    if(data.ID > max_id)
+      max_id = data.ID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).ID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanRoadEdgeFileReader::ParseNextLine(const vector_map_msgs::RoadEdge& _rec, AisanRoadEdge& data)
+{
+  data.ID = _rec.id;
+  data.LID = _rec.lid;
+  data.LinkID = _rec.linkid;
+}
+
+AisanRoadEdgeFileReader::AisanRoadEdge* AisanRoadEdgeFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).ID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanRoadEdgeFileReader::ReadNextLine(AisanRoadEdge& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 3) return false;
+
+    data.ID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.LID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.LinkID = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanRoadEdgeFileReader::ReadAllData(vector<AisanRoadEdge>& data_list)
+{
+  data_list.clear();
+  AisanRoadEdge data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//CrossWalk
+
+AisanCrossWalkFileReader::AisanCrossWalkFileReader(const vector_map_msgs::CrossWalkArray& _crossWalks) : SimpleReaderBase("d", 1)
+{
+  if(_crossWalks.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanCrossWalk data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _crossWalks.data.size(); i++)
+  {
+    ParseNextLine(_crossWalks.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.ID < m_min_id)
+      m_min_id = data.ID;
+
+    if(data.ID > max_id)
+      max_id = data.ID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).ID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanCrossWalkFileReader::ParseNextLine(const vector_map_msgs::CrossWalk& _rec, AisanCrossWalk& data)
+{
+  data.AID = _rec.aid;
+  data.BdID = _rec.bdid;
+  data.ID = _rec.id;
+  data.LinkID = _rec.linkid;
+  data.Type = _rec.type;
+}
+
+AisanCrossWalkFileReader::AisanCrossWalk* AisanCrossWalkFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).ID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanCrossWalkFileReader::ReadNextLine(AisanCrossWalk& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 5) return false;
+
+    data.ID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.AID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.Type   = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+    data.BdID   = strtol(lineData.at(0).at(3).c_str(), NULL, 10);
+    data.LinkID = strtol(lineData.at(0).at(4).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanCrossWalkFileReader::ReadAllData(vector<AisanCrossWalk>& data_list)
+{
+  data_list.clear();
+  AisanCrossWalk data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//WayArea
+
+AisanWayareaFileReader::AisanWayareaFileReader(const vector_map_msgs::WayAreaArray& _wayAreas) : SimpleReaderBase("d", 1)
+{
+  if(_wayAreas.data.size()==0) return;
+
+  m_min_id = std::numeric_limits<int>::max();
+
+  m_data_list.clear();
+  AisanWayarea data;
+  int max_id = std::numeric_limits<int>::min();
+
+  for(unsigned int i=0; i < _wayAreas.data.size(); i++)
+  {
+    ParseNextLine(_wayAreas.data.at(i), data);
+
+    m_data_list.push_back(data);
+    if(data.ID < m_min_id)
+      m_min_id = data.ID;
+
+    if(data.ID > max_id)
+      max_id = data.ID;
+  }
+
+  m_data_map.resize(max_id-m_min_id+2);
+  for(unsigned int i=0; i < m_data_list.size(); i++)
+  {
+    m_data_map.at(m_data_list.at(i).ID-m_min_id) = &m_data_list.at(i);
+  }
+}
+
+void AisanWayareaFileReader::ParseNextLine(const vector_map_msgs::WayArea& _rec, AisanWayarea& data)
+{
+  data.AID = _rec.aid;
+  data.ID = _rec.waid;
+  //data.LinkID = _rec.;
+}
+
+AisanWayareaFileReader::AisanWayarea* AisanWayareaFileReader::GetDataRowById(int _id)
+{
+  if(m_data_map.size()==0) return nullptr;
+
+  int index = _id-m_min_id;
+  if(index >= 0 && index < m_data_map.size())
+  {
+    return m_data_map.at(index);
+  }
+  else
+  {
+    for(unsigned int i=0; i < m_data_list.size(); i++)
+    {
+      if(m_data_list.at(i).ID == _id)
+      {
+        return &m_data_list.at(i);
+      }
+    }
+  }
+
+  return nullptr;
+}
+
+bool AisanWayareaFileReader::ReadNextLine(AisanWayarea& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 3) return false;
+
+    data.ID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.AID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.LinkID = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanWayareaFileReader::ReadAllData(vector<AisanWayarea>& data_list)
+{
+  data_list.clear();
+  AisanWayarea data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+//Data Conn
+bool AisanDataConnFileReader::ReadNextLine(DataConn& data)
+{
+  vector<vector<string> > lineData;
+  if(ReadSingleLine(lineData))
+  {
+    if(lineData.size()==0) return false;
+    if(lineData.at(0).size() < 4) return false;
+
+    data.LID   = strtol(lineData.at(0).at(0).c_str(), NULL, 10);
+    data.SLID   = strtol(lineData.at(0).at(1).c_str(), NULL, 10);
+    data.SID   = strtol(lineData.at(0).at(2).c_str(), NULL, 10);
+    data.SSID   = strtol(lineData.at(0).at(3).c_str(), NULL, 10);
+
+    return true;
+
+  }
+  else
+    return false;
+}
+
+int AisanDataConnFileReader::ReadAllData(vector<DataConn>& data_list)
+{
+  data_list.clear();
+  DataConn data;
+  //double logTime = 0;
+  int count = 0;
+  while(ReadNextLine(data))
+  {
+    data_list.push_back(data);
+    count++;
+  }
+  return count;
+}
+
+} /* namespace UtilityHNS */

src1/planning/op_utility/Readme.md

@@ -0,0 +1 @@
+include vector_map_msgs copy from ros.

src1/planning/op_utility/UtilityH.cpp

@@ -0,0 +1,461 @@
+/// \file UtilityH.cpp
+/// \brief General Math and Control utility functions
+/// \author Hatem Darweesh
+/// \date May 14, 2016
+
+#include "op_utility/UtilityH.h"
+#include <iostream>
+#include <sstream>
+#include <string.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <pwd.h>
+
+
+using namespace std;
+
+
+namespace UtilityHNS
+{
+
+
+UtilityH::UtilityH()
+{
+}
+
+ UtilityH::~UtilityH()
+{
+}
+
+ std::string UtilityH::GetHomeDirectory()
+ {
+  struct passwd *pw = getpwuid(getuid());
+  const char *homedir = pw->pw_dir;
+  return string(homedir);
+ }
+
+ double UtilityH::GetMomentumScaleFactor(const double& v)
+ {
+   if(v < 0.3)
+     return 0.6;
+   else if(v <6.4)
+     return 0.3;
+   else if(v < 20)
+   {
+     double m = 0.7/3.6;
+     return m*(v - 6.4) + 0.3;
+   }
+   else
+     return 0.9;
+ }
+
+ int UtilityH::GetSign(double x)
+ {
+   if(x < 0 )
+     return -1;
+   else
+     return 1;
+ }
+
+ double UtilityH::FixNegativeAngle(const double& a)
+{
+  double angle = 0;
+  if (a < -2.0 * M_PI || a >= 2.0 * M_PI) {
+    angle = fmod(a, 2.0 * M_PI);
+  } else {
+    angle = a;
+  }
+
+  if(angle < 0) {
+    angle = 2.0 * M_PI + angle;
+  }
+
+  return angle;
+}
+
+ double UtilityH::SplitPositiveAngle(const double& a)
+{
+  double angle = a;
+
+  if (a < -2.0 * M_PI || a >= 2.0 * M_PI) {
+    angle = fmod(a, 2.0 * M_PI);
+  }
+
+  if (angle >= M_PI) {
+    angle -= 2.0 * M_PI;
+  } else if (angle < -M_PI) {
+    angle += 2.0 * M_PI;
+  }
+  
+  return angle;
+}
+
+double UtilityH::InverseAngle(const double& a)
+{
+
+   double angle = 0;
+   if(a < M_PI) {
+     angle =  a + M_PI;
+   } else {
+     angle = a - M_PI;
+   }
+   
+   return angle;
+}
+
+double UtilityH::AngleBetweenTwoAnglesPositive(const double& a1, const double& a2)
+{
+   double diff = a1 - a2;
+   if(diff < 0)
+     diff = a2 - a1;
+
+   if(diff > M_PI)
+     diff = 2.0*M_PI - diff;
+
+   return diff;
+}
+
+double UtilityH::GetCircularAngle(const double& prevContAngle, const double& prevAngle, const double& currAngle)
+{
+
+  double diff = currAngle - prevAngle;
+  if(diff > M_PI)
+    diff = diff - 2.0*M_PI;
+  if(diff < -M_PI)
+    diff = diff + 2.0*M_PI;
+
+  double c_ang = 0;
+  if(prevContAngle == 0 || fabs(diff) < M_PI_2)
+     c_ang = prevContAngle + diff;
+  else
+    c_ang = prevContAngle;
+
+  return c_ang;
+}
+
+void UtilityH::GetTickCount(struct timespec& t)
+{
+  while(clock_gettime(0, & t) == -1);
+}
+
+double UtilityH::GetTimeDiff(const struct timespec& old_t,const struct timespec& curr_t)
+{
+  return (curr_t.tv_sec - old_t.tv_sec) + ((double)(curr_t.tv_nsec - old_t.tv_nsec)/ 1000000000.0);
+}
+
+double UtilityH::GetTimeDiffNow(const struct timespec& old_t)
+{
+  struct timespec curr_t;
+  GetTickCount(curr_t);
+  return (curr_t.tv_sec - old_t.tv_sec) + ((double)(curr_t.tv_nsec - old_t.tv_nsec)/ 1000000000.0);
+}
+
+string UtilityH::GetFilePrefixHourMinuteSeconds()
+{
+  struct timespec now_time;
+  UtilityH::GetTickCount(now_time);
+  tm *gmtm = localtime(&now_time.tv_sec);
+  ostringstream str;
+
+  str << "Y" << gmtm->tm_year;
+  str << "-";
+  str << "M" << gmtm->tm_mon;
+  str << "-";
+  str << "D" << gmtm->tm_mday;
+  str << "-";
+  str << "H" << gmtm->tm_hour;
+  str << "-";
+  str << "M" << gmtm->tm_min;
+  str << "-";
+  str << "S" << gmtm->tm_sec;
+
+  return str.str();
+}
+
+string UtilityH::GetDateTimeStr()
+{
+  time_t now = time(0);
+  char* dateStr = ctime(&now);
+  string str(dateStr, strlen(dateStr)-1);
+  int index = str.find(" ");
+  while(index > 0)
+  {
+    str.replace(index,1, "_");
+    index = str.find(" ");
+  }
+
+  index = str.find(":");
+  while(index > 0)
+  {
+    str.replace(index,1, "-");
+    index = str.find(":");
+  }
+  return str;
+}
+
+int  UtilityH::tsCompare (struct  timespec  time1,   struct  timespec  time2, int micro_tolerance)
+{
+  long nanoDiff =
+    (time1.tv_sec * 1000000000 + time1.tv_nsec) - 
+    (time2.tv_sec * 1000000000 + time2.tv_nsec);
+  
+  if (nanoDiff < -micro_tolerance) {
+    return -1;  // time1 is less than time2
+  } else if (nanoDiff > micro_tolerance) {
+    return 1;  // time1 is greater than time2
+  } else {
+    return 0;  // time1 is equal to time2
+  }
+}
+
+timespec UtilityH::GetTimeSpec(const time_t& srcT)
+{
+  timespec dstT;
+  dstT.tv_sec = srcT/1000000000;
+  dstT.tv_nsec = srcT - (dstT.tv_sec*1000000000);
+  return dstT;
+}
+
+time_t UtilityH::GetLongTime(const struct timespec& srcT)
+{
+  time_t dstT;
+  dstT = srcT.tv_sec * 1000000000 + srcT.tv_nsec;
+  return dstT;
+}
+
+PIDController::PIDController()
+{
+  kp = kp_v = 0;
+  ki = ki_v = 0;
+  kd = kd_v = 0;
+  pid_lim = pid_v = 0;
+  upper_limit = lower_limit = 0;
+  bEnableLimit= false;
+  accumErr = 0;
+  prevErr = 0;
+  bResetD = false;
+  bResetI = false;
+}
+
+PIDController::PIDController(const double& kp, const double& ki, const double& kd)
+{
+  Init(kp, ki, kd);
+  upper_limit = lower_limit = 0;
+  bEnableLimit= false;
+  accumErr = 0;
+  prevErr  = 0;
+  bResetD = false;
+  bResetI = false;
+
+}
+
+void PIDController::Setlimit(const double& upper,const double& lower)
+{
+  upper_limit = upper;
+  lower_limit = lower;
+  bEnableLimit = true;
+}
+
+double PIDController::getPID(const double& currValue, const double& targetValue)
+{
+  double e = targetValue - currValue;
+  return getPID(e);
+}
+
+double PIDController::getPID(const double& e)
+{
+  //TODO Remember to add sampling time and multiply the time elapsed by the error
+  //complex PID error calculation
+  //TODO //De = ( e(i) + 3*e(i-1) - 3*e(i-2) - e(i-3) ) / 6
+
+
+  if(bResetI)
+  {
+    bResetI = false;
+    accumErr = 0;
+  }
+
+  if(bResetD)
+  {
+    bResetD = false;
+    prevErr = e;
+  }
+
+  if(pid_v < upper_limit && pid_v > lower_limit)
+    accumErr += e;
+
+  double edot= e - prevErr;
+
+  kp_v = kp * e;
+  ki_v = ki *  accumErr;
+  kd_v = kd * edot;
+
+  pid_v = kp_v + ki_v + kd_v;
+  pid_lim = pid_v;
+  if(bEnableLimit)
+  {
+    if(pid_v > upper_limit)
+    {
+      pid_lim = upper_limit;
+    }
+    else if ( pid_v < lower_limit)
+    {
+      pid_lim = lower_limit;
+    }
+  }
+
+  prevErr = e;
+
+  return pid_lim;
+}
+
+std::string PIDController::ToStringHeader()
+{
+  std::ostringstream str_out;
+  str_out << "Time" << "," <<"KP" << "," << "KI" << "," << "KD" << "," << "KP_v" << "," << "KI_v" << "," << "KD_v"
+      << "," << "pid_v" << "," << "," << "pid_lim" << "," << "," << "prevErr" << "," << "accumErr" << "," ;
+  return str_out.str();
+}
+
+std::string PIDController::ToString()
+{
+  std::ostringstream str_out;
+  timespec t_stamp;
+  UtilityH::GetTickCount(t_stamp);
+  str_out << UtilityH::GetLongTime(t_stamp) << "," <<kp << "," << ki << "," << kd << "," << kp_v << "," << ki_v << "," << kd_v
+        << "," << pid_v << "," << "," << pid_lim << "," << "," << prevErr << "," << accumErr << "," ;
+
+  return str_out.str();
+
+}
+
+void PIDController::ResetD()
+{
+  bResetD = true;
+}
+
+void PIDController::ResetI()
+{
+  bResetI = true;
+}
+
+void PIDController::Init(const double& kp, const double& ki, const double& kd)
+{
+  this->kp = kp;
+  this->ki = ki;
+  this->kd = kd;
+}
+
+LowpassFilter::LowpassFilter()
+{
+  A = 0;
+  d1 = 0;
+  d2 = 0;
+  w0 = 0;
+  w1 = 0;
+  w2 = 0;
+
+  m = 0;
+  sampleF = 0;
+  cutOffF = 0;
+}
+
+LowpassFilter::~LowpassFilter()
+{
+//  if(A)
+//    delete A;
+//  if(d1)
+//    delete d1;
+//  if(d2)
+//    delete d2;
+//  if(w0)
+//    delete w0;
+//  if(w1)
+//    delete w1;
+//  if(w2)
+//    delete w2;
+}
+
+LowpassFilter::LowpassFilter(const int& filterOrder, const double& sampleFreq, const double& cutOffFreq)
+{
+  Init(filterOrder, sampleFreq, cutOffFreq);
+}
+
+void LowpassFilter::Init(const int& n, const double& sampleFreq, const double& cutOffFreq)
+{
+  if(!(n == 2 || n == 4 || n == 6 || n == 8))
+  {
+    cout << "Undefined LowpassFilter order ! " << endl;
+
+    A = 0;
+    d1 = 0;
+    d2 = 0;
+    w0 = 0;
+    w1 = 0;
+    w2 = 0;
+
+    m = 0;
+    sampleF = 0;
+    cutOffF = 0;
+  }
+  else
+  {
+    m = n/2;
+    sampleF = sampleFreq;
+    cutOffF = cutOffFreq;
+    double ep = 1;
+    double s = sampleFreq;
+    double f = cutOffFreq;
+    double a = tan(M_PI*f/s);
+    double a2 = a*a;
+    double u = log((1.0+sqrt(1.0+ep*ep))/ep);
+    double su = sinh(u/(double)n);
+    double cu = cosh(u/(double)n);
+    double b, c;
+
+//    A  = new double[m];
+//    d1 = new double[m];
+//    d2 = new double[m];
+//    w0 = new double[m];
+//    w1 = new double[m];
+//    w2 = new double[m];
+//
+//    for(int i=0; i < m ; i++)
+//    {
+//      A[i]  = 0;
+//      d1[i] = 0;
+//      d2[i] = 0;
+//      w0[i] = 0;
+//      w1[i] = 0;
+//      w2[i] = 0;
+//    }
+
+    for(int i=0; i< m; ++i)
+    {
+        b = sin(M_PI*(2.0*i+1.0)/(2.0*n))*su;
+        c = cos(M_PI*(2.0*i+1.0)/(2.0*n))*cu;
+        c = b*b + c*c;
+        s = a2*c + 2.0*a*b + 1.0;
+        A = a2/(4.0*s); // 4.0
+        d1 = 2.0*(1-a2*c)/s;
+        d2 = -(a2*c - 2.0*a*b + 1.0)/s;
+    }
+  }
+}
+
+double LowpassFilter::getFilter(const double& value)
+{
+  double ep = 2.3/1.0; // used to normalize
+  double x = value;
+  for(int i=0; i<m; ++i)
+  {
+    w0 = d1*w1 + d2*w2 + x;
+    x = A*(w0 + 2.0*w1 + w2);
+    w2 = w1;
+    w1 = w0;
+  }
+  return ep*x;
+}
+
+
+}

src1/planning/op_utility/include/op_utility/DataRW.h

@@ -0,0 +1,861 @@
+
+/// \file DataRW.h
+/// \brief File operations for loading vector map files, loading kml map files and writing log .csv files
+/// \author Hatem Darweesh
+/// \date Jun 23, 2016
+
+
+#ifndef DATARW_H_
+#define DATARW_H_
+
+#include <string>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <iostream>
+#include <limits>
+
+#include "vector_map_msgs/PointArray.h"
+#include "vector_map_msgs/LaneArray.h"
+#include "vector_map_msgs/NodeArray.h"
+#include "vector_map_msgs/StopLineArray.h"
+#include "vector_map_msgs/DTLaneArray.h"
+#include "vector_map_msgs/LineArray.h"
+#include "vector_map_msgs/AreaArray.h"
+#include "vector_map_msgs/WayAreaArray.h"
+#include "vector_map_msgs/SignalArray.h"
+#include "vector_map_msgs/VectorArray.h"
+#include "vector_map_msgs/CrossRoadArray.h"
+#include "vector_map_msgs/RoadSignArray.h"
+#include "vector_map_msgs/CurbArray.h"
+#include "vector_map_msgs/RoadEdgeArray.h"
+#include "vector_map_msgs/CrossWalkArray.h"
+
+#include "UtilityH.h"
+
+namespace UtilityHNS {
+
+class DataRW
+{
+public:
+  DataRW();
+  virtual ~DataRW();
+
+  static std::string LoggingMainfolderName;
+  static std::string ControlLogFolderName;
+  static std::string PathLogFolderName;
+  static std::string GlobalPathLogFolderName;
+  static std::string StatesLogFolderName;
+  static std::string SimulationFolderName;
+  static std::string KmlMapsFolderName;
+  static std::string PredictionFolderName;
+  static std::string TrackingFolderName;
+
+
+  static void WriteKMLFile(const std::string& fileName, const std::vector<std::string>& gps_list);
+  static void WriteKMLFile(const std::string& fileName, const std::vector<std::vector<std::string> >& gps_list);
+  static void WriteLogData(const std::string& logFolder, const std::string& logTitle, const std::string& header, const std::vector<std::string>& logData);
+  static void CreateLoggingFolder();
+};
+
+class SimpleReaderBase
+{
+private:
+  std::ifstream* m_pFile;
+  std::vector<std::string> m_RawHeaders;
+  std::vector<std::string> m_DataTitlesHeader;
+  std::vector<std::vector<std::vector<std::string> > > m_AllData;
+  int m_nHeders;
+  int m_iDataTitles;
+  int m_nVarPerObj;
+  int m_nLineHeaders;
+  std::string m_HeaderRepeatKey;
+  char m_Separator;
+
+  void ReadHeaders();
+  void ParseDataTitles(const std::string& header);
+
+public:
+  /**
+   *
+   * @param fileName log file name
+   * @param nHeaders number of data headers
+   * @param iDataTitles which row contains the data titles
+   * @param nVariablesForOneObject 0 means each row represents one object
+   */
+  SimpleReaderBase(const std::string& fileName, const int& nHeaders = 2, const char& separator = ',',
+      const int& iDataTitles = 1, const int& nVariablesForOneObject = 0,
+      const int& nLineHeaders = 0, const std::string& headerRepeatKey = "...");
+
+  ~SimpleReaderBase();
+
+protected:
+  int ReadAllData();
+  bool ReadSingleLine(std::vector<std::vector<std::string> >& line);
+
+};
+
+class GPSDataReader : public SimpleReaderBase
+{
+public:
+  struct GPSBasicData
+  {
+    double lat;
+    double lon;
+    double alt;
+    double dir;
+    double distance;
+
+  };
+
+  public:
+  GPSDataReader(const std::string& fileName) : SimpleReaderBase(fileName){}
+  ~GPSDataReader(){}
+
+  bool ReadNextLine(GPSBasicData& data);
+  int ReadAllData(std::vector<GPSBasicData>& data_list);
+};
+
+class SimulationFileReader : public SimpleReaderBase
+{
+public:
+  struct SimulationPoint
+  {
+    double x;
+    double y;
+    double z;
+    double a;
+    double c;
+    double v;
+    std::string name;
+  };
+
+  struct SimulationData
+  {
+    SimulationPoint startPoint;
+    SimulationPoint goalPoint;
+    std::vector<SimulationPoint> simuCars;
+  };
+
+  SimulationFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1){}
+  ~SimulationFileReader(){}
+
+  bool ReadNextLine(SimulationPoint& data);
+  int ReadAllData(SimulationData& data_list);
+};
+
+class LocalizationPathReader : public SimpleReaderBase
+{
+public:
+  struct LocalizationWayPoint
+  {
+    double t;
+    double x;
+    double y;
+    double z;
+    double a;
+    double v;
+  };
+
+  LocalizationPathReader(const std::string& fileName, const char& separator) : SimpleReaderBase(fileName, 1, separator){}
+  ~LocalizationPathReader(){}
+
+  bool ReadNextLine(LocalizationWayPoint& data);
+  int ReadAllData(std::vector<LocalizationWayPoint>& data_list);
+};
+
+class AisanPointsFileReader : public SimpleReaderBase
+{
+public:
+  struct AisanPoints
+  {
+    int PID;
+    double B;
+    double L;
+    double H;
+    double Bx;
+    double Ly;
+    int Ref;
+    int MCODE1;
+    int MCODE2;
+    int MCODE3;
+  };
+
+  AisanPointsFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+
+  AisanPointsFileReader(const vector_map_msgs::PointArray& _points);
+  ~AisanPointsFileReader(){}
+
+  bool ReadNextLine(AisanPoints& data);
+  int ReadAllData(std::vector<AisanPoints>& data_list);
+  void ParseNextLine(const vector_map_msgs::Point& _rec, AisanPoints& data);
+  AisanPoints* GetDataRowById(int _pid);
+  std::vector<AisanPoints> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanPoints*> m_data_map;
+};
+
+class AisanNodesFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanNode
+  {
+    int NID;
+    int PID;
+  };
+
+  AisanNodesFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+
+  AisanNodesFileReader(const vector_map_msgs::NodeArray& _nodes);
+  ~AisanNodesFileReader(){}
+
+  bool ReadNextLine(AisanNode& data);
+  int ReadAllData(std::vector<AisanNode>& data_list);
+  void ParseNextLine(const vector_map_msgs::Node& _rec, AisanNode& data);
+  AisanNode* GetDataRowById(int _nid);
+  std::vector<AisanNode> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanNode*> m_data_map;
+};
+
+class AisanLinesFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanLine
+  {
+    int LID;
+    int BPID;
+    int FPID;
+    int BLID;
+    int FLID;
+  };
+
+  AisanLinesFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanLinesFileReader(const vector_map_msgs::LineArray & _lines);
+  ~AisanLinesFileReader(){}
+
+  bool ReadNextLine(AisanLine& data);
+  int ReadAllData(std::vector<AisanLine>& data_list);
+  void ParseNextLine(const vector_map_msgs::Line& _rec, AisanLine& data);
+  AisanLine* GetDataRowById(int _lid);
+  std::vector<AisanLine> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanLine*> m_data_map;
+};
+
+class AisanCenterLinesFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanCenterLine
+  {
+    int   DID;
+    int   Dist;
+    int   PID;
+    double   Dir;
+    double   Apara;
+    double   r;
+    double   slope;
+    double   cant;
+    double   LW;
+    double   RW;
+  };
+
+  AisanCenterLinesFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanCenterLinesFileReader(const vector_map_msgs::DTLaneArray& _dtLanes);
+  ~AisanCenterLinesFileReader(){}
+
+  bool ReadNextLine(AisanCenterLine& data);
+  int ReadAllData(std::vector<AisanCenterLine>& data_list);
+  void ParseNextLine(const vector_map_msgs::DTLane& _rec, AisanCenterLine& data);
+  AisanCenterLine* GetDataRowById(int _lnid);
+  std::vector<AisanCenterLine> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanCenterLine*> m_data_map;
+};
+
+class AisanAreasFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanArea
+  {
+    int   AID;
+    int   SLID;
+    int   ELID;
+  };
+
+  AisanAreasFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanAreasFileReader(const vector_map_msgs::AreaArray& _areas);
+  ~AisanAreasFileReader(){}
+
+  bool ReadNextLine(AisanArea& data);
+  int ReadAllData(std::vector<AisanArea>& data_list);
+  void ParseNextLine(const vector_map_msgs::Area& _rec, AisanArea& data);
+  AisanArea* GetDataRowById(int _lnid);
+  std::vector<AisanArea> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanArea*> m_data_map;
+};
+
+class AisanIntersectionFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanIntersection
+  {
+    int   ID;
+    int   AID;
+    int   LinkID;
+  };
+
+  AisanIntersectionFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanIntersectionFileReader(const vector_map_msgs::CrossRoadArray& _inters);
+  ~AisanIntersectionFileReader(){}
+
+  bool ReadNextLine(AisanIntersection& data);
+  int ReadAllData(std::vector<AisanIntersection>& data_list);
+  void ParseNextLine(const vector_map_msgs::CrossRoad& _rec, AisanIntersection& data);
+  AisanIntersection* GetDataRowById(int _lnid);
+  std::vector<AisanIntersection> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanIntersection*> m_data_map;
+};
+
+class AisanLanesFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanLane
+  {
+    int LnID  ;
+    int DID    ;
+    int BLID  ;
+    int FLID  ;
+    int BNID  ;
+    int FNID  ;
+    int JCT    ;
+    int BLID2  ;
+    int BLID3  ;
+    int BLID4  ;
+    int FLID2  ;
+    int FLID3  ;
+    int FLID4  ;
+    int ClossID  ;
+    double Span  ;
+    int LCnt  ;
+    int Lno    ;
+    int LaneType;
+    int LimitVel;
+    int RefVel  ;
+    int RoadSecID;
+    int LaneChgFG;
+    int LinkWAID;
+    char LaneDir;
+    int  LeftLaneId;
+    int RightLaneId;
+
+    int originalMapID;
+  };
+
+  AisanLanesFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanLanesFileReader(const vector_map_msgs::LaneArray& _lanes);
+  ~AisanLanesFileReader(){}
+
+  bool ReadNextLine(AisanLane& data);
+  int ReadAllData(std::vector<AisanLane>& data_list);
+  void ParseNextLine(const vector_map_msgs::Lane& _rec, AisanLane& data);
+  AisanLane* GetDataRowById(int _lnid);
+  std::vector<AisanLane> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanLane*> m_data_map;
+};
+
+class AisanStopLineFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanStopLine
+  {
+    int   ID;
+    int   LID;
+    int   TLID;
+    int   SignID;
+    int   LinkID;
+  };
+
+  AisanStopLineFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanStopLineFileReader(const vector_map_msgs::StopLineArray& _stopLines);
+  ~AisanStopLineFileReader(){}
+
+  bool ReadNextLine(AisanStopLine& data);
+  int ReadAllData(std::vector<AisanStopLine>& data_list);
+  void ParseNextLine(const vector_map_msgs::StopLine& _rec, AisanStopLine& data);
+  AisanStopLine* GetDataRowById(int _lnid);
+  std::vector<AisanStopLine> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanStopLine*> m_data_map;
+};
+
+class AisanRoadSignFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanRoadSign
+  {
+    int   ID;
+    int   VID;
+    int   PLID;
+    int   Type;
+    int   LinkID;
+  };
+
+  AisanRoadSignFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanRoadSignFileReader(const vector_map_msgs::RoadSignArray& _signs);
+  ~AisanRoadSignFileReader(){}
+
+  bool ReadNextLine(AisanRoadSign& data);
+  int ReadAllData(std::vector<AisanRoadSign>& data_list);
+  void ParseNextLine(const vector_map_msgs::RoadSign& _rec, AisanRoadSign& data);
+  AisanRoadSign* GetDataRowById(int _lnid);
+  std::vector<AisanRoadSign> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanRoadSign*> m_data_map;
+};
+
+class AisanSignalFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanSignal
+  {
+    int   ID;
+    int   VID;
+    int   PLID;
+    int   Type;
+    int   LinkID;
+  };
+
+  AisanSignalFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanSignalFileReader(const vector_map_msgs::SignalArray& _signals);
+  ~AisanSignalFileReader(){}
+
+  bool ReadNextLine(AisanSignal& data);
+  int ReadAllData(std::vector<AisanSignal>& data_list);
+  void ParseNextLine(const vector_map_msgs::Signal& _rec, AisanSignal& data);
+  AisanSignal* GetDataRowById(int _lnid);
+  std::vector<AisanSignal> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanSignal*> m_data_map;
+};
+
+class AisanVectorFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanVector
+  {
+    int   VID;
+    int   PID;
+    double   Hang;
+    double   Vang;
+  };
+
+  AisanVectorFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanVectorFileReader(const vector_map_msgs::VectorArray& _vectors);
+  ~AisanVectorFileReader(){}
+
+  bool ReadNextLine(AisanVector& data);
+  int ReadAllData(std::vector<AisanVector>& data_list);
+  void ParseNextLine(const vector_map_msgs::Vector& _rec, AisanVector& data);
+  AisanVector* GetDataRowById(int _lnid);
+  std::vector<AisanVector> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanVector*> m_data_map;
+};
+
+class AisanCurbFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanCurb
+  {
+    int   ID;
+    int   LID;
+    double   Height;
+    double   Width;
+    int   dir;
+    int   LinkID;
+  };
+
+  AisanCurbFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanCurbFileReader(const vector_map_msgs::CurbArray& _curbs);
+  ~AisanCurbFileReader(){}
+
+  bool ReadNextLine(AisanCurb& data);
+  int ReadAllData(std::vector<AisanCurb>& data_list);
+  void ParseNextLine(const vector_map_msgs::Curb& _rec, AisanCurb& data);
+  AisanCurb* GetDataRowById(int _lnid);
+  std::vector<AisanCurb> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanCurb*> m_data_map;
+};
+
+class AisanRoadEdgeFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanRoadEdge
+  {
+    int   ID;
+    int   LID;
+    int   LinkID;
+  };
+
+  AisanRoadEdgeFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanRoadEdgeFileReader(const vector_map_msgs::RoadEdgeArray& _roadEdges);
+  ~AisanRoadEdgeFileReader(){}
+
+  bool ReadNextLine(AisanRoadEdge& data);
+  int ReadAllData(std::vector<AisanRoadEdge>& data_list);
+  void ParseNextLine(const vector_map_msgs::RoadEdge& _rec, AisanRoadEdge& data);
+  AisanRoadEdge* GetDataRowById(int _lnid);
+  std::vector<AisanRoadEdge> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanRoadEdge*> m_data_map;
+};
+
+class AisanCrossWalkFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanCrossWalk
+  {
+    int   ID;
+    int   AID;
+    int   Type;
+    int    BdID;
+    int   LinkID;
+  };
+
+  AisanCrossWalkFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanCrossWalkFileReader(const vector_map_msgs::CrossWalkArray& _crossWalks);
+  ~AisanCrossWalkFileReader(){}
+
+  bool ReadNextLine(AisanCrossWalk& data);
+  int ReadAllData(std::vector<AisanCrossWalk>& data_list);
+  void ParseNextLine(const vector_map_msgs::CrossWalk& _rec, AisanCrossWalk& data);
+  AisanCrossWalk* GetDataRowById(int _lnid);
+  std::vector<AisanCrossWalk> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanCrossWalk*> m_data_map;
+};
+
+class AisanWayareaFileReader : public SimpleReaderBase
+{
+public:
+
+  struct AisanWayarea
+  {
+    int   ID;
+    int   AID;
+    int   LinkID;
+  };
+
+  AisanWayareaFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1)
+  {
+    m_min_id = std::numeric_limits<int>::max();
+  }
+  AisanWayareaFileReader(const vector_map_msgs::WayAreaArray& _wayArea);
+  ~AisanWayareaFileReader(){}
+
+  bool ReadNextLine(AisanWayarea& data);
+  int ReadAllData(std::vector<AisanWayarea>& data_list);
+  void ParseNextLine(const vector_map_msgs::WayArea& _rec, AisanWayarea& data);
+  AisanWayarea* GetDataRowById(int _lnid);
+  std::vector<AisanWayarea> m_data_list;
+
+private:
+  int m_min_id;
+  std::vector<AisanWayarea*> m_data_map;
+};
+
+class AisanDataConnFileReader : public SimpleReaderBase
+{
+public:
+
+  struct DataConn
+  {
+    int   LID; // lane id
+    int   SLID; // stop line id
+    int   SID; // signal id
+    int   SSID; // stop sign id
+  };
+
+  AisanDataConnFileReader(const std::string& fileName) : SimpleReaderBase(fileName, 1){}
+  ~AisanDataConnFileReader(){}
+
+  bool ReadNextLine(DataConn& data);
+  int ReadAllData(std::vector<DataConn>& data_list);
+};
+
+class MapRaw
+{
+public:
+  UtilityHNS::AisanLanesFileReader* pLanes;
+  UtilityHNS::AisanPointsFileReader* pPoints;
+  UtilityHNS::AisanCenterLinesFileReader* pCenterLines;
+  UtilityHNS::AisanIntersectionFileReader* pIntersections;
+  UtilityHNS::AisanAreasFileReader* pAreas;
+  UtilityHNS::AisanLinesFileReader* pLines;
+  UtilityHNS::AisanStopLineFileReader* pStopLines;
+  UtilityHNS::AisanSignalFileReader* pSignals;
+  UtilityHNS::AisanVectorFileReader* pVectors;
+  UtilityHNS::AisanCurbFileReader* pCurbs;
+  UtilityHNS::AisanRoadEdgeFileReader* pRoadedges;
+  UtilityHNS::AisanWayareaFileReader* pWayAreas;
+  UtilityHNS::AisanCrossWalkFileReader* pCrossWalks;
+  UtilityHNS::AisanNodesFileReader* pNodes;
+
+  struct timespec _time_out;
+
+  MapRaw()
+  {
+    pLanes = nullptr;
+    pPoints = nullptr;
+    pCenterLines = nullptr;
+    pIntersections = nullptr;
+    pAreas = nullptr;
+    pLines = nullptr;
+    pStopLines = nullptr;
+    pSignals = nullptr;
+    pVectors = nullptr;
+    pCurbs = nullptr;
+    pRoadedges = nullptr;
+    pWayAreas = nullptr;
+    pCrossWalks = nullptr;
+    pNodes = nullptr;
+
+    UtilityH::GetTickCount(_time_out);
+  }
+
+  ~MapRaw()
+  {
+    if(pLanes != nullptr)
+    {
+      delete pLanes;
+      pLanes = nullptr;
+    }
+
+    if(pPoints != nullptr)
+    {
+      delete pPoints;
+      pPoints = nullptr;
+    }
+
+    if(pCenterLines != nullptr)
+    {
+      delete pCenterLines;
+      pCenterLines = nullptr;
+    }
+
+    if(pIntersections != nullptr)
+    {
+      delete pIntersections;
+      pIntersections = nullptr;
+    }
+
+    if(pAreas != nullptr)
+    {
+      delete pAreas;
+      pAreas = nullptr;
+    }
+
+    if(pLines != nullptr)
+    {
+      delete pLines;
+      pLines = nullptr;
+    }
+
+    if(pStopLines != nullptr)
+    {
+      delete pStopLines;
+      pStopLines = nullptr;
+    }
+
+    if(pSignals != nullptr)
+    {
+      delete pSignals;
+      pSignals = nullptr;
+    }
+
+    if(pVectors != nullptr)
+    {
+      delete pVectors;
+      pVectors = nullptr;
+    }
+
+    if(pCurbs != nullptr)
+    {
+      delete pCurbs;
+      pCurbs = nullptr;
+    }
+
+    if(pRoadedges != nullptr)
+    {
+      delete pRoadedges;
+      pRoadedges = nullptr;
+    }
+
+    if(pWayAreas != nullptr)
+    {
+      delete pWayAreas;
+      pWayAreas = nullptr;
+    }
+
+    if(pCrossWalks != nullptr)
+    {
+      delete pCrossWalks;
+      pCrossWalks = nullptr;
+    }
+
+    if(pNodes != nullptr)
+    {
+      delete pNodes;
+      pNodes = nullptr;
+    }
+  }
+
+  int GetVersion()
+  {
+    bool bTimeOut = UtilityH::GetTimeDiffNow(_time_out) > 2.0;
+    bool bLoaded =  pLanes != nullptr && pPoints != nullptr && pCenterLines  != nullptr && pNodes  != nullptr;
+    int iVersion = 0;
+    if(bLoaded && bTimeOut)
+    {
+      iVersion = 2;
+      if(pNodes->m_data_list.size() == 0)
+      {
+        iVersion = 1;
+      }
+    }
+//    else
+//    {
+//      bLoaded =  pLanes != nullptr && pPoints != nullptr && pCenterLines  != nullptr;
+//      if(bLoaded && bTimeOut)
+//      {
+//        iVersion = 1;
+//        if(pNodes  == nullptr)
+//          pNodes = new AisanNodesFileReader(vector_map_msgs::NodeArray());
+//      }
+//    }
+
+    if(bLoaded && bTimeOut)
+    {
+      if(pIntersections  == nullptr)
+        pIntersections  = new AisanIntersectionFileReader(vector_map_msgs::CrossRoadArray());
+
+      if(pLines  == nullptr)
+        pLines  = new AisanLinesFileReader(vector_map_msgs::LineArray());
+
+      if(pStopLines  == nullptr)
+        pStopLines  = new AisanStopLineFileReader(vector_map_msgs::StopLineArray());
+
+      if(pSignals  == nullptr)
+        pSignals  = new AisanSignalFileReader(vector_map_msgs::SignalArray());
+
+      if(pVectors  == nullptr)
+        pVectors  = new AisanVectorFileReader(vector_map_msgs::VectorArray());
+
+      if(pCurbs  == nullptr)
+        pCurbs  = new AisanCurbFileReader(vector_map_msgs::CurbArray());
+
+      if(pRoadedges  == nullptr)
+        pRoadedges  = new AisanRoadEdgeFileReader(vector_map_msgs::RoadEdgeArray());
+
+      if(pWayAreas  == nullptr)
+        pWayAreas  = new AisanWayareaFileReader(vector_map_msgs::WayAreaArray());
+
+      if(pCrossWalks  == nullptr)
+        pCrossWalks  = new AisanCrossWalkFileReader(vector_map_msgs::CrossWalkArray());
+    }
+
+    return iVersion;
+  }
+};
+
+} /* namespace UtilityHNS */
+
+#endif /* DATARW_H_ */

src1/planning/op_utility/include/op_utility/UtilityH.h

@@ -0,0 +1,114 @@
+
+/// \file UtilityH.h
+/// \brief General Math and Control utility functions
+/// \author Hatem Darweesh
+/// \date May 14, 2016
+
+#ifndef UTILITYH_H_
+#define UTILITYH_H_
+
+#include <assert.h>
+#include <string>
+#include <math.h>
+
+
+namespace UtilityHNS
+{
+
+#define DEG2RAD M_PI / 180.
+#define RAD2DEG 180. / M_PI
+#define SIGN(x) (x > 0) ? 1 : ((x < 0) ? -1 : 0)
+#define MIN(x,y) (x <= y ? x : y)
+#define MAX(x,y) (x >= y ? x : y)
+
+
+class UtilityH
+{
+public:
+  UtilityH();
+  virtual ~UtilityH(); 
+
+
+  static double FixNegativeAngle(const double& a);
+  static double SplitPositiveAngle(const double& a);
+  static double InverseAngle(const double& a);
+  static double AngleBetweenTwoAnglesPositive(const double& a1, const double& a2);
+  static double GetCircularAngle(const double& prevContAngle, const double& prevAngle, const double& currAngle);
+
+  //Time Functions
+  static void GetTickCount(struct timespec& t);
+  static std::string GetFilePrefixHourMinuteSeconds();
+  static double GetTimeDiffNow(const struct timespec& old_t);
+  static double GetTimeDiff(const struct timespec& old_t,const struct timespec& curr_t);
+  static std::string GetDateTimeStr();
+  static int tsCompare (struct  timespec  time1,   struct  timespec  time2, int micro_tolerance = 10);
+  static int GetSign(double x);
+  static std::string GetHomeDirectory();
+  static double GetMomentumScaleFactor(const double& v);
+  static timespec GetTimeSpec(const time_t& srcT);
+  static time_t GetLongTime(const struct timespec& srcT);
+};
+
+class PIDController
+{
+public:
+  PIDController();
+  PIDController(const double& kp, const double& ki, const double& kd);
+  void Init(const double& kp, const double& ki, const double& kd);
+  void Setlimit(const double& upper,const double& lower);
+  double getPID(const double& currValue, const double& targetValue);
+  double getPID(const double& e);
+  void ResetD();
+  void ResetI();
+  std::string ToString();
+  std::string ToStringHeader();
+
+
+private:
+  double kp;
+  double ki;
+  double kd;
+  double kp_v;
+  double ki_v;
+  double kd_v;
+  double pid_v;
+  double pid_lim;
+  double upper_limit;
+  double lower_limit;
+  bool   bEnableLimit;
+  double accumErr;
+  double prevErr;
+  bool bResetD;
+  bool bResetI;
+
+};
+
+class LowpassFilter
+{
+public:
+  LowpassFilter();
+  virtual ~LowpassFilter();
+
+  LowpassFilter(const int& filterOrder, const double& sampleFreq, const double& cutOffFreq);
+  void Init(const int& filterOrder, const double& sampleFreq, const double& cutOffFreq);
+  double getFilter(const double& value);
+
+
+private:
+  int m;
+  double sampleF;
+  double cutOffF;
+  double A  ;
+  double d1 ;
+  double d2 ;
+  double w0 ;
+  double w1 ;
+  double w2 ;
+
+};
+
+}
+
+#endif
+
+

src1/planning/op_utility/op_utility.pro

@@ -0,0 +1,37 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2020-03-05T21:39:06
+#
+#-------------------------------------------------
+
+QT       -= gui
+
+
+TARGET = op_utility
+TEMPLATE = lib
+
+
+CONFIG += plugin
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+    DataRW.cpp \
+    UtilityH.cpp
+        
+HEADERS +=
+        
+
+INCLUDEPATH += /opt/ros/melodic/include
+INCLUDEPATH += $$PWD/../include
+INCLUDEPATH += $$PWD/include
+