7. ウェイポイント

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

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

2014年度卒のNSDとHARが作成したプログラム。

auto_wp_setter（tar.gz）をダウンロードして、 ~/catkin_ws/srcに展開する。

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

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というファイルが出来ている。

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

ターミナル10
$ rosrun rviz rviz

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

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