ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to reflect bumper collision to costmap?

asked 2021-03-05 22:26:59 -0500

Kazunari Tanaka gravatar image
local_costmap:
local_costmap:
  ros__parameters:
    update_frequency: 5.0
    publish_frequency: 2.0
    global_frame: odom
    robot_base_frame: base_link
    use_sim_time: True
    rolling_window: true
    width: 3
    height: 3
    resolution: 0.05
    robot_radius: 0.3
    plugins: ["voxel_layer", "obstacle_layer", "inflation_layer"]
    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      cost_scaling_factor: 3.0
      inflation_radius: 0.55
    voxel_layer:
      plugin: "nav2_costmap_2d::VoxelLayer"
      enabled: True
      publish_voxel_map: True
      origin_z: 0.0
      z_resolution: 0.05
      z_voxels: 16
      max_obstacle_height: 2.0
      mark_threshold: 0
      observation_sources: scan
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: True
        data_type: "LaserScan"
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      enabled: True
      observation_sources: bumper_front bumper_rear
      bumper_front:
        data_type: "PointCloud2"
        topic: /bumper_front_collision_cloud
        marking: True
        clearing: True
      bumper_rear:
        data_type: "PointCloud2"
        topic: /bumper_rear_collision_cloud
        marking: True
        clearing: True
    always_send_full_costmap: True
local_costmap_client:
  ros__parameters:
    use_sim_time: True
local_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: True
global_costmap:
global_costmap:
  ros__parameters:
    update_frequency: 1.0
    publish_frequency: 1.0
    global_frame: map
    robot_base_frame: base_link
    use_sim_time: True
    robot_radius: 0.22
    resolution: 0.05
    track_unknown_space: true
    plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      enabled: True
      observation_sources: scan
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: True
        data_type: "LaserScan"
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
    static_layer:
      plugin: "nav2_costmap_2d::StaticLayer"
      map_subscribe_transient_local: True
    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      cost_scaling_factor: 3.0
      inflation_radius: 0.55
    always_send_full_costmap: True
global_costmap_client:
  ros__parameters:
    use_sim_time: True
global_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: True

I want to reflect bumper state to obstacle_layer. I tried to send a singleton pointcloud2 msg that has odom frame. But obstacle plugin says:

      Sensor origin at (0.0, 0.0) is out of map bounds. The costmap cannot raytrace for it.

So cost_maps did not reflect this message.

What is the proper way to set it up in the first place?

Thank you!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-03-08 20:16:29 -0500

Kazunari Tanaka gravatar image

I found the solution. The point is a pointcloud2 msg has to have a sensor frame. Furthermore I changed yaml params like berrow.

  local_costmap:
local_costmap:
  ros__parameters:
    update_frequency: 5.0
    publish_frequency: 2.0
    global_frame: odom
    robot_base_frame: base_link
    use_sim_time: True
    rolling_window: true
    width: 3
    height: 3
    resolution: 0.05
    robot_radius: 0.3
    plugins: ["obstacle_layer", "inflation_layer"]
    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      cost_scaling_factor: 3.0
      inflation_radius: 0.55
    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      enabled: True
      observation_sources: bumper_front bumper_rear scan
      bumper_front:
        data_type: "PointCloud2"
        topic: /bumper_front_collision_cloud
        max_obstacle_height: 2.0
        marking: True
        clearing: True
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
      bumper_rear:
        data_type: "PointCloud2"
        topic: /bumper_rear_collision_cloud
        max_obstacle_height: 2.0
        marking: True
        clearing: True
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: True
        data_type: "LaserScan"
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
    always_send_full_costmap: True
local_costmap_client:
  ros__parameters:
    use_sim_time: True
local_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: True
global_costmap:
global_costmap:
  ros__parameters:
    update_frequency: 1.0
    publish_frequency: 1.0
    global_frame: map
    robot_base_frame: base_link
    use_sim_time: True
    robot_radius: 0.22
    resolution: 0.05
    track_unknown_space: true
    plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      enabled: True
      observation_sources: scan
      scan:
        topic: /scan
        max_obstacle_height: 2.0
        clearing: True
        marking: True
        data_type: "LaserScan"
        raytrace_max_range: 3.0
        raytrace_min_range: 0.0
        obstacle_max_range: 2.5
        obstacle_min_range: 0.0
    static_layer:
      plugin: "nav2_costmap_2d::StaticLayer"
      map_subscribe_transient_local: True
    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      cost_scaling_factor: 3.0
      inflation_radius: 0.55
    always_send_full_costmap: True
global_costmap_client:
  ros__parameters:
    use_sim_time: True
global_costmap_rclcpp_node:
  ros__parameters:
    use_sim_time: True

Then a local costmap was reflected by bumper state.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2021-03-05 22:26:59 -0500

Seen: 324 times

Last updated: Mar 08 '21