작업환경
- Labtop OS : Ubuntu 16.04- Labtop ROS : Kinetic
- Lattepanda alpha OS : Ubuntu 16.04
- Lattepanda alpha ROS : Kinetic
- LIDAR Sensor ( connected to Lattepanda alpha ) : YDLIDAR-G4
- Depth Camera : pico monstar
1. Edit mapping_default.launch
먼저 이 전까지 진행한 내용대로 Lidar sensor와 Depth camera를 작동 시킨 후, rqt의 tf_tree를 사용하여 프레임의 상관관계를 확인해보면 다음과 같다.
그리고 다음의 명령어를 실행하여 hector_mapping 을 실행한 후, tf_tree를 확인해보면 다음과 같다.
1
|
$ roslaunch hector_mapping mapping_default.launch
| cs |
하나하나 살펴보자면,
map은 Lidar sensor를 이용한 2D SLAM으로 얻어낸 Occupancy Grid Map이 되고,
scanmatcher_frame은 scan matching 기법을 이용하여 알아낸 로봇의 위치에 대한 프레임이다.
laser_frame은 Lidar sensor에서 sensor가 위치한 부분의 frame을 의미하고,
base_footprint는 laser_frame의 기반이 되는 frame을 의미한다. (센서를 잡아주는 베이스)
camera_optical_link는 카메라의 렌즈가 위치한 부분의 frame을 의미하고,
camera_link는 카메라의 몸체 부분의 frame을 의미한다.
본 프로젝트에서 로봇의 위치를 인식하는데에 다른 센서는 사용하지 않으므로, 로봇의 위치는 전적으로 scanmatcher_frame에 의지하게 된다.
그러므로 다음 작업을 통해 scanmatcher_frame과 base_footprint, camera_link 를 연결시켜 줄 것이다.
tf 패키지의 static_transform_publisher 노드는 현재 인식되는 frame들 중 두 개 frame들의 tf를 연결시켜주는 역할을 한다. 이를 launch 파일 내에서 동작시키기 위하여 다음의 코드를 mapping_default.launch 파일 안에 넣어주게 된다.
1
2
3
|
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_frame_base_footprint_broadcaster" args="0 0 0 0 0 0 scanmatcher_frame base_footprint 100"/>
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_frame_camera_link_broadcaster" args="0 0 0 0 0 0 scanmatcher_frame camera_link 100"/>
| cs |
이 후에 hector_mapping을 다시 작동시키면 다음과 같이 프레임이 서로 연결된 tf_tree를 얻을 수 있다.
1
|
$ roslaunch hector_mapping mapping_default.launch
| cs |
여기서 한가지 더 수정해야할 내용이 있다.
hector_mapping을 실행한 후, rviz를 이용해 확인해보면 아래그림과 같이 카메라의 포즈가 하드웨어에 설치된 포즈와 같지 않은 경우가 있다.
위 사진상에서 RGB 색으로 표시되는 포인트 클라우드가 Depth Camera의 데이터인데,
원래대로라면 반대방향으로 데이터를 출력하여야 한다.
위 문제를 해결하기 위하여 static_transform_publisher의 내용을 수정해줄 수 있다.
static_transform_publisher의 args 중 "0 0 0 0 0 0" 은 각각 "x y z yaw pitch roll" 가 된다.
그러므로 yaw 값을 180도 돌려줌으로써 카메라의 위치를 원상태로 돌릴 수 있다.
사용되는 단위는 radian이므로 다음과 같이 수정해준다.
1
|
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_frame_camera_link_broadcaster" args="0 0 0 -3.14 0 0 scanmatcher_frame camera_link 100"/>
| cs |
이 후에 프로그램을 실행해보면 다음과 같이 카메라의 방향이 수정된 것을 확인할 수 있다.
이제 mapping_default.launch 파일 내부 내용을 수정해보자.
아래 내용은 2D SLAM을 진행할 때, base_frame과 odom_frame을 결정하는 내용이다. 해당 frame을 기준으로 SLAM이 진행될 것이므로 두 argument를 모두 laser_frame으로 만들어준다. odom_frame의 경우, 로봇의 위치를 나타내는데 laser_frame과 scanmatcher_frame은 연결되어 있으므로 laser_frame으로 적어주어도 무방하다.
1
2
3
|
<arg name="base_frame" default="base_footprint"/>
<arg name="odom_frame" default="nav"/>
| cs |
위 내용을
1
2
3
|
<arg name="base_frame" default="laser_frame"/>
<arg name="odom_frame" default="laser_frame"/>
| cs |
위와 같이 바꿔주자.
최종적인 mapping_default.launch의 내용은 다음과 같아진다.
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
|
<?xml version="1.0"?>
<launch>
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_frame_base_footprint_broadcaster" args="0 0 0 0 0 0 scanmatcher_frame base_footprint 100"/>
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_frame_camera_link_broadcaster" args="0 0 0 -3.14 0 0 scanmatcher_frame camera_link 100"/>
<arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
<arg name="base_frame" default="laser_frame"/>
<arg name="odom_frame" default="laser_frame"/>
<arg name="pub_map_odom_transform" default="true"/>
<arg name="scan_subscriber_queue_size" default="5"/>
<arg name="scan_topic" default="scan"/>
<arg name="map_size" default="2048"/>
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="map_frame" value="map" />
<param name="base_frame" value="$(arg base_frame)" />
<param name="odom_frame" value="$(arg odom_frame)" />
<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="false"/>
<param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="$(arg map_size)"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="map_multi_res_levels" value="2" />
<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.9" />
<param name="map_update_distance_thresh" value="0.4"/>
<param name="map_update_angle_thresh" value="0.06" />
<param name="laser_z_min_value" value = "-1.0" />
<param name="laser_z_max_value" value = "1.0" />
<!-- Advertising config -->
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
<param name="scan_topic" value="$(arg scan_topic)"/>
<!-- Debug parameters -->
<!--
<param name="output_timing" value="false"/>
<param name="pub_drawings" value="true"/>
<param name="pub_debug_output" value="true"/>
-->
<param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
</node>
</launch>
| cs |
2. Edit hector_elevation_mapping_node.launch
(1) Edit local_map_frame_id
1
|
<param name="local_map_frame_id" value="base_stabilized"/>
| cs |
local_map_frame_id 는 elevation_mapping을 하기 위한 기준이 되는 frame이다.
elevation_mapping 또한 로봇의 위치가 기준이 되므로
아래와 같이 base_stabilized를 scanmatcher_frame으로 변경해준다.
1
|
<param name="local_map_frame_id" value="scanmatcher_frame"/>
| cs |
(2) Edit point_cloud_topic
1
|
<param name="point_cloud_topic" value="/openni/depth/points"/>
| cs |
point_cloud_topic은 elevation_mapping을 위해 subscribe하는 depth camera의 topic name이 된다. 그리고 /camera/stream/1/cloud는 Depth camera의 토픽 명이므로, 아래와 같이 변경해준다.
1
|
<param name="point_cloud_topic" value="/camera/stream/1/cloud"/>
| cs |
위와 같이 변경해준다.
(3) Edit grid_map_topic
1
|
<param name="grid_map_topic" value="scanmatcher_map"/>
| cs |
grid_map_topic은 elevation_mapping을 진행할 때, 기반이 되는 2D 지도가 된다.
hector_mapping을 이용해 얻을 수 있으며, "map" 이란 토픽명으로 전달된다.
그러므로 다음과 같이 수정해준다.
1
|
<param name="grid_map_topic" value="map"/>
| cs |
최종적으로는 다음과 같은 code가 된다.
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
|
<?xml version="1.0"?>
<launch>
<node pkg="hector_elevation_mapping" type="ElevationMappingNode" name="ElevationMapping" output="screen">
<param name="elevation_resolution" value="0.01" />
<param name="elevation_zero_level" value="16384" />
<param name="min_observable_elevation" value="-1.0" />
<param name="max_observable_elevation" value="0.5" />
<param name="max_observable_distance" value="4.0" />
<param name="sensor_variance" value="0.001" />
<param name="map_frame_id" value="map" />
<param name="local_map_frame_id" value="scanmatcher_frame" /> <!-- default : base_stabilized -->
<param name="local_elevation_map_topic" value="elevation_map_local" />
<param name="gloabl_elevation_map_topic" value="elevation_map_global" />
<param name="point_cloud_topic" value="/camera/stream/1/cloud"/> <!-- default : openni/depth/points -->
<param name="grid_map_topic" value="map"/><!-- default : scanmatcher_map -->
<param name="sys_msg_topic" value="syscommand" />
<param name="publish_poseupdate" value="false" />
<param name="poseupdate_pub_period" value="1.0" />
<param name="poseupdate_height_covariance" value="0.25" />
<param name="poseupdate_used_pattern_size" value="3" />
<!-- Not necessary to set, set by scanmatcher -->
<!--param name="map_resolution" value="0.05" /-->
<!--param name="max_grid_size_x" value="1024" /-->
<!--param name="max_grid_size_y" value="1024" /-->
</node>
</launch>
| cs |
3. Edit hector_elevation_visualization.launch
아마 해당 파일을 사용시에 에러가 발생하는 경우가 허다할 것이다.
왜인지 원본 파일 자체에 문제가 있기 때문인데, 다음과 같이 수정하여 고칠 수 있다.
(1) Edit node name
1
|
<node pkg="hector_elevation_visualization" type="hector_elevation_visualization" name="hector_elevation_visualization" output="screen" respawn="false">
| cs |
launch 파일에서 node를 실행시키는 라인에서
pkg는 패키지 명이 되고, type은 노드 명이 된다.
하지만 hector_elevation_visualization 패키지의 CMakeList.txt 파일을 보면 아래와 같은 내용을 볼 수 있는데,
1
2
|
## Declare a cpp executable
add_executable(hector_elevation_visualization_node src/hector_elevation_visualization_node.cpp src/hector_elevation_visualization.cpp)
| cs |
위 내용은 노드의 명이 "hector_elevation_visualization_node" 라는 의미이다.
그러므로 아래와 같이 수정해주면 된다.
1
|
<node pkg="hector_elevation_visualization" type="hector_elevation_visualization_node" name="hector_elevation_visualization" output="screen" respawn="false">
| cs |
(2) Edit topic name
1
|
<param name="elevation_map_frame_id" value="/elevation_map_local" />
| cs |
elevation_map_frame_id는 전달받을 elevation_map의 이름이 된다.
하지만 실제로 구동시에 전달받는 토픽 명은 "/ElevationMapping/elevation_map_local"이 되므로, 다음과 같이 수정해준다.
1
|
<param name="elevation_map_frame_id" value="/ElevationMapping/elevation_map_local" />
| cs |
전체적인 코드는 다음과 같다.
1
2
3
4
5
6
7
8
9
10
11
12
|
<?xml version="1.0"?>
<launch>
<node pkg="hector_elevation_visualization" type="hector_elevation_visualization_node" name="hector_elevation_visualization" output="screen" respawn="false">
<param name="max_height_levels" value="10" />
<param name="max_height" value="1.0" />
<param name="elevation_map_frame_id" value="/ElevationMapping/elevation_map_local" />
<param name="sys_msg_topic" value="/syscommand" />
</node>
</launch>
| cs |
4. Edit costmap.launch
(1) Edit local_transform_frame_id
1
|
<param name="local_transform_frame_id" value="base_footprint" />
| cs |
local_transform_frame_id는 costmap을 작성시에 기준이 되는 frame, 즉, 로봇의 위치가 된다. 그러므로 역시 scanmatcher_frame을 사용하여 아래와 같이 변경해준다.
1
|
<param name="local_transform_frame_id" value="scanmatcher_frame" />
| cs |
(2) Edit grid_map_topic
1
|
<param name="grid_map_topic" value="scanmatcher_map"/>
| cs |
grid_map_topic은 costmap을 만들 때, 기반이 되는 2D 지도가 된다.
hector_mapping을 이용해 얻을 수 있으며, "map" 이란 토픽명으로 전달된다.
그러므로 다음과 같이 수정해준다.
1
|
<param name="grid_map_topic" value="map"/>
| cs |
(3) Edit elevation_map_topic
1
|
<param name="elevation_map_topic" value="/elevation_map_local" />
| cs |
elevation_map_topic은 costmap을 만들 때 사용되는 elevation_map의 이름이 된다.
하지만 실제로 구동시에 전달받는 토픽 명은 "/ElevationMapping/elevation_map_local"이 되므로, 다음과 같이 수정해준다.
1
|
<param name="elevation_map_topic" value="/ElevationMapping/elevation_map_local" />
| cs |
(4) Edit cloud_topic
1
|
<param name="cloud_topic" value="/openni/depth/points"/>
| cs |
costmap 노드에서는 실시간으로 받아오는 Depth camera의 데이터를 이용할 수도 있는데, 혹시 사용하게 될 경우를 고려하여 아래와 같이 변경해준다.
1
|
<param name="cloud_topic" value="/camera/stream/1/cloud"/>
| cs |
전체적인 코드는 다음과 같다.
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
|
<?xml version="1.0"?>
<launch>
<node pkg="hector_costmap" type="hector_costmap" name="hector_costmap" output="screen" respawn="false">
<param name="cost_map_topic" value="cost_map" />
<param name="map_frame_id" value="map" />
<param name="local_transform_frame_id" value="scanmatcher_frame" /> <!--default : base_footprint -->
<param name="initial_free_cells_radius" value="0.3" />
<param name="update_radius" value="1.4"/>
<param name="costmap_pub_freq" value="4.0" />
<param name="sys_msg_topic" value="syscommand" />
<param name="use_grid_map" value="true" />
<param name="grid_map_topic" value="map" /> <!-- default : scanmatcher_map -->
<param name="use_elevation_map" value="true" />
<param name="elevation_map_topic" value="/ElevationMapping/elevation_map_local" />
<param name="use_cloud_map" value="false" />
<param name="cloud_topic" value="/camera/stream/1/cloud" /><!--default : openni/depth/points -->
<!-- Not necessary to set, set by dynamic reconfigure -->
<!--param name="max_delta_elevation" value="0.08"/-->
<!--param name="allow_elevation_map_to_clear_occupied_cells" value="true" /-->
<!--param name="max_clear_size" value="4" /-->
<!--param name="slize_min_height" value="0.3" /-->
<!--param name="slize_max_height" value="0.4" /-->
<!-- Not necessary to set, set by scanmatcher -->
<!--param name="map_resolution" value="0.05" /-->
<!--param name="max_grid_size_x" value="1024" /-->
<!--param name="max_grid_size_y" value="1024" /-->
<!-- Not necessary to set, set by elvation_mapping -->
<!-- param name="elevation_resolution" value="0.01" / -->
</node>
</launch>
| cs |
5. Run hector_slam
이제 세팅된 내용을 기반으로 프로그램을 돌려보자.
다음의 명령어를 수행한다.
(한번에 실행시키는 launch 파일을 만드려 했지만 상황에 따라 각각 노드가 수행되는 시간이 달라 일단 따로 실행하게 되었다.)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
|
# terminal_1
$ roslaunch hector_mapping mapping_default.launch
# terminal_2
$ roslaunch hector_elevation_mapping elevation_mapping_node.launch
# terminal_3
$ roslaunch hector_elevation_visualization hector_elevation_visualization.launch
# terminal_4
$ roslaunch hector_costmap costmap.launch
# terminal_5
$ rviz
| cs |
위 내용을 수행하면 다음과 같이 slam이 진행된 결과를 확인할 수 있다.
댓글 없음:
댓글 쓰기