Fixed 3d unknown

stretch_rtabmap/config/3d_unknown/base_local_planner_params.yaml

@@ -1,20 +1,20 @@
 base_local_planner: "dwa_local_planner/DWAPlannerROS"
 
 TrajectoryPlannerROS:
-  acc_lim_x: 0.35
+  acc_lim_x: 0.20
   acc_lim_y: 0.0
-  acc_lim_theta: 0.8
-  max_vel_x: 1.0
+  acc_lim_theta: 0.20
+  max_vel_x: 0.25
   min_vel_x: 0.15
   max_vel_y: 0.0
   min_vel_y: 0.0
-  max_vel_theta: 1.5
-  min_vel_theta: -1.5
+  max_vel_theta: 0.25
+  min_vel_theta: -0.25
   min_in_place_vel_theta: 0.3
   holonomic_robot: false
   # base vel/accel profile is in robot folders
   # tolerances (defaults)
-  yaw_goal_tolerance: 0.1
+  yaw_goal_tolerance: 0.05
   xy_goal_tolerance: 0.1
   latch_xy_goal_tolerance: true
   # forward simulation
@@ -39,35 +39,35 @@ TrajectoryPlannerROS:
 
 DWAPlannerROS:
   #vel params
-  max_vel_x: 0.6 #safe speed for fetch to travel at
+  max_vel_x: 0.3 #safe speed for fetch to travel at
   min_vel_x: 0
-  max_vel_trans: 0.6
-  min_vel_trans: 0.1
-  max_vel_y: 0  #diff drive
+  max_vel_trans: 0.3
+  min_vel_trans: 0
+  max_vel_y: 0 #diff drive
   min_vel_y: 0
   max_vel_theta: 1.5
-  min_vel_theta: 0.4
+  min_vel_theta: -1.5
 
   #accel params
   acc_lim_x: 0.7
-  acc_lim_y: 0  #diff drive
+  acc_lim_y: 0 #diff drive
   acc_lim_trans: 0.7
   acc_lim_theta: 4
 
   #forward sim params
   sim_time: 1.1
-  vx_samples: 5
+  vx_samples: 3 #was 5
   vy_samples: 0 #diff drive
   vth_samples: 15
 
   #scoring params
-  path_distance_bias: 80
-  goal_distance_bias: 60
-  occdist_scale: 1.4
+  path_distance_bias: 32
+  goal_distance_bias: 24
+  occdist_scale: 0.1
   forward_point_distance: 0.3
 
   #higher goal tolerance
-  yaw_goal_tolerance: 0.1
+  yaw_goal_tolerance: 0.05
   xy_goal_tolerance: 0.2
   latch_xy_goal_tolerance: true
 

stretch_rtabmap/config/3d_unknown/costmap_common_params.yaml

@@ -1,7 +1,8 @@
 obstacle_range: 2.5
 raytrace_range: 2.5
 
-robot_radius: 0.50
+# robot_radius: 0.50
+footprint: [[0.05, 0.175], [-0.30, 0.175], [-0.30, -0.175], [0.05, -0.175]]
 
 footprint_padding: 0.0
 
@@ -10,63 +11,63 @@ map_type: costmap
 
 3d_obstacles:
   enabled: true
-  voxel_decay: 15     #seconds if linear, e^n if exponential
-  decay_model: 0      #0=linear, 1=exponential, -1=persistent
-  voxel_size: 0.05   #meters
-  track_unknown_space: true   #default space is unknown
-  observation_persistence: 0.0  #seconds
-  unknown_threshold: 15     #voxel height
-  mark_threshold: 0      #voxel height
+  voxel_decay: 15 #seconds if linear, e^n if exponential
+  decay_model: 0 #0=linear, 1=exponential, -1=persistent
+  voxel_size: 0.05 #meters
+  track_unknown_space: true #default space is unknown
+  observation_persistence: 0.0 #seconds
+  unknown_threshold: 15 #voxel height
+  mark_threshold: 0 #voxel height
   update_footprint_enabled: true
-  combination_method: 1      #1=max, 0=override
+  combination_method: 1 #1=max, 0=override
 
-  origin_z: 0.0    #meters
-  publish_voxel_map: true   # default off
+  origin_z: 0.0 #meters
+  publish_voxel_map: true # default off
   transform_tolerance: 0.4
-  mapping_mode: false  # default off, saves map not for navigation
-  map_save_duration: 60     #default 60s, how often to autosave
+  mapping_mode: false # default off, saves map not for navigation
+  map_save_duration: 60 #default 60s, how often to autosave
   observation_sources: positive_obstacle negative_obstacle
   positive_obstacle:
-    enabled: true                #default true, can be toggled on/off with associated service call
+    enabled: true #default true, can be toggled on/off with associated service call
     data_type: PointCloud2
     topic: /camera/depth/color/points
     marking: true
     clearing: true
-    min_obstacle_height: 0.05                  #default 0, meters
+    min_obstacle_height: 0.05 #default 0, meters
     #robot config defines max height here
     min_z: -0.1
     max_z: 6
-    vertical_fov_angle: 0.7      #default 0.7, radians
-    horizontal_fov_angle: 1.04   #default 1.04, radians
-    decay_acceleration: 1       #default 0, 1/s^2. If laser scanner MUST be 0
-    model_type: 0                #default 0 (depth camera). Use 1 for 3D Lidar
+    vertical_fov_angle: 0.7 #default 0.7, radians
+    horizontal_fov_angle: 1.04 #default 1.04, radians
+    decay_acceleration: 1 #default 0, 1/s^2. If laser scanner MUST be 0
+    model_type: 0 #default 0 (depth camera). Use 1 for 3D Lidar
 
   negative_obstacle:
-    enabled: true                #default true, can be toggled on/off with associated service call
+    enabled: true #default true, can be toggled on/off with associated service call
     data_type: PointCloud2
     topic: /camera/depth/color/points
     marking: true
     clearing: false
-    min_obstacle_height: -5                   #default 0, meters
+    min_obstacle_height: -5 #default 0, meters
     max_obstacle_height: -0.2
     min_z: -6
     max_z: 0.1
-    vertical_fov_angle: 0.785398      #default 0.7, radians
-    horizontal_fov_angle: 0.942478   #default 1.04, radians
-    decay_acceleration: 1       #default 0, 1/s^2. If laser scanner MUST be 0
-    model_type: 0                #default 0 (depth camera). Use 1 for 3D Lidar
-
+    vertical_fov_angle: 0.785398 #default 0.7, radians
+    horizontal_fov_angle: 0.942478 #default 1.04, radians
+    decay_acceleration: 1 #default 0, 1/s^2. If laser scanner MUST be 0
+    model_type: 0 #default 0 (depth camera). Use 1 for 3D Lidar
 
 2d_obstacles:
   observation_sources: laser
-  laser: {data_type: LaserScan,
-          topic: /scan,
-          marking: true,
-          clearing: true,
-          expected_update_rate: 0.3,
-          track_unknown_space: true
-          }
-
+  laser:
+    {
+      data_type: LaserScan,
+      topic: /scan,
+      marking: true,
+      clearing: true,
+      expected_update_rate: 0.3,
+      track_unknown_space: true,
+    }
 
 inflation_layer:
   inflation_radius: 0.75

stretch_rtabmap/config/3d_unknown/global_costmap_params.yaml

@@ -1,8 +1,8 @@
 global_costmap:
   plugins:
-  - {name: 3d_obstacles, type: "nav2_costmap_2d::VoxelLayer"}
-  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
-  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+    - { name: 3d_obstacles, type: "costmap_2d::VoxelLayer" }
+    - { name: 2d_obstacles, type: "costmap_2d::ObstacleLayer" }
+    - { name: inflation_layer, type: "costmap_2d::InflationLayer" }
   global_frame: odom
   robot_base_frame: base_link
   update_frequency: 2.0

stretch_rtabmap/config/3d_unknown/local_costmap_params.yaml

@@ -1,13 +1,13 @@
 local_costmap:
   plugins:
-  - {name: 3d_obstacles, type: "nav2_costmap_2d::VoxelLayer"}
-  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
-  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+    - { name: 3d_obstacles, type: "costmap_2d::VoxelLayer" }
+    - { name: 2d_obstacles, type: "costmap_2d::ObstacleLayer" }
+    - { name: inflation_layer, type: "costmap_2d::InflationLayer" }
   global_frame: odom
   robot_base_frame: base_link
-  update_frequency: 10.0
-  publish_frequency: 5.0
+  update_frequency: 5.0
+  publish_frequency: 2.0
   rolling_window: true
   width: 5.0
   height: 5.0
-  resolution: 0.06
+  resolution: 0.05