From 720af447fa0e79259994923f60407842bea1b236 Mon Sep 17 00:00:00 2001 From: Guilherme Affonso Date: Wed, 26 Oct 2022 11:54:31 +0900 Subject: [PATCH] Force unsubscribe in one-shot-subscribe --- roseus/euslisp/roseus-utils.l | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/roseus/euslisp/roseus-utils.l b/roseus/euslisp/roseus-utils.l index 99fc13e3a..102db560f 100644 --- a/roseus/euslisp/roseus-utils.l +++ b/roseus/euslisp/roseus-utils.l @@ -1132,10 +1132,13 @@ (unless (ros r :success) (ros::ros-warn "Call \"~A\" fails, it returns \"~A\"" srvname (send r :message))) r))) -(defun one-shot-subscribe (topic-name mclass &key (timeout) (after-stamp) (unsubscribe t)) +(defun one-shot-subscribe (topic-name mclass &key (timeout) (after-stamp)) "Subscribe message, just for once" (let (lmsg) - (unless (ros::get-num-publishers topic-name) + (if (ros::get-topic-subscriber topic-name) + (progn + (ros::ros-error (format nil "There is already subscriber for ~A. If you want to use this function, please (ros::unsubscribe \"~A\")." topic-name topic-name)) + (return-from one-shot-subscribe)) (cond (after-stamp (ros::subscribe topic-name mclass @@ -1147,15 +1150,16 @@ (t (ros::subscribe topic-name mclass #'(lambda (msg) (setq lmsg msg)))))) - (let ((finishtm (if timeout (ros::time-now)))) - (when finishtm - (setq finishtm (ros::time+ finishtm (ros::time (/ timeout 1000.0))))) - (while (not (and finishtm - (< (send (ros::time- finishtm (ros::time-now)) :to-Sec) 0))) - (unix::usleep (* 50 1000)) - (ros::spin-once) - (if lmsg (return)))) - (if unsubscribe (ros::unsubscribe topic-name)) + (unwind-protect + (let ((finishtm (if timeout (ros::time-now)))) + (when finishtm + (setq finishtm (ros::time+ finishtm (ros::time (/ timeout 1000.0))))) + (while (not (and finishtm + (< (send (ros::time- finishtm (ros::time-now)) :to-Sec) 0))) + (unix::usleep (* 50 1000)) + (ros::spin-once) + (if lmsg (return)))) + (ros::unsubscribe topic-name)) lmsg)) (defun one-shot-publish (topic-name msg &key (wait 500) (after-stamp) (unadvertise t))