====== 5. 自己位置推定 ======

# 2015-06-08 AND

追記：再度デバックするときは必ず、roscoreを含めて全てのプログラムを停止してやり直すこと。
(前回のAMCLの自己位置推定の結果が残ってしまうため)

追記："odom_model_type"について、"diff"ではバグがあるらしく、最近"diff-correct"を使うことを推奨しているらしい。ただし、これするとその他のパラメータを再調整する必要があるらしいので注意すること。
====== navigationインストール ======
  kinetic環境の人
  $ sudo apt-get install ros-kinetic-navigation

====== AMCLによる自己位置推定(オフライン) ======

  ターミナル1
  $ roscore
  
  ターミナル2
  $ cd ~/ros_study
  $ rosrun map_server map_server map_name.yaml                       //前回生成した地図を開く
  
  ターミナル3
  $ rosrun amcl amcl
  
  ターミナル4
  $ cd ~/ros_study
  $ rosbag play log_name_timestamp.bag                 //前回地図生成に使用したログファイルを再生する


====== AMCLによる自己位置推定(オンライン) ======

ロボットのスタート位置は、地図を作ったときとほぼ同じ位置 姿勢にすること。

  ターミナル1
  $ roscore
  
    ターミナル2（ロボットとPCを接続しておくこと）
  $ ypspur-coordinator -p your_robot.param -d /dev/ttyACM0
  
  ターミナル3
  $ rosrun ypspur_ros_bridge ypspur_ros_bridge
  
  ターミナル4
  $ rosrun hokuyo_node hokuyo_node _port:=/dev/ttyACM1        または、    $ rosrun hokuyo_node hokuyo_node _port:=/dev/ttyACM1 _min_ang:=-2.08621382713 _max_ang:=2.08621382713
  
  ターミナル5
  $ rosrun sensor_tf_test sensor_tf_test
  
  ターミナル6
  $ cd ~/ros_study
  $ rosrun map_server map_server map_name.yaml                       //前回生成した地図を開く
  
  ターミナル7
  $ rosrun amcl amcl
  
  ターミナル8
  $ rosrun joystick_commander joystick_commander /dev/input/js0


====== AMCLのパラメータを設定したい場合（オフライン） ======

  ターミナル1
  $ roscore
  
  ターミナル2
  $ cd ~/ros_study
  $ rosrun map_server map_server map_name.yaml                       //前回生成した地図を開く
  
  ターミナル3
  $ cd ~/catkin_ws/src
  $ catkin_create_pkg ros_study_2dnav
  $ cp /opt/ros/indigo/share/amcl/examples/amcl_diff.launch ~/catkin_ws/src/ros_study_2dnav
  $ gedit ~/catkin_ws/src/ros_study_2dnav/amcl_diff.launch                 //このファイルを書き換えてパラメータを調整する
  $ roslaunch ros_study_2dnav amcl_diff.launch                                         //設定したパラメータでAMCLを実行
  
  ターミナル4
  $ cd ~/ros_study
  $ rosbag play log_name_timestamp.bag              //前回地図生成に使用したログファイルを再生する


各パラメータの意味はhttp://wiki.ros.org/amclを参照。

下記にrobolin.jpのRossyがつくばチャレンジ2014で実際に使用したパラメータを書く。参考までに。

  <launch>
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <remap from="scan" to="scan" />
    <param name="odom_frame_id" value="odom"/>
    <param name="global_frame_id" value="map"/>
    <!-- Publish scans from best pose at a max of 10 Hz -->
    <param name="odom_model_type" value="diff"/>
    <param name="transform_tolerance" value="0.2" />
    <param name="gui_publish_rate" value="10.0"/>
    <param name="laser_max_beams" value="30"/>
    <param name="min_particles" value="500"/>
    <param name="max_particles" value="5000"/>
    <param name="kld_err" value="0.05"/>
    <param name="kld_z" value="0.99"/>
    <param name="initial_cov_xx" value="0.25"/>
    <param name="initial_cov_yy" value="0.25"/>
    <param name="initial_cov_aa" value="0.068539"/>
    <param name="initial_cov_xx" value="0.01"/>
    <param name="initial_cov_yy" value="0.01"/>
    <param name="initial_cov_aa" value="0.03"/>
    <!--<param name="odom_alpha1" value="0.2"/>-->
    <!--<param name="odom_alpha2" value="0.2"/>-->
    <param name="odom_alpha1" value="0.1"/>
    <param name="odom_alpha2" value="0.1"/>
    <!--<param name="odom_alpha1" value="0.1"/>-->
    <!--<param name="odom_alpha2" value="0.05"/>-->
    <!-- translation std dev, m -->
    <param name="odom_alpha3" value="0.8"/>
    <param name="odom_alpha4" value="0.2"/>
    <!--<param name="odom_alpha3" value="0.2"/>-->
    <!--<param name="odom_alpha4" value="0.1"/>-->
    <param name="odom_alpha5" value="0.1"/>
    <param name="laser_z_hit" value="0.5"/>
    <param name="laser_z_short" value="0.05"/>
    <param name="laser_z_max" value="0.05"/>
    <param name="laser_z_rand" value="0.5"/>
    <param name="laser_sigma_hit" value="0.2"/>
    <param name="laser_lambda_short" value="0.1"/>
    <param name="laser_model_type" value="likelihood_field"/>
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0"/>
    <param name="update_min_d" value="0.2"/>
    <!--<param name="update_min_d" value="0.5"/>-->
    <param name="update_min_a" value="0.5"/>
    <param name="resample_interval" value="1"/>
    <!--<param name="resample_interval" value="5"/>-->
    <param name="recovery_alpha_slow" value="0.0"/>
    <param name="recovery_alpha_fast" value="0.0"/>
  </node>
  </launch>

====== AMCLの様子をrvizで見る ======

  $rosrun rviz rviz


下記の画面のようにして、"/particlecloud"を追加する。

{{:ros:study:fig5-1.png|}}

"Fixed Frame"を"map"にする。

{{:ros:study:fig5-2.png|}}

下記の画像を参考にパーティクルを見やすくする。

{{:ros:study:fig5-3.png|}}

<color red>rvizを閉じる前に、設定を保存しておくこと</color>