Giter Club home page Giter Club logo

Comments (10)

ishiguroJSK avatar ishiguroJSK commented on August 25, 2024

僕はあまり使わない機能なので責任が持てませんが，
確かに

rtmlaunch hrpsys_ros_bridge_tutorials samplerobot.launch
rosservice call /SequencePlayerServiceROSBridge/setTargetPose "rarm" [0,0,0] [0,0,0] 5.0
rosservice call /SequencePlayerServiceROSBridge/setTargetPose "rarm" [0,0,0] [0,0,0] 5.0
rosservice call /SequencePlayerServiceROSBridge/setTargetPose "rarm" [0,0,0] [0,0,0] 5.0

とかやると二回目以降，制御がすぐ帰ってきて挙動がおかしいので，普通にバグだと思います．

[ INFO] [1551177549.922333317, 12.456000000]: SequencePlayerServiceROSBridge::setTargetPose() called
[setTargetPose] Solveing IK with frame[ INFO] [1551177549.923991040, 12.458000000]: SequencePlayerServiceROSBridge::setTargetPose() succeeded
[ INFO] [1551177550.407137713, 12.940000000]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 501[Hz] (exec 1760 Hz, dropped 0)
[ INFO] [1551177551.409470944, 13.942000000]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 501[Hz] (exec 1769 Hz, dropped 0)
[ INFO] [1551177551.719477545, 14.252000000]: SequencePlayerServiceROSBridge::setTargetPose() called
[ INFO] [1551177551.720400080, 14.254000000]: SequencePlayerServiceROSBridge::setTargetPose() succeeded
[rtmlaunch] check connection/activation
[ INFO] [1551177552.410021462, 14.944000000]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 501[Hz] (exec 1769 Hz, dropped 0)
[INFO] [WallTime: 1551177552.491191] [15.028000] check 6 collision status, 1.193792 Hz
[ INFO] [1551177552.692453385, 15.226000000]: SequencePlayerServiceROSBridge::setTargetPose() called
[ INFO] [1551177552.693276459, 15.228000000]: SequencePlayerServiceROSBridge::setTargetPose() succeeded

確かにSequencePlayerService_impl.cppの

CORBA::Boolean SequencePlayerService_impl::setTargetPose(const char* gname, const dSequence& xyz, const dSequence& rpy, CORBA::Double tm){
    char* frame_name = (char *)strrchr(gname, ':');
    if ( frame_name ) {
        ((char *)gname)[frame_name - gname] = '\0'; // cut frame_name, gname[strpos(':')] = 0x00
        frame_name++; // skip ":"
    }
    return m_player->setTargetPose(gname, xyz.get_buffer(), rpy.get_buffer(), tm, frame_name);
}

を見るに，"rarm:WAIST"とかではなく"rarm"とだけやるとconst char* frame_nameにNULLが入って再現するので，提案されている修正でPR送っていい気がします．

(本質的にはポインタの多用という問題がある気もしますが・・・)

from hrpsys-base.

k-okada avatar k-okada commented on August 25, 2024

ちゃんと記憶していないので間違ったおとを言っているかもしれないですが，ですが，frame_name == NULL というのはそもそも想定していないので，表示の部分の変更ではなくて，その前でエラーとして返すか，デフォルト値(base_parent_name?)をセットしてあげるのが良いのではないでしょうか？

from hrpsys-base.

hsnuhayato avatar hsnuhayato commented on August 25, 2024

この辺読んだら、
frame_nameが与えた時に入力のtarget poseがこのframe_nameから見たposeで、
frame_nameがNULLの時に入力のtarget poseが絶対座標系からみたposeになります。

hrpsys-base/rtc/SequencePlayer/SequencePlayer.cpp

Lines 627 to 637 in 37cab04

// change start and end must be relative to the frame_name
if ( (frame_name != NULL) && (! m_robot->link(frame_name) ) ) {
std::cerr << "[setTargetPose] Could not find frame_name " << frame_name << std::endl;
return false;
} else if ( frame_name != NULL ) {
hrp::Vector3 frame_p(m_robot->link(frame_name)->p);
hrp::Matrix33 frame_R(m_robot->link(frame_name)->attitude());
// fix start/end references from root to frame;
end_p = frame_R * end_p + frame_p;
end_R = frame_R * end_R;
}

例えばnextageにおいて、WAIST座標系=絶対座標系の場合だと、

$rosservice call /SequencePlayerServiceROSBridge/setTargetPose "rarm:WAIST" [x,y,z] [r,p,y] 1

と

$rosservice call /SequencePlayerServiceROSBridge/setTargetPose "rarm" [x,y,z] [r,p,y] 1

が同じ場所に移動しますので、frame_nameはオプションで与えなくていいと思います。

デフォルト値(base_parent_name?)をセットしてあげるのが良いのではないでしょうか？

nextageみたいなbaseが固定しているロボットだとデフォルトでframe_name = m_robot->rootLink()->nameでいいと思いますが、移動ロボットの場合だとbaseからみたposeと絶対座標系からみたposeが違う可能性があるのではないでしょうか。
設計者の意図を読み間違うかもしれないが、よろしくお願いします。

from hrpsys-base.

k-okada avatar k-okada commented on August 25, 2024

from hrpsys-base.

hsnuhayato avatar hsnuhayato commented on August 25, 2024

テスト追加してみました。
#1267
ですが、ubuntuの環境だと自分の修正を入れなくてもこの追加テストは通ります。
修正前だとターミナルのcerr表示が途中で途切れるが、これをテストケースでチェックする方法ご存じでしたら教えて頂けませんでしょうか。
よろしくお願いします。

from hrpsys-base.

k-okada avatar k-okada commented on August 25, 2024

@ishiguroJSK

とかやると二回目以降，制御がすぐ帰ってきて挙動がおかしいので，普通にバグだと思います．

とあるので、このおかしい挙動を捕まえられるとよいと思ったのですが。。。

from hrpsys-base.

ishiguroJSK avatar ishiguroJSK commented on August 25, 2024

すいません，ちゃんとIK解ける姿勢で確認すると，端末には(seqのcerr出力が？)出なくなるが，seqは動作し続けているようです．シミュレータ上でも動きます．

rosservice call /SequencePlayerServiceROSBridge/setTargetPose "larm" [0.1,0.2,1] [-0.0,-1.29,-0.59] 5.0
operation_return: True
rosservice call /SequencePlayerServiceROSBridge/setTargetPose "larm" [0.2,0.2,1] [-0.0,-1.29,-0.59] 5.0
operation_return: True
rosservice call /SequencePlayerServiceROSBridge/setTargetPose "larm" [0.1,0.2,1] [-0.0,-1.29,-0.59] 5.0
operation_return: True

この状態を発見しようとすると，端末出力を監視しなければならなくなりそうですが・・・

from hrpsys-base.

ishiguroJSK avatar ishiguroJSK commented on August 25, 2024

std::coutにNULLを送るとコアダンプしたり表示が出なくなるのは既知らしく，
https://stackoverflow.com/questions/7019454/why-does-stdcout-output-disappear-completely-after-null-is-sent-to-it
コアダンプしない場合にもstd::cout.good()とかでエラーが立っているか検知は出来るみたいですが，
Pythonのテストスクリプトに実装(sys.stdout.errorsが該当？)してもプロセスやスレッドが違うと意味なさそうですし，
cppのコード内に埋め込むにも，随所にif(std::cout.good())を書くハメになるので不適な気がします．

  const char* s = NULL;
  std::cerr<<"[cerr] cout is alive?="<<std::cout.good()<<std::endl;
  std::cout<<"[cout] s="<<s<<std::endl;
  std::cout<<"[cout] after cout gets NULL"<<std::endl;
  std::cerr<<"[cerr] cout is alive?="<<std::cout.good()<<std::endl;

[cerr] cout is alive?=1
[cout] s=[cerr] cout is alive?=0

ちなみに上記リンクでの結論もstd::string使うか，std::coutに送る前にチェックするか，のようなのでテストで検出は無理なのではないでしょうか？

from hrpsys-base.

hsnuhayato avatar hsnuhayato commented on August 25, 2024

ちなみに上記リンクでの結論もstd::string使うか，std::coutに送る前にチェックするか，のようなのでテストで検出は無理なのではないでしょうか？

そうですね、調べてわかったこと:

  1. streaming に null pointer渡すこと自体がundefined behaviorで、その後なにが起こるかはコンパイラ次第。
    たまたまQCCがSIGSEGVをraiseしたらしい。
  2. pythonから別プロセスのostreamの参照を取得することが無理っぽいです。そもそもpythonはCで実装され、c++のstream にアクセスする手段がない。pythonの sys.stdoutも標準出力をファイルディスクリプタで扱うラッパーで、c++とは違う実装。

cppのコード内に埋め込むにも，随所にif(std::cout.good())を書くハメになるので不適な気がします．

同意見です。
シンプルにNULLをstreamに渡すのを避けましょう。というのが良いと思いますが...

from hrpsys-base.

k-okada avatar k-okada commented on August 25, 2024

@ishiguroJSK @hsnuhayato 丁寧な調査ありがとうございます．#1267 でOKだとおもいます．

from hrpsys-base.

Related Issues (20)

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.