0
$\begingroup$

Rosanswers logo

Hello everyone,

I'm currently implementing the navstack on my robot and I have a problem with the generation of my costmaps : I use the obstacle layer plugin to make dynamic obstacles from the LiDAR that is mounted under my robot. As it is upside down, I use a static tf publisher to indicate "laser" (my laser frame) is upside down compared to "base_link". When I do so, the LaserScan published on /scan topic works well and the obstacles are visibles and coherent with their position around the robot. BUT, when I use move_base, the cosmaps (both global and local leverage from an obstacle layer) don't consider the laser beams behind the robot as obstacles (no obstacle points and no inflation neither). see rviz screenshot here : https://drive.google.com/file/d/1cOVd8EBkpP211TCEeSwLWicVGPWZak-k/view?usp=sharing

When searching for a solution I realized that another robot of mine (with a LiDAR facing up) with the exact same move_base configuration works well (every laser beam is taken as an obstacle). I tested changing the orientation of the laser frame by putting no reversal in the static tf publisher and this time, all points are considered correctly (but obviously every point position is reversed). see attached screenshot : https://drive.google.com/file/d/1o41BcJ6soTZzLnBalyypNAEc2nIv8Fd5/view?usp=sharing

If anyone has any idea or suggestion about how to debugg this, i'd be glad to hear it.

Here is my static tf publisher

<launch>
<node pkg="tf" type="static_transform_publisher" name="laser_tf_pub" args="0 0 0 -0.785 3.14 0 base_link laser 100" />
</launch>

yaw rotation of -0.785 rad is also needed. Here are my local and global costmaps yaml files :

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5  
  
  footprint: [[-0.765, -0.455], [-0.765, 0.455], [0.765, 0.455], [0.765, -0.455]]
  footprint_padding: 0.03
  robot_radius: 0.15
  static_map: true  
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05
  plugins:
    - {name: static_map, type: "costmap_2d::StaticLayer"}
    - {name: obstacles, type: "costmap_2d::ObstacleLayer"}
    - {name: inflation, type: "costmap_2d::InflationLayer"}
  
  inflation:
    inflation_radius: 1.00 # default: 0.55
    cost_scaling_factor: 10.0 # default: 10.0
  static_map:
    unknown_cost_value: 0 # default: 0
    lethal_cost_threshold: 100 # default: 100
    map_topic: map
  obstacles:
    combination_method: 1
    max_obstacle_height: 3
    observation_sources: laser_scan
    map_type: costmap
    unknown_threshold: 2 # default: ~<name>/z_voxels
    mark_threshold: 0 # default: 0
    publish_voxel_map: true
    obstacle_range: 10.0                  # default: 2.5  
    raytrace_range: 11.0                  # default: 3.0 
    laser_scan:
      sensor_frame: laser                 
      topic: /scan               
      data_type: LaserScan                
      marking: true                       
      clearing: true  

Here is the global costmap yaml:

 global_costmap:
  global_frame: map
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5
  
  footprint: [[-0.765, -0.455], [-0.765, 0.455], [0.765, 0.455], [0.765, -0.455]]
  robot_radius: 0.15

  static_map: true  
  rolling_window: false
  
  plugins:
    - {name: static_layer, type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
    - {name: inflater_layer, type: "costmap_2d::InflationLayer"} 
  static_layer:
    unknown_cost_value: 0 # default: 0
    lethal_cost_threshold: 100 # default: 100
    map_topic: map # default: "map"

  obstacle_layer:
    combination_method: 1
    observation_sources: laser_scan
    map_type: costmap
    unknown_threshold: 2 # default: ~<name>/z_voxels
    mark_threshold: 0 # default: 0
    publish_voxel_map: true
    obstacle_range: 10.0                  # default: 2.5  
    raytrace_range: 11.0                  # default: 3.0 
    laser_scan:
      sensor_frame: laser                
      topic: /scan               
      data_type: LaserScan                
      marking: true                       # default: true
      clearing: true                      # default: false

  inflation_layer:
    inflation_radius: 0.45 # default: 0.55
    cost_scaling_factor: 8.01 # default: 10.0

Originally posted by thomzem on ROS Answers with karma: 16 on 2022-07-28

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I found my error ! I considered my LiDAR laser plan to be directly "on the ground" because it didn't seem to be any useful to elevate it from the ground in my case as I didn't see any significant use in 2D navigation. BUT when I oriented my laser frame upside down I added 3.14 rad (not exactly pi) to the pitch rotation value, therefore my laser plan was not exactly parallel to the ground plan and one half of the laser beams was above the ground plan and the other half was under the ground plan (very slightly but half obstacles heights were negative). As my min_obstacle_height and max_obstacle_height params for the obstacle layer were 0.0 and 3.0, hence, every obstacles under the ground plan were not considered.

To solve this in the most coherent way, I added a z translation in the static tf laser->base_link to consider my LiDAR height above the ground so that even if the new laser plan is slightly inclined, all the obstacles will be in the obstacle height range.

<launch>
<node pkg="tf" type="static_transform_publisher" name="laser_tf_pub" args="0 0 0.1 -0.785 3.14159265359 0 base_link laser 100" />
</launch>

Originally posted by thomzem with karma: 16 on 2022-07-29

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

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