前回はHector SLAMを使って地図が更新できるようにいろいろセットアップできるようにしてみました

supernove.hatenadiary.jp

ところが、ここで作成された地図は思ってたのと違う結果になってしまいどうしたことか…という状態でした。

そんな中久々にROS UG JPのLT大会があり、前回エントリーをネタにLTしてきました。

このスライドをTwitterで投稿したら思いのほかバズって笑ってしまったのはここだけの話ですｗ。

で、そのツイートのリプに出力された地図がおかしい原因の解消に繋がりそうな有力情報がありました

こんにちは。
(rosjpの発表、お疲れ様でした！)

なんとなくなのですが、hector slamで地図がきれいに出来ない原因はtfの設定が上手くいってないから、な気がします。

この方の記事が参考になると思います。https://t.co/kFIECxysp8
— nisshan_ にっしゃん (@nisshan_) 2022年5月9日

それだぁぁーーーー！！！！

確かに言われて見れば地図を作成されるときってTFの座標系が表示されてそこから軌跡がrvizに出力されるはずなのにそんな様子がひとつもなかったんです。

というわけで、このリプをヒントに地図がきれいに生成されるように試してみました。(といっても頂いた記事の内容を参考にしただけですが…)

今回の成果物はいつもどおり以下のレポジトリのbacklog/step7ブランチで切っています。

github.com

mapping_default.launchの修正

以下の記事を参考にlauchファイルをいくつか修正していきます。

qiita.com

hector_slamディレクトリのhector_mapping/launch/mapping_default.launchのなかの5,6行目を修正します。

-  <arg name="base_frame" default="base_footprint"/>
-  <arg name="odom_frame" default="nav"/>
+  <arg name="base_frame" default="base_link"/>
+  <arg name="odom_frame" default="base_link"/>

そして54行目のコメントアウトを解除します。

-  <!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->
+  <node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>

最終的に以下のようになっていれば大丈夫です。

<?xml version="1.0"?>

<launch>
  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
  <arg name="base_frame" default="base_link"/>
  <arg name="odom_frame" default="base_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="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>
    
  <node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>
</launch>

tutorial.launchの修正

続いて同じくhector_slamディレクトリのhector_slam_launch/launch/tutorial.launchのuse_sim_timeをfalseにしてリアルタイム動作にします。

これで実機で問題なく地図が作成されるようになります。

<?xml version="1.0"?>

<launch>

  <arg name="geotiff_map_file_path" default="$(find hector_geotiff)/maps"/>

  <param name="/use_sim_time" value="false"/>

  <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_geotiff)/launch/geotiff_mapper.launch">
    <arg name="trajectory_source_frame_name" value="scanmatcher_frame"/>
    <arg name="map_file_path" value="$(arg geotiff_map_file_path)"/>
  </include>

</launch>

このファイルから先程修正したmapping_default.launchを呼び出していることがわかると思います。

hector_slam.launchの修正

最後にmy_roberのlaunchディレクトリ内の hector_slam.launchを先程修正した tutorial.launchを呼び出すように修正します。

  <!-- hector_slam -->
-  <include file="$(find hector_mapping)/launch/mapping_default.launch" />
-  <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch">
-    <arg name="trajectory_source_frame_name" value="scanmatcher_frame"/>
-    <arg name="map_file_path" value="$(arg geotiff_map_file_path)"/>
-  </include>
+  <include file="$(find hector_slam_launch)/launch/tutorial.launch"/> 
  
</launch>