Parse json with json library

F Parse json
ethz-asl · Nov 30, 2024 · d2543e2 · d2543e2
1 parent dcd2e35
commit d2543e2
Show file tree

Hide file tree

Showing 4 changed files with 46 additions and 2 deletions.
diff --git a/terrain_planner_benchmark/CMakeLists.txt b/terrain_planner_benchmark/CMakeLists.txt
@@ -8,6 +8,11 @@ find_package(OpenCV REQUIRED)
 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)
+
 find_package(catkin REQUIRED COMPONENTS
     eigen_catkin
     grid_map_core
@@ -111,4 +116,4 @@ add_executable(run_dynamic_rallypoints
   src/run_dynamic_rallypoints.cpp
 )
 add_dependencies(run_dynamic_rallypoints ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(run_dynamic_rallypoints ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES})
+target_link_libraries(run_dynamic_rallypoints ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES} nlohmann_json::nlohmann_json)
diff --git a/terrain_planner_benchmark/cmake/CPM.cmake b/terrain_planner_benchmark/cmake/CPM.cmake
@@ -0,0 +1,24 @@
+# SPDX-License-Identifier: MIT
+#
+# SPDX-FileCopyrightText: Copyright (c) 2019-2023 Lars Melchior and contributors
+
+set(CPM_DOWNLOAD_VERSION 0.40.2)
+set(CPM_HASH_SUM "c8cdc32c03816538ce22781ed72964dc864b2a34a310d3b7104812a5ca2d835d")
+
+if(CPM_SOURCE_CACHE)
+  set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake")
+elseif(DEFINED ENV{CPM_SOURCE_CACHE})
+  set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake")
+else()
+  set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake")
+endif()
+
+# Expand relative path. This is important if the provided path contains a tilde (~)
+get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE)
+
+file(DOWNLOAD
+     https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake
+     ${CPM_DOWNLOAD_LOCATION} EXPECTED_HASH SHA256=${CPM_HASH_SUM}
+)
+
+include(${CPM_DOWNLOAD_LOCATION})
diff --git a/terrain_planner_benchmark/launch/run_dynamic_rallypoints.launch b/terrain_planner_benchmark/launch/run_dynamic_rallypoints.launch
@@ -8,6 +8,7 @@
     <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="output_directory" value="$(find terrain_planner_benchmark)/../output"/>
     </node>

diff --git a/terrain_planner_benchmark/src/run_dynamic_rallypoints.cpp b/terrain_planner_benchmark/src/run_dynamic_rallypoints.cpp
@@ -55,6 +55,9 @@
 #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)) {
   visualization_msgs::MarkerArray msg;
 
@@ -288,16 +291,27 @@ int main(int argc, char** argv) {
   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);
 
-  std::string map_path, color_file_path, output_directory, location;
+  std::string map_path, color_file_path, output_directory, location, mission_file_path;
   nh_private.param<std::string>("map_path", map_path, "");
   nh_private.param<std::string>("color_file_path", color_file_path, "");
   nh_private.param<std::string>("location", location, "");
+  nh_private.param<std::string>("mission_file_path", mission_file_path, "");
   nh_private.param<std::string>("output_directory", output_directory, "");
 
   // Initialize data logger for recording
   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);