[:self-collision-check] ignore links which do not have :faces

[708yamaguchi]

fetchで:self-collision-checkを行った時に、PQP Error! EndModel() called on model with no trianglesというエラーが出ました。
調べてみると、(send *fetch* :head_camera_rgb_frame_lk)などのように、:facesがnilのbodyset-linkクラスのインスタンスに対しては、:make-pqpmodelが出来ずにこのようなエラーメッセージが出るようです。

このプルリクエストでは、:facesがnilでないリンク同士だけをpqpの:self-collision-checkの判定に使うようにしました。

エラー例:

5.irteusgl$ (send *fetch* :self-collision-check)
PQP Error! EndModel() called on model with no triangles
PQP Error! EndModel() called on model with no triangles
PQP Error! EndModel() called on model with no triangles
PQP Error! EndModel() called on model with no triangles
PQP Error! EndModel() called on model with no triangles
((#<bodyset-link #X7498cc0 base_link  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X6d684b8 bellows_link2  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X7498cc0 base_link  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X6d5cd78 bellows_link  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X712b4e0 torso_fixed_link  -86.875 0.0 377.425 / 0.0 0.0 0.0> . #<bodyset-link #X6d5d5d0 torso_lift_link  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X712b4e0 torso_fixed_link  -86.875 0.0 377.425 / 0.0 0.0 0.0> . #<bodyset-link #X6d684b8 bellows_link2  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X712b4e0 torso_fixed_link  -86.875 0.0 377.425 / 0.0 0.0 0.0> . #<bodyset-link #X6d5cd78 bellows_link  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X6d684b8 bellows_link2  -86.875 0.0 397.43 / 0.0 0.0 0.0> . #<bodyset-link #X6d5cd78 bellows_link  -86.875 0.0 397.43 / 0.0 0.0 0.0>) (#<bodyset-link #X6dcab78 r_gripper_finger_link  35.009 -126.734 756.756 / 1.961 -1.492 -0.445> . #<bodyset-link #X6fa17c0 l_gripper_finger_link  65.797 -128.387 757.797 / 1.961 -1.492 -0.445>))

[Affonso-Gui]

Would be nice to indent the let block under the new when (shouldn't need to concern about increasing the changed lines of the PR)

[k-okada]

最低限 pqp-collision-check など、より上位の関数で対応しましょう。

[708yamaguchi]

レビューありがとうございます。

結局、:facesを持たないオブジェクトに対しては、:make-pqpmodelの中で(pqpbeginmodel)、(pqpaddtri)、(pqpendmodel)を行わないようにしました。

[Affonso-Gui]

Actually I think that the original fix (i.e. suppressing the warning message only on :self-colision-check, not in higher level functions) was correct.

That is because there is no point in having messages such optical-frame model has no triangles when checking for self-collision, but it would be nice to maintain these warning messages when e.g. checking for collisions between the optical frame and gripper-link.

It would also be a nice idea to change the text of the message, replacing PQP Error! for something like PQP Warning! and stating which link model does not have triangles.

[k-okada]

rewrite this PR at #571

• PQP Error! message comes from 3rdparty PQP library, so will not change this.
• display BT_MakeMeshModel with numVertices == 0 when :faces is nil
• skip calling collision-func when :faces is nil. Also print warning message for links with no faces. Not sure if this is necessary. For example fetch robot outpus following message for (:self-collision-check);

"fetch"
(make-collision)

;; extending gcstack 0x55bb03ebb880[32738] --> 0x55bb10b4d430[65476] top=7fe1

;; extending gcstack 0x55bb10b4d430[65466] --> 0x55bb10bcd270[130932] top=ffb9
"(fetch)"
skip collision between head_camera_depth_optical_frame (0) because of no faces
skip collision between head_camera_rgb_optical_frame (0) because of no faces
skip collision between head_camera_depth_frame (0) because of no faces
skip collision between head_camera_rgb_frame (0) because of no faces
skip collision between head_camera_link (0) because of no faces
"(send *fetch* :self-collision-check)"

[k-okada]

closed via #571