add displaying error msg method to controller-action-client in robot-interface.l

tkmtnt7000 commented 3 years ago

I added action-result-cb method to controller-action-client class in robot-interface.l to display error massages in the topic ...[depending on the robots]../follow_joint_trajectory/result. I found it good when I was running KINOVA. This allows us to see error msg in the topic through an euslisp interpreter. cc @708yamaguchi

k-okada commented 3 years ago

@708yamaguchi can you extend https://github.com/jsk-ros-pkg/jsk_pr2eus/commit/73270ba645a307b8b32a45eff06be7bd60234d1b to simulate actual error case?

708yamaguchi commented 3 years ago

@tkmtnt7000

Fetch's follow joint trajectory server:

Topic type

$ rostopic info /arm_controller/follow_joint_trajectory/result
Type: control_msgs/FollowJointTrajectoryActionResult

Success result

Send normal trajectory

$ rostopic echo /arm_controller/follow_joint_trajectory/result
header:
  seq: 7
  stamp:
    secs: 1637579129
    nsecs: 101318115
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1637579124
      nsecs: 105360013
    id: "1637579124099497863_/fetch_eus_interface_1637577840428118257_6380_/arm_controller/follow_joint_trajectory_5"
  status: 3
  text: "Trajectory succeeded."
result:
  error_code: 0
  error_string: ''
---

Abort result

Send trajectory with very small time_from_start (0.1s) The same result returned when I pushed emergency button during the normal trajectory.

rostopic echo /arm_controller/follow_joint_trajectory/result
header:
  seq: 8
  stamp:
    secs: 1637579250
    nsecs: 881366207
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1637579250
      nsecs: 831392874
    id: "/robot_driver-3-1637579250.831392874"
  status: 4
  text: "Controller manager forced preemption."
result:
  error_code: 0
  error_string: ''
---

Preempted result

Send other goal during another trajectory

$ rostopic echo /arm_controller/follow_joint_trajectory/result
header:
  seq: 10
  stamp:
    secs: 1637579740
    nsecs: 465346321
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1637579739
      nsecs: 460864349
    id: "/robot_driver-5-1637579739.460864349"
  status: 2
  text: "Trajectory preempted"
result:
  error_code: 0
  error_string: ''
---

test code: https://gist.github.com/708yamaguchi/2c754cbfc1425345aceb5c3e7a66ae22

708yamaguchi commented 3 years ago

pr2' follow joint trajectory server

topic type

$ rostopic info /r_arm_controller/follow_joint_trajectory/result
Type: control_msgs/FollowJointTrajectoryActionResult

Success result

Send normal trajectory

$ rostopic echo /r_arm_controller/follow_joint_trajectory/result
header:
  seq: 13226
  stamp:
    secs: 1637584064
    nsecs: 195126159
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1637584054
      nsecs: 224153773
    id: "1637584054187325286_/pr2_eus_interface_1637583850124161382_20332_r_arm_controller/follow_joint_trajectory_2"
  status: 3
  text: ''
result:
  error_code: 0
  error_string: ''
---

Abort result

Send trajectory with very small time_from_start (0.1s)

$ rostopic echo /r_arm_controller/follow_joint_trajectory/result

header:
  seq: 13227
  stamp:
    secs: 1637584346
    nsecs: 447756657
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1637584345
      nsecs: 787349148
    id: "/realtime_loop-1-1637584345.787349148"
  status: 4
  text: ''
result:
  error_code: -5
  error_string: ''
---

Preempted result

Send other goal during another trajectory

$ rostopic echo /r_arm_controller/follow_joint_trajectory/result                                                                  
header:                                                                                                                           
  seq: 13230
  stamp:
    secs: 1637584476
    nsecs: 271548854
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1637584475
      nsecs: 268636477
    id: "/realtime_loop-4-1637584475.268636477"
  status: 2
  text: ''
result:
  error_code: 0
  error_string: ''
---

test code https://gist.github.com/708yamaguchi/a331023b0321f01d8fc9f36d7b59273a

708yamaguchi commented 3 years ago

kinova' follow joint trajectory server

topic type

$ rostopic info /arm_gen3/kinova_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/result
Type: control_msgs/FollowJointTrajectoryActionResult

Success result

Send normal trajectory

$ rostopic echo /arm_gen3/kinova_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/result                                                   
header:                                                                                                                                                 
  seq: 2                                                                                                                                                
  stamp:                                                                                                                                                
    secs: 1637586252                                                                                                                                    
    nsecs: 211414560                                                                                                                                    
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1637586239
      nsecs: 876266440
    id: "1637586239878205593_/spotkinova_interface_1637586163323500035_10152_arm_gen3/kinova_gen3_lite_joint_trajectory_controller/follow_joint_trajectory_0"
  status: 3
  text: ''
result:
  error_code: 0
  error_string: ''
---

Abort result

pattern 1

$ rostopic echo /arm_gen3/kinova_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/result

header:
  seq: 6
  stamp:
    secs: 1637586913
    nsecs: 991771956
  frame_id: ''
status:
  goal_id:
    stamp:
      secs: 1637586912
      nsecs: 362710512
    id: "/arm_gen3/arm_gen3_driver-3-1637586912.362710512"
  status: 4
  text: ''
result:
  error_code: -4
  error_string: "After validation, trajectory execution failed in the arm with sub error code SUB_ERROR_NONE"
---

pattern 2

$ rostopic echo /arm_gen3/kinova_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/result
header: 
  seq: 7
  stamp: 
    secs: 1637586975
    nsecs:  91155403
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1637586975
      nsecs:  84985092
    id: "/arm_gen3/arm_gen3_driver-4-1637586975.84985092"
  status: 4
  text: ''
result: 
  error_code: 0
  error_string: ''
---

Preempted result

Send other goal during another trajectory Be careful that the preempt status number for kinova is 4, while other robots show 2 when preempted. Once this error occurs, the next time you send a normal trajectory, you will get this error instead of the normal result.

$ rostopic echo /arm_gen3/kinova_gen3_lite_joint_trajectory_controller/follow_joint_trajectory/result
header:                                                                                                                           
  seq: 13                                                                                                                         
  stamp:                                                                                                                          
    secs: 1637587168                                                                                                              
    nsecs: 782398936                                                                                                              
  frame_id: ''                                                                                                                    
status:                                                                                                                           
  goal_id:                                                                                                                        
    stamp:                                                                                                                        
      secs: 1637587168                                                                                                            
      nsecs: 773036725                                                                                                            
    id: "1637587168763316512_/spotkinova_interface_1637586163323500035_10152_arm_gen3/kinova_gen3_lite_joint_trajectory_controller/follow_joint_trajectory_4"
  status: 4
  text: ''
result:
  error_code: -4
  error_string: "Trajectory execution failed in the arm with sub error code 55\nThe speed while executing\
  \ the trajectory was too damn high and caused the robot to stop.\n"
---

test code: https://gist.github.com/708yamaguchi/a2a982e9e9d44d356c7522376168ec6c and kinovaeus.

tkmtnt7000 commented 3 years ago

@Affonso-Gui Thank you very much for advice.

This should be made safer against other types of actions. Take a look in the :action-feedback-cb just below and notice how it verifies that the action type is correct before trying to get any values (for example pr2_controller_msgs/JointTrajectoryActionResult doesn't have :error_code)

Maybe also consider the more universal :status and cases in which the error-string is empty for better error formatting? When trying on the simulator with an empty goal I got status: 5, error_code: -2, error_string: ' '

I'll use :status for checking error and update action-result-cb more safer against the type of action. And I'll display error_string if error_string is published.

tkmtnt7000 commented 3 years ago

Sorry, I mistakenly closed this PR. I reopened.

tkmtnt7000 commented 3 years ago

Changes from 0c38069

Use :status for checking and support for the case where :error_string is empty
Delete global function controller-result-error-to-string and set local variables which have the same behavior
Support for the case where some action types do not have :error_code

tkmtnt7000 commented 3 years ago

Outputs of test code

PR2


$ rostest pr2eus ri-jta.test robot:=pr2
... logging to /home/tsukamoto/.ros/log/rostest-tsukamoto-p15s-25014.log
[ROSUNIT] Outputting test results to /home/tsukamoto/.ros/test_results/pr2eus/rostest-test_ri-jta.xml
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 irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x55bdd5670690[16374] --> 0x55bdd5af7800[32748] top=3c74
irtgl irtglc irtviewer 
EusLisp 9.27( 1.2.2) for Linux64 created on ip-10-0-1-65(Tue May 4 17:29:14 PST 2021)
roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-10-0-1-65 Tue May 4 17:29:14 PST 2021  1.2.2))
eustf roseus_c_util [WARN] [1637724141.874458] [/dummy_jta_server]: Started Dummy Joint Trajectory Action Server
[WARN] [1637724141.875127] [/dummy_jta_server]: If joint < 0, set aborted
[WARN] [1637724141.875703] [/dummy_jta_server]: If joint >= 100, set preempted
;;
;;
;; roseus/euslisp/unittest.l is deprecated
;; use (require :unittest "lib/llib/unittest.l")
;;
output to /home/tsukamoto/.ros/test_results/pr2eus/rosunit-pr2_ri_test_arm.xml
start testing [test-abort]
[WARN] [1637724143.941277] [/dummy_jta_server]: Set aborted
[ERROR] [1637724143.954150842] [/pr2_ri_test_arm]: joint trajectory status: (4 . aborted)

start testing [test-preempted] [WARN] [1637724143.964566] [/dummy_jta_server]: Set preempted [ERROR] [1637724143.970247514] [/pr2_ri_test_arm]: joint trajectory status: (2 . preempted)

[ WARN] [1637724143.971185554] [/pr2_ri_test_arm]: [dummy_controller/follow_joint_trajectory] :wait-for-result finished with preempted status start testing [test-suceeded] [Testcase: testpr2_ri_test_arm] ... ok

[ROSTEST]-----------------------------------------------------------------------

[pr2eus.rosunit-pr2_ri_test_arm/all-test][passed] [pr2eus.rosunit-pr2_ri_test_arm/test-abort][passed] [pr2eus.rosunit-pr2_ri_test_arm/test-preempted][passed] [pr2eus.rosunit-pr2_ri_test_arm/test-suceeded][passed]

SUMMARY

RESULT: SUCCESS
TESTS: 3
ERRORS: 0
FAILURES: 0

rostest log file is in /home/tsukamoto/.ros/log/rostest-tsukamoto-p15s-25014.log

- KINOVA

$ rostest pr2eus ri-jta.test robot:=kinova ... logging to /home/tsukamoto/.ros/log/rostest-tsukamoto-p15s-25088.log [ROSUNIT] Outputting test results to /home/tsukamoto/.ros/test_results/pr2eus/rostest-test_ri-jta.xml 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 irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb ;; extending gcstack 0x55aa16610690[16374] --> 0x55aa16a97800[32748] top=3c74 irtgl irtglc irtviewer EusLisp 9.27( 1.2.2) for Linux64 created on ip-10-0-1-65(Tue May 4 17:29:14 PST 2021) roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-10-0-1-65 Tue May 4 17:29:14 PST 2021 1.2.2)) eustf roseus_c_util [WARN] [1637724154.436002] [/dummy_jta_server]: Started Dummy Joint Trajectory Action Server [WARN] [1637724154.436638] [/dummy_jta_server]: If joint < 0, set aborted [WARN] [1637724154.437243] [/dummy_jta_server]: If joint >= 100, set preempted ;; ;; ;; roseus/euslisp/unittest.l is deprecated ;; use (require :unittest "lib/llib/unittest.l") ;; output to /home/tsukamoto/.ros/test_results/pr2eus/rosunit-kinova_ri_test_arm.xml start testing [test-abort] [WARN] [1637724156.504510] [/dummy_jta_server]: Set aborted [ERROR] [1637724156.512910851] [/kinova_ri_test_arm]: joint trajectory status: (4 . aborted)

[ERROR] [1637724156.515526707] [/kinova_ri_test_arm]: After validation, trajectory execution failed in the arm with sub error code SUB_ERROR_NONE

start testing [test-preempted] [WARN] [1637724156.527162] [/dummy_jta_server]: Set preempted [ERROR] [1637724156.537943989] [/kinova_ri_test_arm]: joint trajectory status: (4 . aborted)

[ERROR] [1637724156.538150123] [/kinova_ri_test_arm]: Trajectory execution failed in the arm with sub error code 55 The speed while executing\n\ the trajectory was too damn high and caused the robot to stop.

start testing [test-suceeded] [Testcase: testkinova_ri_test_arm] ... ok

[ROSTEST]-----------------------------------------------------------------------

[pr2eus.rosunit-kinova_ri_test_arm/all-test][passed] [pr2eus.rosunit-kinova_ri_test_arm/test-abort][passed] [pr2eus.rosunit-kinova_ri_test_arm/test-preempted][passed] [pr2eus.rosunit-kinova_ri_test_arm/test-suceeded][passed]

SUMMARY

RESULT: SUCCESS
TESTS: 3
ERRORS: 0
FAILURES: 0

rostest log file is in /home/tsukamoto/.ros/log/rostest-tsukamoto-p15s-25088.log

- Fetch

$ rostest pr2eus ri-jta.test robot:=fetch ... logging to /home/tsukamoto/.ros/log/rostest-tsukamoto-p15s-25163.log [ROSUNIT] Outputting test results to /home/tsukamoto/.ros/test_results/pr2eus/rostest-test_ri-jta.xml 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 irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb ;; extending gcstack 0x565380ba5690[16374] --> 0x56538102c800[32748] top=3c74 irtgl irtglc irtviewer EusLisp 9.27( 1.2.2) for Linux64 created on ip-10-0-1-65(Tue May 4 17:29:14 PST 2021) roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-10-0-1-65 Tue May 4 17:29:14 PST 2021 1.2.2)) eustf roseus_c_util [WARN] [1637724164.439482] [/dummy_jta_server]: Started Dummy Joint Trajectory Action Server [WARN] [1637724164.440101] [/dummy_jta_server]: If joint < 0, set aborted [WARN] [1637724164.440647] [/dummy_jta_server]: If joint >= 100, set preempted ;; ;; ;; roseus/euslisp/unittest.l is deprecated ;; use (require :unittest "lib/llib/unittest.l") ;; output to /home/tsukamoto/.ros/test_results/pr2eus/rosunit-fetch_ri_test_arm.xml start testing [test-abort] [WARN] [1637724166.506555] [/dummy_jta_server]: Set aborted [ERROR] [1637724166.517102039] [/fetch_ri_test_arm]: joint trajectory status: (4 . aborted)

[ERROR] [1637724166.519296506] [/fetch_ri_test_arm]: Controller manager forced preemption.

start testing [test-preempted] [WARN] [1637724166.529134] [/dummy_jta_server]: Set preempted [ERROR] [1637724166.542320274] [/fetch_ri_test_arm]: joint trajectory status: (2 . preempted)

[ERROR] [1637724166.542413961] [/fetch_ri_test_arm]: Trajectory preempted

[ WARN] [1637724166.542925382] [/fetch_ri_test_arm]: [dummy_controller/follow_joint_trajectory] :wait-for-result finished with preempted status start testing [test-suceeded] [Testcase: testfetch_ri_test_arm] ... ok

[ROSTEST]-----------------------------------------------------------------------

[pr2eus.rosunit-fetch_ri_test_arm/all-test][passed] [pr2eus.rosunit-fetch_ri_test_arm/test-abort][passed] [pr2eus.rosunit-fetch_ri_test_arm/test-preempted][passed] [pr2eus.rosunit-fetch_ri_test_arm/test-suceeded][passed]

SUMMARY

RESULT: SUCCESS
TESTS: 3
ERRORS: 0
FAILURES: 0

rostest log file is in /home/tsukamoto/.ros/log/rostest-tsukamoto-p15s-25163.log

tkmtnt7000 commented 3 years ago

I changed the order of conditional branch in cond clause to make it simpler.

708yamaguchi commented 3 years ago

melodic test fails. https://github.com/jsk-ros-pkg/jsk_pr2eus/runs/4311911161?check_suite_focus=true

It seems that test-collision-object-publiser fails randomly.

We created PR to solve it. https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/471

k-okada commented 2 years ago

LGTM, please pass the test.

tkmtnt7000 commented 2 years ago

Could you please give us permissions to press Re-run jobs in Github actions? This repository does not seem to be set up yet.

k-okada commented 2 years ago

sorry, i had granted write permission to developers . Please confirm you can restart but not merge.

◉ Kei Okada

2022年10月18日(火) 13:58 Naoto Tsukamoto @.***>:

Could you please give us to press Re-run jobs in Github actions? This repository does not seem to be set up yet.

— Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/460#issuecomment-1281815703, or unsubscribe https://github.com/notifications/unsubscribe-auth/AADYNXGYZ7522WI5K7T635LWDYU3LANCNFSM46P7CKLA . You are receiving this because you commented.Message ID: @.***>

tkmtnt7000 commented 2 years ago

Thank you. I can push Re-run button.

knorth55 commented 2 years ago

actions/checkout@v3.0.2 is good. https://gitlab.jsk.imi.i.u-tokyo.ac.jp/doc/bib/-/commit/0494be63b1f19d861719be60c5e60b15698b58f8

tkmtnt7000 commented 2 years ago

actions/checkout@v3.0.2 is good.

Thank you!

tkmtnt7000 commented 2 years ago

noetic test fails with following error. In my local environment, the same failure occurred. https://github.com/jsk-ros-pkg/jsk_pr2eus/actions/runs/3272413879/jobs/5384061347 https://gist.github.com/tkmtnt7000/a2cd600f1cee134fecbb5d456e1c3550

tkmtnt7000 commented 2 years ago

Test fails randomly in noetic(I do not know what is caused...), but Re-run test many times may make test pass. Especially, in pr2-ri-test-base.l, test-go-stop and test-move-to usually fail.

tkmtnt7000 commented 2 years ago

Ummm... noetic gazebo may have something weird???

jsk-ros-pkg / jsk_pr2eus