Perspective Image

config/hdl64_grizzly_learned.yaml

@@ -0,0 +1,342 @@
+/**:
+  ros__parameters:
+    log_to_file: true
+    log_debug: true
+    log_enabled:
+      - navigation
+      - navigation.graph_map_server
+      #- navigation.command
+      #- navigation.sensor_input
+        # 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.cbit
+        # mission planner
+      #- mission.server
+      #- mission.state_machine
+        # pose graph
+      - pose_graph
+        # pipeline specific
+      #- lidar.pipeline
+      - lidar.velodyne_converter_v2
+      - lidar.preprocessing
+      - lidar.odometry_icp
+      - lidar.range
+        # "lidar.odometry_map_maintenance",
+      #- lidar.vertex_test
+      - lidar.range_change
+        # "lidar.localization_map_recall",
+      - lidar.localization_icp
+        # "lidar.intra_exp_merging",
+        # "lidar.dynamic_detection",
+        # "lidar.inter_exp_merging",
+        # "lidar.change_detection",
+        # "lidar.ground_extraction",
+        # "lidar.obstacle_detection",
+    robot_frame: base_link
+    env_info_topic: env_info
+    # lidar related
+    lidar_frame: velodyne
+    lidar_topic: /velodyne_points
+
+    ############ map projection configuration ############
+    graph_projection:
+      origin_lat: 43.78220 # PETAWAWA: 45.8983, # UTIAS: 43.78220
+      origin_lng: -79.4661 # PETAWA: -77.2829, # UTIAS: -79.4661
+      origin_theta: 0.0
+    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
+        - range_change
+        #- change_detection_sync
+        - memory
+      submap_translation_threshold: 1.5
+      submap_rotation_threshold: 30.0
+      save_nn_point_cloud: false
+    preprocessing:
+      conversion:
+        type: lidar.velodyne_converter_v2
+        visualize: true
+        estimate_time: true
+        downsample_ratio: 2
+
+      filtering:
+        type: lidar.preprocessing
+        num_threads: 8
+
+        crop_range: 15.0
+
+        frame_voxel_size: 0.25 # grid subsampling voxel size
+
+        #velodyne lidar has 1/3 degree upper block and 1/2 degree lower block
+        vertical_angle_res: 0.00699 # vertical angle resolution in radians (0.4deg),
+        polar_r_scale: 3.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
+        #horizontal resolution 0.1728degree 0.1728/(0.5)
+        h_scale: 0.0864 # scale  down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution when 5Hz spin frequence is ~1.17 degree, so 1.17 / 0.76 = 1.54
+
+        num_sample1: 10000 # max number of sample after filtering based on planarity
+        min_norm_score1: 0.85 # min planarity score
+
+        num_sample2: 10000 # 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: 10000 # 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: 10
+        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.2
+        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.5
+        max_rotation: 10.0
+      intra_exp_merging:
+        type: lidar.intra_exp_merging_v2
+        depth: 6.0
+        map_voxel_size: 0.2
+        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.00301 # 0.02042
+        vertical_resolution: 0.00699 # 0.01326
+        max_num_observations: 10000
+        min_num_observations: 4
+        dynamic_threshold: 0.3
+        visualize: true
+      inter_exp_merging:
+        type: lidar.inter_exp_merging_v2
+        map_voxel_size: 0.2
+        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: 10
+        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.3
+      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
+      range_change:
+        type: torch.range_change
+        use_gpu: true
+        abs_filepath: false
+        filepath: exported_model_temporal_ground_8m.pt
+        visualize: true
+
+        influence_distance: 0.5 # 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.8 # was 0.3 Jordy
+        resolution: 0.25
+        size_x: 16.0
+        size_y: 16.0
+      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
+      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: stationary # teb.lidar for old teb version, 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.1
+        state_update_freq: 1.0
+        update_state: true
+        rand_seed: 1
+
+        # Planner Tuning Params
+        initial_samples: 400
+        batch_samples: 200
+        pre_seed_resolution: 0.5
+        alpha: 0.5
+        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: 30 # 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: 0.75
+        max_lin_vel: 1.0
+        max_ang_vel: 0.75
+        robot_linear_velocity_scale: 1.1 # 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
+
+        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: [25.0, 10.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: [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

config/honeycomb_grizzly_default.yaml

@@ -93,6 +93,7 @@
         - icp
         - safe_corridor
         - change_detection_sync
+        - costmap_inflation
         - memory
       submap_translation_threshold: 1.5
       submap_rotation_threshold: 30.0
@@ -240,8 +241,10 @@
         support_radius: 0.25
         support_variance: 0.05
         support_threshold: 2.5
-        influence_distance: 0.5 # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist!
-        minimum_distance: 0.8
+      costmap_inflation:
+        type: lidar.costmap_inflation
+        influence_distance: 0.5 # 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.8 # was 0.3 Jordy
 
         # cost map
         costmap_history_size: 30 # Keep between 3 and 30, used for temporal filtering
@@ -295,7 +298,7 @@
         visualize: true
 
     path_planning:
-      type: cbit
+      type: cbit.lidar # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version
       control_period: 100 # ms
       cbit:
         obstacle_avoidance: false