이 후, erle-copter의 gazebo 3D model에 depth camera를 추가하여 3D SLAM의 기초 토대를 마련하겠다. depth camera model은 intel realsense로 한다.
#05. Use a Gazebo Depth Camera with ROS 에서 작업한 Kinetic Sensor를 토대로 erle-copter 에 intel realsense를 부착하여 볼 것이다.
SLAM 작업환경
OS : Ubuntu 16.06 LTS
ROS version : Kinetic
Compiler : catkin
Depth Camera : Intel realsene D435
Simulation 작업환경
OS: Ubuntu 14.04 LTS
ROS version : Indigo
Compiler : catkin
Depth Camera : Kinect Sensor
1. erle-copter에 sensor 추가하는 방법 확인.
http://docs.erlerobotics.com/simulation/vehicles/erle_copter/tutorial_1
위 링크의 lidar sensor 추가하는 법을 보면
/home/user/simulation/ros_catkin_ws/src/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin/urdf 에 존재하는 erlecopter.xacro 파일에 다음과 같은 문구를 추가함으로써 gazebo erle-copter model에 lidar sensor를 추가할 수 있다고 명시되어 있다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
|
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/sensors/lidar_sensor.urdf.xacro" />
<xacro:lidar_sensor
name="sonar2"
parent="base_link"
ros_topic="sonar_front"
update_rate="10"
min_range="0.01"
max_range="10.0"
field_of_view_horizontal="${70*M_PI/180}"
field_of_view_vertical="${1*M_PI/180}"
ray_count_horizontal="140"
ray_count_vertical="1"
sensor_mesh="lidar_lite_v2_withRay/meshes/lidar_lite_v2_withRay.dae">
<origin xyz="0.13 0.0 0.02" rpy="0 0 0"/>
</xacro:lidar_sensor>
| cs |
위 소스를 살펴보면 먼저 line 1에 존재하는 파일의 경로는 다음과 같다.
/home/user/simulation/ros_catkin_ws/src/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin/urdf/sensors/lidar_sensor.urdf.xacro
위 파일을 열어보면 다음과 같은데
lidar_sensor.urdf.xacro
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
|
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="lidar_sensor" params="name parent *origin ros_topic update_rate min_range max_range field_of_view_horizontal field_of_view_vertical ray_count_horizontal ray_count_vertical sensor_mesh">
<joint name="${name}_joint" type="fixed">
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
<child link="${name}_link"/>
</joint>
<link name="${name}_link">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.000000017" ixy="0" ixz="0" iyy="0.000000017" iyz="0" izz="0.000000017" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<!--<box size="0.01 0.01 0.01" /> -->
<!-- <mesh filename="package://rotors_description/meshes/sonar_sensor/max_sonar_ez4.dae"/> -->
<!--<mesh filename="package://ardupilot_sitl_gazebo_plugin/meshes/${sensor_mesh}"/>-->
<!-- <mesh filename="package://ardupilot_sitl_gazebo_plugin/meshes/lidar_lite_v2/meshes/lidar_lite_v2.dae"/> -->
</geometry>
</visual>
<!-- <collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.01 0.01 0.01" />
</geometry>
</collision> -->
</link>
<gazebo reference="${name}_link">
<sensor type="ray" name="${name}">
<always_on>true</always_on>
<update_rate>${update_rate}</update_rate>
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
<samples>${ray_count_horizontal}</samples>
<resolution>0.5</resolution>
<min_angle>-${field_of_view_horizontal/2}</min_angle>
<max_angle> ${field_of_view_horizontal/2}</max_angle>
</horizontal>
<vertical>
<samples>${ray_count_vertical}</samples>
<resolution>1</resolution>
<min_angle>-${field_of_view_vertical/2}</min_angle>
<max_angle> ${field_of_view_vertical/2}</max_angle>
</vertical>
</scan>
<range>
<min>${min_range}</min>
<max>${max_range}</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="gazebo_ros_${name}_controller" filename="librotors_gazebo_sonar_plugin.so">
<gaussianNoise>0.005</gaussianNoise>
<topicName>${ros_topic}</topicName>
<frameId>${name}_link</frameId>
</plugin>
<plugin name="gazebo_ros_${name}_controller" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>${name}_link</frameName>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
| cs |
위 파일에서 line 4의 내용을 살펴보면 다음과 같다.
<xacro:macro name="lidar_sensor" params="name parent *origin ros_topic update_rate min_range max_range field_of_view_horizontal field_of_view_vertical ray_count_horizontal ray_count_vertical sensor_mesh">
이 내용이 lidar sensor를 erlecopter.xacro에 추가할 때 언급해주는 parameter 들에 대한 내용이다.
2. erle-copter에 kinect sensor 추가하기
1번에서 살펴본 내용을 기반으로 kinect sensor를 추가하여 보겠다.
이 때, 우리가 추가할 센서는 intel realsense인데 어째서 kinect를 추가하는 것인지 의아할 수 있다. 우리가 사용할 sensor는 intel realsense지만 gazebo에서 제공하는 plugin 중에서는 intel realsense로 특정된 depth camera는 존재하지 않는다. 대신 intel realsense를 대체할 수 있는 depth camera 혹은 kinect 등의 plugin이 존재하며, 그래서 이를 이용하여 우리가 사용할 intel realsense의 스펙에 맞게 조정해 depth camera로 이용하는 것이다.
그러면 이제 /home/user/simulation/ros_catkin_ws/src/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin/urdf/sensors 에 존재하는 kinect.urdf.xacro를 확인해보자.
Kinect.urdf.xacro
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
|
<?xml version="1.0"?>
<robot name="sensor_kinect" xmlns:xacro="http://ros.org/wiki/xacro">
<property name="cam_px" value="1.63" />
<xacro:property name="kinect_cam_py" value="-0.0125"/>
<property name="cam_pz" value="0.68" />
<property name="cam_or" value="0" />
<property name="cam_op" value="0" />
<property name="cam_oy" value="0" />
<xacro:property name="M_PI" value="3.1415926535897931" />
<!-- Parameterised in part by the values in turtlebot_properties.urdf.xacro -->
<xacro:macro name="sensor_kinect" params="parent">
<joint name="camera_rgb_joint" type="fixed">
<origin xyz="${cam_px} ${kinect_cam_py} ${cam_pz}" rpy="${cam_or} ${cam_op} ${cam_oy}"/>
<parent link="${parent}"/>
<child link="camera_rgb_frame" />
</joint>
<link name="camera_rgb_frame"/>
<joint name="camera_rgb_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_rgb_frame" />
<child link="camera_rgb_optical_frame" />
</joint>
<link name="camera_rgb_optical_frame"/>
<joint name="camera_joint" type="fixed">
<origin xyz="-0.031 ${-kinect_cam_py} -0.016" rpy="0 0 0"/>
<parent link="camera_rgb_frame"/>
<child link="camera_link"/>
</joint>
<link name="camera_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
<geometry>
<mesh filename="package://ardupilot_sitl_gazebo_plugin/meshes/meshes_sensors/kinect/kinect.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
<geometry>
<box size="0.07271 0.27794 0.073"/>
</geometry>
</collision>
<inertial>
<mass value="0.564" />
<origin xyz="0 0 0" />
<inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
iyy="0.000498940" iyz="0.0"
izz="0.003879257" />
</inertial>
</link>
<!-- The fixed joints & links below are usually published by static_transformers launched by the OpenNi launch
files. However, for Gazebo simulation we need them, so we add them here.
(Hence, don't publish them additionally!) -->
<joint name="camera_depth_joint" type="fixed">
<origin xyz="0 ${2 * -kinect_cam_py} 0" rpy="0 0 0" />
<parent link="camera_rgb_frame" />
<child link="camera_depth_frame" />
</joint>
<link name="camera_depth_frame"/>
<joint name="camera_depth_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_depth_frame" />
<child link="camera_depth_optical_frame" />
</joint>
<link name="camera_depth_optical_frame"/>
<!-- Microsoft Kinect / ASUS Xtion PRO Live for simulation -->
<gazebo reference="camera_link">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${60.0*M_PI/180.0}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>camera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>10</updateRate>
<imageTopicName>rgb/image_raw</imageTopicName>
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>camera_depth_optical_frame</frameName>
<baseline>0.1</baseline>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.4</pointCloudCutoff>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>
| cs |
line 14를 보면 아래와 같다.
<xacro:macro name="sensor_kinect" params="parent">
params에 parent가 필요한 것을 알 수 있다.
그러므로 erlecopter.xacro에는 다음과 같은 구문을 추가해주면 된다.
1
2
3
4
5
6
7
|
<!-- [Edited] Kinect sensor -->
<xacro:include filename="$(find ardupilot_sitl_gazebo_plugin)/urdf/sensors/kinect.urdf.xacro" />
<xacro:sensor_kinect
parent="base_link">
</xacro:sensor_kinect>
| cs |
문제가 발생했다.
kinetic sensor가 drone에 부착되지 않았다.
사실 생각해보면 당연하다.
kinect.urdf.xacro 파일을 보면 초기 kinect sensor의 parameter 값이 아래와 같이 설정되어 있다.
1
2
3
|
<property name="cam_px" value="1.63" />
<xacro:property name="kinect_cam_py" value="-0.0125"/>
<property name="cam_pz" value="0.68" />
| cs |
해당 값들은 parent인 base link를 기준으로 kinect sensor의 x, y, z 축 position을 의미하는데, 1.63은 1.63[m]라는 의미이므로 위 사진에서 빨강색인 x축에 대해 1.63[m] 만큼 떨어져 있게 되고, 초록색인 y축에 대해서는 1cm만큼 파랑색인 z축에 대해서는 68cm만큼 떨어져있게 되는 것이다. 하지만 mass값으로 인해 kinect는 바닥에 존재하는 상태이다.
그러므로 이 상태에서 만약 주행을 한다면, drone은 사방으로 곤두박질 칠 것이다. 위치는 잘못되어 있어도 kinect sensor와 drone은 연결되어 있는 상태이며, kinect sensor는 mass 값이 적용되어있기 때문이다.
그러므로 위의 파라미터 값을 다음과 같이 변경하여 준다.
1
2
3
|
<property name="cam_px" value="0" /> <!-- Original value is 1.63[m] -->
<xacro:property name="kinect_cam_py" value="0"/> <!-- Original value is -0.0125[m] -->
<property name="cam_pz" value="0.05" /> <!-- Original value is 0.68[m] -->
| cs |
위와 같이 수정해 준 후, drone을 띄우면 다음과 같이 잘 작동하는 것을 확인할 수 있다.
3. intel realsense 적용하기
이제 gazebo depth camera의 기본적인 틀은 갖춰졌다. 이제 기존에 제공되는 intel realsense의 model을 가져와서 우리 환경에 적용시키는 작업을 해보겠다.이 작업을 시작하기전에 중요한 이야기를 하나 해보자면, 이미 intel realsense는 ros를 위한 패키지를 제공하고 있다. 두 패키지의 차이점을 아는 것이 중요하다.
(1) realsense_camera
http://wiki.ros.org/realsense_camera위 모델들에 대하여 호환
(2) realsense2_camera
http://wiki.ros.org/realsense2_camera우리가 사용할 모델은 D435이다.
여기서 혼란스러운 부분이 있는데,
분명히 ROS wiki에 indigo에 대한 페이지가 존재하고 (http://wiki.ros.org/realsense2_camera) Github에도 indigo 환경에서 설치가 가능하다는 언급[
The following instructions support ROS Indigo, on Ubuntu 14.04, and ROS Kinetic, on Ubuntu 16.04.] 이 있다.
(https://github.com/IntelRealSense/realsense-ros)
하지만 Github에 존재하는 Install Step 1을 보면 [latest Intel® RealSense™ SDK 2.0]을 설치해야 하는데 해당 소프트웨어는 적어도 Ubuntu 16.04 이상의 환경에서 설치되어야 한다고 해당 사이트에 언급되어 있다. (https://github.com/IntelRealSense/librealsense/releases/tag/v2.24.0)
일단 이 부분에 대해서 개발자의 확인을 받고자 Github에 issue를 올려 놓았으니, 확인되는 대로 글을 수정하겠다.
(https://github.com/IntelRealSense/realsense-ros/issues/865)
댓글 없음:
댓글 쓰기