added shortest angle

[HiroIshida]

Function named shortest_angle is called inside go-pos-unsafe-no-wait but, the function is not defined. I have just grepped the entire package and found the one here. I guess @k-okada forgot pasting the function.

I found this bug when running this demo. For your information, I paste the following obtained errors:

[ INFO] [1585661265.009992663, 682.273000000]: go-pos-unsafe (x y d) = (0.045974 -0.03073 0.810181)
[ WARN] [1585661265.010143658, 682.273000000]: pseudo :go-pose-unsafe-no-wait is called because move-base-trajectory-action is not found
Call Stack (max depth: 20):
  0: at (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2)))
  1: at (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffrot (send org-cds :difference-rotation cur-cds)) (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2))) (when (< (abs d-err) rotation-threshold) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((d-vel (* rotation-gain d-err))) (send self :send-cmd-vel-raw 0 0 d-vel)) (ros::sleep))
  2: at (let (org-cds cur-cds diffpos diffrot x-err y-err d-err (translation-threshold 0.05) (rotation-threshold (deg2rad 5)) (translation-gain 1.0) (rotation-gain 1.0) (stop t)) (send self :spin-once) (setq org-cds (send self :state :odom :pose)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffpos (send org-cds :difference-position cur-cds)) (setq x-err (- x (* 0.001 (elt diffpos 0)))) (setq y-err (- y (* 0.001 (elt diffpos 1)))) (when (and (< (abs x-err) translation-threshold) (< (abs y-err) translation-threshold)) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((x-vel (* translation-gain x-err)) (y-vel (* translation-gain y-err))) (send self :send-cmd-vel-raw x-vel y-vel 0)) (ros::sleep)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffrot (send org-cds :difference-rotation cur-cds)) (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2))) (when (< (abs d-err) rotation-threshold) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((d-vel (* rotation-gain d-err))) (send self :send-cmd-vel-raw 0 0 d-vel)) (ros::sleep)) (ros::rate 10))
  3: at (progn (ros::ros-warn "pseudo :go-pose-unsafe-no-wait is called because move-base-trajectory-action is not found") (ros::rate 100) (let (org-cds cur-cds diffpos diffrot x-err y-err d-err (translation-threshold 0.05) (rotation-threshold (deg2rad 5)) (translation-gain 1.0) (rotation-gain 1.0) (stop t)) (send self :spin-once) (setq org-cds (send self :state :odom :pose)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffpos (send org-cds :difference-position cur-cds)) (setq x-err (- x (* 0.001 (elt diffpos 0)))) (setq y-err (- y (* 0.001 (elt diffpos 1)))) (when (and (< (abs x-err) translation-threshold) (< (abs y-err) translation-threshold)) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((x-vel (* translation-gain x-err)) (y-vel (* translation-gain y-err))) (send self :send-cmd-vel-raw x-vel y-vel 0)) (ros::sleep)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffrot (send org-cds :difference-rotation cur-cds)) (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2))) (when (< (abs d-err) rotation-threshold) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((d-vel (* rotation-gain d-err))) (send self :send-cmd-vel-raw 0 0 d-vel)) (ros::sleep)) (ros::rate 10)) (return-from :go-pos-unsafe-no-wait t))
  4: at (if (not move-base-trajectory-action) (progn (ros::ros-warn "pseudo :go-pose-unsafe-no-wait is called because move-base-trajectory-action is not found") (ros::rate 100) (let (org-cds cur-cds diffpos diffrot x-err y-err d-err (translation-threshold 0.05) (rotation-threshold (deg2rad 5)) (translation-gain 1.0) (rotation-gain 1.0) (stop t)) (send self :spin-once) (setq org-cds (send self :state :odom :pose)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffpos (send org-cds :difference-position cur-cds)) (setq x-err (- x (* 0.001 (elt diffpos 0)))) (setq y-err (- y (* 0.001 (elt diffpos 1)))) (when (and (< (abs x-err) translation-threshold) (< (abs y-err) translation-threshold)) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((x-vel (* translation-gain x-err)) (y-vel (* translation-gain y-err))) (send self :send-cmd-vel-raw x-vel y-vel 0)) (ros::sleep)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffrot (send org-cds :difference-rotation cur-cds)) (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2))) (when (< (abs d-err) rotation-threshold) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((d-vel (* rotation-gain d-err))) (send self :send-cmd-vel-raw 0 0 d-vel)) (ros::sleep)) (ros::rate 10)) (return-from :go-pos-unsafe-no-wait t)))
  5: at (when (not move-base-trajectory-action) (ros::ros-warn "pseudo :go-pose-unsafe-no-wait is called because move-base-trajectory-action is not found") (ros::rate 100) (let (org-cds cur-cds diffpos diffrot x-err y-err d-err (translation-threshold 0.05) (rotation-threshold (deg2rad 5)) (translation-gain 1.0) (rotation-gain 1.0) (stop t)) (send self :spin-once) (setq org-cds (send self :state :odom :pose)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffpos (send org-cds :difference-position cur-cds)) (setq x-err (- x (* 0.001 (elt diffpos 0)))) (setq y-err (- y (* 0.001 (elt diffpos 1)))) (when (and (< (abs x-err) translation-threshold) (< (abs y-err) translation-threshold)) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((x-vel (* translation-gain x-err)) (y-vel (* translation-gain y-err))) (send self :send-cmd-vel-raw x-vel y-vel 0)) (ros::sleep)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffrot (send org-cds :difference-rotation cur-cds)) (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2))) (when (< (abs d-err) rotation-threshold) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((d-vel (* rotation-gain d-err))) (send self :send-cmd-vel-raw 0 0 d-vel)) (ros::sleep)) (ros::rate 10)) (return-from :go-pos-unsafe-no-wait t))
  6: at (unless move-base-trajectory-action (ros::ros-warn "pseudo :go-pose-unsafe-no-wait is called because move-base-trajectory-action is not found") (ros::rate 100) (let (org-cds cur-cds diffpos diffrot x-err y-err d-err (translation-threshold 0.05) (rotation-threshold (deg2rad 5)) (translation-gain 1.0) (rotation-gain 1.0) (stop t)) (send self :spin-once) (setq org-cds (send self :state :odom :pose)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffpos (send org-cds :difference-position cur-cds)) (setq x-err (- x (* 0.001 (elt diffpos 0)))) (setq y-err (- y (* 0.001 (elt diffpos 1)))) (when (and (< (abs x-err) translation-threshold) (< (abs y-err) translation-threshold)) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((x-vel (* translation-gain x-err)) (y-vel (* translation-gain y-err))) (send self :send-cmd-vel-raw x-vel y-vel 0)) (ros::sleep)) (while (ros::ok) (send self :spin-once) (setq cur-cds (send self :state :odom :pose)) (setq diffrot (send org-cds :difference-rotation cur-cds)) (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2))) (when (< (abs d-err) rotation-threshold) (when stop (send self :send-cmd-vel-raw 0 0 0)) (return)) (let ((d-vel (* rotation-gain d-err))) (send self :send-cmd-vel-raw 0 0 d-vel)) (ros::sleep)) (ros::rate 10)) (return-from :go-pos-unsafe-no-wait t))
  7: at (apply #'send self :go-pos-unsafe-no-wait args)
  8: at (apply #'send self :go-pos-unsafe-no-wait args)
  9: at (send* self :go-pos-unsafe-no-wait args)
  10: at (apply #'send *ri* :go-pos-unsafe target-pos)
  11: at (apply #'send *ri* :go-pos-unsafe target-pos)
  12: at (send* *ri* :go-pos-unsafe target-pos)
  13: at (cond ((member :go-pos-unsafe (send *ri* :methods)) (send* *ri* :go-pos-unsafe target-pos)) (t (send* *ri* :go-pos target-pos)))
  14: at (let* ((target-pos (coerce (scale 0.001 (v+ (send *bbox-coords* :pos) (float-vector -500 0 0))) cons))) (cond ((member :go-pos-unsafe (send *ri* :methods)) (send* *ri* :go-pos-unsafe target-pos)) (t (send* *ri* :go-pos target-pos))))
  15: at (go-pos-to-bbox)
  16: at (reach-object-demo)
  17: at #<compiled-code #X55698f5e7e18>
/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: undefined function shortest-angle in (setq d-err (shortest-angle (deg2rad d) (elt diffrot 2)))

[pazeshun]

So could you remove https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/793fefc4f84fd058d21ff677f91fd341bff63043/pr2eus/test/pr2-ri-test-base.l#L12-L13?

[knorth55]

can you write how you find this bug? can you tell us which launch or demo are you trying?

[HiroIshida]

@knorth55
I tried this demo. And when executing this, I got the mentioned error.

[knorth55]

@HiroIshida You find this bug when you are trying in reach_object demo, not detect_can demo. OK, I got it.