Add the publisher about "true" pose of heliport for the evaluation of…

[tongtybj]

[jsk_mbzirc_common/include/jsk_mbzirc_common/mbzirc_gazebo_truck_plugin.h & jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp] Add Feature: the publisher about the "true" pose of heliport, which is useful for the evaluation of the performance of motion tracking related to uav in task1

[k-okada]

You can use tf for this purpose

◉ Kei Okada

2016/06/02 15:14、趙　漠居 notifications@github.com のメッセージ:

[jskmbzirc${files changed}] Add Feature: the publisher about the "true" pose of heliport, which is useful for the evaluation of the performance of motion tracking related to uav in task1

Description

[jsk_mbzirc_XXXXXX/YYYYYY.ZZZ] fix: XXXXXX description... [jsk_mbzirc_XXXXXX/YYYYYY.ZZZ] Add feature XXXXXX description... [jsk_mbzirc_XXXXXX/YYYYYY.ZZZ] Add test codes for feature XXXXXX Follow code style … motion tracking performance in the task1.

You can view, comment on, or merge this pull request online at:

https://github.com/start-jsk/jsk_mbzirc/pull/77

Commit Summary

Add the publisher about "true" pose of heliport for the evaluation of motion tracking performance in the task1. File Changes

M jsk_mbzirc_common/include/jsk_mbzirc_common/mbzirc_gazebo_truck_plugin.h (3) M jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp (14) Patch Links:

https://github.com/start-jsk/jsk_mbzirc/pull/77.patch https://github.com/start-jsk/jsk_mbzirc/pull/77.diff — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub, or mute the thread.

[tongtybj]

@k-okada I change to tf. Please use following command to check. roslaunch jsk_mbzirc_tasks jsk_mbzirc_task_1.launch use_ground_truth:=true

[tongtybj]

@furushchev I can not figure out the reason why travis has failed, and can not restart the build...Please help....

[furushchev]

@tongtybj I think build failed because some codes you changed does not follow coding style.

<?xml version="1.0" encoding="UTF-8"?>
<testsuites tests="1" failures="1" errors="0" name="Linters">
  <testsuite name="Linters" tests="1" failures="1">
    <testcase name="roslint" classname="Linter">
      <failure message="One or more linter errors was reported." type=""><![CDATA[make[4]: warning: jobserver unavailable: using -j1.  Add `+' to parent make rule.
Scanning dependencies of target roslint_jsk_mbzirc_common
Done processing /home/travis/catkin_ws/src/jsk_mbzirc/jsk_mbzirc_common/src/mbzirc_gazebo_treasure_plugin.cpp
/home/travis/catkin_ws/src/jsk_mbzirc/jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp:188:  Lines should be <= 120 characters long  [whitespace/line_length] [2]
/home/travis/catkin_ws/src/jsk_mbzirc/jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp:189:  Missing space after ,  [whitespace/comma] [3]
Done processing /home/travis/catkin_ws/src/jsk_mbzirc/jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp
Done processing /home/travis/catkin_ws/src/jsk_mbzirc/jsk_mbzirc_common/src/mbzirc_gazebo_panel_plugin.cpp
Done processing /home/travis/catkin_ws/src/jsk_mbzirc/jsk_mbzirc_common/include/jsk_mbzirc_common/mbzirc_gazebo_treasure_plugin.h
Done processing /home/travis/catkin_ws/src/jsk_mbzirc/jsk_mbzirc_common/include/jsk_mbzirc_common/mbzirc_gazebo_truck_plugin.h
Done processing /home/travis/catkin_ws/src/jsk_mbzirc/jsk_mbzirc_common/include/jsk_mbzirc_common/mbzirc_gazebo_panel_plugin.h
Total errors found: 2

[k-okada]

@tongtybj you do not have write C code to publish tf, just to add heliport frames in urdf

[tongtybj]

@k-okada After adding joint info into urdf, we can have the tf between /truck and /heliport. But how about the tf between /world and /truck, since we need know /world to /heliport. I suppose we have to write the C code in the truck plugin, which is almost same with what I wrote, broadcasting tf from /world to /truck.

[k-okada]

You can use p3d plugin to publish exact location of the robot Or how about update GPU plugin to publish tf data ( check if there is an option to do this) or write program that takes GPU and altitude information and publish tf data ( check is some one already write this) Or you may use ekf_robot_pose or others to subscribe sensor information and publish estimated drone pose

2016年6月3日金曜日、趙 漠居notifications@github.comさんは書きました:

@k-okada https://github.com/k-okada After adding joint info into urdf, we can have the tf between /truck and /heliport. But how about the tf between /world and /truck, since we need know /world to /heliport. I suppose we have to write the C code in the truck plugin, which is almost same with what I wrote, broadcasting tf from /world to /truck.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/start-jsk/jsk_mbzirc/pull/77#issuecomment-223490329, or mute the thread https://github.com/notifications/unsubscribe/AAeG3Cj6a42UtvwrSzHDRdmf9wfYQkUFks5qH7MXgaJpZM4IsPE_ .

◉ Kei Okada

[tongtybj]

@k-okada Thanks for the advice.

日本語ですみません。 おっしゃることの大半はUAVの位置推定に関係するものと理解していますが、 そもそもこのPRはトラック(ヘリポート)の位置の真値を使って、 ドローンの移動追跡の追跡制御を評価することがモチベーションになっています。

追跡制御のフィードバックにはまずヘリポートの真値を使って、 制御則(PID制御かP制御か、速度制御か加速度制御か)とパラメータ等 をある程度ちゃんと決まってから、 今度はビジョンなりレーザなりのセンサデータをフィードバックにつかう という段取りを考えています。

[tongtybj]

@k-okada プラス、 トラック(車)の位置推定の精度評価にも、真値を使って比較する必要があるかと。 ROSで真値が知りたいだけなので、tfでもただのposeでもなんでもいいです。

[k-okada]

真値の時は，p3dプラグインでのトラックの真値をだして，そこからヘリポートまではURDFから出てくるTFを使いましょう． http://gazebosim.org/tutorials?tut=ros_gzplugins#P3D(3DPositionInterfaceforGroundTruth)

◉ Kei Okada

2016-06-03 23:10 GMT+09:00 趙 漠居 notifications@github.com:

@k-okada https://github.com/k-okada プラス、 トラック(車)の位置推定の精度評価にも、真値を使って比較する必要があるかと。 ROSで真値が知りたいだけなので、tfでもただのposeでもなんでもいいです。

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/start-jsk/jsk_mbzirc/pull/77#issuecomment-223589087, or mute the thread https://github.com/notifications/unsubscribe/AAeG3HsOhssZULbUs_fdCMFK2eSO8naSks5qIDXigaJpZM4IsPE_ .

[tongtybj]

@k-okada 2点確認です。

1. p3dで出せるのは/nav_msgs/odometryだけですよね、tfはbroadcastされてません。ということは、/wordと/truck間はnav_msgs/odomを使用、それかodom情報からtfを作る(手間がひとつ増えてる？). それに加え、/truckと/heliport間のtfがあれば、/worldと/heliport間treeが完成という理解でしょうか。
2. 現在のtruckはSDF形式が記述されてますが、TFを出すためにわざわざURDFに直す必要はあるのでしょうか。

[k-okada]

2016年6月6日月曜日、趙 漠居notifications@github.comさんは書きました:

@k-okada https://github.com/k-okada 2点確認です。 1. p3dで出せるのは/nav_msgs/odometryだけですよね、tfはbroadcastされてません。ということは、/wordと/truck間はnav_msgs/odomを使用、それかodom情報からtfを作る(手間がひとつ増えてる？). それに加え、/truckと/heliport間のtfがあれば、/worldと/heliport間treeが完成という理解でしょうか。

そうですね。それでいいとおもいます。odom ->tfはrobot_pose_ekfでも使えばいいんじゃないでしょうか。

1. 現在のtruckはSDF形式が記述されてますが、TFを出すためにわざわざURDFに直す必要はあるのでしょうか。

そうですね。わざわざ、というほどの作業じゃないとおもいます。

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/start-jsk/jsk_mbzirc/pull/77#issuecomment-223947909, or mute the thread https://github.com/notifications/unsubscribe/AAeG3EyY-n-_n04CF-8gdsgxAVTVv9DCks5qJBYZgaJpZM4IsPE_ .

◉ Kei Okada

[tongtybj]

@k-okada では、このPRの結論について、 1) heliportの真値をPoseStampedで直接publishする(#6db9a54) 2) heliportの真値をTFで直接broadcastする(#cf2ac22) 3) 無しにする の中から一つ選んでもらえませんか。

個人的には真値があったほうがいいかと。。。

[k-okada]

んp3d -> ekf + urdf -> tf の一択なんですが

2016年6月6日月曜日、趙 漠居notifications@github.comさんは書きました:

@k-okada https://github.com/k-okada では、このPRの結論について、 1) heliportの真値をPoseStampedで直接publishする(#6db9a54) 2) heliportの真値をTFで直接broadcastする(#cf2ac22) 3) 無しにする の中から一つ選んでもらえませんか。

個人的には真値があったほうがいいかと。。。

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/start-jsk/jsk_mbzirc/pull/77#issuecomment-223962067, or mute the thread https://github.com/notifications/unsubscribe/AAeG3A_mZ-LK9tPoTfUqENE5rJyWNSAFks5qJCOYgaJpZM4IsPE_ .

◉ Kei Okada

[tongtybj]

@k-okada やってみました。 いくつか問題が残っていますが、 roslaunch jsk_mbzirc_common mbzirc_arena_1.launch を実行して、rviz上でtruckモデルの移動が確認できるとおもいます。

問題: 1) robot_pose_ekfがcovariance=0を認めない件 p3dはデフォだとcovarianceの要素は全部0になりますが、それだと https://github.com/ros-planning/navigation/blob/jade-devel/robot_pose_ekf/src/odom_estimation.cpp#L337 に引っかかってしまう。 とりあえず、p3dのgussianNoiseに非常に小さい値(0.00001)を入れて対処していますが、 https://github.com/ros-simulation/gazebo_ros_pkgs/blob/jade-devel/gazebo_plugins/src/gazebo_ros_p3d.cpp#L281-293 のように、ノイズが乗ってしまいます。

2) トラックが地面にめり込む件 URDFにしたら、トラックが何故か地面にめり込む時があります。intertiaで重量を設定して、重力が働くようになったからでしょうか。https://github.com/tongtybj/jsk_mbzirc/blob/78eaa72ff80b607e0106453c411ccd096faa5fcc/jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp#L164 でちゃんと設定しているのに、なぜz方向だけずれるのでしょうか。

3) rayShapeが自身の他のlinkによってcollision判定が起こる heliportがbase_linkの子リンクになってため、 https://github.com/tongtybj/jsk_mbzirc/blob/78eaa72ff80b607e0106453c411ccd096faa5fcc/jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp#L175-176 のようにしていますが、 https://github.com/tongtybj/jsk_mbzirc/blob/78eaa72ff80b607e0106453c411ccd096faa5fcc/jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp#L190 のようにしないと、自身と他のrayshapeにあたるようになります。

[k-okada]

お，偉い偉い． １）covariance = 0 でエラーにならないようにパッチをつくってPRを送るか，以下の物を試しましょう https://github.com/cra-ros-pkg/robot_localization （ http://wiki.ros.org/robot_localization/Tutorials , clearpath's recommendation http://answers.ros.org/question/39790/robot_pose_ekf-and-gps/ ） https://github.com/ros-perception/graft/tree/hydro-devel/src (fetchがつかっている） ２）inertia はちゃんと設定している？．サイズに対してちゃんとしたinertiaにしていないと良くない． https://github.com/ros-simulation/gazebo_ros_demos/blob/master/rrbot_description/urdf/rrbot.xacro#L48-L51 みたいに間違いが無いように書くのが重要（これは円柱？） あとは http://sdformat.org/spec?ver=1.6&elem=world#include_static で static false にすればいいんじゃないか．

[tongtybj]

@k-okada

１）covariance = 0 でエラーにならないようにパッチをつくってPRを送るか，以下の物を試しましょう https://github.com/cra-ros-pkg/robot_localization （ http://wiki.ros.org/robot_localization/Tutorials , clearpath's recommendation http://answers.ros.org/question/39790/robot_pose_ekf-and-gps/ ） https://github.com/ros-perception/graft/tree/hydro-devel/src (fetchがつかっている）

robot_localizationでできましたが、よくよく考えたら、robot_pose_ekfもrobot_localizationもsensor fusionが目的で、ここでground_truth用にカスマイズするのはちょっとカッコ悪いと自分は思っていて、 そもそもsimulation上のground_truthをtfにしたいという基本的な要望はとっくに対応されてもおかしくないと思います。で、探してみたらドンピシャのものがやはりありました。http://wiki.ros.org/fake_localization。 最新コミットはこちらを使っていますが、どうでしょうか。 https://github.com/tongtybj/jsk_mbzirc/blob/98eff80991cd380521c36fe8139b789b78bc6151/jsk_mbzirc_common/launch/mbzirc_arena_1.launch#L23-L28

２）inertia はちゃんと設定している？．サイズに対してちゃんとしたinertiaにしていないと良くない． https://github.com/ros-simulation/gazebo_ros_demos/blob/master/rrbot_description/urdf/rrbot.xacro#L48-L51 みたいに間違いが無いように書くのが重要（これは円柱？） あとは http://sdformat.org/spec?ver=1.6&elem=world#include_static で static false にすればいいんじゃないか．

こちらはいろいろ試してみて、わかったことは、https://github.com/tongtybj/jsk_mbzirc/blob/98eff80991cd380521c36fe8139b789b78bc6151/jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp#L164 をコメントアウトすれば、めり込み現象はなくなります。 また、最新コミットのようにすべてのmodelの<\gravity>を0にすれば、コメントアウトしなくてもめり込みません。 自分が出した結論は重力があって、なおかつWorldPoseを無理やりいじると、物理演算が狂ってしまう(?). 確か旧SDFファイルではmassとinertiaは記述されてませんでした。

[k-okada]

2016-06-08 7:56 GMT+09:00 趙 漠居 notifications@github.com:

robot_localizationでできましたが、よくよく考えたら、robot_pose_ekfもrobot_localizationもsensor fusionが目的で、ここでground_truth用にカスマイズするのはちょっとカッコ悪いと自分は思っていて、

そもそもsimulation上のground_truthをtfにしたいという基本的な要望はとっくに対応されてもおかしくないと思います。で、探してみたらドンピシャのものがやはりありました。 http://wiki.ros.org/fake_localization。 最新コミットはこちらを使っていますが、どうでしょうか。 https://github.com/tongtybj/jsk_mbzirc /blob/98eff80991cd380521c36fe8139b789b78bc6151/jsk_mbzirc_common/launch/mbzirc_arena_1.launch#L23-L28

あ，はい，考え方，それが正解です． 実機で使うときはGPSもふくめてsensor fusionするんだと思うので，速くrobot_localizationを使う段階になるといいですね．

２）inertia はちゃんと設定している？．サイズに対してちゃんとしたinertiaにしていないと良くない．

https://github.com/ros-simulation/gazebo_ros_demos/blob/master/rrbot_description/urdf/rrbot.xacro#L48-L51 みたいに間違いが無いように書くのが重要（これは円柱？） あとは http://sdformat.org/spec?ver=1.6&elem=world#include_static で static false にすればいいんじゃないか．

こちらはいろいろ試してみて、わかったことは、https://github.com/tongtybj/jsk_mbzirc /blob/98eff80991cd380521c36fe8139b789b78bc6151/jsk_mbzirc_common/src/mbzirc_gazebo_truck_plugin.cpp#L164 をコメントアウトすれば、めり込み現象はなくなります。 また、最新コミットのようにすべてのmodelの<\gravity>を0にすれば、コメントアウトしなくてもめり込みません。 自分が出した結論は重力があって、なおかつWorldPoseを無理やりいじると、物理演算が狂ってしまう(?). 確か旧SDFファイルではmassとinertiaは記述されてませんでした。

うごけばこれでいいです．static false ではなくて，static trueでした．これで同じ効果になると思うんだけど．

◉ Kei Okada

[tongtybj]

When I click the restart bottom in travis, there is always the same message: "An error occurred. The build could not be restarted". And the reason of fault is that size of log is bigger than 4MB.

[k-okada]

https://s3.amazonaws.com/archive.travis-ci.org/jobs/136584384/log.txt https://travis-ci.org/start-jsk/jsk_mbzirc/jobs/136584384

I do not think 4MB limit is the problem, just fail to pass the test. and that triggersafter_failure: section , which display huge logs, https://github.com/start-jsk/jsk_mbzirc/blob/master/.travis.yml#L61

◉ Kei Okada

On Fri, Jun 10, 2016 at 1:01 AM, 趙 漠居 notifications@github.com wrote:

When I click the restart bottom in travis, there is always the same message: "An error occurred. The build could not be restarted". And the reason of fault is that size of log is bigger than 4MB.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/start-jsk/jsk_mbzirc/pull/77#issuecomment-224942331, or mute the thread https://github.com/notifications/unsubscribe/AAeG3JjUSbHcPsnvNJiFXaf6vERQheSRks5qKDj3gaJpZM4IsPE_ .

[tongtybj]

I see... It seems like the task1 test still has problem with my commit. I will figure out the reason,

[tongtybj]

Found the bug is the prefix of topic name in truck plugin which avoid the "topic_test" to receive correct topic，since I add group ns "truck" for all node related to truck in order to distinguish from uav.

Right now add "/" solved the topic name problem and code is passed in Travis finally.

@k-okada If it is ok,please merge