[voxblox] voxblox_ros/src/tsdf_server.cc

About_work·2024년 9월 3일
0

voxblox

목록 보기
6/10

0. 참고자료

0.1. 역링크

0.2. 링크

0. TsdfServer

0.1. 생성자

  • tf_buffer_ / tf_listener_ 생성
  • tsdfmap
    • TsdfMap(config)
  • tsdfintegrator
    • FastTsdfIntegrator(integratorconfig, tsdf_map->getTsdfLayerPtr())
  • meshintegrator
    • MeshIntegrator<TsdfVoxel>( mesh_config, tsdf_map_->getTsdfLayerPtr(), mesh_layer_.get())
      • meshlayer
        • MeshLayer
  • subscriber
    • imusubscription
      • /camera/camera/imu
      • OnImu 콜벡이 아무것도 안하고 있음
    • pointcloudsubscription
      • "/camera/camera/depth/color/points"
      • OnPclFrame 콜벡
  • publisher

1. tsdf_server.h

1. 클래스 개요

  • TsdfServer 클래스는 로봇이 3D 공간을 탐색하면서 수집한 포인트 클라우드 데이터를 처리하여 TSDF 맵을 생성하고,
    • 이를 기반으로 메쉬를 생성하거나 다른 형태로 시각화할 수 있게 해주는 서버 역할

2. 주요 기능

a. 포인트 클라우드 데이터 처리

  • 포인트 클라우드 삽입 (insertPointcloud 함수):

    • 로봇의 센서에서 수집된 포인트 클라우드 데이터를 TSDF 맵에 삽입
    • 이 데이터는 로봇이 주변 환경을 이해하고, 맵을 생성하는 데 사용

  • 자유 공간 포인트 클라우드 삽입 (insertFreespacePointcloud 함수):

    • 포인트 클라우드 중 자유 공간(즉, 장애물이 없는 공간)을 나타내는 데이터를 처리하여, TSDF 맵에 삽입
    • 이 정보는 로봇이 장애물을 피하는 데 유용

  • 포인트 클라우드 처리 및 통합 (processPointCloudMessageAndInsert 함수):

    • 수신된 포인트 클라우드 데이터를 TSDF 맵에 통합하는 핵심 로직을 담당
    • ICP(Iterative Closest Point) 알고리즘을 사용해 포인트 클라우드를 정밀하게 정렬

b. 메쉬 생성 및 퍼블리싱

  • 메쉬 생성 및 업데이트 (updateMesh 및 generateMesh 함수):
    • TSDF 데이터를 기반으로 3D 메쉬를 생성
      • 이 메쉬는 로봇의 환경을 시각적으로 표현하는 데 사용
    • 메쉬는 주기적으로 업데이트되며, 이를 통해 실시간으로 로봇의 환경 변화에 대응할 수 있음
  • 메쉬 및 TSDF 데이터 퍼블리싱 (publishSlices, publishPointclouds, publishMap 함수 등):
    • 생성된 메쉬나 TSDF 데이터를 ROS 토픽을 통해 퍼블리시하여, 다른 시스템에서 사용할 수 있도록 함
    • 이를 통해 로봇은 다른 장치나 시스템과 데이터를 공유할 수 있음

c. 맵 관리 및 서비스

  • 맵 저장 및 불러오기 (saveMap, loadMap 함수):

    • 현재 TSDF 맵을 파일로 저장하거나, 이전에 저장된 맵을 불러옴
    • 이를 통해 로봇은 이전 탐색 데이터를 재사용할 수 있음

  • 맵 초기화 (clear 함수):

    • 현재의 TSDF 맵을 초기화하여, 새로운 환경을 탐색할 준비

  • ROS 서비스 처리:

    • 다양한 ROS 서비스를 제공하여, 외부 요청에 따라
      • 메쉬를 생성하거나,
      • 맵을 저장/불러오는 등의 작업을 수행

d. 타이머를 통한 주기적 작업

  • 주기적 메쉬 업데이트 및 맵 퍼블리싱 (updateMeshEvent, publishMapEvent 함수):
    • 타이머를 사용해 주기적으로 메쉬를 업데이트하거나 맵을 퍼블리시하는 작업을 수행

3. 클래스 내부 변수

  • TSDF 맵 및 통합기:
    • 2가지가 정의되어 있습니다.
      • TSDF 데이터를 저장하고 관리하는 tsdf_map_과,
      • 포인트 클라우드 데이터를 TSDF 맵에 통합하는 tsdf_integrator_
  • ICP 및 메쉬 통합기:
    • 포인트 클라우드 데이터를 정밀하게 정렬하는 데 사용되는 icp_와,
    • TSDF 데이터를 기반으로 메쉬를 생성하는 mesh_integrator_가 포함되어 있음
  • ROS 관련 핸들 및 퍼블리셔/서브스크라이버:
    • ROS 노드 핸들(nh_, nh_private_), 퍼블리셔, 서브스크라이버 등이 정의되어 있어, ROS 환경에서 데이터를 송수신할 수 있습니다.

4. 접근자 및 설정자 함수

  • TSDF 맵 접근자 (getTsdfMapPtr 함수):

    • 현재의 TSDF 맵을 접근할 수 있는 함수를 제공

  • 설정 관련 함수:

    • 메쉬 생성 시 사용할 컬러 맵, 슬라이스 레벨, 월드 프레임 등의 설정을 조정할 수 있는 함수들이 포함되어 있음

2. tsdf_server.cc

1. TSDF 서버 초기화

  • 이 코드는 TsdfServer 클래스 초기화
    • TSDF 맵
    • 포인트 클라우드 통합기
    • 메쉬 생성기
    • ICP(Iterative Closest Point) 정렬기
  • ROS 노드 핸들을 통해 파라미터를 가져와 서버의 동작 방식을 설정
    • 예를 들어, 아래의 설정이 이뤄짐
      • 어떤 메소드로 TSDF를 통합할지
      • 어떤 컬러맵을 사용할지
  • 다양한 ROS 토픽과 서비스를 publsih하여 다른 노드와의 통신을 가능하게 함
    • 예를 들어, 포인트 클라우드를 받아들이거나, TSDF 맵을 퍼블리시하는 등의 역할

2. 포인트 클라우드 데이터 처리

  • 서버는 3D 센서에서 수신된 포인트 클라우드 데이터를 받아들여 TSDF 맵에 통합
  • 포인트 클라우드 변환:
    • 센서에서 받은 포인트 클라우드를 내부적으로 사용하기 적합한 형태로 변환
      • 여기서 RGB 컬러 정보나 강도(intensity) 정보를 처리
  • ICP 정렬:
    • 옵션에 따라 ICP 알고리즘을 사용해 포인트 클라우드를 정밀하게 정렬할 수 있음
    • ICP는 주어진 포인트 클라우드를 기존의 TSDF 맵과 정렬하여 오차를 최소화하는 변환을 계산
    • 이를 통해 로봇의 위치 추정을 더욱 정확하게 할 수 있음
  • TSDF 통합:
    • 변환된 포인트 클라우드는 TSDF 맵에 통합
    • 통합된 결과는 3D 공간의 표면을 보다 정확하게 표현

3. 메쉬 생성 및 퍼블리싱

  • TSDF 맵에서 데이터를 기반으로 메쉬를 생성하고, 이를 ROS 토픽으로 퍼블리시
  • 메쉬 생성:
    • TSDF 데이터를 기반으로 3D 메쉬를 생성
    • 이 메쉬는 3D 환경을 시각화하거나 다른 작업에 사용할 수 있음
      • 메쉬는 주기적으로 업데이트되며, 필요에 따라 결과를 저장할 수도 있음
  • 메쉬 퍼블리싱:
    • 생성된 메쉬는 ROS 토픽을 통해 퍼블리시되어 다른 시스템에서 사용할 수 있도록 함

4. 지도 관리 기능

  • 서버는 TSDF 맵을 저장하고 불러오거나, 현재 맵을 초기화하는 등의 기능을 제공
  • 맵 저장 및 불러오기:
    • TSDF 맵을 파일로 저장하거나, 이전에 저장된 맵을 불러올 수 있음
    • 이를 통해 로봇이 이전에 탐색한 공간의 맵을 복원하거나, 새로운 탐색을 시작할 수 있음
  • 맵 초기화:
    • 현재의 TSDF 맵을 초기화하여 새로운 탐색을 시작할 수 있음

5. 타이머를 통한 주기적 작업

  • 코드는 타이머를 사용해 주기적으로 메쉬를 업데이트하거나 맵을 퍼블리시하는 작업을 수행
  • 메쉬 업데이트:
    • 일정 시간 간격으로 TSDF 맵을 기반으로 메쉬를 재생성하여 최신 상태를 반영
  • 맵 퍼블리싱:
    • 일정 간격으로 TSDF 맵을 퍼블리시하여 다른 시스템과 맵 정보를 공유

6. 에러 처리 및 로그 출력

  • 코드는 여러 에러 조건을 체크하고, 상황에 따라 ROS 로그를 통해 정보를 출력합니다.
    • 이는 디버깅 및 시스템 상태를 모니터링하는 데 중요한 역할을 합니다.
profile
About_work
새로운 것이 들어오면 이미 있는 것과 충돌을 시도하라.
이전 포스트

[voxblox] voxblox_ros/launch/rgbd_dataset.launch

다음 포스트

[voxblox] voxblox/src/core/tsdf_map.cc

0개의 댓글