fixed sizing issue in terrain2

.gitignore

@@ -12,6 +12,7 @@ src/graph_navigation/__init__.py
 terrain_models
 terrain_models/*
 *.bag
+*.png
 
 # Prerequisites
 *.d

Makefile

@@ -14,7 +14,7 @@ docker_all: docker_build_q
 	docker run --rm --volume "$(shell pwd)":/home/dev/graph_navigation graph_navigation "cd graph_navigation && make -j"
 
 docker_shell: docker_build_q
-	if [ $(shell docker ps -a -f name=graph_navigation_shell | wc -l) -ne 2 ]; then docker run -dit --name graph_navigation_shell --volume "$(shell pwd)":/home/dev/graph_navigation --workdir /home/dev/graph_navigation -p 10272:10272 graph_navigation; fi
+	if [ $(shell docker ps -a -f name=graph_navigation_shell | wc -l) -ne 2 ]; then xhost +local:root; docker run -dit --name graph_navigation_shell --volume "$(shell pwd)":/home/dev/graph_navigation --workdir /home/dev/graph_navigation -p 10272:10272 -e DISPLAY=$(DISPLAY) -v /tmp/.X11-unix:/tmp/.X11-unix graph_navigation; fi
 	docker exec -it graph_navigation_shell bash -l
 
 docker_stop:

config/navigation_spot_icl.lua

@@ -4,7 +4,8 @@ end
 
 NavigationParameters = {
   laser_topic = "velodyne_2dscan_highbeams";
-  odom_topic = "odom";
+  odom_topic = "/odom";
+  -- odom_topic = "/odometry/filtered";
   localization_topic = "localization";
   image_topic = "/bev/single/compressed";
   init_topic = "initialpose";
@@ -24,13 +25,14 @@ NavigationParameters = {
   carrot_dist = 250;  -- large carrot distance so the goal does not latch onto the map
   system_latency = 0.24;
   obstacle_margin = 0.15;
-  num_options = 31;
+  num_options = 11;
   -- num_options = 15;
   robot_width = 0.44;
   robot_length = 0.5;
   base_link_offset = 0;
-  -- max_free_path_length = 4.0;
-  max_free_path_length = 3.5;  -- ahg demo
+  max_free_path_length = 4.0;
+  -- max_free_path_length = 3.5;  -- ahg demo
+  -- max_free_path_length = 1;  -- ahg demo
   max_clearance = 1.0;
   can_traverse_stairs = false;
   evaluator_type = "terrain2";
@@ -46,11 +48,12 @@ NavigationParameters = {
 
 AckermannSampler = {
   max_curvature = 2.5;
+  -- max_curvature = 5;
   clearance_path_clip_fraction = 0.8;
 };
 
 TerrainEvaluator = {
-  patch_size_pixels = 64;
+  patch_size_pixels = 1;
   bev_pixels_per_meter = 150;
   min_cost = 0.0;
   max_cost = 1;
@@ -62,7 +65,8 @@ TerrainEvaluator = {
 
   -- dist_to_goal_weight = -0.2;
   -- dist_to_goal_weight = -0.7;
-  dist_to_goal_weight = -2.0;
+  -- dist_to_goal_weight = -2.0;
+  dist_to_goal_weight = 0;
 
   clearance_weight = 0; -- -0.25;
   fpl_weight = 0; -- -0.75;

src/navigation/terrain_evaluator.cc

@@ -187,7 +187,12 @@ std::shared_ptr<PathRolloutBase> TerrainEvaluator::FindBest(
   // cv::imwrite("latest_vis.png", latest_vis_image_);
   // cv::cvtColor(latest_vis_image_, latest_vis_image_, cv::COLOR_BGR2RGB);
   // cv::imwrite("latest_cost.png", latest_cost_image_);
-  // exit(0);
+  // static int functionCallCount = 0;
+  // functionCallCount++;
+  // if (functionCallCount > 1) {
+  //   std::cout << "Exiting here" << std::endl;
+  //   exit(0);
+  // }
 
 
   return best_path;
@@ -375,19 +380,25 @@ void TerrainEvaluator::DrawPathCosts(const std::vector<std::shared_ptr<PathRollo
         color[1] = 255.0 * (2 - 2 * normalized_path_costs[i]);
       }
 
-      int thickness = 0.5;
+      int thickness = 2;
       if (paths[i] == best_path) {
         // a negative thickness value fills in the drawn circle
         thickness = -thickness;
       }
-      cv::circle(latest_vis_image_, cv::Point(P_image_state.x(), P_image_state.y()), 2, color,
+      cv::circle(latest_vis_image_, cv::Point(P_image_state.x(), P_image_state.y()), 8, color,
                  thickness, cv::LineTypes::LINE_AA);
     }
+
+    // print normalized path costs[i]
+    std::cout << "Normalized path costs " << normalized_path_costs[i] << std::endl;
   }
 
   const Eigen::Vector2f P_image_goal = GetImageLocation(latest_vis_image_, local_target);
   cv::drawMarker(latest_vis_image_, cv::Point(P_image_goal.x(), P_image_goal.y()), {255, 255, 255},
-                 cv::MARKER_TILTED_CROSS, 32, 4, cv::LineTypes::LINE_AA);
+                 cv::MARKER_TILTED_CROSS, 32, 8, cv::LineTypes::LINE_AA);
+
+  // print he best path and its cost
+  std::cout << "Best path " << best_path << " " << normalized_path_costs[0] << std::endl;
 
 }
 

src/navigation/terrain_evaluator2.h

@@ -20,13 +20,35 @@ class CustomTerrainEvaluator : public TerrainEvaluator {
 
   cv::Mat1f GetScalarCostImage(const cv::Mat3b& bev_image) override {
     // print here
-    std::cout << "Custom GetScalarCostImage" << std::endl;
+    // std::cout << "Custom GetScalarCostImage" << std::endl;
     cv::Mat3b latest_bev_image = image.clone();
     // print the shape
     std::cout << "latest bev image " << latest_bev_image.rows << " " << latest_bev_image.cols << std::endl;
+    int rows = latest_bev_image.rows;
+    int cols = latest_bev_image.cols;
     // print the number of channels
     std::cout << "latest bev image channels " << latest_bev_image.channels() << std::endl;
 
+    // // for debugging
+    // cv::Mat1f scalar_cost_map(rows, cols);
+    // // make the left half of the image black and the right half white
+    // for (int i = 0; i < scalar_cost_map.rows; i++) {
+    //   for (int j = 0; j < scalar_cost_map.cols; j++) {
+    //     if (j < scalar_cost_map.cols / 2) {
+    //       scalar_cost_map.at<float>(i, j) = 0;
+    //     } else {
+    //       scalar_cost_map.at<float>(i, j) = 1;
+    //     }
+    //   }
+    // }
+
+    // // write the latest_bev_image to a file
+    // cv::imwrite("latest_bev_image.png", latest_bev_image);
+    // // write the scalar_cost_map to a file
+    // cv::imwrite("scalar_cost_map.png", scalar_cost_map*255);
+    // return scalar_cost_map;
+
+
     // resize the image
     cv::resize(latest_bev_image, latest_bev_image, cv::Size(256, 128));
 
@@ -39,18 +61,18 @@ class CustomTerrainEvaluator : public TerrainEvaluator {
     img_tensor = img_tensor / 255.0;
 
     // print the context shape
-    std::cout << "Context shape: ";
-    for (auto& size : context_tensor_.sizes()) {
-    std::cout << size << " ";
-    }
-    std::cout << std::endl;
+    // std::cout << "Context shape: ";
+    // for (auto& size : context_tensor_.sizes()) {
+    // std::cout << size << " ";
+    // }
+    // std::cout << std::endl;
 
 
     // print the shape
-    std::cout << "Image shape: ";
-    for (auto& size : img_tensor.sizes()) {
-    std::cout << size << " ";
-    }
+    // std::cout << "Image shape: ";
+    // for (auto& size : img_tensor.sizes()) {
+    // std::cout << size << " ";
+    // }
 
     // print the min and max
     std::cout << std::endl;
@@ -71,6 +93,7 @@ class CustomTerrainEvaluator : public TerrainEvaluator {
     // apply sigmoid to the output
     output = torch::sigmoid(output);
 
+
     // Print the shape of the output
     std::cout << "Output shape: ";
     for (auto& size : output.sizes()) {
@@ -86,13 +109,18 @@ class CustomTerrainEvaluator : public TerrainEvaluator {
     cv::Mat1f scalar_cost_map(output.size(2), output.size(3));
     std::memcpy(scalar_cost_map.data, output.data_ptr(), output.numel() * sizeof(float));
 
+    // resize the scalar_cost_map to the original size
+    cv::resize(scalar_cost_map, scalar_cost_map, cv::Size(cols, rows));
+    // print the new size
+    // std::cout << "Scalar cost map size: " << scalar_cost_map.rows << " " << scalar_cost_map.cols << std::endl;
+
     // todo: out-of-view cost
 
 
 
-    // write the latest_bev_image to a file
+    // // write the latest_bev_image to a file
     // cv::imwrite("latest_bev_image.png", latest_bev_image);
-    // write the scalar_cost_map to a file
+    // // write the scalar_cost_map to a file
     // cv::imwrite("scalar_cost_map.png", scalar_cost_map*255);
 
     // return the output