http://wiki.ros.org/hector_slam
위 사이트를 통해 download 받을 수 있지만,
환경설정 방법과 사용 방법은 그렇게 쉽지 않다.
그러므로 여기에 해당 내용들을 정리해보겠다.
Simulation 작업환경 [Desktop]
OS: Ubuntu 14.04 LTS
ROS version : Indigo
Compiler : catkin
Depth Camera : Kinect Sensor
0. 실험환경 구성하기
먼저, 실험환경은 본 블로그의 SLAM 카테고리에 존재하는 Gazebo를 이용한 드론 환경을 기준으로 한다. 다음 링크를 참고하면 된다.
How to 2D LRF SLAM with erle-copter
https://suho0515.blogspot.com/2019/08/slam-11-how-to-2d-lrf-slam-with-erle.html
참고로 위 링크에서는 hector_slam의 package 중 LRF 를 이용한 2D SLAM 만을 진행하므로
환경을 구성하는데에만 참고하도록 하자.
위 링크를 통해서 다음과 같은 요소들이 충족된다고 가정한다.
- hector_slam package
- erle-copter simulation package
또한 추가적으로 hector_navigation package를 받아야한다. git 주소는 다음과 같다.
https://github.com/tu-darmstadt-ros-pkg/hector_navigation.git
다음으로 hector_slam을 실행하기위한 과정을 진행하도록 한다.
1. Edit Depth camera frame
일전에 다음 링크와 같은 과정을 통해 erle-copter에 Depth camera를 추가하였다.https://suho0515.blogspot.com/2019/07/slam-05-use-gazebo-depth-camera-with-ros.html
https://suho0515.blogspot.com/2019/07/slam-06-erlecopter-gazebo-depth-camera.html
그리고 이 때,
~/simulation/ros_catkin_ws/src/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin/urdf/sensors
위 경로에 "kinect.urdf.xacro" 파일을 만들었는데, 이 파일에서 맨 마지막에 plugin을 설정하는 line에서 <frameName>에 다음과 같이 수정이 필요하다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
|
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>camera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>0</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> <!-- original value was camera_link -->
<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>
| cs |
즉 <frameName>camera_link</frameName> 를
<frameName>camera_depth_optical_frame</frameName> 로 수정하는 것이다.
그 이유는 차후에 tf를 관리할 때, kinect camera의 frame을 hector_slam에서 만들어지는 2D Occupancy Grid Map과 동일시 해줄 때 kinect camera의 frameName을 이용하게 되는데, 이 때 frameName이 kinect의 end link와 동일시 되야 하기 때문이다.
2. Add tf static_transform_publisher
이후에~/simulation/ros_catkin_ws/src/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin/launch
위 경로에 "erlecopter_spawn.launch" 파일의 <launch> 아래에 다음과 같은 code를 추가해준다.
1
2
|
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_frame_to_sonar2_link_tf" args="0 0 0 0 0 0 /scanmatcher_frame /sonar2_link 100"/>
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_frame_to_camera_depth_optical_frame_tf" args="0 0 0 -1.58 0 -1.58 /scanmatcher_frame /camera_depth_optical_frame 100"/>
| cs |
위 code는 tf package에 있는 static_transform_publisher란 node를 실행시키는 code이다.
이 때, arguments 값들이 중요한데.
첫번째의 경우 "/scanmatcher_frame"에서 "/sonar2_link"로 위치나 각도의 변화 없이 tf를 변환시킨다는 의미이고,
두번째의 경우 "/scanmatcher_frame"에서 "/camera_depth_optical_frame"로 위치의 변화 없이 각도는 roll -1.58 / pitch 0 / yaw -1.58 로 tf를 변환시킨다는 의미이다.
이 때, /sonar2_link는 erle-copter의 LRF Sensor의 frame,
/camera_depth_optical_frame은 Depth camera의 frame,
그리고 /scanmatcher_frame은 LRF Sensor를 이용해 만들어진 2D Occupancy Grid Map의 frame이다.
필자의 경우 어째선지 가상환경에서 Depth camera의 각도와 위치가 정상이 아니었는데,
위 코드를 통해서 Depth camera의 위치와 각도를 조정해줄 수 있었다.
3. Make hector_slam.launch
~/simulation/ros_catkin_ws/src/hector_slam/hector_slam_launch/launch위 경로에서 "hector_slam.launch" 란 파일을 만들고 다음과 같은 내용을 입력 후 저장한다.
명령어는 다음과 같겠다.
cd ~/simulation/ros_catkin_ws/src/hector_slam/hector_slam_launch/launch
nano hector_slam.launch
다음 내용 입력 후 저장
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
|
<?xml version="1.0"?>
<launch>
<param name="/use_sim_time" value="true"/>
<node pkg="rviz" type="rviz" name="rviz"
args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>
<include file="$(find hector_mapping)/launch/mapping_default.launch"/>
<!--include file="$(find hector_imu_attitude_to_tf)/launch/example.launch"/-->
<include file="$(find hector_elevation_mapping)/launch/elevation_mapping_node.launch"/>
<include file="$(find hector_costmap)/launch/costmap.launch"/>
<include file="$(find hector_elevation_visualization)/launch/hector_elevation_visualization.launch"/>
</launch>
| cs |
위 소스를 하나하나 살펴보자.
line 1 : xml의 format을 정해주는 code이다. 없어도 문제 없다.
line 5 : simulation time을 이용한다는 내용이다.
line 7 : 저장된 rviz 설정파일을 이용해 rviz를 열어준다는 내용이다.
line 10 : mapping_default.launch 파일을 실행한다. LRF Sensor를 이용해 2D SLAM을 진행하는 파일이다.
line 12 : imu sensor data를 SLAM 과정에 포함시키기 위해서는 활성화 시켜줘야 한다. 필자는 imu data를 사용하지 않기 때문에 비활성화 되어있다.
line 14 : 3D Depth Camera를 이용해 elevation mapping을 진행하는 파일이다.
line 16 : 2D Occupancy Grid Map에 Elevation Map으로부터 얻은 3D Depth Data를 합쳐 Cost Map을 만드는 파일이다.
line 18 : elevation mapping을 통해 얻어낸 Elevation Map을 rviz에 display하기 위한 파일이다.
4. Edit mapping_default.launch
이제 2D SLAM을 하는 mapping_default.launch 파일을 수정해보자.수정할 부분은 다음과 같다. ( Gazebo를 이용한 Simulation을 하는 것을 기준으로 )
- base_frame/odom_frame -> sonar2_link ( LRF Sensor이 sonar2_link이므로 이를 base_frame이자 odom_frame으로 잡는다. )
- map_frame -> world (Gazebo의 frame이 world이므로 world로 수정 )
참고로, 정신없이 수정하다보니 제대로 언급을 못한 부분이 있을 수 있다.
수정된 내용은 다음과 같으면 된다.
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
|
<?xml version="1.0"?>
<launch>
<arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
<arg name="base_frame" default="sonar2_link"/>
<arg name="odom_frame" default="sonar2_link"/>
<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="world" />
<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>
<!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->
</launch>
| cs |
5. Edit elevation_mapping_node.launch
이제 Elevation Mapping을 하는 elevation_mapping_node.launch 파일을 수정해보자.수정할 부분은 다음과 같다. ( Gazebo를 이용한 Simulation을 하는 것을 기준으로 )
- map_frame_id를 /world로 ( Gazebo의 frame이 world이므로 world로 수정 )
- local_map_frame_id를 /scanmatcher_frame으로 ( hector_slam에서 local_map_frame_id는 2D SLAM으로부터 생성된 /scanmatcher_frame이 됨.)
- point_cloud_topic을 /camera/depth/points로 ( kinect로부터 받아오는 topic명은 /camera/depth/points임. )
- grid_map_topic을 /map으로 ( 2D SLAM으로부터 만들어진 Map은 /map이란 topic명으로 전달됨. )
참고로, 정신없이 수정하다보니 제대로 언급을 못한 부분이 있을 수 있다.
수정된 내용은 다음과 같으면 된다.
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="tf" type="static_transform_publisher" name="map_to_base_stabilized" args="0 0 0 0 0 0 /map /base_stabilized 100"/-->
<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="/world" />
<param name="local_map_frame_id" value="/scanmatcher_frame" /> <!-- original value was "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/depth/points"/>
<param name="grid_map_topic" value="/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 |
6. Edit costmap.launch
이제 2D Occupancy Grid Map과 Elevation Map을 합치는 costmap.launch 파일을 수정해보자.수정할 부분은 다음과 같다. ( Gazebo를 이용한 Simulation을 하는 것을 기준으로 )
- map_frame_id를 /world로 ( Gazebo의 frame이 world이므로 world로 수정 )
- local_map_frame_id를 /scanmatcher_frame으로 ( hector_slam에서 local_map_frame_id는 2D SLAM으로부터 생성된 /scanmatcher_frame이 됨.)
- grid_map_topic을 /map으로 ( 2D SLAM으로부터 만들어진 Map은 /map이란 topic명으로 전달됨. )
- elevation_map_topic을 /ElevationMapping/elevation_map_local로 수정 ( elevation map topic 의 이름이 /ElevationMapping/elevation_map_local 이므로 수정해준다. )
- cloud_topic을 /camera/depth/points로 ( kinect로부터 받아오는 topic명은 /camera/depth/points임. )
참고로, 정신없이 수정하다보니 제대로 언급을 못한 부분이 있을 수 있다.
수정된 내용은 다음과 같으면 된다.
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="world" />
<param name="local_transform_frame_id" value="scanmatcher_frame" />
<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" />
<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/depth/points" /> <!-- original value is "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 |
7. Execute hector_slam
드디어 모든 준비가 끝나고 hector_slam을 실행할 수 있게 되었다.다음과 같은 명령어를 통해 실행시킬 수 있다.
(1) Access ArduCopter
$ cd ~/simulation/ardupilot/ArduCopter(2) Spawn erle-copter
$ roslaunch ardupilot_sitl_gazebo_plugin erlecopter_spawn.launch(3) Launch hector_slam.launch
$ roslaunch hector_slam_launch hector_slam.launch(4) Make erle-copter fly
$ rosrun ros_erle_takeoff_land ros_erle_takeoff_land
( (4)번 부분은 다음 링크에서 참고할 수 있다.
https://suho0515.blogspot.com/2019/09/slam-17-drone-automatic-driving.html )
성공적으로 SLAM이 되었다면 다음과 같은 결과를 얻을 수 있을 것이다.
( (4)번 부분은 다음 링크에서 참고할 수 있다.
https://suho0515.blogspot.com/2019/09/slam-17-drone-automatic-driving.html )
성공적으로 SLAM이 되었다면 다음과 같은 결과를 얻을 수 있을 것이다.
댓글 없음:
댓글 쓰기