Skip to content

Commit

Permalink
Add *enable-roseus-resume* for installing default interruption handlers
Browse files Browse the repository at this point in the history
  • Loading branch information
Affonso-Gui committed Oct 25, 2022
1 parent b9fd4fb commit ced7925
Showing 1 changed file with 16 additions and 0 deletions.
16 changes: 16 additions & 0 deletions pr2eus/robot-interface.l
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,12 @@
(ros::roseus-add-msgs "nav_msgs")
(require :speak "package://pr2eus/speak.l")

;; ROSEUS_RESUME
(defvar *enable-roseus-resume*
#+:condition (ros::rospack-find "roseus_resume"))
;; make dummy package to avoid symbol search error
(unless (find-package "ROSEUS_RESUME") (make-package "ROSEUS_RESUME"))

(defun shortest-angle (d0 d1)
(atan2 (sin (- d0 d1)) (cos (- d0 d1))))

Expand Down Expand Up @@ -219,6 +225,9 @@
(pushnew `(lambda () (send ,self :robot-interface-simulation-callback)) *timer-job*)
(warning-message 3 "current *timer-job* is ~A~%" *timer-job*)
))
;;
(when *enable-roseus-resume*
(roseus_resume::install-interruption-handler self))
self)
;;
(:add-controller (ctype &key (joint-enable-check) (create-actions))
Expand Down Expand Up @@ -2253,4 +2262,11 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
(funcall (eval (read-from-string (format nil "#'~A-init" robot-name))))
))))

;; roseus_resume must be loaded after `robot-interface' definition
(if *enable-roseus-resume*
(progn
(require :roseus_resume "package://roseus_resume/euslisp/interruption-handler.l")
(warning-message 2 ";; roseus_resume is enabled.~%"))
(warning-message 3 ";; roseus_resume is disabled.~%"))

(provide :robot-interface "robot-interface.l")

0 comments on commit ced7925

Please sign in to comment.