add test code for #461

[k-okada]

add test code for #461

• add test-pr2eus-moveit-sim.*

This will fail with

k-okada@p51s:~/catkin_ws/ws_pr2eus/src/jsk_pr2eus/pr2eus_moveit/test$ DISPLAY=  rostest test-pr2eus-moveit-sim.test 
... logging to /home/k-okada/.ros/log/rostest-p51s-15855.log
[ROSUNIT] Outputting test results to /home/k-okada/.ros/test_results/pr2eus_moveit/rostest-test_test-pr2eus-moveit-sim.xml
configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l"
;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin 
Xserver connection failed;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb 
;; extending gcstack 0x55c4b3c8f690[16374] --> 0x55c4b40eb460[32748] top=3c88
irtgl irtglc irtviewer 
EusLisp 9.27( 1.2.2) for Linux64 created on ip-10-0-1-65(Tue May 4 17:29:14 PST 2021)
roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-10-0-1-65 Tue May 4 17:29:14 PST 2021  1.2.2))
eustf roseus_c_util output to /home/k-okada/.ros/test_results/pr2eus_moveit/rosunit-test_pr2eus_moveit_sim.xml

;; extending gcstack 0x55c4b40eb460[32738] --> 0x55c4b6172090[65476] top=7e66
[ WARN] [1637301335.693790613]: [l_arm_controller/follow_joint_trajectory] action server is not found
[ WARN] [1637301335.699646132]:      goal=0, cancel=0, feedback=0, result=0
[ WARN] [1637301335.699844718]: #<controller-action-client #X55c4b8f7c008 l_arm_controller/follow_joint_trajectory> is not respond, pr2-interface is disabled
[ WARN] [1637301335.699910555]: Starting 'Kinematics Simulator'
[ WARN] [1637301335.699982158]:  (If you do not intend to start Kinematics Simulator, make sure that you can run 'rostopic info /l_arm_controller/follow_joint_trajectory/goal' and 'rostopic info /l_arm_controller/follow_joint_trajectory/cancel' and check whether Subscribers exists. If there is no Subscribers, please check joint_trajectory_action node.)
[ WARN] [1637301335.700068430]:  (Please also check 'rostopic info /l_arm_controller/follow_joint_trajectory/feedback' and 'rostopic info /l_arm_controller/follow_joint_trajectory/result' and check whether Publishers exists. If there is no Publishers, please check joint_trajectory_action node.)
[ WARN] [1637301335.700140487]:  (If joint_trajectory_action node already exists, you might have a network problem. Please make sure that you can run 'rosnode ping JOINT_TRAJECTORY_ACTION_SERVER_NODE_NAME' and 'rosnode ping /test_pr2eus_moveit_sim')
current *timer-job* is ((lambda nil (send #<pr2-interface #X55c4b5a09398> :robot-interface-simulation-callback)) lisp::count-up-timer)
[ WARN] [1637301338.944800137]: [/base_controller/follow_joint_trajectory] action server is not found
[ WARN] [1637301338.944976410]:      goal=0, cancel=0, feedback=0, result=0
[ WARN] [1637301338.945033870]: move-base-trajectory-action is not found
[ WARN] [1637301339.029252732]: #<ros::simple-action-client #X55c4b8fb4e00 /r_gripper_controller/gripper_action> is not respond, pr2-interface is disabled
start testing [test-angle-vector-motion-plan]
Call Stack (max depth: 20):
  0: at (error "keyword argument :move-arm must not be nil")
  1: at (progn (error "keyword argument :move-arm must not be nil"))
  2: at (if (not move-arm) (progn (error "keyword argument :move-arm must not be nil")))
  3: at (when (not move-arm) (error "keyword argument :move-arm must not be nil"))
  4: at (unless move-arm (error "keyword argument :move-arm must not be nil"))
  5: at (apply #'send self :parse-end-coords args)
  6: at (apply #'send self :parse-end-coords args)
  7: at (send* self :parse-end-coords args)
  8: at (let* ((r (send* self :parse-end-coords args)) (confkey (car r)) (ed-lst (cdr r)) time-from-start scene points mdof-points tmp-ret ret) (when (atom av) (return-from :angle-vector-make-trajectory (send* self :planning-environment :planning-make-trajectory confkey :set-angle-vector av :end-coords ed-lst args))) (dolist (tmp-av av) (setq scene (send self :planning-environment :get-planning-scene)) (send scene :robot_state :joint_state (joint-list->joint_state (send robot :joint-list))) (setq tmp-ret (send* self :planning-environment :planning-make-trajectory confkey :set-angle-vector tmp-av :scene scene :end-coords ed-lst args)) (unless tmp-ret (return-from :angle-vector-make-trajectory nil)) (setq points (send tmp-ret :trajectory :joint_trajectory :points)) (cond ((null ret) (setq ret tmp-ret)) (t (setq mdof-points (send tmp-ret :trajectory :multi_dof_joint_trajectory :points)) (when time-from-start (dolist (pt points) (send pt :time_from_start (ros::time+ time-from-start (send pt :time_from_start)))) (dolist (pt mdof-points) (send pt :time_from_start (ros::time+ time-from-start (send pt :time_from_start))))) (send ret :planning_time (+ (send ret :planning_time) (send tmp-ret :planning_time))) (if (send ret :trajectory :joint_trajectory :points) (when points (when (ros::time= (send (car points) :time_from_start) time-from-start) (pop points)) (nconc (send ret :trajectory :joint_trajectory :points) points)) (send ret :trajectory :joint_trajectory :points points)) (if (send ret :trajectory :multi_dof_joint_trajectory :points) (when mdof-points (when (ros::time= (send (car mdof-points) :time_from_start) time-from-start) (pop mdof-points)) (nconc (send ret :trajectory :multi_dof_joint_trajectory :points) mdof-points)) (send ret :trajectory :multi_dof_joint_trajectory :points mdof-points)))) (setq time-from-start (copy-object (send (car (last points)) :time_from_start))) (send robot :angle-vector tmp-av)) ret)
  9: at (apply #'send self :angle-vector-make-trajectory av args)
  10: at (apply #'send self :angle-vector-make-trajectory av args)
  11: at (send* self :angle-vector-make-trajectory av args)
  12: at (setq ret (send* self :angle-vector-make-trajectory av args))
  13: at (let (traj ret orig-total-time sent-controllers other-joints) (setq ctype (or ctype controller-type)) (unless (gethash ctype controller-table) (warn ";; controller-type: ~A not found" ctype) (return-from :angle-vector-motion-plan)) (setq ret (send* self :angle-vector-make-trajectory av args)) (unless ret (send self :check-state-validity) (return-from :angle-vector-motion-plan nil)) (setq traj (send ret :trajectory)) (setq orig-total-time (send (send (car (last (send traj :joint_trajectory :points))) :time_from_start) :to-sec)) (when (< orig-total-time 0.001) (unless reset-total-time (ros::ros-error "Trajectory has very short duration") (return-from :angle-vector-motion-plan nil)) (ros::ros-warn "reset Trajectory Total time") (setq traj (send* self :trajectory-filter traj :total-time reset-total-time args))) (ros::ros-info ";; Planned Trajectory Total Time ~7,3f [sec]" orig-total-time) (setq total-time (cond ((eq total-time :fast) (* scale (* orig-total-time 1000))) ((or (null total-time) (> orig-total-time (/ total-time 1000.0))) (* orig-total-time 1000)) (t total-time))) (setq traj (send* self :trajectory-filter traj :total-time total-time args)) (ros::ros-info ";; Scaled Trajectory Total Time ~0,3f(~0,3f) [sec]" (send (send (car (last (send traj :joint_trajectory :points))) :time_from_start) :to-sec) (/ total-time 1000.0)) (ros::ros-info ";; generated ~A points for ~A sec using [~A] group" (length (send traj :joint_trajectory :points)) (/ total-time 1000.0) (send ret :group_name)) (ros::ros-info ";; will send to ~A" (send traj :joint_trajectory :joint_names)) (mapcar #'(lambda (action param) (setq other-joints (not (intersection (send traj :joint_trajectory :joint_names) (cdr (assoc :joint-names param)) :test #'string=))) (when other-joints (maphash #'(lambda (ac ct) (when (and (= (length ct) 1) (equal (send-all (list action) :name) (send-all ct :name)) (not (member (send (car ct) :name) sent-controllers :test #'string=))) (ros::ros-info ";; send self :angle-vector ~A ~A (without planning)" ac ct) (push (send (car ct) :name) sent-controllers) (if (listp av) (send-message self robot-interface :angle-vector-sequence av total-time ac) (send-message self robot-interface :angle-vector av total-time ac)))) controller-table))) (gethash ctype controller-table) (send self ctype)) (cond (use-send-angle-vector (send* self :joint-trajectory-to-angle-vector-list move-arm (send traj :joint_trajectory) args)) (t (ros::ros-info ";; send self :send-trajectory ~A" ctype) (if start-offset-time (send* self :send-trajectory (send traj :joint_trajectory) :controller-type ctype :starttime start-offset-time args) (send* self :send-trajectory (send traj :joint_trajectory) :controller-type ctype args)))) ret)
  14: at (send *ri* :angle-vector-motion-plan (send *pr2* :reset-manip-pose))
  15: at (setq ret (send *ri* :angle-vector-motion-plan (send *pr2* :reset-manip-pose)))
  16: at (let (ret) (setq ret (send *ri* :angle-vector-motion-plan (send *pr2* :reset-manip-pose))) (assert ret "angle-vector-motion-plan returns something"))
  17: at (funcall func)
  18: at (funcall func)
  19: at (let ((func (symbol-function func-sym)) tm) (send *unit-test* :init-result func-sym) (format t "TEST-NAME: ~A~%" func-sym) (format t "  now testing...~%") (send *unit-test* :increment-test func-sym) (setq tm (now)) (funcall func) (send *unit-test* :set-time-to-current-result (send (now) :elapsed tm)))
  And more...
/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl unittest-error:  keyword argument :move-arm must not be nil in (error "keyword argument :move-arm must not be nil"), exitting...
[ERROR] test (error keyword argument :move-arm must not be nil) failed ... ( 76).
[Testcase: testtest_pr2eus_moveit_sim] ... ok

Close #461