add dynamic reconfigure client test

roseus/CMakeLists.txt

@@ -207,4 +207,5 @@ if(CATKIN_ENABLE_TESTING)
   add_rostest(test/test-compile-message.test)
   add_rostest(test/test-print-ros-msg.test)
   add_rostest(test/test-dynamic-reconfigure-server.test)
+  add_rostest(test/test-dynamic-reconfigure-client.test)
 endif()

roseus/test/simple-dynamic-reconfigure-server.l

@@ -0,0 +1,50 @@
+#!/usr/bin/env roseus
+
+(load "package://roseus/euslisp/dynamic-reconfigure-server.l")
+
+(ros::roseus-add-msgs "std_msgs")
+(ros::roseus-add-msgs "dynamic_reconfigure")
+
+(ros::roseus "simple_dynamic_reconfigure_server" :anonymous nil)
+
+(setq *int-param* nil)
+(setq *double-param* nil)
+(setq *str-param* nil)
+(setq *bool-param* nil)
+
+(setq *reconfigure-server*
+      (def-dynamic-reconfigure-server
+        ;;; ((name type level description (default) (min) (max) (edit_method)) ... )
+        (("int_param"  int_t  1  "Int parameter" 0  -10  10)
+         ("double_param"  double_t  2  "double parameter" 0.0  -2.0  10.0)
+         ("str_param"  str_t  4  "String parameter" "foo")
+         ("bool_param"  bool_t  8  "Boolean parameter"  nil))
+        ;;; callbackfunc (defun func (config level) ) -> return updated-config
+        '(lambda-closure nil 0 0 (cfg level)
+           (setq *updated* t)
+           (setq *int-param* (cdr (assoc "int_param" cfg :test #'equal)))
+           (setq *double-param* (cdr (assoc "double_param" cfg :test #'equal)))
+           (setq *str-param* (cdr (assoc "str_param" cfg :test #'equal)))
+           (setq *bool-param* (cdr (assoc "bool_param" cfg :test #'equal)))
+           cfg)))
+
+(ros::advertise "~/int_param" std_msgs::Int16)
+(ros::advertise "~/double_param" std_msgs::Float32)
+(ros::advertise "~/str_param" std_msgs::String)
+(ros::advertise "~/bool_param" std_msgs::Bool)
+(ros::advertise "~/updated" std_msgs::Bool)
+
+(setq *updated* nil)
+
+(ros::rate 10)
+(while (ros::ok)
+  (if *int-param*
+    (ros::publish "~/int_param" (instance std_msgs::Int16 :data (round *int-param*))))
+  (if *double-param*
+    (ros::publish "~/double_param" (instance std_msgs::Float32 :data *double-param*)))
+  (if *str-param*
+    (ros::publish "~/str_param" (instance std_msgs::String :data *str-param*)))
+  (ros::publish "~/bool_param" (instance std_msgs::Bool :data *bool-param*))
+  (ros::publish "~/updated" (instance std_msgs::Bool :data *updated*))
+  (ros::spin-once)
+  )

roseus/test/test-dynamic-reconfigure-client.l

@@ -0,0 +1,121 @@
+#!/usr/bin/env roseus
+;;
+
+(require :unittest "lib/llib/unittest.l")
+(load "package://roseus/euslisp/roseus-utils.l")
+
+(ros::roseus-add-msgs "std_msgs")
+
+(ros::roseus "test_dynamic_reconfigure_client" :anonymous nil)
+
+(init-unit-test)
+
+
+(deftest test-dynamic-reconfigure-client-default ()
+  (let* ((server-name "/simple_dynamic_reconfigure_server")
+         (int-topic-name (format nil "~A/int_param" server-name))
+         (double-topic-name (format nil "~A/double_param" server-name))
+         (str-topic-name (format nil "~A/str_param" server-name))
+         (bool-topic-name (format nil "~A/bool_param" server-name))
+         (int-msg nil)
+         (double-msg nil)
+         (str-msg nil)
+         (bool-msg nil)
+         (cnt 0))
+    (ros::subscribe int-topic-name std_msgs::Int16
+                    #'(lambda (m) (setq int-msg m)))
+    (ros::subscribe double-topic-name std_msgs::Float32
+                    #'(lambda (m) (setq double-msg m)))
+    (ros::subscribe str-topic-name std_msgs::String
+                    #'(lambda (m) (setq str-msg m)))
+    (ros::subscribe bool-topic-name std_msgs::Bool
+                    #'(lambda (m) (setq bool-msg m)))
+
+    (while (and (ros::ok)
+                (< cnt 100)
+                (or (null int-msg) (null double-msg)
+                    (null str-msg) (null bool-msg))
+                )
+      (incf cnt)
+      (ros::spin-once)
+      (ros::sleep))
+
+    (ros::unadvertise int-topic-name)
+    (ros::unadvertise double-topic-name)
+    (ros::unadvertise str-topic-name)
+    (ros::unadvertise bool-topic-name)
+
+    (pprint double-msg)
+    (assert (and int-msg (equal (send int-msg :data) 0))
+            (format nil "int default value is wrong: ~A" (send int-msg :data)))
+    (assert (and double-msg (eps= (send double-msg :data) 0.0))
+            (format nil "double default value is wrong: ~A" (send double-msg :data)))
+    (assert (and str-msg (equal (send str-msg :data) "foo"))
+            (format nil "str default value is wrong: ~A" (send str-msg :data)))
+    (assert (and bool-msg (equal (send bool-msg :data) nil))
+            (format nil "bool default value is wrong: ~A" (send bool-msg :data)))
+    ))
+
+(deftest test-dynamic-reconfigure-client-update ()
+  (let* ((server-name "/simple_dynamic_reconfigure_server")
+         (int-topic-name (format nil "~A/int_param" server-name))
+         (double-topic-name (format nil "~A/double_param" server-name))
+         (str-topic-name (format nil "~A/str_param" server-name))
+         (bool-topic-name (format nil "~A/bool_param" server-name))
+         (update-topic-name (format nil "~A/updated" server-name))
+         (int-msg nil)
+         (double-msg nil)
+         (str-msg nil)
+         (bool-msg nil)
+         (update-msg nil)
+         (cnt 0))
+    (ros::set-dynamic-reconfigure-param server-name "int_param" :int 8)
+    (ros::set-dynamic-reconfigure-param server-name "double_param" :double 7.3)
+    (ros::set-dynamic-reconfigure-param server-name "str_param" :string "test")
+    (ros::set-dynamic-reconfigure-param server-name "bool_param" :bool t)
+
+    (ros::subscribe update-topic-name std_msgs::Bool
+                    #'(lambda (m) (setq update-msg m)))
+    (while (and (ros::ok) (or (null update-msg) (null (send update-msg :data))))
+      (incf cnt)
+      (ros::spin-once)
+      (ros::sleep))
+    (ros::unsubscribe update-topic-name)
+
+    (ros::subscribe int-topic-name std_msgs::Int16
+                    #'(lambda (m) (setq int-msg m)))
+    (ros::subscribe double-topic-name std_msgs::Float32
+                    #'(lambda (m) (setq double-msg m)))
+    (ros::subscribe str-topic-name std_msgs::String
+                    #'(lambda (m) (setq str-msg m)))
+    (ros::subscribe bool-topic-name std_msgs::Bool
+                    #'(lambda (m) (setq bool-msg m)))
+
+    (while (and (ros::ok)
+                (< cnt 100)
+                (or (null int-msg) (null double-msg)
+                    (null str-msg) (null bool-msg))
+                )
+      (incf cnt)
+      (ros::spin-once)
+      (ros::sleep))
+
+    (ros::unsubscribe int-topic-name)
+    (ros::unsubscribe double-topic-name)
+    (ros::unsubscribe str-topic-name)
+    (ros::unsubscribe bool-topic-name)
+
+    (pprint double-msg)
+    (assert (and int-msg (equal (send int-msg :data) 8))
+            (format nil "int default value is wrong: ~A" (send int-msg :data)))
+    (assert (and double-msg (eps= (send double-msg :data) 7.3))
+            (format nil "double default value is wrong: ~A" (send double-msg :data)))
+    (assert (and str-msg (equal (send str-msg :data) "test"))
+            (format nil "str default value is wrong: ~A" (send str-msg :data)))
+    (assert (and bool-msg (equal (send bool-msg :data) t))
+            (format nil "bool default value is wrong: ~A" (send bool-msg :data)))
+    ))
+
+(run-all-tests)
+
+(exit)

roseus/test/test-dynamic-reconfigure-client.test

@@ -0,0 +1,4 @@
+<launch>
+  <node name="simple_dynamic_reconfigure_server" pkg="roseus" type="roseus" args="$(find roseus)/test/simple-dynamic-reconfigure-server.l" />
+  <test test-name="test_dynamic_reconfigure_client" pkg="roseus" type="roseus" args="$(find roseus)/test/test-dynamic-reconfigure-client.l" />
+</launch>