Skip to content

Instantly share code, notes, and snippets.

@tkmtnt7000

tkmtnt7000/20221019_pr2-ri-test-simple.log Secret

Created October 19, 2022 03:02
Show Gist options
  • Save tkmtnt7000/a2cd600f1cee134fecbb5d456e1c3550 to your computer and use it in GitHub Desktop.
Save tkmtnt7000/a2cd600f1cee134fecbb5d456e1c3550 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
20221019_pr2-ri-test-simple.log
tsukamoto@tsukamoto-desktop-ryzen ~/tmp/pr2eus_ws/src/jsk_pr2eus/pr2eus/test (master=)
$ roseus pr2-ri-test-simple.l
configuring by "/opt/ros/noetic/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=:1
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 gnuplotlib ___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 0x564571962680[16374] --> 0x564571e457a0[32748] top=3dad
irtgl irtglc irtviewer
EusLisp 9.29( 1.2.5) for Linux64 created on ip-10-0-1-44(Mon Sep 26 10:47:40 PST 2022)
roseus ;; loading roseus("1.7.5") on euslisp((9.29 ip-10-0-1-44 Mon Sep 26 10:47:40 PST 2022 1.2.5))
eustf roseus_c_util
;; extending gcstack 0x564571e457a0[32738] --> 0x564573a1dbd0[65476] top=7fe1
[ WARN] [1666148210.452235461, 130.462000000]: [/base_controller/follow_joint_trajectory] action server is not found
[ WARN] [1666148210.453109350, 130.463000000]: goal=0, cancel=0, feedback=0, result=0
[ WARN] [1666148210.453131893, 130.463000000]: move-base-trajectory-action is not found
;; #<linear-joint #X564574c12630 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
TEST-NAME: test-angle-vector
now testing...
start testing [test-angle-vector]
[ WARN] [1666148211.309891387, 131.318000000]: topic /robot_interface_marker_array already advertised
[ WARN] [1666148211.311541484, 131.320000000]: topic /l_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148211.311561211, 131.320000000]: topic /l_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148211.314168463, 131.322000000]: topic /r_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148211.314183160, 131.323000000]: topic /r_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148211.316401016, 131.325000000]: topic /head_traj_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148211.316418168, 131.325000000]: topic /head_traj_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148211.319138149, 131.327000000]: topic /torso_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148211.319162565, 131.327000000]: topic /torso_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148211.324828138, 131.333000000]: topic /move_base/goal already advertised
[ WARN] [1666148211.324849158, 131.333000000]: topic /move_base/cancel already advertised
[ WARN] [1666148211.327388072, 131.336000000]: topic /base_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148211.327406085, 131.336000000]: topic /base_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148214.433609586, 134.437000000]: [/base_controller/follow_joint_trajectory] action server is not found
[ WARN] [1666148214.433665481, 134.437000000]: goal=0, cancel=0, feedback=0, result=0
[ WARN] [1666148214.433685177, 134.437000000]: move-base-trajectory-action is not found
[ WARN] [1666148214.438264005, 134.442000000]: topic /r_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148214.438286547, 134.442000000]: topic /r_gripper_controller/gripper_action/cancel already advertised
[ WARN] [1666148214.441039751, 134.445000000]: topic /l_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148214.441059678, 134.445000000]: topic /l_gripper_controller/gripper_action/cancel already advertised
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(6.08864) violate min-angle(11.5)
[ WARN] [1666148214.451994286, 134.456000000]: continuous joint (r_wrist_roll_joint) moves 180.086 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(6.08864) violate min-angle(11.5)
[ WARN] [1666148214.453378988, 134.457000000]: continuous joint (r_wrist_roll_joint) moves 180.086 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1666148214.453403745, 134.457000000]: original trajectory command :
[ WARN] [1666148214.453420746, 134.457000000]: : (#f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)) (2000)
[ WARN] [1666148214.453437297, 134.457000000]: current angle vector : #f(11.5 -0.027523 1.05345 -0.064024 -14.8824 0.198945 -20.3756 0.077152 -0.006434 1.05732 0.096124 -14.9297 -0.161161 -20.3039 -0.08556 -0.015879 0.001453)
[ WARN] [1666148214.453448538, 134.457000000]: new trajectory command : dvi = 2
[ WARN] [1666148214.453468095, 134.457000000]: : #f(30.75 29.9862 0.526724 34.968 -67.4412 10.0995 -25.1878 90.0386 -30.0032 37.5287 -34.9519 -67.4648 -10.0806 -25.152 89.9572 -0.007939 0.000726) 1000
[ WARN] [1666148214.453483744, 134.457000000]: : #f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) 1000
[ WARN] [1666148214.453496257, 134.457000000]: new trajectory command :
[ WARN] [1666148214.453515984, 134.457000000]: : (#f(30.75 29.9862 0.526724 34.968 -67.4412 10.0995 -25.1878 90.0386 -30.0032 37.5287 -34.9519 -67.4648 -10.0806 -25.152 89.9572 -0.007939 0.000726) #f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)) (1000 1000)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(6.08864) violate min-angle(11.5)
[ WARN] [1666148214.456940439, 134.461000000]: continuous joint (r_wrist_roll_joint) moves 180.086 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1666148214.456964124, 134.461000000]: original trajectory command :
[ WARN] [1666148214.456984481, 134.461000000]: : (#f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 0.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) #f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)) (2000 2000)
[ WARN] [1666148214.457000041, 134.461000000]: current angle vector : #f(11.5 -0.027523 1.05345 -0.064024 -14.8824 0.198945 -20.3756 0.077152 -0.006434 1.05732 0.096124 -14.9297 -0.161161 -20.3039 -0.08556 -0.015879 0.001453)
[ WARN] [1666148214.457011332, 134.461000000]: new trajectory command : dvi = 2
[ WARN] [1666148214.457029145, 134.461000000]: : #f(30.75 29.9862 0.526724 34.968 -67.4412 10.0995 -25.1878 90.0386 -30.0032 0.528658 -34.9519 -67.4648 -10.0806 -25.152 89.9572 -0.007939 0.000726) 1000
[ WARN] [1666148214.457043131, 134.461000000]: : #f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 0.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) 1000
[ WARN] [1666148214.457054632, 134.461000000]: new trajectory command :
[ WARN] [1666148214.457077565, 134.461000000]: : (#f(30.75 29.9862 0.526724 34.968 -67.4412 10.0995 -25.1878 90.0386 -30.0032 0.528658 -34.9519 -67.4648 -10.0806 -25.152 89.9572 -0.007939 0.000726) #f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 0.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) #f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)) (1000 1000 2000)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
[ WARN] [1666148214.458369654, 134.462000000]: [l_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.458388449, 134.462000000]: expected goal id 134462000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_1
[ WARN] [1666148214.458403547, 134.462000000]: received goal id 134458000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_0
[ WARN] [1666148214.459259343, 134.463000000]: [r_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.459274461, 134.463000000]: expected goal id 134462000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_1
[ WARN] [1666148214.459288858, 134.463000000]: received goal id 134459000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_0
[ WARN] [1666148214.459713540, 134.463000000]: [head_traj_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.459727967, 134.463000000]: expected goal id 134463000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_1
[ WARN] [1666148214.459741031, 134.463000000]: received goal id 134459000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_0
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
[ WARN] [1666148214.460343565, 134.464000000]: [torso_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.460358042, 134.464000000]: expected goal id 134463000000_/pr2_eus_interface_1666148206507538114_60096_torso_controller/follow_joint_trajectory_1
[ WARN] [1666148214.460371747, 134.464000000]: received goal id 134459000000_/pr2_eus_interface_1666148206507538114_60096_torso_controller/follow_joint_trajectory_0
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(6.08864) violate min-angle(11.5)
[ WARN] [1666148214.461796133, 134.466000000]: continuous joint (r_wrist_roll_joint) moves 180.086 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1666148214.461818214, 134.466000000]: original trajectory command :
[ WARN] [1666148214.461837490, 134.466000000]: : (#f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) #f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)) (2000 3000)
[ WARN] [1666148214.461854071, 134.466000000]: current angle vector : #f(11.5 -0.027523 1.05345 -0.064024 -14.8824 0.198945 -20.3756 0.077152 -0.006434 1.05732 0.096124 -14.9297 -0.161161 -20.3039 -0.08556 -0.015879 0.001453)
[ WARN] [1666148214.461865072, 134.466000000]: new trajectory command : dvi = 2
[ WARN] [1666148214.461886271, 134.466000000]: : #f(30.75 29.9862 37.5267 34.968 -67.4412 10.0995 -25.1878 90.0386 -30.0032 37.5287 -34.9519 -67.4648 -10.0806 -25.152 89.9572 -0.007939 0.000726) 1000
[ WARN] [1666148214.461904976, 134.466000000]: : #f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) 1000
[ WARN] [1666148214.461915976, 134.466000000]: new trajectory command :
[ WARN] [1666148214.461938989, 134.466000000]: : (#f(30.75 29.9862 37.5267 34.968 -67.4412 10.0995 -25.1878 90.0386 -30.0032 37.5287 -34.9519 -67.4648 -10.0806 -25.152 89.9572 -0.007939 0.000726) #f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) #f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)) (1000 1000 3000)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
[ WARN] [1666148214.463220048, 134.467000000]: [l_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.463235847, 134.467000000]: expected goal id 134467000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_2
[ WARN] [1666148214.463248090, 134.467000000]: received goal id 134462000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_1
[ WARN] [1666148214.464430325, 134.468000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X564577d58408> received wrong goal
[ WARN] [1666148214.464883750, 134.469000000]: [r_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.464898327, 134.469000000]: expected goal id 134467000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_2
[ WARN] [1666148214.464913185, 134.469000000]: received goal id 134462000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_1
[ WARN] [1666148214.465486915, 134.469000000]: [head_traj_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.465502153, 134.469000000]: expected goal id 134468000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_2
[ WARN] [1666148214.465515267, 134.469000000]: received goal id 134463000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_1
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
[ WARN] [1666148214.466151083, 134.470000000]: [torso_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.466168015, 134.470000000]: expected goal id 134469000000_/pr2_eus_interface_1666148206507538114_60096_torso_controller/follow_joint_trajectory_2
[ WARN] [1666148214.466181199, 134.470000000]: received goal id 134463000000_/pr2_eus_interface_1666148206507538114_60096_torso_controller/follow_joint_trajectory_1
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(6.08864) violate min-angle(11.5)
[ WARN] [1666148214.467720239, 134.471000000]: length of tms should be the same or smaller than avs
[ WARN] [1666148214.467783517, 134.472000000]: continuous joint (r_wrist_roll_joint) moves 180.086 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1666148214.467802833, 134.472000000]: original trajectory command :
[ WARN] [1666148214.467822860, 134.472000000]: : (#f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) #f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)) (1000 2000)
[ WARN] [1666148214.467838309, 134.472000000]: current angle vector : #f(11.5 -0.027523 1.05345 -0.064024 -14.8824 0.198945 -20.3756 0.077152 -0.006434 1.05732 0.096124 -14.9297 -0.161161 -20.3039 -0.08556 -0.015879 0.001453)
[ WARN] [1666148214.467848137, 134.472000000]: new trajectory command : dvi = 2
[ WARN] [1666148214.467863576, 134.472000000]: : #f(30.75 29.9862 37.5267 34.968 -67.4412 10.0995 -25.1878 90.0386 -30.0032 37.5287 -34.9519 -67.4648 -10.0806 -25.152 89.9572 -0.007939 0.000726) 500
[ WARN] [1666148214.467879305, 134.472000000]: : #f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) 500
[ WARN] [1666148214.467890887, 134.472000000]: new trajectory command :
[ WARN] [1666148214.467914040, 134.472000000]: : (#f(30.75 29.9862 37.5267 34.968 -67.4412 10.0995 -25.1878 90.0386 -30.0032 37.5287 -34.9519 -67.4648 -10.0806 -25.152 89.9572 -0.007939 0.000726) #f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0) #f(50.0 60.0 0.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)) (500 500 2000)
;; #<linear-joint #X564573f2fa58 torso_lift_joint> :joint-angle(11.4981) violate min-angle(11.5)
[ WARN] [1666148214.470066123, 134.474000000]: [l_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.470082674, 134.474000000]: expected goal id 134473000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_3
[ WARN] [1666148214.470091891, 134.474000000]: received goal id 134467000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_2
[ WARN] [1666148214.470956243, 134.475000000]: [r_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148214.470969888, 134.475000000]: expected goal id 134474000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_3
[ WARN] [1666148214.470983153, 134.475000000]: received goal id 134467000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_2
TEST-NAME: test-angle-vector-limb
now testing...
start testing [test-angle-vector-limb]
[ WARN] [1666148214.841381134, 134.844000000]: topic /robot_interface_marker_array already advertised
[ WARN] [1666148214.843212198, 134.846000000]: topic /l_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148214.843230001, 134.846000000]: topic /l_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148214.845708382, 134.849000000]: topic /r_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148214.845724713, 134.849000000]: topic /r_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148214.847895831, 134.851000000]: topic /head_traj_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148214.847911811, 134.851000000]: topic /head_traj_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148214.850446607, 134.853000000]: topic /torso_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148214.850469119, 134.853000000]: topic /torso_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148214.856208591, 134.859000000]: topic /move_base/goal already advertised
[ WARN] [1666148214.856229250, 134.859000000]: topic /move_base/cancel already advertised
[ WARN] [1666148214.859055770, 134.862000000]: topic /base_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148214.859071479, 134.862000000]: topic /base_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148217.964454757, 137.964000000]: [/base_controller/follow_joint_trajectory] action server is not found
[ WARN] [1666148217.964494431, 137.964000000]: goal=0, cancel=0, feedback=0, result=0
[ WARN] [1666148217.964506653, 137.964000000]: move-base-trajectory-action is not found
[ WARN] [1666148217.968169824, 137.968000000]: topic /r_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148217.968186715, 137.968000000]: topic /r_gripper_controller/gripper_action/cancel already advertised
[ WARN] [1666148217.970381307, 137.970000000]: topic /l_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148217.970398729, 137.970000000]: topic /l_gripper_controller/gripper_action/cancel already advertised
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-126.027) violate min-angle(-121.542)
;; #<rotational-joint #X56457a965780 r_shoulder_lift_joint> :joint-angle(78.0508) violate max-angle(74.2725)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-125.835) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
[ WARN] [1666148217.987622545, 137.987000000]: [l_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148217.987639397, 137.987000000]: expected goal id 137987000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_0
[ WARN] [1666148217.987661668, 137.987000000]: received goal id 134473000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_3
[ WARN] [1666148217.987795648, 137.987000000]: [l_arm_controller/follow_joint_trajectory] result-cb 134473000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_3 -> state ros::*comm-state-done*
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-126.027) violate min-angle(-121.542)
;; #<rotational-joint #X56457a965780 r_shoulder_lift_joint> :joint-angle(78.0508) violate max-angle(74.2725)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-125.835) violate min-angle(-121.542)
;; #<rotational-joint #X56457a1f3370 r_elbow_flex_joint> :joint-angle(-121.61) violate min-angle(-121.542)
;; #<rotational-joint #X56457815cf70 l_elbow_flex_joint> :joint-angle(-121.583) violate min-angle(-121.542)
TEST-NAME: test-angle-vector-duration
now testing...
start testing [test-angle-vector-duration]
[ WARN] [1666148218.311690584, 138.311000000]: topic /robot_interface_marker_array already advertised
[ WARN] [1666148218.313179971, 138.312000000]: topic /l_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148218.313193626, 138.312000000]: topic /l_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148218.315186733, 138.314000000]: topic /r_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148218.315198555, 138.314000000]: topic /r_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148218.317172826, 138.316000000]: topic /head_traj_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148218.317185149, 138.316000000]: topic /head_traj_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148218.319138381, 138.318000000]: topic /torso_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148218.319150463, 138.318000000]: topic /torso_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148218.323477211, 138.323000000]: topic /move_base/goal already advertised
[ WARN] [1666148218.323489594, 138.323000000]: topic /move_base/cancel already advertised
[ WARN] [1666148218.325189163, 138.324000000]: topic /base_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148218.325204772, 138.324000000]: topic /base_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148219.630331949, 139.628000000]: [head_traj_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148219.630368046, 139.628000000]: expected goal id nil
[ WARN] [1666148219.630381291, 139.628000000]: received goal id 134474000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_3
[ WARN] [1666148219.630567207, 139.628000000]: [head_traj_controller/follow_joint_trajectory] result-cb 134474000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_3 -> state ros::*comm-state-done*
[ WARN] [1666148219.630660852, 139.628000000]: [torso_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148219.630672063, 139.628000000]: expected goal id nil
[ WARN] [1666148219.630685648, 139.628000000]: received goal id 134475000000_/pr2_eus_interface_1666148206507538114_60096_torso_controller/follow_joint_trajectory_3
[ WARN] [1666148219.630813186, 139.628000000]: [torso_controller/follow_joint_trajectory] result-cb 134475000000_/pr2_eus_interface_1666148206507538114_60096_torso_controller/follow_joint_trajectory_3 -> state ros::*comm-state-done*
[ WARN] [1666148219.630893386, 139.628000000]: [r_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148219.630904186, 139.628000000]: expected goal id nil
[ WARN] [1666148219.630913062, 139.628000000]: received goal id 134474000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_3
[ WARN] [1666148219.631046110, 139.629000000]: [r_arm_controller/follow_joint_trajectory] result-cb 134474000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_3 -> state ros::*comm-state-done*
[ WARN] [1666148221.429307978, 141.426000000]: [/base_controller/follow_joint_trajectory] action server is not found
[ WARN] [1666148221.429373210, 141.426000000]: goal=0, cancel=0, feedback=0, result=0
[ WARN] [1666148221.429387095, 141.426000000]: move-base-trajectory-action is not found
[ WARN] [1666148221.433106240, 141.429000000]: topic /r_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148221.433123352, 141.429000000]: topic /r_gripper_controller/gripper_action/cancel already advertised
[ WARN] [1666148221.435318655, 141.432000000]: topic /l_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148221.435333974, 141.432000000]: topic /l_gripper_controller/gripper_action/cancel already advertised
[ WARN] [1666148221.447865310, 141.444000000]: [l_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148221.447881410, 141.444000000]: expected goal id 141444000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_0
[ WARN] [1666148221.447891569, 141.444000000]: received goal id 137990000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_1
[ WARN] [1666148221.448055715, 141.444000000]: [l_arm_controller/follow_joint_trajectory] result-cb 137990000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_1 -> state ros::*comm-state-done*
[ INFO] [1666148221.448714032, 141.445000000]: wait-interpolation debug: start
[ INFO] [1666148224.475568259, 144.464000000]: wait-interpolation debug: end
TEST-NAME: test-go-pos
now testing...
start testing [test-go-pos]
[ WARN] [1666148224.803863465, 144.792000000]: topic /robot_interface_marker_array already advertised
[ WARN] [1666148224.805599202, 144.794000000]: topic /l_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148224.805618398, 144.794000000]: topic /l_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148224.807682717, 144.796000000]: topic /r_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148224.807697043, 144.796000000]: topic /r_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148224.809720406, 144.798000000]: topic /head_traj_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148224.809738300, 144.798000000]: topic /head_traj_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148224.812185613, 144.800000000]: topic /torso_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148224.812202475, 144.800000000]: topic /torso_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148224.816182405, 144.804000000]: topic /move_base/goal already advertised
[ WARN] [1666148224.816196101, 144.804000000]: topic /move_base/cancel already advertised
[ WARN] [1666148224.818040580, 144.806000000]: topic /base_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148224.818054436, 144.806000000]: topic /base_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148227.922002606, 147.908000000]: [/base_controller/follow_joint_trajectory] action server is not found
[ WARN] [1666148227.922045656, 147.908000000]: goal=0, cancel=0, feedback=0, result=0
[ WARN] [1666148227.922061095, 147.908000000]: move-base-trajectory-action is not found
[ WARN] [1666148227.926010349, 147.912000000]: topic /r_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148227.926027531, 147.912000000]: topic /r_gripper_controller/gripper_action/cancel already advertised
[ WARN] [1666148227.928090157, 147.914000000]: topic /l_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148227.928104454, 147.914000000]: topic /l_gripper_controller/gripper_action/cancel already advertised
[ INFO] [1666148227.930266896, 147.916000000]: move-to : send-goal to #f(1000.25 1.34799 -4.35684) at map (0)
[ INFO] [1666148234.144528234, 154.122000000]: move-to : succeeded
[ WARN] [1666148234.244964142, 154.222000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148234.245095457, 154.222000000]: :move-to current-coords #<cascaded-coords #X56457b947548 map 681.902 1.963 -4.689 / 0.009 -0.0 1.332e-06>
[ WARN] [1666148234.245139479, 154.222000000]: mapgoal-coords #<coordinates #X56457b947a70 1000.249 1.348 -4.357 / 0.001 -0.0 -2.416e-06>
[ WARN] [1666148234.245161971, 154.222000000]: error-coords #<coordinates #X56457d349480 318.33 -3.414 0.281 / -0.007 -1.204e-06 -4.917e-06>
[ WARN] [1666148234.245186006, 154.222000000]: target-coords #<coordinates #X56457fc7b958 1000.0 0.0 0.0 / 0.0 0.0 0.0>
[ERROR] [1666148234.245216673, 154.222000000]: too far from goal position 318.348 mm (> 200mm)
[ERROR] [1666148234.245231541, 154.222000000]: :move-to try to send /move_base_simple/goal
[ERROR] test (send *ri* :go-pos 1 0 0) failed ... ((send *ri* :go-pos 1 0 0) ^[0mmstart testing [test-go-pos]
ctor-duration]
w_flex_joint&gt; :joint-angle(-121.583) violate min-angle(-121.542)
�qEV).
[ INFO] [1666148237.246635921, 157.222000000]: move-to : send-goal to #f(673.206 1001.92 -4.51571) at map (0)
[ INFO] [1666148248.460103950, 168.423000000]: move-to : succeeded
[ WARN] [1666148248.559546961, 168.523000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148248.559660853, 168.523000000]: :move-to current-coords #<cascaded-coords #X56457fe2e248 map 897.369 887.979 -4.348 / 1.802 -0.0 -3.155e-06>
[ WARN] [1666148248.559710766, 168.523000000]: mapgoal-coords #<coordinates #X56457fde15e0 673.206 1001.923 -4.516 / 1.58 -1.045e-06 -0.0>
[ WARN] [1666148248.559736765, 168.523000000]: error-coords #<coordinates #X56457fde1a48 162.194 192.16 -0.184 / -0.222 0.0 -0.0>
[ WARN] [1666148248.559759006, 168.523000000]: target-coords #<coordinates #X56457b921e68 0.0 1000.0 0.0 / 1.571 0.0 0.0>
[ERROR] [1666148248.559784854, 168.523000000]: too far from goal position 251.46 mm (> 200mm)
[ERROR] [1666148248.559801405, 168.523000000]: :move-to try to send /move_base_simple/goal
[ WARN] [1666148248.559819459, 168.523000000]: topic /move_base_simple/goal already advertised
[ERROR] test (send *ri* :go-pos 0 1 90) failed ... ((send *ri* :go-pos 0 1 90) ]
w_flex_joint&amp;gt; :joint-angle(-121.583) violate min-angle(-121.542)
�qEV).^[0m
^[0mmstart testing [test-go-pos]
ctor-duration).
[ INFO] [1666148251.560948341, 171.519000000]: move-to : send-goal to #f(152.089 -314.092 -4.64216) at map (0)
[ WARN] [1666148251.664605570, 171.623000000]: [move_base] action-result-cb may received old client's goal
[ WARN] [1666148251.664641417, 171.623000000]: expected goal id 171519000000_/pr2_eus_interface_1666148206507538114_60096_move_base_2
[ WARN] [1666148251.664661946, 171.623000000]: received goal id /move_base_node-1-168.523000000
[ INFO] [1666148273.102335603, 193.029000000]: move-to : succeeded
[ WARN] [1666148273.202719825, 193.130000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148273.202840270, 193.130000000]: :move-to current-coords #<cascaded-coords #X56457e163030 map 56.446 -141.035 -4.493 / 0.174 -0.0 -4.592e-05>
[ WARN] [1666148273.202881096, 193.130000000]: mapgoal-coords #<coordinates #X56457e163618 152.089 -314.092 -4.642 / 0.23 -4.020e-06 0.0>
[ WARN] [1666148273.202901454, 193.130000000]: error-coords #<coordinates #X56457fd2e818 64.302 -186.98 -0.166 / 0.057 0.0 0.0>
[ WARN] [1666148273.202919127, 193.130000000]: target-coords #<coordinates #X5645803869d8 -1000.0 1000.0 0.0 / -1.571 0.0 0.0>
[ WARN] [1666148273.202944685, 193.130000000]: :move-to -> :go-velocity x:0.03252 y:-0.094564 d:0.028705 msec:1977.27
[ WARN] [1666148275.650002713, 195.573000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148275.650104423, 195.573000000]: :move-to current-coords #<cascaded-coords #X564573980a08 map 57.618 -144.663 -5.648 / 0.184 -0.0 -6.291e-06>
[ WARN] [1666148275.650132696, 195.573000000]: mapgoal-coords #<coordinates #X56457370da88 152.089 -314.092 -4.642 / 0.23 -4.020e-06 0.0>
[ WARN] [1666148275.650190854, 195.573000000]: error-coords #<coordinates #X564573729970 61.916 -183.841 0.998 / 0.047 0.0 0.0>
[ WARN] [1666148275.650219437, 195.573000000]: target-coords #<coordinates #X5645803869d8 -1000.0 1000.0 0.0 / -1.571 0.0 0.0>
[ WARN] [1666148275.650251096, 195.573000000]: :move-to -> :go-velocity x:0.031917 y:-0.09477 d:0.024036 msec:1939.87
[ WARN] [1666148278.092980588, 198.014000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148278.093107424, 198.014000000]: :move-to current-coords #<cascaded-coords #X56457c871c70 map 114.403 -244.156 -7.406 / 0.212 -7.695e-05 9.965e-05>
[ WARN] [1666148278.093135837, 198.014000000]: mapgoal-coords #<coordinates #X56457ca69998 152.089 -314.092 -4.642 / 0.23 -4.020e-06 0.0>
[ WARN] [1666148278.093157908, 198.014000000]: error-coords #<coordinates #X56457cea7868 22.154 -76.292 2.77 / 0.019 7.479e-05 1.977e-05>
[ WARN] [1666148278.093177355, 198.014000000]: target-coords #<coordinates #X5645803869d8 -1000.0 1000.0 0.0 / -1.571 0.0 0.0>
[ INFO] [1666148278.093239050, 198.014000000]: go-pos-unsafe (x y d) = (-1 1 -90)
[ WARN] [1666148278.093255881, 198.014000000]: pseudo :go-pose-unsafe-no-wait is called because move-base-trajectory-action is not found
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2913) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2913) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2913) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2913) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2913) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2769) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2769) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2769) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2769) violate max-angle(74.2725)
;; #<rotational-joint #X5645790b18b0 l_shoulder_lift_joint> :joint-angle(74.2769) violate max-angle(74.2725)
TEST-NAME: test-move-to
now testing...
start testing [test-move-to]
[ WARN] [1666148292.454735880, 212.357000000]: topic /robot_interface_marker_array already advertised
[ WARN] [1666148292.456261325, 212.358000000]: topic /l_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148292.456278507, 212.358000000]: topic /l_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148292.458343137, 212.360000000]: topic /r_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148292.458357083, 212.360000000]: topic /r_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148292.460438955, 212.362000000]: topic /head_traj_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148292.460455185, 212.362000000]: topic /head_traj_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148292.462440697, 212.364000000]: topic /torso_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148292.462454223, 212.364000000]: topic /torso_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148292.467053279, 212.369000000]: topic /move_base/goal already advertised
[ WARN] [1666148292.467071083, 212.369000000]: topic /move_base/cancel already advertised
[ WARN] [1666148292.469367926, 212.371000000]: topic /base_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148292.469390538, 212.371000000]: topic /base_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148295.579341306, 215.474000000]: [/base_controller/follow_joint_trajectory] action server is not found
[ WARN] [1666148295.579393213, 215.474000000]: goal=0, cancel=0, feedback=0, result=0
[ WARN] [1666148295.579408131, 215.474000000]: move-base-trajectory-action is not found
[ WARN] [1666148295.583009327, 215.477000000]: topic /r_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148295.583030927, 215.477000000]: topic /r_gripper_controller/gripper_action/cancel already advertised
[ WARN] [1666148295.585150460, 215.479000000]: topic /l_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148295.585172862, 215.479000000]: topic /l_gripper_controller/gripper_action/cancel already advertised
[ INFO] [1666148295.587354870, 215.482000000]: move-to : send-goal to #f(1000.0 0.0 0.0) at map (0)
[ INFO] [1666148309.620147176, 229.491000000]: move-to : succeeded
[ WARN] [1666148309.720501362, 229.591000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148309.720640741, 229.591000000]: :move-to current-coords #<cascaded-coords #X5645783006f0 map 687.971 19.289 -4.597 / -0.065 -0.0 -2.266e-06>
[ WARN] [1666148309.720660619, 229.591000000]: mapgoal-coords #<cascaded-coords #X5645783382b8 map 1000.0 0.0 0.0 / 0.0 0.0 0.0>
[ WARN] [1666148309.720680876, 229.591000000]: error-coords #<coordinates #X564577f99f68 312.624 0.897 4.54 / 0.065 0.0 1.410e-05>
[ WARN] [1666148309.720695984, 229.591000000]: target-coords #<coordinates #X564579f75ce0 1000.0 0.0 0.0 / 0.0 0.0 0.0>
[ERROR] [1666148309.720716683, 229.591000000]: too far from goal position 312.626 mm (> 200mm)
[ERROR] [1666148309.720733124, 229.591000000]: :move-to try to send /move_base_simple/goal
[ WARN] [1666148309.720746559, 229.591000000]: topic /move_base_simple/goal already advertised
[ERROR] test (send *ri* :move-to (make-coords :pos #f(1000.0 0.0 0.0))) failed ... ((send *ri* :move-to (make-coords :pos #f(1000 0 0))) ^[0mmstart testing [test-move-to]
90b18b0 l_shoulder_lift_joint&gt; :joint-angle(74.2769) violate max-angle(74.2725)
angle(-121.583).
[ INFO] [1666148312.721659542, 232.590000000]: move-to : send-goal to #f(1000.0 1000.0 0.0) at map (0)
[ WARN] [1666148312.724701234, 232.593000000]: [move_base] action-result-cb may received old client's goal
[ WARN] [1666148312.724724127, 232.593000000]: expected goal id 232590000000_/pr2_eus_interface_1666148206507538114_60096_move_base_1
[ WARN] [1666148312.724736490, 232.593000000]: received goal id /move_base_node-2-229.591000000
[ INFO] [1666148324.737549835, 244.592000000]: move-to : succeeded
[ WARN] [1666148324.836923826, 244.692000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148324.837019194, 244.692000000]: :move-to current-coords #<cascaded-coords #X564581e8b7b8 map 1143.305 844.331 -4.503 / 1.486 -0.0 4.734e-05>
[ WARN] [1666148324.837044030, 244.692000000]: mapgoal-coords #<cascaded-coords #X564581e8f418 map 1000.0 1000.0 0.0 / 1.571 0.0 0.0>
[ WARN] [1666148324.837070109, 244.692000000]: error-coords #<coordinates #X564581e8bda0 142.945 156.001 4.472 / 0.085 0.0 -3.289e-05>
[ WARN] [1666148324.837093382, 244.692000000]: target-coords #<coordinates #X564578075a78 1000.0 1000.0 0.0 / 1.571 0.0 0.0>
[ERROR] [1666148324.837116495, 244.692000000]: too far from goal position 211.588 mm (> 200mm)
[ERROR] [1666148324.837132796, 244.692000000]: :move-to try to send /move_base_simple/goal
[ WARN] [1666148324.837149888, 244.692000000]: topic /move_base_simple/goal already advertised
[ INFO] [1666148327.837510459, 247.690000000]: move-to : send-goal to #f(0.0 0.0 0.0) at map (0)
[ WARN] [1666148327.840410247, 247.693000000]: [move_base] action-result-cb may received old client's goal
[ WARN] [1666148327.840436606, 247.693000000]: expected goal id 247690000000_/pr2_eus_interface_1666148206507538114_60096_move_base_2
[ WARN] [1666148327.840450161, 247.693000000]: received goal id /move_base_node-3-244.692000000
[ INFO] [1666148351.186976426, 271.001000000]: move-to : succeeded
[ WARN] [1666148351.237786685, 271.051000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148351.237937917, 271.052000000]: :move-to current-coords #<cascaded-coords #X56457e49a310 map 75.603 184.542 -4.443 / -0.04 -0.0 7.641e-05>
[ WARN] [1666148351.237963084, 271.052000000]: mapgoal-coords #<cascaded-coords #X56457e5ca8c8 map 0.0 0.0 0.0 / 0.0 0.0 0.0>
[ WARN] [1666148351.237987469, 271.052000000]: error-coords #<coordinates #X56457e5c7250 -68.185 -187.409 4.465 / 0.04 0.0 -7.213e-05>
[ WARN] [1666148351.238004060, 271.052000000]: target-coords #<coordinates #X564581e8aa38 0.0 0.0 0.0 / 0.0 0.0 0.0>
[ WARN] [1666148351.238036941, 271.052000000]: :move-to -> :go-velocity x:-0.034191 y:-0.093973 d:0.019997 msec:1994.27
[ WARN] [1666148353.735646430, 273.547000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148353.735749242, 273.547000000]: :move-to current-coords #<cascaded-coords #X56457d8fbee8 map 56.044 137.72 -2.486 / -0.049 -0.0 5.444e-05>
[ WARN] [1666148353.735775681, 273.547000000]: mapgoal-coords #<cascaded-coords #X56457d8f6b00 map 0.0 0.0 0.0 / 0.0 0.0 0.0>
[ WARN] [1666148353.735793023, 273.547000000]: error-coords #<coordinates #X56457d5ae910 -49.182 -140.317 2.499 / 0.049 0.0 -4.904e-05>
[ WARN] [1666148353.735806789, 273.547000000]: target-coords #<coordinates #X564581e8aa38 0.0 0.0 0.0 / 0.0 0.0 0.0>
[ WARN] [1666148353.735826005, 273.547000000]: :move-to -> :go-velocity x:-0.033078 y:-0.094371 d:0.033189 msec:1486.87
[ WARN] [1666148355.776762055, 275.586000000]: :move-to wait-for transform map to base_footprint -> t
[ WARN] [1666148355.776875577, 275.586000000]: :move-to current-coords #<cascaded-coords #X564580642e60 map 22.57 52.623 -3.172 / -0.019 -0.0 9.020e-05>
[ WARN] [1666148355.776907727, 275.586000000]: mapgoal-coords #<cascaded-coords #X564580646ac0 map 0.0 0.0 0.0 / 0.0 0.0 0.0>
[ WARN] [1666148355.776935879, 275.586000000]: error-coords #<coordinates #X5645806435c8 -21.554 -53.047 3.18 / 0.019 0.0 -8.766e-05>
[ WARN] [1666148355.776965114, 275.586000000]: target-coords #<coordinates #X564581e8aa38 0.0 0.0 0.0 / 0.0 0.0 0.0>
TEST-NAME: test-wrong-controller
now testing...
start testing [test-wrong-controller]
[ WARN] [1666148356.079547173, 275.889000000]: topic /robot_interface_marker_array already advertised
[ WARN] [1666148356.081337151, 275.891000000]: topic /l_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148356.081356647, 275.891000000]: topic /l_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148356.083509212, 275.893000000]: topic /r_arm_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148356.083524120, 275.893000000]: topic /r_arm_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148356.085687314, 275.895000000]: topic /head_traj_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148356.085709044, 275.895000000]: topic /head_traj_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148356.087661956, 275.897000000]: topic /torso_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148356.087681232, 275.897000000]: topic /torso_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148356.092839212, 275.902000000]: topic /move_base/goal already advertised
[ WARN] [1666148356.092855823, 275.902000000]: topic /move_base/cancel already advertised
[ WARN] [1666148356.094749384, 275.904000000]: topic /base_controller/follow_joint_trajectory/goal already advertised
[ WARN] [1666148356.094764622, 275.904000000]: topic /base_controller/follow_joint_trajectory/cancel already advertised
[ WARN] [1666148359.199176270, 279.007000000]: [/base_controller/follow_joint_trajectory] action server is not found
[ WARN] [1666148359.199231032, 279.007000000]: goal=0, cancel=0, feedback=0, result=0
[ WARN] [1666148359.199244888, 279.007000000]: move-base-trajectory-action is not found
[ WARN] [1666148359.203394176, 279.011000000]: topic /r_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148359.203413231, 279.011000000]: topic /r_gripper_controller/gripper_action/cancel already advertised
[ WARN] [1666148359.205593207, 279.013000000]: topic /l_gripper_controller/gripper_action/goal already advertised
[ WARN] [1666148359.205608796, 279.013000000]: topic /l_gripper_controller/gripper_action/cancel already advertised
[ WARN] [1666148359.216204597, 279.024000000]: continuous joint (l_wrist_roll_joint) moves -190.0 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1666148359.216253939, 279.024000000]: continuous joint (r_wrist_roll_joint) moves -190.0 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1666148359.217661504, 279.025000000]: continuous joint (l_wrist_roll_joint) moves -190.0 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1666148359.217699084, 279.025000000]: continuous joint (r_wrist_roll_joint) moves -190.0 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
[ WARN] [1666148359.217715615, 279.025000000]: original trajectory command :
[ WARN] [1666148359.217734731, 279.025000000]: : (#f(300.0 75.0 50.0 110.0 -110.0 -20.0 -10.0 -10.0 -75.0 50.0 -110.0 -110.0 20.0 -10.0 -10.0 0.0 50.0)) (3000)
[ WARN] [1666148359.217748467, 279.025000000]: current angle vector : #f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)
[ WARN] [1666148359.217755299, 279.025000000]: new trajectory command : dvi = 2
[ WARN] [1666148359.217772952, 279.025000000]: : #f(175.0 67.5 62.0 90.0 -115.0 -1.421085e-14 -20.0 85.0 -67.5 62.0 -90.0 -115.0 7.105427e-15 -20.0 85.0 0.0 25.0) 1500
[ WARN] [1666148359.217796005, 279.025000000]: : #f(300.0 75.0 50.0 110.0 -110.0 -20.0 -10.0 -10.0 -75.0 50.0 -110.0 -110.0 20.0 -10.0 -10.0 0.0 50.0) 1500
[ WARN] [1666148359.217803710, 279.025000000]: new trajectory command :
[ WARN] [1666148359.217819489, 279.025000000]: : (#f(175.0 67.5 62.0 90.0 -115.0 -1.421085e-14 -20.0 85.0 -67.5 62.0 -90.0 -115.0 7.105427e-15 -20.0 85.0 0.0 25.0) #f(300.0 75.0 50.0 110.0 -110.0 -20.0 -10.0 -10.0 -75.0 50.0 -110.0 -110.0 20.0 -10.0 -10.0 0.0 50.0)) (1500 1500)
[robot-interface.l] (angle-vector-sequence) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil) or joint-id nil (av length 17)
[robot-interface.l] (angle-vector-sequence) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil) or joint-id nil (av length 17)
[robot-interface.l] (send-ros-controller) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil)
[robot-interface.l] (angle-vector-sequence) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil) or joint-id nil (av length 17)
[ WARN] [1666148359.288123913, 279.095000000]: [l_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148359.288159058, 279.095000000]: expected goal id 279095000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_1
[ WARN] [1666148359.288176000, 279.095000000]: received goal id 279026000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_0
[robot-interface.l] (send-ros-controller) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil)
TEST-NAME: test-sub-angle-vector-1000
now testing...
start testing [test-sub-angle-vector-1000]
:sub-angle-vector : original angle-vector from 0 to 0.0, expected 0.0 t
:sub-angle-vector : original angle-vector from 10 to 10.0, expected 10.0 t
:sub-angle-vector : original angle-vector from 20 to 20.0, expected 20.0 t
:sub-angle-vector : original angle-vector from 30 to 30.0, expected 30.0 t
:sub-angle-vector : original angle-vector from 40 to 40.0, expected 40.0 t
:sub-angle-vector : original angle-vector from 50 to 50.0, expected 50.0 t
:sub-angle-vector : original angle-vector from 60 to 60.0, expected 60.0 t
:sub-angle-vector : original angle-vector from 70 to 70.0, expected 70.0 t
:sub-angle-vector : original angle-vector from 80 to 80.0, expected 80.0 t
:sub-angle-vector : original angle-vector from 90 to 90.0, expected 90.0 t
:sub-angle-vector : original angle-vector from 100 to 100.0, expected 100.0 t
:sub-angle-vector : original angle-vector from 110 to 110.0, expected 110.0 t
:sub-angle-vector : original angle-vector from 120 to 120.0, expected 120.0 t
:sub-angle-vector : original angle-vector from 130 to 130.0, expected 130.0 t
:sub-angle-vector : original angle-vector from 140 to 140.0, expected 140.0 t
:sub-angle-vector : original angle-vector from 150 to 150.0, expected 150.0 t
:sub-angle-vector : original angle-vector from 160 to 160.0, expected 160.0 t
:sub-angle-vector : original angle-vector from 170 to 170.0, expected 170.0 t
:sub-angle-vector : original angle-vector from 180 to 180.0, expected 180.0 t
:sub-angle-vector : original angle-vector from 190 to -170.0, expected -170.0 t
:sub-angle-vector : original angle-vector from 200 to -160.0, expected -160.0 t
:sub-angle-vector : original angle-vector from 210 to -150.0, expected -150.0 t
:sub-angle-vector : original angle-vector from 220 to -140.0, expected -140.0 t
:sub-angle-vector : original angle-vector from 230 to -130.0, expected -130.0 t
:sub-angle-vector : original angle-vector from 240 to -120.0, expected -120.0 t
:sub-angle-vector : original angle-vector from 250 to -110.0, expected -110.0 t
:sub-angle-vector : original angle-vector from 260 to -100.0, expected -100.0 t
:sub-angle-vector : original angle-vector from 270 to -90.0, expected -90.0 t
:sub-angle-vector : original angle-vector from 280 to -80.0, expected -80.0 t
:sub-angle-vector : original angle-vector from 290 to -70.0, expected -70.0 t
:sub-angle-vector : original angle-vector from 300 to -60.0, expected -60.0 t
:sub-angle-vector : original angle-vector from 310 to -50.0, expected -50.0 t
:sub-angle-vector : original angle-vector from 320 to -40.0, expected -40.0 t
:sub-angle-vector : original angle-vector from 330 to -30.0, expected -30.0 t
:sub-angle-vector : original angle-vector from 340 to -20.0, expected -20.0 t
:sub-angle-vector : original angle-vector from 350 to -10.0, expected -10.0 t
:sub-angle-vector : original angle-vector from 360 to 0.0, expected 0.0 t
:sub-angle-vector : original angle-vector from 370 to 10.0, expected 10.0 t
:sub-angle-vector : original angle-vector from 380 to 20.0, expected 20.0 t
:sub-angle-vector : original angle-vector from 390 to 30.0, expected 30.0 t
:sub-angle-vector : original angle-vector from 400 to 40.0, expected 40.0 t
:sub-angle-vector : original angle-vector from 410 to 50.0, expected 50.0 t
:sub-angle-vector : original angle-vector from 420 to 60.0, expected 60.0 t
:sub-angle-vector : original angle-vector from 430 to 70.0, expected 70.0 t
:sub-angle-vector : original angle-vector from 440 to 80.0, expected 80.0 t
:sub-angle-vector : original angle-vector from 450 to 90.0, expected 90.0 t
:sub-angle-vector : original angle-vector from 460 to 100.0, expected 100.0 t
:sub-angle-vector : original angle-vector from 470 to 110.0, expected 110.0 t
:sub-angle-vector : original angle-vector from 480 to 120.0, expected 120.0 t
:sub-angle-vector : original angle-vector from 490 to 130.0, expected 130.0 t
:sub-angle-vector : original angle-vector from 500 to 140.0, expected 140.0 t
:sub-angle-vector : original angle-vector from 510 to 150.0, expected 150.0 t
:sub-angle-vector : original angle-vector from 520 to 160.0, expected 160.0 t
:sub-angle-vector : original angle-vector from 530 to 170.0, expected 170.0 t
:sub-angle-vector : original angle-vector from 540 to 180.0, expected 180.0 t
:sub-angle-vector : original angle-vector from 550 to -170.0, expected -170.0 t
:sub-angle-vector : original angle-vector from 560 to -160.0, expected -160.0 t
:sub-angle-vector : original angle-vector from 570 to -150.0, expected -150.0 t
:sub-angle-vector : original angle-vector from 580 to -140.0, expected -140.0 t
:sub-angle-vector : original angle-vector from 590 to -130.0, expected -130.0 t
:sub-angle-vector : original angle-vector from 600 to -120.0, expected -120.0 t
:sub-angle-vector : original angle-vector from 610 to -110.0, expected -110.0 t
:sub-angle-vector : original angle-vector from 620 to -100.0, expected -100.0 t
:sub-angle-vector : original angle-vector from 630 to -90.0, expected -90.0 t
:sub-angle-vector : original angle-vector from 640 to -80.0, expected -80.0 t
:sub-angle-vector : original angle-vector from 650 to -70.0, expected -70.0 t
:sub-angle-vector : original angle-vector from 660 to -60.0, expected -60.0 t
:sub-angle-vector : original angle-vector from 670 to -50.0, expected -50.0 t
:sub-angle-vector : original angle-vector from 680 to -40.0, expected -40.0 t
:sub-angle-vector : original angle-vector from 690 to -30.0, expected -30.0 t
:sub-angle-vector : original angle-vector from 700 to -20.0, expected -20.0 t
:sub-angle-vector : original angle-vector from 710 to -10.0, expected -10.0 t
:sub-angle-vector : original angle-vector from 720 to 0.0, expected 0.0 t
:sub-angle-vector : original angle-vector from 730 to 10.0, expected 10.0 t
:sub-angle-vector : original angle-vector from 740 to 20.0, expected 20.0 t
:sub-angle-vector : original angle-vector from 750 to 30.0, expected 30.0 t
:sub-angle-vector : original angle-vector from 760 to 40.0, expected 40.0 t
:sub-angle-vector : original angle-vector from 770 to 50.0, expected 50.0 t
:sub-angle-vector : original angle-vector from 780 to 60.0, expected 60.0 t
:sub-angle-vector : original angle-vector from 790 to 70.0, expected 70.0 t
:sub-angle-vector : original angle-vector from 800 to 80.0, expected 80.0 t
:sub-angle-vector : original angle-vector from 810 to 90.0, expected 90.0 t
:sub-angle-vector : original angle-vector from 820 to 100.0, expected 100.0 t
:sub-angle-vector : original angle-vector from 830 to 110.0, expected 110.0 t
:sub-angle-vector : original angle-vector from 840 to 120.0, expected 120.0 t
:sub-angle-vector : original angle-vector from 850 to 130.0, expected 130.0 t
:sub-angle-vector : original angle-vector from 860 to 140.0, expected 140.0 t
:sub-angle-vector : original angle-vector from 870 to 150.0, expected 150.0 t
:sub-angle-vector : original angle-vector from 880 to 160.0, expected 160.0 t
:sub-angle-vector : original angle-vector from 890 to 170.0, expected 170.0 t
:sub-angle-vector : original angle-vector from 900 to 180.0, expected 180.0 t
:sub-angle-vector : original angle-vector from 910 to -170.0, expected -170.0 t
:sub-angle-vector : original angle-vector from 920 to -160.0, expected -160.0 t
:sub-angle-vector : original angle-vector from 930 to -150.0, expected -150.0 t
:sub-angle-vector : original angle-vector from 940 to -140.0, expected -140.0 t
:sub-angle-vector : original angle-vector from 950 to -130.0, expected -130.0 t
:sub-angle-vector : original angle-vector from 960 to -120.0, expected -120.0 t
:sub-angle-vector : original angle-vector from 970 to -110.0, expected -110.0 t
:sub-angle-vector : original angle-vector from 980 to -100.0, expected -100.0 t
:sub-angle-vector : original angle-vector from 990 to -90.0, expected -90.0 t
:sub-angle-vector : original angle-vector from 0 to 0.0, expected 0.0 t
:sub-angle-vector : original angle-vector from -10 to -10.0, expected -10.0 t
:sub-angle-vector : original angle-vector from -20 to -20.0, expected -20.0 t
:sub-angle-vector : original angle-vector from -30 to -30.0, expected -30.0 t
:sub-angle-vector : original angle-vector from -40 to -40.0, expected -40.0 t
:sub-angle-vector : original angle-vector from -50 to -50.0, expected -50.0 t
:sub-angle-vector : original angle-vector from -60 to -60.0, expected -60.0 t
:sub-angle-vector : original angle-vector from -70 to -70.0, expected -70.0 t
:sub-angle-vector : original angle-vector from -80 to -80.0, expected -80.0 t
:sub-angle-vector : original angle-vector from -90 to -90.0, expected -90.0 t
:sub-angle-vector : original angle-vector from -100 to -100.0, expected -100.0 t
:sub-angle-vector : original angle-vector from -110 to -110.0, expected -110.0 t
:sub-angle-vector : original angle-vector from -120 to -120.0, expected -120.0 t
:sub-angle-vector : original angle-vector from -130 to -130.0, expected -130.0 t
:sub-angle-vector : original angle-vector from -140 to -140.0, expected -140.0 t
:sub-angle-vector : original angle-vector from -150 to -150.0, expected -150.0 t
:sub-angle-vector : original angle-vector from -160 to -160.0, expected -160.0 t
:sub-angle-vector : original angle-vector from -170 to -170.0, expected -170.0 t
:sub-angle-vector : original angle-vector from -180 to -180.0, expected -180.0 t
:sub-angle-vector : original angle-vector from -190 to 170.0, expected 170.0 t
:sub-angle-vector : original angle-vector from -200 to 160.0, expected 160.0 t
:sub-angle-vector : original angle-vector from -210 to 150.0, expected 150.0 t
:sub-angle-vector : original angle-vector from -220 to 140.0, expected 140.0 t
:sub-angle-vector : original angle-vector from -230 to 130.0, expected 130.0 t
:sub-angle-vector : original angle-vector from -240 to 120.0, expected 120.0 t
:sub-angle-vector : original angle-vector from -250 to 110.0, expected 110.0 t
:sub-angle-vector : original angle-vector from -260 to 100.0, expected 100.0 t
:sub-angle-vector : original angle-vector from -270 to 90.0, expected 90.0 t
:sub-angle-vector : original angle-vector from -280 to 80.0, expected 80.0 t
:sub-angle-vector : original angle-vector from -290 to 70.0, expected 70.0 t
:sub-angle-vector : original angle-vector from -300 to 60.0, expected 60.0 t
:sub-angle-vector : original angle-vector from -310 to 50.0, expected 50.0 t
:sub-angle-vector : original angle-vector from -320 to 40.0, expected 40.0 t
:sub-angle-vector : original angle-vector from -330 to 30.0, expected 30.0 t
:sub-angle-vector : original angle-vector from -340 to 20.0, expected 20.0 t
:sub-angle-vector : original angle-vector from -350 to 10.0, expected 10.0 t
:sub-angle-vector : original angle-vector from -360 to 0.0, expected 0.0 t
:sub-angle-vector : original angle-vector from -370 to -10.0, expected -10.0 t
:sub-angle-vector : original angle-vector from -380 to -20.0, expected -20.0 t
:sub-angle-vector : original angle-vector from -390 to -30.0, expected -30.0 t
:sub-angle-vector : original angle-vector from -400 to -40.0, expected -40.0 t
:sub-angle-vector : original angle-vector from -410 to -50.0, expected -50.0 t
:sub-angle-vector : original angle-vector from -420 to -60.0, expected -60.0 t
:sub-angle-vector : original angle-vector from -430 to -70.0, expected -70.0 t
:sub-angle-vector : original angle-vector from -440 to -80.0, expected -80.0 t
:sub-angle-vector : original angle-vector from -450 to -90.0, expected -90.0 t
:sub-angle-vector : original angle-vector from -460 to -100.0, expected -100.0 t
:sub-angle-vector : original angle-vector from -470 to -110.0, expected -110.0 t
:sub-angle-vector : original angle-vector from -480 to -120.0, expected -120.0 t
:sub-angle-vector : original angle-vector from -490 to -130.0, expected -130.0 t
:sub-angle-vector : original angle-vector from -500 to -140.0, expected -140.0 t
:sub-angle-vector : original angle-vector from -510 to -150.0, expected -150.0 t
:sub-angle-vector : original angle-vector from -520 to -160.0, expected -160.0 t
:sub-angle-vector : original angle-vector from -530 to -170.0, expected -170.0 t
:sub-angle-vector : original angle-vector from -540 to -180.0, expected -180.0 t
:sub-angle-vector : original angle-vector from -550 to 170.0, expected 170.0 t
:sub-angle-vector : original angle-vector from -560 to 160.0, expected 160.0 t
:sub-angle-vector : original angle-vector from -570 to 150.0, expected 150.0 t
:sub-angle-vector : original angle-vector from -580 to 140.0, expected 140.0 t
:sub-angle-vector : original angle-vector from -590 to 130.0, expected 130.0 t
:sub-angle-vector : original angle-vector from -600 to 120.0, expected 120.0 t
:sub-angle-vector : original angle-vector from -610 to 110.0, expected 110.0 t
:sub-angle-vector : original angle-vector from -620 to 100.0, expected 100.0 t
:sub-angle-vector : original angle-vector from -630 to 90.0, expected 90.0 t
:sub-angle-vector : original angle-vector from -640 to 80.0, expected 80.0 t
:sub-angle-vector : original angle-vector from -650 to 70.0, expected 70.0 t
:sub-angle-vector : original angle-vector from -660 to 60.0, expected 60.0 t
:sub-angle-vector : original angle-vector from -670 to 50.0, expected 50.0 t
:sub-angle-vector : original angle-vector from -680 to 40.0, expected 40.0 t
:sub-angle-vector : original angle-vector from -690 to 30.0, expected 30.0 t
:sub-angle-vector : original angle-vector from -700 to 20.0, expected 20.0 t
:sub-angle-vector : original angle-vector from -710 to 10.0, expected 10.0 t
:sub-angle-vector : original angle-vector from -720 to 0.0, expected 0.0 t
:sub-angle-vector : original angle-vector from -730 to -10.0, expected -10.0 t
:sub-angle-vector : original angle-vector from -740 to -20.0, expected -20.0 t
:sub-angle-vector : original angle-vector from -750 to -30.0, expected -30.0 t
:sub-angle-vector : original angle-vector from -760 to -40.0, expected -40.0 t
:sub-angle-vector : original angle-vector from -770 to -50.0, expected -50.0 t
:sub-angle-vector : original angle-vector from -780 to -60.0, expected -60.0 t
:sub-angle-vector : original angle-vector from -790 to -70.0, expected -70.0 t
:sub-angle-vector : original angle-vector from -800 to -80.0, expected -80.0 t
:sub-angle-vector : original angle-vector from -810 to -90.0, expected -90.0 t
:sub-angle-vector : original angle-vector from -820 to -100.0, expected -100.0 t
:sub-angle-vector : original angle-vector from -830 to -110.0, expected -110.0 t
:sub-angle-vector : original angle-vector from -840 to -120.0, expected -120.0 t
:sub-angle-vector : original angle-vector from -850 to -130.0, expected -130.0 t
:sub-angle-vector : original angle-vector from -860 to -140.0, expected -140.0 t
:sub-angle-vector : original angle-vector from -870 to -150.0, expected -150.0 t
:sub-angle-vector : original angle-vector from -880 to -160.0, expected -160.0 t
:sub-angle-vector : original angle-vector from -890 to -170.0, expected -170.0 t
:sub-angle-vector : original angle-vector from -900 to -180.0, expected -180.0 t
:sub-angle-vector : original angle-vector from -910 to 170.0, expected 170.0 t
:sub-angle-vector : original angle-vector from -920 to 160.0, expected 160.0 t
:sub-angle-vector : original angle-vector from -930 to 150.0, expected 150.0 t
:sub-angle-vector : original angle-vector from -940 to 140.0, expected 140.0 t
:sub-angle-vector : original angle-vector from -950 to 130.0, expected 130.0 t
:sub-angle-vector : original angle-vector from -960 to 120.0, expected 120.0 t
:sub-angle-vector : original angle-vector from -970 to 110.0, expected 110.0 t
:sub-angle-vector : original angle-vector from -980 to 100.0, expected 100.0 t
:sub-angle-vector : original angle-vector from -990 to 90.0, expected 90.0 t
TEST-NAME: test-angle-vector-over-640
now testing...
start testing [test-angle-vector-over-640]
[ WARN] [1666148359.318305921, 279.126000000]: [r_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148359.318333322, 279.126000000]: expected goal id 279095000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_1
[ WARN] [1666148359.318344383, 279.126000000]: received goal id 279026000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_0
[ WARN] [1666148359.318861117, 279.126000000]: [head_traj_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148359.318874812, 279.126000000]: expected goal id 279096000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_1
[ WARN] [1666148359.318884059, 279.126000000]: received goal id 279026000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_0
[ WARN] [1666148359.322139861, 279.129000000]: [l_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148359.322157174, 279.129000000]: expected goal id 279129000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_2
[ WARN] [1666148359.322166210, 279.129000000]: received goal id 279095000000_/pr2_eus_interface_1666148206507538114_60096_l_arm_controller/follow_joint_trajectory_1
[robot-interface.l] (send-ros-controller) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil)
[ INFO] [1666148359.322722167, 279.130000000]: wait-interpolation debug: start
[ WARN] [1666148359.322789583, 279.130000000]: [r_arm_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148359.322799942, 279.130000000]: expected goal id 279129000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_2
[ WARN] [1666148359.322816663, 279.130000000]: received goal id 279095000000_/pr2_eus_interface_1666148206507538114_60096_r_arm_controller/follow_joint_trajectory_1
[ WARN] [1666148359.332706628, 279.140000000]: [head_traj_controller/follow_joint_trajectory] action-result-cb may received old client's goal
[ WARN] [1666148359.332729060, 279.140000000]: expected goal id 279130000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_2
[ WARN] [1666148359.332743056, 279.140000000]: received goal id 279096000000_/pr2_eus_interface_1666148206507538114_60096_head_traj_controller/follow_joint_trajectory_1
[ WARN] [1666148359.333478257, 279.141000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X56458102a9e8> received wrong goal
[ WARN] [1666148359.333736508, 279.141000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X564581032738> received wrong goal
[ WARN] [1666148359.333991684, 279.141000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X5645810436c0> received wrong goal
[ WARN] [1666148359.334251238, 279.142000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X564581053c40> received wrong goal
[ WARN] [1666148359.353120178, 279.160000000]: [l_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X564581115ed8> received wrong goal
[ WARN] [1666148359.353331201, 279.161000000]: [l_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X564581128c18> received wrong goal
[ERROR] [1666148360.528092065, 280.333000000]: [torso_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists)
[ INFO] [1666148360.528147528, 280.333000000]: wait-interpolation debug: end
*ri* 60.0024, *pr2* 60.0 t
[robot-interface.l] (send-ros-controller) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil)
[ INFO] [1666148361.638188895, 281.442000000]: wait-interpolation debug: start
[ERROR] [1666148363.395043223, 283.198000000]: [torso_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists)
[ INFO] [1666148363.395099038, 283.198000000]: wait-interpolation debug: end
*ri* 60.0004, *pr2* 420.0 t
[robot-interface.l] (send-ros-controller) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil)
[ INFO] [1666148363.397217849, 283.200000000]: wait-interpolation debug: start
[ WARN] [1666148363.408132355, 283.211000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X56457a650790> received wrong goal
[ WARN] [1666148363.408464454, 283.212000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X56457a89d630> received wrong goal
[ WARN] [1666148363.408769313, 283.212000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X56457a92d220> received wrong goal
[ WARN] [1666148363.428860592, 283.232000000]: [l_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X56457e2d6ce0> received wrong goal
[ERROR] [1666148366.903044504, 286.704000000]: [torso_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists)
[ INFO] [1666148366.903093315, 286.704000000]: wait-interpolation debug: end
*ri* 60.0, *pr2* 780.0 t
TEST-NAME: test-angle-vector-under-640
now testing...
start testing [test-angle-vector-under-640]
[robot-interface.l] (send-ros-controller) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil)
[ INFO] [1666148366.910485982, 286.711000000]: wait-interpolation debug: start
[ERROR] [1666148368.667952008, 288.467000000]: [torso_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists)
[ INFO] [1666148368.668000789, 288.467000000]: wait-interpolation debug: end
*ri* -60.0338, *pr2* -60.0 t
[robot-interface.l] (send-ros-controller) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil)
[ INFO] [1666148368.671320580, 288.471000000]: wait-interpolation debug: start
[ WARN] [1666148368.703115016, 288.502000000]: [r_arm_controller/follow_joint_trajectory] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X56457f1d7058> received wrong goal
[ERROR] [1666148370.427271096, 290.226000000]: [torso_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists)
[ INFO] [1666148370.427322582, 290.226000000]: wait-interpolation debug: end
*ri* -60.0055, *pr2* -420.0 t
[robot-interface.l] (send-ros-controller) could not find joint-name 'torso_lift_joint-NOT-FOUND' (nil)
[ INFO] [1666148370.434089161, 290.232000000]: wait-interpolation debug: start
[ERROR] [1666148373.944116073, 293.739000000]: [torso_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists)
[ INFO] [1666148373.944170865, 293.739000000]: wait-interpolation debug: end
*ri* -60.0001, *pr2* -780.0 t
TEST-NAME: instantiate-pr2-sensor-robot-test
now testing...
start testing [instantiate-pr2-sensor-robot-test]
each robot should have different camera param
#2f((525.0 0.0 319.5 0.0) (0.0 525.0 239.5 0.0) (0.0 0.0 1.0 0.0) (0.0 0.0 0.0 1.0))
#2f((525.0 0.0 319.5 0.0) (0.0 525.0 239.5 0.0) (0.0 0.0 1.0 0.0) (0.0 0.0 0.0 1.0))
ALL RESULTS:
TEST-NUM: 10
PASSED: 8
FAILURE: 2
RESULT: all-test
TEST-NUM: 0
PASSED: 0
FAILURE: 0
RESULT: test-angle-vector
TEST-NUM: 1
PASSED: 1
FAILURE: 0
RESULT: test-angle-vector-limb
TEST-NUM: 1
PASSED: 1
FAILURE: 0
RESULT: test-angle-vector-duration
TEST-NUM: 1
PASSED: 1
FAILURE: 0
RESULT: test-go-pos
TEST-NUM: 1
PASSED: 0
FAILURE: 1
RESULT: test-move-to
TEST-NUM: 1
PASSED: 0
FAILURE: 1
RESULT: test-wrong-controller
TEST-NUM: 1
PASSED: 1
FAILURE: 0
RESULT: test-sub-angle-vector-1000
TEST-NUM: 1
PASSED: 1
FAILURE: 0
RESULT: test-angle-vector-over-640
TEST-NUM: 1
PASSED: 1
FAILURE: 0
RESULT: test-angle-vector-under-640
TEST-NUM: 1
PASSED: 1
FAILURE: 0
RESULT: instantiate-pr2-sensor-robot-test
TEST-NUM: 1
PASSED: 1
FAILURE: 0
[ INFO] [1666148375.999995846, 295.792000000]: cell* ROSEUS_EXIT(context*, int, cell**)
[ INFO] [1666148376.000032134, 295.792000000]: exiting roseus 1
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment