From 8209c4abfc9dd535c8f73cded0614f000ba88771 Mon Sep 17 00:00:00 2001
From: zichao <zichao2018@gmail.com>
Date: Mon, 13 Mar 2023 15:17:22 -0500
Subject: [PATCH] modify graphnav for challenge

---
 CMakeLists.txt                    | 16 ++++++++--------
 config/navigation.lua             | 22 ++++++++++++----------
 src/navigation/navigation_main.cc |  2 +-
 src/shared                        |  2 +-
 4 files changed, 22 insertions(+), 20 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index bacb688..10ee905 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -75,14 +75,14 @@ TARGET_LINK_LIBRARIES(navigation
     shared_library ${libs} ${TORCH_LIBRARIES} ${OpenCV_LIBS})
 
 
-ADD_EXECUTABLE(navigation_tests
-    src/navigation/navigation_tests.cc
-    src/navigation/motion_primitives.cc
-)
-TARGET_LINK_LIBRARIES(navigation_tests
-    gtest
-    gtest_main
-    ${libs})
+# ADD_EXECUTABLE(navigation_tests
+#     src/navigation/navigation_tests.cc
+#     src/navigation/motion_primitives.cc
+# )
+# TARGET_LINK_LIBRARIES(navigation_tests
+#     gtest
+#     gtest_main
+#     ${libs})
 
 ROSBUILD_ADD_EXECUTABLE(social_nav
                         src/navigation/social_nav.cc
diff --git a/config/navigation.lua b/config/navigation.lua
index 127fb10..ef83933 100644
--- a/config/navigation.lua
+++ b/config/navigation.lua
@@ -6,23 +6,23 @@ NavigationParameters = {
   laser_topic = "/velodyne_2dscan";
   odom_topic = "/jackal_velocity_controller/odom";
   localization_topic = "localization";
-  image_topic = "/camera/rgb/image_raw/compressed";
   init_topic = "initialpose";
   enable_topic = "autonomy_arbiter/enabled";
+  image_topic = "/camera/rgb/image_raw/compressed";
   laser_loc = {
-    x = 0.065;
+    x = 0.12;
     y = 0;
   };
   dt = 0.025;
   max_linear_accel = 0.5;
   max_linear_decel = 0.5;
-  max_linear_speed = 0.5;
-  max_angular_accel = 0.5;
-  max_angular_decel = 0.5;
-  max_angular_speed = 1.0;
-  carrot_dist = 10;
+  max_linear_speed = 2.0;
+  max_angular_accel = 5;
+  max_angular_decel = 5;
+  max_angular_speed = 5.0;
+  carrot_dist = 1;
   system_latency = 0.24;
-  obstacle_margin = 0.15;
+  obstacle_margin = 0.05;
   num_options = 31;
   robot_width = 0.44;
   robot_length = 0.5;
@@ -34,7 +34,8 @@ NavigationParameters = {
   target_dist_tolerance = 0.1;
   target_vel_tolerance = 0.1;
   target_angle_tolerance = 0.05;
-  local_fov = deg2rad(90);
+
+  local_fov = deg2rad(60);
   use_kinect = true;
   camera_calibration_path = "config/camera_calibration_kinect.yaml";
   model_path = "../preference_learning_models/jit_cost_model_outdoor_6dim.pt";
@@ -44,4 +45,5 @@ NavigationParameters = {
 AckermannSampler = {
   max_curvature = 2.5;
   clearance_path_clip_fraction = 0.8;
-};
+  clip_cpoa = true;
+};
\ No newline at end of file
diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
index 79caa7a..bed5fb9 100644
--- a/src/navigation/navigation_main.cc
+++ b/src/navigation/navigation_main.cc
@@ -113,7 +113,7 @@ CONFIG_FLOAT(laser_loc_x, "NavigationParameters.laser_loc.x");
 CONFIG_FLOAT(laser_loc_y, "NavigationParameters.laser_loc.y");
 
 DEFINE_string(map, "UT_Campus", "Name of navigation map file");
-DEFINE_string(twist_drive_topic, "navigation/cmd_vel", "Drive Command Topic");
+DEFINE_string(twist_drive_topic, "cmd_vel", "Drive Command Topic");
 DEFINE_bool(debug_images, false, "Show debug images");
 
 // DECLARE_int32(v);
diff --git a/src/shared b/src/shared
index 77c713d..2b5257c 160000
--- a/src/shared
+++ b/src/shared
@@ -1 +1 @@
-Subproject commit 77c713d10be17807e665e71e479f850e2be88a34
+Subproject commit 2b5257c3dd5e8e47498092bc42c19683fc79e4b0