Parse geofence information from QGC mission file

terrain_planner_benchmark/CMakeLists.txt

@@ -9,9 +9,7 @@ find_package(ompl REQUIRED)
 find_package(Boost REQUIRED COMPONENTS serialization system filesystem)
 
 include(cmake/CPM.cmake)
-CPMAddPackage("gh:nlohmann/[email protected]")
-
-# find_package(nlohmann_json 3.10.5 REQUIRED)
+CPMAddPackage("gh:nlohmann/[email protected]")
 
 find_package(catkin REQUIRED COMPONENTS
     eigen_catkin

terrain_planner_benchmark/launch/rallypoints.rviz

@@ -15,8 +15,9 @@ Panels:
         - /MarkerArray1
         - /MarkerArray2
         - /MarkerArray3
+        - /MarkerArray4
       Splitter Ratio: 0.5
-    Tree Height: 839
+    Tree Height: 845
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -65,15 +66,15 @@ Visualization Manager:
       Marker Topic: /start_position
       Name: Marker
       Namespaces:
-        "": true
+        {}
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
       Enabled: true
       Marker Topic: /goal_position
       Name: Marker
       Namespaces:
-        "": true
+        {}
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -186,23 +187,31 @@ Visualization Manager:
       Marker Topic: /path_segments
       Name: MarkerArray
       Namespaces:
-        normals: true
+        {}
       Queue Size: 100
       Value: true
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /rallypoints_marker
       Name: MarkerArray
       Namespaces:
-        rallypoints: true
+        {}
       Queue Size: 100
       Value: true
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /abort_path_segments
       Name: MarkerArray
       Namespaces:
-        normals: true
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /geofence_marker
+      Name: MarkerArray
+      Namespaces:
+        geofence: true
       Queue Size: 100
       Value: true
   Enabled: true
@@ -233,33 +242,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3155.947265625
+      Distance: 4618.54638671875
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -509.687255859375
-        Y: -545.7493896484375
-        Z: 1358.635009765625
+        X: 79.93209075927734
+        Y: -697.7658081054688
+        Z: 860.8773193359375
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6647965312004089
+      Pitch: 0.5997965931892395
       Target Frame: <Fixed Frame>
-      Yaw: 0.7581591010093689
+      Yaw: 5.066342830657959
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 1136
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001a1000003d2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000591000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001a1000003d6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003d6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000591000003d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

terrain_planner_benchmark/launch/run_dynamic_rallypoints.launch

@@ -1,15 +1,15 @@
 <launch>
     <arg name="visualization" default="true"/>
     <!-- <arg name="location" default="dischma_valley"/> -->
-    <arg name="location" default="sertig"/>
+    <arg name="location" default="duebendorf"/>
 
     <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/>
 
     <node pkg="terrain_planner_benchmark" type="run_dynamic_rallypoints" name="rrt_planner" output="screen">
         <param name="location" value="$(arg location)"/>
-        <param name="map_path" value="$(find terrain_models)/models/$(arg location).tif"/>
-        <param name="mission_file_path" value="$(find terrain_planner_benchmark)/Tools/mission.plan"/>
-        <param name="color_file_path" value="$(find terrain_models)/models/$(arg location)_color.tif"/>
+        <param name="map_path" value="$(find terrain_planner_benchmark)/resources/$(arg location).tif"/>
+        <param name="mission_file_path" value="$(find terrain_planner_benchmark)/resources/mission.plan"/>
+        <param name="color_file_path" value="$(find terrain_planner_benchmark)/resources/$(arg location)_color.tif"/>
         <param name="output_directory" value="$(find terrain_planner_benchmark)/../output"/>
     </node>
     <group if="$(arg visualization)">

terrain_planner_benchmark/src/run_dynamic_rallypoints.cpp

@@ -51,14 +51,16 @@
 #include <grid_map_ros/GridMapRosConverter.hpp>
 
 #include "terrain_navigation/data_logger.h"
+#include "terrain_navigation_ros/geo_conversions.h"
 #include "terrain_navigation_ros/visualization.h"
 #include "terrain_planner/common.h"
 #include "terrain_planner/terrain_ompl_rrt.h"
 
 #include <nlohmann/json.hpp>
 using json = nlohmann::json;
 
-void publishPathSegments(ros::Publisher& pub, Path& trajectory, Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)) {
+void publishPathSegments(ros::Publisher& pub, Path& trajectory,
+                         Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)) {
   visualization_msgs::MarkerArray msg;
 
   std::vector<visualization_msgs::Marker> marker;
@@ -80,8 +82,8 @@ void publishPathSegments(ros::Publisher& pub, Path& trajectory, Eigen::Vector3d
   pub.publish(msg);
 }
 
-visualization_msgs::Marker getGoalMarker(const int id, const Eigen::Vector3d &position,
-                                                         const double radius, const Eigen::Vector3d color) {
+visualization_msgs::Marker getGoalMarker(const int id, const Eigen::Vector3d& position, const double radius,
+                                         const Eigen::Vector3d color) {
   visualization_msgs::Marker marker;
   marker.header.frame_id = "map";
   marker.header.stamp = ros::Time::now();
@@ -113,13 +115,13 @@ visualization_msgs::Marker getGoalMarker(const int id, const Eigen::Vector3d &po
   return marker;
 }
 
-void publishRallyPoints(const ros::Publisher &pub, const std::vector<Eigen::Vector3d> &positions,
-                                        const double radius, Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0),
-                          std::string name_space = "rallypoints") {
+void publishRallyPoints(const ros::Publisher& pub, const std::vector<Eigen::Vector3d>& positions, const double radius,
+                        Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0),
+                        std::string name_space = "rallypoints") {
   visualization_msgs::MarkerArray marker_array;
   std::vector<visualization_msgs::Marker> markers;
   int marker_id = 1;
-  for (const auto &position : positions) {
+  for (const auto& position : positions) {
     visualization_msgs::Marker marker;
     marker = getGoalMarker(marker_id, position, radius, color);
     marker.ns = name_space;
@@ -130,7 +132,6 @@ void publishRallyPoints(const ros::Publisher &pub, const std::vector<Eigen::Vect
   pub.publish(marker_array);
 }
 
-
 void publishCircleSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& position, const double radius) {
   visualization_msgs::Marker marker;
   marker.header.stamp = ros::Time::now();
@@ -168,6 +169,51 @@ void publishCircleSetpoints(const ros::Publisher& pub, const Eigen::Vector3d& po
   pub.publish(marker);
 }
 
+void publishGeoFence(const ros::Publisher& pub, const std::vector<Eigen::Vector2d>& geofence_polygon,
+                     Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), std::string name_space = "geofence") {
+  visualization_msgs::MarkerArray marker_array;
+  std::vector<visualization_msgs::Marker> markers;
+  for (int marker_id = 0; marker_id < geofence_polygon.size() - 1; marker_id ++) {
+    visualization_msgs::Marker marker;
+    marker.header.frame_id = "map";
+    marker.header.stamp = ros::Time::now();
+    marker.id = marker_id;
+    marker.type = visualization_msgs::Marker::LINE_STRIP;
+    marker.action = visualization_msgs::Marker::ADD;
+    std::vector<geometry_msgs::Point> points;
+    for (auto vertex : geofence_polygon) {
+      geometry_msgs::Point point;
+      point.x = vertex[0];
+      point.y = vertex[1];
+      point.z = 500.0;
+      points.push_back(point);
+    }
+    points.push_back(points.front()); // Close the polygon
+    marker.points = points;
+    marker.scale.x = 10.0;
+    marker.scale.y = 10.0;
+    marker.scale.z = 10.0;
+    marker.color.a = 0.5;  // Don't forget to set the alpha!
+    marker.pose.orientation.w = 1.0;
+    marker.pose.orientation.x = 0.0;
+    marker.pose.orientation.y = 0.0;
+    marker.pose.orientation.z = 0.0;
+    marker.color.r = color(0);
+    marker.color.g = color(1);
+    marker.color.b = color(2);
+    marker.ns = name_space;
+    markers.push_back(marker);
+  }
+  marker_array.markers = markers;
+  pub.publish(marker_array);
+}
+
+void publishGridMap(const ros::Publisher& pub, const grid_map::GridMap& map) {
+  grid_map_msgs::GridMap message;
+  grid_map::GridMapRosConverter::toMessage(map, message);
+  pub.publish(message);
+}
+
 void getDubinsShortestPath(std::shared_ptr<fw_planning::spaces::DubinsAirplaneStateSpace>& dubins_ss,
                            const Eigen::Vector3d start_pos, const double start_yaw, const Eigen::Vector3d goal_pos,
                            const double goal_yaw, std::vector<Eigen::Vector3d>& path) {
@@ -290,6 +336,7 @@ int main(int argc, char** argv) {
   auto path_segment_pub = nh.advertise<visualization_msgs::MarkerArray>("path_segments", 1, true);
   auto abort_path_pub = nh.advertise<visualization_msgs::MarkerArray>("abort_path_segments", 1, true);
   auto rallypoint_pub = nh.advertise<visualization_msgs::MarkerArray>("rallypoints_marker", 1);
+  auto geofence_pub = nh.advertise<visualization_msgs::MarkerArray>("geofence_marker", 1);
 
   std::string map_path, color_file_path, output_directory, location, mission_file_path;
   nh_private.param<std::string>("map_path", map_path, "");
@@ -302,16 +349,6 @@ int main(int argc, char** argv) {
   auto data_logger = std::make_shared<DataLogger>();
   data_logger->setKeys({"x", "y", "z"});
 
-  ///TODO: Parse json file to get geofence information
-  ///TODO: Generate path from waypoint list
-  std::ifstream f(mission_file_path);
-  json data = json::parse(f);
-  json polygons = data["geoFence"]["polygons"];
-  // json polygon = polygons["inclusion"];
-  for (json::iterator it = polygons.begin(); it != polygons.end(); ++it) {
-    std::cout << *it << '\n';
-  }                   
-
   // Load terrain map from defined tif paths
   auto terrain_map = std::make_shared<TerrainMap>();
   terrain_map->initializeFromGeotiff(map_path);
@@ -324,6 +361,38 @@ int main(int argc, char** argv) {
   terrain_map->AddLayerHorizontalDistanceTransform(radius, "ics_+", "distance_surface");
   terrain_map->AddLayerHorizontalDistanceTransform(-radius, "ics_-", "max_elevation");
 
+  // Parse json file to get geofence information
+  std::vector<Eigen::Vector2d> geofence_polygon;
+  std::ifstream f(mission_file_path);
+  json data = json::parse(f);
+  json polygons = data["geoFence"]["polygons"];
+  std::cout << "Number of vertices: " << polygons[0]["polygon"].size() << std::endl;
+  for (auto polygon_vertex : polygons[0]["polygon"]) {
+    Eigen::Vector2d vertex_wgs84 = Eigen::Vector2d(polygon_vertex[0], polygon_vertex[1]);
+    std::cout << "  - Vertex WGS84: " << vertex_wgs84.transpose() << std::endl;
+    // Convert vertex into lv03
+    Eigen::Vector3d vertex_lv03;
+    GeoConversions::forward(vertex_wgs84[0], vertex_wgs84[1], 0.0, vertex_lv03.x(), vertex_lv03.y(),
+                            vertex_lv03.z());  /// FIXME: Assuming zero AMSL
+
+    // Convert vertex into local frame (Relative to map origin)
+    ESPG map_coordinate;
+    Eigen::Vector3d map_origin;
+    terrain_map->getGlobalOrigin(map_coordinate, map_origin);
+    Eigen::Vector3d vertex_local = vertex_lv03 - map_origin;
+    geofence_polygon.push_back(vertex_local.head(2));
+  }
+
+  /// TODO: Visualize Geofence
+  for (auto vertex : geofence_polygon) {
+    std::cout << "Parsed vertex: " << vertex.transpose() << std::endl;
+  }
+  publishGeoFence(geofence_pub, geofence_polygon);
+  terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec());
+  publishGridMap(grid_map_pub, terrain_map->getGridMap());
+
+  /// TODO: Generate path from waypoint list
+
   // Initialize planner with loaded terrain map
   auto planner = std::make_shared<TerrainOmplRrt>();
   planner->setMap(terrain_map);
@@ -351,7 +420,6 @@ int main(int argc, char** argv) {
     throw std::runtime_error("Specified goal position is NOT valid");
   }
 
-
   Path path;
   planner->setupProblem(start, goal);
   if (planner->Solve(20.0, path)) {
@@ -372,7 +440,6 @@ int main(int argc, char** argv) {
 
   path.appendSegment(goal_loiter_path);
 
-
   /// Generate candidate rally points
   std::cout << "Generating rally points" << std::endl;
   const int num_rally_points = 6;
@@ -381,12 +448,13 @@ int main(int argc, char** argv) {
     bool sample_is_valid = false;
     while (!sample_is_valid) {
       Eigen::Vector3d random_sample;
-      random_sample(0) = getRandom(map_pos(0) - (0.4 * map_width_x - radius), map_pos(0) + (0.4 * map_width_x - radius));
-      random_sample(1) = getRandom(map_pos(1) - (0.4 * map_width_y - radius), map_pos(1) + (0.4 * map_width_y - radius));
+      random_sample(0) =
+          getRandom(map_pos(0) - (0.4 * map_width_x - radius), map_pos(0) + (0.4 * map_width_x - radius));
+      random_sample(1) =
+          getRandom(map_pos(1) - (0.4 * map_width_y - radius), map_pos(1) + (0.4 * map_width_y - radius));
       Eigen::Vector3d candidate_loiter_position = random_sample;
       Eigen::Vector3d new_loiter_position;
-      sample_is_valid =
-          validatePosition(terrain_map->getGridMap(), candidate_loiter_position, new_loiter_position);
+      sample_is_valid = validatePosition(terrain_map->getGridMap(), candidate_loiter_position, new_loiter_position);
       if (sample_is_valid) {
         rally_points.push_back(new_loiter_position);
       }
@@ -396,10 +464,10 @@ int main(int argc, char** argv) {
 
   std::cout << "Successfully Generated rally points" << std::endl;
 
-  ///TODO: Iterate over path segments to find abort paths
+  /// TODO: Iterate over path segments to find abort paths
   int segment_id = 0;
   Path abort_path_list;
-  for (const auto &path_segment : path.segments) {
+  for (const auto& path_segment : path.segments) {
     std::cout << "  - segment ID: " << segment_id << std::endl;
     Eigen::Vector3d segment_end = path_segment.states.back().position;
     Eigen::Vector3d segment_end_tangent = path_segment.states.back().velocity;
@@ -416,9 +484,7 @@ int main(int argc, char** argv) {
 
   // Repeatedly publish results
   terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec());
-  grid_map_msgs::GridMap message;
-  grid_map::GridMapRosConverter::toMessage(terrain_map->getGridMap(), message);
-  grid_map_pub.publish(message);
+  publishGridMap(grid_map_pub, terrain_map->getGridMap());
   publishPathSegments(path_segment_pub, path, Eigen::Vector3d(0.0, 1.0, 0.0));
   publishPathSegments(abort_path_pub, abort_path_list, Eigen::Vector3d(1.0, 0.0, 0.0));