실제 뎁스 카메라(Realsense)로 Moveit 장애물 회피하기

1. 배경

인터넷을 좀만 찾아보면 

Moveit!을 이용해 Depth 카메라로 장애물을 인식하고 충돌 회피를 하는 것을 구현한 예제를 쉽게 찾을 수 있습니다.

 

그런데 문제는 모두 Gazebo에서 구현한다는 것이죠...

octomap이 실제로는 이렇게 깔끔하게 잘 안 나온다..

 

아무튼 저도 실물 매니퓰레이터를 받기 전까지는 가상환경에서 실험을 하고 그랬는데

 

이제는 실물로 넘어가야 하는 상황입니다.

 

그런데 인터넷을 아무리 찾아봐도 Gazebo에서 한 걸

실제 환경에서 하려면 어떻게 해야 하는지 잘 안 나와있더라구요..

 

인터넷을 아무리 뒤져도 가제보 예제만 많고

어떻게 해야 하냐고 질문은 간간히 있긴 하지만 답변은 없는 상태가 대부분이었습니다. 

https://www.youtube.com/watch?v=A_fnKm0a5fE 

예제는 못 찾고 성공한 유튜부만 겨우 찾음

 

2. 배경

위의 기능을 구현하기 전에 제가 이미 해놓은 것들입니다.

 

2-1. Gazebo상에서 Depth 카메라로 매니퓰레이터 Collision avoidance

 

이거는 인터넷에 워낙 예제도 많고 제 블로그에도 어떻게 해야 하는지 여러 글을 올려놨었습니다.

 

2-2. ROS로 Realsense Depth 정보 받아오기

 

이것도 깃허브 예제를 보고 잘 따라해서 뎁스 정보를 받아올 수 있어야 합니다.

GitHub - IntelRealSense/librealsense: Intel® RealSense™ SDK

SDK와 ROS Wrapper을 잘 맞춰서 깔아야 하는 등 은근 까다롭지만

이것도 제 블로그에 정리를 해놨습니다.

 

2-3. 실제 매니퓰레이터 제어

저는 Umbratek사이 UTRA를 가지고 있어서 이 매니퓰레이터를 ROS로 제어할 수 있는 상태였습니다.

 

3. 해결방법

3-1. 토픽 경로 바꾸기

Moveit!으로 만든 폴더 중 config 파일 안의 3D_sensor.yaml을 바꿔줍니다.

# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /robot_camera/depth/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

 

gazebo 안에서 만든 Depth 카메라로 만든 토픽의 경로로 되어 있는데

실제 Depth 카메라에서 나오는 경로로 바꿔줍니다.

 

# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /camera/depth/color/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

 

3-2. tf 연결하기

Moveit!과 카메라를 따로 실행시켜서 TF tree를 확인해보면

rosrun tf_tree tf_tree

(로 확인 가능합니다. rviz에서 GUI 상으로도 가능한 거 같은데 못 찾겠습니다.)

 

매니퓰레이터의 TF 따로 카메라 TF가 따로 존재합니다.

 

tf - ROS Wiki

이곳을 참고하여 분리된 두 tf를 연결해줍니다.

 

이전까지는 gazebo를 썼기 때문에 demo_gazebo.launch 파일로 Moveit!을 실행시켰지만

이제는 gazebo가 필요없기 때문에 그냥 demo.launch 파일에서 코드를 수정해줍니다.

 

<node pkg="tf" type="static_transform_publisher" name="link_broadcaster" args="0 0 0 0 0 1 depth_link camera_link 100" />

launch 파일 맨 밑에 이렇게 한 줄을 추가해줍니다.

pkg, name은 바꿀 필요 없고

args에서 앞의 0 0 0은 x y z

뒤의 0 0 0 은 roll pitch yaw입니다.

 

URDF를 작성해보신 분들은 쉽게 쓸 수 있을 거라 생각합니다.

(저는 0 0 0 0 0 1로 고정해서 사용하고 URDF상에서 Joint의 옵션을 바꿔가면서 읽어온 Depth 정보가 올바른 방향으로 나올 수 있도록 방향 조절을 해주었습니다.)

 

그 뒤에 depth_link는 parent link

camera_link는 child link입니다.

 

마지막 100은 통신할 때 주기로 ms입니다.

 

연결을 완료하면

이렇게 모든 tf들이 연결된 것을 확인할 수 있습니다.

 

그리고 launch 파일 밑에 리얼센스를 일일이 매번 키기 귀찮으니 자동으로 실행되게끔 해줍니다.

  <!-- Use real realsense -->
  <include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="filters" value="pointcloud"/>
  </include>

 

그리고 demo.launch 파일을 실행하면

잘 나오는 것을 알 수 있습니다. 뎁스가 나오긴 하는데 뒤틀린다고 하면

 

tf를 잘 확인해서

 

좀전에 연결했던 두 tf가 일치할 수 있도록 바꿔줍니다.

 

3-3. 실제 매니퓰레이터 연동

다른 패키지에 있는 실제 매니퓰레이터 폴더로 이동해 3-1, 3-2를 반복해줍니다.

(실제 매니퓰레이터를 제어하는 launch파일 안에 demo.launch 파일이 포함되어 있는지 확인 필수)

 

정리하며..

 

알고보면 그렇게 어렵지 않은 작업인데 ROS를 잘 아는 것도 아니고, 인터넷에 예제가 없다보니 구현하는데 꽤 오랜 시간이 걸렸었습니다.

 

저와 비슷한 기능을 구현하려는 분들은 이걸 보고 빠르게 해결하셨으면 좋겠네요