From 979055eb2bd1ee4e18f3f707f60e77b7ae1ba3f5 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 14 Nov 2022 17:47:31 +0900 Subject: [PATCH] add dynamic reconfigure client test --- roseus/CMakeLists.txt | 1 + .../test/simple-dynamic-reconfigure-server.l | 50 ++++++++ roseus/test/test-dynamic-reconfigure-client.l | 121 ++++++++++++++++++ .../test/test-dynamic-reconfigure-client.test | 4 + 4 files changed, 176 insertions(+) create mode 100644 roseus/test/simple-dynamic-reconfigure-server.l create mode 100644 roseus/test/test-dynamic-reconfigure-client.l create mode 100644 roseus/test/test-dynamic-reconfigure-client.test diff --git a/roseus/CMakeLists.txt b/roseus/CMakeLists.txt index f880db98b..4812d008a 100644 --- a/roseus/CMakeLists.txt +++ b/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() diff --git a/roseus/test/simple-dynamic-reconfigure-server.l b/roseus/test/simple-dynamic-reconfigure-server.l new file mode 100644 index 000000000..a4c759274 --- /dev/null +++ b/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) + ) diff --git a/roseus/test/test-dynamic-reconfigure-client.l b/roseus/test/test-dynamic-reconfigure-client.l new file mode 100644 index 000000000..8e92d2dff --- /dev/null +++ b/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) diff --git a/roseus/test/test-dynamic-reconfigure-client.test b/roseus/test/test-dynamic-reconfigure-client.test new file mode 100644 index 000000000..bd774bf6c --- /dev/null +++ b/roseus/test/test-dynamic-reconfigure-client.test @@ -0,0 +1,4 @@ + + + +