Occupancy grid map placed at sensor_frame instead of base_frame

[tseco15]

Hi everyone,

I realized that the occupancy grid map is not placed at base_link but at os_lidar (the link corresponding to the lidar):

The occupancy_grid_map_method is selected as pointcloud_based_occupancy_grid_map and scan_origin_frame param is set to os_lidar in pointcloud_based_occupancy_grid_map.param.yaml:

/**:
  ros__parameters:
    map_length: 150.0     # [m]
    map_resolution: 0.5 # [m]

    height_filter:
      use_height_filter: true
      min_height: -1.0
      max_height: 2.0

    enable_single_frame_mode: false
    # use sensor pointcloud to filter obstacle pointcloud
    filter_obstacle_pointcloud_by_raw_pointcloud: false

    # grid map coordinate
    map_frame: "map"
    base_link_frame: "base_link"
    # center of the grid map
    gridmap_origin_frame: "base_link"
    # ray-tracing center: main sensor frame is preferable like: "velodyne_top"
    # base_link should not be used with "OccupancyGridMapProjectiveBlindSpot"
    scan_origin_frame: "os_lidar" 
    #"base_link"

    grid_map_type: "OccupancyGridMapFixedBlindSpot"
    OccupancyGridMapFixedBlindSpot:
      distance_margin: 1.0
    OccupancyGridMapProjectiveBlindSpot:
      projection_dz_threshold: 0.01 # [m] for avoiding null division
      obstacle_separation_threshold: 1.0 # [m] fill the interval between obstacles with unknown for this length
      pub_debug_grid: false

With this configuration I would expect that the grid map were placed at base_link frame. However, it is shown at the height of the os_lidar frame.

Any suggestions?

Thanks in advance!

[badai-nguyen]

I think a related topic was discussing here:
autowarefoundation/autoware.universe#2906
Could you check it?

[badai-nguyen]

@YoshiRi do you have any comment?

[YoshiRi]

Thank you for your report. I will investigate it.

occupancy grid map is not placed at base_link but at os_lidar

This seems weird.
The OGM frame is set by robot_pose_z in this part.

So, It should be in base_link frame.

@tseco15

• Could you show me the result of following command?

ros2 param dump /perception/occupancy_grid_map/occupancy_grid_map_node

Here shows my sample output and ogm visualization with sample rosbag.

/perception/occupancy_grid_map/occupancy_grid_map_node:
  ros__parameters:
    OccupancyGridMapFixedBlindSpot:
      distance_margin: 1.0
    base_link_frame: base_link
    enable_single_frame_mode: false
    filter_obstacle_pointcloud_by_raw_pointcloud: false
    grid_map_type: OccupancyGridMapFixedBlindSpot
    gridmap_origin_frame: base_link
    height_filter:
      max_height: 2.0
      min_height: -1.0
      use_height_filter: true
    map_frame: map
    map_length: 150.0
    map_resolution: 0.5
    probability_matrix:
      free_to_free: 0.8
      free_to_occupied: 0.2
      occupied_to_free: 0.05
      occupied_to_occupied: 0.95
    qos_overrides:
      /clock:
        subscription:
          depth: 1
          durability: volatile
          history: keep_last
          reliability: best_effort
      /parameter_events:
        publisher:
          depth: 1000
          durability: volatile
          history: keep_last
          reliability: reliable
    scan_origin_frame: velodyne_top
    updater_type: binary_bayes_filter
    use_sim_time: true
    v_ratio: 10.0

I think it is the right behavior.

[tseco15]

Hi @YoshiRi ,

This is the output of the command:

/perception/occupancy_grid_map/occupancy_grid_map_node:
  ros__parameters:
    OccupancyGridMapFixedBlindSpot:
      distance_margin: 1.0
    base_link_frame: base_link
    enable_single_frame_mode: false
    filter_obstacle_pointcloud_by_raw_pointcloud: false
    grid_map_type: OccupancyGridMapFixedBlindSpot
    gridmap_origin_frame: base_link
    height_filter:
      max_height: 2.0
      min_height: -1.0
      use_height_filter: true
    map_frame: map
    map_length: 150.0
    map_resolution: 0.5
    probability_matrix:
      free_to_free: 0.8
      free_to_occupied: 0.2
      occupied_to_free: 0.05
      occupied_to_occupied: 0.95
    qos_overrides:
      /clock:
        subscription:
          depth: 1
          durability: volatile
          history: keep_last
          reliability: best_effort
      /parameter_events:
        publisher:
          depth: 1000
          durability: volatile
          history: keep_last
          reliability: reliable
    scan_origin_frame: os_lidar
    updater_type: binary_bayes_filter
    use_sim_time: true
    v_ratio: 10.0

It looks exactly the same as yours...

The robot_pose is obtained fromt input_raw_msg here. In our case, the frame_id of the raw_pointcloud we use as input is os_lidar, so robot.pose.z provides the height of the os_lidar link, instead of base_link. Am I right?

[YoshiRi]

The robot_pose is obtained fromt input_raw_msg here. In our case, the frame_id of the raw_pointcloud we use as input is os_lidar, so robot.pose.z provides the height of the os_lidar link, instead of base_link. Am I right?

That's right! Now I understood what happened.

robot_pose = utils::getPose(input_raw_msg->header, *tf2_, map_frame_);
// upper line will cause robot pose described in input_raw_msg frame

So I think the correspondent part should be fixed like:

robot_pose = utils::getPose(input_raw_msg->header, *tf2_, base_link_frame_, map_frame_);

Could you try and check it?

I created PR.
autowarefoundation/autoware.universe#6485

[tseco15]

Hi @YoshiRi,

I have just checked it and it works!

The only thing is that there is a little mistake in your suggestion. The params to call the getPose function should be:

robot_pose = utils::getPose(input_raw_msg->header.stamp, *tf2_, base_link_frame_, map_frame_);

Thank you very much for your help!