nav2 - (STVL) Using an External Costmap Plugin

https://docs.nav2.org/tutorials/docs/navigation2_with_stvl.html

(STVL) Using an External Costmap Plugin — Nav2 1.0.0 documentation

 

 

overview

• 해당되는 튜토리얼은 external plugin을 어떻게 load하는지 보여줄것이다.
  • 하나의 예시로는 STVL plugin을 사용할것이다.
    여기에서 방식대로 한다면 planner , controller , behavior에 대한 plugin도 적용하는 것이 가능할것이다.

 

 

Costmap2D and STVL

• Costmap 2D는 sensor data를 저장하고, 전역적으로 robot이 planning과 control에 사용할수 있도록 구성된 data structure이다.
  • 이 안에는 여러 pluginlib기반의 costmap plugins이 존재하며, runtime에 load가 가능하다.
    • ex) obstacle payer와 voxel layer와 static layer등을 런타임에 로드하는 것이 가능.
• STVL은 3D 인식용 costmap plugin이다.
  • STVL은 depth camera , lidar 등의 3D sensor data를 희소한 3d voxel형태로 저장하여 , 시간에 따라서 장애물을 자동으로 사라지게한다.
  • 이는 복잡하고 동적인 환경에서 유용하며, 3d sensor data 처리를 기존보다 최대 2배 효율적으로 할 수 있다.

 

 

0. setup

sudo apt install ros-<ros2-distro>-spatio-temporal-voxel-layer

 

 

1. modify navigation2 parameter

• 기존의 parameter를 봐볼건데, 예제를 적용시킬 global costmap에 대해서 살펴보자.

# global costmap이 2번 중첩되는 이유는, launchg 파일내에서 global_costmap 실행시 namespace를 동일하게 설정했기 때문
global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0 # costmap을 초당 몇 번(Hz) 업데이트 할지. 1.0은 1초에 한번 갱신
      publish_frequency: 1.0 # topic을 몇 Hz로 publish할지. 
      global_frame: map # costmap이 기준으로 삼는 전역 프레임이다. 
      robot_base_frame: base_link # 로봇의 기준 프레임이다. 여기서는 로봇의 중심이다.(base_link)
      # 로봇의 반지름이다. 미터 단위 ( m ) . 장애물 회피 계산 등에 사용. 이는 꼭 시뮬레이션 할때, gazebo와 일치시켜야한다. collsion블럭과...
      robot_radius: 0.22
      resolution: 0.05 # costmap의 격자 해상도를 의미. m단위이며, 0.05는 5cm이다.
      track_unknown_space: true # 미탐색 영역을 no information으로 표시할지 
      # 레이어 플러그인 설정. 해당 할목은 costmap에 사용할 레이어들을 정의한다. 각 이름은 아래 정의된 설정 블록의 키와 매칭됨
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      # lidar나 sensor data로부터 실시간 장애물을 costmap에 반영하는 역할을 하는 레이어다.
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan # 사용할 센서 소스이다. 이름은 아래의 블록과 동일해야함.
        scan:
          topic: /scan # 센서 데이터가 들어오는 Ros 2의 토픽이름 
          max_obstacle_height: 2.0 # 특정 높이 이상이면 무시됨
          clearing: True # 센서로 free space를 감지해서 제거 가능하게함. 이를 ray tracing이라 함.
          marking: True # 감지된 장애물을 costmap에 표지할지 안할지 정함. true면  표시함.
          data_type: "LaserScan" # 수신되는 sensor 메시지의 타입.
          raytrace_max_range: 3.0 # clearing 범위의 최대거리
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5 # 장애물로 인식할 최대 거리
          obstacle_min_range: 0.0
      # slam이나 map server에서 제공하는 정적 맵을 반영. 보통은 pgm지도이다. 맵은 대부분은 occupancy grid(nav_msgs/OccupancyGrid)
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        # Ros의 qos를 설정하는 부분. 여기서는 transient local로 설정했다. 즉, /map 토픽을 transient local모드로 구독을 의미
        # transient local은 과거에 publisher가 보낸 마지막 message를 cache에 저장해두다가, 나중에 subscribe한 node에게 보내주는것
        map_subscribe_transient_local: True
      inflation_layer: # 장애물 주변을 cost로 퍼뜨려서 경로 생성 시에 간접 회피가 가능하게 한다.
        plugin: "nav2_costmap_2d::InflationLayer" 
        cost_scaling_factor: 3.0 # 거리 대비 비용 감소 속도. (급할수록 크게 감소 )
        inflation_radius: 0.7 # 장애물 주변 0.7m범위까지 퍼진다.
      always_send_full_costmap: True # 항상 전체 costmap을 publish할지 여부. false라면 부분만 보내서 대역폭 절약. 대신에 시각화에 문제가 생길수있으니 비추

 

---

 

• 그리고 한번 stvl을 적용시키기 위해서 nav2_params.yaml을 수정해보자.

# global costmap이 2번 중첩되는 이유는, launchg 파일내에서 global_costmap 실행시 namespace를 동일하게 설정했기 때문
global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0 # costmap을 초당 몇 번(Hz) 업데이트 할지. 1.0은 1초에 한번 갱신
      publish_frequency: 1.0 # topic을 몇 Hz로 publish할지. 
      global_frame: map # costmap이 기준으로 삼는 전역 프레임이다. 
      robot_base_frame: base_link # 로봇의 기준 프레임이다. 여기서는 로봇의 중심이다.(base_link)
      # 로봇의 반지름이다. 미터 단위 ( m ) . 장애물 회피 계산 등에 사용. 이는 꼭 시뮬레이션 할때, gazebo와 일치시켜야한다. collsion블럭과...
      robot_radius: 0.22
      resolution: 0.05 # costmap의 격자 해상도를 의미. m단위이며, 0.05는 5cm이다.
      track_unknown_space: true # 미탐색 영역을 no information으로 표시할지 
      # 레이어 플러그인 설정. 해당 할목은 costmap에 사용할 레이어들을 정의한다. 각 이름은 아래 정의된 설정 블록의 키와 매칭됨
      plugins: ["static_layer", "obstacle_layer", "inflation_layer","stvl_layer"]
      # lidar나 sensor data로부터 실시간 장애물을 costmap에 반영하는 역할을 하는 레이어다.
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan # 사용할 센서 소스이다. 이름은 아래의 블록과 동일해야함.
        scan:
          topic: /scan # 센서 데이터가 들어오는 Ros 2의 토픽이름 
          max_obstacle_height: 2.0 # 특정 높이 이상이면 무시됨
          clearing: True # 센서로 free space를 감지해서 제거 가능하게함. 이를 ray tracing이라 함.
          marking: True # 감지된 장애물을 costmap에 표지할지 안할지 정함. true면  표시함.
          data_type: "LaserScan" # 수신되는 sensor 메시지의 타입.
          raytrace_max_range: 3.0 # clearing 범위의 최대거리
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5 # 장애물로 인식할 최대 거리
          obstacle_min_range: 0.0
      # slam이나 map server에서 제공하는 정적 맵을 반영. 보통은 pgm지도이다. 맵은 대부분은 occupancy grid(nav_msgs/OccupancyGrid)
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        # Ros의 qos를 설정하는 부분. 여기서는 transient local로 설정했다. 즉, /map 토픽을 transient local모드로 구독을 의미
        # transient local은 과거에 publisher가 보낸 마지막 message를 cache에 저장해두다가, 나중에 subscribe한 node에게 보내주는것
        map_subscribe_transient_local: True
      inflation_layer: # 장애물 주변을 cost로 퍼뜨려서 경로 생성 시에 간접 회피가 가능하게 한다.
        plugin: "nav2_costmap_2d::InflationLayer" 
        cost_scaling_factor: 3.0 # 거리 대비 비용 감소 속도. (급할수록 크게 감소 )
        inflation_radius: 0.7 # 장애물 주변 0.7m범위까지 퍼진다.
      always_send_full_costmap: True # 항상 전체 costmap을 publish할지 여부. false라면 부분만 보내서 대역폭 절약. 대신에 시각화에 문제가 생길수있으니 비추
      stvl_layer:
        plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
        enabled: true # 레이어의 활성화 여부 
        voxel_decay: 15. # 각 voxel의 수명이 감소되는 속도. 클수록 빨리 사라짐. 단위는 decay_model에 따라서 달라짐.
        decay_model: 0 # 0은 linear , 1은 지수 , 2는 센서 거리 기반 모델
        voxel_size: 0.05 # voxel 격자의 크기. m단위 . 여기서는 0.05이니 5cm이다. 작을수록 더 정밀한 3D 모델 생성
        track_unknown_space: true # 센서가 아직 비추지 않은 공간을 unknown으로 추적할지 여부.
        unknown_threshold: 15 # unknown으로 간주되기 위한 voxel의 계수. 이 값보다 작으면 known공간으로 간주.
        mark_threshold: 0 # 장애물로 간주하기 위한 voxel의 최소 갯수. 0이라면 voxel이 하나만 존재해도 marking됨
        update_footprint_enabled: true # robot의 footprint영역을 실시간 업데이트 할지의 여부. 
        combination_method: 1 # 여러 레이어를 합칠때 사용하는 방법. 1=max , 0=overwrite
        origin_z: 0.0 # costmap의 z축 기준 원점.
        publish_voxel_map: true # /voxel_grid 토픽에 3d voxel 맵을 publish할지 여부
        transform_tolerance: 0.2 # tf transform이 약간 늦어져도 허용할 시간이며 '초 단위' 이다.
        mapping_mode: false # true이면 slam이나 정적 맵 생성 모드로 동작하며, false라면 장애물 탐지를 위한 costmap모드
        map_save_duration: 60.0 # mapping mode가 true일때, 맵 저장 주기 ( 초 단위 )
        observation_sources: pointcloud # 관찰할 소스이름이며, 여러개가 가능하다.
        pointcloud: # 관찰 소스에 대한 설정. 여기는 pointcloud이다.
          data_type: PointCloud2 # 들어오는 데이터 타입. 여기서는 아마도 sensor_msg/msg/pointcloud2 일거임
          topic: /oakd/rgb/preview/depth/points # 실제로 받아오는 센서의 토픽 이름 
          marking: true # 해당 센서를 통해서 인식한 장애물을 costmap에 추가할지의 여부
          clearing: true # 해당 센서를 통해서 공간을 비어있음으로 표시할지 여부 
          obstacle_range: 3.0 # 장애물로 인식할 최대 거리. m단위이며 이보다 멀면 무시 
          min_obstacle_height: 0.0 # 장애물로 인식할 최소 높이.  m단위
          max_obstacle_height: 2.0 # 장애물로 인식할 최대 높이. m단위
          expected_update_rate: 0.0 
          observation_persistence: 0.0
          inf_is_valid: false
          filter: "voxel"
          voxel_min_points: 0
          clear_after_reading: true # 센서 데이터를 처리후 내부 버퍼에서 제거할지 여부이며, true이면 메모리를 절약 가능
          max_z: 7.0
          min_z: 0.1
          vertical_fov_angle: 0.8745 # 센서의 수직 FOV 각도
          horizontal_fov_angle: 1.048
          decay_acceleration: 15.0 # decay 속도를 가속 시키는 계수이며, 움직이는 장애물을 빨리 지우고 싶을때 사용 
          model_type: 0 # stvl 내부 모델. 0은 기본이며, 1은 거리 기반 모델