diff --git a/ros/src/data/packages/vector_map_server/CMakeLists.txt b/ros/src/data/packages/vector_map_server/CMakeLists.txt index a840e077ad9..72cb4cc3c7a 100644 --- a/ros/src/data/packages/vector_map_server/CMakeLists.txt +++ b/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( diff --git a/ros/src/data/packages/vector_map_server/nodes/vector_map_server/vector_map_server.cpp b/ros/src/data/packages/vector_map_server/nodes/vector_map_server/vector_map_server.cpp index cd89400567f..eb78a0d765a 100644 --- a/ros/src/data/packages/vector_map_server/nodes/vector_map_server/vector_map_server.cpp +++ b/ros/src/data/packages/vector_map_server/nodes/vector_map_server/vector_map_server.cpp @@ -59,6 +59,7 @@ #include #include #include +#include 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; namespace { @@ -437,6 +441,85 @@ std::vector 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(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(line.bpid)); + if (point.pid == 0) + return null_polygon; + polygon.push_back(convertPointToGeomPoint(point)); + + if (line.flid == 0) + break; + + line = vmap.findByKey(Key(line.flid)); + if (line.lid == 0) + return null_polygon; + } + Point point = vmap.findByKey(Key(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(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(); diff --git a/ros/src/data/packages/vector_map_server/srv/PositionState.srv b/ros/src/data/packages/vector_map_server/srv/PositionState.srv new file mode 100644 index 00000000000..0a5b5af13fc --- /dev/null +++ b/ros/src/data/packages/vector_map_server/srv/PositionState.srv @@ -0,0 +1,3 @@ +geometry_msgs/Point position +--- +bool state