modify graphnav for challenge

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

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;
+};

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);