Menu
▾
▴
#160 move-toでゴールに到達していなくてもsuccessが返ってくる
jsk_2013_04_pr2_610デモ(rospack find jsk_2013_04_pr2_610/euslisp/setup.l) において、
move-toでPR2を動かすときにeng8-610の入口付近からlaundry-spotへ動かそうとすると、
目標位置に到達していないにも関わらずSuccessと出てmove-toが完了してしまいます。
一方、別の位置（例えばbroom-spot）へ一度move-toしてからlaundry-spotへ動かすと正常に動きます。
サンプルのコマンドは
(send *ri* :move-to (make-coords :pos #f(3376.358 1309.163 -0.854) :rpy #f(1.544 0.004 0.002)) :frame-id "/eng8/6f/610")
になります．
rostopic echo /move_base/result
をすると
---
header:
seq: 97
stamp:
secs: 1369911817
nsecs: 426065503
frame_id: ''
status:
goal_id:
stamp:
secs: 1369911817
nsecs: 325378450
id: #i(1369911817 312803158)
status: 3
text: Goal reached.
result:
---
となるのでmove_baseがおかしいかもしれません。
send ri :move-to でゴールにたどり着かなければnilがかえるようにしました．[r4460]
Related
Commit: [r4460]
~/ros/groovy/navigation/base_local_planner/latched_stop_rotate_controller.cpp にデバッグプリントを追加したノードをおきました．
export ROS_PACKAGE_PATH=/home/k-okada/ros/groovy/navigation/base_local_planner/:$ROS_PACKAGE_PATH
としてから立ち上げると，使えるようになると思います．ROS_DEBUGなのでmove_base_nodeのデバッグレベルを
上げる必要があります．
上手く行かない時は
となってglobal_poseが更新されていませんね．これはisGoalReachedの第三引数ですがそれがどこからやってきているかが知りたいところ．ｃ２が重すぎてｔｆがとれていない，とかでしょうか？
まだ直ってないようで、念の為出力だけ貼らせていただきます。
move-to-chair2
[ INFO] [1370337764.177810841]: move-to : send-goal to #f(3900.0 50.0 0.0) (1)
<move_base_msgs::movebaseactiongoal #X8bd6050="">
"wiat for result"
[ INFO] [1370337767.630888319]: move-to : succeeded
[ WARN] [1370337767.705487066]: :move-to wait-for transform /map to /base_footprint -> t
[ WARN] [1370337767.705836182]: :move-to current-coords #<cascaded-coords #X56382c0="" map="" 2325.87="" -13553.222="" -2.038="" -0.128="" -0.004="" -0.001="">
[ WARN] [1370337767.705979276]: mapgoal-coords #<cascaded-coords #X5639520="" map="" 3900.0="" -13628.0="" 0.0="" 0.0="" 0.0="" 0.0="">
[ WARN] [1370337767.706107779]: coords #<cascaded-coords #Xaa4b0b8="" 3900.0="" 50.0="" 0.0="" 0.0="" 0.0="" 0.0="">
[ERROR] [1370337767.706188685]: too far from goal position (> 200mm)
<topic_tools::muxselectresponse #X96bd008="">
と表示されます。
どうすればよいでしょうか
[#165]と同じ問題のようです。pr2_2dnavを立ち上げただけだと、ちゃんと動いているように思います。
Related
Tickets:
#165roslaunch jsk_pr2_startup pr2.launch だけをした状態でもガクガクするでしょうか？
pr1040ではガクガクしないので、jsk_pr2_startup pr2.launchした状態でload averageを比べてみるというのがあるかもしれません。
こんな感じです。有線接続、rviz無し。です。
image_view2が重いのは余計だよね、と思って、
<arg name="launch_tablet" default="false"/>
とすると、よくなった気がします。
一番重いnodeletは以下のものでした。
/opt/ros/groovy/lib/nodelet/nodelet manager name:=openni_nodelet_manager log:=/home/applications/.ros/log/f33f530a-ce5d-11e2-87f5-001517c3b779/openni_nodelet_manager-31.log
pr1012のpr2.launchは
としてあるので、使うときは注意してください。
pr1040だとこんな感じです
pr1012より軽いですね
diffはjsk_pr2_sensors/kinect_head.launchで
check_openni_node.pyをコメントアウトしているぐらいです
実はCPUが違ったりするのかな？
~~~~~~
$ cat /proc/cpuinfo
processor : 7
vendor_id : GenuineIntel
cpu family : 6
model : 26
model name : Intel(R) Xeon(R) CPU L5520 @ 2.27GHz
stepping : 5
microcode : 0x11
cpu MHz : 2266.708
cache size : 8192 KB
physical id : 0
siblings : 4
core id : 3
cpu cores : 4
apicid : 6
initial apicid : 6
fpu : yes
fpu_exception : yes
cpuid level : 11
wp : yes
flags : fpu vme de pse tsc msr pae mce cx8 apic sep mtrr pge mca cmov pat pse36 clflush dts acpi mmx fxsr sse sse2 ss ht tm pbe syscall nx rdtscp lm constant_tsc arch_perfmon pebs bts rep_good nopl xtopology nonstop_tsc aperfmperf pni dtes64 monitor ds_cpl vmx est tm2 ssse3 cx16 xtpr pdcm dca sse4_1 sse4_2 popcnt lahf_lm ida dtherm tpr_shadow vnmi flexpriority ept vpid
bogomips : 4532.16
clflush size : 64
cache_alignment : 64
address sizes : 40 bits physical, 48 bits virtual
power management:
~~~~~~~
新しいPR2はこっそりCPUがバージョンアップしている，なんていうサプライズはなかった見たいですね．
https://sourceforge.net/p/jsk-ros-pkg/tickets/160/#8fba
から調査の続きになりますが、
rvizのGUIから指示する（/move_base_simple/goalを利用する）だと動いている、というところから
調べてみると、rvizからの指示のframe_idはmap相対ですが, move-toを使うときは/eng8/6f/610相対
になり、さらに、map-> /eng8/6f/610のtfは10hzでしか出していないということが分かりました。
確かに(send tfl :lookup-transform "/map" "/eng8/6f/610" (ros::time-now))とすると、2回に一回ぐらいしか値がとれません。
ということで、対応策としては move-toするときにmap相対で出力する、あるいは、tfの周期をあげる
というのがありそうです。添付は前者のパッチになります。前者の弱点は移動するframe相対に位置を
指定したい場合でしょうか？（人についていくなど）
一方で、/eng8/6f -> /eng8/6f/610等はstatic tf publisherを使って10hz出していますが、これは
30hzにすると、:lookup-transformは毎回値がとれるようになりますが、やはり:move-toでは
動かずにsuscessがかえることがありますね。
ということで、前者で対応しました.[r4745]
ただ、これでもまだ時々症状は出るみたいです。失敗した後のmove_base_simpleを送るコードも[r4746]で入れましたがうまくないですねぇ。今のamclの値が同じなので、効果が無いかも。
move-toでfailしたら、一度４５度ぐらい向きをかえて、もう一度やる、みたいなコードをpr2-interfaceの外側で書いた方が良さそうです。
デモが5回ぐらいは連続で成功するようにするのがいいと思います。
Related
Commit: [r4745]
Commit: [r4746]
staticなtfを読むなら、time-now ではなくて time 0 を使うという作戦は有りでしょうか？
(send *tfl* :lookup-transform "/map" "/eng8/6f/610" (ros::time 0))
time-now だと、lookupのタイムアウトまでにnowより新しいtfがsubscribeされる必要がある.
time 0 だと、最も時間が新しいバッファにある値を返してくれる
なので、少々tfの間隔が開いても何かしら返ってくるはずです。
eus側はそれでいいんですが，
https://sourceforge.net/p/jsk-ros-pkg/tickets/160/#8fba
でみた感じだとおそらくmove_base側で取り切れていないんだと思っています．
ところで，look-upで時刻をしていしたらどれだけのtolerance があるのかな．
http://www.ros.org/wiki/tf/Tutorials/tf%20and%20Time%20(Python)
などを斜め読みすると0.001ぐらいなのかな．とするとtfは1000hzで動かさなければいけなくなる．
いまはそれぞれのstatic tfが勝手にtimestampをつけてtfを出しているように見えて，
うまくつながっていない気がするんだけど，statick_tf で，1hzにしたら，ちゃんと
msecは毎回０になるようにする（どうせstaticだから）というふうにするべきなのかあ．
そう言えばほとんど無いようですね。
ちゃんと lookup の前に wait-for-transform してwaitする必要があるようですね。
Last edit: Yohei Kakiuchi 2013-07-04
結局この問題は/mapと/eng8/6f/610間のtfがとれないから起こる、ということになったのでしょうか？
現在73B2でも起きていて、こちらではmove-toを:world相対にしているので上記の原因とは別と考えられます。
Too Far From Goalとなった時（MoveBaseActionが目的地に未到達のままSucceedになる時）に/move_base/goalと/move_base/feedbackをrostopic echoしたままにしていると、
これらの値は常に正しく出ているようなのでtfの取得時間の問題ではないように思います。
ただ、現象としては、
1. あるスポットAにmove-toで移動したあと、ジョイスティックでどこかへ移動して、もう一度スポットAに行くと起こる時がある。
2, 起きたときは別のスポットBへmove-toしたあとにスポットAにmove-toすると行ける。
という感じなのでmove_baseが最新のtfを参照していないくらいしか原因が思いつかないです。
この問題が確実に発生する、パターンはあるでしょうか？
また、問題が起こったときに以下のようにして(ros.roseusの部分はmove_baseに変更)して
DEBUGレベルのメッセージを見えるようにした、rxconsoleの出力はどうなっているでしょうか？
おおよそのところわかりました。
プランを実行する順番が、
１．ゴール判定、２．cmd_velの計算、３．cmd_velを送る
というようになっていて、ロボットの現在位置を２．で計算（tfを聞く）するという仕様でした。
ゴール判定は現在プランのゴール位置と一つ前のゴール近くの位置で判定することになり、
プランナのみで動かしていると問題ないのですが、プランナの２．を経由せずにロボットを動かしても
ゴール判定に使われるロボット位置がアップデートされないことになっていました。
古田くんのバグ再現の方法が当たっていたことになります。
ある地点へmove-toして、そのあとunsafeで動かして、もとのある地点へ戻るmove-toは
ロボットの位置がアップデートされないので、最初のmove-toのゴール判定と同じになりsuccessが
返ることになっていました。
hydroでは修正されているようです。（修正は２ヶ月前で、修正の理由はJSKとは違うように見えます）
ということで、現在はpr1040は体内のnavigationスタックをソースからインストールしてパッチを当てた状態にしています。
パッチはプルリクエストします。
参考：
hydroでは修正されていた。
ちゃんと読んでいないが、修正の直接原因はJSKでの問題とは違うように思える。
http://answers.ros.org/question/94536/testing-dwa-local-planner-approaches-goal-stops-keeps-swinging-left-right/#96786
https://github.com/ros-planning/navigation/issues/147
https://github.com/ros-planning/navigation/commit/97c2c7844cc00b489d3daa21b505fe7f080e6d5b
以下を直接修正した
index 20c84e2..051c68c 100644 --- a/dwa_local_planner/src/dwa_planner_ros.cpp +++ b/dwa_local_planner/src/dwa_planner_ros.cpp @@ -141,6 +141,11 @@ namespace dwa_local_planner { ROS_ERROR("This planner has not been initialized, please call initialize() before using thi s planner"); return false; } + if ( ! costmap_ros_->getRobotPose(current_pose_)) { + ROS_ERROR("Could not get robot pose"); + return false; + } + if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) { ROS_INFO("Goal reached"); return true;