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

tkmtnt7000 · 2021-06-11T05:47:12Z

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

708yamaguchi

It works with kinova arm. (Related to jsk-ros-pkg/jsk_robot#1313 )
Looks good to me.

Affonso-Gui

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)
Does controller-result-error-to-string needs to be defined as a global function? Is it usefull outside of this scope? If so maybe the name should be changed to something that makes more clear that it is only functional for control_msgs/FollowJointTrajectoryResult
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: ' '

…g#460

k-okada · 2021-11-19T10:36:53Z

@708yamaguchi can you extend 73270ba to simulate actual error case?

708yamaguchi · 2021-11-22T11:18:41Z

@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 · 2021-11-22T12:36:50Z

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 · 2021-11-22T13:33:36Z

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 · 2021-11-22T13:45:34Z

@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.

…g#460

tkmtnt7000 · 2021-11-24T03:10:34Z

Sorry, I mistakenly closed this PR. I reopened.

tkmtnt7000 · 2021-11-24T03:19:36Z

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 · 2021-11-24T12:33:32Z

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

708yamaguchi · 2021-11-24T14:33:57Z

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.
#471

pr2eus/robot-interface.l

Affonso-Gui · 2022-10-18T03:21:42Z

pr2eus/robot-interface.l

+                        INVALID_JOINTS
+                        OLD_HEADER_TIMESTAMP
+                        PATH_TOLERANCE_VIOLATED
+                        GOAL_TOLERANCE_VIOLATED)))


Can we make these strings to avoid making new symbols?

Yes, but since there has been no mapping between these strings and error_code yet, we will have to send a pull request to roseus or something.

Maybe I was unclear. I mean:

(let ((error-word '("SUCCESSFUL" "INVALID_GOAL" "INVALID_JOINTS" "OLD_HEADER_TIMESTAMP" "PATH_TOLERANCE_VIOLATED" "GOAL_TOLERANCE_VIOLATED"))) ...)

Sorry. I see.
I updated in 2ce0b4c

k-okada · 2022-10-18T04:46:02Z

LGTM, please pass the test.

tkmtnt7000 · 2022-10-18T04:57:15Z

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 · 2022-10-18T05:07:25Z

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 <#460 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AADYNXGYZ7522WI5K7T635LWDYU3LANCNFSM46P7CKLA> . You are receiving this because you commented.Message ID: ***@***.***>

tkmtnt7000 · 2022-10-18T06:02:57Z

Thank you. I can push Re-run button.

knorth55 · 2022-10-18T09:24:33Z

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

tkmtnt7000 · 2022-10-18T09:55:20Z

actions/checkout@v3.0.2 is good.

Thank you!

same fix as jsk-ros-pkg/jsk_robot#1643

tkmtnt7000 · 2022-10-19T03:02:20Z

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 · 2022-10-19T04:36:52Z

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 · 2022-10-20T03:01:36Z

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

tkmtnt7000 added 3 commits June 11, 2021 14:38

add displaying error msg method to controller-action-client

b7c660e

fix typo in robot-interface.l

25a6da0

Display error code in :action-result-cb

0c38069

708yamaguchi reviewed Jun 11, 2021

View reviewed changes

708yamaguchi approved these changes Jun 11, 2021

View reviewed changes

Affonso-Gui requested changes Jun 11, 2021

View reviewed changes

k-okada added the PR/WaitForFix label Jul 21, 2021

k-okada added a commit to k-okada/jsk_pr2eus that referenced this pull request Nov 19, 2021

add dummy_jta_server.py and pr2-ri-jta for example code of jsk-ros-pk…

73270ba

…g#460

708yamaguchi mentioned this pull request Nov 22, 2021

Test jta of pr2, fetch and kinova k-okada/jsk_pr2eus#19

Merged

k-okada and others added 12 commits November 23, 2021 19:42

add dummy_jta_server.py and pr2-ri-jta for example code of jsk-ros-pk…

4cea577

…g#460

Add pr2, fetch and kinova's actual jta behavior

9f9d389

Add jta test for pr2, fetch and kinova

aec2985

Publish feedback in DummyJTA

0ac1d0b

use :status for checking error

a43e553

delete global function controller-result-error-to-string

4f33570

fix typo

e8e2a40

change log level in a specific case

9e4907f

Update action-result-cb to make safer against other types of actions

9315d5a

Update indent in :add-result-cb

2e52976

not displaying warning when succeeded

7f8fbd0

use cond in place of unless and when

f4cbe84

tkmtnt7000 closed this Nov 24, 2021

tkmtnt7000 reopened this Nov 24, 2021

tkmtnt7000 added 3 commits November 24, 2021 18:26

replace similar code

1ae2de4

return-from when follow joint trajectory succeeded

99fde4d

change order to simplify cond clause

8e827f9

tkmtnt7000 added 2 commits April 27, 2022 22:59

Merge branch 'master' into add_result_cb

f6f919a

Merge branch 'master' into add_result_cb

73e30b0

k-okada reviewed Oct 15, 2022

View reviewed changes

pr2eus/robot-interface.l Outdated Show resolved Hide resolved

k-okada added the PR/WaitForReview label Oct 15, 2022

tkmtnt7000 added 2 commits October 18, 2022 10:47

[pr2eus] Use goal-to-status-to-string to show status message

6df961c

Merge branch 'master' into add_result_cb

4899f6f

Affonso-Gui reviewed Oct 18, 2022

View reviewed changes

[pr2eus] fix github action

81ebea6

tkmtnt7000 force-pushed the add_result_cb branch from 4366b2b to 81ebea6 Compare October 18, 2022 09:40

[pr2eus] Use string

2ce0b4c

specify full version of actions/Checkout for github

dbbb3d8

same fix as jsk-ros-pkg/jsk_robot#1643

tkmtnt7000 force-pushed the add_result_cb branch from 1ec17a8 to dbbb3d8 Compare October 18, 2022 09:59

Merge branch 'master' into add_result_cb

46d738c

k-okada added the PR/MergeOk label Oct 25, 2022

k-okada merged commit 7e03b4d into jsk-ros-pkg:master Oct 25, 2022

tkmtnt7000 deleted the add_result_cb branch October 25, 2022 14:12

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

tkmtnt7000 commented Jun 11, 2021

708yamaguchi left a comment •

edited

Affonso-Gui left a comment •

edited

k-okada commented Nov 19, 2021

708yamaguchi commented Nov 22, 2021 •

edited

708yamaguchi commented Nov 22, 2021 •

edited

708yamaguchi commented Nov 22, 2021 •

edited

tkmtnt7000 commented Nov 22, 2021

tkmtnt7000 commented Nov 24, 2021 •

edited

tkmtnt7000 commented Nov 24, 2021

tkmtnt7000 commented Nov 24, 2021

708yamaguchi commented Nov 24, 2021

Affonso-Gui Oct 18, 2022

tkmtnt7000 Oct 18, 2022

Affonso-Gui Oct 18, 2022

tkmtnt7000 Oct 18, 2022

k-okada commented Oct 18, 2022

tkmtnt7000 commented Oct 18, 2022 •

edited

k-okada commented Oct 18, 2022 via email

tkmtnt7000 commented Oct 18, 2022

knorth55 commented Oct 18, 2022

tkmtnt7000 commented Oct 18, 2022

tkmtnt7000 commented Oct 19, 2022 •

edited

tkmtnt7000 commented Oct 19, 2022

tkmtnt7000 commented Oct 20, 2022

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

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

Conversation

tkmtnt7000 commented Jun 11, 2021

708yamaguchi left a comment • edited

Choose a reason for hiding this comment

Affonso-Gui left a comment • edited

Choose a reason for hiding this comment

k-okada commented Nov 19, 2021

708yamaguchi commented Nov 22, 2021 • edited

Topic type

Success result

Abort result

Preempted result

708yamaguchi commented Nov 22, 2021 • edited

Success result

Abort result

Preempted result

708yamaguchi commented Nov 22, 2021 • edited

Success result

Abort result

Preempted result

tkmtnt7000 commented Nov 22, 2021

tkmtnt7000 commented Nov 24, 2021 • edited

tkmtnt7000 commented Nov 24, 2021

tkmtnt7000 commented Nov 24, 2021

708yamaguchi commented Nov 24, 2021

Affonso-Gui Oct 18, 2022

Choose a reason for hiding this comment

tkmtnt7000 Oct 18, 2022

Choose a reason for hiding this comment

Affonso-Gui Oct 18, 2022

Choose a reason for hiding this comment

tkmtnt7000 Oct 18, 2022

Choose a reason for hiding this comment

k-okada commented Oct 18, 2022

tkmtnt7000 commented Oct 18, 2022 • edited

k-okada commented Oct 18, 2022 via email

tkmtnt7000 commented Oct 18, 2022

knorth55 commented Oct 18, 2022

tkmtnt7000 commented Oct 18, 2022

tkmtnt7000 commented Oct 19, 2022 • edited

tkmtnt7000 commented Oct 19, 2022

tkmtnt7000 commented Oct 20, 2022

708yamaguchi left a comment •

edited

Affonso-Gui left a comment •

edited

708yamaguchi commented Nov 22, 2021 •

edited

708yamaguchi commented Nov 22, 2021 •

edited

708yamaguchi commented Nov 22, 2021 •

edited

tkmtnt7000 commented Nov 24, 2021 •

edited

tkmtnt7000 commented Oct 18, 2022 •

edited

tkmtnt7000 commented Oct 19, 2022 •

edited