Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[jsk_fetch_startup] Add fetch gazebo demo using roseus #1213

Merged
merged 31 commits into from
Sep 25, 2021

Conversation

708yamaguchi
Copy link
Member

I add fetch gazebo demo using roseus.

The original repository is fetch_gazebo_demo.
In this repository, I replace rospy script with roseus script.

I think this repository is suitable for ROS and roseus beginners to understand these software without real robots.

@98shimpei
Copy link

98shimpei commented Apr 13, 2020

READMEにあるとおり、demo.launchを実行したのですが、ロボットが机の前まで来て、腰を伸ばしたあと、以下のようなエラーが出て、roseusのactionlib client でbasic_grasping_perception/find_objectsの認識結果を得ることができませんでした。以下にログを示します。

roslaunchのログ
すべてのログはこちらにあります。: https://drive.google.com/open?id=11gEjKRZcPMYptXI7P3n-38YjxsCuMQ_u

$ roslaunch jsk_fetch_gazebo_demo demo.launch 
... logging to /home/tork/.ros/log/8f458f24-7d9f-11ea-b088-c0b883ebd679/roslaunch-oem-ThinkPad-T490-25894.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt

...

[ INFO] [1586793813.098868347, 71.523000000]: Waiting for basic_grasping_perception/find_objects ...
[ INFO] [1586793813.366642666, 71.634000000]: Picking object...
Call Stack (max depth: 20):
  0: at (send #:inst314671 :init)
  1: at (let ((#:inst314671 (instantiate object))) (send #:inst314671 :init) #:inst314671)
  2: at (instance object :init)
  3: at (cons (instance object :init) ros::r)
  4: at (setq ros::r (cons (instance object :init) ros::r))
  5: at (setf ros::r (cons (instance object :init) ros::r))
  6: at (push (instance object :init) ros::r)
  7: at (while (< ros::i #:dotimes314670) (push (instance object :init) ros::r) (setq ros::i (1+ ros::i)))
  8: at (let ((ros::i 0) (#:dotimes314670 ros::n)) (declare (integer ros::i #:dotimes314670)) (while (< ros::i #:dotimes314670) (push (instance object :init) ros::r) (setq ros::i (1+ ros::i))) nil)
  9: at (dotimes (ros::i ros::n) (push (instance object :init) ros::r))
  10: at (let (ros::r) (dotimes (ros::i ros::n) (push (instance object :init) ros::r)) ros::r)
  11: at (setq ros::_support_surfaces (let (ros::r) (dotimes (ros::i ros::n) (push (instance object :init) ros::r)) ros::r))
  12: at (let (ros::n) (setq ros::n (system:peek ros::buf ros::ptr- :integer)) (incf ros::ptr- 4) (setq ros::_support_surfaces (let (ros::r) (dotimes (ros::i ros::n) (push (instance object :init) ros::r)) ros::r)) (dolist (ros::elem- ros::_support_surfaces) (send ros::elem- :deserialize ros::buf ros::ptr-) (incf ros::ptr- (send ros::elem- :serialization-length))))
  13: at (send ros::_result :deserialize ros::buf ros::ptr-)
  14: at (ros::spin-once)
  15: at (cond (ros::groupname (ros::spin-once ros::groupname)) (t (ros::spin-once)))
  16: at (send self :spin-once)
  17: at (while (ros::ok) (ros::ros-debug "[~A] :wait-for-result ~A ~A" ros::name-space (ros::simple-goal-states-to-string ros::simple-state) (send ros::comm-state :state)) (send self :spin-once) (if (= ros::simple-state ros::*simple-goal-state-done*) (return)) (if (> ros::timeout 0) (let* ((ros::tm (ros::time- (ros::time-now) ros::start))) (if (> (send ros::tm :to-sec) ros::timeout) (return-from :wait-for-result nil)))) (when ros::return-if-server-down (when (= (ros::get-num-publishers ros::result-topic) 0) (ros::ros-error "[~A] Unexpected returns from :wait-for-result : no publishers found for ~A" ros::name-space ros::result-topic) (return-from :wait-for-result nil)) (when (and ros::status-stamp ros::maximum-status-interval (> (send (ros::time- (ros::time-now) ros::status-stamp) :to-sec) ros::maximum-status-interval)) (ros::ros-error "[~A] Unexpected returns from :wait-for-result : status did not received for 5 seconds" ros::name-space) (return-from :wait-for-result nil))) (ros::sleep))
  18: at (let ((ros::start (ros::time-now)) (ros::result-topic (format nil "~A/result" ros::name-space))) (ros::ros-debug "[~A] :wait-for-result ~A, timeout=~A" ros::name-space (if ros::goal_id (send ros::goal_id :id)) ros::timeout) (unless ros::goal_id (ros::ros-error "[~A] :wait-for-result (return nil when no goal exists)" ros::name-space) (return-from :wait-for-result nil)) (ros::rate ros::wait-rate) (while (ros::ok) (ros::ros-debug "[~A] :wait-for-result ~A ~A" ros::name-space (ros::simple-goal-states-to-string ros::simple-state) (send ros::comm-state :state)) (send self :spin-once) (if (= ros::simple-state ros::*simple-goal-state-done*) (return)) (if (> ros::timeout 0) (let* ((ros::tm (ros::time- (ros::time-now) ros::start))) (if (> (send ros::tm :to-sec) ros::timeout) (return-from :wait-for-result nil)))) (when ros::return-if-server-down (when (= (ros::get-num-publishers ros::result-topic) 0) (ros::ros-error "[~A] Unexpected returns from :wait-for-result : no publishers found for ~A" ros::name-space ros::result-topic) (return-from :wait-for-result nil)) (when (and ros::status-stamp ros::maximum-status-interval (> (send (ros::time- (ros::time-now) ros::status-stamp) :to-sec) ros::maximum-status-interval)) (ros::ros-error "[~A] Unexpected returns from :wait-for-result : status did not received for 5 seconds" ros::name-space) (return-from :wait-for-result nil))) (ros::sleep)) (ros::ros-debug "[~A] :wait-for-result finished ~A" ros::name-space (ros::goal-status-to-string (send self :get-state))) (if (eq (send self :get-state) actionlib_msgs::goalstatus::*preempted*) (ros::ros-warn "[~A] :wait-for-result finished with preempted status" ros::name-space)) (eq (send self :get-state) actionlib_msgs::goalstatus::*succeeded*))
  19: at (send find-client :wait-for-result :timeout 0.5)
  And more...
/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl roseus-error: cannot find method :init in (send #:inst314671 :init), exitting...
[ INFO] [1586793813.624357219, 71.736000000]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [1586793813.624500594, 71.736000000]: exiting roseus 1
[gazebo_demo-17] process has died [pid 26119, exit code 1, cmd /home/tork/catkin_ws/src/jsk_robot/jsk_fetch_robot/jsk_fetch_gazebo_demo/euslisp/demo.l (main) __name:=gazebo_demo __log:=/home/tork/.ros/log/8f458f24-7d9f-11ea-b088-c0b883ebd679/gazebo_demo-17.log].
log file: /home/tork/.ros/log/8f458f24-7d9f-11ea-b088-c0b883ebd679/gazebo_demo-17*.log

goalのtopicのログ

$ rostopic echo /basic_grasping_perception/find_objects/goal
WARNING: topic [/basic_grasping_perception/find_objects/goal] does not appear to be published yet
WARNING: no messages received and simulated time is active.
Is /clock being published?
header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
goal_id: 
  stamp: 
    secs: 0
    nsecs:         0
  id: "71634000000_/gazebo_demo_26119_basic_grasping_perception/find_objects_0"
goal: 
  plan_grasps: True
---

resultのtopicのログ

$ rostopic echo /basic_grasping_perception/find_objects/result --noarr
WARNING: topic [/basic_grasping_perception/find_objects/result] does not appear to be published yet
WARNING: no messages received and simulated time is active.
Is /clock being published?
header: 
  seq: 0
  stamp: 
    secs: 71
    nsecs: 714000000
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 71
      nsecs: 634000000
    id: "71634000000_/gazebo_demo_26119_basic_grasping_perception/find_objects_0"
  status: 3
  text: "Succeeded."
result: 
  objects: "<array type: grasping_msgs/GraspableObject, length: 1>"
  support_surfaces: "<array type: grasping_msgs/Object, length: 1>"
---

cancelのtopicのログ

$ rostopic echo /basic_grasping_perception/find_objects/cancel --noarr
WARNING: topic [/basic_grasping_perception/find_objects/cancel] does not appear to be published yet
WARNING: no messages received and simulated time is active.
Is /clock being published?

feedbackのtopicのログ

$ rostopic echo /basic_grasping_perception/find_objects/feedback --noarr
WARNING: topic [/basic_grasping_perception/find_objects/feedback] does not appear to be published yet
WARNING: no messages received and simulated time is active.
Is /clock being published?

statusのtopicのログ
すべてのログはこちらにあります。: https://drive.google.com/open?id=17EcAFIl-OrQ_XTk8kuii4fAy_vjhAvsJ

$ rostopic echo /basic_grasping_perception/find_objects/status --noarr
WARNING: topic [/basic_grasping_perception/find_objects/status] does not appear to be published yet
header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
status_list: "<array type: actionlib_msgs/GoalStatus, length: 0>"
---
header: 
  seq: 1
  stamp: 
    secs: 0
    nsecs: 200000000
  frame_id: ''
status_list: "<array type: actionlib_msgs/GoalStatus, length: 0>"
---
header: 
  seq: 2
  stamp: 
    secs: 0
    nsecs: 400000000
  frame_id: ''
status_list: "<array type: actionlib_msgs/GoalStatus, length: 0>"
---
header: 
  seq: 3
  stamp: 
    secs: 0
    nsecs: 600000000
  frame_id: ''
status_list: "<array type: actionlib_msgs/GoalStatus, length: 0>"
---

@708yamaguchi
よろしくお願いします。

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Apr 13, 2020

山口です。

この問題ですが、@708yamaguchi@Kanazawanaoakiの環境では問題なく動き、@98shimpeiとB4松浦くんの環境ではこの問題が起きてデモが出来ていません。

demo.lとroseusのエラーを見ると、roseusの中で立ち上げているactionlib clientの:send-goal後の:wait-for-resultの中で呼ばれている(send self :spin-once)でエラーが起きているようなのですが、エラーメッセージを見てもどこがエラーを起こしているのか分かりません。
https://github.com/708yamaguchi/jsk_robot/blob/fetch-gazebo-demo/jsk_fetch_robot/jsk_fetch_gazebo_demo/euslisp/demo.l#L62-L69

そこで、自分と@98shimpeiのそれぞれの環境でactionlibのgoalとresultのトピックをechoして見比べましたが、違いがあるようには思えませんでした。(つまりactionlibのserverが問題ではないような気がします。)
それぞれのrostopic echo結果はコメントに貼っています。

佐藤くんのこのエラーは、fetchが物体を見ている時(すなわち、rostopic echo /basic_grasping_perception/find_objects/resultresult/objectsresult/support_surfacesにインスタンスが格納されている時)にのみ発生していました。
物体が見えていないときに:send-goalおよび:wait-for-serverしても、エラーが起きませんでした。

また、問題が発生している:send-goalwait-for-serverの部分を、roseusのfibonacciのactionlib clientに置き換え、fetchのデモと同時にfibonacci actionlib serverを立ち上げると、fibonacci計算は走り、fibonacci actionlibのresultも受け取ることが出来ました。
https://github.com/jsk-ros-pkg/jsk_roseus/blob/master/roseus/test/fibonacci-client.l#L42-L53

佐藤くんの環境でも、fetchrobotics本家のpythonによるでもプログラムは正常に動き、上記actionlibも使えるようです。
https://github.com/fetchrobotics/fetch_gazebo/blob/gazebo9/fetch_gazebo_demo/scripts/demo.py#L131-L135

画面共有越しで一緒にデバッグしましたが、結局原因がよくわからなかったので助けていただけると嬉しいです。@knorth55 @k-okada

以下、僕の環境での実行結果です。

roslaunchの結果(僕の環境では問題なく動いているのでエラーはありません)
ログ全体は以下にあります。
https://drive.google.com/file/d/1tY_tTaIdXt0nNgdmXxwGUl5JyzXurSkx/view?usp=sharing

$ roslaunch jsk_fetch_gazebo_demo demo.launch
... logging to /home/naoya/.ros/log/ee45a652-7da5-11ea-9bff-b808cf4bc0d8/roslaunch-naoya-ThinkPad-T460s-3508.log

...

[ INFO] [1586796441.520353208, 74.913000000]: Picking object...
;; quaternion2matrix : invalid input #f(0.0 0.0 0.0 0.0), the norm is not 1
[ INFO] [1586796441.961522017, 75.054000000]: Received new planning service request...
[ INFO] [1586796441.963531610, 75.055000000]: Planner configuration 'arm_with_torso[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1586796441.964699642, 75.055000000]: arm_with_torso[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1586796441.967661114, 75.056000000]: arm_with_torso[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1586796441.968684204, 75.056000000]: arm_with_torso[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1586796441.986217348, 75.059000000]: arm_with_torso[RRTConnectkConfigDefault]: Created 14 states (2 start + 12 goal)
[ INFO] [1586796442.037867182, 75.081000000]: arm_with_torso[RRTConnectkConfigDefault]: Created 32 states (3 start + 29 goal)
[ INFO] [1586796442.055140143, 75.084000000]: arm_with_torso[RRTConnectkConfigDefault]: Created 59 states (5 start + 54 goal)
[ INFO] [1586796442.056412207, 75.084000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.092339 seconds
[ INFO] [1586796442.141936939, 75.105000000]: SimpleSetup: Path simplification took 0.085420 seconds and changed from 6 to 5 states
[ INFO] [1586796442.150382320, 75.107000000]: ;; Planned Trajectory Total Time   8.440 [sec]
[ INFO] [1586796442.150817229, 75.107000000]: ;; Scaled Trajectory Total Time 10.000(10.000) [sec]
[ INFO] [1586796442.150877427, 75.107000000]: ;; generated 17 points for 10.0 sec using [arm_with_torso] group

goalに送ったトピック

$ rostopic echo /basic_grasping_perception/find_objects/goal --noarr
WARNING: topic [/basic_grasping_perception/find_objects/goal] does not appear to be published yet
WARNING: no messages received and simulated time is active.
Is /clock being published?
header:
  seq: 0
  stamp:
    secs: 0
    nsecs:         0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs:         0
  id: "74913000000_/gazebo_demo_3781_basic_grasping_perception/find_objects_0"
goal:
  plan_grasps: True
---

resultから受け取ったトピック

$ rostopic echo /basic_grasping_perception/find_objects/result --noarr
WARNING: topic [/basic_grasping_perception/find_objects/result] does not appear to be published yet
WARNING: no messages received and simulated time is active.
Is /clock being published?
header:
  seq: 0
  stamp:
    secs: 74
    nsecs: 993000000
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 74
      nsecs: 913000000
    id: "74913000000_/gazebo_demo_3781_basic_grasping_perception/find_objects_0"
  status: 3
  text: "Succeeded."
result:
  objects: "<array type: grasping_msgs/GraspableObject, length: 1>"
  support_surfaces: "<array type: grasping_msgs/Object, length: 1>"
---

cancel

$ rostopic echo /basic_grasping_perception/find_objects/cancel --noarr
WARNING: topic [/basic_grasping_perception/find_objects/cancel] does not appear to be published yet
WARNING: no messages received and simulated time is active.
Is /clock being published?

feedback

$ rostopic echo /basic_grasping_perception/find_objects/feedback --noarr
WARNING: topic [/basic_grasping_perception/find_objects/feedback] does not appear to be published yet
WARNING: no messages received and simulated time is active.
Is /clock being published?

status
ログ全体は以下にあります。
https://drive.google.com/file/d/1CmHvv-TJpUXpSo9IJe0n0T_jhAoNDdLj/view?usp=sharing

$ rostopic echo /basic_grasping_perception/find_objects/status --noarr
WARNING: topic [/basic_grasping_perception/find_objects/status] does not appear to be published yet
header:
  seq: 0
  stamp:
    secs: 0
    nsecs:         0
  frame_id: ''
status_list: "<array type: actionlib_msgs/GoalStatus, length: 0>"
---
header:
  seq: 1
  stamp:
    secs: 0
    nsecs: 200000000
  frame_id: ''
status_list: "<array type: actionlib_msgs/GoalStatus, length: 0>"
---

...

@knorth55
Copy link
Member

オンライン一週間デモのドキュメンテーションにjsk_fetch_gazebo_demoの存在ってかいてあるかな?
README.mdに行き着くにはどうすればいいかってかいてあるかな
そこらへんをちゃんとB4の人たちと共有できているんだろうか

あとjsk_fetch_gazebo_demoはどのパッケージにもrun_dependとして設定されていないけどいいのかな

@knorth55
Copy link
Member

@98shimpei @708yamaguchi can you try these commands?

roseus
# inside roseus
(ros::load-ros-manifest "grasping_msgs")
(instance grasping_msgs::FindGraspableObjectsAction :init)
# error

@knorth55
Copy link
Member

@708yamaguchi @98shimpei also, can you try rospack find roseus and paste the result?

@knorth55
Copy link
Member

@708yamaguchi maybe, it is better to test the demo running with docker image and create whole new environment and build it in docker.

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Apr 14, 2020

grasping_msgsについて

@98shimpei に昨日打ってもらったコマンドです。

1.irteusgl$ (ros::load-ros-manifest "grasping_msgs")
t
2.irteusgl$ (instance grasping_msgs::FindGraspableObjectsGoal :init)
#<grasping_msgs::findgraspableobjectsgoal #X56309a597858>
3.irteusgl$ 

僕が今打ったコマンドです

1.irteusgl$ (ros::load-ros-manifest "grasping_msgs")
t
2.irteusgl$ (instance grasping_msgs::FindGraspableObjectsAction :init)
#<grasping_msgs::findgraspableobjectsaction #X564cd0ee0440>

3.irteusgl$ (ros::load-ros-manifest "grasping_msgs")
t
4.irteusgl$ (instance grasping_msgs::FindGraspableObjectsGoal :init)
#<grasping_msgs::findgraspableobjectsgoal #X564cd0fc4128>

roseusについて

昨日2人でrosversion roseusしたところ、2人とも1.7.4と返ってきました。
今僕が打ったコマンドです。

$ rospack find roseus
/opt/ros/melodic/share/roseus

@98shimpei
北川さんが言っているとおり、

roseus
# inside roseus
(ros::load-ros-manifest "grasping_msgs")
(instance grasping_msgs::FindGraspableObjectsAction :init)
# error

rospack find roseus

の結果を貼ってくれると助かります。

@knorth55
Copy link
Member

ちなみにぼくの環境だと

$ roseus
configuring by "/home/knorth55/install/jskeus/eus/lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb
;; extending gcstack 0x5644f220e690[16374] --> 0x5644f268c1f0[32748] top=3d64
irtgl irtglc irtviewer
EusLisp 9.26(f70dd58 36d68661) for Linux64 created on melodic-p50(Tue Aug 6 19:08:50 JST 2019)
roseus ;; loading roseus("1.7.4") on euslisp((9.26 melodic-p50 Tue Aug 6 19:08:50 JST 2019 f70dd58 36d68661))
eustf roseus_c_util 1.irteusgl$ (ros::load-ros-manifest "grasping_msgs")
t
2.irteusgl$ (instance grasping_msgs::FindGraspableObjectsAction :init)
Call Stack (max depth: 20):
  0: at (send #:inst1058 :init)
  1: at (let ((#:inst1058 (instantiate object))) (send #:inst1058 :init) #:inst1058)
  2: at (instance object :init)
  3: at (send #:inst1057 :init)
  4: at (let ((#:inst1057 (instantiate grasping_msgs::graspableobject))) (send #:inst1057 :init) #:inst1057)
  5: at (instance grasping_msgs::graspableobject :init)
  6: at (send #:inst1056 :init)
  7: at (let ((#:inst1056 (instantiate grasping_msgs::findgraspableobjectsfeedback))) (send #:inst1056 :init) #:inst1056)
  8: at (instance grasping_msgs::findgraspableobjectsfeedback :init)
  9: at (send #:inst1050 :init)
  10: at (let ((#:inst1050 (instantiate grasping_msgs::findgraspableobjectsactionfeedback))) (send #:inst1050 :init) #:inst1050)
  11: at (instance grasping_msgs::findgraspableobjectsactionfeedback :init)
  12: at (send #:inst1034 :init)
  13: at (let ((#:inst1034 (instantiate grasping_msgs::findgraspableobjectsaction))) (send #:inst1034 :init) #:inst1034)
  14: at (instance grasping_msgs::findgraspableobjectsaction :init)
  15: at #<compiled-code #X5644f22c7b60>
/home/knorth55/install/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :init in (send #:inst1058 :init)

こうなって失敗します

@knorth55
Copy link
Member

@708yamaguchi @98shimpei あとrospack find grasping_msgs

@708yamaguchi
Copy link
Member Author

オンライン一週間デモのドキュメンテーションにjsk_fetch_gazebo_demoの存在ってかいてあるかな?
README.mdに行き着くにはどうすればいいかってかいてあるかな
そこらへんをちゃんとB4の人たちと共有できているんだろうか

ありがとうございます。一週間デモのドキュメンテーションをアップデートしました。
このPull RequestのREADME.mdでも、実行方法をアップデートしておきます。

あとjsk_fetch_gazebo_demoはどのパッケージにもrun_dependとして設定されていないけどいいのかな

jsk_fetch_gazebo_demoはデモを動かすためだけのパッケージのつもりで、他のパッケージから使われる用途を想定していませんでした。
例えばどういうrun_dependが考えられるでしょうか?

@708yamaguchi maybe, it is better to test the demo running with docker image and create whole new environment and build it in docker.

So, should I use demo.launch always in docker? I'll try it.

@708yamaguchi
Copy link
Member Author

rospack findの結果です。

$ rospack find grasping_msgs
/opt/ros/melodic/share/grasping_msgs

@knorth55
Copy link
Member

jsk_fetch_gazebo_demoはjsk_robotに依存させなくていいの?という話
catkin b jsk_robotでjsk_fetch_gazebo_demoが今はbuildされないよね
jsk_fetch_startupにdependしてもいいんだけど

dockerの話は、手元で再現できない問題をdockerなら再現できるんじゃないかって話です
docker imageをつくっておけばデバグも簡単にできると思います

@98shimpei
Copy link

@knorth55 @708yamaguchi
先程教えていただいたコマンドを実行したところ、以下のようになりました。

roseusのログ

$ roseus
configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin 
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x560476e11eb0[16374] --> 0x5604772970f0[32748] top=3d66
irtgl irtglc irtviewer 
EusLisp 9.27( 1.2.1) for Linux64 created on ip-172-30-1-167(Wed Mar 4 01:33:18 UTC 2020)
roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-167 Wed Mar 4 01:33:18 UTC 2020  1.2.1))
eustf roseus_c_util 1.irteusgl$ 
nil
1.irteusgl$ (ros::load-ros-manifest "grasping_msgs")
t
2.irteusgl$ (instance grasping_msgs::FindGraspableObjectsAction :init)
#<grasping_msgs::findgraspableobjectsaction #X560478204c90>
3.irteusgl$ 

rospackのログ

$ rospack find roseus
/opt/ros/melodic/share/roseus

$ rospack find grasping_msgs
/opt/ros/melodic/share/grasping_msgs

@knorth55
Copy link
Member

@98shimpei これはできるかな?
(instance grasping_msgs::FindGraspableObjectsActionResult :init)

@98shimpei
Copy link

@knorth55
実行したところ以下のようになりました。

$ roseus
configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin 
connected to Xserver DISPLAY=:0
X events are being asynchronously monitored.
;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtpointcloud irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x560476e11eb0[16374] --> 0x5604772970f0[32748] top=3d66
irtgl irtglc irtviewer 
EusLisp 9.27( 1.2.1) for Linux64 created on ip-172-30-1-167(Wed Mar 4 01:33:18 UTC 2020)
roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-167 Wed Mar 4 01:33:18 UTC 2020  1.2.1))
eustf roseus_c_util 1.irteusgl$ 
nil
1.irteusgl$ (ros::load-ros-manifest "grasping_msgs")
t
2.irteusgl$ (instance grasping_msgs::FindGraspableObjectsAction :init)
#<grasping_msgs::findgraspableobjectsaction #X560478204c90>
3.irteusgl$ (instance grasping_msgs::FindGraspableObjectsActionResult :init)
#<grasping_msgs::findgraspableobjectsactionresult #X56047821ad78>

@sktometometo
Copy link
Contributor

手元の ubuntu18.04 + melodic 環境で実行しましたが, 同様のエラーが表示されました.

[ INFO] [WallTime: 1586848524.073149106, 92.752000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; Planned Trajectory Total Time   3.868 [sec]
[ INFO] [WallTime: 1586848524.073346521, 92.752000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; Scaled Trajectory Total Time 3.868(3.868) [sec]
[ INFO] [WallTime: 1586848524.073376006, 92.752000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; generated 11 points for 3.8678 sec using [arm_with_torso] group
[ INFO] [WallTime: 1586848524.073403436, 92.752000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; will send to (torso_lift_joint shoulder_pan_joint shoulder_lift_joint upperarm_roll_joint elbow_flex_joint forearm_roll_joint wrist_flex_joint wrist_roll_joint)
[ INFO] [WallTime: 1586848524.073499554, 92.752000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; send self :angle-vector :head-controller (#<controller-action-client #X557e1a54a1e0 /head_controller/follow_joint_trajectory>) (without planning)
;; #<linear-joint #X557e08e8f5e0 torso_lift_joint> :joint-angle(-4.004933e-05) violate min-angle(0.0)
;; #<linear-joint #X557e08e93828 l_gripper_finger_joint> :joint-angle(50.0041) violate max-angle(50.0)
;; #<linear-joint #X557e08e8f0b8 r_gripper_finger_joint> :joint-angle(50.0488) violate max-angle(50.0)
[ INFO] [WallTime: 1586848524.107602547, 92.784000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; send self :send-trajectory :default-controller
[ INFO] [WallTime: 1586848524.107786009, 92.784000000] [node:/gazebo] [func:ControllerManager::requestStart]: Stopped head_controller/point_head
[ INFO] [WallTime: 1586848524.107837080, 92.785000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started head_controller/follow_joint_trajectory
[ INFO] [WallTime: 1586848524.111140608, 92.787000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started torso_controller/follow_joint_trajectory
[ INFO] [WallTime: 1586848528.242483974, 96.669000000] [node:/move_group] [func:MoveGroupPlanService::computePlanService]: Received new planning service request...
[ INFO] [WallTime: 1586848528.243505821, 96.670000000] [node:/move_group] [func:ModelBasedPlanningContext::useConfig]: Planner configuration 'arm_with_torso[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [WallTime: 1586848528.244184716, 96.671000000] [node:/move_group] [func:OMPLPlannerManager::OMPLPlannerManager]: arm_with_torso[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [WallTime: 1586848528.244234833, 96.671000000] [node:/move_group] [func:OMPLPlannerManager::OMPLPlannerManager]: arm_with_torso[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [WallTime: 1586848528.247996160, 96.674000000] [node:/move_group] [func:OMPLPlannerManager::OMPLPlannerManager]: arm_with_torso[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [WallTime: 1586848528.255331326, 96.680000000] [node:/move_group] [func:OMPLPlannerManager::OMPLPlannerManager]: arm_with_torso[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [WallTime: 1586848528.256773839, 96.682000000] [node:/move_group] [func:OMPLPlannerManager::OMPLPlannerManager]: arm_with_torso[RRTConnectkConfigDefault]: Created 5 states (2 start + 3 goal)
[ INFO] [WallTime: 1586848528.258677208, 96.684000000] [node:/move_group] [func:OMPLPlannerManager::OMPLPlannerManager]: arm_with_torso[RRTConnectkConfigDefault]: Created 4 states (2 start + 2 goal)
[ INFO] [WallTime: 1586848528.258793464, 96.684000000] [node:/move_group] [func:OMPLPlannerManager::OMPLPlannerManager]: ParallelPlan::solve(): Solution found by one or more threads in 0.015067 seconds
[ INFO] [WallTime: 1586848528.266039082, 96.691000000] [node:/move_group] [func:OMPLPlannerManager::OMPLPlannerManager]: SimpleSetup: Path simplification took 0.007166 seconds and changed from 3 to 2 states
[ INFO] [WallTime: 1586848528.270242089, 96.695000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; Planned Trajectory Total Time   0.468 [sec]
[ INFO] [WallTime: 1586848528.270442274, 96.695000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; Scaled Trajectory Total Time 3.000(3.000) [sec]
[ INFO] [WallTime: 1586848528.270478121, 96.695000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; generated 10 points for 3.0 sec using [arm_with_torso] group
[ INFO] [WallTime: 1586848528.270537175, 96.695000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; will send to (torso_lift_joint shoulder_pan_joint shoulder_lift_joint upperarm_roll_joint elbow_flex_joint forearm_roll_joint wrist_flex_joint wrist_roll_joint)
[ INFO] [WallTime: 1586848528.270855683, 96.696000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; send self :angle-vector :head-controller (#<controller-action-client #X557e1a54a1e0 /head_controller/follow_joint_trajectory>) (without planning)
;; #<linear-joint #X557e08e93828 l_gripper_finger_joint> :joint-angle(50.0131) violate max-angle(50.0)
;; #<linear-joint #X557e08e8f0b8 r_gripper_finger_joint> :joint-angle(50.0398) violate max-angle(50.0)
[ INFO] [WallTime: 1586848528.288113013, 96.712000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: ;; send self :send-trajectory :default-controller
[ INFO] [WallTime: 1586848531.484068654, 99.718000000] [node:/gazebo_demo] [func:ROSEUS_ROSINFO]: Waiting for basic_grasping_perception/find_objects ...
Call Stack (max depth: 20):
  0: at (send #:inst250827 :init)
  1: at (let ((#:inst250827 (instantiate object))) (send #:inst250827 :init) #:inst250827)
  2: at (instance object :init)
  3: at (send #:inst250826 :init)
  4: at (let ((#:inst250826 (instantiate grasping_msgs::graspableobject))) (send #:inst250826 :init) #:inst250826)
  5: at (instance grasping_msgs::graspableobject :init)
  6: at (send #:inst250825 :init)
  7: at (let ((#:inst250825 (instantiate grasping_msgs::findgraspableobjectsfeedback))) (send #:inst250825 :init) #:inst250825)
  8: at (instance grasping_msgs::findgraspableobjectsfeedback :init)
  9: at (send #:inst250819 :init)
  10: at (let ((#:inst250819 (instantiate grasping_msgs::findgraspableobjectsactionfeedback))) (send #:inst250819 :init) #:inst250819)
  11: at (instance grasping_msgs::findgraspableobjectsactionfeedback :init)
  12: at (send #:inst250803 :init)
  13: at (let ((#:inst250803 (instantiate ros::spec))) (send #:inst250803 :init) #:inst250803)
  14: at (instance ros::spec :init)
  15: at (setq ros::action-spec (instance ros::spec :init))
  16: at (send #:inst250802 :init find-topic grasping_msgs::findgraspableobjectsaction)
  17: at (let ((#:inst250802 (instantiate ros::simple-action-client))) (send #:inst250802 :init find-topic grasping_msgs::findgraspableobjectsaction) #:inst250802)
  18: at (instance ros::simple-action-client :init find-topic grasping_msgs::findgraspableobjectsaction)
  19: at (setq find-client (instance ros::simple-action-client :init find-topic grasping_msgs::findgraspableobjectsaction))
  And more...
/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl roseus-error: cannot find method :init in (send #:inst250827 :init), exitting...
[ INFO] [WallTime: 1586848531.484533667, 99.719000000] [node:/gazebo_demo] [func:ROSEUS_EXIT]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [WallTime: 1586848531.484578263, 99.719000000] [node:/gazebo_demo] [func:ROSEUS_EXIT]: exiting roseus 1
[gazebo_demo-18] process has died [pid 11612, exit code 1, cmd /home/sktometometo/ros/ws_jsk_tutorial_fetch_demo/src/jsk_robot/jsk_fetch_robot/jsk_fetch_gazebo_demo/euslisp/demo.l (main) __name:=gazebo_demo __log:=/home/sktometometo/.ros/log/766794cc-7e1f-11ea-bde2-e4a7a054ed6d/gazebo_demo-18.log].
log file: /home/sktometometo/.ros/log/766794cc-7e1f-11ea-bde2-e4a7a054ed6d/gazebo_demo-18*.log

@knorth55
Copy link
Member

このエラーですがこちらのエラーでした。
jsk-ros-pkg/jsk_roseus#574

Guigaくん(@Affonso-Gui)が解決していました。
jsk-ros-pkg/jsk_roseus#609

@sktometometo
Copy link
Contributor

@708yamaguchi @knorth55 とデバッグしましたが,
このIssue jsk-ros-pkg/jsk_roseus#574 と同様の問題のようで, こちらのPR jsk-ros-pkg/jsk_roseus#609 を source から入れると解決するようです.

@708yamaguchi
Copy link
Member Author

708yamaguchi commented Apr 16, 2020

@HiroIshida
topic_toolsのrelayで、/odom_combinedを出すようにしました。

@k-okada k-okada merged commit 3b24d9c into jsk-ros-pkg:master Sep 25, 2021
@708yamaguchi
Copy link
Member Author

After mergning this PR, melodic test fails.
For example,

This is because this commit is not included when testing jsk_fetch_gaezebo_demo.

2021-09-25T12:18:54.9615685Z Call Stack (max depth: 20):
2021-09-25T12:18:54.9615936Z 
2021-09-25T12:18:54.9616241Z                                                                                 
2021-09-25T12:18:54.9616604Z   0: at (send obj :bodies)
2021-09-25T12:18:54.9616842Z 
2021-09-25T12:18:54.9617148Z                                                                                 
2021-09-25T12:18:54.9617813Z   1: at (mapcar #'(lambda (body) (send body :worldcoords)) (send obj :bodies))
2021-09-25T12:18:54.9618197Z 
2021-09-25T12:18:54.9618503Z                                                                                 
2021-09-25T12:18:54.9623593Z   2: at (let ((org-cds (send obj :copy-worldcoords))) (send obj :reset-coords) (send obj :worldcoords) (mapcar #'(lambda (body) (send body :worldcoords)) (send obj :bodies)) (let ((fs (body-to-faces obj)) (geom (instance shape_msgs::mesh :init)) idx-lst vertices (cntr 0)) (dolist (f (send fs :faces)) (let* ((vs (send f :vertices)) (v0 (car vs)) (v1 (cadr vs)) (v2 (caddr vs)) (p0 (instance geometry_msgs::point :init :x (/ (elt v0 0) 1000.0) :y (/ (elt v0 1) 1000.0) :z (/ (elt v0 2) 1000.0))) (p1 (instance geometry_msgs::point :init :x (/ (elt v1 0) 1000.0) :y (/ (elt v1 1) 1000.0) :z (/ (elt v1 2) 1000.0))) (p2 (instance geometry_msgs::point :init :x (/ (elt v2 0) 1000.0) :y (/ (elt v2 1) 1000.0) :z (/ (elt v2 2) 1000.0)))) (push p0 vertices) (push p1 vertices) (push p2 vertices) (push (integer-vector cntr (+ cntr 1) (+ cntr 2)) idx-lst) (incf cntr 3))) (send geom :triangles (mapcar #'(lambda (idx) (instance shape_msgs::meshtriangle :init :vertex_indices idx)) (nreverse idx-lst))) (send geom :vertices (nreverse vertices)) (let ((m (send colobj :meshes)) (mp (send colobj :mesh_poses))) (send colobj :meshes (append m (list geom))) (send colobj :mesh_poses (append mp (list (ros::coords->tf-pose (if relative-pose relative-pose org-cds))))))) (send obj :transform org-cds) (send obj :worldcoords))

I will fix this problem.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

6 participants