# ROS- SLAM/LIDER

## もくもくじ
1. [LIDARを回す](#LIDARを回す)
1. [gmapping](#gmapping)
1. [hector君](#hector_slam)
1. [bagファイル](#bagファイル)
1. [yamlファイル](#YAMLファイル)
1. [navigation](#Navigation)

## SLAM
[参考資料](https://ssk0109.hatenablog.com/entry/2018/12/30/063109)

## LIDERを回す
[参考にした資料](http://robonchu.hatenablog.com/entry/2018/09/15/092733
)

cd catkin_ws
source devel/setup.bash

//ポートに権限付与
ls -l /dev | grep ttyUSB
sudo chmod 666 /dev/ttyUSB0

roslaunch rplidar_ros view_rplidar.launch

//ノードだけ立ち上げるときは
roslaunch rplidar_ros rplidar.launch

出力されているTopicは
scan (sensor_msgs/LaserScan) : it publishes scan topic from the laser.

## gmapping
[ROS.wiki](http://wiki.ros.org/slam_gmapping)

[paramまとめ](https://sy-base.com/myrobotics/ros/gmapping/)

**やったこと雑記**
- wikiの手順でネットのbagファイルから地図作成に成功
- 自作bagデータからのマッピング
- rosbag record -O mylaserdata /base_scan /tf
- rosbag recode -a
- エラー:use_sim_time set to true and no clock published.
lidarのlaunchファイルを再起動すれば直る
- 安定のtfに送れていない問題
てかなぁにこれ
さっき撮ったときと全然違うやんけ
↓
launchファイル起動したら変わった
おそらく
$ rosrun gmapping slam_gmapping scan:=base_scan
の起動し忘れが原因

- tf(というかgmappingのlaunchファイルの書き換え)roslaunch gmapping slam_gmapping_pr2.launchの名前を変えていく
- launchファイルの
<remap from="scan" to="base_scan"/>
という項目を削除

- launchファイルでtfのframeがいじれなかったのでとりまstatic_tfで試してみる
- launchファイルではなく
rosrun gmapping slam_gmapping scan:=scan _base_frame:=map
コマンドを使用
map→odomのみ
- launchファイルに記述する<param name="base_frame" value="map" />
→rosrun gmapping slam_gmappingのみで大丈夫
- tfは反映できたがtfのマークが中心で固定されたまま
- tfツリーの参考例 [link](https://www.seeedstudio.com/blog/2018/11/09/rplidar-and-ros-the-best-way-to-learn-robot-and-slam/)
- 

- rosrun tf static_transform_publisher 0 0 0 0 0 0 odom base_link 100

- rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link laser 100

- rosrun tf static_transform_publisher 0 0 0 0 0 0 map odom 100

[tfパラメーター説明](https://memo.soarcloud.com/ros%E3%81%AE%E5%BA%A7%E6%A8%99%E5%A4%89%E6%8F%9Btf%E3%81%AB%E3%81%A4%E3%81%84%E3%81%A6/)

- 地図生成されなくなったが　launchファイルに<param name="base_frame" value="map" />　を書き加えることで解決

- tf設定にもどる

- なんか[ここ](https://github.com/Winwinindustrial/rplidar/blob/master/rplidar_ros-slam/README.md)のサイトのファイル使ったら行けてしまった。tfは若干おかしい。
今までとさほど(というか全然)変わらないじゃないか。腑に落ちん。

[なんかできてしまったんやが(怒)](https://github.com/Winwinindustrial/rplidar/blob/master/rplidar_ros-slam/launch/gmapping.launch)

[角度変更の記事](http://geduino.blogspot.com/2015/04/gmapping-and-rplidar.html)

- 最終的にrosparam set use_sim_time を falseに書き換えてやることで解決

**とりあえずよく使うコマンド**
- rosparam set use_sim_time true
- roslaunch gmapping slam_gmapping_pr2.launch

## hector_slam
[ROS.wiki](http://wiki.ros.org/hector_slam)

**地図作成**
- ポートに権限付与
ls -l /dev | grep ttyUSB
sudo chmod 666 /dev/ttyUSB0
- lider起動
roslaunch rplidar_ros view_rplidar.launch
- hector_slam起動
roslaunch hector_mapping mapping_default.launch
- 動きまわってデータ収集
- 地図保存
#地図ファイルを保存する場所に移動
rosrun map_server map_saver -f [地図名]

**コマンドまとめ**
- hector_slam起動
roslaunch hector_slam_launch tutorial.launch(マスター有り)

- hector_slam起動(今回はこちらを使用)
roslaunch hector_mapping mapping_default.launch

- 地図保存
#地図ファイルを保存する場所に移動
rosrun map_server map_saver [地図名]

**やったこと雑記**

[参考資料1](https://qiita.com/PINTO/items/b3eb7205baf9c55de81b)
[参考資料2(wiki)](
http://wiki.ros.org/hector_slam/Tutorials/MappingUsingLoggedData
)

あとなんかQT必要みたいだった
sudo apt-get install qt4-dev-tools
sudo apt-get install qtcreator

TF一切データ送れてなかったため地図作成できない

- 自分でとったbagファイル(失敗)
- lidar値直(失敗)

waiting mapで止まる

- チュートリアルのbagファイル再生(成功)

[tf設定１](https://answers.ros.org/question/209276/rviz-slam-rplidar-problem/)

[tf設定２](https://robotics.stackexchange.com/questions/12309/hector-slam-in-ros-with-rplidar)
ちゃんと一致してるか確認

Transform [sender=unknown_publisher]　For frame [laser]: Fixed Frame [map] does not exist
RVis上のエラー
[static tf 設定](https://qiita.com/dendensho/items/959c6d510593546f4020)

ここをfalseにすることでmap→odom変換が有効に

<!-- Tf use -->
<param name="use_tf_scan_transformation" value="false"/>

その他launchファイル訂正

<node pkg="tf" type="static_transform_publisher" name="tf_odom_to_base_link_broadcaster" args="0 0 0 0 0 0 odom base_link 100" />
<node pkg="tf" type="static_transform_publisher" name="tf_base_link_to_laser" args="0 0 0 0 0 0 base_link laser 100" />

これらをもとにgmapping-defaultlaunchファイル書き換え
あとはトライ&エラーでピンポイントで修正かけていく


[最終的に変える必要があったフレーム名はここ](https://uenota.github.io/dronedoc/slam/localization_and_mapping.html)

base_frame
odom_frame
scan_topic
の３つを使うノードに合わせた形に変える必要がある

[ERROR] [1574212104.226735000]: Transform failed during publishing of map_odom transform: Lookup would require extrapolation into the past. Requested time 1574212103.618088670 but the earliest data is at time 1574212103.632924227, when looking up transform from frame [base_link] to frame [odom]

tf_publisherの設定で速度が違っていたためこのエラー吐いてた

## bagファイル
[Wiki(lidarのデータからbagファイル生成)](http://wiki.ros.org/slam_gmapping/Tutorials/MappingFromLoggedData#Making_your_own_bag)

rosbag info [bagファイル]

http://wiki.ros.org/rplidar

**よく使うコマンド**
- rosbag record -a
topicの名前をかけばそのトピックのみ保存してくれる
例)rosbag record /topic1 /topic2
ファイル名を指定して保存
例)rosbag record -O file.bag

- $ cd ~/Downloads
rosbag play Team_Hector_MappingBox_RoboCup_2011_Rescue_Arena.bag --clock

## YAMLファイル

**中身の概要**
image: testmap.png
resolution: 0.1
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0

- image: イメージファイル（YAMLからの相対・もしくは絶対パス）
- resolution: 解像度[meters/pixel]
- origin: マップの左下端の座標(x,y,θ)
- occupied_thresh: この数値より大きいところを障害物とみなす
- free_thresh: この数値より小さいところをフリースペースとみなす
- negate: 白黒反転させるかどうか

## Navigation
[ROS.wiki](http://wiki.ros.org/navigation)

[map_serverのwiki](https://wiki.ros.org/map_server)

[base_link-laser間のtf変換](http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF)

[ナビゲーションガイド](http://kaiyuzheng.me/documents/navguide.pdf)

**まずはシミュレーション**

- navigation
[参考資料](https://qiita.com/MoriKen/items/8387b279e968368783f1)

roslaunch husky_gazebo husky_playpen.launch

roslaunch husky_viz view_robot.launch

roslaunch husky_navigation move_base_mapless_demo.launch

2Dnav_goalでナビゲーションできたよ！やったね！
- rqt_graph
参考
- amclで遊ぼう！

**概要理解**
- 経路探索
- コストマップcostmap
…センサ情報をもとに検出された障害物に近いほど値が大きくなるコスト情報
黄色：障害物
シアン：ロボットの領域から逆算して，侵入したら障害物と衝突する領域
青：ぶつからないかもしれないけど，すれすれを通るのも怖いのでできれば避けたい領域
- 経路計画plannner
…コスト情報を使って障害物を回避しながら経路計画
- 大域的探索
…事前に得られた地図などの情報を元に，予め壁や障害物があるところにコストを割り振り，経路を決定

経路情報　/move_base/NavfnRos/plan
NavfnRos
- 局所的探索
…現在得られたセンサ情報を元に，事前知識では存在しない障害物（歩行中の人など）を検知し，リアルタイムに経路を補正する

/move_base/DWAPlannerROS/local_plan
DWSPlannerROS

- 積み込み状態から回復(rotate_recoveru)
囲われたときにそこから抜け出す
その場旋回が可能なディファレンシャルドライブ型，あるいはオムニドライブ型のロボットであることが前提
こんな感じ


- 尤度
レーザーの値と照らし合わせたときの一致度のようなもの

**やってみる**

- hector_navigationパッケージを作成
- hasky_navigationからlaunchファイルとamclファイルをコピー
- パッケージ名など書き換える

- まずはmove_base

- map配信のコマンド

cd ~/ファイルに移動
rousun map_server map_server [yamlファイル]
rosrun map_server map_server hector_map_use.yaml

- launchファイル記述の仕方

  <arg name="map_file" default="$(find hector_navigation)/maps/hector_map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)">
    <param name="frame_id" value="map"/>
  </node>

~~<arg name="no_static_map" default="true"/> をあとでfalseに変えないといけないかもしれない~~
trueにしちゃだめです

-
- コマンド
roslaunch hector_navigation move_base.launch

- 次にamcl
bagファイルとamclのlaunchファイルを再生(amclにmap_saver書き込み済み)
座標変換も追加(map→odom変換は提供される)
- use_map_topic (bool, default: false)とは
マップを受信するためのサービス呼び出しを行うのではなく、マップトピックをサブスクライブします。
- amclが使えない原因[これ](https://github.com/ros-planning/navigation2/issues/339)では…？
要約：odomが配信されてないから使えないやで
→ためしにmap odom間をブロードキャストしたらいけた
- まずもってodom_base_link間の通信おかしいから直すhttps://qiita.com/deltaMASH/items/abf4a675669706290d3b
[参考](http://wiki.ros.org/ja/tf/Tutorials/Writing%20a%20tf%20broadcaster%20(C++))

[まじに自己位置どうすりゃええの…](https://qiita.com/deltaMASH/items/abf4a675669706290d3b)

流れ的にはやはりmap->(amcl)->odom->(計算など)->base_linkになるはず

[roswiki](http://wiki.ros.org/navigation/Tutorials/RobotSetup)よりnavigationを使うには次の情報が必要
1. tf変換の公開
2. センサー情報
3. 走行距離情報
tfおよびnav_msgs/Odometryメッセージを使用
4. ベースコントローラー
cmd_velのgeometry_msgs/Twistを使用
5. map情報

3.走行距離をすすめる
とりあえずwikiを参考にerratic_player を使ってみる。
port_name（string、デフォルト： "/ dev / ttyUSB0"）
ロボットが接続されているシリアルポートの名前。
odometry_frame_id（string、デフォルト： "odom"）
TFメカニズムを介してスタンプされた変換を送信するために使用されるフレームの名前。
max_trans_vel（string、デフォルト： "0.5"）
最大並進速度。
max_rot_vel（string、デフォルト： "100"）
最大回転速度。
max_rot_vel（string、デフォルト： "100"）
最大回転速度。
trans_acc（string、デフォルト： "0"）
最大の並進加速度、非負。「0」の値は、ロボットのデフォルト値を使用することを意味します。
trans_decel（string、デフォルト：trans_acc）
最大の並進減速度、非正。「0」の値は、ロボットのデフォルト値を使用することを意味します。
rot_acc（string、デフォルト： "0"）
最大回転加速度、非負。「0」の値は、ロボットのデフォルト値を使用することを意味します。
rot_decel（文字列、デフォルト：rot_acc）
最大回転減速度、非正。「0」の値は、ロボットのデフォルト値を使用することを意味します。
enable_ir（bool、デフォルト：false）
IRセンサーを有効にするかどうか。
enable_sonar（bool、デフォルト：false）
Sonarセンサーを有効にするかどうか。
enable_vision_pan_tilt（bool、デフォルト：false）
ビジョンパン/チルトサーボモーターを有効にするかどうか。
enable_ranger_tilt（bool、デフォルト：false）
レンジャーチルトサーボモーターを有効にするかどうか。