diff --git a/.gitignore b/.gitignore index e7a814bf..e66076b2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ Log/* build/* +images/ +data/ \ No newline at end of file diff --git a/config/camera_hk_mems.yaml b/config/camera_hk_mems.yaml new file mode 100644 index 00000000..b2d0cf8d --- /dev/null +++ b/config/camera_hk_mems.yaml @@ -0,0 +1,12 @@ +cam_model: Pinhole +cam_width: 1280 +cam_height: 720 +scale: 1 +cam_fx: 897.4596557617188 +cam_fy: 657.0736083984375 +cam_cx: 897.8496704101562 +cam_cy: 358.958251953125 +cam_d0: 0.1318076252937317 +cam_d1: -0.449052631855011 +cam_d2: 0.0010584293631836772 +cam_d3: 0.0007639264222234488 diff --git a/config/camera_urban_loco_ca.yaml b/config/camera_urban_loco_ca.yaml new file mode 100644 index 00000000..0363884b --- /dev/null +++ b/config/camera_urban_loco_ca.yaml @@ -0,0 +1,12 @@ +cam_model: Pinhole +cam_width: 2048 +cam_height: 1536 +scale: 1 +cam_fx: 1857.4752797615988 +cam_fy: 1869.2155909761746 +cam_cx: 1039.692658811044 +cam_cy: 739.3362262994145 +cam_d0: -3.3474604323919716e-01 # k1 +cam_d1: 2.0887608302549923e-01 # k2 +cam_d2: 8.4845632526369460e-04 # p1 +cam_d3: 4.2905149321811908e-04 # p2 diff --git a/config/hk_mems.yaml b/config/hk_mems.yaml new file mode 100644 index 00000000..ffc70852 --- /dev/null +++ b/config/hk_mems.yaml @@ -0,0 +1,91 @@ +common: + img_topic: "/camera/color/image_raw/compressed" + is_compressed_image: true + lid_topic: "/ouster/points" + imu_topic: "/imu/data" + img_en: 1 # Enable visual mode + lidar_en: 1 # Enable lidar mode + ros_driver_bug_fix: false + +extrin_calib: + # Hilti-2022 + extrinsic_T: [0, 0, 0] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + + + Rcl: [ 0, 0, -1, + 0, 1, 0, + 1, 0, 0] + Pcl: [0, 0, 0 ] + +time_offset: + imu_time_offset: 0.0 + img_time_offset: 0.0 + exposure_time_init: 0.0 + +preprocess: + point_filter_num: 3 + filter_size_surf: 0.1 + lidar_type: 3 # Ouster + scan_line: 32 + blind: 0.5 + +vio: + max_iterations: 5 + outlier_threshold: 500 + img_point_cov: 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + +imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.5 # 0.1 + gyr_cov: 0.01 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 + +lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.05 + min_eigen_value: 0.0001 # 0.0025 + voxel_size: 0.4 + max_layer: 2 + max_points_num: 100 + layer_init_num: [5, 5, 5, 5, 5] + +local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8 + +uav: + imu_rate_odom: false + gravity_align_en: false + +publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + +evo: + seq_name: "exp09_cupola" + pose_output_en: true + +pcd_save: + pcd_save_en: false + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/config/urban_loco_ca.yaml b/config/urban_loco_ca.yaml new file mode 100644 index 00000000..02bb2299 --- /dev/null +++ b/config/urban_loco_ca.yaml @@ -0,0 +1,90 @@ +common: + img_topic: "/cam0/image_raw" + lid_topic: "/rslidar_points_ring" + imu_topic: "/imu_raw" + img_en: 1 # Enable visual mode + lidar_en: 1 # Enable lidar mode + ros_driver_bug_fix: false + +extrin_calib: + # Hilti-2022 + extrinsic_T: [0, 0, 0] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + + + Rcl: [0, 0, 1, + -1, 0, 0, + 0, -1, 0] + Pcl: [ 1.8499563388803192e-01, -2.4227446820327757e-02, -3.3589470159409335e-01] + +time_offset: + imu_time_offset: 0.0 + img_time_offset: 0.0 + exposure_time_init: 0.0 + +preprocess: + point_filter_num: 3 + filter_size_surf: 0.1 + lidar_type: 8 # Urban Loco + scan_line: 32 + blind: 0.5 + +vio: + max_iterations: 5 + outlier_threshold: 500 + img_point_cov: 1000 + patch_size: 8 + patch_pyrimid_level: 4 + normal_en: true + raycast_en: false + inverse_composition_en: false + exposure_estimate_en: true + inv_expo_cov: 0.1 + +imu: + imu_en: true + imu_int_frame: 30 + acc_cov: 0.5 # 0.1 + gyr_cov: 0.01 + b_acc_cov: 0.0001 # 0.1 + b_gyr_cov: 0.0001 # 0.1 + +lio: + max_iterations: 5 + dept_err: 0.02 + beam_err: 0.05 + min_eigen_value: 0.0001 # 0.0025 + voxel_size: 0.4 + max_layer: 2 + max_points_num: 100 + layer_init_num: [5, 5, 5, 5, 5] + +local_map: + map_sliding_en: false + half_map_size: 100 + sliding_thresh: 8 + +uav: + imu_rate_odom: false + gravity_align_en: false + +publish: + dense_map_en: true + pub_effect_point_en: false + pub_plane_en: false + pub_scan_num: 1 + blind_rgb_points: 0.0 + +evo: + seq_name: "exp09_cupola" + pose_output_en: true + +pcd_save: + pcd_save_en: false + colmap_output_en: false # need to set interval = -1 + filter_size_pcd: 0.15 + interval: -1 + # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/include/LIVMapper.h b/include/LIVMapper.h index c8101794..6f77e815 100644 --- a/include/LIVMapper.h +++ b/include/LIVMapper.h @@ -20,7 +20,7 @@ which is included as part of this source code package. #include #include #include - +#include class LIVMapper { public: @@ -49,6 +49,7 @@ class LIVMapper void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg_in); void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in); void img_cbk(const sensor_msgs::ImageConstPtr &msg_in); + void compressed_cbk(const sensor_msgs::CompressedImageConstPtr &msg_in); void publish_img_rgb(const image_transport::Publisher &pubImage, VIOManagerPtr vio_manager); void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes, VIOManagerPtr vio_manager); void publish_visual_sub_map(const ros::Publisher &pubSubVisualMap); @@ -61,7 +62,7 @@ class LIVMapper template void pointBodyToWorld(const Eigen::Matrix &pi, Eigen::Matrix &po); template Eigen::Matrix pointBodyToWorld(const Eigen::Matrix &pi); cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg); - + cv::Mat getImageFromCompressedMsg(const sensor_msgs::CompressedImageConstPtr &img_msg); std::mutex mtx_buffer, mtx_buffer_imu_prop; std::condition_variable sig_buffer; @@ -72,6 +73,7 @@ class LIVMapper string lid_topic, imu_topic, seq_name, img_topic; V3D extT; M3D extR; + bool is_compressed_image; int feats_down_size = 0, max_iterations = 0; diff --git a/include/preprocess.h b/include/preprocess.h index 69fa04df..794fa64c 100755 --- a/include/preprocess.h +++ b/include/preprocess.h @@ -91,7 +91,7 @@ struct EIGEN_ALIGN16 Point }; } // namespace urbanloco_ros POINT_CLOUD_REGISTER_POINT_STRUCT(urbanloco_ros::Point, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)) + (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint32_t, ring, ring)) /****************/ /*** Ouster ***/ diff --git a/launch/mapping_hk_mems.launch b/launch/mapping_hk_mems.launch new file mode 100755 index 00000000..69a802e2 --- /dev/null +++ b/launch/mapping_hk_mems.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/launch/mapping_urban_loco_ca.launch b/launch/mapping_urban_loco_ca.launch new file mode 100755 index 00000000..e61ec4f3 --- /dev/null +++ b/launch/mapping_urban_loco_ca.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/rviz_cfg/hk_mems.rviz b/rviz_cfg/hk_mems.rviz new file mode 100644 index 00000000..81d94dcd --- /dev/null +++ b/rviz_cfg/hk_mems.rviz @@ -0,0 +1,684 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + - /Axes1 + - /mapping1 + - /mapping1/currPoints1 + - /mapping1/surround1 + - /mapping1/surround1/Autocompute Value Bounds1 + - /mapping1/PointCloud21 + - /Odometry1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Path1 + - /currPoints1/Autocompute Value Bounds1 + - /MarkerArray1/Namespaces1 + - /currPoints2/Autocompute Value Bounds1 + - /Odometry2/Shape1 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /Image1 + - /Image2 + Splitter Ratio: 0.34272301197052 + Tree Height: 360 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz/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: 160 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 238; 238; 236 + Color Transformer: Intensity + Decay Time: 100 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 159 + Min Color: 0; 0; 0 + Min Intensity: 5 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.004999999888241291 + Style: Points + Topic: /cloud_registered + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + 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/PointCloud2 + Color: 239; 41; 41 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.019999999552965164 + Style: Squares + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/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: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.15000000596046448 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.800000011920929 + Shaft Radius: 0.5 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.03999999910593033 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_voxel + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /voxels + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + 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/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + 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/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + 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/PointCloud2 + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 12 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_visual_sub_map + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_sample_points + Unreliable: false + 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/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 239; 41; 41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + 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/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 20 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map_fov + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz/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: 1 + Name: Odometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.699999988079071 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /waypoint_planner/visualize + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /fsm_node/visualization/exp_traj + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /fsm_node/visualization/exp_sfcs + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Image + Enabled: false + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 7.5756306648254395 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.23840096592903137 + Y: -3.2277991771698 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: true + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.670202374458313 + Target Frame: drone + Yaw: 1.3503977060317993 + Saved: + - Class: rviz/Orbit + Distance: 117.53474426269531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -35.713138580322266 + Y: 36.932613372802734 + Z: 4.459701061248779 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.19539840519428253 + Target Frame: + Yaw: 0.17540442943572998 + - Class: rviz/Orbit + Distance: 109.3125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -22.092714309692383 + Y: 63.322662353515625 + Z: 14.125411987304688 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.035398442298173904 + Target Frame: + Yaw: 5.793589115142822 + - Class: rviz/Orbit + Distance: 85.65605163574219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 28.252656936645508 + Y: -35.49672317504883 + Z: -36.31112289428711 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5653983950614929 + Target Frame: + Yaw: 0.9104044437408447 + - Class: rviz/Orbit + Distance: 60.1053581237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 30.61589241027832 + Y: 29.98663330078125 + Z: -12.290168762207031 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.315398633480072 + Target Frame: + Yaw: 5.788588047027588 +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/rviz_cfg/urbanloco.rviz b/rviz_cfg/urbanloco.rviz new file mode 100644 index 00000000..884b5430 --- /dev/null +++ b/rviz_cfg/urbanloco.rviz @@ -0,0 +1,684 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + - /Axes1 + - /mapping1 + - /mapping1/currPoints1 + - /mapping1/surround1 + - /mapping1/surround1/Autocompute Value Bounds1 + - /mapping1/PointCloud21 + - /Odometry1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Path1 + - /currPoints1/Autocompute Value Bounds1 + - /MarkerArray1/Namespaces1 + - /currPoints2/Autocompute Value Bounds1 + - /Odometry2/Shape1 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /Image1 + - /Image2 + Splitter Ratio: 0.34272301197052 + Tree Height: 360 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1 + Class: rviz/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: 160 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 238; 238; 236 + Color Transformer: Intensity + Decay Time: 100 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 159 + Min Color: 0; 0; 0 + Min Intensity: 5 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.004999999888241291 + Style: Points + Topic: /cloud_registered + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + 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/PointCloud2 + Color: 239; 41; 41 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.019999999552965164 + Style: Squares + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/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: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.15000000596046448 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.800000011920929 + Shaft Radius: 0.5 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.03999999910593033 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_voxel + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /voxels + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + 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/PointCloud2 + Color: 245; 121; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 15 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /visualization_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + 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/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + 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/PointCloud2 + Color: 115; 210; 22 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 12 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_visual_sub_map + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 237; 212; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_sample_points + Unreliable: false + 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/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 99999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 239; 41; 41 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_visual_map + Unreliable: false + 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/PointCloud2 + Color: 92; 53; 102 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 20 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_ray_sub_map_fov + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz/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: 1 + Name: Odometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 0.699999988079071 + Axes Radius: 0.20000000298023224 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /aft_mapped_to_init + Unreliable: false + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /waypoint_planner/visualize + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /fsm_node/visualization/exp_traj + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /fsm_node/visualization/exp_sfcs + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: false + Image Topic: /camera/color/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: false + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 771.019287109375 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 38.612308502197266 + Y: 4.728164196014404 + Z: -5.862999023520388e-05 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: true + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.5697963237762451 + Target Frame: drone + Yaw: 0.006785278208553791 + Saved: + - Class: rviz/Orbit + Distance: 117.53474426269531 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -35.713138580322266 + Y: 36.932613372802734 + Z: 4.459701061248779 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.19539840519428253 + Target Frame: + Yaw: 0.17540442943572998 + - Class: rviz/Orbit + Distance: 109.3125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -22.092714309692383 + Y: 63.322662353515625 + Z: 14.125411987304688 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: far2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.035398442298173904 + Target Frame: + Yaw: 5.793589115142822 + - Class: rviz/Orbit + Distance: 85.65605163574219 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 28.252656936645508 + Y: -35.49672317504883 + Z: -36.31112289428711 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near1 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5653983950614929 + Target Frame: + Yaw: 0.9104044437408447 + - Class: rviz/Orbit + Distance: 60.1053581237793 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 30.61589241027832 + Y: 29.98663330078125 + Z: -12.290168762207031 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: near2 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.315398633480072 + Target Frame: + Yaw: 5.788588047027588 +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/rviz_cfg/urbanloco_ca.rviz b/rviz_cfg/urbanloco_ca.rviz new file mode 100644 index 00000000..1dc5df03 --- /dev/null +++ b/rviz_cfg/urbanloco_ca.rviz @@ -0,0 +1,205 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /Axes2 + - /Group1 + - /Group1/CurrPointCloud1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: CurrPointCloud +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + 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: + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: aft_mapped + Show Trail: false + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: CurrPointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03999999910593033 + Style: Flat Squares + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Group + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 71.97770690917969 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 14.618456840515137 + Y: 0.9966355562210083 + Z: -4.635408401489258 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5947967767715454 + Target Frame: + Yaw: 5.023595809936523 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 897 + Y: 323 diff --git a/scripts/calculate_transform.py b/scripts/calculate_transform.py new file mode 100644 index 00000000..fa054075 --- /dev/null +++ b/scripts/calculate_transform.py @@ -0,0 +1,19 @@ +import numpy as np + + +T_camera_lidar = np.array( + [ 0, 0, 1, + -1, 0, 0, + 0, -1, 0] +) +T_camera_lidar = T_camera_lidar.reshape(3, 3) + +T_lidar_imu = np.array( + [ 0, -1, 0, + 1, 0, 0, + 0, 0, 1] +) +T_lidar_imu = T_lidar_imu.reshape(3, 3) + +T_camera_imu = T_camera_lidar @ T_lidar_imu +print(T_camera_imu) \ No newline at end of file diff --git a/scripts/rosbag_to_video.py b/scripts/rosbag_to_video.py new file mode 100644 index 00000000..d7f77cd9 --- /dev/null +++ b/scripts/rosbag_to_video.py @@ -0,0 +1,88 @@ +import rosbag +import cv2 +from cv_bridge import CvBridge +import numpy as np +from sensor_msgs.msg import CompressedImage +import argparse +import os + +def extract_video_from_rosbag(bag_file, topic, output_file, fps=30): + """ + Extract video from rosbag compressed image topic + """ + bridge = CvBridge() + + bag = rosbag.Bag(bag_file, 'r') + + info = bag.get_type_and_topic_info() + if topic not in info.topics: + print(f"Topic {topic} not found in bag file") + print("Available topics:") + for t in info.topics.keys(): + print(f" - {t}") + return False + + fourcc = cv2.VideoWriter_fourcc(*'XVID') + out = None + frame_count = 0 + + print(f"Processing topic: {topic}") + print(f"Output file: {output_file}") + + try: + for topic_name, msg, t in bag.read_messages(topics=[topic]): + try: + if isinstance(msg, CompressedImage): + np_arr = np.frombuffer(msg.data, np.uint8) + cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) + else: + cv_image = bridge.imgmsg_to_cv2(msg, "bgr8") + + if out is None: + height, width, _ = cv_image.shape + out = cv2.VideoWriter(output_file, fourcc, fps, (width, height)) + print(f"Video dimensions: {width}x{height}") + + out.write(cv_image) + frame_count += 1 + + if frame_count % 100 == 0: + print(f"Processed {frame_count} frames...") + + except Exception as e: + print(f"Error processing frame {frame_count}: {e}") + continue + + except Exception as e: + print(f"Error reading bag file: {e}") + return False + + finally: + bag.close() + if out is not None: + out.release() + + print(f"Video export completed! Total frames: {frame_count}") + print(f"Output saved to: {output_file}") + return True + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Extract video from ROS bag file') + parser.add_argument('bag_file', help='Path to the ROS bag file') + parser.add_argument('--topic', default='/camera/color/image_raw/compressed', + help='Image topic to extract (default: /camera/color/image_raw/compressed)') + parser.add_argument('--output', default='output_video.avi', + help='Output video file (default: output_video.avi)') + parser.add_argument('--fps', type=int, default=30, + help='Output video FPS (default: 30)') + + args = parser.parse_args() + + if not os.path.exists(args.bag_file): + print(f"Bag file not found: {args.bag_file}") + exit(1) + + success = extract_video_from_rosbag(args.bag_file, args.topic, args.output, args.fps) + + if not success: + exit(1) diff --git a/scripts/video_to_rosbag.py b/scripts/video_to_rosbag.py new file mode 100644 index 00000000..88febd8e --- /dev/null +++ b/scripts/video_to_rosbag.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python3 + +import rosbag +import cv2 +from cv_bridge import CvBridge +import rospy +from sensor_msgs.msg import Image, CompressedImage +import argparse +import os +from datetime import datetime + +def video_to_rosbag(video_file, output_bag, topic_name="/camera/color/image_raw", compressed=True, fps=None): + """ + Convert video file to ROS bag + """ + if not os.path.exists(video_file): + print(f"Video file not found: {video_file}") + return False + + # Initialize CV bridge + bridge = CvBridge() + + # Open video file + cap = cv2.VideoCapture(video_file) + if not cap.isOpened(): + print(f"Error opening video file: {video_file}") + return False + + # Get video properties + total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) + video_fps = cap.get(cv2.CAP_PROP_FPS) + width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) + height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) + + # Use video FPS if not specified + if fps is None: + fps = video_fps + + print(f"Video properties:") + print(f" - Resolution: {width}x{height}") + print(f" - Total frames: {total_frames}") + print(f" - Original FPS: {video_fps}") + print(f" - Output FPS: {fps}") + print(f" - Compressed: {compressed}") + + # Create rosbag + bag = rosbag.Bag(output_bag, 'w') + + # Calculate time step between frames + time_step = rospy.Duration(1.0 / fps) + start_time = rospy.Time.now() + current_time = start_time + + frame_count = 0 + + try: + while True: + ret, frame = cap.read() + if not ret: + break + + # Convert BGR to RGB (ROS standard) + frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + + if compressed: + # Create compressed image message + msg = CompressedImage() + msg.header.stamp = current_time + msg.header.frame_id = "camera" + msg.format = "jpeg" + + # Encode frame as JPEG + encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90] + result, encimg = cv2.imencode('.jpg', frame, encode_param) + if result: + msg.data = encimg.tobytes() + else: + print(f"Failed to encode frame {frame_count}") + continue + + # Write to bag with compressed topic name + compressed_topic = topic_name + "/compressed" + bag.write(compressed_topic, msg, current_time) + else: + # Create raw image message + try: + msg = bridge.cv2_to_imgmsg(frame_rgb, "rgb8") + msg.header.stamp = current_time + msg.header.frame_id = "camera" + + # Write to bag + bag.write(topic_name, msg, current_time) + except Exception as e: + print(f"Error converting frame {frame_count}: {e}") + continue + + # Update time and frame count + current_time += time_step + frame_count += 1 + + if frame_count % 100 == 0: + progress = (frame_count / total_frames) * 100 + print(f"Progress: {frame_count}/{total_frames} frames ({progress:.1f}%)") + + except KeyboardInterrupt: + print("\nConversion interrupted by user") + + except Exception as e: + print(f"Error during conversion: {e}") + return False + + finally: + cap.release() + bag.close() + + print(f"\nConversion completed!") + print(f" - Processed frames: {frame_count}") + print(f" - Output bag: {output_bag}") + print(f" - Topic: {compressed_topic if compressed else topic_name}") + + return True + +def add_additional_topics(bag_file, imu_topic="/imu/data", lidar_topic="/velodyne_points"): + """ + Add placeholder IMU and LiDAR topics for SLAM compatibility + """ + print(f"Adding placeholder topics to {bag_file}...") + + # This is a placeholder - in real scenarios you'd need actual sensor data + # For now, we just document what topics would be needed + topics_info = { + imu_topic: "sensor_msgs/Imu - IMU data needed for SLAM", + lidar_topic: "sensor_msgs/PointCloud2 - LiDAR data needed for SLAM", + "/tf": "tf2_msgs/TFMessage - Transform data", + "/tf_static": "tf2_msgs/TFMessage - Static transforms" + } + + print("Note: For FAST-LIVO2 SLAM, you'll also need:") + for topic, description in topics_info.items(): + print(f" - {topic}: {description}") + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Convert video file to ROS bag') + parser.add_argument('video_file', help='Path to input video file') + parser.add_argument('--output', '-o', default='output.bag', + help='Output bag file name (default: output.bag)') + parser.add_argument('--topic', default='/camera/color/image_raw', + help='Image topic name (default: /camera/color/image_raw)') + parser.add_argument('--fps', type=float, default=None, + help='Output FPS (default: use video FPS)') + parser.add_argument('--raw', action='store_true', + help='Save as raw images instead of compressed (default: compressed)') + parser.add_argument('--add-slam-topics', action='store_true', + help='Show info about additional topics needed for SLAM') + + args = parser.parse_args() + + # Convert video to bag + success = video_to_rosbag( + video_file=args.video_file, + output_bag=args.output, + topic_name=args.topic, + compressed=not args.raw, + fps=args.fps + ) + + if success and args.add_slam_topics: + add_additional_topics(args.output) + + if not success: + exit(1) + + print(f"\nTo play the bag file:") + print(f" rosbag play {args.output}") + print(f"\nTo view images:") + if args.raw: + print(f" rosrun image_view image_view image:={args.topic}") + else: + print(f" rosrun image_view image_view image:={args.topic} _image_transport:=compressed") diff --git a/src/LIVMapper.cpp b/src/LIVMapper.cpp index 5e6861b3..e97cdeb5 100755 --- a/src/LIVMapper.cpp +++ b/src/LIVMapper.cpp @@ -55,7 +55,7 @@ void LIVMapper::readParameters(ros::NodeHandle &nh) nh.param("common/img_en", img_en, 1); nh.param("common/lidar_en", lidar_en, 1); nh.param("common/img_topic", img_topic, "/left_camera/image"); - + nh.param("common/is_compressed_image", is_compressed_image, false); nh.param("vio/normal_en", normal_en, true); nh.param("vio/inverse_composition_en", inverse_composition_en, false); nh.param("vio/max_iterations", max_iterations, 5); @@ -190,8 +190,14 @@ void LIVMapper::initializeSubscribersAndPublishers(ros::NodeHandle &nh, image_tr nh.subscribe(lid_topic, 200000, &LIVMapper::livox_pcl_cbk, this): nh.subscribe(lid_topic, 200000, &LIVMapper::standard_pcl_cbk, this); sub_imu = nh.subscribe(imu_topic, 200000, &LIVMapper::imu_cbk, this); - sub_img = nh.subscribe(img_topic, 200000, &LIVMapper::img_cbk, this); - + if (is_compressed_image) + { + sub_img = nh.subscribe(img_topic, 200000, &LIVMapper::compressed_cbk, this); + } + else + { + sub_img = nh.subscribe(img_topic, 200000, &LIVMapper::img_cbk, this); + } pubLaserCloudFullRes = nh.advertise("/cloud_registered", 100); pubNormal = nh.advertise("visualization_marker", 100); pubSubVisualMap = nh.advertise("/cloud_visual_sub_map_before", 100); @@ -810,6 +816,20 @@ cv::Mat LIVMapper::getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg) return img; } +cv::Mat LIVMapper::getImageFromCompressedMsg(const sensor_msgs::CompressedImageConstPtr &img_msg) +{ + cv::Mat img; + try + { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8); + img = cv_ptr->image; + } + catch (cv_bridge::Exception &e) + { + ROS_ERROR("cv_bridge exception: %s", e.what()); + } + return img; +} void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in) { if (!img_en) return; @@ -865,6 +885,62 @@ void LIVMapper::img_cbk(const sensor_msgs::ImageConstPtr &msg_in) sig_buffer.notify_all(); } +void LIVMapper::compressed_cbk(const sensor_msgs::CompressedImageConstPtr &msg_in) +{ + if (!img_en) return; + sensor_msgs::CompressedImage::Ptr msg(new sensor_msgs::CompressedImage(*msg_in)); + // if ((abs(msg->header.stamp.toSec() - last_timestamp_img) > 0.2 && last_timestamp_img > 0) || sync_jump_flag) + // { + // ROS_WARN("img jumps %.3f\n", msg->header.stamp.toSec() - last_timestamp_img); + // sync_jump_flag = true; + // msg->header.stamp = ros::Time().fromSec(last_timestamp_img + 0.1); + // } + + // Hiliti2022 40Hz + if (hilti_en) + { + static int frame_counter = 0; + if (++frame_counter % 4 != 0) return; + } + // double msg_header_time = msg->header.stamp.toSec(); + double msg_header_time = msg->header.stamp.toSec() + img_time_offset; + if (abs(msg_header_time - last_timestamp_img) < 0.001) return; + ROS_INFO("Get image, its header time: %.6f", msg_header_time); + if (last_timestamp_lidar < 0) return; + + if (msg_header_time < last_timestamp_img) + { + ROS_ERROR("image loop back. \n"); + return; + } + + mtx_buffer.lock(); + + double img_time_correct = msg_header_time; // last_timestamp_lidar + 0.105; + + if (img_time_correct - last_timestamp_img < 0.02) + { + ROS_WARN("Image need Jumps: %.6f", img_time_correct); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + cv::Mat img_cur = getImageFromCompressedMsg(msg); + img_buffer.push_back(img_cur); + img_time_buffer.push_back(img_time_correct); + + // ROS_INFO("Correct Image time: %.6f", img_time_correct); + + last_timestamp_img = img_time_correct; + // cv::imshow("img", img); + // cv::waitKey(1); + // cout<<"last_timestamp_img:::"< pl_orig; - pcl::fromROSMsg(*msg, pl_orig); - int plsize = pl_orig.points.size(); - if (plsize == 0) return; - pl_surf.reserve(plsize); - - bool is_first[MAX_LINE_NUM]; - double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point - double omega_l = 3.61; // scan angular velocity - float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point - float time_last[MAX_LINE_NUM] = {0.0}; // last offset time - - - given_offset_time = false; - memset(is_first, true, sizeof(is_first)); - double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; - double yaw_end = yaw_first; - int layer_first = pl_orig.points[0].ring; - for (uint i = plsize - 1; i > 0; i--) - { - if (pl_orig.points[i].ring == layer_first) - { - yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; - break; - } - } - - if (feature_enabled) - { - for (int i = 0; i < N_SCANS; i++) - { - pl_buff[i].clear(); - pl_buff[i].reserve(plsize); - } + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + std::cout << "[UrbanlocoHandler] plsize: " << plsize << std::endl; + if (plsize == 0) + return; + pl_surf.reserve(plsize); + + bool is_first[32]; + double yaw_fp[32] = {0}; // yaw of first scan point + double omega_l = 3.61; // scan angular velocity + float yaw_last[32] = {0.0}; // yaw of last scan point + float time_last[32] = {0.0}; // last offset time - for (int i = 0; i < plsize; i++) + given_offset_time = false; + memset(is_first, true, sizeof(is_first)); + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + std::cout << "layer_first: " << layer_first << std::endl; + for (uint i = plsize - 1; i > 0; i--) { - PointType added_pt; - added_pt.normal_x = 0; - added_pt.normal_y = 0; - added_pt.normal_z = 0; - int layer = pl_orig.points[i].ring; - if (layer >= N_SCANS) continue; - added_pt.x = pl_orig.points[i].x; - added_pt.y = pl_orig.points[i].y; - added_pt.z = pl_orig.points[i].z; - added_pt.intensity = pl_orig.points[i].intensity; - - if (!given_offset_time) - { - double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; - if (is_first[layer]) + if (pl_orig.points[i].ring == layer_first) { - // printf("layer: %d; is first: %d", layer, is_first[layer]); - yaw_fp[layer] = yaw_angle; - is_first[layer] = false; - added_pt.curvature = 0.0; - yaw_last[layer] = yaw_angle; - time_last[layer] = added_pt.curvature; - continue; + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; } - - if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } - else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } - - if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; - - yaw_last[layer] = yaw_angle; - time_last[layer] = added_pt.curvature; - } - - pl_buff[layer].points.push_back(added_pt); } - - for (int j = 0; j < N_SCANS; j++) - { - PointCloudXYZI &pl = pl_buff[j]; - int linesize = pl.size(); - if (linesize < 2) continue; - vector &types = typess[j]; - types.clear(); - types.resize(linesize); - linesize--; - for (uint i = 0; i < linesize; i++) - { - types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); - vx = pl[i].x - pl[i + 1].x; - vy = pl[i].y - pl[i + 1].y; - vz = pl[i].z - pl[i + 1].z; - types[i].dista = vx * vx + vy * vy + vz * vz; - } - types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); - give_feature(pl, types); - } - } - else - { + std::cout << "yaw_end: " << yaw_end << std::endl; for (int i = 0; i < plsize; i++) { - PointType added_pt; + PointType added_pt; - added_pt.normal_x = 0; - added_pt.normal_y = 0; - added_pt.normal_z = 0; - added_pt.x = pl_orig.points[i].x; - added_pt.y = pl_orig.points[i].y; - added_pt.z = pl_orig.points[i].z; - added_pt.intensity = pl_orig.points[i].intensity; - - if (!given_offset_time) - { - int layer = pl_orig.points[i].ring; - double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; - if (is_first[layer]) + if (!given_offset_time) { - // printf("layer: %d; is first: %d", layer, is_first[layer]); - yaw_fp[layer] = yaw_angle; - is_first[layer] = false; - added_pt.curvature = 0.0; - yaw_last[layer] = yaw_angle; - time_last[layer] = added_pt.curvature; - continue; + int layer = pl_orig.points[i].ring; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if(layer > 31) + { + continue; + } + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer] = yaw_angle; + is_first[layer] = false; + added_pt.curvature = 0.0; + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; + continue; + } + + // compute offset time + if (yaw_angle <= yaw_fp[layer]) + { + added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; + } + else + { + added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; + } + + if (added_pt.curvature < time_last[layer]) + added_pt.curvature += 360.0 / omega_l; + + // added_pt.curvature = pl_orig.points[i].t; + + yaw_last[layer] = yaw_angle; + time_last[layer] = added_pt.curvature; } - // compute offset time - if (yaw_angle <= yaw_fp[layer]) { added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l; } - else { added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } - - if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l; - - // added_pt.curvature = pl_orig.points[i].t; - - yaw_last[layer] = yaw_angle; - time_last[layer] = added_pt.curvature; - } - - // if(i==(plsize-1)) printf("index: %d layer: %d, yaw: %lf, offset-time: - // %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature, - // prints); - if (i % point_filter_num == 0) - { - if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind_sqr) + // if(i==(plsize-1)) printf("index: %d layer: %d, yaw: %lf, offset-time: + // %lf, condition: %d\n", i, layer, yaw_angle, added_pt.curvature, + // prints); + if (i % point_filter_num == 0) { - pl_surf.points.push_back(added_pt); - // printf("time mode: %d time: %d \n", given_offset_time, - // pl_orig.points[i].t); + if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind) + { + pl_surf.points.push_back(added_pt); + // printf("time mode: %d time: %d \n", given_offset_time, + // pl_orig.points[i].t); + } } - } } - } - // pub_func(pl_surf, pub_full, msg->header.stamp); - // pub_func(pl_surf, pub_surf, msg->header.stamp); - // pub_func(pl_surf, pub_corn, msg->header.stamp); - + std::cout << "[UrbanLocoHandler] end. " << std::endl; + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_surf, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); } void Preprocess::Pandar128_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)