fix fetch robot localization and navigation

[knorth55]

I add nodes which publish correct odom /odom_corrected and imu /imu_corrected.
Then I use these two corrected topics for robot_localization ukf for localization.
I wonder this change suppress odometry error.

• add odom_corrector.py
  • /odom does not have frame_id: /imu has an empty frame_id ZebraDevs/fetch_ros#70
  • /odom has zero covariance and overwrited in graft: https://github.com/fetchrobotics/fetch_robots/blob/indigo-devel/fetch_bringup/config/graft.yaml
• add imu_corrector.py
  • /imu has zero covariance and overwrited in graft: https://github.com/fetchrobotics/fetch_robots/blob/indigo-devel/fetch_bringup/config/graft.yaml
• use robot_localization instead of graft
  • graft is no more maintenanced: https://github.com/ros-perception/graft/issues/29#issuecomment-461223365 use robot_localization instead of graft ZebraDevs/fetch_robots#56
• refactor fetch_bringup.launch navigation parameters
• update correct_position.py
• update costmap parameters
• update move_base local planner parameters

now testing on fetch15.

cc. @708yamaguchi @sktometometo

related:

[708yamaguchi]

I am sorry, but I am not sure if this PR works correctly.
(I do not know the effect of ukf_localization_node.)

I think that this PR can be merged if jsk_patrol demo works correctly everyday.

[knorth55]

I'm working in go-to-kitchen first.
jsk_patrol is a bit complicated because of mail notification.
also I'm wondering if jsk_patrol totally shouldbe done in jsk_demos.

[knorth55]

Going kitchen demo is now working stably.
(x5 gif)

Robot head camera: https://drive.google.com/open?id=1K945-KXcfUQIeGudZpfd_9qWPyd95a8w
GoPro: https://drive.google.com/open?id=1gtCfZoSeAgj8Xs1SzmjneXrwqGx6qYAH

[knorth55]

@708yamaguchi
簡単に説明すると変更したことは、

• graft から robot_localizationに移行したい
  • graftはもうメンテナンスされてない
  • /odomがおかしい
    • frame_idが設定されてない
    • covarianceが設定されてない（graftでoverrideされていたのでそれをodom_correctorで上書きする）
  • /imuがおかしい
    • covarianceが設定されてない (graftでoverrideされていたのでそれをimu_correctorで上書きする）
• amclのrotationに対する不確実性をあげたい
  • odom_alphaの更新
    • 0.1 -> 0.5
• costmapを更新してロボットがぶつからないようにしたい、車輪がすべったり、その場回転で微妙に移動したりするのでものからできるだけ離れておきたい
  • inflation_radiusとcost_scaling_factorを変更する
    • できるだけ障害物から離れるパスを出すようにする
  • footprint_paddingを大きくする
    • 0.01 -> 0.05
• local plannerがおかしい
  • dwaを有効化する
    • もともと有効化されてなかった！
    • これを有効化するとFetchのやばげな早い動きがなくなった！
• correct_position.pyを更新する
  • covarianceを小さくする
    • dockの中なので小さくてOK
  • 1Hzでpublishする
    • dockに長時間で止まっているとずれていくので常に修正する

[knorth55]

NOTE: it seems that head_camera is not used for costmap observation resource... weird... why is fetch15 nodding head while moving...?
head camera is subscribed in Fetch Depth Layer by default. it was my misunderstanding.

[708yamaguchi]

Thank you for robot demo and your explanation.
I think this PR can be merged.

[k-okada]

failing due to missing packages https://travis-ci.org/jsk-ros-pkg/jsk_robot/jobs/602916381

  <system-out><![CDATA[[
[/home/travis/ros/ws_jsk_robot/src/jsk_robot/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_gazebo_bringup.launch]:
	cannot find package [robot_localization] for node [ukf_localization_node]
unable to find node [robot_localization/ukf_localization_node]: robot_localization

[knorth55]

@k-okada thank you! I updated.