Tsukuba Challenge 2016

つくばチャレンジ2016用のリポジトリです。
GitHub-Flowによる運用を行いたいと思います。

How to use ?

1. catkin_workspaceの作成
   好きなディレクトリでcatkin_workspaceを作ってください。ここでは分かりやすいようにtc2016_wsとします。特別な理由が無い限りこのディレクトリ名を推奨します。

cd <your favorite directory to develop>
mkdir -p tc2016_ws/src
cd tc2016_ws/src
catkin_init_workspace
cd ..
catkin_make

catkin_makeが成功することを確かめましょう。

2. 本リポジトリのクローン
   本リポジトリを先ほど作ったワークスペースのsrcにクローンしましょう。

cd tc2016_ws/src
git clone git@github.com:CIR-KIT/TC2016.git

チームメンバーであればsshでクローンできるので、もし失敗する場合にはsshの設定を見直すか、チームメンバーに参加できているかを確認してください。

3. 他のパッケージのダウンロード wstoolを使って必要なパッケージをダウンロードしてきます。

cd tc2016_ws/
wstool init src src/TC2016_for_thirdrobot/third_robot.rosinstall

他のパッケージのアップデートは

cd tc2016_ws
wstool update -t src

でできます。

.rosinstallファイルに追記した場合は

cd tc2016_ws
wstool merge -t src src/TC2016_for_thirdrobot/third_robot.rosinstall

とします。

自律移動の流れ

ゼロの状態から自律移動を行うまでの流れを説明します。

1. 地図の作成

環境中をロボットを走らせながらgmapping等を使って地図を作ります。リアルタイムに作ってもいいし、rosbagで保存してからやってもいいです。個人的にはパラメータを変えながら試せるのでrosbagに記録してからのほうがいいと思います。以下rosbagに記録してから行うことを想定します。地図作成以外にも使うのでrosbag record -aで全部保存しましょう。rosbagから地図を作る場合にはgmappingの場合、

rosrun gmapping slam_gmapping_replay --scan_topic=/scan_multi --bag_filename=2016-10-19-15-49-19.bag  _particles:=50 _delta:=0.1 _iterations:=30 _str:=15.5 _stt:=15.5 _linearUpdate:=0.3 _angularUpdata:=0.1

こんな感じで実行できます。トピック名、bagファイル名、パラメータは適宜変更してください。パラメータに関しては実際のところちゃんとオドメトリがとれていれば特にデフォルトで綺麗にできます。
rviz上でいい感じに地図が出来たら、地図を保存します。

rosrun map_server map_saver -f filename

カレントディレクトリに-fオプションで指定した名前の地図が出来ます。pgmファイルが実態ですが、地図の情報が含まれているyamlファイルも重要です。地図の名前を変更したいときはファイル名自体を変更することに加えて、yamlファイル内の画像ファイルの名前も変更しないといけません。

2. Waypointの作成

今回はros_waypoint_generatorを使います。catkin workspaceにクローンしましょう。ちゃんとwstoolでやればすでにあるかもしれません。

git clone https://github.com/AriYu/ros_waypoint_generator.git

catkin_makeをしてコンパイル後、起動します。

rosrun ros_waypoint_generator ros_waypoint_generator

次に、map_serverを立ち上げて、地図を配信します。

rosrun map_server map_server map.yaml

map.yamlには1.で作った地図のファイルパスを指定します。
次にamclを走らせます。

roslaunch third_robot_2dnav amcl_diff.launch

次にbagファイルを再生し、自己位置推定を開始します。--clockオプションは指定しないでください。

rosbag play example.bag

ここが難しいですが、amclの初期位置がrosbagを開始しないと指定出来ないため、rosbag playを始めたらすぐに頑張って初期位置合わせをしてください。
自己位置推定が始まるとwaypointが生成されていきます。rvizでinteractive markerを表示するとwaypointを見ることができます。waypointの姿勢はtfを表示してください。また、markerはマウスのドラッグで移動出来ます。特に最初はどうしても思っているのと違うところにwaypointが出来てしまうので移動してあげてください。他にもrviz上部にあるPublish pointを使ってもwaypointを追加できます。ただし、現状ではwaypoint最後にしか追加出来ないので間に追加したかったら保存後に自分で編集してください。このへんを便利で出来そうなプルリクを待ってます。
また、どれだけ移動したらwaypointを追加するかとかもパラメータで変えられます。
waypointを作り終えたらwaypointを保存します。

rosrun ros_waypoint_generator ros_waypoint_saver

カレントディレクトリに次の形式のcsvファイルが生成されます。

x, y, z, qx, qy, qz, qw, is_searching_area, reach_threshold

is_searching_areaは探索エリアかどうかを判定するフラグで1なら探索エリアです。表計算ソフトで開くと一気に編集可能なのでこの辺という感じで一気に1にしましょう。reach_thresholdはそのwaypointにどれだけ近づいたらゴールとするかです。直径なので例えば3を指定していたらwaypointから1.5[m]以内に入ればゴールになります。
waypointを確認したいときは

rosrun ros_waypoin_generator ros_waypoint_generator --load waypoints.csv

でロードできます。探索エリアが指定されていればmarkerの色が黄色になります。

ここまで準備は終わりです。

3. Navigation

2.で作ったwaypointファイルをwaypoint_navigator/waypointsにコピーしておきましょう。本当はどこでもいいです。waypoint_navigator/launch/waypoint_nagigator.launchのwaypointsfileをさっき作ったやつに書き換えます。

実際にナビゲーションしましょう。実機なら

roslaunch third_robot_2dnav autorun.launch

でロボットドライバ他move_baseなどを立ち上げます。 navigatorをlaunchします。

roslaunch waypoint_navigator waypoint_navigator.launch

ロボットが自律移動を始めます。

4. 探索対象をみつける

実機の場合：

roslaunch target_object_detector target_object_detector.launch

実際に実験出来ない時：

fake_target_detectorを使って探索対象が見つかったことにできます。
まず、どこに探索対象がいることにするかを決めます。地図をrvizで表示しながら探索対象が居る場所にPublish pointを使ってクリックします。

rostopic echo /clicked_point

をすればクリックした座標がわかります。そのx, y座標をtargetlist/targetlist.csvにおきます。

rosrun fake_target_detector fake_target_detector

とすればtargetlist.csvに書かれた座標にBoundingBoxが表示されているはずです。そうすればちゃんとpublishもされています。
到達したwaypointが探索エリアでロボットから5[m]以内に探索対象がいればアプローチするはずです。
また一度アプローチした探索対象から近い場合には無視します。

How to use in Gazebo?

1. 人物付きでGazeboを起動する.

roslaunch third_robot_2dnav_gazebo autorun_with_human.launch

人を追加したかったら，上記launchを参考に追加して下さい．

2. Rviz 上で2D Pose Estimate で初期位置を修正する．

下記コマンドで可視化した時の最初のwaypointとほぼ同じ場所．

rosrun ros_waypoint_generator ros_waypoint_generator --load 2016-11-01-04-01-48.csv

3. Rviz 上で人の位置を好きに移動させる．

色々な場所で認識させたい場合は，Gazeboで一時停止して移動→探索　を繰り返して使いまわすといいかもしれません．

4. detectorを起動する．

topicはrelayしているので，実機と同じファイルで大丈夫です．

roslaunch target_object_detector target_object_detector.launch

5. navigaterを起動する．

こちらはwaypointファイルが異なるので，実機とは別ファイルです．

roslaunch waypoint_navigator waypoint_navigator_gazebo.launch