0
$\begingroup$

Rosanswers logo

Hello everyone.

I have the issue as i said in the title that ObstacleLayer and VoxelLayer don't generate obstacle. I am using a Realsense camera, model D435 to generate a pointcloud and then using the depthimage_to_laserscan package transform the depth image topic to laser scan topic. It seems to work.

Then I have configured the costmap package as you can see below:

local_costmap_params.yaml

local_costmap:
  plugins:
    - {name: laser_layer, type: "costmap_2d::ObstacleLayer"} #Laser sensors
    - {name: pointcloud_layer, type: "costmap_2d::VoxelLayer"} #Laser sensors
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}
    #- {name: ultrasonic,       type: "range_sensor_layer::RangeSensorLayer"}

  update_frequency: 2.0 #HIGH CPU usage with sensors
  publish_frequency: 50.0

  global_frame: "odom" #To inflate obstacles
  robot_base_frame: "base_link"

  #static_map: false
  rolling_window: true
  width: 6.0 #6
  height: 6.0 #6
  resolution: 0.05 #0.01

global_costmap_params.yaml

global_costmap:

  plugins:
    - {name: static_map,       type: "costmap_2d::StaticLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
    #- {name: ultrasonic,   type: "range_sensor_layer::RangeSensorLayer"}


  global_frame: "map"
  robot_base_frame: "base_link"

  publish_frequency: 50.0
  update_frequency: 2.0

  resolution: 0.5 #0.01 #The resolution of the map in meters/cell.
  transform_tolerance: 0.2 #Specifies the delay in transform (tf) data that is tolerable in seconds
  map_type: costmap

costmap_common_params.yaml

footprint: [[-0.30 , 0.38], [0.70, 0.38], [0.70, -0.38], [-0.30, -0.38]]

laser_layer: #Laser
  enabled: true
  obstacle_range: 2.0
  raytrace_range: 4.0
  max_obstacle_height: 2.0
  inflation_radius: 0.7
  combination_method: 1
  observation_sources:  scan
  scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0, sensor_frame: camera_depth_frame}

pointcloud_layer: #Nube de puntos
  enabled: true
  obstacle_range: 2.0
  raytrace_range: 4.0
  inflation_radius: 0.7
  observation_sources: pointcloud
  pointcloud: {data_type: PointCloud2, topic: /camera/depth/color/points, marking: true, clearing: true, expected_update_rate: 0}


inflation_layer:
  enabled: true
  inflation_radius: 0.75

base_local_planner_params.yaml

#recovery_behavior_enabled: false
#clearing_rotation_allowed: false
controller_frequency: 10 #Default 20 took many time

TrajectoryPlannerROS:

  max_vel_x: 0.4 #meters/sec #0.6
  min_vel_x: -0.1
  max_vel_y: 0.0  # zero for a differential drive robot
  min_vel_y: 0.0  #radians/sec
  max_vel_theta: 1.0 #3
  min_vel_theta: -1.0
  min_in_place_vel_theta: 0.1 #radians/sec, in-place rotations
  escape_vel: -0.1 #0.1
  acc_lim_x: 0.4 #meters/sec^2
  acc_lim_y: 0.0  # zero for a differential drive robot
  acc_lim_theta: 1.0 #radians/sec^2

  holonomic_robot: false

   #####Trajectory Scoring Parameters#####

  meter_scoring: true #goal_distance and path_distance are expressed in units of meters or cells. Cells false.
  #pdist_scale: 0.4 #The weighting for how much the controller should stay close to the path it was given
  #gdist_scale: 0.8 #The weighting for how much the controller should attempt to reach its local goal, also controls speed

  yaw_goal_tolerance: 0.5 # about 30 degrees, The tolerance in radians for the controller in yaw/rotation when achieving its goal
  xy_goal_tolerance: 0.20  # 5 cm, The tolerance in meters for the controller in the x & y distance when achieving a goal
   #latch_xy_goal_tolerance: false


   #heading_lookahead: 0.325 #How far to look ahead in meters when scoring different in-place-rotation trajectories
   #heading_scoring: false #Whether to score based on the robot's heading to the path or its distance from the path
   #heading_scoring_timestep: 0.8 #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring
  occdist_scale: 0.07 #The weighting for how much the controller should attempt to avoid obstacles

   #dwa: false

   #####Oscillation Prevention Parameters######
   #oscillation_reset_dist: 0.05 #How far the robot must travel in meters before oscillation flags are reset

   ####Others#######
   #publish_cost_grid_pc: false
   #prune_plan: true
   #simple_attractor: false

  ####Forward Simulation Parameters####

  sim_time: 3.0 #The amount of time to forward-simulate trajectories in seconds
  sim_granularity: 0.05 #The step size, in meters, to take between points on a given trajectory
  #angular_sim_granularity: 0.15 #The step size, in radians, to take between angular samples on a given trajectory
  vx_samples: 10 #The number of samples to use when exploring the x velocity space
  vy_samples: 0  # zero for a differential drive robot
  vtheta_samples: 20.0

I am not able to generate obstacle as you can see in the following image of Rviz. (I have to upload the image through drive, because I do not have enough points)

Rviz Capture

What you see in red is the topic /move_base/local_costmap/pointcloud_layer/clearing_endpoints. What you see in green is the topic /scan.

If I try to navigate, it does not avoid the obstacle even If it is seen in the pointcloud.

Anyone of them generate obstacle or inflation and I do not know why. If you know any reason or you see any error in the config files please tell me.

Thank you in advance

Best regrets. Alessandro


Originally posted by Alessandro Melino on ROS Answers with karma: 113 on 2020-03-10

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Please your static layer first. The order matters.

Also, publishing your costmap at 50hz is really unnecessary and will cost you a ton of CPU. Reduce that to at most update rate. Its really there to throttle publishing to less than update rate to save cycles.


Originally posted by stevemacenski with karma: 8272 on 2020-03-10

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Alessandro Melino on 2020-03-10:
Thank you so much. It worked.

About update_frequency and publish_frequency, what is the difference? Because in the costmap manual I do not understand it very well. (Sorry about this questions, but I am a bit noob on this of ROS)

Best regrets.

Comment by stevemacenski on 2020-03-10:
update is how often the costmap thread runs a costmap update cycle (processing all the sensors into a costmap and updating the master costmap). This is costly.

Publish is just how often to publish the costmap to its topic.

Comment by Alessandro Melino on 2020-03-10:
Okay, thank you again, now I understand it.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.