diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml new file mode 100644 index 00000000..4b8863fb --- /dev/null +++ b/config/ouster_warthog_default.yaml @@ -0,0 +1,379 @@ +/**: + ros__parameters: + log_to_file: true + log_debug: true + log_enabled: + # navigator + - navigation + #- navigation.graph_map_server + #- navigation.command + + # tactic + #- tactic + - tactic.pipeline + #- tactic.module + #- tactic.module.live_mem_manager + #- tactic.module.graph_mem_manager + + # path planner + #- path_planning + #- path_planning.cbit + #- path_planning.cbit_planner + - mpc.cbit + - mpc_debug + #- obstacle detection.cbit + - grizzly_controller_tests.cbit + #- ouster + + # mission planner + #- mission.server + #- mission.state_machine + + # pose graph + #- pose_graph + + # lidar pipeline + - lidar.pipeline + #- lidar.preprocessing + #- lidar.ouster_converter + - lidar.odometry_icp + #- lidar.odometry_map_maintenance + - lidar.vertex_test + #- lidar.localization_map_recall + - lidar.localization_icp + - lidar.intra_exp_merging + #- lidar.dynamic_detection + - lidar.inter_exp_merging + #- lidar.ground_extraction + #- lidar.obstacle_detection + #- lidar.terrain_assessment + + robot_frame: base_link + env_info_topic: env_info + lidar_frame: os_sensor + lidar_topic: /ouster/points + queue_size: 1 + graph_map: + origin_lat: 43.7822 + origin_lng: -79.4661 + origin_theta: 1.3 + scale: 1.0 + tactic: + enable_parallelization: true + preprocessing_skippable: false + odometry_mapping_skippable: false + localization_skippable: false + task_queue_num_threads: 1 + task_queue_size: -1 + + route_completion_translation_threshold: 0.5 + + chain: + min_cusp_distance: 1.5 + angle_weight: 7.0 + search_depth: 5 + search_back_depth: 10 + distance_warning: 5.0 + save_odometry_result: true + save_localization_result: true + visualize: true + rviz_loc_path_offset: + - 0.0 + - 0.0 + - 0.0 + pipeline: + type: lidar + preprocessing: + - conversion + - filtering + odometry: + - icp + - mapping + - vertex_test + - intra_exp_merging + - dynamic_detection + - inter_exp_merging + - memory + localization: + - recall + - icp + - safe_corridor + - change_detection_sync + - memory + submap_translation_threshold: 1.5 + submap_rotation_threshold: 30.0 + preprocessing: + conversion: + type: lidar.ouster_converter + visualize: false + filter_warthog: true + filter_z_min: -0.2 + filter_z_max: 0.35 + filter_radius: 0.8 + filtering: + type: lidar.preprocessing + num_threads: 8 + crop_range: 40.0 + + frame_voxel_size: 0.3 # grid subsampling voxel size + + vertical_angle_res: 0.0061365 # vertical angle resolution in radians, equal to 0.3516 degree documented in the manual + polar_r_scale: 2.0 # polar_r_scale x vertical_angle_res = nearest neighbor search radius for normal computation + r_scale: 4.0 # scale down point range by this value after taking log, whatever works + h_scale: 2.0 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution when 5Hz spin frequence is ~0.7031 degree, so 0.7031 / 0.3516 = 2.00 + + num_sample1: 20000 # max number of sample after filtering based on planarity + min_norm_score1: 0.95 # min planarity score + + num_sample2: 20000 # max number of sample after filtering based on planarity + min_norm_score2: 0.2 # 0.2 is when the incident angle 5/12 * pi + min_normal_estimate_dist: 1.0 # minimum distance to estimate normal in meters + max_normal_estimate_angle: 0.44 # must <1/2, this value will be timed by M_PI + + cluster_num_sample: 20000 # maxnumber of sample after removing isolated points + + visualize: true + odometry: + icp: + type: lidar.odometry_icp + + # continuous time estimation + use_trajectory_estimation: false + traj_num_extra_states: 0 + traj_lock_prev_pose: false + traj_lock_prev_vel: false + traj_qc_diag: + - 1.0 + - 0.1 + - 0.1 + - 0.1 + - 0.1 + - 1.0 + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.85 + visualize: true + mapping: + type: lidar.odometry_map_maintenance_v2 + + map_voxel_size: 0.3 + + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + point_life_time: 20.0 + visualize: true + vertex_test: + type: lidar.vertex_test + + max_translation: 0.3 + max_rotation: 10.0 + intra_exp_merging: + type: lidar.intra_exp_merging_v2 + depth: 6.0 + + map_voxel_size: 0.3 + + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + visualize: true + dynamic_detection: + type: lidar.dynamic_detection + depth: 12.0 + + horizontal_resolution: 0.0122718 # 0.02042 + vertical_resolution: 0.00613587 # 0.01326 + max_num_observations: 2000 + min_num_observations: 4 + dynamic_threshold: 0.3 + visualize: true + inter_exp_merging: + type: "lidar.inter_exp_merging_v2" + + map_voxel_size: 0.3 + max_num_experiences: 128 + distance_threshold: 0.6 + planar_threshold: 0.2 + normal_threshold: 0.8 + dynamic_obs_threshold: 1 + visualize: true + memory: + type: live_mem_manager + window_size: 5 + localization: + recall: + type: lidar.localization_map_recall + map_version: pointmap + visualize: true + icp: + type: lidar.localization_icp + use_pose_prior: true + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.65 + safe_corridor: + type: lidar.safe_corridor + lookahead_distance: 5.0 + corridor_width: 3.5 + influence_distance: 1.0 + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true + change_detection_sync: + type: lidar.change_detection_v3 + detection_range: 8.0 + search_radius: 0.25 + + negprob_threshold: 0.015 # was 0.015 # -1.86 without prior, 0.015 with prior # Jordy: I found I needed to bump this up abit (approx 0.075+) to reduce false positives + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + + influence_distance: 0.6 # was 0.5 Jordy # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 0.9 # was 0.3 Jordy + + # cost map + costmap_history_size: 15 # Keep between 3 and 20, used for temporal filtering + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: true + memory: + type: graph_mem_manager + vertex_life_span: 5 + window_size: 3 + obstacle_detection: + type: lidar.obstacle_detection + z_min: 0.5 + z_max: 2.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + ground_extraction: + type: lidar.ground_extraction + z_offset: 0.2 + alpha: 0.035 + tolerance: 0.25 + Tm: 0.3 + Tm_small: 0.1 + Tb: 0.5 + Trmse: 0.1 + Tdprev: 1.0 + rmin: 2.0 + num_bins_small: 30.0 + bin_size_small: 0.5 + num_bins_large: 10.0 + bin_size_large: 1.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: true + terrain_assessment: + type: lidar.terrain_assessment + lookahead_distance: 15.0 + corridor_width: 1.0 + search_radius: 1.0 + resolution: 0.5 + size_x: 40.0 + size_y: 20.0 + run_online: false + run_async: true + visualize: true + + path_planning: + type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + control_period: 100 # ms + cbit: + obs_padding: 0.0 + curv_to_euclid_discretization: 10 + sliding_window_width: 12.0 + sliding_window_freespace_padding: 0.5 + corridor_resolution: 0.2 + state_update_freq: 2.0 + update_state: true + rand_seed: 1 + + # Planner Tuning Params + initial_samples: 400 + batch_samples: 200 + pre_seed_resolution: 0.5 + alpha: 0.25 + q_max: 2.5 + frame_interval: 50 + iter_max: 10000000 + eta: 1.0 + rad_m_exhange: 1.00 + initial_exp_rad: 0.75 + extrapolation: false + incremental_plotting: false + plotting: true + costmap: + costmap_filter_value: 0.01 + costmap_history: 15 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + + mpc: + # Controller Params + horizon_steps: 20 + horizon_step_size: 0.3 + forward_vel: 1.0 + max_lin_vel: 1.25 + max_ang_vel: 2.0 + robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 1.8 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + + vehicle_model: "unicycle" + + # Cost Function Covariance Matrices + pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + vel_error_cov: [100.0, 200.0] # was [0.1 2.0] # No longer using this cost term + acc_error_cov: [100.0, 200.0] + kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + lat_error_cov: [20.0] + + # Cost Function Covariance Matrices (Tracking Backup) + #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] + #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term + #acc_error_cov: [10.0, 10.0] + #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + #lat_error_cov: [10.0] + + # Cost Function Weights + pose_error_weight: 1.0 + vel_error_weight: 1.0 + acc_error_weight: 1.0 + kin_error_weight: 1.0 + lat_error_weight: 0.01 + + #backup for tracking mpc + #pose_error_cov: [0.5, 0.5, 0.5, 10.0, 10.0, 10.0] # Used to have yaw components set to 1000 but it seems to cause some instability, others set to 1.0 + #vel_error_cov: [0.1, 1.0] # was [0.1 2.0] + #acc_error_cov: [0.1, 0.75] + #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] + + # Misc + command_history_length: 100 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/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/odometry/odometry_icp_module.cpp b/main/src/vtr_lidar/src/modules/odometry/odometry_icp_module.cpp index b6483504..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,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/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 b8a5fd2d..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 @@ -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; @@ -79,29 +88,55 @@ 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; + //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.odometry_icp") << "point info - x: " << *iter_x << " y: " << *iter_y << " z: " << *iter_z << " timestamp: " << static_cast(*iter_time * 1e9); + //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); + } + + 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( diff --git a/main/src/vtr_lidar/src/path_planning/cbit.cpp b/main/src/vtr_lidar/src/path_planning/cbit.cpp index 223bf1ac..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 @@ -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; @@ -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_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_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 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 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; 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/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 f0fe275a..e8d5de85 100644 --- a/main/src/vtr_tactic/include/vtr_tactic/types.hpp +++ b/main/src/vtr_tactic/include/vtr_tactic/types.hpp @@ -37,6 +37,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; @@ -69,7 +70,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 { @@ -84,4 +85,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 diff --git a/main/src/vtr_tactic/src/tactic.cpp b/main/src/vtr_tactic/src/tactic.cpp index 86c2e540..db386f60 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; } 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 diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz new file mode 100644 index 00000000..2ec92ad1 --- /dev/null +++ b/rviz/ouster.rviz @@ -0,0 +1,1728 @@ +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_link: + Value: true + chassis_link: + Value: true + diff_link: + Value: true + front_left_wheel_link: + Value: true + front_right_wheel_link: + Value: true + imu_link: + Value: true + left_diff_unit_headlight_link: + Value: true + left_diff_unit_link: + Value: true + left_diff_unit_taillight_link: + Value: true + lidar: + Value: false + loc vertex frame: + Value: false + loc vertex frame (offset): + Value: false + novatel: + Value: true + odo vertex frame: + Value: false + odom: + Value: true + os_imu: + Value: true + os_lidar: + Value: true + os_sensor: + Value: true + os_sensor_base_link: + Value: true + os_sensor_baseplate: + Value: true + planning frame: + Value: false + rear_left_wheel_link: + Value: true + rear_right_wheel_link: + Value: true + right_diff_unit_headlight_link: + Value: true + right_diff_unit_link: + Value: true + right_diff_unit_taillight_link: + Value: true + robot: + Value: true + robot planning: + Value: false + robot planning (extrapolated): + Value: false + robot planning (extrapolated) mpc: + Value: false + sensor_tower: + Value: true + stereo_camera: + Value: true + top_chassis_link: + 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: + 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: 41.55506134033203 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 12.343294143676758 + Y: -3.8882362842559814 + Z: 0.2389792650938034 + 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.441279411315918 + 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