|
| 1 | +from time import sleep |
| 2 | + |
1 | 3 | from lifecycle_msgs.srv import ChangeState |
2 | 4 | from rcl_interfaces.msg import SetParametersResult |
3 | 5 |
|
4 | 6 | import rclpy |
| 7 | + |
5 | 8 | from rclpy.executors import MultiThreadedExecutor |
6 | 9 | from rclpy.node import Node |
7 | 10 | from rclpy.parameter import Parameter |
@@ -102,33 +105,28 @@ def main(args=None): |
102 | 105 | lc.configure_system() |
103 | 106 | executor.spin_once(timeout_sec=1) |
104 | 107 | executor.spin_once(timeout_sec=1) |
105 | | - executor.spin_once(timeout_sec=1) |
106 | | - executor.spin_once(timeout_sec=1) |
| 108 | + sleep(2) |
107 | 109 |
|
108 | 110 | lc.activate_system() |
109 | 111 | executor.spin_once(timeout_sec=1) |
110 | 112 | executor.spin_once(timeout_sec=1) |
111 | 113 | executor.spin_once(timeout_sec=1) |
112 | | - executor.spin_once(timeout_sec=1) |
113 | | - executor.spin_once(timeout_sec=1) |
114 | | - executor.spin_once(timeout_sec=1) |
| 114 | + sleep(2) # give the system some time to converge |
115 | 115 |
|
116 | 116 | lc.change_A_mode('AA') |
117 | 117 | executor.spin_once(timeout_sec=1) |
118 | 118 | executor.spin_once(timeout_sec=1) |
119 | 119 | executor.spin_once(timeout_sec=1) |
120 | 120 | executor.spin_once(timeout_sec=1) |
121 | | - lc.change_A_mode('AA') # redundant, should be ignored |
122 | | - executor.spin_once(timeout_sec=1) |
123 | | - executor.spin_once(timeout_sec=1) |
124 | | - executor.spin_once(timeout_sec=1) |
125 | | - executor.spin_once(timeout_sec=1) |
126 | | - lc.change_A_mode('BB') |
127 | | - executor.spin_once(timeout_sec=1) |
128 | 121 | executor.spin_once(timeout_sec=1) |
| 122 | + sleep(3) # give the system some time to converge |
| 123 | + |
| 124 | + lc.change_A_mode('AA') # this is the tested aspect: call redundant, should be ignored |
129 | 125 | executor.spin_once(timeout_sec=1) |
130 | 126 | executor.spin_once(timeout_sec=1) |
| 127 | + sleep(2) # give the system some time to converge |
131 | 128 |
|
| 129 | + lc.change_A_mode('BB') |
132 | 130 | executor.spin() |
133 | 131 | finally: |
134 | 132 | executor.shutdown() |
|
0 commit comments