From 58808b8044648bd705b73fc4c868d9e85a7df6f0 Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Mon, 31 Jul 2023 10:17:04 -0400 Subject: [PATCH 01/13] Parameter update for new Ouster OS1 Lidar --- config/ouster_warthog_default.yaml | 372 +++++ launch/online_ouster_warthog.launch.yaml | 42 + main/src/deps/steam | 2 +- rviz/ouster.rviz | 1732 ++++++++++++++++++++++ 4 files changed, 2147 insertions(+), 1 deletion(-) create mode 100644 config/ouster_warthog_default.yaml create mode 100644 launch/online_ouster_warthog.launch.yaml create mode 100644 rviz/ouster.rviz diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml new file mode 100644 index 00000000..2059c2a3 --- /dev/null +++ b/config/ouster_warthog_default.yaml @@ -0,0 +1,372 @@ +/**: + 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 + + # 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 + 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: true + filtering: + type: lidar.preprocessing + num_threads: 8 + crop_range: 40.0 + + frame_voxel_size: 0.2 # 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.5 + visualize: true + mapping: + type: lidar.odometry_map_maintenance_v2 + + 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_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.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.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: 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.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 + change_detection_sync: + type: lidar.change_detection_v3 + detection_range: 8.0 + search_radius: 0.25 + + negprob_threshold: 0.15 # 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.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 + 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.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: 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.5 + max_ang_vel: 1.25 + 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 \ No newline at end of file diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml new file mode 100644 index 00000000..453c381c --- /dev/null +++ b/launch/online_ouster_warthog.launch.yaml @@ -0,0 +1,42 @@ +## Online Lidar VTR3 +session_name: vtr_online_ouster_grizzly + +environment: + CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=ouster_warthog_default.yaml + data_dir:=${VTRTEMP}/lidar/$(date '+%F')/$(date '+%F') + start_new_graph:=false + use_sim_time:=false + + #- htop # monitor hardware usage + + - window_name: vtr_gui + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-horizontal + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/ouster.rviz + # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/deps/steam b/main/src/deps/steam index 63272905..62a2188b 160000 --- a/main/src/deps/steam +++ b/main/src/deps/steam @@ -1 +1 @@ -Subproject commit 632729057b1c80e4f727b9123b5b347bcd114d58 +Subproject commit 62a2188bceb81e056e5156c1b48bd6f20b109f3e diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz new file mode 100644 index 00000000..cc2eeb7b --- /dev/null +++ b/rviz/ouster.rviz @@ -0,0 +1,1732 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1 + - /TF1/Frames1 + - /odometry path1/Shape1 + - /localization path1 + - /localization path1/Topic1 + - /curr map loc1/Topic1 + - /reference path1 + - /reference path1/Topic1 + - /planned path1/Offset1 + - /planned path (poses)1/Topic1 + - /global path1/Offset1 + - /planning change detection scan1 + - /Path2 + - /Corridor Path Left1/Topic1 + - /PoseArray1 + - /PoseArray2 + Splitter Ratio: 0.4507042169570923 + Tree Height: 871 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: robot + Value: false + - Class: rviz_default_plugins/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: world + Value: false + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 99999 + Frames: + All Enabled: false + base_footprint: + Value: true + base_footprint_bumper_part: + Value: true + base_link: + Value: true + chassis_link: + Value: true + front_axle_link: + Value: true + front_bumblebee: + Value: true + front_left_wheel_link: + Value: true + front_right_wheel_link: + Value: true + front_xb3: + Value: true + hc: + Value: true + honeycomb: + Value: true + imu_link: + Value: true + lidar: + Value: false + loc vertex frame: + Value: false + loc vertex frame (offset): + Value: false + m600_base: + Value: true + microstrain: + Value: true + navsat_link: + Value: true + odo vertex frame: + Value: false + odom: + Value: true + planning frame: + Value: false + rear_antenna_link: + Value: true + rear_bumblebee: + Value: true + rear_left_wheel_link: + Value: true + rear_right_wheel_link: + Value: true + rear_sensor_base_link: + Value: true + rear_sensor_plate_link: + Value: true + rear_xb3: + Value: true + robot: + Value: true + robot planning: + Value: false + robot planning (extrapolated): + Value: false + robot planning (extrapolated) mpc: + Value: false + sensor_anchor_link: + Value: true + velodyne: + Value: true + world: + Value: false + world (offset): + Value: false + Marker Scale: 4 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + world: + hc: + {} + loc vertex frame: + {} + odo vertex frame: + robot: + lidar: + {} + planning frame: + robot planning: + {} + robot planning (extrapolated): + {} + robot planning (extrapolated) mpc: + {} + world (offset): + loc vertex frame (offset): + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex21 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 0.09950866550207138 + Min Color: 255; 0; 0 + Min Intensity: -0.09992675483226776 + Name: raw scan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/raw_point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: time + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 999999 + Min Color: 0; 0; 0 + Min Intensity: 999999 + Name: filtered scan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/filtered_point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 500 + Name: odometry path + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/odometry + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: time + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 999999 + Min Color: 0; 0; 0 + Min Intensity: 999999 + Name: undistorted scan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/udist_point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1.0234375 + Min Color: 0; 0; 0 + Min Intensity: 0.00022029876708984375 + Name: curr map odo + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/submap_odo + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 1 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: localization path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.10000000149011612 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep All + Reliability Policy: Best Effort + Value: /vtr/loc_path + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: icp_score + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 3 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: curr map loc + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/submap_loc + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 1 + Line Style: Billboards + Line Width: 0.07999999821186066 + Name: reference path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.10000000149011612 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/planning_path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 1 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: planned path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.10000000149011612 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/local_plan + Value: true + - Alpha: 1 + Arrow Length: 0.5 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 255; 1 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Name: planned path (poses) + Shaft Length: 0.30000001192092896 + Shaft Radius: 0.10000000149011612 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/teb_poses + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex24 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: change detection input + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/change_detection_fake_pcd + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex24 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: change detection scan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/change_detection_scan + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 0; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 5 + Min Color: 0; 0; 0 + Min Intensity: 0.9500057697296143 + Name: change detection map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.03500000014901161 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/change_detection_map + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1.0234375 + Min Color: 0; 0; 0 + Min Intensity: 0.00022029876708984375 + Name: intra exp merging (old) + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/intra_exp_merging_old + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1.0234375 + Min Color: 0; 0; 0 + Min Intensity: 0.00022029876708984375 + Name: intra exp merging (new) + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/intra_exp_merging_new + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex21 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: annotation map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /annotation_map + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex24 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: temp + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/fake_pcd + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1.0234375 + Min Color: 0; 0; 0 + Min Intensity: 0.00022029876708984375 + Name: dynaimc detection (old) + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/dynamic_detection_old + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex11 + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 3 + Min Color: 255; 0; 0 + Min Intensity: 2 + Name: dynamic detection (new) + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/dynamic_detection_new + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1.0234375 + Min Color: 0; 0; 0 + Min Intensity: 0.00022029876708984375 + Name: dynamic detection (scan) + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/dynamic_detection_scan + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex21 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 0; 0 + Max Intensity: 2 + Min Color: 25; 255; 0 + Min Intensity: 1 + Name: inter exp merging priv + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/inter_exp_merging_priv + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 25; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1.0234375 + Min Color: 0; 0; 0 + Min Intensity: 0.00022029876708984375 + Name: inter exp merging curr + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/inter_exp_merging_curr + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex11 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 2 + Min Color: 255; 0; 0 + Min Intensity: 1 + Name: global map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/global_map + Use Fixed Frame: true + Use rainbow: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 0; 0 + Max Intensity: 9.06832218170166 + Min Color: 0; 255; 0 + Min Intensity: -2.8154680728912354 + Name: global me map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.15000000596046448 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/global_map + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 255 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.5 + Name: global path + Offset: + X: 0 + Y: 0 + Z: 0.5 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/global_path + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: icp_score + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 3 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: planning localization map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/submap_loc + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.5 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: safe corridor cost map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/safe_corridor_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/safe_corridor_costmap_updates + Use Timestamp: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 7.071067810058594 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: safe corridor costmap + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Tiles + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/safe_corridor_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex23 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: planning change detection scan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 5 + Size (m): 0.10000000149011612 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/change_detection_scan + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: flex24 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: planning fake obs. scan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2.5 + Size (m): 0.07999999821186066 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /vtr/fake_obstacle_scan + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.800000011920929 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 7.8003997802734375 + Min Value: -3.096583127975464 + Value: true + Axis: Z + Channel Name: flex11 + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 3 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: planning curr map loc + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.03999999910593033 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/submap_loc + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.07999999821186066 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/mpc_prediction + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/robot_path + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: obstacle cost map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/change_detection_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/change_detection_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: raw + Draw Behind: false + Enabled: false + Name: filtered_obstacle_map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/filtered_change_detection_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/filtered_change_detection_costmap_updates + Use Timestamp: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.07999999821186066 + Name: Corridor Path Left + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/corridor_path_left + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 170; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.07999999821186066 + Name: Corridor Path Right + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/corridor_path_right + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: submap_loc + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/submap_loc + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 127 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: submap_odom + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/submap_odo + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 0 + Enabled: true + Head Length: 0.15000000596046448 + Head Radius: 0.05000000074505806 + Name: PoseArray + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.10000000149011612 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/mpc_ref_pose_array1 + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 255; 255 + Enabled: true + Head Length: 0.15000000596046448 + Head Radius: 0.10000000149011612 + Name: PoseArray + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vtr/mpc_ref_pose_array2 + Value: true + Enabled: true + Global Options: + Background Color: 189; 189; 189 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 14.386004447937012 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -2.6177289485931396 + Y: -0.18078717589378357 + Z: 0.22488124668598175 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 3.25809907913208 + Saved: + - Class: rviz_default_plugins/Orbit + Distance: 111.39698028564453 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: odometry_kitty + Near Clip Distance: 0.009999999776482582 + Pitch: 0.515203058719635 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 3.468143939971924 + - Class: rviz_default_plugins/Orbit + Distance: 111.39698028564453 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: localization_kitty + Near Clip Distance: 0.009999999776482582 + Pitch: 0.41520315408706665 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 3.81314754486084 + - Class: rviz_default_plugins/Orbit + Distance: 163.4339141845703 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 74.81483459472656 + Y: -49.70417785644531 + Z: -22.4315185546875 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: global map + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 0.022728919982910156 + - Class: rviz_default_plugins/Orbit + Distance: 86.62259674072266 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 2.636601448059082 + Y: 13.734512329101562 + Z: -1.928588628768921 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: global map 3d marsdome + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6647973656654358 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 6.185899257659912 + - Class: rviz_default_plugins/Orbit + Distance: 147.65463256835938 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 68.21656799316406 + Y: -45.86554718017578 + Z: -16.68381118774414 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: global map 3d parkinglot + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9697971343994141 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 0.04772702977061272 + - Class: rviz_default_plugins/Orbit + Distance: 202.10983276367188 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 40.724178314208984 + Y: -33.86809158325195 + Z: -38.78361129760742 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: maybe is global map 3d grove + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8647971153259277 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 3.5477232933044434 + - Class: rviz_default_plugins/Orbit + Distance: 39.54384231567383 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ta-qualitative-with-map + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6597965955734253 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 2.544668197631836 + - Class: rviz_default_plugins/Orbit + Distance: 15.331032752990723 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ta-qualitative-no-map + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6597965955734253 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 2.544668197631836 + - Class: rviz_default_plugins/Orbit + Distance: 25.798072814941406 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.0496435165405273 + Y: -0.9416981339454651 + Z: 2.5215511322021484 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: fake object demo + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6997966170310974 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 3.1646721363067627 + - Class: rviz_default_plugins/Orbit + Distance: 39.34256362915039 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 6.799346923828125 + Y: -5.26158332824707 + Z: -2.355532169342041 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: planning plot + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7647982835769653 + Target Frame: robot + Value: Orbit (rviz_default_plugins) + Yaw: 2.295583963394165 +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000281000003a2fc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003bd000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004b1000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 From aff2027c6ff28356743ccf41ed8beec5bfeafc53 Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Mon, 31 Jul 2023 10:23:28 -0400 Subject: [PATCH 02/13] ouster conversion module debugging --- .../preprocessing/conversions/ouster_conversion_module.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp index b8a5fd2d..a8127807 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp @@ -90,7 +90,8 @@ void OusterConversionModule::run_(QueryCache &qdata0, OutputCache &, // pointwise timestamp point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); - CLOG(WARNING, "lidar.odometry_icp") << "point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time * 1e9); + CLOG(WARNING, "lidar.ouster_converter") << "First point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time * 1e9); + CLOG(WARNING, "lidar.ouster_converter") << "Second point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time); } // ouster has no polar coordinates, so compute them manually. From d56b08a715970cb2d9a449e6f7c05f92c20b7f3b Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Mon, 31 Jul 2023 15:30:18 -0400 Subject: [PATCH 03/13] Modifications to run VTR3 on the Warthog --- config/ouster_warthog_default.yaml | 23 ++++++++++--------- .../conversions/ouster_conversion_module.cpp | 9 ++++++-- 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 2059c2a3..bc8770c3 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -22,6 +22,7 @@ #- mpc.cbit #- mpc_debug #- obstacle detection.cbit + - grizzly_controller_tests.cbit # mission planner #- mission.server @@ -33,7 +34,7 @@ # lidar pipeline - lidar.pipeline - lidar.preprocessing - #- lidar.ouster_converter + - lidar.ouster_converter - lidar.odometry_icp #- lidar.odometry_map_maintenance #- lidar.vertex_test @@ -108,7 +109,7 @@ num_threads: 8 crop_range: 40.0 - frame_voxel_size: 0.2 # grid subsampling voxel size + 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 @@ -158,7 +159,7 @@ mapping: type: lidar.odometry_map_maintenance_v2 - map_voxel_size: 0.1 + map_voxel_size: 0.3 crop_range_front: 40.0 back_over_front_ratio: 0.5 @@ -173,7 +174,7 @@ type: lidar.intra_exp_merging_v2 depth: 6.0 - map_voxel_size: 0.2 + map_voxel_size: 0.3 crop_range_front: 40.0 back_over_front_ratio: 0.5 @@ -191,7 +192,7 @@ inter_exp_merging: type: "lidar.inter_exp_merging_v2" - map_voxel_size: 0.2 + map_voxel_size: 0.3 max_num_experiences: 128 distance_threshold: 0.6 planar_threshold: 0.2 @@ -333,18 +334,18 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 1.0 - max_lin_vel: 1.5 - max_ang_vel: 1.25 - 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 + forward_vel: 0.75 + 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.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] + vel_error_cov: [25.0, 25.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [10.0, 50.0] kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] lat_error_cov: [20.0] diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp index a8127807..edf473be 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp @@ -89,9 +89,14 @@ void OusterConversionModule::run_(QueryCache &qdata0, OutputCache &, point_cloud->at(idx).z = *iter_z; // pointwise timestamp + //CLOG(DEBUG, "lidar.ouster_converter") << "Timestamp is: " << *iter_time; + //CLOG(DEBUG, "lidar.ouster_converter") << "Message Header Stamp (sec)" << (msg->header).stamp.sec; + //CLOG(DEBUG, "lidar.ouster_converter") << "Message Header Stamp (nanosec)" << (msg->header).stamp.nanosec; + + point_cloud->at(idx).timestamp = static_cast(*iter_time * 1e9); - CLOG(WARNING, "lidar.ouster_converter") << "First point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time * 1e9); - CLOG(WARNING, "lidar.ouster_converter") << "Second point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time); + //CLOG(DEBUG, "lidar.ouster_converter") << "First point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time * 1e9); + //CLOG(DEBUG, "lidar.ouster_converter") << "Second point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time); } // ouster has no polar coordinates, so compute them manually. From 958f80090574bf1ba18eaed333a264196d6189c0 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Tue, 1 Aug 2023 12:26:25 -0400 Subject: [PATCH 04/13] Pull iproved cache warning. --- .../vtr_tactic/include/vtr_tactic/cache.hpp | 5 ++-- .../vtr_tactic/include/vtr_tactic/types.hpp | 23 +++++++++++++++++-- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/main/src/vtr_tactic/include/vtr_tactic/cache.hpp b/main/src/vtr_tactic/include/vtr_tactic/cache.hpp index 0f43978f..5b2a66f2 100644 --- a/main/src/vtr_tactic/include/vtr_tactic/cache.hpp +++ b/main/src/vtr_tactic/include/vtr_tactic/cache.hpp @@ -81,8 +81,9 @@ class Cache { const DataType& operator*() const { if (valid()) return *datum_; - else - throw std::runtime_error("cache datum is unset on reference request."); + else { + throw std::runtime_error("cache datum is unset on reference request. Cache type is: " + type_name(datum_)); + } } DataType& operator*() { return const_cast(*static_cast(*this)); diff --git a/main/src/vtr_tactic/include/vtr_tactic/types.hpp b/main/src/vtr_tactic/include/vtr_tactic/types.hpp index 7766caed..192cdc23 100644 --- a/main/src/vtr_tactic/include/vtr_tactic/types.hpp +++ b/main/src/vtr_tactic/include/vtr_tactic/types.hpp @@ -36,6 +36,7 @@ using Timestamp = storage::Timestamp; using Graph = pose_graph::RCGraph; using GraphBase = Graph::Base; using VertexId = pose_graph::VertexId; +using RunIdSet = std::set; using Vertex = pose_graph::RCVertex; using EdgeId = pose_graph::EdgeId; using Edge = pose_graph::RCEdge; @@ -67,7 +68,7 @@ enum class PipelineMode : uint8_t { std::ostream& operator<<(std::ostream& os, const PipelineMode& signal); /** \brief the vertex creation test result */ -enum class VertexTestResult : int { CREATE_VERTEX = 0, DO_NOTHING = 1 }; +enum class VertexTestResult : int { CREATE_VERTEX = 0, DO_NOTHING = 1, CREATE_CANDIDATE = 2}; /** \brief Full metric and topological localization in one package */ struct Localization { @@ -82,4 +83,22 @@ struct Localization { }; } // namespace tactic -} // namespace vtr \ No newline at end of file +} // namespace vtr + + +//Helper to print the nice name of variable types +#ifndef TYPE_HPP +#define TYPE_HPP + +#include +#include + +std::string demangle(const char* name); + +template +std::string type_name(const T& t) { + + return demangle(typeid(t).name()); +} + +#endif \ No newline at end of file From 9748fe05d9055920b92f952a07e7af452374a698 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Tue, 1 Aug 2023 12:39:49 -0400 Subject: [PATCH 05/13] Add c++ file --- main/src/vtr_tactic/src/types.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/main/src/vtr_tactic/src/types.cpp b/main/src/vtr_tactic/src/types.cpp index e90fe489..94957777 100644 --- a/main/src/vtr_tactic/src/types.cpp +++ b/main/src/vtr_tactic/src/types.cpp @@ -48,4 +48,31 @@ std::ostream& operator<<(std::ostream& os, const PipelineMode& mode) { } } // namespace tactic -} // namespace vtr \ No newline at end of file +} // namespace vtr + +#ifdef __GNUG__ +#include +#include +#include + +std::string demangle(const char* name) { + + int status = -4; // some arbitrary value to eliminate the compiler warning + + // enable c++11 by passing the flag -std=c++11 to g++ + std::unique_ptr res { + abi::__cxa_demangle(name, NULL, NULL, &status), + std::free + }; + + return (status==0) ? res.get() : name ; +} + +#else + +// does nothing if not g++ +std::string demangle(const char* name) { + return name; +} + +#endif \ No newline at end of file From 1ad557b79c6927af3eab08eb2b0cfe0bebd6713b Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Tue, 1 Aug 2023 13:05:45 -0400 Subject: [PATCH 06/13] Switched to ROS2 Ouster Driver --- config/ouster_warthog_default.yaml | 2 +- .../modules/odometry/odometry_icp_module.cpp | 2 + rviz/ouster.rviz | 60 +++++++++---------- 3 files changed, 31 insertions(+), 33 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index bc8770c3..f2a9c7cd 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -50,7 +50,7 @@ robot_frame: base_link env_info_topic: env_info lidar_frame: os_sensor - lidar_topic: /ouster/points + lidar_topic: /points graph_map: origin_lat: 43.7822 origin_lng: -79.4661 diff --git a/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp b/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp index b6483504..5f58c9cb 100644 --- a/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp +++ b/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp @@ -181,6 +181,8 @@ void OdometryICPModule::run_(QueryCache &qdata0, OutputCache &, if (prev_time == query_time) { CLOG(WARNING, "lidar.odometry") << "Skipping point cloud with duplicate stamp"; + CLOG(WARNING, "lidar.odometry") << "The prev_time was: "<< prev_time; + CLOG(WARNING, "lidar.odometry") << "The query_time was: "<< query_time; *qdata.odo_success = false; return; } diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz index cc2eeb7b..2ec92ad1 100644 --- a/rviz/ouster.rviz +++ b/rviz/ouster.rviz @@ -74,29 +74,23 @@ Visualization Manager: Frame Timeout: 99999 Frames: All Enabled: false - base_footprint: - Value: true - base_footprint_bumper_part: - Value: true base_link: Value: true chassis_link: Value: true - front_axle_link: - Value: true - front_bumblebee: + diff_link: Value: true front_left_wheel_link: Value: true front_right_wheel_link: Value: true - front_xb3: + imu_link: Value: true - hc: + left_diff_unit_headlight_link: Value: true - honeycomb: + left_diff_unit_link: Value: true - imu_link: + left_diff_unit_taillight_link: Value: true lidar: Value: false @@ -104,31 +98,33 @@ Visualization Manager: Value: false loc vertex frame (offset): Value: false - m600_base: - Value: true - microstrain: - Value: true - navsat_link: + novatel: Value: true odo vertex frame: Value: false odom: Value: true - planning frame: - Value: false - rear_antenna_link: + os_imu: + Value: true + os_lidar: + Value: true + os_sensor: + Value: true + os_sensor_base_link: Value: true - rear_bumblebee: + os_sensor_baseplate: Value: true + planning frame: + Value: false rear_left_wheel_link: Value: true rear_right_wheel_link: Value: true - rear_sensor_base_link: + right_diff_unit_headlight_link: Value: true - rear_sensor_plate_link: + right_diff_unit_link: Value: true - rear_xb3: + right_diff_unit_taillight_link: Value: true robot: Value: true @@ -138,9 +134,11 @@ Visualization Manager: Value: false robot planning (extrapolated) mpc: Value: false - sensor_anchor_link: + sensor_tower: Value: true - velodyne: + stereo_camera: + Value: true + top_chassis_link: Value: true world: Value: false @@ -153,8 +151,6 @@ Visualization Manager: Show Names: true Tree: world: - hc: - {} loc vertex frame: {} odo vertex frame: @@ -1492,16 +1488,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 14.386004447937012 + Distance: 41.55506134033203 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -2.6177289485931396 - Y: -0.18078717589378357 - Z: 0.22488124668598175 + X: 12.343294143676758 + Y: -3.8882362842559814 + Z: 0.2389792650938034 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -1510,7 +1506,7 @@ Visualization Manager: Pitch: 1.5697963237762451 Target Frame: robot Value: Orbit (rviz_default_plugins) - Yaw: 3.25809907913208 + Yaw: 3.441279411315918 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 From d3db1e76868df59c272bddf97734f0aca3ddad0e Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Tue, 1 Aug 2023 13:20:35 -0400 Subject: [PATCH 07/13] fixed build error due to CLOG message type --- .../vtr_lidar/src/modules/odometry/odometry_icp_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp b/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp index 5f58c9cb..86818fb0 100644 --- a/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp +++ b/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp @@ -181,8 +181,8 @@ void OdometryICPModule::run_(QueryCache &qdata0, OutputCache &, if (prev_time == query_time) { CLOG(WARNING, "lidar.odometry") << "Skipping point cloud with duplicate stamp"; - CLOG(WARNING, "lidar.odometry") << "The prev_time was: "<< prev_time; - CLOG(WARNING, "lidar.odometry") << "The query_time was: "<< query_time; + //CLOG(WARNING, "lidar.odometry") << "The prev_time was: "<< prev_time; + //CLOG(WARNING, "lidar.odometry") << "The query_time was: "<< query_time; *qdata.odo_success = false; return; } From 794d7a9fdc52e6e11e3026a6a7da13998a048c53 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Sat, 5 Aug 2023 20:21:09 -0400 Subject: [PATCH 08/13] Added a filter of the static ouster points. Fixed crashing on point clouds received with duplicate stamps. --- config/ouster_warthog_default.yaml | 19 +++++++---- .../conversions/ouster_conversion_module.hpp | 8 +++++ .../planning/change_detection_module_v3.cpp | 6 ++++ .../pointmap/inter_exp_merging_module_v2.cpp | 12 +++++++ .../pointmap/intra_exp_merging_module_v2.cpp | 10 ++++++ .../conversions/ouster_conversion_module.cpp | 32 +++++++++++++++++-- 6 files changed, 77 insertions(+), 10 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index f2a9c7cd..de2f4c45 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -23,6 +23,7 @@ #- mpc_debug #- obstacle detection.cbit - grizzly_controller_tests.cbit + - ouster # mission planner #- mission.server @@ -37,7 +38,7 @@ - lidar.ouster_converter - lidar.odometry_icp #- lidar.odometry_map_maintenance - #- lidar.vertex_test + - lidar.vertex_test #- lidar.localization_map_recall - lidar.localization_icp #- lidar.intra_exp_merging @@ -50,7 +51,7 @@ robot_frame: base_link env_info_topic: env_info lidar_frame: os_sensor - lidar_topic: /points + lidar_topic: /ouster/points graph_map: origin_lat: 43.7822 origin_lng: -79.4661 @@ -103,7 +104,11 @@ preprocessing: conversion: type: lidar.ouster_converter - visualize: true + 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 @@ -334,11 +339,11 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 0.75 + forward_vel: 0.9 max_lin_vel: 1.25 - max_ang_vel: 2.0 + max_ang_vel: 5.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 + robot_angular_velocity_scale: 3.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" @@ -370,4 +375,4 @@ #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] # Misc - command_history_length: 100 \ No newline at end of file + command_history_length: 100 diff --git a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/ouster_conversion_module.hpp b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/ouster_conversion_module.hpp index c7344e10..775d20ba 100644 --- a/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/ouster_conversion_module.hpp +++ b/main/src/vtr_lidar/include/vtr_lidar/modules/preprocessing/conversions/ouster_conversion_module.hpp @@ -42,6 +42,14 @@ class OusterConversionModule : public tactic::BaseModule { bool visualize = false; + bool filter_warthog_points = false; + + //A centered cylinder to remove points related to the vehicle's sturcture. + float filter_z_max = 0; + float filter_z_min = 0; + float filter_radius_sq = 0; + + static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node, const std::string ¶m_prefix); }; diff --git a/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp b/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp index 151a9b9e..b90ae38d 100644 --- a/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp +++ b/main/src/vtr_lidar/src/modules/planning/change_detection_module_v3.cpp @@ -144,6 +144,12 @@ void ChangeDetectionModuleV3::run_(QueryCache &qdata0, OutputCache &output0, publisher_initialized_ = true; } + if (!qdata.undistorted_point_cloud.valid()) { + CLOG(WARNING, "lidar.change_detection_v3") << "Point clouds are not aligned. Skipping Change Detection."; + return; + } + + // inputs const auto &stamp = *qdata.stamp; const auto &T_s_r = *qdata.T_s_r; diff --git a/main/src/vtr_lidar/src/modules/pointmap/inter_exp_merging_module_v2.cpp b/main/src/vtr_lidar/src/modules/pointmap/inter_exp_merging_module_v2.cpp index 8dfaf0ea..ab767084 100644 --- a/main/src/vtr_lidar/src/modules/pointmap/inter_exp_merging_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/pointmap/inter_exp_merging_module_v2.cpp @@ -82,6 +82,13 @@ void InterExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, { const auto msg = curr_vertex->retrieve( "pointmap_ptr", "vtr_lidar_msgs/msg/PointMapPointer"); + + if (msg == nullptr) { + CLOG(WARNING, "lidar.inter_exp_merging") + << "Pointmap pointer, skipped."; + return; + } + auto locked_msg = msg->sharedLocked(); const auto &pointmap_ptr = locked_msg.get().getData(); // @@ -95,6 +102,11 @@ void InterExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, /// retrieve the map for the curr vertex const auto curr_map_msg = curr_vertex->retrieve>( "pointmap", "vtr_lidar_msgs/msg/PointMap"); + if (curr_map_msg == nullptr) { + CLOG(WARNING, "lidar.inter_exp_merging") + << "Pointmap pointer, skipped."; + return; + } auto pointmap = curr_map_msg->sharedLocked().get().getData(); auto &pointcloud = pointmap.point_cloud(); diff --git a/main/src/vtr_lidar/src/modules/pointmap/intra_exp_merging_module_v2.cpp b/main/src/vtr_lidar/src/modules/pointmap/intra_exp_merging_module_v2.cpp index 7341ccef..e3da1edc 100644 --- a/main/src/vtr_lidar/src/modules/pointmap/intra_exp_merging_module_v2.cpp +++ b/main/src/vtr_lidar/src/modules/pointmap/intra_exp_merging_module_v2.cpp @@ -92,6 +92,11 @@ void IntraExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, { const auto msg = target_vertex->retrieve( "pointmap_ptr", "vtr_lidar_msgs/msg/PointMapPointer"); + if (msg == nullptr) { + CLOG(WARNING, "lidar.inter_exp_merging") + << "No pointmap pointer, skipped."; + return; + } auto locked_msg = msg->sharedLocked(); const auto &pointmap_ptr = locked_msg.get().getData(); // @@ -106,6 +111,11 @@ void IntraExpMergingModuleV2::runAsync_(QueryCache &qdata0, OutputCache &, { const auto map_msg = target_vertex->retrieve>( "pointmap", "vtr_lidar_msgs/msg/PointMap"); + if (map_msg == nullptr) { + CLOG(WARNING, "lidar.inter_exp_merging") + << "No pointmap pointer, skipped."; + return; + } auto locked_map_msg_ref = map_msg->sharedLocked(); // lock the msg auto &locked_map_msg = locked_map_msg_ref.get(); diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp index edf473be..1f68be99 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp @@ -51,6 +51,15 @@ auto OusterConversionModule::Config::fromROS( -> ConstPtr { auto config = std::make_shared(); // clang-format off + config->filter_warthog_points = node->declare_parameter(param_prefix + ".filter_warthog", config->filter_warthog_points); + config->filter_z_max = node->declare_parameter(param_prefix + ".filter_z_max", config->filter_z_max); + config->filter_z_min = node->declare_parameter(param_prefix + ".filter_z_min", config->filter_z_min); + auto filter_radius = node->declare_parameter(param_prefix + ".filter_radius", config->filter_radius_sq); + config->filter_radius_sq = filter_radius * filter_radius; + + CLOG(INFO, "ouster") << config->filter_z_max << "-" << config->filter_z_min; + CLOG(INFO, "ouster") << config->filter_warthog_points << "-" << config->filter_radius_sq; + config->visualize = node->declare_parameter(param_prefix + ".visualize", config->visualize); // clang-format on return config; @@ -99,15 +108,32 @@ void OusterConversionModule::run_(QueryCache &qdata0, OutputCache &, //CLOG(DEBUG, "lidar.ouster_converter") << "Second point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time); } + auto filtered_point_cloud = + std::make_shared>(*point_cloud); + + /// Range cropping + if (config_->filter_warthog_points){ + std::vector indices; + indices.reserve(filtered_point_cloud->size()); + for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { + auto& p = (*filtered_point_cloud)[i]; + if (p.x*p.x + p.y*p.y > config_->filter_radius_sq && (p.z > config_->filter_z_max || p.z < config_->filter_z_min)) + indices.emplace_back(i); + } + *filtered_point_cloud = + pcl::PointCloud(*filtered_point_cloud, indices); + } + + // ouster has no polar coordinates, so compute them manually. - ousterCart2Pol(*point_cloud); + ousterCart2Pol(*filtered_point_cloud); // Output - qdata.raw_point_cloud = point_cloud; + qdata.raw_point_cloud = filtered_point_cloud; // Visualize if (config_->visualize) { - pcl::PointCloud point_cloud_tmp(*point_cloud); + pcl::PointCloud point_cloud_tmp(*filtered_point_cloud); std::for_each(point_cloud_tmp.begin(), point_cloud_tmp.end(), [&](PointWithInfo &point) { point.flex21 = static_cast( From dd0461a3becc89caf46df2c2b9355071e563d605 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Wed, 9 Aug 2023 10:06:24 -0400 Subject: [PATCH 09/13] Improvements to Warthog stability --- config/ouster_warthog_default.yaml | 24 +++++++++---------- .../path/localization_chain.hpp | 7 ++++++ main/src/vtr_tactic/src/tactic.cpp | 4 ++++ 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index de2f4c45..1b312605 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -4,7 +4,7 @@ log_debug: true log_enabled: # navigator - #- navigation + - navigation #- navigation.graph_map_server #- navigation.command @@ -19,8 +19,8 @@ #- path_planning #- path_planning.cbit #- path_planning.cbit_planner - #- mpc.cbit - #- mpc_debug + - mpc.cbit + - mpc_debug #- obstacle detection.cbit - grizzly_controller_tests.cbit - ouster @@ -34,16 +34,16 @@ # lidar pipeline - lidar.pipeline - - lidar.preprocessing - - lidar.ouster_converter + #- 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.intra_exp_merging #- lidar.dynamic_detection - #- lidar.inter_exp_merging + - lidar.inter_exp_merging #- lidar.ground_extraction #- lidar.obstacle_detection #- lidar.terrain_assessment @@ -159,7 +159,7 @@ averaging_num_steps: 2 verbose: false max_iterations: 1 - min_matched_ratio: 0.5 + min_matched_ratio: 0.85 visualize: true mapping: type: lidar.odometry_map_maintenance_v2 @@ -226,7 +226,7 @@ averaging_num_steps: 2 verbose: false max_iterations: 1 - min_matched_ratio: 0.3 + min_matched_ratio: 0.65 safe_corridor: type: lidar.safe_corridor lookahead_distance: 5.0 @@ -339,11 +339,11 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 0.9 + forward_vel: 0.6 max_lin_vel: 1.25 - max_ang_vel: 5.0 + 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: 3.0 # Used to scale the output twist angular 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" diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.hpp index 47f78656..a338baae 100644 --- a/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.hpp +++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.hpp @@ -88,6 +88,11 @@ class LocalizationChain : public Path { return is_localized_; } + void lostLocalization() { + LockGuard lock(this->mutex_); + is_localized_ = false; + } + unsigned trunkSequenceId() const { LockGuard lock(this->mutex_); return trunk_sid_; @@ -237,6 +242,8 @@ class LocalizationChain : public Path { /** \brief configuration */ const Config config_; + + }; } // namespace pose_graph diff --git a/main/src/vtr_tactic/src/tactic.cpp b/main/src/vtr_tactic/src/tactic.cpp index 7160b58e..7c7448ef 100644 --- a/main/src/vtr_tactic/src/tactic.cpp +++ b/main/src/vtr_tactic/src/tactic.cpp @@ -634,6 +634,7 @@ bool Tactic::teachMetricLocLocalization(const QueryCache::Ptr& qdata) { if (!(*qdata->loc_success)) { CLOG(WARNING, "tactic") << "Localization failed, skip updating pose graph " "and localization chain."; + chain_->lostLocalization(); return true; } @@ -673,6 +674,7 @@ bool Tactic::teachMergeLocalization(const QueryCache::Ptr& qdata) { if (!(*qdata->loc_success)) { CLOG(DEBUG, "tactic") << "Localization failed, skip updating pose graph " "and localization chain."; + chain_->lostLocalization(); return true; } @@ -710,6 +712,8 @@ bool Tactic::repeatMetricLocLocalization(const QueryCache::Ptr& qdata) { if (!(*qdata->loc_success)) { CLOG(DEBUG, "tactic") << "Localization failed, skip updating pose graph " "and localization chain."; + chain_->lostLocalization(); + return true; } From 2961fc2ec7fca7a5d6d35aa12ba08e8ad3ed88a2 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Thu, 10 Aug 2023 10:17:58 -0400 Subject: [PATCH 10/13] Change point cloud queue to drop oldest frames --- config/ouster_warthog_default.yaml | 1 + .../conversions/ouster_conversion_module.cpp | 5 ++- .../include/vtr_navigation/navigator.hpp | 1 + main/src/vtr_navigation/src/navigator.cpp | 32 +++++++++---------- 4 files changed, 21 insertions(+), 18 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 1b312605..fb06338c 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -52,6 +52,7 @@ 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 diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp index 1f68be99..2fe08326 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp @@ -88,14 +88,17 @@ void OusterConversionModule::run_(QueryCache &qdata0, OutputCache &, // clang-format off sensor_msgs::PointCloud2ConstIterator iter_x(*msg, "x"), iter_y(*msg, "y"), iter_z(*msg, "z"); sensor_msgs::PointCloud2ConstIterator iter_time(*msg, "t"); + sensor_msgs::PointCloud2ConstIterator iter_intensity(*msg, "intensity"); + // clang-format on for (size_t idx = 0; iter_x != iter_x.end(); - ++idx, ++iter_x, ++iter_y, ++iter_z, ++iter_time) { + ++idx, ++iter_x, ++iter_y, ++iter_z, ++iter_time, ++iter_intensity) { // cartesian coordinates point_cloud->at(idx).x = *iter_x; point_cloud->at(idx).y = *iter_y; point_cloud->at(idx).z = *iter_z; + point_cloud->at(idx).intensity = *iter_intensity; // pointwise timestamp //CLOG(DEBUG, "lidar.ouster_converter") << "Timestamp is: " << *iter_time; diff --git a/main/src/vtr_navigation/include/vtr_navigation/navigator.hpp b/main/src/vtr_navigation/include/vtr_navigation/navigator.hpp index 0fab9d97..1cf6b7c5 100644 --- a/main/src/vtr_navigation/include/vtr_navigation/navigator.hpp +++ b/main/src/vtr_navigation/include/vtr_navigation/navigator.hpp @@ -75,6 +75,7 @@ class Navigator { mutable std::condition_variable cv_thread_finish_; std::queue queue_; + int max_queue_size_ = 5; tactic::EnvInfo env_info_; #ifdef VTR_ENABLE_LIDAR bool pointcloud_in_queue_ = false; diff --git a/main/src/vtr_navigation/src/navigator.cpp b/main/src/vtr_navigation/src/navigator.cpp index 860233b1..5151d8e2 100644 --- a/main/src/vtr_navigation/src/navigator.cpp +++ b/main/src/vtr_navigation/src/navigator.cpp @@ -126,6 +126,9 @@ Navigator::Navigator(const rclcpp::Node::SharedPtr& node) : node_(node) { // environment info const auto env_info_topic = node_->declare_parameter("env_info_topic", "env_info"); env_info_sub_ = node_->create_subscription(env_info_topic, rclcpp::SystemDefaultsQoS(), std::bind(&Navigator::envInfoCallback, this, std::placeholders::_1), sub_opt); + + max_queue_size_ = node->declare_parameter("queue_size", max_queue_size_); + #ifdef VTR_ENABLE_LIDAR lidar_frame_ = node_->declare_parameter("lidar_frame", "lidar"); T_lidar_robot_ = loadTransform(lidar_frame_, robot_frame_); @@ -138,7 +141,7 @@ Navigator::Navigator(const rclcpp::Node::SharedPtr& node) : node_(node) { // lidar pointcloud data subscription const auto lidar_topic = node_->declare_parameter("lidar_topic", "/points"); // \note lidar point cloud data frequency is low, and we cannot afford dropping data - auto lidar_qos = rclcpp::QoS(10); + auto lidar_qos = rclcpp::QoS(max_queue_size_); lidar_qos.reliable(); lidar_sub_ = node_->create_subscription(lidar_topic, lidar_qos, std::bind(&Navigator::lidarCallback, this, std::placeholders::_1), sub_opt); //lidar_sub_ = node_->create_subscription(lidar_topic, rclcpp::SystemDefaultsQoS(), std::bind(&Navigator::lidarCallback, this, std::placeholders::_1), sub_opt); @@ -180,12 +183,6 @@ void Navigator::process() { while (true) { UniqueLock lock(mutex_); - /// print a warning if our queue is getting too big - if (queue_.size() > 5) { - CLOG_EVERY_N(10, WARNING, "navigation.sensor_input") - << " Input queue size is " << queue_.size(); - } - cv_set_or_stop_.wait(lock, [this] { return stop_ || (!queue_.empty()); }); if (stop_) { --thread_count_; @@ -199,8 +196,7 @@ void Navigator::process() { auto qdata0 = queue_.front(); queue_.pop(); #ifdef VTR_ENABLE_LIDAR - if (auto qdata = std::dynamic_pointer_cast(qdata0)) - pointcloud_in_queue_ = false; + auto qdata = std::dynamic_pointer_cast(qdata0); #endif // unlock the queue so that new data can be added @@ -223,22 +219,25 @@ void Navigator::envInfoCallback(const tactic::EnvInfo::SharedPtr msg) { #ifdef VTR_ENABLE_LIDAR void Navigator::lidarCallback( const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - LockGuard lock(mutex_); // set the timestamp Timestamp timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; CLOG(DEBUG, "navigation") << "Received a lidar pointcloud with stamp " << timestamp; - if (pointcloud_in_queue_) { + // Convert message to query_data format and store into query_data + auto query_data = std::make_shared(); + + LockGuard lock(mutex_); + + + /// Discard old frames if our queue is too big + if (queue_.size() > max_queue_size_) { CLOG(WARNING, "navigation") - << "Skip pointcloud message because there is already " - "one in queue."; - return; + << "Dropping old pointcloud message because the queue is full."; + queue_.pop(); } - // Convert message to query_data format and store into query_data - auto query_data = std::make_shared(); // some modules require node for visualization query_data->node = node_; @@ -256,7 +255,6 @@ void Navigator::lidarCallback( // add to the queue and notify the processing thread queue_.push(query_data); - pointcloud_in_queue_ = true; cv_set_or_stop_.notify_one(); }; #endif From 3986c1e733bad44022a031dab5a458ea5dda1644 Mon Sep 17 00:00:00 2001 From: a-krawciw Date: Mon, 14 Aug 2023 12:03:08 -0400 Subject: [PATCH 11/13] Fix static point filtering bug. --- .../preprocessing/conversions/ouster_conversion_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp index 2fe08326..0b87be1d 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/conversions/ouster_conversion_module.cpp @@ -120,7 +120,7 @@ void OusterConversionModule::run_(QueryCache &qdata0, OutputCache &, indices.reserve(filtered_point_cloud->size()); for (size_t i = 0; i < filtered_point_cloud->size(); ++i) { auto& p = (*filtered_point_cloud)[i]; - if (p.x*p.x + p.y*p.y > config_->filter_radius_sq && (p.z > config_->filter_z_max || p.z < config_->filter_z_min)) + if (p.x*p.x + p.y*p.y > config_->filter_radius_sq || (p.z > config_->filter_z_max || p.z < config_->filter_z_min)) indices.emplace_back(i); } *filtered_point_cloud = From 841eb4a7fe4575bf0c1b5274e57c5a0df054d7cb Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Mon, 28 Aug 2023 13:20:41 -0400 Subject: [PATCH 12/13] Fix to speed scheduler and switch to direct tracking control --- config/ouster_warthog_default.yaml | 6 +-- main/src/vtr_lidar/src/path_planning/cbit.cpp | 6 +-- .../src/path_planning/mpc_path_planner2.cpp | 4 +- .../src/cbit/generate_pq.cpp | 44 ++++++++++++++----- 4 files changed, 42 insertions(+), 18 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index fb06338c..60eee20c 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -306,7 +306,7 @@ visualize: true path_planning: - type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: cbit.lidar # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version control_period: 100 # ms cbit: obs_padding: 0.0 @@ -340,11 +340,11 @@ # Controller Params horizon_steps: 20 horizon_step_size: 0.3 - forward_vel: 0.6 + 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.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 2.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" diff --git a/main/src/vtr_lidar/src/path_planning/cbit.cpp b/main/src/vtr_lidar/src/path_planning/cbit.cpp index 223bf1ac..45f5840d 100644 --- a/main/src/vtr_lidar/src/path_planning/cbit.cpp +++ b/main/src/vtr_lidar/src/path_planning/cbit.cpp @@ -469,7 +469,7 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { // To do this, we need to conver the p_interp_vec,q_interp_vec into euclidean, as well as pairs of (p_interp,q_max), (q_interp,-q_max) // Then collision check between them //Node test_node = curve_to_euclid(Node(0.0, 0.0)); - Node test_node = curve_to_euclid(Node(10.0, 0.6)); + //Node test_node = curve_to_euclid(Node(10.0, 0.6)); //CLOG(ERROR, "mpc_debug.cbit") << "The Euclidean Node is x: " << test_node.p << ", y: "<< test_node.q << ", z: " << test_node.z; std::vector barrier_q_left_test; std::vector barrier_q_right_test; @@ -550,12 +550,12 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { { CLOG(INFO, "mpc.cbit") << "Attempting to solve the MPC problem"; // Using new sychronized measurements: - auto mpc_result = SolveMPC2(applied_vel, T0, measurements4, measurements, barrier_q_left_test, barrier_q_right_test, K, DT, VF, lat_noise_vect, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization, pose_error_weight, vel_error_weight, acc_error_weight, kin_error_weight, lat_error_weight); + //auto mpc_result = SolveMPC2(applied_vel, T0, measurements4, measurements, barrier_q_left_test, barrier_q_right_test, K, DT, VF, lat_noise_vect, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization, pose_error_weight, vel_error_weight, acc_error_weight, kin_error_weight, lat_error_weight); // Solve using corridor mpc //auto mpc_result = SolveMPC2(applied_vel, T0, measurements3, measurements, barrier_q_left, barrier_q_right, K, DT, VF, lat_noise_vect, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization3, pose_error_weight, vel_error_weight, acc_error_weight, kin_error_weight, lat_error_weight); // Old path tracking configs - //auto mpc_result = SolveMPC2(applied_vel, T0, measurements, measurements, barrier_q_left, barrier_q_right, K, DT, VF, lat_noise_vect, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization, pose_error_weight, vel_error_weight, acc_error_weight, kin_error_weight, lat_error_weight); + auto mpc_result = SolveMPC2(applied_vel, T0, measurements, measurements, barrier_q_left, barrier_q_right, K, DT, VF, lat_noise_vect, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization, pose_error_weight, vel_error_weight, acc_error_weight, kin_error_weight, lat_error_weight); //auto mpc_result = SolveMPC(applied_vel, T0, measurements, K, DT, VF, pose_noise_vect, vel_noise_vect, accel_noise_vect, kin_noise_vect, point_stabilization); // Tracking controller version applied_vel = mpc_result.applied_vel; // note dont re-declare applied vel here mpc_poses = mpc_result.mpc_poses; diff --git a/main/src/vtr_lidar/src/path_planning/mpc_path_planner2.cpp b/main/src/vtr_lidar/src/path_planning/mpc_path_planner2.cpp index 9b76fb80..bbba7d02 100644 --- a/main/src/vtr_lidar/src/path_planning/mpc_path_planner2.cpp +++ b/main/src/vtr_lidar/src/path_planning/mpc_path_planner2.cpp @@ -285,9 +285,9 @@ struct mpc_result SolveMPC2(Eigen::Matrix previous_vel, lgmath::se // Generate least square cost terms and add them to the optimization problem const auto lat_cost_term_right = steam::WeightedLeastSqCostTerm<1>::MakeShared(lat_barrier_right, sharedLatNoiseModel, latLossFunc); - opt_problem.addCostTerm(lat_cost_term_right); + //opt_problem.addCostTerm(lat_cost_term_right); const auto lat_cost_term_left = steam::WeightedLeastSqCostTerm<1>::MakeShared(lat_barrier_left, sharedLatNoiseModel, latLossFunc); - opt_problem.addCostTerm(lat_cost_term_left); + //opt_problem.addCostTerm(lat_cost_term_left); } diff --git a/main/src/vtr_path_planning/src/cbit/generate_pq.cpp b/main/src/vtr_path_planning/src/cbit/generate_pq.cpp index 38c63474..68c99c43 100644 --- a/main/src/vtr_path_planning/src/cbit/generate_pq.cpp +++ b/main/src/vtr_path_planning/src/cbit/generate_pq.cpp @@ -108,15 +108,27 @@ double CBITPath::delta_p_calc(Pose start_pose, Pose end_pose, double alpha) Eigen::Spline CBITPath::spline_path_xy(std::vector input_path) { + std::vector valid_points; - Eigen::MatrixXd points(2, input_path.size()); for (int i = 0; i < input_path.size(); i++) { - points(0, i) = input_path[i].x; - points(1, i) = input_path[i].y; - //CLOG(ERROR, "path_planning.corridor_debug") << "Original point at - x: " << disc_path[i].x << " y: " << disc_path[i].y; + if (i % 5 == 0) + { + valid_points.push_back(Eigen::Vector2d(input_path[i].x, input_path[i].y)); + //CLOG(ERROR, "path_planning.corridor_debug") << "Original point at - x: " << input_path[i].x << " y: " << input_path[i].y; + } } + Eigen::MatrixXd points(2, valid_points.size()); + + for (int i = 0; i < valid_points.size(); i++) + { + points(0, i) = valid_points[i](0); + points(1, i) = valid_points[i](1); + } + + //CLOG(ERROR, "path_planning.corridor_debug") << "TEST" << points; + Eigen::Spline spline = Eigen::SplineFitting>::Interpolate(points, 3); //CLOG(ERROR, "path_planning.corridor_debug") << "Computed Spline"; // To query the spline, we use spline(test), where test is a normalized distance along the spline from 0 to 1 @@ -131,15 +143,27 @@ Eigen::Spline CBITPath::spline_path_xy(std::vector input_path) Eigen::Spline CBITPath::spline_path_xz_yz(std::vector input_path) { + std::vector valid_points; - Eigen::MatrixXd points(2, input_path.size()); for (int i = 0; i < input_path.size(); i++) { - points(0, i) = sqrt((input_path[i].x * input_path[i].x) + (input_path[i].y * input_path[i].y)); - points(1, i) = input_path[i].z; - //CLOG(ERROR, "path_planning.corridor_debug") << "Original point at - x: " << disc_path[i].x << " y: " << disc_path[i].y; + if (i % 5 == 0) + { + valid_points.push_back(Eigen::Vector2d(sqrt((input_path[i].x * input_path[i].x) + (input_path[i].y * input_path[i].y)), input_path[i].z)); + //CLOG(ERROR, "path_planning.corridor_debug") << "Original point at - x: " << input_path[i].x << " y: " << input_path[i].y; + } } + Eigen::MatrixXd points(2, valid_points.size()); + + for (int i = 0; i < valid_points.size(); i++) + { + points(0, i) = valid_points[i](0); + points(1, i) = valid_points[i](1); + } + + //CLOG(ERROR, "path_planning.corridor_debug") << "TEST" << points; + Eigen::Spline spline = Eigen::SplineFitting>::Interpolate(points, 3); //CLOG(ERROR, "path_planning.corridor_debug") << "Computed Spline"; // To query the spline, we use spline(test), where test is a normalized distance along the spline from 0 to 1 @@ -182,8 +206,8 @@ double CBITPath::radius_of_curvature(double dist, Eigen::Spline splin double x_pred = curvature(0,0); double y_pred = curvature(1,0); - CLOG(INFO, "path_planning.cbit") << "splined x is: " << x_pred; - CLOG(INFO, "path_planning.cbit") << "splined y is: " << y_pred; + CLOG(INFO, "path_planning.cbit") << "splined x is: " << pred_x; + CLOG(INFO, "path_planning.cbit") << "splined y is: " << pred_y; CLOG(INFO, "path_planning.cbit") << "Radius of Curvature Before sign is: " << roc_magnitude; CLOG(INFO, "path_planning.cbit") << "Radius of Curvature is: " << roc_signed; From 61ff111c9cd461dc805c08ee0312b45b979b655e Mon Sep 17 00:00:00 2001 From: Jordy Sehn Date: Wed, 30 Aug 2023 16:24:25 -0400 Subject: [PATCH 13/13] Tuned Controller For Warthog with angular scaling --- config/ouster_warthog_default.yaml | 18 +++++++++--------- main/src/vtr_lidar/src/path_planning/cbit.cpp | 7 ++++--- main/src/vtr_path_planning/src/cbit/cbit.cpp | 8 +++++--- 3 files changed, 18 insertions(+), 15 deletions(-) diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 60eee20c..4b8863fb 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -23,7 +23,7 @@ - mpc_debug #- obstacle detection.cbit - grizzly_controller_tests.cbit - - ouster + #- ouster # mission planner #- mission.server @@ -242,7 +242,7 @@ detection_range: 8.0 search_radius: 0.25 - negprob_threshold: 0.15 # 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 + 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 @@ -251,8 +251,8 @@ support_variance: 0.05 support_threshold: 2.5 - 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 + 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 @@ -306,7 +306,7 @@ visualize: true path_planning: - type: cbit.lidar # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version control_period: 100 # ms cbit: obs_padding: 0.0 @@ -322,7 +322,7 @@ initial_samples: 400 batch_samples: 200 pre_seed_resolution: 0.5 - alpha: 0.5 + alpha: 0.25 q_max: 2.5 frame_interval: 50 iter_max: 10000000 @@ -344,14 +344,14 @@ 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: 2.0 # Used to scale the output twist angular 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: [25.0, 25.0] # was [0.1 2.0] # No longer using this cost term - acc_error_cov: [10.0, 50.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] diff --git a/main/src/vtr_lidar/src/path_planning/cbit.cpp b/main/src/vtr_lidar/src/path_planning/cbit.cpp index 45f5840d..238f0c32 100644 --- a/main/src/vtr_lidar/src/path_planning/cbit.cpp +++ b/main/src/vtr_lidar/src/path_planning/cbit.cpp @@ -77,8 +77,8 @@ auto LidarCBIT::Config::fromROS(const rclcpp::Node::SharedPtr& node, const std:: config->forward_vel = node->declare_parameter(prefix + ".mpc.forward_vel", config->forward_vel); config->max_lin_vel = node->declare_parameter(prefix + ".mpc.max_lin_vel", config->max_lin_vel); config->max_ang_vel = node->declare_parameter(prefix + ".mpc.max_ang_vel", config->max_ang_vel); - config->robot_linear_velocity_scale = node->declare_parameter(prefix + ".robot_linear_velocity_scale", config->robot_linear_velocity_scale); - config->robot_angular_velocity_scale = node->declare_parameter(prefix + ".robot_angular_velocity_scale", config->robot_angular_velocity_scale); + config->robot_linear_velocity_scale = node->declare_parameter(prefix + ".mpc.robot_linear_velocity_scale", config->robot_linear_velocity_scale); + config->robot_angular_velocity_scale = node->declare_parameter(prefix + ".mpc.robot_angular_velocity_scale", config->robot_angular_velocity_scale); // COST FUNCTION Covariances @@ -283,7 +283,7 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { CLOG(INFO, "mpc_debug.cbit") << "THE AVERAGE XY CURVATURE IS: " << avg_curvature_xy; CLOG(INFO, "mpc_debug.cbit") << "THE AVERAGE XZ CURVATURE IS: " << avg_curvature_xz_yz; //CLOG(ERROR, "mpc_debug.cbit") << "THE AVERAGE YZ CURVATURE IS: " << avg_curvature_yz; - double xy_curv_weight = 5.0; // hardocded for now, make a param + double xy_curv_weight = 2.5; // hardocded for now, make a param double xz_yz_curv_weight = 0.5; // hardocded for now, make a param double end_of_path_weight = 1.0; // hardocded for now, make a param @@ -591,6 +591,7 @@ auto LidarCBIT::computeCommand(RobotState& robot_state0) -> Command { command.linear.x = saturated_vel(0) * config_->robot_linear_velocity_scale; command.angular.z = saturated_vel(1) * config_->robot_angular_velocity_scale; + CLOG(ERROR, "grizzly_controller_tests.cbit") << "Robot Twist Angular Scale: " << config_->robot_angular_velocity_scale; // Temporary modification by Jordy to test calibration of the grizzly controller CLOG(DEBUG, "grizzly_controller_tests.cbit") << "Twist Linear Velocity: " << saturated_vel(0); CLOG(DEBUG, "grizzly_controller_tests.cbit") << "Twist Angular Velocity: " << saturated_vel(1); diff --git a/main/src/vtr_path_planning/src/cbit/cbit.cpp b/main/src/vtr_path_planning/src/cbit/cbit.cpp index 0d963a3f..64debbe6 100644 --- a/main/src/vtr_path_planning/src/cbit/cbit.cpp +++ b/main/src/vtr_path_planning/src/cbit/cbit.cpp @@ -80,8 +80,8 @@ auto CBIT::Config::fromROS(const rclcpp::Node::SharedPtr& node, const std::strin config->forward_vel = node->declare_parameter(prefix + ".mpc.forward_vel", config->forward_vel); config->max_lin_vel = node->declare_parameter(prefix + ".mpc.max_lin_vel", config->max_lin_vel); config->max_ang_vel = node->declare_parameter(prefix + ".mpc.max_ang_vel", config->max_ang_vel); - config->robot_linear_velocity_scale = node->declare_parameter(prefix + ".robot_linear_velocity_scale", config->robot_linear_velocity_scale); - config->robot_angular_velocity_scale = node->declare_parameter(prefix + ".robot_angular_velocity_scale", config->robot_angular_velocity_scale); + config->robot_linear_velocity_scale = node->declare_parameter(prefix + ".mpc.robot_linear_velocity_scale", config->robot_linear_velocity_scale); + config->robot_angular_velocity_scale = node->declare_parameter(prefix + ".mpc.robot_angular_velocity_scale", config->robot_angular_velocity_scale); // COST FUNCTION COVARIANCE @@ -378,7 +378,7 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command { CLOG(INFO, "mpc_debug.cbit") << "THE AVERAGE XY CURVATURE IS: " << avg_curvature_xy; CLOG(INFO, "mpc_debug.cbit") << "THE AVERAGE XZ CURVATURE IS: " << avg_curvature_xz_yz; //CLOG(ERROR, "mpc_debug.cbit") << "THE AVERAGE YZ CURVATURE IS: " << avg_curvature_yz; - double xy_curv_weight = 5.0; // hardocded for now, make a param + double xy_curv_weight = 2.50; // hardocded for now, make a param double xz_yz_curv_weight = 0.5; // hardocded for now, make a param double end_of_path_weight = 1.0; // hardocded for now, make a param @@ -592,6 +592,8 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command { command.linear.x = saturated_vel(0) * config_->robot_linear_velocity_scale; command.angular.z = saturated_vel(1) * config_->robot_angular_velocity_scale; // Temporary modification by Jordy to test calibration of hte grizzly controller + + CLOG(DEBUG, "grizzly_controller_tests.cbit") << "Twist Linear Velocity: " << saturated_vel(0); CLOG(DEBUG, "grizzly_controller_tests.cbit") << "Twist Angular Velocity: " << saturated_vel(1); // End of modification