Robotics StackExchange | Archived questions

Navfn doesn't plan to avoid Obstacle from PointCloud Data

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> 

costmapcommonparams.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

globalcostmapparams.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

localcostmapparams.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

ebandlocalplanner_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

Asked by Carls on 2020-05-11 07:13:20 UTC

Comments

I think the problem is the costmap, it does not look like it is fully occupied under the table, might be that navfn considers it good enough despite the high cost. Could you change the color scheme to "costmap" and deactivate the lidar layer then post another image?

Asked by bob-ROS on 2020-05-11 08:32:52 UTC

Thank you for your comment, you're right, nothing wrong with Navfn. I just found out that I should've used VoxelLayer instead of ObstacleLayer for PointCloud observation.

Asked by Carls on 2020-05-13 03:13:46 UTC

Yes, that was the actual error with this! As @Carls mentioned above it is a common mistake to define the type of obstacles layers in the glocal costmap cofniguration files as "costmap_2D::ObstacleLayer" but it always should be "costmap_2d::VoxelLayer" in global costmap configurations and "costmap_2D::ObstacleLayer" in local costmap configurations! Hope this clarification helps!

Asked by geoporus on 2020-11-13 06:29:39 UTC

Answers