Added a lidar clearing source with a PointCloud2.

Changed some configuration parameters: * global costmap resolution: to 0.05 to improve the quality of the global plan. * inflated obstacles radius: to 0.5 to allow navigation in narrow passages. * planner frequency: to 1 to replan when temporal obstacles appear on the path.

Added a lidar clearing source with a PointCloud2.
Changed some configuration parameters: * global costmap resolution: to 0.05 to improve the quality of the global plan. * inflated obstacles radius: to 0.5 to allow navigation in narrow passages. * planner frequency: to 1 to replan when temporal obstacles appear on the path.
350ef325 · Sergi Hernandez · ba77beeb · 350ef325 · 350ef325 · 350ef325
Commit 350ef325 authored 4 years ago by Sergi Hernandez
--- a/config/lidar_angle_filter_config.yaml
+++ b/config/lidar_angle_filter_config.yaml
@@ -17,6 +17,6 @@ range3:
 range4:
  min: -2.2
  max: -2.0
-range4:
+range5:
  min: 2.0
  max: 2.2
--- a/config/lidar_obstacle_detector_config.yaml
+++ b/config/lidar_obstacle_detector_config.yaml
+# sensor information
+base_frame_id: "helena/base_footprint"
+min_range: 0.1
+max_range: 50.0
+sensor_height: 0.3
+# node configuration
+debug: True
+max_inc: 0.3
+min_inc: 0.05
+inc_area_filter: 10
+max_ring: 10
+# farthest point computation
+farthest_enable: False
+farthest_num_rings: 9
+# virtual point computation
+virtual_enable: True
+virtual_floor_threshold: 0.03
--- a/launch/include/3d_nav.launch
+++ b/launch/include/3d_nav.launch
@@ -16,7 +16,7 @@
    <arg name="lidar_filter_config_file" value="$(find iri_helena_rosnav)/config/lidar_angle_filter_config.yaml"/>
    <arg name="lidar_filter_cloud_in" value="/helena/sensors/rslidar_points" />
    <arg name="lidar_detector_node_name" value="lidar_detector"/>
-    <arg name="lidar_detector_config_file" value="$(find iri_lidar_obstacle_detector)/config/default_config.yaml"/>
+    <arg name="lidar_detector_config_file" value="$(find iri_helena_rosnav)/config/lidar_obstacle_detector_config.yaml"/>
  </include>

  <include file="$(find iri_rosnav)/launch/include/3d_nav_camera.launch">

--- a/params/costmap/3d_common_params.yaml
+++ b/params/costmap/3d_common_params.yaml
@@ -11,7 +11,7 @@ always_send_full_costmap: false
 transform_tolerance: 1.0
 width: 10.0
 height: 10.0
-resolution: 0.1
+resolution: 0.05
 origin_x: 0.0
 origin_y: 0.0

@@ -22,11 +22,11 @@ obstacle_layer_lidar:
  max_obstacle_height: 10.0
  footprint_clearing_enabled: false
  combination_method: 1 
-  observation_sources:  lidar_marking
+  observation_sources:  lidar_marking lidar_clearing
  lidar_marking: {
    sensor_frame: helena/base_footprint,
-    data_type: LaserScan,
-    topic: /helena/lidar_detector/scan_out,
+    data_type: PointCloud2,
+    topic: /helena/lidar_detector/obstacles,
    observation_persistence: 0.0,
    marking: true,
    clearing: true,
@@ -36,6 +36,19 @@ obstacle_layer_lidar:
    obstacle_range: 5.0,
    raytrace_range: 200.0
  }
+  lidar_clearing: {
+    sensor_frame: helena/base_footprint,
+    data_type: PointCloud2,
+    topic: /helena/lidar_detector/free_space,
+    observation_persistence: 0.0,
+    marking: false,
+    clearing: true,
+    min_obstacle_height: -10.0,
+    max_obstacle_height: 10.0,
+    expected_update_rate: 1,
+    obstacle_range: 5.0,
+    raytrace_range: 200.0
+  }

 obstacle_layer_camera:
  enabled: true
@@ -137,6 +150,5 @@ hole_layer_far_camera:
 inflation_layer:
  enabled: true
  inflate_unknown: false
-  inflation_radius: 2.0
  cost_scaling_factor:  1.8
-  inflation_radius:     1.0
+  inflation_radius:     0.5
--- a/params/costmap/3d_local_params.yaml
+++ b/params/costmap/3d_local_params.yaml
@@ -5,16 +5,16 @@ publish_frequency: 2.0
 rolling_window: true
 width: 10.0
 height: 10.0
-resolution: 0.1
+resolution: 0.05
 #origin_x: 0.0
 #origin_y: 0.0

 #costmap plugins
 plugins:
 - {name: obstacle_layer_lidar,   type: "costmap_2d::ObstacleLayer"}
- - {name: obstacle_layer_camera,  type: "costmap_2d::ObstacleLayer"}
- - {name: hole_layer_near_camera, type: "costmap_2d::ObstacleLayer"}
- - {name: hole_layer_far_camera,  type: "costmap_2d::ObstacleLayer"}
+   # - {name: obstacle_layer_camera,  type: "costmap_2d::ObstacleLayer"}
+   # - {name: hole_layer_near_camera, type: "costmap_2d::ObstacleLayer"}
+   # - {name: hole_layer_far_camera,  type: "costmap_2d::ObstacleLayer"}
 - {name: inflation_layer,        type: "costmap_2d::InflationLayer"}


--- a/params/costmap/map/3d_global_params.yaml
+++ b/params/costmap/map/3d_global_params.yaml
@@ -7,9 +7,9 @@ rolling_window: false
 plugins:
  - {name: static_layer,          type: "costmap_2d::StaticLayer"}
  - {name: obstacle_layer_lidar,  type: "costmap_2d::ObstacleLayer"}
-  - {name: obstacle_layer_camera, type: "costmap_2d::ObstacleLayer"}
-  - {name: hole_layer_near_camera,type: "costmap_2d::ObstacleLayer"}
-  - {name: hole_layer_far_camera, type: "costmap_2d::ObstacleLayer"}
+    #  - {name: obstacle_layer_camera, type: "costmap_2d::ObstacleLayer"}
+    #  - {name: hole_layer_near_camera,type: "costmap_2d::ObstacleLayer"}
+    #  - {name: hole_layer_far_camera, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,       type: "costmap_2d::InflationLayer"}
    
 static_layer:

--- a/params/costmap/no_map/3d_global_params.yaml
+++ b/params/costmap/no_map/3d_global_params.yaml
@@ -6,13 +6,13 @@ rolling_window: true
 transform_tolerance: 1.0
 width: 20.0
 height: 20.0
-resolution: 0.2
+resolution: 0.05
 origin_x: 0.0
 origin_y: 0.0

 plugins:
  - {name: obstacle_layer_lidar,   type: "costmap_2d::ObstacleLayer"}
-  - {name: obstacle_layer_camera,  type: "costmap_2d::ObstacleLayer"}
-  - {name: hole_layer_near_camera, type: "costmap_2d::ObstacleLayer"}
-  - {name: hole_layer_far_camera,  type: "costmap_2d::ObstacleLayer"}
+    #  - {name: obstacle_layer_camera,  type: "costmap_2d::ObstacleLayer"}
+    #  - {name: hole_layer_near_camera, type: "costmap_2d::ObstacleLayer"}
+    #  - {name: hole_layer_far_camera,  type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,        type: "costmap_2d::InflationLayer"}
--- a/params/move_base_params.yaml
+++ b/params/move_base_params.yaml
@@ -10,7 +10,7 @@ recovery_behaviors:
 controller_frequency: 10.0
 planner_patience: 5.0
 controller_patience: 5.0
-planner_frequency: 0.0 
+planner_frequency: 1.0 
 max_planning_retries: -1 

 conservative_reset_dist: 3.0

--- a/rviz/3d_helena.rviz
+++ b/rviz/3d_helena.rviz
@@ -8,10 +8,11 @@ Panels:
        - /TF1/Frames1
        - /Nav1
        - /3d_nav_detection1
-        - /3d_nav_detection1/lidar1
+        - /3d_nav_detection1/lidar1/lidar_obstacles1
+        - /3d_nav_detection1/lidar1/lidar_free_space1
        - /3d_nav_detection1/camera1
      Splitter Ratio: 0.5486725568771362
-    Tree Height: 691
+    Tree Height: 664
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
@@ -30,7 +31,7 @@ Panels:
    Experimental: false
    Name: Time
    SyncMode: 0
-    SyncSource: lidar_obstacle
+    SyncSource: lidar_image
  - Class: rviz_plugin_tutorials/Teleop
    Name: Teleop
    Topic: ""
@@ -647,13 +648,43 @@ Visualization Manager:
            - Alpha: 1
              Autocompute Intensity Bounds: true
              Autocompute Value Bounds:
-                Max Value: 1.410619621111664e-8
-                Min Value: -1.076524824838998e-8
+                Max Value: 10
+                Min Value: -10
+                Value: true
+              Axis: Z
+              Channel Name: intensity
+              Class: rviz/PointCloud2
+              Color: 204; 0; 0
+              Color Transformer: FlatColor
+              Decay Time: 0
+              Enabled: true
+              Invert Rainbow: false
+              Max Color: 255; 255; 255
+              Max Intensity: 4096
+              Min Color: 0; 0; 0
+              Min Intensity: 0
+              Name: lidar_obstacles
+              Position Transformer: XYZ
+              Queue Size: 10
+              Selectable: true
+              Size (Pixels): 3
+              Size (m): 0.009999999776482582
+              Style: Points
+              Topic: /helena/lidar_detector/obstacles
+              Unreliable: false
+              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/LaserScan
-              Color: 255; 0; 0
+              Class: rviz/PointCloud2
+              Color: 115; 210; 22
              Color Transformer: FlatColor
              Decay Time: 0
              Enabled: true
@@ -662,14 +693,14 @@ Visualization Manager:
              Max Intensity: 4096
              Min Color: 0; 0; 0
              Min Intensity: 0
-              Name: lidar_obstacle
+              Name: lidar_free_space
              Position Transformer: XYZ
              Queue Size: 10
              Selectable: true
-              Size (Pixels): 10
+              Size (Pixels): 3
              Size (m): 0.009999999776482582
              Style: Points
-              Topic: /helena/lidar_detector/scan_out
+              Topic: /helena/lidar_detector/free_space
              Unreliable: false
              Use Fixed Frame: true
              Use rainbow: true
@@ -686,7 +717,7 @@ 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: 0
@@ -703,7 +734,7 @@ Visualization Manager:
              Unreliable: false
              Use Fixed Frame: true
              Use rainbow: true
-              Value: true
+              Value: false
            - Class: rviz/Image
              Enabled: true
              Image Topic: /helena/lidar_detector/obstacles_img/image_raw
@@ -900,7 +931,7 @@ Visualization Manager:
              Use Fixed Frame: true
              Use rainbow: true
              Value: true
-          Enabled: true
+          Enabled: false
          Name: camera
      Enabled: true
      Name: 3d_nav_detection
@@ -932,33 +963,33 @@ Visualization Manager:
  Views:
    Current:
      Class: rviz/Orbit
-      Distance: 10
+      Distance: 9.795035362243652
      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
+        X: -1.1274361610412598
+        Y: -1.261415719985962
+        Z: 0.8315092921257019
      Focal Shape Fixed Size: false
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7853981852531433
+      Pitch: 0.08039873093366623
      Target Frame: helena/base_footprint
      Value: Orbit (rviz)
-      Yaw: 0.7853981852531433
+      Yaw: 4.043567657470703
    Saved: ~
 Window Geometry:
  Displays:
    collapsed: false
-  Height: 1052
+  Height: 1025
  Hide Left Dock: false
  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001d30000033efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000033e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f00720000000288000000d80000001600fffffffb0000000c00540065006c0065006f00700000000315000000b30000004700fffffffb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002f9000001090000000000000000000000010000010f00000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000007efc0100000003fb00000016006c0069006400610072005f0069006d00610067006501000000000000073d0000008400fffffffb0000000800540069006d006500000000000000073f000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005640000033e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d300000323fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000323000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e007200650061006c00730065006e00730065005f0063006f006c006f00720000000288000000d80000001600fffffffb0000000c00540065006c0065006f00700000000315000000b30000004700fffffffb0000001e007200650061006c00730065006e00730065005f0063006f006c006f007200000002f9000001090000000000000000000000010000010f00000323fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000323000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000007efc0100000003fb00000016006c0069006400610072005f0069006d00610067006501000000000000073d0000008400fffffffb0000000800540069006d006500000000000000073f000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005640000032300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Teleop:
@@ -971,7 +1002,7 @@ Window Geometry:
    collapsed: false
  Width: 1853
  X: 67
-  Y: 0
+  Y: 27
  lidar_image:
    collapsed: false
  realsense_color: