====== 7. ウェイポイント ======

# 2015-07-08 AND \\
# 2016-05-24 waypoint_publisherの更新 \\
# 2019-08-25 HND 

===== ウェイポイントの設定 =====

地図生成(自己位置推定)と同時に、一定間隔おきにウェイポイントを設定する。

==== インストール方法 ====
2014年度卒のNSDとHARが作成したプログラム。

{{ :ros:study:auto_wp_setter.tar.gz |auto_wp_setter（tar.gz）}}をダウンロードして、
~/catkin_ws/srcに展開する。
----

2019.08.25 HND：姿勢差分が[-180deg, 180deg]の範囲を超えた場合に補正する処理を追加

{{ :ros:study:auto_wp_setter_ver2.tar.gz |}}

   $ cd ~/catkin_ws
   $ catkin_make

==== 実行方法 ====

  ターミナル1
  $ roscore
  
  ターミナル2
  $ rosparam set use_sim_time true
  $ cd ~/ros_study
  $ rosrun map_server map_server map_name.yaml                       //前回生成した地図を開く
  
  ターミナル3
  $ $ rosrun amcl amcl
  
  ターミナル4
  $ mkdir ~/ros_study
  $ cd ~/ros_study
  $ rosrun auto_wp_setter auto_wp_setter

上の4つのターミナルを実行させて準備が整ったら、また新しくターミナルを作り、次のコマンドでログデータを再生を始める。

  ターミナル5
  $ cd ~/ros_study
  $ rosbag play log_name_timestamp.bag --clock                      //"-r 0.3" などしてゆっくり再生してもより

再生が終わったらauto_wp_setterをCtrl + C で止める

すると、~/ros_studyにwp.txtというファイルが出来ている。

===== ウェイポイントへの追従 =====

==== インストール方法 ====
{{ :ros:study:waypoint_publisher.tar.gz |waypoint_publisher（tar.gz）}}をダウンロードして、
~/catkin_ws/srcに展開する。

   $ cd ~/catkin_ws
   $ catkin_make

==== 実行方法 ====

地図生成したときのスタート位置とだいたい同じ位置にロボットを置き、下記のコマンドを実行する。

ターミナル8を実行後にrviz上で2D pose estimate してスタート位置を指定してもよい。

追記： ターミナル9を実行後に2D pose estimateをした場合、次のウェイポイントを現在位置から最も近い点から始めるようになったので、スタート位置からでなくても良くなった

  ターミナル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    または、 $ roslaunch ros_study_2dnav amcl_diff.launch
  
  ターミナル8
  $ roslaunch ros_study_2dnav navigation.launch

<color red>下記を実行するとロボットが動き始めるので注意すること</color>

  ターミナル9
  $ cd ~/ros_study
  $ rosrun waypoint_publisher waypoint_publisher wp.txt

===== rviz =====

  ターミナル10
  $ rosrun rviz rviz

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

追記: "/waypoints" も追加して表示すると良い。

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


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