Merge pull request #504 from CPFL/add-way-area-detection-service

Add WayArea detection service

ros/src/data/packages/vector_map_server/CMakeLists.txt

@@ -41,6 +41,7 @@ add_service_files(
   GetWall.srv
   GetFence.srv
   GetRailCrossing.srv
+  PositionState.srv
 )
 
 generate_messages(

ros/src/data/packages/vector_map_server/nodes/vector_map_server/vector_map_server.cpp

@@ -59,6 +59,7 @@
 #include <vector_map_server/GetWall.h>
 #include <vector_map_server/GetFence.h>
 #include <vector_map_server/GetRailCrossing.h>
+#include <vector_map_server/PositionState.h>
 
 using vector_map::VectorMap;
 using vector_map::Category;
@@ -99,8 +100,11 @@ using vector_map::Wall;
 using vector_map::Fence;
 using vector_map::RailCrossing;
 
-using vector_map::convertGeomPointToPoint;
 using vector_map::isValidMarker;
+using vector_map::convertPointToGeomPoint;
+using vector_map::convertGeomPointToPoint;
+
+using Polygon = std::vector<geometry_msgs::Point>;
 
 namespace
 {
@@ -437,6 +441,85 @@ std::vector<Lane> createFineLanes(const VectorMap& vmap, const waypoint_follower
   return null_lanes;
 }
 
+bool isValidPolygon(const Polygon& polygon)
+{
+  return polygon.size() > 3;
+}
+
+Polygon createPolygon(const VectorMap& vmap, const Area& area)
+{
+  Polygon null_polygon;
+  if (area.aid == 0)
+    return null_polygon;
+
+  Line line = vmap.findByKey(Key<Line>(area.slid));
+  if (line.lid == 0)
+    return null_polygon;
+  if (line.blid != 0)
+    return null_polygon;
+
+  Polygon polygon;
+  Line start_line = line;
+  while (true)
+  {
+    Point point = vmap.findByKey(Key<Point>(line.bpid));
+    if (point.pid == 0)
+      return null_polygon;
+    polygon.push_back(convertPointToGeomPoint(point));
+
+    if (line.flid == 0)
+      break;
+
+    line = vmap.findByKey(Key<Line>(line.flid));
+    if (line.lid == 0)
+      return null_polygon;
+  }
+  Point point = vmap.findByKey(Key<Point>(line.fpid));
+  if (point.pid == 0)
+    return null_polygon;
+  polygon.push_back(convertPointToGeomPoint(point));
+
+  Line end_line = line;
+  if (start_line.bpid != end_line.fpid)
+    return null_polygon;
+
+  if (!isValidPolygon(polygon))
+    return null_polygon;
+
+  return polygon;
+}
+
+bool isWinding(const Polygon& polygon, const geometry_msgs::Point& geom_point, size_t i)
+{
+  double variation_x = polygon[i + 1].x - polygon[i].x;
+  variation_x *= (geom_point.y - polygon[i].y) / (polygon[i + 1].y - polygon[i].y);
+  return geom_point.x < polygon[i].x + variation_x;
+}
+
+bool isInPolygon(const Polygon& polygon, const geometry_msgs::Point& geom_point)
+{
+  if (!isValidPolygon(polygon))
+    return false;
+
+  // Winding Number Algorithm
+  int winding_number = 0;
+  for (size_t i = 0; i < polygon.size() - 1; ++i)
+  {
+    if (polygon[i].y <= geom_point.y && polygon[i + 1].y > geom_point.y)
+    {
+      if (isWinding(polygon, geom_point, i))
+        ++winding_number;
+    }
+    else if (polygon[i].y > geom_point.y && polygon[i + 1].y <= geom_point.y)
+    {
+      if (isWinding(polygon, geom_point, i))
+        --winding_number;
+    }
+  }
+
+  return winding_number != 0;
+}
+
 class VectorMapServer
 {
 private:
@@ -944,6 +1027,25 @@ class VectorMapServer
     }
     return true;
   }
+
+  bool isWayArea(vector_map_server::PositionState::Request& request,
+                 vector_map_server::PositionState::Response& response)
+  {
+    response.state = false;
+    for (const auto& way_area : vmap_.findByFilter([](const WayArea& way_area){return true;}))
+    {
+      Area area = vmap_.findByKey(Key<Area>(way_area.aid));
+      if (area.aid == 0)
+        continue;
+      Polygon polygon = createPolygon(vmap_, area);
+      if (isInPolygon(polygon, request.position))
+      {
+        response.state = true;
+        break;
+      }
+    }
+    return true;
+  }
 };
 } // namespace
 
@@ -1006,6 +1108,8 @@ int main(int argc, char **argv)
                                                          &VectorMapServer::getFence, &vms);
   ros::ServiceServer get_rail_crossing_srv = nh.advertiseService("vector_map_server/get_rail_crossing",
                                                                  &VectorMapServer::getRailCrossing, &vms);
+  ros::ServiceServer is_way_area_srv = nh.advertiseService("vector_map_server/is_way_area",
+                                                           &VectorMapServer::isWayArea, &vms);
 
   ros::spin();
 

ros/src/data/packages/vector_map_server/srv/PositionState.srv

@@ -0,0 +1,3 @@
+geometry_msgs/Point position
+---
+bool state