Dynamic rally points

Author: Jaeyoung-Lim

terrain_planner_benchmark/src/run_dynamic_rallypoints.cpp

@@ -428,7 +428,7 @@ int main(int argc, char** argv) {
   /// Visualize Geofence
   publishGeoFence(geofence_pub, geofence_polygon);
 
-  ///TODO: Parse mission to generate reference path
+  // Parse mission to generate reference path
   std::vector<Eigen::Vector3d> waypoint_list;
   json mission = data["mission"]["items"];
   for (auto mission_item : mission) {
@@ -440,7 +440,6 @@ int main(int argc, char** argv) {
         const double waypoint_lat = mission_item["params"][4];
         const double waypoint_lon = mission_item["params"][5];
         const double waypoint_relative_alt = mission_item["params"][6];
-        Eigen::Vector3d waypoint_wgs84(waypoint_lat, waypoint_lon, waypoint_relative_alt);
         Eigen::Vector3d waypoint_lv03;
         GeoConversions::forward(waypoint_lat, waypoint_lon, 0.0, waypoint_lv03.x(), waypoint_lv03.y(),
                                 waypoint_lv03.z());  /// FIXME: Assuming zero AMSL
@@ -457,6 +456,30 @@ int main(int argc, char** argv) {
 
   publishWaypoints(waypoint_pub, waypoint_list);
 
+  /// Parse rally points from mission file  
+  std::vector<Eigen::Vector3d> rallypoint_list;
+  const double map_width_x = terrain_map->getGridMap().getLength().x();
+  const double map_width_y = terrain_map->getGridMap().getLength().y();
+  const Eigen::Vector2d map_pos = terrain_map->getGridMap().getPosition();
+
+  json mission_rallypoints = data["rallyPoints"]["points"];
+  for (auto rallypoint : mission_rallypoints) {
+    std::cout << "  - Rallypoint: " << rallypoint << std::endl;
+    const double rallypoint_lat = rallypoint[0];
+    const double rallypoint_lon = rallypoint[1];
+    const double rallypoint_relative_alt = rallypoint[2];
+    Eigen::Vector3d rallypoint_lv03;
+    GeoConversions::forward(rallypoint_lat, rallypoint_lon, 0.0, rallypoint_lv03.x(), rallypoint_lv03.y(),
+                            rallypoint_lv03.z());  /// FIXME: Assuming zero AMSL
+    ESPG map_coordinate;
+    Eigen::Vector3d map_origin;
+    terrain_map->getGlobalOrigin(map_coordinate, map_origin);
+    Eigen::Vector3d rallypoint_local = rallypoint_lv03 - map_origin;
+    rallypoint_local.z() = terrain_map->getGridMap().atPosition("elevation", rallypoint_local.head(2)) + rallypoint_relative_alt;
+    rallypoint_list.push_back(rallypoint_local);
+  }
+  publishRallyPoints(rallypoint_pub, rallypoint_list, radius, Eigen::Vector3d(1.0, 0.0, 0.0));
+
   terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec());
   publishGridMap(grid_map_pub, terrain_map->getGridMap());
 
@@ -468,10 +491,6 @@ int main(int argc, char** argv) {
   /// TODO: Get bounds from gridmap
   planner->setBoundsFromMap(terrain_map->getGridMap());
 
-  const double map_width_x = terrain_map->getGridMap().getLength().x();
-  const double map_width_y = terrain_map->getGridMap().getLength().y();
-  const Eigen::Vector2d map_pos = terrain_map->getGridMap().getPosition();
-
   Eigen::Vector3d start{Eigen::Vector3d(map_pos(0) + 0.4 * map_width_x, map_pos(1) - 0.35 * map_width_y, 0.0)};
   Eigen::Vector3d updated_start;
   if (validatePosition(terrain_map, start, updated_start)) {
@@ -509,28 +528,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;
-  std::vector<Eigen::Vector3d> rally_points;
-  for (int i = 0; i < num_rally_points; i++) {
-    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));
-      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);
-      if (sample_is_valid) {
-        rally_points.push_back(new_loiter_position);
-      }
-    }
-  }
-  publishRallyPoints(rallypoint_pub, rally_points, radius, Eigen::Vector3d(1.0, 1.0, 0.0));
-
   std::cout << "Successfully Generated rally points" << std::endl;
 
   /// TODO: Iterate over path segments to find abort paths
@@ -540,7 +537,7 @@ int main(int argc, char** argv) {
     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;
-    planner->setupProblem(segment_end, segment_end_tangent, rally_points);
+    planner->setupProblem(segment_end, segment_end_tangent, rallypoint_list);
     Path abort_path;
     if (planner->Solve(10.0, abort_path)) {
       std::cout << "[TestRRTCircleGoal] Found Solution!" << std::endl;