Merge pull request #459 from k-okada/support_duration

add ros::duration-sleep

roseus/euslisp/roseus.l

@@ -138,6 +138,21 @@
   "check if given ros::time is less the or equal to the others"
   (not (ros::time> a b)))
 
+(defclass ros::duration
+  :super ros::time
+  :slots ())
+(defmethod ros::duration
+  (:init (&optional sec nsec)
+         (cond
+          ((and (null sec) (null nsec))
+           (setq sec 0 nsec 0))
+          ((and sec (null nsec))
+           (setq nsec (round (* (mod sec 1) 1000000000)))
+           (setq sec  (floor sec))))
+         (send-super :init :sec sec :nsec nsec))
+  (:sleep () (ros::duration-sleep (send self :to-sec)))
+  )
+
 ;;
 (defclass timer-event
   :super propertied-object

roseus/roseus.cpp

@@ -718,6 +718,16 @@ pointer ROSEUS_SLEEP(register context *ctx,int n,pointer *argv)
   return (T);
 }
 
+pointer ROSEUS_DURATION_SLEEP(register context *ctx,int n,pointer *argv)
+{
+  isInstalledCheck;
+  numunion nu;
+  ckarg(1);
+  float sleep=ckfltval(argv[0]);
+  ros::Duration(sleep).sleep();
+  return(T);
+}
+
 pointer ROSEUS_OK(register context *ctx,int n,pointer *argv)
 {
   if (ros::ok()) {
@@ -1793,6 +1803,7 @@ pointer ___roseus(register context *ctx, int n, pointer *argv, pointer env)
   _defun(ctx,"TIME-NOW-RAW",argv[0],(pointer (*)())ROSEUS_TIME_NOW, "");
   _defun(ctx,"RATE",argv[0],(pointer (*)())ROSEUS_RATE, "frequency\n\n" "Construct ros timer for periodic sleeps");
   _defun(ctx,"SLEEP",argv[0],(pointer (*)())ROSEUS_SLEEP, "Sleeps for any leftover time in a cycle. Calculated from the last time sleep, reset, or the constructor was called.");
+  _defun(ctx,"DURATION-SLEEP",argv[0],(pointer (*)())ROSEUS_DURATION_SLEEP, "second\n\nSleeps for amount of the time specified by this duration.");
   _defun(ctx,"OK",argv[0],(pointer (*)())ROSEUS_OK, "Check whether it's time to exit. ");
 
   _defun(ctx,"ROS-DEBUG",argv[0],(pointer (*)())ROSEUS_ROSDEBUG,

roseus/test/test-roseus.l

@@ -18,6 +18,24 @@
     (assert (< vmrss 300000) "check memory leak")
     ))
 
+(deftest test-sleep ()
+  (let (d tm0 tm1)
+    (setq tm0 (ros::time-now))
+    (send (instance ros::duration :init 1.5) :sleep)
+    (setq tm1 (ros::time-now))
+    (assert (eps= (send (ros::time- tm1 tm0) :to-sec) 1.5 0.2))
+
+    (setq tm0 (ros::time-now))
+    (send (instance ros::duration :init 2 500000000) :sleep)
+    (setq tm1 (ros::time-now))
+    (assert (eps= (send (ros::time- tm1 tm0) :to-sec) 2.5 0.2))
+
+    (setq tm0 (ros::time-now))
+    (ros::duration-sleep 0.5)
+    (setq tm1 (ros::time-now))
+    (assert (eps= (send (ros::time- tm1 tm0) :to-sec) 0.5 0.2))
+    ))
+
 (deftest test-master ()
 
   (ros::ros-info "get-host ~A" (ros::get-host))