tuning changes, log addition, timestamp fix

config/ouster_jackal_default.yaml

@@ -3,23 +3,23 @@
     log_to_file: true
     log_debug: true
     log_enabled:
-      #- navigation
+      - navigation
       #- navigation.graph_map_server
       #- navigation.command
         # tactic
       #- tactic
       #- tactic.pipeline
-      # - tactic.module # GOOD speed on to check
+      - tactic.module # GOOD speed on to check
       #- tactic.module.live_mem_manager
       #- tactic.module.graph_mem_manager
       #- mission.state_machine
       - mission.server
-      - navigator
+      # - navigator
         # planner manager
       - cbit.control
-      - cbit.debug
+      # - cbit.debug
       # - cbit.path_planning
-      - cbit.visualization
+      # - cbit.visualization
       #- cbit.obstacle_filtering
       #- grizzly_controller_tests.cbit
         # mpc
@@ -67,13 +67,13 @@
 
       chain:
         min_cusp_distance: 1.5
-        angle_weight: 7.0
+        angle_weight: 0.25
         search_depth: 5
         search_back_depth: 10
         distance_warning: 5.0
       save_odometry_result: true
       save_localization_result: true
-      visualize: true
+      visualize: false
       rviz_loc_path_offset:
         - 0.0
         - 0.0
@@ -87,8 +87,8 @@
         - icp
         - mapping
         - vertex_test
-        - intra_exp_merging
-        - dynamic_detection
+        # - intra_exp_merging
+        # - dynamic_detection
         # - inter_exp_merging
         - memory
       localization:
@@ -106,31 +106,31 @@
         filter_warthog: true # look at code: need to be true to filter
         filter_z_min: -0.5
         filter_z_max: 2.5
-        filter_radius: 0.2
+        filter_radius: 0.5 # was 0.2
 
       filtering:
         type:  lidar.preprocessing #lidar.preprocessing_v2
         num_threads: 8
         crop_range: 20.0
 
-        frame_voxel_size: 0.3 # grid subsampling voxel size
+        frame_voxel_size: 0.3 #0.3 # grid subsampling voxel size
 
         vertical_angle_res: 0.0122718463 # (pi/2)/128 for OS0 128
         polar_r_scale: 2.0 # polar_r_scale x vertical_angle_res = nearest neighbor search radius for normal computation
         r_scale: 2.5 # scale down point range by this value after taking log, whatever works
         h_scale: 0.5 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution is 360/1024 ~= 0.3516 degree, so 0.3516 / (90/128) = 0.5
 
-        num_sample1: 12500 # max number of sample after filtering based on planarity
+        num_sample1: 1500 #12500 # max number of sample after filtering based on planarity
         min_norm_score1: 0.95 # min planarity score
 
-        num_sample2: 12500 # max number of sample after filtering based on planarity
+        num_sample2: 1500 #12500 # max number of sample after filtering based on planarity
         min_norm_score2: 0.2 # 0.2 is when the incident angle 5/12 * pi
         min_normal_estimate_dist: 1.0 # minimum distance to estimate normal in meters
         max_normal_estimate_angle: 0.44 # must <1/2, this value will be timed by M_PI
 
-        cluster_num_sample: 12500 # maxnumber of sample after removing isolated points
+        cluster_num_sample: 1500 #12500 # maxnumber of sample after removing isolated points
 
-        visualize: true
+        visualize: false
     odometry:
       icp:
         type: lidar.odometry_icp
@@ -159,7 +159,7 @@
         verbose: false
         max_iterations: 1
         min_matched_ratio: 0.5
-        visualize: true
+        visualize: false
       mapping:
         type: lidar.odometry_map_maintenance_v2
 
@@ -168,7 +168,7 @@
         crop_range_front: 40.0
         back_over_front_ratio: 0.5
         point_life_time: 20.0
-        visualize: true
+        visualize: false
       vertex_test:
         type: lidar.vertex_test
 
@@ -182,7 +182,7 @@
 
         crop_range_front: 40.0
         back_over_front_ratio: 0.5
-        visualize: true
+        visualize: false
       dynamic_detection:
         type: lidar.dynamic_detection
         depth: 12.0
@@ -192,7 +192,7 @@
         max_num_observations: 10000
         min_num_observations: 4
         dynamic_threshold: 0.3
-        visualize: true
+        visualize: false
       inter_exp_merging:
         type: "lidar.inter_exp_merging_v2"
 
@@ -202,15 +202,15 @@
         planar_threshold: 0.2
         normal_threshold: 0.8
         dynamic_obs_threshold: 1
-        visualize: true
+        visualize: false
       memory:
         type: live_mem_manager
         window_size: 5
     localization:
       recall:
         type: lidar.localization_map_recall
         map_version: pointmap
-        visualize: true
+        visualize: false
       icp:
         type: lidar.localization_icp
         use_pose_prior: true
@@ -234,7 +234,7 @@
         resolution: 0.25
         size_x: 16.0
         size_y: 8.0
-        visualize: true
+        visualize: false
       change_detection_sync:
         type: lidar.change_detection_v3
         detection_range: 8.0
@@ -257,7 +257,7 @@
         resolution: 0.25
         size_x: 16.0
         size_y: 8.0
-        visualize: true
+        visualize: false
       change_detection:
         type: lidar.change_detection_v2
         detection_range: 8
@@ -289,7 +289,7 @@
         size_x: 40.0
         size_y: 20.0
         run_async: true
-        visualize: true
+        visualize: false
       ground_extraction:
         type: lidar.ground_extraction
         z_offset: 0.2
@@ -309,7 +309,7 @@
         size_x: 40.0
         size_y: 20.0
         run_async: true
-        visualize: true
+        visualize: false
       terrain_assessment:
         type: lidar.terrain_assessment
         lookahead_distance: 15.0
@@ -320,7 +320,7 @@
         size_y: 20.0
         run_online: false
         run_async: true
-        visualize: true
+        visualize: false
 
     path_planning:
       type: cbit
@@ -358,7 +358,7 @@
         planar_curv_weight: 2.50
         profile_curv_weight: 0.5 
         eop_weight: 1.0
-        min_vel: 0.5
+        min_vel: 1.0 #0.5
 
       mpc:
         # Controller Params
@@ -368,7 +368,7 @@
         horizon_steps: 20
         horizon_step_size: 0.25 # JH: I think this is planning time step which I need to be 0.25s for DRL model.
         forward_vel: 0.85
-        max_lin_vel: 1.0
+        max_lin_vel: 1.35 #1.0
         max_ang_vel: 1.0
         robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration
         robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration

main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp

@@ -34,7 +34,7 @@ class CasadiUnicycleMPC {
     static constexpr int nControl = 2;
     static constexpr double alpha = 0.8;
     static constexpr int N = 15;
-    static constexpr double DT = 0.2;
+    static constexpr double DT = 0.25;
     DM previous_vel{nControl, 1};
     DM T0{nStates, 1};
     std::vector<DM> reference_poses;

main/src/vtr_path_planning/scripts/unicycle_solver.py

@@ -9,20 +9,20 @@
 # Pose Covariance
 Q_x = 10
 Q_y = 10
-Q_theta = 10
+Q_theta = 1
 # Command Covariance
-R1 = 1
-R2 = 10
+R1 = 1.0 #0.1
+R2 = 1.0 #0.1
 
 # Acceleration Cost Covariance
-Acc_R1 = 200.0
-Acc_R2 = 0.01
+Acc_R1 = 2.0
+Acc_R2 = 0.5 #0.01
 
-step_horizon = 0.2  # time between steps in seconds
+step_horizon = 0.25  # time between steps in seconds
 N = 15           # number of look ahead steps
 
 # The first order lag weighting for the angular velocity
-alpha = 0.8
+alpha = 0.4
 
 # state symbolic variables
 x = ca.SX.sym('x')
@@ -76,7 +76,7 @@
 R_acc = ca.diagcat(Acc_R1, Acc_R2)
 
 
-RHS = ca.vertcat(v*cos(theta), v*sin(theta), omega)
+RHS = ca.vertcat(v*cos(theta), v*sin(theta), last_omega*alpha + (1-alpha)*omega)
 motion_model = ca.Function('motion_model', [states, controls, last_controls], [RHS])
 # RHS_angle = ca.vertcat(x + v/omega*sin(theta + omega*step_horizon) - v/omega*sin(theta), 
 #                  y + v/omega*cos(theta) - v/omega*cos(theta + omega*step_horizon), 

main/src/vtr_path_planning/src/cbit/cbit.cpp

@@ -275,7 +275,7 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command {
     << "Final control command: [" << command.linear.x << ", "
     << command.linear.y << ", " << command.linear.z << ", "
     << command.angular.x << ", " << command.angular.y << ", "
-    << command.angular.z << "] for timestamp: " << robot_state.chain->leaf_stamp();
+    << command.angular.z << "] for timestamp: " << prev_cost_stamp_;
 
   return command;
 }

main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp

@@ -73,7 +73,8 @@ std::map<std::string, casadi::DM> CasadiUnicycleMPC::solve(const Config& mpcConf
   auto stats = solve_mpc.stats();
 
   if(stats["success"].as_bool() == false) { 
-    throw std::logic_error("Casadi was unable to find a feasible solution. Barrier constraint likely violated");
+    CLOG(WARNING, "mpc.solver") << "Casadi error: " << stats["return_status"];
+    // throw std::logic_error("Casadi was unable to find a feasible solution. Barrier constraint likely violated");
   }
 
   std::map<std::string, DM> output;