Merge pull request #194 from utiasASRL/warthog_update

Add Warthog Configuration

config/ouster_warthog_default.yaml

@@ -0,0 +1,379 @@
+/**:
+  ros__parameters:
+    log_to_file: true
+    log_debug: true
+    log_enabled:
+        # navigator
+      - navigation
+      #- navigation.graph_map_server
+      #- navigation.command
+
+        # tactic
+      #- tactic
+      - tactic.pipeline
+      #- tactic.module
+      #- tactic.module.live_mem_manager
+      #- tactic.module.graph_mem_manager
+
+        # path planner
+      #- path_planning
+      #- path_planning.cbit
+      #- path_planning.cbit_planner
+      - mpc.cbit
+      - mpc_debug
+      #- obstacle detection.cbit
+      - grizzly_controller_tests.cbit
+      #- ouster
+
+        # mission planner
+      #- mission.server
+      #- mission.state_machine
+
+        # pose graph
+      #- pose_graph
+
+        # lidar pipeline
+      - lidar.pipeline
+      #- lidar.preprocessing
+      #- lidar.ouster_converter
+      - lidar.odometry_icp
+      #- lidar.odometry_map_maintenance
+      - lidar.vertex_test
+      #- lidar.localization_map_recall
+      - lidar.localization_icp
+      - lidar.intra_exp_merging
+      #- lidar.dynamic_detection
+      - lidar.inter_exp_merging
+      #- lidar.ground_extraction
+      #- lidar.obstacle_detection
+      #- lidar.terrain_assessment
+
+    robot_frame: base_link
+    env_info_topic: env_info
+    lidar_frame: os_sensor
+    lidar_topic: /ouster/points
+    queue_size: 1
+    graph_map:
+      origin_lat: 43.7822
+      origin_lng: -79.4661
+      origin_theta: 1.3
+      scale: 1.0
+    tactic:
+      enable_parallelization: true
+      preprocessing_skippable: false
+      odometry_mapping_skippable: false
+      localization_skippable: false
+      task_queue_num_threads: 1
+      task_queue_size: -1
+
+      route_completion_translation_threshold: 0.5
+
+      chain:
+        min_cusp_distance: 1.5
+        angle_weight: 7.0
+        search_depth: 5
+        search_back_depth: 10
+        distance_warning: 5.0
+      save_odometry_result: true
+      save_localization_result: true
+      visualize: true
+      rviz_loc_path_offset:
+        - 0.0
+        - 0.0
+        - 0.0
+    pipeline:
+      type: lidar
+      preprocessing:
+        - conversion
+        - filtering
+      odometry:
+        - icp
+        - mapping
+        - vertex_test
+        - intra_exp_merging
+        - dynamic_detection
+        - inter_exp_merging
+        - memory
+      localization:
+        - recall
+        - icp
+        - safe_corridor
+        - change_detection_sync
+        - memory
+      submap_translation_threshold: 1.5
+      submap_rotation_threshold: 30.0
+    preprocessing:
+      conversion:
+        type: lidar.ouster_converter
+        visualize: false
+        filter_warthog: true
+        filter_z_min: -0.2
+        filter_z_max: 0.35
+        filter_radius: 0.8
+      filtering:
+        type: lidar.preprocessing
+        num_threads: 8
+        crop_range: 40.0
+
+        frame_voxel_size: 0.3 # grid subsampling voxel size
+
+        vertical_angle_res: 0.0061365 # vertical angle resolution in radians, equal to 0.3516 degree documented in the manual
+        polar_r_scale: 2.0 # polar_r_scale x vertical_angle_res = nearest neighbor search radius for normal computation
+        r_scale: 4.0 # scale down point range by this value after taking log, whatever works
+        h_scale: 2.0 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution when 5Hz spin frequence is ~0.7031 degree, so 0.7031 / 0.3516 = 2.00
+
+        num_sample1: 20000 # max number of sample after filtering based on planarity
+        min_norm_score1: 0.95 # min planarity score
+
+        num_sample2: 20000 # 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: 20000 # maxnumber of sample after removing isolated points
+
+        visualize: true
+    odometry:
+      icp:
+        type: lidar.odometry_icp
+
+        # continuous time estimation
+        use_trajectory_estimation: false
+        traj_num_extra_states: 0
+        traj_lock_prev_pose: false
+        traj_lock_prev_vel: false
+        traj_qc_diag:
+          - 1.0
+          - 0.1
+          - 0.1
+          - 0.1
+          - 0.1
+          - 1.0
+        num_threads: 8
+        first_num_steps: 2
+        initial_max_iter: 4
+        initial_max_pairing_dist: 1.5
+        initial_max_planar_dist: 1.0
+        refined_max_iter: 50
+        refined_max_pairing_dist: 1.0
+        refined_max_planar_dist: 0.3
+        averaging_num_steps: 2
+        verbose: false
+        max_iterations: 1
+        min_matched_ratio: 0.85
+        visualize: true
+      mapping:
+        type: lidar.odometry_map_maintenance_v2
+
+        map_voxel_size: 0.3
+
+        crop_range_front: 40.0
+        back_over_front_ratio: 0.5
+        point_life_time: 20.0
+        visualize: true
+      vertex_test:
+        type: lidar.vertex_test
+
+        max_translation: 0.3
+        max_rotation: 10.0
+      intra_exp_merging:
+        type: lidar.intra_exp_merging_v2
+        depth: 6.0
+
+        map_voxel_size: 0.3
+
+        crop_range_front: 40.0
+        back_over_front_ratio: 0.5
+        visualize: true
+      dynamic_detection:
+        type: lidar.dynamic_detection
+        depth: 12.0
+
+        horizontal_resolution: 0.0122718 # 0.02042
+        vertical_resolution: 0.00613587 # 0.01326
+        max_num_observations: 2000
+        min_num_observations: 4
+        dynamic_threshold: 0.3
+        visualize: true
+      inter_exp_merging:
+        type: "lidar.inter_exp_merging_v2"
+
+        map_voxel_size: 0.3
+        max_num_experiences: 128
+        distance_threshold: 0.6
+        planar_threshold: 0.2
+        normal_threshold: 0.8
+        dynamic_obs_threshold: 1
+        visualize: true
+      memory:
+        type: live_mem_manager
+        window_size: 5
+    localization:
+      recall:
+        type: lidar.localization_map_recall
+        map_version: pointmap
+        visualize: true
+      icp:
+        type: lidar.localization_icp
+        use_pose_prior: true
+        num_threads: 8
+        first_num_steps: 2
+        initial_max_iter: 4
+        initial_max_pairing_dist: 1.5
+        initial_max_planar_dist: 1.0
+        refined_max_iter: 50
+        refined_max_pairing_dist: 1.0
+        refined_max_planar_dist: 0.3
+        averaging_num_steps: 2
+        verbose: false
+        max_iterations: 1
+        min_matched_ratio: 0.65
+      safe_corridor:
+        type: lidar.safe_corridor
+        lookahead_distance: 5.0
+        corridor_width: 3.5
+        influence_distance: 1.0
+        resolution: 0.25
+        size_x: 16.0
+        size_y: 8.0
+        visualize: true
+      change_detection_sync:
+        type: lidar.change_detection_v3
+        detection_range: 8.0
+        search_radius: 0.25
+
+        negprob_threshold: 0.015 # was 0.015 # -1.86 without prior, 0.015 with prior # Jordy: I found I needed to bump this up abit (approx 0.075+) to reduce false positives
+        use_prior: true
+        alpha0: 3.0
+        beta0: 0.03
+        use_support_filtering: true
+        support_radius: 0.25
+        support_variance: 0.05
+        support_threshold: 2.5
+
+        influence_distance: 0.6 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist!
+        minimum_distance: 0.9 # was 0.3 Jordy
+
+        # cost map
+        costmap_history_size: 15 # Keep between 3 and 20, used for temporal filtering
+        resolution: 0.25
+        size_x: 16.0
+        size_y: 8.0
+        visualize: true
+      memory:
+        type: graph_mem_manager
+        vertex_life_span: 5
+        window_size: 3
+      obstacle_detection:
+        type: lidar.obstacle_detection
+        z_min: 0.5
+        z_max: 2.0
+        resolution: 0.6
+        size_x: 40.0
+        size_y: 20.0
+        run_async: true
+        visualize: true
+      ground_extraction:
+        type: lidar.ground_extraction
+        z_offset: 0.2
+        alpha: 0.035
+        tolerance: 0.25
+        Tm: 0.3
+        Tm_small: 0.1
+        Tb: 0.5
+        Trmse: 0.1
+        Tdprev: 1.0
+        rmin: 2.0
+        num_bins_small: 30.0
+        bin_size_small: 0.5
+        num_bins_large: 10.0
+        bin_size_large: 1.0
+        resolution: 0.6
+        size_x: 40.0
+        size_y: 20.0
+        run_async: true
+        visualize: true
+      terrain_assessment:
+        type: lidar.terrain_assessment
+        lookahead_distance: 15.0
+        corridor_width: 1.0
+        search_radius: 1.0
+        resolution: 0.5
+        size_x: 40.0
+        size_y: 20.0
+        run_online: false
+        run_async: true
+        visualize: true
+
+    path_planning:
+      type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version
+      control_period: 100 # ms
+      cbit:
+        obs_padding: 0.0
+        curv_to_euclid_discretization: 10
+        sliding_window_width: 12.0
+        sliding_window_freespace_padding: 0.5
+        corridor_resolution: 0.2
+        state_update_freq: 2.0
+        update_state: true
+        rand_seed: 1
+
+        # Planner Tuning Params
+        initial_samples: 400
+        batch_samples: 200
+        pre_seed_resolution: 0.5
+        alpha: 0.25
+        q_max: 2.5
+        frame_interval: 50
+        iter_max: 10000000
+        eta: 1.0
+        rad_m_exhange: 1.00
+        initial_exp_rad: 0.75
+        extrapolation: false
+        incremental_plotting: false
+        plotting: true
+      costmap:
+        costmap_filter_value: 0.01
+        costmap_history: 15 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now
+
+      mpc:
+        # Controller Params
+        horizon_steps: 20
+        horizon_step_size: 0.3
+        forward_vel: 1.0
+        max_lin_vel: 1.25
+        max_ang_vel: 2.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.8 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration
+
+        vehicle_model: "unicycle"
+
+        # Cost Function Covariance Matrices
+        pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0]
+        vel_error_cov: [100.0, 200.0] # was [0.1 2.0] # No longer using this cost term
+        acc_error_cov: [100.0, 200.0]
+        kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
+        lat_error_cov: [20.0]
+
+        # Cost Function Covariance Matrices (Tracking Backup)
+        #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0]
+        #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term
+        #acc_error_cov: [10.0, 10.0]
+        #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
+        #lat_error_cov: [10.0]
+
+        # Cost Function Weights
+        pose_error_weight: 1.0
+        vel_error_weight: 1.0
+        acc_error_weight: 1.0
+        kin_error_weight: 1.0
+        lat_error_weight: 0.01
+
+        #backup for tracking mpc
+        #pose_error_cov: [0.5, 0.5, 0.5, 10.0, 10.0, 10.0] # Used to have yaw components set to 1000 but it seems to cause some instability, others set to 1.0
+        #vel_error_cov: [0.1, 1.0] # was [0.1 2.0]
+        #acc_error_cov: [0.1, 0.75]
+        #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
+
+        # Misc
+        command_history_length: 100