From ad044d1d76e75f3fb861ee1ae711abb23fac470a Mon Sep 17 00:00:00 2001 From: rostest Date: Wed, 20 Nov 2019 19:48:53 +0100 Subject: [PATCH] Release 2.0.1 sick_lidar_localization phase II: time synchronization, configuration of time sync during initialization --- CMakeLists.txt | 17 +- README.md | 114 +++++- doc/pll_regression.png | Bin 0 -> 31533 bytes doc/scannbuffering.png | Bin 0 -> 35010 bytes doc/sequenceDiagramResultTelegrams.png | Bin 0 -> 23015 bytes doc/sequence_time_pll.png | Bin 0 -> 33133 bytes doc/software_pll.md | 77 ++++ doc/timing.md | 19 + doc/timing_sync.png | Bin 0 -> 25225 bytes include/sick_lidar_localization/SoftwarePLL.h | 200 +++++++++++ .../sick_lidar_localization/client_socket.h | 116 ++++++ .../cola_transmitter.h | 9 +- .../driver_check_thread.h | 13 + .../sick_lidar_localization/driver_thread.h | 34 +- .../testcase_generator.h | 38 +- .../time_sync_service.h | 192 ++++++++++ include/sick_lidar_localization/utils.h | 11 + launch/sim_loc_driver.launch | 4 + msg/SickLocResultPortTelegramMsg.msg | 15 + src/SoftwarePLL.cpp | 261 ++++++++++++++ src/client_socket.cpp | 163 +++++++++ src/cola_transmitter.cpp | 27 +- src/driver_check_thread.cpp | 54 ++- src/driver_monitor.cpp | 2 +- src/driver_thread.cpp | 65 ++-- src/pointcloud_converter_thread.cpp | 14 +- src/testcase_generator.cpp | 107 +++++- src/time_sync.cpp | 104 ++++++ src/time_sync_service.cpp | 338 ++++++++++++++++++ srv/SickLocColaTelegramSrv.srv | 2 +- srv/SickLocTimeSyncSrv.srv | 23 ++ .../test_server_thread.h | 3 +- .../sick_lidar_localization/verifier_thread.h | 0 test/scripts/cleanup.bash | 3 +- test/scripts/clion.bash | 2 +- test/scripts/make.bash | 20 +- test/scripts/run.bash | 3 +- test/scripts/run_demo_simu.bash | 6 +- test/scripts/run_error_simu.bash | 18 +- test/scripts/run_simu.bash | 36 +- test/src/test_server.cpp | 0 test/src/test_server_thread.cpp | 44 ++- test/src/unittest_sim_loc_parser.cpp | 0 test/src/verifier_thread.cpp | 34 +- test/src/verify_sim_loc_driver.cpp | 0 yaml/message_check_default.yaml | 4 + yaml/message_check_demo.yaml | 5 +- yaml/message_check_error_simu.yaml | 84 +++++ yaml/sim_loc_driver.yaml | 7 + 49 files changed, 2109 insertions(+), 179 deletions(-) create mode 100644 doc/pll_regression.png create mode 100644 doc/scannbuffering.png create mode 100644 doc/sequenceDiagramResultTelegrams.png create mode 100644 doc/sequence_time_pll.png create mode 100644 doc/software_pll.md create mode 100644 doc/timing.md create mode 100644 doc/timing_sync.png create mode 100644 include/sick_lidar_localization/SoftwarePLL.h create mode 100644 include/sick_lidar_localization/client_socket.h create mode 100644 include/sick_lidar_localization/time_sync_service.h create mode 100644 src/SoftwarePLL.cpp create mode 100644 src/client_socket.cpp create mode 100644 src/time_sync.cpp create mode 100644 src/time_sync_service.cpp create mode 100644 srv/SickLocTimeSyncSrv.srv mode change 100755 => 100644 test/include/sick_lidar_localization/test_server_thread.h mode change 100755 => 100644 test/include/sick_lidar_localization/verifier_thread.h mode change 100755 => 100644 test/src/test_server.cpp mode change 100755 => 100644 test/src/test_server_thread.cpp mode change 100755 => 100644 test/src/unittest_sim_loc_parser.cpp mode change 100755 => 100644 test/src/verifier_thread.cpp mode change 100755 => 100644 test/src/verify_sim_loc_driver.cpp create mode 100644 yaml/message_check_error_simu.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index d3544ad..74442a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,6 +74,7 @@ add_service_files( FILES SickLocColaTelegramSrv.srv SickLocRequestTimestampSrv.srv + SickLocTimeSyncSrv.srv ) ## Generate actions in the 'action' folder @@ -143,6 +144,7 @@ include_directories( ## Declare a C++ library add_library(sick_localization_lib + src/client_socket.cpp src/cola_converter.cpp src/cola_parser.cpp src/cola_transmitter.cpp @@ -152,7 +154,9 @@ add_library(sick_localization_lib src/pointcloud_converter_thread.cpp src/random_generator.cpp src/result_port_parser.cpp + src/SoftwarePLL.cpp src/testcase_generator.cpp + src/time_sync_service.cpp src/utils.cpp src/crc/crc16ccitt_false.cpp ) @@ -169,6 +173,7 @@ add_library(sick_localization_lib ## Executables for ros driver sick_lidar_localization add_executable(sim_loc_driver src/driver.cpp) add_executable(sim_loc_driver_check src/driver_check.cpp) +add_executable(sim_loc_time_sync src/time_sync.cpp) add_executable(pointcloud_converter src/pointcloud_converter.cpp) ## Executables for test purposes @@ -203,6 +208,11 @@ add_dependencies(sim_loc_test_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) +add_dependencies(sim_loc_time_sync + sick_localization_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) add_dependencies(pointcloud_converter sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} @@ -239,6 +249,11 @@ target_link_libraries(sim_loc_test_server ${Boost_LIBRARIES} ${catkin_LIBRARIES} ) +target_link_libraries(sim_loc_time_sync + sick_localization_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ) target_link_libraries(pointcloud_converter sick_localization_lib ${Boost_LIBRARIES} @@ -270,7 +285,7 @@ target_link_libraries(verify_sim_loc_driver # ) ## Mark executables and/or libraries for installation -install(TARGETS sick_localization_lib sim_loc_driver sim_loc_driver_check sim_loc_test_server pointcloud_converter unittest_sim_loc_parser verify_sim_loc_driver +install(TARGETS sick_localization_lib sim_loc_driver sim_loc_driver_check sim_loc_test_server sim_loc_time_sync pointcloud_converter unittest_sim_loc_parser verify_sim_loc_driver ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/README.md b/README.md index a757264..6da0d27 100644 --- a/README.md +++ b/README.md @@ -93,6 +93,9 @@ int32 | telegram_payload.CovarianceY | Covariance c5 of the pose Y [mm^2] int32 | telegram_payload.CovarianceYaw | Covariance c9 of the pose Yaw [mdeg^2] uint64 | telegram_payload.Reserved3 | Reserved uint16 | telegram_trailer.Checksum | CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of the trailer +bool | vehicle_time_valid | true: vehicle_time_sec and vehicle_time_nsec valid, false: software pll still in initial phase +uint32 | vehicle_time_sec | System time of vehicles pose calculated by software pll (seconds part of the system time) +uint32 | vehicle_time_nsec | System time of vehicles pose calculated by software pll (nano seconds part of the system time) Example output of a result telegram (ros message of type [msg/SickLocResultPortTelegramMsg.msg](msg/SickLocResultPortTelegramMsg.msg)): @@ -130,11 +133,110 @@ telegram_payload: Reserved3: 0 telegram_trailer: Checksum: 25105 +vehicle_time_valid: True +vehicle_time_sec: 1571732539 +vehicle_time_nsec: 854719042 ``` Note: Result telegrams always have the same MagicWord: `1397310283` (`0x5349434B` hex resp. `SICK` in ascii/ansi) and a length of 106 bytes (`Length: 106`). +## Time synchronization + +The localization controller sends the timestamp of a pose in millisecond ticks (telegram_payload.Timestamp of a result +port telegram). This timestamp in milliseconds is accurate, but differs from the ros system time by a time offset. +This time offset can be queried by a LocRequestTimestamp command. + +LocRequestTimestamp commands are transmitted to the localization controller by the Cola protocoll. See SICK manuals for +a description of the Cola protocoll and SOPAS commands. + +Cola commands can be transmitted to the localization controller by ros service "SickLocColaTelegram", implemented by +sim_loc_driver and defined in file [srv/SickLocColaTelegramSrv.srv](srv/SickLocColaTelegramSrv.srv). +Example for a Cola command to query a LocRequestTimestamp: + +``` +> rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" +cola_ascii_response: "sAN LocRequestTimestamp 1EDB" +send_timestamp_sec: 1573118218 +send_timestamp_nsec: 365014292 +receive_timestamp_sec: 1573118218 +receive_timestamp_nsec: 367832063 +``` + +`cola_ascii_response` is the response of the localization controller. `send_timestamp` and `receive_timestamp` are ros +system timestamps immediately before sending the request resp. immediately after receiving the controllers response. +Using send and receive timestamps, the time offset can be calculated: + +``` +delta_time_ms := mean_time_vehicle_ms - timestamp_lidar_ms +mean_time_vehicle_ms := (send_time_vehicle + receive_time_vehicle) / 2 + := vehicles mean timestamp in milliseconds +send_time_vehicle := vehicles timestamp when sending LocRequestTimestamp +receive_time_vehicle := vehicles timestamp when receiving the LocRequestTimestamp response +timestamp_lidar_ms := lidar timestamp in milliseconds from LocRequestTimestamp response +``` + +This time offset calculation is provided by ros service "SickLocRequestTimestamp", implemented by the +sim_loc_driver and defined in file [srv/SickLocRequestTimestampSrv.srv](srv/SickLocRequestTimestampSrv.srv). +Example to query the time offset: + +``` +> rosservice call SickLocRequestTimestamp "{}" +timestamp_lidar_ms: 23745 +mean_time_vehicle_ms: 1573118234209 +delta_time_ms: 1573118210464 +send_time_vehicle_sec: 1573118234 +send_time_vehicle_nsec: 208421663 +receive_time_vehicle_sec: 1573118234 +receive_time_vehicle_nsec: 211120716 +``` + +`delta_time_ms` gives the time offset in milliseconds. See operation manuals for details +about time synchronization, time offset calculation and Cola telegrams. + +The time offset depends on network latencies and transmission delays. To get a more accurate value, the time offset can be +calculated from N measurements by a software pll. The software pll estimates the mean time offset and calculates the +system time from any ticks. See [doc/software_pll.md](doc/software_pll.md) for further details. + +The system timestamp of a vehicle pose is calculated from lidar ticks for each result port telegram and published by +ros message [msg/SickLocResultPortTelegramMsg.msg](msg/SickLocResultPortTelegramMsg.msg). This way, an application does +not need to care about time synchronization itself. It's sufficient to use `vehicle_time_sec` and `vehicle_time_nsec` in +the result telegrams. Example output of a result telegram: + +``` +> rostopic echo "/sick_lidar_localization/driver/result_telegrams" +Timestamp: 525924 # Lidar timestamp in millisecond ticks +vehicle_time_valid: True # System timestamp by software pll is valid +vehicle_time_sec: 1573119569 # System timestamp of vehicle pose (second part) +vehicle_time_nsec: 854719042 # System timestamp of vehicle pose (nanosecond part) +``` + +The system timestamp of a vehicle pose can be calculated from ticks using ros service "SickLocTimeSync", too. This service +returns the system timestamp from ticks using the software pll running in the driver. It's is defined in file +[srv/SickLocTimeSyncSrv.srv](srv/SickLocTimeSyncSrv.srv). Example: + +``` +> rosservice call SickLocTimeSync "{timestamp_lidar_ms: 123456}" +vehicle_time_valid: True +vehicle_time_sec: 1573119167 +vehicle_time_nsec: 380565047 +``` + +**Important note:** The driver sends LocRequestTimestamp commands to the localization controller with a constant rate, +each 10 seconds by default (configurable by parameter `time_sync_rate` in file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml)). +The software pll is updated after each successful LocRequestTimestamp with the current lidar ticks and the current system +time. The software pll uses a fifo buffer to calculate a regression, with a buffer size of 7 by default (configurable by +parameter `software_pll_fifo_length` in file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml)). Therefore, it takes +at least 7 LocRequestTimestamp commands before the software pll is fully initialized and time synchronization becomes valid. +Within the initialization phase (at least 7 LocRequestTimestamp commands), a system time can not be calculated from +lidar ticks. During initialization phase, `vehicle_time_valid` will be false, `vehicle_time_sec` and `vehicle_time_nsec` +will have value 0. + +SICK recommends a time_sync_rate of 0.1 or below („SICK recommends you set the request cycle time from the vehicle +controller to 10 seconds or higher”). Therefore, the initial phase after start will take round about 70 seconds (7 LocRequestTimestamp +commands with 10 seconds delay by default). During the initial phase, the time synchronization service is not available +and the vehicle system time is not valid. + ## Diagnostics The sick_lidar_localization driver publishes diagnostic messages on ros topic "/sick_lidar_localization/driver/diagnostic", @@ -172,7 +274,7 @@ PARSE_ERROR | 2 | Parse error, telegram could not be decoded CONFIGURATION_ERROR | 3 | Invalid driver configuration INTERNAL_ERROR | 4 | Internal error (should never happen) -## Configuration +## Driver configuration The sick_lidar_localization driver is configured by file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml): @@ -180,6 +282,8 @@ Parametername | Defaultvalue | Description --- | --- | --- localization_controller_default_ip_adress | "192.168.0.1" | Default IP adress "192.168.0.1" of the localization controller (if not otherwise set by parameter "localization_controller_ip_adress") result_telegrams_tcp_port | 2201 | TCP port number of the localization controller sending localization results +cola_telegrams_tcp_port | 2111 | For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols +cola_binary | 0 | 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) tcp_connection_retry_delay | 1.0 | Delay in seconds to retry to connect to the localization controller, default 1 second result_telegrams_topic | "/sick_lidar_localization/driver/result_telegrams" | ros topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) result_telegrams_frame_id | "sick_lidar_localization" | ros frame id of result port telegram messages (type SickLocResultPortTelegramMsg) @@ -187,6 +291,12 @@ diagnostic_topic | "/sick_lidar_localization/driver/diagnostic" | ros topic to p diagnostic_frame_id | "sick_lidar_localization" | ros frame id of diagnostic messages (type SickLocDiagnosticMsg) monitoring_rate | 1.0 | frequency to monitor driver messages, once per second by default monitoring_message_timeout | 1.0 | timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, 1 second by default +point_cloud_topic | "/cloud" | ros topic to publish PointCloud2 data +point_cloud_frame_id | "pointcloud_sick_lidar_localization" | ros frame id of PointCloud2 messages +tf_parent_frame_id | "tf_demo_map" | parent frame of tf messages of of vehicles pose (typically frame of the loaded map) +tf_child_frame_id | "tf_sick_lidar_localization" | child frame of tf messages of of vehicles pose +software_pll_fifo_length | 7 | Length of software pll fifo, default: 7 +time_sync_rate | 0.1 | Frequency to request timestamps from localization controller using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 Note: The IP address of the SICK localization controller (192.168.0.1 by default) can be set by commandline argument `localization_controller_ip_adress:=` when starting the driver with @@ -341,6 +451,8 @@ pointcloud_convert in file [src/pointcloud_converter.cpp](src/pointcloud_convert sim_loc_driver messages. The driver messages are converted to both PointCloud2 on topic "/cloud" and and TF messages, which can be viewed by rviz. +![doc/sequenceDiagramResultTelegrams.png](doc/sequenceDiagramResultTelegrams.png) + To run and visualize an example with a simulated vehicle moving in circles, run the following commands: ```console diff --git a/doc/pll_regression.png b/doc/pll_regression.png new file mode 100644 index 0000000000000000000000000000000000000000..0823e57c006e42f4e6fe9b2e0366ba4a11d0203c GIT binary patch literal 31533 zcmd431y_}8*fqK=R6;dNu6sA(JK+J+ zJ@Ci$C2UN&YT%BH+})|Ib&6(mzH{Sc7*#*6 zXvrVF@2|I~Mh$~r8hy~f;rRUL!?SuKIqR3W?}{lteG1gT3Zwn$8f~+G^{oAh6RqH0 ze(l=v{=&bpYRBcCSWVwnzUO8HxMJAw=?FzNS+TpwzaUB}bZ;TQVJSI+{AVOlKL_&9 z%#ThO@W+c>%Kuk>+!0?i)!?1nyZnR~xh#k~D*aA{MMbv51)Ar_+v-jf$Zw)%Wvq^G zH(XDriUb4(2G-T_i?t$`EJd>8ThZd;qN7?QMT*zPazYoxmjw~wCC`d`6K=D=b zj>^njOyF@8^pHee9W`5Ym55SQMWw7nRZB~Yob93c$MEp*@86&1{)u7L*Q<4rquhnp zc@RZD(LJx?_quTV6HLfYrtrn6NV9BdFjvKaWJhGY%HGJ*lJQd0gTk8g-E`gCJpmbb zQ}Uw>tL~9m=LuI1qd&nH7aqH5u%e3e)~DaYdy<73%zDXAzi5)jbP9M~oMcEvtcdNt z*U-{}<<+TnSoB1O#!l|pMn6ZcMbh~Ac$SmZ_pbSbTr}OpIWqSmjBX>eU0h6$#j*d>ipuO^WK7# zeeV#e|J<|HZqclET8zSbk#tMD&ugM>_14r1>(z3#4 zVz}P(9T!*S+x?}e$VkbLk;%zI_H&}bZT*=tbn*$`zI~Itr&(&;4)0iPq*F-pSnPO2 zPEP(HVE;F!(3|Q>a<7y3zyF<_oHVz#2DMsjyfA-vv~eE|E$GvyZ`}5Ce?o{&5~$om z@mX}%)E;wxB+L{@l!L*=C~R{_UH>ZL0&>iHV6V>7OzEu=SDha4W9+q?5Q7?k};Ygg<3w znn3v5+n0R){5hds0fF%8y`NbyT%`M?wJ%FPk=N)Cda9x%D+&rq&$nkHbw{174e03T z8T6K?dy6XB@{cOfZr>zKT%A~B2+YgRpMS&cx;>pqpI1|(rK>AILz9`7CM+k%{P?l& zG^3D^zL;1hVM$BNJmZ9e_XEB$F);(1umVX!-h!mw;^H6OREi_v~=}5YsP>{!F-V!2k-*ivdrTGAq9@ zPMh(kvDZ2m-JP95pFd;I;9sxwrIQJ`V+lhNa+7>m6 zrX=@yQ&&?{vyVJ5D2O;XxpNT~d$>@0V5&L$V6ihMq3u!8mtS-s39x9kn$x7dY+x^&TCg#Q~D7Q6UTr1hKq|kIzHahGR_O4_F9eh6 zL^5ULIhh3nJa^|>u(7e1dlJvD&bL=z89|p})vwp7bK6Z4cz5&Gt&u`)y7V$?+6Y1{ zEGz;7Zm45tAKl?me*O9dwStu2Z3ik5pUcL{YIgFP1l#LhK6dq|rf|Wru`vgJtXy3v zCrJUBc%w33mL$*_X^ z?EZ}mtTZF@(L_F1_oEGl;y#Ef4-b!(Y}WgfkO+?@zR1eT27mb?FD<<`m>U64pO?oN z-%ZBrWG*Z$Y-ngWQDKA6s#lPiDdy@0LG$X>D+h>CfQc%()6j2CO(qtHuUtLjLT$J^^ zygp!M#a~?s3q)N#s=UNqUBPPaTU(P{?g_uqkLmeod(4h3RS`Zm+9=NVtlHg(Cc}APA5leepn5CJq@l&F;-mWfqWI~}><@xnhsXO#uEU7dr0U9PGd@AZO8QC*$4_0=k zYRefJ<8HoRP-uAR%fGo9kL!I|mYOYbPf0~ZyT)mC`sX{>jZsQM*v1dg3}a%{(J+xs zpzM$^1FA-<$o&W@Q7V(u+$tlx&4pK6Tk_aV6ry|CNkM296BYgv?C*}O&qA=n$A4Jh z5D|?xRf`~t<=FDqqQ2@6dlwgBSGBISE}I+UV1mXWzDDVFngk%hS0Jkyot=k=hbk&6HrCc_ z#f!UcOn5;phJ8Vejg62}-WR)fe@ZIhiYdv`s#xN*w{zyU{2>DPX^c+w^%JyAg5WLfY@gPm02WJ!6ngYT?d0t2Xmx;&i76PX)cg9X%&b>jOsqSO!wM4GFKlvBM_*sR z=6Xcmo0OQi*8R{@TYIA3^PCp?4mI_`=ETVGuuEq&^Ue1@TP}+oQLr?Ss_t%Xetv#f z!thL{-Em^~G|MfA3N*_gSMO2IcE+8F9Qr%RLbbChBP|W}X$)?Yjg4(3I@#-tfsgNet|bV- zNX3sI)PCqh>}{>Bt$ls=?cp>xZrtz-gYNY08$pe?fdRlUw8Vsjt;ya5#QR6A0KvuEBf>^{G-xlLd^q5`}}CLTE|vN2|KY!YE$VV z%r7x3V_p(H&08;vdPYPz~p zVU!{@uG@kx8yb%uJ!+SDNh~7NO`(U2>hFi8sybwP_f|6Xe*qI~ST=Tfeq32u>H6l) zXr&#wK&q;W%D~2r{k=UZDk^{k;=Mz;ITy>Un!C?$hi`0bKvVG(ro5D0nW}XyEG+Eq z>N4KW$;c3ukhrTkwS4P;mgNB%zqz@&q&FG&n-pK)-vGHtNl7Qlt)P;YY)#c#Sy*5T zpB-(U&xZ)-DX zeg_+4Dt$`Ozgk0D42On?mt=Mh4%nYR9~c=Cf|UXWnv^u1B^U4HGVV~ZzL2n_y}f<5eBw&LLuj^4Ob5ru*{(`jTH`g&RkO8q)R#KA!W9Kn=j&M7!@@S@L9*%7<=lPss(Xtv9c| zOwA%#bm;@bnM%--`3L{2$1$JpSHF$m=H)zz__vVZaLn?%T`DheajvP#IlrWE`Yht@ zIcH}}W}{11nWK?*?&QxWMsOh_qKBUYh-aU_9|IlYB;&KFU;9OdSJU#{8PHHX`d2QN zRL<@vlY1bkF+v&x86*fFWsw>vw1|{JWLyI==lBn6{r}*{za%JFSXdrE7U@&^(e^t4 zv#hifm$XIVKJvDGl;<6)pQsg#R@tY&pip#TVpi7B;C{Qe01%L#H!L(XFb%29AT*ui_dWlnOqm#HZno2Pu}MPj zF3+5GbS8uFS%ZUvQ67+(TUp86TLlOk7}!y2(y3OU@eHoqjtzC(`1R{IbIqRt71nQ? z1DD}~QtY;CXl7<6{OJWn6*NCBEg~@`US3`tI{6GtgXT}z@W~;cKWjo+4rUO!)3kwH zz;H-{8c50puS;Cgw(ED0-rvjjF1g+eEoFOMC2OzNp z;9p=ao_hP>r_HatOY4Y6lkWW zroOBMgw{DaI{NFE57ykuin*nw`&{#VdvD-Ilp-H5fSoJPA8gNn9&!#nlI=(p+8``! zxViVqzW`u%wbj*qK{_m4PoKVl{09u8s_G1wQ0VF;{luLms6`7T9KbnRRRhPj|0|*$ zfM@WTG@d#vv_bC|zL(q!5hDuCVO1(6T2fki@A!D;yTL8W1t3-cvp|M;mfY)ov=Q6T za1Hgu(a~{+SJK(}uLb{NY@%M!*4sN%>slNarv(_(!eSL#z8KZlSINL<)e5zwDsb`evNAJ~ z>dS;(17Jb`gY&JS*>+vwXk+sd5)!a2Qqxn1JO4!5&)0zW(BM!Ae~O90kLir68AzU{ zrlZTIN$t2Y9q8}x9~jWp(V-TJ;*SN){9(2th$04)y@UaYu%A2?H0X0J(V3EC6Id9%up4R8=jP2qp6nOvc&+ zxVBw?{-wo`keGOYYl=%87|-aa8^pH`i|hJup_iA}p-4w}clnPWk=B4OlDX{kV50#- zR@hGQS`9PN(b0{W%>qPx`}QqHQE5rZ?$MF$*$E*bA*AfrjpW!^hUzwBwrZUs;~=O6 zz|Y;?YmC!@-hBKh{_^GfU`73(?f|-I>SGJMa!MP~mxW!3yZR9~MhY=kh+}y>3IczrWZ(fC*bHptgVi{sFNT6cqFe1EG+V zG!ej3i3Sr{s4HwM3}P`SK;4nUj=JoorK7_U{?>J^D^@^GaO#?*5DavIm@(c}X5_>}@KUS;EwmNr zM8v!gl;*?3jWI_6{(fNqu0ain1_-hJ96}bJm80r(=ihHGyJ?+<%a$N~$i^(k589Qs z@Ro@B$>_*PVb-ZF-1T<7*v>*mXYc1?xxWerBX66p}F)CHcDmD zu8>tw*e5UIG92^|sm1wfDY$hN%ftK{{xD7%(uA`@?sHTROkB<)|Bnpa@M!}aa zU&=KQNF%kSuV24HY0kul@i%4NcA11z+5bc~UYC zw~G_|moIM!s6%-fC^2dUT~kIcO|Q&UPEu0RC_W}Snnl09&ZzYZMBAJ=D{C>VHs=!u z=vi3+lM{KJwE)ri-N&J34U;PU@xyvU$rlAD2(~mMJ3C0_2Po4wZlU~5hQL4Lu3ENIq85kH)Mr}?0e5WoU(fa$hNa@|ko*!ktG(;-)In+mZuNf!>yBD+8KD7~kEy0gb+QWu~fU z9!5n0Gb}PL8}kc;bcNdE>gwt~lqME5USYGowRJUY#u+P{6&!YUyqz2!eYDg~>|O#w zueSEPy@hspdHJlYEXbkmj*hhL(wLyiFWcMO#gL`*e=zKWpmYIrdHC=lNXwnZYyfT~ za?!e5L&?ecTnYxM)kH=8K!HASDp&NKDF8eTh#7K|cC5<0FHI?3+~3cSM99kn8oHjI zUN}mykjUM`KF+3>rpJYJ5b`+renlIC0W!o+5wr>QLamnN{YRv2 zwW-&80&Y7gfF_cGkbtzD4>7@z`1tYT&5aGcdJl#nm>BGUeAzzD2?6*0`%YnP*lu^d z+iuguL@n$DL#i@vITT2!m!Cz`XpgSLGR#Kk1LFkVRh%RiKtK59a|5PqAXx&v1&$D& z2$^JMV4&p9Qc}-af6~<0n0ob`RhD*OU|=8SZQb%*kjX^9=XXIlPfAR@1nJ@q3JT-$ zI)%yW5)I(IUhC&^f*DX7PR`GJQ#+=?^KhOo6BR|76!P- zSFg_VyaGmlARIY3I)V(j%Ao)3(W5Bf;She>NAIYOpFe$yrLYeweOur9NO1rLaRtCQ zkT+y>L(ess^y*IFmgd~?P_xt0_PV)dx&TJ@CJD@OaSs6*!zU(w2Q1Ncsz$01^}&Oy zxgge+HM;KMW;>U`93_~AOqLi0Y-F1QsJ;Tt1hRc?eZ7)8?>PItwG+TT_{P=gaxw=y zyL=KqkNbf+UXU|1wq5N?>bLLS9dAy!!UVD5@+iLQ85dU!%Ln0mtH48jF$X`buB~yN zM6!WAiKeNo4b60@sYy7`eM}+)C~Lq2(m0q}M6{Ez8GmByy3EPTJA)c|a?DmRyQ}NAS}=9g2M(IyuAUinQ&KzSj>vn z*49ppjp^38+5b&>2}Q(JSokKTsEkZdTFzNwtn3otqRWd5JSGhaTUj|d0gscXmtzA1 zDlRp&=jgD!?D3F=8|r8txS;&Fz^D&!7ZCx$U~+*n?wGR%an}Oy@?VP%zkmPEoGutb z-#+M;{^?X+fA(lLQtp;nM2levw8VPlt;Iz#bqoiHXAy7uX5FHpl_Pw>1QK~Tw7 zNWL<=Ygen3l9#uZUK`;7w2mmXF$L)9Y zC3%nyHKTMgAp=A?U;qXlBl^8j!&D!88mieR{Ei zS}b==(8;kEEW3wyqKq6J_hF2IoXd87d*&46@9z&o&cQ_YIkaBD4G_Bua&k5slwb6{ zPFtp?JV9{q!x9n_f|7Q2Xh8LK1$qW3H&G-1F7kvLzdwGkbH+~rif>0^N4V0`!a|$aY1-02EppPsAR^blO;3GIMit)6+}p@xcohBU_m3wGR>+UkBT5X0K=lnVM&#NBmmO^-(8NI+!!IJ@^?O!&6S-h)R}s8OYYkz#=qCVV_T(f18>bgkdnrE^TvQ#Fa%&0lr#RNa(E*%bfw z_L$4La?m`pqgh`7v5j)w3n?F@=qFv!QLl8u&P2zXN>%nv{bi+9*`UCoO+frP`cvo1 zKDNHs9RDYvBJ%N~j$j-pP!ugi9&v-|jF} zwq~_~_iMM@EPg$&u9IKWoc?r`j_M({x;optqw_%G*>wN^K03ok-K+Ryu{~ks9Sv)l zWsra5C#)vpr#em(2hX+$eyz@B7+K_WcrDxGJZ!r-%_#bUB^=F4*VEHHqCQ=6%VYAt zm14@Q%llTk@I7HAUhKY|CEDpVUPZZ?qdTE9LCb2t(#Uwm7|lRnP?*v6FXW(8x>%@!t4f<5t+Dn@pzvx1BvGn_Yv4 zgv<=Ff0iRo_uB`uuPtQdX`h>h4P@_*-tZy#kB=ntVqY7Kt)rb{_Aa($4K1jnT`^c& zWf}bK{`<^le%)&2uU1)<2kM0Fl(c-3b;{*9$KI5y@dPKo3*yCpLFK1#KRGg6m%)uB zi8$g_l**}{!GxM6uP*vTDEslM#X`UMfMk-t4XZ8{F(=m7ocF4@VlG?MNlC_lEH)p^ z8Xn@Ic8C@BWUv?j%g2{lu@HixUWrod3wBRH#aOkpt0$RTcH}E`7tEz6YAB+ za2htYQb~{A8EW6RqlaUPDWk=vA=@C;&f=SBx~Mz_V+?OnJJMOmnzNW_gQ2%<#IF1qZR&V2b443 z3cxDdHr4gN`uH4I9UpIlA`9~h&_n>0}YhbrTQ zmfWi#E6eY)@hvykLQ5;IiY~dg0i+){H#Y#$&ma1OL;@fJrvAUj#;O+$WZ>2d^nQ*H z4LO4z1SQZR*b&e%^gF|9*g+5jYqb*dlY;~%$@d&?&Bs!l!^5V(X%x`UGw35Vq? zqjZja59JO^sDFVEZXjk=BD4}+pU(f8JtyvBQ7}s&eAyHbvGdQ;Oyb>)CSj!8h5cL0 z%#@?wVk+tqA%r5>RF{C(UO68Ipv_-$dtdP*%Q3)(54Ndv7FQ?pnAO4hs;Y}?Yk8TO zkBEt%*y4T`1(6=u#~fa&VwK0~9tbAcnVB4R5+Hc=B=W%`)xZOz8AVeai?nri0);`U zanN}d2$sR@L4f{ew8RKRh^l9Nd=tNZz1W+Om6Y7s-{*r#AUJP=`{G;$zd~j4I4iXb zQZSbI;<1z75~bPYUB({uJbluZ`SR>E^;>@iThfRAbbt*a0oMzbHWSKr)AY0i)$Ugp zFyqM2$}$D6G!_2!YhRw~6S7dcVqKWgXv@hVC;R2)*D|Gm>0uDo90Y@^bwEkZK?5@& zFu)*{nwt8@8xjTwq1%GVLeN`S8!nW;yxcpz06<@+Q2<`1_9LkX+Rp>!U_k=?1N<5s z_Fb+5$;bp1)V$-o_^hq zbJ~Pkd+g5LxvU0n3IHZx9$}M#t@8jjsR3!~b%8@8I2gAQs76qw=jmQ%6|tC75FT@! zpr^aaoE`5e*-d6HE_H2fV9|?(?|OTC6{Mvtr4tw!7+{7J9UTp_NH`xO3rm)e-T04O zSe5VryS@MWd>k}&c`~P=OM+uYuA4GLW$7zSA%2*4uAWz2t+gzRLLJM|jJ$uYdA$;p zRWmi1EVuQ<0;ssaLN?mqeGN_p7#;xhWe7w?N7rfa`ngfKk_6BPa0U!w!o4rKazGmb zi;*?wIcRQ&Bj6DD^!uM=&oZEU(Em@*&nxX_k~$Z`PjYg4%EQNpcKZYD9)QhCn8F`K zHGlYUQ(u1uranQHA)h|IE*pc?l@q)xM^!8mHn|qWTCefby*xNkO;z>&?GJ!9SaoY6 z&yIg|Rv5KD5N-n*aNV2X=KB=57{Kmu>#y6Vku#6SpzNx z$kdn1?O6=+v-uHBtKlRx#Ixfva@bJYP5ye$Hm#_5<@L2y7eT2tVXQ!WS3>TJ;=&RT zL=ZK8f5ZTh9JngAALQCO0Nc@wiV7D1T;SjOJ2WIGBLkY^fl4k>(;;|4pu!|}F0yF- z$SEq4x3n=f&WMkXhjgm)Z=HYZ=C(1@AOvz56_p{-I8eXN!K&8)Dg-dqu&}Vm$jF== zdf`tcC7^z8PgEAWO+b#StE&rvCaky* zfsqP$o(*<(%KT9S8Epc#8pJDE*{_i`X1`$7ot96_q~hHWeM61n`l87D8zZ38Q1fmsfr8YhLFym|XlF)hZVc+-%>)GKT?0>AbAGydZE_ z;9_9hftCk?R%-|`UAj;=%=BUCuHz!gCMkR`zB}5|?(`HFi3)a769iy5zhkgQlfxq=-CSRn@h0Q290a37 zrS%y6AAd~yS*$E*wvdJ)L>!ioF(B!{8Wf&`5C;~`S(wp)8l9;I5-{je!!rv|1RxeV zL4j-N`2Bkxly?vcK~;W$h8Cfvt)^xQ(%9o~5E~%t{40DW^L3jNOBf_VlUA^q@z{*B z(?-07wFV)hgWm56EiNiPvlctN7A%a?g)k_AQhSzbiPoD^ysj4w=27jb>#=!o{<0bq#za2lD6@NB>ofq|$`p5#}G zAIdLy-SZ2BwgoeJLcUBZdo|I#V1T9qc~_>vl&gAB4-O1#YwHZP z_Dv7Y#kaTpe?_E%iK4ANH6igjSGCbt*=j`D?%kXMQz7rQ;!-!|G1oW!Bub@G&Xj?~ zM>9Y9iL~SPUZPD1E*IM47kRM>T4SSBp8mXWXUoxU(Q4{K_;N1fD4#VI91Wo73RMt zWnQWhl^GIAyUESpG-|1eQoDc!f<@=HW zNPz&O-S8&>Zr(R7hqk-P^9>AaKx7X5aubnaB1#3(2+oiiUG*59suxK6Hp`r+XOC}OEm6Sw5;Pj6ga24x;9JD6TqrhnENpF>C@ zy|RlAt1Xa);MZ7yjX1RPD=tn;GlXIdBSf5ro$kmz0P}vRWF)kzZ2}dg< zXs7no9crU^xY86@pU6#eU>u||8PPglsQ1$yVx-h89%&DTkHH`pM4Abd^ZkVXfs6(# z%fO?b;}-x;!prLl+=SV(u~g@UiQ5YxDddIBSk!<)%f1(SH6$qwcz-(=J1#v8U4Xv) ztoDe#eLHMzO`PxKOpv=`f4jp^=F~oy<5Y(-)V5-feQi9?6VW=-`s;nV!7qwnOx&j) z)PUijjS_3;7ZtJan1RX&?k`W+eaMU)cI7c!=u|K#03Y+lKsKiDZnBUac$m;JF+qKH zP}yp=vJV31qD!-L3K){ zUjt;%PuAPr9nx+Lcxz#4siwNx0$?@opW`pB^BihN!AK{sCKF!7*e`M(!DNUGswhW# zzYP&{=>SaM*7;`y=%+~vxO<>l*qt5It&c<$H_&2S{^stKbev=BeW?2NJ>x{{UoXFE z1%=1>#LNSsS>HD$<^T!VeywZU;&9!8v?NRAX}J}!@f z*}BK6gOL$(7Tw;yDsY#@taP7%aOB*$;Oy6QTp)u;k+{~IJd+p59yR@xz`$v6-(Qx2)bD|ATaf#tsufIzO-_LwBI1ruJ(wRKk5kNO*cUqCYYtO8qc1 zBZHkY1?XK#iM?+=3MSdc))q@~+ds!&|K3cGBRyR{9t&?|wqxpflSm0w;v!YG!8I=Y z3DtKj1-CmOLEBYSAY~()e)fc`@sBgz#8~F90gc~a{!le zFtCE?9~kuks~tqW^_xclzb!gu>Ji(>A^(dY++9W^=?G!REit@?t{etjMPcE4zKw5E z(4g;)zijQ$(2c@pFasl`g^gtM-z$64{ zLd>M7w)T13*T*3kqmk}~$%Wu6gwqfJYe4&C(XH{=P?&;K1=d*s&})_fX~bO_837o*qQ;xi{P}Hdelb=Y+wH3I1HNpl1zO zjzL&eR8)j>CiUlAe8O!o+5&p8sPRCKks2oIfM=9(!AwV2^cCy{s)~vfxTxS#hP}QR zPKDVFAOkSu&=A9f0la)wFeC%S1-u2k0N`kEyQH6%PFmH1{DU$MIUsqjy|;G+hW~wi zu)Ihq7E|bgqLuW9&)(c_+lM;~8=7h9B4@|yPV9T?%nO2y3IZ|5JIY>@>uYu6-e?nA z<`ro&S*HY}+U6CORo|I?Uj_#YnpeGmBLEU#NLI(opZu%>*bTYepA7cG>-FLWEe(x9 zZ;7x{aK)U$@f6TUT4E*C)P}*G`Q*tHxTlr*`GGnEfUY$FAiy08?aRW_@}V#cf|Znz zBKrLNJk2}s&jNY^A0{v>$)j(;s!PkuhyMOunQ6F&*0A~qB0%8X5xhJb4qtSml?%3q zftwUY%Do9Z&+X@$eY~#=HDF2yw5z!OEDszw47dIpM!A5&5awWk&t+g*0cS9f2uw5$ z5Gza_8K`w5mSu9pg9+@9HZ;#1!4CuELRRUOi3#9U6EK<9e)$-qc*Da(l!}r5lOY>} zFukMWS__)kU1}~J%5AUq{PNGO0=CQpfz|Dj%n2oSx1vt*`OFm&{pt`cH)u z26$j0fWbhZ_7ap$7{cQ{0Beb|a2v>C)WqN(0Bbz6UItyxpHOmrSd(T0m=M6RB*&8- z1N%;3#Zc}D2nolUe2CFjL2LuUfQN?{&Kiu5>JLYBkYh%i02nia%^`u)hUoIcTPZ-J zs{m?YRIW4Ut?nQ*vbVnvmJ^U-R$IV7yIcDcOo{a-ozbqOSgS<8T9=C(=$#L*f8q>u zhjetSI>|CO;om2V%CwadoLb!Cs==rIRL1U$hI1}V4-O_;DZB$sq$n}SQv%89sG3;nnDqua1F}N*J{_$ z{(%UupwpC>gALb#=b!G0HZ4DjtyN|{`gAe8Ge>P7o#p7|aolECndK)`+_r#vYDa$W zh5U&}eQc~IsEB!amKquaVoHF6Cnxy<%QM8YYL;d|$d#1Gu;{|kz1{tNE9*6L;EgaP zkFb6V4+MQ=wbPvHD?K%}F-(#V7{5yX_@A!2=T>(n0Wj7crcYVTOxl-W^vGjIAcMsO z21>(JCfEj!=fD9(80Nz?mmv^%Qv5Tck0*N+VoETJc6N2eN4<_k&Vn}KJ;uD}|9%h#;ZX4#5j@6UVx)pa2G|@@&v~pC7 zi~XDUtl__2P(5Uz%5oLQ4p&aB6`DT_J12fObMEzTz|%D_DP|{+U2|<3i%w{4Dr3`L!y%@+9zxUMz+-3{2hu}5*3Az;c4LqT=K>dSfdg4{t52xPd z=jLEEl>ohkfPkJS5{!Vq-rrJ4;O+~4PQgbFj(~BxQQ<}39PI(D`B2sD5%COR`YzhZwx zO6yU-HPf)Yx+-;#5EyEnYCf0};gn9@&z~j@3<)%n2rcfL6efkY^YgdM>~vk9Y4j zFc@61A#dEl@$obGPP8RBvB5_iLh29k50%+CfO=_T;{qnwVB#`GPL;3DtbA_)JCxJc zZ-DGT=Id|)vK>Gbd;FG`hvW+0IKjmW6G!1Tc#B%G-qb9ulZo*A7k`^HZoH7XJf-^_Z0%I|;>8Ew_HWOc z2M=!j8Ki#eocWBVz14xrP&%B=3XKL8fQiYVf$VybytRx7$E(qsEkR$tWF_Z<^l}1* zQ2-bRKtL)5vA|Y_4Gk7{7W@=&gJ4@xVtj-Hhb*<>^d|VTLA0PPX9R$VWyrXD%^K03 zv781<9SPhuHulSrl60wh-+V7ZRxs-v9605Qv&6s>&v3fng4On7uangzfZ75MF#x~n z>Fl(!esSy8En!IrN<3CQepsxl!z2SZ+zV4(rvzAq@aLA{5@(bgW^CTTn1q0b1L}n` z#%4YG^o%ckW~uM>^`G$@vpCPZ_<5W@u^)vRq4;|2V<@$yZ->w?sS47|Jm~%$$mIT2 zO@)HR+NyWS$!?Lq!dYEi*9yn^XXTe$JB^@r4meW5{X;^)>41-C=zXGn*%PYW0eptDC)u4MkWuuY^4WIrg<>GS7 z%B+W4CK@X?##%6#?d2g1Bw<*(?!U`0gp@uxwX$@z%gMV}*D~!o`<}kC4nrJ*z8w`E zU12ryMUMp9D{xIHPrNQ0^zTFfmO-JJYHFH;Z3lC}$$qcU>gUWePU}&y0)Wpz<9dzJ zld|;tcg4KvQK&9FBt4*aL2r%MyRP{D{clHybl%s-*KP89aRd)3shOE=S^^)zVn)_@ zG172xArGm*7K3&%$EEj42^V?1PDuWar95h8emiXp%-n080*Zi*+fW`Vryube) z$_2;?peG^S4|S{F`~&e0w2p8OgIA`eS@7eC_8+CC{eS;TDk!+RxMZo5a=&>&k6RB@ zCQ$5ms5}TCT{dK z3Q-3LFe?;4+6x|Z4*2%=GWooTZ;F%TSnFAlb0ue-c-znmi${2PunWU*G!?kFQ&4Q| zSo3EbIGZ$m@Dg-uqRyexva%Q^%~G2Q&i0A%+cz71ZrlP6b&nECD?B2^F@7-+t=SM_ zF67Z0K!M<$f|{HUvR(;aJN54A1tbM{Q7Trya^-kQ{s40>bZC@2ccgl-k!Bt!&T}nQ zRaMZjEhek%r>CbuYtJt%yo5uiV3471<#~5h0D%_~p#sa3y)<-9&&*8YCpk>f-qr@w z7D$%^Luu*2s^(5u_LY97?c2^H2tmP{H_ENYgdUA(m8TgR33=S}_&RI$m)QwIjl&iUPo%S#Yusf(V8^SK|; z`!-^6A-#l9L}6xifYd%g!lZ;wq86?kgOa<=lfcRiHRthXZ{NnFjF{N85 z=o(dTQeTlqLdwDH53E~RU02LX6%Wrf~(eH4zK z`w1JIHbmiyQ7HkTb8y&$F-6>^xv{dc@{1S00JLOgmBvXhW_ z>8PnM4h9q;=dC)v!^t`p+GFr`GYqLVnoeJl6B6kZ!%l zTM$pW5`h}n7MM%{^5$* zSL@_HUf4hN!y0mJ=pz8FkV3@NM(LH5p}G7kYYPXw%5P_t-wV^fn0VuGc|W(q*Ok$I z)bdL5=E*CdSn&HckU6~m5l9_ar-w#H)RV&20L#OzzX9Q0XegCGj;D5bmJ|eLP+!M` zMKKU5&)zzllw`KImOkE&sq4sL#=@^CANhB{?m*bRyP0A&BrW$0lMCUo2TVS$(g04$ zEfRm$C=;TXS*! z?*SqO9jqcgdyM#l{ERXJ@yD~4jS=AYpKkECDJ+-uK;LME6Y3deW~pP+B`g-_X z{9+YZiPbWap%SyOa%S?X>x@RH8;p*nOsx#-dSZ-&qsA~zg9zb4hP-&`?C0AGPoA?Z zwkCE4d1}V0(#-a6im%lq9!!M%=iabRH|a2>pVIw>Kx{t(kzDcV4H^%n(X;AR? zV!gJSF#y%z2BH~m{MX%ZO>L)!1yytTLErv`u5sZSrI6hIB%8k8=I)s~mAkrD0=U2` za*#Szoj&ZCy$J*CSi1DX=a#5qNXL%h_l>b7fnXhRM-RR=t@Q8A2p@D-PqGnTinHqr2~j6*BG_-hT7+ezzU+?J`|I>R8bE$ z>|;OM5$AXEff{WyPd?C5$9%y+hCDw=ahId1#xF2<|I~hY&oCn2!P3v!iBID9UzHgb zK!~~+q=i3%l}7OpDTMHRFZOpuKY$@7W{(`XF-oUd%8D|TmC6IV{guQPTLS~5nT64@ zsK{ISfq+|aGrKAMO(V+lCxsyY_DheqT7(U8wj(M-4kK3UcGK0xCGkHisu^(_3=oI~ zM1$;&lw26fIm=ChyUc*6FSJu7$nB9+ZAGB_U0tX}dcJaXBu?B!vK%dc?jv?Szl80v zdx;&c7eTIc!Nx>)28`iBrJwD{>s;PdjR#M62~3CE#YRw(yYt`imA^S$;{Rxs{rw14qdSTxRGk4YaXy^5SGYl5U(=R~-XD79A&^h%p{wGgVmjlJAL}Q40!O8flQ* ziOs_v7Rc|a3&VG1VLeh+t$<7C*3`$mpCXVSDJmSFFpo_`#z_|{W!>sE?Yi>U=Y zr#u2-jtZfe9LZ$63t1Dvqtr)J+z&iuyo|>)SSKLEaGO8#f zaQQIX>iF9AMD4(L+MaUESxjncN^1j5dcJ!fW>HZx1D(@bJzp}HztRpZ6r0E!+N+?d zP_x2+S40*w-(N$H-yB$OneW`{|A8tkNAU&4jV7WaYU=HPT#E-{cl=_}FTBa!O_F3y zx7>Q}cqf@Hk{b%-X`2tp&hzIs(p?q4rWF1&tw1lN+Qg7o$3GZA_s+s$>`HJ?8f z2*ecn9Fb2^M1JJ46zM>tXX{T{5=jS%6aS|+rN|&X2c7H_f;qtfM)e#b^f;8Cs`=f?j#EJza`_nR7e8QOAktg zVOtO@)Ss*nHZ)P4w(84dc=|teT(*@GMF&QQ{mr;0$L(s~tYFEo@;yezzK{IJDwG`( z)xa7j&FN?DhmQThcgy_)VLjf(jt1Tq9Pp#0yqT=`hf_8NR(c8#)??i=5+j!5j?km* z1V*{e$VcWx6R06~ZZQjFp^5!2@SN&{dV{DTn^;S(ajXNzv|Zz3yFV&?s+1QeRPYC8~y)O z_TBMZ?*IF$Q`9Mnk`z%gvgJ*b^`_+@E0LL%5XmYMg{)+SqKsr`Z^G#$BSiK{3fZG% zrSQ9Mo%7Kj-+z7{=kdt;w61J1()C57`a{ zeR{QrM^5qdMWgStzLcjbYD5n+OIii0_TOF9;V=7Q(v2aIUDPg&BOW`vvw}2iXioax zUS55lNiP0??NG4okax+krE{kv4mYP;FDHe+@NsLEHkt~f)P8fo7Gs|zzi~i$oDKlA)t-f(%f7~ zXmM_2PZ#a;h!Y9BR7VavYkZ*MJ9`zHMmBFi~|DBq(^iA zRjwb`)6vGR`B9?BW>{G!`UCsw6diATWHXVPQ3l9hv1tx8?GW2Veg{ey!lb2d-+h2W zg88luY8Xe)*I*MAdL`AWQ7pp0?#so_}-=*e%WlDVr z&+1}`*mxY9lF{HZS^q2N4quyQ^YLbBcmcv1ggrpQDFRu zobVTB2fdl2PIrl;wS&VPeE%e9WH{pAIV4jE?o3G=lPfS8EFMU|=Pw-fdbOFF8ptfR zz6+wFR~S2sLyb-Im(!z|zYj_IJkh13ey%P$)E2JCSrW-vb}MHzC?ShC+G5Xt_@QTJKga5|Jp@cuNt?RZ=>R$$dXm);n1))^HQ>a)$ zL^4uW2V5R0>3w3^yr(j=vTAB-E>GRKZL*K4W_ihY#_T|7{xl+Z1N{L$ddD<4&Y92l z-_1_lH6P%e@geD^`g_eY#JVh9_y-MO7vD;qdzJLr?i172C(?%xVKw<~AyXwMvs%B9 z;nQI$bv~ktO=mdQp^6016I@a9b36{{OCi=PV&93nZGGWi2F>B?V<_r4#_w#S)H+R#^ru%GdYhK(KsM^1IKW{GxBgx{~2Es@T0HT46;660w zACJv&tG6Erubu4@(R$k3Iltk~WQ4%&=X#2gi{8SbD`I_pOU+YO_W6s>mJW?HcKw?&v~=3{qmP|3G(zOpVbLWUo7NRO zwsghEM+Fg~6%`dlt{7CY7K5u-uOh@@W;SdqrKhI$ZCs9| z-d{AuUsW>{U9j91 z#>>X(T^D)ES_bkTj7&ZjKmjA*MJVqey#)lY;x>fmByauH%n6W>S3MlM#q$o)0iUlG z#m2JEY9M>5>y<7@snK_>MzlBZ@xE)Hw|t+|rloHi)oXpTSdv{;#n#>+-#pd26K9Ng zq$W+)bOhf{&n=5HB3bbTGjG-O&q^-136t`c(^b!Cq_c&vznja1VxvJg^#_T0gy(^8 zMACslYD95rXec~5_-W@w>`5z2Q?7gW?%m7^nmsVJgo;mAmP%^t$jAutOs-#>Zj%;n zW?!NayB_#r*~!mwG-5Aoye0MEY1=cSPeyMl8fiNptX&s2+3fjLz(R^f?bMUY58G6Z zGi&8~-(2_Cl{f6NKmw#8QF^IO%>UK^wDEH4i&s0`q+v6(17iv{8IfVz^%C2jiS56A?W zpVZOSML?gRYns+EkiDU(77)_LF9GH1_zC&^!kRY&9nfk9-C<~x2KexTli`OSZT=0 z6I$hDb+DQnqQpXKjG<<8MeGDv+EC7fN9NA7@i)NX&(A*r61z7{l&iT8@X&6Q&>WIC#zeN#-UB#cML zdyolhlfHOS=Y&CQaKdPJtLr{`4t=ij`AiLkZP#Xx@*VFM7%NlrWM$R(Aab{Q%mcx+ z9TF1Rh8wN;aq#3z>$vYe7n@WQC?R=3Ns=&~wdF|pt?kXWB3YZWrtjM^(i?t0D12+# zXsZ*HEVE6wHJ`wVWx@XW$;mU!P6yAqZf1(RFhCRddm_7`Qp=Mkpa`4H}GVgD=sVs|Hx|M)sb93OiO zKD`~9%Ord!ihE=ocd5PI3F;;s-XX9A8`{k+$KK?`F?LgD&*Y9~5w<)9gJ&U}SCMl) zL7(^2>X&MUeIGt!q9d?dCa;#d?(9D#`u4&5Vr{h;Wv)`XCOQ6lS%>-UBRr0lXMOzCHEL9zvm?_Dv7@!U&TkQA94VLrsF zr*7H%hVMa-1bb1CPSClzXIZ=f76UJB9}XWie$nG56ZXr^`di%FVm#r%=+nE?W1*|( zXjm^07RvpV`!<}9TDo4cj*f|vCi{!r>HyA>b&L*?SYIo`yAeC(o358q7=AwSvSqD} zVAh`n+aC5Y&{}@>Umd(2iay5L2X9iR-b~Z%I;782@*^hSn{i-xWL_w1$L7Vlop*ho z|L9ot-lRqE6joC2^6mOT@`QVp(=>x$;7^l{#?Z{1$9HLR=ImE}3b#or8-G1>l9+Rt zhZmhvq*dK>6%pfB`Xx-%Vqsp{IZZfEFFUf!J*&3Hnt3Sdi$=LXmkM2|Fj`mG*3t1W zJiHwV_)FuqNG`d#QScQcR#wEFsrDBYJ!DUH&VBpz%Mps<7_RHcMQQ@^d4k;2#ly=BM9dX)bBOo=q>RurhX;Vsu=2Wr zSeU5zmVNT%rW-%5GOfm~6#Oo}qM!>;)PAh`Qpp*QzHN;i zHtCj@%F)vyyg!QJGvd7IBvFDI{sWn?mO-R;==pRQ7#PfqjdLwKv(HK`*#Q>f5jZ?N zOh~y^6l$o*=sEj?szCNuI63(C&~Co%92{hCa?j5~nMI3{iT>aR81a-S;r!3G{&1^8 znB1#6Hq92;og#+XEh@RWuL~#&7y$tRG^}#*s=N>$1~D8dd`VU| z2X|hHRnIcU%L0~inM;W@%R``!@^0W&@%Rkg5(1We^ytG2Uw8Vw2G$t5ZVY)1Ae}+w zgdpJ?R!~9i2ONE9N4Wm%-~VKRWY+^TMb_&AHFEx@pDIL$3X^82SSubHHFj56J=R_E zxi5Q%x3rP)e~Li*GG_MpVML^e8fYu-oHe_r=%ZRr?CJ!7^tgg)yOP z)d&}0FN7G7cm_>@RW4Z_MjwO}caVXi0y;0rMfl=mM)UBB{Q=2neJ!ozPFNZ+&UUd5 zKn@ZB33pzs)ERa91EH1qUo2CDb=LzKQFwxqOs&W z?{48D^`3b~$zw_SHfwq53Z&a9qze)gHB!5OJc299z0()cgUA9JVH{ym?V~<*_AI}c z*bRsVcm!Isr~Akqm^c`~bWDSd_Vw%6JIH#s~X=Zl}F%F4@2%gD$`O+7*)L4Zg#`N5-?Y1%hZ&(npb zDxN2-|bITlZ8Xf zk%?ShUTzs%Ty&a<+rz-1T$=#HtYO903Bs{$EiE%s?k#@#G}G6QLuB#G$2XyD2i>6% z_(tk;lFC3%LtrZ&_i>MS(@SmvflLq#pw}XQ`m{H|9GiN2dLVeMR5}Up69|TYekCjV z$OT75G4I=_a0TkVYAVkY1q4{M^8?-7!uqYOjNAF9*#>Y7P~@f@w1L_;q3O%cUWo+f zt5@!fV6p(d7O&Uc(NPBw5ahA}Wdd7C52Ye3 zk@$WSHdU)5v3a4T9s6C83kA2|`o-6~rfkS$V!yF1?Go+1d2r_zeV5r*IJd{d#0Vr# z%XMgE6Np4e4&1TNISc10*N1_TmP{4?mxv`xY^*{%!diq0C{lzy3$qbIV@>xwyu6@! zS&)?E9dUw=>JLbETrn~_c;gz^-b1{+_3)5eTW38}Op~>8U`~pPS~3V(GiwzFj+o1b2}KfA;z#^0`P%M=iZXF1{gxlxoB=L9L+h!pPA3 z(Vk|A4#0G>dp5-=fytRHd8OwbIaWa6ZzE|8MogO1=u zQeg& zfmVnDEVlNCZa`&zYlYeBv|9a|@H8%PbHK&j0LTCKox`ENYcG*+-7C3vw@5E9%H~7F zLnB$z-|-3w1+RT`u6JcQeC?x#re^ipJINlT!9`}f@#@K~=4&_X2GIPsM^8>pu63uV z4j~=;>z#gUERKlZuF%|y_a@$g&o8hT%jARaGn@MVt`YD&8?`AOGw4%)o$MvUTg2*U|LKRbRfp{0uU1*q8`yD27?&d zGysxNP2r{8F8mK-#t`=e8)@yH+yG>4rehcQ-z7aq_ffC)aY}yv8~`rAeg9Wiz;KG& z#drgEm3C%Lf49Y-5H*e|-QI5Id6F5wHtOM7(8a_m^imoHb>o)QXBRzqKJ${&oM?himTzXnXO^??S&_($b>Nvi-jf4>LY;I3H`X3A3MRqYa>FpIAsYDK_w&{U zr!M^+5GDL5IIz|$heB%5+5ZiWU+n)z5?}TQQRaVJM;fs7`1$X3@l7F7(M!KJi9Hzh z*{R;>Uu)6?jZQ@X3{2Jk8iDr&Q1jOYqqzxF0I08D%a6ys@FX{6m3}SMu$O4AJT`Vk zMU#K0I@k-{X+{lkmC;#1!k|n^Nj?oD6YC7o5B(VP z9%9P?PX$`~ZmX>wwne-JGE(H6k&hn*%v(753h4N%Qqt0h5+u2oo0IAr8E z2B)87p0`*;3D_bZm>+3rt>e#ClauN(O~8|^f4jx=05|s-czAe5%NZCay)N^*iaKhC zLsgS=UvuSJG%Exzl|F=c56!-#%0Ny=f@20QNy}JfW+uRuj^oivl-P1cl+vl!lY3) zgh(lkxz5JShHg%PsgCM%Yc!x|WGkA$81(dt{ zqSiLYyG@u$uiw6Y* zT5>WM)Ciq#O@dhf#B(haH;e7LNpT6P$@22@NshhO`?oO&e`9cuP*Vm~_6jI5A+|{7Y;m{PwRys`#4{O47*U%Uo9_~1|@F@9>d>B*`)me#!3Y03EZjp=w1}XXoX^sBa+*p>&KQK#_ui z@Az=mMM1n7j0o8C_(AjAZe~Kf0JwC(nDz24JDbaPu(Cm+A+YOm(em!#e#XK9_d zP;lA&$`QGu5PyPNMG$)e;R`TI*z+kGsmY$zkQ-*h26X7qljAqCQCQ{J^}Nq4>it?1 zT&Z{OZo*-Hf7kc6_aFP&)uQv8C%QIbu^74gppu!I2D zjM(LfWzE+F7_vsH#PQ=m@m>W46H67x3kvtZXSvMB<2-4y;40;<%n7JFK-o{J4}{db zjgkN|xZ~1(uiDRH;o)@T+>FZyJuvf;gn}G^az7f`Rqz13?f7+ri}pj+7Rf$Fx2u zl7bDo$h4l(+|^}CBfAOdZ=&R-*aRz^t@i|oNHI{zymHdc=@tD z!`tcT;yRn5y$Y8FE;tr3Pbm`GR>C~j?Dcs;4FztITm z!-2+PA=J(3mPz&$m!R#e(B4>#)8jsa;GL1f;*TJ^Uhusf6%>Y1ZYl-}%m}4m!+;)6 zu7cf-9zym zMmjoKf7_$`z(v7L!{RwZ^F>79WSbjj-zqDEKF7Y$H{IRGs(Yii3vqIq0am*!J|6jy z=nOrG5-LPQ-Z-vjb~V#KFE2UEW}!MLRQ0m7w6wDH-=%eReRtM_WpiyrmD)>oW@dbR z0!K`#aSd(CrU`yRfDbfJ$co<0cm2i}-)?BC>WKJH)$7-Sx+geFu6`oJ<$=Hah!0+c z8$E=G?%hLRrmp_j_wwdMQQIP*PmvX@0#-V&fh)WgfccQ%U=SW&waicI;HYkeC5_rj ztG0t1pITz@8+k*M*6%AMp}y_dai_6c<#}7clZO%4!=AbWM=y_*RBu}weM2m)#^IeH zw_z#5J??UEu}v**w${d1-e7>#n9VVhC_J`fWgSQjn%vdGX*M7OR59C{9gh|JudPsG zaPhtdNa4dGOkuFJ-IyQh#Y8ybRe-U79tILDcVItUA@9i zY{LDu@&!=G-&a(4thT@jI}8MvbyZZdAVCL~3k0w8f2vkuk6yoiePcCT7dM&pw^^`5 z^6>ER@e%X}gpNjhqXNo0^Ip7oL5zgU3TApr>nI<$*B-<&AnXj*ylF91>fEA0uf~D& zPjj;^((Fh!j$Dg6arV+dmDNnhzUFgw*$D2y)z{M06wsw2Y?=g(R6^w++Sx(ie=w4K zamj}E^mpd)p;t}7#^%uQ@F~=;_~)Mke0=wJrAeUI?Xii-xNo{B2wTeyL@7gX-*8(n z08#J2p!jhMCZUJLcTuhbJRuLWD={I zTT03mp|-`vaSOlO_$!jQLWF@UeRg1o_2j?l3 z3>cP>>docffvlx!@=WKcz#3yHyF}E38dHZum0Nyeql~x36I0;u!MmiKU4$lf%Rq<@ z-wNtD37;+Kfcb;wz|K{q5LX+jOI=xZq34ySctKT>r_>BqB3NmtBBqgw1mW4Vtswbe?HoOJOzj6G#ra7e>IzBt(LBH`00+-oaWH<2waU3OLeY~fvqXCQ`0;yac7lLxeiPjd*9CAB%dcO7 za)DI>g~mn=0gB-S@F1{&z{pdJJRJI_p}`t<9MB^TOO^;}ac8_Bgo$8uPuQU1U4ZLP z_Y;1!6#VOL(wcd|^F16oiIWd^N2Cg2vQ4beNG>cY!dzDQVRlw>2f6Tl`QiuA({_-f zO7=-70)!7409C28km&}~6dKj610WTE77@{d;VZ?u$Bx+7k3w$X6e4YqkYj2Prpfam z%ZPIJLcFSqYwml@E~sDMYuhLciUjs=uR)RCWs}{!hkK-cdqdf%i-`bU@&F9gvxo*lKOg3*w1g1C`Z5?BuezR{@%J)o*Y2wr1X^B^=#>JMx5$C4D#C6_?L6`CouWDIoD4tU%3*8GS! z*0z?psPctFXkH~FkH4v%2@@al02|Sd`SDIz486+kC6we{+^OpOldS znZ1_WXPc?0V#1bjE`zm!UFofvy)@-*=`Wp^qIn^h_2R`(P~!9oteEnwSm^zTic5w= zZft)DOST`!Oag`*Evr|&y45jAi=w4V>NiP%Ajy%^cn1nwy1++ZI8pWYl-dD0?HE0`q91QCwUjpov^7 zBBblUle$oNm30=g6A|h}dTKi_a_muP?#ao?k;!*V^{yi7GC$R8U0e0eF-_atM#Ts$ z6QDmfv*M+@c9ZxtGo_m!TSFQcHH})Wme=<{tl{g-jKW`kL2#&TYrf{CMxav`;Gm)6 zj_ibq7X(m46odABwMj`UeA;%>BWx3VtD^$wC7Il{GbSR4bV`!V|zE z>MB9JU^FppaAQ1(r!3nWwFDXs;Lc$#fzjB2#ej~2$(!WIh+`$!SrEPjVhy8tP-TZU z%g>(mJ(|E_!Nvs1KL^zU>YH^yJVB7GcXSy=2dtEo-ddM0CG7eB(VeCZdq3DNSgT8& zLrwK3HoK)aCLH3u&W8Yk`JLNfPUAf z9@K}nLS4^b{7>odPPpnw{ZVdZ$i!Tz?_tBfhDThnca@Arfy-;2RtmAoJUWGhI|9=g zOmS`aO@@X~Vf3O;Dx zyY^QY&l!37COs;6hXSIa?QhM2&;o|9F^{?j2UQhLxa3Gy2(F=<&MMM@-TasH#aaZb zICsJaf3927*4lcGbLZ&D$PO}sR*;q_REIZ}ueaNQnju(KfU!*YF+wdq%0VSAA$*&i zH550Wr=&bJeU0+BFc4wmvc6e{H(e?V1Fzj-G6 z9N4o2EslmsTu%F<^j=T0pjjgegcXZ)mf>8hu3G|UD+*HCkmpEpfleCCZ&Zq_khZ{qCwEVZz?%Y`s z_WUH(r>jR1t#O^)kN|0SEEt8eXQYpeKCOh91P)g7sI5P+uaFRlreZA&%&LNj6aop7 zP{iY4M$`w&<$CS(hbW4v(skYMKRo7`K3Ft+HU|qz#G!ek0|Yx(utuT5az?iW8^Ac? z+bc^?Pm-xfV<)L@EPRPYeUC1_8DngOqwg<0Mt!oSG5wXNe1;o_=Q0_#P)yh&y9szE zu2YMI-2oaEljP0oCr=W>o)Fqo(0%EEgC61+2nW%U(f+zmS3wT1HtwOIcRNW30@8aZ>{5oiHttJHS5_;@fP944HHyD?~-%opH{%_uO&*xqBpI?YXko9y4>#XFYSSnUx3?C21T?GE5{SBpg{8Ni`%S)aO&= zF?1jj5+L>)<>|8oa8{ERN2>TuvGXjTScoZ#At6=AV%?iOUyA%vML}Ka>FKGnv-9fe zs;{r_`5HVtyrZL|nVA_C6%`s98YU*DoSdBH<>lDeSafvs;^Ja|e}8RlZA(kbl#~<* z1hTcY<>BGc*w|QESs4}<#=*fcIyy>1LSk%e4244Z`1oXGWFm{_C7pWj?(RH2J*lXu zL`6k8IXN#bE>>4p(a_Lzbac$k%>@JmK7IO>m6e5sg|)l8+tk!#XJ_}~#R~%igQ=;h z@bK`So*rpw>HPfs`T2QORn@OwzbYy!E-fwD*x0nRv^Y6AMMXtzZf*_?45+KCv$C>s zad9aqD73e?_xJa2A6(be)R>r<? z=s3hcRCFQ+;rF`fk&%&N_H7c-E*2SICP^1g^>{k|PptAV5vw|6ASx~ytGBoJ6;~@ZY1xf$f8nn~lAz z<*kdYgR6m=!_?}fj^BG`k-vK<_jix3eX^!yTz}T}ZG{)k)&AUC+&K5iobH*}pIJE@ zojwV)a>j45*BbGZGTG=btyA&!NN=2`R)~R#Hse z!|wO%@9{8=L7UoNa03Z&!E!24IyV#dnabzl&gNV!aCg8j{ z@l>&+Qc?>u))-{wPhehvU1oH)ev=X~dx2`3{!*9aef{p8So8Ymz)c(2HX#WM*KnBF9jjvX|Sem_ zAybCrAgF8!KY+4dW+TXE;z{F)*iZvpkCEl`REzr*(-rW7n{yGGU*V0&=}T~mh205L zE>GQ7vFPsR^LKh{XNf9%-L11Y%99joFXn$t#czYZ<=t5kbT?fI*(sf1fWZsGnowel zgjL0NytnLFAUb>d2EFUg`TXt4SGdJUVb+p;pZaB9?#E;r8?B?j=X-WCofCXda%8zqM>2wGc@o{4Zo7L@w4lv zp~(a(r7Cu(Kwtj91VS}U)nM~FKF#2xQ_S1&Vp`_H1fox~p^8jTAp(|yCfp4LKiI6; zJWEx-OWnb_-gHk`lh`?bAh52V>(Sul7i0U$8w|AaFp%I=k?CvAT3_wx$GH6G_OBRc zAN;E9pRNn>S*K>s_2d{T-96J+nO%8y3i0r)hr(FLBSU^gX{|5oK?!XAe|-MqfEh+- zf1Q@N8A`2K__TD~u^6wvkRp|h$nB%JMiIrLI@5opoZEk&pEg@$wmET|xiyE3k9@K- z;nD(D=?^J%OLh%QDJ2;W+_8p3&xuQg8igo>Bb-hoj=RknlvRWs)*C{MWiE{hr@{;r z!Ts$E%w`-|a}?jZ2gef*wbFkEsFG6qAK=6JmvO0s(l_~JE?nd&NP zHvTMpG!?0gTf1CbZe%Y%xH~vdp1!24jD~S3$h>hvwRq7x*4)JBXyy=!-JQRE`ZEPl|SXtM~{%>i3IX1#hZ)sI%}Pn1#~^6VmA z+KiFu99k0s5FDV$ZA(=mfiH}-@iPB=Hq9)8Mf#kNdfbS9^(sQ<+${f*J093;^x7$R zhwr91;~6~h1p7m&jN9g9$3^mmO6PPBo~Ir8${g)B4L@~P_fE6x)AsLD8MG%hn@t(s zx-kDyq4j=wg6eutQvd?8INpAIq>WRUAm;CS28?FBHJB)Am6Yu>@z8LjIuY#^s`MeM z2k*pc1|TPyhId>)P(fm5;gY%e_rU_eN2y1-ChTQGu+(vBXp|}~34M&=zk`m^JJ7_r z`ZZ%o*_M#>Y*6GolVUp063rH0O^?8n5f=Z9+2V}PsDj*;UkM~w|M>gnhaQ}s6J0%s-61#gmGWCLo6p!1z?qI^YS_D`4 zRO7;HwS!Np?LEbTf8B|;c7uZaCKF$|76|ZQ`PG0xaub^ds`ic?I-N@=mUIrldQJnbQo!!kFHHdbEB-};MNZm7n zm{(Dp95-%=vJj|#Qq-Pl_9*16n!<=9nT?xq45|rN@`ugVdkyqV+vzRBPS#?&gDSh& zW0aoFN;5(M157>(0p5F6_PMJzjV_F9V;(Giv3{vNFX>b~)TNm`Jf?pE?HOw{gmFI% z$AR@|S)**B3i7#cb%3i*1p+^u&I7)OM#jR%j_c4~bFXdZ$zO1hWXcL%r4!7`S0Dah zL1#Zle!RPlAT{(i-ec+I;ZwY4JcB=RpeUWMsW*gX!yPN3HrET0?@tmpJrh@;4-4Vs zjs|0q)mXMvWiw;!_XutiIMKdzS{kndABugxw0B$X69!}wOxmJxn+Ywx?LLoekX2Kss7$HBzV(PNl7q5s~dMH z-}&yY-V78WFU0~EDBK+kC#NV8A+kE-UCRbuq(%AWT{bCY_v1m#=f?^% zsSbc%C_KRx0&n(=4>&5zs9^CY27~3&OW9Oed3GG?^(p#M9EA4X7T3WznKh(WoZw$}p2 zI$KmYz?%+jKZ2x+@e^Fr2Ua82;t^L9sP~7byDI?MfRh8|rz^Ln7<*}sdCtqnFhMJ< zqCu?pBvo}&;u}90)WN+$QW7l50gJ024pN*TWu^I-=j>^q6~oB@pI$d>R~*tHww=ni68N#1kf8rnEqdn?9!AC zP3Z5G?pM%l$_i!C-Q)m8p+gmJ@&(UGVsh6oiHhhu|KGS0)!eHBS^9Z6o->Wj1MZsa6d+}nW0l~vfHc0yRk%sJFp5~=3h8;fq6UH9agp+~Lui?1hDVzGG(=4)JeraQUF1hD1 zOeztKRz{^z&*4XTZmfaBU0Zp*p5JsJI(ccr>8t!B@xT_IsIvZ%8#BgB><9Y?4&%$a zIKuC2cl31gXLU#$LaWW<%&Wmeo6Q8p(jxouchLl3p8n`Z02Qr?$2NUXjp+584RgKf z7eQPRJyQPl>zAZvgv)H_eC%(2X;1IXM7~3a28gCSnWZH7KWYBnJGkVL9?LdI3-$&q zu~5P7-nr|+Ezs0!7xiXBDyP5OA|N=;PvnN0W%x1k2M43`oo5qncZXNkcS8*W9#!lT z1e)AEKaX%ukcHn81xvcG9F@}xvm3r@w2r|$2Si8KhK+d9EEAr;re5COI%+vDrBXmg z6{zyHcgD{UC^(%9AsgMWQCv^NYl^lSDI;d7o_7&C~v(oAac1C#J;?NDl}M+<6R z%pj`LeNaU~)hPXNZ?ooEuD=`7#2)HK4v2W$oNA++Umv1tA^`uZQtEmAFo5Ot!6puX z-DK8Tb=-f!azox|JS@FNU=Hl$+191*2 zoDR;CheJy1q}>SKOAL}G6~E~g+k(PARSEdYS6&J*$Bg|UF!S~bW#lT@{Kl1`H7i~^ z?pnh$1;z>IQf-3Ty4r)kh>qUZc@NFK zAdDDSVr*=M@3ZIB<&YZaeeTwzWREjctuEa6Qm_1rplrYBv;m8=ny^g`p`7PAus7qoLb+FcPY8W4uO$}P_ zpTBQ{;ciPl40aGR)tBJKuZ#jD$|a7B!#kZit1LrXMT^ivTgag@k_B!?8vvs{E!iZH z7VcGtiPMBxbw)T12hM9aZ1`Gsi%Pi{|3yNZI{0*gd5YGiyHj+=k#hy_5+^;2LN@pElyn#GsxiwH<05E}F$KPof_#3d^Us<77RLbJ zWMh?x1_l0URxV{(vabN|9lL&iP2|!(L`~H?nYMd^_CfC=*duAXlTO|pI2Y0yJiFIB zPfqjsPD8bffJ|ll^EjJ_(vp~o!TTVqi5t8}-dG;WM6WFbDw<@$5acn*^xk~Cr;%~( zgWdLvI>ERLn(uGDL7gQ733^aJG!!e;SAPY^f7GHMj;J^%?A&t95YUx{KkcsYIoUci zj4zMa6%q6YiqNhtPQpv)7X)HH%hZ)jWN3TEYwB8)L{iyvn`{MY6gI`D6$6yf|ATq| z8x8-5@X`{FtCCUS5x%CWL5PdT`S9&7blSBvfe|O1mCw@+jiOA$b2*bI<&yRSMDW|2c#Tz=PwrHq*T#oIOo4{ z;LLkPnPDLu5ApwzZ2yz~pP9E-UxKyz)7T+y66;TmjzT_>{Zpr&16o$>>usqZM)Tr8 z8-rxcYF}Fyy(Kk@-L0F@-zml?-9?jDK=I7N@zqIf9~6U7qsec6`f_Y3C#8B{h(ql@ zL44A6Z`JSk=iU?9eX0CY)RQ3gIhT5>V z-~GP}PRM8rh_%PiSfFxjA|R>WjoDDf%+dm);bj~$F0vl-(c)>m`+`3u-t=0y&{ z8TmX0yE@(r)jjg;(0X3hlXl07<04v5|8)B(~ z(=Eq8nSv@YJsXnGgUTLeaT^lPwlUUIQAlz11vd_Zg~!g`0?-!VU7FKahVRf=sNN$f z5hxc`uYlH&^A$R%1p_iI!}UPR6PA*W8m2TVKJ5 zl>d0y9o&f~X^#rBbYlRV$*nnEdRPr$F_Np)9eKt_psNQZA=G3|d zVq5xRg+1J3wf|z``@-htElxFiuj(fw4eWlU*18aDERG@^H>ZUgpLBwEY(xpi==}2o z+j(db>@p31=QLw^&NO_{6p{I&_%pmwIQu|J2A2if7A|`1Vh*Ws|8w|pxcmHz%iC?= zb^pegH8Pbqh29YCO706vof&5nmv`n2`r#u$-2MA3Q!QK{^upxzq{C_l@%rkOEvm#a zaPsoshz~q8RgIcY(R)D3ZA%E#Me_iQYdsdNS_$zo3)> zHzZ@ac9}`#AVUKQJ2LAaYqP&gkwMOVj)*XK?y#OoLR)xyl67m&FZK%K4`PB7hw%Ye ze}qG#$H!q*Q`b{+u^o5fSK&*AdgsZ6Zq*aLz#L*NW!JI2Q>uvbpUL769o!#uvJY&# z^ZdIWE-fX7&J1lkPTpG7=FGx0%$dYM?bPZUS>1eFa(RMPfTU87=0hSzHcr?$vaUFW#@sNRZRqZ@>}9x!@3gK8B#j^IW9aVdoHd0MFl` z-A<<^NZAc#!+i2felzMu%3mVlu)7erHU%VQ}(dl8j?)8R}22ZBx7( zxoUoYmgCR%G{jc?^jMyG&@cf4anPh9l^;*B>~O$D;ybbQoKQV=t9vRF^FA$fGXE-! z!8c5PqHQ!oy!Z9#7~_ryS)>SDaOB% z$!31-Zhjda`s8tQ>QP(2J1k?aHa&AIo)C;y*DQq1bM`5<5h!J>;2W4 zOm$ewEdBSH5#4H9CkbN=E5NXY=2u;0yswN*q78e9^}1URMz;fw{Fff99#$SM`^rPm zeC|#+9M~O}&qNB_f_ogMEKqT=C&;vK6kglb;TwwWzQ%8HDzVN~YZaTKzj{i{ zj|U`q*?w2{prs?Ko4~B}m;-_INi)+9d(`)WrUZ=tQkTvA@eIpN1HGJ}w`8hnBhRyW(Abj!oyE;1+2@RV}5h+$JQ@)cC`xZ(m1$tL~MIsDAgG z16!v!LWbyH$%9vegtd_!tIqQ&vrXHqfqf*0i6P7gv|(qIg{C$ZJW?5g>qR5|=C%i9 zLA-kQiCf?gi0r;)@egQ07*__3O2C$9GAJ*Q=6IZ516qyiP+;v_s1FKz7+a}zglEMk z3$gF-5MG`mXvW>I21C7}a{yOkbXj?f2@5LrUwd}Rii3%1i>ROBBd^IATl;qfn!89X z-?T@nE!5^1X1KxIkPmwfyh!yUle;$*Co$H~-=#nXda?B>DnE-MTPGqf*q zl&`BvDwlV>s}1A-8gO7TWH$JUiTbo~`@M)!CggvCUpNu!)ASoq(6blRYgM+z>8S5m zPqTr!u2HchPL5!UI`axjm?LH;i}*QJ)cdLJ5%K|jgTKAiI2HIBA@=HnWmv}9+!7^t z4#4{dT(X$^3!p{%6f>zkYL-QDZ?Pqj2YX4vYE!4=5KgGFXAoCDe5zpmzMiDgoo1i8 zjMvPKSV00!Syz}@okta7kkX8Pv|)ysik+c9}Aq^LFQ=>x?LMZMt}$K+e-BnkaPjk*kbFH133>;eSt3{xe(o=Oj_A$Eos4sb*!x zxNXG>6pGsdMy@Lz89JrYcKN6 zof3>-IwzvKR;emo6ei?=2nQ0>x{Si~KQzgsR4k)lFeye?ha^CzBnk7J9@i zgfR)zgKYP=%nZNj*}mDMyNdoO*-T6sm@dzX`-Qqg-N3q6j*$N<9eHxspJ_-L| zOR+KQVdNxertR{!Q)r?M_+F$7ll}B9aRyey1>J^m!ojY5xf6u%>Wh4IbyqgjUS17y zN|(ddZh!B2 zBj&Ldme51;=AkDo*du>iyfn7phFa(@kOjAcEmw;l(eRZ#JDZAE^ca=9D2&C{b1r~N z%mVe#2AewU=X+eyhJ}^B5Bo5cUVD|l>8P~{)rEYKxcX zffZWQn_r0|ncPv0OwQIZ`L?&9)*f#5NjVlI3jkLKml+P{ZyU)TQ;kj<_VgvfupflD z8?|^Gj!75Q5_b~V5#?S}pWF2%zq6FlxVYZAQ4(Giq|W)&s8O(7J=XyLzO3aa@s)a@ zVqxq*|2v0njcre9Jukhc5qj{=(hWnUXJ+^E;1z<8*LzRC2~|s{VHi*!@=ink;Y;AD z-?{Jy=hC+An>vm`F(oKaJN?OOdZP`yfRXKmzo~ zaDDBSmcET+Sxd18dOz?(9k6@VNW)$Nf!6-F0Jy59Sfr!n<~T&HlMP^8hs{Oy*ObV< zIuCc%j03h%5k8)Lq{ChJ#ckyDA5Hs60S$eAG>bcqOvwE)#9_iy2{4UWy18(m)BN># z^UiG+94E|nF?CsBk)1#e!O~7T`Y^Lcqy1etT1Zlqc{Yg58Z?!&n~f?=^+S>Vv%C-3 zi?k@9% zteLYl81v3IcFRKNs#JtrM)eRDO{PwKdT-Pub`hk6Q+hPvU>CG2g^aAZnAyT0{D&$- zsGId#r`g~KAzoW)shtyr3m>W}d#RO*fy^k{pPa56UQ3mBAvZaLOmp`Y;lN};8vX5! z6%6ynX+v(gAnUwvDj;rF`q6u96=8B_EPSxOrCGwm4b<9DT|PFqHjsj2Twdh;u+}ZP zBwac`hJAX~$V01VP=eCf2?EJrOl*hafc)d zf|IYvZeOFAvhBZGmcRV!|GpqtU4z+X=S5vu)W!4D3~pZ!0_u8~VTfa_8JE`qKk+x# zOu}wxz_L7biPC2U?!*qo7YJxi)+UHjae}KiQ*~c=+Vw|*c zk5=9jX|lS8coXB5a>|xc9D_f!sfx+AZZc^u#N-@kWSV?*`Qf1LneeY*ig_s;e1ebLXAj`=i?>1)X2%vpcbvo4u`~kjKlPHL2 zWQl5;22gjWby0udlSv1GpVBJF=D&qK2_ORrLpOtubswlI=+60*F z6<`r}?7I(1!|oJZlFCZ#Y2j)HIZWnAN*;${_hzJoJTZ6trn%OOqG*a}Ab#wL<>p2; zvVePrR9{|aI=ET#2q}A^4|0p$gcg!hPKGRr$bR#5&YXrV~FG>EQ4P~Xa` zzq*erCD7*~E;X@nh2f5&iioo|RjM)y37Qwfgac#X-r1OnFF^Z>I+2kW{AgbU$zw7` zgsc{@PD|>PMsdHiNH!#1^wFSKokUJ{SR6I;A$S`0038N8%;<+b8b_50zAr1kVlkq$ zL%7KfyMJ8!mc?BP_vmB=WlMRKIPt(b)si)g{|>Fv(UIz`W^`m=?V%u zN!r9J1|Kqk$mKp&#?MA!J&FYC5Z1qK`(>PP${-dV;zK=uK*)?JRMY%k=+-~vBKEKqS z@;qDT+ELStN35pAx*wspDRzF2R}=Qf*}tbDZFeYNKeg{1aqJqd;w#TEcN;8`JbUQONHVtpEr zF7Cq443n!izjIhdcIxQYOya-MMwNv@4ZFO8MCW@isS`{;A6)+$ z(74XhX0+7Ccl(41*6&N zi0f`Z>}k2mgRQHh@tCEprw*ejLUyrAR$Orzj@aivEKUrlHdP6jd`PmIfp_TLV>=7w|AwtP|-GvcVkLI_OV3>m;4hT*_x)9{|s zeWQG)!e>{j^#xzMu~drBy%QuATmy|qN^UKb@b-kvdl`*r zzkirg#eXlEf`H2C?cP;2Ptz^!C7N`o^m&Fbg&OoWg`ttRv5}W|@iA_=^tHc$I&qcq z)q$NBv4Z!w80r)!f>Qywm{RHVWvSLING*(d+F&UGSA&9k4R+O!Are}F)I^=$jNEyq zd4Is)sU2#(H`nnYQDjAVjm=AIS4H^4y`@UJ8_K*z+a0dWo&N*nlksaD+s6Ql#`#X2 zqsG3g&Qmo}eCF#Ar6wi^t@wm@KC)h0_KD{pA2U=e)UZL+3mV}g{a>xCd0&ExD394< zHWVsy=ee#iMaS)SF$F@0aDMlF?rp8d4_9TrZO2}gnt$K?aK4u6obI;T=YD*4c6ZR) zxkf#Fe?Q4S?U1VqUFZw~c5kj-Hg-BV#5Ov(+9E2ZS@$m`_S3RtN@&t~l*X8A49|aB z92aCdP@T-1di}r-i)48HdJEP>jp7RzyU5!OvWHKqPZP1#vrKK1`0`OjW5i0MQ;uD_ zJT>6t<-VKpv?YG;Z6l>a``UT?1N4g<-3sjb?s22FvvYyU!Khp8qzM7*Y@7u&iatSc_=h*CMB?-Ni8? z%_?Ol-Ydu-VwWOoAF>u&@U2hm*|q<9Hj>4NO^H|CAuW$N1muIdqp7mSN=IxnNVF|8 zN3=V~+A8P!$umNl?RpzT$>G@~4z}r2y_#(8LT2N=D^(n8L%Sd!i`0$7`J!z5wL?Yh zHIoQLp+)?9zwb6bfMpx3#1m^3Ls%v2;Ma5&LnY9iDJ|?;`hFSpWo{;oK*sd|pkl5B z>G0+}l_t8gsQ5Cin74M-g=jMtYesz$ej#(~|O|ciW3vV4(k-E##cxJv|PQny~zkITk3S=>)WQ{Qbc; zEsHp-639l-jEa>&*Jx~_X1*o0wFgsiH$pt8bf`JlDp$vBK80=^dlJIHT}nY~T6*-) zxuP$c;EpuaXi-4cl`8c>4)8e%n%^v-+H*h8=1Bg3mZbixw)WrB*8h#3?t$td1ymD` z_k&x__!We(g5K4{4=RMg*y{}T#nr(!lq$`-x5yODv(tN+vp-JltiXTcn^vZ*Fv;U^ zSZV4!eqzDNlxbhrH~sSc*z-3~)kBE`JJs)~3zxki|2N=jMb+wh)hRG|O`X8>_Y_8o zBQWp5xggO!si}-|__YvQ{dUlq!R$$gjF&7&*wCAoF7a>ykV}ucf}AQlvTKqW=f(%H z3<30Y`l)FjWA2n!0(y$!x-aFlT5pJ@hbD)7Wv1a4Y^6)fx}>_MYxJu{+ea2dQ&2Du zQ-e3N#|I|9(a3LouZV-7%hzyFeJw#XI+`kA)Ftmb@b=B0UEsCOwl%tBoFk;2Lbei3 zJ{4m?p8bPgV}uKtW&|)s($vo!w~)F5U*&+|E2v4d>wt~bY#XNSF=!P&w-#$o@NyIi zeAywM(+T78kjX3nuCK!Jsk(;dM)5t-24mXZ9l+d{))E?xX>|3+oAFnKNwSpfyXaqd zkpm;_1Y7h`QD1ojH#`hJ^r|Jy!5Tg5N&ui>X6%IuJ|24IJjn|hC?3k=YjkylCkY;| zKPp)E5e70#HbWl z$6VP!{j9evb$;upLgU?uayrK;P#Y&?E?tf8jpLWL&URXroiH*K)~{cY)A-t;tgKdb ztsffY_@tL81Zda;8kVY9$$Oi5-$Z#ItCw-5l_q-2#J6X-~mm! zX-vw9k8qg=*KM)@(Cz<<+tU?Bg#d12?SS0yLsR&&Ek{RZ|q#*02jqPDT#?_Z%AM0JdAuut6@vRQ*)jPz=dNXc1l|25ZlzpKE>LWzh)WjVrGTO$Hw;FQ zJyJm_MHe?AdDMk#e7&flHG0t15zJ7Wo=D^v>~=w!qNPw42H;X)LKx^d<*@mRNL-c_ zPa*0b1FX#py^v08MwJH3{4VRNeeDZgx#&jsu(Z21=}sKh5dT6M*T(aYaSo;)-o0qi z2J{p%>|0#t@>5E0!~AN#Ci0o?JEqT47Z~iD*6R>LO_@mVdq1ag7|gyYFqIResoV&& zT3KzG1*`?(@qkIt#S3 zjBbx!d&cJ#DhS@o7g?rvATbgwrqsXJuZwacVrZgZ71Ygdga6;m%f8QRWkkWU9zBz_ zMSW7qsDFI5Opd6%nqkiG$}y35>GD_AyrKW`;UaZEfYrv}_Pd@`S<4e~VSaVAT<*iP zckvVGK+d0Ft}q9DNLjqAHq>QrYpmw?JyY>ByRuuE`iRfrl34}Z;8?@#K#d)fvP#Id z8Q4NNsCZ6d5Vhoh^QLjp`b$hPs5z5S7PvN^nlrpIKrEQ^fAOU03Ir5D& zD(mOu8k>UzX+fG?Vk{xBd-2of0Jf46B?MnYM4ug1cUqG3U;C+mHKOr*jO#(C0| z(HU!R4E2r-ycP3aMje~52Wr6|s`mZIWql@?p38c?YRdexBXru|J$K%Q_-eR*#{f;G z%-`St?gjA9e%9F0xN7P@%_W!Sj~lJFPCZoM3|exC_T4{AuYkKACT^J6`r`fpZ>SuF zZ={r#6MFIMTNO&F4KD*^f0cx7y8bmIcZ8qlpiV8a{C?m0GrGGCTR+X)eWxn~U%@%P zcpX+ebi+sUGG4wR5}3`#UV8?Yt4Ig5MQxsIE83Rlb2pR25T;sDbh^7I(^(=He3(MeK8Z)9^ zYJ2loptvX#yv}0zd&Eue*xlLLyS6<<=Xxe1q z0K1X-83Ykz_MPX$??*?dubI!(Ne@x@{E4!1ki~UI(LinJRp+1~kQ;;oT~^REhRUL4 z!|J(!YJkV(46#*TFFocV(G<@W@BUmK(o?gUwn_IBkio$YQc%lxlTuidT7@a5mIEhj z3PYbw@oz$d2^l*+$TpDAm}m0(Fc1$^_JMGWV-@yCVXYNQoT7?M719$9)Z~gjVncU? zTT23P=)u|}+#Rhny?`GtP`RW2&ke-hZv_#R#J+U>HDWV|VA=1%W?a~C#-M%-ziXC5 zl{R#4c~;~j$<#PCY}$q2C?Cdxj_PmZhuC;&jBCTPzdd@&a`&b8L1awpcKcwm`r|Ia zFeuie`5(2v1XB6zvE44Y%1UD^$d=s$gsF0Y_C1E#CY^aLmj}b`$|uB1pS8w2Z9L~- zcT~(9*+E`YI!WF3rAkQwlz1M27Ib!l88ezYz`H^4H7dajRzQyuvLCTM5$`)$1J!Dh$>Tt#a^-rEWOC07HC{$zs_1#X%N$(zXDWVA2_)x8U-w<>MPq+S9PL+%11Qx=*ZPtv9d6roEq!1H3q&^^U*E=7F-+dxu%zzq?h z3NmwcIPiDe4L?oQY1N>e5u)Zu>a#&Q6tat)?Omn}@!ivUQuRMibH#Z=-?tIHtE8gD zU%|2rI6M^TJ2<#Zwvc~=Q92q-{Dqc{1dCR70z!QW-|>?#%M}r77R|A^9v5E$OU?V$cnlQq(%o61#;^^+zPlXijG*| zz=x2z0_5B4xaCkz#A_FbaInJ*U&6RzN<5HlcN+yqgl6Z5aO7;ZT37h{SScxha>@;} zF_t7infO038O@Yy@_$1z9~S!JKcO6sO!nbFkxY+iN%H@7kdiFmB+_!9p+pZf$@t|N z`91T+I-to{=iA1*!gQS)%qmHFITL=f<)+14w(=nX-yNpEuoV%nz=A-+*VEGbF6Um0 z8_v7(i;B9^Gf+|PJt@M=9;H7bnitkQgd5-{&w$R^PO|||^qS@!ig5fyeHqV02jFYa)`>3P~Q=`^I~TO)&qL*ioNt% zs%pG@i--Q$;=)m0pDUu)zMKN&&rN=}Cu>z*-wh~WE22J2$4;^;SR+SZhUSJ5r>Q^` zu&-O&`iAVr#6*wGXWJLwN*QdgIgDMZ$Ee`jz2t2a=HmB9sZ4MW7R}_@mE9c-Ak&LoDLdlHKaGe%wxS=DaSQH`V7?~grNUBMJJ4iL z!6hz}Y~D-#zbQ3ypsHmrN^CW((9baHg>lznxX|3g~ri z_TfBM$*%FO{+J)cK%p_Yx`nNXWpXoej;%=g+q3#iKsJoUk0LEAee&(EC2(0ZK;;LY zydLsam)hKd<+`Y}|25JU#J&4a&znrXFHnUHhWT<2 z-)@tcx44=Mapc2_mV)iOvqIU1Q(R~%Uyo_@T0)4Jkj6>(Ej^jVwB$SBrg@=VXgg* z=&)QSUXHu0nl?c%HhE8#J1+FbZu%wz1s=Xhk9Z6agD4aAMPc9;$PErQVYLb3oH93o1Y%F$KGkoz_qTM)ZPi*QbQj30xymp9bM+x;3> z=9AnYOt~o+-we78TCz+Ql7bDvwXb|vx4t_=?%UsX#A-6^#YAFHeSCzer^96!Hzu#` zF`zfcTJLt{`I&s75OE3`U)Hp?;AW!4?rU#N!d!5!a25XM=lAYYHtZ2H}h8{ znmb%Dr+aa`gX*HNXN~nV`pzKqrbYTtPjs00ps25aJQrJgNFA&_F3WGr^eH`(!Vmyx zGH*No<=)gq6Z8YpJ`rgamjdEeci?&kN?}&wJU7_-#k2irK?^E3mHTQIefJbqZ3(9Y z?eCc% zLY%yFQg_%ahnV-q9LNi?-^M0T-yo-Rae>OVzv;&Y>GpFQPBCulTOGmTss&zn`*Ob@ zq4DSCZJgH=X|$G`stEu41`%lNzbo@ID_4*=ngaSwX|Pxoj`}W|FDi-&u`|&l7YT(t zhj4tt>^Z6E$@D>mV>DS930e?swh8?bWk(RTx%D2^iz zQyrDgWimuhxfStBXs$h&c2sT1&ZU-A7mR|-sm2JKHm%-pIl@7=SZNMgFSVb@ht%%& zz`l5jV%{L35}1IDPe~bj5KUDXjvHM;jX0Naz!HsHbicX>fRM+P&oTF z0qb?4h+wuqD8)^UH&0FA4Fc4rf9J1bG=cxN>}%jwAg^CjoydMtm@l@-Yh2%9a>f9* z(^F#|qfbT|3yvRiMd(3o6$&M3Ou255{d&8DeHn~$VZ%Mjjm#!|HzzpIu)+(2c#q=y z5u5LW^-&&vKJYeWngA-N?R)aVKPS<*MiA2_-b1&gfIs9fz`o;B7z$~;-D}a|&z*MG z(-}n|SO1XNvyXHD73U=YS1GRsUXghef^$0IJauILE^?yJ%T*bGvcI!aHZ$MM#1CLO z&iTfj)px}_*UTvFh1}fhjOzSe5k$h2hxW2g#8|CMEGiYh@-NI>4YqASK4Sg~uoO=D# z(u3S67GeUr7gB?e55io1$;m=~6As}dlBhU%JqDV0hGk4k_jA{^q_UAFl8ys9of6MT z4Sv0vC2QTDXDN+t5`(&#!M9Y#ajV+AfsNHX`gP@pdxXI@q9f@k9sM}&E3Hm{csVeR zO|TQwsTP0i*154^qC=X!LtREz;+%wgO-zg8tE(xGIo}!$f2ptn=F>i>EVE!fWv_1| zbQDmsWdpp=-$tHyRg*p=mCu;q7ZiSQPa?9mdn0!ygn1D&f6JpVUF-zr7bM!_jRAL0T?MiYWYGQ%2so*PyII zb=2V0#m`5B@T=;`$uxN>o+*gDsV=*16o?drj2MW#uA&)Yx@2nB93(AvsZMTMb= zziya*N_yFCH`IXQU&Fn`hFDVvSUpCAzUeCOxPNeQg!l#+J-N$$WX~u1OGO|F81iI;xK5YZpA{;1VoY@Zc^%g1ft0@Bl#)B)A{kf@^TsgF~B?pm{E4gawA>F(;P>Z+%n{p{V<^)a2i;$Gq2FXK3j}>T@Vn}A1N~2XBR}QZM@~8axSE|`=K0nT8t&pB__&@?zJx(? zqRzA_J1(_$MRB;E5eO+0^2K$-wCiND@fs-K76TG2^ELblmc|XH`+4DUUfCh<*pq*1 zkqg}M-(KyPj)OCMKf@;N+EF5A!(^EU{aYC5&w)qYJ*MQCw)cj@J`Y@<&*$86T(Oi! z;_wmS3sJ#0GYnK-+j#*ByxWi7gig$g8i|ru&?GDp8S*kR0>*3YF@HFH@a(UmCImML zl5uvS$)evm$;F~Ek-!>C@RQ4$6Ih_0hYG;3zXHDm0o?ariVd9o|KKf0@#dHULyXTa zRT%K1!#F0lE-lRnGNgNdg44&l<)cU)1TI2%pIsJ0RC@9vw8H0sMg>q8<}UCv)ob&^z#`8@=@a5K|vViYF}6! zoRKsJuNQ(K&l@rDSgSWuptpNO=Am1lXPLEpS7f4%+qdwX^M+>Z4shvHuuvB9z1=zN zI4+-I?+cQM^!d3eZ#8_R-+2e3fZMSy>b7MSlz3(We5{IuZ`cvj+=}5bzW4E^HJsx& zN(}jI@~@^*LX0PLF4seKD^P-On+7@jU4r+M4ldtF8FR|pB@rvPtDq`rOYo?j!l0H_ zSCVBFOvCBKAh>U&wmhhweJk`0IZz7gX{yNx1BJ2M=_QB(J?jMCT}kKlt2YNy576;| ztn^mMT7zqFsO86@2{j$T1)O%nWry&3U{?=`Qf_M0+k5#}#3(2vSd1hEJ~faotpK=6 z9e15xn)?S))UY#TR$_*n=Q1uGI<&T9ef!#mDY|$Zi+XZreoKoTj!_ikX3aI>~3J_1EvGmFTU>A4O=N%%VuK?tSJ)z#LtpI{b)EI-<;1uAUj< zq`J;!3J023ee9&94pNzx^%NMWABLQY2k!Asymwz?07)dni+Cp)dO>gz09nlg$SNF> zG_XKBigp8oq>)wxrP<>z?cbEmtu0Fd&Xq*_mG1)gORB#xM1OtK5tsl|IIkJ+6)-+p z&0*{iVhbw2Sg24VfXUc(7As&faVyW3w#%jUM*`!Jy_dy(&psaTC{sB|96kg8Q>1uv zfFVVRs;d-}!{#v`E%M+eScfGEE`PP;_T*g0jERc?vvfwd_xm<D#;P{D2OX8XuP#z)5?VZlR+?5hyQ}4 zF}37O7nIq^?7}? zfr5d6p-X&T!YnvtACp|}f0^wKpBu9 zW{12&Erz`6>u>y}ThC-$EUPs7W!i)a_;ham57{ZE<+naJe9tvD2b06>KDVaZC2ZT_ z_O7P|a+tAo;}`vJG?$`)6q8ezR}Ur~#h}1D&vO;U?LjoQJ^Hi{Wa#e*54P##NM*|m zVOz67MAUYKk}oFe1aZ7|b7Xhfq(i()rVw^^6l z9NQ$-KL2XLb@PZx-cfxK7yyaK_lf)ygtxr^?po$>6z{4!@Pxa?AL`}NcE|`QhBpKBj$%Ob_g*f^+jUY4B>Ct(qWUC?g)B* zV%r`XaMDfZKpcQ0n_?7u%S%zF-*s_LPQRie()NVCy>gB!>dQPHH@A0KoU>S`tB_E39t)N(jof_ zbiqwTS0rfcN%16P>|ycMB)BwPQ>H>L-`f+q#)xOm#P+GWbP${r8+g&|D(We)6yJV= zG+1Y%_xQE|?&zvS=Omj<+f;+u(<#A9pA+w#zk*okV2VZCCYsyn=}!O368R-7Tdrje zKAvEwGV%2;mFMb<*2(l(IK@t{YEq@n@-H9fVXH@}o^xx$v`8eiqtj7jQMZW0IU^KE zf+$Z@fe9zX%{(OD`#4>B_!GA8g$947lt|sixyENCEn=3o={1wS_%S&ei*kz_`N4=J zu|i7=Bj6#s??E|dt7kHdzY1lM zw4z_ahbo=ne?P@Pgf2NBsL-wXfr{QW)_31osz;vCE8#iV(_MY;CWvi7MP7M{!_4gY z>lR^w+rR{ty>5j%HK`OCMuzcA4Xjf9=)aqFm5Qjk%TX%Tv^WhuR2Yh~At7s2fIi)X zhz4U=dDtAj7eHJZxz(47>bwioi3O8mSPl^AcokU3pgO-rQsfHCB9eC#TEHNspQQW& z0bInv_CaMY>;@0S%j`lrBZG(b|CW zYO1#kh+`s+VxjQxzXH{slYerr&=VP+Dnb{oE&E7Lek?Nw1&OIfs%`Lr;|FVJUJ z7jb&MVe$$M`(xLD-{yltIk3_{s!JAnD89F-zdC4s5 zc4Fnx4`-9P75C;q5Ru5=5v$6SpR_A1lXUUD2{{VbGSr(LbQD~=#0AuR?;lE7UnHWi z54gFdEs%b?B3&~fZ@mWG+{zb?Cd@ahEU8PXSi=$+R?Uo>qVGtx7q^W5r`-$wFJ7Sf zfAgG>wVcG(wVUYcKlqtc_9q~vYs|xA z@@ilGy{dcYDyY+I>bAo7B8YdVy>@!#C*zNwnTCZ=o9`Z?4i65?2h&519gPCmWq4Tk zq<>lk7q>yeRyF41-c4X|1$O_Hs0n=^K>Jrc3-6OkRs#n1df_H*rmh`pjW)(7dJc&j zZIK+7*EGtYVAr4&c+M2g4BzkAvW3YC&yRDL0g+o+VC`hVjG@^nD$FI%3B3KG1f*}NFFfxGMYrMG1-H1~k zp|}bpe&Q@f(`e=?>I7ll3{Fc1Pxt@n+zOhfhR4#&H3>!u(z>7gIoX1c?pT%Ktz}1J zy6ypS8%@WlxBv9JIy?oz^b%U0g0F%;r2+Cm&SD(g(9QtSLpAM~nA5sue*{BPc@MvU z)JFti;@#iQe$I#DnI$%5kd}R0dsX0)gv*}JQ*CFjr&ArQgWPq{DUUyv>{#tz{;|}w zp>xKSwfN9A!_l>=xr7oA2aV!T-?t!k#*QG_-L;hzak)eDIQbp+n(=C~9)l8LsANxcQZcJoq;s6k^$LNV;|lpO*iee6`GMPwV7wA;abzz{jjcHb9uEe{EmXKo(}A4D;-0G zBHI@VP^+4VpJpfg5eUQs_B{}rql>o@W@knI0&4t`Ak+^CmSu0^x`uHgCQ*fqKPbh* zeCR#T&4uFLtMRfEim0`VdN2({5pt3_S82(J2X=9>DH-s->F56VuorK4j%-h#FX2}L#>jmIJ4AlNP{~wagc^% zmm@+(lZ+f2e;y(eT-*Rv*$&1h+XUWc8)7N_{#h%rYEq?%K9W|#w4p*5Ea#c9P@Rm| zNN7z7E&V$H>*z7>M6`9GmJSmhagKX2M#lgZ{SDk0hhLNawIn^%eM(lOO`>k%v<&_P z+SRN~*1}a|b~3FIA{V!;uUpew7lELvGmC;Px_Xe~jacv(&oaq8vmU))G_E=}qF`U6 zIU0Em;KWk{qU%fg+bOVT?Xw;=^tVoD z6HJB87(sb$Yj36}JPA})@$ub+b{NO9DhDGOl`2|{%^yl=mtO4@AKR%Y=Ay9HD<-&F zNrNW2Qe<#p=vudl_Hbf)d~fqOQ;n0mWY{8n)o8@A*xHT^xQjOM|8& z#A(Eh22(&w9e*J(r{X~BemZT4;6>L9Nna)sCPQAS6PT`-TKW4|_+Z@8WTrtE17T4i zhEd;PJ2#7CxWO_B6pBhzrw=ONpkPSX%aLqLOT!D76%X3M&}s%ZE+uBD^Uo65GAZ=aUkM;8_a;+pY)WgD>3ObxJ!%Wt-)DwNsN4Bq-)7kIoSV- z-g^Kq&+{*gVwki~d7DMEkzT+u`%?vpI!HGY=C+z&F$zhI0A0M!)zh8)G8XNLV5J5r z$j~o?-b@X{%G)pmQ*ypbw55IlZnkbgGuB!JpIWl5O{y{7c^9WR{NikCHQQl z-YXL-GqXWUGcw-AJ&qEFEsShlu+&xF@>lba1x6mIG>_K> z4E~&)PkY_6vRAQd-tQ@fS_(n?5Zc~ayK1b45fVkbn9QvI8f#;4YgF&PBmsZLp&wPj zntU&mU9dMxP&xpw*SB+FGPCdQ_oL2drq~%o<@3SOEG30l$WU6%YKndy+bK7(q8c(A%W%fL#&CbGO7q6f7V34v?d zjzHqt2Sg!uh5D7;3BI-M9Kha2Z^Kc+j;5zZ4ycCr_Wl5RVnsFj@xPE${C}Ga_obgW z3zc)*@5I?)zHg5{ridvegcY^6`-aCf-ZnGRKQu=RmJ!c|Y@Dr`xutLK&N!T5ePqt^ zeu;jwL};YafFShKg3GJ#jq42|^Jv!?TfcnL{ipuMSvK4ccJMlL%FZU0YvArCl#y0) zm;Mk9)l@<;iAzkE)$9Zv9-E^GiCXMAPuaaHkMRhsiyURlct1a}S(!O*#Eaz9&^AYT zq*&m=a{TcpqrM@d`8>$c{kO&{USoj;EL_&Jz$VmONot8fOCt_nwmD8RbFa;Hww}vc z9kb&@Vxu;x!8Zx<5D4;Dmt$VBVv_kDADnps*&FJrO2B8kFw(8bQ)gj$d zm?X!A+ziG*+V>T^Qhi1!?aPM9Su}2-v^P&?YT)E1vWbXGlvijJe7`CqyswV4dY~eV!Fb# z9U?xXwf&?nh;*2S*y67ln&;0!jIGuP{DtuepDlVQsl3!T$_Y5`Amet8=8{N%H+_(c zRGvbatCIHYA258ZDFbG?Bqg*i=Z72>!+a@IhVyC8QB6o$TuIp5q92S{rqk*ElCSBY zssTIdf6DR{lqZtJWIUAT_N>Zvg+8ZeCNGhd&U5Q=*r~#Rb~=|bZ>}1Cx|TdlhKbW0 zDiLiN8>if1E1zxEf0z>TGnHyR*5E*F7PH zOTs)PgW$P-)i6T~4RYR@?ABZwPfo5^nN7S5xLGI$+P$%QU1>?JCg4@As_c~za`Vg+Yn3Z0yINU{3TWx49vs@6V zECUIP+ZSFcW(fyDXnCm{y^#btsm}0aB@Io%bP_t%fK95k8EMi-ykEsS7%fV1fib1K zp>uTUN>VJ1?x7*Fk6goCDYw8@0~1pEr~*{&0X5g-f+1IT&FYiwH* z*jA|50H7q3z4@NI*%*b!7^(LzD|(PRPQEtHQf`2skx8eb=0miQ(EdWJX&;H|!e)Ht zi(nC%#sMW13cC)_fYybqGRGI>9BbN|lX!G*BIkpge2f8x5WRSRf<%}?OI0R3t#E}j zbupo(9G`{QKnpcoHg~)RdL1A+dK$l2(@@(S?cd}0lm03 zz)!94=0uN*k7lgg-a*n*Wfki}=8CA#`cEl=%=~PcYf1~;kDlD)3uGv9;vbN2SIrsm z%U(|;h{(#ifATRYQp}^8JxRut@~l`K&dOKzD7p0BwBdH-jXw%mhoS<-7fH8&YRUeJ zUL4iUQWor*nJo8kr!__~Mm353j5@4~jY71tF%#a}d&Y2LM-xSr5oR=8%k zwqX--h6bHic_>~-wd`;Teii^bPvhP1dE=Dad0Ke25MGN~2+REbzL@0*iuf%MX=-wWgxG7CJj}{CH12t$Y}@tCuBG1*7F_irM>}4e%vy zaSzkcR-}D;oz%Kqxux#RUGRi;C!8;eBw@SVyFp7OIhi@AhG*NL`Usc4TQ_I8>x=9) zd%CnfPxO*&q@W&JV;3fd`nT+hV=jw6gVy1?`RhVo;8h9iLW_g_Us?b?G||=!63>-e z>dT9-gxI!x&2y=>!@+j>=0{?C=D@Trb;nucB90**shfX~X7{(F*^j&|-*5<3>Aj)U zStvx7O^o6!I=CnqF{DX?90M)aFw87?-SIWkD4nWm&h83V`>eShdijn|#N`OW2_JD{ zo|!GlG@AZgeDXk9k6X+d+Q35dNAkWjK?8%Rw_5cg@Ac_6D7d1zl1YW?eT#_tJI&GXc3Vt$-QaF3om63v7WE9 zS{9rYfMM!dpxYxo))m9ZDy&jq&;+dIQG^EG*^SjToSf3UhgVbluaR=CVQP;*>~H4O z9&_9ZNK-fTRTetBCk1CU^4mXwA9^KYNl}ejw%NJ_IGojde)Opc;I|sf`$4_WhdsCb4o-OHQSBifNBwhz zDYf1UP9@kn4q8r(N(9_@I=k!K(yXKEdaq!wo59Om++pzj^?;}o06o&^ABp5c5cP9M9clmDh{K`nE?%nCyIKGig2N z{KiiD*pdpmaCjL4{aQ!!o!_UhI*7^U>FS9ff1S?Pd@7y#l`)HZSzzbfJGf(#sZI2} z^&JVGhrY$dMRmq*`__|lHf%(d$IC6(FXp5vJ2c3$B)DL`b=BX_Q`zQT6E~<7Dt(1Z zjU8|tas@AY82?>mn&3;4SiLJ8bFcpfk|AXTHUdre!^Z(7u=A~}` z3yix+FdRhDKE*wE4un?;_8|(O1oaW;GjBbacL-##M1=%rqw(h_J3F{Ao`{ghzNB1c zpO|r~*Z4YQ0pzw0Lq~lq0Nw!zvdxBp@SS8zk)n&IKX{%Z*STGMRN&vh6}o(Qf5m{h zGU3zP4gW3ohrioZ!zCiUx`32KwSLV6%H6;(@wiWLdR0f>=jU$qunZ`h>7w6p zyf~T_&8?1gf)|5Ph=fi#=CPGa4ssQLG=w9y~1-&2yQ@>@5B`KA<(*^?Ss~b>460 z<^5v`&9W0%^=|qqJ~C4}ypL9mj-~T5dmz==+4p)yY1J^%Hb<8Lf< zpih)SMw3);o)|3qs{icNGt*!ReMY69wyb8X7A8w^qhH1pX|cUCS4!?VakLLz^PvMz zQ%0%@J&M2E$Pq4dd9j-TGY*{f?Ot2BxOB~8mIDd=w0XZ3cBViuXHBq+;jQVxk8dce z+sH)4Ucq*;V|_Rivw65fBg>>G`!j9wGb>owznfOU2yVhCGOw1f$X4@vv67NU_Y{8Y zI7GJ=^VsgOuKMF**yv5K>6p}M!$qK||IWurBgL1t=^1PE4Q@H&JJ1AmjMx0QCj7W~ zh&h4&3G+h=wsgJ@(?OcB@O7bS{i6qhd3G)!I-k+RjOh8L=k6~0Z;s(L9NYEZ9Q+RR?Cc!cg*%~plcC*c#T-q#q-1xb(+ryLC6EG4i!p8QY0S2ggj zmn(hS3xYBvBY~3;dH~`VBku;DzGlguQUEC9Ka_hL&}>xm{+?(F?R*`>WGk(~5o&js zAxidXaMQ|h(CE+Iykb$1mt_DGS&G%L4s_(lvw&||+p6d3F?~xpFdKJrFWNpk3WG~% z$GFYqP@0$Q>_Mr_;P2DvhgMG!hqqMef2cKRcA6@2rSa)((au^!W%kd<%xh`ku>`0g zJ5fS|S$0E#F*jipntiEB3%5)uQYaKEG8Snm6bcb8I3M)m{rhNeL}P@FF}N|b_b;0U zs_>^EO{o0d8*~t>RGQ61Yco8sAYBetBL?&zl57BhuRu#@`P|M^5-4m>?I0A#iZ@FE zu_BD4oMM&*6?2?eWBHD}Wv7G!6dA-SiwAU0Le8f#QzdLL*B9A1YkX?x6yY@tu!iH| zflU#sp9wQ?Ha5eX5dct#{$u^0*Vah`P)jLQq!$ZPen#kL;Kk`cD1D}p0P+w?7xZ(X z;dEI4$Fh`zG*M{j#`})}`doJy?pI4O2yfJXEMPnn6rlb?JO6ui^{Ad3in0=*X)1>( zYzu%smAZKv0?G=oW;|PU)L?^HM&^Nz4^hZDrjikgbrj%?-v3z1)KE0_V47F|H0OV3 z<1)~P;dbY_c8>ly3KD7PQ2bk~S12pTWDHpOD(77QVop0n?S3wm(YX;s`#Yx1>tdLO zObPA%8U-SI?PL}rXe2=!{PDIm-)K1iVZbg$;XxiD4p%R^>brdd33ssqjQE0dOmFr* z3$&>BS0)wk1inh+&i8qqPy3X;B6T@6DK@It?x-9fvSC|UMgl(6Fcw@Oy{n8r-4pi8y($GE`T#90(dXpyx~sc|V}8^!muZ`ujmYt>xV|yN%Y;AS%pge&sGFbYlBik=ci}jR zM$2NA9)j$&?r~J$7RBXRn`KIZ7Nmh)yzny5cq&mao7@pP`0Ak)bD7H%t)kT>>+1pQ}$I3FJ;(z6yzY>jK!Yz3oYiEan zE8&q2%#~Ev6x5lwP26=LGd)BnXOml(pG+5!+Tc2E%nS*7C{|V0w;YAqy}a0G&4Zm~ z)0D;69em(WDwASoagIpYQvOL_Xf2g?`HQ@+g8i5>1Dp5k?+j~2ekEA3F+{LMrN*WU zvIC`9a?6k*C4>9M47O?dcDN@Vp5SS@9ELH_GBtl!iYn^$_D6X~usWIiSgi9|p&>|0 z*!!tEd|BL~r|A_Xv;n5sdIAgzKqyjuhf-teU)O7K7ob+Guo;o>tI_mG@T2y{QbJP0 zmB%&C&pR@@5&AH0noX6D!E7WU7IHV1nr2{h1!srjGc8f3(iKH5&lH=}w)s{@)h33Y zwl^VOaws0RG@>Di{l%#46Kx>Qfx=5!ZPvd5bAf&34GC6kItpSi28OHa@xV84;l@Hf zqKpIErPivE-K*EumXCWHH}j6|AeGxCi^6q2&vp9iT*b$OcwNs zk(tL50NNa!9ALI5C)!I}Y?irz4tDjH|7_w=Sdj|7fN!!#nijJW$AgqtH^+Q-Z+@j9 z-)}7s9a=+N`MtGhw-8#og2eMfP7>5d>!~Qn{5-RYm}zT>ELD5#;QpA#njXJ`dzrOx zIAX%Lq=|&_FZOXk`llSf{tGMs%y5QthJ&(zaUKFd7*d_A5i}K^XsS-sAeqLt^I5gM=usv>E(WWfo%K?dtjeDr0L-y=6jRqGlu3UB$;!M ztkgW3-jSo>i#)S#v=&V{?J32V`~LypP26V={;xGbB_yn#&CKY>N~IUK-WXdy0%8lR zcKmWWZPF?(i3GmixQ*ZDr{c4ajUG#&0{d35!q&+f_S<>a(w0pvn^fv7^~RT+6J;On z=Hx5<`V>43a;RY9v-MkUVp@~5y|V=Jve(QQ65CA~)>zdS#e$CpI8WBctH2`*g^M3i zbMxq|HCkQa=o9Ic+b|aCIn`pDar695K?w~!+ap%wWn=vDL;Z=E`jZ*`BE7Z9)R3Mx ziJ-w)PM~+5@Ez#4$3ysLpxu#+FdfRn$Dr2raGbk?IZw$8y185k*^f2Ru@Tl z2VM%a12gb>cHS8I6Cqt(aa(cePboysmlHH@=RJ(UdQ@n5`XkIl@#`JuDXcojfCAWs$Jj76JUxUOsS(_ecSb(2~QCu zO_BRNNSKF(MzL7WuBUB)5VG+%TL^6`>E+8y>s#tl8cHalgL7w4JiZx1gKb7VADH@Z zaTl_VSPoCNVPis$kC!5|QP;8qTH!2+C0%5hmPuVrG8e!0IIEAjSK6rCPS$^ceT-ITpn(^}KXKYMyPYLlODqW`iCQMjWq>><)QT7M{& zMDe&^+kKxlFmE0>Mxly~QoTbrcA4U>vo}bb6+QHOD^V6?94sY9%2eq`I2+T2BV1FP z1fDaF1x3HyU8fkdI{)|}e zj!3I5>Fs8=ocE=-lRQ0Cjp5H-)pcvWWJ^AIULU4&44HOUL{;Fw5*c2@+vt#&R3hX2yW#Tqj;EQ_F_y)UUM|^!aawvEg^w^g^y125egs9uF`BduX z;$4A)omumZw^4jYIt9sr&jmAO-Yw?powF@`d8W+5EKeg5T;g3)v~Y zqyw;NGQv`Qil%0)?h7Z{l-tWo%ZUdu1;YVeG@M|X9#7yW-&0Jm6zGK$gwLX0`|;U7 z{};->NZ(S=m@Z=6J)p@RJ>s~4cj%YTj8^#n%GMHDrsFP5lR|grAh(q(t}IyFMd2(D#(0;lF%z;MEavOHaFf_ z7xT+TB6z~syH}_y=j=g1xhcCTNz*^H&%HoKV4VKqm*<6MDUxbYvC8-6^)*mX*_&)K z!-IX)uI)B$gr(uqt`e?c5fe-tnd(b@H;ni20}R~`KPwlfb$D&d znTTtVeF^tOTrP@vXoTj8-irWiaWM`T{}O;~PQCfzd9e4V7H6q21Q%sR z+^aVNr#44I(}{UUgLge;Pl6VjxbE!;MR?lPAhlgoS>)~a`Q#9P#lCl6=ppQL(9%@= zbiklovWU!W$-9U#7^BK_UUMaiIoIi*=dx&RV&;bR=SR7FdmYu;?-HiMZK{nlU{rr} z8bq{hk3QN6I}0u^??_gt6d_ZGkfc=$SizrT9@(mq`!(EfO}IIGvrXr!ukE%@hf`Ng zK?q&_WLoRE1RZ5OQ+OC1QL14QnlEM5Op&8gt{m2E$>+F9aU_@#yY7E+o5;=P%I%Q% z|Hg9(8^l|<^)6F@3Q`s+v>%YG>B=ZjC*8zDuE(|X(Ro^?1m-*Whd8#(vRU@AX5iyB zLi|IR-G7dll-LeMWj)zDT&Y~Xe2al5KdZ*-lHUckYPQ$>k~H-B;wG`Y!?Uj%zQjI) z*_DhN|Kx&QAr}-sU+DS)?Cre;K}JnI!kVD~^eWB~iCF}ieZn|9%BaA2q953gm%_|k zBgHW{*14?bzpqs>?c%tsa4-t@3|GwCL*LPmM$DaZb~w0s!*xU41>L|TaR@))zfwzghJ^RQVM!YL|{_VN(DPWtRbfzm8-(AWk# zy6Yy_xe>VY(}GT#Xp?p(YF=(=;{umGw@9{jG&+f_(tJwvS6VzlhePU6SXXY$e=imj z6UdFnnnJ+Xb}4K)F{_wP+gfiBj@GYTbu_NBN<*%_MYTLU?p_ra+o^OwV9I+U!7nKp zkXZ4lhdsTjcnRiGHY^Y>ppF{ir^g%c+}1CZSpj+esuE*}yqM9r==bLpS>H5eXk^#S zR5YMf$K9PM%#9@D`z|Obg%~mneB5H%?K>}8Jt$y*UDXYy$wND%T;ce*QarooxL>b- zq>4TzhC3`?$dB7G9kE1))V4s6+33#In{ct$W+WqV6b>RP;-Ad-TIT=41SNXX7wFE> z?AtR=e^va0Yqn|{!2VRvSBKET}K-3=45#& zMWC)J?JhGij+TFSgq<#eks!ps&fRTzl&*Y3hp&NS^llZ9wh z+53w%wKk+eW^EZCoG5z%Ua5iGUxpGzi>L7@XDKZlnlK(T9e<&+k>0$N43)|Y!@<`` z{q+1|XaPTCfXzxosa6qBC1WlI-ktN4Vdw$GO9|fL9*08n<@xwipgK``45sn)TgySl zo*Hpr`|R;lP_oh9pmz8oAN!5&Y3HmDz2Yfc?N=-jxB`AyawTaK5@zssfr8mR910^? zzJ1<6lqZ>>V$~pQJZ*aiuKI1k?93)EC!hrEzfe@+Ml9l=?W_FSr90B#X-TGu*X!w= zo!1qk4>6~JBlB>zI!hrkq9Cxd6sLzKD-{3eeoK_q291hj6LVlSB1WY!uqgN(PT?>F z_Pgv-+QlPq5IjB!76#ct**%vU)e*=7Z)N-NJNo=fZ8V}deLm%spyBqmfOS6fY8Xa! z65!nE9#{WuqLn{a$m#v?qJ{zt<2cc|I`*bpl$}{|}~pB0@CBR~QX&a*4=KaQ6IbCRltXiJgx;?US{t-0`xlDIqy zK#Ai4*Hb7#!RPMm@^VBlREj7JL2e=*Ue%0y_^Sb9G)nD(npjbDf}AqPaKNBhJWR;F z_2hM&Bq$1)hD37OxdQ*?E#MpdmjnVzlaANgiAK(O<@>Pg@sGox>;501W8|cGuBrh( z;y)9dLGM{J;k@6y7#Vr;$D#l4cwl)w)xK=k*r!c3aRV|8z%2tse)QrO zHNJi7vo+#i1@6x$2lsU3ug-Wa$>uFZCE9ypvcYwr&&%H2oQ3wG8<2d4b)U{}X}D{b z?Mf*Hd4Mav_69P&xW|jSc)7-OyfEFQ6LUc!3hyS@uED10@<)VN5;OvER}nN40_={k z_53FKL>4?f@*`u3uRCtMUO{}!cn_6vw3-GsmhL78^l8#LN@u8MU26mft!@sA`IZ(n zjBp>MwNJ8yzrT#i@rU6++@~Seyd-JwJMT9~_S5H{g8bfA*+06}FZ(_2i#%Oy(J(*! z-E?n%`-Ixg#$Y)mYW_AC1bKTe?JmezxZ}sK(rpm4p$UG|??hJS#I7Z4{o+jqz7`tA z&e(&hQMKkd^Vdz|2~ihfM&ueIE@nKf=ZdW;b?*F}2>^QFyHP)(q^ELkkn4A=<56R5P&jtHaLsZt@y!dxF8^N+&k5UC%F6@1cCodE?4K%RYZ#K-BS7S>HoJdbneqA%$`&ia-Jr)qkE99vX*$_6$$VhMzJ>!Qu zhmA^+5)uFY8!73BDu8C)r7B2`^hT7mPCK}p;F|D8?Mqs4@tJW*mRa-AYd3)Qa{bjl zUhob}&ITmbntI+w1UU@c>x^chVHg@Zd$4!QKy*22)-<#Xr2qGYtx_&N z_AQL8Rps5!+mK>}#aysx`z)ksD&K!gUg|MWv!+h3wB%2Rr@M)akVD4ZXg+V;>e?E1 zWOLAq&2K7>>M~j|)e5Per2_d3V()w8h&ycGqtyt~-Ouz;E?(YGM$8&XAp4lU_A?kQ z1GK@xD#JYZjBcMie1PHf?9xJwfgN9THt)4`QpHfVZ|aDG=4<>8F)jZFL=0nH3AC{? zL)hjRXw?pqr1s$jWg;VC{vwgwxNOYnr2B!I7N`e#^=s{yVTk0^FJ zBAQG$ZPtTO-#g+LMBnFEERo2$f<{bci49iNAH}~`!V{vbjLqIHq||`KzhcbpcnsZc zc2C^Cz|R@Jg2TV@eQmftZr}V}Ht*&wGtVR9-Tld9%2USxTl->LLrwwm6>phY_i&81 zzDr+G2`*4pc|49hL7CIwI>qp73zxz#AdDi^*te@4j~daOg;J-`L%6=O<+qZBd0v`~ zouRY6>e9u*?mW2KgB8I{MfqwE6p^ zoy4;R{+<3vMi#r=u&&vDV`M>=PV~MDhoT2_9$~To0ad%$uaxq>kxJQ@@>Ng-%Zx|& z_-s`*2olyK`h)yH*2S0+{Al(-HoMrhs%pY1HEP|jU{b=}Q#Kw~mQfqA!Iq2KQi*GE z2Lst?mE=>-uG7qPh&ps0qSe#8Xgk8LscT#DISRN-)v*&t>K6}1D-vb+;+EqppQPBP z1tSlsrAixXJ#2X3mf%mOt(+st6+kMb@FhdOp2G2xzy87@FTC3?PFYwWmqm23((GC; znf~yglJS%te3uJan^ej1sTP3qTIG-vq}#giJL3xu(oaxDc!1<#YVmE_4(^D?Y9qjy zOf8Tj**)*D3&OVQ8K>31ipbfLnR2FxvjpJyBMwrP>e@Dxti3Yf^HGp8EU*g26do%5 z{Usw5bc7S!9u+{zo``HZtK+3(nl9UqB%q5V^}ehseG@(loVOGFxp+IFy_&9{!0I=f z!?*UlpA-C~w;%Sek1jv#%jiD5!Rc6D$7e+TY#l2K-uR6&&~FUmmeZ=PtD>HtVktha zKz|k9a9KcWttAqLn;8l^8^Mx_C5ONMCA;7!%`g)4B3Tu(0nw|SF5Z3*4l;B`IT0xt zz3aYfkBbo70lxhcNd0(?k?fmC=OZ%dt9hR9qKg&8BC;YOIxZ$WU$7zX*Q=}k2n2Vq zJK98l+atW`gYMj@xM*1!fHPpECd|@r-+GF-+sm$4Wz9`x-RDWn1v%n9M#h{Xx)o_b zA7^rnyK=ADZ~I@q$+`z9o(z07+w@1eo2V~OU*H`YzTlWB*yVz*hwiTWS`Y(@hf>dg zpa=6$ z{9!wkc;9%?RV}&*WUI0mF_+m_)-4_w*tJDuR8wDWvpwCvy}#Oa^kdNef%~NO7_+r= zJa#?hc}8$8GP|6e|HlMcv#A0<&Z`x7doArnI(v9NndP+$hm(He;^v_BkR_0}rT)@~ znYt`l&fDhY1MhvW!_)y>qLbyYviN%-puO%IMuf6*ebz|NP!@p~A+(20uLxvb!-Mf% zjJ0m_MVRr_0(9V(W#bbZu3@rZ%&3I5SSzy{7)4qAN0BJM>#@e zXgaQ^j z6WPnhs#p@QrZV1#IkAztkkZ5e;@nk*NiY;I4}EWgjj}>a{}2x9Rb6%$wSH0Y(xpb6 zrG8Cf;Upsu(0AjEG~^5m77r!l3LL92O|hHQ3bbm>zNXdG48NoN&I6>*E40!eOmDKw zao&axW>TS13@(?-$GzGsl-)j)CJ*ER2yfDmjl%Nn=78_Tmuys?*lkZZjjxrerfY5V zd9pDrY~}oW?>AMLM0c(|=tz=>4_DrPs1D%wcb!S&;fXl^4n)49Ju3WejqeZafneA- zkguV>#J;!|EQj0F=Uag$E*b?vZ(35sAk}8{m2)E|y(Dh=vHz@KhX-%r&K{pS`DNJy z2H|cZ?81ln3%Xlx!&oU>;>-=LW{@8_#K(&OebXszrekSby=kY=JK6edn+xw)uJVGb zupix}Cd6#rn7~F&hS4n83={a~54`5lKvD#ZX`5K3BJ(Z>E2dO4Na&1<5l_p|CNJ`< zgLM=expO1NFXRyD;=qW>_ouGA*$EmwH#(i+b+=_z>#A4*+C6+G&&;EW^WyG{otQ|s z^FK1}E^MOm(2zJBioa~jy!1R6LM|#cYQ`-Fw#VvfDYAE$+#Fi97)rmcWtM|Vi07d9 z9mS{e-WKmQ2DmHNT6+-B*-XDz!@K)m;gdQS%go*c-6qv&>r{6>Qe#U$m;EeGLyLB? zY@gI8;?V`U)KIX_QBYlVMW>RC4Qt^lx=`Bj*KkbsYJiZvrYhM_=@@&2@)ZJ``5I7i zn;YP3K8NyXpV>Sa(F3}JQUva-gS93R%SVGY1-FP;e9^tPv>2!|YsF0ZAdg6+oaQrE z9e~Hi;pA9Lg0`j4yM*%1n4y(qbUZZBQXP#)Zu~1$gjh&0q(p#F(S;{V;y)F{24}ol z&w%&-c?8ey*%`^>b+{)_IK?YJq5Ju^9pZgZEkm!EIdaS>*woJ9RJGmcf}j;t{~5mLmJVkFc9j} za{v}dWRwI2Kg-L{fzt#)srXO4^*?q0&jBE)KneXP{pC3lDraW+uP-5s@;?qddk1Dh zJUcvo?xPZ*iyUXM-@No{NbLF51^0Y%+rv|Yx@JNOKyV=8N9V>XEf8IE980)0I&)1H z&Gu{PYGX$wKS?541tH!Tu<#)4b>%|xVzOHfGan7ZPy!ri6*W%Dhzt?59Wi}Y-*!|Q zZQ!hwAc%=!1*ysUq757GO}_0g6&cGtTkUJNeVo+6f?pL3Lr}Ff{Do~J4T@8;mhlCW zzB#GYAd4$bZPoCEs1z{g?`Q|(zft6G9kBm*z7}U{syq9H$?%>hxRvE883g>vNhwQK Iikk%dA8v&RsvqS`V?Rk}Kaz+1wK)UX-$}_H)F?={p?Ou|vL+xfp7Yc^-y-%JuGi#WS2qvC zp2-xQY_Pr@!nk5TLK+6XTwSh{QW>AiH$d1qG?Ga(TTZDKgCidIW?TFk{cQy+-nPjA z=eczMgrH0Fl&Ve$>$+dKX9@O5N zHALh2PV7gyc4A=aVQ{e1*H{+%NWUS<>U$%Ss)@z96du;JYUmT;xos9091wxGwe+=g zGmP8aiRm6t|%VYM8lqkXJqI=Y3OMUMV5b9#8r*Mjs zgfd>26{pU_n)zABcb_M>R9wn;hpG(Mj#rPDkNGCHPOOIQzzjk|dzsO~;dS;7O&WUo z%3OQW}hz z{tt&p;8bfOzs^zyP>DGocL~AQr`q|ov$6@%(A3GUdf!%qg|Rf=va%e-4|Y90MxJ`= zi9}*?;ZA;ub@`;I!iRU=fWdeH8eD)pfEHnE((I+*7Iu}LkdE44`xE89&mE{eB-lQ!sq2&ebwN60qxUl zFz%LB5Al}(G&eooy++rk;HT5cYX zld7<=ncm*8&CPGKt=@?Hi3Z+b%&ObP(@lv5gA?_hT3zJWcZTVO1Wu1`L8>lWV$Uu6 zm8YhpC@MzyT!?K{*KpG^m?Ll2Dl8PF+n+euP16+T*XcH)ShC&Xgv#tYaCK zPY2i^Z~V|Y-ON;&Q2+S;-4$lm(KoQR))^+X-H9TJahuLW_cfQ(A1&dOn=;}H^MVsH z>aJ;;MNFyg*Sg=*LUM5~pncyoPiggA2uclvRP2+Di^4b-{TX+Cg}3AM@w8Zu;yTa z2{R|B#J*JhTu-x+{>vB1Bi96ux1Ehx85tD{C$zFUZn9G+>=&k9K`ED+e3CLf3y3w5KXoZjWOdA*brmzsUCwUgM|_<-w5 z?P*eXG{OB7?;(XkTrjI2Au9NcU^Uc|62_V}Rin1IN^)Clxup2=?GLuats;l7s+!N_ z@0j3Jf0%995u$0D4~whHluL@}DHmBYBKCE(EZfjFUD7k`todwYsvyO{d+Kpt!;1KBa~yY%J#wsYJMqbfN==XIFQkv{3}f`gzgULr^^zyucPMp<#wFIl zG1A#aB-!r2!Dh!lGkFa67CVl_*yin5 z!$XNK<~HW5Y4rp3ZpdTLsG%A+JDo(aNcX3k)>ZnvmWx%U_a{!>iz{v(4C-cO=2sZ zZV3uL&w|fr$v_dWscbfMy!``LuBeJKJV%^7?6)5AhKsXi>Q%q}Xi$!Saxj2FMQP1M zLvsxlroB2Dz%{IYwRy?9_PJ7IWmH8p8=JY3OL|Y#Fxu=od@2D-(9z= z{tzXIQI-ZKNd+!9^T_JrGmo7t!-7J=4Ejh9ea3X%hHEj5F`J$C$mi8)XjDGs@tjP#En{QK6=pvMAkI%|U^6p&aUo_um;xF{U1IqCF}!bMH>OJU8<{fls}wKA zs`8H`9NLeM=6PpkVC1d)d`Ii0W-Eu}r&i$=GZ3N76PP>{A!|y|q1-yQqG{?pUe(#^ zfLzJy@k9h-UaNgCc#GgTy62DpzJ4#*g{8UW0BX16c95yvK=4Cn$pE)irZ#%a<8-2I zZAa3{0XCyIgOkU`ecXRn&BqMc7F6&k!BHniFTvS3KCUvifmCXY6=`#JNB*s$G3~-* zE_`z{=55w$wYR!%^Yksx>{UH%fmu4zy5NP#51A`j*|5fuZ>mHDk3&1 z_xnQ1T_92PZaLHIOO_VDz8xu>) z-~4%J+NKd9>-l|6rXwZKCQ2Oq@MXYcu+Cfx=z~0&iL=A5UpdTgGKS8HuJO}Df@zmn zABtF;A3R9TkEpD28*#nhN#ww?XQU#y`DJ}T=WHgP-`m)m18Pbxq}-Bqit1+=%;O$< z_JcdDAD()bM1-{wQ2M-Bir@`OblW|Y4DubXg2?jvv2Btcedj%>T~{qM$Z)g>!c%kdL`xUXm3fCUUg%GW0$mLXQ>Fb}uNW%NIoEMvLnDKn- zBMa9L1yM0q6F+#FxgdM{)Hyrkx{|7DW|-fAmC67>i7(&UI(4_Ocbc}f9VPkuk0|9H znG~X{DG_vy3EO5N9@3!JV`d^{%1F1L%l8A4!y7_IYR*R#n?mJk z+Q*6hFOQdXsYkS91F;P7?JztP`Y@WFFSsN*j*iu!++hu{bPhMXVZ(oiDO z?_|P1Au#MMr2ab^@{taRp5HfxX%Wr2a7~YGoPB}j5IyDp4wrG7sc|O1guC3$*F;0J zbfN;0v8$Ju)CIP^YT8u`lIdOwVh3g=K2>YMXf zLR{B|EY1i=D7moE=Je#a(tgGGC5xW2o~0hVBaZvCB>&{!X#J7t9rM@3u`A!9iclKK zxSIrh-QAE058Uf|kIs*0QZM_o%J=K|@0PltPG}yCL#HW#@YpT&q7e85K6l?`bhf)} z3~UeCph~&w&m9she_jx_Wm&#mnU9{SlsNGKF z`6w9Ns%g1o#{3(#x81sy(X`7as;>M#J4pXTf1@vqVU3l*M zviK5sEUeKDYL|SkpqXHxp?%W5=nJpA_-Pna-Rn^=Avzz?KH7t#iSOQfI1vL{_8ss( z=qfD){n~##yngD!ufsn&{yzMy?0VUJ4V(q_k_J%^^`eEG|BElfS08G_V)yg3;*g)m zp}{Vn9q+QBL(o4w%8fw{T4&CN}r+Z@pDJk(iJ$-re2Z)fF5cA0HOhJ3gL^MMO%ZWK(y4u12M-tSsiWYdegtA#_Yk5%+w3eYXQFE0iRQRa(qkR!@|S#42#ETu!Fg z!ry*jYiHMMt^G<&L(^Y>{Z(9C3=yP) zMm$ovBdwz1baLd>3@1wmm8xYTuJbQg99pdHZ}yO*S3V+qZ3X~@6~WZ znBIsDawF2;SjW=x+j6-%S4Fvj2{_B$DD-<)^z;b0Y@O?t+I0dMP0jJep0phypE)&E z)kgWWl+TmLq(ns04#g=cDe(`jrtT5QltuU{FQhyeNvI5}rMqY5!bDSJK2`yykT%PL zTka5Q-d{<5B?84!**fUMGcoxwRA`8T zTxWxXfFqQk;o;%c)z#!2U4zQf(l<420Q^{CG{ZT(TUU`Db_oMZy>gfc* zW`b|(wAIvNL(~o`e4Qg-z90(Lo1Fe`%VYhiCHy{h&$n-Pwd6hh2$>Z-=|nu|Y@j$8 zO$?n8obsQ`{w1@?Qu1}vH(c*_(5nyJ{ ztE}X$n)AFyxTK^Tw5VY-U{Qr@2J)4xlfC_J1fyCbNs`(s-nF%~UO}5V@x)G=Sy2)r zyRe9e-M#4`@1QqMJP{=&=4138svosqRVdpYH&)JpS)N=nHKntqVN>#En&Sm(h)(P3 z9vdlb>&eN5u8cY7Z5RpcaXhuFfMhlYW;rYB|AW-W0zCGDD1Y!@Or$ za4?QrspiFR1LJfNRmdc*3fCC|RBCv+cT*aw%;)=+taidVm?XlbTH8!&n=ihh-9{2MNyr7kK3p|*Ojg6#aplwE3nQ}EAU+}}q!s?BZ z*KWigPF)5{cT?Rn3XeS0n24XRfD3ByK8u~+&}Tjf?A7;T@dLr9g4i=hDNfB-P^s*$ zfF7<4^uZe#gxb>hSc;myr*i)0S+MbipPlpFSM{ZbHAr2p@kpF*-*h^s)oUL4)(Zpk zXK{M!={+3S+b>V2T0aB> zsn}!s<+N{6Y4#@hd348(EA7-+9sjtM!|tYx3Vj6%$bA2P=w@$kudgrWmK0b>Ae6s) zbQertE-vIN46>-@q#5gHY!A(ECyr^2)#5s_+r>x5W~=CIbm$zLWWX122=(pK&`xp<88u{EVj*QxUyYP(89baZoltO@N`7u~f zpZZzQVPP>bnVSG5$`gu<&$UG-vDIl?j#b!cy5?Mtcdymzg^7uY2?+_=+n2lu+TFFc z`}F<0wup#N*xM%*ZWl6hawJ4u1++k5?@AKVz^tsOFz8HRoCQIb*M3>r^U|eDo}Qks z9%*mQwI5tIyUk|BO<5T7*R@;lhvn0OygafUR@1}9CK`}80B(Tbv-svM=fMdc zlIQdMmqYlFF_{~?kCRfYm`#aW#I(LCSpH^ffrYL`D<2Q520PxH_Zp2;lEji)9&8)! zBUWQ)Dl04P@Jc?v7$K(NWfl5tv;ZD7bq}N6k{aIFjW$n^Q*%bf%sy2vI$w`T*|OHu zq2YB{?e&!0LeO+nTd!Sf$oR1C|7^na)`>>i?01egYNh7w&CRS5t#MaIf>|*h6SORD z@9t{WTG?#+6SJ!;`ko+$d{qR*D zJu30AOxvE8J7fR>V91tu;?9x^xb7VUq&;rX&wOGe1NcB;wqHYw%Z<}a`q1(bhVoM0 zR3WG>7Eja`!BV|VLK1dOt3XerfyWFrs^w6@f%rU}=CZzXw2{(j?W(Jr)L=NkuTEL; zb7Y6oSE__K9w>yTAK=kQ3eOSN?s>kIHZU~x?kZu|ycO2-jZw2o**H|Nx!i@7r8H%1 z&~dFA>70LIE%k>Cj)++oxzeiJtB0I3MF+Q-)M@RU_sttlzG`J< zWih7eOTJzC`j#2+t%~r1kT#IH!)JMO@Q-1I1ck1AyKmuhcLaY7?~40iUv)vHj!%R5 z!OxC-%hZlt5^^8~qv9ZGQrCE#9PL|wEaMqD*wMWmGC&w##C!en#RH-Kwl?u_W=6)G z^z@rZ1I^?D#-g2m*`PA<9o`g|r;~#T!74tPL*2QgD)RC(2u%FN#l@u`n|ZrPNjsi) zKP1lWP)N5h4RuUcp}~kDw>Xj;0r@>ZavxXCP3a4*rk`E?NJW_`RW10GT$|UwBz^;z zjpv4jme$11?aoY3@9C1p@2`oTfgDPBd)>T(j6mkuGcS^rETz1PiHA!ZR+B=`T6b67 z3b%z`Tc%!UN&{WUYhZ5CydXifA#8uU7@I| zYRm!mL<(xHYI8m~Je(!kqBa^2XsXgf$g7+nS|57!rr!kU1w?Xk78&wX$pW$fKT5WmllP<+@nlJ>dw>0%- zDP>jdk+ovG95jcr(m{?}O&-eD+S1*>-DSF*p@ulDZG(2b_62z_E@mG4Lyu4FN>y1J@JPA#XRlHp;jQDM8-7;w8UwC?lg z&#@28WE~&@3OFM1nIWK{c>VgK0?~E!J~OJh_NfxjbCfxy->$Yxt)-;oaNHPwuzDl9 zEDo#*Ss!m~ax9NIA;|K8rb|0@$-51CWMwN|RFFtnFR=jgxR5x%E5obT+*orKM|0J* zlDBk53|lSc=cu!P*mvTb%@0$e(dx~4TD9Y{=BUMCRZn^&?SmZl3W$!v zo|@O&e3q7sxp9uWO15O8a)n@2pAqAH<;H#V8bnr_qxX?Dk4A5I6OVYKo7I|Bl0ryo zTh#7>g<*Qb`bU3Gh*Cd+Rf@7|K3_N97C!~Tsj0J~gkNszX+0S}&MoYl)ev0rh{(t^ z_eMM>jS6`gnPAs9sBk<_j(>c74C;=Ij0}N9=9^dGrTr5mp%7dj0!+__0Fg>nHxR(u zGMX;dl-<6z?SfO)+u=D0-E3?|kLH#vm zP(d%$ub*bgSHBglVZv;34UZ6upWK>qYmj$+_Y;MC6J8@^>Xq^=Z^$ljD>*{9aH?pN zjWzQgyYa~$z1`SseykFLD;9ACDiX%ovy_(hm>reUsIVe*Z!svpXSxOBV4=~wESV^f zBRFg?fZ4N?4)1MkNR>)t`i!_n+2Q9hJ5C69dCJ4C+vvZ3$~?GRS<*^@*vP8VhnNaA z_TTMH6l8WX+g-i?rQ3Jbpb$*8k|>OZe+=Qv;#0EVn+KYN+UU9?#BN>Mrsmee9H|7# z1Eua7wQj4Mi)M#j%vj%9&%^UaPVuWp?x&yIO2|bz?JFX+7#ZR}PZkqUtA5Ma|62ph^%)L2Y!JD)v z$@+J0O9?V#$;gU*&!QPHcbRXAbre5;(r1OO99-;96Vg?0e05}Xbfwb7p<6?!rl8Hb z`AU!oK!g|1;BeGM#8;*Fxr8esHBD7|Z<|nP=mOPp|077SFcoey|IR8m)D;eR%nKh; zRSgweBu^lhn`3UsBZ+34#PVv#Q}WK!zNa_TDIutDo*g7kz}<_xx0DdncDQ&HF1vYB zj6A=!GdTV7vQVu_M-}bQbDjUJK<()%JY-!Kvn=+MsY;A+RnJd=aP;C_Gf*Hdg< zve1thu*;uNgu=_F%b?u&-+KR(%tMvW7qn3YlB2jbD1gw;iVPILa{U%sKi1i)@NwS! zb#^$_+orhPwLdr^_tKA!F>2a;EG*jY%ZrOwJ)bl;H>;?qfMBz!EHd-$sBu8cWu+K_ zfGigFGLMmwlA;C}hw|cCVdbxavv{nVXw~)g(VMGNL9e z{Sgo}MDz%47TW4N)sOA$iUA!4mNP5lV=X9?9&P&y2gk*^hN{A36cmb|e?~vvm}w3L z6U_GmgsmLly>{E!-@^tucnONk-d_3ewY4>7CMHQyQF?m%?$XbT)6|7_KlL4N*f5IK^P zD0z9+9F%{vPRVPPCMG7KxQU&Sk&zDD3o?=Qxw*Mu`HGbn)c|TpftE&AS()g{9SoJ_-p*!6L z&!7er%(=OeF!e0?iZU3K(C$SNL3z&z63!JO*X)2hs4-*hF5Vr zu4|i{=ZZaNA>wo0`=}Qa8w+#D04P!~mV=78@O1J9pz^_l25jz2AEh%Q=<2AqmsfA% zOw!vOQMyIT37|l7)L&~#hL6UwYyp^`sjHNySxk$ltE($0U>hSAdwO7N%1e6Xvb=$# z<3o0Kxmi#@+^469?nl`T-x^K`>rIjr<(V7*edke*u$ft@Nu^h&c77^RVAueVKbC<$ z0VWi}zx34#Gx8V)if=Kh&TPB$jbt+1VVNjN!vy4IZ?!9+rT`6Z$*f*#t^p_5InJ_H1HH^2ft2~1yRk{@!U7?+}#C&|C}Csjef)YKcim zx;+8MgB*9s>*)~%VoVJSL7=VC;qD5U*i4?FCN&wzH8V2{7l?{1OrDR(t>AbIfjql> z#zs%c;GmE&KR2aL<1RO1T{I*lWLv`+f(zvaVEp?*0Ac&=%nTsl)B;P*o>cEYD)N3o z@1_SzWl-+vDdwoAVD=?Fj%3oXzBWZkhiGXL4OSq(E~A^&n~HhPB`t;5eAiWr3JK}% z>7hJma;*^A1j$JhuY&83t5Cwp7^oE4)>2nDJ)*ODy)L(0)zlPrCQ;DJdxr6dw^D z4D;&02E5E7i1sM>{xiVDjqV+v=r#og1P1m4zfE|!(|!vow%bq{f$+Kp$dCB@7Jm>O z+;m3f<{iz=B9f9!OiZgker)aKfLI?Zz^7!8Ig9rpb*5bKFl$YjiTJ~Bb|m`i3mGaA zXczn9mpN)m6dpJWY0x!Rkx(8G=6{dgdX!IFO5n7M0GmZ=-lL57{2q={Aji~ZJNcXZ zanU5!3nj0|YAWqL99KU$i7^Wxw~%?&go1z%|634YVOnr#k0$h-GW#R4>I5B}!eazn zOJ@-`QRwxjit$IVuh~n40l{=daTUOKvr>S~QUEDs+l%gREi zKS&F2W`6vCz!Ox2mfXMr^a$GE1du=OI>YccC?IrN|BP*b$@eA2)kk2ZXJ_*P)0Q3x z`d~e!V9w@=jsv!wg^Oz-g@x!knSiS@TYGyuqEh}tQJKxWB&e1vbpQ1P+yV&?%wD_@ zo`;dB)ZG3Lv1?&E9@FfQatBUY;XYmQ&gCfzw47@pL|U+!1g<=`eOCd6hb%18>gsU- z`b9+8Pc>Xc)`RpC9vO-8=CMXv4H`i{1uBc7o#jFCWJq^ug(o`?PlemzZgw_9Wc|_6 z5eVNUB_#>`E{X%uKv`j9gG7J$K*9WTKv3kjwhq3GzP`SN#TrHq*MnR5`04f9EQ9_1 z_lUQAC`anSxf2q|x&*;sR7f@t^!14teDNh%{bNM6AwOwkT+h(R$bqr3oHN>{BW`hP zKUqEFZd!eRzoLxjPo$fp{tM~$Oa5crp7YOr{VJ@gs(LZ*>mL!|_k_b)KRj*$P%SYpNMtKHbFVUclbOc0FKbjP(B_z=*rb6fsjK=Mt*? z5fbh?2Z_Y^XY2@ur;D#8{7w~ULD661#)ifn)n1ABkM}y=*+1PH8qE_t9I`lC`rze| zTjdo0M;s7$C$^DR5XvhmamQv#PW@w@Qc09>2P#*^#<<*PY>bgAIG!U(E!}{6T4l2HE92h-F|7E zcYdm+|0;sMhfm=o#J$P5bRJevWH*>X;Pp#YMX3wm^;1mMUZR8iA9lPF?D^8qp@{|o z^cMCRihkLIx>sesGo+117J5eJpP^^~d4HkkKi~gP=-O(D4q;@UzNyT~&%$zZj1U6@ z15n?IAkD|br~>YDVq$IfWpHrtdXH7y6Ym~EXJ_XpAEW_OtEkwYDiK>>I|Y{3B7kh` zfnNqYPO8OZf@Y%TsCDkM7Tx$MsDdY@|FMy|t^Mm)_^KZ^-})mvyPbs_#>U3L;I}gS z5`Y#UcBU(MBl7Z?xwuL*GDM75|3zpuz|k#zh6Cdd(T$0O=YR)D!rMc$V(AS8pDHw5BndEZw?T%8^rOu^tT3gn;RQvt~`(Bvgl}Uw@Q@IIozpEQL;Y!=86ncsg(t40xy;JPri_-% zuX==+B;S7Z=n)B<)Z?IMN=gIELxred$BYrv zsV;f_NCCgB7vdbOmw5ZI+$JNlCxI9}S$y>d1DihMdoDb;QnOE`$}!(I6}Y5_Wjzts zXOluS#UdE4u7yXM0PS%AWAH>FTOSk`-NemN`~xfKZ|XjKV~eGhm#a{{6Jpo(0P+-A z-@Y&E@q=#>ghWR5SwFgaQ<~Vvft{Vb`WjheDG9BsKw_n6KL4ZFrYRh~PX+vLG5VH6 zx|7q=czu}mvufP+d7+3=Q0qXk7AdNUVL)026ptEpQRBOs46LuUsq5dWHxY1CmsNqC z5E+TaVbO^Tze->6OFDBLJ9}iNNr4XCb$oGupuulC$s?8A)jM$x|!M zt*YW9y6()yem42>alqu4$65S=txLd%gMG#G34p?~`;(KC_4W1nfJMK@11tJhjrV%+ zw&giGkFdyafV(I`K^>wn(QV-y`m8Vf))r27K0O48$Wqm|k18loNpvJ(etterqY`NT zJ7990ML^W4?^Q!Gv$L01R*v8Mpct~(>oH(HkCM+>`8tz3et#7cL$YJN0?@L0{R?J{ z%Wvv{QuCPZm6sR#F}x#6QtP#y3>^j*X_@;p%!Zqr8-NX&`;ZZe5R|!qt~@0#E8XF$ za3?IpQ|eWZaLa}JO1WYFae1yX1Wcf27~`iUi0QwX0e4Co$~o0y(d{y=y7vAT+t=QLUG*;Sob0YG_rI_*9>C`%{TcnbL+ zA*AN9bvzI_UxHy;&KHsC=I@Z z{3L>Y3ZsueKD_DR`*jUT&TDUKYg+^i8vq>5?bHJORHrRO^`aJkx%~*Q(s%>Pe+0)q z3;Kd7KC7t>R*3R)ZW4qgaRc@Tjx=a#klukSAnONy}}H8k#eY zpEhlI>_}f4Kn2{N7lbVb!z#X9Q!!7u<<8@g5?1wZdhmIX@cV2i8F}mQG>FzI7t{!- zeErDv>zUFkY&kCAQ10@%qqJlDr>gxYmIpxLH?aS|`Q!h+qv-lY6d*aH&w&36^*)M3 z?K8(4fHy%J6pLh9tOZ;{O?I|;3(x^T!QUnbEGl3&|8?R-ECA;6dBVMR0{^r5>%=|nEF3Om( zv9T$9iV{&tBwvyp`MUW|wikQqv{@=*nub^RR9sBWczrP$TPb||&1fKVpJxTeSV!>? z>}yw52FR2!|JDpjyj+>Fz@)N0KG;FH_BH|1;)@?4a(i3&1SpUg*kn~}HHXR4#|l^@ z6?R!+V(xl+$p`46&S40Fq>Vrv#x2~e1sY1q&l%h}S7&Eu$?kLf*l=IP!{ru90=*dr zF>DW+nVG%fuaXB0LYx&&vj&Td64hlR&UL4lat=Q;Q)|IPW0kTK5 zi1k0j>bg*fY}ZG7iYW&z$H~81H-KG;J~S*$FG5T&Iw3(ILx^sPz2dLhm{`<>G?_xG zP>EW``@=>yyx}ES)d(@FT7&i&B71|X&MfdKPB?drWn{Vl6)O$pQt50**4aaJ%(1!H z$8?AB5VuqIWT2DxJ@-)c-rFyi)IJm8%YbCu(g{d!4Hi~V`Ukdvcn4}2r;TD<+k<0w z8A2e<-??*VXlMw*|98r#rlyv~#q`|%EBzn2+m3E|X(@)^r6kNyPY*zirvzBnujAjk zg?{l>do0%vydtx&4YU4jq9DZFotg>cU0qmSE;_QCZVUiai=EGPAPf2V`K4gv_AeYt zjsCcvNVrC6kma=9PBo`t#?(Hvs}dNQ529^xHE+jeSIOtqEgjxB6*6f~K`%_LKLJ`} z)~~`MWNme|Ut$m|$;>crxdA0Rj1-$hikJ3gSV-7%u@HKBdl#EKl%ga{))x^Stxi7i z%fLEC2&BJ+hsc&{=9_6=_3R{Wg(`av@eyx&t^+R;Ck;cr#&KPSDcK%3SxGU9UF z*xt-;&}OMZci?Am-(cTy`6^C3uqhO;MC1Mx`(M%$bbb5w?O^8zA){)tfNE|u?RL0t zq?WsiYm{Ru5PbUk`w6vNyE0B@TO<7naJdwvrKQ!>?st_qvS~Q!>!)1491_eTli4kC z-@$SH`{O&pL$^f3Z1&bCe{dzB<3wg)roen)$rQ~CzlL@ON@er?(}g?Q8MVHhn&s9h zhZZSdse)2hn#mP-#&CWV#e-3u!#!4asCh;;t*E6{3vxrGKu8D)_epeo{+(wd9gZr}l21z=H*_*2PN?QrzkC{vu-Bai zblQ(E=vA>ONr#}nudmtXYxd%kG^Z}S`7`ya_g&ADTAAgC-o@EUoI^^FoZQ@!s0ePL zf#KEYDP}8Ew1~2elv*f7tQE;_6Sgb1ZEg(WL1G+^vQ0+VsedV9a!`_u`%~sSYfdG; zU#;0e{*2r^+25LPt(#gh{V@<^CakGt+r7$i^U4Cb3II5J-E}w`M0&?gT%iy?;45XD@24TsyRItk%RB9%@Hq(n%2^KFal=@R{8`~ zL+4tdv_p4>WSY~4fKHbNd~81!*VZC++ai;l zxX9KTEhM4P9|}%61yH{9_32^6K(~D{=;fP0s{`V_SF z$pIeB*_?7=5L{lQk!nnaZ!_mj3Lkf;{2SHMf3&08FtT$S8S7+3J4GCA71YBGniT=1 zJIHSvA&~w}h$LhV(r?6z(ma(QNjSy80k8!cFn~}u|K+DpmjuCGtvOr;HmKuAGJCr6 zb7wdF+M1*Si2Ru%T^6g>Tr4z!Wc+|#)um4^;-3PG0{Vx`C*2^~;wpQvvdt z)o#E5qvjSS4kE@$h8=K!+~zWumqN$J#c_0PImj*|1 z`HkF4JqMhToEAfot{NI+JGUK{D!KHgtihTPr3Ose!=q?&pN~M>;fx%7(kM5!#+0Tl zaBou9aIeadoONeC|1##>!>HPgSR~@13fwG_u{J(7@?-7@zYsoZe*7(*=fd@@scYSj zoyE$}7ROZ^jyNw#{vwn!lE9gjg(fpAYZkgKG}0mT99SiSulqS~&15QM**DtfKC9W{P5%92WKPJzJx=$*xbroYr6mA8grGj ztSsW%=hHmd9bEeT3R==F1yv?HuTJ1EklT2ysyaL!BuGZ4$)H{)El4njTR6Nsw0XN~ zc9BiYtmKY87upChM^U^LlmA2J)o>T!Gr6(Y2AoUvr?uK6Yr{<5@W#B&2%nF4 zh}GVpsMML_p~^gbvHKCR?O>`6>X6+$bKiVMkyAY*yEn8*%ZP)nUfu@l$8Ua&mi;x;4c!OZ>vaM!uY>%J`(8C3Q9Ok+hk&fb0hllD(PO~a0=i*f*< zg_9QYvbi|7b9v9_AyfoI;5hQ91(fgJx#6S4t0DOe-_vk&mxDs2st0`a>YD$dh6tWy z!JSZqHQa+89%?r>uv-YA{#wmShoRtcq>F6Lpvp-t;0EDm{-JY~)yE$fwxlBi82&@H zlPur{J(q`t2XkIQQ7UkV*|6J4{FxnegTnX9Pu2C@4N9{kCP993td9UXK#ZW~HuJ>z zSI#qi5={-%37^8=J*^b99RPOFrw;t5o)(&er}c*e$Ash2Q zv|pbyl^|rbL><5WA4mVPg9c9=1f1*DXUx`j)-x&hf9L&wkbzNk&cDgP-&ub%k5?yI z(~fz8YXW#dv$L~nYbO{P66S6ZVPC%fCWqVfMG|h6!2## zuRsI*(+Q|^P+k@uK^~rAV9D3iB(r-8vLVj|Ac13JM|yjy`T+F>Xl~Q-DsC@dTB7Ts z7D5)%8XAm%4#t2%{jZ#n6~N{pOGQI7k*p{q4`j8$kB>e0oHl2&RSI=L#{ls2+p~|| z`z6-DhYM)i+H!DmD$E|9p-fQlqa6M#mIsc_re_FG2$(c;3w*N%?IC z;p*`4aB{=^7N{8R@H_XWd<2Tz?(&c6uWe}%7_k_A zUszgx1T~05WA{6OJ0n;a7cYbplaY0_waM=TCKDt|83BX%63-GE>Yen^kihakq|AGo zI8OU=dkS~e#&mAY&CbRPxT$*J2Y>7Co`O9d>Nj1w-)ffwOg!Ax$qU>ZPT>~EL2eI~^CQ>3~4r&Xw>r3FGZ0Xhcodc*ii zOBer?N<(A+um?@m)Ce$ZIZZFl>jzS;NE~->P*0$|Kz~R|k`yTRCw6zXnojHJ5y_an z;bCGJ=nME$9o|Ev@9SzQ4*1!h*2VvuK+L*w{FN_!am?llD$M-U81SOE7=`Q)S7mY2 zR|+Qv0riu3+%|Ad|NJuLb5qZJO9*pX@z|U|rn>DLdhEmlsd}PzSM%tsZ|%H`uD)#j zG~tCpq*t5YlZ^*|=tlWM#~x6ZlXbFI7{xEm`a3J$A6)?(f`1$@=kSNp{2x6$e+F+< z;}xs&f42Pm)W=bW147_uYCutPzYqT@;NN5>vx&J=GV6b4$2^nqgq4Ggy7+|eo@tzC zf)nK8AMdUAT0mRMk-G;_M}Q~f%u{X;+N@k$DtWX)tVm9Viaz=9ZbsobXr2iJej`9H z1NUSFzWsT7RSW1l0+PiCRK)^Xvo5{*G}j($M`L#T+=TVh7(_eu6eh!V7D{dZ4{6^fdFr)S$_`Y$K2cI7i0{`T!%haX3Q zE7#2{>22%lmIel?EdV_#%FDwyyP9|jhIfUQaJWH9Vn>1Wx&m??+F6=nsqy{FZ=tEb zx$l~3sjdCKtn4B1V1kv`;Dfj73EYhLnVGTC-&9DtU7*+bLu8H;h9z6rT{8sK)xTL% z_+56hbJ-~=b@lY1s8%LTlHabhPYtxdPSP?yK8~$wVah;DEA3gQjS}P8=J166_J1Ng zK+3R<56J`Bqs^D#{^%b!{0T~rc=hTP=%>S^IbZ(-sK)`essLnNx$I*}PgF-x7yXe;z!)7K9N-!S{x|9uWg`2DZN&B5 znSEXVT5|46m~m+nd-f}Mz{hn%?%6H<>%#x(HW7IbtR|hKnpLMk^QTK6&~tw;51Y)d zA0j>}q5OwZ^Lb$*|7+He&b3oP#7udF-;-Zg0K(=&&|EcI{d``{@Lw2kFl@bE=1x3JWh{2C{(&rJ_kS@H|EIxZe)M^ zIUoljN~G)e@O&eua-mW?d;0{?QrVj!M+n-|op_@Jmxub8~X0RPovFZ+=*4 zNgJY=MQs$Zw~@xMTWUF}wA}M+fC8|c0q)4`m_M_aq~URNFi=}%+{Z=8Kuc9G zb{(mWv&B~48ga~J7K$9*N}D(wKdrEyg}dV89qe~H;1f<{5a#%0OuXix@P(TIPf1yo z+TteUASojWZeHVEDvKk*qw5E(_xy&$p4;0&p7#;n9%ZWJ_Y9f3zmV3|*E7TM z^ZhgY2f%hQeIXgidgivXOo}_pg%g6-da*6!f+xL{K}Yc?J)YM({oaL(OG@^GO<|yp zSLeuj3&bTPWSms8=9aUzlgQm1+GA3{PDeU2*2$d?YMSGQ#YWL4&qqo)Uwvwtt&|e1 z)>IW26APdA*bF6ioFLXuduN`OOx2pn?Xb#SJ*RBV9MOKkaC2C9Si2efH)S)Vp-yy9X`Zq%=xrrQE%)#-ElzU;SPE46p&Oj4@B z7Cv&JF_Fy7T=AYcS!`qNcv9K%0{n8LDfX`;ZabNxHavR4ZFSt$yunRPleg5HK5J{< z?V{@SbC(XDp`pSxOA@s{yjF?lC4>)j^5C!Qqy)FWO^7O~qYJYcb`*`dAsLR-q!)4r zv@BSKq{pHSh~-$-sfl-*7FxijK&enKWoQgh^#ZNN$@e#{#>O?&(S2EwW$N{m^|4i} zk3%%`QM-p#d1ZBj?ad52j>MsIrAN=++waaNjKswrW~>)mPPlnc7?fCO#ShMRr%b%7 zBDEO8Qs-eFX+hR%6{*!$dfdekX3NDtXp!VgZr+pJzSNRgz=`#J(wEGk_vuy}Nr z0wU*6ufj}`4LrWJCp#CK;c>0a%};Q^E@X~i<202g1r?(fgMG71sphF*>yZt^%SP^ufdrfI0V7z z_-dZ){nieZgs}-+Yg?ORZih>@AJUV+f?Egz8}`g?yZ;5L5}6?rFDMX@(Qr$wp(nEX zQUUhBYOwCLFhFf4x9U|B>SuQ}y_sylE{h}!;x4~kINF~F+xC*=r_=<=3lw1;3Y+3cp<3|8H;Q?-gHE{+vqy4%Z#HQdoJZi?6=^^X$dHre~;! zn*z71fsY;A+-979>kIf$x}50mmonDuf%pu1D$rNM+uPP&-2V2il4}SsG0a(2mAth> zz%u=AbHVSrHyfoU?z~jp4;%;Ys^amRG-vic-pn;m_44nV#XBe-ljW(4y4GJ~_-CsG zC?!Jt=JtI--N_SY|4!Kzv+`O&;Kli!jH`X!eO_ExxE6S#*HI6pS-YN|dz?Q1@4m~6 z7n!f`xN>e$&mVUCAASGy_gq^Oxk|2Oa-Mt5=0m~$YpXvTOnt=|Y?-a!^`L|>>Qwj`IL4759TwgFH{Y3_C34(%l{WIo~%;ZHG9gGC&ka_1+O{` zPK&?(x?QR)-QU0aadG8lr;68t=e67JU6z5K%Qp>pPFCpgn4Lu`kACky{&bz?Mb0E) zrBbHGesBIQReam4Eh{a3_6sO4QktZkD&X4Wt)=^Sk=*W$)2t)UEOL$i{nCGXv>cOQ zuSxc`=Vzw>+8O@;FwrVXJu+vie<325SbYy{!M>+jaZ@LQ_R{l(v|L5~wkzF`0w^d%@|+yX@rVJz8i6K3(bj zU)EE>cit`n9`~f7gmL~8((#@kd7eI?0q`TBltcvC*wIdqI$%2ia`2Q8a6G={Amh8n zuZJL~hz7V!4i(({?}hTG+DWP>HwfT3I(3Jp(j;%CL*3eNdzReLTC~#G;#=1XU=#kU ze68zJ<=N$LRWExIzqcIuo=5z;1B|*iUdVG6Ns#G LtDnm{r-UW|bOYve literal 0 HcmV?d00001 diff --git a/doc/sequence_time_pll.png b/doc/sequence_time_pll.png new file mode 100644 index 0000000000000000000000000000000000000000..0bedb86ce957a092b5ed6c00ea33c88c2964156d GIT binary patch literal 33133 zcma%jbzIYL+xDF$ycP>_-yD2TwMOS)rp zj2H}fuYtd~@8@|wpO=5c!M?llJdg7@j+21LiqgavuU~{fAjGmVlFAUs*-{9Ep!obL z@Jft~H6{3m#o?i*gOQD`tCfkV14P>7g^9g^gNgACL)RP69UN>$xVUVs3|=@mT3c}% z*;tct3)4a%r!KjvYC8P&It1d9n5m**Gs%6qM!Us}+AiHM^~}?E_L`0{MI-8xsn>r` zwJ4l3W+{ZZ-|zLR6-=Xib6?}>jIq1v=+Ek5E%LVuMdvl^M8qgqX)*QC@bA84Ez#lU z6$roE)NRG<`F_JXT)f~gnqtsj4A){Sg^urQq<Q>n z;*$sg#kJNpk^F4=P-sNeMa|%pcFP?1yS`GP61`5WFZ74P z`>?N#SXqDj2&jj?B^^js%G;-{IWz9lrf7!x=FPI)aJ4+k`x{S2$>R$~JbtB}jIpK{ zgf$7zdI%o1eN!kj^r`Vq(~g49R0x;RqeN)+ysrBe+q6{{s%!I~K~C3|Pi%#eeeO5X z4#6xLa4x#a$52h{gxhoW&RsZ^%Q#}DUe-+9*?X_KyV_RqWxrye`trMI^p#;Joc=|- z0_cyj8ke2}bB15Mub-b^zg=;S$wwlP*@FN%{>~9PtMa8%@Y;P&j%!$Dca{9#5x+0G zv3Ew(7TT>47t#i`oNmaRPBJBECSj;mX@oI)(0P)*Aii#RJ>2+S+@>NZnJ z6D*aJJtbUJ=%mxCeQJkdn`%Zrow6jMWdj01%*jgLQ*{|!8X?uB>`z*=i_pH$a`R2J zaV$P%8!+;-Eldo&*xnwu`F;<2$eo7%Gh|WQ%?)$!XQlH_k4t02oCR@9^;9jf z9tgL&IXG^YioaEUm1j|IaZvAIzJP@08h$>FMbq z#_L|aeS1!V9x@NP{KPy(-UIm@Q6iEqau@NYNpYAEA_9?A9b~3R;fjZT4*p0I3cXoG zu|x15t93F8ihzAc-M^Rmc+wv5g_?CKY&qv<2TEy}kUL8RGR4DA@dTY}&&YT2AtU_$ z;-tv6y5B7!xPGsi$cTt*^a#hhb{`_>; z58h*9l1WHNc=nv$O_bM8)r08Oq-Qs@WfW>k)6yQ*W|$fiB|C|KYhk>!6K(R91#Sne zV?;wPO(slM%pq>FF*C?ti~cgpozBYHKeNC#TTe=wu4J1p((N~+X+7Qzwft82vD&2S5d3lwa zLpmfpmmWz;sCTXQLOJ7-U9)a5Fzn96l<95|QE$K=r097%nkg%L7)|M!KNNMI{rN+I z7rB=()+6Iy+#X_^B(F-yoq_Y$`5u3Gp5m%g6v?OeRAVgq^%2ZUJ!4g?HDzXpxVWOn zqqk8r+FH_i9NK*i4U&~QF*IE76;!O2#ttV)9ar3Al5*K-;tqFz>btfOSN)V^30P-?^%!Z;5?1P1+ z`#ts0JTGhk=8W-SS?%YjneA8oSJuVON-22_Fu^Cwmb-WIjg6h{n+=}c_PdeUb+m;c zSa)zx%;#rkYpwaX(ph(y{=;koM#6=yK9}&$*;Weps^nhziAX z+_5+ro#6~GY>Zbp(fOSi^#VPlWxtY1R%PX5<}cX_RfxGZkFB{decUaj`ys)Sfootu0EQpgsDvoG zpWE-cIevW3eVRjX3mQs~62oQqZALYYHsMgj`VWpqB9eoc%SsU_$BJOdJ<_v!J~mQP zxjJ84FoTv#6}6R~`%2TV8z+3iiZ*P2cp6~|-C{R>q~_O&S*y&>+;35NBL3sx%^Q|R zm1j7qLKY5iW7oFf)<~8t#e?dVm=<}L-CjWh>%~5(nMo$#7)$vs*77aKH~Yfxe788YUk2y=0%@*4Ss~n zYK~ye?X)JdIoL*dCZolXY!%_EjCmc zQm4O#YU1t`(%hwZch^#ZXlPI`)RN@D?DBr~Zd(#|gOXRr&g-@xi@qr@m%*Mi+^uf0 zJG^ZQX$KJ$m`{x`VmQ9m*{wpp!E#w#B&aXj+e7X02~IV?WY)Y6wsbo?QzBe1Oa%qG zAJ6Nfudi-YRMesV(H1$D3~rNxbY`5=lj)v1M^Zq0t`Zr6-!zXq@X59CcFV`%zqi1N zQ{Nf?$WZjA2;Dt}k*RB2g-(dXeI$AIv37O8k!D5&YvHO^=(rd%HOVA?xVxZ#STw&h zpg%|`xt~ot)Ag-{NUI2(phkUK?wXP2shP%WO71(A#&`X4YMFEMh*VT;bW0bYZ`oE7 zI0b0+O2~Ey0ws^Sn}vM)zgz`z~1Y@ca9iIJRpAK?4(n z#;J>-Z+2H#fuj%^zDq}&`Q#3Lz+d?oLk0|H{uEVne|vLOKK3UxM)`!*p>DROW7`jQkz>r)M~@1MSEo z5MG;bNBdq|N82`eMa6uWkEZxbZAwCWyYEB%j|R!yd%0sCFP#l1gHG4vhK?M}OOt+F zPo#I7*XGTc@gs%vyqu-NeD+bCER9qjT}gfuy&6s`;WBEUC^%>5mFmZ`^`S9hhyQAf z=de)pz#Xpa*)VBjUas8iR>*mBuo0$5-LnbzqfuUJv3omSUXLq3tQfRV_Zgd1=*0+_ z9Kw$_4P4Dp(E_J0R{NX}oR-FY_1qhc3fy1aZ<+C9?}S@S{h6^+lxgMj_3OkqL3lca zcGu&%_7rrEqSNwTj(jdjCU0ggVG_|S{#2#f*tYlMc7JMZ(9PQGa`u1P_*?+XI@5>2 z6v$pl5ZT(S-Rq68n0U|i;>m|p*^G$u5*>N(;D_rP_PylPE)Q*OLzm>ZLV^zy>#NJx z*X;))uXL6CJPx(X?t8l8e)w5qTi8T1qI*K((3V%`?8Y=rRj^Qmip@}}$Gz{xJnnU{-zr`3#zXr2c6k~eu z;x;-$gE!YNM`#iu9S}k4zh0}@x1(yFmF^aF4v_$TVx|!P5j^N|Nlt{m z^p8KH77uHCB>`6JAc0W)gJDmPPhrF0hsffsj)Gnct{uvyukvLxrGMFFjqC!kyo9Lr z2)JhBm-Q+B0gNQx`qR>Z3kwUmd3kbja%JC0n*yS{Ihbn-3&TPPASJ+Pdw|H1mi#Kh9Uv|*Z$b#-+~ zQkb$e6-yFd2&1j7tv$pbcfK9N(F^}fEK%HDJ-rTKX)q*`UBAzyxJ`muhQHo*0^I8Y z3O5CKveJd{Q|~28;qrQpIP6}-!}73@=DFI>pA}BsGcq#r@w$C!_r8yQxC%S-Q^m4G z%zG-XjdjR)!*6UnwMu7#|D*ls1>g1qlA{0;s=?A#q!AeUPmgOfg=miZwfys(+WUw% zcS#|VHs0H$9cvK$)6Td-Zn&K;@$!C4h=1nIj?@1PG#KZ9_FViaNV?G7eZuE15Zpy9 z+3`n>uQr7oe*^K7Mi_f<2^hRy*LRM|5KFJA8L{_mYimnPOk7)A^Y-@s{rh*RWq+{b zwMIqpzFDTIwqONWSy{;al`1_Y zeXymfs;V|N1?Cm(Aic0~aY46zt~a)nnUiNLCWT)QWexwp#?J2G^&T~&Tsk;7XfgKN zxU}NsTy9G(gTixaxM8wHZT+q*ZNOKaz#rRJOz~^B0W*qpXvWmWd{wa^CnGbc5aZ)3 zE-x?7&X%67s;b)D3rb@6aVIJ^R#R1#p>%L)C?O>UZrR7KMNLJe@#IP8kP@qw+wMy5 zt?avfDI$)!5#7x01J$m@<_5B|vK}6|Sdp`Wq5?HNU0vifG}?N4oN7J8!-4NJ`$llK*lc7K z6naCBsD>#C?kL~T{jkVL23^bUD)#+)Dd`aUsL!$~49v{gxw#8~IM~bl z^of1o(=Wg_NVnvOx|0xz2`+xySWf<$V;aq+kF*{V>^-gX)hcfDL0eN(Xj_HDa)#M+ zH(9}Qw=Ihr7K(j`w_#VzXMe@*)iyUbcQu#5RRpnQ!c)0boZ`ynq|v&be%Z2ur3Urq zv~CaCB}46gyBHVK2))oLH1s{NNJ>fq=XHMa$9wDGwxTfY{G6|e2?^RdI_!K(V(V|* znk_R2JQ^}OuToMr2U@AX9gN1VTfZ|qBBOqcXh5h$pIv)ur4;mYk=YsDm09#yg11PY zvl|4eig?7y?vrwZqu2mg$(dbvZ6t2q>B!mgDw|_9_Qd3j2INyaIdV+MddQSFJ-b49c zsqILX?uqL6^x&rMVa$zm(Xb-4ISpIG?u{s`*4PJyVj`&e5*-z5t+}S}_4R6Iqn;-F zP;*{HLk~B9*JjMy>~4O=r{&p`#HhK>=IrJgh~E3<>UEccf6vS4$6dnsB6KtZ6H~rc zu~cQbf`WqH{-s>^?FI@Z$%hY#H4qKIc~mei#L*vQNJ{FAK7(z1QLBI^%}f6B=|-VuWVqpGq)clVbsH`5=!mw#XtJIcA=wJ0iY19q_*(L0usLexQd!116Nb;@#ECDiAxI$ ztnBQ++S=5)KxB|Ds4DR4Q4;6(**q1}>h{H>>Oe4tQ#bdVdZ53++ZtsTG}#!RsoR}K$6TkbDihC_Um zM+t02zkmPU1X2H%n1ro$7RJ<9pbx!%-2#_nit2lA1=;>V_!_bj>_I2ql;~3+@2B z7ino}spDw1D+tL&GXN)4SBs(RKE{ZMi1gTAC>r}99fCg*_=^or#Fh&CM!3V*Ai&DL zY|2(jiH^$ro@uyMo)8ummSv~8H53H1l$p~C(psFxm@kmg!1h_yl78dwPaF+Y;*o+v z)Xe(&I=KE0WA8V+GZ2&DNM==654%P@o-^FTpq1a^FP9Vg@dm6bD8}fC!r-5T;D!Rs zbeL<+@Nr~o2!lyuF4G8p?)(i|dM^Z^5bBf*$F}!t(8P7MS=V%tAR(_2pQ2obdYHjF?w29*~gw$T*Lp-lpn zkoSCj^2}{K?rcnqj;{?XlhRUYg3fRnD!FY2J9;J^NWziv*3psTmYEY+1`Zn zjuX8A+jqWyTDtBh0eI2-KhO6VuS~?9FF!^Js%flgZp}4fa9N4-+}{y&wJuNeg=BS7#}~5QxNKc zW61riJQP}3cUJ|%%^3G}d4Zfc#kP}1L4dxGwj!Jm1j!z8rTy_?jXxHJ?c>MUWo12@ZW!}NwwmW)h=~r{} z1yWMd^XL78f^@5#K5tH4Pdh!ga2CHFF(`&yZ%|W6T+8Q@MKIjDWmq2kf?8;%6xZlm zQ&U4pNqOzsHIn|a$hbHy9i8FsZrJ9?$cXC!Eq(uax;rOevs#Dg$=MbB>Vwt6g-=T- z+L-x^ULS3(*RNdR z2SEKGFyI$}O+YG$iHw{VSX^ABASVad8KicnrQe-qE{`65?&$2~abD5Ux+x%_Hfv2z zz^n2H%T{D%HBMwr_bMa^{%CA``r=#B8QRx zccm#LPzGTPU=8@Kp|94cmKJ3q6H^dGi@2GYr3W>D1FADU!==PU8lJ!p=JS-GUfSH;>U~t1sB81aIohe0JWR}riT=UCPF|}Wi} zh>vO1r00^LaiY#EfUDi|8gpCxJUwkZaB&9EIx%y()_4IuNpfr`U6bTFFGl3d|GeUY z*s#jVO7KDG2ow(8ovos*yuCVBAUemUkslccWVV^v*<4>?Vd2HT&k=EIdFtp<_s5Sr za#XS_tEz&0!q1#JGtaD2R8$n7kO1eD)zVT{4p)f&##=Svh#yEaBW)6{X=4UObn&8U zTow2vc4ITMDri5LyiN^1YMk%@<@Ck~$;g*2$`NN4m54}kAw8HF{{gBTfXSSkoTB%;yDn49tR!Fwgk}Wb zhFah6O-sv*aqwGMTPF$G=-JwSLCstwC2dEc7FSlB;Q8Pvox~F$F3uBp+luev{1rsJ z-<>_aq~6d;)=+TIk=OE@L%c&ygLL3cphF0X#6S1e!xl?+U`$wdoM!OU0|?Z4VaGwM z4vr6Gff`^H{uR@8!R@J|KaPNEf;a8hl@f1&(`EMt9|nirr^7&0?>Ck;Fqi;X-NMpR zDM>_N@8NNHANz79@!{E`aNZr-Aa$oTMX=KTc>8N*G&b{o#xsP#V6#=6cL!ScZj7Z zZm*au$Ap4h+hRe=I{P?5HG(t*fs8p6F5{Y|omC&U0-SKWdtZAeaCWpAPr2YH0)Zg2 zme&@KcG{w5G=8~=*y6MOM%^{D9a{PsUXT_5d!P@p_%r-X3JLza=UZuzQ4Qxv&&q}i zyu?JnJV7?=B?5&P_h=||BK<1FY^SU|+7d_DZu;Kz>MR;fo02SGgX8W2@ClNps2fWOfs#t z&U+RvS63!Uj>y=%$x*7E$=8E`opHI3XgGCRfxw&VJ8v~Xc>=w}_{=l4e_YirX`QuYoFSw7{IGc+`8L7|r0H^#5CYH*DgU4y(mx*o`S1B@y~+6H}L z(aSGWNkRvtEcO^PQ`4(dR67_#=I#z+uvu$7Bhl34Br7Yc+rDgXW_ET}ibIa3me)d0j^ah8e{)vVKiR&# z65>X^ZRsT{w>pQ%ANU(^NMED2a|Z@9@*nA+I11l>{MOqWGUX*IDq8>j`&-Y|qL>=@ zo$E2G>gsyVBaSiKzx@!8vlzw2#b3R8mF%>T9QcJ@^WT&dHEBmtn*G)(&IE#Izd7L_ zPdT4#lgf_TVU>Az*6-h%aRmhhG5H0U64-IFPSd!}$(ek|E}gx9N9W_53P2|w9@U6D z5Bu0zSuX-2n1~2e-k&~y4hjwyM4~VmA?^bp@oa2va=ahu>?{Nk6WyznET%QKI8j}v zC%U%BLO#$rT=85k6M zi`w@C{n!@^w9E2SQ|l&V!ya(29E^YS*_$O(+FC`bh}&qHgD8$Nh1>wAuXTyVdflJb6NP{d%rO6ABffSn3eIrErn=!(RRV&Z8g=RDT-1SUY#?&K+Q=_=rS~V#2L#w-$QcRG7BWmtZVi z;Qd=(VA^bKY|D#dvoaiP#1Noc5J1Me=A;3=&%b5g>wTlqcXPHC_3PI#AgvIxHG=~K z=9`=ax^@pM_hh9(k6c|8fuH%wTlyWT8hmM zlU`~s^2`LG4IdzN_Hh2m0C=>6ucjt{JZmxoSQFP!eAUPIzq2MvN~JCfqn2*j$s|7! z3UgcLI`fDsg_N@FX8`L43dMrTi&L+y2b;)r7Fxu*Ruy~Cn`Og}B2zr$Tczx>#=!J_HK?aM&n7*{PU)tZav$I=Z ze#Y)cwH^-*E1PGw3q0W;ZiwTMhjUyPiHHE#*ej={rPT#rU4%kaL*SFuby-&a>pPw^ zcc*LjsH}~pb0K(wLRMCA2veD5vL5&|pNdnrQkD4ptxVovxZ%qi$`=+cgCGN4VBWNt zjsb?Qfkb>{aqZ1jBT;&%41yA1zSY!B05NgIO3L^H)!JHGz@M*IXTk^0)irC}<7<#z zV+``Q6ViN60_*ILB|{%PDiG!8cK~>I8~ql5Pdu>F;heIW5or&}$jn5~i3UE^)}Cuk zprWN^*CsheE)mEwx10|j9t6qU+81}j%-4QkdrMYUlvhPR+gQW)$)CJcNZ|s zgI3*(@zFo{=&ROuECH)*q5^2d#<7Az;n41EB~(ihOAcqw?NV-D-Y$NUJVe#it5>6w zl0Hj?I@&Aq1F<;F^C0gL$%PAerr$Hu?l_2fA16ufjQJ6#-7PbivWahWya?;XKSk{c zijbhG;rf~c$Ib`mestXkmtVCRdBE}Nq@0aDhFW!mADo9K`j1wV*iZ$r>ki|WwDjoj zOf4nXtCIaG6fgYAXjA-di!@R#``x!J-1b!XWP`o`RTq0m9%pfG9_)}Lkc+_JAdsUe z!y&8+O|sjIt`SNNW;mTk!h%q=KF`#zVlHK2kSxF8rKNEn36F{Xt(^iE(!?j&0pSc8 z@RMY0LKXs$R%a0(Fa4vrf#(3tZG68|l!vDfSP4qLq^IAHCUtWLrIWe2`Mm=W zi`tV!nTh_?9$Mto)VGO5MMT(C@k&c#CX;%ZH%G@w|~Z{C~%69K6<$9)j6enkv7ZrlKD0FZX~bb)dMSkhwC zR#1Cvm8_4B>S?tesE4hryg-T41&+A8uAXQ)fn3*8lz77K_x1G5AJL1sJRTF9L|-0u z&Yc5GKl?}0arC;d{9S?dIT;8M;N zARw@Zq(Oae%FDF}H7JyZW)VrGByC;G#cT$|L40Pe6WWBGJ$tsxIy#OL@B_M4eJa<# z|IV~AHwUSnLUN~u1DwLRa|G!5c^M_87|u%1bN4FefdB?Vp;Igr{F3kV?)nt(%cb8u zO0bY}o1eUJa(eoFQ-D}NalQQh`qishQhrfh))w_l@bsTvVXnG-+rDw2ax^uaJv`VC z5IQ)W}HUIWuG9h95t) zX0HtUQSpS+m)rS{j*k9#Pqj!WBp>po++X0;0X~kGf<%(n4(Y;Bcr68Idt0p zu(Fv5+-5*$_J->6{8b&ZyDbLW85pGfg73|L$eQ`17>wG3!%#?7V?Wb^JPebxfB8~j zDDeGzl_}3hs|RTa-hy-0cbr!e>oXo`Xv9?8f!vxEgI9oSrL6vyVdv48G3PQ%lvRZ< zPwm25TU&Wm0Q>*LX)LUeS}ulD?ZK-ir*lGw{+?~$i@24W-FH^hM0~|4oj(h+w3;}_?(rCD=>(=JpEJ;muZtkPX$+WaIpab%6rvr81^y$-7?3$@B zgt$dK_Z%KQdRKj&mNqdaCi|+ClvI49UMYLvN$MQw?KNRldipee#^$soo|jkhOEpzR z++<)d1~{@Y`<=3iiis=G%K^zVvSY|D1yo|-1o*@*EnW`Na=Vu1)hr|u0g4sQ7`f;? z$$%WyZD{~_3WbYLV$ihoUBGarm;KC_@R(cpN!8ANLh3v&xcu?dah5sArV5e(R~+!6 z?0>(=F#w-(?)boiM6@J23R|d@KG7Ur2&V{dPjAi#f9*p03!%bG6=gVj_H7c^#JkqH%u`KGFCY`G-09?F1E}Ua%yDYLdm{0J)z$^3p|e zN9Lc2uF(Oq-PmpWrT&;_?=mcj7JXcAa|4?Hgpj819VvU7{gAV{f#nZ@4pyt-RDU^y zTk*pVU_r74#Y$~jgmRBwAeKLm4blyJ!J21ev&&SU!2h5F;*;F?ZT?__s21VaJUH^L ztao%Hr2Vj!E<@&oT7tNrplWH0mIe*#V<9{PA@Tz7Zdhy`EgL}W1^%lEz(yOU7h{X569 z__VTNDhqIb$;-zG!tJ-QHl(*6-p8CfnQT)CKzI}s5v$YE(__W$Ui;`gShB5D1y}!_D;GLxPA=pP zIPWhych*<4uv$8t z%KX1jE|P`fQBJl3&s?ucP3l}SDy%qGOz>;QgTsc+&Qk6S^9kRcr{Lw#xij-CZnfpR z(CBX8a+y9%?$l}BqA#TaKWp@jt7D@D=6=Ot+C))Xau&106<)kGi`co^Ym|;A&O4+E zC=*%vUusI?rFBI{SweTp&HZhlY0X8dy}d-4ZME3ea-JbsdIuA|@M7CTj~7ncRQ2^f zr%rLE@S_(5ERhx-r8#KPuIkLKbA*J$?yJPP>k08?D95Cswp(~V`k2cjmt6ndbU&!q1OJtOL;kLhQgR1`Y#b!+CDpzoS&Rrw(8Hc8y z-y%!)_G@b;nCIjG`;$~Zf|UYJ4`BK$mJbVjkMqywE$1Qy`pqK*z=CzVW55>n<;$1C zP!zRqG51wP{^Mv3&gUs{v03Ti{kbg0t^uM;4#Mb_b7wuj%f@K#9j(MgCfFP%I^1gx z*_{s4`~HBhbZ&F*W&L-FoLP;}kK87F_vIJo=Ei1oN*a~!z1(~^KOefX0+o8GYhJzB zVCsChhEZ^IEH!y&oQfsGu4XAD7#bOwAdyIkn>TO%4N%>HfIAsL!Q8SDAAkM-Ffu&{ z+6X9$0Yl^9P_nMOr$w%$v=e)EdwUz~Jt-rc{EslyHQgN1o+u3C+gQcht>|KMN`R{k zRLD_|_}`jVchHMeB^70!xRZU~z_DwrA2n)jEVis&N|!8@U)hlWgs> zQS-qVJ4-iYsleQ#en?aqV0fClKiu6zQ);!&RLVO!kKfIjiC4aG?_NqdI6wztzS*~| zhwqqmW#oxl_Z6uYwGP?TGTpiJY=0ZOB4G51XKH&!x7bWhW6j>R0AZX{ODfFF&;QtM zyvliO6Sp}QNncfY(6CXC>0>8i;=>;Y+TE>wuf4U2hk|~o@0jyyju6pqk~4#r_2-1yGd3x?gjd;NOOqT z+TGWfE)&zaHtfKFeo1qP4E!;r{TR~bqw%Z#c+gD3kYpfiG4RPVEZJitzpX7QSqxusPo&kL`CqJ&Jb+|g*xq$~~1Lc>bTTBfrnisK0uTh?rv>9dR=3^lh* zzL@p4%Vm4O*S;ET<)he$&CK+4)wqFbEXh{acyPZ;Zv|Cjx0YrpYx8WKzULOyxVW?P zrn0wLvaI_|)DOY3N7q(g@n#-fB&v;$+B#dqz%-lt_AR0DY?5=d{!4|A5hHPmeg_rB zpX5=MMLN&5G>U#M56z*1jT=uk^s{!^BU0Bfq-WQhjg?ik?ZoTuRk5<<3I}g%^+hhP z5c7#@*G&*TGllZ?l?wooI#`X8MMx465k*K;*!lQ0h1>@9=m|rK#Ngx}J?i=1((*{_ z79XhLovp1uAK0O>YiqUw-Mzg)tVXZZ5k^7_$0CnyF1GvOalo>QJWSJePzUK0Agi~0 z@br{B+{CB42IGruC$8+ki>sV(WG*@$Y^8w8>PcB>_4c50oUj`PDjp;@iKIi}WL#WA z7JZ1If;`c=qlljO6{YdD?-}X5t;YTOuM)TY-oH zc!o#q`Xu+v{1a(o)}@^iY0FK*tTteJiyg;KGTbjW*1*;p?qddvy9Wfp!oVacw8Dtj zQP3U_MfWDJ;Awu%SpoU>L9tEwc+8^lEOI||)*~%Bs@8 z$Zjk3kxUb%bjaZc&n{^VU(YZxe28mY*ouc1az$lRR+Cs7YH=%V%_6e~Binjp2(`C< z5js~#tC?w07npr(-eoGNMe>V?NRAmBw6J(!I!PUJ+e|H;M_oUo+gP+F9i0WU>kT5l zeqvK18d+@F&~Z6fx`e^TgOi(^Z3RMwJs|AU=Kc`IfXnag>6tF28tFee|Ldf56mZ&r zF*OeRr4#a#0dHnSMa3SoJy@O@>nnPs!&t4gN|uEEJ67++Q~4})?v2FsjEpR&)V@89 zhj-5+fpH1mJO(lK);zIK0PNB%?pZ>r{QQw}+B9X?69-tm&?cST4F zX1Z+@5uwkK@&GZJ8)YE5yqa1um)s1ZPxVxAJL!xIM_yYIw_f|}neEI<`LgtR14(=^ zIU1otu)S{-I=W>BU`8f-+eTxG4~EEIt}6F0tsYSlb)8 z_PbyxGRhi|A%KzrY!w%B7bNbfj!oE~+PTdZPo!{(aB{u?zGGe^;cEYWoPYI0_yA8j zHf#nJ16)!3LP9%R@K9w<%~(L>>g(1bJb_X^+nTUNj+)|)7Gz_SvsPGu1JVTps)#5w6`_6pe`o|{8L z!EoMk;J*82oc8EyPv58|tyhDmLQM`n7<1i@6_1?SQgvMMU3a_6URG;0(7M>qocCIm zZRE>MlOONUjbl5Wo2@1bwY;!>0TM6Ef#(_phwte9G?m~7=w*vY^@I^(kg&Akq8&{t zI*Rg*^^!gjcsiYngbtKi#;lVifPP`W&LJkavYHyB_;N=_M-YwB;7&R)*8un9f_+5> zMZVkY>6T-v9=hU~4CPrb>G=4!&ri7r@ADH2s2SPog-Hj`-xRb8MrjU$7}C?D>M9^1 zaR`9ZhZJZb-YqK%FV6aYg^CI#b(LLn+`~H|Yz>ge$q3v){@B--f2jASO!RM+3$QS# z`E!?N+e-njUgeLEd#mMzV^R(jVs^Jl_22kYetGkNU5|6q@6Tmg+$2pUin?QGH1` zR73xOFhO7|D?T0x+jPT%d|9->PBgW}uK7YgcO_UZT$e>oPQ+JT#E+P<{dO||FJoGPIsMJuRbWD3N?^8&jrFhroD_6E zyX39iEjX_Oz($sRpR{V+1k`vq0gEQ2bg(hSjACNIdkN9W)o}iU=sYUhflJ4Bm1$|v zH^dW!X5r^w`$4s;64D9EF<)F0^_3G-qVT1QS5RiDoS$Eo7OWr_w%>IO!4)HQqRY*? z#u8pIrycklE2j5Tnc=*g%=_7|Vq+UuRrL@%0{w3u5v7Q`r?1Q2+JM!4$&g77l2O|U zkNxLOyg2IT?~Y5&*rcURP$VM+)DBv81iEn;g1?j|0x(M^&_?CGYRQ_zQY9uUajMG4 ztdHq%>Sdub+<95t<_Pr_kSaOgFJt_vrzx;x8?d#Q>89e8eYa%}Ikx)|`g*(L6@wZ_ zz_i!{%$I)(XW&b~lI{BL(7UUPOV<+vA~_WLt^5~Q4<4SN21&#jL}q4|V*CLxNL~~Q zgy4GuN_aYY*1|{MV+=BwVFz)#5N=xbO_-$3ZgPmk?%MQTIGq10kWqj<4mu$j>qw)B z3jiGp+Ll0#U=88+V91rR3OnEZ`?n0fTL{?i@YU0SrZZ^90J#~+@Q=()MIo061DMIx z7{1E95e6Mp__iaTwVc0ngGL44It8h}HyI#F8Nfkq09t2`MHawX-2nUr zbP@dB0XDi=(3#Z>{>lL`B;Ht;On=vKHGmr!WsVz$fT*I?DEu3v*dAm5)t5QUh8`6sQZSsi`R`)igElTr~Isx}37C>Py~uC7>_L>47tZ}kaD&^Gt<>C?xxK$ZceWApd2Q8zYWVXbHx zl`wph)Kt1!xgQL#T^w^ehDB{5O|>?wZXd;r``8kat-zP1WtGeNGF;PAGfY6U;) zddC|;%iW307eU~5i%d{rGtz-cN(E@Q;iS1pRY&940+uh;G$;owf zb#|Ss@)Q1ewen!UZbBczSUy7J3Kny4CLyiJ}^eVQDtgP#+n=$zaE2~e)p|LD4M!ts+9|Atzz`&r( z=ku{|rO%yzt6@pVBqN1tP4#|vnyWadH~e%mx^J@q|0cNGAKKR>eR3E`7Q1iXxwG34 zK#?pG4f^Jyv)w{rx@9kP@7;T~&X^D%^Oq+@kYMLDe-g{iDNXfwI{_me_B>y8gKpM9 zMlKGH-m-VcUW4e+^)A+?kW1%)_X^UPaLgon%}N`KR5I>NU1{F?S(7D`NT|01m#{>| zaE*Pn*494?B?f|3cTE!BuK!&2r)-UokdVfnJ?|!H>6o6LZjx75?~CIzGnNvuu?TQ8 z0y-J+eWdv8OB*vCOI*@>b-+~6mBY7U*CB4us&YZWZ+2P}a4#8SxA_3d>vAk(Nv_}H zPf7y53p8_H7?VT627m=!%?@K>0iHj5W!~|F6n&tN&eKp=&vo5DT_UE8dbH`_^k;^V zpcok+e`Zb%u3J`&u}_{x0lo9**SMxVcP|(AzpnM2x97?NJHRV(bGCg8z`JL|t=F=8QoZjP=S& z_nV$oYUgqpMAnnL=@!@2h{NG<7!1bA*@nb{F}vIVUM;-&r03&3#`ySn3^mVo>4L<0 z-+eoD6tEeq9-07ktdiJ8{4VHoipc)b-q*>xCk8(YOSxl|DGSr8cCoXxTy(Lb1P;sZ z=Rl+e4YSz*5$9=mF^Y&VaCdBFavii<7H|k4_cl;FJ4Y8qIj+M%5Q;g?J`n{D<7 z;qk)GD=G4;ZiiSKK-~?D96ac@*CMBQ*{h<&u~t%$=CJv8cbhDpT~1B6=CNV}piG2J z_ka21%cF!Ae7m-tR*^q37OvQlqT%i#4k4HSld2`6QVg2~a+NLMITM99b$7Ox@Zhe= zf`enAai_0ul&#>=X(yy;%C8U{frtn`5EKuj=5!}-ZodKjqa0c>oKEO^yYPKTC2)KSrCAPI(>(5#jrqvIyLkNjo+q!S&6)Sn}!WQ{R5l z_Y%sU(Fq9&R2)x@uPZ*MExot_(=h@|jjoLmaH&62S2Y;C3iCcAuxL|+DV<*q4pcFd&3&CMB{4zrO6?0u9 zo6<i*7W*@KvvG3yrPgM%^s6u=gN*xWX;;sMkbH~OH>XLJ);FAL zHt%(Eo|YSpQl2>{7ny<_%v49(J`pA@)r;(X#lC-E`H&k@`DQ%!Vw=-=*{Xdt~ z(gN(ql}4b5SAXA7nqHqx*ro!W3UZz9H^C{ZHXzTvoAp>GGgVd0^>$wqZG2d#8PF|< z%kQQw2QlHYP9*kad2DYi7x_)_tsFxSM4nb1l6sfCw|n2ggRRiwcIWXr0h=TXvpRiJ zV~*}Sk-VCD7So>IG7u;^kf3)LIu8vD#E<@6JNBu_v#Bb9*(66bn_dTKX}t*8j2J@7 zft6*r&4Z3G$9;04Cg=TKy;{&x;F(|LIA6W|>bb8tmuJ6SQO>6hgs8^lxR(DbDDhePUKaHC6VIW}Ma0|Z4NMb_g1O2j; zn{2TouOLI8EUYYyqO2w2HQ8mWnkEnFEL)V~Fzq)q^NoES`ymN*vtnjqd*LH49w~t_ z7XQB8^e+VbY4vXeoJY>)gP3jS4F`f5dNPo1gjZyxX!#^l!f^Z(J0yzVz8z&^AXwt? zrc)=KF*um2ImeTDi}89uIy7o%L#@tlFO;qk+o$DD$z%W!TRHjay4d8y1!0vw9iQvs zo}%R>rxZY#MOzy6Yw@1=+Rn$lcQmgC7r$RO6$$!A))_}#W zTit}Y?n&JIR?xL${ur_h%6T6J+gf@2pe&4SUY&N(quV{yA+cl%9) z!kqs=n7@bXTk<_;BPhEsMqZ1j5tUn+$s=)OaR3~*ewJn{TK|}DMPI#JR}>FM|K>(% ze!kEz;9%*CiAr;bELIg8%kpb@So7u*no;>OtsZ1-BeaDLZmN(}B;yo&3>n~b8iruI z!WmNozy#zl;K3i9IJiTfJ5jVrA5Fh8yja6c$W~97Bp3PYHVHUPHu2*85|Ms7)C5V$ zVx3g(9;UG4zd23k%jDhQFBP*Hv9!b?)9dgT;Olz7^PO95xO@>A?+rWsdMp0l@ZOA| z{uc*uuY|t;h4t=dk}(W!n>Fzr$lqB}ueVaUW9)P1RprcR+E0@7^o3Yr#-fpZMVc|+ zs@g0p{Vxq?-s*}mE^L)9EsQm*sZBLsPx3TnKIHPDL=6}-0U?t`i%l|6Uo}Ode=8sbsBf2zyxJN4Sm{*&2*90gn z7)r~;;hI(lpJ)GqXlG>iB&3HgXbk>_A1&wJNHrhtf7#M?z%*Fw9e-Sw&pk73$7|Mh z>9qwHXX;(67Hd<$f^e94=BS9h$Tj^WS`l{Sqkk1MZW`MctE3$KD}i54a*yriMG5Pu z*2>jy_UdKVni+~Ji*t~{j{PVTZ^K_BiUNF?*9)>H@@(>`{dfiq0BXDY)WR*klY)mlitgpx7#|X zmoh6YcPilqt1tTT+LPcoz`^1svfr$Geuw(rIPAZXyI-b)ab6JiJA)_QD|0B+l!z_* zQnNWJOlbU5`W~yfl*8T?cFyNSL@;~u$@#Avk=wY}!8HyTy@XdahY#P2zMC*pmfdr< zK)jL}y7Dt=jKuZ3F1McHrMa0Q8y3BYtrvc4R>s3O2qiFp@PYhi>bDGCa7wJ!g7b>m zQ~+T#2>p(vw@d>ReLg_tT)x6c*s=e`n`9@Km8ZOo6G=&Y?0!qVzv0mOji&{fwqPN_ zsv2H&&s^ph3D_F!EE0Ks^VU!*eseG}QHU{E=lY$;vYX>3WkU>>+T9Slq_ORN$C$+_ zfg9r;024D;r}7)JNA1A|^4!n|4F{-ir3WRxm}maN&S)IafHFs2pP;p~Ule;TH(HZ9 zqNAmi+jjg&8{5cOw9?tt+1_}h-Xzo_%P;|9Yb48>*FPVii@6sDaWps@K_ibSr5 zyvD4<%1)HC*R={^kWkqd?6X9nW-Ka-c!mRbRzcMw%ZY2*kZZ>b>jLc zhv1K%KzGlf4-0Iq0z&J>8^p!#!q7;bH3mepU<*YJ!Q3DRO zgQJk~2(o2^?}qN@cS-CY{_xXd0URL5CBF$bmay-!p<|Yi~CS>UUBd6*{yIZ~l3JnY^_4 z{-Lnsd9=__{j_AQec9G>I4tP{hU-TwP_|CaQ39>!s5o>E z$ee@9ij>O_dC=5cb9`T~Jkeg6a_wVCB;wR9#fPDfUV;gn`RHrQeM%ZGot$ZejA)37 zib63<(MJs8_=zR0Jm(Fj===1iDHQ8Mb|2o)1BJnhOQsG|q(+ip?)F-r$A?IrZ_qC$ z1hn%(v`p~+T5_wb?{AZ821PN0>2?-SP%SK6K=^g{QW4|-Ie>%51M?vbYSshqdW zcvVrehsz>QQS98VX9As3w)XZ>g6bfEK#8D!vis0}P6joubmJ>F0#L9=42(N|BbK{nSS;c+a1)H{tE&MD-<3(s!kslYY%d(og>aJM7~r8 z^6_BNIsWy9Vdg`JQlE?d=|uBLZ=_^txl~$G0zDFYus{-*gP@k*Z?>R}+~lAF;z0a! zzkYp}>KuovBbm0|XaBXYw^!-P73})@tC-eE06n3$t93z}(owGQbh0598ZXlKrau|~ z1z2QRZ&3FJKb))4Nlz(2gwzk&L(D3pqFPZr*@kIO1l9nLP&xhn1 zXgqgpQ2WttST_N|WWHU|o3gc8vfh%lJ?Jf+@2p+`JXO+VQ=d?VLJ4zmrNaW~Wcwk1 z`plXCi2xtPR^vMly;!YyvZTa~bLdv6<*6Tpg8nGb=c}XSUamVAYj=^`pP_FL=+8H< z038+(rv!0FS2JzX=vQq`O>Z%&`Rl*Z!mX^VOb0Z>j--~C-((8BSnEv?)NN&%XgtRN zGEpZ#IPCoq^+!F6Jp38SgbL)HlTg*6uD|&nWUc!Qh%s^Hp<|?4}c0R~wJxUpkIB zt$W~NC{?U*qy*Nlc8jya!`3POnBZy|LKZtT4VXyZK6+GKs?IHKS~6S|Z8NlDYn#?Coyod(_ zWMf$w!YpuTAMBjC!k1MERpIMSxeP}FUNG(YnCrbBoE-IXbg4j%`OrJG-lxOo?=;bK zN!b9oKNFMvW{~xqy^X>UT%X~j!9D_vlaamUssdq;kyS}V zGf?VFg&(Z9!Cnjv-Orc^&Ec&ZLrc6}mr8q@T7DX-=gGbSZ2;Rb6-Hrb3F*(z=@H>{ zDp`sF?*Dr*_{pue2V`5okL`>%(;*4NqGl2lrvvmQEyFjKv@?!cGwpz{D8y? zb1vr8xZQDop(K4^j;`9aM|lp@QbIwW;CNJ;UPKA#Qv5F!lw@O_L=ZEa937Mh^q|GL z!vW43^&HBT6AuDz1q1{D$BNp#TVWtgII_gMr(}kc7Sp2Q&$T_71p9lS4Hi3~9ABvn zWJE@l4w`{U%dWmg>gnX@bioMfADKq7_I~qZQP|*Q^Vx4>uAIN z`02&PvWnFZLSYHUhMuz1O&ioBB)%D)5&_k+VqJDEi}0rF23KD7bd3*R1NzV&T39s) z>lNyFz0PDgJH@v?#+LrQ^xYW$E^P-}nlJ3pX7xBNuDN|)fZHGVwqiIAhFupm5Jv2KfCxXI= zdm$K0LHK)(`rMKIz@t%ecZyX`YiAv_GKKPpQUv?T+#T_6tjB!rc#vd^HE|x%n!gr z9nO5EO5*RVndE?yyW)grdNMr_Uzn-Bl+8qzGZ2rXL!tEjR;9j$9(A}}@%enj%a z4F#p!uyj)7kVJv}@TR!$Yr}PmajCWaBWrX`ne0cFUMYY~Z}7m<2%CsM6(*gF~CQ ze>!MOB9VG%tZEGZUDO^m%=L%H$b*_)r6Se3x&BK~tp=Gv=uwdxvcV4oDA6W!yIgn^d zml+r)&yniS^RZdxQXS@+>1}Mr-eBj9XgP~Rr|(?O!&r2XYKltqhk5^ieKimQdd!`;Y~xs+~J#-3^Qs;Azu&ayf1!#0F{YTk59zg1(l402AjCA zho7HuautZR1sjENom2 z4HN5kAftgblGf^0Rs{HTfS$_3SW9P=T@Ua4Wd=m5>@LLpJZg5ykqA6?(%V46M8<*UV=;8m0GSa@8$%iB zO-+OFlA=NFue+PZgK$w^uMzjdcE=nyrQN==TE!n9Ky<$Jp5a%um)_YUxrK##wQJN- z4?eBUa_(kB=x1CKzjT4z^f>9W&L-=+jrC>sTb77jG%3%;u4f3GM^6pXz6+IK{M_m% zYXqT~_Jr|de z-Y$#u@9BP3BVdUAsj{~pGOJU3G;SX`P9lT_nh~W6u63?3dupBf`+E=wI9r0dqww@* z`_wv09b#kiCwNeVAP5q-m_K3g7~baV`gBH=@QFaPp}8@x5Y1hP$=wgDs0G}eB8eLJTa7w=7J)z3T1&OsKTf-fiQV7M5p$7uOL zwYI-M&hXSK*nSbl>Q?67zCP%bdvy6V)B%A)*IFmf@x8bA*ejrX0rA0R;Di{GfOq$H z^zo_qDG5W%OeqJ=$@;qCO3&6O{rMYe;yJb=r`-4_hnEVk94UB%+jYnbap(hc{TcA; z_h*aCs0SdR1jbkoLp-MZ?ifGqB&2UNs=(BMB5(5i9aRf5f6Oj@cyN$U6{vzx z`0!j9$}uG~Emt?81wLSN#WGKV?(yXnP+YxIXjbdwlMxu;B~%18c(TS0@gxDKO`RSA zD!jf9b7f!=C7yN8FM(R($51gK68>wmU8>M+BJx&efp7x56-Lv{ zQhUp=67hV4;xkY1SAlZ^=Q{wK18CF0gm&mJEEIGf4zs;8uH`s3hpCl=Txy$hXWWT% ze~o)*{R;TnbIyECkZ65tVAWe-g!>UlXBd5QO#u(&hUOf;OWpm0I=Z^iKhJPLwHb<@ z!ir@@98HJ(aSOu`X}jKJk58<4G!A42;Vulu*?lTv33t9Aoa^*yTM-DAfpD^}uCAum zy+F%H8gQUVz=u?jp0?!)+`+^F=@^y^VGB1~mfq8NmuT<}$G>BicD(WNr1{LfjnJhS zP-_;10p&mN%dfVvn5Pi1@Ia67?p>K;kCz6Y{%YQ5OnP+vYzc^?Vn5T=>tC)5#?6SI z&UB}#_jH85Zn6?$W_r3JDNTAk&)kr`!>k^tE_cl4T*}2@V%MGR2TmC9%&W?vT1)Ja z)I#CqWBIbJKa+taPL6n7jQkac{BgsdKYyN4;E`({1J#ei-`Re1dY=(&32QxG+)d?riqBv8o7daKxj1$v*(ajNUC(hDVhmrzK~ zHwmFX@EDR_*21LKSC7^2j30(`U1X8O?WNekB5b-lM2F;Iu+5xDfiB~M+#tcuyvlu6 zFCGaH8%?XfECzCXixOikt=H^Vip**QTxT>+!FdBgIO^TI1_-j2YXO)e@qJEScb7-6 z1YZeWUcT%76Kb*4bN~j=AjA4OPEcoNWR#Yb%^o)_c<7^GTOg{dr-&p)Ja|A6Z|md~ zV-<{iQNsv*-vb5m3hrCUU4jaf((cxlI`}76@1h8 z?=RZ&o=SDEtgm}8K7k;kyQgPbxdSXC*g(kAIt3-ZO92A}+?DhUZVTuMC68gH1o;-d z5?4}DQ{xAHXm)mqk}S|@Su1<6Wy7 zUvQmB&cN;3MTf8o0W-h`5kF#?4vZz)Pr#o;QPs}#4T6a3&V8z4_bS*DF6Ked+5f z8>w;A9J(=GQ`RWJI^_H4(yrg1cJnp(O&2=%PgiGk3=|XA{p`?Tg3r}%km1|7G4g6L zzMY+?+@t8!46_8!Rg#|@8iO%w8<}o=np(8oQv1e6iY-N5nVnY^s#@b^rh?ar@4WR_ z=W7XHQf`HqlkHMj?X^%c2uJ^Cjz6-ZjZ8G&m|S(8M0=29D{`Q+GxEh3l=oXvxC{kn z55EN@a>5g?o@UREDJKUWf6v;V*u(M9|Mvbr3?g1sQ|vf)uLD^6!(>gBe?9>>>#sjq zf8RT4-*%CE#hPWc_y!QQ>z5EdbEa`>6_hhgYy8klhFuaR!s6n3T3R4BdxV)eAtq)W zth!Q%Ltd{^*&a1qU0DC|;(mwUPFU)%Yo3g~QU|J6$__?nkAP4cA1^P;)}z6l2e|n* zHjs+H*qrSv(pQf!=>3*Gb$1Lt#zuU{UjOic0E`?$xmd0fxw@6}2M*Op12JjxG=1M` z7zhaiI392{(RTA;{k!hXa{j&@IP&UPYLi^-3;w!-$w~>h2)vF-AX18mh=7m$R$q7h>^N@AQE7`_ z74bkmH8Znu?KPjG=BuPL9e**Uu-Kd9t@jdYLpK{SJ9D%y*H|F)=N5P~F=! ze$M*G2v6cesP8)_pAi`#NUa?N5iznC6fI=yWMgG!F7}R&!iNB0eD;njfmun5y#tg= z{@@+LE0}8EviIjsP`5C|LtBV1h$7DSseT2v>AxI0t-hZjmVT>D*b&eHWt@Sq$PV~1 zdrVgs_Vtd>wDwp0YWBf31HzT;?6ZgV#q|oW$;n40BtTO$Gdnv5B8Anq>0V_P@j*Ls zTM$X26>JB#s?u$ z$PO=OAscahE9Zm<5C;G4m`^=_!<|*Ls~Gy7Hdm*Rt}~o_$#}r|gWbx0zrF4` z@Vd9;o)b8~aonS(q4!U+fo>4(b_ zH^u->2Kflz->moPjQm7P1BSt3V^g|1<>vkTUPFPppvHxAu127NN5n zI3=(|VGrjiZ{hLX0^$z6@cm#w$V)#Bk_XUMuG+-XckNYlc)u!RZ=7iZW>V@$paZ`SzgKqaldFt z=~ieF2^>2&7ghFj)2{|1^{&BG`8#;lv!m>6Ztu3RGAV|=u7k0Lw-@X}t7pHbe4fa`<=l)lRHx>~+YuPNsEo;?^ za5?=a72L7XY*_)djgt@QA^%Re4*s4lngW4l<{dwFZ zFLLHdY(#irfXnpvgybZy4qul7IUnS+F^c;NDQ#U1+}yrVTA`XgLdh~URmTF@_DiEK z{+Pyx#LUG>istuP5I+BzDdeR+~i_++cRUi$0=0C1sq20&1y60lnp!Di`6pq60&0fU4^8!&?jH#VbANH$f$X<5q z(A>M!oEz@9^r_>?lhO3__J&5HlHtGnc18Dt?uf3)Fd zps7U5$B4@fRLxvOBJcTCk%HwMswP-Z%y5dNm?Hi4JvbDLmV=fb5~FqB z`>dm9_8h!%`?gb;=TjPOxHYodZ@IU-CKO7O!_KdISzgMsvud1o9 zqqXY_t7^>dH#gPi!zEGYILDsOv|m^mw@yaKy>pDIuV?2(=x0MlcW%X! zTCU1ZVqj6k404+)G!|cbriVml%8sHp+{`qUWHl;Sf*zv|bn;@&@+ceZ<`dUM)g%nP zwCjr>_-B0F)@t&CUq&8JVk3!6pGsH`*+KdnmCa{tRYLm z;!ehcohz*I)4%$25A;7hPFg555gSNRPWD4xyAj@^7+*+3@z@J?i_gy52OmkJQ zbslTpu&9Izs6!)c%XuCF4*n+~QRYxLG%+Sj+4IR)7Gsl&6ct-+k8`oD_Ef+ zIdFPFa?ZBjL6jT)F}G_p6CYA9m_i6Je#`NBhR8%GQ=%bWw7Qg)xds~h;f~z6S3G~;8Rztq((SP8J2T%7iNKzw$)%Wy^ZKAIIdkjT^7 zRh!zFAsht@C81<}=pNn6=%X!duf%Xo**BPPUL)Pamwr23q<*`bjyk=y#%b8>I{UJh zUquo3;qx}E>nz<{ihW(*oypfVQ!u0hhs{hBW5O94VWO*Hh0iaD&j^Y`+clpAPF15W zFhH1KLEa8FUG_!uexspC;}5R!l2%`?3;nG-VR4O&f1+{aji) zIejl%F0jAvv*&zngm#m|1R*{@GqKIWv~!JsBRMuWt6ps1!ZW|Mm0)#4JqPJB0knQ#yT4O4-@637L(9je?KECZAfocD` zWsf8}m)t3XWrC+=G0jT%B590`$(Qvs?j@IJHHk<+&%{y?!k^>N9C^Ru@Y~f{!p6*dIb>E}yGB|mTrA^p?8!dd;z`&w@ZpAHU$!mR|mgrIG>ErfzIG?d< zsJX8dE25XecDK1iPdkbG;>Guq6T@(Dk6F9kh=iLMi^o6{tiFbguIBhtw+&U*woACY z3xrYTms-M++i%1drI(c=PUM6iN-3OfR;0EHrfPCaS+q!#Y=&fYE{#uiq^R)I=Em3^ zLkA$drr2WCo}H538SD&aSL+;pFw`~*UkCJ3!F{#7x(sC%MD@x~BPBDV&CQLbeAsQ=5YnSVaCX%Boyt?zex4l%4aI5B7TUq#Ni3L{C{m=`ObNqHBGILG8 zJ7I)DSI+GE5_d*MUWwcjJF|CMWzH^fUTIgK!3;B#H$~jY1wCZz-e9T_d{Ov z8_#Ux{4=&rs&0mmRc@+#0kW9V4umNhai0xy zH0_0hbZY1LtdGSbr;>h@FgPCx?U*Sl(jfAxDkXI-Pq!pL6R*>8W(c?v8bzr{GUIM9 z8?Xt=RBGFsTo7*KtA1H$m+r+U;)COrnBfxm73vPlzyioyen#!8{GjT(V|BDdr%CckII6pxj#*di2FO5qj2%| zA5X)55o8b0PN0>kUd6d%(Ygh*s}EKRLt{ zi6lLsqi9*S>*xD?VtuH$YXj8xZS zalG|;%&>rzzn51W=*S=b9c=y`rw0ZGM()9HZTb56(0=^;)oyMt|J$owx8d9o)l(OI z*XL}@U(MY{cc3aQv&Bu>y8r+H literal 0 HcmV?d00001 diff --git a/doc/software_pll.md b/doc/software_pll.md new file mode 100644 index 0000000..6025089 --- /dev/null +++ b/doc/software_pll.md @@ -0,0 +1,77 @@ +# SoftwarePLL + +Software PLL for synchronisation between ticks and timestamps. + +## Introduction + +Many sensor devices, e.g. lidar devices, provide sensor data with timestamps. These timestamps can be synchronized with +the current system time by additional hardware, e.g. by GPS. But without specialized hardware, sensor timestamps and +system time is normally unsynchronized. Sensor timestamps are often quite accurate, but have a different time base and +a bias to the system time or to other sensor clocks. This difference is estimated and compensated by this Software PLL. + +To compensate the different time base and bias between sensor and system clock, the system time when receiving sensor data +is gathered together with the sensor timestamp. While the system time is often measured in seconds resp. nanoseconds, +the sensor timestamp is normally received in clock ticks. SoftwarePLL estimates the correlation between system time in +seconds/nanoseconds and sensor ticks, and computes a corrected time from ticks. This way you know at which time stamp the data +have been measured by your sensor. + +SoftwarePLL is a generic module and independant from specific sensor types. It just uses the system timestamps and ticks, +estimates their correlation and predicts the time from sensor ticks. See [timing.md](timing.md) for an application +example using SICK laser scanner https://github.com/SICKAG/sick_scan. + +## How Software PLL works + +SoftwarePLL computes a linear regression between ticks and system timestamps. The system time is measured immediately +after receiving new sensor data, while sensor ticks represent the sensor clock at the time of measurement. Thus we have +three different times for each measurement: + +- The time when the system receives the sensor data (receive time t_rec), measured in seconds resp. nanoseconds. + +- The sensor ticks (or just ticks) at the time of the measurement. These ticks are contained in the sensor data and +received later by the system. + +- The time of the measurement (measurement time t_mea). We don't know this time yet, but we estimate it from both the ticks +and their receive time t_rec using the SoftwarePLL. + +During initialization, ticks and system timestamps are stored in fifo buffer (first-in, first-out). After initialization, +typically after N=7 measurements, a regression line is computed, i.e. the slope `m` (gradient) of a function +`f(ticks) = m * ticks + c` is estimated from ticks `x(i)` and timestamps `y(i)` by a linear regression +`m = (N * sum(x(i) * y(i)) - sum(x(i)) * sum(y(i))) / (N * sum(x(i) * x(i)) - sum(x(i))*sum(x(i)))` with `0 <= i < N` and +unbiased values `x(i) = tick(i) - tick(0)`, `y(i) = t_rec(i) - t_rec(0)`. + +![pll_regression.png](pll_regression.png) + +The estimated system time `t_esti(i)` of a measurement can be computed from its sensor tick by `t_esti(i) = m * (ticks(i) - ticks(0)) + t_rec(0)`. +If the difference between estimated times `t_esti(i)` and the measured system timestamps `t_rec(i)` is small (typically +less than 100 milliseconds), the estimation can be considered to be valid. With a valid estimation of `m`, we can +get a corrected timestamp for new measurements by applying function `SoftwarePLL::GetCorrectedTimeStamp`, which returns +the estimated system time of a measurement `t_esti = m * (ticks - ticks(0)) + t_rec(0)`. + +If the estimation is not valid (i.e. the difference between estimated times and measured system timestamps in the buffer is +significant), we can't estimate system timestamps from sensor ticks. If this happens more than a given number of times +after initialization (typically 20 times), the fifo is reset and a new initialization is done. + +## Source code example + +Use the following code snippet as an example: + +``` +#include "softwarePLL.h" + +// Create an instance of SoftwarePLL +SoftwarePLL& software_pll = SoftwarePLL::Instance("Sensor1"); + +// Get system time t_rec in seconds and nanoseconds when receiving sensor data +ros::Time t_rec = ros::Time::now(); +uint32_t sec = t_rec.nsec; +uint32_t nanosec = t_rec.nsec; + +// Get sensor ticks from sensor data +uint32_t ticks = scanner_msg.ticks; + +// Update SoftwarePLL +software_pll.UpdatePLL(sec, nanosec, ticks); + +// Get corrected timestamp (time of measurement from ticks) +software_pll.GetCorrectedTimeStamp(sec, nanosec, ticks); +``` diff --git a/doc/timing.md b/doc/timing.md new file mode 100644 index 0000000..4986e4a --- /dev/null +++ b/doc/timing.md @@ -0,0 +1,19 @@ +## Time synchronization + +Many laser scanner applications require accurate timing down to the level of individual points, so time synchronization is implemented in this ROS driver. + The scanner has an internal time base of microseconds since system startup. Against this "tick" time base all time stamps are made in the scanner. + When sending messages from the scanner, two time stamps are added: +1. scan generation ticks--> timestamp at the time of the first shot +2. scan transmission ticks--> time stamp for the transmission of the data + +When data packets are received, they are timestamped by the driver against the systemtime in ros::time format. See following Fig. + +![timing_syn.png](timing_sync.png) +The relationship between system time and ticks is then derived from the time stamps and kept synchronous.The time required for the transmission of data over the network is assumed to be short and constant and is therefore neglected. +The function of the algorithms is shown in the following Fig. +![sequence_time_pll.png](sequence_time_pll.png) + +# Data buffering in MRS 1xxx + +Due to their construction the MRS 1xxx scanners generate different layers at the same time which are output sequentially by the scanner firmware. In order to ensure that only point cloud messages that follow one another in time are sent, buffering can be activated in the driver. +![scannbuffering.png](scannbuffering.png) diff --git a/doc/timing_sync.png b/doc/timing_sync.png new file mode 100644 index 0000000000000000000000000000000000000000..41e22e4ed0534b377a09d0e8d8fcae92b38f4702 GIT binary patch literal 25225 zcmdqIXH-+$_wOC_APQ>iCfN|@|EV>DyeC=WK?OjxVe|EiI z221|4_vXyDg|@fglc)dByya)VwedjaO;8{9zX=L1E7Z;Y3CT*(46MdNAC`Vj@#>JN6`7MckpEZY4iyhK_S^vGDw^Y?g4uiu zv(`hWVBqgR?Tr1#?1+`iya?hQ{HKeqxG#*=YH43|9+Y1jbuOuuE3Rp7ii28A=Ti4U zbJe)R?OM~LqoZRPH^VbzVh4t3W(aqd`lGMARi6Avn3V|L`a4*%x?Y2A*Gt04uRv;I z4Ao>3cbw8grX61P={ne@etdV$G3{e2l%NVo>eI9lojYBW-mr4urQaT42EVdGl z6o|#Sk<}2h0z69t!IgeUsDju0P3&uSBBXm00;pH1U%lHcTdNA3N}+MmXF z7UnGQ*mv=Z5O=}^u1O6uAM+iua%+n?(dyl?P|sXYz^_j_xIK%q+Z@aE^=X)@ESz=@ zh)nYvb!_#0@7Jg1SsmqeH0}b{4__f9=AbTyv$$NfuON{8-9N$Ff4PWev$Z0;vya+0 z5Y*dnAtfulI<8lP;cU5i^?df*%82DmpX~db40cg6Bi1xE&J<3+7N_>+T+;+>2c9NrFoNe?DhU@=T%{MdW%_f78yWXg#Qz3NVB|b=a-xCw%=y_? zxbw3e#e%5z4rGb^%*&Aqd(n8a5A?b<)G(-7aXT&>}>QMzTX=iimAiple?pLRqCLWBv* z@6SQZS&8rQmY5CdD>|w2DOz7mYsvS{z>7lPQ_!XfKUXV#!m<-x4t`!f_X5_u1FX%+ ze-)gXqCF)ghcV-?yTaJ(oCe zv=c7;DT}HO1>2y^Syr;xocndvcx&Og5Fg7)M=8Pd8(eb*i1~729DcAYsEu%WuTKbD zx=?6|SyWjUlhu!pCAcHV)#y?-6l;?@MIsnuKu&OjF)tE@(*k<;5O&NbcuYN6F?fF2 zXMCa}@>#I+v+?@HW)X68hzR+|hs(S`O|mKOTDyB^({hI#&j>fvmUiG%*uJ}ITI6Ww zJntXxWlS~$ZD%qnZL`^K^^EFSt1n?%E2m+W_@G+IYUn`hMccNT4c3gViFy{*BA`cY zDM(H;-v!rsi~L|uzpmdCkgFDIGeTF^b542!f_vqog4!&`{Z8|;;XJq7o%$CtEr z_A`TXLU^E+2e93q6PzWp2FS~-Ey+plpN-#b;W!(*G`bOIrv$qzmv1RIpmJjnFTZj# zd!r+%t=>e-eA$T>-%(I)OLw$HR+_qHD6U54Pq3qe{g>eUg|AJ?kyB4^ZT zn;(5!G#n7MK35d)>as`rF2tGk&B)6@mXtcER|qpTVlyYpsH(q5Le)as+D%kUd#YQ@ z;5`n?o#22;Fg31yl0Q@3$l%rfcsJyPJs%TwVrWN5ddPEKz)2R442#fNuaC-YdUR{9 zt+aUgMEE2>0I#Sq8(0~t-`gU^MI(KUPTBQk>7_*Z%Q$|yo>}KL`!URK&*;m^jnvEb zQQ*-m!Pj@F7R?R(#W2{4&cLLUUvG5wFSQuApVFAIfW#~{*niiJv+L^{ zIvjHjnr5o3M}a$19Xkd>jJI9EoL*hTUu*%#@0+&M90WPt?KgI>7f!lV@ZDe4oU^wy z=Nl@2PZ@jXU&P7IS1sqi$C=jp_6>8Y$*VC>F79g_+7feqS7a81?2l0ECu*$viFY)$ z4P@1;Q$O+c;_OI+`lN&VT&*pQ@hz^c^gHfRHiHcajwhn4+JeBLl&lJRC|Z%_oMxZmt<>EWzNA)Esbu9CQXX`^ zT>kL%mEy#ilQcJpzlmPe5*FQk9a8~PW0+JJscjI)K&0T5s+vNb1&Re-8lR4gtawlg z*dKnxl%@_t|2ADUf7!0EB^(_KtTY(`=Fx8l2w7_8ZtBLelqEJQ(S4w0k(J2`>AR%o zJNi&!(6!Ig??#QQr<{@WEQ9)fbRLYt3r0@H9$C(BrhMr_##lBci zOMk+H%7*C>uA9%^s>XGcpFp}He1i(b%Qf4rJbaYbfuj7;xvjh?u!7DrFGY z5V8^J4kz6v2yx8#s^)Xa3?0iW%SCIB8<(^Ds(3`XBK}DUkr+S2Z||aw?HuJxmR4{> zs=DR<&z`CQ_gTTQ_7Cky@q}I|EfKs!-Dx%A|2o!eaJiy8cn~*r7MQ?XPzX(9H9htz zJ@{OrOgwWP)!2#4At|?LSkx_f9fD@s#XP^QH}l=t@k3?M%etf?m)SfN}Z3K6(>6O2~7(8lp7&msOiqm4mVMtYco$hoBEo7sJC12?!O$%X5 zTET_jSEXsYoeyCL&FycRs0C5wkV`G%?hUzdp!V$Sz+Z25IX2wZ z$4-1zhG-N6eQIcdb@w4|L98~BrM#=Q?(-oZLnu%Zi@jH<@yG>A7&XzV(3Pvj!imusUkNe#dhh0QKcIqMjPE?lphD6kDRqv`UiCrLM+_w%KWrlu)gm=sV^Pjx0 zA#dh=U;M^`VHz-1HyZP1h665cshT1@(PXS4GTGb5EPGBabtLE73Q%|VTMt8Q1 z@7|5OgXbAd9oKyo45^vr_f-+*l0H4xFzcBRULIk|l=5;ZICMaTRguQ~0VZ}GroR7V z+s4@sJ$m7lJ4``_qBo-wqhuv;vS7^yKVbH_A$cBh>^$T0X}vFXHCcZlH!tS}eWhbH zfCbTMlGdH7NdmL24pXA$UULTeH_iuLHOZa>LYV{I?*4Or3A$)y`P;eOcQ+r#%h`gi zNIT`5#f-wQw&vgmEo*sMzbN*-WX!pDIpVH3-r3`g`t|{)m#IeF=tHnZUIsL=?_yEE z+}IQN@IZGX;*w^?WvN0OX>a#3;%Th?=LV4_)L$Cdmc4#E&{>rw3Oo8I+^nl~8v zx--!;A~d}ekq2ie)VW9bK2^ojt=+F4waagwl!j7uNa2L#53`Qegn?MR+RSbETyWo@ z^QmP2<@J$j4R2`AX-A5|=xT?=44z}Ts#JpVg!cFs&Nr9$lvS>wTBFS$`pL3N$46bm zCRA?`W{#Y(=*018Vy8Z^%Z3(ru!3%H_ZdW2(hx@`dzm_ zO~vuouV`yP(K(YFrBhBDyW@KsQh!}36i8P!v{pWT6uWzVwaB>h-9!>lT!D}`RLfuTeV)1 ze4+g0N`{?g*95j%&Udf=J&vc7W;Xw299J=JIhT5)2>yQ1zllr!%2x`nGIr#@W8;p> z@bXP^Dkb&6(TA7@@uZAoMO4 zbYghtuS@|$H_*vuqgh08eek}lN^D4MOQCs1kVFKw`o3R05>t!Z3Ke&g9we!HPamSyZ>D+$CY05dO64YMnkOqsNcKgA|715>BGV=3u@73esU~q z--9jIOm}pgbkFL$VSjACg08vt0&~CIrEa8H`cCe%b9^K*rza^_yN{*^JCmK|T9S)4 zzwI7vqq?N_p(+B34aLaID!R{v7fy2RKBX|dd@stR`F*sWd!!{fFXylD9T=}}*4)Wt zTiJWy$qfmInUn=d#g-~L`)AauWcF7&d5N8j@WFdxo@xq90giS_=OAK3m0pDRvO5KP9_+Fa)8mEQfp=UsKuAc( zQ@niZ`^(CBl|R#}9~4Ij^rtc;dq|Qz8|lvVkc3&-Nj;xGD@0n>m8Vj%TLFB^TM>nt zbmAUm%*3g|0G~b$z0tIV>9TjE-REKp&c-^|hL2@2Xu5CCTK&vn+u6KZe#1Wgr1n%a z|F)6V+58Avdqz2jv~e9}JTu9;L}{eO+^QpC4M(w=1tVC)>@HR1MoZ0~4J}o{Kz868 zXesg-mMZ`X7Rcl60H%qPWKv1t>;UFke}2=B#$(hx*k0o((65#!FP zNl&;z)B=wdh`UViFhyjNI({2#mb3`3c0xG%x71W|4Q_(8hjx#&WEwsD3udj5qU@ z^_J~zB3nY5N>Nv{-<;a)tP<~H9R5+~QH@U608^}Ds-!5p(q-B*~! zP!`)*Ym#6-PVC6OFDX~G?^S$3b|xb?7WwpAal9*TY!&byyDIBsiI&v&aA{}>b?$bp znz<9w6~cIqB)y)|Vdq(n3+;%#1}xwSse`IOmcbSV=C+OSp;lv>6S6XD^*Zv^QcwuRbhIZge@?PIjg%ms)GCa z)$Zt27BMzH`~d=06&Yej6%QB*)Ma0nxQat&om+iy!GFP$Qewz$HGhOW1jt8}nqz>@ zqkW}+Q|IFHSI+Bl!{n(#N=zy12?*4K^?+`RKu+l))`IzGrW)7AN&c14ip;L$0ml^} z`3g@#DIzMr!2#8YCo;4HI5%(C4%E>ckqv#t#(pw}%ScJRn-jiP(WOS9gl6Uz0;6Z^ zmGib9wQ}XZNl-mu|5M@VcIU2@;zT||X&2~QLE3obRk0TBTfgNk*@7oKQB8H!4Trs; zH)cbWjYmqM&msV6Yoirr2TE5SaM3QYSuFki*GPjifa(Hzqq}Lc3*>Yd4X9$AyM&yo z-s8J2ZRu_6X7`T)ii~#J={Gw;+V~}_{h+E>|Dz(No#sR?KP|Scml^HZmdLczCjFqG zZ;D$oJO4fT)It7xf#A^_|CecPzOIanC5F>?b@4HBbdzYK_ks$pX3P_OjlO;X zB(be+*IE+mNljViE?pB~&^oZ9d;GsOM*&y{ryl}E{}W!+lw@V z<*mQ7<$L`px&?Z(Q)kx@*E^u)&A~0rndN76mM&xTT!K{pPEek!@E~s@nz^o>YQ0RV zr}zNFE5M%sw5BXmm*WTuW)$S?K((VEOYzgVq1SwS-#25In>N3H?^OqB7ZGbMeH3uV zP-^jMUcZVlCwI0Kw)ijUNCb+^8-qcoAS>_>Z?;r3v3-m16QDeaD{Dr3tL7cZqtn?C(d^Pi;5H*fnyX#ZoP zO4a-U#EgJ8Cf_&CLjf{8ubwg(0D7Z1==)r{)V~=iTGtN(^@k>IhGZ*7=X`nL&?^8^ z%^5ULCfHDDD`6s_jZADuUhEYsdk~;9IXjC+4!u9>T)mhLoYr1@^13GQB5GR=VPjQ% zuiZrqf%LY1mw()$6uq?FaD)G*FMlK}0qz8B{|lnCaRt1#(P_(h!3?Lt6x#*%F0xR* z@c7%C(=EySsPPnTUXq4q0;K&jiA_5LtO{((V*Y{ne?r6lf}rzyGm7R|LXqa z{=U9B1g=_D<*Cbn#oWRF2>zE4^<}MKadB~B@Ij~J*6cXczugj12-d9XDJX#4KWS!E zf`GFBy;WV4NP8F}VNW=9PsPoH1T)P#l4>tN1DC7?5H ziuN6qv)G(Q3=Q|s-(r;N=aSrX_Pv<1q>m1f=cjUNW1bj>=a?FjpGjGc4r+Kp_&Ihl zm-;cQUC-KF3oP}~-Ydm&)0fjWe!yEc@hRZ5`D_$nvzUe5BI}Gv)-!%~)h?UkW|wlE z6|kqb&qJ#~Ks~21T`tEpet=toQ?a*iscT-ao#I@=nk5`42#+z`Qu-*jzbcrJe3t)KWtsa;`Zo}`|QMdH7jQ$ zjjl{HRReVWM^wN*(fFE*>K;TXmP4RH*zfjsAYHtvRK&mB}j$^R*!u zkGcvi)(1#@{D$X*PumImP%-@+Q&gWQlpUn)K9=j$=UFeE?yQRffTs_WENNj9^@HYj+$6jBt*nIk}uLtYRXsXvE%@Q^y zuS+3IN>`peD4L>y(?*A zCyKK9{V>Uw|Dk&mpQ>oeM~A=5dE3gaG>oG-M_lf@I>qy~^y#W(WtVw7CoXP&O1C~| zF?ZBrRwnb-iP!d*j&zkw*UX|%vOc&cbZ=grUgHHNNk)k$J)vE&PMWIwT(j}KW$eut zw${(3Id$>P6!P|5#CDL{*`CvvCeZM1+Q6E3lWV; zXl*^}r{H%lhb+q**G?yw&}`6Gw`Z~(sD-|@l<66)@OkNPHSG#55^7a*bHeANKhC)u2wI5_(#G zSW1~ZXqW8Sb^_usm9eIo?IWLOmLG>|J%(!h4!Hccl5UBr0`BC+dTx5FzXG-cP81A# zevZEiHW_<TNGl;#7}IxSc9JaNE>{7_ljy+H(_;iey29}(a@=_hB_O^n~1pq zI#|jH1lo08P-AQQbF*tTSgStcsX=<&3E}t2OHKT{7fK@bbn0Z3b={8>J{ux%aUbJ> z^Lr_YwP;9rCKlVG>CTy0Jr)%f*fiD%C9>^1f{k%M7SGUef`YgkS5F* zc#I)mZ?6OAVi;o{|MW-JBs4k+=i<$RV1}%g!py>Pjt0jse?@P^#MG}4{6{*&)vF#K zu6oJ&uCm_;i}DNMZWQT{nMEf}ZaIza$E5BE=VKW9N6mEu%zoOpY1xj=+e&eT&L0<4 zXxWqtwk`U6UwO1Gd4$XLeUEoDs3yIeDrDArW`@=9w`OQ~aJ4&c#SsP%7RNP_Do*A5Fynx# zW6v}@or5%?G55D_1;{*zZms^v9tcZ~ec_g^#pjRUYTRXW@63yWK(kFgjMH^b^1c$? z)wdYCfgU;{VmP+-tsay2ndAu>`5qlU-M&*b1N8Gl8o5PC{Yc^Z)MX}@hFH6-J3pyt zdSub))D1`3zjN)B?x*Vroa&4=NpsB%Ras{r87ud#Ou18($-5_s^_atJDRwkjXKczz z2tHpi2DKrcjVM<}Wn;Ui2)sJs9j~&phbD2rWxD(2#=PpYt4bqY+KEkD(alFiF#$0D zQHsG5tAB?PH~aH|fQhbeM`j0tmI8!CA6CCBoQ;ugx(xpCvz;0E$>T0@E;z?-j}dMC zp@^C*k}k9Rcz3}n38Cgx>s86``4hK_LKc1xXnNG zFyr3(gLSTD36alFAhrC;!3yXFp>QBnLJ`QU^}q#AAjL~j-fCvt692vEA!+h_l&hER zt^Iy5{DPf-D-*dy*xcH}B|p*@VvNkskT+%nwG0H+e4M9VLC7PF2Avr2%OKih`|gO&g2=C?vpz3Z7{^Q%c3eN~li z-f(<*Tgl}ok2AUJ5fbP(ZG#)+M)#Tm(S~X3K!4jnNs2cyCu%uh{d;6d4YCtCx!}^> zQZJ`m*A{xM7?1CoS>g>?j6GjA2nQVh91?Pr+l8gn(F+^i|9j-jZeIl{ad zu5p2=c$1nx71Y17$8Dse6T`o5>bG3a9hcn&8kkAnVK?W!I47u&w)Xu|$&?kfP%wBH zDoZM25W;9-x4`a;0Z*G6U5;sN8hY}Hb4yqJ@eAV?EoOFTJw;EgViqOTLVB6S%>B1^K$PMeK^Jo$PQf2KC0#MQ9Des z^yAtig65*jM5FZXKM~`IW=C$F<~^mRGRLgIU)>?;uMldEiPnzvuei&0lck2FUg+mb z?i1n|hsqQ>Gk}Pz=Kd7ArE4P6e@LI`>zqBTEU$fd^CBE--nMVtgHvk1~JD=)ot*ZHHAFh>^b>BxRK4O@>Z!P$st_PX|=CX2p8g8ytJ zD}q25JcfOr7av588b@yjn2VxEgukVrmTVUa;`k6DT0 zA6WU_k7-Wh;rc@xOIPR?^)Bwt{f~`{W`T+&JPe{#Ow_UpY7+{l7NPr=q^a-E__K60 zPo4|E5d4ICWTD=HGS$vVR2R8+i(LIFNcN4xBt2K=kIN!<&d`+f%Hr_pc;B`1|MQps^Xm4^X zLNUvCvZzHq@&ar^-$kN~2fZmECnDBrE>c!`43#Lk>wUB5J=K?X7xVC?Oavvz!svn{ zcIBOM(eMo-tbzZq34$`xJ+{v9BYb+t~@oA|JacZNNcq0b!=!8LdZl-VZpbcsyu&Ue`^Um5vQb&(;RIpIq;qiM> z!KlKJhX|3znhVl)gqLsB`U#Auhtj1A-0x=2}4Sw zYl}59-LgB$gP+pN+-wIAzZ_8f8QrATDN*h|q1QxpAPN7H;Z?>ZJt|tCJdz$$i7M2P zb-?-QI4=#)(VFk*3oHPs{iArb|1|!2fDi-UX@y#q%8(iP-NX4uj$9T_m{m3!@eM+0 z!=JtJ%oO5!Y6`C8o0)Q#o`SyaxUNS$dU8*z1*!F%K{#%XC`cr@7q2m6v92m()s1Fy zSt1|Ya=C0+{?p4RJ+0*#>nAQA~bd!b9!0{iI4bmkA znVj_WjMX!klaq7>C( z3ADrM^fSVF((CD_eoi)7rhf1S%iSH&fZPKbYSeF5;R3=I*1Wz}i1fj}OM~#Fhl_WJ z(Ams5mnHoASr^y%uY+X`w;LW%dL}Z*7|{)MO4tVORD=U$|Co0nB-%u5DabKV^3T)4 zw@wgrhYuyC^P4-fUu=kW)joS` z-++?=T)22xG>8U`SjQdcADr65_ zSF}Xp1`ux+NK#B%{M#(vJM#G>oL+JDPh}=sAkb$J&L-de!w{L|qO+a09{+hH;vUQ& z$eT-o(vW5;-hX48i=Jy_nED-y=yk18R76vRIF%8aLD!~WSACHxS5OB&PbnUKZn+u^ zYHU{01v6&F4IM3vZlOEQ3Hme-`aPnH(TR8IvfAgDOwprJJ1+UjWi4l(ilD-|CFlfY zyT|b>HyzI!AZx38M)ko5RQciDbtU$x1A8p`Lb%SvswNw(i%pL%;jAmX*~c}l|4`?u z-o|=#iNw!^hLy~%;e`erlZ)#|`c|V7wZ@|xXeeV;iA?sHMb5AMy*E)zbg5 z9exW-!B;*EQ&2@Grk!8n_04=+)lUP!MNpX*3pIMFdmH=v3uIDyjfdtfeON;;-n=F( zxPAlL9i_^JYyEXr*khI*4GZ` zFU#%~vO041`=QUTaMj^Lyk)0J9Rq1Up4f69K zTRcyRLCd#+xK$Ugs*GRnerb2FIK&!oBRy_9Lk=QzSK`4~E=dxDGdb}Ru+JAT1m$*f z?ozhpC1$HaJA&#t76A5B+cn*GoE+I4VHOE*tR23LX5pnoo$1+*Rs&Mr=VEr}xF>-rgS0nX!CqD0-=;%gyRR&kM)(*A!0wDgy?Lz=T`K}gy3 zAu}jz?{*2`_ja_$3_acYuP!G07oho|{gPY(vhtr3dPD}CKG^@OfswN?%Xg~~({#Q?P>x1G3-2VGplwt;wNYDhSe@}_8d;arm(3Pm|&~|(KzZKR6%25A%Qp$Os%#i2-pzIY0sQVxzgCOuuZN4@jWgl`G zF8v~bUg<3^E>qBjr`b&sr3?~} zbjl>C`xiI;g`fLWuo-Bvz#c;StBiTPNV;fyYw0>_bA8F}*SEKrpus|Ox7GQPt!%2R zdG-)wFGOn$T;;B;^=3HQgqPkC(=aWrjakwhQ(2ds4m_^Szc)0fASMKQpUz*Q?|2A1 za14q#V!*f~YN0C`kOfIoaZgp>_!Yyexu-_q4C2OO&8%tI8-F8qeG^e^FiJkp5!iO> z^(8w(oNr=S&^_mv!-iMCY@^Uakb!KCZ_apu6<82B6L{RN9!k%3FzAxl#YZn8arHNE z^2za8kb{@(I*#}g4ODzN_tN78dnqeX|vl&+jUD5FFQwg~woSobAP04;5R{0RZ{10qriUV}q5(pLcIH zf?RhBNGH8mu|^Aqz=GUrsd1AUfhpLnjeJ;CapNi?EfiX?6t0M`AuO8{AB-z8P0~2` zm9l#{*NE>puy>UkWO95>7JI2r;yr4So0FoL#P4;;C0jRA zkA6GtyHwf=A%uNRkt*YS!miUlA1xGoXCg3xY16WH0cgM|EzSa@9YBbKIy*NFyF2sW z_Eckd90w=+nPl&%x{He~3kgb&?~riP-$dmr?!TerVrwkVw`mN(Iz~gmkcD_NtrjK4 zrcE!twIWm{aCQFCC_)*}a7WWEtm1H$Go4AI(1AuDQAO+dna*U@%K!!BnucEe-oW2( zNg%TkN8ohCESEa62FnBb$`M1$@8eRV3gj(oN=+mIG-MksXo~Q$Z+aJ1>pIkz0Kl0L zSxBP2@1Oc7^zwrWBF?%}D%^I0%KKDr&YVd>ZjYvi^kvN-YZorO0cO$Q2%m|^qBf56 ze=0QebX^y9LbWP30PIh{^34Eoji(-S<;pO}RNnd?R-wpk1NEQ5J2PY?l)4rE)Q{CO zu0vRhHrD;z+p72@l6uAxUr&l4ReQR62EsL#ZYWh1RN6WQr~xz?a0wyq4!m#G1Z5oO z_k-ABqR30wJre!g7K~$s|FWSmx|>`-fmv=SPn>KPVB&k3H1NzRx23n z=Fqg8)l!JN^Vkl(7Q`+5Af=>aVU2F9sh5ts{>U(1G@(5;$L|x79#%{4N zPp2YPI3LS`lDGMLX)^hVr4@1a(D3kVyM=`X@4|O`E!GKGL@v9Y!s`OVxFg-1kB{}> z^D?jX>MD@i+GhPe%5?OG7I%KCxM>RoBR>qNM*ga22F<8r#w`6t>@e$n`idJi$Fi4G zd?^Cyhqk*UfjQ9yc-}4rz+-xL#s+m*t!MrMP@qLPnW!)7?mhkEp8i}pT#0xy86*88 z@rv9<_h`-41jyWVGA3`BK3~zj3VRf%h+K$&YoCT51aBjPu)9REPi!}vKIC6}f%i>U zYf|wwc5@0MO3>lqEqwz4$GI^zOk$2(>uq|> zSGVsyrRsOsDyJq`OZK+eWjQI*B<{hTk!= zX~tT%o2_18HoNw4A5`52F=`)cD~$;^K%}0PX)Hxb_A;&|n$5+1FjmZVRus84d~RPe@vT`_uvjAhoTHd8Q1iL16!%z$A>l zE}{wgb{jtA-1KvK$`QJAR55*Xah2r0)>62VW~N`CyOK8LHLAD}r2ir-e#^v#TgAk4 z0njs~2@+=_Yt>odQzWQkViJRe^d0{7)y@7MefJ)B)}GkWCtSgxkwNh~J6bi#&6@$c zYRSD>K^fX!dw`E%ln~9enNeyT;EG=|GMcfr3F82xomu9Sy-Kedvi5EX-NzzSxdy3} z#fCVbR5Nv>S9QqMRgYRw=;-*W?cF&>Jwo^;cR#4HbYbjbzv9|^YTeoG7WoRc83%2sY`GFvd_f?YMAyRdizb1LH)U@pD)0xErqQgtwQIw}@TCAs-YH-pw`@N~QR`gP7@#0^_d;0?v1j0oL%kb)*UV9x*9S zzy_wE$3J(B3to%Htek;G)B}jF8oM?l+)fW+O+bv-Cly!iy-mpNxY;inwN+o(^?jl{^Y4TLVz!;Sce6f=yKd|FeeryQuwvlyoeq=1mG_GJx`h0bD2q6~_OXs7B%h7U zl#!4JkD8hSA(HP!aGX0qKZsH^p2yExg{WbGBL5%T79I>|+5l7T5g^?Rj#$xowD|MO z)^nNgr$VhLTC=gJg-vdElCnEn)E32F347UR!l|2;zZp;tfvy@$PNm!uw~`+X$*@Lj ze-6_-wjWrZwZgy<>q}{-PpBQLu1StCt?RP(5@8-zjc^kMI9X3|vn<_Aqlqk|KhU04 zeqRisNNa!2dvjYYDm#l>Uk}U{nyuUGFyMWiDwLMbpC9DH7!V-7elrOKalWEfK5vs$ z+^xT30)cRwx}>=mWVM|ar2oNYR{&ukx2rE6Y7sZ;ei%TVG=PSkIstA4a*&rHBBC6~ z!HOrHlez$S^`H$1=+17}$hG%uFA}^(U}i6d>;MJNO#~hXz4Ih&b5+3h=1k>(s`3Ae zpWivP0ae$NkwBi+8zZU~kmjD|C{8xg~cuPMJlqdO5#uIFvnXL)B z5B?8-@c$FO_c*V76ao$WS3>kZ{sNx#|NQ`QMGVNt{(1QQ&N_OxXA!~r)i}^T4Vg7f zL!gU{G`C;nK?Rj2=+=o%x$zem_&jkEXuFfpHn%kp0^40(6bMV~C{hD~ehEC&1PyP^ zjgH=&0?4i(ud%9E#pK)<1um0IZM(c6{PBu~E`6Ao0Z?HiiBP5hl*j>i-{A!W(&|^W zhQHYgkTm{!g>Sd8M!$?(1sUDk?x_0)n@v3S89xt$ux>wtu+(1^2)mQJ-mL`!U6G{) z2F+fyuRAStl0jiw%wkc_LO^jr-9}K?QUv^CJ2G@w%6t0Pp97i zg9Hf50ov@vb{9IW14-$BcVN2{{wHjV_JE7U8lJSgN#4#-0K=wn7wWrt({b&Y{-l5G z;?l6juiNLpc^0(|xq&uVpT*T{u}yQSSe^tHMwMACN zVba>IXI;jPRvx+Fzt%`ieHu^&h)iN@zX%1%1qI&Gjg=id+J~cVJ?Uz|#})?n0MQ^l zP9EdjSiiA_36|i0x^sq^*I?weiF5bxDA%WX(ZB{Fhfu`f%_pA6pbZ>zg63Q!(` z5iu*83LJ$d4k@s1X{sH7$I|&mga$qyoiDEZsM6cQwB|qDM(^0%&+WK7jfI}M`9))- zI!Lcfu~8BI$luHi_ANSTu*h`*i+%)o9sqz(SGkhWO@Qk3>@Ue+j5DL{7|iAPrRFOU z$-mqh>|fPmzFfv2sT3WRECut&w=%41FIWH|qso~}Z^p5_ z^?aafX0f;Cv`BtVf2YTbYYbWiur5Dfm~5A2(&1IEh#6&=kLyaY4xOY}(+Q_cx5K&h zG8l&_EgxNg(*nIYyu|dZ>Jq<{kd_GIG{qS>i}3p$eiJ*%b^;)@6U&5^Syrq(cK@jY z5vT&^6R~R>CBUHE__B62_}Qm2$HyN-tm~a{zpq}8}i9R(E?RnQ%xLT@Nav?u0wR4l5pAT{;9hnpjnehlP5=zZr}cPa5CdC@op8= z-zS3q>qJGpDu5hF!HzUn+l!WXuze+>#gKTf6mgZD#Y{pQKv-=fS_}Kzl~w8ixo2#@ z&QR@|N|c9a#8xe-V$0W`78gT)L0^+I#7!jR7_?Fu>bD&WA6Ch-l>fO_Co~o^y5@Pc zlIfBJ!In@G^c*j4lS0;E6Sb~W)xB;HSzTNH`iH6zZdTm7B=nflKZI3BzWU-zeNv*- z!j$?lv9T$VWLg`?c>=q9QV5hTHD7;&a@pIjp^HH&9p;hR+-q*ih5B>G_k@X8?_FjA z-pF{(w~lBM3f}rQX>Yu$qEAQ7R2Ol*Jyo$Z(Fp1YqgFR`_cl%a)uh^~U z=jvF5pH8S!ZQB=}vFgx!n183+S|3SDs&z+F0hkKCbcGVXZSzIb<0nF*r zvdA6A*E8&xF>(9ax9xPHL??wXk9!^cG1de&d`0O>y*)ChlcqrCttA}joYkaUci;S_ zV@awmDlWztDUJ9BFq4GcTS$g!2QziI{FYq+H*~DOe!F1E~HvO01UaeBe&^Jt* z-k+e>k>X9Q(9*sy_a2wFMSSKq*VS^ErEJT50?{!3AoKJr6k3EfHZ(uSGZ~CxWGedo z!ghSM!S4**pc#adJJ*^R?XKv3O&%+c*FrwTZ2H_g;Qo}DK8eau>IOt#%$R}?w_GcG zK$PI$cab(Acr`d-C*vzRy0B?^^6jg239A_p)UFMkF^WR7eEe>abDZAnWvH zCTes5y_7&E6T^pU+_!%1ffS#djWbxdtDS1&w-J)jn~Bgpq1GcbHTYyqQ@D7Ez0_<)GDKOfEQn?!m2 zuu-qS466@=G;x=x3(d%;pMb23PP`n5T?07S`3Gp(2gvz^m(LzNKLE`2=kJ$hTMq>S zrPNv5GbfAeoL=oXb$L~Cs@)y?_=B^Hi}Ws7e+Lc1?~Xx>{fK!$M`V;!#6%fB)*;+?kS=$ORc4B_3GX2c1NFFEG1lLmQ%@U*DuCxjN0 zl0t>*hZPzRYhQASWL~sMFcheWIE-(jT@+#GB66jsbBaB#C?~sw%~&-P+>$qnuhKeU zp=Dh%No1f8sh^*^1@`3gx{1!51i%ae#XF$v?O-PF^@XRpB8{Wo+%qBC5zo)nSs4q2 zpIw9T@DR+(3-$HC3~w{RqZK{6qdMJ1ivnQH^q)ex3-q0m!V$CCnBntE;3b0QF@p-z zN7WJ5;gs>8tdDix9dGOX^mL=R-wd@LX1mg}sp7i^xOmd7eeR2_Bb6K5v=Ku#&vznz z&3hZXhQT*_Gz5K)yf3e;G@u%=CIg5{R09A4S!sb5h1gxEE-wN_PQULjb~wig#Z>b< z%xd`A4@DZUX6L#(jnf$chh5!qds;Tv(eR^D{$heflGuC?`6xa;oGQa9f^hv*IsC{MBl^Q+V8x`a^n3qNFiX5T?S)ov#c-`<~CH8?mjET`xx0> z^PTNpL$cUvYoyDree+uxw_v(2Upra#$k=E>M3=cx?dI&=-BdkT1%5r~>f$Se`Kxjp zEp~P!Pg`93@M6u$FHPM^1!e%hG=OsAWv9m@uvU$K<=0QW^&ew0u>{@(oXOcxY^oh6 zv%Y#YEkxOd)})QCzvd2Xo?Ga;$*l7?b6L6e#j{gqYzBKOY(c zkR^Yvfh3K!{7cT+(zfN484@{TxH;S9HY1T3^oFW03Y%U)xzyIPzxW4@`%4b}TK%BZ zhs)yR@`IXxV@%U9{FK(xM+x*_Gb3h0>&r&tM?NPj`@VaM37 zV1KyqTWk*+OTyR_uiQLz8o4}xBBd+4EK#$(cwh2ErZVGP*EH~BiY40pxv=sWm?T)E zzB%4m1|SobeQya=g)q3%=Jc4^u?wzF70A_)p`qHqH2JKL)EuT}8yxHQ9kB+3H!`ge zb#3lC(5XXoeUDA&4WMH|>&_W;?jf`Mo1*)zIFpP0kdsw)(*Zgl4vVQ$qUgAM#Xq-9+f_~?bbcKLAS{&HP3OAEG_V1%QG600b(Go{Fu z(#>U`yMP5y#NTXBx@ALEK-SB~bc++@kGS3W^Id!oh?-o8*DFwmD$7>j#uz9e+< z_POOZ)qGS4x1^F-Na>#o6Mi`2nv-D~-2q?UxUUw+EX(YR4j=3-cOsu(val@D`VW@$ z;bB_k-9EV^X~mZ;n)1&yebkTuGHpKxJ-v2&aR{_#}Ls_EE&oh5bfQQ~uhQlY*cU4f-j6 za+9R7%RT$PRK%xB*VI8cO7Hm3Jr)ijlg|=?=sL5ks}hrd%Q}Y2SSid-0Gjmq-n-Oa zAJw^O$F$3}R{oz(&OIE;_1)uQ(T-ZvS~;zp>Q{1VP|Kl&AyP==6q#{M$YIDihe3o& zC^;?4A&gVw)R?KkOtNBS5)FpTXu=v{Oq0_v!=FjvdyuiVH)<+|2&v@S_pv8Lq ziaOJ5dDt8v6p=X}mVNt(l-xalm!6xj@F;fYbV1KMwF&pwsViBZ=;cin-|a#ly0IH@Ns zai6@rUv9P=rgMxXpqFva$8L;;GBE;cV5s8m9IQcpsUjtcxU6EPZcM7OpRHR==E3iA z`jp)(qsgg0vn6Nf>ZU_i%dL-WwRGeUR?9w#QS6b84-A?Ty5C!NV}}%6x)?uBAWJ(| z7onC;&xOhe@O3gvO|*_byKq3KS^HB=qsV0Dx(X9MH6KGerIgS$5&=b4x@n86bt>GY}oAWC{ zsZ?2R)*STZ0fvtg$tPd39n7greeQA$ww;k6*=LKh&Gf6u^tqG6zTYJUexH{(d&gsI z_*Owv4icvOZrmWR|M{6bSky!~#<$To&qcZY1~B7Zy~2j`55~@Y>iG-6NolwvsLK`r z)2=FsC5*u)KV-_nKBkXE*n&GoUfY@*SDB@o9QS^|$mgGwjJVzXlLLOLXIB(W4SIYP zg4(TpWq&X1p2ib!@|X>0_UAFHYHZBT0s z!ly?1FO>!(gKa*5Us^}cV4!v!{?uJq#6+u(Yx(_AH3ox5q*EuCW{449XObGqj|C3< zaTm@ue%Lti!L)wSvhV0|x;aBAsFHlwuN)0w$5pzCqppI=1cl=#>P@kv=yLq%??w5*u}>80=c?MEQ*-vdp8-D=_5 zJ+g^Uz5l2$GM*Jvv0*o(Sf4Xt0i0ZA9ql7QQ-0=$;i24xfj3fz!v&Z=j zxJkiolZWeuIJwZ7%Oa{Fl?64bF7EO#ep=fs%r~r2;*13xKXffb1;fV}Uw^XEIRr5A zl^ZvHLeP~773&LqENmtn9IAHLgyAyHl`4mY;KqV}saYQ=H+G;kU8gpvz&!gLpv`<> z<#MK`-VaT*v~GY>#-7G)^6ir3lUuCFQ3%^UDjDay+?$YYoYCjS2=-HGmTNN+IxhJX zgX7Vgb~GB(yKF@1=Ia{7U!Z;k`?20lLh0y+;H3i0bSkN1ex`nxOo*4{wJV3JX(!0w zhe6RbuQFh?0!i=N^BVSf(QY3$^{nGCjGj238&-MPa*XE%WZg$Zk)gb@M`T)vRQ}-WB2qtnu%Q~@a zp*;2Ul$M6Nv!>miG+UdV!tCY)(7sOrZ_}=6ly4aHYsg&GDsXuLh96UeUgd&tZb6%Q z`Ti3|)})0lr2t=2wHB^@M@FW z%ERlL%L^(wlVj^XD60D476fGF1PFgA+s!nHmR${e_BZ^mgycx-vi_6JA9jgUkX=H9 zBFYt)QZ~=dB(8s^h|xyDeH-)aFvY1}JsIBHhUVZkcC0-9Ju#q@(~E15rk#v``&;>q zM`}Mt3Szh(Uq8M{-+it4+m#F^ps4g`BeYkCj|H+3$kFT@?#@Mdy{g-UGp{@ICz8D0 zI{F84H4Eq)n-Ml>ub?xNe$3iYTm)6?GDuScTyA7#%^mL4Eq7yfPMNUd1`~RK+ATs1 zUFY8@9Gkx=oag@ly}ID|oieC^BH}mVaZ`m^w9jJx_@C4~q=n)uP%2mq)Wm%yhIzqG z{tqsvz_GjbBWmaC*nrRI7K+$_F6BxVo6=|kcEGWg=GGq44 z?hy*pYY0!d?C&=$4jI257!P0= z`MUGHEkP)_ob%l|P_`3CmGFE9zT32-cE^T03_XCvCG7P7>-CYG|90A+SrRKOp#~lu zDolzv@B2iz4AkyAh(;(xnX zD8BBr|6|hRU;ST&6?IL!Y>~P{2XqKO+nLAK@5KShgLv0CV4pr!2Dn*^m|P5xeoN^&51cZ7yz*lxXm(e(x( z^xSn-sY9AA59PF&Rxc3>uIJhPq#08ei=Imf9pOhZt=O8)&BKG5q^0J9&cZ6Z*O(qT zHw>VWKnbtOAF(lUdcsL%rV6|giT#_mJRzZNVJ5@)KOM$+l*lAl9=@{bX=~XB&hmIa z;v)5+D2#x@F8ZmK*x$OmdXcyxYNa2$7+;a0WO zK2*^br1;C4)+5}KmWVZ{A*;Yn_><8t?QHANfpOR38n zZ2nD}Fz*6Ph4A3!64t?E(AcU=d^f$?S6Hv0PY*y(7BiBU7q189*Y0jwLjRZ@8mSmF zE#BnwdiPTe@r4#Ieu>6lS}h+(n#Pa|$8czMG7SB@CQ*x1Io4qXO!o?Tis z;4h86iEt_Dp{^)+ozKhpD0GQa<-7oT#4~sNk?r!A1jY8ptTer^OGTl7$|WuIWf!`g zSzp=Sc=M%zlzDBS5tZK8cNFc_q+MrU!2gAGi2;f zNt`#Fj&S-ByyB4ok|TZCl!>Ax0xzoq0=OXEl=0m7_IFfg$cR3FO^&}gurAPSE%2oF z>VLShn}5gCkbO~*=DACzYsAPJvW$Djzj)%w;p8Q2fdv(&&GiHEFvl9{-+^k1YgKb)SE@yBE!S1toy|-Xc(S+?ob6!K zxe5lRb26xo3lzr4zI*i3bM0^4Qb6=;YEzHUM^NVqwLKPUwO&+KCL6`8K>U-qb&zu!MNR$TJsy#K`>jq6-`?!zXmZ(5y?zl?YatVyUkK_l zbX#e_KN{g_)hGGaO9Ac^pFx5!U}eM(-%<@mV8**sA5@raeUM7P%7(|;2ykiEX63SH z!l`Hr8|-#hd)mAneuz%?Rl6uj8q!oL?u1g_;NB(VIp%QzzdW7GsH@h zq+e8?S5Ii%Td++TQ(@;74ZyK4!GpA~eLayvSEy?b%olmH5&Br6fQJDc(=SKXg1*&T zB#XZu>F>_4Zr_w#GQrGpZ_>SE>^W8_O~86n8H!OFitvRD-KMZt^9p zWrWqv4p!GN#ni^OtGv9N)g+MDVwT34^JpU%pNT@3&gjC?c1P|XZ5LrYH|A?fvis-W zTora77<{+3C}!_;o1)<6L`jnosx3?9yq4G=NScwXf?)mK$g)c#9y91SE{VG$n|6@) zEmHZoc5~F2drX^3FEgkRal71?O00HiYG5mX1TIVR7hrC1x8Db~upmaxKERwqUeWFk z#pc$JSNMT%TwAHtUPXHw&;_KcTE+NE54iN*&__=8w&oe%c6ZHT&FQc&w}PK)q>J1( zx$+djw}I@{ICW^s2puMPx&O~obHm$RNiP&iT@4dM!Z+b%9VC!U%bnd$(*NNsWmKO$ z>jIHx(rvkjjZA@p9m^XzSD*g0-}2TT7A`f*+8pJ$DIXfV@=S0a6=Sa~^X*P;M@;_| zK?X*va5%%O6xH^ru*Bq~>IJc`p=+Yv*)j&Nm==W2)F#W@MH3z?tOr7IPeh@T+Ub3s z(dIBSjDaIVfCsiT3daxMf<+z_I;Im1+@!LL+AI^8%NV7D9sm; z8|OY6Dc7ikv4Wy6GL+~|p$RftVO0@O%`l|)+HcCs50fu!xi=_t2Sb1Fub*#*vM`;6 z3Tvamd{f);=_Z=NbUBhr+&Iz3Q<_zEd+13+WBXKQ zG7n1?-)4@3+wp#`kZ*9W7h_@Y#q(PaEd05ZA;@)Ah$3dDEJ;4V+(KM*0K2a-vBeyX zdYwePKbs|b8n5I?YB-nQF1?o{st@SD%Tmml#*TG!~d~k(AJ|V{f z=F!4$j+1$yWxd8dsZ%oDtLZ7t^L*O#{IMXPKCTCmvkf093}Bga^U##q7rL2 zrW?V3b)K&W*wecLh*S3-;)t1qRH`l5dqPU0sTH9Gwm(b6jl>Z>@dMh5Fg&LR-ks(M zZj#n@sIUe1KU4K2m9v-7g;-WZ3(*6*au6p?xoqaJ>lKi(;I2OW7Hz6hQmlig z{6YvY(S~Fl)>vV{*E_Q@s0fYr+(H+}1KJqa(dn16((Wy7BCoqfs>)GTN0=leen$9Gf z4!51`)yS-tfB`NsWgCcmbyKc9Pzw=Oo0Ox%I>AAR163+(pDHBiFa(g*woR<3gQXOC zhiq5HOzPM2^cFpD-b@jV56Z6DZac`?Rv9d20c+D8v?vu^_Ej;Y-9 z#<>%Nj&X)dW|^XX@mE{OqpdDxmLRhhGdCFC=xelv5m}RxcEK_Dy<^W$G^sh?4NgWLyu4v&}DGY;?(YNo&!7VC2eA&UeM(D zy3oWLeajL;S+nrOQ4E?B2EN!qIB$5jhrxsi)a<@8PB>&x9a>VBuZAG5wZC39pE(oi z03JICHch@c;yTG=4zfmS%Y{~WL^*3qhFx_FZmm3D{@Xaj3r1?fbY_*^E1^8S*loQ= zGFTjs+)B{GVF;h1XjFq(3}<*Vq77r}WIA$kWVv;XKgQS1FC4?1RKF}|iH7v>RNeZW zB*JXj7R}B|(8l1v;h9~-8=FUCvRWr);W@xwua}NJvzVN;n%Hhfd(37w=5|#(Azeju zS*PmSn-)?M|Bv+(??q(-!L8EXcC&dnEZs-^e#_wqC`QDj~`^KLaIK=ZjHR~OhM*+rseA$0+j zI@%yLHSG92KR{#2NV&*mjEw3NL%E68Vd1xBbbNDf>`pDu|rvb8n}vjvOum&TAplh4oyGHfIUzh zaFw#6t`87R!6mT$U#k$*(80^Ls^W*pu$1~YKE`a}vWpGBIL#~wa*&lcwdnJ+rcMWX zwCJs!z~v9GC~B2BuD`%K_GtTiM|;#YQ_j{M(4SFi%}XaQJd?7i>MCmpwdvHZZa?~{ z#rTovXVl1&tEZ}GQ%#OAvTfW;a8E^xHYq5D9O!VLb%iV>E0I;2r}x&3kQB#0X!8uvNPQ|wx<2P z&AUGHa&1l*%Is3?a*4Bq$3iOkdUI`H^kH!G&z~Y-4fvNYZo6dTt{wGtPk)8km literal 0 HcmV?d00001 diff --git a/include/sick_lidar_localization/SoftwarePLL.h b/include/sick_lidar_localization/SoftwarePLL.h new file mode 100644 index 0000000..54a1448 --- /dev/null +++ b/include/sick_lidar_localization/SoftwarePLL.h @@ -0,0 +1,200 @@ +#ifndef SOFTWARE_PLL_H +#define SOFTWARE_PLL_H + +#ifdef _MSC_VER +#pragma warning(disable : 4996) +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** +@brief class SoftwarePLL implements synchronisation between ticks and timestamp. + See https://github.com/michael1309/SoftwarePLL/blob/master/README.md for details. +*/ +class SoftwarePLL +{ +public: + + /** + @brief Creates an instance of SoftwarePLL or returns an existing one, given its id. + * + @param id: identifier of the instance, f.e. "Sensor1", "TIM571". + @param fifo_length: length of fifo (buffer size, i.e. number of ticks in fifo). Applied only for new instances of SoftwarePLL, existing instance will keep their buffer size. + @return instance of a SoftwarePLL + */ + static SoftwarePLL& Instance(const std::string & id = "", int fifo_length = 7); + + /** + @brief Destructor of class SoftwarePLL + */ + ~SoftwarePLL(); + + /** + @brief Computes the timestamp of a measurement from sensor ticks + * + @param[out] sec: Estimated system Timetamp (seconds) + @param[out] nanoSec: Estimated system Timetamp (nanoseconds) + @param[in] tick sensor ticks + @return true if timestamp is computed, false otherwise (SoftwarePLL not initialized) + */ + bool GetCorrectedTimeStamp(uint32_t& sec, uint32_t& nanoSec, uint32_t tick); + + /** + @brief Updates PLL internale State should be called only with network send timestamps + * + @param[in] sec: System Timetamp from received network packed + @param[in] nanoSec: System Timestamp from received network packed + @param[in] curtick sensor ticks + @return PLL is in valid state (true) + */ + bool UpdatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick); + + /** + @brief Returns the initialization status, i.e. true, if SoftwarePLL is initialized, or false otherwise (inconsistent or not enough samples in the fifo buffer) + * + @return initialization status + */ + bool IsInitialized() const { return IsInitialized_; } + +protected: + + /** + @brief Sets the initialization status, i.e. true, if SoftwarePLL is initialized, or false otherwise (inconsistent or not enough samples in the fifo buffer) + * + @param[in] val initialization status + */ + void IsInitialized(bool val) { IsInitialized_ = val; } + + /** + @brief Returns the first sensor tick in the fifo buffer + * + @return first sensor tick + */ + uint64_t FirstTick() const { return FirstTick_; } + + /** + @brief Sets the first sensor tick in the fifo buffer + * + @param[in] val first sensor tick + */ + void FirstTick(uint64_t val) { FirstTick_ = val; } + + /** + @brief Returns the first timestamp in the fifo buffer in seconds + * + @return first timestamp + */ + double FirstTimeStamp() const { return FirstTimeStamp_; } + + /** + @brief Sets the first timestamp in the fifo buffer in seconds + * + @param[in] val first timestamp + */ + void FirstTimeStamp(double val) { FirstTimeStamp_ = val; } + + /** + @brief Returns the interpolated slope (gradient of the regression line) + * + @return interpolated slope + */ + double InterpolationSlope() const { return InterpolationSlope_; } + + /** + @brief Sets the interpolated slope (gradient of the regression line) + * + @param[in] val interpolated slope + */ + void InterpolationSlope(double val) { InterpolationSlope_ = val; } + + /** + @brief Returns the max. allowed time difference between estimated and measured timestamp time in seconds + * + @return allowed time difference between estimated and measured timestamp + */ + double AllowedTimeDeviation() const { return AllowedTimeDeviation_; } + + /** + @brief Set the max. allowed time difference between estimated and measured receive timestamp in seconds + * + @param[in] val allowed time difference between estimated and measured timestamp + */ + void AllowedTimeDeviation(double val) { AllowedTimeDeviation_ = val; } + + /** + @brief Returns the counter of extrapolated divergences (number of times the estimated and measured receive timestamp differed more than AllowedTimeDeviation + * + @return counter of extrapolated divergences + */ + uint32_t ExtrapolationDivergenceCounter() const { return ExtrapolationDivergenceCounter_; } + + /** + @brief Sets the counter of extrapolated divergences (number of times the estimated and measured receive timestamp differed more than AllowedTimeDeviation + * + @param[in] val counter of extrapolated divergences + */ + void ExtrapolationDivergenceCounter(uint32_t val) { ExtrapolationDivergenceCounter_ = val; } + + /** + @brief Pushes measurement timestamp and sensor ticks to the fifo, + updates tick fifo and clock (timestamp) fifo + * + @param curTimeStamp: measurement timestamp in seconds (receive time) + @param curtick: sensor ticks + @return always true + */ + bool PushIntoFifo(double curTimeStamp, uint32_t curtick);// update tick fifo and update clock (timestamp) fifo; + + /** + @brief Extrapolates and returns the measurement timestamp in seconds + relative to FirstTimeStamp. + The timestamp of a measurement in seconds can be estimated from sensor ticks + by ExtraPolateRelativeTimeStamp(tick) + FirstTimeStamp(). + * + @param tick: sensor ticks + @return measurement timestamp in seconds relative to FirstTimeStamp + */ + double ExtraPolateRelativeTimeStamp(uint32_t tick); + +private: + + int NumberValInFifo_; + const int FifoSize_; + static const double MaxAllowedTimeDeviation_; + static const uint32_t MaxExtrapolationCounter_; + std::vector TickFifo_; + std::vector ClockFifo_; + bool IsInitialized_; + double FirstTimeStamp_; + double AllowedTimeDeviation_; + uint64_t FirstTick_; + uint32_t Lastcurtick_ = 0; + double InterpolationSlope_; + bool NearSameTimeStamp(double relTimeStamp1, double relTimeStamp2); + bool UpdateInterpolationSlope(); + uint32_t ExtrapolationDivergenceCounter_; + SoftwarePLL(int fifo_length = 7) : FifoSize_(fifo_length), TickFifo_(fifo_length,0), ClockFifo_(fifo_length,0) + { + AllowedTimeDeviation(SoftwarePLL::MaxAllowedTimeDeviation_); // 1 ms + NumberValInFifo_ = 0; + } + // verhindert, dass ein Objekt von ausserhalb von N erzeugt wird. + // protected, wenn man von der Klasse noch erben moechte + SoftwarePLL(const SoftwarePLL&); /* verhindert, dass eine weitere Instanz via + Kopier-Konstruktor erstellt werden kann */ + SoftwarePLL & operator = (const SoftwarePLL &); //Verhindert weitere Instanz durch Kopie + + static std::map _instances; // list of SoftwarePLL instances, mapped by id +}; + +#endif \ No newline at end of file diff --git a/include/sick_lidar_localization/client_socket.h b/include/sick_lidar_localization/client_socket.h new file mode 100644 index 0000000..eac11dc --- /dev/null +++ b/include/sick_lidar_localization/client_socket.h @@ -0,0 +1,116 @@ +/* + * @brief client_socket encapsulates connecting, closing and setting socket options + * for tcp client sockets using boost::asio::ip::tcp::socket and boost::asio::io_service. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SIM_LOC_CLIENT_SOCKET_H_INCLUDED +#define __SIM_LOC_CLIENT_SOCKET_H_INCLUDED + +#include +#include +#include +#include +#include + +namespace sick_lidar_localization +{ + /*! + * Class sick_lidar_localization::ClientSocket encapsulates connecting, closing and setting socket options + * for tcp client sockets implemented by boost::asio::ip::tcp::socket. + */ + class ClientSocket + { + public: + + /*! + * Constructor. + * @param[in] io_service boost io service for tcp connections (several sockets may share one io_service) + */ + ClientSocket(boost::asio::io_service & io_service); + + /*! + * Destructor, closes all tcp connections. + */ + virtual ~ClientSocket(); + + /*! + * Connects to a server. + * @param[in] io_service boost io service for tcp connections (several sockets may share one io_service) + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] tcp_port tcp port for command requests, default: 2111 for command requests and 2112 for command responses + * @return true on success, false on failure (server unknown or unreachable) + */ + virtual bool connect(boost::asio::io_service & io_service, const std::string & server_adress, int tcp_port); + + /*! + * Closes the tcp connection to the server. + * @param[in] force_shutdown if true, the socket is shutdown even if it's state is not opened or connected + * (otherwise the socket is closed, if its state is currently opened) + * @return true on success (socket closed), false on failure + */ + virtual bool close(bool force_shutdown = false); + + /*! + * Returns the tcp client socket implementation + * @return tcp client socket implementation + */ + virtual boost::asio::ip::tcp::socket & socket(void) { return m_tcp_socket; } + + protected: + + boost::asio::ip::tcp::socket m_tcp_socket; ///< tcp client socket implementation + + }; // class ClientSocket + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_CLIENT_SOCKET_H_INCLUDED diff --git a/include/sick_lidar_localization/cola_transmitter.h b/include/sick_lidar_localization/cola_transmitter.h index 8068b92..8633405 100644 --- a/include/sick_lidar_localization/cola_transmitter.h +++ b/include/sick_lidar_localization/cola_transmitter.h @@ -56,12 +56,7 @@ #ifndef __SIM_LOC_COLA_TRANSMITTER_H_INCLUDED #define __SIM_LOC_COLA_TRANSMITTER_H_INCLUDED -#include -#include -#include -#include -#include - +#include "sick_lidar_localization/client_socket.h" #include "sick_lidar_localization/fifo_buffer.h" namespace sick_lidar_localization @@ -195,7 +190,7 @@ namespace sick_lidar_localization std::string m_server_adress; ///< ip adress of the localization controller, default: 192.168.0.1 int m_tcp_port; ///< tcp port of the localization controller, default: 2111 for command requests and 2112 for command responses boost::asio::io_service m_ioservice; ///< boost io service for tcp connections - boost::asio::ip::tcp::socket m_tcp_socket; ///< tcp socket connected to the localization controller + sick_lidar_localization::ClientSocket m_tcp_socket; ///< tcp socket connected to the localization controller double m_receive_timeout; ///< default timeout in seconds for receive functions bool m_receiver_thread_running; ///< true: m_receiver_thread is running, otherwise false boost::thread* m_receiver_thread; ///< thread to receive responses from localization server diff --git a/include/sick_lidar_localization/driver_check_thread.h b/include/sick_lidar_localization/driver_check_thread.h index 0dea684..983ee83 100644 --- a/include/sick_lidar_localization/driver_check_thread.h +++ b/include/sick_lidar_localization/driver_check_thread.h @@ -133,6 +133,14 @@ namespace sick_lidar_localization * @return true, if test passed, false otherwise. */ virtual bool checkTelegram(sick_lidar_localization::SickLocResultPortTelegramMsg & telegram); + + /*! + * Checks the vehicle time of a result telegram message (system time from ticks by software pll) against min and max + * allowed difference to ros::Time::now(). Returns true, if test passed (vehicle time within their range), or false otherwise. + * @param[in] telegram result telegram message (SickLocResultPortTelegramMsg) + * @return true, if test passed, false otherwise. + */ + virtual bool checkVehicleTime(sick_lidar_localization::SickLocResultPortTelegramMsg & telegram); /* * member data @@ -141,6 +149,11 @@ namespace sick_lidar_localization sick_lidar_localization::FifoBuffer m_result_port_telegram_fifo; ///< fifo buffer for result port telegrams from sim_loc_driver sick_lidar_localization::SickLocResultPortTelegramMsg m_result_port_telegram_min_values; ///< min allowed values of result port telegrams sick_lidar_localization::SickLocResultPortTelegramMsg m_result_port_telegram_max_values; ///< max allowed values of result port telegrams + double m_vehicle_time_delta_min; ///< min. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + double m_vehicle_time_delta_max; ///< max. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + bool m_vehicle_time_check_enabled; ///< true: check of vehicle time is enabled (default), false in case of simulated network errors (LocRequestTimestamp not available) + double m_software_pll_expected_initialization_duration; ///< expected initialization time for software pll (system time from lidar ticks not yet available) + ros::Time m_timestamp_valid_telegram; ///< timestamp of a telegram with valid vehicle time bool m_message_check_thread_running; ///< true: m_message_check_thread is running, otherwise false boost::thread* m_message_check_thread; ///< thread to check sim_loc_driver messages double m_message_check_frequency; ///< frequency to check sim_loc_driver messages (default: 100) diff --git a/include/sick_lidar_localization/driver_thread.h b/include/sick_lidar_localization/driver_thread.h index 667fd77..a9a60b0 100644 --- a/include/sick_lidar_localization/driver_thread.h +++ b/include/sick_lidar_localization/driver_thread.h @@ -62,16 +62,12 @@ #ifndef __SIM_LOC_DRIVER_THREAD_H_INCLUDED #define __SIM_LOC_DRIVER_THREAD_H_INCLUDED -#include -#include -#include -#include -#include #include #include #include #include +#include "sick_lidar_localization/client_socket.h" #include "sick_lidar_localization/fifo_buffer.h" namespace sick_lidar_localization @@ -162,20 +158,22 @@ namespace sick_lidar_localization * member data */ - bool m_initialized; ///< true after successfull initialization (publisher, config parameter, etc.), otherwise false - bool m_tcp_connected; ///< true if a tcp connection to the localization controller is established, otherwise false - std::string m_server_adress; ///< ip adress of the localization controller, default: 192.168.0.1 - int m_tcp_port; ///< tcp port of the localization controller, default: The localization controller uses IP port number 2201 to send localization results - double m_tcp_connection_retry_delay; ///< delay in seconds to retry to connect to the localization controller, default: 1 second - boost::asio::io_service m_ioservice; ///< boost io service for tcp connections - boost::asio::ip::tcp::socket m_tcp_socket; ///< tcp socket connected to the localization controller - boost::thread* m_tcp_receiver_thread; ///< thread to receive telegrams from the localization controller - bool m_tcp_receiver_thread_running; ///< true: m_tcp_receiver_thread is running, otherwise false - boost::thread* m_converter_thread; ///< thread to convert and publish localization data - bool m_converter_thread_running; ///< true: m_converter_thread is running, otherwise false + bool m_initialized; ///< true after successfull initialization (publisher, config parameter, etc.), otherwise false + bool m_tcp_connected; ///< true if a tcp connection to the localization controller is established, otherwise false + std::string m_server_adress; ///< ip adress of the localization controller, default: 192.168.0.1 + int m_tcp_port; ///< tcp port of the localization controller, default: The localization controller uses IP port number 2201 to send localization results + double m_tcp_connection_retry_delay; ///< delay in seconds to retry to connect to the localization controller, default: 1 second + boost::asio::io_service m_ioservice; ///< boost io service for tcp connections + sick_lidar_localization::ClientSocket m_tcp_socket; ///< tcp socket connected to the localization controller + boost::thread* m_tcp_receiver_thread; ///< thread to receive telegrams from the localization controller + bool m_tcp_receiver_thread_running; ///< true: m_tcp_receiver_thread is running, otherwise false + boost::thread* m_converter_thread; ///< thread to convert and publish localization data + bool m_converter_thread_running; ///< true: m_converter_thread is running, otherwise false sick_lidar_localization::FifoBuffer, boost::mutex> m_fifo_buffer; ///< fifo buffer to transfer data from receiver thread to converter thread - ros::Publisher m_result_telegrams_publisher; ///< ros publisher for result port telegram messages (type SickLocResultPortTelegramMsg) - std::string m_result_telegrams_frame_id; ///< ros frame id of result port telegram messages (type SickLocResultPortTelegramMsg), default: "sick_lidar_localization" + ros::Publisher m_result_telegrams_publisher; ///< ros publisher for result port telegram messages (type SickLocResultPortTelegramMsg) + std::string m_result_telegrams_frame_id; ///< ros frame id of result port telegram messages (type SickLocResultPortTelegramMsg), default: "sick_lidar_localization" + ros::ServiceClient m_timesync_service_client; ///< client to call ros service "SickLocTimeSync" to calculate system time from ticks by software pll + double m_software_pll_expected_initialization_duration; ///< expected initialization time for software pll (system time from lidar ticks not yet available) /* * configuration and member data for diagnostic messages diff --git a/include/sick_lidar_localization/testcase_generator.h b/include/sick_lidar_localization/testcase_generator.h index 695b4b3..f09a033 100644 --- a/include/sick_lidar_localization/testcase_generator.h +++ b/include/sick_lidar_localization/testcase_generator.h @@ -100,9 +100,43 @@ namespace sick_lidar_localization * @return Synthetical cola response from server */ static sick_lidar_localization::SickLocColaTelegramMsg createColaResponse(const sick_lidar_localization::SickLocColaTelegramMsg & cola_request); + + /*! + * Returns the result pose interval, i.e. the interval in number of scans + * 1 (default): result with each processed scan + * 2: result with every second processed scan + * n: result with every n.th processed scan + * This parameter can be set by cola command "LocSetResultPoseInterval", f.e. "sMN LocSetResultPoseInterval 1" + * @return result pose interval + */ + static uint32_t ResultPoseInterval(void){ return s_u32ResultPoseInterval; } protected: - + + /*! + * Converts value x to hex string + * @param x value to be converted + * @return x as hex string + */ + template static std::string hexstr(const T & x) + { + std::stringstream hex_stream; + hex_stream << std::hex << std::uppercase << x; + return hex_stream.str(); + } + + /*! + * Converts value x to decimal string + * @param x value to be converted + * @return x as hex string + */ + template static std::string decstr(const T & x) + { + std::stringstream dec_stream; + dec_stream << std::dec << std::uppercase << x; + return dec_stream.str(); + } + /*! * Creates and returns a timestamp in milliseconds ticks. * To simulate time jitter, network latency and time drift, @@ -111,6 +145,8 @@ namespace sick_lidar_localization */ static uint32_t createTimestampTicksMilliSec(void); + static uint32_t s_u32ResultPoseInterval; ///< result pose interval, i.e. the interval in number of scans (default: 1, i.e. result telegram with each processed scan) + }; // class TestcaseGenerator } // namespace sick_lidar_localization diff --git a/include/sick_lidar_localization/time_sync_service.h b/include/sick_lidar_localization/time_sync_service.h new file mode 100644 index 0000000..f000758 --- /dev/null +++ b/include/sick_lidar_localization/time_sync_service.h @@ -0,0 +1,192 @@ +/* + * @brief time_sync_service implements ros services "SickLocRequestTimestamp" and "SickLocTimeSync" + * for time synchronization. + * + * ROS service SickLocRequestTimestamp requests a timestamp from the localization controller + * by sending cola command LocRequestTimestamp ("sMN LocRequestTimestamp"). + * + * The service receives and decodes the current timestamp (uint32 timestamp in milliseconds) + * and calculates the time offset with the following formular: + * + * delta_time_ms := mean_time_vehicle_ms - timestamp_lidar_ms + * mean_time_vehicle_ms := (send_time_vehicle + receive_time_vehicle) / 2 + * := vehicles mean timestamp in milliseconds + * send_time_vehicle := vehicles timestamp when sending LocRequestTimestamp + * receive_time_vehicle := vehicles timestamp when receiving the LocRequestTimestamp response + * timestamp_lidar_ms := lidar timestamp in milliseconds from LocRequestTimestamp response + * + * See Operation-Instruction-v1.1.0.241R.pdf for details about time synchronization and + * time offset calculation. See Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF + * for further details about Cola telegram LocRequestTimestamp. + * + * time_sync_service implements a time synchronization thread, running a software pll + * to estimate system time from lidar timestamp ticks. This synchronization thread can + * be started by function start() and stopped by function stop(). When running the + * synchronization thread, ros services "SickLocRequestTimestamp" is called each 10 seconds. + * The software pll is then updated to estimate the system time from lidar timestamp ticks. + * The system timestamp of a vehicle pose can be queried using ros service + * "SickLocTimeSync". + * + * Note: The software pll uses a default fifo size of 7 measurements, thus it requires at least 7 successful + * LocRequestTimestamp requests. Depending on time_sync_rate configured in the launch-file, this initial phase can + * take several seconds (up to 70 seconds). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SIM_LOC_TIME_SYNC_SERVICE_H_INCLUDED +#define __SIM_LOC_TIME_SYNC_SERVICE_H_INCLUDED + +#include "sick_lidar_localization/utils.h" + +namespace sick_lidar_localization +{ + /*! + * Class sick_lidar_localization::TimeSyncService implements ros services "SickLocRequestTimestamp" + * and "SickLocTimeSync" for time synchronization. + * It sends LocRequestTimestamp requests to the localization controller, receives the response and + * calculates the time offset. + */ + class TimeSyncService + { + public: + + /*! + * Constructor + */ + TimeSyncService(ros::NodeHandle* nh = 0); + + /*! + * Destructor + */ + virtual ~TimeSyncService(); + + /*! + * Class TimeSyncService implements a time synchronization thread, running a software pll + * to estimate system time from lidar timestamp ticks. This synchronization thread can + * be started by function start() and stopped by function stop(). When running the + * synchronization thread, ros services "SickLocRequestTimestamp" is called each 10 seconds. + * The software pll is then updated to estimate the system time from lidar timestamp ticks. + * The system timestamp of a vehicle pose can be queried using ros service + * "SickLocTimeSync". + * @return true on success, false in case of errors. + */ + virtual bool start(void); + + /*! + * Stops the time synchronization thread and the software pll. + * @return true on success, false in case of errors. + */ + virtual bool stop(void); + + /*! + * Callback for service messages (SickLocRequestTimestamp). Sends LocRequestTimestamp requests + * to the localization controller, receives the response and calculates the time offset. + * @param[in] service_request ros service request + * @param[out] service_response service response with timestamps and calculated time offset. + * @return true on success, false in case of errors. + */ + virtual bool serviceCbRequestTimestamp(sick_lidar_localization::SickLocRequestTimestampSrv::Request & service_request, sick_lidar_localization::SickLocRequestTimestampSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocTimeSync). Calculates the system time of a vehicle pose from lidar ticks, + * using a software pll. + * Note: ros service SickLocTimeSync uses service SickLocRequestTimestamp to query lidar ticks and to update the + * software pll. Therefore, at least 7 LocRequestTimestamp measurements are required before time sync becomes valid + * The default fifo size of the software pll is 7 measurements, thus it requires at least 7 successful + * LocRequestTimestamp requests. Depending on time_sync_rate configured in the launch-file, this initial phase can + * take several seconds (up to 70 seconds). + * @param[in] time_sync_request ros service request (input: lidar ticks) + * @param[out] time_sync_response service response (output: system time from ticks, calculated by software pll) + * @return true on success, false in case of errors (software pll still in initialization phase or communication error). + */ + virtual bool serviceCbTimeSync(sick_lidar_localization::SickLocTimeSyncSrv::Request & time_sync_request, sick_lidar_localization::SickLocTimeSyncSrv::Response & time_sync_response); + + /*! + * Thread callback, runs time synchronization, calls ros service "SickLocRequestTimestamp" each 10 seconds + * and updates the software pll. + */ + virtual void runTimeSyncThreadCb(void); + + protected: + + /*! + * Update the software pll with lidar ticks and system time from response by "SickLocRequestTimestamp" servcice + * @param[out] service_response service response by "SickLocRequestTimestamp" servcice + */ + void updateSoftwarePll(sick_lidar_localization::SickLocRequestTimestampSrv::Response & service_response); + + /*! + * Returns true, if the initialization phase of the software pll is completed, otherwise false. + * @return initialization phase of the software pll completed + */ + bool isSoftwarePllInitialized(void); + + ros::ServiceServer m_timestamp_service_server; ///< provides ros service "SickLocRequestTimestamp" to send a LocRequestTimestamp, receive the response and to calculate the time offset + ros::ServiceServer m_timesync_service_server; ///< provides ros service "SickLocTimeSync" to calculate system time from ticks by software pll + ros::ServiceClient m_cola_service_client; ///< client to call ros service "SickLocColaTelegram" to send cola telegrams and receive cola responses from localization controller + bool m_time_sync_thread_running; ///< true: m_time_sync_thread is running, otherwise false + boost::thread* m_time_sync_thread; ///< thread to synchronize timestamps, runs the software pll + bool m_cola_binary; ///< false: send Cola-ASCII (default), true: send Cola-Binary + int m_cola_binary_mode; ///< 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) + int m_software_pll_fifo_length; ///< length of software pll fifo, default: 7 + ros::Rate m_time_sync_rate; ///< frequency to request timestamps using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 (LocRequestTimestamp queries every 10 seconds) + ros::Rate m_time_sync_initial_rate; ///< frequency to request timestamps and to update software pll during initialization phase, default: 1.0 (LocRequestTimestamp queries every second) + int m_time_sync_initial_length; ///< length of initialization phase with LocRequestTimestamps every second, default: 10 (i.e. 10 LocRequestTimestamp queries every second after start, otherwise LocRequestTimestamp queries every 10 seconds) + double m_cola_response_timeout; ///< Timeout in seconds for cola responses from localization controller, default: 1 + ros::ServiceClient m_request_timestamp_client; ///< client to call ros service "SickLocRequestTimestamp" + boost::mutex m_software_pll_mutex; ///< mutex to protect access to software pll used in service "SickLocTimeSync + + + }; // class TimeSyncService + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_TIME_SYNC_SERVICE_H_INCLUDED diff --git a/include/sick_lidar_localization/utils.h b/include/sick_lidar_localization/utils.h index 85fc0ae..af87e1f 100644 --- a/include/sick_lidar_localization/utils.h +++ b/include/sick_lidar_localization/utils.h @@ -68,6 +68,7 @@ #include "sick_lidar_localization/SickLocResultPortTelegramMsg.h" #include "sick_lidar_localization/SickLocResultPortTestcaseMsg.h" #include "sick_lidar_localization/SickLocRequestTimestampSrv.h" +#include "sick_lidar_localization/SickLocTimeSyncSrv.h" namespace sick_lidar_localization { @@ -102,6 +103,16 @@ namespace sick_lidar_localization MutexType m_value_mutex; ///< mutex to protect value }; + /*! + * Shortcut for SetGet + */ + class SetGet32 : public SetGet + { + public: + /*! Constructor with optional initialization of value */ + SetGet32(const uint32_t & value = 0) : SetGet(value) {} + }; + /*! * class Utils implements utility functions for SIM Localization. */ diff --git a/launch/sim_loc_driver.launch b/launch/sim_loc_driver.launch index 9aa1c8a..570a9f0 100644 --- a/launch/sim_loc_driver.launch +++ b/launch/sim_loc_driver.launch @@ -14,6 +14,10 @@ + + + + diff --git a/msg/SickLocResultPortTelegramMsg.msg b/msg/SickLocResultPortTelegramMsg.msg index 6cdde0f..4535c90 100644 --- a/msg/SickLocResultPortTelegramMsg.msg +++ b/msg/SickLocResultPortTelegramMsg.msg @@ -3,9 +3,24 @@ # for sick localization. See chapter 5.9 ("About result port telegrams") # of the operation manual for further details. +# +# Header +# + Header header # ROS Header with sequence id, timestamp and frame id +# +# Result port telegram +# + SickLocResultPortHeaderMsg telegram_header # 52 byte header of a result port telegram SickLocResultPortPayloadMsg telegram_payload # 52 byte payload of a result port telegram SickLocResultPortCrcMsg telegram_trailer # 2 byte CRC trailer of a result port telegram +# +# System time of vehicles pose calculated by ros service "SickLocTimeSync" using a software pll +# + +bool vehicle_time_valid # true: vehicle_time_sec and vehicle_time_nsec valid, false: software pll still in initial phase +uint32 vehicle_time_sec # Time of vehicles pose calculated by software pll (seconds part of the system time) +uint32 vehicle_time_nsec # Time of vehicles pose calculated by software pll (nano seconds part of the system time) diff --git a/src/SoftwarePLL.cpp b/src/SoftwarePLL.cpp new file mode 100644 index 0000000..668e2ee --- /dev/null +++ b/src/SoftwarePLL.cpp @@ -0,0 +1,261 @@ +/* +==================================================================================================== +File: SoftwarePLL.cpp +Note: SoftwarePLL intentionally copied from https://github.com/michael1309/SoftwarePLL +See https://github.com/michael1309/SoftwarePLL/blob/master/README.md for details. +==================================================================================================== +*/ +#include "sick_lidar_localization/SoftwarePLL.h" + +// #include "softwarePLL.h" +#include +// #include +// #include +#include +#include +#include +#include +#include +#include +#include + + +std::map SoftwarePLL::_instances; // list of SoftwarePLL instances, mapped by id + +const double SoftwarePLL::MaxAllowedTimeDeviation_ = 0.1; +const uint32_t SoftwarePLL::MaxExtrapolationCounter_ = 20; + +SoftwarePLL& SoftwarePLL::Instance(const std::string & id, int fifo_length) +{ + SoftwarePLL* pll = _instances[id]; + if(!pll) + { + pll = new SoftwarePLL(fifo_length); + _instances[id] = pll; + } + return *pll; +} + +SoftwarePLL::~SoftwarePLL() +{ + // Remove this from the list of SoftwarePLL instances + for(std::map::iterator iter = _instances.begin(); iter != _instances.end(); ) + { + if(iter->second == this) + iter = _instances.erase(iter); + else + iter++; + } +} + +bool SoftwarePLL::PushIntoFifo(double curTimeStamp, uint32_t curtick) +// update tick fifo and update clock (timestamp) fifo +{ + for (int i = 0; i < FifoSize_ - 1; i++) + { + TickFifo_[i] = TickFifo_[i + 1]; + ClockFifo_[i] = ClockFifo_[i + 1]; + } + TickFifo_[FifoSize_ - 1] = curtick; // push most recent tick and timestamp into fifo + ClockFifo_[FifoSize_ - 1] = curTimeStamp; + + if (NumberValInFifo_ < FifoSize_) + { + NumberValInFifo_++; // remember the number of valid number in fifo + } + FirstTick(TickFifo_[0]); + FirstTimeStamp(ClockFifo_[0]); + + return(true); +} + +double SoftwarePLL::ExtraPolateRelativeTimeStamp(uint32_t tick) +{ + int32_t tempTick =0; + tempTick = tick-(uint32_t)(0xFFFFFFFF & FirstTick()); + double timeDiff = tempTick * this->InterpolationSlope(); + return(timeDiff); + +} + +bool SoftwarePLL::UpdatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick) +{ + if(curtick!=this->Lastcurtick_) + { + this->Lastcurtick_ = curtick; + double start = sec + nanoSec * 1E-9; + // bool bRet = true; + + if (false == IsInitialized()) + { + PushIntoFifo(start, curtick); + bool bCheck = this->UpdateInterpolationSlope(); + if (bCheck) + { + IsInitialized(true); + } + } + + if (IsInitialized() == false) + { + return (false); + } + + double relTimeStamp = ExtraPolateRelativeTimeStamp(curtick); + double cmpTimeStamp = start - this->FirstTimeStamp(); + + bool timeStampVerified = false; + if (NearSameTimeStamp(relTimeStamp, cmpTimeStamp) == true)// if timestamp matches prediction update FIFO + { + timeStampVerified = true; + PushIntoFifo(start, curtick); + UpdateInterpolationSlope(); + ExtrapolationDivergenceCounter(0); + } + + if (timeStampVerified == false) + { + // BEGIN HANDLING Extrapolation divergence + uint32_t tmp = ExtrapolationDivergenceCounter(); + tmp++; + ExtrapolationDivergenceCounter(tmp); + if (ExtrapolationDivergenceCounter() >= SoftwarePLL::MaxExtrapolationCounter_) + { + IsInitialized(false); // reset FIFO - maybe happened due to abrupt change of time base + } + // END HANDLING Extrapolation divergence + } + return(true); + } + else + { + return(false); + //this curtick has been updated allready + } + +} + +bool SoftwarePLL::GetCorrectedTimeStamp(uint32_t& sec, uint32_t& nanoSec, uint32_t curtick) +{ + if (IsInitialized() == false) + { + return(false); + } + + double relTimeStamp = ExtraPolateRelativeTimeStamp(curtick); // evtl. hier wg. Ueberlauf noch einmal pruefen + double corrTime = relTimeStamp + this->FirstTimeStamp(); + sec = (uint32_t)corrTime; + double frac = corrTime - sec; + nanoSec = (uint32_t)(1E9 * frac); + return(true); +} + +bool SoftwarePLL::NearSameTimeStamp(double relTimeStamp1, double relTimeStamp2) +{ + double dTAbs = fabs(relTimeStamp1 - relTimeStamp2); + if (dTAbs < AllowedTimeDeviation()) + { + return(true); + } + else + { + return(false); + } +} + +bool SoftwarePLL::UpdateInterpolationSlope() // fifo already updated +{ + + if (NumberValInFifo_ < FifoSize_) + { + return(false); + } + std::vector tickFifoUnwrap; + std::vector clockFifoUnwrap; + clockFifoUnwrap.resize(FifoSize_); + tickFifoUnwrap.resize(FifoSize_); + uint64_t tickOffset = 0; + clockFifoUnwrap[0] = 0.00; + tickFifoUnwrap[0] = 0; + FirstTimeStamp(this->ClockFifo_[0]); + FirstTick(this->TickFifo_[0]); + + uint64_t tickDivisor = 0x100000000; + + + + for (int i = 1; i < FifoSize_; i++) // typical 643 for 20ms -> round about 32150 --> near to 32768 standard clock in many watches + { + if (TickFifo_[i] < TickFifo_[i - 1]) // Overflow + { + tickOffset += tickDivisor; + } + tickFifoUnwrap[i] = tickOffset + TickFifo_[i] - FirstTick(); + clockFifoUnwrap[i] = (this->ClockFifo_[i] - FirstTimeStamp()); + } + + double sum_xy = 0.0; + double sum_x = 0.0; + double sum_y = 0.0; + double sum_xx = 0.0; + for (int i = 0; i < FifoSize_; i++) + { + sum_xy += tickFifoUnwrap[i] * clockFifoUnwrap[i]; + sum_x += tickFifoUnwrap[i]; + sum_y += clockFifoUnwrap[i]; + sum_xx += tickFifoUnwrap[i] * tickFifoUnwrap[i]; + } + + // calculate slope of regression line, interception is 0 by construction + double m = (FifoSize_ * sum_xy - sum_x * sum_y) / (FifoSize_ * sum_xx - sum_x*sum_x); + + int matchCnt = 0; + for (int i = 0; i < FifoSize_; i++) + { + double yesti = m * tickFifoUnwrap[i]; + if (this->NearSameTimeStamp(yesti, clockFifoUnwrap[i])) + { + matchCnt++; + } + } + + bool retVal = false; + if (matchCnt == FifoSize_) + { + InterpolationSlope(m); + retVal = true; + } + + return(retVal); +} + +/* +Example CMakeLists.txt to generate test-binary-file for testing this class +--- CUT --- +# +# +# softwarePLL +# +# +cmake_minimum_required(VERSION 2.8) +cmake_policy(SET CMP0015 NEW) +project( softwarePLL ) +# +# +add_definitions(-D${PROJECT_NAME}_MAINTEST) + +MESSAGE( STATUS "CMKAKE for " ${PROJECT_NAME} ) + +include_directories( inc) +file( GLOB LIB_SOURCES src/ *.cpp ) + +if(WIN32) +else() +set(CMAKE_CXX_STANDARD 11) +endif() + +add_executable( ${PROJECT_NAME} ${LIB_SOURCES} inc/${PROJECT_NAME}.h) +target_link_libraries( ${PROJECT_NAME}) +--- CUT --- + +*/ diff --git a/src/client_socket.cpp b/src/client_socket.cpp new file mode 100644 index 0000000..003e4c8 --- /dev/null +++ b/src/client_socket.cpp @@ -0,0 +1,163 @@ +/* + * @brief client_socket encapsulates connecting, closing and setting socket options + * for tcp client sockets using boost::asio::ip::tcp::socket and boost::asio::io_service. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include "sick_lidar_localization/client_socket.h" + +/*! + * Constructor. + * @param[in] io_service boost io service for tcp connections (several sockets may share one io_service) + */ +sick_lidar_localization::ClientSocket::ClientSocket(boost::asio::io_service & io_service) : m_tcp_socket(io_service) +{ +} + +/*! + * Destructor, closes all tcp connections. + */ +sick_lidar_localization::ClientSocket::~ClientSocket() +{ + close(); +} + +/*! + * Connects to a server. + * @param[in] io_service boost io service for tcp connections (several sockets may share one io_service) + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] tcp_port tcp port for command requests, default: 2111 for command requests and 2112 for command responses + * @return true on success, false on failure (server unknown or unreachable) + */ +bool sick_lidar_localization::ClientSocket::connect(boost::asio::io_service & io_service, const std::string & server_adress, int tcp_port) +{ + try + { + // Connect to server + boost::asio::ip::tcp::resolver tcpresolver(io_service); + boost::asio::ip::tcp::resolver::query tcpquery(server_adress, std::to_string(tcp_port)); + boost::asio::ip::tcp::resolver::iterator it = tcpresolver.resolve(tcpquery); + boost::system::error_code errorcode; + m_tcp_socket.connect(*it, errorcode); + + if(!errorcode && m_tcp_socket.is_open()) + { + // Get and set options for client sockets + boost::system::error_code socket_option_errorcodes[3]; + boost::asio::ip::tcp::no_delay socket_option_no_delay; + boost::asio::socket_base::send_buffer_size socket_option_send_buffer_size; + boost::asio::socket_base::receive_buffer_size socket_option_receive_buffer_size; + + m_tcp_socket.get_option(socket_option_no_delay, socket_option_errorcodes[0]); + m_tcp_socket.get_option(socket_option_send_buffer_size, socket_option_errorcodes[1]); + m_tcp_socket.get_option(socket_option_receive_buffer_size, socket_option_errorcodes[2]); + + if (socket_option_errorcodes[0] || socket_option_no_delay.value() == false) + m_tcp_socket.set_option(boost::asio::ip::tcp::no_delay(true), socket_option_errorcodes[0]); + if (socket_option_errorcodes[1] || socket_option_send_buffer_size.value() < 64 * 1024) + m_tcp_socket.set_option(boost::asio::socket_base::send_buffer_size(64 * 1024), socket_option_errorcodes[1]); + if (socket_option_errorcodes[2] || socket_option_receive_buffer_size.value() < 64 * 1024) + m_tcp_socket.set_option(boost::asio::socket_base::receive_buffer_size(64 * 1024), socket_option_errorcodes[2]); + + m_tcp_socket.get_option(socket_option_no_delay, socket_option_errorcodes[0]); + m_tcp_socket.get_option(socket_option_send_buffer_size, socket_option_errorcodes[1]); + m_tcp_socket.get_option(socket_option_receive_buffer_size, socket_option_errorcodes[2]); + + if(socket_option_errorcodes[0] || socket_option_errorcodes[1] || socket_option_errorcodes[2]) + { + ROS_WARN_STREAM("## ClientSocket::connect(): socket connected to " << server_adress << ":" << tcp_port << ", but socket::get_option() failed, " + << " socket options error messages: no_delay=" << socket_option_errorcodes[0].message() << ", send_buffer_size=" << socket_option_errorcodes[1].message() + << ", receive_buffer_size=" << socket_option_errorcodes[2].message()); + } + ROS_INFO_STREAM("ClientSocket::connect(): socket connected to " << server_adress << ":" << tcp_port << ", socket options values: no_delay=" << socket_option_no_delay.value() + << ", send_buffer_size=" << socket_option_send_buffer_size.value() << ", receive_buffer_size=" << socket_option_receive_buffer_size.value()); + } + else + { + ROS_WARN_STREAM("ClientSocket::connect(): no connection to localization controller " << server_adress << ":" << tcp_port << ", error " << errorcode.value() << " \"" << errorcode.message() << "\""); + return false; + } + return true; + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("## ERROR ClientSocket::connect(): connect to " << server_adress << ":" << tcp_port << " failed, exception " << exc.what()); + } + return false; +} + +/*! + * Closes the tcp connection to the server. + * @param[in] force_shutdown if true, the socket is shutdown even if it's state is not opened or connected + * (otherwise the socket is closed, if its state is currently opened) + * @return true on success (socket closed), false on failure + */ +bool sick_lidar_localization::ClientSocket::close(bool force_shutdown) +{ + try + { + if (force_shutdown || m_tcp_socket.is_open()) + { + m_tcp_socket.shutdown(boost::asio::ip::tcp::socket::shutdown_both); + m_tcp_socket.close(); + } + return true; + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("ColaTransmitter::closeTcpConnections(): exception " << exc.what() << " on closing connection."); + } + return false; +} + diff --git a/src/cola_transmitter.cpp b/src/cola_transmitter.cpp index f682e2e..c6fb559 100644 --- a/src/cola_transmitter.cpp +++ b/src/cola_transmitter.cpp @@ -89,21 +89,7 @@ sick_lidar_localization::ColaTransmitter::~ColaTransmitter() */ bool sick_lidar_localization::ColaTransmitter::connect(void) { - try - { - // Connect to localization controller - boost::asio::ip::tcp::resolver tcpresolver(m_ioservice); - boost::asio::ip::tcp::resolver::query tcpquery(m_server_adress, std::to_string(m_tcp_port)); - boost::asio::ip::tcp::resolver::iterator it = tcpresolver.resolve(tcpquery); - boost::system::error_code errorcode; - m_tcp_socket.connect(*it, errorcode); - return !errorcode && m_tcp_socket.is_open(); - } - catch(std::exception & exc) - { - ROS_WARN_STREAM("## ERROR ColaTransmitter::connect(): connect to " << m_server_adress << ":" << m_tcp_port << " failed, exception " << exc.what()); - } - return false; + return m_tcp_socket.connect(m_ioservice, m_server_adress, m_tcp_port); } /*! @@ -114,11 +100,7 @@ bool sick_lidar_localization::ColaTransmitter::close(void) { try { - if (m_tcp_socket.is_open()) - { - m_tcp_socket.shutdown(boost::asio::ip::tcp::socket::shutdown_both); - m_tcp_socket.close(); - } + m_tcp_socket.close(); m_ioservice.stop(); return true; } @@ -137,7 +119,7 @@ bool sick_lidar_localization::ColaTransmitter::close(void) */ bool sick_lidar_localization::ColaTransmitter::send(const std::vector & data, ros::Time & send_timestamp) { - return send(m_tcp_socket, data, send_timestamp); + return send(m_tcp_socket.socket(), data, send_timestamp); } /*! @@ -177,7 +159,7 @@ bool sick_lidar_localization::ColaTransmitter::send(boost::asio::ip::tcp::socket */ bool sick_lidar_localization::ColaTransmitter::receive(std::vector & telegram, double timeout, ros::Time & receive_timestamp) { - return receive(m_tcp_socket, telegram, timeout, receive_timestamp); + return receive(m_tcp_socket.socket(), telegram, timeout, receive_timestamp); } /*! @@ -200,6 +182,7 @@ bool sick_lidar_localization::ColaTransmitter::receive(boost::asio::ip::tcp::soc // Read 1 byte uint8_t byte_received = 0; boost::system::error_code errorcode; + // Possibly better than receiving byte for byte: use boost::asio::read_until to read until received if (socket.available() > 0 && boost::asio::read(socket, boost::asio::buffer(&byte_received, 1), boost::asio::transfer_exactly(1), errorcode) > 0 && !errorcode) { if (telegram.empty()) diff --git a/src/driver_check_thread.cpp b/src/driver_check_thread.cpp index 7b9bcc8..554cafe 100644 --- a/src/driver_check_thread.cpp +++ b/src/driver_check_thread.cpp @@ -73,11 +73,29 @@ sick_lidar_localization::MessageCheckThread::MessageCheckThread() : m_message_check_thread_running(false), m_message_check_thread(0), m_message_check_frequency(100) { // Read configuation + int software_pll_fifo_length = 7, time_sync_initial_length = 10; + double time_sync_rate = 0.1, time_sync_initial_rate = 1.0; ros::param::param("/sick_lidar_localization/sim_loc_driver_check/message_check_frequency", m_message_check_frequency, m_message_check_frequency); + ros::param::param("/sick_lidar_localization/time_sync/software_pll_fifo_length", software_pll_fifo_length, software_pll_fifo_length); + ros::param::param("/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); + ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); + ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_length", time_sync_initial_length, time_sync_initial_length); + assert(software_pll_fifo_length > 0 && time_sync_rate > FLT_EPSILON); + m_software_pll_expected_initialization_duration = (software_pll_fifo_length / time_sync_initial_rate + 1); // expected initialization time for software pll (system time from lidar ticks not yet available) // Read min allowed values in a result port telegrams m_result_port_telegram_min_values = readYamlResultPortTelegram("/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values"); + m_result_port_telegram_min_values.vehicle_time_valid = false; + m_result_port_telegram_min_values.vehicle_time_sec = 0; + m_result_port_telegram_min_values.vehicle_time_nsec = 0; + ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/vehicle_time_delta", m_vehicle_time_delta_min, -1.0); + ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/check_vehicle_time", m_vehicle_time_check_enabled, true); // Read max allowed values in a result port telegrams m_result_port_telegram_max_values = readYamlResultPortTelegram("/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values"); + m_result_port_telegram_max_values.vehicle_time_valid = true; + m_result_port_telegram_max_values.vehicle_time_sec = UINT32_MAX; + m_result_port_telegram_max_values.vehicle_time_nsec = UINT32_MAX; + ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/vehicle_time_delta", m_vehicle_time_delta_max, 1.0); + ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/check_vehicle_time", m_vehicle_time_check_enabled, true); ROS_INFO_STREAM("MessageCheckThread: min allowed values in result port telegrams: " << sick_lidar_localization::Utils::flattenToString(m_result_port_telegram_min_values)); ROS_INFO_STREAM("MessageCheckThread: max allowed values in result port telegrams: " << sick_lidar_localization::Utils::flattenToString(m_result_port_telegram_max_values)); } @@ -182,6 +200,11 @@ sick_lidar_localization::SickLocResultPortTelegramMsg sick_lidar_localization::M std::vector u8_vec; sick_lidar_localization::SickLocResultPortTelegramMsg telegram; + // Default values, can be overwritten by parameter settings + telegram.vehicle_time_valid = false; + telegram.vehicle_time_sec = 0; + telegram.vehicle_time_nsec = 0; + // Read ros header ros::param::param(param_section+"/header/seq", d_value, 0); telegram.header.seq = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // sequence ID, consecutively increasing ID, uint32, size:= 4 byte @@ -298,5 +321,34 @@ bool sick_lidar_localization::MessageCheckThread::checkTelegram(sick_lidar_local CHECK_TELEGRAM_VALUE(telegram_payload.CovarianceX, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && CHECK_TELEGRAM_VALUE(telegram_payload.CovarianceY, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && CHECK_TELEGRAM_VALUE(telegram_payload.CovarianceYaw, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.Reserved3, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values); + CHECK_TELEGRAM_VALUE(telegram_payload.Reserved3, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + (!m_vehicle_time_check_enabled || checkVehicleTime(telegram)); // Check vehicle time (system time from ticks by software pll), if enabled by default +} + +/* + * Checks the vehicle time of a result telegram message (system time from ticks by software pll) against min and max + * allowed difference to ros::Time::now(). Returns true, if test passed (vehicle time within their range), or false otherwise. + * @param[in] telegram result telegram message (SickLocResultPortTelegramMsg) + * @return true, if test passed, false otherwise. + */ + +bool sick_lidar_localization::MessageCheckThread::checkVehicleTime(sick_lidar_localization::SickLocResultPortTelegramMsg & telegram) +{ + if(m_timestamp_valid_telegram.sec <= 0) + m_timestamp_valid_telegram = ros::Time::now(); + if(telegram.vehicle_time_valid) + { + m_timestamp_valid_telegram = ros::Time::now(); + ros::Time message_time(telegram.header.stamp.sec, telegram.header.stamp.nsec); + ros::Time vehicle_time(telegram.vehicle_time_sec,telegram.vehicle_time_nsec); + ros::Duration delta_time = message_time - vehicle_time; + return delta_time.toSec() >= m_vehicle_time_delta_min && delta_time.toSec() <= m_vehicle_time_delta_max; + } + else if((ros::Time::now() - m_timestamp_valid_telegram).toSec() <= m_software_pll_expected_initialization_duration // software pll is initializing + || (std::abs(m_vehicle_time_delta_min) >= FLT_MAX && std::abs(m_vehicle_time_delta_max) >= FLT_MAX)) // checkVehicleTime disabled for error simulation (unreachable controller etc.) + { + return true; // software pll initializing, system time from lidar ticks not yet available + } + return false; // vehicle time out of range } + diff --git a/src/driver_monitor.cpp b/src/driver_monitor.cpp index 12018dd..40ee0e1 100644 --- a/src/driver_monitor.cpp +++ b/src/driver_monitor.cpp @@ -142,7 +142,7 @@ bool sick_lidar_localization::DriverMonitor::initColaTransmitter(const std::stri { if(!m_cola_transmitter) { - m_cola_transmitter = new sick_lidar_localization::ColaTransmitter(server_adress, ip_port_cola); + m_cola_transmitter = new sick_lidar_localization::ColaTransmitter(server_adress, ip_port_cola, receive_timeout); if (!m_cola_transmitter->connect()) { ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: can't connect to localization server " << server_adress << ":" << ip_port_cola); diff --git a/src/driver_thread.cpp b/src/driver_thread.cpp index c50ba5e..94a98ce 100644 --- a/src/driver_thread.cpp +++ b/src/driver_thread.cpp @@ -91,10 +91,19 @@ sick_lidar_localization::DriverThread::DriverThread(ros::NodeHandle * nh, const ros::param::param("/sick_lidar_localization/driver/result_telegrams_frame_id", m_result_telegrams_frame_id, "sick_lidar_localization"); ros::param::param("/sick_lidar_localization/driver/diagnostic_topic", diagnostic_topic, diagnostic_topic); ros::param::param("/sick_lidar_localization/driver/diagnostic_frame_id", m_diagnostic_frame_id, "sick_lidar_localization"); + int software_pll_fifo_length = 7, time_sync_initial_length = 10; + double time_sync_rate = 0.1, time_sync_initial_rate = 1.0; + ros::param::param("/sick_lidar_localization/time_sync/software_pll_fifo_length", software_pll_fifo_length, software_pll_fifo_length); + ros::param::param("/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); + ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); + ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_length", time_sync_initial_length, time_sync_initial_length); + assert(software_pll_fifo_length > 0 && time_sync_rate > FLT_EPSILON); + m_software_pll_expected_initialization_duration = (software_pll_fifo_length / time_sync_initial_rate + 1); // expected initialization time for software pll (system time from lidar ticks not yet available) // ros publisher for result port telegram messages (type SickLocResultPortTelegramMsg) m_result_telegrams_publisher = nh->advertise(result_telegrams_topic, 1); // ros publisher for diagnostic messages (type SickLocDiagnosticMsg) m_diagnostic_publisher = nh->advertise(diagnostic_topic, 1); + m_timesync_service_client = nh->serviceClient("SickLocTimeSync"); m_initialized = true; } } @@ -210,21 +219,7 @@ bool sick_lidar_localization::DriverThread::isRunning(void) void sick_lidar_localization::DriverThread::closeTcpConnections(bool force_shutdown) { m_tcp_connected = false; - try - { - if (force_shutdown || m_tcp_socket.is_open()) - { - publishDiagnosticMessage(NO_ERROR, "sim_loc_driver: closing socket"); - ROS_INFO_STREAM("DriverThread::closeTcpConnections(force_shutdown=" << force_shutdown << "): shutdown socket"); - m_tcp_socket.shutdown(boost::asio::ip::tcp::socket::shutdown_both); - ROS_INFO_STREAM("DriverThread::closeTcpConnections(force_shutdown=" << force_shutdown << "): close socket"); - m_tcp_socket.close(); - } - } - catch(std::exception & exc) - { - ROS_WARN_STREAM("DriverThread::closeTcpConnections(): exception " << exc.what() << " on closing connection."); - } + m_tcp_socket.close(force_shutdown); } /* @@ -240,12 +235,7 @@ void sick_lidar_localization::DriverThread::runReceiverThreadCb(void) // Connect to localization controller while(ros::ok() && m_tcp_receiver_thread_running && !m_tcp_connected) { - boost::asio::ip::tcp::resolver tcpresolver(m_ioservice); - boost::asio::ip::tcp::resolver::query tcpquery(m_server_adress, std::to_string(m_tcp_port)); - boost::asio::ip::tcp::resolver::iterator it = tcpresolver.resolve(tcpquery); - boost::system::error_code errorcode; - m_tcp_socket.connect(*it, errorcode); - if(!errorcode && m_tcp_socket.is_open()) + if(m_tcp_socket.connect(m_ioservice, m_server_adress, m_tcp_port)) { m_tcp_connected = true; publishDiagnosticMessage(NO_ERROR, std::string("sim_loc_driver: tcp connection established to localization controller ") + m_server_adress + ":" + std::to_string(m_tcp_port)); @@ -254,30 +244,29 @@ void sick_lidar_localization::DriverThread::runReceiverThreadCb(void) else { publishDiagnosticMessage(NO_TCP_CONNECTION, std::string("sim_loc_driver: no tcp connection to localization controller ") + m_server_adress + ":" + std::to_string(m_tcp_port)); - ROS_WARN_STREAM("DriverThread: no connection to localization controller " << m_server_adress << ":" << m_tcp_port - << ", error " << errorcode.value() << " \"" << errorcode.message() << "\", retry in " << m_tcp_connection_retry_delay << " seconds"); + ROS_WARN_STREAM("DriverThread: no connection to localization controller " << m_server_adress << ":" << m_tcp_port << ", retry in " << m_tcp_connection_retry_delay << " seconds"); ros::Duration(m_tcp_connection_retry_delay).sleep(); } } // Receive binary telegrams from localization controller size_t telegram_size = sick_lidar_localization::TestcaseGenerator::createDefaultResultPortTestcase().binary_data.size(); // 106 byte result port telegrams - while(ros::ok() && m_tcp_receiver_thread_running && m_tcp_socket.is_open()) + while(ros::ok() && m_tcp_receiver_thread_running && m_tcp_socket.socket().is_open()) { size_t bytes_received = 0, bytes_required = telegram_size; std::vector receive_buffer(bytes_required, 0); boost::system::error_code errorcode; std::string error_info(""); - while(ros::ok() && m_tcp_receiver_thread_running && m_tcp_socket.is_open() && bytes_received < bytes_required) + while(ros::ok() && m_tcp_receiver_thread_running && m_tcp_socket.socket().is_open() && bytes_received < bytes_required) { size_t bytes_to_read = bytes_required - bytes_received; - bytes_received += boost::asio::read(m_tcp_socket, boost::asio::buffer(&receive_buffer[bytes_received],bytes_to_read), boost::asio::transfer_exactly(bytes_to_read), errorcode); + bytes_received += boost::asio::read(m_tcp_socket.socket(), boost::asio::buffer(&receive_buffer[bytes_received],bytes_to_read), boost::asio::transfer_exactly(bytes_to_read), errorcode); if(errorcode) { std::stringstream error_info_stream; error_info_stream << "DriverThread: tcp socket read errorcode " << errorcode.value() << " \"" << errorcode.message() << "\""; if(error_info != error_info_stream.str()) { - publishDiagnosticMessage(NO_TCP_CONNECTION, std::string("sim_loc_driver: tcp socket read errorcode") + std::to_string(errorcode.value()) + ", " + errorcode.message()); + publishDiagnosticMessage(NO_TCP_CONNECTION, std::string("sim_loc_driver: tcp socket read errorcode ") + std::to_string(errorcode.value()) + ", " + errorcode.message()); ROS_WARN_STREAM(error_info_stream.str()); } error_info = error_info_stream.str(); @@ -318,6 +307,7 @@ void sick_lidar_localization::DriverThread::runConverterThreadCb(void) ROS_INFO_STREAM("DriverThread: converter thread started"); // Decode and publish result port telegrams ros::Time diagnostic_msg_published; + ros::Time timestamp_first_telegram; sick_lidar_localization::ResultPortParser result_port_parser(m_result_telegrams_frame_id); while(ros::ok() && m_converter_thread_running) { @@ -338,8 +328,27 @@ void sick_lidar_localization::DriverThread::runConverterThreadCb(void) } else { + sick_lidar_localization::SickLocResultPortTelegramMsg & result_telegram = result_port_parser.getTelegramMsg(); + // Query system time of vehicle pose from lidar tick, using software pll with ros service "SickLocTimeSync" + result_telegram.vehicle_time_valid = false; + result_telegram.vehicle_time_sec = 0; + result_telegram.vehicle_time_nsec = 0; + if(timestamp_first_telegram.sec <= 0) + timestamp_first_telegram = ros::Time::now(); + sick_lidar_localization::SickLocTimeSyncSrv time_sync_msg; + time_sync_msg.request.timestamp_lidar_ms = result_telegram.telegram_payload.Timestamp; + if (m_timesync_service_client.call(time_sync_msg) && time_sync_msg.response.vehicle_time_valid) + { + result_telegram.vehicle_time_valid = time_sync_msg.response.vehicle_time_valid; + result_telegram.vehicle_time_sec = time_sync_msg.response.vehicle_time_sec; + result_telegram.vehicle_time_nsec = time_sync_msg.response.vehicle_time_nsec; + ROS_DEBUG_STREAM("sim_loc_driver: Lidar ticks: " << result_telegram.telegram_payload.Timestamp << ", Systemtime by pll: " << result_telegram.vehicle_time_sec << "." << result_telegram.vehicle_time_sec); + } + else if((ros::Time::now() - timestamp_first_telegram).toSec() <= m_software_pll_expected_initialization_duration) // software pll still initializing + ROS_DEBUG_STREAM("sim_loc_driver: no system time from ticks, software pll still initializing"); + else // time sync error + ROS_WARN_STREAM("## ERROR sim_loc_driver: service \"SickLocTimeSync\" failed, could not get system time from ticks"); // Publish the decoded result port telegram (type SickLocResultPortTelegramMsg) - sick_lidar_localization::SickLocResultPortTelegramMsg &result_telegram = result_port_parser.getTelegramMsg(); m_result_telegrams_publisher.publish(result_telegram); ROS_INFO_STREAM("DriverThread: result telegram received " << sick_lidar_localization::Utils::toHexString(binary_telegram) << ", published " << sick_lidar_localization::Utils::flattenToString(result_telegram)); if( (ros::Time::now() - diagnostic_msg_published).toSec() >= 60) diff --git a/src/pointcloud_converter_thread.cpp b/src/pointcloud_converter_thread.cpp index f01c478..d955585 100644 --- a/src/pointcloud_converter_thread.cpp +++ b/src/pointcloud_converter_thread.cpp @@ -180,7 +180,12 @@ sensor_msgs::PointCloud2 sick_lidar_localization::PointCloudConverter::convertTo 1.0e-3 * msg.telegram_payload.PoseYaw * M_PI / 180.0, // yaw angle in radians 0.6); // triangle_height in meter (demo only) // set pointcloud header - pointcloud_msg.header.stamp = msg.header.stamp; // timestamp of telegram + pointcloud_msg.header.stamp = msg.header.stamp; // telegram timestamp + if(msg.vehicle_time_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll + { + pointcloud_msg.header.stamp.sec = msg.vehicle_time_sec; + pointcloud_msg.header.stamp.nsec = msg.vehicle_time_nsec; + } pointcloud_msg.header.frame_id = m_point_cloud_frame_id; pointcloud_msg.header.seq = 0; // clear cloud data @@ -227,7 +232,12 @@ geometry_msgs::TransformStamped sick_lidar_localization::PointCloudConverter::co double posy = 1.0e-3 * msg.telegram_payload.PoseY; // y-position in meter double yaw = 1.0e-3 * msg.telegram_payload.PoseYaw * M_PI / 180.0; // yaw angle in radians geometry_msgs::TransformStamped vehicle_transform; - vehicle_transform.header.stamp = ros::Time::now(); + vehicle_transform.header.stamp = msg.header.stamp; // telegram timestamp + if(msg.vehicle_time_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll + { + vehicle_transform.header.stamp.sec = msg.vehicle_time_sec; + vehicle_transform.header.stamp.nsec = msg.vehicle_time_nsec; + } vehicle_transform.header.frame_id = m_tf_parent_frame_id; vehicle_transform.child_frame_id = m_tf_child_frame_id; vehicle_transform.transform.translation.x = posx; diff --git a/src/testcase_generator.cpp b/src/testcase_generator.cpp index 0b16a20..1dc42a6 100644 --- a/src/testcase_generator.cpp +++ b/src/testcase_generator.cpp @@ -60,6 +60,9 @@ #include "sick_lidar_localization/testcase_generator.h" #include "sick_lidar_localization/utils.h" + +uint32_t sick_lidar_localization::TestcaseGenerator::s_u32ResultPoseInterval = 1; ///< result pose interval, i.e. the interval in number of scans (default: 1, i.e. result telegram with each processed scan) + /*! * Creates and returns a deterministic default testcase for result port telegrams (binary telegrams and SickLocResultPortTelegramMsg) * @return SickLocResultPortTestcaseMsg with the binary telegram and SickLocResultPortTelegramMsg @@ -137,8 +140,8 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T // Update telegram timestamps double delta_time_seconds = (ros::Time::now() - start_time).toSec(); - telegram_msg.telegram_payload.Timestamp += (uint32_t)(1000.0 * delta_time_seconds); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte - telegram_msg.telegram_header.SystemTime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte + telegram_msg.telegram_payload.Timestamp = createTimestampTicksMilliSec(); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte + telegram_msg.telegram_header.SystemTime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte // Re-encode the modified result port telegram (SickLocResultPortTelegramMsg) sick_lidar_localization::ResultPortParser result_port_parser(testcase.header.frame_id); @@ -180,7 +183,7 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T // Update telegram timestamps double delta_time_seconds = (ros::Time::now() - start_time).toSec(); telegram_msg.telegram_payload.Timestamp = createTimestampTicksMilliSec(); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte - telegram_msg.telegram_header.SystemTime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte + telegram_msg.telegram_header.SystemTime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte // Re-encode the modified result port telegram (SickLocResultPortTelegramMsg) sick_lidar_localization::ResultPortParser result_port_parser(testcase.header.frame_id); @@ -206,12 +209,6 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T */ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::TestcaseGenerator::createColaResponse(const sick_lidar_localization::SickLocColaTelegramMsg & cola_request) { - // Generate a synthetical response to LocMapState requests: "sRA LocMapState 1" - if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocMapState") - { - return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {"1"}); - } - // Generate a synthetical response to LocRequestTimestamp requests: "sAN LocRequestTimestamp " with uint32_t timestamp in hex and ms if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocRequestTimestamp") { @@ -222,9 +219,95 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::Testcas uint32_t ticks_ms = createTimestampTicksMilliSec(); // Simulate some network latency ros::Duration(0.001 * time_jitter_network_ms.generate()).sleep(); - std::stringstream timestamp_hex; - timestamp_hex << std::hex << std::uppercase << ticks_ms; - return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {timestamp_hex.str()}); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {hexstr(ticks_ms)}); + } + + // static test server settings (sMN or sRN requests) + // tbd: export settings by function LocalizationEnabled(), ResultTelegramsEnabled(), ResultTelegramsBigEndian(), + // and use test server settings in test_server_thread to configure result telegrams: + // s_controller_settings["LocState"]==2 && s_controller_settings["LocResultState"]>0: localization on and result telegrams activated, otherwise result telegrams deactivated + // s_controller_settings["LocResultEndianness"]==0: big endian (default), s_controller_settings["LocResultEndianness"]>0: little endian + static std::map s_controller_settings = { + {"IsSystemReady", 1}, // 0:false, 1:true (default) + {"LocState", 2}, // controller state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + {"LocResultPort", 2201}, // tcp port for result telegrams (default: 2201) + {"LocResultMode", 0}, // 0:stream (default), 1:poll + {"LocResultState", 1}, // result output: 0: disabled, 1: enabled + {"LocResultEndianness", 0}, // 0: big endian (default), 1: little endian + {"LocMapState", 1}, // map state: 0:not active, 1:active + {"LocRequestResultData", 1} // in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + }; + + // Set settings from Configuration Telegrams + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocStartLocalizing") + { + s_controller_settings["LocState"] = 2; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocStop") + { + s_controller_settings["LocState"] = 1; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocStopAndSave") + { + s_controller_settings["LocState"] = 1; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetResultPort" && cola_request.parameter.size() == 1) + { + s_controller_settings["LocResultPort"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetResultMode" && cola_request.parameter.size() == 1) + { + s_controller_settings["LocResultMode"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetResultPoseEnabled" && cola_request.parameter.size() == 1) + { + s_controller_settings["LocResultState"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetResultEndianness" && cola_request.parameter.size() == 1) + { + s_controller_settings["LocResultEndianness"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0);; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetPose" && cola_request.parameter.size() == 4) + { + int32_t posex_mm = std::strtol(cola_request.parameter[0].c_str(), 0, 0); + int32_t posey_mm = std::strtol(cola_request.parameter[1].c_str(), 0, 0); + int32_t yaw_mdeg = std::strtol(cola_request.parameter[2].c_str(), 0, 0); + int32_t uncertainty = std::strtol(cola_request.parameter[3].c_str(), 0, 0); + bool success = (posex_mm >= -300000 && posex_mm <= +300000 && posey_mm >= -300000 && posey_mm <= +300000 + && yaw_mdeg >= -180000 && yaw_mdeg <= +180000 && uncertainty >= 0 && uncertainty < 300000); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(success?1:0)}); + } + + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetResultPoseInterval" && cola_request.parameter.size() == 1) + { + s_u32ResultPoseInterval = std::strtoul(cola_request.parameter[0].c_str(), 0, 0); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + // Create sAN responses to sMN requests resp. sRA responses to sRN requests + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN || cola_request.command_type == sick_lidar_localization::ColaParser::sRN) + { + sick_lidar_localization::ColaParser::COLA_SOPAS_COMMAND response_type = sick_lidar_localization::ColaParser::sINVALID; + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN) + response_type = sick_lidar_localization::ColaParser::sAN; // sAN responses to sMN requests + else if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN) + response_type = sick_lidar_localization::ColaParser::sRA; // sRA responses to sRN requests + for(std::map::iterator iter_settings = s_controller_settings.begin(); iter_settings != s_controller_settings.end(); iter_settings++) + { + if(cola_request.command_name == iter_settings->first) + { + return sick_lidar_localization::ColaParser::createColaTelegram(response_type, cola_request.command_name, {decstr(iter_settings->second)}); + } + } } // Default response: "sAN " without parameter (sAN: Response to sMN) diff --git a/src/time_sync.cpp b/src/time_sync.cpp new file mode 100644 index 0000000..815d962 --- /dev/null +++ b/src/time_sync.cpp @@ -0,0 +1,104 @@ +/* + * @brief time_sync advertises and runs ros services "SickLocRequestTimestamp" and "SickLocTimeSync" + * for time synchronization. + * + * ROS service SickLocRequestTimestamp requests a timestamp from the localization controller + * by sending cola command LocRequestTimestamp ("sMN LocRequestTimestamp"). + * + * The service receives and decodes the current timestamp (uint32 timestamp in milliseconds) + * and calculates the time offset with the following formular: + * + * delta_time_ms := mean_time_vehicle_ms - timestamp_lidar_ms + * mean_time_vehicle_ms := (send_time_vehicle + receive_time_vehicle) / 2 + * := vehicles mean timestamp in milliseconds + * send_time_vehicle := vehicles timestamp when sending LocRequestTimestamp + * receive_time_vehicle := vehicles timestamp when receiving the LocRequestTimestamp response + * timestamp_lidar_ms := lidar timestamp in milliseconds from LocRequestTimestamp response + * + * See Operation-Instruction-v1.1.0.241R.pdf for details about time synchronization and + * time offset calculation. See Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF + * for further details about Cola telegram LocRequestTimestamp. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include "sick_lidar_localization/time_sync_service.h" + +int main(int argc, char** argv) +{ + // Ros configuration and initialization + ros::init(argc, argv, "time_sync"); + ros::NodeHandle nh; + ROS_INFO_STREAM("time_sync started."); + + // Initialize TimeSyncService + sick_lidar_localization::TimeSyncService time_sync_service(&nh); + + // Start time synchronization thread to run the software pll + if(!time_sync_service.start()) + { + ROS_ERROR_STREAM("## ERROR time_sync: could not start synchronization thread, exiting"); + return EXIT_FAILURE; + } + + // Run ros event loop + ros::spin(); + + // Cleanup and exit + std::cout << "time_sync finished." << std::endl; + ROS_INFO_STREAM("time_sync finished."); + time_sync_service.stop(); + std::cout << "time_sync exits." << std::endl; + ROS_INFO_STREAM("time_sync exits."); + return EXIT_SUCCESS; +} diff --git a/src/time_sync_service.cpp b/src/time_sync_service.cpp new file mode 100644 index 0000000..65f0b47 --- /dev/null +++ b/src/time_sync_service.cpp @@ -0,0 +1,338 @@ +/* + * @brief time_sync_service implements ros services "SickLocRequestTimestamp" and "SickLocTimeSync" + * for time synchronization. + * + * ROS service SickLocRequestTimestamp requests a timestamp from the localization controller + * by sending cola command LocRequestTimestamp ("sMN LocRequestTimestamp"). + * + * The service receives and decodes the current timestamp (uint32 timestamp in milliseconds) + * and calculates the time offset with the following formular: + * + * delta_time_ms := mean_time_vehicle_ms - timestamp_lidar_ms + * mean_time_vehicle_ms := (send_time_vehicle + receive_time_vehicle) / 2 + * := vehicles mean timestamp in milliseconds + * send_time_vehicle := vehicles timestamp when sending LocRequestTimestamp + * receive_time_vehicle := vehicles timestamp when receiving the LocRequestTimestamp response + * timestamp_lidar_ms := lidar timestamp in milliseconds from LocRequestTimestamp response + * + * See Operation-Instruction-v1.1.0.241R.pdf for details about time synchronization and + * time offset calculation. See Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF + * for further details about Cola telegram LocRequestTimestamp. + * + * time_sync_service implements a time synchronization thread, running a software pll + * to estimate system time from lidar timestamp ticks. This synchronization thread can + * be started by function start() and stopped by function stop(). When running the + * synchronization thread, ros services "SickLocRequestTimestamp" is called each 10 seconds. + * The software pll is then updated to estimate the system time from lidar timestamp ticks. + * The system timestamp of a vehicle pose can be queried using ros service + * "SickLocTimeSync". + * + * Note: The software pll uses a default fifo size of 7 measurements, thus it requires at least 7 successful + * LocRequestTimestamp requests. Depending on time_sync_rate configured in the launch-file, this initial phase can + * take several seconds (up to 70 seconds). + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include "sick_lidar_localization/cola_parser.h" +#include "sick_lidar_localization/SoftwarePLL.h" +#include "sick_lidar_localization/time_sync_service.h" + +/*! + * Constructor + */ +sick_lidar_localization::TimeSyncService::TimeSyncService(ros::NodeHandle* nh) +: m_time_sync_thread_running(false), m_time_sync_thread(0), m_cola_binary(false), m_cola_binary_mode(0), m_software_pll_fifo_length(7), + m_time_sync_rate(0.1), m_time_sync_initial_rate(1.0), m_time_sync_initial_length(10), m_cola_response_timeout(1.0) +{ + if(nh) + { + // Configuration and parameter + ros::param::param("/sick_lidar_localization/driver/cola_binary", m_cola_binary_mode, m_cola_binary_mode); + m_cola_binary = (m_cola_binary_mode == 1) ? true : false; // 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) + ros::param::param("/sick_lidar_localization/time_sync/software_pll_fifo_length", m_software_pll_fifo_length, m_software_pll_fifo_length); + double time_sync_rate = 0.1, time_sync_initial_rate = 1.0; + ros::param::param("/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); + ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); + m_time_sync_rate = ros::Rate(time_sync_rate); + m_time_sync_initial_rate = ros::Rate(time_sync_initial_rate); + ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_length", m_time_sync_initial_length, m_time_sync_initial_length); + ros::param::param("/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); + // Advertise service "SickLocRequestTimestamp" to send a LocRequestTimestamp, receive the response and to calculate the time offset + m_timestamp_service_server = nh->advertiseService("SickLocRequestTimestamp", &sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp, this); + ROS_INFO_STREAM("TimeSyncService: advertising ros service \"SickLocRequestTimestamp\" for LocRequestTimestamp commands, message type SickLocRequestTimestamp"); + // Advertise service "SickLocTimeSync" to calculate system time from ticks by software pll + m_timesync_service_server = nh->advertiseService("SickLocTimeSync", &sick_lidar_localization::TimeSyncService::serviceCbTimeSync, this); + ROS_INFO_STREAM("TimeSyncService: advertising ros service \"SickLocTimeSync\" for time synchronization by software pll, message type SickLocTimeSync"); + // Clients for ros services "SickLocColaTelegram", "SickLocRequestTimestamp" and "SickLocTimeSync", required for time synchronization using a software pll + m_cola_service_client = nh->serviceClient("SickLocColaTelegram"); + m_request_timestamp_client = nh->serviceClient("SickLocRequestTimestamp"); + } +} + +/*! + * Destructor + */ +sick_lidar_localization::TimeSyncService::~TimeSyncService() +{ + stop(); +} + +/*! + * Callback for service messages (SickLocRequestTimestamp). Sends LocRequestTimestamp requests + * to the localization controller, receives the response and calculates the time offset. + * @param[in] service_request ros service request + * @param[out] service_response service response with timestamps and calculated time offset. + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_lidar_localization::SickLocRequestTimestampSrv::Request & service_request, sick_lidar_localization::SickLocRequestTimestampSrv::Response & service_response) +{ + // Sends cola command "sMN LocRequestTimestamp" and receive timestamp from localization controller using ros service "SickLocColaTelegram" + sick_lidar_localization::SickLocColaTelegramSrv cola_telegram; + cola_telegram.request.cola_ascii_request = "sMN LocRequestTimestamp"; + cola_telegram.request.wait_response_timeout = m_cola_response_timeout; + // cola_telegram.request.send_binary = m_cola_binary; + m_cola_binary = ((m_cola_binary_mode == 2) ? (!m_cola_binary) : (m_cola_binary)); // m_cola_binary_mode == 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) + if (!m_cola_service_client.call(cola_telegram) || cola_telegram.response.cola_ascii_response.empty()) + { + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): calling ros service \"SickLocColaTelegram\" failed with request: " + << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) << " response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram.response)); + return false; + } + ROS_DEBUG_STREAM("TimeSyncService::serviceCbRequestTimestamp(): request " << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) + << " response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram.response) << " succesfull."); + + // Decode response, get timestamp_lidar_ms from parameter + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram.response.cola_ascii_response); + if(cola_response.command_name != "LocRequestTimestamp" || cola_response.parameter.size() != 1) + { + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): invalid or unexpected cola response: \"" << cola_telegram.response.cola_ascii_response + << "\" decoded to " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; + } + try + { + service_response.timestamp_lidar_ms = (std::stoul(cola_response.parameter[0],nullptr,16) & 0xFFFFFFFF); // Lidar timestamp in milliseconds from LocRequestTimestamp response + } + catch(const std::exception & exc) + { + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): failed to parse timestamp parameter in cola response " << sick_lidar_localization::Utils::flattenToString(cola_response) + << ", exception " << exc.what()); + return false; + } + if(service_response.timestamp_lidar_ms <= 0 || cola_telegram.response.send_timestamp_sec <= 0 || cola_telegram.response.receive_timestamp_sec <= 0) + { + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): invalid timestamps in cola response " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; + } + + // Set timestamps in service_response + service_response.send_time_vehicle_sec = cola_telegram.response.send_timestamp_sec; // Vehicle timestamp when sending LocRequestTimestamp (seconds part of ros timestamp immediately before tcp send) + service_response.send_time_vehicle_nsec = cola_telegram.response.send_timestamp_nsec; // Vehicle timestamp when sending LocRequestTimestamp (nano seconds part of ros timestamp immediately before tcp send) + service_response.receive_time_vehicle_sec = cola_telegram.response.receive_timestamp_sec; // Vehicle timestamp when receiving the LocRequestTimestamp response (seconds part of ros timestamp immediately after first response byte received) + service_response.receive_time_vehicle_nsec = cola_telegram.response.receive_timestamp_nsec; // Vehicle timestamp when receiving the LocRequestTimestamp response (nano seconds part of ros timestamp immediately after first response byte received) + + // Calculate time offset + uint64_t send_time_vehicle_nsec = service_response.send_time_vehicle_sec * 1000000000UL + service_response.send_time_vehicle_nsec; + uint64_t receive_time_vehicle_nsec = service_response.receive_time_vehicle_sec * 1000000000UL + service_response.receive_time_vehicle_nsec; + uint64_t mean_time_vehicle_nsec = send_time_vehicle_nsec / 2 + receive_time_vehicle_nsec / 2; + service_response.mean_time_vehicle_ms = mean_time_vehicle_nsec / 1000000; // Vehicle mean timestamp in milliseconds: (send_time_vehicle + receive_time_vehicle) / 2 + service_response.delta_time_ms = service_response.mean_time_vehicle_ms - service_response.timestamp_lidar_ms; // Time offset: mean_time_vehicle_ms - timestamp_lidar_ms + + // Update software pll + updateSoftwarePll(service_response); + + // Get system timestamp from ticks via ros service "SickLocTimeSync" + sick_lidar_localization::SickLocTimeSyncSrv time_sync_msg; + time_sync_msg.request.timestamp_lidar_ms = service_response.timestamp_lidar_ms; + if(serviceCbTimeSync(time_sync_msg.request, time_sync_msg.response) && time_sync_msg.response.vehicle_time_valid) + ROS_INFO_STREAM("TimeSyncService::serviceCbRequestTimestamp(): Lidar ticks: " << service_response.timestamp_lidar_ms << ", Systemtime: " << time_sync_msg.response.vehicle_time_sec << "." << time_sync_msg.response.vehicle_time_sec); + else if(isSoftwarePllInitialized()) + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): service \"SickLocTimeSync\" failed, could not get system time from ticks"); + else + ROS_INFO_STREAM("TimeSyncService::serviceCbRequestTimestamp(): no system time from ticks, software pll still initializing"); + + return true; +} + +/*! + * Update the software pll with lidar ticks and system time from response by "SickLocRequestTimestamp" servcice + * @param[out] service_response service response by "SickLocRequestTimestamp" servcice + */ +void sick_lidar_localization::TimeSyncService::updateSoftwarePll(sick_lidar_localization::SickLocRequestTimestampSrv::Response & service_response) +{ + boost::lock_guard software_pll_lockguard(m_software_pll_mutex); + SoftwarePLL & software_pll_send_time = SoftwarePLL::Instance("sick_lidar_localization::TimeSyncService::SendTime", m_software_pll_fifo_length); + SoftwarePLL & software_pll_receive_time = SoftwarePLL::Instance("sick_lidar_localization::TimeSyncService::ReceiveTime", m_software_pll_fifo_length); + software_pll_send_time.UpdatePLL(service_response.send_time_vehicle_sec, service_response.send_time_vehicle_nsec, service_response.timestamp_lidar_ms); + software_pll_receive_time.UpdatePLL(service_response.receive_time_vehicle_sec, service_response.receive_time_vehicle_nsec, service_response.timestamp_lidar_ms); +} + +/*! + * Returns true, if the initialization phase of the software pll is completed, otherwise false. + * @return initialization phase of the software pll completed + */ +bool sick_lidar_localization::TimeSyncService::isSoftwarePllInitialized(void) +{ + boost::lock_guard software_pll_lockguard(m_software_pll_mutex); + SoftwarePLL & software_pll_send_time = SoftwarePLL::Instance("sick_lidar_localization::TimeSyncService::SendTime", m_software_pll_fifo_length); + SoftwarePLL & software_pll_receive_time = SoftwarePLL::Instance("sick_lidar_localization::TimeSyncService::ReceiveTime", m_software_pll_fifo_length); + return software_pll_send_time.IsInitialized() && software_pll_receive_time.IsInitialized(); +} + + +/*! + * Callback for service messages (SickLocTimeSync). Calculates the system time of a vehicle pose from lidar ticks, + * using a software pll. + * Note: ros service SickLocTimeSync uses service SickLocRequestTimestamp to query lidar ticks and to update the + * software pll. Therefore, at least 7 LocRequestTimestamp measurements are required before time sync becomes valid + * The default fifo size of the software pll is 7 measurements, thus it requires at least 7 successful + * LocRequestTimestamp requests. Depending on time_sync_rate configured in the launch-file, this initial phase can + * take several seconds (up to 70 seconds). + * @param[in] time_sync_request ros service request (input: lidar ticks) + * @param[out] time_sync_response service response (output: system time from ticks, calculated by software pll) + * @return true on success, false in case of errors (software pll still in initialization phase or communication error). + */ +bool sick_lidar_localization::TimeSyncService::serviceCbTimeSync(sick_lidar_localization::SickLocTimeSyncSrv::Request & time_sync_request, sick_lidar_localization::SickLocTimeSyncSrv::Response & time_sync_response) +{ + boost::lock_guard software_pll_lockguard(m_software_pll_mutex); + SoftwarePLL & software_pll_send_time = SoftwarePLL::Instance("sick_lidar_localization::TimeSyncService::SendTime", m_software_pll_fifo_length); + SoftwarePLL & software_pll_receive_time = SoftwarePLL::Instance("sick_lidar_localization::TimeSyncService::ReceiveTime", m_software_pll_fifo_length); + time_sync_response.vehicle_time_sec = 0; + time_sync_response.vehicle_time_nsec = 0; + time_sync_response.vehicle_time_valid = false; + uint32_t ticks = time_sync_request.timestamp_lidar_ms; + uint32_t system_timestamp_1_sec = 0, system_timestamp_1_nsec = 0, system_timestamp_2_sec = 0, system_timestamp_2_nsec = 0; + if(software_pll_send_time.GetCorrectedTimeStamp(system_timestamp_1_sec, system_timestamp_1_nsec, ticks) + && software_pll_receive_time.GetCorrectedTimeStamp(system_timestamp_2_sec, system_timestamp_2_nsec, ticks)) + { + ros::Time system_timestamp_1(system_timestamp_1_sec, system_timestamp_1_nsec); + ros::Time system_timestamp_2(system_timestamp_2_sec, system_timestamp_2_nsec); + ros::Time system_timestamp = system_timestamp_1 + ros::Duration(0.5 * (system_timestamp_2 - system_timestamp_1).toSec()); + time_sync_response.vehicle_time_sec = system_timestamp.sec; + time_sync_response.vehicle_time_nsec = system_timestamp.nsec; + time_sync_response.vehicle_time_valid = true; + ROS_INFO_STREAM("TimeSyncService::serviceCbTimeSync(): Lidar ticks: " << ticks << ", Systemtime: " << time_sync_response.vehicle_time_sec << "." << time_sync_response.vehicle_time_nsec); + } + else if(software_pll_send_time.IsInitialized() && software_pll_receive_time.IsInitialized()) + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbTimeSync(): SoftwarePLL::GetCorrectedTimeStamp() failed"); + else + ROS_INFO_STREAM("TimeSyncService::serviceCbTimeSync(): no system time from ticks, software pll still initializing"); + return time_sync_response.vehicle_time_valid; +} + + +/*! + * Class TimeSyncService implements a time synchronization thread, running a software pll + * to estimate system time from lidar timestamp ticks. This synchronization thread can + * be started by function start() and stopped by function stop(). When running the + * synchronization thread, ros services "SickLocRequestTimestamp" is called each 10 seconds. + * The software pll is then updated to estimate the system time from lidar timestamp ticks. + * The system timestamp of a vehicle pose can be queried using ros service + * "SickLocTimeSync". + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::TimeSyncService::start(void) +{ + m_time_sync_thread_running = true; + m_time_sync_thread = new boost::thread(&sick_lidar_localization::TimeSyncService::runTimeSyncThreadCb, this); + return (m_time_sync_thread != 0); +} + +/*! + * Stops the time synchronization thread and the software pll. + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::TimeSyncService::stop(void) +{ + m_time_sync_thread_running = false; + if(m_time_sync_thread) + { + m_time_sync_thread->join(); + delete(m_time_sync_thread); + m_time_sync_thread = 0; + } + return (m_time_sync_thread == 0); +} + +/*! + * Thread callback, runs time synchronization, calls ros service "SickLocRequestTimestamp" each 10 seconds + * and updates the software pll. + */ +void sick_lidar_localization::TimeSyncService::runTimeSyncThreadCb(void) +{ + int time_sync_cnt = 0; + while(ros::ok() && m_time_sync_thread_running) + { + // Run LocRequestTimestamp every second during initialization of software pll, otherwise every 10 seconds + ros::Rate & time_sync_rate = ((time_sync_cnt < m_time_sync_initial_length) ? m_time_sync_initial_rate : m_time_sync_rate); + time_sync_rate.sleep(); + if(ros::ok() && m_time_sync_thread_running) + { + // Call ros service "SickLocRequestTimestamp" + sick_lidar_localization::SickLocRequestTimestampSrv timestamp_service; + if (!m_request_timestamp_client.call(timestamp_service) || timestamp_service.response.timestamp_lidar_ms == 0) + { + ROS_WARN_STREAM("## ERROR TimeSyncService::runTimeSyncThreadCb(): calling ros service \"SickLocRequestTimestamp\" failed, response: " + << sick_lidar_localization::Utils::flattenToString(timestamp_service.response)); + } + else + { + time_sync_cnt++; + ROS_INFO_STREAM("TimeSyncService::runTimeSyncThreadCb(): ros service \"SickLocRequestTimestamp\" successfull, response: " + << sick_lidar_localization::Utils::flattenToString(timestamp_service.response)); + } + } + } + m_time_sync_thread_running = false; +} diff --git a/srv/SickLocColaTelegramSrv.srv b/srv/SickLocColaTelegramSrv.srv index 49d4cc8..a6023aa 100644 --- a/srv/SickLocColaTelegramSrv.srv +++ b/srv/SickLocColaTelegramSrv.srv @@ -11,7 +11,7 @@ string cola_ascii_request # Cola-ASCII telegram to send to the localization controller, f.e. "sRN SetAccessMode 3 F4724744". float32 wait_response_timeout # Timeout for the controller response in secondes. Use 0.0 for Cola requests without controller response. -# bool send_binary # Not used (true: send binary telegram, false/default: send ASCII telegram) +# bool send_binary # Not used (true: send binary telegram, false/default: send ASCII telegram) --- diff --git a/srv/SickLocTimeSyncSrv.srv b/srv/SickLocTimeSyncSrv.srv new file mode 100644 index 0000000..d9c7ebc --- /dev/null +++ b/srv/SickLocTimeSyncSrv.srv @@ -0,0 +1,23 @@ +# Definition of ROS service SickLocTimeSync for sick localization. +# +# ROS service SickLocTimeSync returns the system time of a vehicle pose from lidar ticks in ms. +# SickLocTimeSync uses a software pll to calculate the system time from ticks. Therefore at least +# 7 LocRequestTimestamp measurements are required before time sync becomes valid (default fifo +# size of the software pll is 7 measurements). Depending on time_sync_rate configured in the +# launch-file, this initial phase can take several seconds (up to 70 seconds). + +# +# Request (input): Timestamp in ticks +# + +uint32 timestamp_lidar_ms # Lidar timestamp in milliseconds from LocRequestTimestamp response + +--- + +# +# Response (output): System time calculated by software pll +# + +bool vehicle_time_valid # true: vehicle_time_sec and vehicle_time_nsec valid, false: software pll still in initial phase +uint32 vehicle_time_sec # Time of vehicles pose calculated by software pll (seconds part of the system time) +uint32 vehicle_time_nsec # Time of vehicles pose calculated by software pll (nano seconds part of the system time) diff --git a/test/include/sick_lidar_localization/test_server_thread.h b/test/include/sick_lidar_localization/test_server_thread.h old mode 100755 new mode 100644 index 3bd55db..116880f --- a/test/include/sick_lidar_localization/test_server_thread.h +++ b/test/include/sick_lidar_localization/test_server_thread.h @@ -131,8 +131,9 @@ namespace sick_lidar_localization /*! * Closes all tcp connections + * @param[in] force_shutdown if true, sockets are immediately forced to shutdown */ - virtual void closeTcpConnections(void); + virtual void closeTcpConnections(bool force_shutdown = false); /*! * Closes a socket. diff --git a/test/include/sick_lidar_localization/verifier_thread.h b/test/include/sick_lidar_localization/verifier_thread.h old mode 100755 new mode 100644 diff --git a/test/scripts/cleanup.bash b/test/scripts/cleanup.bash index 40b2bc8..61502f6 100755 --- a/test/scripts/cleanup.bash +++ b/test/scripts/cleanup.bash @@ -2,7 +2,8 @@ pushd ../../../../ source /opt/ros/melodic/setup.bash -./src/sick_lidar_localization/test/scripts/killall.bash +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi killall rosmaster ; sleep 1 echo -e "\n# cleanup.bash: Deleting ros cache and logfiles and catkin folders ./build ./devel ./install" diff --git a/test/scripts/clion.bash b/test/scripts/clion.bash index 8e5c685..3bcdfcc 100755 --- a/test/scripts/clion.bash +++ b/test/scripts/clion.bash @@ -14,7 +14,7 @@ echo -e " Click 'File' -> 'Reload Cmake Project'" echo -e " cmake/clion: Project 'XXX' tried to find library '-lpthread' -> delete 'thread' from find_package(Boost REQUIRED COMPONENTS ...) in CMakeLists.txt" echo -e " rm -rf ../../../.idea # removes all clion settings" echo -e " rm -f ~/CMakeCache.txt" -echo -e " 'File' -> 'Settings' -> 'CMake' -> 'CMake options' : CATKIN_DEVEL_PREFIX=~/TASK013_PA0160_SIM_Localization/catkin_ws/devel" +echo -e " 'File' -> 'Settings' -> 'CMake' -> 'CMake options' : -DCATKIN_DEVEL_PREFIX=~/TASK013_PA0160_SIM_Localization/catkin_ws/devel" echo -e " 'File' -> 'Settings' -> 'CMake' -> 'Generation path' : ../cmake-build-debug" echo -e "Note: Do NOT install Hatchery plugin for launch.file support. It doesn't work but crashes clion. If done by accident:" echo -e " 'File' -> 'Settings' -> 'Plugins' -> 'Installed' -> Deactivate Hatchery" diff --git a/test/scripts/make.bash b/test/scripts/make.bash index 895919a..761b3f7 100755 --- a/test/scripts/make.bash +++ b/test/scripts/make.bash @@ -11,15 +11,15 @@ rm -f ./devel/lib/*sick* 2> /dev/null rm -f ./devel/lib/sick_lidar_localization/*sick* 2> /dev/null rm -f ./install/lib/*sick* 2> /dev/null rm -f ./install/lib/sick_lidar_localization/*sick* 2> /dev/null -if [ ! -d ./src/sick_lidar_localization ] && [ -d ./src/sick_lidar_localization_pretest ] ; then - pushd ./src # Note: development repo is https://github.com/michael1309/sick_lidar_localization_pretest.git, relase repo is https://github.com/SICKAG/sick_lidar_localization.git - ln -s ./sick_lidar_localization_pretest ./sick_lidar_localization - popd -fi -if [ ! -d ./src/sick_lidar_localization ] ; then - echo -e "\n## ERROR make.bash: directory ./src/sick_lidar_localization not found.\n" - exit -fi +# if [ ! -d ./src/sick_lidar_localization ] && [ -d ./src/sick_lidar_localization_pretest ] ; then +# pushd ./src # Note: development repo is https://github.com/michael1309/sick_lidar_localization_pretest.git, relase repo is https://github.com/SICKAG/sick_lidar_localization.git +# ln -s ./sick_lidar_localization_pretest ./sick_lidar_localization +# popd +# fi +# if [ ! -d ./src/sick_lidar_localization ] ; then +# echo -e "\n## ERROR make.bash: directory ./src/sick_lidar_localization not found.\n" +# exit +# fi # # Build and install sick_lidar_localization binaries. @@ -32,6 +32,8 @@ source ./install/setup.bash # lint, install by running catkin_lint -W1 ./src/sick_lidar_localization +if [ -d ./src/sick_lidar_localization ] ; then catkin_lint -W1 ./src/sick_lidar_localization ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then catkin_lint -W1 ./src/sick_lidar_localization_pretest ; fi # print warnings and errors echo -e "\nmake.bash finished.\n" diff --git a/test/scripts/run.bash b/test/scripts/run.bash index f47a9bd..c584345 100755 --- a/test/scripts/run.bash +++ b/test/scripts/run.bash @@ -19,7 +19,8 @@ source ./devel/setup.bash # Cleanup # -./src/sick_lidar_localization/test/scripts/killall.bash +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi diff --git a/test/scripts/run_demo_simu.bash b/test/scripts/run_demo_simu.bash index c6aa6b6..35f0664 100755 --- a/test/scripts/run_demo_simu.bash +++ b/test/scripts/run_demo_simu.bash @@ -19,7 +19,8 @@ source ./devel/setup.bash # Cleanup # -./src/sick_lidar_localization/test/scripts/killall.bash +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi @@ -58,7 +59,8 @@ rosrun rviz rviz & # sleep 600 -./src/sick_lidar_localization/test/scripts/killall.bash +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi killall roslaunch killall rviz killall static_transform_publisher diff --git a/test/scripts/run_error_simu.bash b/test/scripts/run_error_simu.bash index 521a2b5..a02d533 100755 --- a/test/scripts/run_error_simu.bash +++ b/test/scripts/run_error_simu.bash @@ -32,17 +32,18 @@ source ./devel/setup.bash # Cleanup # -./src/sick_lidar_localization/test/scripts/killall.bash +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y -if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log/error_simu ; fi +if [ ! -d ~/.ros/log/error_simu ] ; then mkdir -p ~/.ros/log/error_simu ; fi # # Start sick_lidar_localization driver, log diagnostic messages # echo -e "sick_lidar_localization error simulation: starting sick_lidar_localization sim_loc_driver.launch ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/error_simu/sim_loc_driver_error_simu.log & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 sim_loc_driver_check_cfg:=message_check_error_simu.yaml 2>&1 | unbuffer -p tee -a ~/.ros/log/error_simu/sim_loc_driver_error_simu.log & echo -e "sick_lidar_localization error simulation: sick_lidar_localization sim_loc_driver.launch started." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log sleep 10 @@ -83,19 +84,22 @@ if true ; then fi # -# Start test server and run in error simulation mode (disconnect and reconnect, send no or invalid telegrams). -# sick_lidar_localization driver must handle errors with error and diagnosis messages and reconnect if required. +# Run test server in error simulation mode (disconnect and reconnect, send no or invalid telegrams). +# sick_lidar_localization driver handles errors with diagnostic messages and reconnects if required. # if true ; then - roslaunch sick_lidar_localization sim_loc_test_server.launch error_simulation:=true 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log + roslaunch sick_lidar_localization sim_loc_test_server.launch error_simulation:=true 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log & + sleep 180 + rosnode kill /sim_loc_test_server ; sleep 1 ; killall -9 sim_loc_test_server fi # # Cleanup and exit # -./src/sick_lidar_localization/test/scripts/killall.bash +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi killall roslaunch grep "WARN" ~/.ros/log/error_simu/*.log ; grep "ERR" ~/.ros/log/error_simu/*.log sleep 15 diff --git a/test/scripts/run_simu.bash b/test/scripts/run_simu.bash index c095633..d438832 100755 --- a/test/scripts/run_simu.bash +++ b/test/scripts/run_simu.bash @@ -18,7 +18,8 @@ source ./devel/setup.bash # Cleanup # -./src/sick_lidar_localization/test/scripts/killall.bash +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi @@ -55,8 +56,9 @@ roslaunch sick_lidar_localization verify_sim_loc_driver.launch 2>&1 | unbuffer - # Run test server and driver for some time and verify results # -sleep 120 -./src/sick_lidar_localization/test/scripts/killall.bash +sleep 180 +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi sleep 1 ; killall roslaunch ; sleep 1 # @@ -64,34 +66,14 @@ sleep 1 ; killall roslaunch ; sleep 1 # roslaunch sick_lidar_localization unittest_sim_loc_parser.launch 2>&1 | tee -a ~/.ros/log/unittest_sim_loc_parser.log & -sleep 10 ; ./src/sick_lidar_localization/test/scripts/killall.bash ; sleep 1 ; killall roslaunch +sleep 10 +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +sleep 1 ; killall roslaunch cat ~/.ros/log/sim_loc_driver_diagnostic_messages.log grep "WARN" ~/.ros/log/*.log ; grep "ERR" ~/.ros/log/*.log sleep 20 -# -# Start sim_loc_driver, start test server and send and receive cola commands -# Note: debug only - time synchronization is started by sim_loc_driver.launch and by default running in background. -# -# source ./devel/setup.bash -# ps -elf -# roslaunch sick_lidar_localization sim_loc_test_server.launch 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_test_server.log & -# sleep 1 -# roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver.log & -# sleep 1 -# rossrv show SickLocColaTelegramSrv -# rosservice info SickLocColaTelegram -# rossrv show SickLocRequestTimestampSrv -# rosservice info SickLocRequestTimestamp -# for ((n=0;n<=10;n++)) ; do -# sleep 1 ; rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocMapState', wait_response_timeout: 1}" # expected reponse: cola_ascii_response="sRA LocMapState 1" -# sleep 1 ; rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" # expected reponse: cola_ascii_response="sAN LocRequestTimestamp " -# sleep 1 ; rosservice call SickLocRequestTimestamp "{}" # expected reponse: "timestamp_lidar_ms: , mean_time_vehicle_ms: , delta_time_ms: , ..." -# done -# sleep 1 ; ./src/sick_lidar_localization/test/scripts/killall.bash -# sleep 1 ; killall roslaunch ; sleep 20 -# ps -elf - # # Cleanup and exit # diff --git a/test/src/test_server.cpp b/test/src/test_server.cpp old mode 100755 new mode 100644 diff --git a/test/src/test_server_thread.cpp b/test/src/test_server_thread.cpp old mode 100755 new mode 100644 index ec05b69..a22fd25 --- a/test/src/test_server_thread.cpp +++ b/test/src/test_server_thread.cpp @@ -175,15 +175,16 @@ bool sick_lidar_localization::TestServerThread::stop(void) /*! * Closes all tcp connections + * @param[in] force_shutdown if true, sockets are immediately forced to shutdown */ -void sick_lidar_localization::TestServerThread::closeTcpConnections(void) +void sick_lidar_localization::TestServerThread::closeTcpConnections(bool force_shutdown) { for(std::list::iterator socket_iter = m_tcp_sockets.begin(); socket_iter != m_tcp_sockets.end(); socket_iter++) { boost::asio::ip::tcp::socket* p_socket = *socket_iter; try { - if(p_socket && p_socket->is_open()) + if(p_socket && (force_shutdown || p_socket->is_open())) { p_socket->shutdown(boost::asio::ip::tcp::socket::shutdown_both); p_socket->close(); @@ -291,15 +292,25 @@ templatevoid sick_lidar_localization::TestServerThread::runCo // normal mode: listen to tcp port, accept and connect to new tcp clients boost::asio::ip::tcp::socket* tcp_client_socket = new boost::asio::ip::tcp::socket(m_ioservice); boost::system::error_code errorcode; - if(m_error_simulation_flag.get() != DONT_ACCECPT) + if(m_error_simulation_flag.get() != DONT_LISTEN && m_error_simulation_flag.get() != DONT_ACCECPT) ROS_INFO_STREAM("TestServerThread: listening to tcp connections on port " << ip_port_results); tcp_acceptor_results.listen(); - if(m_error_simulation_flag.get() == DONT_ACCECPT) // error simulation: testserver does not does not accecpt tcp clients + if(m_error_simulation_flag.get() == DONT_LISTEN || m_error_simulation_flag.get() == DONT_ACCECPT) // error simulation: testserver does not does not accecpt tcp clients { ros::Duration(0.1).sleep(); continue; } tcp_acceptor_results.accept(*tcp_client_socket, errorcode); // normal mode: accept new tcp client + if(m_error_simulation_flag.get() == DONT_LISTEN || m_error_simulation_flag.get() == DONT_ACCECPT) // error simulation: testserver does not does not accecpt tcp clients + { + if(tcp_client_socket->is_open()) + { + tcp_client_socket->shutdown(boost::asio::ip::tcp::socket::shutdown_both); + tcp_client_socket->close(); + + } + continue; + } if (!errorcode && tcp_client_socket->is_open()) { // tcp client connected, start worker thread @@ -323,16 +334,15 @@ templatevoid sick_lidar_localization::TestServerThread::runCo void sick_lidar_localization::TestServerThread::runWorkerThreadResultCb(boost::asio::ip::tcp::socket* p_socket) { ROS_INFO_STREAM("TestServerThread: worker thread for result telegrams started"); - ros::Duration send_telegrams_delay(1.0 / m_result_telegram_rate); - sick_lidar_localization::SickLocResultPortTestcaseMsg testcase = sick_lidar_localization::TestcaseGenerator::createDefaultResultPortTestcase(); // initial testcase is the default testcase sick_lidar_localization::UniformRandomInteger random_generator(0,255); sick_lidar_localization::UniformRandomInteger random_length(1, 512); sick_lidar_localization::UniformRandomInteger random_integer(0, INT_MAX); double circle_yaw = 0; while(ros::ok() && m_worker_thread_running && p_socket && p_socket->is_open()) { - boost::system::error_code error_code; + ros::Duration send_telegrams_delay((double)sick_lidar_localization::TestcaseGenerator::ResultPoseInterval() / m_result_telegram_rate); send_telegrams_delay.sleep(); + boost::system::error_code error_code; if (m_error_simulation_flag.get() == DONT_SEND) // error simulation: testserver does not send any telegrams continue; if (m_error_simulation_flag.get() == SEND_RANDOM_TCP) // error simulation: testserver sends invalid random tcp packets @@ -343,6 +353,8 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadResultCb(boost::a } else { + // create testcase is a result port telegram with random based sythetical data + sick_lidar_localization::SickLocResultPortTestcaseMsg testcase = sick_lidar_localization::TestcaseGenerator::createRandomResultPortTestcase(); if(m_demo_move_in_circles) // simulate a sensor moving in circles { testcase = sick_lidar_localization::TestcaseGenerator::createResultPortCircles(2.0, circle_yaw); @@ -380,8 +392,6 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadResultCb(boost::a testcase.header.frame_id = m_result_testcases_frame_id; m_result_testcases_publisher.publish(testcase); } - // next testcase is a result port telegram with random data - testcase = sick_lidar_localization::TestcaseGenerator::createRandomResultPortTestcase(); } closeSocket(p_socket); ROS_INFO_STREAM("TestServerThread: worker thread for result telegrams finished"); @@ -412,6 +422,9 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadColaCb(boost::asi sick_lidar_localization::SickLocColaTelegramMsg telegram_msg = sick_lidar_localization::ColaParser::decodeColaTelegram(ascii_telegram); // Generate a synthetical response depending on the request sick_lidar_localization::SickLocColaTelegramMsg telegram_answer = sick_lidar_localization::TestcaseGenerator::createColaResponse(telegram_msg); + + // tbd: handle DONT_SEND, SEND_RANDOM_TCP and SEND_INVALID_TELEGRAMS as done in runWorkerThreadResultCb + // Send command response to tcp client std::vector binary_response = sick_lidar_localization::ColaParser::encodeColaTelegram(telegram_answer); std::string ascii_response = sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(binary_response); @@ -490,8 +503,9 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) number_testcases++; m_error_simulation_flag.set(DONT_LISTEN); ROS_INFO_STREAM("TestServerThread: 2. error simulation testcase: server not responding, not listening, no tcp connections accepted."); - closeWorkerThreads(); - closeTcpConnections(); + m_worker_thread_running = false; + ros::Duration(1.0 / m_result_telegram_rate).sleep(); + closeTcpConnections(true); errorSimulationWait(10); m_error_simulation_flag.set(NO_ERROR); ROS_INFO_STREAM("TestServerThread: 2. error simulation testcase: switched to normal execution, expecting telegram messages from driver"); @@ -508,8 +522,9 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) number_testcases++; m_error_simulation_flag.set(DONT_ACCECPT); ROS_INFO_STREAM("TestServerThread: 3. error simulation testcase: server not responding, listening on port " << m_ip_port_results << ", but accepting no tcp clients"); - closeWorkerThreads(); - closeTcpConnections(); + m_worker_thread_running = false; + ros::Duration(1.0 / m_result_telegram_rate).sleep(); + closeTcpConnections(true); errorSimulationWait(10); m_error_simulation_flag.set(NO_ERROR); errorSimulationWait(10); @@ -527,7 +542,6 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) ROS_INFO_STREAM("TestServerThread: 4. error simulation testcase: server not sending telegrams"); errorSimulationWait(10); m_error_simulation_flag.set(NO_ERROR); - closeWorkerThreads(); errorSimulationWait(10); if(!errorSimulationWaitForTelegramReceived(10, telegram_msg)) { @@ -543,7 +557,6 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) ROS_INFO_STREAM("TestServerThread: 5. error simulation testcase: server sending random tcp data"); errorSimulationWait(10); m_error_simulation_flag.set(NO_ERROR); - closeWorkerThreads(); errorSimulationWait(10); if(!errorSimulationWaitForTelegramReceived(10, telegram_msg)) { @@ -559,7 +572,6 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) ROS_INFO_STREAM("TestServerThread: 6. error simulation testcase: server sending invalid telegrams"); errorSimulationWait(10); m_error_simulation_flag.set(NO_ERROR); - closeWorkerThreads(); errorSimulationWait(10); if(!errorSimulationWaitForTelegramReceived(10, telegram_msg)) { diff --git a/test/src/unittest_sim_loc_parser.cpp b/test/src/unittest_sim_loc_parser.cpp old mode 100755 new mode 100644 diff --git a/test/src/verifier_thread.cpp b/test/src/verifier_thread.cpp old mode 100755 new mode 100644 index 917ab76..07a4acc --- a/test/src/verifier_thread.cpp +++ b/test/src/verifier_thread.cpp @@ -155,7 +155,7 @@ void sick_lidar_localization::VerifierThread::runVerificationThreadCb(void) size_t total_verification_cnt = 0, total_verification_failed_cnt = 0; while(ros::ok() && m_verification_thread_running) { - while(ros::ok() && m_verification_thread_running && m_result_port_testcase_fifo.size() < 4) // delay verification by 4 messages (messages may cross each other) + while (ros::ok() && m_verification_thread_running && m_result_port_testcase_fifo.size() < 4) // delay verification by 4 messages (messages may cross each other) { ros::Duration(1.0 / m_result_telegram_rate).sleep(); } @@ -165,25 +165,23 @@ void sick_lidar_localization::VerifierThread::runVerificationThreadCb(void) TelegramCounterCondition condition(server_telegram_counter); sick_lidar_localization::SickLocResultPortTelegramMsg driver_telegram = m_result_port_telegram_fifo.findFirstIf(condition, true); // Compare testcase and driver message - if(server_telegram_counter != driver_telegram.telegram_header.TelegramCounter) + if (server_telegram_counter == driver_telegram.telegram_header.TelegramCounter) // Testcase from the server corresponds to the result telegram from the driver { - ROS_WARN_STREAM("## ERROR VerifierThread: No driver message found for testcase with TelegramCounter " << server_telegram_counter); - total_verification_failed_cnt++; + if (!sick_lidar_localization::Utils::identicalByStream(driver_telegram, server_testcase.telegram_msg)) + { + ROS_WARN_STREAM("## ERROR VerifierThread: driver message differs from server testcase (TelegramCounter " << server_telegram_counter << ")"); + ROS_WARN_STREAM("## driver message (received): " << sick_lidar_localization::Utils::flattenToString(driver_telegram)); + ROS_WARN_STREAM("## server testcase (expected): " << sick_lidar_localization::Utils::flattenToString(server_testcase.telegram_msg)); + total_verification_failed_cnt++; + } + else + { + ROS_INFO_STREAM("VerifierThread: testcase verified and okay (TelegramCounter " << server_telegram_counter << ")"); + ROS_DEBUG_STREAM("VerifierThread: driver message (received): " << sick_lidar_localization::Utils::flattenToString(driver_telegram)); + ROS_DEBUG_STREAM("VerifierThread: server testcase (expected): " << sick_lidar_localization::Utils::flattenToString(server_testcase.telegram_msg)); + } + total_verification_cnt++; } - else if(!sick_lidar_localization::Utils::identicalByStream(driver_telegram, server_testcase.telegram_msg)) - { - ROS_WARN_STREAM("## ERROR VerifierThread: driver message differs from server testcase (TelegramCounter " << server_telegram_counter << ")"); - ROS_WARN_STREAM("## driver message (received): " << sick_lidar_localization::Utils::flattenToString(driver_telegram)); - ROS_WARN_STREAM("## server testcase (expected): " << sick_lidar_localization::Utils::flattenToString(server_testcase.telegram_msg)); - total_verification_failed_cnt++; - } - else - { - ROS_INFO_STREAM("VerifierThread: testcase verified and okay (TelegramCounter " << server_telegram_counter << ")"); - ROS_DEBUG_STREAM("VerifierThread: driver message (received): " << sick_lidar_localization::Utils::flattenToString(driver_telegram)); - ROS_DEBUG_STREAM("VerifierThread: server testcase (expected): " << sick_lidar_localization::Utils::flattenToString(server_testcase.telegram_msg)); - } - total_verification_cnt++; } ROS_INFO_STREAM("VerifierThread: verification thread for sim_loc_driver messages finished"); std::stringstream info_msg; diff --git a/test/src/verify_sim_loc_driver.cpp b/test/src/verify_sim_loc_driver.cpp old mode 100755 new mode 100644 diff --git a/yaml/message_check_default.yaml b/yaml/message_check_default.yaml index cdfc795..444ae1d 100644 --- a/yaml/message_check_default.yaml +++ b/yaml/message_check_default.yaml @@ -45,6 +45,8 @@ sick_lidar_localization: Reserved3: 0.0 # Reserved. uint64, size:= UInt64 = 8 byte telegram_trailer: # 2 byte trailer of a result port telegram Checksum: 0 # CRC16-CCITT checksum. uint16, size:= UInt16 = 2 byte + vehicle_time_delta: -5.0 # min. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + check_vehicle_time: true # check of vehicle time is enabled (default) result_telegram_max_values: # configuration of max allowed values in a result port telegram header: # ros header with sequence id, timestamp and frame id seq: 4294967296.0 # 4294967296 = 0xFFFFFFFF # sequence ID, consecutively increasing ID, uint32, size:= 4 byte @@ -77,4 +79,6 @@ sick_lidar_localization: Reserved3: 18446744073709551616.0 # 18446744073709551616 = 0xFFFFFFFFFFFFFFFF # Reserved. uint64, size:= UInt64 = 8 byte telegram_trailer: # 2 byte trailer of a result port telegram Checksum: 0xFFFF # CRC16-CCITT checksum. uint16, size:= UInt16 = 2 byte + vehicle_time_delta: 5.0 # max. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + check_vehicle_time: true # check of vehicle time is enabled (default) diff --git a/yaml/message_check_demo.yaml b/yaml/message_check_demo.yaml index adcddc9..18b4105 100644 --- a/yaml/message_check_demo.yaml +++ b/yaml/message_check_demo.yaml @@ -45,6 +45,8 @@ sick_lidar_localization: Reserved3: 0.0 # Reserved. uint64, size:= UInt64 = 8 byte telegram_trailer: # 2 byte trailer of a result port telegram Checksum: 0 # CRC16-CCITT checksum. uint16, size:= UInt16 = 2 byte + vehicle_time_delta: -1.0 # min. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + check_vehicle_time: true # check of vehicle time is enabled (default) result_telegram_max_values: # configuration of max allowed values in a result port telegram header: # ros header with sequence id, timestamp and frame id seq: 4294967296.0 # 4294967296 = 0xFFFFFFFF # sequence ID, consecutively increasing ID, uint32, size:= 4 byte @@ -77,4 +79,5 @@ sick_lidar_localization: Reserved3: 18446744073709551616.0 # 18446744073709551616 = 0xFFFFFFFFFFFFFFFF # Reserved. uint64, size:= UInt64 = 8 byte telegram_trailer: # 2 byte trailer of a result port telegram Checksum: 0xFFFF # CRC16-CCITT checksum. uint16, size:= UInt16 = 2 byte - + vehicle_time_delta: 1.0 # max. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + check_vehicle_time: true # check of vehicle time is enabled (default) diff --git a/yaml/message_check_error_simu.yaml b/yaml/message_check_error_simu.yaml new file mode 100644 index 0000000..89eaec6 --- /dev/null +++ b/yaml/message_check_error_simu.yaml @@ -0,0 +1,84 @@ +# Default configuration of sim_loc_driver_check +# +# sim_loc_driver_check subscribes to sim_loc_driver messages and checks the telegram data +# against min and max values configured in this yaml-file. +# Using different yaml configuration files, scene specific plausibility checks +# of sim_loc_driver messages can be performed by running automated tests against +# localization controllers like SIM1000FXA. +# sim_loc_driver_check will log a warning in case of failures or values out of range. +# +sick_lidar_localization: + + # sim_loc_driver_check configuration + sim_loc_driver_check: + result_telegrams_topic: "/sick_lidar_localization/driver/result_telegrams" # ros topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) + message_check_frequency: 100 # frequency to check sim_loc_driver messages + result_telegram_min_values: # configuration of min allowed values in a result port telegram + header: # ros header with sequence id, timestamp and frame id + seq: 0.0 # sequence ID, consecutively increasing ID, uint32, size:= 4 byte + stamp: 0.0 # time stamp + frame_id: "sick_lidar_localization" # frame id of ros header + telegram_header: # 52 byte header of a result port telegram + MagicWord: 0x5349434B # Magic word SICK (0x53 0x49 0x43 0x4B). uint32, size:= 4 × UInt8 = 4 byte + Length: 106 # Length of telegram incl. header, payload, and trailer. uint32, size:= UInt32 = 4 byte + PayloadType: 0x0642 # Payload type, 0x06c2 = Little Endian, 0x0642 = Big Endian. uint16, size:= UInt16 = 2 byte + PayloadVersion: 1 # Version of PayloadType structure. uint16, size:= UInt16 = 2 byte + OrderNumber: 0.0 # Order number of the localization controller. uint32, size:= UInt32 = 4 byte + SerialNumber: 0.0 # Serial number of the localization controller. uint32, size:= UInt32 = 4 byte + FW_Version: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] # Software version of the localization controller. uint8[], size:= 20 × UInt8 = 20 byte + TelegramCounter: 0.0 # Telegram counter since last start-up. uint32, size:= UInt32 = 4 byte + SystemTime: 0.0 # Not used. uint64, size:= NTP = 8 byte + telegram_payload: # 52 byte payload of a result port telegram + ErrorCode: 0 # ErrorCode 0:= OK, ErrorCode 1:= UNKNOWNERROR. uint16, size:= UInt16 = 2 byte + ScanCounter: 0.0 # Counter of related scan data. uint32, size:= UInt32 = 4 byte + Timestamp: 0.0 # Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. uint32, size:= UInt32 = 4 byte + PoseX: -2147483648 # Position X of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + PoseY: -2147483648 # Position Y of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + PoseYaw: -360000 # Orientation (yaw) of the vehicle on the map [mdeg]. int32, size:= Int32 = 4 byte + Reserved1: 0.0 # Reserved. uint32, size:= UInt32 = 4 byte + Reserved2: -2147483648 # Reserved. int32, size:= Int32 = 4 byte + Quality: 0 # Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte + OutliersRatio: 0 # Ratio of beams that cannot be assigned to the current reference map [%]. uint8, size:= UInt8 = 1 byte + CovarianceX: 0 # Covariance c1 of the pose X [mm^2]. int32, size:= Int32 = 4 byte + CovarianceY: 0 # Covariance c5 of the pose Y [mm^2]. int32, size:= Int32 = 4 byte + CovarianceYaw: 0 # Covariance c9 of the pose Yaw [mdeg^2]. int32, size:= Int32 = 4 byte + Reserved3: 0.0 # Reserved. uint64, size:= UInt64 = 8 byte + telegram_trailer: # 2 byte trailer of a result port telegram + Checksum: 0 # CRC16-CCITT checksum. uint16, size:= UInt16 = 2 byte + vehicle_time_delta: -1.0e308 # min. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + check_vehicle_time: false # we can't check the vehicle time without LocRequestTimestamps, which are not available due to simulation of network errors + result_telegram_max_values: # configuration of max allowed values in a result port telegram + header: # ros header with sequence id, timestamp and frame id + seq: 4294967296.0 # 4294967296 = 0xFFFFFFFF # sequence ID, consecutively increasing ID, uint32, size:= 4 byte + stamp: 2147483648.2147483648 # time stamp (max. 2 int values, each 2^31 = 2147483648) + frame_id: "sick_lidar_localization" # frame id of ros header + telegram_header: # 52 byte header of a result port telegram + MagicWord: 0x5349434B # Magic word SICK (0x53 0x49 0x43 0x4B). uint32, size:= 4 × UInt8 = 4 byte + Length: 106 # Length of telegram incl. header, payload, and trailer. uint32, size:= UInt32 = 4 byte + PayloadType: 0x06c2 # Payload type, 0x06c2 = Little Endian, 0x0642 = Big Endian. uint16, size:= UInt16 = 2 byte + PayloadVersion: 0xFFFF # Version of PayloadType structure. uint16, size:= UInt16 = 2 byte + OrderNumber: 4294967296.0 # 4294967296 = 0xFFFFFFFF # Order number of the localization controller. uint32, size:= UInt32 = 4 byte + SerialNumber: 4294967296.0 # 4294967296 = 0xFFFFFFFF # Serial number of the localization controller. uint32, size:= UInt32 = 4 byte + FW_Version: [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF] # Software version of the localization controller. uint8[], size:= 20 × UInt8 = 20 byte + TelegramCounter: 4294967296.0 # 4294967296 = 0xFFFFFFFF # Telegram counter since last start-up. uint32, size:= UInt32 = 4 byte + SystemTime: 18446744073709551616.0 # 18446744073709551616 = 0xFFFFFFFFFFFFFFFF # Not used. uint64, size:= NTP = 8 byte + telegram_payload: # 52 byte payload of a result port telegram + ErrorCode: 0xFFFF # ErrorCode 0:= OK, ErrorCode 1:= UNKNOWNERROR. uint16, size:= UInt16 = 2 byte + ScanCounter: 4294967296.0 # 4294967296 = 0xFFFFFFFF # Counter of related scan data. uint32, size:= UInt32 = 4 byte + Timestamp: 4294967296.0 # 4294967296 = 0xFFFFFFFF # Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. uint32, size:= UInt32 = 4 byte + PoseX: 2147483647 # Position X of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + PoseY: 2147483647 # Position Y of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + PoseYaw: 360000 # Orientation (yaw) of the vehicle on the map [mdeg]. int32, size:= Int32 = 4 byte + Reserved1: 4294967296.0 # 4294967296 = 0xFFFFFFFF # Reserved. uint32, size:= UInt32 = 4 byte + Reserved2: 2147483647 # Reserved. int32, size:= Int32 = 4 byte + Quality: 100 # Quality of pose [1 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte + OutliersRatio: 100 # Ratio of beams that cannot be assigned to the current reference map [%]. uint8, size:= UInt8 = 1 byte + CovarianceX: 2147483647 # Covariance c1 of the pose X [mm^2]. int32, size:= Int32 = 4 byte + CovarianceY: 2147483647 # Covariance c5 of the pose Y [mm^2]. int32, size:= Int32 = 4 byte + CovarianceYaw: 2147483647 # Covariance c9 of the pose Yaw [mdeg^2]. int32, size:= Int32 = 4 byte + Reserved3: 18446744073709551616.0 # 18446744073709551616 = 0xFFFFFFFFFFFFFFFF # Reserved. uint64, size:= UInt64 = 8 byte + telegram_trailer: # 2 byte trailer of a result port telegram + Checksum: 0xFFFF # CRC16-CCITT checksum. uint16, size:= UInt16 = 2 byte + vehicle_time_delta: 1.0e308 # max. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + check_vehicle_time: false # we can't check the vehicle time without LocRequestTimestamps, which are not available due to simulation of network errors + diff --git a/yaml/sim_loc_driver.yaml b/yaml/sim_loc_driver.yaml index bee99fc..efe7e2d 100644 --- a/yaml/sim_loc_driver.yaml +++ b/yaml/sim_loc_driver.yaml @@ -19,3 +19,10 @@ sick_lidar_localization: tf_parent_frame_id: "tf_demo_map" # parent frame of tf messages of of vehicles pose (typically frame of the loaded map) tf_child_frame_id: "tf_sick_lidar_localization" # child frame of tf messages of of vehicles pose + # Configuration for time sync service + time_sync: + cola_response_timeout: 1.0 # Timeout in seconds for cola responses from localization controller + software_pll_fifo_length: 7 # Length of software pll fifo, default: 7 + time_sync_rate: 0.1 # Frequency to request timestamps from localization controller using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 + time_sync_initial_rate: 1.0 # Frequency to request timestamps and to update software pll during initialization phase, default: 1.0 (LocRequestTimestamp queries every second) + time_sync_initial_length: 10 # Length of initialization phase with LocRequestTimestamps every second, default: 10 (i.e. 10 LocRequestTimestamp queries every second after start, otherwise LocRequestTimestamp queries every 10 seconds)