Navfn doesn't plan to avoid Obstacle from PointCloud Data [closed]

edit

Greeting everyone,

I'd like to ask you for a favor. Right now, I'm working on a robot that attaches 2 sensors
1) Lidar sensor(LaserScan)
2) Point cloud sensor(PointCloud2) which come from Intel Realsense D435i

The problem is when Navfn generate a path, it (probably) doesn't consider the obstacle that coming from Realsense sensor but the obstacle detected from lidar is fine. This problem is also acually occur to the local planner(I'm using eband).

Here's images https://imgur.com/a/sU1gyz7

What I expect is, the generated path should've shifted to the right where the robot can actually move like the drawn blue line, https://imgur.com/a/WbxXnPw

Please have a look at the config file, I think I might've missing something important.

move_base.launch

<launch>
 <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
  <param name="base_global_planner" value="navfn/NavfnROS" />
  <param name="base_local_planner" value="eband_local_planner/EBandPlannerROS" />
  <param name="planner_patience" type="double" value="2.0" />
  <param name="conservative_reset_dist" type="double" value="30"/>

  <rosparam file="$(find faso)/config/move_base_config/costmap_common_params.yaml" command="load" ns="global_costmap" />
  <rosparam file="$(find faso)/config/move_base_config/costmap_common_params.yaml" command="load" ns="local_costmap" />
  <rosparam file="$(find faso)/config/move_base_config/eband_local_planner_params.yaml" command="load" />
  <rosparam file="$(find faso)/config/move_base_config/local_costmap_params.yaml" command="load" />
  <rosparam file="$(find faso)/config/move_base_config/global_costmap_params.yaml" command="load" /> 
 </node>
</launch> 

costmap_common_params.yaml

obstacle_range: 3.0
raytrace_range: 8.0
#footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
robot_radius: 0.26

inflation_layer:
  inflation_radius: 1.2
  cost_scaling_factor: 3

global_costmap_params.yaml

global_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  resolution: 0.03

  plugins:
  - {name: static_layer,              type: "costmap_2d::StaticLayer"}
  - {name: obstacles_layer_lidar,     type: "costmap_2d::ObstacleLayer"}
  - {name: obstacles_layer_camera,    type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,           type: "costmap_2d::InflationLayer"}

  static_map: true

  obstacles_layer_lidar:
    observation_sources: laser_scan_sensor 
    laser_scan_sensor: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

  obstacles_layer_camera:
    observation_sources: point_cloud_sensor
    point_cloud_sensor: {sensor_frame: camera_link, data_type: PointCloud2, topic: /camera/depth/color/points, marking: true, clearing: true, min_obstacle_height: 0.5}


  static_layer:
    map_topic: /map_global
    subscribe_to_updates: false

local_costmap_params.yaml

local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 8.0
  publish_frequency: 4.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.03

  plugins:
  - {name: static_layer,            type: "costmap_2d::StaticLayer"}
  - {name: obstacles_layer_lidar,   type: "costmap_2d::ObstacleLayer"}
  - {name: obstacles_layer_camera,  type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

  obstacles_layer_lidar:
    observation_sources: laser_scan_sensor 
    laser_scan_sensor: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

  obstacles_layer_camera:
    observation_sources: point_cloud_sensor
    point_cloud_sensor: {sensor_frame: camera_link, data_type: PointCloud2, topic: /camera/depth/color/points, marking: true, clearing: true, min_obstacle_height: 0.5}


  static_layer:
    map_topic: /map_local
    subscribe_to_updates: true

eband_local_planner_params.yaml

EBandPlannerROS:

  yaw_goal_tolerance: 0.10
  xy_goal_tolerance: 0.3
  rot_stopped_vel: 0.05
  trans_stopped_vel: 0.025

  costmap_weight: 2.5

  max_vel_lin: 0.5
  min_vel_lin: 0.05
  max_vel_th: 0.75
  min_vel_th: 0.02

  max_translational_acceleration: 2.5
  max_rotational_acceleration: 2.5
  eband_external_force_gain: 1.0

  bubble_velocity_multiplier: 3.0

  k_prop: 2.0
  k_damp: 5.0

  rotation_threshold_multiplier: 0.7
  Ctrl_Rate: 10

Thank you. Norawit