diff --git a/config/camera_hole_detector_far_config.yaml b/config/camera_hole_detector_far_config.yaml index 0e90a86df029fd7bc09020b710bc16cb8e38bbe7..fe972dc80f908a370e131d90445118b5e8bcb811 100755 --- a/config/camera_hole_detector_far_config.yaml +++ b/config/camera_hole_detector_far_config.yaml @@ -1,7 +1,7 @@ hole_min_p: 25 num_cells: 3 -box_y: 0.8 +box_y: 0.9 box_z_ini: -0.1 box_z_end: 0.1 -box_x_ini: 1.0 +box_x_ini: 0.9 box_x_end: 1.4 diff --git a/config/camera_hole_detector_near_config.yaml b/config/camera_hole_detector_near_config.yaml index 84ad071579b43db7c8ab6123f87944dc686a4a04..adb3fd0a019745284cc1a700f659fc855eaf7c5d 100755 --- a/config/camera_hole_detector_near_config.yaml +++ b/config/camera_hole_detector_near_config.yaml @@ -1,7 +1,7 @@ hole_min_p: 25 num_cells: 2 -box_y: 0.45 +box_y: 0.65 box_z_ini: -0.1 box_z_end: 0.1 -box_x_ini: 0.6 -box_x_end: 1.0 +box_x_ini: 0.5 +box_x_end: 0.9 diff --git a/config/lidar_angle_filter_config.yaml b/config/lidar_angle_filter_config.yaml index 99147047bed3d60aca3e61333c670801f61c41fe..8207f4b3f54d3b19334292521690f63506b61826 100644 --- a/config/lidar_angle_filter_config.yaml +++ b/config/lidar_angle_filter_config.yaml @@ -1,3 +1,18 @@ -n_pairs: 0 +n_pairs: 4 ranges: - range1 + - range2 + - range3 + - range4 +range1: + min: 0.75 + max: 1.0 +range2: + min: -1.17 + max: -0.90 +range3: + min: 1.85 + max: 2.15 +range4: + min: -2.35 + max: -2.05 diff --git a/params/costmap/3d_common_params.yaml b/params/costmap/3d_common_params.yaml index 402900c58a3d01018470d7ee2f613ee24cb94e7e..6b9295d6a7a8442e3f34dae8ee12a262ff10fe35 100755 --- a/params/costmap/3d_common_params.yaml +++ b/params/costmap/3d_common_params.yaml @@ -29,7 +29,7 @@ obstacle_layer_lidar: topic: /ana/lidar_detector/obstacles, observation_persistence: 0.0, marking: true, - clearing: false, + clearing: true, min_obstacle_height: -10.0, max_obstacle_height: 10.0, expected_update_rate: 1, diff --git a/rviz/3d_ana.rviz b/rviz/3d_ana.rviz index 40689e0035212c2c17611b7d7b5f5648dd84be16..7343f4b4b6034c96eaa37fcdd1e5723ed46de1c5 100644 --- a/rviz/3d_ana.rviz +++ b/rviz/3d_ana.rviz @@ -6,7 +6,7 @@ Panels: Expanded: - /Global Options1 - /TF1/Frames1 - - /Nav1 + - /Sensors1 - /3d_nav_detection1 - /3d_nav_detection1/lidar1 - /3d_nav_detection1/camera1 @@ -30,7 +30,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: far_hole_vision + SyncSource: Velodyne - Class: rviz_plugin_tutorials/Teleop Name: Teleop Topic: "" @@ -78,150 +78,149 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - base_footprint: + ana_realsense_support_top: Alpha: 1 Show Axes: false Show Trail: false - base_link: + base_footprint: Alpha: 1 Show Axes: false Show Trail: false - Value: true - camera_bottom_screw_frame: + base_link: Alpha: 1 Show Axes: false Show Trail: false - camera_color_frame: + Value: true + front_left_axle: Alpha: 1 Show Axes: false Show Trail: false - camera_color_optical_frame: + Value: true + front_left_hub: Alpha: 1 Show Axes: false Show Trail: false - camera_depth_frame: + Value: true + front_left_wheel: Alpha: 1 Show Axes: false Show Trail: false - camera_depth_optical_frame: + Value: true + front_right_axle: Alpha: 1 Show Axes: false Show Trail: false - camera_infra1_frame: + Value: true + front_right_hub: Alpha: 1 Show Axes: false Show Trail: false - camera_infra1_optical_frame: + Value: true + front_right_wheel: Alpha: 1 Show Axes: false Show Trail: false - camera_infra2_frame: + Value: true + front_sonar: Alpha: 1 Show Axes: false Show Trail: false - camera_infra2_optical_frame: + Value: true + imu_bno055: Alpha: 1 Show Axes: false Show Trail: false - camera_link: + imu_sim_bno055: Alpha: 1 Show Axes: false Show Trail: false Value: true - camera_pan_tilt_bottom_screw_frame: + imu_sim_bno055_base: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_color_frame: + nav_cam_bottom_screw_frame: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_color_optical_frame: + nav_cam_color_frame: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_depth_frame: + nav_cam_color_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_depth_optical_frame: + nav_cam_depth_frame: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_infra1_frame: + nav_cam_depth_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_infra1_optical_frame: + nav_cam_infra1_frame: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_infra2_frame: + nav_cam_infra1_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_infra2_optical_frame: + nav_cam_infra2_frame: Alpha: 1 Show Axes: false Show Trail: false - camera_pan_tilt_link: + nav_cam_infra2_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_left_axle: + nav_cam_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - front_left_hub: + nav_cam_top_bottom_screw_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_left_wheel: + nav_cam_top_color_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_right_axle: + nav_cam_top_color_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_right_hub: + nav_cam_top_depth_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_right_wheel: + nav_cam_top_depth_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - front_sonar: + nav_cam_top_infra1_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - imu_bno055: + nav_cam_top_infra1_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - imu_bno055_base: + nav_cam_top_infra2_frame: Alpha: 1 Show Axes: false Show Trail: false - pan_frame: + nav_cam_top_infra2_optical_frame: Alpha: 1 Show Axes: false Show Trail: false - Value: true - realsense_support_pan_tilt: + nav_cam_top_link: Alpha: 1 Show Axes: false Show Trail: false + Value: true rear_left_axle: Alpha: 1 Show Axes: false @@ -257,11 +256,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - tilt_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true top_plate: Alpha: 1 Show Axes: false @@ -292,77 +286,79 @@ Visualization Manager: Value: true ana/ana_realsense_support: Value: true + ana/ana_realsense_support_top: + Value: true ana/base_footprint: Value: true ana/base_footprint_robot: Value: true ana/base_link: Value: true - ana/camera_aligned_depth_to_color_frame: + ana/front_left_axle: Value: true - ana/camera_bottom_screw_frame: + ana/front_left_hub: Value: true - ana/camera_color_frame: + ana/front_left_wheel: Value: true - ana/camera_color_optical_frame: + ana/front_right_axle: Value: true - ana/camera_depth_frame: + ana/front_right_hub: Value: true - ana/camera_depth_optical_frame: + ana/front_right_wheel: Value: true - ana/camera_infra1_frame: + ana/front_sonar: Value: true - ana/camera_infra1_optical_frame: + ana/imu_bno055: Value: true - ana/camera_infra2_frame: + ana/imu_sim_bno055: Value: true - ana/camera_infra2_optical_frame: + ana/imu_sim_bno055_base: Value: true - ana/camera_link: + ana/nav_cam_aligned_depth_to_color_frame: Value: true - ana/camera_pan_tilt_bottom_screw_frame: + ana/nav_cam_bottom_screw_frame: Value: true - ana/camera_pan_tilt_color_frame: + ana/nav_cam_color_frame: Value: true - ana/camera_pan_tilt_color_optical_frame: + ana/nav_cam_color_optical_frame: Value: true - ana/camera_pan_tilt_depth_frame: + ana/nav_cam_depth_frame: Value: true - ana/camera_pan_tilt_depth_optical_frame: + ana/nav_cam_depth_optical_frame: Value: true - ana/camera_pan_tilt_infra1_frame: + ana/nav_cam_infra1_frame: Value: true - ana/camera_pan_tilt_infra1_optical_frame: + ana/nav_cam_infra1_optical_frame: Value: true - ana/camera_pan_tilt_infra2_frame: + ana/nav_cam_infra2_frame: Value: true - ana/camera_pan_tilt_infra2_optical_frame: + ana/nav_cam_infra2_optical_frame: Value: true - ana/camera_pan_tilt_link: + ana/nav_cam_link: Value: true - ana/front_left_axle: + ana/nav_cam_top_aligned_depth_to_color_frame: Value: true - ana/front_left_hub: + ana/nav_cam_top_bottom_screw_frame: Value: true - ana/front_left_wheel: + ana/nav_cam_top_color_frame: Value: true - ana/front_right_axle: + ana/nav_cam_top_color_optical_frame: Value: true - ana/front_right_hub: + ana/nav_cam_top_depth_frame: Value: true - ana/front_right_wheel: + ana/nav_cam_top_depth_optical_frame: Value: true - ana/front_sonar: + ana/nav_cam_top_infra1_frame: Value: true - ana/imu_bno055: + ana/nav_cam_top_infra1_optical_frame: Value: true - ana/imu_bno055_base: + ana/nav_cam_top_infra2_frame: Value: true - ana/odom: + ana/nav_cam_top_infra2_optical_frame: Value: true - ana/pan_frame: + ana/nav_cam_top_link: Value: true - ana/realsense_support_pan_tilt: + ana/odom: Value: true ana/rear_left_axle: Value: true @@ -384,6 +380,14 @@ Visualization Manager: Value: true ana/velodyne_base: Value: true + camera_accel_frame: + Value: true + camera_accel_optical_frame: + Value: true + camera_gyro_frame: + Value: true + camera_gyro_optical_frame: + Value: true map: Value: true Marker Scale: 0.10000000149011612 @@ -418,30 +422,54 @@ Visualization Manager: {} ana/top_plate: ana/ana_box: - ana/pan_frame: - {} + {} ana/ana_realsense_support: - ana/camera_bottom_screw_frame: - ana/camera_link: - ana/camera_aligned_depth_to_color_frame: - ana/camera_color_optical_frame: + ana/nav_cam_bottom_screw_frame: + ana/nav_cam_link: + ana/nav_cam_aligned_depth_to_color_frame: + ana/nav_cam_color_optical_frame: + {} + ana/nav_cam_color_frame: + {} + ana/nav_cam_depth_frame: + ana/nav_cam_depth_optical_frame: {} - ana/camera_color_frame: + ana/nav_cam_infra1_frame: + ana/nav_cam_infra1_optical_frame: + {} + ana/nav_cam_infra2_frame: + ana/nav_cam_infra2_optical_frame: + {} + ana/ana_realsense_support_top: + ana/nav_cam_top_bottom_screw_frame: + ana/nav_cam_top_link: + ana/nav_cam_top_aligned_depth_to_color_frame: + ana/nav_cam_top_color_optical_frame: + {} + ana/nav_cam_top_color_frame: {} - ana/camera_depth_frame: - ana/camera_depth_optical_frame: + ana/nav_cam_top_depth_frame: + ana/nav_cam_top_depth_optical_frame: {} - ana/camera_infra1_frame: - ana/camera_infra1_optical_frame: + ana/nav_cam_top_infra1_frame: + ana/nav_cam_top_infra1_optical_frame: {} - ana/camera_infra2_frame: - ana/camera_infra2_optical_frame: + ana/nav_cam_top_infra2_frame: + ana/nav_cam_top_infra2_optical_frame: + {} + camera_accel_frame: + camera_accel_optical_frame: + {} + camera_gyro_frame: + camera_gyro_optical_frame: {} ana/velodyne_base: ana/velodyne: {} - ana/imu_bno055_base: - ana/imu_bno055: + ana/imu_bno055: + {} + ana/imu_sim_bno055_base: + ana/imu_sim_bno055: {} ana/base_footprint_robot: {} @@ -459,7 +487,7 @@ Visualization Manager: Unreliable: false Value: false - Class: rviz/Image - Enabled: true + Enabled: false Image Topic: /ana/sensors/nav_cam/color/image_raw Max Value: 1 Median window: 5 @@ -469,7 +497,7 @@ Visualization Manager: Queue Size: 2 Transport Hint: raw Unreliable: false - Value: true + Value: false - Alpha: 0.5 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -482,7 +510,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -495,16 +523,16 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points - Topic: /ana/sensors/nav_cam/depth_registered/points + Topic: /ana/sensors/nav_cam_top/depth_registered/points Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 16.410022735595703 - Min Value: -0.006432712078094482 + Max Value: 5.824672222137451 + Min Value: -0.5254461169242859 Value: true Axis: Z Channel Name: intensity @@ -542,7 +570,7 @@ Visualization Manager: Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -559,8 +587,8 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true - Enabled: false + Value: false + Enabled: true Name: Sensors - Class: rviz/Group Displays: @@ -720,7 +748,7 @@ Visualization Manager: Topic: /ana/move_base/current_goal Unreliable: false Value: true - Enabled: true + Enabled: false Name: Nav - Class: rviz/Group Displays: @@ -738,7 +766,7 @@ Visualization Manager: Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -755,7 +783,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -768,10 +796,10 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 157 + Max Intensity: 121 Min Color: 0; 0; 0 Min Intensity: 1 Name: lidar_zone @@ -785,7 +813,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Class: rviz/Image Enabled: false Image Topic: /ana/lidar_detector/obstacles_img/image_raw @@ -982,7 +1010,7 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true - Enabled: true + Enabled: false Name: camera Enabled: true Name: 3d_nav_detection @@ -1014,25 +1042,25 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 3.5477471351623535 + Distance: 7.981015682220459 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.5333200693130493 - Y: 0.21448162198066711 - Z: -9.5367431640625e-7 + X: -0.01600027084350586 + Y: 0.20595836639404297 + Z: -1.1920928955078125e-6 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.3147964477539062 + Pitch: 0.6047971844673157 Target Frame: ana/base_footprint Value: XYOrbit (rviz) - Yaw: 1.6722356081008911 + Yaw: 3.2740530967712402 Saved: ~ Window Geometry: Displays: @@ -1040,7 +1068,7 @@ Window Geometry: Height: 1025 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001d3000003a7fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002630000011b0000001600fffffffb0000000c00540065006c0065006f00700000000315000000b30000004700fffffffb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002f9000001090000000000000000000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000007efc0100000003fb00000016006c0069006400610072005f0069006d00610067006500000000000000073d0000008400fffffffb0000000800540069006d006500000000000000073f000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000564000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001d3000003a7fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f0072000000030c000000d80000001600fffffffb0000000c00540065006c0065006f00700000000315000000b30000004700fffffffb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002f9000001090000000000000000000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000007efc0100000003fb00000016006c0069006400610072005f0069006d00610067006500000000000000073d0000008400fffffffb0000000800540069006d006500000000000000073f000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000564000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Teleop: