From 98b3b076f6f44a280c094983f462ecbd801e7f69 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Tue, 3 Dec 2024 04:59:21 +0100 Subject: [PATCH] Demonstrate abort path generation --- .../src/run_dynamic_rallypoints.cpp | 15 +++++++++++++-- terrain_planner/src/terrain_ompl.cpp | 6 +++++- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/terrain_abort_planner/src/run_dynamic_rallypoints.cpp b/terrain_abort_planner/src/run_dynamic_rallypoints.cpp index 3973c07d..338e8598 100644 --- a/terrain_abort_planner/src/run_dynamic_rallypoints.cpp +++ b/terrain_abort_planner/src/run_dynamic_rallypoints.cpp @@ -303,6 +303,10 @@ int main(int argc, char** argv) { json data = json::parse(f); json polygons = data["geoFence"]["polygons"]; std::cout << "Number of vertices: " << polygons[0]["polygon"].size() << std::endl; + + grid_map::Polygon map_polygon; + map_polygon.setFrameId(terrain_map->getGridMap().getFrameId()); + for (auto polygon_vertex : polygons[0]["polygon"]) { Eigen::Vector2d vertex_wgs84 = Eigen::Vector2d(polygon_vertex[0], polygon_vertex[1]); // Convert vertex into lv03 @@ -316,12 +320,19 @@ int main(int argc, char** argv) { terrain_map->getGlobalOrigin(map_coordinate, map_origin); Eigen::Vector3d vertex_local = vertex_lv03 - map_origin; geofence_polygon.push_back(vertex_local.head(2)); + map_polygon.addVertex(vertex_local.head(2)); } - for (auto vertex : geofence_polygon) { - std::cout << "Parsed Geofence vertex: " << vertex.transpose() << std::endl; + terrain_map->getGridMap().add("valid"); + terrain_map->getGridMap()["valid"].setConstant(0.0); + for (grid_map::PolygonIterator iterator(terrain_map->getGridMap(), map_polygon); !iterator.isPastEnd(); ++iterator) { + terrain_map->getGridMap().at("valid", *iterator) = 1.0; } + // for (auto vertex : polygon) { + // std::cout << "Parsed Geofence vertex: " << vertex.transpose() << std::endl; + // } + // Parse mission to generate reference path std::vector waypoint_list; json mission = data["mission"]["items"]; diff --git a/terrain_planner/src/terrain_ompl.cpp b/terrain_planner/src/terrain_ompl.cpp index 51849bdc..6ba32e1c 100644 --- a/terrain_planner/src/terrain_ompl.cpp +++ b/terrain_planner/src/terrain_ompl.cpp @@ -17,7 +17,11 @@ bool TerrainValidityChecker::checkCollision(const Eigen::Vector3d state) const { if (check_collision_max_altitude_) { in_collision_max = isInCollision("max_elevation", state, false); } - return in_collision | in_collision_max; + bool valid; + if (map_.isInside(state.head(2))) { + valid = bool(map_.atPosition("valid", state.head(2)) > 0.5); + } + return in_collision | in_collision_max | !valid; } bool TerrainValidityChecker::isInCollision(const std::string& layer, const Eigen::Vector3d& position,