Merge pull request #179 from utiasASRL/path_planner_dev_merge2

Major Motion Planner Updates Supporting Corridor MPC

Dockerfile

@@ -94,6 +94,11 @@ RUN pip3 install \
   python-socketio[client] \
   websocket-client
 
+RUN mkdir -p ${HOMEDIR}/.matplotcpp && cd ${HOMEDIR}/.matplotcpp \
+  && git clone https://github.com/lava/matplotlib-cpp.git . \
+  && mkdir -p ${HOMEDIR}/.matplotcpp/build && cd ${HOMEDIR}/.matplotcpp/build \
+  && cmake .. && cmake --build . -j${NUMPROC} --target install
+
 RUN apt install htop
 
 # Install vim

config/honeycomb_grizzly_default.yaml

@@ -3,28 +3,30 @@
     log_to_file: true
     log_debug: true
     log_enabled:
-      - lidar.terrain_assessment
-      - navigation
-      - navigation.graph_map_server
-      - navigation.command
+      #- lidar.terrain_assessment
+      #- navigation
+      #- navigation.graph_map_server
+      #- navigation.command
         # tactic
-      - tactic
+      #- tactic
       - tactic.pipeline
-      - tactic.module
-      - tactic.module.live_mem_manager
-      - tactic.module.graph_mem_manager
+      - lidar.preprocessing
+      #- tactic.module
+      #- tactic.module.live_mem_manager
+      #- tactic.module.graph_mem_manager
         # path planner
-        # "path_planning",
-        # "path_planning.teb",
-        # "path_planning.cbit",
-        # "path_planning.cbit_planner",
-        # "mpc.cbit",
-        # "mpc_debug.cbit",
+        #"path_planning",
+        #"path_planning.teb",
+        #"path_planning.cbit",
+        #"path_planning.cbit_planner",
+        #"mpc.cbit",
+        #"mpc_debug.cbit",
+        #"obstacle_detection.cbit",
         # mission planner
-      - mission.server
-      - mission.state_machine
+      #- mission.server
+      #- mission.state_machine
         # pose graph
-      - pose_graph
+      #- pose_graph
         # pipeline specific
         # "lidar.pipeline",
         # "lidar.honeycomb_converter",
@@ -37,7 +39,6 @@
         # "lidar.intra_exp_merging",
         # "lidar.dynamic_detection",
         # "lidar.inter_exp_merging",
-        # "lidar.change_detection",
         # "lidar.ground_extraction",
         # "lidar.obstacle_detection",
       # "lidar.terrain_assessment",
@@ -57,7 +58,9 @@
       localization_skippable: false
       task_queue_num_threads: 1
       task_queue_size: -1
-      route_completion_translation_threshold: 2.0
+
+      route_completion_translation_threshold: 0.5
+
       chain:
         min_cusp_distance: 1.5
         angle_weight: 7.0
@@ -100,23 +103,31 @@
         type: lidar.preprocessing
         num_threads: 8
         crop_range: 40.0
-        frame_voxel_size: 0.2
-        vertical_angle_res: 0.0132645
-        polar_r_scale: 2.0
-        r_scale: 4.0
-        h_scale: 1.54
-        num_sample1: 10000
-        min_norm_score1: 0.95
-        num_sample2: 10000
-        min_norm_score2: 0.2
-        min_normal_estimate_dist: 1.0
-        max_normal_estimate_angle: 0.44
-        cluster_num_sample: 10000
+
+        frame_voxel_size: 0.1 # grid subsampling voxel size
+
+        vertical_angle_res: 0.0132645 # vertical angle resolution in radius, equal to 0.76 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: 1.54 # 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.95 # 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
-        use_trajectory_estimation: true
+
+        # continuous time estimation
+        use_trajectory_estimation: false
         traj_num_extra_states: 0
         traj_lock_prev_pose: false
         traj_lock_prev_vel: false
@@ -142,34 +153,41 @@
         visualize: true
       mapping:
         type: lidar.odometry_map_maintenance_v2
-        map_voxel_size: 0.2
+
+        map_voxel_size: 0.1
+
         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_translation: 1.0 # was 0.3
         max_rotation: 10.0
       intra_exp_merging:
         type: lidar.intra_exp_merging_v2
         depth: 6.0
-        map_voxel_size: 0.2
+
+        map_voxel_size: 0.1
+
         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.041
-        vertical_resolution: 0.026
-        max_num_observations: 2000
+
+        horizontal_resolution: 0.041 # 0.02042
+        vertical_resolution: 0.026 # 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
+        type: "lidar.inter_exp_merging_v2"
+
+        map_voxel_size: 0.1
         max_num_experiences: 128
         distance_threshold: 0.6
         planar_threshold: 0.2
@@ -212,39 +230,25 @@
         type: lidar.change_detection_v3
         detection_range: 8.0
         search_radius: 0.25
-        negprob_threshold: 0.1
+
+        negprob_threshold: 0.25 # 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.55
-        minimum_distance: 0.35
+
+        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: 15 # Keep between 3 and 20, used for temporal filtering
         resolution: 0.25
         size_x: 16.0
         size_y: 8.0
         visualize: true
-      change_detection:
-        type: lidar.change_detection_v2
-        detection_range: 8
-        search_radius: 0.25
-        negprob_threshold: 0.1
-        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
-        resolution: 0.5
-        size_x: 16.0
-        size_y: 8.0
-        run_online: false
-        run_async: true
-        visualize: false
-        save_module_result: false
       memory:
         type: graph_mem_manager
         vertex_life_span: 5
@@ -290,8 +294,8 @@
         run_async: true
         visualize: true
     path_planning:
-      type: cbit
-      control_period: 33
+      type: cbit # teb.lidar for old teb version, cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version
+      control_period: 100 # ms
       teb:
         visualize: true
         extrapolate: false
@@ -310,54 +314,63 @@
         weight_costmap: 100.0
       cbit:
         obs_padding: 0.0
-        curv_to_euclid_discretization: 20
-        sliding_window_width: 10.0
+        curv_to_euclid_discretization: 10
+        sliding_window_width: 12.0
         sliding_window_freespace_padding: 0.5
-        corridor_resolution: 0.05
-        state_update_freq: 0.5
+        corridor_resolution: 0.2
+        state_update_freq: 1.0
         update_state: true
         rand_seed: 1
-        initial_samples: 250
+
+        # Planner Tuning Params
+        initial_samples: 300
         batch_samples: 100
         pre_seed_resolution: 0.5
         alpha: 0.5
         q_max: 2.5
         frame_interval: 50
         iter_max: 10000000
-        eta: 1.1
-        rad_m_exhange: 1.0
+        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
+        costmap_history: 30 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now
+
       mpc:
-        horizon_steps: 40
-        horizon_step_size: 0.1
-        forward_vel: 1.0
-        max_lin_vel: 1.25
-        max_ang_vel: 0.75
-        vehicle_model: unicycle
-        pose_error_cov:
-          - 0.5
-          - 0.5
-          - 0.5
-          - 10.0
-          - 10.0
-          - 10.0
-        vel_error_cov:
-          - 0.1
-          - 1.0
-        acc_error_cov:
-          - 0.1
-          - 0.75
-        kin_error_cov:
-          - 0.001
-          - 0.001
-          - 0.001
-          - 0.001
-          - 0.001
-          - 0.001
+        # Controller Params
+        horizon_steps: 20
+        horizon_step_size: 0.3
+        forward_vel: 1.1
+        max_lin_vel: 1.5
+        max_ang_vel: 1.5
+        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: [30.0, 30.0, 300.0, 300.0, 300.0, 6.50]
+        vel_error_cov: [10.0, 100.0] # was [0.1 2.0] # No longer using this cost term
+        acc_error_cov: [10.0, 10.0]
+        kin_error_cov: [0.004, 0.004, 0.004, 0.004, 0.004, 0.004]
+        lat_error_cov: [20.0]
+
+        # Cost Function Weights
+        pose_error_weight: 5.0
+        vel_error_weight: 5.0
+        acc_error_weight: 5.0
+        kin_error_weight: 1000.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

launch/online_honeycomb_grizzly.launch.yaml

@@ -37,7 +37,7 @@ windows:
   - window_name: rviz2
     layout: main-horizontal
     shell_command_before:
-      - source /opt/ros/galactic/setup.bash
+      - source ${VTRSRC}/main/install/setup.bash
     panes:
-      - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/lidar.rviz
+      - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/honeycomb.rviz
       # - ros2 run rqt_reconfigure rqt_reconfigure

main/src/vtr_gui/vtr_gui/socket_server.py

@@ -128,42 +128,42 @@ def handle_change_env_info(data):
 def handle_server_state(json):
   logger.info('Broadcasting server state')
   server_state = json['server_state']
-  socketio.emit(u"mission/server_state", server_state, broadcast=True)
+  socketio.emit(u"mission/server_state", server_state)
 
 
 @socketio.on('notification/graph_state')
 def handle_graph_state(json):
   logger.info('Broadcasting graph state')
   graph_state = json['graph_state']
-  socketio.emit(u"graph/state", graph_state, broadcast=True)
+  socketio.emit(u"graph/state", graph_state)
 
 
 @socketio.on('notification/graph_update')
 def handle_graph_update(json):
   logger.info('Broadcasting graph update')
   graph_update = json['graph_update']
-  socketio.emit(u"graph/update", graph_update, broadcast=True)
+  socketio.emit(u"graph/update", graph_update)
 
 
 @socketio.on('notification/robot_state')
 def handle_robot_state(json):
   logger.info('Broadcasting robot state')
   robot_state = json['robot_state']
-  socketio.emit(u"robot/state", robot_state, broadcast=True)
+  socketio.emit(u"robot/state", robot_state)
 
 
 @socketio.on('notification/following_route')
 def handle_following_route(json):
   logger.info('Broadcasting following route')
   following_route = json['following_route']
-  socketio.emit(u"following_route", following_route, broadcast=True)
+  socketio.emit(u"following_route", following_route)
 
 
 @socketio.on('notification/task_queue_update')
 def handle_task_queue_update(json):
   logger.info('Broadcasting task queue update')
   task_queue_update = json['task_queue_update']
-  socketio.emit(u"task_queue/update", task_queue_update, broadcast=True)
+  socketio.emit(u"task_queue/update", task_queue_update)
 
 
 def main():