Skip to content

Commit 52b52b9

Browse files
knorth55knorth55
committed
add dynamic reconfigure client test
1 parent 3268f67 commit 52b52b9

4 files changed

Lines changed: 176 additions & 0 deletions

File tree

‎roseus/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -207,4 +207,5 @@ if(CATKIN_ENABLE_TESTING)
207207
add_rostest(test/test-compile-message.test)
208208
add_rostest(test/test-print-ros-msg.test)
209209
add_rostest(test/test-dynamic-reconfigure-server.test)
210+
add_rostest(test/test-dynamic-reconfigure-client.test)
210211
endif()

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

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
#!/usr/bin/env roseus
2+
3+
(load "package://roseus/euslisp/dynamic-reconfigure-server.l")
4+
5+
(ros::roseus-add-msgs "std_msgs")
6+
(ros::roseus-add-msgs "dynamic_reconfigure")
7+
8+
(ros::roseus "simple_dynamic_reconfigure_server" :anonymous nil)
9+
10+
(setq *int-param* nil)
11+
(setq *double-param* nil)
12+
(setq *str-param* nil)
13+
(setq *bool-param* nil)
14+
15+
(setq *reconfigure-server*
16+
(def-dynamic-reconfigure-server
17+
;;; ((name type level description (default) (min) (max) (edit_method)) ... )
18+
(("int_param" int_t 1 "Int parameter" 0 -10 10)
19+
("double_param" double_t 2 "double parameter" 0.0 -2.0 10.0)
20+
("str_param" str_t 4 "String parameter" "foo")
21+
("bool_param" bool_t 8 "Boolean parameter" nil))
22+
;;; callbackfunc (defun func (config level) ) -> return updated-config
23+
'(lambda-closure nil 0 0 (cfg level)
24+
(setq *updated* t)
25+
(setq *int-param* (cdr (assoc "int_param" cfg :test #'equal)))
26+
(setq *double-param* (cdr (assoc "double_param" cfg :test #'equal)))
27+
(setq *str-param* (cdr (assoc "str_param" cfg :test #'equal)))
28+
(setq *bool-param* (cdr (assoc "bool_param" cfg :test #'equal)))
29+
cfg)))
30+
31+
(ros::advertise "~/int_param" std_msgs::Int16)
32+
(ros::advertise "~/double_param" std_msgs::Float32)
33+
(ros::advertise "~/str_param" std_msgs::String)
34+
(ros::advertise "~/bool_param" std_msgs::Bool)
35+
(ros::advertise "~/updated" std_msgs::Bool)
36+
37+
(setq *updated* nil)
38+
39+
(ros::rate 10)
40+
(while (ros::ok)
41+
(if *int-param*
42+
(ros::publish "~/int_param" (instance std_msgs::Int16 :data (round *int-param*))))
43+
(if *double-param*
44+
(ros::publish "~/double_param" (instance std_msgs::Float32 :data *double-param*)))
45+
(if *str-param*
46+
(ros::publish "~/str_param" (instance std_msgs::String :data *str-param*)))
47+
(ros::publish "~/bool_param" (instance std_msgs::Bool :data *bool-param*))
48+
(ros::publish "~/updated" (instance std_msgs::Bool :data *updated*))
49+
(ros::spin-once)
50+
)

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

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
#!/usr/bin/env roseus
2+
;;
3+
4+
(require :unittest "lib/llib/unittest.l")
5+
(load "package://roseus/euslisp/roseus-utils.l")
6+
7+
(ros::roseus-add-msgs "std_msgs")
8+
9+
(ros::roseus "test_dynamic_reconfigure_client" :anonymous nil)
10+
11+
(init-unit-test)
12+
13+
14+
(deftest test-dynamic-reconfigure-client-default ()
15+
(let* ((server-name "/simple_dynamic_reconfigure_server")
16+
(int-topic-name (format nil "~A/int_param" server-name))
17+
(double-topic-name (format nil "~A/double_param" server-name))
18+
(str-topic-name (format nil "~A/str_param" server-name))
19+
(bool-topic-name (format nil "~A/bool_param" server-name))
20+
(int-msg nil)
21+
(double-msg nil)
22+
(str-msg nil)
23+
(bool-msg nil)
24+
(cnt 0))
25+
(ros::subscribe int-topic-name std_msgs::Int16
26+
#'(lambda (m) (setq int-msg m)))
27+
(ros::subscribe double-topic-name std_msgs::Float32
28+
#'(lambda (m) (setq double-msg m)))
29+
(ros::subscribe str-topic-name std_msgs::String
30+
#'(lambda (m) (setq str-msg m)))
31+
(ros::subscribe bool-topic-name std_msgs::Bool
32+
#'(lambda (m) (setq bool-msg m)))
33+
34+
(while (and (ros::ok)
35+
(< cnt 100)
36+
(or (null int-msg) (null double-msg)
37+
(null str-msg) (null bool-msg))
38+
)
39+
(incf cnt)
40+
(ros::spin-once)
41+
(ros::sleep))
42+
43+
(ros::unadvertise int-topic-name)
44+
(ros::unadvertise double-topic-name)
45+
(ros::unadvertise str-topic-name)
46+
(ros::unadvertise bool-topic-name)
47+
48+
(pprint double-msg)
49+
(assert (and int-msg (equal (send int-msg :data) 0))
50+
(format nil "int default value is wrong: ~A" (send int-msg :data)))
51+
(assert (and double-msg (eps= (send double-msg :data) 0.0))
52+
(format nil "double default value is wrong: ~A" (send double-msg :data)))
53+
(assert (and str-msg (equal (send str-msg :data) "foo"))
54+
(format nil "str default value is wrong: ~A" (send str-msg :data)))
55+
(assert (and bool-msg (equal (send bool-msg :data) nil))
56+
(format nil "bool default value is wrong: ~A" (send bool-msg :data)))
57+
))
58+
59+
(deftest test-dynamic-reconfigure-client-update ()
60+
(let* ((server-name "/simple_dynamic_reconfigure_server")
61+
(int-topic-name (format nil "~A/int_param" server-name))
62+
(double-topic-name (format nil "~A/double_param" server-name))
63+
(str-topic-name (format nil "~A/str_param" server-name))
64+
(bool-topic-name (format nil "~A/bool_param" server-name))
65+
(update-topic-name (format nil "~A/updated" server-name))
66+
(int-msg nil)
67+
(double-msg nil)
68+
(str-msg nil)
69+
(bool-msg nil)
70+
(update-msg nil)
71+
(cnt 0))
72+
(ros::set-dynamic-reconfigure-param server-name "int_param" :int 8)
73+
(ros::set-dynamic-reconfigure-param server-name "double_param" :double 7.3)
74+
(ros::set-dynamic-reconfigure-param server-name "str_param" :string "test")
75+
(ros::set-dynamic-reconfigure-param server-name "bool_param" :bool t)
76+
77+
(ros::subscribe update-topic-name std_msgs::Bool
78+
#'(lambda (m) (setq update-msg m)))
79+
(while (and (ros::ok) (or (null update-msg) (null (send update-msg :data))))
80+
(incf cnt)
81+
(ros::spin-once)
82+
(ros::sleep))
83+
(ros::unsubscribe update-topic-name)
84+
85+
(ros::subscribe int-topic-name std_msgs::Int16
86+
#'(lambda (m) (setq int-msg m)))
87+
(ros::subscribe double-topic-name std_msgs::Float32
88+
#'(lambda (m) (setq double-msg m)))
89+
(ros::subscribe str-topic-name std_msgs::String
90+
#'(lambda (m) (setq str-msg m)))
91+
(ros::subscribe bool-topic-name std_msgs::Bool
92+
#'(lambda (m) (setq bool-msg m)))
93+
94+
(while (and (ros::ok)
95+
(< cnt 100)
96+
(or (null int-msg) (null double-msg)
97+
(null str-msg) (null bool-msg))
98+
)
99+
(incf cnt)
100+
(ros::spin-once)
101+
(ros::sleep))
102+
103+
(ros::unsubscribe int-topic-name)
104+
(ros::unsubscribe double-topic-name)
105+
(ros::unsubscribe str-topic-name)
106+
(ros::unsubscribe bool-topic-name)
107+
108+
(pprint double-msg)
109+
(assert (and int-msg (equal (send int-msg :data) 8))
110+
(format nil "int default value is wrong: ~A" (send int-msg :data)))
111+
(assert (and double-msg (eps= (send double-msg :data) 7.3))
112+
(format nil "double default value is wrong: ~A" (send double-msg :data)))
113+
(assert (and str-msg (equal (send str-msg :data) "test"))
114+
(format nil "str default value is wrong: ~A" (send str-msg :data)))
115+
(assert (and bool-msg (equal (send bool-msg :data) t))
116+
(format nil "bool default value is wrong: ~A" (send bool-msg :data)))
117+
))
118+
119+
(run-all-tests)
120+
121+
(exit)

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
<launch>
2+
<node name="simple_dynamic_reconfigure_server" pkg="roseus" type="roseus" args="$(find roseus)/test/simple-dynamic-reconfigure-server.l" />
3+
<test test-name="test_dynamic_reconfigure_client" pkg="roseus" type="roseus" args="$(find roseus)/test/test-dynamic-reconfigure-client.l" />
4+
</launch>

0 commit comments

Comments
 (0)