fix a importing vectormap

haopo2005 · Jan 10, 2018 · e4a3efb · e4a3efb
1 parent a038319
commit e4a3efb
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 7 deletions.
diff --git a/ros/src/computing/planning/decision/libs/libstate/include/state_drive.hpp b/ros/src/computing/planning/decision/libs/libstate/include/state_drive.hpp
@@ -249,7 +249,7 @@ class DriveBehaviorAcceptLanechangeState : public State<DriveBehaviorAcceptLanec
   friend class State<DriveBehaviorAcceptLanechangeState>;
   DriveBehaviorAcceptLanechangeState(void)
   {
-    StateName = "AcceptLaneChange";
+    StateName = "AcceptLane-Change";
     StateNum = DRIVE_BEHAVIOR_ACCEPT_LANECHANGE_STATE;
     StateKind = BEHAVIOR_STATE;
   }

diff --git a/...puting/planning/decision/packages/decision_maker/nodes/decision_maker/cross_road_area.cpp b/...puting/planning/decision/packages/decision_maker/nodes/decision_maker/cross_road_area.cpp
@@ -51,6 +51,9 @@ std::vector<geometry_msgs::Point> convhull(const CrossRoadArea *_TargetArea)
 {
   std::vector<int> enablePoints;
 
+  if(_TargetArea->points.size() < 3)
+	  return {};
+
   // Jarvis's March algorithm
   size_t l = 0;
   for (auto i = begin(_TargetArea->points); i != end(_TargetArea->points); i++)
@@ -72,12 +75,12 @@ std::vector<geometry_msgs::Point> convhull(const CrossRoadArea *_TargetArea)
       geometry_msgs::Point pp = _TargetArea->points.at(p);
       geometry_msgs::Point pi = _TargetArea->points.at(i);
       geometry_msgs::Point pq = _TargetArea->points.at(q);
-      if ((pi.y - pp.y) * (pq.x - pi.x) - (pi.x - pp.x) * (pq.y - pi.y) < 0)
+      if (((pi.y - pp.y) * (pq.x - pi.x) - (pi.x - pp.x) * (pq.y - pi.y)) < 0)
       {
         q = i;
       }
     }
-    enablePoints.push_back(p);
+    enablePoints.push_back(q);
     p = q;
   } while (p != l);
 

diff --git a/...anning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_init.cpp b/...anning/decision/packages/decision_maker/nodes/decision_maker/decision_maker_node_init.cpp
@@ -126,13 +126,14 @@ void DecisionMakerNode::initVectorMap(void)
   std::vector<CrossRoad> crossroads = g_vmap.findByFilter([](const CrossRoad &crossroad) { return true; });
   if (crossroads.empty())
   {
-    ROS_INFO("crossroad have not found\n");
+    ROS_INFO("crossroads have not found\n");
     return;
   }
 
   vector_map_init = true;  // loaded flag
   for (const auto &cross_road : crossroads)
   {
+    geometry_msgs::Point _prev_point;
     Area area = g_vmap.findByKey(Key<Area>(cross_road.aid));
     CrossRoadArea carea;
     carea.id = _index++;
@@ -147,9 +148,8 @@ void DecisionMakerNode::initVectorMap(void)
         g_vmap.findByFilter([&area](const Line &line) { return area.slid <= line.lid && line.lid <= area.elid; });
     for (const auto &line : lines)
     {
-      geometry_msgs::Point _prev_point;
       std::vector<Point> points =
-          g_vmap.findByFilter([&line](const Point &point) { return line.bpid == point.pid || point.pid == line.fpid; });
+          g_vmap.findByFilter([&line](const Point &point) { return line.bpid == point.pid;});
       for (const auto &point : points)
       {
         geometry_msgs::Point _point;
@@ -172,7 +172,6 @@ void DecisionMakerNode::initVectorMap(void)
         y_min = (y_min == 0.0) ? _point.y : std::min(_point.y, y_min);
         y_max = (y_max == 0.0) ? _point.y : std::max(_point.y, y_max);
         z = _point.z;
-
       }  // points iter
     }    // line iter
     carea.bbox.pose.position.x = x_avg / (double)points_count;