From d81801053b5f117e26a1df2e249c55060d222dee Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 02:37:55 -0500 Subject: [PATCH 01/22] cros added to components --- src/hal/components/cros/.gitignore | 27 + src/hal/components/cros/CHANGELOG | 24 + src/hal/components/cros/CMakeLists.txt | 40 + src/hal/components/cros/Doxyfile | 1869 ++++++++ src/hal/components/cros/LICENSE | 12 + src/hal/components/cros/NOTICE | 8 + src/hal/components/cros/README.md | 63 + src/hal/components/cros/docs/cROS_manual.doc | Bin 0 -> 491008 bytes src/hal/components/cros/docs/cROS_manual.pdf | Bin 0 -> 319161 bytes src/hal/components/cros/include/cros.h | 27 + src/hal/components/cros/include/cros_api.h | 407 ++ .../components/cros/include/cros_api_call.h | 54 + .../cros/include/cros_api_internal.h | 138 + src/hal/components/cros/include/cros_clock.h | 59 + src/hal/components/cros/include/cros_defs.h | 77 + .../components/cros/include/cros_err_codes.h | 162 + .../components/cros/include/cros_gentools.h | 37 + src/hal/components/cros/include/cros_log.h | 67 + .../components/cros/include/cros_message.h | 209 + .../cros/include/cros_message_internal.h | 119 + .../cros/include/cros_message_queue.h | 122 + src/hal/components/cros/include/cros_node.h | 344 ++ .../components/cros/include/cros_node_api.h | 100 + .../components/cros/include/cros_service.h | 34 + .../cros/include/cros_service_internal.h | 41 + src/hal/components/cros/include/cros_tcpros.h | 137 + src/hal/components/cros/include/dyn_buffer.h | 249 + src/hal/components/cros/include/dyn_string.h | 165 + src/hal/components/cros/include/md5.h | 43 + .../components/cros/include/tcpip_socket.h | 304 ++ .../components/cros/include/tcpros_process.h | 95 + src/hal/components/cros/include/tcpros_tags.h | 99 + .../components/cros/include/xmlrpc_params.h | 280 ++ .../cros/include/xmlrpc_params_vector.h | 126 + .../components/cros/include/xmlrpc_process.h | 81 + .../components/cros/include/xmlrpc_protocol.h | 56 + src/hal/components/cros/include/xmlrpc_tags.h | 67 + .../defusedxml/ElementTree.py | 143 + .../defusedxml/__init__.py | 62 + .../defusedxml/cElementTree.py | 40 + .../master_python_source/defusedxml/common.py | 134 + .../defusedxml/expatbuilder.py | 107 + .../defusedxml/expatreader.py | 61 + .../master_python_source/defusedxml/lxml.py | 155 + .../defusedxml/minidom.py | 63 + .../defusedxml/pulldom.py | 41 + .../master_python_source/defusedxml/sax.py | 60 + .../master_python_source/defusedxml/xmlrpc.py | 153 + .../master_python_source/rosgraph/__init__.py | 56 + .../rosgraph/impl/__init__.py | 34 + .../rosgraph/impl/graph.py | 582 +++ .../rosgraph/masterapi.py | 481 ++ .../master_python_source/rosgraph/names.py | 329 ++ .../master_python_source/rosgraph/network.py | 419 ++ .../master_python_source/rosgraph/rosenv.py | 75 + .../rosgraph/rosgraph_main.py | 103 + .../rosgraph/roslogging.py | 270 ++ .../master_python_source/rosgraph/xmlrpc.py | 304 ++ .../rosmaster/__init__.py | 38 + .../rosmaster/exceptions.py | 39 + .../master_python_source/rosmaster/main.py | 139 + .../master_python_source/rosmaster/master.py | 89 + .../rosmaster/master_api.py | 889 ++++ .../rosmaster/paramserver.py | 402 ++ .../rosmaster/registrations.py | 473 ++ .../rosmaster/threadpool.py | 228 + .../master_python_source/rosmaster/util.py | 90 + .../rosmaster/validators.py | 199 + .../cros/master_python_source/runmaster.py | 25 + .../components/cros/msvs_projects/cros.sln | 99 + .../cros/msvs_projects/cros_library.vcxproj | 207 + .../cros_library.vcxproj.filters | 168 + .../parameters_test/parameters_test.vcxproj | 159 + .../parameters_test.vcxproj.filters | 22 + .../performance_test/performance_test.vcxproj | 159 + .../performance_test.vcxproj.filters | 22 + .../components/cros/msvs_projects/readme.txt | 50 + .../msvs_projects/ros_master_python.pyproj | 62 + .../sample_listener/sample_listener.vcxproj | 191 + .../sample_listener.vcxproj.filters | 22 + .../sample_talker/sample_talker.vcxproj | 191 + .../sample_talker.vcxproj.filters | 22 + src/hal/components/cros/resources/README | 3 + .../components/cros/resources/ROS-QUICKSTART | 29 + .../components/cros/resources/cROS_logo.jpg | Bin 0 -> 13108 bytes .../cros/resources/cros_testbed/.gitignore | 1 + .../cros/resources/cros_testbed/BUILD | 17 + .../resources/cros_testbed/CMakeLists.txt | 160 + .../cros/resources/cros_testbed/include/.keep | 0 .../cros_testbed/msg/DoubleVector.msg | 1 + .../cros/resources/cros_testbed/package.xml | 60 + .../cros_testbed/src/add_two_ints.cpp | 23 + .../cros_testbed/src/clock_listener.cpp | 58 + .../cros_testbed/src/counter_listener.cpp | 58 + .../cros_testbed/src/std_msgs_listener.cpp | 125 + .../cros_testbed/src/std_msgs_talker.cpp | 125 + .../cros_testbed/src/test_services.cpp | 36 + .../cros_testbed/src/vector_listener.cpp | 20 + .../cros_testbed/src/vector_talker.cpp | 87 + .../cros_testbed/srv/add_two_ints.srv | 4 + .../cros/resources/rosgraph_msgs/Log.msg | 19 + .../components/cros/samples/CMakeLists.txt | 19 + src/hal/components/cros/samples/api-test.c | 402 ++ src/hal/components/cros/samples/listener.c | 223 + .../components/cros/samples/parameters-test.c | 249 + .../cros/samples/performance-test.cpp | 583 +++ .../cros/samples/plot_performance.m | 28 + .../cros/samples/ros-i-trajectory-test.c | 219 + .../rosdb/gripping_robot/CloseGripper.srv | 3 + .../rosdb/gripping_robot/GripperJoints.msg | 1 + .../rosdb/gripping_robot/GripperStatus.msg | 4 + .../samples/rosdb/gripping_robot/MoveArm.srv | 5 + .../rosdb/gripping_robot/OpenGripper.srv | 3 + .../rosdb/gripping_robot/Reconfigure.srv | 4 + .../rosdb/gripping_robot/RestPosition.srv | 2 + .../samples/rosdb/gripping_robot/Transfer.srv | 2 + .../cros/samples/rosdb/roscpp/GetLoggers.srv | 2 + .../cros/samples/rosdb/roscpp/Logger.msg | 2 + .../samples/rosdb/roscpp/SetLoggerLevel.srv | 3 + .../rosdb/roscpp_tutorials/TwoInts.srv | 4 + .../cros/samples/rosdb/rosgraph_msgs/Log.msg | 19 + .../samples/rosdb/sensor_msgs/JointState.msg | 26 + .../cros/samples/rosdb/std_msgs/String.msg | 1 + .../rosdb/trajectory_msgs/JointTrajectory.msg | 3 + .../trajectory_msgs/JointTrajectoryPoint.msg | 9 + src/hal/components/cros/samples/talker.c | 152 + src/hal/components/cros/src/cros_api.c | 2305 ++++++++++ src/hal/components/cros/src/cros_api_call.c | 112 + src/hal/components/cros/src/cros_clock.c | 122 + src/hal/components/cros/src/cros_err_codes.c | 163 + src/hal/components/cros/src/cros_gentools.c | 93 + src/hal/components/cros/src/cros_log.c | 239 + src/hal/components/cros/src/cros_message.c | 3280 ++++++++++++++ .../components/cros/src/cros_message_queue.c | 138 + src/hal/components/cros/src/cros_node.c | 4005 +++++++++++++++++ src/hal/components/cros/src/cros_node_api.c | 1275 ++++++ src/hal/components/cros/src/cros_service.c | 433 ++ src/hal/components/cros/src/cros_tcpros.c | 1049 +++++ src/hal/components/cros/src/dyn_buffer.c | 249 + src/hal/components/cros/src/dyn_string.c | 286 ++ src/hal/components/cros/src/md5.c | 296 ++ src/hal/components/cros/src/tcpip_socket.c | 844 ++++ src/hal/components/cros/src/tcpros_process.c | 79 + src/hal/components/cros/src/xmlrpc_params.c | 1454 ++++++ .../cros/src/xmlrpc_params_vector.c | 171 + src/hal/components/cros/src/xmlrpc_process.c | 62 + src/hal/components/cros/src/xmlrpc_protocol.c | 410 ++ 147 files changed, 33258 insertions(+) create mode 100644 src/hal/components/cros/.gitignore create mode 100644 src/hal/components/cros/CHANGELOG create mode 100644 src/hal/components/cros/CMakeLists.txt create mode 100644 src/hal/components/cros/Doxyfile create mode 100644 src/hal/components/cros/LICENSE create mode 100644 src/hal/components/cros/NOTICE create mode 100644 src/hal/components/cros/README.md create mode 100755 src/hal/components/cros/docs/cROS_manual.doc create mode 100644 src/hal/components/cros/docs/cROS_manual.pdf create mode 100644 src/hal/components/cros/include/cros.h create mode 100644 src/hal/components/cros/include/cros_api.h create mode 100644 src/hal/components/cros/include/cros_api_call.h create mode 100644 src/hal/components/cros/include/cros_api_internal.h create mode 100644 src/hal/components/cros/include/cros_clock.h create mode 100644 src/hal/components/cros/include/cros_defs.h create mode 100644 src/hal/components/cros/include/cros_err_codes.h create mode 100644 src/hal/components/cros/include/cros_gentools.h create mode 100644 src/hal/components/cros/include/cros_log.h create mode 100644 src/hal/components/cros/include/cros_message.h create mode 100644 src/hal/components/cros/include/cros_message_internal.h create mode 100644 src/hal/components/cros/include/cros_message_queue.h create mode 100644 src/hal/components/cros/include/cros_node.h create mode 100644 src/hal/components/cros/include/cros_node_api.h create mode 100644 src/hal/components/cros/include/cros_service.h create mode 100644 src/hal/components/cros/include/cros_service_internal.h create mode 100644 src/hal/components/cros/include/cros_tcpros.h create mode 100644 src/hal/components/cros/include/dyn_buffer.h create mode 100644 src/hal/components/cros/include/dyn_string.h create mode 100644 src/hal/components/cros/include/md5.h create mode 100644 src/hal/components/cros/include/tcpip_socket.h create mode 100644 src/hal/components/cros/include/tcpros_process.h create mode 100644 src/hal/components/cros/include/tcpros_tags.h create mode 100644 src/hal/components/cros/include/xmlrpc_params.h create mode 100644 src/hal/components/cros/include/xmlrpc_params_vector.h create mode 100644 src/hal/components/cros/include/xmlrpc_process.h create mode 100644 src/hal/components/cros/include/xmlrpc_protocol.h create mode 100644 src/hal/components/cros/include/xmlrpc_tags.h create mode 100644 src/hal/components/cros/master_python_source/defusedxml/ElementTree.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/__init__.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/cElementTree.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/common.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/expatbuilder.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/expatreader.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/lxml.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/minidom.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/pulldom.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/sax.py create mode 100644 src/hal/components/cros/master_python_source/defusedxml/xmlrpc.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/__init__.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/impl/__init__.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/impl/graph.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/masterapi.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/names.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/network.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/rosenv.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/rosgraph_main.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/roslogging.py create mode 100644 src/hal/components/cros/master_python_source/rosgraph/xmlrpc.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/__init__.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/exceptions.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/main.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/master.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/master_api.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/paramserver.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/registrations.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/threadpool.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/util.py create mode 100644 src/hal/components/cros/master_python_source/rosmaster/validators.py create mode 100644 src/hal/components/cros/master_python_source/runmaster.py create mode 100644 src/hal/components/cros/msvs_projects/cros.sln create mode 100644 src/hal/components/cros/msvs_projects/cros_library.vcxproj create mode 100644 src/hal/components/cros/msvs_projects/cros_library.vcxproj.filters create mode 100644 src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj create mode 100644 src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj.filters create mode 100644 src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj create mode 100644 src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj.filters create mode 100644 src/hal/components/cros/msvs_projects/readme.txt create mode 100644 src/hal/components/cros/msvs_projects/ros_master_python.pyproj create mode 100644 src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj create mode 100644 src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj.filters create mode 100644 src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj create mode 100644 src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj.filters create mode 100644 src/hal/components/cros/resources/README create mode 100644 src/hal/components/cros/resources/ROS-QUICKSTART create mode 100644 src/hal/components/cros/resources/cROS_logo.jpg create mode 100755 src/hal/components/cros/resources/cros_testbed/.gitignore create mode 100755 src/hal/components/cros/resources/cros_testbed/BUILD create mode 100755 src/hal/components/cros/resources/cros_testbed/CMakeLists.txt create mode 100755 src/hal/components/cros/resources/cros_testbed/include/.keep create mode 100755 src/hal/components/cros/resources/cros_testbed/msg/DoubleVector.msg create mode 100755 src/hal/components/cros/resources/cros_testbed/package.xml create mode 100755 src/hal/components/cros/resources/cros_testbed/src/add_two_ints.cpp create mode 100755 src/hal/components/cros/resources/cros_testbed/src/clock_listener.cpp create mode 100755 src/hal/components/cros/resources/cros_testbed/src/counter_listener.cpp create mode 100755 src/hal/components/cros/resources/cros_testbed/src/std_msgs_listener.cpp create mode 100755 src/hal/components/cros/resources/cros_testbed/src/std_msgs_talker.cpp create mode 100755 src/hal/components/cros/resources/cros_testbed/src/test_services.cpp create mode 100755 src/hal/components/cros/resources/cros_testbed/src/vector_listener.cpp create mode 100755 src/hal/components/cros/resources/cros_testbed/src/vector_talker.cpp create mode 100755 src/hal/components/cros/resources/cros_testbed/srv/add_two_ints.srv create mode 100644 src/hal/components/cros/resources/rosgraph_msgs/Log.msg create mode 100644 src/hal/components/cros/samples/CMakeLists.txt create mode 100644 src/hal/components/cros/samples/api-test.c create mode 100644 src/hal/components/cros/samples/listener.c create mode 100644 src/hal/components/cros/samples/parameters-test.c create mode 100644 src/hal/components/cros/samples/performance-test.cpp create mode 100644 src/hal/components/cros/samples/plot_performance.m create mode 100644 src/hal/components/cros/samples/ros-i-trajectory-test.c create mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/CloseGripper.srv create mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/GripperJoints.msg create mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/GripperStatus.msg create mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/MoveArm.srv create mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/OpenGripper.srv create mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/Reconfigure.srv create mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/RestPosition.srv create mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/Transfer.srv create mode 100644 src/hal/components/cros/samples/rosdb/roscpp/GetLoggers.srv create mode 100644 src/hal/components/cros/samples/rosdb/roscpp/Logger.msg create mode 100644 src/hal/components/cros/samples/rosdb/roscpp/SetLoggerLevel.srv create mode 100755 src/hal/components/cros/samples/rosdb/roscpp_tutorials/TwoInts.srv create mode 100644 src/hal/components/cros/samples/rosdb/rosgraph_msgs/Log.msg create mode 100644 src/hal/components/cros/samples/rosdb/sensor_msgs/JointState.msg create mode 100644 src/hal/components/cros/samples/rosdb/std_msgs/String.msg create mode 100644 src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectory.msg create mode 100644 src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectoryPoint.msg create mode 100644 src/hal/components/cros/samples/talker.c create mode 100644 src/hal/components/cros/src/cros_api.c create mode 100644 src/hal/components/cros/src/cros_api_call.c create mode 100644 src/hal/components/cros/src/cros_clock.c create mode 100644 src/hal/components/cros/src/cros_err_codes.c create mode 100644 src/hal/components/cros/src/cros_gentools.c create mode 100644 src/hal/components/cros/src/cros_log.c create mode 100644 src/hal/components/cros/src/cros_message.c create mode 100644 src/hal/components/cros/src/cros_message_queue.c create mode 100644 src/hal/components/cros/src/cros_node.c create mode 100644 src/hal/components/cros/src/cros_node_api.c create mode 100644 src/hal/components/cros/src/cros_service.c create mode 100644 src/hal/components/cros/src/cros_tcpros.c create mode 100644 src/hal/components/cros/src/dyn_buffer.c create mode 100644 src/hal/components/cros/src/dyn_string.c create mode 100644 src/hal/components/cros/src/md5.c create mode 100644 src/hal/components/cros/src/tcpip_socket.c create mode 100644 src/hal/components/cros/src/tcpros_process.c create mode 100644 src/hal/components/cros/src/xmlrpc_params.c create mode 100644 src/hal/components/cros/src/xmlrpc_params_vector.c create mode 100644 src/hal/components/cros/src/xmlrpc_process.c create mode 100644 src/hal/components/cros/src/xmlrpc_protocol.c diff --git a/src/hal/components/cros/.gitignore b/src/hal/components/cros/.gitignore new file mode 100644 index 00000000..b83fb152 --- /dev/null +++ b/src/hal/components/cros/.gitignore @@ -0,0 +1,27 @@ +CMakeCache.txt +CMakeFiles/ +Makefile +bin/ +lib/ +cmake_install.cmake +.cproject +.project +*.kdev4 +.kdev4/ +build/ +debug/ +release/ +/docs/html +/docs/latex +gmon.out +*~ +.idea/ + +# Python byte-compiled code +__pycache__/ +*.py[cod] + +# Microsoft Visual Studio +.vs/ +x64/ +Win32/ diff --git a/src/hal/components/cros/CHANGELOG b/src/hal/components/cros/CHANGELOG new file mode 100644 index 00000000..dcb93f61 --- /dev/null +++ b/src/hal/components/cros/CHANGELOG @@ -0,0 +1,24 @@ +cROS 1.0-rc1 (21/12/2017) +------------------------- +Most significant changes of this second release: + * Now cROS compiles and works on OS X + * Fixed asynchronously connections management with select() function + * Added polling support for receiving topic messages, blocking message sending and blocking service call + * Fixed diverse bugs + * Improvements in message serialization/deserialization + * Added error checking and return error code to some principal functions + * Added some checks for the syntax the specified message and service definition files + * Fixed many memory leaks (and some double memory frees). So far no leaks are detecting when running the sample programs + * A signal handler has been implemented in several test program to allow the user to end the program safely by pressing Ctr-C + * Several segmentation faults have been fixed + * The return value of API functions has been changed from int to cRosErrCodePack (encoding pre-defined global error codes. See cros_err_codes.h) + * Some helper functions have been implemented to print the content of a cRosMessage, error code, copy messages, etc. + * A subscriber now supports receiving messages on the same topic from several publishers + * cRosNodeDestroy() automatically unregisters all topic publishers, topic subscribers, service providers and parameter subscribers in the ROS master + * The ROS node now supports the operation as a service caller (client) + * Added compatibility with MATLAB ROS toolbox (tested with MATLAB R2017a) + * Added cROS library manual in PDF + +cROS 0.9 +-------- + * First public release diff --git a/src/hal/components/cros/CMakeLists.txt b/src/hal/components/cros/CMakeLists.txt new file mode 100644 index 00000000..aaaec434 --- /dev/null +++ b/src/hal/components/cros/CMakeLists.txt @@ -0,0 +1,40 @@ +project(cros) + +cmake_minimum_required (VERSION 2.8) + +cmake_policy(SET CMP0015 NEW) + +execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion + OUTPUT_VARIABLE GCC_VERSION) + +set(CMAKE_C_FLAGS "-Wall -Wno-unused") +if (GCC_VERSION VERSION_GREATER 4.8 OR GCC_VERSION VERSION_EQUAL 4.8) + set(CMAKE_C_FLAGS_DEBUG "-g -pg -DDEBUG -fsanitize=address -static-libasan") +else() + set(CMAKE_C_FLAGS_DEBUG "-g -pg -DDEBUG") +endif() + +set(LINK_FLAGS_DEBUG "-pg") +set(CMAKE_C_FLAGS_RELEASE "-DNDEBUG -O1") +set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin) + +include_directories (include) +aux_source_directory(${PROJECT_SOURCE_DIR}/src CROSLIB_SRCS) + +add_library(cros SHARED ${CROSLIB_SRCS} ) + +add_subdirectory(samples) + +set_target_properties(cros PROPERTIES ARCHIVE_OUTPUT_DIRECTORY lib) + +find_package(Doxygen) +if (NOT DOXYGEN_FOUND) + message(ERROR "Doxygen is needed to build the documentation. Please install it correctly") +else() + + configure_file(Doxyfile ${PROJECT_BINARY_DIR}/Doxyfile @ONLY IMMEDIATE) + #-- Add a custom target to run Doxygen when ever the project is built + add_custom_target (docs COMMAND ${DOXYGEN_EXECUTABLE} ${PROJECT_BINARY_DIR}/Doxyfile + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + COMMENT "Generating API documentation with Doxygen" VERBATIM) +endif() diff --git a/src/hal/components/cros/Doxyfile b/src/hal/components/cros/Doxyfile new file mode 100644 index 00000000..769c0db0 --- /dev/null +++ b/src/hal/components/cros/Doxyfile @@ -0,0 +1,1869 @@ +# Doxyfile 1.8.3.1 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" "). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# http://www.gnu.org/software/libiconv for the list of possible encodings. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or sequence of words) that should +# identify the project. Note that if you do not use Doxywizard you need +# to put quotes around the project name if it contains spaces. + +PROJECT_NAME = "cROS" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = "0.1" + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer +# a quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify an logo or icon that is +# included in the documentation. The maximum height of the logo should not +# exceed 55 pixels and the maximum width should not exceed 200 pixels. +# Doxygen will copy the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = ./docs + +# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create +# 4096 sub-directories (in 2 levels) under the output directory of each output +# format and will distribute the generated files over these directories. +# Enabling this option can be useful when feeding doxygen a huge amount of +# source files, where putting all generated files in the same directory would +# otherwise cause performance problems for the file system. + +CREATE_SUBDIRS = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, +# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, +# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English +# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, +# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, +# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator +# that is used to form the text in various listings. Each string +# in this list, if found as the leading text of the brief description, will be +# stripped from the text and the result after processing the whole list, is +# used as the annotated text. Otherwise, the brief description is used as-is. +# If left blank, the following values are used ("$name" is automatically +# replaced with the name of the entity): "The $name class" "The $name widget" +# "The $name file" "is" "provides" "specifies" "contains" +# "represents" "a" "an" "the" + +ABBREVIATE_BRIEF = + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = YES + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user-defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the +# path to strip. Note that you specify absolute paths here, but also +# relative paths, which will be relative from the directory where doxygen is +# started. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of +# the path mentioned in the documentation of a class, which tells +# the reader which header file to include in order to use a class. +# If left blank only the name of the header file containing the class +# definition is used. Otherwise one should specify the include paths that +# are normally passed to the compiler using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter +# (but less readable) file names. This can be useful if your file system +# doesn't support long names like on DOS, Mac, or CD-ROM. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like regular Qt-style comments +# (thus requiring an explicit @brief command for a brief description.) + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will +# interpret the first line (until the first dot) of a Qt-style +# comment as the brief description. If set to NO, the comments +# will behave just like regular Qt-style comments (thus requiring +# an explicit \brief command for a brief description.) + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen +# treat a multi-line C++ special comment block (i.e. a block of //! or /// +# comments) as a brief description. This used to be the default behaviour. +# The new default is to treat a multi-line C++ comment block as a detailed +# description. Set this tag to YES if you prefer the old behaviour instead. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# re-implements. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce +# a new page for each member. If set to NO, the documentation of a member will +# be part of the file/class/namespace that contains it. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 2 + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user-defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding +# "class=itcl::class" will allow you to use the command class in the +# itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C +# sources only. Doxygen will then generate output that is more tailored for C. +# For instance, some of the names that are used will be different. The list +# of all members will be omitted, etc. + +OPTIMIZE_OUTPUT_FOR_C = YES + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java +# sources only. Doxygen will then generate output that is more tailored for +# Java. For instance, namespaces will be presented as packages, qualified +# scopes will look different, etc. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources only. Doxygen will then generate output that is more tailored for +# Fortran. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for +# VHDL. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, +# and language is one of the parsers supported by doxygen: IDL, Java, +# Javascript, CSharp, C, C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, +# C++. For instance to make doxygen treat .inc files as Fortran files (default +# is PHP), and .f files as C (default is Fortran), use: inc=Fortran f=C. Note +# that for custom extensions you also need to set FILE_PATTERNS otherwise the +# files are not read by doxygen. + +EXTENSION_MAPPING = + +# If MARKDOWN_SUPPORT is enabled (the default) then doxygen pre-processes all +# comments according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you +# can mix doxygen, HTML, and XML commands with Markdown formatting. +# Disable only in case of backward compatibilities issues. + +MARKDOWN_SUPPORT = YES + +# When enabled doxygen tries to link words that correspond to documented classes, +# or namespaces to their corresponding documentation. Such a link can be +# prevented in individual cases by by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should +# set this tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. +# func(std::string) {}). This also makes the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. +# Doxygen will parse them like normal C++ but will assume all classes use public +# instead of private inheritance when no explicit protection keyword is present. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES (the +# default) will make doxygen replace the get and set methods by a property in +# the documentation. This will only work if the methods are indeed getting or +# setting a simple type. If this is not the case, or you want to show the +# methods anyway, you should set this option to NO. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# Set the SUBGROUPING tag to YES (the default) to allow class member groups of +# the same type (for instance a group of public functions) to be put as a +# subgroup of that type (e.g. under the Public Functions section). Set it to +# NO to prevent subgrouping. Alternatively, this can be done per class using +# the \nosubgrouping command. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and +# unions are shown inside the group in which they are included (e.g. using +# @ingroup) instead of on a separate page (for HTML and Man pages) or +# section (for LaTeX and RTF). + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and +# unions with only public data fields will be shown inline in the documentation +# of the scope in which they are defined (i.e. file, namespace, or group +# documentation), provided this scope is documented. If set to NO (the default), +# structs, classes, and unions are shown on a separate page (for HTML and Man +# pages) or section (for LaTeX and RTF). + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum +# is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically +# be useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. + +TYPEDEF_HIDES_STRUCT = NO + +# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to +# determine which symbols to keep in memory and which to flush to disk. +# When the cache is full, less often used symbols will be written to disk. +# For small to medium size projects (<1000 input files) the default value is +# probably good enough. For larger projects a too small cache size can cause +# doxygen to be busy swapping symbols to and from disk most of the time +# causing a significant performance penalty. +# If the system has enough physical memory increasing the cache will improve the +# performance by keeping more symbols in memory. Note that the value works on +# a logarithmic scale so increasing the size by one will roughly double the +# memory usage. The cache size is given by this formula: +# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +SYMBOL_CACHE_SIZE = 0 + +# Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be +# set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given +# their name and scope. Since this can be an expensive process and often the +# same symbol appear multiple times in the code, doxygen keeps a cache of +# pre-resolved symbols. If the cache is too small doxygen will become slower. +# If the cache is too large, memory is wasted. The cache size is given by this +# formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range is 0..9, the default is 0, +# corresponding to a cache size of 2^16 = 65536 symbols. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal +# scope will be included in the documentation. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) +# defined locally in source files will be included in the documentation. +# If set to NO only classes defined in header files are included. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. When set to YES local +# methods, which are defined in the implementation section but not in +# the interface are included in the documentation. +# If set to NO (the default) only methods in the interface are included. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base +# name of the file that contains the anonymous namespace. By default +# anonymous namespaces are hidden. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these classes will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all +# friend (class|struct|union) declarations. +# If set to NO (the default) these declarations will be included in the +# documentation. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any +# documentation blocks found inside the body of a function. +# If set to NO (the default) these blocks will be appended to the +# function's detailed documentation block. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower-case letters. If set to YES upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = NO + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put a list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = YES + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen +# will list include files with double quotes in the documentation +# rather than with sharp brackets. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the +# brief documentation of file, namespace and class members alphabetically +# by member name. If set to NO (the default) the members will appear in +# declaration order. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen +# will sort the (brief and detailed) documentation of class members so that +# constructors and destructors are listed first. If set to NO (the default) +# the constructors will appear in the respective orders defined by +# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. +# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO +# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the +# hierarchy of group names into alphabetical order. If set to NO (the default) +# the group names will appear in their defined order. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be +# sorted by fully-qualified names, including namespaces. If set to +# NO (the default), the class list will be sorted only by class name, +# not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the +# alphabetical list. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to +# do proper type resolution of all parameters of a function it will reject a +# match between the prototype and the implementation of a member function even +# if there is only one candidate or it is obvious which candidate to choose +# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen +# will still accept a match between prototype and implementation in such cases. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or +# disable (NO) the bug list. This list is created by putting \bug +# commands in the documentation. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or +# disable (NO) the deprecated list. This list is created by putting +# \deprecated commands in the documentation. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if section-label ... \endif +# and \cond section-label ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines +# the initial value of a variable or macro consists of for it to appear in +# the documentation. If the initializer consists of more lines than specified +# here it will be hidden. Use a value of 0 to hide initializers completely. +# The appearance of the initializer of individual variables and macros in the +# documentation can be controlled using \showinitializer or \hideinitializer +# command in the documentation regardless of this setting. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated +# at the bottom of the documentation of classes and structs. If set to YES the +# list will mention the files that were used to generate the documentation. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. +# This will remove the Files entry from the Quick Index and from the +# Folder Tree View (if specified). The default is YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the +# Namespaces page. +# This will remove the Namespaces entry from the Quick Index +# and from the Folder Tree View (if specified). The default is YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command , where is the value of +# the FILE_VERSION_FILTER tag, and is the name of an input file +# provided by doxygen. Whatever the program writes to standard output +# is used as the file version. See the manual for examples. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. +# You can optionally specify a file name after the option, if omitted +# DoxygenLayout.xml will be used as the name of the layout file. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files +# containing the references data. This must be a list of .bib files. The +# .bib extension is automatically appended if omitted. Using this command +# requires the bibtex tool to be installed. See also +# http://en.wikipedia.org/wiki/BibTeX for more info. For LaTeX the style +# of the bibliography can be controlled using LATEX_BIB_STYLE. To use this +# feature you need bibtex and perl available in the search path. Do not use +# file names with spaces, bibtex cannot handle them. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some +# parameters in a documented function, or documenting parameters that +# don't exist or using markup commands wrongly. + +WARN_IF_DOC_ERROR = YES + +# The WARN_NO_PARAMDOC option can be enabled to get warnings for +# functions that are documented, but have no documentation for their parameters +# or return value. If set to NO (the default) doxygen will only warn about +# wrong or incomplete parameter documentation, but not about the absence of +# documentation. + +WARN_NO_PARAMDOC = NO + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. Optionally the format may contain +# $version, which will be replaced by the version of the file (if it could +# be obtained via FILE_VERSION_FILTER) + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = ./include ./src + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is +# also the default input encoding. Doxygen uses libiconv (or the iconv built +# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for +# the list of possible encodings. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank the following patterns are tested: +# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh +# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py +# *.f90 *.f *.for *.vhd *.vhdl + +FILE_PATTERNS = + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = NO + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. Note that the wildcards are matched +# against the file with absolute path, so to exclude all test directories +# for example use the pattern */test/* + +EXCLUDE_PATTERNS = + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude +# commands irrespective of the value of the RECURSIVE tag. +# Possible values are YES and NO. If left blank NO is used. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command , where +# is the value of the INPUT_FILTER tag, and is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. +# If FILTER_PATTERNS is specified, this tag will be +# ignored. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. +# Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. +# The filters are a list of the form: +# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further +# info on how filters are used. If FILTER_PATTERNS is empty or if +# non of the patterns match the file name, INPUT_FILTER is applied. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse (i.e. when SOURCE_BROWSER is set to YES). + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) +# and it is also possible to disable source filtering for a specific pattern +# using *.ext= (so without naming a filter). This option only has effect when +# FILTER_SOURCE_FILES is enabled. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MD_FILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page (index.html). +# This can be useful if you have a project on for instance GitHub and want reuse +# the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. +# Note: To get rid of all source code in the generated output, make sure also +# VERBATIM_HEADERS is set to NO. + +SOURCE_BROWSER = YES + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C, C++ and Fortran comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES +# then for each documented function all documented +# functions referencing it will be listed. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES +# then for each documented function all documented entities +# called/used by that function will be listed. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) +# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from +# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will +# link to the source code. +# Otherwise they will link to the documentation. + +REFERENCES_LINK_SOURCE = YES + +# If the USE_HTAGS tag is set to YES then the references to source code +# will point to the HTML generated by the htags(1) tool instead of doxygen +# built-in source browser. The htags tool is part of GNU's global source +# tagging system (see http://www.gnu.org/software/global/global.html). You +# will need version 4.8.6 or higher. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = YES + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for +# each generated HTML page (for example: .htm,.php,.asp). If it is left blank +# doxygen will generate files with .html extension. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. Note that when using a custom header you are responsible +# for the proper inclusion of any scripts and style sheets that doxygen +# needs, which is dependent on the configuration options used. +# It is advised to generate a default header using "doxygen -w html +# header.html footer.html stylesheet.css YourConfigFile" and then modify +# that header. Note that the header is subject to change so you typically +# have to redo this when upgrading to a newer version of doxygen or when +# changing the value of configuration settings such as GENERATE_TREEVIEW! + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If left blank doxygen will +# generate a default style sheet. Note that it is recommended to use +# HTML_EXTRA_STYLESHEET instead of this one, as it is more robust and this +# tag will in the future become obsolete. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional +# user-defined cascading style sheet that is included after the standard +# style sheets created by doxygen. Using this option one can overrule +# certain style aspects. This is preferred over using HTML_STYLESHEET +# since it does not replace the standard style sheet and is therefor more +# robust against future updates. Doxygen will copy the style sheet file to +# the output directory. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that +# the files will be copied as-is; there are no commands or markers available. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. +# Doxygen will adjust the colors in the style sheet and background images +# according to this color. Hue is specified as an angle on a colorwheel, +# see http://en.wikipedia.org/wiki/Hue for more information. +# For instance the value 0 represents red, 60 is yellow, 120 is green, +# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. +# The allowed range is 0 to 359. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of +# the colors in the HTML output. For a value of 0 the output will use +# grayscales only. A value of 255 will produce the most vivid colors. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to +# the luminance component of the colors in the HTML output. Values below +# 100 gradually make the output lighter, whereas values above 100 make +# the output darker. The value divided by 100 is the actual gamma applied, +# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, +# and 100 does not change the gamma. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting +# this to NO can help when comparing the output of multiple runs. + +HTML_TIMESTAMP = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of +# entries shown in the various tree structured indices initially; the user +# can expand and collapse entries dynamically later on. Doxygen will expand +# the tree to such a level that at most the specified number of entries are +# visible (unless a fully collapsed tree already exceeds this amount). +# So setting the number of entries 1 will produce a full collapsed tree by +# default. 0 is a special value representing an infinite number of entries +# and will result in a full expanded tree by default. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files +# will be generated that can be used as input for Apple's Xcode 3 +# integrated development environment, introduced with OSX 10.5 (Leopard). +# To create a documentation set, doxygen will generate a Makefile in the +# HTML output directory. Running make will produce the docset in that +# directory and running "make install" will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find +# it at startup. +# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. + +GENERATE_DOCSET = NO + +# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the +# feed. A documentation feed provides an umbrella under which multiple +# documentation sets from a single provider (such as a company or product suite) +# can be grouped. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that +# should uniquely identify the documentation set bundle. This should be a +# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen +# will append .docset to the name. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely +# identify the documentation publisher. This should be a reverse domain-name +# style string, e.g. com.mycompany.MyDocSet.documentation. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can +# be used to specify the file name of the resulting .chm file. You +# can add a path in front of the file if the result should not be +# written to the html output directory. + +CHM_FILE = + +# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can +# be used to specify the location (absolute path including file name) of +# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run +# the HTML help compiler on the generated index.hhp. + +HHC_LOCATION = + +# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag +# controls if a separate .chi index file is generated (YES) or that +# it should be included in the master .chm file (NO). + +GENERATE_CHI = NO + +# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING +# is used to encode HtmlHelp index (hhk), content (hhc) and project file +# content. + +CHM_INDEX_ENCODING = + +# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag +# controls whether a binary table of contents is generated (YES) or a +# normal table of contents (NO) in the .chm file. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members +# to the contents of the HTML help documentation and to the tree view. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated +# that can be used as input for Qt's qhelpgenerator to generate a +# Qt Compressed Help (.qch) of the generated HTML documentation. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can +# be used to specify the file name of the resulting .qch file. +# The path specified is relative to the HTML output folder. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#namespace + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating +# Qt Help Project output. For more information please see +# http://doc.trolltech.com/qthelpproject.html#virtual-folders + +QHP_VIRTUAL_FOLDER = doc + +# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to +# add. For more information please see +# http://doc.trolltech.com/qthelpproject.html#custom-filters + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see +# +# Qt Help Project / Custom Filters. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's +# filter section matches. +# +# Qt Help Project / Filter Attributes. + +QHP_SECT_FILTER_ATTRS = + +# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can +# be used to specify the location of Qt's qhelpgenerator. +# If non-empty doxygen will try to run qhelpgenerator on the generated +# .qhp file. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files +# will be generated, which together with the HTML files, form an Eclipse help +# plugin. To install this plugin and make it available under the help contents +# menu in Eclipse, the contents of the directory containing the HTML and XML +# files needs to be copied into the plugins directory of eclipse. The name of +# the directory within the plugins directory should be the same as +# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before +# the help appears. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have +# this name. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) +# at top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. Since the tabs have the same information as the +# navigation tree you can set this option to NO if you already set +# GENERATE_TREEVIEW to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. +# If the tag value is set to YES, a side panel will be generated +# containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). +# Windows users are probably better off using the HTML help feature. +# Since the tree basically has the same information as the tab index you +# could consider to set DISABLE_INDEX to NO when enabling this option. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values +# (range [0,1..20]) that doxygen will group on one line in the generated HTML +# documentation. Note that a value of 0 will completely suppress the enum +# values from appearing in the overview section. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 250 + +# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open +# links to external symbols imported via tag files in a separate window. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of Latex formulas included +# as images in the HTML documentation. The default is 10. Note that +# when you change the font size after a successful doxygen run you need +# to manually remove any form_*.png images from the HTML output directory +# to force them to be regenerated. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are +# not supported properly for IE 6.0, but are supported on all modern browsers. +# Note that when changing this option you need to delete any form_*.png files +# in the HTML output before the changes have effect. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax +# (see http://www.mathjax.org) which uses client side Javascript for the +# rendering instead of using prerendered bitmaps. Use this if you do not +# have LaTeX installed or if you want to formulas look prettier in the HTML +# output. When enabled you may also need to install MathJax separately and +# configure the path to it using the MATHJAX_RELPATH option. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# thA MathJax output. Supported types are HTML-CSS, NativeMML (i.e. MathML) and +# SVG. The default value is HTML-CSS, which is slower, but has the best +# compatibility. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the +# HTML output directory using the MATHJAX_RELPATH option. The destination +# directory should contain the MathJax.js script. For instance, if the mathjax +# directory is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to +# the MathJax Content Delivery Network so you can quickly see the result without +# installing MathJax. +# However, it is strongly recommended to install a local +# copy of MathJax from http://www.mathjax.org before deployment. + +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest + +# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension +# names that should be enabled during MathJax rendering. + +MATHJAX_EXTENSIONS = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box +# for the HTML output. The underlying search engine uses javascript +# and DHTML and should work on any modern browser. Note that when using +# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets +# (GENERATE_DOCSET) there is already a search function so this one should +# typically be disabled. For large projects the javascript based search engine +# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. + +SEARCHENGINE = YES + +# When the SERVER_BASED_SEARCH tag is enabled the search engine will be +# implemented using a web server instead of a web client using Javascript. +# There are two flavours of web server based search depending on the +# EXTERNAL_SEARCH setting. When disabled, doxygen will generate a PHP script for +# searching and an index file used by the script. When EXTERNAL_SEARCH is +# enabled the indexing and searching needs to be provided by external tools. +# See the manual for details. + +SERVER_BASED_SEARCH = NO + +# When EXTERNAL_SEARCH is enabled doxygen will no longer generate the PHP +# script for searching. Instead the search results are written to an XML file +# which needs to be processed by an external indexer. Doxygen will invoke an +# external search engine pointed to by the SEARCHENGINE_URL option to obtain +# the search results. Doxygen ships with an example indexer (doxyindexer) and +# search engine (doxysearch.cgi) which are based on the open source search engine +# library Xapian. See the manual for configuration details. + +EXTERNAL_SEARCH = NO + +# The SEARCHENGINE_URL should point to a search engine hosted by a web server +# which will returned the search results when EXTERNAL_SEARCH is enabled. +# Doxygen ships with an example search engine (doxysearch) which is based on +# the open source search engine library Xapian. See the manual for configuration +# details. + +SEARCHENGINE_URL = + +# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed +# search data is written to a file for indexing by an external tool. With the +# SEARCHDATA_FILE tag the name of this file can be specified. + +SEARCHDATA_FILE = searchdata.xml + +# When SERVER_BASED_SEARCH AND EXTERNAL_SEARCH are both enabled the +# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is +# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple +# projects and redirect the results back to the right project. + +EXTERNAL_SEARCH_ID = + +# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen +# projects other than the one defined by this configuration file, but that are +# all added to the same external search index. Each project needs to have a +# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id +# of to a relative location where the documentation can be found. +# The format is: EXTRA_SEARCH_MAPPINGS = id1=loc1 id2=loc2 ... + +EXTRA_SEARCH_MAPPINGS = + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = YES + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be +# invoked. If left blank `latex' will be used as the default command name. +# Note that when enabling USE_PDFLATEX this option is only used for +# generating bitmaps for formulas in the HTML output, but not in the +# Makefile that is written to the output directory. + +LATEX_CMD_NAME = latex + +# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to +# generate index for LaTeX. If left blank `makeindex' will be used as the +# default command name. + +MAKEINDEX_CMD_NAME = makeindex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4 + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for +# the generated latex document. The footer should contain everything after +# the last chapter. If it is left blank doxygen will generate a +# standard footer. Notice: only use this tag if you know what you are doing! + +LATEX_FOOTER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = NO + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = YES + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +# If LATEX_HIDE_INDICES is set to YES then doxygen will not +# include the index chapters (such as File Index, Compound Index, etc.) +# in the output. + +LATEX_HIDE_INDICES = NO + +# If LATEX_SOURCE_CODE is set to YES then doxygen will include +# source code with syntax highlighting in the LaTeX output. +# Note that which sources are shown also depends on other settings +# such as SOURCE_BROWSER. + +LATEX_SOURCE_CODE = NO + +# The LATEX_BIB_STYLE tag can be used to specify the style to use for the +# bibliography, e.g. plainnat, or ieeetr. The default style is "plain". See +# http://en.wikipedia.org/wiki/BibTeX for more info. + +LATEX_BIB_STYLE = plain + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimized for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using WORD or other +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load style sheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assignments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +# Set optional variables used in the generation of an rtf document. +# Syntax is similar to doxygen's config file. + +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, +# then it will generate one additional man file for each entity +# documented in the real man page(s). These additional files +# only source the real man page, but without them the man command +# would be unable to find the correct page. The default is NO. + +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. + +GENERATE_XML = NO + +# The XML_OUTPUT tag is used to specify where the XML pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `xml' will be used as the default path. + +XML_OUTPUT = xml + +# The XML_SCHEMA tag can be used to specify an XML schema, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_SCHEMA = + +# The XML_DTD tag can be used to specify an XML DTD, +# which can be used by a validating XML parser to check the +# syntax of the XML files. + +XML_DTD = + +# If the XML_PROGRAMLISTING tag is set to YES Doxygen will +# dump the program listings (including syntax highlighting +# and cross-referencing information) to the XML output. Note that +# enabling this will significantly increase the size of the XML output. + +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- + +# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will +# generate an AutoGen Definitions (see autogen.sf.net) file +# that captures the structure of the code including all +# documentation. Note that this feature is still experimental +# and incomplete at the moment. + +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- + +# If the GENERATE_PERLMOD tag is set to YES Doxygen will +# generate a Perl module file that captures the structure of +# the code including all documentation. Note that this +# feature is still experimental and incomplete at the +# moment. + +GENERATE_PERLMOD = NO + +# If the PERLMOD_LATEX tag is set to YES Doxygen will generate +# the necessary Makefile rules, Perl scripts and LaTeX code to be able +# to generate PDF and DVI output from the Perl module output. + +PERLMOD_LATEX = NO + +# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be +# nicely formatted so it can be parsed by a human reader. +# This is useful +# if you want to understand what is going on. +# On the other hand, if this +# tag is set to NO the size of the Perl module output will be much smaller +# and Perl will parse it just the same. + +PERLMOD_PRETTY = YES + +# The names of the make variables in the generated doxyrules.make file +# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. +# This is useful so different doxyrules.make files included by the same +# Makefile don't overwrite each other's variables. + +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_DEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# pointed to by INCLUDE_PATH will be searched when a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. To prevent a macro definition from being +# undefined via #undef or recursively expanded use the := operator +# instead of the = operator. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition that +# overrules the definition found in the source code. + +EXPAND_AS_DEFINED = + +# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then +# doxygen's preprocessor will remove all references to function-like macros +# that are alone on a line, have an all uppercase name, and do not end with a +# semicolon, because these will confuse the parser if not removed. + +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES option can be used to specify one or more tagfiles. For each +# tag file the location of the external documentation should be added. The +# format of a tag file without this location is as follows: +# +# TAGFILES = file1 file2 ... +# Adding location for the tag files is done as follows: +# +# TAGFILES = file1=loc1 "file2 = loc2" ... +# where "loc1" and "loc2" can be relative or absolute paths +# or URLs. Note that each tag file must have a unique name (where the name does +# NOT include the path). If a tag file is not located in the directory in which +# doxygen is run, you must also specify the path to the tagfile here. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed +# in the modules index. If set to NO, only the current project's groups will +# be listed. + +EXTERNAL_GROUPS = YES + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base +# or super classes. Setting the tag to NO turns the diagrams off. Note that +# this option also works with HAVE_DOT disabled, but it is recommended to +# install and use dot, since it yields more powerful graphs. + +CLASS_DIAGRAMS = YES + +# You can define message sequence charts within doxygen comments using the \msc +# command. Doxygen will then run the mscgen tool (see +# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the +# documentation. The MSCGEN_PATH tag allows you to specify the directory where +# the mscgen tool resides. If left empty the tool is assumed to be found in the +# default search path. + +MSCGEN_PATH = + +# If set to YES, the inheritance and collaboration graphs will hide +# inheritance and usage relations if the target is undocumented +# or is not a class. + +HIDE_UNDOC_RELATIONS = YES + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = YES + +# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is +# allowed to run in parallel. When set to 0 (the default) doxygen will +# base this on the number of processors available in the system. You can set it +# explicitly to a value larger than 0 to get control over the balance +# between CPU load and processing speed. + +DOT_NUM_THREADS = 0 + +# By default doxygen will use the Helvetica font for all dot files that +# doxygen generates. When you want a differently looking font you can specify +# the font name using DOT_FONTNAME. You need to make sure dot is able to find +# the font, which can be done by putting it in a standard location or by setting +# the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the +# directory containing the font. + +DOT_FONTNAME = Helvetica + +# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. +# The default size is 10pt. + +DOT_FONTSIZE = 10 + +# By default doxygen will tell dot to use the Helvetica font. +# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to +# set the path where dot can find it. + +DOT_FONTPATH = + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for groups, showing the direct groups dependencies + +GROUP_GRAPHS = YES + +# If the UML_LOOK tag is set to YES doxygen will generate inheritance and +# collaboration diagrams in a style similar to the OMG's Unified Modeling +# Language. + +UML_LOOK = NO + +# If the UML_LOOK tag is enabled, the fields and methods are shown inside +# the class node. If there are many fields or methods and many nodes the +# graph may become too big to be useful. The UML_LIMIT_NUM_FIELDS +# threshold limits the number of items for each type to make the size more +# managable. Set this to 0 for no limit. Note that the threshold may be +# exceeded by 50% before the limit is enforced. + +UML_LIMIT_NUM_FIELDS = 10 + +# If set to YES, the inheritance and collaboration graphs will show the +# relations between templates and their instances. + +TEMPLATE_RELATIONS = NO + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT +# tags are set to YES then doxygen will generate a graph for each documented +# file showing the direct and indirect include dependencies of the file with +# other documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and +# HAVE_DOT tags are set to YES then doxygen will generate a graph for each +# documented header file showing the documented files that directly or +# indirectly include this file. + +INCLUDED_BY_GRAPH = YES + +# If the CALL_GRAPH and HAVE_DOT options are set to YES then +# doxygen will generate a call dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable call graphs +# for selected functions only using the \callgraph command. + +CALL_GRAPH = YES + +# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then +# doxygen will generate a caller dependency graph for every global function +# or class method. Note that enabling this option will significantly increase +# the time of a run. So in most cases it will be better to enable caller +# graphs for selected functions only using the \callergraph command. + +CALLER_GRAPH = NO + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will generate a graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# If the DIRECTORY_GRAPH and HAVE_DOT tags are set to YES +# then doxygen will show the dependencies a directory has on other directories +# in a graphical way. The dependency relations are determined by the #include +# relations between the files in the directories. + +DIRECTORY_GRAPH = YES + +# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images +# generated by dot. Possible values are svg, png, jpg, or gif. +# If left blank png will be used. If you choose svg you need to set +# HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible in IE 9+ (other browsers do not have this requirement). + +DOT_IMAGE_FORMAT = png + +# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to +# enable generation of interactive SVG images that allow zooming and panning. +# Note that this requires a modern browser other than Internet Explorer. +# Tested and working are Firefox, Chrome, Safari, and Opera. For IE 9+ you +# need to set HTML_FILE_EXTENSION to xhtml in order to make the SVG files +# visible. Older versions of IE do not have SVG support. + +INTERACTIVE_SVG = NO + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found in the path. + +DOT_PATH = + +# The DOTFILE_DIRS tag can be used to specify one or more directories that +# contain dot files that are included in the documentation (see the +# \dotfile command). + +DOTFILE_DIRS = + +# The MSCFILE_DIRS tag can be used to specify one or more directories that +# contain msc files that are included in the documentation (see the +# \mscfile command). + +MSCFILE_DIRS = + +# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of +# nodes that will be shown in the graph. If the number of nodes in a graph +# becomes larger than this value, doxygen will truncate the graph, which is +# visualized by representing a node as a red box. Note that doxygen if the +# number of direct children of the root node in a graph is already larger than +# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note +# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. + +DOT_GRAPH_MAX_NODES = 50 + +# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the +# graphs generated by dot. A depth value of 3 means that only nodes reachable +# from the root by following a path via at most 3 edges will be shown. Nodes +# that lay further from the root node will be omitted. Note that setting this +# option to 1 or 2 may greatly reduce the computation time needed for large +# code bases. Also note that the size of a graph can be further restricted by +# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. + +MAX_DOT_GRAPH_DEPTH = 0 + +# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent +# background. This is disabled by default, because dot on Windows does not +# seem to support this out of the box. Warning: Depending on the platform used, +# enabling this option may lead to badly anti-aliased labels on the edges of +# a graph (i.e. they become hard to read). + +DOT_TRANSPARENT = NO + +# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output +# files in one run (i.e. multiple -o and -T options on the command line). This +# makes dot run faster, but since only newer versions of dot (>1.8.10) +# support this, this feature is disabled by default. + +DOT_MULTI_TARGETS = YES + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will +# remove the intermediate dot files that are used to generate +# the various graphs. + +DOT_CLEANUP = YES diff --git a/src/hal/components/cros/LICENSE b/src/hal/components/cros/LICENSE new file mode 100644 index 00000000..1ba70868 --- /dev/null +++ b/src/hal/components/cros/LICENSE @@ -0,0 +1,12 @@ +Copyright (c) 2016, IT+Robotics srl, Italy +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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 HOLDER 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. diff --git a/src/hal/components/cros/NOTICE b/src/hal/components/cros/NOTICE new file mode 100644 index 00000000..e8d9bf90 --- /dev/null +++ b/src/hal/components/cros/NOTICE @@ -0,0 +1,8 @@ + cROS + Copyright (c) 2014 IT+Robotics Srl + + This product is licensed under the terms of the GNU Lesser + General Public License v3.0. + + Portions of this software were developed by Alexander Peslyak + under the terms of a reduced BSD license. diff --git a/src/hal/components/cros/README.md b/src/hal/components/cros/README.md new file mode 100644 index 00000000..eca8a63d --- /dev/null +++ b/src/hal/components/cros/README.md @@ -0,0 +1,63 @@ +![cROS logo](resources/cROS_logo.jpg) + +# cROS 1.0-rc2 + +cROS is a library written in stadard C language that provides a single-thread +implementation of the basic features required to implement a ROS +(Robot Operating System) node. +This ROS node supports the concurrent operation as several: + + * Topic subscribers + * Topic Publishers + * Service providers (servers) + * Service callers (clients) + * Parameter subscribers + +cROS can run on Linux, OS X and Windows. + +### Requirements + +On a Linux-based distribution it can be compiled with: + + * The GCC compiler + * CMake 2.6 or later + * (Optional) Doxygen + +On Windows it can be compiled with Microsoft Visual Studio 15 or later. + +### Build cROS + +On Linux cROS uses CMake, the cross platform build system. To create a build directory +an compile the project, type the following commands in a terminal starting +from the source root directory: + +```bash +$ mkdir build +$ cd build +$ cmake .. +$ make +``` + +You will find a static library (*libcros.a*) inside the *build/lib* directory, and +some test executables that makes use of the libcros library inside the +*build/bin* directory. The entrypoint sample to learn how to use cROS is +*samples/ros_api.c*, which shows a non trivial example of a ROS node with +publishers/subcribers and calls to ROS services. + +If you want to build the create the library documentation, type: (you'll need +Doxygen) + +```bash +$ make docs +``` + +You'll find the html documentation inside the *docs/html* directory. To browse +the documentation, open with a web browser the index.html file. + +On Windows cROS can be compiled opening the solution file msvs_projects/cros.sln with +Microsoft Visual Studio and building the solution. + +### Licensing + +cROS is licensed under the BSD license. For more +licensing options and for paid support please write an email to cros@it-robotics.it. diff --git a/src/hal/components/cros/docs/cROS_manual.doc b/src/hal/components/cros/docs/cROS_manual.doc new file mode 100755 index 0000000000000000000000000000000000000000..cd4f7a58e1376a5959b788b858125409f4070248 GIT binary patch literal 491008 zcmeFa31D1R+5bPKg_I?LKF{nLGE~vpws1 z&T{Yl(MvX5eakC;lPdZ-Dm6Cs&*yhe?G$`Jp3h$xYrohtmHHB&$8h=2=bwLGzWprM zp97xOu`u5r5G-3OP` zkT*`JCES18tW;_e=|9O|`6>Q8;lxyGBHVq*$*I(1+~4DrRBD*NedMzPe^2MH{M7ST z>i_ZkksKa8`N33b2G{T6`3aIi(T{8Q1(a9r=YuO!sW)>&>L))go%<{KhCi-f-Cx&E zmqxDta3J;Lxruza_f6(b@n6aR!?O*&zAOD}`_0wYT}%IjKQ5o}yLvvm_EgGG^1Vkg zx+|4>kNn*)mHLHYRJq#ZpZ>eeqpm%U-`Y?9pXWcY;{(0~_x%CyPNfcNpbpeypP z$wy1lgCpsF`DVAq*4*GQPZqe?J6gW1H99R9O=_7oYf^MtUv%1;(P<;mK6^*Up4{Hn zv3Swa+o5JO^U|svuJQQpX(Xvmg0BbJ8GYQOj}3W!YGkW z@*A_yw5TPTHemu~RnGEo$x>=Rm>FK1%df7Gp`Sd`gTF$ILSLirqpZ0w2 z^i20~)RWEgCzsDwZ2zqKbDr({Gz$mid(+*SD7Pk)Uz5$Ot$I3WfBlOjO7W z_EctyLtXxd)~v0OE}uWbyCzFNchfPI^FOTq{8iLcFy;>p^=G?{0)bzI{RSu^LbMHW zE6~(SVa>+OiKTEb*F$He2k9$OTjjH!pbl!y@xnZ_rjD4S(kyChNxA@8YRSbMFU~V-3dS6j=Q3S* z(vrm;?XA&J8ji?Fp@rc*n!d6Uo#VxP){I&VK1b!eEuYWjqi&d@;3-!_r(KU25cQi+>>3lxh-=CWr&0i(Lm>rBx%<2=ud-FAE)Sq725d1Xm7FuIl z-)tdDTeo3a6}KU+PD4{iZe?ybS~ir)qYo&yb3NQPFgfb(&q`PHXIJLa`Ss0>!inS@ zI~mAycCddv^~9&>$@E0MLRY*L%_gterej4@h)&%*Qbz&e%szC++?LsgAC5xF8wJ)f z;SJ5vqG7RyYqG){=|Roe*c#0`H!v_VsA`L9s>`5E)U>uQ+ua98jr8|KD>IR(fr9j2 zI$GY^K7CQUb1f!M<=gK55z*FOdk>`x;$cmV@|l4gTAlK{Gd(c8>fNXbahR;a?`2}z znsomN#Oq7vd9XQJ-bc4|$CZP`v61AKci8BnmFpoojF9fv1|<3zLtWfOv**l$KV28i zn(&6HQDKC(rVCL%SLhxZqT0E_(0cvTpDhe&O@(QvMz&Yz#_n8Z9Sm*m`Fdue^gxb| zznHw24#>DbHTren1)teg; z127m;NWJ3OGY-RaStgzd6E~`)SZ}tG8kt7gFGPgkm7!b#ZlEp!8~ctod?HmY?in~Z zB&(B$3^g2Ho5>7HI|qa#GxT(vwY0(KJ`cGV8d(XqNb9LWIvUOmWxJy$bVY7tRUhQe z^kjOogVaOvuzpT~!ci+DD@~u~dCEPiH>~}$@|%yE$anpa5?o{`Od}pmCSNW)FJx1N zLmW?0No`o;vAMM(JyVsQQa8t$NbeaV*yW#Vem#g`|llMGH8WK-4#V?rmyMR&_aA zm2yp&rz2~5z2>lWu(n^TLa*i4Vl=f9F?FE>#Dbe(U1?Ohr-xRman&}eI#eOFWEV0@ z=k!8!Lwfq!BrwWXv(X-f6I?UORtOCsmeNXCKaNUdw4)(e&}heUssrPCN#BeN^@u;s zmjkf3;bNOjcVBvNRi=PYEvIE+XF$;rQ3@C!wK+y;1~TbE-*QyrfN`Cis;~klh4!Zz zX7usYh^T+lVpOW=B+=z0X^uR*d8OvGuQWg?Fr3Tvugt9zF(dO&@SqLeNn^Xin`o^N?j*dgb-fT~2^q7o*cqGGd5% zC_9X72vg#HIJOT8hH;!ll{Oi6P+s6c$NSAuqMSenBn#4(mXv0jSiYp3RcMsvvn`+* z=su~BeKwzAyaJP{%*pq@sn)zW&K?$9Y#M=$#G4vXX_S5#i~{!q&!c75+!=N>urRwy z41UW5Rh>pP_}YlCO0cDS*r)(sGDdi{aE_QFsghIhPW+?^L%IC0PvFLj49hd>H%i8X z#s#!oXp}(tT4V~taFA$isfcvEe|l<^S!X4Mb{3m6shlg@+%w&qM^px~ct5DB0hEM( zOCeNQs+e8lc+>JMlo4z(pSdUs8M=Q9Eh7=tDvl{-j617&GvSI@VQ`u|-7Bt2> z3fV6m@Sc>ZBs8W2+gytmUCpzDLs%>k6D*f}tj3%;z6>%<6I=#jp{SN-VK~;>p*TTd zC^v{JR7)4koYgWnsS9RKh?>gsb%Jeen?>Ko{7G-e+*v5ko}0y=GvQCyM8|H^EtwMI z5YL>k?-B5^sE|!E5^-+EU#7w2rJf7q$Y3}_MpY&=Cf+Kp_<3O*%a92r8`d&upawH* z1E<3H8g)?jFdH1P!amDW*9AQ62q&a&Scz>)-&xQ%?jzH6`6?R7L@8XbI}8GdMl@rH zX>L5@#{tGBwkomwSiy4y164IL?lC(I%8xJ_VhoFEi033wRD+^j7!9I>ln(~g@gAJE zvOm|YR#GM}rmxN8qDMIsYCtfXqsCT-CQ?i1>QeD2GaPrCU@@z4&YTtjion| zBMIpQBUIiZnG$SqJ`3BQQuR_d)kG-_flQrkqh(NpwAqJEt%48>Q6)S0c7CdHN&`CzWHxwV7+xwHg-Yl8Su?dAA}RS^u6Xq3=FqGx!Y>3jjMcZJAT&JP^Utb=MY$j|Cf zwKv;O%BJRl!YX~VxsYEoS$I3MF4H}tNrtmTf|>?%5*}ES&5aNWv=bmOWgp_r$k_Cj zkeL9L^%5s*tt_`l=!2pRo0z-}n=j4|j;zz7Q3fYPO>lW1y}43@8fsOH^S`RQ8y&q& z;*Q-KQwmCoEOR@qmT;2@n6;5TLT^epgLA^$eYqa38(A=p&kRWvlQ7t{-h3wG;+{u{ zyag;u`;FKkBl(_bbn)9aC(mLxCg^?i{Rbac(iqn!4W`^o^!a~O->cu+$-IXYHAVV3B`T>>bU^jw^2eTR* z&UP1UP&is{hP%jCtF{A)wB}~57-3;19g*V1OB$lH zjg9krWyZ&}oPLsMxfZDpOve*omTP3Cn^zVGfsPi&aZZA05xMBbS~RCc324}~IqI}A zZ_^MBgcyxOyamIIldds&xW%bY;?CjRIMH?&-uIDMQ>~TR!DO}Q#iEIIT1LJ^1+i>0 zUZ9_J!d)upbpvI`;exnnCTJ|(tdEL=&BN*jH^eTY)X zjpPZtYggb4ksGYzABvmo6{R2QR%*M{2s0%1t+CxSQ?nl=jNh;zn6hfXsag%SrDX&a zn8C^98}wnTsePG+?7&F>aC$I<%17(cPudf-AR^EihY+OI33a1+JL4nTs?gBjZB}8L zWad;x1*sqpb;`u5a%k}6RL4TbF)DFVEOlJc#C4~sRLiK~9(!TH^f4`!}5KE?4quffRB+r_( zh7r_SOOG_Q!d*-Ggj1yB869nlNco`*?6roON>#dpBLgduE^>=~NpUgVbPyx5g~*qk zdLxvRX0kC(8h@l3I6_FHa;{N?1L<|Bih(FDL?4$_IA(4}Nn%H#&L2o)EutoOTMQ^o z>XLRe_o)jio<+q#?x8KSXUv`>^T@K`%nt8oPoQr%CjvN|5xDDFv4Ya~hA2t}?PQFH z5nw-~(cNKa(W+6ErpaD`gtXzY*iXs~4Beq;m^YpLQNgb4UObw&e3oYqAhdmD(hZT&4U|rh519-4m(}Y>BF!z zWjasl-4#CY8s0B{w%u`5`A37>#ZPt`@oA+|nSA(khZ*-5-LLRCwoh7^$RMb>DI-qg za`8=Sm&v-Y^c91i8up$Iu1S~s$2`TwO6fGWx&o2wQhy&NFdn@4$mEce389`OD98lW zi133roF?FH=N7GNCW^p@Fq(+FGOu8=E23nwPu(OkB}mZ21=77|Vsi{*VL|hK8;aT( zcdt*eDXny&Q`1gAM?hVZcSNJ(A)-;W#jeX(g_D!Xd=ZNEG)-egX5f)#fU{PqBKk4c z-90kIY6l!T1Vcx`Mva9Yi_DqXP|0A1{t3DYg18D{;j9-PlY_CniHm!o(wY=;ukR`6 z1xdBZ%mq(a(2#QuP3+;Q8^CBC9*7G>RcaVkD9WUg7$lir_Ti{BaVFs^A>q9{yo z$LRnkDT|Pf=akbUKeUSyCfmc=507@+Wk) zNPBW9i_dPfnpS(74=ng>@p~AIA!9nVCt3Qqgw5B6ks!4?fQ;LuceNPbmsT_TYbI-T zj$QJ@G1TgsxpLIBelyKkDpwPl!&HiDV!Q`SKpQ-4X5>-g+r>&-n9tYA=VvE)gjrpL z#l_jh?8;INq`^*j^hkh6S1=hd(1|@6ozw()Tg%B2jim$vIQLMKnQ!5|+A3 zAz=}hkr~6n^#NgQJ^03FqaC9e(K{ldO0(ij!XC1{C<7vA>8ldS<)tkvtxPPIo=CnL zj-i4^#nWr6WPE5BX%rbNSETC$seUX^-fGs_1npI}HV|Gks>G-*i3OS&ZWvySp<5m1 zNf8(6f2=C!E0eL)T+SUDZjaY>5jPTMx-3y5KarB=W3#!sYRm`G5WjPsS zNYm7+xw)Mvm^?Dxj|pxYiz(X7bnB3bDPyVaa(FWsDlu*9V%P{ZZj~~Jil4ULIAG2) z8JosR1vku)XmOEovp$UtCE&ZHBRZDFw@vg#e`atME2)>vKdI}46>Tfpx)!%B^}|fk z>NwJt)}@azp!5!9pbDM%br|EhTxzkTunPB|4zS!NU6<<@!SSZmx>BY>&i?0C4l`_* zIq*d`6iiJ`64@`)i+7=9D9#I#WeeSDrU{snP=>bMXNUj;Nezq|?vGdHJDzv}luo7YDPcKA%`kiJZBj0_RyYgp{o7sbwln zs4SXKN9XQwA@<6QXjJFT=tFUba$hD_2W7>w3~+RiLDno-H+q-^I#DMUO&8c0dj)@p zv4f4Ye*I|{p1E~5CWW)`uwP@vJx)gKD{q6LPC;1Jc+bqLIgz0x^kjZzmSCt1PUr?5 z#t?pHYaNJVbRi!jR=v%#^681DGZ6d#xPBEy4R0ToDLLMMFwoz;`K~XsGc+fIr+{E z&tfCuo}-{GsuQ&(8LOwXMR=QKbT+r>9Z^PZiZhdE!4Kip8hvWiqbvrASruXM)=~P2 zWgpFvbv?9S{o)b1Yr3MMB+Sz>#-?(-s)?Ab>3StP&2_Wb$_(|YU#3i$X7*!UvEAx0 zisNc^gO3k*tp@9e#jvB^(#kk4lUaVh4=waZVAEpxEV^B8bgPqqVY)hn)G~Sn$7d78 zbh+qe&q}(XP9zsg>{=f&0ZcazP)mt%yZv}!1MHaIKcQ@7q}lqZu~`}+hM>XIgl7lU z?ZQFj09gww7JXBB(=35eu`)Ybt%Z-PFP$k`=Ecfz#^To8HEP&T9fBsE36+Vv++a+- zG9Ig&I~G?OYp(ls%&d``s_uS?ZZNlngI{j%}k)$s5T}mRt)iWAmU1Sqw1q6 zj;m~Jo`FF=nj#UfE}0rdfyH0+F^B8E;CTwobeZo)m?G0<4a^?ODrtLD)?>H_tcLE& z=W@ectg`5vES(b2-cB08zZ!sZ4xEk(&042Gb`pU+G(|YSY}cV^!yT_Pf1`!z9DwRv zJMk7JB)k2h zOQ%w0MMJya_M&sp@ueZQ*+ALgDbcW0Hk?^!%ny}~U!bEWb**v2WxhmAr>+~t6=$j# z4~OYb%}VIiaI8$myXE+1-xP!pesXJ5q~X30_F(wbwgMO|!iJLBWHXWLnW15Ra+i(d zVgvC~fgrVTM?<^t3$--0aK*-p56u)jDnR!@QcJhDrKM%otdBd?ZdU1JI~9o$Y6z1eFna z%N5wk+e%y8Wz)WsFB2<>O`8FUzIsA8`qwd z2GXo;))9?yB29G5Kj*^fvdeWSG2S*!;yXD*g+YRC6?wC6KxTZmsHnmQn<}Z=7ux7J zOhPSXDIL8&qlj4QZYZ^c&>SbCi$RQXFsmA6wpQGB2DWS;Gv>vx+Y2IMm1WFE!js}v zB23y~T`TOE%P&OIffjLN%U;qUo*|nfyGB)d7VfZ&DP~6JI$$|Er*v*vGKswmjjCce z;^7b*JqV|pUJb%xY^3H%qmBY|poJJqu>~3K1yGb_saSp$OOl>=>=ItLp1-=kZY)sp zy=-Kk8ZYz6lYc9O({Q~gLisQ?S_%pAf0(=SH<=IMvMWxrCYSADG$QA4t-}FAVMoK= zL%Q6lKg~G~^2HQF;@O^cT|>k95~ef0nT#HM+?DcOoO~>k!)pEeXQ)BSmYHCAB=;dBWk~afT=U%u4pk^PWwqCl%%q- z6_q8E3n(t@gEP2sGY{g`Xkwvr!*Y9Sfi$DqLvmV)jqplR8SF{+X&3D@a}ma`k%f+k zrZ{@;01|mdz7}7Ma4#!xWhX9Gam*4rKIz!*h}R%B>~ytHaAUc}2)YbU6>)L~MlFsD zZY;BYKF(QAc2~Ful}KlZWqOa( z0q`iBaalB88550)n_*+HXa_et;CtakeF!}oPa6C6ZJ8bh4neoz)7Wy|ZbnhmvMvY> z3fYWI$1M++g~+5F#(++A)K!84$Q)d1g83=ks3E7KPy`5W6GwUaQ`>A zC8;aY1_#0v{?Htyal8Z=-_41`Q2mW*C=Ir(l?X(R|}Q0f>Uv-Z3wr7R=r^ zCKiGrI!hGsg@|tHnq2KxT|4TRuFGm&7}*5-6Wl0?gU3l^R(@G-d;u)+a%!ZS$&PCr z+j&l+C9EcIwf3^Z#aPQKGMz76VyEG!SOQIr#C%TCp+c9HH?@#*zyUM;W}ZfR#yhNb zSq9zX_{-FyzOk?^x4~6}kz2NRy&K1QJ!q_iO$`6a?7OR<=K-q%WFd>u6mcbu1ewrx zS=Yr^p3D=JWnHETX02SWC#b;P@!BlorpJ1*lps1}&9G}g;u{i%@ZNokoWT*q59F3} z7u}Xl?`ly0Az#I!2~5Va#f;&!xAUNqlu-|<)Ij)^?wzXpR}_ffnvAwNSD0cO7U9E* zI47Bccs28R)Dd#3d7^KfpD?enRf7l1=PzH;i85Q-HqV}OBeJV1mvjN5|{FH`f;z zKUo$XbeUk^)W~POMSb%rtN^O*it92VJx;5dCmy7#;E0RnDsjR^dg4c|xyMxTwbeLD z|JlfUVtkNv&XHuFp9zoIe9YHwxs&xdO#k9Ji%~Z-<2W)2C&Zs&wPV(LpA zZ?R!+VVSxD)6_Te1ah<1erm)zSX9qxes7F$&&mub+TE^=P==~I=E)I7Yx{e^d)2m0& ze{`Y*Ty%63Pra6TNE9)hX4JeNyVvNuQj2AZRA&=P2ni6Jcq}FY>?mAoL=&egJ5Gt$ zO~#{}?PC{4({+wrWZSLNT`1F0jb7q0Q=A3N1I4V}Nbt?*1_3eIVda18F+h76>X% z0wm^g%&uEEm6I3_z<135gZIcGaJRftbfHryo_pTBWE%RN-9R_`AhM z9i9}PpJC@JgO1=_KY}SV3-O`u$*%Bm2Hoh^EoY^;R>)G>qQQrqT8LdCr=(4$PNn)T zEzN=9i&VlrXDxw$K!Yz^W_&~EY#-ok<^ZRC#nFowbsk$XzKL0}irL#5!<)EZXBLFa zHK!U9+bpY%aO@hIGV=}Yjs#N7Y(U^bF$ED85qfFP!`tLp2)w9*o&!;NR$@(e?V*sZ z|1@=XYRpUoyRdW^9nT=(Rmyaj`fQ02PplGbK`P1A-K+_eQV9n;kB}?Qv&#GJl%pHK zjBfQ(Xy!%r_}~bfJx`Rg3FNLq35H+|gFSwSk?N;+4ocu_q|+hjt75f$vAKDldw)TYMJ#T@4^ zM0VOi#x}OekkrkSIO}6~6~-2i{AI#RAHsmU9erK0>{Qo31))Ll{Op9gIftHd@gg-= zS6ZYGNycT{m8{7k<;kG9xR93QxWP%1=U92r%uv@JTE%HAjHn2VQL@WO17-0DQlpo; z-=glh6_qtj7yAl|lnOm%tlab0Ve8Pz22qEB1IU#Ri0ApCEOTZgcGKW@;1C z*xj$k)f>l^$WoxVmGkXRd^|awa)tu33uHh}=IS*Ea4Rdw)Cn;G6EX;@AUu#+mE&kF zSui2|4!^}*&;njDe{f_EOR_`>yk>86c}r8p!JjEMoT?bd$3}4iJMSz3;d0XB^?%PLpPsbr)jq81e2{@ zFu_!AbSbIyz-^I-P+INd#!+%s4(4Uqb6tZ>242awCk>Ashe?uzN;S&C4dA3kei({y zy1uQ^sPK@}WSfk|(m`Fd+SxH)I0A>|onXq`ysM!-Pe)3rXdtP0vM@itXv=FUQ=Y`+VM{rCOZmXJ#`U?p#3L?8 z8WhhAxdT&J*09Bb1)ZS`dP7J4RRRX;)fd_M8D$#l&E__kr4&6tk3twZzLBdE!Qhsj z+~BEG($Ok^;>qy>T%KEWlsKi}4OF$IX@)DNd1s>Pw3ux> zqUl~!pw#qPvOI2;i=TdLIQJ?gLDEEZF;U!rEo4VS}khI+rD`cvACWvC9AV;n@ z_~;Z^rFbVv67xZohF0R5S#9rDXE;OPRHO8Gay4d+CU4q|j^k!imR$o-vP8`@Zk526 zFmufC$#L2CwP><%N)}l-k;n4;wP;&rJ|~MuRKt}lecl2gf<<8E5F>DzA}!wx-8r-?I+B!~L)Y{g?+o(=lw4h{8snam2^TT*`0V^`~H3OwvZnnnMk}cmU#!yHq zju!knKkZ__64Gp7Pwr#keHUe?fSj9Wf*VCUP^Rc|*?8S@71v%<%G*1pk=(rrvYLAz zCi4z{!j;#fFpOW$>qmS6Y=P2uCZM=tH(p-n;a2@i*xa0mEj~a{4zkG`NaYZwE#=Tt ztnML^;_c8Pt|v}?>6Vg+0}%A&ZW(O5&QCZFp7P$u-ZY2POI>x3)1MtKofTK!cBa;{ zz6}h{;Yt{q|G*$ST;?JakXU;(mY@WsN9Q6Y8TsGIN!;X>-Po#TlO zCJCKgtJKEE(s|PQ_ndUvBcDT^R?#0>;rGaOdMBX7E0GoY^gByNO5BOq(=s8u*uI+u zUY=aM=v!Vd5l2CE4vvjjMdyB-?W6IMi!EjMP5m1dm2A@rYKZxd+Y{8Uc+_LIer)Y? zBPy*?ib>|zHkG9qOXN0%Vmzj=rhTCaW~yI?U~vUsWb{&_D07p9RLj2Npm<6^I(^Ij>h!0h6RMGSkBF3+WKne0ljqf0m3;*}gcA>lU^1t+xOln-PEc(ay2$wH4H zbOA%}U1^}feXA4D>3+6b_sCjw(W-PkVLX>78<<#aA>Mhm+t(y^)q>M8T$WaIniB!Y zYCUz5g`j@jiPi6B`2@O6Pv6G7rC@QOeLlRh!o4q&9ba~Vpp4F$RIb;xv6&SOL=*k% z3KZSiwoYHTCr2h`%sadwcYKsZ6Cx#K>0dvYeX8Obn~t#^&$jNMnGtUhJtF)j;6 zoqR;&Md1zGAx-vP2`{<(Vdic#cnLd6R~g-Hxj{G7+MSB7Z*?dMtrMK)p5mP;tNfd^ zJh!=}3Tl?@QWBe;%uafCW#1krfCN>QX8R$PzLigzHpw5&k$LT+JHI{%D>?rD~_(*2Ae2N4urRE+cn)e$Oeif&jag6>$Yyke+Z_7k%$6%KcFT^p_q z^phC=NIKhnZcbfvw0+P@eihxJ#=@mG?5%q*ZC9389aGQ}a`4d+jrK!yWZ1h(>>?#1 zJ=bJYqhLxck|qd96xC26G}?-uibkkL;*qLrSY<_`V?mWO3wZ5K72sk=VW9@e+6lB2`wInq_blhTb}>1K(JK+}A$drb zhH$*y5U-2Z^Ofl14AB%vK0DpXv94HHJuX<|DM&u6PB>5#*;P%Zv@Rj6&?h#@TA)UR zkP$4msh6C5uJg6BuUM2R6C(1$YJx-YL9dQc;@7!qX6R14!`?3mH^Gh>o7MADcx;j4 z?sr|;u++Z-awVq{%Gs3tBLilmEa%6e&TOd7HpL#O)1T>e@3nObpjas@In6=KON^R6 zCNr#G7I}lbV%(jUtRyO-ZXuJ-a}J`}oT1(7*MjK-u8OW_&AMzk^i=#vx6mK8ESfke zWvYOD{unkUmRN?g+>&MIMfaJcGhV!ZqqERDX4yrYZDU_j87S|CN{3jT$}@Pi@TtD@M*KTm z6l1g#@l#u;<)knD-pOL}m~0}vvLit`3BM|Fq@A90gmcNM}_+R#t?NT-2)f+}Snj9g0+3 z?^=0FooZ!n(lm%Da<+=r+DcyadM`8*TfOEs5sTAQhJ);DRb8K~Rd<`n34b8{)$48q z9?zzH?dK1h1SpB}37qVfRUBFi8Oc;7`;x+{D&=#xX5pkMU%L*%XlpWyF?%FwXi$%R z3NPnlT@F=ho-8`DO5I%JPT{Eg@&&ZwW9Gwf(=vTunlqo?Fw59nZLjn3de!W|ubX#f z_2|0#%dLwYdt^3=@wiNyWL!!$Xe5iBaLKwf`BWTM%~G!tK`WndK%!ZqSXHT*}jABEtMF$nEP(+*)rNmt%g#^`=hvT-!g=ouU z+{}k?tgYQOUyPBXD>~t@(3!U#YI~B$#SYOmlWS)sx}!>7#*WpS8F{tLty(d)WA)dp zT5eLcQDjHmX3eZxH)X5(>sBo{&!zQcN8RSmu39&>NA+c9tU=}6q;8|g&g%cl`Sq&R zsjqHU+gHxa8kzcXGrGHSW*R$`r6u_(bxH%;wn}!fZ#hLm>pEUeZgJoxH#VHf(g>Zb z(G%l1SDM&|9oDJdt~<7%G98W_Wt^p7x)EBiI7aT7?6Dc?{ssw4xr#7&qISh2#~E&X z?GMgntq3D2J-s$~fyOj8c?Ju_WkNz@axN~(u+P3{Lf%Yn>SrS|JSRIIiP16yFtK^5 zr!P?A7qFzd&Vl>BPz;S~J>!B`rV9@fY%s5mSB-DmAfjL)zAV#Go50tlvxeEo~)TyrPXq)zt_C>tCn5;{pJ-`we0HeIis&C=LIbbW?jYR-gUvZQO_K9%E z5hka82=U3Bu6Qvv7U<%GHu|d3eUy|;1Zxb}J&7XYMZ>b;J|_Vg&V816V!hL^s){y6 ze>#oAvqrTz{q5={3RPoHE)41$CkmPzCxS@+#CYDl}Ct|Z2O;O^M1dh#W&Z5{K=ara{ zr;=pT378y-#4AIU@3aIOsPziT?n_VX>+0sfTKd_>pH0PzVyiD63mFnDlwqS7Afb78 zb7=FTDcUA;x`yM1aHC2s$7sF`K*_o?H-s82akz)gxv0qRj#as~V@p@868x{>P zgfTnZ!-W^c2j`eR^Alf1)8EM%DVzt{Hm4mNEwV?Rtd$o}2~)Ut@zM$oO>~RwWWACu za$z*ZIbfR7QC#vHBsEL%bTy5ahEQ{DvN{bJHR!omw0PUAp>Hs)Eq4u9Tl4DcD%wvu zJ!{vAnVutAJXU_Off9$3&LM?3qRbs`ea8jq)uh5Eg zW%3=I4Z);GS#GMZX<<{i$;q6$8CFHc9OgDVxRWi4*3V$=HlNq@mEzLon0i}^)l5H? zTVZdv{p;e`{Al?tL8rLZFJ7K&&zUoIWocC9cU?^PM=?paz{SN|Y}A5?g6PQTdjg$~ zD9@v+@e*S(R$9thKL7e}6KGmF(%aj`JY$}a*xKN3;^p8amdL6E>!4>>=F=?rS5HM9 zEKxeSetGdJzKXo*(eb%EVoEkU;Q+{PO|y=gg?e7itY;LfE5%`O=Ww$`^YwDpuJLS= znX0#MUY+RPg(F%}wu5pDIUbQ?Hi)AZ}e^*bxM*OULy zP=-a~oC8V!kpY#LJZ!zx%1g)UqsB*cy`!&imUFGx_UvBHuB*qjALP(-b_#GtQ$Hp9 zX2u78X<&_<2kcA4{R^Qvm04otEsjX5u>p&=nZiDJmnypv2>U03hvwibKtuFJ8A6P!pG zZV*QE;|?9(iE@wufS!-&8yS!9;6r8mf;p$U1*0d2Yj>5tdYY3m(xjD51Buo5i8m!# zc7zE`oQ4m@@0x}+x>v)U0o%r0=HJ*Y{Tp!aO4bQexF6SORY;=yxP3ZdJ{<@ayPn%- z|E}MMvuIe(5x3fo1sPt5JfGO5xrTwQLl^V0-Fxk#$10WbOO7j$Hu6H!kpUUliBgo- zYYMwoS{_au?jp*?ZI-gWRB~-ayyvGapKqm{_H?&+OS}fLC$X$T_D8t=KK8y{)v&aT zozYs!$XIqIvus#43UR*8ip7hkMssG&n86B%Y;Tj#=g6oml_obv{Dkoni6j)v(sPrg zknh zMXuJQv;AqAa;6xOt8HcBDvq*@g5b8-u2e$tL zg`Kbo;=Iwh@c@>KJh2EpZNpU&YB<%_ z8Wlc$deknj4D8oU!7OvMnFS^$p)|wCr-}{pW+hO^5^a|aX5?RSfE0`n%W7gXFfG!= z46?hl%%HGniX0_rzV_p$8cN!|{wDcq1}o^YHwm}Luz?6_taqKKX$Skp$76hv*y!LA z`Qrt|M-bmB-k;+RzpQ&oLb^S4T%}rJ#AsU&<1HbItK>vRgU0@~&#ps~ArhY!%a7yj z0#1I%o@e2JQ+&)P96C7ihn3l+@q?p^f1jv5OROI6T=~R?Ong50%HbPLUwGB#-}{tg ze7i$EY=a5=M24?6QM)<1+q_1h;&`{j8LD06B@+p6SY5$rO4_eRUTHp6Rf>{bv0^O8 zRlNC0l%r`h$}7+^-V2u+iC(icu-Ps#wme?L%$jsVCsgv|le14BtH5}a`4+Vx-E>Tk~yJ(k*f)YE%OaIUEWE?PqcaEo0eNjn`QF40Y? zndwd(YJ1eOI@cz;iEpJxmYYWC4R=trIkY64GcFzoN{CTknjr=eHeg)r;w?CsxMplA zH9=C}3bzr{tT)s6PH?bbqKZOiGq)v8-+p*0;b-L;BnA&{O*)^IVW3=)jW4F{o zNXHtMZ!&o+ge885k0=tY%)*N{03{)gV6x23C`zo*tYdNK@dxKZ)+9FdiYEaR1Wru8 zmyPo9W^Y+SA=`07Sr_%uE&JU4$_rt63ClEb6o9R^Kk!1Ck{k3&$!lGmc=@g)jz$zr zcS#kiJzlpbFGXcmg);)&&5n$|xko&F-r!TD5LNj}@FUF`NUL1MykPKNYCC{+gfpeL z`#D|FDwPi5HZ@Vb5E9MLe+>8KWtzZ9spEuLW6>L?&Z5}L9#!deN4%rmo!n@&OYMVD zg(OwaK{ZuS(zz(+U@kvEC$EoM7<C5|<%yw-rilwo9Nvaf$iiiXahzd_X5LFG+PHi<*j3O7z=vsbhTe zN+7MBldml{nss_(tC{6u-$Hf1aQ)^Q=k0biw?@vOc+uI_+HTXotR+RjV};^WM#7vX zluE~o&!%S9WOE}0jM9I+d1VOJ%t&Wa(n%>yLbZCutvj^~jlqH~980q@6a8M$ofW*$ zp)bqgSnI?kO`V!zIdv}1l6S-m4mZX}9l2aE526+|_ZOAHbu8^&OYuL;8{7p&<&c&}H3PC8icl-Ni4m ze#lm|$KJt|5v#tfbNgr+=PY(v?NqNLg`2AgQuy~gRUEm>e1$(et>VkzFoe#d(n2ne#a`)Dw&aU* zxz!^>HuzsMbPl8KcIs(rlz3!|Sacmvi&Nz1cdZePL(H~x(b4h(ZMA3HdJJvr-~#K( z$cikqSU>HI5%ycLbAlNFVi7LX;~v}=R^OHtcjbzX#coo4`%v60j@@pwgIS9gc~HB& zwn^CSM3U{6`$cTI-B>)IEAZl}Xhp}OdC{rnV&3%WeZ0R+)aJaQTz+`^8)SrTwV6yI zGlEGB(A|*=Nj2=$pJ=@%)_d}xnTwJ=m|mWZq4Ci^kv=0(;M@{;fo%Didgq$P_x+FRqIrQ6Nyi+k_J zy7(<(x}+m9Bd=91nNyE9BFNObEQC!QVa{7Od{g7{?D)(Nr2Ha!hSH;b1BYgBR*1SK z<$5NISOy9G*c_AZ-zZxr-JT;1;QK{l`|-3P*_n561l?p;NB9-0MMv!iIcuQ zW>k~!;3(HHVn2Q#K&xyymE(=!?{uUbaZ))7MYUhi5i*AsdqiW<~)OkU5c;>^H$js{*wa_jM&<s=iMx zt105M(siX?&?M;(zeGZgDlprt+-_RKS+(BY;mgu>4Z@jjD&D@oqqso$S7IWvgJ6^2 z-{A|@7v7jfFkAAy9m)koOda;5-Z!sba{5XJL5Wu$P6-;mbo1WeQCUkCYb_m8Wrs(2 zGo#v#>M!*OYAC!=_e(v3=VXyqO-)nxJY&LJ4C3XY0o{62Py56cXVj`LY>50i2dib> z+9WXp{!!K-$RT?|vubLntYNZiFH7F>aZ+rk^kCKWfO*S@@``+9L155v$`VCdNLRf1 z^s=h0L=9<@(xNKpAo@Vrpo|`h2ft;QIcYG9RFJwYqT|FAnVd(fl^iXLH4>e&^%PRw zT&NwAkbcj6W%^2(NEgTi#5Mb_>L8Q8D8sLLtK(H4NJ5rfHyj`17C~`_TdfyOFFUgQ?Z+@XSHr!ElZP&P7L!FA}8DK;}s0LRK&EX0-DOi=X%~5Vb>&o1qfg}| z4SBng@iyzqtD?G3GHiKo&>SEj1JyuQMM<*1dG!42{gTp0%Td?uXE*9#>vFERfG3*6 zr_KJCpv{}lm_cSZzw))?x8_O&rJV&8#yYz765g(+iMO6*7}3B7`ULCNTAQMb3&E-NY%agBP$ z9b15uzN%Z2oQ8rHN-@bVmF!xQIi#Cvs>$#iakoNfJEO?tm}8U|IWHK_s7rU$W(F_8 z@gPJ;|2`J^qR||iW+b+*cc+=^mhpxUHZiwf;JiqG61xj>`|P8&au_rN96fzF{#v*5pDr(v z&f8bvh_hmLYNKL#aShhTz0gHNNm7TAJezU!P|{>t9LH%bGG8TJ=L*roN98Fw$y6I@ zUwLg$xr{d?YoJ%F?keM6!YzlI%MpWQtxG%zgYr@L_PfIe)@$!HH~Q1vw59HF{JJ|- zw!3F|qX4~YI+Z=(rqViw8c;gO`3h_dq85HaRO`n3u#HhEzvyZ$IhxNd`KPaRR#;v=K&55BI)|v4 zR~;qVFJB7tb8a3nLF23_msuW=K&_cyHVO4~dBmob$0kQjX5vJ8#UA3;y?U~)r&!gW zTS*_wfqZT*)^~wn9Xs_hoKFuH`Z?1}4v?^ZRq~W7EqBF&Qu!mNRm)XUW}9WD z87<|w6I)3Rt6VLdX-1)m(?MFRe(UktVTh(wPp0i;0L|;? ztYnx|2;^0Ek%}m5Tl7tIo^7mz>J*BTV3D##{tUZ742=jH1)%LPJ?#pEnygHTcN*r{ z%}#f9n_#&~YpCL1OQPo?(C2id&akT3m+4M-WSq{xmd?K7ZbA(0#*)J_i~E*($eC_k zQgqT?>Kz!C*KSXZnwy(TETx?Q)+mo^VSCD!2qXuv%Sj~WcokNTSpf@MED|k~FaAwS zv5OTaV_rb&G$hAb2+8b9B(qgXk6rd=*f3SyZg$w{hBj|Sv_Jy=@-7Wi4c{t5N_y2D z5HM0;Q9C!6&i zqr4-^LfFroj;*c-POQi!2Mowt2$T-JVR;?K>?kJ(p>&z#f%h_bc5LRe!*T$%-E&!n zf5P=8l$|)#*O#UIrH3R9r&;AGGb2VKjDkS$3OS)3Z7}vL$r;HGwrAEt3sFrf^&~K- z4cZU(P%B@eIYLUcz@nuK7cE`1ysc~bq9tw1RxF=-upBeQ3+U5wG+=Oq)PdE`p%u|6 zkr!Jyhs#noM3e2VxDy<#t!IJAHcC0mOODeDcZF)-Fb}YpWyd@xZ3mfAFi zfgT|V>~3k%Ih+rZ+t->$jtDsDjZiEt%6JRwxdC3UI94c-sW7DKp z7fu_Tgl7{xXU-d>1kE6Mb1Hm+56w1g`;Oq}ew-Jj7Owwb|9#=6tTu zoXfA8j%k>_XmB{6>tU%2%fvNNfm9@5{0HSY2CToiz~(sdmpK2r8DDgIVR|yRE z)B8g`y=Aj+?axB+SI?ZWSYNoo;Gt|Xsm1cvcK%ent&sy+6ijBF?9tE<43!(lHa3oH zw7>UmpMOkS6ir}hr~SCkv}op>XqtQz9AenmDE2v;KMx;P-u1zXcz3+Dy>)baD?ZD< zv2hwo$h<;G-5U3hP6FDTnh3U_$df7KJjx~&;~dTRl|a=LXXYdkrXS2GrLeQTym!u& zW`=Jv%HdTX*j_2$xnGDa?}H>Vwu)7y`f~3k)7R9JWHx7pVvMg`u|7diUQafU{t{ne ziEdMhJJD_2p&RwEzf3;v#rU4COXp6qXWSFgFL7GmiRr>>RXR12xeheN^X5Cdi|3>* z&_Qut9F-|=Sc>BQn--L0#uGT@@$IKQH~gOcya!hL6$FHB{>Ax*o?+%*RT zkYuG_k}{~#{Eqoc+LpI;%Sxoxz@9A20z-1WUkDuncs7PS6Ebf^N_Q z20#uBfwzLUf#;rl z7(Zs<*qz50z#$X#8}t2~@#nQ37p0aRw?pa#@J5j4+NIsyq;i+3eJAw&{w+_E=j+bA zH`RLFY!c7*iBqKh+2Yml^+6N*t~ul}uZxvf{u(_z+$ zi!IxpP02c}T0<%|_MkE8gT}1t5%x$0*MB+qKFChuR|-v>><;o-bkYBX zt@>klk+}1MTqLTq*%yf>Y)V&!o#Q_5I5E|D&yJ}v%?Iw1+HtS=v(jI5^d;c!;1l3F za6PyI+zM_3w}bn@kHP)mXW-}H7eILBVem`v|AD`Pzk$Dl26$&Y*crSE90Xnsp8oZN zzy8wGpSt?8tIxaa>dT%Umg`gWdeLR6RJ6nR^TwWj=^W8PV0i@#V-Hn|)!BE3$`{YAOy&A0wzJ8h&QeE5vC2GJ7cEnpz^D5Q9li~|1AYj8 z1nvc*_kRrT2Y&$1fIosific*D9l%(yAJ`up01g7L1_uMN8BJg^XaftuG2mG6hYb&G zxck<-ulvY#7v4Smz=j99{q>@|Q&zhF@37HR_e}FlS)EZIb^kFcp4zt5Ew5*))7)_V zxu#X+`ek3C$En~n@CL9JtOM)8+29;-E)e^FKDYpU6kG+a1|I`=f-itCf(O8Z;HThW z@JsM3Ail!}@Eh8xK78z%AF@a>YBZDBL2~EB$q;+*kU>v~qA>W!W|w zbf5-iA8Gg$tdc8=m zo&8_=+FI0n{D>EVnubn&rtED-r~352;qs@99Mor__7!@(6}$~x1Fi+10M~)9fxE!n z;9>Af@GI~rcnmxaWE}86;0Z8}vA~XCC(r;Qa3GijUJH%{M}d0$zY~wDMgN~4IU4#` zwylPurqCK3b(lr;+O7nSF&GtzZFY12U#N1{@1|KnC=JRp6cAUEtl| z2JlI6Blrxs1$-9Bc<}S!R&XEqF}NT61UvzL2c87uWW2ap{@)M&Yc%vXRBkuVl%;Zg z9>V>GFSiQCD)VSvwA>U6vA@t^N3avv73>BibSPu`J;0t|DwqbE!E~?;w1X4CYS0e` zKmiPc5g_q^wO}2%9K0V~0X_ig@&E3cKYHpeFwblTb-(qx(UdwmdE8v;Zo2<>oM~0L ze%V*(@k8(9o}TLWeUz9$W>k1|I`o0$&DS0p9@M1or@m?R^`32Rsgb3;qW@0d^$5w-aap z2ZBl9RbVoh0uBKZ7yNA<{@>KL%G5U0ym`S3K}|!anWpS*MyLApzv-7-jhxhHq4pJe zOasl}IB+~z43>b`gOk9?pa6!!2sjJ88N3BZjQ1Si;=AwTbFqKd_okDmQ?;L_iG7zJ zvQcJ0=F0TWzD3hJ`}+9`nX!?Sey*cK$1_xtbJlVMr% zN4_72Ki?x_l-fz;)zSYNKHmUl?8%rP%ms&nHgFtR0!{>jUa)&GWQ#(HRvT?9sIgEeswA8rE zpa1^2mAia$!Zin`b~j-x2-}+i+}~H0ezkAuSNoNIHT%Jx zQe$=`Q~j{Z*#1;niWsWDHS33k;ZkFT=6?rMsN5l7DwqbE!5^fd?k90UpgsOc+^KwF zAGl4q;+Pi`R~T0K*P^fXrgCffPnN!W)R57ggwE4}==>Hi6U+j$L4uZ>Lh?igA^n+w zd=Y7d|77|LokLr{fNQb$V!zw=PNg7J{DUr!e-an-KK!1Bk$ZU=ny0I0(ENOb17Tqd*(D7Tf}E1NVZTgGaz`!SBE>@ZP@QAg};D z{hOcv=DzRT^&P=ocY^zVbKiCH-CZB%-{qHHexCeWKe*m;6@T%VKEG27{*(Onz7`%+ z;+j+kuUt(k`5mT8_N2KkT~d>!Z$-{81Rns`gB!rz;OpQU;G5tc@E!0| z@N2LE^zD;MWx?s7AFKfvfNQ`fz~{kj;EUjoV2=s#1h@#i16%>F0(XHQgI|FS;LqTB zz`~`}9$-&!HQ4W^tTg}!fGJ?(GfzD8@DtyC_=$&~x%>9JZ@%v4>ppzl zeDkF@UwYo9H(z)8&FgP>j6r|?v-6p_|Dfd3$vZ$v`R#j^RK?eapYuq3pQnUBSU-B? z^YOPlRZQE&eg9N3ZzZk`<^JXZ z)iA-Y=-lH~SJ-j<=ik(txbyb0PbaR&WH;E9!eps&J78_~lMG~DCUP_#w15uK4RYYU z;8);x;O}5}cwiQA{P0>n9}RL~2)qwG1$KQob4XwU*cVI&D?v9{56%QmRGVB^ypAK~9G`1i=hAAi^VyZgJhefOH%KKQQNzI*q@cVB$g z#kq?U9q6S*WI2$M`ec4Wa=o1_MQ1L7XRluK1QMCh!bWD-9d9-NM_zUpDT*||uZ`Rd zYJ<$){`vzGGAb!TnH_iL6N&4y3a^c?snPde_F2l4-~OqJx+Y9DS^D;W!PDUP;1A%B z;7{Oi}E1HE7s=mS}BIuN~(2L*5mcsqCpxD>n#+y+E%SR0P@zUkVV-gn_m zZ$2Y?M)r-_Gft$}l0Ve_xHsHLi-dW7i|D7sb$R;iVb(pGwDhv4SK?XnO7tNy-uz+yFiaeg+-}{|lUr`2(MK!oD;B zvH2TH|NZ3qU--$**Zk!B&wTKicU}Dbi#I-V=7uwqEEQb&9urj=^aeejpah&F*QMW@ zzC8WE-WOB5JTH3QOHj;EW``wKR|q$h=nCqR;1XYF`8HW+X@%!Ho)d!*UMK3w{m}j& zB7Hg@>4Ne%+tIvtZ{4egV6J-N5ePrQl^?3OF3h z1IK}(S21@8&Iadzwu4fsMPM;l0y@A+U;yO7yTN9R*YvhrPt@P86Gd|;d}L401Q*nx zr=+FtJv|f8LZ?e%%?W+-t&!|;d4-PsQIR`g^PlG$aouKVC=znlA_!1b`#P|)Y z04IXKrh=d6g3qaEHuvI*FoP|7k*MgSVL88fAuhr)~8>m_`JN5Jn|PkwXi0dc*^>CiD_6W7HYd%~i>C+?p-DaqjpIUv7c zjtx0J65ZSba^Oth=y%*?e0gvxI1TJDg>`)31h5hef`m@5^I~JHw;+7Qi$ugHk?i;3 zGnFrX7A8wvgg5>LKNFi{SFfLVPs{dRnaVA<)?=Kk^AN@~_CW+d=UDW6VT07}B7e_< zKZEB$Gqi32^S}|n@xdv47C!hqxCcB8eg&QdW8j7HU{CN8Fb&KFhl1mQ!M@9i=MER9#?Bs7bVR9q&ya2Dpf7+x41!ET+94{Cd?=pKO>sD;O z`^F#li$7La5Yjt)%+P~nE9cgKhzykxc2@FuVloDNol z4}*_@Pl6l4Pr=W@@4+9yF;f{|faAdN;7o8acn5elcn|mhcy7b@Hr)2yjaS_Ejw{|F zzb($aks~ADkXeygAs$}i@e5*U<@&5iffvQ~p6UL?{f6B?8eb#xCZP098tZ*N#xy(FKqrXKQjl02QxJ5Xd^=qxl&L$48GVE41q zeUQJ2i}1z^|G7TxaEWM#Uu+BQuo(+Vwx;|mp;S#V7h~6d3C%^GMV9~jKY;#cwr>OL z@GnGvp}FX9(cM4$51{`mPS{R=aaElB3(dt|i>*EuJN)mi|G%)E{_03N`U}lN|E!Da zH-Qg=4}o8TU8fcC#ZYMSSBDpg;X^vSQ~6?fm@IJ--q?+`Ko@>^R6erNcI>r4npTyxxsr^5tW4rtZ^1rkA-a_Y(f~&yC!9(C-AhdrB zJPw`!JHrRN0O5z-!5&~wuopNS%mb}p0ayqw1DAs!YERn~f_8CiQ^4ot)s^aT6uQ)rt@9QY3s@9R55@Qdp6XP16 zlYCn>Ct52xRZTe)v@F(ziZ_zX~LYP zHsSRj-_#vnqi0NfBJnNqD!MLwN{Aa?OV5PY30=wa#oA4brc1xS&|v{M9xMh+z*693 z{{%kwfh;&3tOor+biyzg0q+2pf_H*!w( zomfduQu_%lM5liy(Bog^^OwOlz{B8|;4!dxCi4m4bZ|NNAoy=^J-8Ko4SW|o0DcPo z2(FpMXYgkrryYuoxDdP_d<<-S^k?^e`?jm)|E0Hm``&L~{q1|#-pk+IDY*{dI6;)4 zrxcuU;PBnFQGVBwK}i!n`TOoy@TB~%`Xprt_Zc3nmD1!X>8qy1KG#fB%=KJL?!E=k zSNdY2{`TKeTT=(Q*LvJuMf0G0#m|%9HJB0i%h;z@4#Pyv){YV!ES&(!OOsY zpc||P4}j-^oQJr}Vb}}s9q?VS3r_iaz{&yW9Se4mo*=g-IXIx&8doSuEfXA;*TwZs=S z)K6$ezBT0~q=dW@yzf(m-%b)g&C&Ql=(!4d{tgH|g_c6Y{{fBA=LnDn?*Q|Tz{dmE zzLvcn;M3q{uq%^_r-AdqXTV=WH|M!+bdhlT>byueo|P|t9X8*;V@^MjXpaleNWBrP>Jg?jsU&^)e0(P!C*OVM&snl3 za(6X&2s{h^0fe?~U=ip5rvjn1(D^1y=g;u>bKo1`JK(S*c?O&SGT=<`KJX*33w$7a zaS)gbhQPVt*Wg+3)T0mG|Iqio&cBDg$aw(#yF&lH>vIoX{LmQ>omSK_dWnolBaP(N zrw1f`)s#rgtu0?oNZ&gbo)ceF=9+yDqfGf-DUPxZcSW&YtI`K*Jm9$ zKXLv0&g7~OX-$}4LSs==xvs4xBazXsgZ~4s{4acTumb!My!~~^I=B{m0=yaaI%ENK z1`EN{ZOs2I#0LQ10N(^h9>e$mbb|j&1wVfXJ|}k;C+;Mkc`|$^aq)O~XG3_ANLKmc zq41dp!;AYWUL=xzKTP(m`Y!GclLeo3PtN>Te#uJj1N|som2vY)R&fYp8rc>`Fn8lm z!VIB)kSF9j)d6G7YCwN&R|o1d6Gi@>11aeJQt)zcH0T7v2mN3eTnlah&j8_#OI{cA z$EAG!Bp5fJaRN98ya&7&+yWLK&AJcp)T2Lq>d~hjz2_GG-Ej34@4DyFi_gxUoj&_y zc*)C_NL(%1N=Rj~)RNNyuYL!-FTZQ$Bxyn^Dwe{8)M7!6{LdX|OFUJ4J+jwB@qOwe zgu;*GcdhzJ8ebpZCqk-PPfD8bNhANYo_vMSB?H!jFN6ERj>!30K;(UjmG$w+`YXYH z;41JGaI$|Ja(}0l{~z%8A@CRQPp~I?U>0~SaJr#`&!QhTKDzOT8}IqbZJ)mFgBRX* z&uyQ6^YEMdd-_k~-}2%9&2QfmfFJH7%;{GA6y@_iPuCT`p z*NC81?Q5taX^gF?nvyys=*e}leMwM_`w33+oLo~TJg3x5Ng1K^>@Rd!4t@@P0UkV- z@f!Fo_&fM(K!a4ml}g;HeDTvTgTzI6WBcd&dSJpRR{xca`iW2&OCbY|RD+}c8=-wa z_!)4#@M}K*9{d61;Dxo|9B?7H2wV!@3qB041)l)df$PC7;LG4E;BIg~cmO#5+W5$~ z9=YWcAGzXPS6pz#Sy%L4@kagEd%+dGZ|po?MqHnG!}YOrqIsp_6Z^zSdYL8FXJ^le zuaN?iN`BXp%!8-?Fn-eKFHeT~|BbyV;dSx-KkgQkPcCvjQGffVczeD?6e5R01H4H zSO|^_z?IA_$2rg zxCwk3d<8uB0^nj%D z^x^t}*X|l$!$oGrsPI;`u9VSuoYLp#za88A`8ZeqZQIKS%6xm#5g#q{ZTH|I@wdKS z!nyU;>!O#uIey;PAmsdmMaK)xh2F=2-cSz_=!}077xCBI=F?s}YimxmTh$|~tK53$ zD`kaO1D-?iiDPxG|GZ)6-8gV;!Y(B(+DSIWCx5ou`p+?hN&SIg<0x%0(u z{12di+v~T>e<=S#^Kjg?`|->uf%Ibb1%NX^!X^xb?%wL~;*H@&;!foYMjk~!6E-)( zA%i;;;fOzJtm6{#{~q5?|4{zN%Q*Xc;QQdG;1^&w=O1^hSo z2>2@aHh2s?4kCEwKrjh(fj0riPjBP%<3GCd6L7zWVkFPE5&$if=;jkoCu!U@YIF}zAL{r$nU#9_1;gN)pu6MrwAO{()|0n^Cixc{{|n7U&PB#Q^dCU~N4L{I^#8^R z&Ho<|oxU4-`rlptFF0kp{DAv-DAA}va!M3(qlDqF(O-f~m@ezsdwN~{0pnYlCb zzQqC(oVn-wKF|BS%go7Lm;NAhgD0Rn41#em9^`qh@(iE@I}aTF zeEH`mj=ndQ*El$FpihfFR^1BuKhfFuE$$&Ul;1>sQATHJM}-TK>ODVM!$m zcepc4tVMJ5LS?N_*dvJxwe>(8Bi9h=80|VaN3C+ob(2@GqomlkXm>NzgDf%s8!Eyx z=6~13!ouUFujm)e#b%qpKcM;gIE??FWi`3y|DE|?(fnT!Km9nwyZ)t{dityJpJ=YO z&9lhQhJN*UCK!wYtxj}Po8nGtMhp40J)tuhi~oW~ebW4y|hy;!P|NnXVtNp(u ziNU79Ot7@S2F?4X>Hix4zX@C6zi=bVz;1$@p&GP=f~=`21P6DnTEsUyOc}~+UcdG= z}Bj2)$F8a7G?Wk~}5|NO$7SE38BF0$3BSOyiSErgzgh*bC z#5l^FGtjo$pNVXZ@R=~S>7tZ9w&|j*FTdFLY!IE_f;DjVjsLgAw`6*&9WOCmR6S_X zdo754SHhMxUmu6@U&-b%s7YC;|L+&gRi7??yfB>CkeCpPf>tNa`u1O(y4n6aDRG77 z_|HmDf35wu^sa#BSHU%KE!+xK;eMzFHK7hX4C7!t9D|=B7j?v{FV~T-1UEu0s10?Y z2|NmYp+7ti!(kb$gq*C)$_4p=JK}xU!tvA}zDnKkLn^P$J5qP7+p#ot{*?J+hRmPx zILsf?#`tYU7cAOk|1awv zW=((d7U}fb-I8QY|CdQ>7;2;wrxp&}c22hcqPN(m*k(WIrG6?|v;7ZMlZ^gfiIy>_ zNm;1<7tO^Vi!V;X=f=DKD?I(x_TMj>OS>y=?uv6j|EBn~boC^oX-Tq>{Y#ux1^U4t z7zWGXW7rHw;9n@gy4liD9wx%eFbzJ2Rqz@74}OAQ!Ox1lT&%Uo4Ta!SSP%b!LvShc z@RtD#!dC5AB&);b&X`LwWyFBTcr_m(|8@{urv2EF*sj`#oS4Vnszpk6)b~yQ9xmfE zDQQPq_VMoHJ1y$8uS$#O?5%42CC|MuW674KK6QDmQqsOw`>Z2)BBQ<4KC5gE7x$JF zn-{G=hn46S+W#}*8~dO9hP7JveQqsPG3Xjmy;Rut{~yo(HUBS~TlRY%b{SfKEf|b_ zO$hgnQRqL7B`co)cjo^@bIV76b`I!Yxs9j4I{zn{TkZ0Lw8!yo|6h6fYyMwiu<}q7 zYQZDW4OYPc_yaCrMa?a6D|Ch~&=n@aEASf3gN3jjHo`7A1joS7nhRO|zi7j}mxjI6 zXI{5?ZR@nH!mAE1+ppV^O#V(QftTFOJG&i|Wi-;$VqN=F)8b0zi2MD{(ta45SzDb-LiA7PH6K#p)=ab;LydiJ+>=|J`r>!h?t)_6J(^!@jn(g zdivY?Cy5{L2Xg17`17f-0kk;)EmKETN4p15%c!M-r}e14V4R?{C|S99Htn|7EL#Ewh)TwgJ4Tq4n&8MkTCAY0A2 zP20*@SKr#2n9NP|CS_cdBo>T#jl8cJ3r2ijH<2arqV`vj*G3BMNKY$eZw(%6M!K>B zV)K`S=)W7{-TsSp@a$i;f6?5s`JC9U*u7=nmThZwLW|f!FYHavV&JG7k)TnZRCzP% zb5deR=K7yyp8l%;PZHlf0p0@f;o`r404?OxGG`+GYtk_WR(npyf1>yIX__DB|MM~2 z>!1y^g?7*#hQbRl9hSfvSPSc52c*I**4E92C9o8h!CLqh4ng4-eE$p7f?mLLT{rAl zx#Q4W<27r_kd8x|O?hZaE%U{=Q^5{*=6T5(%OS6=c(S7bZT79AwQ^jxM!#}A?Wk~r zCR~U1j^wpA*^Y7|wZ6`n)!>`vIe(|!rr+Nv*hpB!xWqnQyDEFD9B++}T`abK0oqww z_aMC*j`)&-9?$I*pj$r91V)i1B#lZTrPRy=XO#}iywXxwt}Vk zAEf_*BCM4v1GhmHkhN2v!JqI5!vT*%8yF0e;dNL8U%(DXfmFDT!N5w;7@EO2mP(IrZMz|Aut=>h6S(^K4;AKKd=WbVkD?ITm~nO{%~UNy0q6)WnX|HVj|V_O4RW@~)154{W#0H6L2*W?z+5+pt$iy_^0z~tFp)lHJPPpd_{LwvBsI?u&_+Mk3My8M2W?KB0Ew<0} zPL5*yFNf%D+4-%cHD8bIIzL***?cyn+|?w*{}<{UgPN3qSRym)iXqiWeuFjsCz^|Y zuL{c{-t}MX>96T8noGOB9-3g{@vi^F3|ZpO zc>Go9L#!R|`cLla*}vNUi{@6pYcu_;c-Q}?$36X3{Y7)>UrXP*E`92F*MFC%zvlmC z9Hl&519hMo^n|sr4vxS-kdJi}w?k!k9J;|1Fd3%9-OQ^!2rZyJJPqSvE>!NwSbxfK z?(SK#;FaO8^k2|pL5KQjuWAAPM|*5z$K|g(<0$snh|IUU8yk_5>Nt!!HWIkiTuq>D zWv)KryKp^H#zjfSTePzWjK4S|Oz9l;u|JteD?OdDD0QW15wZDt&>Yr4yxV_o=C8f@ z-|7F0=9aA=CN1`E*|}xoTAeuS=l{jr%<+FGCB|jW{}+0~(_d--lf;J)gcm`4`3$%V zpRJ7nYne=%|9_y+13_n~jPc*a=~XS7RePB>{+pR=`pQZBX^iQWv&Qt2qZt295Ur){ zt_4#;vvtMD*FkG&18rd>jDt^LCmcWY z&3`tnT{vywwC8(F>#(-z+Ip4iRn~lk9l6R126r)i;3^q2rVrd|#`M}&-pgDe9Pzy< zDT@UPc5qYD>;ptBNsl688TtiM#_q9@4?btC^(O4z(lB2i=U98I)`zmUs;h#M#pW%| z50Dn!EqyI*HJ@Y?9Vn?XdU%H0&7zuYE4Ye~IF&e}sIju(%iQgDOxL+CV4h2QR{_FdN>7EijL{ z)OVp9b1FSy0!)NSunbnhJ~#mX!g-9nKMOBF0mk8q!);I>j&sjQ+H3RTsf))Ae|Gqj z!#h3Xyr?jCiy`LhuEhxSp=OJrZN<~}KL0Yudj|2mW{aX7Rnb~yDqEvpWtw(WxIZGW zMX>$1)=t=;iEIsbI%oVxT~%6I?A+449-7XCBdVopVZXLV%czIMJUv`i_FY6Xm9CMU z_>An`dDk4Gx7hV85ZktF+Op%vBQ%uWj|DBW(2^h8HeXo{i?Z4=|6vSVz zg1wNQram1uuh-D;Zmkx2{HOkt>9p-`NvlFU|Fce!z)Q}&_P@dk1t4W6a=j$DVnv{|8pG>SPhDZV9KcczBX1R&U212~+|DLD6>i_+s zxy0n7;3DGic-Oz~Q=a{+tfl1tOI_c2BNr#j% z1sMYf+kduu+S08EThi{SPpbTaGp=ACFI(Lm|9R)*L5_ESFOpoTJ)&%1m3oKL^#Frww2kp?~Bd~?{V?Xa!f(vcUa%dsH}E& z+pDK<{hc$W9kErg-^qRZ6Wsr+tx`#`c}wH+=!+!IkJjbaGESMZA(h{-Y3nKBp+=I7 z{{L3b{MC zr5vZ{_tMyBnm^NE>?vw7?q~Jww6!`FtyMU(HTqRJX-9?Y@C8{N6ukc@&tDC9B4_ufK%Yx~2ECXiPP5ezY#1mT}6Q4Jmh?#PI*~`=`_Y zN8?E{GXI;2zQT|yVE$J$x9qzM_8af|*B;>MueASu(Oi6bAJ}{j=>L zf1oVAx4WSh)P{$l1+<3lFb-zJC-6C>!f_}|tab&|ga*(9`oKUK1LN@J6JRFHhPUAd zNQD#dxBP$9di`*eCxR}RvtSOt_s&Us$umMFZ9OY=&fW#GVTx0$gQVz-n9uK2pQPZ# z_Z!Q6=*+s?@wsdb8=u=HrP_uqX}`aeROk22IWcW(kp7E$eK(1H&jrzR0c?QG>F%HM zLF!R^s9BqrWNrVJ^jdAVq!S_5|2|(N@PaaL{coGIw*Q|)^tSByC~3{tYh6Aq6ZqMk z*}S`&WVHXI2Bp*gM^lqB(*MiEx?+fQlHXuG|5G#<|NSpKiLZ`#{jVDA>96W9noGMq z5L(gZ#=HKTJ^eNN7tJLO>j5>0sp4J#E<-%~SM6Ukx8my=#LMxn|M^2b{Z;)%bLl@x z-|3Qp@vr|pPk+t-OCQ;X?xo=xr~;E<1$+(%;7WR5<)JAwgXS<4UVx9`Q%Hql(3SZ5 zY3Kvb!4!BE-iK}Q74ZF#JPLC5xY@&&^jXra39mZ5A^EpEc5|)Z$|Em%_C$5#rtd zKRC?u|Em8N&BfkdgykT1Z`r$L>sp=AEHrfHY*_yzhUUw`^Jv=`DWm_N>glig|0MC{ zU111_Pag#4@jT1MS*=F{$Kl41Wfo{vFI%EsEP(f5A?${);UfChm%?q(7G}abuo6Cj-S9hb zZ}$GZpS}Ot`_qTN-+yn9z4BE6)zZEY!0zviW~-V)K^nl}L;Bmd2L8norO|er^4SmT9I&1=^m_8TAlX>7c9rjc0x3kf`5D zIq`q_5uW`k{l6r!-~WSMAa?u>h#lV!TG+2;T=oINcL?;TEtJ%ov|DRLhF;)KpXn%!L4_+s|6n4S4@CW3hUR(k7p#fNRr7!7$ zFcoINOn41;!#+5kx_{Gqn_i#(qP#}GIDPQ+r&>Q%tMya&K4nKN+KM`R6njkH%Hc=( z?UE5!+1c-;u7Z=;$czuQt&G)c*F#>e#GXYUua)O4=KO)SwW4*8l5CCs9;LLS!cB4% zDA=J&kuGxXJFd*8UR_`eY|a#@TZ@W~w}n0+`aKJ;fY#h=8JBH;WQ*24RM_=@>(gts z*X7zw`#&w$DOZy?wQ%6J%p3o^<*Z%*mmpe;y^5{w19l!dC$03L&-upj;hmlDC3 zfwJ&098O`S0IyZ@pP5X{uzr9IOJFN1GE+G48>ZbTd0 z7&M>IO>J*OXU>N8Khn^e>wk^2u zt#BLEfYvYwHo-x-hQ3ub=m-5_0K5RBU^=Xa4e%9w13$t~kU*P!5imZqdCC5Hv-WS^ zKX3o2LB?xVm;H^F?4QM}(UMvFmFG3Tc=^VNF_{@JnI-f4YFE$Us|(GbW%c|{L}EM4#sMM~pu0>(?U7z(|u)<`|b+>JqW<4$vP&f6;z7 z98}F*%hXf%+!->X?g*XMd+vXB+z{+LU?z*<-ajs#j{EpovIMNqZ+@Sz=*}cQ45JTT z%t;n^k|msE#hr=1YzZ7>B+m19_O+B#w5yn{+s*>kS<1<0Yn(5A>%TPyToAH4pPxQFVe|76}6;NhaDs{({ozmW6g&+@UhER(H$ z8*w9@KgpY0|5JVP$>tEf#he2F)GUw#9t>u%AQf67^&B6`5RSYP5&z0Fj74OzT+TtG7awYkA2N{ub^$yJ6$<>`Y zf2T%j&KkIgk)!>2X~)}J)hM-)*!n$iFFd8%X-p_$>|Ce^}d23l@@dO{Gz$)<1L?V`SYguaIH>sR|W5)W?HBjZBOV-b@doKo$Lg$ zB2c&>HuxGKai`|UkCPH-Xy{*-?!;NX1UzQ?CyBm2K-%r6VFGB~{W`WPB(w#cT1GA9 zGOb%|*!3T)(<@mtv-X+S_shgN{S5a1bBNwnthSAGFIBu&Av5ZZGoN->lZ^4-fiI@h z_@k*w8FBuZSXT_80{p*dz8zBFKE}r5UH?)HCZ@~z$Ep6(NBahjLlyL|3irdq@C0;+ z7hp7ufq&p%I0!Bi4f*$ZDyahZ2e)s0Z zQx^|g+-G;U-D%H;-@UnM+NOrPee~?clw0Za|H|6Ercm86>4?$mF-N_%{vTVjZm&@L zOETDtE-f~%>Tc<5X?y|NX+EKw>h85ndZzpyRL>TWPCm_1pyB^ldG>G27o$d7z@9i1NwjL z>96{Kzi6(;YgTMFmzeBrkl8uSCumK+mdO(Be={EKo#`z}*0KL2iM=hYx02og$KV8% zW|aIAxD;-H`XDoGU7;I10V(h;?1v-(iE?B6e% zw}tMo^BmCsk?EfPs{W$6+CQ`UV^&}64fi_+s zxizl(BI(7j1ipjs;c{kPuK>+2Xw7~n)nl|%GuocUA@^v-w2XR)lV)k>g+tb5&o|_@ zf12@s3;u84X4u33vf?v43%|{jECF{?#wyU^viSFrN4-sJd?Wk~nF6*Jv&K<54f2sO66cpRO8EQdoco_PE76WRT zI;w2i8elD>mI|KEZ&;$a2A|tMi@q7`hBqJA(y7hOv(jm`Z{?Dwb|;^A$`vFQDjc{i z^Wwiue5QW}*Z(Gn{-XOsAli3;$Dlhr1O1>s425Cv9880^U^%RW|G@#s&fK~masVs% zc5K+O^7!JnUYUUP_i5Ll-JLhdoYa)d);o5Eg_`T|WNWx;>*HIP+}OKI&%kxGy77AF zI>>82hJ2tu$X{-QQ@&_&ho0qg;#qdwo;&cqEqmXDoM&>2<*MGsyz~E$ zMh^zr+W-3H&Q^PtP|gtc6U2v^XH~BtS!~_X*wXi7wu!D^fM|OVL|2zFfYyYE&NNf) z&t*nEgmlnIK!h{z{cxWDDu1%iX#anUg(t~K{GW-w!ua&`_21`N_If?~-T)h!Y25_- z;28X=e&nfY=777*Hno_RQFnOhaGyYO6f0qkCm)N82yPw59;1x7TwhM{)d`RUxu6&n zhik#A6Q7da20P(K>crhp1vY?HUw$V22VBhjYDqY<^T@I}JLgOp->;MWYcamzT@9IP zKIO;uUvgpY{Fmnc?3hv8N^24Ee7v*ST0|^~*VfK%M}yi{I~tU&k)uH=Wu7QD1-J^XhRSd^RDt_J9_74w z<>tkk7th~3<$3efga4X=8$V%{!M>e(AITGZe^ov#M1F@YdD{I-;JAWZf$iJuv4S)! z5BUBD*=m=yz2mYj7X{g>m9?X4*&6QeEiSgMYQBd2A89)0MJv%!G^_>>KTMPH@-a2ejfcuBv^qe1_(@s%#Bs|M|tHMR!YEOV3x)b20pB zNhtq&<3H-B5xaEM<3A$1tKLyg(O7JE6?_d@zyDwF^%!{gc!?GLqWwzv2EK*;@B{o8 z{)T_xJXT*7gA#Bt+z8d+yqP{--Win9Nvo!4D^ktv>E`&5Fr ztoCOe$^RD+n-}dFYw9uJMeg1>t1^BV~;LrcR?&vix;0BRkR>_H-q-D5HwrQn)m;O8c8zjzw4Wx z{VV-HzqHqu{_m6i02aZguow2h0XPgt;1Bo<{)Y2dOV@IxCet-sw`lh?*a>>gd<)_Oforj4C)Yz`jmltZ4dYqrQzHbSq;Zin5K zHJ{w;qAdMEDHY|YE?1)+6|RHEC!~(o2J7jWt2!dJ^v+rnDQPK?Uu<60-O@GzJrhCm z2Ut?X!~3LSdNN86(w5>47@(d7dS-or8W7-ftZ?tu3Hg10>TSL{Eh*sY~?1=80* z185A7z-)LQcEVn`klEFIP#h}2Rd6>{ffV=__QMfKg+Jg=NO+B7As-Zml3+f!VadGx z^ES`hKWFUFIb-Lf&UxnXXC80!%;QZTI<-pVN$$|1Xi==~x#G_tesYKU)!wsmb{yQG zT_wAAJ9WNTtpP!{?z(rowpB_M?4Xq;l6JO+GgPrm?MVBIG>>OT=2A_=p3g4nroUwR z06C{+*>XKvp6fA3+tjTWi;X8kc@X`th5DeyfLf-GDoo{&8FfeSbQJO5yRop^b20u) z5Us`D#n$J5=I>jn!f6?G$C*#Nt4YIUC2{Aup8r?;zh5+e9mL1y#ZSk({(0W^^jGy4 z&80oJ+HGmGrM=Ee`>fRkE%wtgkEjkHbVemjAdGw1)_@$^^k|4I~{t(dG7 z>1wJpmDJ27YDQ1%{#QQiAYWZ%ej_Q@8JjdFZ>3-Lr&)7lOPuqgW_-%Tn*Pi zMYt30f_hLN8bCv63+>=3cp9F85ik;_!E#suTVWfdz_+j;PQV|K%xux|@3$QPEb_0l zMj7+>XUFH;$7~Vdga6nuy!2Q?MEmAwMqjPDyNWzDXHdMBqZBqgj~!d;@h zrTZ$k<4K*`Ts`lM`GAa!-^b9hjStpvNNOt0 zvh`0ol($W=?E6tP7yB05ZmKG#X|Ktx?NJX2lh&^)V=aYIOaANC5`WNd2(|scS~p0i z)t9tdlEd=l1JPZ4x%ly{r2q7HJui_F{jGL;4!TRbEp2vI(*L3Pp8grp--^-Jqr1dt z5}#!y{V!PH>7Nn(t$6%LbeDKsV)3k`|0kaQ8EXHnzE^&9m%f+uyRwr0{onKKKO^>U z^|LFXyY#W8f1Q=|zj2|be@67T##5T0yNspCI7(L1|3^>%4EcX+-0WF&SI5oLQwLlO z2%Pr*|LgC2_MZX!Pq4;FH=@6ck;?dJdbAHS=A-U9?fN(Sz|%hi`sc95!w;joH6DHw zW8&$l182hi3oi2X&w&03GXHTSh~_s#9jFUkpeu}n@h}(OhC^@|jzA)-vU5TiC<_lj zb!Y{xVE_z-+3*^C3@hPF_zJSI7B@TaX%jDy2FH?G}yc;{i`w~{nB zMEt$VI9T<3A=1qI)UCfOX+?%CyiBhLY!hVlQ;J^Rm4{HNOZRJ0cx*XHr6t14^rKjHL04ayEi;psB58G^e2L(N1903PbHWgkGR6kQv43lfYyjHJ7@3TZZ3P)4%WxeywL#E5 zd#LzNFBR7RS6%Mup8@?7t+CLAx#k$CjDOaG#~~}||F5TiM*M%G=sX@Kz#Py*_UdX7 zXFtqO-RL%5kZ%|MB*B;Mp4{0P)3d(wMebS-HcD(nb|0?rEIR|1?aqh#Wv;{0|7Fsz zfz*R}@GfkDFW@j7fmApSzda) zp1-Qy&u`z0BTo$oH>cu$_Qkm;M|%>1{bU0fwevAUBzxt5BX93Dw%Sh_2tHG^wOoO) z*IG=p7M(+Btoeh`nd+(|sG?@>3Yjr?U7mDG3RDhW&T_*t7rBX8)lgt(;=V zmdy@Lv(XRue-RvmBJZ#t2Ewy22sT4e?z?FZAA+BIFzUe!_z`YnP5yA$2jy9xTLJ!5 ztAUnDRrh3KMtf5&lcHY1S(e$SUYnCPY{huKO`c%ihFJOHD?&USN8F)lX@<=e5uf}% zh~~>+KOBIg@E80GInZ0`!NpJp>On)0deIVEK}UET`ojQF>&r0mZ@^ag1%BM~=|bl< z^T&}t?iu;x$iaQuR&Q(gk&4M3K4FcCh5gP))BS~h3IoIyMSs7>uQQrYb`SJVpox7zKX zrmOwT8*OV4^|fbgQU7kZwGZ`Aen(UPw4=gZE>C%uD~j;cvdp&w-^JpaEp2*}9tD<0 znv>Kr-#ea&+q%_E4K;JKnn_FRacTZmcLePLyd-x3ms=Tq1aPe{`^1ypw?6t>mdVz> zO{o5r(RxS)^d*Wd$~`nWkl22+pF2Ct zVd>t3`)GPXUziLt;A8jWLAVs21A&f;>GJE!J{wvHreBQ3pUE%kpOCo6CBLM!MF<6%0?g4ysb zd=Eds0XPUbS<_b-ia>d&02QGUn9q4#y?^!od5}6~OwVRDc-Ee_Imvisw- zwx1?6iqt=%$M3qwmA*b5S7$zXI_>JrXEkwkl8?}Pv)f=P8?H0xi~^^-%ND`p)=+@Z(3O`V{A)~HrA0wGx#?F7<-&3$guy_p8m!&;L_s% zB#F=3gQfWx(vx5vY=jG0g+Bo%!He)ROo3&v2{ywH_!)kIU*QBKvwFKW)Pa$3BK5@n z&Ae9cKQaHc;XUT}Xxd|b)8S3)X%ok3m4e7cqCYztv|~hVE770iql*+>M3qE}+Iii5 z)0Z77rd^{QDY|$^jy0d(zlA$-?7!NM|I!&ha#l0m)1pc~TQy%q?Ay|OE$OTu|Fv2h z0}mfJ@t-9z>%biMBj{TmAS7nLJw7A%_Gb_ziZ<9}^W=*)v^qo8Hf9i*da$AZpqOE_h&bvIIm|DV3zvwt=ImpI+hyD{nJ@ECN3 zeeezB=T5wfpb*>)4?rDg3eBK7w1BR#_hXKOFCjZCGLzsUCstLYb{&Y#$S zeuw_^o2E82wpHJ63ME z09Sp6vwHj=s&*Lu|L<7Tjw}@aiRPAnxBR%}zYpWXv*E9`I-xb}+K`{NCv-+@^0iDr zEy|Skf9l2A>O&Xk4?|%XJO|IiGFT2P;5#@7qgj192FAki zlpV)+9N(~b>afMb7H{a^qko6`9r{=Eh5fOk`ej`%a$;FK7M2LvT`a7{(st~tZDoX9 z%iH|{*&2D)KuVcsh-TKn#kuXzK)X7&nyW;#UXcCC$X0isj2izJ6q~nne-QoR-TtR+ zjzQ~?h3sE6w`|?AYs;oLv2TB1ozMcivpfD1^D)PNjFb`o-}Sy|GX@ZsI@DyfBo7Q4k0IKUD27?Ej@Lp8YHK?-$M0m~9vN-7sM_ z>%m|)XmvvK|DAlwPctR$)AoeUoEi-#N3s4p=%~L0j;7alQjY(>`3p~fMgJs;(_evU zuoRZT7T5th!Kx2EsRz%)c(CfmQqmv8UicPDtl@q?cmjICZaBDe*_?x8=Ik8Ow|(D7 z`_{cZ?d3e#MMQEJw`x5CJEGUN5)Dhfpz%BGS&99%Bkh=5+e(kXIbVbxf!%wMBO;Wo z7;s*9Ys^`57PI+3#Iqx2kxpLT1z*XFlz&CK>Jji(5VaulRqzXnr@;f?el; z{x@#(^jGy4&83~*1AC$DTGoTWzi<*RV7c z^2Ep!PoC&BvPBJEcS*xxk7|lU_Syw|R8u4j8^_tlOG*29wwh}lw5?G^huV>LY$sbI z#8kv~QmUZwo3gHBI~V7+V>=gR`3%gJ@lw|QWCG&e!eZZLK(xCO?t`;0{(EwJ47~O^ z8UKmimc4#JTJ!aB82?rLPYh~O78?H-|2zubg1N97et?5eoqMHf!h_HnIze~n0X?A~ z425AZ5hlSqFb^(doqb-&2gW=gbAg-JtuS7TX1wr3lPXPK&^r$JwyAj+;5X%W1Rrbr zV;5!d$J%*a`(qd9<~r_bcE7u8#`DS z^NT=quYqpyZvX%B>|c%l{Gz#KzNw>txklF|J$kd+*r+MdqQW_L&Btk zj{1senNh!)%Fee6h&d@e0VDqJxx@4SO8+m((t03zkAtZ&4RU=(tPcgz->L_9kiG|+ zK}%>0z2P}{6aEi2!&bPQb(UAcO>i@8gw3!MegavCdEQ3e6K299NMLPFB3uCZp*GZk z&HjM8 z^>VTA%b+rdzV|{?IH-PNTBeS=CnE#G`l#Kn{VohW{##&Y46OECjQ)IJ^>^K&q1sm@A}u<4_RQdA z{Qh9Z2P3u)>a2|zS-J-KpH)!O&y74nY>v#jC>z*wrvENg#~iN>+kdCz|LQ$TNjGBx z<~bvl4AQSxd+a`*pB3>O_I1gT;r@bRV$*}*xisB;Xr$^kjQn%(2kU14gj`>6A3yx8 zDwCc}s(OM0YG$9B(e{MSFnaC$Xc=eo*`7+y=*m~A>ZZSQQpOl)o{--0e6h(p;Vu{g zL&4JgJJP8z1>Jv#6JXVYQq+Mma2MPS_d|2&0K?&jEgx-p{gnwLCk&qO)PydLy4>5S z%gtR-z#j3^#gAlH0zHI1mZqJ> zwP%u}+KX7V%!Bt}IqZQi;RpB^^0IEW1T=$*Fa@^3PWUhU1vOY(R}&gQBWMAAVJHlP=V3U! z058KdSOc5j036=Aap%TW2CMjAQcLD8nZ;{t|Bg#K^1quV+>U+3%Z6YZ2(Q5$cpct=MerdoruXT>PZ!P{IdkNvPj+ooB~UhZd3w(>SmAbD&qY~cEp6nC zsK^|Hixd^fD1($SM+G9qkZ#UqjuJ+U+gwJZ@|~1nugd;QBfVo@vF(Ro45-?cXPsmP zSO=fL4mb=yt72-IAJjcsCPm%D`#3)-PLf+nLS|N|d$f$YBdev8GdkX9jP2!Myxv^t z7}Pr&J}~GW!*Avm`&|zk;0pA;5-jZ>A>9lH!&|Txtom?-^ijyOgXgJl zzafeBHu<0gTnRV8UCh7Mm6=k3nwq z$`a#$Veb|1_H_&*ytoP55W_+O~nVZ?t|e-ne+k%h+pM03k$=fn@k zyZ)bh`m6E3Uo^Mc=*P|h{kx`k_OJH;MRO|#d;c8J|AKEl{Z;)%^AF(vpe$>n*=rgf9ZRP_B){*`d5Vf&Z%J4P%?Z}O^}2RVIE@>-wPoC0fV-KD zyBBIeP52!yV9qT++zxj@Rp_iX)S&$8E-z5dGBSH>=Th16JKXt$xV zaaj|~I*kAAR_~tfe@ASU&Gj{dZ<-grBxAZxNmV^_>zX3dG)>K|V{7+ww%S)E`iG4b z?Dq<+Ddt$Tww0Qqy-Se#WsXH_N4cxlzS82O$HSW-dcO;w!9n%&)-rX}J(WXd)E&Xo z`HhzNfp)C9!3 z7yl{opItBBRe(ub3LLF0GW_o5%BT_t(#Oo9Eh zjP?%g<0JNY?N9O2LN%RF(cUV@Tm9QYV)LSR1sDr)VEiZrS(}(yD!1 zwyo6(ZQMU}MqAdw=E9ZNeob05pxc0E z-RgH6ut<4oz{QQBh{u+UCl!=!9h16RWTn2#T8uZ#jK zd+jz+wp)idD~+$I`&*E$|1Uwb78@7)z8gBIA8&(@8FfcY(l|UC zfCt)1b!guvgv8W^$_&!Pey`V4g{}NC6lM=7X8@Mgg znu~0>tJBw9gl{YSqPh6;2Vf2KR{zY_Av5ZZvmouZc04b&qb$A8Os^doZ~@seOMgQs z-TwWexwP9G!MFdM(0{e3zwQ71qPfIr5{GHF-{veZ>u4^)RSaO*e{%vx&;HH6LUz%; z9y|gwz-8PQjWU^E)|K{V|NrSDhWODx|0kNBWConSKeq9o-_pNvS{>-j|2<$ROoVAL z6JCY+umIM=I>`PVpCuH6!od1mI&!Oa9A7ePOwVT3u5UKRls?MUVa{r)2rIrguemx* zj)+jUVzROR%3Os-S=Xz>T%6lp9p<7eqrzIR((b99nf_ybv3W~h(Xa?ifQc|0J_5}r zXhVLjRof0du&3>*rXHhZj2+2N*N;FbY}e0R4=Epd@bN!mk1_r?MZV&~=>HYrxsy15 z&;CvSpJZu19-S@Smy-S%_QHO6kq>w}{2#1`?aZNm3swG0zX3WzFBl2?z|R#QF#qwa`BT5sXIUbHsYP>}B>>E3;mUJHLK*Q(OK#N_i%`FmnI zR$Sqd)7bfsI9BWz&Be~YgGt!7=IdLjABC1tcbxgOyV_y+|K2~Q)Bi_PJ2DdgWupFu zPyzja(cJRy+wke}u79DQJpGmVKfh>hwdWmZyW>Fru)SM6Ukmze!a$WAOSvAY$wTQR%l6SV$c=uB%hDhORn+oK*5Wjg4+ zZxZM#eMCe5-;a9wEBYr%pKJ;&0O^}8frILYuVw0}d$hG5>JFD_cTt-W(@#E@PSv6r zwac^#z04fbD<>az`qzKt5WW8as~>(B{qUO592hEfezfqvwzHkDX;m{fhs>BeGBmRv z*0VSKIRh@^xHPMNpvy8p8g_+Pn)$gpG{|`|Stoi@rW>qj(3q&*j zKPBnbLcYXFN$Q7Sq!VYduuA{QGKL6l8W>~+TbbXq6>KuHsjN{8Siu&s z*B%i}-*{+Iv3b${P52SCIbbb_(=ve)o#DK`{v)(^cwPnw7yM#06q)NCGSEzP%pr zhG+r||Nq=^&;Bj@&n~)49Jdkj5~FEjJzAen%S5YA=9YJ*y*dB?DzTO~|L@qpXl}*p zFOr@NDex^Eg+vz1X>~#yvD0EgZI718)b$_dUAHX6`VR@f&GA1YHC%$fyMOoeSI7UP zUp5dX!X&Wjz&E6=dXSSkkQ*+8^6(gRhxPC^+|EGJUC=7Dn1P8B6fuK)Npy%rwL41cB# z{`fO-{y)fAv0pT|?7Aaq&DU#9zLrsI$(c__(f-dkkxu^~P3_1)=RY&`H-rl4|BL40 z&wD^8e0IF+f9D^b{!07r7tO79`%T*Fc-Q|MPk(LwhiGoaX?L(TE)Mh$yZ&dupPv1z z_Al{yF?25pcR~$l3Gcx|*aKg~G58tEv3}}uxB{BMqtFxvgL(hQk_E2}@7cU(g9Q&P zxFdXMU`O&=^c+0$W^0g0o2cK3b;Cyel$7NlW>g%u8h)uF`q6{a{ZA6#-5Ca@`S20^zXcc$c7C)>9cMd_3=Nr4cLYzn>$Mr{zncG@POJ6Se}%t; zESnFX0dv&j3nW60#^|5vkWxCcZxWbz@V{l+F)PhwSwiGV&K;bd7d4qvL}? zR%+DtWmBIuB(5g(N{xWMcC@37GurL9tltP5N!Z70D?RM3@^1EMrhQh~8t$y6#nvsY z^P}a(a2;qqUmN<-GBs4w)-vjjsMCdH06xT-tCy zZSc)-4(OlE5V-gJ7xVsqzi5673=h=4joJ`ofwZgMgt7i-p{KvS{v*5SF7f$z*b6S> zzO$=BxYFJn|Jg(Y@2&r__4kYBRzIx6&)f?NyI?o`5B>qIPIObHce(yYJLW<41X@Pj zK|0y#1VnriOwf@a=>VD|K}O0L|LL5~(_b0?OEUW2)Pa*AU)ER;+Ch7;>cdXbR-O2l zbaocdUkv4-8MJ^7um*NPX%^UC0X3lkG&|0{WLuZb8MExjkiJj!ZCAHl-KzW&>Zfx)eTBMdycf=x{KP_YvN6@}as2;vCR+uMy zIxRe!6&`W3{zo*oY`HmU&DU!~Jz6Ffn!A4wceTUt{~KaaJ2KGui}m^+(Omp@M<|7_ zj(7bZ^T)tDWFh@UbE~~>N;@6z`k$A>(_e}I{Gz$UXPw~}=%spH&E;zuwbYqUN8$hH zdityLzkW;mFVOx!@GJP_9vNr{qhSKP1#=+<_QQ9OEGw;{5LAYH;XddF{or{R0ps8Z z$lc#O9(Kv>(NB+ly6ckGOUN|3w-LYX`Kt8Kf(423gU9r|m$?#1;^VOW2}(*&q@eLT zQb}_?P{eZv_B-sqJmQi3HRc`UQqrF3v$x9q0rpn=tg52HZr-Lh}XwzWE;jpc;Sq-SI(0~N{j_|G9vf7Smd ziSI54w}bd_@!!KiTk@l2>ZmfsX#LNK#B^G%xBe&W;t5}ZXl=F8n@I15!=Tx`=K8ga zw&X+0sI?^ObRlQxH#-NP#`Pc7a>k%0Wu*TX68kac3h(EbL|A4b7AkbC83!rL$p-iH+U7JS@C zlN}mEGiVM^!PD>z^nt$c3QUD*Fdg23_u&)R401otFK`lk+?#PN82yBlRfkyyxF}@@ zue7Cr{de@I2XQ!MRmziH1JwwYC)S?Z_A4hUCa-<>f*m^_ZE&Yk%HA4e{oga^Y3u(a z5)ZxpFJP~I-r%zvvNc?!?#ym<%vUbR)oQ&0JN}og;m%q{Y+kgl2xFnQ`axi>-^DXTt{10sZgF<>_xd z|HUtwTYmiQK>f8fKw9Q3u=#*Xa5V;C*#B3a{?7Uj(OlZ(58z*L8TSW#_-8olN_%tt z&+oM0-ufR$f6?5Ey{<`%zZ&!ZBk(TwO3eqYPH00uomAnqr2~ya?$L^A8TF7b>7Zjh z66l$0eauvLmI*l9obP{{oZHi18UIU?c>6l22$kSIr~(gzRSyo5{t+IfE_8)?VAYKs zr1!!3+@W;=9NBqf3ww1F= z-i-Uho;9#JeKic_Nr{kP2!a5zwZY=X*+IvSRTQT6g?$-414z0VEclQoFgGjDC z!gcsA68rub{sWeldC*ZC^3yVzH2$}*dYRDQ>2y<`bXx7XxTYuB89a-)ZZbi%wrsZ` z=|a#FT0t)u4P)UQm)zFnv{XgUyRrPT|A%ck+_y25+;mNfo7RYDPKdXT^uEp~6}Dtx z+g3EWL(lR#op$4nE3;YcXtHRv4tBvF*bC=#``qPl8{7vSp$80wiLe8{hTkB8o2drD zU>E{(VLohtuOJ2X!;kP29Nd2JvsFus*KB^zUiH$dQBRHPGHTWKN0eF-KIyJ)qeVC8 z%jHwE{$A%>8D(qOPtiVJiwM{nF7AkURKE8?`;5 zGjcN5Eqnb7Vo;B=PRp*3iG+%-bBRy>N74Hhw5|{1U?!}GJ#ZBMfIq>i6IHpFN}h<& z02)D4Xa>*12$%?yU>dvwpTG}W)-L~G`HbZw_&s2G&yGDCK*yfTt1kb*7p9$Z{C324 z()Qb$pS8ET^X87X4d-g*s^om8Uk+O`-F`osKBaTjZX>B;V%xdVr8Hau*T4kOx_nwj zGvvyuy=xhDNAR?}8v2q%?Z3prwli%)H#1}UAalCWv=IrSwQ8r+$$tf3!*5VP?w5n> z;ReX+{lE7v^jZmJ{4cloWYPK%9EPLtD~R?d!H4GAAumW>NCv47WuY8g1J{AnkBU$U zPJFj@<<_|?=Wd-eckA4NbGy!Me4=V_7ocx-7uUP*snM(Y{;GW8qkPV0dQCSarHs3C zy6^YNyN7K#GPQ45Kx|rcYYNTaS?-S-1moZhVA$9B(L#Q0Pw0#m|7#iNyk5%fClHNq zcN0116nniHZh>m>05k!sKJ+F%7e0nh;UwguUKE0|P!8I`<1i4c`tv5~gFDTsfS!Hj z)m(lx==(sVKQ+oo&81Bee@RKZNA0@b?VLZtSb8-5?uyCA)tJ}hHdgbw^N-QY68H*= zjf+^)^@Ee9z`vH>;Hd+iTEcxn_&Yy7kub zht99&$He3;=DALMsd8V$$5 zboc;5$NbuqjW#h8I`g1f6|{`HgY@Zc>x}uI3-iUmJ7k?vq9n0bOYcRbm%0EgiS zq{2}+28rB1nG=dYF(?itp%h#XH$W9Qp1ObY{?+@JzA^5NCtFl+Vg45&aHuRTm}!B% zd*_KXgKf9EF3QsCN~wazZ`ZA^-JZI52U=(srLb%%Wyd164QpP}wqe;C&W1y*kO<;G zPMx_(7W)?6m%$;(`tASxSXg-6wEv>HW#?kgb>Mke4cWL?E<3yhbHQahpfw6EGohOc zZBHnV5qy?FjLcjUW~2=N-zC52|CRBd^Tb!z0@1xLG=PTC4%)-hFbt$l42KtB6ub(v z-~(6%QeW1GnI0;r=N}wJkLQcpF`5+k%ZeKNb)tK#1pZb5-cFp7!MEXEK zO1|(FIW@m(#*Lb{jiz-uTUv~0S~<$fr(k{-5#nNBY5D96!lh6KDnS)^6D+&`g7gkZ zX@g;G^-XwxzL;JzqCsIt0T4jwmNBL zqm0yDyaVmIi&A#`{ba8joZ4r%-?BAq`)!w$dK$K*{Z3kqW~WE%S?5*=Bd2IPAYR~QVSgyledn_fbHdXrLW_ujC)og#Y zt+diwo@33JRofa(R76>G^hC}ODgG-a_APo|0F@z2^#8x_-Y}?#H`a>ePL6?v$4ji> z7tJj@w(Ru}w*3jYxKX+!#FPJDAB)W#J)s7#@Kp&;`1} zKo|tWVFbJaQ{i1$03X6)*a(~8Ye<3b;lJ=F`~`N8G4-97`**07p#{jg%^A{B}JaOrOxjxw#JQ+~Jy$%xQ0ev@EB#Kc&RJMdxzBqb%aU|933t`G3FL z_2hoh+_G=6WwBea(F?grJ}@&hPhzXg>9sF!Oh%De+yKF7SIv`Z6X)SNZ|Jqjj z^O3EQTR=1ZcTv`Nk=XV!*b1WSZn#NwR8`V4nKb?vHm3hH7FHWCF?}>6C<&sqWyk$V z4}c4KMnYaF36;R5@7G*+=!`bxqh*XL7tPf>zCGw#M=^MF{!dK79RC?w*z^C&{9lsz z>>khy`hryl#*rQm^I;{dhaccLoCF{BA}3q`R(-jV^li`?xT(Gd^15x?OTN z8%0~;e*CAf*th6*J9L7qKmJp;NDM4IUfK%3Xl~i-a?+Zw*XDn;OfYjjbKCU!pHPv6 z5&wS|i`tQO;(xzrz5+JDh4|`t*MD%)7w}7AF7s$??`GrBN6QSq- zv^}9S+LB)_6W~Cii3Eejf6}0v@t=`0?ElhYp8jh5C-K?EP!ev2Tc8F!2u;DN6WvMo zfYC4=-h#QX7XAarz^XR~zg{zM@{q3cS~sb~s|wuOMAeqq%A6kyjJPBpDh|`i28q_p zI7}-=B;=eiLa&+FO3FqkWyeR;7Z+%Y4ADL>v7Wg#B%hgmMe#kYcwND z7w_vUAog7ZNmH@{ zyR~C?WlcMFQ5HLvYmWZPwWGofIR<(@yN@i^9_~PAtWru^J=y|d^Oo+TNo#Gr7V>Ku zMW?eMsmK4Jyo1sHKU>^u|CRRNFPe{msqoD?pnvHSp8l%-qPgYo#kY$e{}?XhIT?B3 z9he82U(ka4(6fA@_h=^wopJA4g2w;iu+8}2NE!D3f1dv8_dn&5_IesjhXt@4ME{L& z5DvlLa6WawsuQJ1UkYWQ65Im!K`m$okHK&l4+pj_e|_87Uh?~~R*%$vq}BbcYPWji zc4>{wQCMmDtk%d!>y~=kZ%*DWB?79Y0*})6b5xK~+01X7&xQzus3|S(cNH}i5{+iV zYcLny2FvERlim%vxTF4jxB!a6#c(;ahsU5h^oBk#3todcun@Mu7jWX}i3499*tTNZ ziiO*Fy|M6(@oy{~Va*Qn$(b>hs2?GALq{oP)|6#5@eYBt(2lX>Sa&g&l(g$B z_0hZ=TdTG9S*7l}J8NJq53sI9T4v*pDxXtm>|PZ!O&S%PS7+4p6nwRT<$`L#@NMbFe$cQwgq z|4WvPK~2g+2HU2&>niic$feI>i|d07}UA$ZMML6yq6ZMh6{h}crTi`Rz|_ic+bT< z1ja~qWS0JT&y36hqd>FAqiyBAwLGz+*>};l+MkzfjSwGG)?EMLqO31j>|69)28SR^ zjQ@v?6;H;(!s8`Y^o!<);RLkBZZ%)8g?w7Zw&b%S5k>sBqO|A#760!S%`N}^2I)7U zG51J60v%xhXmvts*w60vUtE9S{D0F+JpC2@lccSl1D5tnNiT!nAt%~j54S-hu|69428$p|wEv-I_>8fle_~Bhp5d(C{)eg^M*nZwrJny+{J&o`xBT~5(y36Bd!!e@ zdvG6jQ&s`3UWASTw16OVMms_1jQeU8wEr&-FtGh^QpVF?(La~8(W61Mp9UYnQrHAv z!(os*@h23aJ`{xta0gTesVj})DR>%2!ZdgntU9!o^g7rNKSK%XQ#rU74sKsDYx@gN z2L9KnbtB_-N)4cG%}6V%MGoPb_GnsBMkuuNp8kl0tBI!lH`hVAY5z-#29}L?BHayM zh1X#hd=mULWIJ^OG!cy1-pM&vCSa~X}u$bSA_ou!( ztlJ~^;Q{OjaC+sRBSkH?Htx!rpPY-b=qRNM8o#5TS#qguT8sZnWB@$C)ZF%bQqfSI68`DvLts!eKBep*H?6+G>()iz?fzhdFF<0Ym` zj^_JcMQh96e;}>3^IFKKWr8burnb7PNk;tlQCYA3SK5ESXm0uU?D*(-*S~Q&Pk&W^ z(cBMt;hA$l|35tawfSGs+=|;IM%w~ixJSM#^n(eY`2?-EruFbb-}0~R(Qf(IGDhB(rrh1v%S>dWqH-$ok1!1!;6r@uP>d!EE!Kf=Ety1&Bx z@l)X~_yj(Ki@E#qDyRXSU_Pvbjj#!J!Cpv(AK^mkPF^Sr<)9NteR={0!en>{CQ!Ft zff?`~ECj309sX|LciZLl@%9hp^{Vlk)QcZJOXlehpZKujhXK{W%m#@BwQrMV|1(by z(IUepr5Z3cDDh;Z$j~eq7+K1ZE{^A&wV0Kd)I6X34*Sj=DP`=n0pztGbN#5KV z?#yMyz8Aw1*aVxwkF8raUx0KGC<+zeD!3Ewg{sgR`oUay2e!f2aAeQNZ;pTSxdA;p zTdx5vYX;RT_(nTk7p=nm1qlH>wD+;9RnVTGRF}a1Uyo}%FQN4R zb4$Bi5vs!dPz{5|x07sj;tTuzX}PuQ z32gN_${f#^>$8uFCRU-W*-MZ!M2KrDs@F(a*S!S!8zP+BS6E^*OY09vi^gkVFNoGZ zTNWn&b32Xm6FRfYDdh8MnLE@yKA#cgUvzB@Eb^R<@(mvt+B@bHjm4IWW2a)jS3-ZW zSyd@5qXh-p9xbCDpV?_6{@WUhdX!b;zfh5Lf@p2|>UE?)fnuzYEe_@3ZV27xYfZM+ zFo(`)Q~p{epql(Yd)EPo)%X7Iix6d#ts)h&Wn`6%lpQK7iHz)7R%wu|5=tal$ttsC zWM?Ik)iSbo_I&^6l2zKjZ}t8D-bNmKt^Y76TY}h~h07L*p0z?7i0W9dkOOS2=cm+W5 zQ7`s@y*xlMz*~S`fJuN^0AhH4Ex-l6woe^(Vg5%t#&fv=oj<^HSw zY#mtqejUb7!~<}ti$47O;(pg;CM>i;*u?R!Cez- zYc>~85OV&87XDv}C(vP;y8-wCkb?HQ!q%^F^(zj()?k1CcPmE;zN8&<_P^qHXve?D z{-eV(U$EEvLHYndBtR5EDnJ>)3Y}Qt*#CY0-<0b&SciX){IOvl%?+?1`(q%j2XGP~ z901mXa)6frH2_Tj7+?-H34kNOSpY8pAAl_U1v496o2!Z!eC+!?~c(yL=oTc)-GH@wtCP3;)mfe^};N0C0Qu23TS1SGfEY2fyFP zf9;Mx;Qz~ihj#pX>_0jz^X0ZX5FD@jXtTG1y)l3(KoGzR-B=L^teF4xod+vsd41=> zCrYqtuOBwk^S%D16l`kefB)H-=Qmh~e~6dC|X3_YiczAr>&-~rV2KgWcrafU`QHge$OsV071m)hOk;JmNNuo2g?~90G2Z>+ZE3ZzVl$k6NQfD zZM|jrLH+WBisc6@zA0G#&5DD(<=^~+2WiXiGoQxAhse)QBPq_qqpU~i8PHA~pOM9c zAizW%@XrY0N&sIRZU<(s!mpcrng=W3r+Kil%-g&96qtW=d?^oBoz>^b&RIuNSXOud za9gAU*a)x*U^BoD09F7gfPDbR0dxVp0K5Sn0i*-G0(cED4KM?M2jW&mAxRy69!n&0acJb46vXJt|09Ru%Hu(ApH#MKm)*n&b$Tb z5r8Rx;jZ_MuPYlX3+GXoRoPgWRhS+V;v3^?XOOOaI18>Gu)}cGfEAtpy;E?Eak^va zn1)n8O?4Bnk8Nz05a* zv|mDyCI{FAzy!bozzM(wu+XmegR~5Q62L)#g?6qA(nkSw0rUV)09XK60@wq%09*j@ z1n>gz1_%JS1rQ1l1u!z$*;G^2S=3aNot#}%6>-D;M#P{?r%O$ht);Dztj>Zyd<@SwnCI|{Z^79|M6i~w20F6j|5ww6Rdokk1?%O!Hhy*$ zxbsq(i&9)g17KYy(|4m&TA~L~4<9d4zv5d^#;5IJ#rNQ9xJ>x!`LdU{7Dm`6e=45^ zdE5oxMFYG5s0Nq>m;#^y^Kxkc*Z|l8lmS!#Q~|62;CZ{g0M`IQ0B!@s0K@{60<19Z zD-Ooz?brPF-}%j7R)ysZolDR823XzUm;<7Q+u4Gg(?I$$Kn6fIKpsE|Kp8+4z=9ss zfb<~15Wob$f?mvkGzO3_DF7M327m=!VFl?0ec=RYetW7oZOS0p?wz0gwXxYYh7eLjRoy%Ui-+ z`V;#Pw<{F@Re(bP3-UGv=>?hFgY<&jFM#xd>^(vH2EeB}kOJ~D0Ty(j2&5|j7IdN- zq!;u80{XC^8*4!t2Y?6w))O)SMu4pVL+^)Pg0Z2>)R?HCE0sZ4f-0|6Ub%GT0*uoJ z7r^!soV;`e+DNL8ZV$&{@Ky}OyK>SPkOF`vfHpj01X^njA;sbuL?D+4yb3da1ss*b zS1#IHaQ0GppXcs5>9KS!DB}n0!J^ezVN*V}=L;4H*oFCi4Se=zcI@j`4%mfNuY$HO z@EPXuHUNGA0RUM5IRG7i;{dt<7Xe%W?f`@VWB_CW)B)53GyuE;_y=3cis)*^!9RG@ z?<~6Sv$pn!Gd>+Df=86*M~YyDo{RrT7Ua$b~J|>&niUHd(XFot`^SNKN zJBw$0@qU2P)$cT4=Epm&TW*nHdj((fxn+W{S~co!rhzhM__ISp!d17h5=y9eGpgQ&qpH%Sm%?ECIR8G^AoZosO1qqS;Ag3 zxC4Uv{8uvZYHwLTOh5A!K78ejk5Ll^_{;FLrRoB_B5bP!{}!_;CB z?f?r&V(f+`y(WhQY%vjNV2cD9*pLpS2+2W8kQQ_jGKEe6z#oDE_;T<$qX=OQm~(+E z*E}vKQR76Aw9}upVh$6hBtt?nARjJu@7$>nbX$UEKpfe7$}Nmrw{2(Q=Gn>1$1fnZ zS6o6;YM->yL1h)yLx+#(9M{!5p>JSlX=QC=Yj^7Ox$_q;y1HF*zv_F<&p+V$jler$ z;SrHhccbqmCMBn&ragR=o|~6nP+0V|xTNYu^~;*ty84Eu=9c%ZZS5VMU4tKnhDS!n z#wRAhbD{a_fbD#ZmemKaAAyE}fsTO%*9U=S3s;s9W6dTGOrqTiSlZ^qn>o*7lkB+? z`XFa5J(uDD=`o8+95M#(rY(bTmF8;neHC)~sTwU-Xt6%X2522R=*$qJ6G9@;WaJqf zXd84J?LWC&4{`F}@wx9Mek=+Li!Z`T$-}?HkyBJ2A-V>lPru`DgA*3LH3EHK+HnSx ztJ{trEjdka`uTe9o+4UIeF9ocIcexzuMH9!dMh&3p_BwoX=G22dCyLg96sY1|8Ca4 z2njjjusjj2HAO-~El4Qq0@+bIqckLRYY+*&+jE(H!{XHndKi~kP713k4RfC{+vxQv z#~Bb;MQMi-6!>2bJzK9jd7^OI;zYnt~JH3NrLsMfBbG^c3F-e9Cj zles@wU{smul*BP**H99r1ObylokCr^lj_t=dnZeq-d)xnbiEK(O@Q%GAePx3m57=4v6F>!Nt zireBgV@eGV)nVRO@6;vU-VsymEa-D-*KO+epxycWjZA8qC5utj_PTT@KN^sn8OSU`e#FtQuBF%P!;KN`I!7)|>ywH5kC?PZj|scKu*Q{#^l38^I_S>~8mK=3k(&t4NaxW`rFx1-JIxaJGemLj z;zvUIZRxR6C6My5C?+Z4YQ*EF3UZ@a>W7Du9TmIk&psm(J2w)0PvL@i|E9>wVY)n# z1{E>xox(fMoIFONnJeI&kzJEfCZSdn^7=@KWoI0-V$#dlV;L_@jy+2C(Pi*V#7@|( zU~Z##AzhRNnlmb=`U}|H8}QHUz|IKh74XG>)3Dd#oysuj3)kFdNiVed863h|yuEI* z4RR+k@Onnp^P*Adob$GPn|Y59=ir#Co~-dLZOc=Z7CmFcrSu%dEctDZNfhg@;((u} zWKb$4#N1AUJ==ZE&lf#=;6x$*hH(|Cg7~N%1^19pX=cvr>+yyU1(bQkw{ttn4S2Vh zc(w%l^By&rEEK7JG;tgWS=D-8%8~VoIjA3gquVcEevagilz-frYf(5Lp>b7#qWZ~+ zt`o8r|#LLtVbKy`JdnQKZ-4d`x}r)?Et9X*je z?m8=~iiAcAvi(j!AHOo-+NPe}P1j}AedU=z!&;MgdY895{SBxLH+%RCyZ9O~-xtst zSIuHWLZPP0Na%@8Y}SMQ6Fw7%oO3ef1He#*aE7M*JrTha98WoI3h zn92>DIj`GqW#2(y&9)XTS|xH+Tklw+AK%g4!#uao-^M>UPCuv#?sd9MUZa#@lDZ7z zImfe#(SWLl5A39BJZoAdRD`=;Bp8n{W3#bD95#*;9ZV0ph!vU;Y!Ib)@=!QJ6v0O! zv<2YNbJ_Z9Q> z+_)r46^MV{dIrcbKV)CVW37I1!i`Qm*SeKl^9y}XsBOq$quob4ct;9TfFmc?DO2*{ z#9_}k2kt>9PpW;6x8JesuMv0iqw`efI2e_Dq2$?aKR>slyJ_6w&#o1v4INNx&I)U! z(d5&+63@Nw`r6w%!6ChDq49#6L_A^Ej}-j8)2(C_UmU55OQ8|a4XkNG4~u>#?$C3m zxu^Vc%0{4OSRvC!gVUPO!1;iflo4EyA|nbRwb~9&<|y3VHNojPvQNjUj=VIrj+3^Z zQEc9S`O)>Hr|nmbI<>KLEQ)l{{NyPow_Z%iv`+1q)M#x&LXjL~vF$1Z?3tG+Oa zts5qmP!$Qb+7rHY;IVt5#-U5bta?e0yqarN-Sz7@1*l2617!}M9qsSw92A=+cEs*% zh`ncB%0N{i$1h+~)EYR^W3r*HWU&6GgsRZhhtkX3Y&pazncXUi+0;KFk9IB)O1N8} zbN5iWfi3yfx4Ly5rwI(W1Cv;T_bCMiX$0B|%Z*?o@BVh&ciX(Kgy zPiLEwH+S3B5nBrYBHg%3tCsoRS5PDp@3p)Wb0_WPBK#?OkZFTDEB+=!kg$l=Qa~k@x z?P)SE2Af|LEa^}tIepbYDU|;)gP^9-h1{Hy@Y*P)ZqV9@&gIN*@Rsw4d6sAO0l&q; z683|4qQ$OF-{dWexPFXAHBJ1zvE7mpu*z$e^mo!NoqaGHne~XeU3{Q+RdyO^WF4~pGAxc}*fk{;pEd1URwuy|OAs9*;&;-{daTIUObfmb%;~7yf z^I9dM z*NEI!*d-|`HSxlxWEVc2-4_n3L(JvInTL-LpOF-|Zs;OEO(;sMJIK~pdvV&xlqFkl z{|O3T^bQCAY)F@Cl&Q!R(GC*34rtvYo9`&44WU7r5u36zy> zmwZ+D1M4K8XD6oRU*ZlnaM*EA1_=>O6_zLQH}eY#ySjfEqUPmx47U}})xf`Rl*a>_ z33{_>LutN+VK(8j>+wz%k$WHXR8Ung%Wj{$-amk?zU@ukm`;tudTuZAvCFc zO?s+Rq|nZ6H%yng7-Kj(=^4&T@Gy_X9h*nN_j3AChor&&#)C)1ugz{8I4{}rAh?!H zXB_L^6u$H*yWQBb|}S{b7=Gbsn{)+%J-NS*jL0S@=}rd4q?C*M?UPV@g!- zoE^2`dLJSxWIo;fte{2c=KG87bpvB~Eb1!sgagAG75L6{b`gDoE%BQD!$brktbT2d zdU5?_jwQurID%Qljz#efo5mkmIy>WeHXkN@tCUK02kojoX0PT&V#@auj1FGTdF&A~ z&FnGa=|ZpRf@ZKr66D7@^br(d>j<#NZnKJ$CUbD;H+!z_QZ^4%k&2d8eB*1`eZYri zE;6HF*~ip-g|xQvqP}5^qWMdX%@;OnJlMQ>a$5nz)wTOdRf>G1a=7#52l1!RBp{*g z%g#)99i;Hf9y!;DRM*zh2X$moBB7^9=(cn1d*^2l@||%srk?;n$d+u}k;2>nU+ z$sKsi6=?NcXGI?-JwkWl-?kgW>9~_}r>?JIjJMU^8rQ;?{S>tQQ?&1R((;W;9i{_z z9tgL;s1#C_@%kBWc*qU2*Hgk7#0p7LKKUP><|vRP5!c}*BPO~BqY4to^O`Xu9G=M} z%W9{o58TZ#AK@4AbQC7?P7DuFqS%#mA^xgdEE3X{yJ+5E&{B||zbXDib2+tR0bRk! zRuMnD8(v$|dRo$y4oOB22cHtZ0L{fq)NBPv2yu9PEH{8m@lM>Zp=QHL3(r~5U5oFi zxN(v(-n*h)F}LMFwF!%<4u|@yM_zk{l+Wgn4p&~in$Aw2{P1c`+xA9_tA#m!XLB%= z40!agI=pk`NZC9xup+wh&+RHCGTSA^XIhh`MYzBIpgFH>M-MaN0_0>jZF;43Jg!;N zcb8{{^O#Cj2-YxPfC zZJU>@phCsED+kWDSTmJKV$j|>--DH-MV>|TPG5wWbvSPGGkdrAiV(vqn~o`8YVkHR z>STZDC%~j=PhM9CJigwK{`yLe+kMKwr7Jx4J91qv~_<6c}+{ZZEElS@MLQ=}` zcFRkKpb+-g#X(aXq(eMvmd0Zu7Nt$(+6IMG770&FW1r=!soM1&vn$4n$EL64uJLfc z#FsopXJcSd%o9W;`&idbK5>%PQcI4kK~)4hj=V7Zn$07VyJZWy_OT(Ni{y)ZK+8r# z@jIsPg$`~M;cP`hhmeqxmb#baopB@3v79}NgtV?8A%P;o>FuZ2n>bxzdqXEZp=7o& zUV^`VCVu45$t7ESLPV>|e#X8EN#q`Rg=QV5sKSn0PTr=?*Oo4KqJ5p}ykLE>(avn8fpsW61 z^rn<)Xm`#qXJGZfk>RJpcV|3!ng@(dlXwT8QO^$_rpTE&lA&I8>zutKn_}x-#l{$9M`ZXzs3Yb($ zL`Pb^^qCt=Kx0h15Jk)_rHhVu`X0v??Z`skYfhW!I2}%oTb_#Ss->*)!!3_`W?MpV zJpT<6!W(JPF?+^o=~XtvbI+|l98e8%^sMZp!_K1B9QQq( zmXUe3xoVSfSr11W;q>Om><$o5`S>oW`(o#W+D6CZ<7Uh?Z>dsMn?AkN<8W59^Gcs{ zBCS`TYyORX2KCFh5`Oq$L>neJ5_q|Sqtza>;wtqD49M(RP%Rv2kgegBq=s12rOa1D zonvcR`$e7PW&$67sQ*xtX0>-zm@y%gQ^5>4@5^Idb*j|Dlk*Z}V&xvsdD+7`fP`L%_hm=eG(He6Jl#`#HC>oIJGeoMuYsy3$$k;cQp-aafL3F)qF^hyi^iW6{(=8qK~gRpw+v+ z`OO*~S9!aX{kkp0@#Of&I3{b?tEXQZ@wyP5^cpC&q@B{Cda`>-29{}b856!DnlIa5 zmfkCBs*d13J?b52kY1kn+Ca5tOg>HGQXWpzHQfyc%w+lp&wSFQM%O@(R)M0su)9TH z7`!2`vQrYf`d(f5%Fz>ag-0o-yG3+l#~#!rQ7d{g-RwKZJG3JB-2uN>lN?VdC6TAPYgu z7W^kOY=!--VUdZ4j2MnT)bdxFtsSqmp`v0E?tV$U?e)Zl!dr;J4d?pa(iq7G^5smN zP)OfN`-nz_a<=*#8DJ6`|#Qy z+u@D|ygaGN>`dBlx!dWY-9sOIb=!oUsfRbj@gA$53OQiMRoH;&i2Zz_P<0s*-}B=Sgf48~C$4d}-x*&-?~RsXAae+584Jto z;6>r%rly8N%xwxsl&Vwk+FuG24;1<~$uR8@5l{Adi$U$fuhS)Wda&!D-#*KIfm?J8 zlgQ<L8mksydQC-*wx1maz{Ff zY3^PwS;<1+shJ0m>VT(XJ@tLT%J z4>5C&uvI8y%9D&{U3-hL1;4((V3P0@i;nlfR83DP&-oosM^%}@en)Jx6TXH=jQavC zY9>8nYhJJ&^uyl#IBZ+71}Bc6CJlxLoRsVzphns%3-p;uyGl z9HR%eHjkuvvxr{|S6pvHI&qD9t?SqYhgml4j>mDHI?T7S)y0^NAj^%vd zxf`tN#H1O{r@-)&3h5w0c#6=0q-_b;Wvgp0+nizvN#0S^kmJ2~w~0XX%{CD)=5tf~ z-Zcq7FjGHe9i0%`nT;>Z&?(Fez8tXB;X7Z>OeO}LJpO19`#+3rhRt!vuAd`8!M;Nh?~wS z@o?TSjXmJk?#k9wIs9-352o7$@wTZrFQ(3xdxiUo-=Mn-Z^}--l{;#`|6v2On|fH7 zwW5IgyW7$PAIG-qF8EnTof%4u0YNivh*%9l0ta2K{s>y=_zFmK-2W4ptF)HAz9sxnVTPgQA3SPCm8 z=$B10k8)I#v5|GFu+|NooGuvKRPDUksHB{}PeN1o$>fI&G5ICyE|@SHSglgjg!zEh zd`qzx!(_jrv#3!Lg4VF(#z_Y%EvaLtReM^GnP#zf;HGV-dYpZqFjYidl~zC{baU9F zu~vur`%89?+-b|eE!So>CCrq_R~nl-+cq-TyH}B(z_NS1HoO93aIB})fOEfP6K=(3 zO{vtNt5F^Hs$r5`SF7%9%6l**=Pt+?6F*xpn$9Z%p zlL;f5G7X3F8d`5fM@d({ekDaC#aD56lCVHrYI{L}=PtTCx1d34T9yH?o3XF1T)uwR ze5Qt)aKe4Uu=)-E!>o(NZ)n!&vCr2XLE}QtMZJcC8!^b1FJsuQG3XRwZA~v+AIOa{ za4(9S&d;H3b6Bv%bVYrl1A|$efSL5O;%pU5e>fZy z#k+Pp{Y{FC7dLxE%V=;rmg<@X$QKEo*v?>BI?SICA=}m7th}*l>xN4I?LKpHyf zi}pK}3Se+Tz^4S8AIrz6O~z%Z)lQrneKv@5*^w%5P^3)#bqS*Lj40OZWcld{4I%Zi zwn;h`)kHnV^;1;(k=+H8k@vc<#~G(?AGXn+;nLJJ*tJ{oF)LNbQPqopP9mt7j|3s{ zO7%rw%G%f+2^;-CkiKEq)|lwg!r;UsyMwt7_vOZq{IyU#!FZL+QuB8WVECek!Do z-h%I*MU_6#?!gxJ(8Ggg5p7s)WRB`mp zfU0X>#{Onz8G-V(tc{O#&TGD}Nj6CLRglEf@w|wS&y>{T_}Ccd$z>89Y`1$@wBOc&Y3qed}YF5Onx zi6dva$+Oz4v9?-5dLQ>uF1g2nxsz}E>PzG>E^lV<+5I$%Aw2fU48gckbDhJ9M$cW! zmvXL07S{^Ko;w$kfHj!lpRZ_jkMCxbmGy{Q?isFW@ksGFebvd6jpTi|^}Xzmn>h>I z-B&0rgIV z2!pf@wNWfIydOGCUi5~}&ryQXJ2CRk z(Kq$QHO<2BXZ)vG-_ng6uYWz*lV1I*;-nXNj)>o3)NYBBF><8Gc+GKpQnPE?|z)@)AYl{46IeKQu-Bf8o2s%!jhVXtNC#hr=!<} zUIUs*pXt!FXuz?6g-JaNW)qwb*o%YJ;8bQyJ7r7l^eDlIDVrtpK$}C0=7Sp}SUm^g z+U?@JUo)Fv)v{>{8;v^YU8{(WDdPU9@;Fezpyb8mY+{36gP2z=cXv0`P58d1d={~- zTIK34_Cu57{6m#~-s}O5WEV}*BSTN8&b;uLwTs^xuxBbn`e7+?@rgB-l@2tGGgP@Y zS+c&yDX;oV@>t&~X;|Ieb2V9EsBZcNp;rKp>W2<1oqpn8HCnsKGlEk^A*9@LYhP1| zrH_s~cHGha;3#9ETDZqIwP$i&f-47K$IvuK-!m?maUkunUDoTFq?)!~^!y+?7H`)k zJsDOXm!nK#DjQMXBTQS%jf4mcitQuR83uUGtLgUaq8#2{?eVg?!n>e}S=&&++V4hW zrU+%y^=$~>{nQ+SNdn>exLh1h&}%OC@D&t?rwMu&h)`QOa(3-EW~%Rh!#5BXmA5tVVV{Y22ihqe{ zDUSYTYX*k?DP2n1S&4x7oMwCM2S$z`)Q_4a;vpfoqE^kfzZq;}_8!en^TSxGWBkyhl(P-J8*Sl{qXbr|Y z$kM2|GE-~Z7>jao&8Lx7vZhdh`AXkUD!tXOYEPdmz39_zk$@Kx7A<@!l{BU942WO=pQ zUgXtGOkNO8Oq<%{$2+%#uhCE`*|Gn? zr(fRdQEaC@?RDOMG$Ew0iH6@&QqPD&eT->n-m|RBCNgTUb>+S>LZjKbqxX*b)uM#^ zV=?Eq@LS|lJ(xg3gGPml);@3adfPq9h!J2lmhR#cX%F67V68Wn(M7BcF54q@K?A6mIY#+ zOA*?Gi7Ti;w=JY!$Gs$DV*4Z3qENpk73K}H9;p_!&&T7tPMO3r+-jgk0qnI(v zIQ32al=>!eaS5hkLPeGl@A_ByT%EI=-upP6@4SmabG@yr_+<8ae9szH%Aib&4-Oh< zFQ`)`7;8`P>U42WMRhp`+q<^jc@CnJD%yg_3I73d{z8Eng#&JFV* zQB~@GD7J>OE=t9yQR~tWsa;eTLX_^n?6V`&4XzhVtzO;0O}TuGWdvK?{;;FuHoh#^ zHbHF{PwE4PIS%`^l&Y5xrDbk&kQyKQp}{o%Y`3fpX@jokdDhdu$A2f`kV@YtqI0D3 zUT3+C6|t&hyKGYA>yF8|nh-|{ueJs;YIl1Ab1kK{+r{^h6V$7hp0HhC{y0&F7*j&Y z1v|3Junhls#d%>4@4{KE`!CVi8f?x#RG8+(OZhM!$m%CaY}Mygb)Q^*&3b34tm!@SEnp;C2o^nwKMVkpvoo@!Yp>Xn?*P^W44U7a$9GGLivz$M+jp@qv65L`olCdI+j(3U-3;d+vz%&<+s^sc?q6s#;1HUG1)upyNzG! zZ0^?QHVM*Rv?P9a+>Da~;?7$+K?A`^$YWZ@AiT4Zi-P)8JQiV;g)>e`rhe5qUrN;z z9nKeqjhylAobvW1%HKbdPiX$GdiGR8K+x;=?=u+WD( zpVsp0bKDzZH6u5aaB}+L5$FS3!>+tq%7lw?DY#N&H!)9K!+R_%62OylUD3jov%}F8 zTPIA6q*JDoXFP;m)y?S+31Yamr@d)n_#MvME?$LeMr~z>yYJ(7Yu0GV6ynK6Pq>on z=%^7>$~)uKR*$d|fMC%81of#QN3;t)9n-ZqIC$VOnB&WNn zB?lrpcpgpe&9-u6Zk1WIeZo>R@zZ!GQ|wJi>I&RgD*`N&tsKmpsVPSjOU7hxaseON z1i~XQbe-Vk>gu50ux0rA{g=YZ#Z6nN&y9Z&p&YWLq#kGoeXNlOqrrBJp&*z(I$n_* zsCs$$aPk|yH@fb-KcosD7jhHwG?0#sE)Cpl*j|uLDvWFN7WYwLbIsQ5Do*JN%J;WH z`~oH?6z#c}RC$9h`P6>2a2IU)2l35&=6hIp0Xdr3wGKK?sSP`>U3C#PkjmX!UBhFP zdo|;F)yXN5#@gH+Tun1Rw~W&Q%PvoqUew%BFKIZyap;kX0RD~z`c;I+N&YEc_FE^M51^+P?|!7(jU!%?&ABPP-zr-DN3Ceqv9 z8Xdkx<#ubke3M@xHcvNWU^*9lBs~3<1j3WWIs)}&(K@gW(s}k(p4>r8#pz@peek>j z7>N$dVhY|8QyoE8ks?gyz*pr&r{DZGNAOiKR)60ihiA^W6VM4S`59;gM(8|@H^1=N zXd)%lC}=&9BsQp<^Z3ez{2R-Sgf)u-!|Is;fdTfwIOtd84n8ecmKv$ZdmS-3YZUnd#0rUQyMPfy0|* z*;U+y#Yiv5xZmwk@v9GC2m$x{N>rAdZ7~yLUgxsSu*D-tqo&>0E~L@M^`6E)^T=|> zTNM=nnHmtE~R+`n&ZG>ng2Y-RN{HQXF8UoX2$7`FXK95^};CuML}t5oz6eg{Z-))PFQU{`Kx=`)h$) z@jNzC6IkFMk>mDNG2i{vkt!;0ONwU(v9z8)UvBdGqmR%j5;*z}3vC2O3Be$K1xG6M z+6cyRO6UU;qL{`Tu)P$1sv-Zd1~21&Q{S!CXKT_sbqrrrjcqC5D&g_2*OM=^3N}AP zdzn**&d;WZXI9_$>Cx<-%;w;Cj8PGOV|qvk=f#vF&4s6>leRr6F-hLN>-B25N8HFn z?na-AJ-ye#HFR8c)OaA(f3Wmk>CPx7OVau&KP0tOnM~$hZUJR=-Bv_SxEahXG!-!h4;;fiP7SV4m~emV<gW z;Zgxtk@dTJ!^$@y5;fs&HeLsx(s|nEaR-1|o753SJ&&7@w)ad%2A00EXP@;;EZ3h5`8BQ>8qz+M&uF81|Mr$ESh$7L_pEWCvBqkHV<#a*W+(w`A}xIp)y{n5Dadq42o zL10-n@W#(U@V1W*!~?NI9MDdP8{&fa0r|G~kcP{14xp<4@N7 zkKUZ~PuBd8-WmFpT2)lQA3* z=%@cdh5Ms7|AS!^4;Uu-gKYnUVH6J-8iJr%h>QWTfdR2k4_-e6;SWKZY#AzdYy$-#VK9Qt*HXka zuuDT?%&%DTU3-#Ia5Cvm1W(Nx2!YPG2JFFFCjRgq4IK~%_UIVslelKzPsJ%Kc%7=J zKUx1@_!WvDt_jS}d48^*Le*qmC;vW~{D$FHMMxL?t`s-K3*tnu-WW*n&;9)tqqg$q z+`yJ{fxq8m)aGx#G|CMRIP(3jXnCRL^UUU$t2I2_{@dBk3nb4EZp=AXb00v!(u!m7 zQ-P(=M(Rd_01R)ivenl=@QP!5!6^$Xjv?SHz=~sBV8t;akaUJb!Lus}K>#6<2n2ZT z2_l9z55fzXiL3$HU_Zag7)YJ@jSXO45}bfc_@M_ZiLWtZ-KP7*9q1kC;IyxPK4k{l zPcj3D;!iPy53uBWm=XHJ%z#%(`x(suH#(E?iQ{cRg1`f9KJdr^!3F~D9OZnlApxOt zBmTMHgxkxw7+2)LaRi7f&Qa=dBg|?@KmtskTQ7d#XCchEPd!E=;C5Q@I# zaV5zpihrQ>{C55Ojdq_G*m~IY!_f!q1HNwRe%1-D`AJg;F8j>X&GoT9nmSS(*wpE4 zSm;;%lcsKY$Ac7S-qh)A_)$~$4eEbE)xL+mZ<{(4eQ+1~OY%kW4{%`BA%~Si4F4Ve zeHv(eqka5if!6o5?&Tv#s|PaQpdMv?7DtLu{973aevkfrZXHprXE_)DzuC@DyLo?` z^+9Rp|7Y#|n)F{|!~KdN^55OBc#S>ZuLvUlm43x4)c=n>`ED<5^rifj{W28)zQm_5<^L@IzUE!Dv0(4A;o|`P$$xf$-fq+40KGQWym#5~ z!vploBezwk{{`=Y{vGrI+5AAi3`O6k^8K8DDDC{d$oIdgonMpwDzVvb8PocbXZo=* ztyQT11>3ZI%;QV+ecrZEk?;Hn@y~ir6#r2CTh{MJwcj6zef|df_ciabN^JIzd6)0; z-9P4CR-yhEwDWuD`!Vl=q7N1MuIN>wwDS|&`8C@#Z#(5FmieB4&Ni`ZUhI`GM3(Xt zzdq)>g8E;P{&&##W437(?xN`XL_5D7`J&{zG_4e+e}DsDn2UpsYQMiWZ4<@6Kfu4Q zd1u18C$h-jxiQ7Q~9F!_Z9E*Q~LL{);%8` zy8WKkeSE2PFT~z%ztp-{q5fA|_jl0uW7>(L?^F4r`1h68jncngi+%o+{QH{qo*#EC z{XO#;mPO_Z^BGG22lE+LQ2#5|`#b3SG3&DmcTx2Hz{ttpz`t*|o)zP^|J=Cacg(gy zJrntgZ~PAaecrmiCjH>Gzi*~7I{#AS8@%>^G1GVz>VL&HeFuG?bN6ebrYQP;ZKg4b zfB$~|J^Ey^h0o`5u84j9Ip4#w8JHaKa_#qrZK*Kn`}E+2SoSj@4lp1P{^sxsTOZ&4 zCYl=)tS4G{V)xIH-c31AI?>0`zDD6aP=fBf)&Mz3Z;kPX8uzV8U!h%>EeraKcRjB{9-sa*Y`hKJJKk53(!tJP1f#*eZL|KviQ|4r_GEha=+F5su; zrow~K-T$Ot_}iubx6{|S+893i{Ces7bIY-Kf-M@l0rA4aq&W+Px$-w!Xkj&jvD^bJ z<^7y5&|G>bum5p@7EgE&ewoF9c^9?()@IA_&pzI0Cp1`f)BUVYF>iJnlR7d)w)rw8x{MYVxK?rG!kWfP}T=!eNfit&sd+oJ+}Rtb^Q3Vr@v=S z7dyJeHB%PWvHi7mF;=1eS7yR|2YsJM=5tE~e|eqV987qPat?xpSEK*2HC>kf;Cc?` z`~u}11YcchcW(XD?< zm^nc$1p*ea_yvpiJ^TaBW${%f)YPTrU6}uBDEqb6y^3}G=UVr7ggQU=fOr+^f2H+& z2YsKno^OO=ese#0a2xNphGKpnfA!!t-mipWpOXFytX&>ZtV-PHeEmi!h9Yj!ihpX) z|7y;m;zW`qc`c>B?TP;eTlY0jf{GJiX~OZue>INyS}$f5>HmVeK*h1BegJ4qiyrRV zFHWyw9Z_-YvY+nybjb5_?tUYVMGYy!hJAS)yXtu7=Y0J}H1;W9f5B@0*N8)fil|U= z`FOz3t=9S-ZyGPZ$+rV>RH*Z9XMze9QC)$B5wAZH%6_e*iV79s!1`lH^`98>od1FpsE7&`mq&4^ zP!TkbZ;zU+63YH##`=54^p=b8-w}v^Ezm%5cUhqE9o?|c9U_Xm%jYL7aP!ms&pvNJ zaTmqi`7r9=XLtYh0R~jtLba`bYuiHUE=qU*t-AZS`-Q)qK2+QPC)@ttF8#lqK9qK% zwDa$Msy@H_qSXAIekTK{y%YGb`1qY1R)?x1U`hdSaFQGCA{EeycPg2=d~IZ zFZqWO@cn=1=ST1NqvykQS>WmKtk>UuFHTGdA_qhkqC-#(KpVg)044?mkpVCQ>;#Yi zPyx^bumQLT;1BSn>)>*?fi>ysz>6mx06$yK|M%!UZ8wMu-;T!9FV{y%E|kwGon&MuQXp z)UnTMf}J+VJ_4c5m3(Bbl6S&|Q!g)AW1(znKp#4w2Mz4u zdJnGSS*2{Qxw6q$mW@U^Pc`N8g6Aow1m`b(WTD+hKOjQ^FW0SE$fbfL08exw6Tll? zz?b85TQjH$aN7cKTW^kAW+1IQ_Z@te1;jdcxgumBI)qFDOKL8aw+_&+4IKkq)&`}S ztdJTtv;j7t+%=dG0tfx%d}0mA(T20pAY4!?3uLr$7?}h1&?1d%AtlHFl(i9*Y5-XR zO0~h6XwaOxKzpE#Kqeo+ejTv7^U_`q$%8Xj*5|}r8Ss_sA$(|kAvS~-M8LNR0qYVU zv|aQLgh22uD}w;=X%c80TwhpYmN>Ii5(EiCbUjWWg z{NjZKR7ifN*954Co3JRfd9If8EjrYu1LWa?I_d&h>VSK}wa@|EAwUCMV^R>;&9y@k zxb106lK_1qnJa;Hj!%6&a64Gy6Wro(ATkL5`~aj00D<#KfmG1I9_H(OTUt6Fz8VMG z0M2hjXaWG0gl6&YKxYx$b2@cQ9Hs*gIt6&K12PBDo8#LnIm83?*J6^f^IF(ChDai%3Bk->er)Ons<8?i=?^2**gngUYi35p3UOpCdKH*i9 zd}yUSHjHOx&ft|>E4(J}zI27JrC6-C6+QJn%^{(_)`{y3%|6NLB0FVcrNyw%B|W)X z-E((BqL^$CYssUnBdp=fmqN=A=N_D(xZk0v%Tvj7_wB|HxE@4#_t#z0syMNcVn_Plq6VIKQB*ZvEc=PDf)s`ALU*sw+Th_Qt^LQUJ8^*2+!qUwGnSA>c4 z82fy9|5&upZ2aYkrfcmR$pY_T<~56nlgj6cK9}&J=y-BV>xTyhr1` z?-nDI^4QC6$>lgFu(XO}nDAaZVDSD}=v}rJrA(i@y7#JSpQk*WsF?v(`Mjb=r~Jvz zfeN|l*8_E!)ayn`ocVwSYBbU6~9{-bMn^CdW`)pL$-M$J@k@C6>lN9)-k$ zD0dg#t_pco+uCxD_t`s=mpAvXKg<$@wccH~oYgIDz=0)->TZJ+bpgxCrXtd|PCN4A zWKZIE`<_}VIMToLxe%A~W@AkX^UF>BEN?U)F&GM6mQ?L{F}A%~yqlR32aRua;%n-% zxEnykdIh)=vek&^fXVJTTydF-mn3V$q{uo;+i9ah5r4aLLa9l{HogSAw(}br*N4UG z9hdlEasd0<`|ACnKK`_n9dWLTl=`=KBkLeJ}sr9|l&VFu2!zwQ*ajoECD=WKgLa)OW9qdg)HM z`9XL7HncRc`{sppk9cnQ5Zc%V^x2b4x~&OVPan80+%l+Mziv?B@aAzBEN*izLgPoz zUtAAfHz?L5Kw^%-KVq_P=25|$81ZCZ3WWy+DoNcj*=4L;Tf>5^A2! zFhis_A8KZ*cE7Z6#yKNFPi{~mKGta)7#ts>%_D!>VDL?xZHz8@!bF0JnaR|L6OE+X zM*58#r)Ho#`XF5}MP*kJql#AXgjwD_!ML$I!lpKc%5DwL_DkM7pKr-lS??cKzKK|3 zEOsEWf27;5YyxLZ(xwoN#LH_h7oEQdPQ+l7f9&E`eT*SK(QDL<`Sn|0 z5HP-7SKuLL8LikAaBo0sR?s?Pvx^OTRqPhFqq)3+40NvEM4ZGs#w-WA41JPR6J$d& z1RXz2b-sv-l5;AbREb~rScpW<`Z2GF<50yATX44X)L?m%>5KT1W^61%NQGx7hicNcI`ZSTUzpOF-lHV^|4 zu@DOk6bn%d48&Fx5fKAaFhRub!tU2l50?_3aliqeQlf zOC41?8}`;mzk9aCXL*Ue$*e3aA{>+i}Wa zhan5IKkw;O!S3KLN89*~*)xrb&pZD}{P4o}i%n^qc3AytAJUJqjk;w1z_G~a!S
)pL86 zD+c|~=3VAID7r{ZmrYwoOe);V$kd~JgDUB5#Or)cs~P!Uwte~ZYuA@%jy>M)i8wsy z+Q_+=v)TQ=apuGg)-9U)Mzyuto_>St6-~9@?(8}L#C7ui4TW9C9liCY_WH7i)_>eF z;Ys^PzkeS3rs|CHoA0i7zq)+M(WteZpOtYKYqqz~t)9!*Kg+uK)JF5+r)q55bn8>o zu#CIsH92^qcke0AJ{L=*wQgo%X;#6luV>7~aW&?bj_|yDY*@z&AC|OQ5Pg}DE6t6gB`Pup_0yjnN>#{gt!UX-fPrQ9vPn+blIHT2@ z;5q5ydXyYEv!Tzz(c3a*^fetiAaY0bvxcTK$A=iK_cd#`zR~h$z0+pi)i}fKZZ5CJ z&hIg)>9`~BCdIek<l7L9A2qPLozvR*R&ljP*LEK^(0Kcy zh35GhnyfKcWqCKl@S*oMZ>qYee9ZwZHf5>j_Hg(HgWIb&8E)y^e&(v>Hf~*NGzwex zaL-uVSq%?Q+qtA{lS)M=mTD5#)pFYIOlwE`oHFQT8$2mjlSoVR)3NzaRyk|$m?pPb zuG)vk?KvKzH+LsEz`4*haYW;rhB-4v+zAVcyzj~K9 z;X~S(&3iKFScS25ZJHjbvoY@F)DfEx2hF@195f)djl)5cn>YP4Ht<|n$^Gb(N{0g5 zp7p%$Q>lja(Yi<7{Jrb>Zkd?H)8+8aMlJ>G-*K;Z*urbmnf%fDu0D#p+@#IbOJy8Z zSXn$T9W#FMM>B)+bJjmRGSk^M{LsoXkC&cZn(eS=y4RDYL;BT=Zc}YT+m46Z_FGq~ z^;ZA8MqA!AH1!>Gce|f=qiHb@_FF%Gt{GH0QyKl^i*Do_*Wvvr`wJH7{nu3w)*FAo zYjlD18#~=g8@g#^)tJks`C@A2asL#sYe|oJ750SJH*J4?#rUY0O=q6?XZH%U4D^3{ zdd!3}(F0}=ef-ILUHH^p6%Q9|U%!$|z9OC1`}yxZYg>I&i>bTj-p_liR`yjJ%`_*w z`!9Jt`rUC)U#zTC?Z;2j zFLyC^9T>QMQs|Zm(+*k(6}eWp_2MkX9`WNkPTtjiFn{PMUKp4iDOchxadPYm(@J$8ACIV0=cahx;1{DCbeeBO2LIrGI7P4qse zZ7nRK&Wi2$T9d!7`CKB);E;kjGB*8EyL`mO{swtw|90d?&MAwl>mO>odGp@u-=eNb zQ-A?{Y6Dzemh4BzaJN>qKL(I;#P|K(g-Fk2?%< z6cHL8=IL12wUDEIa9B`y%h0e8Psh6cl?s$_w2z1k3~L$KE<7yQ)3JMSgk#w<=}k%p zdIYuW&)+)SH`@l&4+bN`A%T9q^ z3AlZ`f<+1yDps(4U}%`5eT$I7o{sGz3p?6JcH>#gwmfSQQbcVlqPFp@Wm}#F1_klq zhF2s#(Qe`IiCezCrl@vHQSEEov|HS?Timr<+_hVZX}1(}v~S%mG^{N#QV)*yt-{;+ zByP|?b1Ya!rJgz;plYQT+G8R%<1(JWhIDp=H)^9j24XcX;uFj$2JWbXPEcnPhhQ=G z<1OqsTj7bCXpCTV$8gNRDjdNLyoMp?B{HJ`ybz4un1T(soLj@ERqfALe)QR@e*U=C zK<8E!`j4+Q)wxxjA^GvOW;(Z8>D-!C=ho~xx7z63YNvB+Zk=26=-irD=hpl>x4P)u z>Z)_AI-CDv>J-t8;5PomF~iLPqzIYU@PrG?6QYbo*Vxq7)uFO@=yN*7fcl2h?YjXDmkgd<>o zgOH1W_QQuH{(=FVaw|ii`XulKbRdacNZ>K}OcGb*0Q0evM63mDsnnD82B^l}y z@!LeZ3(=jxPF@g6uY&Yxg+yk%iNtw`yrM^LSb=`DvdDQKk?mDEj_Sh(t{lsXR?&<8+adGub|y?QcL1gGAk-}R=itb<*aTl|+dqjrq6S3US`Q9TU`HzVVKEcfGDUtQ38Sb59 zcJU%%UuOF?k^VOrTil}5-4<#7fa88h`aKcp`%L8F3z2{~B9Gq@_YainPa=Eq>mUSX=4M&isha>7C*W2`jN zpTUh+MvWZHq>**D8hMyiBWvw7a?L>_MV&M#-4=u@k#N zvdUhNv_jI#K^y`}EJtw`=Wzu$aTg?|kc{#g@9`08X&lKS>5&1Mkpo0mh+4vUuOyG2 zsDR3-0e>_{3k0Jz+Mpf65rNK#!XS*pL`=d|%)~q_#!9TmR_wxF?8kAO#2KjZSXm~qo%rO#@mHao;W)BaWsHs`=vbYy1~Xrg$w)^#QlyrM zA3h_THf6XU{8JV%#k(!HlhwHWFq zZK7i=e@C<4#1lRGP;X$^k9vsG{ppVfkQW1~+qi_-LA-V_={|iBw1DG6rfw3s5AV(Nb;z}al`30lJ7Bqu*U`7QV-|dOQXY_L7v&docGFg%91Y}onC&C2 zu@=>SqdXoYU2p_>5Ale&!;~Q!&Zb9c`%cgvU@0!+!b#eLQyj}_`tmc>xwDjC7@ed3 zV-0ehr#xMt9$%*H;Uy|up>4TJS-M6&yiU1C@f-99Pz`cj&XM= zD|cyA?@`t;^gj8EW)G+nc#87z)En%E#Y58b5i6k(infnM7CvEo@|3m#O`lOOQ204{ zfR8Bgg7yiQ@cAX>=oR6^{xz?EL%)vTx1{SkUWZ3;c~3l``JHlrFIe%Bw&WB27Z!h^ zJ^D&H75Ys2;sx}@7qFK8_#qY>XvBd2*a=2P8u3I9`sKP4@A-HPRMaVNO4t znU1LjM&LS@nQO$DK07z&V67!#BbYwCmoC0#0F4EAa_rMrBw}UXV)yM$M z&7zS@$YH0E2{5Fuzm8>&j1iEBe*ZFJa%rTzvqlCYLvD>6fngrT1enTrAboy~)MG4g zxB!1=44}pVlgL`Cr%+Xd>LH6jb&w@k4yu7{z$R<~MO!G^vKe z9*T=lJcMGwnh?qXC<;PR5Q>11<*e!;pYa)FdBRtVuXi%ur0SAv^3r775uSIYIUa*&<|# zkR3vHNIsAyLY4?aGa);q5DKFRiozYmP#h)Tfl?@qGAN63C=V}Ggf}X|2USoN)leP2 zsEJyr4L{UDUDQK;G(bZ%LSr<6I(Hd>W(Y(OTA~#~5Q?^Fk1%vVM|46Yx}Yn%p*wmY z8a)w%UWi3+^g&DW2guUg8yA;|<>89e&40e8OjZ z!BI~9A4+b!V5!8yzv@k(Bq=!1UY7Prn!5TKmh)l=~bwWQ2?4VZCWJeCz!vT(P zLM}KXH`Hp{e8>+M6o4xVp)lN_&gm9M33#A1%Ag#|qawUf2|lQTYN(DHsEOL}gE}|t zk9uf;hG>MQ2tYG5M+*d@C0Zc_tmq3M@Ojh%@K%17j#88bVoE|&}>?%vz5a#-{drOf#FW z8KzpN`~I50x8=w1(@~%(2nlBzv-6k>E^3>@IH>)Z(`}Ss3px0ltd{oF6{@P#164l( zAA6;oWPf47W8zuRuM!&@tHxp$oX}14e7=KvX8c^0U-d>c`Cu-3&nvRsT=bvkVVg$M zK34&x{7EAw3GWkCN%^CC-z_SSdS4v>*S>EF+eYz%4HUKu6i)5?O7lJqM_g1)pHmm4 z7v1ts%^;czI%$5}OJ8w(QHcMIOxQL`Pd$uIcpjJVd~w2aUYmx0>JX%?LE>bd#d88@ zqx!K-c&?H}9a(c8SL-JpxoV2iP*sCC0w=cjA}aa`11;6OmXwkV7C7mb(A^qKS@x#V z!8{=~QpPIfe%yFvg{1wIP1=w4y^XWbJ<=@X&z9H}qAnuimBNpFC_e-KFB6JnYPboj47a7|&)#iN|=J zCZ_{*hn@L{u+^kl%CKz-J8?2CG3>IGTh$cmQ8H2w+cqKWM(nwQCNWJ^_tA&ru0xur z%283H5<_)}nuOt<1s$Nu@2?t*WI49Sj?Rcw-^*hORp3&Tj4-xnQ(bjV>gc`LgQ{Gy z$imU9?K=qCBB@MRQ$k7jgl)}v{GMhL$*e~-_pPa?$@laqM2L zy=v2}IPX_=!pKHr&(y8_kPwuHbJmfLu9+H<6HF2CW$Fh-QYWvS~tReAmAP#oq z+Tav>`krIizU5evq*km>iZoFN<>J`cbpZX>*OqKV9$D_L9yi-kZFaRAHoiuRmL~?RD;?_a^Vj(vk0K zyY^WAS}&O{Ay<=!t(L*4GL<~+^n{(bJm!0gua41=_@<2K_mZP7efnb$tQOOx4BL#b z)pqT1Tab1tY}HGq9=2IR*vZ?We1x%^k0X zw*S1gsh;EKy`*_Uypr$LMaNz(*sI$9^S%DHUQ&&Y6KjyR1uKxW*T2((T}?{UYkwh4 z)#9j>X=+LA)b^jpt}7!z)%UBik~Lw_{(bcOy&PKp5WB*EF?J`C##PDN=bH2bo|OAJ zS@_V%W6FA}+6QeNFOsw$ZP`xV>+7oH$;)$5-rMJw%X9Kx|3r!uQF*M6PTT9dr8qj( zC#!xy^~p9r>Gj={-ur8{P3=MTp=u9>c}$sh$;-W3AeXXaSWz<6_Mevw)$0#V>h)ic ziplr%y<}MZP}|f3yp(&jX0K}d&-Z$qF~&#A#|nEsfAHB)_g<}kD32wQ_L_QaODu`H z#`&J?Z$0A?b7}H>tMm(^1=!$4)JXOc}cj#7=GhdF=WUJ11f_hS)t+AA)p_hBEw6 zV$_1mlzYv{Ue)%W?^TV#W+#ooOenLu_sZp_jKNm_!Wc}Cz5n-)!PK!PKU!@Mq8)>! zNlF)Onf~t@gV`tTN884#F<2X@F_=e+Pw?6nP&JR?<=0lDF;IQoQ$9 z#&7yb?@d0&(okm9c5OWV+W3u$-xOmk107+fA#Al>8@3vYXvc4Vvz=AkcCuedNf|p6VyCwMJa%eaT9%QU8kcfE zw?FLZdu=oMAx-Uc?A4ULs_j4DtLhKb*|QL{KRbJ7%3k%J-{Di0>Zjv4HtpH7Eo{5V zk?*IU9PLE>lg^&iW$*vJBe#TUq-3Mplp{C)q;%2d`mc=KQpPG}uGb(1v}dvECGE$W z{#c!Uj71J4o?|j6eaA5t3n6{3=45lSBmSPNITF$}d8q0lAyt-=hid;V)Q*I@ijm!Q zYQ)hLt>pJm?SBYWU1XFpREKY&jv*gasQQ$?)I)XnA=E}XLUsHWsyfy_)LM0^#I#gv zt>X`&Hr5fUO2EW)Rma+!P-_z6)Ko*wnGkAg-lfegb&-@xfuiJ=>O5oN^cU*M2GT_B z@14`1?a%3lSg6adQl@w=_L&%q2-3!pvp+XT8&5SUrn7Y9`XN+x`ButMoxg=Tmb6i! z8gk63hwA)8sLgeRO0i9LthEWTD=kGkMg+Q#HTMsp2I>ek&$m!jKB`cs(DJ1oYMviL z4bl;6-fy9*GWUY?%tz?yQ~}qSu6ch5wWW?w^L-07j?h%7)i~bNL(P{EYVtDIDk;?D zWiJ0;EOTmq)?`BZRAZ6h;1kVSg3Kb*rSlOvC=v^>lS;GgKu_>{ZorL(T#6 z+gSAV7!0x2AaNbT_p`Slq^1$A#(he6D7mPX;iz*O+8Z zO!%80H|}c1>(!cylDxMl?^a=`_xy(;lV*sHpj^bqIl+fwUHshN+cnWY#%ELZJ1HLIEvi+O1u+EZIUxv((O zjYW4QSoKqy(MuWTO3JcmaRduHx3hTl6cy|xBX(<~G!3N^kh&ToiM^lCiBZ#9}0TyNx$GOC@B5~Y9 z9Q}yndg9oRICdnCuEcRYakM8s#Sn`H_zmx%jU(M-!m*7bQj;(l)8NhVRKaniA)@*) zgb~c~8o7vkZWKdtltvkNqbh2k4RH#?5RAZd?1#$Fqc{c?Z$A>PG5TOIYLR&A{KZZZ z@C9Ds9p2+3TuG-AXhNcn!e}hQ5uAqyWh4yY7>K3V099_(YAEidDm74;OmN2-Ou+Bp zPNOm%3($(pX@mYW%!4ojqv6aNpu8xIV%S7JZ9{!NMl{BD?165B$vidJZij_88* z*o*@>iP7ZsI9M@k&Vm*Q#$XJ`Y|O&}9Kl6A#Zbz}NIbw}RaU4EX0Sp=sQU39_LLzf z6hj%L&CT(_7QPsTiI|F+n2*KSjT2~4Kd&1f%?{vV#K(;yhlWAN6|-R6WOvsOdsDRpNhe)i&R@jx`lPi2ma?A)tPXv3g zkQm!=61hrIUh%9n@rSV|@5L_cgK8&d;U(TewV4&*k0uB}J47HF{V*KkFbk`(9lzl` zikBlTF#yA`8r!f3r|}tvCNR%)Pk)17?-DSP9ut1VvE_?a>i6Diane`_R9jQx!f-q7OzR4wG>m zx3RG*`GFGEcn^FKh!$vr7!1OA%*1SLz((xEZk)kc+`${X$M4XuPFf)?OpyVVQ4QX{ zls(i&W5i$(($?Vf6AHl_+i?_+@dfv5QWo$UdbLP1w18=C+6x@SX`J+~ISr360XoJ;Qj~DnI>FbiWut#B(hc8;yBaJZ~8?X;&a28MS6mMZt zpLPX~aDgidqX;}v7Iol{CI~`@4XQ3X{|7tPTY zUC{$k7>ovCT=0ah=#R;mjSS(W3;JLbmS7F+I*amem|1ms3JYM4~OuCbo$b+0cXs_Xg zYKX!>tiefKk0MWzBbqTLO2HR3&-!pxqG?Qj8ap&vtCferE^ABsTTM4%}G&mi+f_lc!!)r`8$fD4*bys zEfIp&2tzLn!8u$))?w@idEg3P)W=0UKs;nP?Hkg<5;n*HdlW`#48tg#!Z}<+JWNLL zdd$T_Y{L;8#RV9QB!6)M4-q_yG{g=Z!eJc430%ib+(A4(;xlA4#|ArOMQ(&68vAh$ z#o|b7sGCXTM*)<=RvgA_=#8cP;4D7CU>xIOn85;>kOg_*3Qv@QH)YnNPWRNJi=3afXigYrN}yk=NN-Y$UK!gi<~Hkk|+;v z_@N0}qBS}r7K1SyQ!o$9unOC85T|esckmQ%@c~BDI9Avp2MWL)W#I)s_@faTBN(CR zh8~DPZw$duOvN;8!8RPgVVuNS+`uEe#4Ef%6i44es5OhFCbiroqz-}DDQJg~NS&T2>fc$VlVRS_ldSL`cVg**> z1Ww{L-a<2*G6qXnp&$yu7c~%uaE!(nOu!UO#|+HIJS@X<9LEVfM6Nl62OrcyeS{zs zozV?_(I2A_hpAYA)mVpJ*o*VHg(rA{kN5(UxuhR5!4^))4G)xpH+71m-sHsLCs;0rYKspBw2ew2bQ zYQYb6(H(;^8PhNWv#}rN5s$}siWkVVfHSbtk)3+GNA-Yq6+*_7xmB-#WoN&YHs9jsDt(hLkvc3B7HFpo3RCZa2UsM9A^=^ znK1>9;S?_62C{9TO~obL#eEoWrGJAZY>^e&kP|oX3MIF3T<}IU496l2*v^;*6T7e<2XO}Ha1jsj4n{lZqu{fXdIG;)qy+-d5(}{lM{xrAc2j-rY7pHI9N~<-D1{0LL|1f2G=^dZW?})BU^lMg z4c_4c)ZJvPV1rD^ihRhAvM2{1)IuHjqY;`Q1fgh$X!L{I0m=``p%MmT7M5WpuHX&^ z9HKqIBuv8tyuus2#|Ic6Cf{KJD`Y@sxT7>Gzz4PAhq|bTrf7~xM4>nOVgN>BI%Z%t z7GNFL;~);<6fWT^uHg>u;RRmd13tm%2*(FYI3O?7UH0^m4p#6-2SlSUF5?r7j}b4l zMFd(Nr_DiEMBy;b;R3GXCLZ7+KHwAdPS6e^J0Z76UOKTd^Jc za0U+$kLP#=!&CGDC<-@}LK)P89~vS8z0n7w5r@fGgYDRl^SFQ;c!Ae=3%%2{2iOI# zGqfM54u6E6CB3j8#}Rdow8HH3jJL2I$8a2{a2gkJ8S!X(fw-eT;xO_eeGoQc7c`g1 zFF3;k{V^7^umUTw7yED&XYmk^@Cxb%nMO#9jEKb`9K$JG!Y$m!Bka0DULwa;$_P&5 z1|DA{Jz#vDa)j(~fFtrCA3U)N8*m3N@azVCE-Kul9-IQBmSH*8Vm%JR^BMa^2lPbi=k&?wjG-8f z8F+v%X!wHHAqH`{j#n^y$uYtm72%Cq@I!q>VG!c+6ir^SADqP{#N#bmyr!-r3=!yz zei(qE7=_uGi$z$Cz1W8nIEC}b`-U+G`eQ8K!r(3GfEBofCwPX>82yeu4+n7p*WPnX zc!bw@i?2xYJMAwrA{U%d5GCP*D)2`=G{#bFLZ%PA9t*Jy*YOO`@e!Yq<|FMUHsb{h zKT$_v1smAH9z{?TWl#>@sEn$ph8n1gRtP~SM4~(TVgQC?7G^`;Y;OhDVjFg1KQ7}6 z?js&g@d;mX;xl;%!!NWU$d588i>j!O+A#h~KZA;BfQD#^R%i=*(QqphjnqdYG)8-b zp*x}wiy@eTsaSwTSdI-ij3YRYi@1h+_<)Zvu*Y-9l~L$&-^ZsIYX!o*M`rpSoQ$c7wnL{2y( z4~oJawNM*P5P%@GM+bC5PxQhxOvfTD!AfkxR&2*X97a4I;tk&66HJUq2b{(Qq&3z^ zI?TgLtj2Miz-3&;Ev!qck#!MPEw~vY23Zn!jDxyqi3XSY*_Q4YH#Ej^I3+WFo!L5wUQ}Oc}viY`{hw!BJenRm9^RzTlKCOq^RNXwa2^+-*2EY|))e&ke@ZD|qyC@# zHQK)?-=>}?e_hJw+V}iVc+?*2|Ab@t_ZWUGlmDj4ziIMs8Thvh{HMx*wjcZ7y1)Na zy#C$Kuh&^^+0#D$Z%#wI6l2f-&2jy!ar^%@P5x8m{Xemv|HM9$m#6>q{*v$e-{*;K z{J(kd>wVq-jk2T7C-wMWwSRrWN+XHC)w4uKJSIN-*?#nx$YJgvE$aj@I4jT5#h!ER zIDnIw;lz1%+`)aQ>s>72g-Tcmb=|1CcI6}j@~}n+>e`j5P}i)Shq_iJLtf@eQ4i|c zlrd1(q=e#pe&%AJu00tGb*+d=5$0hK0CjE1AgF6YWK?|t0_-Y-#T2rsq z)2p@eYMs1VORm+t2y^=P;>2So?XqEtNC&@m#yZp)ts}MZ&q``3G=|W z66b5xoT{2nRdb(e-c!vHs`)`RccA76H7}#)Uevscnp;uxDr&Ao&6B9P5H$~?<~r0ohnmAs z^A~E4Ld{R8IS4iXpym|Re1e)YQ1b=pY?u##I;XGB=c{w?v#=BD{JT2$uFkuwbM5Lp zyE>Py&ZBG3l{e3EPVL;Z_Iz~Wx#Xg>cmMjYNdwiJ{Qr`c z1}WnDy${v)rD>RNP(K!+3rP6Fo4!W0qflRVqOO%zUudGPmp+z>HUgC#nWx5Gyn(v5 zv@ND%3DmWu&rmBj{SOM!FV@BCytJW(X%CCAzMvTEGw=>|i_MX-}Z8$6Ez;J)SM5U?I}`&<>*x8siI0>GNW$vMvF8aT2D!w7pQ* zy)}Wl?rkvCb#GQs*Sj6V_?onXP}j9(!Xli3lON$DjKrG?b^Y1}%V5fuLwqHQ*o8hH>Uy6g_#K715x4Ff zAGTvZ)O9?Aa1{@+I-33k>N=fesMm`)VJc=qU1xI#y<^EgZ12r6A$uRn7aI0uT?H=S z3)FQr!*LkuTAGRj7^k7eK*okp*TYP~Y23iqL5yhzlfI~qz8HliLl{RuU9a*Uw}w*o zhLK+=h+uTW#Nm{8sOv`_!f^zDN8VA4i*O{4u@Z8PB`**+p8P|)iL5_@x_%-AhLcGz z_@fc#V-0Ripv2y)t;a2aRXB$+dpIt9*w20rFup=%sP(C< zpw_3}#l?e+7oi<{{dR=-L9OewhFZ_r4{9B!CDc03NUX&!v^dQgEgZyAyu$BL>kHM| z!ZK$`E2#B^gOTGrbqd8Vu&xF(FdsKzc#$%ID%gv|@Vi95V(ewo32L36TH`nV3hgV@ zy1ttTy~gJl486{pLwte34dRVpjKD%X!&g+eN!Zwf!?<~iINxTBjQmjR@YEW-c6W#` z)VjM3D0`21q3M0DZ@~$iht&h(gy!ghPtc3!HHgE7hy4E$^$+TcO4YhJwINDyfhF~;~;03ZgXN(H9&TS@^z928K;uUos{oc~v;L|(O@;&X+2l59a zKQd;=;?I;1sC7^#xCOJXqyd~6Q!dr8&IT5G8YvC6ey9Vs;SgRi&dk9$vnKp85>v6v zh`D~K^*QO02kscj8k{Y7g!gE~TAKk_k3H~a4NFJN!hEQ8Ef1l`xYGh*tX-LmLpTm| z#-Ul!i?u3Ca2YpX&svoNSjt+G>q+ZLDzT1aIkwU)`h)vjxLpXvnxP;62h)+oG#Fx9m4elrgk0jN2^haF;qZL}C6Cx3XKIo6( z7=clkfa#cp1z3p1*nl0_gF`riQ#g&YIENQ_h2QZ3pP-+M&k)FntjGpe6hvLrLjyEK zbF@GNBGCn1p}vqd7I7GZ>6n38Sb}9(i*4A6LpY41ID@OWf%|xXhj<5#GhfgPGgu%a zGQk#EP!#Sch0-X4vZ#mpXo>)YqXVK44Yh`*canZQDr!xQ`a<2wn1bb4fpyr1o!Eu_ zIF3_L-}0(KTA08J>I-?DksAe32*pqmzNmp9w1jpojyfY{z&KZphLfMAQvXjW@$V_O z{`>zQQ|jL|`FWbC&zk?H(|;}v5^K8d{b_5y$`AEieezBDd01cF&yVeikLvlqek~33 zME!(g0oH>-ZNk@-tFsD)j2SiJHZ*DZw1I>>f|>9s6xut14K`&Yh!Gfx@tAY3OMb$2iIHLU7x zY58+=WhL4`eeJ5cQ<}Q7hq|jrJ(QlnRT(Ic3h+S{ygtO)5EwFgcY^lnpRSCy$6-7s zKz&VX6KJnOQD4;>4eeDM_KaE`kQ;g6f&wTCH_YQqPX-P(R-b*%Obq*@i5d=#ffa{o6+aUqPV)F>_jx<+v$*{*98w}|b3Hi|pL-*t`R zKC=C&*5)8^z7!?`jmc6uL%nN7$}w6c^9-KO4nmW(1{c6z9qI|5rzG z+BQ7#QMJ^nE!RH&adh{6yXu9%|1a^m_O*$BOKsctSABoKc5f*kwXgkak7~a^&TOhJ ziF^9@@$aXDkqj!gKzyeA?fhz<@}GtH8r@01aE47}%D;Ryw~%SYoiaopEpkBD-^#>X z%G%<=Q#W0I`|32hi0&{sW8&jo{f$? zmi;DvNeFnR0Rhi6XwLs@R@Em^eFD`_8DMy-0cJ=Du!Hfp06Q2HUc*Ei!M0KV0x4>3f6WB`Ye-PE!jNbx_%}8Le8UJBaYuSDau$C{{-@=@{-u_#dDL#qy_J4L^9!|@AC$Rw?u3F~dw9I#Yp)l*H zCI7a}|F+B&i^LB6XZ`KpXqm-A^}2~at&^UnPw2c=VU9>qm?OUYp`T5|`&<*o0Q#eT z494V71{L3%w=}#zF^n{EsfD4&5!Dk%Jm1C<)xRbWBTZ5mlIn5j9zG-wL+__?(LHKN z9)|u;xae+4lZRpS)3~VemK2w7BY{80#W*R9 z)JL=bbO4|&yvZK_S(}+GAnnt?KbqBsknHjAk7knvqJ8?$Mzh*bl07CK-QO9_CJRLS zG%+yUrL`u5_S6UN+7OaG{<8reO8Fq*JH(lK)?w zf=m8?b-FJ3|JCWj?>`F)snc@b|IZ>zP78kj|AIna|MW*4(I0=lepTD7B%{B}d_764 zZvd7x=<9lJCB~1c=F#<+7qv{`^&DeTIy(6_a#!Fo&+^>d3a*XLT4Ny3Obuj7 zWdp)8kcv?T^2yv###S+u&ovAsb6z8f%x@%(${NYcZbmZM+*qn+G?vV{jYU(C>D#Kt zl0C>+@`M=6v$SbNzj9j1-Xg86<13e{$CyYfYg3t9(p1itHkF#`(#hPc=_J2LIy6MFy!*BZKs;nL$c+$RG|e8DwHWMrj$5QI4Br zl5n$3Qnx}TXrLBY)u$5tzY-LhSTUneji#!d=A`9AQ zk@1v`110RFZAClz&D&0N|*SYX`ot%26C_9mTn@qdY3&C}}(# zWudR5TsFuloo#Z;zObAUZ|5YJt2)U?KPRy+lS@J^ouy|9XK7p2S%%egmage@OShuA zWp2+rd~si1sa+?pwBQ?nwsy=bLGJlvT(f+#0h`^;S-hNp~-_LL!-GBUVi z8Cg)b3}5b1MxOKSJP#X|l}D}0N}tfOk|jquY2a2)W)>(fQJ&?chI0j(RkDKIX@AV$Dv6I}CF$9^l9UUmEJeaA%R^S{cX9EN*~NUM1FQCLd-zDh`aV*%k&nCz z@sW(JePmO(j|5s*kt&(0FlApw8ar2!gBhzz>mpU9UeBs5;H)M&GgX(5ZL7;bYhMYl z^_9LwedYHCzS1htSJp@P$~D6plHRPQ)HA6imE3Ddp1idsU)|agZRaPyXY&(hA3xcN z>FxYvP>wnh?^{OvaT;l5lO1tKPvcaf@OwZ9mj^=D3dGfW8Y9>Ll+apNQ_y);qzUk#f^B{SewWWOK zn_8l(v=q@_X@hkyTpBWB*pP3$5f(Xe+7Jwv`Nz z36YisTFVyGP_bwnD$iSnN}q0_5*HmReNEcPq55s)M#DC8p?X_!ZqZg8tlLSg0`0^i zu$`>z)s8Q(YcJlm?PZ`_d(kf!CM~?fu7NHNb2V&LCFnl|eoOEPql7G*j~9is@jS1>}#R*jI1(GkQaLR#BK zN~_|LVpS?qyembDXQN1&-Xv1yb&Ql!T_WXj-p&&4(OClgI?J(UouyCtE^@M47ul1g zt8D4eReA<=lZ@frq-9h$S!d8)3fgp+j(kf=fzsXOW%=&1Ow&UKX6+#ZxMgA*>nNF= zB})8qM@f91C@Ja@B?0B5WM$`je5$cnmuJ{NKYAV6(iY8 z#E7e3j6~Osk>QnkiCv9eQp~@X6qHzb=^QHs3&hIv7O}F!xVIGX=`9P}_m%=B`bc!c zKH_EHS28r}%eu;b(%Gt?Ty^Ltk(K+&smA@JNB4fB7u8Q{Gs*A>-vYzLmlu=L9Ls)fLB0CrOsj zNz$X!BspR>S)Sr_smbD7eX?9=Hd)H%nj#y#r-)1N6q%qmP4?JMlVgRZiN5u8c~yM6 ze5yTNCU%-GX-sE`k=YC>SY(FuFEvB5ww)oZ+s%-Nxo1k<{4-@qU7SN|HklVEu$fOJl-@O(`C}mbGe+Yv|MJ?SS|x&mdm!B zE4WB&g&aUf{}mD)yh5g0tdv(-R?2C+l`_PBrR46kQugOuCDqHVlI#svNl=qjV%B~Y zH%?qFUO}s6kn+lR_>HrCu{1jBahdMzU6v3?6h7cx~`WT zrPfQopbe6@^#-vs+9)qeZIn3UO_DC}CRtE?llbUwmI_9jrBI>G5?5-o1ozr3yN$Pq zwfz<`DZE9T>THqnfm@_q$QJ3#UFjF3+se%?w@P`ttrAmYt5o#eD)(A$75|8>;@fSj zEYGk_8fMuhl^bjmtG3%DZO?7uRAIY}blxHB3h$7+Rd>kZPCI04<(=}b*-p6_vQyHg z-6ip+yJULiUDC49E=d=-ORih)mSF$gGP(Y4ITx^793pqiZ1X+x&~}f^>$XS2toKS} z_Pw&d&R&@rvsW6M?vrB<`{an;Z<4d{Z(_mS*#qtOOX*tsML%Z0)XQ-|`uZJ^$+-{8 z?m7o$a^OL^mi~}L*c=io=R@LO`jDKiaY&{F9Fh;Ahh%)7!?K7wuCJ+eSjN{mEDbvz zmMF&~lD6#;^74orsdZF-YkO3jU5-geiDQ!9`xqC(9g{vSkBLR$;}Y$8T-Mh=&Y0r3 zY~ar1tBReF-@;BvIqpFI*7>BEa_8`srB2H1f~Um2G`OpGLyOaVnc`_V?{!+@E1eeG z+NWi1{xfp6*cs6nos~L%XJvQevyw5>Ik}$aoS3+tlT1C&adXV`vNZd7=~w1FW5Dw= z!Q+CgYJEZ0Wx6OQs$Z1dJub@PG?!@CE=#S3mnC10D^jAs70FidifpTPMXt2GB8FjC zWPjePGS>5|G%bHs%D1>Gi-WJqtkA1+&f=PEx4R~R`LD@b*K2a5*m*)23aQDGoPejnfS|Qt*aoLT|`@{hPAg>89*+zbQtg zZ%V(0HzmO4mRu`&OY(T!l6*n8q-3vK5@mE->Xp1Lq4jTzli?jX>w>y>#Ln%mG_QSE zjMCquExspZqwk5c(R~^2bzcUA+?QEh9*90y+;p&rm+hFJH(sigjh8)D;$>J!yzGmK zm-96rib>Ol(hg61K9ncincB_vk(9N5EIw@>%MiUMGSu;jOltN-N_TxCU3xwd-||mo zl=oAy@_8!e^`44-x2JL|=X3c|=edN0KIe->U&z4xFC>HK3n|d}h0Hg3DaNiZWxD4} zNn`g)HWYd#{XAcZrsgZz+xC@Qto&N`XMZEva=wvS?r&spu{ZLd(i=I}?#=&c?>?ZS zxYhuUUse{8pb`^_CDApS#8?mkQEZ@KAr`7)M+8I^Sf$tt_TGE%y_cAxMvW=P#Kgpy zXiU*W6H_cP21VNYf3qx2Jwf-qciuT~=bRsRc9{9@%$?a~?%lZ;RiSj63U^Oa(YW3u zO-=DlQ}u$<)W9BTDx%_D72|nV+4sDw7Sz6{X2;x94&Lc1Gb~+Ygr}>$4eqOZI9xSD z?W&QX8aB>QOG7f$lqQ)f$Ujrv2+33}8BzJ-=uCCBe3mL(KTBu*< zRu|f3t4U$mYII_@I^QfuU5U)$pUvf{L&h8xoJ7wcCUYlknU@B$93Puh9;7^FddSnX z)bv{BWw}f`CsV5Zx9=s=t=5N4b0k1X)k!6&0Whi|{FJ3qkv31JwPbC=Xg>?jOA|ty zD)QBqPG?s_Z>!gpu+4E%m%0yC$9RlZo3xCW3ste)Fh;8+Wl$YShmZ!7Z@h{iXL_a* zN2m098=Xx_JDuL#VqxBSiIplpLKx#YUiD$iI9|ECXt_USXRDLxk-4`}iR91iCEK*h z-A1o%qO+YWUl$gbR_D(9vX==eS*5V;F!r_$c_yoV#EE1L?`B%gB}GV2VEoa|{ z95>YwEkZ2O67!Co(HP2T$6Fv@ZT5lV4I{q<#SLq2n#5`?oaflsD4ngIGPf(CllG5? zPQDzml$Gh&4XOjL$e8wR10}aow{ptb*xPv1iFy*A0UD0`ce!^891WTOmubMh^HaPF(ZNlji#E~XC? zN7i%XbIg(FSsmG3NBY4y@_Wa^8(CjjS_0Ypxp1JMi%xP1FHKy1J3XME)AqUcbZgju z!IfoE{DNK*+9{qxDqHf0iqphEJ&@~X54bASVaO>x2|2CwZ`S@~T_cr0Id`C4B*b)J z-aslRxDKf_z&9c-L+VFbmb5*o6KNy$1;79Wl|?++$yc! zPNFqfN?m=Rtc#J5Hh>9`c7qv^hexH6QkslbKdI9+_4lbM07J$Dqv_cvi~PmLPR_X}1Wyo}3bA)I!Q3 z%3?XB(Aea-=)9#YXv$JkzNU5vS-BA;z7OQJ%**kF2m1Skw{7p=v0aF^OxiG7u>1~% zmPjgbyeKJ2rODkKS5hh0s*rdx$-K0agwt?uq`|v^a#i(}r}AK+QZHV4^1FiZq*2+D zcnylg>tkU&k0SAUSQyW}NW9J##&atYFVMnxG$R!{AFo*$&owWevldt0r%!%hrg-MpEY_Ak-u3Ju;psm-{~H=qB}dspHG{f~)u$zu+2v#c%i>f8aW9;3jV2Pu#{|xP!luhP$|jblgVSB~KS94W?|SBy2U4g12_@~RvdOJ4E6j#PO#<2h6S zW6LW>4^@mOuNX&OF^0Ti+;~+L22=yhb*egQfKe$GBTUNJ-Kr*DMJ?2ZJQq|KE~p1r z)Q20~;eiJ5L_^5)Ltbc%CTNOgXbx}qpaojO7xJ7@E3`%%w1qzyWkR)sv~_ksM+6`c zK?p_&UK_sFOjlPIMKg6Ox1|SacNPrOoF$jrB zLNZb?7(*}=!!R5pFcPCM8e@=(u^5N(n1G3xgvpqKshEc8n1Pv?h1rmYYUg4e=3@aC zVi6W&36^3RmSY80Vii_n4c1~E)?))UViPuF3$|h#wqpl&Vi$H}5B6do_TvCv$3eV- zH}MwU#vvTW5xj$=IELeR7box@-p2>{5GU~wPT^yGg46gEpWzHX#}_z@FL4fE;cJ}7 z1$={x_!i&cd;EY)xQrk16RzMYe#S4jhF|d;e#alUjvKg%Tlf>V@fYskZ=~Ta?jarb zk%3HPAsac6hG(AR=7DhD|FDG~b|?vZl!61Ef+M7@y$s622~VROp24#y4`)1w3aE%m zs0_Mrs~1oORbfCiyoljWj zj|rHFNtlc&n2Kqbjv1JVS(uGEn2ULsj|EtWMOcg_Sc+_(MP5$20xPi!tFZ=au@398 z0UNOio3RC3u?^d?13R$`yRip*u@C!k0I%a9-oTr93vc5P4&w;k!BHH;alDHYcn|O6 z1AK^+_z0))F+Ra*e2UL-2A|^#oW++ohp+H8&f@~U!9{$F@9;f-z$IM9kN62!a1}q} z7hJ=y_zl0~4_wC$+{7*XiQD)KcknmTa2NNGj{C?!CbE!?97v<3JdCPC3D`mpJCuYy zO2Gk7!4aiV24&%dr%?{ifMFR_c{t-aR6s>kLS;OU7f=ONVE|nb)QhN&8h8mW;}tN( zgL)OUP#bl?a1Y7__27#7aDzKM&;WA1H6(2WFEj>2MDP!Kc>kk0yy1fuXbE5Vp%q$# z;UrXB_~SLSLwj^UM+6`cK?p_&LeUAG5r!`4if-tR9_Wc)2uE*3pbsJug=q9e4EiA! z{V@P>h(`j97>GegL=uvbg25O9hQd(8FdQQ=5~DC0V~~om7>DtgfQgud$(Vwvn1<kbOSbG6Eh$(b61xhyGSWmif*SWI;* zlGfUb+#hppWaY^7??+fMHFuexl5S2B1y_5ldHOEoUi={o~21wcuLdm@k zb?fNjZ2G`DWyoJ5*`c`kdvg4HrIN@LM(HO~^YLy#rb|G4>(zQ>{J)>jL{vi7#OOk1SFly}$+R6|4QS%APf1O+?PNrat$W!FVA8&se zk-sgW0WgvcV+*524M?_%E6rIEVhwQo?ttIj(AzfuDK0t-Cqs{jB1 literal 0 HcmV?d00001 diff --git a/src/hal/components/cros/docs/cROS_manual.pdf b/src/hal/components/cros/docs/cROS_manual.pdf new file mode 100644 index 0000000000000000000000000000000000000000..7ef6c77357b135caa17b9ab63953f1119df268f0 GIT binary patch literal 319161 zcmb5VWmH_v(l9y8O+HzOTbMbSTL=omxqWiAFmZtM%5hgwul!tvZuQXgC60z&4psWyvqK} z{zp%y%lCoKU3H%a&hs|flb2fB`1nK=+cX@lwQ2HN?O2(=~yq&*}+g=~X5165K&OejU!j(fr>?`)K{eGu&VF{%x@GaQ}V!Pape#iJhXg`E zfkDH(uK*A}<^fT_Q2+q2bkKhW{688*N<>|0at*Y9mY4dM4f_(#bX$7duHby;AJiOd z78$sW2v2ji$3x>3iRk#B1=>C6@`pfyA8);(&T&4j-jqLWC^?eA9Jz|M^3m z3j_wURx81}JuN`Ed@a>d|U6Zdbjm6?!egiRdmD%f_y?sl9v!#Qx zy;@$z0^JP&3ip?@V^>YNMd{Oh$+P_J@9hV7VwltGKY1=AN5~n(RtcSO@qCmGjC`)U zXLn5^lLS}3Ag~~k_PKC>dfXkW-j*^gs!78eKEprSSMv*GiakBfP*!w_jwt7R!n8P)Cx@9*x)7tIIO&5I;E4^< zN1<++H?q-#+-}rBWyyPYaTFon(LD)GADh&ItIVSBjDDa^!cJy#WY-}%+C^zu0tijS zmm)O$$FtSHEtUmjbG!yPDd{~OZC~X4+yiX6DVD0e+Oh67GYU@XnG;-1V%I}(b>rhN zM#@3CeZlKla!l}yDosW1iO;eNc%y4J&g#r(saGye1KTCj*KVsKYBLo3g}{^fw49@C zk`djk66`iMWKxlYH3zpeC)Jq1Eq4@3&A^0VU**G%W4L(i%LC`*J1237z)K}LqkGy z!yxA49g6*{g<*R{FSYFRrz-P}yb9=w#bt(psxOdY^u;E4MAfQDAnm1%4UQ)pC7zeA z&O;`lM14J)1Wj%6=pwkchhzsZgzG>7PJd%NK8)0$06oof^08Px_(jpz3 z3m9c)*wdt+jd!J!Vp5)-*o;8$D%s#|-|;c6C@lgC|2eqkTDz`cJw8li{UQZ2Io-cQ z<>mSfHPn=72&-qiB1Mb_w@v!{f!;MQ!NcNKbr|RQ1DgN)0{tQMSW!MiA`lD+4g>rU zjeqAt0>S^vJOBy^6`dIk0}Gp#okIkZgp8D&MOf6>`9l=Ke27ReNZ>nQbEdJPnYPzC z-9-ENkZY2s(|i18!YR+q?-3;gWee5K= zUt&pAKG($Jn)aMelEjvuqQ<`I&Y2L>LuEtpCog|9Bycrut{9whHbJgakMytc^31!u z5ZK=E?Q4IC^qM zuYDDZ29CT{i3LQHB41F8JNKW2!$N&kZoGSa+HyT4!o>NXe*%$i3w;C8%}S>4+Y_>e zRv%o6O>4@U=C0qcm?pzxZ?;We9<{rO;RRXU5|PrfEABpa6BkWF8@90p+=Of8HGpUD zG1tpmCR>FGjAzhg=8Jo>P6nKrIO$!hPO3I4r(Hk!HA-}G{X!S-IpN}=ZvV9E5M4ZP z6TzY>2+X!<3a>wanTPi64mG}Li%j{MlHpp$DRur4pzqT7sMg2bf#V>-(h z53dqW`|=1b5A=ej(v8`aQO?YoztJm!=dZ?fMva<5XB2Yp zJWj3{&n(PPZReqQ2fVc8cwI4&5SB|~hZVsL!Tbx7cYr~gpN|prxDXYC_T!Ij*j9X% z-IsZH0yg5NbzRF*T6rr{X`6Z0efb4s;IXHsmhC~OoK2|*1FQ=dT*Iz?_Z<)H3knAi z+N0vRN21L9=SqDBX_=he49Pw1bA?gr1)E@5EA_>W=}%JyF!d=3^_O1y(G$98u;Kf7TBsDJTEdn|@>bAZVX=IG(!pRO^}I zVX{nXZ;^B$Qbm7*%ibWJl`7L^8S=}WHr*KF@Tt_H&zv6^aut^C7PVp!I>;4_N>OLxG0`f+Nm_W1fNE;wMM%!x`p zG}g`kF|th;Gm_nfTGv9IwrXF+kF!xNGi)|cm(69P{#cWjH$9`%5xo%y zi$JZ}MAF}e(D%=o$sv;D;eN$x=S<1QBA1f1w@%3si*~R74-A$o)xJ6Bw%OQM13tf7 zC)3^mvv-z~C z3N6K55lRfykmeLi`q7IY8PFn~$~&~kx(_WwQs`X0DERDYQ-xf5960Hg>9D1hH%KTm zG#qQ0s^`lN?D%cwYqta%1c5kfq8X7~fUwJi14C8EF~RN&0EFKYi!(kp_)M+v2n#ZM z6X8JzRtv?|tSP*!@kZ8}=??sVx8UeS+t@)xeIS-mSyG*1Z{z9l`~%yoM|kxF|0{1i zUhSmmR0g`|f+PEGJWcxjVp+ev;`~W6?UjI7+7jjG!McPD5OM;mkVFx@jDRPUS*ZIj z?gWy?rpT3>rHRj0JcOlL#C$)eKF7yD(N5ycI@IrZT+b3}#L+>yPq{vQDZ$*$FWW#jFC4zv%oUkL0S0E8S53Efc} z!BUlpzO&GhVZa!KZHA-H=MSMC7%_@cnC()xIDIQC^h3ib#v-w7(5aliDB{RMn$ic3 zTZYzD=NZQRN_qs9(ixW~R^Sh03|Ej=RgE%k7mkIi*C#ab6fZHIY#raZ;rwacnWwL; zO0mA@uhpi<8ur{%n6m9bHJ{y0=7fQ-#Da;X%szVDM2=}Kn8?)U>aE&l|KXfD2DlYo zMm2LPTP-=u^P@zBt|y$mSKvI5n8KRw^NP!1b*%J%4cEKwqnAiK>(3XG6fwuL`@~1( znB5e4{-v3ezQ&v=xHGdb#WQLLn?n84XGPN8!pf-#G7<#+&!7@F21u-Ho6f}O%mMk} z&DQm_{SYeLK!>&>yZdK-bS0{=zFm>??0U*!{2OP5O);XQHKIOKOewU=a!Yh+X_Y|> zulTmSQRLXEdU|tQzLlL9q1%;I#B(m<-okJ04q`N``vFXS&tBS_>+|k(rRBtm7kDtO zge|D<^+;MUE$l|AAy8wWdf16p<9Un-s%ZS5=U<;B;CDv#B7N$2*2L~;Q#mR*KVDYu zjwwcq|5|aKKs-Bf@K#x2o2#e%9OwVD*D*58%y>n;umbDjo@{Qoe%{BMBMqtE6R)vmrFg4 zU4Wt#qEjSI>0V^#ZdD(fv|{`?kF))`YV(T$ zHnfXVTawg<&H*{1)G;_Sgakx596fM*jHOUr`<_oMxcuReYE)-j8jZ^lMO@o*SdCu^ z(^5F0ENPZf-$=O7Jcp^jng9MZuLBi(zMd#yUL9Y=lumGARufRr8d!U4@-Tf~@pj!; zv>_3tkY31g7^|=C1_^(sO;nEioyN8=zL-R__qgb`5MCh>&L>pS5n~=Xe|?&r)N-Ge z7ooZ>QF)!)ph>^eV(dV^FGs1eMjYW4l0B;)yByUTSk0AlYchC+Qip0IBJUmFGp?+` zYVt=ZI>om)70KovpT=e{a*bDiYufdT3jTQT34WnHV@q1#6@nz)S-XeYIC;e6w%9#h zg~WFO9P2mMKs!{SVeTOv`T)nmelkz1dxJbo3&H4LAItK;hNs+QZdP6wTJoA2FC$X> zsU5~+P|APVXMX#cUXr1h5M%mS9It${jD;4?YVx-RiZ$<=HHCAkQjbROB~b|2nVLy_ z#BZ3K!sGe+ROhmT1xpLE+a>+ZCcSmhS7CzuGk%->=_DT69ya*E6E`_U)vsy2ms=g^ zY`5xj$Tsc%y%-I|F(rkwJbUXJbJq2JZoLr>T>F9=;y{n3@6*2~u@#DQH!Yd-D;cs+`! zK5{Ozc49j%`}C}<=sA}WyRAW~*lQ4<$e3^LW^9!zpJ4ly-Tx8T$Q zcC(baOJQT@?W=#Q_)tP0Dt^}lGyxBHRzHEH;m?Ofec#h+fxAVb=hH-g_o2=ox8_Rz z5aRY$n31E*?|^g@Vwly`HhyJu<57=<*h6fVzC(LD7m%Yf_nGh_ zGIUSvidFBbJs}cnZ#+LsJ}k~~b#^9q+}*}s)D%1Uo2K&Sb0CZ@6G~Z%MSY75+u4c! zV?O^pGX@_`<{;l*60&;eX|S$QG-SX!7%(9KY|Qa}+p_T7+u6Mj*7Toma1Y z(o88rPk&wi%#Te`kI9Uue$#)0Z1V$BJeU}rY+-N*e?7N9mYvltuMI|8`ftlM71e?E z3#=;&I2ZEeDJ&@i&qp4fF*1e5S3d`&SBFzWxq0O|<*45QqIJBAN?9v%Ts~E1M+(o= zHujcqd?KGLVERYQ5A$)CH)#2yaQIabL1#_d<}q50HhN2wz4liz8K zgirWkOc}h(_z+?%>}WU(?06w@B|JMiC}$>0fjzM&5gC!55puqb+>nmW8=P}$lq7f3ZCtP^+AUxXxKY!hv;lV9dzp77}!ymLk>gh_mtOdH`4E~9ifvCtPx8jJ{ zl|6E{;11Pn_OJgbl@~&VRYoh>~d^Sw2LoP)-!%?_(WxHOJq5TpOsynv#d8nUh|seAyfhKC!+P?$_%xP z6R&*s)b8|_yB3ZE3Da*&%@2aClfQ{c7Uc;Q$2UZ9EJog+)*%00G$dZFZZ~Sy_R+%L z#OaO8U~8Z7Xc;2yL+J(io3~-8Yt|wx73o7MA}V~Tbaf8P?eRfEHR?vbW_5N$Zhv&e z+W|6MaBUZ~TYsBxqyE$LO$j99z}LHLo2C6I$t0b4fn6mkC$pe%+88)HxySBpgCJ|3 zJONkaK%YbUmWb3oaEhnrD;;Jnl+%tA*tB^;1(Qf~#4Y{~aN11`jeOvtpo}+H_{CCq z|Fq!xhN!NKiS2TcXw(|#)<4a?)JHpRM<)SVp^M6gxk$1hS_~A)Wtm92Gc&Mk)Ik_u zezxQASKrll_y_kBxKN**KoDBDsDCP+VP5B7Ap%Zv85~scL)wS%5&2aa%?i3h>x;73 zvW{FJG&*+fGTjCbR)@EqnpxyCl8EMXHiwUxpl#*niLO$*O$ubFUX(<>xH}N^UBTIK zvl=pVo+zlBv;`NqbU8{pel@L^T#9LGS86m67)p=Vmm@HjI_ve0K{?uh)QLt{v-r3LLLPPPY{pCAwYH!7Ir2K>7t`ZJ8WzPo(-q8ZZnMRB1inn zz_ugCGu1yEza38Tp{Hsf6uoZQIlNO;3Vge)^wu=;9yq=nqJqoKl)Bo_tTsd7BrL4M zMhjcl`8>U)*u*097BF-bn(j(cRK-g}%m$1M~WA-y@HFaf3 z0$MHSyEN(df613oMf7lg^=I;P`-oo6J1zmkgFPyE;r7LYv540vv>gGKTr{cQaC)&i z;4}-I2_oJ&wDIIw{KTf49BPw+PUOrcC+dgNtKqbuaSa>*s~r5Dx$5`3ifkzyoK57J zIy_oFy6@v1QN}F46Xe$PhsokSva-&O6f4bK_prnEkUL1z zr^>saT>Ne=PW%W#V9fGzxZqqw!vz&1CuL6xoi;|!?u6iQDTC=$GNbj;xx-@h*!We6 z43r&sFf{4<_Q_Xhr2t&BJdUBL{^kgsYI1#qq3rE{A6olEy~#|hhW43GBS&gnr7cr@ z*-7_{Dv@f<)+H$%Tv*xUDiuR%WQ}2+4@-0c9qzmC#eG_l)u>gL>cH*Pt*+n&TuxjYl;iXx(FM zKyLO*7VfI;*BldsspTQmJ(2}VM3vj&{^sw1ZDswJuNMQ{_G}#22MyU}pgzL+yVQ3; zIC*rhBcUSbK;3#g@56fBl^l!{!)UxiAyC?=a%;Eco1+<a+b2>a1fXH6uI_}XI z5PV*M*)E4+W*Wzf;OLMM_h(?rI$LihKYYNAv1<5Yuw_wmw-1jf+5eez$|G`ye3LN? zCI(aTtI$=~hdN1%@?q@#xHv(6IL7|v8UsTGkf401k{}b8-~#64x}M9c>Fxg-hJ{Ez z48t679A-#R^eEsQUjgzd*{TD93`S#G*YhtwJ{m)sl7{gJ&$N)gb-JS{zwaGD6w>oA zjvLU23_tTK-%Mr6#O*hNDC{wkz7;5o=L(pTHZ;QdM9`~mW<>=zP*#$GU$_@tz!10k zw>pm_fg6`xTAJch03n;ECx4Ku2^F?Px-Z_|y$S>>8O?q)#xvj=PoaF>DXVZB)~r8e z0bZ$-w}mx=I!>?#htJ&tMGK*pQKR>8=5+FIM8iCB}-B5v6SN zwIvSr>@4F8*`=~Y+;!ABF@Sl+anTuscYw>uL+XkZ@uS;2KwQ)CVuI+Lx91(e@-j1d z-Gy#@jprMWNV~KJo`7SEiE&V?N-gf;tX&J2UnU&J^_7RaQoTLw@MR{qFs^)bJ0;;a z(j8uUz4C%1r!3zLOjH&n6hjEr3A?lC?MSy*H1(pVsH{wm=4n_E!7~{8rxsZ(#*knF zRy@N;UWklv^_0kbt7E~ixx4BU+=2#*Ygy5ma*|pm)#RmEq^2LRKa0517dkH{P`|95q|p@ z_~y?&?S6Cn!*K%jB~#ZuiV-pMQ-8W8MtEjpR)~AY|8yw+u0<`W`_+&6ycL--#d{e;w_wg+N_fY=+Wwk{8@1 zEkj*Z=|m_hF$mC7a&ZTLyMNHC0BUcY>}i}DA}6Dpqo>=nUVw>q9#R?7yB-%ie) zBX4@c?2AIhu(e3@6X{yvASsF-SagA{^r+SFui_HA475UH7+xKLM=YqBT#4syLy2Z~ zrtn)Tc_uBkEoZyg^lj+OTMTXsxSY`m(n8szW2G2Sd@Df?Tx_PPCoe@g-q++MCAX+$ zZCG0Y7|r1*fm-u#g$_Tc>cxuwSrf-2{&Z@{wj_Il-+J#_>p@>2`NNhpiJSiZwy{r` zpt0DfDj+V$xD7$d1nj!!(1x@BIY%9KCP=$`YE&&M@1FH7YHDKLK|UXL6*9#nRT#pz z8bnSW&yWP2q+`~;RW_H)VDVKF0Uu(ULWwue4^921?9eFshc{vX)UEjz{}_h8Bh`>k z9x|OviM)z+T$+uB&rhAaCf^-WINSrhaaKzxYka|}*^pA$LepUL`l_#HEV{5G+X@-FCb1*@c^EL*_#^#=!2!B!iELpb#TWcuM_hYQ zpWU&RA7W0?nsut?=Jt`+fxy} zm&q7?=y9W35Er~C0h1ZOS3R?V)q-;W*&YVn|5sAu+M-UNIz9~kqW)|~_a8Ar`sAuO zCKtbF9jAIsBQ0Jk-KcPeZhB$a5AR|pni=mwjAHe(cpCS8<9plZyw;?#|>CWxVVesMdBJH=v`oU-#dAG;CQHG)!9b|}C*;NZi+_3ua=z+YcA z%FQJI|6c2hjypaE10L9KCVx3G@!yycUwn*B6r2;hWGW1Se&8#gXt@~6f1BXBndGFg zJG*(NQJdsieto%F_5WV=BDl$aQ>Op%?!rUvqWmtSN&e;47l8!7n@NGc9~>3@bAEo~ zq;GdgGhSnSEs@^iSF}KCC>D!eYP#o*x zUy%9kvs+1}_^;Am*gEz+CxI6n?|}b(A%92zuk}MUqd@(zVgL1Td_*n%kCy|L1%*Tf zB=Vt}B^M~y_59bnf$|0kU@#7)@uurt?qcVFm3@)k=4ahT03So1O*L!|IMFSb@un)i z%}YX(M&MBX<%PB;;?^v!SI5wgREzg<%anvR$?$Nv2)l3*ozd@&DckiC5W)8i1X8@T zh1^S=X+69H{N%q$cmtSz>%*rVsAXOnm(%L&GDKCyoR3PxJlMgM{_TzVA{P)-#L|T6 z(J5MYLhjlGW{}%uW?$GJf>99RvG;Wa zAX{UR68*Rl{|}g-q2L-Pji){iazE^mMLh(~lnK2OgcLKNNbeyL;?f$}B$WR_a_WCb z4g`n&Pna4Q0QIk3*Te+{^g;5vp6Tt&E9L)qFi_qBA&q)$9xgAzjhWj=lnF13`Akb& z+mZJHlLuWEU6W_9(I;AKj-`jq`HoIF6pO=u;=K%$FzrtFS=sM1BwU0DOyUxSU$dWI zKuU3fZBK)h(j8i~nRM~hY2xaK0--vPeRBCLa%Voo6>y#zOK>EG$)#NvmA36K;Iopsb&k!ZNEwK;X@ znqphN36T9>ldGHqodz@h-1B>pSm139xDVEqrgRLf&z`xO4k28ny#dD?Aav(vRq zV=2G!-P`>c78s{S+gJnW$9}U^^Z~nKB5G2nq-f=R zONEG`GfIV`i`v|)ixul9cGF>v{uqsf+RD$bB%ddg#2(tMZ(V2@avbf(+)XZMnlV4D zjF}=1@Nm=1+iQT^qk$bPxegbDC`~(+w|*`Tx0fjUyI_q;YzWb&j<3LoKX1KGM7679 z-};fXuumA|jQ;%NZJ_&zANm{X`4L3)PxR17cq9M?6-2_qtYRYKQg9jkaX;J^o;G&= zAFl$K&<9Tn_F$8Jfnf*@%GNir*aN5jDhoh!(GGzDuiQ_k)uxagB2@=A{+BoRp*oCRQ*!(JogML?jjIQ9@*1Z$LP;U^CE zilYbuqk>_fq8w823W6mq6cv+(VHTwi(p6?g5fBMc`Q!~PMvlhXJ;4jI7aIqMrw9cA zluCocJOM}${!|i#o#Dif#!e@rbZj)G^}vizrhu5uIRqRjRG>nb2Pw`R%;alwH@55y zbz0odK4@H*3|!}LDDH-!5K0$7Ki4s!Ws9?1vbzAo7zhLcNTEbl#l%9Lk-(&xMEa3y zgR;Q03IRd%jH}dO!LrH#P7sJS=`y{-D5n=40s_OsxhCagva|R^QLq+JD^_OpoD*X&%y@W&rpiNLhEO(9%_4Y9_Kk%BcW05Svl@B?Szf-AJu(ibNlG$lVEDjHt6< zqx~bCcvI9|1za9y#kN2iceWxDO4wr*H69{5EjuhAC3b{H8HhW*7KH1(Fctye4xy3&NFJ!noIw;9 zvBjpY7!-{8Lqr~hO1GXH(m+@^qK_FMCWe}{TL~iQkupsfz;>=V?c z523}3K;a|-ETJa_WdOv`fR7%KH~_eQk=X?#O%(ZV5Vhd3g9>4`yWY0)R-y%mGhy=r{CG zY1CHejkph-Nh``rL=R`OBiv&E){KIMx52rzr+B;o5V%0dM-T+C9?ux-2!;apD8l_( z$;;2c;01!%iag+u7D<7Eg1LZ6qSP0GLNS!`hCxvBVB-M`YL~u)C9?ZG01=FJ5d_D< zNZAblHj2xIK>*6tR*e}&dPu?Hy5X{BNT4FszXJZKGD84iz^K9aj{($pAyW|Y!8`3J z)p?Lb78oM4(qVDp64XJC*i3ibL2=!|@5}#7HUj5=GY|fq{=xOp^Z%9pv92eVxXOvr zd8&0pU~JGy0~Qf{XNKGXsr@ywu$hw~Mj`lMrpw5cn>-SEBqq7iv&fiGZqH9_oGS-c zg?;tvs%v{QIsTXbY{N#hi>e=2ywMv@z!UQuTkU!={jAyRdxj7+-^Cnt`qJ_=4pr~G zB5%vpY-BLqZjxyBKA*xWMDw|=^{y-r!VGo1GkA|AgPc0f_dkc9i1Q;ykQZ9>T>aks zUyet%Gq0XzYkUWhkxeK2MSgz?t2x;cWEWtG+{_CkVw~(7A}V)@PzKXT(1(~K%U&88 zqxG=F=4|*5zS)O4@v_gB{aLur;a>R(*`I*utn*KcF%`#nTZ`Fu0c?4<=r=KEtpoaVyX==Xhv zj)8=Ntg$OR^q=9a=i+kh=rWro#|I+D5?;}{m!!8@qzf^=Kk2tZg{{mVN-oY*pY3uB z>r9;C-R2n@f72H^c)*W(jqbB7ibZ~J6s;AohvD>u_HfESB6b>v$IEuO15z|=ctICD z^b(QZ$bnh)9;nr!q-p7i1t;Ybj7EwF(605Y7v5y<+CsMMv8Hz2ZTPplNoiBHw``t< z^|4lB-869s4uu=ZK+`hAE}?9w9UZc($_wwh7M2cB=C&NrgQKWf5tSV%sy4o7-Wq|l;;T;hHtD|J4xb~oVK@` z1N$p%Y%4N&8%t<@k*bmZCL}6Z&8vjGVPb6q_wpQaX#eq$&|W1-0l86W3t!-bVLL#! z1<2y|VbzF&e1VZW`WjUIRgSf%K~7mqmVz6n~%<=bu`CB zL4jnVM>>E@xREH6E}gySGhOjID_Hu*)wx$^ZgLk7Lk*g;UbeVT4WlBhMYb~@B}r$P zrVHtT{EzlWYX6`k;kT)w_#6$*m^cFi=ahw?Hh{taCzZ8(bjsXY4BRN286nM9QQ=c6!=Vr2z)beF>EuacbgcUuE4Usu zyk030UPCO-?xP9Nemm2HmO>B z??ADrO=(m7$ptDa&U*saY=%7;cr{hksD!%k!)zrh7rGqU6(F)&8K5ej)^>VnkPUs$ zUhyG$)t*GFuE(D=-oE>-eHcm!>(Cs^7`y%4+=yCv1j3XnrGVk4dxW6dundx2nST^W zTPi^nCeG)^id?H?3-%bN!5Vb=gSyG$&)*&`TQEqBnp#1#YR^VNv*~-&yrPHci;b8m z*h=JjCXCWXK~pW)Y6?;){8_fx*Qw{y;2o9Iq7YSQ*`S`mCxkedj;Uvu{5siFGC!Il zJyP>p7J=+KVqIcPWqwtO_ z^k)feYG{x(^>F*1>7J~peQau>Gj2I+=}61s4*yceudV^*8Iam}jp~Rjb!^FZnQz&p zg;}(UjN`NHr=w9iL&oiSNu#_DKO{>lnp*S7_DwWWlrimKQS7DVQhY$gP_&n_nD8os zx~x{K;ShN}V-TilX_aC+2(s*78kcES^7_XL<2qCYe^@xETqC(-D$q8Gj{C&dvY8n) zV1nbFuqLNbgl7$y<5bCxrG)EJRnRp!$iq0yOK6O(xDR?&ECT#=PkrLa_`pM;A}DKVr>=dcLD!jysij2{7USBxSa+k@cVqpjh{ z<0}c79Lsx{MR3oTu4Eo+X{NlD--5Z;m>~4y_#>!EF4^TzZeV`47Qi@A5Zdhr_s$Mr ziKqMOvOEM%w+%GRXd2_hyp`=m>MAO|<(S>;x_(a64ze_na(%E8ygcbS|K)*Li@*n- z)zD$Y{Y$xLIA!rvGR4hhC!`EOp%>c3ny3%e_7lJU5%(7xA3fC+UKv<2a3RrUmO%$+ z0wJ=(ds#KoXf6!iE7e8e=_AuAK~t#tHx}u2y|qI!`^}WDsLT2L+EjuIs{Z~aZdhVN z+L5a_G|=BW+z7u}{9A6a935bGjaD_CB$*&f6-|E z>M#G~SF&(({U^ba=l`YVurU8$=sDkX$EtTZ5!N?rhFgVIJM71VQ9vzeP@ON8=8GpOxtZ%0&gkZ^@}`Mib2q?J+Vjje8IaKQAux zA7j&%n0thf9V3Y8bqm6o-}u@Oz4@3Ne?V7JuXDJXHFrjKg@r>-psyd*+$gOWUs&<9 z9Xh$j4|;`ux2}m`>Mz*})V1i%=h9zIs|a~*q^)E&nvcDI&ciLS>Z%)Q8^(|Sh}KjT zOPs9esQfmxD{IXB#d7Yn(cN%}L#hCr@2QwV^JSjGh;(0jQBba9+W`9;W;T*_WNs zAN3StCRl-jj6cG?BJ>D%X!01{x}!1!NgR7R`Fh7)@OfnfZ*xHR@+F{X=3U$-nlT*Bsc7a5>@l{Rl5 zVl%V5m5jy^<+2ve+1f~2;;VlNucMGV?qd;J@R+%|x&|zGCLn8Cucg+HA%oa06-Ix= zr?aI9h5ZB{6`s)eHnKzx^k@9+>GfMwpf@`>c^yA_0yfkQ{da865gCQpy0dzUT|C3L z;zmd%)bpO zv+^`$!3xPmNP=ew>DuWCXH z^sC4uQ6X)MzPSr+LCt;RZDVFA#~JRmi9=cnjTUbWM#R@hpvfS8rLpQBV&OMS5v|W0 zs4XKYE5`X~a6cc&+|(PgUAw$qJD^PMIYK%NExJPF8%pV<2NqP=eTuYFI7Ark$BJ^S zf{b6|Ds=fERT<0uBU2x8eVxf%5&c+li%k^RVpzXQ?XJ3ZaZtOn*-t!H;}jtqIb3!S zJRCr!j5c23)EQYPB?uqD(FP@IwJ>0r&U}Sv@p)=x;@0$3Pthi_FRr`hz3qb=% z@+hzd%^#**dA4Rx4G}suh^7IHb+kWWyfl?zsV2wAt6ZYP&m~VVaJ^YKe!AsV&;nta z;D;;HEe!@+p4~6I&!tDm`#@gzHlfim^2_^ar0ViFCtNtU6()o`c6L@ZFslLVW*Cs~XrX~)>8&g%feO5hV z`1K5O7mw2QatAgWI~NH%l6uo(&onS6xS4GsX?DLpqV`L_iK|0w(*z4owfh|Dl|E!f zAkg+d6j(35EHJ4H^NCH0Z8OmAEESn5jtjF)g?DfHX<)HuUu3bIcui=Pwp4u^(haSp zaQFHoC6ALO`30jcD8BUP=s~)q!ese)73GGCU7`VFNxACz4e)e5!%vcvK)xU`&%VcR zG`qiKkHNVTaoP<|NzCtZzOM=u_k5UKrm5U%8vLhP2vK#pcA|f!)-r7@ZUvNO^d!?I z#|)4Zn#NkGGNI1WrzgWaFxZ^`XYhNyIx?Lm#)>9Mm0?Q}4R@_?F{XW>dw<{5y4=1! zJ+=dWY}|(cMYKhl3G-l;&S06DBMMCqTi>w`&=OhP7d&simf3*K)o~%aHtU)!DbF6V z7MW;6W6xhd?@dOXqg}6o_qE2{VeW)y5G{4HC|6I2aZm_K9}hc7n40pb{@H8|K>T9G3?uwc*4EbbF`+coemj$mckL8|3K`Ab$t%xIkP7mJ&6qlKHB z0LWpBSgM@k_h<1_xze5 zgwiA!w$o2PXezZr=doi8mPy!AD`#M(I*iTFSEslIAlmYr(Z|UV00VyJIOgi8A8>l+12RF z|6%MMV?^ziwcWOD+qQeQZQIst+qP}nwmsXnZCl@b*E#!JC)q1m=kJq|N=E9(bB(II z?(%ZeHuNmjQyUQEYOPxOJ_N)ry$uK_f4K$>VzM$jNqT#e?%+M-WV(qApkO&Hhxdi= zXZQNNI9 z^;kzkF1{bo+<_yb>ZAuBUxIs2!w-NL$#^3VD2t8FBFeZSkM=@opJo4g`RzMuHNFjL zeYorf5Q&m)^!w7p>fF)?gco*09acsgUo0C^ozgwZsg|uA=o?gX@a|SAZD^i8WUcHA+>&yGpDGJW6<6N?V+S_@S88tI| z%?o|_lu-1G2G{Z2juPHi{@Y8}7IA4Y-Ofw|Q`W5akW{{U_OqOpHb8#5=<|D`s@|7}jf`2WY9 zWa)2t>Yxp==R@tai=W#|3J3;x!N}DQ)FDk8*b~A*iY;=Oga5g~m+y&4BZ)%m`tgwn zqpvz4LS$J@q~TS<2+rl_wW-zg=lx+PW(3RC!$IsohJ*L5Fq|B^_Y8z@@a(3R@0zZ*lb+de z8ZLB|(Us-k>}lmU^hpP;J^-?_$*;X!vxjuBej%<0msSPOYW>GG$7QCxncj2IZa`d^N4-&_VFB3k7#K-&UAMRO3ex0LODg;1r4euTHkrv+2Os%0<5{f1`++DY-h zh6{O`(E&@ZuIR*Di4#V$LP&|)1GxgA5TJQiIoD_&+3~)DI9Z@8aAoVa3ki0d`%+%L zxi|p@*9X0;8kNFnlAGY;%(H^W!gkS=ghHZm_>I5Zx)XOwo+AcqCXSwaqW46=O8S)<>~JeS^a)Y6}(bGA_AZb^abj@M=q zJ}oqDCXi$79e02b+qEc8ZC1-HO6#riw1-W6Ya*0F05`esoTJPKO#lx!Mee@Olf*a| zr#+W!1VmUoy7@)4styIxl_FD%D@+}DmTU?(1HRnL@@7wLqa(elDUWTwROXb{jYVP) zGaugcxeEl#!B)W4S`Rf{$kC1Qx9J}T(`|}>ZUjFOU9^(aRZ=U|mem3|(YLyILsUZn1*$mH#^(VEgEk`0b9f=@IeDl za_CGtO<%Ppt$&-4y^LG3BHmM)BkNod9Vq4dsXG{YyU1k>g%jHR>2dwQ-^;6R0EMEb0+Bo5kwiUW}#+))Ho-DrPkXma$+$2ODO-$cy4M4in_-(dHh@xhe zusS(sT)85uI7jj~xCciD{@G%QdCglFZ3cCmQz7Sot*XqRc;KVYtYQ>}%w-$Cmv_qp zrMXPXn5lCXX&AZ|DffzKL8&!+teAvt*zqW*Gd%&jchAZz8Ho(bR+2K`wC57Ml#s(E ze->JTIh!gT|JR4px3dO67;xzlA{?k~gm6x(!+u9*fVNK{ zR}BH2Qhob0RmWB9mg($v;{>VlvO0xl!X0#30PnDuq}lJ`k3wu&I2TW67=NL0Oi6Iz z=!&}2U&%;@n&dtnyI`8#4W;gp;n`;>CFSkYn(G6=2)(|eQhDCOVeEicdTfAQntOqk zw0F+U8}IODRO(y>g9hz>F%M7X7Tkj95x{SA{Fd=c5ZUFr=(rk4B;C7si`0?nr-!!y zbhWv7n#)Bpl)@l{6wTxnxqErqRFhnOZC+x8O4)PHj&|NX1E2%Eh2FVII(+*`i6tf+ zJNs#$N9tr;{w2c})pt_w_$9@9ApJkPxFMS^W>Ln{0iR>#{42i;1_;eyQep4IkS7kB z*&F`8^Qx4XN8oYlrq{0)>qTqD<4@6zS%05^3v>oVg`|ifq_8ZTzP>}l^`970EfM~H zMDrMLHsn6~WKa^Oyek+=NWPl82w(<2OXvkGQo+_SwLIE1rS3sRsb2;ZNpKEsA}>43 ztH8LAF=#y;Mfr0J&Cq!zaZa=zsSHWz)<*Z&%3(ntYnVR)QCkCPGPxvAyON5%97vjk z5;~h%^S+SIqkxLX5jK}q<4-b8>2Cu7m;|4*HOC*8?f- zpa|`_;%3ZR*<%4ET_lm}CAQF>ybQZ(ZXjY4B8o+pDCPa0?S(mR|u-k;1fSLWU~AVs!CGgY7p?NI&0#+*5(S{tprCfwp_Qxy7E z8tY_?xY#bs%>rOVLHD@d#TIpG4O|;R@rf7mZ z;g% zptCx}V_PCm2CH51wxex^y7g?pcIVxpZG@a1JwY#c^B2)?tQ;`;GZ!v}L_l5~V&yLB zrLKYmspRy`Xl?cke%mDGh<@q>tR1p1&-&vmF1Er}duzmhH=H^?R!+*CiM9G#@ z?v7x23n5g@DoA8X)eup33Ra^@n9p*I{Ed%6G`uZM%fimn`(T` zh#kE7V2Y`AUq#2@GU@z$-gU~+cWhtyMh9oT4{x<9QDFPg31cWjA{QacWN2Ab!D)u1 zb6&Nfl{O^HW(U}v2c;9`3z0xKm&8~|!#pOJe|Rj;gq?y~k16h6YCL}1QONp1qDG0R zkzg4-o<@Y&ukgHIcAl@TX*yLhyU44YPzloU+n$e&mA{~>Es|W2}juOb!;M#=OHP( zT_;X0+qyA%$ds&o^}IQrYJAwGHu{la{3%}6hNyknYg^c%TEHRp@Xy%4EpKdA()DqP*r$b+uBoIRoW8-Ft5{`gX?rC2l;<`iSG z_9xt-ZP)I9oA3eKxV+~ufPbwzIXEf3xqSAnqkNM7C+z;eW^^1Z4FAS%*8g2C{ijd- z-@3xXno_ZcEr>lYwXivZE52EO2ib4ww+DZGzD~~Gt_}zM z*WMfPx4(aV?y11%g|z1Wcwp+)+F_>7MboP}W1iF%TI1yuY&qlYYl^ z>ZVtAjs@T`G3*PN-0G|AgtzB(a?OB<^KRO zmZvW#b}O$?4U5_CMahAjv6C*I-fO*&PJqR;rc0DyYee0zUhHi9@NDhK@>M6vU6ss`kOOz_jVj<1=FEZFIy$3{Wv;moh zE>2ZVr`)80VQ@+cm@6=V*qrrIo$7Po{@x%c7u_Lr1MJ=7hLX+&lq<z9TIq!5~nN}m#3m2U(6fD+Rsv`%ju?p#5HL@b(hGyJl75dd;;U&;08v3 zGut!dpP~}FOUOv;CH2_Kk9|sI%q(Iy4|-DABM&}TdBVu`O!YIRp^8m7PPFGo86S&k z7txaQr${BjafXlC+0L0u#DoMysWR!1YSDo)p4UIz!znLrP7BP`PJ}8u{VM<)Ra>c{ zjIt&giRKi5%WRp5J#^jABsQ$K(ujK);Sa={HYJAOl6pfJjb? z86;mVj?cyN5$^cTTIJe>6xN4C33&u{pZNOebe%CQ-Z$b44vKC2_&KbN5gE6|B|DM!(%SU1rNG&Jroh z*WR8QIujkqW5OtGy}+2*^AIdL%Er3$qy~Mg=Qw8?T7B91MkXl=>~#dAJNH5gADx9V zRFvh3%s-Kkj>_$2s^Clz-k%#f%^9zGXPI3cuiusxtgl?Lu#G!5fI;G8fuL;Kyv~qN)v_O`vJ{@N^3JYxxVG?!3!zxoBlS zELKRO+!uvNVePG}?0&H%^CO6ywT=AlShwr0V6(K01fRmm9Ik+)EG&sh`2*|9o8H8p zku5>-g?MRQabD|m%w<&02A-^d@Bn$NP!DM3l%KBd;$73ObbnO5?eRHBXkDaKY__df zVF#7n2BpNd*4{dKiLg4y;bxvsti>>+?Cxz76&io%-9spiM{w z>e4b##U&4HMXWCF;pLs6?$hjI9AW2~&VE<8q3KMk*h-H9_c@LSAwKp6fh?ZcqERi~ z?yXnTn9@hh;S^QkrKh6f=UDuEEiOUN)>*eZ!dznYZVY(@Q8peuTZ0iV6Ufw*P{a^0 zYWtm%L=mU%F#@HB1xa(58CkOPxrAyW(^435lk=|;!-tYW42#ioyEQKbzoe9b#%0oP z`vF@hF^mDMuN4hwgx}eunjaGEJUX03C($ec=ba56HHR z&(=(e!QWIIN?FJpmG0veJKMw89@`rNR@tgLG!)!JR{Lg7^fu+fryw_rcnof&4gBdp= zX$3Lts6ME~8k7y~ZrSKx5XMUEyrUEXFMC@i#H(K?Xc6mA^T|mdWBpY~>WZ8V)?TPA zibxzGo4&H8wua0y@L589z3*VZ`V;#L)R3mz3^vT|u-qT|`LfA%#rLmj=zQGU!k}?54C?_P)RAN%qUaux3oQ*z%o$ z)Z-Psz@JHt+m+WIQs8p)%uSA@eU<3xFfs`cdyy)?QX7MyKtZ0D1OJjdD*KL8sRED> za1e2kew#vL1v4#c7}-?`uvXb0`J1z7jYjpk8d~afho|(R&#hptGV^DXUmk91q+6ko z%jzu8)DK`b^w+@w2t;UN)plrG#tc~%nPQ{Sn~5#n zpsv^7_uZLbqb}rB?b!(bQ=#S@-HxCWU&WGHMbZu3qCfNHgxBM3jbsb2L`^?L-J|yF zE`8PHdSXx!;x*Z`BQaD#GOmE&Ry#FYpe1Mu^N>J-XJ&~i7iF+20}i5ioX2Hn z3e*gi7_s%8DDVR$qpyVKOG);|x4)@9ed@D+w3g7vx-{~P-|F+H5p)?R32Fys&iwSLC`wQ+ z_E~*lf;m{|NI3selsK;{{mJ&pT`bz2Aopcca(c%p<@oUtCndik~8(X#qVqL&PMXf3K)IT`}s@LO^Iv^ zEA;3#DYZX2czUI|p|Du9wJSeg1;5W78PSiL2l!0i`usiU-V3g=N*{GVagJ=Yjk7xF z@V^T#?->uK{WAafXd5rxBO-`n_Ay0QW<9%ab$RWo4>QI0n?jt|WW_Kzu0Z;nnDnI8 zx%t*xq%RF`BDm^Wmpj}?sbTq~liQjUrfY*XHoS&bSdfkX^4)oe~d!~xRL>_ZURGo-Q3$CWjV6x5i#&NMWE|&c9P|c3aA7E%u!D*R+2#vjR!`r zhWcS4t5*#{Ay%q6FH1^IGc#P$4W}t$pSkacGbuUp_n_J9pfmvE@8~JYHof1$DMv0= z)P=>FU@23ou3oT)eXw1+uco?UQD6la8rUEekKqLgZ^dY8Jd1LknWZ zRRvpON{`l46DdAmop5p4g5eF%d?=*7YTm)Kx>8>8w)y!4*amy|@m{XChryQ5gusKd zkLzoYWC@Q(o=Q@g08*&wuBJzr%q~Ep9xUJyv((*#t~$g{0&kqj!|w#TrzkseoX2*^ z5_-ykmGFVz_#l+(bxc`iuKhE)PVutpw+^WYac}@mlng65@VX~nEOasNSDuKlr_Udw z_Q0p$AV*cBkT!T zlY2*x9~?3azSQRs7E~mk$sMrcaWl*QizY*7V_dYHj_0X;taW%qEI-*UtXnCUIUAl0 z3RWsN(oiocs|=tw85yHZ6bU8`W|d+oxbzqLYYT&&Aji;l`OMigBzAB}@eF7^J%p@u z7(g~hf4{cGfWJ-FtwIH~oHyN_jf8`u{Xan)qmh686I9AjFCmM6ikDs1G!<|ueOl~8 z)HW7JC&M^k zKRWjWOg{&qYixg{EDJcjBOh5RC#s^ugD)`xbtUSx zI1nwNGlqGiBkm)M62({+ML3(Sm47qt{WWWYLqVL`@^qU3FUuvC340p54yf=HmL?}% zv$?t*`rrnRi7UubO|Ks7i1qPjWitb=q6d>7ta_v|Q-d8z!#1FQMYm9;8d3iOxyVES z4=mmZ1*~e*reIg>Xn7Qsz5NpCIs3xcr&0zzU}c$)Fs;Rj%1Wq2H>ME{0lx~1$*3{Z zHTX0@!Sx8^Sx~>ybAUw3a31@sM^M#fo3A{RVV<2O?Va}4jWIX&u((}G%b<&JfMqsQ z-?WJ_F$5ans&4@3GnPz$4x|sqiw%Myat=pYleNcoBlWEtGQmx+?j?b`wQEb5H@7>L zDiD5MR+jl>v;T(2`CW(Y5cVDZ_tYOp0H3RQY@4HdD!rGRUp|=CL&Du1I-h_C)i39= zL*!F<*iFY80nM@7P}-~v?G^^DCnl5U-e;x?j+(6bzXX6E{84S5vN~`HSuX)4oWbpN zD61tO)8`JP;SZiN_B%ZJphHY}ET<|m@PD_{rrx8ju8P`3k0>B)S6_4XL$E^Z7xQ~{ z5`8IwDpsdpf|)c6*?aL_$W~u3Mv!|LVuKH~NU!fRHzCWA5 zz|=ZQQ9D`_pVb$uRM|b@giw<)^{qM-#gOuE$(zQ&kUq^0mtb=usx6PP)y8S|&VSoY z-0kCn3^xUiuqN^^Xp}3ty^{wb$Hoo8E^@Prd(Ke7T!gYOFTHa3vp#5CrNZXZnx;=i zfG2jN`nrKm+WNFY)l!87YDL`mW)ISXt!LXilIUW$%;#ytR!6a{EoCBfJ+}ETDo!tG zJFqo8VFzr?>yr{B-HR+zekghe_O5gV6K0#k+CP$U`~lCd`-2;bv$T@@DJ!FQFQZUX zkAHp|b0xQXWA>ItqDQS|F^#U%{>kT0n&bPQaQ1)1uS{(0|JGJ8G5#-{W%@^M@!xRv zZ;`i>njL97S8wj3@2*Tzk*^QKx)m2a6Ud8*#yb{={xA&`8DZ3y(`x3BouFv4o=i}^3Z^!A!ac8b~$H%?Xt1G%oziz?c z{Fl$=w+lX(-}S*sA=V-cfTRpd$F2I79DZ-l*01dz`WOJ{H_Q&;B4!^Bs><3@S1I3o zz9%uRBaeZ?`BK*m^&@gSWjBw~d_5^F&% zm_Rz8Di#dqT{a*d(ypyC1l_7;U~AcAD@QDl#mQ}Qx*$tvj5#73U~?9<#4kX|X$F>= z^phmJ(p{LNMKp=1@JTzdgZ*4j>2MyUzZ`fuKaUT`VEjc2$>f1W>|ViKN;wXeG-E=}t3J&pQm-zIK@mpHQJMWRsT~2+xB#$< z8A)D!8(Rlz#BfpKcageoB~iWZO0`a5!P#)9@%K1VTCmUogNnMO$U7nSnV`C=vcwO$F+?{)DE(CM?T@RTzyGtQyU2o(v$3N;H*Q>i z`vcB2{4_IeEPP<*?yyBWcYZD7{^&+Qyi>ws|JCR^l$-MCiCET+R!+b@yn5I`lF4(>4WROzs+D}bY8F^L-uTW&sj z+yf>R5XxY<1kU_Ng`TS%$kEBtC75N$*ugaHv)P=MS79hiwSB#Bs5p;7LnEH$7alpS zCTdXXd1qaZX~MixLPO#mhUmE$O>~Gzn6Dyv^=o zNt59~tq>C`QbJQx4YJOPpp}&#Icgs*Z_%=ca=j)ek*TZ`I}8aG(KvB7mWp#tP8YJE z&A`E-AE>M9UcH8qBfDpE^vp0G4)g%!hsj@DPjF$*P34*CgI_E^e=aVl&b;)+{j9@s z5kSyt9P2WjHWBd~ttCWE%ILw86q&|)L85J8g_B>&3Wl~lSLVnPbY)E4c&*Z<+zQmC5CJ@ zr)vI{XaWVdDG?Rq0`N7`xe#iRsn$EL%OdhZI8}VJnRnnj#2^bS(Akoe_nj&g%kVZo zJ`@uuJ;qg$u9ud5SNt)vF|*Uf8T@&mwLE1|w{?T-vNM;c*SR7Xl{(Hf4ro4E4MmaYCBdFby zL=A0i8IlvdaROa1OF@*)cUj*tcF6CfxJ^qZh43k=GPXVtvo`%$DDZbH`GYXz9K^c- zwXCx>*qg^s803*bX0Mg(dp(I zxQsfy;=7j1w|i!KK%;UxGu@oWr0lwHk9}HKk8&n#J36nYQ<%Xq`N@EggHr_MhIK=- ziT!I_@khyo3)NLuQwBMTn0k-1!r@Q$N~mtt%n@_&nRP?hff9UAz$VS~cISz2UiELq z0-1vk5Zs!LJW!vlU*!l(?x#i_{@a0wKT~GEH8#6P!Suu%k_F`*RLO zA_;`jGu$rAo*adhW74x(HN}H<1$K{d zBh;)zCK?}#O}cQrg^?TT(zctd_>9vA1mk{ z-F{Epeqm$#!QJTjb}+Wr-H9#-*JERb2PIn@>+Xu;L3bmzXp?ots6DaP{P=aFf|YAc zfG)~QPR*)0i)DB^weP0JNAoYL_#-=DEQzGuGCu7~!1w=MB7i3|bz37pF&oR5GI83w zT5dnR#FPKMB-dRYd-}B44M<*O%OMM|mpV~}=fS*xb{G75{3q;1jW{)MIJ{7ddB299 zulMJ+p}W_s+|pmJtU(zX6h|^krZ%MB&?63_ia|=%b&^T7Ic_mW=fi{8 zRox$87eJOwc^fLq;huo`n`w2|#{H+8jTqP^yK;boh1GmuSBkc2jG+?Z%Nh98Xn2?F zD5@B%5Vd&^wQ)Fttzt2+F9XOpxtk~ev}}|zhwo;`TG+Z$EY@(c3Wz5aNA)+-AZH+k zI`V!_9qGoYTj9gNSTqvs6mF<6G!=S7;5!M3kLBVUK%xSbxsT!@<@X%Et0O4pV_!P` zWype}$*U?N;_sVa#=3O0RW4qY=Y3)>mh=GoJ?y$o%yN()38)j|7Y=o?>(S4WB_-Tn+h zW?x>-9SVCtR9cHF7!fqnE|iOh*;HN}5Fv{&D%2tUY?888x5TqK-GtUF)WBRMDejGe1}S__Z4Yg<#FLfHB{uR3q+5ijjwT!)G^>`7=uX{mjB1wQLvgrxeZkl znlaM){g7?&+o*<~i{J0gxtxm+~AYG8WAOd)34U2d21=xV-L#2hC@}+WpFH43-`1&U@FE5f|T|Kym3`Y z(H!9Iw0+j`=v_T0on#t_~9l3Ex96L$fnIt4^ZheJ5Ky2y>3 zQIakQz)~Jl3z?aq1K%ArF7r~es815)Y;dV^^Q9FP_|zf@^O?F9!M5`8R^s#n1uHsU zEn^X&H>M6@F^(H`)sKd!vlR+|s-Yb(#jemxp5*>hBRb2`#aChITAa-F^nlcz((>%z zo|Vs#oD<|`!{9_gK8^(bFxz=F56PtZtJ9Y}BqsyfE$AE|aRsi${q<(yC{VS3#*vsz z5!nfUA@~3}SJU)dqFyeSoPr8;VUn0k0!SI=MFVja#KOc7UbX;5EPBvFyZ85Iz`hm+ zvxw3$?p89>S5`PiQ;}g(A5@E+hfJzJ8x@4rDXE%f>qhmZiq=khGTuTB1@Kh1pfLnn zl_D29R79LwTk_=B8z!_<{Sa3DxvQ{6Mc-2T>5iPKKhM2NZ({0gDn&}KS|+M9E$!E5 zn4snh6kz;D(8J1{K8ONL<`Jg1WdxfusBi`Un%+vAc8Q}$p1BXlt%|q{t&`Mr*G$CO zvt$!pPg78_jzF6mf_b3&9QnJIs~{9RBr8MRX0z!%3c&X6i4%qmb-^x19()%LnjGymxf>p17DEL8rZdz-bw2603Y z(>k@f@vv>?`e5hN=R6%C2azij?-1Xi18c8m5#yz`X~m86i7zW zR&XN~UZZVGifo1?J%l?wPfrBmyjC`c+8L*)CT8*My!>?ZMjAXHj0r<$$wGNLkIwPk-LF$=Ua zEym6A>2Hp(@0#P>&!^Hw+-ApeJj37;o8_g@(8Yq$mbQv%BJ~D;Y>J#|4A(c%HG=LP zlDw6vjiZr5$*5|~jXHFGE&)TEV78NOx$T(17?9~5Rz38275H6cDPL1B?Vn4z3Kyq2iV-4FZD~I;fijIL6IQPPPlX|~_on@r-sOAkN|E^Vu-|$skjJ=O zjD8Rjxi8n%pi+-Qq-?7Ju7l31(c-|l7E#foT}t2l4Ins|R_pq*rm>^On`u-!#3HeVq3zcSNM19?XSacwU%pAKuWy$j;#+pV7bIynw^$}lJ^5=k;}R8i z?e}ULrkI@v--jFSnW3Z}$rhg~n{cMK>5*kU5G!~GMeO}F7Tl*24&uLz8S0nMZJnMH zzGC|d!9DyZ2KxuC{-1#*7RLYEBWC)a;q!lQQ)c?7ZT#OD?CP&_^6#8^&rI!DJHMg= z`~+A4fPEH@hnBt{Uc~l0fCt5F_v1|}%2N{w_geNZxM4$b<`$Z}yZBFWv8>(OlMQ@u zdxY1_^hpUkQP|SE+qaX}n_kY(>6hD`6TO||msW{gTC0);C%2!o+bbble!Oj8&cLp& zpJx|e-;X8_fFO&ipFNu4BM7tnVIbE}3wpbd4UwGSJ$P4vS=)mn^Sg$&d3@*^y~V5h zgAdoKg~cE!b>2LtYaZ-FWZt(Z+T1Xt0e&P8OnGtFo(>#vq+@}73x?FfN9eo#u>CFj z@Q=V>i^)F)ONK~A7&omT)k$aN1wlON$yEFmVVGzL0Cu4MCp1Vu3GCs${ve%Zmp=lS@LID8F-v{YHUmm z%3Z65z8f51G20|b$vK~9;q0*PPZUWE3Cf1Y8~vgU$W0g`Sf9wA z5t>k~qU)OivnPeOc`oCfGulFVZBq+5SMUytsb4ZlbUL8wY`mDC1Ae_0o%SiySMaxaKZg?FXuRx3y`AVhCvY~F zm*0z4?zB0tATm$|O@`J6SAzmm_4ARu2p4aE1-CMrq^xSKVu}25T`^CNhd*}StJ_lQ zJ{pxjGQ_&`yy};ClPrX1sMDaqjU1VMRC0(5LJH=T>a+@yYRT4P4g*Xdh|9FLqbP*7 z0PgX-XbEB(46-PBOv#Agm-Y$N*63d@BHe2ubU6G&O&4`jx%zzEb>c>?fX%Gk_eLeA zyf~PTQp9L~sNnP}}?{2myxEN@9tcL`&^s znX)v@sg~Q70k)V(jas&0kFpgBevcUC2)vh=H>87FWJ$Ec1dUQ%91T8I9~-hE{`9qt z;D%Af8S-&ANy>B}m(wzb)^X$a>;+Z*AUKMmOYLqKSGQDIjFw@aFn^eX(^_#+3{>@l z;6q-1{?2cc2XQmy9igC8U5@9$49<)#GfC0)ikI6xX;FP+-ee0LdVSjCC_*%Kuj0B^ zel)39s$C*X=nLe*Gn23=z3R#HKe#tohnWRHA8(UESon_wF}uK?8>o5brdG+yn{@Z_ z_Kt&?+-yYs5roMoWYvmEinPkG%2~wThS0KIO8xmc^`cS$dUQ@CphsSz2AK{!C@|P| z*?IitW_NFEGQbyXL5Fx^i}dp0Uqo~t!E=1w8sK90wvOl6sp2b!pYC33rLj}EDM7tu9hbDn05PO@o*f?@Nwdpa}rRF(Y zLMj60YV+PiR~V+LEW~wBGhtiTuBX;#_#Tm3m~7%%n%l%E^O2tonsy4U0k#h?@%U`X z4@BdMrpjp?bM|r?X)rn=*OsG?AX#EmNv`rf3+F5cE8I_(JkYRXT8`?(j}oSL7bhPftGN=&^dix zQ{!WtUKzQpqK%@c&eu7(9NW)qMA24Vh;9IU}j9Ji=C=Evs`&ZTJ66OH(5cW=dE zk2S2a1HJA>{fXG59@BB{`BOUVi%%=Mjp86&FxJ?& z_-;Z5@!o=|-a;nL1C&q1lkM$vCXeU=TA-NO9DQ(*>)qS%kxWlpVo;tw&;|U>`?D^H=ZWHl_XHyd0 z`6yBUZJHV>oFhgCDSkV^@D8ksGJEg_a`g|qc&86nl_x+no2m)0UCLReoi8BcmZj5w zf}a18z%em${=4wR{Ew5)KPdEX=wbe+x%l6pXG>ekmZTl_@3=^rqo2KGEfuOhM%~tx z&~HGnF@t!m2FQRlfHkRaABym#Uy>CqC-njdF#1}w@%x!H4OYwGjh`R)wI({M@63bCAVrY3}YtJ6Abl+)^%;2Rc0&iOURiVcWPpjNxR07rO7#w9lTpa~%x)YdGoqEY8Z zq(l|8bS0hHYAX$606%26MqPyU-Q|^n?s%X3{rLu;x^XgrUkdbsT`mfp6{iXl9#JZ% zAZ*!{GFXlUr7<%5#Ap#m0!P(E@j`S!)8w$*{;eN=2l`f6*pEVpOOrYp`P37H}K~>T>WUhQu`6mfLm%LH@KTVWayi@zq(Tc_U}@JD9`uL zSj;;65N(*e%nEe~`))?LKSaRw4fXwQRstk74c9A`mzzOG_K2xXp{gsyfUaBzo7EeG zY(`t01J0z|y31ybnu)3;FpfkZVnk6lkbAtKY1wv8%4wLWO+@W+$Ky1zNe-71l(57L z52tLq`GWFEMIS3q26{e}gv_*`_a^znIp`s1l=@uFP!0^>3u7EiY1NUvdK{}A9-#p` zSk?nl(_+2$N9VK_%_>7-iM8X8DJ1t&QtYA}YYjOdNNNIby2iFvKye~Re=Yw}E{Qxr zZTCRbYPN|yvBmjyl<43fML8hrkx*RX0BV5n6SE&5potc=8NTtDRI{YXt8j7?vmJo< zm2ldUr3E!2Luo#O2wFzDl;dxZL-@S{@8gOe*;Z}Eslvm%g@Pn+xn zBdZgHp;tKkRpJ6uTU-fN=j;buDOQWG+D9O(w7ZM1aG|c{vx3t0fgBUi1jHqwwK)6# zbrco%x>OH~EnpveCZWA7Mb*%qaX zW~FW0woz%@Mx|}rwr$(CtxDUrjhp9m_w9~&9dX~UwPNnM#+(ZS-yXsc1)+jE2)vf7 z6+qZGGS~Zg3iZcwfUeSQ>_R(%s4D%Y_}dn1gFjUJ#fJ$pZvZ)VxsWTxLT{77BdAnu z^kmQ(_AE2iqFwG!I-G`$F`6#&*3{1{4^{>??J-Fr4xD*&pm_|gzG^EnlzS4cJhi4iKj zJLAme-nX^N3iM>CMgUY-26*7THf-prbJSXmk68}n!d5e!j_PmbKB-JFRb;(?3%2jNr+n???IN_5yBvvb;uK%={0Wa z0%&zHaJ)HKsDKWtQHkr*RyrO}#IBN6rH@--Hov$zo?16pu^bqTI?-R{KMkd=j99>6 z@Tg3uXJ9-i*JOo)K3W!7o{kcQ0af4*fyGo19;#a?27;+wHbb?GafR!&LuMX%%ckq_@37Hhu*6kQ9}WRt~RlSctrc8k|}&iQ^)b=MseN1vJ?h1;{?Xu5`t`L*;YbyartcLLWR?-TH5)_j^Q(JP&_^>^v9 z3HgOl4LB}XD!qSxb7{dCn z#6J#L6HW?GWv}y(4mBHD3Rx3U{ZU`9br|XG0lPpri(Np}%?=4N=T)qixvEbLLG5e$ zm(Vu)_V>Nmo4U4Gs63hz$8f-l3h9%lmgXOLmNt3f5f=naV}Gl2{~hE zx&~qpf4nBi>Cw57>c|MIGArsWd6H`l`}2Q*xGGA(*OJC+!+qgX^r0b-SF?69j)JVJvUVJ@ec2ja}(Y2f11Ru7MvJ{@FPN8ROwR|>=$ z%l!$<#6QoLH@dvW%GDLW7h*$CpDR=(Rz`jS83z%X#=2W;x=h=x*_nka_@ z){6guQI@)b&pZA@Pg8nN#Bt}z_*22{4Xm#aV$_PTa|$}H&sEF|EHBSj|L1o(aTX|B zf00U!VWNV7Eb%)XNPUtMH#J6Mse1xdf~ewji0rC{5bpFRZD_?krlnM)<@luraKlK5 zGE0ahF{x1f(3t~@Xh>j*xQCh9Z7hs^CX#m(#);9u#Nw_p>30@xgiHgXJ;?0kL{o_C zrslf@o~OiV;~h4_UP8dVSj*5jj`}9#dUCmR8nC)3p0?`f-zF7>q;K}*aIbB`e{4zQ zGKPtO59WLIgX3FWCzM8ziO7osFDh-yyz>=p<&(=}jz*+3z1FO6&CH!RBm)_`>O;s5 zm(|$y#T+V$>2JT|*H$HmJf)pp>!VmQ0X*`urZ)4OJPat*{M5>w$hg}x-EHMk_?i#S z6~(R1KFn+e8}+MOta>g5Q;naRP-WQ9^|4jpFVy5hFavwpZ8MOSej^{&4+`Os)z1<5 zMM(@$=OAj*;>a*pC2WqC-2zo0vSVnzX_BM*@-Owl6wNKZI!QuAhIbw(F7@JcmFxom z^H1utw)Y!ID=$YyF)zY$cyNJt*BQSeKce%rrf3AthfGUDExu2)rxm7%h5%n?6C5|) zy5j_y@J^7F)Wl#1(AxVRX^>PnDqy0Sbr^iu|6)3x$H}a0z3`&i_nf(RdDdjge*HDH znT~VIL1vv;4n5!P5KA_7PMq?L>pR&j%IZ03)kgcQ*ws{ZpN-m-mV;VF5MF@~<_0Qn zYuloNI?I{xqg>{6HA^6Wbc+HffhujM^$rgTZELLnn#|5< z&ie9kE9U}~-D@S_P#<+gxz&omDjsx`{_kM(asaoki13e}#KScf^G6L*X(7MRg-Y2G zyqLt|u8p;Sq3=# zSxKJm@*u2)_iIIVj##je6n;18%?4lJm{j4iSczD2KQ{}`)ygO9gMHk?iV92xI@U;!@2oln#vnxhS6=(0j_&2DDsqgrgy zQlQgYyeEnEYlh^UDWyDOJ2c3xNl_aL6PAHI`F&V+U<$?~^yrni)ox0gdmkC-Nxrze z8q4;K>9oy(^=5sr&530{a=IYkLb+K7gOqn|(YT305qT*f%KItW0h_jEI?~O5E^&_p z7ZW#mI-hw)(AzLG3-sMwLgyy+)$ZLlJK?N}4JOY2(|bXV;Ng!>4)1rkKrqsK*{9hXQ#%_TSrn0r=gI)?T)mI#z@ad75<)@4T1(U8Wu zsnd$;8&%aGY4_FLA@`2-0jt>)I98(^2=<5*xtLylCAHCG7CUxpzLHK&U z#n>eO_io1={*lW$s2r(@wl!_B@TE#Rd>&QskC1B9*0QzjkVl=Ji2ug|u0?3)Y_)$( zp1Xn{3`NSgIIMEm9eiI3l#W;e8~Cvm1F z>LMH{=@AC_{89=B5#b53*yaieN-zj=7lB);z0_f5Zz-W~7r*9PM1s%gkEyYP)xeEx z`PX`cxfOMtX&UQ>R9ID%4M4%9Ev0ol?TCgah2;T-T@Fdz+TQ3;;dI~Ls7U&Lp5Bq6 zDbomgieU1w$POD091C$79L{5K$ z-3H=mK3`21Ym!$0f`P1NC5P3M<7Db16!8X@`@KZkWTz+gI{$O9b{+hrV=W6S{etQ% zk%b7;V0s0d{T7el{N5&QpHHlGbd7XPd&HJ}_53W=K2?BS(=9)fqS;->du}RQ6)Y6k zu=7dH0}ndxTd8P&ERl88#%svirz~O)hrk^tWL>;T>yt%xE!ijh4hemOs#dPP?;mwA=&RuDGH0*rDmJ!z@qkUyh1J;$va5z*FM@`H^zN-$3o* zt&SkK=U&QfZd@3q$-7n`AHHHB$K4au(q{`h+etVRIl2rXw-8xgiRLwMk7cWpmU&{3 zz*LejWI{evvTXI;uAmHbw#QR|6-VYJeNDJiXu~RrsOMu%C-x-pA;UwzH)QGDrCr%s zaF;jlS5$`i(CHYMDq8f0q8PFk6c!!n8dYd_uZy(;N>~gCi(?^wAypDMAXHQ@sB0&J zAXts}<^*3?K)zL0pHbkis)lPHN)r zsx)MD8jiqAI&W3ps&97JOyaw1{7qtryqenT?iZ|?3`#(mXz1WHw8f$(D}pyx8_%@T za@WLHimX%QsH_v}1! zkRR6q6a;(`_+rczUhyq3$!G|s+X$1=H57&=*bw8)q5y6{R9!2pSIM>ZK+`)28MmKC{_+96Yy7 zwFEamc_ao{wJTw4{Y!rf8c7#VLkLOnbnw)H0}?q3l%AISIV3Vp1QDv$(fvU|546{M z@(KJwf+gBZZ7P?SQN-jY+Hfa%`+SAP@Jw=_LVZ(ZiZlc=%L6+?`*^+Cdd8hhC2V~f z1*KgT^VLyARL|o!VD|LLm5?SBC$pVdsX4xR37$LlI(r4(A6v4Qo&RSWb4&VpP2q7Q z)ImtVlX`b|jVBH>?Vu&6n_(aRf=D+FiN~DJ1r*PQ=)?hn`!`V61*HCobhH;^^kT#O zrZHTvNXzKj-q0k?aGYIWYh=#nWM8ev$JB%mS9*Bp$`?|Hbx8IuC?Z2dzgVHk8!@ld zfX)8eL`Js9bfAt}*M~O?Y|wpjqEb_nZqox$H5EA^nZST^f?hk+}Zt z!d=>eh1$iN3+4qdRmo`zw;iD4bSmV}_*R}J&V7o}?ePLSO@#tdeqsJ7l1fUQk;JI0Cj#K*5P=NpRI>6IG)IZ7_j z-xc~H9DCxVWp~vzW!C^);+S|Uuz}=Ejjz?e0BA`|@paVkEvB&1#C<^@ah7^DFYnnw zQkY_E#1Q~ci2IEn23u1lkj0@_9O5Mfz_?36{kHgcK6L^#VX8mobOfUgDm5($Q2BLj z%w%>tGwcjt>19BTmduY$BX>QHUS*qMWEgHi93LXR!ZSy3ysX%%k_3CH0A1`>oZbyX za>6iNV=j5nS2;{ze+z`Ayx7| zW0b01%;_-J+A|7wAr5-=Uos)73bTa)ln6l;TWBw%^gt%TVh}#GViOA%Pclz#>uHM zM7}39s)3q~irz%h&tEOQz7{B0slBEl9Ylkt-mfrj@Xz<#+>UMSWiz6i&MsecSV^_| zb4?-hK(3LI9LLy#$`5ncE9|Psto+Lyt^YEIb$L?}YrPC7%x}AXQ3Ns-8Axa?6|hL8 zAGT8ohqum4j-l2oyQb8IRkTI)T=$j$I$NFZWtLeX-}?^^U9tPg4s4S2SZ-(HsShmb ziqOy6@UEw2<`oBGOkg(k{)Z^;T1Rh6P;^50KO62iBKwE!JzA#(L8AH?g#6D_u9vW1TPL1qpklFhR1wIY$mh(SNNcawIJUI#-CF8f zm5%llqRrC-Y#=}(BOJ39`33f|ppK1GS;l=K z;DjP)rhiq-Y?IXait?o)p|>o+_pb8Xf&Tqc)lLYJ(_Nnir008XJLt^A6>BZF+snro%YSnkfZbXt?a zAuX>MajF>IsNb*6n{Sk7|K3?G1tiJSU2RR{>*`w=f>lb=QwGQg2p+ zZ4k6xnj!kiTPyX}s<4-S8XlS4@1n>o>69Q441NeAs|wZ#-I2r37F}P%f|*B{VS_mI zd!Wvea^_CVX;t3ZtVrB|V^D-^xNwIY&v0MG zK^^Z(wwJXq=?>XenDxU=ZznJY$smuH2xnJziY*(Tw3Md{<-X>-udkj4}BRfW~_ zdLaI`{?HlD0)c*b*d4BGCF&r`2tN%NQ7FJm7uQ%^E2sKs>1&x_s$u>k+045@he0z1 zDG{X$ZD77l=EWJ7S;zeRlQf7`IT{!W(FhS{UHYwC@-cTx!?kCv3dYK~=ifsrW~}jQ zyP!fhm#7xudGe5s#1?Amv~S~(onvW$DmtS$Qv(VHw_8VG(ix(9H!j7u? ztzdkYtR0(P0sS+1&XNo{J zLR#e2fDI6N@ak*_cVdwQSxS$3$t8S4CEklGZbSCFx3`RUlrmw=sNl^p!z6|H`cJ@f za#Seq6j za7y?1>Lj`DD2%bS-(3uvPnS9<6a9xs{xc!jRJ_RloN@lXi_&P&$5eIBV|MOC_hHLu;!y4zty>#*eirHB$^w4< z3+2#$O&N62PnFm-hLJZ6$d`p=ZBY4YX@IVIms?h9)9Fd25qYILYTvTUBjjO+u^g8o z?M#HXQ6_SYErj>D^4D_(?(pbWMcHY|lFO4Oy!wh})-*Zh8<tDwvOfWZGm>ixeU z@Fv!THF4YJ)*ty}kc-gxawWh>(6vwYgJxepZ2ygTY4{7!QJuT@JYoexlBCPD$0K<3 zeY57l=K`T*%WS>ZnbYkEb!cym{Wr^$&4K&wkGGcj9B;KRC&rl$IAiU%Z=cV1ib0be zOH_EWw_~SY-xu&UucskMz|YubQyekxgZjhwuMr$N>KXfeXYvXFCCm64$nDgeYAnhL z4@6;?h=4oAe<0i72`!zD>j{&E@H#J~52*#O)zR|MJX5 zvx~V)10^>u=Ry>0%yCs}d=)k(Tc=Xu3;(4()!q#0D^`x>{IWl5!mAYe!s6y03^crZ zb6(>n6;?}lv-t-K+d{1zUY79o__AF=(;c}t(~)A@I5%im2)tb>bUPgPbbj7C`bawB z8&5~c^$H0H(82E-<1O`9G%XPzU!w4myg;&jxHWBIHZD4Qzmp+2SaZdWR~{u3Gdy*Y zAmEB;eo#SK%*ZBt17+p_UWKsuq%7DLU)&vkf(UFGPIuy^WH$@(TD108d5Sgu!iX+^ zGi;M-rxr(~*Q7xp$9Dd@)(JGx7OR9(F%4$4yrm$1&VfYEFIyCl|hcKx)JMqbHc2{Nr z;W3n8-?QI<9FlbOIQ{HJ4-6R;sr z6#3%V>I>6EgsA|`;)_UdAyZfg+6%*cwGR6&@M-W@k%vi9)Rut#s=@<(Ez-fs?}imM zBM@nv)8XNQIvQD)nGt>nuW2Z`%A3^(`%=b|LI9we!Dw<|7@3hEyX}W9BSwX*0OUd3 z%e6w)ptK8XK(vhqEko*wE1D7B8(O*fdsrB|f*6^ZXSYfOu>5sXur8l54c2xQo?`3> z56u^{BA}dSGdzK2v2hf=tI#hb_{S%IbzA0{E{TATQ8m6Ct z!bqwacQDbdAA9?=c(9s5@5|p{cJ`uYQVN60vUjm+uLty;-Xh;7JtV67vBq`EG-VFB zLWkIRSJKU6OaRC#{k<*GTwx-~3($c!v}#BBqKW=!=mN+B+OUYO=ym}*>L}@m%e|w<=z3jAkPW-xUtSN(q5LC z1@$!(pUN4;^a-@#?rxEzy{RnYJIOzn;N|QFG>FM(bvzq#s4dcmBv)wFxwoDH185jC z^}u4RZs%kS2Wj6JqY4QoE^EHCGnu=~Q0->z}7ctgVQEk6f5*@gG$|Z@UM{qgA zt=qDH&P9Jhp?lhlM1yU1kHnj#baM=W@oH3XrF4%+CvrKWUTD5QNOI$igBp9gCeneo zL9J=Z%4GdYFa?wHqE{Ii%r9OKL9;$1x!=V6I7S3UQ##eJnNo26L*nPmS}<*33X>-^ zTGdQ?+j&l!tu$s+IN{ebIJC;a?TLRIfcM9EdPvr0@-jMS?hS-j`~KZNWMn;~t37nP z=yMrZouRw$WG)hvrLIN%dZr35Abwpw9Km^$Js;m`i5<4F(-d?p=e1LOYFay6_^?kI z6Io%x8OR*HBOAK9LCn#i<5 zwF)R_!C=)??Kz929iJ_vE^>DiOc%Foe(>d&bdoAFkjF&lX7 zxUw(I(r+K|Hi}2qT#z|7xV-fkbuK(Oq0Ry|);Q@+Z$!AosFeUzc^rY2FQSWk!Mt-Z zs}(Q|ywWUbA^1KcaXPc|h&(G%f6@29E>wZ_cnXr4QqkvbV1+0PKYxHbL+e{n9iu_9 zriV;|lm7Xv^UL}Pw6LikHNxWRpeT(B1juqm3*+5YnJDpk zSZU3?crH^6d?EZhsm)Tl=Kf)*dAfNn0IQ-u`+@)KQ)<_XqIAvk0bEy~Iv2iJ*1LB9 zv$iVwo2>CzkuE!u+Z8q>Mw1BC`o0D35}xRS-|1d4*_XLr3aPwP5CRT3#9V$#y3c4tj)$WhyUA zSXqNLEe5tCE!m>DRMf&-Q+aqPAn4Y24lgfUyHvV$+2Oqd>7t)-d>}aVRSA%ABg?Fs zr;_hJ%*NyENCmICOS?;)uZ#4TX0GH|8A05S1$Vk;oyQ*l3_3`odGwEFhS_tf4%9kS z(2yeiVO9pEw)Ls>%4+R(8NQA@s=TcK(9HLDYtG5ZuH;%vfhX~#fc8mHHEw3V#)|2k z>$J6h7Lbnjbsp0Y;IjGM)TEt%nMFD%xJzqQrTsVgAJx28edAmehO(T8D0i%-d#WQ5 zr^3f&V<=tODl_1_5coqjd2}2L74aXkRdayNJ*?xGy00gIlEStos{5nlS1gn; z)WK~&95Q$F67L;{QeRKd1Ft8qo}qXw3;k#r=N)%gQD1jwokZ<1RVI3;Sq6{ zua*d;t6Ylipkrzzp6uP1paiP78SH`ehXs1rp!O3Ibejtrl(!QXOjE}pf|UK+3eveu z(Bxsd_iPj6r>Wa6eks$;W-54Xkqi*weK^cdt+I9Lz~=p`@TXyHSQ}r8)tL66u*6ze zMMH|B036;Jg??8bxFW~%2(JU3gOl_j8?87)x>!pie-2yZzEY8 z-}5=uoR=QZX>azsJt+%}`TrY=`mZ!5CMNp-E`Z7OUv-538$~hy|7oK6(~YUZpbcT` zT{ZKu*GVaUIu3Z)w&t>9ly){&c$Ah2B9AO~>)^WNfxJv&?$)3k3^Dz-iMmW_rbZ%0 zUR3SN;pI6WHE4UO7WZ+F2K)2w@^~P}N8`&$PShq#BjxM5r3X)oeQn6pfmyP4#OYVW zcK2uJ$KCv|Opf3=lgJ^vO2wf=$X{LTV66`|Y3UAPKCkV%14YB(FyL>kVaX+aVBnx8 zCYWaF)loj!ghWQxI)anhrq;zBl-o^G+9b`*5~=gl6NcVSTG_;Zaas7{PuP2=4QXV7 z!sdkvWG12r#rK*k36A24gRJ8Mf3666W7LyLkgQh^Sco95t3#Bm%c~&nJT%+MdvxQO zk`yr6ckBCgoJX%IuiN>&SxGM3)DW4@Vv!_GM>ByJ2&?P%mF zjlW4Qt7zmQJK~@SEAMWi)P>d?VC7JoSoT{8(XbpWgf4ee&PeiL9O@^rz`ztoB%d-C zkEx$-T&b05SqY6o#rkJ;zc;A}7;}1zf0obVnbHqv`Dd%p5ioU9Ix8aVMB=sbej|{7 zEUN~kamC_^F^R>^Y{kbh^=ZvYXQxsJe>ST?OcBU2MH)s6&M-n1EdYY(;ae>>QD*nX zVf`#u*yowH(vTMhBNH`0^S2Rg-TD;2!bA`WD>FSe$I}bf!TnUX&_nd3-P%LR!k!oUv$2T8Oagg7}K3%*LzRLfw#8vi7$( zTgBZ+N7USJf7_OaGBdivrh^qDNP!*h#6`&*CU;}CZ2gBpz-NM3pmk<>rTX7Rj$-RK z7K3C=`P16J(MD%(!%`@EaD^ZgePdGCs?f0}n)7=1d4?^(HG(~QrJ{jO@jpXmt6)OlS*W+tDtJ~>_uIQT%yel4D$|rJ{+cyl~VzKM9xR`%iFA%$xSFexDg@3uG>}^R8yK} zs7#y_@7YC~ckXB$v$-&)jKYSOFDE!3Cv_8%wT5!~NkUZ~_jtb+ECgxG&nTezx3O{C zg2lB7)h>r<#O*TX9`hiGWA!HR%%fHG3utt z-oXT1FXVI#=OlmqGPA!8X9Ok+DvB`6b(e(QvO0%~Q$eNbtz<_of%7TRTJt5yD#icXqZ9_0adX?m$|U; z%dS-Z@VJ~hJVlDuf(1+`sza1uh>!nG;U4eQ-<)M`3QXCO=cqGfN8Vr<#*4e1 zak+tRYD2Lp&k0tInbTeW4#kI&ARk`%Tj`kGY?0xG@zKvgq8Pk9B8`zFqGEuCk;~m$ zKCvNno6{{}S(Je&+K80vVCcpqy4Uv{!$UBzni`NFErxvkRv0wU#maFuFKx1rUlBpa zzt4o;XJMmQM0PbqqB_?<&Rs~v!rpRbFUtfGsg9ywQh+r6seg}Qw{pBVLg0?xixMAi zg3)x)TF@*{047QMW;^mS0EJ<`6zU7@W__eUsTL#3Y3^HuQn!2Brq- z9{Q;fxeiRI;*s#hE*)l`=+?Xsli{5m)w~i(w{VB2RzTKQW<&U(#FMcE5eu908l{{e z4V+Bk-cMb)-;znd`S54W9LwCBY;u%wjV*bh+zc?yY8VI2+)prRVYZ>rfu5_tYhf+P zXy>dgj22r)(NCqYnb3@|Z-IpNv3$iND0`Ki$Rxw108->$ZFi6@!e8)I8QoAa5T8y-N)&?8IGuIC?JW_`+T1;HBx2is#+rH%W_B z9XuT7f~yRx7Q;U9uML7Qm}VLF60jHW87ptUyl>D2+)rBg6Nab~52c@j%{VUa;TN=W z04zgw-@S(NZTuJWl;V-mi!A``(i;e;O-IjsD6h1`pRo!U4eY_-`0NzY{eVtQK14i-ltr5Zl&xPWXWj=zM#ob z7E;NfUyn1=^tu}2M+9N#z0C_JGbu@%S=Ec6y*FL+O!j2v)<3zoX^x@jHf_!tFveaj zj1I0t$rx?fw~Mt3EbQCgCxp747OIa?SEU|c4W@@!+9bZqBI7-pW5Ti+z(wttGA?fg z*kxE6^L2b|C`kgh$2$pxTP z6y`OAfXj)JL$$kP@-l&O?v;Ov$dZ(80 z5kieBQEvh1*ynJt95`%3;Lr3V_uDA3E{)=_ysXa8WJJ>$9oaz3=N*|5UU<;vJ_3am$4k$yPW@mD+riHVU8!;xv)CKujq_jepD)+FB3I zEk>s#Y6yfQ=GXV+i9|C9uN<_Rq@2zr$IFrus7?P8k6=P_W)ExdYIAD}sg~I{ebqw!GzTl`=Qi^o08W zIKd!~I=yxD?ND-n(UwglIM zC;BPtI_<|umh(1fxDG-<6jn3b7z&k3*WM3~x4V4VzrQU*P;4*5tF5n%d!gZAgf_>` zmm=c;2E-*eY|-{+Ew*?ryiILoqcZnCt4cXxC; zS$41g?1384Uv&-4ZrA_G7hHRipcvI_eD~U|=b?;sJ&|3Ld^=~Oh;atpgbKBcex><9 z5-nq*JM-t~I-y&Ae$w?)JE4J2xpI;Dd&ctd)dk)oydPRuGQkm>HFCa$wQjYwJ(qch2 zUrY>T&wby4mY{onE+j=)aIX~e!-By~*1#UxmTNJYML_;u-^tBV{I+xW{n*z|gNbFN z^|YuD_gXYjU-ACiTXJs-ilHO7RNYYgMba#J4PJDK%iZ0>0XHCF<$4T9sMlWA*tM4o z6zBFmNNh z6JKU!-^(`c-@YEtd2_;`S|8MkUr=QB#XJ8Ca`_jK{2Kzv!1~|Fh57&6N0#}YsL219 z|ECIjBAoc&5J;1O#>eqQU}-@oqsP9MpkH{AHcxA*I04Ul+b%*U2nY(s%Fopp?d6H_ zdqu7i;|z4puh)mXKi;1Yjr+H~WaJA;4e!tQ!x`!6}!?l$qdb%YSKRR)jJ%h=4M#6{z_BG zF5(hA>e(g-K$sCGVX78Cv@1z_4Sku=wQ#{H?baCvviw6X)GJAI9feFw3|=a-74Rrx z%UWo|f2T&+Yb<0kW1C;~do?={YIYm!c9$LrPeD}Xp8w32Q)cNK9%k~0D5U_xA_Fe4 zQ^1P<3458cLM*q903A!dR-+_CsSO;7&RK z$2v1DpEX`ze>-5xA?%DnUVDF*3~x+j-+kXs=JkFVYCn9R@o{rHy3FumxrN~Y^jcld z@L<8kyQ#T(qUpl)tqT5le9xhMz6Ree5_-GsK-iF+@A3-BhC8bR6> z?F<5kMmhtJ!;$OJ0KYi`^;unOx&AIMu)6)F2SQ;cF~wE8Oi}M5jsCYj!y}KhBSt`? z{<*~l0c=OgMt80SCPVciY7**~ju8RomcL4nOn3&9Bm!Ju|ogay!P0Q4I*XmKM*W(jon3;?>h*jYA81v4u`G}*0!Vn74 zCatORSbb|R!n}fnz1iBt#pkBFpDp1=3nj1*e2C3cdnG^U2;#4^$GAjR1aGWQhU& zQag`?F$QYvXt;^0h7bc_=~M2ROTgm!m7Mkg&pbN~K+Qe7XXql2<{BR$hsWCeMZYx?VHy=p)g2p?*NgjN$y|zJ{Xbd7Ym! z?+w9*-GX<4ANh>x=`vs??XNe}x<1Z3M(jC?R2?$1K>fCGrM%aLJ|YBsEJt+w*Cs>2 zP8ESvyb)y57+SFr_|w3>d(jPySR|MA4p#6|;i0_I^6H zi?5H7Eyd|2I$}vsF^X>%9l?N>Ei=Dw-Rt}$lhp7S+ZmIPT!~suJ;Cj?1!FENM7O}$jr;N~u_{&4xFOe@=WNF&|hluo_z|-h9 z=v!L)C7EjPP;x={wUaekW#8FH)c&DqpCIu0(7v#H2B%*dHU~@a4vI3%n+l%T+ z6i$$_}=ys)4Er9B@(oL8J7_t2;%PZgO{}K8V=Go1b zq8V~g3GHBu9>8SzNo&1O&qf?p2Zeib4ro_MnW6uA=jljPa3GZ%_{~-+aivNBL2lek z-eKmV4?xsCzQ>jXU&9+%1bWeg_i*F*#7Db1y9>+drDLe++-WJp^Ueh=<`- z`f$1900f~L;U6WXg6kr^MJ-_noik<|1q~tHM|RHh#F(&;9cCK^4k0N(MuqLk$7Xgo zA@ULJrRsu7Xy=oBHEs5xh8tNlz-qjhS(ANK0LJm8e#LXx3po`~$Y7BesV`NDku@#B zvCTDctdz`Z(v4F&9_utWdb(oQUexNEnmUf`JbKEm$vG2zge@C850R7!O@T(ncpMfk zQ;19A%xQ-?`+I^gU*L`WT%TF6elFg(fseO@hKPYs8Sw_$zrdHeze0vGdLEfBo(?0) zNd0X;Ut$UK#E4^J=vk{c2BfRrS)I$7Ilp>CXlN{YuhKueTt299?M>~)s+`!6EiY70 zASF*riM+h2BE8ZNnSD6Ukewg7q`1*#y}6lKdJ|f2 z{hFRC!RTqAgiK=7gnvGf+3__<7(9GOcmtp$8a+_D@ntz;sjpE|Z}nc~n;!G{8VAX> zSb35%F=3pEsGUk`C}QxX_Q5QS=uA}zaOY(D=}9&p;JyWvE>Eli788!|XHGSZ>ps2T z_b0gbc~cM{^f{8XiQTqRrowvW=*e+I1ej3U7M>R+lM=;ZZwWTjtrC@|n@|u#W4`_0c`U zyCCQ3a>Hfg#7c=0uy7YQ;1z46%?FMu3sqgyrt3>k1^E*5YSfyxUiv$S6QtARQ4QJ8 z$$2tKkUBjXwNKWXVGizQ<{c?0ySFJJ@!-XZeBUg@7} zX!@d+!_O~LnJDvKvAUwoUE9nwQJ%+%9@9w}o+{N-2TV@_u&tU*b6#X&yP zM;`XmW*iV4oEhY|$`zX10=}4yaPxtw*~Q^DZ5fDS6iNpG*L|BmaY1k?^Yi!eV(KV6 zOv$!G*iHTh(GQ7qm1cp5%Y8x)oYW#0kTbtb?7f{7DtfnnGHmSYCI^jIb^?PfpZWKl z3EwcgqrStI*J?(D@80^)O{tp&-}2$tfRUa|6|&{@M3kti(37P=r_61B6jz&;4k@|k z+sDn2OMf&@mv&$zGDB=$zp+^)ZyrJU!MI^*<<7;YNl4smO#B`m>GQ+S)2JRMXlJ!xoKK!b1C2Mts$jutYvR z`cPHGb%r=afyh02#JbfUquOT)5B5pYx(cj0&>CsVeB2FbNiO; z;n=oq+c@!wZQHhOCnvUTCnvUT+qU2QZufn=Kio0;|Ml5x*I3UOwQJQ}RW;|-b=^3X zcDW>;@LjN^)4^$RWON^tt2o4BdwpRtI^sSFV%E#VM+eU`>HX8}4un?eI!t5JgVLEU zQKN%aLw-kP`P3Gpc_eB5^e)n=+HXDyN>fY$gp4ixGpubLq&{}0Fh2JJCP)wwgEQP* z0dBlDEyCuGboz-aYOSTa{qV2JdJea=!;{V2qXgQ`*XbM zPgPfThFsD-vB) zZlN}a?ljzkQ5rWsp*WkDbS)nO|cnl;FRMn z-I0%(_hpKFeNpLjtq2OFU$TT-!1=zq5?zwiQP6v{z(KQxkd7qUS||~9a0&8pzt)%> zB?{N4&5yAZ`Yz_Rum{CH7+e><3Ji@Df6dQ*52IDSFn@J3`K;ssS=tP)d;>rQwx#?h zl>YCLL}pIL{~=Fg`8TEF|Ee$kzsOEcVvk!`wb^g$+AjgSJaBm6n8AGB5p@6x{m@rW zkOX-GxBUq0Nhl;Kk)ArW@VX+xH2=sBDAOk`oGH_P^M07hM+SUv9O+CVseWRm;CJJG z-HqQ}2YN(X%GsQLf4vM7TGgEI?L0S|8S7r=tG+z$w;Vp}!f~Z9`z0=E{2C`?&JEeN znO!UX#@sRS^GItX`&~)J7*RNh6aZG-jqXGUU^?Cc3PuQz1B1jwFB*>qLZ>tc&QF`Mdb01Iv$4Tt)HFuL=S-CYp$@APwX@nz%y2 zNCvSxh=v)Ie5^+lZFyEIujdvLQV(A{=~oFXd65M$o80N5{81qdiMA;Y2^U-|%5R~b z1TX~pvy5n~qJRVfgO~6n5F!%frNXo5QwhWvhb{HP|4@Z`LIK^yO==6C-Q6 zEI>+!mcJTmp!D^fS;iB6W-O!1mkUcp7&xiGW|}Mg5^eOq(8C*Ih_+?mFKr?$Ut4)t z{EJ;MJ%j;WFsZfE&C+#3{gF9ofieKF{a{?Ry-S5DwFhGK7#?f|daXQ=@Be{aIME#K zISgy%9XV_;s%2aHOq4gjd?(viwLcNi2#$Z%I-aRD>W>8UK=I}%6q@KMZD$->NsXh< z-!7)^W9(2;MW`7}=`_KrZYM<2!lP`%Hx%REAx;!t><-SGt0=>n=9Z`4b~N&fU5_RC zaPd!Kv0dUC-^zGTIlIvBjRldc9dH)dJbO^O8Vw;cr8U2Pog{fv23pWOMSsF~7F(8& zU@RmPU<`-1OM=qu20c)0FEGGZ0`H-+#X+MenuBzs)Gf-0b&)fH2-Q5JBf$~83@AK^z1fzFh6uYC)WwOGcr8^o*yrTnB1592idZj7@&iWdUMOaS znG!SxJsuHYMePD3f&#zjKqLT!U$k#hes+79q8V+WaPRe5StZ>QGc^R zHQvk|vB51#^bSxfAjCmWu8@IZTRUrKRA}sjK#BKLeX*1#(G;RAs2?Rp1JROT!c@4M z&lvrJ0;(U;D`j(x!W+{ojc|me>>oB(E3kNv19Qfo%e@4~2i0&hbHoNme*c96av;cT zjsId+alVFg8Au@a8*7=zfR)-13~ZA4gc6e}2ZGW8%m1_4lQDAqe>MwE@sHWHs;pi8UxZ)^qMWBB zrElGo2-5X1pV0+-8^l+bS_AW;rj2o)>kqOo*la#jwDNlNc(8=c@LjAs_EtdM`XyU* zDmM^*e!SSlAw(h3`}A84$HURSAQ!6)tWpeCK)%fvZL#*#0zKKVirT6A`!A7;m#e;N zT%lL142zu@Z2tV~BDNWmGeH%PQmGa%ecA3~e>1G!9#+X!lB+uwhQv2UL09bc+Nw;0 z4(I6Z8IYcZoRbWar;N(ak$}nV$rIYCW?9xpq!tFPMfZGip(9~7c!_D`o%E9uQ$-8V zRNa&=SM}tz^eU{=RNTDyA4I}UdqvPRyb+hs&9s9DECVHqmteC8Q$_zMW2l8jqbjKm z+&oAdX;?hdEFj27eo_t1=+ii~a|N2NNEDmsP7}!xuNCH5m{bb2EBKKM5X~bMn)-)^ zaW%6m*+mrzrc}9il^aP_5`!)=x}b!o-McTAZ}0#pZcOUhXoMG#2il^rzIS z%q73!^85Ehw+Kj}SFuD_Z_buk0<^_7E8t-bswZbL8A=@;&tF%+@7BcUU&xNUpiA&)p@wke8PJwwuZeCx(RMLaM#!}r4LyDmo!kfs7Six zECGI}xaV>LxOQe%i)9%`1DoAZID6WBeJ!S_r|+2x?Px1|31> z1PH%nO6@$6x@!GpbT{n(BV7WxJ>J_SxbE*%V23uZqjLq>*{G4J?G@BE|$4BUz%(d+Sp{fZw0PSLR0zJ zX}_6ocYbRMV0d6XL(Iuh-06MoZ3gQx5-w}aQ9fvE7{asNrn#Ryo&d8sniKAkWg}Rii+{ zk1TdaMz^h?u0lL>N%V3fB~*cTy*rR}PO`(BHnqZaEA9yVpfYuOZ-as7k-AZJ9Pn65 zh&)8YYL>eKredlDU3o#JWI=$ES;^V51pQn>%dgew>eY zU0UFgbvqWb?Z4`^Rb85a zei__h?`Vx<@g%DgjD8EoB5^D&m_t!_FUtPVVw88}tpRl>G(^K7I}PXE)B90}E_p>3 zJ1|Kuu`iEq_ZP>35H#xw3crc)>PAvvNg|kW;wmD#35nMZ@p=Dl&wB#iV#5{h>dt3U z3Bp*w$0^tQ={%e-6Q5cCz>DDf?M-gA^_3^~GIN(AS6yC_jXJKLUDIq$I=AU1duhal z)%W0KxaXamDXGpR=MXu9?b-2?oN0OkRp((1wWx@8GnzCtJ7}MR$H^AJa%pafS~X*c_zUIy4D^f&r2GSvkOBfvQ`Z$)Alhjrp5|IqVG}N)Jcxjq(B} z4p`F18#c>jDFomkEe;5{Ic<={cUIDU+(Eiw?a+P+wvwkP=?!cR=9DrbX44bFa2N}E zJ5WiMrsM&i{P3M{E%lDc<~}$^7?ZKMqLlBm`se$WvP4dy% z6}dvEG?-a=#4S_%H1`+dhrp`5!R3Sb$vqV2pDNeLIBp4Ss^Xg+rPc`W^3ijm@d5Pg zQMbB{h>RaLLtG0FT-;MWHrO*%H>^h0OD`02!mbc{j+>BVu!|(NS%g~2Kw4Gi*KuqX zCa7biBo+;7gP6o_4+`A{0&ymK67jIdqHgqp-_YUnQ3V0kpB=vWgnHsg1Ca?crN0A| z%xw0tc8s9(r?+O!rmtS+d)UQSDCPtc1{#(DSauQ!U;!m@+}n}HH|9!z@15RNPHc10=a7ZBGT1kG=RXW&Y9z#@O?Xp~wu+;qndWHz&sZS&9&2D&@ z#ePmU|Lq2Js;tNMSxQefn7ZCN$d8!LT1+boyQFMbAuGF|qigB1bv|#Me^_UQ;a@98 z?cCVItP*!tz&H5_-uYCTAZm|ye~64kg%P=p3CyqBu6+#E(n6^bR|-?uw=ul~*IOT6 zJJtH~;htgf(!246SI9~X2|J4l=#)oU$ohE613ifis0dANU8;KSd5D-|(CJAupjW_&!!`Np}3wINtU_ zy52+u-!Gdw02bsJi@BP<*^;03AH&3h}Yqx9*r*92k`;>^sa@_AiYBTGT zkQ!Fp^{F&nx$VX2c7L=l&^sTCDW3KQB>A|27;@(2eZU*o07v~NeE08pJ7yME_J5xL zQ;~&Y`8Rv)|H60ww3Pl|RoMTY!qfR@3U6b!f&^8Mg~6o-z73QiPAKjln%Fc@7y8#v zIrxae!n1ntL|6ovA)6-Qh@WQk`u3B@?eUrl4OFkSr%QvSlLyl3?oaR6d)agP$aZ1$ zh32Pj-}hmpr%27qD*!Mbs|pu%XuI=iep;@Tah>PN2D=z*6OJn24W>|P&m}I%A2p6g zgNBe?{{vf4O^9F1Z_dFYBayIZ9_;A!Dv0oO9xVXcc18^IXx~~$gs9CMgnFt$v~KL3 zz@yg2N}G}`g-MoSoKqgsdFXC1(a2e?qB45D1glhgy}?gF;Lro&#m!qkudK}vKvqMM zKccNc1JOaqe4zOykBMujkwBtF!@uFI=QRWKcRLsbKo4y9FLe$HeP(j@C|Z+m_uSgU zHS6st9TL#;IK-g|Q|gNFw9afz^}39L{>v<}#X6N^K+DV)-Hyv>xZ?pn_KV(qY$L(< zPDL1aJo51jzo4d_%o#2a8fCc~byF)<=7Odj{AIcwl*|K|!Y`SJ5V#IV%R=R15oS2% z57ihc2l?A+0_dr|VXva|wlRTD|9xUOsmC&6c=GNlfkJq5S~hzt9N+q9;2L6J(b{Z? zKw$*Bn2wd5c(IELyPIr~e(g0DPm`4f?TM;C>pGwp;S9e)hRJU3SJtdTCw_4V%LWM$ zanPj^$n;S`Cxe@p%F1`eZlR#V@8d<{n{o!!i=*pM+Tns1dnfg(#5FwhZa)9yTx0SB z2=+T9ef4TIz{qW0*K-@3#&PN3{D@?2iuYD8)`)5KLr~cwWk4t8!0Mb~Fyqw%hpwkI z#KpqLSSRv(#VGVj(+!Y10p&IJvBreUexRI@BOOHix02;|B1k=wY5{hzFBG?RD5FW! z1@;}H%#BRh+&ZTZoZ?3G)gLL(r0pb%HM=A4+e`d^K`GRipL22+BR!OLcXWfc>BqFU zzneC;k%P8~^&m7O7%&H_*8oOzv!mPjPEDQX&n&W$_1=tYZjux=)i4E)uz&2AMPXo6yY!ps-TkT*2roauxjPR_0nGsgZ)vijzD38xHR3} ztYER?j*HPy?zRb-SKb2Ej%T<@?issmkYuu#no&W;f-O}jqD>@>M2n`(z>K7&8kooI zq~31+{0rSq;bk_dq&jG-$PvHS;EAZ)rbm`iI6YyqHovGL6D5uif!3}pkM?nemGg+} z!$N4%cH&7$8i`&MB*ud2-mC3H<^>UIQ0GbzmsOc8e-(2^N7E4@R+36G2pnS<`0a@( z7Iz`)ZEA9>ZIoQ)1dSlFhCW6uFJxWH?ZjQxq+E038!Ag*!YKUs_j8&ELSYW$HD4JB zJd!*FZa;WEfkf2hH!MCZXeZzc1|t*l5oNrR~Y7YXTn@Ejt&24>Za~A`zDI zBfBP#j3rCaUczoWt5|I3=Gk$$&`aFh)((PVM0~15VA&C^jpUGG*Rm@fVe78_YyfPJ zwg(=R!^cX!gLkvxZ;r`H(SfBvo@Su96XS{Y8AeGL^UBr9DMw{4mNDamiodFI@2-90 z9;=SUt-K2kM{eNy+pBO?ZK=QXfch5{2iYY7IV0TA-^YZgvwr9mvYLV6ZLsey-4{5X zdp>y6e>(sbGbaOG)545W?ENXc{2Nt&cxu=OCkZ)tB1-R9dD=yPm(Kee<~UY_eLeaVV{&w@UP?3E#N6q;b+R>0HsvWDF}1q@vv> z)9k${OCImC&S9H6oKyLgoP@GM8a9 z$oFtpVh;BkmPvXm8%0WZqc@uVanl}JbDq*KEx4kR#8dfR2D82AMnLAW9}Ww8Z_47` z-b+&6b^$)O**IjsSUPH}8-R7w=INz`csDDtx~Q_*!UI^N#_)1#;8wP@%d^f40zqk6 z4CZ^M$_V1QYt*aKBh0G4V8Xd^Q?}$=+p0CrN~tJSq;q>?gZydS(Re>9lF_&0VDi`B zkCIj6d@Al%OHuju7wYtj9`<{2fl-k1iC`!aN3aj~22pjjYm?Mo66dP0v9>qsm_@GV z>LPldfXbX61C`V{zuo#b76`PTrS%FtsKAA%Yd!uA z&{~>#fX?)D?6?ee2DI4qsWxv0c*cnT()yv}=9ZqD&!2>y??WQDYTExfCSBl~T{9QB zK`tablR5OM*KP7TwR=Xzx!g?m`@yi5i-U>zRxE76?dkVEIDJugj4pVURw*&4ldOzy zNgU$hk@y67pJB(n`=PdL``E4SVb|?g7ZbRpAr^7L|q2Fcv%RnTNr;Gh)xhRJ6KQ;xDHKTre3@W(Wy z_Etc?tain|*n;A-qmKG;GJ0|@ZLh%XFSR7AY%UIS3;!0J32qykL zAVw*Vf{^4c=RxP^Alm}$ElUJ=>WkOnfibfo`;3y1rP~XX9AnECAUpzm{&&9YKdPzV z4X!c9I5g85mN#y}yi9a3Uo324{lj@xPn7mNR*)xo&aL5WvZvDX{b-DuaIlG-XSE9S zmw`kFLbiP32$#})f_qu#6k!B65{u0Ou z-KK<^ee*pIcVo-@&p}hTW*FLkGR!t>K@Y#|AH;<34lB34n-N#P zaM9@BnCK7>`vIHfb1^%u&N6v7{o|ZW`I|3eJKz0A!kR+9?jTgh7<6g{px6HuM0RrRLTao!NUOYrChYI7Y+noH4nWYX)e_Mb1oBb8C{LF(I6%Rn za_Q9IG5&d_H(jCw{U^?qpLR_`%A=D9P45ZIN)9 z+w*nP3W6cO#@UEK>4@ecy}AKb(GPebbeg?p;7$73v8tgmZ0&H?r# zJaA3{fPwMW8&2qY0JKpyo6O}8(9GCAKNAut;^%JnkH7?Yib-#26Y`xV5tF}hzdBL} zcUo$L3$znL4K;k;hj;1uX}>vB4ew@yj6dGj`Qh!k-$+}-sC&8I9>hYSQ*?c})C^!i|TX^>V_H!DHAjBq>Ya4(Fe^lv3Af{>fXd{GSLO>u;!CLbm zj=qEt)1UWr!zD$pn3Xm`*U-8JigSnwR?8EbS%;BMNfh5ecZ~wF1Rnw@{L&=Pt&%=q z#5o;Fhe?}`^43xog&Lyr_rsayxk*K@_`ByLZPYu)515SFPL@A<&L!-7K2UxfP!KWc z7w2#OU)Ce5Ne{4+h(L*xIXXxxW4$F`X4^ztEWe2D#V>*z)MW!7{@NM!xe&W*OQ?G5 zndtxRoXlm9=qZ8NXz*VsdZHej(0|(NUD#ag!u^A+GA{vsnPIVI$6zx_7ybZD)|W8x z)v#3b&MkO_OFBHf3@Cu&1^RJ&em$PUw+~e7^?3&eFr{_#4ZHL3c-?;)X(CkToIB04 zsn0NY>g&ypPnI@5+J)^J;M#h~Ka{gI*8zEy=vUF^--}thI{h(7(wenJB%PZywUghH z)WaMts$nE+`@%{LhcptHq@ymxf*^wTI|=ZhbLU$RB`uMu1V%l_w0->4R@ZmQ;LR^$ zQDb$-g1;&l-trQ5erS>_U?++w>$1plte60ig_kg^<(HB5E|FwAs&!tloVatqs!aIc zbU`JRr-pb6U~@okPA0Jc`4ev2rsM^&lYa&CQ&e*s!G$?q?~?@aOJpGPmy132m#8H@ z$f_m!3OKR23{by^M<7*%SpF(tJ!`}T^LMSxT42TP(O5-OV%=rr*h=*waXjDcyy`X0 zc`!Gsu*7VsZXxJImzd+PTvX)*fD~T(Dw@??YlWw@+rRcJBnR*ACEls<40^Zki|%&w z+5Jd^uxqts%nnr<;9fO9$8`?blh@9|GAq@$8` zs|^`e9wS*rU~-8aOF;lEdooZ*b!LT@aQfmvH!L17CnEHBB76{gjVNZWO9SYn6izzU ztyewy(w_5gs6#M~4a&t-@zs&T+qj9fO?eD@X-xsTYexE4wI(npv#CC{mg#;e6OXpb zr@Q%0RwDpC>eJ`B1shaH2>)Kc-2WEMFIJ6$+st6m~WmlAD2*GM*!JVuRC{ zz1w@16{P<=k2NMT)i8&(2Wu=;Wr~8zszuS2*Y6<=9zryZ>R#>f>);GsBiv;m0B4y< zF$u8|Q}mfSI{yQtBplb$JqOZ-h#E}VUxK(10}{ZuJO2(O!Rj}cPL%kR^(wRG1AyY* zK$oME?H5WOsl~M^%Hi|!6SOFyX3M3C_cUU#6xmc>EIWGI6^#Pva3tj=65I{Op&+ya zC+Vq?N5Jp&UsGbre>Q@wcn{4t*a&-3E^=J~psLM4aJO5cDUn$H4?2mj|R2E6Q7{_o3QC z(gs~HQ{tr32$y%Mlqg(}?XZ_2GfX#s75tH3aRg~chc6-Euh&n54gZl}iPz_sDdK#I zu~IV;GDv(GkLg6`Z#IUAAjVE!jawo4s!zPSJa2{RwJ_>u!bZyI%i;al@>x$N42$~L z8$Wt3$CK{_a9i5w|7Bu+aJat^*Ht3EZNT1;k!1<&%3LOkI`|Vxc#+ER%tcl~hifYQ zL&u*$BAl_Iy#&)-bBL!Sr`xzV2PwxhoneouxYzSk8RUzRzOCfeOLK_Enf^VThqD;= znP5Xk|3TaVX>P=intH?*TwZgDg05Ufo;XrNLv>Y1gi2h=6wMf)_!2a41w0*99sqtY zpF|#4UCAh_aoNHE-b_WVDFqwh941il(|MCTbf<{N2nj0|K((7R2( zvkI~I!s3Vhrg4`yMxZ#^wKFODsZ~2%E-4{j&J~W$eKm%K6BKsjkiRZAHoTW)%Vd{< z&GA;9rCqZl(^ES%Y1U(g?6G3#SL&8&{sa( zB&{A|zq0nWQrobum((eJBwsimn#zzPzVldbI+o9F*STCBbl43lvD%lrl(3p|+LV)2 zf2TZG7!`1CwsB-NeIg;sZ^jSSEbwe47H2fD$&CLfn^2(gO&=0})6p~j<~2Va8lWyg z)vmc;aYlw+9|asUQEG|pUcYp?W?{d|-n4!?94eu%V)JXCR%5RZEW^ZhZX|=lm`?ZS zZ|%5Zr*(Z6RMYMB8#4fzzCGY}?Tf5lbaCpI*L0;fy$<@_X1o+eDQWlN)!=D)^0$%r zAn3U)S07Ekp>HjkeWl zKFSq}o`4NG4j!$ssJ``{yq1cmucQN=i`uSXsFAVhvXAViYvu~NI6YJa+u*g!_~KQq z=J#9cFnRb+fQvu&Ih(eWytmNs?Zsp87zcwMNau_1us8Ilt-3Ex!RDs}OYdEh?G-mY z;?y>hiezcHERJ>0m4QO;tJFADZpGJczZ#xc7Ooaj%lM~G=ygmL30%yx@Z#N~WSvsZ z7^Q63< zb-uD@Z*4U@YW);^JPD0tQK!^e*F=T8f1OKS8c{O5-|d{geoS+AX~=MU-9dhAc~XNF z6tAg3`Ib}a@}yPE;aaTuESj=zC$Or<=o`Rslee{c^|9Xy2;}7Y5t5XL%bV1>!R~z( z0OAd+2V-lI2%a5vj4i>DR^TT@S$|5A*<+NcUJ+!ryw_C^39I7kLZ)al**^W7HuoCG z>o}@1FXtN#e&1zY^DNN^tJQpYjYI);BABTDFi!Pkn-eRj-wi`D$R#B<56tBa@TUaX z?XNjL=>^qGA`2l+{uZ=}iFJfwY2yQ5@D}E(s*JE*0lD`>jJa)dksWrlvpnCV6UJj& zgraTQm?Q@F=Szq@`rHlYU(Y%|nQUgfcF&QD$B&!z`{!&_kGVt)opEh=nm-wLrQJTk zegMx8Rm^+t?Lp04RXnJ4=e|iJ3k8Nl-7Rwq*%+K*L0Va2+smQ?dxx$ z;{Gq3u=vALNZfY2r8|2JT6rVQrUwy6`x>_n1m*zYFm4l|heO~vdR-D>6t8R~SD?th8E2G8Dyo-ps86lD=CKrC=6XTbL-omQE8cl+VkBXNJpcfoJ~nJE}Eg? zvKv|QZdBl~^Uwlr$$L@;27$p`EV`U|b%r1*`;jxOf{l!D$Pw5tFOv#Dbbwc1x; zA?(XESq`G9KaR|AfT(4B(15Ym$n=P}YM#&KC6pk9adm1TgifI^ZW#Qt zG&}Z4p0lYeg;)T)=jjCjFNwZ8dr^d1Z8pPw6rSpI{FZPJY@oLMQud}0-=R(@IqB>QSS1*TS7oR0CiHz_>Y9C2& z2>7Mm_^oY?Z!(x8o)!y+6?MC-0s27!>RqYAe!4fA!*6JVp`?$|(0pdr-|SH=emK9} z$hZJO*V3lPY3C`_+WC&{2!xFYW>f^JNjmK@>x88Lfab}CyLiy@wzz0?*9C4m;{wd# zCJFev(0S8B{T+E+!NT_c`^37*sxOCZc4{fVBT^O}n|HcbV9G?e@Y$R9iyK`78ix8e zFBGtU^IU}K*Nz;rab8nTZC|c`WZQu%E>3bHS#)IRl>{8O%6hF8H`}t0GDt+9uvik+&r<2#hPlMsmu!GZY^Fqi65dAQg}_NFOzIO zMDX6DdSaEHs=v=MQxQc<;y?>}uQq0sGDtsQ$mRqqGE>~;?2*E-PBA{1LwaE*ZGmA| zs=n$u6JIn%#fT6N**PO1BxQuV$e1q85!J=86OTHzBkffm*K(CciaIxc!Vb(;A%S#l z@{IIsBlY!4wA#uLY=e@a66IE_H_!!og-rw2^FW|Nkgt>08K4+%hwd5uFXPE@^CO<& z8}UL6Jf!TEB%tJQ+&ZpmdY0oA{#cr?O87IxOkfeK1$XGA1$7UYb5WVP?@k)1dp0}JsttLSnom{@EI|zTryM@-7c0T5uTW~DsqeNnHEq2VIe;N zesM+UUH9@+!hB73&?*(LnzF^O+P+TwvMNW$Dfz@J&&8GnO`kbd8C_>^MnC$~z(H+C z^~GzdiJj!lS4ME1-F4w(X+a2;lo``N9G&J7f7^AA%=o8?TA=U>r*s*pP%_;XS)Uv= z;wYKCs!_mKqvX;p2V#{;Ir%J{`IHcetZsn{>7FjK-j z3(nsC-K^=gXr;?e52R|B!H(+Jz%Px|WReSpm5u7!5tG`M^$RaQ+ctd*{Y@~`Q(cs~ z*QFYAq&txN&|sD|6dVPKen-CvUi~e>$s&)8ZGdVl-MGVe*y}!9xxhqv3ko(3z{%3U zp#r>%>L{P3DPcdEn~PmYp9&tIsU?_ox?SinlQ& z3daZuY2qU}usF(J@~E#n3;=`!&a z`#Pd%LeW?|a;hNbt8v?Qt48qtTgikoIz51XuBc->?}twp>tsO+gWp5pvy+heCU2^C z=bBkxnQM4n=Z4tcGSS_rwAbC+YZ-TB-lZvOps@m7HU+3pIxRrjUkfL(sb#a3ckC34 z+7F(2(`Asa-kv3vodxU(jKI~BqXgS*bLqBsp8UUa%BI!IOztco_|oO7t$%sh1Rd z3_#!d(knne5RMDDr-lia>4E^Mk?{9f`0wKAzyi66=Cr;&7K&``8daF8Y%P)~h^M_J z-3IlNP058l*dlmk^MrC}n=Djc`Gs8k8aK=h)WPooUDuMlXBiw-T}uit_IpDZiT;=H zWgK_5M!bsP%oo_k4IP*(sI1%YwA!aSN9vEO>gv(c52uVmM`k=d4VS>msj2Jm>=Ds2 zO<|Vp$>jda3^dn5SI_I}dv4L-*oSp)QAWcRv*9>9h~L~PSXN)6hKTVdsy^T5fNK-X zJcJ*W_GFaoc^R-)Os;_WhRNVnY4p71pAnlnYsz39G(Db`NCmv5g!}%8U#m_6IsAnQ3z;NbcX#%)~Lm)$yMP%YW$>M_|{|KS~Sch z1rTq^a8|AKwkS7MlPo*@THdgCyQg1omSF}C1;+;l_q-kJnYwomjy>Imt?XmZLu(Yv z{M({EEo>1&*=$R_MKh&U41kqY#CSliq*}{eaEEdhJQ5uOMw}vixk)fy=ekWT;X=#s z63}F!Jl0xo8hd8usK8jaKklZ>EFBA1G*nPS;-6HA$M_)IifEQYc76iisec?-5?v@( zJUi2;kMSBF+Cv&nwMq4J=dv$A?Qc1qhSJ$9`gAqk^B!qJL|s*2mcrJ`KMd#{{{N|o1lFI@sXkopx zLKN_Y&eO9HFGYO9+wSSAfD4#e2I(y+pK$$}1^l>ehb~$Z#mA(zO-+iPK7O*U@7m{Q z=wUZ^6Z{f&%LBjfr~7eBr#27A2Z%KS{>2u--rZ|xmWs#qSbneUFgkL+=H^z8&_tx8 zFtV3$8u8H}C(%k&>th(9wM^KH=T79Q*$sdB`&~2it=Hf6r}ew1nnT$durgZP9A7V% z^+X>QD3mX`w^P7{0*Bf%Y0$?vkk~UnX!pm`zWKmmV`l|2I^N-u5VphZFkufPC7+W$ z!!`Y;sRyLj7YI96loi#!=yvIG8FiFA9&fdR)NrYtiMe}WNSf9W6qP>|d@y0!Ag)Ki zcdQA=L4jma#ffP}LiV!XZtvq)56$l^O<~%zE^|bsCR~4zOIF}GYRVPaL^uP0R(`a> z%{$4Qbft(X(@=B6RfC06ghpppKkjq{MTV#8BV(t%9Qx@coivFF#IO@3Lj{TRPN6z*_Nw{|OIJn?%;Z zf`YgdJD}Uw{Sw&o?jQW@C$y)ru~Zh>?j^lEaFBxpR1mWI!K6#^}=L!qZ9*7Mv0E zmMSQfWA(Petm=hE7S9oD7Z3e5!+@?;?wT|XSjweZDSE8F<2`&QbgPuisg`Z{DrYiA zS7C>C*rRrWG>f)yQCUPm6YdN+tu~`ySBkYCoz;L6e3(Lqh7A3e3ow?R%k9KQrfRO} z_?{Khw?|(gUyc)TAlU()$i09kS^POp78DQ_`*ltFF;IG$6{jP=ybg}dLB&Sb%`h$e zu-CLQ?ibjoRfR{?{%{?{?y8W#Nq?EDCao?MH^lTCvsVg4NnQ*mz9iaeW>?}Z|9TH*?)Q(1s zH@mPDZR_Gs3KGEg{&Cu@QvNK_Qe{ZtOx~+Oda@oQjz7WGi}=j7Tiw1O_}CVFl0Hskp*}H-kGr;W>nlcdPaG}&qK{;|(b0t@L;f)_zzH-B77^`j`| ze)dc>Z<~`ck>nO4p3Ifm)bI-|0cerPGGBXW^&mcV5yXVu3<9`E#*kywcmqC=?NYg`@0v$jxF7}jiTg5E9Wa_bITj%>ijuV)AsZ!^E|TadgYuGdci{H z`Z04gr0bze=URbt>(Adj$CuxLe4L<{A!FDyLXlYlJwA#JT~5eX$x|iqWua}o^4<9X z^ct3dGsW;PnnCb(1K;#(1}$xAO}I^U)K8YoX?ujl&qV6qw4R2Rws-pvkbWiSXNpFEy3z;isGJy3@Ls?>=t+&v?y+(N|&#jqv2kjzsIG67aD{F9dvYHfx6T)Tn}3 zNZ~@9k%kB3Tx+$o^e=FxU;goA?X`3p52b&6o)n^z;L$Fx$q8{rnE<__sT@2I$C`xq zN4Nn^LshjE4G$=Vgvu$h5fv&4yb9!BZhx0@64L$2e9@_b@UQQ~GyICW=MjN^Hu}gr z*SuzuRLLEMcSZ7nAkD}e*s`wU<;HO4`80QkwS?@1T$b~=MTp_a!0ca1SR&~07s?R9 z>EH|r*w53FMBK{@?sL=u9T;hYiy+%t5;sow7jX$3#j4YUySRdh#T7i}B_8C6%+(SE zN79ErlDtewC7Pl~L>oHcp`Baw`>+uX9yT|HsZVEaVAC*y;S3U?^O$ z99Q^q5b&1gtbzrw8wKvWyYU&f7w5mZihy%WF~~^M8-xvyyvb4#c|~j|3t(oSmR}@# ze}!XC;^x;=fX}OJKj2;ymq_cHi^7WMJym58zthPeo&jDMW#%oC+%^fTGu1BE91gTV zz&;~B2Ppbtuj@nmR zGTBYdmI<`Dfkxl0k*b418OSlE)#}}GdxqX#9Zsb9*{krashx}wM`kj#i%F5*ZJP?9 z(_uRdZ9pkx0?=sDgfkx6C!unyYcAO-%E+=gjw+M#%!Z$F3_{-1AZag9XR)#Y-SREk&s@7*3nH5b~sNaPGR8ExpK{0a!&GAGKBT96QN3W18KBW*L zu&BgxB}9lbu!#aOYzbr`(e@BK=@?i%3>!te3ec@kKWjyh3tk~HI8_!7QI6Pd3o0IE z)H=g|Kn*OmVO`=~)>m?LT`ETUz)JZ1VYTP%j?dnRp z2fy-YI|(Lc#}zayLjpnk`ey9xUyGt{69XAb5j?R-h_YH4EFQmI{XRcM;dAEzwH@vM*G{+#e^QdI*#C9*Q~EW`L#T9Y-k~eWnll5yqQ|(4_|8qhPT*SfW?nUgmvoy z|5+yp`JR5x64d-u`8v9;@!cvrB>X%aG}xcz`5`oD*?2nO2g8Ymrq74TTs&_f8ywF-h?S{;jJy^q)}4 zzeiJ;8JYg4Aj9?#yZQe|*f&RK_AHCWb~5246Wg|J+qP{d6Wg|JCllMYZS!S*_nvp| zd+V-s{`*$%>aTZIS5;TnF8sfNTg-oBx&G6wWCBylhNAW2QitvcbT=%IF%1v~SnM3- zxYEiO2bHU_T?2*YJ7eX$=A2l8pc%L2Q6aC+wdEXvw@+w-WfbkZ_p=jiK(37UqX>s2 zT-4n4noLOd2krZjZd5$=3}9zne5a0{e1273h456z3CNUf@=bCFFcIKt-Vh-NH)9o0#o@!6d2R>+ z+|F{<^#sj(6n#}A+aWMoRwMty4Q70A#K z$GG-(%(CbSpoi-v?m7h=8XD7gy@I)$X)tji3q!HszwbdF)pO+n3fK&0W%p!6gshAU zzFxqP+=;{A&Oxi~lI;&o>UySVLxg9&HXuoTP6`Ch9=)IN=XD!7=4Hywj+Wmf;h4P_ z*W&e*-&usBRF4x%h}x&Nv3h&o-Xjp?lZ8*(C?&_Fa6(7WK%!cYKxn*L{th7MdAYjs zS!M3Ji_6|J`6LWOrvo(X8zhvgDA4jA2_`-rslnS-3Wy1NLQn7PFq`U+fuVT}nn@K^Zj2n}-guIN z*{iLV6T3W8a%28&XIC}fMG;6DNVVc=u)lUfLM&->PG`pusESYm#;J-pT$bE~s>0(q z;J!9B#_ol;jZS_qt}d3m0nV+R!^OU@>?1EFiJhP0+3Y8G3wEYH)}s|JE@2NMf`))4 zt`6$zJh?E`7J04BpKiEyd3*((p;44l>cLAjK2i#JM9fsv!XcQtgi^83l8Xk4TV#MA zGJM*oIH&bFGyaQukDGi>q|3U=&$?H(>^QSuIe0UQC5#ji~wcy)4wp7g=&1Ba;FTPgAeol zU4_=PZMh+~MX`Qo)~Y1&Y2UQ@$zaPr?-Hv;N0bUE=-za(u!FK04Kcz+YwKR_P0Jam zG}E&E{LOix?PwV1(u$L;)x@_@FaF^IYzF{d7EkIz;-o(BKse$+H>jskvL3%8{YJu= z6wc4GY&tk1hIfm_9&KThYN;OFAiVvsd-=-(oVrN*RewJRf}WKy8FZufoFn1Pw~@6hJIyUr53-HczO>}3$^ZMb?;3S9OM_^TyXn+!aOW>XKpwLvGG9bBP6csU zR8mcc%8h%>?Zr(ZEX%AZqfXySwfv-4@YZiI4+&eHpsC<)I}oj7HZW&!b}&W~M+~M@ zE0N!9Mi^wMlu&>gswK)PF5LJNZW!Pis$yS&l_lQjd2L&i)H$!mie}!81V}?A4q~~| z&a`iY_hgj{_Cy0zqRsEh*dj2{Xr!`$fJ*IVgW8u?bSZn+dMm=^*Cd48VEE-r5?CGi zaAq6sN3*ji=6^Fy(PeP`k3}#4G1;?#8o~IY#c7#3Dykps9~iZ0f5Om12HV&jU9h7l z{5m8N&J{|dDb{ZW=hs|W+|yZ?HmChaB5O?lSStf+#9O6?P8U}!3X2Ho{aK~VH=`Ur zQ=8q7Uo5&`gA*}J{ABrfM5BGS?7pBJmXg^}$=szcw%VE#*3s~9oxyY7wJ^jv9%%0} zDpvC>xS6E*SP{{89~ywvS5-_k8VWKAcW!dn&=7l3I%qNSRdb1{ecF~Dx!L*ATytV1 zy=B4VPLWoV($4I$Yg%@?3RzWtGz$OJpdpeFN>L9}6qW>nQ@%#!3m&1RKbs*M;r~nl zOXrp#_x$zD+VAkW_0n-iGCJSk49TZlz=f;vl~_1cPL9a zfRC|i&|343d+rOd>QudXnhCej!mgRp;+%2ArID)Bs-ZE-DyaJ5aPZ@1_2LG|!ppR& znt1az*w|9q1xW%ZTIUDa56kRx9aoNuG2t$#x79a&b%wzW(83NoF3nW@OEPx&O&kg< zJ$2G0lUSxg$pv#em7;Qj;7je|Omv&|jPrtbRqK3ZIHdEv=DCrjCoP$f1&iJGtME1~ zK04-cZ#UZ7?)%8A@v$HEmgV+> z*T>J*RjFn&A5|8Iia4l`+2gxEx4-tO+clKnTFa)>ld7Kq@yaYDphew7^8KOktu4cde+QfP`7Q>!{S}e{`c^XtXtc#ZNQ*Rg z^C@N32QdRPXOCZjL}`qT!QOFJW~I|7Q=y&c%Kfa2iwn8!N7svy>5%?j_gE*<$}A zO~H>w_j*ME(AJoZ(sJt$;GpkwRC)^y_FMahEp`2nKztvPm4za%{7py^!OCN*vCb(> zxYxYb8dT@UNbgKfP{S38do2l=X!af<-`4!Etfuaav^_P@COZcp!R>=D(8iPS!~exN z`^V<`YlhJ?Ff;#s{`WT&^S^3H{o7^C{5OE+KN)8WS|d?E+wC`&ROVj+%}GXnit?KB zo+hDgYVrD_1-m>p>WPBAGkymqyQ<@|iO)U#~S@DYf=tgE_42AS}DH7@v|=yS`uzBm(FqTHvhn#m8;a5X7^-O@`l^3kg13HYv81Ll`#rX#@?_^W z_M`IA>-TpNZuyU{ii={^a_CH&?l~I`>oiO~kLMbtY2~ldcgX5rkMCG=$}vn|DKFSB z(FAeorXbnJ$<5@}7UVH}Xp5qf+2Ho%r>s_I1W2lx`gwR;PHsZ0qLn`x(FR z|H%%i{WR;YF4Isqcqw+#=Kh>uTL?16zhYK{wI1{6gZ<2RJT)~p}Javv4rw^1!3&o z^UP(0GEPjA1k|{fHVAkl3ExBy+5)aIZzc!DeyY0Kf;W*A+^ba#^yu%3uD%Gc(}WRF zh^tvb0uqt3imW4YV@Mq-QJ}+F2ZaJ%o9=D0=XoFBgx0gi_&_i3tM$ei)iZOF2vG@v zQyde8L49y0M60&E7mWB?S*T;NV_$^xr;S5xf<6DW5P_Ol?!0)gWgTRZQX+%DWz%PU zuv0fgi{GWmZpq=}GqkLaXhhPn_R<*Rir+l2^+6-4ezdOBdmUO zl*GPC;&h&xxr!^!i`(}FS967Qzg|9jIAV+hmpPY#h5^F5RAIjv!?V|*TkY3{p!XG} z;Q{tiMKu@Vo@J{^{M8gpI{fjw&se(4*xOn2SJC2TA}AZ?G2zjhDP4`JP2_{|IBnD1 zDGzxrdV6b%Rp7N6Oi4B^aR@h#2#&#=#L>o)kxSPz&Dj<&!Pi zR=u7=M6S$i$T2>a#Fb$u%S^s2Kv{~+Y%#xv)srzm_@CQ`ShYb{OB$Gr77i(6S*43- zZO>_mKVK%`Ig~jYbA;iXBivNkcY+#T>bL7tU!AR9Y;{~IBw|^$9+v`ncxJ?P1js8i zXLF`kt@l+aflne@&RtnFz|+ZWiijjri3=C(GK-Dp8bExf{hxbJ-euF zBsTLJ-412bXvnT0lq>eT>}h@?C~jDoAZ1P>XaGt=l zee{zwRh?8nGj5Al*9f7y5Y)Y8V>M#Db8l1{^6i2t{WutBH&fr*tC95HT@|2yVSK0{6iB#K|Y(sY(8Q}_zSx?LT&7sq(R^3#34p`!ruTcGz3RGT=O#q zy?FQv82!&b!clRWFAvzsF?dCmuPYCqgHrO%A=~+wsFB|v<3EWyL!XqOa7hxF%|x9r zcG~^bUMOBge}Kjo1j=tG8B(yw4hWnq9W!bxKgfi_wnM8B!e48u+1}>6Lo% zCq$CJ?M$2Dv1NzxMql{z^4XydK1Xh04H;1av-L1nDf&7xxvorTNUjkqqILSyx5x*% z^Y?`Q=5|RBuV<=#gznIJ?$VfLEC+0#j6dr z=hHCmcq7R|5X`i;Sd;c=05kMs+`Go4=hEK-pjitJ={UF}y;2* zecBa)^t`A&W^yu^_PoBwSES$4q=QcoW{Yw5Q1JONd{dO67do)u|-f5FaPz<`FNvqY*f>j^*UG+12 z+|=s`q`>sKQwlUpM`~ZCa9jqa_q_K5e7Li_I{vo*rLmywRF5eXoPSP(TiJij{jwb| z!+ymdHfKG9pXyQfRt-*V`M1i8)laK)q!ueyR!qbZpTb^gD$g6N*ci3_O2N`aZLDk= zP3tkZ9L?hzLyNcbTg{QRwJ5&5Ywi`K#lnnz2D>&5RIvaeCt~=%M*ZEw&nU77ddNO& zw%;`ve}W?N?~LG0*xQ!%ECE!6Fc;TOF6kp?_dD8DiQ~(W>OJs*&)}2iT;orBB-c*YU3HF&*NS`15P*4TGLi>1lP0JvzTe$Jm~71sA}K+G(|| z*<3%Gc|PzxY)d#v_G!(W_$=ge0i55w9~k)k*Dc9HSyZ3~2JMH!rUdKd%?JXoAVh*O z6j0w8M$IFSDj#SI`>AZWuV$gjLoIKi&Ghs|uf$jDyh_)#0_0JM(`l9t(9!8;{A|!B zRh3$CxwS&8FdLOPy^5eM^ZLcVB~DRJ2s@BRcCjs3ypE5|PGS)hkf4ZkY$UVo7mL97 zD88;d>US)`4tun?Fmt7k$GoY?GW)VOaV4>`HGpxV&@-@6rA0{r0x6O8 z$oHiFhEaUx;1mw2p6{XWOOF#`VuYpoR~-kkkXmca@46;gP(Bg8-rdo`SGvpM6k@X4wnzsk)?`P9iyo$^FfFJ-f5Rv7 z>;4x3G~)%;Ds>aS2|%;}s)U+=)ZAgFMekUu@OQRLC3G3(H~#rf-`~Y;o2HKP3!KII z@Vi@ip<;_8dZdnBI^9J<6;TY}6Dp#iR~=U4_NQ^GJ%=2OYYHj{mNjLD27Zy0V*W5Y@yDL6F5D;MH16jY6S?N>^+}3 z^55~NW-ku2?@aqiI+B{Y14PaKm*vFmbMTiahW*?#QjtN#$bIEez%;=}?Ub3QiL@LG zal@-nT$_3v2Dgnh3BseGN^l2%k(NrMVJH3Ftqr~7Hld+^n9XrU?|Z8&Ibbri9D_PD ztP7ti$^=ZHIng2nP@H-)A;{5%5S>L-EL@k-wuQL3luOih92qD}1p52pnlL9fm6&+R zBH|!?athcRgK8354y4ZFa?c{fjQ=6n zBf>ufGd!-KJMomwu0rWox8DqB0Ga|FD7RQY?j}f4`iEeX0Wu-~5DW*~l{IoUjrSh|(;Af*88I7`mJox~v$wj2L)^ z4+y-eFc+i&8KeLiBmo&DFxh;BIX-rYzp-}cZJXDxEEgM{tcRXp|087~hQwzM0E`KQ z`v`G(GqQwBY@8@gAaKfjH2!SlMcB4Ml2+LV{_mVVh+os!`r^=g6MIS8@N96AkFmO2 zLat<_5YJ4vOz;l_bfT-_J9M;Va!ftdy?OP=%Lppx1J~#h^SmhA;LD}$zVes?cAe-r ze{3+@0pBs>QigP!=-d~+=ja#i!1YvXzpE|lLpyTaU>2J+s7LwoKXMI**(Lqy9V zI#&X=yGJvStULh*X#+&-fjo! z8njFD-C@%RV2xJcf!>udZ(lGNO|TR06mSR(o5j@6@trFL+g{w<|C;{H$X>0Uuz(Pw zof3<-WgI2-eTvdagrDEPT0)p>yei^1*>{r{NU>UKzdKxlSaxTDFwwwsdfc9E`Y;>) zyZVOjha<_Pf4N47IzJtq#sgb~8sP)CsB{|?Pgw*tz zbsuOk8G@@L&_PQ1>bbWaFa}ih1~tJGT*37*gM7BX0#tl3gJ-ht0fWJD>Td!}@L&KF zc}0#AblB2cNOWG_wI72glezAe=HKj(7a!@Tufhyz(ls};X`At-tsCx}wW2=Lg0-bs z+ud|8ku^8L=$?enLBXX?vyaY)&fxAmP!0!!Ix%xjuInvbPE)iuczxTUm`kgE@7HtI zj(ln#UP>ix73DL5dSvNg$IsnWG>=5D304u)$v7m$H(aW9xr#<$GNYw(^Arfl)Ky~k zHJ1uX7iJob;etEImOIjT>BH^(GE5!ZU+3I1p$}kAEriUdf0z7T>0lhU-FS{S1Bh+R zURU2F&a6(2G+rJMEczDMK;0;-v^PUKId~f3owKj2ufjdy%(o&GcF*GZm6IKo?ubqW z*FP=8AJ)%e`H8a3Lcb9n{{kVtxz_+PYcfB-WlaaXgf1a7c)}aUG%zRk_uwQ@2l)A* zE8}iq&o34Sy_mdY`_$?6#`8-(I2T4255`?5c0BC>aPW>{y*&DF@*0WF%0&PKL zW=BROS{+@2B1D@+XP2*WDn3rk8>r-(-27PIm(yh0ptqDim253KA>Yv6NdE<$hF=5; zbp7GQ;J^SIE!@FOXnAZ(sM}>ND9IA*+EHqB>9{i!nrxF2sX1QS<7gzJ+PBfV6Fg^| zIYgTqFr#T@M)^Do%@D|8LKp#6MtN;IFM-?V8zl^MP)^W4Enq*fS!WhPpiRyP=E#~B z{sU$-nK~42o$T1UEQN#WFOI-^{Kbg9w??3Pkj+8C9yqoHAko57o6yMYn2?L9-a#Pc zUERO*tmRvxKz~IM6G_*^QIjMcxxidA`?F!vIZ;Z&>DuX5r`%KWz^A?&T$rH#iX_G! z>&Yl1Dxw&#bY_fNx!S;qOesSl#$gc>vxAZlb%k5e8z$JBfpFtFjI(W*X<1`9;4BS0 zSz@$&bYxuJPZMir@@|<~Ti*w4r^AVScu>@d&i>5ol-+!Oaedr`>2Lz7mK^95CJ$Nq zT0eV4+ypw=8aU^s0j*}Lw1yI`KCgUtBY9OFG4Eb|+5SLH+V1RvS?shyFZ(gmnF&Gz z0oHu^0{FT~FNI9+3TwJS1=c0xg2F&B-+QuEu+gXD9ZfOzdZ_spl?;6b^h>x*-v+dB zFFOm-Q#jQBi8TLM!}COkJ3TyWGEisCl6$FjtoT;*8FHs|5qgyLy^X$4!QG|&D(K0m zSIg{E^Bv=)_3{Y&`ZrzH{AU4lm8Bfo>(~BV2QYGB#yEI^hWsBlwWWo0L2arASwhhV`&&Po0 z@Bhtd`mba{CI$w^e>+Y8RhaRAI8FbC-u@@2X{CmQ4Ph&6*HU%K9gy=qHKZw{Zu12| ztCc=(h>*0{chN){-_)_s4_PEKx#&~tb`P-Ygs@$a6XXGrUILth*P~mnALYIzSm*ib zow3p0A2(YE**ukB8iEOxJl+~j0CiUJv{xEG(a|ANo@@JbC4ixUTQNQ}9B&&~>q4)4 zceod~l68F4>x>7pEsPaGzpN)nr^ki-yn`x3P5l+?V}+fF7GNxlTpcHd(Fl}<>L5}K z@=_&3Yp>%={QbxjdvhqOxkb)cBMCK#!+OZ&V&VFKG0F`t4hs7jQ!6zwxoZ$f;9o0` z@G~e8Q>a<)ho_RSVRlv$L6a^7Q4&EZ(NEt#JM@dj&%PDB_4=vjA+*nhBMAzUFOzO_ zCggxdEyV>At1l4p$rkuUa!v|Brfs`s;G)6c67bn=hvu8uG+Jc@BEb8`8zT+_ zChdqMQ7GQ%56QM)KTC+9YB@Ryzmqy|LCTM>x`kE2}2(twE`rX*RE&g|C4nyzi^ERxnImN9H{R#?VIPQ!mW5xl#Rt^K{ABFCA?H(+K39?)NGu^evyfd_NK*X{ zr%3Q35eg>%?($IRjTnYz$W*OzpHcHj?|4j+=8PDoLdW!j{vKuLh>QvWn~CiyT($$C zWF=syIEQr#C>Wyjr9vlom#rR5Gnht|X&ppbrIn=d`+rDMG0Vx_yW;Lz82XQ$A;X3a z+!rvIpYL4F&af@}G`azl1TeL_R?jp^x7F0R&x(_(<}lp@9fGRKcl?~7?{Ea;N~`L+ zROuJ_ZJX_fOxm8B4HP9#| zbh{F?FX7c|w^@Cf&(&|u%}X22>czztAzxq?uTq{r07(V;vV$@NpnJ3rLOoNx-7%JNlNV2ej`uda|FHxULv_TypeDkOC+}Fzvc$V7n;=ssF zkMoo0&YmbyXWg|Q7QN#uY$fa1q7uF?%0WDh4;Ncxg3m@@9)j>za4l9H-b$>mr14F9D#$2D9ta4Z8k&~>AYNoE%z9PqUvIgsfhSDHh1Uc-a6OA;9F@1ht zd?L0@=lCg%{+33yk@BFVBfL$f?U{ebv5Sol_KK5seCBB_AEywrHa+ND$7F(r&qZt% z9O$G5gcnN;^i_6&?+hb)ixl5}2Xd!J5q+;Sfka}yiZ2X6{P0X{7+EL70`$H1Nju!` z)6yS3iK}ss3E`8Ss(a~om2x`7&fcO%TqNIBj8+CA>C2ftgIy)`2!z`QO!vlCy4YT; zpxF2uJl|j(Qe_=@_)NgUA~3-g14om?HsprVjEsyNM&GiiOI%HMFfBN6AK}{ri&ma^ zfh>I%7Sc>BOiia6<`yi`-yIxePf_#k4g%ve+jn~n)AycS8V^IMFtEh9C7x`?V`#4V zg|f;WB>*;r7sXF#2E*cPa}5WMJUaO#5am|f;?m_Eyq6x zH)879!=y3*i3WM=7ImW6>GXQqks63WGeLL46}s#*zxC3INyFQ%7 z9>O%LjW#J9j$pL=$Cva0!AoFVwY4IC$`%<#2V1ZVnUjz7S53o{7vb`EhQ$@r&ZMh^ zSza9)c5N)MLD5>KBCWsL^{18QlJVO%-5znZna#lLDY~Kz9lbBdplIh%DcBO$S%r@8Rg9Jjx=u_qVgi zt_LBc{d%awvZV=t%lHLFO3^eD?8m zd`tbz@n_$;{{pVjp8Lm2ba1pc(zE)vz4))ZEP5t}e;2f4{#Tj1e_N3(e?ukzlNGt4 z!4^%?ia7Z|!LF@qsC;9}j0=%cOkiuQ3jiL>aJ5RiNnMlveAVuX7M-O2vEPr*JNL7$ zDtdp(Fj4rY-s{xPlbED`)_qsSu^3v;U^pE8d^2F1W?WR9I=9uar;6Vn151 z4|^b3e$5~*pN}`&H}-cTC_WS*-~5xp{^Doi0@;>OHPY8sUql9N-f)bnz8pkR9<=#u z$f~a>7CXf3YglSS*i0qb$cgd|nVHGh`JY+`^Ff9l>+X?1ivF!iLXLTvqMz2k=gZWY5t?^x0~$*m3xb zx0aS}o%plC-G=c@Y14eX#ATVTAG`ySOT6M-__+CWAe$XW;#BX;EnNv=BV7VsnUKi< z9CiDAu;s+iyz^fAO+s#4S&X`ZFERAM-Vt=#~BH6bgc0 z{~2pIUD{iTwHarZ;laEDw1g%FBJ+DC?rYGM=;kW;Gl4*tDige@i%k$Af$qy{{yUSo zg6?%noj#JYfSa0WY14ue^{X_u?rYw6m%!qL@mluPn0j##!Ah}C;UoYF)MCW4@O~mu z6Zv-oBfAiPWL;7yhUM?TMn}vP{2TT0EuIrQHi8|s!Xs!QU54(G4mSHxJYs5?~fujs$U&M^@#{#^5afPqO?_`Hk zG+1a`?MfCwNxH!Md$UwRnG~q!l*@hUiaoN*G)K%SqAW`VxL0hQc?m5*2JkJO8a->U zEb9k<)q@|UnzNwbe?G{{Il7O~zD`zWTpA_lDEz2(0Jc*m|CAKRXig(#_tsFn`oT?- z(N!1AN+L&<&lMlEbeu6Ta1`huK_7oua6}j%G^9DE)|!UwQilAAV4UPwHWosQ;)ujv zSU;|r9X}e+?z3#$w=1x#HiRw3_bl}F2h94N^%&lFH2d>-Sv?aZ)v6qVARn7}^Zuty zh5TIWllYM`woDNtYVS<4w15UpHD5P2t21QqngT8*UqJ@bUTT66hCgIBjxTxv9H909 zRbb0D*J_Akjn~0&YOX9SLdnKK9v~kdu%Evxh|dj3rk|L}t)GZ#+Z@k^$0$;c27Kv! zDBp<5GnS6-=8+#}N6dju{@zPsQUqmrl^QqTCM}6X3iub^ok#{Dnskcn1-J=?x0!p6 zzw-F?j4B8j(MQx6nWSyaH0$0G=IHq;vnelsP4A_=%t&IDk&E|_TLAB1YU_Ie7lh%4 zf-1MtIfX}KCodA*utOB+BO3I;-0DZxh*BRnHjb`|0&2@(lK7Q$KhDiKnK@QrXp)^@XqiqbZOU~-18V^Lc72LIZZaxz`gAx7 ztvy?DP@hd8j55-9r}RjjK65F%4?bPIfMv|UjGQzPZ%rs)+DRJHcr4iUE?6r zM0tpaRr;mZU%+Irx#^ESBly zG!XZOLc^_XE=nAe(ebubF%Oesf71yoF@(i7DuZ{Njd zDf@m5+5%BtH=)|04URg(Z@otAcIsqUJFWYp(=b0yDps(Q4r%cl{&emqpfxc%YTkUM z<{!i45B-+!`Y*>_~|g6*-1vwr#>>F*iw*zy?3X3vF z3?GO#^)JFoTH9J_`lPfrIvExl zdo)<<+6P#a-+O4jy+|o2QLQXON=pAM`_T(|jc0WXi2cY&p}}vxBF4Qj$E6abtn|0SJdSpf8*6Zc?;0&FT>-xhu+4!gzB;_g{7DlGKes4!8(_?@q(h$cw(dTJlL1 zz=YbYqkPR|%*WM?r%kicNnxjdng!GC3CG!u z97m}PHAMfImv-ZD`Ndkdd@y1jkLGcRpA#^>sl4T%x^#K<-L)ro({(BaK0T^gLSe<+ zs(%UohNd=(q_|-2hX_SGd!cF=b<~L}wQiHknv#8+{LIc3<8EXLCKjKhwzJzCe*L2SxuEFXKOE-#?+5 zjBNkTn_>A|-T3e5|F94LMr-~j`|v>Xzr2i3f4z*C3ED(Vy6807weyZh?`g~p^?@Cwy{rpM}i1#s>5OopVr$bnP>j+>}eF@^s+Rc}%m z?I>bW^Va4l!pUyZ^qg8l8#86`Cz9hVHJza93oI9$-Ai515X7%$2jV*1;u7CAOGM3= zZc#m*r0(o#kfB?X3?*Y)M9buaKi{op1~Cgb73HWNCcJH?CV+80?|5bXSY|QZWrHB! zki~zsB1s6ofgYG1{a(!2KCNhm^p5-Bb#3|uF=vUQm=jstgi54w?#YWC=3-`1@(MO$0|WF;t526G<>u>*M`+W~!}+{*Mtbf7Bn5C|fXI)NOn##Rlt zl-ydNJ6;5rprEfEVPdYgv@utO5-pv?8Y-iASWv&P=GlN67>O?KI%#pF7k&0YKe*k&z&$jm+E*?9o zU)`TCGgUJ1hjx)N*ME|E#?RutG_^j3-uAO`X7?}`3!2^!_M(fMqbqt+8wo#HC|t`6 zyxe^gpX&6LmyN2799}xKmk1q6;OW99`7FauThM{aerc()EWdUH7f1VSYg^=WvI6g< ztGiX+&h^(Um9avcmB&7q*imAA)%03Bl=_c0eWxm2oZuPL4HTZeDrdGWyUd%K3ZPys zj$+8V%AL3s(*-;c^CXjBQH26+(VR8jZT8E&!D*vD9U3$38E~zI#0VhXZML1H`hc8~ zDWfAf{J{I88zZEpNIa7nXgD!Y#l@suumL)YD5#+^;Z@ZgFQ(PUglM4Cn}_BQ>#U^G z10C^mfVaq!Ya6p&!m0h;_8qvMr(FRUfpvPnGa0b5j+T6p^+{VW$++9y_=O0lx644hb8#@Y zGi2O@x%z?;aFG2p#xuks7tkNhrg7myT zQd&$c;ihu)iPz-gdFDc8X*p?B5{mPs8@XD3uwH4Sm;7D?GvLuo5Xt2mS40;qm8GrZ z#mgf^8pP2xh0Hrf{znBhLN|<*rMy^nsFn1#Re1cInelJ~?Fcp~Q!a7#6@}bHak^v* z(i=a16375eY1Ja++ChEGbRAJU{R*vSqplnp*blT5=gtW3{t=021NE{M| znL|%8&{bD+3$ePwN#yxl*W@PSbQ1sXKdmGhNNyl0R2)IZb-kVx#5y7TVY*jZDKxT} zt56_tLv2+FOZIp1N}++`8M5r3`?&6Lkik5@x;(2#J?XBX&)+I-6grOQmN-HE!ZI8C zYBw>DZ&U%t)ebdvJ8nX7OyqmvP!l<$m_2V+=-Ka;2iL%)#Ml=7vs~R!#igw13Y!iS zFNeSuo0%`=+-dk?Pfw#CQ% zunFE9?DWA2vKDk_8|Fpp4bQ~EYc!#ibIz?P#jA?yr$#4!GC5rwnG+Mtii7Ebfh4|11=lVg<68~$;xB4kQ>DJ1(n&pI>kPg_>L6AP}t#JHWbQvvDE0d`Pu6yK2 z?nW%~R{0qrDnz$SCO@9b@mPN~0PaOK_v=?qa0;4%7IIn%mb_TeGPP@4*QHrHkb1{U z0f5i)j(PbHu8+F1$DW<8_95!TFLa0sS=RYX9F6?$psqoObNiJBiI2s*(Mc&THbf`0 z^@oC>@!P5+tkM*6%;ICg8bWHx!;|bm>N=0EMjgqno{ryI(Y3IL>snv72AX&A8ZY{w zp8-gWXA4?mOXRm6S^JFuuSMW_S}Z?Mdf5Ed`1fo8B#2i%slEjf(WQ(Y;X9Q%qUx_G z^ivG6elxB-2^JEik$7NbmyuW&x*wKNUgUKgRKv~uVw~Zbt;Sy#Z(1Z4`i7icjN6@S zp<3^P)pIbSC5Ts^x~gCk0$ml&LcthEePvk|{h!A$)$reAxHeKlfQzA*R)rWdgYf$m zX5z_#x$yX1`PosfW?k1TDanS^%SZ@SM-zrMxTZSKF~f>zNUEU!N{U=owi~)a>wgQ1 zj@K_4{L^P71DjwX2I) z(lI5X#B-3qGui2yEeyyYxo#|u{1cB7j2v31G;%#8%25x4!_Fom%nX>c3OsoMJ}e8H z@6%K!c0&X-(vIB31u*R@K*0+fG<%@54)BpXgZ%5lmn-40Jmhl?K7R|mu}x#eYdWd3 zWoPFL8TeIl{OBg$j<@ECyGnYeJ|N_8;yn8j+J7e7YRnx6ikv(}{hne8cJ-jobtqmGK(J;z_Le zBr|`wvj5=nmK4i0wo+BAh(XmvaOkoIbeWY zys1mPUz)IqTq&R{b<5O(hOFLNgu+m$iV?CM!6H}ga}Kx!y9 zr_)Ql>TLQ|$O2<}7EW_)MJJYnts7N=+*l)D0-LDut)iT&Y6I~rO(oNvuUxXT+7L(V zJk4Kt=XrkpZbGFBA)#26gQZuRL7GJ4fi@t$AyVt&@j=L1oYvQN^-_wFf%w5|I5;8f z36rl|=gDhi#?E%64COQ@v-7IKIY9lBoG-b{)SpB&nEqw;68?izu8Vv(LP0n4{5Ww( zsGko_cYk>?EQr%D`%WXXMkmk&%eB%LSLgJgi~+Aw<&?zCpY5hTaPq0Vt(PM{xF(pf zE6aOH;X9>fb`l{D&7{u{RK-Ih8y$aluar1*(>++StGyHLaR;Q&C^&xa5bJ`zBMEdRXLw9I( z5|mHU&Z2XhAZxR;x2=pPWD&K8+okU(4;r&R=0gO~kGMNRm{*J-)<5{uP<;r{hF;~1 zYE+fhyhhtMGIoAWuXL*@RelIj(ZWfivtD*EQi4~X&$7{KHt)PhlAn{C&q|BK6yY*GaP@;c@vl^ zXcq=xa8vGrMyI!UYD%u96~B7vdc6AWst^*m3*n_RT#XTdK0(dc^aSIr5=d-AelMFY zJ0^45ob|Y{#*B@W^sMFk2=V)m?jKv%HV#bvOLuzCSLY---^48wJ`Er3=U@CI#yYp% zFnjE-;yJ2cfCU)8ul~3D+P{+K7#L~U{;!vxOdoZ=lG3y02YmNZBxaM}@qp zjJx?2#F3gA;?iy}kK{Ik0^Q5rFItE1ZUAjae0h_JCy1I2N^cs5d+YgCFd=md-XM;o zE^)3N{0ix&F{XQ_`1vPaBd$5x?BzfvWb32m{X!QfUPnrG@ayaL=#RVSy~I5vStZ9| z_a|bE?aN6>0K*C0yjahcm^2ce{R#QNOG$&ypSVz64MQSp18LEv$q@U#Je{en z>Sa5pA{lOli6CIdXYUT`d+sBLzAOh7-=8Zw~L(vsyn+yn>sr*VqVM>M}T0;|n~&J5O~mTt9~-{^ajBGFa>@wznLbpGB_>Mpv0 zGdw~(qoMpZR^V{#-_X04Y+pjlRKN?bY~naJrF3GQ0h?VDYe<-(WzAU9p|&L}N6}%1 z`|fDfw{Bk4wx@~+!m}Vlwyp>a{e-i+JEn@IrkW-lW|8^wC%V*h3THXAhiUnnP!;Bd zD3*T!{Aq~xH!N{QMLxt|2p%oT(?BPI9q+R?6|egZ)d*_0>^*UpQ#2G`xsMN?`oKG? zStlz`)E|A>s{E!j6%ZcetKokoi`K4cOr8Wq-c%HcoI62kR4C1@l^o4o-VToA|4I}r zjrlSWOGXqLEyY(=sX>SglpHBX@WM1g41nAQ5_Sj~5kZ>N10)Xql|4BbBLGINA&bSn1%m|mWMz1+BmVdHO@hAHj zB`Ci(d37w{G8nQnwZ&~C_%82qW=aaU%lFh_$7F8FHq7KI2fkGM6<|j!agv^m<91-8 z8xilAl6oOHjX?UhoVZ2~T(YQyF$REbamtx-leq-Q`&Do$!$Gp@?~0ceC=m(^cA}zO z2Ibo<7^O4Q+AWgnAg))#S`4HBvo@JAg=D0+8abW}iM9t&{COQ092pHxec-@y;mR8N zJa58lI#u+NKhaBUBxuek;8wG~ef|AXU8bDMFU~@SgM`eW2)o9YM1s8H#2+;8F(K0G zv>8eudE~|{jDy(K5JV=Z%zzsVSw`Sgj|gdIhzE4pIW}iPrLZ)u06l-#`~gruq5esf zd282KZQu6#b;J7|HI1&8)vzGm@BAUrFd8h|R9KFJ9a=tB&qS#~P<4I%{CH@X#<`p- zS9SDVB|F{QvV{a=NFb02tqZ*{5_t!RV#+WTq|au*=D!&fRzK7VBM1lbYSnpghTd-J zWXU|!O>|{S(EJFi45Mbsk%3AB!JBf!C;c!v%ee^f0gHkWuYFw& z=pmVY72a$fAdWma3Ofp0|5c@ugUXlw5Q^C=>yG_UDlS$80*&$VEr&&+tV`e2ig~b< zu`6hz{>Lq~9cN(Ek|{=)&SQiehv%vxfXQA5k)$3&X^_y;uoiyi8H~!Xv^ykimI^rg zcW|FlKb~#wDxm4Wl*xAxLd@iXUT7;=MYl!iba29KT=Fk%RuB1OB1O4l8IJK2d_rk7rlf%+SmSq}ry}G& zt7ORm)p;{}l0b}2sh^T>p31x8B+v1akTWDAB~<#tSEiwnU9;oBdr}NgHUu+OMHiHl zjj+||6bY0pF)C6FK)PdS>=I3smjc6J53BaZ5?18CuDRQo6T~%+MNYi2Q%L3*aSDg}mj3v9OkfYMaSl|Ykg-*yW zMzVcn&>=}%YIgQ#r{y)nUTM&IXk#u6q;CtqkWe4(Xg|aV5o_O8SWdhpG9Zg;OfMi? zpskK5M0&DCEy)W7;5%Y`h*Nhsc66061}Y*_P@A?{&0>Y9pKFiXvD8@1O6o&|@F;1q zt*{Ao91H9I6t>U~%{XYide()W`n38xuv=J9k^Q-_))C3Sv293bscIUp zLiDz3Hnv`X0CzUFZszPK0v32DPsz6yx?u6<$vm))e(|4m(j~V=50co;H?1?c5_ZOF ztFiF#kR;84+yqu{%ZXs9rqbQ9OliBhRr`hfUQ024SFf;AAZ&CI^l1p^?65E3T=O*0 zKqbKBC0l#^EE)gEcCBeqbgRdLN#!yEyC&BY15`VV;ow^CQ0Hl__c#0T{BYS3Rd*tw zQTL%vw6ooy?Bm_$|Jg=u11(CL_gW_%&Kc&k!IRRQz=}S7l)r7so-TTSZPnQ^Vf9#O zbD{Mo<&*oWq4Hc6Ff4c~J8urJjz*WB)xzHt(-WOowr7b^&3`lyLzOKKTjZnXf|j<=EA&XV5X|;Q-Nr+)P2;?PO1HkF>kILKl=dtn?h_ zQ5m#YiACS<-TA`fTQ^^Vh01c}On!JA>DY8l37bemSw~^a3653`;}!$IRxL9zbm3cP ziO4$t=34LWsX8IF7dZcOepIiN8$YUG4lU)7%;9xDsT{7_YNx~zzsvQw-dtUI+1cNC zpBSBb$3Sy_tyI&O?Uo8>iQ=}GKU0-pZH`;_bHqGwY*fkH1#3v=YN2M^5^x~U6mpwa zfr8iPr2cH^H6#QX2wZU~ezZyIpc7;7zl)J(Dg93MjjDzb`p9pt_3i*NBnwvWUtEnM zHl)6Rl<5{!l;7#BD%C1-3mfH~M|-t zNi0f&LpA3vv90PeXhyQ0%!3m8Te)}4XiY94sGWvi=uBjGE~ zd^O0=+C>5-I@;~p1>qS&T1Nb71jbAOK*zdpqqhoQ*Q1~SDpKEWQXK`mO!1ihPjxb` zg$x(^h3>?zRfdZm{;p@H3*5RGSj*3g%X-9jL`t?(PA063RHVf5z3RktTN~(GTguj_ zM%(s)Lg1-ep{mX)7$5VqU(mxvZR79aQZu$m;X8Bl8;1AS z+w;hrJGN&wn9o#A4;SsXKYD@|Ef3c1H#Ql)g~bLqbud$u?w>}!D-z`ND{*Si@heJ0 z;RpM1&#`!7KHt+PLp7;6kH9DL`GpgiWT9szhzY9bN3dxHOiouC`}o>iC@vder=Jz+7nZN#-PMPC4GVc-T03Trgvci93gtPsd`_0I~#`GWT z|8M8B{{#E~L+|;Yuzwf!|MocBy9bgGktj$91Ojdzp`G1m2J|5Ju~q`W=zK?f!& zw5dxUd#{;OGu1vctTn+x8@->G(6}|ICj%8mF&alGq?ndi9Y3g*E@HKwf+mYG{5Fyb zpe=})8$XuW;PDhHNG3@+5p8TMx1n&`K?{P~mV#l*F#$8Y>xX~_($4VyeaH;DW`|sE zra~90HxF$9C>X?f$TiJfY+CQZDdIz+2R+bcJ^{f^PiNbs@ea;7+Bb%$2jIOaDRVHY z|Bl>^Y4r<%H_2M}ZuL+f$_ZEEc7Cx*6N$w1!~BLPP$2vKA9nOd zC?;6sMUW7(W%?F~reRN>E@?*Op;{9BFmi;h+p3IgA7mDL1Md|3S5`#lHwSDzdPsMCn0Ah8r zw4F|lz2EUn=bUw{dw1kX8Wyioj?+nNWbVLJA@% zA15g!jd3-?fGSkHH>joz;ikyToMz$5Jhj&ELFTAxQs6BnPzAxJCv(g;YaRP=6X%-| zo~9{|0Dgp7{m@$Vrp^cK3agU!(bvtwGdj{Fw0&;-o0(3=1JoeO<4nusmf{W+2U1@c zJ%!<2D)d{}gp}sucOSKTwhm5U;q$tSQmttqmpvzkD)-&3WZ&(q`?8%#NRZW)oDZ|n z7KBxq&=SRtBzju0VZuX&l5u=hth)MB-V1q8-Ny+iEW*JNn6U^yNWFO2E!ogqB zA^?o@*b#r$FYuDGM{e>sWJG&KO9G)}hIGhd>6b`j5+ESg`8zkba0p%XUF^T4P5N##YaU91|d zJxsWRqz|j)w69hKjRPUzv(!FY5MDBdUIdM6P7C7G;W$Q*obB;RG1!N9e1b%v{j0&_XI zGK}$66nzN8?s;wc|GIBh>Fe@8pWmNFuMgW`Z^+RNTVR7K#F5vsA+=4$%nZvXlI;Kk z<85v?Rx211qS_aWXn>M_Y^YjaNzxC6h@}e3^LEB=d8+<{tqCjPJXznp{ByX?563cG zbe9wKgF#>`00nT_n6s#`Wg z$D*tja#+Fb@h-9q;t*k&qAAJFSj}5Hnx-h}FUbH^+GnqFuL846enwyNa3X779$h{i zyRd~sByQ0t+cj@p9q>kMx!*Fb?=j{t$*$TW*pfBU+Vozq5o6PJIcUCn5i&1+0d*#tp(2H}22 z9vm^Kd}E1X-M^H>HwrGOtBt4xfS$A-Ru*Qv@BeEZse+{hog>T9y$okyB9X6)y6h#t zL44^$}Bm-hWY8aj#g0hOcJ zArVVP4lQ-3z1+C6**(qBfmfJZsoGbbrLM?HGhk7VDH|b@uupoLt&tXmJLB)Th;iXx z@>%RP&{Ncy55X$L*9_=7Xnj~WkVFW{|I+i*o23JeQ5Q? z;3jL|w$bmvtB~WBSbMhlXVzGL>yAC+0^fF6o@nA*8^8N3%os_>&j(rYFd82Pz2al9ABYLXfsl%AjGbBzAST)bsh%_1M|b!kEShIoO=3LOEG&>RtG;$s&_7Hx$w>HSFzUX?~{L zl(sTu?TpvJObc#Wgkx5QqnAD)7TuwXlXC^g;-e$M>Dt55A+xpaFSRZZbTeL|ws^!? zTNRQAMC$njmDzP*>QRS=T#BmdIYfq~M^U-27wf+8cM!OH)z#^a)JSNEb2;UBg_8ODTgf zClo=bZcXE7S;r|Fv;#p2MCt%hYsR&R!A0y7(7wDtJAz8*-%p?V^l9JY(HneUU-~h0 zi*=td(-DdhGdnT9oFD24a#lS8diAlJNlQH4%gT1e-X$Lu=+Q6QN-cA)(=5>e83S-R zhELO63L|epk#~SJxu3vUvCetYf4+co118Pl%AZbh6xyxswN}+F#zQIKbUfjqXRFLomGP&C%S=n;LZYW)VhxktT;h#h*&>m%?} zg;?$f-vR>l3#OX_{^_Kcge#X%v`*xTx5ea#x!{Li>LE4J11yq^+?`Rl=bd|iwIz1> zM5ljM-7CDk$ky#BJ~xN%$!G)3B(PVC8uU89Wiqv#@{&I7vKWeje%b&n)x{Ohdvu#L zUsdSNL_jsCb5Vx}9&O|0&6+B%(&2%L-kC0z5P{lemo@wM$2mvz=MP!??+3BG(jS_(_TiPm+v9_?W8#3Eh19c;Cedci0-Z14 z=WEH?4w-Au@K90N_jjsqR>~!>EHv>4tdEda*fStvi5y>cr`9zxEZ9fq-@bs;6*ufU zxGRLnyqCbqT)v!T>dS=Ycbm7DmNQp6z%m&;zRwSU%#F>zy109N*?6D8>>o$8w%(mu zxY_|nw{Ux)q37Z`2VSp?!Gk+nBp^D}h32)BFw?=(90Gk-#$nvDJ*`yJZJA6D%Ir(!1mE z-ag*%8xOBj_EmCAE~J0#*~!qvw8BLN?twn;g<~*+Li@6@Y>Ok|g1@cEAyQ$c1l1Ra zSyi@?$n-RY{mlcmdswAGNpeOL(69nj0q2ZSalukq2-*w70|M;#>I!M{)|7|g?9r_D zwF!iY;9et0p(MHEStyu6s(>_+_YLed2>(dUQ<6iV;{w|NP8HMVpvSToLIP3GcKT^b z!IESQ6JdTgkeW8?zwsPXGFN0_K{3aQQG(0Nn8&Hc^U+)!O92`tDhWuC-mOw%9ma}& z33|siL5UHS0ry+ymy%Tnq6E+eP-A9EIhe-zH=G*mi+5|U60a}>+n}~ zi?1e-A}K86F2@|Pc7@D~1^KOTM6hI_vKhZcA2-a^U~97NlmOO`&7Wm|5X)3?cu5rf zw46n*%X<)Yx{4ChgVfK!J+O;J1G*4124)tSp5fV_G4L7bAWP7D8W1uQ@^rYoF^$fD z=R;8Y&YAZ}R8ee7NLCc`h|L;x0d{~^Bk|bQmx!Le+SF@|gsB2n0GGKnQ>O<4jK)W& zfPTFxZm%@mNV50|Bx9?|DVe?hL55p!)`8OU8xEF9?bVTi2<1>Q84t`*5=CK=`ocf5 z_!UAT5SFn&cd}$37r#t91LN2NQSQ_^Ce{x7>c~g~MZ&q&-MDPElOnLwYq2xW6HrUA zL)c&5k_PI&$&`L2`JO_1I9Z(hvWD`lAwO~%W$P7X*Q7~BJ=zi$UOwB97xMDljR`Pm7iGXY1k$0 z=h-F)zuR{-wSO?;J0}g__5sQ~FFy=@c?D*eO|{{(pg|l2QD*IUB3N{p-~i#laRNIl ztvcO~I70I~Om*3PiowRjdB&QkX(Oa6;)ja8MLMu-cOij@ucBw(II zi-f(ZabyJ$#lgD=@``Zco|$iT^nV17y|tnTvyK`sM>bU?rdZ2CL;WCMQ3s`;@^gX3ujWga-3rf)h#Qm)fctI={q6pN1#EVG+UF#RfDK|w2LeqB{oz6Q3s->WmYUO&>f8;yE~l)@gj>w8 zp)WE^+afz;+Xy+DW#vn`GAJ&MM*rpV66l;($%wfn%hYColShRNH;eJj3F2L!gI9rt z%!OoQBA%j%6*V$}VFcX&%i)VO_55;g|BAcyc^rYgU?2VP?s)b&qP%QUJ`z=CGgMjz z8hjf7-VQQp1G-0!z+%HpFl>B+jrp2W-PVd)M3iw34E;L*^>odX8XepED=L@KD?}S9 zw+@bDwZGDm|E>)XdsLWUz`~(o*aCc$^4@f(nkL{e!O}jH(NAtbt5&V1;(Xnz?P8`% zYxoTQc4?1gvLno-Q7vJngJTl4XMXGp&)3&+L1%4On&Dda>hIOQTnT)1nQOuJ z-^v=A)>WOla|Siknici#Qgv5RY#045_npO*cigP{J?I13HUK!k#igutXE3=d~Q{RJ(Bu&q{ z7L`Z}NJ^`-$ovXl1<(-~3Y~$rpCsX`kQ(7Spxozk9X4rY*L$D7SyU}hOha#8iRoXX zu^r2d?2IUV!c-eQp~dX+c4;e+^7Np1)uBxHT2Xc|-7L0ene`I?_kNG`Hq znzQ^ODzaE_QwDediG(`gDyTv{HLV4|SzGTqU7p@^*DmWiL+73TZhx2f8b+Ucs;dk| zRHyICWrZcQv!2?|Y_JhfPS=obu`V6Yj-;u~*5#~gIH6W_ca`uA2d(?RMSABhDc)e! zD$h4r0a{dNaq2hqbV#|GuIyKH6vz6=l*3MZ>44F)Sf z%wr+=IiE=g+lhbtaqaUN2rDhHWNHPM^~%W{Eja zQOt2;{o0AxsG*c*Zvi=Zdk4YOUI0+>{LRT5hb#d}|MvxCyu5wjpxR;!W(~)5Lm7m) za}AIT+&hjjVoZULnT)ihfYWOSwLg-M<0tqH#)bB?4^d!qEm|)yqu%srmhSloFiCEN zH6r;0=H0$Ez1%RgRsz2zG(kc4`i*qAh_G0b89EgZPu7w@W*tFI;7b2G>@;xCJ?ok^_8-}!vh{_}?uk|$K>s6y;pZNiblGF^}vLT1enm)ol! zGhzrz^DYQoGHAN^Kc(og8}Hxq?kogPKL(zHN)0q42KlT<&!$GmZ)m00;QbD$O-;A8 z=k%WvdX)nP>A=aR@Yp~boDWURotnWZWi?fjIEczCk2+=0HK^hH&CNGkl%?elPfg^V z2}3fr+bbQU<>ll$`4He|&XjQYuplNdwggCkpuc zG{;_lr4khW*T{5fZ!*B6!bqtr+r?;06av}Nh_G@!RwFK&T}FASkz8yURW~h;=$b1~ zBW~nZ>{)ap(L@Dm&$e6rt&wh(7u)M|x8|*s*T&DaQKo?J;R;l~j;#vRo7YaC8_OXw zcaWO`-Kx$Lh=}2RBM1#5cFveLz#6rT!KaAnk(X#W#c@C`ELYJ26QisN1LtSvB2^`V z&+ZY5*E#8Lb`yux-97gQ@uBxiHlMeF~{>D`uXXGUw-fnxWE(4dd%WFa3VPW}7dUts6W z9sTN1CG1b^t;uOS`*usyzo)D0dNY#}7!W$8;P*7DugChf8!PYl|Ii|F!FS=bIZso{ zfsR{iBTltwX_8+Ha82~gtxXv;_?YJDMARSO454Et(LIAtaJoi%d>lj1e_idfx!*r_ z8#|oyZ_;}A=sWmQ+~hnP7N9elZ+XAHKA|&4Og{I-VU9iU-3o%~-}K&mUjAxxC!>3ZDCh=J> zaRr*IDpmfhWa%8T$5-MDZz=zBNb0&QRBI16Frz2u`)TrWP&nNeGYl7?mt)VT?)C9{ezQSGR@J{&BoR0xAj`=65le5YSHdqY#+F^OcaArZn*(C=@^)#L z`?vTi75=#lXL4M7S4ksnF{$C+AKrY5oI0G&P~5-2t4g zHTEKRuI_DQsE;o1Fp!=sq^w1F+w}xI8l^H@nn1pVdn5cu$<^peWg}*CUkd;mOZl2P za#D&Nz@)R#C1NsGL-~_JIwlQPu;9K(p=&E<>JEaU7kY+khxz9rYut9c(QCyZ3E(hPvnWA zUZQu@Te>dX9B|?olv+K6_HIJ&9QkyuiX|bG6dx^==xI)h#&%LTnI-n&rQmo2+_sI| zIG6&4#zy0yZzjw=P)2sfD{>D;o|mBz3JSXN~;G?sLW^= zjjds)(w*xeq!y!#S2XUMhG_i4H-eTmKy9@A5KO!|SNe}kgBA3%d zglBKR(Q*L>PsY1jqzz>1pXl4!tGzvpS_yxQ0Euh|H0$9d=^WbX>6|p&G^TKx)wClp zYc$X9d#9<}4X{L*1AMBb;0Ay~yMk_U#b^s3n-qH|v-*y=JKzypZ#3!i z4@WF+WG#L8h%_*q032532LHrCkyOs`-k{Usp+yS%;L<%ImsSp}Q`s|w7HacK=fJbe zHD%=ALHSB+Iv2pUckj?ONJFfx2$_CHfa$lY5F`Wg8p(%JVGEzG23kY)T?sAVirU+o z2_RClhQk`1>9gkX#No=^%>GWJt@Ih&B#I^n9yGI;9-`vm!$huXi+5Ov&@o~yn6XW9 zz+yI3?hHZP%9od=pCcykUkhtHkr`Ukh$ZG2lzU*dGe_^I=a+TmPi5m+N70ZDS1TXK zy|4!)sZ6`4I3QFL8W4tgk=o8TfL3CLIA~x??H-ce)Ji%AM`;p7CWNe;EScw>o;D6P zvgY@qeC{w!&4eVRW`%eV`z`R?4KLws69n~yG)vXG^LPmM65$#)Eh#t~lO^ed=WOLU z&NjM>tIk7661x~U6cV?RW|d{QN~(~tJ@};89Nm$P80>dj+Ankjjnwfs!*)K32>_r@$T9mt^ zUi{ucYV(`M-y8m$v#0=5Y1i&z$SiaYZLoo&!u${#43aKipYvdECe~3p>1+jg*Y21< zMq4w*%>26rgp?fu)Tf6DB`qG9gV91KCme&gGbE4!Eq4C$(a3FSL2GOEzJjrtrMC+vPyD zEwE(_1rV#}nMWgp_o)@+EaT$Y{xuNm`*aE!UB9ASK(&E5Iv>jHTsfQ9`NezOTw)#* zBx*{~FB+Szt6-K98y-ka&T3j!t^9^x)Yf$vst0;Jpqxth{49`;bX}(+_1eXANm>`pBHJeOU%IHS(n#wg1Q9#DZk)RaB zUh18zC<8=-2@r`pERo#7C=)v6v{oikihJW#N$xEK2py}?HC9urv{Zsi`3hymlvCr4 zGb8*=A|6bfgk(unh*eiAxHXmt)m~YPj4Z`QQ-&DtS}HT@iP$@Y2AMHZu>>feAvIm2|Ic0B4HJiiK7bz_+&t>|J(!{|cd zxTgDMwi&zoRC2pjU!Kg%;FEVJ8`J@dua9EBxk(0?EUnFlE&jRS%#3Kz1yOqYqtdS_ zY9E~J7jH;8jYm$M*WapbDww0|=FG|+DiN`PjZ0sh(}yuL$%~zar&i^R3z|slh`F^F zVP(TNq;A~iAw1pD@-{(de>BjkGY{#d94A@MtIfpedcYS7)ASTQQ~=Gh_|5q>FU*j@%ap>cV4PY^3@W!C(hyp5r(ke#xi8wIWI|c#;Bs< zYbP$6&^{v{%6m22gX>*{-jsP$%NRL!9en2aS16~XgI^r!>7<;wN`$E#wbv5Xkiz3h zHYmO23VvN1-1;A8^gabhc6(ad#ge^!tuz{b4jvvUqP){dZVR;iH^ljGWdI`!+kf-| z?EiKJ`aclo|IM`aOPq-qqMx~l|H!+yD0rlRV1T!5o`2-sSfd8G}SANX8fI-F%`3rT@`9(OrT_w+Gc zM+5B3T~Gk$UcME2kF~wedR9y@rVWLh(Le%ujoIBMUp50kUjwAkS2P7my^IVv5@qPZOv55M$=aiXQ-BVSVBXs{b~zP zZNLo!-xj85u4XfPPCPLB+Oy`ti3Mx^wmUmXX`fyy^s{)v)Tdh%Nxq%g?Fq3WrFM1bo!ou*R0>AR&T~1^7BrIHr9)aU zg$ip*35vm^8a&?=(>uTr3;M+C`Kqx^`Mh$UB3C0^V>%4~AFV%fg};fOmUTY0O@3}$ zn&5rHG_Bqwlp&jKl`H*9$i+EdvC$0Ca+B~t{)40vUULFTd-b8r@epUG%~kh7-|b|z zhP|}3ZI+l&JCZ!GC^n)hEDC7&LPmusaVZ{zP^gtsqnGm`^&vDHy{_3-|DZ60-qePh zK^=^AFGJr_JD;&Rp7N>jD({;1Wk=KTlIA(U_!De5>dTb&-b}@#3(MM&tpK`p_{XpG zh!Qjo`!pwPZaDCdHXkftRmcmjrK-p(W9z0Xo7zmn3-;h*fl!PlZJNSA0=1L_$%bCY z+dF~1gAFi@1KX}c`3|KR7)ET=GEDqx?J&@A$Y7Z6L7_g~3kyQJ%asoIH)TzJ@`{jo zoNHWZLF&E~QpzLBqLmTAwqWm_iE=Ch+Rp8k``t4AZyZe+UtFtK-T<|sq-5)H&g>neD@dAmKU0&H0Wa)#Q1qV z8BL7coN0QKo?@1gRql5M1|Y)wmY9~}WgSF!$(t*Q;e~lhr>;?II>f=&?+Mia@v0i2 z_5ev;F(Hi3K|k^`tX2@Byy=TyEjRM5Qa_LKz3b_prOb_Jc{xa_D8@xxHX^;xGg(JG ztji8=pC_|hi|&vH)k7)C=J6crs;qi<*sg|X=1mhsjIgq20sN{3P#i@WpjN}Rg1}(q zvG6|;(Kp(&6rup0_R-4xjTO@0+6zTsKPX*5Kx?VXf*75-ADfBi5|#tp$Ty2xG*VOr zv7OFrB=$&(!Yj1g7C`_UZCi@kt!>TbR$|Yk zE*^@OyX^^3PXh9W@Q&D*6q$>;9u@^WrKCBc_m0yO)to17g#CJz(^O`U0#e5W*h1%g zG@Wvge!n3X`z4}J7T@?gp(7XHMR7Y#a)g=dC?EGIRDpVJvh9l5w2wl89dTHUAO$U( zHHo~0K}JU5LfFgVJGHx#=#52P5*fxU`RT{LG8|!}0z|w4>JN2>?)@`vveACX z`&F&QOTrnk;+^Kh7i06#<*@6L>o43TCv{PXw|W>P?{h4b{uYI5c3K6Y^Yupaiu;7m zGkM-aTz_Cxt-If^4B4s2H$V zo5WG-i*jJFrmW^%38j_XHXQCsl`aCcu|ggF)fVby@ExRd+=bR$z37TNam~Cd^-P$} ziI(ir@w@96x1*4fdbQ|HeSdQt&1jfjQn;9i{dcuCQy7KhXG1=%qr$#3|A>~PhOJ^u zhNYpgs$sg4>(|`zFaS$LH~1zm1k?81w}x2KbpJ|bRR*+KOdM^;uYG&9BEEwv8;d=Z z_W=aG60^x?R4j~N!E&HeDgq~W;Q(;d@%V|oU)-qgpO6GL{v(fW>7XO~6S}g&XhDF< zoWq;ezHI67QVZn{g`xou`4cN_19q}2t8^+Ljq-P1QHY4obY-_MAjrRACj)%xMX(0#cIs9P-7*UF18B? z99dyipt@R}yenEpRNP{uyNXY0s=JzTwSGAVbnNNRY6k8bA@ z`Ksv{vX#S6;%sU+BwfAu@ji8)r22GoAyhHcGSPaDj?r!5;JO~X0iG~hFKaFJ0&mKD26rRw#uZLQL!GmOm$@Y zaC9g;*Tfs%!1S)( z0p#j(5I~AtW{!UfG?Sx;2{67(bB?Yb;W&%6hyH1gUdXYNqdiLaWvA(9uMYyzv*Z>w zkpG*ifWZPtakU`hSR2okK;A=wxDOg66*xBl67UP-1$Sktb8(<52x&1hKaygFj3IHe z$fRIRx^~5bQiWV2?b*5x-pf|MUG(vU73S7&6*j0YSUa69LjEykGy;edE#`b_nk#~W z(2*J#p~jWPy6z#8g5y%;TTz`~^Ea4kVlYc1m%d)iyRAu9Z*P8o1HNq$GTSmKvb#Rq zYcoQy90FV(Gs>L6$tO~KH`Bca!#k;03hBEGL18gpx+I0nVpFeClM7;&o9RjbR`5;g zZ(B80+*UA?z;3W1sdNq7_qf&HHV67>Wy>w@*~^Bxcv`Y0qc8WQ)q@|S!3nsNP4%Yd z#~>BIo3CVQnbiQ=tIkk3;=(c^A;e+BzQWSeOcRv^ z;ocRb(6GqSlF!gQF?}LG8M(c#UH3o{ehSEln8($k#Cf(i>|1Yc2Zc7^nD#xEBT-%X zL{|t+l*^IdIZ8`ugY4K6mJvRWm!IO$PC%GHC^V$7_*AWDG2 zr2+zBOgPTx8o;nzvNndS*UaABQT2x(18`h8NEonjT!^WQ>qkf5l^Qc9k7_5}1J`b% zke0NOF`a#%l6ah2Qa`%F-W2yYChZMor`e)AB;bSB&Cy#v60uXOa*Q2dE)i9yFTo>qvppL%>$+V>>%bIKHT%K%g@^zOBbJum$8GxT=cJ4 zaj0nRuUOQQBc4Fj2m2Ra!y`MD|5ic&TVKP(&hQ_G9LGO)x&L2>G>(5rbN^G%W(Y?n z*0}6aw`V_FEdvfO7F@h4rM;sazO9N`5w2o0K?Y??bX@oD*{GM5^`_iXH2lxs2=hwEUEQB74#+2{X+MzX&~9{ z+WF7Sk78I6WU$4Yu~ANa8Anp%QXzDlt1?FHN;K31NNY zS_eamh!FCI4Z}vicP%1xqS8k14S+CI2mlmAT>*biRZ&O_W#*CI9Xj$bGh8TFqD@h@ zRS0k5!*nTsI8tg0!c5ZZt6kT5V^?Z~NrQ_Ike~l8#mbi*VupMX!#Ao)QRE|(xDlVy zUygAP?O?=br2^=pG860u+;`|jO<5txpzY)ylE-CF1q$M*g)u!kqL;A_N%mjI>CFW) z;-q$ZbQYmUC_)O~=X%Y2ULLjIm*+J-+&{8R^mprRz0HO(Q?@aX6KU$6uI8POiQJyA z+q>JQSqGO?V|@n={Cn$$u6DVTi%h3!;G``sFRGi4uKu0OjqJKLJUYE^|3WW&k)mK} z(lo31f!0(ym4F#Sna|St=G^m=Cel^;y+8hJRU@`WHZ1>8eQ`4k`*b%_1s94_=@5Oe zn>HunU;|VzBdncUp~8m==lFh>yij-%w*cDnR$eVIq{q-xR9QZTJfKXXT?klsm@^ik z*}JxUlAY1a7pClHUkA1pw%TKIcv+IUc(4h_5;X$Tslel_1mP z?@wSA5Q%wvT~I65tlv0Zi=&@bkQUH@p9Y?DP1^6f-JK%$G6gi@dJpAGPjd`={562H ztYL@4Uj<73Vw^Qo!d!iXEbL3;wk2N0q+ci|%^X3T7Rw)?0Ch`C+1RC_B+~k}@awz$l&r$hxe*@@+@U{wE1C6u<)A;{ z9Q0EqR<*b<3;#quG*{l#FN2tdpA-UQfeA(pYOJCv7g_w0NR$j@7T~uG;6|a1Mwq9e z%J+2@FP(viPew60Ub=AXlDz1A5G(1e&{9l%$ZV>#QBx--h%vlzj9VoBWDPY#OKvg9 zk20){m;5jz-)iv(^sGVAPEmeok3lKkZ-s^__nrJ>yESp3jxKFMEZCZbZGxd zyhYWqvPn-7+70f)vBfSspdDq)ZxZ{IP{ATvAnPmXOM+l<%nh)j!ZVrasEozzelpXT zqK)J?{L9#!;gn11X1r}_F2>yYBrio2Bt3bc#U?v6Mq7C^d9Fi=Gp;emvC?Ps>)glc zav1kg)lw3~4*zgh#>FltzH$d;-T#NMcZ|;L*|vvc8=a)1j%_@#(XnmYwr$(C(Xl(W zZQIV9-#O#_&waT650DX=UezTJ5Y=YcmiRqP1EJi|K6EUYl26 z1Z*MZ1-kyKIreF?+*LkNM1!Ka<_m^7agFj@b3J*$y0kC)%{aHzss*T>RD;`!yE+0H z*CDt`BiEfcULH#(_@}mI2w53|cG2uLwcte{L>xADPzSO-QO93FdKy`MR*y*k+QJdL z0&z_lG&O8rfhCAvOL18F3B$5YdCi5_@=L|yk(Ow;i))%oB79*h_us$G=E4W2FE`x) z@8LCt<>{8f%)4v5Ybd(fO+$P)RIX}SsgCkRX*jWx+Zp7?MztUfUri0I!&+;jC3Tjn z#c0uWyt;2B9o82bYOaR#ky($>poA5ZF{}F<$+?z4sNKLbs~=M~)JCH-Ct#(f zO3E*Qs$UQbFt-TI7(fvp4_>XWl^ld%#|KWBE1%#OGd6;mo{0lDEF&{>0Aq)N60?ep z5-8Ra=(7rPDK!tUl*J~dHiNOErft4k;PqfVc%WVN5KB*&(gHskW3(<9lLLX)Q!ojj zlBj36@odsvrQ!BN5e@yxiHgb+Va#bVL@m{rN2%AAN*!rrktlhZSZ#TZNdp;|#71q7 zQIZiBN$=u$i(@myR~(QD2NpkpYl79nfRrhWi5$@=b6I^kpAZ)0HuVtkD-YNBNzlYv zgd-+MrOVWZ<|EzEuKw}h=c2dO1$~l5l7$Y$+!ih6biTp~zbYxRfj_>Caeixg#bcxMVbj!no!AFdFunaO%SFaZ!&cOCjhlZ-UsuV5TfJgo%*>xh&$ZR zv1tz+L4a26XjC>%@YTjz*oQJ&hOoS~6FzfH4vX$)|FhP~sm)d3N^S>63J-m&Rpgq@ z8|OJKKJS4kMuFM60{Ze622uCX?)X}lEsnN9Y!ibiy*3j;x0=SgBozaz zcU_}KXj2TEdn3_@VO86K!N$;&qKrl;kMC|)RH)zQ5ueVHY)E z5Dmwz<3&@7--+VSx~sH;?ZAnyWeYv!wATSEK9*wUAG4syIpW0LekWdhucm_&4Lkm# zZw@T(T(pPixCxmOi`&Xcib>9?n>hPZUjr%!g#ovE!vd^^<{$ONiG@3cTdOwR?U@Rl z7PiS%7YS|tp-^u>ZamWq9vSQXp|)Kn0Xu8ra7G@}Q(NzYV|;$bb~4=7D|_qi*voe1 z5PJ~wM3V&jH%F9bV?88lR}xig6o0KIWX}p8^vj!I(XI?`igK7KMY&cqkVHxiX1ZKT3>Eto zy{?55!_yrc{PcJH>MsZ&U^)M9JWOlm?~<(%#UJf!IERTXhXRYbA)yaDU4j2k zox@Cj0GAzD9+gN!KMcBQ~=pJ$b$eX%6;wK!gzynsQ%6n(XODD(Jm={jk|8VkR9 zjW8}}R-Nxk0@>fkZ{c|ZmKshtpzu1S-Z%ER+HMW%_*ki_gW3Y^27~i^icSfFBjFTUn zXXtruw{}ASoY9klZtj14aCg5rS{hKZMKBQljJjOea7NcQljH5IbWzXqUpPmt3{v}Cx{c-F&U-!gXBUS!dzd_ zc>!t+$cO%x!!C{@x}FM4DlICsF(kAXvLO2ajqoj2nFJK?yU))xlHRSZB8eo1BV2C0 za8o&jIW9*l@}`)qvGzv&X!gXsR>}wdf`4yz9Y@xe0!b=Bk1Mj)n&Yg6Wc%f0dgt;5 z9xtLYCpeO`&3#dt?6x42cCBOwSZ;%gl*XvN&10Wgizi&{RoBZY+xm!oV73(BQ(*?8 z)H>x!54#dEVY1~ULatmz<;))7dBPZ}0yn4EC%f`w5aVnrbnRHbQ5g!UEV=$IF4N+{ z+@)+7YLw4~7MsKBtzz=y*z6CWak)Or^Nwf57$`f^ef5Uoyc29#J;e#101&i=-b_|a zOpKumscrfG35g-Bc=X6b$e}7K2FnW1$f}{M$ARWBwtHBmNfIQt=z2mcmq~joyenSf z=}}>hnS(THgNc)Q@tDest*cgkL6N3lh>Xe6+I%g%xN+E}#Y6zo=#GS*OnF{fX|V8i zip^=Ij;?u1-=-rjdTXK9yg|$=rm+y3N*Q1ab+)V5=Hkw}C29Lz*V*B+vog-Gd6t!R ztWs_49rmKy*ml)$dc|e+5xzSOK+^}NCCtsdIF`7aBk8(ob5RioRu!jVNkM zWy@s>nLVr8;cT(GVB-;&(7NRKHpyXhtvAAT#jNH$Q&?G2qTrJ&RknbG@tX2ltuj0` z9`0bdo$%u#4X#WWE_dJcEd0yQw(i&>&UD&V=7ae%`}DTy{%Dua0Y2UCJw1N<1Z-68 z?gVn(^p@#frO6wkjZaV0l1k0JxtDvs$Jkb~`@sK}nLQ|zLRCf!c^-U1p}qe2JVaiY z1Qo89eAdF=&5~De`tUKmEvPK`xc#7i^Lz1+f{OFrj4*mh7t8C8Ri-4S|HZ^)XZdekoc%xUdjGA9v;RZR{J&6rm8L`t(Z783=H2`*kJaRX;XyB( z-(uH5;P-2XX;#2`CI97njz%J1sA%Le1s3(7N*ni1&LhNaKGC4{>G3+H{{wVu_eah~ ze1O;F82-2L?I7h~i|j5_kkJn7>theUq>($s$mxe^^ED0zb~g9)ck$f5e&m}!o(8~r zE(^i-jHw98xr5;vU}_mUn$KoWU?zZZWba2@v;tB^@lRaFlSYw}tL1l=5dUG?t>R4K zCo=Y3wui<>{uz{}QuP~GPS%<$cHC|+rv&1=Rw>+$WOaaT<8~KxZ$iLmgc-1L5lpR&mYX@-O&Z}=Kzh>XdZ%~4)08RUsLhA(yyDA1DUJA zuauZAACJ1;=LRnZ8@9v+^Dh3AyRFoIGj5C_B2z!$b*%qDdVwR8Dp|r5#msXUp8Te3Y%zEJ z(o5XnKp_jZCRC6UZ8;vz>44C1&QbvUxAO|Aaz zAStlEcZV?XTax}?Jqz1+AU?V%cQDS~X$u_3d$~$EKuuF2_t#X<)5f$xLuAiMYeD7h z6;2TAPEu(@4#80SHH*2=6^M&#Ptw}xkj;@9IIPArHHbJIYkK$z?-q>#ThC!T1wu%1FSxas22P$_`UVazChyO0sAjsgrJ zy$q@95@Wd=l5h?zRmb3GLE*aU@Jzjm(W;_2P^9;J4Vz}}`6{J?^GUdWZWV`^k|YaE zRnF^#^Y_|0QK#bRdHg$VGA1wLD?sJs*f<{!xchjb{bW zMJ*TE!N$!;Py9%Nq}co^A3-WOGYiGsMFR5bvXpJu3|5zfV^pN{VaLmHj&p0kxLumO zPW*LB^2S5O6$d4Xi31Uzv9Ckzx-B|T3poJguhnoT-(ja|f*q4ASdZ+VAies*N7~&k zCRpSjfW}$8MFDV&V|gS}A3FJfBX>B6W$S=C35c~AV2nC1Axj$$9)2Xe97>Jdq}kK# zWsh<*g5pdI?#@vRAL2V3X_w(N7nn_0h+PbVst+5@8g4dGnhGRBVu*ozQ4am(5jis9 zFncG-+J|(aWj6qHUqQJgU!;nC#&W)s?oM0j<`eBtwneUe2+$ZH zfScI0M>XYFZK(xYOEGjc_@+jbYMWM>%%0a7xP@A+<>TQ*SH44TC0~T9wsD0MdL`W2 za6gA-=~IZ4DiV+454vrdKXi5$_?laAu!>Edjo`ga2)_eoB9DICu7MCciK2V2kA3Dh zYw731MZDRCF`h%%X{xuL)FtneX@t3FueY({C5_rwT=p=}QVPYnW z*IIQBR6TFcFBubTM*O=}oI@gr|eYJ0d`GvSlP6pY|uQQaP8&Wxai(HAX^Oy4tSU+D)F z=dJF2)xr$`Io$E2uxn?$99`}ATwdJ;P7q%6pB+2N@w)2U9x>6z{j$N_6r6TT^&vh* zU$Eu%LFOq`{c;9GPjOe6I7&7U^V4R_OM;+n)2lRZT&dMibe!Ej+qVAUKvzIi_*BsBou#p&r)W*Q^1Zt7}>B?IRV}0;r<|%S*je-&;6hc%jWifiDGJl8k zxy}^UxRu}_=8SPQN`O50A+w2LO_jWPU8XJ0;){^p0TWQAjpnoF$(5bj14%Tm@{vX< z2e3OFy}#gxg%rh!```(Ret2}7vt*4UqhGCco9|Y+=F}KaIem=8^xJ_6MbMP$uU&fq zeEYM(vWv52-2?%Gij{Hci-H;oljRr4hnJ0b3CWc(w`H+&lhr zf+MMXW)IY*o*R+R9u}UHxVF}n^LO~?Gft#O5o-E0Rs)q^c<2zP;$FCSXoytGCIyvFr>Tj>?@l!-gK6KzPe z&Ub5K%y{otb|YXj+_x12LB>716t%3TT31?&i5ZiA4#>%k&@cZpb-Y6jcwQbgbB1Ul zhrQe%Z;)2ZhjGz*t3j?f-+t1FiW`*h4s4CC4-c}Kb+5$ynCtZ!)(2XPBPzI3>bBeYGy%icYimvC=oYrzl)L*u}K z1m@HM@7g}KsEm(1lEObXTzj#MXxrjbe=9h4 z%cQZ zYWaF?>{D%R0oNN0dj?-76u25J8VQPTQ;bK?fWUPq?S@OyKB5Pe>BQoRAiUOq8-gl7 zBH5VEVJWVgLdNi|gc2v=tY9ED`>O7s$}>0K+74WGa<Kg) zRj{1vP~^=TdgT%BW;H`7X$>!_2&F=z^kUeKGTTlH(@gK{*&Pp+?S6h1w(<0@4SG75 z!2D|W5j!%eF2AQAykoP`V5?E?`eHzX>U8oiRl44X)84vvez7TL}dv%%W zJnf{9|MfD0dhA|y!>Qv#{EqNLGK?j*YPoGDVcq7XDY)Z6^j&mxoT~z&{y>A*J${ge zmn0uX$}^*PM-RsgC~Dd+TzNnGp1Kx-0`58PO)@FXK#)Q`+x`2TL{fZ32+6}no9gm-DE!0;tkEF^G0MFC47wAf8>AXY(B}~PPFN%){sOOx{0`v^#O9u^cx7q zjRDBc^rHwOE4}2Lm`0F9_pGZ3v8T|TP5NvS1qCTp!?V2z~IApD>#Y13YM@Jdoqi!v~-;ndSr0 z-w?H30=6z@LMXN&^PnxME7)}(LJ=|-BApEbeWMtrVT`M$JU#fTZChE6wgV!Lp;A(J zUcZ|0+A#%^4q!pfS65AQta%huz$E}@_K}}sVL(@SS^fPKfy&|~Zgbi|8bL1(y9eo) z@1J1~DQA?*A9E^WK5HR)5ZKDvhXo_^W?r6XBd6$-epsv|qnBho)Qj3+NQQtRsb<|e zfpd}je$GOqnwt#u>ITf~J~3SM&UKmBesbTN3~imoDu-`P)U=WI0f4cUrn_wA3Q7ymF}-^g4w)(Y-t9 z9f}d2@9oT{IklLB72PcDV7faz01E0{`qt^fG^*IJx;?-#w5skdpc_QM=hJl>_K_Mg zY!FaJoGob!=b&K=zz@AdR;xuw41 zeU`Ue2Z3Jh8?$rrgV%`<#573QKAT@g$3H!7o{ntzy}PwSbl9!i03(|dr%ywjmnuF8!I}I3r>XCz z@P7$HIsbZ1WLI;(MkT$yR#^|g@MsLYZeUjv$ygF2P z+u(oOAHhB}=gjHy*{x>pW4IH0$2iqV^DMsH!|ED;d|{j0Dih3JOwr1N#P||j&hfx@ zrAxB91NTh^4SqF&La2PXk_ORfZ6Unt9OgeA3;?p|K}cRg!#LVFWM#-(xqbbJR`(PY z#IXuQzkK1yRB9~oh3F;;$hRNbfgZhUz(67h0-4-)`p&D2BCc{FAguu0H5zsqo@6m}m^86BtcU~CAp%)R zy^9)Z01gZ+H1#a^7b;VMqNyRXhM!MP_^;~QV_S?~qc&i|$(h&+O(=CLJ*kFTJ-5!m zD`8Zd3cl=d*%u1(M6)3Mp|AWf3L5#M?vN4Yh)fuJBkh^$ZxMDn1mSm?9JSbC?6}ta zMWLk0`5R43C2o>5RBT|Pse@PTEzOmFaHy9(+dMKyGUTOcZpw`8*@yeh`+WVq4FPB} zI)777LNKm}vUUbFryd6_hcXQPn@DF}YTmOrzb-yxO5QkU>b6<1G(~(3wRHnByc&Mc zE~u2H`;4+_NjO#|MRQ1L7`IR0!DTCaN-E?nQ&tFRoI+8pgOq?nluk&<=S~(yp zfeLn&t*3$}XuwreNo|QbBt;=*O<~>EP$?|&?!weH11c*{JUWQ192ut#+X7uuLRs!5 zm;iP5Z4#CUZYIaiTX_|giY~eie{o03Fc{3AP`5owbjbnJZtcsz2A(Vo*C0>63=il1 zXbc(y9FEWkEJ~}kUg0+AbNnN)v(6SkrD{7wIBxK}zX-8x{hBxTx+ZX`53RD7UdsT5 zGu)ImeOZ?AhLSp3D;7D-)&UQvgT=qn+?S#aqMI+8LFJ;_fEw>b3?xIV;CG`;_Bf(( zVeZl>z>UFVdI5Y~fCL*=_U`7?l-obea}Nii}2^BCHNHlv@%&TL*aE1@|mOVDUmWB1)7>Tyl|Qtvu=J=b}}BSkOmgm zy!zPA)Zq(69I=u94d@)XIc%;Y5+1JgjMlOIealocQoRx+K?TLBdxU|Vk5Ny@`d=Yg zmB!-Y6NkX_zZK!%DvQBV2|k6n`xT#dyF~6qnkc!@pULUT#goZdom|h-NTLLmuroiP zIUc-H8QX+xV|D-Ta~c&lZ^kS(Xcyrh@n%tkH13@GZ)(|_6I_rM^O#Y%;|3Hw( z>}AoHuou9MNtbNbl+bb?1udeutl76slx~#8DMnx;hI=?1v%60VIwq~UVN)w?*WlW7u36&n+AoUUS zJb6@L4YmXm(;YbxYr)z&Dtl|;gmuJ@q|?~q4MXWXFPkDt78>mJth86!6jqv-Lh>Ii zrC{Z-7(L}b7qI3L)(YzfuX z-KowIA`a>>uqi+CFzzUWORaiM#i}L_*a7F0L!?a>1b)P=b^IXeOql_p8tO62AWDOF zRL^2osvyoHu@06zi4k?gXHn8SNTZPWVYALsrMu%llMF@0eR?VZug4(O8R;%RXN~~7Y zPOfIt5P-DhM*d+y8IpXl>@+2lj1{%>cR9Sc_9Qli6+#LdylAvIY7l|$arIg1EjafO z+dtigxp8z{0b2rha~oT4j)2ap@bfXXx}Enliw zzKJ4s@n0hbpVz<3G0?1{u?lB=yI?7<@g2#LiqbB#>UZuRe<8071cxb8ON4%Dmkb z0%UTp&Pumfw0+`F+_-rhSYRoIi7tqQdV910H=z5iUiO9q{=D8q*+$VE&ByUJ^Q_K( zrhBj__Xz%}hk>v+*qs=xBO+yZXQczfa0xX6VI4u7KQF$JP04U9ck{Lm;}S(FAo1C_ zsgjwah}FTxZ3tV+ddRmH=)6BV zP)`b?$ZkksifuFrb=1K6KX&7Di6< zd~eWw!SV|CLz;|oI2>QotaY_xcyR=WK3vXCbSj-Pom9hxbl-dYZ5pJQ+9Y6~S>im! z74sFXqM9e~wfB&z(wxqU;$wBYp{36G=zav7Njvz-WpwW)VeJt8iyl8L@Kj8gvqphv zV^%A``0+Q#y7Eb@muTA*R;ffLqPgNS#H!r{t ztNt@5ZrFd>d8MEeg>HzdV|??jv~L-#Ka;wPd<)=tIHz}kj_Gi^+lDWNi*LF2EwQIQ zgPNgLHjSs-L0rcqD5v^Le^r-SeJZswz9d`duhsiz=x>0|9qD z!mkwPQ8fW>VOAo^JeIjsaL}~PoDNoGRlt`P>C37hsDg2HFz&ZiEZklPlB*q zuewuFZ&-3(rkzKfrcmGNm8tM;xAgbH{9j zY{7npR5F~XpJH@>GU9G+wPE|IB2&lYyh{2sw|ql0k8T}nBjhizeWtN@X%)dd>HVcH zp&bdL#ZFA)(>GPgxSV72;gRuM%ZxBq_g4bsZdr^Mha|$=0PpjvykDr_G11dT zi(|bgihmB|KEZHjc1S-2gHuX3;(`JJ_#p{CtE=hg@+S;qooOF)I4KCZRN%k!7o!Y| zVI|tiFr-E1hX(xSzBrj;AZ)=YXJGITBCon3%aUaay3hh7_{DI~MuAoJ zQ^!32<^95(FSvuR+wwubk;9J(xUL7K2$n~?B*B`SDu(WZQh0OdC%qj{#i$mhl&uDH zJQtVH2dU@P#sMT~$NkJEFCv(VU!Q?hr~p}*uPYsydxV3OpfAIdDIc!10}7LpK?O-( zyMd1&Y*LYX2cVuJDr6V;bbE2dNrsCv_QWwb5B4L$f%9MQ**b|nU2|WO6WeILBLRx- z*CXB^s1fqZ$DoTb;S)Q+U8g?}yVY}>om{`Yb+zS!t1ELh&Zf=$)&7Deeas@PX3B&#$Q_ugxE(ZF42+RK$?%L4Mu%T)}_Fk%4J_3?=)4Bxl2VRSZ zy{D=2<3aNpEfOYx9CjPonu^WDLZG9P;Njty#Sv0>XMe)F#g>BrZfBih$ zcSop0D$ufQ)%R{MX_~l77*R7MR1xljqfq36t7Z@L4l091&2`ku{2YScxalWd|fo&uU%1a>#jsa@9j1% z`T$3FyN3F#hkt8PV(FR2;|sI-$~;&84yf59=RPV)usWB-o}w~w}pJ5j5NG65BhxSPfx~!;-G)r^|2&Uh4)#% z&jdnNL?a!?UnTBS6$(ExzhHrev~46%O4(^YJ>(-~g; z4og}sezG!D)4Ub1yrrGuT6H>s*EOfR5%L~smNB&_#dY8{jY>d&%3U%Zxo2r894M&= ztuD}2Jt+Oq|2?)vkdFiW$jgUi%X9vIcOQHW3*p=*Ne^WQ6oWjkdUNg;FUBcf-2K}) z<)oW4P%!z$X1b4w+Q_$HwBbO_j=2GB1X0+K8}xYW-6KbFOeTdGa1zUw-wa%=??%(hLOuaKdRY*;X9#5U^Y#d2KBE2FUchK{G7qz z*>PO;*wi21!X5$6G4EGvhUN+5%qIbOXdMaCoi@Ycl+#ThA`s2b^2LWqJ+HJH&ru;cHObOBUTTX6? zU_y=sv$r<=dRDAJpu^FRRlptAet+eqdXs3a6F98ebm;i1i`*P+&2Se>jNhp0f&WzL z)|5bmK6*@ea9v&?;~b}MG@m8Ojwy5-#p<^ny}%4;ivBT!(y)lK?|%pu8J;NJ*?@1b zWN)1EFzpxIFtlz-JtacjCjYbvT(%w?)aka|SDCB0|#u%D;D7h^aI9%7KX# zME$~0YAlF)Aj@CNUgZjCpB79k8V`IaAeQuEVzvoQTKHWe%XlsO1Y7sm+nS;NxFNWz z^j>=|TKPk_@|5KAP2lEiv+joL9PCSyVM8E*yS{_FrgW{(a1%P*@2v8q($%gybAg0| z`t#|b+TKoTd#EbU_nA$19UL?m^gNVhx6N62P zHrjZmj=j=}04cNIvPEq!gY4S<;-|l{p};r{`w<#9=ypE@gQOESEYJx$Y3LVo#2PA? z)e_P)j7lVfIzkr)nHCLd20mJ9Is!V3@j)Y!T$D>P{9Pi?<$TjcgIyz4&af>OBe^f$ z&bmMCMEkiGSF7AROpnn$D%6Q$yhdV`GpS09-lE=P43@lD<5hk6O5NP_WmJxQNh`*t zaxx;Ugt1;tBdoPX;NUBqi|chcE=irZ^3arT!HK!fsk+-m(fS1jj1T@qdyG(LK~`94 zR`OKaf`L}6%qq0atM34LjjyF2xpVlk*g)%VxLT4{9ZZPyFzRe=i5JCEp$2{~TepC- z5@Px*YYLpBN5pb}mnTM<5J4K>!d=v)whGm2yjNuSM`?Aeii`L}btV6e6y&G@vFSxF zwmrn0Gc7+r-NTCLmy`$@T(n`g0BWKJLGkB~6$a<+*0s>XmCr_*Sxqmz%{GE6VD8T7 zRn*#>F>N;FhZRu1&0P`r3>lnaJ#rhU4geD11o7k?%MP)o0)w6E$qD!P2i^}p+nq~X zA5|n(4hvg+t2(DKfE@g7+1DiFbu7v{r_^@|>l0gF!BktuJ=poN=;#8y$|2GwXN~<) z8V)X=;{&38qAfm$)uvqZX&$uAuEKiP1qm=;R^FdFn1wNCDMX($J8iQrJk~_IRqJ(o z(dd@C7j0<9Jpk@F?Xs>M=%ORhK;+fF^;!2c{oNM1j|d4lwrd854m`&zP74rxwfRS@ zUCJ2H{_JLBpz8U`gXAg-+c|_vpEvZtLQCMWmOl^n28vU`)l?mW8@xVr1CT(^ec2WoR&YD zdUDJQ;(NOq`r!IR1-+g*(O`T#<1B-)-*O?lfrLotDLXBudB`-;|E=uCQA-c|kTq6+ z7Q0{}Mt_>O+b%7N3b4l_7R@HjmtjO?#D?sHh?kp@4pMHYIT)@-Yef2Pc~~DXg@-9j zxTaa8lx1BkO>|rI-~Fo8n~3ziUx_#1?9tnz2)A#M)+ry?K6TVA>aw-?WW&zqSiZKx@G* zTRv(44F@l=?LYY5-px>*5#S1VjZoDoH9Z3PIIB@O9GhWFy< z5UJWC;lR)A3rdL$bQi$b+w}- zg2*;D+CY5~t}S;IJtX3wi=G+mnQg4(tjAa+9pH#E7%q ztohC@qsMvx@iKCN?c;WCB>mz1?$_*HIV4d5;CBvZq?zq_Xa*6L3+CK*>P*`~$2miB zF9VV9#!@N-=MbjCtEk=Uy}P=gF2=X}@tIBuuV--?edp(`Y40yrri_Q%*Rzv@hkFO( zz;USBX!ad&YR#yiL9X5eH`Yb2Ilz=LgG#%F4^4Rb%R>N2J(={DWqo@ilR~ zZZaUy$pWBh6U;mo{}*~7&_7pDJ*X?zk+5enQ#pH>6Kxb|u(kSyOQaN^cSP10Z=sFO z7_Q8Zx4QTzH+tU8c&rU^`%`+`Y2VF~A|L`*nJ32X*>-wEYO<4_2ooJ%oACyTO{R~5+IbRURF42*d<7gy zFr?(HAL!9^4P4>A&}h#!pepQPEETeeJ*ISbeA@ADd$UhT^)%Sb#Gz}C&lLr7rQHt< zQ3Na@>DfvFnZd+DTc`@t6L?3cM-~#53&Q!4Qm}Wm9GfkYV42e0t(}25bU)K;zF{7+ zwH_WgD#7fBX1rg(@`besWz+%%rBBCYbs1mp!R=(wp(0BV8xw+1_k`baF$e^+jD=b% zhx%kP7V^!=-AIpT?7OQ%MVZ0~OXM0OR~i|!jBG+pcKyHvM34#T?kFZ6g_STin>9;$ z)Q3lhyHFaaa>=2ECo?uHd1_oh^Vh0z5r7O5%Ob~`&?OiWn=AC0)78IcyG+P8WZgzG zN{A4fTT~HJK4{budGLXeg=~RP8fm59x~bp(y78ImqeN8w3}qUTWG|o?zl(>grZ1+u+717?&ag@p9KQg*RzG4ozU5^wyWTKl6YLAAeFn zKzvhrybF&!9~6D@Fo4Q+rj^cmi1y-m>E|uQMnyr-6a%5$L*Mjr8c;xvn7)55@Rb0$ z_!Qa7;;4lPKWP7L*A&@ zE(aXxJU=c3BuK8FSnBHPE* zH{yX_z0x}Ec}@-N-AIkL)Wf?-T^;{HNYOO71JH6Mtlpyxh_^1gbU)DPcOB1kGxJIb zOvn3Ri)i|lNW;(%Pky*E^gvv` z0wluB?bzo>jDceEDO9W!{#BL49{0CN4$954Yk?IDw4i*FD{Wa@@@RcC6kZofKg z%}4JB&c)*6xb+0w`o0Gbb4yr9bCJq370a&GrD^DEWq{dZ>E&5iB~G-Qoo#$K)W9z* zdd$JnV4}{jE6=Y`c(U!aiyijz)722S(%$Jcuat_6X;@OOC{-3D%jWApm!hRhsx4Z6 z6$orLYyxmlslt2Anaoj-?@`V?D7q1mC$fI`V(0Hxd+&srw;I;kZ$T<%)~!X^w6>U7ZQ8#iLmqkIDrhkGYr=ofi`qYO4;uMCZg}E*Y+yR4QOy zfjs=h?=PDxxIChO0*!|2P!WuhMU25qyoM8L786d$Ov7ySPUw+%R9A*Ek9*`_3-`^d zES7CFl&ROHQP`_Q3%W^f{m;}=zRWc9$V=?`7+X%%S`L+kSBr1#fx83qcU)@#kIxzBd^nj%(e>sX9*rmS~^ZqOJK#0NV5s^8n2D9O#l@q-H!TP zJ4JUdJNvi1ir5eWvZ2jOB~h{yCGohW9S+lLu1?MNM6G5O8tK8#Fy{- zshCNi_GTj_(DJv5{Tpk2#JW=au=h5s>;?!{HgaKahTP}#<+v4Z$nnsv=!UslyN9I- z|EV*CH3(gtt-mhwfsuY6KYw2eVWohA*lQ|mp$!;%5EoMpYy32!%-)^jTk?fV`4evF zZ0+2?gR5zC`BUe=gKJS-@%M@uS(ZJYZngwfZQto3q&F$Yokwn7RSBo4T8*wy#@Q1mIW9xYP*f{NY^Lk31SW)+O3wk3 zTV>WLi)6gwbaTX9YavLoowP3ws2b0z`FTAQiU{ax#N}wKx{h;cWG*5mA>=?_spSq5 z=B$ppNOLD@4mFW`P`sxc2ESMANmK7;2%&B;i}vwrJB`rlYHU!Vg?XGW0nKjCdL2dy zI8v{UyYlCIImcxxUa8y?&t9;WH(0S0H>4qA!2={|Zh!?S(G&kZ<{B-1e0_lBfw|yN zt9v0wNG|MB2!#LEmk9!imYFhAoBmSklQC(8hAA9a6K{TsHhrCWq0d^Be$er!AoqO+ z;(=3!2iah|P~pSn>*BBjc4V3u#&3}dZf3d2;Dj>^AvEy7U zIfMKA-1~vozyYEzrPx7J)ZIl>)Y(M?gH6$QSGg}__?0iDL~FQX&;_D`=Ep^&mNQ6Y z_A{Qe`)`Hr+rIgn^sT7G+--rDmgT^Vd8Y<8^_uPM&U~1AYlm}evlqLr2AkE7olBJ8 zY-fQ#$tWGEs^iE#5qJ@|9=B^dpBwn@b3_WoaU-~VV+7y%5-|L>}o zf&M=RkN@+o_aEBL|CRe5X#5Y&iZgK?u@Z1FuuJ>DMJq`A$s1t3(*F{zxW}SB*uKnY zQ!_>Y7iT0t-w?!;%r2PMlzd%&GL?x1ZJnDx8LJz2>J;nvtbQDXoNRG6=@{3X&wsu@ z;B$6g9*t0RyCkq$Y%ks6f$ZJiA#xTZcMb|eb<^21F!)R4L}T6Urp}mRIBV*TN8lEO z|Hz*-zSV}@w85Hu)1AE^LxakJ!VZOkKsPcoKxzCXqR%Ax?hq0VG*;?LwmZ?P{8lR} z-y=_WfT^mVpIUhZXZhE%57JQd*I^$S`53ktygxGpDl2v9rUOl6=np6u)1&A`K3o1O zFSfj-2gE>wkm3dd9)7je-`>)ghO8*F5Q*RMI0!@5Mn$eiei{ZWIdu>v01q<2inac7RF0XJi_c&h#IU<0EfAX*Klj~we%2-&sCH^QX0F&kI-v^qw=bSgJ+ zwM7d((6Nz_PeT3yx@2vhI(e8)m%vmEpBF*xFP7XvbmQKGLyOQXCvM~O``dj9BCi>@ z*Qckfv|4IR&ev+E&ybf$2vhS-;T$UzID_Tencm`G3Np|>gcs}wDCO92w%H& zhkH=A(;S~Q=XUHp?(^c7OBKq<#>%88ISesAZ|@C_F3r6SO`X`U_&T0)EO{oD*pk?< ze>=e8BZfWEa{Hkd86PCAL$eV$==_!pBV=BfOqK9^Fwx(!-_xBm6y>reBi=Wkm2*bg zyIB7SOrKx|avtR_i53BZ`ChLje-e*ZqI|fLHF7wjILng8;SY$2rty$WF5(hAUTlz= zAw0Yctc7c^EB%l}zg`8aihMjRlsT2cdMK9Sx{v2swHOH(7ZScpb|gu6>|iY1xceRJ zjSoF*L+Q<0k-(8*E5GCG20!BynG19VrP!&n$m=N}(1$LnYM}TV?q~_(w2-eNwZQMC zkO##fP!4+90yr+qOp09Wp)OD|%48B6ZI;#O^Jgqu`t9krx#m1{oTFSA4x3V7A*j!k zrJDKfoGxcrOd@vw2*!Q?1? z8vgZ-$Oha7u`9oSW6O#Ra-whA2PL7^qBVl}krZ31>BDIc2lDi;%^%^=s-kd;{v{XB zllr3!Oq4Kgn+bjHXuk(Vsjc;=1qB8Z>oeGZPnXQ+vE(d+NpMIRTtT2=Ab{F_?LbfV z_EjFnYR0Mp$?}li@q41OsAg7wKrc%&BHLpN|8|a&Lg*KHr?XOwDF^FUeh?b!rf!<2 z7-c*c@}ose`t&PU6HVZPRf8A%U6xQ?o*S)3W#oJ@C$Ll^H;NA1GslOVKg@27n`Akc z#7hA*RZz|t0J9(Kq^VGGN+mO{|I}>AltfSTeQ4tm2KO$ZQHhO+qP}nw(V86waU2lo&9#) z{a)-B=ieN2_8220W@e6-JzHY)So7R!pf30h_fWPW5Jwi8W@| z8?-7m*?Hb*yI;+Sl1C26IVdDr=6Dbx?0Null|b3RH~;mf`EJCE7QAFn=n){#>wR_a zUGI7x%4FS})phW}c#KY%#(T>(sBwCX9+-GOin`*4vG<3o$IKMbBZwhK8V%JaWG8Bw z6g$)r_n5mX`_b~{H^gImhMXEEZXox|0V71^Cb|R};ayb}4AqGZ0X)pn_1#P#FZaZs zFLq&W%A2VRwf@m=#@dKu+GbzAzio-{9v)_8!@9-~I zfumoPNxO2h@AJh6@`ne&E1T1w8$|XOltk=YL3XPLw)gtZrc!>J|DGsw6DJ%Jwgv^I z3SKce10*Mb%rO)dP3hAxVTva+Wc-O>u@oXG13>|e00s)31#o+LO@2Vhe&(OINiwlU zYP|}PM4#oWNkkEqHIXC&n0i1^6Ns@=*CGDUuom1uK?L#>L|6q!V_iw4+HbAF{)}1$ z$}i+PBlDck;_DUqV$k6#d!>q4BTC5RV{8A#PXs{=T3l9~#vR>Cz@J4L z>N@39&stXpP@?m|Qmht7kXe{&is^BFo)>LUsVeJRHe#_j7@8CMRHiH`iyntiO*2Ur zHue`G5_faOyon~;;9A^U8T~P$cVhvkIW@pa`yrWcSNp(i-iU?#D!eDFkJv7yERBm;3R8{uX!UezM0l!p`VrpKVraQz61Z&K;0)dK}d+dgk`DyR+Lm zp->TaeZEz`Ku@=-1mZ=lqgHRl%E;_>SKX>l7F$PULF7NPN-f(r9&SC4LW(wS+!t?B z!+xiD1pU&oL=)B;3MniGv}yEpg?Iv6q2m9_khDoibxB^hgnHrRo!cLyct+xQZQTqT z5s;n#c!D${SYx{j0{@T8G=SL8BmB5bJ~Jsd-mp>sL{iNvnRdsBSW*t(&y5p^e+RBh zafaieBh#7(wUt%p`4!J)#N)&nBVw-6^+);NO!ibzUy@0>5G2=FE zu5HmV=icn?eEWkTXT6+?IR|slU;D9`3)Zar>beWdlDDDtQu5_$#s~Qz(iJ!u*3Q8x zZu%8`{J0WR6^T9EIITdL^Ptj)`-%DOR8-xox+zX0`!o#vIjiQY~ z%D98~@3f)4A59+@c`^EbCulx*cE0UzH|@2%ob6j!G!IuV;0X@yFJyle4$QCchvfp} zPDP<20fOpt-pr|1Id*xW&HJBN0M~R77|evn-miJ%LApF`OoT z8>w>Wu5e_;o_OM+V%LLcsQU9vuvdskoW0 z$VrO*?jPK=b*`B&=d*^IUz#`};FW^%q=>M-BjVb6eX-8g`C@v%Pg2Ccx#Z>|;gIw! z_nKV=5$Y^z3nGB6vY@0NUqU%!Qog`Pi$l=%C{r!LuLm=ZHZK{uc?me?lOF0Klsf}1 zV{<#<**sz+^?i*(F-rHM5AfkPHwvXe&oVr2gxK_~@;8@`ZX^SU;5W2CkLXGAZolI8&tJD$H@Z+=u9%evT9_HFUC&(3vedO-)Do zELw1_|T6K>- z;v0~WuZ?Ia65=Xfoyq|zr&=JT=cqBnVCC?6u1a4+5H+ng^&yHWOv2oElSqjm)4#3`JL6%Xe@e0gWbh2NO@f+0A!(iR#=)F)@d_&`klXZu%!H$*;5V6Cno+H7HYldqQLHF&KHE} zWWy@$+E#<$PWQ83T5Yb2vs-fW9k^8Ms!-D;Zz+{DRci~;o+Kj4wU6*y*5pF62qc7^ z320@+97${-0qF0K4LZS&2+{(6t*i#NySWiA(+fWMC49G3mXb`=KH8L@2C{qqNinJY zibReP3X^y%CNKEJzR)AlBwmYvoooe6etI6&2BVBTuq${Nx`@w$1a`g@->uOH0MI0qZn}VeUPh5#DLE+_rzq zzzVg0_PYqM=x1Abdc-4&Z9mhJmWpvZ+kV)j+TmBa)_1?XTIvXp9!C9UbMtmpZr3wl zJXSd88kx^K!Ey@YM{#bM$e}8;r+pDEpzmIezkg$e?QGDw zi-}J2xVn;Mg;KJ6H#B({7|}y3Ver`>@I1;F1Iu;^1EpP#8 zJiz@Bi&d$23sd;dkRJMY2|Fc_qu8!|iE5s;kn>$Cj6L}SiU3>iIeXFupINa(<5-Z%wLE_#0W^(_ zN}fq4IYf_HU8c(#|DH4c+awvMwk<34^`H{z_se!LhWWImJPI@Bx|O5{r3*6w!R!U@ zCND-Um_F2}bdka7Hv-B#AV4!7U+ukf8-@YoZlu+Fbqy}f4x0sJWN#9Smx2Xk1FAJ0voAG04Hv_U zUXu@N7vW`~#HVvN@~*>|_zvvWnBeFbwdsECC16EpxSG}sy%YMzsRz|3T#_fC;+yj2 zD8it9N&D^VDF!u_f+SJuW$Z$eQEKFrj_Ll}kO6nVD2m|{r-G@oT&SR%dzd^D@h+!) z4K^um_vE%h@OGcQYohvu{RjJ$lL{^S7=b8Ax3qrD+*`g2lW6lG&R-M#D{ryPkjxeT z1FRNCHsEu9{;Z8Pk{6uChk6 zpnORIVfhSA3J*O^F>AdS$Wp895JFACR0@2qvU|@NK8DKdr~nRFQI-&z-^jw`WlzV_ z1Z4e-vp<|y5%sLFb&y9&Az6^;f^)boG=GZLL!MH(5c2&@X7A6~ zE>@v$>8o#^Xn`~o4UlZ+I+OMEwmroam`)Ga40)o!DcF;u4&oUct>HHicHK+v z!=wd*Ya?kMQX90%oNj|3mzJ1h-un~3R5x>`ChI)Z(JBh{sX|sxxzNf&nvp}OqCb^y zd#h3N`-ZBhpk{n69*dsQ5XSJ=ii0$GZTLy$p2=gm+cYUz8*=SDkJdY*0{83h(dXbb zvtME2*`iiWi@MzXVo%e-vBx#{@&P4&75M}=Ei|P^UXUeL2GJ@hjNTzvNcv2 zDVjTfo|pEqw_99(8zBUkVaT5O(Na778)#JN;lG`{rKkHh+nbS&{XZH5^#90F|9?m^ z^#2fL{wpfMMgCYQ?8NCH{IY(3Cci%A5Xj>bDkiuYL-PPz zl@rK#c;%JUyU+KY7i$ACkh~rueVGR@A1}B*o@N(yeKEc~6>1wZlW@*;=(hFuA1hWi_rXZaN&i*GPv!Qy`JOBD z{Jc1;ZQKcOrA+=VhwdTJ4%F$vPrj5rL=hqZ43nH$B#y8nm(3AHfGpym&e*ziv=qfL z?!qr~f1bC_HmI$0%(qmOzbfSS=p+esboTsMtj2IR6bjtBR^%kvWLH6rpZk)aG62zTY!YXA^I?WBKz=K^ok$Z}nVW!LSdKB#XgOvD3d^$^fPv5M0wXFcj(B z+Sj0jdxGjo`skP>5DFg)S{6;*Z*Kg7j7xDwc$}zRvB7fNjT8cj+tktag*cy7V)a*> z#TC>DC!gWD8sB^vQfv@X9!?5`O$HQslvr3-t^P#@RK569TL*?)s{bq1g9|5<6q_1D z&y%1bC8|2qAp!TJ5Hgr%@Ad+ZJoO z&TiKG?+*s!R>MpZ$CV}VJE3=SXc>zRovHLaK3R2xo8%KT2Ov=o8TC;N^{&)Jiz7a} zE|z|U<&?ph7J?FlonQ>miG*tH>CM$fi@B!IuU{3sWBS}L=Gm(3$Sn-%l(z8m0H2Jd z_u81@^EwI#=v{tplA#3M5lV>w^-$jXr)|Vp!P*MXLBRGZ4|F2E=!R5HD2{=)`QE_w zsxLQD3z68YAkN#sT#q2aUZX)~76~v>Y1)}Gh;X$)PB47Yk6bpt>G968diR75Hz6g- zt-snL`Kl%{+>*k=>!;G(I9lR$u9Fxmv{h^ZtQm-4uhk4GCXn7kgQC5eV_T|^u1x!vT(~+^(_3OsV1O(FLElee9k|gEkOouCKW{*Zq!g%hRph zIIN_QWmZSzq{y3Cy);!ad&k!lT<}@eXogoi^ z;rqijF=Q%0wTt(bKSY?VuQeDo!{r?8R-ZRe$q){PHrJOMQc4KG=@!~?-b&5q>JoU( zt$4ivtSr69eweg62Mi@sE~Y&{m5CIp*jWPtPSr%zc0PZs>P zXLXRfU4XHCv46s-JPpiLrroI7@@>s=CO znL}OT5(R?JK}7R=Izehct}_mYH$1Qt+;rc8h=~e#pDx!?O7XKFe~#tbNo)Gf?0*BG z?XgS1VdM~jM-u?sx?{Lec>fSMB+1R7>0QaZg<-ZE+>;krBi$R5jI?wdm4ZUVsnE@) z;ueq6tB%a^#1YKnh}L!!b}gQR7@K&hM4D#ppn-V`;y3BEe?3Sp|a9~K@feLY)H#FQTE<=8_y`kvs5p??f zZvdDy@R0D*_Z0o?qNSh6DhiQ_6s+;_1}8AMF7?-@1A`FyW%QN#`n0I)(P160j()xL zAA}79hwjyq0?wnEVfC1AnzIsV+iGu7V39u%KnaV6)Eee=H0#!72X6)iqEQR5@Q>;3 zvL25IWGg~HYiI-?Na}!8FYGU*k=pK2YldhN8|_n&*v+&AZ#$W?Cu1;8T5WE&sYCU9 zk*1g{IlbQK@Ey*?Diri#D2ne)ZZra8vsYH`uVO^%*q&1-b?;6QOzPAxfclx7-}|dC zn%j1IX&0BI2^<_fb!bESKF#%A6KSc-V=1cP6JwVd8{3S768c1tA#;>;m4W=hDPgf> zBUl`XOgvubK}vqKF|RzFT928K6Aq;^=a1e90AvvZ`3M=rC>J zVDKCzA(2#Wh6dLg0he%p)hY`vQ#EvB9(yz3;`E@?irtiLkIa@5z zAv}dYjbgf5KGr=oWQqZgC06DD z0=!lKC!i*&Jn#)>ak73XDj-Zo7F&b@=B=g0l7&oTJNc@a;ggCqo{)z@7-YmCkQ;z} zZEd;P;1jfGm%!}O%9C~r9#w(!apu&Z9i^e3M(A4IJ{r~MeS_ON@JPBL2aQ(mS6$jS zEn#Kc9Zh9)*YyblbG~;wxM6{x9M_QdnPdr|(!>QL8TyCmCov$7niTA0vL6i=qfkd4 zEc8dK>^{2DQ++u3zl0Jh)u0E9luO^H%mSCs%Ih=u92wUMb!Ri1MvU=Vs$;kp@Qj+U zk9Vs}__HxN08)J_5I&4){tD2NAWAvSZD@y&?4 zC5n{2N@DeJnP|Btrc|j1To9i$T8>^u7<%oAf%UNxU}o`=#WyquGV69F3#uL{YGmE z^AFPL{iLYn8BuEU)!s(IYqHrM{P23sCk`;zg>9`zSqM7N;_%Q ze6!VWf5tDGY~XZ4aLlrB7--{i*IXTKFrsNLvi_7Rv}x-!8PxP186GWle{#O8k8R68 zG;!A|OLqAU(0-sT_1~H*|5LI2Q^hdSv;SXRA$t0M%MJZUQ-%H?Ce;5{+;nS9$5OUJ z_rBMrIr=#Zdq;!OL9O9%^4VIWp5l*@TO%OdAdAtwJ_|||2#GUZc-toG)jy3P@CqyN zPN;~c`EY(Pl#A~rYHrEP_7BiY^?p6y-q@CW<1W#!skfK=e*Y~U?83S_L}jBvJOw@L z$ALzW;q`HxqrlS6VS2$jG$L}E$*aR2U}A?85x#>B*42geZ_4h@(fMZlyz+b@s1>I9dJ1zOD`RH8P1y-*ZT6o`lvlotrR|Yg`$dtY}W2 zyE7(^CNUK4d8S=Te9cD?<(+onr)9>{7olkcwYz1ty{r|h6eBo4=4JRFW^;@L?g=(2#TnEe8=)R3$?5Kvq2I0pFQ zj;IoU9U}(F>h=Bfbg;9h9btP@argSn0>WtoOWa!w3S@`jf+5?IW_0MHD-jsuB1oqR z%!OrFmWrRBlab1N;Yg#OJRCzJXSxrrKTIDM@>V4Atzo}eN5DuEAQz26#W~+17)LP| zCu6|aD4$6;2**k%D4wB){{TpbtgC*8LHqWgB@9Ew~E{4 z(ZSjXYgwp;cf(04pQ)i)%3)$`984J+0WOMN>Kr~!Hw*T%WhiIGrQZA<UEI%6OK zQmEb#e%=5f=H4wURskycS05vP=f^!c%L6I!$;a|jNB-N{shXt zyZsG6#yqzqaAK2jNoL%I&Yjp@RH>WoG%)2yFCL}PkKGk+;<@90qIEX(67vlJ)$z-j zOt+qe`6xJUf?e4PTU~#2e#UKVpRy5K%UP$*r_xzmP}KekTDDnKL&u7C& zTEaF_4=kzBMfEi3D18jHCj8p|`k=v64Oo{Kf!=?nWZZz;m-@jJ^HbjoX8tn4$?zAk zJ!82#e1mPD{+S5ru}8X9-9Y9-iBl$hp~*zWO+N(0!#n53MYohd-pMkn$&tv~KI9^m zh}gLTV?&?W9!{`nDhpv-w|;g}W>HcTwG=HSXqo|OLfhF>0-qSp?W8||$8LCdu#;wHq$PhrarhV+ge5o*((4b&IsKft2U7Gj zP{%F}04b6T4&CrAG2D?D7N3&y$IQ&u6Q)=-2}DZ7_Sgx3C;~+NBuhOBV4yjRc|iQ^ z4qdl$Cn(}8f0L;S^Rv&jf{h)FB9b^-nVhV3!<8{r>xnJ;g|SF1$UD(9;tztY#1H#Z z4*i>me#}@GOt+1}qyI6Zg@`eQ%Q!Ug$&s^TDvimTBe^SX3*&2k($kYC$e}jGgqf%{ z#J9NiAAghS&TfrU(CqL6J?e*c3J{GZ`i9@h;G%-I4)BnoZ(Z_VX2{ z<1FfacLqU4tdqvsT9zXe!&g?!6)fCsGj?WUNTzT|ER?}AGWdHfHIFmO$o0K@ZpxjA zO*TS^_$hJv7_#Ixh_XpB#Xzq zB((R%sbsFS;*J%iwK>)1SYd|&{g*@Qk>!SAV1PkcY0MKo>TFQ29%8ZvIsfk8cps%$po5~osx5G8Hass5Mnw8bqAK!8H zdB|e20+pIgA$0x_o5~20VsPzxS!I9>4@+2#sGi}5rlNwPP?*i#_Cppa&+Y2*fB=<^ zk~yf?MU^al*=n4L15SsWKvGew6mCbS$j#qzx`X0(b!-j26FW60na{S3!{8Xoyy=>& z_?ow$pt|ejCcbrxC27>ODb{sLdO=j@U~{4n z3pZ>4IJa^eI&rZz9L2S+(p^;B?JE8Ew`8g0u`^hVzj}o9Bs{NH^{QNmLu*LRD~NYL zRwx2vz?J}}i!PuPx*Y{FNitThI^l2zUC^FkrbC#w*&>cqIuA8yN(xIUlHwXP+{YqY z47DJN8RE=1HMcxF4f@lh-3Z^zYUCX!LGMW6WwYz*El3wOq9aII2zaG2ABX8VEg*Y( z0h^v}qH`o;)_;3ZL|O7U;xD)N^#bI{*1jCR@0ZjIG>z{3j_Se_3ahI><)p}E?U4&v zp38*!Ah5~8FJcWlV$Oc-OEq-$5(h!i%DQ(F8HttK>2phf`2eLPPrlH;#GFR=akU>H zyP95qvllUxS!iUGK#Q~d+s&>2K+Y)2Ognx$6oz85wv*T8AU$hGg^l85!w;XMQaZW> zR)@1s5e38A#3$~m2Bu5?r#W1IVcA5oK^euwf#detNAF)rnxptsL&nj~e;U}w1G zN;2t{u-Y(h!W~Js`d73y3#uiSSMd3zy(bll%NniJA@+Ff&~$gWy%-akL@-o1QCwoY<9Z`hx6JZIWZavr1*3cP{! ziv&C2BM(<<`m$cSXMrcmfQP%AO0|%k(+}KoZN;XV-yKwun|KWkr)Vk&IY2ab?rKUTSiB{0^l3 zLX#*w!cQ7`mB$bWUrZ#%5zitqEHdE%&@U#9yk`!;5$^40R}(0Fxf%248Kuz*NFjjn z23xsD%iwFpr__kF8AVA7zcj`d z_5ur2=+FdoyP8kuk?W1hOnfau(S&L~@708{DG%Navy%$|yTjjFkh^P3%oD0yeQJ?b zi=bS1KExRANCx|-4|{hTq$rRvEh>zdC+9*?FU9YJ5=?`WQ35-wEN-FG{v+yPt$3z9(f@kfSMMp!yt1J%0L zx>-BpEdd+IXM$&x-q0P|kfE3MUk5wk-(+7GLx|zJfmdjHKD-sc;X0rycK{qQI{xcB zLbipxaPR)FCkN0k3~I%=y0NyyyaT+$y(74NH1MA@ddG~+br!ITzZ^O;USS+5tG*Dt za*ehPwoSeUyaw*Az&JzyOl*&itkFLay_3!iy|xR~1Zj?2?;_--EBMg&hVTyk+-@=# znIf1aU-hNYt=X;me23(oMaB!Z2lpGz1;A5JXqUTBiP16*4$-8>_=$m) zL3H;W?E&)G`Dgkil-;$Sx0N5Yerf~en&g_znqbnOaUIBn?qKgU?{M#6@7Pa9Pq3!r zT@!oYE#xijEx|4L>(!58FI2CcS=5;ZJ zBmwd?xkoWUZuH))Dnj#I*?%v%9JHdwKKOO8b10NOhFxktDo z&x8?5DVbBhn z!rS@P>~G8&e{VtVLp*Xm`h0k1<-DW3l+rtKnmx)t!hGSsaULV$2DlpbV+Qw4ZUH>` zU=!%nqR*_;u*K5c;$;s=o`Sc_4>{-qx`J{ZAKE)caL^x_S!yh_2#qqeX@b>c7+8=Sg^Ol(AMiPjD&HZD-=x7fY+; zvH;tt1-;OTI`gn-$1)R;+lKTB((R9a>4)7s@J5kVigVZnUxy0 zx#@SpAg?AHl5X>>))~m{4f_pIjFbO$;#aR+;Od}OZoX|Ei<&-+MPI1A_CA?g1#(%) zCu^K52}^!38}K*4sm5*)h8#!5?swSF^ful2v`L}0(!CgQzHhoMo$^zHF61`40UnY# z=vpD;yJa%nl%onzROOjmRr=O0Dz?t1qPDU+Jl?p}%*3nNiAQ5M;@PUlCZ*QNv%1YH za+Jn#v$=~VjAjWWw-{r(iQ2!*&0_Svl2H%uDOBWm*1F&v0*o9Q1_(~e*|CU;8e|zx zJS9+RSGog9iD~ypBrM{|Wes?`6@SI+Q7yd|(6;L;;CC@Cy`&W`p@LHtUjTyt0(f@w z;VpA;S`?w##9J6TZ*qiKXj=e|M;NfDgm;o*UAvwNyz#5JMNdbW862u?R&FPT47$_f1$GWtculPIBF{zLh1gJlrSPS zQXT&c_HHgJJ#ru~d7^%FhSJ^!s<4M0o;%0hhHurxk7E)M+~g!72>}gIHc%|2S!WfV?yP_cklBD|5SP28fP*H;uQq=td zxzU2z(r_Eq3#nY8O=P`ZQWLPGGri(u!eOKM!QU^z40?IyKr<@)6uEhe45>W^7Fs(W z&oB3@hqguN`aaqd)ALmuG>nBTR+V2cW@cqGq|4CFP{AVzovg1!iMD20?qTj`Wvwm7 z3i*cNhVP1o&a6!RBpkP0K;XZ_n%rMPFja&gW9isv!VIO7vjW^`W(cCkhUp#3L3n2B zWc7I!1oh5@X8g_;C&DL?OJXTgWt9pj{G?BI7^B<%e-uqJh4+=Yoz zY?2{$p}4Y{ndM673gKFDJr`ZBYv?oiE%TXi&Hcr_H|wz|SfNY*O~O_BmL7vEcZFD^ zTz^bY!cj2Kk(W@NWmXOMhq}DGT&ki!QtNueqQkM_raG44^&(q&aZX26q}pt zo~xGY>&^5i%sM(G-@U@`2wqp~{kdltY>b}gIeY`sRz9l@koV@ozy?Jr`ywkj=dw0> zm|x2LiqRD#FFoO?;G1VA^LeOF8q^{fVDDFADGY(J_*s79TQYQD>pAi0T)S8<4Ba!0 zGRK6ZE*4Eb+4@i?F{ghy0`x%=^s#v=2RTv;D-y0fEPtjWw4X(Vr@1jQ@f2e(El^4) zD<>-+s47!MgYDnw`+QSQyVgF^HR3?eTDQ3n zUGIH75JDJga+3e zkXE8F7tFAS)LRL~=!;JEx9~X+^mB~Ru^J4j#7&Sj#4}v^3y0B7S30xVivWj$W{p!8{MgRLlQp2lSu7P)IUFLWQ@mfopL z%1rGPW0M-T{Armd4x%xr*8jh)(wJ@|TTDVDoa@qiG#wO$SV$vn5L2up8jX zTCmAkFv+?CrjJj1BMC2>RddQ~g`Mi&KTcu3Vh=@c;-6`n#SQz!QwI4mjSegLpYnTU z@1WDO&b5}?bKW#QC99?Ai&hR69&;p^Q~ZTjZCAvYmV`M2Y?To!V8528q8nz+>X^_p zprZQC`|0eKk}-xFsh28?B*kwM;xns&LB}`JMJ(6N=)gk9kBos@ST(4G;Imfti6X|9 zBE}*kt+tcFtn5&yH`ppoSNhers0vkyFQWOADY+B4toQ&f})J5Ei zGIrxlY9RVl6_-JxVhJJX0MSqepz!7~ZpFA6!Xe)9o&6B-(CU=nN<^`WBBCppANe$MA@O)tC5Ee)*o42{yD|A$@dZbvZ#8k8^Bq+)@&xv%T7702T4*F%vGZKm7|B^RbdsnzSp_vQ2ICKa`S$v zpm}COEoUhfQ|Gql;$7?8JS47Cw-olmc5z$Zoy<^+ROS6}3gi1Yisf5pQs*Kk%93IE z=u0DCrc`#vr;Xiq^7CMqT2CB(%FD8ujLux;(i{&n2W4WDY_tbM zqIGI!%aIa5%jsQKR7Vu&-&PZ@+&hbq-RZeA_8IAk%B-!g@$_HKl(DTZwwSk>Ag-ooOs-sjJd^vPnDI zw{Hv3$UA@=xK?F~b%$57pchBFe{VdmQPaQ?5SIE1=ELe6al-8dxigu2)c%L-#WO^h zBE%a}O9ATM>Ivj`5|L7rj6kH|AFc6zaoSofYvtreG(`NFkj+c5tU=s&Pwd-z)PX?< zHg^&yv6=S3Q43(5!PW<0E=?nICMPrx>MRlKC3mN(>5ib7@`dUp`VM*%xfx{Pwsduo zxkJ2WaqX&WWk-u=nn`}h#`%WH`?RB?;KsB#+(tY6NsW;8=um&h)OHK3()>eZt^vffjG&r0Z9ilNNKj`(^*3ON!4h zBWW{wJHj4z>WfP*69>*i557TZJgMDM;l?(?7Ulrk`xc0p%rmGit}e2r&0Qm0%Xw8R zJx#B2hNlbzJKS6HEYUR!HBBn*{}ihvO1cKEcARBbB14P7rH{m0&b`yGr_6_KvCa-O zO6k}=Mkg~4R7@P1l2CjHCA_CckmGeMhz5m`0Np8fGA58PvATB~@2WE5H8u6c;B|my zW=7Pu$$dng4`Swo`NI3U99mpv)FCZv!FPOZOj~>2bQk?L4P<53Tdj^d)4CZ%b>+Pc zB)#SYhuQ{z*B*SuAL&qa@fz$}_u`zb08Nz}SIW3Z_jvG(G9$udaMa*jSgTTr))1?I zvt|)`GOAk?n~XuUJN*_fDprvWR-^Dz306J{RbxD;JTj$_)(x-ANW^>=eA$%-LvlQZ zi_@mRIU7p}r5&M}{n& zUm;?cSUkS1AEid{M48R0^#fl{eN5icA6QFPvu_}8Dz|0bcb?kg7+8&KtkaFtyJyym zB2S0hQye_o6R{6}MSH$-k9gMzri5M|q!hX|`-=dS^DFS$)Pq4c{w*41rWCU*WG~|l zVGoloY#6Q*X2>Hzzn!}TMmsP>Yal{Pz#<^&-bIXj1eqfR4py4C30zQcywEnNBzGqb}g4<7}TrLM$WnzC`0 z@)qz`_>9kZF4J7)DgN2iyq=F3T<1)93U~rrsOh>C77K*LslVW(YUO~tRd|Pv|=Qy5v%c5zx;Z0yx+lq6xjjdJ86A+bA%ix3vBUxczAn&uW?Ue@nKENKd<^F(!_41 z*u?=b+@+IncoKzyCwE;cRsxQ|HuuJ*eo~AB&90N zIM+RnuhkK4tm*ylf~4b!Cp4ohjIZv9Jv0lJs7)*pD1dD8tzXR3?)utu&P03a8&i?4f&T zdKkZ)ytsN0Q$Nmb_jr3ghY(BCRAv9Hb0Van-o)%+f7^8ZTDXhp9YzcvYM<@J%P~^l zm)+9khk0cq(Pgs1k>t&IX!Meig13%A3Y|s=f7r_^tJB7vc2|7vkXmLvjr^8ZtD{(3JFa%vsfGJ=HK!Y2FUy;Bmv@Z+nIDDJEK-fi0FoUW_Mn+Jz$Fe!2=ikf;;&`6Br9|bye_Lf~>yVsH1-)*#C ziHU!ZNnjj7-7QW;fB$kBBag`|)R}tfgydcyEJ+KeUp|PP5*Ktq?yBk{E!SOiLmPL@ zOhKds3k7w~iw&}5(%{UldIu^M_Y)z6#z-*s<5E~e_2ZJE_=_v5AV%Z@LGgbmd&ekC z!e(8t&}Ca)wr$(oW!tvdW!tv9Y;@VS*=5^P{hc%So;7pl$LzH--gqM;GIy>WnQLc8 z#`BnefZU>)XD3CcKzA9J&&(K-`1pm;V7%U=VUtr*q6MVu$x2R_iP(?s3v?3(f`aZ& z!R#JG5nb7(7>xSZUrn}XJ3H9F^J_g`z8;5S-r6yKij5_!={7Y|_|e65vDgcP;KuqP z`r-CuyN*9_tAeGLXhI*=Zr|*09j$%6N`EXZ)kj6>t15ae&%e2S+QK}0{KhB$S;8Oi zu`-*c%@x}3k!74PA-iEqUb`2=jg`4Mp-$`CKsXpC6M`Bg6QxqkWO*ZIBQ$P4YEp&t zdo``oS<)#5>Mt)0pntMTak@sSTA8&>T_vV`q;qy6O|q|+z33V1OpL{v-@xpgq*UcB z&^6WQjRQpzTs@$AH0x8Fx#*$B*j-c`9u-+tEs2VgS7dgQvZ4GSDR0a)6V9T0`P^ws z%VD|q(f6wW3?sSg-SNH4i_@<^$7=X|-qg%Vyba1EGBwplAQZqe2epnWm>Q}knm%fnp@IvNCf{#BwJUQDNPY`Ty4%$6I6^Z zzQ=lTit47)agpaxu_Q?4$or?DM_%rqm_1qNF;_8ZQ ze7{wt(}|j_p6&&`94xZtZWO{@xfvI>9R3(bLC{G=b^(Ec1}ImZIJkx-21A152RS)wqXi;Qd61(!VhF4nQUw4(%1RQ9ZJP^-Q;0xAm*ixiP6X*KPJXZz!ijUH(SH=xP*7A>+ z*61eEtAnfI)^zL1cf(_ngH*N>k_J29H+6z<&Q{Fj<|CjjJTGKV-z+%Sg3`WH4E2zxHqI>@H}5U$`~9bD#V=wi2P z(Pd@UlLXiOScUcxm11ZL(W%hOP)dAlQGW0=_)z|CWqNK=Mw3{T1rWp=3)SmXvSPu( z*N$z%XvA_Z>odH{l4U?!c+J{6>J_Px+W1*99kXZWc#q1E#!m8ZhrE_nBYx(qz}0ez~$SkNgt%Ci=8^ zpKNzQxoX%ijP-WA&wrC-W5GSC$xm<2lHSuEsZ8JWj^h85d%!PMvfS}(aJ)lUWxUxF>oNPKDXECJaK)Zl|zN^$`!r)`aWi)%@CH;8pmTHe2f*DUc z)R4Yf6@|eVxk6@?*hFdNbNMO&m_7PC4VbulXt=m@GCdNO(65~?ALXyiv^Tc}=_y=1 zY4YImY5sHh8|~P|zMPY}?iL>E9oJ{kqNMg!tf}2NJ2BNe`ea;ciS*CGnM@OHGFNRh z&urCjnk-dNi9dz96-qW8A-&pJ=mbJzg=^c!Wu~+})UmtFQr>F1Ze*o9)%=J0sLLo8 z2XFaacm5ReaTC+eLr`(qd0g3pZJ|I+i=i+*ZxxoQCcW9%|8^yMZ$`+rH;#c{d{m!K*~};jh|o{Q3RB3>4ri^2IurYo?`J$ljAnvV{z|eWlng{@zuJh5CE>B_`~god+TRqNemI}BY*kw?*CpJl z+MkhTmwwjk1>-46aCgJ!TP^tO+~|z!WWN2pIChj9%9}I!%LHT&e%@%lackXh7pbZ1 zqCFIxf5XI8-fbaFIq_DjJQJsiJ{?tuSARv5S+`5t45;!O?J-_;r*GJ;^m@yIC*NB;h)}8bfx+I`Iwg5c}edRa7$=#=ejyNEU3yt7e zsT;s4RG2SPcASRUnluPltWd_zUJ9{^3U;a7%z#zS^AW5J0ywBnNu~y34_H-?6i*6y ztPy0CZbmL>7VUmlZ!!#i1O09fH2$h*rX??1^tvDP)R;cOg>R7r6l+K(W*cgZGooqM zL}-oHZ7`N#?he?lmQf3<2kK`H(rw=LPx(iN(d=mZmSDxmHCTz-wEmTEl$8zC{+R*| zYZeklJtL#rGf#XtxP=PQ_6+F6ophA&5k&qpqZNJ5Uun~8LFDs0I?RhwxC{tHh}67X-(_`M8LB++|#X-UGX3@K=8dIB7ny-?Lu=KAmm= z58Eucr_j%J1Jw0gF1m4d{ldH{?vc$bMa7vvg^NgMQkm|;`s7DEff|{3auyvsSyI&b zsLU4>^k`D+CCDwnQWgopow!n#%|)A}6vBz7oqj6T$bo8#We^D`2Qd1JsVrJr3m}J8 z!C2&vgbjueBfFmD;tyXMdtbQH=Gt?UlWo*MnJEQPly^`a-m;S>dhLB3qom`a-4C)# zbqAK%6%aZL9DF<=Rae#ee|Ym1)cRj|`zq_@C%Y6KorlHT$4L>SC#E#Y$###GtRl84 zhdUH~my^cpTPgAS|6=6y4)yo+H9r&I^zb?SfHj6<78}E`V~mxl0$pyzAbQeDb}QT_ z&E@c+OEsq+17MUgp(DeQ7zIp=IbHm{#`|0uSk>CU8vH`%$MHK$DDg_~giw9N^FBEC*(TLB z#&?g8{_RHl`oUp{Vi$Wlsj?7L{{`FZ2PThmM zS-*C<(HrJ(3*mRY1a#7VR)>#YufOfZ2BwV2$NYpC4U-6x9*$3Q7gkMG^Y;r5l`sm+ zs?Q_n2i3G2f!ngi3=*ds+rNGODI0G~$MK75^YGY_Y@+!Nw_-AbzhTG(#8laGqEB*P zgf(Pcv5|-b+A=25{m4#_n^nM%IQahhT_*dJ z6+JX2uS|#bQ9k?jrOx@V4s-yIae1JZ|M^q?Q=L}AM$j8fljS3*S?~4F_w~g2hCUDR zNxTtzjCP`br2nDNl44K>tUW+h+$-OYHVG%+4)@*%;@S>_N&%$@k2Z)61e-jH7Hxi3 zu6JGD9uh8Udq|n&GL(^ar!wD;zW2>P$9qzce&6Joqai_O_=DSSxAt%vs;fY7ql~H2 z7QkQQs|_fDJ9gh>ZFl(40k=M8B1#-|?#;6G)mR;+Vd1t2zGlfMQifw+bp()$kQccT zyMYn$FN*l}rtoPxlZBojq`p+p$K$+dK~)EORs}-{;G){d4Y&Nc4}h7Q!N8X(#@>MRhTLNR!+2Buo5$z0ouO=hfH(yOVQ za67VKcRnHak_~dU3a45Kf}@a643ecEBeA*DDjdLh;L0OlfcMH8v;X~FJN7Y9(ew1| zI}3Y%)^ltX|CYGldzJ|h#EAJ3tTI#UR6@ZJnk9!2yeQH$

7;(LRQX`PV^(lHqu{ zNj<$OoTmk=B(yr?Lnc!%H+KlqP!2|~W zz9EY0s`zdSM)s^lnI&aS@PrNqjx*8~+IR9RccUK~_WgiQ&;ViZD;NajU5z43^4r7b zpU20qJ`WK-4^J@gmjlul8iE`GdbNNyNp;afelXB4j|E?4XFZQf)U5I228mT=Ed0SQR{>UQRs6ciWJcY)k7Afcf%a`m03Nf|3#@RNaAd5GjQN6KQ8VvkxKhb1<*s7R7E9gWTqvImp z?Eqc6DqtonrdO;HcJ5LY#TiBFbLlCFMcgz>upLhNB#vl$qQZs+tlk5xWG-!$gb9Pc zfZH=f)Y3*5i~fdNOL)aiZ8G*_L)s!_Q@1@D`zfs{iV{^v6hHcb(L+0hlq(Y{kRREq zc!HM|g^S9rJ0tM6`6w4;C*{~n-LDoa$kxnAowQI^iX~-}O%taZXhL83OPWeQ%8j!Y zNVJHrO8(kIm|1F6eJf9PRl20JMoH{$^0TBju8N#u=Y_bz&_cBpj(FE9UrtN{a8Wj} zwLhA!WfqZ>(-a*44$A4!9TfB2k(9e<6I~m{D(UhGJEgL~MNT%Ttp9>s)FdA~DK?|G zh~7+MYD-LC!cDKIalCDz&NwO9KwRG_$*L~eKFQiEjBGBcW`8ZlDgTmwvr<`F5i!He zOR(jhX6rYnzL%FuS#dr4CJ`>TkzN3A2CO~;IwIv>5{s6gu14?hR650OSstiW*)Dh! z3w4x<37x@;cLR^&6-JQPMR zfG1$nXZ5}NxdjVaGD>b-N2#d#m+HYh`Qj#t>U;dqh3Yy0FAF*vZ0tezT{8}R$XO-UV{>koz;${sZQd1EYb9IJmy-E=+O)6%pp}Tl#4cNpX2-*+2TLBj?*6-xVsA? ze8qZ<&x+zS9eI?lu5pj z%d6BH;?L(idE>mAac4Bg7CT?*Ep8Z}i(Y2^M$RX7ON@HbELZL5DGKVQANBJ5>x*3t zKU2o;lyxMexS{rtmbBzNuqJi%e#)7%pnfb}Z~KMs zz)gEtTVwy};2Ix1hG$b>QyfXNp&M);MmqXAM4GJ+bU5P{^uT@SxV zoi@mz2|_FgMkn5ok66K32I~l{G;;pc&~fSK$1E3v<|F9|c-mdV?b>}*uu}iNs5KMQ z&|aNfh6|-OmX+4Fl+vR{e*jy@)MxNj~21=W`h=$>S?aQIg_HH4T4=$ z&?Z%Ia~ChhPvXw{TDY|GZ?w>EsBU6FxzL@TSZ8KA;a$W_EO$QacKb59SnO^nQJQ%a zHPx2(l=5b9M>C-WJ)+OFEh#<6H8#^PpkG2i*u;CTeNi4M>A5=<@nc`Yv`EI=QbL!V zElI%TmmQ=o(WR<3w_r%+U$Rkdtm5OH12y)Qr|VDiX1)n^;c}b01{Y^^&*&FQ=Z8Qe&W0NJ0-?!W6vl+M`Yzz6jvXb!U5452>A42U8W3J0>UuUQm9anaJh0(}>{g3L@bV zeAwZsA1~pyF5s7B+r-~q$jk#k9?*OnrP}STkzgM@K3fHu5Apw60#>Ie?`@VacS69z zKL<*e#@(ILjgyFNty zXIF-y4w}TK&xn0Oj)HQ()tys=$E9T;&a%c`TV`%=|NFNz<4WIBPEA3Kxtf`xTGG2u zWkJnnG-3UX(A*0>GHeKUKD&&hYL#|JqwPt-X_K^CO`PhIBnG{Xa)+U8wQ`rci_OLD z8k*Guc9*FQM3{?`rO1E?c`wB|ffT7{0I?XmFl_p{$2`1+_%zV>Gs-1!$C4uoixmBz z3u1yj_V4Y$yp3)2Kgm+mDOiCu#33j0%uqPR1h9Bxu?(|=2D19{S{dS<^;%h>J#8U4 z1r-KORAP=H4*0EV=l{pbKLebEC zKWt-4Y*)}+mbGymKA_W=k2nIkgO~j1+FgR=$gaKA2BCBXSP?hoH#bApLH!=%J59cm zbi+@k<=xaXtYDr)Bg6?5rP$J!-%j}~idXU~%H|E)QGj4`5qsqx;bKrz+R)K2>*X)2 za0^Xh(G?Xsq8B-=YxMDS8t}5gg?ySf>Ah+Sd23I`y{GgB9DK&)1Wd7U2(J_E{weBs z9eaerGiQRu%d=Ss-Q}!_64H!G8a#5j!J2z@Cb2k z@i#I_yn@+ZtCTzO0&}107;fx>TfnIY&y&i` zP|w`Eq{#C#AJ&Tlp~XLQns)E6y%T48M_A<%m8qegz4ry!_OZ;{n%fXAiC=Z7B6-t z!&@^L`QL6a8CAu%m|n|ejK#_|Y;`U9;1sYCb>p*86tJfTc7h#w4F09Xji$?@W>MtD7*pxNbuOpfETeII1 zB6OgfYACb_Wy3#^HN(CC*ahIRW=Og9d#d>B4zBe=d8}A2#YpAX7;I7`7r;efCY-@W z9ULU9TCIpyB^F?6@VXNtR2mM(E5+WEP=l5-d3}^=N@v-F_@2sJLPg+ z3Y&D=lYHbxy;wI!SGW5UX317A_7F0MvS^v|&L$Am1x4bEqI1z(=4VS;@z}swh6xQS zv+jxM5TA3t)(A;qW=8^D7$4$S2Amb(RQ?j7;@-ztZ)X$itn})Y6v2d`JDL z1kUs*A0nkbu12{W0y#OMD@Tkb#*6q=vWGgQeXG+waBzQw8RQt}1Vs+@cMe7&(WSAc zJ1B!a;LO+tdHy_IRlvu&`P^V0ii#Ej>)d88Xi1!8nvVVvf!do`3>2db_IpLTvLVeZ z-An;n<_4#XIo~&gGk__N555WI6h#L0S57gMV{6o1dFla3iXQ$2ObeGjCYWZpr^57@3zOhET_}=n7O}R818^ja3!DAD_AFd zT9N!F% zYWV&_6q^18VE;t0QX`NkjKV)rOz__PZ!ian0MGj;q-KFo;RODPqyk_HZ2y7)r4a!_ z&|p|prsB8&A#i9c3S+7No>PEF80P&GQi`ysg986VQUNAKL_k5Bl7s*uaPSXQ2)81rh-eaDb+eQ~d7{c#u-+-~4+t zy=L&Le=QYoM3#^P8I_d!?~#C0Qu5!NfV141S1S0A|LXa#dYCa=ieHaEW^#bmPz1aJ z=W_D?vH}Iq4EYm*qcY}&DMnx_4E#rzLIVA#Qw%aFz=Hl0fCz&;Oz?lA1hs#`9wHAh z;;T*ZGghTvI~YgBzKyBFfv(yuAHkvvrAzRPde%VG4~AFb%&wr)!Uv41^nu`Z9{X>h z3#x~*`vAbUsyPTo9m<3_36BCIp9lPq12tZVUeE(VDv&afnWL~(iHCxZ3=iXFcmP0T zgsKV+5j^~BALJ{|UP`~{R9Q^^gQ6APnc3$u@JL)?2RXnDEx00$pbF;)|2MY1AQ?sG zv;_X)ZtCsz4bQteUNTJjRLBN#1+|~g0A=T+QjpGtp}X2 zL;RAe@NjQ#78;m$J`D#dxufNd7T$T2E< zC7N^fkNya0Fl0pH%vzK+pjJ#ANOdDkdzUeZ@u2EH>@$%GzKp79~R?uw;j+-uXsHo}Vs@O1DF=9vGnknb@|CqK}*u$T(` zM4B$nDTT}|gJNfj{rSMs`%BsYLxVVHP{Sx`kZT$~_I&hFSwbG^rJ!xpV;m-j2w9+b zbn#BG+e5=>U}!lx)}SGaJO>RSEHw;f@TEY00?{n|q(H?)FITsbfTz(`e8 zpU~5VPSB%mfhJRg%yD}_VQn~)nBU_Sj#*_m!-OhraF+hc3gh5&E<#(NGKifSChNmOSN43BoqX$YUgY3j$kQOk0zMHFw=7AiHV z08v*tpDWtSHq#ImabayWmKki3rRC2fPb{Mg%em=?B^KZ{iwN)~Yao^+St$iavb#d_4FddeiGcJ#J!-Jdx*wV9QOQWsu@SY-=UlA{-os+;UA ztB1y5WK<=|tVpa>RP^*)Gho%?n<_>%7mcf_+9GOHa0p$uXWZ3;(b;I*Su|pqG~rh4 zr7K!2@;4Vc{4wvJy=$G!P8s2Mg(YyzBTl|MmOLqp%ktXuGGM=9@P)x|dnPD^r?T(5 z%7?EBO-6ofnr`=%_ho4F7=Q*waQyckzdc*(W_%O0^Ff;z|K5|xu|BX_r>|t+0 zFJ@=!EMnqh@(Xb%Z5 z`d1t>G6}j*nFYW9ig1K-l*ve7Pp#RIO);Io@P6A;{IJ1hjwUb7Ze{ktci{m+QzNak zBblJa9>*DxTnNtbhWd59j#==yKGj86-`a&g*#xn>t;?gXa^>fOb8skucOCS3o}J;H zqTa@NL0>!&yXyq?0T@= z265j5&%n+j-!Z&axmDt2N-bupyx?^dgTp2ew9D^{9|D0G#(Dcf7Dz+B?Qt#XJFH8R)iTi((3jPlqE!Kb7TK^{;t^Wn%z|6$V#_>N> z`NGG;TY2%}S!avwEpwEFInib-&P0d|C%`)F09aC;h~!*Eaug99c{B$oP}N>^R!URr z30>1o8#B;QfOer^N42cFgLbB%AZ4sY)JDbB_o({`IdHvYd;98b`D$P0vg2se>$2+` z#on=K@TL*!^#Q4)Shh!VO(cU!$^OQ=U z{9aizwbi{`#~-2~PgQw!8t%;7`dZ_*>kA^Zj>W$L27o+haz}fj1qEi8&|3>`NmB{r;7HI&I3O!xq^JZ z0lq6FXeE;G4_zZ}X+Fk`(J@?BB%Sbg^^*e1V!jlO!NCUYKmB%865lJZke1WoiV|Q9kpkD3d_I!IcN6ZGP3?>xKX{bWeX$7Sf>D!UMF(BYTj)tACFT8Kk zsr|{J?-{`D+487oAI!-Y*6L8m_rRWy4jdiB-m|^p{D^%ae-Gs^3e2k^ zl|4>9CTxVc0QQ70+%9aH?GSL`)D5Ta@9CA_lfOA45fF=qDoT8h5*fw2#mEpboA+qU z+!DIVo1W#jm$PM38~?~!AM7GMfFU2SKi^u>+i%#5+*2FyL-u3(lzB(E=^;j`2L{8U zKxhJ~?J&?i3rLJD(CdF9@Wky3|F<1C!BfQMJyh*Lvz<0~KHGlYXR=PLFVyXj+`%4V z(>;>#qL_t5v;$4YxSs`R_dHx9hxUWotZsxY31|B?hmpJ7i9PXt{@X+R1G#%WsM>vO zP?Gf7qr7)V>eALO=kg9t8+h`u7=v2-xdeE{{ingR&^x2qy@_4|TzFz{GrlunJ>q^p zPPYX8g#5(B{rmh1=<9IE4d}fA_IM*pG~SHeKlFer-Eq26wxiv41>aFWX??NqhkAx< zci68P-!a|^Ke_otDQ=MXM+pe)e>KKETz92x$k>v*#bF4@!N>O>usQ~26*1w8!dP&6 zlg~&xGi{3C7lgKnOo49H?5jGQ`R_kDytQuI_Z)qW5$pa%d6waetv$5cdAfmUzl*vsCdALN&^r4(MH;)QA$`NNmRqvW?E`pnswt?Q3- zEyjbeeoeOFzd`cLchn^jYeUDFd6X1^Q6@hyvObG1%(ftnqt*A-=pAs+F?7u1%$gIn zw3FK_#UI)s(0)6E>r5XVj}2=Hyd5+?TY8UpT%;DDwSSGtgEmP3(YAd9fo?~VegN?- zdkGO3?ghUolXH;KiVP2Gw;Wi}6Y~j~T+lPG>-p1FlzSP0`<7`U|8CUdnB|~00Ctok z5;aeXC;Qc&S0^UXAgAi0GLKYq6kaL(aX&X9nFT^GB660TBE=p4 zI;mf<(@q*pMKUaANbPrweUIjM7VDCfHG;6UaZz%3}wc;9^wn-1Ni== zR*;HhrY+XK_}F1BKyH?dNn&bedc^D)<5+Q~Z(S0xM^@QcNxctu@XB?~flCLTD>~0h zkea^OPdssNI1c}55Ml6X4|Rp!OMw0Z<4bEU+!y3^Z#hqq4uTgQ+`?8^wG-XdAGe!Z z@bL@y^$+NHe-QkIzmF%x@i=cDy)Ia_1IX#NU~b=57jBmrNbH=*cC&%f~*&>JL?!Ey>2Y;|44WD}7mZ6h34 zvF1w-8{VQAP=AS|vWTKlRB&RDSFl)^kF}*y>C=5dmsA*4^rxw-lI20Ycu2W~Ap5jY z31balOzHl(j;&zAPCAx^%PIe(-Rx=iG^INP?q)COcJD5%bDafeEFUp@OT6UY*-j z)=ou=Xg)F>xkovPC0$5i)E7)(7oE*&3J5qb zc42RY8(kbL@rYysnH@`t3b9u?g=b%5F}skg1HJ9R|=R3>XSu^O5x3 zIu)!ZD3n$u0Da7V6`iRv@Fu+GLDaAG?9Yox>63NtKNo%N3QQ@1C1_$;KqQr|GTl~? z-Sbv$Feo!^^#Fi}T^VI~Wn(7Shx+|=R1$I-f2NRojX4d|)#GL_bbSntOgfd71-G+0 z%62MUPDNP8LR_Nu-&Lf8;b7$jC9$0fj#LMJzt&*ITCA3x*AE4bp8Z7`X%1~MLzp1c zR3l)Gf$%h8i>{%er=5}){!vc>R<{WKTL~pOv*ctM+=<}2+6G+ILZtA|Gv$)h_!Jv+ z2`Vl=*VAXG)8%yjRC_o`mo{5@w%cxr>|du-O9644HRZ(o1#8PSRJ8-f;lV8jzrlY0g6FkhoDj+m|P^pZfz-GPaU>H#x_r*d_KION^ zFh%xn`l#1mNMFbzXmSVQZWVAnlq@ohR*8v4k z(6_J&Hrx?ign4asZ_nxb4F^M)Ir2DWIk2N#Cp0z(FhcxEPqCDJ(Pa)a@TgvMZ6fSQ zV2UG5F)Gom7u6z#Y@XNb0!fT`Z<$A?e{>bIQ;;o3uyyVV$4ZhkQ z-H7bQNEkIoPIA;1+*V$8*7G@ayhPr1=?0XeYtTNbj9JEXD8VbuGGlJHpg^nFpbO@c zo7b-}fw8ujXXVM!A}yRIT=WqJ&>*qFtOvLD5(=WD=Ie5hUiT5ys3aySXPr(Zt%fM~ z(gE&m$BxsjC2?@%$2NP5aug_>kyUbFgVhar{_t)>F+rmiinPXzO%3hWGmAk96>wWv zJ7}DkxRbeh|0-UoSrKo^I?f-o@|{`H^IJtwU49anKpv1bG~0?9G+frXg|E@^wYXZ3 z^s0%r4%wv#v(<*%kPz%xxJ^RMva%ugF5N$Cr6x$9aCK8(v?pC03)V;TWxeZ<0r_U} z>_(N4{+c*6#?D^}J*R`fCMi^oq#x0Ehwoetz&-DtLu(A9KvCP1yJjPVCU%H|*=>d| z_(khZFQObnWt@5uk-e5UpTHDP5gFkQ-MQ@s5T~DUcW_LZ_*De5G7Es*;Be}!gP@aB z1AAx40Rk}4+hY~hOAoa%4WpBLJfAmcQx+HgF6DZR4Z!%?iWTR*T~PEoH*)H4Z2Mrw zP*bz({dn=pc^SsX|FiFUzMZkea8*CO=KI?li@Wd`A}1@voCj-Kfu1ly+ac$~`$=YA z=82alSKn1L?BIbYmmT8JJ$b=mt7f6PJQOfVftDGMB@Hk6h*WznJymbYIi6TbI^8nu zMAt&wLhn>VSb6REI*-80#6=sDj8>mkW zQf8Q3p54{Wtw9IctW1bOd^ZQe-1xHFr z36R|A?53&7H8rUsG=d9FxOyTZD?JXE&f8|{Bt}PWNYa{pVnIPNUg05^&wNP|nzh0y zk)ujm?gTx*XK|o_E$5ZM6D^e2SPFMA72v}o?1B+&4}e?_eibHh2IR~q{)Gy7-Rjy0 zi-Xk(c=88N6=X_tKCaKVxEV`CIY`WKW&oZ|=1fH39`^w4l}jxJX`^CWJN$SHrB5KE zijLldJRn5{0d(VV?vaBZKtJWBtlraLO|TnN#_`grQRudTgf_~9HehiOq%h$sjMaqO z#)Bgh*aG_ikr`v6|G0oxF5o@Hh}Cs0t~__kKC_eRqH{gx(vut=^Ee76sxw;lq>6w^ zT`W-tqzuP6tg3C)Su6UiyXctUOji*u*CyT#xlVuT&}3_`VvW$!>TxkU`Aop1n7|!J z)5S?3DCw7eJaa9R@!vexoq^B0G*@1b%W4ObWF}0os7SHB(dw4cGyb_!!X~ju?412- zv`GCRonJ5Mktz^n=`f~2NgvC~qI;`+UySewuxYc&b`L*8{(9J(PU?_-TFXIxk_s(i zFYf=_LW3WMFP$6tmGZg!N%YXjPkGQeC5ry4_Uva;5^=Cuxibk3{&i=x7(}pBGhBpc z1ZG6jIu{64a6%fM-dL68K%<7@bFzO*s!eV{GMYeyn<6LK$s+OnUr|SB#xkCcxhv`K zM|^6LM_mgv>35|KS$2&uT1Cp;$9#ckVX8S-iNry_7Y`z4MlYMa9Qr0=u+)hAKM!fCcS(e!;+)ya6K^Ws2od zrZ*&g1`OuG*-Ci)#t$xyTE_Z^Q*e>lTDD{l^NE}5g>7gTNbnRAQ~$v&$T9to*GiV& zBHD38`gkqx63AMeRxA$fw?UoSLAly442-YGu9c(by^s+6mutMzlRaub?DjjkbB^1k z=BLlI$&^8;zl)W%UC_>JAirA41p~d^i(k_;eo5#cizqtU(@82QosymsKP$fA-hkeK zrgI0#aquObz}yBDfo`>!Tr!@aGzaYg!?@|~`8STgrv+0#EI>Y`Uo|*r-Gq(;eIC@1|;eXnD+C5?E9OXjEwUXr3# zsXD73;F5??sdSUeMtHN8YWDQbuuMN$WR0}7^%QDXnpKuMWT!xxA_r>IP^*AlMNp?G+ zxadzmSek>A0ZUd(SXz4oU`AvD(E`Zfr^1}>wQzlm7`41la zm&j}Rp-qhLm`=YAt6vt!F_HZR3kr*@brviQCPDzq* zH)uJIm$%}35rwtAtwdXThiLm?V=@Z52R&o;gDoOk7>=<)Qf)@2rKSi*IyQ2!qEfSD zJahI-_^K{pq_JW!;MpG$NQwr1Dl^G2_F_qhBDn zJG4AL?Qa|$w9%W0dMvJ1x{*IXesu;n0Ll#q2INs6W& zFEAQ}?T@BJfqNp|jp@W;zP?XizblCD-l+ctb89xyZUayS&HgxTJ~I5M9V)4E0k0oW z!>!aDI`2jJ<4pjLGDisBkVkC!2gIo>T4F<}UO}D0u~!XBee7<)7#xSNjTSwed_bO) zz4&Jz4dh+85R!cYvlh)SDKg1DP9oqAlau8lXs*K@XkcOsg-vOiP#STX`!F!#adiJZ z^eLvWbv%I0iWoRBu>n4>j3knZ1#hu);No7sW>xdlN_<|6SK2KvC8%1cE2(xpYbXoJRgU}TGh%3J;QD6APai8hF~s5hu{^4P*9P}9wmL{9iX=E*`&h7XLWkQ5Hm z91FrAW5M+6!@vZQ;xm))=c`$uvkX*6_i@A_EK)3w2rT#WlFdWXz)}vDfCFCSS}Ncb zt7(8GYd}x!5=3NMx~xUI2<E1#DdaD*I@m73ady^a?lT^Oo_=} z!BXkDut3w4hoobio2s76GTgx6r2ClA`5MTYZzb%u(n4wrXEPaz-kJ7nIM+S%+381K zdK}s-rLQbzs@go69m#Qi<5kyx_C$B%+BXI_C$o4cHR%HS){AA)r**?Ou`8HS!KJNj zTQjJoa$J7DVA0q5>_B;Ej|h=&0i`LWpa4(xryvjeAc!69xzadGBkUS^AzXrYEe1Y6 z7O51hXo(9eu&|#=F)icI)A>Yh6*uu_hzN5k$w1bMLGG`4ALOZPNsfMUgBPG@%nbY) z`=5Lm%w)O=Oooc{*EoAYAh>^$vSklNwcOyzDalk;2fE4FX-6+!hhdo z{f#X(nk=4sQ0rpj8+RsNpw+M;2#&#FAmqYzFn~Zv)HQUKB@AChR`VwaJfYK(^4kmP z14s(kM2v7)Z&Et|$sw^-kYrDGosg@}ig5(R1^&EM($)PjZj%G*1n2l!vh8MmC}%%| zoa6i_tPLUWt+8$Zy41Zhdwm`t5`w4T@}VDI={KwM_+vPLJJ5sK_Wz>nE`TEI`32z* zFu>sM4ucKu?(R0YySuyV;O_43?hXSq?(Xh14$E`*zIAtRz2EMBRb7?jbdn>T(^V&( z{{3}hH3uf1t~MWijjlXyhn8ixqiI+(USWxS*)|J!lV$2)NEgJH#KjVNL4X|#KOKm7 zls+1-(}F!yI;FM_HXG~!Hvs6G1l5xY+@3$aT5i$YQaJ}-e^F<>vA@Oij@+{?=G(DB z^P%35d4jvw_$UstTxUb^VNsV^y0%1NMgCfw9-8}Tpj47kb7+Br9=qZcH<~i-P}cuj zsGI`f@1Svp~p|Xd8>d>ea*M-2@ag?S5%{9T3U>*9BXa9;K4tM$H$2mi76OXB$uDL+I z%YLY+o5Sp=8=^ZE5xZX#)i5~=Ri|%(i`_SDyFxM3R!2~FyJ~2A`mdma6#)Qyda?K8 zy=dr`b9!O{yJKWfC`7Pi|H-<9NQFMe<}LjGlQ~7R=UGfz&N-HjoAM!yho|AFHV@tu z@58;6!}WJ|pZ7o61dlQ*I*zzTra>6QgP(Zk^2pHJcMDRapww)nH$I`dvW^k74OY$? zC=Lm(=}%gkF1PNggz>6#c2)r=d*>*wtQE*)My)-CQW#x2$@ zZ=DAsYj?M6%`N+ESy$I=7G*{ z!^Du2k?NE7a(}~l3UygFQ6pA!q361cS*?3-p2rngC&x#p>g)UFog(|2uxk<_5$x32@8$&fbuI%Dt>Qe_<qHY6dP6P~+>47&20tB(dpQ>*->H}YF|Gza*82xRs-&&$${ zAxX+mHZ)MW5~`}RQSXeeMT?#CD1F)RE&H^uBKysBwe=ZcB)bDP@+`H{by!zLIcLwf zqKWAvTYlgZWQ+ptp{%>`yKOw(N<#90v8HBx_EC02sx+=G(WSucuHR%LXj(eCye&NA zSaAWFg`~>6%tw{asp}!(agdcrYL*rokq2s4zz5_Q9@iXIMB2^q$)!n2{KmOj{MfyO zo`9<%D22W1p&B^HM$li(B#HXQJ+TyYiQKHdx}Fgf-JsTv%c|w#o-W+iT7;voFm#v8 zlb$c1sIOtabyhxcGCge1XOz3hR#b`D$Q?^t;%~&Xh-=0-yS*s+W_HsUyN<&wdQSyX0``9fNXZ*!;g?S4 zvXCt>Qguz~mROCVeD49AOUT`)M5mjVH7pcXd{BZo&-0u>qGV5cP`%TLh;qxGOZQ6z zwhA~~JzhNE-756dOsk$0@M`1t5&7QOWAJ?6;T4kHUI>b)@=DwqQA$&NBF?^z{97iF zkQm5#ARYfKp%|SRhLUin=*WrZ_Qyp$vxKvD*d830SNGVpcSk*!(Zu3N@@1yRCbI)a zXat7}5Y&z%$95{#Ga+EGsX(%0X7@~bJw(7%BM^!a4cz2rFng3s!KTg>|Z=Ixjt|W9ID+NneTG>u@<&M+CJIIFG3mj z@^dP<@(AwpclZ+vZ#5*M)(n<275ch{?}-g1V8AN85c^Z`##9 zrv0GZarp}t;8wS6p#!Kwp^IBwt0^|9Iv}=h{%xCXsw1IMrWPRC#^4p)wytPiBWeKu zYyURLi$Qg~@s|q&PEx>14SM+?7W#ru*TJtdf(esIisox{khFwa05LK><)J{m#)F=n z4dsYc_&22PUYPjsi0?cS0rZ+w&r8*C=w9;yG=ahjJSNN~C|WHAxneq;2E&J3DXE+? zgZGwQ3;kn@z(=)9=y^zJb#%EHvWHV&QqCpn<&fM;P8h4J!Xk{?}j ztQoFJ*cS<|bALEa)GlXBG65!Blj}c|+0bJ! zi$X5u&4MuRU?9*R56jw6DZk$=eGAXNV1ok*IyT3UXP>~j=Gfn_Oug8Y> z>My^%|K2A**W7#09Tfv6>-lBK65OFL@>FMAWNb48%q|(@sKb(&24TylRenVxjfk`K zquR$e#?sQ$9i_lv{Gpj`>W5gPxp%p7Iq|vw=EL8*_4XYP6=6DXSK!3?9^EIfQ(|jh z=SUZA6Ll+5rvbM)tM_WoH5g$Kp%Cx{2=bN6N#|vX*;`I$t5qBRE@g8#X@b8i4SLBX zESM=wjV|JgD9$0-%D8!c=+4iaMk<2Z`xjA9LL?zrq-u1+5mevSZG0BDC#;1Zac~M$ z%Lm`~aF1J`j?en&@fPxfRBn=ZHRjCixRw12GMh}6w@O_yoXC9v_KsU@;E+BxvwUQ9l0US~S z6fywEfmpuOzm33<0R=u2F_$MWqaMlU!4AYR z30(}xw)r+@m;3uZ!oyNU5--c3$`NoqCEoQ@nH#{p$;E4|VBq}{a{oHrTnKjT?bQE) z^+NvMvXX5%C84!DZVeuhmHfNAl*2UfY9AnZ z26r70?a*-zA>Fb|K}HP027=*le`MdDSw^sv9gzCG!F-No9aKP01YK3!g8}yzYJ_nRc%gu2oye|J*kP(^bqb1s^cb@3FSS5AojYUPStGV*$T{rn* zDx=9BXxu>escd^uXm~YvqrE1+h>-)dQhSyyd@Npu+FmvY)2<-I9^*H>u9=^ba;TQ+ zW;{rrm*MDyS=R!ODneJ4aZnz=-*z5bf^;S4%XxFTUF(x;ppoF!Vh6H#b>CD&s8&=Axt7_oc@RI-+?GGGY=!S51QHyuqd#A#PW(={!N|e{-%Vq8 zL?ka8D-$WRuuwhSnJ*~wviJsyk)9JPCws8xwMS{&jRby(Lq4}kQe^a_<}T0%rqwEp zeY+oYdSIL8M;LfZ0T?wiY;7rhZ+`zbt}K`DcDS8+TC%7WZzpQ9p-Hl)0`m3F@W)L2 zgxb;G@cB787#zFs_>o2j)pZneeXulo9*z~<7RF=92qqspL;$2~MDL*l-Dx;e*B`JZ zEybjsvN>IyG*9?Hh}EyPva!Olw`4$wGfBDt$EHZdM=?hbv>TJlpjXts42aUI=5U% z+If&0p-Wdr@{mH04W%7@O~kXd)Nd3ERsz9CWzn%iEf;VS6+34fm5C@PQ;jR|Sg2C| z0{2m)1PmFD9E}Va7aR!>ep7DzZq2)o3xI@fdzoe7&@NIfv7Oi%!H||IlgLSK8T|_> z!~uQ)bFvkVZQ8K1A_WkVrd7uihy8kD~-Z0|H}W5NrI zhj zmvM!0%|W6_BkZo}LZV`3xXt*Ku#ga148Ax&2FG-T9<>0<-W4v|U(!_fe;v4k z2m4RVhxb)P(0Lxk$nD-%X~&gxckV1`ThAqNxsV}Vtv>M7P+x2|+Po{$!#E-#i%gfR zyam5tbm$?=D6*6nvmC;kH`&3h2GyX;rd5i#Ech(T;^mKF|8!r(GKg~KX+`&}BFmv% zIc+g5&{D#`3n>lO*b*X;B(E$v5BR3CF7R{nCp0q4A3(m)EXbW>a-hnG%3apt$aKQ& zUNL!Pzw;qZ6(2@(m}cRcOn*`F-^z?&g7A!@Ga11@O=I2YRTztZQY8DG%*&nQ%;TDr z%us@%mD-wQTlx1ZjW~j~K#ahdELhSe3#e5P38?_~1r+uT1wPVM=z_+f#~9&H%x%~p z#BIoF;tPf@G2~cp^;Gkjq{$@c^_$2oh@s%V6Sp+zoht!L1F+OtmGib`4f22SxxXEH z@XM4BN?jvK-&jup^CvwZ95P!>H3Ivxc4TA;nJE<%ap`VvY>bU99Uy+9bRA5j}oxb=xa&aW9$jy zNQ_73)Xrc^5Kpd*8>8fdvV#=6;;2645&$VpsWpaUT>ErynG)N5@w!aUS~iE5HirXV zwHCHrbpg*nPE0TzjQ-whlKoEzA%VLbz*f;i4!Y$9pFq50Ns3EpyMB&43~B z>*tDk({#h*bvP@3aj82U+%PizW2( zCJ{Q*r;R(yG@DQ!z^C9{Vf+j{%a(BbQ)W846=TatOq&~~KFB}pE@LXtRp=N<7H5A_qwh|?k5_`F^;aP_xG)iUv4@p_K#u!Hsw4G9 zlRl(GEaF|)i7>4Yw-|aU0hhyk-xQ>Q{%z2to)1s>qDDgD^ul277u#vEF`sKkkycJz zgBqWH$Jo1*8>7fuY}@7hfj5B`ffWYPesLF&yIxEOu{g#Bu(-P;yC-JQI(~;c#;q7N zN!9&5{I0t+`$zXKa~Fqe)bGuZ3iH6uXTl&M%F$#$)q>TN|jX2|Wi5nS_r1)9=ZYIGwqPK^q3QJ2G$4xTs33TMb&0H7; z7NG%)7EGE1zn{b!FGtpjFuB%~D~yM_A11J9hGYZ| zsyccia*<5$3H$+u_&L*mgjgg~Av^9vCprT0K)$5Xx^;HdVSmEbZsKfoO)=JCp9sJj zwa(A5v%~I~FB@{WsxFiZy%54zWRt`ageSb|R9`x@Ws{L%JFxP-_d} ztiM3F6g(oVA@(D+3pHu>jw4Bz|H$FNi+pLw`Q}jx(#mMhTPvEe$mB4tuqPZQ@7`!3 ziapH|bj~}B3-pky4ZjjnKOzeIYEL6ncL$ONXLIau`q7C4{J1fpq>Up*rpom= z)}?cx99YSijQ&NB=!n$1nDpBQF~;SiHIUt`%WX!t8pVMY=6^?VIs+3#Vph$pg)~uKCWjS9&6mn z_g-2UHL;(6JV%Xy2CMw$;l2DYX>Y9-HjF5D__rv_G6@1>1Jxvqls_MW)<7o{tZ_Z> zHtr{FyEPx!w$?=35w}Uf4CUDH`S3V-Zy#KbIBbG-ON za-w()HKdPX_8z^E*PbORk5DdBK#1bDKC9}*ecZzh`^n?wW_@TbFIPOJLVa}UeSEE0 zg_R~8G9tHz7FWYj3>&5scXX*>32!oWmGHrb5&S)8df(Bshad8#h^{c^rk(tjzIFOp z3Ax5X7w+)$LXO`FFvx|hLl|Q~qnVNSeA9-1o|I=rS*;BAV%RUQg_{fp;>q)2tS%-* zJiW`ia`g~?z|PZ!0Aok^{5{lsnbCPd00V&dwPS|t>yco&Ah|)RqTh3Bb4-Zs@3_>H zYt4MCF&v9yEgZH^)>5sD)-H5*wvrPvZ_MTbvkU;aPLM#=0(mHyRjegF0D8VeZqla_ z9%QF}krX>nGx&E!jJaxg6O*OuZ_A`0Qm|q(4p}xm_^5tW9D0Ph5pM|UT_3c~-1`hCG>lQZQCWz%)37m4Fm!4T$E9gLxQh#&xRGyx)ac-CoH+Mf;}xxS#&X zzAPBocKtmTy@=t#1H=ST3AbJ^cl~$|iylbZz7Pa;e$g$%SFZt%5g-}zO^*H`84}Cx zB_Nv9sX=t1zI(ovTw8wD_kykoAZrT#q0cRPD}K#R04T6S`D)c^J^b;6E)j@{JgX~M zIP~20iwj0Qc*@NR7B!=@-L4ToeVR^>&-g_E7sSNN^Y1_zD{t>I2-q{2nIL!}zOl5g zYzFtHUmjq;>YwaeB*?%ES&wmVZTA=hSjdA6R=@v)42S+#xs2zl0uH9>SJqz>7ITs9 zy6CI!p)kwxgd5AMMQ?A#OdGb9|Lw%g9c-$QNCDl99#zKcF(fSJA+JWsNOe*Uy&eUC z6q{P;xpW=(>)-{dr|!NFC#*}sVm`%sCO~xLakH>l^x}?>c}oPMVDYq-z+oOkxwEi` z&k&@W^z!?fwL!ctx90hPh1d&$VQ2Oq!{V{%b80^L@<>xk6mZv-L!A8(*=i-$=)qkiho z77&`wZ&eTzEBr3`GPDxFuu11+WMG-ptCVI_a}EIV$tuj~>Z~CnTCg zTXQCzvku5%taPM;Qrv3jaesU)S%mp}K@Io|(N9Qk+NWfEK=TBFjXUl8o#-}qc@^Xy zknuR?Jt-2u5qTUF@AHwWh~NCX+dB09gSnF^^NS`+CU3#gb8Q54D`h3mYX_!PXPRAA zU|otO6*`31A}wfFJ&Q&`lTjC%7nn@DlT&|dWHD{8_G^_IlFToxP3)+0w1K^5Y{s0L zF;Q|23wa)?3iBCuYzT|n5c8`U3k{a%Y?viw?1h)p;|NiSoZ$na?uTzQ(0aje zP>Tn(%6spOGPG%hE@p;NiD4!MHMOERmMhf{qsKP>H)~D!&X-i!3;NcbCWb8c*rLug zb@=+$(H^zD%m$78#Oa8HdsV( zuq{ewQUso*T1PH9eQ_p2ik>|oOOW28%+-y8)sGuiiIubLlRr-@rCqT-8LiBMd7~7Y zvoAy|@{rP-fE7^+no((|TuBdh$uNIG5*~_EvJWgP`Zi<@$*QnFeqCIimjf*nSBn)M zCm9(E<2*x;`cGGzKBj6p9b8mh-nw;$UAk$y4GhDIRiVn*B`H}X8A$VR2SS;~^&8yW z0|)6=ROzr<`8Um~2_4<~-uX401=?Fz-XV+Au1YmT#->drI&0Nc;+R!)t7pF(d@`C= zzgBO@bW884q%=z{!UGJ<&f#TD>tAYz!|@Gan?s#wf#w&X6!@oIQVDr3GPdwDD!J+t zyrnn@$*qVl##^9pmW3fS(~U}8dW~kDWpKGJew>YH5oWGkX5nODFA*59h96hawW0}_ zcwf3mqI&Mhk-to(KQs8xm_BFuacX`zExPIPiaaA)tu(9pp4#7VTWK06+(v!XRO-UfYpMyRmCM- z(}&V-Uro5I70dGzG!42q{j5=Q$vH>7EZ(^0zq1uBD!mXTVq zC-eJ^>G{z?2)7SPPlyZAo|=&`ovG{1(uk&s8b6u86w73(QrEm%8hpW#twvrhYlS06 zk>!4k)X;8|UX(I#md@OXRe*&>o{qz4V^b*$=8&rv8B`oZ(9>q83x@M_0!Pg{A5TzMraD zNB1qFnx9Fl7SZ=Yq4#_06SBPvJoC!fBkG?t19vkg*pwR=A)*VBaES43kH%TnV&(J*p;Ch97MMdPY|4gmMUDRT3 z+~$IB&x_Fe1LlWr1rnuq0>{G zY^*jrIa59q_8{S@Ao)x7&=|5t=Gsi7RQwpcrjZfaj$flpL1Hmn6McVQ?;im({wFCj zt!k1dt1Ij8tEexWW8V+qD#jdT?03MNB)HrT_($v8g2W1g?eST95-7JcF zaia@`w;!i(wIac#Qhky;Q&RiEUzkL@5k}J4MI7}xP&4oW9uO{0)%&0hjm2o{2nuDM z9hNFQDz!D0ZLe;|5=TlWo{FZnq6V7vj1iA3b88zoD#w8e&g4EPzdr{1xJZw7W44r(r0JQqrdMdZxbu$S(#m(YYl5K zVUAvc38wp(bqN`JTN2}WS0VMBTViqQPZ z?QutgEu`D7U%ZZxRp}`PcdKp`UTyay$r_JXTtI>oSVPh;f?LjO6!UY%tD5w~zdVij zhQpOkLLWADsWMDW>HsBO%ro1pqOS;DKR5hhW@|mSyk>e{r(@V@VphkTUwH~1H`|JJ zqB9hxy)f}qLwyxQDHKCr*3C;sPXCboqtcTg41!vTu2vdFuGJc$=0}a?gPzW*YU+CuyasjzUvx?JTkV2ZUh`SilFdRCAyti4w!6<+7R#kE+j?wJ z7gdY4>AfVLsykLBNhlrtRs~dMvj9w*(~g?HX@<5At4%C^{}GIJZ;tyOLQ^ag0>zkd zH`_G6I;C7`%#^^o-OwE4RQG*n+6Z+KqgfX+H&pdrm{=MaCK1AH5gCHvSZll)sqvgE z6q78=GL>CKZ-+Q?jwkMrSMBJaccj6oj`Um$X-2-06yF9?3T(E+NtwV2&d8-d8k-?c z?+Q_YaCD#&(`KSjMjg*a6HNyi8T}F*3TvgEKW!^-?0hv(y*Cqf5_}g=#iN6GvMSf9 zo7WM%bvaK0#}Y@UxiV?&ySQ;oaqXR&eax_?wX1J%Y4ajbg_Q_XctCS*rJ-IGLs165 zseg(kS6-FI+Va9$e~(|6vtvy_-rq;9yt*lZwi6{ve56tJv0S`{752F6 zP+TsQArL>%2Cl88$PrB~G<)=PaTHry+uQmD7D0s(6kBga9gU4!j8ICca?;y$DOHBr zO1|qYqhmoF-$w7;vw#GXe4QqawYZlE#F>5glz6jgMu2H$z+|q zH93k#X0ef0 zHM?M?>SYU+LgYP*BQ1tBY@IbpBp!Gxw;89FkqzkJZ`HOo4^f!+oG;89a*zGakc!7D zFF(CgFAF<%Y=qaYdS5^zXL4v7$J69p@Nt(T98B-d}Uh&s%m48@A2gTdTvse0irnN;Sq^8DTS&p0iN3 zJQCh=Ccf8pdk9gjll)`uZ0!vkG_Ptl6$3uxY{w*q$p(Ioz)GCGsjNI>lN}wAIULz5 zwJKI;Dz)9UX;gz!YB463uK%RKaj(vMgIvCgOlUPc?wC63#;Z8&*|}p|SgPmb4mrWp zVx|tI0+>Hqvp0!m)+&BQHTPw&JUNT5vAgNG=48g1j^LfY0CUOjnTB27AzAaqnfgqI zf6|Q){bcd@(0vF6auM9j#ReQZ-^1l>h!-)rqDRxHK%Pwaha0LlH+Z!BXx41NYn7VD z)#0caOGc=)%^fT{{;67Kpp2UmJ|psm4bPWsT@+Y_rGyU`f-lIPhH=?d_$V$hr&(XB z@0k`TxyocLa(ma|#IyEy`i{IH`G+~b_==ix=B}T?10VMExs)1n8TvRWc2S7ai211L zusCS9Ujl>^tlXrRJ!7XGW0pu4er;v^@DuiYJj-1tSeNCYUJM zb#ufROLFli%*Srn%z8b!z15iM(t$Q5=99%2%wt2<8#mtjSs>1eb@0EXr~UY@GU>~n zViLUe0n?>noq@l_AHI6U3voi7r4%O?w7UpxL{mid*dBi&k0Pe4|K!Vi&!SWgREDFS z-6n=pVOC?XyA;8kwfM3f@0}2c_wiXQ$G+lniUXlNMe4PeY2#`7(c|W!oQds!z!yv~ zl*P$HK*)T(Leym!lN}iCycjh=&AAG_V|Qb}{}z{=@eu#cv3(jkDEW(M;p;-0!xVPK z{%pjnRvWhsg4#`5 z_`UUQmR~P(_!1-34xh7Xl$fZ=2HxNNjj_3G@X#Rgm+nBz1tK>R(m0$|-EIoFhyr{N?`89+_FmS%~F zG9M0gI{4k$Ab^uWAS*RjR$88%tQ6({x)~xAlsZ&-l=`Njr>xYN?s(0n- z?-QhB1f$GnlrFFlTM?xYRJ<)VAQluhR0V#W8M{85D+%^9UV_Yn_cW*xf7V1Cw`#fU z`|r~O7JZs^r|KA~TQm?P2w7{j77lt9sGGW^!e7vy@peB9pgUIgkN8*K&+I!4wpA@EEyTUXc5D zkPbDWr|urCu7s}8Mm}Ak_x_m~?+MiGa82={-`K#xo7O`e#)QB=0D*zH_}8CY?Be*V z2cOg}Q=j^mxk0Yae52ZDzAUD(dJ8+BhJAlOR6*%peKVAfeT&iPmH+tap8fsM1$&8> zt_tLG@L!Yn^0aCD9BCtj1(%0?w9Gv@T@U>VmVySmbh1>-1seDSUuuo8c^(jp9)^{aajY{beYd+ zS{%_v*hpE1p)(O?FUsR|`Bc}t&DiXcN-QXMgohf#{c&;R3UBd%r#f;`VPT%{N49j+ z+vxNQWmJc7bAh6SMWA$%7$8A7ar4>LcjKT2SZIi`?wX{gLN|ZPDb@pfO@#`!K39uQq zyIqu?(^U5)Mo=umlUNA_$*}ZqNt?0UAQ-HIf&LGeY^-L}Be2Mo3Lw+M;+3Iypqqb{ zfObFlav$`94Kfsx_$YmmD?rmsm0f^xv{ApV4vxSTf!mOgzpx^*=r7r1@>*Kx=DF&( zaq2!A5N=gkR8IJGNCX}$8t0eRMtHjxl&|MPEehHkEej6L5pcr|oPS%FTlm!nyhuPtk~8oZ8P5% zfNYY|JLiy|{eg31&QFxeP;ne6U88~4-z7Bka`gGFgs|HB-LQa z63hRi2_^d9O{o6~J^hyq#mfG_(oifc9RHJsVqjol`4XO(=>K;biiYX`8Hf6(=D)?E zSpETZ{+qGG`d_Cb{q@x8WJbuu%JKgThXTE``WciMOIUX-RP_GM0D)4GVKf%pR~G>L zA2t*a@6~UF?#$8pWyrUwz(Nm6+qN;-y;Bo>7rJHBjS;n7^)G9F3-;>Rowjwu6bx+B z$uRfh8(Rx2oeNq=0e;Q(_A{MI;KiiOIq&#zRK_c_ci%S?B!5*@L7^j{yG6;7%eS0z zV3W(=_xeliG2LzNEdC(o0h=qQ0>Q>lya(^6;AR6 z5J!UdV%e)=Ne+K-kWC@)2OR-MDVZisCouNKrslFCcj@_gZ`ET)$)^ZnrKaZ8O%D;D z`=!$&*!$eEu6d6sSNQrg#y-?TI-hf5SNKE-+McEMCv$Gk3HDqifO1MkK)Gq%dS4CV z248Xb48DV}>b8mJ1WMWOhWD^5d;>fK7oX%#>T6ZoP5Qd;je+v6mfBSEcM1sG7mpA= zua4N{mH9_FKZ9En{_XH&`>)UK_rGYe{{tEIe;SSd7LHMviKkdJOssHHXU+w=X|FfSjfC~Db z^q=y-gQWhs8~+<5_0Nd@Z-bouzb@8)4swRC+wK1XNij0AGSdHVkd!Bk7wW>oQx*r) zYwCuTOM(?4f4qd?k3Ld~z+CP`nNbK(qVO>Y0jWu{4x&F2E*l$�K&(AOr*|q`ZIT zcTuF`6lGAGntwNnTUOjt=~{n9eej7&{8G^oL$21j8dhX}pbjlPzJ9vabGcsfJaz)w zOrtTOoS(wdDv9_rWvsLH-bpb4Bte~`KT_r!iHQN28@GnIpCCPwj&|yrrlxf%h`pxu z*6Nn3j*}HMDuO|pP<1|CLq|8!ZAVdmen>o{RLfcRg_lWoWFvM&0`@dliK$F639JT> z7`;9`EOW<{4lxq~0h-rsrIQVePoCWxy+=zo>Nby!&zw52%DZ<8f+k%kut6lMu{g3w zimzna2q9j5hXwrtUc91V%K?tKI`@XddC|OggkhXreH3XEP|3m-G?W}e*^(by&vr@@ z1S_FSQu|Q1-yVPPvh3Znur~y6Ni4?0XFfx<#H{to4Y!GnD%?T{0%N_egZNvYji9HH z9pv}N7^Y-gh@wxlpxLx*P{QO%&yg*Hd3S1SbF_X21-}ahA|4j^3hff#Aic?k=G;o` zE5}Q`w6;j3a>EexqAt@ULZRQXelfYDuj1erQOvCIRYqvH@%`Z9Y&{V*Lup;RJ`Myr zAX-}h>%&d{%j^RiV#rY*!t1+Hlgn@8O)+m->)&mTXn28El%LmH6`Y7+;%dDY$;l5^ZS86&h|GWgje0U@NTESCUjPz2iwEu{7;` z*tTRUf^l+xu^(bTvAQ_n{pDGZ=u!;!Wr2c)2qaZ9dCv@OMb;&LY+8zyn2fCElKAGT z^$uhQFVUUE73XU_5sZuTnxjYOR!cw=bp>%3EQNz!e3)OGF^R!9$=T2KtM zwGXiu%CSe?RWmk=hF>B&-qhslhpx{PZ&`J5m<~^>Y(wuDO;wMFWZw^(!oOm;D(NtT zVC?auF513u(}g6;Q|g(W9;iB6O1;{-3f9q|SFE2O zHU63lz#R?1^=WYk0w}9=0pvaxUJ_KR_y{#*i~E3k7d-TF>FGRJ_6h7C>zAx7iqg@n~Qh(DXL z@fYc4Jav$0-lNIPczmTmp%IA6aWSz<0z3)qqLbd+JnMNUo>!4L34F9R>rCb;<|!>b zCvo_M0q!|=rF8RjbJj88!{IxWqVQsgv0%zzphOu86Pd~3-)Hr`yG4mJ>9dJ5 z+%*YKWb{IyYJ^CYu@HuO)R8E&QSQOd4RS>MuskM z-L1kl!*Pwr_sRW^Rv!oqN7=A36-hRp?ZUx#<0L-{E>lX;YR)BNvwthbVdF~^NT7^a z39-{|M?K8JV5e!v2r3!1(bQ?lY{ zrUYiDyj+todv%JW+=4VMHMqHW%;=06r8B`rey7f?9x*s$x=Lo>U8* z=k20q&AInk6(7~rFM>kTw_b*mXN(#vi+vNjUa0lR;bQXA4+G$+PssYDBZMMtZgZ>z zvp|5dQv{1aASP-|1n0sJvv1ntJrJcdH5?Dw(@~t_M1~U69NEhpdz=FvGau~az>Kyd z(MR%!G+R8j#VMR~h)0{8qJiH(X+=godHobn^j6IHi9 zi7O=)^KV^I&7TP{p)a=^B(Jyp^C_;ZrA~W1D?nRr^%MsOCTHU$+6ghPRq;3`5z|?e z3@8t3@8Jy(2~)z5+n=ZH)U-1$w%wvpO%-I%eMZN{@RGanyVfNB|9 zOvsLSt5+TNHWaXuHVIU9c(q_3Cae>QpDE`qhegKeqF6NhHwx`Oe`Iu0p3hr9aHY!% zCX7%ciy2J=h=y1|4ty?TA_;JmUb2NoWc5V7vED~mH(0Y?dwI-*XNPD)Nf zLZU(@f$Ono6F*EK4)hk&BU#cc_gT~nQu6O-mZme9UyqDr{H^hh)Ha; zprUferMMDf8>;5Ysg1Fp#u>hZlNRYRuUnk#U4|$e$GmB2#*ZaUf&L-IU}-w%2v8?A zlo?KjO;?`A`wetMCojX@(>OrJKRwI1nRkAbjZ=E9_B`F+6uWBOs~fp&-BWN&{~Ydt zz`yfoWt{R;O%toT;_`AM@R>2~FWE%7tG$&P=;R{X;Yz-kCBu2d`O34^%O)OIm>Q46L13f% zyv`VZOtfrs*4k)C7#aps^%h#GcoXF)J%1loww=fr;ThEj@8w}uu;-qCV4s0tcZTH5seMWYefG?GAr%GZ>BnMoon_)lU*I!~{oe z>M-+^e$#i;60;Ss^k;7i-`AoO$=%eYX=G&0Tui+COpMLvGx$S(WLWG)D655Mc8kwb zY^iBYZ@`&~*CQol#}z+Uf7I6J_yeQoFMP6b)KZ!0euG=Hl?YbpB%O0~)M_Wdf$EX_ zTvp5zV4O@>qC#gf*6VRBKYuP;VDZRj^kaQ?T_9S%jl13S4P+pgK{7|<0h`15v`Ja+ z2BYABplEAE)@DgU&W&f><{-!I0%+$Ojh!v*T_2d`U2uIawfI0D=i{h5TU8Jx5uPFt zzrm93UZ8SMD8|t+rxK^!ZsXmOx3)~bS`JbKKhPOH6(ORmAuuw#$6By`z)>>}k=pM4yaPee5HJN(3{4}Pqxm7vI8r_-np z9tJ1pWoAf9En+KSaCS0p7llHCfv2*qBnycBiq1t{^sJpSHDi?{)W8kr{gK0!j_wdV zPWS41zWmWid$5_FTpUFXy_14dk-Zku?ulk;$+oySX0}$6sZ=KmAz=yzBAh|UPuY#S zDo|_7SZpQP+81QIsmBy=UxuzADTQLypj2pY;;)X!#F!k#s-lTL#mUM~W$6K@vh<{U zCTR*mRa7H$C`{}?j1KUq)Vt7v&WV8fnX>QD_V#^rPG(w;UYdR!Yp&JYi$is)B@9n6 zbUCe4r$xs%<`UnV4E>K9VYK9wKj$Fk8_nHsHugMt#&x=Q`&>^r1xrtjbJ7N6m+ z{x}r2to7XWI`cWax6App8dZ(f%gx~goI$Xjk4bC5?E-ns{kqef#;F{03htnm;5#Mc zySWnbiqQx9LrLQxWvoeyqAw@X9XYKg*X0eH+$8on#_gt}ti$!9CT>Zl2MkPB(zwTg z@j;0s!V;;9F#27$v-TMlz(2OFRPc-+wb>-gm;eF$u2yU`UZ9 zM&Kl*ONTW{Fk&vHoA_FhskT+v*Q6IZ>3tA@Cvi`n_K~`-Y7mQrZMPnPXYyS_1UFP z{dBkEnYK>zeCO)m3h64|<*s8W=gxBn!=QZ>M}Rz4cubFRchc%;j!+qsB$_3Xr8xE6 z1e=t@3*QK;&7hf9Gskk2DLSQIn&a?g3M4zbKbll`XEZxTQ9beEIC_a=9`s{>~$b(~o6vJHy z^oRGG_ds8-9=Yd>zMhYZ+-bXfjO4ki5zXAkL5D?*{gib}sjlZxRnn=KFe7X=-}98M zw~=GD7HRcDxsgh#Frr2N0hBfwi5S~5ILo>yWMG0x-5<4Q&05fIzwb{c`=kuv+*HM} zmd9$CgiHMJZDCV=UbmHYsg?Pb26Rwoj|{D*!H-D5ltTG+ah(O_c>1Y_W1BpmsHR@@ z`rwKk6^D+enKx!1m`_hH^6FqdC=6)JAlD&-n~dGFb}A>$`#o6j$PR@dpuqgxl;ma6 z><<@Sw5!G?RtOkiDeNQc_<;ULyBN;hk4J55SoY6SR&={gC7eHl(%DPHoRz`43&RkV z16$L>Wbv}_=6gYfwvIEGmBCu6o$5H6;grv+tIMK6`&&dJvh>74B4bx(hS7RW`BCl$ zwMXWLz0ZQy)Z5ENQ6h}1K1HmHaj2eZI5^_XU1B&Hqp+`gtL#LsA@4laRzx&Rb4_(g zg;QD;QPzX>cOM-7da9#6m88USR^usu{zMdyMrk{g@gYYCd1bSPYRzo^CQNB;9&bYZ zMZNUW-#rbT)56bbea76raG^fa%st&LP5aQ8sc)EO+h(fp^|ZQcu6qdcH#=4i*z&h8 zdRWCb;T*2f{p~^_l~c~Ml^L;>^OC`)brb{o73{*nzAv%t)Crg0F&D2)R6ZdidJrh^SiOA?nQo6AOHWs**ix`^7v`G zUAAr8Rav%e+qS!G+qPX@>auOStGaC4TfdpRcV~9Lb9U~{KQbdTPb5x!@|=i#pLic{ zcwCH8XYGzaI>WVXbl6~GzPe3Z=%z6;qHXAy6iT?#0(e#78xvA0F+8%nQEKZTt5ov5 z2a+7Sc;}W9p6CXCT918+(Y=Re4(&5BtT5T*jFV$+)n7bv@p8qxzN+f3c3jlDH~Ws3z6Gu zZ3SQK!jvV&o{O$l!y ziN03B4cj~{Qj(q&?5B(6z=oG#cK*LK=Sz;2(p*z+n zv&}cNxP)vb5hU0+h8x}S7lwBS&_A(?9>b>8an43gi-CQA6|WDK*lSoxClW4sV}p=5 z$$U>FV}n_!br2+gutrXa8q_e4fNs(NUGB14#ax*s+*&W?4Qv9-Y^*O*;s!9$cDT_* zLT!HCT2L^wN@-|qi$^=7mP5zN_IX+Cp=C_|+0Ksy&rUt^wn{GS{9MG_rMXim$>jJI zj7J7*a~oP^V>EU}ga-goBI6&V0}7sOWKk+2pi)Npvr)(tO{5<<%%KcA_S5M zqYD8Xg`8^0X9%pjnrMT;(Od(>fr*9H(;}G4^Y9G#;&eqV9;CJ*6tU>;N1|_QFUFl+ z-R9io5@zf#YNkUKs2Q^*hx`;VRu&xAL~*;4H4tMmXv}+n?uBM(pdoGx1nFsI-eg5+ ze1C8V?W%St8Bpe&+hAD4J<4GiOIJd|o5o?E%`Q$I`<$ zu4*M+xhcJW>eN`lr;^3IijvSR9Q|_RhdeWcGOpG z)~>DA(AW_xzO-sl6;|D;EE+{`w>p{}7)hLu9EW~F4pY(FsTIl|)v3`TUP)eTr#W0y zPhA{meQ0J~a-|a{EipF2_acZY!B#Z76vG z92PxVskpC|5r9gV7B1{YSD1Bnn4+RKUV?VXsFjGV>A~&8}6lUDRw8`O@;|-KOHn#ik7qdJmj*6K?i&|vg6t;~WF`>SYCY~%pRp&^TE61AdZEaC7 zQ43-$uM|Gf$Tj|*>WuK@o=ZadRu)#7!_zEUA%7Se{lwdfj8lt7Qa?sZQt9HlZAmH- zyZ%U(Ml#Q^hIGh^>_vH5YCgXGukxbQe8%_PO=~i$TD3@oZK4C@+@o|-zA7il`EjzB z&T%qJ$d2=_o5pW)`_bQfi!A9rBUL6*jd?7u+axhGP3E?YDx(-ln*1dxSthZ=WUl{Z zDfuGNBd?Mx(I_!b-j!6eisZ$+l7y<8E0JH-l{7{JZ|q1JPthC?nKCFO&Skh?{1f#1 z_OV}_FRWkOC}d`KeP;kV+2B+joQi3KC_1W0#7F_E6(_x09NF%i9pw%Af zL?E0z_<_-euVr9>G!aRf`x6|g6zS#_NpAQ9sQA**df;Y}iPmtR(tl;iTm#R;4QY=t z`X@-V71J$ehzuSuFLS6_i*O2Sp=RJXzTmWZyijK+_B(VigFx$#En&)-xC=O@HWY!>CgttEdG!xs`G|pGiKxMJ0b1V%p|m^CII&OZr+DA{m&ClaXe^J?{Q(^gz2f>X?0Ve>?>erwV-)hrJO)Vk({E=}U3nZ3kmDj~kNR@}8Mri_nP z;$MovQwj{$#_iqMN5B>Vej_j`)uB7q#;ZnKSLQs;^{jd1E6idgx{d9^jU85WDVe9n zmHgb8`aotMZY})fqUlmZ{u(5iaWAY4*NnK8rDSmC!H!Iud6}5F55j0(s30L~9_l`8 zBfF{(UFRGOV`iyEUR~$-X5Oqj_;KWUgN6A}yiGBN5%nqMzL}1Tm7?>5g80U$7J3Em z;wG=&X9hvx9EpZ8ywN#FH!7j;#G^s@U_IlpIYKc0$u!nCPk$Ig=1t z8p{zDnU==RH^`!qn+qnAX?1k~nLb6AteEG2|Z2p(|4aR|enubc7H9J5zJnZiV!INb$ZwUuvN5QUzrK zsW4(k*|6}j$ak~Sw52>G6VsMqZ?(>mD@e3Vdv7`j6NHv!&;~It$_Sf-7v+20M502k z5mzm$2Lb4LAiGavGG+*SI!0|C<#iS73Ny$vn&(&sW!cqh-7$ZRU-ootkPIo1csY49 zMZkmst}R_E6uRqcg-T^y_GUbutnyZ-zS!8=iku}}55sFbTW5t7&9vCPj|lA;^t_xo#0kLi)(aHUfdC6BewbEN5kbHTQ- zt9S=G&vmmAj|^;VXJi0 zsbfvNs1^&-x$y$m`bS7h?ocCG=D2Qbzcfp95Y7*-;EwJe^jgh6@abj=kfi-5cG~^bcl}1f~UaWECugpaj z9qL%E_mbAMzpz7ejpnaSKX$4RG5(wpM`6KX7*z>RS31f#G7PRsb#?$a+Ku6-7x!53 zX2_bLRZmoR=-H$2+T(5uolb{eFqaB+R2d@`r_pzn>2~wVYElU=p+nJK1S1V#E%_5!Qom9qhk8OCjtPyKtPa1>As-3ilOejZORnal_1e$}#=Wsh$2yLO6 zFkYNk=oF;v;O6_zB@U>J->8&>Mwyi+v##ah82#$7)8(HUeI_|YCt5=luU)4oDURzo1180( zU9c7HfIMo$b{uN3=7odkuXNN(FsY_(PDidN%^s;nyJQ0PqH}yfnzTj@7p8J#v$OU- zOe*u64u!o=-`N2p&V0SQnNz{(&{KA7am7~WF+A#8%8LSYTH{ZZ3P%n0ayT?0E*L#l z_qJ;tYoja9guYJR`~BEDY~v*h?>{Hu6U{d%{+S_1%&RSi4!<-XY$bUcjjp(Enl~iu zSLc2kH>bv_b^SUOp89jjt?0dUX8O>k1riETmH49y1SBX$l0rKs=R~l^9U1dBSDeag zb?pkX7>zf|sCBV@e&G9?(D4Ma&3Fq*mUUr)uX)rdotW%}t7Ry>Z~-*9{4VN$?^t{t zx!{bFtP@gCNTdjNehN=+30~-q&j4i3#(?4M!$r0yjJOK072d3D2deS!_#$n=v9?t6 zxirv6+R{$6h^^vc=aV(MBOhD4-)k4l|D=6TKLiyMedO z&dy48Q^l5P)R91Y*x@ZEaM8}EEvqNWpV3Hu_0*xRs7JbDe%e2#6cbULr>&?biv84n zqrOimMtYNCis(-7PIO82?Za45PaLJVpc(qCS}V?Vit&+h%c3TYPeEPJFI( zO(SJ)>Hs*o??s)^OPbHs;+FN*xur=O@ws`j-VM2D6xTVVNn(GW-PLvGo1ja6vh9|! z&rm%<>Jvzx^xo#0L0TFl8t%%sr?%ttpYs8yB z)8qcV%GTW4wQqM2{3-p0V4oVWR|J(P+gBmNkUz`ex4eEUcwcdJYC~R8UPFLw(T8rA z^3F@29PP=yN9U8}ncDq3cRuhPvhmhPa@-(tdm}-@@KFw4agz1XS0BzzmevoxmMv zKji|aj&$?RyAKbAO-yxFaQn3g&K|7iJwR ztPtQ2d;#5L*0%&r`#U9a;;gq$V8Tix6k!Vb8TFcawpgGDI2Y@?)wJ6%v;31187=1@I*thP5vQ zi;)Z{vd0WL3hg-)DiT`OND&w{GvA;=k+#0ygoGn3qKYI4I)@8-*m?#+hyPv48g4_i^H<~>lZLm zNgCciGF$&bj{b!da(6fw0jRC84}v~&LE2mepUBoiYuBOx7t z?OO)P&iQ@C%)$qe}K za)h7I3=5*5Za+zpg`_0oHSP=MeSiE4L#@D^_!meRNAV33)=%?X9$ST$Tgk;>r5-QI zR)&N;17V^r^cDc2(S&2HUe0NQw+oBbgAD6sM!mp%9te%X3FE?25C9@S6mU&AtLrhNhin`BVl`!U) zN|f}4dIWQu7mRRjgy0rlN|v4Z_D40vwW*B**F1&>>S@7$?xYm zzF+SjFu#BMiTr2G@81UQf1AO7@I|uyUwx6xjEqdo{|WQsfih58YJQsXUUhP_#|0w7#uW zaM@#(LJjSv=x1G1Q!{5@qsmSex)r3Z)#v)SKHl`9o2liEXTt1)aNh&Nq(Y@dpL)~F*U`asl5 zKJV~L=#TF@!;C_|@V}G16t5-5os;|Z-)Qc$A%{^L^~E8mzdZd(%!>Z&%OIpjuTRx> z!%es?N89RG(Fq}`QM^{1=8Z1ie0wo(=S1Lv_KP8{RuP@5ilfP3(6XzFhq$W*poJ|N zhZ{eW=sub@!HsC7@Hm#1bn$G{be{5i6k|+&0wV#RK4Wp(uMO9vu}LFSicOnMnocQW z(P&K=ANe+q#5Q$WAy*}r^li+y0*2^{To|32m%NK^Nt$;ae$X?mlSXyNrq;CfzVu!^ z`6tR63@!jAX!ZrUm%^p`*aj&ziKn}U&0 zZF0+)b%WP|Qv;`Uwi2VTgiZQ`PvGTdHf>^&{$PGfgp>4gEGPIWD#{IA+`k z_~)ky6DcVhDO4K;ZkujxZl@Dr8{QtJwvy@y8&p#cP3b0h)>;m~Iv~QlLnm4C(qYbhjzrk~eOGC&<6guAr3_XEBjn@S}`T?lwt~ z{3v=8lcNZE6MKxAq;62twyvsK;>*Ad7kUX$e~4Lox$40i<5DF>IpKVc+eVo&-xvfF*Gy4PJ^fJV!;xl&I2@uEOnA+}PQ7Z4bGuzqT%6e*QIbSW zE=OBKM^hqdvhiGP+qNQ8Tq2s3?;xd#o^aPt;9|7%d_Ip`PRYYKUw<`+WW6^ib~ZS? zAo-DTl~0FF0brXJ3Npu_w!hZj)Y5D;o=26``lF)CSJ7#6yMKH8P3@*k(zqsbNTsFG zGF((HN;S>1rhrwIgnBq1$?(fkH|8^C=qP2U=u*kjR)VW+-DVl~_&u|IC`os?M5?%4 ztZ=adE|z@wv%l70{${KCbmMhSS53L!1E@X2y4p_ zCQl44Ev&Xh8Ex65Lt&G}=5I9Mo22EmMW%KpG<;<@XFZ5-h!71ieK0*rrm}d)mX^E; zUV}q9oc`r+=zasb{TN2D_1P6h@R_^FU_1ZzAp0uondg%9PV><_diDw=>FXnsSQ62$ z-XpkO?o~oJuVtCjBsw_w0HLpFRwz>z&z;E^Vs3InN;6eaohV%_zMm*gZreD%Vf9;s zeX6{eQ}tl2xRbVrqodkPY$u8Y^Jkpa++<8lgoa1+wQ3Vf?M|Mi?B#WZlCh|@V;~5{-uRqNzQk)q2w;^}6?K{8arDeu@Bob3Cxj_y-qN`zUQM{0Q6ZNa z%(eD|n8yMPj$=B5B*U2o`0J#6Kg2$1>gL#wwIATb;E6L**q6Qz2*C2FIPUC=t+HEv zAN6i>v)do;V~C|lndx1Tj6x7+Y3A#`&$u#0-*V%7@tqGE>*j_T87JBIOsBgHJt39( zB%DgbPtjvxY8-_%VzTcRnjhq(ve&4*$(mr&g}Q%+0^cI+6#)keJJKE#ag$2-&* zzKNUs73ex?eE97LjX#>a;V>fWVHx2p4t!t2J(6t|r3Q?=n8?lX9=68sqjuTE)`-w! z3&tk$;DcqV-luY=N?1a%jG!~un&fhV_e)K)jWEYDFpfZHs5nJ%P7|k);Y{6dC@J59 zIA;r^C-d0H-dSNJqi61U&-)DO=V7b zhOG(t(PBqgSoIEP3(Dg)BET|Ygheu^p)V1ivD)MDg1X|1yH=fnSPS>@wJ2Qh!x}}k zsOj>Bvxi}yFBHneSWFU_?DTB`^@Mo{11%BA^tx2_9rw@rv)hgjXELl1EBaB6VxOAL zus%L7PXZM5DZ_;Z70Ny}Xgl+}x1!F@AkGR4`ohKULWq5|bQPkSp&99s)u}BZ*hnfZ ztJQHJ;c~P_Vub>@mf09_FXNc~2yr6^0StGdLO^D57H6`G$UMP);BY}DjRk-qyd~uf zBCYvTmqR4K<`7t1`Ux1Dj3`o*XHoh&R1e9 zMAhE~m*wb%1w*sbZ0ImdtQ?d5$E+fd55u!YzJP#n9!h}ds76m}ebNXDbTCpj$lz-N zmzE}s*qkS8KPhV&F%yGw(OmpU9WnnYoCi3F7GvpfD-Yk-4=zjaGD<`Z_fZTSp}UoA zYSO-^Oo>tM<2g{QIs&x2C-`9iJ;;f1g00~|+v6b0JzLnrPSSn7|2+yz|CN-($> zDilznAR_G0ES#coiWgaSnMoRR@i` ziqZ0EL~3XDe0bv)!uFM`yE_482qR2S4uQhT6&e7$%m|7~OukMKh*hwYm>%8f6$~`f zYqX9iDSkmEM1%;Zm{9Jqg1eo6h^%_ z;z8)XuVispC{vjz=)3~uZcIa_3I4LfRs@{Zz(R{yIxO#k-Jx_{XvSa#d?7-s>~h$< zWM31EaXOO>To_Z=;ucVhJ!U(JDPKNS*?kq$a-CR0A^K!JR9w4?i`SaLtTl8tatibg zEL`0F5+(jt`cl8`rX)4a-!h0pDPYEG&*ciAa?C?RMiGy4-`4zbgAfSx$a1*nR)r|s z>gL9QsdjGdKQ!MWL}#}~di5hG^QfYbrPU5cJ!4~<>`T#{iA`7{ok#d5spYltb3B?g zWOMPT7*5Z2o!Cpe^%JA%65R_qh+0nW%#E~W4(|0;p~16bmqxcEr81m|c5bjr$?=op z!@*Q8ovA1XEB#{|_jdJli>ie`0RTk~jK)U#j)@B!y=6Q1U*EfWwbOPG^A>^)c+;_J z7*39~uKCCkaS-w9OrR2AuE zZlZA43!`FqLel33a3Mw-XF<1coL@-PXGI0T06@@-CL9aIVMc!1WR2P4#_VFUPRnJ5 zYwaQSBHFWGT;h{v*Vr-KS(gBMQ2b+KY0{ECy@-z zv`i81CCn6wri)jkqM|dR``tib>1A|EXtgMKu5n73SsAp-v@Cc(%Qecg&l{)&a5Rrr z%rAvT_vC3tmR^*)Pg0~&96%t@tPzO`=B?F=$nDjND77MUi*81qDu;jZ zjv29QA8a8)24pm#N&!XejTd`v@`FFRHL^7DHN=##bCS?VxD2}iD zLPVhmQScMKSsLEMrxTGYsz*pLDIDdS4Kq6kLjda?qlypkO1;SBJ<4wx*at0v)0;ik zofQ&qy#A1D?!a~{V0Pkar7!?s&{DQ2wO*-TIcykYdoOh*_z93qFA|EF`gS__(_h(Y zAm{tNG17fEVPM*f`!_a$kplYmf|Pan$Ek}RBt%yY#DekUa3b@5xv+PZ?Na`+3LEAU z-Nt(h!&m*xey+yb*rv+)XcZo!Bv<>H3>I9ovZDpbdo9@DFx3v)<)5EA4$>1}p?_{! zs^=3ZJ$v6ib>Fc=7nFr$EXK1DtV6W(k>87u)n?`ZNwk!XW$eoWts*#34`8(%4n{57 z8zcGq6Bm0C1(lsf^=abGS7A&>L$J{-g8;D1E4?>KkH+xL!GlJv8h=k?MzCJDGEAo?|4A z)k&Mje3PKSNW&x|B#@8Y#fXelsPJZ&7-TLH=EC`SH-**N)I&Tzct}6&?ovw%3DjS< zVRHkii;a5Ogfa?jw|)5+>@}GuA0*Z}_1Q$W99VNqMC#P1O4x8vUZAm&!o)b0#=%YjvN{S0ou2A-@% z*lK&3%2p*)p|rr^F!(qxQd;{hS(uI*$so4_nmB5M?jiBviu|5=CM?Y{yAAE-X*aVXUXUQE5U(;%Tyo-p8nafCO zHGDxcQ_-LPv$hJRDCl5Vfri~s+A=D(NH0^~$v}C%egb3pI*DdLhU4b9B zZ~E+Z42|6m4)?tLtn8p%R(|h6X+QQvu&f&ZS`{-ce&ST@ZR%~ZeMroGNB zdfym5?L2QOIh57O78VVcMWS%l>?Lv11k5n(-)SBU})b;eeK~#8|n{xkUF)J*j zCusZd+Ci_^6v>ve(U>#$-Jx6^w(^pN=O0UHZtDdapgmMOl==WghYHm_+p*S0r7NzJ z<_!(|)rjB5MXB;CT@|MT933(4h4Z_T^MdBkKN{)JDo;n16D<>lEw*23%@tJc7*t#A*E+2$kX8hqf`@V?0 zmA(pQWLJ_&R7+V;VB=OZiXg~pvV zKFF_9E|c9#QbkD%>V@LR7}?p~$Yca9b;+;VWhiUvQU}^*`X>&*uXZ}5hu_*1zg3y- z(#a-nX~UOx+1Z!Kyn(KonPB{Dy5!VG=d)Ia`|cUX6s;2?;;tD=SM3yy6C(We6N7SE zX%W8R^eAtm?J_E>2AoQ|w8gB|wj&=&E8K;~ukU-Ocyq#@;^)R^cyj}fu}Zx8aoz4B z{32J!5-$AfTZ+3Tyzeni79(N!!Xp%2($2)(eD|$fd|};}!PS%{6EBI{DbD{HiM#?orSmninvRo2i3;(zle#niV)UPSD7xC-S`CoXiQhLg-wM$n=?( zBIP-oR&9LHIq|0)>WtD2Z9=3ZpAsDs3mW;US@I5O#Xn4M50~1{DYGK>Q*LjXVPlhDNrS*O~B6nV2NF9JNyuYe9kJs&8 zS7|*TY-t^&IX(~F4UIEM?Yw-1jNc8__aHv(}u7g2`=_@$EeMYur4( zSG99P{_;!fxdQ%cCq%qQt(>QFfJFJPAs+txP9Fq1CBpDdfY*2m1bXiWe@6A@_7DCw zTUB3XEVnCa1vhQO6`!{8THsa&4BzNU-|7K5JYN-H{_G!LwF3w)&)x9c=wV)npSe!0I6aqJKem^4YEQ21xi%{X@VIQ2cJ!$>YX{uzCe*u&25es( zw3EDPuuqpcym1t}b)mNV>+FYivdZ7t%X;a!Y}!218brgg*z45nw!ql+$?dya)Ydw& z+1SU}PwJtHjFaHmm;1y-HZ|5<5bXpOaWK6}Flt9i&eD2UZqClQbi&+PYf-z^AXk?A z)m>Y~!s?E%P4Q}%cci&=9)`3IsPVcrVDx8qr0{Aj{AcWgzc;oM9z9KvG?h-_05~4~ z@=hU#H$D6S7i`UTCG1JPFAePQcuF6~;5ZR>*eLjOHT+JzEYunyW|#mj_-?&iSTonN z>+#b*^#|N+Sx_AifI$_2*NNIG+fbH9ovU+Pl3z=uxENC})&q!E^@y1of{Jq?xSf~?-;m#U@`Y*fz6xDgGi(53$g_rxHzs>=3PoE zu*Gy($fK62A?~@vf*y?Q5h|rDSGKcTx04)Db`FAhPDWX_Dzy1wAC{g*zOXTqw^rDT zD5L$QiK!mlWR7z6WDcw|LGsZokEtBfWSdOxpZKat`p`95Ct><*KI*LL%=m@GO{32$dpV)hDw>*%1}x$fw=^>^n8$IkE@Lbfjo^)k=)*&*+i=al>CHo z4!UHt1^`dis-^@pq(|A%4~kdK@%vQaI3)00s3ac}8bA3ChUkl#6T$sXStRf z)xHk-0#Cl|?N7YnllU6pRG}1FJXC0@B-#<6P6I*OsMs>~Tw=|D{eULrTRo2$m-Vk$ zf?G$JYot$=_%CovGt2aU%r^gLD-Sy(E8~BN(Eq!oJVO6_gia@9Z)5UbmoEo&j?zD%q0Q5X6A?)I3UR=SPc3`uE=xr9|hvQ>mD2o*dTfy(osi^NBFI=LU{S zO?tMRd}z)WhX%hs!?jSXp;Zqu1h+-=*~im5HyfdDY2sqxto^U-oAo=} zW@rDd!D0PZMGfP3*3J2kx%K~9PxEgZ_^)vu>HUP(e%B?>DvJ@JMuC9n6EN z$n0XVT67{C(pbeD?TG^f0YS-f*;%@FMuD)e=CoWpID5Qs!XYB6{A+hjw#!@X4JffCztm3 z@u$zHKPGGl$(#lcLqeVm8fs}&q_ot;I1P1GrM1=BDGqi?spHfhg-o`~@IICDOg0|I9@UU7F}!20(pXt1{r=X2J>o~jcV(3y&68rk2`RWhSr5&dRmPF7%Z49S*uyQSmRDW$}#W>>X}A2S07Mzo;7O1 z*9E0pjlLAK|EpG-%bDHDb8XC7u z8{SfIHd}P;a}RMZKFO?Q#>NungX^Kd%Y{Q1u+@pQe$VKMhBs`b6KY)_%rFwibmVyc z7e*irw?P;oSJ3OrvkC9@-uIr=6R0aManN0{N4MikvF1~L3z z#!qxTgqokYelZYO5YhT*3q$8dTZT#JIGa3|5q-Q^US{ftXEJPW7A^d5XXj@w-ZX!C zrx<~4%OM`-kv3qiJdp52{lvC=xsy45*z$tNhg@~Ro$^P^19MemL}iSm(`wJ?jdYJg zZQ5p>WGrQDFn2VXu37qP(<%`HrS$2;87$x~Lc4yUdfw^XLq<>9_2xd!>es`(IU?mo zAML*UWcoe|bVSgLVi?2_tmBJ*`U~>R`!4>D5o(+qjrU45Mr-OjzBzhssAsTc9NQ3{ zGc;|!(ty#x-r%*$zBa>|EA1${39w`OozNi=fLc9Y;QLX0w}0A47UT~5$)%i*mB-26 z$a*(1>6H=J*nZx3P9YHynIBVQ0oHUwas822WIf00PY@@7!!MLqz+gD}1@6;l1m`9k z1>&-qS@35e|KQGnMi1VP=7#$CYwB6C6NW!1Z=`3iZg=JS>N@P%8eI4cW~UCl+yK52 zw|fWvP_Z=c1?1gl;5{hj7gb9zohi=h&^4=iy?X!C=D6>uZ$DFZ4$YyNV^(V#TD4Ke zF8Yw$5KN>xsy?O1K_G5i`nS`)zK*45AA6O)31rGZ;}Pc2T?X$H>Wt+U_~RO_LoL)$KwO+44;JM$g8$3gHIK>=o?)F?DWhL- zC%p^FCzp)Z;2(KM`ConfLvHz- zSB5sklTL%bz|E*U!z@D-{ZO>ig}j;mo>(=Z*$Yzfgy)IZv8)t9Z}#!ID`8F=Uym98 zr0@b?kLWqlJJ2)bC_hP?%@Xb9CGCy(3J{+J8i#FQ>dmAsHc;~avh4+$5ivW&@kUw* z;kvOs&%61MLHYJY*)XsmZ705jS{q3hj68y{TdJv5B^1i-?2Q`?@(aWH$Iu`u)$YU7 z_(~Z(+CGZ3u;NV8Dq*q^6vE`YPye`VL9;l4y$5pp>wK-LlbSJZ+UPu2G?}y+rb)59 zN9aidgPOV)b>=`yv)inR#|Ojsj(ac=^xIjxZKCyIQR?sr=Q5Otvs9RugCiI6R zJkM|3n!tCxcm<5YsDRHp0d)|BRxn=wEhBuxOT(bal*y<`4;hNrA3nX^lTttFJ;OR8 zH-gOyAh}?)ccxMprGS23o}O=9w}5_Eo-u$>gLTgMX816?&Hg1SzXGy&gBy1|mNsP( zN+e}vTU=RMR#8_?O-o5d#VCP+Q5t&}(c?jn2^JP)`upU;Yu9(+vTCaqmBEe$=b58c zq(+5?J7JKViJ04}aUMr^d1SQ1GEGT%;=pav6gRH1$sn{m_{;QlXs9Wjqoo*>EG;VqViZjbXL*jx?@)WZeFoNol?3bza11BnJB}Zvq;!FqC)TeiQyL0RIePMJ?a8hfDR#Q{-f}Q-U1LZKoTTsGX~ec4 z>b1#RxkM8;8xEj<`Hg7aiEqOt);FV_`kQAQ4-RRQ_gB@0%4Ks&2mfYDYo5LuN$aB# zy(QbP+P^x3lqckjQjhq^yl%Z7r>jCAm~H)iUzC9p)Uj!2ss9M+B#-!eGM51Uk=?Vu z=3=jl+ty-YGNuDe5_NcsO5$RF%4y^hkm=;V#fW{3*)2|_l`Wub4*x_;P6sUy9(;_E z7CD>vV)$H2jqIXQKqp<5kNIvrbglBmjF~X)D|@#~=M6VTG4n)I=&iAN3kM7fN$_T6 zBj-ggj&;x@2uk{sIois>5Asr;{O#@zDzZG&P^KUT4xpm_0{e7LEHdk30e)}m zWi5-{&W}k7(&;K7BuzNi^HTldq$MKF(-EI6*Yd~`HI{P}cDZ^K&z%)^3CrKj0h`_# zwvO`X>uU4d6XC8wQC)p966@#KA>7UHs`m3#(|iVl!bUUuY@?!aqPWKp{Ln{ z@66B%anJBXS71D3AVkuryE`wc-;Yf6)D(o8)qrfOZ+Fnw_Xf*pwGs~eYbp%9VPs4h zfbBuMGWnUgzv^<4exH7L^1FW4fPPpe$LBGXZk_3g92jD~!PH12yVdk2^)cZ5kjyXv z!F~$$*l1&3&?JI?oC;_-bNCh6ZR_p0zHI*heCy#^m*JKZs#+;6;4_O54HTN> zBhT+B$iQClBR1~_GkG#?jri5G!Ir0~D@al2?_!kum5GnS*u~%r`P);Rh4^7E;4Weu z?v^-}mCz6-Ei(8nCM%RxlFZrQ8S5@8UoCcj=+##hllDl?=mbJ1Ewws+ZPDyu~~3#(4I$b@pL z;lDUZ)&cy$l4sg3IQxMMtB;}QrkVKVc0I`SIrz)?dTt^1Cg{h?&B(JcrB7kaC8i&$ zU5Vs$gt1vvW2VlK=oC&#yJVwJgoTUMBs(`@!CzR&>1dhM>tayQPfb0j8L?eWqE@ub zIc738+5UvTtZUrsW`jqX z{h*1wkIKkTYf60Id6)I!w;}gb? zSxTX!MRE|E^mG-J3`H}8#_MyXcv@VzK}}_>=Z~dHOLuL_=3se}x3J>^$EV--Nv= zB3h}EMK!ZBdZ`p!Ij3TaG$T_M1(2HF{q2-(q!}7=1T#DQnO2_q^G(G^Pc)=*kXw+o zk6XNiQb~Oofl*#!c!0Il%t%;P)3st>MJxPU3+NL}RDmhaH&@@1kxnB}dps z(pSg8lhJW-NP~)ySaEGQ7JJzslWqyh_SZs_f`x!2mf^$&%0N@Fsdm|x6=jCZq?=2y z;g+>47w9Mb7Tk8h2EV*2TAItYN+=l)$#eqMW~f}|Krs*uLX>8?`X*KOA?l^C;0=;N zWx!p)MY{Kq7negzAH9BLk@!!^Y86j?f;l==G!PJy>2SO~BC_o@_c}cFpZe z35J$L{_@A}>+z%{WFm`PslKF@P<30Tk)J$3Cs3EU%)S4+)XHjx$IF3%uq&!fHIui% z_}Ug)f176}O@o|52fZq3?Fbl>@tM%zI>rDknT7pA80uCwbQtu7Eh~3eDN1yuwg1&W z#iIcBXydjhB;viH^Y&E2xQTC0qh-8>x?0>Lhe3Ho z#Clv!R4s+y$_r2(og6k`@CQX^k!l`0M5qReX$!IVDgOsN=%{o6Rg%W8Yw2)rzXKs6 zbXcdMih|ThQMrYYrlO*yW_f<~W_1x7)`C`%Vi{ZpuUleErVbFsp3@&Y=eF~f_z^SN zA&Jjo5hFuDLb#}Kg#{^gMEHRz+g}F5+1VOm=nX_@5^ImaQpi1pnicE12TJ4Siuv`x zK$ZUiML@d0iqX|t%t&2}8Le#qACWZOVI00A8ktkRAMZd>$T9SkZ7yTPvZAuZWo+31 zbFbv9v?64|qP9n~S;O!eQBwrK4o$yCsSzp;;JeUfnXwaMSxabz&<23V2%B#^2DCy* zgwPA6mSYfMLNHfwg-MEr*6cgG{dJAj0f@boax9d{v9o~2yKH%li1O@Ke1+mCFI-_P zbR`u(;a_ke^)TW5H4S7mIysiiPPsWY2@>J7;wmFw2a*_Mgg!@A`%pOh)h?Slxb1ObCvxU$E*Fb3EcfKYYO}_ygM6uk85n!yWhh=}*wN&jKfMD9oS_deJ5+ zKO~e0k}y+PD)b3I6I50MKi_5x8I*d>e1#&UcX)-H?e>tDeUTZ!H$CN58uVI(58y@c zJOeaomp@hRcM??FVrm?eEoow-9)3I|NBMRu@~9NCYNCHt|=X4eJ@ zdj-p8M8uI!t{h3`=!U)gu~#mXX30`*<@7fm-A^GauAs5o9ftQMAo^Rm~ z@+>b1qd6#Hp8tY#{wUYHek{-Y8{wHlVqpYBD8B^Em(G_vU_zza z%_+UVa0bIVW39S%M*v?Jcs{@c+$mcyM#z6h44Yzs*mW^BCL~`N#R1mhAUeHuVuc+y4QZm8oZ>>p9{F1o=Ka(xb@z5&X{3ynv>(s z#q7(kVLjoJ*FkU3h2G9X6?lns)+4I>0*@53F;z5B%k^0|xX<%!wq54F!FIFzQPmFH zeeNgHdsWXEciQ&2_xWElo+!3!v4Hb2dx!ZZH*;a(mcs3Yj~E{EY8VZK;EIz9yMIhzy)$6S3nQx_Q@vL|+1 z)P?F{y=g6yJtQ_LdI0&Mh&B7X>=D6jl-OhPFgwd-JCJ5n=7xBf+GF%yZuHstPrrHV zBS+q^xw2*R<|R8t4e!(%m)<$!o?WuyRr|BaPp_E1VcqHjOE=zed-nxTnfNOwEU(tO zEM~3AoqyNT!FLFu;a`~fmfD$TuAH}sjQwU9`!tNbfJF4B)TwpcZf8oEam-9+Df1HZ zlJz&j2bK?n4?X|w`XumAhe7b>d&`(A{}j)efrXy41Kpmkz{Q@cJ-2&r_dl&Lt#|DA zzQDdU#Wi4>;~?OvDp`Ns<@z8r=RVv-GOiqQ1}4;L~@42T3oxshP!PCZHGXJ zayB8D|JW!Jns)3M8LwkU=~%%~5axVrjCkDTuz@9he2>Q#@G}EB-xC=Eupn^IF@|u4 zDF>-!^@A%o`H0zT8l163k}4 zWFGds`ZUPA|G8xA6OUa3d$uZ>%j;M+C#$=pt`)Wm(?|(1)2f83mUaR{cM`W z?EB^cd|>A@t|WBpAlR5kQ5qTsET;|Q9QNUydd40ZTxj-~1ub*0&So?VipW3bM+KAK zu6W346ihn6n(F~;>HuqW$cfL8sw@u9VRJgzSMjU5cbPvb{-kPrF4x8E8U?YLmX0nkIFGv;uv|2q6PeQ=3ra&D|%7*#KzgA4Nt|9y!ZCTZ2 zj)5BW7i@XqSJ*n|qa8awA{T!0`OI-_KKMK~XO92!?*BS`*PTZW10Eg99%eHzHskSM zr7LZX-X_L!CTc~U;~x_rtNeM@>(*Dt{g?GU$9oNb9QUR5XxZoEPFhcteK*dcvnm~m z2F0wyQ;HlRqX{;7uQ}@bye;Cwp}*uwY7T#%vtJWA{;B0 z>g&qfE|XDZ*H@#mlA~L+u9+U_UIAFA;*6svO2osuBtTLtz>dwho!bF#D#DqIcJAsqO z_(5669vnP|$bh#&FlYvzF0=As7bBIr)fU++UR62HB6_3NsI$QyK((Ig0In1vu#AJG zqtO*WPFF*Hjjt|%J?`4Nsz5~m*_zDMbvWV?A9jb2GI{pY+RC)vXR@CmCn$O2!L)9$ zRRWUi4ms+)Rl`_Dyc=uVs2isWFh~PZtJVfV6~QMLZ-*2F;|T(dw*ktGw}I;8CG|kh zkU~!rvayk16aoS6mYwEdS5X%`b)wVkF}`&NxhFf<Vh+Td-r`i&YGEd`N}ijx$y}$=>s8jo!{*c>f%+^$@029 zugU5PUv$Q~4|arXMz{ZQu-WW|fuhC>#!pYD#j=%kU7Lum*9Cg%Jiyl~+$B}Mt9)0@ zyr6tR&D^WrrQD@v*Qz$Fn5C*^>SZ2wyXSr-b75dN-ovooz^VX)Fvl={&|0at#bgiI z8T&-RZg&YGOCn9JhSb8MR4QE*O33a)BQj#+#GJ=a)=YR5<)ch=VNN}%Dj}JM@`=Ue zuFx-8$hDeayOt9dR2;i@4*I-+r!+a-K+hT z_HFG^t>Qf6<;I(h4;Ww4y{A(;)mTM5BphBR+0`7UQim{a)7Z^qO{h&F=o16@UdimQ zjYL$n7$c=VDClfgas&8b$(Bl~HDWCI67uq1(c9xa=v8Wkf{q`B}eOsi|qte zIY8N{;|Gt)$~xgAr(1kRw@0hfxitZ#_2^}L4+enravL5pgv_>6)sRu9UXBm}$zGuy zSharboR=zX2Hs^5|GxIdC#Z?HoscBEgc^5mOINTjnH;NYJbFwX1OC{* z@Nxbt-^s2#ebIi9`2`^JlX`=QiOes~{%L1CJi?2KyeEd2?4EMS&W|6({xLp~sgJF{ z=y$!D!Dm0Z`gJ;Z-J@I~cfEbv?eDyE`>pS=OKxAVukqwo-SuxC0vg!Qs5mRL3Dh39q({#)$gN-mZt0_hogC#qMd@ioWwQs%IqS&>nN8=P z56@I(v)oPW5|qcLr2F$6ap&diBaZu>1I#|h9;X^HJhREM&GDq;Imcl~#-ZN9^fQMT zR?VsHF3x4oV-j4RJ?^aHs_bWQXV~X)^KA3&^M(0&%kjC~O8W}u3SmXwh1~h}+Z?wz zA7CEk9e1NMIB)56nvuR30K{?YM{^Usc>&ZN%aaU_|fBk8V=N3Yu;JTS{IK_eN6sg3{v7$}EaiZO31F@NhkmwR@E||p@!$`%|Weh3D zkaCQ~o6DF1{Gen|h>Pvm{%ad|qpem!rXY*TNwmAFx{M~1auQxcW2k|~@H~y7G7?J` zhqFu}*~>R6wka5-Xi+eVXYt1>ZCJ-Rc2wYxwz>I1k{EP-D)57CZr3sS z=7~?qP2`fL9U)ulPd-rx*bBhmu`3jXN%c?o7a(?x03?R!il4lYO#bVMskXMYYfsNR zJ(F+&JfipS%~K0%jvrCH@VcOiW#jA+3(mJtdF5a^=2(l}{uHMSn(ub8p_g zY+&0e;=B5k$i6(xcn6PS)wvsH9v!-wvXl04!u&+zO5 zQ>%BCX_dFnv>9#2o0-k*%_ie?^)~fA>PNl%K`yC1gnN0jCw3`x>H+-tQydr4$rB!u zQKxjz7DS8H=(y330K*nZW`ABYx1c1l6@YO4TNNxt1bQ|!f68wzR=t3@wE0i<@o#nXP++vWN-&_9?(rK(y zty4lP@k;ag*7HMGTCWW4Ge2wHA9^YDe#l7nuasEKB~~p@*F+c$X|ut=n?qW&#S`*x z_GAC;AwNhVPbi!$CiBUKMby@CO1hH5!s3!pvc#&9r|A@m+vQ0*4MuJoWev_ECw8W- zPN&Tpa$8ICBP4fqJf05626! z{(eerR16+%Tg!h6XgV|@B!&Tn{yx{%J|ULh0DzoMD8Q)%c&ZVx5-Q%kGV ze4VkI%msqqZLm8&rGgR9!!xOH{3PJ!ZH0qhT}q{< zm*S8LXvku)YbX)5Fgs-eGB{Mw0#JaSpGxi9QlJRbc#k9FJ!@{ zvk?33_oS<+S6DH(@22*-Qfyq@i}^iWkIpdLEso^OH#!%bbH+JWm0b4O6>oBZTGIP} z4!B*O*-fp-As{vL<8Es6Q5o07+l)l*bnH zxEat@FOalYr`2M$&+7+%n36mQrKD)NDb~B^;1_cUV_WF5*_^JB`Ba;p*UC1%0UH)-3_FKkM8jke?|+Hju@em`&0gl#Q@e;={r7hF&vZW#RP z)Y&@*&4)364md;ss>DCvhl1G`r0HU?AqivHu%LW-@%my`m8>bAQhc_1esQ1JmpZ@v zn)3VeA1OW*dpGd5csTZM!PhafDW)l&5SSP|KXqAPOX~W-F9MIIUJ+jnewsA+4`jbX z8e}3oL!adJdK2%{kYzZKR+Lil?o+!gBwcGyn(cOr+2=|H#QbDX87fGH1BIv}CJ2_8 zizz_;22_JbUvs5=r&k`Dl0Mn@`s4 zKI`F)nb#O?X3AJr^YTy5zNft5PC7x2ozIoep3zk``!vB@Kx|&W- zmsu>sQj@XUEY6%)G{(f3wz@~+dZR^9tvTnMH7dbk)JGqZ@ZWkdgJ*_yF0)quo6Nv{ z_h$xP)oaayE`+CK5XJ?Wlgy@EYZDqq;}#gf&5;&sEJ~+?S16&iS}imh+G|1qXs_FM z-+nx8sJ+B{ec~uxeE`iJ(^?zD49^W(%FCE|EVm@u;$SxYU~B){o5^M#59tMH?W_2V z`|ihQ{03U<(uFclW#H9jvR)ZzGanV=r=(|+nvhK z2P4bo@whE!v!5w|z9@);NhO)cU z6S{^uKk*KoAc`FY2T#z&nK@gkjV=@cDkiq=h$TBlCvRK(ueI?1MMo!3tN6M)mAtO9 z4F}m%mpcwo?5XP@SvJhF7Y_b@?awJke~E-mxR-hh@q_q|CYt>+q2nDlejfM;Uy*s^ z6n)@V!^6#J#(3AP>^^I`Dq=vTp`(eHyNBFb|Us|%K< zmzG{&xG1r<^y&6Ao#8imGPUKo@Ywe^R&gW*HrBjMv=E-aMg zw~v`drqlk!paQ-N^7|NdEgt267gDm9$m^*huAYU{s0{w= zlnrsm74JU&{r8W*d&O(lT=SdXTyxE9%q!HxaB|=5ajA3isIh(K z+!`;72AjcjimQXvc&dz&={?5po@k3o#j$WqYU)mGN^DEqlXxt4I6h}`;|*j5W~y}N7N3cuYQKI4qbe)b&Et!KfwTm-zh2!{@U zJ@_38044=^FIkG~$$lq=Hh7LIKlOeQ_y_Zi@*B^0f$v3)j^UKpqYGT-*{)QQiLd)4 z9@%+2V=tu>Up`BIk!)@i;DQ7yC@4a{e6yCSMur4cl6~exn4~AgN<*ZVe2F5^c*3zn z!WFYxi*lM!(2sF9+J+btnAc=ct-C~U`_*bfeBIRbs?Z3&|OZSoRib50%(TRQUWN=5l877#o5)q_1xTrkP+HtPMdy|ewf zpU%2!1)WflGq{GqaXXJMdHHT-60vM?<=(NWr z7w6_YDnANXJ(fV!Q|_7I*_YJjTjB%RzexOg_s`rw0Qu@0-8TmqjcMQqwbY6+T59o# zmTHL!E+&lpQHv?kNc`R|P(>3_lNWp4=>qIG^FrabBMyk%J~%pKQb*(wqPA#+sI(DD zV}bzmT4IXkRy6+G3S}Ev6G~R4ow}g!R0WZE2&2JawXh$HRH}GJ>4l#^{>MorQ)kr7 z&HMxF+wPtE$WJrx;v<>9v9jj%tIqmqw90LrJ?s32rFRnwsU_!~uuD)FFOrHM#gAGZ zv$CRA)QNi0AR0xJ$k(7+TxF@TE@xJlJ8hleC!x?IR*MwCvaiq%hAVGK8(72iVV9x2 z+}a!}b+dmbpLiit}#ANxM_GZWF2s^`(0=xzLa z?_0ia(YN?ppEio(zPLYDHOV{A`;dR1{~h!We#iF({@iDn@5g#dEY^EW*klTr7*oP( zGFhxXeIP3H!O)Bl6H3HFp=it(NS9MeD=DogDJie;rF9CLR+n;WwSx2M$l#L7Tv#67 z1RY+Vr!Y_EpUD|XCYgvQlX-Dp;Xrn)`BdVM}WW^9;*{wjq0KEDmJ5V8Dp{%9-^ z@cX^K7$)f{UazOBl40#J4^x4r=FdHR@Rb)ZJ z`&+DstcR^%TRE#x{i|+Q&Or&vsaxQWxj=-<|J&9=vMrfh>*7Ck6FrvZl3gY8(q}R~ zPzP`w-IFvpya{Ot@zl9pL`m0=^X|3(SFSGd7$D?2IwcDj`Qz>`JgirpR)o2GapqZ` za{0_}sGF;tgulfNxcaw5)h(MSyT!;H&UOo4#2h~97|V$UQ_MRg9-U9Hcm`ncX29ZR zrc}DwQjTkaRpIhxN#EjVDQv1bOTWm`R(MubyMB#hO<{Z0-G$dxJs29W3d7R4fkLgAiJ z5(`7_-uQAKyoub`%Cq*YPZ)my$FLeWm0gzn!n6}EU{wNyL>LLvj=C&E*d~y%B{ylF z0Eywuww#Jb)xKk+1A=NqkokE@C~4#+X8{DZyhc7`@i+2Dg98G+tueO*t2M_C>4GYY zybsQj+XqKucoO2zcf-Rp4bPK!*Vrmd@Q<=XT2E>Auaed1Y&4t9-bS;+P6FQEh@l?I z3RyuH+-xQR?=sos`XL4z;q#}-!at+a)sU-PUhCKT1KZTbWK@)%GNE$X3=8n!+wnG8 zzW$AbmuK$JJbXD#pYXeRTPfa>xiXNOVQ~k}!Q=6`a|lG|Uqoi!;NY-5_grS23>zIv zhQ*%^={uP=eDARC!=ibB^R0mMUa;d{(ImL98Wia0GeIWjeQxlvJKsugR3HPOcW z+T;Y&Bwnpa3!*pZiF)!q>98q3souyx6L~q!P826c&l4{fw-nth-Y@PI)hV^tAtVv< zIT*Dfi9v%Z5{+y)&sZYHeTkSO9#aELE-oo?s2PVk95v|!`n0}L-=bfn@7AmI1Nc%Y zQ4mFD-prWH*O{L;ziB>VK5kZ;-DUASKc)_)<3N)_>6DFQeRp-a4S3&@-I5A z>rUMc(Y#UeM^YA>R%eYRqxrUi0vy$b3vkMkC_oWitU%t4OqUVLtA^;}thVSF9ZThq zr`K|LV-1#6=CmzyPVXX$7oxfr!{7Dd{g+-a->h+ zySHY3H(O?NdoQ?SQTonvW^Y|WJ-QWP@2bj{%T7AZSXC{Z-vFJk8ODA-jC~MwNmXW@ zvD{+u>4F}AIShb@)hPeSFg}gV?JT!ieQqYE^SBwEnykNYhY|K$Ekfx2jiZihAe+o` z)0(t@_+viJ+7?J_ZQgd)vNLtQsk4i3!%oeOx-P2?W_m87@)-0?(u zx8q=%lbwP(Lo(+Jbct zNS+BG zZi8lj5hF3zF>DL-Cc~JRMNButGS4sv(0DWjzlZ4h`L?yxpargwBqKF^tiiF|q8HhN zpB`gu{oCt1|NZk1erWWm90WPt@G2@48&1Lfhc&kHPw?ji6ul2}TST>^7+JSjkJ@cmc}i^O^i)Eaoc+4B#%Q&duPME*y!u%TX-qM@UB)SSSY8_SfF5m+>~SF{HsW zG&?kJYK~}*YZRLDSga5g@C8i40B(>R(P#u{Su@j`wtQ_lZecA##gy(LiYDI1+@v8- zcJ{+1OOC;umOV&$AK`*+@6-(gqq$=;Ido$6|Hyj;PNGPxW)hq;{g zbNi@eioI0feE=7@ZD(zVf<<)5;GCwPXy!7U6}F%14Auj0;u2suBHm6}z9>f)pO9depP?^e!%48KB6+lKp*a9XH1L8;+z}-*rycs9}EP-xG zG5JC%uQgYVV(;#Iz)Rdt!JY8F4I~f~BY8?CH~tLx?&8 zFxP!RWz*kD_(A3&!VTER>0;S2qABwy!d&o%%vCfF0|ub%VGePRBLfofNh$lP=|zFD zeCqhr^$-3ZmJ^N>g7Rg@ANW65-gCU~`ZND$i^|QrEp~^)#l39#r|DZOdza>B{e8^C ziib7#>3^gAjaq#vbG70c^=3UFv;8J!yFy*5tW=k3>hv{ysio9W<5K4{Nqw4+TB44$ ztCmqcV>-z1vh1?%viCaVXd@RyQZ6_@DnU*ol>jzC_Un zIknYf;E>80~=R_klTPZ*hY6X%ZG=MQ=y28nbR_f z+ng9XdmUP@nOHF2?uM8j$bPrm;%an2EE#Q{Mu*GcYjlx71LRA#punv!NURvhzBioK zSxEXMvK#`OC9A zzsIWkpZyCn@uAE|J8&yDViTUfBlGdYZ{UfUS3mqB^FE$Hv>!gVo*{`_C9evpTt(k<>nf9`Z4Io2>otrFNdT*?oGuBa~0FyeQTaV-VQwi(_xbxL7O! z5-30E3nh_`sy(i56*j2?s%{mlQY9Fb3N#=cgM$LOIoancb@_aP%NKMx?F`0#AR{mc zTwn$Hoi3Zx>2f;qV<90Hw8ivnOcx3U^?DtHfFA-|iWRwLxcXhkU7U*uES-~yrR|IC z2kopK(z~-x?A(tpMGof8-6=ZfNu7_Mwo%_>8!^%A#;)i6mk!9&mh7IkUiSyr>aNZC zA7v{zSdz1Maz4mYbD8_sXIh(_HiNnKg4Ofo~L|8ayB^x*~ab(U!7Xled>Wl=ndXeT8bvs^ZR4MjL07 zp13V(D~O>fMa^Z+6_cx{*0e>=j?Rs@70oK0Q#P-1LG{9#Wyxj5E6NsEK2daU*^bI* zi=HaoRn}j*ujZxXOGTj~eI-!TQ#INmb%cN?b``5j2|=_b^>KBgAW>bO*O*^ZP*XiQ zGAVgYWNZA=}5jmhf@Z!WsM;(qi%^6liGtN&T_ZP~XK|E%#=R94q; zWu--IDi#9s)i1A8^(_ZJ0K9zeF5Pm~0KQ7HvQcLhC^IF|gfqd0HOA#BG=ST7;bS6M zD(5OsN6E%(F;$e>ky4~Klt*JiEpU+`6PR)m{@cN$gyA=)iA3cGj~*31aX~~e!uXRH zMwm?7$V@0+EAOYA7_JjcjfKnnmd0cr1cij@RQMq%BSD4D!#9?Zpu!L3Dr`<8Bq6A< zISc?nkhB30!KV;@D{fP$=%R?ylHsKtbk&5sT2h`as*Ew^ge4HhU>(I2m~}m~D(26r z3)W2X>Mc&KeM(hD{!J5#&*-SPY0NI?fd{_^egQlq^X4CiIYw}{S#+i?POC}h5`_xA z*{pEcZ2T1oJmqu3LoyF#{+jtF^B}Wq3@2gG=FG+1BkX37&S`w+K9tOUyUSFaAe&38 zOw~`I2R%mU^Jjd|w!EPdq9< znqcM0bmQpBba^+N&i+bnBb>bPZ6?|HHgr@La>ol?wH9)$m9Ze`1-q!0QO1Ml#*iw4 z1&ttX)?$1|Z6_B?q#9+L<_I?~Z(G!jJlc9)*s`4K8^9-r?Q!;*B?4`{q=9Y zNzT>4{hXCKjG}0xG$pEzX%c?6)whbjjo%mH)B@J3eF0v!3D}EtiRJQ@N~1-#CEd`>yA5w*zlg8wzVidZSceiATc9dUxIRH6XZNjgwn`C zS!0gXAy_}OYG`<-8$k9;-u1-ff`xOcYh6BfxZY>hnw{=(D~l5|8as+zlVDX4Xt`QXgPcy*g^c~lSI+5wVq6}tpAGZ#oV2h0HrW2rLFGcz8tV>=L9 zj8|K_gWci9&A-OK=6`GXZSeK*>m|P``&F|EbaQlDi2Wf~H+64|vU{7ly+zsW&D7#r zQ!QVswboi{gSBF3*z|&5BVQkB<^Zx4@6}cDE6?HgS1V*S5ePbsux_M~8gfx!8?dr+B()ptz!N$a(#n zQ=He2<(${w?zV(U{4v=n5AlEP^3*k)=Khw#dGZKQ1pu?w;cNhGu{1O{0q4dfXs!gNjY-g42}~Q4pxFkT z8MqTE?RT53F?>npk%$%Ue?sOpmtha~ zEhE!KpOCu|{9uBy+aMu^FBERjQb3g zq1U9Sq@SVKthiKvN%0Q-F8!Y5uafU1wN5pdY_E_LdCCe=F)kipxTg>b6@uY5fF;RI z&ghBcg;2CD5%eKs5rx76mr|qFhR6mpZ3P(NqWcg%Jnm-6khV+q9{Zbi&MuU%--ln% z?E{)d9TIi?r&P78BkKYNkJ2?YWBu1{r#P~W`ALsA-;!*V|TU= zFTSD7*FNNiOk?VO7^E8}Uwev#Pd#?+hAT?#E}PnV>&i77@Kw}3VHlh=cC+xNRd+hn z4vWRfI=d!p8k%2jg}%KVbo6n|WU0+n;Hq#pMM}#{D=Q`i7c{L1b~SAXUevTj+R}8p zw7u!crUOl{m0C@xqI5#(yfV%dN>)rPYbu{x{6gcerRST}o{*=wGt^mpQ`wUR4_AC1 z`bWV(Dzqiz5Gp=(J%{WGHX<+fmWp0)K=cXn0vv}}a9sgYRDcT#t}8AmC@S{l7bDrj zLB;jhWy-qY1t9X;oEb54s8CUijU{tybV9+VGO2=V92Y7^7UU0xfJKH-usFntxG0t% zj^!s3LUAZ0268K#t14p+jg4wGACoj{G{9W2Cm3{TfbY$JS{yg77>$dS?8gtG5U_Hm zG^2QNaZfReilyQi#q5#dj76atd`b&W{!Rzd)L z43UXHB#j(nNlvjuI%NqWL2lX`j*Ii7+Ks&a(?l%Jy+o0WVW2{DjocuQ4G6Hm*!vG*L0k& zo$fr8PmV5UR4fMWQh>{(OKo00EP20ie-}|S3s+b-+t`375IHl#=0_GAmRVOvUU&YF z<%H*WM4ie9shp>^8P&F+C6ymHX%$=)6%<6G5nD7GiG(B3a75mD<8gbu?ugZ5vsx`0 zwK^iNfGLPZ!d?Z+b6YKDjl!soAd9AeL(u`2l`LkJWr13aRFP>O(efr ztA{lqa&dr0MzRRkIJCcX48Y0y6O<+UWpalblEId@QRIB3(cB@0D#g&+@c6J15zQTu z;ly(Z7MXRw7$tM*I=1%cx!@=Q0e8_Py?|WiGn8k&Izb^?3TJrN*^qi;5z|O71F{$cm z@eYwm^NoB9&ra4)jLg-w>E}is&^-`&M!8?F;lj>vOdk)&A{7y31**OdfoSkml&1-2 zDm9hhl8VBTl60Z3T&oVm`2s8UJ6#P37gXd2d^{WU)Ws^&v5Mv8<(xGbHL{)o+$o7R zo0Z9ravJ~g!omVS#>m|ekD1f~HKP{lujoEyOC4Q7$PXRD&`6oc(UGln)w%Jb$8?Qx z>gFg~PVg^`*>%^XMLd`kitGuL%6@?oaOGvc*c)+#9AU>8qk<4W(V+{O+@?BbS;c9q zOX)s5j_kuT5ATqr>IIixd~nefZ(Q4Q)nBfD?P`_7NGRFt#LBlW>^pE)1x6p8`O|r_ zM;+fprtR^r%x&crGp^fp)g4=~VoP^1s3!jB0)o>wr>o=Iw)1X(>sv7oSJG9!PO>Eu zF}Nz5&mz#p4WL=im(0%_-!%T7KVtkdf86*FUgfk7;74{BnXqX9yFmc#LZ<)Wf9c=` zutPHF%8b7=fkOO&fRwqzs({Nw*Pl2+jCzpz0LnWM#+(hp0-ELUjF}y+YMD_9!C4Ej zI`fGgFkV!$OJ-EoPAAvk>zSgE#c$VJiQStaXA*Jb*Pxovw^GeTs*5xiRsRORE-GsC z=N7F9uSj30`dPzOO%JPn(eUdAZ6uwL%F{JcTWn5^GE!7r9IkEx2B#sLxdx=-^78F; zH&;njaX4ID96=fz+-!>AB2E*oX7iQ9pouM95Rb>83-Cc|KYSn$>hN*c= zZ?@STf<3-aINjaIUUztOn#A}Bl|J85e$&|-Sp;J2sk zJ!Qx4L@ho^S2nP8MZ+kVd3tC$YUb@xD?$)zV&CVcAv3yM3LR9wrT?3r)oL^mWVRtt zo6TwhV}P{?YZsUaQ$^|pxJic}L3-vEq{Yu5GxI1?LyVb6B|X}gR$`@KK{GA{W)i$C ziG(zE$-B~yl9g^~*m*$q@l$n$ET|*^9jIYRj&r&O`~Jy3ht4d7SnbKbhXk|E!A#01 z^@3Tj;(pw31waoJ`&ss5_A>?fWju2B&lS>m%Y&icpueEM=s7p%vnLDYB|8{}&d9ku zMw@Gk>n41M`VQTV@q3bY7Cemajqha+YWM5+C*RQiCTZP>?+-G?wgTdI^M?Hc*+18rOY()CJ1cmV%F z;u8r6AFF3Sa@RLHS{#gH03VZdrD8q*QL0`el>B@@{h^c8Fz+RpPtW5_e>!yb<1uQ5 z78MkG0%kj>_FF^=d2Mb97phVcDpJHG95DMN?6E^opiWDun1g`W4MQueEcF{bWzR~W2;&zf}CwX>~H^z0* zigV_#%=vTMOsI>xa+EuAjBC=`dG3)NJC2;YYGHoOySLo>PEEq_^Ywi{zvsLS_c$Nh zy!o-mH*I>H*;@MG;+sGC;O0dSmX%kZwPeeoLtB>2sQGi(_B&TCx$(w~s{6kC)~tKz zA@Zv?z|KGCz5{yOxyz#VkV!q0S&7sdkK~zQnc?9yrv1#rNRPKm8fw%&r%^K`M*%q& zYNbA>&i$I1%Hpx@XWj>lU-2|jXw-Uvu^nJ80qSuwzm>FTh1rZ(ki$)$WqObo{T%;R z9#l|P=|NiZYWBus$Nrbitz?M<9i`J&Gbx6TN@cD)WhK*JY#NiGRSVpA-!B~Alw35I zXx;|31~euM$TT-lNEj92MWH6eC&>Je`Ld2r7&w)=;IApB zQf#(gA@Ea+!^R5oM0>?l0@kLtOURjXLlA<9T;mN)q~qr?lZRe>%S!qbvH;(A*aips zmUviio7BSEzgz5~0Zv{A^kzYz=GXB<65Tf&31^e(Xm^%0y?v(?p8n@k_gKBv1~Z7(Ae-u&X!Jv(zN>;fW)op*DY3i`gsCiV1zL4L@e z>w5ygh~3@Q0$bAF(p9yUmF=p2%0|onzRB*zlc(rHhW_e?^{(S|XcBZznXRx>aF`Hl#KaKuwUS#Noe5E9sjNnZ zR~aR8en6{WjUBgFlDy1HM<=vw0p+G?yK?wi^sl5Ff70k7Jc@$!Q1Gz!3-hB{&gqSK zLc*c#Drwht|9$bwtDP9H$$6B!E+;4q%YGqM?}mHBaq?6qMQZe(x#g}tRH8l9J=H7w zBGqf1@#JZ#ZY$&TWWwtbsgtNwaZdgutDa%Ipr7Z) z{!iFzl5a#)py5S4Ms2BL{g7_lBNOaAp=rPFa!Uzm&^jtzugus|tK!^SdfDi9L3DxnkhHM`iH{ z2s|`y-hF~VJX1G8*~FBxZf_FV?}gq#8T~~_+@=;)Z5jiYO+HeaQcKYJsL!Kw-g<}B zbJh#elOkFN6}~6~I=;W+Hlhb{z)ZIA{*BNf>oOChN3AIvL3Vva&lW;hBOB5v6yz7O zw^_zfx*)&c?AH+jb_k7^Rp#tTndkXH+1SIHddRWDczA|L2A@ta_9wR6N`H}h80aeq zY1rVPP_)72yBy_L6n=hoSp=8kd+j_ml~45ZgTsw~E@AmzCbR2-6+0#Xxm88gTE%f% zn}~6+@PIVaf-{_L6s0q2(=YylC6yM${s}B`FJ!;D%yZ-RyLJs~O0^He-DWXHL4^Ug z-VDte4Y2*P6`ofX=qLI66>-U2?Y3#{at@hh(9+>|w`jihf!KZW8VHT{ z$>K^r-#DN}cgA??&4{xBWPQ)^0Nx`^b1ZQqc0+z+bhBs{>M7zz7UpT!M9_s=;<>r; z#kDC-Nu+Nrn(?UGBkU}RV&+CCuOmYSWWCi>=0 z;99LHS(r$oiNz@5*Jm)f!YO}g@=U2+WJ?V3C>f<#UXEJYT53m~9kuP4Di%Gfz#llU z=uGNhg2tM*yZt{Xwg$ir(1Z{vv8WgkkM>FmuI8OHaQgEWz+X4x2ehhP7jNjD%1(f^ zJuc^=@eFx!;IGBSp1EWrmf=vNa9nZ%Vof5k`@j(U={~e*R)FsG4e47g=~7plz=6YG2R;Ng`w1c^L;%zi&c?JHjK+;O8hgI9r#s&Eqyb1 z0x7DdSGJZv3p3tcpYib>Gw%2bb(4yvFZ{>amTJ#U}uHL#;eofnYZ#K3Q8mo45XWb_Yh;JySX4jw*f^91n zOxbK}C46ZVwSp=)3UEA^$ny#`h4_Z>WjbmHgmtpHjfGx5fB!kPiU$wV?;e}(&S96k zm;G|75O$bRI#)=pB}QE^Yv(t$FH9{u2g63-VxkJ)f>Axcrkxd)zUPF3E>490D1DKtnVofpq+iax=)LpAfUkm3uC(gU zv@2fA69x$Nu*(*!r{vCrCwMushxA%UxvWhtCNyCpv)*kdZq6v&4Jn@IHLKf1YP|4@LS?=Hk%Y&qcNm~PK&_B2RAo7ABp&?0KZ~SV%#v6O{wptY4Xk0~ zlq-9H$A~rTPuB&>bxKeVtC<~fB|fXzvWDxLnZTv=!wFp ze0TvTGS)W^u2fJ_{c@fqEnpD!BfrCu`Qf_Zh9I$Vg0|}q!Y$+9u_`DYRBGbQGMWZX zF9|wayLrJ=>kY`^jY*hw?vIudlqC6Fq&^k`jsJGWpdhonPykaE?9X*wN14ji*OeE& z7qmL|dd;(57fNPQ)esQNxyl;BpzQSKJsGfC8~sYqUi21in_OMjy{s22Iy^jbMl`Y)tH!D(4U*P&pCn3gNpxJXd9;~ii#PLan-u2nRa;D+ZxSCbQ z1LN*TNcK6~MdkcF0WmlhS#rU?Z^G3+o}_+JW3>g27mGYF=$U_Hs4IEH*Z9mg1rPKq z4e!%nSIMqv2i}}J0i%dY{e5o4pm@3m(`cv7#EK_!AF(5zTo&;&3i2~K3B(cIzZ^zT z-MB$yq7XDfFwHq*@)@{jD#+w;0oj7xakj|6r{oYmI0rc?%K{dL8_)~y_8@*LcI+is z3Q28U7{&9nK#HDBvMS^%`vmw=Dg~I*C+E*f6YrP+WTRtWE{}VgZO{3M2=i`?phXWL zu9YxH$TvCPYKcamST(F*T5-8x#8Q;FOIzl4UA#sHGDT$2Zdb@#6}Yw%)l_(KDug4- zU0&Qb*ittFq zMTkG76^##IFH$D%JvDBD$>)hVE_Rd3tqV4DKx09g7V4I-y(i8wCD?%P8#EztGrev> zsQuHUnHvyAeB?X#OL#!KqzM5?=8LM!lRG)3WFyuU#?u+T(XV|A@eJkv(>{#mop2Kh zRN5X! zCG*_=zB+LM73M=lxbtjbI^ zo~DcFXPlM%PnT?jm24y*TLP{?XxFBD`0LY{Se`hl;Gvlpe1bX`N+Qnwnc~Q$q+l%< zZj^mP6uwIdzLX3)cL0j|7ZyKC*zzgJ$3mhCNwy*eSnJqieEMqO>lh}VW_WA!qH_%i zlZNWPVX#xo&IOUpSHwGxa7SRm`%j|CxCzDBVEh91&$Bq#cm&R@u)tQQ8i-?LLZ=!G zeB|T4$4Bl^6YOS8^cNNH%eGSrQCgA z3;2U?)u)vR2n<6%G{>>jXI{xd)i}EXdaV!^RWtzuL;OszeR#`72ZneQI^HN@Y;~@O zpT*U3KD>b;H{O5Fy=Mg*MV>7XA(@zNCjV0e?0^1k{?jcv_Oq^=y#HBTO#Y0ZbwCYm z+-rlluHVCr^H7p75*o(M#Nt-wl{Ipb%~g_BSwg6h_}#TGIohDOB!8+UY6LqjlH3d7 z>joJr)DXGGP0HlV5ekX*&KFA2@VbjNQG5xGfQvSQX4zG0`QlvjTICL3k$fYPSM$m= zIAzm-eh0Rw8ND*C{6pfFRkuE{56y$%MACZeWHb>&iKAcvT(;M32vk7z>7W&KhzL%g zaI^a1UqL6d#*WTw#IsjP$~=qcnLwVjsS#PWT@BYsS?H4Vo z)p5vuRj@#Z15@}4hKTay8fhue#j|g9!eEEYGM~~gn$M9j9r1v}BT|VrG z=Kn+|8Az@+1p19*l%#LXv1_+x9>_sy4h&=Wfe7AUf|`gM)@7VKM4uH0I&(?TEh_=G zG*l~e3(T;eE|<9XQ`K4sz7#W8M9+TL(BzUF$3s}6V{Tlz9{0Iky+i4T4nzV zEEr$s6C}2>Cp~XVj3qcu1u9z62{sa3ROY%OZCjGRX`-&#JplhpHQ?khTiIE z0N|DVxaQKrYEVZSX0@fuaG6GoF45d7ayPYjU&lWDHyZrsgEPwEUtw z9zX|W7pPmu;q@iNeC8ECmIF~dNG2h`TP~o>lGB}=os6jiZr)w_>_Q-F7j=KsCpX-w zG>b^cp6Gz^gXAtcQr|rKu&P*-^DBVRvKpl$!RN{|-(%S8xgt^ua2|JM#l^(Z6r$*j z+y4`V9+}@_13_D6kkt&0$ZovDmTFBTCh0WH4au>TeC_=z3x8*xUF0N(b1Kz(ng3T* zCZ|8$z9J!AXLWx5Bj9a5^QFpegiX?FIkx;Je4~kR&>o(IIhT{F$xGVKbYaixJs17g zY&F5E3i~D>F@wCn^C7FbP|VDR*Y|3@mW_+T#=EzpHblA)O~B~GOd59UV~w�(4Wjn#x@ z?Phb8X*c`}ck#);vD6cPc+T=CSDjv*>u)6EshM4rd}*;6%4D-%InzBscf4=C2NIXB zE@m>gv3w+xeyKXuoL*jCtgSLhU22ikU8$|A&iuQ6x_B$5))Cr=+KcO6t+}{S@3f1* zPPnqjgjwu%d*|v}LAW`++G30>xUs?TC!*je%r#Rn_uGq?=L5gvb+hH?`uaD*oTXM2 z$K}?0Wb40Fg#AUpQ~uS(ADD|`3RPqZ!>B*%`CIvu#4(e8P5Bm(=PiA;0_m=7P%(o9 zN%fazmz!J)nx7a zgQlfgg0Q@56PLcW#fQ5JP(G8^B6#NfVmijlN^&eYCXWjsJKpPsKdDL=Kv-Ylef&I6 zZ2fGJjZ<}YdRi)7%ww~p*v!A^8YZMn1MAOY=0r?H+C>~W-cEjEeHvY&qVQb$M**6}%86uBm{#~itvi#YJ~FEI1e zu)B@>*XW{aHHjrIlDNUcFBEN4$lpYscKBQ8trJwIFF95g)cDUzq*x$gE3B;v2Mr{V zHR;&9_WP>4bD7qh<&F+w-~4Q5tiy`qlcV!OQiU#vPC6;d0M2t`dxgQ7iT!}gV&4|H zj^?6j=W&E*o0mFZCKL;p!h(`t?3d+Ji_PAbXL(yyjl+`(VSEltz99P!b}OZjg4A_U zexcK37uWkLN>E_y-i7{pr&7p$pW>YjB{9zoz{KF|i^=tMT#U6ry^?NQXMS>L*vwEX zR||_~vNaFWT3emB?;?B7uN!^=Zbc&Aw;O3byy0m1Ezj#8aZY`kaN|L^w~Z|4g!m@c zM&~W+EFaremI>jcnh2juDh3rLLLP4CV#J!2#VZ)A=xOO0=w~brWJAaD zc6RF6;TU8X`bU0UpIlo7-%FKT7+_FH7dnsUyTFAh^Kh~K`2GzkBx(uF*c|&uwunkD zmy10PcJJS>xfk&FC~h<_jNWKfzGr^-wI95*@4K=uF&wr_C2uht=OPmg|Izf{D<5hT z4Q%R@aBK3TA5$GM@}5N|kwTQGDI&V&xc!$(mJ^fM&MGa^x3>;|V zER2$~y!V`gTug8v4vGLokgfe5HBtiWm2CcY!R_S_>kW3-cPFT8-z(vC518;9T1Mr2 zAuo<27ESR@^R?2T1gqYt*ZJ|=XMNAiJ)cn*$y=xm8#?#G+36}zw;g%M=STHu|6+}f z`#g-S9KrL=gaIHWG$PQx&3UmPDc%>Wq$0ojJ~!jMjhtqRvc_G!{31ks9lo*^Q~Qo9 z1kY+6+P9$zQ*&@S*roa< zI?k*fc~PYwI%IM?$7iMl3MMqukFrDVVuIKsT4Y(h@&(7ku_UNh__5Silx{m!SAls& zB;Qzqyph^t_F}@hBg46)!sWm4t!}M=FB+xQ!}ZGjb1w_2*aS%4)3&RB+6K{3mH>hT zFpRa}-wC#>18oBY?iBx!i2tn2-_(RHJlT?SoziesbS6ep_~)lw>x%ZvhL1i-Cde8_ zH)2~^REW-l8T$Nt523OR_%BgCu>&j9V9l|9a5IK$z?u1k;O<6f^TB(tuZL2_Qblvo=R@f?F6DLj$2Wex0`^ME$+2#;cT8i=90nOYztP9!UiF~`t1qHwp z|7+Pio5E0w{Lc_-Uba}k{T&}igS*gPp|=%6MDWBGQ1ji1Uu}cCthYe~k8K3+e|jyJ zvSdkQ*&{~UMtKtL8M=yJhp&V8%OGV1UYX2rWVH7jzyJ*?^B2@v+f(AE-5&+pyPUdG zfS+OV!yQu&F1g!@p<%1I1a|rsDP2>nzPUy_;sCGQ&$*zu4Cy}7gs{{2uoh{6#EfDV z=WlA{p9Q(Ykvdm`v9XiW4n)W@7I=BNsFSNe(KPcRY8oy>!DtY?KE@*@Z}+xdpiD5N z)ek@VVw@o*rePm?MJ!$PZ^L|Se7~Tw_j6!4VMW4JdFcvOtIKdoDb*Uw6xDJ@RQlD7 z)I@mMYhj%1)Gfv|}8UrboT^c3+S^%BzaD$dB6>6-Ms_LXeja7~=#wSiM~aic zX)xvh2ni;9WNw!I&SbO#Psa?Jss4*bSmG#@RH72Gn3y(o3BQvz{zsP*^>Z*#^3H6q znRID~^A#0DAtG2;{}cpIybT}GhsnRCz1DTto>=uhF7wrq^pnsWr(-NasMBtG{2imo z9ck;Kmw5Ljy>GWaQ-zuL&F=$x@Ba7YW9Cs1eElJ)UGxO6%la25P`ugK=k!AQ{3#~J zb6pKky_&lD4w=}PFmgTIMlJLX*+fdyL=)#OB)@UT>$ym2iQ$Xe?rQ~ zb+nzTCl&eK)(`l@Cy3HQ?GhdS(b2)TwfVZay1MEv58~^fEc)<%S?H5!DhM23m-6$j zmsZ%<0c&R49@p7!O}n=E)6~?cupvI%MiRI3{Eb!liM4^mYM{e}yZtWfIp(5Q>>7Ih zu4>RlhX{SPIYk_Wmgs>rGSJD~CtLikUd zxVXKC4ih^Y851`T9~n0XHyIl%JL~^Or{v{eM_~EqQb0QwGA0gIRsH(6RlJ5EgMZOUJ9uN6}t6nh*$kA2&48_%e^^>?lutk4`FA#LK* zM}70utgPF4xjy4ohl-qvaE*5LxVQ|9$^4>!R*~e|B zFkL4tlYvqi-!rq^_k?chN)zd{@>AafJdxh8PmtfhC0>(2jQvB;t3uPCDJ{xV(NHVLF}wYNHz#Re`Uk5v16i?Ur3{)}%pBvM_WgVXj#Znrv- zuD3d`IMIF)e*Di&UOPt*tZjRX1kX&*$45<%=R|LL*3+alxP$Kz3&jtx-tM>InSMjZ z5&bk#9EX!iaXb%}+R^YrDYrh|!&7%xpk*!;3d!S5`&{iW^8Zsx@v#5rL-!x4!=mD9 z?D9_@sXDm=|D{#j$Qk%ga{R9u^gm)fd3gUD*Yh8?=l|n@@c$V1@7MePI$rNzh3P*9 zjg0MI6i*p}Mb%!-&hj5k_-y(|KK?I7`3HgfkGKd5vizH(eDZSt>GnUU%D?qsBje=d z;^Y3W8gt&^>7zRT#OpKBQ9o&VFcH5TpW0GSol3fMeG}&?}IO&{qlW~}Fdv9YS#XlFWNm-$}xN;X24(fm=zm*B8vFtC>*LNK+ z1&P1%ijTx-xK(Q7>bITKz?E*du$!w5CMEh;ZthF{cmd0uxIDRE_@mG?e~f-oFG^fk zPQDu$M@moTOI`q3zIdnq#M76bAA1qxVlQ?0Yas?`;Ey;%^UX`|G*NF->w6-cUKiBN z>+^!^`F#I@R%|Cm?yEu@KhG_R7`ews_?~ZPd2z>S-uKbA*o~jDp459rk5nT3Kj^=u z83CO_l?xXvT(!I)%{fzC7n*OapceZFF)DX)eT&KcU1_{}IXK8-z0T?H552_In;q$c|{7 z>FcLhchmxd?nQRyfnYqBuQ*?;ciEPumi`KdaBo*Wg3j1WK1fQ8e)DmvhvVd=Pc^Ac z*JkISVNCJ6AU{(-qi`E;EZ}LddeRZ6y>6Kr zP;p1JO<6wQ7Pn!w6SW4ARcpUU2DHzBIpEiqOH{PDiTRwZNFDTkLI*%-3X}XU(pAVi zDMjSD2J9R^%r~S@XlGyjR)?1+)>W^|)VyIyn%-!F|tlpZzWR z%mSs|7vD;%T#@EPET$V2laol}h*5mWXofR8w${7WFF3hD-lpzG966?b%SqOV6kZ!} zxD)#5_-g%XeN@SG)+h>x@VD<_;%noP_#Dz^8>Hk3)yhfJG~B>cP+~KtK?G@c+ZFmN z>;be}+!@5OlnKM2-Pb*5xhVtv-{EDsVG^vJB4Xat;2tliVAJ5XEIpQwoz-l(?CDFC zt3&!D`h6Fsq{f@EdS70KEu3zvO?aY&c<;xS(oMRE3B#Tvhr*35m~U^tG(hPgQ#{Ds z3YFXyA2Hp+-|ymk2)z?@V)~+t=tlNr}dj zWP?uMgP(EUQWvB^Nkqx?!QUo)CM_n;16g?9erM;kT@d@%vk zF)ChXS?9LgWEwN-NAIn3U+5FTwPJx7e3Cj`hP$`a3NBj0;L_bIa}4>m=?;&Z9{o*D5{J>YiB^EtIG22vFK$3P|5HGd)5-kmz0)5;eaGq`v?^ zSx0tJhOC^B>ad{hgfn{OQ>UQl#IAXB${Ewi6^h9pyn+)zs4KhG=elS9Q=6;53jTK@V4a494Jw{bcY$BPQZ(@R*s}W%0c-D3VajWA) zco~H_C9w311n(4227;Re-#W-`wf&nu*~192Q?M?D0NqQ+S;-mSGODQoPU)NqV!Rh#=qZR9;7pl38JGh;;bfwwKIB?UQN+2Fj#1dj+-# zgDs$rPLsPeKmm?V;J4=(TMiJ;;0IH*&sfL+)DHMb)Ep`BLGFAUms(?VtAHyMaNq`+ zk%_?NiWFIYuV$(t%r`)c0i1~wzHxxeVUO$!#Pjr0@R@Om2`W{RRLUZ&$g}TnSY0!r zF$L%8>`Rwh+{g9s`=~Y4DH1h72t`y{5(b z`!bV{e@USM&b5JqSY*LlW$ZLRU4CJ9c*U~H=LX()mi-yyEvVH{O^nA zSFLr-#C*LQ(o)}3&(k!q&ZjNN%R)7~#y1;bSBp#tjBFYcz3~c0g4FK>-s-Jhh~Kpe z;NNU&{xnv0UFA_V);qOt6lpe)PU>zOY{a~BJR_rPLyZ?0K4WvdBw`_Th54YgMr~pW zwZEMP6KQW?OgBEQYP0CfO#b0U#~Xm_&YZVzI|=dq0-u;}#=Ok^WQo$&vCtVtGkm_< zboJK8qRswPn*1myrwH3V3T578lZ~9yCGRy&5*`+Nkf4ArQnqd&n zqeaR-_DgHUaNPZsIj3Yq!qOV_?)p#Zh*#GDn73C1679ObsHDpB?e|U%z7l*`!P(SE z_IFJ^tQE%^lQzo}YVkhTeqDk?PfzFBU+~;ME(cYdwF`+PVj&6MQf}ek5b>dc4^ut- zDQ8GgFxwR;LIOj$<%^G18Cxx5`F21%9&kA?in|ydm8I+`z881#N*%U}qNC)~o9@#X zY~%U+e30LMNTofe;#}`J97x|AUl*|4n7-cOh~$8TTkVD~G(rbHkTT|V+gj^Lbw2uc z-L9TKHLC@eNw0U&&OYwh?IMqk?#lD&k%Ml*KC2~U+^7IAp>L)*;Ae6aF7;2URzEGY zcPy-gRhUG2QHiwRr{y=@QRuwQf-A3&zGRp|`M4$v)~mF+vG6!CYE|qj)k(p}5`|{ub>^H3*KcT4fU`X}19k|VEdw2*)w#b*ieQP^Z_In=b zk5oP%d8`ge^e?54rDgAuWDkC}XqSnPPTI+V&sq&LXNFx&7K6I;sb#-Tn%^Gjlw~m~ zw~*&FpkG=HyW$2ad%(lv8oIw3r4RV-R0lWiu^-;8xHz3i*Qz{wTgr z#q>|u>(0c_u6(79c^wXpNWNvTc<_QJOnjD65OcUysBlA{vh@c*y$5_xufnC9W$5HS zhP5&Tq?D)QL_Cv+Swx~fZKFEDDPJ>*hV`89^ZD7&ZY^mdZkIJcrX-Omkl5P?mUQ_W zr3fNl@pYqUi_2XSAOg+%RO@_SY(zx7y+`jCdTKlO{jzTCMY^@MPVcK$l91w1qKVs? ziv8~w)4^CYc1Q5wbVGtf7bICB8KyaVDiyPW1Y6FqA~+GAKB=%7R)uF+k|^_Q<>zrv zYsY1t6lymulVt~v6l%13T*?xie(Oc?m_HooX}NXRZWczp7y@BWr9;X?8J+ci&A&zh zovy!FaSn09!$|06FYp{C9}C>++#$Tp%3@aB{~km34xT_*b&jPMcRQgOKmqZj#FJ=7 zA?={vCsq3|k!%ZFN2?+`_f5Te7MF*mw;252G<_Flt>A{uL&IrBI%uq6g zl@XsP5sGA#*1_Z3;WIL2;tqGDgz%61_V|42=nTcRJ2Mk+Z0+_(V*6n~bjW@Pq(V#9 zr>(53u1zichZg(0c7SwYd=Z#wZA_ZUgr6p?-!iu4E9-M?@&k2y)CP`yvc|S+2rS!%Gjji|Sx`y$mjh?e9VcX7 z(d$9Mh17I&e<#QD|gSP*#uec5QFM>u+d+>~tu zl8XUO(~}{i*~T!~_EsAUT_(?R_v!bWyc@%ONiUu6&IgDR3lY({&UmvmPhao7qhp** zK}(xsZ?T>RS6K%XZ{gIy^tWGY;d?P9qzmt?r%J(2$o0z8Ra)g;y>n)2n(h3BvkbFN z!DGf^!Dz7M=ewet3LM!G+Dj?@3Z8}*t*R9#V0+C0?O{LjzpDN29=`xHM+N|4{YKoq#=paRu_>@l}wg}KVh^E{E!Ywve#E;mdjO}@nS zJS3^J&+Drw!p^7n=9dZSe*S%TAiEZ?$D~!^R=57%s=z3w7(s|7f7DwA?AuAAgJSXT zw#o%W)YewOorAyYKS{AJKDQEF%H0vB@kDH>bQ!p4KWMF$^n7nou4Ein_&solH5OeC z5u)DO5XOnoJxd0u{xmvu8?^ilUFu!bk6u_n*j#5?NWvh8?G?3afO4^p-&Hrc#*&FY zd41P`S~ii#(q86O`(?HHz2ldNsvo+!C!su)jg54!e6T4xu-kf`4?~t5^>-{;mYu`d z<3{gJfg}RCuSX7hA^NAEUW2#~0=Z{vlOwtGY^-p#Jz6&4+h!JELzot35c+&f6(V5t zTG6%;DmWx)B{jLC!q}ByoNShSjv9gK=U4JmCKWHMriH8uAaWyeV`MSTBEq|2ojR@D ze5aP4USSP9Uv0tpca#!PbF#!nL-JMU@!Xy#lQuY>+oG4 zy3d#Rs+};3xU7qV+e>a=qdn~l=&$grsz!C_ty!s2qqM-40D3;7`Q>KOH z81~#(RzG)= z3dj8eOw_~+<;o1RI>{sM=8e;6iywV%J#siQ56ZYkYnUZdHXvb-?nm|)q{0-_gk|bP ztl}Kiy2t~w1L|i8fpQPu`Ge&w*3GBU0WP7+rjAt?g_SK1s(>b_h`TF7mQG(k)X-ct!m=@a-{y?n(V+dYCZQ}r3eEtIy#tso*XifyMiTcVVLbz#Y-ZXCmLJ;Z zPx8gXS_VySzIr4+-2ei9`E*fb@bxuj(~? zXE@<~c7xjxgD8fgxT-~p*^rRMgjgAgbm1&E3M)R+)r$jIA$?Yj-}fVD2g4tFiNpa3 zIN}G!fh@EZ!=Mq+sQFE(rP{EMIaUu;^ssI)G;N49In8aTqH6rD1AO#0RJ~A3a9;++ z7n=f?Sz>T4Sks1Y1*t!*w`AZ`5QZCI-r+Bx$W!bdDV%dljdhGD>&{nj&#+$LAf<$$ zAZu$)M>vyG`)znJ6Cx&~{MzFu^z;3z6%?wsrCm~{rx`P#wR+2K`iC-iOmQkreX&DB zu^-16#s~=LSR&Q3*m~Evi9H^|ohXn@@L14y2r9p`Oqwa2vK8N%dV4UBS??}8W}$v? z->zOt<;L`yM^e0|1_a`Au9#{TDA~s2&V_NxEtAJnr15A_nUVP3?Q+HLKwupsbiU-X>@U>e4LsBRkTx zu*tjrh?dD5nrsb>K-W(~SAY`;7BPW-U^~7^)q!dgQLw*G4*+8Q3j!}q zfy*L`L~{&ffU|Zrvh6fb&$)T5RS)LKL+~B(W?J_7Gs*Ixy=D2w^_31^JH=k<;j!dd zB_t+C6$3piEo<*ju2fyu-FRw&~mecStN2yYPRf4Du5bS*m66EbTv+XO-DTh!brRPPP-+|<#8z2bQERZ> z-djK8Pg6fyKl?#3iTe8CVxjk)ubc~ZO;ZAnPB)b@H^=!g-nT5D{s3zzDtPI+V3?^{ z+3tF1YbPOt5b;8or@k+0J$Q+^AlC32(&Sp4l4oCRDS%+g`Uf$EHC`q;yv>+QGmb}6 z!=km?NPEVDToGS`7UF1C$NRD+!31&~+SUu?@UC`+YZyd&poI=pgw|!T-~ls-2~Hi7 zbUlUUBDtJ%oF@ehoN@Q#n1CFRmQ2G$LU3kn9n!OpGr;mxx@qgOX#KbSUIaO3T9_Xu zGVC?ePRPuyhInUysmlcg6f%fZbZm(*XvIRtJ8!GUAIGhMN!fGq*fx$Grv>?%yYoevn@M^9EFByQ61Z zA$wXjVaCtOYLdP%Dyq9vW}DT9y?B`<(QM&JSW~|i5-P>;EtgbJ)fDrEgO)LEN)3sT z*FjbK(@dDZB2gM-u=IZ!vf>5&tdc%dt-aE;!d!;N7+0%hEC?Hs81b`~bzJ&=YyLvE zkQP8cBfcURDrh)T&}^Mbpum#21oMhvXsuJrXDu(!9MbSeK^@-n|zNuEOPIHA}jI4qO_Jv1o%m2vzh%jKfvU z#gQ?peEII1#N(yWUL!**73ODp%eJD3=q}1B4}Y#)M|A?5ZiG2I%sp8yjW&{#Sl$Q{ zCC<&i%bgFX72TgDJ{g=~X%lnLrylHcrTW=>3S7qR_XW%x56zIb^qvjXp49PjB?mjh z)VgP*>Z#?Bn!dNh+~Cm82RG|0av+p17ORaFH&5~@>s{>H6;1ojjW=hqact`pRiR0e z3;%|54K*c~y7eyrs`6}WC4)BK&khP!3pS(8;)*rJ>hSDiUj6;I--B+p9o;DaI* z6U8V@y)X+hOu!w@g(vqnPCdJenA$buDxiy^75SyGoQLACk3EFcM>ej#Hb{irg7}7$fjx#@#g@1y;NedBR0A zLUHOpL^qibUS=|eX})J(?8N6`NGhGk!Z`FI>(duA2FU1MfjN1jjrp72Q+`h;K}VmA zxB6a$!12BJqM*M$dw4V-FD*!VItGLW%t$_+6(?2*CGxpFPtaTEx(mHI6fmAW{uF0qX-s;Snca4wB(99EoHfYT7=5yNCz-$bmHfnmMT3={ST>RGUE6#b zdGNJ)Ec|jDx|vKjX)g6Bp6Vj3RAIKn2udzHY}zAP!V!Qal@Ej@MerYGJ%e%op8HRWcM&ipFF zyN1k;VuVIkH;`1s7vx9OBh-r39hhn=vEu)Zm(kN(_I}x_jx&p;fs^3M=ln((G8XVEuZXZCYbH4mW=ee~ z9;tO9o>%cRs= zsTE<0Re@P8>|ytQLXuVt5d{(wvPl%hc^os{{P1PeeLur^uAqB*e02g_>Ztw5z{sx8 z&qXBm7(~es4>UrkxuDiq2tL6Glr8su7P~AS3_fJuWa?`ad3p72_GO#|&W?z3(*ZSn zEo}}Vtjap~3e9Pw>R&LBWn*R#7K)Zir(!*9TfDoz{S8Vu`IT0QC#Kuja0(+C-@F#uvyj@LaaMLw34I4ofAozvZldnf@i~@p$W?t&OTIlo@wVt*8*j zqYPg*>=4-|9O1pUn)aGbOUiUYF24!RfAo4DpH|}Pyan=hrSnVuxBM%*(!ot2d_QY}A zQ95;a$oO=_5@GTPks->g*kNJUJg$Z|7Eg=>tlG~%KqW?LeC{dV*0BGIMK#lb63Oa4 z5WjrR+69@>N!T066ZY%1iA49(Z;jqCu93cBrYFCwY4iqzyHXq&1OB8kM#g3AiIMx{ zq0yBn3k8HMWelrg7-^jbnL|H_i}w7GeAo%@Jru8+H%vBQurw2k+(h=>FYhP#AX{0N zo<4FP$Pg8x?i-K2i;x_%kD?juY{5EYzz-`PcRe0`+I7JFbo7Yp)u_s(B+SC;r`rmG zH`|{yb$U7rw|ce~?(pm=+~wI-IOG{B)D)?P%7@ZSnv_=SljWqe86qYDJVb3?D=Mc= zt9q8%H5Cssx*~(VMnCx()iV*DlS$M+!KzW7u`Thq&r{Ygp{EjbCV9pzr7(6-EGpvH zj2eMhu2fBvasj^#2OxUCkn4_xQ&>5}>D7e2vdtRh3`HuL3bm)Jy~y_ve{HP(g@P z5D2A0w1Uu3=wxUtBx`}CLSJYnEEmW-A@WWLY8pVXLU?tcikn8YFQL?~Y1lAr_q3sD z$ETf|CV!1aSo$!!G?tzwRHu{Egqj)2X?-*2KK1qKqF0bru!@sX(p>^vQ`ceJuPYcUaO0SLKXlFk^P&HAT3NH zO;uDe_8ED>N?xGdWrH`Yq@a})w308hk}s5WvlIkOpA%$wnux1(O;k}k3z|j-Xjg52 ze(S6z`U=@JY!;Fq>neKYTrNzXUjMw$?{fuHH0t*UW)}%*iM5EF zq_QX&jYcEYjb9s05r)BRiw8`O+R+taqA(bnDaOT(FOjF<2S=Y`FTfV0MiGI-NOlb2 z=MeKP^bAsBvJFl$ zpSwXmyN~@kNZwxvK8;Urh7~M$8og7$MzeN0f`xHPd&jkv1!zWaCDBlXUfQCe1W3EOU^#1hIq>=qu7J2Ji}yyyQUZkJOyCxyzs#ql;ueWq*i*s0eQqD)f0w5u?=EbY4@DX@+UgGw6NE z^OErQ*V^!_vN%gE{2r$yR4qWdnQ3kZnh^=Y(8Nc9O>5U~sX zf*c|4@{1}`7oIo{lIbq5yUt<@nJiXk++~d#3cP+Lx{ASyxYwmM=tG;agYb8axG3>! zc7Y6YNd1~x)}=m7gc*-Iu5KXWW=W%|#l)CijFPpY#ltu7%lJ;di zMLBW5ERh@X;~eJ{{kT4mpBF6f%ku;9EQq9Mla%ABjB?y)QH--Xpn%k5W%*jW8VJ4D=qE0QA=2coL4)IzBV5@`NoEtJz;=dh!j0}-#w z`KjYdm(s2%_6C3m^?78F{Rv*+8+jJ-9MAK|d09V(kT$5R~(lzUQWQT-1A)>}SN_N1sPM_e0xsU1OND;zalFSdg{RJUk zDD2Mj;1WN0`JRXeC%wfPv>=Evjv2vFB2?^`6$Rsd=-?@sVTu;!)9IT61+nhVb} zm?LhU&}lUx?&v0{m;m$@BU`wI#ETKSDcmTwjR45xJ>pLqpwy2kwVi?vv5O%yV>Yof5KaMy1K3Ib#qqt7=^wrO`5U2+5EW z7L>JnZYQJh+2j>9HDi*d(o$0@$z(L?ba#Mx?yJ|+`)+)&x3mfdXb&#ky$hG_$#=ne z+IRJyo;`H_h54XE_ADx8o)OH3kS8=$%9_x`0ksVNnaTW0?3LJwvuYD_Y`r)w#|q;` z#U-iIQZ&ho37#PrxLj}=Qcf$IFzuc|`g$~B0e)Ue5pBj{MHkGx6mZv@N~0kY0%r(% zg6UvGa9MCya44uGNfMO>2bt>xAMo0!)*p2;T9qN!lN--v$8vG*bXfgyYF^z1JaZ7Q z7c_-}BUKm!FGdX9La2~w67(UyG*K#)HkP)O%1xz%*tQ=Bfgk@OWF;y%M=2Ta(ms%LOZz~wE$suzwzMxJ**4MrlL!|9so_L(H{rv; zj|oW<@i<6OJ1A-9f|6#Agacf*C{9*UJ)a8^YM}NNqOMtha!ANYK_ojv8=P3QloTx` zMN3K1Qc|>(6f1@DQqf381wrkJT;vTe@l=v=g-i;SQmBwZIU!LOvA!UsJ;ZYp&qcs_ ziKmiGJ7iL*ltP6R(i%*}285LM5YJ7#^BQcx;@jwzlw!tn^vXtyXs_WlmWb9Fl3fOn z5WbVq61qJzZ9=Kc-b>cJqrQDq<-zf#6|l7a=jiL5lqR8i~sY4#^T@UXOLHxSm<4Axw+_N|4WJ2y`Os3$oqih0pGoeA6pLk z4Nklv@->LGx#3rI`>@WM|Cd=+a}KZOtJzh6y~J3LcVZ! zVSnMNLOCiNDr5@D=lW)(h6-_EVWQKz%Q|FbcU$`*IBu1BtUIhv5EYA+2x3;Ecu|83 zN0U~eB4zD!q^wp;O_tQIOq(<9g?0|)3T7&u8`Tr%lQK=x!^#jL0Xhx|3sYo!Mo^nA z7PHyn_xVVwq9|c0DoXgg2_ZL_v{3H?M@m72ZGcs;ygv>07qIaF z*|tJr&_ts42v-uPoVvNY2cpdpYFa@pd$R(5zy_Zm`VArPHcnNgszPMeYyzo=N_}3k znj)$fva{`#zMonI>h-p?MSzq=fLJa;a!Ak+(rzj3meMXM?UK^@QaWEshop2!O6N)G zJSpv$K+_P?l=935OFEB?kDqux>PhdBQ1C&)1ZlWM^mG2o4kp zGzOAL5%GLP(N;t}H}PD=%N3I#vABqrD{Ah*%O_qQ@y=^I`G4@!)Dvk&VwB1_MLy^u zN_}ehI-d_dir8*G1r0%@^Te0vk|NUG-P4SkdwRONyYYmsWIR$PEgQw9q!O1*(wvp$ z;pa|G676?8rq}<4#45hP4OJxaaednNHeNjXIGs1>X2;09>mxa$BK(!nW8$2W7b0gY zwvPQ#wm|kV)2%aZ%(=n(2dMrvy%?Pv&wuLsIS3=E|yBKT#RN8`WgoLx3<&A9$UC73nRWt$}+S_g3vM zdD-w{$uYy5CGS;#k@H!#zM4QOfp?q)X7x0Cjx4*}&ijJN9OBvecrnDNSDCVdqeW82 zz#y&`_*9;kO~)Ey%VM3e9kHR9A|AuBpM@Af)2M0Du$n>44zMaj%4&>xv(%nIJix%K z!-Bn-;7W16&5_(yjEe<6nDi9Ki<#m<=6XR>Uo22){+dK7G5AQxn%-$I!9yKY4m+hRJcQYs5r`(Ktx)Lc8iq{mZ;`2F= zAHWGjHIG#wJOi`p9wzKXvRrQNt{U!+K*lf#)x#&!!_CBu8Dhq9bLEUJ4Pk5%V&;IG zX?}3Bq3?vHFWJH@WF+(7v&?EF#e@x}pp3aNo|faa$15u0fg-2gb?1^>mz0-Q=im0| zkJ>)_dBe7{{Cu*24CPv_Mpy5Jo!6&Ie0bVpU2DJbyTuPCc~iP>dwn|8#CZfEuOc+l zYH)g%>~6l}_x?a$y6*n^w9UBE8cMDz56!XYr`LaX-MyDyNl&3%50cG=AlcZ_b%J)1 z)Hzv_JimO2&XF(G#Nt-1lJrhaBS-iLJ%>T0#TJkeWt_v2QPz#`lBKEm_tna|Bj?e~ ziRO$Gga=;|fiHQ4{W_{(E<1pXKwQR|DFdHQv37STnM1rXAUSfF#Y;%93?kO(&!R(y z-$Z2>)s8D68mQZHZax!1#FV~5M(c^$u_f7kV{=k*HJYg*i(U8dckeL{Ok zE7xlEaYBOAvu(+EJci6zFgJoQx!c@tmYdC7fdCH*3Sdo8!9^oD<}!HnT!q^e^8mhk zJbjDXlFh`7i0Kea5o`f0tcbZ>Bzu7iyGZtrxC#r$RbV~ma;t2bir9h30gt;lsCY&H zbMD2+%ds>1(=b1O8B={L@}1aY`bW5D^n1B|k*8z7)IZO?5P3DGGe+DoZ=^6*5~+&K zj9eJIO@AYIb7Xr=cd>pccX4ECY^DARZe`?(*nRqYIE_tj=PZ$&SiOEWr--TMreX;Y z$yAl#W~MlPZVFn+$(*H;;<+hL1nXropP>|wmEzJ;8C}24P00jbDA^#xoUC7VSjN)j z9%o9*L>vB`q)ax==X288yfL-Ixy$*4^N{m3r`!orLP^|6^ZuB1Hfxsev{8AkoT`Fad0xw7pW6 zr<{_+!O+Bcj*<;d;}VaC9jN)S0k#+^(c7>r$t!CPQqeK#)$_MdZ1RXg}#(146V8)co@J=rKO5G^sGM(Hm?g{P?Cqu9> zJ;c36*iCZMiL>s=ywfMRs$qKS44D3j6Lh1ciW^RMi?%ky>EYpw+yT-hIl&72zK5s{ zzCuALF@1@%6RtjPZPPyG*w}L7FMs*a;-PeTfjP8w`O0~?O0v1pPu*gefAbzsc{Dyv z>8Os)m-ZZph2&ba=k_J}y-zFPf_{VLBDx=F1OLnB& zc8#_{ui~tCu9|+)f?(x0*Bv{;`Y=(w;`)ln-FL2C{I zyn##h8<;auy@<89ASrhDDym1Z^-QQ;yTAR;v^sU_AzFKcfaiWWUhJ zoq~ZIWR!x2H*Py+!lrG=M~Vo`%-BrwIld8L8M2^8)Q9?!9O~V@*S~d#J>npg;T&mp z+F>M{hn)@@CEMo1;?}vTVw*}_t3ha&R%+s$P2_P*w1RY5p@#?-$Og7xi%D4!;q2sK z4DpO0r9PE8YdA?QM-Oj4iTWTHQDHA8hL(I9j_-xz=AhY^H^~Ua@5k^v$t(r1gjxOj z1-4>93J0Ms-{@m+jy~q1S|4*=I$>Y<1%6TQsTApLq=q>p(q4tz05~vOO6&VEv!Zn(+cSc3^wwgUxz3p(t1oM3*6ytStW#MEl$FR5ve+zQfH@0qN8_8Df96I`h$lM8fvY$@h74@KyBT+j zr^ij=kpo6N8E%`z?Uf2M5%;O4(Wk%<*WY~MgP;82rw<;z;@3F$yVusns>1eIj$`?+ z=cN?mB5 zPM6j0B4-c;BbSrTabvgAZnG(coKRBv9J2sfkqSSI^eC6PMaV)IUY&I*b0GGq`sA852&Rk^WtqH4OZKO6-yRAp9YLaBE&oGTZhbKQu}$67@IzFA z`E&bVkaGnX;amZRH}?b#ZEhbx&+P-qxswj6Hqnh3jZ4V!Cv%7=p6?Ll8bIovj44U> zRIw)IO<}VUvvKT4%RSM*l;)l!B8H$5A+Z^$o->>^GRxwz3w(OTeI+jbkU zVN_3yqO@Hu%(=F@j=NZ(`+N3pg|=gipTQ5HLX^gDiQ1U%;pXlU$LVJKa5E4fwIUGZ z4%>v_0dj|UfZSFg9)FD}sR~%U#z3OP5BAzTM2-7~6jEb4LRC*e$oUKv`pFbVXe-j) z+(TbBP+7&3M)i7g+R#V5j6{P@ko|Y+Nt#^)A#148AZRD3RXjgMSBS7uL_I+qnyg(k zn|+hlf>@eGGK7aukA9~(nXZ2t$Zggqr)B`r{j@G!ni>tyvnPNIYX}+sQppS|%JW72 z;%EVWOB7vT?RRFmXm=eWv+dZgVcs|cQcMAg0_R*CAp%+`QWjYixh}FJ@^VDskKjj* zuSDNQZ{oMOH;u;}$DHqnKX+&@ajat27*{$r;48zMqT7sLbKDrYDSBV{?r4_WnB`E1 z<XLODcijshlR3_F6( zU_mIH;>sP{I89Z!B+7)zbu%a&DZtT4gb}iFM$8`9LOlhD9ujf{YWQi&0;C7=NI8BD zE;>sjbAk(j%aM$Dbp-jd6z7Utgx(B7)_Io1gx*9#0fE~w(Tqsc7-5(}X04#%j2392 zBT`U6lDyMlfs#hLnQ99AkSXdz268GK`XvtGATuIp)jqFR1MP4eF_P`>~3CdyOOcjnekj$`T-%3xE z)6M+@C@P-BiAr_=G@Kg}Lzp+BQ%>k`;yYpSov`#ySb8Tcy%V1DPWr?!QKCpH6bO(} zB1V4He)iMvm={x~06lZu{Jiv$OnRUe(ek_m`AQly&jHOO-S_>LeRO#)gA7-EIupWK+HmAto5+7G$ zC53I&q?OJn3hK#n&|(wCn~bHna+eibGsUcqh-9S|k<7H{L9cGQ_b#1f?*@6MT-tMY z7KC}PTzu?|EleS^l#p03nZ#^hCjwXwW zM&CL5i_uTR1;rDp;E6Ns7^we`vV*`0r(+Jo)0bvE)QG?+z@(;J&8^zj%(yasv5#*EYeLD;jzBSrv{J>oE@A) zVn|Nu^g%8)?$l3Q?tO?kHi?JXL<3ssitkB!9imsT;mkhx#71}w@`>Ftu~!Zgv7paD zug4%Z%nsn4PQk>5Cs{Ec%hbk&lLZAvCO1z8-05lNVshi{*x{!P*-89)k)2+6Vbma4 zXfkTSoSjsEo*-9P(8Gg74@8XY31R^UsuEX`fK&k|tg^E#lcsI`|a^#KxaoI5y)p&S{lWtv7SO%BONw7AK32@iZCXdR zEyp2q+Oi#CThJjhC4~G+kbl8T2t?E+UP5s45ChMEu-%a0?7UsDAGIrYpdE?Zif$|3 zg?1(GE4r`vC+H`M9~J$m_z*gjIBq{>=h};|D82~_+);FY@%M{OtAn3ib;WD zyA2B197Ty@8%_udQ@av4K@2QlA%JW}#VUi{1LI^@*^<_zGwEQ8;I&T#Esm=4^YX}{ zX*;otX-Uywp%^rZ!E7<8qQTx7Gm|6~%E~K}GP}jzX1~+E&wj#w+OD+N1NKGs8)?Q1 z_BZURfISHrqy$MS?J$<{LGj9%c;+~6Jf4m-acTv~_j!+c8RX^YwP-T0P>}Kt;tvEa zd7c|&9GpRzQJXXnyObRU>@ma*jKNXz^1~TzHqpeAlQERPSB;PzHHWDp-qW2SCuEm| zgOSgw(x*?KFjmBy={ek<2>fC3NG`cmCKBnECUYq&3E41@lNG2mlT$&#PA;;TowgI^ zsQ1>S69RaMcy=l=$A}>Vc!+pnRz5Li01pw*E(y`}kt<{8fQVOY*P6(6UM9!`kCwDA!`LRu-Nv)j_^cm?CS8eBT9&T=1GZt)pX$@) zDY^juq%NJVoBPoKn;7}dgpruMJPhn&=$en_e*g((+>V z&x40UU*IvLMU54VI;aRau$wUkt)XeSBvg-Qg)YV$vHmC?G9R@Jk)z=WMI}kj?{NP-hYLkRa z%I|sEclH97m@>kVZV+>(~hy3Iy}g{~W-#B74L^RJ`5ICr)ITqr@Vq z>bJW}yo$zFK~DX?e3lEN_<5&{mTalJtPKf+Y87&%WGG;?h0v zEtvdYhDjUbL@!ce&g|)A_R0Os>Tmq?N97gi`a|{UKu*K_t^4~LFC==9pOT&NN1wx2 zjo!>05_iW73witK(|F$KUR*03J};<{jnn`SZUfFS06e$^KPhmAfG5D5$H2u%H@~&a zxhALNtbr&M6_=Et3CXB_T%Y!DPvxBVfP#3XeJ~L4XI_f9>%CFCDG)6&`J*LiU?j^> zCz2Tu`R_7FWoNi35nv+3;C_&4dNL4&Sd{1txPYf5UeZ!>xI|VW2Rieem}5K!pey2L8;C0BoV!!&XGKbAIE<}8QYvl=2XIbZj>IY zBiaRW&5O7(NNGq?H~2*y@=M1J#4wwNd&Cn086`tNEE$kW2BeaKDJ3(K2l^LRz?{!i zU$WGH=ht*PubE`1|8u(4_vyazy9o^Vi_tgEnr=paL5%b}M&A;R^y|~aMjxXZSV8_w z_FGhimkO?>St|=)$$BOD%g|e@xAbpCju)zxdSx)Ac)d_&CPqb0e<)d+H9KpG@m|%V zs{L8}LrS&Iu6rB!7_mLl>Ls&3h#>LsdKF#T{UEcqH+lC@pbI#4<~oV-+Y6}8Gq%H= zh3)a;Bu28bWN9>ksKDtV>LzjuJT5qMlj(Rvd|A9Relk85SH$D~sA&*a@AKyw*qlMk zf<(ncVcBPL+gxs&-5w_xrQHn-lW9aPfO>=CCC$h=J)hj&tVIRX*GP9aYp{mg-CVu? z#wm9+by}kEWbRpPLpN`?=duLBU1#tnOURL#5~2+(iTaWxktB&^)Mwh|#Rjj(yUe@G zJLHvsj2rp(Tncp zjlk++m{2;o^C?Y@yiXI;?2NfJp`(PuzWK0xm(=gm{TFRR&c1i#2wOIooVC z+sraWVK5LZ<`MxmW={lgz9tmFetjsw78y$dn2VdE0hFgL2;g9rFM#w0dNWj{Dk4p? zL|f23e$Lq|-32Ax-#yML%#sDB-)r<{VlPl;020XYWU95*?+lgP**WqsVH3lD*wR?K zvUBuPy!_gdy!&^K{_Tj!6pq|Jb5n8U1Iy>nEOCcZRHOKh>VoRhi_W|o%uP7EAuyp@6Z2MShh`mqxy-wCn6{0C)A(G zKU1rk@|(hIBkUZNUa&fo`lyi_obV7wwh+=6%xUmrgWu!d;b;B+xND9@9-pJkaz4Xc zj%-+uLd@lRS!W4P(nd%}A+2>3-*Esh6WKVqkC)t~J}li=NS1%y!xi+fE*&2_Jjqy5 zj>lw`Y1Bb^$Zh5$C^y>~!G)@51SJ%q2=~Egs@0Q*^USH*+c&qb+`M^Z`)2!X zt5)51sUG&HMv@?x4(Em>n-lzJY%0V3d`1wXJfEV`XfMd`tF~72VY!V312y zd~-axiCiW(w_K?gv{^~L-U2vhDDo6BMMaT0<#G>5421uXbrV#%&z_S+`Gd^m1JkD2 zhz$lgyRYW5lPsG~CdnK3qcY&P3^v;&?OhB2@Ywu8=kk&?H=% zbb?hhw4hh`)4a0jjC6IFGIQot>_hgpOP6l`<5PdVa^|$K%M;D3TC;a|^ZeM_lCrA8 zmCF3aq757FoNv=-JHu6%-Td-%@6BRkcfd4_8L3EpCmzd2xCpZ&I=q;b;|> zMduaNEW6mD)K*1nYnC0mx8YWTYwH2mfF@B8=hAD@glTh9TllU1t$}NTvY@uW9|_C} z+#GmO^MY16SF;#(p!UG>;12XzP-*gUelEZTkNS@Kj|PqgRmApx@APz%o&hjargnJ` zXC2LA`!raA*+G0_FJ@Uhh(F$^@CT4>kkRhr=D6kbc+ZSENg7_bkcI<`QtClw-hPbc zC{?NZE8RktLLw{n zO&n4#&D`s$z>nac2EFmJGfSE9hRPe(t*;B&^hJSib?0xc{n0>dhuZZ(n}c!YgslZhnj zkPnhRpUI4kz;e{4oTNJG1j)c;R1smG9LVhFj&r9tB{_@vyt%J9WiY4BTg^Mnhs-C< zV`gRCjLpyUlqKkkQ^YJVIc^qMx7Sbh8Hrh7#5oM(2ac?j$&J(}jM7yPrpfDK2%MRv za5MRr8xf4B9LH1;Gd+orB~6f@S&0kcqFG*jI$@hLdg;$cFT217i#(i7BynH_%nOnf zR>pH@jO=LifOUSHXDOuvsYFjIPu@s(TT=ta}5!qk9j1sQVQCQ>QfrQB+r8 zyBIa;&}5li0XbMZL~O>@L^t*Xk@8HP}41 zxQ!jNVcT?|w^6~W@?jn-XIhv(W+$_o>1PfzN`@R5w>z>e)3qK=Li2>?kmfbbNsU6| zuuC_A17)4AqL;vooFG1>tE%(~ki_1Tc3JIW*AgR19X+l+Zks^1&KaT^g{%(-vW%J2 zPtC!MB_HceVXtVvmNLq-@ptgA#BI{cnC{V=NoTT+NA9I-rUyXmxq}W#D}D*Y{|iPJ z$^I(caq+Z}I_5ZS9dVFbF8-eQds3-2DO}7Wjsv!rZEqL68T)I&d$IS5RGi7<;!Sav z%v3;bx=0cc)ktr;$RRgfSPE3*CM#C<{F86yAKd;sxtfP5x!4l%<|y5t@*YL8*P`KY zn_8teeZsjto;;51mFiF@x&6B8Wzc^%k-j0YZkOqV@XR2O6IG))P4Y^^5Mi z;BO@VZ?nE}`Tb4wb_?dvndO=5EpTID;rbh29mK9g(QK0WSMhp_h4VwSJ_6A>mSln0bt`e78kt*s@gQyX*c&_?NE&DiuXEY_ajgzB6 z%0~SqhGnr9{VGFeOu>;OjQd@|q@DxM6j#SJacx`|&*F`|i8u2WKAX2T>lbm0W6kj^ z^eefQu`A+(`UCOj@hh%ZJ->**Q2l%Sw(A|wyVd`UdH6g&pD*CUe1wn26Y-*Wal9m+ zjHh&oyhMJYAQ4VP647)bU6d|Pm!y;FlrF7KYtpbRPG=>I2~)zHuq3h*)&_Nhra{}F zYseDvgnXeu2n&%JQLCxI5GY(?xTNrw!g~veE#aAcg)&YSKR9*%9AbJYf} z+M{=S^sabe47=ijLBwW?2|@XU{oiC}HevdA)+rl1_SprHBi(05jcW4S)cU{b**YnF-l6oqo;FtyD zSiG9!$oc@XIg;&j#VsyZ+!Kq%~unHvy?KLatzm2kc0VqKq4 zKM0ieCvm+sd^$25aXLmun(e2Z!`z5uFGUW55sBussPG^YZDgAs9k<}8^ z;igwEOBKdQ6;_Dosqq-`^gga44vr)Ln2D>15kp39lA@iQcP5n*Gn;>PK&=EzW{EcG zf1cUOq|5fD4N_(pUtC?Cu7AHSU0L}*M=L7R_3zcEr&r-cqt8@LOV>YJpSEPh4m};q zvZU*usZUo|GFObQz!h}&Gg85fjxv=I@&AY5UHAs3B1%OzU#`*q=RezxxxQE>B;g^# z__eVimPIF#7I^{L53yPf(BKV)Lw683V_`;z(TvD{s|s4A1p~Qvo?URk1q+B=c@gWE0|6gWv6szEvEG5Fnv*25a+9&`cBl2-g0 zlrKiZ_;*Z)tXN*JNGYeo@6)Oc>R)K~Y3sD#)-BH}%vx$#Vz?3T_Z8zWO`n@*S^k>+ zy#0hT;k+x?=90OdbU)=;;TiQ_>~{vfAKVpsI{$YC*GIk^JskU4;nDcL2}k08itZ}D zy<~cFRjQ(Fec4^~%PXJ-Ed6c<7Ef_>2|~&p=9!SkK;AOhRR~GXN2$*uan=}>;tW!e zrdwf=fTdB^X4Oh@2FbD-r8tXwT!%^w>!o-W zyVUTY6xX9d)0fZ^EF-k1YEXVSQyV7_;S6qliV z?*`yc4bMISH9Qxe*dm6Wp_ISUP;?N$Sn#Qvs-{Ol)aT$vGM7UN` z-q$I`N%?DO+(FBKUy76RPtkY|!2#c&q&VP!ZZsvuT{{CTjd8QXIxt-$Uau!U_CONpV>wuA==v9*6e(e@^4{yZj$X zagzT9iR)gRj>$vrY8s!1I?;M? zy;3P2&4g4B#EIVu`De~RW2WOzsh=nCeI@-af>AS|B~j^nCy&WyskR9Hnw2{`)OJSYA z$2)1SbWwkzPkLz0ofJnl(6;x|wutqKz27O}X{NoswC`3!YNBV_pwtR@YaKjWPwU9w zX(9bu0=W{nN*wAVzlD?fNNjB({nm9*f|OqewZ_5kg)*(QR+8F)mPr_#!H)IxI}69( z`F|GHW{N3d+4f0L=A#CnkvhP&I>6Q%8gGDH9{kzhUr6(7A!i}@1efLj@2iF1c{Fzs z%0e3I2-UBl*xh^1OpwVJ`9c@vTwM}9ZyDzs{~7g8jNTfF7jL9kl%b(5v{y4Q$ro>$ z#7j2PcSStPv`56Nb+o+L5<=}A6ers0EYVK$R?%A8$1#K8aFYaWH9X%$%XLCKGF&Qy z+h5%?8U8KuqjpLIYowO-P;6X7^H)#~wT0q)4;{tx^;f6#U7eI;Ow_Vjs`GsC+9ZBU z@NhY$DY4bdrIA@L)qK7Y%A;R3wWmbBb1qh#3wg~rXWc|;ZzHAG4ym^{(As*x5_Tcw zAM0uP4YZ~$=Zu9oPE%(Rk<+x&7Io2{T|>FyivMWLc?lQSQ?9TcQWJ2=Y}iJ7TI3fZ zKk1nyH$=xv_e{cD@%wuJZBLQbt&{lR1kaqPFT>w9OyOD+9CD&$r!?yj9=e>?yMaO% zzejA>Wb7t0Ji!LB|3#|mlJFygrDx;NzxtvH{HUYfe(uPT9wzm7Lw-B0JM%RY^GFZn z+mpwj=dAvosF{3UC!Gu0q`6`f<-40F@qzzj^fUE|G)ZW6lQbF=G?uCF+;J3pOZ_k=p;y=DPp=%$Ru= zKdbk~=fg!?y4qK^u4v~U;}@-N=jU~<@9c#ver9J+S7%Ra@0!l_d{@VcLVi|jZ|lD) z6epGO3p+bDlAH~E-FkShsG=ef1FyJ{uj%OE7pz&ex_1M=pnXGo&!+aanVlPZ*0lHV z^V>Ji=>lXVRpc%41%6?BNBatB!U`U`k~D

4a@U83H_zfMc%V`xW>GyT6 zU(w#x3zaS205$Y>^6l$3cC_}kPi}bU#@?=ty|g7g?a*1h8=`z~>vAXu&F_RhCH3~U zuUNgFHm0p}#m05*>w9UR7Ow8??W&B&H*eltxK8T2!WEtC;;Vbtb;Q^8u5Vq}9$&X% z*$S~c*0*miBv1asw>GzTKyEvIWq!k=y4iIzYZlct%;y_s^B305tewBGmamz!ptg2i z?fgYqnk>zt)oV7;PoslD;(!ZXJ)K=J&RYm>oM(9H=dA%O-ngNijL??OjpWT0otr3@ zY+T<4eMIVkzFN0|ROG#7Y@|(-rfX(qW@cu_HZxP3sSRzWHdC9Knb~b-W@hF#GrLS> zuiu&P?C8vynIrArl~QplDzfybsv`3NRm3$v?p~!c*G&}BQgdeQK%K2J%H3yMsmI-P zpFII98^4`1j}y4#FMa9rhF4Y<=AgF#4!&I%Vzjor4d zhjW7aNo;9Wp6ww~h{i#@M8%(f-+=Yw4ll4OQ8C66?g3Qf??D%v2yt-rsncO|5I)M3ie9>Rkdm2Jj z%t#)ML|8!OBA1%nV?i_zJXIxfPyM^Z@8pw8Ox4Z@7cR79A|nl1uj%c6p^5HZA~3%h zt>kRgqaNAbPdd=gN{IJbw449l`Banby=_{___PY4N@+(CLv`l2o*u6Cx!z20rr}<@ zNp>*HnF(DofSgLoAd2Ux0XDL-Vpj_gcwjPirFE;tzCc+xl=D#cc2YWZEm*wcQo zs2V*VrRWj+ZAq{DZt6Zcd-Kf*D5xeoCQyI+)fPOxUR9?}K2BreXq>M5Z0bJ6;K4y_ z51rEDbD|q1cbZZ#Ff#95D>;*fRV%@6+W!m6ZHzHi4%HMi#-DV6<-UC4{tLGHBxh z$R5W!W>j$1aSTl>W$K z$9xRM)P5VgyQRM`$jLqn!DS7tn=D z*&F`{H49<8o%sJ5xi}pGGXeX3l)bRKczv|HV|q6SjDvam7w0D!JWzLrVVx`a59i9S zt~5YCgzW>2385K=ptae74}WVStNX!xWT@}G&|f3(a%7D=uZG*oz(K^op?Bk7W44`2 zm7&AM-|ilvUKl@3CNTHvvDp)WQ}RN-%$I6+QvNwFeXt-V8#LPm=Y4+{r#Bn-#sx~9 z4P188UnwWx;DUfFQ0&G6)0tJikjM?MNJe{1%37N=o)gOLGGCp#bFNN!~d}Qqj5yGux3vpcST)wo=v#F_0fB#(dlC>pSsG+d-f}%*jAK;+`Dh0Lnf8!t^x?*h$?3P@X7*yVlOu zdf~2ru;GD^6mT50_@9Hdpl=#GI-O)|?QXcS_fSB~_hR=p`I8?SiSqXnf{pYyr1o}! zwH4Kv@0vCyw?P>UU4VYmOQ-L%19iu2tqkyQX6xGJ3fzFOXP-I+TS2UW-h|3n0JSG< zeca`k8~jS2{~@dEN4fZ~K;wsO*}nqm(}MNm8LS}6upo4x zB9xFQr67MnNRa+9ilBp_A_O2`CJQVa+@A;J9|MU34e}G@AM*ykN>6q3o?5Q;5-v)BeSah zlHul%tz3?myPEUI6JI~SEo4j9kv`lNo8#0jQz>t%zaCf(?51i)1IlJW2I~!W5P2LP zh$^^9Md`n*4ahe{j4%Hm1t|{@`*#H;;Z**||5A`5D7b&L7n>6#3kr@J(UA@m3cL<; zcfGeMNKYo05qt%#26NLX!Ojdt5coa|XKPChf8t13~KL(B*iRCZIKL+F9XMIhjjUx5GMG9esUV z_?Lp=h-3WYe<_G*c>ljE%y$HpfBY{6jUojLYJ(D<)Q`>+(dPgri63vU=5+U#K z*If6vU;-Ua1IJ)PrzwV`p#pXJ>d+ewmVE%)1%Z2K^rKe9Fb|EWY-p+~kWWi2`|@N0 zPq^_lclH%JKe{8ox+8DOZuUtoFT^>om_2_;WA+ZBks0bTO3eE{Z%foF8(H@>WchAV ztZ%RTjo25D0QPIX!`M14l{=EJ#%JJ<;D;%6}?5n%l zXXc=^k?JS$qOc-xqTERGSI(=wK4)CWV$kyfr~{`D;1tvW9AW@``w&HdYgfT5rw^ID z7J(f3qt74UGz$FicL#;2gzBV)(M0lyBuQi4&4v}75_0IjSGvwafQOnP?i9>1@;(;? zZo|z4Q8@HAL<-G&d3B;Hb#Px;qcsiO)v-@jCMF} zMC0B}<4%>tPd=V)d^jEFWGT|NP`G)js;f?gSvp_oLKiJttXgJ9Cl#WO<)r11lyzKB z!-Gb1($k+f$1{SGn?KTp5~P7pAyYC?gw0z#cak%sDe6ERo zk}T^tHX2>!2D~Xa&uvPwli^=DzY)S8KQVV&ohFEuwD&kH>l69D62e>E-Bgu)phP2? zK-`+pGKqAmMgT;8Vlcw)6oBUOZ9(I8kif(EHsIN?T3K<`pk0Q2Rhr8wLTm#(8MC}H?uj`nKm21CdQ9-H%CTbi=D3B^|jvgJ@hPxfCSMk@JE^XOT-ez z4?E&P@sB4R;KF;uvmiWI_FJbPx@Tvvur2bsdWY3mj)K@*RRCmvpswF}RY@0OI%n`JUh0KPKCS{gGx|VR zlqR_LPQt!jhVfy0i5gnkQIk=svvTN7;WvEivy~Ui2g?BRSQE}VygL|}T!DGGJ* z9{UXLV-jC=uR+n~Ibm>6*sOas{`1e59mCg#J>lmUCE0?>dqeNb44uSJ8Wm|Ta{OK9 z`k$R--$aM3f|BMsNX0%`lWHhyDc9M;?0&X*q<`H=`9*qRX=<1)i7#v6+&MDi_3SZ% zG0O@oN8*SU@i`$9sCR=#f?AWQyP}Xj@s$<9I-G35K=$!pD;bYmteeq zIBEaJn&jr;=KN>IzbK8I|G`xI-@v^8K(qY^Yf|R_%bH{qbNFHQ-vB2$xJVeeSy@Q9 zzaV+6Y;3ImXTZs?H~u%kN$!7GaQ_Jp$Mqi{4lZ8i{~zFF;)MMM3yLJ*6^>~^ zlH`_>(x0^V7W}3FdGQo_6zviwoH8H<`{V6yT0<`n9zSex@&=)Aj63Yo_~iIqnqb2; zZK;j89mjrOuvs*_znGZKPJN~2zCPjeX3~3;$O_BP6>Barv z?t}%*zF63iQFzsRFgbPjSyS92d7bisb(``*#K{O0-1awkr%e4ij?SxhdC%sjBGJvA z>TMFMt)G-c698wyV^m~Z|2x@N583QB>#R7+o*gHOE1Lf@{ki`mH~x2;{|{Wq|1sPD zOE@Ga7aX&ewV8{Rv)=y=aL4}&;`q;2@}D4%|18P>v`YUW#r|iA<3AwW{}JNI$@aC8 z{J$WMKu;g^ku9E>nTwjn_NIg?y%fzBv+QPu4K}FSx+(Ksq!v;nu4Yi^Af*}#jAIfA zr(G_Dub{z=Si#*7{jbx_G58Z7Rgx!9xuS&T=cIdA z7j9Vj{OXREdObfKHzF-CLe41`b(XdkwwqaBN0d<4R}f#Ixti_HzgkF8iCU#49dkF? zUFbjlVqD}z01R>io)8e+NOpL{n0>Pk08j2uAB?R}wT?j98bp|Z*!Mo5wrmY;M*KIO zbp3qwa!y^vQf>?!1&hcf3wI1YgU*fp#~1KEiq0wpl9-1}(&b7*snOP{B z_vlXX(by}|w|M02k)acF7q}_$*nGUj;486)xy%!k`Mk?#=x|X(^{^Zf-wQE}jF3s~ zz$@wN0(uqUS3$lYu`*B~)Omi6*J7kR4f_Y_GvZ0gExNL7fXy8DoXlo4b|J*zv9bz5 z0QEJ#aL6PL$H%~}`|erg31F4m7iTJ9_3c|&pcVBi+k9c=9AJc5OzxKzq8EWtn6QNX zVuZc9Yx7lnZ?0GYp^P z(5dbi=wKm8MhsQ~*Ju}3S0rL<8rE>oKKsaZ*y6~XQ5npmK}Lw1#H@ZJ-c}4zz_tf| zwxr*XY}T;RkkY{Q$a4?nwa5nEtc45W?y^=sfJk~K54{x!Tg_q|q03y#xDn&-DL?4u zz3_y?zIW;wAr$EOr$B$R~?XOvEgl)t>KzoyRa_RxsNQ#JV^#o1%rXDw~^N zr!lk_OtzG8sM|Z#E3o$Dx)sK^r?^|`ST>!>B7HeE_4?A00RVn)sjz_U^9f}Opim)IhUN6^TG*W7IpB0+y=lt(0LK! z{{$J?jouD@6V5MDalP?Q@=AO~?26nT(NEgU5=;crysbSO4uU03h5D{WYa3)tegHsv~td^XtcR|Se{4r;`+ zNbT9%GRS?`#;-g;rXZY9+|;CAhnNG+vHf}CKcsF*9a2xjL(Zm>rcCCB*urbD%a69z<(}_wav*;hSK((-LR2b; z8*cTV8GE&Q9caJslgT%>1GYC_&zFPZ&rm+(-LN|`*I0glz7E1crN8?(-blH@J9^K8 z??NBsAMlVO@^c_lzI5Lx%Md!P896g;im>_|2$#Tn3@dWi!N?zYT$A`jyA<%Pgb52b zPJ~{4Uo#&j9%fFIa3g-R!Inhm3|E=jn{AuFDZ&A1KW1gVNs zX15*)ACxF;Z#^?cdV0N2nY;#`3m+KHu3eA~rMiLz8|74GY9(9pf4*}(>Yaf-LJx=Bjn;Bx8NT> z#GW9<>L{fNaN2&PxOdYRRK;C2qI(fpG`fG6H!> z6f1nTN$Qo&OR=!pP~63)8V7H5

{t8+$i)3|=(0fm1fuy|cJzN{geE~+qN$m0Wyv+99P#xa1#pD#`q@j~6Ks*#_tn4;m>vhQNQ-Yd z(%$L0;FncIJ$CPmX2BmfBFNRCOkP-a5}$}bt}vkx)wPhty;~g@X2&wXEarY}Y{wt4 zYT|3C@i#EOQCa^d1k%!jJQA2C_7W|H%1QO4)M7a`%Aqj{0jy|@5b-|$o#V$=&(wxl z;}#u83$*~z+WkhnCfV}13GMRWeA=jj$8DYJ_iuTw!LT03lfUC_9q=g{6=bXk_eiam z56z{XHS2KvJY_&-Yk>MPq*|(J48M81;WUN{e7aJJUyQR;iHGQGwLxz;&O4{xM|oMh zMWT=+e{v3P*|!GNwp7*bOFPmV3A5QwymmcSyDD!P7Z)t+ZIp*g&KZZG708B1r*(CO z)p;SZV@1t=yf(+gm(Xi2uB>fc9cjBjtTcPCM3G!i zqS{F?1lkOZM18ZB*1l!+!S=*=q4(qQvbnY^B+y;JQ*rg+U$4<;6>H&4v^*iZ*=$F> z=d@%@Wy*K@bs4dx=6w0E?Ink;#00q7nu;Z8H5ppe=f2I%%#T4G7p`=@X7ISNn+-vX z^z^`Ls`MO!*@i?u81s&EAMY|KZOJKWG@ObZ?PBjb2N-U_ftzmd2V&W|6W6T;qwibB zRp;z#YXCtS~sqq&Gx%1fpAsP6~Uw6g~B zuoRx?sTl93Asun{HaxAy;FvqrAaD8TXSbi=XrTP2aIWx9{clh3;#Zol4(Uj<$pNw7 z-m_A7-n1b0uLS|OhqQJt80EQd^kgbvl!^XOFX*@N-^_~2!a-p4;K||avPca}P3IU3 z7sFdp_euS)#1&>+?52`oPkdT35w5EX!F0i~$;Gv*Mh)(*qJc1IXz@p=70Cx#8KbOB zl)U2TyooI>UZ63#m`$kr;S4M z8mAY`M9&wTX>*aVSQg)t@-{<|E=hW=IEd%eXG_oX4&7sr3Q>G%geH0`{-vx)6*t`~ zxLv^bSjN~89A6NL;vN6(KZL4Z_XwqRI2?%&%!tJ zgS&I)mn9^()DWUAzSxj2kYbYN@oxi% z>K_Ux1oBFGI#1X;T8Vv@gnh(kI&Dq1jS8YCd9L+h^X= z+NBv8QowSETz)9Ir*h^h%A7g@p&|9vTHRAeRch?V-}~n?j?}@fG4QeYgLH~Sms_GP z-%?Nj&Y!-F)*DVai0wbc#WtYQ+fBvHcQSGmWQO@?&Py5u?_@VxSwC7$-u9eyF5O6X zk!W3rT2>Tfnm+P&O7~2|7a-O5a5U*QvxGy%WN*k&5n3YLu?_qL?N7mF@{k8G6+5C0 zk?A?l;EGghsZkJ$T#G?#0jl+hvdKxwqBbnM#ePvUMHytn^wV9pnSeweU?2u(t;_Ej zT^_%kR|8?4T^Ob~2;hgV*3K&P_VXn<^5*QjmTZq{{!Z@GV=SS)PodxTZk;>Lr=pPT zLukxx!=l~;sk;0`$R7clXjQLY`pOmsq~^n_vsJpd9&sHL)GB`>;>b2L&J+-fi`Exu zjzYH1QtFAz{PH}901=Gl-cJkGV zcJL07+f|d=khT>>A&(QpFM6Q}yTF>!zm2{VSB-5y(i(2L z#--S*ci7_Pam;eHs?8cjS>t$a0m=d@>ImQVm`&`A>U9~vE*A28lN?_kWzP@=hhMq3 zuG>BjO127J0`e}F{%YFQx@pVVO8-p%j<{a+GZkT?;4P??bM>nlc;1Hvwi+0^jx&-?rS~r`cw-UIsgC9+mVktHcbbc6P|S^U^VlWA@?n>BA8M zmEk3v=-$28FB|g!-RjNi-_jiXty&q3;cw6VZ7IH!*&U8d*w}?bG9XgYG#7t)vy)s; zH<1_+CkVoivycWuDUjs9v!Jzoc!BvUx4pAJpWYDVLZhdWpyNq7^ zy;gaC;5Q@lUja7Hw}{o6I9en`EUsfE^3vG$6rSy)g-*O#=&Y?nTTop{w=?3ys&E-- z*f4nb^EOZ=ho$~ZSBwsl($evHN)7QoF5%^Ph_J-2vYULLdX^dy<}n%da1MYA+z*vs zG#Ongo>?kh8X$kTf%!!>em*963|Wm8Va?7_8hTtN19=SDg_lzp%C^hEHfe^&G=Vm9kxiIQSe*)TRh{If zP?l%}xY(yCl9!#T>6KV43Hn5cA+kdU8j`oF9gB2E#;xOx>z(uUqd^)WuN(oUf)sKVh+u!e^geb5}E);d#1We1ZOI6 zCMz5YtX^bL28;GBgV)(!7;Kt5=bD0b$&A2XZ;1wo&Viw|6FoIJ$%KiqG~Lp`0KUTZ zJes&{3sw`Q28`x74sz<7ZOe zG(1Wt`Q)5sno04UEPwNxE5lppBu2euBh{wqy7jtcfdV!hT&NijEq;oM%$m@eH(N#` zESm@9TKGkjJe{|!so5-1!PuONXzn6R1{Gp7M^@;{>@ladxGq`P^x`6)t@8=~>G`!0 z%QAI=13bjBKdB>x`CH`-=-|US8*JdBpj7Vx%w9Oe9{Z5YW*{A%vJrfZ!EF_Wam1PJ z)%i>i?isWTZ-BN(+I*%8Y+V#yR+NYb&A<^DE$l@by*CrHU-75`t&^71t?cY5yVP_> z-CcWB26i8TG*i%{MGyycrA5CcTb-!H*ryKgTUNJ#VNMemClea7H1Hf;Y&V{%6j;!eS@qV#rFj z>u41c^vZ!xP@$L+H)muQaQuv-u}l{nI~RyhFB$&q+F=;uWCox!#=~qBBX|lC3bOK1`{RBjWEYN}#y=($8w|)`tBpp~M>;q6U)7laINV=g{#G z&-ZN~&WwEV{uXTEZ|7~1&$YpN`@SR&tJ+eAncq1?t!OL33Al@+u3Th1b$E^c6mhTS z5zi2>8VZ#z9vj#%;+x6@c^yUlFiRP!h%4*j(oznHe#j>VmT}=RR5HUDC1>*sTLE5t zsHQ7z`!AtO_#3+oPY994=6n1v5(js%gl2XBK#fh}Rq_=f+D4{PROZH%QfDvLu1$w1L-5D<0F4X`fV0$+F8=7|WxKD=Nm4ng3BjPJ^Jepj^yr zDxuHV?q zX`LW)@}@~OIC^qo>VSnHN)r49j(e5ZY z=-|>Jyb8Zjk{cRs@~77U8uVa|()fL2w!v|<0!!57LWqh@9U|tLnFhSJlW{A2#eDTe z&&gj2t)$N^Tq^ID8yl4!Dh?{bDsbCOyUn^Ab7}#TwHOrujByOS6zQC{R#8?hUhXYr zLp@&iSEob1L#7vnh|HvQ$`X8qq)IPW&Lujz{P_wG9?e&QW^zo`=v+P|0Rl4#Oj4od zagI_ORTVIgYPt;UAVxYV4RcZn{;Ek1T}?eN2ABT&1u>oOgTE}t?)77I`4JTnY+ZHv zIGA!_g)(IRr#R;C>IfscK$>FBa#=aGnSIyMu=krhFa@X=gAdZrKe?#y$H2un;r&?Yzll}JR}L5B#gl|Hmv&< zv`A6Kd7|Y1LrN4#ymNAYdj{1qT$9v|mndLN5@${$K#sH4KZv~nk=?TwvPRRm^)sO?jbZ5%2`p!wglL?lQ_ zGNe>YK|R&rz|bjF8XN%03P!)G+24a=Q<-a?pvxqBb8*)Eruwasy85dQ)rTv#n}(8~ zx2;UK>LXsb?G|+$THBjj?KjwVa+-*g8rck0@$|R|+@_)~#=Cv@8^JUUG=3^=Ww#cO zL@&5VV2eTfirwFPFGIHikPo=p$``xb5U#O+{z?UlrurV8hl34k1i@EKV7i4 zp@`Na-BZtCN=olnyrikdbjfjCJ{QEA1xU6(YO%-rmi(G$~^y z`4Gvzf?jnzd&XZss8x!{gPo&)n?S;<#KfRNs-=z{q+^j9fLMD)Ca19Z(%ZRxhn@Ay z1dq%xlTas<6hs5}G3j3)j2~;@=)aUqfdc2@yhTfu+Y6QMo}Zn{&I5XA_livDdr^Ia{G^%JS=R9GG}IPhRSxPj$dsPXEy;4m3D)A?hjjWj6Ya0!Ok&qLi|65ad7##bcr1FVop2DA(mOW_Uf8KIDQ+$FiLhB^ zj9pueuc*Mvgt~S z3i1;2ShR4=-`|ncD=V3PYcRGVqYuH-q^uH3q|jy3$r#u&W@lH|#B6XdMKTr*@Qo4X zE-_TnJiPqSkh9+FTjQFh7gK=ScEV5JkY0gfv?on!Zr^YrH0Sk>9Wr0(5Y(L8CU;+R zwXZdlR?)Vo^E&9NCyl#0{Ym!19nh#$o06T7oibO?)x9DJ5eS*>!ri8~#evFBqf+Ld zO8Q5{pY&6T*CkToT@BaghK(M9)Q8q)WA%QF5;RhGFM=vj{nzbZe~s#0a+#hTqe&xi z_gZJYM=Vy~TP9Y;?dtFEGwPW4c4pEJxguw`$ia9T7Mz9(DoY`N^Q)F_GoLXI8({DF z74x9?k5-kXGQ(=!>W@mun=LEYKnU|jZ95Js!vG*iNwkaw#s(yGN#ZQLs)jDs5}krW z;N1f?x!ncBpPqoE!e>*`@gOTgC5#vh)esod(D-M#3Pu z@On&*S^LwX%%Y!B*_7Y=3==UgoakH;_=dRKDm7;}2{DlQ87wLYlB~v}I3~G)qNy z(q_R}U;sq&{2@eg1+6N9DyM0|3zH)z-L}7a5(r`lJS)%TU%UFzIwms-zCj?4x=5mEN6#3%d3Ci9Lwew6mH(@hD~ z%l1xW(olU!kb*j*X@gj$2imhY>3Bu|#hc=MwJVuQnL^T$V;`-S_?yhnV=#<_q<(oH zmQ1$bFicrsC(dCHV#Lhu4FkOr`4s)#U#zsJ(Rd=0f2Y%8D~21ZlzVd1PkbY_-C`k( zX#3a6dZakC18_K1XiHZOc;i}Y?&rMAk8N*DS#Q1dtRn^HG9F^W;|oUV-#nRPkU?S~ z%#Oxvr=UNgPrWvQK#4@DNU2TAnv|#9uWlB_kj#+G_a{>dv8jDT% z?N2G%F*(Kz`L=>Ql5Cv)S>?0T2c2(qL1s_rPVCdYQXTc`H5M&6o4B$@p*!ALREVfr z^ni}`ao|Rh{31-0#B_TB51#9sEVgR91{w7nzMg`3i8$4|IUbivNo>D@>YOhcGv%Ih z!@~T!hh2<=1PI3Ps~*gRzi=MTAc!)eRonv0eMAXQd$UMzJR}fQ2PZTNlzH&gQ=j7Z z9gU?Q`U|v(JmeKQ*6v3)7771+mJcM*K~d@Mm=-|_yHtXWVqANtk}$)4CcVec(z`IF zWtGgPDiSoYZX{{#{-?Euw}~XMBK}I_+(O~=VoB@v0#eD>6^XPy7x?;B_uCn>Q+__h zQR8>{CQRLEjrDy)pqGogdrt>9=+ThP5L~iO^?>r(Op`fP8EF8`qRy<&1-7HVyT7j{ z@kcyDJ}IE#NlTPiILMbdK6QW1Bi+2*w5erj{%qCc)cs;<#~5cBQIBZAs1(UF_OHYK zb=^*bFAe|sGr1qAUeTfe&4af{>610Vl}$y<<*_bM3|#zd7231nRgLM~0V{SwR-ETtfG}Tu}5E?%}8y zaa_npcU^Z%!!O_<7RGP&T>cemIBZK;9SU)JBqkV(f;NmIw?Un7)cg0J;!NY3d3Hr(LMQ<;{t!P#Jqli$BP9Ry^|-@jjPKF>nq2zR_?4$WCBX3w8z&#?j{ zq@SvEv<)L_1)c~Qeq9q*t+kNE!QiyCX5B9f+&#|N>~Fw^Y6gseh0?`C16BVZy*XCa zu5e%Q@A6+K-gHwDQ=-gB&*l{ssUy(SIU6&i!*}_zV*J(AE2q5Rn7l6=Py`Cp@>^B! zWnb|nYnbDtn0%f0V6c!3VTlf?ll*<=2n`X!)*sxTdX(|~An|G<; z?VBESLm8nNjv3J=P_Hre#2aJ5sd6TCyhH@Sf7(TaP+Jbzf#xj6QdbFxtRbXMQ1-$0 zQRs8n>8hwKH8Sf&+SWHU{p`qcT_+$#WQyAqWvhsYC^6qMeXoxaPttJQ*izZfXt?n4 zd}p{4Wfm8ff`CRVlGTk1A|u1DSg1Vo=y{LJO1M@v_=RZQvFal6t;bKcl0_DV^VUOQ zYNCKx2?hcid!N(>(p-w80?R{|O?_3RSuIesK#H6rng)q`Pg5WYd3QlOLi*<^E5{&% zj!X{QvY6Q##3?yhvLu3ip*Jff?pqYwj+~7UGG3y3JyH5>msr4e>kuR%Q{y_--EFB*}}c|j(W%$uf(Xni9Rz2LfJMYW%Z-A$xF_iP<-HH!FU9 zHxs4(Qs4}vVR-agl{qaA_ z%X)6@QB3s7p7O(C3wG3hFcoyCTT^^VdvcXR!a;;C5P{zJi z$Mog$AY(k?CcSoUoeml56~U3Bq?>aBVJ!wVqgf2C=EXz%JF5d(NM!czkl-A~-D`r6 z3|wKj)ilbI+IPD$qsI1Z5#PKsIx)aBcmNXWfTU>X-zo~P51dpPCtZ>tG9+@7pUxdW zr|WN&G`YzVQqy3l>Tr4?l?Az}!F$Pca=WNA6>n{*k6|X2Vg++`kGS~);Z>h|c3$?n zrE`ZGquXfI&RRp zFE$-ORx{W?^v1Zxpo+k7KmI8D)PBG|$qXeDG+;Vdy6@|0muQwCk*?z;j5BI5@rkO1 zA6exn&Vw=LA7e#u)bLGV!;wUsSMcyMmo_?K&;D+UiFqQ3d0^&g@h3;0Mros9O!?li zT#K*@7V0lm97)RGJm?4t&zcBfk|jnspb00?HHjl`F^topui8n}()gX|l;?oTm-m}j zmHJ5hDM(igxXLv_${v2hika}ZZ(P@{7BlejxkS+YhrsK6sW+Dlmzj6H@Wcm62hmPI zrQ;=j*zuJw*_+tYT4BpBnSM~M&nuRgUg)qNyweVpx+o)d)oVv#+T|c#!3*Y?=3ML3Rn1k+HO}nfUmNHKiuHv_< zHmkR*+hjWU?;!3aA{Hvu&Qx97Sv0hD(-e0hFikzEui?T>%W*xRG^Qx3Wi02DU6h!! zb8-rE)EXt)M2twmnoZ%66n>mQVi3(>wlQHg=*3bKotvK zXxt?)^pB1E{^~J~y%u`v9t4h2qm_#7FumFzs2{$J2Zq$c@t@^igxzGO_4Bh1Q;UAH zM)~@aWCY<;KT+z|sesNmRoQA}42*#0s(Fku^^EX}ZvzkqCM1u4S%Zu6P1f=0;?!oo z_np!Xtzz~Ds9a#S247JUKIS0Zw~My}TpI0@)`3~)xR$}-6_RkRPb0T-x57gsU&Z)o z*;s!a`XQtL`Uv0(V_N=Y-qoQAvNV>lg>ThR4Wvw@bH7o(}Kg zpJDkuZ=Xb}@nF&j)^(eF%#Nk$J0WUl|iWVNT-x3WS?fRh&$BRg3|&XA8hdXMd}l$9u2Ko3-JcJ_HktBm0dbnGXYVl5%{zG>RNJZW_o% zA!7QgJ1(K8>{8v|#pQWTt&IJGaEPssW))pQ)2BD0$NbmAYff^r?@;QWD3dqm&gEEM z;V1Ml&2qX&;aFY_5vf@ujX<(GKL3zjWq+`P_h+4#nD1h4Xy95SZla)GghX)J$SS?P ziiAOW;?UX!>g$>P9yC$Ew^EVe_u_BLZ%$e8?V}ZRwWkfj8y|G+j)$kgVjgvvR;P7% z6U>4jMw_#mviF%#+?i$y?FOH~=%^vk?=jk*+X%3(Qw~tH4>s7);^if%RLM|#?fXYQ zyJno~>2x&ghY?y15GF`e(Gicd{U_7nhp?ncIpSc$#n_Cwa53U`-%*{1mB0!;TvyZ5 z55%WP6420t*1nUZG=0ZoTH9<2@BGX8G`|`NdbQ|5bGh7gXH!mTRnb2){S#|c(XWFf zt?%k^?;^IpXXaxm7bs~giTNuv_a~qJT%GW~!$l37tyNDhm-9B*{;JW9rFmyS`0ult zNMaS|k(|QvHd;=QSR_&@NL^)KP6eD53w>N!=0leE9oil})+z!`!LnZ>fV>aTm5)i4 z5Hi!LRZZtg*Ghve{n(n3M-s}rs8#tJ}pZN z>o?5Irg{jC!CPOUxKV$h(K;!6TEjucJzVA%0_K(O8DMhaw6|*qm%5qTY%2%f=5;3XPj1)^(M=US9r}NQu{bJ`-D54sy|JZiu!^)%&Vz)OU zctL~3IQ~oC8=^Qusqj+(gE&U*b~$2_X4`F1;pm(kp84T1F~k+58NSVzEmw|5UCw9; zLnjHO9rGb5SSXPcA)>MZEkK){wLy*y22T4f!$^@26SzfjjhRB(00jP$`22e6O6Aw9 z^D04`g#Rf*GfQ7-K&mX-iwVc_L24IDR-}`zk|2^gcLe6iL^6Ck&@OMtm94y__eW8m zJuXb2b^=enAOy#3iOocO0DXP(cm<2Ymq&@x8Ty7Wl55%LVGd7e-ZDGB8gW&8O;~#$ zzW!Fnuu!#f&+dz-bVT8NSWw6nIP4yMz=cu)AA;L*2f)8Zo^^G}wuy1Y?vkVQ^zZwnhZPE1khJ zNg3Jim5o8eZ8`F!(s^>yjS~~CnoH0p8Lu)4d!~$Y-o$rBugmxpKg21mrzbb`r=AU4 zQ|nmFs165?Dy+jrJ7_E+YbnEUiuNl()J>yvii$ZhQ>~mTt+4zdoJAor=wP-TcrzPd z)it6YwKQT+*%*KSUSs$B(~EI~5ALfr8W0F61pSfa#^!30s}0k6icuFe6}S7Lm8SmpE_{M2Ww#= zFy8U9zMI(oQG;h~VQYn=Klo|wgy>cdzY>0DW*A3tf3Omn666cB@AJMMf1X_H#)g`rJ)OilW~$ zkJ?5Q`!0VbINga_MIFth-!=Bh_qcFf`f;Rb<}{g&8mh|``d9VO%u_e7+SAGh!Dh>o zW9znvWft{4g7)+j3bU+f%P_?hg2D?=g?*k+e~N>=TbjphRs%B#7RRI--e#pPBZYG5 z(_j=TJV>yvqG0c%IHV4jB|ce8bdi=gty`?BrdEa|^BTX1_O>|Nm*6yZ=7)ePRw!_{ zJ>Z!$x-1Bwokz8Ej!C1=S5E|&<)zro6QMk6cdRM_M>B1y4z|(_sr-(|e6%_rLdGE1 z$ZgoelWi^!a9HG=_j@wdpa%C^xc&{Bzd5Vpy5!oyy?QFgPssaZaC^V?RnPs3etrY~ zhb+pNngh{JB5=*SaIFUcAgJPa@!Kz}^BKJJ;DwheNHBf>XrAI14C9DvU}2Kr);l%2 zAOxN`d%Al}*{vh{k-5*RHEuDJ&^awKeX zzon=|+MWTPzH-B0TcaWtlM#toKPj#WupyPvva$fpPrX>VURA1pI!7Z504RYE+i=M_*7c8-y86$+zzbKdNlrtr34~5L}Yny3kGH-E+r`Y=LH3MMkdlj zmNj28tWGA2YwEtp+?v8o9s1b%)UxZ0>+Wcn+vhf^6=l=2RfcJ9vx2b)m!&t%rzm6P ztZec8j8fu`HkPFq$E;)_LUoBvG|lE#B7EGT4HR{(-qz5twDP*_mSw!j<@1@vm3Mo4 z<3f6lGLmh$Dh`Rj5*I;Qu}~N-&l*>j*?m6yw59l}d!ugAjEku8PqXvrlK?wGP?{Ox zONwb^#v{Yc_}yHA<^RQ&Q|w3@fNs z#gO8Vf>wOQQwC9c;Q1zZubUcjV>0EDo92+XTPt5~n3$iCnV zBjAw3+eDgG%lxGQRUqk4vn4!EsKHCg%XnO@jPswv+kOy|S|YSRWt=3WqLLk)7|)cc z{L;j}BnK75CYK*~3fls+_ciXlvhSAOr{J^qS_RMYA*eRFAiE#lp2Ge$uzl zq9^-Sre|l-zl716hUTJXU`99SO-=U(?k}TJj9W!*DcZ7_LPgTJbkVImQ(fM;s;7K& zpm)_iW}oCn^G%k0>Af>LuLSDjgPewE$E z8&h1mpv<*O^0;hXS+yOR=rZ>_JF?bZ+J`7N7z*UFAwSlOY9pvaBove&SMK*8^1tM# z{iFC{L9-$P!>HfyKIDFh>>|h&6+pVrmikqxMR?7M<>L*Bw4}iN?gnNs&37qt?Pw5*~*;>8_Ql zPU3CIjTMJM=Z$<4ArwqpOn0vTEEfUcpfA#Yu8#wCx1T7ox%Jdz?~~Ql)!ey$(0uzj zxal0Y+7-`|Z5UKbb3bUy#EkvyoBsFwh8CqO<{AT|2A5HhK;7S*c-EiGtqkqR4GOi3{*lu_dSf?9&5a+y>rlQ1e3^E{pl z6h(&D3QDmttL7xW1LK^K+gByZxK#@- zkWB&ai<`r$Gl{%u26PZzMkfN~?s$YDGbs?&YPH2l)Zc;u8L-i-^XlAboPq$;CypN+ zdzsq2>0s7}FWixR0IwaQZ@73H^~l(2GOy)9h>3_RqoR1>2^0qmy|Xf%h;OzG+6Nui zg?i(6JJ{`(=X@tazqJ3-@e7~S8sy@kK*pa5P75XDD}!CZ-uO^l`CWu zED8a*Q8HGhM>fuuvFW|jVY{3pY|e66^@CcUU|{k?N;T9Rs_uFY<~d_Jk+$Tg3h*34EY9vND3 zZ-W7(j`r*39Uh zRFz8YbGe3jtmSbl=ziyUnmT@&mp-5F#q^PQD=(Pi@RusE! zc3qp>5;t9%pWi2D7e%VF{Kd5&ot^3RmZ;g~{^eh$Zo4<)&d<^7#?R8{=(`Y4RSI(b zV#Nx}Dl2Uz3q+J@59#eTW4gdvVDrj73b&5e`z*Ybw^hnBib_4%RZ(f1Cz~s;Q`B4P zt#fT%vWH~%%fD)Sz`oz}Bzi)2Q2sUR*KAMNUz8n_A6Fc=JZC*=d)|J^^Oofk#V3}} zY_a`v>=Bnyw3dotq&OEk!?|$w>|7WO=0dMG7wU9kC$ z=JrB1O{LvS>81Q~_Xfun$#%zXYOigt<5Btv`2p`4uNHZ+cn+A+WR?}EDVipXP{+Fq zc$(n>cN{cpVI+{c|0vJ94JYxZ$U++g;F$Qo5cOYfH)7C9JOkPB>|;aXB7{-=S3x0o zvEXe5f)AwS)8iaVFc5<$)j+yRc_te%tjZv3Ih56`v`qize~eWK~-EXtd5&xr;f z4BU`RQ{*@3AaVi2)R$kLsNBHx=sTRflHNiI+2?>+H_PM}C$f`dV#0_+`W(~S7iQNt zWBG%-mfv*6!1il;;$B-Y(Xe3i;fHVC_yT4m^Pf5%eE62p4abLq<%>%k5zbwDc;~fm zRmNCKOZ2|a$O==*9mqt^1QWJ%0sM98ap^Jkk1j?MAUYr)*i3I>Zl-rLkI_%d*x4+u zluaz3wwP6jv6~PFUD8akcE4QdEOtqTB~+_qNODL*OFmMW5VH7GDo)*`?o|(~nIZ5- z)HG6aYF?d$=qdGSHLE6=Qk7P>`d`j(H=I=UCv?1XL^KA_zeH(6R-RU}0iJg9*ru>J zkkz78Ib?9EJRgZ*df9upyS(m|6AJuXd!yN4<(>0=Lxs|&|MW4|g% zz4p?`x9kNVv#bE9J`7Tv1%=Q#p=h&wi(;$h2Kl@G5B*Xpjd#%3FxQxNnVBkC2!v0s zHDs02{AwANjpEwld;pM3i%0S8M=eOgk7DUjtr`QE3Iy6gRkEQXp@ICkxJ9$Zh98Ckh$;6I0=02S*)# zxxUchbWl>AUmftvy=$%rpF)M;2Zw(t#7Qm?u}d9Ui=Ey{(ZyP8~WL^QgfH@xn)OSxn#Xx z-T1Zd?P)(V^3-ilU3nB29>;UCE5_<}FK+$q%*5C4d-@+dp&(?h@Ijy;O7x8rh#5a~ z)L@?>Av7%l5vvSK=py+{B&b`}2h`ukuT$^fcc?RJ=tQi z46T-cv>^E}ph&aR(*GjoI*j7`j~`HCrBx+4NqvB5>JI{70mqPK)CU*|^8)ogQssM^ zBP2jH1z#X5Wh30VNOcvNq};6`ONeqy#-Y=IxB?k<6Qfb^N7T==RlWG`?5%ytC8D*V*W9)-LCk z7OZeytzE~hcW&W^nBAJa+TGl4{T}Bnt_QUba`)>Vbe_Ws>@4R*=}o#b*-sL<(j$TbSP+~hFZO)?-PGNZ1*ZrtY7 za%9WC!=vL0biF!E><${Ttx`uh3n?X{}AL_ zxQ9BZ5p<(e@@S(IJ4dOH1r1MjJD=Ckn#cJKd&NPr+QwSWS;*{i%ehNaa2HyLiHR%< z5qDFzklDB0k{w^jY;*o@ZpSNCY!&>ARDDv<1^pJg#g(gM8cf^@kixk+F>!`WNzqS^ z{m+%2X=}1ems(RZ@UOl2olMK(u@A4vgf_kZG5+q`jX_s}_4~D!`_#??3|5zq@iaaq2 z8tT(P+njioa06>mX3Q4H^iqoeo|Sl7r^ys%tJt~hldM$WS1~JPtIR7c8)SpJLH$F@ zuWIhsJ)?X^^Sb19^J|uO&F@;y@SifDnvF(W$XF$Iqt#?J7g|`kS!q!gmRe_9_n7zb zti?)EW}8)Im8xkgC6QW0bAW*X&3C;ZmlG=z*-br4rvw!zvF)?s{nkTP%6gJcAsQ$t zrc_R#sDc_v|Etlk+R$UzX<$G@Uy5J)nk2;vv~uJXq=%qO_jKeP%}XV zT5{WLG3`zxl%tERw`_uVf-K?&)u5bydiBLKxDE3UZ|c||@LS(}@ZhhL^B((j242&> ze3lJMvY+|!Onl#y*FUzo@5Fb1HoR`#*N$b+S8&BKcrjr3z?A2sYU*nOw_S(TR(YTj zZ$n$%61|I1NsSgX_ks*pBSeH6x`t4JU%0%bThKP$HJ;wcHL(Xg_j@11k8w}BpY%NG zeJu85;(71${^tWHD~?yc#=UBPm47XBs^%^ITl}ZW^ED2Ag5&ib-WLhQ6N%|uQlI3f zxyypd$ZVw6qZ+;@S#!FE`ECpk#;!~3itI@+wUK63vzwNCtzOfN>Y4^yZBVK&h+|*8 z(|yo=FwVsJ1dh4+1P3(1qpwCdrmvQAtX*p3?RHx*7P1F3)s^;)1T%JtRd2Cd10KIU zP?0FNSHwIV2;i{-jAMyvyinlc}f(1_PAMYL!YQvh~%tI)cbH zJ02(TmADo6;t_lbpTXy`0*_Mv5VUptV!i`3#}ec@;GsOD)OU^xHTSp)X#u7FLJL5^ z*_KOGC2y<4x#ebr5s1_JB=WynnKISa=1eIqWN$2V43Qb(5BJrsIG(^^Phtk$(kw18 z>vDHQdJ?TQjCgcOKR_ChH??bnT@_T31(X0UrzTVX9|#6j#tVoX2j`t*$^?=oaY?{& zQ^pfAk;5m>95H1I$f{MPxLcee`)ZZ?OiZTFczAuLf^5IlW`emuPqa)eS=~>9NG_NG z!C`IYGE3l;B?T?wHgh~dAIr;fW;z;6rw~w}phWFRPp+iNm6}*gBW3A2?A^Y3<=Dwe zhtV!)lkaEG#`It^FCvv*YnjeHU9_e(h2idFb7ynBB;Szr@)_-AKd$HiqKqmeV}FeGWs z3%dwy3?sxm2iV4=n*}?EII_qD3!dfFI%+Gmm%pEXl0U(#KpDh$2q{fR*;49CCna~; zX}8BzZr4rsC|q{V?d4s(xL1Qle{kqH%HgGG8F~hHQ=`-?g3@I6fHwQEyR9X^%o!Ng z1#!6)*~xjfC6lul!H9!4VWvN4+%X2E()_Fzu{<#MxCcLbe`2LDwYGaXjoY=7uW z>+x1LJM6FE2k8wv`6%>Fk+A*ZGmBlug7{{TO6Gx#`yB8A4z!4qN7`gWP(31%7)hX3 z+eV2zDz~A45TJxWYv4fOOn?dKNU~-%>OnivK6C&{kkx-lL0%T>d#cH-vQa7VpgIrSE8eE`(m`MdOALiF!y9_)%XIz7< z3genfYlDkKS5$Ppp;TWk8e((>lqj&4bGh}CW%)gjWOirXZG$UYAG+?rTYkFcyJ{Yh9Vq}MsNIvz|uX?Z63eDX|Mw#?cq?PYh$cFKpOL+n1;KDpfI zvKP8NewRJs_R8EEjZ1Eqv2KcTN$sqIvs2g$qF`YP9gM_Lj3dhxsGkaRqR|LtEI3$b z|G?oWl*ylw$)wLzlQj&8<=94+X1BzjiAG$p1l-VVdxp0Q_V?_xeQ{H|H+>*YrxEAz z`yMVma{}+4IQER3KLPT=*|ASrTFB~ILfJkBu!A5g3QV9#KrzDSxN{#P?yoo(Iof2y718;mV8)|oWL)n^@K{KD2u_OD8-^>Z^bl>&P zkU#tL4NXREey;?vI+&0fy(z3x+LeVjb9ZxZagr_EmV({feTMsuuiIZQ{23>+==24J zPMS61-L_ktR7fUu*%3(XF1y<8HM^~@kVd1XtRa&L$sARUdW`g(p4TV!f?lGZ>&=^m z1uy2kxYtXJ7PQw*7NI}vX=|T)!iQ)M7T3P#wq&2v5k$^eer-;zk@FV>oLa{+Y%_va zsBsnj8 zeSLJBY`bE+b*pXAzAd^(vB!EJx=((u?ViZ<=yd7trCzyQ7KucQiWFEh)>(;#FItkf z@C9VN)n-c;DGH!KG!hYQdyyizrN|~{6tXCUR)t(9^X9F5LGi30Xl*dz%@k@&%^**b z<+1jC3jChpJeiK}Rs2aoD|S@N8|ACzw0wt*Se6STZ)=IUct1#n`&LJBB3d1#qSjRD zlVqKOXfy6#a26Q;*ahJHEq!D8wF#hZM{>M>a;>>+%2L(*#K^zo|Cd&y;yz-zh|+Y+p_G zIj!5unN###{$OFSK3jtS5f16)N;PO37PqGQ;-+1<)J2O@Chd&o{nSxc+^gbLx$}?c z{{h{j4E;k$R0u_BMFm-PqV;P7Llwgn%t*zliqjQzM2ed#S}S@w9cCtCjZ2Tv5(91@un#7Kw4 z>cFhb#uiRpFVZK@p!5Oq>Aqm8$fYHZQ&xnOUv@IUf}R^{7WUc=8bwmb&M*ok3ffhd zEWUc4F*7S$Io(@e(YkELga+#+cZ{vMrheI#!c*B7mh%?z1PX2;uDj=|L}_EzA+Gi? zC@PlG({nprL2S4Vc=KN1%~ljZDSVA^vKeD}%9Sb#_N1=y3@L|H&c+2lc!+HCWy{h9`R8@^G!OY|q&{!#Uz=ELBJC1F-A4_2ZKt|*bz$&ktxWJHfMi&BMD5+i{@U05T> zS|rDU=#miU@LGXGX(3IK15Xa5&!m~O_XWtH@eEW1!iFMg7Raz?Hg zNznI2PDZ{Pd6)ia2_vG6F6#^Wq}I6j42ij#C?J? zXtH?hWc~~cpcM3<)m~eD=hYy6eALjBO*hW-XO~?%1!LkwjO^R1n?rSJo2h2=D<)cQZnd7a(sGPghSAFO=(t``DVa*G(PbPm(jZY62}+lCwU*LaEf0M? z55QwB;^CS43EM5<6Na2=NfcTq=MDhoHm&QEv#ipuFDR~TD4V%%U-s#!XJ3;+EiaH) zriy0`tX_ARv`#<%Tgf-cTs!`Tpi^k2E=te2u}P-PJZZwBq0o{uBc{?hGh#GRnll{o z-qQckf95*xrcatrSf00yxIbkjPga(7C*Sjj53S++`Xm8RJ!5!_8^3B6phSSsP&rA&o!7MiP( z_@!YsBnufsCJA!$?xeewtP?{LY)2p`*|i#(+OBb#op#sg_-;XGVx_!{Wj)XnCX-Pj zl@cpJ+GHjx)LmMwMuR9;D*w!k&3{Om1kJMBU_Glz%rzH#t++*K+AEH(QsRsuSc2i#`MDlX(2&aV{g1 z8OVW$?;JN|6rtQ{kW+MNGHEn9D;J!iAY+`LGD=InIme$4XM_GM6XdLOXHZ2~Rm5>K z77~^95|w1WU+pgL{On82msb|Jyr2flL#0aXg# zdvuTMsGC)Lbd=&gxencj4HzMX{0WVxNs6UI1xv2V8N&0242im&s0bLLJye=Q<1Kqf{kAd4QbB~Hcf z>)iKH_RYUtyMKY(+Hjquqo`r+SF&5*%6>nKH~H&7!W&+FYh>?ZqyHv-D-4f{t z#!XZ!MNw9nd=wWSb^l~#ZmUR6tIwUiHnG-q0a3VeT2>C<-8#1s9MzhggGaN!!Jh2Z zKsrAqx@JSRi=xGL-X!=0`2jiAD#zmIlL8oV5+*>=`f52Q&Bd4+kxFunC4u0C#!7fe zuVhHV43U|=0}?FRokWK~bAoohfQx~dEP(;f3BL<2Oq`CCTQkuzrK!1=_O%$GnQ3qh z(9Su~&fiEj{5{(!?I5ejfG>@R>w8`oDp%r_)XKt@&JB12wV`l>Qo{VTOc4Zk8ua;DE)w+s6btqGstemTC z8z-+96(sXGm?bq)!C3IMq1`)ud zD%@dha$;w~-{alsrM&yR*lYF2ZOQ!toJ zsR~jl6_7KFTxCuL{Z?f~LMRGY6tt?8O=}(4;c`V2iMWBIDnLQh>4?S`XJR-Ob2oH+d+z%?BD=Y*uNF}De7IAWW zVxO0C{QMCG+0OO%=j0hPIcOT<9~_0yNX)%*hpt*J*tu#8NrelGXaoktkdTGAtOdL> z600OL^bm$aT2kBjLhQn7$WV(Z7aT$%$5?)vwnTM%~hh*t|0dKqJ3_A~p{`!!E8qYS&>jMe7N>f*8{v_jitr0tAZW6)m3EYiNm zoYu1F6OOAfI>}LKKA}V+c~;?6G8(PcM>7R9&Cp7U(PEXxtfsY;hG_y8kW{Or+-fb> zCNZVZK2Oa+8pKdDgeZ;U`w4e$QsbmrQ1_~7wJl+;HaCLWrHTW&q%dVQnIFla4sf>x z7syHkV(tR@iGv^cphAC@=uL?4`Gw890cCc)V!@Z$6$!|r{s;&ZBD`zHPYZH@ZaN8` zc-Wa5B3gl5`AlR(MOJUgysZD? z&%9B79hu7?89$4M=@Fy^4PfSB+B%B;LZNzH*|7D1m68gGB}cJo1tTrUONWgIjFj;O z>_-;#6OrA5G;<-hVmv3yL@rxbIQ4)XH#t@ZTr^QLlZ5n0?xdY~&6r_kE;f@u5V3@Q z6}WFb`kv6&JNv-w)3eXaW(>0*b_iun5T*1kyVC9PkV9NNr7nBi?WuR!XNX53E5xr) z*xg>g%N}!k(=PjTw->7P`h4~o)2Azy3Mw9pIUIJG-r%7G4}Q;sc~8>Q>p9>#?K$I- zdPXT;uyM0nXP=r)^Rw~ndcP;#RN7ifl|DR+996i0yC5D?$dOfYmzivGb~RDsn;52C zhY+dg^1}-!PcOX0hu!~L>C4}O4^msilMRzpooH*$9eFtRLUM7yI<{BLD*oJIh{=e{ zLj4@yv_7Z)nYo&di+5kr1@Tw2?Nc|z7<$TVRm8_XY|X}4&dF4}7g<8ZTH>Gc;{SXQdB+mXwqAg%tFpi3uj=4TP|wooMQ;wq=}X1JVrh2CIE&C8Y; zDhdlMt8SUQ>&Lj@JDJu%W%|ZoM|JOkM+c@|Ngw)b?TQkI-_I#Ci}2p=#=rdFL+s~y zhi@!_zuB;?>cwxLIF&-QxGw#Dz#J!vpbDy4NUlUHoqNzN&ONF7ZC?#OV|ym}q3w^s zKO|HY=$hd6)B`2=rw;m_O1)!yC-_cC!Bmb?e>kdLS4NoZQHQ6Lgu?HP=F*hlj)G@( zmX-+K5P0^&(mG$Ae~;~5{I>5Gso(oq#)tjt5{@=X?Y07^$!7`~lkt*z-@MZ0c!hOk z@E%IXA&}4TN?&VbZ{<+sfl8SzX)9?$G{@R}&X6_1NGaNBb~dJN@jc*sH^uUmLS<8B zJJn9NN?N6@Y-@6hbig)X?{yCP27=dwZj#(-4E1Bp5ncT~kUfkJio- zL!kiD=dX5Pg)`}(97~yqt0G!VGPwG*USJL|6aw!ILyNRtq1Kfm2J_^j3HZmyqaIHC zaHjGF{A18k+QhS9#5MP1RYs(xkI47Mx%KwNWW9Za>=`5LZO@5Br2o?N3t7A=uHP`{ zmVBEhptB}X>J3?(n6=rhc1kJ@`23VV6$n`ZDV$)7Q`qYar07yyoT7vFa0(|S@f7kG zdQ!+)LZ?$8nQ~Q;s;RO@QSPfGeYn3L_*&njF%c0kH|3A2{QN3?ao>y7pImhy7mz*%U0j8VCxg6&wA{SZfpF?5=J6wBICbh zcL4lns3!!gT&iYOGLZWfilj8d>NOTaMwS0^$x)fS)J}#OLI+WX$01TCPbcQdo0%2z z$K}$1G$M;CL#mJ=WDDDiLc!uPX~tHXoGqD+PV8cClkIiflDH+gr}!)E{mMHHcUtbZ z-Dkfmba&#e@z@BT*=mdB}GLl8IsvMoEq%SqzuO27NFT z%4GD2QYsWO87rUmu=F4x&(IMeU7SNb5tRb5WNET^sCc-TZY*A1+*;f#UYsdDUo0#B zK=uduBBlM9O?i^ykq!StP?GcQPpfFv7ljTiiIestN$_&9^Zq96KV%~t}O|v`FGo1yI?9HL<53)b>WjDrD1@+VL zCzf8p8~xqJ^fwbjf^wpwxTQXV25^7r)A^{gOH0_xqC` z@0Z3{YIpytl7K<2mq{ThoZ30&qCUDVX%5LWe!#Es-;e)M@)Y1#5bqTl_Uf^IAI6l> znBGTWeIbQ|RLoFe*k-todXE~XScAu-=ZHw=aTAfuLlesEEg;{n@apwCOi>=ar$Ddw zct-Kp1a0sctdPqwWw*)na+-W|LeS_J>vTMq~}81rF#AbciFg$)H_ zIk!9M!KXafV-2chAiW%P|`o_;l2sqaBo>#xy2h@ZsI<74_C;D2EKpD0Yshs~%jf^#}9HU0!qXvsRI zqcG5VBEp+9Sa1-1?}H=xz%B;Itr;w{hzpa5da5_+GZbeO58yN8)KiLlZbuCMeykuv z370;Wgr_J0sD*ap$%QnzgR#Az-$W%YUa;d{dOG>c1%8Lvc6=9aum>8G*%0~q#kA?u z3a3ftUu0>`goWp`JxtxjZ%uyf;!u6mASW{6mE*ssr%5(|tPw{a9FxnmNs1!cZbdR( znn+eB8T~B>urALrk*K~+U07~2J7g-mUujVnBDsw%#459* z5X&6kO`Z%)1o%GeEz2zc%gb%XsoSNPE~cj8)l0aRQYzVS>90|qA}3f666H&iH*9r?=tm`{ugsM*p^r2uKN5W4t-e2 zX>!6p{_)S#IaoZk@hO0wQqVD%D41Ds${MtVD9*xJDZWf7Yc1O*>$UV+w-pUrhOHx( z5vwwm*rMF6q%CD}TT@wY*=@`>m@{RJioRKSs*Ii^bGq!7|MBR_yq~vJJfGvJcs>U{ ziV)o(M2a7Xk`LlYLs422^2l+-<@D%uP5NOyrPVj;DY7H!7dS;#QvhoN zt^gI7o9@^l^nkTYKd0AI#g{ zQ(-XJX~~A{k^@(4{^2+N^m6vp-2p$oWo@-P5GeI;%(l&_y!frZ9sT-ux|UnQMlaDZ z-XA~9+QgG~%>a@23cb8v@KZ>Tssz17a60vRsnRaEh1#@Rs7~)ETgA2RM)y95Fg@;< z+~WX@?`Q=h6flwk#`lbr*4Sv=M?N&xV|>Xt4vN`3v>+dvdRw;#&M%EbOlTAs9L;{p1WTY53 z;<1kn1$`lZJeiz@h2#}@1-sI@BGi-|kPff|p@E{_2KS~`) zzEb#B;h7}AOS+rg9ilM{xRRT^a0Pg~3x(o#l$*wIS_U%+BEdxC)ikVIISjPTlU zFa%eiSeO$bHYp3S!GJ~L;xGc8+exN9%qEW@lRv|YYn$}yH2Dal5Nr~^$T%EigQL`a zM-xO;BU=h1wy`RkCI88X$u-G&J6mQqlb^d}@?sgDT$~h{B85dR?K31Fl@hm9MMRD) zA*;!O72^<*xrk@UeU^vK1!Q^fVO3^g&k&KvL_rG#vd@1$D)0qjSino4#_!r}9gC{6 zCmexA(Xms6wPtUdnV44)pz557#_8CO6;*|0WgvFNm$i+JWuKYgsknx!=qT|j{QhXv zcU86lf9sB@4BJJmK#FmC%ozWZ8;Ff=BUf z&zj9hZ6Q0gY*_nTZjm6ZAH|)59)*SQP?#3NtzjZ)F=0&hdby#Ppm<3^DXgK(PMA8E zJ7Q`qx6iOTzljU@v1l~SkElo^i|js`P8Hz%j(|7dQoGlnLM>SsD}xxXbgsd69e5tO zf9g)IiGz$<%w&GFeBx_JIp#|NZ^9EAX8u5h?iFCqW8b09Uq5%`)+hHq{n>wIw_krXb>fDFZNJ@i;NhQcT^1#0 zic?K=ujCH22>nsG-MD0zc2(I4%}chUvz}Y@rtLekeq;ODELpi^nx;ZqX`2>EmzT{- zEs_-!avrX#pr&AEL2c9?t)Fh6Ucb=3uzt0Db^UhBfMcL``IK?wNC+CMEjH^0+ou_IP zKPPzgrH$9I2;HZC%UJ4qD|lboTh-P4{N(&M=bxU>%-^**ci1^u3OhD-VGIc5g$uyE z@TCJf#Bd^-Kl3|6bdXESgYriYi4qQ*JJllhLEPYF3x~?eNq(NJ=8n4vPS#_>|IJuD z#XeU&EiI4M&GfiqGjCduDs9-_5OX@`&InJZjJ~AZ?=;3$l4;Soew)LS42SKlGp3a_ zTw556IoZ%;m2v)S2uW)6AeVS~RyflBwMi36&Qbq{*4eLj}0N8c&Sk!K3aa zEAvy(=f=ni<*}ChRC)CRQ9M0&X)#d#+hy1|c@9Ww3U~5GR5K zyLVBmIt8UtrHq=w(-&0Cy!vLzhH$)NnpdsSPLC$*2bXqyBM_*(a)v{r;ig53=kzb_ z`UV;wpXdclqxlFik0N+yqHa?NQ$=(bMR1GQSJ_wS#p2qa1iDq2{*e8l_>+mz#A}HU z6Vhumo6UPQyUdKm>IfptXx*|RmF0Mm;8W(73lDw|?h8*{~mVsuRGzym=X{LW?T zh>mKl$>OP9N90qWB`y7A+w!@y@J~8wYLE4`{Hs+(l7Ept&{?fesud=asij7W0UX6Q=N zC%b4b^#Z_;8rjf8&y8ANv;Iv*t3XX7Ut$o$Sh5t8dCa18DH)8CQF{eHj%=}WlG5`F8^1tpZ3sET+bW5{IJl$+AAXr--; z_F~_5tF^kavUq8G_Ln$xO;o6yRvf$|`!4xZ`JD0Br0wwDG}3|gSBz?Kz2F5Ul%}4P zKBjt-d(!_{^vUE2>2cKw?u7q%^h8qi6&VfMA&5u?7!_a)u;nr$dPbRjmO?*EF^Zdy zGcaX&10e8S`vY4zU z5ZL{N{#0>UA*%7sD8!iv#HNKIX3!Z6kw@Yx#5N_w9CC=I6D5VXIsjgItg;YI4}n*p z)fZx33Z6-AFGNlmc$0IvQ>O2VCS&4v%ZTM?QZAkN^p7z&8}%* z>AmP{rhj#D>F&Fq*wT1YW7Dm(QjI2^&*(~eN{V^<$=SCrcxrI&p2o&Kvy?y^ zl1aB@!#`RikDmGRH(y--Ojp<8jzaJF-i@H~xFbyAty5AZ>@gP-mbAmY``5n7fmX`fm-Xwofu~b>CQmHBRW9px% z$2EQ0{aj4LPbWABp}h=8J8M{V?8<+)!Lsd|SyCsc`9&WzFTQDhw6ZWL9N< zmHEds^|aGfFHEnVzG(WdW~`{bN*wJ*v=V4Z66h$|MV>%4Kw5sNwE$Hi!#tZ_iO9)v z zT#jz0{*sR?B+KY+`M47G%YKxPtI%4xkdLdS&&nUm$2I6mZsjB#@8pi>;}~gm>3p0* ztgbE}r%^`NkdHG^=B9jHf>gRY^KmKC>GtR2Eb7*c?)MP~7r6XbEOWudA#&F)kq^Vl5QoQZYRy##u2vBgSQ7 ze6ARmlQFV3=i@L&w&nRajFD|~J`Q7KyD=Y!F|z$NABQor8}e}&BYPwthcU7*$j4!f z9R7S9#>ml-kHZ)_ew>fP7agMx8QLPv!UmejGT_MH`#4_z-+$h%5BgRc)`c5%!74N-OjN8RBxwaLG zWu6q{E-`*gjQhm$-xlLVV*E`p9uwogCUIG={U3?(+_U~h;wmvcLW^c9Q#f6cC@c0Q^RH_F8dV zNejt+olxt5*!K1L+Tvf(rW_3Qz;j8PdZFz0e_5(igk4f@tJtb_lh5tSV@6Db#X+$R z-H@_9-w*xbTaj9$kg`lHKPaa2phJ=VBR#oEd?x~FF3ezJ9! z@RGEnU+g_zd>4}2_PKtXsD0vkVzj1?cy1i$i|y^mkI^QvCkLR;HnHA*@lDp|-(joR z)^_koy@O&(n^;$eSSP`-O=3I92oc;QW!C4*3;+hK5yz?zV!3|k7W-}uT(ygE*D1DZ zQ$Acf1tPYJZRm!&NnIO7{20vF+Aj9Y0Q}a9SjJBo({{0E+NRKqT-zpkBsXg7#C~cM z@9ns3><7eW=OBw0$EQP#Z5Dg2Q|y<24-3J34|R!kv`@i^HDc+14<|Vc_~-GP+QYd% ze7ys7WX~2inoMllwr$&XI_AXo#F|Vzu`{vljx({%iEZ;`{`cN@@AuZXUazjMs;$~} z&e?nQ>eFZcR+1c!iiX)mB{-wiV<*+}6&n=#+Z48~>`+clM|O;>Vx6r!w>!}~(}VLF zNblBJBhd=0Uv4JPbMW&J5}6;oUPtcwJ`jhT9k|p&GV%2vq#(V63$X0+Qu z4Wks%3ze6d@AbroM^1|dek4q~=MfDYsN7GGg~=~kaCHcgT%xo+**c_m#7B2=b=bb7 zZ)1=@43U5kCda6Sm`X<)@_+a~#*8Uv7M1cKI-d+T1ZC1bJoN8pwB7ckc%byq<{a9g ztI;OO9XiaT)R~h<6dS0*mGFu$YdBHxoZ}j406rN@QHt@!`^4Y7y7Lw9a`x$}B0(>MFf{$T^nU!XjkK?zyBka3V1P<^Tdu52OMMUN{t4-OB;~hEmxH zx6!FUuLP5d;n*?tji&+h$Ls8otAv2Cr~rRwEkY@;!&XieL?HREhDhIhwL&?->(`kV zxf_1gA+@HDS?%4yBj+?`DVWuM%)2Oo)#PH*j|`TBn;LGBp+yBR#ykQ76QzSkrynm5 zO4DfhiN9WoyU!?EcQ+r@o25D01f^z~=DB*9pu`{B$uZ=oI^-yPa=$7DZP4Fymj=A; zzU;1WV6<`Vmzq_5xM;&5R@oGaHLYrPc3@6D z*aL(Np^+`zw{{u61kA6CL4bgami{P}&ebt09b7(~?X<%Bk=i z#`8Z8IAVY?w4%pfAR(6ztIowSrG>1S{H7Tn8Wjnr#=4*tv!0@kAZz^CylVS6$aErQ zx@z6X*bSF!dT_6@v%T2O*-fdq*u}d2dW;i7-qlnLY%T`c)s$PJZLY2p9-OQ|uD&CM zq876HJ1IR_B(Qc*u7f{dSh5addr~-c6}4vo<{Itawm^U-F1hsqa+X60RWVs$ue}qM zuldCLPwRM&I32buwcCf?S z;^T#CTd+GmjktMT6Y&=)g#T*DHnrT0voh701(;plsIN`@G$C_LMo`;W3~V?3wz&j` zfW5S~vBAS!9uzJ}!SkchP+>}_w-k&6IeHo0s`vjdx26e#pRl(Xui{C4Bp zmv_MX=ze%Ueip23bpz5`Us?}rc5!scfTmMex66QRH_B{xUWZ3+&AK=`--3c{?we|= zb~tee_Q`ANsk#UCAKzblY;bG1OTdNld34(q;B9X7aQ8^WI)}tIn4MqeXdxi%Xl=%W zwL8PrpR7JP0{tD*bo+G;vC?p3ei{a>)r%O4%GTSP6FvfAWvQv^EWJu03yi1a@B$nj zJ0v_J)onL0D#R$#qTZXIaF>Awo$Y{OD2Tm>W&n>drBl0lsN!&B+6kPq8h$a zqT9h|(!$x&XA(0WeOLJB9N12SN`DP!7ap%I8oGnt;^wjpFZ~2ppWVr-QH}Qke3J-L z-=V<*w&-xvm+k8;15)sPVM0i22G`MF?jjCI_Z25&pfotk!!Jh2kT2~Z;<+|f7KD7o zAdX!s-zX9dSiIQ!S05Sd)-xm$W;mIrc)0J*xq;}+sLbXs{d(Pedo&~<#&|+gtIHb@ z#MhG!6JkuL*Z``H)rIM4iA(G$AYd0pC1H)}zEWCFaIxgPD#pM|8fz4#10Q>A+U$#$ ziOHlU@l?j5s|P{_)ns6s^Li(WFzO1D*nE4|WT16dIAcKE5|PWIp*K%ZsPHLd{}9Mb zgU7wY2v8)TYu)5e;GIAQsLl&tvn!|=3G?M-wbDqBwELIreW&=O3|}cLOnxKen%5cZ zL@e9UX74jBEjmuG>oE6R2U}h$+Mt651kHpma#)KXYA_l}Kf|A{w_)pG$Z1lT?7sDz zIXPL=Jnoe`cXiNb$ym-s1q8Z%VB!pGkZ$s$Ac^Rlqs+_aV;HrOKdb;}lg~fT4gK=W(0-4;_J3*h;bp3i-X*_as;buuxujYAo=9VXo4P#addZd| z5ID6JpfM_nVhMdV-1+9sA4HTFAA!Z8Tn5uT33@$438%5y&cIy=A^P&|vekk}emXu#$!W+}HgYf&q z(PG}O-KNFZ5;qFvKM#ioafGdhJGZsI6SW1n1~6qfxeij;SzkWbTiCWHf=!i?-nds_ z?1@^$wy+B!HX#^6tKti%lxCI0_>dBQoo-Qe-+fz!`XM5BSWN5(c6CH`SRM~`-Wh}c z0`;fzHp}GE4cZRbA7Lg7S@6pu`C1OpA6YcvLs;<5zlBr86Cl`uHFJi%0k%lq!57dO zx{tv*8UqLIQ9td$o@(uoP<8m(UdXde8s8% z3}eV@NTt4U7sT}l@$i{o%7riYz?~Sft}27t#h^<5dl%w}K9qS?Fzcd7HTe;!=LX2~ z2#6rm4=5=yAd)aJ@fPJ$DUgq$XcPRMxdm)DY6JESJ52K|uUZh*88ohvFC9g;ET?!; zR15BQ<+B7(f?rD2F6@^GWSi@}pGjxvE_|^g8sv>PC6&xrqNNBNxNQUma6d#LwqClE zgUZ2%rp}~%VHRb)Nmi)Ef(9ECS7a{C++|bE2o`|iDY=w5U*hIwS=1pQdaLF&S~@f~ z=!g<#Gc!}D_JTTO7?1KiW?rmTB&ZH;+TslrqKhBvl4e8x6dq^v zM)>C_RAH?#b3;CRaunX&YHVrtSyhBpwZP7xJVIA-I~IT9lZ(RWTjynBj#!guz?JGs zOKc@(QaV;{dEd0o3};6ANI%_>L+K+sbHvGVB4vVp@J9w_Kxx^usP-k}%4QWYio)?+ zdO0d=S+ShKbAQ9Vq`FRcdewAPW%=mPwjfm^eo(~2qRv!Km{&E-R|4-$S% z!&ZAYMYl%q2`E(_Vr1#&w>bylV=iT~miaAm$3-saNYKsXNwTAt0e)1nr#bT{_KsIJ zDO$A{j+?;KD#&Icd;~Wku&2q0UoN#`CeP51mi@PbzE5B#yfk}XxSWVcg6Jfin^C76 zDas(5arK!L?&&*an4~TOgI9Vy0#6`3pz+13LpGhgdF~J%1;hlzq#)dCr$gl_jflLn zzL)=@#^?^h6(V_lh9n?e?sHD8NfouUO*x;0mBjFc%W5 z9%@j4jta)_UswhibQ)i_jQpLT)Ehe}0Kc?t_U_9&dUORsfOEgcf`(~7)}?-!{)dv_ zD_eGScGx9<$O*A8yoRw*2tY{HO@QWudhG*5KLEkYtnihrM5e<*D4VV*fFod2Oe)`Y zs}zYa(muRc@LT@2+JykB09>gDVdQz>7M)c7I`S#sHWfn0v$RHhxn%ev36aD|6Pb{N zF00T-Kaa8tS4h1J*CJ75Li_P=FiEE~6bBY?k(m*!8Z3^;SQmn;C?=hV4igN-T?{-s zDSwtZqsk>uj_ZiL(})M}5{c)EkBSoqjmbnTsepkMGNg9@hc6wQtSzrh{13lptB7)l z*WLQ>EL_oFV%z<9{Wk8(9MGBgws|oKl0^pPrd42xTnNGx2?OiU1#lL2<_iHgR%yTmas?b zLo0$A;*QKS<;m2B(OJ{vX-&i3`hJ(X&((wxJ(;m|gl|VBlc8S7!?D_xHmRRj9YSUtlts>iX^yJl!#T5@WfEH~e8I<~WN}icA<@Lut;9%?{y7$p)wVHS3 zgzpo%dE;5pv68IWUdI6kN4M`LoX=l^^$~@4s;$UR0A`g}%75>y0`KT@;VOJ}1xVbU zW!CI?0>OhJd(44365wGz_g?Vx9HGd<+Kz^Q*d`6x;pI|Ala0#)D zK>osVT><&ednjb6-;#qqJrXZn6aQyk+IXYZes4jND(K^sjR)5xyHh8d_-^Wclkc#9 zf=G8$(kE7Nvd4Y|F8*-Te7}t}rV-WfjBp)ij``CJd-EH)KWWZqp^FJVG>N!hUFG)I z0Y*kfaV_H+WLrq~O%c|?-x|(8L38S>v|CLIZ3@f$#jPrBF}Xg!UVhA1A-ANIZB*oY zm$_@6m9GHYN@VN2T-eA@0M{Kyd73`n{Com0{4WCG{Hh5~dr>>0^Zt`L{I?e0DEXlS z?%cN7xWu48cAXt>K%T@dzkxco4oPrI*=WsqD)T7_>dOT?fE+-id}+qc1owk_%?WxT zX!!sg%EjveqOBEd{YXIPl^i-|FmT(A97hp#rFR{kHft+1LGNv9Y<0XE(bontV`*OW>zj@7G`En111?)?^fi^ z?JZrch&ebonLZU<%|AEt)!5Zs+}zZ`%=}a7--pLD(;#X5M+P%`=mS5*=#dF{7pJFW z6zQHNsrfV!$1D`_R-rTkaWDP#{ax#j-7iLqTjTRiUqMi;_MLa`Te&Mc!c{fxYdIlT zmzG!}7rbkimU}S^@{4D88LekvCx@D5|HkZ2SIDYU%lC2(P2bpu^U>GeIh^aBh&grJ zCu^atL{(3-vFkL3If6w;S}mc*!=qn2C)yn&T?DyiQ}!`o&%6cQuiSzB*`}kwpobxJ z=V@E9WcSlvKbsY1u09U4@;d#4@;>9N(7cz6?1!&BfR&tad^rVrhS4%zDNJ*R9?jMT zw;?xBtr68ATk~cpp9Q>2xF>|y5yPzdKhKCA<`;CHL%2{I_4^?HWGByo@LXyM)3%39 zo7tL%3kyX_gyrgoa44RmFd=Fsd@@fBb5i7H+_QXp$oTJ1vxAMCl zGQ@wYWPO9Mw=d{~Olx7l@o~Myaw`aUO}n}(sz%RYnMe~F65H?}bK%FmWu9RJEfx_9 z0Upe=D;~}OH+dR}&i`*w#m)NPOYZ-T9ws$66W70?r0(oy{ts2u*v0(swfp}UMAptO zu3}ck&VOS=-uT~RE-qLmEo(DZD;GU3E-qp&R#sv*ZXRNGb~a)*Ha22b4rXHRf6qCX znTh{4)=#;Ac%S`$XrJ=1f9Zea|MLFQIXOB1vG*Aw|LQ+!{|E1X^grcc|H}Qf@gJN2 z+yC|JKfJ$VPL_Z9^0)sx|40Ad{{3U;(~p1l^6AUpHUHE8qxW}zf9?HS_ph(8JfF0` z^Z(_2CYFCK^dG^$PW=`5r!jLAbAK8)_*dTeZ%!j-`6ubA!ZN8lXxLl-%?O{QzsckO zlTXqI5~MZ|2M;&b;5Y!4>U6C&I`P()K6Nm&kB?m z;>1XfS~b!)(#si|j~ab9NB&74L%&?-zWke>(}$Uy8(EX82iaYkUT*L=y$DQ593Cnk z9rp zL{hT}sgl?H30K>fQjW!R6x}YOXX>MnpougTj$J^AZ;Q*&)IiXmeL9j4%w6Ez`s%{5 zcjH+sbR~9>@7s@s=$EKU*lxfT`)+9V0>RPT>^aTv55&BDpJ#8V@NBU^Iw6c??67`d zM_uU$iV`FN$RwjX%3*)R-r+&Yq_d4NWRrCZ=8q606VvLo?cBp>_wLO++5G$sp>scS zK4^eJo_jX9tjpCTOrrI`a7Wl)fQ9CSf5qAlQMOeJr8-U{O>Nj~0JuR+?;+D6=PkXB zq>zQSdic$hWMQ}ct_VD%R5^3FzI3D;HD2fx?OX)D6~!|dXTDeo$Q#CD0i7dwr364q zqydsg1>u>NHzH-cE3!%$*llpAV)cfMse-8+lQP-$VQp~`3F9!*+uAQ#v2D|) z+))y~pbc-l_0g@F%YB6A0mZ*HJJ|F%U+`7)$hQn52?M>}(BHmv!e5sSAv`XQ?VYw< zuPn=b+aNy<(b11Nml~S#(T;3f`gW$>EzyE*;SQ7C547}^k68X~Z;V9akV%mC#tQqb zioJlnVJ)mmbTOA|%IOyBmWw@!^IOQ%nkzha(;2qcj)hE*aI zHS9L>L~ko|`^}YP2i!$p^VZ8H+~dO|`g^L#_du9ls=*{?x-_^ny0qrhSRDd96WgWe z<<;e!dLmlMq|i$*^{19=gLxV^@tc_;s3Jgy2%7<0^EiGg1Ehg^;Qn&=zN!_>;hJom zGv9IeB*;pnJ7}ys*bopg-32-(uXrkYt24ImK;xEAmg%VjCV@MqXd2!^I9s+NFjm}0 zP`U`S;TJ@)NYl&#sxL(s;U;Jf*ki^c;nv0E<>VF9<;f$$E8!Rn3rta%4kVk68KY79 zpZYXw(<&o-qaWP|#becJT4_SeM0NPK#$KG!m8Q*g0F#H>f>V_Hl+$W-6^R&=R+IMa z@Ti=gu@pKjYA(9fBrfU;6@R%Wzr6rzB^pbO=8|&_g_8IDg;Ke>nB%+SlX;}_9Mum+ zf%4%mel{S-E|@K?dY2GU9N*O_`cKq}XFZN8AHUA*I3Jaz6G|*H(^sr19Z!F&bmhRH<};8(M-N zzGB|+7FZyH4sHtLW=wl61;4A+f_8$v3RSH^@mE?J_QkR?G1G}80z@8Il=AqdMUn=8 z2^;Wv=(I78QVV8C=Hn=vip&hgzF=doBmGbr0VmoYV*`K37RB{$rz1U~r>gD09U zEXQbkfIF1(JIEuc(|zwUU$Vjo_cM)Q48ft!rJ?=Sww>(R7`hQ5D4#C|!w^^QRhm0H z%t;iUlp~4NY?5=aGw>Q^4`t6ISx5yVH@e3L=^OgTH^sg(k}b}y0l+j=?ulHtw{wrb z&9|t%nlLvug;TZ?Ov_xs%fRKW<`fVl&OmagycD|W7$c>JKHO`yTgLs%(8Kmaj2e8b zB`>`1)q(z+c{jqx!H1xiKeYZhQCmy-qk{?hMJTj<#|gTM$mv9QJtd#|L?x!W#IH}P z((iy~f;ei-LyBPNgQh4!A(FhSml8o+$o+LHcNj|zUw(pGGyRfl0CyM$^Q7E*jWCwG zmc~(UE|2vB@7`-;^`%l5iSET@4M{!Km~i%Y;9~xEnt2KS!cu36iWiVNYF;Cn4of2` zB^k3g3Ks4<(JOMzlb(u?4i_do2sH5Fg|!ohuzB>TX0-xy;wvleEpMe{r7Ep9WWidS zvR^bat{sksxY%e4m9*d-EJP(7tga*kb(s*JrMgv~Rqt{tsgb+y7qukj0WbM6nB)@H zc)GdJkm($H1Q;~W*|r#v059tj8SJclN3hTbwIygrVi1H7UXKG+WAtWdw}Lx9zK8in z`p|R46Su~^l?KN6XvC9SkYi9>x6o%qQ!R`lke#w}?lIyRlnC5$XogC>yO5u#u#(Ht z2DUF&J8dy;a-D&~YNQ_Mi%p?T-}9#uR9rIib9H&gO3K{T6~a;4et(@p=>4-c?8l|( zXeB4NHbrRT(CN5Cy}BYkSkrI3_N$<3y?D*CZmPA>ea8*Xw)3=$*CR!^?&aQq>N}x+ zvy<`XZ59QEDX0c>(mRHiW8*`-z{v6m3?uga^rD!8M_g=GVm6Zcg#8r6`S^sNcD*H> z4dipJ4tj7Al%|{1yq4t5KjTRVGXx@8k3;-UxWc{^%x@^Goy?5&xi?&7#yiDQdrnlz z>=N$NqnaXRjIkOqGlI|DxS{7Cg6q0X@LRIvQ8#=+H&M4j`<4(xlB$LiIb&`G(8I~? z0icCx7Mu~sSE<<9jS5x;B_-Y$j6h?d-gkxzgNdp+WEsrRrj(Gb`9ypZSK8M=I6l~# z_Vi`PB%{r*f=K%kqrncy{JqZ8vqo^d11|l|uLQ+&bwxcM{2uCycsG}PgcY2b(e3&)Qe>G6%$^rw9WlBr#?TlpRAdGUjfG@jzhs(KT2z7GjE z-71mh87Z`)n?aooHDT59B|Tqy1_sa2zSN@`B|~W&(Dp)1vM+x?;7&L>Mcfr zIZ{zS{+kQZvRpURy}3=pE_85?S}B!wjFp(|fD8RhDaNGn?sQ4gB+Kb+3#7Gi?aH?` zr}MXwqY;j^qmi&9S#<>F2NrRNCs4U=)Iex5h-YmhDk03|Lo#w|%3*8F3DGF1t<#^; zU?#!3dGmX9B5$&ts=w5a^5v6aX!F}j1Z}iO7fu$A`CR$j7LVoIUG5JAsyy`{g0>;n zbI=23I;(vF#cVKwKFqslzCVnZ+Zee#f`&1h&7blumSC&cu>C9UtmAOY7g@$)H3S<0 zm!ZKVF}eii@U}5yy7+#aL1Z`uUXHwdlJ${XEJd_AI1`reW!Eo$*&*xk83Yz}PE9>l z@T&@4&Z@{F%kj)RQ{>m#Vdiuoa!I({C{pwD$!crpT3&54CJaiNiqr5#V}FX$TaWff zWQ{a#UdH1-&>pCw(1+IM^dfw!pU|xN4dY@njQPGZNQ6>Q{F3;o%#I7IbE;^m5>KYA zudc1Ds;l}{CVplcq;xzgEH%GE0n8R5FYfHmnNHQpb^mRCf4*aw%mui%lhY4tN2$He zsI((M(&;*j-}?h&n)R8yxdbVq+SGcFq3l+gTorJ|HtcB#*+g{2zAzlEzdU~yxVT-$ zskeolJgmn}ms08+>U9F{{~|qwz0Uv+-P-57ul8ui&0cOnB_UTv?j{|oLE0N8pFE#G zt0-(unMBG^Y~DbcVal8u(-F@yxWgxh?m#r4(Vwc!An*us@GY1j=p+czBoLn5n}M zfovghfkp4MAiG1BDKqP4($<3d7((EAQCVyFEDN%Nt*z82FYrS9f}eSh*+=ss=wuKwn^EpUwrCAZYN0rA8eloo zo|x|N=(}K;KZ?7;t zA+DA?f0q%%FqR72)HR^c9WC#-(kzp`y+P(#D}AMvFB_^nSDyzjM<#G@w{j^w?V4Fd zfCMBM515HWWf`$afJt5jttK9~l}vVIvQO)k%g`Qw(>3NX>{>HmVet`lMfWa?!Dz{! z63Df<1gRdBsVRohq}bnOM9D)D-#^|({c70DWc-sJ$+0l5AQbBe!X{4E^UVQWO~9KDI>NTs{-U<%sx?l}vjHlp3@uIeDu&Xp^3Z(*f!A4^ZGY_R zJ)mChvV%?&B+i;M%k`7cCQPsUZ714El$9O*{WoA^xs^T4{d;<JWCf9d3 zo`~Mod$F*x+UpifulOPXmDvKnsEvrkMSO2q&>{B)Ysy+!d_Nz4Krj>=tLe?9=2}aB za@-d$+9tSL#igV^jwuC+9FsNMNzPQbYQvo@V?n$bQT~m;ZF>M3rkb1q9S51HBbAMWW_31G7)j4)kpEg?_N zf2Ru8(?&;B_ZIKDKGq0_DIz(TT@ zvnHL@YS4PL!?C@xP5?Wq4jO2O;#WeP_(J#Nc=XO4OskR#4H8HA`M`-OCo0ibB&yJ7 z=+Pwo?&+5ao0J;M=()U;lW&H9K<2d1_n%42P{rN&8$(a6_!#co28`$(DGzE77v6yn zDi1`kH#{H_U2#=Y^v`tV<0cl?jb7ym8YL<}MlNR4GUCr=I&f$%_o?p7l}eP#bnd|Y zRp$!u>!YEn3E?0j^))e|N>9~j(!C+lquk5cIB@#?M7{^<$HBaFHc4J1CFBaj7b|h~ zmHEhCUlT`q;=d2fQzB{_k|NZ&>68#-YJ=ia7>(C`gzurz<9uEIQ`eq3!z=s~w8mN+ z+JDa1R6)m1!SD*VapG|Fw-dCso%GM~609MPHY>rGEhb=FYgL>3o@(d_PKEtveff&a z8}B7YZJn+R6;56@wj>m7jm3#LjQEx|wiw-&E@|&|X8|z*PFAMrI#df*jZTGs*n`66 z+36stdhaG+GX47T8cUPWmW1r5Y_mfAye}IRr_tnj+L3o-MHk?0#Ik@i5~|a7G?F;O zMsAKSmsz=_)VtD@hQCqByNhY)1=wTui=}CC;fv;A5M8o`m$k^-NlqaOcqBOat~>)e zKns;=S`VyED2g+bjL)UsAf6h@@;A31B-R*XqgBjxy@W<;_}-QruAXa0K~dsW`3a)| zff=PFcRL_0HjYid7>_G^Nc2QPj+5wx!h`Lyv*+#0Y%AAPG4%yloK0TBj-zl<; zq^L2pLpFWtUe;D_vY@KCJqPLo27Yi|bRL*&=+3zkgIBa5(w8VKN9rGz701H_;9-#= zbbVMk*0*6{0&&S0RCWS-5r-l#q%2-RorNP4jl~mnf|GC?L1pkWj2cNTSvk^u#>2tEeQG|2(&E1^S(9uCZxBL|b>W zEwu3T!5Xvowi^>>MO3r!@Kh*u6@B)1@Hw&q*O3D&Gy4o)oaGlH3;}K9=K3Gy*cGU^ zPu?n%$50K{iP^?fH8Qs}cTxyAh84=h^$A+6BdOy@*ntC5>`Uc#;&$4=)I6aT4>h#p%N^p*BVo)Wl=TD@J^xbwutmKXR8)zpOG1 zpjCHT{BY*Lcz2V^V5~iK2l#tNnt1QWHyv3nQux_T3^xt*tX>L+LmOl| z1LkjzvY?Rh1Et1gUgNLn_kxf8VKm~4jL>t8Dcc}e#-yJQJhRcZj~kn+mz8j_kYovl0y$UGsJ5>=`mvgD(x!tzX$g` zAq;zBu#^1C1i}RLb1bWmDf<-G3@4+<;5zJM+a{MRKkN0#bS}0m`X=fHXQ;RYpq{PAVL%2->L$ z)pD|#$giX5k-_fmNUv~yCjOeBE(ASXOf-!rtAD1dSK&CLB9Y%=2vM$xIDyh>RPYJs z&e~<=6jf*<=y^LBga#q+AE-U0J*AaPT{H4RQ-I-q@B}$MWtGaF$xMxq{mSt+BiM4^ z8#5FK+I9tAhET}YEIiAC1whcZCq2MNri-VQV2D$OGn)&7C^14G@86S`p(3R=IbxMr zl!*f(kGs7H#Cy(RjYsXaxzst%A*s-l#fR3jk?BAW=R?$@wlcy-&yT8-pq@EhTwO$- z4{N|LKX^X-ri4A$-S=15Zeq8M`?tR+eq{B4{ee0~f+B(;Y7`=9VstZV6AOAzC!IsY z%I^Mx2ohbgMG=rAv8T|qvIT!dqUHZ~yLs3HA5fZWH4s*b+o1GSC2Zti{3O>ACs6-gg6Ff!tSpA6fUAMTWResE0a5Hax%OMI* z^mXUH|XKf{A zQoYdAZIjP#nayqq8cXcS*hr#tNQsWetGHn5#+!*d!cx2@%S&_S_I{7(E%_c&u>y}5VV?^{1M z1|QLAO?7_v>%Bo(6FMhkYwW_|JQJ zj=A_Kjcniyf7EF%QcWNQTES-IJcM0QSK!da3g^J@I5q zct60fn+|0Gw7*Bck-J0>9dp|befEXYs@(d{<35p6puC)Qk44HP9?wRIW@=y5PCal6?Y{;!_@Jz|D6ZX#|EgbcHLw4o`Q9=SE&+iiLdANx_N$o6y zXf28jd0-3V=!FR%5$#@+piDXOA{NlpFKmv3;2Vr#{7{)6HM+^uEi1>dx_8$$pEbW} zd;>8)@gC7~i$OYhDE5MGFEgMfLZFb6j1~Ww+1S~*lyNmuxlw;sn;geA?6VTLA{OcS zo_X3DEXFyJ?ZUU5OKPjtG~(ngNo}cBM+b~Rp2GhS{=LgTLa@@xLM8X`b<6Bmmrx6AhkE#tl`>AxwN-OVtw?( z+;W5b7ub!AnW0x@PXkwg0!S=19Ojk_89unT8(wfA0 zE0;GGgb$zgxi!mZ+V2tZc^+Z=h`TvYafZWUnIw37)rP>4G;w9YMCfNL)SmZ437u|3 zfokW3NqpgPZbV*7nt05tdID+{F%BK~3HP!|j?>CjPaVbk?KG>rA)6SVdaZ+WyJ(C2 zX#SrSwAvmU+FGmr>pf|DzYVms34TlO_=I6X$evi-o@M7!;%mZ8oSmvXbw#JW38+=B z{m@n?eMTOfhqtxF#;nC=^S(zOQQj7lui8H1)dp2mnl>fAds(Ep*-mkq?4x&tW}9bg z^tq}ZIU_n(KPU6|;}9(~zC5`b!;J?F8kHwwxxK5#EQism2snTZE|fdUlv0iL{k+&C z>k&_#gcNRovhDRXr>0&{kJ91B-mj-j)yjsBjCQn61%TIJ!!f|z+~vjbrc%R{)b;{& zmhvt=4qah6%Cs~XM<(i4T}LGxy>40G`2(0ke-h~nw)r}@nzW848?$D3v6=ob_Y|%a z5D&k&=Re^&HY)D^yW-m(tW9O~X+wMXZ zKJKBP9vIhvLc9S*t0h%5-IIsp!(kP5CjI5}t>iCSp>!Mu<^88x*|S;8$J(A$b&TiQ z#jO-ZKO-!0E>W#+BHdbl@rLUlNxE)MnPFQn9cgH*xl}72E0T5UP`#fy`V%Ga#$qGH zj314Lt(vWx^^)zHLaq8bgB2Q;9KI#0-xhk4(o1mYsca)+S-mq|)XOO?K5X&f1B;9Z z9jm-O1RJq`IZIn_RHR&6v}Px+ipXAv-|bmuvhz@K^mNad7o6N66h)=ApOTp8)Z`Iy z=r>x-o(u0V#8d`HR+QlYS{H3%U$*hkFtx_zEi`+-DtXs~4V#qQsI{9Ab(r~`JtJh` zW>f*~_M^_xm*QDRSEC%dzFOzDc}u2*#n7JGi$WZ1WjsU}Lji3h7d0#M7Z%1H>l#sV z3}M|-EYbUmKlYsbS~Ww_T)DbFf0PMK-kKz%z2_RMUIv)gAZoC9dfzWTx-pq@WGI6S zc%fl!ZHJFdrgJQ1SR>t73DB zNtEbTdghS|Yj;th9KH>9&h1Ok?n5XK(1Fq)kb3iCvHdXKe~}Tn$rUhj(I;v8Z4KC43w<|f zkku|I7B5s;Kgj^}zJtJ8`QGW0{-TX*n?_t8XGNbJ+q8Nb) zzXD#LwcvtQE$3HQ_ZXbzM^6`|xAr&MG>Lq$3G6-CKsqLLx|AY&>MjYt+|*x6(t27L zL<~bI!mGpS9f0^%4{lZT$gSLc_K`T_c4HTv3OR0I(P_Fc)b7%X8Oq{fHI zcQ9SHY3egP9NZt;F5HQng-$eq=l-K_wHa>h5@;#TY#r&-87@z2X7+AXq#o9al{~#t zslNdzvNq+7J`IyL7+=W0-;!q^@>6M^Rhsmd{iH2ACV_JI96qhXt1q|gT*8z3g-=fq zDtjH#r1eylFsv0dB55A2ns-aj$Tukv2v-%Klgc4~mvbH)P@P8Ry}~c-dh@2{C|yN4 zX*!Iie@#q?8Fe&UY*g!*9L(300L@8zdqw|1hM)z*8*Oxl9fvq$j*&(}Fho0s0$@j; zmKrUf1P;W)2Q9<~=fO73#kR;75S;`eC(~d)P4Y^Q+Cmc^I!J=;!hBKL^yDW6m`g>O zYsoc@-(am-y6UxI7E#$m%l6XJnrZSs{~EMIYgwYOSnOwIEuh2H$l-CxEkO}<69 zXf0H=VWcqWX{n}TT?YXf#3SC?>d?&dff0Y0ST>CbO_%BWD(MSmPqsiRZgr7(W4UoG zDf?P#6sw0byBVY^Wfn`KofNjVu$ycS6A4UZ*?PiaQ*G*LsJXw`NRo$H*{`=RyDJ-8LuN2zp0a+MUIOJ*NwH zUi28{H~BQm@94COAoh;C+;WWh*b^tLK0Ng>bZiES#k!TM=A@kuVD{vj^yzrx6|Kv+ zTJqZROd#OC$ZWgF0r>ST@yeWrt(+1xw_$av{?k^9PPcC+@YD^OeE;U$bcV&CYs(mr*lhR15CYLII{I5l2S88qQMY{x4&|F(}s4) z{u+FAU=yOo(ua9{C>+T0S)>?Wh|c5tj)W77ljBHRv7m}@?czT;K-GZz(An_;MN*H8Pu@Bp z6m+2xfAR{KJ=I^$iOud!*o2)w#DN;IGCzeuilm970N={Pwe#aot0k932PRvA5gU%o zktQq6<{8zIi%l@id@6su*>eI^qI^Q^c8PROQbLOLm`ig46*t7eFkME#yl`7J1*s2U zL_y|3U9EW7TX0~SDtJ7n<<}(Q;P)m+a&%+$1uJ(=Ma%+LiJ&Gc%qSd^DD5vg+@tt8 zz{=PGQ`h=WaDh*pr69lB>0L9`dCL{k^`jNEte&A zZ)a$p2mDME56yZQ<=*O4p)X$4&J5!ssQ|wg=A=M&vM)_%G(5&?NqJVgBBOag)eQNF zMOez+LaayCg>OfFlS}+U9B{WSd6n#3nl;(c+(ExU|7}{1?-W!7#p-b-tSned(B*}% zU5$g=w0$-OC!_10&i(!50pFk8W)*6IFx4-39mCxpg}s!80R;mL(!pQG|;FN#pF%-b2wKrIgav)Lu&4C|W8n6%`rni8K_IP$4Z1k@iq23i;p9IVa9J zz3=;dukZc;_~D$c?)$l}``Y()kH>t@Le3|6@>9R~hrHt-^1i_aUhaR^HEgHp^CAwR zf4b|{EAIXM-PiYb?XveN@M$m-C#Y$iTf^kP!-sGhufCC?@yKv}qBNe6r@N)Y{ar%+ zbnoPsX^ue?xc}p)+Sb;5#>N!_d>@WCC6~Is7MRfy){#BzGm(?az0H3p|A9VkW1^h* zyo8OZcvDW}ZG_zfUyu8@)F6aP9^(n#&o`A+-7|-Buy40V9?CFlF-q36K*PdnZCgf# zK7?(L$lJOlhtIrN)%hXg$Bm0zyJF7uDTmPS#?Yr*YH8;i4H-rmD|_kbW}7`_G<_h9 ze0nO6!AEQIo5@jDpWO7mBKKt7XURGKm3lqf4NuqTolHeuddBYTaOaflLl(nT*H&t7 zaGRablt{k#$MK@KC%hBJ7e3FuLkYPo9YxME#nefO<_uFial^B;8tBLMt9Tj7}#fR*fT(~+~XPVq_I<2_z zkEi90_J`Yia~u5?r{a$!MtV+{9gvEWDsSsdLEqNgyV<@y0*jD!O}FP<$t>-V|4005 z|17&pN@sCGV(qcd*H%>X3=_hcZpNOxcI=s?Y26TybjnK>XRKeHpzW>Gvajv%DygF@ zEP5nm1bY>#kKP;Uim3_XaqaZ9z7-jMV|Uu~Tg8zLyckK34H*rg>XTTNXsig)wI4^NIi&5xo zJ>xjt>8|xDQYXLEWwow2gou1Eu)&MF;qFr{RO0Z=#i&iPd*MTy{Bz8rZ+^HL!hiOS zTh4k7uMXJg|6xHs1$bumP zuj#pS`mL&jYLBVJyK>Px-a@nW-}1a1lfnx*^L*re%X|W6FTYeem4kZ!D0lziIq6`< z(jrCn36nZ~)48u6T{B7d>x`L9Ii8(6bKDq#bQ{n^!#=K=WAvjtqVnb0m#}X^JBHM& zZmd|#x!-a)q{J|sG2tvzt;3^eX;^30BgV)#E)B=OOg(t2>xAAPQtyb&8ha=x*Jw9p zI=x~v<3{nl+cCA4w^ca`CmRTb$(P?RO0UO9FD7(3YTicpyg7q7{`8n(;NHj@_U%my zL%c%$Cpd=to-@xx=>*87bma-Nef9n_Cutr&`FiNA#7(`N#n0=kJ0`r>WK}OTcP5;7 zGH^fPKlyT^FqCay6? zy$(LM8_&2Yv;V-Eul)xn#V2##Bq~by1a6UxY3UmNYE>)Oz0F6qL1!l>eBFS7n*5xC z)=GW2lQ`zhi=li1E!(ckwA$qdd65#@AXPGKkz3dRos zpwe^o6t7j^72Lzv^ujoNu$L}u2;gH_9g@fSoFQmk*2ZG`jqi?XjbYkWKRLBe3BM{r zXZZ1+YBO5_qxjO#TyxE(K7R}=T()Jv?d|!LQWo*PewD0=M(?`RoOoR2H@atep9IUy z(iERr_O9_Ox$|*MW_Mdk-zo33j+n!KqNfXyurpE|8kgUl+o>C#c-bM(zlh(!=*65A zua>H_@OgXY_ZOWmr88=7ce;MN;N#&Fiz;bvPu?#-XdzVYQJtmYtIWlM2==m@`Pgvw zSa~#3s#mVKvTQ={u!OJM9_z+Shh7~BsHrGEQXbKPb@R~nfrD!lXY)?1Hd=TnSn7E5 zt<9NBm!)~lOAucB0yBsH7++hVI%gAQ_I@(B7qeY1;DG2zzoetrkZDQ9n2+GQ3r`bI z1eP3o@o_eKZ@P4>i|N3PW8*e>*_JO%&UyGJ}{TU;CMrik?uynst zc6&sf@*z&sGtTAT9$hrHb_$rf^m&E2UxCN?#LPsOQR#8bFDu_$*Jw38ZL+aWNuB>( znNp{EIl}qSx9;4guTBGtYr`2;#^&kbhyChB@*L+fwy#*wJ(q7d(I_WpV{lUcVqR<3 zx!cp-)&|n%=iCShmO7^*+%%60PGu9?1`luZ!(%r1A+2_&9x*aFwZ7=_;p#-VAS>r* z|HZw*=Q+KZdB2=};HETkE|oS-4B6*^AbX_RTb2rW^P&dww((cl#~D zx_o)~L!FsD@-M-s>0)<~89N5d*F(shL;t2z69M0UE& zn;+W0FNWpB<@+{D6LR&)y-rd?cW-Yw?i>@<+htK>?K8VGH{;X0fiUgqjPo@)(MNRk zZz?6s*ZDlplD2-(lN6mv?34)4`?^iAKH8$FB^O4x$(9~$&yxV!qG_lgo zBG<(? z#M*v=KC-3%tQ1eG5u5*n8N4Fr%9~GjEAAZIS}S*O;lRoVi5$%4n~*o6>K_oeB2Z3V z_(O@E@3)i=++`|ob4hvdrqR5M=R#+e>22#<{P#3YsGm6e?Vgd<&Pmr>OlQ|WN{vE2 z_V)01^mn_Ds}Ks<|8?Td&VWsWV{Q{(1&z%444nMKUv}xQe6at{=i?#=7tqQ*{+Lh3 zL*4oXu|3X$kIP$I4!rN$5pB)Xy}ka()Ww9jyP{8a?cP1?#rF2wvkyXnuOj?k3Z0Ls z98xM63zqoO+;)7kf45MR-y=58C>|b3BgdO(JbJ%bm|IS%b-yqOh?jJB*HC=UJL^## z-+e9VV!-Da8XLWM^AOl70&DUl?< zNBV~$zHHp*Y1h&p zaLNF8t5+Xf4%!eWa!&OQe0{_)6Vt$mi-x+&`WipSl##MMDWc}n?D*lCnc9q|hrGL= z=4s!6-;Uxj9iOuLkm9nvIzm%q&wQ+qeo{}xNz|Kee%bnnGjY2r@J-H&jGLmAQp)1a z3tnW3!|hDnSD@iCa9&^9^q_JO;{k8y+dYTkV835a`Fdx zv%rVDr^`>*u~(`Kv$r36TGF|H&sfz4>65v&4Wm&_Q`k@b;^1{{@m(YVL8)_i=REN)3+aut_&EC+9~Z9d$_r8UF^<` zmK3pT)^dC6n5VnJ^0roeo1@vKw>y)YMbjkL_hsEq3qv?9P9A&HZk^EMO#k|hXvbJi zmVeaP8S%Z?y6zR#p7i_p`-69-<4rkjO0J53JNUii{@N*vuNKva#kyKtl%$sFIzcJk zOD{2>asotlpRK5pt8CjPW|jWYRYT0A>)zpOOgWd5QvBLDz2jbP6P;T(Iua`qB^E8^ zHuGX}BHXVv=W)bKy*=rcGP~2FuC6vM_I}N7J@r)c(?b(2e3pABa`&Wq8@JeX4lsz%bgE>-VvyvPiQ=FKWIA1b>n_m%M~u= zS7V9KMHuY4r!O%vnAAxJuU9y=>OT8O;T0#AjN`T(HYaSiR?V(J-BN2lwu-JNjE#}M z9ag#d#fs4F?^&NqGRQLM>JyHI-g9hV5nek|Wq*Y|>%k{Ppjlupmx1ZJ)XJN0Z~3(t z9LssJc|`i26tk{H+|)5tU3Tex+wqd(_Sai{>!RKAcef=^Za{4@wHn~}30G)2X(%a2 zf4td?<%44g^$84W0Lx9-B_`B@pBr$<*NnA43kizk|=kDQv5LOLjN zx*88yl}3EYVYaX=5lt|s-z9q6DoI5!L6!R;tn9rJ_rdKU@5Q+f@@KwZ$$c>0>B&oE zmIBtkgxOv<+arc~G)XNx@kt)v1u55oK>b_w2(?Qw-Tim;!}kg|eOs>+yeV{a=Xve$ zO~OqanR)CD9w|Gy4_-;ijH(t9Ht?wstzq0a+ofoZi8(u%#2g|j-J70A-@uZBvoZ{F zm6_Bv)a<{sA=*4TH!;zCsIJT~T%n>zk~>_sJi>~rfAm`vbBKR=V4``n^L1_TtBYa{DQ1g50-emTgx3bb&EM zu5Z;@Y^sqD?w#)%(`Ic~&t653$Y%1&Y@0b<2=ev3B?yowqKGi=y`l_Ol5uMmQewnqM4r_SAZEw3>sZ?O-|CIFN#@5;) zMJd~hCtKz3oqhXJ@zCy%m7(WeFYfBxEt{DP?=4v6cqN^svFV(S0e$|-f~&paNi{24 zm=%XkF1js_r+hC9XevrHO0|2<;IAc`^m3tzdsOOxuQGOAN3DUr*;9B&{9@`8{gW@y z>KRO`ThrWNu52)Em+*emCOV@vSCWc2b>h#pt(Y?6Dhp{y)KTxecC}4mk6&h&agR;2 zz_6Y~q^9i%U02Na&Aaz{B)u6=JK&hStyHrldiATv{Ckve@saiZTvrOWmk%gDravR6 z{A5khAJgYu3L+X$4;|ZZ!9d7Sa0RAuY*%!dO~SiGtR`)-=KFqFm2Tq5LAgJ!@mw z@^yn>?s~@@yvF%z!N^+mw@SG8-J9!+!E2KD-}CJ+vItTgOGCTA>^;H0qR?Pq#c*+y zt2LYK*62~wDojwPfo~As`i7!s$CYC{4>n{T>@4{v+QIrFU+@|G4gbzvXsx>m*Zt?) z?nMi_-8eh~0FnRJ^yqh1usNZig=v*$T~Wks5Ri{@^Rm=VxZB_YVdtrNV?f=v zTS~h!BawZZuVrNbwoWkFKJ!IlkzbOl;fP&QqX|!8%$$nvmf-2uH38Q)40P`KG8sF5 zcU06d!M~s3TGpi!t_;+8LUB%9L%u$rm;9WUrDw>2=^C33)~gAIU0b4*c~{@M5G!4O z|MiXB(Ue4&(lNDC`JwAAQS~Uk>lFt_>^QTBGsQ!!EVFtd0FZS)^$(}eiR$XoR zq3w4SiZ0HH9ZK%swsxSc{{DQz_oj1q&h9zdD3ro&#bWr_H*dk0p*XLO|AUdRVC#PE z^0m8iLt@?dca~kT%1di|kQtPaqo%shWn603J^zsrWzciCiu2>+!+aM%J^kJz;-`Ah zq#$0f`;VD`9S4pK)(W6{?wT3oH9u*5zN6#ixi$8NyNl^H3Vr)~jCXCayLWvLkLt(! z8hlZPgZnzZJ74W7(=@%CaePs#@#*+W&&RvP^RMNsk#)J6m?(NvsWqYLtwFA|M0Nhh zLn-c!)^akYWn&ujH{P>0`9+EqSZ>ZqP8|_8eIhlXtIJ4ue2kv+ zsj87(=Zyn?viu=I+8@4XHCYCfyGyi*=L%Z(@+a#Ie66b7@$K%Sz$L9Gqr)AeZx08i zCZsPbU*J**vz-N)$4j#cuz_XzcBkTzqR00w^77BU%$|b zi4tjZyHTCdL71S1P4(UK?wSls9mD5zs}tKjC%aFtFHBA(`BDX-obSCMfHoIRudB$ zV~yA1Z0d@5uN%dgQZlR1%$%JgYz&Mu zn=t}o$F^?ld>LY5?Dt~aCphn@`cP=>gFdm4;YIT+SN)XRW1U93V>bKxy~vQOK4nr) z*F7Fmea;kS=5i-nOa|YM=-R8n(MkUzJ|d){?MYmB87#1$9bUUhFWYimSn zA@^o5>h#J>@NY6KDlGLAiczlnu%d_$#?iSCj}wx((uWeDuMxA`y4v(p>|#)&yb|ls zJuSRTj)^UuCu=Fo$`>DCb{$E4Lz=8CrFtfIV@~7TBdZ8r8V>YPTQkK6QCLs-y-xZtv6@(aqu0h?)XyuGS06;4|M)7VE!DFT9ajYAj2g$ByJNtCUh40oiCc{o?r;8r{Q9NqUl6xcjQ038a z9&^X?JYN;Z?myf{L;tWEYfSmxH2YNIeGvxBTxN}aZo(v56k+(JK` ztin8HS61Z`=Fa&_UgN!j`qruhl>5rk2;b7{f`Wpv$(&~`p6wcGy zX!uxq_W&gYaT9enW8+86c;{20mFv+E>W|61wyL@9X8Q_dEsbsWqWGM z-4%xgBr7hII668{Zuev!kdtoJgY!MB zZKuhmA~ERD>OUr*jVcn@3pGN zJwn8%y=k_tjPCw^tn2$tlg2>Y z+Gq65ZbPWWbIvWc8?5It@}`n33oZ>45c@|L1ZSjUdb2K zw7im`wW`+Qbak7(J7Z{1P?d1>s*v;7vug`y7<*4_o=!?uu#VK&^<@fg6Y;!#_;_Kq zPF7!gV45leGmjN(x&{ODh*htW9}Ipnz*baOp8hCn)Tmb2mN%6ls~P+bO+QNtVYB#y zlb`LYSFPi|7vECro^I}(n9cthzdF)wqPmtx>I=AeQ*EqCmtksb8*El-K}r2YbVXGK zgZ-+jGZn{C26s4Xzihq|cy?>hnh2Xo8?SUl^=(Hb)J)IJT|&g68=Y$lEW$97a|&~M zXOQ)CH4Qa|-D7k4il*LY%2|asb(Gc?`zrPtTU<{6z&*FuQ>uWjbyt<^)PPMpUwo*% zH4>47**uYp$m4)NTW7|^iazR&i&B{zv}oQC`3YXeNw_+J_gmvCzrf)4J}D&C|ID|O zY316!K~oc3Uv7-C?6MWPob|lmlj6AWm(;4M?-}XIeCHE3rTro7x1%AVYQ_HE6+e`>QYX)(2?XQwzzLt32es5@jW?dHcjOXy32L}wlF@HfCF$JxBdxYETE|XvC(MrY4 zg21kEL6!i*bpQ{@+yj}AL&$`CB7EM#~3qF4~&!j(e|q6~Sf%b9|LrdJ(c>D$TBq=;XqprL6D zgX^TQ?kJMKzikzr%ufC$1yM!YpghiM1%^1bp0F#bbseiCl`1gttW&1oeQTyRkG&#% zI}Tsjk?3D|W>a$!JwM;L`NYQEANY#aPMXi<`EB%z_q!vrIm&R$W3i2;F~aASJJ>Fi z*nTPUD|V|?3!80qG$@%eVz&C|&Hv;qm z6?3=f@4dC}#=cuU`n!1b#cs*IGJO^BO5ws81EE*1j_F?WF1?=Mvnk1!?X4~Yb0^bO z0ORo@YxcJs=*rXqW7tHx&xW_EjA5`wA+}JqkF&;AKr;duR-f-zVjGN;4rROZp;!LU zwufK%;ZL0(J~az4WJVn}WIzc=_OiU==!-MAMoV*B@3a?5G7!FyyW&Ewj>wUqgxu!* zBS8TxgV|OcJ96Dyg0APu#!0KH9nCfuisif1_J3dinF0_$zp;N7)SH zL+|h?N<6#BKL9YUJf$*_GnLgsu{~QuM^&mQ5wJuB9TsR=H z*7=3^C0j<`@9fwLHf+Fdhx<4Of5yt7V4V$YsNh;T`l#;)&7YNrARyG1`2U1AK%kp7-$QxK|gb(ve~O)n=m=Uzq#RtgAeEsnM!+ zYC4GYpiCpsE6t&^X{+#_mk;vkB@TQ#kBR16sO5=`{&)_vQK{fW)`(EFdz60i_PwQV zYq*Mdy{>RwEAq(Np&AgTo1%_txc-VW?P%vrK|XzTtnUkrs10q8IS!ox=Q3C&9z@tl zyfqZv)_kk>sIK~E&C9p9jGj``!zQpu*Iu*e=w01-p`NavZC*O6MxK6H7`@qVWAx|z zb6eEET5R=*_SVpZHS9cpOhrFl_=#bp*>MX*(0B3H^!1(T>s6*q)aLDmKBeDUci%kn zyUyk2H;Ba$2laE)&KQTj@2&8auZO=?U)a!!B^5PY{J=gcH?e6iJd zzO`Mb?6az4k)IT~*k)RqZb%q7>*g9r1bGN)Zq;4e5>RYn@ydlKuJ+1vJ2cM) z)vM-c^$;$p`LII#eB*tSM={FRVEMt-Ta=EfF3hi>x92KikLm<(oxbIk7@1IOu=j>d z6SvseTpoR#Y2FmP_K02eZB?Py&9}s=hrSGk@62=RzOHL!yN<6oNAY&6#BsW5;hAge zB3qp=R~2SF!kA76wrx}58Z@1;lF%5T>0@9te=T8yLj zk`6f7i1U4Y%MEu<_N_gSm_SuluH7r!_4D7y&m+2@mfX8mmozS5*Xhmdh3xhijCSq1 zTePluvo89;BZsx<7I}7g3vp%r)hrDXhfjvHDowqp(bs2xVdS3#3woY)P5qKUMrWA% z7|Wf6?XL=OIvLJ;=xR@{=h6{acV$7afk38wNuEBlN{dzrzFbzzhBwFcjSJdjfIKifj0JA zno73Z<>oe-e(sZ-74Th!aR(UE9g3_pjfFIfU#3eb4PQ%_xq7X-?-8$Dy6=vgrdy1B zxg4{ME2loFBFfzz^6%UcZ@E`w=l^Vz)i;|ck8fWMtqlBCZ49Ct8|n@17v$3Kr5?!T zx0sE~UwCY2{}AJzr9UY8;5HAAul0&x|0f1J$;KfMGcmCq`IqcMZ3ZfPZFH+n-2J4n zrka)i#C^LKHY4P*Q2ATznA1FZYj*CvrKRJLaJwr}g3&wmA?suPEgOv$HJhW$a_o0~ za5}LUJcw=-wTC91}uRN-nl`^8B*Uk*!I0h zUKALc)|SVwlR6*=P>3bX@&xc3I2k+}h}~aeCF8KF2lKmEk|Kr#kb>(J}A2P*ZgJfFwS* z@{RF#XUVMOdw+TrC3G_(bipeUj=^FmZbikA;s5D^SG7OA;8je~*}?L!Zh6K0xaAcE zu4IKHF~4}rD-!b;x4hz^D;Q~RWyJm*5BXL`6b}9;H!@P*!Dtfu%qaxUd~#A+6^^SCGs-r(7?R2le6iVh-aqf{pygx%J$6; z&Q3=^>9#nT^kxn-(ZkO&+?VDQbI)nokCMUBeLhf}YgF1TG1_zCtKj`y4h9wb$0EKC z<|#ZZB?#Py0$9Du!GybOuYW7$3_t8T{t;N^E%%!WY=UEUMMFjdfVL+WpVtzVw-aj^_1r`F?7lFi=aTLvH)F zx$}r4`GKLIuD8p{G+Q|41~-Mf&7#&T_GWve%h#x(PUiOVeok((yPS8X!uf;#9j8B4 zP~1(+g8k;I=f13sa#BMVBrng)Salx*HU_2o<_3bm`(z;$^1f-;*yo zWrQwTC4VR0BMN;dwxhVnbm?d4{?Q-*pi5FQSVriARp|GBcK;}3lQj2_LTXK;>ChF& zkVON_RYkxMScrk(?onWBSm@qa@D&9vE+v0O!w?wqR}A<{Zi@vC$X~%d%s6rbJork4 zGE!flymUu2gb-X;3;L37S(Y%=K6=c`!X3ah9MLf19jL&(By?PzE%sZv8%yXYt4bWO z@^%NmHn*`-B7JC(J`_!;ko+7IQ3-TFdkGzLSLpg@1e!#uo|T)khpUB^8z2B8Nl#+0 zm8G3Ibh9*cTQ!D&f#K0e6A3j}XAc)Z(|&N%uDO$&3-lBV9|@)X;6iCnI}0m4HAM*x zm^-+IS5{U+2@D7zaf1qriVk|t=7znLbYic-=9S@7?riR5T0Xd-&r-%JzH$y3q<+2(Ji zi8f-O{>_l_YdTJfD-?V}|C<5e*Jwi1dV6Bf`@OOw#al`~{eIVuCPq~Xnm`8syLB8x zOtvU#`u%gho$x7*x&ER!6X(;ASn6t z`~5hUHj&5uem_o{T2t@|_itwHzZP3?v?)4q(*JvPob=u%3O+4+0oDI9|Nb>Tk>346 zNz?E5QC1($)noa}(FsUno=ro~a3p;lQD``~~RaH0~O#m-< zRDr{BXz&jSz5^H*`VN*puqYB73$=mZDqzh3yob%*m?&xq!$U?eJVqP=!(j;G0FlX$ zg#ePjme>cD6ae|OUXery6~S;SqA-9EG(d;}1c0wZ0Wh`<58;woMLYXL5Gec&NiV2b1s+b4jWQqsVihDSCGsbbD;D>IEMhh8Cs{ZYLL3Bw zzaon$v?U2Hb2yZN?a#=9BnNQ`yF?a565tXfO^D1Nss?@_eqU<)lZ!OuqR|L(K+|85 zOJv*OL7+&!{*+uwk`zd!E@?ke0f9w8oLv(3((fRvAi^N7Q!4=t`Cy$77|LIg zPh{{C`NXbNx(Y%5sjfmKkqw(j8lukpy5W$sCtRCrja{8Pbx2M54t3&cD)YlBXq-1GqpF zVM&m`A&2PQfeAup2nihW>_jdBZW1}Rqy*&O;~-8hnJ(mL$-gJUp#BiIm!3i8T}TU@ zKe>!WVE<0viHs#F2$GoSQ3gX2>7q(dXn;(+!)mljz- zOT(hg9l^VlY0VgVyD=2vk#Lf`g~XHK-&_lAhiY)wF>cym*0DxHN$pGO_5$B>ktEBJl(&K`o7vn42tVCE5GXCIMih+TRZu(l=t>xXirB zLtA18@F>KgLi+dr>a9q4kbu1qzW?1l5Q~Hq>_nijRL-1+ofJt561-fUdb&h{{Egl( zGk>y15hV^eWk~v9Ttrs>*Wr=0MenCZK-nr~Jfax_MWTWv&N`_b_kT!K(Ep+lVDMm) z0G@{i+6A!&(l#h_AW9a}JfK~~+ziq(2rtAQ8a6Glju;@o@0T(OD6=5;g+6~|62zpJ zLfR+{nrhlZ!zzj-i0slSayk;^Z={VTlRzYcoMsXw3pscSPX`Tw2>(w$9S(4U!fl4PrVke;P@zrJ&eF{oMw~>#h$a9d#%Qo146So{y2qQ$#3Yoo#oxx!x^Ere?c zFDUFR=@c#ArAaUq-v7ZTbw=%;tYkt?@~dJ z3h%G^Dbi>mTuXQ{h#xvdi+8CGN`?2={1gce7V|@=Xz?zUi>dJbnx7)!g>Wt5 z#SzFl^;flXDy+Zer-)Gqivx#3hzSNU*&t&j=1)IGyR>>hh4k0l6!1JCJ%bh_&`1RM z9}B2JEsd495=BaEp%_nqAu%YR18@^6#J}dJkRUe%rV6HVD3D3vVOZo((NYw1kzm4# zfuTSK1PqlL@2~kOD47D;8wLkztY|!x<=}7#@ue_CAzlOyWLlsJVmant^Hd1g5+c5=avS zqHZAoJD_MqAQna`@uDH0_Rswlm~VoCfDyxxVALR=q;Qjz$ty{@5I88!r55tC4YdDV zLPthS^jKunlpbrDUjHfTU-MaH)EG$0WYkznDgTT~e>3V|b6aH8a6BYpGQ+_t$PWek zuTcM*??R#=eMW$^0}XTxoCE_dWhwaoiKKyo1MX6pGjUJwKUXmX@Y2w6I6SZw1P%px zY#dm~`pYpRQ6N&&qW(1xhQ@+1!mwc34*^4C34jbNNcVmOie*ZMgq#cIn1PIF%u31J z-Q2<11}szCK!?-F6Kx4?7b_E;IYB5$r3pr`0PkcYY~>`Tzke6y z(mj&u2ouG!JQ5Bbqu}9g>+DKis7FGJ?@F##=I%d0n5D&2P&fTq*#gbLxjst|3#+BB zP=$=R>I3R7OI0msd7W5)0c9cL@5pr*sFZ{P^%o>4j-wDuC;@Af&E2hpl_f#(9tTdl zfdg=8u080brYrp=Tvz#a@#Y@vbyW^B_3R)?u*Al21N-2c(G88iV_A#;;|@NyeP0m3D_QG zJJ1&g)te|`K*4}M(!#)SY4`{F5}*)C-4}&|%3stl911#*K?y?vrY_SV6b^-@9S0hm z@uYbk=nL{A>gU1HK=o<)hX$3LWi%sE&>m1qTHt6j7Fsi*hT-r~6sCj$ydVXoh5>T} zjHZDBZGn6kO9?_=b`g%l(Kwlhmz?c#S922N()57rh z<$eQ&U#7b_JYkvNz$4JK&qKk{z%A0?z$3tXh!%z>(Ap~Ki-68tP|=LXz-fI1Fg7@C zJJ1(J>$9PD(9w6Q=MlhpY8t+PzUXCs4YY$Q=hS`CAU&XlfxcL%icQ;&RuA9=9Ew)& zL0=qn9-q1|4unQ(7;v!2<^CB>ptTRE9ql-Pms}=u6!4r-m7WqWXa_tcEsQ{1LZWO3 z`U1zXyd6{yp=^f$$I&q~ehBnkW(!a|+E{@A7D$T&^j&5PP&-;a0}l&LOQ~o9eQ~t9 z3$>%wU7#7r<-Q$*r?o+-9W4&f6TMtUSoku$pdED9iHcX?`U&XJJ|zsys6i@D4MTu- z%X9+@m^8cxf?OU~K^TM%`cvTm`)QZyGXm07+I9d)>ra8;pv02;c?i@pzYF%ELTBJ; z+F@v81q!4p&_My3c6i!22-*>r@rsx!()I;rPOAqXsazIAf#9HxG1PdG7^vJy1H&%M z8Gt~Rz07KEnWB@}i9|vlg9|G-`>lFe8JSr`H zs6UZ88V>CXr5*DFmaHC4ixz3oR(|0vL{#_aMUr znG;Pru&4`NfI`(5z-V=0yASU*Q2HdI8!)nT!3J-;{`Aztxp6n9PRuFiAE9ksZ)&ul>V0C1u$9} z0oS>Vr$`(gWbQOHgH(ALUIGCmFf{D|3_3eUMGJv|qSbpOfq? z!mub>`GUBBTIOHCZieNt7Q5V^g2a=S_b6z0AI&%b3_+XU0~kmsY3TzsSQ`BU?QqNd zE|~t(=4K#STQ(mAFchr~0vMK7p8*VHl(e)Ez*JxvY?;haZ~}%l?gJQpdF+9MC`$7@ z1bR7rAiPn|f8AZp?HsIJq5GKj+xc2SH3|tmXJ@eEl2|DMCnGeRtewG6fS1tqP8!NE zW0*AX3wV?w2Cjrt1aVpkt*oqyS5QPM;8hgx1eBtJEaQJ}0_~OnWg;b8D+_x!4@Z~; zIC?=qV&QmYJVHfLMG221fO#cGNdd@5Raq5{hl)gQ?&hxU#BvG>cvMhN5foI>R%QG@ DWBTWq literal 0 HcmV?d00001 diff --git a/src/hal/components/cros/include/cros.h b/src/hal/components/cros/include/cros.h new file mode 100644 index 00000000..c6c034f6 --- /dev/null +++ b/src/hal/components/cros/include/cros.h @@ -0,0 +1,27 @@ +/*! \file cros.h + * \brief Top header file of cROS + * + * This cROS library is an upgrade from the version of 27/apr/2015. + * It now supports service calls (service clients). + * \author Nico and others (cROS 0.9) + * \author Richard R. Carrillo (of the Service-client implementation). Aging in Vision and Action lab, Institut de la Vision, Sorbonne University, Paris, France. + * \date 10 Oct 2017 + */ + +#ifndef INCLUDE_CROS_H_ +#define INCLUDE_CROS_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "cros_api.h" +#include "cros_log.h" +#include "cros_clock.h" + +#endif /* INCLUDE_CROS_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/src/hal/components/cros/include/cros_api.h b/src/hal/components/cros/include/cros_api.h new file mode 100644 index 00000000..c12ec625 --- /dev/null +++ b/src/hal/components/cros/include/cros_api.h @@ -0,0 +1,407 @@ +#ifndef _CROS_API_H_ +#define _CROS_API_H_ + +#include "xmlrpc_params.h" +#include "cros_node.h" +#include "cros_message.h" +#include "cros_err_codes.h" + +#define CROS_INFINITE_TIMEOUT ~0UL + +typedef enum CrosTransportType +{ + CROS_TRANSPORT_TCPROS, + CROS_TRANSPORT_UPDROS +} CrosTransportType; + +typedef enum CrosTransportDirection +{ + CROS_TRANSPORT_DIRECTION_IN, + CROS_TRANSPORT_DIRECTION_OUT, + CROS_TRANSPORT_DIRECTION_BOTH +} CrosTransportDirection; + +typedef struct LookupNodeResult LookupNodeResult; +typedef struct GetPublishedTopicsResult GetPublishedTopicsResult; +typedef struct GetTopicTypesResult GetTopicTypesResult; +typedef struct GetSystemStateResult GetSystemStateResult; +typedef struct GetUriResult GetUriResult; +typedef struct LookupServiceResult LookupServiceResult; +typedef struct GetBusStatsResult GetBusStatsResult; +typedef struct GetBusInfoResult GetBusInfoResult; +typedef struct GetMasterUriResult GetMasterUriResult; +typedef struct ShutdownResult ShutdownResult; +typedef struct GetPidResult GetPidResult; +typedef struct GetSubscriptionsResult GetSubscriptionsResult; +typedef struct GetPublicationsResult GetPublicationsResult; + +struct LookupNodeResult +{ + int code; + char *status; + char *uri; +}; + +struct TopicTypePair; + +struct GetPublishedTopicsResult +{ + int code; + char *status; + struct TopicTypePair *topics; + size_t topic_count; +}; + +struct GetTopicTypesResult +{ + int code; + char *status; + struct TopicTypePair *topics; + size_t topic_count; +}; + +struct ProviderState; + +struct GetSystemStateResult +{ + int code; + char *status; + struct ProviderState *publishers; + size_t pub_count; + struct ProviderState *subscribers; + size_t sub_count; + struct ProviderState *service_providers; + size_t svc_count; +}; + +struct GetUriResult +{ + int code; + char *status; + char *master_uri; +}; + +struct LookupServiceResult +{ + int code; + char *status; + char *service_result; +}; + +struct PubConnectionData; + +struct TopicPubStats +{ + char *topic_name; + size_t message_data_sent; + struct PubConnectionData *datas; + size_t datas_count; +}; + +struct SubConnectionData; + +struct TopicSubStats +{ + char *topic_name; + struct SubConnectionData *datas; + size_t datas_count; +}; + +struct ServiceStats +{ + size_t num_requests; + size_t bytes_received; + size_t bytes_sent; +}; + +struct BusStats +{ + struct TopicPubStats *pub_stats; + size_t pub_stats_count; + struct TopicSubStats *sub_stats; + size_t sub_stats_count; + struct ServiceStats service_stats; +}; + +struct GetBusStatsResult +{ + int code; + char *status; + struct BusStats stats; +}; + +struct BusInfo; + +struct GetBusInfoResult +{ + int code; + char *status; + struct BusInfo *bus_infos; + size_t bus_infos_count; +}; + +struct GetMasterUriResult +{ + int code; + char *status; + char *master_uri; +}; + +struct ShutdownResult +{ + int code; + char *status; + int ignore; +}; + +struct GetPidResult +{ + int code; + char *status; + int server_process_pid; +}; + +struct GetSubscriptionsResult +{ + int code; + char *status; + struct TopicTypePair *topic_list; + size_t topic_count; +}; + +struct GetPublicationsResult +{ + int code; + char *status; + struct TopicTypePair *topic_list; + size_t topic_count; +}; + +struct DeleteParamResult +{ + int code; + char *status; + int ignore; +}; + +struct SetParamResult +{ + int code; + char *status; + int ignore; +}; + +struct GetParamResult +{ + int code; + char *status; + XmlrpcParam *value; +}; + +struct SearchParamResult +{ + int code; + char *status; + char *found_key; +}; + +struct HasParamResult +{ + int code; + char *status; + int has_param; +}; + +struct GetParamNamesResult +{ + int code; + char *status; + char **parameter_names; + size_t parameter_count; +}; + +struct TopicTypePair +{ + char *topic; + char *type; +}; + +struct ProviderState +{ + char *provider_name; + char **users; + size_t user_count; +}; + +struct PubConnectionData +{ + int connection_id; + size_t bytes_sent; + size_t num_sent; + int connected; +}; + +struct SubConnectionData +{ + int connection_id; + size_t bytes_received; + int drop_estimate; + int connected; +}; + +struct BusInfo +{ + int connectionId; + int destinationId; + CrosTransportDirection direction; + CrosTransportType transport; + char *topic; + int connected; +}; + +typedef struct LookupNodeResult LookupNodeResult; +typedef struct GetPublishedTopicsResult GetPublishedTopicsResult; +typedef struct GetTopicTypesResult GetTopicTypesResult; +typedef struct GetSystemStateResult GetSystemStateResult; +typedef struct GetUriResult GetUriResult; +typedef struct LookupServiceResult LookupServiceResult; +typedef struct GetBusStatsResult GetBusStatsResult; +typedef struct GetBusInfoResult GetBusInfoResult; +typedef struct GetMasterUriResult GetMasterUriResult; +typedef struct ShutdownResult ShutdownResult; +typedef struct GetPidResult GetPidResult; +typedef struct GetSubscriptionsResult GetSubscriptionsResult; +typedef struct GetPublicationsResult GetPublicationsResult; +typedef struct DeleteParamResult DeleteParamResult; +typedef struct SetParamResult SetParamResult; +typedef struct GetParamResult GetParamResult; +typedef struct SearchParamResult SearchParamResult; +typedef struct HasParamResult HasParamResult; +typedef struct GetParamNamesResult GetParamNamesResult; + +typedef void (*LookupNodeCallback)(int callid, LookupNodeResult *result, void *context); +typedef void (*GetPublishedTopicsCallback)(int callid, GetPublishedTopicsResult *result, void *context); +typedef void (*GetTopicTypesCallback)(int callid, GetTopicTypesResult *result, void *context); +typedef void (*GetSystemStateCallback)(int callid, GetSystemStateResult *result, void *context); +typedef void (*GetUriCallback)(int callid, GetUriResult *result, void *context); +typedef void (*LookupServiceCallback)(int callid, LookupServiceResult *result, void *context); +typedef void (*GetBusStatsCallback)(int callid, GetBusStatsResult *result, void *context); +typedef void (*GetBusInfoCallback)(int callid, GetBusInfoResult *result, void *context); +typedef void (*GetMasterUriCallback)(int callid, GetMasterUriResult *result, void *context); +typedef void (*RequestShutdownCallback)(int callid, ShutdownResult *result, void *context); +typedef void (*GetPidCallback)(int callid, GetPidResult *result, void *context); +typedef void (*GetSubscriptionsCallback)(int callid, GetSubscriptionsResult *result, void *context); +typedef void (*GetPublicationsCallback)(int callid, GetPublicationsResult *result, void *context); +typedef void (*DeleteParamCallback)(int callid, DeleteParamResult *result, void *context); +typedef void (*SetParamCallback)(int callid, SetParamResult *result, void *context); +typedef void (*GetParamCallback)(int callid, GetParamResult *result, void *context); +typedef void (*SearchParamCallback)(int callid, SearchParamResult *result, void *context); +typedef void (*HasParamCallback)(int callid, HasParamResult *result, void *context); +typedef void (*GetParamNamesCallback)(int callid, GetParamNamesResult *result, void *context); + +typedef uint8_t CallbackResponse; +typedef CallbackResponse (*ServiceCallerApiCallback)(cRosMessage *request, cRosMessage *response, int call_resp_flag, void *context); +typedef CallbackResponse (*ServiceProviderApiCallback)(cRosMessage *request, cRosMessage *response, void *context); +typedef CallbackResponse (*SubscriberApiCallback)(cRosMessage *message, void *context); +/*! \brief Application-defined callback function which is called by the library */ +typedef CallbackResponse (*PublisherApiCallback)(cRosMessage *message, void *context); + +// Transfer data from message buffer (context_) of the Publisher/Service caller to the output ROS packet buffer (buffer) +cRosErrCodePack cRosNodeSerializeOutgoingMessage(DynBuffer *buffer, void *context_); +// Transfer data from packet buffer (buffer) of the Service caller to the input mesage buffer (context_) +cRosErrCodePack cRosNodeDeserializeIncomingPacket(DynBuffer *buffer, void *context_); + +// Intermediary functions that call the user callback functions +// context is a structure (object) opaque for the caller function +cRosErrCodePack cRosNodeSubscriberCallback(void *context_); +cRosErrCodePack cRosNodePublisherCallback(void *context_); +cRosErrCodePack cRosNodeServiceCallerCallback(int call_resp_flag, void* contex_); +cRosErrCodePack cRosNodeServiceProviderCallback(void *context_); +void cRosNodeStatusCallback(CrosNodeStatusUsr *status, void* context_); + +// Master api: register/unregister methods +cRosErrCodePack cRosApiRegisterServiceCaller(CrosNode *node, const char *service_name, const char *service_type, int loop_period, ServiceCallerApiCallback callback, NodeStatusApiCallback status_callback, void *context, int persistent, int tcp_nodelay, int *svcidx_ptr); +void cRosApiReleaseServiceCaller(CrosNode *node, int svcidx); +cRosErrCodePack cRosApiRegisterServiceProvider(CrosNode *node, const char *service_name, const char *service_type, ServiceProviderApiCallback callback, NodeStatusApiCallback status_callback, void *context, int *svcidx_ptr); +cRosErrCodePack cRosApiUnregisterServiceProvider(CrosNode *node, int svcidx); +void cRosApiReleaseServiceProvider(CrosNode *node, int svcidx); +cRosErrCodePack cRosApiRegisterSubscriber(CrosNode *node, const char *topic_name, const char *topic_type, SubscriberApiCallback callback, NodeStatusApiCallback status_callback, void *context, int tcp_nodelay, int *subidx_ptr); +cRosErrCodePack cRosApiUnregisterSubscriber(CrosNode *node, int subidx); +void cRosApiReleaseSubscriber(CrosNode *node, int subidx); +cRosErrCodePack cRosApiRegisterPublisher(CrosNode *node, const char *topic_name, const char *topic_type, int loop_period, PublisherApiCallback callback, NodeStatusApiCallback status_callback, void *context, int *pubidx_ptr); +cRosErrCodePack cRosApiUnregisterPublisher(CrosNode *node, int pubidx); +void cRosApiReleasePublisher(CrosNode *node, int pubidx); + +// Master api: name service and system state +cRosErrCodePack cRosApiLookupNode(CrosNode *node, const char *node_name, LookupNodeCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiGetPublishedTopics(CrosNode *node, const char *subgraph, GetPublishedTopicsCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiGetTopicTypes(CrosNode *node, GetTopicTypesCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiGetSystemState(CrosNode *node, GetSystemStateCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiGetUri(CrosNode *node, GetUriCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiLookupService(CrosNode *node, const char *service, LookupServiceCallback callback, void *context, int *caller_id_ptr); + +// Slave API + +/*! + * \param if host == NULL, it will contact ROS Master + * \param So, host and port are only used if host != NULL + */ +cRosErrCodePack cRosApiGetBusStats(CrosNode *node, const char* host, int port, GetBusStatsCallback callback, void *context, int *caller_id_ptr); + +/*! + * \param if host == NULL, it will contact ROS Master + * \param So, host and port are only used if host != NULL + */ +cRosErrCodePack cRosApiGetBusInfo(CrosNode *node, const char* host, int port, GetBusInfoCallback callback, void *context, int *caller_id_ptr); + +/*! + * \param if host == NULL, it will contact ROS Master + * \param So, host and port are only used if host != NULL + */ +cRosErrCodePack cRosApiGetMasterUri(CrosNode *node, const char* host, int port, GetMasterUriCallback callback, void *context, int *caller_id_ptr); + +/*! + * \param if host == NULL, it will contact ROS Master + * \param So, host and port are only used if host != NULL + */ +cRosErrCodePack cRosApiShutdown(CrosNode *node, const char* host, int port, const char *msg, GetMasterUriCallback callback, void *context, int *caller_id_ptr); + +/*! + * \param if host == NULL, it will contact ROS Master + * \param So, host and port are only used if host != NULL + */ +cRosErrCodePack cRosApiGetPid(CrosNode *node, const char* host, int port, GetPidCallback callback, void *context, int *caller_id_ptr); + +/*! + * \param if host == NULL, it will contact ROS Master + * \param So, host and port are only used if host != NULL + */ +cRosErrCodePack cRosApiGetSubscriptions(CrosNode *node, const char* host, int port, GetSubscriptionsCallback callback, void *context, int *caller_id_ptr); + +/*! + * \param if host == NULL, it will contact ROS Master + * \param So, host and port are only used if host != NULL + */ +cRosErrCodePack cRosApiGetPublications(CrosNode *node, const char* host, int port, GetSubscriptionsCallback callback, void *context, int *caller_id_ptr); + +// Parameter Server API: subscribe/unsubscribe params +cRosErrCodePack cRosApiSubscribeParam(CrosNode *node, const char *key, NodeStatusApiCallback callback, void *context, int *paramsubidx_ptr); +cRosErrCodePack cRosApiUnsubscribeParam(CrosNode *node, int paramsubidx); + +// Parameter Server API: other methods +cRosErrCodePack cRosApiDeleteParam(CrosNode *node, const char *key, DeleteParamCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiSetParam(CrosNode *node, const char *key, XmlrpcParam *value, SetParamCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiGetParam(CrosNode *node, const char *key, GetParamCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiSearchParam(CrosNode *node, const char *key, SearchParamCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiHasParam(CrosNode *node, const char *key, HasParamCallback callback, void *context, int *caller_id_ptr); +cRosErrCodePack cRosApiGetParamNames(CrosNode *node, GetParamNamesCallback callback, void *context, int *caller_id_ptr); + +// Message polling +cRosErrCodePack cRosNodeReceiveTopicMsg(CrosNode *node, int subidx, cRosMessage *msg, unsigned char *buff_overflow, unsigned long time_out); +cRosErrCodePack cRosNodeQueueTopicMsg( CrosNode *node, int pubidx, cRosMessage *msg ); +cRosErrCodePack cRosNodeSendTopicMsg(CrosNode *node, int pubidx, cRosMessage *msg, unsigned long time_out); +cRosErrCodePack cRosNodeServiceCall(CrosNode *node, int svcidx, cRosMessage *req_msg, cRosMessage *resp_msg, unsigned long time_out); +cRosMessage *cRosApiCreatePublisherMessage(CrosNode *node, int pubidx); +cRosMessage *cRosApiCreateServiceCallerRequest(CrosNode *node, int svcidx); + +#endif // _CROS_API_H_ diff --git a/src/hal/components/cros/include/cros_api_call.h b/src/hal/components/cros/include/cros_api_call.h new file mode 100644 index 00000000..dfe5fb29 --- /dev/null +++ b/src/hal/components/cros/include/cros_api_call.h @@ -0,0 +1,54 @@ +#ifndef _CROS_API_CALL_H_ +#define _CROS_API_CALL_H_ + +#include "cros_node_api.h" +#include "xmlrpc_params_vector.h" + +typedef void (*ResultCallback)(int callid, void *result, void *context); +typedef void * (*FetchResultCallback)(XmlrpcParamVector *response); +typedef void (*FreeResultCallback)(void *result); + +typedef struct RosApiCall RosApiCall; +typedef struct ApiCallNode ApiCallNode; +typedef struct ApiCallQueue ApiCallQueue; + +struct RosApiCall +{ + int id; //! Progressive id of the call + int user_call; //! 1 = user api call, 0 = internal call + CrosApiMethod method; //! ROS api method + XmlrpcParamVector params; //! Method arguments + char *host; //! Host to contact for the api + int port; //! Tcp port of the host to contact for the api + int provider_idx; //! Provider (sub, pub, service provider or service caller) index + ResultCallback result_callback; //! Response callback + void *context_data; //! Result callback context + FetchResultCallback fetch_result_callback; //! Callback to fetch the result + FreeResultCallback free_result_callback; +}; + +struct ApiCallNode +{ + RosApiCall *call; + ApiCallNode* next; +}; + +struct ApiCallQueue +{ + ApiCallNode* head; + ApiCallNode* tail; + size_t count; +}; + +RosApiCall * newRosApiCall(void); +void freeRosApiCall(RosApiCall *call); + +void initApiCallQueue(ApiCallQueue *queue); +int enqueueApiCall(ApiCallQueue *queue, RosApiCall* apiCall); +RosApiCall * peekApiCallQueue(ApiCallQueue *queue); +RosApiCall * dequeueApiCall(ApiCallQueue *queue); +void releaseApiCallQueue(ApiCallQueue *queue); +size_t getQueueCount(ApiCallQueue *queue); +int isQueueEmpty(ApiCallQueue *queue); + +#endif // _CROS_API_CALL_H_ diff --git a/src/hal/components/cros/include/cros_api_internal.h b/src/hal/components/cros/include/cros_api_internal.h new file mode 100644 index 00000000..ca078deb --- /dev/null +++ b/src/hal/components/cros/include/cros_api_internal.h @@ -0,0 +1,138 @@ +#ifndef _CROS_NODE_INTERNAL_H_ +#define _CROS_NODE_INTERNAL_H_ + +#include "cros_node.h" + +/*! \brief Register a new topic to be published by a node. + * \param n Pointer to CrosNode structure that has previously been created with cRosNodeCreate + * \param message_definition Full text of message definition (output of gendeps --cat) + * \param topic_name The published topic namespace + * \param topic_type The published topic data type (e.g., std_msgs/String, ...) + * \param md5sum The MD5 sum of the message typedef + * \param loop_period Period (in msec) for publication cycle + * \param data_context Pointer to user data than will be passed to the callback function as context information. Can be NULL + * \return Returns the index of the created publisher on success, -1 on failure (e.g., the maximum number of publisher topics has been reached) + */ +int cRosNodeRegisterPublisher(CrosNode *n, const char *message_definition, const char *topic_name, + const char *topic_type, const char *md5sum, int loop_period, void *data_context); + +/*! \brief Register the node in roscore as topic subscriber. + * \param n Pointer to CrosNode structure that has previously been created with cRosNodeCreate + * \param message_definition Full text of message definition + * \param topic_name The published topic namespace + * \param topic_type The published topic data type (e.g., std_msgs/String, ...) + * \param md5sum The MD5 sum of the message typedef + * \param data_context Pointer to user data than will be passed to the callback function as context information. Can be NULL + * \param tcp_nodelay If this parameter is 1, the publisher is asked to disable the Nagle algorithm for the socket, + * so small packets are sent immediately, reducing the latency but increasing the bandwidth usage. + * \return Returns the index of the created subscriber on success, -1 on failure (e.g., the maximum number of subscriber topics has been reached) + */ +int cRosNodeRegisterSubscriber(CrosNode *n, const char *message_definition, const char *topic_name, const char *topic_type, + const char *md5sum, void *data_context, int tcp_nodelay); + +/*! \brief Register the service provider in roscore + * \param n Pointer to CrosNode structure that has previously been created with cRosNodeCreate + * \param service_name The published service namespace + * \param service_type The published service data type (e.g., roscpp_tutorials/TwoInts) + * \param md5sum The MD5 sum of the message typedef + * \param data_context Pointer to user data than will be passed to the callback function as context information. Can be NULL + * \return Returns the index of the created service provider on success, -1 on failure (e.g., the maximum number of service providers has been reached) + */ +int cRosNodeRegisterServiceProvider(CrosNode *n, const char *service_name, + const char *service_type, const char *md5sum, void *data_context); + +/*! \brief Register the service caller (not in roscore) + * \param n Pointer to CrosNode structure that has previously been created with cRosNodeCreate + * \param service_name The published service namespace + * \param service_type The published service data type (e.g., roscpp_tutorials/TwoInts) + * \param md5sum The MD5 sum of the message typedef + * \param data_context Pointer to user data than will be passed to the callback function as context information. Can be NULL + * \param persistent If this parameter is 1, the RPCROS connection is kept opened for multiple calls. + * This reduces bandwidth usage and latency. Otherwise the parameter value should be to 0. + * \param tcp_nodelay If this parameter is 1, the service provider is asked to disable the Nagle algorithm for the socket, + * so small packets are sent immediately, reducing the latency but increasing the bandwidth usage. + * \return Returns the index of the created service caller on success, -1 on failure (e.g., the maximum number of services has been reached) + */ +int cRosNodeRegisterServiceCaller(CrosNode *n, const char *message_definition, const char *service_name, + const char *service_type, const char *md5sum, int loop_period, + void *data_context, int persistent, int tcp_nodelay); + +/*! \brief Unregister the topic subscriber + * + * \param subidx Index of the subscriber + */ +int cRosNodeUnregisterSubscriber(CrosNode *node, int subidx); + +/*! \brief Unregister the topic publisher + * + * \param pubidx Index of the topic publisher + */ +int cRosNodeUnregisterPublisher(CrosNode *node, int pubidx); + +/*! \brief Unregister the service provider + * + * \param serviceidx Index of the service provider + */ +int cRosNodeUnregisterServiceProvider(CrosNode *node, int serviceidx); + + +/*! \brief Frees the memory of a publisher node + * + * \param node Pointer to the publisher node to be freed + */ +void cRosNodeReleasePublisher(PublisherNode *node); + +/*! \brief Frees the memory of a subscriber node + * + * \param node Pointer to the subscriber node to be freed + */ +void cRosNodeReleaseSubscriber(SubscriberNode *node); + +/*! \brief Frees the memory of a service-provider node + * + * \param node Pointer to the service node to be freed + */ +void cRosNodeReleaseServiceProvider(ServiceProviderNode *node); + +/*! \brief Frees the memory of a service-caller node + * + * \param node Pointer to the service node to be freed + */ +void cRosNodeReleaseServiceCaller(ServiceCallerNode *node); + +/*! \brief Frees the memory of a parameter subscription + * + * \param node Pointer to the parameter subscription to be freed + */ +void cRosNodeReleaseParameterSubscrition(ParameterSubscription *subscription); + +/*! \brief Search for a Tcpros client proc that is currently not assigned + * to any subscriber and assign it to the specified subscriber + * + * \param node Pointer to CrosNode structure that has previously been created with cRosNodeCreate + * \param subidx Index of the subscriber + * \return Returns the found Tcpros client index on success or -1 on failure (e.g., No Tcpros client available) + */ +int cRosNodeRecruitTcprosClientProc(CrosNode *node, int subidx); + +/*! \brief Search for a Tcpros client proc that is currently assigned + * to the specified subscriber and is connected to the specified hostname and port + * + * subidx, tcpros_hostname and tcpros_port set the search condition for the Tcpros client proc + * \param node Pointer to CrosNode structure that has previously been created with cRosNodeCreate + * \param subidx Index of the subscriber or -1 for any subscriber + * \param tcpros_hostname Pointer to the hostname string or NULL for any hostname + * \param tcpros_port Port number of -1 for any port + * \return Returns the found Tcpros client index on success or -1 on failure (e.g., No Tcpros client matching the search criteria) + */ +int cRosNodeFindFirstTcprosClientProc(CrosNode *node, int subidx, const char *tcpros_hostname, int tcpros_port); + +void restartAdversing(CrosNode* node); +int enqueueRequestTopic(CrosNode *node, int subidx, const char *host, int port); +int enqueueMasterApiCall(CrosNode *node, RosApiCall *call); +int enqueueSlaveApiCall(CrosNode *node, RosApiCall *call, const char *host, int port); + +void getMsgFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *topic_type); +void getSrvFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *service_type); + +#endif // _CROS_NODE_INTERNAL_H_ diff --git a/src/hal/components/cros/include/cros_clock.h b/src/hal/components/cros/include/cros_clock.h new file mode 100644 index 00000000..c513bba2 --- /dev/null +++ b/src/hal/components/cros/include/cros_clock.h @@ -0,0 +1,59 @@ +#ifndef _CROS_CLOCK_H_ +#define _CROS_CLOCK_H_ + +// declaration of struct timeval: +#ifdef _WIN32 +# include +#else +# include +#endif + +#include + +/*! \defgroup cros_clock cROS clock + * + * Utility functions for time management + */ + +/*! \addtogroup cros_clock + * @{ + */ + +/*! \brief Return the current time, expressed as seconds and microseconds since the Epoch + * + * \return The current time + */ +struct timeval cRosClockGetTimeSecUsec( void ); + +/*! \brief Return the current time, expressed as milliseconds since the Epoch + * + * \return The current time + */ +uint64_t cRosClockGetTimeMs( void ); + +/*! \brief Convert an interval expressed as milliseconds in a timeval structure, + * that express the same interval as seconds and microseconds + * + * \return The time interval express with a timeval structure + */ +struct timeval cRosClockGetTimeVal( uint64_t msec ); + +/*! \brief Get a high-precision real-time time stamp from the system. + * Time stamps can be substracted to obtain time-stamp differences, which + * can be converted to us using cRosClockGetTimeStamp() + * + * \return The time interval expressed in arbitrary units + */ +int64_t cRosClockGetTimeStamp(void); + +/*! \brief Convert a time stamp in arbitrary units to us. + * These time stamps can be obtained by using the cRosClockGetTimeStamp() function. + * Time differences obtained substracting these time stamps can be also converted. + * + * \return The time stamp (or difference) in microseconds. + */ +double cRosClockTimeStampToUSec(int64_t time_stamp); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/cros_defs.h b/src/hal/components/cros/include/cros_defs.h new file mode 100644 index 00000000..f57d73eb --- /dev/null +++ b/src/hal/components/cros/include/cros_defs.h @@ -0,0 +1,77 @@ +#ifndef _CROS_DEFS_H_ +#define _CROS_DEFS_H_ + +#include +#include "cros_node.h" + +/*! \defgroup cros_defs Cros defintions */ + +/*! \addtogroup cros_defs + * @{ + */ + +#ifndef CROS_DEBUG_LEVEL +# define CROS_DEBUG_LEVEL 1 +#endif + +#if CROS_DEBUG_LEVEL >= 1 +# define PRINT_INFO(...) fprintf(cRosOutStreamGet(),__VA_ARGS__) +#else +# define PRINT_INFO(...) +#endif + +#if CROS_DEBUG_LEVEL >= 2 +# define PRINT_DEBUG(...) printf(__VA_ARGS__) +#else +# define PRINT_DEBUG(...) +#endif + +#if CROS_DEBUG_LEVEL >= 3 +# define PRINT_VDEBUG(...) printf(__VA_ARGS__) +#else +# define PRINT_VDEBUG(...) +#endif + +#if CROS_DEBUG_LEVEL >= 4 +# define PRINT_VVDEBUG(...) printf(__VA_ARGS__) +#else +# define PRINT_VVDEBUG(...) +#endif + +#define PRINT_ERROR(...) fprintf(cRosOutStreamGet(), __VA_ARGS__) + +#define FLUSH_PRINT() fflush(cRosOutStreamGet()) + +// macros for detecting and converting endianness + +// We ignore the existence of mixed endianness +#ifdef _WIN32 +# define LITTLE_ENDIAN_ARC 1 +#else +# ifdef __APPLE__ +# ifndef __BIG_ENDIAN__ +# define LITTLE_ENDIAN_ARC 1 +# endif +# else +# include +# if __BYTE_ORDER == __LITTLE_ENDIAN +# define LITTLE_ENDIAN_ARC 1 +# endif +# endif +#endif + +// We only support processors with sizeof(char)==1 +#if LITTLE_ENDIAN_ARC +# define HOST_TO_ROS_UINT32(val) (val) +#else +# define HOST_TO_ROS_UINT32(val) (((val)>>24)&0x000000FFUL) | \ + (((val)<<8)&0X00FF0000UL) | \ + (((val)>>8)&0X0000FF00UL) | \ + (((val)<<24)&0XFF000000UL) +#endif + +#define ROS_TO_HOST_UINT32 HOST_TO_ROS_UINT32 + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/cros_err_codes.h b/src/hal/components/cros/include/cros_err_codes.h new file mode 100644 index 00000000..c4ee397d --- /dev/null +++ b/src/hal/components/cros/include/cros_err_codes.h @@ -0,0 +1,162 @@ +/// \file cros_err_codes.h +/// \brief This header file define the codes for the error messages of the cROS library. +/// The functions for managing these codes are declared as well. +/// +/// \author Richard R. Carrillo, Aging in Vision and Action lab, Institut de la Vision, Sorbonne University, Paris, France. + +#ifndef __CROS_ERR_CODES_H__ +#define __CROS_ERR_CODES_H__ +#include + +///////////////////////////////// ERROR messages ///////////////////////////////// +//! Definition of all the error codes. One code per line. Up to 255 error codes +#define ERROR_CODE_LIST_DEF \ + MSG_COD_ELEM(CROS_NO_ERR, "Operation completed successfully") \ + MSG_COD_ELEM(CROS_UNSPECIFIED_ERR, "An unspecified error occurred") \ + MSG_COD_ELEM(CROS_MEM_ALLOC_ERR, "Error allocating memory") \ + MSG_COD_ELEM(CROS_BAD_PARAM_ERR, "An invalid value has been specified for at least one of the input parameters") \ + MSG_COD_ELEM(CROS_OPEN_MSG_FILE_ERR, "The message file definition the cannot be opened") \ + MSG_COD_ELEM(CROS_READ_MSG_FILE_ERR, "Error reading the message definition file") \ + MSG_COD_ELEM(CROS_LOAD_MSG_FILE_ERR, "Error loading or processing the message definition file") \ + MSG_COD_ELEM(CROS_OPEN_SVC_FILE_ERR, "The file defining the service cannot be opened") \ + MSG_COD_ELEM(CROS_READ_SVC_FILE_ERR, "Error reading the service definition file") \ + MSG_COD_ELEM(CROS_UNREG_TIMEOUT_ERR, "The unregistration from the ROS master was abandoned before it finished because it was taking too long") \ + MSG_COD_ELEM(CROS_SELECT_FD_ERR, "An error ocurred while monitoring the socket file descriptors (select() function)") \ + MSG_COD_ELEM(CROS_MANY_PARAM_ERR, "The maximum number of paramter subscriptions has been reached") \ + MSG_COD_ELEM(CROS_PARAM_SUB_IND_ERR, "The provided parameter subscriber index does not corresponds to a valid subscriber to be unsubscribed") \ + MSG_COD_ELEM(CROS_TOPIC_PUB_IND_ERR, "The provided topic publisher index does not corresponds to a valid publisher to be unregistered") \ + MSG_COD_ELEM(CROS_TOPIC_SUB_IND_ERR, "The provided topic subscriber index does not corresponds to a valid subscriber to be unregistered") \ + MSG_COD_ELEM(CROS_SVC_FILE_DELIM_ERR, "The delimiter string between resquest and response definition could not be found in the service definition file") \ + MSG_COD_ELEM(CROS_FILE_ENTRY_TYPE_ERR, "The definition file contains an entry that specifies and incorrect data type") \ + MSG_COD_ELEM(CROS_LOAD_SVC_FILE_REQ_ERR, "Error loading the service request definition in the file") \ + MSG_COD_ELEM(CROS_LOAD_SVC_FILE_RES_ERR, "Error loading the service response definition in the file") \ + MSG_COD_ELEM(CROS_CREATE_CUSTOM_MSG_ERR, "Error loading the specified custom message definition file or creating a message of its type") \ + MSG_COD_ELEM(CROS_FILE_ENTRY_NO_SEP_ERR, "The definition file contains an entry that is syntactically incorrect (a white space is expected between data type and data name)") \ + MSG_COD_ELEM(CROS_DEPACK_INSUFF_DAT_ERR, "Error decoding a received packet: The length of the packet is too small for the expected data type") \ + MSG_COD_ELEM(CROS_DEPACK_NO_MSG_DEF_ERR, "Error decoding a received packet: The definition of the custom message does not corresponds to the fields of the message to send") \ + MSG_COD_ELEM(CROS_TOP_PUB_CALLBACK_ERR, "The callback function specified for a topic publisher returned a non-zero value") \ + MSG_COD_ELEM(CROS_TOP_SUB_CALLBACK_ERR, "The callback function specified for a topic subscriber returned a non-zero value") \ + MSG_COD_ELEM(CROS_SVC_REQ_CALLBACK_ERR, "The callback function specified for a service client returned a non-zero value when generating the service request") \ + MSG_COD_ELEM(CROS_SVC_RES_CALLBACK_ERR, "The callback function specified for a service client returned a non-zero value when generating the service response") \ + MSG_COD_ELEM(CROS_SVC_SER_CALLBACK_ERR, "The callback function specified for a service server returned a non-zero value") \ + MSG_COD_ELEM(CROS_SVC_RES_OK_BYTE_ERR, "The response received from the service server contains an 'ok' byte codifying a value different from true (1)") \ + MSG_COD_ELEM(CROS_RCV_TOP_TIMEOUT_ERR, "The specified timeout was up while waiting for a topic message") \ + MSG_COD_ELEM(CROS_SEND_TOP_TIMEOUT_ERR, "The specified timeout was up while waiting for space in the message transmission queue") \ + MSG_COD_ELEM(CROS_CALL_SVC_TIMEOUT_ERR, "The specified timeout was up while waiting for the service call response") \ + MSG_COD_ELEM(CROS_CALL_INI_TIMEOUT_ERR, "The specified timeout was up while the service caller was waiting for the previous call to finish") \ + MSG_COD_ELEM(CROS_XMLRPC_CLI_CONN_ERR, "An error occurred when an XMLRPC client process was trying to establish the connection to the target address") \ + MSG_COD_ELEM(CROS_XMLRPC_CLI_REFUS_ERR, "An XMLRPC client process could not establish the connection to the target address because the connection was refused") \ + MSG_COD_ELEM(CROS_XMLRPC_CLI_WRITE_ERR, "An error arised when the XMLRPC client process tried to write the request on the socket") \ + MSG_COD_ELEM(CROS_XMLRPC_CLI_READ_ERR, "An error arised when the XMLRPC client process tried to read the response from the socket") \ + MSG_COD_ELEM(CROS_TCPROS_CLI_CONN_ERR, "A TCPROS client process could not establish the connection to the target address due to an error") \ + MSG_COD_ELEM(CROS_TCPROS_CLI_REFUS_ERR, "An TCPROS client process could not establish the connection to the target address because the connection was refused") \ + MSG_COD_ELEM(CROS_RPCROS_CLI_CONN_ERR, "A RPCROS client process could not establish the connection to the target address due to an error") \ + MSG_COD_ELEM(CROS_RPCROS_CLI_REFUS_ERR, "An RPCROS client process could not establish the connection to the target address because the connection was refused") \ + MSG_COD_ELEM(CROS_SOCK_OPEN_TIMEOUT_ERR, "The specified timeout was up while waiting for the specified port to be open") \ + MSG_COD_ELEM(CROS_SOCK_OPEN_CONN_ERR, "An error occurred when the specified target port was tried to be connected (target address could not be resolved?)") \ + MSG_COD_ELEM(CROS_EXTRACT_MSG_INT_ERR, "An internal error occurred when sending an inmediate message: The message could not be extracted from the queue") \ + MSG_COD_ELEM(LAST_ERR_LIST_CODE, "") // Sentinel code used to mark the last element of the global error list + +#define CROS_SUCCESS_ERR_PACK 0U //! Function return value indicating success + +// Declare the enum data type used for encoding error codes and declare the error code for errors (CROS_NO_ERR,...) +#define MSG_COD_ELEM(code,msg) code, +enum cRosErrorCode { ERROR_CODE_LIST_DEF }; +#undef MSG_COD_ELEM + +typedef enum cRosErrorCode cRosErrCode; +// Declare an entry type of the message list variable that will contain the global list of error messages +struct cRosErrCodeListElem +{ + cRosErrCode code; + const char *msg; +}; + +//! Error data type returned by many cROS library functions. It is a pack that can contain up to 4 accumulated error codes (uint8_t) +typedef uint32_t cRosErrCodePack; + + +/*! \brief Locates the message string corresponding to the specified error code in the global error message list. + * + * The function locates the message string describing the specified error code (a single error code, not a pack). + * \param err_code number of the error + * \return Pointer to the message string. If no message is found for the specified code, it returns NULL + */ +const char *cRosGetErrCodeStr(cRosErrCode err_code); + +/*! \brief Add a new error code to the specified error code pack. + * + * This function is intended to add more information (a new error code) to an error code pack. + * If the new error code is CROS_NO_ERR (0), nothing is added, that is, the prev_err_pack is returned. + * parameter list. + * \param prev_err_pack is the original error code pack. + * \param err_code number of the new error. + * \return the new error pack (containing the specified err_code if err_code was not CROS_NO_ERR) + */ +cRosErrCodePack cRosAddErrCode(cRosErrCodePack prev_err_pack, cRosErrCode err_code); + +/*! \brief Add a new error code to the specified non-empty error code pack. + * + * This function is intended to add more information (a new error code) to an error code pack that already contains errors. + * If the specified error code pack does not contain any error, this function returns the same error code pack specified in the + * parameter list. + * \param prev_err_pack is the original error code pack. + * \param err_code number of the new error. If this code is CROS_NO_ERR, prev_err_pack is returned + * \return the new error pack (containing the specified err_code if prev_err_pack was not initially CROS_SUCCESS_ERR_PACK) + */ +cRosErrCodePack cRosAddErrCodeIfErr(cRosErrCodePack prev_err_pack, cRosErrCode err_code); + +/*! \brief Remove the last error code inserted in the specified error code pack. + * + * \param err_pack the original error code pack. + * \return the new error code pack with the last code removed, or the original pack if no error code was contained in it. + */ +cRosErrCodePack cRosRemoveLastErrCode(cRosErrCodePack prev_err_pack); + +/*! \brief Obtain the last error code inserted in the specified error code pack. + * + * \param err_pack the error code pack. + * \return the last error code of the pack or CROS_NO_ERR (0) if no error is codified in the pack + */ +cRosErrCode cRosGetLastErrCode(cRosErrCodePack err_pack); + +/*! \brief Compile the error codes contained in two error code packs into a single error code pack. + * + * This function is intended to add new error codes to an error code pack that already may contain errors. + * If the specified error code packs do not contain any error, this function returns an empty code pack (CROS_SUCCESS_ERR_PACK). + * \param prev_err_pack_0 is the first error code pack. + * \param prev_err_pack_1 is the second error code pack. + * \return the new error pack joining the two error packs + */ +cRosErrCodePack cRosAddErrCodePackIfErr(cRosErrCodePack prev_err_pack_0, cRosErrCodePack prev_err_pack_1); + +/*! \brief Composes and print the error message corresponding to the specified error code pack. + * + * The function search for the error strings indicated in the specified error pack. The composed message is printed + * in the output file stream. The error pack can contain up to 4 error codes, so up to 4 message string can be printed. + * Additionally the text string specified by fmt_str is printed before the error strings. This string can be used to + * supply the user with context information about the error. + * \param err structure encoding the occurred error (usually returned by a failing function). + * \param fmt_str string that specifies how subsequent arguments are printed. It has the same format as printf + * function. + * \return the number of characters written in the output stream. + */ +int cRosPrintErrCodePack(cRosErrCodePack err_cod_pack, const char *fmt_str, ...); + +/*! \brief Composes the error message corresponding to the specified error code pack and writes it to the specified buffer. + * + * The function search for the error strings indicated in the specified error pack. The error pack can contain up to 4 error + * codes, so up to 4 message string can be printed. + * Additionally the text string specified by fmt_str is printed before the error strings. This string can be used to + * supply the user with context information about the error. + * If not enough space if available in the output buffer the output is truncated and a '\0' is added at the end of the buffer. + * \param out_str_buf Pointer to the buffer where the output string will be stored. + * \param out_str_buf_len Length of the output buffer. + * \param err structure encoding the occurred error (usually returned by a failing function). + * \param fmt_str string that specifies how subsequent arguments are printed. It has the same format as printf + * function. + * \return the number of characters that would be written to the output buffer if it had enough space. + */ +int cRosErrCodePackStr(char *out_str_buf, size_t out_str_buf_len, cRosErrCodePack err_cod_pack, const char *fmt_str, ...); + +#endif /* __CROS_ERR_CODES_H__ */ diff --git a/src/hal/components/cros/include/cros_gentools.h b/src/hal/components/cros/include/cros_gentools.h new file mode 100644 index 00000000..d9df0c45 --- /dev/null +++ b/src/hal/components/cros/include/cros_gentools.h @@ -0,0 +1,37 @@ +#ifndef _CROS_GENTOOLS_H_ +#define _CROS_GENTOOLS_H_ + +/*! \defgroup cros_gentools cROS message generation tool + * + * Implemenation of the ROS message generation tool and + * dependency resolutor (gentools and gendeps) + */ + +/*! \addtogroup cros_gentool + * @{ + */ + +/*! \brief Generate md5 hash of files + * + * \param filename Full path of the message/service file + */ +char* cRosGentoolsMD5(char* filename); + +/*! \brief Generate SHA1 hash of files + * + * \param filename Full path of the message/service file + * + * \return Returns 1 on success, 0 on failure + */ +int cRosGentoolsSHA1(char* filename); + +/*! \brief Generate concatenated list of files + * + * \param filename Full path of the message/service file + * + */ +int cRosGentoolsFulltext(char* filename); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/cros_log.h b/src/hal/components/cros/include/cros_log.h new file mode 100644 index 00000000..d99531a2 --- /dev/null +++ b/src/hal/components/cros/include/cros_log.h @@ -0,0 +1,67 @@ +/*! \file cros_log.h + * \brief This file declares the function and macros (ROS_INFO, ROS_DEBUG, ROS_WARN, ROS_ERROR and ROS_FATAL) + * for printing messages using the ROS log. These messages are sent through the /rosout topic to the /rosout node. + * + * These macros must not be confused with the macros for printing messages localy (either in a local log file or local console) which + * are defined in cros_defs.h: PRINT_INFO, PRINT_DEBUG, PRINT_VDEBUG, PRINT_VVDEBUG and PRINT_ERROR + */ + +#ifndef _CROS_LOG_H_ +#define _CROS_LOG_H_ + +#include +#include + +#define PRINT_LOG(node,log_level,...) \ + cRosLogPrint(node,\ + log_level,\ + __FILE__,\ + __FUNCTION__,\ + __LINE__,\ + __VA_ARGS__) + +#define ROS_INFO(node,...) PRINT_LOG(node, CROS_LOGLEVEL_INFO, __VA_ARGS__) +#define ROS_DEBUG(node,...) PRINT_LOG(node, CROS_LOGLEVEL_DEBUG, __VA_ARGS__) +#define ROS_WARN(node,...) PRINT_LOG(node, CROS_LOGLEVEL_WARN, __VA_ARGS__) +#define ROS_ERROR(node,...) PRINT_LOG(node, CROS_LOGLEVEL_ERROR, __VA_ARGS__) +#define ROS_FATAL(node,...) PRINT_LOG(node, CROS_LOGLEVEL_FATAL, __VA_ARGS__) + +typedef struct CrosLog CrosLog; + +struct CrosNode; // We forward declare CrosNode struct since it is used by cRosLogPrint() before it is declared + +struct CrosLog +{ + uint8_t level; //! debug level + char* name; //! name of the node + char* msg; //! message + char* file; //! file the message came from + char* function; //! function the message came from + uint32_t line; //! line the message came from + char** pubs; //! topic names that the node publishes + uint32_t secs; + uint32_t nsecs; + size_t n_pubs; +}; + +typedef enum CrosLogLevel //!Logging levels +{ + CROS_LOGLEVEL_DEBUG = 1, + CROS_LOGLEVEL_INFO = 2, + CROS_LOGLEVEL_WARN = 4, + CROS_LOGLEVEL_ERROR = 8, + CROS_LOGLEVEL_FATAL = 16 +} CrosLogLevel; + +CrosLog *cRosLogNew(void); +void cRosLogFree(CrosLog *log); +int stringToLogLevel(const char* level_str, CrosLogLevel *level_num); +const char *LogLevelToString(CrosLogLevel log_level); +void cRosLogPrint(struct CrosNode *node, + CrosLogLevel level, // debug level + const char *file, // file the message came from + const char *function, // function the message came from + uint32_t line, + const char *msg, ...); + +#endif //_CROS_LOG_H diff --git a/src/hal/components/cros/include/cros_message.h b/src/hal/components/cros/include/cros_message.h new file mode 100644 index 00000000..ab748985 --- /dev/null +++ b/src/hal/components/cros/include/cros_message.h @@ -0,0 +1,209 @@ +#ifndef _CROS_MESSAGE_H_ +#define _CROS_MESSAGE_H_ + +#include "dyn_buffer.h" +#include "cros_err_codes.h" + +/*! \defgroup cros_message cROS TCPROS + * + * Implemenation of the TCPROS protocol for topic message exchanges + */ + +/*! \addtogroup cros_message + * @{ + */ + +typedef enum CrosMessageType +{ + CROS_CUSTOM_TYPE = 0, + CROS_STD_MSGS_INT8, + CROS_STD_MSGS_UINT8, + CROS_STD_MSGS_INT16, + CROS_STD_MSGS_UINT16, + CROS_STD_MSGS_INT32, + CROS_STD_MSGS_UINT32, + CROS_STD_MSGS_INT64, + CROS_STD_MSGS_UINT64, + CROS_STD_MSGS_FLOAT32, + CROS_STD_MSGS_FLOAT64, + CROS_STD_MSGS_STRING, + CROS_STD_MSGS_BOOL, + CROS_STD_MSGS_TIME, + CROS_STD_MSGS_DURATION, + CROS_STD_MSGS_HEADER, + // deprecated + CROS_STD_MSGS_CHAR, + CROS_STD_MSGS_BYTE +} CrosMessageType; + +typedef struct cRosMessageField cRosMessageField; +typedef struct cRosMessage cRosMessage; + +struct cRosMessageField +{ + char *name; + + union data_t + { + uint8_t opaque[8]; + int8_t as_int8; + uint8_t as_uint8; + int16_t as_int16; + uint16_t as_uint16; + int32_t as_int32; + uint32_t as_uint32; + int64_t as_int64; + uint64_t as_uint64; + float as_float32; + double as_float64; + char *as_string; + cRosMessage *as_msg; + int8_t *as_int8_array; + uint8_t *as_uint8_array; + int16_t *as_int16_array; + uint16_t *as_uint16_array; + int32_t *as_int32_array; + uint32_t *as_uint32_array; + int64_t *as_int64_array; + uint64_t *as_uint64_array; + float *as_float32_array; + double *as_float64_array; + char **as_string_array; + cRosMessage **as_msg_array; + void *as_array; + } data; + int size; + int is_const; + int is_array; + int is_fixed_array; + int array_size; + int array_capacity; + CrosMessageType type; + char *type_s; +}; + +typedef struct t_msgDef cRosMessageDef; + +struct cRosMessage +{ + cRosMessageField **fields; + cRosMessageDef *msgDef; + char *md5sum; + int n_fields; +}; + +cRosMessage * cRosMessageNew(void); + +void cRosMessageInit(cRosMessage *message); + +cRosErrCodePack cRosMessageDefBuild(cRosMessageDef **msg_def_ptr, const char *msg_root_dir, const char *msg_type); + +cRosErrCodePack cRosMessageNewBuild(const char *msg_root_dir, const char *msg_type, cRosMessage **new_msg_ptr); + +void cRosMessageFieldsPrint(cRosMessage *msg, int n_indent); + +int cRosMessageFieldCopy(cRosMessageField *new_field, cRosMessageField *orig_field); + +int cRosMessageFieldsCopy(cRosMessage *m_dst, cRosMessage *m_src); + +cRosMessage *cRosMessageCopyWithoutDef(cRosMessage *m_src); + +cRosMessage *cRosMessageCopy(cRosMessage *m_src); + +cRosErrCodePack cRosMessageBuildFromDef(cRosMessage **message, cRosMessageDef *msg_def ); + +void cRosMessageFree(cRosMessage *message); + +void cRosMessageFieldsFree(cRosMessage *message); + +void cRosMessageRelease(cRosMessage *message); + +cRosMessageField * cRosMessageFieldNew(void); + +void cRosMessageFieldInit(cRosMessageField *field); + +void cRosMessageFieldRelease(cRosMessageField *field); + +void cRosMessageFieldFree(cRosMessageField *field); + +cRosMessageField * cRosMessageGetField(cRosMessage *message, const char *field); + +int cRosMessageSetFieldValueString(cRosMessageField *field, const char *value); + +int cRosMessageFieldArrayPushBackInt8(cRosMessageField *field, int8_t val); + +int cRosMessageFieldArrayPushBackInt16(cRosMessageField *field, int16_t val); + +int cRosMessageFieldArrayPushBackInt32(cRosMessageField *field, int32_t val); + +int cRosMessageFieldArrayPushBackInt64(cRosMessageField *field, int64_t val); + +int cRosMessageFieldArrayPushBackUInt8(cRosMessageField *field, uint8_t val); + +int cRosMessageFieldArrayPushBackUInt16(cRosMessageField *field, uint16_t val); + +int cRosMessageFieldArrayPushBackUInt32(cRosMessageField *field, uint32_t val); + +int cRosMessageFieldArrayPushBackUInt64(cRosMessageField *field, uint64_t val); + +int cRosMessageFieldArrayPushBackFloat32(cRosMessageField *field, float val); + +int cRosMessageFieldArrayPushBackFloat64(cRosMessageField *field, double val); + +int cRosMessageFieldArrayPushBackString(cRosMessageField *field, const char *val); + +int cRosMessageFieldArrayPushBackZero(cRosMessage *msg, int n_field); + +int cRosMessageFieldArrayPushBackMsg(cRosMessageField *field, cRosMessage *msg); + +cRosMessage *cRosMessageFieldArrayRemoveLastMsg(cRosMessageField *field); + +int8_t * cRosMessageFieldArrayAtInt8(cRosMessageField *field, int position); + +int16_t * cRosMessageFieldArrayAtInt16(cRosMessageField *field, int position); + +int32_t * cRosMessageFieldArrayAtInt32(cRosMessageField *field, int position); + +int64_t * cRosMessageFieldArrayAtInt64(cRosMessageField *field, int position); + +uint8_t * cRosMessageFieldArrayAtUInt8(cRosMessageField *field, int position); + +uint16_t * cRosMessageFieldArrayAtUInt16(cRosMessageField *field, int position); + +uint32_t * cRosMessageFieldArrayAtUInt32(cRosMessageField *field, int position); + +uint64_t * cRosMessageFieldArrayAtUInt64(cRosMessageField *field, int position); + +float * cRosMessageFieldArrayAtFloat32(cRosMessageField *field, int position); + +double * cRosMessageFieldArrayAtFloat64(cRosMessageField *field, int position); + +char *cRosMessageFieldArrayAtStringGet(cRosMessageField *field, int position); + +int cRosMessageFieldArrayAtStringSet(cRosMessageField *field, int position, const char *val); + +cRosMessage *cRosMessageFieldArrayAtMsgGet(cRosMessageField *field, int position); + +int cRosMessageFieldArrayAtMsgSet(cRosMessageField *field, int position, cRosMessage *val); + +int cRosMessageFieldArrayClear(cRosMessageField *field); + +size_t cRosMessageSize(cRosMessage *message); + +cRosErrCodePack cRosMessageSerialize(cRosMessage *message, DynBuffer *buffer); + +cRosErrCodePack cRosMessageDeserialize(cRosMessage *message, DynBuffer *buffer); + +CrosMessageType getMessageType(const char *type); + +const char * getMessageTypeString(CrosMessageType type); + +const char * getMessageTypeDeclaration(CrosMessageType type); + +size_t getMessageTypeSizeOf(CrosMessageType type); + +int isBuiltinMessageType(CrosMessageType type); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/cros_message_internal.h b/src/hal/components/cros/include/cros_message_internal.h new file mode 100644 index 00000000..efb799ec --- /dev/null +++ b/src/hal/components/cros/include/cros_message_internal.h @@ -0,0 +1,119 @@ +#ifndef _CROS_MESSAGE_INTERNAL_H_ +#define _CROS_MESSAGE_INTERNAL_H_ + +#include "dyn_string.h" +#include "cros_err_codes.h" + +static const char* FILEEXT_MSG = "msg"; + +// e.g. std_msgs/String +static const char* CHAR_SEP = "/"; + +// character that designates a constant assignment rather than a field +static const char* CHAR_CONST = "="; + +// char that denotes the comment line start +static const char* CHAR_COMMENT = "#"; + +static const char* HEADER_DEFAULT_PACK = "std_msgs"; +static const char* HEADER_DEFAULT_NAME = "Header"; +static const char* HEADER_DEFAULT_TYPE = "std_msgs/Header"; + +static const char* HEADER_DEFAULT_TYPEDEF = +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.secs: seconds (stamp_secs) since epoch\n" +"# * stamp.nsecs: nanoseconds since stamp_secs\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"# 0: no frame\n" +"string frame_id\n\n"; + + +struct t_msgFieldDef +{ + CrosMessageType type; + char* type_s; + char* name; + int is_array; + int array_size; + struct t_msgFieldDef* prev; + struct t_msgFieldDef* next; + cRosMessageDef* child_msg_def; // Stores the definition of a type defined trough a custom message file +}; + +typedef struct t_msgFieldDef msgFieldDef; + +struct t_msgConst +{ + CrosMessageType type; + char* type_s; + char* name; + char* value; + struct t_msgConst* prev; + struct t_msgConst* next; +}; + +typedef struct t_msgConst msgConst; + +struct t_msgDef +{ + char* name; + char* package; + char* root_dir; + char* plain_text; + msgFieldDef* fields; + msgFieldDef* first_field; + msgConst* constants; + msgConst* first_const; +}; + +typedef struct t_msgDef cRosMessageDef; + +struct t_msgDep +{ + cRosMessageDef* msg; + struct t_msgDep* prev; + struct t_msgDep* next; +}; + +typedef struct t_msgDep msgDep; + +cRosErrCodePack getFileDependenciesMsg(char* filename, cRosMessageDef* msg, msgDep* deps); + +// Compute full text of message, including text of embedded +// types. The text of the main msg is listed first. Embedded +// msg files are denoted first by an 80-character '=' separator, +// followed by a type declaration line,'MSG: pkg/type', followed by +// the text of the embedded type. +char* computeFullTextMsg(cRosMessageDef* msg, msgDep* deps); + +cRosErrCodePack getDependenciesMsg(cRosMessageDef* msg, msgDep* msgDeps); + +void cRosMD5Readable(unsigned char* data, DynString* output); + +cRosErrCodePack getMD5Txt(cRosMessageDef* msg, DynString* buffer); + +cRosErrCodePack initCrosMsg(cRosMessageDef* msg); + +void initMsgConst(msgConst *msg); + +void initCrosDep(msgDep* dep); + +void initFieldDef(msgFieldDef* field); + +cRosErrCodePack loadFromStringMsg(char* text, cRosMessageDef* msg); + +cRosErrCodePack loadFromFileMsg(char* filename, cRosMessageDef* msg); + +cRosErrCodePack cRosMessageDefCopy(cRosMessageDef** ptr_new_msg_def, cRosMessageDef* orig_msg_def ); + +void cRosMessageDefFree(cRosMessageDef *msgDef); + +#endif // _CROS_MESSAGE_INTERNAL_H_ diff --git a/src/hal/components/cros/include/cros_message_queue.h b/src/hal/components/cros/include/cros_message_queue.h new file mode 100644 index 00000000..6db796b9 --- /dev/null +++ b/src/hal/components/cros/include/cros_message_queue.h @@ -0,0 +1,122 @@ +/*! \file cros_message_queue.h + * \brief This header file declares the cRosMessageQueue type and associated management functions + * + * cRosMessageQueue implements a queue of TCPROS-protocol messages. This queue behaves as a + * FIFO (First In First Out). + * This queue only stores the fields of the messages ('fields' fields of the struct), not the message definition (msgDef field). + * \author Richard R. Carrillo. Aging in Vision and Action lab, Institut de la Vision, Sorbonne University, Paris, France. + * \date 31 Oct 2017 + */ + +#ifndef _CROS_MESSAGE_QUEUE_H_ +#define _CROS_MESSAGE_QUEUE_H_ + +#include "cros_message.h" + + +#define MAX_QUEUE_LEN 10 //! Maximum number of messages that can be hold in the queue + +struct cRosMessageQueue +{ + cRosMessage msgs[MAX_QUEUE_LEN]; //! Content of the queue + unsigned int length; //! Number of messages currently in the queue + unsigned int first_msg_ind; //! Index of the oldest message in the queue (the one that was inserted first) +}; + +typedef struct cRosMessageQueue cRosMessageQueue; + +/*! \brief Initializes a queue. + * + * This function must be called before using a queue for the first time. + * \param q Pointer to the queue. + */ +void cRosMessageQueueInit(cRosMessageQueue *q); + +/*! \brief Empty queue. + * + * This function removes all the messages in a queue. + * \param q Pointer to the queue. + */ +void cRosMessageQueueClear(cRosMessageQueue *q); + +/*! \brief Deletes the content of a queue and free all its allocated memory. + * + * This function frees the memory allocated for its content so cRosMessageQueueInit() must be called before + * using the queue again. + * \param q Pointer to the queue. + */ +void cRosMessageQueueRelease(cRosMessageQueue *q); + +/*! \brief Calculates the free space still available in the queue. + * + * \return Number of messages that can still be added to the queue without causing an overflow. + */ +unsigned int cRosMessageQueueVacancies(cRosMessageQueue *q); + +/*! \brief Calculates the number of messages stored in the queue. + * + * \return Number of messages that can be removed from the queue. + */ +unsigned int cRosMessageQueueUsage(cRosMessageQueue *q); + +/*! \brief Add a new message at the end of the queue. + * + * This function adds a new element (message) at the end of the queue. The fields of the message pointed by m will be copied + * in internal memory of the queue, so the message pointed by m can be freed after being added. + * \param q Pointer to the queue. + * \param m Pointer to the message to be added. + * \return 0 on success, otherwise an error code: -1 = error allocating memory, -2 = No free space to add a new element. + */ +int cRosMessageQueueAdd(cRosMessageQueue *q, cRosMessage *m); + +/*! \brief Extract the first message of the queue. + * + * This function removes a element (message) at the start of the queue. The fields of the message at the start of the queue will be copied + * to the message pointed by m, so the message pointed by m should be freed independently after being used. + * \param q Pointer to the queue. + * \param m Pointer to the message where the fields of the removed message are copied. + * \return 0 on success, otherwise an error code: -1 = error allocating memory, -2 = No messages in the queue. If an error occurs, the + * message is not removed from the queue. + */ +int cRosMessageQueueExtract(cRosMessageQueue *q, cRosMessage *m); + +/*! \brief Get a copy of the first message from the queue. + * + * This function obtains the element (message) at the start of the queue. The fields of the message at the start of the queue will be copied + * to the message pointed by m, so the message pointed by m should be freed independently after being used. + * The queue is not modified. + * \param q Pointer to the queue. + * \param m Pointer to the message where the fields of the removed message are copied. + * \return 0 on success, otherwise an error code: -1 = error allocating memory, -2 = No messages in the queue. + */ +int cRosMessageQueueGet(cRosMessageQueue *q, cRosMessage *m); + +/*! \brief Delete the first message from the queue. + * + * This function removes a element (message) at the start of the queue. + * \param q Pointer to the queue. + * \return 0 on success, otherwise an error code: -2 = No messages in the queue. + */ +int cRosMessageQueueRemove(cRosMessageQueue *q); + +/*! \brief Get a reference of the first message from the queue. + * + * This function obtains a pointer to the element (message) at the start of the queue (oldest element). The fields of the message at the + * start of the queue will not be copied, so if the message queue is modified after obtaining this pointer, the pointer become invalid. + * The queue is not modified. + * \param q Pointer to the queue. + * \return Pointer to the first message. If the first message could not be obtained, NULL is returned. + */ +cRosMessage *cRosMessageQueuePeekFirst(cRosMessageQueue *q); + +/*! \brief Get a reference of the last message from the queue. + * + * This function obtains a pointer to the element (message) at the end of the queue (newest element). The fields of this message + * of the queue will not be copied, so if the message queue is modified after obtaining this pointer, the pointer become invalid. + * The queue is not modified by this function. + * \param q Pointer to the queue. + * \return Pointer to the last message. If the last message could not be obtained, NULL is returned. + */ +cRosMessage *cRosMessageQueuePeekLast(cRosMessageQueue *q); + +#endif // _CROS_MESSAGE_QUEUE_H_ diff --git a/src/hal/components/cros/include/cros_node.h b/src/hal/components/cros/include/cros_node.h new file mode 100644 index 00000000..56def9cb --- /dev/null +++ b/src/hal/components/cros/include/cros_node.h @@ -0,0 +1,344 @@ +/*! \file cros_node.h + * \brief This file declares the CrosNode structure, which codifies a ROS node in cROS. It also declares + * associated substructures, functions and macros. + * + * This ROS node can implement at the same time several: + * - Topic subscribers + * - Topic publishers + * - Service providers (servers) + * - Service callers (clients) + * - Parameter subscriber + */ + +#ifndef _CROS_NODE_H_ +#define _CROS_NODE_H_ + +#include +#include + +#include "cros_log.h" +#include "xmlrpc_process.h" +#include "tcpros_process.h" +#include "cros_api_call.h" +#include "cros_message_queue.h" +#include "cros_err_codes.h" + +/*! \defgroup cros_node cROS Node */ + +/*! \addtogroup cros_node + * @{ + */ + +/*! Max num published topics */ +#define CN_MAX_PUBLISHED_TOPICS 5 + +/*! Max num subscribed topics */ +#define CN_MAX_SUBSCRIBED_TOPICS 5 + +/*! Max num service providers */ +#define CN_MAX_SERVICE_PROVIDERS 8 + +/*! Max num service callers */ +#define CN_MAX_SERVICE_CALLERS 8 + +/*! Max num parameter subscriptions */ +#define CN_MAX_PARAMETER_SUBSCRIPTIONS 20 + +/*! Max num serving XMLRPC connections */ +#define CN_MAX_XMLRPC_SERVER_CONNECTIONS 5 + +/*! Max num serving TCPROS connections */ +#define CN_MAX_TCPROS_SERVER_CONNECTIONS 5 + +/*! Max num serving RPCROS connections */ +#define CN_MAX_RPCROS_SERVER_CONNECTIONS CN_MAX_SERVICE_PROVIDERS + +/*! + * Max num XMLRPC connections against another subscribed nodes + * (first connection index reserved to roscore) + * */ +#define CN_MAX_XMLRPC_CLIENT_CONNECTIONS 1 + CN_MAX_SUBSCRIBED_TOPICS + +/*! + * Max num TCPROS connections against another subscribed nodes + * */ +#define CN_MAX_TCPROS_CLIENT_CONNECTIONS CN_MAX_SUBSCRIBED_TOPICS + +/*! + * Max num RPCROS connections against other service-providing nodes + * (service calls are one to one, so, one TcprosProcess per ServiceCallerNode) + * */ +#define CN_MAX_RPCROS_CLIENT_CONNECTIONS CN_MAX_SERVICE_CALLERS + +/*! Node automatic XMLRPC ping cycle period (in msec) */ +#define CN_PING_LOOP_PERIOD 1000 + +/*! Maximum I/O operations timeout (in msec) */ +#define CN_IO_TIMEOUT 3000 + +/*! Maximum time that the node will wait for unregistering all publishers, subscribers, servicer providers... in the ROS master (in msec) */ +#define CN_UNREGISTRATION_TIMEOUT 3000 + +typedef struct PublisherNode PublisherNode; +typedef struct SubscriberNode SubscriberNode; +typedef struct ServiceProviderNode ServiceProviderNode; +typedef struct ServiceCallerNode ServiceCallerNode; +typedef struct ParameterSubscription ParameterSubscription; + +typedef enum CrosNodeStatus +{ + // TODO: this is a work in progress + CROS_STATUS_NONE = 0, + CROS_STATUS_PUBLISHER_UNREGISTERED, + CROS_STATUS_SUBSCRIBER_UNREGISTERED, + CROS_STATUS_SERVICE_PROVIDER_UNREGISTERED, + CROS_STATUS_PARAM_UNSUBSCRIBED, + CROS_STATUS_PARAM_SUBSCRIBED, + CROS_STATUS_PARAM_UPDATE, +} CrosNodeStatus; + +typedef struct CrosNodeStatusUsr +{ + // FIXME: this is a work in progress + // int callid; // This may be useful to track register/unregister + CrosNodeStatus state; // May be useful to understand what the node, or the particular sub/pub/svc is doing + int provider_idx; + const char *xmlrpc_host; + int xmlrpc_port; + const char *parameter_key; + XmlrpcParam *parameter_value; +} CrosNodeStatusUsr; + +/*! \brief Callback to communicate publisher or subscriber status */ +typedef void (*NodeStatusApiCallback)(CrosNodeStatusUsr *status, void* context); + +/*! Structure that define a published topic */ +struct PublisherNode +{ + char *topic_name; //! The published topic name + char *topic_type; //! The published topic data type (e.g., std_msgs/String, ...) + char *md5sum; //! The MD5 sum of the message type + char *message_definition; //! Full text of message definition (output of gendeps --cat) + int tcpros_id_list[CN_MAX_TCPROS_SERVER_CONNECTIONS+1]; //! List of node->tcpros_server_proc IDs allocated for this publisher. The last element of the list is always -1 (sentinel) + void *context; + int loop_period; //! Period (in msec) for publication cycle + uint64_t wake_up_time; //! The time for the next automatic message publication (in msec, since the Epoch) + cRosMessageQueue msg_queue; //! Messages on this topic wait in this queue to be send for every process +}; + +/*! Structure that define a subscribed topic */ +struct SubscriberNode +{ + char *message_definition; //! Full text of message definition (output of gendeps --cat) + char *topic_name; //! The subscribed topic name + char *topic_type; //! The subscribed topic data type (e.g., std_msgs/String, ...) + char *md5sum; //! The MD5 sum of the message type + unsigned char tcp_nodelay; //! If 1, the publisher should set TCP_NODELAY on the socket, if possible + void *context; //! Pointer to an internal library structure that stores received messages and its type + cRosMessageQueue msg_queue; //! Each time a message on this topic is received it is queued here + unsigned char msg_queue_overflow; //! If 1, the subscriber tried to insert a message in the queue but it was full +}; + +struct ServiceProviderNode +{ + char *service_name; + char *service_type; + char *servicerequest_type; + char *serviceresponse_type; + char *md5sum; + void *context; +}; + +struct ServiceCallerNode +{ + char *service_name; + char *service_type; + char *servicerequest_type; + char *serviceresponse_type; + char *md5sum; + char *message_definition; //! Full text of message definition (output of gendeps --cat) + int rpcros_id; //! Index of the node->service_callers allocated for this ServiceCallerNode + char *service_host; //! The hostname of the service provider. + int service_port; //! The host port of the the service provider. + unsigned char persistent; //! If 1, the service RPCROS connection should be kept open for multiple requests + unsigned char tcp_nodelay; //! If 1, the service caller should set TCP_NODELAY on the socket, if possible + void *context; + int loop_period; //! Period (in msec) for service-call cycle + uint64_t wake_up_time; //! The time for the next automatic service call (in msec, since the Epoch) + cRosMessageQueue msg_queue; //! Service requests and service responses for this service wait in this queue to be send +}; + +struct ParameterSubscription +{ + char *parameter_key; + XmlrpcParam parameter_value; + void *context; + NodeStatusApiCallback status_api_callback; +}; + +/*! \brief CrosNode object. Don't modify its internal members: use the related functions instead */ +typedef struct CrosNode CrosNode; +struct CrosNode +{ + char *name; //! The node name: it is the absolute name, i.e. it includes the namespace + char *host; //! The node host (ipv4, e.g. 192.168.0.2) + unsigned short xmlrpc_port; //! The node port for the XMLRPC protocol + unsigned short tcpros_port; //! The node port for the TCPROS protocol + unsigned short rpcros_port; //! The node port for the RPCROS protocol + + int pid; //! cROS node process ID + int roscore_pid; //! Roscore PID + + char *roscore_host; //! The roscore host (ipv4, e.g. 192.168.0.1) + unsigned short roscore_port; //! The roscore port + + char *message_root_path; //! Directory with the message register + + CrosLogLevel log_level; + int rosout_pub_idx; //! Index of the publisher of the /rosout topic for ROS log messages + + uint64_t xmlrpc_master_wake_up_time; //! The time (in msec, since the Epoch) for the next automatic operation cycle of the xmlrpc_client_proc[0] (xmlrpc master-node client proc) + + uint32_t log_last_id; //! Sequence number of the last transmitted rosout log message + + unsigned int next_call_id; + ApiCallQueue master_api_queue; + ApiCallQueue slave_api_queue; + + //! Manage connections for XMLRPC calls from this node to others + XmlrpcProcess xmlrpc_client_proc[CN_MAX_XMLRPC_CLIENT_CONNECTIONS]; + XmlrpcProcess xmlrpc_listner_proc; //! Accept new XMLRPC connections from roscore or other nodes + /*! Manage connections for XMLRPC calls from roscore or other nodes to this node */ + XmlrpcProcess xmlrpc_server_proc[CN_MAX_XMLRPC_SERVER_CONNECTIONS]; + + //! Manage connections for TCPROS calls from this node to others + TcprosProcess tcpros_client_proc[CN_MAX_TCPROS_CLIENT_CONNECTIONS]; + TcprosProcess tcpros_listner_proc; //! Accept new TCPROS connections from roscore or other nodes + + /*! Manage connections for TCPROS between this and other nodes */ + TcprosProcess tcpros_server_proc[CN_MAX_TCPROS_SERVER_CONNECTIONS]; + + //! Manage connections for RPCROS calls from this node to others + TcprosProcess rpcros_client_proc[CN_MAX_RPCROS_CLIENT_CONNECTIONS]; + TcprosProcess rpcros_listner_proc; //! Accept new TCPROS connections from roscore or other nodes + + /*! Manage connections for RPCROS between this and other nodes */ + TcprosProcess rpcros_server_proc[CN_MAX_RPCROS_SERVER_CONNECTIONS]; + + PublisherNode pubs[CN_MAX_PUBLISHED_TOPICS]; //! All the published topic, defined by PublisherNode structures + SubscriberNode subs[CN_MAX_SUBSCRIBED_TOPICS]; //! All the subscribed topic, defined by PublisherNode structures + ServiceProviderNode service_providers[CN_MAX_SERVICE_PROVIDERS]; //! All the provided services to register + ServiceCallerNode service_callers[CN_MAX_SERVICE_CALLERS]; //! All the services to call + ParameterSubscription paramsubs[CN_MAX_PARAMETER_SUBSCRIPTIONS]; + + int n_pubs; //! Number of node's published topics + int n_subs; //! Number of node's subscribed topics + int n_service_providers; //! Number of registered services to provide + int n_service_callers; //! Number of services to call + int n_paramsubs; +}; + +/*! \brief Resolve the namespace of the resource name + * + * \param node the CrosNode which is owner of the resource. If NULL the resource is a node as well. + * \param resource_name the name of the resource. + * + * \return A string with the resource name. + */ +char *cRosNamespaceBuild(CrosNode *node, const char *resource_name); + +void cRosGetMsgFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *topic_type); + +/*! \brief Dynamically create a CrosNode instance. This is the right way to create a CrosNode object. + * Once finished, the CrosNode should be released using cRosNodeDestroy() + * + * \param node_name The node name: it is the absolute name, i.e. it should includes the namespace + * \param node_host The node host (ipv4, e.g. 192.168.0.2) + * \param roscore_host The roscore host (ipv4, e.g. 192.168.0.1) + * \param roscore_port The roscore port + * \param select_timeout_ms Max timeout for the select() in ms. NULL defaults to UINT64_MAX + * + * \return A pointer to the new CrosNode on success, NULL on failure + */ +CrosNode *cRosNodeCreate(const char *node_name, const char *node_host, const char *roscore_host, unsigned short roscore_port, + const char *message_root_path); + +/*! \brief Unregister from ROS master and release all the internal allocated memory for a CrosNode + * object previously crated with cRosNodeCreate() + * + * \param n A pointer to the CrosNode object to be released + * \return CROS_SUCCESS_ERR_PACK (0) on success. Otherwise an error code pack containing one or more error codes + */ +cRosErrCodePack cRosNodeDestroy( CrosNode *n ); + +/*! \brief Perform a loop of the cROS node main cycle + * + * \param n A pointer to a CrosNode object (e.g., created with cRosNodeCreate()) + * \param max_timeout Maximum time in milliseconds that this function will take to finish (it may finish before). + * + * cRosNodeDoEventsLoop() perform a file-based event loop: check the read and write operation availability + * for considered sockets, start new write and read actions, open new connections, close dropped connections, + * manage the internal state of the node (i.e., advertise new topics, ...) + * + * cRosNodeDoEventsLoop() should be used inside a cycle, e.g.: + * + * while(1) + * { + * // If you want, do here something + * cRosNodeDoEventsLoop( node ); + * } + * \return CROS_SUCCESS_ERR_PACK (0) on success + */ +cRosErrCodePack cRosNodeDoEventsLoop( CrosNode *n, uint64_t max_timeout ); + +/*! \brief Run the cROS node for a specific time while the exit flag provided by the user is 0 + * + * This function repeatedly calls cRosNodeDoEventsLoop() while these three conditions are met: + * No error occurs, the exit_flag variable is 0 and the timeout is not reached. + * \param n A pointer to a CrosNode object (e.g., created with cRosNodeCreate()) + * \param time_out Time in milliseconds that this function will block running the node. If CROS_INFINITE_TIMEOUT + * is specified, the function will not finish until an error occurs or the exit flag becomes different + * from 0. + * \param exit_flag Pointer to an unsigned char variable: the function will exit if this variable becomes + * different from zero. If this pointer is NULL, the node will be run until the timeout is reached or an + * an error occurs. + * \return CROS_SUCCESS_ERR_PACK (0) on success + */ +cRosErrCodePack cRosNodeStart( CrosNode *n, unsigned long time_out, unsigned char *exit_flag ); + +XmlrpcParam *cRosNodeGetParameterValue( CrosNode *n, const char *key); +/*! @}*/ + +/*! \brief Waits until a network port is open is a host address. + * + * This function repeatedly tries to connect to the specified port until it succeed, an error occurs or the + * specified timeout is reached. When the connection is established it is closed immediately. + * \param host_addr The target address + * \param host_port The target port + * \param time_out Maximum time in milliseconds that this function will block trying to connect. If + * CROS_INFINITE_TIMEOUT is specified, the function will not finish until an error occurs or the + * the port connection is established. + * \return CROS_SUCCESS_ERR_PACK if the port could be connected. CROS_SOCK_OPEN_CONN_ERR is an error + * occurred when trying to connect or CROS_SOCK_OPEN_TIMEOUT_ERR if the timeout was reached before + * the port could be connected. + */ +cRosErrCodePack cRosWaitPortOpen(const char *host_addr, unsigned short host_port, unsigned long time_out); + +/*! \brief Set the file stream used when printing messages locally, that is, through macros: + * PRINT_INFO, PRINT_DEBUG, PRINT_VDEBUG, PRINT_VVDEBUG and PRINT_ERROR. + * + * There are two types of message printing: + * - local message printing (which are printed to a file or to stdout depending on the file stream specified + * when calling this function). + * - ROS log messages. These messages are not affected by this function. + * \param new_stream The file stream used. It must be a valid file stream or NULL if stdout must be used. + */ +void cRosOutStreamSet(FILE *new_stream); + +/*! \brief Get the file stream used for printing local messages. + * + * \return The file stream used. If NULL was specified when calling cRosOutStreamSet(), stdout is returned. + */ +FILE *cRosOutStreamGet(void); + +#endif diff --git a/src/hal/components/cros/include/cros_node_api.h b/src/hal/components/cros/include/cros_node_api.h new file mode 100644 index 00000000..3e9cc211 --- /dev/null +++ b/src/hal/components/cros/include/cros_node_api.h @@ -0,0 +1,100 @@ +#ifndef _CROS_NODE_API_H_ +#define _CROS_NODE_API_H_ + +#define CROS_TRANSPORT_TCPROS_STRING "TCPROS" +#define CROS_TRANSPORT_UPDROS_STRING "UDPROS" + +typedef enum +{ + CROS_API_NONE = 0, + CROS_API_REGISTER_SERVICE, + CROS_API_UNREGISTER_SERVICE, + CROS_API_UNREGISTER_PUBLISHER, + CROS_API_UNREGISTER_SUBSCRIBER, + CROS_API_REGISTER_PUBLISHER, + CROS_API_REGISTER_SUBSCRIBER, + CROS_API_LOOKUP_NODE, + CROS_API_GET_PUBLISHED_TOPICS, + CROS_API_GET_TOPIC_TYPES, + CROS_API_GET_SYSTEM_STATE, + CROS_API_GET_URI, + CROS_API_LOOKUP_SERVICE, + CROS_API_GET_BUS_STATS, + CROS_API_GET_BUS_INFO, + CROS_API_GET_MASTER_URI, + CROS_API_SHUTDOWN, + CROS_API_GET_PID, + CROS_API_GET_SUBSCRIPTIONS, + CROS_API_GET_PUBLICATIONS, + CROS_API_PARAM_UPDATE, + CROS_API_PUBLISHER_UPDATE, + CROS_API_REQUEST_TOPIC, + CROS_API_DELETE_PARAM, + CROS_API_SET_PARAM, + CROS_API_GET_PARAM, + CROS_API_SEARCH_PARAM, + CROS_API_SUBSCRIBE_PARAM, + CROS_API_UNSUBSCRIBE_PARAM, + CROS_API_HAS_PARAM, + CROS_API_GET_PARAM_NAMES +} CrosApiMethod; + +/*! \defgroup cros_api cROS APIs + * + * Internal implemenation (via XMLRPC protocol) of the ROS + * Master, Parameter Server and Slave APIs + * NOTE: this are cROS internal functions, usually you don't need to use them. + */ + +/*! \addtogroup cros_api + * @{ + */ + +struct CrosNode; + +/*! \brief Prepare a XMLRPC message for a ROS XMLRPC protocol function call, + * This function modify the internal member of the XmlrpcProcess client_proc + * of a cROS node. The function to call is chosen checking the currente + * node's internal state (enum CrosNodeState) , + * + * \param n Pointer to the CrosNode object + * \param client_idx The client id that manage the request + */ +void cRosApiPrepareRequest(struct CrosNode *n, int client_idx ); + + +/*! \brief Parse a XMLRPC response generated from a previous API function call ( see cRosApiPrepareRequest() ), + * eventually changing the internal state of the node n + * + * \param n Ponter to the CrosNode object + * \param client_idx The client id that manage the response + * + * \return Returns 0 on success, -1 on failure + */ +int cRosApiParseResponse(struct CrosNode *n, int client_idx ); + +/*! \brief Parse a XMLRPC request (i.e., an API function call generated from roscore or anther node), given + * the XmlrpcProcess server_proc with index server_idx. + * Eventually this function perfomrs the requested actions, eventually changing the internal state of the node n. + * Moreover, it prepares the related XMLRPC response. + * + * \param n Ponter to the CrosNode object + * \param server_idx Index of the XmlrpcProcess ( xmlrpc_server_proc[server_idx] ) to be considered for the parsing and + * the response generation + * + * \return Returns 0 on success, -1 on failure + */ +int cRosApiParseRequestPrepareResponse(struct CrosNode *n, int server_idx ); + +const char * getMethodName(CrosApiMethod method); +CrosApiMethod getMethodCode(const char *method); +int isRosMasterApi(CrosApiMethod method); +int isRosSlaveApi(CrosApiMethod method); + +struct CrosNodeStatusUsr; + +void initCrosNodeStatus(struct CrosNodeStatusUsr *status); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/cros_service.h b/src/hal/components/cros/include/cros_service.h new file mode 100644 index 00000000..67404d61 --- /dev/null +++ b/src/hal/components/cros/include/cros_service.h @@ -0,0 +1,34 @@ +#ifndef _CROS_SERVICE_H_ +#define _CROS_SERVICE_H_ + +#include "cros_node.h" +#include "cros_message.h" +#include "cros_err_codes.h" + +/*! \defgroup cros_service cROS TCPROS + * + * Implementation of the TCPROS protocol for service message exchanges + */ + +/*! \addtogroup cros_service + * @{ + */ + +struct cRosService +{ + cRosMessage *request; + cRosMessage *response; + char* md5sum; +}; + +typedef struct cRosService cRosService; + +cRosService * cRosServiceNew(); +cRosErrCodePack cRosServiceInit(cRosService* service); +cRosErrCodePack cRosServiceBuild(cRosService* service, const char* filepath); +void cRosServiceRelease(cRosService* service); +void cRosServiceFree(cRosService* service); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/cros_service_internal.h b/src/hal/components/cros/include/cros_service_internal.h new file mode 100644 index 00000000..192f81d8 --- /dev/null +++ b/src/hal/components/cros/include/cros_service_internal.h @@ -0,0 +1,41 @@ +#ifndef _CROS_SERVICE_INTERNAL_H_ +#define _CROS_SERVICE_INTERNAL_H_ + +#include "cros_message.h" +#include "cros_message_internal.h" +#include "cros_err_codes.h" + +static const char* FILEEXT_SRV = "srv"; + +// string that denotes the separation between request/response service parts +static const char* SRV_DELIMITER = "---"; + +struct t_srvDef +{ + char* name; + char* package; + char* root_dir; + char* plain_text; + cRosMessageDef* request; + cRosMessageDef* response; +}; + +typedef struct t_srvDef cRosSrvDef; + +cRosErrCodePack cRosServiceBuildInner(cRosMessage **request, cRosMessage **response, char **message_definition, char *md5sum, const char *srv_filepath); +cRosErrCodePack initCrosSrv(cRosSrvDef* srv); + +cRosErrCodePack getFileDependenciesSrv(char* filename, cRosSrvDef* srv, msgDep* deps); + +// Compute full text of service, including text of embedded +// types. The text of the main srv is listed first. Embedded +// srv files are denoted first by an 80-character '=' separator, +// followed by a type declaration line,'MSG: pkg/type', followed by +// the text of the embedded type. +char* computeFullTextSrv(cRosSrvDef* srv, msgDep* deps); + +cRosErrCodePack loadFromFileSrv(const char *filename, cRosSrvDef* srv); + +void cRosServiceDefFree(cRosSrvDef* service_def); + +#endif // _CROS_SERVICE_INTERNAL_H_ diff --git a/src/hal/components/cros/include/cros_tcpros.h b/src/hal/components/cros/include/cros_tcpros.h new file mode 100644 index 00000000..f030758e --- /dev/null +++ b/src/hal/components/cros/include/cros_tcpros.h @@ -0,0 +1,137 @@ +#ifndef _CROS_TCPROS_H_ +#define _CROS_TCPROS_H_ + +#include "cros_node.h" + +typedef enum +{ + TCPROS_PARSER_ERROR = 0, + TCPROS_PARSER_HEADER_INCOMPLETE, + TCPROS_PARSER_DATA_INCOMPLETE, + TCPROS_PARSER_DONE +} TcprosParserState; + +enum +{ + TCPROS_OK_BYTE_FAIL = 0, + TCPROS_OK_BYTE_SUCCESS = 1 +}; + +/*! \brief Parse a TCPROS header sent initially from a subscriber + * + * \param n Ponter to the CrosNode object + * \param server_idx Index of the TcprosProcess ( tcpros_server_proc[server_idx] ) to be considered for the parsing + * + * \return Returns TCPROS_PARSER_DONE if the header is successfully parsed, + * TCPROS_PARSER_HEADER_INCOMPLETE if the header is incomplete, + * or TCPROS_PARSER_ERROR on failure + */ +TcprosParserState cRosMessageParseSubcriptionHeader( CrosNode *n, int server_idx ); + +/*! \brief Parse a TCPROS header sent from a publisher + * + * \param n Ponter to the CrosNode object + * \param client_idx Index of the TcprosProcess ( tcpros_client_proc[server_idx] ) to be considered for the parsing + * + * \return Returns TCPROS_PARSER_DONE if the header is successfully parsed, + * TCPROS_PARSER_HEADER_INCOMPLETE if the header is incomplete, + * or TCPROS_PARSER_ERROR on failure + */ +TcprosParserState cRosMessageParsePublicationHeader( CrosNode *n, int client_idx ); + +/*! \brief Prepare a TCPROS header to be initially sent to a publisher + * + * \param n Ponter to the CrosNode object + * \param client_idx Index of the TcprosProcess ( tcpros_client_proc[server_idx] ) to be considered + */ +void cRosMessagePrepareSubcriptionHeader( CrosNode *n, int client_idx ); + +/*! \brief Prepare a TCPROS header to be initially sent back to a subscriber + * + * \param n Ponter to the CrosNode object + * \param server_idx Index of the TcprosProcess ( tcpros_server_proc[server_idx] ) to be considered + */ +void cRosMessagePreparePublicationHeader( CrosNode *n, int server_idx ); + +/*! \brief Prepare a TCPROS message (with data) to be sent to a subscriber + * + * \param n Ponter to the CrosNode object + * \param server_idx Index of the TcprosProcess ( tcpros_server_proc[server_idx] ) to be considered + * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code + */ +cRosErrCodePack cRosMessagePreparePublicationPacket( CrosNode *n, int server_idx ); + +/*! \brief Read the TCPROS message (with data) received from the publisher + * + * \param n Ponter to the CrosNode object + * \param client_idx Index of the TcprosProcess ( tcpros_client_proc[server_idx] ) to be considered for the parsing + * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code + */ +cRosErrCode cRosMessageParsePublicationPacket( CrosNode *n, int client_idx ); + +/*! \brief Parse a RCPROS header sent from a service caller + * + * \param n Ponter to the CrosNode object + * \param server_idx Index of the TcprosProcess ( rpcros_server_proc[server_idx] ) to be considered for the parsing + * + * \return Returns TCPROS_PARSER_DONE if the header is successfully parsed, + * or TCPROS_PARSER_ERROR on failure + */ +TcprosParserState cRosMessageParseServiceCallerHeader( CrosNode *n, int server_idx); + +/*! \brief Parse a RCPROS header sent by a service provider + * + * \param n Ponter to the CrosNode object + * \param client_idx Index of the TcprosProcess ( rpcros_server_proc[server_idx] ) to be considered for the parsing + * + * \return Returns TCPROS_PARSER_DONE if the header is successfully parsed, + * or TCPROS_PARSER_ERROR on failure + */ +TcprosParserState cRosMessageParseServiceProviderHeader( CrosNode *n, int client_idx); + +/*! \brief Prepare a RCPROS header to be sent to a service provider + * + * \param n Ponter to the CrosNode object + * \param client_idx Index of the TcprosProcess ( rpcros_client_proc[client_idx] ) to be considered + */ +void cRosMessagePrepareServiceCallHeader( CrosNode *n, int client_idx); + +/*! \brief Prepare a RCPROS packet to be sent to a service provider + * + * \param n Ponter to the CrosNode object + * \param client_idx Index of the TcprosProcess ( rpcros_client_proc[client_idx] ) to be considered + * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code + */ +cRosErrCodePack cRosMessagePrepareServiceCallPacket( CrosNode *n, int client_idx ); + +/*! \brief Prepare a RCPROS header to be initially sent back to a service caller + * + * \param n Ponter to the CrosNode object + * \param server_idx Index of the TcprosProcess ( rpcros_server_proc[server_idx] ) to be considered + */ +void cRosMessagePrepareServiceProviderHeader( CrosNode *n, int server_idx); + +/*! \brief Read the RPCROS packet (with data) received from the service provider + * + * \param n Ponter to the CrosNode object + * \param client_idx Index of the TcprosProcess ( rpcros_client_proc[client_idx] ) to be considered for the parsing + * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code + */ +cRosErrCodePack cRosMessageParseServiceResponsePacket( CrosNode *n, int client_idx ); + +/*! \brief Prepare a RCPROS response to be sent back to a service caller + * + * \param n Ponter to the CrosNode object + * \param server_idx Index of the TcprosProcess ( rpcros_server_proc[server_idx] ) to be considered + * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code + */ +cRosErrCodePack cRosMessagePrepareServiceResponsePacket( CrosNode *n, int server_idx); + +/*! \brief Prepare a RCPROS header to be initially sent to a service provider + * + * \param n Ponter to the CrosNode object + * \param server_idx Index of the TcprosProcess ( rpcros_client_proc[client_idx] ) to be considered + */ +void cRosMessagePrepareServiceCallerHeader(CrosNode *n, int server_idx); + +#endif // _CROS_TCPROS_H_ diff --git a/src/hal/components/cros/include/dyn_buffer.h b/src/hal/components/cros/include/dyn_buffer.h new file mode 100644 index 00000000..f51ba5a1 --- /dev/null +++ b/src/hal/components/cros/include/dyn_buffer.h @@ -0,0 +1,249 @@ +#ifndef _DYN_BUFFER_H_ +#define _DYN_BUFFER_H_ + +#include + +/*! \defgroup dyn_buffer Dynamic buffer */ + +/*! \addtogroup dyn_buffer + * @{ + */ + +/*! \brief Dynamic buffer object. Don't modify its internal members: use + * the related functions instead */ +typedef struct DynBuffer DynBuffer; +struct DynBuffer +{ + size_t size; //! Current buffer size + size_t pos_offset; //! Current position indicator + size_t max; //! Max buffer size + unsigned char *data; //! buffer data +}; + +/*! \brief Initialize a dynamic buffer + * + * \param d_buf Pointer to a DynBuffer object to be initialized + */ +void dynBufferInit( DynBuffer *d_buf ); + +/*! \brief Release a dynamic buffer + * + * \param d_buf Pointer to a DynBuffer object to be released + */ +void dynBufferRelease( DynBuffer *d_buf ); + +/*! \brief Append a copy of the n bytes pointed by new_buf to the end of the dynamic buffer pointed by d_buf + * + * \param d_buf Pointer to a DynBuffer object + * \param new_str Pointer to the buffer to be appended + * \param n Number of the bytes to be copied + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackBuf( DynBuffer *d_buf, const unsigned char *new_buf, size_t n ); + +/*! \brief Replace the content of the dynamic buffer starting from current position indicator with the content + * of the buffer cont_buf. + * + * If the length of cont_buf (cont_buf_len) is shorter that the remaining data in the dynamic buffer, the + * dynamic buffer is truncated so that the end to the end of the dynamic buffer if occupied by pointed by cont_buf. + * If the length of remaining content in the dynamic buffer is shorter that cont_buf the dynamic buffer content is + * increased. + * \param d_buf Pointer to a DynBuffer object + * \param cont_buf Pointer to the buffer to write in the dynamic buffer + * \param cont_buf_len Length of the buffer cont_buf + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferReplaceContent( DynBuffer *d_buf, const unsigned char *cont_buf, size_t cont_buf_len ); + +/*! \brief Append a 8 bit signed integer + * to the end of the dynamic buffer pointed by d_buf + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackInt8( DynBuffer *d_buf, int8_t val ); + +/*! \brief Append a 16 bit signed integer (in binary format) + * to the end of the dynamic buffer pointed by d_buf + * WARNING: The byte order depends on the endianess of your platform + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackInt16( DynBuffer *d_buf, int16_t val ); + +/*! \brief Append a 32 bit signed integer (in binary format) + * to the end of the dynamic buffer pointed by d_buf + * WARNING: The byte order depends on the endianess of your platform + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackInt32( DynBuffer *d_buf, int32_t val ); + +/*! \brief Append a 64 bit signed integer (in binary format) + * to the end of the dynamic buffer pointed by d_buf + * WARNING: The byte order depends on the endianess of your platform + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackInt64( DynBuffer *d_buf, int64_t val ); + +/*! \brief Append a 8 bit signed integer + * to the end of the dynamic buffer pointed by d_buf + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackUInt8( DynBuffer *d_buf, uint8_t val ); + +/*! \brief Append a 16 bit unsigned integer (in binary format) + * to the end of the dynamic buffer pointed by d_buf + * WARNING: The byte order depends on the endianess of your platform + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackUInt16( DynBuffer *d_buf, uint16_t val ); + +/*! \brief Append a 32 bit unsigned integer (in binary format) + * to the end of the dynamic buffer pointed by d_buf + * WARNING: The byte order depends on the endianess of your platform + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackUInt32( DynBuffer *d_buf, uint32_t val ); + +/*! \brief Append a 64 bit unsigned integer (in binary format) + * to the end of the dynamic buffer pointed by d_buf + * WARNING: The byte order depends on the endianess of your platform + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackUInt64( DynBuffer *d_buf, uint64_t val ); + +/*! \brief Append a single precision floating point value (in binary format) + * to the end of the dynamic buffer pointed by d_buf + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackFloat32( DynBuffer *d_buf, float val ); + +/*! \brief Append a double precision floating point value (in binary format) + * to the end of the dynamic buffer pointed by d_buf + * + * \param d_buf Pointer to a DynBuffer object + * \param val The value to be appended + * + * \return The new dynamic bufer size, or -1 on failure + */ +int dynBufferPushBackFloat64( DynBuffer *d_buf, double val ); + +/*! \brief Clear a dynamic buffer (the internal memory IS NOT released) + * + * \param d_buf Pointer to a DynBuffer object + */ +void dynBufferClear( DynBuffer *d_buf ); + +/*! \brief Get the current dynamic buffer size + * + * \param d_buf Pointer to a DynBuffer object + * + * \return The current dynamic buffer size + */ +size_t dynBufferGetSize( DynBuffer *d_buf ); + +/*! \brief Get a const pointer to the internal buffer data + * + * \param d_buf Pointer to a DynBuffer object + * + * \return The const pointer to the internal buffer data + */ +const unsigned char *dynBufferGetData( DynBuffer *d_buf ); + +/*! \brief Move the position indicator, adding offset to the current position + * The position indicator may be exploited to mark alreadey "used" bytes + * + * \param d_buf Pointer to a DynBuffer object + * \param offset Offset in bytes added to the position indicator + */ +void dynBufferMovePoseIndicator( DynBuffer *d_buf, int offset ); + +/*! \brief Move the position indicator, setting the current position + * The position indicator may be exploited to mark alreadey "used" bytes + * + * \param d_str Pointer to a DynBuffer object + * \param pos The new the position indicator (in bytes) + */ +void dynBufferSetPoseIndicator( DynBuffer *d_buf, size_t pos ); + +/*! \brief Reset the position indicator + * The position indicator may be exploited to mark alreadey "used" bytes + * + * \param d_buf Pointer to a DynBuffer object + */ +void dynBufferRewindPoseIndicator( DynBuffer *d_buf ); + +/*! \brief Get the current offset of the position indicator + * + * \param d_buf Pointer to a DynBuffer object + + * \return Offset in bytes of the position indicator + */ +size_t dynBufferGetPoseIndicatorOffset( DynBuffer *d_buf ); + +/*! \brief Get a const pointer to the internal buffer data, starting from the current position indicator + * + * \param d_buf Pointer to a DynBuffer object + * + * \return The const pointer to the internal buffer data + */ +const unsigned char *dynBufferGetCurrentData( DynBuffer *d_buf ); + +/*! \brief Copy data from the dynamic buffer starting from current position indicator to a buffer. + * + * If the amount of data to copy is larger than the available data in the dynamic buffer, an error is returned and + * nothing is done. + * \param d_buf Pointer to a DynBuffer object + * \param cont_buf Pointer to the buffer where the data is copied + * \param cont_buf_len Length of the data to copy + * + * \return 0 on success, or -1 on failure + */ +int dynBufferGetCurrentContent ( unsigned char *cont_buf, DynBuffer *d_buf, size_t cont_buf_len ); + +/*! \brief Get the remaining dynamic buffer size, starting from the current position indicator + * + * \param d_buf Pointer to a DynBuffer object + * + * \return The remaining dynamic buffer size + */ +size_t dynBufferGetRemainingDataSize( DynBuffer *d_buf ); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/dyn_string.h b/src/hal/components/cros/include/dyn_string.h new file mode 100644 index 00000000..82fc3363 --- /dev/null +++ b/src/hal/components/cros/include/dyn_string.h @@ -0,0 +1,165 @@ +#ifndef _DYN_STRING_H_ +#define _DYN_STRING_H_ + +/*! \defgroup dyn_string Dynamic string */ + +/*! \addtogroup dyn_string + * @{ + */ + +/*! \brief Dynamic string object. Don't modify its internal members: use + * the related functions instead */ +typedef struct DynString DynString; +struct DynString +{ + int len; //! Current string length not including the \0 character at the end of the string + int pos_offset; //! Movable current string position indicator (cursor) that indicates the start of a last string chunk + int max; //! Max string size not including the space for \0 character at the end of the string + char *data; //! Pointer to the \0-terminated string data +}; + +/*! \brief Initialize a dynamic string + * + * \param d_str Pointer to a DynString object to be initialized + */ +void dynStringInit( DynString *d_str ); + +/*! \brief Release a dynamic string + * \param d_str Pointer to a DynString object to be released + */ +void dynStringRelease( DynString *d_str ); + +/*! \brief Append a copy of the string pointed by new_str to the end of the dynamic string pointed by d_str + * + * \param d_str Pointer to a DynString object + * \param new_str Pointer to the new string to be appended + * + * \return The current dynamic string length, not including the terminating null byte, or -1 on failure + */ +int dynStringPushBackStr( DynString *d_str, const char *new_str ); + +/*! \brief Append a copy of the first n characters of the + * string pointed by new_str to the end of the dynamic string pointed by d_str + * + * \param d_str Pointer to a DynString object + * \param new_str Pointer to the new string to be appended + * \param n Number of characters to be copied + * + * \return The current dynamic string length, not including the terminating null byte, or -1 on failure + */ +int dynStringPushBackStrN( DynString *d_str, const char *new_str, int n ); + +/*! \brief Replace the content of the dynamic string pointed by d_str by the first n characters of the + * character array pointed by new_str + * + * \param d_str Pointer to a DynString object + * \param new_str Pointer to the string that will be the new d_str content + * \param n Number of characters of the inserted in d_str + * + * \return The current dynamic string length, not including the terminating null byte, or -1 on failure + */ +int dynStringReplaceWithStrN ( DynString *d_str, const char *new_str, int n ); + +/*! \brief Append a character to the end of the dynamic string pointed by d_str + * + * \param d_str Pointer to a DynString object + * \param c The character to be appended + * + * \return The current dynamic string length, not including the terminating null byte, or -1 on failure + */ +int dynStringPushBackChar( DynString *d_str, const char c ); + +/*! \brief Copy the string pointed by new_str (not including the terminating null byte) + * inside the dynamic string pointed by d_str, starting from the position pos + * + * \param d_str Pointer to a DynString object + * \param new_str Pointer to the string to be copied + * \param pos Starting position for the copy, must be smaller than the current dynamic string length + * + * \return The new dynamic string length, not including the terminating null byte, or -1 on failure + */ +int dynStringPatch( DynString *d_str, const char *new_str, int pos ); + +/*! \brief Clear a dynamic string (the internal memory IS NOT released) + * + * \param d_str Pointer to a DynString object + */ +void dynStringClear( DynString *d_str ); + +/*! \brief Truncates a DynString object. If more characters than available are tried to be removed, the string just becomes empty. + * The offset index of the DynString is moved left the number of positions removed from left up to pointing to 0. If + * characters are removed from the right, the offset is adjusted to limit its right position to the \0 character at most. + * + * \param d_str Pointer to a DynString object + * \param rem_left Number of character to remove from the beggning of the string + * \param rem_right Number of character to remove from the end of the string + */ +void dynStringReduce ( DynString *d_str, int rem_left, int rem_right); + +/*! \brief Get the current dynamic string length, not including the terminating null byte + * + * \param d_str Pointer to a DynString object + * + * \return The new dynamic string length, not including the terminating null byte + */ +int dynStringGetLen( DynString *d_str ); + +/*! \brief Get a const pointer to the internal string data + * + * \param d_str Pointer to a DynString object + * + * \return The const pointer to the internal string data + */ +const char *dynStringGetData( DynString *d_str ); + +/*! \brief Move the position indicator, adding offset to the current position + * The position indicator may be exploited to mark already "used" characters + * + * \param d_str Pointer to a DynString object + * \param offset Offset in bytes added to the position indicator + */ +void dynStringMovePoseIndicator( DynString *d_str, int offset ); + +/*! \brief Move the position indicator, setting the current position + * The position indicator may be exploited to mark already "used" characters + * + * \param d_str Pointer to a DynString object + * \param pos The new the position indicator (in bytes) + */ +void dynStringSetPoseIndicator( DynString *d_str, int pos ); + +/*! \brief Reset the position indicator + * The position indicator may be exploited to mark already "used" characters + * + * \param d_str Pointer to a DynString object + */ +void dynStringRewindPoseIndicator( DynString *d_str ); + +/*! \brief Get the current offset of the position indicator + * + * \param d_str Pointer to a DynString object + + * \return Offset in bytes of the position indicator + */ +int dynStringGetPoseIndicatorOffset( DynString *d_str ); + +/*! \brief Get a const pointer to the internal string data, starting from the current position indicator + * + * \param d_buf Pointer to a DynString object + * + * \return The const pointer to the internal buffer data + */ +const char *dynStringGetCurrentData( DynString *d_str ); + +/*! \brief Get the remaining dynamic string length starting from the current position indicator, + * not including the terminating null byte + * + * \param d_str Pointer to a DynString object + * + * \return The remaining dynamic string length + */ +int dynStringGetRemainingDataSize( DynString *d_str ); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/md5.h b/src/hal/components/cros/include/md5.h new file mode 100644 index 00000000..7faca7d9 --- /dev/null +++ b/src/hal/components/cros/include/md5.h @@ -0,0 +1,43 @@ +/* + * This is an OpenSSL-compatible implementation of the RSA Data Security, Inc. + * MD5 Message-Digest Algorithm (RFC 1321). + * + * Homepage: + * http://openwall.info/wiki/people/solar/software/public-domain-source-code/md5 + * + * Author: + * Alexander Peslyak, better known as Solar Designer + * + * This software was written by Alexander Peslyak in 2001. No copyright is + * claimed, and the software is hereby placed in the public domain. + * In case this attempt to disclaim copyright and place the software in the + * public domain is deemed null and void, then the software is + * Copyright (c) 2001 Alexander Peslyak and it is hereby released to the + * general public under the following terms: + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted. + * + * There's ABSOLUTELY NO WARRANTY, express or implied. + * + * See md5.c for more information. + */ + +#ifndef _MD5_H +#define _MD5_H + +/* Any 32-bit or wider unsigned integer data type will do */ +typedef unsigned int MD5_u32plus; + +typedef struct { + MD5_u32plus lo, hi; + MD5_u32plus a, b, c, d; + unsigned char buffer[64]; + MD5_u32plus block[16]; +} MD5_CTX; + +extern void MD5_Init(MD5_CTX *ctx); +extern void MD5_Update(MD5_CTX *ctx, const void *data, unsigned long size); +extern void MD5_Final(unsigned char *result, MD5_CTX *ctx); + +#endif diff --git a/src/hal/components/cros/include/tcpip_socket.h b/src/hal/components/cros/include/tcpip_socket.h new file mode 100644 index 00000000..0985c85a --- /dev/null +++ b/src/hal/components/cros/include/tcpip_socket.h @@ -0,0 +1,304 @@ +#ifndef _TCPIP_SOCKET_H_ +#define _TCPIP_SOCKET_H_ + +// _WIN32 is defined when compiling 32 bit and 64 bit applications, so _WIN64 does not have to be checked here +#ifdef _WIN32 +# include +# define FN_SOCKET_ERROR SOCKET_ERROR +# define FN_INVALID_SOCKET INVALID_SOCKET + +# define FN_ERROR_INVALID_PARAMETER ERROR_INVALID_PARAMETER +# define MAX_HOST_NAME_LEN 256 +#else +# include +# include +# ifndef __USE_POSIX +# define __USE_POSIX +# endif +# include +# define MAX_HOST_NAME_LEN _POSIX_HOST_NAME_MAX +# define FN_SOCKET_ERROR (-1) +# define FN_INVALID_SOCKET (-1) + +# define FN_ERROR_INVALID_PARAMETER ENOSPC +# define MAX_HOST_NAME_LEN _POSIX_HOST_NAME_MAX +#endif + +#include "dyn_string.h" +#include "dyn_buffer.h" + +/*! \defgroup tcpip_socket TcpIp socket */ + +/*! \addtogroup tcpip_socket + * @{ + */ + +typedef enum +{ + TCPIPSOCKET_FAILED = 0, + TCPIPSOCKET_IN_PROGRESS, + TCPIPSOCKET_DISCONNECTED, + TCPIPSOCKET_DONE, + + TCPIPSOCKET_UNKNOWN, + TCPIPSOCKET_REFUSED +} TcpIpSocketState; + +/*! \brief TcpIpSocket object. Don't modify directly its internal members: use + * the related functions instead */ +typedef struct TcpIpSocket TcpIpSocket; +struct TcpIpSocket +{ + int fd; //! File descriptor for the new socket + struct sockaddr_in rem_addr; //! Adress of the (remote) host to which this socket is connected or address to which the socket is binded (if it is listening) + unsigned char open; //! It is 1 if the socket has been already created (successful socket() funciton call). Otherwise it is 0 + unsigned char connected; //! It is 1 if the socket is connected (inbound or outbound). Otherwise it is 0 + unsigned char listening; //! It is 1 if the socket is already in the listening state (ready to accept connections). Otherwise it is 0 + unsigned char is_nonblocking; //! It is 1 if the socket has been configured as non blocking. Otherwise it is 0 +}; + +/*! \brief Initialize the TcpIpSocket object with default values + * + * \param s Pointer to a TcpIpSocket object + */ +void tcpIpSocketInit( TcpIpSocket *s ); + +/*! \brief Open a TCP/IP4 socket + * + * \param s Pointer to a TcpIpSocket object + * + * \return Returns 1 on success, 0 on failure + */ +int tcpIpSocketOpen( TcpIpSocket *s ); + +/*! \brief Close a socket + * + * \param s Pointer to a TcpIpSocket object + * + * \return Returns 1 on success, 0 on failure + */ +int tcpIpSocketClose( TcpIpSocket *s ); + +/*! \brief Set non-blocking operation for a TCP/IP4 socket + * + * \param s Pointer to a TcpIpSocket object + * + * \return Returns 1 on success, 0 on failure + */ +int tcpIpSocketSetNonBlocking( TcpIpSocket *s ); + +/*! \brief Set no-delay operation for a socket, that is, disables the Nagle's algorithm. + * This option is intended for applications that require low latency on every packet sent + * + * \param s Pointer to a TcpIpSocket object + * + * \return Returns 1 on success, 0 on failure + */ +int tcpIpSocketSetNoDelay ( TcpIpSocket *s ); + +/*! \brief Set a TCP/IP4 socket to be re-bound immediately without timeout + * + * \param s Pointer to a TcpIpSocket object + * + * \return Returns 1 on success, 0 on failure + */ +int tcpIpSocketSetReuse ( TcpIpSocket *s ); + +/*! \brief Set a TCP/IP4 socket to prevent disconnection + * + * \param s Pointer to a TcpIpSocket object + * \param idle The interval between the last data packet sent and the first keepalive probe + * \param interval The interval between subsequential keepalive probes + * \param count The number of unacknowledged probes to send before considering the connection dead + * + * \return Returns 1 on success, 0 on failure + */ +int tcpIpSocketSetKeepAlive( TcpIpSocket *s, unsigned int idle, unsigned int interval, unsigned int count ); + +/*! \brief Connect a TCP/IP4 socket to a server + * + * \param s Pointer to a TcpIpSocket object + * \param host The server address + * \param port The server port + + * \return Returns TCPIPSOCKET_DONE on success, + * TCPIPSOCKET_IN_PROGRESS if the connection is not yet completed, + * or TCPIPSOCKET_FAILED on failure + */ +TcpIpSocketState tcpIpSocketConnect( TcpIpSocket *s, const char *host, unsigned short port ); + +/*! \brief Checks if a network port is open is a host address. + * + * This function tries to connect to a target port and reports the success. If the connection + * is established it is closed immediately. + * \param host_addr The target address + * \param host_port The target port + + * \return Returns TCPIPSOCKET_DONE if the port is open, + * TCPIPSOCKET_REFUSED if the connection was refused (e.g. the target port is closed), + * or TCPIPSOCKET_FAILED on failure + */ +TcpIpSocketState tcpIpSocketCheckPort ( const char *host_addr, unsigned short host_port ); + +/*! \brief Bind and listen for TCP/IP4 socket connections + * + * \param s Pointer to a TcpIpSocket object + * \param host The address to be bound to the socket + * \param port The port to be bound to the socket + * \param backlog the maximum length of the listen queue + * + * \return Returns 1 on success, 0 on failure + */ +int tcpIpSocketBindListen( TcpIpSocket *s, const char *host, unsigned short port, int backlog ); + +/*! \brief Accept a new connection on a TCP/IP4 socket + * + * \param s Pointer to a TcpIpSocket object used to accept new connection + * (it should be opened and listening) + * \param new_s Pointer to a TcpIpSocket object used to return the accepted connection + * + * \return Returns TCPIPSOCKET_DONE on success, + * TCPIPSOCKET_IN_PROGRESS if the connection is not yet completed, + * or TCPIPSOCKET_FAILED on failure + */ +TcpIpSocketState tcpIpSocketAccept( TcpIpSocket *s, TcpIpSocket *new_s ); + +/*! \brief Shutdown a connectd TCP/IP4 socket to a server + * + * \param s Pointer to a TcpIpSocket object + * + * \return Returns 1 on success, 0 on failure + */ +int tcpIpSocketDisconnect( TcpIpSocket *s ); + +/*! \brief Send a binary message on a connected socket + * + * \param s Pointer to a TcpIpSocket object + * \param d_buf The dynamic buffer to be written + * + * \return Returns TCPIPSOCKET_DONE on success, + * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) + * if the write operation is not yet completed, + * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, + * or TCPIPSOCKET_FAILED on failure + */ +TcpIpSocketState tcpIpSocketWriteBuffer( TcpIpSocket *s, DynBuffer *d_buf ); + +/*! \brief Send a string on a connected socket + * + * \param s Pointer to a TcpIpSocket object + * \param d_str The dynamic string to be written + * + * \return Returns TCPIPSOCKET_DONE on success, + * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) + * if the write operation is not yet completed, + * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, + * or TCPIPSOCKET_FAILED on failure + */ +TcpIpSocketState tcpIpSocketWriteString( TcpIpSocket *s, DynString *d_str ); + +/*! \brief Receive a binary message from a connected socket + * + * \param s Pointer to a TcpIpSocket object + * \param d_buf Pointer to the input dynamic string + * + * \return Returns TCPIPSOCKET_DONE on success, + * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) + * if the read operation would block, + * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, + * or TCPIPSOCKET_FAILED on failure + */ +TcpIpSocketState tcpIpSocketReadBufferEx( TcpIpSocket *s, DynBuffer *d_buf, size_t length, size_t *reads); + +/*! \brief Receive a binary message from a connected socket + * + * \param s Pointer to a TcpIpSocket object + * \param d_buf Pointer to the input dynamic string + * + * \return Returns TCPIPSOCKET_DONE on success, + * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) + * if the read operation would block, + * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, + * or TCPIPSOCKET_FAILED on failure + */ +TcpIpSocketState tcpIpSocketReadBuffer( TcpIpSocket *s, DynBuffer *d_buf ); + +/*! \brief Receive a string from a connected socket + * + * \param s Pointer to a TcpIpSocket object + * \param d_str Pointer to the input dynamic string + * + * \return Returns TCPIPSOCKET_DONE on success, + * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) + * if the read operation would block, + * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, + * or TCPIPSOCKET_FAILED on failure + */ +TcpIpSocketState tcpIpSocketReadString( TcpIpSocket *s, DynString *d_str ); + +/*! \brief Return the file descriptor associated with the TcpIpSocket object + * + * \param s A TcpIpSocket object + * + * \return Returns the file descriptor + */ +int tcpIpSocketGetFD( TcpIpSocket *s ); + +/*! \brief Return the current (local) port to which the TcpIpSocket object is associated. + * + * \param s A TcpIpSocket object + * + * \return Returns the TCP/IP port number. + */ +unsigned short tcpIpSocketGetPort( TcpIpSocket *s ); + +/*! \brief Return the (remote) port to which the TcpIpSocket object is connected, or the + * port to which the socket is binded in case of being a listening port. + * + * \param s A TcpIpSocket object. + * + * \return Returns the TCP/IP port number. + */ +unsigned short tcpIpSocketGetRemotePort( TcpIpSocket *s ); + +/*! \brief Return the (remote) address to which the TcpIpSocket object is connected, or the + * address to which the socket is binded in case of being a listening port. + * + * \param s A TcpIpSocket object. + * + * \return Returns a pointer to the address string or NULL if the address string could not be obtained. + */ +const char *tcpIpSocketGetRemoteAddress( TcpIpSocket *s ); + +/*! \brief Check several file descriptors simultaneously waiting until at least one of them is ready + * for reading, writing or atteding an exceptional condition. That is, the select() function is called. + * + * \return Returns the error code + */ +int tcpIpSocketSelect( int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds, uint64_t time_out ); + +/*! \brief Initiate the use of the TcpIpSocket library. + * This function must be called when before using the tcpIpSocketStartUp library functions. + * + * \return Returns 0 on success or an error code in case of failure. + */ +int tcpIpSocketStartUp( void ); + +/*! \brief Terminate the use of the TcpIpSocket library. + * This function must be called when the application called tcpIpSocketStartUp and it does not need to use the library anymore to free resources. + * + * \return Returns 0 on success or FN_SOCKET_ERROR in case of failure. + */ +int tcpIpSocketCleanUp( void ); + +/*! \brief Get the error code returned by the last socket-function call that failed. + * So this code is significant only when the return value of the call indicated an error. + * + * \return Returns the number of file descriptors that this function has set sets in the fd_set variables or -1 on error. + */ +int tcpIpSocketGetError( void ); + + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/tcpros_process.h b/src/hal/components/cros/include/tcpros_process.h new file mode 100644 index 00000000..e3866159 --- /dev/null +++ b/src/hal/components/cros/include/tcpros_process.h @@ -0,0 +1,95 @@ +#ifndef _TCPROS_PROCESS_H_ +#define _TCPROS_PROCESS_H_ + +#include "tcpip_socket.h" + +/*! \defgroup tcpros_process TCPROS process */ + +/*! \addtogroup tcpros_process + * @{ + */ + +typedef enum +{ + TCPROS_PROCESS_STATE_IDLE, // 0 + TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING, // 1 + TCPROS_PROCESS_STATE_CONNECTING, // 2 + TCPROS_PROCESS_STATE_READING_HEADER_SIZE, // 3 + TCPROS_PROCESS_STATE_READING_HEADER, // 4 + TCPROS_PROCESS_STATE_WRITING_HEADER, // 5 + TCPROS_PROCESS_STATE_WAIT_FOR_WRITING, // 6 + TCPROS_PROCESS_STATE_START_WRITING, // 7 + TCPROS_PROCESS_STATE_READING_SIZE, // 8 + TCPROS_PROCESS_STATE_READING, // 9 + TCPROS_PROCESS_STATE_WRITING // A +} TcprosProcessState; + +/*! \brief The TcprosProcess object represents a client or server connection used to manage + * peer to peer TCPROS connections between nodes. It is internally used to emulate the + * "process descriptor" in a multi-task system (here used in a mono task system), including + * the process file descriptors (i.e., a socket), process memory and the state. + * NOTE: this is a cROS internal object, usually you don't need to use it. + */ +typedef struct TcprosProcess TcprosProcess; +struct TcprosProcess +{ + TcprosProcessState state; //! The state of the process. When it is TCPROS_PROCESS_STATE_WAIT_FOR_WRITING the publisher/caller waits until a new message must be sent (periodic or immediate sending) + TcpIpSocket socket; //! The socket used for the TCPROS or RPCROS communication + DynString topic; //! The name of the topic + DynString service; //! The name of the service + DynString type; //! The message/service type + DynString servicerequest_type; //! The service request type + DynString serviceresponse_type; //! The service response type + DynString md5sum; //! The MD5 sum of the message type + DynString caller_id; //! The name of subscriber or service caller + unsigned char latching; //! If 1, the publisher is sending latched messages. Otherwise 0 + unsigned char tcp_nodelay; //! If 1, the publisher should set TCP_NODELAY on the socket, if possible. Otherwise 0 + unsigned char persistent; //! If 1, the service connection should be kept open for multiple requests. Otherwise it should be 0 + DynBuffer packet; //! The incoming/outgoing TCPROS packet + uint64_t last_change_time; //! Last state change time (in ms) + int topic_idx; //! Index used to associate the process to a publisher or a subscriber + int service_idx; //! Index used to associate the process to a service provider or a service client + size_t left_to_recv; //! Remaining to receive + uint8_t ok_byte; //! 'ok' byte send by a service provider in response to the last service request + int probe; //! The current session is a probing one + int sub_tcpros_port; //! Port (obtained from a publisher node) to which the process must connect + char *sub_tcpros_host; //! Host (obtained from a publisher node) to which the process must connect +}; + + +/*! \brief Initialize an TcprosProcess object, starting to allocate memory + * and settings default values for the objects' fields + * + * \param s Pointer to TcprosProcess object to be initialized + */ +void tcprosProcessInit( TcprosProcess *p ); + +/*! \brief Release all the internally allocated memory of an TcprosProcess object + * + * \param s Pointer to TcprosProcess object to be released + */ +void tcprosProcessRelease( TcprosProcess *p ); + +/*! \brief Clear internal i/o data buffer (packet) of a TcprosProcess object + * + * \param s Pointer to TcprosProcess object + */ +void tcprosProcessClear( TcprosProcess *p ); + +/*! \brief Reset the state and clear all the content of a TcprosProcess object (the + * internal memory IS NOT released, so we can continue using the process) + * + * \param s Pointer to TcprosProcess object + */ +void tcprosProcessReset( TcprosProcess *p ); + +/*! \brief Change the internal state of an TcprosProcess object, and update its timer + * + * \param s Pointer to TcprosProcess object + * \param state The new state + */ +void tcprosProcessChangeState( TcprosProcess *p, TcprosProcessState state ); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/tcpros_tags.h b/src/hal/components/cros/include/tcpros_tags.h new file mode 100644 index 00000000..bd27b0e7 --- /dev/null +++ b/src/hal/components/cros/include/tcpros_tags.h @@ -0,0 +1,99 @@ +#ifndef _TCPROS_TAGS_H_ +#define _TCPROS_TAGS_H_ + +/*! \defgroup tcpros_tags TCPROS tags */ + +/*! \addtogroup tcpros_tags + * @{ + */ + +/*! \brief Structure that contains a TCPROS tag string and the related string length + */ +typedef struct +{ + char *str; + int dim; +}TcprosTagStrDim; + +static TcprosTagStrDim TCPROS_CALLERID_TAG = { "callerid=", 9 }; +static TcprosTagStrDim TCPROS_TOPIC_TAG = { "topic=", 6 }; +static TcprosTagStrDim TCPROS_TYPE_TAG = { "type=", 5 }; +static TcprosTagStrDim TCPROS_MD5SUM_TAG = { "md5sum=", 7 }; +static TcprosTagStrDim TCPROS_MESSAGE_DEFINITION_TAG = { "message_definition=", 19 }; +static TcprosTagStrDim TCPROS_SERVICE_TAG = { "service=", 8 }; +static TcprosTagStrDim TCPROS_SERVICE_REQUESTTYPE_TAG = { "request_type=", 13 }; +static TcprosTagStrDim TCPROS_SERVICE_RESPONSETYPE_TAG = { "response_type=", 14 }; +static TcprosTagStrDim TCPROS_TCP_NODELAY_TAG = { "tcp_nodelay=", 12 }; +static TcprosTagStrDim TCPROS_LATCHING_TAG = { "latching=", 9 }; // WARNING Not implemented +static TcprosTagStrDim TCPROS_PERSISTENT_TAG = { "persistent=", 11 }; // WARNING Not implemented +static TcprosTagStrDim TCPROS_PROBE_TAG = { "probe=", 6 }; +static TcprosTagStrDim TCPROS_ERROR_TAG = { "error=", 6 }; +static TcprosTagStrDim TCPROS_EMPTY_MD5SUM_TAG = { "md5sum=*", 8 }; + +enum +{ + TCPROS_CALLER_ID_FLAG = 0x1, + TCPROS_TOPIC_FLAG = 0x2, + TCPROS_TYPE_FLAG = 0x4, + TCPROS_MD5SUM_FLAG = 0x8, + TCPROS_MESSAGE_DEFINITION_FLAG = 0x10, + TCPROS_SERVICE_FLAG = 0x20, + TCPROS_TCP_NODELAY_FLAG = 0x40, + TCPROS_LATCHING_FLAG = 0x80, + TCPROS_PERSISTENT_FLAG = 0x100, + TCPROS_ERROR_FLAG = 0x200, + TCPROS_DATA_FLAG = 0x400, + TCPROS_PROBE_FLAG = 0x800, + TCPROS_EMPTY_MD5SUM_FLAG = 0x1000, + TCPROS_SERVICE_REQUESTTYPE_FLAG = 0x2000, + TCPROS_SERVICE_RESPONSETYPE_FLAG = 0x4000 +}; + +// http://wiki.ros.org/ROS/TCPROS mentions message_definition as compulsory but +// it's not sent by roscpp subscribers +static const uint32_t TCPROS_SUBCRIPTION_HEADER_FLAGS = // TCPROS_MESSAGE_DEFINITION_FLAG | + TCPROS_CALLER_ID_FLAG | + TCPROS_TOPIC_FLAG | + TCPROS_MD5SUM_FLAG | + TCPROS_TYPE_FLAG; + +// http://wiki.ros.org/ROS/TCPROS doesn't mention message_definition, caller_id +// or the topic as compulsory but they are sent by roscpp publishers +static const uint32_t TCPROS_PUBLICATION_HEADER_FLAGS = TCPROS_TYPE_FLAG | + TCPROS_MD5SUM_FLAG; +/* +static const uint32_t TCPROS_PUBLICATION_PACKET_FLAGS = TCPROS_CALLER_ID_FLAG | + TCPROS_TOPIC_FLAG | + TCPROS_TYPE_FLAG | + TCPROS_MD5SUM_FLAG | + TCPROS_MESSAGE_DEFINITION_FLAG | + TCPROS_DATA_FLAG; +*/ +static const uint32_t TCPROS_SERVICECALL_HEADER_FLAGS = TCPROS_CALLER_ID_FLAG | + TCPROS_SERVICE_FLAG | + TCPROS_MD5SUM_FLAG | + TCPROS_PERSISTENT_FLAG; + +static const uint32_t TCPROS_SERVICECALL_MATLAB_HEADER_FLAGS = TCPROS_CALLER_ID_FLAG | + TCPROS_TYPE_FLAG | + TCPROS_MESSAGE_DEFINITION_FLAG | + TCPROS_SERVICE_FLAG | + TCPROS_MD5SUM_FLAG | + TCPROS_TCP_NODELAY_FLAG | + TCPROS_PERSISTENT_FLAG; + +static const uint32_t TCPROS_SERVICEPROBE_HEADER_FLAGS = TCPROS_CALLER_ID_FLAG | + TCPROS_SERVICE_FLAG | + TCPROS_PROBE_FLAG | + TCPROS_EMPTY_MD5SUM_FLAG; + +static const uint32_t TCPROS_SERVICEPROBE_MATLAB_HEADER_FLAGS = TCPROS_CALLER_ID_FLAG | + TCPROS_TYPE_FLAG | + TCPROS_SERVICE_FLAG | + TCPROS_EMPTY_MD5SUM_FLAG; + +static const uint32_t TCPROS_SERVICEPROVISION_HEADER_FLAGS = TCPROS_TYPE_FLAG | + TCPROS_MD5SUM_FLAG; +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/xmlrpc_params.h b/src/hal/components/cros/include/xmlrpc_params.h new file mode 100644 index 00000000..bc8ba0e1 --- /dev/null +++ b/src/hal/components/cros/include/xmlrpc_params.h @@ -0,0 +1,280 @@ +#ifndef _XMLRPC_PARAMS_H_ +#define _XMLRPC_PARAMS_H_ + +#include +#include "dyn_string.h" + +/*! \defgroup xmlrpc_param XMLRPC parameters */ + +/*! \addtogroup xmlrpc_param + * @{ + */ + +typedef enum +{ + XMLRPC_PARAM_UNKNOWN = 0, + XMLRPC_PARAM_BOOL, + XMLRPC_PARAM_INT, + XMLRPC_PARAM_DOUBLE, + XMLRPC_PARAM_STRING, + XMLRPC_PARAM_ARRAY, + XMLRPC_PARAM_DATETIME, /* WARNING: Currently unsupported */ + XMLRPC_PARAM_BINARY, /* WARNING: Currently unsupported */ + XMLRPC_PARAM_STRUCT +}XmlrpcParamType; + +/*! \brief Struct used to store a input/oputput XMLRPC param. + * To modify its internal members, you could use the related functions + */ +typedef struct XmlrpcParam XmlrpcParam; + +struct XmlrpcParam +{ + XmlrpcParamType type; //! Param type + char *member_name; + union + { + uint8_t opaque[8]; + int as_bool; + int32_t as_int; + double as_double; + char *as_string; + XmlrpcParam *as_array; // or struct + void* as_time; /* WARNING: Currently unsupported */ + void* as_binary; /* WARNING: Currently unsupported */ + } data; //! Param data + int array_n_elem; //! Used only if type is XMLRPC_PARAM_ARRAY: it stores the array size + int array_max_elem; //! Used only if type is XMLRPC_PARAM_ARRAY: it stores the current max size +}; + +/*! \brief Return an XMLRPC parameter as a boolena value (i.e., an + * unisigned char value either 0 : false or 1 : true ). + * No type control are performed. If the XMLRPC parameter + * is not boolean, return value is undefined + * + * \param param Pointer to a XMLRPC parameter + * + * \return An integer value ( 0 : false, 1 : true ) + */ +int xmlrpcParamGetBool( XmlrpcParam *param ); + +/*! \brief Return an XMLRPC parameter as a integer value. + * No type control are performed. If the XMLRPC parameter + * is not integer, return value is undefined + * + * \param param Pointer to a XMLRPC parameter + * + * \return The integer value + */ +int32_t xmlrpcParamGetInt( XmlrpcParam *param ); + +/*! \brief Return an XMLRPC parameter as a double value. + * No type control are performed. If the XMLRPC parameter + * is not double, return value is undefined + * + * \param param Pointer to a XMLRPC parameter + * + * \return The double value + */ +double xmlrpcParamGetDouble( XmlrpcParam *param ); + +/*! \brief Return an XMLRPC parameter as a string value. + * No type control are performed. If the XMLRPC parameter + * is not a string, return value is undefined + * + * \param param Pointer to a XMLRPC parameter + * + * \return Pointer to the string, passed as a pointer to the internal memory + */ +char *xmlrpcParamGetString( XmlrpcParam *param ); + +/*! \brief Setup a XMLRPC unknown parameter (e.g., in case of errors) + * + * \param param Pointer to a XMLRPC parameter + */ +void xmlrpcParamSetUnknown( XmlrpcParam *param ); + +/*! \brief Setup a XMLRPC boolean parameter with the given value + * + * \param param Pointer to a XMLRPC parameter + * \param val An integer value (0 : false, true otherwise) + */ +void xmlrpcParamSetBool( XmlrpcParam *param, int val ); + +/*! \brief Setup a XMLRPC integer parameter with the given value + * + * \param param Pointer to a XMLRPC parameter + * \param val An integer value + */ +void xmlrpcParamSetInt( XmlrpcParam *param, int32_t val ); + +/*! \brief Setup a XMLRPC double floating point parameter with the given value + * + * \param param Pointer to a XMLRPC parameter + * \param val An double value + */ +void xmlrpcParamSetDouble( XmlrpcParam *param, double val ); + +/*! \brief Setup a XMLRPC string parameter with the given string. + * The string is copied inside a dynamically allocated memory + * + * \param param Pointer to a XMLRPC parameter + * \param val Pointer to a string + * \return 0 on success, -1 on memory allocation error + */ +int xmlrpcParamSetString( XmlrpcParam *param, const char *val ); + +/*! \brief As xmlrpcParamSetString(), but limits the string length to n characters + * + * \param param Pointer to a XMLRPC parameter + * \param val Pointer to a string + * \param n The string length + * \return 0 on success, -1 on memory allocation error + */ +int xmlrpcParamSetStringN( XmlrpcParam *param, const char *val, int n ); + +/*! \brief Setup an empty array XMLRPC parameter, starting to allocate internal memory + * + * \param param Pointer to a XMLRPC parameter + */ +int xmlrpcParamSetArray( XmlrpcParam *param ); + +/*! \brief Setup an empty struct XMLRPC parameter, starting to allocate internal memory + * + * \param param Pointer to a XMLRPC parameter + */ +int xmlrpcParamSetStruct( XmlrpcParam *param ); + +/*! \brief Return the data type for a given XMLRPC parameter + * + * \param param Pointer to a XMLRPC parameter + * + * \return The data type + */ +XmlrpcParamType xmlrpcParamGetType( XmlrpcParam *param ); + +/*! \brief Return the size of a XMLRPC array parameter + * + * \param param Pointer to a XMLRPC parameter + * + * \return The array size, or -1 if the XMLRPC parameter is not an array + */ +int xmlrpcParamArrayGetSize( XmlrpcParam *param ); + +/*! \brief Return the idx-th element of a XMLRPC array parameter + * + * \param param Pointer to a XMLRPC parameter + * \param idx The element index + * + * \return Pointer to the XMLRPC parameter, or NULL + * if the XMLRPC parameter is not an array or if idx exceeds the array size + */ +XmlrpcParam * xmlrpcParamArrayGetParamAt( XmlrpcParam *param, int idx ); + +/*! \brief Append to an array XMLRPC parameter a bool parameter + * + * \param param Pointer to an array XMLRPC parameter + * \param val An integer value (false if it is equal to 0, true otherwise) + */ +XmlrpcParam * xmlrpcParamArrayPushBackBool( XmlrpcParam *param, int val ); + +/*! \brief Append to an array XMLRPC parameter an integer parameter + * + * \param param Pointer to an array XMLRPC parameter + * \param val An integer value + */ +XmlrpcParam * xmlrpcParamArrayPushBackInt( XmlrpcParam *param, int32_t val ); + +/*! \brief Append to an array XMLRPC parameter a double parameter + * + * \param param Pointer to an array XMLRPC parameter + * \param val An double value + */ +XmlrpcParam * xmlrpcParamArrayPushBackDouble( XmlrpcParam *param, double val ); + +/*! \brief Append to an array XMLRPC parameter a string parameter + * + * \param param Pointer to an array XMLRPC parameter + * \param val Pointer to a string + */ +XmlrpcParam * xmlrpcParamArrayPushBackString( XmlrpcParam *param, const char *val ); + +/*! \brief As xmlrpcParamArrayPushBackString(), but limits the string length to n characters + * + * \param param Pointer to an array XMLRPC parameter + * \param val Pointer to a string + * \param n The string length + */ +XmlrpcParam * xmlrpcParamArrayPushBackStringN( XmlrpcParam *param, const char *val, int n ); + +/*! \brief Append to an array XMLRPC parameter an empty array parameter + * + * \param param Pointer to an array XMLRPC parameter + * + * \return A pointer to the new pushed array XMLRPC parameter, or NULL on failure + */ +XmlrpcParam * xmlrpcParamArrayPushBackArray( XmlrpcParam *param ); + +/*! \brief Append to an array XMLRPC parameter an empty struct parameter + * + * \param param Pointer to an array XMLRPC parameter + * + * \return A pointer to the new pushed struct XMLRPC parameter, or NULL on failure + */ +XmlrpcParam * xmlrpcParamArrayPushBackStruct ( XmlrpcParam *param ); + +XmlrpcParam * xmlrpcParamStructGetParam( XmlrpcParam *param, const char *name ); +XmlrpcParam * xmlrpcParamStructPushBackBool( XmlrpcParam *param, const char *name, int val ); +XmlrpcParam * xmlrpcParamStructPushBackInt( XmlrpcParam *param, const char *name, int32_t val ); +XmlrpcParam * xmlrpcParamStructPushBackDouble( XmlrpcParam *param, const char *name, double val ); +XmlrpcParam * xmlrpcParamStructPushBackString( XmlrpcParam *param, const char *name, const char *val ); +XmlrpcParam * xmlrpcParamStructPushBackStringN( XmlrpcParam *param, const char *name, const char *val, int n ); +XmlrpcParam * xmlrpcParamStructPushBackArray( XmlrpcParam *param, const char *name ); +XmlrpcParam * xmlrpcParamStructPushBackStruct ( XmlrpcParam *param, const char *name ); + +XmlrpcParam * xmlrpcParamNew(void); + +void xmlrpcParamFree( XmlrpcParam *param ); + +void xmlrpcParamInit( XmlrpcParam *param ); + +/*! \brief Release internal data dynamically allocated (e.g., string and arrays) + * + * \param param Pointer to a XMLRPC parameter + */ +void xmlrpcParamRelease( XmlrpcParam *param ); + +/*! \brief Append to a dynamic string the parameters given in input, converted in XML + * + * \param param Pointer to the param to be converted in XML + * \param message Pointer to the dynamic string where the parameter will be appended + */ +void xmlrpcParamToXml( XmlrpcParam *param, DynString *message ); + +/*! \brief Look for a XMLRPC parameter inside a dynamic string, and store in a XmlrpcParam object + * + * \param message Pointer to the dynamic string to be parsed + * \param param Pointer to the output parameter + * + * \return Returns 1 on success, 0 on failure + */ +int xmlrpcParamFromXml( DynString *message, XmlrpcParam *param ); + +/*! \brief Print XMLRPC parameter to stdout in human readable form + * + * \param param Pointer to the output parameter + */ +void xmlrpcParamPrint( XmlrpcParam *param ); + +XmlrpcParam * xmlrpcParamClone( XmlrpcParam *param ); + +int xmlrpcParamCopy(XmlrpcParam *dest, XmlrpcParam *source); + +// Functions for internal library use +static void paramPrint( XmlrpcParam *param, char *head, int is_struct_member); + +static void paramArrayPrint( XmlrpcParam *param, char *head, int is_struct_member); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/xmlrpc_params_vector.h b/src/hal/components/cros/include/xmlrpc_params_vector.h new file mode 100644 index 00000000..a5e8fa84 --- /dev/null +++ b/src/hal/components/cros/include/xmlrpc_params_vector.h @@ -0,0 +1,126 @@ +#ifndef _XMLRPC_PARAMS_VECTOR_H_ +#define _XMLRPC_PARAMS_VECTOR_H_ + +#include "xmlrpc_params.h" + +/*! \defgroup xmlrpc_param_vector XMLRPC parameters vector */ + +/*! \addtogroup xmlrpc_param_vector + * @{ + */ + +/*! \brief Dynamic vector object for XMLRPC parameters. Don't modify its internal members directly: + * use the related functions instead */ +typedef struct XmlrpcParamVector XmlrpcParamVector; +struct XmlrpcParamVector +{ + int size; //! Current vector size + int max; //! Max vector size + XmlrpcParam *data; //! buffer data +}; + +/*! \brief Initialize a dynamic vector + * + * \param p_vec Pointer to a XmlrpcParamVector object to be initialized + */ +void xmlrpcParamVectorInit( XmlrpcParamVector *p_vec ); + +/*! \brief Release a dynamic vector. It also release all the internal + * data dynamically allocated (e.g., string and arrays) calling + * the xmlrpcParamReleaseData() function + * + * \param p_vec Pointer to a XmlrpcParamVector object to be be released + */ +void xmlrpcParamVectorRelease( XmlrpcParamVector *p_vec ); + +/*! \brief Append a new XMLRPC boolean parameter to the vector. + * + * \param p_vec Pointer to a XmlrpcParamVector object + * \param val An integer value (false if it is equalt to 0, true otherwise) + * + * \return The new dynamic vector size, or -1 on failure + */ +int xmlrpcParamVectorPushBackBool( XmlrpcParamVector *p_vec, int val ); + +/*! \brief Append a new XMLRPC integer parameter to the vector. + * + * \param p_vec Pointer to a XmlrpcParamVector object + * \param val An integer value + * + * \return The new dynamic vector size, or -1 on failure + */ +int xmlrpcParamVectorPushBackInt( XmlrpcParamVector *p_vec, int32_t val ); + +/*! \brief Append a new XMLRPC double floating point parameter to the vector. + * + * \param p_vec Pointer to a XmlrpcParamVector object + * \param val A double value + * + * \return The new dynamic vector size, or -1 on failure + */ +int xmlrpcParamVectorPushBackDouble( XmlrpcParamVector *p_vec, double val ); + +/*! \brief Append a new XMLRPC string parameter to the vector. + * The string is copied inside a dynamically allocated memory + * + * \param p_vec Pointer to a XmlrpcParamVector object + * \param val Pointer to a string + * + * \return The new dynamic vector size, or -1 on failure + */ +int xmlrpcParamVectorPushBackString( XmlrpcParamVector *p_vec, const char *val ); + +/*! \brief Append a new (empty) XMLRPC array parameter to the vector, + * and start to allocate internal memory for the array + * + * \param p_vec Pointer to a XmlrpcParamVector object + * + * \return The new dynamic vector size, or -1 on failure + */ +int xmlrpcParamVectorPushBackArray( XmlrpcParamVector *p_vec ); + +/*! \brief Append a new (empty) XMLRPC struct parameter to the vector, + * and start to allocate internal memory + * + * \param p_vec Pointer to a XmlrpcParamVector object + * + * \return The new dynamic vector size, or -1 on failure + */ +int xmlrpcParamVectorPushBackStruct( XmlrpcParamVector *p_vec ); + +/*! \brief Append a new XMLRPC parameter to the vector. + * Warning: data as string and arrays ARE NOT copied, i.e. only references are copyed. + * + * \param p_vec Pointer to a XmlrpcParamVector object + * \param param Pointer to the XMLRPC parameter to be appended + * + * \return The new dynamic vector size, or -1 on failure + */ +int xmlrpcParamVectorPushBack( XmlrpcParamVector *p_vec, XmlrpcParam *param ); + +/*! \brief Get the current dynamic vector size + * + * \param p_vec Pointer to a XmlrpcParamVector object + * + * \return The current dynamic vector size + */ +int xmlrpcParamVectorGetSize( XmlrpcParamVector *p_vec ); + +/*! \brief Return a reference to a XMLRPC parameter + * + * \param p_vec Pointer to a XmlrpcParamVector object + * \param pos Index of the parameter + * + * \return Pointer to the pos-th XMLRPC parameter + */ +XmlrpcParam *xmlrpcParamVectorAt( XmlrpcParamVector *p_vec, int pos ); + +/*! \brief Print a vector of XMLRPC parameters to stdout in human readable form + * + * \param p_vec Pointer to a XmlrpcParamVector object + */ +void xmlrpcParamVectorPrint( XmlrpcParamVector *p_vec ); + +/*! @}*/ + +#endif \ No newline at end of file diff --git a/src/hal/components/cros/include/xmlrpc_process.h b/src/hal/components/cros/include/xmlrpc_process.h new file mode 100644 index 00000000..9d35fc86 --- /dev/null +++ b/src/hal/components/cros/include/xmlrpc_process.h @@ -0,0 +1,81 @@ +#ifndef _XMLRPC_PROCESS_H_ +#define _XMLRPC_PROCESS_H_ + +#include "tcpip_socket.h" +#include "xmlrpc_protocol.h" +#include "cros_api_call.h" + +/*! \defgroup xmlrpc_process XMLRPC process */ + +/*! \addtogroup xmlrpc_process + * @{ + */ + +typedef enum +{ + XMLRPC_PROCESS_STATE_IDLE, + XMLRPC_PROCESS_STATE_CONNECTING, + XMLRPC_PROCESS_STATE_READING, + XMLRPC_PROCESS_STATE_WRITING +}XmlrpcProcessState; + +/*! \brief The XmlrpcProcess object represents a client or server connection between + * the node and the roscore node or other nodes. It is internally used to emulate the + * "precess descriptor" in a multitask system (here used in a mono task system), including + * the process file descriptors (i.e., a socket), proecess memory and the state. + * NOTE: this is a cROS internal object, usually you don't need to use it. + */ +typedef struct XmlrpcProcess XmlrpcProcess; +struct XmlrpcProcess +{ + RosApiCall *current_call; + XmlrpcProcessState state; //! The state + TcpIpSocket socket; //! The socket used for the XMLRPC communication + XmlrpcMessageType message_type; //! The incoming/outgoing XMLRPC message type + DynString method; //! The incoming/outgoing XMLRPC method + XmlrpcParamVector params; //! The incoming/outgoing XMLRPC response + XmlrpcParamVector response; //! The incoming/outgoing XMLRPC response + /*! The incoming/outgoing XMLRPC message + * (e.g., generated using generateXmlrpcMessage() ) */ + DynString message; + uint64_t last_change_time; //! Last state change time (in ms) + char host[256]; + int port; +}; + + +/*! \brief Initialize an XmlrpcProcess object, starting to allocate memory + * and settins default values for the objects' fields + * + * \param s Pointer to XmlrpcProcess object to be initialized + */ +void xmlrpcProcessInit( XmlrpcProcess *p ); + +/*! \brief Release all the internally allocated memory of an XmlrpcProcess object + * + * \param s Pointer to XmlrpcProcess object to be released + */ +void xmlrpcProcessRelease( XmlrpcProcess *p ); + +/*! \brief Clear internal message buffer of an XmlrpcProcess object + * + * \param s Pointer to XmlrpcProcess object + */ +void xmlrpcProcessClear( XmlrpcProcess *p); + +/*! \brief Reset the state and internal data of an XmlrpcProcess object (the internal memory IS NOT released) + * + * \param s Pointer to XmlrpcProcess object + */ +void xmlrpcProcessReset( XmlrpcProcess *p); + +/*! \brief Change the internal state of an XmlrpcProcess object, and update its timer + * + * \param s Pointer to XmlrpcProcess object + * \param state The new state + */ +void xmlrpcProcessChangeState( XmlrpcProcess *p, XmlrpcProcessState state ); + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/include/xmlrpc_protocol.h b/src/hal/components/cros/include/xmlrpc_protocol.h new file mode 100644 index 00000000..f999dd38 --- /dev/null +++ b/src/hal/components/cros/include/xmlrpc_protocol.h @@ -0,0 +1,56 @@ +#ifndef _XMLRPC_PROTOCOL_H_ +#define _XMLRPC_PROTOCOL_H_ + +#include "xmlrpc_params_vector.h" +#include "dyn_string.h" + +/*! \defgroup xmlrpc_protocol XMLRPC protocol */ + +/*! \addtogroup xmlrpc_protocol + * @{ + */ + +typedef enum +{ + XMLRPC_MESSAGE_REQUEST, + XMLRPC_MESSAGE_RESPONSE, + XMLRPC_MESSAGE_UNKNOWN +}XmlrpcMessageType; + +typedef enum +{ + XMLRPC_PARSER_ERROR, + XMLRPC_PARSER_INCOMPLETE, + XMLRPC_PARSER_DONE +}XmlrpcParserState; + + +/*! \brief Generate a XMLRPC over HTTP message and store it into a dynamic string + * + * \param host The hostname where the master runs + * \param port The port where the master runs + * \param type The message type (XMLRPC_MESSAGE_REQUEST or XMLRPC_MESSAGE_RESPONSE ) + * \param method The RPC method to invoke ( used only if type == XMLRPC_MESSAGE_REQUEST ) + * \param params Vector of arguments to the RPC call + * \param message Pointer to the (output) dynamic string that will contain the generated message + */ +void generateXmlrpcMessage( const char*host, unsigned short port, XmlrpcMessageType type, + const char *method, XmlrpcParamVector *params, DynString *message ); + +/*! \brief Parse a XMLRPC over HTTP message + * + * \param message Pointer to the input dynamic string that will contain the message to be parsed + * \param type The message type (XMLRPC_MESSAGE_REQUEST or XMLRPC_MESSAGE_RESPONSE ) + * \param method The RPC method to invoke ( used only if type == XMLRPC_MESSAGE_REQUEST ) + * \param response Output vector of XMLRPC parameters with the set of arguments to the RPC call + * + * \return XMLRPC_PARSER_DONE if the message has ben successfully, + * XMLRPC_PARSER_INCOMPLETE if the message is incomplete, + * XMLRPC_PARSER_ERROR on failure + */ +XmlrpcParserState parseXmlrpcMessage(DynString *message, XmlrpcMessageType *type, + DynString *method, XmlrpcParamVector *response, + char host[256], int *port); + +/*! @}*/ +#endif \ No newline at end of file diff --git a/src/hal/components/cros/include/xmlrpc_tags.h b/src/hal/components/cros/include/xmlrpc_tags.h new file mode 100644 index 00000000..638289c1 --- /dev/null +++ b/src/hal/components/cros/include/xmlrpc_tags.h @@ -0,0 +1,67 @@ +#ifndef _XMLRPC_TAGS_H_ +#define _XMLRPC_TAGS_H_ + +/*! \defgroup xmlrpc_tags XMLRPC tags */ + +/*! \addtogroup xmlrpc_tags + * @{ + */ + +/*! \brief Structure that contains a XMLRPC tag string and the related string length + */ +typedef struct +{ + char *str; + int dim; +}XmlrpcTagStrDim; + +static XmlrpcTagStrDim XMLRPC_VERSION = { "Custom XMLRPC", 13 }; +static XmlrpcTagStrDim XMLRPC_MESSAGE_BEGIN = { "", 21 }; +static XmlrpcTagStrDim XMLRPC_MESSAGE_END = { "", 0 }; +static XmlrpcTagStrDim XMLRPC_REQUEST_BEGIN = { "", 12 }; +static XmlrpcTagStrDim XMLRPC_REQUEST_END = { "", 13 }; +static XmlrpcTagStrDim XMLRPC_METHODNAME_BEGIN = { "", 12 }; +static XmlrpcTagStrDim XMLRPC_METHODNAME_END = { "", 13 }; +static XmlrpcTagStrDim XMLRPC_RESPONSE_BEGIN = { "", 16 }; +static XmlrpcTagStrDim XMLRPC_RESPONSE_END = { "", 17 }; + +static XmlrpcTagStrDim XMLRPC_PARAMS_TAG = { "", 8 }; +static XmlrpcTagStrDim XMLRPC_PARAMS_ETAG = { "", 9 }; +static XmlrpcTagStrDim XMLRPC_PARAM_TAG = { "", 7 }; +static XmlrpcTagStrDim XMLRPC_PARAM_ETAG = { "", 8 }; +static XmlrpcTagStrDim XMLRPC_FAULT_TAG = { "", 7 }; +static XmlrpcTagStrDim XMLRPC_FAULT_ETAG = { "", 8 }; + +static XmlrpcTagStrDim XMLRPC_VALUE_TAG = { "", 7 }; +static XmlrpcTagStrDim XMLRPC_VALUE_ETAG = { "", 8 }; + +static XmlrpcTagStrDim XMLRPC_BOOLEAN_TAG = { "", 9 }; +static XmlrpcTagStrDim XMLRPC_BOOLEAN_ETAG = { "", 10 }; +static XmlrpcTagStrDim XMLRPC_DOUBLE_TAG = { "", 8 }; +static XmlrpcTagStrDim XMLRPC_DOUBLE_ETAG = { "", 9 }; +static XmlrpcTagStrDim XMLRPC_I4_TAG = { "", 4 }; +static XmlrpcTagStrDim XMLRPC_I4_ETAG = { "", 5 }; +static XmlrpcTagStrDim XMLRPC_INT_TAG = { "", 5 }; +static XmlrpcTagStrDim XMLRPC_INT_ETAG = { "", 6 }; +static XmlrpcTagStrDim XMLRPC_STRING_TAG = { "", 8 }; +static XmlrpcTagStrDim XMLRPC_STRING_ETAG = { "", 9 }; +static XmlrpcTagStrDim XMLRPC_DATETIME_TAG = { "", 18 }; +static XmlrpcTagStrDim XMLRPC_DATETIME_ETAG = { "", 19 }; +static XmlrpcTagStrDim XMLRPC_BASE64_TAG = { "", 8 }; +static XmlrpcTagStrDim XMLRPC_BASE64_ETAG = { "", 9 }; +static XmlrpcTagStrDim XMLRPC_ARRAY_TAG = { "", 7 }; +static XmlrpcTagStrDim XMLRPC_ARRAY_ETAG = { "", 8 }; +static XmlrpcTagStrDim XMLRPC_DATA_TAG = { "", 6 }; // start-tag +static XmlrpcTagStrDim XMLRPC_DATA_ETAG = { "", 7 }; // end-tag +static XmlrpcTagStrDim XMLRPC_DATA_NTAG = { "", 7 }; // empty-element tag (no content) +static XmlrpcTagStrDim XMLRPC_STRUCT_TAG = { "", 8 }; +static XmlrpcTagStrDim XMLRPC_STRUCT_ETAG = { "", 9 }; +static XmlrpcTagStrDim XMLRPC_STRUCT_NTAG = { "", 9 }; +static XmlrpcTagStrDim XMLRPC_MEMBER_TAG = { "", 8 }; +static XmlrpcTagStrDim XMLRPC_MEMBER_ETAG = { "", 9 }; +static XmlrpcTagStrDim XMLRPC_NAME_TAG = { "", 6 }; +static XmlrpcTagStrDim XMLRPC_NAME_ETAG = { "", 7 }; + +/*! @}*/ + +#endif diff --git a/src/hal/components/cros/master_python_source/defusedxml/ElementTree.py b/src/hal/components/cros/master_python_source/defusedxml/ElementTree.py new file mode 100644 index 00000000..b1504e4a --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/ElementTree.py @@ -0,0 +1,143 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defused xml.etree.ElementTree facade +""" +from __future__ import print_function, absolute_import + +import sys +import warnings +from xml.etree.ElementTree import TreeBuilder as _TreeBuilder +from xml.etree.ElementTree import parse as _parse +from xml.etree.ElementTree import tostring + +from .common import PY3 + +if PY3: + import importlib +else: + from xml.etree.ElementTree import XMLParser as _XMLParser + from xml.etree.ElementTree import iterparse as _iterparse + from xml.etree.ElementTree import ParseError + + +from .common import ( + DTDForbidden, + EntitiesForbidden, + ExternalReferenceForbidden, + _generate_etree_functions, +) + +__origin__ = "xml.etree.ElementTree" + + +def _get_py3_cls(): + """Python 3.3 hides the pure Python code but defusedxml requires it. + + The code is based on test.support.import_fresh_module(). + """ + pymodname = "xml.etree.ElementTree" + cmodname = "_elementtree" + + pymod = sys.modules.pop(pymodname, None) + cmod = sys.modules.pop(cmodname, None) + + sys.modules[cmodname] = None + pure_pymod = importlib.import_module(pymodname) + if cmod is not None: + sys.modules[cmodname] = cmod + else: + sys.modules.pop(cmodname) + sys.modules[pymodname] = pymod + + _XMLParser = pure_pymod.XMLParser + _iterparse = pure_pymod.iterparse + ParseError = pure_pymod.ParseError + + return _XMLParser, _iterparse, ParseError + + +if PY3: + _XMLParser, _iterparse, ParseError = _get_py3_cls() + +_sentinel = object() + + +class DefusedXMLParser(_XMLParser): + def __init__( + self, + html=_sentinel, + target=None, + encoding=None, + forbid_dtd=False, + forbid_entities=True, + forbid_external=True, + ): + # Python 2.x old style class + _XMLParser.__init__(self, target=target, encoding=encoding) + if html is not _sentinel: + # the 'html' argument has been deprecated and ignored in all + # supported versions of Python. Python 3.8 finally removed it. + if html: + raise TypeError("'html=True' is no longer supported.") + else: + warnings.warn( + "'html' keyword argument is no longer supported. Pass " + "in arguments as keyword arguments.", + category=DeprecationWarning, + ) + + self.forbid_dtd = forbid_dtd + self.forbid_entities = forbid_entities + self.forbid_external = forbid_external + if PY3: + parser = self.parser + else: + parser = self._parser + if self.forbid_dtd: + parser.StartDoctypeDeclHandler = self.defused_start_doctype_decl + if self.forbid_entities: + parser.EntityDeclHandler = self.defused_entity_decl + parser.UnparsedEntityDeclHandler = self.defused_unparsed_entity_decl + if self.forbid_external: + parser.ExternalEntityRefHandler = self.defused_external_entity_ref_handler + + def defused_start_doctype_decl(self, name, sysid, pubid, has_internal_subset): + raise DTDForbidden(name, sysid, pubid) + + def defused_entity_decl( + self, name, is_parameter_entity, value, base, sysid, pubid, notation_name + ): + raise EntitiesForbidden(name, value, base, sysid, pubid, notation_name) + + def defused_unparsed_entity_decl(self, name, base, sysid, pubid, notation_name): + # expat 1.2 + raise EntitiesForbidden(name, None, base, sysid, pubid, notation_name) # pragma: no cover + + def defused_external_entity_ref_handler(self, context, base, sysid, pubid): + raise ExternalReferenceForbidden(context, base, sysid, pubid) + + +# aliases +# XMLParse is a typo, keep it for backwards compatibility +XMLTreeBuilder = XMLParse = XMLParser = DefusedXMLParser + +parse, iterparse, fromstring = _generate_etree_functions( + DefusedXMLParser, _TreeBuilder, _parse, _iterparse +) +XML = fromstring + + +__all__ = [ + "ParseError", + "XML", + "XMLParse", + "XMLParser", + "XMLTreeBuilder", + "fromstring", + "iterparse", + "parse", + "tostring", +] diff --git a/src/hal/components/cros/master_python_source/defusedxml/__init__.py b/src/hal/components/cros/master_python_source/defusedxml/__init__.py new file mode 100644 index 00000000..6f3b7584 --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/__init__.py @@ -0,0 +1,62 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defuse XML bomb denial of service vulnerabilities +""" +from __future__ import print_function, absolute_import + +from .common import ( + DefusedXmlException, + DTDForbidden, + EntitiesForbidden, + ExternalReferenceForbidden, + NotSupportedError, + _apply_defusing, +) + + +def defuse_stdlib(): + """Monkey patch and defuse all stdlib packages + + :warning: The monkey patch is an EXPERIMETNAL feature. + """ + defused = {} + + from . import cElementTree + from . import ElementTree + from . import minidom + from . import pulldom + from . import sax + from . import expatbuilder + from . import expatreader + from . import xmlrpc + + xmlrpc.monkey_patch() + defused[xmlrpc] = None + + for defused_mod in [ + cElementTree, + ElementTree, + minidom, + pulldom, + sax, + expatbuilder, + expatreader, + ]: + stdlib_mod = _apply_defusing(defused_mod) + defused[defused_mod] = stdlib_mod + + return defused + + +__version__ = "0.6.0" + +__all__ = [ + "DefusedXmlException", + "DTDForbidden", + "EntitiesForbidden", + "ExternalReferenceForbidden", + "NotSupportedError", +] diff --git a/src/hal/components/cros/master_python_source/defusedxml/cElementTree.py b/src/hal/components/cros/master_python_source/defusedxml/cElementTree.py new file mode 100644 index 00000000..fdc761ed --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/cElementTree.py @@ -0,0 +1,40 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defused xml.etree.cElementTree +""" +from __future__ import absolute_import + +from xml.etree.cElementTree import TreeBuilder as _TreeBuilder +from xml.etree.cElementTree import parse as _parse +from xml.etree.cElementTree import tostring + +# iterparse from ElementTree! +from xml.etree.ElementTree import iterparse as _iterparse + +from .ElementTree import DefusedXMLParser +from .common import _generate_etree_functions + +__origin__ = "xml.etree.cElementTree" + + +# XMLParse is a typo, keep it for backwards compatibility +XMLTreeBuilder = XMLParse = XMLParser = DefusedXMLParser + +parse, iterparse, fromstring = _generate_etree_functions( + DefusedXMLParser, _TreeBuilder, _parse, _iterparse +) +XML = fromstring + +__all__ = [ + "XML", + "XMLParse", + "XMLParser", + "XMLTreeBuilder", + "fromstring", + "iterparse", + "parse", + "tostring", +] diff --git a/src/hal/components/cros/master_python_source/defusedxml/common.py b/src/hal/components/cros/master_python_source/defusedxml/common.py new file mode 100644 index 00000000..7e4bb0bd --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/common.py @@ -0,0 +1,134 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Common constants, exceptions and helpe functions +""" +import sys +import xml.parsers.expat + +PY3 = sys.version_info[0] == 3 + +# Fail early when pyexpat is not installed correctly +if not hasattr(xml.parsers.expat, "ParserCreate"): + raise ImportError("pyexpat") # pragma: no cover + + +class DefusedXmlException(ValueError): + """Base exception + """ + + def __repr__(self): + return str(self) + + +class DTDForbidden(DefusedXmlException): + """Document type definition is forbidden + """ + + def __init__(self, name, sysid, pubid): + super(DTDForbidden, self).__init__() + self.name = name + self.sysid = sysid + self.pubid = pubid + + def __str__(self): + tpl = "DTDForbidden(name='{}', system_id={!r}, public_id={!r})" + return tpl.format(self.name, self.sysid, self.pubid) + + +class EntitiesForbidden(DefusedXmlException): + """Entity definition is forbidden + """ + + def __init__(self, name, value, base, sysid, pubid, notation_name): + super(EntitiesForbidden, self).__init__() + self.name = name + self.value = value + self.base = base + self.sysid = sysid + self.pubid = pubid + self.notation_name = notation_name + + def __str__(self): + tpl = "EntitiesForbidden(name='{}', system_id={!r}, public_id={!r})" + return tpl.format(self.name, self.sysid, self.pubid) + + +class ExternalReferenceForbidden(DefusedXmlException): + """Resolving an external reference is forbidden + """ + + def __init__(self, context, base, sysid, pubid): + super(ExternalReferenceForbidden, self).__init__() + self.context = context + self.base = base + self.sysid = sysid + self.pubid = pubid + + def __str__(self): + tpl = "ExternalReferenceForbidden(system_id='{}', public_id={})" + return tpl.format(self.sysid, self.pubid) + + +class NotSupportedError(DefusedXmlException): + """The operation is not supported + """ + + +def _apply_defusing(defused_mod): + assert defused_mod is sys.modules[defused_mod.__name__] + stdlib_name = defused_mod.__origin__ + __import__(stdlib_name, {}, {}, ["*"]) + stdlib_mod = sys.modules[stdlib_name] + stdlib_names = set(dir(stdlib_mod)) + for name, obj in vars(defused_mod).items(): + if name.startswith("_") or name not in stdlib_names: + continue + setattr(stdlib_mod, name, obj) + return stdlib_mod + + +def _generate_etree_functions(DefusedXMLParser, _TreeBuilder, _parse, _iterparse): + """Factory for functions needed by etree, dependent on whether + cElementTree or ElementTree is used.""" + + def parse(source, parser=None, forbid_dtd=False, forbid_entities=True, forbid_external=True): + if parser is None: + parser = DefusedXMLParser( + target=_TreeBuilder(), + forbid_dtd=forbid_dtd, + forbid_entities=forbid_entities, + forbid_external=forbid_external, + ) + return _parse(source, parser) + + def iterparse( + source, + events=None, + parser=None, + forbid_dtd=False, + forbid_entities=True, + forbid_external=True, + ): + if parser is None: + parser = DefusedXMLParser( + target=_TreeBuilder(), + forbid_dtd=forbid_dtd, + forbid_entities=forbid_entities, + forbid_external=forbid_external, + ) + return _iterparse(source, events, parser) + + def fromstring(text, forbid_dtd=False, forbid_entities=True, forbid_external=True): + parser = DefusedXMLParser( + target=_TreeBuilder(), + forbid_dtd=forbid_dtd, + forbid_entities=forbid_entities, + forbid_external=forbid_external, + ) + parser.feed(text) + return parser.close() + + return parse, iterparse, fromstring diff --git a/src/hal/components/cros/master_python_source/defusedxml/expatbuilder.py b/src/hal/components/cros/master_python_source/defusedxml/expatbuilder.py new file mode 100644 index 00000000..7bfc57e4 --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/expatbuilder.py @@ -0,0 +1,107 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defused xml.dom.expatbuilder +""" +from __future__ import print_function, absolute_import + +from xml.dom.expatbuilder import ExpatBuilder as _ExpatBuilder +from xml.dom.expatbuilder import Namespaces as _Namespaces + +from .common import DTDForbidden, EntitiesForbidden, ExternalReferenceForbidden + +__origin__ = "xml.dom.expatbuilder" + + +class DefusedExpatBuilder(_ExpatBuilder): + """Defused document builder""" + + def __init__( + self, options=None, forbid_dtd=False, forbid_entities=True, forbid_external=True + ): + _ExpatBuilder.__init__(self, options) + self.forbid_dtd = forbid_dtd + self.forbid_entities = forbid_entities + self.forbid_external = forbid_external + + def defused_start_doctype_decl(self, name, sysid, pubid, has_internal_subset): + raise DTDForbidden(name, sysid, pubid) + + def defused_entity_decl( + self, name, is_parameter_entity, value, base, sysid, pubid, notation_name + ): + raise EntitiesForbidden(name, value, base, sysid, pubid, notation_name) + + def defused_unparsed_entity_decl(self, name, base, sysid, pubid, notation_name): + # expat 1.2 + raise EntitiesForbidden(name, None, base, sysid, pubid, notation_name) # pragma: no cover + + def defused_external_entity_ref_handler(self, context, base, sysid, pubid): + raise ExternalReferenceForbidden(context, base, sysid, pubid) + + def install(self, parser): + _ExpatBuilder.install(self, parser) + + if self.forbid_dtd: + parser.StartDoctypeDeclHandler = self.defused_start_doctype_decl + if self.forbid_entities: + # if self._options.entities: + parser.EntityDeclHandler = self.defused_entity_decl + parser.UnparsedEntityDeclHandler = self.defused_unparsed_entity_decl + if self.forbid_external: + parser.ExternalEntityRefHandler = self.defused_external_entity_ref_handler + + +class DefusedExpatBuilderNS(_Namespaces, DefusedExpatBuilder): + """Defused document builder that supports namespaces.""" + + def install(self, parser): + DefusedExpatBuilder.install(self, parser) + if self._options.namespace_declarations: + parser.StartNamespaceDeclHandler = self.start_namespace_decl_handler + + def reset(self): + DefusedExpatBuilder.reset(self) + self._initNamespaces() + + +def parse(file, namespaces=True, forbid_dtd=False, forbid_entities=True, forbid_external=True): + """Parse a document, returning the resulting Document node. + + 'file' may be either a file name or an open file object. + """ + if namespaces: + build_builder = DefusedExpatBuilderNS + else: + build_builder = DefusedExpatBuilder + builder = build_builder( + forbid_dtd=forbid_dtd, forbid_entities=forbid_entities, forbid_external=forbid_external + ) + + if isinstance(file, str): + fp = open(file, "rb") + try: + result = builder.parseFile(fp) + finally: + fp.close() + else: + result = builder.parseFile(file) + return result + + +def parseString( + string, namespaces=True, forbid_dtd=False, forbid_entities=True, forbid_external=True +): + """Parse a document from a string, returning the resulting + Document node. + """ + if namespaces: + build_builder = DefusedExpatBuilderNS + else: + build_builder = DefusedExpatBuilder + builder = build_builder( + forbid_dtd=forbid_dtd, forbid_entities=forbid_entities, forbid_external=forbid_external + ) + return builder.parseString(string) diff --git a/src/hal/components/cros/master_python_source/defusedxml/expatreader.py b/src/hal/components/cros/master_python_source/defusedxml/expatreader.py new file mode 100644 index 00000000..890e1d16 --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/expatreader.py @@ -0,0 +1,61 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defused xml.sax.expatreader +""" +from __future__ import print_function, absolute_import + +from xml.sax.expatreader import ExpatParser as _ExpatParser + +from .common import DTDForbidden, EntitiesForbidden, ExternalReferenceForbidden + +__origin__ = "xml.sax.expatreader" + + +class DefusedExpatParser(_ExpatParser): + """Defused SAX driver for the pyexpat C module.""" + + def __init__( + self, + namespaceHandling=0, + bufsize=2 ** 16 - 20, + forbid_dtd=False, + forbid_entities=True, + forbid_external=True, + ): + _ExpatParser.__init__(self, namespaceHandling, bufsize) + self.forbid_dtd = forbid_dtd + self.forbid_entities = forbid_entities + self.forbid_external = forbid_external + + def defused_start_doctype_decl(self, name, sysid, pubid, has_internal_subset): + raise DTDForbidden(name, sysid, pubid) + + def defused_entity_decl( + self, name, is_parameter_entity, value, base, sysid, pubid, notation_name + ): + raise EntitiesForbidden(name, value, base, sysid, pubid, notation_name) + + def defused_unparsed_entity_decl(self, name, base, sysid, pubid, notation_name): + # expat 1.2 + raise EntitiesForbidden(name, None, base, sysid, pubid, notation_name) # pragma: no cover + + def defused_external_entity_ref_handler(self, context, base, sysid, pubid): + raise ExternalReferenceForbidden(context, base, sysid, pubid) + + def reset(self): + _ExpatParser.reset(self) + parser = self._parser + if self.forbid_dtd: + parser.StartDoctypeDeclHandler = self.defused_start_doctype_decl + if self.forbid_entities: + parser.EntityDeclHandler = self.defused_entity_decl + parser.UnparsedEntityDeclHandler = self.defused_unparsed_entity_decl + if self.forbid_external: + parser.ExternalEntityRefHandler = self.defused_external_entity_ref_handler + + +def create_parser(*args, **kwargs): + return DefusedExpatParser(*args, **kwargs) diff --git a/src/hal/components/cros/master_python_source/defusedxml/lxml.py b/src/hal/components/cros/master_python_source/defusedxml/lxml.py new file mode 100644 index 00000000..1320ca52 --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/lxml.py @@ -0,0 +1,155 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""DEPRECATED Example code for lxml.etree protection + +The code has NO protection against decompression bombs. +""" +from __future__ import print_function, absolute_import + +import threading +import warnings + +from lxml import etree as _etree + +from .common import DTDForbidden, EntitiesForbidden, NotSupportedError + +LXML3 = _etree.LXML_VERSION[0] >= 3 + +__origin__ = "lxml.etree" + +tostring = _etree.tostring + + +warnings.warn( + "defusedxml.lxml is no longer supported and will be removed in a " "future release.", + category=DeprecationWarning, + stacklevel=2, +) + + +class RestrictedElement(_etree.ElementBase): + """A restricted Element class that filters out instances of some classes + """ + + __slots__ = () + # blacklist = (etree._Entity, etree._ProcessingInstruction, etree._Comment) + blacklist = _etree._Entity + + def _filter(self, iterator): + blacklist = self.blacklist + for child in iterator: + if isinstance(child, blacklist): + continue + yield child + + def __iter__(self): + iterator = super(RestrictedElement, self).__iter__() + return self._filter(iterator) + + def iterchildren(self, tag=None, reversed=False): + iterator = super(RestrictedElement, self).iterchildren(tag=tag, reversed=reversed) + return self._filter(iterator) + + def iter(self, tag=None, *tags): + iterator = super(RestrictedElement, self).iter(tag=tag, *tags) + return self._filter(iterator) + + def iterdescendants(self, tag=None, *tags): + iterator = super(RestrictedElement, self).iterdescendants(tag=tag, *tags) + return self._filter(iterator) + + def itersiblings(self, tag=None, preceding=False): + iterator = super(RestrictedElement, self).itersiblings(tag=tag, preceding=preceding) + return self._filter(iterator) + + def getchildren(self): + iterator = super(RestrictedElement, self).__iter__() + return list(self._filter(iterator)) + + def getiterator(self, tag=None): + iterator = super(RestrictedElement, self).getiterator(tag) + return self._filter(iterator) + + +class GlobalParserTLS(threading.local): + """Thread local context for custom parser instances + """ + + parser_config = { + "resolve_entities": False, + # 'remove_comments': True, + # 'remove_pis': True, + } + + element_class = RestrictedElement + + def createDefaultParser(self): + parser = _etree.XMLParser(**self.parser_config) + element_class = self.element_class + if self.element_class is not None: + lookup = _etree.ElementDefaultClassLookup(element=element_class) + parser.set_element_class_lookup(lookup) + return parser + + def setDefaultParser(self, parser): + self._default_parser = parser + + def getDefaultParser(self): + parser = getattr(self, "_default_parser", None) + if parser is None: + parser = self.createDefaultParser() + self.setDefaultParser(parser) + return parser + + +_parser_tls = GlobalParserTLS() +getDefaultParser = _parser_tls.getDefaultParser + + +def check_docinfo(elementtree, forbid_dtd=False, forbid_entities=True): + """Check docinfo of an element tree for DTD and entity declarations + + The check for entity declarations needs lxml 3 or newer. lxml 2.x does + not support dtd.iterentities(). + """ + docinfo = elementtree.docinfo + if docinfo.doctype: + if forbid_dtd: + raise DTDForbidden(docinfo.doctype, docinfo.system_url, docinfo.public_id) + if forbid_entities and not LXML3: + # lxml < 3 has no iterentities() + raise NotSupportedError("Unable to check for entity declarations " "in lxml 2.x") + + if forbid_entities: + for dtd in docinfo.internalDTD, docinfo.externalDTD: + if dtd is None: + continue + for entity in dtd.iterentities(): + raise EntitiesForbidden(entity.name, entity.content, None, None, None, None) + + +def parse(source, parser=None, base_url=None, forbid_dtd=False, forbid_entities=True): + if parser is None: + parser = getDefaultParser() + elementtree = _etree.parse(source, parser, base_url=base_url) + check_docinfo(elementtree, forbid_dtd, forbid_entities) + return elementtree + + +def fromstring(text, parser=None, base_url=None, forbid_dtd=False, forbid_entities=True): + if parser is None: + parser = getDefaultParser() + rootelement = _etree.fromstring(text, parser, base_url=base_url) + elementtree = rootelement.getroottree() + check_docinfo(elementtree, forbid_dtd, forbid_entities) + return rootelement + + +XML = fromstring + + +def iterparse(*args, **kwargs): + raise NotSupportedError("defused lxml.etree.iterparse not available") diff --git a/src/hal/components/cros/master_python_source/defusedxml/minidom.py b/src/hal/components/cros/master_python_source/defusedxml/minidom.py new file mode 100644 index 00000000..78033b6c --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/minidom.py @@ -0,0 +1,63 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defused xml.dom.minidom +""" +from __future__ import print_function, absolute_import + +from xml.dom.minidom import _do_pulldom_parse +from . import expatbuilder as _expatbuilder +from . import pulldom as _pulldom + +__origin__ = "xml.dom.minidom" + + +def parse( + file, parser=None, bufsize=None, forbid_dtd=False, forbid_entities=True, forbid_external=True +): + """Parse a file into a DOM by filename or file object.""" + if parser is None and not bufsize: + return _expatbuilder.parse( + file, + forbid_dtd=forbid_dtd, + forbid_entities=forbid_entities, + forbid_external=forbid_external, + ) + else: + return _do_pulldom_parse( + _pulldom.parse, + (file,), + { + "parser": parser, + "bufsize": bufsize, + "forbid_dtd": forbid_dtd, + "forbid_entities": forbid_entities, + "forbid_external": forbid_external, + }, + ) + + +def parseString( + string, parser=None, forbid_dtd=False, forbid_entities=True, forbid_external=True +): + """Parse a file into a DOM from a string.""" + if parser is None: + return _expatbuilder.parseString( + string, + forbid_dtd=forbid_dtd, + forbid_entities=forbid_entities, + forbid_external=forbid_external, + ) + else: + return _do_pulldom_parse( + _pulldom.parseString, + (string,), + { + "parser": parser, + "forbid_dtd": forbid_dtd, + "forbid_entities": forbid_entities, + "forbid_external": forbid_external, + }, + ) diff --git a/src/hal/components/cros/master_python_source/defusedxml/pulldom.py b/src/hal/components/cros/master_python_source/defusedxml/pulldom.py new file mode 100644 index 00000000..e3b10a46 --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/pulldom.py @@ -0,0 +1,41 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defused xml.dom.pulldom +""" +from __future__ import print_function, absolute_import + +from xml.dom.pulldom import parse as _parse +from xml.dom.pulldom import parseString as _parseString +from .sax import make_parser + +__origin__ = "xml.dom.pulldom" + + +def parse( + stream_or_string, + parser=None, + bufsize=None, + forbid_dtd=False, + forbid_entities=True, + forbid_external=True, +): + if parser is None: + parser = make_parser() + parser.forbid_dtd = forbid_dtd + parser.forbid_entities = forbid_entities + parser.forbid_external = forbid_external + return _parse(stream_or_string, parser, bufsize) + + +def parseString( + string, parser=None, forbid_dtd=False, forbid_entities=True, forbid_external=True +): + if parser is None: + parser = make_parser() + parser.forbid_dtd = forbid_dtd + parser.forbid_entities = forbid_entities + parser.forbid_external = forbid_external + return _parseString(string, parser) diff --git a/src/hal/components/cros/master_python_source/defusedxml/sax.py b/src/hal/components/cros/master_python_source/defusedxml/sax.py new file mode 100644 index 00000000..b2786f74 --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/sax.py @@ -0,0 +1,60 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defused xml.sax +""" +from __future__ import print_function, absolute_import + +from xml.sax import InputSource as _InputSource +from xml.sax import ErrorHandler as _ErrorHandler + +from . import expatreader + +__origin__ = "xml.sax" + + +def parse( + source, + handler, + errorHandler=_ErrorHandler(), + forbid_dtd=False, + forbid_entities=True, + forbid_external=True, +): + parser = make_parser() + parser.setContentHandler(handler) + parser.setErrorHandler(errorHandler) + parser.forbid_dtd = forbid_dtd + parser.forbid_entities = forbid_entities + parser.forbid_external = forbid_external + parser.parse(source) + + +def parseString( + string, + handler, + errorHandler=_ErrorHandler(), + forbid_dtd=False, + forbid_entities=True, + forbid_external=True, +): + from io import BytesIO + + if errorHandler is None: + errorHandler = _ErrorHandler() + parser = make_parser() + parser.setContentHandler(handler) + parser.setErrorHandler(errorHandler) + parser.forbid_dtd = forbid_dtd + parser.forbid_entities = forbid_entities + parser.forbid_external = forbid_external + + inpsrc = _InputSource() + inpsrc.setByteStream(BytesIO(string)) + parser.parse(inpsrc) + + +def make_parser(parser_list=[]): + return expatreader.create_parser() diff --git a/src/hal/components/cros/master_python_source/defusedxml/xmlrpc.py b/src/hal/components/cros/master_python_source/defusedxml/xmlrpc.py new file mode 100644 index 00000000..fbc674da --- /dev/null +++ b/src/hal/components/cros/master_python_source/defusedxml/xmlrpc.py @@ -0,0 +1,153 @@ +# defusedxml +# +# Copyright (c) 2013 by Christian Heimes +# Licensed to PSF under a Contributor Agreement. +# See https://www.python.org/psf/license for licensing details. +"""Defused xmlrpclib + +Also defuses gzip bomb +""" +from __future__ import print_function, absolute_import + +import io + +from .common import DTDForbidden, EntitiesForbidden, ExternalReferenceForbidden, PY3 + +if PY3: + __origin__ = "xmlrpc.client" + from xmlrpc.client import ExpatParser + from xmlrpc import client as xmlrpc_client + from xmlrpc import server as xmlrpc_server + from xmlrpc.client import gzip_decode as _orig_gzip_decode + from xmlrpc.client import GzipDecodedResponse as _OrigGzipDecodedResponse +else: + __origin__ = "xmlrpclib" + from xmlrpclib import ExpatParser + import xmlrpclib as xmlrpc_client + + xmlrpc_server = None + from xmlrpclib import gzip_decode as _orig_gzip_decode + from xmlrpclib import GzipDecodedResponse as _OrigGzipDecodedResponse + +try: + import gzip +except ImportError: # pragma: no cover + gzip = None + + +# Limit maximum request size to prevent resource exhaustion DoS +# Also used to limit maximum amount of gzip decoded data in order to prevent +# decompression bombs +# A value of -1 or smaller disables the limit +MAX_DATA = 30 * 1024 * 1024 # 30 MB + + +def defused_gzip_decode(data, limit=None): + """gzip encoded data -> unencoded data + + Decode data using the gzip content encoding as described in RFC 1952 + """ + if not gzip: # pragma: no cover + raise NotImplementedError + if limit is None: + limit = MAX_DATA + f = io.BytesIO(data) + gzf = gzip.GzipFile(mode="rb", fileobj=f) + try: + if limit < 0: # no limit + decoded = gzf.read() + else: + decoded = gzf.read(limit + 1) + except IOError: # pragma: no cover + raise ValueError("invalid data") + f.close() + gzf.close() + if limit >= 0 and len(decoded) > limit: + raise ValueError("max gzipped payload length exceeded") + return decoded + + +class DefusedGzipDecodedResponse(gzip.GzipFile if gzip else object): + """a file-like object to decode a response encoded with the gzip + method, as described in RFC 1952. + """ + + def __init__(self, response, limit=None): + # response doesn't support tell() and read(), required by + # GzipFile + if not gzip: # pragma: no cover + raise NotImplementedError + self.limit = limit = limit if limit is not None else MAX_DATA + if limit < 0: # no limit + data = response.read() + self.readlength = None + else: + data = response.read(limit + 1) + self.readlength = 0 + if limit >= 0 and len(data) > limit: + raise ValueError("max payload length exceeded") + self.stringio = io.BytesIO(data) + gzip.GzipFile.__init__(self, mode="rb", fileobj=self.stringio) + + def read(self, n): + if self.limit >= 0: + left = self.limit - self.readlength + n = min(n, left + 1) + data = gzip.GzipFile.read(self, n) + self.readlength += len(data) + if self.readlength > self.limit: + raise ValueError("max payload length exceeded") + return data + else: + return gzip.GzipFile.read(self, n) + + def close(self): + gzip.GzipFile.close(self) + self.stringio.close() + + +class DefusedExpatParser(ExpatParser): + def __init__(self, target, forbid_dtd=False, forbid_entities=True, forbid_external=True): + ExpatParser.__init__(self, target) + self.forbid_dtd = forbid_dtd + self.forbid_entities = forbid_entities + self.forbid_external = forbid_external + parser = self._parser + if self.forbid_dtd: + parser.StartDoctypeDeclHandler = self.defused_start_doctype_decl + if self.forbid_entities: + parser.EntityDeclHandler = self.defused_entity_decl + parser.UnparsedEntityDeclHandler = self.defused_unparsed_entity_decl + if self.forbid_external: + parser.ExternalEntityRefHandler = self.defused_external_entity_ref_handler + + def defused_start_doctype_decl(self, name, sysid, pubid, has_internal_subset): + raise DTDForbidden(name, sysid, pubid) + + def defused_entity_decl( + self, name, is_parameter_entity, value, base, sysid, pubid, notation_name + ): + raise EntitiesForbidden(name, value, base, sysid, pubid, notation_name) + + def defused_unparsed_entity_decl(self, name, base, sysid, pubid, notation_name): + # expat 1.2 + raise EntitiesForbidden(name, None, base, sysid, pubid, notation_name) # pragma: no cover + + def defused_external_entity_ref_handler(self, context, base, sysid, pubid): + raise ExternalReferenceForbidden(context, base, sysid, pubid) + + +def monkey_patch(): + xmlrpc_client.FastParser = DefusedExpatParser + xmlrpc_client.GzipDecodedResponse = DefusedGzipDecodedResponse + xmlrpc_client.gzip_decode = defused_gzip_decode + if xmlrpc_server: + xmlrpc_server.gzip_decode = defused_gzip_decode + + +def unmonkey_patch(): + xmlrpc_client.FastParser = None + xmlrpc_client.GzipDecodedResponse = _OrigGzipDecodedResponse + xmlrpc_client.gzip_decode = _orig_gzip_decode + if xmlrpc_server: + xmlrpc_server.gzip_decode = _orig_gzip_decode diff --git a/src/hal/components/cros/master_python_source/rosgraph/__init__.py b/src/hal/components/cros/master_python_source/rosgraph/__init__.py new file mode 100644 index 00000000..44bc101e --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosgraph/__init__.py @@ -0,0 +1,56 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. + +import sys + +from . rosenv import get_master_uri, ROS_MASTER_URI, ROS_NAMESPACE, ROS_HOSTNAME, ROS_IP, ROS_IPV6 +from . masterapi import Master, MasterFailure, MasterError, MasterException +from . masterapi import is_online as is_master_online + +# bring in names submodule +from . import names + +def myargv(argv=None): + """ + Remove ROS remapping arguments from sys.argv arguments. + + :returns: copy of sys.argv with ROS remapping arguments removed, ``[str]`` + """ + if argv is None: + argv = sys.argv + return [a for a in argv if not names.REMAP in a] + +__all__ = ['myargv', + 'get_master_uri', 'ROS_MASTER_URI', 'ROS_NAMESPACE', 'ROS_HOSTNAME', 'ROS_IP', 'ROS_IPV6', + 'Master', 'MasterFailure', 'MasterError', 'MasterException', + 'is_master_online'] + diff --git a/src/hal/components/cros/master_python_source/rosgraph/impl/__init__.py b/src/hal/components/cros/master_python_source/rosgraph/impl/__init__.py new file mode 100644 index 00000000..0212c3f0 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosgraph/impl/__init__.py @@ -0,0 +1,34 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id: __init__.py 5735 2009-08-20 21:31:27Z sfkwc $ + diff --git a/src/hal/components/cros/master_python_source/rosgraph/impl/graph.py b/src/hal/components/cros/master_python_source/rosgraph/impl/graph.py new file mode 100644 index 00000000..5f78df97 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosgraph/impl/graph.py @@ -0,0 +1,582 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +from __future__ import print_function + +""" +Data structures and library for representing ROS Computation Graph state. +""" + +import sys +import time +import itertools +import random +import logging +import traceback +try: + from xmlrpc.client import ServerProxy +except ImportError: + from xmlrpclib import ServerProxy +import socket + +import rosgraph.masterapi + +logger = logging.getLogger('rosgraph.graph') + +_ROS_NAME = '/rosviz' + +def topic_node(topic): + """ + In order to prevent topic/node name aliasing, we have to remap + topic node names. Currently we just prepend a space, which is + an illegal ROS name and thus not aliased. + @return str: topic mapped to a graph node name. + """ + return ' ' + topic +def node_topic(node): + """ + Inverse of topic_node + @return str: undo topic_node() operation + """ + return node[1:] + +class BadNode(object): + """ + Data structure for storing info about a 'bad' node + """ + + ## no connectivity + DEAD = 0 + ## intermittent connectivity + WONKY = 1 + + def __init__(self, name, type, reason): + """ + @param type: DEAD | WONKY + @type type: int + """ + self.name =name + self.reason = reason + self.type = type + +class EdgeList(object): + """ + Data structure for storing Edge instances + """ + __slots__ = ['edges_by_start', 'edges_by_end'] + def __init__(self): + # in order to make it easy to purge edges, we double-index them + self.edges_by_start = {} + self.edges_by_end = {} + + def __iter__(self): + return itertools.chain(*[v for v in self.edges_by_start.values()]) + + def has(self, edge): + return edge in self + + def __contains__(self, edge): + """ + @return: True if edge is in edge list + @rtype: bool + """ + key = edge.key + return key in self.edges_by_start and \ + edge in self.edges_by_start[key] + + def add(self, edge): + """ + Add an edge to our internal representation. not multi-thread safe + @param edge: edge to add + @type edge: Edge + """ + # see note in __init__ + def update_map(map, key, edge): + if key in map: + l = map[key] + if not edge in l: + l.append(edge) + return True + else: + return False + else: + map[key] = [edge] + return True + + updated = update_map(self.edges_by_start, edge.key, edge) + updated = update_map(self.edges_by_end, edge.rkey, edge) or updated + return updated + + def add_edges(self, start, dest, direction, label=''): + """ + Create Edge instances for args and add resulting edges to edge + list. Convenience method to avoid repetitve logging, etc... + @param edge_list: data structure to add edge to + @type edge_list: EdgeList + @param start: name of start node. If None, warning will be logged and add fails + @type start: str + @param dest: name of start node. If None, warning will be logged and add fails + @type dest: str + @param direction: direction string (i/o/b) + @type direction: str + @return: True if update occured + @rtype: bool + """ + + # the warnings should generally be temporary, occuring of the + # master/node information becomes stale while we are still + # doing an update + updated = False + if not start: + logger.warn("cannot add edge: cannot map start [%s] to a node name", start) + elif not dest: + logger.warn("cannot add edge: cannot map dest [%s] to a node name", dest) + else: + for args in edge_args(start, dest, direction, label): + updated = self.add(Edge(*args)) or updated + return updated + + def delete_all(self, node): + """ + Delete all edges that start or end at node + @param node: name of node + @type node: str + """ + def matching(map, pref): + return [map[k] for k in map.keys() if k.startswith(pref)] + + pref = node+"|" + edge_lists = matching(self.edges_by_start, pref) + matching(self.edges_by_end, pref) + for el in edge_lists: + for e in el: + self.delete(e) + + def delete(self, edge): + # see note in __init__ + def update_map(map, key, edge): + if key in map: + edges = map[key] + if edge in edges: + edges.remove(edge) + return True + update_map(self.edges_by_start, edge.key, edge) + update_map(self.edges_by_end, edge.rkey, edge) + +class Edge(object): + """ + Data structure for representing ROS node graph edge + """ + + __slots__ = ['start', 'end', 'label', 'key', 'rkey'] + def __init__(self, start, end, label=''): + self.start = start + self.end = end + self.label = label + self.key = "%s|%s"%(self.start, self.label) + # reverse key, indexed from end + self.rkey = "%s|%s"%(self.end, self.label) + + def __ne__(self, other): + return self.start != other.start or self.end != other.end + def __str__(self): + return "%s->%s"%(self.start, self.end) + def __eq__(self, other): + return self.start == other.start and self.end == other.end + +def edge_args(start, dest, direction, label): + """ + compute argument ordering for Edge constructor based on direction flag + @param direction str: 'i', 'o', or 'b' (in/out/bidir) relative to \a start + @param start str: name of starting node + @param start dest: name of destination node + """ + edge_args = [] + if direction in ['o', 'b']: + edge_args.append((start, dest, label)) + if direction in ['i', 'b']: + edge_args.append((dest, start, label)) + return edge_args + + +class Graph(object): + """ + Utility class for polling ROS statistics from running ROS graph. + Not multi-thread-safe + """ + + def __init__(self, node_ns='/', topic_ns='/'): + self.master = rosgraph.masterapi.Master(_ROS_NAME) + + self.node_ns = node_ns or '/' + self.topic_ns = topic_ns or '/' + + # ROS nodes + self.nn_nodes = set([]) + # ROS topic nodes + self.nt_nodes = set([]) + + # ROS nodes that aren't responding quickly + self.bad_nodes = {} + import threading + self.bad_nodes_lock = threading.Lock() + + # ROS services + self.srvs = set([]) + # ROS node->node transport connections + self.nn_edges = EdgeList() + # ROS node->topic connections + self.nt_edges = EdgeList() + # ROS all node->topic connections, including empty + self.nt_all_edges = EdgeList() + + # node names to URI map + self.node_uri_map = {} # { node_name_str : uri_str } + # reverse map URIs to node names + self.uri_node_map = {} # { uri_str : node_name_str } + + # time we last contacted master + self.last_master_refresh = 0 + self.last_node_refresh = {} + + # time we last communicated with master + # seconds until master data is considered stale + self.master_stale = 5.0 + # time we last communicated with node + # seconds until node data is considered stale + self.node_stale = 5.0 #seconds + + + def set_master_stale(self, stale_secs): + """ + @param stale_secs: seconds that data is considered fresh + @type stale_secs: double + """ + self.master_stale = stale_secs + + def set_node_stale(self, stale_secs): + """ + @param stale_secs: seconds that data is considered fresh + @type stale_secs: double + """ + self.node_stale = stale_secs + + def _master_refresh(self): + """ + @return: True if nodes information was updated + @rtype: bool + """ + logger.debug("master refresh: starting") + updated = False + try: + val = self.master.getSystemState() + except rosgraph.masterapi.MasterException as e: + print("Unable to contact master", str(e), file=sys.stderr) + logger.error("unable to contact master: %s", str(e)) + return False + + pubs, subs, srvs = val + + nodes = [] + nt_all_edges = self.nt_all_edges + nt_nodes = self.nt_nodes + for state, direction in ((pubs, 'o'), (subs, 'i')): + for topic, l in state: + if topic.startswith(self.topic_ns): + nodes.extend([n for n in l if n.startswith(self.node_ns)]) + nt_nodes.add(topic_node(topic)) + for node in l: + updated = nt_all_edges.add_edges( + node, topic_node(topic), direction) or updated + + nodes = set(nodes) + + srvs = set([s for s, _ in srvs]) + purge = None + if nodes ^ self.nn_nodes: + purge = self.nn_nodes - nodes + self.nn_nodes = nodes + updated = True + if srvs ^ self.srvs: + self.srvs = srvs + updated = True + + if purge: + logger.debug("following nodes and related edges will be purged: %s", ','.join(purge)) + for p in purge: + logger.debug('purging edges for node %s', p) + self.nn_edges.delete_all(p) + self.nt_edges.delete_all(p) + self.nt_all_edges.delete_all(p) + + logger.debug("master refresh: done, updated[%s]", updated) + return updated + + def _mark_bad_node(self, node, reason): + try: + # bad nodes are updated in a separate thread, so lock + self.bad_nodes_lock.acquire() + if node in self.bad_nodes: + self.bad_nodes[node].type = BadNode.DEAD + else: + self.bad_nodes[node] = (BadNode(node, BadNode.DEAD, reason)) + finally: + self.bad_nodes_lock.release() + + def _unmark_bad_node(self, node, reason): + """ + Promotes bad node to 'wonky' status. + """ + try: + # bad nodes are updated in a separate thread, so lock + self.bad_nodes_lock.acquire() + bad = self.bad_nodes[node] + bad.type = BadNode.WONKY + finally: + self.bad_nodes_lock.release() + + def _node_refresh_businfo(self, node, api, bad_node=False): + """ + Retrieve bus info from the node and update nodes and edges as appropriate + @param node: node name + @type node: str + @param api: XML-RPC proxy + @type api: ServerProxy + @param bad_node: If True, node has connectivity issues and + should be treated differently + @type bad_node: bool + """ + try: + logger.debug("businfo: contacting node [%s] for bus info", node) + + # unmark bad node, though it stays on the bad list + if bad_node: + self._unmark_bad_node(node) + # Lower the socket timeout as we cannot abide by slow HTTP timeouts. + # If a node cannot meet this timeout, it goes on the bad list + # TODO: override transport instead. + old_timeout = socket.getdefaulttimeout() + if bad_node: + #even stricter timeout for bad_nodes right now + socket.setdefaulttimeout(0.2) + else: + socket.setdefaulttimeout(1.0) + + code, msg, bus_info = api.getBusInfo(_ROS_NAME) + + socket.setdefaulttimeout(old_timeout) + except Exception as e: + # node is (still) bad + self._mark_bad_node(node, str(e)) + code = -1 + msg = traceback.format_exc() + + updated = False + if code != 1: + logger.error("cannot get stats info from node [%s]: %s", node, msg) + else: + # [[connectionId1, destinationId1, direction1, transport1, ...]... ] + for info in bus_info: + # #3579 bad node, ignore + if len(info) < 5: + continue + + connection_id = info[0] + dest_id = info[1] + direction = info[2] + transport = info[3] + topic = info[4] + if len(info) > 5: + connected = info[5] + else: + connected = True #backwards compatibility + + if connected and topic.startswith(self.topic_ns): + # blindly add as we will be able to catch state change via edges. + # this currently means we don't cleanup topics + self.nt_nodes.add(topic_node(topic)) + + # update node->topic->node graph edges + updated = self.nt_edges.add_edges(node, topic_node(topic), direction) or updated + + # update node->node graph edges + if dest_id.startswith('http://'): + #print("FOUND URI", dest_id) + dest_name = self.uri_node_map.get(dest_id, None) + updated = self.nn_edges.add_edges(node, dest_name, direction, topic) or updated + else: + #TODO: anyting to do here? + pass + return updated + + def _node_refresh(self, node, bad_node=False): + """ + Contact node for stats/connectivity information + @param node: name of node to contact + @type node: str + @param bad_node: if True, node has connectivity issues + @type bad_node: bool + @return: True if node was successfully contacted + @rtype bool + """ + # TODO: I'd like for master to provide this information in + # getSystemState() instead to prevent the extra connection per node + updated = False + uri = self._node_uri_refresh(node) + try: + if uri: + api = ServerProxy(uri) + updated = self._node_refresh_businfo(node, api, bad_node) + except KeyError as e: + logger.warn('cannot contact node [%s] as it is not in the lookup table'%node) + return updated + + def _node_uri_refresh(self, node): + try: + uri = self.master.lookupNode(node) + except: + msg = traceback.format_exc() + logger.warn("master reported error in node lookup: %s"%msg) + return None + # update maps + self.node_uri_map[node] = uri + self.uri_node_map[uri] = node + return uri + + def _node_uri_refresh_all(self): + """ + Build self.node_uri_map and self.uri_node_map using master as a + lookup service. This will make N requests to the master for N + nodes, so this should only be used sparingly + """ + for node in self.nn_nodes: + self._node_uri_refresh(node) + + def bad_update(self): + """ + Update loop for nodes with bad connectivity. We box them separately + so that we can maintain the good performance of the normal update loop. + Once a node is on the bad list it stays there. + """ + last_node_refresh = self.last_node_refresh + + # nodes left to check + try: + self.bad_nodes_lock.acquire() + # make copy due to multithreading + update_queue = self.bad_nodes.values()[:] + finally: + self.bad_nodes_lock.release() + + # return value. True if new data differs from old + updated = False + # number of nodes we checked + num_nodes = 0 + + start_time = time.time() + while update_queue: + # figure out the next node to contact + next = update_queue.pop() + # rate limit talking to any particular node + if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale): + updated = self._node_refresh(next.name, True) or updated + # include in random offset (max 1/5th normal update interval) + # to help spread out updates + last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0) + num_nodes += 1 + + # small yield to keep from torquing the processor + time.sleep(0.01) + end_time = time.time() + #print("Update (bad nodes) took %ss for %s nodes"%((end_time-start_time), num_nodes)) + logger.debug("ROS stats (bad nodes) update took %ss"%(end_time-start_time)) + return updated + + def update(self): + """ + Update all the stats. This method may take a while to complete as it will + communicate with all nodes + master. + """ + + last_node_refresh = self.last_node_refresh + + # nodes left to check + update_queue = None + # True if there are still more stats to fetch this cycle + work_to_do = True + # return value. True if new data differs from old + updated = False + # number of nodes we checked + num_nodes = 0 + + start_time = time.time() + while work_to_do: + + # each time through the loop try to talk to either the master + # or a node. stop when we have talked to everybody. + + # get a new node list from the master + if time.time() > (self.last_master_refresh + self.master_stale): + updated = self._master_refresh() + if self.last_master_refresh == 0: + # first time we contact the master, also refresh our full lookup tables + self._node_uri_refresh_all() + + self.last_master_refresh = time.time() + # contact the nodes for stats + else: + # initialize update_queue based on most current nodes list + if update_queue is None: + update_queue = list(self.nn_nodes) + # no nodes left to contact + elif not update_queue: + work_to_do = False + # contact next node + else: + # figure out the next node to contact + next = update_queue.pop() + # rate limit talking to any particular node + if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale): + updated = self._node_refresh(next) or updated + # include in random offset (max 1/5th normal update interval) + # to help spread out updates + last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0) + num_nodes += 1 + + # small yield to keep from torquing the processor + time.sleep(0.01) + end_time = time.time() + #print("Update took %ss for %s nodes"%((end_time-start_time), num_nodes)) + logger.debug("ROS stats update took %ss"%(end_time-start_time)) + return updated + diff --git a/src/hal/components/cros/master_python_source/rosgraph/masterapi.py b/src/hal/components/cros/master_python_source/rosgraph/masterapi.py new file mode 100644 index 00000000..7e391b9e --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosgraph/masterapi.py @@ -0,0 +1,481 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id: masterapi.py 9672 2010-05-11 21:57:40Z kwc $ +""" +Python adapter for calling ROS Master API. While it is trivial to call the +Master directly using XML-RPC, this API provides a safer abstraction in the event +the Master API is changed. +""" + +try: + from xmlrpc.client import ServerProxy # Python 3.x +except ImportError: + from xmlrpclib import ServerProxy # Python 2.x + +from . names import make_caller_id +from . rosenv import get_master_uri +from . network import parse_http_host_and_port + +class MasterException(Exception): + """ + Base class of ROS-master related errors. + """ + pass + +class MasterFailure(MasterException): + """ + Call to Master failed. This generally indicates an internal error + in the Master and that the Master may be in an inconsistent state. + """ + pass + +class MasterError(MasterException): + """ + Master returned an error code, which indicates an error in the + arguments passed to the Master. + """ + pass + +# backwards compat +ROSMasterException = MasterException +Error = MasterError +Failure = MasterFailure + +def is_online(master_uri=None): + """ + @param master_uri: (optional) override environment's ROS_MASTER_URI + @type master_uri: str + @return: True if Master is available + """ + return Master('rosgraph', master_uri=master_uri).is_online() + +class Master(object): + """ + API for interacting with the ROS master. Although the Master is + relatively simple to interact with using the XMLRPC API, this + abstraction layer provides protection against future updates. It + also provides a streamlined API with builtin return code checking + and caller_id passing. + """ + + def __init__(self, caller_id, master_uri=None): + """ + :param caller_id: name of node to use in calls to master, ``str`` + :param master_uri: (optional) override default ROS master URI, ``str`` + :raises: :exc:`ValueError` If ROS master uri not set properly + """ + + if master_uri is None: + master_uri = get_master_uri() + self._reinit(master_uri) + + self.caller_id = make_caller_id(caller_id) #resolve + if self.caller_id[-1] == '/': + self.caller_id = self.caller_id[:-1] + + def _reinit(self, master_uri): + """ + Internal API for reinitializing this handle to be a new master + + :raises: :exc:`ValueError` If ROS master uri not set + """ + if master_uri is None: + raise ValueError("ROS master URI is not set") + # #1730 validate URL for better error messages + try: + parse_http_host_and_port(master_uri) + except ValueError: + raise ValueError("invalid master URI: %s"%(master_uri)) + + self.master_uri = master_uri + self.handle = ServerProxy(self.master_uri) + + def is_online(self): + """ + Check if Master is online. + + NOTE: this is not part of the actual Master API. This is a convenience function. + + @param master_uri: (optional) override environment's ROS_MASTER_URI + @type master_uri: str + @return: True if Master is available + """ + try: + self.getPid() + return True + except: + return False + + def _succeed(self, args): + """ + Check master return code and return the value field. + + @param args: master return value + @type args: (int, str, XMLRPCLegalValue) + @return: value field of args (master return value) + @rtype: XMLRPCLegalValue + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + code, msg, val = args + if code == 1: + return val + elif code == -1: + raise Error(msg) + else: + raise Failure(msg) + + ################################################################################ + # PARAM SERVER + + def deleteParam(self, key): + """ + Parameter Server: delete parameter + @param key: parameter name + @type key: str + @return: 0 + @rtype: int + """ + return self._succeed(self.handle.deleteParam(self.caller_id, key)) + + def setParam(self, key, value): + """ + Parameter Server: set parameter. NOTE: if value is a + dictionary it will be treated as a parameter tree, where key + is the parameter namespace. For example::: + {'x':1,'y':2,'sub':{'z':3}} + + will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it + will replace all existing parameters in the key parameter + namespace with the parameters in value. You must set + parameters individually if you wish to perform a union update. + + @param key: parameter name + @type key: str + @param value: parameter value. + @type value: XMLRPCLegalValue + @return: 0 + @rtype: int + """ + return self._succeed(self.handle.setParam(self.caller_id, key, value)) + + def getParam(self, key): + """ + Retrieve parameter value from server. + @param key: parameter to lookup. If key is a namespace, + getParam() will return a parameter tree. + @type key: str + getParam() will return a parameter tree. + + @return: parameterValue. If key is a namespace, + the return value will be a dictionary, where each key is a + parameter in that namespace. Sub-namespaces are also + represented as dictionaries. + @rtype: XMLRPCLegalValue + """ + return self._succeed(self.handle.getParam(self.caller_id, key)) + + def searchParam(self, key): + """ + Search for parameter key on parameter server. Search starts in caller's namespace and proceeds + upwards through parent namespaces until Parameter Server finds a matching key. + + searchParam's behavior is to search for the first partial match. + For example, imagine that there are two 'robot_description' parameters:: + + /robot_description + /robot_description/arm + /robot_description/base + /pr2/robot_description + /pr2/robot_description/base + + If I start in the namespace /pr2/foo and search for + 'robot_description', searchParam will match + /pr2/robot_description. If I search for 'robot_description/arm' + it will return /pr2/robot_description/arm, even though that + parameter does not exist (yet). + + @param key: parameter key to search for. + @type key: str + @return: foundKey + @rtype: str + """ + return self._succeed(self.handle.searchParam(self.caller_id, key)) + + def subscribeParam(self, caller_api, key): + """ + Retrieve parameter value from server and subscribe to updates to that param. See + paramUpdate() in the Node API. + @param key: parameter to lookup. + @type key: str + @param caller_api: API URI for paramUpdate callbacks. + @type caller_api: str + @return: parameterValue. parameterValue is an empty dictionary if the parameter has not been set yet. + @rtype: XMLRPCLegalValue + """ + return self._succeed(self.handle.subscribeParam(self.caller_id, caller_api, key)) + + def unsubscribeParam(self, caller_api, key): + """ + Retrieve parameter value from server and subscribe to updates to that param. See + paramUpdate() in the Node API. + @param key: parameter to lookup. + @type key: str + @param caller_api: API URI for paramUpdate callbacks. + @type caller_api: str + @return: numUnsubscribed. If numUnsubscribed is zero it means that the caller was not subscribed to the parameter. + @rtype: int + """ + return self._succeed(self.handle.unsubscribeParam(self.caller_id, caller_api, key)) + + def hasParam(self, key): + """ + Check if parameter is stored on server. + @param key: parameter to check + @type key: str + @return: [code, statusMessage, hasParam] + @rtype: [int, str, bool] + """ + return self._succeed(self.handle.hasParam(self.caller_id, key)) + + def getParamNames(self): + """ + Get list of all parameter names stored on this server. + This does not adjust parameter names for caller's scope. + + @return: [code, statusMessage, parameterNameList] + @rtype: [int, str, [str]] + """ + return self._succeed(self.handle.getParamNames(self.caller_id)) + + + ################################################################################ + + def getPid(self): + """ + Get the PID of this server + @return: serverProcessPID + @rtype: int + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.getPid(self.caller_id)) + + def getUri(self): + """ + Get the URI of this Master + @return: masterUri + @rtype: str + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.getUri(self.caller_id)) + + def registerService(self, service, service_api, caller_api): + """ + Register the caller as a provider of the specified service. + @param service str: Fully-qualified name of service + @param service_api str: Service URI + @param caller_api str: XML-RPC URI of caller node + @return: ignore + @rtype: int + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.registerService(self.caller_id, service, service_api, caller_api)) + + def lookupService(self, service): + """ + Lookup all provider of a particular service. + @param service: fully-qualified name of service to lookup. + @type: service: str + @return (int, str, str): (code, message, serviceUrl). service URL is provides + and address and port of the service. Fails if there is no provider. + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.lookupService(self.caller_id, service)) + + + def unregisterService(self, service, service_api): + """ + Unregister the caller as a provider of the specified service. + @param service: Fully-qualified name of service + @type service: str + @param service_api: API URI of service to unregister. Unregistration will only occur if current + registration matches. + @type service_api: str + @return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1). + If this is zero it means that the caller was not registered as a service provider. + The call still succeeds as the intended final state is reached. + @rtype: (int, str, int) + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.unregisterService(self.caller_id, service, service_api)) + + + def registerSubscriber(self, topic, topic_type, caller_api): + """ + Subscribe the caller to the specified topic. In addition to receiving + a list of current publishers, the subscriber will also receive notifications + of new publishers via the publisherUpdate API. + @param topic str: Fully-qualified name of topic to subscribe to. + @param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name. + @type topic_type: str + @param caller_api: XML-RPC URI of caller node for new publisher notifications + @type caller_api: str + @return: (code, message, publishers). Publishers is a list of XMLRPC API URIs + for nodes currently publishing the specified topic. + @rtype: (int, str, list(str)) + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.registerSubscriber(self.caller_id, topic, topic_type, caller_api)) + + + def unregisterSubscriber(self, topic, caller_api): + """ + Unregister the caller as a publisher of the topic. + @param topic: Fully-qualified name of topic to unregister. + @type topic: str + @param caller_api: API URI of service to unregister. Unregistration will only occur if current + @type caller_api: str + registration matches. + @return: (code, statusMessage, numUnsubscribed). + If numUnsubscribed is zero it means that the caller was not registered as a subscriber. + The call still succeeds as the intended final state is reached. + @rtype: (int, str, int) + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.unregisterSubscriber(self.caller_id, topic, caller_api)) + + def registerPublisher(self, topic, topic_type, caller_api): + """ + Register the caller as a publisher the topic. + @param topic: Fully-qualified name of topic to register. + @type topic: str + @param topic_type: Datatype for topic. Must be a + package-resource name, i.e. the .msg name. + @type topic_type: str + @param caller_api str: ROS caller XML-RPC API URI + @type caller_api: str + @return: subscriberApis. + List of current subscribers of topic in the form of XMLRPC URIs. + @rtype: [str] + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.registerPublisher(self.caller_id, topic, topic_type, caller_api)) + + def unregisterPublisher(self, topic, caller_api): + """ + Unregister the caller as a publisher of the topic. + @param topic: Fully-qualified name of topic to unregister. + @type topic: str + @param caller_api str: API URI of service to + unregister. Unregistration will only occur if current + registration matches. + @type caller_api: str + @return: numUnregistered. + If numUnregistered is zero it means that the caller was not registered as a publisher. + The call still succeeds as the intended final state is reached. + @rtype: int + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.unregisterPublisher(self.caller_id, topic, caller_api)) + + def lookupNode(self, node_name): + """ + Get the XML-RPC URI of the node with the associated + name/caller_id. This API is for looking information about + publishers and subscribers. Use lookupService instead to lookup + ROS-RPC URIs. + @param node: name of node to lookup + @type node: str + @return: URI + @rtype: str + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.lookupNode(self.caller_id, node_name)) + + def getPublishedTopics(self, subgraph): + """ + Get list of topics that can be subscribed to. This does not return topics that have no publishers. + See L{getSystemState()} to get more comprehensive list. + @param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace + is resolved relative to the caller's namespace. Use '' to specify all names. + @type subgraph: str + @return: [[topic1, type1]...[topicN, typeN]] + @rtype: [[str, str],] + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.getPublishedTopics(self.caller_id, subgraph)) + + def getTopicTypes(self): + """ + Retrieve list topic names and their types. + + New in ROS 1.2. + + @rtype: (int, str, [[str,str]] ) + @return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs. + """ + return self._succeed(self.handle.getTopicTypes(self.caller_id)) + + def getSystemState(self): + """ + Retrieve list representation of system state (i.e. publishers, subscribers, and services). + @rtype: [[str,[str]], [str,[str]], [str,[str]]] + @return: systemState + + System state is in list representation:: + [publishers, subscribers, services]. + + publishers is of the form:: + [ [topic1, [topic1Publisher1...topic1PublisherN]] ... ] + + subscribers is of the form:: + [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] + + services is of the form:: + [ [service1, [service1Provider1...service1ProviderN]] ... ] + + @raise rosgraph.masterapi.Error: if Master returns ERROR. + @raise rosgraph.masterapi.Failure: if Master returns FAILURE. + """ + return self._succeed(self.handle.getSystemState(self.caller_id)) diff --git a/src/hal/components/cros/master_python_source/rosgraph/names.py b/src/hal/components/cros/master_python_source/rosgraph/names.py new file mode 100644 index 00000000..b5465eb1 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosgraph/names.py @@ -0,0 +1,329 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id: names.py 14589 2011-08-07 18:30:21Z kwc $ + +""" +Library for manipulating ROS Names. See U{http://ros.org/wiki/Names}. +""" + +import os +import sys + +from .rosenv import ROS_NAMESPACE + +#TODO: why are these here? +MSG_EXT = '.msg' +SRV_EXT = '.srv' + +SEP = '/' +GLOBALNS = '/' +PRIV_NAME = '~' +REMAP = ":=" +ANYTYPE = '*' + +if sys.hexversion > 0x03000000: #Python3 + def isstring(s): + return isinstance(s, str) #Python 3.x +else: + def isstring(s): + """ + Small helper version to check an object is a string in a way that works + for both Python 2 and 3 + """ + return isinstance(s, basestring) #Python 2.x + +def get_ros_namespace(env=None, argv=None): + """ + @param env: environment dictionary (defaults to os.environ) + @type env: dict + @param argv: command-line arguments (defaults to sys.argv) + @type argv: [str] + @return: ROS namespace of current program + @rtype: str + """ + #we force command-line-specified namespaces to be globally scoped + if argv is None: + argv = sys.argv + for a in argv: + if a.startswith('__ns:='): + return make_global_ns(a[len('__ns:='):]) + if env is None: + env = os.environ + return make_global_ns(env.get(ROS_NAMESPACE, GLOBALNS)) + +def make_caller_id(name): + """ + Resolve a local name to the caller ID based on ROS environment settings (i.e. ROS_NAMESPACE) + + @param name: local name to calculate caller ID from, e.g. 'camera', 'node' + @type name: str + @return: caller ID based on supplied local name + @rtype: str + """ + return make_global_ns(ns_join(get_ros_namespace(), name)) + +def make_global_ns(name): + """ + Convert name to a global name with a trailing namespace separator. + + @param name: ROS resource name. Cannot be a ~name. + @type name: str + @return str: name as a global name, e.g. 'foo' -> '/foo/'. + This does NOT resolve a name. + @rtype: str + @raise ValueError: if name is a ~name + """ + if is_private(name): + raise ValueError("cannot turn [%s] into a global name"%name) + if not is_global(name): + name = SEP + name + if name[-1] != SEP: + name = name + SEP + return name + +def is_global(name): + """ + Test if name is a global graph resource name. + + @param name: must be a legal name in canonical form + @type name: str + @return: True if name is a globally referenced name (i.e. /ns/name) + @rtype: bool + """ + return name and name[0] == SEP + +def is_private(name): + """ + Test if name is a private graph resource name. + + @param name: must be a legal name in canonical form + @type name: str + @return bool: True if name is a privately referenced name (i.e. ~name) + """ + return name and name[0] == PRIV_NAME + +def namespace(name): + """ + Get the namespace of name. The namespace is returned with a + trailing slash in order to favor easy concatenation and easier use + within the global context. + + @param name: name to return the namespace of. Must be a legal + name. NOTE: an empty name will return the global namespace. + @type name: str + @return str: Namespace of name. For example, '/wg/node1' returns '/wg/'. The + global namespace is '/'. + @rtype: str + @raise ValueError: if name is invalid + """ + "map name to its namespace" + if name is None: + raise ValueError('name') + if not isstring(name): + raise TypeError('name') + if not name: + return SEP + elif name[-1] == SEP: + name = name[:-1] + return name[:name.rfind(SEP)+1] or SEP + +def ns_join(ns, name): + """ + Join a namespace and name. If name is unjoinable (i.e. ~private or + /global) it will be returned without joining + + @param ns: namespace ('/' and '~' are both legal). If ns is the empty string, name will be returned. + @type ns: str + @param name str: a legal name + @return str: name concatenated to ns, or name if it is + unjoinable. + @rtype: str + """ + if is_private(name) or is_global(name): + return name + if ns == PRIV_NAME: + return PRIV_NAME + name + if not ns: + return name + if ns[-1] == SEP: + return ns + name + return ns + SEP + name + +def load_mappings(argv): + """ + Load name mappings encoded in command-line arguments. This will filter + out any parameter assignment mappings. + + @param argv: command-line arguments + @type argv: [str] + @return: name->name remappings. + @rtype: dict {str: str} + """ + mappings = {} + for arg in argv: + if REMAP in arg: + try: + src, dst = [x.strip() for x in arg.split(REMAP)] + if src and dst: + if len(src) > 1 and src[0] == '_' and src[1] != '_': + #ignore parameter assignment mappings + pass + else: + mappings[src] = dst + except: + #TODO: remove + sys.stderr.write("ERROR: Invalid remapping argument '%s'\n"%arg) + return mappings + + +################################################################################ +# NAME VALIDATORS + +import re + +#~,/, or ascii char followed by (alphanumeric, _, /) +NAME_LEGAL_CHARS_P = re.compile('^[\~\/A-Za-z][\w\/]*$') +def is_legal_name(name): + """ + Check if name is a legal ROS name for graph resources + (alphabetical character followed by alphanumeric, underscore, or + forward slashes). This constraint is currently not being enforced, + but may start getting enforced in later versions of ROS. + + @param name: Name + @type name: str + """ + # should we enforce unicode checks? + if name is None: + return False + # empty string is a legal name as it resolves to namespace + if name == '': + return True + m = NAME_LEGAL_CHARS_P.match(name) + return m is not None and m.group(0) == name and not '//' in name + +BASE_NAME_LEGAL_CHARS_P = re.compile('^[A-Za-z][\w]*$') #ascii char followed by (alphanumeric, _) +def is_legal_base_name(name): + """ + Validates that name is a legal base name for a graph resource. A base name has + no namespace context, e.g. "node_name". + """ + if name is None: + return False + m = BASE_NAME_LEGAL_CHARS_P.match(name) + return m is not None and m.group(0) == name + +def canonicalize_name(name): + """ + Put name in canonical form. Extra slashes '//' are removed and + name is returned without any trailing slash, e.g. /foo/bar + @param name: ROS name + @type name: str + """ + if not name or name == SEP: + return name + elif name[0] == SEP: + return '/' + '/'.join([x for x in name.split(SEP) if x]) + else: + return '/'.join([x for x in name.split(SEP) if x]) + +def resolve_name(name, namespace_, remappings=None): + """ + Resolve a ROS name to its global, canonical form. Private ~names + are resolved relative to the node name. + + @param name: name to resolve. + @type name: str + @param namespace_: node name to resolve relative to. + @type namespace_: str + @param remappings: Map of resolved remappings. Use None to indicate no remapping. + @return: Resolved name. If name is empty/None, resolve_name + returns parent namespace_. If namespace_ is empty/None, + @rtype: str + """ + if not name: #empty string resolves to parent of the namespace_ + return namespace(namespace_) + + name = canonicalize_name(name) + if name[0] == SEP: #global name + resolved_name = name + elif is_private(name): #~name + # #3044: be careful not to accidentally make rest of name global + resolved_name = canonicalize_name(namespace_ + SEP + name[1:]) + else: #relative + resolved_name = namespace(namespace_) + name + + #Mappings override general namespace-based resolution + # - do this before canonicalization as remappings are meant to + # match the name as specified in the code + if remappings and resolved_name in remappings: + return remappings[resolved_name] + else: + return resolved_name + +def script_resolve_name(script_name, name): + """ + Name resolver for scripts. Supports :envvar:`ROS_NAMESPACE`. Does not + support remapping arguments. + + :param name: name to resolve, ``str`` + :param script_name: name of script. script_name must not + contain a namespace., ``str`` + :returns: resolved name, ``str`` + """ + if not name: #empty string resolves to namespace + return get_ros_namespace() + #Check for global name: /foo/name resolves to /foo/name + if is_global(name): + return name + #Check for private name: ~name resolves to /caller_id/name + elif is_private(name): + return ns_join(make_caller_id(script_name), name[1:]) + return get_ros_namespace() + name + +def anonymous_name(id): + """ + Generate a ROS-legal 'anonymous' name + + @param id: prefix for anonymous name + @type id: str + """ + import socket, random + name = "%s_%s_%s_%s"%(id, socket.gethostname(), os.getpid(), random.randint(0, sys.maxsize)) + # RFC 952 allows hyphens, IP addrs can have '.'s, both + # of which are illegal for ROS names. For good + # measure, screen ipv6 ':'. + name = name.replace('.', '_') + name = name.replace('-', '_') + return name.replace(':', '_') + diff --git a/src/hal/components/cros/master_python_source/rosgraph/network.py b/src/hal/components/cros/master_python_source/rosgraph/network.py new file mode 100644 index 00000000..b6670048 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosgraph/network.py @@ -0,0 +1,419 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id: network.py 15125 2011-10-06 02:51:15Z kwc $ + +""" +Network APIs for ROS-based systems, including IP address and ROS +TCP header libraries. Because ROS-based runtimes must respect the +ROS_IP and ROS_HOSTNAME environment variables, ROS-specific APIs +are necessary for correctly retrieving local IP address +information. +""" + +import logging +import os +import socket +import struct +import sys +import platform + +try: + from cStringIO import StringIO #Python 2.x + python3 = 0 +except ImportError: + from io import BytesIO #Python 3.x + python3 = 1 + +try: + import urllib.parse as urlparse +except ImportError: + import urlparse + +from .rosenv import ROS_IP, ROS_HOSTNAME, ROS_IPV6 + +SIOCGIFCONF = 0x8912 +SIOCGIFADDR = 0x8915 +if platform.system() == 'FreeBSD': + SIOCGIFADDR = 0xc0206921 + if platform.architecture()[0] == '64bit': + SIOCGIFCONF = 0xc0106924 + else: + SIOCGIFCONF = 0xc0086924 + +logger = logging.getLogger('rosgraph.network') + +def parse_http_host_and_port(url): + """ + Convenience routine to handle parsing and validation of HTTP URL + port due to the fact that Python only provides easy accessors in + Python 2.5 and later. Validation checks that the protocol and host + are set. + + :param url: URL to parse, ``str`` + :returns: hostname and port number in URL or 80 (default), ``(str, int)`` + :raises: :exc:`ValueError` If the url does not validate + """ + if not url: + raise ValueError('not a valid URL') + p = urlparse.urlparse(url) + if not p.scheme or not p.hostname: + raise ValueError('not a valid URL') + port = p.port if p.port else 80 + return p.hostname, port + +def _is_unix_like_platform(): + """ + :returns: true if the platform conforms to UNIX/POSIX-style APIs + @rtype: bool + """ + return platform.system() in ['Linux', 'FreeBSD', 'Darwin'] + +def get_address_override(): + """ + :returns: ROS_IP/ROS_HOSTNAME override or None, ``str`` + :raises: :exc:`ValueError` If ROS_IP/ROS_HOSTNAME/__ip/__hostname are invalidly specified + """ + # #998: check for command-line remappings first + # TODO IPV6: check for compatibility + for arg in sys.argv: + if arg.startswith('__hostname:=') or arg.startswith('__ip:='): + try: + _, val = arg.split(':=') + return val + except: #split didn't unpack properly + raise ValueError("invalid ROS command-line remapping argument '%s'"%arg) + + # check ROS_HOSTNAME and ROS_IP environment variables, which are + # aliases for each other + if ROS_HOSTNAME in os.environ: + hostname = os.environ[ROS_HOSTNAME] + if hostname == '': + msg = 'invalid ROS_HOSTNAME (an empty string)' + sys.stderr.write(msg + '\n') + logger.warn(msg) + else: + parts = urlparse.urlparse(hostname) + if parts.scheme: + msg = 'invalid ROS_HOSTNAME (protocol ' + ('and port ' if parts.port else '') + 'should not be included)' + sys.stderr.write(msg + '\n') + logger.warn(msg) + elif hostname.find(':') != -1: + # this can not be checked with urlparse() + # since it does not extract the port for a hostname like "foo:1234" + msg = 'invalid ROS_HOSTNAME (port should not be included)' + sys.stderr.write(msg + '\n') + logger.warn(msg) + return hostname + elif ROS_IP in os.environ: + ip = os.environ[ROS_IP] + if ip == '': + msg = 'invalid ROS_IP (an empty string)' + sys.stderr.write(msg + '\n') + logger.warn(msg) + elif ip.find('://') != -1: + msg = 'invalid ROS_IP (protocol should not be included)' + sys.stderr.write(msg + '\n') + logger.warn(msg) + elif ip.find('.') != -1 and ip.rfind(':') > ip.rfind('.'): + msg = 'invalid ROS_IP (port should not be included)' + sys.stderr.write(msg + '\n') + logger.warn(msg) + elif ip.find('.') == -1 and ip.find(':') == -1: + msg = 'invalid ROS_IP (must be a valid IPv4 or IPv6 address)' + sys.stderr.write(msg + '\n') + logger.warn(msg) + return ip + return None + +def is_local_address(hostname): + """ + :param hostname: host name/address, ``str`` + :returns True: if hostname maps to a local address, False otherwise. False conditions include invalid hostnames. + """ + try: + if use_ipv6(): + reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, 0, 0, socket.SOL_TCP)] + else: + reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, socket.AF_INET, 0, socket.SOL_TCP)] + except socket.error: + return False + local_addresses = ['localhost'] + get_local_addresses() + # 127. check is due to #1260 + if ([ip for ip in reverse_ips if (ip.startswith('127.') or ip == '::1')] != []) or (set(reverse_ips) & set(local_addresses) != set()): + return True + return False + +def get_local_address(): + """ + :returns: default local IP address (e.g. eth0). May be overriden by ROS_IP/ROS_HOSTNAME/__ip/__hostname, ``str`` + """ + override = get_address_override() + if override: + return override + addrs = get_local_addresses() + if len(addrs) == 1: + return addrs[0] + for addr in addrs: + # pick first non 127/8 address + if not addr.startswith('127.') and not addr == '::1': + return addr + else: # loopback + if use_ipv6(): + return '::1' + else: + return '127.0.0.1' + +# cache for performance reasons +_local_addrs = None +def get_local_addresses(): + """ + :returns: known local addresses. Not affected by ROS_IP/ROS_HOSTNAME, ``[str]`` + """ + # cache address data as it can be slow to calculate + global _local_addrs + if _local_addrs is not None: + return _local_addrs + + local_addrs = None + if _is_unix_like_platform(): + # unix-only branch + v4addrs = [] + v6addrs = [] + import netifaces + for iface in netifaces.interfaces(): + try: + ifaddrs = netifaces.ifaddresses(iface) + except ValueError: + # even if interfaces() returns an interface name + # ifaddresses() might raise a ValueError + # https://bugs.launchpad.net/ubuntu/+source/netifaces/+bug/753009 + continue + if socket.AF_INET in ifaddrs: + v4addrs.extend([addr['addr'] for addr in ifaddrs[socket.AF_INET]]) + if socket.AF_INET6 in ifaddrs: + v6addrs.extend([addr['addr'] for addr in ifaddrs[socket.AF_INET6]]) + if use_ipv6(): + local_addrs = v6addrs + v4addrs + else: + local_addrs = v4addrs + else: + # cross-platform branch, can only resolve one address + if use_ipv6(): + local_addrs = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, 0, 0, socket.SOL_TCP)] + else: + local_addrs = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, socket.AF_INET, 0, socket.SOL_TCP)] + _local_addrs = local_addrs + return local_addrs + +def use_ipv6(): + return ROS_IPV6 in os.environ and os.environ[ROS_IPV6] == 'on' + +def get_bind_address(address=None): + """ + :param address: (optional) address to compare against, ``str`` + :returns: address TCP/IP sockets should use for binding. This is + generally 0.0.0.0, but if \a address or ROS_IP/ROS_HOSTNAME is set + to localhost it will return 127.0.0.1, ``str`` + """ + if address is None: + address = get_address_override() + if address and (address == 'localhost' or address.startswith('127.') or address == '::1' ): + #localhost or 127/8 + if use_ipv6(): + return '::1' + elif address.startswith('127.'): + return address + else: + return '127.0.0.1' #loopback + else: + if use_ipv6(): + return '::' + else: + return '0.0.0.0' + +# #528: semi-complicated logic for determining XML-RPC URI +def get_host_name(): + """ + Determine host-name for use in host-name-based addressing (e.g. XML-RPC URIs): + - if ROS_IP/ROS_HOSTNAME is set, use that address + - if the hostname returns a non-localhost value, use that + - use whatever L{get_local_address()} returns + """ + hostname = get_address_override() + if not hostname: + try: + hostname = socket.gethostname() + except: + pass + if not hostname or hostname == 'localhost' or hostname.startswith('127.'): + hostname = get_local_address() + return hostname + +def create_local_xmlrpc_uri(port): + """ + Determine the XMLRPC URI for local servers. This handles the search + logic of checking ROS environment variables, the known hostname, + and local interface IP addresses to determine the best possible + URI. + + :param port: port that server is running on, ``int`` + :returns: XMLRPC URI, ``str`` + """ + #TODO: merge logic in rosgraph.xmlrpc with this routine + # in the future we may not want to be locked to http protocol nor root path + return 'http://%s:%s/'%(get_host_name(), port) + + +## handshake utils ########################################### + +class ROSHandshakeException(Exception): + """ + Exception to represent errors decoding handshake + """ + pass + +def decode_ros_handshake_header(header_str): + """ + Decode serialized ROS handshake header into a Python dictionary + + header is a list of string key=value pairs, each prefixed by a + 4-byte length field. It is preceeded by a 4-byte length field for + the entire header. + + :param header_str: encoded header string. May contain extra data at the end, ``str`` + :returns: key value pairs encoded in \a header_str, ``{str: str}`` + """ + (size, ) = struct.unpack(' header_len: + raise ROSHandshakeException("Incomplete header. Expected %s bytes but only have %s"%((size+4), header_len)) + + d = {} + start = 4 + while start < size: + (field_size, ) = struct.unpack(' size: + raise ROSHandshakeException("Invalid line length in handshake header: %s"%size) + line = header_str[start-field_size:start] + + #python3 compatibility + if python3 == 1: + line = line.decode() + + idx = line.find("=") + if idx < 0: + raise ROSHandshakeException("Invalid line in handshake header: [%s]"%line) + key = line[:idx] + value = line[idx+1:] + d[key.strip()] = value + return d + +def read_ros_handshake_header(sock, b, buff_size): + """ + Read in tcpros header off the socket \a sock using buffer \a b. + + :param sock: socket must be in blocking mode, ``socket`` + :param b: buffer to use, ``StringIO`` for Python2, ``BytesIO`` for Python 3 + :param buff_size: incoming buffer size to use, ``int`` + :returns: key value pairs encoded in handshake, ``{str: str}`` + :raises: :exc:`ROSHandshakeException` If header format does not match expected + """ + header_str = None + while not header_str: + d = sock.recv(buff_size) + if not d: + raise ROSHandshakeException("connection from sender terminated before handshake header received. %s bytes were received. Please check sender for additional details."%b.tell()) + b.write(d) + btell = b.tell() + if btell > 4: + # most likely we will get the full header in the first recv, so + # not worth tiny optimizations possible here + bval = b.getvalue() + (size,) = struct.unpack('= size: + header_str = bval + + # memmove the remnants of the buffer back to the start + leftovers = bval[size+4:] + b.truncate(len(leftovers)) + b.seek(0) + b.write(leftovers) + header_recvd = True + + # process the header + return decode_ros_handshake_header(bval) + +def encode_ros_handshake_header(header): + """ + Encode ROS handshake header as a byte string. Each header + field is a string key value pair. The encoded header is + prefixed by a length field, as is each field key/value pair. + key/value pairs a separated by a '=' equals sign. + + FORMAT: (4-byte length + [4-byte field length + field=value ]*) + + :param header: header field keys/values, ``dict`` + :returns: header encoded as byte string, ``bytes`` + """ + str_cls = str if python3 else unicode + + # encode all unicode keys in the header. Ideally, the type of these would be specified by the api + encoded_header = {} + for k, v in header.items(): + if isinstance(k, str_cls): + k = k.encode('utf-8') + if isinstance(v, str_cls): + v = v.encode('utf-8') + encoded_header[k] = v + + fields = [k + b"=" + v for k, v in sorted(encoded_header.items())] + s = b''.join([struct.pack(' (3, 2): + # Dummy last argument to match Python3 return type + return co.co_filename, f.f_lineno, func_name, None + else: + return co.co_filename, f.f_lineno, func_name + +logging.setLoggerClass(RospyLogger) + +def renew_latest_logdir(logfile_dir): + log_dir = os.path.dirname(logfile_dir) + latest_dir = os.path.join(log_dir, 'latest') + if os.path.lexists(latest_dir): + if not os.path.islink(latest_dir): + return False + os.remove(latest_dir) + os.symlink(logfile_dir, latest_dir) + return True + +def configure_logging(logname, level=logging.INFO, filename=None, env=None): + """ + Configure Python logging package to send log files to ROS-specific log directory + :param logname str: name of logger, ``str`` + :param filename: filename to log to. If not set, a log filename + will be generated using logname, ``str`` + :param env: override os.environ dictionary, ``dict`` + :returns: log file name, ``str`` + :raises: :exc:`LoggingException` If logging cannot be configured as specified + """ + if env is None: + env = os.environ + + logname = logname or 'unknown' + log_dir = rospkg.get_log_dir(env=env) + + # if filename is not explicitly provided, generate one using logname + if not filename: + log_filename = os.path.join(log_dir, '%s-%s.log'%(logname, os.getpid())) + else: + log_filename = os.path.join(log_dir, filename) + + logfile_dir = os.path.dirname(log_filename) + if not os.path.exists(logfile_dir): + try: + makedirs_with_parent_perms(logfile_dir) + except OSError: + # cannot print to screen because command-line tools with output use this + if os.path.exists(logfile_dir): + # We successfully created the logging folder, but could not change + # permissions of the new folder to the same as the parent folder + sys.stderr.write("WARNING: Could not change permissions for folder [%s], make sure that the parent folder has correct permissions.\n"%logfile_dir) + else: + # Could not create folder + sys.stderr.write("WARNING: cannot create log directory [%s]. Please set %s to a writable location.\n"%(logfile_dir, ROS_LOG_DIR)) + return None + elif os.path.isfile(logfile_dir): + raise LoggingException("Cannot save log files: file [%s] is in the way"%logfile_dir) + + # the log dir itself should not be symlinked as latest + if logfile_dir != log_dir: + if sys.platform not in ['win32']: + try: + success = renew_latest_logdir(logfile_dir) + if not success: + sys.stderr.write("INFO: cannot create a symlink to latest log directory\n") + except OSError as e: + sys.stderr.write("INFO: cannot create a symlink to latest log directory: %s\n" % e) + + config_file = os.environ.get('ROS_PYTHON_LOG_CONFIG_FILE') + if not config_file: + # search for logging config file in ROS_HOME, ROS_ETC_DIR or relative + # to the rosgraph package directory. + rosgraph_d = rospkg.RosPack().get_path('rosgraph') + for config_dir in [os.path.join(rospkg.get_ros_home(), 'config'), + rospkg.get_etc_ros_dir(), + os.path.join(rosgraph_d, 'conf')]: + for extension in ('conf', 'yaml'): + f = os.path.join(config_dir, + 'python_logging{}{}'.format(os.path.extsep, + extension)) + if os.path.isfile(f): + config_file = f + break + if config_file is not None: + break + + if config_file is None or not os.path.isfile(config_file): + # logging is considered soft-fail + sys.stderr.write("WARNING: cannot load logging configuration file, logging is disabled\n") + logging.getLogger(logname).setLevel(logging.CRITICAL) + return log_filename + + # pass in log_filename as argument to pylogging.conf + os.environ['ROS_LOG_FILENAME'] = log_filename + if config_file.endswith(('.yaml', '.yml')): + with open(config_file) as f: + dict_conf = yaml.safe_load(f) + dict_conf.setdefault('version', 1) + logging.config.dictConfig(dict_conf) + else: + # #3625: disabling_existing_loggers=False + logging.config.fileConfig(config_file, disable_existing_loggers=False) + + return log_filename + + +def makedirs_with_parent_perms(p): + """ + Create the directory using the permissions of the nearest + (existing) parent directory. This is useful for logging, where a + root process sometimes has to log in the user's space. + :param p: directory to create, ``str`` + """ + p = os.path.abspath(p) + parent = os.path.dirname(p) + # recurse upwards, checking to make sure we haven't reached the + # top + if not os.path.exists(p) and p and parent != p: + makedirs_with_parent_perms(parent) + s = os.stat(parent) + os.mkdir(p) + + # if perms of new dir don't match, set anew + s2 = os.stat(p) + if s.st_uid != s2.st_uid or s.st_gid != s2.st_gid: + os.chown(p, s.st_uid, s.st_gid) + if s.st_mode != s2.st_mode: + os.chmod(p, s.st_mode) + +_logging_to_rospy_names = { + 'DEBUG': ('DEBUG', '\033[32m'), + 'INFO': ('INFO', None), + 'WARNING': ('WARN', '\033[33m'), + 'ERROR': ('ERROR', '\033[31m'), + 'CRITICAL': ('FATAL', '\033[31m') +} +_color_reset = '\033[0m' +_defaultFormatter = logging.Formatter() + +class RosStreamHandler(logging.Handler): + def __init__(self, colorize=True, stdout=None, stderr=None): + super(RosStreamHandler, self).__init__() + self._stdout = stdout or sys.stdout + self._stderr = stderr or sys.stderr + self._colorize = colorize + try: + from rospy.rostime import get_time, is_wallclock + self._get_time = get_time + self._is_wallclock = is_wallclock + except ImportError: + self._get_time = None + self._is_wallclock = None + + def emit(self, record): + level, color = _logging_to_rospy_names[record.levelname] + record_message = _defaultFormatter.format(record) + msg = os.environ.get( + 'ROSCONSOLE_FORMAT', '[${severity}] [${time}]: ${message}') + msg = msg.replace('${severity}', level) + msg = msg.replace('${message}', str(record_message)) + msg = msg.replace('${walltime}', '%f' % time.time()) + msg = msg.replace('${thread}', str(record.thread)) + msg = msg.replace('${logger}', str(record.name)) + msg = msg.replace('${file}', str(record.pathname)) + msg = msg.replace('${line}', str(record.lineno)) + msg = msg.replace('${function}', str(record.funcName)) + try: + from rospy import get_name + node_name = get_name() + except ImportError: + node_name = '' + msg = msg.replace('${node}', node_name) + time_str = '%f' % time.time() + if self._get_time is not None and not self._is_wallclock(): + time_str += ', %f' % self._get_time() + msg = msg.replace('${time}', time_str) + msg += '\n' + if record.levelno < logging.WARNING: + self._write(self._stdout, msg, color) + else: + self._write(self._stderr, msg, color) + + def _write(self, fd, msg, color): + if self._colorize and color and hasattr(fd, 'isatty') and fd.isatty(): + msg = color + msg + _color_reset + fd.write(msg) diff --git a/src/hal/components/cros/master_python_source/rosgraph/xmlrpc.py b/src/hal/components/cros/master_python_source/rosgraph/xmlrpc.py new file mode 100644 index 00000000..c8ff8022 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosgraph/xmlrpc.py @@ -0,0 +1,304 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id: xmlrpc.py 15336 2011-11-07 20:43:00Z kwc $ + +from __future__ import print_function + +""" +Common XML-RPC for higher-level libraries running XML-RPC libraries in +ROS. In particular, this library provides common handling for URI +calculation based on ROS environment variables. + +The common entry point for most libraries is the L{XmlRpcNode} class. +""" + +import errno +import logging +import select +import socket + +try: + import _thread +except ImportError: + import thread as _thread + +import traceback + +try: + from xmlrpc.server import SimpleXMLRPCServer, SimpleXMLRPCRequestHandler #Python 3.x +except ImportError: + from SimpleXMLRPCServer import SimpleXMLRPCServer #Python 2.x + from SimpleXMLRPCServer import SimpleXMLRPCRequestHandler #Python 2.x + +try: + import socketserver +except ImportError: + import SocketServer as socketserver + +import rosgraph.network + +def isstring(s): + """Small helper version to check an object is a string in a way that works + for both Python 2 and 3 + """ + try: + return isinstance(s, basestring) + except NameError: + return isinstance(s, str) + +class SilenceableXMLRPCRequestHandler(SimpleXMLRPCRequestHandler): + + protocol_version = 'HTTP/1.1' + + def log_message(self, format, *args): + if 0: + SimpleXMLRPCRequestHandler.log_message(self, format, *args) + +class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, SimpleXMLRPCServer): + """ + Adds ThreadingMixin to SimpleXMLRPCServer to support multiple concurrent + requests via threading. Also makes logging toggleable. + """ + + daemon_threads = True + + def __init__(self, addr, log_requests=1): + """ + Overrides SimpleXMLRPCServer to set option to allow_reuse_address. + """ + # allow_reuse_address defaults to False in Python 2.4. We set it + # to True to allow quick restart on the same port. This is equivalent + # to calling setsockopt(SOL_SOCKET,SO_REUSEADDR,1) + self.allow_reuse_address = True + # Increase request_queue_size to handle issues with many simultaneous + # connections in OSX 10.11 + self.request_queue_size = min(socket.SOMAXCONN, 128) + if rosgraph.network.use_ipv6(): + logger = logging.getLogger('xmlrpc') + # The XMLRPC library does not support IPv6 out of the box + # We have to monipulate private members and duplicate + # code from the constructor. + # TODO IPV6: Get this into SimpleXMLRPCServer + SimpleXMLRPCServer.__init__(self, addr, SilenceableXMLRPCRequestHandler, log_requests, bind_and_activate=False) + self.address_family = socket.AF_INET6 + self.socket = socket.socket(self.address_family, self.socket_type) + logger.info('binding ipv6 xmlrpc socket to' + str(addr)) + # TODO: set IPV6_V6ONLY to 0: + # self.socket.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_V6ONLY, 0) + self.server_bind() + self.server_activate() + logger.info('bound to ' + str(self.socket.getsockname()[0:2])) + else: + SimpleXMLRPCServer.__init__(self, addr, SilenceableXMLRPCRequestHandler, log_requests) + + def handle_error(self, request, client_address): + """ + override ThreadingMixin, which sends errors to stderr + """ + if logging and traceback: + logger = logging.getLogger('xmlrpc') + if logger: + logger.error(traceback.format_exc()) + +#class ForkingXMLRPCServer(socketserver.ForkingMixIn, SimpleXMLRPCServer): +# """ +# Adds ThreadingMixin to SimpleXMLRPCServer to support multiple concurrent +# requests via forking. Also makes logging toggleable. +# """ +# def __init__(self, addr, request_handler=SilenceableXMLRPCRequestHandler, log_requests=1): +# SimpleXMLRPCServer.__init__(self, addr, request_handler, log_requests) + + +class XmlRpcHandler(object): + """ + Base handler API for handlers used with XmlRpcNode. Public methods will be + exported as XML RPC methods. + """ + + def _ready(self, uri): + """ + callback into handler to inform it of XML-RPC URI + """ + pass + + def _shutdown(self, reason): + """ + callback into handler to inform it of shutdown + """ + pass + +class XmlRpcNode(object): + """ + Generic XML-RPC node. Handles the additional complexity of binding + an XML-RPC server to an arbitrary port. + XmlRpcNode is initialized when the uri field has a value. + """ + + def __init__(self, port=0, rpc_handler=None, on_run_error=None): + """ + XML RPC Node constructor + :param port: port to use for starting XML-RPC API. Set to 0 or omit to bind to any available port, ``int`` + :param rpc_handler: XML-RPC API handler for node, `XmlRpcHandler` + :param on_run_error: function to invoke if server.run() throws + Exception. Server always terminates if run() throws, but this + enables cleanup routines to be invoked if server goes down, as + well as include additional debugging. ``fn(Exception)`` + """ + super(XmlRpcNode, self).__init__() + + self.handler = rpc_handler + self.uri = None # initialize the property now so it can be tested against, will be filled in later + self.server = None + if port and isstring(port): + port = int(port) + self.port = port + self.is_shutdown = False + self.on_run_error = on_run_error + + def shutdown(self, reason): + """ + Terminate i/o connections for this server. + + :param reason: human-readable debug string, ``str`` + """ + self.is_shutdown = True + if self.server: + server = self.server + handler = self.handler + self.handler = self.server = self.port = self.uri = None + if handler: + handler._shutdown(reason) + if server: + server.socket.close() + server.server_close() + + def start(self): + """ + Initiate a thread to run the XML RPC server. Uses thread.start_new_thread. + """ + _thread.start_new_thread(self.run, ()) + + def set_uri(self, uri): + """ + Sets the XML-RPC URI. Defined as a separate method as a hood + for subclasses to bootstrap initialization. Should not be called externally. + + :param uri: XMLRPC URI, ``str`` + """ + self.uri = uri + + def run(self): + try: + self._run() + except Exception as e: + if self.is_shutdown: + pass + elif self.on_run_error is not None: + self.on_run_error(e) + else: + raise + + # separated out for easier testing + def _run_init(self): + logger = logging.getLogger('xmlrpc') + try: + log_requests = 0 + port = self.port or 0 #0 = any + + bind_address = rosgraph.network.get_bind_address() + logger.info("XML-RPC server binding to %s:%d" % (bind_address, port)) + + self.server = ThreadingXMLRPCServer((bind_address, port), log_requests) + self.port = self.server.server_address[1] #set the port to whatever server bound to + if not self.port: + self.port = self.server.socket.getsockname()[1] #Python 2.4 + + assert self.port, "Unable to retrieve local address binding" + + # #528: semi-complicated logic for determining XML-RPC URI + # - if ROS_IP/ROS_HOSTNAME is set, use that address + # - if the hostname returns a non-localhost value, use that + # - use whatever rosgraph.network.get_local_address() returns + uri = None + override = rosgraph.network.get_address_override() + if override: + uri = 'http://%s:%s/'%(override, self.port) + else: + try: + hostname = socket.gethostname() + if hostname and not hostname == 'localhost' and not hostname.startswith('127.') and hostname != '::': + uri = 'http://%s:%s/'%(hostname, self.port) + except: + pass + if not uri: + uri = 'http://%s:%s/'%(rosgraph.network.get_local_address(), self.port) + self.set_uri(uri) + + logger.info("Started XML-RPC server [%s]", self.uri) + + self.server.register_multicall_functions() + self.server.register_instance(self.handler) + + except socket.error as e: + if e.errno == errno.EADDRINUSE: + msg = "ERROR: Unable to start XML-RPC server, port %s is already in use"%self.port + else: + msg = "ERROR: Unable to start XML-RPC server: %s" % e.strerror + logger.error(msg) + print(msg) + raise #let higher level catch this + + if self.handler is not None: + self.handler._ready(self.uri) + logger.info("xml rpc node: starting XML-RPC server") + + def _run(self): + """ + Main processing thread body. + :raises: :exc:`socket.error` If server cannot bind + + """ + self._run_init() + while not self.is_shutdown: + try: + self.server.serve_forever() + except (IOError, select.error) as e: + # check for interrupted call, which can occur if we're + # embedded in a program using signals. All other + # exceptions break _run. + if self.is_shutdown: + pass + elif e.errno != errno.EINTR: + self.is_shutdown = True + logging.getLogger('xmlrpc').error("serve forever IOError: %s, %s"%(e.errno, e.strerror)) + diff --git a/src/hal/components/cros/master_python_source/rosmaster/__init__.py b/src/hal/components/cros/master_python_source/rosmaster/__init__.py new file mode 100644 index 00000000..a56ceb75 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/__init__.py @@ -0,0 +1,38 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2010, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +from .main import rosmaster_main + +from .master import DEFAULT_MASTER_PORT + diff --git a/src/hal/components/cros/master_python_source/rosmaster/exceptions.py b/src/hal/components/cros/master_python_source/rosmaster/exceptions.py new file mode 100644 index 00000000..0120de16 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/exceptions.py @@ -0,0 +1,39 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2010, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +""" +Exceptions for rosmaster package. +""" + +class InternalException(Exception): pass diff --git a/src/hal/components/cros/master_python_source/rosmaster/main.py b/src/hal/components/cros/master_python_source/rosmaster/main.py new file mode 100644 index 00000000..ae94dd18 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/main.py @@ -0,0 +1,139 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +"""Command-line handler for ROS zenmaster (Python Master)""" + +import logging +import os +import sys +import time +import optparse + +import rosmaster.master +from rosmaster.master_api import NUM_WORKERS + +def configure_logging(): + """ + Setup filesystem logging for the master + """ + filename = 'master.log' + # #988 __log command-line remapping argument + import rosgraph.names + import rosgraph.roslogging + mappings = rosgraph.names.load_mappings(sys.argv) + if '__log' in mappings: + logfilename_remap = mappings['__log'] + filename = os.path.abspath(logfilename_remap) + _log_filename = rosgraph.roslogging.configure_logging('rosmaster', logging.DEBUG, filename=filename) + +def rosmaster_main(argv=sys.argv, stdout=sys.stdout, env=os.environ): + parser = optparse.OptionParser(usage="usage: zenmaster [options]") + parser.add_option("--core", + dest="core", action="store_true", default=False, + help="run as core") + parser.add_option("-p", "--port", + dest="port", default=0, + help="override port", metavar="PORT") + parser.add_option("-w", "--numworkers", + dest="num_workers", default=NUM_WORKERS, type=int, + help="override number of worker threads", metavar="NUM_WORKERS") + parser.add_option("-t", "--timeout", + dest="timeout", + help="override the socket connection timeout (in seconds).", metavar="TIMEOUT") + parser.add_option("--master-logger-level", + dest="master_logger_level", default=False, type=str, + help="set rosmaster.master logger level ('debug', 'info', 'warn', 'error', 'fatal')") + + options, args = parser.parse_args(argv[1:]) + + # only arg that zenmaster supports is __log remapping of logfilename + for arg in args: + if not arg.startswith('__log:='): + parser.error("unrecognized arg: %s"%arg) + configure_logging() + + port = rosmaster.master.DEFAULT_MASTER_PORT + if options.port: + port = int(options.port) + + if not options.core: + print(""" + + +ACHTUNG WARNING ACHTUNG WARNING ACHTUNG +WARNING ACHTUNG WARNING ACHTUNG WARNING + + +Standalone zenmaster has been deprecated, please use 'roscore' instead + + +ACHTUNG WARNING ACHTUNG WARNING ACHTUNG +WARNING ACHTUNG WARNING ACHTUNG WARNING + + +""") + + logger = logging.getLogger("rosmaster.main") + logger.info("initialization complete, waiting for shutdown") + + if options.timeout is not None and float(options.timeout) >= 0.0: + logger.info("Setting socket timeout to %s" % options.timeout) + import socket + socket.setdefaulttimeout(float(options.timeout)) + + if options.master_logger_level: + levels = {'debug': logging.DEBUG, 'info': logging.INFO, 'warn': logging.WARN, 'error': logging.ERROR, 'fatal': logging.FATAL} + if options.master_logger_level.lower() in levels.keys(): + logger.info("set rosmaster.master logger level '{}'".format(options.master_logger_level)) + rosmaster.master_api.LOG_API = True + logging.getLogger("rosmaster.master").setLevel(levels[options.master_logger_level.lower()]) + else: + logger.error("--master-logger-level received unknown option '{}'".format(options.master_logger_level)) + + try: + logger.info("Starting ROS Master Node") + master = rosmaster.master.Master(port, options.num_workers) + master.start() + + import time + while master.ok(): + time.sleep(.1) + except KeyboardInterrupt: + logger.info("keyboard interrupt, will exit") + finally: + logger.info("stopping master...") + master.stop() + +if __name__ == "__main__": + main() diff --git a/src/hal/components/cros/master_python_source/rosmaster/master.py b/src/hal/components/cros/master_python_source/rosmaster/master.py new file mode 100644 index 00000000..fc8652ac --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/master.py @@ -0,0 +1,89 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +""" +ROS Master. + +This module integrates the lower-level implementation modules into a +single interface for running and stopping the ROS Master. +""" + +import logging +import time + +import rosgraph.xmlrpc + +import rosmaster.master_api + +DEFAULT_MASTER_PORT=11311 #default port for master's to bind to + +class Master(object): + + def __init__(self, port=DEFAULT_MASTER_PORT, num_workers=rosmaster.master_api.NUM_WORKERS): + self.port = port + self.num_workers = num_workers + + def start(self): + """ + Start the ROS Master. + """ + self.handler = None + self.master_node = None + self.uri = None + + handler = rosmaster.master_api.ROSMasterHandler(self.num_workers) + master_node = rosgraph.xmlrpc.XmlRpcNode(self.port, handler) + master_node.start() + + # poll for initialization + while not master_node.uri: + time.sleep(0.0001) + + # save fields + self.handler = handler + self.master_node = master_node + self.uri = master_node.uri + + logging.getLogger('rosmaster.master').info("Master initialized: port[%s], uri[%s]", self.port, self.uri) + + def ok(self): + if self.master_node is not None: + return self.master_node.handler._ok() + else: + return False + + def stop(self): + if self.master_node is not None: + self.master_node.shutdown('Master.stop') + self.master_node = None diff --git a/src/hal/components/cros/master_python_source/rosmaster/master_api.py b/src/hal/components/cros/master_python_source/rosmaster/master_api.py new file mode 100644 index 00000000..c5312618 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/master_api.py @@ -0,0 +1,889 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ +""" +ROS Master API. + +L{ROSMasterHandler} provides the API implementation of the +Master. Python allows an API to be introspected from a Python class, +so the handler has a 1-to-1 mapping with the actual XMLRPC API. + +API return convention: (statusCode, statusMessage, returnValue) + + - statusCode: an integer indicating the completion condition of the method. + - statusMessage: a human-readable string message for debugging + - returnValue: the return value of the method; method-specific. + +Current status codes: + + - -1: ERROR: Error on the part of the caller, e.g. an invalid parameter + - 0: FAILURE: Method was attempted but failed to complete correctly. + - 1: SUCCESS: Method completed successfully. + +Individual methods may assign additional meaning/semantics to statusCode. +""" + +from __future__ import print_function + +import os +import sys +import logging +import threading +import time +import traceback + +from rosgraph.xmlrpc import XmlRpcHandler + +import rosgraph.names +from rosgraph.names import resolve_name +import rosmaster.paramserver +import rosmaster.threadpool + +from rosmaster.util import xmlrpcapi +from rosmaster.registrations import RegistrationManager +from rosmaster.validators import non_empty, non_empty_str, not_none, is_api, is_topic, is_service, valid_type_name, valid_name, empty_or_valid_name, ParameterInvalid + +NUM_WORKERS = 3 #number of threads we use to send publisher_update notifications + +# Return code slots +STATUS = 0 +MSG = 1 +VAL = 2 + +_logger = logging.getLogger("rosmaster.master") + +LOG_API = False + +def mloginfo(msg, *args): + """ + Info-level master log statements. These statements may be printed + to screen so they should be user-readable. + @param msg: Message string + @type msg: str + @param args: arguments for msg if msg is a format string + """ + #mloginfo is in core so that it is accessible to master and masterdata + _logger.info(msg, *args) + +def mlogwarn(msg, *args): + """ + Warn-level master log statements. These statements may be printed + to screen so they should be user-readable. + @param msg: Message string + @type msg: str + @param args: arguments for msg if msg is a format string + """ + #mloginfo is in core so that it is accessible to master and masterdata + _logger.warn(msg, *args) + if args: + print("WARN: " + msg % args) + else: + print("WARN: " + str(msg)) + + +def apivalidate(error_return_value, validators=()): + """ + ROS master/slave arg-checking decorator. Applies the specified + validator to the corresponding argument and also remaps each + argument to be the value returned by the validator. Thus, + arguments can be simultaneously validated and canonicalized prior + to actual function call. + @param error_return_value: API value to return if call unexpectedly fails + @param validators: sequence of validators to apply to each + arg. None means no validation for the parameter is required. As all + api methods take caller_id as the first parameter, the validators + start with the second param. + @type validators: sequence + """ + def check_validates(f): + try: + func_code = f.__code__ + func_name = f.__name__ + except AttributeError: + func_code = f.func_code + func_name = f.func_name + assert len(validators) == func_code.co_argcount - 2, "%s failed arg check"%f #ignore self and caller_id + def validated_f(*args, **kwds): + if LOG_API: + _logger.debug("%s%s", func_name, str(args[1:])) + #print "%s%s"%(func_name, str(args[1:])) + if len(args) == 1: + _logger.error("%s invoked without caller_id paramter" % func_name) + return -1, "missing required caller_id parameter", error_return_value + elif len(args) != func_code.co_argcount: + return -1, "Error: bad call arity", error_return_value + + instance = args[0] + caller_id = args[1] + def isstring(s): + """Small helper version to check an object is a string in + a way that works for both Python 2 and 3 + """ + try: + return isinstance(s, basestring) + except NameError: + return isinstance(s, str) + if not isstring(caller_id): + _logger.error("%s: invalid caller_id param type", func_name) + return -1, "caller_id must be a string", error_return_value + + newArgs = [instance, caller_id] #canonicalized args + try: + for (v, a) in zip(validators, args[2:]): + if v: + try: + newArgs.append(v(a, caller_id)) + except ParameterInvalid as e: + _logger.error("%s: invalid parameter: %s", func_name, str(e) or 'error') + return -1, str(e) or 'error', error_return_value + else: + newArgs.append(a) + + if LOG_API: + retval = f(*newArgs, **kwds) + _logger.debug("%s%s returns %s", func_name, args[1:], retval) + return retval + else: + code, msg, val = f(*newArgs, **kwds) + if val is None: + return -1, "Internal error (None value returned)", error_return_value + return code, msg, val + except TypeError as te: #most likely wrong arg number + _logger.error(traceback.format_exc()) + return -1, "Error: invalid arguments: %s"%te, error_return_value + except Exception as e: #internal failure + _logger.error(traceback.format_exc()) + return 0, "Internal failure: %s"%e, error_return_value + try: + validated_f.__name__ = func_name + except AttributeError: + validated_f.func_name = func_name + validated_f.__doc__ = f.__doc__ #preserve doc + return validated_f + return check_validates + +def publisher_update_task(api, topic, pub_uris): + """ + Contact api.publisherUpdate with specified parameters + @param api: XML-RPC URI of node to contact + @type api: str + @param topic: Topic name to send to node + @type topic: str + @param pub_uris: list of publisher APIs to send to node + @type pub_uris: [str] + """ + msg = "publisherUpdate[%s] -> %s %s" % (topic, api, pub_uris) + mloginfo(msg) + start_sec = time.time() + try: + #TODO: check return value for errors so we can unsubscribe if stale + ret = xmlrpcapi(api).publisherUpdate('/master', topic, pub_uris) + msg_suffix = "result=%s" % ret + except Exception as ex: + msg_suffix = "exception=%s" % ex + raise + finally: + delta_sec = time.time() - start_sec + mloginfo("%s: sec=%0.2f, %s", msg, delta_sec, msg_suffix) + + +def service_update_task(api, service, uri): + """ + Contact api.serviceUpdate with specified parameters + @param api: XML-RPC URI of node to contact + @type api: str + @param service: Service name to send to node + @type service: str + @param uri: URI to send to node + @type uri: str + """ + mloginfo("serviceUpdate[%s, %s] -> %s",service, uri, api) + xmlrpcapi(api).serviceUpdate('/master', service, uri) + +################################################### +# Master Implementation + +class ROSMasterHandler(object): + """ + XML-RPC handler for ROS master APIs. + API routines for the ROS Master Node. The Master Node is a + superset of the Slave Node and contains additional API methods for + creating and monitoring a graph of slave nodes. + + By convention, ROS nodes take in caller_id as the first parameter + of any API call. The setting of this parameter is rarely done by + client code as ros::msproxy::MasterProxy automatically inserts + this parameter (see ros::client::getMaster()). + """ + + def __init__(self, num_workers=NUM_WORKERS): + """ctor.""" + + self.uri = None + self.done = False + + self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers) + # pub/sub/providers: dict { topicName : [publishers/subscribers names] } + self.ps_lock = threading.Condition(threading.Lock()) + + self.reg_manager = RegistrationManager(self.thread_pool) + + # maintain refs to reg_manager fields + self.publishers = self.reg_manager.publishers + self.subscribers = self.reg_manager.subscribers + self.services = self.reg_manager.services + self.param_subscribers = self.reg_manager.param_subscribers + + self.topics_types = {} #dict { topicName : type } + + # parameter server dictionary + self.param_server = rosmaster.paramserver.ParamDictionary(self.reg_manager) + + def _shutdown(self, reason=''): + if self.thread_pool is not None: + self.thread_pool.join_all(wait_for_tasks=False, wait_for_threads=False) + self.thread_pool = None + self.done = True + + def _ready(self, uri): + """ + Initialize the handler with the XMLRPC URI. This is a standard callback from the XmlRpcNode API. + + @param uri: XML-RPC URI + @type uri: str + """ + self.uri = uri + + def _ok(self): + return not self.done + + ############################################################################### + # EXTERNAL API + + @apivalidate(0, (None, )) + def shutdown(self, caller_id, msg=''): + """ + Stop this server + @param caller_id: ROS caller id + @type caller_id: str + @param msg: a message describing why the node is being shutdown. + @type msg: str + @return: [code, msg, 0] + @rtype: [int, str, int] + """ + if msg: + print("shutdown request: %s" % msg, file=sys.stdout) + else: + print("shutdown requst", file=sys.stdout) + self._shutdown('external shutdown request from [%s]: %s'%(caller_id, msg)) + return 1, "shutdown", 0 + + @apivalidate('') + def getUri(self, caller_id): + """ + Get the XML-RPC URI of this server. + @param caller_id str: ROS caller id + @return [int, str, str]: [1, "", xmlRpcUri] + """ + return 1, "", self.uri + + + @apivalidate(-1) + def getPid(self, caller_id): + """ + Get the PID of this server + @param caller_id: ROS caller id + @type caller_id: str + @return: [1, "", serverProcessPID] + @rtype: [int, str, int] + """ + return 1, "", os.getpid() + + + ################################################################ + # PARAMETER SERVER ROUTINES + + @apivalidate(0, (non_empty_str('key'),)) + def deleteParam(self, caller_id, key): + """ + Parameter Server: delete parameter + @param caller_id: ROS caller id + @type caller_id: str + @param key: parameter name + @type key: str + @return: [code, msg, 0] + @rtype: [int, str, int] + """ + try: + key = resolve_name(key, caller_id) + self.param_server.delete_param(key, self._notify_param_subscribers) + mloginfo("-PARAM [%s] by %s",key, caller_id) + return 1, "parameter %s deleted"%key, 0 + except KeyError as e: + return -1, "parameter [%s] is not set"%key, 0 + + @apivalidate(0, (non_empty_str('key'), not_none('value'))) + def setParam(self, caller_id, key, value): + """ + Parameter Server: set parameter. NOTE: if value is a + dictionary it will be treated as a parameter tree, where key + is the parameter namespace. For example::: + {'x':1,'y':2,'sub':{'z':3}} + + will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it + will replace all existing parameters in the key parameter + namespace with the parameters in value. You must set + parameters individually if you wish to perform a union update. + + @param caller_id: ROS caller id + @type caller_id: str + @param key: parameter name + @type key: str + @param value: parameter value. + @type value: XMLRPCLegalValue + @return: [code, msg, 0] + @rtype: [int, str, int] + """ + key = resolve_name(key, caller_id) + self.param_server.set_param(key, value, self._notify_param_subscribers, caller_id) + mloginfo("+PARAM [%s] by %s",key, caller_id) + return 1, "parameter %s set"%key, 0 + + @apivalidate(0, (non_empty_str('key'),)) + def getParam(self, caller_id, key): + """ + Retrieve parameter value from server. + @param caller_id: ROS caller id + @type caller_id: str + @param key: parameter to lookup. If key is a namespace, + getParam() will return a parameter tree. + @type key: str + getParam() will return a parameter tree. + + @return: [code, statusMessage, parameterValue]. If code is not + 1, parameterValue should be ignored. If key is a namespace, + the return value will be a dictionary, where each key is a + parameter in that namespace. Sub-namespaces are also + represented as dictionaries. + @rtype: [int, str, XMLRPCLegalValue] + """ + try: + key = resolve_name(key, caller_id) + return 1, "Parameter [%s]"%key, self.param_server.get_param(key) + except KeyError as e: + return -1, "Parameter [%s] is not set"%key, 0 + + @apivalidate(0, (non_empty_str('key'),)) + def searchParam(self, caller_id, key): + """ + Search for parameter key on parameter server. Search starts in caller's namespace and proceeds + upwards through parent namespaces until Parameter Server finds a matching key. + + searchParam's behavior is to search for the first partial match. + For example, imagine that there are two 'robot_description' parameters:: + + /robot_description + /robot_description/arm + /robot_description/base + /pr2/robot_description + /pr2/robot_description/base + + If I start in the namespace /pr2/foo and search for + 'robot_description', searchParam will match + /pr2/robot_description. If I search for 'robot_description/arm' + it will return /pr2/robot_description/arm, even though that + parameter does not exist (yet). + + @param caller_id str: ROS caller id + @type caller_id: str + @param key: parameter key to search for. + @type key: str + @return: [code, statusMessage, foundKey]. If code is not 1, foundKey should be + ignored. + @rtype: [int, str, str] + """ + search_key = self.param_server.search_param(caller_id, key) + if search_key: + return 1, "Found [%s]"%search_key, search_key + else: + return -1, "Cannot find parameter [%s] in an upwards search"%key, '' + + @apivalidate(0, (is_api('caller_api'), non_empty_str('key'),)) + def subscribeParam(self, caller_id, caller_api, key): + """ + Retrieve parameter value from server and subscribe to updates to that param. See + paramUpdate() in the Node API. + @param caller_id str: ROS caller id + @type caller_id: str + @param key: parameter to lookup. + @type key: str + @param caller_api: API URI for paramUpdate callbacks. + @type caller_api: str + @return: [code, statusMessage, parameterValue]. If code is not + 1, parameterValue should be ignored. parameterValue is an empty dictionary if the parameter + has not been set yet. + @rtype: [int, str, XMLRPCLegalValue] + """ + key = resolve_name(key, caller_id) + try: + # ps_lock has precedence and is required due to + # potential self.reg_manager modification + self.ps_lock.acquire() + val = self.param_server.subscribe_param(key, (caller_id, caller_api)) + finally: + self.ps_lock.release() + return 1, "Subscribed to parameter [%s]"%key, val + + @apivalidate(0, (is_api('caller_api'), non_empty_str('key'),)) + def unsubscribeParam(self, caller_id, caller_api, key): + """ + Retrieve parameter value from server and subscribe to updates to that param. See + paramUpdate() in the Node API. + @param caller_id str: ROS caller id + @type caller_id: str + @param key: parameter to lookup. + @type key: str + @param caller_api: API URI for paramUpdate callbacks. + @type caller_api: str + @return: [code, statusMessage, numUnsubscribed]. + If numUnsubscribed is zero it means that the caller was not subscribed to the parameter. + @rtype: [int, str, int] + """ + key = resolve_name(key, caller_id) + try: + # ps_lock is required due to potential self.reg_manager modification + self.ps_lock.acquire() + retval = self.param_server.unsubscribe_param(key, (caller_id, caller_api)) + finally: + self.ps_lock.release() + return 1, "Unsubscribe to parameter [%s]"%key, 1 + + + @apivalidate(False, (non_empty_str('key'),)) + def hasParam(self, caller_id, key): + """ + Check if parameter is stored on server. + @param caller_id str: ROS caller id + @type caller_id: str + @param key: parameter to check + @type key: str + @return: [code, statusMessage, hasParam] + @rtype: [int, str, bool] + """ + key = resolve_name(key, caller_id) + if self.param_server.has_param(key): + return 1, key, True + else: + return 1, key, False + + @apivalidate([]) + def getParamNames(self, caller_id): + """ + Get list of all parameter names stored on this server. + This does not adjust parameter names for caller's scope. + + @param caller_id: ROS caller id + @type caller_id: str + @return: [code, statusMessage, parameterNameList] + @rtype: [int, str, [str]] + """ + return 1, "Parameter names", self.param_server.get_param_names() + + ################################################################################## + # NOTIFICATION ROUTINES + + def _notify(self, registrations, task, key, value, node_apis): + """ + Generic implementation of callback notification + @param registrations: Registrations + @type registrations: L{Registrations} + @param task: task to queue + @type task: fn + @param key: registration key + @type key: str + @param value: value to pass to task + @type value: Any + """ + # cache thread_pool for thread safety + thread_pool = self.thread_pool + if not thread_pool: + return + + try: + for node_api in node_apis: + # use the api as a marker so that we limit one thread per subscriber + thread_pool.queue_task(node_api, task, (node_api, key, value)) + except KeyError: + _logger.warn('subscriber data stale (key [%s], listener [%s]): node API unknown'%(key, s)) + + def _notify_param_subscribers(self, updates): + """ + Notify parameter subscribers of new parameter value + @param updates [([str], str, any)*]: [(subscribers, param_key, param_value)*] + @param param_value str: parameter value + """ + # cache thread_pool for thread safety + thread_pool = self.thread_pool + if not thread_pool: + return + + for subscribers, key, value in updates: + # use the api as a marker so that we limit one thread per subscriber + for caller_id, caller_api in subscribers: + self.thread_pool.queue_task(caller_api, self.param_update_task, (caller_id, caller_api, key, value)) + + def param_update_task(self, caller_id, caller_api, param_key, param_value): + """ + Contact api.paramUpdate with specified parameters + @param caller_id: caller ID + @type caller_id: str + @param caller_api: XML-RPC URI of node to contact + @type caller_api: str + @param param_key: parameter key to pass to node + @type param_key: str + @param param_value: parameter value to pass to node + @type param_value: str + """ + mloginfo("paramUpdate[%s]", param_key) + code, _, _ = xmlrpcapi(caller_api).paramUpdate('/master', param_key, param_value) + if code == -1: + try: + # ps_lock is required due to potential self.reg_manager modification + self.ps_lock.acquire() + # reverse lookup to figure out who we just called + matches = self.reg_manager.reverse_lookup(caller_api) + for m in matches: + retval = self.param_server.unsubscribe_param(param_key, (m.id, caller_api)) + finally: + self.ps_lock.release() + + def _notify_topic_subscribers(self, topic, pub_uris, sub_uris): + """ + Notify subscribers with new publisher list + @param topic: name of topic + @type topic: str + @param pub_uris: list of URIs of publishers. + @type pub_uris: [str] + """ + self._notify(self.subscribers, publisher_update_task, topic, pub_uris, sub_uris) + + ################################################################################## + # SERVICE PROVIDER + + @apivalidate(0, ( is_service('service'), is_api('service_api'), is_api('caller_api'))) + def registerService(self, caller_id, service, service_api, caller_api): + """ + Register the caller as a provider of the specified service. + @param caller_id str: ROS caller id + @type caller_id: str + @param service: Fully-qualified name of service + @type service: str + @param service_api: Service URI + @type service_api: str + @param caller_api: XML-RPC URI of caller node + @type caller_api: str + @return: (code, message, ignore) + @rtype: (int, str, int) + """ + try: + self.ps_lock.acquire() + self.reg_manager.register_service(service, caller_id, caller_api, service_api) + mloginfo("+SERVICE [%s] %s %s", service, caller_id, caller_api) + finally: + self.ps_lock.release() + return 1, "Registered [%s] as provider of [%s]"%(caller_id, service), 1 + + @apivalidate('', (is_service('service'),)) + def lookupService(self, caller_id, service): + """ + Lookup all provider of a particular service. + @param caller_id str: ROS caller id + @type caller_id: str + @param service: fully-qualified name of service to lookup. + @type: service: str + @return: (code, message, serviceUrl). service URL is provider's + ROSRPC URI with address and port. Fails if there is no provider. + @rtype: (int, str, str) + """ + try: + self.ps_lock.acquire() + service_url = self.services.get_service_api(service) + finally: + self.ps_lock.release() + if service_url: + return 1, "rosrpc URI: [%s]"%service_url, service_url + else: + return -1, "no provider", '' + + @apivalidate(0, ( is_service('service'), is_api('service_api'))) + def unregisterService(self, caller_id, service, service_api): + """ + Unregister the caller as a provider of the specified service. + @param caller_id str: ROS caller id + @type caller_id: str + @param service: Fully-qualified name of service + @type service: str + @param service_api: API URI of service to unregister. Unregistration will only occur if current + registration matches. + @type service_api: str + @return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1). + If this is zero it means that the caller was not registered as a service provider. + The call still succeeds as the intended final state is reached. + @rtype: (int, str, int) + """ + try: + self.ps_lock.acquire() + retval = self.reg_manager.unregister_service(service, caller_id, service_api) + mloginfo("-SERVICE [%s] %s %s", service, caller_id, service_api) + return retval + finally: + self.ps_lock.release() + + ################################################################################## + # PUBLISH/SUBSCRIBE + + @apivalidate([], ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api'))) + def registerSubscriber(self, caller_id, topic, topic_type, caller_api): + """ + Subscribe the caller to the specified topic. In addition to receiving + a list of current publishers, the subscriber will also receive notifications + of new publishers via the publisherUpdate API. + @param caller_id: ROS caller id + @type caller_id: str + @param topic str: Fully-qualified name of topic to subscribe to. + @param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name. + @type topic_type: str + @param caller_api: XML-RPC URI of caller node for new publisher notifications + @type caller_api: str + @return: (code, message, publishers). Publishers is a list of XMLRPC API URIs + for nodes currently publishing the specified topic. + @rtype: (int, str, [str]) + """ + #NOTE: subscribers do not get to set topic type + try: + self.ps_lock.acquire() + self.reg_manager.register_subscriber(topic, caller_id, caller_api) + + # ROS 1.1: subscriber can now set type if it is not already set + # - don't let '*' type squash valid typing + if not topic in self.topics_types and topic_type != rosgraph.names.ANYTYPE: + self.topics_types[topic] = topic_type + + mloginfo("+SUB [%s] %s %s",topic, caller_id, caller_api) + pub_uris = self.publishers.get_apis(topic) + finally: + self.ps_lock.release() + return 1, "Subscribed to [%s]"%topic, pub_uris + + @apivalidate(0, (is_topic('topic'), is_api('caller_api'))) + def unregisterSubscriber(self, caller_id, topic, caller_api): + """ + Unregister the caller as a subscriber of the topic. + @param caller_id: ROS caller id + @type caller_id: str + @param topic: Fully-qualified name of topic to unregister. + @type topic: str + @param caller_api: API URI of service to unregister. Unregistration will only occur if current + registration matches. + @type caller_api: str + @return: (code, statusMessage, numUnsubscribed). + If numUnsubscribed is zero it means that the caller was not registered as a subscriber. + The call still succeeds as the intended final state is reached. + @rtype: (int, str, int) + """ + try: + self.ps_lock.acquire() + retval = self.reg_manager.unregister_subscriber(topic, caller_id, caller_api) + mloginfo("-SUB [%s] %s %s",topic, caller_id, caller_api) + return retval + finally: + self.ps_lock.release() + + @apivalidate([], ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api'))) + def registerPublisher(self, caller_id, topic, topic_type, caller_api): + """ + Register the caller as a publisher the topic. + @param caller_id: ROS caller id + @type caller_id: str + @param topic: Fully-qualified name of topic to register. + @type topic: str + @param topic_type: Datatype for topic. Must be a + package-resource name, i.e. the .msg name. + @type topic_type: str + @param caller_api str: ROS caller XML-RPC API URI + @type caller_api: str + @return: (code, statusMessage, subscriberApis). + List of current subscribers of topic in the form of XMLRPC URIs. + @rtype: (int, str, [str]) + """ + #NOTE: we need topic_type for getPublishedTopics. + try: + self.ps_lock.acquire() + self.reg_manager.register_publisher(topic, caller_id, caller_api) + # don't let '*' type squash valid typing + if topic_type != rosgraph.names.ANYTYPE or not topic in self.topics_types: + self.topics_types[topic] = topic_type + pub_uris = self.publishers.get_apis(topic) + sub_uris = self.subscribers.get_apis(topic) + self._notify_topic_subscribers(topic, pub_uris, sub_uris) + mloginfo("+PUB [%s] %s %s",topic, caller_id, caller_api) + sub_uris = self.subscribers.get_apis(topic) + finally: + self.ps_lock.release() + return 1, "Registered [%s] as publisher of [%s]"%(caller_id, topic), sub_uris + + + @apivalidate(0, (is_topic('topic'), is_api('caller_api'))) + def unregisterPublisher(self, caller_id, topic, caller_api): + """ + Unregister the caller as a publisher of the topic. + @param caller_id: ROS caller id + @type caller_id: str + @param topic: Fully-qualified name of topic to unregister. + @type topic: str + @param caller_api str: API URI of service to + unregister. Unregistration will only occur if current + registration matches. + @type caller_api: str + @return: (code, statusMessage, numUnregistered). + If numUnregistered is zero it means that the caller was not registered as a publisher. + The call still succeeds as the intended final state is reached. + @rtype: (int, str, int) + """ + try: + self.ps_lock.acquire() + retval = self.reg_manager.unregister_publisher(topic, caller_id, caller_api) + if retval[VAL]: + self._notify_topic_subscribers(topic, self.publishers.get_apis(topic), self.subscribers.get_apis(topic)) + mloginfo("-PUB [%s] %s %s",topic, caller_id, caller_api) + finally: + self.ps_lock.release() + return retval + + ################################################################################## + # GRAPH STATE APIS + + @apivalidate('', (valid_name('node'),)) + def lookupNode(self, caller_id, node_name): + """ + Get the XML-RPC URI of the node with the associated + name/caller_id. This API is for looking information about + publishers and subscribers. Use lookupService instead to lookup + ROS-RPC URIs. + @param caller_id: ROS caller id + @type caller_id: str + @param node: name of node to lookup + @type node: str + @return: (code, msg, URI) + @rtype: (int, str, str) + """ + try: + self.ps_lock.acquire() + node = self.reg_manager.get_node(node_name) + if node is not None: + retval = 1, "node api", node.api + else: + retval = -1, "unknown node [%s]"%node_name, '' + finally: + self.ps_lock.release() + return retval + + @apivalidate(0, (empty_or_valid_name('subgraph'),)) + def getPublishedTopics(self, caller_id, subgraph): + """ + Get list of topics that can be subscribed to. This does not return topics that have no publishers. + See L{getSystemState()} to get more comprehensive list. + @param caller_id: ROS caller id + @type caller_id: str + @param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace + is resolved relative to the caller's namespace. Use '' to specify all names. + @type subgraph: str + @return: (code, msg, [[topic1, type1]...[topicN, typeN]]) + @rtype: (int, str, [[str, str],]) + """ + try: + self.ps_lock.acquire() + # force subgraph to be a namespace with trailing slash + if subgraph and subgraph[-1] != rosgraph.names.SEP: + subgraph = subgraph + rosgraph.names.SEP + #we don't bother with subscribers as subscribers don't report topic types. also, the intended + #use case is for subscribe-by-topic-type + retval = [[t, self.topics_types[t]] for t in self.publishers.iterkeys() if t.startswith(subgraph)] + finally: + self.ps_lock.release() + return 1, "current topics", retval + + @apivalidate([]) + def getTopicTypes(self, caller_id): + """ + Retrieve list topic names and their types. + @param caller_id: ROS caller id + @type caller_id: str + @rtype: (int, str, [[str,str]] ) + @return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs. + """ + try: + self.ps_lock.acquire() + retval = list(self.topics_types.items()) + finally: + self.ps_lock.release() + return 1, "current system state", retval + + @apivalidate([[],[], []]) + def getSystemState(self, caller_id): + """ + Retrieve list representation of system state (i.e. publishers, subscribers, and services). + @param caller_id: ROS caller id + @type caller_id: str + @rtype: (int, str, [[str,[str]], [str,[str]], [str,[str]]]) + @return: (code, statusMessage, systemState). + + System state is in list representation:: + [publishers, subscribers, services]. + + publishers is of the form:: + [ [topic1, [topic1Publisher1...topic1PublisherN]] ... ] + + subscribers is of the form:: + [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] + + services is of the form:: + [ [service1, [service1Provider1...service1ProviderN]] ... ] + """ + edges = [] + try: + self.ps_lock.acquire() + retval = [r.get_state() for r in (self.publishers, self.subscribers, self.services)] + finally: + self.ps_lock.release() + return 1, "current system state", retval diff --git a/src/hal/components/cros/master_python_source/rosmaster/paramserver.py b/src/hal/components/cros/master_python_source/rosmaster/paramserver.py new file mode 100644 index 00000000..d29b4d30 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/paramserver.py @@ -0,0 +1,402 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. + +from threading import RLock + +from rosgraph.names import ns_join, GLOBALNS, SEP, is_global, is_private, canonicalize_name + +def _get_param_names(names, key, d): + """ + helper recursive routine for getParamNames() + @param names: list of param names to append to + @type names: [str] + @param d: parameter tree node + @type d: dict + @param key: parameter key for tree node d + @type key: str + """ + + #TODOXXX + for k,v in d.items(): + if type(v) == dict: + _get_param_names(names, ns_join(key, k), v) + else: + names.append(ns_join(key, k)) + +class ParamDictionary(object): + + def __init__(self, reg_manager): + """ + ctor. + @param subscribers: parameter subscribers + @type subscribers: Registrations + """ + self.lock = RLock() + self.parameters = {} + self.reg_manager = reg_manager + + def get_param_names(self): + """ + Get list of all parameter names stored on this server. + + @return: [code, statusMessage, parameterNameList] + @rtype: [int, str, [str]] + """ + try: + self.lock.acquire() + param_names = [] + _get_param_names(param_names, '/', self.parameters) + finally: + self.lock.release() + return param_names + + def search_param(self, ns, key): + """ + Search for matching parameter key for search param + key. Search for key starts at ns and proceeds upwards to + the root. As such, search_param should only be called with a + relative parameter name. + + search_param's behavior is to search for the first partial match. + For example, imagine that there are two 'robot_description' parameters: + + - /robot_description + - /robot_description/arm + - /robot_description/base + + - /pr2/robot_description + - /pr2/robot_description/base + + If I start in the namespace /pr2/foo and search for + 'robot_description', search_param will match + /pr2/robot_description. If I search for 'robot_description/arm' + it will return /pr2/robot_description/arm, even though that + parameter does not exist (yet). + + @param ns: namespace to begin search from. + @type ns: str + @param key: Parameter key. + @type key: str + @return: key of matching parameter or None if no matching + parameter. + @rtype: str + """ + if not key or is_private(key): + raise ValueError("invalid key") + if not is_global(ns): + raise ValueError("namespace must be global") + if is_global(key): + if self.has_param(key): + return key + else: + return None + + # there are more efficient implementations, but our hiearchy + # is not very deep and this is fairly clean code to read. + + # - we only search for the first namespace in the key to check for a match + key_namespaces = [x for x in key.split(SEP) if x] + key_ns = key_namespaces[0] + + # - corner case: have to test initial namespace first as + # negative indices won't work with 0 + search_key = ns_join(ns, key_ns) + if self.has_param(search_key): + # resolve to full key + return ns_join(ns, key) + + namespaces = [x for x in ns.split(SEP) if x] + for i in range(1, len(namespaces)+1): + search_key = SEP + SEP.join(namespaces[0:-i] + [key_ns]) + if self.has_param(search_key): + # we have a match on the namespace of the key, so + # compose the full key and return it + full_key = SEP + SEP.join(namespaces[0:-i] + [key]) + return full_key + return None + + def get_param(self, key): + """ + Get the parameter in the parameter dictionary. + + @param key: parameter key + @type key: str + @return: parameter value + """ + try: + self.lock.acquire() + val = self.parameters + if key != GLOBALNS: + # split by the namespace separator, ignoring empty splits + namespaces = [x for x in key.split(SEP)[1:] if x] + for ns in namespaces: + if not type(val) == dict: + raise KeyError(val) + val = val[ns] + return val + finally: + self.lock.release() + + def set_param(self, key, value, notify_task=None, caller_id=None): + """ + Set the parameter in the parameter dictionary. + + @param key: parameter key + @type key: str + @param value: parameter value + @param notify_task: function to call with + subscriber updates. updates is of the form + [(subscribers, param_key, param_value)*]. The empty dictionary + represents an unset parameter. + @type notify_task: fn(updates) + @param caller_id: the caller id + @type caller_id: str + """ + try: + self.lock.acquire() + if key == GLOBALNS: + if type(value) != dict: + raise TypeError("cannot set root of parameter tree to non-dictionary") + self.parameters = value + else: + namespaces = [x for x in key.split(SEP) if x] + # - last namespace is the actual key we're storing in + value_key = namespaces[-1] + namespaces = namespaces[:-1] + d = self.parameters + # - descend tree to the node we're setting + for ns in namespaces: + if not ns in d: + new_d = {} + d[ns] = new_d + d = new_d + else: + val = d[ns] + # implicit type conversion of value to namespace + if type(val) != dict: + d[ns] = val = {} + d = val + + d[value_key] = value + + # ParamDictionary needs to queue updates so that the updates are thread-safe + if notify_task: + updates = compute_param_updates(self.reg_manager.param_subscribers, key, value, caller_id) + if updates: + notify_task(updates) + finally: + self.lock.release() + + + def subscribe_param(self, key, registration_args): + """ + @param key: parameter key + @type key: str + @param registration_args: additional args to pass to + subscribers.register. First parameter is always the parameter + key. + @type registration_args: tuple + """ + if key != SEP: + key = canonicalize_name(key) + SEP + try: + self.lock.acquire() + # fetch parameter value + try: + val = self.get_param(key) + except KeyError: + # parameter not set yet + val = {} + self.reg_manager.register_param_subscriber(key, *registration_args) + return val + finally: + self.lock.release() + + + def unsubscribe_param(self, key, unregistration_args): + """ + @param key str: parameter key + @type key: str + @param unregistration_args: additional args to pass to + subscribers.unregister. i.e. unregister will be called with + (key, *unregistration_args) + @type unregistration_args: tuple + @return: return value of subscribers.unregister() + """ + if key != SEP: + key = canonicalize_name(key) + SEP + return self.reg_manager.unregister_param_subscriber(key, *unregistration_args) + + def delete_param(self, key, notify_task=None): + """ + Delete the parameter in the parameter dictionary. + @param key str: parameter key + @param notify_task fn(updates): function to call with + subscriber updates. updates is of the form + [(subscribers, param_key, param_value)*]. The empty dictionary + represents an unset parameter. + """ + try: + self.lock.acquire() + if key == GLOBALNS: + raise KeyError("cannot delete root of parameter tree") + else: + # key is global, so first split is empty + namespaces = [x for x in key.split(SEP) if x] + # - last namespace is the actual key we're deleting + value_key = namespaces[-1] + namespaces = namespaces[:-1] + d = self.parameters + # - descend tree to the node we're setting + for ns in namespaces: + if type(d) != dict or not ns in d: + raise KeyError(key) + else: + d = d[ns] + + if not value_key in d: + raise KeyError(key) + else: + del d[value_key] + + # ParamDictionary needs to queue updates so that the updates are thread-safe + if notify_task: + updates = compute_param_updates(self.reg_manager.param_subscribers, key, {}) + if updates: + notify_task(updates) + finally: + self.lock.release() + + def has_param(self, key): + """ + Test for parameter existence + + @param key: parameter key + @type key: str + @return: True if parameter set, False otherwise + @rtype: bool + """ + try: + # more efficient implementations are certainly possible, + # but this guarantees correctness for now + self.get_param(key) + return True + except KeyError: + return False + +def _compute_all_keys(param_key, param_value, all_keys=None): + """ + Compute which subscribers should be notified based on the parameter update + @param param_key: key of updated parameter + @type param_key: str + @param param_value: value of updated parameter + @param all_keys: (internal use only) list of parameter keys + to append to for recursive calls. + @type all_keys: [str] + @return: list of parameter keys. All keys will be canonicalized with trailing slash. + @rtype: [str] + """ + if all_keys is None: + all_keys = [] + for k, v in param_value.items(): + new_k = ns_join(param_key, k) + SEP + all_keys.append(new_k) + if type(v) == dict: + _compute_all_keys(new_k, v, all_keys) + return all_keys + +def compute_param_updates(subscribers, param_key, param_value, caller_id_to_ignore=None): + """ + Compute subscribers that should be notified based on the parameter update + @param subscribers: parameter subscribers + @type subscribers: Registrations + @param param_key: parameter key + @type param_key: str + @param param_value: parameter value + @type param_value: str + @param caller_id_to_ignore: the caller to ignore + @type caller_id_to_ignore: str + """ + + # logic correct for both updates and deletions + + if not subscribers: + return [] + + # end with a trailing slash to optimize startswith check from + # needing an extra equals check + if param_key != SEP: + param_key = canonicalize_name(param_key) + SEP + + # compute all the updated keys + if type(param_value) == dict: + all_keys = _compute_all_keys(param_key, param_value) + else: + all_keys = None + + updates = [] + + # subscriber gets update if anything in the subscribed namespace is updated or if its deleted + for sub_key in subscribers.iterkeys(): + ns_key = sub_key + if ns_key[-1] != SEP: + ns_key = sub_key + SEP + if param_key.startswith(ns_key): + node_apis = subscribers[sub_key] + if caller_id_to_ignore is not None: + node_apis = [ + (caller_id, caller_api) + for (caller_id, caller_api) in node_apis + if caller_id != caller_id_to_ignore] + updates.append((node_apis, param_key, param_value)) + elif all_keys is not None and ns_key.startswith(param_key) \ + and not sub_key in all_keys: + # parameter was deleted + node_apis = subscribers[sub_key] + updates.append((node_apis, sub_key, {})) + + # add updates for exact matches within tree + if all_keys is not None: + # #586: iterate over parameter tree for notification + for key in all_keys: + if key in subscribers: + # compute actual update value + sub_key = key[len(param_key):] + namespaces = [x for x in sub_key.split(SEP) if x] + val = param_value + for ns in namespaces: + val = val[ns] + + updates.append((subscribers[key], key, val)) + + return updates + diff --git a/src/hal/components/cros/master_python_source/rosmaster/registrations.py b/src/hal/components/cros/master_python_source/rosmaster/registrations.py new file mode 100644 index 00000000..6cbfddf2 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/registrations.py @@ -0,0 +1,473 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +from rosmaster.util import remove_server_proxy +from rosmaster.util import xmlrpcapi +import rosmaster.exceptions + +"""Data structures for representing registration data in the Master""" + +class NodeRef(object): + """ + Container for node registration information. Used in master's + self.nodes data structure. This is effectively a reference + counter for the node registration information: when the + subscriptions and publications are empty the node registration can + be deleted. + """ + def __init__(self, id, api): + """ + ctor + @param api str: node XML-RPC API + """ + self.id = id + self.api = api + self.param_subscriptions = [] + self.topic_subscriptions = [] + self.topic_publications = [] + self.services = [] + + def clear(self): + """ + Delete all state from this NodeRef except for the api location + """ + self.param_subscriptions = [] + self.topic_subscriptions = [] + self.topic_publications = [] + self.services = [] + + def is_empty(self): + """ + @return: True if node has no active registrations + """ + return sum((len(x) for x in + [self.param_subscriptions, + self.topic_subscriptions, + self.topic_publications, + self.services,])) == 0 + + def add(self, type_, key): + if type_ == Registrations.TOPIC_SUBSCRIPTIONS: + if not key in self.topic_subscriptions: + self.topic_subscriptions.append(key) + elif type_ == Registrations.TOPIC_PUBLICATIONS: + if not key in self.topic_publications: + self.topic_publications.append(key) + elif type_ == Registrations.SERVICE: + if not key in self.services: + self.services.append(key) + elif type_ == Registrations.PARAM_SUBSCRIPTIONS: + if not key in self.param_subscriptions: + self.param_subscriptions.append(key) + else: + raise rosmaster.exceptions.InternalException("internal bug") + + def remove(self, type_, key): + if type_ == Registrations.TOPIC_SUBSCRIPTIONS: + if key in self.topic_subscriptions: + self.topic_subscriptions.remove(key) + elif type_ == Registrations.TOPIC_PUBLICATIONS: + if key in self.topic_publications: + self.topic_publications.remove(key) + elif type_ == Registrations.SERVICE: + if key in self.services: + self.services.remove(key) + elif type_ == Registrations.PARAM_SUBSCRIPTIONS: + if key in self.param_subscriptions: + self.param_subscriptions.remove(key) + else: + raise rosmaster.exceptions.InternalException("internal bug") + +# NOTE: I'm not terribly happy that this task has leaked into the data model. need +# to refactor to get this back into masterslave. + +def shutdown_node_task(api, caller_id, reason): + """ + Method to shutdown another ROS node. Generally invoked within a + separate thread as this is used to cleanup hung nodes. + + @param api: XML-RPC API of node to shutdown + @type api: str + @param caller_id: name of node being shutdown + @type caller_id: str + @param reason: human-readable reason why node is being shutdown + @type reason: str + """ + try: + xmlrpcapi(api).shutdown('/master', reason) + except: + pass #expected in many common cases + remove_server_proxy(api) + +class Registrations(object): + """ + All calls may result in access/modifications to node registrations + dictionary, so be careful to guarantee appropriate thread-safeness. + + Data structure for storing a set of registrations (e.g. publications, services). + The underlying data storage is the same except for services, which have the + constraint that only one registration may be active for a given key. + """ + + TOPIC_SUBSCRIPTIONS = 1 + TOPIC_PUBLICATIONS = 2 + SERVICE = 3 + PARAM_SUBSCRIPTIONS = 4 + + def __init__(self, type_): + """ + ctor. + @param type_: one of [ TOPIC_SUBSCRIPTIONS, + TOPIC_PUBLICATIONS, SERVICE, PARAM_SUBSCRIPTIONS ] + @type type_: int + """ + if not type_ in [ + Registrations.TOPIC_SUBSCRIPTIONS, + Registrations.TOPIC_PUBLICATIONS, + Registrations.SERVICE, + Registrations.PARAM_SUBSCRIPTIONS ]: + raise rosmaster.exceptions.InternalException("invalid registration type: %s"%type_) + self.type = type_ + ## { key: [(caller_id, caller_api)] } + self.map = {} + self.service_api_map = None + + def __bool__(self): + """ + @return: True if there are registrations + """ + return len(self.map) != 0 + + def __nonzero__(self): + """ + @return: True if there are registrations + """ + return len(self.map) != 0 + + def iterkeys(self): + """ + Iterate over registration keys + @return: iterator for registration keys + """ + return self.map.keys() + + def get_service_api(self, service): + """ + Lookup service API URI. NOTE: this should only be valid if type==SERVICE as + service Registrations instances are the only ones that track service API URIs. + @param service: service name + @type service: str + @return str: service_api for registered key or None if + registration is no longer valid. + @type: str + """ + if self.service_api_map and service in self.service_api_map: + caller_id, service_api = self.service_api_map[service] + return service_api + return None + + def get_apis(self, key): + """ + Only valid if self.type != SERVICE. + @param key: registration key (e.g. topic/service/param name) + @type key: str + @return: caller_apis for registered key, empty list if registration is not valid + @rtype: [str] + """ + return [api for _, api in self.map.get(key, [])] + + def __contains__(self, key): + """ + Emulate mapping type for has_key() + """ + return key in self.map + + def __getitem__(self, key): + """ + @param key: registration key (e.g. topic/service/param name) + @type key: str + @return: (caller_id, caller_api) for registered + key, empty list if registration is not valid + @rtype: [(str, str),] + """ + # unlike get_apis, returns the caller_id to prevent any race + # conditions that can occur if caller_id/caller_apis change + # due to a new node. + return self.map.get(key, []) + + def has_key(self, key): + """ + @param key: registration key (e.g. topic/service/param name) + @type key: str + @return: True if key is registered + @rtype: bool + """ + return key in self.map + + def get_state(self): + """ + @return: state in getSystemState()-friendly format [ [key, [callerId1...callerIdN]] ... ] + @rtype: [str, [str]...] + """ + retval = [] + for k in self.map.keys(): + retval.append([k, [id for id, _ in self.map[k]]]) + return retval + + def register(self, key, caller_id, caller_api, service_api=None): + """ + Add caller_id into the map as a provider of the specified + service (key). caller_id must not have been previously + registered with a different caller_api. + + Subroutine for managing provider map data structure (essentially a multimap). + @param key: registration key (e.g. topic/service/param name) + @type key: str + @param caller_id: caller_id of provider + @type caller_id: str + @param caller_api: API URI of provider + @type caller_api: str + @param service_api: (keyword) ROS service API URI if registering a service + @type service_api: str + """ + map = self.map + if key in map and not service_api: + providers = map[key] + if not (caller_id, caller_api) in providers: + providers.append((caller_id, caller_api)) + else: + map[key] = providers = [(caller_id, caller_api)] + + if service_api: + if self.service_api_map is None: + self.service_api_map = {} + self.service_api_map[key] = (caller_id, service_api) + elif self.type == Registrations.SERVICE: + raise rosmaster.exceptions.InternalException("service_api must be specified for Registrations.SERVICE") + + def unregister_all(self, caller_id): + """ + Remove all registrations associated with caller_id + @param caller_id: caller_id of provider + @type caller_id: str + """ + map = self.map + # fairly expensive + dead_keys = [] + for key in map: + providers = map[key] + # find all matching entries + to_remove = [(id, api) for id, api in providers if id == caller_id] + # purge them + for r in to_remove: + providers.remove(r) + if not providers: + dead_keys.append(key) + for k in dead_keys: + del self.map[k] + if self.type == Registrations.SERVICE and self.service_api_map: + del dead_keys[:] + for key, val in self.service_api_map.items(): + if val[0] == caller_id: + dead_keys.append(key) + for k in dead_keys: + del self.service_api_map[k] + + def unregister(self, key, caller_id, caller_api, service_api=None): + """ + Remove caller_id from the map as a provider of the specified service (key). + Subroutine for managing provider map data structure, essentially a multimap + @param key: registration key (e.g. topic/service/param name) + @type key: str + @param caller_id: caller_id of provider + @type caller_id: str + @param caller_api: API URI of provider + @type caller_api: str + @param service_api: (keyword) ROS service API URI if registering a service + @type service_api: str + @return: for ease of master integration, directly returns unregister value for + higher-level XMLRPC API. val is the number of APIs unregistered (0 or 1) + @rtype: code, msg, val + """ + # if we are unregistering a topic, validate against the caller_api + if service_api: + # validate against the service_api + if self.service_api_map is None: + return 1, "[%s] is not a provider of [%s]"%(caller_id, key), 0 + if self.service_api_map.get(key, None) != (caller_id, service_api): + return 1, "[%s] is no longer the current service api handle for [%s]"%(service_api, key), 0 + else: + del self.service_api_map[key] + del self.map[key] + # caller_api is None for unregister service, so we can't validate as well + return 1, "Unregistered [%s] as provider of [%s]"%(caller_id, key), 1 + elif self.type == Registrations.SERVICE: + raise rosmaster.exceptions.InternalException("service_api must be specified for Registrations.SERVICE") + else: + providers = self.map.get(key, []) + if (caller_id, caller_api) in providers: + providers.remove((caller_id, caller_api)) + if not providers: + del self.map[key] + return 1, "Unregistered [%s] as provider of [%s]"%(caller_id, key), 1 + else: + return 1, "[%s] is not a known provider of [%s]"%(caller_id, key), 0 + +class RegistrationManager(object): + """ + Stores registrations for Master. + + RegistrationManager is not threadsafe, so access must be externally locked as appropriate + """ + + def __init__(self, thread_pool): + """ + ctor. + @param thread_pool: thread pool for queueing tasks + @type thread_pool: ThreadPool + """ + self.nodes = {} + self.thread_pool = thread_pool + + self.publishers = Registrations(Registrations.TOPIC_PUBLICATIONS) + self.subscribers = Registrations(Registrations.TOPIC_SUBSCRIPTIONS) + self.services = Registrations(Registrations.SERVICE) + self.param_subscribers = Registrations(Registrations.PARAM_SUBSCRIPTIONS) + + + def reverse_lookup(self, caller_api): + """ + Get a NodeRef by caller_api + @param caller_api: caller XML RPC URI + @type caller_api: str + @return: nodes that declare caller_api as their + API. 99.9% of the time this should only be one node, but we + allow for multiple matches as the master API does not restrict + this. + @rtype: [NodeRef] + """ + matches = [n for n in self.nodes.items() if n.api == caller_api] + if matches: + return matches + + def get_node(self, caller_id): + return self.nodes.get(caller_id, None) + + def _register(self, r, key, caller_id, caller_api, service_api=None): + # update node information + node_ref, changed = self._register_node_api(caller_id, caller_api) + node_ref.add(r.type, key) + # update pub/sub/service indicies + if changed: + self.publishers.unregister_all(caller_id) + self.subscribers.unregister_all(caller_id) + self.services.unregister_all(caller_id) + self.param_subscribers.unregister_all(caller_id) + r.register(key, caller_id, caller_api, service_api) + + def _unregister(self, r, key, caller_id, caller_api, service_api=None): + node_ref = self.nodes.get(caller_id, None) + if node_ref != None: + retval = r.unregister(key, caller_id, caller_api, service_api) + # check num removed field, if 1, unregister is valid + if retval[2] == 1: + node_ref.remove(r.type, key) + if node_ref.is_empty(): + del self.nodes[caller_id] + else: + retval = 1, "[%s] is not a registered node"%caller_id, 0 + return retval + + def register_service(self, service, caller_id, caller_api, service_api): + """ + Register service provider + @return: None + """ + self._register(self.services, service, caller_id, caller_api, service_api) + def register_publisher(self, topic, caller_id, caller_api): + """ + Register topic publisher + @return: None + """ + self._register(self.publishers, topic, caller_id, caller_api) + def register_subscriber(self, topic, caller_id, caller_api): + """ + Register topic subscriber + @return: None + """ + self._register(self.subscribers, topic, caller_id, caller_api) + def register_param_subscriber(self, param, caller_id, caller_api): + """ + Register param subscriber + @return: None + """ + self._register(self.param_subscribers, param, caller_id, caller_api) + + def unregister_service(self, service, caller_id, service_api): + caller_api = None + return self._unregister(self.services, service, caller_id, caller_api, service_api) + + def unregister_subscriber(self, topic, caller_id, caller_api): + return self._unregister(self.subscribers, topic, caller_id, caller_api) + def unregister_publisher(self, topic, caller_id, caller_api): + return self._unregister(self.publishers, topic, caller_id, caller_api) + def unregister_param_subscriber(self, param, caller_id, caller_api): + return self._unregister(self.param_subscribers, param, caller_id, caller_api) + + def _register_node_api(self, caller_id, caller_api): + """ + @param caller_id: caller_id of provider + @type caller_id: str + @param caller_api: caller_api of provider + @type caller_api: str + @return: (registration_information, changed_registration). changed_registration is true if + caller_api is differet than the one registered with caller_id + @rtype: (NodeRef, bool) + """ + node_ref = self.nodes.get(caller_id, None) + + bumped_api = None + if node_ref is not None: + if node_ref.api == caller_api: + return node_ref, False + else: + bumped_api = node_ref.api + self.thread_pool.queue_task(bumped_api, shutdown_node_task, + (bumped_api, caller_id, "new node registered with same name")) + + node_ref = NodeRef(caller_id, caller_api) + self.nodes[caller_id] = node_ref + return (node_ref, bumped_api != None) + + diff --git a/src/hal/components/cros/master_python_source/rosmaster/threadpool.py b/src/hal/components/cros/master_python_source/rosmaster/threadpool.py new file mode 100644 index 00000000..6d195dca --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/threadpool.py @@ -0,0 +1,228 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +""" +Internal threadpool library for zenmaster. + +Adapted from U{http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/203871} + +Added a 'marker' to tasks so that multiple tasks with the same +marker are not executed. As we are using the thread pool for i/o +tasks, the marker is set to the i/o name. This prevents a slow i/o +for gobbling up all of our threads +""" + +import threading, logging, traceback +from time import sleep + +class MarkedThreadPool(object): + + """Flexible thread pool class. Creates a pool of threads, then + accepts tasks that will be dispatched to the next available + thread.""" + + def __init__(self, numThreads): + + """Initialize the thread pool with numThreads workers.""" + + self.__threads = [] + self.__resizeLock = threading.Condition(threading.Lock()) + self.__taskLock = threading.Condition(threading.Lock()) + self.__tasks = [] + self.__markers = set() + self.__isJoining = False + self.set_thread_count(numThreads) + + def set_thread_count(self, newNumThreads): + + """ External method to set the current pool size. Acquires + the resizing lock, then calls the internal version to do real + work.""" + + # Can't change the thread count if we're shutting down the pool! + if self.__isJoining: + return False + + self.__resizeLock.acquire() + try: + self.__set_thread_count_nolock(newNumThreads) + finally: + self.__resizeLock.release() + return True + + def __set_thread_count_nolock(self, newNumThreads): + + """Set the current pool size, spawning or terminating threads + if necessary. Internal use only; assumes the resizing lock is + held.""" + + # If we need to grow the pool, do so + while newNumThreads > len(self.__threads): + newThread = ThreadPoolThread(self) + self.__threads.append(newThread) + newThread.start() + # If we need to shrink the pool, do so + while newNumThreads < len(self.__threads): + self.__threads[0].go_away() + del self.__threads[0] + + def get_thread_count(self): + """@return: number of threads in the pool.""" + self.__resizeLock.acquire() + try: + return len(self.__threads) + finally: + self.__resizeLock.release() + + def queue_task(self, marker, task, args=None, taskCallback=None): + + """Insert a task into the queue. task must be callable; + args and taskCallback can be None.""" + + if self.__isJoining == True: + return False + if not callable(task): + return False + + self.__taskLock.acquire() + try: + self.__tasks.append((marker, task, args, taskCallback)) + return True + finally: + self.__taskLock.release() + + def remove_marker(self, marker): + """Remove the marker from the currently executing tasks. Only one + task with the given marker can be executed at a given time""" + if marker is None: + return + self.__taskLock.acquire() + try: + self.__markers.remove(marker) + finally: + self.__taskLock.release() + + def get_next_task(self): + + """ Retrieve the next task from the task queue. For use + only by ThreadPoolThread objects contained in the pool.""" + + self.__taskLock.acquire() + try: + retval = None + for marker, task, args, callback in self.__tasks: + # unmarked or not currently executing + if marker is None or marker not in self.__markers: + retval = (marker, task, args, callback) + break + if retval: + # add the marker so we don't do any similar tasks + self.__tasks.remove(retval) + if marker is not None: + self.__markers.add(marker) + return retval + else: + return (None, None, None, None) + finally: + self.__taskLock.release() + + def join_all(self, wait_for_tasks = True, wait_for_threads = True): + """ Clear the task queue and terminate all pooled threads, + optionally allowing the tasks and threads to finish.""" + + # Mark the pool as joining to prevent any more task queueing + self.__isJoining = True + + # Wait for tasks to finish + if wait_for_tasks: + while self.__tasks != []: + sleep(.1) + + # Tell all the threads to quit + self.__resizeLock.acquire() + try: + self.__set_thread_count_nolock(0) + self.__isJoining = True + + # Wait until all threads have exited + if wait_for_threads: + for t in self.__threads: + t.join() + del t + + # Reset the pool for potential reuse + self.__isJoining = False + finally: + self.__resizeLock.release() + + + +class ThreadPoolThread(threading.Thread): + """ + Pooled thread class. + """ + + threadSleepTime = 0.1 + + def __init__(self, pool): + """Initialize the thread and remember the pool.""" + threading.Thread.__init__(self) + self.daemon = True #don't block program exit + self.__pool = pool + self.__isDying = False + + def run(self): + """ + Until told to quit, retrieve the next task and execute + it, calling the callback if any. + """ + while self.__isDying == False: + marker, cmd, args, callback = self.__pool.get_next_task() + # If there's nothing to do, just sleep a bit + if cmd is None: + sleep(ThreadPoolThread.threadSleepTime) + else: + try: + try: + result = cmd(*args) + finally: + self.__pool.remove_marker(marker) + if callback is not None: + callback(result) + except Exception as e: + logging.getLogger('rosmaster.threadpool').error(traceback.format_exc()) + + def go_away(self): + """ Exit the run loop next time through.""" + self.__isDying = True diff --git a/src/hal/components/cros/master_python_source/rosmaster/util.py b/src/hal/components/cros/master_python_source/rosmaster/util.py new file mode 100644 index 00000000..ee3a4848 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/util.py @@ -0,0 +1,90 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +""" +Utility routines for rosmaster. +""" + +try: + from urllib.parse import urlparse +except ImportError: + from urlparse import urlparse +try: + from xmlrpc.client import ServerProxy +except ImportError: + from xmlrpclib import ServerProxy + +from defusedxml.xmlrpc import monkey_patch +monkey_patch() +del monkey_patch + +import errno +import socket + +_proxies = {} #cache ServerProxys +def xmlrpcapi(uri): + """ + @return: instance for calling remote server or None if not a valid URI + @rtype: xmlrpc.client.ServerProxy + """ + if uri is None: + return None + uriValidate = urlparse(uri) + if not uriValidate[0] or not uriValidate[1]: + return None + if not uri in _proxies: + _proxies[uri] = ServerProxy(uri) + close_half_closed_sockets() + return _proxies[uri] + + +def close_half_closed_sockets(): + if not hasattr(socket, 'TCP_INFO'): + return + for proxy in _proxies.values(): + transport = proxy("transport") + if transport._connection and transport._connection[1] is not None and transport._connection[1].sock is not None: + try: + state = transport._connection[1].sock.getsockopt(socket.SOL_TCP, socket.TCP_INFO) + except socket.error as e: # catch [Errno 92] Protocol not available + if e.args[0] is errno.ENOPROTOOPT: + return + raise + if state == 8: # CLOSE_WAIT + transport.close() + + +def remove_server_proxy(uri): + if uri in _proxies: + del _proxies[uri] diff --git a/src/hal/components/cros/master_python_source/rosmaster/validators.py b/src/hal/components/cros/master_python_source/rosmaster/validators.py new file mode 100644 index 00000000..417f6ab9 --- /dev/null +++ b/src/hal/components/cros/master_python_source/rosmaster/validators.py @@ -0,0 +1,199 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# 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 Willow Garage, Inc. 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. +# +# Revision $Id$ + +"""Internal-use Python decorators for parameter validation""" + +from rosgraph.names import resolve_name, ANYTYPE + +TYPE_SEPARATOR = '/' +ROSRPC = "rosrpc://" + +def isstring(s): + """Small helper version to check an object is a string in a way that works + for both Python 2 and 3 + """ + try: + return isinstance(s, basestring) + except NameError: + return isinstance(s, str) + + +class ParameterInvalid(Exception): + """Exception that is raised when a parameter fails validation checks""" + def __init__(self, message): + self._message = message + + def __str__(self): + return str(self._message) + +def non_empty(param_name): + """Validator that checks that parameter is not empty""" + def validator(param, context): + if not param: + raise ParameterInvalid("ERROR: parameter [%s] must be specified and non-empty"%param_name) + return param + return validator + +def non_empty_str(param_name): + """Validator that checks that parameter is a string and non-empty""" + def validator(param, context): + if not param: + raise ParameterInvalid("ERROR: parameter [%s] must be specified and non-empty"%param_name) + elif not isstring(param): + raise ParameterInvalid("ERROR: parameter [%s] must be a string"%param_name) + return param + return validator + +def not_none(param_name): + """Validator that checks that parameter is not None""" + def validator(param, context): + if param is None: + raise ParameterInvalid("ERROR: parameter [%s] must be specified"%param_name) + return param + return validator + + +# Validators ###################################### + +def is_api(paramName): + """ + Validator that checks that parameter is a valid API handle + (i.e. URI). Both http and rosrpc are allowed schemes. + """ + def validator(param_value, callerId): + if not param_value or not isstring(param_value): + raise ParameterInvalid("ERROR: parameter [%s] is not an XMLRPC URI"%paramName) + if not param_value.startswith("http://") and not param_value.startswith(ROSRPC): + raise ParameterInvalid("ERROR: parameter [%s] is not an RPC URI"%paramName) + #could do more fancy parsing, but the above catches the major cases well enough + return param_value + return validator + +def is_topic(param_name): + """ + Validator that checks that parameter is a valid ROS topic name + """ + def validator(param_value, caller_id): + v = valid_name_validator_resolved(param_name, param_value, caller_id) + if param_value == '/': + raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) + return v + return validator + +def is_service(param_name): + """Validator that checks that parameter is a valid ROS service name""" + def validator(param_value, caller_id): + v = valid_name_validator_resolved(param_name, param_value, caller_id) + if param_value == '/': + raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) + return v + return validator + +def empty_or_valid_name(param_name): + """ + empty or valid graph resource name. + Validator that resolves names unless they an empty string is supplied, in which case + an empty string is returned. + """ + def validator(param_value, caller_id): + if not isstring(param_value): + raise ParameterInvalid("ERROR: parameter [%s] must be a string"%param_name) + if not param_value: + return '' + #return resolve_name(param_value, namespace(caller_id)) + return resolve_name(param_value, caller_id) + return validator + +def valid_name_validator_resolved(param_name, param_value, caller_id): + if not param_value or not isstring(param_value): + raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) + #TODO: actual validation of chars + # I added the colon check as the common error will be to send an URI instead of name + if ':' in param_value or ' ' in param_value: + raise ParameterInvalid("ERROR: parameter [%s] contains illegal chars"%param_name) + #return resolve_name(param_value, namespace(caller_id)) + return resolve_name(param_value, caller_id) +def valid_name_validator_unresolved(param_name, param_value, caller_id): + if not param_value or not isstring(param_value): + raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) + #TODO: actual validation of chars + # I added the colon check as the common error will be to send an URI instead of name + if ':' in param_value or ' ' in param_value: + raise ParameterInvalid("ERROR: parameter [%s] contains illegal chars"%param_name) + return param_value + +def valid_name(param_name, resolve=True): + """ + Validator that resolves names and also ensures that they are not empty + @param param_name: name + @type param_name: str + @param resolve: if True/omitted, the name will be resolved to + a global form. Otherwise, no resolution occurs. + @type resolve: bool + @return: resolved parameter value + @rtype: str + """ + def validator(param_value, caller_id): + if resolve: + return valid_name_validator_resolved(param_name, param_value, caller_id) + return valid_name_validator_unresolved(param_name, param_value, caller_id) + return validator + +def global_name(param_name): + """ + Validator that checks for valid, global graph resource name. + @return: parameter value + @rtype: str + """ + def validator(param_value, caller_id): + if not param_value or not isstring(param_value): + raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) + #TODO: actual validation of chars + if not is_global(param_value): + raise ParameterInvalid("ERROR: parameter [%s] must be a globally referenced name"%param_name) + return param_value + return validator + +def valid_type_name(param_name): + """validator that checks the type name is specified correctly""" + def validator(param_value, caller_id): + if param_value == ANYTYPE: + return param_value + if not param_value or not isstring(param_value): + raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) + if not len(param_value.split(TYPE_SEPARATOR)) == 2: + raise ParameterInvalid("ERROR: parameter [%s] is not a valid package resource name"%param_name) + #TODO: actual validation of chars + return param_value + return validator diff --git a/src/hal/components/cros/master_python_source/runmaster.py b/src/hal/components/cros/master_python_source/runmaster.py new file mode 100644 index 00000000..6a4afc4f --- /dev/null +++ b/src/hal/components/cros/master_python_source/runmaster.py @@ -0,0 +1,25 @@ +"""!@brief This Python script runs the ROS Master node, which is needed to use a ROS network. + +This script does not requires that a ROS distribution is installed. + +@date 5 Mar 2019 +""" + +#import sys +#sys.path.append('.') +import rosmaster + +try: + input = raw_input +except NameError: + pass + +ros_master=rosmaster.master.Master() +ros_master.start() + +ros_master_ok=ros_master.ok() +print("Master is running: {}".format(ros_master_ok)) + +input("Press Enter to shutdown ROS Master and exit...") + +ros_master.stop() diff --git a/src/hal/components/cros/msvs_projects/cros.sln b/src/hal/components/cros/msvs_projects/cros.sln new file mode 100644 index 00000000..3d77542d --- /dev/null +++ b/src/hal/components/cros/msvs_projects/cros.sln @@ -0,0 +1,99 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 15 +VisualStudioVersion = 15.0.28307.421 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "cros_library", "cros_library.vcxproj", "{57EB7322-CD61-4FD1-B595-C511C4648A37}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "sample_listener", "sample_listener\sample_listener.vcxproj", "{A5BAFDD4-D98D-4922-9F36-928F7C6C738A}" + ProjectSection(ProjectDependencies) = postProject + {57EB7322-CD61-4FD1-B595-C511C4648A37} = {57EB7322-CD61-4FD1-B595-C511C4648A37} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "sample_talker", "sample_talker\sample_talker.vcxproj", "{231D4B94-3DAF-4FF5-AA68-770FA7901E2A}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "parameters_test", "parameters_test\parameters_test.vcxproj", "{5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}" +EndProject +Project("{888888A0-9F3D-457C-B088-3A5042F75D52}") = "ros_master_python", "ros_master_python.pyproj", "{539A16EC-20A8-4148-9A97-542B39A0A62F}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "performance_test", "performance_test\performance_test.vcxproj", "{5ABA6561-E322-4334-83E4-F3FEE2965133}" +EndProject +Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Solution Items", "Solution Items", "{748C592B-7E5D-4A0F-BD18-3EB7C39C92FC}" + ProjectSection(SolutionItems) = preProject + readme.txt = readme.txt + EndProjectSection +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|x64.ActiveCfg = Debug|x64 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|x64.Build.0 = Debug|x64 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|x86.ActiveCfg = Debug|Win32 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|x86.Build.0 = Debug|Win32 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|Any CPU.ActiveCfg = Release|Win32 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|x64.ActiveCfg = Release|x64 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|x64.Build.0 = Release|x64 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|x86.ActiveCfg = Release|Win32 + {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|x86.Build.0 = Release|Win32 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|x64.ActiveCfg = Debug|x64 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|x64.Build.0 = Debug|x64 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|x86.ActiveCfg = Debug|Win32 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|x86.Build.0 = Debug|Win32 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|Any CPU.ActiveCfg = Release|Win32 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|x64.ActiveCfg = Release|x64 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|x64.Build.0 = Release|x64 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|x86.ActiveCfg = Release|Win32 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|x86.Build.0 = Release|Win32 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|x64.ActiveCfg = Debug|x64 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|x64.Build.0 = Debug|x64 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|x86.ActiveCfg = Debug|Win32 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|x86.Build.0 = Debug|Win32 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|Any CPU.ActiveCfg = Release|Win32 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|x64.ActiveCfg = Release|x64 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|x64.Build.0 = Release|x64 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|x86.ActiveCfg = Release|Win32 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|x86.Build.0 = Release|Win32 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|x64.ActiveCfg = Debug|x64 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|x64.Build.0 = Debug|x64 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|x86.ActiveCfg = Debug|Win32 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|x86.Build.0 = Debug|Win32 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|Any CPU.ActiveCfg = Release|Win32 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|x64.ActiveCfg = Release|x64 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|x64.Build.0 = Release|x64 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|x86.ActiveCfg = Release|Win32 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|x86.Build.0 = Release|Win32 + {539A16EC-20A8-4148-9A97-542B39A0A62F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {539A16EC-20A8-4148-9A97-542B39A0A62F}.Debug|x64.ActiveCfg = Debug|Any CPU + {539A16EC-20A8-4148-9A97-542B39A0A62F}.Debug|x86.ActiveCfg = Debug|Any CPU + {539A16EC-20A8-4148-9A97-542B39A0A62F}.Release|Any CPU.ActiveCfg = Release|Any CPU + {539A16EC-20A8-4148-9A97-542B39A0A62F}.Release|x64.ActiveCfg = Release|Any CPU + {539A16EC-20A8-4148-9A97-542B39A0A62F}.Release|x86.ActiveCfg = Release|Any CPU + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|x64.ActiveCfg = Debug|x64 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|x64.Build.0 = Debug|x64 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|x86.ActiveCfg = Debug|Win32 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|x86.Build.0 = Debug|Win32 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|Any CPU.ActiveCfg = Release|Win32 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|x64.ActiveCfg = Release|x64 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|x64.Build.0 = Release|x64 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|x86.ActiveCfg = Release|Win32 + {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {B97C4EE5-3641-461A-9398-88A34183EB84} + EndGlobalSection +EndGlobal diff --git a/src/hal/components/cros/msvs_projects/cros_library.vcxproj b/src/hal/components/cros/msvs_projects/cros_library.vcxproj new file mode 100644 index 00000000..477ecb12 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/cros_library.vcxproj @@ -0,0 +1,207 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {57EB7322-CD61-4FD1-B595-C511C4648A37} + Win32Proj + 10.0.17763.0 + + + + StaticLibrary + true + v141 + + + StaticLibrary + false + v141 + + + StaticLibrary + true + v141 + + + StaticLibrary + false + v141 + + + + + + + + + + + + + + + + + + + + + true + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + cros + + + true + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + cros + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + cros + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + cros + + + + _CRT_SECURE_NO_WARNINGS;_CRT_NONSTDC_NO_DEPRECATE;%(PreprocessorDefinitions) + MultiThreadedDebugDLL + Level3 + ProgramDatabase + Disabled + + + MachineX86 + true + Windows + + + xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ + + + Copy ROS message definition files to working directory + + + + + _CRT_SECURE_NO_WARNINGS;_CRT_NONSTDC_NO_DEPRECATE;%(PreprocessorDefinitions) + MultiThreadedDLL + Level3 + ProgramDatabase + + + MachineX86 + true + Windows + true + true + + + xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ + + + Copy ROS message definition files to working directory + + + + + _CRT_SECURE_NO_WARNINGS;_CRT_NONSTDC_NO_DEPRECATE;%(PreprocessorDefinitions) + + + xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ + + + Copy ROS message definition files to working directory + + + + + _CRT_SECURE_NO_WARNINGS;_CRT_NONSTDC_NO_DEPRECATE;%(PreprocessorDefinitions) + + + xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ + + + Copy ROS message definition files to working directory + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/cros_library.vcxproj.filters b/src/hal/components/cros/msvs_projects/cros_library.vcxproj.filters new file mode 100644 index 00000000..7eabdd86 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/cros_library.vcxproj.filters @@ -0,0 +1,168 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav + + + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + Header Files + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj b/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj new file mode 100644 index 00000000..19da4d62 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj @@ -0,0 +1,159 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26} + parameterstest + 10.0.17763.0 + + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + + Level3 + Disabled + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + Console + + + + + Level3 + Disabled + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + Console + + + + + Level3 + MaxSpeed + true + true + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + true + true + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + Console + + + + + Level3 + MaxSpeed + true + true + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + true + true + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + Console + + + + + {57eb7322-cd61-4fd1-b595-c511c4648a37} + + + + + + + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj.filters b/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj.filters new file mode 100644 index 00000000..5a5df6c7 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Archivos de origen + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj b/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj new file mode 100644 index 00000000..53720019 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj @@ -0,0 +1,159 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {5ABA6561-E322-4334-83E4-F3FEE2965133} + performancetest + 10.0.17763.0 + + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + + Level3 + Disabled + true + true + _MBCS;_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;ws2_32.lib;%(AdditionalDependencies) + Console + + + + + Level3 + Disabled + true + true + _MBCS;_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;ws2_32.lib;%(AdditionalDependencies) + Console + + + + + Level3 + MaxSpeed + true + true + true + true + _MBCS;_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + true + true + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;ws2_32.lib;%(AdditionalDependencies) + Console + + + + + Level3 + MaxSpeed + true + true + true + true + _MBCS;_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) + + + true + true + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;ws2_32.lib;%(AdditionalDependencies) + Console + + + + + + + + {57eb7322-cd61-4fd1-b595-c511c4648a37} + + + + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj.filters b/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj.filters new file mode 100644 index 00000000..a446c2cc --- /dev/null +++ b/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Archivos de origen + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/readme.txt b/src/hal/components/cros/msvs_projects/readme.txt new file mode 100644 index 00000000..54cc727b --- /dev/null +++ b/src/hal/components/cros/msvs_projects/readme.txt @@ -0,0 +1,50 @@ +This is the readme file for the cROS solution of MS Visual Studio. +------------------------------------------------------------------ + +This solution comprises the following projects: +- cros_library: it creates the cros.lib static library, which is used by cROS programs. +- ros_master_python: it creates a ROS master. Since one ROS master is required to have a ROS network working, this program (or other ROS master) must be run before executing any ROS program. +- sample_listener and sample_talker: they create sample_listener.exe and sample_talker.exe, which can be run together to try the operation of a topic publisher, a topic subscriber, a service provider and a service client. +- parameter_test: it checks the parameter subscription and parameter-value update. It must be used together with a ROS program able to update and check parameter values. +- performance_test: this program measures the communication performance of cROS. + +Requirements +------------ +This solution has been designed for Visual Studio 2017. +Windows 10 version 1703 or later ir required to set the socket options TCP_KEEPIDLE, TCP_KEEPINTVL and TCP_KEEPCNT. + +Creation of new projects +------------------------ +If you want to create a new (empty) project in the cros solution for building a new cROS program, the following project options must be set: + +* In the new-project tree: +In references: + Add reference: cros_library +In files: + Add source (and header) files. + +* In the new-project properties: + First select the option: In configuration: "All configurations" and in platform: "All platforms". So the following option changes are applied to all configurations and platforms. +In General: + Output folder: $(SolutionDir)$(Platform)\$(Configuration)\ + Intermediate folder: $(Platform)\$(Configuration)\ +In Debug: + Working directory: $(SolutionDir)$(Platform)\$(Configuration)\ +In VC++ directories: + Directory of header files: add: $(SolutionDir)..\include +In C/C++ -> Preprocessor: + Preprocessor definitions: Add: _CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS +In Linker -> Input: Add: ;ws2_32.lib +In Linker -> System: Set Subsystem to Console + +In Build events -> Post build events (only in cros_library project): + Command line: xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ + Description: Copy ROS message definition files to working directory + +You may also want to disable Win32 Control-C exception catching when debugging, so that this event is catched by the cROS program being run. + +cROS library verbose option +--------------------------- +When debugging the debug and information messages of the cROS library can be helpful. They can be activated by changing the value of CROS_DEBUG_LEVEL in cros_defs.h. +The default debug level value for console messages is 1 (#define CROS_DEBUG_LEVEL 1). +The maximum value (the most vebose level) is 4. diff --git a/src/hal/components/cros/msvs_projects/ros_master_python.pyproj b/src/hal/components/cros/msvs_projects/ros_master_python.pyproj new file mode 100644 index 00000000..4bb39a49 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/ros_master_python.pyproj @@ -0,0 +1,62 @@ + + + + Debug + 2.0 + {539a16ec-20a8-4148-9a97-542b39a0a62f} + ..\master_python_source\ + runmaster.py + + . + . + {888888a0-9f3d-457c-b088-3a5042f75d52} + Standard Python launcher + + + + + + 10.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj b/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj new file mode 100644 index 00000000..447b758a --- /dev/null +++ b/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj @@ -0,0 +1,191 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {A5BAFDD4-D98D-4922-9F36-928F7C6C738A} + samplelistener + 10.0.17763.0 + + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + + Level3 + MaxSpeed + true + true + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + true + true + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + + + + + + + + + + + + + Level3 + Disabled + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + + + + + + + + + + + + + Level3 + Disabled + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + + + + + + + + + + + + + Level3 + MaxSpeed + true + true + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + true + true + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + + + + + + + + + + + + + + + + {57eb7322-cd61-4fd1-b595-c511c4648a37} + + + + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj.filters b/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj.filters new file mode 100644 index 00000000..9b304868 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Archivos de origen + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj b/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj new file mode 100644 index 00000000..dbbdd978 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj @@ -0,0 +1,191 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 15.0 + {231D4B94-3DAF-4FF5-AA68-770FA7901E2A} + sampletalker + 10.0.17763.0 + + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + Application + true + v141 + MultiByte + + + Application + false + v141 + true + MultiByte + + + + + + + + + + + + + + + + + + + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + $(SolutionDir)$(Platform)\$(Configuration)\ + $(Platform)\$(Configuration)\ + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include + + + + Level3 + Disabled + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + + + + + + + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + + + + + Level3 + Disabled + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + + + + + + + + + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + + + + + Level3 + MaxSpeed + true + true + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + true + true + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + + + + + + + + + + + + + Level3 + MaxSpeed + true + true + true + true + _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS + + + true + true + Console + kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib + + + + + + + + + + + + + {57eb7322-cd61-4fd1-b595-c511c4648a37} + + + + + + + + + \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj.filters b/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj.filters new file mode 100644 index 00000000..f0da3499 --- /dev/null +++ b/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj.filters @@ -0,0 +1,22 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Archivos de origen + + + \ No newline at end of file diff --git a/src/hal/components/cros/resources/README b/src/hal/components/cros/resources/README new file mode 100644 index 00000000..bd35d6fe --- /dev/null +++ b/src/hal/components/cros/resources/README @@ -0,0 +1,3 @@ +* rosgraph_msgs/Log.msg: standard message type for ROS logs +* ROS_QUIKSTART: some sample official ROS command samples +* ros_testbed/: Sample ROS package definition used for tests diff --git a/src/hal/components/cros/resources/ROS-QUICKSTART b/src/hal/components/cros/resources/ROS-QUICKSTART new file mode 100644 index 00000000..2611f0d9 --- /dev/null +++ b/src/hal/components/cros/resources/ROS-QUICKSTART @@ -0,0 +1,29 @@ +# Launch a ROS master node +$ roscore + +# Enumerate active topics +$ rostopic list + +# Enumerate publishers and subscribers on the topic /test +$ rostopic info /test + +# Register a publisher on the topic /test, publishing a "Hello world" string with frequency 1 +$ rostopic pub /test std_msgs/String "Hello world" -r1 + +# Register an echo subscriber on the topic /test +$ rostopic echo /test + +# Run a ROS node registered with catkin +$ rosrun + +# Run sample "gripperstatus_talker" node +$ rosrun gripping_robot gripperstatus_talker + +# Request a service +$ rosservice call + +# Sample service "add_two_ints" request +$ rosservice call /add_two_ints 1 2 + +# Sample publisher with internal fields defined +$ rostopic pub /test cros_testbed/Test "{header: { frame_id: /base_link}}" diff --git a/src/hal/components/cros/resources/cROS_logo.jpg b/src/hal/components/cros/resources/cROS_logo.jpg new file mode 100644 index 0000000000000000000000000000000000000000..d88d7fc19b4df926e60418e883c95dca19c71044 GIT binary patch literal 13108 zcmeHtXH-*NxaL8WDnzREsGxL^COtusF1<-t5u^x6M+k%>y(tK21R`C!^bXQ{?}!3H zdP}IGX7cs#-np}8?);oJhZRm%vJdQczwLRRy>U~xdElC=qKYCwaOo1Df&T$;-vBBS zAcJ6%kbnxfL`6VIMS%Mbya9+VUm?13nTY5L5iv0l2^keR87V0l9q8IsDn@!HCI)(j z>&#%To6M}Y*{(C(5@f&4&BMpf$9z*r^e(R`7cU<#0WmQ#87UbpIXNva3j+%a?|=8g zwGpHP0KpZ4-!I_Li{KI=eqzKVq-5mRfJ+2~gqJQ8Ub%AlGJd2l{yX3@)fMWSLidSi zw4V~+a;6myc$Y@P{-C6pPG=a+A@a;6kd%!6Is+rq?K_-Y+&rRU_rxV6rQ{zfC@Lwd zsOswJ8yG$|GPZnfW&Og&*3Q+<-NV!CwRg~);E>R;@QAqh_X&wfA3h?|GcvQXbH3!} zm6nxPR902j)V8#?wRd!Ob@z;nj*U-DPEF4&pca2EEw8Mutz&lg_74t^u*W9^0O23z z;D7&N(7)`#FF(Ph%a;i+6aQup!6i@pi;(K_m779D)c3WCpE}dr5)L4teef==q?wdm zLs2h2T)ynFrcr?sswNAkUd80zke`oFpn^Pg9ogCI_h zZJ%Ipy-^H35!Y9nSd;Tz>cEIo%U*dnmp`6vI0Oe=hh2>=$40avwQt-iKgn`hqI_=t z6&pXlaYX8Xk`+tdw~%!`Tek|$XGOKFt~(SxsSUTPOt>8#BG3MusXM!hk^jcOT@9cm zKVNMyX)}hr70I*nNDR98q)MN6_kE=ydGh#RJCg9*l1ZDl5BTs7QRQRf!Oaran{Us| z(4qyjUJ>oC=WB9l3%qb%%I9uOnvpPQ{#P>*qp!7=Lkf58dh~Kwhs>D$1BEFSyT-O*Z3@&Z z)UqZ&>sb^9+}RN92gw+-0X7{p{O5(!NqR$UsBC9k{Py z5j&7a#=tVlI7^b~wMs&n zaaJk)a=^ZMmp1oqvfF9N>@N1*5)$EJ{^ND*MZCD~nRR1{0?VS`cXu2B6@kxLBYA*R zhsm?Eete`NB(;dYJ2tH>JDqM0_xMBp9u%WTaQMCW`1sSssi@26I~5aR%tqqh=S&rU zrpxQBwQmQmSS{>CvE3W`p$qLy1Fbp^e1m{i_?;Ds_6`zA~+ zZoS(5c<^+}SH#@?WvQC6<5KPsBx2N^TwS9_x&eZcjz6PW`c0##_@P>yQtyQ@whXmH z*M-6XdWpRF1Wmj~9R?*kSbMUjJIz>g?^UT9M~<+xBo44yt}RgR0ra2c@F+vZ2S)yVnjpS> z)^JFD>iXK%W%G!pmtClvuWh-GR%a@X>Z^&4U383IOi2`fVtdj79jiHhjsx5#qr;muV)FHD zGCyu4v}yf5hEko=C&!%d(`v~gnAv$$;nblQJn2+1Nqi5?VmN>- z4F-b2`T=+I@;rVa4A{l>cisToqo zBKE+a_Y!TH?QTUSLIUG$dA~|oU=14uaX^}rJ`VWiQP5bTcKr7EF}$T713xGCF1{eF z06)$;>yrGhJ_3g}3U;1F{%e$x;%N)w{EgL)o6qVscR|uixdz#XPpzMoCP+NFzc2Rr z&1aAvmK|e`|6ksHT4uc*CoFxJhlBLV4It~I#SRC&B!YZdMh-c0 znA+qJ;x_wF!l=1aP#ekX}xCaL>T~goLms z_gTY=0!Zc9($FTT#*BGZa|!ErMWQMnNIzSqAh!h5ie${G#WLJ$v=d@$-PCnPjYkSA zSf#8G6AOeHxdRHY&2-dee`LiCe3%g<&+IX0<^cLlQhvn&c~qx8qpZo~sxWU3`TdEM zS~86;EVJ>rlY616&$$jtEwbiAS=UOO{n}HKW|vOOO9@&`C!8S_4ephzCuU0vI3Qby zzF@`R8a5@=+P$)gNXGM3K+=O0k(vAD6ChRvcQ7yg#(Y!E)>z_D4$LFUG0pNzV_MIE zv@v?SB(00aaMZD2YM(~Y5SrrlAd?ea7tvw%INU+Nx~wJ!HDMTGGbJp=LMMt)Hwrl1 z*<8cOU(ooHtj!eUx%YA~4{AzEId(1~jyD~zPxWA?KPei@##O8StIhTUca)lz3lU?9r8${=gwsrCGm@l41q@P+nQSe4$jr zYxzo!`L&Qo7|y_h9gumq6FkS1PPeTj?fKJnl47DPviCm_P~+z1DN|G0+PT&_~qdBPZ4lyMJXlvS@m9&Tx2)au&gMcCohz?2rru0 zFWh6;u{!%qv*$PT%`yT;8uez)@>jWlnZ`hW3j7;_*vlGsG3+4s&y=7}I4O*Gk^Hsa zWKR2)MNr~09ohW3eRd2_7H0-c#EW(?tqeJQ9!#T_HRrd>zIR`OMY*xb_~fdr*F>nW zkX`3Zldx0pp*+SEQT(Q8UpkjuE&Hp`^Svb`dfqVTqs~%pUoE0})#vxi0y_?@-@^nI<7&;PzwqAIa-oYn=osunU%2b(9H)n${W4Ks!R{Qy^ zL8Y_IB=uLlhm3r-s;WGjpu$Gb#h?e1Lt$giBG<(Fxxah4Os09~*kou#-)+ub&4>03 zo{+$@vdYrno8^HLmiL48Oab10hX$kP`~FG_?lbEQ-c)jIRv5wg$&fR9C%z_YHGZu? zon^Xa&hxZcL$j+^s+vCOrn(S*Y`}bjNi)-Ctz$8%DNFm^)6(KEVmm{{OvB6J@dlEJ zDW##N(yF#_(G*)4f1WxHpg!&BFOx(`%0faTcK2AsMSU|p6LF zbFpip8u(D;H<KY@rEEJd=hPN+yEF}719R!WE#7D zJBwg9u{Ex6IAt`-i0Y?qC3E_vI3J=d^AH-QU&nV~G4R&AS?Z{}`3kkNv*6@&;)7x> ztdI=>qx{utBuAeG6zIx@gltgJr0e?52m|F@4V@qHt{acuvi^?D82MCqw%*ZA4u{y- zE$TRr3*8F7WmaM8bK#z?4CKHSh`O*yJR8 z@@gX!4(P%GpW&0g-~%OXa5D2C6J(~^kZvhAF*&52dD+4sWElI}gq^a;^Lj?~W%h-V@{zM4IS z)YE8}DZg#o?P}JhEu)#DEGIbLz~r=-Vp~x}S+D{1a*d}|rsyyA&IgjvXkQ3*Y-wha zE~9i=a8aCQ0S>Uz3~`>ZU2LyyyIJ%C)z9MF4sJi-geG`>jJaL4xmcz9SSfcu<+VaE z@H<{&5p2f+1V-4SmLz(e^um2R^O=_}(HD5SRV7j(kp*LZ;tq zVjjxs2Q<^|568vT-eoVZj+bpk)i|#xHh?Q|EDf33(N1AxaKZtCFu#^mMWhdG=&^)Wp;Zt-STrt0`Qq$di zAg0>Uj;G+iuOY?Tgy@KZU9rVV@M8h>8f8C_e>z*t&F!B~xqES$S!dp0K%EXwuIZ zLP-i4Ix^pT)Hr_W`zzM|OhiIx8+7+l6AmciKg&zQ+<*!%;ef|DK-a=F#x)&lk0+KF z0XV?o9S)G{q&nmFrf~FoC-?)bdaV2EFTDgrab9%r_+=w=`}01Tjot(Wb1YB^SKSw$ z5;s17M>B;jYV_|q5}YO*FM{0Bw!jZ}!h5)c1z0%p}Y+C4)Qp;TKeM z!6!>7$n;O}9jK0JFf>hUI5=mECzD&6eQ!LRBhE!r*Fx)wBlfy{d|RsEh+EkElG+q~ z|BKb7COLbaaoH4cfi7D!WX6epl$7Hqd#jgePphIW<%pjMS@3*!+?@Tb1pbLkAR^~Q z^*jj8X#R>%HixabX~Iulae&8m?SJ(0?A>qP7etZp;tG1tW*qzJj znPgbyr0(kdi-ZOJG%3ds#oTPS@wdNg3776v1?SL+Dq9f}bp0apykz_r_xe4Wz{0SX zSl8Ft>yu0q3FQ4b1D-wCp11wL0h9-emahhcU1R#rMM7MA+_^d_^k>u*mSb=5Om~bT z9~mb5Mh~xnNgCPAvGGP#^$h{2k(&;En?lP}XSeF^R@>?&{Os(1al9nMdR^mF{oK6o zlMG7dQJag39dLZiU-9tw3X7b{#~lvSNOHvlPzLq&W1(Vkk(6AMIstOsEh)76gTF$H48RD^ zd{;7)UkGpbzPENnDzGExr+JR3+nwH>sB(LF2)R?7CM@f7_;{Qmw7rh})vKzKZgM{| z6MXS4y4oS0KKCrG=12jC`Km=6=QnIYsTm`~BOr=x9^Vv32wvK@f z$Q*64MS=Qp#WGaOt3u6ck*0_N^i1evlESq1{XtA{ZrLn8sFl2QA5YWgmD##hf$NXs zq2}W&vHJQfljq9#86V zcaRMm+*x*i%$n0O2-ulJFtqpXX_p%F5<0sm_2RKXY=tYznJR<1p(9~%2t;91>Bl;> zxnFosh_$f%={1n!!Jf#c`>tLbD zuhzcMQ#z9%R}^nDteG0x7j(|s*>BMjeZyDNT=Mfjo?!uX=0EjB$G17(UFKsU$S6NMrGuiE&l(uL=~xGcW{!iN;C&(Ex(NDq zF4(ankda;b*q-BgwMC-t#UyspgPEBJvN2A>HG6!$Bb5MiJ!tzUtGxz7qWSoRcI`d3 z8dfON;o39qaq|sf+l<7*Hhj?{$__$NnB1>3Nx5}_8nId(uA!sJVa!*dT&eRSKTH~b zY0e9$n~B`;3-PLc3o`XAzE^2=TSCdc~nD7k;=)S}Jb z*p-~gJ$fL^_ex5XpzK&5wZzc#64_vG6$P_^9XJST{lh{B!HcBpj#W^uj6-P99q|k5Jp53F@|d5OS>8%ou4{T@*7XupTvZUaE$zw}w1~TLKxHj> ztN3d7lt~X1vtT};U*4$b4no*Zia@f?Ba&a6bHvsNRd`JoVMMci<<0N^+p|g?_)g7> zVHVdcv)p^a3lqd-taHEg^D-R!z=~|n6vRMV^PnrM;Gs!~AXa#v#S{8)W2yg-zEYW% zGh=se>&kh#;k7EgcZ7t;qv|fgl`f z5K(zr=ECAJtHNJ0@76)iX5~U!`RTLA1(WQ>Np-!CkE8>R!VZh#+vxwP{C@(h!T%)} zm_dsrGlfRE`AC;jn+&>y;q?#ATV;iuOb$h@CdgN@fqsPFa6mXk)%l)<^uf$rpM6iz z1#OZ)4p=30zyYw~-#%mc0$ot?0$upun>*?Un-!il_e8<3pOK&wRzd|AhWD%a#fZX= zX}C@cV|dmU^EyqteMa8=xPdnWy;0jWO5zr)Yc zDMQK1sCgseHr;JJ4o_;q0rahlnpf6N+7up^-k`a0K}!4+oC2%x)<{sIcT5=`pJ4xu z!Y*5U*Xc_UZ#}B>GD5JN--nJgyv#ivF;{ej=oZ`bo$wwCjnNCzZx{$n?Y%r}Ke{;v zzh&Rk&%Ui_ZuRYCw_aZBpXx4Ce25q`=v*KwVROFS9m8?5qXU<-FC$>HTmASF#%!Vd z%-e8t>6ugG-Bt1`ZsxkCuT+&#Q$sc>gS1;2HAhRZ>97CPIR?Mm=+sb@i7rt^_II}o~G0adBi5p43*;=xv86hK5 z;ANrZBkR&(Z}fiNiA8*9UufWffMH?dL&tn7`2H^PLxA)71Pj%1#IfDj56SArknSI>SFG>;VLJh9 zAiD_56-H44j9{C(-HI8;FU#GX9Wsp&y^PMwVE`wYm%=z+O-a2{9q4W{ zaMcfAL-;X|7rCiBj$HJ}vkY~SuvhQr=ZKSXvwva?3Aq)Ot8OOh+iUeINvl)l1viIX z@4jSlmd5%DO8>?J=k?)4?zg|=#((7z5(Pdcu)qP6^cVVC*tg)1E;yh7o?Bd`BsmNd z!vT0cAcq5r@su{jGVFr3fv5_qoU|RA1S40ngrFVE+of5gA z#bmtswuuEoCN}N;r0zHS(HloQ)8d`-3K-Ew)}|zS@7R;%RC?{fbrJ03$B$aeq2(s& z^C#fvhHvcf6C{4?H#WmKenDq85LvT-tTRt}?tvVR_E_3mDyZ*93}K%&Mz!r+T*kif zazgnowxC!(4mrh-{a}Ih`G6~ z^nIl|0cPYA!w~#Dsex_@S^~xIhNIgRqy_xNHxhU0esFNjmqsme_=#!o^G=bE-}rOA zT~j5{jk z5#uWS0hH|Ik*t+5nR0u+$os-JU*A(Edywk_zlGYnD&M$Y$~Bt|1hM_S zf$Uo(ttClV9`R?!;tbWEPSi;J`(&h!-SZHoLV%Gi-T!K=_Ma{J-G&^I(wEPK>|(2X!ka_+eh0I$PwUC6}?1kU{5J4|Y9bLdCjJD3m)bQ`-uu6L-X~+@?E$-0xRi?pPz!bh;Tm7BfFNrl_hS`b0#t4$*S*b9=f+^K#^^yPNmBayT*z zzMWsg>Y}D#FXp4A^~0M#WOq->6a)n!i-|GCiETP=c@m#;+}sbsTKq-M6tWcxZ46Id z&NFO(wu$k1{t7PjMWsVk8PwlIy{2p9eUX-Jn!!<7Ci-05l(6~itNr>~zXp$5ZqU$R z_zX9-@>G7>)l83D1xj;Bf_hitXc&df7G9&anf?;EEVK7k0{Q?C%==KQQu<7*{ps7W z%0)VVMlKnmGR+~&w(9!iNP)r7O-~OAfXq2EzLapU6@`qn!W2ilQ#?H)$@Kl2cEQm3 z1DxQi$P`p(G!*{mJ=KKL>QzmEu7wO?^1t3NQ_bhz@| zhwjz;r`S1DI0e>FXW~|+xTfv(54VV!Yrq$^Bkv`bGiKGNuuYv%HW?F?lQ6Rna`XG0 z#hxk&4sV*D4eR**yaVqt&U-y`Wh+t0RtuV!ISn8CD~?V?H~Wzdqj=O!uF0&F8mabK zVH)Y3do}GnJ~G)GU<-d2aAJIgsSLoMCU}$dkc@c7ug{b1)RV=prr4gRiLc?&GHlVj zbdRhhMbj{qpJcujT-kUlbrHV-sGu&Nuv)rNCGGX(|_Ln*BMAC_B zlFl;A*B)YhSJR*F`l`Dac07o$+&sBj94aKSx_2hDJs_fv#e4fm+MR+&d4V$|dROkK z$ZJ{kBxAa;S)bNPBWrpS!Zs<21FqS2`(&ANpv9fXzz?LE_PNKR#!-W@?F+n5?WEij zK4v#SnaVzJ5yY#p2+5R5W!jL72z?`*2-%cq@6N1}iEW3nxcLgNt2yvY@BEP1lYc65 zZacnT@E0C~CjwSiEf=k$Mvcyrshd=gv+`hM;ij ze{l5wHk^)=jDB z1IczvOq7(+Neq*NR+u|MB-qx);mL~6WCFx$+t z&+yXM;551eJv*MaI*JrVAHw=#ir(aNcd?fgKJ?8wm4+ErU?7oBX&W>J-s5?jIxB46 zF>ibhi;*1*tdg!u*7l61M?8Cfbe5}4)b=x;p`srTSk0ICx6)1HI*Y#^<-fTt>C(nr za*P8o_8mIdm5wSzS>RWAewF5ShZI~G&U9ib2C`It*rKtyX#ZJh8eVeo<^=A)BPRJa z-(of;*?p`WdEzFmAM+Pq@Ol-$6$3o{D{Hf1h$czP^uxh{H| zvkp)330+Db!lRQBwj0%>GUE0*#@%IGo4o>3TMUbs<@&Rhj3`<|-HmGbD@;>42KMt7 z;d^vmIqL*+;71n&#%D93!A@@9Kay8OKI1zeQSdSHRpOFp3|*Fa8W_#;$gUZtW}!Rw zZz-*T3#N?S`)`S9l1*Wa*Q+q0Z^S6reu3r`VU_ILP)pq^yWh!k}LNwP=m`ana2Zy-Xk9T&u3 z#|Z^zJl<4mp9xb|>|zlD2}(3>+c6_w8MLlYz?I7<-FyngwZp9{Y97R7x)bG5JKL$f zf?UQnp|?*Lj>3LbB$%s1bkr+4B&_x!2m95gkL(rCA3ZO4{OBP0kD1Gzr~T;sElj8hu}lMUcCIB#y!-{|pu)8j4$QOyN@WMkZa7!yVYj!^{+v%|0uurR#{% zcZoAzFDrOo+p?EMjLd5ki0Q}i$<}^6m!xt)3+IqWv1$w(Z9;mOLs?Sc8#g}yoq_|R z&eUx)*M>zHSzZ?rQ{}k8$$A=WMuOs*3}39mgZJ&>ATPgGl~S#*#%)y2b7L2;i<6RP zfBic7l%ewv`}?zFOU{KEi)>tg#`ag`M0%;HUSw}N8ySYmiqlQ)3)kzkpw=c5yEj&0 zhOZsOQpPHegvwnN+sz(739`61s?m2|l%hNqk!&`4Gt+1TT2>Y6D0udit;iAwJp5`P zJ@}wqb@8C+>0n|(Tn$#bsEqlvE~mtE}_D!zD^{T8S=Di>T6vY+eX z+VAa``;0cef;P-2EYv$FHFJ+c87wVLXqdF%WRqhDg4ghSf@C=0qPl9gSQShSkvr>6 zI=4Jm_jQpRh+HT>i#;Je_ftCCU{OM6t%}v0JZy6F1n9xv|>CEl@4`0V2`rQk+S%R>;P^LXs zCKmJxo?{)99Hw~PNyCHbC)mLjgkj{qQTY$+A660fcPk;!Wy55nZB+|$dl9$nR@$5B zAmsKx$!id(3C^YwVbwvVU!U>#1%Ei9^FdzO@JEUA*J_tC14-CRick;!U|5t%&E_PN zFiXG68UFWifpWIWEu3Q$V)iX@^&duFobE{I%bQ3Kr@c`sb66_3)tdz}=kgozvOcXRcaWcLyREWJ zZa37z$zpQGP7wPyHSQWjK;-En^CP~j8zf9;{sjRa#w!l@1T4Oi=e-)pWw#iWY&NkH z?}eQA2+cE7anA0)lR7w<#BfDqf6J6k7_0JA3TzUHI@4$PqHj9)VU7ZJb=d9umu)fL zaHk&ErD_6^{Tem!YK%X4De7>kGF+?3S41x*r5obtG;PrR0f|)+ki?%#)Pi<2Xl|+C0B)ABA@s`T zpl$j_B~u(W@U`)T)%%-oD5hkk#x#YKQHu`E_rVV-0lMYHzf=l>&_xT18Lz5V@tJ#{ zp{_2UJS}fix{#b)<5ksXmCO|gv|9;)xAGXlhq|3{fFCh-GUGfSy2Sa8dd9Ce@i0ki z_CCtzJ%W=m>ITa-7m%^0XuN^*{VreToYvbF(e*T{)`=1=bt6hs^FI_HG@K4{XF&zZfGNso}BQ&@7D5Rz6TO@p5>o?2xXUt{tq$02s16Y-|6= abJ~~wQEL9v2KK)&;NM*D{|^Ch)Bgj_b*Bpe literal 0 HcmV?d00001 diff --git a/src/hal/components/cros/resources/cros_testbed/.gitignore b/src/hal/components/cros/resources/cros_testbed/.gitignore new file mode 100755 index 00000000..b25c15b8 --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/.gitignore @@ -0,0 +1 @@ +*~ diff --git a/src/hal/components/cros/resources/cros_testbed/BUILD b/src/hal/components/cros/resources/cros_testbed/BUILD new file mode 100755 index 00000000..6def7d22 --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/BUILD @@ -0,0 +1,17 @@ +BUILD +===== + +Create a catkin environment following the guide at: + + http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment + +Assume the catkin workspace is located at "~/catkin_ws", link +the current folder in the catkin source folder, for example: + + $ ln -s ~/itr-cros/cros_testbed ~/catkin_ws/src + +To compile and install do the following: + + $ cd ~/catkin_ws + $ catkin_make + $ catkin_make install diff --git a/src/hal/components/cros/resources/cros_testbed/CMakeLists.txt b/src/hal/components/cros/resources/cros_testbed/CMakeLists.txt new file mode 100755 index 00000000..eb2b3001 --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/CMakeLists.txt @@ -0,0 +1,160 @@ +cmake_minimum_required(VERSION 2.8.3) +project(cros_testbed) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate messages in the 'msg' folder +add_message_files( + FILES + DoubleVector.msg +) + +## Generate services in the 'srv' folder +add_service_files( + FILES + add_two_ints.srv + ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) +generate_messages() + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include +# LIBRARIES cros_testbed +# CATKIN_DEPENDS roscpp rospy std_msgs message_runtime +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +add_executable(clock_listener src/clock_listener.cpp) +target_link_libraries(clock_listener ${catkin_LIBRARIES}) + +add_executable(counter_listener src/counter_listener.cpp) +target_link_libraries(counter_listener ${catkin_LIBRARIES}) + +add_executable(vector_listener src/vector_listener.cpp) +target_link_libraries(vector_listener ${catkin_LIBRARIES}) + +add_executable(vector_talker src/vector_talker.cpp) +target_link_libraries(vector_talker ${catkin_LIBRARIES}) + +add_executable(add_two_ints src/add_two_ints.cpp) +target_link_libraries(add_two_ints ${catkin_LIBRARIES}) + +add_executable(test_services_list src/test_services.cpp) +target_link_libraries(test_services_list ${catkin_LIBRARIES}) + +add_executable(std_msgs_talker src/std_msgs_talker.cpp) +target_link_libraries(std_msgs_talker ${catkin_LIBRARIES}) + +add_executable(std_msgs_listener src/std_msgs_listener.cpp) +target_link_libraries(std_msgs_listener ${catkin_LIBRARIES}) + +## Declare a cpp library +# add_library(cros_testbed +# src/${PROJECT_NAME}/cros_testbed.cpp +# ) + +## Declare a cpp executable +# add_executable(cros_testbed_node src/cros_testbed_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +add_dependencies(clock_listener cros_testbed_generate_messages_cpp) +add_dependencies(counter_listener cros_testbed_generate_messages_cpp) +add_dependencies(vector_listener cros_testbed_generate_messages_cpp) +add_dependencies(vector_talker cros_testbed_generate_messages_cpp) +add_dependencies(add_two_ints cros_testbed_generate_messages_cpp) +add_dependencies(test_services_list cros_testbed_generate_messages_cpp) +add_dependencies(std_msgs_talker cros_testbed_generate_messages_cpp) +add_dependencies(std_msgs_listener cros_testbed_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(cros_testbed_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS cros_testbed cros_testbed_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_cros_testbed.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/hal/components/cros/resources/cros_testbed/include/.keep b/src/hal/components/cros/resources/cros_testbed/include/.keep new file mode 100755 index 00000000..e69de29b diff --git a/src/hal/components/cros/resources/cros_testbed/msg/DoubleVector.msg b/src/hal/components/cros/resources/cros_testbed/msg/DoubleVector.msg new file mode 100755 index 00000000..7353eb1f --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/msg/DoubleVector.msg @@ -0,0 +1 @@ +float64[] val diff --git a/src/hal/components/cros/resources/cros_testbed/package.xml b/src/hal/components/cros/resources/cros_testbed/package.xml new file mode 100755 index 00000000..cfddf4bb --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/package.xml @@ -0,0 +1,60 @@ + + + cros_testbed + 0.0.0 + The cros_testbed package + + + + + albe + + + + + + TODO + + + + + + + + + + + + + + + + + + + message_generation + + + + message_runtime + message_runtime + + + catkin + rospy + roscpp + std_msgs + rospy + roscpp + std_msgs + + + + + + + + + + + diff --git a/src/hal/components/cros/resources/cros_testbed/src/add_two_ints.cpp b/src/hal/components/cros/resources/cros_testbed/src/add_two_ints.cpp new file mode 100755 index 00000000..d4ef647b --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/src/add_two_ints.cpp @@ -0,0 +1,23 @@ +#include "ros/ros.h" +#include "cros_testbed/add_two_ints.h" + +bool add(cros_testbed::add_two_ints::Request &req, + cros_testbed::add_two_ints::Response &res) +{ + res.sum = req.a + req.b; + ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b); + ROS_INFO("sending back response: [%ld]", (long int)res.sum); + return true; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "add_two_ints_server"); + ros::NodeHandle n; + + ros::ServiceServer service = n.advertiseService("add_two_ints", add); + ROS_INFO("Ready to add two ints."); + ros::spin(); + + return 0; +} diff --git a/src/hal/components/cros/resources/cros_testbed/src/clock_listener.cpp b/src/hal/components/cros/resources/cros_testbed/src/clock_listener.cpp new file mode 100755 index 00000000..62134d35 --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/src/clock_listener.cpp @@ -0,0 +1,58 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" + +/** + * This tutorial demonstrates simple receipt of messages over the ROS system. + */ +void chatterCallback(const std_msgs::String::ConstPtr& msg) +{ + ROS_INFO("I heard: [%s]", msg->data.c_str()); +} + +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. For programmatic + * remappings you can use a different version of init() which takes remappings + * directly, but for most command-line programs, passing argc and argv is the easiest + * way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "clock_listener"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called chatterCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + ros::Subscriber sub = n.subscribe("topic_clock", 1000, chatterCallback); + + /** + * ros::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). ros::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/src/hal/components/cros/resources/cros_testbed/src/counter_listener.cpp b/src/hal/components/cros/resources/cros_testbed/src/counter_listener.cpp new file mode 100755 index 00000000..0db818ab --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/src/counter_listener.cpp @@ -0,0 +1,58 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" + +/** + * This tutorial demonstrates simple receipt of messages over the ROS system. + */ +void chatterCallback(const std_msgs::String::ConstPtr& msg) +{ + ROS_INFO("I heard: [%s]", msg->data.c_str()); +} + +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. For programmatic + * remappings you can use a different version of init() which takes remappings + * directly, but for most command-line programs, passing argc and argv is the easiest + * way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "counter_listener"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The subscribe() call is how you tell ROS that you want to receive messages + * on a given topic. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. Messages are passed to a callback function, here + * called chatterCallback. subscribe() returns a Subscriber object that you + * must hold on to until you want to unsubscribe. When all copies of the Subscriber + * object go out of scope, this callback will automatically be unsubscribed from + * this topic. + * + * The second parameter to the subscribe() function is the size of the message + * queue. If messages are arriving faster than they are being processed, this + * is the number of messages that will be buffered up before beginning to throw + * away the oldest ones. + */ + ros::Subscriber sub = n.subscribe("topic_counter", 1000, chatterCallback); + + /** + * ros::spin() will enter a loop, pumping callbacks. With this version, all + * callbacks will be called from within this thread (the main one). ros::spin() + * will exit when Ctrl-C is pressed, or the node is shutdown by the master. + */ + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/src/hal/components/cros/resources/cros_testbed/src/std_msgs_listener.cpp b/src/hal/components/cros/resources/cros_testbed/src/std_msgs_listener.cpp new file mode 100755 index 00000000..43ee8cbc --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/src/std_msgs_listener.cpp @@ -0,0 +1,125 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void chatter1(const std_msgs::Bool::ConstPtr& msg) +{ + ROS_INFO("I heard: [%s]", msg->data ? "true" : "false"); +} + +void chatter2(const std_msgs::Byte::ConstPtr& msg) +{ + ROS_INFO("I heard: [%i]", msg->data); +} + +void chatter3(const std_msgs::Char::ConstPtr& msg) +{ + ROS_INFO("I heard: [%i]", msg->data); +} + +void chatter4(const std_msgs::Duration::ConstPtr& msg) +{ + ROS_INFO("I heard: sec=[%i] nsec=[%i]", msg->data.sec, msg->data.nsec); +} + + +void chatter5(const std_msgs::Header::ConstPtr& msg) +{ + ROS_INFO("I heard: seq = [%i], time = [%i, %i], frame_id = [%s]", msg->seq, msg->stamp.sec, msg->stamp.nsec, msg->frame_id.c_str()); +} + +void chatter6(const std_msgs::Int16::ConstPtr& msg) +{ + ROS_INFO("I heard: [%i]", msg->data); +} + +void chatter7(const std_msgs::Int32::ConstPtr& msg) +{ + ROS_INFO("I heard: [%i]", msg->data); +} + +void chatter8(const std_msgs::Int64::ConstPtr& msg) +{ + ROS_INFO("I heard: [%lli]", msg->data); +} + +void chatter9(const std_msgs::Int8::ConstPtr& msg) +{ + ROS_INFO("I heard: [%i]", msg->data); +} + +void chatter10(const std_msgs::Time::ConstPtr& msg) +{ + ROS_INFO("I heard: sec=[%i] nsec=[%i]", msg->data.sec, msg->data.nsec); +} + +void chatter11(const std_msgs::UInt16::ConstPtr& msg) +{ + ROS_INFO("I heard: [%i]", msg->data); +} + +void chatter12(const std_msgs::UInt32::ConstPtr& msg) +{ + ROS_INFO("I heard: [%i]", msg->data); +} + +void chatter13(const std_msgs::UInt64::ConstPtr& msg) +{ + ROS_INFO("I heard: [%lli]", msg->data); +} + +void chatter14(const std_msgs::UInt8::ConstPtr& msg) +{ + ROS_INFO("I heard: [%i]", msg->data); +} + +void chatter15(const std_msgs::Float32::ConstPtr& msg) +{ + ROS_INFO("I heard: [%f]", msg->data); +} + +void chatter16(const std_msgs::Float64::ConstPtr& msg) +{ + ROS_INFO("I heard: [%f]", msg->data); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "std_msgs_listener"); + + ros::NodeHandle n; + ros::Subscriber sub1 = n.subscribe("bool", 1000, chatter1); + ros::Subscriber sub2 = n.subscribe("byte", 1000, chatter2); + ros::Subscriber sub3 = n.subscribe("char", 1000, chatter3); + ros::Subscriber sub4 = n.subscribe("duration", 1000, chatter4); + ros::Subscriber sub5 = n.subscribe("header", 1000, chatter5); + ros::Subscriber sub6 = n.subscribe("/int16", 1000, chatter6); + ros::Subscriber sub7 = n.subscribe("/int32", 1000, chatter7); + ros::Subscriber sub8 = n.subscribe("/int64", 1000, chatter8); + ros::Subscriber sub9 = n.subscribe("/int8", 1000, chatter9); + ros::Subscriber sub10 = n.subscribe("/time", 1000, chatter10); + ros::Subscriber sub11 = n.subscribe("/uint16", 1000, chatter11); + ros::Subscriber sub12 = n.subscribe("/uint32", 1000, chatter12); + ros::Subscriber sub13 = n.subscribe("/uint64", 1000, chatter13); + ros::Subscriber sub14 = n.subscribe("/uint8", 1000, chatter14); + ros::Subscriber sub15 = n.subscribe("/float32", 1000, chatter15); + ros::Subscriber sub16 = n.subscribe("/float64", 1000, chatter16); + + ros::spin(); + + return 0; +} diff --git a/src/hal/components/cros/resources/cros_testbed/src/std_msgs_talker.cpp b/src/hal/components/cros/resources/cros_testbed/src/std_msgs_talker.cpp new file mode 100755 index 00000000..5e4bdd58 --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/src/std_msgs_talker.cpp @@ -0,0 +1,125 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "std_msgs_talker"); + ros::NodeHandle n; + ros::Publisher chatter1 = n.advertise("bool", 1000); + ros::Publisher chatter2 = n.advertise("byte", 1000); + ros::Publisher chatter3 = n.advertise("char", 1000); + ros::Publisher chatter4 = n.advertise("duration", 1000); + ros::Publisher chatter5 = n.advertise("header", 1000); + ros::Publisher chatter6 = n.advertise("int16", 1000); + ros::Publisher chatter7 = n.advertise("int32", 1000); + ros::Publisher chatter8 = n.advertise("int64", 1000); + ros::Publisher chatter9 = n.advertise("int8", 1000); + ros::Publisher chatter10 = n.advertise("time", 1000); + ros::Publisher chatter11 = n.advertise("uint16", 1000); + ros::Publisher chatter12 = n.advertise("uint32", 1000); + ros::Publisher chatter13 = n.advertise("uint64", 1000); + ros::Publisher chatter14 = n.advertise("uint8", 1000); + ros::Publisher chatter15 = n.advertise("float32", 1000); + ros::Publisher chatter16 = n.advertise("float64", 1000); + ros::Rate loop_rate(1); + std::cout<getServerURI()< + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "test_services_list"); + + /*if (argc != 3) + { + ROS_INFO("usage: add_two_ints_client X Y"); + return 1; + }*/ + + ros::NodeHandle n; + ros::ServiceClient client; + + //Test add_two_ints service + client = n.serviceClient("add_two_ints"); + + cros_testbed::add_two_ints srv_add_two_ints; + srv_add_two_ints.request.a = 3; + srv_add_two_ints.request.b = 5; + + if (client.call(srv_add_two_ints)) + { + ROS_INFO("Sum: %ld", (long int)srv_add_two_ints.response.sum); + } + else + { + ROS_ERROR("Failed to call service add_two_ints"); + return 1; + } + + return 0; +} diff --git a/src/hal/components/cros/resources/cros_testbed/src/vector_listener.cpp b/src/hal/components/cros/resources/cros_testbed/src/vector_listener.cpp new file mode 100755 index 00000000..7603a0eb --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/src/vector_listener.cpp @@ -0,0 +1,20 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "cros_testbed/DoubleVector.h" + + +void chatterCallback(const cros_testbed::DoubleVector::ConstPtr& msg) +{ + for(int i = 0; i < msg->val.size(); i++) + ROS_INFO("I heard: [%f]", msg->val[i]); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "vector_listener"); + ros::NodeHandle n; + ros::Subscriber sub = n.subscribe("double_vector", 1000, chatterCallback); + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/src/hal/components/cros/resources/cros_testbed/src/vector_talker.cpp b/src/hal/components/cros/resources/cros_testbed/src/vector_talker.cpp new file mode 100755 index 00000000..be00b75b --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/src/vector_talker.cpp @@ -0,0 +1,87 @@ +#include "ros/ros.h" +#include "std_msgs/Float64.h" +#include "ros/xmlrpc_manager.h" +#include "cros_testbed/DoubleVector.h" + +#include + +/** + * This tutorial demonstrates simple sending of messages over the ROS system. + */ +int main(int argc, char **argv) +{ + /** + * The ros::init() function needs to see argc and argv so that it can perform + * any ROS arguments and name remapping that were provided at the command line. For programmatic + * remappings you can use a different version of init() which takes remappings + * directly, but for most command-line programs, passing argc and argv is the easiest + * way to do it. The third argument to init() is the name of the node. + * + * You must call one of the versions of ros::init() before using any other + * part of the ROS system. + */ + ros::init(argc, argv, "vector_talker"); + + /** + * NodeHandle is the main access point to communications with the ROS system. + * The first NodeHandle constructed will fully initialize this node, and the last + * NodeHandle destructed will close down the node. + */ + ros::NodeHandle n; + + /** + * The advertise() function is how you tell ROS that you want to + * publish on a given topic name. This invokes a call to the ROS + * master node, which keeps a registry of who is publishing and who + * is subscribing. After this advertise() call is made, the master + * node will notify anyone who is trying to subscribe to this topic name, + * and they will in turn negotiate a peer-to-peer connection with this + * node. advertise() returns a Publisher object which allows you to + * publish messages on that topic through a call to publish(). Once + * all copies of the returned Publisher object are destroyed, the topic + * will be automatically unadvertised. + * + * The second parameter to advertise() is the size of the message queue + * used for publishing messages. If messages are published more quickly + * than we can send them, the number here specifies how many messages to + * buffer up before throwing some away. + */ + ros::Publisher chatter_pub = n.advertise("double_vector", 1000); + + ros::Rate loop_rate(1); + std::cout<getServerURI()<() call, as was done + * in the constructor above. + */ + chatter_pub.publish(msg); + + ros::spinOnce(); + + loop_rate.sleep(); + ++count; + } + + + return 0; +} \ No newline at end of file diff --git a/src/hal/components/cros/resources/cros_testbed/srv/add_two_ints.srv b/src/hal/components/cros/resources/cros_testbed/srv/add_two_ints.srv new file mode 100755 index 00000000..43158244 --- /dev/null +++ b/src/hal/components/cros/resources/cros_testbed/srv/add_two_ints.srv @@ -0,0 +1,4 @@ +int32 a +int32 b +--- +int32 sum diff --git a/src/hal/components/cros/resources/rosgraph_msgs/Log.msg b/src/hal/components/cros/resources/rosgraph_msgs/Log.msg new file mode 100644 index 00000000..9a9597c2 --- /dev/null +++ b/src/hal/components/cros/resources/rosgraph_msgs/Log.msg @@ -0,0 +1,19 @@ +## +## Severity level constants +## +byte DEBUG=1 #debug level +byte INFO=2 #general level +byte WARN=4 #warning level +byte ERROR=8 #error level +byte FATAL=16 #fatal/critical level +## +## Fields +## +Header header +byte level +string name # name of the node +string msg # message +string file # file the message came from +string function # function the message came from +uint32 line # line the message came from +string[] topics # topic names that the node publishes diff --git a/src/hal/components/cros/samples/CMakeLists.txt b/src/hal/components/cros/samples/CMakeLists.txt new file mode 100644 index 00000000..e2e710d7 --- /dev/null +++ b/src/hal/components/cros/samples/CMakeLists.txt @@ -0,0 +1,19 @@ +file(COPY rosdb DESTINATION ${EXECUTABLE_OUTPUT_PATH}) + +add_executable(parameters-test parameters-test.c) +target_link_libraries(parameters-test cros) + +add_executable(api-test api-test.c) +target_link_libraries(api-test cros) + +add_executable(ros-i-trajectory-test ros-i-trajectory-test.c) +target_link_libraries(ros-i-trajectory-test cros) + +add_executable(talker talker.c) +target_link_libraries(talker cros) + +add_executable(listener listener.c) +target_link_libraries(listener cros) + +add_executable(performance-test performance-test.cpp) +target_link_libraries(performance-test cros m) diff --git a/src/hal/components/cros/samples/api-test.c b/src/hal/components/cros/samples/api-test.c new file mode 100644 index 00000000..bfa0fb7d --- /dev/null +++ b/src/hal/components/cros/samples/api-test.c @@ -0,0 +1,402 @@ +#include +#include +#include +#include +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN +# include +# include + +# define DIR_SEPARATOR_STR "\\" +#else +# include +# include +# include + +# define DIR_SEPARATOR_STR "/" +#endif + +#include "cros.h" + +// TODO Signal handler + +static void printHelp( char *cmd_name ) +{ + printf("Usage: %s [OPTION] ... \n", cmd_name); + printf("Options:\n"); + printf("\t-name Set the node name (default: /test_node)\n"); + printf("\t-host Set the node host (default: 127.0.0.1)\n"); + printf("\t-chost Set the roscore host (default: 127.0.0.1)\n"); + printf("\t-cport Set the roscore port (default: 11311)\n"); + printf("\t-sub run as a subscriber (default mode)\n"); + printf("\t-pub run as a publisher\n"); + printf("\t-h Print this help\n"); +} + +CrosNode *node; + +static CallbackResponse gripperstatus_sub_callback(cRosMessage *message, void* data_context) +{ + CallbackResponse cb_resp; + cRosMessageField *PowerStatus_field = cRosMessageGetField(message, "PowerStatus"); + cRosMessageField *ClampStatus_field = cRosMessageGetField(message, "ClampStatus"); + cRosMessageField *AlarmCodes_field = cRosMessageGetField(message, "AlarmCodes"); + cRosMessageField *Configuration_field = cRosMessageGetField(message, "Configuration"); + + uint32_t PowerStatus = PowerStatus_field->data.as_uint32; + uint32_t NAlarmCodes = AlarmCodes_field->array_size; + uint32_t Configuration = Configuration_field->data.as_uint32; + uint32_t ClampStatus[4]; + ClampStatus[0] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 0); + ClampStatus[1] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 1); + ClampStatus[2] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 2); + ClampStatus[3] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 3); + + uint32_t *AlarmCodes = (uint32_t *)malloc(NAlarmCodes*sizeof(uint32_t)); + if (AlarmCodes != NULL) + { + uint32_t it; + for (it = 0; it < NAlarmCodes; it++) + { + AlarmCodes[it] = *cRosMessageFieldArrayAtUInt32(AlarmCodes_field, it); + } + + printf("GripperStatus: PowerStatus: %d\n", PowerStatus); + printf("GripperStatus: ClampStatus: [%d %d %d %d]\n", ClampStatus[0], ClampStatus[1], + ClampStatus[2], ClampStatus[3]); + printf("GripperStatus: AlarmCodes: ["); + int first = 1; + + for (it = 0; it < NAlarmCodes; it++) + { + if (first) + first = 0; + else + printf(" "); + + printf("%d", AlarmCodes[it]); + } + printf("]\n"); + printf("GripperStatus: Configuration: %d\n", Configuration); + fflush(stdout); + + free(AlarmCodes); + cb_resp = 0; //success + } + else + { + ROS_ERROR(node, "Error allocating memory for AlarmCodes\n"); + cb_resp = 1; //fail + } + + return(cb_resp); +} + +static CallbackResponse gripperjoints_sub_callback(cRosMessage *message, void* context) +{ + cRosMessageField *Position_field = cRosMessageGetField(message, "Position"); + double Position[9]; + Position[0] = *cRosMessageFieldArrayAtFloat64(Position_field, 0); + Position[1] = *cRosMessageFieldArrayAtFloat64(Position_field, 1); + Position[2] = *cRosMessageFieldArrayAtFloat64(Position_field, 2); + Position[3] = *cRosMessageFieldArrayAtFloat64(Position_field, 3); + Position[4] = *cRosMessageFieldArrayAtFloat64(Position_field, 4); + Position[5] = *cRosMessageFieldArrayAtFloat64(Position_field, 5); + Position[6] = *cRosMessageFieldArrayAtFloat64(Position_field, 6); + Position[7] = *cRosMessageFieldArrayAtFloat64(Position_field, 7); + Position[8] = *cRosMessageFieldArrayAtFloat64(Position_field, 8); + + printf("GripperJoints: Position: [%f %f %f %f %f %f %f %f %f]\n", Position[0], Position[1], Position[2], + Position[3], Position[4], Position[5], Position[6], Position[7], Position[8]); + + fflush(stdout); + return 0; +} + +static CallbackResponse callback_pub_gripperstatus(cRosMessage *message, void* data_context) +{ + cRosMessageField *PowerStatus_field = cRosMessageGetField(message, "PowerStatus"); + cRosMessageField *ClampStatus_field = cRosMessageGetField(message, "ClampStatus"); + cRosMessageField *AlarmCodes_field = cRosMessageGetField(message, "AlarmCodes"); + cRosMessageField *Configuration_field = cRosMessageGetField(message, "Configuration"); + + PowerStatus_field->data.as_uint32 = 1; + + *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 0) = 0; + *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 1) = 1; + *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 2) = 2; + *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 3) = 3; + + cRosMessageFieldArrayClear(AlarmCodes_field); + cRosMessageFieldArrayPushBackUInt32(AlarmCodes_field, 1); + cRosMessageFieldArrayPushBackUInt32(AlarmCodes_field, 8); + + Configuration_field->data.as_uint32 = 3; + + return 0; +} + +static CallbackResponse callback_pub_gripperjoints(cRosMessage *message, void* context) +{ + cRosMessageField *Position_field = cRosMessageGetField(message, "Position"); + + *cRosMessageFieldArrayAtFloat64(Position_field, 0) = 8; + *cRosMessageFieldArrayAtFloat64(Position_field, 1) = 7; + *cRosMessageFieldArrayAtFloat64(Position_field, 2) = 6; + *cRosMessageFieldArrayAtFloat64(Position_field, 3) = 5; + *cRosMessageFieldArrayAtFloat64(Position_field, 4) = 4; + *cRosMessageFieldArrayAtFloat64(Position_field, 5) = 3; + *cRosMessageFieldArrayAtFloat64(Position_field, 6) = 2; + *cRosMessageFieldArrayAtFloat64(Position_field, 7) = 1; + *cRosMessageFieldArrayAtFloat64(Position_field, 8) = 0; + + return 0; +} + +/* + * Service callbacks definition + */ + +static CallbackResponse callback_srv_close_gripper(cRosMessage *request, cRosMessage *response, void* context) +{ + srand((unsigned int)time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *closed_field = cRosMessageGetField(response, "closed"); + closed_field->data.as_uint8 = response_val; + + printf("Service close_clamps. Arguments: none . Response %d \n", response_val); + + fflush(stdout); + return 0; +} + +static CallbackResponse callback_srv_open_gripper(cRosMessage *request, cRosMessage *response, void* context) +{ + srand((unsigned int)time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *opened_field = cRosMessageGetField(response, "opened"); + opened_field->data.as_uint8 = response_val; + + printf("Service open_clamps. Arguments: none . Response %d \n", response_val); + fflush(stdout); + + return 0; +} + +static CallbackResponse callback_srv_rest(cRosMessage *request, cRosMessage *response, void* context) +{ + srand((unsigned int)time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *parked_field = cRosMessageGetField(response, "parked"); + parked_field->data.as_uint8 = response_val; + + printf("Service park. Arguments: none . Response %d \n", response_val); + fflush(stdout); + + return 0; +} + +static CallbackResponse callback_srv_reconfigure(cRosMessage *request, cRosMessage *response, void* context) +{ + cRosMessageField *id_field = cRosMessageGetField(request, "id"); + int64_t reconfigure_id; + reconfigure_id = id_field->data.as_int64; + + srand((unsigned int)time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *reconfigured_field = cRosMessageGetField(response, "reconfigured"); + reconfigured_field->data.as_uint8 = response_val; + + printf("Service reconfigure. Arguments: %lld. Response %u \n", (long long)reconfigure_id, response_val); + fflush(stdout); + + return 0; +} + +static CallbackResponse callback_srv_move_arm(cRosMessage *request, cRosMessage *response, void* context) +{ + cRosMessageField *arm_field = cRosMessageGetField(request, "arm"); + cRosMessageField *rad_field = cRosMessageGetField(request, "rad"); + cRosMessageField *velocity_field = cRosMessageGetField(request, "velocity"); + + int32_t arm = arm_field->data.as_int32; + int32_t rad = arm_field->data.as_int32; + int32_t velocity = arm_field->data.as_int32; + + srand((unsigned int)time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *positioned_field = cRosMessageGetField(response, "positioned"); + positioned_field->data.as_uint8 = response_val; + + printf("Service reconfigure. Arguments: {arm: %d, rad: %d, velocity: %d} . Response %d \n", arm, rad, velocity, response_val); + fflush(stdout); + + return 0; +} + +static CallbackResponse callback_srv_transfer(cRosMessage *request, cRosMessage *response, void* context) +{ + srand((unsigned int)time(NULL)); + uint8_t response_val = rand() % 2; + + cRosMessageField *transfered_field = cRosMessageGetField(response, "transfered"); + transfered_field->data.as_uint8 = response_val; + + printf("Service transfer. Arguments: none . Response %d \n", response_val); + fflush(stdout); + + return 0; +} + +int main(int argc, char **argv) +{ + const char *def_pub_node_name = "/Gripper_pub"; + const char *def_sub_node_name = "/Gripper_sub"; + const char *node_name; + const char *default_host = "127.0.0.1", + *node_host = default_host, + *roscore_host = default_host; + unsigned short roscore_port = 11311; + int run_as_publisher = 0; + + node_name = NULL; + int i; + for(i = 1; i < argc; i++) + { + if( strcmp(argv[i],"-name") == 0) + node_name = argv[++i]; + else if (strcmp(argv[i], "-sub") == 0) + run_as_publisher = 0; + else if (strcmp(argv[i], "-pub") == 0) + run_as_publisher = 1; + else if (strcmp(argv[i], "-h") == 0) + { + printHelp(argv[0]); + return(EXIT_SUCCESS); + } + else if( strcmp(argv[i],"-host") == 0) + node_host = argv[++i]; + else if( strcmp(argv[i],"-chost") == 0) + roscore_host = argv[++i]; + else if( strcmp(argv[i],"-cport") == 0) + { + int i_port = atoi(argv[++i]); + if( i_port < 0 || i_port > USHRT_MAX ) + { + fprintf(stderr,"Invalid port value %d.\n",i_port); + return(EXIT_FAILURE); + } + roscore_port = (unsigned short)i_port; + } + else + { + fprintf(stderr,"Invalid option %s\n",argv[i]); + return(EXIT_FAILURE); + } + } + + if (node_name == NULL) // Node name has not been specified, so use the default one + { + if (run_as_publisher) + node_name = def_pub_node_name; + else + node_name = def_sub_node_name; + } + + printf("Running node \"%s\" with host : %s, roscore host : %s and roscore port : %d\n", + node_name, node_host, roscore_host, roscore_port ); + printf("To set a different node/host/port, take a look at the options: "); + printf("%s -h\n", argv[0]); + + char path[4096]; + getcwd(path, sizeof(path)); + strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); + + srand((unsigned int)time(NULL)); + + node = cRosNodeCreate(node_name, node_host, roscore_host, roscore_port, path); + + cRosErrCodePack err_cod; + ROS_INFO(node, "cROS Node created!\n"); + + if (run_as_publisher) + { + ROS_INFO(node, "Example running as publisher\n"); + + // Topics + err_cod = cRosApiRegisterPublisher(node, "/gripperstatus", "gripping_robot/GripperStatus", 1000, + callback_pub_gripperstatus, NULL, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + err_cod = cRosApiRegisterPublisher(node, "/gripperjoints", "gripping_robot/GripperJoints", 1000, + callback_pub_gripperjoints, NULL, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + // Services + err_cod = cRosApiRegisterServiceProvider(node, "/close_clamps", "gripping_robot/CloseGripper", + callback_srv_close_gripper, NULL, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + err_cod = cRosApiRegisterServiceProvider(node, "/open_clamps", "gripping_robot/OpenGripper", + callback_srv_open_gripper, NULL, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + err_cod = cRosApiRegisterServiceProvider(node, "/park_configuration", "gripping_robot/RestPosition", + callback_srv_rest, NULL, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + err_cod = cRosApiRegisterServiceProvider(node, "/reconfigure", "gripping_robot/Reconfigure", + callback_srv_reconfigure, NULL, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + err_cod = cRosApiRegisterServiceProvider(node, "/transfer", "gripping_robot/Transfer", + callback_srv_transfer, NULL, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + err_cod = cRosApiRegisterServiceProvider(node, "/move_arm", "gripping_robot/MoveArm", + callback_srv_move_arm, NULL, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + } + else + { + ROS_INFO(node, "Example running as subscriber\n"); + + err_cod = cRosApiRegisterSubscriber(node, "/gripperstatus", "gripping_robot/GripperStatus", + gripperstatus_sub_callback, NULL, NULL, 0, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + err_cod = cRosApiRegisterSubscriber(node, "/gripperjoints", "gripping_robot/GripperJoints", + gripperjoints_sub_callback, NULL, NULL, 0, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + } + + unsigned char exit_flag = 0; + + err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned error code(s)"); + + // All done: free memory and unregister from ROS master + err_cod=cRosNodeDestroy( node ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); + + return EXIT_SUCCESS; +} diff --git a/src/hal/components/cros/samples/listener.c b/src/hal/components/cros/samples/listener.c new file mode 100644 index 00000000..494ff676 --- /dev/null +++ b/src/hal/components/cros/samples/listener.c @@ -0,0 +1,223 @@ +/*! \file listener.c + * \brief The file is an example of cROS usage implementing a subscriber and service provider, which + * can be used together with talker to prove the operation of message transmissions and service + * calls between two nodes. + * + * It creates a subscriber to the topic /chatter. Each time a message of this topic is + * published a string is received and the callback function callback_sub() is executed. + * This node also implements a provider of the service /sum. Each time a the service is called + * two 64bit integers are received, the callback function callback_srv_add_two_ints() is executed, + * this function computes the sum of them and this result is sent back to service caller. + * To exit safely press Ctrl-C or 'kill' the process once. If this actions are repeated, the process + * will be finished immediately. + */ +#include +#include +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN +# include +# include + +# define DIR_SEPARATOR_STR "\\" +#else +# include +# include +# include + +# define DIR_SEPARATOR_STR "/" +#endif + +#include "cros.h" + +#define ROS_MASTER_PORT 11311 +#define ROS_MASTER_ADDRESS "127.0.0.1" + +CrosNode *node; //! Pointer to object storing the ROS node. This object includes all the ROS node state variables +static unsigned char exit_flag = 0; //! ROS node loop exit flag. When set to 1 the cRosNodeStart() function exits + +// This callback will be invoked when the subscriber receives a message +static CallbackResponse callback_sub(cRosMessage *message, void* data_context) +{ + cRosMessageField *data_field = cRosMessageGetField(message, "data"); + if(data_field != NULL) + ROS_INFO(node, "I heard: [%s]\n", data_field->data.as_string); + + return 0; // 0=success +} + +// This callback will be invoked when the service provider receives a service call +static CallbackResponse callback_srv_add_two_ints(cRosMessage *request, cRosMessage *response, void* data_context) +{ + cRosMessageField *a_field = cRosMessageGetField(request, "a"); + cRosMessageField *b_field = cRosMessageGetField(request, "b"); + //cRosMessageFieldsPrint(request, 0); + + if(a_field != NULL && a_field != NULL) + { + int64_t a = a_field->data.as_int64; + int64_t b = b_field->data.as_int64; + + int64_t response_val = a+b; + + cRosMessageField *sum_field = cRosMessageGetField(response, "sum"); + if(sum_field != NULL) + { + sum_field->data.as_int64 = response_val; + ROS_INFO(node,"Service add 2 ints. Args: {a: %lld, b: %lld}. Resp: %lld\n", (long long)a, (long long)b, (long long)response_val); + } + } + + return 0; // 0=success +} + +// Ctrl-C-and-'kill' event/signal handler: (this code is no strictly necessary for a simple example and can be removed) +#ifdef _WIN32 +// This callback function will be called when the console process receives a CTRL_C_EVENT or +// CTRL_CLOSE_EVENT signal. +// Function set_signal_handler() should be called before calling cRosNodeStart() to set function +// exit_deamon_handler() as the handler of these signals. +// These functions are declared as 'static' to allow the declaration of other (independent) functions with +// the same name in this project. +static BOOL WINAPI exit_deamon_handler(DWORD sig) +{ + BOOL sig_handled; + + switch(sig) + { + case CTRL_C_EVENT: + case CTRL_CLOSE_EVENT: + SetConsoleCtrlHandler(exit_deamon_handler, FALSE); // Remove the handler + printf("Signal %u received: exiting safely.\n", sig); + exit_flag = 1; // Cause the exit of cRosNodeStart loop (safe exit) + sig_handled = TRUE; // Indicate that this signal is handled by this function + break; + default: + sig_handled = FALSE; // Indicate that this signal is not handled by this functions, so the next handler function of the list will be called + break; + } + return(sig_handled); +} + +// Sets the signal handler functions of CTRL_C_EVENT and CTRL_CLOSE_EVENT: exit_deamon_handler +static DWORD set_signal_handler(void) + { + DWORD ret; + + if(SetConsoleCtrlHandler(exit_deamon_handler, TRUE)) + ret=0; // Success setting the control handler + else + { + ret=GetLastError(); + printf("Error setting termination signal handler. Error code=%u\n",ret); + } + return(ret); + } +#else +struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); + +// This callback function will be called when the main process receives a SIGINT or +// SIGTERM signal. +// Function set_signal_handler() should be called to set this function as the handler of +// these signals +static void exit_deamon_handler(int sig) +{ + printf("Signal %i received: exiting safely.\n", sig); + sigaction(SIGINT, &old_int_signal_handler, NULL); + sigaction(SIGTERM, &old_term_signal_handler, NULL); + exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) +} + +// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler +static int set_signal_handler(void) + { + int ret; + struct sigaction act; + + memset (&act, '\0', sizeof(act)); + + act.sa_handler = exit_deamon_handler; + // If the signal handler is invoked while a system call or library function call is blocked, + // then the we want the call to be automatically restarted after the signal handler returns + // instead of making the call fail with the error EINTR. + act.sa_flags=SA_RESTART; + if(sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) + ret=0; + else + { + ret=errno; + printf("Error setting termination signal handler. errno=%d\n",ret); + } + return(ret); + } +#endif + +int main(int argc, char **argv) +{ + char path[4097]; // We need to tell our node where to find the .msg files that we'll be using + const char *node_name; + int subidx; // Index (identifier) of the created subscriber + cRosErrCodePack err_cod; + + if(argc>1) + node_name=argv[1]; + else + node_name="/listener"; // Default node name if no command-line parameters are specified + getcwd(path, sizeof(path)); + strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); + + printf("Using the following path for message definitions: %s\n", path); + // Create a new node and tell it to connect to roscore in the usual place + node = cRosNodeCreate(node_name, "127.0.0.1", ROS_MASTER_ADDRESS, ROS_MASTER_PORT, path); + if( node == NULL ) + { + printf("cRosNodeCreate() failed; is this program already being run?"); + return EXIT_FAILURE; + } + + err_cod = cRosWaitPortOpen(ROS_MASTER_ADDRESS, ROS_MASTER_PORT, 0); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "Port %s:%hu cannot be opened: ROS Master does not seems to be running", ROS_MASTER_ADDRESS, ROS_MASTER_PORT); + return EXIT_FAILURE; + } + + // Create a subscriber to topic /chatter of type "std_msgs/String" and supply a callback for received messages (callback_sub) + err_cod = cRosApiRegisterSubscriber(node, "/chatter", "std_msgs/String", callback_sub, NULL, NULL, 0, &subidx); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterSubscriber() failed; did you run this program one directory above 'rosdb'?"); + cRosNodeDestroy( node ); + return EXIT_FAILURE; + } + + // Create a service provider named /sum of type "roscpp_tutorials/TwoInts" and supply a callback for received calls + err_cod = cRosApiRegisterServiceProvider(node,"/sum","roscpp_tutorials/TwoInts", callback_srv_add_two_ints, NULL, NULL, NULL); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterServiceProvider() failed; did you run this program one directory above 'rosdb'?"); + cRosNodeDestroy( node ); + return EXIT_FAILURE; + } + + ROS_INFO(node, "Node %s created with XMLRPC port: %i, TCPROS port: %i and RPCROS port: %i\n", node->name, node->xmlrpc_port, node->tcpros_port, node->rpcros_port); + + // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed + set_signal_handler(); + + // Run the main loop until exit_flag is 1 + err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + + // Free memory and unregister + err_cod=cRosNodeDestroy( node ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} diff --git a/src/hal/components/cros/samples/parameters-test.c b/src/hal/components/cros/samples/parameters-test.c new file mode 100644 index 00000000..8c745197 --- /dev/null +++ b/src/hal/components/cros/samples/parameters-test.c @@ -0,0 +1,249 @@ +/*! \file parameters-test.c + * \brief The file is an example of cROS usage implementing a parameter subscriber. This sample + * can be used together with rosparam or MATLAB to prove the operation of parameter sharing + * between two nodes. If MATLAB is used, it must not be used as ROS master node since it seems that + * it does not notify parameter updates. + * + * It creates a parameter subscriber to the key /testparam. Each time this parameter is updated by + * other node the master node informs about it and the callback function getNodeStatusCallback() is executed. + * This function checks if the parameters /testparam/x, /testparam/y and /testparam/z have been updated. + * When all of these parameters are updated by other node, the function changes their value to + * {x: 5, y: hello, z: [0,1,2,4]} and continues checking again when all the them change. + * - With ROS core it can be tested by running: + * $ rosparam set /testparam "{x: 3, y: ciao, z: [1,2,3]}" + * - With MATLAB R2015a (or later) it can be tested by running: + * >> rosinit + * >> rosparam('set', '/testparam/x', int32(3)) + * >> rosparam('set', '/testparam/y', 'ciao') + * >> rosparam('set', '/testparam/z', {int32(1),int32(2),int32(3)}) + * >> rosparam('get', '/testparam') + * To exit safely press Ctrl-C or 'kill' the process once. If this actions are repeated, the process + * will be finished immediately. + */ + +#include +#include +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN +# include +# include + +# define DIR_SEPARATOR_STR "\\" +#else +# include +# include +# include + +# define DIR_SEPARATOR_STR "/" +#endif + +#include "cros_log.h" +#include "cros_node.h" +#include "cros_api.h" +#include "cros_clock.h" + +CrosNode *node; + +int paramx_updated = 0; +int paramy_updated = 0; +int paramz_updated = 0; +static unsigned char exit_flag = 0; //! ROS-node loop exit flag. When it is set to 1 the cRosNodeStart() function exits + +static void getParamNamesCallback(int callid, GetParamNamesResult *result, void *context) +{ + // OK +} + +static void setParamCallback(int callid, SetParamResult *result, void *context) +{ + // OK +} + +static void getNodeStatusCallback(CrosNodeStatusUsr *status, void* context) +{ +/* + XmlrpcParam *param0; + XmlrpcParam *param1; + XmlrpcParam *param2; + + if (status->parameter_value->array_n_elem == 3) + { + param0 = &status->parameter_value->data.as_array[0]; + param1 = &status->parameter_value->data.as_array[1]; + param2 = &status->parameter_value->data.as_array[2]; + } +*/ + + if (status->state == CROS_STATUS_PARAM_UPDATE) + { + ROS_INFO(node, "Subscribed parameter updated: %s\n", status->parameter_key); + + if (strcmp(status->parameter_key, "/testparam/x/") == 0) + paramx_updated = 1; + + if (strcmp(status->parameter_key, "/testparam/y/") == 0) + paramy_updated = 1; + + if (strcmp(status->parameter_key, "/testparam/z/") == 0) + paramz_updated = 1; + + if (paramx_updated && paramy_updated && paramz_updated) + { + cRosErrCodePack err_cod; + + XmlrpcParam param; + xmlrpcParamInit(¶m); + xmlrpcParamSetStruct(¶m); + + xmlrpcParamStructPushBackInt(¶m, "x", 5); + xmlrpcParamStructPushBackString(¶m, "y", "hello"); + XmlrpcParam *array = xmlrpcParamStructPushBackArray(¶m, "z"); + xmlrpcParamArrayPushBackInt(array, 0); + xmlrpcParamArrayPushBackInt(array, 1); + xmlrpcParamArrayPushBackInt(array, 2); + xmlrpcParamArrayPushBackInt(array, 4); + + ROS_INFO(node, "x, y, and z parameter values changed: Modifying their value\n"); + err_cod = cRosApiSetParam(node, "/testparam", ¶m, setParamCallback, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosApiSetParam() failed"); + + paramx_updated = 0; + paramy_updated = 0; + paramz_updated = 0; + xmlrpcParamRelease(¶m); + } + } +} + +#ifdef _WIN32 +// This callback function will be called when the console process receives a CTRL_C_EVENT or +// CTRL_CLOSE_EVENT signal. +// Function set_signal_handler() should be called to set function exit_deamon_handler() as the handler of +// these signals +static BOOL WINAPI exit_deamon_handler(DWORD sig) +{ + BOOL sig_handled; + + switch (sig) + { + case CTRL_C_EVENT: + case CTRL_CLOSE_EVENT: + SetConsoleCtrlHandler(exit_deamon_handler, FALSE); // Remove the handler + printf("Signal %u received: exiting safely.\n", sig); + exit_flag = 1; // Cause the exit of cRosNodeStart loop (safe exit) + sig_handled = TRUE; // Indicate that this signal is handled by this function + break; + default: + sig_handled = FALSE; // Indicate that this signal is not handled by this functions, so the next handler function of the list will be called + break; + } + return(sig_handled); +} + +// Sets the signal handler functions of CTRL_C_EVENT and CTRL_CLOSE_EVENT: exit_deamon_handler +static DWORD set_signal_handler(void) +{ + DWORD ret; + + if (SetConsoleCtrlHandler(exit_deamon_handler, TRUE)) + ret = 0; // Success setting the control handler + else + { + ret = GetLastError(); + printf("Error setting termination signal handler. Error code=%u\n", ret); + } + return(ret); +} +#else +struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); + +// This callback function will be called when the main process receives a SIGINT or +// SIGTERM signal. +// Function set_signal_handler() should be called to set this function as the handler of +// these signals +static void exit_deamon_handler(int sig) +{ + printf("Signal %i received: exiting safely.\n", sig); + sigaction(SIGINT, &old_int_signal_handler, NULL); + sigaction(SIGTERM, &old_term_signal_handler, NULL); + exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) +} + +// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler +static int set_signal_handler(void) +{ + int ret; + struct sigaction act; + + memset(&act, '\0', sizeof(act)); + + act.sa_handler = exit_deamon_handler; + // If the signal handler is invoked while a system call or library function call is blocked, + // then the we want the call to be automatically restarted after the signal handler returns + // instead of making the call fail with the error EINTR. + act.sa_flags = SA_RESTART; + if (sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) + ret = 0; + else + { + ret = errno; + printf("Error setting termination signal handler. errno=%d\n", ret); + } + return(ret); +} +#endif + +int main(int argc, char **argv) +{ + const char *default_node_name = "/Gripper", + *node_name = default_node_name; + const char *default_host = "127.0.0.1", + *node_host = default_host, + *roscore_host = default_host; + unsigned short roscore_port = 11311; + char path[4097]; + cRosErrCodePack err_cod; + + getcwd(path, sizeof(path)); + strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); + node = cRosNodeCreate(node_name, node_host, roscore_host, roscore_port, path); + + err_cod = cRosApiSubscribeParam(node, "/testparam", getNodeStatusCallback, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiSubscribeParam() failed"); + cRosNodeDestroy( node ); + return EXIT_FAILURE; + } + + printf("Node XMLRPC port: %i\n", node->xmlrpc_port); + + err_cod = cRosApiGetParamNames(node, getParamNamesCallback, NULL, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiGetParamNames() failed"); + cRosNodeDestroy( node ); + return EXIT_FAILURE; + } + + // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed + set_signal_handler(); + + // Run the main loop until exit_flag is 1 + err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + + // Free memory and unregister + err_cod=cRosNodeDestroy( node ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unsubscribing in ROS master"); + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} diff --git a/src/hal/components/cros/samples/performance-test.cpp b/src/hal/components/cros/samples/performance-test.cpp new file mode 100644 index 00000000..778ad725 --- /dev/null +++ b/src/hal/components/cros/samples/performance-test.cpp @@ -0,0 +1,583 @@ +/*! \file performance-tesp.cpp + * \brief This file implements a cROS program for measuring the performance of the cROS library. + * + * To exit safely press Ctrl-C or 'kill' the process once. If this actions are repeated, the process + * will be finished immediately. + */ +#include +#include +#include +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN +# include +# include + +# define DIR_SEPARATOR_STR "\\" +#else +# include +# include +# include + +# define DIR_SEPARATOR_STR "/" +#endif + +#include "cros.h" + +#define ROS_MASTER_PORT 11311 +#define ROS_MASTER_ADDRESS "127.0.0.1" + +CrosNode *node; //! Pointer to object storing the ROS node. This object includes all the ROS-node state variables +static unsigned char exit_flag = 0; //! ROS-node loop exit flag. When set to 1 the cRosNodeStart() function exits + +double **Time_stamps; +size_t N_measure_exper = 0; +size_t N_measure_rep = 0; +size_t Msg_sizes[]={1, 1, 1024/10, 1024*10, 1024*1024, 1024*1024*100}; +size_t Measure_reps[]={1000000, 1000000, 100000, 100000, 1000, 100}; + +#define TOTAL_MEASURE_EXPERS (sizeof(Measure_reps)/sizeof(size_t)) +#define MAX_MEASURE_REPS 1000000 +#define MAX_MSG_SIZE (1024*1024*100) + +double **allocate_heter_2d_array(size_t n_sub_arrays, const size_t *sub_array_sizes) +{ + double **array_2d; + size_t n_total_elems, cur_sub_array_ind; + + n_total_elems=0; + for(cur_sub_array_ind=0;cur_sub_array_inddata.as_string)); + //ROS_INFO(node, "I heard: [%s]\n", data_field->data.as_string); + } + + N_measure_rep++; + + if(N_measure_rep >= Measure_reps[N_measure_exper]) + { + N_measure_rep=0; + N_measure_exper++; + //printf("*** %lu **\n", N_measure_exper); + } + + if(N_measure_exper>=TOTAL_MEASURE_EXPERS) + exit_flag = 1; + + return 0; // 0=success +} + +static CallbackResponse callback_pub(cRosMessage *message, void *data_context) +{ + static int pub_count = 0; + char buf[1024]; + // We need to index into the message structure and then assign to fields + cRosMessageField *data_field; + + data_field = cRosMessageGetField(message, "data"); + if(data_field) + { + snprintf(buf, sizeof(buf), "periodic hello world %d", pub_count); + if(cRosMessageSetFieldValueString(data_field, buf) == 0) + { + ROS_INFO(node, "%s\n", buf); + } + pub_count+=10; + } + + return 0; // 0=success +} + +// This callback will be invoked when the service provider receives a service call +static CallbackResponse callback_service_provider(cRosMessage *request, cRosMessage *response, void* data_context) +{ + Time_stamps[N_measure_exper][N_measure_rep++] = cRosClockTimeStampToUSec(cRosClockGetTimeStamp()); + if(N_measure_rep >= Measure_reps[N_measure_exper]) + { + N_measure_rep=0; + N_measure_exper++; + } + + cRosMessageField *name_field = cRosMessageGetField(request, "name"); + if(name_field != NULL) + { + cRosMessageField *ok_field = cRosMessageGetField(response, "ok"); + if(ok_field != NULL) + { + ok_field->data.as_uint8 = 1; + //ROS_INFO(node,"Service Resp: %hu\n", (unsigned short)ok_field->data.as_uint8); + } + } + + if(N_measure_exper>=TOTAL_MEASURE_EXPERS) + exit_flag = 1; + + return 0; // 0=success +} + +static CallbackResponse callback_service_caller(cRosMessage *request, cRosMessage *response, int call_resp_flag, void* context) +{ + static int call_count = 0; + + if(!call_resp_flag) // Check if this callback function has been called to provide the service call arguments or to collect the response + { + cRosMessageField *a_field = cRosMessageGetField(request, "a"); + cRosMessageField *b_field = cRosMessageGetField(request, "b"); + if(a_field != NULL && b_field != NULL) + { + a_field->data.as_int64=10; + b_field->data.as_int64=call_count; + ROS_INFO(node, "Service add 2 ints call arguments: {a: %lld, b: %lld}\b\n", (long long)a_field->data.as_int64, (long long)b_field->data.as_int64); + } + } + else // Service call response available + { + cRosMessageField *sum_field = cRosMessageGetField(response, "sum"); + if(sum_field != NULL) + ROS_INFO(node, "Service add 2 ints response: %lld (call_count: %i)\n", (long long)sum_field->data.as_int64, call_count++); + } + + if(call_count > 10) exit_flag=1; + return 0; // 0=success +} + + +// Ctrl-C-and-'kill' event/signal handler: (this code is no strictly necessary for a simple example and can be removed) +#ifdef _WIN32 +// This callback function will be called when the console process receives a CTRL_C_EVENT or +// CTRL_CLOSE_EVENT signal. +// Function set_signal_handler() should be called before calling cRosNodeStart() to set function +// exit_deamon_handler() as the handler of these signals. +// These functions are declared as 'static' to allow the declaration of other (independent) functions with +// the same name in this project. +static BOOL WINAPI exit_deamon_handler(DWORD sig) +{ + BOOL sig_handled; + + switch(sig) + { + case CTRL_C_EVENT: + case CTRL_CLOSE_EVENT: + SetConsoleCtrlHandler(exit_deamon_handler, FALSE); // Remove the handler + printf("Signal %u received: exiting safely.\n", sig); + exit_flag = 1; // Cause the exit of cRosNodeStart loop (safe exit) + sig_handled = TRUE; // Indicate that this signal is handled by this function + break; + default: + sig_handled = FALSE; // Indicate that this signal is not handled by this functions, so the next handler function of the list will be called + break; + } + return(sig_handled); +} + +// Sets the signal handler functions of CTRL_C_EVENT and CTRL_CLOSE_EVENT: exit_deamon_handler +static DWORD set_signal_handler(void) + { + DWORD ret; + + if(SetConsoleCtrlHandler(exit_deamon_handler, TRUE)) + ret=0; // Success setting the control handler + else + { + ret=GetLastError(); + printf("Error setting termination signal handler. Error code=%u\n",ret); + } + return(ret); + } +#else +struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); + +// This callback function will be called when the main process receives a SIGINT or +// SIGTERM signal. +// Function set_signal_handler() should be called to set this function as the handler of +// these signals +static void exit_deamon_handler(int sig) +{ + printf("Signal %i received: exiting safely.\n", sig); + sigaction(SIGINT, &old_int_signal_handler, NULL); + sigaction(SIGTERM, &old_term_signal_handler, NULL); + exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) +} + +// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler +static int set_signal_handler(void) + { + int ret; + struct sigaction act; + + memset (&act, '\0', sizeof(act)); + + act.sa_handler = exit_deamon_handler; + // If the signal handler is invoked while a system call or library function call is blocked, + // then the we want the call to be automatically restarted after the signal handler returns + // instead of making the call fail with the error EINTR. + act.sa_flags=SA_RESTART; + if(sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) + ret=0; + else + { + ret=errno; + printf("Error setting termination signal handler. errno=%d\n",ret); + } + return(ret); + } +#endif + +cRosErrCodePack run_topic_subscriber(void) +{ + cRosErrCodePack err_cod; + int subidx; // Index (identifier) of the subscriber to be created + + // Create a subscriber to topic /chatter of type "std_msgs/String" and supply a callback for received messages (callback_sub) + err_cod = cRosApiRegisterSubscriber(node, "/chatter", "std_msgs/String", callback_sub, NULL, NULL, 0, &subidx); + if (err_cod == CROS_SUCCESS_ERR_PACK) + { + // Run the main loop until exit_flag is 1 + err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + } + else + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterSubscriber() failed."); + } + return err_cod; +} + +cRosErrCodePack run_service_provider(void) +{ + cRosErrCodePack err_cod; + + // Create a service provider named /bulk of type "controller_manager_msgs/LoadController.srv" and supply a callback for received calls + err_cod = cRosApiRegisterServiceProvider(node,"/bulk","controller_manager_msgs/LoadController", callback_service_provider, NULL, NULL, NULL); + if(err_cod == CROS_SUCCESS_ERR_PACK) + { + // Run the main loop until exit_flag is 1 + err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + } + else + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterServiceProvider() failed; did you run this program one directory above 'rosdb'?"); + } + + return err_cod; +} + +cRosErrCodePack run_topic_publisher(void) +{ + cRosErrCodePack err_cod; + cRosMessage *msg; + cRosMessageField *data_field; + int pubidx; // Index (identifier) of the publisher to be created + + // Create a publisher to topic /chatter of type "std_msgs/String" + err_cod = cRosApiRegisterPublisher(node, "/chatter", "std_msgs/String", -1, NULL, NULL, NULL, &pubidx); // callback_pub + if (err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterPublisher() failed; did you run this program one directory above 'rosdb'?"); + cRosNodeDestroy(node); + return err_cod; + } + + msg = cRosApiCreatePublisherMessage(node, pubidx); + // Popullate msg + data_field = cRosMessageGetField(msg, "data"); + if (data_field != NULL) + { + static char buf[MAX_MSG_SIZE+1]; + int pub_count; + + memset(buf,' ',MAX_MSG_SIZE+1); + for (pub_count = 0; pub_count < TOTAL_MEASURE_EXPERS; pub_count++) + buf[Msg_sizes[pub_count]]='\0'; // Previously set end of string for all message sizes + + + + data_field->data.as_string = buf; + + printf("Publishing strings...\n"); + //cRosMessageSetFieldValueString(data_field, buf); + err_cod = cRosNodeStart( node, 200, &exit_flag ); + for (pub_count = 0; pub_count < TOTAL_MEASURE_EXPERS && err_cod == CROS_SUCCESS_ERR_PACK && exit_flag == 0; pub_count++) + { + int rep_count; + printf("Publishing %lu bytes %lu times\n", strlen(buf),Measure_reps[pub_count]); + //cRosMessageSetFieldValueString(data_field, buf); + for(rep_count=0;rep_countdata.as_string = NULL; // Prevent cRosMessageFree from trying to free the char array + + } + else + printf("Error accessing message fields\n"); + + cRosMessageFree(msg); + + if (err_cod == CROS_SUCCESS_ERR_PACK) + { + err_cod = cRosNodeStart( node, 20000, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + } + + return err_cod; +} + +cRosErrCodePack run_service_caller(void) +{ + cRosErrCodePack err_cod; + cRosMessage *msg_req, msg_res; + cRosMessageField *name_field; + int calleridx; // Index (identifier) of the service caller to be created + + + // Create a service caller named /bulk of type "controller_manager_msgs/LoadController.srv" and request that the associated callback + err_cod = cRosApiRegisterServiceCaller(node,"/bulk","controller_manager_msgs/LoadController", -1, NULL, NULL, NULL, 1, 1, &calleridx); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterServiceCaller() failed; did you run this program one directory above 'rosdb'?"); + cRosNodeDestroy( node ); + return err_cod; + } + + + msg_req = cRosApiCreateServiceCallerRequest(node, calleridx); + cRosMessageInit(&msg_res); + + name_field = cRosMessageGetField(msg_req, "name"); + if (name_field != NULL) + { + static char buf[MAX_MSG_SIZE+1]={0}; + int call_count = 0; + + printf("Calling service...\n"); + + err_cod = cRosNodeStart( node, 200, &exit_flag ); + for (call_count = 0; call_count < TOTAL_MEASURE_EXPERS && err_cod == CROS_SUCCESS_ERR_PACK && exit_flag == 0; call_count++) + { + int rep_count; + //snprintf(buf, sizeof(buf), "hello world %d", call_count); + memset(buf,' ',Msg_sizes[call_count]); + + cRosMessageSetFieldValueString(name_field, buf); + + for(rep_count=0;rep_countdata.as_uint8); + //err_cod = cRosNodeStart( node, 1000, &exit_flag ); + } + else + cRosPrintErrCodePack(err_cod, "cRosNodeServiceCall() failed: service call not made"); + } + } + + } + else + printf("Error accessing message fields\n"); + + + cRosMessageFree(msg_req); + cRosMessageRelease(&msg_res); + + printf("End of service call.\n"); + + // Run the main loop until exit_flag is 1 + if (err_cod == CROS_SUCCESS_ERR_PACK) + { + err_cod = cRosNodeStart( node, 200, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + } + + return err_cod; +} + + +int main(int argc, char **argv) +{ + char path[4097]; // We need to tell our node where to find the .msg files that it will be using + const char *node_name; + + cRosErrCodePack err_cod; + int op_mode; + + if(getcwd(path, sizeof(path)) == NULL) + { + perror("Could not get current directory"); + return EXIT_FAILURE; + } + + strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); + + printf("PATH ROSDB: %s\n", path); + + printf("Press s for subscriber, p for publisher, r for service server or c for service client: "); + op_mode = getchar(); + if (op_mode == 's') + node_name = "/node_sub"; + else if (op_mode == 'r') + node_name = "/node_server"; + else if (op_mode == 'p') + node_name = "/node_pub"; + else if (op_mode == 'c') + node_name = "/node_caller"; + else + { + printf("Invalid option\n"); + return EXIT_FAILURE; + } + + Time_stamps = allocate_heter_2d_array(TOTAL_MEASURE_EXPERS, Measure_reps); + if(Time_stamps == NULL) + { + printf("Cannot allocate memory for the time stamps\n"); + return EXIT_FAILURE; + } + + // Create a new node and tell it to connect to roscore in the usual place + node = cRosNodeCreate(node_name, "127.0.0.1", ROS_MASTER_ADDRESS, ROS_MASTER_PORT, path); + if( node == NULL ) + { + printf("cRosNodeCreate() failed; is this program already being run?"); + free_heter_2d_array(Time_stamps); + return EXIT_FAILURE; + } + + err_cod = cRosWaitPortOpen(ROS_MASTER_ADDRESS, ROS_MASTER_PORT, 0); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "Port %s:%hu cannot be opened: ROS Master does not seems to be running", ROS_MASTER_ADDRESS, ROS_MASTER_PORT); + free_heter_2d_array(Time_stamps); + return EXIT_FAILURE; + } + + printf("Node RPCROS port: %i\n", node->rpcros_port); + + // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed + set_signal_handler(); + + if (op_mode == 's') + { + err_cod = run_topic_subscriber(); + } + else if (op_mode == 'r') + { + err_cod = run_service_provider(); + } + else if (op_mode == 'p') + { + err_cod = run_topic_publisher(); + } + else if (op_mode == 'c') + { + err_cod = run_service_caller(); + } + + + printf("Unregistering in ROS master\n"); + // Free memory and unregister + err_cod=cRosNodeDestroy( node ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); + free_heter_2d_array(Time_stamps); + return EXIT_FAILURE; + } + + printf("Node end. Current N_measure_exper: %lu N_measure_rep: %lu.\n", N_measure_exper, N_measure_rep); + + if(op_mode == 's' || op_mode == 'r') + { + store_heter_2d_array("times.txt", Time_stamps, TOTAL_MEASURE_EXPERS, Measure_reps); + } + + free_heter_2d_array(Time_stamps); + return EXIT_SUCCESS; +} diff --git a/src/hal/components/cros/samples/plot_performance.m b/src/hal/components/cros/samples/plot_performance.m new file mode 100644 index 00000000..5219d1e6 --- /dev/null +++ b/src/hal/components/cros/samples/plot_performance.m @@ -0,0 +1,28 @@ +times={}; +fd = fopen('../build/bin/times.txt','r'); +n_measure_expers=0; +while(~feof(fd)) + n_measure_expers=n_measure_expers+1; + tline = fgetl(fd); + times{n_measure_expers} = sscanf(tline,'%f')'; +end +fclose(fd) +clear(tline) + +diff_times = cellfun(@diff, times, 'UniformOutput', false); + +x_axis = 1:n_measure_expers; +mean_times = cellfun(@mean, diff_times); +stddev_times = cellfun(@std, diff_times); +%bar(x_axis, mean_times) +%hold on, +h = errorbar(x_axis, mean_times, stddev_times); +set(gca,'YScale','log') +%ylabel('\mus/msg') +c_axis=axis; +axis([0.9 n_measure_expers+0.1 c_axis(3) c_axis(4)]) + +figure +for n_hist=1:n_measure_expers + subplot(n_measure_expers,1,n_hist), hist(diff_times{n_hist}, length(diff_times{n_hist})/5) +end \ No newline at end of file diff --git a/src/hal/components/cros/samples/ros-i-trajectory-test.c b/src/hal/components/cros/samples/ros-i-trajectory-test.c new file mode 100644 index 00000000..a34c6602 --- /dev/null +++ b/src/hal/components/cros/samples/ros-i-trajectory-test.c @@ -0,0 +1,219 @@ +// +// Created by nico on 26/05/16. +// + +#include +#include +#include +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN +# include +# include + +# define DIR_SEPARATOR_STR "\\" +#else +# include +# include +# include + +# define DIR_SEPARATOR_STR "/" +#endif + +#include "cros.h" + +CrosNode *node; +static unsigned char exit_flag = 0; //! ROS-node loop exit flag. When it is set to 1 the cRosNodeStart() function exits + +static void printHelp( char *cmd_name ) +{ + printf("Usage: %s [OPTION] ... \n", cmd_name); + printf("Options:\n"); + printf("\t-name Set the node name (default: /test_node)\n"); + printf("\t-host Set the node host (default: 127.0.0.1)\n"); + printf("\t-chost Set the roscore host (default: 127.0.0.1)\n"); + printf("\t-cport Set the roscore port (default: 11311)\n"); + printf("\t-h Print this help\n"); +} + +static CallbackResponse jointstates_sub_callback(cRosMessage *message, void* data_context) +{ + int joints_size, joint_ind; + + cRosMessageField *field_name = cRosMessageGetField(message, "name"); + cRosMessageField *field_position = cRosMessageGetField(message, "position"); + cRosMessageField *field_velocity = cRosMessageGetField(message, "velocity"); + cRosMessageField *field_effort = cRosMessageGetField(message, "effort"); + + joints_size = field_name->array_size; + + printf("Joint names:"); + for(joint_ind = 0; joint_ind < joints_size; joint_ind++) + { + char* joint_name; + joint_name = cRosMessageFieldArrayAtStringGet(field_name, joint_ind); + printf(" %s", joint_name); + } + printf("\n"); + + return 0; +} + +#ifdef _WIN32 +// This callback function will be called when the console process receives a CTRL_C_EVENT or +// CTRL_CLOSE_EVENT signal. +// Function set_signal_handler() should be called to set function exit_deamon_handler() as the handler of +// these signals +static BOOL WINAPI exit_deamon_handler(DWORD sig) +{ + BOOL sig_handled; + + switch (sig) + { + case CTRL_C_EVENT: + case CTRL_CLOSE_EVENT: + SetConsoleCtrlHandler(exit_deamon_handler, FALSE); // Remove the handler + printf("Signal %u received: exiting safely.\n", sig); + exit_flag = 1; // Cause the exit of cRosNodeStart loop (safe exit) + sig_handled = TRUE; // Indicate that this signal is handled by this function + break; + default: + sig_handled = FALSE; // Indicate that this signal is not handled by this functions, so the next handler function of the list will be called + break; + } + return(sig_handled); +} + +// Sets the signal handler functions of CTRL_C_EVENT and CTRL_CLOSE_EVENT: exit_deamon_handler +static DWORD set_signal_handler(void) +{ + DWORD ret; + + if (SetConsoleCtrlHandler(exit_deamon_handler, TRUE)) + ret = 0; // Success setting the control handler + else + { + ret = GetLastError(); + printf("Error setting termination signal handler. Error code=%u\n", ret); + } + return(ret); +} +#else +struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); + +// This callback function will be called when the main process receives a SIGINT or +// SIGTERM signal. +// Function set_signal_handler() should be called to set this function as the handler of +// these signals +static void exit_deamon_handler(int sig) +{ + printf("Signal %i received: exiting safely.\n", sig); + sigaction(SIGINT, &old_int_signal_handler, NULL); + sigaction(SIGTERM, &old_term_signal_handler, NULL); + exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) +} + +// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler +static int set_signal_handler(void) +{ + int ret; + struct sigaction act; + + memset(&act, '\0', sizeof(act)); + + act.sa_handler = exit_deamon_handler; + // If the signal handler is invoked while a system call or library function call is blocked, + // then the we want the call to be automatically restarted after the signal handler returns + // instead of making the call fail with the error EINTR. + act.sa_flags = SA_RESTART; + if (sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) + ret = 0; + else + { + ret = errno; + printf("Error setting termination signal handler. errno=%d\n", ret); + } + return(ret); +} +#endif + +int main(int argc, char **argv) +{ + const char *default_node_name = "/robot_listener", + *node_name = default_node_name; + const char *default_host = "127.0.0.1", + *node_host = default_host, + *roscore_host = default_host; + unsigned short roscore_port = 11311; + + if(argc == 2) + { + printHelp( argv[0] ); + return EXIT_SUCCESS; + } + + // srand (time(NULL)); + + int i = 1; + for( ; i < argc - 1; i+=2) + { + if( strcmp(argv[i],"-name") == 0) + node_name = argv[i+1]; + else if( strcmp(argv[i],"-host") == 0) + node_host = argv[i+1]; + else if( strcmp(argv[i],"-chost") == 0) + roscore_host = argv[i+1]; + else if( strcmp(argv[i],"-cport") == 0) + { + int i_port = atoi(argv[i+1]); + if( i_port < 0 || i_port > USHRT_MAX ) + { + fprintf(stderr,"Invalid port %d\n",i_port); + exit(EXIT_FAILURE); + } + roscore_port = (unsigned short)i_port; + } + else + { + fprintf(stderr,"Invalid option %s\n",argv[i]); + exit(EXIT_FAILURE); + } + } + + printf("Running node \"%s\" with host : %s, roscore host : %s and roscore port : %d\n", + node_name, node_host, roscore_host, roscore_port ); + printf("To set a different node/host/port, take a look at the options: "); + printf("%s -h\n", argv[0]); + + char path[4096]; + getcwd(path, sizeof(path)); + strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); + node = cRosNodeCreate(node_name, node_host, roscore_host, roscore_port, path); + + cRosErrCodePack err_cod; + ROS_INFO(node, "cROS Node created!\n"); + + err_cod = cRosApiRegisterSubscriber(node, "/arm/joint_states", "sensor_msgs/JointState", + jointstates_sub_callback, NULL, NULL, 0, NULL); + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + if (err_cod != CROS_SUCCESS_ERR_PACK) + return EXIT_FAILURE; + + // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed + set_signal_handler(); + + // Run the main loop until exit_flag is 1 + err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + + // All done: free memory and unregister from ROS master + err_cod=cRosNodeDestroy( node ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); + + return EXIT_SUCCESS; +} diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/CloseGripper.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/CloseGripper.srv new file mode 100644 index 00000000..6045b07b --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/gripping_robot/CloseGripper.srv @@ -0,0 +1,3 @@ +--- +bool closed + diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/GripperJoints.msg b/src/hal/components/cros/samples/rosdb/gripping_robot/GripperJoints.msg new file mode 100644 index 00000000..1bc7c0b7 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/gripping_robot/GripperJoints.msg @@ -0,0 +1 @@ +float64[9] Position diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/GripperStatus.msg b/src/hal/components/cros/samples/rosdb/gripping_robot/GripperStatus.msg new file mode 100644 index 00000000..95c32b04 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/gripping_robot/GripperStatus.msg @@ -0,0 +1,4 @@ +uint32 PowerStatus +uint32[4] ClampStatus +uint32[] AlarmCodes +uint32 Configuration diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/MoveArm.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/MoveArm.srv new file mode 100644 index 00000000..5e9e3f61 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/gripping_robot/MoveArm.srv @@ -0,0 +1,5 @@ +int32 arm +int32 rad +int32 velocity +--- +bool positioned diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/OpenGripper.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/OpenGripper.srv new file mode 100644 index 00000000..08078611 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/gripping_robot/OpenGripper.srv @@ -0,0 +1,3 @@ +--- +bool opened + diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/Reconfigure.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/Reconfigure.srv new file mode 100644 index 00000000..897ceaf8 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/gripping_robot/Reconfigure.srv @@ -0,0 +1,4 @@ +int64 id +--- +bool reconfigured + diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/RestPosition.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/RestPosition.srv new file mode 100644 index 00000000..988d5c0e --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/gripping_robot/RestPosition.srv @@ -0,0 +1,2 @@ +--- +bool parked diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/Transfer.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/Transfer.srv new file mode 100644 index 00000000..979b249f --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/gripping_robot/Transfer.srv @@ -0,0 +1,2 @@ +--- +bool transfered diff --git a/src/hal/components/cros/samples/rosdb/roscpp/GetLoggers.srv b/src/hal/components/cros/samples/rosdb/roscpp/GetLoggers.srv new file mode 100644 index 00000000..855e6bc0 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/roscpp/GetLoggers.srv @@ -0,0 +1,2 @@ +--- +Logger[] loggers \ No newline at end of file diff --git a/src/hal/components/cros/samples/rosdb/roscpp/Logger.msg b/src/hal/components/cros/samples/rosdb/roscpp/Logger.msg new file mode 100644 index 00000000..bf45c288 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/roscpp/Logger.msg @@ -0,0 +1,2 @@ +string name +string level \ No newline at end of file diff --git a/src/hal/components/cros/samples/rosdb/roscpp/SetLoggerLevel.srv b/src/hal/components/cros/samples/rosdb/roscpp/SetLoggerLevel.srv new file mode 100644 index 00000000..29841db3 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/roscpp/SetLoggerLevel.srv @@ -0,0 +1,3 @@ +string logger +string level +--- \ No newline at end of file diff --git a/src/hal/components/cros/samples/rosdb/roscpp_tutorials/TwoInts.srv b/src/hal/components/cros/samples/rosdb/roscpp_tutorials/TwoInts.srv new file mode 100755 index 00000000..3a68808e --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/roscpp_tutorials/TwoInts.srv @@ -0,0 +1,4 @@ +int64 a +int64 b +--- +int64 sum diff --git a/src/hal/components/cros/samples/rosdb/rosgraph_msgs/Log.msg b/src/hal/components/cros/samples/rosdb/rosgraph_msgs/Log.msg new file mode 100644 index 00000000..9a9597c2 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/rosgraph_msgs/Log.msg @@ -0,0 +1,19 @@ +## +## Severity level constants +## +byte DEBUG=1 #debug level +byte INFO=2 #general level +byte WARN=4 #warning level +byte ERROR=8 #error level +byte FATAL=16 #fatal/critical level +## +## Fields +## +Header header +byte level +string name # name of the node +string msg # message +string file # file the message came from +string function # function the message came from +uint32 line # line the message came from +string[] topics # topic names that the node publishes diff --git a/src/hal/components/cros/samples/rosdb/sensor_msgs/JointState.msg b/src/hal/components/cros/samples/rosdb/sensor_msgs/JointState.msg new file mode 100644 index 00000000..f67f4bcf --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/sensor_msgs/JointState.msg @@ -0,0 +1,26 @@ +# This is a message that holds data to describe the state of a set of torque controlled joints. +# +# The state of each joint (revolute or prismatic) is defined by: +# * the position of the joint (rad or m), +# * the velocity of the joint (rad/s or m/s) and +# * the effort that is applied in the joint (Nm or N). +# +# Each joint is uniquely identified by its name +# The header specifies the time at which the joint states were recorded. All the joint states +# in one message have to be recorded at the same time. +# +# This message consists of a multiple arrays, one for each part of the joint state. +# The goal is to make each of the fields optional. When e.g. your joints have no +# effort associated with them, you can leave the effort array empty. +# +# All arrays in this message should have the same size, or be empty. +# This is the only way to uniquely associate the joint name with the correct +# states. + + +Header header + +string[] name +float64[] position +float64[] velocity +float64[] effort diff --git a/src/hal/components/cros/samples/rosdb/std_msgs/String.msg b/src/hal/components/cros/samples/rosdb/std_msgs/String.msg new file mode 100644 index 00000000..ae721739 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/std_msgs/String.msg @@ -0,0 +1 @@ +string data diff --git a/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectory.msg b/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectory.msg new file mode 100644 index 00000000..99998b91 --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectory.msg @@ -0,0 +1,3 @@ +Header header +string[] joint_names +JointTrajectoryPoint[] points \ No newline at end of file diff --git a/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectoryPoint.msg b/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectoryPoint.msg new file mode 100644 index 00000000..55fd3c0a --- /dev/null +++ b/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectoryPoint.msg @@ -0,0 +1,9 @@ +# Each trajectory point specifies either positions[, velocities[, accelerations]] +# or positions[, effort] for the trajectory to be executed. +# All specified values are in the same order as the joint names in JointTrajectory.msg + +float64[] positions +float64[] velocities +float64[] accelerations +float64[] effort +duration time_from_start diff --git a/src/hal/components/cros/samples/talker.c b/src/hal/components/cros/samples/talker.c new file mode 100644 index 00000000..b7a6da99 --- /dev/null +++ b/src/hal/components/cros/samples/talker.c @@ -0,0 +1,152 @@ +/*! \file talker.c + * \brief This file is an example of cROS usage implementing a publisher and a service caller, which + * can be used together with listener to prove the operation of message transmissions and service + * calls between two nodes. + * + * It creates a publisher to the topic /chatter. Each 100ms the callback function callback_pub() is + * executed composing a string that is sent to the subscriber through a message of this topic. + * This node also implements a caller of the service /sum. Each 200ms the service is called: + * First the callback function callback_srv_add_two_ints is executed with call_resp_flag parameter set to 0, + * in this way two 64bit integers are generated by the function, which are sent to the service provider as + * service call parameters. The result of the service call (sum of the integers) is sent to the service + * caller and the callback function is executed again receiving the result with the call_resp_flag + * parameter set to 1. + * When the number of service calls or published messages is 10 the ROS node exits and the program finishes. + */ + +#include +#include +#include + +#ifdef _WIN32 +//# define WIN32_LEAN_AND_MEAN +//# include +# include + +# define DIR_SEPARATOR_STR "\\" +#else +# include + +# define DIR_SEPARATOR_STR "/" +#endif + +#include "cros.h" +#include "cros_clock.h" + +#define ROS_MASTER_PORT 11311 +#define ROS_MASTER_ADDRESS "127.0.0.1" + +CrosNode *node; //! Pointer to object storing the ROS node. This object includes all the ROS node state variables +unsigned char exit_flag = 0; //! ROS node loop exit flag. When set to 1 the cRosNodeStart() function exits + +// This callback will be invoked when it's our turn to publish a new message +static CallbackResponse callback_pub(cRosMessage *message, void* data_context) +{ + static int pub_count = 0; + char buf[1024]; + // We need to index into the message structure and then assign to fields + cRosMessageField *data_field; + + data_field = cRosMessageGetField(message, "data"); + if(data_field) + { + snprintf(buf, sizeof(buf), "hello world %d", pub_count); + if(cRosMessageSetFieldValueString(data_field, buf) == 0) + { + ROS_INFO(node, "%s\n", buf); + } + } + + if(++pub_count > 10) exit_flag=1; + + return 0; // 0=success +} + +static CallbackResponse callback_srv_add_two_ints(cRosMessage *request, cRosMessage *response, int call_resp_flag, void* context) +{ + static int call_count = 0; + + if(!call_resp_flag) // Check if this callback function has been called to provide the service call arguments or to collect the response + { + cRosMessageField *a_field = cRosMessageGetField(request, "a"); + cRosMessageField *b_field = cRosMessageGetField(request, "b"); + if(a_field != NULL && b_field != NULL) + { + a_field->data.as_int64=10; + b_field->data.as_int64=call_count; + ROS_INFO(node, "Service add 2 ints call arguments: {a: %lld, b: %lld}\b\n", (long long)a_field->data.as_int64, (long long)b_field->data.as_int64); + } + } + else // Service call response available + { + cRosMessageField *sum_field = cRosMessageGetField(response, "sum"); + if(sum_field != NULL) + ROS_INFO(node, "Service add 2 ints response: %lld (call_count: %i)\n", (long long)sum_field->data.as_int64, call_count++); + } + + if(call_count > 10) exit_flag=1; + return 0; // 0=success +} + +int main(int argc, char **argv) +{ + // We need to tell our node where to find the .msg files that we'll be using + char path[4097]; + const char *node_name; + cRosErrCodePack err_cod; + int pubidx, svcidx; + + if(argc>1) + node_name=argv[1]; + else + node_name="/talker"; // Default node name if no command-line parameters are specified + getcwd(path, sizeof(path)); + strncat(path, "/rosdb", sizeof(path) - strlen(path) - 1); + // Create a new node and tell it to connect to roscore in the usual place + node = cRosNodeCreate(node_name, "127.0.0.1", ROS_MASTER_ADDRESS, ROS_MASTER_PORT, path); + + // Create a publisher to topic /chatter of type "std_msgs/String" and request that the associated callback be invoked every 100ms (10Hz) + err_cod = cRosApiRegisterPublisher(node, "/chatter","std_msgs/String", 1000, callback_pub, NULL, NULL, &pubidx); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterPublisher() failed; did you run this program one directory above 'rosdb'?"); + cRosNodeDestroy( node ); + return EXIT_FAILURE; + } + + // Create a service caller named /sum of type "roscpp_tutorials/TwoInts" and request that the associated callback be invoked every 200ms (5Hz) + err_cod = cRosApiRegisterServiceCaller(node,"/sum","roscpp_tutorials/TwoInts", 2000, callback_srv_add_two_ints, NULL, NULL, 1, 1, &svcidx); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterServiceCaller() failed; did you run this program one directory above 'rosdb'?"); + cRosNodeDestroy( node ); + return EXIT_FAILURE; + } + + printf("Node TCPROS port: %i\n", node->tcpros_port); + + // Run the main loop + uint64_t start_time, end_time; + unsigned long elapsed_time; + + start_time = cRosClockGetTimeMs(); + err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); + end_time = cRosClockGetTimeMs(); + + elapsed_time = (unsigned long)(end_time - start_time); + + printf("Elapsed time: %lums\n", elapsed_time); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + + + // All done: free memory and unregister from ROS master + err_cod=cRosNodeDestroy( node ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} diff --git a/src/hal/components/cros/src/cros_api.c b/src/hal/components/cros/src/cros_api.c new file mode 100644 index 00000000..7d71a324 --- /dev/null +++ b/src/hal/components/cros/src/cros_api.c @@ -0,0 +1,2305 @@ +#include +#include +#include // for PATH_MAX + +#include "cros_defs.h" +#include "cros_api.h" +#include "cros_api_internal.h" +#include "cros_message_internal.h" +#include "cros_service.h" +#include "cros_service_internal.h" +#include "cros_message_queue.h" +#include "xmlrpc_process.h" + +#ifdef _WIN32 +# define OS_MAX_PATH _MAX_PATH +#else +# define OS_MAX_PATH PATH_MAX +#endif + +static LookupNodeResult * fetchLookupNodeResult(XmlrpcParamVector *response); +static GetPublishedTopicsResult * fetchGetPublishedTopicsResult(XmlrpcParamVector *response); +static GetTopicTypesResult * fetchGetTopicTypesResult(XmlrpcParamVector *response); +static GetSystemStateResult * fetchGetSystemStateResult(XmlrpcParamVector *response); +static GetUriResult * fetchGetUriResult(XmlrpcParamVector *response); +static LookupServiceResult * fetchLookupServiceResult(XmlrpcParamVector *response); +static GetBusStatsResult * fetchGetBusStatsResult(XmlrpcParamVector *response); +static GetBusInfoResult * fetchGetBusInfoResult(XmlrpcParamVector *response); +static GetMasterUriResult * fetchGetMasterUriResult(XmlrpcParamVector *response); +static ShutdownResult * fetchShutdownResult(XmlrpcParamVector *response); +static GetPidResult * fetchGetPidResult(XmlrpcParamVector *response); +static GetSubscriptionsResult * fetchGetSubscriptionsResult(XmlrpcParamVector *response); +static GetPublicationsResult * fetchGetPublicationsResult(XmlrpcParamVector *response); +static DeleteParamResult * fetchDeleteParamResult(XmlrpcParamVector *response); +static SetParamResult * fetchSetParamResult(XmlrpcParamVector *response); +static GetParamResult * fetchGetParamResult(XmlrpcParamVector *response); +static SearchParamResult * fetchSearchParamResult(XmlrpcParamVector *response); +static HasParamResult * fetchHasParamResult(XmlrpcParamVector *response); +static GetParamNamesResult * fetchGetParamNamesResult(XmlrpcParamVector *response); + +static void freeLookupNodeResult(LookupNodeResult *result); +static void freeGetPublishedTopicsResult(GetPublishedTopicsResult *result); +static void freeGetTopicTypesResult(GetTopicTypesResult *result); +static void freeGetSystemStateResult(GetSystemStateResult *result); +static void freeGetUriResult(GetUriResult *result); +static void freeLookupServiceResult(LookupServiceResult *result); +static void freeGetBusStatsResult(GetBusStatsResult *result); +static void freeGetBusInfoResult(GetBusInfoResult *result); +static void freeGetMasterUriResult(GetMasterUriResult *result); +static void freeShutdownResult(ShutdownResult *result); +static void freeGetPidResult(GetPidResult *result); +static void freeGetSubscriptionsResult(GetSubscriptionsResult *result); +static void freeGetPublicationsResult(GetPublicationsResult *result); +static void freeDeleteParamResult(DeleteParamResult *result); +static void freeSetParamResult(SetParamResult *result); +static void freeGetParamResult(GetParamResult *result); +static void freeSearchParamResult(SearchParamResult *result); +static void freeHasParamResult(HasParamResult *result); +static void freeGetParamNamesResult(GetParamNamesResult *result); + +typedef enum ProviderType +{ + CROS_SUBSCRIBER, + CROS_PUBLISHER, + CROS_SERVICE_CALLER, + CROS_SERVICE_PROVIDER +} ProviderType; + +typedef struct ProviderContext +{ + ProviderType type; //! Type of role: provider, subscriber, service provider or service caller + cRosMessage *incoming; //! Message that has been received + cRosMessage *outgoing; //! Message that will be sent + char *message_definition; + char *md5sum; + void *api_callback; //! The application-defined callback function called to generate outgoing data or to handle the received data + NodeStatusApiCallback status_api_callback; //! The application-defined callback function called when the state of the role has chnaged + cRosMessageQueue *msg_queue; //! It is just a reference to the queue declared in node. For the publisher: it is msgs to send. For the subscriber: it is msgs received. For the svc caller: it is first svc request and then svc response + void *context; //! Context parameter specified by the application and that will be passed to the application-defined callback functions +} ProviderContext; + +static void initProviderContext(ProviderContext *context) +{ + context->type=-1; + context->incoming=NULL; + context->outgoing=NULL; + context->message_definition=NULL; + context->md5sum=NULL; + context->status_api_callback=NULL; + context->api_callback=NULL; + context->msg_queue=NULL; + context->context=NULL; +} + +static void freeProviderContext(ProviderContext *context) +{ + if(context != NULL) + { + cRosMessageFree(context->incoming); + cRosMessageFree(context->outgoing); + free(context->message_definition); + free(context->md5sum); + free(context); + } +} + +static cRosErrCodePack newProviderContext(const char *provider_path, ProviderType type, ProviderContext **context_ptr) +{ + cRosErrCodePack ret_err; + + ret_err = CROS_SUCCESS_ERR_PACK; // Default return value: success + + ProviderContext *context = (ProviderContext *)malloc(sizeof(ProviderContext)); + if (context == NULL) + return CROS_MEM_ALLOC_ERR; + + initProviderContext(context); + context->type = type; + context->md5sum = (char *)calloc(sizeof(char), 33);// 32 chars + '\0'; + if (context->md5sum == NULL) + { + free(context); + return CROS_MEM_ALLOC_ERR; + } + + switch(type) + { + case CROS_SUBSCRIBER: + { + ret_err = cRosMessageNewBuild(NULL, provider_path, &context->incoming); + if (ret_err == CROS_SUCCESS_ERR_PACK) + { + strcpy(context->md5sum, context->incoming->md5sum); + context->message_definition = strdup(context->incoming->msgDef->plain_text); // Alloc new mem so that it can be freed independently + } + break; + } + case CROS_PUBLISHER: + { + ret_err = cRosMessageNewBuild(NULL, provider_path, &context->outgoing); + if (ret_err == CROS_SUCCESS_ERR_PACK) + { + strcpy(context->md5sum, context->outgoing->md5sum); + context->message_definition = strdup(context->outgoing->msgDef->plain_text); + } + break; + } + case CROS_SERVICE_PROVIDER: + { + ret_err = cRosServiceBuildInner(&context->incoming, &context->outgoing, NULL, context->md5sum, provider_path); + break; + } + case CROS_SERVICE_CALLER: + { + ret_err = cRosServiceBuildInner(&context->outgoing, &context->incoming, &context->message_definition, context->md5sum, provider_path); + break; + } + default: + { + PRINT_ERROR ( "newProviderContext() : Unknown ProviderType specified\n" ); + ret_err = CROS_BAD_PARAM_ERR; + } + } + + if(ret_err == CROS_SUCCESS_ERR_PACK) + *context_ptr = context; // No error occurred: return the created context + else + freeProviderContext(context); // An error occurred: free the allocated memory and return the error code + + return ret_err; +} + +cRosErrCodePack cRosNodeSerializeOutgoingMessage(DynBuffer *buffer, void *context_) +{ + cRosErrCodePack ret_err; + ProviderContext *context = (ProviderContext *)context_; + + ret_err = cRosMessageSerialize(context->outgoing, buffer); + return(ret_err); +} + +cRosErrCodePack cRosNodeDeserializeIncomingPacket(DynBuffer *buffer, void *context_) +{ + cRosErrCodePack ret_err; + ProviderContext *context = (ProviderContext *)context_; + + ret_err = cRosMessageDeserialize(context->incoming, buffer); + return(ret_err); +} + +cRosErrCodePack cRosNodePublisherCallback(void *context_) +{ + cRosErrCodePack ret_err; + ProviderContext *context = (ProviderContext *)context_; + + ret_err = CROS_SUCCESS_ERR_PACK; // Default return value + + if(cRosMessageQueueUsage(context->msg_queue) > 0) // An inmediate message is waiting to be sent + { + if(cRosMessageQueueExtract(context->msg_queue, context->outgoing) != 0) + ret_err = CROS_EXTRACT_MSG_INT_ERR; + } + else // It is time to send a periodic message + { + CallbackResponse ret_cb; + + // Cast to the appropriate public api callback and invoke it on the user context + PublisherApiCallback publisher_user_callback = (PublisherApiCallback)context->api_callback; + if(publisher_user_callback != NULL) + { + ret_cb = publisher_user_callback(context->outgoing, context->context); // Use the callback if it is available and no msg is waiting in the queue + if(ret_cb != 0) + ret_err = CROS_TOP_PUB_CALLBACK_ERR; + } + } + + if(ret_err != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(ret_err, "cRosNodePublisherCallback() failed encoding the packet to send"); + + return ret_err; +} + +cRosErrCodePack cRosNodeSubscriberCallback(void *context_) +{ + cRosErrCodePack ret_err; + ProviderContext *context = (ProviderContext *)context_; + cRosMessageQueueAdd(context->msg_queue, context->incoming); + + // Cast to the appropriate public api callback and invoke it on the user context + SubscriberApiCallback subs_user_callback_fn = (SubscriberApiCallback)context->api_callback; + if(subs_user_callback_fn != NULL) + { + CallbackResponse ret_cb = subs_user_callback_fn(context->incoming, context->context); + if(ret_cb == 0) + ret_err = CROS_SUCCESS_ERR_PACK; + else + ret_err = CROS_TOP_SUB_CALLBACK_ERR; + } + else + ret_err = CROS_SUCCESS_ERR_PACK; + + return ret_err; +} + +cRosErrCodePack cRosNodeServiceCallerCallback(int call_resp_flag, void* contex_) +{ + cRosErrCodePack ret_err; + CallbackResponse ret_cb; + ServiceCallerApiCallback svc_call_user_callback_fn; + ProviderContext *context = (ProviderContext *)contex_; + + svc_call_user_callback_fn = (ServiceCallerApiCallback)context->api_callback; + + if(call_resp_flag) // Process service response + { + // If msg_queue is empty, this is a periodic call (send msg response to callback fn), otherwise this is a non-periodic call (put msg response in the queue) + if(cRosMessageQueueUsage(context->msg_queue) == 0) // Periodic service call + { + if(svc_call_user_callback_fn != NULL) + { + ret_cb = svc_call_user_callback_fn(context->outgoing, context->incoming, call_resp_flag, context->context); + if(ret_cb != 0) // The callback indicated and error in the return value when processing the service response + ret_err=CROS_SVC_RES_CALLBACK_ERR; + else + ret_err = CROS_SUCCESS_ERR_PACK; + } + else + ret_err = CROS_SUCCESS_ERR_PACK; + } + else // Non-periodic service call + { + if(cRosMessageQueueAdd(context->msg_queue, context->incoming) == 0) // Add response msg to the queue (in case the svc was called non periodically) + ret_err = CROS_SUCCESS_ERR_PACK; // A new message has been aded to the queue to later store the received response + else + ret_err = CROS_MEM_ALLOC_ERR; + } + if(ret_err != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(ret_err, "cRosNodeServiceCallerCallback() failed decoding the received service response packet"); + } + else // Generate service request + { + // If msg_queue is empty, this is a periodic call (get the msg request from the callback fn), otherwise this is a non-periodic call (get msg request from the queue) + if(cRosMessageQueueUsage(context->msg_queue) == 0) // Periodic service call + { + if(svc_call_user_callback_fn != NULL) + { + ret_cb = svc_call_user_callback_fn(context->outgoing, context->incoming, call_resp_flag, context->context); + if(ret_cb == 0) // The callback returned success when generating the service request: send the service request + ret_err = CROS_SUCCESS_ERR_PACK; + else // The callback indicated and error in the return value when generating the service request + ret_err = CROS_SVC_REQ_CALLBACK_ERR; + } + else + ret_err = CROS_SUCCESS_ERR_PACK; + } + else // Non-periodic service call + ret_err = (cRosMessageQueueGet(context->msg_queue, context->outgoing) == 0)?CROS_SUCCESS_ERR_PACK:CROS_EXTRACT_MSG_INT_ERR; // Copy the message request from the queue msg + + if(ret_err != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(ret_err, "cRosNodeServiceCallerCallback() failed getting the service request packet"); + } + + return ret_err; +} + +cRosErrCodePack cRosNodeServiceProviderCallback(void *context_) +{ + cRosErrCodePack ret_err; + ProviderContext *context = (ProviderContext *)context_; + + ServiceProviderApiCallback serviceProviderApiCallback = (ServiceProviderApiCallback)context->api_callback; + CallbackResponse ret_cb = serviceProviderApiCallback(context->incoming, context->outgoing, context->context); + + if(ret_cb == 0) + ret_err = CROS_SUCCESS_ERR_PACK; + else + ret_err = CROS_SVC_SER_CALLBACK_ERR; + + return ret_err; +} + +void cRosNodeStatusCallback(CrosNodeStatusUsr *status, void* context_) +{ + ProviderContext *context = (ProviderContext *)context_; + if(context->status_api_callback != NULL) // If the application defined a status callback function, call it + context->status_api_callback(status, context->context); +} + +cRosErrCodePack cRosApiRegisterServiceCaller(CrosNode *node, const char *service_name, const char *service_type, int loop_period, + ServiceCallerApiCallback callback, NodeStatusApiCallback status_callback, void *context, int persistent, int tcp_nodelay, int *svcidx_ptr) +{ + cRosErrCodePack ret_err; + char path[OS_MAX_PATH]; + ProviderContext *nodeContext = NULL; + int svcidx; + + if(loop_period >= 0 && callback == NULL) + return CROS_BAD_PARAM_ERR; + + getSrvFilePath(node, path, OS_MAX_PATH, service_type); + ret_err = newProviderContext(path, CROS_SERVICE_CALLER, &nodeContext); + if (ret_err == CROS_SUCCESS_ERR_PACK) + { + nodeContext->api_callback = callback; + nodeContext->status_api_callback = status_callback; + nodeContext->context = context; + + // NB: Pass the private ProviderContext to the private api, not the user context + svcidx = cRosNodeRegisterServiceCaller(node, nodeContext->message_definition, service_name, service_type, nodeContext->md5sum, + loop_period, nodeContext, persistent, tcp_nodelay); + if(svcidx >= 0) // Success + { + if(svcidx_ptr != NULL) + *svcidx_ptr = svcidx; // Return the index of the created service caller + nodeContext->msg_queue = &node->service_callers[svcidx].msg_queue; + } + else + ret_err=CROS_MEM_ALLOC_ERR; + } + return ret_err; +} + +void cRosApiReleaseServiceCaller(CrosNode *node, int svcidx) +{ + ServiceCallerNode *svc = &node->service_callers[svcidx]; + ProviderContext *context = (ProviderContext *)svc->context; + freeProviderContext(context); + cRosNodeReleaseServiceCaller(svc); +} + +cRosErrCodePack cRosApiRegisterServiceProvider(CrosNode *node, const char *service_name, const char *service_type, + ServiceProviderApiCallback callback, NodeStatusApiCallback status_callback, void *context, int *svcidx_ptr) +{ + cRosErrCodePack ret_err; + char path[OS_MAX_PATH]; + ProviderContext *nodeContext = NULL; + int svcidx; + + if (callback == NULL) + return CROS_BAD_PARAM_ERR; + + getSrvFilePath(node, path, OS_MAX_PATH, service_type); + ret_err = newProviderContext(path, CROS_SERVICE_PROVIDER, &nodeContext); + if (ret_err == CROS_SUCCESS_ERR_PACK) + { + nodeContext->api_callback = callback; + nodeContext->status_api_callback = status_callback; + nodeContext->context = context; + + // NB: Pass the private ProviderContext to the private api, not the user context + svcidx = cRosNodeRegisterServiceProvider(node, service_name, service_type, nodeContext->md5sum, nodeContext); + if(svcidx >= 0) // Success + { + if(svcidx_ptr != NULL) + *svcidx_ptr = svcidx; // Return the index of the created service provider + } + else + ret_err=CROS_MEM_ALLOC_ERR; + } + return ret_err; +} + +cRosErrCodePack cRosApiUnregisterServiceProvider(CrosNode *node, int svcidx) +{ + int ret_err; + if (svcidx < 0 || svcidx >= CN_MAX_SERVICE_PROVIDERS) + return CROS_BAD_PARAM_ERR; + + ServiceProviderNode *service = &node->service_providers[svcidx]; + if (service->service_name == NULL) + return CROS_TOPIC_SUB_IND_ERR; + + ret_err = cRosNodeUnregisterServiceProvider(node, svcidx); + + return (ret_err != -1)? CROS_SUCCESS_ERR_PACK: CROS_UNSPECIFIED_ERR; +} + +void cRosApiReleaseServiceProvider(CrosNode *node, int svcidx) +{ + ServiceProviderNode *svc = &node->service_providers[svcidx]; + ProviderContext *context = (ProviderContext *)svc->context; + freeProviderContext(context); + cRosNodeReleaseServiceProvider(svc); +} + +cRosErrCodePack cRosApiRegisterSubscriber(CrosNode *node, const char *topic_name, const char *topic_type, + SubscriberApiCallback callback, NodeStatusApiCallback status_callback, void *context, int tcp_nodelay, int *subidx_ptr) +{ + cRosErrCodePack ret_err; + char path[OS_MAX_PATH]; + ProviderContext *nodeContext = NULL; + int subidx; + + cRosGetMsgFilePath(node, path, OS_MAX_PATH, topic_type); + ret_err = newProviderContext(path, CROS_SUBSCRIBER, &nodeContext); + if (ret_err == CROS_SUCCESS_ERR_PACK) + { + nodeContext->api_callback = callback; + nodeContext->status_api_callback = status_callback; + nodeContext->context = context; + + // NB: Pass the private ProviderContext to the private api, not the user context + subidx = cRosNodeRegisterSubscriber(node, nodeContext->message_definition, topic_name, topic_type, + nodeContext->md5sum, nodeContext, tcp_nodelay); + if(subidx >= 0) // Success + { + nodeContext->msg_queue = &node->subs[subidx].msg_queue; // Allow the callback functions to access the msg queue + if(subidx_ptr != NULL) + *subidx_ptr = subidx; // Return the index of the created service caller + } + else + ret_err=CROS_MEM_ALLOC_ERR; + } + return ret_err; +} + +cRosErrCodePack cRosApiUnregisterSubscriber(CrosNode *node, int subidx) +{ + int ret_err; + if (subidx < 0 || subidx >= CN_MAX_SUBSCRIBED_TOPICS) + return CROS_BAD_PARAM_ERR; + + SubscriberNode *sub = &node->subs[subidx]; + if (sub->topic_name == NULL) + return CROS_TOPIC_SUB_IND_ERR; + + ret_err = cRosNodeUnregisterSubscriber(node, subidx); + + return (ret_err != -1)? CROS_SUCCESS_ERR_PACK: CROS_UNSPECIFIED_ERR; +} + +void cRosApiReleaseSubscriber(CrosNode *node, int subidx) +{ + SubscriberNode *sub = &node->subs[subidx]; + ProviderContext *context = (ProviderContext *)sub->context; + freeProviderContext(context); + cRosNodeReleaseSubscriber(sub); +} + +cRosErrCodePack cRosApiRegisterPublisher(CrosNode *node, const char *topic_name, const char *topic_type, int loop_period, + PublisherApiCallback callback, NodeStatusApiCallback status_callback, void *context, int *pubidx_ptr) +{ + cRosErrCodePack ret_err; + char path[OS_MAX_PATH]; + ProviderContext *nodeContext = NULL; + int pubidx; + + if(loop_period >= 0 && callback == NULL) + return CROS_BAD_PARAM_ERR; + + cRosGetMsgFilePath(node, path, OS_MAX_PATH, topic_type); + ret_err = newProviderContext(path, CROS_PUBLISHER, &nodeContext); + if (ret_err == CROS_SUCCESS_ERR_PACK) + { + nodeContext->api_callback = callback; + nodeContext->status_api_callback = status_callback; + nodeContext->context = context; + + // NB: Pass the private ProviderContext to the private api, not the user context + pubidx = cRosNodeRegisterPublisher(node, nodeContext->message_definition, topic_name, topic_type, + nodeContext->md5sum, loop_period, nodeContext); + if(pubidx >= 0) // Success + { + // Allow the callback functions to access the msg queue and send-now flag + nodeContext->msg_queue = &node->pubs[pubidx].msg_queue; + if(pubidx_ptr != NULL) + *pubidx_ptr = pubidx; // Return the index of the created service caller + } + else + ret_err=CROS_MEM_ALLOC_ERR; + } + return ret_err; +} + +cRosErrCodePack cRosApiUnregisterPublisher(CrosNode *node, int pubidx) +{ + int ret_err; + if (pubidx < 0 || pubidx >= CN_MAX_PUBLISHED_TOPICS) + return CROS_BAD_PARAM_ERR; + + PublisherNode *pub = &node->pubs[pubidx]; + if (pub->topic_name == NULL) + return CROS_TOPIC_PUB_IND_ERR; + + ret_err = cRosNodeUnregisterPublisher(node, pubidx); + + return (ret_err != -1)? CROS_SUCCESS_ERR_PACK: CROS_UNSPECIFIED_ERR; +} + +void cRosApiReleasePublisher(CrosNode *node, int pubidx) +{ + PublisherNode *pub = &node->pubs[pubidx]; + ProviderContext *context = (ProviderContext *)pub->context; + freeProviderContext(context); + cRosNodeReleasePublisher(pub); +} + +cRosErrCodePack cRosApicRosApiLookupNode(CrosNode *node, const char *node_name, LookupNodeCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApicRosApiLookupNode() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_LOOKUP_NODE; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchLookupNodeResult; + call->free_result_callback = (FreeResultCallback)freeLookupNodeResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, node_name); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetPublishedTopics(CrosNode *node, const char *subgraph, GetPublishedTopicsCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetPublishedTopics() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_PUBLISHED_TOPICS; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetPublishedTopicsResult; + call->free_result_callback = (FreeResultCallback)freeGetPublishedTopicsResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, subgraph == NULL ? "" : subgraph); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetTopicTypes(CrosNode *node, GetTopicTypesCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetTopicTypes() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_TOPIC_TYPES; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetTopicTypesResult; + call->free_result_callback = (FreeResultCallback)freeGetTopicTypesResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetSystemState(CrosNode *node, GetSystemStateCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetSystemState() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_SYSTEM_STATE; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetSystemStateResult; + call->free_result_callback = (FreeResultCallback)freeGetSystemStateResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetUri(CrosNode *node, GetUriCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetUri() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_URI; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetUriResult; + call->free_result_callback = (FreeResultCallback)freeGetUriResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiLookupService(CrosNode *node, const char *service, LookupServiceCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiLookupService() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_LOOKUP_SERVICE; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchLookupServiceResult; + call->free_result_callback = (FreeResultCallback)freeLookupServiceResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, service); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetBusStats(CrosNode *node, const char* host, int port, + GetBusStatsCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetBusStats() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_BUS_STATS; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetBusStatsResult; + call->free_result_callback = (FreeResultCallback)freeGetBusStatsResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueSlaveApiCall(node, call, host, port); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetBusInfo(CrosNode *node, const char* host, int port, + GetBusInfoCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetBusInfo() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_BUS_INFO; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetBusInfoResult; + call->free_result_callback = (FreeResultCallback)freeGetBusInfoResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueSlaveApiCall(node, call, host, port); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetMasterUri(CrosNode *node, const char* host, int port, + GetMasterUriCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_MASTER_URI; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetMasterUriResult; + call->free_result_callback = (FreeResultCallback)freeGetMasterUriResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueSlaveApiCall(node, call, host, port); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiShutdown(CrosNode *node, const char* host, int port, const char *msg, + GetMasterUriCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiShutdown() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_SHUTDOWN; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchShutdownResult; + call->free_result_callback = (FreeResultCallback)freeShutdownResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, msg == NULL ? "" : msg); + + caller_id = enqueueSlaveApiCall(node, call, host, port); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetPid(CrosNode *node, const char* host, int port, + GetPidCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetPid() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_PID; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetPidResult; + call->free_result_callback = (FreeResultCallback)freeGetPidResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueSlaveApiCall(node, call, host, port); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetSubscriptions(CrosNode *node, const char* host, int port, + GetSubscriptionsCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_SUBSCRIPTIONS; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetSubscriptionsResult; + call->free_result_callback = (FreeResultCallback)freeGetSubscriptionsResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueSlaveApiCall(node, call, host, port); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetPublications(CrosNode *node, const char* host, int port, + GetSubscriptionsCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetPublications() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_PUBLICATIONS; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetPublicationsResult; + call->free_result_callback = (FreeResultCallback)freeGetPublicationsResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueSlaveApiCall(node, call, host, port); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiDeleteParam(CrosNode *node, const char *key, DeleteParamCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiDeleteParam() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_DELETE_PARAM; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchDeleteParamResult; + call->free_result_callback = (FreeResultCallback)freeDeleteParamResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, key); + + caller_id=enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiSetParam(CrosNode *node, const char *key, XmlrpcParam *value, SetParamCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id, vec_size; + + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiSetParam() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + XmlrpcParam param; + int rc = xmlrpcParamCopy(¶m, value); + if (rc == -1) + { + PRINT_ERROR ( "cRosApiSetParam() : Can't allocate memory\n"); + freeRosApiCall(call); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_SET_PARAM; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchSetParamResult; + call->free_result_callback = (FreeResultCallback)freeSetParamResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, key); + vec_size = xmlrpcParamVectorPushBack(&call->params, ¶m); + if (vec_size == -1) + { + freeRosApiCall(call); + xmlrpcParamRelease(¶m); + return CROS_MEM_ALLOC_ERR; + } + + caller_id=enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetParam(CrosNode *node, const char *key, GetParamCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetParam() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_PARAM; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetParamResult; + call->free_result_callback = (FreeResultCallback)freeGetParamResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, key); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiSearchParam(CrosNode *node, const char *key, SearchParamCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiSearchParam() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_SEARCH_PARAM; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchSearchParamResult; + call->free_result_callback = (FreeResultCallback)freeSearchParamResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, key); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiHasParam(CrosNode *node, const char *key, HasParamCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiHasParam() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_HAS_PARAM; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchHasParamResult; + call->free_result_callback = (FreeResultCallback)freeHasParamResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + xmlrpcParamVectorPushBackString(&call->params, key); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosApiGetParamNames(CrosNode *node, GetParamNamesCallback callback, void *context, int *caller_id_ptr) +{ + int caller_id; + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosApiGetParamNames() : Can't allocate memory\n"); + return CROS_MEM_ALLOC_ERR; + } + + call->method = CROS_API_GET_PARAM_NAMES; + call->result_callback = (ResultCallback)callback; + call->context_data = context; + call->fetch_result_callback = (FetchResultCallback)fetchGetParamNamesResult; + call->free_result_callback = (FreeResultCallback)freeGetParamNamesResult; + + xmlrpcParamVectorPushBackString(&call->params, node->name); + + caller_id = enqueueMasterApiCall(node, call); + if(caller_id_ptr != NULL) + *caller_id_ptr = caller_id; + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +XmlrpcParam *GetMethodResponseStatus(XmlrpcParamVector *response, int *status_code, char **status_msg) +{ + XmlrpcParam *resp_param; + + resp_param = xmlrpcParamVectorAt(response, 0); + if (resp_param != NULL) + { + XmlrpcParam *status_code_param; + status_code_param = xmlrpcParamArrayGetParamAt(resp_param, 0); // resp_param must be a vector or structure in order to be accessed by index + if (status_code_param != NULL) + { + if( status_code_param->type == XMLRPC_PARAM_INT ) + { + XmlrpcParam *status_msg_param; + + *status_code = status_code_param->data.as_int; + status_msg_param = xmlrpcParamArrayGetParamAt(resp_param, 1); + if (status_msg_param != NULL) + { + if( status_msg_param->type == XMLRPC_PARAM_STRING ) + *status_msg = strdup(status_msg_param->data.as_string); + else + *status_msg = NULL; // status_msg = NULL: No human-readable string describing the returned status has been found + } + else + *status_msg = NULL; // The array or structure returned in the ROS master response does not contain a second element (the human-readable string describing the status) + } + else + { + PRINT_ERROR ( "GetMethodResponseStatus() : The status code integer cannot be found in the responsed returned by the ROS master.\n"); + resp_param = NULL; + } + } + else + { + PRINT_ERROR ( "GetMethodResponseStatus() : The responsed returned by the ROS master does not contain a non-empty array or structure.\n"); + resp_param = NULL; + } + } + else + PRINT_ERROR ( "GetMethodResponseStatus() : The ROS master returned an XML response with an unexpected format.\n"); + + return(resp_param); +} + +LookupNodeResult * fetchLookupNodeResult(XmlrpcParamVector *response) +{ + LookupNodeResult *ret = (LookupNodeResult *)calloc(1, sizeof(LookupNodeResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchLookupNodeResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeLookupNodeResult(ret); + return NULL; + } + + XmlrpcParam* uri = xmlrpcParamArrayGetParamAt(array, 2); + ret->uri = strdup(uri->data.as_string); + if (ret->uri == NULL) + { + freeLookupNodeResult(ret); + return NULL; + } + + return ret; +} + +GetPublishedTopicsResult * fetchGetPublishedTopicsResult(XmlrpcParamVector *response) +{ + int it; + + GetPublishedTopicsResult *ret = (GetPublishedTopicsResult *)calloc(1, sizeof(GetPublishedTopicsResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetPublishedTopicsResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetPublishedTopicsResult(ret); + return NULL; + } + + XmlrpcParam* topics = xmlrpcParamArrayGetParamAt(array, 2); + ret->topics = (struct TopicTypePair *)calloc(topics->array_n_elem, sizeof(struct TopicTypePair)); + if (ret->topics == NULL) + goto clean; + ret->topic_count = topics->array_n_elem; + + for (it = 0; it < topics->array_n_elem; it++) + { + struct TopicTypePair *pair = &ret->topics[it]; + XmlrpcParam* pair_xml = xmlrpcParamArrayGetParamAt(topics, it); + XmlrpcParam* topic = xmlrpcParamArrayGetParamAt(pair_xml, 0); + pair->topic = (char *)malloc(strlen(topic->data.as_string) + 1); + if (pair->topic == NULL) + goto clean; + strcpy(pair->topic, topic->data.as_string); + + XmlrpcParam* type = xmlrpcParamArrayGetParamAt(pair_xml, 1); + pair->type = (char *)malloc(strlen(type->data.as_string) + 1); + if (pair->type == NULL) + goto clean; + strcpy(pair->type, type->data.as_string); + } + + return ret; + +clean: + freeGetPublishedTopicsResult(ret); + return NULL; +} + +GetTopicTypesResult * fetchGetTopicTypesResult(XmlrpcParamVector *response) +{ + int it; + + GetTopicTypesResult *ret = (GetTopicTypesResult *)calloc(1, sizeof(GetTopicTypesResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetTopicTypesResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetTopicTypesResult(ret); + return NULL; + } + + XmlrpcParam* topics = xmlrpcParamArrayGetParamAt(array, 2); + ret->topics = (struct TopicTypePair *)calloc(topics->array_n_elem, sizeof(struct TopicTypePair)); + if (ret->topics == NULL) + goto clean; + ret->topic_count = topics->array_n_elem; + + for (it = 0; it < topics->array_n_elem; it++) + { + struct TopicTypePair *pair = &ret->topics[it]; + XmlrpcParam* pair_xml = xmlrpcParamArrayGetParamAt(topics, it); + XmlrpcParam* topic = xmlrpcParamArrayGetParamAt(pair_xml, 0); + pair->topic = (char *)malloc(strlen(topic->data.as_string) + 1); + if (pair->topic == NULL) + goto clean; + strcpy(pair->topic, topic->data.as_string); + + XmlrpcParam* type = xmlrpcParamArrayGetParamAt(pair_xml, 1); + pair->type = (char *)malloc(strlen(type->data.as_string) + 1); + if (pair->type == NULL) + goto clean; + strcpy(pair->type, type->data.as_string); + } + + return ret; + +clean: + freeGetTopicTypesResult(ret); + return NULL; +} + +GetSystemStateResult * fetchGetSystemStateResult(XmlrpcParamVector *response) +{ + int it1; + + GetSystemStateResult *ret = (GetSystemStateResult *)calloc(1, sizeof(GetSystemStateResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetSystemStateResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetSystemStateResult(ret); + return NULL; + } + + if (array->array_n_elem < 3) + return ret; + + XmlrpcParam* publishers = xmlrpcParamArrayGetParamAt(array, 2); + ret->publishers = (struct ProviderState *)calloc(publishers->array_n_elem, sizeof(struct ProviderState)); + if (ret->publishers == NULL) + goto clean; + ret->pub_count = publishers->array_n_elem; + + for (it1 = 0; it1 < publishers->array_n_elem; it1++) + { + int it2; + + struct ProviderState *state = &ret->publishers[it1]; + XmlrpcParam* state_xml = xmlrpcParamArrayGetParamAt(publishers, it1); + XmlrpcParam* name = xmlrpcParamArrayGetParamAt(state_xml, 0); + state->provider_name = (char *)malloc(strlen(name->data.as_string) + 1); + if (state->provider_name == NULL) + goto clean; + strcpy(state->provider_name, name->data.as_string); + + XmlrpcParam* users_xml = xmlrpcParamArrayGetParamAt(state_xml, 1); + state->users = (char **)calloc(1, users_xml->array_n_elem * sizeof(char *)); + if (state->users == NULL) + goto clean; + + for (it2 = 0; it2 < users_xml->array_n_elem; it2++) + { + char *user = state->users[it2]; + XmlrpcParam* user_xml = xmlrpcParamArrayGetParamAt(users_xml, it2); + user = (char *)malloc(strlen(user_xml->data.as_string) + 1); + if (user == NULL) + goto clean; + strcpy(user, user_xml->data.as_string); + } + } + + if (array->array_n_elem < 4) + return ret; + + XmlrpcParam* subscribers = xmlrpcParamArrayGetParamAt(array, 3); + ret->subscribers = (struct ProviderState *)calloc(subscribers->array_n_elem, sizeof(struct ProviderState)); + if (ret->subscribers == NULL) + goto clean; + ret->sub_count = subscribers->array_n_elem; + + for (it1 = 0; it1 < subscribers->array_n_elem; it1++) + { + int it2; + + struct ProviderState *state = &ret->subscribers[it1]; + XmlrpcParam* state_xml = xmlrpcParamArrayGetParamAt(subscribers, it1); + XmlrpcParam* name = xmlrpcParamArrayGetParamAt(state_xml, 0); + state->provider_name = (char *)malloc(strlen(name->data.as_string) + 1); + if (state->provider_name == NULL) + goto clean; + strcpy(state->provider_name, name->data.as_string); + + XmlrpcParam* users_xml = xmlrpcParamArrayGetParamAt(state_xml, 1); + state->users = (char **)calloc(users_xml->array_n_elem, sizeof(char *)); + if (state->users == NULL) + goto clean; + + for (it2 = 0; it2 < users_xml->array_n_elem; it2++) + { + char *user = state->users[it2]; + XmlrpcParam* user_xml = xmlrpcParamArrayGetParamAt(users_xml, it2); + user = (char *)malloc(strlen(user_xml->data.as_string) + 1); + if (user == NULL) + goto clean; + strcpy(user, user_xml->data.as_string); + } + } + + if (array->array_n_elem < 5) + return ret; + + XmlrpcParam* services = xmlrpcParamArrayGetParamAt(array, 4); + ret->service_providers = (struct ProviderState *)calloc(services->array_n_elem, sizeof(struct ProviderState)); + if (ret->service_providers == NULL) + goto clean; + ret->svc_count = services->array_n_elem; + + for (it1 = 0; it1 < services->array_n_elem; it1++) + { + int it2; + + struct ProviderState *state = &ret->service_providers[it1]; + XmlrpcParam* state_xml = xmlrpcParamArrayGetParamAt(services, it1); + XmlrpcParam* name = xmlrpcParamArrayGetParamAt(state_xml, 0); + state->provider_name = (char *)malloc(strlen(name->data.as_string) + 1); + if (state->provider_name == NULL) + goto clean; + strcpy(state->provider_name, name->data.as_string); + + XmlrpcParam* users_xml = xmlrpcParamArrayGetParamAt(state_xml, 1); + state->users = (char **)calloc(users_xml->array_n_elem, sizeof(char *)); + if (state->users == NULL) + goto clean; + + for (it2 = 0; it2 < users_xml->array_n_elem; it2++) + { + char *user = state->users[it2]; + XmlrpcParam* user_xml = xmlrpcParamArrayGetParamAt(users_xml, it2); + user = (char *)malloc(strlen(user_xml->data.as_string) + 1); + if (user == NULL) + goto clean; + strcpy(user, user_xml->data.as_string); + } + } + + return ret; + +clean: + freeGetSystemStateResult(ret); + return NULL; +} + +GetUriResult * fetchGetUriResult(XmlrpcParamVector *response) +{ + GetUriResult *ret = (GetUriResult *)calloc(1, sizeof(GetUriResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetUriResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetUriResult(ret); + return NULL; + } + + XmlrpcParam* uri = xmlrpcParamArrayGetParamAt(array, 2); + ret->master_uri = strdup(uri->data.as_string); + if (ret == NULL) + { + freeGetUriResult(ret); + return NULL; + } + + return ret; +} + +LookupServiceResult * fetchLookupServiceResult(XmlrpcParamVector *response) +{ + LookupServiceResult *ret = (LookupServiceResult *)calloc(1, sizeof(LookupServiceResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchLookupServiceResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeLookupServiceResult(ret); + return NULL; + } + + XmlrpcParam* service = xmlrpcParamArrayGetParamAt(array, 2); + ret->service_result = strdup(service->data.as_string); + if (ret == NULL) + { + freeLookupServiceResult(ret); + return NULL; + } + + return ret; +} + +GetBusStatsResult * fetchGetBusStatsResult(XmlrpcParamVector *response) +{ + int it1; + + GetBusStatsResult *ret = (GetBusStatsResult *)calloc(1, sizeof(GetBusStatsResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetBusStatsResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetBusStatsResult(ret); + return NULL; + } + + if (array->array_n_elem < 3) + return ret; + + XmlrpcParam* pubs_stats = xmlrpcParamArrayGetParamAt(array, 2); + ret->stats.pub_stats = (struct TopicPubStats *)calloc(pubs_stats->array_n_elem, sizeof(struct TopicPubStats)); + if (ret->stats.pub_stats == NULL) + goto clean; + ret->stats.pub_stats_count = pubs_stats->array_n_elem; + + for (it1 = 0; it1 < pubs_stats->array_n_elem; it1++) + { + int it2; + + struct TopicPubStats *pub_stats = &ret->stats.pub_stats[it1]; + XmlrpcParam* pub_stats_xml = xmlrpcParamArrayGetParamAt(pubs_stats, it1); + if (pub_stats_xml->array_n_elem < 3) + goto clean; + + XmlrpcParam* name_xml = xmlrpcParamArrayGetParamAt(pub_stats_xml, 0); + pub_stats->topic_name = (char *)malloc(strlen(name_xml->data.as_string) + 1); + if (pub_stats->topic_name == NULL) + goto clean; + strcpy(pub_stats->topic_name, name_xml->data.as_string); + + XmlrpcParam* message_data_sent = xmlrpcParamArrayGetParamAt(pub_stats_xml, 1); + pub_stats->message_data_sent = (size_t)message_data_sent->data.as_int; + + XmlrpcParam* pub_datas = xmlrpcParamArrayGetParamAt(pub_stats_xml, 2); + pub_stats->datas = (struct PubConnectionData *)calloc(pub_datas->array_n_elem, sizeof(struct PubConnectionData)); + if (pub_stats->datas == NULL) + goto clean; + pub_stats->datas_count = pub_datas->array_n_elem; + + for (it2 = 0; it2 < pub_datas->array_n_elem; it2++) + { + struct PubConnectionData *pub_data = &pub_stats->datas[it2]; + XmlrpcParam* pub_data_xml = xmlrpcParamArrayGetParamAt(pub_datas, it2); + XmlrpcParam* connection_id = xmlrpcParamArrayGetParamAt(pub_data_xml, 0); + XmlrpcParam* bytes_sent = xmlrpcParamArrayGetParamAt(pub_data_xml, 1); + XmlrpcParam* num_sent = xmlrpcParamArrayGetParamAt(pub_data_xml, 2); + XmlrpcParam* connected = xmlrpcParamArrayGetParamAt(pub_data_xml, 3); + pub_data->connection_id = connection_id->data.as_int; + pub_data->bytes_sent = (size_t)bytes_sent->data.as_int; + pub_data->num_sent = (size_t)num_sent->data.as_int; + pub_data->connected = connected->data.as_int; + } + } + + if (array->array_n_elem < 4) + return ret; + + XmlrpcParam* subs_stats = xmlrpcParamArrayGetParamAt(array, 3); + ret->stats.sub_stats = (struct TopicSubStats *)calloc(subs_stats->array_n_elem, sizeof(struct TopicSubStats)); + if (ret->stats.sub_stats == NULL) + goto clean; + ret->stats.sub_stats_count = subs_stats->array_n_elem; + + for (it1 = 0; it1 < subs_stats->array_n_elem; it1++) + { + int it2; + + struct TopicSubStats *sub_stats = &ret->stats.sub_stats[it1]; + XmlrpcParam *sub_stats_xml = xmlrpcParamArrayGetParamAt(subs_stats, it1); + if (sub_stats_xml->array_n_elem < 3) + goto clean; + + XmlrpcParam *name_xml = xmlrpcParamArrayGetParamAt(sub_stats_xml, 0); + sub_stats->topic_name = (char *)malloc(strlen(name_xml->data.as_string) + 1); + if (sub_stats->topic_name == NULL) + goto clean; + strcpy(sub_stats->topic_name, name_xml->data.as_string); + + XmlrpcParam* sub_datas = xmlrpcParamArrayGetParamAt(sub_stats_xml, 1); + sub_stats->datas = (struct SubConnectionData *)calloc(sub_datas->array_n_elem, sizeof(struct SubConnectionData)); + if (sub_stats->datas == NULL) + goto clean; + sub_stats->datas_count = sub_datas->array_n_elem; + + for (it2 = 0; it2 < sub_datas->array_n_elem; it2++) + { + struct SubConnectionData *sub_data = &sub_stats->datas[it2]; + XmlrpcParam *sub_data_xml = xmlrpcParamArrayGetParamAt(sub_datas, it2); + XmlrpcParam *connection_id = xmlrpcParamArrayGetParamAt(sub_data_xml, 0); + XmlrpcParam *bytes_received = xmlrpcParamArrayGetParamAt(sub_data_xml, 1); + XmlrpcParam *drop_estimate = xmlrpcParamArrayGetParamAt(sub_data_xml, 2); + XmlrpcParam *connected = xmlrpcParamArrayGetParamAt(sub_data_xml, 3); + sub_data->connection_id = connection_id->data.as_int; + sub_data->bytes_received = (size_t)bytes_received->data.as_int; + sub_data->drop_estimate = drop_estimate->data.as_int; + sub_data->connected = connected->data.as_bool; + } + } + + if (array->array_n_elem < 5) + return ret; + + XmlrpcParam *services_stats = xmlrpcParamArrayGetParamAt(array, 4); + XmlrpcParam *numRequests = xmlrpcParamArrayGetParamAt(services_stats, 0); + XmlrpcParam *bytesReceived = xmlrpcParamArrayGetParamAt(services_stats, 1); + XmlrpcParam *bytesSent = xmlrpcParamArrayGetParamAt(services_stats, 2); + + ret->stats.service_stats.num_requests = (size_t)numRequests->data.as_int; + ret->stats.service_stats.bytes_received = (size_t)bytesReceived->data.as_int; + ret->stats.service_stats.bytes_sent = (size_t)bytesSent->data.as_int; + + return ret; + +clean: + freeGetBusStatsResult(ret); + return NULL; +} + +GetBusInfoResult * fetchGetBusInfoResult(XmlrpcParamVector *response) +{ + int it; + + GetBusInfoResult *ret = (GetBusInfoResult *)calloc(1, sizeof(GetBusInfoResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetBusInfoResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetBusInfoResult(ret); + return NULL; + } + + if (array->array_n_elem < 3) + return ret; + + XmlrpcParam* businfos = xmlrpcParamArrayGetParamAt(array, 2); + ret->bus_infos = (struct BusInfo *)calloc(businfos->array_n_elem, sizeof(struct BusInfo)); + if (ret->bus_infos == NULL) + goto clean; + ret->bus_infos_count = businfos->array_n_elem; + + for (it = 0; it < businfos->array_n_elem; it++) + { + struct BusInfo *businfo = &ret->bus_infos[it]; + XmlrpcParam *businfo_xml = xmlrpcParamArrayGetParamAt(businfos, it); + + XmlrpcParam *connectionId = xmlrpcParamArrayGetParamAt(businfo_xml, 0); + XmlrpcParam *destinationId = xmlrpcParamArrayGetParamAt(businfo_xml, 1); + XmlrpcParam *direction = xmlrpcParamArrayGetParamAt(businfo_xml, 2); + XmlrpcParam *transport = xmlrpcParamArrayGetParamAt(businfo_xml, 3); + XmlrpcParam *topic = xmlrpcParamArrayGetParamAt(businfo_xml, 4); + XmlrpcParam *connected = xmlrpcParamArrayGetParamAt(businfo_xml, 5); + + businfo->topic = (char *)malloc(strlen(topic->data.as_string) + 1); + if (businfo->topic == NULL) + goto clean; + strcpy(businfo->topic, topic->data.as_string); + businfo->connectionId = connectionId->data.as_int; + businfo->destinationId = connectionId->data.as_int; + switch (connectionId->data.as_string[0]) + { + case 'i': + businfo->direction = CROS_TRANSPORT_DIRECTION_IN; + break; + case 'o': + businfo->direction = CROS_TRANSPORT_DIRECTION_OUT; + break; + case 'b': + businfo->direction = CROS_TRANSPORT_DIRECTION_BOTH; + break; + default: + goto clean; + } + businfo->transport = (CrosTransportType)connectionId->data.as_int; + businfo->connected = connectionId->data.as_int; + } + + return ret; + +clean: + freeGetBusInfoResult(ret); + return NULL; +} + +GetMasterUriResult * fetchGetMasterUriResult(XmlrpcParamVector *response) +{ + GetMasterUriResult *ret = (GetMasterUriResult *)calloc(1, sizeof(GetMasterUriResult)); + if (ret == NULL) + return ret; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetMasterUriResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetMasterUriResult(ret); + return NULL; + } + + XmlrpcParam* uri = xmlrpcParamArrayGetParamAt(array, 2); + ret->master_uri = strdup(uri->data.as_string); + if (ret->master_uri == NULL) + { + freeGetMasterUriResult(ret); + return NULL; + } + + return ret; +} + +ShutdownResult * fetchShutdownResult(XmlrpcParamVector *response) +{ + ShutdownResult *ret = (ShutdownResult *)calloc(1, sizeof(ShutdownResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchShutdownResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeShutdownResult(ret); + return NULL; + } + + XmlrpcParam *ignore = xmlrpcParamArrayGetParamAt(array, 2); + if(ignore != NULL) + ret->ignore = ignore->data.as_bool; + else + { + PRINT_ERROR ( "fetchShutdownResult() : The ROS master response does not contain the 'ignore' parameter.\n" ); + freeShutdownResult(ret); + return NULL; + } + + return ret; +} + +GetPidResult * fetchGetPidResult(XmlrpcParamVector *response) +{ + GetPidResult *ret = (GetPidResult *)calloc(1, sizeof(GetPidResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetPidResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetPidResult(ret); + return NULL; + } + + XmlrpcParam* roscore_pid_param = xmlrpcParamArrayGetParamAt(array, 2); + if(roscore_pid_param != NULL) + ret->server_process_pid = roscore_pid_param->data.as_int; + else + { + PRINT_ERROR ( "fetchGetPidResult() : The ROS master response does not contain the requested PID.\n" ); + freeGetPidResult(ret); + return NULL; + } + + return ret; +} + +GetSubscriptionsResult * fetchGetSubscriptionsResult(XmlrpcParamVector *response) +{ + int it; + + GetSubscriptionsResult *ret = (GetSubscriptionsResult *)calloc(1, sizeof(GetSubscriptionsResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetSubscriptionsResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetSubscriptionsResult(ret); + return NULL; + } + + XmlrpcParam* topics = xmlrpcParamArrayGetParamAt(array, 2); + ret->topic_list = (struct TopicTypePair *)calloc(topics->array_n_elem, sizeof(struct TopicTypePair)); + if (ret->topic_list == NULL) + goto clean; + ret->topic_count = topics->array_n_elem; + + for (it = 0; it < topics->array_n_elem; it++) + { + struct TopicTypePair *pair = &ret->topic_list[it]; + XmlrpcParam* pair_xml = xmlrpcParamArrayGetParamAt(topics, it); + XmlrpcParam* topic = xmlrpcParamArrayGetParamAt(pair_xml, 0); + pair->topic = (char *)malloc(strlen(topic->data.as_string) + 1); + if (pair->topic == NULL) + goto clean; + strcpy(pair->topic, topic->data.as_string); + + XmlrpcParam* type = xmlrpcParamArrayGetParamAt(pair_xml, 1); + pair->type = (char *)malloc(strlen(type->data.as_string) + 1); + if (pair->type == NULL) + goto clean; + strcpy(pair->type, type->data.as_string); + } + + return ret; + +clean: + freeGetSubscriptionsResult(ret); + return NULL; +} + +GetPublicationsResult * fetchGetPublicationsResult(XmlrpcParamVector *response) +{ + int it; + + GetPublicationsResult *ret = (GetPublicationsResult *)calloc(1, sizeof(GetPublicationsResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetPublicationsResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetPublicationsResult(ret); + return NULL; + } + + XmlrpcParam* topics = xmlrpcParamArrayGetParamAt(array, 2); + ret->topic_list = (struct TopicTypePair *)calloc(topics->array_n_elem, sizeof(struct TopicTypePair)); + if (ret->topic_list == NULL) + goto clean; + ret->topic_count = topics->array_n_elem; + + for (it = 0; it < topics->array_n_elem; it++) + { + struct TopicTypePair *pair = &ret->topic_list[it]; + XmlrpcParam* pair_xml = xmlrpcParamArrayGetParamAt(topics, it); + XmlrpcParam* topic = xmlrpcParamArrayGetParamAt(pair_xml, 0); + pair->topic = (char *)malloc(strlen(topic->data.as_string) + 1); + if (pair->topic == NULL) + goto clean; + strcpy(pair->topic, topic->data.as_string); + + XmlrpcParam* type = xmlrpcParamArrayGetParamAt(pair_xml, 1); + pair->type = (char *)malloc(strlen(type->data.as_string) + 1); + if (pair->type == NULL) + goto clean; + strcpy(pair->type, type->data.as_string); + } + + return ret; + +clean: + freeGetPublicationsResult(ret); + return NULL; +} + +static DeleteParamResult * fetchDeleteParamResult(XmlrpcParamVector *response) +{ + DeleteParamResult *ret = (DeleteParamResult *)calloc(1, sizeof(DeleteParamResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchDeleteParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeDeleteParamResult(ret); + return NULL; + } + + XmlrpcParam* ignore = xmlrpcParamArrayGetParamAt(array, 2); + if(ignore != NULL) + ret->ignore = ignore->data.as_bool; + else + { + PRINT_ERROR ( "fetchDeleteParamResult() : The ROS master response does not contain the 'ignore' parameter.\n" ); + freeDeleteParamResult(ret); + return NULL; + } + + return ret; +} + +static SetParamResult * fetchSetParamResult(XmlrpcParamVector *response) +{ + SetParamResult *ret = (SetParamResult *)calloc(1, sizeof(SetParamResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchSetParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeSetParamResult(ret); + return NULL; + } + + XmlrpcParam* ignore = xmlrpcParamArrayGetParamAt(array, 2); + if(ignore != NULL) + ret->ignore = ignore->data.as_bool; + else + { + PRINT_ERROR ( "fetchSetParamResult() : The ROS master response does not contain the 'ignore' parameter.\n" ); + freeSetParamResult(ret); + return NULL; + } + + return ret; +} + +static GetParamResult * fetchGetParamResult(XmlrpcParamVector *response) +{ + GetParamResult *ret = (GetParamResult *)calloc(1, sizeof(GetParamResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetParamResult(ret); + return NULL; + } + + XmlrpcParam* value = xmlrpcParamArrayGetParamAt(array, 2); + if( value != NULL ) + ret->value = xmlrpcParamClone(value); + else + { + if(ret->code != 1) // Only if ret->code is not 1, parameterValue can be ignored. + { + PRINT_ERROR ( "fetchGetParamResult() : The ROS master response does not contain the 'parameterValue' parameter.\n" ); + freeGetParamResult(ret); + return NULL; + } + } + + return ret; +} + +static SearchParamResult * fetchSearchParamResult(XmlrpcParamVector *response) +{ + SearchParamResult *ret = (SearchParamResult *)calloc(1, sizeof(SearchParamResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchSearchParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeSearchParamResult(ret); + return NULL; + } + + XmlrpcParam* found_key = xmlrpcParamArrayGetParamAt(array, 2); + if(found_key != NULL) + { + ret->found_key = strdup(found_key->data.as_string); + if(ret->found_key == NULL) + { + PRINT_ERROR ( "fetchSearchParamResult() : Error allocating memory for the 'foundKey' parameter.\n" ); + freeSearchParamResult(ret); + return NULL; + } + } + else + { + PRINT_ERROR ( "fetchSearchParamResult() : The ROS master response does not contain the 'foundKey' parameter.\n" ); + freeSearchParamResult(ret); + return NULL; + } + + return ret; +} + +static HasParamResult * fetchHasParamResult(XmlrpcParamVector *response) +{ + HasParamResult *ret = (HasParamResult *)calloc(1, sizeof(HasParamResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchHasParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeHasParamResult(ret); + return NULL; + } + + XmlrpcParam* has_param = xmlrpcParamArrayGetParamAt(array, 2); + if(has_param != NULL) + ret->has_param = has_param->data.as_bool; + else + { + PRINT_ERROR ( "fetchHasParamResult() : The ROS master response does not contain the 'has_param' parameter.\n" ); + freeHasParamResult(ret); + return NULL; + } + + return ret; +} + +static GetParamNamesResult * fetchGetParamNamesResult(XmlrpcParamVector *response) +{ + int it; + + GetParamNamesResult *ret = (GetParamNamesResult *)calloc(1, sizeof(GetParamNamesResult)); + if (ret == NULL) + return NULL; + + XmlrpcParam *array; + array = GetMethodResponseStatus(response, &ret->code, &ret->status); + if (array == NULL) + { + free(ret); + return NULL; + } + if(ret->code == -1 || ret->code == 0) + { + PRINT_ERROR ( "fetchGetParamNamesResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); + freeGetParamNamesResult(ret); + return NULL; + } + + XmlrpcParam* param_names = xmlrpcParamArrayGetParamAt(array, 2); + ret->parameter_names = (char **)calloc(param_names->array_n_elem, sizeof(char *)); + if (ret->parameter_names== NULL) + goto clean; + + for (it = 0; it < param_names->array_n_elem; it++) + { + ret->parameter_names[it] = (char *)malloc(strlen(param_names->data.as_array[it].data.as_string) + 1); + if (ret->parameter_names[it] == NULL) + goto clean; + + strcpy(ret->parameter_names[it], param_names->data.as_array[it].data.as_string); + } + ret->parameter_count = (size_t)param_names->array_n_elem; + + return ret; + +clean: + freeGetParamNamesResult(ret); + return NULL; +} + +cRosMessage *cRosApiCreatePublisherMessage(CrosNode *node, int pubidx) +{ + cRosMessage *new_msg; + ProviderContext *pub_context; + PublisherNode *pub; + + if (pubidx < 0 || pubidx >= CN_MAX_PUBLISHED_TOPICS) + return NULL; + + pub = &node->pubs[pubidx]; + if (pub->topic_name == NULL) + return NULL; + + pub_context = pub->context; + new_msg = cRosMessageCopy(pub_context->outgoing); + + return new_msg; +} + +cRosMessage *cRosApiCreateServiceCallerRequest(CrosNode *node, int svcidx) +{ + cRosMessage *new_msg; + ServiceCallerNode *svc_caller; + ProviderContext *caller_context; + + if (svcidx < 0 || svcidx >= CN_MAX_SERVICE_CALLERS) + return NULL; + + svc_caller = &node->service_callers[svcidx]; + if (svc_caller->service_name == NULL) + return NULL; + + caller_context = svc_caller->context; + new_msg = cRosMessageCopy(caller_context->outgoing); + + return new_msg; +} + + +void freeLookupNodeResult(LookupNodeResult *result) +{ + free(result->status); + free(result->uri); + free(result); +} + +void freeGetPublishedTopicsResult(GetPublishedTopicsResult *result) +{ + size_t it; + + free(result->status); + for (it = 0; it < result->topic_count; it++) + { + free(result->topics[it].topic); + free(result->topics[it].type); + } + free(result->topics); + free(result); +} + +void freeGetTopicTypesResult(GetTopicTypesResult *result) +{ + size_t it; + + free(result->status); + for (it = 0; it < result->topic_count; it++) + { + free(result->topics[it].topic); + free(result->topics[it].type); + } + free(result->topics); + free(result); +} + +void freeGetSystemStateResult(GetSystemStateResult *result) +{ + size_t it1; + + free(result->status); + for (it1 = 0; it1 < result->sub_count; it1++) + { + size_t it2; + + free(result->publishers[it1].provider_name); + for (it2 = 0; it2 < result->publishers[it1].user_count; it2++) + free(result->publishers[it1].users[it2]); + free(result->publishers[it1].users); + } + free(result->publishers); + + for (it1 = 0; it1 < result->sub_count; it1++) + { + size_t it2; + + free(result->subscribers[it1].provider_name); + for (it2 = 0; it2 < result->subscribers[it1].user_count; it2++) + free(result->subscribers[it1].users[it2]); + free(result->subscribers[it1].users); + } + free(result->subscribers); + + for (it1 = 0; it1 < result->svc_count; it1++) + { + size_t it2; + + free(result->service_providers[it1].provider_name); + for (it2 = 0; it2 < result->service_providers[it1].user_count; it2++) + free(result->service_providers[it1].users[it2]); + free(result->service_providers[it1].users); + } + free(result->service_providers); + + free(result); +} + +void freeGetUriResult(GetUriResult *result) +{ + free(result->status); + free(result->master_uri); + free(result); +} + +void freeLookupServiceResult(LookupServiceResult *result) +{ + free(result->status); + free(result->service_result); + free(result); +} + +void freeGetBusStatsResult(GetBusStatsResult *result) +{ + size_t it1; + + free(result->status); + for (it1 = 0; it1 < result->stats.pub_stats_count; it1++) + free(result->stats.pub_stats[it1].topic_name); + free(result->stats.pub_stats); + + for (it1 = 0; it1 < result->stats.sub_stats_count; it1++) + free(result->stats.sub_stats[it1].topic_name); + free(result->stats.sub_stats); + + free(result); +} + +void freeGetBusInfoResult(GetBusInfoResult *result) +{ + size_t it; + + free(result->status); + for (it = 0; it < result->bus_infos_count; it++) + free(result->bus_infos[it].topic); + free(result->bus_infos); + free(result); +} + +void freeGetMasterUriResult(GetMasterUriResult *result) +{ + free(result->status); + free(result->master_uri); + free(result); +} + +void freeShutdownResult(ShutdownResult *result) +{ + free(result->status); + free(result); +} + +void freeGetPidResult(GetPidResult *result) +{ + free(result->status); + free(result); +} + +void freeGetSubscriptionsResult(GetSubscriptionsResult *result) +{ + size_t it; + + free(result->status); + for (it = 0; it < result->topic_count; it++) + { + free(result->topic_list[it].topic); + free(result->topic_list[it].type); + } + free(result->topic_list); + free(result); +} + +void freeGetPublicationsResult(GetPublicationsResult *result) +{ + size_t it; + + free(result->status); + for (it = 0; it < result->topic_count; it++) + { + free(result->topic_list[it].topic); + free(result->topic_list[it].type); + } + free(result->topic_list); + free(result); +} + +static void freeDeleteParamResult(DeleteParamResult *result) +{ + free(result->status); + free(result); +} + +static void freeSetParamResult(SetParamResult *result) +{ + free(result->status); + free(result); +} + +static void freeGetParamResult(GetParamResult *result) +{ + free(result->status); + xmlrpcParamFree(result->value); + free(result); +} + +static void freeSearchParamResult(SearchParamResult *result) +{ + free(result->status); + free(result->found_key); + free(result); +} + +static void freeHasParamResult(HasParamResult *result) +{ + free(result->status); + free(result); +} + +static void freeGetParamNamesResult(GetParamNamesResult *result) +{ + size_t it; + + free(result->status); + for (it = 0; it < result->parameter_count; it++) + free(result->parameter_names[it]); + free(result->parameter_names); + free(result); +} + diff --git a/src/hal/components/cros/src/cros_api_call.c b/src/hal/components/cros/src/cros_api_call.c new file mode 100644 index 00000000..90b04cb9 --- /dev/null +++ b/src/hal/components/cros/src/cros_api_call.c @@ -0,0 +1,112 @@ +#include +#include + +#include "cros_api_call.h" +#include "cros_defs.h" + +RosApiCall * newRosApiCall(void) +{ + RosApiCall *ret = (RosApiCall *)malloc(sizeof(RosApiCall)); + ret->id = -1; + ret->user_call = 0; + ret->provider_idx = -1; + ret->host = NULL; + ret->port = -1; + xmlrpcParamVectorInit(&ret->params); + ret->result_callback = NULL; + ret->context_data = NULL; + ret->fetch_result_callback = NULL; + ret->free_result_callback = NULL; + return ret; +} + +void freeRosApiCall(RosApiCall *call) +{ + xmlrpcParamVectorRelease(&call->params); + if (call->host != NULL) + free(call->host); + free(call); +} + +void initApiCallQueue(ApiCallQueue *queue) +{ + queue->tail = NULL; + queue->head = NULL; + queue->count = 0; +} + +RosApiCall * peekApiCallQueue(ApiCallQueue *queue) +{ + if (queue->head == NULL) + return NULL; + + return queue->head->call; +} + +int enqueueApiCall(ApiCallQueue *queue, RosApiCall* apiCall) +{ + ApiCallNode *node = (ApiCallNode *)malloc(sizeof(ApiCallNode)); + + if(node == NULL) + { + PRINT_ERROR("enqueueApiCall() : Can't enqueue call\n"); + return -1; + } + + node->call = apiCall; + node->next = NULL; + + if(queue->head == NULL) + { + queue->head = node; + queue->tail = node; + } + else + { + queue->tail->next = node; + queue->tail = node; + } + + queue->count++; + + return 0; +} + +RosApiCall * dequeueApiCall(ApiCallQueue *queue) +{ + ApiCallNode* head = queue->head; + queue->head = head->next; + if(queue->head == NULL) + queue->tail = queue->head; + + RosApiCall *call = head->call; + free(head); + + queue->count--; + + return call; +} + +void releaseApiCallQueue(ApiCallQueue *queue) +{ + ApiCallNode *current = queue->head; + while(current != NULL) + { + freeRosApiCall(current->call); + ApiCallNode *next = current->next; + free(current); + current = next; + } + + initApiCallQueue(queue); +} + +size_t getQueueCount(ApiCallQueue *queue) +{ + return queue->count; +} + +int isQueueEmpty(ApiCallQueue *queue) +{ + return queue->count == 0; +} diff --git a/src/hal/components/cros/src/cros_clock.c b/src/hal/components/cros/src/cros_clock.c new file mode 100644 index 00000000..68260c26 --- /dev/null +++ b/src/hal/components/cros/src/cros_clock.c @@ -0,0 +1,122 @@ +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN +# include +# include +#else +# include +# include +#endif + +#include "cros_defs.h" +#include "cros_clock.h" + +struct timeval cRosClockGetTimeSecUsec( void ) +{ + struct timeval time_since_epoch; + int ret_val; + + PRINT_VVDEBUG ( "cRosClockGetTimeSecUsec()\n" ); + +#ifdef _WIN32 + const uint64_t epoch_filetime = 116444736000000000ULL; // FILETIME on Jan 1 1970 00:00:00 + FILETIME cur_filetime; + SYSTEMTIME cur_system_time; + + GetSystemTime(&cur_system_time); + + if(SystemTimeToFileTime(&cur_system_time, &cur_filetime) != 0) // If Success: + { + ULARGE_INTEGER cur_filetime_large; + + // It is recomended that the calculations are done using a ULARGE_INTEGER + cur_filetime_large.LowPart = cur_filetime.dwLowDateTime; + cur_filetime_large.HighPart = cur_filetime.dwHighDateTime; + + time_since_epoch.tv_sec = (long) ((cur_filetime_large.QuadPart - epoch_filetime) / 10000000L); + time_since_epoch.tv_usec = (long) (cur_system_time.wMilliseconds * 1000); + ret_val = 0; + } + else + ret_val = -1; +#else + ret_val = gettimeofday(&time_since_epoch, NULL); +#endif + + if(ret_val == -1) // Failure obtaning the time: set default values: + { + time_since_epoch.tv_sec = 0; + time_since_epoch.tv_usec = 0; + } + + return(time_since_epoch); +} + +uint64_t cRosClockGetTimeMs( void ) +{ + uint64_t ms_since_epoch; + struct timeval tv; + + PRINT_VVDEBUG ( "cRosClockGetTimeMs()\n" ); + + tv = cRosClockGetTimeSecUsec(); + ms_since_epoch = (uint64_t)tv.tv_sec*1000 + (uint64_t)tv.tv_usec/1000; + + return(ms_since_epoch); +} + +struct timeval cRosClockGetTimeVal( uint64_t msec ) +{ + PRINT_VVDEBUG ( "cRosClockGetTimeVal() msec: %lu\n", msec ); + struct timeval tv; + if (msec > ( LONG_MAX * 1000ULL )) + { + // Given timespan would overflow timeval + tv.tv_sec = LONG_MAX; + tv.tv_usec = 999999L; + } + else + { + tv.tv_sec = (long)(msec / 1000); + tv.tv_usec = (long)(msec - tv.tv_sec * 1000ULL ) * 1000; + } + + return tv; +} + +int64_t cRosClockGetTimeStamp(void) +{ + int64_t time_stamp_ret; +#ifdef _WIN32 + LARGE_INTEGER time_stamp_init; + QueryPerformanceCounter(&time_stamp_init); + time_stamp_ret = time_stamp_init.QuadPart; +#else + struct timespec time_stamp_init; + int fn_ret_val; + fn_ret_val = clock_gettime(CLOCK_MONOTONIC, &time_stamp_init); + if (fn_ret_val == 0) // clock_gettime success + time_stamp_ret = time_stamp_init.tv_sec * 1000000000LL + time_stamp_init.tv_nsec; + else + time_stamp_ret = 0; +#endif + return(time_stamp_ret); +} + +double cRosClockTimeStampToUSec(int64_t time_stamp) +{ + double time_sec; +#ifdef _WIN32 + LARGE_INTEGER counter_freq; + BOOL fn_ret_val; + fn_ret_val = QueryPerformanceFrequency(&counter_freq); + if (fn_ret_val) // QueryPerformanceFrequency success + time_sec = (time_stamp * 1.0e6L) / counter_freq.QuadPart; + else + time_sec = 0; +#else + time_sec = time_stamp / 1.0e3; +#endif + return(time_sec); +} diff --git a/src/hal/components/cros/src/cros_err_codes.c b/src/hal/components/cros/src/cros_err_codes.c new file mode 100644 index 00000000..fc768d9c --- /dev/null +++ b/src/hal/components/cros/src/cros_err_codes.c @@ -0,0 +1,163 @@ +#include +#include +#include +#include "cros_node.h" +#include "cros_err_codes.h" + +// Populate the error message list global variable using the messages defined before (ERROR_CODE_LIST_DEF) but +// using the format defined by MSG_COD_ELEM for them. +// This an array of structures where the error messages are searched for. +#define MSG_COD_ELEM(code,msg) {code,msg}, +struct cRosErrCodeListElem CRosErrCodeList[]= +{ + ERROR_CODE_LIST_DEF + {LAST_ERR_LIST_CODE, NULL} // Last value of the list not used +}; +#undef MSG_COD_ELEM + +const char *cRosGetErrCodeStr(cRosErrCode err_code) +{ + int msg_ind; + const char *ret_msg; + + // Search for a message in the error message list according to the specified message code + ret_msg=NULL; + for(msg_ind=0;CRosErrCodeList[msg_ind].code!=LAST_ERR_LIST_CODE && ret_msg==NULL;msg_ind++) + { + if(CRosErrCodeList[msg_ind].code == err_code) // We have found a message with the specified code + ret_msg = CRosErrCodeList[msg_ind].msg; // Use the found message as return value + } + return ret_msg; +} + +cRosErrCodePack cRosAddErrCode(cRosErrCodePack prev_err_pack, cRosErrCode err_code) +{ + cRosErrCodePack new_err_pack; + + if(err_code == CROS_NO_ERR) // If no new error: + new_err_pack = prev_err_pack; // do not add any error code to the pack + else // if we have some new error code + new_err_pack = (prev_err_pack << 8) | err_code; // add more info to the error pack: add the new code + + return new_err_pack; +} + +cRosErrCodePack cRosAddErrCodeIfErr(cRosErrCodePack prev_err_pack, cRosErrCode err_code) +{ + cRosErrCodePack new_err_pack; + + if(prev_err_pack == CROS_SUCCESS_ERR_PACK) // If no error stored in the error pack + new_err_pack = prev_err_pack; // do not add any error code to the pack + else // if we had some error code in the pack + new_err_pack = cRosAddErrCode(prev_err_pack, err_code); // add more info to the error pack: add the new code + + return new_err_pack; +} + +cRosErrCodePack cRosRemoveLastErrCode(cRosErrCodePack prev_err_pack) +{ + cRosErrCodePack new_err_pack; + + new_err_pack = prev_err_pack >> 8; // Remove the last error code from the error pack (8 least-significant bits) + + return new_err_pack; +} + +cRosErrCode cRosGetLastErrCode(cRosErrCodePack err_pack) +{ + cRosErrCode last_err_code; + + last_err_code = err_pack & 0xFFU; // Get the last error code from the error pack (8 least-significant bits) + + return last_err_code; +} + +cRosErrCodePack cRosAddErrCodePackIfErr(cRosErrCodePack prev_err_pack_0, cRosErrCodePack prev_err_pack_1) +{ + cRosErrCodePack new_err_code_pack; + cRosErrCode curr_err_cod; + + // First, add the error codes from prev_err_pack_0 to the output + new_err_code_pack = prev_err_pack_0; + // Then, add the error codes from prev_err_pack_1 to the output: + // Iterate throughout all the errors contained in prev_err_pack_1 + while((curr_err_cod=cRosGetLastErrCode(prev_err_pack_1)) != CROS_NO_ERR) + { + new_err_code_pack = cRosAddErrCode(new_err_code_pack, curr_err_cod); + prev_err_pack_1 = cRosRemoveLastErrCode(prev_err_pack_1); // Pass to the next error code + } + + return new_err_code_pack; +} + +int cRosPrintErrCodePack(cRosErrCodePack err_cod_pack, const char *fmt_str, ...) +{ + int n_prn_chars; + const char *msg_str; + cRosErrCode curr_err_cod; + FILE *msg_out = cRosOutStreamGet(); + + n_prn_chars=0; + + if(fmt_str != NULL) + { + va_list arg_list; + + va_start(arg_list, fmt_str); + n_prn_chars+=vfprintf(msg_out, fmt_str, arg_list); + va_end(arg_list); + } + + // Iterate throughout all the errors contained in the pack (up to 4) + while((curr_err_cod=cRosGetLastErrCode(err_cod_pack)) != CROS_NO_ERR) + { + // Print error info included in err + msg_str = cRosGetErrCodeStr(curr_err_cod); + if(msg_str != NULL) // The error code has been found in the list + n_prn_chars+=fprintf(msg_out, ". Err %u: %s", curr_err_cod, msg_str); + else + n_prn_chars+=fprintf(msg_out, ". Error code %u (The description string for this error code has not been found).", curr_err_cod); + + err_cod_pack = cRosRemoveLastErrCode(err_cod_pack); // Pass to the next error code + } + n_prn_chars+=fprintf(msg_out, "\n"); + return(n_prn_chars); +} + +int cRosErrCodePackStr(char *out_str_buf, size_t out_str_buf_len, cRosErrCodePack err_cod_pack, const char *fmt_str, ...) +{ + int n_prn_chars; + const char *msg_str; + cRosErrCode curr_err_cod; + + n_prn_chars=0; + if(out_str_buf_len == 0) + return(n_prn_chars); + + out_str_buf[0]='\0'; + + if(fmt_str != NULL) + { + va_list arg_list; + + va_start(arg_list, fmt_str); + n_prn_chars+=vsnprintf(out_str_buf+strlen(out_str_buf), out_str_buf_len-strlen(out_str_buf), fmt_str, arg_list); + va_end(arg_list); + } + + // Iterate throughout all the errors contained in the pack (up to 4) + while((curr_err_cod=cRosGetLastErrCode(err_cod_pack)) != CROS_NO_ERR) + { + // Print error info included in err + msg_str = cRosGetErrCodeStr(curr_err_cod); + if(msg_str != NULL) // The error code has been found in the list + n_prn_chars+=snprintf(out_str_buf+strlen(out_str_buf), out_str_buf_len-strlen(out_str_buf), ". Err %u: %s", curr_err_cod, msg_str); + else + n_prn_chars+=snprintf(out_str_buf+strlen(out_str_buf), out_str_buf_len-strlen(out_str_buf), ". Error code %u (The description string for this error code has not been found).", curr_err_cod); + + err_cod_pack = cRosRemoveLastErrCode(err_cod_pack); // Pass to the next error code + } + + return(n_prn_chars); +} + diff --git a/src/hal/components/cros/src/cros_gentools.c b/src/hal/components/cros/src/cros_gentools.c new file mode 100644 index 00000000..f58d2be5 --- /dev/null +++ b/src/hal/components/cros/src/cros_gentools.c @@ -0,0 +1,93 @@ +#include +#include +#include + +#include "cros_gentools.h" +#include "cros_message.h" +#include "cros_message_internal.h" +#include "cros_service.h" +#include "cros_service_internal.h" +#include "md5.h" + +char* cRosGentoolsMD5(char* filename) +{ + char* filename_tokenized = (char *)malloc(strlen(filename)+1); + strcpy(filename_tokenized, filename); + strtok(filename_tokenized, "."); + char* file_ext = strtok(NULL,"."); + + if(strcmp(file_ext,FILEEXT_MSG) == 0) + { + char *md5sum; + cRosMessage *msg=NULL; + cRosMessageNewBuild(NULL, filename, &msg); + if(msg != NULL) + { + md5sum = (char *)calloc(strlen(msg->md5sum)+1,sizeof(char)); + if(md5sum != NULL) + strcpy(md5sum,msg->md5sum); + cRosMessageFree(msg); + } + else + md5sum=NULL; + return md5sum; + } + + if(strcmp(file_ext,FILEEXT_SRV) == 0) + { + cRosService srv; + cRosServiceInit(&srv); + cRosServiceBuild(&srv,filename); + char *md5sum = (char *)calloc(strlen(srv.md5sum)+1,sizeof(char)); + if(md5sum != NULL) + strcpy(md5sum,srv.md5sum); + cRosServiceRelease(&srv); + return md5sum; + } + + free(filename_tokenized); + return NULL; +} + +int cRosGentoolsSHA1(char* filename) +{ + //FILE * fp = fopen(filename, "r"); + //fclose(fp); + return 0; +} + +int cRosGentoolsFulltext(char* filename) +{ + char* full_text = NULL;; + + char* filename_tokenized = (char *)malloc(strlen(filename)+1); + strcpy(filename_tokenized, filename); + strtok(filename_tokenized, "."); + char* file_ext = strtok(NULL,"."); + + if(strcmp(file_ext,FILEEXT_MSG) == 0) + { + msgDep messageDependencies; + cRosMessageDef msg; + initCrosMsg(&msg); + initCrosDep(&messageDependencies); + getFileDependenciesMsg(filename, &msg, &messageDependencies); + full_text = computeFullTextMsg(&msg, &messageDependencies); + } + + if(strcmp(file_ext,FILEEXT_SRV) == 0) + { + msgDep messageDependencies; + cRosSrvDef srv; + initCrosSrv(&srv); + initCrosDep(&messageDependencies); + getFileDependenciesSrv(filename, &srv, &messageDependencies); + full_text = computeFullTextSrv(&srv, &messageDependencies); + } + + printf("%s",full_text); + + free(filename_tokenized); + //free(full_text); + return 1; +} diff --git a/src/hal/components/cros/src/cros_log.c b/src/hal/components/cros/src/cros_log.c new file mode 100644 index 00000000..e1e3b8c9 --- /dev/null +++ b/src/hal/components/cros/src/cros_log.c @@ -0,0 +1,239 @@ +#include +#include +#include +#include + +#ifdef _WIN32 +# define strcasecmp _stricmp // This is the POSIX verion of strnicmp +#endif + +#include "cros_log.h" +#include "cros_defs.h" +#include "cros_node.h" +#include "cros_clock.h" +#include "cros_message.h" +#include "cros_api.h" + +CrosLog *cRosLogNew(void) +{ + CrosLog *ret = (CrosLog *)malloc(sizeof(CrosLog)); + ret->file = NULL; + ret->level = CROS_LOGLEVEL_INFO; + ret->function = NULL; + ret->msg = NULL; + ret->name = NULL; + ret->pubs = NULL; + ret->n_pubs = 0; + return ret; +} + +void cRosLogFree(CrosLog *log) +{ + free(log->file); + free(log->function); + free(log->msg); + free(log->name); + size_t i; + for(i = 0; i < log->n_pubs; i++) + { + free(log->pubs[i]); + } + free(log->pubs); + free(log); +} + +int stringToLogLevel(const char* level_str, CrosLogLevel *level_num) +{ + int ret; + ret = 0; // Default return value: success + if(strcasecmp("Info",level_str) == 0) + { + *level_num = CROS_LOGLEVEL_INFO; + } + else if(strcasecmp("Debug",level_str) == 0) + { + *level_num = CROS_LOGLEVEL_DEBUG; + } + else if(strcasecmp("Warn",level_str) == 0) + { + *level_num = CROS_LOGLEVEL_WARN; + } + else if(strcasecmp("Error",level_str) == 0) + { + *level_num = CROS_LOGLEVEL_ERROR; + } + else if(strcasecmp("Fatal",level_str) == 0) + { + *level_num = CROS_LOGLEVEL_FATAL; + } + else + { + ret = -1; // Unknown input level string: return error + } + return ret; +} + +const char *LogLevelToString(CrosLogLevel log_level) +{ + char *ret; + + switch(log_level) + { + case CROS_LOGLEVEL_INFO: + ret = "INFO"; + break; + case CROS_LOGLEVEL_DEBUG: + ret = "DEBUG"; + break; + case CROS_LOGLEVEL_WARN: + ret = "WARN"; + break; + case CROS_LOGLEVEL_ERROR: + ret = "ERROR"; + break; + case CROS_LOGLEVEL_FATAL: + ret = "FATAL"; + break; + default: + ret = "UNKNOWN"; + } + return ret; +} + +cRosMessage *cRosLogToMessage(CrosNode* node, CrosLog* log) +{ + cRosMessage *message; + size_t pub_ind; + + message = cRosApiCreatePublisherMessage(node, node->rosout_pub_idx); + if(message != NULL) + { + cRosMessageField* header_field = cRosMessageGetField(message, "header"); + cRosMessage* header_msg = header_field->data.as_msg; + cRosMessageField* seq_id = cRosMessageGetField(header_msg, "seq"); + seq_id->data.as_uint32 = node->log_last_id++; + cRosMessageField* time_field = cRosMessageGetField(header_msg, "stamp"); + cRosMessage* time_msg = time_field->data.as_msg; + cRosMessageField* time_secs = cRosMessageGetField(time_msg, "secs"); + time_secs->data.as_uint32 = log->secs; + cRosMessageField* time_nsecs = cRosMessageGetField(time_msg, "nsecs"); + time_nsecs->data.as_uint32 = log->nsecs; + cRosMessageSetFieldValueString(cRosMessageGetField(header_msg, "frame_id"), "0"); + + + cRosMessageField* level = cRosMessageGetField(message, "level"); + level->data.as_uint8 = log->level; + + cRosMessageField* name = cRosMessageGetField(message, "name"); // name of the node + cRosMessageSetFieldValueString(name, node->name); + + cRosMessageField* msg = cRosMessageGetField(message, "msg"); // message + cRosMessageSetFieldValueString(msg, log->msg); + + cRosMessageField* file = cRosMessageGetField(message, "file"); // file the message came from + cRosMessageSetFieldValueString(file, log->file); + + cRosMessageField* function = cRosMessageGetField(message, "function"); // function the message came from + cRosMessageSetFieldValueString(function, log->function); + + cRosMessageField* line = cRosMessageGetField(message, "line"); // line the message came from + line->data.as_uint32 = log->line; + + cRosMessageField* topics = cRosMessageGetField(message, "topics"); // topic names that the node publishes + + cRosMessageFieldArrayClear(topics); + for(pub_ind = 0; pub_ind < log->n_pubs; pub_ind++) + cRosMessageFieldArrayPushBackString(topics, log->pubs[pub_ind]); + } + return(message); +} + +void cRosLogPrint(CrosNode* node, + CrosLogLevel level, // debug level + const char* file, // file the message came from + const char* function, // function the message came from + uint32_t line, + const char* msg_fmt_str, ...) // message +{ + // Only print the message localy if its priority level is equal or higher than the current log + // priority set for this ROS node or we cannot find out the node's prioroty level + if(node == NULL || level >= node->log_level) + { + struct timeval wall_time; + int msg_str_size; + va_list msg_str_args; + + wall_time = cRosClockGetTimeSecUsec(); + + fprintf(cRosOutStreamGet(), "[%s] [%d,%ld] ", LogLevelToString(level), (int)wall_time.tv_sec, (long)wall_time.tv_usec*1000); + va_start(msg_str_args, msg_fmt_str); + msg_str_size = vfprintf(cRosOutStreamGet(), msg_fmt_str, msg_str_args); + va_end(msg_str_args); + + // Only print the message in rosout if its priority level is equal or higher than the node's current log priority + if(node != NULL) + { + int pub_ind; + + CrosLog* log = cRosLogNew(); + + log->secs = wall_time.tv_sec; + log->nsecs = (uint32_t)wall_time.tv_usec*1000; + + log->level = level; + + log->file = strdup(file); // We need to make a string memory copy since it is freed later in the cRosLogFree() call + + log->function = strdup(function); + + log->line = line; + + log->n_pubs = node->n_pubs; + log->pubs = (char **)calloc(log->n_pubs,sizeof(char*)); + + for(pub_ind = 0; pub_ind < node->n_pubs; pub_ind++) + { + if(node->pubs[pub_ind].topic_name != NULL) + log->pubs[pub_ind] = strdup(node->pubs[pub_ind].topic_name); + else + log->pubs[pub_ind] = NULL; + } + + log->msg = (char *)malloc((msg_str_size + 1)*sizeof(char)); + + // We need to use the argument list twice (the 1st time to call vfprintf and the 2nd time to call vsprintf), so we start the arg list again here + va_start(msg_str_args, msg_fmt_str); + vsprintf(log->msg, msg_fmt_str, msg_str_args); + va_end(msg_str_args); + + cRosMessage *topic_msg; + topic_msg = cRosLogToMessage(node, log); + if(topic_msg != NULL) + { + cRosErrCodePack err_cod; + int rosout_pub_idx = node->rosout_pub_idx; + + //err_cod = cRosNodeSendTopicMsg(node, rosout_pub_idx, topic_msg, 0); + err_cod = cRosNodeQueueTopicMsg(node, rosout_pub_idx, topic_msg); + if (err_cod != CROS_SUCCESS_ERR_PACK) + { + // Check if the rossout publisher has any TCP process associated, that is, check if a node is subscribed to this topic + int srv_proc_ind, subs_node; + subs_node = 0; + for(srv_proc_ind=0;srv_proc_indtcpros_server_proc[srv_proc_ind].topic_idx == rosout_pub_idx) + subs_node = 1; // there is a subscriber + // Only print the error message if there is a subscriber node, that is, if there is a node that should receive the rosout message + if(subs_node == 1) + cRosPrintErrCodePack(err_cod, "cRosLogPrint() : A message of /rosout topic could not be sent"); + } + cRosMessageFree(topic_msg); + } + else + { + PRINT_ERROR ( "cRosLogPrint() : A message of /rosout topic could not be created to be sent.\n" ); + } + cRosLogFree(log); + } + } +} diff --git a/src/hal/components/cros/src/cros_message.c b/src/hal/components/cros/src/cros_message.c new file mode 100644 index 00000000..00c18b6b --- /dev/null +++ b/src/hal/components/cros/src/cros_message.c @@ -0,0 +1,3280 @@ +#include +#include +#include + +#include "cros_message.h" +#include "cros_message_internal.h" +#include "cros_defs.h" +#include "md5.h" + +#ifdef _WIN32 +# define SPLIT_REGEX "\\." +# define DIR_SEPARATOR_CHAR '\\' +# define DIR_SEPARATOR_STR "\\" +// strtok_s is the Windows implementation of strtok_r +# define strtok_r strtok_s +#else +# define SPLIT_REGEX "/." +# define DIR_SEPARATOR_CHAR '/' +# define DIR_SEPARATOR_STR "/" +#endif + + +static void *arrayFieldValueAt(cRosMessageField *field, int position, size_t element_size); +static const char *getMessageTypeDeclarationConst(msgConst *msgConst); +static const char *getMessageTypeDeclarationField(msgFieldDef *fieldDef); + +char *base_msg_type(const char* type) +{ + // """ + // Compute the base data type, e.g. for arrays, get the underlying array item type + // @param type_: ROS msg type (e.g. 'std_msgs/String') + // @type type_: str + // @return: base type + // @rtype: str + // """ + char* base = NULL; + int char_count = 0; + const char* iterator = type; + + while( *iterator != '\0' && *iterator != '[') + { + char_count++; + iterator++; + } + + base = (char *)malloc(char_count + 1); + memcpy(base, type, char_count); + base[char_count] = '\0'; + + return base; +} + +int is_valid_msg_type(char* type_statement) +{ + //@return: True if the name is a syntatically legal message type name + // @rtype: bool + //if not x or len(x) != len(x.strip()): + // return False + char* iterator = type_statement; + while(*iterator != '\0') + { + if(*iterator == ' ') + { + return 0; + } + iterator ++; + } + +// base = base_msg_type(x) + char* base = base_msg_type(type_statement); + // if not roslib.names.is_legal_resource_name(base): + // return False + int slash_found = 0; + char* base_itr = base; + + while(*base_itr != '\0') + { + if((*base_itr == ']') || + (*base_itr == DIR_SEPARATOR_CHAR && slash_found)) + { + free(base); + return 0; + } + if(*base_itr == DIR_SEPARATOR_CHAR) + slash_found = 1; + base_itr++; + } + + // TODO Check that the array index is a number + + free(base); + return 1; +} + +int is_array_type(char* type_statement, int* size) +{ + //@return: True if the name is a syntactically legal message type name + // @rtype: bool + //if not x or len(x) != len(x.strip()): + // return False + char* type_it = type_statement; + char* array_size; + char* num_start = NULL; + int count = 0; + + while(*type_it != '\0' && *type_it != ']' && *type_it!= ' ') + { + if(num_start != NULL) + count++; + + if(*type_it == '[') + num_start = type_it; + + type_it++; + } + + if(num_start == NULL) + return 0; + + array_size = (char *)calloc(count + 1, sizeof(char)); + memcpy(array_size, num_start + 1, count); + char *endptr; + long int res = strtol(array_size, &endptr, 10); + if (endptr == array_size) + *size = -1; + else + *size = (int)res; + free(array_size); + + return 1; +} + +int containsDep(msgDep* iterator, char* depName) +{ + char* dep = (char *)malloc(strlen(depName)+1); + memcpy(dep,depName, strlen(depName)+1); + char* pack = dep; + char* name = dep; + while(*name != DIR_SEPARATOR_CHAR) name++; + *(name++) = '\0'; + + int found = 0; + //rollback + while((iterator)->prev != NULL) iterator = iterator->prev; + while(iterator->next != NULL && iterator->msg != NULL) + { + if(strcmp(iterator->msg->package, pack) == 0 && + strcmp(iterator->msg->name, name) == 0) + { + found = 1; + break; + } + iterator = iterator->next; + } + + free(dep); + return found; +} + +//Add the list of message types that spec depends on to depends. +cRosErrCodePack getDependenciesMsg(cRosMessageDef* msg, msgDep* msgDeps) +{ + cRosErrCodePack ret_err = CROS_SUCCESS_ERR_PACK; // Default return value + + //move on until you reach the head + while(msgDeps->next != NULL) msgDeps = msgDeps->next; + msgDep* currentDep = msgDeps ; + + msgFieldDef* fields = msg->first_field; + + while(fields->next != NULL && ret_err == CROS_SUCCESS_ERR_PACK) + { + msgFieldDef* currentField = fields; + + if(!isBuiltinMessageType(currentField->type)) + { + char *base_type = base_msg_type(currentField->type_s); + if(containsDep(msgDeps, base_type)) + { + fields = fields->next; + free(base_type); + continue; + } + + currentDep->msg = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); + // special mapping for header + if(currentField->type == CROS_STD_MSGS_HEADER) + { + //have to re-names Header + currentDep->msg->package = (char *)malloc(strlen(HEADER_DEFAULT_PACK) + 1); + currentDep->msg->package[0] = '\0'; + strcpy(currentDep->msg->package,HEADER_DEFAULT_PACK); + currentDep->msg->name = (char *)malloc(strlen(HEADER_DEFAULT_NAME) + 1); + currentDep->msg->name[0] = '\0'; + strcpy(currentDep->msg->name,HEADER_DEFAULT_NAME); + + currentDep->msg->plain_text = (char *)malloc(strlen(HEADER_DEFAULT_TYPEDEF) + 1); + memcpy(currentDep->msg->plain_text,"\0",1); + strcpy(currentDep->msg->plain_text, HEADER_DEFAULT_TYPEDEF); + msgDep *next = (msgDep *)malloc(sizeof(msgDep)); + next->msg = NULL; + next->prev = currentDep; + currentDep->next = next; + currentDep = next; + } + else + { + + if(strstr(base_type,"/") == NULL) + { + char* trail = msg->package; + currentDep->msg->name = (char *)malloc(strlen(trail) + strlen(base_type) + 1 + 1); + memcpy(currentDep->msg->name, trail, strlen(trail) + 1); + strcat(currentDep->msg->name, "/"); + strcat(currentDep->msg->name, base_type); + } + else + { + + char* dep = (char *)malloc(strlen(base_type)+1); + memcpy(dep,base_type, strlen(base_type)+1); + char* pack = dep; + char* name = dep; + while(*name != DIR_SEPARATOR_CHAR) name++; + *(name++) = '\0'; + currentDep->msg->name = name; + currentDep->msg->package = pack; + } + + currentDep->msg->fields = (msgFieldDef *)calloc(1, sizeof(msgFieldDef)); + initFieldDef(currentDep->msg->fields); + currentDep->msg->first_field = currentDep->msg->fields; + currentDep->msg->constants = (msgConst *)malloc(sizeof(msgConst)); + currentDep->msg->first_const = currentDep->msg->constants; + + char* path = (char *)malloc(strlen(msg->root_dir) + 1 + + strlen(currentDep->msg->package) + 1 + + strlen(currentDep->msg->name) + 4 + 1); + memcpy(path, msg->root_dir, strlen(msg->root_dir) + 1); + strcat(path, DIR_SEPARATOR_STR); + strcat(path, currentDep->msg->package); + strcat(path, DIR_SEPARATOR_STR); + strcat(path,currentDep->msg->name); + strcat(path,".msg"); + + ret_err = loadFromFileMsg(path,currentDep->msg); + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + msgDep *next = (msgDep *)malloc(sizeof(msgDep)); + next->msg = NULL; + next->prev = currentDep; + currentDep->next = next; + currentDep = next; + ret_err = getDependenciesMsg(currentDep->prev->msg, currentDep); + } + } + + free(base_type); + } + fields = fields->next; + } + return ret_err; +} + +// Compute dependencies of the specified message file +cRosErrCodePack getFileDependenciesMsg(char* filename, cRosMessageDef* msg, msgDep* deps) +{ + cRosErrCodePack ret_err; + ret_err = loadFromFileMsg(filename, msg); + if(ret_err == CROS_SUCCESS_ERR_PACK) + ret_err = getDependenciesMsg(msg,deps); + return ret_err; +} + +// Compute full text of message, including text of embedded +// types. The text of the main msg is listed first. Embedded +// msg files are denoted first by an 80-character '=' separator, +// followed by a type declaration line,'MSG: pkg/type', followed by +// the text of the embedded type. +char *computeFullTextMsg(cRosMessageDef* msg, msgDep* deps) +{ + char* full_text = NULL; + char* msg_tag = "MSG: "; + int full_size = 0; + char separator[81]; separator[80] = '\0'; + int i; + for(i = 0; i < 80; i++) + { + separator[i] = '='; + } + + full_size += strlen(msg->plain_text); + + while(deps->next != NULL) + { + full_size = strlen(deps->msg->plain_text) + strlen(separator) + strlen(msg_tag) + 3/*New lines*/; + deps = deps->next; + } + + //rollback + while(deps->prev != NULL) deps = deps->prev; + full_text = (char *)malloc(full_size + 1); + memcpy(full_text,msg->plain_text,strlen(msg->plain_text) + 1); + while(deps->next != NULL) + { + strcat(full_text, "\n"); + strcat(full_text, separator); + strcat(full_text, "\n"); + strcat(full_text, msg_tag); + strcat(full_text, deps->msg->package); + strcat(full_text, "/"); + strcat(full_text, deps->msg->name); + strcat(full_text, "\n"); + strcat(full_text, deps->msg->plain_text); + deps = deps->next; + } + return full_text; +} + +cRosErrCodePack initCrosMsg(cRosMessageDef* msg) +{ + cRosErrCodePack ret_err; + if(msg != NULL) + { + msg->constants = (msgConst *)malloc(sizeof(msgConst)); + msg->fields = (msgFieldDef *)calloc(1, sizeof(msgFieldDef)); + if(msg->constants != NULL && msg->fields != NULL) + { + initMsgConst(msg->constants); + msg->first_const = msg->constants; + initFieldDef(msg->fields); + msg->first_field = msg->fields; + msg->name = NULL; + msg->package = NULL; + msg->plain_text = NULL; + msg->root_dir = NULL; + ret_err = CROS_SUCCESS_ERR_PACK; + } + else + { + free(msg->constants); + free(msg->fields); + ret_err = CROS_MEM_ALLOC_ERR; + } + } + else + ret_err = CROS_BAD_PARAM_ERR; + return ret_err; +} + +void initMsgConst(msgConst *msg) +{ + msg->type = CROS_CUSTOM_TYPE; + msg->type_s = NULL; + msg->name = NULL; + msg->value = NULL; + msg->prev = NULL; + msg->next = NULL; +} + +void initCrosDep(msgDep* dep) +{ + dep->msg = NULL; + dep->prev = NULL; + dep->next = NULL; +} + +void initFieldDef(msgFieldDef* field) +{ + field->array_size = -1; + field->is_array = 0; + field->name = NULL; + field->type = CROS_CUSTOM_TYPE; + field->type_s = NULL; + field->next = NULL; + field->prev = NULL; + field->child_msg_def = NULL; +} + +unsigned char *getMD5Msg(cRosMessageDef* msg) +{ + DynString buffer; + cRosErrCodePack ret_err; + unsigned char *result; + + result = (unsigned char *)malloc(16); + if(result == NULL) + return(NULL); + + dynStringInit(&buffer); + msgConst* const_it = msg->first_const; + + while(const_it->next != NULL) + { + const char *type_decl = getMessageTypeDeclarationConst(const_it); + dynStringPushBackStr(&buffer,type_decl); + dynStringPushBackStr(&buffer," "); + dynStringPushBackStr(&buffer,const_it->name); + dynStringPushBackStr(&buffer,"="); + dynStringPushBackStr(&buffer,const_it->value); + dynStringPushBackStr(&buffer,"\n"); + const_it = const_it->next; + } + + msgFieldDef* fields_it = msg->first_field; + + ret_err = CROS_SUCCESS_ERR_PACK; + while(ret_err == CROS_SUCCESS_ERR_PACK && fields_it->next != NULL) + { + const char *type_decl = getMessageTypeDeclarationField(fields_it); + + if(isBuiltinMessageType(fields_it->type)) + { + if(fields_it->is_array) + { + dynStringPushBackStr(&buffer, type_decl); + dynStringPushBackStr(&buffer,"["); + if(fields_it->array_size != -1) + { + char num[15]; + sprintf(num, "%d",fields_it->array_size); + dynStringPushBackStr(&buffer,num); + } + dynStringPushBackStr(&buffer,"]"); + } + else + { + dynStringPushBackStr(&buffer, type_decl); + } + + dynStringPushBackStr(&buffer," "); + dynStringPushBackStr(&buffer,fields_it->name); + dynStringPushBackStr(&buffer,"\n"); + } + else if(fields_it->type == CROS_STD_MSGS_HEADER) + { + cRosMessageDef *msg_header; + char *header_text; + unsigned char *res; + + msg_header = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); + if(msg_header != NULL) + { + initCrosMsg(msg_header); + header_text = (char *)malloc(strlen(HEADER_DEFAULT_TYPEDEF) + 1); + if(header_text != NULL) + { + memcpy(header_text,HEADER_DEFAULT_TYPEDEF,strlen(HEADER_DEFAULT_TYPEDEF) + 1); + loadFromStringMsg(header_text, msg_header); + free(header_text); + res = getMD5Msg(msg_header); + cRosMessageDefFree(msg_header); + if(res != NULL) + { + cRosMD5Readable(res, &buffer); + free(res); + dynStringPushBackStr(&buffer," "); + dynStringPushBackStr(&buffer,fields_it->name); + dynStringPushBackStr(&buffer,"\n"); + } + else + { + dynStringRelease(&buffer); + return NULL; + } + } + else + { + cRosMessageDefFree(msg_header); + dynStringRelease(&buffer); + return NULL; + } + } + else + { + dynStringRelease(&buffer); + return NULL; + } + } + else + { + cRosMessage *msg_fn; + + ret_err = cRosMessageBuildFromDef(&msg_fn, fields_it->child_msg_def); // it is faster than ret_err = cRosMessageNewBuild(msg->root_dir, type_decl, &msg_fn) + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + char *md5sum = strdup(msg_fn->md5sum); + cRosMessageFree(msg_fn); + if(md5sum != NULL) + { + dynStringPushBackStr(&buffer, md5sum); + dynStringPushBackStr(&buffer," "); + dynStringPushBackStr(&buffer,fields_it->name); + dynStringPushBackStr(&buffer,"\n"); + free(md5sum); + } + else + ret_err = CROS_MEM_ALLOC_ERR; + } + + } + fields_it = fields_it->next; + } + + if(dynStringGetLen(&buffer) == 0) + dynStringPushBackStr(&buffer,"\n"); + + MD5_CTX md5_t; + MD5_Init(&md5_t); + MD5_Update(&md5_t, dynStringGetData(&buffer), dynStringGetLen(&buffer) - 1); + MD5_Final(result, &md5_t); + dynStringRelease(&buffer); + + return (ret_err == CROS_SUCCESS_ERR_PACK)? result : NULL; +} + +cRosErrCodePack getMD5Txt(cRosMessageDef* msg_def, DynString* buffer) +{ + cRosErrCodePack ret_err; + + msgConst* const_it = msg_def->first_const; + + ret_err = CROS_SUCCESS_ERR_PACK; + while(ret_err == CROS_SUCCESS_ERR_PACK && const_it->next != NULL) + { + const char *type_decl = getMessageTypeDeclarationConst(const_it); + dynStringPushBackStr(buffer,type_decl); + dynStringPushBackStr(buffer," "); + dynStringPushBackStr(buffer,const_it->name); + dynStringPushBackStr(buffer,"="); + dynStringPushBackStr(buffer,const_it->value); + dynStringPushBackStr(buffer,"\n"); + const_it = const_it->next; + } + + msgFieldDef* fields_it = msg_def->first_field; + + while(ret_err == CROS_SUCCESS_ERR_PACK && fields_it->next != NULL) + { + const char *type_decl = getMessageTypeDeclarationField(fields_it); + + if(isBuiltinMessageType(fields_it->type)) + { + dynStringPushBackStr(buffer, type_decl); + if(fields_it->is_array) + { + dynStringPushBackStr(buffer,"["); + if(fields_it->array_size != -1) + { + char num[15]; + sprintf(num, "%d",fields_it->array_size); + dynStringPushBackStr(buffer,num); + } + dynStringPushBackStr(buffer,"]"); + } + + dynStringPushBackStr(buffer," "); + dynStringPushBackStr(buffer,fields_it->name); + dynStringPushBackStr(buffer,"\n"); + } + else if(fields_it->type == CROS_STD_MSGS_HEADER) + { + char *header_text; + unsigned char *res; + cRosMessageDef *msg_header; + + msg_header = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); + if(msg_header != NULL) + { + initCrosMsg(msg_header); + header_text = (char *)malloc(strlen(HEADER_DEFAULT_TYPEDEF) + 1); + if(header_text != NULL) + { + memcpy(header_text,HEADER_DEFAULT_TYPEDEF,strlen(HEADER_DEFAULT_TYPEDEF) + 1); + loadFromStringMsg(header_text, msg_header); + free(header_text); + res = getMD5Msg(msg_header); + if(res != NULL) + { + cRosMD5Readable(res, buffer); + free(res); + dynStringPushBackStr(buffer," "); + dynStringPushBackStr(buffer,fields_it->name); + dynStringPushBackStr(buffer,"\n"); + } + else + ret_err = CROS_MEM_ALLOC_ERR; + } + else + ret_err = CROS_MEM_ALLOC_ERR; + cRosMessageDefFree(msg_header); + } + else + ret_err = CROS_MEM_ALLOC_ERR; + } + else + { + char *msg_name_str; + cRosMessage *msg_fn; + + msg_name_str = base_msg_type(type_decl); + ret_err = cRosMessageBuildFromDef(&msg_fn, fields_it->child_msg_def); // it is faster than ret_err = cRosMessageNewBuild(msg_def->root_dir, msg_name_str, &msg_fn) + free(msg_name_str); + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + dynStringPushBackStr(buffer, msg_fn->md5sum); + cRosMessageFree(msg_fn); + + dynStringPushBackStr(buffer," "); + dynStringPushBackStr(buffer,fields_it->name); + dynStringPushBackStr(buffer,"\n"); + } + + } + fields_it = fields_it->next; + } + + if(dynStringGetLen(buffer) == 0) + dynStringPushBackStr(buffer,""); // Ensure that the returned DynString has allocated memory + else + dynStringReduce (buffer, 0, 1); // Remove the ending \n character which must not be processed by the MD5 algorithm + + return ret_err; +} + +void cRosMD5Readable(unsigned char* data, DynString* output) +{ + char val[5]; + int i; + for(i = 0; i < 16; i++) + { + snprintf(val, 4, "%02x", (unsigned char) data[i]); + dynStringPushBackStr(output,val); + } +} + +void cRosMessageInit(cRosMessage *message) +{ + message->fields = NULL; + message->n_fields = 0; + message->msgDef = NULL; + + message->md5sum = (char *)calloc(33, sizeof(char)); // 32 chars + '\0'; +} + +cRosMessage * cRosMessageNew(void) +{ + cRosMessage *ret = (cRosMessage *)calloc(1, sizeof(cRosMessage)); + + if (ret == NULL) + return NULL; + + cRosMessageInit(ret); + + if (ret->md5sum == NULL) + { + free(ret); + return NULL; + } + + return ret; +} + +void cRosMessageFieldInit(cRosMessageField *field) +{ + if(field != NULL) + { + field->is_const = 0; + field->name = NULL; + field->type = CROS_CUSTOM_TYPE; + field->type_s = NULL; + field->size = -1; + field->is_array = 0; + field->is_fixed_array = 0; + field->array_size = -1; + field->array_capacity = -1; + memset(field->data.opaque, 0, sizeof(field->data.opaque)); + } +} + +// Load message specification from a string: +// types, names, constants +cRosErrCodePack loadFromStringMsg(char* text, cRosMessageDef* msg) +{ + char* new_line = NULL; + char* new_line_saveptr = NULL; + const char* delimiter = "\n"; + + msg->plain_text = strdup(text); + if(msg->plain_text == NULL) + return CROS_MEM_ALLOC_ERR; + + new_line = strtok_r(text, delimiter, &new_line_saveptr); + while(new_line != NULL) + { + char* iterator = new_line; + int comment_char_found = 0; + int char_count = 0; // Number of characters in the line before the comment + + //strip comments and new line + while(((char)*iterator) != '\0') + { + if(((char)*iterator) == *CHAR_COMMENT) + { + comment_char_found = 1; + break; + } + char_count++; + iterator++; + } + + // ignore empty lines + if(comment_char_found) + { + if(char_count == 0) + { + new_line = strtok_r(NULL, delimiter, &new_line_saveptr); + continue; + } + else + { + //remove spaces between the message entry and the comment + while(*(new_line + char_count - 1) == ' ') + { + char_count--; + } + } + } + + char* msg_entry = (char *)calloc(char_count + 1, sizeof(char)); // type/name + strncpy(msg_entry, new_line, char_count); + char* entry_type; + char* entry_name; + + char* msg_entry_itr = msg_entry; + + while(*msg_entry_itr != '\0' && *(msg_entry_itr++) != ' '); // Skip character at the beginning of the line until a space is found + if(*(msg_entry_itr - 1) != ' ') // Error no space found + { + free(msg_entry); + return CROS_FILE_ENTRY_NO_SEP_ERR; + } + *(msg_entry_itr - 1) = '\0'; // Convert the found space into a \0 + + entry_name = (char *)calloc(strlen(msg_entry_itr) + 1, sizeof(char)); + strcpy(entry_name, msg_entry_itr); + entry_type = base_msg_type(msg_entry); // Return the type of the entry in a new allocated memory string + + char* filled_count = msg_entry_itr; + + while(*filled_count != '\0') // ??? Any purpose here + { + if(*filled_count == ' ') + { + msg_entry_itr = filled_count + 1; + while(*msg_entry_itr != '\0' && *msg_entry_itr == ' ' ) + msg_entry_itr++; + *filled_count = *msg_entry_itr; + if(*msg_entry_itr != '\0') + *msg_entry_itr = ' '; + } + else + { + filled_count ++; + } + } + + if(!is_valid_msg_type(entry_type)) + { + free(entry_name); + free(entry_type); + free(msg_entry); + return CROS_FILE_ENTRY_TYPE_ERR; + } + + char* const_char_ptr = strpbrk(entry_name, CHAR_CONST); + + if( const_char_ptr != NULL) // The entry is a constant definition + { + char* entry_const_val; + if(strcmp(entry_type, "string") == 0) + { + // String constants + // Strings contain anything to the right of the equals sign, + // there are no comments allowed + + char* entry_name_saveptr = NULL; + strtok_r(entry_name,CHAR_CONST,&entry_name_saveptr); + entry_const_val = strtok_r(NULL,CHAR_CONST,&entry_name_saveptr); + } + else + { + //Number constants + char* entry_name_saveptr = NULL; + strtok_r(entry_name,CHAR_CONST,&entry_name_saveptr); + entry_const_val = strtok_r(NULL,CHAR_CONST,&entry_name_saveptr); + } + + // TODO: try to cast number values + + msgConst* current = msg->constants; + + current->name = entry_name; + current->type = getMessageType(entry_type); + if (current->type == CROS_CUSTOM_TYPE) + { + current->type_s = entry_type; + entry_type = NULL; + } + current->value = strdup(entry_const_val); // strdup() will allocate a memory buffer that is independent of entry_name memory buffer so that it can freed independently as well + current->next = (msgConst *)malloc(sizeof(msgConst)); + msgConst* next = current->next; + initMsgConst(next); + next->prev = current; + msg->constants = next; + } + else // The entry is a data type declaration + { + msgFieldDef* current = msg->fields; + + current->name = entry_name; + current->type = getMessageType(entry_type); + if (current->type == CROS_CUSTOM_TYPE) + { + cRosErrCodePack ret_err; + + if(strstr(entry_type,"/") == NULL) + { + current->type_s = (char *)calloc(strlen(msg->package)+ 1 + strlen(entry_type) + 1, sizeof(char)); + memcpy(current->type_s, msg->package, strlen(msg->package)+ 1); + strcat(current->type_s,"/"); + strcat(current->type_s,entry_type); + } + else + { + current->type_s = entry_type; + entry_type = NULL; // Indicate that this buffer must not be freed later since it is used for the current msg def field + } + // Recursively load the msg definition of the custom type and store it into current->child_msg_def + ret_err = cRosMessageDefBuild(¤t->child_msg_def, msg->root_dir, current->type_s); + if(ret_err != CROS_SUCCESS_ERR_PACK) + { + free(current->type_s); + current->type_s = NULL; + free(entry_name); + current->name = NULL; + free(entry_type); + free(msg_entry); + return ret_err; + } + } + + int array_size; + if(is_array_type(msg_entry, &array_size)) + { + current->is_array = 1; + current->array_size = array_size; + } + + current->next = (msgFieldDef *)malloc(sizeof(msgFieldDef)); + msgFieldDef* next = current->next; + initFieldDef(next); + next->prev = current; + msg->fields = next; + } + + if (entry_type != NULL) + free(entry_type); + + free(msg_entry); + + new_line = strtok_r(NULL, delimiter, &new_line_saveptr); + } + + return CROS_SUCCESS_ERR_PACK; +} + +cRosErrCodePack loadFromFileMsg(char* filename, cRosMessageDef* msg) +{ + cRosErrCodePack ret_err; + size_t f_read_bytes; + char* file_tokenized; + FILE *f; + char* token_pack = NULL; + char* token_root = NULL; + char* token_name = NULL; + + f = fopen(filename, "rb"); + if (f == NULL) + return CROS_OPEN_MSG_FILE_ERR; + + // Load the entire message definition file in a buffer + fseek(f, 0, SEEK_END); + long fsize = ftell(f); + fseek(f, 0, SEEK_SET); + char *msg_text = (char *)malloc(fsize + 1); + if(msg_text == NULL) + { + fclose(f); + return CROS_MEM_ALLOC_ERR; + } + + f_read_bytes = fread(msg_text, 1, fsize, f); + fclose(f); + + if(f_read_bytes != (size_t)fsize) + { + free(msg_text); + return CROS_READ_MSG_FILE_ERR; + } + + msg_text[fsize] = '\0'; + + file_tokenized = (char *)calloc(strlen(filename)+1, sizeof(char)); + if(file_tokenized == NULL) + { + free(msg_text); + return CROS_MEM_ALLOC_ERR; + } + strcpy(file_tokenized, filename); + char* tok = strtok(file_tokenized,SPLIT_REGEX); + + while(tok != NULL) + { + if(strcmp(tok, "msg") != 0) + { + token_root = token_pack; + token_pack = token_name; + token_name = tok ; + } + tok = strtok(NULL,SPLIT_REGEX); + } + + //build up the root path + char* it = file_tokenized; + while(it != token_root) + { + if(*it == '\0') + *it=DIR_SEPARATOR_CHAR; + it++; + } + + msg->root_dir = (char*) malloc ((strlen(file_tokenized)+1) * sizeof(char)); // msg->root_dir[0] = '\0'; + msg->package = (char*) malloc ((strlen(token_pack)+1) * sizeof(char)); // msg->package[0] = '\0'; + msg->name = (char*) malloc ((strlen(token_name)+1) * sizeof(char)); // msg->name[0] = '\0'; + if(msg->root_dir != NULL && msg->package != NULL && msg->name != NULL) + { + strcpy(msg->root_dir,file_tokenized); + strcpy(msg->package,token_pack); + strcpy(msg->name,token_name); + ret_err = loadFromStringMsg(msg_text, msg); + ret_err = cRosAddErrCodeIfErr(ret_err, CROS_LOAD_MSG_FILE_ERR); // If an error occurred (the file could not be loaded), add more info (CROS_LOAD_MSG_FILE_ERR error code) + } + else + { + free(msg->root_dir); + free(msg->package); + free(msg->name); + ret_err = CROS_MEM_ALLOC_ERR; + } + + free(msg_text); + free(file_tokenized); + + return ret_err; +} + +cRosMessage *build_time_field(void) +{ + cRosMessage* time_msg = cRosMessageNew(); + if(time_msg != NULL) + { + time_msg->fields = (cRosMessageField **)calloc(2,sizeof(cRosMessageField*)); + if(time_msg->fields != NULL) + { + time_msg->n_fields = 2; + + cRosMessageField* sec = cRosMessageFieldNew(); + cRosMessageField* nsec = cRosMessageFieldNew(); + if(sec != NULL && nsec != NULL) + { + //int32 sec + sec->name = strdup("secs"); + sec->type = CROS_STD_MSGS_UINT32; + sec->size = getMessageTypeSizeOf(sec->type); + time_msg->fields[0] = sec; + + //int32 nsec + nsec->name = strdup("nsecs"); + nsec->type = CROS_STD_MSGS_UINT32; + nsec->size = getMessageTypeSizeOf(nsec->type); + time_msg->fields[1] = nsec; + + if(sec->name == NULL || nsec->name == NULL) + { + cRosMessageFree(time_msg); + time_msg = NULL; + } + } + else + { + cRosMessageFieldFree(sec); + cRosMessageFieldFree(nsec); + cRosMessageFree(time_msg); + time_msg = NULL; + } + } + else + { + cRosMessageFree(time_msg); + time_msg = NULL; + } + } + + return time_msg; +} + +cRosMessage *build_duration_field(void) +{ + cRosMessage* durat_msg = cRosMessageNew(); + if(durat_msg != NULL) + { + durat_msg->fields = (cRosMessageField **)calloc(2,sizeof(cRosMessageField*)); + if(durat_msg->fields != NULL) + { + durat_msg->n_fields = 2; + + cRosMessageField* sec = cRosMessageFieldNew(); + cRosMessageField* nsec = cRosMessageFieldNew(); + if(sec != NULL && nsec != NULL) + { + //int32 sec + sec->name = strdup("secs"); + sec->type = CROS_STD_MSGS_INT32; + sec->size = getMessageTypeSizeOf(sec->type); + durat_msg->fields[0] = sec; + + //int32 nsec + nsec->name = strdup("nsecs"); + nsec->type = CROS_STD_MSGS_INT32; + nsec->size = getMessageTypeSizeOf(nsec->type); + durat_msg->fields[1] = nsec; + + if(sec->name == NULL || nsec->name == NULL) + { + cRosMessageFree(durat_msg); + durat_msg = NULL; + } + } + else + { + cRosMessageFieldFree(sec); + cRosMessageFieldFree(nsec); + cRosMessageFree(durat_msg); + durat_msg = NULL; + } + } + else + { + cRosMessageFree(durat_msg); + durat_msg = NULL; + } + } + + return durat_msg; +} + +cRosMessage *build_header_field(void) +{ + cRosMessage* header_msg = cRosMessageNew(); + if(header_msg != NULL) + { + header_msg->fields = (cRosMessageField **)calloc(3,sizeof(cRosMessageField*)); + if(header_msg->fields != NULL) + { + header_msg->n_fields = 3; + + cRosMessageField* sequence_id = cRosMessageFieldNew(); + cRosMessageField* time_stamp = cRosMessageFieldNew(); + cRosMessageField* frame_id = cRosMessageFieldNew(); + if(sequence_id != NULL && time_stamp != NULL && frame_id != NULL) + { + //uint32 seq + sequence_id->name = strdup("seq"); + sequence_id->type = CROS_STD_MSGS_UINT32; + sequence_id->size = getMessageTypeSizeOf(sequence_id->type); + header_msg->fields[0] = sequence_id; + + // time stamp + // Two-integer timestamp that is expressed as: + // * stamp.secs: seconds (stamp_secs) since epoch + // * stamp.nsecs: nanoseconds since stamp_secs + time_stamp->name = strdup("stamp"); + time_stamp->type = CROS_STD_MSGS_TIME; + time_stamp->data.as_msg = build_time_field(); + header_msg->fields[1] = time_stamp; + + //string frame_id + frame_id->name = strdup("frame_id"); + frame_id->type = CROS_STD_MSGS_STRING; + header_msg->fields[2] = frame_id; + if(sequence_id->name == NULL || time_stamp->name == NULL || frame_id->name == NULL) + { + cRosMessageFree(header_msg); + header_msg = NULL; + } + } + else + { + cRosMessageFieldFree(frame_id); + cRosMessageFieldFree(time_stamp); + cRosMessageFieldFree(sequence_id); + cRosMessageFree(header_msg); + header_msg = NULL; + } + } + else + { + cRosMessageFree(header_msg); + header_msg = NULL; + } + } + + return header_msg; +} + +cRosErrCodePack cRosMessageDefBuild(cRosMessageDef **msg_def_ptr, const char *msg_root_dir, const char *msg_type) +{ + cRosErrCodePack ret; + cRosMessageDef* msg_def = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); + if(msg_def == NULL) + return CROS_MEM_ALLOC_ERR; + + ret = initCrosMsg(msg_def); + if(ret == CROS_SUCCESS_ERR_PACK) + { + char* msg_file_path; + if(msg_root_dir != NULL) // If msg_root_dir parameter is not NULL, the file path is composed, otherwise, msg_type is used directly as file path + { + msg_file_path = (char *)malloc(strlen(msg_root_dir) + strlen(DIR_SEPARATOR_STR) + strlen(msg_type) + strlen(FILEEXT_MSG) + 2); + if(msg_file_path != NULL) + { + strcpy(msg_file_path, msg_root_dir); + strcat(msg_file_path, DIR_SEPARATOR_STR); + strcat(msg_file_path, msg_type); + strcat(msg_file_path, "."); + strcat(msg_file_path, FILEEXT_MSG); + } + else + ret = CROS_MEM_ALLOC_ERR; + } + else + msg_file_path = (char *)msg_type; + if(ret == CROS_SUCCESS_ERR_PACK) + { + ret = loadFromFileMsg(msg_file_path, msg_def); + if(msg_root_dir != NULL) + free(msg_file_path); + + if (ret == CROS_SUCCESS_ERR_PACK) + *msg_def_ptr = msg_def; + else + cRosMessageDefFree(msg_def); + } + } + else + free(msg_def); + + return ret; +} + +int cRosMessageFieldCopy(cRosMessageField* new_field, cRosMessageField* orig_field) +{ + int ret; + if(new_field != NULL && orig_field != NULL) + { + new_field->size = orig_field->size; + new_field->is_const = orig_field->is_const; + new_field->is_array = orig_field->is_array; + new_field->is_fixed_array = orig_field->is_fixed_array; + new_field->array_size = orig_field->array_size; + new_field->array_capacity = orig_field->array_capacity; + new_field->type = orig_field->type; + new_field->type_s = (orig_field->type_s != NULL)? strdup(orig_field->type_s):NULL; + new_field->name = (orig_field->name != NULL)? strdup(orig_field->name):NULL; + if((new_field->type_s == NULL && orig_field->type_s != NULL) || (new_field->name == NULL && orig_field->name != NULL)) + { + free(new_field->type_s); + free(new_field->name); + ret=-1; + } + else + { + ret=0; // Default return value + if(!orig_field->is_array) + { + switch (orig_field->type) + { + case CROS_STD_MSGS_INT8: + case CROS_STD_MSGS_UINT8: + case CROS_STD_MSGS_INT16: + case CROS_STD_MSGS_UINT16: + case CROS_STD_MSGS_INT32: + case CROS_STD_MSGS_UINT32: + case CROS_STD_MSGS_INT64: + case CROS_STD_MSGS_UINT64: + case CROS_STD_MSGS_FLOAT32: + case CROS_STD_MSGS_FLOAT64: + case CROS_STD_MSGS_BOOL: + case CROS_STD_MSGS_CHAR: + case CROS_STD_MSGS_BYTE: + { + new_field->data = orig_field->data; + break; + } + case CROS_STD_MSGS_STRING: + { + new_field->data.as_string = (orig_field->data.as_string != NULL)? strdup(orig_field->data.as_string) : NULL; + if(orig_field->data.as_string != NULL && new_field->data.as_string == NULL) + ret=-1; // Error allocating memory for string + break; + } + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_HEADER: + case CROS_CUSTOM_TYPE: + { + if(!orig_field->is_array) + { + new_field->data.as_msg = cRosMessageCopyWithoutDef(orig_field->data.as_msg); + if(new_field->data.as_msg == NULL) + ret=-1; + } + break; + } + } + } + else + { + // Allocate the array + if(orig_field->type == CROS_STD_MSGS_STRING) // The array is encoded as a pointer to pointers + new_field->data.as_array = calloc(orig_field->array_capacity, sizeof(char *)); // String pointers + else if(orig_field->type == CROS_STD_MSGS_TIME || orig_field->type == CROS_STD_MSGS_DURATION || + orig_field->type == CROS_STD_MSGS_HEADER || orig_field->type == CROS_CUSTOM_TYPE) + new_field->data.as_array = calloc(orig_field->array_capacity, sizeof(cRosMessage *)); // Message pointers + else // The array is encoded as a pointer to elements + new_field->data.as_array = calloc(orig_field->array_capacity, orig_field->size); // Element pointers + if(new_field->data.as_array != NULL) // If the array was allocated, copy the elements + { + switch (orig_field->type) + { + case CROS_STD_MSGS_STRING: + { + int n_str; + for(n_str=0;n_strarray_size && ret==0;n_str++) + { + new_field->data.as_string_array[n_str] = (orig_field->data.as_string_array[n_str] != NULL)?strdup(orig_field->data.as_string_array[n_str]):NULL; + if(new_field->data.as_string_array[n_str] == NULL && orig_field->data.as_string_array[n_str] != NULL) + ret=-1; + } + if(ret != 0) // Error allocating memory for strings: free previously allocated string memory + for(n_str=0;n_strarray_size && ret==0;n_str++) + free(new_field->data.as_string_array[n_str]); + break; + } + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_HEADER: + case CROS_CUSTOM_TYPE: + { + int n_msg; + for(n_msg=0;n_msgarray_size && ret==0;n_msg++) + { + new_field->data.as_msg_array[n_msg] = cRosMessageCopyWithoutDef(orig_field->data.as_msg_array[n_msg]); + if(new_field->data.as_msg_array[n_msg] == NULL && orig_field->data.as_msg_array[n_msg] != NULL) + ret=-1; + } + if(ret != 0) // Error allocating memory for messages: free previously allocated message memory + for(n_msg=0;n_msgarray_size && ret==0;n_msg++) + cRosMessageFree(new_field->data.as_msg_array[n_msg]); + break; + } + default: + { + memcpy(new_field->data.as_array, orig_field->data.as_array, orig_field->size * orig_field->array_size); + break; + } + } + if(ret != 0) + { + free(new_field->data.as_array); + } + } + else + ret=-1; // Error allocating the array + } + if(ret != 0) // Free memory to leave the message in a determined state + { + free(new_field->type_s); + free(new_field->name); + } + } + } + else + ret=-1; + return ret; +} + +void printNSpaces(int n_spaces) +{ + int spc_ind; + for(spc_ind=0;spc_indname!=NULL)?msg_field->name:"NULL", msg_field->size, msg_field->is_const, \ + msg_field->is_array, msg_field->is_fixed_array, msg_field->array_size, msg_field->array_capacity, \ + (msg_field->type_s!=NULL)?msg_field->type_s:"NULL", getMessageTypeDeclaration(msg_field->type)); + switch(msg_field->type) + { + case CROS_STD_MSGS_INT8: + { + if(!msg_field->is_array) + fprintf(msg_out,"%hi", (short)msg_field->data.as_int8); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%hi,", (short)msg_field->data.as_int8_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_UINT8: + { + if(!msg_field->is_array) + fprintf(msg_out,"%hu", (unsigned short)msg_field->data.as_uint8); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%hu,", (unsigned short)msg_field->data.as_uint8_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_INT16: + { + if(!msg_field->is_array) + fprintf(msg_out,"%hi", msg_field->data.as_int16); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%hi,", msg_field->data.as_int16_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_UINT16: + { + if(!msg_field->is_array) + fprintf(msg_out,"%hu", msg_field->data.as_uint16); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%hu,", msg_field->data.as_uint16_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_INT32: + { + if(!msg_field->is_array) + fprintf(msg_out,"%li", (long)msg_field->data.as_int32); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%li,", (long)msg_field->data.as_int32_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_UINT32: + { + if(!msg_field->is_array) + fprintf(msg_out,"%lu", (long unsigned)msg_field->data.as_uint32); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%lu,", (long unsigned)msg_field->data.as_uint32_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_INT64: + { + if(!msg_field->is_array) + fprintf(msg_out,"%lli", (long long)msg_field->data.as_int64); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%lli,", (long long)msg_field->data.as_int64_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_UINT64: + { + if(!msg_field->is_array) + fprintf(msg_out,"%llu", (long long unsigned)msg_field->data.as_uint64); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%llu,", (long long unsigned)msg_field->data.as_uint64_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_FLOAT32: + { + if(!msg_field->is_array) + fprintf(msg_out,"%f", msg_field->data.as_float32); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%f,", msg_field->data.as_float32_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_FLOAT64: + { + if(!msg_field->is_array) + fprintf(msg_out,"%f", msg_field->data.as_float64); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%f,", msg_field->data.as_float64_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_BOOL: + { + if(!msg_field->is_array) + fprintf(msg_out,"%hu", (unsigned short)msg_field->data.as_uint8); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%hu,", (unsigned short)msg_field->data.as_uint8_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_CHAR: + { + if(!msg_field->is_array) + fprintf(msg_out,"%hu", (unsigned short)msg_field->data.as_uint8); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%hu,", (unsigned short)msg_field->data.as_uint8_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_BYTE: + { + if(!msg_field->is_array) + fprintf(msg_out,"%hi", (short)msg_field->data.as_int8); + else + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"%hi,", (short)msg_field->data.as_int8_array[elem_ind]); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + break; + } + case CROS_STD_MSGS_STRING: + { + if(!msg_field->is_array) + fprintf(msg_out,"'%s'", (msg_field->data.as_string!=NULL)?msg_field->data.as_string:"NULL"); + else + { + if(msg_field->data.as_string_array != NULL) + { + for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) + fprintf(msg_out,"'%s',", (msg_field->data.as_string_array[elem_ind]!=NULL)?msg_field->data.as_string_array[elem_ind]:"NULL"); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + else + fprintf(msg_out,"NULL"); + } + break; + } + default: + { + break; + } + } + // The value of these last types are printed after the switch statement because they share the same code + if(msg_field->type == CROS_STD_MSGS_TIME || msg_field->type == CROS_STD_MSGS_DURATION || + msg_field->type == CROS_STD_MSGS_HEADER || msg_field->type == CROS_CUSTOM_TYPE) + { + if(!msg_field->is_array) + cRosMessageFieldsPrint(msg_field->data.as_msg, n_indent); + else + { + fprintf(msg_out,"\n"); + for(elem_ind=0;elem_ind < msg_field->array_size;elem_ind++) + cRosMessageFieldsPrint(msg_field->data.as_msg_array[elem_ind], n_indent); + if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) + fprintf(msg_out,"..."); + } + } + else + fprintf(msg_out,"\n"); + } + else + fprintf(msg_out,"Field NULL\n"); +} + +// This function prints the MD5 and all the fields (fields field struct) of one message (msg) to the output file stream. +void cRosMessageFieldsPrint(cRosMessage *msg, int n_indent) +{ + printNSpaces(n_indent); + if(msg != NULL) + { + fprintf(cRosOutStreamGet(), "MsgAt 0x%p: MD5:'%s' N.Flds:%i Flds:%s\n", msg, (msg->md5sum != NULL)? msg->md5sum: "NULL", msg->n_fields, (msg->fields != NULL)?"":"NULL"); + if(msg->fields != NULL) + { + int field_ind; + // Print all the filed in source message while no error occurs + for(field_ind=0;field_indn_fields;field_ind++) + cRosMessageFieldPrint(msg->fields[field_ind], n_indent+1); + } + } + else + fprintf(cRosOutStreamGet(), "MsgAt NULL\n"); +} + +// This function copies the MD5 and all the fields (fields field struct) from one message (m_src) to another (m_dst). +// It if expected than the fields field of m_dst is NULL. +int cRosMessageFieldsCopy(cRosMessage *m_dst, cRosMessage *m_src) +{ + int ret; + + if(m_src != NULL && m_src != NULL) + { + if(m_src->md5sum != NULL) // If source message has a valid MD5 field, copy it + { + if(m_dst->md5sum != NULL) + { + strcpy(m_dst->md5sum, m_src->md5sum); + ret=0; + } + else + { + m_dst->md5sum = strdup(m_src->md5sum); + ret = (m_dst->md5sum != NULL)?0:-1; + } + } + else + { + free(m_dst->md5sum); + m_dst->md5sum=NULL; + ret=0; + } + } + else + ret=-1; + if(ret == 0) // If no error copying MD5 field, continue + { + // Remove previous fields from destination message + cRosMessageFieldsFree(m_dst); + // Create a new field array in destination message + m_dst->fields = (cRosMessageField **)calloc(m_src->n_fields, sizeof(cRosMessageField*)); + if(m_dst->fields != NULL) + { + int field_ind; + + m_dst->n_fields = m_src->n_fields; + // Copy all the filed in source message while no error occurs + for(field_ind=0;field_indn_fields && ret==0;field_ind++) + { + cRosMessageField *new_field = cRosMessageFieldNew(); + if(new_field != NULL) + { + ret = cRosMessageFieldCopy(new_field, m_src->fields[field_ind]); + if(ret == 0) + m_dst->fields[field_ind] = new_field; + else + free(new_field); + } + else + ret=-1; + } + if(ret != 0) // Error copying fields + { + // Destroy already created fields + cRosMessageFieldsFree(m_dst); + } + } + else + ret=-1; + } + return ret; +} + +cRosMessage *cRosMessageCopyWithoutDef(cRosMessage *m_src) +{ + cRosMessage *m_dst; + if(m_src != NULL) + { + m_dst = cRosMessageNew(); + if(m_dst != NULL) + { + if(cRosMessageFieldsCopy(m_dst, m_src) != 0) + { + // Error copying fields: free the new message + cRosMessageFree(m_dst); + m_dst=NULL; // Return NULL (error indicator) + } + } + } + else + m_dst = NULL; + return m_dst; +} + +cRosMessage *cRosMessageCopy(cRosMessage *m_src) +{ + cRosMessage *m_dst; + if(m_src != NULL) + { + m_dst = cRosMessageNew(); + if(m_dst != NULL) + { + if(cRosMessageFieldsCopy(m_dst, m_src) == 0) + { + cRosErrCodePack ret_err; + ret_err = cRosMessageDefCopy(&m_dst->msgDef, m_src->msgDef ); + if(ret_err != CROS_SUCCESS_ERR_PACK) + { + cRosMessageFree(m_dst); + m_dst=NULL; // Return NULL (error indicator) + } + } + else + { + // Error copying fields: free the new message + cRosMessageFree(m_dst); + m_dst=NULL; // Return NULL (error indicator) + } + } + } + else + m_dst = NULL; + return m_dst; +} + +cRosErrCodePack cRosFieldDefCopy(msgFieldDef* new_field_def, msgFieldDef* orig_field_def ) +{ + cRosErrCodePack ret; + if(new_field_def != NULL && orig_field_def != NULL) + { + new_field_def->type = orig_field_def->type; + new_field_def->is_array = orig_field_def->is_array; + new_field_def->array_size = orig_field_def->array_size; + new_field_def->next = NULL; + new_field_def->prev = NULL; + new_field_def->type_s = (orig_field_def->type_s != NULL)? strdup(orig_field_def->type_s):NULL; + new_field_def->name = (orig_field_def->name != NULL)? strdup(orig_field_def->name):NULL; + if((new_field_def->type_s == NULL && orig_field_def->type_s != NULL) || (new_field_def->name == NULL && orig_field_def->name != NULL)) + ret=CROS_MEM_ALLOC_ERR; + else + ret = cRosMessageDefCopy(&new_field_def->child_msg_def, orig_field_def->child_msg_def ); + + if(ret != CROS_SUCCESS_ERR_PACK) + { + free(new_field_def->type_s); + free(new_field_def->name); + new_field_def->type_s = NULL; + new_field_def->name = NULL; + } + } + else + ret=CROS_BAD_PARAM_ERR; + return ret; +} + +cRosErrCodePack cRosConstDefCopy(msgConst* new_const_def, msgConst* orig_const_def ) +{ + cRosErrCodePack ret; + if(new_const_def != NULL && orig_const_def != NULL) + { + new_const_def->type = orig_const_def->type; + new_const_def->next = NULL; + new_const_def->prev = NULL; + new_const_def->type_s = (orig_const_def->type_s != NULL)? strdup(orig_const_def->type_s):NULL; + new_const_def->name = (orig_const_def->name != NULL)? strdup(orig_const_def->name):NULL; + new_const_def->value = (orig_const_def->value != NULL)? strdup(orig_const_def->value):NULL; + if((new_const_def->type_s == NULL && orig_const_def->type_s != NULL) || + (new_const_def->name == NULL && orig_const_def->name != NULL) || + (new_const_def->value == NULL && orig_const_def->value != NULL)) + { + free(new_const_def->type_s); + free(new_const_def->name); + free(new_const_def->value); + ret=CROS_MEM_ALLOC_ERR; + } + else + ret=CROS_SUCCESS_ERR_PACK; + } + else + ret=CROS_BAD_PARAM_ERR; + return ret; +} + +// Make a copy of a message definition struct allocating new memory for it, so that all fields can be freed independently from the originals +cRosErrCodePack cRosMessageDefCopy(cRosMessageDef** ptr_new_msg_def, cRosMessageDef* orig_msg_def ) +{ + cRosErrCodePack ret; + + if(orig_msg_def == NULL) + { + *ptr_new_msg_def = NULL; + return CROS_SUCCESS_ERR_PACK; + } + + *ptr_new_msg_def = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); + if(*ptr_new_msg_def != NULL) + { + initCrosMsg(*ptr_new_msg_def); + (*ptr_new_msg_def)->name = (orig_msg_def->name != NULL)? strdup(orig_msg_def->name):NULL; // NULL should not be passed to strdup() + (*ptr_new_msg_def)->package = (orig_msg_def->package != NULL)? strdup(orig_msg_def->package):NULL; + (*ptr_new_msg_def)->root_dir = (orig_msg_def->root_dir != NULL)? strdup(orig_msg_def->root_dir):NULL; + (*ptr_new_msg_def)->plain_text = (orig_msg_def->plain_text != NULL)? strdup(orig_msg_def->plain_text):NULL; + + ret=CROS_SUCCESS_ERR_PACK; // Default return value: no error + // Copy the first field + if(orig_msg_def->first_field != NULL) + { + msgFieldDef *new_field_itr, *orig_field_itr; + + orig_field_itr = orig_msg_def->first_field; + new_field_itr = (*ptr_new_msg_def)->first_field; + + ret=cRosFieldDefCopy(new_field_itr, orig_field_itr); // Copy the first msg def field + + // Copy the rest of message definition fields + while(orig_field_itr->next != NULL && ret == CROS_SUCCESS_ERR_PACK) + { + new_field_itr->next = (msgFieldDef *)malloc(sizeof(msgFieldDef)); + if(new_field_itr->next != NULL) + { + initFieldDef(new_field_itr->next); + ret=cRosFieldDefCopy(new_field_itr->next, orig_field_itr->next); + if(ret == CROS_SUCCESS_ERR_PACK) + { + new_field_itr->next->prev = new_field_itr; + new_field_itr = new_field_itr->next; + orig_field_itr = orig_field_itr->next; + } + else // Error allocating memory: discard last filed and exit the loop + { + free(new_field_itr->next); + new_field_itr->next = NULL; + } + } + else + ret=CROS_MEM_ALLOC_ERR; // Error allocating memory: exit the loop + } + (*ptr_new_msg_def)->fields=new_field_itr; // Last valid field + } + else + { + free((*ptr_new_msg_def)->first_field); + (*ptr_new_msg_def)->first_field=NULL; + (*ptr_new_msg_def)->fields=NULL; + } + + // Copy the first constant + if(orig_msg_def->first_const != NULL) + { + msgConst *new_const_itr, *orig_const_itr; + + orig_const_itr = orig_msg_def->first_const; + new_const_itr = (*ptr_new_msg_def)->first_const; + + ret=cRosConstDefCopy(new_const_itr, orig_const_itr); + + // Copy message definition constants + while(orig_const_itr->next != NULL && ret == CROS_SUCCESS_ERR_PACK) + { + new_const_itr->next = (msgConst *)malloc(sizeof(msgConst)); + if(new_const_itr->next != NULL) + { + initMsgConst(new_const_itr->next); + ret=cRosConstDefCopy(new_const_itr->next, orig_const_itr->next); + if(ret == CROS_SUCCESS_ERR_PACK) + { + new_const_itr->next->prev = new_const_itr; + new_const_itr = new_const_itr->next; + orig_const_itr = orig_const_itr->next; + } + else // Error allocating memory: discard last filed and exit the loop + { + free(new_const_itr->next); + new_const_itr->next = NULL; + } + } + else + ret=CROS_MEM_ALLOC_ERR; // Error allocating memory: exit the loop + } + (*ptr_new_msg_def)->constants = new_const_itr; // Last valid constant + + } + else + { + free((*ptr_new_msg_def)->first_const); + (*ptr_new_msg_def)->first_const=NULL; + (*ptr_new_msg_def)->constants=NULL; + } + if(ret != CROS_SUCCESS_ERR_PACK) // Error making the msg. def. copy: free all the memory that we have allocated + { + cRosMessageDefFree(*ptr_new_msg_def); + *ptr_new_msg_def = NULL; + } + } + else + ret=CROS_MEM_ALLOC_ERR; + return(ret); +} + +cRosErrCodePack cRosMessageNewBuild(const char *msg_root_dir, const char *msg_type, cRosMessage **new_msg_ptr) +{ + cRosErrCodePack ret_err; + + if(new_msg_ptr != NULL) + { + cRosMessageDef *msg_def; + ret_err = cRosMessageDefBuild(&msg_def, msg_root_dir, msg_type); + if(ret_err == CROS_SUCCESS_ERR_PACK) // Check if we got any error while building message definition + { + ret_err = cRosMessageBuildFromDef(new_msg_ptr, msg_def); // Copy msg_def into message->msgDef and build the message (original msg_def can be freed after the copy) + cRosMessageDefFree(msg_def); + } + } + else + ret_err = CROS_BAD_PARAM_ERR; + return ret_err; +} + +cRosErrCodePack cRosMessageBuildFromDef(cRosMessage** message_ptr, cRosMessageDef* msg_def ) +{ + cRosErrCodePack ret; + cRosMessage* message; + + ret = CROS_SUCCESS_ERR_PACK; // Default return value: success + + message = cRosMessageNew(); + if(message == NULL) + return CROS_MEM_ALLOC_ERR; + + DynString output; + dynStringInit(&output); + + cRosMessageDefFree(message->msgDef); // Just in case there was a previous message definition in the message + cRosMessageDefCopy(&message->msgDef, msg_def ); + + unsigned char* res = getMD5Msg(msg_def); + cRosMD5Readable(res, &output); + free(res); + strcpy(message->md5sum, dynStringGetData(&output)); + dynStringRelease(&output); + + msgFieldDef* field_def_itr = msg_def->first_field; + while(field_def_itr->next != NULL) + { + message->n_fields++; + field_def_itr = field_def_itr->next; + } + + message->fields = (cRosMessageField **)calloc(message->n_fields, sizeof(cRosMessageField*)); + + field_def_itr = msg_def->first_field; + cRosMessageField** msg_field_itr = message->fields; + + while(field_def_itr->next != NULL && ret == CROS_SUCCESS_ERR_PACK) + { + *msg_field_itr = (cRosMessageField *)malloc(sizeof(cRosMessageField)); + cRosMessageField* field = *msg_field_itr; + cRosMessageFieldInit(field); + + field->type = field_def_itr->type; + field->name = (char *)calloc(strlen(field_def_itr->name)+1,sizeof(char)); + strcpy(field->name, field_def_itr->name); + if (field_def_itr->type_s != NULL) + { + field->type_s = (char *)calloc(strlen(field_def_itr->type_s)+1,sizeof(char)); + strcpy(field->type_s, field_def_itr->type_s); + } + + field->is_array = field_def_itr->is_array; + if(field_def_itr->is_array) + { + if (field_def_itr->array_size == -1) // Variable-length array + { + field->array_size = 0; + field->array_capacity = 1; // If we don't now the array length, allocate memory for at least one element + } + else // Fixed-length array + { + field->array_size = field_def_itr->array_size; + field->array_capacity = field_def_itr->array_size; + field->is_fixed_array = 1; + } + } + switch (field->type) + { + case CROS_STD_MSGS_INT8: + case CROS_STD_MSGS_UINT8: + case CROS_STD_MSGS_INT16: + case CROS_STD_MSGS_UINT16: + case CROS_STD_MSGS_INT32: + case CROS_STD_MSGS_UINT32: + case CROS_STD_MSGS_INT64: + case CROS_STD_MSGS_UINT64: + case CROS_STD_MSGS_FLOAT32: + case CROS_STD_MSGS_FLOAT64: + case CROS_STD_MSGS_BOOL: + case CROS_STD_MSGS_CHAR: + case CROS_STD_MSGS_BYTE: + { + field->size = getMessageTypeSizeOf(field->type); + if(field_def_itr->is_array) + { + field->data.as_array = calloc(field->array_capacity,field->size); + if(field->data.as_array == NULL) + ret=CROS_MEM_ALLOC_ERR; + } + break; + } + case CROS_STD_MSGS_STRING: + { + if(field_def_itr->is_array) + { + field->data.as_string_array = (char **)calloc(field->array_capacity,sizeof(char *)); + if(field->data.as_string_array == NULL) + ret=CROS_MEM_ALLOC_ERR; + } + break; + } + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_HEADER: + case CROS_CUSTOM_TYPE: + { + cRosMessage *new_msg; + if(field_def_itr->is_array) + { + field->data.as_msg_array = (cRosMessage **)calloc(field->array_capacity,sizeof(cRosMessage *)); + if(field->data.as_msg_array != NULL) + { + int msg_ind; + + for(msg_ind=0;msg_indarray_size && ret == CROS_SUCCESS_ERR_PACK;msg_ind++) + { + new_msg = NULL; + if(field->type == CROS_STD_MSGS_TIME) + new_msg = build_time_field(); + else if(field->type == CROS_STD_MSGS_DURATION) + new_msg = build_duration_field(); + else if(field->type == CROS_STD_MSGS_HEADER) + new_msg = build_header_field(); + else if(field->type == CROS_CUSTOM_TYPE) + { + ret = cRosMessageBuildFromDef(&new_msg, field_def_itr->child_msg_def); // faster alternative to ret = cRosMessageNewBuild(msg_def->root_dir, field_def_itr->type_s, &new_msg) + ret = cRosAddErrCodeIfErr(ret, CROS_CREATE_CUSTOM_MSG_ERR); // If the message could not be created, add more info to the error code pack + } + + if(new_msg != NULL) // New message created: insert it into the array + cRosMessageFieldArrayAtMsgSet(field, msg_ind, new_msg); + else // Error creating the new message + { + if(field->type == CROS_STD_MSGS_TIME || field->type == CROS_STD_MSGS_DURATION || field->type == CROS_STD_MSGS_HEADER) + ret = CROS_MEM_ALLOC_ERR; + } + } + } + else + ret = CROS_MEM_ALLOC_ERR; + } + else + { + if(field->type == CROS_STD_MSGS_TIME) + field->data.as_msg = build_time_field(); + else if(field->type == CROS_STD_MSGS_DURATION) + field->data.as_msg = build_duration_field(); + else if(field->type == CROS_STD_MSGS_HEADER) + field->data.as_msg = build_header_field(); + else if(field->type == CROS_CUSTOM_TYPE) + { + ret = cRosMessageBuildFromDef(&field->data.as_msg, field_def_itr->child_msg_def); + ret = cRosAddErrCodeIfErr(ret, CROS_CREATE_CUSTOM_MSG_ERR); + } + + if(field->data.as_msg == NULL && (field->type == CROS_STD_MSGS_TIME || field->type == CROS_STD_MSGS_DURATION || field->type == CROS_STD_MSGS_HEADER)) + ret = CROS_MEM_ALLOC_ERR; + } + break; + } + } + msg_field_itr++; + field_def_itr = field_def_itr->next; + } + + if(ret == CROS_SUCCESS_ERR_PACK) + *message_ptr = message; + else + cRosMessageFree(message); + + return ret; +} + +void cRosMessageConstDefFree(msgConst* msg_const) +{ + if(msg_const != NULL) + { + free(msg_const->name); + msg_const->name = NULL; + free(msg_const->type_s); + msg_const->type_s = NULL; + free(msg_const->value); + msg_const->value = NULL; + free(msg_const); // Assignment to NULL not needed + } +} + +void cRosMessageFieldDefFree(msgFieldDef* msg_field) +{ + if(msg_field != NULL) + { + cRosMessageDefFree(msg_field->child_msg_def); + free(msg_field->name); + msg_field->name = NULL; + free(msg_field->type_s); + msg_field->type_s = NULL; // Assignment to NULL not needed + free(msg_field); + } +} + +void cRosMessageDefFree(cRosMessageDef *msgDef) +{ + if (msgDef == NULL) + return; + + free(msgDef->name); + msgDef->name = NULL; + free(msgDef->package); + msgDef->package = NULL; + free(msgDef->root_dir); + msgDef->root_dir = NULL; + free(msgDef->plain_text); + msgDef->plain_text = NULL; + + msgConst* it_const = msgDef->first_const; + while(it_const != NULL) + { + msgConst* next_const = it_const->next; + cRosMessageConstDefFree(it_const); + it_const = next_const; + } + msgDef->first_const = NULL; + msgDef->constants = NULL; + + msgFieldDef* it_field = msgDef->first_field; + while(it_field != NULL) + { + msgFieldDef* next_field=it_field->next; + cRosMessageFieldDefFree(it_field); + it_field = next_field; + } + msgDef->first_field = NULL; + msgDef->fields = NULL; + free(msgDef); // All the previous pointer assignments to NULL make no sense if we free the cRosMessageDef struct +} + +void cRosMessageFieldsFree(cRosMessage *message) +{ + int i; + + for(i = 0; i < message->n_fields; i++) + cRosMessageFieldFree(message->fields[i]); + free(message->fields); + message->fields = NULL; + message->n_fields = 0; +} + +void cRosMessageRelease(cRosMessage *message) +{ + cRosMessageFieldsFree(message); + + cRosMessageDefFree(message->msgDef); + message->msgDef = NULL; + + free(message->md5sum); + message->md5sum = NULL; +} + +void cRosMessageFree(cRosMessage *message) +{ + if (message == NULL) + return; + + cRosMessageRelease(message); + free(message); +} + +cRosMessageField * cRosMessageFieldNew(void) +{ + cRosMessageField *ret = (cRosMessageField *)calloc(1, sizeof(cRosMessageField)); + cRosMessageFieldInit(ret); + return ret; +} + +void cRosMessageFieldRelease(cRosMessageField *field) +{ + free(field->name); + field->name = NULL; + + free(field->type_s); + field->type_s = NULL; + + if(field->is_array) + { + if(field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_STRING && + field->type != CROS_STD_MSGS_TIME && field->type != CROS_STD_MSGS_DURATION && + field->type != CROS_STD_MSGS_HEADER) + { + free(field->data.as_array); + field->data.as_array = NULL; + } + else + { + int elem_ind; + for(elem_ind = 0; elem_ind < field->array_size; elem_ind++) + { + if(field->type == CROS_STD_MSGS_STRING) + free(field->data.as_string_array[elem_ind]); + else + cRosMessageFree(field->data.as_msg_array[elem_ind]); + } + free(field->data.as_array); + field->data.as_array = NULL; + } + } + else + { + if(field->type == CROS_STD_MSGS_STRING) + { + free(field->data.as_string); + field->data.as_string=NULL; + } + else if(field->type == CROS_CUSTOM_TYPE || field->type == CROS_STD_MSGS_TIME || + field->type == CROS_STD_MSGS_DURATION || field->type == CROS_STD_MSGS_HEADER) + { + cRosMessageFree(field->data.as_msg); + field->data.as_msg=NULL; + } + } +} + +void cRosMessageFieldFree(cRosMessageField *field) +{ + if (field == NULL) + return; + + cRosMessageFieldRelease(field); + free(field); +} + +cRosMessageField *cRosMessageGetField(cRosMessage *message, const char *field_name) +{ + cRosMessageField* matching_field = NULL; + + int i; + for(i = 0; i < message->n_fields; i++) + { + cRosMessageField* curr_field = message->fields[i]; + if(strcmp(curr_field->name, field_name) == 0) + { + matching_field = curr_field; + break; + } + } + + return matching_field; +} + +int cRosMessageSetFieldValueString(cRosMessageField* field, const char* value) +{ + int ret; + if (field->type != CROS_STD_MSGS_STRING) + return -1; + + size_t str_len = strlen(value); + field->data.as_string = (char *)realloc(field->data.as_string, sizeof(char)*(str_len+1)); + if(field->data.as_string != NULL) + { + strcpy(field->data.as_string, value); + field->size = (int)str_len; + ret=0; // Success + } + else + ret=-1; + return ret; +} + +int arrayFieldValuesPushBack(cRosMessageField *field, const void* data, int element_size, int n_new_elements) +{ + if(field == NULL || !field->is_array || field->is_fixed_array) + return -1; + + if(field->array_capacity < field->array_size + n_new_elements) + { + void *new_location; + size_t new_arr_cap; + + new_arr_cap = (field->array_size + n_new_elements) * 3 / 2; + new_location = realloc(field->data.as_array, new_arr_cap * element_size); // If field->data.as_array is NULL, realloc() behaves as malloc() + if(new_location != NULL) + { + field->data.as_array = new_location; + field->array_capacity = (int)new_arr_cap; + } + else + { + return -1; + } + } + + memcpy((void *)((char *)field->data.as_array + field->array_size*element_size), data, element_size * n_new_elements); + field->array_size += n_new_elements; + return 0; +} + +int arrayFieldValuePushBack(cRosMessageField *field, const void* data, int element_size) +{ + return arrayFieldValuesPushBack(field, data, element_size, 1); +} + +int cRosMessageFieldArrayPushBackInt8(cRosMessageField *field, int8_t val) +{ + if(field->type != CROS_STD_MSGS_INT8) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_INT8)); +} + +int cRosMessageFieldArrayPushBackInt16(cRosMessageField *field, int16_t val) +{ + if(field->type != CROS_STD_MSGS_INT16) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_INT16)); +} + +int cRosMessageFieldArrayPushBackInt32(cRosMessageField *field, int32_t val) +{ + if(field->type != CROS_STD_MSGS_INT32) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_INT32)); +} + +int cRosMessageFieldArrayPushBackInt64(cRosMessageField *field, int64_t val) +{ + if(field->type != CROS_STD_MSGS_INT64) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_INT64)); + +} + +int cRosMessageFieldArrayPushBackUInt8(cRosMessageField *field, uint8_t val) +{ + if(field->type != CROS_STD_MSGS_UINT8) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_UINT8)); +} + +int cRosMessageFieldArrayPushBackUInt16(cRosMessageField *field, uint16_t val) +{ + if(field->type != CROS_STD_MSGS_UINT16) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_UINT16)); +} + +int cRosMessageFieldArrayPushBackUInt32(cRosMessageField *field, uint32_t val) +{ + if(field->type != CROS_STD_MSGS_UINT32) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_UINT32)); +} + +int cRosMessageFieldArrayPushBackUint64(cRosMessageField *field, uint64_t val) +{ + if(field->type != CROS_STD_MSGS_UINT64) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_UINT64)); +} + +int cRosMessageFieldArrayPushBackFloat32(cRosMessageField *field, float val) +{ + if(field->type != CROS_STD_MSGS_FLOAT32) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_FLOAT32)); +} + +int cRosMessageFieldArrayPushBackFloat64(cRosMessageField *field, double val) +{ + if(field->type != CROS_STD_MSGS_FLOAT64) + return -1; + + return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_FLOAT64)); +} + +int cRosMessageFieldArrayPushBackString(cRosMessageField *field, const char* val) +{ + if(field->type != CROS_STD_MSGS_STRING) + return -1; + + if(!field->is_array || field->is_fixed_array) + return -1; + + if(field->array_capacity == field->array_size) + { + char** new_location; + new_location = (char **)realloc(field->data.as_string_array, 2 * field->array_capacity * sizeof(char*)); + if(new_location != NULL) + { + field->data.as_string_array = new_location; + field->array_capacity *= 2; + } + else + { + return -1; + } + } + int ret; + char* element_val; + ret=0; + if(val != NULL) + { + element_val = (char *)calloc(strlen(val) + sizeof(char), 1); + if(element_val != NULL) + strcpy(element_val, val); + else + ret=-1; + } + else + element_val = NULL; + if(ret == 0) + { + field->data.as_string_array[field->array_size] = element_val; + field->array_size ++; + } + return ret; +} + +// Add a new blank element at the end of the array in field n_field of message msg +int cRosMessageFieldArrayPushBackZero(cRosMessage* msg, int n_field) +{ + cRosMessageField *field; + CrosMessageType elem_type; + size_t elem_size; + + if(n_field >= msg->n_fields || n_field < 0) + return -1; + + field = msg->fields[n_field]; + + if(!field->is_array || field->is_fixed_array) + return -1; + + elem_type = field->type; + elem_size = getMessageTypeSizeOf(elem_type); + if(field->array_capacity == field->array_size) + { + void *new_location; + new_location = realloc(field->data.as_array, 2 * field->array_capacity * elem_size); + if(new_location != NULL) + { + field->data.as_array = new_location; + field->array_capacity *= 2; + } + else + { + return -1; + } + } + + switch(elem_type) + { + case CROS_STD_MSGS_INT8: + case CROS_STD_MSGS_UINT8: + case CROS_STD_MSGS_INT16: + case CROS_STD_MSGS_UINT16: + case CROS_STD_MSGS_INT32: + case CROS_STD_MSGS_UINT32: + case CROS_STD_MSGS_INT64: + case CROS_STD_MSGS_UINT64: + case CROS_STD_MSGS_FLOAT32: + case CROS_STD_MSGS_FLOAT64: + case CROS_STD_MSGS_BOOL: + case CROS_STD_MSGS_CHAR: + case CROS_STD_MSGS_BYTE: + case CROS_STD_MSGS_STRING: + { + char *elem_array = field->data.as_array; + memset(elem_array + elem_size*field->array_size, 0, elem_size); + break; + } + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_HEADER: + case CROS_CUSTOM_TYPE: + { + if(msg->msgDef != NULL) + { + msgFieldDef* field_def_itr; + int field_ind; + cRosMessage **msg_array; + cRosErrCodePack ret_err; + + // Look for the definition corresponding to the specified field + field_def_itr = msg->msgDef->first_field; + for(field_ind=0;field_ind < n_field && field_def_itr != NULL;field_ind++) + field_def_itr = field_def_itr->next; + + if(field_def_itr == NULL) + return -1; // msg definition not found + + msg_array = field->data.as_msg_array; + ret_err = cRosMessageBuildFromDef(&msg_array[field->array_size], field_def_itr->child_msg_def); // instead of cRosMessageNewBuild(msg->msgDef->root_dir, field->type_s, &msg_array[field->array_size]); + if(ret_err != CROS_SUCCESS_ERR_PACK) + return -1; // Error creating the message + } + else + return -1; // msgDef not available in current message, we don't know the type of the message to build + } + default: + break; + } + + field->array_size ++; + return 0; +} + +int cRosMessageFieldArrayPushBackMsg(cRosMessageField *field, cRosMessage* msg) +{ + if(field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_TIME && + field->type != CROS_STD_MSGS_DURATION && field->type != CROS_STD_MSGS_HEADER) + return -1; + + if(!field->is_array || field->is_fixed_array) + return -1; + + if(field->array_capacity == field->array_size) + { + cRosMessage **new_location; + new_location = (cRosMessage **)realloc(field->data.as_msg_array, 2 * field->array_capacity * sizeof(cRosMessage*)); + if(new_location != NULL) + { + field->data.as_msg_array = new_location; + field->array_capacity *= 2; + } + else + { + return -1; + } + } + + field->data.as_msg_array[field->array_size] = msg; + field->array_size ++; + return 0; +} + +cRosMessage *cRosMessageFieldArrayRemoveLastMsg(cRosMessageField *field) +{ + if(field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_TIME && + field->type != CROS_STD_MSGS_DURATION && field->type != CROS_STD_MSGS_HEADER) + return NULL; + + if(!field->is_array || field->is_fixed_array) + return NULL; + + cRosMessage *msg; + + if(field->array_size > 0) + { + msg = field->data.as_msg_array[field->array_size-1]; + field->array_size--; + } + else + msg=NULL; // Array empty + + return msg; +} + +int8_t *cRosMessageFieldArrayAtInt8(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_INT8) + return NULL; + + return (int8_t *)arrayFieldValueAt(field, position, sizeof(int8_t)); +} + +int16_t *cRosMessageFieldArrayAtInt16(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_INT16) + return NULL; + + return (int16_t *)arrayFieldValueAt(field, position, sizeof(int16_t)); +} + +int32_t *cRosMessageFieldArrayAtInt32(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_INT32) + return NULL; + + return (int32_t *)arrayFieldValueAt(field, position, sizeof(int32_t)); +} + +int64_t *cRosMessageFieldArrayAtInt64(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_INT64) + return NULL; + + return (int64_t *)arrayFieldValueAt(field, position, sizeof(int64_t)); +} + +uint8_t *cRosMessageFieldArrayAtUInt8(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_UINT8) + return NULL; + + return (uint8_t *)arrayFieldValueAt(field, position, sizeof(uint8_t)); +} + +uint16_t *cRosMessageFieldArrayAtUInt16(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_UINT16) + return NULL; + + return (uint16_t *)arrayFieldValueAt(field, position, sizeof(uint16_t)); +} + +uint32_t *cRosMessageFieldArrayAtUInt32(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_UINT32) + return NULL; + + return (uint32_t *)arrayFieldValueAt(field, position, sizeof(uint32_t)); +} + +uint64_t *cRosMessageFieldArrayAtUInt64(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_UINT64) + return NULL; + + return (uint64_t *)arrayFieldValueAt(field, position, sizeof(uint64_t)); +} + +float *cRosMessageFieldArrayAtFloat32(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_FLOAT32) + return NULL; + + return (float *)arrayFieldValueAt(field, position, sizeof(float)); +} + +double *cRosMessageFieldArrayAtFloat64(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_FLOAT64) + return NULL; + + return (double *)arrayFieldValueAt(field, position, sizeof(double)); +} + +cRosMessage *cRosMessageFieldArrayAtMsgGet(cRosMessageField *field, int position) +{ + if(field == NULL || (field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_TIME && + field->type != CROS_STD_MSGS_DURATION && field->type != CROS_STD_MSGS_HEADER) || !field->is_array) + return NULL; + + if(position >= field->array_size) + return NULL; + + cRosMessage* msg = field->data.as_msg_array[position]; + return msg; +} + +int cRosMessageFieldArrayAtMsgSet(cRosMessageField *field, int position, cRosMessage* msg) +{ + int ret; + if((field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_TIME && + field->type != CROS_STD_MSGS_DURATION && field->type != CROS_STD_MSGS_HEADER) || !field->is_array) + return -1; + + if(position > field->array_size) + return -1; + + if(position == field->array_size) + ret = cRosMessageFieldArrayPushBackMsg(field, msg); + else + { + cRosMessageFree(field->data.as_msg_array[position]); + field->data.as_msg_array[position] = msg; + ret = 0; + } + + return ret; +} + +char *cRosMessageFieldArrayAtStringGet(cRosMessageField *field, int position) +{ + if(field->type != CROS_STD_MSGS_STRING || !field->is_array) + return NULL; + + char* str = (char*) field->data.as_string_array[position]; + return str; +} + +int cRosMessageFieldArrayAtStringSet(cRosMessageField *field, int position, const char* val) +{ + int ret; + if(field->type != CROS_STD_MSGS_STRING || !field->is_array) + return -1; + + char *current = field->data.as_string_array[position]; + ret = 0; // Default return value: success + if(val != NULL) + { + current = (char *)realloc(current, strlen(val) + sizeof(char)); // Realloc buffer for the string + if(current != NULL) + strcpy(current, val); + else + ret=-1; + } + else + { + if(current != NULL) + { + free(current); + current = NULL; + } + } + + field->data.as_string_array[position] = current; + + return ret; +} + +int cRosMessageFieldArrayClear(cRosMessageField *field) +{ + int n_elem; + if(!field->is_array || field->is_fixed_array) + return -1; + + for(n_elem=0;n_elemarray_size;n_elem++) + { + switch(field->type) + { + case CROS_STD_MSGS_STRING: + { + free(field->data.as_string_array[n_elem]); + break; + } + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_HEADER: + case CROS_CUSTOM_TYPE: + { + cRosMessageFree(field->data.as_msg_array[n_elem]); + break; + } + default: + break; + } + } + + field->array_size = 0; + + return 0; +} + +void *arrayFieldValueAt(cRosMessageField *field, int position, size_t size) +{ + if(!field->is_array) + return NULL; + + if (position < 0 || position > field->array_size) + return NULL; + + return((void *)((char *)field->data.as_array + (position * size))); // sizeof(char) is 1 (byte), so we cast to char * before calculating the array field position +} + +size_t cRosMessageFieldSize(cRosMessageField* field) +{ + size_t single_size = 0; + + if (isBuiltinMessageType(field->type)) + { + single_size = field->size; + } + else + { + single_size = cRosMessageSize(field->data.as_msg); + } + + size_t ret = 0; + if(field->is_array) + { + ret = single_size * field->array_size; + } + else + { + ret = single_size; + } + return ret; +} + +size_t cRosMessageSize(cRosMessage* message) +{ + size_t ret = 0; + cRosMessageField** fields_it = message->fields; + int i; + for( i = 0; i < message->n_fields; i++) + { + cRosMessageField* field = *fields_it; + ret += cRosMessageFieldSize(field); + fields_it++; + } + + return ret;// + sizeof(uint32_t); +} + +cRosErrCodePack cRosMessageSerialize(cRosMessage *message, DynBuffer* buffer) +{ + cRosErrCodePack ret_err; + int field_ind; + + ret_err = CROS_SUCCESS_ERR_PACK; // default error value: success + for (field_ind = 0; field_ind < message->n_fields && ret_err == CROS_SUCCESS_ERR_PACK; field_ind++) + { + cRosMessageField *field = message->fields[field_ind]; + + if(field->is_array && !field->is_fixed_array) + ret_err = (dynBufferPushBackInt32(buffer, field->array_size) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + + switch (field->type) + { + case CROS_STD_MSGS_INT8: + case CROS_STD_MSGS_UINT8: + case CROS_STD_MSGS_INT16: + case CROS_STD_MSGS_UINT16: + case CROS_STD_MSGS_INT32: + case CROS_STD_MSGS_UINT32: + case CROS_STD_MSGS_INT64: + case CROS_STD_MSGS_UINT64: + case CROS_STD_MSGS_FLOAT32: + case CROS_STD_MSGS_FLOAT64: + case CROS_STD_MSGS_BOOL: + case CROS_STD_MSGS_CHAR: + case CROS_STD_MSGS_BYTE: + { + size_t size = getMessageTypeSizeOf(field->type); + if (field->is_array) + ret_err = (dynBufferPushBackBuf(buffer, field->data.as_array, size * field->array_size) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + else + ret_err = (dynBufferPushBackBuf(buffer, field->data.opaque, size) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + break; + } + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_HEADER: + case CROS_CUSTOM_TYPE: + { + if(field->is_array) + { + int elem_ind; + for(elem_ind = 0; elem_ind < field->array_size && ret_err == CROS_SUCCESS_ERR_PACK; elem_ind++) + { + cRosMessage *arr_elem_msg; + arr_elem_msg = cRosMessageFieldArrayAtMsgGet(field, elem_ind); + if(arr_elem_msg != NULL) + ret_err = cRosMessageSerialize(arr_elem_msg, buffer); + } + } + else + { + if(field->data.as_msg != NULL) + ret_err = cRosMessageSerialize(field->data.as_msg, buffer); + } + break; + } + case CROS_STD_MSGS_STRING: + { + if(field->is_array) + { + int elem_ind; + for(elem_ind = 0; elem_ind < field->array_size && ret_err == CROS_SUCCESS_ERR_PACK; elem_ind++) + { + const char* arr_elem_str; + size_t arr_elem_str_len; + arr_elem_str = cRosMessageFieldArrayAtStringGet(field, elem_ind); + if(arr_elem_str != NULL) + arr_elem_str_len = strlen(arr_elem_str); + else + arr_elem_str_len = 0; + dynBufferPushBackInt32(buffer, arr_elem_str_len); + ret_err = (dynBufferPushBackBuf(buffer, (unsigned char *)arr_elem_str, arr_elem_str_len) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + } + } + else + { + if(field->data.as_string != NULL) + { + size_t str_len = strlen(field->data.as_string); + dynBufferPushBackInt32(buffer, str_len); + ret_err = (dynBufferPushBackBuf(buffer,(unsigned char*)field->data.as_string, str_len) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + } + else + ret_err = (dynBufferPushBackInt32(buffer, 0) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + } + break; + } + default: + { + ret_err = CROS_BAD_PARAM_ERR; + break; + } + } + } + return ret_err; +} + +// In this function we assume that the message is already build according to its definition. +// Only when receiving a variable-length array, new elements if the message field may need to be created +cRosErrCodePack cRosMessageDeserialize(cRosMessage *message, DynBuffer* buffer) +{ + int field_ind; + cRosErrCodePack ret_err; + + ret_err = CROS_SUCCESS_ERR_PACK; // default error value: no error + + msgFieldDef* field_def_itr = (message->msgDef != NULL)? message->msgDef->first_field : NULL; // Keep track of the message definition (if available) corresponding to the current message field in case we need to build a msg of type custom + + for (field_ind = 0; field_ind < message->n_fields && ret_err == CROS_SUCCESS_ERR_PACK; field_ind++) + { + cRosMessageField *field = message->fields[field_ind]; + + switch (field->type) + { + case CROS_STD_MSGS_INT8: + case CROS_STD_MSGS_UINT8: + case CROS_STD_MSGS_INT16: + case CROS_STD_MSGS_UINT16: + case CROS_STD_MSGS_INT32: + case CROS_STD_MSGS_UINT32: + case CROS_STD_MSGS_INT64: + case CROS_STD_MSGS_UINT64: + case CROS_STD_MSGS_FLOAT32: + case CROS_STD_MSGS_FLOAT64: + case CROS_STD_MSGS_BOOL: + case CROS_STD_MSGS_CHAR: + case CROS_STD_MSGS_BYTE: + { + size_t elem_size = getMessageTypeSizeOf(field->type); + if (field->is_array) + { + if (field->is_fixed_array) + { + ret_err = (dynBufferGetCurrentContent( field->data.as_array, buffer, elem_size * field->array_size ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: memcpy(field->data.as_array, dynBufferGetCurrentData(buffer), size * field->array_size); + dynBufferMovePoseIndicator(buffer, elem_size * field->array_size); + } + else + { + uint32_t array_n_elems; + cRosMessageFieldArrayClear(field); + ret_err = (dynBufferGetCurrentContent( (unsigned char *)&array_n_elems, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: array_n_elems = *((uint32_t*)dynBufferGetCurrentData(buffer)); + dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + if(dynBufferGetRemainingDataSize(buffer) >= array_n_elems*elem_size) + { + ret_err = (arrayFieldValuesPushBack(field, (const void *)dynBufferGetCurrentData(buffer), elem_size, array_n_elems) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + dynBufferMovePoseIndicator(buffer, array_n_elems*elem_size); + } + else + ret_err = CROS_DEPACK_INSUFF_DAT_ERR; // Not enough data available in the packet buffer + } + } + } + else + { + ret_err = (dynBufferGetCurrentContent( field->data.opaque, buffer, elem_size ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: memcpy(field->data.opaque, dynBufferGetCurrentData(buffer), elem_size); + dynBufferMovePoseIndicator(buffer, elem_size); + } + + break; + } + case CROS_STD_MSGS_STRING: + { + if (field->is_array) + { + uint32_t curr_array_size; + if(field->is_fixed_array) // If it is a fixed array, use the fixed array size + curr_array_size = field->array_size; + else // Otherwise, obtain the number of elements of the received array + { + cRosMessageFieldArrayClear(field); // Remove previous strings from the array to start added the new ones one by one + ret_err = (dynBufferGetCurrentContent( (unsigned char *)&curr_array_size, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: curr_array_size = *((uint32_t*)dynBufferGetCurrentData(buffer)); + dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); + } + + uint32_t elem_ind; + for(elem_ind = 0; elem_ind < curr_array_size && ret_err == CROS_SUCCESS_ERR_PACK; elem_ind++) + { + uint32_t element_size; + ret_err = (dynBufferGetCurrentContent( (unsigned char *)&element_size, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: element_size = *((uint32_t*)dynBufferGetCurrentData(buffer)); + dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + char* tmp_string = (char *)calloc(element_size + 1, sizeof(char)); + if(tmp_string != NULL) + { + ret_err = (dynBufferGetCurrentContent( (unsigned char *)tmp_string, buffer, element_size ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: memcpy(tmp_string, dynBufferGetCurrentData(buffer), element_size); + if(field->is_fixed_array) + ret_err = (cRosMessageFieldArrayAtStringSet(field, elem_ind, tmp_string) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + else + ret_err = (cRosMessageFieldArrayPushBackString(field, tmp_string) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + dynBufferMovePoseIndicator(buffer, element_size); + free(tmp_string); + } + else + ret_err=CROS_MEM_ALLOC_ERR; + } + } + } + else + { + uint32_t curr_data_size; + ret_err = (dynBufferGetCurrentContent( (unsigned char *)&curr_data_size, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: curr_data_size = *((uint32_t*)dynBufferGetCurrentData(buffer)); + dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); + field->data.as_string = (char *)realloc(field->data.as_string, (curr_data_size + 1) * sizeof(char)); // If field->data.as_string was NULL previously, realloc behaves as malloc + if(field->data.as_string != NULL) + { + dynBufferGetCurrentContent( (unsigned char *)field->data.as_string, buffer, curr_data_size ); // equiv. to: memcpy(field->data.as_string, dynBufferGetCurrentData(buffer), curr_data_size); + field->data.as_string[curr_data_size] = '\0'; + dynBufferMovePoseIndicator(buffer, curr_data_size); + } + else + ret_err=CROS_MEM_ALLOC_ERR; + } + break; + } + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_HEADER: + case CROS_CUSTOM_TYPE: + { + if (field->is_array) + { + uint32_t received_arr_siz; + uint32_t msg_ind; + + if(field->is_fixed_array) // If it is a fixed array, we use the fixed array size + received_arr_siz = field->array_size; + else // Otherwise, obtain the number of array elements from the received packet + { + ret_err = (dynBufferGetCurrentContent( (unsigned char *)&received_arr_siz, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); + + // Adapt the array size of the message field to the received array length + for(msg_ind = field->array_size;msg_ind < received_arr_siz && ret_err == CROS_SUCCESS_ERR_PACK;msg_ind++) // received more elements than the available messages: create more + { + cRosMessage *new_msg; + new_msg = NULL; + if(field->type == CROS_STD_MSGS_TIME) + new_msg = build_time_field(); + else if(field->type == CROS_STD_MSGS_DURATION) + new_msg = build_duration_field(); + else if(field->type == CROS_STD_MSGS_HEADER) + new_msg = build_header_field(); + else if(field->type == CROS_CUSTOM_TYPE) + { + if(field_def_itr != NULL) + ret_err = cRosMessageBuildFromDef(&new_msg, field_def_itr->child_msg_def); // instead of cRosMessageNewBuild(message->msgDef->root_dir, field->type_s, &new_msg); + else + ret_err =CROS_DEPACK_NO_MSG_DEF_ERR; + } + + if(new_msg != NULL) + cRosMessageFieldArrayPushBackMsg(field, new_msg); // new_msg is now used in the array so it cannot be freed independently + else + { + if(ret_err == CROS_SUCCESS_ERR_PACK) + ret_err=CROS_MEM_ALLOC_ERR; + } + } + + while(field->array_size > (int)received_arr_siz) // received less elements than the available messages: delete + cRosMessageFree(cRosMessageFieldArrayRemoveLastMsg(field)); + } + + for(msg_ind = 0;(int)msg_ind < field->array_size && ret_err == CROS_SUCCESS_ERR_PACK;msg_ind++) + { + cRosMessage *curr_msg; + curr_msg = cRosMessageFieldArrayAtMsgGet(field, msg_ind); + ret_err = cRosMessageDeserialize(curr_msg, buffer); + } + } + else + ret_err = cRosMessageDeserialize(field->data.as_msg, buffer); + break; + } + default: + { + ret_err=CROS_BAD_PARAM_ERR; + break; + } + } + field_def_itr = (field_def_itr != NULL)? field_def_itr->next : NULL; + } + return ret_err; +} + +const char *getMessageTypeDeclarationConst(msgConst *msgConst) +{ + if (msgConst->type_s == NULL) + return getMessageTypeDeclaration(msgConst->type); + else + return msgConst->type_s; +} + +const char *getMessageTypeDeclarationField(msgFieldDef *fieldDef) +{ + if (fieldDef->type_s == NULL) + return getMessageTypeDeclaration(fieldDef->type); + else + return fieldDef->type_s; +} + +int isBuiltinMessageType(CrosMessageType type) +{ + switch(type) + { + case CROS_STD_MSGS_INT8: + case CROS_STD_MSGS_UINT8: + case CROS_STD_MSGS_INT16: + case CROS_STD_MSGS_UINT16: + case CROS_STD_MSGS_INT32: + case CROS_STD_MSGS_UINT32: + case CROS_STD_MSGS_INT64: + case CROS_STD_MSGS_UINT64: + case CROS_STD_MSGS_FLOAT32: + case CROS_STD_MSGS_FLOAT64: + case CROS_STD_MSGS_BOOL: + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_CHAR: + case CROS_STD_MSGS_BYTE: + case CROS_STD_MSGS_STRING: + return 1; + default: + return 0; + } +} + +size_t getMessageTypeSizeOf(CrosMessageType type) +{ + switch(type) + { + case CROS_STD_MSGS_INT8: + case CROS_STD_MSGS_UINT8: + return sizeof(int8_t); + case CROS_STD_MSGS_INT16: + case CROS_STD_MSGS_UINT16: + return sizeof(int16_t); + case CROS_STD_MSGS_INT32: + case CROS_STD_MSGS_UINT32: + return sizeof(int32_t); + case CROS_STD_MSGS_INT64: + case CROS_STD_MSGS_UINT64: + return sizeof(int64_t); + case CROS_STD_MSGS_FLOAT32: + return sizeof(float); + case CROS_STD_MSGS_FLOAT64: + return sizeof(double); + case CROS_STD_MSGS_BOOL: + return sizeof(int8_t); + case CROS_STD_MSGS_STRING: + return sizeof(char *); + case CROS_STD_MSGS_TIME: + case CROS_STD_MSGS_DURATION: + case CROS_STD_MSGS_HEADER: + case CROS_CUSTOM_TYPE: + return sizeof(cRosMessage *); + // deprecated + case CROS_STD_MSGS_CHAR: + case CROS_STD_MSGS_BYTE: + return sizeof(char); + default: + PRINT_ERROR ( "getMessageTypeSizeOf() : Invalid CrosMessageType specified\n" ); + return 0; + } +} + +CrosMessageType getMessageType(const char *type_s) +{ + if (strcmp(type_s, "int8") == 0 || strcmp(type_s, "std_msgs/Int8") == 0) + return CROS_STD_MSGS_INT8; + else if (strcmp(type_s, "uint8") == 0 || strcmp(type_s, "std_msgs/UInt8") == 0) + return CROS_STD_MSGS_UINT8; + else if (strcmp(type_s, "int16") == 0 || strcmp(type_s, "std_msgs/Int16") == 0) + return CROS_STD_MSGS_INT16; + else if (strcmp(type_s, "uint16") == 0 || strcmp(type_s, "std_msgs/UInt16") == 0) + return CROS_STD_MSGS_UINT16; + else if (strcmp(type_s, "int32") == 0 || strcmp(type_s, "std_msgs/Int32") == 0) + return CROS_STD_MSGS_INT32; + else if (strcmp(type_s, "uint32") == 0 || strcmp(type_s, "std_msgs/UInt32") == 0) + return CROS_STD_MSGS_UINT32; + else if (strcmp(type_s, "int64") == 0 || strcmp(type_s, "std_msgs/Int64") == 0) + return CROS_STD_MSGS_INT64; + else if (strcmp(type_s, "uint64") == 0 || strcmp(type_s, "std_msgs/UInt64") == 0) + return CROS_STD_MSGS_UINT64; + else if (strcmp(type_s, "float32") == 0 || strcmp(type_s, "std_msgs/Float32") == 0) + return CROS_STD_MSGS_FLOAT32; + else if (strcmp(type_s, "float64") == 0 || strcmp(type_s, "std_msgs/Float64") == 0) + return CROS_STD_MSGS_FLOAT64; + else if (strcmp(type_s, "string") == 0 || strcmp(type_s, "std_msgs/String") == 0) + return CROS_STD_MSGS_STRING; + else if (strcmp(type_s, "bool") == 0 || strcmp(type_s, "std_msgs/Bool") == 0) + return CROS_STD_MSGS_BOOL; + else if (strcmp(type_s, "time") == 0 || strcmp(type_s, "std_msgs/Time") == 0) + return CROS_STD_MSGS_TIME; + else if (strcmp(type_s, "duration") == 0 || strcmp(type_s, "std_msgs/Duration") == 0) + return CROS_STD_MSGS_DURATION; + else if (strcmp(type_s, "char") == 0 || strcmp(type_s, "std_msgs/Char") == 0) // deprecated + return CROS_STD_MSGS_CHAR; + else if (strcmp(type_s, "byte") == 0 || strcmp(type_s, "std_msgs/Byte") == 0) // deprecated + return CROS_STD_MSGS_BYTE; + else if (strcmp(type_s, "std_msgs/Header") == 0 || (strcmp(type_s, "Header") == 0) + || (strcmp(type_s, "roslib/Header") == 0)) + return CROS_STD_MSGS_HEADER; + else + return CROS_CUSTOM_TYPE; +} + +const char *getMessageTypeString(CrosMessageType type) +{ + switch(type) + { + case CROS_STD_MSGS_INT8: + return "std_msgs/UInt8"; + case CROS_STD_MSGS_UINT8: + return "std_msgs/UInt8"; + case CROS_STD_MSGS_INT16: + return "std_msgs/Int16"; + case CROS_STD_MSGS_UINT16: + return "std_msgs/UInt16"; + case CROS_STD_MSGS_INT32: + return "std_msgs/Int32"; + case CROS_STD_MSGS_UINT32: + return "std_msgs/UInt32"; + case CROS_STD_MSGS_INT64: + return "std_msgs/Int64"; + case CROS_STD_MSGS_UINT64: + return "std_msgs/UInt64"; + case CROS_STD_MSGS_FLOAT32: + return "std_msgs/Float32"; + case CROS_STD_MSGS_FLOAT64: + return "std_msgs/Float64"; + case CROS_STD_MSGS_STRING: + return "std_msgs/String"; + case CROS_STD_MSGS_BOOL: + return "std_msgs/Bool"; + case CROS_STD_MSGS_TIME: + return "std_msgs/Time"; + case CROS_STD_MSGS_DURATION: + return "std_msgs/Duration"; + case CROS_STD_MSGS_CHAR: // deprecated + return "std_msgs/Char"; + case CROS_STD_MSGS_BYTE: // deprecated + return "std_msgs/Byte"; + case CROS_STD_MSGS_HEADER: + return "std_msgs/Header"; + default: + PRINT_ERROR ( "getMessageTypeString() : Invalid CrosMessageType specified\n" ); + return NULL; + } +} + +const char *getMessageTypeDeclaration(CrosMessageType type) +{ + switch(type) + { + case CROS_STD_MSGS_INT8: + return "int8"; + case CROS_STD_MSGS_UINT8: + return "uint8"; + case CROS_STD_MSGS_INT16: + return "int16"; + case CROS_STD_MSGS_UINT16: + return "uint16"; + case CROS_STD_MSGS_INT32: + return "int32"; + case CROS_STD_MSGS_UINT32: + return "uint32"; + case CROS_STD_MSGS_INT64: + return "int64"; + case CROS_STD_MSGS_UINT64: + return "uint64"; + case CROS_STD_MSGS_FLOAT32: + return "float32"; + case CROS_STD_MSGS_FLOAT64: + return "float64"; + case CROS_STD_MSGS_STRING: + return "string"; + case CROS_STD_MSGS_BOOL: + return "bool"; + case CROS_STD_MSGS_TIME: + return "time"; + case CROS_STD_MSGS_DURATION: + return "duration"; + case CROS_STD_MSGS_CHAR: // deprecated + return "char"; + case CROS_STD_MSGS_BYTE: // deprecated + return "byte"; + case CROS_STD_MSGS_HEADER: + return "header"; + case CROS_CUSTOM_TYPE: + return "custom"; + default: + PRINT_ERROR ( "getMessageTypeString() : Invalid CrosMessageType specified\n" ); + return NULL; + } +} diff --git a/src/hal/components/cros/src/cros_message_queue.c b/src/hal/components/cros/src/cros_message_queue.c new file mode 100644 index 00000000..4a3d45e5 --- /dev/null +++ b/src/hal/components/cros/src/cros_message_queue.c @@ -0,0 +1,138 @@ +#include +#include + +#include "cros_message_queue.h" + +void cRosMessageQueueInit(cRosMessageQueue *q) +{ + unsigned int msg_ind; + // Initialize all messages in the queue so that the inserted messages only need to be copied over these ones + for(msg_ind=0;msg_indmsgs[msg_ind]); + q->length = 0; + q->first_msg_ind = 0; +} + +void cRosMessageQueueClear(cRosMessageQueue *q) +{ + // Empty the queue + while(q->length > 0) + { + // Delete fields from message to remove + cRosMessageFieldsFree(&q->msgs[q->first_msg_ind]); + // The queue is internally implemented as a circular buffer + q->first_msg_ind = (q->first_msg_ind + 1) % MAX_QUEUE_LEN; + q->length--; + } + q->first_msg_ind = 0; +} + +unsigned int cRosMessageQueueVacancies(cRosMessageQueue *q) +{ + return MAX_QUEUE_LEN - q->length; +} + +unsigned int cRosMessageQueueUsage(cRosMessageQueue *q) +{ + return q->length; +} + +void cRosMessageQueueRelease(cRosMessageQueue *q) +{ + unsigned int msg_ind; + cRosMessageQueueClear(q); + // Release the memory of all container messages in the queue + for(msg_ind=0;msg_indmsgs[msg_ind]); +} + +int cRosMessageQueueAdd(cRosMessageQueue *q, cRosMessage *m) +{ + int ret; + if(q->length < MAX_QUEUE_LEN) + { + unsigned int next_msg_pos; + // The queue is internally implemented as a circular buffer + next_msg_pos = (q->first_msg_ind + q->length) % MAX_QUEUE_LEN; + ret = cRosMessageFieldsCopy(&q->msgs[next_msg_pos], m); + q->length++; + } + else + ret=-2; + + return ret; +} + +int cRosMessageQueueGet(cRosMessageQueue *q, cRosMessage *m) +{ + int ret; + if(q->length > 0) + { + cRosMessage *msg_to_peek; + msg_to_peek = &q->msgs[q->first_msg_ind]; + ret = cRosMessageFieldsCopy(m, msg_to_peek); + } + else + ret=-2; + + return ret; +} + +int cRosMessageQueueRemove(cRosMessageQueue *q) +{ + int ret; + + if(q->length > 0) + { + cRosMessage *msg_to_remove; + msg_to_remove = &q->msgs[q->first_msg_ind]; + // Delete fields from removed message + cRosMessageFieldsFree(msg_to_remove); + // The queue is internally implemented as a circular buffer + q->first_msg_ind = (q->first_msg_ind + 1) % MAX_QUEUE_LEN; + q->length--; + ret=0; + } + else + ret=-2; + + return ret; +} + +int cRosMessageQueueExtract(cRosMessageQueue *q, cRosMessage *m) +{ + int ret; + + ret = cRosMessageQueueGet(q, m); + if(ret == 0) + ret = cRosMessageQueueRemove(q); + + return ret; +} + +cRosMessage *cRosMessageQueuePeekFirst(cRosMessageQueue *q) +{ + cRosMessage *first_msg; + if(q->length > 0) + first_msg = &q->msgs[q->first_msg_ind]; + else + first_msg = NULL; + + return first_msg; +} + +cRosMessage *cRosMessageQueuePeekLast(cRosMessageQueue *q) +{ + cRosMessage *last_msg; + if(q->length > 0) + { + unsigned int last_msg_pos; + // The queue is internally implemented as a circular buffer + last_msg_pos = (q->first_msg_ind + q->length - 1) % MAX_QUEUE_LEN; + last_msg = &q->msgs[last_msg_pos]; + } + else + last_msg = NULL; + + return last_msg; +} diff --git a/src/hal/components/cros/src/cros_node.c b/src/hal/components/cros/src/cros_node.c new file mode 100644 index 00000000..e04ec739 --- /dev/null +++ b/src/hal/components/cros/src/cros_node.c @@ -0,0 +1,4005 @@ +#include +#include +#include +#include + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN // This define speeds up the build process by excluding some parts of the Windows header +# include +#else +# include +#endif + +#include "cros_node.h" +#include "cros_api_internal.h" +#include "cros_api.h" +#include "cros_message.h" +#include "cros_clock.h" +#include "cros_defs.h" +#include "cros_node_api.h" +#include "cros_tcpros.h" +#include "tcpip_socket.h" +#include "cros_log.h" + +static void initPublisherNode(PublisherNode *node); +static void initSubscriberNode(SubscriberNode *node); +static void initServiceProviderNode(ServiceProviderNode *node); +static void initServiceCallerNode(ServiceCallerNode *node); +static void initParameterSubscrition(ParameterSubscription *subscription); +static int enqueueSubscriberAdvertise(CrosNode *node, int subidx); +static int enqueuePublisherAdvertise(CrosNode *node, int pubidx); +static int enqueueServiceAdvertise(CrosNode *node, int servivceidx); +static int enqueueServiceLookup(CrosNode *node, int serviceidx); +static int enqueueParameterSubscription(CrosNode *node, int parameteridx); +static int enqueueParameterUnsubscription(CrosNode *node, int parameteridx); +static void getIdleXmplrpcClients(CrosNode *node, int array[], size_t *count); +static int enqueueSlaveApiCallInternal(CrosNode *node, RosApiCall *call); +static int enqueueMasterApiCallInternal(CrosNode *node, RosApiCall *call); +static void printNodeProcState( CrosNode *n ); + +FILE *Msg_output = NULL; //! The pointer to file stream used to print local messages (except debug messages). If it is NULL (default value), stdout is used. + +static int openXmlrpcClientSocket( CrosNode *n, int i ) +{ + int ret; + if( !tcpIpSocketOpen( &(n->xmlrpc_client_proc[i].socket) ) || + !tcpIpSocketSetReuse( &(n->xmlrpc_client_proc[i].socket) ) || + !tcpIpSocketSetNonBlocking( &(n->xmlrpc_client_proc[i].socket) ) ) + { + PRINT_ERROR("openXmlrpcClientSocket() at index %d failed", i); + ret=-1; + } + else + { + ret=0; // success + } + return(ret); +} + +static int openTcprosClientSocket( CrosNode *n, int i ) +{ + int ret; + if( !tcpIpSocketOpen( &(n->tcpros_client_proc[i].socket) ) || + !tcpIpSocketSetReuse( &(n->tcpros_client_proc[i].socket) ) || + !tcpIpSocketSetNonBlocking( &(n->tcpros_client_proc[i].socket) ) ) + { + PRINT_ERROR("openTcprosClientSocket() at index %d failed", i); + ret=-1; + } + else + { + ret=0; // success + } + return(ret); +} + +static int openRpcrosClientSocket( CrosNode *n, int i ) +{ + int ret; + if( !tcpIpSocketOpen( &(n->rpcros_client_proc[i].socket) ) || + !tcpIpSocketSetReuse( &(n->rpcros_client_proc[i].socket) ) || + !tcpIpSocketSetNonBlocking( &(n->rpcros_client_proc[i].socket) ) ) + { + PRINT_ERROR("openRpcrosClientSocket() at index %d failed", i); + ret=-1; + } + else + { + ret=0; // success + } + return(ret); +} + +static int openXmlrpcListnerSocket( CrosNode *n ) +{ + int ret; + if( !tcpIpSocketOpen( &(n->xmlrpc_listner_proc.socket) ) || + !tcpIpSocketSetReuse( &(n->xmlrpc_listner_proc.socket) ) || + !tcpIpSocketSetNonBlocking( &(n->xmlrpc_listner_proc.socket) ) || + !tcpIpSocketBindListen( &(n->xmlrpc_listner_proc.socket), n->host, 0, CN_MAX_XMLRPC_SERVER_CONNECTIONS ) ) + { + PRINT_ERROR("openXmlrpcListnerSocket() failed"); + ret=-1; + } + else + { + n->xmlrpc_port = tcpIpSocketGetPort( &(n->xmlrpc_listner_proc.socket) ); + PRINT_VDEBUG ( "openXmlrpcListnerSocket () : Accepting xmlrpc connections at port %d\n", n->xmlrpc_port ); + ret=0; // success + } + return(ret); +} + +static int openRpcrosListnerSocket( CrosNode *n ) +{ + int ret; + if( !tcpIpSocketOpen( &(n->rpcros_listner_proc.socket) ) || + !tcpIpSocketSetReuse( &(n->rpcros_listner_proc.socket) ) || + !tcpIpSocketSetNonBlocking( &(n->rpcros_listner_proc.socket) ) || + !tcpIpSocketBindListen( &(n->rpcros_listner_proc.socket), n->host, 0, CN_MAX_RPCROS_SERVER_CONNECTIONS ) ) + { + PRINT_ERROR("openRpcrosListnerSocket() failed"); + ret=-1; + } + else + { + n->rpcros_port = tcpIpSocketGetPort( &(n->rpcros_listner_proc.socket) ); + PRINT_VDEBUG ( "openRpcrosListnerSocket() : Accepting rcpros connections at port %d\n", n->rpcros_port ); + ret=0; // success + } + return(ret); +} + +static int openTcprosListnerSocket( CrosNode *n ) +{ + int ret; + if( !tcpIpSocketOpen( &(n->tcpros_listner_proc.socket) ) || + !tcpIpSocketSetReuse( &(n->tcpros_listner_proc.socket) ) || + !tcpIpSocketSetNonBlocking( &(n->tcpros_listner_proc.socket) ) || + !tcpIpSocketBindListen( &(n->tcpros_listner_proc.socket), n->host, 0, CN_MAX_TCPROS_SERVER_CONNECTIONS ) ) + { + PRINT_ERROR("openTcprosListnerSocket() failed"); + ret=-1; + } + else + { + n->tcpros_port = tcpIpSocketGetPort( &(n->tcpros_listner_proc.socket) ); + PRINT_VDEBUG ( "openTcprosListnerSocket() : Accepting tcpros connections at port %d\n", n->tcpros_port ); + ret=0; // success + } + return(ret); +} + +static void closeTcprosProcess(TcprosProcess *process) +{ + tcpIpSocketClose(&process->socket); + tcprosProcessReset(process); +} + +static void closeXmlrpcProcess(XmlrpcProcess *process) +{ + tcpIpSocketClose(&process->socket); + xmlrpcProcessReset(process); + xmlrpcProcessChangeState(process, XMLRPC_PROCESS_STATE_IDLE); +} + +// This method is used to communicate that some api calls at least attempted +// to complete, like in the case of unregistration when a gracefully shutdown +// is requested +static void handleApiCallAttempt(CrosNode *node, RosApiCall *call) +{ + CrosNodeStatusUsr status; + + initCrosNodeStatus(&status); + status.provider_idx = call->provider_idx; + + switch(call->method) + { + case CROS_API_UNREGISTER_PUBLISHER: + { + if (call->provider_idx == -1) + break; + + PublisherNode *pub = &node->pubs[call->provider_idx]; + status.state = CROS_STATUS_PUBLISHER_UNREGISTERED; + cRosNodeStatusCallback(&status, pub->context); // calls the publisher application-defined callback function (if specified when creating the publisher) + + // Finally release publisher + cRosApiReleasePublisher(node, call->provider_idx); + initPublisherNode(pub); + call->provider_idx = -1; + break; + } + case CROS_API_UNREGISTER_SUBSCRIBER: + { + if (call->provider_idx == -1) + break; + + SubscriberNode *sub = &node->subs[call->provider_idx]; + status.state = CROS_STATUS_SUBSCRIBER_UNREGISTERED; + cRosNodeStatusCallback(&status, sub->context); + + // Finally release subscriber + cRosApiReleaseSubscriber(node, call->provider_idx); + initSubscriberNode(sub); + call->provider_idx = -1; + break; + } + case CROS_API_UNREGISTER_SERVICE: + { + if (call->provider_idx == -1) + break; + + ServiceProviderNode *service = &node->service_providers[call->provider_idx]; + status.state = CROS_STATUS_SERVICE_PROVIDER_UNREGISTERED; + cRosNodeStatusCallback(&status, service->context); + + // Finally release service provider + cRosApiReleaseServiceProvider(node, call->provider_idx); + initServiceProviderNode(service); + call->provider_idx = -1; + break; + } + case CROS_API_UNSUBSCRIBE_PARAM: + { + if (call->provider_idx == -1) + break; + + ParameterSubscription *subscription = &node->paramsubs[call->provider_idx]; + status.state = CROS_STATUS_PARAM_UNSUBSCRIBED; + status.parameter_key = subscription->parameter_key; + subscription->status_api_callback(&status, subscription->context); + + // Finally release parameter subscription + cRosNodeReleaseParameterSubscrition(subscription); + initParameterSubscrition(subscription); + call->provider_idx = -1; + break; + } + default: + { + break; + } + } +} + +static void cleanApiCallState(CrosNode *node, RosApiCall *call) +{ + switch(call->method) + { + case CROS_API_REQUEST_TOPIC: + { + // transitory xmlrpc client processes are checked and cleared one by one in the subscriber unregistration function + break; + } + default: + { + break; + } + } +} + +static void handleXmlrpcClientError(CrosNode *node, int i) +{ + XmlrpcProcess *proc = &node->xmlrpc_client_proc[i]; + RosApiCall *call = proc->current_call; + + switch (call->method) + { + case CROS_API_REGISTER_SERVICE: + case CROS_API_REGISTER_PUBLISHER: + case CROS_API_REGISTER_SUBSCRIBER: + case CROS_API_SUBSCRIBE_PARAM: + { + proc->current_call = NULL; + enqueueApiCall(&node->master_api_queue, call); + break; + } + default: + { + ResultCallback callback = call->result_callback; + if (callback != NULL) + { + // Notifies an error with a NULL result + callback(call->id, NULL, call->context_data); + } + break; + } + } + + handleApiCallAttempt(node, call); + cleanApiCallState(node, call); + closeXmlrpcProcess(proc); +} + +static void handleTcprosClientError(CrosNode *n, int i) +{ + TcprosProcess *process = &n->tcpros_client_proc[i]; + closeTcprosProcess(process); + // CHECK-ME Riaccoda register subscriber? +} + +static void handleXmlrpcServerError(CrosNode *n, int i) +{ + XmlrpcProcess *process = &n->xmlrpc_server_proc[i]; + closeXmlrpcProcess(process); +} + +static void handleTcprosServerError(CrosNode *n, int proc_idx) +{ + int list_elem; + TcprosProcess *process = &n->tcpros_server_proc[proc_idx]; + PublisherNode *pub = &n->pubs[process->topic_idx]; + + // Look for the tcpros_server_proc index (proc_idx) in the publisher tcpros_server_proc list (to remove it) + for(list_elem=0;pub->tcpros_id_list[list_elem]!=-1 && pub->tcpros_id_list[list_elem] != proc_idx;list_elem++); + if(pub->tcpros_id_list[list_elem] != proc_idx) // tcpros_server_proc index (proc_idx) found + { + // Remove index + for(;pub->tcpros_id_list[list_elem]!=-1;list_elem++) + pub->tcpros_id_list[list_elem] = pub->tcpros_id_list[list_elem+1]; + } + else + PRINT_ERROR("handleTcprosServerError() : TcprosProcess index %i has not been found in Publisher %i", proc_idx, process->topic_idx); + + closeTcprosProcess(process); +} + +static void handleRpcrosClientError(CrosNode *n, int i) +{ + TcprosProcess *process = &n->rpcros_client_proc[i]; + closeTcprosProcess(process); +} + +static void handleRpcrosServerError(CrosNode *n, int i) +{ + TcprosProcess *process = &n->rpcros_server_proc[i]; + closeTcprosProcess(process); +} + +static cRosErrCodePack xmlrpcClientConnect(CrosNode *n, int i) +{ + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "xmlrpcClientConnect()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; + XmlrpcProcess *client_proc = &(n->xmlrpc_client_proc[i]); + + PRINT_VDEBUG ( "xmlrpcClientConnect() : Connecting\n" ); + + if( !client_proc->socket.connected ) + { + TcpIpSocketState conn_state; + RosApiCall *xml_call; + + if(!client_proc->socket.open) + openXmlrpcClientSocket(n, i); + + xml_call = client_proc->current_call; + if(xml_call == NULL) + { + PRINT_ERROR ( "xmlrpcClientConnect() : Invalid XMLRPC call\n" ); + return CROS_UNSPECIFIED_ERR; + } + + if (i == 0 || isRosMasterApi(xml_call->method)) + { + conn_state = tcpIpSocketConnect( &(client_proc->socket), + n->roscore_host, n->roscore_port ); + } + else // slave api or client xmlrpc to be invoked from a subscriber + { + conn_state = tcpIpSocketConnect(&client_proc->socket, + xml_call->host, xml_call->port); + } + + if( conn_state == TCPIPSOCKET_DONE) + { + xmlrpcProcessChangeState( client_proc, XMLRPC_PROCESS_STATE_WRITING ); + } + else if( conn_state == TCPIPSOCKET_IN_PROGRESS ) + { + PRINT_VDEBUG ( "xmlrpcClientConnect() : Wait: connection is established asynchronously\n" ); // Not connected yet + } + else if( conn_state == TCPIPSOCKET_REFUSED ) + { + PRINT_ERROR("xmlrpcClientConnect() : Connection of XMLRPC client number %i was refused (is the target port not open?)\n", i); + handleXmlrpcClientError( n, i); + ret_err = CROS_XMLRPC_CLI_REFUS_ERR; + } + else + { + if( conn_state != TCPIPSOCKET_FAILED ) + PRINT_ERROR ( "tcprosClientConnect() : invalid connection state\n" ); + PRINT_ERROR("xmlrpcClientConnect() : XMLRPC client number %i can't connect\n", i); + handleXmlrpcClientError( n, i); + ret_err = CROS_XMLRPC_CLI_CONN_ERR; + } + } + + return ret_err; +} + +static cRosErrCodePack doWithXmlrpcClientSocket(CrosNode *n, int i) +{ + cRosErrCodePack ret_err; + XmlrpcProcess *client_proc; + PRINT_VVDEBUG ( "doWithXmlrpcClientSocket()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; + client_proc = &(n->xmlrpc_client_proc[i]); + + switch( client_proc->state ) + { + case XMLRPC_PROCESS_STATE_CONNECTING: + { + ret_err = xmlrpcClientConnect(n, i); + break; + } + case XMLRPC_PROCESS_STATE_WRITING: + { + PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : writing. Xmlrpc client index: %d\n", i); + + cRosApiPrepareRequest( n, i ); + + TcpIpSocketState sock_state = tcpIpSocketWriteString( &(client_proc->socket), + &(client_proc->message) ); + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + { + // NB: Node release is done by handleApiCallAttempt when the ros master response is received + xmlrpcProcessClear(client_proc); + xmlrpcProcessChangeState( client_proc, XMLRPC_PROCESS_STATE_READING ); + break; + } + + case TCPIPSOCKET_IN_PROGRESS: + { + PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : Write in progress...\n" ); + break; + } + + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + { + PRINT_ERROR("doWithXmlrpcClientSocket() : Unexpected failure writing request\n"); + handleXmlrpcClientError( n, i ); + ret_err = CROS_XMLRPC_CLI_WRITE_ERR; + break; + } + } + break; + } + case XMLRPC_PROCESS_STATE_READING: + { + PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : Reading. Xmlrpc client index: %d\n", i); + TcpIpSocketState sock_state = tcpIpSocketReadString( &client_proc->socket, + &client_proc->message ); + XmlrpcParserState parser_state = XMLRPC_PARSER_INCOMPLETE; + + int disconnected = 0; + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + { + parser_state = parseXmlrpcMessage( &client_proc->message, + &client_proc->message_type, + NULL, + &client_proc->response, + client_proc->host, + &client_proc->port); + break; + } + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + { + parser_state = parseXmlrpcMessage( &client_proc->message, + &client_proc->message_type, + NULL, + &client_proc->response, + client_proc->host, + &client_proc->port); + disconnected = 1; + break; + } + + case TCPIPSOCKET_FAILED: + default: + { + PRINT_ERROR("doWithXmlrpcClientSocket() : Unexpected failure reading response\n" ); + handleXmlrpcClientError( n, i ); + ret_err = CROS_XMLRPC_CLI_READ_ERR; + break; + } + } + + switch ( parser_state ) + { + case XMLRPC_PARSER_DONE: + { + PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : Done with no error\n" ); + + int rc = cRosApiParseResponse( n, i ); + handleApiCallAttempt(n, client_proc->current_call); + if (rc != 0) + { + handleXmlrpcClientError( n, i ); + } + else + { + cleanApiCallState(n, client_proc->current_call); + closeXmlrpcProcess(client_proc); + } + break; + } + + case XMLRPC_PARSER_INCOMPLETE: + { + if (disconnected) + handleXmlrpcClientError( n, i ); + break; + } + + case XMLRPC_PARSER_ERROR: + default: + { + handleXmlrpcClientError( n, i ); + break; + } + } + break; + } + default: + { + PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : Invalid XMLRPC process state\n" ); + } + } + return ret_err; +} + +static cRosErrCodePack doWithXmlrpcServerSocket( CrosNode *n, int i ) +{ + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "doWithXmlrpcServerSocket()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; + XmlrpcProcess *server_proc = &(n->xmlrpc_server_proc[i]); + + if( server_proc->state == XMLRPC_PROCESS_STATE_READING ) + { + PRINT_VDEBUG ( "doWithXmlrpcServerSocket() : Reading. Xmlrpc server index: %d\n", i ); + TcpIpSocketState sock_state = tcpIpSocketReadString( &(server_proc->socket), + &(server_proc->message) ); + XmlrpcParserState parser_state = XMLRPC_PARSER_INCOMPLETE; + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + parser_state = parseXmlrpcMessage( &server_proc->message, + &server_proc->message_type, + &server_proc->method, + &server_proc->params, + server_proc->host, + &server_proc->port); + break; + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + xmlrpcProcessReset( &(n->xmlrpc_server_proc[i]) ); + xmlrpcProcessChangeState( &(n->xmlrpc_server_proc[i]), XMLRPC_PROCESS_STATE_IDLE ); + tcpIpSocketClose( &(n->xmlrpc_server_proc[i].socket) ); + break; + case TCPIPSOCKET_FAILED: + default: + PRINT_ERROR("doWithXmlrpcServerSocket() : Unexpected failure reading request\n"); + handleXmlrpcServerError( n, i ); + break; + } + + switch ( parser_state ) + { + case XMLRPC_PARSER_DONE: + { + + PRINT_VDEBUG ( "doWithXmlrpcServerSocket() : Done reading and parsing with no error\n" ); + int rc = cRosApiParseRequestPrepareResponse( n, i ); + if (rc == -1) + { + handleXmlrpcServerError( n, i ); + break; + } + xmlrpcProcessChangeState( server_proc, XMLRPC_PROCESS_STATE_WRITING ); + break; + } + case XMLRPC_PARSER_INCOMPLETE: + break; + + case XMLRPC_PARSER_ERROR: + default: + { + handleXmlrpcServerError( n, i ); + break; + } + } + } + else if( server_proc->state == XMLRPC_PROCESS_STATE_WRITING ) + { + PRINT_VDEBUG ( "doWithXmlrpcServerSocket() : writing. Xmlrpc server index %d \n", i ); + TcpIpSocketState sock_state = tcpIpSocketWriteString( &(server_proc->socket), + &(server_proc->message) ); + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + xmlrpcProcessReset( server_proc ); + xmlrpcProcessChangeState( server_proc, XMLRPC_PROCESS_STATE_READING ); + break; + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + xmlrpcProcessReset( server_proc ); + xmlrpcProcessChangeState( server_proc, XMLRPC_PROCESS_STATE_IDLE ); + tcpIpSocketClose( &(server_proc->socket) ); + break; + + case TCPIPSOCKET_FAILED: + default: + PRINT_ERROR("doWithXmlrpcServerSocket() : Unexpected failure writing response\n"); + handleXmlrpcServerError( n, i ); + break; + } + } + return ret_err; +} + +static cRosErrCodePack tcprosClientConnect( CrosNode *n, int client_idx) +{ + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "tcprosClientConnect()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; + TcprosProcess *client_proc = &(n->tcpros_client_proc[client_idx]); + + if(!client_proc->socket.open) + openTcprosClientSocket(n, client_idx); + + tcprosProcessClear( client_proc ); // clear packet buffer and variable indicating bytes left to receive (left_to_recv) + TcpIpSocketState conn_state = tcpIpSocketConnect( &(client_proc->socket), + client_proc->sub_tcpros_host, client_proc->sub_tcpros_port ); + switch (conn_state) + { + case TCPIPSOCKET_DONE: + { + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WRITING_HEADER ); + break; + } + case TCPIPSOCKET_IN_PROGRESS: + { + PRINT_VDEBUG ( "tcprosClientConnect() : Wait: connection is established asynchronously\n" ); + // Wait: connection is established asynchronously + break; + } + case TCPIPSOCKET_REFUSED: + { + PRINT_ERROR("tcprosClientConnect() : Connection of TCPROS client number %i was refused (is the target port not open?)\n", client_idx); + handleTcprosClientError( n, client_idx); + ret_err = CROS_TCPROS_CLI_REFUS_ERR; + break; + } + default: + { + PRINT_ERROR ( "tcprosClientConnect() : invalid connection state\n" ); + // no break execute next case too + } + case TCPIPSOCKET_FAILED: + { + PRINT_ERROR ( "tcprosClientConnect() : An error occurred when TCPROS client number %i was trying to connect\n", client_idx); + handleTcprosClientError( n, client_idx); + ret_err = CROS_TCPROS_CLI_CONN_ERR; + break; + } + } + + return ret_err; +} + +static cRosErrCodePack doWithTcprosClientSocket( CrosNode *n, int client_idx) +{ + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "doWithTcprosClientSocket()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; + TcprosProcess *client_proc = &(n->tcpros_client_proc[client_idx]); + + switch ( client_proc->state ) + { + case TCPROS_PROCESS_STATE_CONNECTING: + { + ret_err = tcprosClientConnect( n, client_idx); + break; + } + case TCPROS_PROCESS_STATE_WRITING_HEADER: + { + cRosMessagePrepareSubcriptionHeader( n, client_idx); + + TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(client_proc->socket), + &(client_proc->packet) ); + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + tcprosProcessClear( client_proc ); + client_proc->left_to_recv = sizeof(uint32_t); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_HEADER_SIZE ); + + break; + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + closeTcprosProcess( client_proc ); + break; + + case TCPIPSOCKET_FAILED: + default: + handleTcprosClientError( n, client_idx ); + break; + } + break; + } + case TCPROS_PROCESS_STATE_READING_HEADER_SIZE: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), + &(client_proc->packet), + client_proc->left_to_recv, + &n_reads); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + client_proc->left_to_recv -= n_reads; + if (client_proc->left_to_recv == 0) + { + const unsigned char *data = dynBufferGetCurrentData(&client_proc->packet); + uint32_t header_size; + header_size = ROS_TO_HOST_UINT32(*(uint32_t *)data); + tcprosProcessClear( client_proc ); + client_proc->left_to_recv = header_size; + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_HEADER); + goto read_header; + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + handleTcprosClientError( n, client_idx ); + break; + } + + break; + } + read_header: + case TCPROS_PROCESS_STATE_READING_HEADER: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), + &(client_proc->packet), + client_proc->left_to_recv, + &n_reads); + TcprosParserState parser_state = TCPROS_PARSER_ERROR; + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + client_proc->left_to_recv -= n_reads; + if (client_proc->left_to_recv == 0) + { + parser_state = cRosMessageParsePublicationHeader( n, client_idx ); + break; + } + parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; + break; + case TCPIPSOCKET_IN_PROGRESS: + parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; + break; + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + handleTcprosClientError( n, client_idx ); + break; + } + + switch ( parser_state ) + { + case TCPROS_PARSER_DONE: + tcprosProcessClear( client_proc ); + client_proc->left_to_recv = sizeof(uint32_t); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_SIZE ); + break; + case TCPROS_PARSER_HEADER_INCOMPLETE: + break; + case TCPROS_PARSER_ERROR: + default: + handleTcprosClientError( n, client_idx ); + break; + } + break; // To avoid blocking ??? + } + case TCPROS_PROCESS_STATE_READING_SIZE: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), + &(client_proc->packet), + client_proc->left_to_recv, + &n_reads); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + client_proc->left_to_recv -= n_reads; + if (client_proc->left_to_recv == 0) + { + const unsigned char *data = dynBufferGetCurrentData(&client_proc->packet); + uint32_t msg_size = 0; + msg_size = ROS_TO_HOST_UINT32(*(uint32_t *)data); + tcprosProcessClear( client_proc ); + client_proc->left_to_recv = msg_size; + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING); + goto read_msg; + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + handleTcprosClientError( n, client_idx ); + break; + } + break; + } + read_msg: + case TCPROS_PROCESS_STATE_READING: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), + &(client_proc->packet), + client_proc->left_to_recv, + &n_reads); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + client_proc->left_to_recv -= n_reads; + if (client_proc->left_to_recv == 0) + { + ret_err = cRosMessageParsePublicationPacket(n, client_idx); + tcprosProcessClear( client_proc ); + client_proc->left_to_recv = sizeof(uint32_t); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_SIZE ); + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + handleTcprosClientError( n, client_idx ); + break; + } + break; + } + default: + { + PRINT_ERROR( "doWithTcprosClientSocket() : Invalid TCPROS process state\n" ); + } + + } + return ret_err; +} + +static cRosErrCodePack doWithTcprosServerSocket( CrosNode *n, int i ) +{ + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "doWithTcprosServerSocket()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; // Default return value + TcprosProcess *server_proc = &(n->tcpros_server_proc[i]); + + if( server_proc->state == TCPROS_PROCESS_STATE_READING_HEADER) + { + PRINT_VDEBUG ( "doWithTcprosServerSocket() : Reading header. Tcpros server index %d \n", i ); + tcprosProcessClear( server_proc ); + TcpIpSocketState sock_state = tcpIpSocketReadBuffer( &(server_proc->socket), + &(server_proc->packet) ); + TcprosParserState parser_state = TCPROS_PARSER_ERROR; + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + parser_state = cRosMessageParseSubcriptionHeader( n, i ); + break; + + case TCPIPSOCKET_IN_PROGRESS: + parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; + break; + + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + PRINT_INFO( "doWithTcprosServerSocket() : Client disconnected?\n" ); + handleTcprosServerError( n, i ); + break; + } + + switch ( parser_state ) + { + case TCPROS_PARSER_DONE: + + PRINT_VDEBUG ( "doWithTcprosServerSocket() : Done reading and parsing with no error\n" ); + tcprosProcessClear( server_proc ); + cRosMessagePreparePublicationHeader( n, i ); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING ); // Proceed to write the header + break; + case TCPROS_PARSER_HEADER_INCOMPLETE: + break; + + case TCPROS_PARSER_ERROR: + default: + PRINT_INFO( "doWithTcprosServerSocket() : Parser error\n" ); + handleTcprosServerError( n, i ); + break; + } + } + else if( server_proc->state == TCPROS_PROCESS_STATE_START_WRITING || + server_proc->state == TCPROS_PROCESS_STATE_WRITING ) // It is time to write a message or header + { + PRINT_VDEBUG ( "doWithTcprosServerSocket() : writing. Tcpros server index: %d \n", i ); + if( server_proc->state == TCPROS_PROCESS_STATE_START_WRITING ) // Start to publish a message + { + tcprosProcessClear( server_proc ); + ret_err = cRosMessagePreparePublicationPacket( n, i ); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING ); + } + TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(server_proc->socket), + &(server_proc->packet) ); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + PRINT_VDEBUG ( "doWithTcprosServerSocket() : Done writing with no error\n" ); + tcprosProcessClear( server_proc ); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WAIT_FOR_WRITING ); // Wait before publishing a new message + break; + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + PRINT_INFO( "doWithTcprosServerSocket() : Client disconnected?\n" ); + handleTcprosServerError(n, i); + break; + } + } + return ret_err; +} + +static cRosErrCodePack rpcrosClientConnect(CrosNode *n, int client_idx) +{ + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "rpcrosClientConnect()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; + TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); + + if(!client_proc->socket.open) + openRpcrosClientSocket(n, client_idx); + + ServiceCallerNode *service_caller = &(n->service_callers[client_proc->service_idx]); + tcprosProcessClear( client_proc ); + TcpIpSocketState conn_state = tcpIpSocketConnect( &(client_proc->socket), service_caller->service_host, service_caller->service_port ); + switch (conn_state) + { + case TCPIPSOCKET_DONE: + { + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WRITING_HEADER ); + break; + } + case TCPIPSOCKET_IN_PROGRESS: + { + PRINT_VDEBUG ( "rpcrosClientConnect() : Wait: connection is established asynchronously\n" ); + // Wait: connection is established asynchronously + break; + } + case TCPIPSOCKET_REFUSED: + { + PRINT_ERROR("xmlrpcClientConnect() : Connection of RPCROS client number %i was refused (is the target port not open?)\n", client_idx); + handleRpcrosClientError( n, client_idx); + ret_err = CROS_RPCROS_CLI_REFUS_ERR; + break; + } + default: + { + PRINT_ERROR ( "rpcrosClientConnect() : invalid connection state\n" ); + // no break execute next case too + } + case TCPIPSOCKET_FAILED: + { + PRINT_ERROR ( "rpcrosClientConnect() : An error occurred when RPCROS client number %i was trying to connect\n", client_idx); + handleRpcrosClientError( n, client_idx); + ret_err = CROS_RPCROS_CLI_CONN_ERR; + break; + } + } + + return ret_err; +} + +static cRosErrCodePack doWithRpcrosClientSocket(CrosNode *n, int client_idx) +{ + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "doWithRpcrosClientSocket()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; + TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); + + switch ( client_proc->state ) + { + case TCPROS_PROCESS_STATE_CONNECTING: + { + ret_err = rpcrosClientConnect(n, client_idx); + break; + } + case TCPROS_PROCESS_STATE_WRITING_HEADER: + { + cRosMessagePrepareServiceCallHeader(n, client_idx); + + TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(client_proc->socket), &(client_proc->packet) ); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + tcprosProcessClear( client_proc ); + client_proc->left_to_recv = sizeof(uint32_t); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_HEADER_SIZE ); + + break; + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + closeTcprosProcess( client_proc ); + break; + + case TCPIPSOCKET_FAILED: + default: + handleRpcrosClientError( n, client_idx ); + break; + } + break; + } + case TCPROS_PROCESS_STATE_READING_HEADER_SIZE: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), + &(client_proc->packet), + client_proc->left_to_recv, + &n_reads); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + client_proc->left_to_recv -= n_reads; + if (client_proc->left_to_recv == 0) + { + const unsigned char *data = dynBufferGetCurrentData(&client_proc->packet); + uint32_t header_size; + header_size = ROS_TO_HOST_UINT32(*(uint32_t *)data); + tcprosProcessClear( client_proc ); + client_proc->left_to_recv = header_size; + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_HEADER); + goto read_header; + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + tcpIpSocketClose(&client_proc->socket); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); + break; + case TCPIPSOCKET_FAILED: + default: + handleRpcrosClientError( n, client_idx ); + break; + } + + break; + } + read_header: + case TCPROS_PROCESS_STATE_READING_HEADER: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), + &(client_proc->packet), + client_proc->left_to_recv, + &n_reads); + TcprosParserState parser_state = TCPROS_PARSER_ERROR; + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + client_proc->left_to_recv -= n_reads; + if (client_proc->left_to_recv == 0) + { + parser_state = cRosMessageParseServiceProviderHeader( n, client_idx ); + break; + } + parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; + break; + case TCPIPSOCKET_IN_PROGRESS: + parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; + break; + case TCPIPSOCKET_DISCONNECTED: + tcpIpSocketClose(&client_proc->socket); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); + break; + case TCPIPSOCKET_FAILED: + default: + handleRpcrosClientError( n, client_idx ); + break; + } + + switch ( parser_state ) + { + case TCPROS_PARSER_DONE: + tcprosProcessClear( client_proc ); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_WRITING ); + break; + case TCPROS_PARSER_HEADER_INCOMPLETE: + break; + case TCPROS_PARSER_ERROR: + default: + handleRpcrosClientError( n, client_idx ); + break; + } + break; + } + + case TCPROS_PROCESS_STATE_START_WRITING: + { + tcprosProcessClear( client_proc ); + ret_err = cRosMessagePrepareServiceCallPacket(n, client_idx); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WRITING ); + } + + case TCPROS_PROCESS_STATE_WRITING: + { + PRINT_VDEBUG ( "doWithRpcrosClientSocket() : writing. Rpcros client index: %d \n", client_idx ); + TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(client_proc->socket), + &(client_proc->packet) ); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + PRINT_VDEBUG ( "doWithRpcrosClientSocket() : Done writing with no error\n" ); + tcprosProcessClear( client_proc ); // Clears only packet and left_to_recv vars + client_proc->left_to_recv = sizeof(uint32_t) + sizeof(uint8_t); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_SIZE ); + break; + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + tcpIpSocketClose(&client_proc->socket); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); + break; + case TCPIPSOCKET_FAILED: + default: + PRINT_INFO( "doWithRpcrosClientSocket() : Client disconnected\n" ); + handleRpcrosClientError(n, client_idx); + break; + } + break; + } + + case TCPROS_PROCESS_STATE_READING_SIZE: // Read 'ok' byte and response-packet size field + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), + &(client_proc->packet), + client_proc->left_to_recv, + &n_reads); + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + client_proc->left_to_recv -= n_reads; + if (client_proc->left_to_recv == 0) + { + const uint8_t *data = (const uint8_t *)dynBufferGetCurrentData(&client_proc->packet); + uint32_t msg_size; + client_proc->ok_byte = *data; + msg_size = ROS_TO_HOST_UINT32(*(uint32_t *)(data+1)); + tcprosProcessClear( client_proc ); + client_proc->left_to_recv = msg_size; + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING); + goto read_msg; + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + tcpIpSocketClose(&client_proc->socket); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); + break; + case TCPIPSOCKET_FAILED: + default: + handleRpcrosClientError( n, client_idx ); + break; + } + break; + } + read_msg: + case TCPROS_PROCESS_STATE_READING: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), + &(client_proc->packet), + client_proc->left_to_recv, + &n_reads); + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + client_proc->left_to_recv -= n_reads; + if (client_proc->left_to_recv == 0) + { + ret_err = cRosMessageParseServiceResponsePacket(n, client_idx); + if(client_proc->persistent) + { + tcprosProcessClear( client_proc ); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_WRITING ); + } + else + { + tcprosProcessClear( client_proc ); + tcpIpSocketClose( &(client_proc->socket) ); + openRpcrosClientSocket(n, client_idx); + // The master should be checked before contacting the service provider again, + // that is, before going to state TCPROS_PROCESS_STATE_CONNECTING again. + // Services are stateless (unless the persistent parameter is set to 1) + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_IDLE ); + if(enqueueServiceLookup(n, client_proc->service_idx) == -1) + handleRpcrosClientError( n, client_idx ); + } + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + tcpIpSocketClose(&client_proc->socket); + tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); + break; + case TCPIPSOCKET_FAILED: + default: + handleRpcrosClientError( n, client_idx ); + break; + } + break; + } + default: + { + PRINT_ERROR ( "doWithRpcrosClientSocket() : Invalid RPC ROS process state\n" ); + } + + } + return ret_err; +} + +static cRosErrCodePack doWithRpcrosServerSocket(CrosNode *n, int i) +{ + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "doWithRpcrosServerSocket()\n" ); + + ret_err = CROS_SUCCESS_ERR_PACK; + TcprosProcess *server_proc = &(n->rpcros_server_proc[i]); + + switch (server_proc->state) + { + case TCPROS_PROCESS_STATE_READING_HEADER_SIZE: + { + size_t n_reads; + if (server_proc->left_to_recv == 0) + server_proc->left_to_recv = sizeof(uint32_t); + + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(server_proc->socket), + &(server_proc->packet), + server_proc->left_to_recv, + &n_reads); + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + server_proc->left_to_recv -= n_reads; + if (server_proc->left_to_recv == 0) + { + const unsigned char *data = dynBufferGetCurrentData(&server_proc->packet); + uint32_t header_size; + header_size = ROS_TO_HOST_UINT32(*(uint32_t *)data); + tcprosProcessClear( server_proc ); + server_proc->left_to_recv = header_size; + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING_HEADER); + goto read_header; + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + closeTcprosProcess( server_proc ); + break; + case TCPIPSOCKET_FAILED: + default: + handleRpcrosServerError( n, i ); + break; + } + + break; + } + read_header: + case TCPROS_PROCESS_STATE_READING_HEADER: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(server_proc->socket), + &(server_proc->packet), + server_proc->left_to_recv, + &n_reads); + TcprosParserState parser_state = TCPROS_PARSER_ERROR; + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + server_proc->left_to_recv -= n_reads; + if (server_proc->left_to_recv == 0) + { + parser_state = cRosMessageParseServiceCallerHeader( n, i ); + break; + } + parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; + break; + case TCPIPSOCKET_IN_PROGRESS: + parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; + break; + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + handleRpcrosServerError( n, i ); + break; + } + + switch ( parser_state ) + { + case TCPROS_PARSER_DONE: + tcprosProcessClear( server_proc ); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING_HEADER ); + PRINT_VDEBUG("doWithRpcrosServerSocket() : Header parsed. Probing session: %d. Rpcros server index: %d \n", server_proc->probe, i); + break; + case TCPROS_PARSER_HEADER_INCOMPLETE: + break; + case TCPROS_PARSER_ERROR: + default: + handleRpcrosServerError( n, i ); + break; + } + + break; + } + case TCPROS_PROCESS_STATE_WRITING_HEADER: + { + PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Writing header. RpcrosServer index: %d \n", i ); + + cRosMessagePrepareServiceProviderHeader( n, i ); + + TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(server_proc->socket), + &(server_proc->packet) ); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Done writing header with no error. RpcrosServer index: %d \n", i); + if(server_proc->probe) + { + if(server_proc->persistent) + { + tcprosProcessClear( server_proc ); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING_HEADER_SIZE); + server_proc->left_to_recv = sizeof(uint32_t); + } + else + closeTcprosProcess( server_proc ); + } + else + { + tcprosProcessClear( server_proc ); + server_proc->left_to_recv = sizeof(uint32_t); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING_SIZE ); + } + break; + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + closeTcprosProcess( server_proc ); + break; + + case TCPIPSOCKET_FAILED: + default: + handleRpcrosServerError( n, i ); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_IDLE ); + break; + } + + break; + } + case TCPROS_PROCESS_STATE_READING_SIZE: + { + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(server_proc->socket), + &(server_proc->packet), + server_proc->left_to_recv, + &n_reads); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + server_proc->left_to_recv -= n_reads; + if (server_proc->left_to_recv == 0) + { + const uint32_t *data = (const uint32_t *)dynBufferGetCurrentData(&server_proc->packet); + uint32_t msg_size; + msg_size = ROS_TO_HOST_UINT32(*data); + tcprosProcessClear( server_proc ); + if (msg_size == 0) + { + PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Done reading size with no error\n" ); + ret_err = cRosMessagePrepareServiceResponsePacket(n, i); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING); + goto write_msg; + } + else + { + server_proc->left_to_recv = msg_size; + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING); + goto read_msg; + } + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + case TCPIPSOCKET_FAILED: + default: + handleRpcrosServerError( n, i ); + break; + } + break; + } + read_msg: + case TCPROS_PROCESS_STATE_READING: + { + PRINT_VDEBUG ( "doWithRpcrosServerSocket() : reading. RpcrosServer index: %d\n", i ); + + size_t n_reads; + TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(server_proc->socket), + &(server_proc->packet), + server_proc->left_to_recv, + &n_reads); + + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + server_proc->left_to_recv -= n_reads; + if (server_proc->left_to_recv == 0) + { + PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Done reading with no error\n" ); + ret_err = cRosMessagePrepareServiceResponsePacket(n, i); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING ); + } + break; + case TCPIPSOCKET_IN_PROGRESS: + break; + case TCPIPSOCKET_DISCONNECTED: + closeTcprosProcess( server_proc ); + break; + case TCPIPSOCKET_FAILED: + default: + handleRpcrosServerError( n, i ); + break; + } + + break; + } + write_msg: + case TCPROS_PROCESS_STATE_WRITING: + { + PRINT_VDEBUG ( "doWithRpcrosServerSocket() : writing. Rpcros server index %d \n", i ); + + TcpIpSocketState sock_state; + + //If the rpc response is empty, e.g. log procedures + //if(server_proc->packet.size > 0) + //{ + sock_state = tcpIpSocketWriteBuffer( &(server_proc->socket), &(server_proc->packet) ); + //} + //else + //{ + //sock_state = TCPIPSOCKET_DONE; + //} + switch ( sock_state ) + { + case TCPIPSOCKET_DONE: + PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Done writing with no error\n" ); + if(server_proc->persistent) + { + server_proc->left_to_recv = sizeof(uint32_t); + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING_SIZE); + } + else + closeTcprosProcess( server_proc ); + break; + + case TCPIPSOCKET_IN_PROGRESS: + break; + + case TCPIPSOCKET_DISCONNECTED: + closeTcprosProcess( server_proc ); + break; + + case TCPIPSOCKET_FAILED: + default: + handleRpcrosServerError( n, i ); + break; + } + + break; + } + default: + { + PRINT_ERROR ( "doWithRpcrosServerSocket() : Invalid RPCROS server process state\n" ); + } + } + return ret_err; +} + +/* + * Callback functions for the service callS of the logging mechanism + */ +static CallbackResponse callback_srv_set_logger_level(cRosMessage *request, cRosMessage *response, void* context) +{ + cRosMessageField* level = cRosMessageGetField(request, "level"); + const char* level_str = level->data.as_string; + CrosNode* node = (CrosNode*) context; + if(stringToLogLevel(level_str, &node->log_level) == 0) + PRINT_INFO( "callback_srv_set_logger_level() : Node log priority level changed to %s\n", LogLevelToString(node->log_level)); + return 0; +} + +static CallbackResponse callback_srv_get_loggers(cRosMessage *request, cRosMessage *response, void* context) +{ + const char *log_level_str; + cRosMessage *logger_msg; + cRosMessageField *loggers_field, *level_field; + CrosNode *node = (CrosNode*) context; + + loggers_field = cRosMessageGetField(response, "loggers"); + + logger_msg = cRosMessageFieldArrayAtMsgGet(loggers_field, 0); + if(logger_msg == NULL) // If the message array is empty (it is the first time that the callback fn is called), create a new message (array element) + { + cRosMessageField *logger_field; + + cRosMessageNewBuild(node->message_root_path, "roscpp/Logger", &logger_msg); + logger_field = cRosMessageGetField(logger_msg, "name"); + cRosMessageSetFieldValueString(logger_field, "ros.cros_node"); + + cRosMessageFieldArrayPushBackMsg(loggers_field, logger_msg); + } + level_field = cRosMessageGetField(logger_msg, "level"); + log_level_str = LogLevelToString(node->log_level); + cRosMessageSetFieldValueString(level_field, log_level_str); + + return 0; +} + +int checkNamespaceFormat(const char* name_space) +{ + const char* it_ns = name_space; + + if(*it_ns == '~') + it_ns++; + + while(*it_ns != '\0') + { + if(*it_ns == '/') + { + if(*(it_ns + 1) == '\0' || + !isalpha(*(it_ns + 1))) + return 0; + it_ns++; + } + else + { + if(!isalnum(*it_ns) && *it_ns != '_') + return 0; + } + it_ns++; + } + + return 1; +} + +char* cRosNamespaceBuild(CrosNode* node, const char* resource_name) +{ + char *resolved_name = NULL; + + if(!checkNamespaceFormat(resource_name)) + return NULL; + + if(node == NULL) + { + if(resource_name[0] == '/') + { + resolved_name = (char *)calloc(strlen(resource_name) + 1, sizeof(char)); + strncat(resolved_name, resource_name,strlen(resource_name)); + } + else + { + resolved_name = (char *)calloc(strlen(resource_name) + strlen("/") + 1, sizeof(char)); + strncat(resolved_name, "/",strlen("/")); + strncat(resolved_name, resource_name,strlen(resource_name)); + } + } + else + { + char* node_name = node->name; + switch(resource_name[0]) + { + case '/': + { + //global namespace + if(node != NULL || 1) + { + resolved_name = (char *)calloc(strlen(resource_name) + 1, + sizeof(char)); + strncat(resolved_name,resource_name,strlen(resource_name)); + } + break; + } + case '~': + { + //private namespace + if(node != NULL || 1) + { + resolved_name = (char *)calloc(strlen(node_name) + + strlen("/") + + strlen(resource_name) + 1, + sizeof(char)); + strncat(resolved_name,node_name,strlen(node_name)); + strncat(resolved_name,"/",strlen("/")); + strncat(resolved_name,resource_name + 1,strlen(resource_name) - 1 ); + } + break; + } + default: + { + // the resource has a name that is not global or private + if(node != NULL || 1) + { + char *node_namespace = (char *)calloc(strlen(node_name) + 1, sizeof(char)); + strcpy(node_namespace, node_name); + char *it = node_namespace + strlen(node_name); + while(*(it--) != '/'); + *(it + 2) = '\0'; + resolved_name = (char *)calloc(strlen(node_namespace) + strlen(resource_name) + 1,sizeof(char)); + strncat(resolved_name,node_namespace,strlen(node_namespace)); + strncat(resolved_name,resource_name,strlen(resource_name)); + } + break; + } + } + } + return resolved_name; +} + +int cRosInitializeLibrary(void) +{ + int ret; + ret = tcpIpSocketStartUp(); // Initialize the socket library (it's mandatory on Windows) + // Enable escape character processing for console messages so that debug messages can use colors + // This processing is supported on Windows 10 build 16257 and later (and on Linux). +#ifdef _WIN32 + HANDLE stdout_handle = GetStdHandle(STD_OUTPUT_HANDLE); + if (stdout_handle != INVALID_HANDLE_VALUE) + { + DWORD console_mode = 0; + if (GetConsoleMode(stdout_handle, &console_mode)) + { + // Set the console output mode to process virtual-terminal sequences + if (!SetConsoleMode(stdout_handle, console_mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING)) + PRINT_ERROR("cRosInitializeLibrary() : Can't set console-output mode (Error code: %lu).\n", GetLastError()); + } + else + PRINT_ERROR("cRosInitializeLibrary() : Can't obatin console-output mode (Error code: %lu).\n", GetLastError()); + } + else + PRINT_ERROR("cRosInitializeLibrary() : Can't obatin console handle to configure output mode (Error code: %lu).\n", GetLastError()); +#endif + return(ret); +} + +CrosNode *cRosNodeCreate (const char *node_name, const char *node_host, const char *roscore_host, unsigned short roscore_port, + const char *message_root_path) +{ + CrosNode *new_n; // Value to be returned by this function. NULL on failure + PRINT_VVDEBUG ( "cRosNodeCreate()\n" ); + + if(node_name == NULL || node_host == NULL || roscore_host == NULL ) + { + PRINT_ERROR ( "cRosNodeCreate() : NULL parameters\n" ); + return NULL; + } + + if(cRosInitializeLibrary() != 0) + return NULL; // Initialization of the library failed: exit + + new_n = ( CrosNode * ) malloc ( sizeof ( CrosNode ) ); + + if ( new_n == NULL ) + { + PRINT_ERROR ( "cRosNodeCreate() : Can't allocate memory\n" ); + return NULL; + } + + new_n->name = new_n->host = new_n->roscore_host = NULL; + + new_n->name = cRosNamespaceBuild(NULL, node_name); + new_n->host = ( char * ) malloc ( ( strlen ( node_host ) + 1 ) *sizeof ( char ) ); + new_n->roscore_host = ( char * ) malloc ( ( strlen ( roscore_host ) + 1 ) *sizeof ( char ) ); + new_n->message_root_path = ( char * ) malloc ( ( strlen ( message_root_path ) + 1 ) *sizeof ( char ) ); + + if (new_n->name == NULL || new_n->host == NULL + || new_n->roscore_host == NULL || new_n->message_root_path == NULL ) + { + PRINT_ERROR ( "cRosNodeCreate() : Can't allocate memory\n" ); + cRosNodeDestroy ( new_n ); + return NULL; + } + + strcpy ( new_n->host, node_host ); + strcpy ( new_n->roscore_host, roscore_host ); + strcpy ( new_n->message_root_path, message_root_path ); + + new_n->log_level = CROS_LOGLEVEL_INFO; + new_n->xmlrpc_port = 0; + new_n->tcpros_port = 0; + new_n->roscore_port = roscore_port; + new_n->roscore_pid = -1; + + xmlrpcProcessInit( &(new_n->xmlrpc_listner_proc) ); + + new_n->next_call_id = 0; + initApiCallQueue(&new_n->master_api_queue); + initApiCallQueue(&new_n->slave_api_queue); + + new_n->xmlrpc_master_wake_up_time = 0; + + int i, fn_ret; + for (i = 0 ; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++) + xmlrpcProcessInit( &(new_n->xmlrpc_server_proc[i]) ); + + for ( i = 0 ; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++) + xmlrpcProcessInit( &(new_n->xmlrpc_client_proc[i]) ); + + tcprosProcessInit( &(new_n->tcpros_listner_proc) ); + + for ( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++) + tcprosProcessInit( &(new_n->tcpros_server_proc[i]) ); + + for ( i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++) + tcprosProcessInit( &(new_n->tcpros_client_proc[i]) ); + + tcprosProcessInit( &(new_n->rpcros_listner_proc) ); + + for ( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++) + tcprosProcessInit( &(new_n->rpcros_server_proc[i]) ); + + for ( i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++) + tcprosProcessInit( &(new_n->rpcros_client_proc[i]) ); + + for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS; i++) + initPublisherNode(&new_n->pubs[i]); + new_n->n_pubs = 0; + + for ( i = 0; i < CN_MAX_SUBSCRIBED_TOPICS; i++) + initSubscriberNode(&new_n->subs[i]); + new_n->n_subs = 0; + + for ( i = 0; i < CN_MAX_SERVICE_PROVIDERS; i++) + initServiceProviderNode(&new_n->service_providers[i]); + new_n->n_service_providers = 0; + + for ( i = 0; i < CN_MAX_SERVICE_CALLERS; i++) + initServiceCallerNode(&new_n->service_callers[i]); + new_n->n_service_callers = 0; + + for ( i = 0; i < CN_MAX_PARAMETER_SUBSCRIPTIONS; i++) + initParameterSubscrition(&new_n->paramsubs[i]); + new_n->n_paramsubs = 0; + +#ifdef _WIN32 + new_n->pid = (int)GetCurrentProcessId(); +#else + new_n->pid = (int)getpid(); +#endif + + fn_ret = 0; + for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS && fn_ret == 0; i++) + { + fn_ret = openXmlrpcClientSocket( new_n, i ); + } + + for(i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS && fn_ret == 0; i++) + { + fn_ret = openTcprosClientSocket( new_n, i ); + } + + for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS && fn_ret == 0; i++) + { + fn_ret = openRpcrosClientSocket( new_n, i ); + } + if(fn_ret == 0) + fn_ret = openXmlrpcListnerSocket( new_n ); + if(fn_ret == 0) + fn_ret = openTcprosListnerSocket( new_n ); + if(fn_ret == 0) + fn_ret = openRpcrosListnerSocket( new_n ); + + if(fn_ret != 0) + { + PRINT_ERROR ( "cRosNodeCreate() : socket could not be opened\n" ); + cRosNodeDestroy ( new_n ); + return NULL; + } + + new_n-> log_last_id = 0; + + /* + * Registering logging callback + */ + + cRosErrCodePack ret_err; + + // Create a publisher of the topic /rosout of type "rosgraph_msgs/Log". + // The messages of this topic will not be periodically sent but on demand (loop_period = -1). + ret_err = cRosApiRegisterPublisher(new_n,"/rosout","rosgraph_msgs/Log", -1, NULL, NULL, new_n, &new_n->rosout_pub_idx); + if (ret_err != CROS_SUCCESS_ERR_PACK) + { + PRINT_ERROR ( "cRosNodeCreate(): Error registering rosout.\n"); + cRosPrintErrCodePack(ret_err, "cRosNodeCreate()"); + } + + ret_err = cRosApiRegisterServiceProvider(new_n,"~get_loggers","roscpp/GetLoggers", + callback_srv_get_loggers, NULL, (void *)new_n, NULL); + if (ret_err != CROS_SUCCESS_ERR_PACK) + { + PRINT_ERROR ( "cRosNodeCreate(): Error registering loggers.\n"); + cRosPrintErrCodePack(ret_err, "cRosNodeCreate()"); + } + + ret_err = cRosApiRegisterServiceProvider(new_n,"~set_logger_level","roscpp/SetLoggerLevel", + callback_srv_set_logger_level, NULL, (void *)new_n, NULL); + if (ret_err != CROS_SUCCESS_ERR_PACK) + { + PRINT_ERROR ( "cRosNodeCreate(): Error registering loggers.\n"); + cRosPrintErrCodePack(ret_err, "cRosNodeCreate()"); + } + + return new_n; +} + +int cRosUnregistrationCompleted(CrosNode *n) +{ + int i, unreg_finished; + + unreg_finished = 1; + + for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS && unreg_finished == 1; i++) + if(n->pubs[i].topic_name != NULL) + unreg_finished = 0; + + for ( i = 0; i < CN_MAX_SUBSCRIBED_TOPICS && unreg_finished == 1; i++) + if(n->subs[i].topic_name != NULL) + unreg_finished = 0; + + for ( i = 0; i < CN_MAX_SERVICE_PROVIDERS && unreg_finished == 1; i++) + if(n->service_providers[i].service_name != NULL) + unreg_finished = 0; + + for ( i = 0; i < CN_MAX_PARAMETER_SUBSCRIPTIONS && unreg_finished == 1; i++) + if(n->paramsubs[i].parameter_key != NULL) + unreg_finished = 0; + + return(unreg_finished); +} + +int cRosOutputQueuesEmpty(CrosNode *n) +{ + int i, queues_empty; + + queues_empty = 1; + + for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS && queues_empty == 1; i++) + if(n->pubs[i].topic_name != NULL && cRosMessageQueueUsage(&n->pubs[i].msg_queue) > 0) + queues_empty = 0; + + for ( i = 0; i < CN_MAX_SERVICE_CALLERS && queues_empty == 1; i++) + if(n->service_callers[i].service_name != NULL && cRosMessageQueueUsage(&n->service_callers[i].msg_queue) > 0) + queues_empty = 0; + + return(queues_empty); +} + +// This function continues to run the cROS node until the specified function returns true (1) +cRosErrCodePack cRosNodeWaitUntilFnRetTrue(CrosNode *n, int (*fn_to_check)(CrosNode *n)) +{ + int unreg_incomp; + cRosErrCodePack ret_err = CROS_SUCCESS_ERR_PACK; + uint64_t start_time, elapsed_time; + + start_time = cRosClockGetTimeMs(); + while( (unreg_incomp = !fn_to_check(n)) && (elapsed_time=cRosClockGetTimeMs()-start_time) <= CN_UNREGISTRATION_TIMEOUT && ret_err == CROS_SUCCESS_ERR_PACK) + ret_err=cRosNodeDoEventsLoop( n, CN_UNREGISTRATION_TIMEOUT - elapsed_time); + + if(ret_err == CROS_SUCCESS_ERR_PACK && unreg_incomp) + ret_err = CROS_UNREG_TIMEOUT_ERR; // the unregistration process could not be completed + + return ret_err; +} + +void cRosNodePauseAllCallersPublishers(CrosNode *n) +{ + int i; + + for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS; i++) + if(n->pubs[i].topic_name != NULL) + n->pubs[i].loop_period = -1; + + for ( i = 0; i < CN_MAX_SERVICE_CALLERS; i++) + if(n->service_callers[i].service_name != NULL) + n->service_callers[i].loop_period = -1; +} + +cRosErrCodePack cRosNodeUnregisterAll(CrosNode *n) +{ + int i; + cRosErrCodePack ret_err=CROS_SUCCESS_ERR_PACK; + + for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS && ret_err == CROS_SUCCESS_ERR_PACK; i++) + if(n->pubs[i].topic_name != NULL) + ret_err = cRosApiUnregisterPublisher(n, i); + + for ( i = 0; i < CN_MAX_SUBSCRIBED_TOPICS && ret_err == CROS_SUCCESS_ERR_PACK; i++) + if(n->subs[i].topic_name != NULL) + ret_err = cRosApiUnregisterSubscriber(n, i); + + for ( i = 0; i < CN_MAX_SERVICE_PROVIDERS && ret_err == CROS_SUCCESS_ERR_PACK; i++) + if(n->service_providers[i].service_name != NULL) + ret_err = cRosApiUnregisterServiceProvider(n, i); + + for ( i = 0; i < CN_MAX_PARAMETER_SUBSCRIPTIONS && ret_err == CROS_SUCCESS_ERR_PACK; i++) + if(n->paramsubs[i].parameter_key != NULL) + ret_err = cRosApiUnsubscribeParam(n, i); + + return ret_err; +} + +cRosErrCodePack cRosNodeDestroy ( CrosNode *n ) +{ + cRosErrCodePack ret_err; + + PRINT_VVDEBUG ( "cRosNodeDestroy()\n" ); + + if ( n == NULL ) + return CROS_BAD_PARAM_ERR; + + cRosNodeWaitUntilFnRetTrue(n, cRosOutputQueuesEmpty); // Wait until all pendind topic messages and service call have been sent + + cRosNodePauseAllCallersPublishers(n); + ret_err = cRosNodeUnregisterAll(n); + if(ret_err == CROS_SUCCESS_ERR_PACK) + ret_err = cRosNodeWaitUntilFnRetTrue(n, cRosUnregistrationCompleted); // Wait until all roles have been unsuscribed + + xmlrpcProcessRelease( &(n->xmlrpc_listner_proc) ); + + releaseApiCallQueue(&n->master_api_queue); + releaseApiCallQueue(&n->slave_api_queue); + + int i; + for (i = 0; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++) + xmlrpcProcessRelease( &(n->xmlrpc_server_proc[i]) ); + + for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++) + xmlrpcProcessRelease( &(n->xmlrpc_client_proc[i]) ); + + tcprosProcessRelease( &(n->tcpros_listner_proc) ); + + for ( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++) + tcprosProcessRelease( &(n->tcpros_server_proc[i]) ); + + for ( i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++) + tcprosProcessRelease( &(n->tcpros_client_proc[i]) ); + + for ( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++) + tcprosProcessRelease( &(n->rpcros_server_proc[i]) ); + + for ( i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++) + tcprosProcessRelease( &(n->rpcros_client_proc[i]) ); + + if ( n->name != NULL ) free ( n->name ); + if ( n->host != NULL ) free ( n->host ); + if ( n->roscore_host != NULL ) free ( n->roscore_host ); + + for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS; i++) + cRosApiReleasePublisher(n, i); + + for ( i = 0; i < CN_MAX_SUBSCRIBED_TOPICS; i++) + cRosApiReleaseSubscriber(n, i); + + for ( i = 0; i < CN_MAX_SERVICE_PROVIDERS; i++) + cRosApiReleaseServiceProvider(n, i); + + for ( i = 0; i < CN_MAX_SERVICE_CALLERS; i++) + cRosApiReleaseServiceCaller(n, i); + + for ( i = 0; i < CN_MAX_PARAMETER_SUBSCRIPTIONS; i++) + cRosNodeReleaseParameterSubscrition(&n->paramsubs[i]); + + tcpIpSocketCleanUp(); + + return ret_err; +} + + int cRosNodeRegisterPublisher (CrosNode *node, const char *message_definition, const char *topic_name, + const char *topic_type, const char *md5sum, int loop_period, void *data_context) +{ + PRINT_VVDEBUG ( "cRosNodeRegisterPublisher()\n" ); + + if ( node->n_pubs >= CN_MAX_PUBLISHED_TOPICS ) + { + PRINT_ERROR ( "cRosNodeRegisterPublisher() : Can't register a new publisher: \ + reached the maximum number of published topics\n"); + return -1; + } + + char *pub_message_definition = ( char * ) malloc ( ( strlen ( message_definition ) + 1 ) * sizeof ( char ) ); + char *pub_topic_name = cRosNamespaceBuild(node, topic_name); + char *pub_topic_type = ( char * ) malloc ( ( strlen ( topic_type ) + 1 ) * sizeof ( char ) ); + char *pub_md5sum = ( char * ) malloc ( ( strlen ( md5sum ) + 1 ) * sizeof ( char ) ); + + if ( pub_message_definition == NULL || pub_topic_name == NULL || + pub_topic_type == NULL || pub_md5sum == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterPublisher() : Can't allocate memory\n" ); + return -1; + } + + strcpy ( pub_message_definition, message_definition ); + strcpy ( pub_topic_type, topic_type ); + strcpy ( pub_md5sum, md5sum ); + + PRINT_INFO ( "Publishing topic %s type %s \n", pub_topic_name, pub_topic_type ); + + int pubidx = -1; + int it = 0; + for (; it < CN_MAX_PUBLISHED_TOPICS; it++) + { + if (node->pubs[it].topic_name == NULL) + { + pubidx = it; + break; + } + } + + PublisherNode *pub = &node->pubs[pubidx]; + pub->message_definition = pub_message_definition; + pub->topic_name = pub_topic_name; + pub->topic_type = pub_topic_type; + pub->md5sum = pub_md5sum; + + pub->loop_period = loop_period; + pub->wake_up_time = 0; + pub->context = data_context; + cRosMessageQueueClear(&pub->msg_queue); + + node->n_pubs++; + + int rc = enqueuePublisherAdvertise(node, pubidx); + if (rc == -1) + return -1; + + return pubidx; +} + +int cRosNodeRegisterServiceProvider(CrosNode *node, const char *service_name, + const char *service_type, const char *md5sum, void *data_context) +{ + PRINT_VVDEBUG ( "cRosNodeRegisterServiceProvider()\n" ); + + if (node->n_service_providers >= CN_MAX_SERVICE_PROVIDERS) + { + PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't register a new service provider: \ + reached the maximum number of service providers\n"); + return -1; + } + + char *srv_service_name = cRosNamespaceBuild(node, service_name); + char *srv_service_type = ( char * ) malloc ( ( strlen ( service_type ) + 1 ) * sizeof ( char ) ); + char *srv_servicerequest_type = ( char * ) malloc ( ( strlen ( service_type ) + strlen("Request") + 1 ) * sizeof ( char ) ); + char *srv_serviceresponse_type = ( char * ) malloc ( ( strlen ( service_type ) + strlen("Response") + 1 ) * sizeof ( char ) ); + char *srv_md5sum = ( char * ) malloc ( ( strlen ( md5sum ) + 1 ) * sizeof ( char ) ); + + if ( srv_service_name == NULL || srv_service_type == NULL || + srv_md5sum == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't allocate memory\n" ); + return -1; + } + + strncpy ( srv_service_type, service_type, strlen ( service_type ) + 1 ); + strncpy ( srv_servicerequest_type, service_type, strlen ( service_type ) + 1 ); + strncat ( srv_servicerequest_type, "Request", strlen("Request") + 1 ); + strncpy ( srv_serviceresponse_type, service_type, strlen ( service_type ) + 1 ); + strncat ( srv_serviceresponse_type, "Response", strlen("Response") +1 ); + strncpy ( srv_md5sum, md5sum, strlen(md5sum) + 1 ); + + PRINT_INFO ( "Registering service provider %s type %s \n", srv_service_name, srv_service_type); + + int serviceidx = -1; + int it = 0; + for (; it < CN_MAX_SERVICE_PROVIDERS; it++) + { + if (node->service_providers[it].service_name == NULL) + { + serviceidx = it; + break; + } + } + + ServiceProviderNode *service = &(node->service_providers[serviceidx]); + + service->service_name = srv_service_name; + service->service_type = srv_service_type; + service->servicerequest_type = srv_servicerequest_type; + service->serviceresponse_type = srv_serviceresponse_type; + service->md5sum = srv_md5sum; + service->context = data_context; + + node->n_service_providers++; + + int rc = enqueueServiceAdvertise(node, serviceidx); + if (rc == -1) + return -1; + + return serviceidx; +} + +int cRosNodeRecruitTcprosClientProc(CrosNode *node, int subidx) +{ + int ret; // Return value: -1 on error, or the recruited proc index on success + + SubscriberNode *sub; + TcprosProcess *client_proc; + + // Look for a free Tcpros client proc + int clientidx; + client_proc=NULL; + sub = &node->subs[subidx]; + ret=-1; // Default return value (No free Tcpros client proc is available) + for(clientidx=0;clientidxtcpros_client_proc[clientidx]; + if(client_proc->topic_idx == -1) // A free Tcpros client proc has been found + { + client_proc->topic_idx = subidx; + client_proc->tcp_nodelay = (unsigned char)sub->tcp_nodelay; + ret=clientidx; // Exit loop + } + } + return ret; +} + +int cRosNodeFindFirstTcprosClientProc(CrosNode *node, int subidx, const char *tcpros_hostname, int tcpros_port) +{ + int ret; // Return value: -1 on error, or the found proc index on success + int clientidx; + + // Look for the first Tcpros client proc that was recruited for subidx subscriber and a specific publisher host and port + ret=-1; + for(clientidx=0;clientidxtcpros_client_proc[clientidx]; + if((subidx == -1 || cur_cli->topic_idx == subidx) && + (tcpros_port == -1 || cur_cli->sub_tcpros_port == tcpros_port) && + (tcpros_hostname == NULL || strcmp(cur_cli->sub_tcpros_host,tcpros_hostname)==0) + ) + ret=clientidx; + } + return ret; +} + +int cRosNodeRegisterSubscriber(CrosNode *node, const char *message_definition, const char *topic_name, + const char *topic_type, const char *md5sum, void *data_context, int tcp_nodelay) +{ + PRINT_VVDEBUG ( "cRosNodeRegisterSubscriber()\n" ); + + if (node->n_subs >= CN_MAX_SUBSCRIBED_TOPICS) + { + PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't register a new subscriber: \ + reached the maximum number of published topics\n"); + return -1; + } + + char *pub_message_definition = ( char * ) malloc ( ( strlen ( message_definition ) + 1 ) * sizeof ( char ) ); + char *pub_topic_name = cRosNamespaceBuild(node, topic_name); + char *pub_topic_type = ( char * ) malloc ( ( strlen ( topic_type ) + 1 ) * sizeof ( char ) ); + char *pub_md5sum = ( char * ) malloc ( ( strlen ( md5sum ) + 1 ) * sizeof ( char ) ); + + if ( pub_topic_name == NULL || pub_topic_type == NULL ) + { + PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n" ); + return -1; + } + + strcpy ( pub_message_definition, message_definition ); + strcpy ( pub_topic_name, topic_name ); + strcpy ( pub_topic_type, topic_type ); + strcpy ( pub_md5sum, md5sum ); + + PRINT_INFO ( "Subscribing to topic %s type %s \n", pub_topic_name, pub_topic_type ); + + int subidx = node->n_subs; + int it = 0; + for (; it < CN_MAX_SUBSCRIBED_TOPICS; it++) + { + if (node->subs[it].topic_name == NULL) + { + subidx = it; + break; + } + } + + SubscriberNode *sub = &node->subs[subidx]; + sub->message_definition = pub_message_definition; + sub->topic_name = pub_topic_name; + sub->topic_type = pub_topic_type; + sub->md5sum = pub_md5sum; + sub->context = data_context; + sub->tcp_nodelay = (unsigned char)tcp_nodelay; + sub->msg_queue_overflow = 0; + cRosMessageQueueClear(&sub->msg_queue); + + node->n_subs++; + + int rc = enqueueSubscriberAdvertise(node, subidx); + if (rc == -1) + return -1; + + return subidx; +} + +int cRosNodeRegisterServiceCaller(CrosNode *node, const char *message_definition, const char *service_name, + const char *service_type, const char *md5sum, int loop_period, + void *data_context, int persistent, int tcp_nodelay) +{ + PRINT_VVDEBUG ( "cRosNodeRegisterServiceCaller()\n" ); + + if (node->n_service_callers >= CN_MAX_SERVICE_CALLERS) + { + PRINT_ERROR ( "cRosNodeRegisterServiceCaller() : Can't register a new service caller: \ + reached the maximum number of service callers\n"); + return -1; + } + + char *srv_message_definition = ( char * ) malloc ( ( strlen ( message_definition ) + 1 ) * sizeof ( char ) ); + char *srv_service_name = cRosNamespaceBuild(node, service_name); + char *srv_service_type = ( char * ) malloc ( ( strlen ( service_type ) + 1 ) * sizeof ( char ) ); + char *srv_servicerequest_type = ( char * ) malloc ( ( strlen ( service_type ) + strlen("Request") + 1 ) * sizeof ( char ) ); + char *srv_serviceresponse_type = ( char * ) malloc ( ( strlen ( service_type ) + strlen("Response") + 1 ) * sizeof ( char ) ); + char *srv_md5sum = ( char * ) malloc ( ( strlen ( md5sum ) + 1 ) * sizeof ( char ) ); + + if ( srv_service_name == NULL || srv_service_type == NULL || + srv_md5sum == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterServiceCaller() : Can't allocate memory\n" ); + return -1; + } + + strcpy ( srv_message_definition, message_definition ); + strncpy ( srv_service_type, service_type, strlen ( service_type ) + 1 ); + strncpy ( srv_servicerequest_type, service_type, strlen ( service_type ) + 1 ); + strncat ( srv_servicerequest_type, "Request", strlen("Request") + 1 ); + strncpy ( srv_serviceresponse_type, service_type, strlen ( service_type ) + 1 ); + strncat ( srv_serviceresponse_type, "Response", strlen("Response") +1 ); + strncpy ( srv_md5sum, md5sum, strlen(md5sum) + 1 ); + + PRINT_INFO ( "Starting service caller %s type %s \n", srv_service_name, srv_service_type); + + int serviceidx = -1; + int it = 0; + for (; it < CN_MAX_SERVICE_CALLERS; it++) + { + if (node->service_callers[it].service_name == NULL) + { + serviceidx = it; + break; + } + } + + ServiceCallerNode *service = &(node->service_callers[serviceidx]); + service->message_definition = srv_message_definition; + service->service_name = srv_service_name; + service->service_type = srv_service_type; + service->servicerequest_type = srv_servicerequest_type; + service->serviceresponse_type = srv_serviceresponse_type; + service->md5sum = srv_md5sum; + service->context = data_context; + service->loop_period = loop_period; + service->wake_up_time = 0; // cRosClockGetTimeMs() + 10; + service->persistent = (unsigned char)persistent; + service->tcp_nodelay = (unsigned char)tcp_nodelay; + + int clientidx = serviceidx; // node->service_callers[0] is assigned node->rpcros_client_proc[0] and so on + service->rpcros_id = clientidx; + + TcprosProcess *client_proc = &node->rpcros_client_proc[clientidx]; + client_proc->service_idx = serviceidx; + client_proc->persistent = (unsigned char)persistent; + client_proc->tcp_nodelay = (unsigned char)tcp_nodelay; + + node->n_service_callers++; + + int rc = enqueueServiceLookup(node, serviceidx); + if (rc == -1) + return -1; + + return serviceidx; +} + + +int cRosNodeUnregisterSubscriber(CrosNode *node, int subidx) +{ + int client_tcpros_ind, client_xmlrpc_ind; + if (subidx < 0 || subidx >= CN_MAX_SUBSCRIBED_TOPICS) + return -1; + + SubscriberNode *sub = &node->subs[subidx]; + if (sub->topic_name == NULL) + return -1; + + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeUnRegisterSubscriber() : Can't allocate memory\n"); + return -1; + } + + while((client_tcpros_ind=cRosNodeFindFirstTcprosClientProc(node, subidx, NULL, -1)) != -1) + { + TcprosProcess *tcprosProc = &node->tcpros_client_proc[client_tcpros_ind]; + closeTcprosProcess(tcprosProc); + } + + // Check if any xmlrpc_client_proc is working for the subscriber being unregistered and if so, close them + for(client_xmlrpc_ind=0;client_xmlrpc_ind < CN_MAX_XMLRPC_CLIENT_CONNECTIONS;client_xmlrpc_ind++) + { + XmlrpcProcess *xmlrpcProc = &node->xmlrpc_client_proc[client_xmlrpc_ind]; + if(xmlrpcProc->state != XMLRPC_PROCESS_STATE_IDLE && xmlrpcProc->current_call != NULL && + xmlrpcProc->current_call->method == CROS_API_REQUEST_TOPIC && xmlrpcProc->current_call->provider_idx == subidx) + closeXmlrpcProcess(xmlrpcProc); + } + + XmlrpcProcess *coreproc = &node->xmlrpc_client_proc[0]; + if (coreproc->current_call != NULL + && coreproc->current_call->method == CROS_API_REGISTER_SUBSCRIBER + && coreproc->current_call->provider_idx == subidx) + { + // Delist current registration + closeXmlrpcProcess(coreproc); + } + + call->method = CROS_API_UNREGISTER_SUBSCRIBER; + call->provider_idx = subidx; + + xmlrpcParamVectorPushBackString( &call->params, node->name); + xmlrpcParamVectorPushBackString( &call->params, sub->topic_name ); + char node_uri[256]; + snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); + xmlrpcParamVectorPushBackString( &call->params, node_uri ); + + // NB: Node release is done in handleApiCallAttempt() + + if(enqueueMasterApiCallInternal(node, call) < 0) + return -1; + else + return 0; +} + +int cRosNodeUnregisterPublisher(CrosNode *node, int pubidx) +{ + int list_elem; + if (pubidx < 0 || pubidx >= CN_MAX_PUBLISHED_TOPICS) + return -1; + + PublisherNode *pub = &node->pubs[pubidx]; + if (pub->topic_name == NULL) + return -1; + + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeUnRegisterPublisher() : Can't allocate memory\n"); + return -1; + } + + for(list_elem=0;pub->tcpros_id_list[list_elem]!=-1;list_elem++) + { + TcprosProcess *tcprosProc = &node->tcpros_server_proc[pub->tcpros_id_list[list_elem]]; + closeTcprosProcess(tcprosProc); + } + + XmlrpcProcess *coreproc = &node->xmlrpc_client_proc[0]; + if (coreproc->current_call != NULL + && coreproc->current_call->method == CROS_API_REGISTER_PUBLISHER + && coreproc->current_call->provider_idx == pubidx) + { + // Delist current registration + closeXmlrpcProcess(coreproc); + } + + call->method = CROS_API_UNREGISTER_PUBLISHER; + call->provider_idx = pubidx; + + xmlrpcParamVectorPushBackString( &call->params, node->name); + xmlrpcParamVectorPushBackString( &call->params, pub->topic_name ); + char node_uri[256]; + snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); + xmlrpcParamVectorPushBackString( &call->params, node_uri ); + + // NB: Node release is done in handleApiCallAttempt() + + if(enqueueMasterApiCallInternal(node, call) < 0) + return -1; + else + return 0; +} + +int cRosNodeUnregisterServiceProvider(CrosNode *node, int serviceidx) +{ + if (serviceidx < 0 || serviceidx >= CN_MAX_SERVICE_PROVIDERS) + return -1; + + ServiceProviderNode *svc = &node->service_providers[serviceidx]; + if (svc->service_name == NULL) + return -1; + + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeUnregisterServiceProvider() : Can't allocate memory\n"); + return -1; + } + + XmlrpcProcess *coreproc = &node->xmlrpc_client_proc[0]; + if (coreproc->current_call != NULL + && coreproc->current_call->method == CROS_API_REGISTER_SERVICE + && coreproc->current_call->provider_idx == serviceidx) + { + // Delist current registration + closeXmlrpcProcess(coreproc); + } + + call->method = CROS_API_UNREGISTER_SERVICE; + call->provider_idx = serviceidx; + // 3 parameters are expected for this method + xmlrpcParamVectorPushBackString( &call->params, node->name); + xmlrpcParamVectorPushBackString( &call->params, svc->service_name); + char uri[256]; + snprintf( uri, 256, "rosrpc://%s:%d/", node->host, node->rpcros_port); + xmlrpcParamVectorPushBackString( &call->params, uri ); +// snprintf( uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); +// xmlrpcParamVectorPushBackString( &call->params, uri ); + + // NB: Node release is done in handleApiCallAttempt() + + if(enqueueMasterApiCallInternal(node, call) < 0) + return -1; + else + return 0; +} + +cRosErrCodePack cRosApiSubscribeParam(CrosNode *node, const char *key, NodeStatusApiCallback callback, void *context, int *paramsubidx_ptr) +{ + PRINT_VVDEBUG ( "cRosApiSubscribeParam()\n" ); + PRINT_INFO ( "Subscribing to parameter %s\n", key); + + if (node->n_paramsubs >= CN_MAX_PARAMETER_SUBSCRIPTIONS) + { + PRINT_ERROR ( "cRosApiSubscribeParam() : Can't register a new parameter subscription: \ + reached the maximum number of parameters\n"); + return CROS_MANY_PARAM_ERR; + } + + char *parameter_key = ( char * ) malloc ( ( strlen ( key ) + 1 ) * sizeof ( char ) ); + if (parameter_key == NULL) + { + PRINT_ERROR ( "cRosApiSubscribeParam() : Can't allocate memory\n" ); + return CROS_MEM_ALLOC_ERR; + } + + strcpy (parameter_key, key); + + int paramsubidx = -1; // This value should never be used + int it; + for (it = 0; it < CN_MAX_PARAMETER_SUBSCRIPTIONS; it++) + { + if (node->paramsubs[it].parameter_key == NULL) + { + paramsubidx = it; + break; + } + } + + ParameterSubscription *sub = &node->paramsubs[paramsubidx]; + sub->parameter_key = parameter_key; + sub->context = context; + sub->status_api_callback = callback; + + node->n_paramsubs++; + + int callid = enqueueParameterSubscription(node, paramsubidx); + if (callid == -1) + { + free(parameter_key); + sub->parameter_key = NULL; + node->n_paramsubs--; + return CROS_MEM_ALLOC_ERR; + } + + if(paramsubidx_ptr != NULL) + *paramsubidx_ptr = paramsubidx; + + return CROS_SUCCESS_ERR_PACK; +} + +cRosErrCodePack cRosApiUnsubscribeParam(CrosNode *node, int paramsubidx) +{ + int caller_id; + + if (paramsubidx < 0 || paramsubidx >= CN_MAX_PARAMETER_SUBSCRIPTIONS) + return CROS_BAD_PARAM_ERR; + + ParameterSubscription *sub = &node->paramsubs[paramsubidx]; + if (sub->parameter_key == NULL) + return CROS_PARAM_SUB_IND_ERR; + + XmlrpcProcess *coreproc = &node->xmlrpc_client_proc[0]; + if (coreproc->current_call != NULL + && coreproc->current_call->method == CROS_API_SUBSCRIBE_PARAM + && coreproc->current_call->provider_idx == paramsubidx) + { + // Delist current registration + closeXmlrpcProcess(coreproc); + } + + caller_id = enqueueParameterUnsubscription(node, paramsubidx); + + return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; +} + +cRosErrCodePack cRosNodeTriggerPublishersWriting( CrosNode *n, uint64_t cur_time ) +{ + cRosErrCodePack ret_err; + int pub_idx; + + ret_err = CROS_SUCCESS_ERR_PACK; // Default return value: success + // Check whether it is time to send a new topic message and trigger the corresponding TcprosProcesses + for(pub_idx = 0; pub_idx < CN_MAX_PUBLISHED_TOPICS; pub_idx++) + { + PublisherNode *cur_pub = &n->pubs[pub_idx]; + if(cur_pub->topic_name != NULL) // Is this publisher active? + { + if((cur_pub->loop_period >= 0 && cur_pub->wake_up_time <= cur_time) || cRosMessageQueueUsage(&cur_pub->msg_queue) > 0) // Is it time to publish a message (periodic or immediate)? + { + int list_elem, all_procs_ready; + // Check whether all tcpProcess are ready to start writing a new message (or idle) + all_procs_ready = 1; + for(list_elem=0;cur_pub->tcpros_id_list[list_elem]!=-1;list_elem++) + { + TcprosProcess *server_proc = &n->tcpros_server_proc[cur_pub->tcpros_id_list[list_elem]]; + if(server_proc->state != TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) // server_proc->state != TCPROS_PROCESS_STATE_IDLE && + { + all_procs_ready = 0; + break; + } + } + if(all_procs_ready && cur_pub->tcpros_id_list[0]!=-1) // All associated processes are ready to write and there is at least one associated process + { + if(cRosMessageQueueUsage(&cur_pub->msg_queue) == 0) // There is no immediate message waiting, so a periodic message must be sent + cur_pub->wake_up_time = cur_time + cur_pub->loop_period; + + // The next function will store the next message to be sent in cur_pub->context->outgoing + ret_err = cRosNodePublisherCallback(cur_pub->context); // Calls the publisher application-defined callback + + // Make all the waiting processes start writing + for(list_elem=0;cur_pub->tcpros_id_list[list_elem]!=-1;list_elem++) + { + TcprosProcess *server_proc = &n->tcpros_server_proc[cur_pub->tcpros_id_list[list_elem]]; + // if(server_proc->state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) + tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_START_WRITING ); + } + } + } + } + } + return(ret_err); +} + + +cRosErrCodePack cRosNodeTriggerServiceCallersWriting( CrosNode *n, uint64_t cur_time ) +{ + cRosErrCodePack ret_err; + int caller_idx; + + ret_err = CROS_SUCCESS_ERR_PACK; // Default return value: success + // Check whether it is time to make a service call, and if so, trigger the corresponding TcprosProcesses + for(caller_idx = 0; caller_idx < CN_MAX_SERVICE_CALLERS; caller_idx++) + { + ServiceCallerNode *cur_caller = &n->service_callers[caller_idx]; + if(cur_caller->service_name != NULL) // Is this caller active? + { + if((cur_caller->loop_period >= 0 && cur_caller->wake_up_time <= cur_time) || cRosMessageQueueUsage(&cur_caller->msg_queue) == 1) // Is it time to make a call (periodic or immediate)? + { + // Check whether the corresponding process is ready to start making a new call + TcprosProcess *caller_proc = &n->rpcros_client_proc[cur_caller->rpcros_id]; + if(caller_proc->state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) // caller_proc->state == TCPROS_PROCESS_STATE_IDLE || + { + if(cRosMessageQueueUsage(&cur_caller->msg_queue) == 0) // There is no immediate call waiting, so a periodic call will be made + cur_caller->wake_up_time = cur_time + cur_caller->loop_period; + + // Now the service-call parameters are stored in cur_caller->context->outgoing + ret_err = cRosNodeServiceCallerCallback( 0, cur_caller->context); // calls the service-caller application-defined callback function to generate the service request + + //if(caller_proc->state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) + { + tcprosProcessChangeState( caller_proc, TCPROS_PROCESS_STATE_START_WRITING ); + } + } + } + } + } + return(ret_err); +} + +#define NODE_STATUS_STR_MAX_LEN (CN_MAX_XMLRPC_CLIENT_CONNECTIONS+CN_MAX_XMLRPC_SERVER_CONNECTIONS+CN_MAX_TCPROS_CLIENT_CONNECTIONS+CN_MAX_TCPROS_SERVER_CONNECTIONS+CN_MAX_RPCROS_CLIENT_CONNECTIONS+CN_MAX_RPCROS_SERVER_CONNECTIONS+6*3+3*4+2) +void printNodeProcState( CrosNode *n ) +{ + static char prev_stat_str[NODE_STATUS_STR_MAX_LEN]="\0"; + char stat_str[NODE_STATUS_STR_MAX_LEN]; + int i; + + sprintf(stat_str, "XL%X XC",n->xmlrpc_listner_proc.state); + for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++ ) + sprintf(stat_str+strlen(stat_str), "%X",n->xmlrpc_client_proc[i].state); + + sprintf(stat_str+strlen(stat_str), " XS"); + for( i = 0; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++ ) + sprintf(stat_str+strlen(stat_str), "%X",n->xmlrpc_server_proc[i].state); + + sprintf(stat_str+strlen(stat_str), " TL%X TC",n->tcpros_listner_proc.state); + for(i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++ ) + sprintf(stat_str+strlen(stat_str), "%X",n->tcpros_client_proc[i].state); + + sprintf(stat_str+strlen(stat_str), " TS"); + for( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++ ) + sprintf(stat_str+strlen(stat_str), "%X",n->tcpros_server_proc[i].state); + + sprintf(stat_str+strlen(stat_str), " RL%X RC",n->rpcros_listner_proc.state); + for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++ ) + sprintf(stat_str+strlen(stat_str), "%X",n->rpcros_client_proc[i].state); + + sprintf(stat_str+strlen(stat_str), " RS"); + for( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++ ) + sprintf(stat_str+strlen(stat_str), "%X",n->rpcros_server_proc[i].state); + + if (strcmp(prev_stat_str, stat_str) != 0) // If the node status has changed: + { + // Print a compact string indicating the state of all the node processed for debug purposes + PRINT_DEBUG("Node status: %s\n", stat_str); + strcpy(prev_stat_str, stat_str); + } +} + +uint64_t cRosNodeCalculateSelectTimeout(CrosNode *n, uint64_t max_timeout) +{ + uint64_t wakeup_timeout, select_timeout, cur_time; + int pub_idx, svc_idx; + + select_timeout = max_timeout; + cur_time = cRosClockGetTimeMs(); + + if( n->xmlrpc_master_wake_up_time > cur_time ) + wakeup_timeout = n->xmlrpc_master_wake_up_time - cur_time; + else + wakeup_timeout = 0; + + if( wakeup_timeout < select_timeout ) + select_timeout = wakeup_timeout; + + for (pub_idx = 0;pub_idx < CN_MAX_PUBLISHED_TOPICS;pub_idx++) // < n_pubs? + { + PublisherNode *cur_pub = &n->pubs[pub_idx]; + if(cur_pub->topic_name != NULL && cur_pub->loop_period >= 0) // Is this publisher active and automatically publishing messages? + { + if( cur_pub->wake_up_time > cur_time ) // Is not it time to publish a new message yet? + wakeup_timeout = cur_pub->wake_up_time - cur_time; + else + wakeup_timeout = 0; + + if( wakeup_timeout < select_timeout ) + select_timeout = wakeup_timeout; + } + } + + for (svc_idx = 0;svc_idx < CN_MAX_SERVICE_CALLERS;svc_idx++) // service_callers[svc_idx]; + if(cur_svc_caller->service_name != NULL && cur_svc_caller->loop_period >= 0) // Is this service caller active and automatically publishing messages? + { + if( cur_svc_caller->wake_up_time > cur_time ) // Is not it time to make a service call yet? + wakeup_timeout = cur_svc_caller->wake_up_time - cur_time; + else + wakeup_timeout = 0; + + if( wakeup_timeout < select_timeout ) + { + //printf("[i:%i cur time: %lu wake:%lu new timeout: %lu prev timeout:%lu]", i, cur_time, cur_svc_caller->wake_up_time, wakeup_timeout, select_timeout); + select_timeout = wakeup_timeout; + } + } + } + + return(select_timeout); +} + +cRosErrCodePack cRosNodeDoEventsLoop( CrosNode *n, uint64_t max_timeout ) +{ + cRosErrCodePack ret_err, new_errors; + uint64_t cur_time, select_timeout; + int nfds = -1; + fd_set r_fds, w_fds, err_fds; + int i; + + PRINT_VVDEBUG ( "cRosNodeDoEventsLoop ()\n" ); + + if(n == NULL) + return CROS_BAD_PARAM_ERR; + + #if CROS_DEBUG_LEVEL >= 2 + PRINT_DEBUG("*"); + FLUSH_PRINT(); + printNodeProcState( n ); + #endif + + cur_time = cRosClockGetTimeMs(); + + ret_err = cRosNodeTriggerPublishersWriting( n, cur_time ); + + new_errors = cRosNodeTriggerServiceCallersWriting( n, cur_time ); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + + FD_ZERO( &r_fds ); + FD_ZERO( &w_fds ); + FD_ZERO( &err_fds ); + + int xmlrpc_listner_fd = tcpIpSocketGetFD( &(n->xmlrpc_listner_proc.socket) ); + int tcpros_listner_fd = tcpIpSocketGetFD( &(n->tcpros_listner_proc.socket) ); + int rpcros_listner_fd = tcpIpSocketGetFD( &(n->rpcros_listner_proc.socket) ); + + XmlrpcProcess *coreproc = &n->xmlrpc_client_proc[0]; + if (coreproc->state == XMLRPC_PROCESS_STATE_IDLE && !isQueueEmpty(&n->master_api_queue)) + { + RosApiCall *call = dequeueApiCall(&n->master_api_queue); + coreproc->current_call = call; + xmlrpcProcessChangeState( coreproc, XMLRPC_PROCESS_STATE_CONNECTING ); + } + + size_t idle_client_count; + int idle_clients[CN_MAX_XMLRPC_CLIENT_CONNECTIONS]; + getIdleXmplrpcClients(n, idle_clients, &idle_client_count); + + int next_idle_client_idx = 0; + while (!isQueueEmpty(&n->slave_api_queue) && next_idle_client_idx != idle_client_count) + { + int idle_client_idx = idle_clients[next_idle_client_idx]; + RosApiCall *call = dequeueApiCall(&n->slave_api_queue); + + XmlrpcProcess *proc = &n->xmlrpc_client_proc[idle_client_idx]; + proc->current_call = call; + xmlrpcProcessChangeState( proc, XMLRPC_PROCESS_STATE_CONNECTING ); + + next_idle_client_idx++; + } + + /* If active (not idle state), add to the tcpIpSocketSelect() the XMLRPC clients */ + for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++) + { + fd_set *fdset = NULL; + if( n->xmlrpc_client_proc[i].state == XMLRPC_PROCESS_STATE_CONNECTING ) + { + new_errors = xmlrpcClientConnect(n, i); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + fdset = &w_fds; // tcpIpSocketSelect() will acknowledge the socket connection completion in the file descriptors checked for writing + } + else if( n->xmlrpc_client_proc[i].state == XMLRPC_PROCESS_STATE_WRITING ) + fdset = &w_fds; + else if( n->xmlrpc_client_proc[i].state == XMLRPC_PROCESS_STATE_READING ) + fdset = &r_fds; + + if (fdset != NULL) + { + int xmlrpc_client_fd = tcpIpSocketGetFD( &(n->xmlrpc_client_proc[i].socket) ); + if(xmlrpc_client_fd != -1) + { + if( xmlrpc_client_fd > nfds ) + nfds = xmlrpc_client_fd; + + FD_SET( xmlrpc_client_fd, fdset); + FD_SET( xmlrpc_client_fd, &err_fds); + } + } + } + + //printf("FD_SET COUNT. R: %d W: %d\n", r_count, w_count); + + /* Add to the tcpIpSocketSelect() the active XMLRPC servers */ + int next_xmlrpc_server_i = -1; + for( i = 0; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++ ) + { + int server_fd = tcpIpSocketGetFD( &(n->xmlrpc_server_proc[i].socket) ); + + if( next_xmlrpc_server_i < 0 && + n->xmlrpc_server_proc[i].state == XMLRPC_PROCESS_STATE_IDLE ) + { + next_xmlrpc_server_i = i; + } + else if( n->xmlrpc_server_proc[i].state == XMLRPC_PROCESS_STATE_READING ) + { + FD_SET( server_fd, &r_fds); + FD_SET( server_fd, &err_fds); + if( server_fd > nfds ) nfds = server_fd; + } + else if( n->xmlrpc_server_proc[i].state == XMLRPC_PROCESS_STATE_WRITING ) + { + FD_SET( server_fd, &w_fds); + FD_SET( server_fd, &err_fds); + if( server_fd > nfds ) nfds = server_fd; + } + } + + /* If one XMLRPC server is active at least, add to the tcpIpSocketSelect() the listener socket */ + if( next_xmlrpc_server_i >= 0) + { + if(xmlrpc_listner_fd != -1) // If the listener socket is still opened, add it to the tcpIpSocketSelect() file descriptors + { + FD_SET( xmlrpc_listner_fd, &r_fds); + FD_SET( xmlrpc_listner_fd, &err_fds); + if( xmlrpc_listner_fd > nfds ) nfds = xmlrpc_listner_fd; + } + } + + /* + * + * TCPROS PROCESSES tcpIpSocketSelect() MANAGEMENT + * + */ + + /* If active (not idle state), add to the tcpIpSocketSelect() the TCPROS clients */ + int next_tcpros_client_i = -1; // Unused ??? + for(i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++) + { + TcprosProcess *client_proc = &(n->tcpros_client_proc[i]); + int tcpros_client_fd; + + if( next_tcpros_client_i < 0 && + i != 0 && //the zero-index is reserved to the roscore communications + client_proc->state == TCPROS_PROCESS_STATE_IDLE ) + { + next_tcpros_client_i = i; + } + else if(client_proc->state == TCPROS_PROCESS_STATE_CONNECTING) + { + new_errors = tcprosClientConnect(n, i); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + + tcpros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); + FD_SET( tcpros_client_fd, &w_fds); + FD_SET( tcpros_client_fd, &err_fds); + if( tcpros_client_fd > nfds ) nfds = tcpros_client_fd; + } + else if(client_proc->state == TCPROS_PROCESS_STATE_WRITING_HEADER) + { + tcpros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); + FD_SET( tcpros_client_fd, &w_fds); + FD_SET( tcpros_client_fd, &err_fds); + if( tcpros_client_fd > nfds ) nfds = tcpros_client_fd; + } + else if(client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE || + client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER || + client_proc->state == TCPROS_PROCESS_STATE_READING_SIZE || + client_proc->state == TCPROS_PROCESS_STATE_READING) + { + tcpros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); + FD_SET( tcpros_client_fd, &r_fds); + FD_SET( tcpros_client_fd, &err_fds); + if( tcpros_client_fd > nfds ) nfds = tcpros_client_fd; + } + } + + /* Add to the tcpIpSocketSelect() the active TCPROS servers */ + int next_tcpros_server_i = -1; // Index of the first server that is idle (ready to be activated). -1 = no one is idle + for( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++ ) + { + int server_fd = tcpIpSocketGetFD( &(n->tcpros_server_proc[i].socket) ); + + if( next_tcpros_server_i < 0 && + n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_IDLE ) + { + next_tcpros_server_i = i; + } + else if( n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER ) + { + FD_SET( server_fd, &r_fds); + FD_SET( server_fd, &err_fds); + if( server_fd > nfds ) nfds = server_fd; + } + else if( n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_START_WRITING || + n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_WRITING ) + { + FD_SET( server_fd, &w_fds); + FD_SET( server_fd, &err_fds); + if( server_fd > nfds ) nfds = server_fd; + } + else if( n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING ) + { + FD_SET( server_fd, &err_fds); + if( server_fd > nfds ) nfds = server_fd; + } + } + + /* If one TCPROS server is available at least, add to the tcpIpSocketSelect() the listener socket */ + if( next_tcpros_server_i >= 0) + { + if(tcpros_listner_fd != -1) // If the listener socket is still opened + { + FD_SET( tcpros_listner_fd, &r_fds); + FD_SET( tcpros_listner_fd, &err_fds); + if( tcpros_listner_fd > nfds ) nfds = tcpros_listner_fd; + } + } + + + /* + * + * RPCROS PROCESSES tcpIpSocketSelect() MANAGEMENT + * + */ + + /* If active (not idle state), add to the tcpIpSocketSelect() the TCPROS clients */ + for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++) + { + int rpcros_client_fd; + + rpcros_client_fd = tcpIpSocketGetFD( &(n->rpcros_client_proc[i].socket) ); + if(n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_CONNECTING) + { + new_errors = rpcrosClientConnect(n, i); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + + rpcros_client_fd = tcpIpSocketGetFD( &(n->rpcros_client_proc[i].socket) ); // update file descriptor after connecting + FD_SET( rpcros_client_fd, &w_fds); + FD_SET( rpcros_client_fd, &err_fds); + if( rpcros_client_fd > nfds ) nfds = rpcros_client_fd; + } + else if(n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_WRITING_HEADER || + n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_START_WRITING || + n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_WRITING) + { + FD_SET( rpcros_client_fd, &w_fds); + FD_SET( rpcros_client_fd, &err_fds); + if( rpcros_client_fd > nfds ) nfds = rpcros_client_fd; + } + else if(n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE || + n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER || + n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING_SIZE || + n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING) + { + FD_SET( rpcros_client_fd, &r_fds); + FD_SET( rpcros_client_fd, &err_fds); + if( rpcros_client_fd > nfds ) nfds = rpcros_client_fd; + } + else if(n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) + { + FD_SET( rpcros_client_fd, &err_fds); + if( rpcros_client_fd > nfds ) nfds = rpcros_client_fd; + } + } + + + /* Add to the tcpIpSocketSelect() the active RPCROS servers */ + int next_rpcros_server_i = -1; + + for( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++ ) + { + int server_fd = tcpIpSocketGetFD( &(n->rpcros_server_proc[i].socket) ); + + if( next_rpcros_server_i < 0 && + n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_IDLE ) + { + next_rpcros_server_i = i; + } + else if (n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE || + n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER || + n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_SIZE || + n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_READING) + { + FD_SET( server_fd, &r_fds); + FD_SET( server_fd, &err_fds); + if( server_fd > nfds ) nfds = server_fd; + } + else if( n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_WRITING_HEADER || + n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_WRITING ) + { + FD_SET( server_fd, &w_fds); + FD_SET( server_fd, &err_fds); + if( server_fd > nfds ) nfds = server_fd; + } + } + + /* If one RPCROS server is available at least, add to the tcpIpSocketSelect() the listner socket */ + if( next_rpcros_server_i >= 0) + { + if(rpcros_listner_fd != -1) // If the listener socket is still opened + { + FD_SET( rpcros_listner_fd, &r_fds); + FD_SET( rpcros_listner_fd, &err_fds); + if( rpcros_listner_fd > nfds ) nfds = rpcros_listner_fd; + } + } + + if (nfds + 1 == 0) + { + PRINT_VDEBUG("cRosNodeDoEventsLoop() : Warning: tcpIpSocketSelect() is being called with no file descriptors to monitor.\n"); + } + + select_timeout = cRosNodeCalculateSelectTimeout(n, max_timeout); + + // The node waits here until the specified file descriptors become ready for the corresponding I/O operation or the timeout is up + // ------------------------------------------------------------------------------------------------------------------------------ + int n_set = tcpIpSocketSelect(nfds + 1, &r_fds, &w_fds, &err_fds, select_timeout); + + cur_time = cRosClockGetTimeMs(); // Update current time after select() + if (n_set == -1) + { + PRINT_ERROR("cRosNodeDoEventsLoop() : tcpIpSocketSelect() function failed.\n"); + ret_err = CROS_SELECT_FD_ERR; + } + else if( n_set == 0 ) + { + PRINT_VDEBUG ("cRosNodeDoEventsLoop() : tcpIpSocketSelect() finished due to timeout (parameter: %llu ms) or it was interrupted\n", (long long unsigned)select_timeout); + + XmlrpcProcess *rosproc = &n->xmlrpc_client_proc[0]; + if(n->xmlrpc_master_wake_up_time <= cur_time ) // It's time to wakeup, ping master, and maybe look up in master for pending services + { + PRINT_VDEBUG("cRosNodeDoEventsLoop() : It is ime to wake up the Master XML RPC process. Current time: %lu Wake up time: %lu\n", cur_time, n->xmlrpc_master_wake_up_time); + if(rosproc->state == XMLRPC_PROCESS_STATE_IDLE) + { + // Prepare to ping roscore ... + PRINT_VDEBUG("cRosNodeDoEventsLoop() : Sending ping to ROS Master\n"); + + RosApiCall *call = newRosApiCall(); + if (call != NULL) + { + call->method = CROS_API_GET_PID; + int rc = xmlrpcParamVectorPushBackString(&call->params, n->name); + if(rc >= 0) + { + rosproc->message_type = XMLRPC_MESSAGE_REQUEST; + generateXmlrpcMessage( n->roscore_host, n->roscore_port, rosproc->message_type, + getMethodName(call->method), &call->params, &rosproc->message ); + + rosproc->current_call = call; + xmlrpcProcessChangeState(rosproc, XMLRPC_PROCESS_STATE_CONNECTING ); + } + else + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : Can't allocate memory\n"); + ret_err=CROS_MEM_ALLOC_ERR; + } + + // The ROS master does not warn us when a new service is registered, so we have to + // continuously check for the required service + for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++ ) + { + TcprosProcess *client_proc = &(n->rpcros_client_proc[i]); + if( client_proc->state == TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING) + { + tcprosProcessChangeState(client_proc, TCPROS_PROCESS_STATE_IDLE); + enqueueServiceLookup(n, client_proc->service_idx); + } + } + n->xmlrpc_master_wake_up_time = cur_time + CN_PING_LOOP_PERIOD; // The process completed doing what it should, so wake up again CN_PING_LOOP_PERIOD milliseconds later + } + else + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : Can't allocate memory\n"); + ret_err=CROS_MEM_ALLOC_ERR; + } + } + else + n->xmlrpc_master_wake_up_time = cur_time + CN_PING_LOOP_PERIOD/50; // The process is busy, so try to wake up again soon (CN_PING_LOOP_PERIOD/50 milliseconds later) to do what is pending + } + if( rosproc->state != XMLRPC_PROCESS_STATE_IDLE && cRosClockGetTimeMs() - rosproc->last_change_time > CN_IO_TIMEOUT ) // last_change_time is updated when changing process state + { + // Timeout between I/O operations... close the socket and re-advertise + PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : XMLRPC client I/O timeout\n"); + handleXmlrpcClientError( n, 0 ); + } + + + for( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS && ret_err==CROS_SUCCESS_ERR_PACK; i++ ) + { + if( (n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER || + n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_WRITING ) && + cur_time - n->tcpros_server_proc[i].last_change_time > CN_IO_TIMEOUT ) + { + // Timeout between I/O operations + PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : TCPROS server I/O timeout\n"); + handleTcprosServerError( n, i ); + } + } + + for( i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS && ret_err==CROS_SUCCESS_ERR_PACK; i++ ) + { + if( (n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER || // Add more states to the condition??? + n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_WRITING ) && + cur_time - n->rpcros_client_proc[i].last_change_time > CN_IO_TIMEOUT ) + { + // Timeout between I/O operations + PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : TCPROS server I/O timeout\n"); + handleTcprosServerError( n, i ); + } + } + } + else + { + PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : tcpIpSocketSelect() finished with num. fd set: %i (timeout parameter was: %llu ms)\n", n_set, (long long unsigned)select_timeout); + for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++ ) + { + XmlrpcProcess *client_proc; + int xmlrpc_client_fd; + + client_proc = &n->xmlrpc_client_proc[i]; + xmlrpc_client_fd = tcpIpSocketGetFD( &client_proc->socket ); + if( client_proc->state != XMLRPC_PROCESS_STATE_IDLE && FD_ISSET(xmlrpc_client_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : XMLRPC client socket error\n" ); + handleXmlrpcClientError( n, i ); + } + /* Check what is the socket unblocked by tcpIpSocketSelect(), and start the requested operations */ + else if( ( client_proc->state == XMLRPC_PROCESS_STATE_CONNECTING && FD_ISSET(xmlrpc_client_fd, &w_fds) ) || + ( client_proc->state == XMLRPC_PROCESS_STATE_WRITING && FD_ISSET(xmlrpc_client_fd, &w_fds) ) || + ( client_proc->state == XMLRPC_PROCESS_STATE_READING && FD_ISSET(xmlrpc_client_fd, &r_fds) ) ) + { + new_errors = doWithXmlrpcClientSocket( n, i );; + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + } + } + + if ( next_xmlrpc_server_i >= 0 && xmlrpc_listner_fd != -1) // Check that there is an available free (idle) xmlrpx process and that the listener socket is still open + { + if( FD_ISSET( xmlrpc_listner_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : XMLRPC server listener-socket error\n" ); + } + else if( FD_ISSET( xmlrpc_listner_fd, &r_fds) ) + { + PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : XMLRPC server listener-socket ready\n" ); + if( tcpIpSocketAccept( &(n->xmlrpc_listner_proc.socket), + &(n->xmlrpc_server_proc[next_xmlrpc_server_i].socket) ) == TCPIPSOCKET_DONE && + tcpIpSocketSetReuse( &(n->xmlrpc_server_proc[next_xmlrpc_server_i].socket) ) && + tcpIpSocketSetNonBlocking( &(n->xmlrpc_server_proc[next_xmlrpc_server_i].socket ) ) ) + + xmlrpcProcessChangeState( &(n->xmlrpc_server_proc[next_xmlrpc_server_i]), XMLRPC_PROCESS_STATE_READING ); + } + } + + for( i = 0; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++ ) + { + XmlrpcProcess *server_proc; + int server_fd; + + server_proc = &n->xmlrpc_server_proc[i]; + server_fd = tcpIpSocketGetFD( &server_proc->socket ); + if( server_proc->state != XMLRPC_PROCESS_STATE_IDLE && FD_ISSET(server_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : XMLRPC server socket error\n" ); + tcpIpSocketClose( &(n->xmlrpc_server_proc[i].socket) ); + xmlrpcProcessChangeState( &(n->xmlrpc_server_proc[next_xmlrpc_server_i]), XMLRPC_PROCESS_STATE_IDLE ); // maybe server_proc instead of &(n->xmlrpc_server_proc[next_xmlrpc_server_i]) ??? + } + else if( ( server_proc->state == XMLRPC_PROCESS_STATE_WRITING && FD_ISSET(server_fd, &w_fds) ) || + ( server_proc->state == XMLRPC_PROCESS_STATE_READING && FD_ISSET(server_fd, &r_fds) ) ) + { + new_errors = doWithXmlrpcServerSocket( n, i ); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + } + } + + for(i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++ ) + { + TcprosProcess *client_proc = &(n->tcpros_client_proc[i]); + int tcpros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); + + if( client_proc->state != TCPROS_PROCESS_STATE_IDLE && FD_ISSET(tcpros_client_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : XMLRPC client socket error\n" ); + handleTcprosClientError( n, i ); + } + + if( (client_proc->state == TCPROS_PROCESS_STATE_CONNECTING && FD_ISSET(tcpros_client_fd, &w_fds) ) || // tcpIpSocketSelect() indicates connection completion through write-fd + ( client_proc->state == TCPROS_PROCESS_STATE_WRITING_HEADER && FD_ISSET(tcpros_client_fd, &w_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_READING_SIZE && FD_ISSET(tcpros_client_fd, &r_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_READING && FD_ISSET(tcpros_client_fd, &r_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE && FD_ISSET(tcpros_client_fd, &r_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER && FD_ISSET(tcpros_client_fd, &r_fds) ) ) + { + new_errors = doWithTcprosClientSocket( n, i ); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + } + } + + if ( next_tcpros_server_i >= 0 && tcpros_listner_fd != -1) + { + if( FD_ISSET( tcpros_listner_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : TCPROS listener-socket error\n" ); + } + else if( FD_ISSET( tcpros_listner_fd, &r_fds) ) + { + PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : TCPROS listener ready\n" ); + if( tcpIpSocketAccept( &(n->tcpros_listner_proc.socket), + &(n->tcpros_server_proc[next_tcpros_server_i].socket) ) == TCPIPSOCKET_DONE && + tcpIpSocketSetReuse( &(n->tcpros_server_proc[next_tcpros_server_i].socket) ) && + tcpIpSocketSetNonBlocking( &(n->tcpros_server_proc[next_tcpros_server_i].socket ) ) && + tcpIpSocketSetKeepAlive( &(n->tcpros_server_proc[next_tcpros_server_i].socket ), 60, 10, 9 ) ) + { + tcprosProcessChangeState( &(n->tcpros_server_proc[next_tcpros_server_i]), TCPROS_PROCESS_STATE_READING_HEADER ); // A TCPROS process has been activated to attend the connection + } + } + } + + for( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++ ) + { + TcprosProcess *server_proc; + int server_fd; + + server_proc = &n->tcpros_server_proc[i]; + server_fd = tcpIpSocketGetFD( &server_proc->socket ); + if( server_proc->state != TCPROS_PROCESS_STATE_IDLE && FD_ISSET(server_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : TCPROS server socket error\n" ); + tcpIpSocketClose( &(server_proc->socket) ); + tcprosProcessChangeState( &(n->tcpros_server_proc[next_tcpros_server_i]), TCPROS_PROCESS_STATE_IDLE ); + } + else if( ( server_proc->state == TCPROS_PROCESS_STATE_READING_HEADER && FD_ISSET(server_fd, &r_fds) ) || + ( server_proc->state == TCPROS_PROCESS_STATE_START_WRITING && FD_ISSET(server_fd, &w_fds) ) || + ( server_proc->state == TCPROS_PROCESS_STATE_WRITING && FD_ISSET(server_fd, &w_fds) ) ) + { + new_errors = doWithTcprosServerSocket( n, i ); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + } + } + + for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++ ) + { + TcprosProcess *client_proc = &(n->rpcros_client_proc[i]); + int rpcros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); + + if( client_proc->state != TCPROS_PROCESS_STATE_IDLE && FD_ISSET(rpcros_client_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : RPCROS client socket error\n" ); + handleRpcrosClientError( n, i ); + } + + if( ( client_proc->state == TCPROS_PROCESS_STATE_CONNECTING && FD_ISSET(rpcros_client_fd, &w_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_WRITING_HEADER && FD_ISSET(rpcros_client_fd, &w_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_READING_SIZE && FD_ISSET(rpcros_client_fd, &r_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_READING && FD_ISSET(rpcros_client_fd, &r_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE && FD_ISSET(rpcros_client_fd, &r_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER && FD_ISSET(rpcros_client_fd, &r_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_START_WRITING && FD_ISSET(rpcros_client_fd, &w_fds) ) || + ( client_proc->state == TCPROS_PROCESS_STATE_WRITING && FD_ISSET(rpcros_client_fd, &w_fds) ) ) + { + new_errors = doWithRpcrosClientSocket( n, i ); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + } + } + + if ( next_rpcros_server_i >= 0 && rpcros_listner_fd != -1) + { + if( FD_ISSET( rpcros_listner_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : TCPROS listener-socket error\n" ); + } + else if( next_rpcros_server_i >= 0 && FD_ISSET( rpcros_listner_fd, &r_fds) ) + { + PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : TCPROS listener ready\n" ); + if( tcpIpSocketAccept( &(n->rpcros_listner_proc.socket), + &(n->rpcros_server_proc[next_rpcros_server_i].socket) ) == TCPIPSOCKET_DONE && + tcpIpSocketSetReuse( &(n->rpcros_server_proc[next_rpcros_server_i].socket) ) && + tcpIpSocketSetNonBlocking( &(n->rpcros_server_proc[next_rpcros_server_i].socket ) ) && + tcpIpSocketSetKeepAlive( &(n->rpcros_server_proc[next_rpcros_server_i].socket ), 60, 10, 9 ) ) + { + tcprosProcessChangeState( &(n->rpcros_server_proc[next_rpcros_server_i]), TCPROS_PROCESS_STATE_READING_HEADER_SIZE ); + } + } + } + + for( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++ ) + { + TcprosProcess *server_proc = &(n->rpcros_server_proc[i]); + int server_fd = tcpIpSocketGetFD( &(server_proc->socket) ); + + if( server_proc->state != TCPROS_PROCESS_STATE_IDLE && FD_ISSET(server_fd, &err_fds) ) + { + PRINT_ERROR ( "cRosNodeDoEventsLoop() : TCPROS server socket error\n" ); + tcpIpSocketClose( &(server_proc->socket) ); + tcprosProcessChangeState( &(n->rpcros_server_proc[next_rpcros_server_i]), TCPROS_PROCESS_STATE_IDLE ); + } + else if( ( server_proc->state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE && FD_ISSET(server_fd, &r_fds) ) || + ( server_proc->state == TCPROS_PROCESS_STATE_READING_HEADER && FD_ISSET(server_fd, &r_fds) ) || + ( server_proc->state == TCPROS_PROCESS_STATE_READING_SIZE && FD_ISSET(server_fd, &r_fds) ) || + ( server_proc->state == TCPROS_PROCESS_STATE_READING && FD_ISSET(server_fd, &r_fds) ) || + ( server_proc->state == TCPROS_PROCESS_STATE_WRITING_HEADER && FD_ISSET(server_fd, &w_fds) ) || + ( server_proc->state == TCPROS_PROCESS_STATE_WRITING && FD_ISSET(server_fd, &w_fds) ) ) + { + new_errors = doWithRpcrosServerSocket( n, i ); + ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); + } + } + } + return ret_err; +} + +cRosErrCodePack cRosNodeStart( CrosNode *n, unsigned long time_out, unsigned char *exit_flag ) +{ + uint64_t start_time, elapsed_time; + cRosErrCodePack ret_err; + PRINT_VVDEBUG ( "cRosNodeStart ()\n" ); + + start_time = cRosClockGetTimeMs(); + ret_err = CROS_SUCCESS_ERR_PACK; + while(ret_err == CROS_SUCCESS_ERR_PACK && (exit_flag==NULL || !(*exit_flag)) && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) + ret_err = cRosNodeDoEventsLoop( n, (time_out == CROS_INFINITE_TIMEOUT)? UINT64_MAX : time_out-elapsed_time); + + return ret_err; +} + +cRosErrCodePack cRosNodeReceiveTopicMsg( CrosNode *node, int subidx, cRosMessage *msg, unsigned char *buff_overflow, unsigned long time_out ) +{ + cRosErrCodePack ret_err; + SubscriberNode *subs_node; + uint64_t start_time, elapsed_time = 0; // Initialized just to avoid a compiler warning + PRINT_VVDEBUG ( "cRosNodeReceiveTopicMsg ()\n" ); + + if(subidx < 0 || subidx >= CN_MAX_SUBSCRIBED_TOPICS) + return CROS_BAD_PARAM_ERR; + + subs_node = &node->subs[subidx]; + if(subs_node->topic_name == NULL) + return CROS_BAD_PARAM_ERR; + + if(buff_overflow != NULL) + *buff_overflow = subs_node->msg_queue_overflow; + subs_node->msg_queue_overflow = 0; // Reset overflow flag + + start_time = cRosClockGetTimeMs(); + ret_err = CROS_SUCCESS_ERR_PACK; // default return value + // While the buffer is empty and the timeout is not reached wait + while(cRosMessageQueueUsage(&subs_node->msg_queue) == 0 && ret_err == CROS_SUCCESS_ERR_PACK && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) + { + ret_err = cRosNodeDoEventsLoop ( node, time_out - elapsed_time); + } + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + if(cRosMessageQueueUsage(&subs_node->msg_queue) > 0) // If no error and there is at least one message in the queue, get the message + ret_err = (cRosMessageQueueExtract(&subs_node->msg_queue, msg) == 0)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + else + ret_err = CROS_RCV_TOP_TIMEOUT_ERR; + } + return ret_err; +} + +cRosErrCodePack cRosNodeQueueTopicMsg( CrosNode *node, int pubidx, cRosMessage *msg ) +{ + cRosErrCodePack ret_err; + PublisherNode *pub_node; + + pub_node = &node->pubs[pubidx]; + { + if(cRosMessageQueueVacancies(&pub_node->msg_queue) > 0) // If no error and there is space in the queue, put the new message + { + if(cRosMessageQueueAdd(&pub_node->msg_queue, msg) == 0) + ret_err = CROS_SUCCESS_ERR_PACK; + else + ret_err = CROS_MEM_ALLOC_ERR; + } + else + ret_err = CROS_SEND_TOP_TIMEOUT_ERR; + } + return ret_err; +} + +cRosErrCodePack cRosNodeSendTopicMsg( CrosNode *node, int pubidx, cRosMessage *msg, unsigned long time_out ) +{ + cRosErrCodePack ret_err; + PublisherNode *pub_node; + uint64_t start_time, elapsed_time = 0; // Initialized just to avoid a compiler warning + PRINT_VVDEBUG ( "cRosNodeSendTopicMsg ()\n" ); + + if(pubidx < 0 || pubidx >= CN_MAX_PUBLISHED_TOPICS) + return CROS_BAD_PARAM_ERR; + + pub_node = &node->pubs[pubidx]; + if(pub_node->topic_name == NULL) + return CROS_BAD_PARAM_ERR; + + start_time = cRosClockGetTimeMs(); + ret_err = CROS_SUCCESS_ERR_PACK; // default return value + // While the buffer is full and the timeout is not reached wait + while(cRosMessageQueueVacancies(&pub_node->msg_queue) == 0 && ret_err == CROS_SUCCESS_ERR_PACK && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) + { + ret_err = cRosNodeDoEventsLoop ( node, time_out - elapsed_time); + } + + if(ret_err == CROS_SUCCESS_ERR_PACK) + cRosNodeQueueTopicMsg( node, pubidx, msg ); + + return ret_err; +} + + +cRosErrCodePack cRosNodeServiceCall( CrosNode *node, int svcidx, cRosMessage *req_msg, cRosMessage *resp_msg, unsigned long time_out) +{ + cRosErrCodePack ret_err; + ServiceCallerNode *caller_node; + TcprosProcess *svc_client_proc; + uint64_t start_time, elapsed_time = 0; // Initialized just to avoid a compiler warning + PRINT_VVDEBUG ( "cRosNodeServiceCall ()\n" ); + + if(svcidx < 0 || svcidx >= CN_MAX_SERVICE_CALLERS) + return CROS_BAD_PARAM_ERR; + + caller_node = &node->service_callers[svcidx]; + if(caller_node->service_name == NULL) + return CROS_BAD_PARAM_ERR; + + svc_client_proc = &node->rpcros_client_proc[caller_node->rpcros_id]; + + start_time = cRosClockGetTimeMs(); + // Wait until the RPCROS process has finished the current call and the timeout is not reached wait + ret_err = CROS_SUCCESS_ERR_PACK; + while(svc_client_proc->state != TCPROS_PROCESS_STATE_WAIT_FOR_WRITING && ret_err == CROS_SUCCESS_ERR_PACK && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) + { + ret_err = cRosNodeDoEventsLoop ( node, time_out - elapsed_time); + } + + if(ret_err == CROS_SUCCESS_ERR_PACK && svc_client_proc->state != TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) + ret_err = CROS_CALL_INI_TIMEOUT_ERR; + if(ret_err != CROS_SUCCESS_ERR_PACK) + return ret_err; // If an error occurred or the process is not in TCPROS_PROCESS_STATE_WAIT_FOR_WRITING state, exit + + if(cRosMessageQueueUsage(&caller_node->msg_queue) > 0) // Unfinished service call? + { + PRINT_ERROR ( "cRosNodeServiceCall () : The service caller is not ready to make a new call. Overwriting previous state and trying anyway...\n" ); + cRosMessageQueueClear(&caller_node->msg_queue); + } + + if(cRosMessageQueueAdd(&caller_node->msg_queue, req_msg) != 0) // Put service-call request msg in the queue + ret_err = CROS_MEM_ALLOC_ERR; + + // Wait while the buffer is full and the timeout is not reached + while(cRosMessageQueueUsage(&caller_node->msg_queue) < 2 && ret_err == CROS_SUCCESS_ERR_PACK && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) + { + ret_err = cRosNodeDoEventsLoop ( node, time_out - elapsed_time); + } + + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + if(cRosMessageQueueUsage(&caller_node->msg_queue) > 1) // If no error and the response is in the buffer + { + cRosMessageQueueRemove(&caller_node->msg_queue); // Remove request msg from queue + if(resp_msg != NULL) + { + int queue_ret_val; + queue_ret_val = cRosMessageQueueExtract(&caller_node->msg_queue, resp_msg); // Extract response msg from queue + if(queue_ret_val == 0) + ret_err = CROS_SUCCESS_ERR_PACK; + else + ret_err = CROS_MEM_ALLOC_ERR; + } + else // The user is not interested in the service response, just remove the msg from queue + cRosMessageQueueRemove(&caller_node->msg_queue); + } + else + ret_err = CROS_CALL_SVC_TIMEOUT_ERR; + } + + return ret_err; +} + +int enqueueSubscriberAdvertise(CrosNode *node, int subidx) +{ + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n"); + return -1; + } + + call->provider_idx= subidx; + call->method = CROS_API_REGISTER_SUBSCRIBER; + + SubscriberNode *sub = &node->subs[subidx]; + xmlrpcParamVectorPushBackString( &call->params, node->name ); + xmlrpcParamVectorPushBackString( &call->params, sub->topic_name ); + xmlrpcParamVectorPushBackString( &call->params, sub->topic_type ); + char node_uri[256]; + snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); + xmlrpcParamVectorPushBackString( &call->params, node_uri ); + + return enqueueMasterApiCallInternal(node, call); +} + +int enqueuePublisherAdvertise(CrosNode *node, int pubidx) +{ + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterPublisher() : Can't allocate memory\n"); + return -1; + } + + call->provider_idx= pubidx; + call->method = CROS_API_REGISTER_PUBLISHER; + + PublisherNode *publiser = &node->pubs[pubidx]; + xmlrpcParamVectorPushBackString( &call->params, node->name); + xmlrpcParamVectorPushBackString( &call->params, publiser->topic_name ); + xmlrpcParamVectorPushBackString( &call->params, publiser->topic_type ); + char node_uri[256]; + snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); + xmlrpcParamVectorPushBackString( &call->params, node_uri ); + + return enqueueMasterApiCallInternal(node, call); +} + +int enqueueServiceAdvertise(CrosNode *node, int serviceidx) +{ + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't allocate memory\n"); + return -1; + } + + call->provider_idx = serviceidx; + call->method = CROS_API_REGISTER_SERVICE; + + ServiceProviderNode *service = &node->service_providers[serviceidx]; + xmlrpcParamVectorPushBackString( &call->params, node->name ); + xmlrpcParamVectorPushBackString( &call->params, service->service_name ); + char uri[256]; + snprintf( uri, 256, "rosrpc://%s:%d/", node->host, node->rpcros_port); + xmlrpcParamVectorPushBackString( &call->params, uri ); + snprintf( uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); + xmlrpcParamVectorPushBackString( &call->params, uri ); + + return enqueueMasterApiCallInternal(node, call); +} + +int enqueueServiceLookup(CrosNode *node, int serviceidx) +{ + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "enqueueServiceLookup() : Can't allocate memory\n"); + return -1; + } + + call->provider_idx= serviceidx; + call->method = CROS_API_LOOKUP_SERVICE; + + ServiceCallerNode *service = &node->service_callers[serviceidx]; + xmlrpcParamVectorPushBackString( &call->params, node->name ); + xmlrpcParamVectorPushBackString( &call->params, service->service_name ); + + return enqueueMasterApiCallInternal(node, call); +} + +int enqueueParameterSubscription(CrosNode *node, int parameteridx) +{ + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't allocate memory\n"); + return -1; + } + + call->provider_idx = parameteridx; + call->method = CROS_API_SUBSCRIBE_PARAM; + + ParameterSubscription *subscrition = &node->paramsubs[parameteridx]; + xmlrpcParamVectorPushBackString( &call->params, node->name ); + char node_uri[256]; + snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); + xmlrpcParamVectorPushBackString( &call->params, node_uri ); + xmlrpcParamVectorPushBackString( &call->params, subscrition->parameter_key); + + return enqueueMasterApiCallInternal(node, call); +} + +int enqueueParameterUnsubscription(CrosNode *node, int parameteridx) +{ + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't allocate memory\n"); + return -1; + } + + call->method = CROS_API_UNSUBSCRIBE_PARAM; + call->provider_idx = parameteridx; + + ParameterSubscription *subscrition = &node->paramsubs[parameteridx]; + xmlrpcParamVectorPushBackString( &call->params, node->name); + char node_uri[256]; + snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); + xmlrpcParamVectorPushBackString( &call->params, node_uri ); + xmlrpcParamVectorPushBackString( &call->params, subscrition->parameter_key ); + + // NB: Node release is done in handleApiCallAttempt() + return enqueueMasterApiCallInternal(node, call); +} + +int enqueueRequestTopic(CrosNode *node, int subidx, const char *host, int port) +{ + RosApiCall *call = newRosApiCall(); + if (call == NULL) + { + PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n"); + return -1; + } + + call->provider_idx = subidx; + call->method = CROS_API_REQUEST_TOPIC; + + SubscriberNode *sub = &node->subs[subidx]; + + CrosNodeStatusUsr status; + initCrosNodeStatus(&status); + status.provider_idx = subidx; + status.xmlrpc_host = host; + status.xmlrpc_port = port; + cRosNodeStatusCallback(&status, sub->context); // calls the subscriber-status application-defined callback function (if specified when creating the subscriber). Undocumented status callback? + + xmlrpcParamVectorPushBackString(&call->params, node->name ); + xmlrpcParamVectorPushBackString(&call->params, sub->topic_name ); + xmlrpcParamVectorPushBackArray(&call->params); + XmlrpcParam* array_param = xmlrpcParamVectorAt(&call->params,2); + xmlrpcParamArrayPushBackArray(array_param); + xmlrpcParamArrayPushBackString(xmlrpcParamArrayGetParamAt(array_param,0),CROS_TRANSPORT_TCPROS_STRING); + + return enqueueSlaveApiCall(node, call, host, port); +} + +void restartAdversing(CrosNode* n) +{ + int it; + for(it = 0; it < n->n_pubs; it++) + { + if (n->pubs[it].topic_name == NULL) + continue; + + enqueuePublisherAdvertise(n, it); + } + + for(it = 0; it < n->n_subs; it++) + { + if (n->subs[it].topic_name == NULL) + continue; + + enqueueSubscriberAdvertise(n, it); + } + + for(it = 0; it < n->n_service_providers; it++) + { + if (n->service_providers[it].service_name == NULL) + continue; + + enqueueServiceAdvertise(n, it); + } +} + +void initPublisherNode(PublisherNode *pub) +{ + pub->message_definition = NULL; + pub->topic_name = NULL; + pub->topic_type = NULL; + pub->md5sum = NULL; + pub->context = NULL; + pub->tcpros_id_list[0] = -1; // Empty list of TcprosProcess indices + pub->loop_period = -1; // Publication paused + pub->wake_up_time = 0; + cRosMessageQueueInit(&pub->msg_queue); +} + +void initSubscriberNode(SubscriberNode *sub) +{ + sub->message_definition = NULL; + sub->topic_name = NULL; + sub->topic_type = NULL; + sub->md5sum = NULL; + sub->context = NULL; + sub->tcp_nodelay = 0; + sub->msg_queue_overflow = 0; + cRosMessageQueueInit(&sub->msg_queue); +} + +void initServiceProviderNode(ServiceProviderNode *srv_prov) +{ + srv_prov->service_name = NULL; + srv_prov->service_type = NULL; + srv_prov->md5sum = NULL; + srv_prov->context = NULL; + srv_prov->servicerequest_type = NULL; + srv_prov->serviceresponse_type = NULL; +} + +void initServiceCallerNode(ServiceCallerNode *srv_caller) +{ + srv_caller->service_name = NULL; + srv_caller->service_type = NULL; + srv_caller->service_host = NULL; + srv_caller->service_port = -1; + srv_caller->md5sum = NULL; + srv_caller->message_definition = NULL; + srv_caller->rpcros_id = -1; + srv_caller->context = NULL; + srv_caller->servicerequest_type = NULL; + srv_caller->serviceresponse_type = NULL; + srv_caller->persistent = 0; + srv_caller->tcp_nodelay = 0; + srv_caller->loop_period = -1; // Calling paused + srv_caller->wake_up_time = 0; + cRosMessageQueueInit(&srv_caller->msg_queue); +} + +void initParameterSubscrition(ParameterSubscription *subscription) +{ + subscription->parameter_key = NULL; + xmlrpcParamInit(&subscription->parameter_value); + subscription->status_api_callback = NULL; + subscription->context = NULL; +} + +void cRosNodeReleasePublisher(PublisherNode *node) +{ + free(node->message_definition); + free(node->topic_name); + free(node->topic_type); + free(node->md5sum); + cRosMessageQueueRelease(&node->msg_queue); +} + +void cRosNodeReleaseSubscriber(SubscriberNode *node) +{ + free(node->message_definition); + free(node->topic_name); + free(node->topic_type); + free(node->md5sum); + cRosMessageQueueRelease(&node->msg_queue); +} + +void cRosNodeReleaseServiceProvider(ServiceProviderNode *node) +{ + free(node->service_name); + free(node->service_type); + free(node->servicerequest_type); + free(node->serviceresponse_type); + free(node->md5sum); +} + +void cRosNodeReleaseServiceCaller(ServiceCallerNode *node) +{ + free(node->service_name); + free(node->service_type); + free(node->servicerequest_type); + free(node->serviceresponse_type); + free(node->md5sum); + free(node->message_definition); + free(node->service_host); +} + +void initCrosNodeStatus(CrosNodeStatusUsr *status) +{ + status->state = CROS_STATUS_NONE; + status->xmlrpc_port = -1; + status->xmlrpc_host = NULL; + status->provider_idx = -1; + status->parameter_key = NULL; + status->parameter_value = NULL; +} + +void cRosNodeReleaseParameterSubscrition(ParameterSubscription *subscription) +{ + free(subscription->parameter_key); + xmlrpcParamRelease(&subscription->parameter_value); +} + +void getIdleXmplrpcClients(CrosNode *node, int idle_clients[], size_t *idle_client_count) +{ + int client_it = 1; // client_it = 0 is for roscore only + int idle_it = 0; + *idle_client_count = 0; + for(; client_it < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; client_it++) + { + if (node->xmlrpc_client_proc[client_it].state == XMLRPC_PROCESS_STATE_IDLE) + { + idle_clients[idle_it] = client_it; + (*idle_client_count)++; + idle_it++; + } + } +} + +int enqueueMasterApiCall(CrosNode *node, RosApiCall *call) +{ + call->user_call = 1; + return enqueueSlaveApiCallInternal(node, call); +} + +int enqueueSlaveApiCall(CrosNode *node, RosApiCall *call, const char *host, int port) +{ + call->user_call = 1; + if (host == NULL) + { + call->host = node->roscore_host; + call->port = node->roscore_port; + } + else + { + call->host = (char *)malloc(strlen(host) + 1); + if (call->host == NULL) + { + PRINT_ERROR("enqueueSlaveApiCall() : Not enough memory\n"); + return -1; + } + + strcpy(call->host, host); + call->port = port; + } + + return enqueueSlaveApiCallInternal(node, call); +} + +int enqueueMasterApiCallInternal(CrosNode *node, RosApiCall *call) +{ + int callid = (int)node->next_call_id; + call->id = callid; + int rc = enqueueApiCall(&node->master_api_queue, call); + if (rc == -1) + return -1; + + node->next_call_id++; + return callid; +} + +int enqueueSlaveApiCallInternal(CrosNode *node, RosApiCall *call) +{ + int callid = (int)node->next_call_id; + call->id = callid; + int rc = enqueueApiCall(&node->slave_api_queue, call); + if (rc == -1) + return -1; + + node->next_call_id++; + return callid; +} + +void cRosGetMsgFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *topic_type) +{ + snprintf(buffer, bufsize, "%s/%s.msg", node->message_root_path, topic_type); +} + +void getSrvFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *service_type) +{ + snprintf(buffer, bufsize, "%s/%s.srv", node->message_root_path, service_type); +} + +XmlrpcParam * cRosNodeGetParameterValue( CrosNode *node, const char *key) +{ + int it = 0; + for (it = 0 ; it < node->n_paramsubs; it++) + { + if (node->paramsubs[it].parameter_key == NULL) + continue; + + if (strcmp(node->paramsubs[it].parameter_key, key) == 0) + return &node->paramsubs[it].parameter_value; + } + + return NULL; +} + +#define MAX_PORT_OPEN_CHECK_PERIOD 1000 //! Maximum time to wait in ms until the target port is checked again by cRosWaitPortOpen() +cRosErrCodePack cRosWaitPortOpen(const char *host_addr, unsigned short host_port, unsigned long time_out) +{ + uint64_t start_time, elapsed_time; + cRosErrCodePack ret_err; + TcpIpSocketState socket_state; + PRINT_VVDEBUG ( "cRosWaitPortOpen ()\n" ); + + start_time = cRosClockGetTimeMs(); + do + { + elapsed_time = cRosClockGetTimeMs()-start_time; + socket_state = tcpIpSocketCheckPort (host_addr, host_port); + // socket_state can be TCPIPSOCKET_FAILED, TCPIPSOCKET_DONE, TCPIPSOCKET_IN_PROGRESS or TCPIPSOCKET_REFUSED + if(socket_state == TCPIPSOCKET_REFUSED) + { + unsigned int pause_ms; + if(time_out == CROS_INFINITE_TIMEOUT || time_out-elapsed_time > MAX_PORT_OPEN_CHECK_PERIOD) + pause_ms = MAX_PORT_OPEN_CHECK_PERIOD; + else + pause_ms = (unsigned int)(time_out-elapsed_time); +#ifdef _WIN32 + Sleep(pause_ms); +#else + usleep(pause_ms*1000); +#endif + } + } + while(socket_state == TCPIPSOCKET_REFUSED && (time_out == CROS_INFINITE_TIMEOUT || elapsed_time <= time_out)); + // We contnue iterating if the connection is refused and the time is not out + if(socket_state == TCPIPSOCKET_FAILED) + ret_err = CROS_SOCK_OPEN_CONN_ERR; + else if(socket_state == TCPIPSOCKET_REFUSED) + ret_err = CROS_SOCK_OPEN_TIMEOUT_ERR; + else // socket_state == TCPIPSOCKET_DONE or TCPIPSOCKET_IN_PROGRESS + ret_err = CROS_SUCCESS_ERR_PACK; // Port is ready + + return ret_err; +} + +FILE *cRosOutStreamGet(void) +{ + return((Msg_output == NULL)?stdout:Msg_output); +} + +void cRosOutStreamSet(FILE *new_stream) +{ + Msg_output = new_stream; +} + diff --git a/src/hal/components/cros/src/cros_node_api.c b/src/hal/components/cros/src/cros_node_api.c new file mode 100644 index 00000000..1f4973e9 --- /dev/null +++ b/src/hal/components/cros/src/cros_node_api.c @@ -0,0 +1,1275 @@ +#include +#include +#include + +#ifdef _WIN32 +# include +# include +# define strtok_r strtok_s +#else +# include +# include +# include +#endif + +#include "cros_node_api.h" +#include "cros_api.h" +#include "cros_api_internal.h" +#include "cros_defs.h" +#include "xmlrpc_params.h" +#include "tcpip_socket.h" + +int lookup_host (const char *host, char *ip_addr_buff, size_t ip_addr_buff_size) +{ + struct addrinfo hints, *res, *res_it; + int errcode; + int ret; + + memset (&hints, 0, sizeof (hints)); + hints.ai_family = PF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags |= AI_CANONNAME; + + errcode = getaddrinfo (host, NULL, &hints, &res); + if (errcode != 0) + { + PRINT_ERROR ("lookup_host() : getaddrinfo failed and returned the error code:: %i", errcode); + return -1; + } + + ret=0; // Default return value=0: no error + res_it = res; + while (res_it != NULL && ret == 0) + { + void *src_addr = NULL; + switch (res_it->ai_family) + { + case AF_INET: + src_addr = &((struct sockaddr_in *) res_it->ai_addr)->sin_addr; + break; +#ifdef AF_INET6 + case AF_INET6: + src_addr = &((struct sockaddr_in6 *) res_it->ai_addr)->sin6_addr; + break; +#endif + default: + break; + } + if (src_addr != NULL) + { + if(inet_ntop (res_it->ai_family, src_addr, ip_addr_buff, ip_addr_buff_size) != NULL) + { + PRINT_VVDEBUG ("IPv%d address: %s (%s)\n", res_it->ai_family == PF_INET6 ? 6 : 4, + ip_addr_buff, res_it->ai_canonname); + res_it = res_it->ai_next; + } + else + { + int fn_error_code; + + fn_error_code = tcpIpSocketGetError(); + if(fn_error_code == FN_ERROR_INVALID_PARAMETER) + PRINT_ERROR ("lookup_host() : buffer for host address too small"); + else + PRINT_ERROR ("lookup_host() : error executing inet_ntop(). Error code = %i", fn_error_code); + ret = -1; + } + } + else + { + PRINT_ERROR ("lookup_host() : unsupported ai_family from getaddrinfo()"); + ret = -1; + } + } + freeaddrinfo(res); + return ret; +} + +// TODO Improve this +static int checkResponseValue( XmlrpcParamVector *params ) +{ + XmlrpcParam *param = xmlrpcParamVectorAt( params, 0 ); + + // TODO + if( param == NULL || + ( xmlrpcParamGetType( param ) != XMLRPC_PARAM_INT && + xmlrpcParamGetType( param ) != XMLRPC_PARAM_ARRAY ) ) + { + return 0; + } + + int res = 0; + + if( xmlrpcParamGetType( param ) == XMLRPC_PARAM_INT ) + res = param->data.as_int; + else if ( xmlrpcParamGetType( param ) == XMLRPC_PARAM_ARRAY && + param->array_n_elem > 0 ) + { + XmlrpcParam *array_param = param->data.as_array; + if( xmlrpcParamGetType( &( array_param[0]) ) != XMLRPC_PARAM_INT ) + res = 0; + else + res = array_param[0].data.as_int; + } + + return res; +} + +void cRosApiPrepareRequest( CrosNode *n, int client_idx ) +{ + PRINT_VVDEBUG ( "cRosApiPrepareRequest()\n" ); + + XmlrpcProcess *client_proc = &(n->xmlrpc_client_proc[client_idx]); + + client_proc->message_type = XMLRPC_MESSAGE_REQUEST; + + if(client_proc->current_call == NULL) + { + PRINT_ERROR ( "cRosApiPrepareRequest() : Invalid call corresponding to XmlrpcProcess\n" ); + return; + } + + RosApiCall *call = client_proc->current_call; + if(client_idx == 0) //requests managed by the xmlrpc client connected to roscore + { + generateXmlrpcMessage(n->roscore_host, n->roscore_port, XMLRPC_MESSAGE_REQUEST, + getMethodName(call->method), &call->params, &client_proc->message); + } + else // client_idx > 0 + { + generateXmlrpcMessage(call->host, call->port, XMLRPC_MESSAGE_REQUEST, + getMethodName(call->method), &call->params, &client_proc->message); + } +} + + +/* + * XMLRPC Return values are lists in the format: [statusCode, statusMessage, value] + * + * statusCode (int): An integer indicating the completion condition of the method. Current values: + * -1: ERROR: Error on the part of the caller, e.g. an invalid parameter. In general, this means that the master/slave did not attempt to execute the action. + * 0: SUCCESS: Method completed successfully. + * Individual methods may assign additional meaning/semantics to statusCode. + * + * statusMessage (str): a human-readable string describing the return status + * + * value (anytype): return value is further defined by individual API calls. + */ +int cRosApiParseResponse( CrosNode *n, int client_idx ) +{ + PRINT_VVDEBUG ( "cRosApiParseResponse()\n" ); + XmlrpcProcess *client_proc = &(n->xmlrpc_client_proc[client_idx]); + int ret = -1; + + if( client_proc->current_call == NULL ) + { + PRINT_ERROR ( "cRosApiParseResponse() : Invalid current call for XMLRPC process\n" ); + return ret; + } + + RosApiCall *call = client_proc->current_call; + if(client_idx == 0 && call->user_call == 0) // xmlrpc client connected to roscore (master) + { + if( client_proc->message_type != XMLRPC_MESSAGE_RESPONSE ) + { + PRINT_ERROR ( "cRosApiParseResponse() : Not a response message \n" ); + return ret; + } + // xmlrpcParamVectorPrint(&client_proc->response); //// + + switch (call->method) + { + case CROS_API_REGISTER_PUBLISHER: + { + PRINT_VDEBUG ( "cRosApiParseResponse() : registerPublisher response \n" ); + if( checkResponseValue( &client_proc->response ) ) + ret = 0; + + break; + } + case CROS_API_REGISTER_SERVICE: + { + PRINT_VDEBUG ( "cRosApiParseResponse() : registerService response \n" ); + + if( checkResponseValue( &client_proc->response ) ) + ret = 0; + + break; + } + case CROS_API_REGISTER_SUBSCRIBER: + { + PRINT_VDEBUG ( "cRosApiParseResponse() : registerSubscriber response \n" ); + + //Get the next subscriber without a topic host + if(call->provider_idx == -1) + { + PRINT_ERROR ( "cRosApiParseResponse() : Invalid provider index in call CROS_API_REGISTER_SUBSCRIBER for XMLRPC process\n" ); + return ret; + } + int subidx = call->provider_idx; + + if(checkResponseValue( &client_proc->response ) ) + { + ret = 0; + + //Check if there is some publishers for the subscription + XmlrpcParam *param = xmlrpcParamVectorAt(&client_proc->response, 0); + XmlrpcParam *array = xmlrpcParamArrayGetParamAt(param,2); + int available_pubs_n = xmlrpcParamArrayGetSize(array); + + if (available_pubs_n > 0) + { + int i ; + for (i = 0; i < available_pubs_n; i++) + { + XmlrpcParam* pub_host = xmlrpcParamArrayGetParamAt(array, i); + char* pub_host_string = xmlrpcParamGetString(pub_host); + //manage string for exploit informations + //removing the 'http://' and the last '/' + int dirty_string_len = strlen(pub_host_string); + char* clean_string = (char *)calloc(dirty_string_len-8+1,sizeof(char)); + if (clean_string != NULL) + { + char topic_host_addr[MAX_HOST_NAME_LEN+1]; + int topic_host_port; + strncpy(clean_string,pub_host_string+7,dirty_string_len-8); + char *progress = NULL; + char *hostname = strtok_r(clean_string,":",&progress); + + int rc = lookup_host(hostname, topic_host_addr, sizeof(topic_host_addr)); + if (rc == 0) + { + topic_host_port = atoi(strtok_r(NULL,":",&progress)); + if(enqueueRequestTopic(n, subidx, topic_host_addr, topic_host_port) == -1) + ret=-1; + } + else + ret=-1; + free(clean_string); + } + else + ret=-1; + } + } + } + break; + } + case CROS_API_LOOKUP_SERVICE: + { + PRINT_VDEBUG ( "cRosApiParseResponse() : Lookup Service response\n" ); + + //Get the next service caller without a topic host + if(call->provider_idx == -1) + { + PRINT_ERROR ( "cRosApiParseResponse() : Invalid provider index in call CROS_API_LOOKUP_SERVICE for XMLRPC process\n" ); + return ret; + } + + int srvcalleridx = call->provider_idx; + ServiceCallerNode* requesting_service_caller = &(n->service_callers[srvcalleridx]); + + TcprosProcess* rpcros_proc = &n->rpcros_client_proc[requesting_service_caller->rpcros_id]; + rpcros_proc->service_idx = call->provider_idx; + + if(checkResponseValue( &client_proc->response ) == 1) + { + ret = 0; + //Check if there is some publishers for the subscription + XmlrpcParam *param = xmlrpcParamVectorAt(&client_proc->response, 0); + if(xmlrpcParamArrayGetSize(param) >= 3) + { + XmlrpcParam *service_prov_host = xmlrpcParamArrayGetParamAt(param,2); + char* service_prov_host_string = xmlrpcParamGetString(service_prov_host); + //manage string for exploit informations + //removing the 'rosrpc://' and the last '/' + int dirty_string_len = strlen(service_prov_host_string); + char* clean_string = (char *)calloc(dirty_string_len-10+1,sizeof(char)); + if (clean_string != NULL) + { + strncpy(clean_string,service_prov_host_string+9,dirty_string_len-10); + char * progress = NULL; + char* hostname = strtok_r(clean_string,":",&progress); + if(requesting_service_caller->service_host == NULL) + { + requesting_service_caller->service_host = (char *)calloc(MAX_HOST_NAME_LEN+1,sizeof(char)); //deleted in cRosNodeDestroy + } + + if (requesting_service_caller->service_host != NULL) + { + int rc = lookup_host(hostname, requesting_service_caller->service_host, (MAX_HOST_NAME_LEN+1)*sizeof(char)); + if (rc == 0) + { + requesting_service_caller->service_port = atoi(strtok_r(NULL,":",&progress)); + + //need to be checked because maybe the connection went down suddenly. + if(!rpcros_proc->socket.open) + { + tcpIpSocketOpen(&(rpcros_proc->socket)); + } + + PRINT_VDEBUG( "cRosApiParseResponse() : Lookup Service response [tcp port: %d]\n", requesting_service_caller->service_port); + + //set the process to open the socket with the desired host + tcprosProcessChangeState(rpcros_proc, TCPROS_PROCESS_STATE_CONNECTING); + } + else + ret=-1; + } + else + ret=-1; + free(clean_string); + } + else + ret=-1; + } + else + { + PRINT_ERROR ( "cRosApiParseResponse() : Invalid response from ROS master when Looking up services (Not enough parameters in response)\n"); + ret=-1; + } + } + else + { + tcprosProcessChangeState(rpcros_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); + } + + break; + } + case CROS_API_SUBSCRIBE_PARAM: + { + int paramsubidx = call->provider_idx; + ParameterSubscription *subscription = &n->paramsubs[paramsubidx]; + + if(checkResponseValue( &client_proc->response ) ) + { + XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response,0); + // XmlrpcParam *status_msg = xmlrpcParamArrayGetParamAt(array, 1); + XmlrpcParam *value = xmlrpcParamArrayGetParamAt(array, 2); + + XmlrpcParam copy; + int rc = xmlrpcParamCopy(©, value); + if (rc < 0) + break; + + subscription = &n->paramsubs[paramsubidx]; + + CrosNodeStatusUsr status; + initCrosNodeStatus(&status); + status.state = CROS_STATUS_PARAM_SUBSCRIBED; + status.provider_idx = paramsubidx; + status.parameter_key = subscription->parameter_key; + status.parameter_value = value; + subscription->status_api_callback(&status, subscription->context); // calls the parameter-subscriber application-defined status callback function (if specified when creating the publisher). + + ret = 0; + xmlrpcParamRelease(&subscription->parameter_value); + subscription->parameter_value = copy; + } + + break; + } + case CROS_API_GET_PID: + { + PRINT_VDEBUG ( "cRosApiParseResponse() : get PID response \n" ); + + if( checkResponseValue( &client_proc->response ) ) + { + ret = 0; + XmlrpcParam* roscore_pid_param = + xmlrpcParamArrayGetParamAt(xmlrpcParamVectorAt(&client_proc->response,0),2); + + if(n->roscore_pid == -1) + { + n->roscore_pid = roscore_pid_param->data.as_int; + } + else if (n->roscore_pid != roscore_pid_param->data.as_int) + { + n->roscore_pid = roscore_pid_param->data.as_int; + restartAdversing(n); + } + } + else + { + restartAdversing(n); + } + + xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); + break; + } + case CROS_API_UNREGISTER_SERVICE: + { + PRINT_VDEBUG ( "cRosApiParseResponse() : unregister service response \n" ); + + if( checkResponseValue( &client_proc->response ) ) + { + ret = 0; + XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response,0); + XmlrpcParam* status_msg = xmlrpcParamArrayGetParamAt(array, 1); + XmlrpcParam* unreg_num = xmlrpcParamArrayGetParamAt(array, 2); + } + + xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); + break; + } + case CROS_API_UNREGISTER_PUBLISHER: + { + PRINT_VDEBUG ( "cRosApiParseResponse() : unregister publisher response \n" ); + if( checkResponseValue( &client_proc->response ) ) + { + ret = 0; + XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response,0); + XmlrpcParam* status_msg = xmlrpcParamArrayGetParamAt(array, 1); + XmlrpcParam* unreg_num = xmlrpcParamArrayGetParamAt(array, 2); + } + + xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); + break; + } + case CROS_API_UNREGISTER_SUBSCRIBER: + { + PRINT_VDEBUG ( "cRosApiParseResponse() : unregister subscriber response \n" ); + if( checkResponseValue( &client_proc->response ) ) + { + ret = 0; + XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response, 0); + XmlrpcParam* status_msg = xmlrpcParamArrayGetParamAt(array, 1); + XmlrpcParam* unreg_num = xmlrpcParamArrayGetParamAt(array, 2); + } + + xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); + break; + } + case CROS_API_UNSUBSCRIBE_PARAM: + { + if( checkResponseValue( &client_proc->response ) ) + { + ret = 0; + XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response, 0); + XmlrpcParam* status_msg = xmlrpcParamArrayGetParamAt(array, 1); + XmlrpcParam* unreg_num = xmlrpcParamArrayGetParamAt(array, 2); + } + + xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); + break; + } + default: + { + PRINT_ERROR ( "cRosApiParseResponse() : Invalid call method for XML RPC process connected to ROS master\n" ); + ret=-1; + } + } + } + else // client_idx > 0 || user_call = 1 + { + switch (call->method) + { + case CROS_API_LOOKUP_NODE: + case CROS_API_GET_PUBLISHED_TOPICS: + case CROS_API_GET_TOPIC_TYPES: + case CROS_API_GET_SYSTEM_STATE: + case CROS_API_GET_URI: + case CROS_API_LOOKUP_SERVICE: + case CROS_API_GET_BUS_STATS: + case CROS_API_GET_BUS_INFO: + case CROS_API_GET_MASTER_URI: + case CROS_API_SHUTDOWN: + case CROS_API_GET_PID: + case CROS_API_GET_SUBSCRIPTIONS: + case CROS_API_GET_PUBLICATIONS: + case CROS_API_DELETE_PARAM: + case CROS_API_SET_PARAM: + case CROS_API_GET_PARAM: + case CROS_API_SEARCH_PARAM: + case CROS_API_HAS_PARAM: + case CROS_API_GET_PARAM_NAMES: + { + ret = 0; + + // xmlrpcParamVectorPrint(&client_proc->response); //// + + ResultCallback callback = call->result_callback; + if (callback != NULL) + { + void *result = call->fetch_result_callback(&client_proc->response); + callback(call->id, result, call->context_data); + if (result != NULL) + call->free_result_callback(result); + } + + break; + } + case CROS_API_REQUEST_TOPIC: + { + if( checkResponseValue( &client_proc->response ) ) + { + int client_tcpros_ind; + char tcpros_host[MAX_HOST_NAME_LEN+1]; + ret = 0; + + XmlrpcParam* param_array = xmlrpcParamVectorAt(&client_proc->response,0); + XmlrpcParam* nested_array = xmlrpcParamArrayGetParamAt(param_array,2); + XmlrpcParam* tcp_host = xmlrpcParamArrayGetParamAt(nested_array,1); + XmlrpcParam* tcp_port = xmlrpcParamArrayGetParamAt(nested_array,2); + + RosApiCall *call = client_proc->current_call; + int sub_ind = call->provider_idx; + + int tcp_port_print = tcp_port->data.as_int; + SubscriberNode* sub = &n->subs[sub_ind]; + + PRINT_VDEBUG( "cRosApiParseResponse() : requestTopic response [tcp port: %d]\n", tcp_port_print); + xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); + + int rc = lookup_host(tcp_host->data.as_string, tcpros_host, sizeof(tcpros_host)); + if (rc == 0) + { + // Check if a Tcpros client is already connected to the received hostname and port for the current subscriber node + client_tcpros_ind = cRosNodeFindFirstTcprosClientProc(n, sub_ind, tcpros_host, tcp_port_print); + if(client_tcpros_ind == -1) // No Tcpros client is already connected to this hostname and port for the current subscriber node, recruit one: + { + client_tcpros_ind = cRosNodeRecruitTcprosClientProc(n, sub_ind); + if(client_tcpros_ind != -1) // A Tcpros client has been recruited to be used + { + TcprosProcess* tcpros_proc = &n->tcpros_client_proc[client_tcpros_ind]; + tcpros_proc->topic_idx = sub_ind; + + // need to be checked because maybe the connection went down suddenly. + if(!tcpros_proc->socket.open) + tcpIpSocketOpen(&(tcpros_proc->socket)); + + // set the process to open the socket with the desired host + tcpros_proc->sub_tcpros_host = (char *)realloc(tcpros_proc->sub_tcpros_host, strlen(tcpros_host)+sizeof(char)); // If already allocated, realloc() does nothing + if (tcpros_proc->sub_tcpros_host != NULL) + { + strcpy(tcpros_proc->sub_tcpros_host, tcpros_host); + tcpros_proc->sub_tcpros_port = tcp_port_print; + tcprosProcessChangeState(tcpros_proc, TCPROS_PROCESS_STATE_CONNECTING); + // printf("HOST: %s:%i\n",tcpros_proc->sub_tcpros_host,tcpros_proc->sub_tcpros_port); + } + else + { + PRINT_ERROR ( "cRosApiParseResponse() : Not enough memory allocating the publisher hostname string\n"); + ret=-1; + } + } + else + { + PRINT_ERROR ( "cRosApiParseResponse() : No TCPROS client process is available to be used to subscribe to the topic. Allocate more TCPROS client processes.\n"); + ret=-1; + } + } + } + else + { + PRINT_ERROR ( "cRosApiParseResponse() : Cannot resolve the publisher hostname received in response of REQUEST_TOPIC\n"); + ret=-1; + } + } + break; + } + default: + { + PRINT_ERROR ( "cRosApiParseResponse() : Invalid call method for XML RPC process connected to ROS node\n" ); + ret=-1; + } + } + } + + return ret; +} + +// return value is different from 0 only when a response message cannot be generated +int cRosApiParseRequestPrepareResponse( CrosNode *n, int server_idx ) +{ + int ret; + PRINT_VDEBUG ( "cRosApiParseRequestPrepareResponse()\n" ); + + XmlrpcProcess *server_proc = &(n->xmlrpc_server_proc[server_idx]); + + if( server_proc->message_type != XMLRPC_MESSAGE_REQUEST) + { + PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Not a request message \n" ); + return -1; + } + + ret=0; // Default return value + server_proc->message_type = XMLRPC_MESSAGE_RESPONSE; + + XmlrpcParamVector params; + xmlrpcParamVectorInit(¶ms); + + CrosApiMethod method = getMethodCode(dynStringGetData(&server_proc->method)); + switch (method) + { + case CROS_API_GET_PID: + { + xmlrpcParamVectorPushBackArray(¶ms); + XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); + xmlrpcParamArrayPushBackInt(array, 1); + xmlrpcParamArrayPushBackString(array, ""); + xmlrpcParamArrayPushBackInt(array, n->pid); + break; + } + case CROS_API_PUBLISHER_UPDATE: + { + PRINT_VDEBUG("publisherUpdate()\n"); + // TODO Store the subscribed node name + XmlrpcParam *caller_id_param, *topic_param, *publishers_param; + +#if CROS_DEBUG_LEVEL >= 3 + xmlrpcParamVectorPrint( &server_proc->params ); +#endif + + caller_id_param = xmlrpcParamVectorAt(&server_proc->params, 0); + topic_param = xmlrpcParamVectorAt( &server_proc->params, 1); + publishers_param = xmlrpcParamVectorAt(&server_proc->params, 2); + + if( xmlrpcParamGetType( caller_id_param ) != XMLRPC_PARAM_STRING || + xmlrpcParamGetType( topic_param ) != XMLRPC_PARAM_STRING || + xmlrpcParamGetType( publishers_param ) != XMLRPC_PARAM_ARRAY ) + { + PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Wrong publisherUpdate message \n" ); + xmlrpcParamVectorPushBackString( ¶ms, "Unable to resolve hostname" ); + break; + } + else + { + int array_size; + char *topic_name_param; + XmlrpcParam *uri; + int sub_idx = -1; + int uri_found = 0; + int i; + + topic_name_param = xmlrpcParamGetString( topic_param ); + array_size = xmlrpcParamArrayGetSize( publishers_param ); + + for(i = 0; i < n->n_subs; i++) + { + if (n->subs[i].topic_name == NULL) + continue; + + if( strcmp( topic_name_param, n->subs[i].topic_name ) == 0) + { + sub_idx = i; + break; + } + } + + if(array_size > 0) + { + uri = xmlrpcParamArrayGetParamAt ( publishers_param, 0 ); + if(xmlrpcParamGetType(uri) == XMLRPC_PARAM_STRING) + { + uri_found = 1; + } + } + + // The condition to be met is: + // Topic name must be registered and + // either some node is unregistering the topic or + // the host is found and the tcpros port is not assigned + if (sub_idx != -1 && (array_size == 0 || uri_found)) + { + // Subscriber that is still waiting for a tcpros connection found + publishers_param = xmlrpcParamVectorAt(&server_proc->params, 2); + int available_pubs_n = xmlrpcParamArrayGetSize(publishers_param); + + { + int pub_host_ind; + // Iterate through all the hosts while no error occurs + for(pub_host_ind=0;pub_host_indparams), 0); + topic_param = xmlrpcParamVectorAt( &(server_proc->params), 1); + protocols_param = xmlrpcParamVectorAt( &(server_proc->params), 2); + + if( xmlrpcParamGetType( node_param ) != XMLRPC_PARAM_STRING || + xmlrpcParamGetType( topic_param ) != XMLRPC_PARAM_STRING || + xmlrpcParamGetType( protocols_param ) != XMLRPC_PARAM_ARRAY ) + { + PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Wrong requestTopic message \n" ); + xmlrpcParamVectorPushBackString( ¶ms, "Wrong requestTopic message" ); + break; + } + else + { + int array_size = xmlrpcParamArrayGetSize( protocols_param ); + XmlrpcParam *proto, *proto_name; + int i = 0, topic_found = 0, protocol_found = 0; + + for( i = 0 ; i < n->n_pubs; i++) + { + PublisherNode *pub = &n->pubs[i]; + if (pub->topic_name == NULL) + continue; + + if( strcmp( xmlrpcParamGetString( topic_param ), pub->topic_name ) == 0) + { + topic_found = 1; + if (strlen(server_proc->host) != 0) + { + CrosNodeStatusUsr status; + initCrosNodeStatus(&status); + status.xmlrpc_host = server_proc->host; + status.xmlrpc_port = server_proc->port; + cRosNodeStatusCallback(&status, pub->context); // calls the publisher-status application-defined callback function (if specified when creating the publisher). Undocumented status callback? + } + + break; + } + } + + for( i = 0 ; i < array_size; i++) + { + proto = xmlrpcParamArrayGetParamAt ( protocols_param, i ); + + if( xmlrpcParamGetType( proto ) == XMLRPC_PARAM_ARRAY && + ( proto_name = xmlrpcParamArrayGetParamAt ( proto, 0 ) ) != NULL && + xmlrpcParamGetType( proto_name ) == XMLRPC_PARAM_STRING && + strcmp( xmlrpcParamGetString( proto_name ), CROS_TRANSPORT_TCPROS_STRING) == 0 ) + { + protocol_found = 1; + break; + } + } + + if( topic_found && protocol_found) + { + xmlrpcParamVectorPushBackArray(¶ms); + XmlrpcParam *array1 = xmlrpcParamVectorAt(¶ms, 0); + xmlrpcParamArrayPushBackInt(array1, 1); + xmlrpcParamArrayPushBackString(array1, ""); + XmlrpcParam* array2 = xmlrpcParamArrayPushBackArray(array1); + xmlrpcParamArrayPushBackString( array2, CROS_TRANSPORT_TCPROS_STRING ); + xmlrpcParamArrayPushBackString( array2, n->host ); + xmlrpcParamArrayPushBackInt( array2, n->tcpros_port ); + } + else + { + PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Topic or protocol for requestTopic not supported\n" ); + xmlrpcParamVectorPushBackString( ¶ms, "Topic or protocol for requestTopic not supported"); + break; + } + } + + break; + } + case CROS_API_GET_BUS_STATS: + { + // CHECK-ME What to answer here? + xmlrpcParamVectorPushBackArray(¶ms); + XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); + xmlrpcParamArrayPushBackInt(array, 0); + xmlrpcParamArrayPushBackString(array, ""); + break; + } + case CROS_API_GET_BUS_INFO: + { + int proc_idx; + char tcpros_url_msg[MAX_HOST_NAME_LEN+60]; + XmlrpcParam *ret_parm_arr, *ret_businfo_arr, *ret_connect_arr; + + // Answer the transport/topic connection information + xmlrpcParamVectorPushBackArray(¶ms); + ret_parm_arr = xmlrpcParamVectorAt(¶ms, 0); + if(ret_parm_arr != NULL) + { + xmlrpcParamArrayPushBackInt(ret_parm_arr, 1); + xmlrpcParamArrayPushBackString(ret_parm_arr, ""); + ret_businfo_arr = xmlrpcParamArrayPushBackArray(ret_parm_arr); + if(ret_businfo_arr == NULL) + ret=-1; + } + else + ret=-1; + + for(proc_idx=0;proc_idxtcpros_client_proc[proc_idx]; + if(cur_cli_proc->topic_idx != -1) + { + ret_connect_arr = xmlrpcParamArrayPushBackArray(ret_businfo_arr); + if(ret_connect_arr != NULL) + { + xmlrpcParamArrayPushBackInt(ret_connect_arr, proc_idx); + snprintf(tcpros_url_msg, sizeof(tcpros_url_msg), "http://%s:%hu/", tcpIpSocketGetRemoteAddress(&cur_cli_proc->socket), tcpIpSocketGetRemotePort(&cur_cli_proc->socket)); + // This URL could also be the name of the node that receives the topic messages + xmlrpcParamArrayPushBackString(ret_connect_arr, tcpros_url_msg); + // In this case, this node is the subscriber, so the connection direction marked as inbound. This means that data (messages) is transmitted + // from other node to this node: published -> subscriber (although the connection is established from this node to other node: subscriber -> publisher) + xmlrpcParamArrayPushBackString(ret_connect_arr, "i"); + xmlrpcParamArrayPushBackString(ret_connect_arr, "TCPROS"); + xmlrpcParamArrayPushBackString(ret_connect_arr, dynStringGetData(&cur_cli_proc->topic)); + xmlrpcParamArrayPushBackInt(ret_connect_arr, 1); + snprintf(tcpros_url_msg, sizeof(tcpros_url_msg), "TCPROS connection on port %hu to [%s:%hu on socket %i]", tcpIpSocketGetPort(&cur_cli_proc->socket), cur_cli_proc->sub_tcpros_host, cur_cli_proc->sub_tcpros_port, tcpIpSocketGetFD(&cur_cli_proc->socket)); + // example of tcpros_url_msg sniffed from a ROS subscriber node (/rosout): TCPROS connection on port 55636 to [host_name:49463 on socket 14] + // 55636 is the TCPROS local port in this (subscriber) node that is connected to the listening port of the other (remote) node (publisher) + // host_name is the address of the other (remote node), which is the publisher. In this case it is also a local address + // 49463 is the listening port of the other (remote) node, which is the publisher node + // 14 is the local file despcriptor of this TCPROS connection, that is a file descriptor of the /rousout node process + xmlrpcParamArrayPushBackString(ret_connect_arr, tcpros_url_msg); + } + else + ret=-1; + } + } + + for(proc_idx=0;proc_idxtcpros_server_proc[proc_idx]; + if(cur_ser_proc->topic_idx != -1) + { + ret_connect_arr = xmlrpcParamArrayPushBackArray(ret_businfo_arr); + if(ret_connect_arr != NULL) + { + xmlrpcParamArrayPushBackInt(ret_connect_arr, proc_idx); + snprintf(tcpros_url_msg, sizeof(tcpros_url_msg), "http://%s:%hu/", tcpIpSocketGetRemoteAddress(&cur_ser_proc->socket), tcpIpSocketGetRemotePort(&cur_ser_proc->socket)); + xmlrpcParamArrayPushBackString(ret_connect_arr, tcpros_url_msg); + // In this case, this node is the publisher, so the connection direction is marked as outbound. This means that data (messages) is transmitted + // from this node to other node: published -> subscriber (although the connection is established from other node to this node: subscriber -> publisher) + xmlrpcParamArrayPushBackString(ret_connect_arr, "o"); + xmlrpcParamArrayPushBackString(ret_connect_arr, "TCPROS"); + xmlrpcParamArrayPushBackString(ret_connect_arr, dynStringGetData(&cur_ser_proc->topic)); + xmlrpcParamArrayPushBackInt(ret_connect_arr, 1); + snprintf(tcpros_url_msg, sizeof(tcpros_url_msg), "TCPROS connection on port %hu to [%s:%hu on socket %i]", tcpIpSocketGetPort(&cur_ser_proc->socket), tcpIpSocketGetRemoteAddress(&cur_ser_proc->socket), tcpIpSocketGetRemotePort(&cur_ser_proc->socket), tcpIpSocketGetFD(&cur_ser_proc->socket)); + // example of tcpros_url_msg sniffed from a ROS publisher node (/turtlesim): TCPROS connection on port 49463 to [127.0.0.1:55636 on socket 26] + // 49463 is the TCPROS local listening port of this (publisher) node, which received the incoming connection + // 127.0.0.1 is the address of the other (remote node). In this case it is also a local address + // 55636 is the other-(remote)-node port, which corresponds to the socket created for this TCPROS connection + // 26 is the local file despcriptor of this TCPROS connection + // So this XML call returns information about the TCPROS connection between publisher and subscriber, although this information is tramsitted through the XMLRPC port connetion + xmlrpcParamArrayPushBackString(ret_connect_arr, tcpros_url_msg); + } + else + ret=-1; + } + } + +// xmlrpcParamVectorPrint( ¶ms ); + break; + } + case CROS_API_GET_MASTER_URI: + { + xmlrpcParamVectorPushBackArray(¶ms); + XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); + xmlrpcParamArrayPushBackInt(array, 1); + xmlrpcParamArrayPushBackString(array, ""); + char node_uri[256]; + snprintf(node_uri, 256, "http://%s:%d/", n->roscore_host, n->roscore_port); + xmlrpcParamArrayPushBackString(array, node_uri); + break; + } + case CROS_API_SHUTDOWN: + { + xmlrpcParamVectorPushBackArray(¶ms); + XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); + xmlrpcParamArrayPushBackInt(array, 1); + xmlrpcParamArrayPushBackString(array, ""); + xmlrpcParamArrayPushBackInt(array, 1); + PRINT_INFO ( "cRosApiParseRequestPrepareResponse() : Received shutdown request through ROS slave API. This call will be ignored.\n" ); + break; + } + case CROS_API_PARAM_UPDATE: + { + ParameterSubscription* subscription = NULL; + + XmlrpcParam *node_param = xmlrpcParamVectorAt(&server_proc->params, 0); + XmlrpcParam *key_param = xmlrpcParamVectorAt(&server_proc->params, 1); + XmlrpcParam *value_param = xmlrpcParamVectorAt(&server_proc->params, 2); + if (xmlrpcParamGetType(key_param) != XMLRPC_PARAM_STRING) + { + PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Wrong paramUpdate message\n" ); + goto PrepareResponse; + } + + int paramsubidx = -1; + char *parameter_key = xmlrpcParamGetString(key_param); + int it = 0; + for (it = 0 ; it < n->n_paramsubs; it++) + { + if (n->paramsubs[it].parameter_key == NULL) + continue; + + if (strncmp(parameter_key, n->paramsubs[it].parameter_key, strlen(n->paramsubs[it].parameter_key)) == 0) + { + paramsubidx = it; + + break; + } + } + + if (paramsubidx != -1) + { + subscription = &n->paramsubs[it]; + CrosNodeStatusUsr status; + initCrosNodeStatus(&status); + status.state = CROS_STATUS_PARAM_UPDATE; + status.provider_idx = it; + status.parameter_key = parameter_key; + status.parameter_value = value_param; + subscription->status_api_callback(&status, subscription->context); // calls the parameter-subscriber-status application-defined callback function (if specified when creating the subscriber). + + XmlrpcParam param; + int rc = xmlrpcParamCopy(¶m, value_param); + if (rc != -1) + { + xmlrpcParamRelease(&subscription->parameter_value); + subscription->parameter_value = param; + } + } + + PrepareResponse: + xmlrpcParamVectorPushBackArray(¶ms); + XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); + xmlrpcParamArrayPushBackInt(array, 1); + xmlrpcParamArrayPushBackString(array, ""); + if (subscription == NULL) + xmlrpcParamArrayPushBackInt(array, 1); + else + xmlrpcParamArrayPushBackInt(array, 0); + break; + } + case CROS_API_GET_SUBSCRIPTIONS: + { + xmlrpcParamVectorPushBackArray(¶ms); + XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); + xmlrpcParamArrayPushBackInt(array, 1); + xmlrpcParamArrayPushBackString(array, ""); + XmlrpcParam* param_array = xmlrpcParamArrayPushBackArray(array); + + int i = 0; + for(i = 0; i < n->n_subs; i++) + { + XmlrpcParam* sub_array = xmlrpcParamArrayPushBackArray(param_array); + xmlrpcParamArrayPushBackString(sub_array, n->subs[i].topic_name); + xmlrpcParamArrayPushBackString(sub_array, n->subs[i].topic_type); + } + + break; + } + case CROS_API_GET_PUBLICATIONS: + { + xmlrpcParamVectorPushBackArray(¶ms); + XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); + xmlrpcParamArrayPushBackInt(array, 1); + xmlrpcParamArrayPushBackString(array, ""); + XmlrpcParam* param_array = xmlrpcParamArrayPushBackArray(array); + + int i = 0; + for(i = 0; i < n->n_pubs; i++) + { + XmlrpcParam* sub_array = xmlrpcParamArrayPushBackArray(param_array); + xmlrpcParamArrayPushBackString(sub_array, n->pubs[i].topic_name); + xmlrpcParamArrayPushBackString(sub_array, n->pubs[i].topic_type); + } + + break; + } + default: + { + PRINT_ERROR("cRosApiParseRequestPrepareResponse() : Unknown method \n Message : \n %s", + dynStringGetData( &(server_proc->message))); + xmlrpcParamVectorPushBackString( ¶ms, "Unknown method"); + break; + } + } + + xmlrpcProcessClear(server_proc); + generateXmlrpcMessage(NULL, 0, server_proc->message_type, + dynStringGetData(&server_proc->method), ¶ms, &server_proc->message); // host and port are not used in XMLRPC_MESSAGE_RESPONSE + xmlrpcParamVectorRelease(¶ms); + + return ret; +} + +const char * getMethodName(CrosApiMethod method) +{ + switch (method) + { + case CROS_API_NONE: + return NULL; + case CROS_API_REGISTER_SERVICE: + return "registerService"; + case CROS_API_UNREGISTER_SERVICE: + return "unregisterService"; + case CROS_API_REGISTER_SUBSCRIBER: + return "registerSubscriber"; + case CROS_API_UNREGISTER_SUBSCRIBER: + return "unregisterSubscriber"; + case CROS_API_REGISTER_PUBLISHER: + return "registerPublisher"; + case CROS_API_UNREGISTER_PUBLISHER: + return "unregisterPublisher"; + case CROS_API_LOOKUP_NODE: + return "lookupNode"; + case CROS_API_GET_PUBLISHED_TOPICS: + return "getPublishedTopics"; + case CROS_API_GET_TOPIC_TYPES: + return "getTopicTypes"; + case CROS_API_GET_SYSTEM_STATE: + return "getSystemState"; + case CROS_API_GET_URI: + return "getUri"; + case CROS_API_LOOKUP_SERVICE: + return "lookupService"; + case CROS_API_GET_BUS_STATS: + return "getBusStats"; + case CROS_API_GET_BUS_INFO: + return "getBusInfo"; + case CROS_API_GET_MASTER_URI: + return "getMasterUri"; + case CROS_API_SHUTDOWN: + return "shutdown"; + case CROS_API_GET_PID: + return "getPid"; + case CROS_API_GET_SUBSCRIPTIONS: + return "getSubscriptions"; + case CROS_API_GET_PUBLICATIONS: + return "getPublications"; + case CROS_API_PARAM_UPDATE: + return "paramUpdate"; + case CROS_API_PUBLISHER_UPDATE: + return "publisherUpdate"; + case CROS_API_REQUEST_TOPIC: + return "requestTopic"; + case CROS_API_DELETE_PARAM: + return "deleteParam"; + case CROS_API_SET_PARAM: + return "setParam"; + case CROS_API_GET_PARAM: + return "getParam"; + case CROS_API_SEARCH_PARAM: + return "searchParam"; + case CROS_API_SUBSCRIBE_PARAM: + return "subscribeParam"; + case CROS_API_UNSUBSCRIBE_PARAM: + return "unsubscribeParam"; + case CROS_API_HAS_PARAM: + return "hasParam"; + case CROS_API_GET_PARAM_NAMES: + return "getParamNames"; + default: + PRINT_ERROR( "getMethodName() : Invalid CrosApiMethod value specified\n" ); + return NULL; + } +} + +CrosApiMethod getMethodCode(const char *method) +{ + if (strcmp(method, "registerService") == 0) + return CROS_API_REGISTER_SERVICE; + else if (strcmp(method, "unregisterService") == 0) + return CROS_API_UNREGISTER_SERVICE; + else if (strcmp(method, "registerSubscriber") == 0) + return CROS_API_REGISTER_SUBSCRIBER; + else if (strcmp(method, "unregisterSubscriber") == 0) + return CROS_API_UNREGISTER_SUBSCRIBER; + else if (strcmp(method, "registerPublisher") == 0) + return CROS_API_REGISTER_PUBLISHER; + else if (strcmp(method, "unregisterPublisher") == 0) + return CROS_API_UNREGISTER_PUBLISHER; + else if (strcmp(method, "lookupNode") == 0) + return CROS_API_LOOKUP_NODE; + else if (strcmp(method, "getPublishedTopics") == 0) + return CROS_API_GET_PUBLISHED_TOPICS; + else if (strcmp(method, "getTopicTypes") == 0) + return CROS_API_GET_TOPIC_TYPES; + else if (strcmp(method, "getSystemState") == 0) + return CROS_API_GET_SYSTEM_STATE; + else if (strcmp(method, "getUri") == 0) + return CROS_API_GET_URI; + else if (strcmp(method, "lookupService") == 0) + return CROS_API_LOOKUP_SERVICE; + else if (strcmp(method, "getBusStats") == 0) + return CROS_API_GET_BUS_STATS; + else if (strcmp(method, "getBusInfo") == 0) + return CROS_API_GET_BUS_INFO; + else if (strcmp(method, "getMasterUri") == 0) + return CROS_API_GET_MASTER_URI; + else if (strcmp(method, "shutdown") == 0) + return CROS_API_SHUTDOWN; + else if (strcmp(method, "getPid") == 0) + return CROS_API_GET_PID; + else if (strcmp(method, "getSubscriptions") == 0) + return CROS_API_GET_SUBSCRIPTIONS; + else if (strcmp(method, "getPublications") == 0) + return CROS_API_GET_PUBLICATIONS; + else if (strcmp(method, "paramUpdate") == 0) + return CROS_API_PARAM_UPDATE; + else if (strcmp(method, "publisherUpdate") == 0) + return CROS_API_PUBLISHER_UPDATE; + else if (strcmp(method, "requestTopic") == 0) + return CROS_API_REQUEST_TOPIC; + else if (strcmp(method, "deleteParam") == 0) + return CROS_API_DELETE_PARAM; + else if (strcmp(method, "setParam") == 0) + return CROS_API_SET_PARAM; + else if (strcmp(method, "getParam") == 0) + return CROS_API_GET_PARAM; + else if (strcmp(method, "searchParam") == 0) + return CROS_API_SEARCH_PARAM; + else if (strcmp(method, "subscribeParam") == 0) + return CROS_API_SUBSCRIBE_PARAM; + else if (strcmp(method, "unsubscribeParam") == 0) + return CROS_API_UNSUBSCRIBE_PARAM; + else if (strcmp(method, "hasParam") == 0) + return CROS_API_HAS_PARAM; + else if (strcmp(method, "getParamNames") == 0) + return CROS_API_GET_PARAM_NAMES; + else + return CROS_API_NONE; +} + +int isRosMasterApi(CrosApiMethod method) +{ + switch (method) + { + case CROS_API_NONE: + return 0; + case CROS_API_REGISTER_SERVICE: + case CROS_API_UNREGISTER_SERVICE: + case CROS_API_REGISTER_SUBSCRIBER: + case CROS_API_UNREGISTER_SUBSCRIBER: + case CROS_API_REGISTER_PUBLISHER: + case CROS_API_UNREGISTER_PUBLISHER: + case CROS_API_LOOKUP_NODE: + case CROS_API_GET_PUBLISHED_TOPICS: + case CROS_API_GET_TOPIC_TYPES: + case CROS_API_GET_SYSTEM_STATE: + case CROS_API_GET_URI: + case CROS_API_LOOKUP_SERVICE: + return 1; + case CROS_API_GET_BUS_STATS: + case CROS_API_GET_BUS_INFO: + case CROS_API_GET_MASTER_URI: + case CROS_API_SHUTDOWN: + case CROS_API_GET_PID: + case CROS_API_GET_SUBSCRIPTIONS: + case CROS_API_GET_PUBLICATIONS: + case CROS_API_PARAM_UPDATE: + case CROS_API_PUBLISHER_UPDATE: + case CROS_API_REQUEST_TOPIC: + return 0; + case CROS_API_DELETE_PARAM: + case CROS_API_SET_PARAM: + case CROS_API_GET_PARAM: + case CROS_API_SEARCH_PARAM: + case CROS_API_SUBSCRIBE_PARAM: + case CROS_API_UNSUBSCRIBE_PARAM: + case CROS_API_HAS_PARAM: + case CROS_API_GET_PARAM_NAMES: + return 1; + default: + PRINT_ERROR ( "isRosMasterApi() : Invalid CrosApiMethod value specified\n" ); + return -1; + } +} + +int isRosSlaveApi(CrosApiMethod method) +{ + switch (method) + { + case CROS_API_NONE: + return 0; + case CROS_API_REGISTER_SERVICE: + case CROS_API_UNREGISTER_SERVICE: + case CROS_API_REGISTER_SUBSCRIBER: + case CROS_API_UNREGISTER_SUBSCRIBER: + case CROS_API_REGISTER_PUBLISHER: + case CROS_API_UNREGISTER_PUBLISHER: + case CROS_API_LOOKUP_NODE: + case CROS_API_GET_PUBLISHED_TOPICS: + case CROS_API_GET_TOPIC_TYPES: + case CROS_API_GET_SYSTEM_STATE: + case CROS_API_GET_URI: + case CROS_API_LOOKUP_SERVICE: + return 0; + case CROS_API_GET_BUS_STATS: + case CROS_API_GET_BUS_INFO: + case CROS_API_GET_MASTER_URI: + case CROS_API_SHUTDOWN: + case CROS_API_GET_PID: + case CROS_API_GET_SUBSCRIPTIONS: + case CROS_API_GET_PUBLICATIONS: + case CROS_API_PARAM_UPDATE: + case CROS_API_PUBLISHER_UPDATE: + case CROS_API_REQUEST_TOPIC: + return 1; + case CROS_API_DELETE_PARAM: + case CROS_API_SET_PARAM: + case CROS_API_GET_PARAM: + case CROS_API_SEARCH_PARAM: + case CROS_API_SUBSCRIBE_PARAM: + case CROS_API_UNSUBSCRIBE_PARAM: + case CROS_API_HAS_PARAM: + case CROS_API_GET_PARAM_NAMES: + return 0; + default: + PRINT_ERROR ( "isRosSlaveApi() : Invalid CrosApiMethod value specified\n" ); + return -1; + } +} diff --git a/src/hal/components/cros/src/cros_service.c b/src/hal/components/cros/src/cros_service.c new file mode 100644 index 00000000..d39e7fbb --- /dev/null +++ b/src/hal/components/cros/src/cros_service.c @@ -0,0 +1,433 @@ +#include +#include +#include +#include + +#include "cros_service.h" +#include "cros_service_internal.h" +#include "cros_message_internal.h" +#include "tcpros_tags.h" +#include "cros_defs.h" +#include "md5.h" + +cRosErrCodePack initCrosSrv(cRosSrvDef* srv) +{ + cRosErrCodePack ret_err; + + if(srv == NULL) + return CROS_BAD_PARAM_ERR; + + srv->request = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); + srv->response = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); + if(srv->request != NULL && srv->response != NULL) + { + ret_err = initCrosMsg(srv->request); + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + ret_err = initCrosMsg(srv->response); + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + srv->name = NULL; + srv->package = NULL; + srv->plain_text = NULL; + srv->root_dir = NULL; + ret_err = CROS_SUCCESS_ERR_PACK; + } + else + { + cRosMessageDefFree(srv->request); + free(srv->request); + free(srv->response); + ret_err = CROS_MEM_ALLOC_ERR; + } + } + else + { + free(srv->request); + free(srv->response); + ret_err = CROS_MEM_ALLOC_ERR; + } + } + else + { + free(srv->request); + free(srv->response); + ret_err = CROS_MEM_ALLOC_ERR; + } + return ret_err; +} + +cRosErrCodePack loadFromFileSrv(const char *filename, cRosSrvDef *srv) +{ + cRosErrCodePack ret_err; + size_t f_read_bytes; + + char* file_tokenized = (char* )malloc(strlen(filename)+sizeof(char)); + if(file_tokenized == NULL) + return CROS_MEM_ALLOC_ERR; + + file_tokenized[0] = '\0'; + strcpy(file_tokenized, filename); + char* token_pack = NULL; + char* token_root = NULL; + char* token_name = NULL; + + FILE *f = fopen(filename, "rb"); + + if(f != NULL) + { + ret_err=CROS_SUCCESS_ERR_PACK; + + fseek(f, 0, SEEK_END); + long fsize = ftell(f); + fseek(f, 0, SEEK_SET); + char *srv_req; + char *srv_res; + char *srv_text = (char *)malloc(fsize + 1); + if(srv_text == NULL) + { + fclose(f); + free(file_tokenized); + return CROS_MEM_ALLOC_ERR; + } + + f_read_bytes = fread(srv_text, 1, fsize, f); + fclose(f); + + if(f_read_bytes != (size_t)fsize) + { + free(file_tokenized); + free(srv_text); + return CROS_READ_SVC_FILE_ERR; + } + + + srv_text[fsize] = '\0'; + srv->plain_text = strdup(srv_text); // equiv. to malloc() + memcpy() + if(srv->plain_text == NULL) + { + free(srv_text); + free(file_tokenized); + return CROS_MEM_ALLOC_ERR; + } + + //splitting msg_text into the request response parts + srv_res = strstr(srv_text, SRV_DELIMITER); + if(srv_res == NULL) // The delimiter must be present in the service definition + { + free(srv_text); + free(file_tokenized); + return CROS_SVC_FILE_DELIM_ERR; + } + + //if the srv has some request parameters + if(srv_res != srv_text) + { + //split before the first delim char + *(srv_res - 1) = '\0'; + srv_req = srv_text; + } + else + srv_req = ""; // The service has no request parameter + + srv_res += strlen(SRV_DELIMITER); // skip the delimiter + if(*srv_res == '\n') + srv_res++; // skip the new line char + + char* tok = strtok(file_tokenized,"/."); + + while(tok != NULL) + { + if(strcmp(tok, "srv") != 0) + { + token_root = token_pack; + token_pack = token_name; + token_name = tok ; + } + tok = strtok(NULL,"/."); + } + + //build up the root path + char* it = file_tokenized; + while(it != token_root) + { + if(*it == '\0') + *it='/'; + it++; + } + + srv->root_dir = (char*) malloc (strlen(file_tokenized)+sizeof(char)); srv->root_dir[0] = '\0'; + srv->package = (char*) malloc (strlen(token_pack)+sizeof(char)); srv->package[0] = '\0'; + srv->name = (char*) malloc (strlen(token_name)+sizeof(char)); srv->name[0] = '\0'; + if(srv->root_dir != NULL && srv->package != NULL && srv->name != NULL) + { + strcpy(srv->root_dir,file_tokenized); + strcpy(srv->package,token_pack); + strcpy(srv->name,token_name); + } + else + ret_err=CROS_MEM_ALLOC_ERR; + + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + srv->request->package = strdup(srv->package); + srv->request->root_dir = strdup(srv->root_dir); + if(srv->request->package != NULL && srv->request->root_dir != NULL) + { + ret_err=loadFromStringMsg(srv_req, srv->request); + ret_err=cRosAddErrCodeIfErr(ret_err, CROS_LOAD_SVC_FILE_REQ_ERR); // If loadFromStringMsg() failed, add more info to the error-code pack, indicating the context + } + else + ret_err=CROS_MEM_ALLOC_ERR; + } + + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + srv->response->package = strdup(srv->package); + srv->response->root_dir = strdup(srv->root_dir); + if(srv->response->package != NULL && srv->response->root_dir != NULL) + { + ret_err=loadFromStringMsg(srv_res, srv->response); + ret_err=cRosAddErrCodeIfErr(ret_err, CROS_LOAD_SVC_FILE_RES_ERR); // If loadFromStringMsg() failed, add more another error code to the to the error-code pack indicating the context + } + else + ret_err=CROS_MEM_ALLOC_ERR; + } + + if(ret_err != CROS_SUCCESS_ERR_PACK) // An error occurred, free allocated memory before exiting + { + free(srv->response->root_dir); + free(srv->response->package); + free(srv->request->root_dir); + free(srv->request->package); + free(srv->root_dir); + free(srv->package); + free(srv->name); + srv->name = NULL; + srv->package = NULL; + srv->root_dir = NULL; + srv->request->package = NULL; + srv->request->root_dir = NULL; + srv->response->package = NULL; + srv->response->root_dir = NULL; + } + + free(srv_text); + } + else + ret_err=CROS_OPEN_SVC_FILE_ERR; + + free(file_tokenized); + + return ret_err; +} + +// Compute dependencies of the specified service file +cRosErrCodePack getFileDependenciesSrv(char* filename, cRosSrvDef* srv, msgDep* deps) +{ + cRosErrCodePack ret_err; + ret_err = loadFromFileSrv(filename, srv); + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + ret_err = getDependenciesMsg(srv->request,deps); + if(ret_err == CROS_SUCCESS_ERR_PACK) + ret_err = getDependenciesMsg(srv->response,deps); + } + return ret_err; +} + +// Compute full text of service, including text of embedded +// types. The text of the main srv is listed first. Embedded +// srv files are denoted first by an 80-character '=' separator, +// followed by a type declaration line,'MSG: pkg/type', followed by +// the text of the embedded type. +char* computeFullTextSrv(cRosSrvDef* srv, msgDep* deps) +{ + char* full_text = NULL; + char* msg_tag = "MSG: "; + int full_size = 0; + char separator[81]; + separator[80] = '\0'; + + memset(&separator,'=', 80); + full_size += strlen(srv->plain_text); + + while(deps->next != NULL) + { + full_size = strlen(deps->msg->plain_text) + strlen(separator) + strlen(msg_tag) + 3/*New lines*/; + deps = deps->next; + } + + //rollback + while(deps->prev != NULL) deps = deps->prev; + full_text = (char *)malloc(full_size + 1); + memcpy(full_text,srv->plain_text,strlen(srv->plain_text) + 1); + + while(deps->next != NULL) + { + strcat(full_text, "\n"); + strcat(full_text, separator); + strcat(full_text, "\n"); + strcat(full_text, msg_tag); + strcat(full_text, deps->msg->package); + strcat(full_text, "/"); + strcat(full_text, deps->msg->name); + strcat(full_text, "\n"); + strcat(full_text, deps->msg->plain_text); + deps = deps->next; + } + return full_text; +} + +cRosService * cRosServiceNew() +{ + cRosService *ret_svc = (cRosService *)calloc(1, sizeof(cRosService)); + if(ret_svc != NULL) + cRosServiceInit(ret_svc); + return ret_svc; +} + +cRosErrCodePack cRosServiceInit(cRosService* service) +{ + cRosErrCodePack ret_err; + + service->request = NULL; + service->response = NULL; + service->md5sum = (char *)malloc(33*sizeof(char));// 32 chars + '\0'; + + ret_err = (service->md5sum != NULL)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; + + return ret_err; +} + +cRosErrCodePack cRosServiceBuild(cRosService* service, const char* filepath) +{ + return cRosServiceBuildInner(&service->request, &service->response, NULL, service->md5sum, filepath); +} + +cRosErrCodePack cRosServiceBuildInner(cRosMessage **request_ptr, cRosMessage **response_ptr, char **message_definition, char *md5sum, const char *srv_filepath) +{ + cRosErrCodePack ret_err; + + cRosSrvDef* srv = (cRosSrvDef *)malloc(sizeof(cRosSrvDef)); + if(srv == NULL) + return CROS_MEM_ALLOC_ERR; + + ret_err = initCrosSrv(srv); + if(ret_err != CROS_SUCCESS_ERR_PACK) + { + free(srv); + return ret_err; + } + + ret_err = loadFromFileSrv(srv_filepath, srv); + if (ret_err != CROS_SUCCESS_ERR_PACK) + { + cRosServiceDefFree(srv); + return ret_err; + } + + DynString buffer; + dynStringInit(&buffer); + + MD5_CTX md5_t; + MD5_Init(&md5_t); + + if(srv->request->plain_text != NULL) + { + getMD5Txt(srv->request, &buffer); + MD5_Update(&md5_t, dynStringGetData(&buffer), dynStringGetLen(&buffer)); + dynStringClear(&buffer); + } + + if(srv->response->plain_text != NULL) + { + getMD5Txt(srv->response, &buffer); + MD5_Update(&md5_t, dynStringGetData(&buffer), dynStringGetLen(&buffer)); + } + dynStringRelease(&buffer); + + unsigned char* result = (unsigned char *)malloc(16); + MD5_Final(result, &md5_t); + DynString output; + dynStringInit(&output); + cRosMD5Readable(result,&output); + free(result); + + strcpy(md5sum, dynStringGetData(&output)); + dynStringRelease(&output); + + if(srv->request->plain_text != NULL) + { + ret_err = cRosMessageBuildFromDef(request_ptr, srv->request); + } + + if(ret_err == CROS_SUCCESS_ERR_PACK && srv->response->plain_text != NULL) + { + ret_err = cRosMessageBuildFromDef(response_ptr, srv->response); + } + + if(ret_err == CROS_SUCCESS_ERR_PACK && srv->plain_text != NULL && message_definition != NULL) + { + *message_definition=(char *)malloc((strlen(srv->plain_text)+1)*sizeof(char)); + if(*message_definition != NULL) + strcpy(*message_definition,srv->plain_text); + else + ret_err = CROS_MEM_ALLOC_ERR; + } + + cRosServiceDefFree(srv); + + return ret_err; +} + +void cRosServiceDefFree(cRosSrvDef* service_def) +{ + if(service_def != NULL) + { + free(service_def->name); + free(service_def->package); + free(service_def->plain_text); + free(service_def->root_dir); + cRosMessageDefFree(service_def->request); + cRosMessageDefFree(service_def->response); + free(service_def); + } +} + +void cRosServiceFree(cRosService* service) +{ + cRosServiceRelease(service); + free(service); +} + +void cRosServiceRelease(cRosService* service) +{ + cRosMessageFree(service->request); + cRosMessageFree(service->response); + free(service->md5sum); + service->md5sum = NULL; +} + +static uint32_t getLen( DynBuffer *pkt ) +{ + uint32_t len; + const unsigned char *data = dynBufferGetCurrentData(pkt); + len = ROS_TO_HOST_UINT32(*(uint32_t *)data); + dynBufferMovePoseIndicator(pkt,sizeof(uint32_t)); + + return len; +} + +static uint32_t pushBackField( DynBuffer *pkt, TcprosTagStrDim *tag, const char *val ) +{ + size_t val_len = strlen( val ); + uint32_t out_len, field_len = tag->dim + val_len; + //PRINT_VDEBUG("pushBackField() : filed : %s field_len ; %d\n", tag->str, field_len); + out_len = HOST_TO_ROS_UINT32( field_len ); + dynBufferPushBackUInt32( pkt, out_len ); + dynBufferPushBackBuf( pkt, (const unsigned char*)tag->str, tag->dim ); + dynBufferPushBackBuf( pkt, (const unsigned char*)val, val_len ); + + return field_len + sizeof( uint32_t ); +} diff --git a/src/hal/components/cros/src/cros_tcpros.c b/src/hal/components/cros/src/cros_tcpros.c new file mode 100644 index 00000000..50aca5cb --- /dev/null +++ b/src/hal/components/cros/src/cros_tcpros.c @@ -0,0 +1,1049 @@ +#include +#include +#include +#include + +#include "cros_api.h" +#include "cros_tcpros.h" +#include "cros_defs.h" +#include "tcpros_tags.h" +#include "tcpros_process.h" +#include "dyn_buffer.h" +#include "cros_log.h" + +static uint32_t getLen( DynBuffer *pkt ) +{ + uint32_t len; + const unsigned char *data = dynBufferGetCurrentData(pkt); + len = ROS_TO_HOST_UINT32(*(uint32_t *)data); + dynBufferMovePoseIndicator(pkt,sizeof(uint32_t)); + + return len; +} + +static uint32_t pushBackField( DynBuffer *pkt, TcprosTagStrDim *tag, const char *val ) +{ + size_t val_len = strlen( val ); + uint32_t out_len, field_len = tag->dim + val_len; + //PRINT_VDEBUG("pushBackField() : filed : %s field_len ; %d\n", tag->str, field_len); + out_len = HOST_TO_ROS_UINT32( field_len ); + dynBufferPushBackUInt32( pkt, out_len ); + dynBufferPushBackBuf( pkt, (const unsigned char*)tag->str, tag->dim ); + dynBufferPushBackBuf( pkt, (const unsigned char*)val, val_len ); + + return field_len + sizeof( uint32_t ); +} + +static void printPacket( DynBuffer *pkt, int print_data ) +{ + /* Save position indicator: it will be restored */ + int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( pkt ); + dynBufferRewindPoseIndicator ( pkt ); + + uint32_t bytes_to_read = getLen( pkt ); + + fprintf(cRosOutStreamGet(),"Header len %d\n",bytes_to_read); + while ( bytes_to_read > 0) + { + uint32_t field_len = getLen( pkt ); + const char *field = (const char *)dynBufferGetCurrentData( pkt ); + if( field_len ) + { + fwrite ( field, 1, field_len, stdout ); + fprintf(cRosOutStreamGet(),"\n" ); + dynBufferMovePoseIndicator( pkt, field_len ); + } + bytes_to_read -= ( sizeof(uint32_t) + field_len ); + } + + if( print_data ) + { + bytes_to_read = getLen( pkt ); + + fprintf(cRosOutStreamGet(),"Data len %d\n",bytes_to_read); + while ( bytes_to_read > 0) + { + uint32_t field_len = getLen( pkt ); + const char *field = (const char *)dynBufferGetCurrentData( pkt ); + if( field_len ) + { + fwrite ( field, 1, field_len, stdout ); + fprintf(cRosOutStreamGet(),"\n"); + dynBufferMovePoseIndicator( pkt, field_len ); + } + bytes_to_read -= ( sizeof(uint32_t) + field_len ); + } + } + + /* Restore position indicator */ + dynBufferSetPoseIndicator ( pkt, initial_pos_idx ); +} + +static TcprosParserState readSubcriptionHeader( TcprosProcess *p, uint32_t *flags ) +{ + PRINT_VVDEBUG("readSubcriptioHeader()\n"); + DynBuffer *packet = &(p->packet); + uint32_t bytes_to_read = getLen( packet ); + size_t packet_len = dynBufferGetSize( packet ); + + if( bytes_to_read > packet_len - sizeof( uint32_t ) ) + return TCPROS_PARSER_HEADER_INCOMPLETE; + + *flags = 0x0; + + PRINT_VDEBUG("readSubcriptioHeader() : Header len=%d\n",bytes_to_read); + + while ( bytes_to_read > 0) + { + uint32_t field_len = getLen( packet ); + + PRINT_VDEBUG("readSubcriptioHeader() : Field len=%d\n",field_len); + + const char *field = (const char *)dynBufferGetCurrentData( packet ); + if( field_len ) + { + if ( field_len > (uint32_t)TCPROS_CALLERID_TAG.dim && + strncmp ( field, TCPROS_CALLERID_TAG.str, TCPROS_CALLERID_TAG.dim ) == 0 ) + { + field += TCPROS_CALLERID_TAG.dim; + + dynStringReplaceWithStrN( &(p->caller_id), field, + field_len - TCPROS_CALLERID_TAG.dim ); + *flags |= TCPROS_CALLER_ID_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TOPIC_TAG.dim && + strncmp ( field, TCPROS_TOPIC_TAG.str, TCPROS_TOPIC_TAG.dim ) == 0 ) + { + field += TCPROS_TOPIC_TAG.dim; + + dynStringReplaceWithStrN( &(p->topic), field, + field_len - TCPROS_TOPIC_TAG.dim ); + *flags |= TCPROS_TOPIC_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TYPE_TAG.dim && + strncmp ( field, TCPROS_TYPE_TAG.str, TCPROS_TYPE_TAG.dim ) == 0 ) + { + field += TCPROS_TYPE_TAG.dim; + + dynStringReplaceWithStrN( &(p->type), field, + field_len - TCPROS_TYPE_TAG.dim ); + *flags |= TCPROS_TYPE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_MD5SUM_TAG.dim && + strncmp ( field, TCPROS_MD5SUM_TAG.str, TCPROS_MD5SUM_TAG.dim ) == 0 ) + { + field += TCPROS_MD5SUM_TAG.dim; + + dynStringReplaceWithStrN( &(p->md5sum), field, + field_len - TCPROS_MD5SUM_TAG.dim ); + *flags |= TCPROS_MD5SUM_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_MESSAGE_DEFINITION_TAG.dim && + strncmp ( field, TCPROS_MESSAGE_DEFINITION_TAG.str, TCPROS_MESSAGE_DEFINITION_TAG.dim ) == 0 ) + { + *flags |= TCPROS_MESSAGE_DEFINITION_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TCP_NODELAY_TAG.dim && + strncmp ( field, TCPROS_TCP_NODELAY_TAG.str, TCPROS_TCP_NODELAY_TAG.dim ) == 0 ) + { + field += TCPROS_TCP_NODELAY_TAG.dim; + p->tcp_nodelay = (*field == '1')?1:0; + *flags |= TCPROS_TCP_NODELAY_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_LATCHING_TAG.dim && + strncmp ( field, TCPROS_LATCHING_TAG.str, TCPROS_LATCHING_TAG.dim ) == 0 ) + { + PRINT_INFO("readSubcriptioHeader() WARNING : TCPROS_LATCHING_TAG not implemented\n"); + field += TCPROS_LATCHING_TAG.dim; + p->latching = (*field == '1')?1:0; + *flags |= TCPROS_LATCHING_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_ERROR_TAG.dim && + strncmp ( field, TCPROS_ERROR_TAG.str, TCPROS_ERROR_TAG.dim ) == 0 ) + { + PRINT_INFO("readSubcriptioHeader() WARNING : TCPROS_ERROR_TAG not implemented\n"); + *flags |= TCPROS_ERROR_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else + { + PRINT_ERROR("readSubcriptioHeader() : unknown field\n"); + *flags = 0x0; + break; + } + } + + bytes_to_read -= ( sizeof(uint32_t) + field_len ); + } + + return TCPROS_PARSER_DONE; + +} + +static TcprosParserState readPublicationHeader( TcprosProcess *p, uint32_t *flags ) +{ + PRINT_VVDEBUG("readPublicationHeader()\n"); + DynBuffer *packet = &(p->packet); + size_t bytes_to_read = dynBufferGetSize( packet ); + + *flags = 0x0; + + PRINT_VDEBUG("readPublicationHeader() : Header len=%lu\n", (long unsigned)bytes_to_read); + + while ( bytes_to_read > 0) + { + uint32_t field_len = getLen( packet ); + + PRINT_VDEBUG("readPublicationHeader() : Field len=%d\n",field_len); + + const char *field = (const char *)dynBufferGetCurrentData( packet ); + if( field_len ) + { + // http://wiki.ros.org/ROS/TCPROS doesn't mention it but it's sent anyway in ros groovy + if ( field_len > (uint32_t)TCPROS_MESSAGE_DEFINITION_TAG.dim && + strncmp ( field, TCPROS_MESSAGE_DEFINITION_TAG.str, TCPROS_MESSAGE_DEFINITION_TAG.dim ) == 0 ) + { + *flags |= TCPROS_MESSAGE_DEFINITION_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } else if ( field_len > (uint32_t)TCPROS_CALLERID_TAG.dim && + strncmp ( field, TCPROS_CALLERID_TAG.str, TCPROS_CALLERID_TAG.dim ) == 0 ) + { + field += TCPROS_CALLERID_TAG.dim; + + dynStringReplaceWithStrN( &(p->caller_id), field, + field_len - TCPROS_CALLERID_TAG.dim ); + *flags |= TCPROS_CALLER_ID_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TYPE_TAG.dim && + strncmp ( field, TCPROS_TYPE_TAG.str, TCPROS_TYPE_TAG.dim ) == 0 ) + { + field += TCPROS_TYPE_TAG.dim; + + dynStringReplaceWithStrN( &(p->type), field, + field_len - TCPROS_TYPE_TAG.dim ); + *flags |= TCPROS_TYPE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_MD5SUM_TAG.dim && + strncmp ( field, TCPROS_MD5SUM_TAG.str, TCPROS_MD5SUM_TAG.dim ) == 0 ) + { + field += TCPROS_MD5SUM_TAG.dim; + + dynStringReplaceWithStrN( &(p->md5sum), field, + field_len - TCPROS_MD5SUM_TAG.dim ); + *flags |= TCPROS_MD5SUM_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_LATCHING_TAG.dim && + strncmp ( field, TCPROS_LATCHING_TAG.str, TCPROS_LATCHING_TAG.dim ) == 0 ) + { + PRINT_INFO("readPublicationHeader() WARNING : TCPROS_LATCHING_TAG not implemented\n"); + field += TCPROS_LATCHING_TAG.dim; + p->latching = (*field == '1')?1:0; + *flags |= TCPROS_LATCHING_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TOPIC_TAG.dim && + strncmp ( field, TCPROS_TOPIC_TAG.str, TCPROS_TOPIC_TAG.dim ) == 0 ) + { + // http://wiki.ros.org/ROS/TCPROS doesn't mention it but it's sent anyway in ros groovy + field += TCPROS_TOPIC_TAG.dim; + + dynStringReplaceWithStrN( &(p->topic), field, + field_len - TCPROS_TOPIC_TAG.dim ); + *flags |= TCPROS_TOPIC_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_ERROR_TAG.dim && + strncmp ( field, TCPROS_ERROR_TAG.str, TCPROS_ERROR_TAG.dim ) == 0 ) + { + PRINT_INFO("readPublicationHeader() WARNING : TCPROS_ERROR_TAG not implemented\n"); + *flags |= TCPROS_ERROR_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TCP_NODELAY_TAG.dim && + strncmp ( field, TCPROS_TCP_NODELAY_TAG.str, TCPROS_TCP_NODELAY_TAG.dim ) == 0 ) + { + field += TCPROS_TCP_NODELAY_TAG.dim; + p->tcp_nodelay = (*field == '1')?1:0; + *flags |= TCPROS_TCP_NODELAY_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else + { + PRINT_ERROR("readPublicationHeader() : unknown field\n"); + *flags = 0x0; + break; + } + } + + bytes_to_read -= ( sizeof(uint32_t) + field_len ); + } + + return TCPROS_PARSER_DONE; +} + +TcprosParserState cRosMessageParseSubcriptionHeader( CrosNode *n, int server_idx ) +{ + PRINT_VVDEBUG("cRosMessageParseSubcriptionHeader()\n"); + + TcprosProcess *server_proc = &(n->tcpros_server_proc[server_idx]); + DynBuffer *packet = &(server_proc->packet); + + // Save position indicator: it will be restored + int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( packet ); + dynBufferRewindPoseIndicator ( packet ); + + uint32_t header_flags; + TcprosParserState ret = readSubcriptionHeader( server_proc, &header_flags ); + if( ret != TCPROS_PARSER_DONE ) + return ret; + + if( TCPROS_SUBCRIPTION_HEADER_FLAGS != ( header_flags&TCPROS_SUBCRIPTION_HEADER_FLAGS) ) + { + PRINT_ERROR("cRosMessageParseSubcriptionHeader() : There are missing fields in the received header.\n"); + ret = TCPROS_PARSER_ERROR; + } + else + { + int topic_found = 0; + int i = 0; + for( i = 0 ; i < n->n_pubs; i++) + { + PublisherNode *pub = &n->pubs[i]; + if (pub->topic_name == NULL) + continue; + + //printf("cRosMessageParseSubcriptionHeader() : checking if the topic in received header is what we expect: Topic name: %s Topic type: %s MD5: %s.\n", pub->topic_name, pub->topic_type, pub->md5sum); + + if( strcmp(pub->topic_name, dynStringGetData(&(server_proc->topic))) == 0 && + (strcmp(pub->topic_type, dynStringGetData(&(server_proc->type))) == 0 || strcmp(dynStringGetData(&(server_proc->type)), "*") == 0) && + (strcmp(pub->md5sum, dynStringGetData(&(server_proc->md5sum))) == 0 || strcmp(dynStringGetData(&(server_proc->md5sum)), "*") == 0)) + { + int list_elem; + + topic_found = 1; + server_proc->topic_idx = i; // Assign a topic (publisher index) to the TCPROS process + // Add the TcprosProcess index to the Publisher + for(list_elem=0;pub->tcpros_id_list[list_elem]!=-1;list_elem++); // Locate the list end + pub->tcpros_id_list[list_elem] = server_idx; + pub->tcpros_id_list[list_elem+1] = -1; // Set a new list end (sentinel) + break; + } + } + + if( ! topic_found ) + { + PRINT_ERROR("cRosMessageParseSubcriptionHeader() : Wrong service, type or md5sum in the received header. " + "Received: Topic name: %s Topic type: %s MD5: %s.\n", \ + dynStringGetData(&(server_proc->topic)), dynStringGetData(&(server_proc->type)), dynStringGetData(&(server_proc->md5sum))); + server_proc->topic_idx = -1; + ret = TCPROS_PARSER_ERROR; + } + else + { + if(server_proc->tcp_nodelay) + tcpIpSocketSetNoDelay(&server_proc->socket); + } + } + + // Restore position indicator + dynBufferSetPoseIndicator ( packet, initial_pos_idx ); + + return ret; +} + +TcprosParserState cRosMessageParsePublicationHeader( CrosNode *n, int client_idx ) +{ + PRINT_VVDEBUG("cRosMessageParsePublicationHeader()\n"); + + TcprosProcess *client_proc = &(n->tcpros_client_proc[client_idx]); + DynBuffer *packet = &(client_proc->packet); + + /* Save position indicator: it will be restored */ + int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( packet ); + dynBufferRewindPoseIndicator ( packet ); + + uint32_t header_flags; + TcprosParserState ret = readPublicationHeader( client_proc, &header_flags ); + if( ret != TCPROS_PARSER_DONE ) + return ret; + + if( TCPROS_PUBLICATION_HEADER_FLAGS != ( header_flags&TCPROS_PUBLICATION_HEADER_FLAGS) ) + { + PRINT_ERROR("cRosMessageParsePublicationHeader() : Missing fields\n"); + ret = TCPROS_PARSER_ERROR; + } + else + { + int subscriber_found = 0; + int i = 0; + for( i = 0 ; i < n->n_subs; i++) + { + SubscriberNode *sub = &n->subs[i]; + if (sub->topic_name == NULL) + continue; + + if( strcmp(sub->topic_type, dynStringGetData(&(client_proc->type))) == 0 && + strcmp(sub->md5sum, dynStringGetData(&(client_proc->md5sum))) == 0) + { + subscriber_found = 1; + break; + } + } + + if( ! subscriber_found ) + { + PRINT_ERROR("cRosMessageParsePublicationHeader() : Wrong topic, type or md5sum\n"); + ret = TCPROS_PARSER_ERROR; + } + else + { + if(client_proc->tcp_nodelay) + tcpIpSocketSetNoDelay(&client_proc->socket); // Not necessary really because subscribers do not write massage packets (only read) + } + } + + /* Restore position indicator */ + dynBufferSetPoseIndicator ( packet, initial_pos_idx ); + + return ret; +} + +void cRosMessagePrepareSubcriptionHeader( CrosNode *n, int client_idx ) +{ + PRINT_VVDEBUG("cRosMessagePrepareSubcriptionHeader()\n"); + + TcprosProcess *client_proc = &(n->tcpros_client_proc[client_idx]); + int sub_idx = client_proc->topic_idx; + DynBuffer *packet = &(client_proc->packet); + uint32_t header_len = 0, header_out_len = 0; + dynBufferPushBackUInt32( packet, header_out_len ); + + header_len += pushBackField( packet, &TCPROS_MESSAGE_DEFINITION_TAG, n->subs[sub_idx].message_definition ); + header_len += pushBackField( packet, &TCPROS_CALLERID_TAG, n->name ); + header_len += pushBackField( packet, &TCPROS_TOPIC_TAG, n->subs[sub_idx].topic_name ); + header_len += pushBackField( packet, &TCPROS_MD5SUM_TAG, n->subs[sub_idx].md5sum ); + header_len += pushBackField( packet, &TCPROS_TYPE_TAG, n->subs[sub_idx].topic_type ); + if(n->subs[sub_idx].tcp_nodelay) + header_len += pushBackField( packet, &TCPROS_TCP_NODELAY_TAG, "1" ); + + header_out_len= HOST_TO_ROS_UINT32( header_len ); + uint32_t *header_len_p = (uint32_t *)dynBufferGetData( packet ); + *header_len_p = header_out_len; +} + +cRosErrCodePack cRosMessageParsePublicationPacket( CrosNode *n, int client_idx ) +{ + cRosErrCodePack ret_err; + SubscriberNode *sub_node; + TcprosProcess *client_proc; + DynBuffer *packet; + void *data_context; + + client_proc = &(n->tcpros_client_proc[client_idx]); + packet = &(client_proc->packet); + sub_node = &n->subs[client_proc->topic_idx]; + data_context = sub_node->context; + + if(cRosMessageQueueVacancies(&sub_node->msg_queue) == 0) + sub_node->msg_queue_overflow = 1; // No space in the queue for the new message + + ret_err = cRosNodeDeserializeIncomingPacket(packet, data_context); + if(ret_err == CROS_SUCCESS_ERR_PACK) + ret_err = cRosNodeSubscriberCallback(data_context); // Calls the subscriber application-defined callback + else + cRosPrintErrCodePack(ret_err, "cRosNodeSubscriberCallback() failed decoding the received packet"); + + return ret_err; +} + +void cRosMessagePreparePublicationHeader( CrosNode *n, int server_idx ) +{ + PRINT_VVDEBUG("cRosMessagePreparePublicationHeader()\n"); + + TcprosProcess *server_proc = &(n->tcpros_server_proc[server_idx]); + int pub_idx = server_proc->topic_idx; + DynBuffer *packet = &(server_proc->packet); + uint32_t header_len = 0, header_out_len = 0; + dynBufferPushBackUInt32( packet, header_out_len ); + + // http://wiki.ros.org/ROS/TCPROS doesn't mention to send message_definition and topic_name + // but they are sent anyway in ros groovy + header_len += pushBackField( packet, &TCPROS_MESSAGE_DEFINITION_TAG, n->pubs[pub_idx].message_definition ); + header_len += pushBackField( packet, &TCPROS_CALLERID_TAG, n->name ); + header_len += pushBackField( packet, &TCPROS_LATCHING_TAG, "1" ); + header_len += pushBackField( packet, &TCPROS_MD5SUM_TAG, n->pubs[pub_idx].md5sum ); + header_len += pushBackField( packet, &TCPROS_TOPIC_TAG, n->pubs[pub_idx].topic_name ); + header_len += pushBackField( packet, &TCPROS_TYPE_TAG, n->pubs[pub_idx].topic_type ); + header_len += pushBackField( packet, &TCPROS_TCP_NODELAY_TAG, (server_proc->tcp_nodelay)?"1":"0" ); + + header_out_len = HOST_TO_ROS_UINT32( header_len ); + uint32_t *header_len_p = (uint32_t *)dynBufferGetData( packet ); + *header_len_p = header_out_len; +} + +cRosErrCodePack cRosMessagePreparePublicationPacket( CrosNode *node, int server_idx ) +{ + cRosErrCodePack ret_err; + PublisherNode *pub_node; + TcprosProcess *server_proc; + int pub_idx; + DynBuffer *packet; + uint32_t *packet_data_size_ptr; + uint32_t packet_size; + PRINT_VVDEBUG("cRosMessagePreparePublicationPacket()\n"); + + server_proc = &(node->tcpros_server_proc[server_idx]); + pub_idx = server_proc->topic_idx; + packet = &(server_proc->packet); + dynBufferPushBackUInt32( packet, 0 ); // Placeholder for packet size + + pub_node = &node->pubs[pub_idx]; + + ret_err = cRosNodeSerializeOutgoingMessage(packet, pub_node->context); + + packet_size = (uint32_t)dynBufferGetSize(packet) - sizeof(uint32_t); + packet_data_size_ptr = (uint32_t *)dynBufferGetData(packet); + *packet_data_size_ptr = packet_size; + + return ret_err; +} + +static TcprosParserState readServiceCallHeader( TcprosProcess *p, uint32_t *flags ) +{ + PRINT_VVDEBUG("readServiceCallHeader()\n"); + DynBuffer *packet = &(p->packet); + size_t bytes_to_read = dynBufferGetSize( packet ); + + *flags = 0x0; + + PRINT_VDEBUG("readServiceCallHeader() : Header len=%lu\n", (long unsigned)bytes_to_read); + + while ( bytes_to_read > 0) + { + uint32_t field_len = getLen( packet ); + + PRINT_VDEBUG("readServiceCallHeader() : Field len=%d\n",field_len); + + const char *field = (const char *)dynBufferGetCurrentData( packet ); + + if( field_len ) + { + if ( field_len > (uint32_t)TCPROS_CALLERID_TAG.dim && + strncmp ( field, TCPROS_CALLERID_TAG.str, TCPROS_CALLERID_TAG.dim ) == 0 ) + { + field += TCPROS_CALLERID_TAG.dim; + + dynStringReplaceWithStrN( &(p->caller_id), field, + field_len - TCPROS_CALLERID_TAG.dim ); + *flags |= TCPROS_CALLER_ID_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TYPE_TAG.dim && + strncmp ( field, TCPROS_TYPE_TAG.str, TCPROS_TYPE_TAG.dim ) == 0 ) + { + field += TCPROS_TYPE_TAG.dim; + + dynStringReplaceWithStrN( &(p->type), field, + field_len - TCPROS_TYPE_TAG.dim ); + *flags |= TCPROS_TYPE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len >= (uint32_t)TCPROS_EMPTY_MD5SUM_TAG.dim && + strncmp ( field, TCPROS_EMPTY_MD5SUM_TAG.str, TCPROS_EMPTY_MD5SUM_TAG.dim ) == 0 ) + { + *flags |= TCPROS_EMPTY_MD5SUM_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_MD5SUM_TAG.dim && + strncmp ( field, TCPROS_MD5SUM_TAG.str, TCPROS_MD5SUM_TAG.dim ) == 0 ) + { + field += TCPROS_MD5SUM_TAG.dim; + + dynStringReplaceWithStrN( &(p->md5sum), field, + field_len - TCPROS_MD5SUM_TAG.dim ); + *flags |= TCPROS_MD5SUM_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_SERVICE_TAG.dim && + strncmp ( field, TCPROS_SERVICE_TAG.str, TCPROS_SERVICE_TAG.dim ) == 0 ) + { + field += TCPROS_SERVICE_TAG.dim; + + dynStringReplaceWithStrN( &(p->service), field, + field_len - TCPROS_SERVICE_TAG.dim ); + *flags |= TCPROS_SERVICE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_PERSISTENT_TAG.dim && + strncmp ( field, TCPROS_PERSISTENT_TAG.str, TCPROS_PERSISTENT_TAG.dim ) == 0 ) + { + //PRINT_INFO("readPublicationHeader() WARNING : TCPROS_PERSISTENT_TAG not implemented\n"); + field += TCPROS_PERSISTENT_TAG.dim; + p->persistent = (*field == '1')?1:0; + *flags |= TCPROS_PERSISTENT_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_PROBE_TAG.dim && + strncmp ( field, TCPROS_PROBE_TAG.str, TCPROS_PROBE_TAG.dim ) == 0 ) + { + //PRINT_INFO("readServiceCallHeader() WARNING : TCPROS_PROBE_TAG implemented only\n"); + field += TCPROS_PROBE_TAG.dim; + p->probe = (*field == '1')?1:0; + *flags |= TCPROS_PROBE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_ERROR_TAG.dim && + strncmp ( field, TCPROS_ERROR_TAG.str, TCPROS_ERROR_TAG.dim ) == 0 ) + { + PRINT_INFO("readServiceCallHeader() WARNING : TCPROS_ERROR_TAG not implemented\n"); + *flags |= TCPROS_ERROR_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_MESSAGE_DEFINITION_TAG.dim && + strncmp ( field, TCPROS_MESSAGE_DEFINITION_TAG.str, TCPROS_MESSAGE_DEFINITION_TAG.dim ) == 0 ) + { + PRINT_INFO("readServiceCallHeader() WARNING : TCPROS_MESSAGE_DEFINITION_TAG not implemented\n"); + *flags |= TCPROS_MESSAGE_DEFINITION_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TCP_NODELAY_TAG.dim && + strncmp ( field, TCPROS_TCP_NODELAY_TAG.str, TCPROS_TCP_NODELAY_TAG.dim ) == 0 ) + { + field += TCPROS_TCP_NODELAY_TAG.dim; + p->tcp_nodelay = (*field == '1')?1:0; + *flags |= TCPROS_TCP_NODELAY_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else + { + PRINT_ERROR("readServiceCallHeader() : unknown field\n"); + *flags = 0x0; + break; + } + } + bytes_to_read -= ( sizeof(uint32_t) + field_len ); + } + + return TCPROS_PARSER_DONE; +} + +TcprosParserState cRosMessageParseServiceCallerHeader( CrosNode *n, int server_idx) +{ + PRINT_VVDEBUG("cRosMessageParseServiceCallerHeader()\n"); + + TcprosProcess *server_proc = &(n->rpcros_server_proc[server_idx]); + DynBuffer *packet = &(server_proc->packet); + + /* Save position indicator: it will be restored */ + int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( packet ); + dynBufferRewindPoseIndicator ( packet ); + + uint32_t header_flags; + TcprosParserState ret = readServiceCallHeader( server_proc, &header_flags ); + if( ret != TCPROS_PARSER_DONE ) + return ret; + + int service_found = 0; + + if( header_flags == ( header_flags & TCPROS_SERVICECALL_HEADER_FLAGS) || header_flags == ( header_flags & TCPROS_SERVICECALL_MATLAB_HEADER_FLAGS) ) + { + int svc_name_match = 0; + int i = 0; + for( i = 0 ; i < n->n_service_providers; i++) + { + if( strcmp( n->service_providers[i].service_name, dynStringGetData(&(server_proc->service))) == 0) + { + svc_name_match = 1; + if(strcmp( n->service_providers[i].md5sum, dynStringGetData(&(server_proc->md5sum))) == 0) + { + service_found = 1; + server_proc->service_idx = i; + break; + } + } + } + if( ! service_found ) + { + if(svc_name_match == 0) + PRINT_ERROR("cRosMessageParseServiceCallerHeader() : Received a service call header specifying a unknown service name\n"); + else + PRINT_ERROR("cRosMessageParseServiceCallerHeader() : Received a service call header specifying a known service name with a wrong MD5 sum\n"); + } + } + else if( header_flags == ( header_flags & TCPROS_SERVICEPROBE_HEADER_FLAGS) || header_flags == ( header_flags & TCPROS_SERVICEPROBE_MATLAB_HEADER_FLAGS) ) + { + int i = 0; + for( i = 0 ; i < n->n_service_providers; i++) + { + if( strcmp( n->service_providers[i].service_name, dynStringGetData(&(server_proc->service))) == 0) + { + service_found = 1; + server_proc->service_idx = i; + break; + } + } + if( ! service_found ) + PRINT_ERROR("cRosMessageParseServiceCallerHeader() : Received a service probe header specifying a unknown service name\n"); + } + else + { + PRINT_ERROR("cRosMessageParseServiceCallerHeader() : Received a service call header missing fields\n"); + ret = TCPROS_PARSER_ERROR; + } + + if( ! service_found ) + { + server_proc->service_idx = -1; + ret = TCPROS_PARSER_ERROR; + } + else + { + if(server_proc->tcp_nodelay) + tcpIpSocketSetNoDelay(&server_proc->socket); + } + + /* Restore position indicator */ + dynBufferSetPoseIndicator ( packet, initial_pos_idx ); + + return ret; +} + +static TcprosParserState readServiceProvisionHeader( TcprosProcess *p, uint32_t *flags ) +{ + PRINT_VVDEBUG("readServiceProvisionHeader()\n"); + DynBuffer *packet = &(p->packet); + size_t bytes_to_read = dynBufferGetSize( packet ); + + *flags = 0x0; + + PRINT_VDEBUG("readServiceProvisionHeader() : Header len=%lu\n", (long unsigned)bytes_to_read); + + while ( bytes_to_read > 0) + { + uint32_t field_len = getLen( packet ); + + PRINT_VDEBUG("readServiceProvisionHeader() : Field len=%d\n",field_len); + + const char *field = (const char *)dynBufferGetCurrentData( packet ); + + if( field_len ) + { + if ( field_len > (uint32_t)TCPROS_CALLERID_TAG.dim && + strncmp ( field, TCPROS_CALLERID_TAG.str, TCPROS_CALLERID_TAG.dim ) == 0 ) + { + field += TCPROS_CALLERID_TAG.dim; + + dynStringReplaceWithStrN( &(p->caller_id), field, + field_len - TCPROS_CALLERID_TAG.dim ); + *flags |= TCPROS_CALLER_ID_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TYPE_TAG.dim && + strncmp ( field, TCPROS_TYPE_TAG.str, TCPROS_TYPE_TAG.dim ) == 0 ) + { + field += TCPROS_TYPE_TAG.dim; + + dynStringReplaceWithStrN( &(p->type), field, + field_len - TCPROS_TYPE_TAG.dim ); + *flags |= TCPROS_TYPE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len >= (uint32_t)TCPROS_EMPTY_MD5SUM_TAG.dim && + strncmp ( field, TCPROS_EMPTY_MD5SUM_TAG.str, TCPROS_EMPTY_MD5SUM_TAG.dim ) == 0 ) + { + *flags |= TCPROS_EMPTY_MD5SUM_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_MD5SUM_TAG.dim && + strncmp ( field, TCPROS_MD5SUM_TAG.str, TCPROS_MD5SUM_TAG.dim ) == 0 ) + { + field += TCPROS_MD5SUM_TAG.dim; + + dynStringReplaceWithStrN( &(p->md5sum), field, + field_len - TCPROS_MD5SUM_TAG.dim ); + *flags |= TCPROS_MD5SUM_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_SERVICE_TAG.dim && + strncmp ( field, TCPROS_SERVICE_TAG.str, TCPROS_SERVICE_TAG.dim ) == 0 ) + { + field += TCPROS_SERVICE_TAG.dim; + + dynStringReplaceWithStrN( &(p->service), field, field_len - TCPROS_SERVICE_TAG.dim ); + *flags |= TCPROS_SERVICE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_SERVICE_REQUESTTYPE_TAG.dim && + strncmp ( field, TCPROS_SERVICE_REQUESTTYPE_TAG.str, TCPROS_SERVICE_REQUESTTYPE_TAG.dim ) == 0 ) + { + field += TCPROS_SERVICE_REQUESTTYPE_TAG.dim; + dynStringReplaceWithStrN( &(p->servicerequest_type), field, field_len - TCPROS_SERVICE_REQUESTTYPE_TAG.dim ); + *flags |= TCPROS_SERVICE_REQUESTTYPE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_SERVICE_RESPONSETYPE_TAG.dim && + strncmp ( field, TCPROS_SERVICE_RESPONSETYPE_TAG.str, TCPROS_SERVICE_RESPONSETYPE_TAG.dim ) == 0 ) + { + field += TCPROS_SERVICE_RESPONSETYPE_TAG.dim; + dynStringReplaceWithStrN( &(p->serviceresponse_type), field, field_len - TCPROS_SERVICE_RESPONSETYPE_TAG.dim ); + *flags |= TCPROS_SERVICE_RESPONSETYPE_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_ERROR_TAG.dim && + strncmp ( field, TCPROS_ERROR_TAG.str, TCPROS_ERROR_TAG.dim ) == 0 ) + { + PRINT_INFO("readServiceProvisionHeader() WARNING : TCPROS_ERROR_TAG not implemented\n"); + *flags |= TCPROS_ERROR_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_MESSAGE_DEFINITION_TAG.dim && + strncmp ( field, TCPROS_MESSAGE_DEFINITION_TAG.str, TCPROS_MESSAGE_DEFINITION_TAG.dim ) == 0 ) + { + PRINT_INFO("readServiceProvisionHeader() WARNING : TCPROS_MESSAGE_DEFINITION_TAG not implemented\n"); + *flags |= TCPROS_MESSAGE_DEFINITION_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else if ( field_len > (uint32_t)TCPROS_TCP_NODELAY_TAG.dim && + strncmp ( field, TCPROS_TCP_NODELAY_TAG.str, TCPROS_TCP_NODELAY_TAG.dim ) == 0 ) + { + field += TCPROS_TCP_NODELAY_TAG.dim; + p->tcp_nodelay = (*field == '1')?1:0; + *flags |= TCPROS_TCP_NODELAY_FLAG; + dynBufferMovePoseIndicator( packet, field_len ); + } + else + { + PRINT_ERROR("readServiceProvisionHeader() : unknown field\n"); + *flags = 0x0; + break; + } + } + bytes_to_read -= ( sizeof(uint32_t) + field_len ); + } + + return TCPROS_PARSER_DONE; +} + +TcprosParserState cRosMessageParseServiceProviderHeader( CrosNode *n, int client_idx ) +{ + PRINT_VVDEBUG("cRosMessageParseServiceProviderHeader()\n"); + + TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); + DynBuffer *packet = &(client_proc->packet); + + /* Save position indicator: it will be restored */ + int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( packet ); + dynBufferRewindPoseIndicator ( packet ); + + uint32_t header_flags; + TcprosParserState ret = readServiceProvisionHeader( client_proc, &header_flags ); + if( ret != TCPROS_PARSER_DONE ) + return ret; + + if( TCPROS_SERVICEPROVISION_HEADER_FLAGS != ( header_flags&TCPROS_SERVICEPROVISION_HEADER_FLAGS) ) + { + PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Missing fields\n"); + ret = TCPROS_PARSER_ERROR; + } + else + { + ServiceCallerNode *svc_caller = &n->service_callers[client_proc->service_idx]; + + if (header_flags&TCPROS_SERVICE_FLAG && strcmp(svc_caller->service_name, dynStringGetData(&(client_proc->service))) != 0) + { + PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong service name from service provider\n"); + ret = TCPROS_PARSER_ERROR; + } + if(strcmp(svc_caller->md5sum, dynStringGetData(&(client_proc->md5sum))) != 0) + { + PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong MD5 sum from service provider\n"); + ret = TCPROS_PARSER_ERROR; + } + if(strcmp(svc_caller->service_type, dynStringGetData(&(client_proc->type))) != 0) + { + PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong service type from service provider\n"); + ret = TCPROS_PARSER_ERROR; + } + if(header_flags&TCPROS_SERVICE_REQUESTTYPE_FLAG && strcmp(svc_caller->servicerequest_type, dynStringGetData(&(client_proc->servicerequest_type))) != 0) + { + PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong service request type from service provider\n"); + ret = TCPROS_PARSER_ERROR; + } + if(header_flags&TCPROS_SERVICE_RESPONSETYPE_FLAG && strcmp(svc_caller->serviceresponse_type, dynStringGetData(&(client_proc->serviceresponse_type))) != 0) + { + PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong service response type from service provider\n"); + ret = TCPROS_PARSER_ERROR; + } + if(client_proc->tcp_nodelay) + tcpIpSocketSetNoDelay(&client_proc->socket); + } + + /* Restore position indicator */ + dynBufferSetPoseIndicator ( packet, initial_pos_idx ); + + return ret; +} + +void cRosMessagePrepareServiceCallHeader(CrosNode *n, int client_idx) +{ + PRINT_VVDEBUG("cRosMessagePrepareServiceCallHeader()\n"); + + TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); + int srv_idx = client_proc->service_idx; + DynBuffer *packet = &(client_proc->packet); + uint32_t header_len = 0, header_out_len = 0; + dynBufferPushBackUInt32( packet, header_out_len ); + + // Same format as MATLAB second header (not the probe one) + header_len += pushBackField( packet, &TCPROS_SERVICE_TAG, n->service_callers[srv_idx].service_name ); + header_len += pushBackField( packet, &TCPROS_MESSAGE_DEFINITION_TAG, n->service_callers[srv_idx].message_definition ); + header_len += pushBackField( packet, &TCPROS_CALLERID_TAG, n->name ); + header_len += pushBackField( packet, &TCPROS_MD5SUM_TAG, n->service_callers[srv_idx].md5sum ); + if(client_proc->persistent) + header_len += pushBackField( packet, &TCPROS_PERSISTENT_TAG, "1" ); + if(client_proc->tcp_nodelay) + header_len += pushBackField( packet, &TCPROS_TCP_NODELAY_TAG, "1" ); + + //header_len += pushBackField( packet, &TCPROS_SERVICE_REQUESTTYPE_TAG, n->service_callers[srv_idx].servicerequest_type ); + //header_len += pushBackField( packet, &TCPROS_SERVICE_RESPONSETYPE_TAG, n->service_callers[srv_idx].serviceresponse_type ); + header_len += pushBackField( packet, &TCPROS_TYPE_TAG, n->service_callers[srv_idx].service_type ); + + header_out_len = HOST_TO_ROS_UINT32( header_len ); + uint32_t *header_len_p = (uint32_t *)dynBufferGetData( packet ); + *header_len_p = header_out_len; +} + +cRosErrCodePack cRosMessagePrepareServiceCallPacket( CrosNode *n, int client_idx ) +{ + cRosErrCodePack ret_err; + + PRINT_VVDEBUG("cRosMessagePrepareServiceCallPacket()\n"); + TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); + int svc_idx = client_proc->service_idx; + DynBuffer *packet = &(client_proc->packet); + dynBufferPushBackUInt32( packet, 0 ); // Placehoder for packet size + + void* data_context = n->service_callers[svc_idx].context; + ret_err = cRosNodeSerializeOutgoingMessage(packet, data_context); // Serialize the call outgoing message into the outgoing packet + + uint32_t data_size = (uint32_t)dynBufferGetSize(packet) - sizeof(uint32_t); + uint32_t *packet_data_size_ptr = (uint32_t *)dynBufferGetData(packet); + *packet_data_size_ptr = data_size; + + return ret_err; +} + +cRosErrCodePack cRosMessageParseServiceResponsePacket( CrosNode *n, int client_idx ) +{ + cRosErrCodePack ret_err; + + TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); + DynBuffer *packet = &(client_proc->packet); + if(client_proc->ok_byte == TCPROS_OK_BYTE_SUCCESS) + { + int svc_idx = client_proc->service_idx; + void* data_context = n->service_callers[svc_idx].context; + + ret_err = cRosNodeDeserializeIncomingPacket(packet, data_context); // Deserialize the message response + + if(ret_err == CROS_SUCCESS_ERR_PACK) + ret_err = cRosNodeServiceCallerCallback(1, data_context); // Call the service-caller application-defined callback function to process the service response + } + else + { + size_t n_char; + PRINT_ERROR("cRosMessageParseServiceResponsePacket() : Error in service call response. 'ok' byte=%i. Error message='",client_proc->ok_byte); + for(n_char=0;n_charrpcros_server_proc[server_idx]); + int srv_idx = server_proc->service_idx; + DynBuffer *packet = &(server_proc->packet); + uint32_t header_len = 0, header_out_len = 0; + dynBufferPushBackUInt32( packet, header_out_len ); + + // http://wiki.ros.org/ROS/TCPROS doesn't mention to send message_definition and topic_name + // but they are sent anyway in ros groovy + header_len += pushBackField( packet, &TCPROS_CALLERID_TAG, n->name ); + header_len += pushBackField( packet, &TCPROS_MD5SUM_TAG, n->service_providers[srv_idx].md5sum ); + + //if(server_proc->probe) + //{ + header_len += pushBackField( packet, &TCPROS_SERVICE_REQUESTTYPE_TAG, n->service_providers[srv_idx].servicerequest_type ); + header_len += pushBackField( packet, &TCPROS_SERVICE_RESPONSETYPE_TAG, n->service_providers[srv_idx].serviceresponse_type ); + header_len += pushBackField( packet, &TCPROS_TYPE_TAG, n->service_providers[srv_idx].service_type ); + //} + + header_out_len = HOST_TO_ROS_UINT32( header_len ); + uint32_t *header_len_p = (uint32_t *)dynBufferGetData( packet ); + *header_len_p = header_out_len; +} + +cRosErrCodePack cRosMessagePrepareServiceResponsePacket( CrosNode *n, int server_idx) +{ + cRosErrCodePack ret_err; + uint8_t ok_byte; // OK field (byte size) of the service response packet + + PRINT_VVDEBUG("cRosMessageParseServiceArgumentsPacket()\n"); + TcprosProcess *server_proc = &(n->rpcros_server_proc[server_idx]); + DynBuffer *packet = &(server_proc->packet); + int srv_idx = server_proc->service_idx; + void* service_context = n->service_providers[srv_idx].context; + DynBuffer service_response; + dynBufferInit(&service_response); + + ret_err = cRosNodeDeserializeIncomingPacket(packet, service_context); // prepare the context incoming message used by the user callback function + if(ret_err == CROS_SUCCESS_ERR_PACK) + ret_err = cRosNodeServiceProviderCallback(service_context); // calls the service-provider application-defined callback function + else + cRosPrintErrCodePack(ret_err, "cRosMessagePrepareServiceResponsePacket() failed decoding the received packet"); + + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + ret_err = cRosNodeSerializeOutgoingMessage(&service_response, service_context); // Create the output packet from outgoing message in the context + if(ret_err != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(ret_err, "cRosNodeServiceProviderCallback() failed encoding the packet to send"); + } + + dynBufferClear(packet); // clear packet buffer + + if(ret_err == CROS_SUCCESS_ERR_PACK) + { + ok_byte = TCPROS_OK_BYTE_SUCCESS; + dynBufferPushBackBuf( packet, &ok_byte, sizeof(uint8_t) ); + dynBufferPushBackUInt32( packet, dynBufferGetSize(&service_response)); // data size field + dynBufferPushBackBuf( packet, dynBufferGetData(&service_response), dynBufferGetSize(&service_response)); // Response data + } + else + { + ok_byte = TCPROS_OK_BYTE_FAIL; + dynBufferPushBackBuf( packet, &ok_byte, sizeof(uint8_t) ); + dynBufferPushBackUInt32( packet, 0); // Serialize an error string of size 0: Just add the data size field + } + + dynBufferRelease(&service_response); + + return ret_err; +} diff --git a/src/hal/components/cros/src/dyn_buffer.c b/src/hal/components/cros/src/dyn_buffer.c new file mode 100644 index 00000000..8acad319 --- /dev/null +++ b/src/hal/components/cros/src/dyn_buffer.c @@ -0,0 +1,249 @@ +#include +#include + +#include "dyn_buffer.h" +#include "cros_defs.h" +#include "cros_log.h" + +enum { DYNBUFFER_INIT_SIZE = 256, DYNBUFFER_GROW_RATE = 2 }; + +void dynBufferInit ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferInit()\n" ); + + d_buf->data = NULL; + d_buf->size = 0; + d_buf->pos_offset = 0; + d_buf->max = 0; +} + +void dynBufferRelease ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferRelease()\n" ); + + if ( d_buf->data != NULL ) + free ( d_buf->data ); + + d_buf->size = 0; + d_buf->pos_offset = 0; + d_buf->max = 0; +} + +int dynBufferPushBackBuf ( DynBuffer *d_buf, const unsigned char *new_buf, size_t n ) +{ + PRINT_VVDEBUG ( "dynBufferPushBackBuf()\n" ); + + if (new_buf == NULL && n > 0) // If n == 0, the function accepts NULL as new_buf since nothing have to be appended + { + PRINT_ERROR ( "dynBufferPushBackBuf() : Invalid function argument values: new buffer content must be different from NULL and no shorter than 0\n" ); + return -1; + } + + if ( d_buf->data == NULL ) + { + PRINT_VVDEBUG ( "dynBufferPushBackBuf() : allocate memory for the first time\n" ); + d_buf->data = ( unsigned char * ) malloc ( DYNBUFFER_INIT_SIZE * sizeof ( unsigned char ) ); + + if ( d_buf->data == NULL ) + { + PRINT_ERROR ( "dynBufferPushBackBuf() : Can't allocate memory\n" ); + return -1; + } + + d_buf->size = 0; + d_buf->max = DYNBUFFER_INIT_SIZE; + } + + while ( d_buf->size + n > d_buf->max ) + { + PRINT_VVDEBUG ( "dynBufferPushBackBuf() : reallocate memory\n" ); + unsigned char *new_d_buf = ( unsigned char * ) realloc ( d_buf->data, ( DYNBUFFER_GROW_RATE * d_buf->max ) * + sizeof ( unsigned char ) ); + if ( new_d_buf == NULL ) + { + PRINT_ERROR ( "dynBufferPushBackBuf() : Can't allocate more memory\n" ); + return -1; + } + d_buf->max *= DYNBUFFER_GROW_RATE; + d_buf->data = new_d_buf; + } + + if(n>0) + { + memcpy ( ( void * ) ( d_buf->data + d_buf->size ), ( void * ) new_buf, n ); + d_buf->size += n; + } + + return d_buf->size; +} + +int dynBufferReplaceContent( DynBuffer *d_buf, const unsigned char *cont_buf, size_t cont_buf_len ) +{ + size_t ret_val; + + d_buf->size = d_buf->pos_offset; // Discard content beyond current position index (it will be replaced by cont_buf) + ret_val = dynBufferPushBackBuf(d_buf, cont_buf, cont_buf_len); + + return ret_val; +} + +int dynBufferPushBackInt8( DynBuffer *d_buf, int8_t val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( int8_t ) ); +} + +int dynBufferPushBackInt16( DynBuffer *d_buf, int16_t val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( int16_t ) ); +} + +int dynBufferPushBackInt32 ( DynBuffer *d_buf, int32_t val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( int32_t ) ); +} + +int dynBufferPushBackInt64( DynBuffer *d_buf, int64_t val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( int64_t ) ); +} + +int dynBufferPushBackUInt8( DynBuffer *d_buf, uint8_t val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( uint8_t ) ); +} + +int dynBufferPushBackUInt16( DynBuffer *d_buf, uint16_t val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( uint16_t ) ); +} + +int dynBufferPushBackUInt32 ( DynBuffer *d_buf, uint32_t val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( uint32_t ) ); +} + +int dynBufferPushBackUInt64( DynBuffer *d_buf, uint64_t val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( uint64_t ) ); +} + +int dynBufferPushBackFloat32( DynBuffer *d_buf, float val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( float ) ); +} + +int dynBufferPushBackFloat64( DynBuffer *d_buf, double val ) +{ + PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); + + return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( double ) ); +} + +void dynBufferClear ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferClear()\n" ); + + if ( d_buf->data == NULL ) + return; + + d_buf->size = 0; + d_buf->pos_offset = 0; +} + +size_t dynBufferGetSize ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferGetSize()\n" ); + + return d_buf->size; +} + +const unsigned char *dynBufferGetData ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferGetData()\n" ); + + return ( const unsigned char * ) d_buf->data; +} + +void dynBufferMovePoseIndicator ( DynBuffer *d_buf, int offset ) +{ + PRINT_VVDEBUG ( "dynBufferMovePoseIndicator()\n" ); + + size_t curr = d_buf->pos_offset; + if (offset > 0 && curr >= (d_buf->size - offset)) + d_buf->pos_offset = d_buf->size; + else if ( offset < 0 && (size_t)(-offset) >= curr) + d_buf->pos_offset = 0; + else + d_buf->pos_offset += (size_t)offset; +} + +void dynBufferSetPoseIndicator( DynBuffer *d_buf, size_t pos ) +{ + PRINT_VVDEBUG ( "dynBufferSetPoseIndicator()\n" ); + + if ( pos > d_buf->size ) + d_buf->pos_offset = d_buf->size; + else + d_buf->pos_offset = pos; +} + +void dynBufferRewindPoseIndicator ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferRewindPoseIndicator()\n" ); + + d_buf->pos_offset = 0; +} + +size_t dynBufferGetPoseIndicatorOffset ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferGetPoseIndicatorOffset()\n" ); + + return d_buf->pos_offset; +} + +const unsigned char *dynBufferGetCurrentData ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferGetCurrentData()\n" ); + + return ( const unsigned char * ) ( d_buf->data + d_buf->pos_offset ); +} + +int dynBufferGetCurrentContent ( unsigned char *cont_buf, DynBuffer *d_buf, size_t cont_buf_len ) +{ + int ret_err; + + if( d_buf->pos_offset + cont_buf_len > d_buf->size ) // There is not enough content in the dynamic buffer + ret_err=-1; + else + { + memcpy(cont_buf, d_buf->data + d_buf->pos_offset, cont_buf_len); + ret_err=0; + } + + return ret_err; +} + +size_t dynBufferGetRemainingDataSize ( DynBuffer *d_buf ) +{ + PRINT_VVDEBUG ( "dynBufferGetRemainingDataSize()\n" ); + + return d_buf->size - d_buf->pos_offset; +} diff --git a/src/hal/components/cros/src/dyn_string.c b/src/hal/components/cros/src/dyn_string.c new file mode 100644 index 00000000..5c3aad1c --- /dev/null +++ b/src/hal/components/cros/src/dyn_string.c @@ -0,0 +1,286 @@ +#include +#include + +#include "dyn_string.h" +#include "cros_defs.h" +#include "cros_log.h" + +enum { DYNSTRING_INIT_SIZE = 256, DYNSTRING_GROW_RATE = 2 }; + +void dynStringInit ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "dynStringInit()\n" ); + + d_str->data = NULL; + d_str->len = 0; + d_str->pos_offset = 0; + d_str->max = 0; +} + +void dynStringRelease ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "dynStringRelease()\n" ); + + if ( d_str->data != NULL ) + { + free ( d_str->data ); + d_str->data = NULL; + } + d_str->len = 0; + d_str->pos_offset = 0; + d_str->max = 0; +} + +int dynStringPushBackStr ( DynString *d_str, const char *new_str ) +{ + PRINT_VVDEBUG ( "dynStringPushBackStr()\n" ); + + return dynStringPushBackStrN ( d_str, new_str, strlen ( new_str ) ); +} + +int dynStringPushBackStrN ( DynString *d_str, const char *new_str, int n ) +{ + PRINT_VVDEBUG ( "dynStringPushBackStrN()\n" ); + + if ( new_str == NULL ) + { + PRINT_ERROR ( "dynStringPushBackStrN() : Invalid new string\n" ); + return -1; + } + + if ( d_str->data == NULL ) + { + PRINT_VVDEBUG ( "dynStringPushBackStrN() : allocate memory for the first time\n" ); + d_str->data = ( char * ) malloc ( (DYNSTRING_INIT_SIZE + n + 1) * sizeof ( char ) ); // Reserve one char at the end of the array (+1) to store the \0 terminating char + + if ( d_str->data == NULL ) + { + PRINT_ERROR ( "dynStringPushBackStrN() : Can't allocate memory\n" ); + return -1; + } + + d_str->len = 0; + d_str->max = DYNSTRING_INIT_SIZE + n; + } + else + if(d_str->len + n > d_str->max) + { + PRINT_VVDEBUG ( "dynStringPushBackStrN() : reallocate memory\n" ); + char *n_d_str = ( char * ) realloc ( d_str->data, ( DYNSTRING_GROW_RATE * d_str->max + n + 1) * sizeof ( char ) ); + if ( n_d_str == NULL ) + { + PRINT_ERROR ( "dynStringPushBackStrN() : Can't reallocate more memory\n" ); + return -1; + } + d_str->max = DYNSTRING_GROW_RATE * d_str->max + n; + d_str->data = n_d_str; + } + + memcpy ( ( void * ) ( d_str->data + d_str->len ), ( void * ) new_str, ( size_t ) n ); + d_str->len += n; + d_str->data[d_str->len] = '\0'; + + return d_str->len; +} + +int dynStringReplaceWithStrN ( DynString *d_str, const char *new_str, int n ) +{ + int new_len; + + d_str->len=0; // Clear old content + d_str->pos_offset=0; + new_len = dynStringPushBackStrN ( d_str, new_str, n ); + + return new_len; +} + +int dynStringPushBackChar ( DynString *d_str, const char c ) +{ + PRINT_VVDEBUG ( "dynStringPushBackChar()\n" ); + + if ( d_str->data == NULL ) + { + PRINT_VVDEBUG ( "dynStringPushBackChar() : allocate memory for the first time\n" ); + d_str->data = ( char * ) malloc ( (DYNSTRING_INIT_SIZE + 2) * sizeof ( char ) ); + + if ( d_str->data == NULL ) + { + PRINT_ERROR ( "dynStringPushBackChar() : Can't allocate memory\n" ); + return -1; + } + + d_str->data[0] = '\0'; + d_str->len = 0; + d_str->max = DYNSTRING_INIT_SIZE + 1; + } + else + if ( d_str->len + 1 > d_str->max ) // If there is not enogh space in the array to store a new char, allocate more meory + { + PRINT_VVDEBUG ( "dynStringPushBackChar() : reallocate memory\n" ); + char *n_d_str = ( char * ) realloc ( d_str->data, ( DYNSTRING_GROW_RATE * d_str->max + 2) * + sizeof ( char ) ); + if ( n_d_str == NULL ) + { + PRINT_ERROR ( "dynStringPushBackChar() : Can't allocate more memory\n" ); + return -1; + } + d_str->max = DYNSTRING_GROW_RATE * d_str->max + 1; + d_str->data = n_d_str; + } + + d_str->data[d_str->len] = c; + d_str->len += 1; + d_str->data[d_str->len] = '\0'; + + return d_str->len; +} + +int dynStringPatch ( DynString *d_str, const char *new_str, int pos ) +{ + PRINT_VVDEBUG ( "dynStringPatch()\n" ); + + if ( new_str == NULL ) + { + PRINT_ERROR ( "dynStringPatch() : Invalid new string\n" ); + return -1; + } + + if ( d_str->data == NULL ) + { + PRINT_ERROR ( "dynStringPatch() : Empty dynamic string (NULL data)\n" ); + return -1; + } + + if ( pos > d_str->len ) + { + PRINT_ERROR ( "dynStringPatch() : The starting position for the copy must be smaller \ + or equal to the current dynamic string lenght\n" ); + return -1; + } + + int new_str_len = strlen ( new_str ); + int str_final_len = pos + new_str_len; + + if ( str_final_len > d_str->max ) + { + PRINT_VVDEBUG ( "dynStringPatch() : reallocate memory\n" ); + char *n_d_str = ( char * ) realloc ( d_str->data, ( DYNSTRING_GROW_RATE * d_str->max + new_str_len + 1) * + sizeof ( char ) ); + if ( n_d_str == NULL ) + { + PRINT_ERROR ( "dynStringPatch() : Can't allocate more memory\n" ); + return -1; + } + d_str->max = DYNSTRING_GROW_RATE * d_str->max + new_str_len; + d_str->data = n_d_str; + } + + memcpy ( ( void * ) ( d_str->data + pos ), ( void * ) new_str, ( size_t ) new_str_len ); + + if ( str_final_len > d_str->len ) + { + d_str->data[str_final_len] = '\0'; + d_str->len = str_final_len; + } + + return d_str->len; +} + +void dynStringClear ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "dynStringClear()\n" ); + + if ( d_str->data == NULL ) + return; + + d_str->data[0] = '\0'; + d_str->pos_offset = 0; + d_str->len = 0; +} + +void dynStringReduce ( DynString *d_str, int rem_left, int rem_right) +{ + PRINT_VVDEBUG ( "dynStringReduce()\n" ); + + if ( d_str->data == NULL ) + return; + + if (rem_left > d_str->len) + rem_left = d_str->len; + if (rem_left + rem_right > d_str->len) + rem_right = d_str->len - rem_left; + + d_str->len -= rem_left + rem_right; + memmove(d_str->data, d_str->data+rem_left, d_str->len); + d_str->data[d_str->len] = '\0'; + d_str->pos_offset -= rem_left; + if(d_str->pos_offset < 0) + d_str->pos_offset = 0; + else + if(d_str->pos_offset > d_str->len) + d_str->pos_offset = d_str->len; +} + +int dynStringGetLen ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "dynStringGetLen()\n" ); + + return d_str->len; +} + +const char *dynStringGetData ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "getDynStringBuf()\n" ); + + return ( const char * ) d_str->data; +} + +void dynStringMovePoseIndicator ( DynString *d_str, int offset ) +{ + PRINT_VVDEBUG ( "dynStringMovePoseIndicator()\n" ); + + d_str->pos_offset += offset; + if ( d_str->pos_offset < 0 ) + d_str->pos_offset = 0; + if ( d_str->pos_offset > d_str->len ) + d_str->pos_offset = d_str->len; +} + +void dynStringSetPoseIndicator ( DynString *d_str, int pos ) +{ + PRINT_VVDEBUG ( "dynStringSetPoseIndicator()\n" ); + + d_str->pos_offset = pos; + if ( d_str->pos_offset < 0 ) + d_str->pos_offset = 0; + if ( d_str->pos_offset > d_str->len ) + d_str->pos_offset = d_str->len; +} + +void dynStringRewindPoseIndicator ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "dynStringRewindPoseIndicator()\n" ); + + d_str->pos_offset = 0; +} + +int dynStringGetPoseIndicatorOffset ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "dynStringGetPoseIndicatorOffset()\n" ); + + return d_str->pos_offset; +} + +const char *dynStringGetCurrentData ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "dynStringGetCurrentData()\n" ); + + return ( const char * ) ( d_str->data + d_str->pos_offset ); +} + +int dynStringGetRemainingDataSize ( DynString *d_str ) +{ + PRINT_VVDEBUG ( "dynStringGetRemainingDataSize()\n" ); + + return d_str->len - d_str->pos_offset; +} diff --git a/src/hal/components/cros/src/md5.c b/src/hal/components/cros/src/md5.c new file mode 100644 index 00000000..e7bb9a37 --- /dev/null +++ b/src/hal/components/cros/src/md5.c @@ -0,0 +1,296 @@ +/* + * This is an OpenSSL-compatible implementation of the RSA Data Security, Inc. + * MD5 Message-Digest Algorithm (RFC 1321). + * + * Homepage: + * http://openwall.info/wiki/people/solar/software/public-domain-source-code/md5 + * + * Author: + * Alexander Peslyak, better known as Solar Designer + * + * This software was written by Alexander Peslyak in 2001. No copyright is + * claimed, and the software is hereby placed in the public domain. + * In case this attempt to disclaim copyright and place the software in the + * public domain is deemed null and void, then the software is + * Copyright (c) 2001 Alexander Peslyak and it is hereby released to the + * general public under the following terms: + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted. + * + * There's ABSOLUTELY NO WARRANTY, express or implied. + * + * (This is a heavily cut-down "BSD license".) + * + * This differs from Colin Plumb's older public domain implementation in that + * no exactly 32-bit integer data type is required (any 32-bit or wider + * unsigned integer data type will do), there's no compile-time endianness + * configuration, and the function prototypes match OpenSSL's. No code from + * Colin Plumb's implementation has been reused; this comment merely compares + * the properties of the two independent implementations. + * + * The primary goals of this implementation are portability and ease of use. + * It is meant to be fast, but not as fast as possible. Some known + * optimizations are not included to reduce source code size and avoid + * compile-time configuration. + */ + +#ifndef HAVE_OPENSSL + +#include + +#include "md5.h" + +/* + * The basic MD5 functions. + * + * F and G are optimized compared to their RFC 1321 definitions for + * architectures that lack an AND-NOT instruction, just like in Colin Plumb's + * implementation. + */ +#define F(x, y, z) ((z) ^ ((x) & ((y) ^ (z)))) +#define G(x, y, z) ((y) ^ ((z) & ((x) ^ (y)))) +#define H(x, y, z) (((x) ^ (y)) ^ (z)) +#define H2(x, y, z) ((x) ^ ((y) ^ (z))) +#define I(x, y, z) ((y) ^ ((x) | ~(z))) + +/* + * The MD5 transformation for all four rounds. + */ +#define STEP(f, a, b, c, d, x, t, s) \ + (a) += f((b), (c), (d)) + (x) + (t); \ + (a) = (((a) << (s)) | (((a) & 0xffffffff) >> (32 - (s)))); \ + (a) += (b); + +/* + * SET reads 4 input bytes in little-endian byte order and stores them + * in a properly aligned word in host byte order. + * + * The check for little-endian architectures that tolerate unaligned + * memory accesses is just an optimization. Nothing will break if it + * doesn't work. + */ +#if defined(__i386__) || defined(__x86_64__) || defined(__vax__) +#define SET(n) \ + (*(MD5_u32plus *)&ptr[(n) * 4]) +#define GET(n) \ + SET(n) +#else +#define SET(n) \ + (ctx->block[(n)] = \ + (MD5_u32plus)ptr[(n) * 4] | \ + ((MD5_u32plus)ptr[(n) * 4 + 1] << 8) | \ + ((MD5_u32plus)ptr[(n) * 4 + 2] << 16) | \ + ((MD5_u32plus)ptr[(n) * 4 + 3] << 24)) +#define GET(n) \ + (ctx->block[(n)]) +#endif + +/* + * This processes one or more 64-byte data blocks, but does NOT update + * the bit counters. There are no alignment requirements. + */ +static const void *body(MD5_CTX *ctx, const void *data, unsigned long size) +{ + const unsigned char *ptr; + MD5_u32plus a, b, c, d; + MD5_u32plus saved_a, saved_b, saved_c, saved_d; + + ptr = (const unsigned char *)data; + + a = ctx->a; + b = ctx->b; + c = ctx->c; + d = ctx->d; + + do { + saved_a = a; + saved_b = b; + saved_c = c; + saved_d = d; + +/* Round 1 */ + STEP(F, a, b, c, d, SET(0), 0xd76aa478, 7) + STEP(F, d, a, b, c, SET(1), 0xe8c7b756, 12) + STEP(F, c, d, a, b, SET(2), 0x242070db, 17) + STEP(F, b, c, d, a, SET(3), 0xc1bdceee, 22) + STEP(F, a, b, c, d, SET(4), 0xf57c0faf, 7) + STEP(F, d, a, b, c, SET(5), 0x4787c62a, 12) + STEP(F, c, d, a, b, SET(6), 0xa8304613, 17) + STEP(F, b, c, d, a, SET(7), 0xfd469501, 22) + STEP(F, a, b, c, d, SET(8), 0x698098d8, 7) + STEP(F, d, a, b, c, SET(9), 0x8b44f7af, 12) + STEP(F, c, d, a, b, SET(10), 0xffff5bb1, 17) + STEP(F, b, c, d, a, SET(11), 0x895cd7be, 22) + STEP(F, a, b, c, d, SET(12), 0x6b901122, 7) + STEP(F, d, a, b, c, SET(13), 0xfd987193, 12) + STEP(F, c, d, a, b, SET(14), 0xa679438e, 17) + STEP(F, b, c, d, a, SET(15), 0x49b40821, 22) + +/* Round 2 */ + STEP(G, a, b, c, d, GET(1), 0xf61e2562, 5) + STEP(G, d, a, b, c, GET(6), 0xc040b340, 9) + STEP(G, c, d, a, b, GET(11), 0x265e5a51, 14) + STEP(G, b, c, d, a, GET(0), 0xe9b6c7aa, 20) + STEP(G, a, b, c, d, GET(5), 0xd62f105d, 5) + STEP(G, d, a, b, c, GET(10), 0x02441453, 9) + STEP(G, c, d, a, b, GET(15), 0xd8a1e681, 14) + STEP(G, b, c, d, a, GET(4), 0xe7d3fbc8, 20) + STEP(G, a, b, c, d, GET(9), 0x21e1cde6, 5) + STEP(G, d, a, b, c, GET(14), 0xc33707d6, 9) + STEP(G, c, d, a, b, GET(3), 0xf4d50d87, 14) + STEP(G, b, c, d, a, GET(8), 0x455a14ed, 20) + STEP(G, a, b, c, d, GET(13), 0xa9e3e905, 5) + STEP(G, d, a, b, c, GET(2), 0xfcefa3f8, 9) + STEP(G, c, d, a, b, GET(7), 0x676f02d9, 14) + STEP(G, b, c, d, a, GET(12), 0x8d2a4c8a, 20) + +/* Round 3 */ + STEP(H, a, b, c, d, GET(5), 0xfffa3942, 4) + STEP(H2, d, a, b, c, GET(8), 0x8771f681, 11) + STEP(H, c, d, a, b, GET(11), 0x6d9d6122, 16) + STEP(H2, b, c, d, a, GET(14), 0xfde5380c, 23) + STEP(H, a, b, c, d, GET(1), 0xa4beea44, 4) + STEP(H2, d, a, b, c, GET(4), 0x4bdecfa9, 11) + STEP(H, c, d, a, b, GET(7), 0xf6bb4b60, 16) + STEP(H2, b, c, d, a, GET(10), 0xbebfbc70, 23) + STEP(H, a, b, c, d, GET(13), 0x289b7ec6, 4) + STEP(H2, d, a, b, c, GET(0), 0xeaa127fa, 11) + STEP(H, c, d, a, b, GET(3), 0xd4ef3085, 16) + STEP(H2, b, c, d, a, GET(6), 0x04881d05, 23) + STEP(H, a, b, c, d, GET(9), 0xd9d4d039, 4) + STEP(H2, d, a, b, c, GET(12), 0xe6db99e5, 11) + STEP(H, c, d, a, b, GET(15), 0x1fa27cf8, 16) + STEP(H2, b, c, d, a, GET(2), 0xc4ac5665, 23) + +/* Round 4 */ + STEP(I, a, b, c, d, GET(0), 0xf4292244, 6) + STEP(I, d, a, b, c, GET(7), 0x432aff97, 10) + STEP(I, c, d, a, b, GET(14), 0xab9423a7, 15) + STEP(I, b, c, d, a, GET(5), 0xfc93a039, 21) + STEP(I, a, b, c, d, GET(12), 0x655b59c3, 6) + STEP(I, d, a, b, c, GET(3), 0x8f0ccc92, 10) + STEP(I, c, d, a, b, GET(10), 0xffeff47d, 15) + STEP(I, b, c, d, a, GET(1), 0x85845dd1, 21) + STEP(I, a, b, c, d, GET(8), 0x6fa87e4f, 6) + STEP(I, d, a, b, c, GET(15), 0xfe2ce6e0, 10) + STEP(I, c, d, a, b, GET(6), 0xa3014314, 15) + STEP(I, b, c, d, a, GET(13), 0x4e0811a1, 21) + STEP(I, a, b, c, d, GET(4), 0xf7537e82, 6) + STEP(I, d, a, b, c, GET(11), 0xbd3af235, 10) + STEP(I, c, d, a, b, GET(2), 0x2ad7d2bb, 15) + STEP(I, b, c, d, a, GET(9), 0xeb86d391, 21) + + a += saved_a; + b += saved_b; + c += saved_c; + d += saved_d; + + ptr += 64; + } while (size -= 64); + + ctx->a = a; + ctx->b = b; + ctx->c = c; + ctx->d = d; + + return ptr; +} + +void MD5_Init(MD5_CTX *ctx) +{ + ctx->a = 0x67452301; + ctx->b = 0xefcdab89; + ctx->c = 0x98badcfe; + ctx->d = 0x10325476; + + ctx->lo = 0; + ctx->hi = 0; +} + +void MD5_Update(MD5_CTX *ctx, const void *data, unsigned long size) +{ + MD5_u32plus saved_lo; + unsigned long used, available; + + saved_lo = ctx->lo; + if ((ctx->lo = (saved_lo + size) & 0x1fffffff) < saved_lo) + ctx->hi++; + ctx->hi += size >> 29; + + used = saved_lo & 0x3f; + + if (used) { + available = 64 - used; + + if (size < available) { + memcpy(&ctx->buffer[used], data, size); + return; + } + + memcpy(&ctx->buffer[used], data, available); + data = (const unsigned char *)data + available; + size -= available; + body(ctx, ctx->buffer, 64); + } + + if (size >= 64) { + data = body(ctx, data, size & ~(unsigned long)0x3f); + size &= 0x3f; + } + + memcpy(ctx->buffer, data, size); +} + +void MD5_Final(unsigned char *result, MD5_CTX *ctx) +{ + unsigned long used, available; + + used = ctx->lo & 0x3f; + + ctx->buffer[used++] = 0x80; + + available = 64 - used; + + if (available < 8) { + memset(&ctx->buffer[used], 0, available); + body(ctx, ctx->buffer, 64); + used = 0; + available = 64; + } + + memset(&ctx->buffer[used], 0, available - 8); + + ctx->lo <<= 3; + ctx->buffer[56] = ctx->lo; + ctx->buffer[57] = ctx->lo >> 8; + ctx->buffer[58] = ctx->lo >> 16; + ctx->buffer[59] = ctx->lo >> 24; + ctx->buffer[60] = ctx->hi; + ctx->buffer[61] = ctx->hi >> 8; + ctx->buffer[62] = ctx->hi >> 16; + ctx->buffer[63] = ctx->hi >> 24; + + body(ctx, ctx->buffer, 64); + + result[0] = ctx->a; + result[1] = ctx->a >> 8; + result[2] = ctx->a >> 16; + result[3] = ctx->a >> 24; + result[4] = ctx->b; + result[5] = ctx->b >> 8; + result[6] = ctx->b >> 16; + result[7] = ctx->b >> 24; + result[8] = ctx->c; + result[9] = ctx->c >> 8; + result[10] = ctx->c >> 16; + result[11] = ctx->c >> 24; + result[12] = ctx->d; + result[13] = ctx->d >> 8; + result[14] = ctx->d >> 16; + result[15] = ctx->d >> 24; + + memset(ctx, 0, sizeof(*ctx)); +} + +#endif diff --git a/src/hal/components/cros/src/tcpip_socket.c b/src/hal/components/cros/src/tcpip_socket.c new file mode 100644 index 00000000..e9c562b7 --- /dev/null +++ b/src/hal/components/cros/src/tcpip_socket.c @@ -0,0 +1,844 @@ +#include +#include +#include + +#include "tcpip_socket.h" +#include "cros_defs.h" +#include "cros_log.h" +#include "cros_clock.h" + +#ifdef _WIN32 +# define WIN32_LEAN_AND_MEAN // speed up the build process by excluding parts of the Windows header +# include +# include +# include // Ws2_32.lib must be used when linking +// We redefine the socket function codes using constants (FN_...) that will have the same name on Windows and Linux, +// so, in the functions below we do not have to implement different source code for each operating system. +# define FN_EISCONN WSAEISCONN +# define FN_EINPROGRESS WSAEINPROGRESS +# define FN_EALREADY WSAEALREADY +# define FN_ECONNREFUSED WSAECONNREFUSED +# define FN_EWOULDBLOCK WSAEWOULDBLOCK +# define FN_EAGAIN WSAEWOULDBLOCK // Windows does not have a different error code for EAGAIN, so we use EWOULDBLOCK +# define FN_ENOTCONN WSAENOTCONN +# define FN_ECONNRESET WSAECONNRESET +# define FN_EINTR WSAEINTR + +# define FN_SHUT_RDWR SD_BOTH +typedef int fn_socklen_t; +#else +# include +# include +# include +# include +# include +# include +# include +# define closesocket close + +// connect()/accept()/send()/recv()/select() error codes: +# define FN_EISCONN EISCONN +# define FN_EINPROGRESS EINPROGRESS +# define FN_EALREADY EALREADY +# define FN_ECONNREFUSED ECONNREFUSED +# define FN_EWOULDBLOCK EWOULDBLOCK +# define FN_EAGAIN EAGAIN +# define FN_ENOTCONN ENOTCONN +# define FN_ECONNRESET ECONNRESET +# define FN_EINTR EINTR +// shutdown() how mode: +# define FN_SHUT_RDWR SHUT_RDWR +typedef socklen_t fn_socklen_t; +#endif + +#define TCPIP_SOCKET_READ_BUFFER_SIZE 2048 +// Definitions for debug messages only. +// Console virtual-terminal color sequences (supported on Linux and on Windows 10 build 16257 and later): +#define ANSI_COLOR_GREEN "\x1b[32m" +#define ANSI_COLOR_YELLOW "\x1b[33m" +#define ANSI_COLOR_MAGENTA "\x1b[35m" +#define ANSI_COLOR_CYAN "\x1b[36m" +#define ANSI_COLOR_RESET "\x1b[0m" + +// This global variable is incremented for each call to tcpIpSocketStartUp() that has been successfuly executed, +// and it decremented after each successful call to tcpIpSocketCleanUp() +int socket_lib_initialized = 0; // Default value: 0 = library not initialized + +void tcpIpSocketInit ( TcpIpSocket *s ) +{ + PRINT_VVDEBUG ( "tcpIpSocketInit()\n" ); + s->fd = FN_INVALID_SOCKET; + memset ( & ( s->rem_addr ), 0, sizeof ( struct sockaddr_in ) ); + s->open = 0; + s->connected = 0; + s->listening = 0; + s->is_nonblocking = 0; +} + +int tcpIpSocketOpen ( TcpIpSocket *s ) +{ + int ret_success; + + PRINT_VVDEBUG ( "tcpIpSocketOpen()\n" ); + if ( s->open ) + return(1); + + s->fd = socket ( AF_INET, SOCK_STREAM, IPPROTO_TCP ); + if ( s->fd == FN_INVALID_SOCKET ) + { + PRINT_ERROR ( "tcpIpSocketOpen() : Can't open a socket. Error code: %i\n", tcpIpSocketGetError()); + ret_success = 0; + } + else + { + PRINT_VDEBUG ( "tcpIpSocketOpen(): Created socket FD: %i\n", s->fd); + s->open = 1; + ret_success = 1; + } + + return(ret_success); +} + +int tcpIpSocketClose ( TcpIpSocket *s ) +{ + int ret_success; + + PRINT_VVDEBUG ( "tcpIpSocketClose()\n"); + + if ( !s->open ) + return(1); + + if ( s->fd != FN_INVALID_SOCKET ) + { + int close_ret_val; + + PRINT_VDEBUG ( "tcpIpSocketClose(): Closing socket FD: %i\n", s->fd); + close_ret_val = closesocket( s->fd ); + ret_success = (close_ret_val != FN_SOCKET_ERROR); + } + else + { + PRINT_ERROR ( "tcpIpSocketClose(): Invalid file descriptor: %i\n", s->fd); + ret_success = 0; + } + tcpIpSocketInit ( s ); + + return(ret_success); +} + +int tcpIpSocketSetNonBlocking ( TcpIpSocket *s ) +{ + int ret_fn_ctl; + + PRINT_VVDEBUG ( "tcpIpSocketSetNonBlocking()\n" ); + + if ( !s->open ) + { + PRINT_ERROR ( "tcpIpSocketSetNonBlocking() : Socket not opened\n" ); + return(0); + } + +#ifdef _WIN32 + u_long enable_non_blocking = 1; + ret_fn_ctl = ioctlsocket(s->fd, FIONBIO, &enable_non_blocking); +#else + int prev_flags; + prev_flags = fcntl( s->fd, F_GETFL, 0 ); + if(prev_flags < 0) + { + PRINT_ERROR ( "tcpIpSocketSetNonBlocking() : fcntl() failed getting the socket flags\n" ); + prev_flags = 0; // Try to continue anyway + } + ret_fn_ctl = fcntl ( s->fd, F_SETFL, prev_flags | O_NONBLOCK ); +#endif + + if(ret_fn_ctl == 0) + { + s->is_nonblocking = 1; + return(1); + } + else + { + PRINT_ERROR ( "tcpIpSocketSetNonBlocking() : fcntl()/ ioctlsocket() failed configuring socket as non blocking. Error code: %i\n", tcpIpSocketGetError()); + return(0); + } +} + +int tcpIpSocketSetNoDelay ( TcpIpSocket *s ) +{ + PRINT_VVDEBUG ( "tcpIpSocketSetNoDelay()\n" ); + + if ( !s->open ) + { + PRINT_ERROR ( "tcpIpSocketSetNoDelay() : Socket not opened\n" ); + return(0); + } + + int enable_no_delay = 1; + int ret = setsockopt ( s->fd, IPPROTO_TCP, TCP_NODELAY, (const void *)&enable_no_delay, sizeof(enable_no_delay) ); + + if ( ret == 0 ) + { + return(1); + } + else + { + PRINT_ERROR ( "tcpIpSocketSetNoDelay() : setsockopt() with TCP_NODELAY failed. System error code: %i \n", tcpIpSocketGetError()); + return(0); + } +} + +int tcpIpSocketSetReuse ( TcpIpSocket *s ) +{ + PRINT_VVDEBUG ( "tcpIpSocketSetReuse()\n" ); + + if ( !s->open ) + { + PRINT_ERROR ( "tcpIpSocketSetReuse() : Socket not opened\n" ); + return(0); + } + + int enable_reuse_addr = 1; + if ( setsockopt ( s->fd, SOL_SOCKET, SO_REUSEADDR, (const char*)&enable_reuse_addr, sizeof(enable_reuse_addr) ) != 0 ) + { + PRINT_ERROR ( "tcpIpSocketSetReuse() : setsockopt() with SO_REUSEADDR option failed. System error code: %i \n", tcpIpSocketGetError()); + return(0); + } + return(1); +} + +int tcpIpSocketSetKeepAlive ( TcpIpSocket *s, unsigned int idle, unsigned int interval, unsigned int count ) +{ + PRINT_VVDEBUG ( "tcpIpSocketSetKeepAlive()\n" ); + + if ( !s->open ) + { + PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : Socket not opened\n" ); + return(0); + } + + int sock_opt_val = 1; + if ( setsockopt ( s->fd, SOL_SOCKET, SO_KEEPALIVE, (const char *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) + { + PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with SO_KEEPALIVE option failed. System error code: %i \n", tcpIpSocketGetError()); + return(0); + } + + // TCP_KEEPIDLE on Linux is equivalent to TCP_KEEPALIVE option on OS X + // see https://www.winehq.org/pipermail/wine-devel/2015-July/108583.html + sock_opt_val = idle; +#ifdef __APPLE__ + if ( setsockopt( s->fd, IPPROTO_TCP, TCP_KEEPALIVE, (void *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) + { + PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with TCP_KEEPALIVE option failed \n" ); + return 0; + } +#else + if ( setsockopt ( s->fd, IPPROTO_TCP, TCP_KEEPIDLE, (const void *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) + { + PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with SO_KEEPALIVE option failed. System error code: %i \n", tcpIpSocketGetError()); + return(0); + } +#endif + + sock_opt_val = interval; + if ( setsockopt ( s->fd, IPPROTO_TCP, TCP_KEEPINTVL, (const void *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) + { + PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with TCP_KEEPINTVL option failed. System error code: %i \n", tcpIpSocketGetError()); + return(0); + } + + // On Windows this option is available since Windows 10 only + sock_opt_val = count; + if ( setsockopt ( s->fd, IPPROTO_TCP, TCP_KEEPCNT, (const void *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) + { + PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with TCP_KEEPCNT option failed. System error code: %i \n", tcpIpSocketGetError()); + return(0); + } + + return(1); +} + +TcpIpSocketState tcpIpSocketConnect ( TcpIpSocket *s, const char *host_addr, unsigned short host_port ) +{ + int connect_ret; + struct sockaddr_in adr; + int fn_error_code; + + PRINT_VVDEBUG ( "tcpIpSocketConnect():\n" ); + + if ( !s->open ) + { + PRINT_ERROR ( "tcpIpSocketConnect() : Socket not opened\n" ); + return TCPIPSOCKET_FAILED; + } + + if( s->connected ) + return TCPIPSOCKET_DONE; + + memset ( &adr, 0, sizeof ( struct sockaddr_in ) ); + + adr.sin_family = AF_INET; + adr.sin_port = htons ( host_port ); + if ( inet_pton ( AF_INET, host_addr, &adr.sin_addr ) <= 0 ) + { + PRINT_ERROR ( "tcpIpSocketConnect() : Invalid network address: %s. It cannot be converted to a binary address. System error code: %i \n", host_addr, tcpIpSocketGetError()); + s->connected = 0; + return TCPIPSOCKET_FAILED; + } + + connect_ret = connect ( s->fd, ( struct sockaddr * ) &adr, sizeof ( struct sockaddr ) ); + fn_error_code = tcpIpSocketGetError(); + if ( connect_ret == FN_SOCKET_ERROR && fn_error_code != FN_EISCONN ) // The connection is not established so far + { + if ( s->is_nonblocking && + ( fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EALREADY || fn_error_code == FN_EWOULDBLOCK) ) + { + PRINT_VDEBUG ( "tcpIpSocketConnect() : Connection in progress to %s:%i through FD:%i\n", host_addr, host_port, s->fd); + return TCPIPSOCKET_IN_PROGRESS; + } + else + { + s->connected = 0; + if(fn_error_code == FN_ECONNREFUSED) + { + PRINT_ERROR ( "tcpIpSocketConnect() : Connection to %s:%i through FD:%i was refused\n", host_addr, host_port, s->fd); + return TCPIPSOCKET_REFUSED; + } + else + { + PRINT_ERROR ( "tcpIpSocketConnect() : Connection to %s:%i through FD:%i failed due to error code: %i\n", host_addr, host_port, s->fd, fn_error_code); + return TCPIPSOCKET_FAILED; + } + } + } + PRINT_DEBUG (ANSI_COLOR_YELLOW"tcpIpSocketConnect() : connection established to %s:%i through FD:%i\n"ANSI_COLOR_RESET, host_addr, host_port, s->fd); + + s->rem_addr = adr; + s->connected = 1; + + return TCPIPSOCKET_DONE; +} + +int tcpIpSocketDisconnect ( TcpIpSocket *s ) +{ + PRINT_VVDEBUG ( "tcpIpSocketDisconnect()\n" ); + + if ( !s->connected ) + return 1; + + s->connected = 0; + if ( shutdown ( s->fd, FN_SHUT_RDWR ) != 0 ) + { + PRINT_ERROR ( "tcpIpSocketDisconnect() : shutdown failed. System error code: %i \n", tcpIpSocketGetError()); + return 0; + } + return 1; +} + +TcpIpSocketState tcpIpSocketCheckPort ( const char *host_addr, unsigned short host_port ) +{ + TcpIpSocketState port_open, fn_ret; + TcpIpSocket socket_struct; + + tcpIpSocketInit ( &socket_struct ); + fn_ret = tcpIpSocketOpen ( &socket_struct ); + if(fn_ret) + { + TcpIpSocketState socket_stat; + socket_stat = tcpIpSocketConnect ( &socket_struct, host_addr, host_port ); + if ( socket_stat == TCPIPSOCKET_IN_PROGRESS ) + port_open = TCPIPSOCKET_FAILED; + else + { + port_open = socket_stat; + if ( socket_stat == TCPIPSOCKET_DONE ) + tcpIpSocketDisconnect ( &socket_struct ); + } + + tcpIpSocketClose ( &socket_struct ); + } + else + port_open = TCPIPSOCKET_FAILED; + + return port_open; +} + +int tcpIpSocketBindListen( TcpIpSocket *s, const char *host_addr, unsigned short port, int backlog ) +{ + PRINT_VVDEBUG ( "tcpIpSocketBindListen()\n" ); + + if ( !s->open ) + { + PRINT_ERROR ( "tcpIpSocketBindListen() : Socket not opened\n" ); + return 0; + } + + if ( !s->listening ) + { + struct sockaddr_in adr; + + memset ( &adr, 0, sizeof ( struct sockaddr_in ) ); + + adr.sin_family = AF_INET; + adr.sin_port = htons ( port ); + adr.sin_addr.s_addr = htonl(INADDR_ANY); + + if ( inet_pton ( AF_INET, host_addr, &adr.sin_addr ) <= 0 ) + { + PRINT_ERROR ( "tcpIpSocketBindListen() : Invalid network address: %s. It cannot be converted to a binary address. System error code: %i \n", host_addr, tcpIpSocketGetError()); + return 0; + } + + + if ( bind ( s->fd, ( struct sockaddr * ) &adr, sizeof ( struct sockaddr ) ) == FN_SOCKET_ERROR ) + { + PRINT_ERROR ( "tcpIpSocketBindListen() : Socket bind failed. System error code: %i \n", tcpIpSocketGetError()); + return 0; + } + + if ( listen ( s->fd, backlog ) == FN_SOCKET_ERROR ) + { + PRINT_ERROR ( "tcpIpSocketBindListen() : Socket listen failed. System error code: %i \n", tcpIpSocketGetError()); + return 0; + } + + s->rem_addr = adr; + s->listening = 1; + } + + return 1; +} + +TcpIpSocketState tcpIpSocketAccept ( TcpIpSocket *s, TcpIpSocket *new_s ) +{ + PRINT_VVDEBUG ( "tcpIpSocketAccept()\n" ); + + TcpIpSocketState state = TCPIPSOCKET_DONE; + + if ( !s->open ) + { + PRINT_ERROR ( "tcpIpSocketAccept() : Failed: Socket is not opened\n" ); + return TCPIPSOCKET_FAILED; + } + + if ( !s->listening ) + { + PRINT_ERROR ( "tcpIpSocketAccept() : Failed: Socket is not listening\n" ); + return TCPIPSOCKET_FAILED; + } + + struct sockaddr_in new_adr; + socklen_t new_adr_len = sizeof(struct sockaddr); + + int new_fd = accept ( s->fd, ( struct sockaddr * ) &new_adr, &new_adr_len ); + + if ( new_fd == FN_INVALID_SOCKET ) + { + int fn_error_code; + + fn_error_code = tcpIpSocketGetError(); + if ( s->is_nonblocking && + ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) + { + PRINT_VDEBUG ( "tcpIpSocketAccept() : Connection accept in progress from port %i, new FD:%i\n", ntohs( new_adr.sin_port ), new_fd); + state = TCPIPSOCKET_IN_PROGRESS; + } + else + { + PRINT_ERROR ( "tcpIpSocketAccept() : accept() failed. Error code: %d\n", fn_error_code ); + return TCPIPSOCKET_FAILED; + } + } + else + PRINT_DEBUG (ANSI_COLOR_YELLOW"tcpIpSocketAccept() : Accepted connecion from port %i, new FD:%i\n"ANSI_COLOR_RESET, ntohs(new_adr.sin_port), new_fd); + + if( new_s->open ) + tcpIpSocketClose ( new_s ); + + new_s->fd = new_fd; + new_s->rem_addr = new_adr; + new_s->open = 1; + new_s->connected = 1; + + return state; +} + +void printTransmissionBuffer(const char *buffer, const char *msg_info, char *msg_color_seq, int msg_fd, int buf_len) +{ + int i; + PRINT_DEBUG("%s", msg_color_seq); + PRINT_DEBUG("%s (%i bytes FD: %i) ["ANSI_COLOR_RESET, msg_info, buf_len, msg_fd); + for(i=0;iconnected ) + { + PRINT_ERROR ( "tcpIpSocketWriteBuffer() : Socket not connected\n" ); + return TCPIPSOCKET_FAILED; + } + + #if CROS_DEBUG_LEVEL >= 2 + printTransmissionBuffer(data, "tcpIpSocketWriteBuffer() : Buffer", ANSI_COLOR_MAGENTA, s->fd, data_size); + #endif + while ( data_size > 0 ) + { + int n_written , fn_error_code; + + n_written = send ( s->fd, data, data_size, 0 ); + fn_error_code = tcpIpSocketGetError(); + if ( n_written > 0 ) + { + dynBufferMovePoseIndicator ( d_buf, n_written ); + data = (const char *)dynBufferGetCurrentData ( d_buf ); + data_size = dynBufferGetRemainingDataSize ( d_buf ); + } + else if ( s->is_nonblocking && + ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) + { + PRINT_VDEBUG ( "tcpIpSocketWriteBuffer() : write in progress, %d remaining bytes\n", data_size ); + return TCPIPSOCKET_IN_PROGRESS; + } + else if ( fn_error_code == FN_ENOTCONN || fn_error_code == FN_ECONNRESET ) + { + PRINT_VDEBUG ( "tcpIpSocketWriteBuffer() : socket disconnected\n" ); + s->connected = 0; + return TCPIPSOCKET_DISCONNECTED; + } + else + { + PRINT_ERROR ( "tcpIpSocketWriteBuffer() : Write failed. Error code: %i\n", fn_error_code); + return TCPIPSOCKET_FAILED; + } + } + + return TCPIPSOCKET_DONE; +} + +TcpIpSocketState tcpIpSocketWriteString ( TcpIpSocket *s, DynString *d_str ) +{ + PRINT_VVDEBUG ( "tcpIpSocketWriteString()\n" ); + + const char *data = dynStringGetCurrentData ( d_str ); + int data_size = dynStringGetRemainingDataSize ( d_str ); + + if ( !s->connected ) + { + PRINT_ERROR ( "tcpIpSocketWriteString() : Socket not connected\n" ); + return TCPIPSOCKET_FAILED; + } + #if CROS_DEBUG_LEVEL >= 2 + printTransmissionBuffer(data, "tcpIpSocketWriteString() : Buffer", ANSI_COLOR_MAGENTA, s->fd, data_size); + #endif + while ( data_size > 0 ) + { + int n_written , fn_error_code; + + n_written = send ( s->fd, data, data_size, 0 ); + fn_error_code = tcpIpSocketGetError(); + if ( n_written > 0 ) + { + dynStringMovePoseIndicator ( d_str, n_written ); + data = dynStringGetCurrentData ( d_str ); + data_size = dynStringGetRemainingDataSize ( d_str ); + } + else if ( s->is_nonblocking && + ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) + { + PRINT_VDEBUG ( "tcpIpSocketWriteString() : write in progress, %d remaining bytes\n", data_size ); + return TCPIPSOCKET_IN_PROGRESS; + } + else if ( fn_error_code == FN_ENOTCONN || fn_error_code == FN_ECONNRESET ) + { + PRINT_VDEBUG ( "tcpIpSocketWriteString() : socket disconnectd\n" ); + s->connected = 0; + return TCPIPSOCKET_DISCONNECTED; + } + else + { + PRINT_ERROR ( "tcpIpSocketWriteString() : Write failed. Error code: %i\n", fn_error_code); + return TCPIPSOCKET_FAILED; + } + } + + return TCPIPSOCKET_DONE; +} + +TcpIpSocketState tcpIpSocketReadBuffer ( TcpIpSocket *s, DynBuffer *d_buf ) +{ + size_t n_read; + return tcpIpSocketReadBufferEx(s, d_buf, TCPIP_SOCKET_READ_BUFFER_SIZE, &n_read); +} + +TcpIpSocketState tcpIpSocketReadBufferEx( TcpIpSocket *s, DynBuffer *d_buf, size_t max_size, size_t *n_reads) +{ + int recv_ret, fn_error_code; + char *read_buf; + + PRINT_VVDEBUG ( "tcpIpSocketReadBufferEx()\n" ); + + *n_reads = 0; + if ( !s->connected ) + { + PRINT_ERROR ( "tcpIpSocketReadBufferEx() : Socket not connected\n" ); + return TCPIPSOCKET_FAILED; + } + + read_buf = (char *)malloc(max_size); + if (read_buf == NULL) + { + PRINT_ERROR("tcpIpSocketReadBufferEx() : Out of memory allocating %lu bytes before reading from socket", (unsigned long)max_size); + return TCPIPSOCKET_FAILED; + } + + TcpIpSocketState state = TCPIPSOCKET_UNKNOWN; + recv_ret = recv ( s->fd, read_buf, max_size, 0); + fn_error_code = tcpIpSocketGetError(); + if ( recv_ret == 0 ) + { + PRINT_VDEBUG ( "tcpIpSocketReadBufferEx() : socket disconnectd\n" ); + s->connected = 0; + state = TCPIPSOCKET_DISCONNECTED; + } + else if ( recv_ret > 0 ) + { + PRINT_VDEBUG ( "tcpIpSocketReadBufferEx() : read %d bytes \n", recv_ret ); + #if CROS_DEBUG_LEVEL >= 2 + printTransmissionBuffer((const char *)read_buf, "tcpIpSocketReadBufferEx() : Buffer", ANSI_COLOR_CYAN, s->fd, recv_ret); + #endif + + dynBufferPushBackBuf ( d_buf, (const unsigned char *)read_buf, recv_ret ); + state = TCPIPSOCKET_DONE; + *n_reads = recv_ret; + } + else if ( s->is_nonblocking && + ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) + { + PRINT_VDEBUG ( "tcpIpSocketReadBufferEx() : read in progress\n" ); + state = TCPIPSOCKET_IN_PROGRESS; + } + else if ( fn_error_code == FN_ENOTCONN || fn_error_code == FN_ECONNRESET ) + { + PRINT_VDEBUG ( "tcpIpSocketReadBufferEx() : socket disconnectd\n" ); + s->connected = 0; + state = TCPIPSOCKET_DISCONNECTED; + } + else + { + PRINT_ERROR ( "tcpIpSocketReadBufferEx() : Read through socket failed. Error code: %i\n", fn_error_code); + state = TCPIPSOCKET_FAILED; + } + + free(read_buf); + + return state; +} + +TcpIpSocketState tcpIpSocketReadString ( TcpIpSocket *s, DynString *d_str ) +{ + int recv_ret, fn_error_code; + + PRINT_VVDEBUG ( "tcpIpSocketReadString()\n" ); + + if ( !s->connected ) + { + PRINT_ERROR ( "tcpIpSocketReadString() : Socket not connected\n" ); + return TCPIPSOCKET_FAILED; + } + + char read_buf[TCPIP_SOCKET_READ_BUFFER_SIZE]; + + TcpIpSocketState state = TCPIPSOCKET_UNKNOWN; + + recv_ret = recv ( s->fd, read_buf, TCPIP_SOCKET_READ_BUFFER_SIZE , 0 ); + fn_error_code = tcpIpSocketGetError(); + if ( recv_ret == 0 ) + { + PRINT_VDEBUG ( "tcpIpSocketReadString() : socket disconnectd\n" ); + s->connected = 0; + state = TCPIPSOCKET_DISCONNECTED; + } + else if ( recv_ret > 0 ) + { + PRINT_VDEBUG ( "tcpIpSocketReadString() : read %d bytes \n", recv_ret ); + #if CROS_DEBUG_LEVEL >= 2 + printTransmissionBuffer(read_buf, "tcpIpSocketReadString() : Buffer", ANSI_COLOR_CYAN, s->fd, recv_ret); + #endif + + dynStringPushBackStrN ( d_str, read_buf, recv_ret ); + state = TCPIPSOCKET_DONE; + } + else if ( s->is_nonblocking && + ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) + { + PRINT_VDEBUG ( "tcpIpSocketReadString() : read in progress\n" ); + state = TCPIPSOCKET_IN_PROGRESS; + } + else if ( fn_error_code == FN_ENOTCONN || fn_error_code == FN_ECONNRESET ) + { + PRINT_VDEBUG ( "tcpIpSocketReadString() : socket disconnectd\n" ); + s->connected = 0; + state = TCPIPSOCKET_DISCONNECTED; + } + else + { + PRINT_ERROR ( "tcpIpSocketReadString() : Read failed. Error code: %i\n", fn_error_code); + state = TCPIPSOCKET_FAILED; + } + + return state; +} + +int tcpIpSocketGetFD ( TcpIpSocket *s ) +{ + return s->fd; +} + +unsigned short tcpIpSocketGetPort( TcpIpSocket *s ) +{ + struct sockaddr_in addr; + int fn_error_code; + unsigned short ret_addr_port; + fn_socklen_t len_adr = sizeof ( addr ); + if ( getsockname ( s->fd, (struct sockaddr *)&addr, &len_adr ) == 0 ) + ret_addr_port = ntohs ( ((struct sockaddr_in *)&addr)->sin_port ); + else + { + fn_error_code = tcpIpSocketGetError(); + PRINT_ERROR ( "tcpIpSocketConnect() : getsockname() failed obtaining the socket local port due to error code: %i\n", fn_error_code); + } + + return ret_addr_port; +} + +unsigned short tcpIpSocketGetRemotePort( TcpIpSocket *s ) +{ + return ntohs( s->rem_addr.sin_port ); +} + +const char *tcpIpSocketGetRemoteAddress( TcpIpSocket *s ) +{ + char host_addr_buff[MAX_HOST_NAME_LEN+1]; + const char *ret_host_addr; + + ret_host_addr = inet_ntop(s->rem_addr.sin_family, &s->rem_addr.sin_addr, host_addr_buff, sizeof(host_addr_buff)); + + return ret_host_addr; +} + +int tcpIpSocketSelect( int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds, uint64_t time_out ) +{ + struct timeval timeout_tv; + int nfds_set; + + PRINT_VVDEBUG( "tcpIpSocketSelect(): nfds: %i\n", nfds); + + timeout_tv = cRosClockGetTimeVal( time_out ); + + nfds_set = select(nfds, readfds, writefds, exceptfds, &timeout_tv); + + if(nfds_set == FN_SOCKET_ERROR)// Using the definition SOCKET_ERROR is not needed to check the return value, but it is recommended on Windows + { + int socket_err_num; + + socket_err_num = tcpIpSocketGetError(); + + if(socket_err_num == FN_EINTR) + { + PRINT_VDEBUG("tcpIpSocketSelect() : select() returned EINTR error code\n"); + nfds_set = 0; + } + else + { + PRINT_ERROR("tcpIpSocketSelect() : select() call failed. Error code: %i\n", socket_err_num); + nfds_set = -1; + } + } + + return(nfds_set); +} + +#define REQUESTED_WS_HIGH_VER 2 +#define REQUESTED_WS_LOW_VER 0 +int tcpIpSocketStartUp( void ) +{ + int ret_val; + + if(socket_lib_initialized == 0) // If the library is not aready initialized: + { +#ifdef _WIN32 + WORD ws_ver_requested; + WSADATA ws_ver_obtained; + + ws_ver_requested = MAKEWORD(REQUESTED_WS_HIGH_VER, REQUESTED_WS_LOW_VER); // Codify the requested version of Winsock + ret_val = WSAStartup(ws_ver_requested, &ws_ver_obtained); // It returns 0 on success, the same as tcpIpSocketStartUp() + if(ret_val == 0) // If success, check the obtained version + { + // Ckeck that WinSock supports the requested version + if(LOBYTE(ws_ver_obtained.wVersion) != REQUESTED_WS_HIGH_VER || HIBYTE(ws_ver_obtained.wVersion) != REQUESTED_WS_LOW_VER) + PRINT_ERROR("tcpIpSocketStartUp(): Could not find the required version of Winsock: %i.%i.\n", REQUESTED_WS_HIGH_VER, REQUESTED_WS_LOW_VER); + } + else + PRINT_ERROR ( "tcpIpSocketStartUp(): Loading of Winsock failed with error code: %i.\n", ret_val); +#else + // Ignore the SIGPIPE signal. That is, prevent the process from being terminated when it tries to write to a closed socket + signal(SIGPIPE, SIG_IGN); + // In Windows this default behavior does not occur, so nothing has to be done + ret_val = 0; // The socket library does not have to be initialized on Linux or OS X +#endif + if(ret_val == 0) + socket_lib_initialized++; + } + else + ret_val = 0; + + return(ret_val); +} + +int tcpIpSocketCleanUp( void ) +{ + int ret_val; + + if(socket_lib_initialized != 0) // If the library is initialized: + { +#ifdef _WIN32 + ret_val = WSACleanup(); +#else + ret_val = 0; // The socket library does not need to be initialized on Linux or OS X +#endif + if(ret_val == 0) + socket_lib_initialized--; + } + else + ret_val = 0; + + return(ret_val); +} + +int tcpIpSocketGetError( void ) +{ + int socket_err_num; + +#ifdef _WIN32 + socket_err_num = WSAGetLastError(); +#else + socket_err_num = errno; +#endif + + return(socket_err_num); +} diff --git a/src/hal/components/cros/src/tcpros_process.c b/src/hal/components/cros/src/tcpros_process.c new file mode 100644 index 00000000..1607ec9e --- /dev/null +++ b/src/hal/components/cros/src/tcpros_process.c @@ -0,0 +1,79 @@ +#include "tcpros_process.h" +#include "cros_clock.h" +#include + +void tcprosProcessInit( TcprosProcess *p ) +{ + p->state = TCPROS_PROCESS_STATE_IDLE; + tcpIpSocketInit( &(p->socket) ); + dynStringInit( &(p->topic) ); + dynStringInit( &(p->caller_id) ); + dynStringInit( &(p->service) ); + dynStringInit( &(p->type) ); + dynStringInit( &(p->servicerequest_type) ); + dynStringInit( &(p->serviceresponse_type) ); + dynStringInit( &(p->md5sum) ); + dynBufferInit( &(p->packet) ); + p->latching = p->tcp_nodelay = p->persistent = 0; + p->probe = 0; + p->last_change_time = 0; + p->topic_idx = -1; + p->service_idx = -1; + p->ok_byte = 0; + p->left_to_recv = 0; + p->sub_tcpros_host = NULL; + p->sub_tcpros_port = -1; +} + +void tcprosProcessRelease( TcprosProcess *p ) +{ + if( p->socket.connected ) + tcpIpSocketDisconnect( &(p->socket) ); + + dynStringRelease( &(p->topic) ); + dynStringRelease( &(p->service) ); + dynStringRelease( &(p->caller_id) ); + dynStringRelease( &(p->type) ); + dynStringRelease( &(p->servicerequest_type) ); + dynStringRelease( &(p->serviceresponse_type) ); + dynStringRelease( &(p->md5sum) ); + dynBufferRelease( &(p->packet) ); + free(p->sub_tcpros_host); +} + +void tcprosProcessClear( TcprosProcess *p) +{ + dynBufferClear( &(p->packet) ); + p->left_to_recv = 0; +} + +void tcprosProcessReset( TcprosProcess *p) +{ + tcprosProcessClear( p ); + + dynStringClear( &(p->topic) ); + dynStringClear( &(p->caller_id) ); + dynStringClear( &(p->service) ); + dynStringClear( &(p->type) ); + dynStringClear( &(p->servicerequest_type) ); + dynStringClear( &(p->serviceresponse_type) ); + dynStringClear( &(p->md5sum) ); + p->latching = 0; + p->tcp_nodelay = 0; + p->persistent = 0; + p->probe = 0; + p->topic_idx = -1; + p->service_idx = -1; + p->ok_byte = 0; + free(p->sub_tcpros_host); + p->sub_tcpros_host = NULL; + p->sub_tcpros_port = -1; + + tcprosProcessChangeState( p, TCPROS_PROCESS_STATE_IDLE ); +} + +void tcprosProcessChangeState( TcprosProcess *p, TcprosProcessState state ) +{ + p->state = state; + p->last_change_time = cRosClockGetTimeMs(); +} diff --git a/src/hal/components/cros/src/xmlrpc_params.c b/src/hal/components/cros/src/xmlrpc_params.c new file mode 100644 index 00000000..56573733 --- /dev/null +++ b/src/hal/components/cros/src/xmlrpc_params.c @@ -0,0 +1,1454 @@ +#include +#include +#include +#include + +#include "xmlrpc_params.h" +#include "xmlrpc_tags.h" +#include "cros_defs.h" +#include "cros_log.h" + +enum { XMLRPC_ARRAY_INIT_SIZE = 4, XMLRPC_ARRAY_GROW_RATE = 2 }; + +typedef enum ParamContainerType +{ + PARAM_CONTAINER_NONE = 0, + PARAM_CONTAINER_ARRAY = 1, + PARAM_CONTAINER_STRUCT = 2 +} ParamContainerType; + +static int paramFromXml ( DynString *message, XmlrpcParam *param, ParamContainerType container); +static int paramValueFromXml ( DynString *message, XmlrpcParam *param, ParamContainerType container); +static int structMemberFromXml ( DynString *message, XmlrpcParam *param); +static int arrayFromXml ( DynString *message, XmlrpcParam *param); +static int structFromXml ( DynString *message, XmlrpcParam *param); +static XmlrpcParam * arrayAddElem ( XmlrpcParam *param ); +static int paramSetMemberName ( XmlrpcParam *param, const char *name ); + + +static void boolToXml ( unsigned char val, DynString *message ) +{ + dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); + dynStringPushBackStr ( message, XMLRPC_BOOLEAN_TAG.str ); + dynStringPushBackStr ( message, val != 0?"1":"0" ); + dynStringPushBackStr ( message, XMLRPC_BOOLEAN_ETAG.str ); + dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); +} + +static void intToXml ( int val, DynString *message ) +{ + dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); + dynStringPushBackStr ( message, XMLRPC_INT_TAG.str ); + char num_str[22]; + snprintf ( num_str, 22, "%d", val ); + dynStringPushBackStr ( message, num_str ); + dynStringPushBackStr ( message, XMLRPC_INT_ETAG.str ); + dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); +} + +static void doubleToXml ( double val, DynString *message ) +{ + setlocale ( LC_NUMERIC, "" ); // Set decimal-point and thousands-separator character to the implementation-defined native environment + dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); + dynStringPushBackStr ( message, XMLRPC_DOUBLE_TAG.str ); + char num_str[26]; + snprintf ( num_str, 26, "%.17g", val ); + dynStringPushBackStr ( message, num_str ); + dynStringPushBackStr ( message, XMLRPC_DOUBLE_ETAG.str ); + dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); +} + +static void stringToXml ( char *val, DynString *message ) +{ + dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); + dynStringPushBackStr ( message, XMLRPC_STRING_TAG.str ); + + int str_len = strlen ( val ); + int i = 0; + for ( i = 0; i < str_len; i++ ) + { + char c = val[i]; + if ( c == '<' ) + dynStringPushBackStr ( message, "<" ); + else if ( c == '>' ) + dynStringPushBackStr ( message, ">" ); + else if ( c == '&' ) + dynStringPushBackStr ( message, "&" ); + else if ( c == '\'' ) + dynStringPushBackStr ( message, "'" ); + else if ( c == '\"' ) + dynStringPushBackStr ( message, """ ); + else + dynStringPushBackChar ( message, c ); + } + + dynStringPushBackStr ( message, XMLRPC_STRING_ETAG.str ); + dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); +} + +static void structToXml ( XmlrpcParam *val, DynString *message ) +{ + dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); + dynStringPushBackStr ( message, XMLRPC_STRUCT_TAG.str ); + + int i; + for ( i = 0; i < val->array_n_elem; i++ ) + xmlrpcParamToXml ( & ( val->data.as_array[i] ), message ); + + dynStringPushBackStr ( message, XMLRPC_STRUCT_ETAG.str ); + dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); +} + +static void arrayToXml ( XmlrpcParam *val, DynString *message ) +{ + dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); + dynStringPushBackStr ( message, XMLRPC_ARRAY_TAG.str ); + dynStringPushBackStr ( message, XMLRPC_DATA_TAG.str ); + + int i; + for ( i = 0; i < val->array_n_elem; i++ ) + xmlrpcParamToXml ( & ( val->data.as_array[i] ), message ); + + dynStringPushBackStr ( message, XMLRPC_DATA_ETAG.str ); + dynStringPushBackStr ( message, XMLRPC_ARRAY_ETAG.str ); + dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); +} + +static void timeToXml ( void *val, DynString *message ) +{ + PRINT_ERROR ( "timeToXml() : ERROR: Not yet implemented!\n" ); +} + +static void binaryToXml ( void *val, DynString *message ) +{ + PRINT_ERROR ( "binaryToXml() : ERROR: Not yet implemented!\n" ); +} + +int paramFromXml (DynString *message, XmlrpcParam *param, ParamContainerType container) +{ + PRINT_VVDEBUG ( "paramFromXml(), is_array : %s \n", (container == PARAM_CONTAINER_ARRAY)?"TRUE":"FALSE" ); + + int rc; + const char *c = dynStringGetCurrentData ( message ); + int len = dynStringGetLen ( message ); + int i = dynStringGetPoseIndicatorOffset ( message ); + const char *param_begin = NULL; + const char *param_end = NULL; + for (; i < len; i++, c++ ) + { + if ((container == PARAM_CONTAINER_NONE || container == PARAM_CONTAINER_ARRAY) + && len - i >= XMLRPC_VALUE_TAG.dim + && strncmp ( c, XMLRPC_VALUE_TAG.str, XMLRPC_VALUE_TAG.dim ) == 0 ) + { + c += XMLRPC_VALUE_TAG.dim; + i += XMLRPC_VALUE_TAG.dim; + param_begin = c; + break; + } + else if (container == PARAM_CONTAINER_STRUCT && len - i >= XMLRPC_MEMBER_TAG.dim + && strncmp ( c, XMLRPC_MEMBER_TAG.str, XMLRPC_MEMBER_TAG.dim ) == 0 ) + { + c += XMLRPC_MEMBER_TAG.dim; + i += XMLRPC_MEMBER_TAG.dim; + param_begin = c; + break; + } + else if (container == PARAM_CONTAINER_ARRAY && len - i >= XMLRPC_DATA_ETAG.dim + && strncmp ( c, XMLRPC_DATA_ETAG.str, XMLRPC_DATA_ETAG.dim ) == 0 ) + { + PRINT_VVDEBUG ( "paramFromXml() : reach end of array\n" ); + return -1; + } + else if (container == PARAM_CONTAINER_STRUCT && len - i >= XMLRPC_STRUCT_ETAG.dim + && strncmp ( c, XMLRPC_STRUCT_ETAG.str, XMLRPC_STRUCT_ETAG.dim ) == 0 ) + { + PRINT_VVDEBUG ( "paramFromXml() : reach end of struct\n" ); + return -1; + } + } + + if (param_begin == NULL) + { + PRINT_ERROR ( "paramFromXml() : no param tag found\n" ); + return -1; + } + + dynStringSetPoseIndicator ( message, i); + + switch (container) + { + case PARAM_CONTAINER_NONE: + case PARAM_CONTAINER_ARRAY: + { + rc = paramValueFromXml(message, param, container); + break; + } + case PARAM_CONTAINER_STRUCT: + { + param = arrayAddElem (param); + if (param == NULL) + return -1; + + rc = structMemberFromXml(message, param); + break; + } + default: + { + PRINT_ERROR("paramFromXml() : Invalid ParamContainerType specified\n"); + return -1; + } + } + + if (rc < 0) + return rc; + + c = dynStringGetCurrentData ( message ); + i = dynStringGetPoseIndicatorOffset ( message ); + + for (; i < len; i++, c++ ) + { + switch (container) + { + case PARAM_CONTAINER_NONE: + case PARAM_CONTAINER_ARRAY: + { + if ( len - i >= XMLRPC_VALUE_ETAG.dim && + strncmp ( c, XMLRPC_VALUE_ETAG.str, XMLRPC_VALUE_ETAG.dim ) == 0 ) + { + param_end = c; + c += XMLRPC_VALUE_ETAG.dim; + i += XMLRPC_VALUE_ETAG.dim; + goto paramFromXml_exit; + } + break; + } + case PARAM_CONTAINER_STRUCT: + { + if ( len - i >= XMLRPC_MEMBER_ETAG.dim && + strncmp ( c, XMLRPC_MEMBER_ETAG.str, XMLRPC_MEMBER_ETAG.dim ) == 0 ) + { + param_end = c; + c += XMLRPC_MEMBER_ETAG.dim; + i += XMLRPC_MEMBER_ETAG.dim; + goto paramFromXml_exit; + } + break; + } + default: + { + break; // Error + } + } + } + +paramFromXml_exit: + if ( param_end == NULL ) + { + PRINT_ERROR ( "arrayFromXml() : no param end tag found\n" ); + return -1; + } + + dynStringSetPoseIndicator ( message, i); + + return 0; +} + +int arrayFromXml(DynString *message, XmlrpcParam *param) +{ + PRINT_VVDEBUG ( "arrayFromXml()\n" ); + + const char *c = dynStringGetCurrentData ( message ); + int len = dynStringGetLen ( message ); + int i = dynStringGetPoseIndicatorOffset ( message ); + const char *data_init = NULL; + const char *data_end = NULL; + + for ( ; i < len; i++, c++ ) + { + if ( len - i >= XMLRPC_DATA_TAG.dim && + strncmp ( c, XMLRPC_DATA_TAG.str, XMLRPC_DATA_TAG.dim ) == 0 ) + { + c += XMLRPC_DATA_TAG.dim; + i += XMLRPC_DATA_TAG.dim; + data_init = c; + break; + } + else if ( len - i >= XMLRPC_DATA_NTAG.dim && + strncmp ( c, XMLRPC_DATA_NTAG.str, XMLRPC_DATA_NTAG.dim ) == 0 ) // Empty array: start-tag and end-tag codified in a single tag: empty-element tag + { + c += XMLRPC_DATA_NTAG.dim; + i += XMLRPC_DATA_NTAG.dim; + data_init = c; + data_end = c; + break; + } + } + + if ( data_init == NULL ) + { + PRINT_ERROR ( "arrayFromXml() : no data start-tag found in array\n" ); + return -1; + } + + if ( data_end == NULL ) + { + dynStringSetPoseIndicator ( message, i); + + while (paramFromXml (message, param, PARAM_CONTAINER_ARRAY ) != -1); + + c = dynStringGetCurrentData ( message ); + i = dynStringGetPoseIndicatorOffset ( message ); + + for ( ; i < len; i++, c++ ) + { + if ( len - i >= XMLRPC_DATA_ETAG.dim && + strncmp ( c, XMLRPC_DATA_ETAG.str, XMLRPC_DATA_ETAG.dim ) == 0 ) + { + data_end = c; + c += XMLRPC_DATA_ETAG.dim; + i += XMLRPC_DATA_ETAG.dim; + break; + } + } + + if ( data_end == NULL ) + { + PRINT_ERROR ( "arrayFromXml() : no data end-tag found in array\n" ); + return -1; + } + } + + dynStringSetPoseIndicator ( message, i ); + + return 0; +} + +int structFromXml(DynString *message, XmlrpcParam *param) +{ + PRINT_VVDEBUG ( "structFromXml()\n" ); + + while (paramFromXml (message, param, PARAM_CONTAINER_STRUCT ) != -1); + + return 0; +} + +int paramValueFromXml (DynString *message, XmlrpcParam *param, ParamContainerType container) +{ + int ret = 0; // Default return value=0 + const char *c = dynStringGetCurrentData ( message ); + int len = dynStringGetLen ( message ); + int i = dynStringGetPoseIndicatorOffset ( message ); + + XmlrpcParamType p_type = XMLRPC_PARAM_UNKNOWN; + const char *value_init = NULL; + const char *type_init = NULL; + const char *type_end = NULL; + + for ( ; i < len; i++, c++ ) + { + if ( len - i >= XMLRPC_BOOLEAN_TAG.dim && + strncmp ( c, XMLRPC_BOOLEAN_TAG.str, XMLRPC_BOOLEAN_TAG.dim ) == 0 ) + { + c += XMLRPC_BOOLEAN_TAG.dim; + i += XMLRPC_BOOLEAN_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_BOOL; + break; + } + else if ( len - i >= XMLRPC_I4_TAG.dim && + strncmp ( c, XMLRPC_I4_TAG.str, XMLRPC_I4_TAG.dim ) == 0 ) + { + c += XMLRPC_I4_TAG.dim; + i += XMLRPC_I4_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_INT; + break; + } + else if ( len - i >= XMLRPC_INT_TAG.dim && + strncmp ( c, XMLRPC_INT_TAG.str, XMLRPC_INT_TAG.dim ) == 0 ) + { + c += XMLRPC_INT_TAG.dim; + i += XMLRPC_INT_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_INT; + break; + } + else if ( len - i >= XMLRPC_DOUBLE_TAG.dim && + strncmp ( c, XMLRPC_DOUBLE_TAG.str, XMLRPC_DOUBLE_TAG.dim ) == 0 ) + { + c += XMLRPC_DOUBLE_TAG.dim; + i += XMLRPC_DOUBLE_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_DOUBLE; + break; + } + else if ( len - i >= XMLRPC_STRING_TAG.dim && + strncmp ( c, XMLRPC_STRING_TAG.str, XMLRPC_STRING_TAG.dim ) == 0 ) + { + c += XMLRPC_STRING_TAG.dim; + i += XMLRPC_STRING_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_STRING; + break; + } + else if ( len - i >= XMLRPC_ARRAY_TAG.dim && + strncmp ( c, XMLRPC_ARRAY_TAG.str, XMLRPC_ARRAY_TAG.dim ) == 0 ) + { + c += XMLRPC_ARRAY_TAG.dim; + i += XMLRPC_ARRAY_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_ARRAY; + break; + } + else if ( len - i >= XMLRPC_DATETIME_TAG.dim && + strncmp ( c, XMLRPC_DATETIME_TAG.str, XMLRPC_DATETIME_TAG.dim ) == 0 ) + { + PRINT_ERROR ( "paramFromXml() : ERROR: Tag %s not yet implemented!\n", XMLRPC_DATETIME_TAG.str ); + ret=-1; + + c += XMLRPC_DATETIME_TAG.dim; + i += XMLRPC_DATETIME_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_DATETIME; + break; + } + else if ( len - i >= XMLRPC_BASE64_TAG.dim && + strncmp ( c, XMLRPC_BASE64_TAG.str, XMLRPC_BASE64_TAG.dim ) == 0 ) + { + PRINT_ERROR ( "paramFromXml() : ERROR: Tag %s not yet implemented!\n", XMLRPC_BASE64_TAG.str ); + ret=-1; + + c += XMLRPC_BASE64_TAG.dim; + i += XMLRPC_BASE64_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_BINARY; + break; + } + else if ( len - i >= XMLRPC_STRUCT_TAG.dim && + strncmp ( c, XMLRPC_STRUCT_TAG.str, XMLRPC_STRUCT_TAG.dim ) == 0 ) + { + c += XMLRPC_STRUCT_TAG.dim; + i += XMLRPC_STRUCT_TAG.dim; + type_init = c; + p_type = XMLRPC_PARAM_STRUCT; + break; + } + else if ( len - i >= XMLRPC_STRUCT_NTAG.dim && + strncmp ( c, XMLRPC_STRUCT_NTAG.str, XMLRPC_STRUCT_NTAG.dim ) == 0 ) // Empty-structure tag + { + c += XMLRPC_STRUCT_NTAG.dim; + i += XMLRPC_STRUCT_NTAG.dim; + type_init = c; + type_end = c; // Indicate that the struct end has been found, so it is an empty structure + p_type = XMLRPC_PARAM_STRUCT; + break; + } + else if ( len - i >= XMLRPC_VALUE_ETAG.dim && + strncmp ( c, XMLRPC_VALUE_ETAG.str, XMLRPC_VALUE_ETAG.dim ) == 0 ) + { + value_init = dynStringGetCurrentData ( message ); + i = dynStringGetPoseIndicatorOffset ( message ); + break; + } + } + + PRINT_VVDEBUG("paramFromXml() : Param type %d \n", p_type ); + + // Update pose indicator (useful in case it is an array parameter) + dynStringSetPoseIndicator ( message, i ); + + const char *str_init = NULL; + int str_len = 0; + + switch (p_type) + { + case XMLRPC_PARAM_BOOL: + { + int val; + if ( sscanf ( c, "%d", &val ) == 1 ) + { + if (container == PARAM_CONTAINER_ARRAY) + xmlrpcParamArrayPushBackBool ( param, val ); + else + xmlrpcParamSetBool ( param, val ); + } + else + { + PRINT_ERROR ( "paramFromXml() : not valid value\n" ); + xmlrpcParamSetUnknown ( param ); + return -1; + } + break; + } + case XMLRPC_PARAM_INT: + { + int val; + if ( sscanf ( c, "%d", &val ) == 1 ) + { + if (container == PARAM_CONTAINER_ARRAY) + xmlrpcParamArrayPushBackInt ( param, val ); + else + xmlrpcParamSetInt ( param, val ); + } + else + { + PRINT_ERROR ( "paramFromXml() : not valid value\n" ); + xmlrpcParamSetUnknown ( param ); + return -1; + } + break; + } + case XMLRPC_PARAM_DOUBLE: + { + double val; + if ( sscanf ( c, "%lf", &val ) == 1 ) + { + if (container == PARAM_CONTAINER_ARRAY) + xmlrpcParamArrayPushBackDouble ( param, val ); + else + xmlrpcParamSetDouble ( param, val ); + } + else + { + PRINT_ERROR ( "paramFromXml() : not valid value\n" ); + xmlrpcParamSetUnknown ( param ); + return -1; + } + break; + } + case XMLRPC_PARAM_STRING: + { + str_init = type_init; + break; + } + case XMLRPC_PARAM_ARRAY: + { + if (container == PARAM_CONTAINER_ARRAY) + // Pushed new param array: start to parse for this new element + param = xmlrpcParamArrayPushBackArray ( param ); + else + xmlrpcParamSetArray ( param ); + + if (param == NULL) + return -1; + + int rc = arrayFromXml(message, param); + if (rc < 0) + return rc; + + break; + } + case XMLRPC_PARAM_DATETIME: + { + // TODO Implement me! + break; + } + case XMLRPC_PARAM_BINARY: + { + // TODO Implement me! + break; + } + case XMLRPC_PARAM_STRUCT: + { + if (container == PARAM_CONTAINER_ARRAY) + // Pushed new param array: start to parse for this new element + param = xmlrpcParamArrayPushBackStruct ( param ); + else + xmlrpcParamSetStruct ( param ); + + if (param == NULL) + return -1; + + if(type_end == NULL) // If it is not an empty structure, so parse the xml text + { + int rc = structFromXml(message, param); + if (rc < 0) + return rc; + } + + break; + } + default: + { + // Maybe string without and tabs + str_init = value_init; + break; + } + } + + c = dynStringGetCurrentData ( message ); + i = dynStringGetPoseIndicatorOffset ( message ); + for ( ; i < len && type_end == NULL; c++ ) + { + if ( p_type == XMLRPC_PARAM_BOOL && len - i >= XMLRPC_BOOLEAN_ETAG.dim && + strncmp ( c, XMLRPC_BOOLEAN_ETAG.str, XMLRPC_BOOLEAN_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_BOOLEAN_ETAG.dim; + i += XMLRPC_BOOLEAN_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_INT && len - i >= XMLRPC_I4_ETAG.dim && + strncmp ( c, XMLRPC_I4_ETAG.str, XMLRPC_I4_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_I4_ETAG.dim; + i += XMLRPC_I4_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_INT && len - i >= XMLRPC_INT_ETAG.dim && + strncmp ( c, XMLRPC_INT_ETAG.str, XMLRPC_INT_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_INT_ETAG.dim; + i += XMLRPC_INT_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_STRING && len - i >= XMLRPC_STRING_ETAG.dim && + strncmp ( c, XMLRPC_STRING_ETAG.str, XMLRPC_STRING_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_STRING_ETAG.dim; + i += XMLRPC_STRING_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_ARRAY && len - i >= XMLRPC_ARRAY_ETAG.dim && + strncmp ( c, XMLRPC_ARRAY_ETAG.str, XMLRPC_ARRAY_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_ARRAY_ETAG.dim; + i += XMLRPC_ARRAY_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_STRUCT && len - i >= XMLRPC_STRUCT_ETAG.dim && + strncmp ( c, XMLRPC_STRUCT_ETAG.str, XMLRPC_STRUCT_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_STRUCT_ETAG.dim; + i += XMLRPC_STRUCT_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_DATETIME && len - i >= XMLRPC_DATETIME_ETAG.dim && + strncmp ( c, XMLRPC_DATETIME_ETAG.str, XMLRPC_DATETIME_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_DATETIME_ETAG.dim; + i += XMLRPC_DATETIME_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_BINARY && len - i >= XMLRPC_BASE64_ETAG.dim && + strncmp ( c, XMLRPC_BASE64_ETAG.str, XMLRPC_BASE64_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_BASE64_ETAG.dim; + i += XMLRPC_BASE64_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_STRUCT && len - i >= XMLRPC_STRUCT_ETAG.dim && + strncmp ( c, XMLRPC_STRUCT_ETAG.str, XMLRPC_STRUCT_ETAG.dim ) == 0 ) + { + type_end = c; + + c += XMLRPC_STRUCT_ETAG.dim; + i += XMLRPC_STRUCT_ETAG.dim; + } + else if ( p_type == XMLRPC_PARAM_UNKNOWN && len - i >= XMLRPC_VALUE_ETAG.dim && + strncmp ( c, XMLRPC_VALUE_ETAG.str, XMLRPC_VALUE_ETAG.dim ) == 0 ) + { + type_end = c; + + // End value tag is feeded outside + } + else + { + str_len++; + i++; // Only increase the string index if an end tag has not been found + } + } + + // Update pose indicator (useful in case it is an array parameter) + dynStringSetPoseIndicator ( message, i ); + + if ( type_end == NULL ) + { + PRINT_ERROR ( "paramFromXml() : no end type tag found\n" ); + return -1; + } + + if( p_type == XMLRPC_PARAM_STRING || p_type == XMLRPC_PARAM_UNKNOWN ) + { + if (container == PARAM_CONTAINER_ARRAY) + xmlrpcParamArrayPushBackStringN ( param, str_init, str_len ); + else + xmlrpcParamSetStringN ( param, str_init, str_len ); + } + + return ret; +} + +int structMemberFromXml ( DynString *message, XmlrpcParam *param) +{ + int rc; + int len = dynStringGetLen ( message ); + const char *c = dynStringGetCurrentData ( message ); + int i = dynStringGetPoseIndicatorOffset ( message ); + + const char *name_begin = NULL; + const char *name_end = NULL; + for ( ; i < len; i++, c++ ) + { + if ( len - i >= XMLRPC_NAME_TAG.dim && + strncmp ( c, XMLRPC_NAME_TAG.str, XMLRPC_NAME_TAG.dim ) == 0 ) + { + c += XMLRPC_NAME_TAG.dim; + i += XMLRPC_NAME_TAG.dim; + name_begin = c; + break; + } + } + + for ( ; i < len; i++, c++ ) + { + if ( len - i >= XMLRPC_NAME_ETAG.dim && + strncmp ( c, XMLRPC_NAME_ETAG.str, XMLRPC_NAME_ETAG.dim ) == 0 ) + { + name_end = c; + c += XMLRPC_NAME_ETAG.dim; + i += XMLRPC_NAME_ETAG.dim; + break; + } + } + + if (name_begin == NULL || name_end == NULL) + { + PRINT_ERROR ( "paramMemberFromXml() : no name type tag found\n" ); + return -1; + } + + size_t name_len = name_end - name_begin; + param->member_name = (char *)malloc(name_len + 1); + if (param->member_name == NULL) + { + PRINT_ERROR ( "paramMemberFromXml() : Can't allocate memory\n" ); + return -1; + } + + memcpy(param->member_name, name_begin, name_len); + param->member_name[name_len] = '\0'; + + const char *value_begin = NULL; + const char *value_end = NULL; + for ( ; i < len; i++, c++ ) + { + if ( len - i >= XMLRPC_VALUE_TAG.dim && + strncmp ( c, XMLRPC_VALUE_TAG.str, XMLRPC_VALUE_TAG.dim ) == 0 ) + { + c += XMLRPC_VALUE_TAG.dim; + i += XMLRPC_VALUE_TAG.dim; + value_begin = c; + break; + } + } + + if (value_begin == NULL) + { + PRINT_ERROR ( "paramMemberFromXml() : no value tag found\n" ); + return -1; + } + + dynStringSetPoseIndicator ( message, i); + + rc = paramValueFromXml(message, param, PARAM_CONTAINER_STRUCT ); + if (rc < 0) + return rc; + + c = dynStringGetCurrentData ( message ); + i = dynStringGetPoseIndicatorOffset ( message ); + + for ( ; i < len; i++, c++ ) + { + if ( len - i >= XMLRPC_VALUE_ETAG.dim && + strncmp ( c, XMLRPC_VALUE_ETAG.str, XMLRPC_VALUE_ETAG.dim ) == 0 ) + { + value_end = c; + c += XMLRPC_VALUE_ETAG.dim; + i += XMLRPC_VALUE_ETAG.dim; + break; + } + } + + if ( value_end == NULL ) + { + PRINT_ERROR ( "paramMemberFromXml() : no value end tag found\n" ); + return -1; + } + + dynStringSetPoseIndicator ( message, i); + + return 0; +} + +XmlrpcParam * arrayAddElem ( XmlrpcParam *param ) +{ + if ( param->type != XMLRPC_PARAM_ARRAY && param->type != XMLRPC_PARAM_STRUCT ) + { + PRINT_ERROR ( "arrayAddElem() : Not array or struct type param \n" ); + return NULL; + } + + if ( param->array_n_elem == param->array_max_elem ) + { + PRINT_VVDEBUG ( "arrayAddElem() : reallocate memory\n" ); + XmlrpcParam *new_param = ( XmlrpcParam * ) realloc ( param->data.as_array, + ( XMLRPC_ARRAY_GROW_RATE * param->array_max_elem ) * sizeof ( XmlrpcParam ) ); + if ( new_param == NULL ) + { + PRINT_ERROR ( "arrayAddElem() : Can't allocate more memory\n" ); + return NULL; + } + param->array_max_elem *= XMLRPC_ARRAY_GROW_RATE; + param->data.as_array = new_param; + } + + XmlrpcParam *ret = ¶m->data.as_array[param->array_n_elem++]; + xmlrpcParamInit(ret); + return ret; +} + + +int xmlrpcParamGetBool( XmlrpcParam *param ) +{ + return param->data.as_bool?1:0; +} + +int32_t xmlrpcParamGetInt( XmlrpcParam *param ) +{ + return param->data.as_int; +} + +double xmlrpcParamGetDouble( XmlrpcParam *param ) +{ + return param->data.as_double; +} +char *xmlrpcParamGetString( XmlrpcParam *param ) +{ + return param->data.as_string; +} + +void xmlrpcParamSetUnknown ( XmlrpcParam *param ) +{ + PRINT_VVDEBUG ( "xmlrpcParamSetUnknown()\n" ); + + param->type = XMLRPC_PARAM_UNKNOWN; +} + +void xmlrpcParamSetBool ( XmlrpcParam *param, int val ) +{ + PRINT_VVDEBUG ( "xmlrpcSetBool()\n" ); + + param->type = XMLRPC_PARAM_BOOL; + param->data.as_bool = ( ( val != 0 ) ?1:0 ); + PRINT_VDEBUG( "xmlrpcSetBool() : Set: %s\n", param->data.as_bool?"TRUE":"FALSE" ); +} + +void xmlrpcParamSetInt ( XmlrpcParam *param, int32_t val ) +{ + PRINT_VVDEBUG ( "xmlrpcSetInt()\n" ); + + param->type = XMLRPC_PARAM_INT; + param->data.as_int = val; + PRINT_VDEBUG( "xmlrpcSetInt() : Set: %d\n", param->data.as_int ); +} + +void xmlrpcParamSetDouble ( XmlrpcParam *param, double val ) +{ + PRINT_VVDEBUG ( "xmlrpcSetDouble()\n" ); + + param->type = XMLRPC_PARAM_DOUBLE; + param->data.as_double = val; + PRINT_VDEBUG ( "xmlrpcSetDouble() : Set: %f\n", param->data.as_double ); +} + +int xmlrpcParamSetString ( XmlrpcParam *param, const char *val ) +{ + PRINT_VVDEBUG ( "xmlrpcSetString()\n" ); + + int str_len = strlen ( val ); + return xmlrpcParamSetStringN ( param, val, str_len ); +} + +int xmlrpcParamSetStringN ( XmlrpcParam *param, const char *val, int n ) +{ + PRINT_VVDEBUG ( "xmlrpcSetStringN()\n" ); + + param->type = XMLRPC_PARAM_STRING; + param->data.as_string = ( char * ) malloc ( ( n + 1 ) *sizeof ( char ) ); + if ( param->data.as_string == NULL ) + { + PRINT_ERROR ( "xmlrpcSetStringN() : Can't allocate memory\n" ); + return -1; + } + int i = 0; + const char *c = val; + for( ; i < n; i++, c++) + { + if ( n - i >= 4 && strncmp ( c, "<", 4 ) == 0 ) + param->data.as_string[i] = '<'; + else if ( n - i >= 4 && strncmp ( c, ">", 4 ) == 0 ) + param->data.as_string[i] = '>'; + else if ( n - i >= 5 && strncmp ( c, "&", 5 ) == 0 ) + param->data.as_string[i] = '&'; + else if ( n - i >= 6 && strncmp ( c, "'", 6 ) == 0 ) + param->data.as_string[i] = '\''; + else if ( n - i >= 6 && strncmp ( c, """, 6 ) == 0 ) + param->data.as_string[i] = '\"'; + else + param->data.as_string[i] = *c; + } + param->data.as_string[i] = '\0'; + + PRINT_VDEBUG ( "xmlrpcSetStringN() : Set: %s\n", param->data.as_string ); + return 0; +} + +int xmlrpcParamArrayGetSize( XmlrpcParam *param ) +{ + if( param->type == XMLRPC_PARAM_ARRAY) + return param->array_n_elem; + else + return -1; +} + +XmlrpcParam *xmlrpcParamArrayGetParamAt( XmlrpcParam *param, int idx ) +{ + if( param->type == XMLRPC_PARAM_ARRAY || param->type == XMLRPC_PARAM_STRUCT ) + { + if( idx >= 0 && idx < param->array_n_elem ) + return &(param->data.as_array[idx]); + else + { + PRINT_ERROR ( "xmlrpcParamArrayGetParamAt() : The index to access the array (or structure) element must be between 0 and the number of elements-1.\n" ); + return NULL; + } + } + else + { + PRINT_ERROR ( "xmlrpcParamArrayGetParamAt() : Only elements of a structure or an array can be accessed through index.\n" ); + return NULL; + } +} + +int xmlrpcParamSetArray ( XmlrpcParam *param ) +{ + PRINT_VVDEBUG ( "xmlrpcSetArray()\n" ); + param->type = XMLRPC_PARAM_ARRAY; + + param->data.as_array = ( XmlrpcParam * ) malloc ( XMLRPC_ARRAY_INIT_SIZE*sizeof ( XmlrpcParam ) ); + if ( param->data.as_array == NULL ) + { + PRINT_ERROR ( "xmlrpcSetArray() : Can't allocate memory\n" ); + param->array_max_elem = param->array_n_elem = 0; + return -1; + } + param->array_n_elem = 0; + param->array_max_elem = XMLRPC_ARRAY_INIT_SIZE; + return 0; +} + +int xmlrpcParamSetStruct( XmlrpcParam *param ) +{ + PRINT_VVDEBUG ( "xmlrpcSetArray()\n" ); + param->type = XMLRPC_PARAM_STRUCT; + + param->data.as_array = ( XmlrpcParam * ) malloc ( XMLRPC_ARRAY_INIT_SIZE*sizeof ( XmlrpcParam ) ); + if ( param->data.as_array == NULL ) + { + PRINT_ERROR ( "xmlrpcSetArray() : Can't allocate memory\n" ); + param->array_max_elem = param->array_n_elem = 0; + return -1; + } + param->array_n_elem = 0; + param->array_max_elem = XMLRPC_ARRAY_INIT_SIZE; + return 0; +} + + +XmlrpcParamType xmlrpcParamGetType ( XmlrpcParam *param ) +{ + return param->type; +} + +XmlrpcParam * xmlrpcParamArrayPushBackBool ( XmlrpcParam *param, int val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackBool()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + xmlrpcParamSetBool ( new_param, val ); + return new_param; +} + +XmlrpcParam * xmlrpcParamArrayPushBackInt ( XmlrpcParam *param, int32_t val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackInt()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + xmlrpcParamSetInt ( new_param, val ); + return new_param; +} + +XmlrpcParam * xmlrpcParamArrayPushBackDouble ( XmlrpcParam *param, double val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackDouble()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + xmlrpcParamSetDouble ( new_param, val ); + return new_param; +} + +XmlrpcParam * xmlrpcParamArrayPushBackString ( XmlrpcParam *param, const char *val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackString()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + xmlrpcParamSetString ( new_param, val ); + return new_param; +} + +XmlrpcParam * xmlrpcParamArrayPushBackStringN ( XmlrpcParam *param, const char *val, int n ) +{ + PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackStringN()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + xmlrpcParamSetStringN ( new_param, val, n ); + return new_param; +} + +XmlrpcParam *xmlrpcParamArrayPushBackArray ( XmlrpcParam *param ) +{ + PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackArray()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + xmlrpcParamSetArray ( new_param ); + return new_param; +} + +XmlrpcParam * xmlrpcParamArrayPushBackStruct ( XmlrpcParam *param ) +{ + PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackStruct()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + xmlrpcParamSetStruct ( new_param ); + return new_param; +} + +XmlrpcParam * xmlrpcParamStructGetParam( XmlrpcParam *param, const char *name ) +{ + if ( param->type != XMLRPC_PARAM_STRUCT ) + { + PRINT_ERROR ( "xmlrpcParamStructGetParam() : Not a struct type param \n" ); + return NULL; + } + + int it = 0; + for (; it < param->array_n_elem; it++) + { + XmlrpcParam *param_arr = ¶m->data.as_array[it]; + if (strcmp(param_arr->member_name, name) == 0) + return param_arr; + } + + return NULL; +} + +XmlrpcParam * xmlrpcParamStructPushBackBool( XmlrpcParam *param, const char *name, int val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamStructPushBackBool()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + paramSetMemberName(new_param, name); + xmlrpcParamSetBool ( new_param, val ); + return new_param; +} + +XmlrpcParam * xmlrpcParamStructPushBackInt( XmlrpcParam *param, const char *name, int32_t val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamStructPushBackInt()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + paramSetMemberName(new_param, name); + xmlrpcParamSetInt ( new_param, val ); + return new_param; +} + +XmlrpcParam * xmlrpcParamStructPushBackDouble( XmlrpcParam *param, const char *name, double val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamStructPushBackDouble()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + paramSetMemberName(new_param, name); + xmlrpcParamSetDouble ( new_param, val ); + return new_param; +} + +XmlrpcParam * xmlrpcParamStructPushBackString( XmlrpcParam *param, const char *name, const char *val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamStructPushBackString()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + paramSetMemberName(new_param, name); + xmlrpcParamSetString ( new_param, val ); + return new_param; +} + +XmlrpcParam * xmlrpcParamStructPushBackStringN( XmlrpcParam *param, const char *name, const char *val, int n ) +{ + PRINT_VVDEBUG ( "xmlrpcParamStructPushBackStringN()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + paramSetMemberName(new_param, name); + xmlrpcParamSetStringN ( new_param, val, n ); + return new_param; +} + +XmlrpcParam * xmlrpcParamStructPushBackArray( XmlrpcParam *param, const char *name ) +{ + PRINT_VVDEBUG ( "xmlrpcParamStructPushBackArray()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + paramSetMemberName(new_param, name); + xmlrpcParamSetArray ( new_param ); + return new_param; +} + +XmlrpcParam * xmlrpcParamStructPushBackStruct ( XmlrpcParam *param, const char *name ) +{ + PRINT_VVDEBUG ( "xmlrpcParamStructPushBackStruct()\n" ); + XmlrpcParam *new_param = arrayAddElem ( param ); + if ( new_param == NULL ) + return NULL; + + paramSetMemberName(new_param, name); + xmlrpcParamSetStruct ( new_param ); + return new_param; +} + +int paramSetMemberName ( XmlrpcParam *param, const char *name ) +{ + param->member_name = (char *)malloc(strlen(name) + 1); + if (param->member_name == NULL) + return -1; + strcpy(param->member_name, name); + + return 0; +} + +void xmlrpcParamInit( XmlrpcParam *param ) +{ + param->type = XMLRPC_PARAM_UNKNOWN; + param->member_name = NULL; + memset(param->data.opaque, 0, sizeof(param->data.opaque)); + param->array_n_elem = -1; + param->array_max_elem = -1; +} + +void xmlrpcParamRelease ( XmlrpcParam *param ) +{ + PRINT_VVDEBUG ( "xmlrpcParamReleaseData()\n" ); + + free(param->member_name); + + switch ( param->type ) + { + case XMLRPC_PARAM_BOOL: + case XMLRPC_PARAM_INT: + case XMLRPC_PARAM_DOUBLE: + break; + + case XMLRPC_PARAM_STRING: + if ( param->data.as_string != NULL ) + { + free ( param->data.as_string ); + param->data.as_string = NULL; + } + break; + + case XMLRPC_PARAM_ARRAY: + case XMLRPC_PARAM_STRUCT: + if ( param->data.as_array != NULL ) + { + int i; + for ( i = 0; i< param->array_n_elem; i++ ) + xmlrpcParamRelease ( & ( param->data.as_array[i] ) ); + free ( param->data.as_array ); + param->data.as_array = NULL; + } + param->array_n_elem = 0; + param->array_max_elem = 0; + break; + + case XMLRPC_PARAM_DATETIME: /* WARNING: Currently unsupported */ + PRINT_ERROR ( "xmlrpcParamReleaseData() : ERROR: Parameter type datetime not yet supported\n" ); + break; + case XMLRPC_PARAM_BINARY: /* WARNING: Currently unsupported */ + PRINT_ERROR ( "xmlrpcParamReleaseData() : ERROR: Parameter type binary not yet supported\n" ); + break; + case XMLRPC_PARAM_UNKNOWN: + break; + default: + PRINT_VVDEBUG ( "xmlrpcParamReleaseData() : Unknown parameter \n" ); + break; + } +} + +void xmlrpcParamToXml ( XmlrpcParam *param, DynString *message ) +{ + PRINT_VVDEBUG ( "xmlrpcParamToXml()\n" ); + + int struct_member = 0; + if (param->member_name != NULL) + { + dynStringPushBackStr ( message, XMLRPC_MEMBER_TAG.str ); + dynStringPushBackStr ( message, XMLRPC_NAME_TAG.str ); + dynStringPushBackStr ( message, param->member_name ); + dynStringPushBackStr ( message, XMLRPC_NAME_ETAG.str ); + struct_member = 1; + } + + switch ( param->type ) + { + case XMLRPC_PARAM_BOOL: + boolToXml ( param->data.as_bool, message ); + break; + case XMLRPC_PARAM_INT: + intToXml ( param->data.as_int, message ); + break; + case XMLRPC_PARAM_DOUBLE: + doubleToXml ( param->data.as_double, message ); + break; + case XMLRPC_PARAM_STRING: + stringToXml ( param->data.as_string, message ); + break; + case XMLRPC_PARAM_ARRAY: + arrayToXml ( param, message ); + break; + case XMLRPC_PARAM_STRUCT: + structToXml ( param, message ); + break; + case XMLRPC_PARAM_DATETIME: + timeToXml ( param->data.as_time, message ); + break; + case XMLRPC_PARAM_BINARY: + binaryToXml ( param->data.as_binary, message ); + break; + case XMLRPC_PARAM_UNKNOWN: + break; + default: + PRINT_ERROR ( "xmlrpcParamToXml() : Unsupported type\n" ); + break; + } + + if (struct_member) + dynStringPushBackStr ( message, XMLRPC_MEMBER_ETAG.str ); +} + +int xmlrpcParamFromXml ( DynString *message, XmlrpcParam *param ) +{ + PRINT_VVDEBUG ( "xmlrpcParamFromXml()\n" ); + + /* Save position indicator: it will be restored */ + int initial_pos_idx = dynStringGetPoseIndicatorOffset ( message ); + dynStringRewindPoseIndicator ( message ); + + int ret = paramFromXml ( message, param, PARAM_CONTAINER_NONE ); + + /* Restore position indicator */ + dynStringSetPoseIndicator ( message, initial_pos_idx ); + + return ret; +} + +static void paramArrayPrint( XmlrpcParam *param, char *head, int is_struct_member) +{ + int elem_ind; + + if ( param->data.as_array != NULL) + { + for ( elem_ind = 0; elem_ind < param->array_n_elem; elem_ind++ ) + { + char elem_head[20]; + snprintf(elem_head, 20, "Elem [%d]",elem_ind); + paramPrint( & ( param->data.as_array[elem_ind] ), elem_head , is_struct_member); + } + } + else + fprintf(cRosOutStreamGet(),"Empty array/struct\n"); +} + +static void paramPrint( XmlrpcParam *param, char *head, int is_struct_member) +{ + fprintf(cRosOutStreamGet(),"%s ", head); + + if(is_struct_member) + fprintf(cRosOutStreamGet(),"Member name: [%s] ", (param->member_name != NULL)? param->member_name:"NULL"); + + switch ( param->type ) + { + case XMLRPC_PARAM_BOOL: + fprintf(cRosOutStreamGet(),"Type : boolean Value : [%s]\n", param->data.as_bool?"TRUE":"FALSE" ); + break; + case XMLRPC_PARAM_INT: + fprintf(cRosOutStreamGet(),"Type : integer Value : [%d]\n", param->data.as_int ); + break; + case XMLRPC_PARAM_DOUBLE: + fprintf(cRosOutStreamGet(),"Type : double Value : [%f]\n", param->data.as_double ); + break; + case XMLRPC_PARAM_STRING: + fprintf(cRosOutStreamGet(),"Type : string Value : [%s]\n", (param->data.as_string != NULL)?param->data.as_string:"NULL" ); + break; + case XMLRPC_PARAM_ARRAY: + fprintf(cRosOutStreamGet(),"Type : array\n======== Array start ========\n"); + paramArrayPrint( param, head, 0); + fprintf(cRosOutStreamGet(),"========= Array end =========\n\n"); + break; + case XMLRPC_PARAM_STRUCT: + fprintf(cRosOutStreamGet(),"Type : struct\n======== Struct start ========\n"); + paramArrayPrint( param, head, 1); + fprintf(cRosOutStreamGet(),"========= Struct end =========\n\n"); + break; + + case XMLRPC_PARAM_DATETIME: /* WARNING: Currently unsupported */ + PRINT_ERROR ( "\nxmlrpcParamPrint() : Printing of parameter type datetime is not supported yet\n" ); + break; + case XMLRPC_PARAM_BINARY: /* WARNING: Currently unsupported */ + PRINT_ERROR ( "\nxmlrpcParamPrint() : Printing of parameter type binary is not supported.\n" ); + break; + default: + PRINT_ERROR ( "\nxmlrpcParamPrint() : Unknown parameter type\n" ); + break; + } +} + +void xmlrpcParamPrint( XmlrpcParam *param ) +{ + paramPrint( param, "XMLRPC parameter.", 0 ); // last parameter = 0 means that this parameter is not the member of a structure +} + +XmlrpcParam * xmlrpcParamNew(void) +{ + XmlrpcParam *ret = (XmlrpcParam *)malloc(sizeof(XmlrpcParam)); + if (ret == NULL) + return NULL; + + xmlrpcParamInit(ret); + + return ret; +} + +void xmlrpcParamFree( XmlrpcParam *param ) +{ + if (param == NULL) + return; + + xmlrpcParamRelease(param); + free(param); +} + +XmlrpcParam * xmlrpcParamClone(XmlrpcParam *source) +{ + XmlrpcParam *ret = (XmlrpcParam *)malloc(sizeof(XmlrpcParam)); + if (ret == NULL) + return NULL; + + int rc = xmlrpcParamCopy(ret, source); + if (rc == -1) + { + free(ret); + return NULL; + } + + return ret; +} + +int xmlrpcParamCopy(XmlrpcParam *dest, XmlrpcParam *source) +{ + int ret_val; + + memcpy(dest, source, sizeof(XmlrpcParam)); + if (source->member_name != NULL) + { + dest->member_name = (char *)malloc(strlen(source->member_name) + 1); + if(dest->member_name != NULL) + strcpy(dest->member_name, source->member_name); + else + return -1; + } + + ret_val = 0; // Default return value = 0 (success) + switch ( source->type ) + { + case XMLRPC_PARAM_BOOL: + case XMLRPC_PARAM_INT: + case XMLRPC_PARAM_DOUBLE: + break; + case XMLRPC_PARAM_STRING: + dest->data.as_string = (char *)malloc(strlen(source->data.as_string) + 1); + if (dest->data.as_string != NULL) + strcpy(dest->data.as_string, source->data.as_string); + else + ret_val = -1; // Failure allocating memory + break; + case XMLRPC_PARAM_ARRAY: + case XMLRPC_PARAM_STRUCT: + dest->data.as_array = (XmlrpcParam *)calloc(source->array_n_elem, sizeof(XmlrpcParam)); + if (dest->data.as_array != NULL) + { + int it; + for (it = 0; it < source->array_n_elem && ret_val == 0; it++) + ret_val = xmlrpcParamCopy(&dest->data.as_array[it], &source->data.as_array[it]); + } + else + ret_val = -1; + break; + case XMLRPC_PARAM_DATETIME: + case XMLRPC_PARAM_BINARY: + PRINT_ERROR ( "xmlrpcParamToXml() : Unsupported type in source XmlrpcParam (binary or datetime)\n" ); + ret_val = -1; + break; + case XMLRPC_PARAM_UNKNOWN: + break; + default: + PRINT_ERROR ( "xmlrpcParamToXml() : Unsupported type in source XmlrpcParam\n" ); + ret_val = -1; + } + + if(ret_val != 0) // If failure, free param before exit + xmlrpcParamRelease(dest); + + return(ret_val); +} diff --git a/src/hal/components/cros/src/xmlrpc_params_vector.c b/src/hal/components/cros/src/xmlrpc_params_vector.c new file mode 100644 index 00000000..86a6314a --- /dev/null +++ b/src/hal/components/cros/src/xmlrpc_params_vector.c @@ -0,0 +1,171 @@ +#include + +#include "xmlrpc_params_vector.h" +#include "cros_defs.h" +#include "cros_log.h" + +enum { XMLRPC_VECTOR_INIT_SIZE = 4, XMLRPC_VECTOR_GROW_RATE = 2 }; + +void xmlrpcParamVectorInit ( XmlrpcParamVector *p_vec ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorInit()\n" ); + + p_vec->data = NULL; + p_vec->size = 0; + p_vec->max = 0; +} + +void xmlrpcParamVectorRelease ( XmlrpcParamVector *p_vec ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorRelease()\n" ); + + if ( p_vec->data == NULL ) + return; + + int i; + for ( i = 0; i < p_vec->size; i++ ) + xmlrpcParamRelease ( & ( p_vec->data[i] ) ); + + free ( p_vec->data ); + p_vec->data = NULL; + + p_vec->size = p_vec->max = 0; +} + +int xmlrpcParamVectorPushBackBool ( XmlrpcParamVector *p_vec, int val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackBool()\n" ); + + XmlrpcParam param; + xmlrpcParamInit(¶m); + xmlrpcParamSetBool ( ¶m, val ); + return xmlrpcParamVectorPushBack ( p_vec, ¶m ); +} + +int xmlrpcParamVectorPushBackInt ( XmlrpcParamVector *p_vec, int32_t val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackInt()\n" ); + + XmlrpcParam param; + xmlrpcParamInit(¶m); + xmlrpcParamSetInt ( ¶m, val ); + return xmlrpcParamVectorPushBack ( p_vec, ¶m ); +} + +int xmlrpcParamVectorPushBackDouble ( XmlrpcParamVector *p_vec, double val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackDouble()\n" ); + + XmlrpcParam param; + xmlrpcParamInit(¶m); + xmlrpcParamSetDouble ( ¶m, val ); + return xmlrpcParamVectorPushBack ( p_vec, ¶m ); +} + +int xmlrpcParamVectorPushBackString ( XmlrpcParamVector *p_vec, const char *val ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackString()\n" ); + + XmlrpcParam param; + xmlrpcParamInit(¶m); + xmlrpcParamSetString ( ¶m, val ); + return xmlrpcParamVectorPushBack ( p_vec, ¶m ); +} + +int xmlrpcParamVectorPushBackArray ( XmlrpcParamVector *p_vec ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackArray()\n" ); + + XmlrpcParam param; + xmlrpcParamInit(¶m); + xmlrpcParamSetArray ( ¶m ); + return xmlrpcParamVectorPushBack ( p_vec, ¶m ); +} + +int xmlrpcParamVectorPushBackStruct ( XmlrpcParamVector *p_vec ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackStruct()\n" ); + + XmlrpcParam param; + xmlrpcParamInit(¶m); + xmlrpcParamSetStruct ( ¶m ); + return xmlrpcParamVectorPushBack ( p_vec, ¶m ); +} + +int xmlrpcParamVectorPushBack ( XmlrpcParamVector *p_vec, XmlrpcParam *param ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBack()\n" ); + + if ( param == NULL ) + { + PRINT_ERROR ( "xmlrpcParamVectorPushBack() : Invalid new param\n" ); + return -1; + } + + if ( p_vec->data == NULL ) + { + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBack() : allocate memory for the first time\n" ); + p_vec->data = ( XmlrpcParam * ) malloc ( XMLRPC_VECTOR_INIT_SIZE * sizeof ( XmlrpcParam ) ); + + if ( p_vec->data == NULL ) + { + PRINT_ERROR ( "xmlrpcParamVectorPushBack() : Can't allocate memory\n" ); + return -1; + } + + p_vec->size = 0; + p_vec->max = XMLRPC_VECTOR_INIT_SIZE; + } + + while ( p_vec->size == p_vec->max ) + { + PRINT_VVDEBUG ( "xmlrpcParamVectorPushBack() : reallocate memory\n" ); + XmlrpcParam *new_p_vec = ( XmlrpcParam * ) realloc ( p_vec->data, + ( XMLRPC_VECTOR_GROW_RATE* p_vec->max ) * sizeof ( XmlrpcParam ) ); + if ( new_p_vec == NULL ) + { + PRINT_ERROR ( "xmlrpcParamVectorPushBack() : Can't allocate more memory\n" ); + return -1; + } + p_vec->max *= XMLRPC_VECTOR_GROW_RATE; + p_vec->data = new_p_vec; + } + + p_vec->data[p_vec->size] = *param; + p_vec->size += 1; + + return p_vec->size; +} + +int xmlrpcParamVectorGetSize ( XmlrpcParamVector *p_vec ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorGetSize()\n" ); + + return p_vec->size; +} + +XmlrpcParam *xmlrpcParamVectorAt ( XmlrpcParamVector *p_vec, int pos ) +{ + PRINT_VVDEBUG ( "xmlrpcParamVectorAt()\n" ); + + if ( pos < 0 || pos >= p_vec->size ) + { + PRINT_ERROR ( "xmlrpcParamVectorAt() : index out of range\n" ); + return NULL; + } + + return & ( p_vec->data[pos] ); +} + +void xmlrpcParamVectorPrint( XmlrpcParamVector *p_vec ) +{ + int elem_ind; + + PRINT_VDEBUG ( "XMLRPC parameter vector (size : %d):\n", p_vec->size); + + for(elem_ind = 0 ; elem_ind < p_vec->size; elem_ind++) + { + PRINT_VDEBUG ( "Vec. elem [%i]: ", elem_ind ); + xmlrpcParamPrint( xmlrpcParamVectorAt( p_vec, elem_ind ) ); + } +} diff --git a/src/hal/components/cros/src/xmlrpc_process.c b/src/hal/components/cros/src/xmlrpc_process.c new file mode 100644 index 00000000..3f3e895e --- /dev/null +++ b/src/hal/components/cros/src/xmlrpc_process.c @@ -0,0 +1,62 @@ +#include +#include + +#include "xmlrpc_process.h" +#include "cros_clock.h" + +void xmlrpcProcessInit( XmlrpcProcess *p ) +{ + p->current_call = NULL; + p->state = XMLRPC_PROCESS_STATE_IDLE; + tcpIpSocketInit( &(p->socket) ); + p->message_type = XMLRPC_MESSAGE_UNKNOWN; + dynStringInit( &(p->method) ); + dynStringInit( &(p->message) ); + xmlrpcParamVectorInit( &(p->params) ); + xmlrpcParamVectorInit( &(p->response) ); + p->last_change_time = 0; + memset(p->host, 0, sizeof(p->host)); + p->port = -1; +} + +void xmlrpcProcessRelease( XmlrpcProcess *p ) +{ + if( p->socket.connected ) + tcpIpSocketDisconnect( &(p->socket) ); + + if (p->current_call != NULL) + freeRosApiCall(p->current_call); + + tcpIpSocketClose( &(p->socket) ); + dynStringRelease( &(p->method) ); + dynStringRelease( &(p->message) ); + xmlrpcParamVectorRelease( &(p->params) ); + xmlrpcParamVectorRelease( &(p->response) ); +} + +void xmlrpcProcessClear( XmlrpcProcess *p) +{ + dynStringClear(&p->message); +} + +void xmlrpcProcessReset( XmlrpcProcess *p) +{ + xmlrpcProcessClear(p); + + if (p->current_call != NULL) + freeRosApiCall(p->current_call); + + p->current_call = NULL; + dynStringClear(&p->method); + xmlrpcParamVectorRelease(&p->params); + xmlrpcParamVectorRelease(&p->response); + p->message_type = XMLRPC_MESSAGE_UNKNOWN; + memset(p->host, 0, sizeof(p->host)); + p->port = -1; +} + +void xmlrpcProcessChangeState( XmlrpcProcess *p, XmlrpcProcessState state ) +{ + p->state = state; + p->last_change_time = cRosClockGetTimeMs(); +} diff --git a/src/hal/components/cros/src/xmlrpc_protocol.c b/src/hal/components/cros/src/xmlrpc_protocol.c new file mode 100644 index 00000000..96744e56 --- /dev/null +++ b/src/hal/components/cros/src/xmlrpc_protocol.c @@ -0,0 +1,410 @@ +#include +#include +#include +#include + +#ifdef _WIN32 +# define strtok_r strtok_s +# define strncasecmp _strnicmp // This is the POSIX verion of strnicmp +#endif + +#include "xmlrpc_protocol.h" +#include "xmlrpc_tags.h" +#include "cros_defs.h" +#include "cros_log.h" + +static XmlrpcParserState parseXmlrpcMessageParams ( const char *params_body, int params_body_len, + XmlrpcParamVector *params ) +{ + PRINT_VVDEBUG ( "parseXmlrpcMessageParams()\n" ); + + int i = 0; + const char *c = params_body; + int found_params = 0; + int found_fault = 0; + + for ( ; i < params_body_len; i++, c++ ) + { + if ( params_body_len - i >= XMLRPC_PARAMS_TAG.dim && + strncmp ( c, XMLRPC_PARAMS_TAG.str, XMLRPC_PARAMS_TAG.dim ) == 0 ) + { + c += XMLRPC_PARAMS_TAG.dim; + i += XMLRPC_PARAMS_TAG.dim; + found_params = 1; + break; + } + else + if ( params_body_len - i >= XMLRPC_FAULT_TAG.dim && + strncmp ( c, XMLRPC_FAULT_TAG.str, XMLRPC_FAULT_TAG.dim ) == 0 ) + { + c += XMLRPC_FAULT_TAG.dim; + i += XMLRPC_FAULT_TAG.dim; + found_fault = 1; + break; + } + } + + if ( !found_params && !found_fault) + { + PRINT_ERROR ( "parseXmlrpcMessageParams() : neither tag nor tag have been found in the ROS master XML response.\n" ); + return XMLRPC_PARSER_ERROR; + } + + if ( found_params ) + { + DynString param_str; + dynStringInit ( ¶m_str ); + int param_str_len = 0; + const char *param_init = NULL; + + for ( ; i < params_body_len; i++, c++ ) + { + if ( params_body_len - i >= XMLRPC_PARAMS_ETAG.dim && + strncmp ( c, XMLRPC_PARAMS_ETAG.str, XMLRPC_PARAMS_ETAG.dim ) == 0 ) + { + c += XMLRPC_PARAMS_ETAG.dim; + i += XMLRPC_PARAMS_ETAG.dim; + break; + } + + if ( param_init == NULL ) + { + if ( params_body_len - i >= XMLRPC_PARAM_TAG.dim && + strncmp ( c, XMLRPC_PARAM_TAG.str, XMLRPC_PARAM_TAG.dim ) == 0 ) + { + c += XMLRPC_PARAM_TAG.dim - 1; + i += XMLRPC_PARAM_TAG.dim - 1; + param_init = c + 1; + param_str_len = 0; + } + } + else + { + if ( params_body_len - i >= XMLRPC_PARAM_ETAG.dim && + strncmp ( c, XMLRPC_PARAM_ETAG.str, XMLRPC_PARAM_ETAG.dim ) == 0 ) + { + if ( param_str_len ) + { + dynStringReplaceWithStrN ( ¶m_str, param_init, param_str_len ); + + XmlrpcParam param; + xmlrpcParamInit(¶m); + + if ( xmlrpcParamFromXml ( ¶m_str, ¶m ) == -1 || + xmlrpcParamVectorPushBack ( params, ¶m ) < 0 ) + return XMLRPC_PARSER_ERROR; + } + + c += XMLRPC_PARAM_ETAG.dim - 1; + i += XMLRPC_PARAM_ETAG.dim - 1; + param_init = NULL; + param_str_len = 0; + } + else + param_str_len++; + } + } + dynStringRelease ( ¶m_str ); + } + else // Found tag + { + DynString fault_str; + dynStringInit ( &fault_str ); + int fault_str_len = 0; + const char *fault_init = c; + + for ( ; i < params_body_len; i++, c++ ) + { + if ( params_body_len - i >= XMLRPC_FAULT_ETAG.dim && + strncmp ( c, XMLRPC_FAULT_ETAG.str, XMLRPC_FAULT_ETAG.dim ) == 0 ) + { + if ( fault_str_len ) + { + dynStringReplaceWithStrN ( &fault_str, fault_init, fault_str_len ); + + XmlrpcParam param; + xmlrpcParamInit(¶m); + + if ( xmlrpcParamFromXml ( &fault_str, ¶m ) == -1 || + xmlrpcParamVectorPushBack ( params, ¶m ) < 0 ) + return XMLRPC_PARSER_ERROR; + } + + c += XMLRPC_FAULT_ETAG.dim - 1; + i += XMLRPC_FAULT_ETAG.dim - 1; + fault_init = NULL; + fault_str_len = 0; + } + else + fault_str_len++; + } + dynStringRelease ( &fault_str ); + } + + return XMLRPC_PARSER_DONE; +} + +static XmlrpcParserState parseXmlrpcMessageBody ( const char *body, int body_len, XmlrpcMessageType *type, + DynString *method, XmlrpcParamVector *params ) +{ + PRINT_VVDEBUG ( "parseXmlrpcMessageBody()\n" ); + + *type = XMLRPC_MESSAGE_UNKNOWN; + int i = 0; + const char *c = body; + + for ( ; i < body_len; i++, c++ ) + { + if ( body_len - i >= XMLRPC_REQUEST_BEGIN.dim && + strncmp ( c, XMLRPC_REQUEST_BEGIN.str, XMLRPC_REQUEST_BEGIN.dim ) == 0 ) + { + c += XMLRPC_REQUEST_BEGIN.dim; + i += XMLRPC_REQUEST_BEGIN.dim; + *type = XMLRPC_MESSAGE_REQUEST; + break; + } + else if ( body_len - i >= XMLRPC_RESPONSE_BEGIN.dim && + strncmp ( c, XMLRPC_RESPONSE_BEGIN.str, XMLRPC_RESPONSE_BEGIN.dim ) == 0 ) + { + c += XMLRPC_RESPONSE_BEGIN.dim; + i += XMLRPC_RESPONSE_BEGIN.dim; + *type = XMLRPC_MESSAGE_RESPONSE; + break; + } + } + + if ( *type == XMLRPC_MESSAGE_UNKNOWN ) + { + PRINT_ERROR ( "parseXmlrpcMessageBody() : unknown message type\n" ); + return XMLRPC_PARSER_ERROR; + } + + if ( *type == XMLRPC_MESSAGE_REQUEST ) + { + const char *method_name_init = NULL, *method_name_end = NULL; + int method_name_size = 0; + for ( ; i < body_len; i++, c++ ) + { + if ( body_len - i >= XMLRPC_METHODNAME_BEGIN.dim && + strncmp ( c, XMLRPC_METHODNAME_BEGIN.str, XMLRPC_METHODNAME_BEGIN.dim ) == 0 ) + { + c += XMLRPC_METHODNAME_BEGIN.dim; + i += XMLRPC_METHODNAME_BEGIN.dim; + method_name_init = c; + break; + } + } + + if ( method_name_init == NULL ) + { + PRINT_ERROR ( "parseXmlrpcMessageBody() : missing method name\n" ); + return XMLRPC_PARSER_ERROR; + } + + for ( ; i < body_len; i++, c++ ) + { + if ( body_len - i >= XMLRPC_METHODNAME_END.dim && + strncmp ( c, XMLRPC_METHODNAME_END.str, XMLRPC_METHODNAME_END.dim ) == 0 ) + { + method_name_end = c - 1; + c += XMLRPC_METHODNAME_END.dim; + i += XMLRPC_METHODNAME_END.dim; + break; + } + else + method_name_size++; + } + + if ( method_name_end == NULL ) + { + PRINT_ERROR ( "parseXmlrpcMessageBody() : missing method name\n" ); + return XMLRPC_PARSER_ERROR; + } + + if ( dynStringPushBackStrN(method, method_name_init, method_name_size ) < 0 ) + return XMLRPC_PARSER_ERROR; + } + + if( *type == XMLRPC_MESSAGE_REQUEST ) + PRINT_VDEBUG("Received request message, method : %s\n", dynStringGetData( method )); + else if ( *type == XMLRPC_MESSAGE_RESPONSE ) + PRINT_VDEBUG("Received response message\n"); + + return parseXmlrpcMessageParams ( c, body_len - i, params ); +} + +void generateXmlrpcMessage ( const char*host, unsigned short port, XmlrpcMessageType type, + const char *method, XmlrpcParamVector *params, DynString *message ) +{ + PRINT_VVDEBUG ( "generateXmlrpcMessage()\n" ); + + dynStringClear ( message ); + + if( type == XMLRPC_MESSAGE_REQUEST ) + { + dynStringPushBackStr ( message, "POST " ); + dynStringPushBackStr ( message, "/" ); // uri + dynStringPushBackStr ( message, " HTTP/1.1\r\n" ); + dynStringPushBackStr ( message, "User-Agent: " ); + dynStringPushBackStr ( message, XMLRPC_VERSION.str ); + dynStringPushBackStr ( message, "\r\nHost: " ); + if(host != NULL) + { + char port_str[50]; + dynStringPushBackStr ( message, host ); + snprintf ( port_str, 50, ":%d", port ); + dynStringPushBackStr ( message, port_str ); + } + dynStringPushBackStr ( message, "\r\n" ); + } + else if( type == XMLRPC_MESSAGE_RESPONSE ) + { + dynStringPushBackStr ( message, "HTTP/1.1 200 OK\r\n" ); + dynStringPushBackStr ( message, "Server: " ); + dynStringPushBackStr ( message, XMLRPC_VERSION.str ); + dynStringPushBackStr ( message, "\r\n" ); + } + else + { + PRINT_ERROR ( "generateXmlrpcMessage() : Unknown message type\n" ); + } + + dynStringPushBackStr ( message, "Content-Type: text/xml\r\nContent-length: " ); + + int content_len_init = dynStringGetLen ( message ); + + dynStringPushBackStr ( message, " \r\n\r\n" ); + int content_init = dynStringGetLen ( message ); + + dynStringPushBackStr ( message, XMLRPC_MESSAGE_BEGIN.str ); + + if ( type == XMLRPC_MESSAGE_REQUEST ) + { + dynStringPushBackStr ( message, XMLRPC_REQUEST_BEGIN.str ); + dynStringPushBackStr ( message, XMLRPC_METHODNAME_BEGIN.str ); + dynStringPushBackStr ( message, method ); + dynStringPushBackStr ( message, XMLRPC_METHODNAME_END.str ); + } + else if ( type == XMLRPC_MESSAGE_RESPONSE ) + { + dynStringPushBackStr ( message, XMLRPC_RESPONSE_BEGIN.str ); + } + int n_params = xmlrpcParamVectorGetSize ( params ); + if ( n_params > 0 ) + { + int i = 0; + dynStringPushBackStr ( message, XMLRPC_PARAMS_TAG.str ); + + for ( i = 0; i < n_params; i++ ) + { + dynStringPushBackStr ( message, XMLRPC_PARAM_TAG.str ); + xmlrpcParamToXml ( xmlrpcParamVectorAt ( params, i ), message ); + dynStringPushBackStr ( message, XMLRPC_PARAM_ETAG.str ); + } + dynStringPushBackStr ( message, XMLRPC_PARAMS_ETAG.str ); + } + + if ( type == XMLRPC_MESSAGE_REQUEST ) + dynStringPushBackStr ( message, XMLRPC_REQUEST_END.str ); + else if ( type == XMLRPC_MESSAGE_RESPONSE ) + dynStringPushBackStr ( message, XMLRPC_RESPONSE_END.str ); + + dynStringPushBackStr ( message, XMLRPC_MESSAGE_END.str ); + + int content_end = dynStringGetLen ( message ); + int content_len = content_end - content_init; + + // Avoid writing numbers with more than the 10 digits + if(content_len > 9999999999L) + { + content_len = (INT_MAX < 9999999999L)?INT_MAX:9999999999L; + PRINT_VVDEBUG ( "generateXmlrpcMessage(): POST content too long. Trimming to %d.\n", content_len ); + } + char content_len_str[12]; + snprintf ( content_len_str, 12, "%010d", content_len ); + + dynStringPatch ( message, content_len_str, content_len_init ); +} + +XmlrpcParserState parseXmlrpcMessage(DynString *message, XmlrpcMessageType *type, + DynString *method, XmlrpcParamVector *params, + char host[256], int *port) +{ + PRINT_VVDEBUG ( "parseXmlrpcMessage()\n" ); + + int msg_len = dynStringGetLen ( message ); + char *msg = ( char * ) dynStringGetData ( message ); + char *body_len_init = NULL; + char *body_init = NULL; + char *host_init = NULL; + + int i; + for ( i = 0; i < msg_len; i++, msg++ ) + { + if ( msg_len - i >= 16 && strncasecmp ( msg, "Content-length: ", 16 ) == 0 ) + { + body_len_init = msg + 16; + } + if ( msg_len - i >= 6 && strncasecmp ( msg, "Host: ", 16 ) == 0 ) + { + host_init = msg + 6; + } + else if ( msg_len - i >= 4 && strncmp ( msg, "\r\n\r\n", 4 ) == 0 ) + { + body_init = msg + 4; + break; + } + else if ( msg_len - i >= 2 && strncmp ( msg, "\n\n", 2 ) == 0 ) + { + body_init = msg + 2; + break; + } + } + + if ( body_init == NULL ) + { + PRINT_VDEBUG ( "parseXmlrpcMessage() : message incomplete\n" ); + return XMLRPC_PARSER_INCOMPLETE; + } + else if ( body_len_init == NULL ) + { + PRINT_ERROR ( "parseXmlrpcMessage() : Content-length not present\n" ); + return XMLRPC_PARSER_ERROR; + } + + int body_len = 0; + if ( sscanf ( body_len_init,"%d", &body_len ) != 1 ) + { + PRINT_ERROR ( "parseXmlrpcMessage() : Content-length not valid\n" ); + return XMLRPC_PARSER_ERROR; + } + + if ( body_len > (int)strlen ( body_init ) ) + { + PRINT_VDEBUG ( "parseXmlrpcMessage() : message incomplete\n" ); + return XMLRPC_PARSER_INCOMPLETE; + } + + if (host_init == NULL) + { + memset(host, 0, sizeof(256)); + *port = -1; + } + else + { + char temp_host[256]; + snprintf(temp_host, 256, "%s", host_init); // Temporary copy + char full_host[256]; + sscanf(full_host, "%s", temp_host); // Remove spaces and newlines + strncpy(temp_host,full_host+7,strlen(full_host)-8); // Remove 'http://' and '/' + char * progress = NULL; + char* host_ = strtok_r(temp_host,":",&progress); + strcpy(host, host_); + *port = atoi(strtok_r(NULL,":",&progress)); + } + + PRINT_VDEBUG ( "parseXmlrpcMessage() : body len : %d\n", body_len ); + + return parseXmlrpcMessageBody ( body_init, body_len, type, method, params ); + +} From 46223a499273cde44ecc5c0fc4e601f9df8be14f Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 02:45:24 -0500 Subject: [PATCH 02/22] cros as submodule and cros instalation added --- .gitmodules | 3 + src/hal/components/cros | 1 + src/hal/components/cros/.gitignore | 27 - src/hal/components/cros/CHANGELOG | 24 - src/hal/components/cros/CMakeLists.txt | 40 - src/hal/components/cros/Doxyfile | 1869 -------- src/hal/components/cros/LICENSE | 12 - src/hal/components/cros/NOTICE | 8 - src/hal/components/cros/README.md | 63 - src/hal/components/cros/docs/cROS_manual.doc | Bin 491008 -> 0 bytes src/hal/components/cros/docs/cROS_manual.pdf | Bin 319161 -> 0 bytes src/hal/components/cros/include/cros.h | 27 - src/hal/components/cros/include/cros_api.h | 407 -- .../components/cros/include/cros_api_call.h | 54 - .../cros/include/cros_api_internal.h | 138 - src/hal/components/cros/include/cros_clock.h | 59 - src/hal/components/cros/include/cros_defs.h | 77 - .../components/cros/include/cros_err_codes.h | 162 - .../components/cros/include/cros_gentools.h | 37 - src/hal/components/cros/include/cros_log.h | 67 - .../components/cros/include/cros_message.h | 209 - .../cros/include/cros_message_internal.h | 119 - .../cros/include/cros_message_queue.h | 122 - src/hal/components/cros/include/cros_node.h | 344 -- .../components/cros/include/cros_node_api.h | 100 - .../components/cros/include/cros_service.h | 34 - .../cros/include/cros_service_internal.h | 41 - src/hal/components/cros/include/cros_tcpros.h | 137 - src/hal/components/cros/include/dyn_buffer.h | 249 - src/hal/components/cros/include/dyn_string.h | 165 - src/hal/components/cros/include/md5.h | 43 - .../components/cros/include/tcpip_socket.h | 304 -- .../components/cros/include/tcpros_process.h | 95 - src/hal/components/cros/include/tcpros_tags.h | 99 - .../components/cros/include/xmlrpc_params.h | 280 -- .../cros/include/xmlrpc_params_vector.h | 126 - .../components/cros/include/xmlrpc_process.h | 81 - .../components/cros/include/xmlrpc_protocol.h | 56 - src/hal/components/cros/include/xmlrpc_tags.h | 67 - .../defusedxml/ElementTree.py | 143 - .../defusedxml/__init__.py | 62 - .../defusedxml/cElementTree.py | 40 - .../master_python_source/defusedxml/common.py | 134 - .../defusedxml/expatbuilder.py | 107 - .../defusedxml/expatreader.py | 61 - .../master_python_source/defusedxml/lxml.py | 155 - .../defusedxml/minidom.py | 63 - .../defusedxml/pulldom.py | 41 - .../master_python_source/defusedxml/sax.py | 60 - .../master_python_source/defusedxml/xmlrpc.py | 153 - .../master_python_source/rosgraph/__init__.py | 56 - .../rosgraph/impl/__init__.py | 34 - .../rosgraph/impl/graph.py | 582 --- .../rosgraph/masterapi.py | 481 -- .../master_python_source/rosgraph/names.py | 329 -- .../master_python_source/rosgraph/network.py | 419 -- .../master_python_source/rosgraph/rosenv.py | 75 - .../rosgraph/rosgraph_main.py | 103 - .../rosgraph/roslogging.py | 270 -- .../master_python_source/rosgraph/xmlrpc.py | 304 -- .../rosmaster/__init__.py | 38 - .../rosmaster/exceptions.py | 39 - .../master_python_source/rosmaster/main.py | 139 - .../master_python_source/rosmaster/master.py | 89 - .../rosmaster/master_api.py | 889 ---- .../rosmaster/paramserver.py | 402 -- .../rosmaster/registrations.py | 473 -- .../rosmaster/threadpool.py | 228 - .../master_python_source/rosmaster/util.py | 90 - .../rosmaster/validators.py | 199 - .../cros/master_python_source/runmaster.py | 25 - .../components/cros/msvs_projects/cros.sln | 99 - .../cros/msvs_projects/cros_library.vcxproj | 207 - .../cros_library.vcxproj.filters | 168 - .../parameters_test/parameters_test.vcxproj | 159 - .../parameters_test.vcxproj.filters | 22 - .../performance_test/performance_test.vcxproj | 159 - .../performance_test.vcxproj.filters | 22 - .../components/cros/msvs_projects/readme.txt | 50 - .../msvs_projects/ros_master_python.pyproj | 62 - .../sample_listener/sample_listener.vcxproj | 191 - .../sample_listener.vcxproj.filters | 22 - .../sample_talker/sample_talker.vcxproj | 191 - .../sample_talker.vcxproj.filters | 22 - src/hal/components/cros/resources/README | 3 - .../components/cros/resources/ROS-QUICKSTART | 29 - .../components/cros/resources/cROS_logo.jpg | Bin 13108 -> 0 bytes .../cros/resources/cros_testbed/.gitignore | 1 - .../cros/resources/cros_testbed/BUILD | 17 - .../resources/cros_testbed/CMakeLists.txt | 160 - .../cros/resources/cros_testbed/include/.keep | 0 .../cros_testbed/msg/DoubleVector.msg | 1 - .../cros/resources/cros_testbed/package.xml | 60 - .../cros_testbed/src/add_two_ints.cpp | 23 - .../cros_testbed/src/clock_listener.cpp | 58 - .../cros_testbed/src/counter_listener.cpp | 58 - .../cros_testbed/src/std_msgs_listener.cpp | 125 - .../cros_testbed/src/std_msgs_talker.cpp | 125 - .../cros_testbed/src/test_services.cpp | 36 - .../cros_testbed/src/vector_listener.cpp | 20 - .../cros_testbed/src/vector_talker.cpp | 87 - .../cros_testbed/srv/add_two_ints.srv | 4 - .../cros/resources/rosgraph_msgs/Log.msg | 19 - .../components/cros/samples/CMakeLists.txt | 19 - src/hal/components/cros/samples/api-test.c | 402 -- src/hal/components/cros/samples/listener.c | 223 - .../components/cros/samples/parameters-test.c | 249 - .../cros/samples/performance-test.cpp | 583 --- .../cros/samples/plot_performance.m | 28 - .../cros/samples/ros-i-trajectory-test.c | 219 - .../rosdb/gripping_robot/CloseGripper.srv | 3 - .../rosdb/gripping_robot/GripperJoints.msg | 1 - .../rosdb/gripping_robot/GripperStatus.msg | 4 - .../samples/rosdb/gripping_robot/MoveArm.srv | 5 - .../rosdb/gripping_robot/OpenGripper.srv | 3 - .../rosdb/gripping_robot/Reconfigure.srv | 4 - .../rosdb/gripping_robot/RestPosition.srv | 2 - .../samples/rosdb/gripping_robot/Transfer.srv | 2 - .../cros/samples/rosdb/roscpp/GetLoggers.srv | 2 - .../cros/samples/rosdb/roscpp/Logger.msg | 2 - .../samples/rosdb/roscpp/SetLoggerLevel.srv | 3 - .../rosdb/roscpp_tutorials/TwoInts.srv | 4 - .../cros/samples/rosdb/rosgraph_msgs/Log.msg | 19 - .../samples/rosdb/sensor_msgs/JointState.msg | 26 - .../cros/samples/rosdb/std_msgs/String.msg | 1 - .../rosdb/trajectory_msgs/JointTrajectory.msg | 3 - .../trajectory_msgs/JointTrajectoryPoint.msg | 9 - src/hal/components/cros/samples/talker.c | 152 - src/hal/components/cros/src/cros_api.c | 2305 ---------- src/hal/components/cros/src/cros_api_call.c | 112 - src/hal/components/cros/src/cros_clock.c | 122 - src/hal/components/cros/src/cros_err_codes.c | 163 - src/hal/components/cros/src/cros_gentools.c | 93 - src/hal/components/cros/src/cros_log.c | 239 - src/hal/components/cros/src/cros_message.c | 3280 -------------- .../components/cros/src/cros_message_queue.c | 138 - src/hal/components/cros/src/cros_node.c | 4005 ----------------- src/hal/components/cros/src/cros_node_api.c | 1275 ------ src/hal/components/cros/src/cros_service.c | 433 -- src/hal/components/cros/src/cros_tcpros.c | 1049 ----- src/hal/components/cros/src/dyn_buffer.c | 249 - src/hal/components/cros/src/dyn_string.c | 286 -- src/hal/components/cros/src/md5.c | 296 -- src/hal/components/cros/src/tcpip_socket.c | 844 ---- src/hal/components/cros/src/tcpros_process.c | 79 - src/hal/components/cros/src/xmlrpc_params.c | 1454 ------ .../cros/src/xmlrpc_params_vector.c | 171 - src/hal/components/cros/src/xmlrpc_process.c | 62 - src/hal/components/cros/src/xmlrpc_protocol.c | 410 -- 149 files changed, 4 insertions(+), 33258 deletions(-) create mode 100644 .gitmodules create mode 160000 src/hal/components/cros delete mode 100644 src/hal/components/cros/.gitignore delete mode 100644 src/hal/components/cros/CHANGELOG delete mode 100644 src/hal/components/cros/CMakeLists.txt delete mode 100644 src/hal/components/cros/Doxyfile delete mode 100644 src/hal/components/cros/LICENSE delete mode 100644 src/hal/components/cros/NOTICE delete mode 100644 src/hal/components/cros/README.md delete mode 100755 src/hal/components/cros/docs/cROS_manual.doc delete mode 100644 src/hal/components/cros/docs/cROS_manual.pdf delete mode 100644 src/hal/components/cros/include/cros.h delete mode 100644 src/hal/components/cros/include/cros_api.h delete mode 100644 src/hal/components/cros/include/cros_api_call.h delete mode 100644 src/hal/components/cros/include/cros_api_internal.h delete mode 100644 src/hal/components/cros/include/cros_clock.h delete mode 100644 src/hal/components/cros/include/cros_defs.h delete mode 100644 src/hal/components/cros/include/cros_err_codes.h delete mode 100644 src/hal/components/cros/include/cros_gentools.h delete mode 100644 src/hal/components/cros/include/cros_log.h delete mode 100644 src/hal/components/cros/include/cros_message.h delete mode 100644 src/hal/components/cros/include/cros_message_internal.h delete mode 100644 src/hal/components/cros/include/cros_message_queue.h delete mode 100644 src/hal/components/cros/include/cros_node.h delete mode 100644 src/hal/components/cros/include/cros_node_api.h delete mode 100644 src/hal/components/cros/include/cros_service.h delete mode 100644 src/hal/components/cros/include/cros_service_internal.h delete mode 100644 src/hal/components/cros/include/cros_tcpros.h delete mode 100644 src/hal/components/cros/include/dyn_buffer.h delete mode 100644 src/hal/components/cros/include/dyn_string.h delete mode 100644 src/hal/components/cros/include/md5.h delete mode 100644 src/hal/components/cros/include/tcpip_socket.h delete mode 100644 src/hal/components/cros/include/tcpros_process.h delete mode 100644 src/hal/components/cros/include/tcpros_tags.h delete mode 100644 src/hal/components/cros/include/xmlrpc_params.h delete mode 100644 src/hal/components/cros/include/xmlrpc_params_vector.h delete mode 100644 src/hal/components/cros/include/xmlrpc_process.h delete mode 100644 src/hal/components/cros/include/xmlrpc_protocol.h delete mode 100644 src/hal/components/cros/include/xmlrpc_tags.h delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/ElementTree.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/__init__.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/cElementTree.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/common.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/expatbuilder.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/expatreader.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/lxml.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/minidom.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/pulldom.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/sax.py delete mode 100644 src/hal/components/cros/master_python_source/defusedxml/xmlrpc.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/__init__.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/impl/__init__.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/impl/graph.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/masterapi.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/names.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/network.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/rosenv.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/rosgraph_main.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/roslogging.py delete mode 100644 src/hal/components/cros/master_python_source/rosgraph/xmlrpc.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/__init__.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/exceptions.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/main.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/master.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/master_api.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/paramserver.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/registrations.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/threadpool.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/util.py delete mode 100644 src/hal/components/cros/master_python_source/rosmaster/validators.py delete mode 100644 src/hal/components/cros/master_python_source/runmaster.py delete mode 100644 src/hal/components/cros/msvs_projects/cros.sln delete mode 100644 src/hal/components/cros/msvs_projects/cros_library.vcxproj delete mode 100644 src/hal/components/cros/msvs_projects/cros_library.vcxproj.filters delete mode 100644 src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj delete mode 100644 src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj.filters delete mode 100644 src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj delete mode 100644 src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj.filters delete mode 100644 src/hal/components/cros/msvs_projects/readme.txt delete mode 100644 src/hal/components/cros/msvs_projects/ros_master_python.pyproj delete mode 100644 src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj delete mode 100644 src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj.filters delete mode 100644 src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj delete mode 100644 src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj.filters delete mode 100644 src/hal/components/cros/resources/README delete mode 100644 src/hal/components/cros/resources/ROS-QUICKSTART delete mode 100644 src/hal/components/cros/resources/cROS_logo.jpg delete mode 100755 src/hal/components/cros/resources/cros_testbed/.gitignore delete mode 100755 src/hal/components/cros/resources/cros_testbed/BUILD delete mode 100755 src/hal/components/cros/resources/cros_testbed/CMakeLists.txt delete mode 100755 src/hal/components/cros/resources/cros_testbed/include/.keep delete mode 100755 src/hal/components/cros/resources/cros_testbed/msg/DoubleVector.msg delete mode 100755 src/hal/components/cros/resources/cros_testbed/package.xml delete mode 100755 src/hal/components/cros/resources/cros_testbed/src/add_two_ints.cpp delete mode 100755 src/hal/components/cros/resources/cros_testbed/src/clock_listener.cpp delete mode 100755 src/hal/components/cros/resources/cros_testbed/src/counter_listener.cpp delete mode 100755 src/hal/components/cros/resources/cros_testbed/src/std_msgs_listener.cpp delete mode 100755 src/hal/components/cros/resources/cros_testbed/src/std_msgs_talker.cpp delete mode 100755 src/hal/components/cros/resources/cros_testbed/src/test_services.cpp delete mode 100755 src/hal/components/cros/resources/cros_testbed/src/vector_listener.cpp delete mode 100755 src/hal/components/cros/resources/cros_testbed/src/vector_talker.cpp delete mode 100755 src/hal/components/cros/resources/cros_testbed/srv/add_two_ints.srv delete mode 100644 src/hal/components/cros/resources/rosgraph_msgs/Log.msg delete mode 100644 src/hal/components/cros/samples/CMakeLists.txt delete mode 100644 src/hal/components/cros/samples/api-test.c delete mode 100644 src/hal/components/cros/samples/listener.c delete mode 100644 src/hal/components/cros/samples/parameters-test.c delete mode 100644 src/hal/components/cros/samples/performance-test.cpp delete mode 100644 src/hal/components/cros/samples/plot_performance.m delete mode 100644 src/hal/components/cros/samples/ros-i-trajectory-test.c delete mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/CloseGripper.srv delete mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/GripperJoints.msg delete mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/GripperStatus.msg delete mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/MoveArm.srv delete mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/OpenGripper.srv delete mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/Reconfigure.srv delete mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/RestPosition.srv delete mode 100644 src/hal/components/cros/samples/rosdb/gripping_robot/Transfer.srv delete mode 100644 src/hal/components/cros/samples/rosdb/roscpp/GetLoggers.srv delete mode 100644 src/hal/components/cros/samples/rosdb/roscpp/Logger.msg delete mode 100644 src/hal/components/cros/samples/rosdb/roscpp/SetLoggerLevel.srv delete mode 100755 src/hal/components/cros/samples/rosdb/roscpp_tutorials/TwoInts.srv delete mode 100644 src/hal/components/cros/samples/rosdb/rosgraph_msgs/Log.msg delete mode 100644 src/hal/components/cros/samples/rosdb/sensor_msgs/JointState.msg delete mode 100644 src/hal/components/cros/samples/rosdb/std_msgs/String.msg delete mode 100644 src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectory.msg delete mode 100644 src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectoryPoint.msg delete mode 100644 src/hal/components/cros/samples/talker.c delete mode 100644 src/hal/components/cros/src/cros_api.c delete mode 100644 src/hal/components/cros/src/cros_api_call.c delete mode 100644 src/hal/components/cros/src/cros_clock.c delete mode 100644 src/hal/components/cros/src/cros_err_codes.c delete mode 100644 src/hal/components/cros/src/cros_gentools.c delete mode 100644 src/hal/components/cros/src/cros_log.c delete mode 100644 src/hal/components/cros/src/cros_message.c delete mode 100644 src/hal/components/cros/src/cros_message_queue.c delete mode 100644 src/hal/components/cros/src/cros_node.c delete mode 100644 src/hal/components/cros/src/cros_node_api.c delete mode 100644 src/hal/components/cros/src/cros_service.c delete mode 100644 src/hal/components/cros/src/cros_tcpros.c delete mode 100644 src/hal/components/cros/src/dyn_buffer.c delete mode 100644 src/hal/components/cros/src/dyn_string.c delete mode 100644 src/hal/components/cros/src/md5.c delete mode 100644 src/hal/components/cros/src/tcpip_socket.c delete mode 100644 src/hal/components/cros/src/tcpros_process.c delete mode 100644 src/hal/components/cros/src/xmlrpc_params.c delete mode 100644 src/hal/components/cros/src/xmlrpc_params_vector.c delete mode 100644 src/hal/components/cros/src/xmlrpc_process.c delete mode 100644 src/hal/components/cros/src/xmlrpc_protocol.c diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..dc8dcce9 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "src/hal/components/cros"] + path = src/hal/components/cros + url = git@github.com:jjrbfi/cros.git diff --git a/src/hal/components/cros b/src/hal/components/cros new file mode 160000 index 00000000..d180cfff --- /dev/null +++ b/src/hal/components/cros @@ -0,0 +1 @@ +Subproject commit d180cfff709ae56cb2d8befa486991a11a0e9321 diff --git a/src/hal/components/cros/.gitignore b/src/hal/components/cros/.gitignore deleted file mode 100644 index b83fb152..00000000 --- a/src/hal/components/cros/.gitignore +++ /dev/null @@ -1,27 +0,0 @@ -CMakeCache.txt -CMakeFiles/ -Makefile -bin/ -lib/ -cmake_install.cmake -.cproject -.project -*.kdev4 -.kdev4/ -build/ -debug/ -release/ -/docs/html -/docs/latex -gmon.out -*~ -.idea/ - -# Python byte-compiled code -__pycache__/ -*.py[cod] - -# Microsoft Visual Studio -.vs/ -x64/ -Win32/ diff --git a/src/hal/components/cros/CHANGELOG b/src/hal/components/cros/CHANGELOG deleted file mode 100644 index dcb93f61..00000000 --- a/src/hal/components/cros/CHANGELOG +++ /dev/null @@ -1,24 +0,0 @@ -cROS 1.0-rc1 (21/12/2017) -------------------------- -Most significant changes of this second release: - * Now cROS compiles and works on OS X - * Fixed asynchronously connections management with select() function - * Added polling support for receiving topic messages, blocking message sending and blocking service call - * Fixed diverse bugs - * Improvements in message serialization/deserialization - * Added error checking and return error code to some principal functions - * Added some checks for the syntax the specified message and service definition files - * Fixed many memory leaks (and some double memory frees). So far no leaks are detecting when running the sample programs - * A signal handler has been implemented in several test program to allow the user to end the program safely by pressing Ctr-C - * Several segmentation faults have been fixed - * The return value of API functions has been changed from int to cRosErrCodePack (encoding pre-defined global error codes. See cros_err_codes.h) - * Some helper functions have been implemented to print the content of a cRosMessage, error code, copy messages, etc. - * A subscriber now supports receiving messages on the same topic from several publishers - * cRosNodeDestroy() automatically unregisters all topic publishers, topic subscribers, service providers and parameter subscribers in the ROS master - * The ROS node now supports the operation as a service caller (client) - * Added compatibility with MATLAB ROS toolbox (tested with MATLAB R2017a) - * Added cROS library manual in PDF - -cROS 0.9 --------- - * First public release diff --git a/src/hal/components/cros/CMakeLists.txt b/src/hal/components/cros/CMakeLists.txt deleted file mode 100644 index aaaec434..00000000 --- a/src/hal/components/cros/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -project(cros) - -cmake_minimum_required (VERSION 2.8) - -cmake_policy(SET CMP0015 NEW) - -execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion - OUTPUT_VARIABLE GCC_VERSION) - -set(CMAKE_C_FLAGS "-Wall -Wno-unused") -if (GCC_VERSION VERSION_GREATER 4.8 OR GCC_VERSION VERSION_EQUAL 4.8) - set(CMAKE_C_FLAGS_DEBUG "-g -pg -DDEBUG -fsanitize=address -static-libasan") -else() - set(CMAKE_C_FLAGS_DEBUG "-g -pg -DDEBUG") -endif() - -set(LINK_FLAGS_DEBUG "-pg") -set(CMAKE_C_FLAGS_RELEASE "-DNDEBUG -O1") -set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin) - -include_directories (include) -aux_source_directory(${PROJECT_SOURCE_DIR}/src CROSLIB_SRCS) - -add_library(cros SHARED ${CROSLIB_SRCS} ) - -add_subdirectory(samples) - -set_target_properties(cros PROPERTIES ARCHIVE_OUTPUT_DIRECTORY lib) - -find_package(Doxygen) -if (NOT DOXYGEN_FOUND) - message(ERROR "Doxygen is needed to build the documentation. Please install it correctly") -else() - - configure_file(Doxyfile ${PROJECT_BINARY_DIR}/Doxyfile @ONLY IMMEDIATE) - #-- Add a custom target to run Doxygen when ever the project is built - add_custom_target (docs COMMAND ${DOXYGEN_EXECUTABLE} ${PROJECT_BINARY_DIR}/Doxyfile - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} - COMMENT "Generating API documentation with Doxygen" VERBATIM) -endif() diff --git a/src/hal/components/cros/Doxyfile b/src/hal/components/cros/Doxyfile deleted file mode 100644 index 769c0db0..00000000 --- a/src/hal/components/cros/Doxyfile +++ /dev/null @@ -1,1869 +0,0 @@ -# Doxyfile 1.8.3.1 - -# This file describes the settings to be used by the documentation system -# doxygen (www.doxygen.org) for a project. -# -# All text after a hash (#) is considered a comment and will be ignored. -# The format is: -# TAG = value [value, ...] -# For lists items can also be appended using: -# TAG += value [value, ...] -# Values that contain spaces should be placed between quotes (" "). - -#--------------------------------------------------------------------------- -# Project related configuration options -#--------------------------------------------------------------------------- - -# This tag specifies the encoding used for all characters in the config file -# that follow. The default is UTF-8 which is also the encoding used for all -# text before the first occurrence of this tag. Doxygen uses libiconv (or the -# iconv built into libc) for the transcoding. See -# http://www.gnu.org/software/libiconv for the list of possible encodings. - -DOXYFILE_ENCODING = UTF-8 - -# The PROJECT_NAME tag is a single word (or sequence of words) that should -# identify the project. Note that if you do not use Doxywizard you need -# to put quotes around the project name if it contains spaces. - -PROJECT_NAME = "cROS" - -# The PROJECT_NUMBER tag can be used to enter a project or revision number. -# This could be handy for archiving the generated documentation or -# if some version control system is used. - -PROJECT_NUMBER = "0.1" - -# Using the PROJECT_BRIEF tag one can provide an optional one line description -# for a project that appears at the top of each page and should give viewer -# a quick idea about the purpose of the project. Keep the description short. - -PROJECT_BRIEF = - -# With the PROJECT_LOGO tag one can specify an logo or icon that is -# included in the documentation. The maximum height of the logo should not -# exceed 55 pixels and the maximum width should not exceed 200 pixels. -# Doxygen will copy the logo to the output directory. - -PROJECT_LOGO = - -# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) -# base path where the generated documentation will be put. -# If a relative path is entered, it will be relative to the location -# where doxygen was started. If left blank the current directory will be used. - -OUTPUT_DIRECTORY = ./docs - -# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create -# 4096 sub-directories (in 2 levels) under the output directory of each output -# format and will distribute the generated files over these directories. -# Enabling this option can be useful when feeding doxygen a huge amount of -# source files, where putting all generated files in the same directory would -# otherwise cause performance problems for the file system. - -CREATE_SUBDIRS = NO - -# The OUTPUT_LANGUAGE tag is used to specify the language in which all -# documentation generated by doxygen is written. Doxygen will use this -# information to generate all constant output in the proper language. -# The default language is English, other supported languages are: -# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, -# Croatian, Czech, Danish, Dutch, Esperanto, Farsi, Finnish, French, German, -# Greek, Hungarian, Italian, Japanese, Japanese-en (Japanese with English -# messages), Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, -# Polish, Portuguese, Romanian, Russian, Serbian, Serbian-Cyrillic, Slovak, -# Slovene, Spanish, Swedish, Ukrainian, and Vietnamese. - -OUTPUT_LANGUAGE = English - -# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will -# include brief member descriptions after the members that are listed in -# the file and class documentation (similar to JavaDoc). -# Set to NO to disable this. - -BRIEF_MEMBER_DESC = YES - -# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend -# the brief description of a member or function before the detailed description. -# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the -# brief descriptions will be completely suppressed. - -REPEAT_BRIEF = YES - -# This tag implements a quasi-intelligent brief description abbreviator -# that is used to form the text in various listings. Each string -# in this list, if found as the leading text of the brief description, will be -# stripped from the text and the result after processing the whole list, is -# used as the annotated text. Otherwise, the brief description is used as-is. -# If left blank, the following values are used ("$name" is automatically -# replaced with the name of the entity): "The $name class" "The $name widget" -# "The $name file" "is" "provides" "specifies" "contains" -# "represents" "a" "an" "the" - -ABBREVIATE_BRIEF = - -# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then -# Doxygen will generate a detailed section even if there is only a brief -# description. - -ALWAYS_DETAILED_SEC = NO - -# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all -# inherited members of a class in the documentation of that class as if those -# members were ordinary class members. Constructors, destructors and assignment -# operators of the base classes will not be shown. - -INLINE_INHERITED_MEMB = NO - -# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full -# path before files name in the file list and in the header files. If set -# to NO the shortest path that makes the file name unique will be used. - -FULL_PATH_NAMES = YES - -# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag -# can be used to strip a user-defined part of the path. Stripping is -# only done if one of the specified strings matches the left-hand part of -# the path. The tag can be used to show relative paths in the file list. -# If left blank the directory from which doxygen is run is used as the -# path to strip. Note that you specify absolute paths here, but also -# relative paths, which will be relative from the directory where doxygen is -# started. - -STRIP_FROM_PATH = - -# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of -# the path mentioned in the documentation of a class, which tells -# the reader which header file to include in order to use a class. -# If left blank only the name of the header file containing the class -# definition is used. Otherwise one should specify the include paths that -# are normally passed to the compiler using the -I flag. - -STRIP_FROM_INC_PATH = - -# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter -# (but less readable) file names. This can be useful if your file system -# doesn't support long names like on DOS, Mac, or CD-ROM. - -SHORT_NAMES = NO - -# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen -# will interpret the first line (until the first dot) of a JavaDoc-style -# comment as the brief description. If set to NO, the JavaDoc -# comments will behave just like regular Qt-style comments -# (thus requiring an explicit @brief command for a brief description.) - -JAVADOC_AUTOBRIEF = NO - -# If the QT_AUTOBRIEF tag is set to YES then Doxygen will -# interpret the first line (until the first dot) of a Qt-style -# comment as the brief description. If set to NO, the comments -# will behave just like regular Qt-style comments (thus requiring -# an explicit \brief command for a brief description.) - -QT_AUTOBRIEF = NO - -# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen -# treat a multi-line C++ special comment block (i.e. a block of //! or /// -# comments) as a brief description. This used to be the default behaviour. -# The new default is to treat a multi-line C++ comment block as a detailed -# description. Set this tag to YES if you prefer the old behaviour instead. - -MULTILINE_CPP_IS_BRIEF = NO - -# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented -# member inherits the documentation from any documented member that it -# re-implements. - -INHERIT_DOCS = YES - -# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce -# a new page for each member. If set to NO, the documentation of a member will -# be part of the file/class/namespace that contains it. - -SEPARATE_MEMBER_PAGES = NO - -# The TAB_SIZE tag can be used to set the number of spaces in a tab. -# Doxygen uses this value to replace tabs by spaces in code fragments. - -TAB_SIZE = 2 - -# This tag can be used to specify a number of aliases that acts -# as commands in the documentation. An alias has the form "name=value". -# For example adding "sideeffect=\par Side Effects:\n" will allow you to -# put the command \sideeffect (or @sideeffect) in the documentation, which -# will result in a user-defined paragraph with heading "Side Effects:". -# You can put \n's in the value part of an alias to insert newlines. - -ALIASES = - -# This tag can be used to specify a number of word-keyword mappings (TCL only). -# A mapping has the form "name=value". For example adding -# "class=itcl::class" will allow you to use the command class in the -# itcl::class meaning. - -TCL_SUBST = - -# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C -# sources only. Doxygen will then generate output that is more tailored for C. -# For instance, some of the names that are used will be different. The list -# of all members will be omitted, etc. - -OPTIMIZE_OUTPUT_FOR_C = YES - -# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java -# sources only. Doxygen will then generate output that is more tailored for -# Java. For instance, namespaces will be presented as packages, qualified -# scopes will look different, etc. - -OPTIMIZE_OUTPUT_JAVA = NO - -# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran -# sources only. Doxygen will then generate output that is more tailored for -# Fortran. - -OPTIMIZE_FOR_FORTRAN = NO - -# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL -# sources. Doxygen will then generate output that is tailored for -# VHDL. - -OPTIMIZE_OUTPUT_VHDL = NO - -# Doxygen selects the parser to use depending on the extension of the files it -# parses. With this tag you can assign which parser to use for a given -# extension. Doxygen has a built-in mapping, but you can override or extend it -# using this tag. The format is ext=language, where ext is a file extension, -# and language is one of the parsers supported by doxygen: IDL, Java, -# Javascript, CSharp, C, C++, D, PHP, Objective-C, Python, Fortran, VHDL, C, -# C++. For instance to make doxygen treat .inc files as Fortran files (default -# is PHP), and .f files as C (default is Fortran), use: inc=Fortran f=C. Note -# that for custom extensions you also need to set FILE_PATTERNS otherwise the -# files are not read by doxygen. - -EXTENSION_MAPPING = - -# If MARKDOWN_SUPPORT is enabled (the default) then doxygen pre-processes all -# comments according to the Markdown format, which allows for more readable -# documentation. See http://daringfireball.net/projects/markdown/ for details. -# The output of markdown processing is further processed by doxygen, so you -# can mix doxygen, HTML, and XML commands with Markdown formatting. -# Disable only in case of backward compatibilities issues. - -MARKDOWN_SUPPORT = YES - -# When enabled doxygen tries to link words that correspond to documented classes, -# or namespaces to their corresponding documentation. Such a link can be -# prevented in individual cases by by putting a % sign in front of the word or -# globally by setting AUTOLINK_SUPPORT to NO. - -AUTOLINK_SUPPORT = YES - -# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want -# to include (a tag file for) the STL sources as input, then you should -# set this tag to YES in order to let doxygen match functions declarations and -# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. -# func(std::string) {}). This also makes the inheritance and collaboration -# diagrams that involve STL classes more complete and accurate. - -BUILTIN_STL_SUPPORT = NO - -# If you use Microsoft's C++/CLI language, you should set this option to YES to -# enable parsing support. - -CPP_CLI_SUPPORT = NO - -# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. -# Doxygen will parse them like normal C++ but will assume all classes use public -# instead of private inheritance when no explicit protection keyword is present. - -SIP_SUPPORT = NO - -# For Microsoft's IDL there are propget and propput attributes to indicate -# getter and setter methods for a property. Setting this option to YES (the -# default) will make doxygen replace the get and set methods by a property in -# the documentation. This will only work if the methods are indeed getting or -# setting a simple type. If this is not the case, or you want to show the -# methods anyway, you should set this option to NO. - -IDL_PROPERTY_SUPPORT = YES - -# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC -# tag is set to YES, then doxygen will reuse the documentation of the first -# member in the group (if any) for the other members of the group. By default -# all members of a group must be documented explicitly. - -DISTRIBUTE_GROUP_DOC = NO - -# Set the SUBGROUPING tag to YES (the default) to allow class member groups of -# the same type (for instance a group of public functions) to be put as a -# subgroup of that type (e.g. under the Public Functions section). Set it to -# NO to prevent subgrouping. Alternatively, this can be done per class using -# the \nosubgrouping command. - -SUBGROUPING = YES - -# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and -# unions are shown inside the group in which they are included (e.g. using -# @ingroup) instead of on a separate page (for HTML and Man pages) or -# section (for LaTeX and RTF). - -INLINE_GROUPED_CLASSES = NO - -# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and -# unions with only public data fields will be shown inline in the documentation -# of the scope in which they are defined (i.e. file, namespace, or group -# documentation), provided this scope is documented. If set to NO (the default), -# structs, classes, and unions are shown on a separate page (for HTML and Man -# pages) or section (for LaTeX and RTF). - -INLINE_SIMPLE_STRUCTS = NO - -# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum -# is documented as struct, union, or enum with the name of the typedef. So -# typedef struct TypeS {} TypeT, will appear in the documentation as a struct -# with name TypeT. When disabled the typedef will appear as a member of a file, -# namespace, or class. And the struct will be named TypeS. This can typically -# be useful for C code in case the coding convention dictates that all compound -# types are typedef'ed and only the typedef is referenced, never the tag name. - -TYPEDEF_HIDES_STRUCT = NO - -# The SYMBOL_CACHE_SIZE determines the size of the internal cache use to -# determine which symbols to keep in memory and which to flush to disk. -# When the cache is full, less often used symbols will be written to disk. -# For small to medium size projects (<1000 input files) the default value is -# probably good enough. For larger projects a too small cache size can cause -# doxygen to be busy swapping symbols to and from disk most of the time -# causing a significant performance penalty. -# If the system has enough physical memory increasing the cache will improve the -# performance by keeping more symbols in memory. Note that the value works on -# a logarithmic scale so increasing the size by one will roughly double the -# memory usage. The cache size is given by this formula: -# 2^(16+SYMBOL_CACHE_SIZE). The valid range is 0..9, the default is 0, -# corresponding to a cache size of 2^16 = 65536 symbols. - -SYMBOL_CACHE_SIZE = 0 - -# Similar to the SYMBOL_CACHE_SIZE the size of the symbol lookup cache can be -# set using LOOKUP_CACHE_SIZE. This cache is used to resolve symbols given -# their name and scope. Since this can be an expensive process and often the -# same symbol appear multiple times in the code, doxygen keeps a cache of -# pre-resolved symbols. If the cache is too small doxygen will become slower. -# If the cache is too large, memory is wasted. The cache size is given by this -# formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range is 0..9, the default is 0, -# corresponding to a cache size of 2^16 = 65536 symbols. - -LOOKUP_CACHE_SIZE = 0 - -#--------------------------------------------------------------------------- -# Build related configuration options -#--------------------------------------------------------------------------- - -# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in -# documentation are documented, even if no documentation was available. -# Private class members and static file members will be hidden unless -# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES - -EXTRACT_ALL = YES - -# If the EXTRACT_PRIVATE tag is set to YES all private members of a class -# will be included in the documentation. - -EXTRACT_PRIVATE = NO - -# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal -# scope will be included in the documentation. - -EXTRACT_PACKAGE = NO - -# If the EXTRACT_STATIC tag is set to YES all static members of a file -# will be included in the documentation. - -EXTRACT_STATIC = NO - -# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) -# defined locally in source files will be included in the documentation. -# If set to NO only classes defined in header files are included. - -EXTRACT_LOCAL_CLASSES = YES - -# This flag is only useful for Objective-C code. When set to YES local -# methods, which are defined in the implementation section but not in -# the interface are included in the documentation. -# If set to NO (the default) only methods in the interface are included. - -EXTRACT_LOCAL_METHODS = NO - -# If this flag is set to YES, the members of anonymous namespaces will be -# extracted and appear in the documentation as a namespace called -# 'anonymous_namespace{file}', where file will be replaced with the base -# name of the file that contains the anonymous namespace. By default -# anonymous namespaces are hidden. - -EXTRACT_ANON_NSPACES = NO - -# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all -# undocumented members of documented classes, files or namespaces. -# If set to NO (the default) these members will be included in the -# various overviews, but no documentation section is generated. -# This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_MEMBERS = NO - -# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all -# undocumented classes that are normally visible in the class hierarchy. -# If set to NO (the default) these classes will be included in the various -# overviews. This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_CLASSES = NO - -# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all -# friend (class|struct|union) declarations. -# If set to NO (the default) these declarations will be included in the -# documentation. - -HIDE_FRIEND_COMPOUNDS = NO - -# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any -# documentation blocks found inside the body of a function. -# If set to NO (the default) these blocks will be appended to the -# function's detailed documentation block. - -HIDE_IN_BODY_DOCS = NO - -# The INTERNAL_DOCS tag determines if documentation -# that is typed after a \internal command is included. If the tag is set -# to NO (the default) then the documentation will be excluded. -# Set it to YES to include the internal documentation. - -INTERNAL_DOCS = NO - -# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate -# file names in lower-case letters. If set to YES upper-case letters are also -# allowed. This is useful if you have classes or files whose names only differ -# in case and if your file system supports case sensitive file names. Windows -# and Mac users are advised to set this option to NO. - -CASE_SENSE_NAMES = YES - -# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen -# will show members with their full class and namespace scopes in the -# documentation. If set to YES the scope will be hidden. - -HIDE_SCOPE_NAMES = NO - -# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen -# will put a list of the files that are included by a file in the documentation -# of that file. - -SHOW_INCLUDE_FILES = YES - -# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen -# will list include files with double quotes in the documentation -# rather than with sharp brackets. - -FORCE_LOCAL_INCLUDES = NO - -# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] -# is inserted in the documentation for inline members. - -INLINE_INFO = YES - -# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen -# will sort the (detailed) documentation of file and class members -# alphabetically by member name. If set to NO the members will appear in -# declaration order. - -SORT_MEMBER_DOCS = YES - -# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the -# brief documentation of file, namespace and class members alphabetically -# by member name. If set to NO (the default) the members will appear in -# declaration order. - -SORT_BRIEF_DOCS = NO - -# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen -# will sort the (brief and detailed) documentation of class members so that -# constructors and destructors are listed first. If set to NO (the default) -# the constructors will appear in the respective orders defined by -# SORT_MEMBER_DOCS and SORT_BRIEF_DOCS. -# This tag will be ignored for brief docs if SORT_BRIEF_DOCS is set to NO -# and ignored for detailed docs if SORT_MEMBER_DOCS is set to NO. - -SORT_MEMBERS_CTORS_1ST = NO - -# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the -# hierarchy of group names into alphabetical order. If set to NO (the default) -# the group names will appear in their defined order. - -SORT_GROUP_NAMES = NO - -# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be -# sorted by fully-qualified names, including namespaces. If set to -# NO (the default), the class list will be sorted only by class name, -# not including the namespace part. -# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. -# Note: This option applies only to the class list, not to the -# alphabetical list. - -SORT_BY_SCOPE_NAME = NO - -# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to -# do proper type resolution of all parameters of a function it will reject a -# match between the prototype and the implementation of a member function even -# if there is only one candidate or it is obvious which candidate to choose -# by doing a simple string match. By disabling STRICT_PROTO_MATCHING doxygen -# will still accept a match between prototype and implementation in such cases. - -STRICT_PROTO_MATCHING = NO - -# The GENERATE_TODOLIST tag can be used to enable (YES) or -# disable (NO) the todo list. This list is created by putting \todo -# commands in the documentation. - -GENERATE_TODOLIST = YES - -# The GENERATE_TESTLIST tag can be used to enable (YES) or -# disable (NO) the test list. This list is created by putting \test -# commands in the documentation. - -GENERATE_TESTLIST = YES - -# The GENERATE_BUGLIST tag can be used to enable (YES) or -# disable (NO) the bug list. This list is created by putting \bug -# commands in the documentation. - -GENERATE_BUGLIST = YES - -# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or -# disable (NO) the deprecated list. This list is created by putting -# \deprecated commands in the documentation. - -GENERATE_DEPRECATEDLIST= YES - -# The ENABLED_SECTIONS tag can be used to enable conditional -# documentation sections, marked by \if section-label ... \endif -# and \cond section-label ... \endcond blocks. - -ENABLED_SECTIONS = - -# The MAX_INITIALIZER_LINES tag determines the maximum number of lines -# the initial value of a variable or macro consists of for it to appear in -# the documentation. If the initializer consists of more lines than specified -# here it will be hidden. Use a value of 0 to hide initializers completely. -# The appearance of the initializer of individual variables and macros in the -# documentation can be controlled using \showinitializer or \hideinitializer -# command in the documentation regardless of this setting. - -MAX_INITIALIZER_LINES = 30 - -# Set the SHOW_USED_FILES tag to NO to disable the list of files generated -# at the bottom of the documentation of classes and structs. If set to YES the -# list will mention the files that were used to generate the documentation. - -SHOW_USED_FILES = YES - -# Set the SHOW_FILES tag to NO to disable the generation of the Files page. -# This will remove the Files entry from the Quick Index and from the -# Folder Tree View (if specified). The default is YES. - -SHOW_FILES = YES - -# Set the SHOW_NAMESPACES tag to NO to disable the generation of the -# Namespaces page. -# This will remove the Namespaces entry from the Quick Index -# and from the Folder Tree View (if specified). The default is YES. - -SHOW_NAMESPACES = YES - -# The FILE_VERSION_FILTER tag can be used to specify a program or script that -# doxygen should invoke to get the current version for each file (typically from -# the version control system). Doxygen will invoke the program by executing (via -# popen()) the command , where is the value of -# the FILE_VERSION_FILTER tag, and is the name of an input file -# provided by doxygen. Whatever the program writes to standard output -# is used as the file version. See the manual for examples. - -FILE_VERSION_FILTER = - -# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed -# by doxygen. The layout file controls the global structure of the generated -# output files in an output format independent way. To create the layout file -# that represents doxygen's defaults, run doxygen with the -l option. -# You can optionally specify a file name after the option, if omitted -# DoxygenLayout.xml will be used as the name of the layout file. - -LAYOUT_FILE = - -# The CITE_BIB_FILES tag can be used to specify one or more bib files -# containing the references data. This must be a list of .bib files. The -# .bib extension is automatically appended if omitted. Using this command -# requires the bibtex tool to be installed. See also -# http://en.wikipedia.org/wiki/BibTeX for more info. For LaTeX the style -# of the bibliography can be controlled using LATEX_BIB_STYLE. To use this -# feature you need bibtex and perl available in the search path. Do not use -# file names with spaces, bibtex cannot handle them. - -CITE_BIB_FILES = - -#--------------------------------------------------------------------------- -# configuration options related to warning and progress messages -#--------------------------------------------------------------------------- - -# The QUIET tag can be used to turn on/off the messages that are generated -# by doxygen. Possible values are YES and NO. If left blank NO is used. - -QUIET = NO - -# The WARNINGS tag can be used to turn on/off the warning messages that are -# generated by doxygen. Possible values are YES and NO. If left blank -# NO is used. - -WARNINGS = YES - -# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings -# for undocumented members. If EXTRACT_ALL is set to YES then this flag will -# automatically be disabled. - -WARN_IF_UNDOCUMENTED = YES - -# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for -# potential errors in the documentation, such as not documenting some -# parameters in a documented function, or documenting parameters that -# don't exist or using markup commands wrongly. - -WARN_IF_DOC_ERROR = YES - -# The WARN_NO_PARAMDOC option can be enabled to get warnings for -# functions that are documented, but have no documentation for their parameters -# or return value. If set to NO (the default) doxygen will only warn about -# wrong or incomplete parameter documentation, but not about the absence of -# documentation. - -WARN_NO_PARAMDOC = NO - -# The WARN_FORMAT tag determines the format of the warning messages that -# doxygen can produce. The string should contain the $file, $line, and $text -# tags, which will be replaced by the file and line number from which the -# warning originated and the warning text. Optionally the format may contain -# $version, which will be replaced by the version of the file (if it could -# be obtained via FILE_VERSION_FILTER) - -WARN_FORMAT = "$file:$line: $text" - -# The WARN_LOGFILE tag can be used to specify a file to which warning -# and error messages should be written. If left blank the output is written -# to stderr. - -WARN_LOGFILE = - -#--------------------------------------------------------------------------- -# configuration options related to the input files -#--------------------------------------------------------------------------- - -# The INPUT tag can be used to specify the files and/or directories that contain -# documented source files. You may enter file names like "myfile.cpp" or -# directories like "/usr/src/myproject". Separate the files or directories -# with spaces. - -INPUT = ./include ./src - -# This tag can be used to specify the character encoding of the source files -# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is -# also the default input encoding. Doxygen uses libiconv (or the iconv built -# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for -# the list of possible encodings. - -INPUT_ENCODING = UTF-8 - -# If the value of the INPUT tag contains directories, you can use the -# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp -# and *.h) to filter out the source-files in the directories. If left -# blank the following patterns are tested: -# *.c *.cc *.cxx *.cpp *.c++ *.d *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh -# *.hxx *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.dox *.py -# *.f90 *.f *.for *.vhd *.vhdl - -FILE_PATTERNS = - -# The RECURSIVE tag can be used to turn specify whether or not subdirectories -# should be searched for input files as well. Possible values are YES and NO. -# If left blank NO is used. - -RECURSIVE = NO - -# The EXCLUDE tag can be used to specify files and/or directories that should be -# excluded from the INPUT source files. This way you can easily exclude a -# subdirectory from a directory tree whose root is specified with the INPUT tag. -# Note that relative paths are relative to the directory from which doxygen is -# run. - -EXCLUDE = - -# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or -# directories that are symbolic links (a Unix file system feature) are excluded -# from the input. - -EXCLUDE_SYMLINKS = NO - -# If the value of the INPUT tag contains directories, you can use the -# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude -# certain files from those directories. Note that the wildcards are matched -# against the file with absolute path, so to exclude all test directories -# for example use the pattern */test/* - -EXCLUDE_PATTERNS = - -# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names -# (namespaces, classes, functions, etc.) that should be excluded from the -# output. The symbol name can be a fully qualified name, a word, or if the -# wildcard * is used, a substring. Examples: ANamespace, AClass, -# AClass::ANamespace, ANamespace::*Test - -EXCLUDE_SYMBOLS = - -# The EXAMPLE_PATH tag can be used to specify one or more files or -# directories that contain example code fragments that are included (see -# the \include command). - -EXAMPLE_PATH = - -# If the value of the EXAMPLE_PATH tag contains directories, you can use the -# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp -# and *.h) to filter out the source-files in the directories. If left -# blank all files are included. - -EXAMPLE_PATTERNS = - -# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be -# searched for input files to be used with the \include or \dontinclude -# commands irrespective of the value of the RECURSIVE tag. -# Possible values are YES and NO. If left blank NO is used. - -EXAMPLE_RECURSIVE = NO - -# The IMAGE_PATH tag can be used to specify one or more files or -# directories that contain image that are included in the documentation (see -# the \image command). - -IMAGE_PATH = - -# The INPUT_FILTER tag can be used to specify a program that doxygen should -# invoke to filter for each input file. Doxygen will invoke the filter program -# by executing (via popen()) the command , where -# is the value of the INPUT_FILTER tag, and is the name of an -# input file. Doxygen will then use the output that the filter program writes -# to standard output. -# If FILTER_PATTERNS is specified, this tag will be -# ignored. - -INPUT_FILTER = - -# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern -# basis. -# Doxygen will compare the file name with each pattern and apply the -# filter if there is a match. -# The filters are a list of the form: -# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further -# info on how filters are used. If FILTER_PATTERNS is empty or if -# non of the patterns match the file name, INPUT_FILTER is applied. - -FILTER_PATTERNS = - -# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using -# INPUT_FILTER) will be used to filter the input files when producing source -# files to browse (i.e. when SOURCE_BROWSER is set to YES). - -FILTER_SOURCE_FILES = NO - -# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file -# pattern. A pattern will override the setting for FILTER_PATTERN (if any) -# and it is also possible to disable source filtering for a specific pattern -# using *.ext= (so without naming a filter). This option only has effect when -# FILTER_SOURCE_FILES is enabled. - -FILTER_SOURCE_PATTERNS = - -# If the USE_MD_FILE_AS_MAINPAGE tag refers to the name of a markdown file that -# is part of the input, its contents will be placed on the main page (index.html). -# This can be useful if you have a project on for instance GitHub and want reuse -# the introduction page also for the doxygen output. - -USE_MDFILE_AS_MAINPAGE = - -#--------------------------------------------------------------------------- -# configuration options related to source browsing -#--------------------------------------------------------------------------- - -# If the SOURCE_BROWSER tag is set to YES then a list of source files will -# be generated. Documented entities will be cross-referenced with these sources. -# Note: To get rid of all source code in the generated output, make sure also -# VERBATIM_HEADERS is set to NO. - -SOURCE_BROWSER = YES - -# Setting the INLINE_SOURCES tag to YES will include the body -# of functions and classes directly in the documentation. - -INLINE_SOURCES = NO - -# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct -# doxygen to hide any special comment blocks from generated source code -# fragments. Normal C, C++ and Fortran comments will always remain visible. - -STRIP_CODE_COMMENTS = YES - -# If the REFERENCED_BY_RELATION tag is set to YES -# then for each documented function all documented -# functions referencing it will be listed. - -REFERENCED_BY_RELATION = NO - -# If the REFERENCES_RELATION tag is set to YES -# then for each documented function all documented entities -# called/used by that function will be listed. - -REFERENCES_RELATION = NO - -# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) -# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from -# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will -# link to the source code. -# Otherwise they will link to the documentation. - -REFERENCES_LINK_SOURCE = YES - -# If the USE_HTAGS tag is set to YES then the references to source code -# will point to the HTML generated by the htags(1) tool instead of doxygen -# built-in source browser. The htags tool is part of GNU's global source -# tagging system (see http://www.gnu.org/software/global/global.html). You -# will need version 4.8.6 or higher. - -USE_HTAGS = NO - -# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen -# will generate a verbatim copy of the header file for each class for -# which an include is specified. Set to NO to disable this. - -VERBATIM_HEADERS = NO - -#--------------------------------------------------------------------------- -# configuration options related to the alphabetical class index -#--------------------------------------------------------------------------- - -# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index -# of all compounds will be generated. Enable this if the project -# contains a lot of classes, structs, unions or interfaces. - -ALPHABETICAL_INDEX = YES - -# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then -# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns -# in which this list will be split (can be a number in the range [1..20]) - -COLS_IN_ALPHA_INDEX = 5 - -# In case all classes in a project start with a common prefix, all -# classes will be put under the same header in the alphabetical index. -# The IGNORE_PREFIX tag can be used to specify one or more prefixes that -# should be ignored while generating the index headers. - -IGNORE_PREFIX = - -#--------------------------------------------------------------------------- -# configuration options related to the HTML output -#--------------------------------------------------------------------------- - -# If the GENERATE_HTML tag is set to YES (the default) Doxygen will -# generate HTML output. - -GENERATE_HTML = YES - -# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `html' will be used as the default path. - -HTML_OUTPUT = html - -# The HTML_FILE_EXTENSION tag can be used to specify the file extension for -# each generated HTML page (for example: .htm,.php,.asp). If it is left blank -# doxygen will generate files with .html extension. - -HTML_FILE_EXTENSION = .html - -# The HTML_HEADER tag can be used to specify a personal HTML header for -# each generated HTML page. If it is left blank doxygen will generate a -# standard header. Note that when using a custom header you are responsible -# for the proper inclusion of any scripts and style sheets that doxygen -# needs, which is dependent on the configuration options used. -# It is advised to generate a default header using "doxygen -w html -# header.html footer.html stylesheet.css YourConfigFile" and then modify -# that header. Note that the header is subject to change so you typically -# have to redo this when upgrading to a newer version of doxygen or when -# changing the value of configuration settings such as GENERATE_TREEVIEW! - -HTML_HEADER = - -# The HTML_FOOTER tag can be used to specify a personal HTML footer for -# each generated HTML page. If it is left blank doxygen will generate a -# standard footer. - -HTML_FOOTER = - -# The HTML_STYLESHEET tag can be used to specify a user-defined cascading -# style sheet that is used by each HTML page. It can be used to -# fine-tune the look of the HTML output. If left blank doxygen will -# generate a default style sheet. Note that it is recommended to use -# HTML_EXTRA_STYLESHEET instead of this one, as it is more robust and this -# tag will in the future become obsolete. - -HTML_STYLESHEET = - -# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional -# user-defined cascading style sheet that is included after the standard -# style sheets created by doxygen. Using this option one can overrule -# certain style aspects. This is preferred over using HTML_STYLESHEET -# since it does not replace the standard style sheet and is therefor more -# robust against future updates. Doxygen will copy the style sheet file to -# the output directory. - -HTML_EXTRA_STYLESHEET = - -# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or -# other source files which should be copied to the HTML output directory. Note -# that these files will be copied to the base HTML output directory. Use the -# $relpath$ marker in the HTML_HEADER and/or HTML_FOOTER files to load these -# files. In the HTML_STYLESHEET file, use the file name only. Also note that -# the files will be copied as-is; there are no commands or markers available. - -HTML_EXTRA_FILES = - -# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. -# Doxygen will adjust the colors in the style sheet and background images -# according to this color. Hue is specified as an angle on a colorwheel, -# see http://en.wikipedia.org/wiki/Hue for more information. -# For instance the value 0 represents red, 60 is yellow, 120 is green, -# 180 is cyan, 240 is blue, 300 purple, and 360 is red again. -# The allowed range is 0 to 359. - -HTML_COLORSTYLE_HUE = 220 - -# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of -# the colors in the HTML output. For a value of 0 the output will use -# grayscales only. A value of 255 will produce the most vivid colors. - -HTML_COLORSTYLE_SAT = 100 - -# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to -# the luminance component of the colors in the HTML output. Values below -# 100 gradually make the output lighter, whereas values above 100 make -# the output darker. The value divided by 100 is the actual gamma applied, -# so 80 represents a gamma of 0.8, The value 220 represents a gamma of 2.2, -# and 100 does not change the gamma. - -HTML_COLORSTYLE_GAMMA = 80 - -# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML -# page will contain the date and time when the page was generated. Setting -# this to NO can help when comparing the output of multiple runs. - -HTML_TIMESTAMP = YES - -# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML -# documentation will contain sections that can be hidden and shown after the -# page has loaded. - -HTML_DYNAMIC_SECTIONS = NO - -# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of -# entries shown in the various tree structured indices initially; the user -# can expand and collapse entries dynamically later on. Doxygen will expand -# the tree to such a level that at most the specified number of entries are -# visible (unless a fully collapsed tree already exceeds this amount). -# So setting the number of entries 1 will produce a full collapsed tree by -# default. 0 is a special value representing an infinite number of entries -# and will result in a full expanded tree by default. - -HTML_INDEX_NUM_ENTRIES = 100 - -# If the GENERATE_DOCSET tag is set to YES, additional index files -# will be generated that can be used as input for Apple's Xcode 3 -# integrated development environment, introduced with OSX 10.5 (Leopard). -# To create a documentation set, doxygen will generate a Makefile in the -# HTML output directory. Running make will produce the docset in that -# directory and running "make install" will install the docset in -# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find -# it at startup. -# See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html -# for more information. - -GENERATE_DOCSET = NO - -# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the -# feed. A documentation feed provides an umbrella under which multiple -# documentation sets from a single provider (such as a company or product suite) -# can be grouped. - -DOCSET_FEEDNAME = "Doxygen generated docs" - -# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that -# should uniquely identify the documentation set bundle. This should be a -# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen -# will append .docset to the name. - -DOCSET_BUNDLE_ID = org.doxygen.Project - -# When GENERATE_PUBLISHER_ID tag specifies a string that should uniquely -# identify the documentation publisher. This should be a reverse domain-name -# style string, e.g. com.mycompany.MyDocSet.documentation. - -DOCSET_PUBLISHER_ID = org.doxygen.Publisher - -# The GENERATE_PUBLISHER_NAME tag identifies the documentation publisher. - -DOCSET_PUBLISHER_NAME = Publisher - -# If the GENERATE_HTMLHELP tag is set to YES, additional index files -# will be generated that can be used as input for tools like the -# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) -# of the generated HTML documentation. - -GENERATE_HTMLHELP = NO - -# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can -# be used to specify the file name of the resulting .chm file. You -# can add a path in front of the file if the result should not be -# written to the html output directory. - -CHM_FILE = - -# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can -# be used to specify the location (absolute path including file name) of -# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run -# the HTML help compiler on the generated index.hhp. - -HHC_LOCATION = - -# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag -# controls if a separate .chi index file is generated (YES) or that -# it should be included in the master .chm file (NO). - -GENERATE_CHI = NO - -# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING -# is used to encode HtmlHelp index (hhk), content (hhc) and project file -# content. - -CHM_INDEX_ENCODING = - -# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag -# controls whether a binary table of contents is generated (YES) or a -# normal table of contents (NO) in the .chm file. - -BINARY_TOC = NO - -# The TOC_EXPAND flag can be set to YES to add extra items for group members -# to the contents of the HTML help documentation and to the tree view. - -TOC_EXPAND = NO - -# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and -# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated -# that can be used as input for Qt's qhelpgenerator to generate a -# Qt Compressed Help (.qch) of the generated HTML documentation. - -GENERATE_QHP = NO - -# If the QHG_LOCATION tag is specified, the QCH_FILE tag can -# be used to specify the file name of the resulting .qch file. -# The path specified is relative to the HTML output folder. - -QCH_FILE = - -# The QHP_NAMESPACE tag specifies the namespace to use when generating -# Qt Help Project output. For more information please see -# http://doc.trolltech.com/qthelpproject.html#namespace - -QHP_NAMESPACE = org.doxygen.Project - -# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating -# Qt Help Project output. For more information please see -# http://doc.trolltech.com/qthelpproject.html#virtual-folders - -QHP_VIRTUAL_FOLDER = doc - -# If QHP_CUST_FILTER_NAME is set, it specifies the name of a custom filter to -# add. For more information please see -# http://doc.trolltech.com/qthelpproject.html#custom-filters - -QHP_CUST_FILTER_NAME = - -# The QHP_CUST_FILT_ATTRS tag specifies the list of the attributes of the -# custom filter to add. For more information please see -# -# Qt Help Project / Custom Filters. - -QHP_CUST_FILTER_ATTRS = - -# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this -# project's -# filter section matches. -# -# Qt Help Project / Filter Attributes. - -QHP_SECT_FILTER_ATTRS = - -# If the GENERATE_QHP tag is set to YES, the QHG_LOCATION tag can -# be used to specify the location of Qt's qhelpgenerator. -# If non-empty doxygen will try to run qhelpgenerator on the generated -# .qhp file. - -QHG_LOCATION = - -# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files -# will be generated, which together with the HTML files, form an Eclipse help -# plugin. To install this plugin and make it available under the help contents -# menu in Eclipse, the contents of the directory containing the HTML and XML -# files needs to be copied into the plugins directory of eclipse. The name of -# the directory within the plugins directory should be the same as -# the ECLIPSE_DOC_ID value. After copying Eclipse needs to be restarted before -# the help appears. - -GENERATE_ECLIPSEHELP = NO - -# A unique identifier for the eclipse help plugin. When installing the plugin -# the directory name containing the HTML and XML files should also have -# this name. - -ECLIPSE_DOC_ID = org.doxygen.Project - -# The DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) -# at top of each HTML page. The value NO (the default) enables the index and -# the value YES disables it. Since the tabs have the same information as the -# navigation tree you can set this option to NO if you already set -# GENERATE_TREEVIEW to YES. - -DISABLE_INDEX = NO - -# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index -# structure should be generated to display hierarchical information. -# If the tag value is set to YES, a side panel will be generated -# containing a tree-like index structure (just like the one that -# is generated for HTML Help). For this to work a browser that supports -# JavaScript, DHTML, CSS and frames is required (i.e. any modern browser). -# Windows users are probably better off using the HTML help feature. -# Since the tree basically has the same information as the tab index you -# could consider to set DISABLE_INDEX to NO when enabling this option. - -GENERATE_TREEVIEW = NO - -# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values -# (range [0,1..20]) that doxygen will group on one line in the generated HTML -# documentation. Note that a value of 0 will completely suppress the enum -# values from appearing in the overview section. - -ENUM_VALUES_PER_LINE = 4 - -# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be -# used to set the initial width (in pixels) of the frame in which the tree -# is shown. - -TREEVIEW_WIDTH = 250 - -# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open -# links to external symbols imported via tag files in a separate window. - -EXT_LINKS_IN_WINDOW = NO - -# Use this tag to change the font size of Latex formulas included -# as images in the HTML documentation. The default is 10. Note that -# when you change the font size after a successful doxygen run you need -# to manually remove any form_*.png images from the HTML output directory -# to force them to be regenerated. - -FORMULA_FONTSIZE = 10 - -# Use the FORMULA_TRANPARENT tag to determine whether or not the images -# generated for formulas are transparent PNGs. Transparent PNGs are -# not supported properly for IE 6.0, but are supported on all modern browsers. -# Note that when changing this option you need to delete any form_*.png files -# in the HTML output before the changes have effect. - -FORMULA_TRANSPARENT = YES - -# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax -# (see http://www.mathjax.org) which uses client side Javascript for the -# rendering instead of using prerendered bitmaps. Use this if you do not -# have LaTeX installed or if you want to formulas look prettier in the HTML -# output. When enabled you may also need to install MathJax separately and -# configure the path to it using the MATHJAX_RELPATH option. - -USE_MATHJAX = NO - -# When MathJax is enabled you can set the default output format to be used for -# thA MathJax output. Supported types are HTML-CSS, NativeMML (i.e. MathML) and -# SVG. The default value is HTML-CSS, which is slower, but has the best -# compatibility. - -MATHJAX_FORMAT = HTML-CSS - -# When MathJax is enabled you need to specify the location relative to the -# HTML output directory using the MATHJAX_RELPATH option. The destination -# directory should contain the MathJax.js script. For instance, if the mathjax -# directory is located at the same level as the HTML output directory, then -# MATHJAX_RELPATH should be ../mathjax. The default value points to -# the MathJax Content Delivery Network so you can quickly see the result without -# installing MathJax. -# However, it is strongly recommended to install a local -# copy of MathJax from http://www.mathjax.org before deployment. - -MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest - -# The MATHJAX_EXTENSIONS tag can be used to specify one or MathJax extension -# names that should be enabled during MathJax rendering. - -MATHJAX_EXTENSIONS = - -# When the SEARCHENGINE tag is enabled doxygen will generate a search box -# for the HTML output. The underlying search engine uses javascript -# and DHTML and should work on any modern browser. Note that when using -# HTML help (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets -# (GENERATE_DOCSET) there is already a search function so this one should -# typically be disabled. For large projects the javascript based search engine -# can be slow, then enabling SERVER_BASED_SEARCH may provide a better solution. - -SEARCHENGINE = YES - -# When the SERVER_BASED_SEARCH tag is enabled the search engine will be -# implemented using a web server instead of a web client using Javascript. -# There are two flavours of web server based search depending on the -# EXTERNAL_SEARCH setting. When disabled, doxygen will generate a PHP script for -# searching and an index file used by the script. When EXTERNAL_SEARCH is -# enabled the indexing and searching needs to be provided by external tools. -# See the manual for details. - -SERVER_BASED_SEARCH = NO - -# When EXTERNAL_SEARCH is enabled doxygen will no longer generate the PHP -# script for searching. Instead the search results are written to an XML file -# which needs to be processed by an external indexer. Doxygen will invoke an -# external search engine pointed to by the SEARCHENGINE_URL option to obtain -# the search results. Doxygen ships with an example indexer (doxyindexer) and -# search engine (doxysearch.cgi) which are based on the open source search engine -# library Xapian. See the manual for configuration details. - -EXTERNAL_SEARCH = NO - -# The SEARCHENGINE_URL should point to a search engine hosted by a web server -# which will returned the search results when EXTERNAL_SEARCH is enabled. -# Doxygen ships with an example search engine (doxysearch) which is based on -# the open source search engine library Xapian. See the manual for configuration -# details. - -SEARCHENGINE_URL = - -# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed -# search data is written to a file for indexing by an external tool. With the -# SEARCHDATA_FILE tag the name of this file can be specified. - -SEARCHDATA_FILE = searchdata.xml - -# When SERVER_BASED_SEARCH AND EXTERNAL_SEARCH are both enabled the -# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is -# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple -# projects and redirect the results back to the right project. - -EXTERNAL_SEARCH_ID = - -# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen -# projects other than the one defined by this configuration file, but that are -# all added to the same external search index. Each project needs to have a -# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id -# of to a relative location where the documentation can be found. -# The format is: EXTRA_SEARCH_MAPPINGS = id1=loc1 id2=loc2 ... - -EXTRA_SEARCH_MAPPINGS = - -#--------------------------------------------------------------------------- -# configuration options related to the LaTeX output -#--------------------------------------------------------------------------- - -# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will -# generate Latex output. - -GENERATE_LATEX = YES - -# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `latex' will be used as the default path. - -LATEX_OUTPUT = latex - -# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be -# invoked. If left blank `latex' will be used as the default command name. -# Note that when enabling USE_PDFLATEX this option is only used for -# generating bitmaps for formulas in the HTML output, but not in the -# Makefile that is written to the output directory. - -LATEX_CMD_NAME = latex - -# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to -# generate index for LaTeX. If left blank `makeindex' will be used as the -# default command name. - -MAKEINDEX_CMD_NAME = makeindex - -# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact -# LaTeX documents. This may be useful for small projects and may help to -# save some trees in general. - -COMPACT_LATEX = NO - -# The PAPER_TYPE tag can be used to set the paper type that is used -# by the printer. Possible values are: a4, letter, legal and -# executive. If left blank a4wide will be used. - -PAPER_TYPE = a4 - -# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX -# packages that should be included in the LaTeX output. - -EXTRA_PACKAGES = - -# The LATEX_HEADER tag can be used to specify a personal LaTeX header for -# the generated latex document. The header should contain everything until -# the first chapter. If it is left blank doxygen will generate a -# standard header. Notice: only use this tag if you know what you are doing! - -LATEX_HEADER = - -# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for -# the generated latex document. The footer should contain everything after -# the last chapter. If it is left blank doxygen will generate a -# standard footer. Notice: only use this tag if you know what you are doing! - -LATEX_FOOTER = - -# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated -# is prepared for conversion to pdf (using ps2pdf). The pdf file will -# contain links (just like the HTML output) instead of page references -# This makes the output suitable for online browsing using a pdf viewer. - -PDF_HYPERLINKS = NO - -# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of -# plain latex in the generated Makefile. Set this option to YES to get a -# higher quality PDF documentation. - -USE_PDFLATEX = YES - -# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. -# command to the generated LaTeX files. This will instruct LaTeX to keep -# running if errors occur, instead of asking the user for help. -# This option is also used when generating formulas in HTML. - -LATEX_BATCHMODE = NO - -# If LATEX_HIDE_INDICES is set to YES then doxygen will not -# include the index chapters (such as File Index, Compound Index, etc.) -# in the output. - -LATEX_HIDE_INDICES = NO - -# If LATEX_SOURCE_CODE is set to YES then doxygen will include -# source code with syntax highlighting in the LaTeX output. -# Note that which sources are shown also depends on other settings -# such as SOURCE_BROWSER. - -LATEX_SOURCE_CODE = NO - -# The LATEX_BIB_STYLE tag can be used to specify the style to use for the -# bibliography, e.g. plainnat, or ieeetr. The default style is "plain". See -# http://en.wikipedia.org/wiki/BibTeX for more info. - -LATEX_BIB_STYLE = plain - -#--------------------------------------------------------------------------- -# configuration options related to the RTF output -#--------------------------------------------------------------------------- - -# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output -# The RTF output is optimized for Word 97 and may not look very pretty with -# other RTF readers or editors. - -GENERATE_RTF = NO - -# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `rtf' will be used as the default path. - -RTF_OUTPUT = rtf - -# If the COMPACT_RTF tag is set to YES Doxygen generates more compact -# RTF documents. This may be useful for small projects and may help to -# save some trees in general. - -COMPACT_RTF = NO - -# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated -# will contain hyperlink fields. The RTF file will -# contain links (just like the HTML output) instead of page references. -# This makes the output suitable for online browsing using WORD or other -# programs which support those fields. -# Note: wordpad (write) and others do not support links. - -RTF_HYPERLINKS = NO - -# Load style sheet definitions from file. Syntax is similar to doxygen's -# config file, i.e. a series of assignments. You only have to provide -# replacements, missing definitions are set to their default value. - -RTF_STYLESHEET_FILE = - -# Set optional variables used in the generation of an rtf document. -# Syntax is similar to doxygen's config file. - -RTF_EXTENSIONS_FILE = - -#--------------------------------------------------------------------------- -# configuration options related to the man page output -#--------------------------------------------------------------------------- - -# If the GENERATE_MAN tag is set to YES (the default) Doxygen will -# generate man pages - -GENERATE_MAN = NO - -# The MAN_OUTPUT tag is used to specify where the man pages will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `man' will be used as the default path. - -MAN_OUTPUT = man - -# The MAN_EXTENSION tag determines the extension that is added to -# the generated man pages (default is the subroutine's section .3) - -MAN_EXTENSION = .3 - -# If the MAN_LINKS tag is set to YES and Doxygen generates man output, -# then it will generate one additional man file for each entity -# documented in the real man page(s). These additional files -# only source the real man page, but without them the man command -# would be unable to find the correct page. The default is NO. - -MAN_LINKS = NO - -#--------------------------------------------------------------------------- -# configuration options related to the XML output -#--------------------------------------------------------------------------- - -# If the GENERATE_XML tag is set to YES Doxygen will -# generate an XML file that captures the structure of -# the code including all documentation. - -GENERATE_XML = NO - -# The XML_OUTPUT tag is used to specify where the XML pages will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `xml' will be used as the default path. - -XML_OUTPUT = xml - -# The XML_SCHEMA tag can be used to specify an XML schema, -# which can be used by a validating XML parser to check the -# syntax of the XML files. - -XML_SCHEMA = - -# The XML_DTD tag can be used to specify an XML DTD, -# which can be used by a validating XML parser to check the -# syntax of the XML files. - -XML_DTD = - -# If the XML_PROGRAMLISTING tag is set to YES Doxygen will -# dump the program listings (including syntax highlighting -# and cross-referencing information) to the XML output. Note that -# enabling this will significantly increase the size of the XML output. - -XML_PROGRAMLISTING = YES - -#--------------------------------------------------------------------------- -# configuration options for the AutoGen Definitions output -#--------------------------------------------------------------------------- - -# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will -# generate an AutoGen Definitions (see autogen.sf.net) file -# that captures the structure of the code including all -# documentation. Note that this feature is still experimental -# and incomplete at the moment. - -GENERATE_AUTOGEN_DEF = NO - -#--------------------------------------------------------------------------- -# configuration options related to the Perl module output -#--------------------------------------------------------------------------- - -# If the GENERATE_PERLMOD tag is set to YES Doxygen will -# generate a Perl module file that captures the structure of -# the code including all documentation. Note that this -# feature is still experimental and incomplete at the -# moment. - -GENERATE_PERLMOD = NO - -# If the PERLMOD_LATEX tag is set to YES Doxygen will generate -# the necessary Makefile rules, Perl scripts and LaTeX code to be able -# to generate PDF and DVI output from the Perl module output. - -PERLMOD_LATEX = NO - -# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be -# nicely formatted so it can be parsed by a human reader. -# This is useful -# if you want to understand what is going on. -# On the other hand, if this -# tag is set to NO the size of the Perl module output will be much smaller -# and Perl will parse it just the same. - -PERLMOD_PRETTY = YES - -# The names of the make variables in the generated doxyrules.make file -# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. -# This is useful so different doxyrules.make files included by the same -# Makefile don't overwrite each other's variables. - -PERLMOD_MAKEVAR_PREFIX = - -#--------------------------------------------------------------------------- -# Configuration options related to the preprocessor -#--------------------------------------------------------------------------- - -# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will -# evaluate all C-preprocessor directives found in the sources and include -# files. - -ENABLE_PREPROCESSING = YES - -# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro -# names in the source code. If set to NO (the default) only conditional -# compilation will be performed. Macro expansion can be done in a controlled -# way by setting EXPAND_ONLY_PREDEF to YES. - -MACRO_EXPANSION = NO - -# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES -# then the macro expansion is limited to the macros specified with the -# PREDEFINED and EXPAND_AS_DEFINED tags. - -EXPAND_ONLY_PREDEF = NO - -# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files -# pointed to by INCLUDE_PATH will be searched when a #include is found. - -SEARCH_INCLUDES = YES - -# The INCLUDE_PATH tag can be used to specify one or more directories that -# contain include files that are not input files but should be processed by -# the preprocessor. - -INCLUDE_PATH = - -# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard -# patterns (like *.h and *.hpp) to filter out the header-files in the -# directories. If left blank, the patterns specified with FILE_PATTERNS will -# be used. - -INCLUDE_FILE_PATTERNS = - -# The PREDEFINED tag can be used to specify one or more macro names that -# are defined before the preprocessor is started (similar to the -D option of -# gcc). The argument of the tag is a list of macros of the form: name -# or name=definition (no spaces). If the definition and the = are -# omitted =1 is assumed. To prevent a macro definition from being -# undefined via #undef or recursively expanded use the := operator -# instead of the = operator. - -PREDEFINED = - -# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then -# this tag can be used to specify a list of macro names that should be expanded. -# The macro definition that is found in the sources will be used. -# Use the PREDEFINED tag if you want to use a different macro definition that -# overrules the definition found in the source code. - -EXPAND_AS_DEFINED = - -# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then -# doxygen's preprocessor will remove all references to function-like macros -# that are alone on a line, have an all uppercase name, and do not end with a -# semicolon, because these will confuse the parser if not removed. - -SKIP_FUNCTION_MACROS = YES - -#--------------------------------------------------------------------------- -# Configuration::additions related to external references -#--------------------------------------------------------------------------- - -# The TAGFILES option can be used to specify one or more tagfiles. For each -# tag file the location of the external documentation should be added. The -# format of a tag file without this location is as follows: -# -# TAGFILES = file1 file2 ... -# Adding location for the tag files is done as follows: -# -# TAGFILES = file1=loc1 "file2 = loc2" ... -# where "loc1" and "loc2" can be relative or absolute paths -# or URLs. Note that each tag file must have a unique name (where the name does -# NOT include the path). If a tag file is not located in the directory in which -# doxygen is run, you must also specify the path to the tagfile here. - -TAGFILES = - -# When a file name is specified after GENERATE_TAGFILE, doxygen will create -# a tag file that is based on the input files it reads. - -GENERATE_TAGFILE = - -# If the ALLEXTERNALS tag is set to YES all external classes will be listed -# in the class index. If set to NO only the inherited external classes -# will be listed. - -ALLEXTERNALS = NO - -# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed -# in the modules index. If set to NO, only the current project's groups will -# be listed. - -EXTERNAL_GROUPS = YES - -# The PERL_PATH should be the absolute path and name of the perl script -# interpreter (i.e. the result of `which perl'). - -PERL_PATH = /usr/bin/perl - -#--------------------------------------------------------------------------- -# Configuration options related to the dot tool -#--------------------------------------------------------------------------- - -# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will -# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base -# or super classes. Setting the tag to NO turns the diagrams off. Note that -# this option also works with HAVE_DOT disabled, but it is recommended to -# install and use dot, since it yields more powerful graphs. - -CLASS_DIAGRAMS = YES - -# You can define message sequence charts within doxygen comments using the \msc -# command. Doxygen will then run the mscgen tool (see -# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the -# documentation. The MSCGEN_PATH tag allows you to specify the directory where -# the mscgen tool resides. If left empty the tool is assumed to be found in the -# default search path. - -MSCGEN_PATH = - -# If set to YES, the inheritance and collaboration graphs will hide -# inheritance and usage relations if the target is undocumented -# or is not a class. - -HIDE_UNDOC_RELATIONS = YES - -# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is -# available from the path. This tool is part of Graphviz, a graph visualization -# toolkit from AT&T and Lucent Bell Labs. The other options in this section -# have no effect if this option is set to NO (the default) - -HAVE_DOT = YES - -# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is -# allowed to run in parallel. When set to 0 (the default) doxygen will -# base this on the number of processors available in the system. You can set it -# explicitly to a value larger than 0 to get control over the balance -# between CPU load and processing speed. - -DOT_NUM_THREADS = 0 - -# By default doxygen will use the Helvetica font for all dot files that -# doxygen generates. When you want a differently looking font you can specify -# the font name using DOT_FONTNAME. You need to make sure dot is able to find -# the font, which can be done by putting it in a standard location or by setting -# the DOTFONTPATH environment variable or by setting DOT_FONTPATH to the -# directory containing the font. - -DOT_FONTNAME = Helvetica - -# The DOT_FONTSIZE tag can be used to set the size of the font of dot graphs. -# The default size is 10pt. - -DOT_FONTSIZE = 10 - -# By default doxygen will tell dot to use the Helvetica font. -# If you specify a different font using DOT_FONTNAME you can use DOT_FONTPATH to -# set the path where dot can find it. - -DOT_FONTPATH = - -# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for each documented class showing the direct and -# indirect inheritance relations. Setting this tag to YES will force the -# CLASS_DIAGRAMS tag to NO. - -CLASS_GRAPH = YES - -# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for each documented class showing the direct and -# indirect implementation dependencies (inheritance, containment, and -# class references variables) of the class with other documented classes. - -COLLABORATION_GRAPH = YES - -# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for groups, showing the direct groups dependencies - -GROUP_GRAPHS = YES - -# If the UML_LOOK tag is set to YES doxygen will generate inheritance and -# collaboration diagrams in a style similar to the OMG's Unified Modeling -# Language. - -UML_LOOK = NO - -# If the UML_LOOK tag is enabled, the fields and methods are shown inside -# the class node. If there are many fields or methods and many nodes the -# graph may become too big to be useful. The UML_LIMIT_NUM_FIELDS -# threshold limits the number of items for each type to make the size more -# managable. Set this to 0 for no limit. Note that the threshold may be -# exceeded by 50% before the limit is enforced. - -UML_LIMIT_NUM_FIELDS = 10 - -# If set to YES, the inheritance and collaboration graphs will show the -# relations between templates and their instances. - -TEMPLATE_RELATIONS = NO - -# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT -# tags are set to YES then doxygen will generate a graph for each documented -# file showing the direct and indirect include dependencies of the file with -# other documented files. - -INCLUDE_GRAPH = YES - -# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and -# HAVE_DOT tags are set to YES then doxygen will generate a graph for each -# documented header file showing the documented files that directly or -# indirectly include this file. - -INCLUDED_BY_GRAPH = YES - -# If the CALL_GRAPH and HAVE_DOT options are set to YES then -# doxygen will generate a call dependency graph for every global function -# or class method. Note that enabling this option will significantly increase -# the time of a run. So in most cases it will be better to enable call graphs -# for selected functions only using the \callgraph command. - -CALL_GRAPH = YES - -# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then -# doxygen will generate a caller dependency graph for every global function -# or class method. Note that enabling this option will significantly increase -# the time of a run. So in most cases it will be better to enable caller -# graphs for selected functions only using the \callergraph command. - -CALLER_GRAPH = NO - -# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen -# will generate a graphical hierarchy of all classes instead of a textual one. - -GRAPHICAL_HIERARCHY = YES - -# If the DIRECTORY_GRAPH and HAVE_DOT tags are set to YES -# then doxygen will show the dependencies a directory has on other directories -# in a graphical way. The dependency relations are determined by the #include -# relations between the files in the directories. - -DIRECTORY_GRAPH = YES - -# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images -# generated by dot. Possible values are svg, png, jpg, or gif. -# If left blank png will be used. If you choose svg you need to set -# HTML_FILE_EXTENSION to xhtml in order to make the SVG files -# visible in IE 9+ (other browsers do not have this requirement). - -DOT_IMAGE_FORMAT = png - -# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to -# enable generation of interactive SVG images that allow zooming and panning. -# Note that this requires a modern browser other than Internet Explorer. -# Tested and working are Firefox, Chrome, Safari, and Opera. For IE 9+ you -# need to set HTML_FILE_EXTENSION to xhtml in order to make the SVG files -# visible. Older versions of IE do not have SVG support. - -INTERACTIVE_SVG = NO - -# The tag DOT_PATH can be used to specify the path where the dot tool can be -# found. If left blank, it is assumed the dot tool can be found in the path. - -DOT_PATH = - -# The DOTFILE_DIRS tag can be used to specify one or more directories that -# contain dot files that are included in the documentation (see the -# \dotfile command). - -DOTFILE_DIRS = - -# The MSCFILE_DIRS tag can be used to specify one or more directories that -# contain msc files that are included in the documentation (see the -# \mscfile command). - -MSCFILE_DIRS = - -# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of -# nodes that will be shown in the graph. If the number of nodes in a graph -# becomes larger than this value, doxygen will truncate the graph, which is -# visualized by representing a node as a red box. Note that doxygen if the -# number of direct children of the root node in a graph is already larger than -# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note -# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. - -DOT_GRAPH_MAX_NODES = 50 - -# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the -# graphs generated by dot. A depth value of 3 means that only nodes reachable -# from the root by following a path via at most 3 edges will be shown. Nodes -# that lay further from the root node will be omitted. Note that setting this -# option to 1 or 2 may greatly reduce the computation time needed for large -# code bases. Also note that the size of a graph can be further restricted by -# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. - -MAX_DOT_GRAPH_DEPTH = 0 - -# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent -# background. This is disabled by default, because dot on Windows does not -# seem to support this out of the box. Warning: Depending on the platform used, -# enabling this option may lead to badly anti-aliased labels on the edges of -# a graph (i.e. they become hard to read). - -DOT_TRANSPARENT = NO - -# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output -# files in one run (i.e. multiple -o and -T options on the command line). This -# makes dot run faster, but since only newer versions of dot (>1.8.10) -# support this, this feature is disabled by default. - -DOT_MULTI_TARGETS = YES - -# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will -# generate a legend page explaining the meaning of the various boxes and -# arrows in the dot generated graphs. - -GENERATE_LEGEND = YES - -# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will -# remove the intermediate dot files that are used to generate -# the various graphs. - -DOT_CLEANUP = YES diff --git a/src/hal/components/cros/LICENSE b/src/hal/components/cros/LICENSE deleted file mode 100644 index 1ba70868..00000000 --- a/src/hal/components/cros/LICENSE +++ /dev/null @@ -1,12 +0,0 @@ -Copyright (c) 2016, IT+Robotics srl, Italy -All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. - -2. 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. - -3. Neither the name of the copyright holder 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 HOLDER 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. diff --git a/src/hal/components/cros/NOTICE b/src/hal/components/cros/NOTICE deleted file mode 100644 index e8d9bf90..00000000 --- a/src/hal/components/cros/NOTICE +++ /dev/null @@ -1,8 +0,0 @@ - cROS - Copyright (c) 2014 IT+Robotics Srl - - This product is licensed under the terms of the GNU Lesser - General Public License v3.0. - - Portions of this software were developed by Alexander Peslyak - under the terms of a reduced BSD license. diff --git a/src/hal/components/cros/README.md b/src/hal/components/cros/README.md deleted file mode 100644 index eca8a63d..00000000 --- a/src/hal/components/cros/README.md +++ /dev/null @@ -1,63 +0,0 @@ -![cROS logo](resources/cROS_logo.jpg) - -# cROS 1.0-rc2 - -cROS is a library written in stadard C language that provides a single-thread -implementation of the basic features required to implement a ROS -(Robot Operating System) node. -This ROS node supports the concurrent operation as several: - - * Topic subscribers - * Topic Publishers - * Service providers (servers) - * Service callers (clients) - * Parameter subscribers - -cROS can run on Linux, OS X and Windows. - -### Requirements - -On a Linux-based distribution it can be compiled with: - - * The GCC compiler - * CMake 2.6 or later - * (Optional) Doxygen - -On Windows it can be compiled with Microsoft Visual Studio 15 or later. - -### Build cROS - -On Linux cROS uses CMake, the cross platform build system. To create a build directory -an compile the project, type the following commands in a terminal starting -from the source root directory: - -```bash -$ mkdir build -$ cd build -$ cmake .. -$ make -``` - -You will find a static library (*libcros.a*) inside the *build/lib* directory, and -some test executables that makes use of the libcros library inside the -*build/bin* directory. The entrypoint sample to learn how to use cROS is -*samples/ros_api.c*, which shows a non trivial example of a ROS node with -publishers/subcribers and calls to ROS services. - -If you want to build the create the library documentation, type: (you'll need -Doxygen) - -```bash -$ make docs -``` - -You'll find the html documentation inside the *docs/html* directory. To browse -the documentation, open with a web browser the index.html file. - -On Windows cROS can be compiled opening the solution file msvs_projects/cros.sln with -Microsoft Visual Studio and building the solution. - -### Licensing - -cROS is licensed under the BSD license. For more -licensing options and for paid support please write an email to cros@it-robotics.it. diff --git a/src/hal/components/cros/docs/cROS_manual.doc b/src/hal/components/cros/docs/cROS_manual.doc deleted file mode 100755 index cd4f7a58e1376a5959b788b858125409f4070248..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 491008 zcmeFa31D1R+5bPKg_I?LKF{nLGE~vpws1 z&T{Yl(MvX5eakC;lPdZ-Dm6Cs&*yhe?G$`Jp3h$xYrohtmHHB&$8h=2=bwLGzWprM zp97xOu`u5r5G-3OP` zkT*`JCES18tW;_e=|9O|`6>Q8;lxyGBHVq*$*I(1+~4DrRBD*NedMzPe^2MH{M7ST z>i_ZkksKa8`N33b2G{T6`3aIi(T{8Q1(a9r=YuO!sW)>&>L))go%<{KhCi-f-Cx&E zmqxDta3J;Lxruza_f6(b@n6aR!?O*&zAOD}`_0wYT}%IjKQ5o}yLvvm_EgGG^1Vkg zx+|4>kNn*)mHLHYRJq#ZpZ>eeqpm%U-`Y?9pXWcY;{(0~_x%CyPNfcNpbpeypP z$wy1lgCpsF`DVAq*4*GQPZqe?J6gW1H99R9O=_7oYf^MtUv%1;(P<;mK6^*Up4{Hn zv3Swa+o5JO^U|svuJQQpX(Xvmg0BbJ8GYQOj}3W!YGkW z@*A_yw5TPTHemu~RnGEo$x>=Rm>FK1%df7Gp`Sd`gTF$ILSLirqpZ0w2 z^i20~)RWEgCzsDwZ2zqKbDr({Gz$mid(+*SD7Pk)Uz5$Ot$I3WfBlOjO7W z_EctyLtXxd)~v0OE}uWbyCzFNchfPI^FOTq{8iLcFy;>p^=G?{0)bzI{RSu^LbMHW zE6~(SVa>+OiKTEb*F$He2k9$OTjjH!pbl!y@xnZ_rjD4S(kyChNxA@8YRSbMFU~V-3dS6j=Q3S* z(vrm;?XA&J8ji?Fp@rc*n!d6Uo#VxP){I&VK1b!eEuYWjqi&d@;3-!_r(KU25cQi+>>3lxh-=CWr&0i(Lm>rBx%<2=ud-FAE)Sq725d1Xm7FuIl z-)tdDTeo3a6}KU+PD4{iZe?ybS~ir)qYo&yb3NQPFgfb(&q`PHXIJLa`Ss0>!inS@ zI~mAycCddv^~9&>$@E0MLRY*L%_gterej4@h)&%*Qbz&e%szC++?LsgAC5xF8wJ)f z;SJ5vqG7RyYqG){=|Roe*c#0`H!v_VsA`L9s>`5E)U>uQ+ua98jr8|KD>IR(fr9j2 zI$GY^K7CQUb1f!M<=gK55z*FOdk>`x;$cmV@|l4gTAlK{Gd(c8>fNXbahR;a?`2}z znsomN#Oq7vd9XQJ-bc4|$CZP`v61AKci8BnmFpoojF9fv1|<3zLtWfOv**l$KV28i zn(&6HQDKC(rVCL%SLhxZqT0E_(0cvTpDhe&O@(QvMz&Yz#_n8Z9Sm*m`Fdue^gxb| zznHw24#>DbHTren1)teg; z127m;NWJ3OGY-RaStgzd6E~`)SZ}tG8kt7gFGPgkm7!b#ZlEp!8~ctod?HmY?in~Z zB&(B$3^g2Ho5>7HI|qa#GxT(vwY0(KJ`cGV8d(XqNb9LWIvUOmWxJy$bVY7tRUhQe z^kjOogVaOvuzpT~!ci+DD@~u~dCEPiH>~}$@|%yE$anpa5?o{`Od}pmCSNW)FJx1N zLmW?0No`o;vAMM(JyVsQQa8t$NbeaV*yW#Vem#g`|llMGH8WK-4#V?rmyMR&_aA zm2yp&rz2~5z2>lWu(n^TLa*i4Vl=f9F?FE>#Dbe(U1?Ohr-xRman&}eI#eOFWEV0@ z=k!8!Lwfq!BrwWXv(X-f6I?UORtOCsmeNXCKaNUdw4)(e&}heUssrPCN#BeN^@u;s zmjkf3;bNOjcVBvNRi=PYEvIE+XF$;rQ3@C!wK+y;1~TbE-*QyrfN`Cis;~klh4!Zz zX7usYh^T+lVpOW=B+=z0X^uR*d8OvGuQWg?Fr3Tvugt9zF(dO&@SqLeNn^Xin`o^N?j*dgb-fT~2^q7o*cqGGd5% zC_9X72vg#HIJOT8hH;!ll{Oi6P+s6c$NSAuqMSenBn#4(mXv0jSiYp3RcMsvvn`+* z=su~BeKwzAyaJP{%*pq@sn)zW&K?$9Y#M=$#G4vXX_S5#i~{!q&!c75+!=N>urRwy z41UW5Rh>pP_}YlCO0cDS*r)(sGDdi{aE_QFsghIhPW+?^L%IC0PvFLj49hd>H%i8X z#s#!oXp}(tT4V~taFA$isfcvEe|l<^S!X4Mb{3m6shlg@+%w&qM^px~ct5DB0hEM( zOCeNQs+e8lc+>JMlo4z(pSdUs8M=Q9Eh7=tDvl{-j617&GvSI@VQ`u|-7Bt2> z3fV6m@Sc>ZBs8W2+gytmUCpzDLs%>k6D*f}tj3%;z6>%<6I=#jp{SN-VK~;>p*TTd zC^v{JR7)4koYgWnsS9RKh?>gsb%Jeen?>Ko{7G-e+*v5ko}0y=GvQCyM8|H^EtwMI z5YL>k?-B5^sE|!E5^-+EU#7w2rJf7q$Y3}_MpY&=Cf+Kp_<3O*%a92r8`d&upawH* z1E<3H8g)?jFdH1P!amDW*9AQ62q&a&Scz>)-&xQ%?jzH6`6?R7L@8XbI}8GdMl@rH zX>L5@#{tGBwkomwSiy4y164IL?lC(I%8xJ_VhoFEi033wRD+^j7!9I>ln(~g@gAJE zvOm|YR#GM}rmxN8qDMIsYCtfXqsCT-CQ?i1>QeD2GaPrCU@@z4&YTtjion| zBMIpQBUIiZnG$SqJ`3BQQuR_d)kG-_flQrkqh(NpwAqJEt%48>Q6)S0c7CdHN&`CzWHxwV7+xwHg-Yl8Su?dAA}RS^u6Xq3=FqGx!Y>3jjMcZJAT&JP^Utb=MY$j|Cf zwKv;O%BJRl!YX~VxsYEoS$I3MF4H}tNrtmTf|>?%5*}ES&5aNWv=bmOWgp_r$k_Cj zkeL9L^%5s*tt_`l=!2pRo0z-}n=j4|j;zz7Q3fYPO>lW1y}43@8fsOH^S`RQ8y&q& z;*Q-KQwmCoEOR@qmT;2@n6;5TLT^epgLA^$eYqa38(A=p&kRWvlQ7t{-h3wG;+{u{ zyag;u`;FKkBl(_bbn)9aC(mLxCg^?i{Rbac(iqn!4W`^o^!a~O->cu+$-IXYHAVV3B`T>>bU^jw^2eTR* z&UP1UP&is{hP%jCtF{A)wB}~57-3;19g*V1OB$lH zjg9krWyZ&}oPLsMxfZDpOve*omTP3Cn^zVGfsPi&aZZA05xMBbS~RCc324}~IqI}A zZ_^MBgcyxOyamIIldds&xW%bY;?CjRIMH?&-uIDMQ>~TR!DO}Q#iEIIT1LJ^1+i>0 zUZ9_J!d)upbpvI`;exnnCTJ|(tdEL=&BN*jH^eTY)X zjpPZtYggb4ksGYzABvmo6{R2QR%*M{2s0%1t+CxSQ?nl=jNh;zn6hfXsag%SrDX&a zn8C^98}wnTsePG+?7&F>aC$I<%17(cPudf-AR^EihY+OI33a1+JL4nTs?gBjZB}8L zWad;x1*sqpb;`u5a%k}6RL4TbF)DFVEOlJc#C4~sRLiK~9(!TH^f4`!}5KE?4quffRB+r_( zh7r_SOOG_Q!d*-Ggj1yB869nlNco`*?6roON>#dpBLgduE^>=~NpUgVbPyx5g~*qk zdLxvRX0kC(8h@l3I6_FHa;{N?1L<|Bih(FDL?4$_IA(4}Nn%H#&L2o)EutoOTMQ^o z>XLRe_o)jio<+q#?x8KSXUv`>^T@K`%nt8oPoQr%CjvN|5xDDFv4Ya~hA2t}?PQFH z5nw-~(cNKa(W+6ErpaD`gtXzY*iXs~4Beq;m^YpLQNgb4UObw&e3oYqAhdmD(hZT&4U|rh519-4m(}Y>BF!z zWjasl-4#CY8s0B{w%u`5`A37>#ZPt`@oA+|nSA(khZ*-5-LLRCwoh7^$RMb>DI-qg za`8=Sm&v-Y^c91i8up$Iu1S~s$2`TwO6fGWx&o2wQhy&NFdn@4$mEce389`OD98lW zi133roF?FH=N7GNCW^p@Fq(+FGOu8=E23nwPu(OkB}mZ21=77|Vsi{*VL|hK8;aT( zcdt*eDXny&Q`1gAM?hVZcSNJ(A)-;W#jeX(g_D!Xd=ZNEG)-egX5f)#fU{PqBKk4c z-90kIY6l!T1Vcx`Mva9Yi_DqXP|0A1{t3DYg18D{;j9-PlY_CniHm!o(wY=;ukR`6 z1xdBZ%mq(a(2#QuP3+;Q8^CBC9*7G>RcaVkD9WUg7$lir_Ti{BaVFs^A>q9{yo z$LRnkDT|Pf=akbUKeUSyCfmc=507@+Wk) zNPBW9i_dPfnpS(74=ng>@p~AIA!9nVCt3Qqgw5B6ks!4?fQ;LuceNPbmsT_TYbI-T zj$QJ@G1TgsxpLIBelyKkDpwPl!&HiDV!Q`SKpQ-4X5>-g+r>&-n9tYA=VvE)gjrpL z#l_jh?8;INq`^*j^hkh6S1=hd(1|@6ozw()Tg%B2jim$vIQLMKnQ!5|+A3 zAz=}hkr~6n^#NgQJ^03FqaC9e(K{ldO0(ij!XC1{C<7vA>8ldS<)tkvtxPPIo=CnL zj-i4^#nWr6WPE5BX%rbNSETC$seUX^-fGs_1npI}HV|Gks>G-*i3OS&ZWvySp<5m1 zNf8(6f2=C!E0eL)T+SUDZjaY>5jPTMx-3y5KarB=W3#!sYRm`G5WjPsS zNYm7+xw)Mvm^?Dxj|pxYiz(X7bnB3bDPyVaa(FWsDlu*9V%P{ZZj~~Jil4ULIAG2) z8JosR1vku)XmOEovp$UtCE&ZHBRZDFw@vg#e`atME2)>vKdI}46>Tfpx)!%B^}|fk z>NwJt)}@azp!5!9pbDM%br|EhTxzkTunPB|4zS!NU6<<@!SSZmx>BY>&i?0C4l`_* zIq*d`6iiJ`64@`)i+7=9D9#I#WeeSDrU{snP=>bMXNUj;Nezq|?vGdHJDzv}luo7YDPcKA%`kiJZBj0_RyYgp{o7sbwln zs4SXKN9XQwA@<6QXjJFT=tFUba$hD_2W7>w3~+RiLDno-H+q-^I#DMUO&8c0dj)@p zv4f4Ye*I|{p1E~5CWW)`uwP@vJx)gKD{q6LPC;1Jc+bqLIgz0x^kjZzmSCt1PUr?5 z#t?pHYaNJVbRi!jR=v%#^681DGZ6d#xPBEy4R0ToDLLMMFwoz;`K~XsGc+fIr+{E z&tfCuo}-{GsuQ&(8LOwXMR=QKbT+r>9Z^PZiZhdE!4Kip8hvWiqbvrASruXM)=~P2 zWgpFvbv?9S{o)b1Yr3MMB+Sz>#-?(-s)?Ab>3StP&2_Wb$_(|YU#3i$X7*!UvEAx0 zisNc^gO3k*tp@9e#jvB^(#kk4lUaVh4=waZVAEpxEV^B8bgPqqVY)hn)G~Sn$7d78 zbh+qe&q}(XP9zsg>{=f&0ZcazP)mt%yZv}!1MHaIKcQ@7q}lqZu~`}+hM>XIgl7lU z?ZQFj09gww7JXBB(=35eu`)Ybt%Z-PFP$k`=Ecfz#^To8HEP&T9fBsE36+Vv++a+- zG9Ig&I~G?OYp(ls%&d``s_uS?ZZNlngI{j%}k)$s5T}mRt)iWAmU1Sqw1q6 zj;m~Jo`FF=nj#UfE}0rdfyH0+F^B8E;CTwobeZo)m?G0<4a^?ODrtLD)?>H_tcLE& z=W@ectg`5vES(b2-cB08zZ!sZ4xEk(&042Gb`pU+G(|YSY}cV^!yT_Pf1`!z9DwRv zJMk7JB)k2h zOQ%w0MMJya_M&sp@ueZQ*+ALgDbcW0Hk?^!%ny}~U!bEWb**v2WxhmAr>+~t6=$j# z4~OYb%}VIiaI8$myXE+1-xP!pesXJ5q~X30_F(wbwgMO|!iJLBWHXWLnW15Ra+i(d zVgvC~fgrVTM?<^t3$--0aK*-p56u)jDnR!@QcJhDrKM%otdBd?ZdU1JI~9o$Y6z1eFna z%N5wk+e%y8Wz)WsFB2<>O`8FUzIsA8`qwd z2GXo;))9?yB29G5Kj*^fvdeWSG2S*!;yXD*g+YRC6?wC6KxTZmsHnmQn<}Z=7ux7J zOhPSXDIL8&qlj4QZYZ^c&>SbCi$RQXFsmA6wpQGB2DWS;Gv>vx+Y2IMm1WFE!js}v zB23y~T`TOE%P&OIffjLN%U;qUo*|nfyGB)d7VfZ&DP~6JI$$|Er*v*vGKswmjjCce z;^7b*JqV|pUJb%xY^3H%qmBY|poJJqu>~3K1yGb_saSp$OOl>=>=ItLp1-=kZY)sp zy=-Kk8ZYz6lYc9O({Q~gLisQ?S_%pAf0(=SH<=IMvMWxrCYSADG$QA4t-}FAVMoK= zL%Q6lKg~G~^2HQF;@O^cT|>k95~ef0nT#HM+?DcOoO~>k!)pEeXQ)BSmYHCAB=;dBWk~afT=U%u4pk^PWwqCl%%q- z6_q8E3n(t@gEP2sGY{g`Xkwvr!*Y9Sfi$DqLvmV)jqplR8SF{+X&3D@a}ma`k%f+k zrZ{@;01|mdz7}7Ma4#!xWhX9Gam*4rKIz!*h}R%B>~ytHaAUc}2)YbU6>)L~MlFsD zZY;BYKF(QAc2~Ful}KlZWqOa( z0q`iBaalB88550)n_*+HXa_et;CtakeF!}oPa6C6ZJ8bh4neoz)7Wy|ZbnhmvMvY> z3fYWI$1M++g~+5F#(++A)K!84$Q)d1g83=ks3E7KPy`5W6GwUaQ`>A zC8;aY1_#0v{?Htyal8Z=-_41`Q2mW*C=Ir(l?X(R|}Q0f>Uv-Z3wr7R=r^ zCKiGrI!hGsg@|tHnq2KxT|4TRuFGm&7}*5-6Wl0?gU3l^R(@G-d;u)+a%!ZS$&PCr z+j&l+C9EcIwf3^Z#aPQKGMz76VyEG!SOQIr#C%TCp+c9HH?@#*zyUM;W}ZfR#yhNb zSq9zX_{-FyzOk?^x4~6}kz2NRy&K1QJ!q_iO$`6a?7OR<=K-q%WFd>u6mcbu1ewrx zS=Yr^p3D=JWnHETX02SWC#b;P@!BlorpJ1*lps1}&9G}g;u{i%@ZNokoWT*q59F3} z7u}Xl?`ly0Az#I!2~5Va#f;&!xAUNqlu-|<)Ij)^?wzXpR}_ffnvAwNSD0cO7U9E* zI47Bccs28R)Dd#3d7^KfpD?enRf7l1=PzH;i85Q-HqV}OBeJV1mvjN5|{FH`f;z zKUo$XbeUk^)W~POMSb%rtN^O*it92VJx;5dCmy7#;E0RnDsjR^dg4c|xyMxTwbeLD z|JlfUVtkNv&XHuFp9zoIe9YHwxs&xdO#k9Ji%~Z-<2W)2C&Zs&wPV(LpA zZ?R!+VVSxD)6_Te1ah<1erm)zSX9qxes7F$&&mub+TE^=P==~I=E)I7Yx{e^d)2m0& ze{`Y*Ty%63Pra6TNE9)hX4JeNyVvNuQj2AZRA&=P2ni6Jcq}FY>?mAoL=&egJ5Gt$ zO~#{}?PC{4({+wrWZSLNT`1F0jb7q0Q=A3N1I4V}Nbt?*1_3eIVda18F+h76>X% z0wm^g%&uEEm6I3_z<135gZIcGaJRftbfHryo_pTBWE%RN-9R_`AhM z9i9}PpJC@JgO1=_KY}SV3-O`u$*%Bm2Hoh^EoY^;R>)G>qQQrqT8LdCr=(4$PNn)T zEzN=9i&VlrXDxw$K!Yz^W_&~EY#-ok<^ZRC#nFowbsk$XzKL0}irL#5!<)EZXBLFa zHK!U9+bpY%aO@hIGV=}Yjs#N7Y(U^bF$ED85qfFP!`tLp2)w9*o&!;NR$@(e?V*sZ z|1@=XYRpUoyRdW^9nT=(Rmyaj`fQ02PplGbK`P1A-K+_eQV9n;kB}?Qv&#GJl%pHK zjBfQ(Xy!%r_}~bfJx`Rg3FNLq35H+|gFSwSk?N;+4ocu_q|+hjt75f$vAKDldw)TYMJ#T@4^ zM0VOi#x}OekkrkSIO}6~6~-2i{AI#RAHsmU9erK0>{Qo31))Ll{Op9gIftHd@gg-= zS6ZYGNycT{m8{7k<;kG9xR93QxWP%1=U92r%uv@JTE%HAjHn2VQL@WO17-0DQlpo; z-=glh6_qtj7yAl|lnOm%tlab0Ve8Pz22qEB1IU#Ri0ApCEOTZgcGKW@;1C z*xj$k)f>l^$WoxVmGkXRd^|awa)tu33uHh}=IS*Ea4Rdw)Cn;G6EX;@AUu#+mE&kF zSui2|4!^}*&;njDe{f_EOR_`>yk>86c}r8p!JjEMoT?bd$3}4iJMSz3;d0XB^?%PLpPsbr)jq81e2{@ zFu_!AbSbIyz-^I-P+INd#!+%s4(4Uqb6tZ>242awCk>Ashe?uzN;S&C4dA3kei({y zy1uQ^sPK@}WSfk|(m`Fd+SxH)I0A>|onXq`ysM!-Pe)3rXdtP0vM@itXv=FUQ=Y`+VM{rCOZmXJ#`U?p#3L?8 z8WhhAxdT&J*09Bb1)ZS`dP7J4RRRX;)fd_M8D$#l&E__kr4&6tk3twZzLBdE!Qhsj z+~BEG($Ok^;>qy>T%KEWlsKi}4OF$IX@)DNd1s>Pw3ux> zqUl~!pw#qPvOI2;i=TdLIQJ?gLDEEZF;U!rEo4VS}khI+rD`cvACWvC9AV;n@ z_~;Z^rFbVv67xZohF0R5S#9rDXE;OPRHO8Gay4d+CU4q|j^k!imR$o-vP8`@Zk526 zFmufC$#L2CwP><%N)}l-k;n4;wP;&rJ|~MuRKt}lecl2gf<<8E5F>DzA}!wx-8r-?I+B!~L)Y{g?+o(=lw4h{8snam2^TT*`0V^`~H3OwvZnnnMk}cmU#!yHq zju!knKkZ__64Gp7Pwr#keHUe?fSj9Wf*VCUP^Rc|*?8S@71v%<%G*1pk=(rrvYLAz zCi4z{!j;#fFpOW$>qmS6Y=P2uCZM=tH(p-n;a2@i*xa0mEj~a{4zkG`NaYZwE#=Tt ztnML^;_c8Pt|v}?>6Vg+0}%A&ZW(O5&QCZFp7P$u-ZY2POI>x3)1MtKofTK!cBa;{ zz6}h{;Yt{q|G*$ST;?JakXU;(mY@WsN9Q6Y8TsGIN!;X>-Po#TlO zCJCKgtJKEE(s|PQ_ndUvBcDT^R?#0>;rGaOdMBX7E0GoY^gByNO5BOq(=s8u*uI+u zUY=aM=v!Vd5l2CE4vvjjMdyB-?W6IMi!EjMP5m1dm2A@rYKZxd+Y{8Uc+_LIer)Y? zBPy*?ib>|zHkG9qOXN0%Vmzj=rhTCaW~yI?U~vUsWb{&_D07p9RLj2Npm<6^I(^Ij>h!0h6RMGSkBF3+WKne0ljqf0m3;*}gcA>lU^1t+xOln-PEc(ay2$wH4H zbOA%}U1^}feXA4D>3+6b_sCjw(W-PkVLX>78<<#aA>Mhm+t(y^)q>M8T$WaIniB!Y zYCUz5g`j@jiPi6B`2@O6Pv6G7rC@QOeLlRh!o4q&9ba~Vpp4F$RIb;xv6&SOL=*k% z3KZSiwoYHTCr2h`%sadwcYKsZ6Cx#K>0dvYeX8Obn~t#^&$jNMnGtUhJtF)j;6 zoqR;&Md1zGAx-vP2`{<(Vdic#cnLd6R~g-Hxj{G7+MSB7Z*?dMtrMK)p5mP;tNfd^ zJh!=}3Tl?@QWBe;%uafCW#1krfCN>QX8R$PzLigzHpw5&k$LT+JHI{%D>?rD~_(*2Ae2N4urRE+cn)e$Oeif&jag6>$Yyke+Z_7k%$6%KcFT^p_q z^phC=NIKhnZcbfvw0+P@eihxJ#=@mG?5%q*ZC9389aGQ}a`4d+jrK!yWZ1h(>>?#1 zJ=bJYqhLxck|qd96xC26G}?-uibkkL;*qLrSY<_`V?mWO3wZ5K72sk=VW9@e+6lB2`wInq_blhTb}>1K(JK+}A$drb zhH$*y5U-2Z^Ofl14AB%vK0DpXv94HHJuX<|DM&u6PB>5#*;P%Zv@Rj6&?h#@TA)UR zkP$4msh6C5uJg6BuUM2R6C(1$YJx-YL9dQc;@7!qX6R14!`?3mH^Gh>o7MADcx;j4 z?sr|;u++Z-awVq{%Gs3tBLilmEa%6e&TOd7HpL#O)1T>e@3nObpjas@In6=KON^R6 zCNr#G7I}lbV%(jUtRyO-ZXuJ-a}J`}oT1(7*MjK-u8OW_&AMzk^i=#vx6mK8ESfke zWvYOD{unkUmRN?g+>&MIMfaJcGhV!ZqqERDX4yrYZDU_j87S|CN{3jT$}@Pi@TtD@M*KTm z6l1g#@l#u;<)knD-pOL}m~0}vvLit`3BM|Fq@A90gmcNM}_+R#t?NT-2)f+}Snj9g0+3 z?^=0FooZ!n(lm%Da<+=r+DcyadM`8*TfOEs5sTAQhJ);DRb8K~Rd<`n34b8{)$48q z9?zzH?dK1h1SpB}37qVfRUBFi8Oc;7`;x+{D&=#xX5pkMU%L*%XlpWyF?%FwXi$%R z3NPnlT@F=ho-8`DO5I%JPT{Eg@&&ZwW9Gwf(=vTunlqo?Fw59nZLjn3de!W|ubX#f z_2|0#%dLwYdt^3=@wiNyWL!!$Xe5iBaLKwf`BWTM%~G!tK`WndK%!ZqSXHT*}jABEtMF$nEP(+*)rNmt%g#^`=hvT-!g=ouU z+{}k?tgYQOUyPBXD>~t@(3!U#YI~B$#SYOmlWS)sx}!>7#*WpS8F{tLty(d)WA)dp zT5eLcQDjHmX3eZxH)X5(>sBo{&!zQcN8RSmu39&>NA+c9tU=}6q;8|g&g%cl`Sq&R zsjqHU+gHxa8kzcXGrGHSW*R$`r6u_(bxH%;wn}!fZ#hLm>pEUeZgJoxH#VHf(g>Zb z(G%l1SDM&|9oDJdt~<7%G98W_Wt^p7x)EBiI7aT7?6Dc?{ssw4xr#7&qISh2#~E&X z?GMgntq3D2J-s$~fyOj8c?Ju_WkNz@axN~(u+P3{Lf%Yn>SrS|JSRIIiP16yFtK^5 zr!P?A7qFzd&Vl>BPz;S~J>!B`rV9@fY%s5mSB-DmAfjL)zAV#Go50tlvxeEo~)TyrPXq)zt_C>tCn5;{pJ-`we0HeIis&C=LIbbW?jYR-gUvZQO_K9%E z5hka82=U3Bu6Qvv7U<%GHu|d3eUy|;1Zxb}J&7XYMZ>b;J|_Vg&V816V!hL^s){y6 ze>#oAvqrTz{q5={3RPoHE)41$CkmPzCxS@+#CYDl}Ct|Z2O;O^M1dh#W&Z5{K=ara{ zr;=pT378y-#4AIU@3aIOsPziT?n_VX>+0sfTKd_>pH0PzVyiD63mFnDlwqS7Afb78 zb7=FTDcUA;x`yM1aHC2s$7sF`K*_o?H-s82akz)gxv0qRj#as~V@p@868x{>P zgfTnZ!-W^c2j`eR^Alf1)8EM%DVzt{Hm4mNEwV?Rtd$o}2~)Ut@zM$oO>~RwWWACu za$z*ZIbfR7QC#vHBsEL%bTy5ahEQ{DvN{bJHR!omw0PUAp>Hs)Eq4u9Tl4DcD%wvu zJ!{vAnVutAJXU_Off9$3&LM?3qRbs`ea8jq)uh5Eg zW%3=I4Z);GS#GMZX<<{i$;q6$8CFHc9OgDVxRWi4*3V$=HlNq@mEzLon0i}^)l5H? zTVZdv{p;e`{Al?tL8rLZFJ7K&&zUoIWocC9cU?^PM=?paz{SN|Y}A5?g6PQTdjg$~ zD9@v+@e*S(R$9thKL7e}6KGmF(%aj`JY$}a*xKN3;^p8amdL6E>!4>>=F=?rS5HM9 zEKxeSetGdJzKXo*(eb%EVoEkU;Q+{PO|y=gg?e7itY;LfE5%`O=Ww$`^YwDpuJLS= znX0#MUY+RPg(F%}wu5pDIUbQ?Hi)AZ}e^*bxM*OULy zP=-a~oC8V!kpY#LJZ!zx%1g)UqsB*cy`!&imUFGx_UvBHuB*qjALP(-b_#GtQ$Hp9 zX2u78X<&_<2kcA4{R^Qvm04otEsjX5u>p&=nZiDJmnypv2>U03hvwibKtuFJ8A6P!pG zZV*QE;|?9(iE@wufS!-&8yS!9;6r8mf;p$U1*0d2Yj>5tdYY3m(xjD51Buo5i8m!# zc7zE`oQ4m@@0x}+x>v)U0o%r0=HJ*Y{Tp!aO4bQexF6SORY;=yxP3ZdJ{<@ayPn%- z|E}MMvuIe(5x3fo1sPt5JfGO5xrTwQLl^V0-Fxk#$10WbOO7j$Hu6H!kpUUliBgo- zYYMwoS{_au?jp*?ZI-gWRB~-ayyvGapKqm{_H?&+OS}fLC$X$T_D8t=KK8y{)v&aT zozYs!$XIqIvus#43UR*8ip7hkMssG&n86B%Y;Tj#=g6oml_obv{Dkoni6j)v(sPrg zknh zMXuJQv;AqAa;6xOt8HcBDvq*@g5b8-u2e$tL zg`Kbo;=Iwh@c@>KJh2EpZNpU&YB<%_ z8Wlc$deknj4D8oU!7OvMnFS^$p)|wCr-}{pW+hO^5^a|aX5?RSfE0`n%W7gXFfG!= z46?hl%%HGniX0_rzV_p$8cN!|{wDcq1}o^YHwm}Luz?6_taqKKX$Skp$76hv*y!LA z`Qrt|M-bmB-k;+RzpQ&oLb^S4T%}rJ#AsU&<1HbItK>vRgU0@~&#ps~ArhY!%a7yj z0#1I%o@e2JQ+&)P96C7ihn3l+@q?p^f1jv5OROI6T=~R?Ong50%HbPLUwGB#-}{tg ze7i$EY=a5=M24?6QM)<1+q_1h;&`{j8LD06B@+p6SY5$rO4_eRUTHp6Rf>{bv0^O8 zRlNC0l%r`h$}7+^-V2u+iC(icu-Ps#wme?L%$jsVCsgv|le14BtH5}a`4+Vx-E>Tk~yJ(k*f)YE%OaIUEWE?PqcaEo0eNjn`QF40Y? zndwd(YJ1eOI@cz;iEpJxmYYWC4R=trIkY64GcFzoN{CTknjr=eHeg)r;w?CsxMplA zH9=C}3bzr{tT)s6PH?bbqKZOiGq)v8-+p*0;b-L;BnA&{O*)^IVW3=)jW4F{o zNXHtMZ!&o+ge885k0=tY%)*N{03{)gV6x23C`zo*tYdNK@dxKZ)+9FdiYEaR1Wru8 zmyPo9W^Y+SA=`07Sr_%uE&JU4$_rt63ClEb6o9R^Kk!1Ck{k3&$!lGmc=@g)jz$zr zcS#kiJzlpbFGXcmg);)&&5n$|xko&F-r!TD5LNj}@FUF`NUL1MykPKNYCC{+gfpeL z`#D|FDwPi5HZ@Vb5E9MLe+>8KWtzZ9spEuLW6>L?&Z5}L9#!deN4%rmo!n@&OYMVD zg(OwaK{ZuS(zz(+U@kvEC$EoM7<C5|<%yw-rilwo9Nvaf$iiiXahzd_X5LFG+PHi<*j3O7z=vsbhTe zN+7MBldml{nss_(tC{6u-$Hf1aQ)^Q=k0biw?@vOc+uI_+HTXotR+RjV};^WM#7vX zluE~o&!%S9WOE}0jM9I+d1VOJ%t&Wa(n%>yLbZCutvj^~jlqH~980q@6a8M$ofW*$ zp)bqgSnI?kO`V!zIdv}1l6S-m4mZX}9l2aE526+|_ZOAHbu8^&OYuL;8{7p&<&c&}H3PC8icl-Ni4m ze#lm|$KJt|5v#tfbNgr+=PY(v?NqNLg`2AgQuy~gRUEm>e1$(et>VkzFoe#d(n2ne#a`)Dw&aU* zxz!^>HuzsMbPl8KcIs(rlz3!|Sacmvi&Nz1cdZePL(H~x(b4h(ZMA3HdJJvr-~#K( z$cikqSU>HI5%ycLbAlNFVi7LX;~v}=R^OHtcjbzX#coo4`%v60j@@pwgIS9gc~HB& zwn^CSM3U{6`$cTI-B>)IEAZl}Xhp}OdC{rnV&3%WeZ0R+)aJaQTz+`^8)SrTwV6yI zGlEGB(A|*=Nj2=$pJ=@%)_d}xnTwJ=m|mWZq4Ci^kv=0(;M@{;fo%Didgq$P_x+FRqIrQ6Nyi+k_J zy7(<(x}+m9Bd=91nNyE9BFNObEQC!QVa{7Od{g7{?D)(Nr2Ha!hSH;b1BYgBR*1SK z<$5NISOy9G*c_AZ-zZxr-JT;1;QK{l`|-3P*_n561l?p;NB9-0MMv!iIcuQ zW>k~!;3(HHVn2Q#K&xyymE(=!?{uUbaZ))7MYUhi5i*AsdqiW<~)OkU5c;>^H$js{*wa_jM&<s=iMx zt105M(siX?&?M;(zeGZgDlprt+-_RKS+(BY;mgu>4Z@jjD&D@oqqso$S7IWvgJ6^2 z-{A|@7v7jfFkAAy9m)koOda;5-Z!sba{5XJL5Wu$P6-;mbo1WeQCUkCYb_m8Wrs(2 zGo#v#>M!*OYAC!=_e(v3=VXyqO-)nxJY&LJ4C3XY0o{62Py56cXVj`LY>50i2dib> z+9WXp{!!K-$RT?|vubLntYNZiFH7F>aZ+rk^kCKWfO*S@@``+9L155v$`VCdNLRf1 z^s=h0L=9<@(xNKpAo@Vrpo|`h2ft;QIcYG9RFJwYqT|FAnVd(fl^iXLH4>e&^%PRw zT&NwAkbcj6W%^2(NEgTi#5Mb_>L8Q8D8sLLtK(H4NJ5rfHyj`17C~`_TdfyOFFUgQ?Z+@XSHr!ElZP&P7L!FA}8DK;}s0LRK&EX0-DOi=X%~5Vb>&o1qfg}| z4SBng@iyzqtD?G3GHiKo&>SEj1JyuQMM<*1dG!42{gTp0%Td?uXE*9#>vFERfG3*6 zr_KJCpv{}lm_cSZzw))?x8_O&rJV&8#yYz765g(+iMO6*7}3B7`ULCNTAQMb3&E-NY%agBP$ z9b15uzN%Z2oQ8rHN-@bVmF!xQIi#Cvs>$#iakoNfJEO?tm}8U|IWHK_s7rU$W(F_8 z@gPJ;|2`J^qR||iW+b+*cc+=^mhpxUHZiwf;JiqG61xj>`|P8&au_rN96fzF{#v*5pDr(v z&f8bvh_hmLYNKL#aShhTz0gHNNm7TAJezU!P|{>t9LH%bGG8TJ=L*roN98Fw$y6I@ zUwLg$xr{d?YoJ%F?keM6!YzlI%MpWQtxG%zgYr@L_PfIe)@$!HH~Q1vw59HF{JJ|- zw!3F|qX4~YI+Z=(rqViw8c;gO`3h_dq85HaRO`n3u#HhEzvyZ$IhxNd`KPaRR#;v=K&55BI)|v4 zR~;qVFJB7tb8a3nLF23_msuW=K&_cyHVO4~dBmob$0kQjX5vJ8#UA3;y?U~)r&!gW zTS*_wfqZT*)^~wn9Xs_hoKFuH`Z?1}4v?^ZRq~W7EqBF&Qu!mNRm)XUW}9WD z87<|w6I)3Rt6VLdX-1)m(?MFRe(UktVTh(wPp0i;0L|;? ztYnx|2;^0Ek%}m5Tl7tIo^7mz>J*BTV3D##{tUZ742=jH1)%LPJ?#pEnygHTcN*r{ z%}#f9n_#&~YpCL1OQPo?(C2id&akT3m+4M-WSq{xmd?K7ZbA(0#*)J_i~E*($eC_k zQgqT?>Kz!C*KSXZnwy(TETx?Q)+mo^VSCD!2qXuv%Sj~WcokNTSpf@MED|k~FaAwS zv5OTaV_rb&G$hAb2+8b9B(qgXk6rd=*f3SyZg$w{hBj|Sv_Jy=@-7Wi4c{t5N_y2D z5HM0;Q9C!6&i zqr4-^LfFroj;*c-POQi!2Mowt2$T-JVR;?K>?kJ(p>&z#f%h_bc5LRe!*T$%-E&!n zf5P=8l$|)#*O#UIrH3R9r&;AGGb2VKjDkS$3OS)3Z7}vL$r;HGwrAEt3sFrf^&~K- z4cZU(P%B@eIYLUcz@nuK7cE`1ysc~bq9tw1RxF=-upBeQ3+U5wG+=Oq)PdE`p%u|6 zkr!Jyhs#noM3e2VxDy<#t!IJAHcC0mOODeDcZF)-Fb}YpWyd@xZ3mfAFi zfgT|V>~3k%Ih+rZ+t->$jtDsDjZiEt%6JRwxdC3UI94c-sW7DKp z7fu_Tgl7{xXU-d>1kE6Mb1Hm+56w1g`;Oq}ew-Jj7Owwb|9#=6tTu zoXfA8j%k>_XmB{6>tU%2%fvNNfm9@5{0HSY2CToiz~(sdmpK2r8DDgIVR|yRE z)B8g`y=Aj+?axB+SI?ZWSYNoo;Gt|Xsm1cvcK%ent&sy+6ijBF?9tE<43!(lHa3oH zw7>UmpMOkS6ir}hr~SCkv}op>XqtQz9AenmDE2v;KMx;P-u1zXcz3+Dy>)baD?ZD< zv2hwo$h<;G-5U3hP6FDTnh3U_$df7KJjx~&;~dTRl|a=LXXYdkrXS2GrLeQTym!u& zW`=Jv%HdTX*j_2$xnGDa?}H>Vwu)7y`f~3k)7R9JWHx7pVvMg`u|7diUQafU{t{ne ziEdMhJJD_2p&RwEzf3;v#rU4COXp6qXWSFgFL7GmiRr>>RXR12xeheN^X5Cdi|3>* z&_Qut9F-|=Sc>BQn--L0#uGT@@$IKQH~gOcya!hL6$FHB{>Ax*o?+%*RT zkYuG_k}{~#{Eqoc+LpI;%Sxoxz@9A20z-1WUkDuncs7PS6Ebf^N_Q z20#uBfwzLUf#;rl z7(Zs<*qz50z#$X#8}t2~@#nQ37p0aRw?pa#@J5j4+NIsyq;i+3eJAw&{w+_E=j+bA zH`RLFY!c7*iBqKh+2Yml^+6N*t~ul}uZxvf{u(_z+$ zi!IxpP02c}T0<%|_MkE8gT}1t5%x$0*MB+qKFChuR|-v>><;o-bkYBX zt@>klk+}1MTqLTq*%yf>Y)V&!o#Q_5I5E|D&yJ}v%?Iw1+HtS=v(jI5^d;c!;1l3F za6PyI+zM_3w}bn@kHP)mXW-}H7eILBVem`v|AD`Pzk$Dl26$&Y*crSE90Xnsp8oZN zzy8wGpSt?8tIxaa>dT%Umg`gWdeLR6RJ6nR^TwWj=^W8PV0i@#V-Hn|)!BE3$`{YAOy&A0wzJ8h&QeE5vC2GJ7cEnpz^D5Q9li~|1AYj8 z1nvc*_kRrT2Y&$1fIosific*D9l%(yAJ`up01g7L1_uMN8BJg^XaftuG2mG6hYb&G zxck<-ulvY#7v4Smz=j99{q>@|Q&zhF@37HR_e}FlS)EZIb^kFcp4zt5Ew5*))7)_V zxu#X+`ek3C$En~n@CL9JtOM)8+29;-E)e^FKDYpU6kG+a1|I`=f-itCf(O8Z;HThW z@JsM3Ail!}@Eh8xK78z%AF@a>YBZDBL2~EB$q;+*kU>v~qA>W!W|w zbf5-iA8Gg$tdc8=m zo&8_=+FI0n{D>EVnubn&rtED-r~352;qs@99Mor__7!@(6}$~x1Fi+10M~)9fxE!n z;9>Af@GI~rcnmxaWE}86;0Z8}vA~XCC(r;Qa3GijUJH%{M}d0$zY~wDMgN~4IU4#` zwylPurqCK3b(lr;+O7nSF&GtzZFY12U#N1{@1|KnC=JRp6cAUEtl| z2JlI6Blrxs1$-9Bc<}S!R&XEqF}NT61UvzL2c87uWW2ap{@)M&Yc%vXRBkuVl%;Zg z9>V>GFSiQCD)VSvwA>U6vA@t^N3avv73>BibSPu`J;0t|DwqbE!E~?;w1X4CYS0e` zKmiPc5g_q^wO}2%9K0V~0X_ig@&E3cKYHpeFwblTb-(qx(UdwmdE8v;Zo2<>oM~0L ze%V*(@k8(9o}TLWeUz9$W>k1|I`o0$&DS0p9@M1or@m?R^`32Rsgb3;qW@0d^$5w-aap z2ZBl9RbVoh0uBKZ7yNA<{@>KL%G5U0ym`S3K}|!anWpS*MyLApzv-7-jhxhHq4pJe zOasl}IB+~z43>b`gOk9?pa6!!2sjJ88N3BZjQ1Si;=AwTbFqKd_okDmQ?;L_iG7zJ zvQcJ0=F0TWzD3hJ`}+9`nX!?Sey*cK$1_xtbJlVMr% zN4_72Ki?x_l-fz;)zSYNKHmUl?8%rP%ms&nHgFtR0!{>jUa)&GWQ#(HRvT?9sIgEeswA8rE zpa1^2mAia$!Zin`b~j-x2-}+i+}~H0ezkAuSNoNIHT%Jx zQe$=`Q~j{Z*#1;niWsWDHS33k;ZkFT=6?rMsN5l7DwqbE!5^fd?k90UpgsOc+^KwF zAGl4q;+Pi`R~T0K*P^fXrgCffPnN!W)R57ggwE4}==>Hi6U+j$L4uZ>Lh?igA^n+w zd=Y7d|77|LokLr{fNQb$V!zw=PNg7J{DUr!e-an-KK!1Bk$ZU=ny0I0(ENOb17Tqd*(D7Tf}E1NVZTgGaz`!SBE>@ZP@QAg};D z{hOcv=DzRT^&P=ocY^zVbKiCH-CZB%-{qHHexCeWKe*m;6@T%VKEG27{*(Onz7`%+ z;+j+kuUt(k`5mT8_N2KkT~d>!Z$-{81Rns`gB!rz;OpQU;G5tc@E!0| z@N2LE^zD;MWx?s7AFKfvfNQ`fz~{kj;EUjoV2=s#1h@#i16%>F0(XHQgI|FS;LqTB zz`~`}9$-&!HQ4W^tTg}!fGJ?(GfzD8@DtyC_=$&~x%>9JZ@%v4>ppzl zeDkF@UwYo9H(z)8&FgP>j6r|?v-6p_|Dfd3$vZ$v`R#j^RK?eapYuq3pQnUBSU-B? z^YOPlRZQE&eg9N3ZzZk`<^JXZ z)iA-Y=-lH~SJ-j<=ik(txbyb0PbaR&WH;E9!eps&J78_~lMG~DCUP_#w15uK4RYYU z;8);x;O}5}cwiQA{P0>n9}RL~2)qwG1$KQob4XwU*cVI&D?v9{56%QmRGVB^ypAK~9G`1i=hAAi^VyZgJhefOH%KKQQNzI*q@cVB$g z#kq?U9q6S*WI2$M`ec4Wa=o1_MQ1L7XRluK1QMCh!bWD-9d9-NM_zUpDT*||uZ`Rd zYJ<$){`vzGGAb!TnH_iL6N&4y3a^c?snPde_F2l4-~OqJx+Y9DS^D;W!PDUP;1A%B z;7{Oi}E1HE7s=mS}BIuN~(2L*5mcsqCpxD>n#+y+E%SR0P@zUkVV-gn_m zZ$2Y?M)r-_Gft$}l0Ve_xHsHLi-dW7i|D7sb$R;iVb(pGwDhv4SK?XnO7tNy-uz+yFiaeg+-}{|lUr`2(MK!oD;B zvH2TH|NZ3qU--$**Zk!B&wTKicU}Dbi#I-V=7uwqEEQb&9urj=^aeejpah&F*QMW@ zzC8WE-WOB5JTH3QOHj;EW``wKR|q$h=nCqR;1XYF`8HW+X@%!Ho)d!*UMK3w{m}j& zB7Hg@>4Ne%+tIvtZ{4egV6J-N5ePrQl^?3OF3h z1IK}(S21@8&Iadzwu4fsMPM;l0y@A+U;yO7yTN9R*YvhrPt@P86Gd|;d}L401Q*nx zr=+FtJv|f8LZ?e%%?W+-t&!|;d4-PsQIR`g^PlG$aouKVC=znlA_!1b`#P|)Y z04IXKrh=d6g3qaEHuvI*FoP|7k*MgSVL88fAuhr)~8>m_`JN5Jn|PkwXi0dc*^>CiD_6W7HYd%~i>C+?p-DaqjpIUv7c zjtx0J65ZSba^Oth=y%*?e0gvxI1TJDg>`)31h5hef`m@5^I~JHw;+7Qi$ugHk?i;3 zGnFrX7A8wvgg5>LKNFi{SFfLVPs{dRnaVA<)?=Kk^AN@~_CW+d=UDW6VT07}B7e_< zKZEB$Gqi32^S}|n@xdv47C!hqxCcB8eg&QdW8j7HU{CN8Fb&KFhl1mQ!M@9i=MER9#?Bs7bVR9q&ya2Dpf7+x41!ET+94{Cd?=pKO>sD;O z`^F#li$7La5Yjt)%+P~nE9cgKhzykxc2@FuVloDNol z4}*_@Pl6l4Pr=W@@4+9yF;f{|faAdN;7o8acn5elcn|mhcy7b@Hr)2yjaS_Ejw{|F zzb($aks~ADkXeygAs$}i@e5*U<@&5iffvQ~p6UL?{f6B?8eb#xCZP098tZ*N#xy(FKqrXKQjl02QxJ5Xd^=qxl&L$48GVE41q zeUQJ2i}1z^|G7TxaEWM#Uu+BQuo(+Vwx;|mp;S#V7h~6d3C%^GMV9~jKY;#cwr>OL z@GnGvp}FX9(cM4$51{`mPS{R=aaElB3(dt|i>*EuJN)mi|G%)E{_03N`U}lN|E!Da zH-Qg=4}o8TU8fcC#ZYMSSBDpg;X^vSQ~6?fm@IJ--q?+`Ko@>^R6erNcI>r4npTyxxsr^5tW4rtZ^1rkA-a_Y(f~&yC!9(C-AhdrB zJPw`!JHrRN0O5z-!5&~wuopNS%mb}p0ayqw1DAs!YERn~f_8CiQ^4ot)s^aT6uQ)rt@9QY3s@9R55@Qdp6XP16 zlYCn>Ct52xRZTe)v@F(ziZ_zX~LYP zHsSRj-_#vnqi0NfBJnNqD!MLwN{Aa?OV5PY30=wa#oA4brc1xS&|v{M9xMh+z*693 z{{%kwfh;&3tOor+biyzg0q+2pf_H*!w( zomfduQu_%lM5liy(Bog^^OwOlz{B8|;4!dxCi4m4bZ|NNAoy=^J-8Ko4SW|o0DcPo z2(FpMXYgkrryYuoxDdP_d<<-S^k?^e`?jm)|E0Hm``&L~{q1|#-pk+IDY*{dI6;)4 zrxcuU;PBnFQGVBwK}i!n`TOoy@TB~%`Xprt_Zc3nmD1!X>8qy1KG#fB%=KJL?!E=k zSNdY2{`TKeTT=(Q*LvJuMf0G0#m|%9HJB0i%h;z@4#Pyv){YV!ES&(!OOsY zpc||P4}j-^oQJr}Vb}}s9q?VS3r_iaz{&yW9Se4mo*=g-IXIx&8doSuEfXA;*TwZs=S z)K6$ezBT0~q=dW@yzf(m-%b)g&C&Ql=(!4d{tgH|g_c6Y{{fBA=LnDn?*Q|Tz{dmE zzLvcn;M3q{uq%^_r-AdqXTV=WH|M!+bdhlT>byueo|P|t9X8*;V@^MjXpaleNWBrP>Jg?jsU&^)e0(P!C*OVM&snl3 za(6X&2s{h^0fe?~U=ip5rvjn1(D^1y=g;u>bKo1`JK(S*c?O&SGT=<`KJX*33w$7a zaS)gbhQPVt*Wg+3)T0mG|Iqio&cBDg$aw(#yF&lH>vIoX{LmQ>omSK_dWnolBaP(N zrw1f`)s#rgtu0?oNZ&gbo)ceF=9+yDqfGf-DUPxZcSW&YtI`K*Jm9$ zKXLv0&g7~OX-$}4LSs==xvs4xBazXsgZ~4s{4acTumb!My!~~^I=B{m0=yaaI%ENK z1`EN{ZOs2I#0LQ10N(^h9>e$mbb|j&1wVfXJ|}k;C+;Mkc`|$^aq)O~XG3_ANLKmc zq41dp!;AYWUL=xzKTP(m`Y!GclLeo3PtN>Te#uJj1N|som2vY)R&fYp8rc>`Fn8lm z!VIB)kSF9j)d6G7YCwN&R|o1d6Gi@>11aeJQt)zcH0T7v2mN3eTnlah&j8_#OI{cA z$EAG!Bp5fJaRN98ya&7&+yWLK&AJcp)T2Lq>d~hjz2_GG-Ej34@4DyFi_gxUoj&_y zc*)C_NL(%1N=Rj~)RNNyuYL!-FTZQ$Bxyn^Dwe{8)M7!6{LdX|OFUJ4J+jwB@qOwe zgu;*GcdhzJ8ebpZCqk-PPfD8bNhANYo_vMSB?H!jFN6ERj>!30K;(UjmG$w+`YXYH z;41JGaI$|Ja(}0l{~z%8A@CRQPp~I?U>0~SaJr#`&!QhTKDzOT8}IqbZJ)mFgBRX* z&uyQ6^YEMdd-_k~-}2%9&2QfmfFJH7%;{GA6y@_iPuCT`p z*NC81?Q5taX^gF?nvyys=*e}leMwM_`w33+oLo~TJg3x5Ng1K^>@Rd!4t@@P0UkV- z@f!Fo_&fM(K!a4ml}g;HeDTvTgTzI6WBcd&dSJpRR{xca`iW2&OCbY|RD+}c8=-wa z_!)4#@M}K*9{d61;Dxo|9B?7H2wV!@3qB041)l)df$PC7;LG4E;BIg~cmO#5+W5$~ z9=YWcAGzXPS6pz#Sy%L4@kagEd%+dGZ|po?MqHnG!}YOrqIsp_6Z^zSdYL8FXJ^le zuaN?iN`BXp%!8-?Fn-eKFHeT~|BbyV;dSx-KkgQkPcCvjQGffVczeD?6e5R01H4H zSO|^_z?IA_$2rg zxCwk3d<8uB0^nj%D z^x^t}*X|l$!$oGrsPI;`u9VSuoYLp#za88A`8ZeqZQIKS%6xm#5g#q{ZTH|I@wdKS z!nyU;>!O#uIey;PAmsdmMaK)xh2F=2-cSz_=!}077xCBI=F?s}YimxmTh$|~tK53$ zD`kaO1D-?iiDPxG|GZ)6-8gV;!Y(B(+DSIWCx5ou`p+?hN&SIg<0x%0(u z{12di+v~T>e<=S#^Kjg?`|->uf%Ibb1%NX^!X^xb?%wL~;*H@&;!foYMjk~!6E-)( zA%i;;;fOzJtm6{#{~q5?|4{zN%Q*Xc;QQdG;1^&w=O1^hSo z2>2@aHh2s?4kCEwKrjh(fj0riPjBP%<3GCd6L7zWVkFPE5&$if=;jkoCu!U@YIF}zAL{r$nU#9_1;gN)pu6MrwAO{()|0n^Cixc{{|n7U&PB#Q^dCU~N4L{I^#8^R z&Ho<|oxU4-`rlptFF0kp{DAv-DAA}va!M3(qlDqF(O-f~m@ezsdwN~{0pnYlCb zzQqC(oVn-wKF|BS%go7Lm;NAhgD0Rn41#em9^`qh@(iE@I}aTF zeEH`mj=ndQ*El$FpihfFR^1BuKhfFuE$$&Ul;1>sQATHJM}-TK>ODVM!$m zcepc4tVMJ5LS?N_*dvJxwe>(8Bi9h=80|VaN3C+ob(2@GqomlkXm>NzgDf%s8!Eyx z=6~13!ouUFujm)e#b%qpKcM;gIE??FWi`3y|DE|?(fnT!Km9nwyZ)t{dityJpJ=YO z&9lhQhJN*UCK!wYtxj}Po8nGtMhp40J)tuhi~oW~ebW4y|hy;!P|NnXVtNp(u ziNU79Ot7@S2F?4X>Hix4zX@C6zi=bVz;1$@p&GP=f~=`21P6DnTEsUyOc}~+UcdG= z}Bj2)$F8a7G?Wk~}5|NO$7SE38BF0$3BSOyiSErgzgh*bC z#5l^FGtjo$pNVXZ@R=~S>7tZ9w&|j*FTdFLY!IE_f;DjVjsLgAw`6*&9WOCmR6S_X zdo754SHhMxUmu6@U&-b%s7YC;|L+&gRi7??yfB>CkeCpPf>tNa`u1O(y4n6aDRG77 z_|HmDf35wu^sa#BSHU%KE!+xK;eMzFHK7hX4C7!t9D|=B7j?v{FV~T-1UEu0s10?Y z2|NmYp+7ti!(kb$gq*C)$_4p=JK}xU!tvA}zDnKkLn^P$J5qP7+p#ot{*?J+hRmPx zILsf?#`tYU7cAOk|1awv zW=((d7U}fb-I8QY|CdQ>7;2;wrxp&}c22hcqPN(m*k(WIrG6?|v;7ZMlZ^gfiIy>_ zNm;1<7tO^Vi!V;X=f=DKD?I(x_TMj>OS>y=?uv6j|EBn~boC^oX-Tq>{Y#ux1^U4t z7zWGXW7rHw;9n@gy4liD9wx%eFbzJ2Rqz@74}OAQ!Ox1lT&%Uo4Ta!SSP%b!LvShc z@RtD#!dC5AB&);b&X`LwWyFBTcr_m(|8@{urv2EF*sj`#oS4Vnszpk6)b~yQ9xmfE zDQQPq_VMoHJ1y$8uS$#O?5%42CC|MuW674KK6QDmQqsOw`>Z2)BBQ<4KC5gE7x$JF zn-{G=hn46S+W#}*8~dO9hP7JveQqsPG3Xjmy;Rut{~yo(HUBS~TlRY%b{SfKEf|b_ zO$hgnQRqL7B`co)cjo^@bIV76b`I!Yxs9j4I{zn{TkZ0Lw8!yo|6h6fYyMwiu<}q7 zYQZDW4OYPc_yaCrMa?a6D|Ch~&=n@aEASf3gN3jjHo`7A1joS7nhRO|zi7j}mxjI6 zXI{5?ZR@nH!mAE1+ppV^O#V(QftTFOJG&i|Wi-;$VqN=F)8b0zi2MD{(ta45SzDb-LiA7PH6K#p)=ab;LydiJ+>=|J`r>!h?t)_6J(^!@jn(g zdivY?Cy5{L2Xg17`17f-0kk;)EmKETN4p15%c!M-r}e14V4R?{C|S99Htn|7EL#Ewh)TwgJ4Tq4n&8MkTCAY0A2 zP20*@SKr#2n9NP|CS_cdBo>T#jl8cJ3r2ijH<2arqV`vj*G3BMNKY$eZw(%6M!K>B zV)K`S=)W7{-TsSp@a$i;f6?5s`JC9U*u7=nmThZwLW|f!FYHavV&JG7k)TnZRCzP% zb5deR=K7yyp8l%;PZHlf0p0@f;o`r404?OxGG`+GYtk_WR(npyf1>yIX__DB|MM~2 z>!1y^g?7*#hQbRl9hSfvSPSc52c*I**4E92C9o8h!CLqh4ng4-eE$p7f?mLLT{rAl zx#Q4W<27r_kd8x|O?hZaE%U{=Q^5{*=6T5(%OS6=c(S7bZT79AwQ^jxM!#}A?Wk~r zCR~U1j^wpA*^Y7|wZ6`n)!>`vIe(|!rr+Nv*hpB!xWqnQyDEFD9B++}T`abK0oqww z_aMC*j`)&-9?$I*pj$r91V)i1B#lZTrPRy=XO#}iywXxwt}Vk zAEf_*BCM4v1GhmHkhN2v!JqI5!vT*%8yF0e;dNL8U%(DXfmFDT!N5w;7@EO2mP(IrZMz|Aut=>h6S(^K4;AKKd=WbVkD?ITm~nO{%~UNy0q6)WnX|HVj|V_O4RW@~)154{W#0H6L2*W?z+5+pt$iy_^0z~tFp)lHJPPpd_{LwvBsI?u&_+Mk3My8M2W?KB0Ew<0} zPL5*yFNf%D+4-%cHD8bIIzL***?cyn+|?w*{}<{UgPN3qSRym)iXqiWeuFjsCz^|Y zuL{c{-t}MX>96T8noGOB9-3g{@vi^F3|ZpO zc>Go9L#!R|`cLla*}vNUi{@6pYcu_;c-Q}?$36X3{Y7)>UrXP*E`92F*MFC%zvlmC z9Hl&519hMo^n|sr4vxS-kdJi}w?k!k9J;|1Fd3%9-OQ^!2rZyJJPqSvE>!NwSbxfK z?(SK#;FaO8^k2|pL5KQjuWAAPM|*5z$K|g(<0$snh|IUU8yk_5>Nt!!HWIkiTuq>D zWv)KryKp^H#zjfSTePzWjK4S|Oz9l;u|JteD?OdDD0QW15wZDt&>Yr4yxV_o=C8f@ z-|7F0=9aA=CN1`E*|}xoTAeuS=l{jr%<+FGCB|jW{}+0~(_d--lf;J)gcm`4`3$%V zpRJ7nYne=%|9_y+13_n~jPc*a=~XS7RePB>{+pR=`pQZBX^iQWv&Qt2qZt295Ur){ zt_4#;vvtMD*FkG&18rd>jDt^LCmcWY z&3`tnT{vywwC8(F>#(-z+Ip4iRn~lk9l6R126r)i;3^q2rVrd|#`M}&-pgDe9Pzy< zDT@UPc5qYD>;ptBNsl688TtiM#_q9@4?btC^(O4z(lB2i=U98I)`zmUs;h#M#pW%| z50Dn!EqyI*HJ@Y?9Vn?XdU%H0&7zuYE4Ye~IF&e}sIju(%iQgDOxL+CV4h2QR{_FdN>7EijL{ z)OVp9b1FSy0!)NSunbnhJ~#mX!g-9nKMOBF0mk8q!);I>j&sjQ+H3RTsf))Ae|Gqj z!#h3Xyr?jCiy`LhuEhxSp=OJrZN<~}KL0Yudj|2mW{aX7Rnb~yDqEvpWtw(WxIZGW zMX>$1)=t=;iEIsbI%oVxT~%6I?A+449-7XCBdVopVZXLV%czIMJUv`i_FY6Xm9CMU z_>An`dDk4Gx7hV85ZktF+Op%vBQ%uWj|DBW(2^h8HeXo{i?Z4=|6vSVz zg1wNQram1uuh-D;Zmkx2{HOkt>9p-`NvlFU|Fce!z)Q}&_P@dk1t4W6a=j$DVnv{|8pG>SPhDZV9KcczBX1R&U212~+|DLD6>i_+s zxy0n7;3DGic-Oz~Q=a{+tfl1tOI_c2BNr#j% z1sMYf+kduu+S08EThi{SPpbTaGp=ACFI(Lm|9R)*L5_ESFOpoTJ)&%1m3oKL^#Frww2kp?~Bd~?{V?Xa!f(vcUa%dsH}E& z+pDK<{hc$W9kErg-^qRZ6Wsr+tx`#`c}wH+=!+!IkJjbaGESMZA(h{-Y3nKBp+=I7 z{{L3b{MC zr5vZ{_tMyBnm^NE>?vw7?q~Jww6!`FtyMU(HTqRJX-9?Y@C8{N6ukc@&tDC9B4_ufK%Yx~2ECXiPP5ezY#1mT}6Q4Jmh?#PI*~`=`_Y zN8?E{GXI;2zQT|yVE$J$x9qzM_8af|*B;>MueASu(Oi6bAJ}{j=>L zf1oVAx4WSh)P{$l1+<3lFb-zJC-6C>!f_}|tab&|ga*(9`oKUK1LN@J6JRFHhPUAd zNQD#dxBP$9di`*eCxR}RvtSOt_s&Us$umMFZ9OY=&fW#GVTx0$gQVz-n9uK2pQPZ# z_Z!Q6=*+s?@wsdb8=u=HrP_uqX}`aeROk22IWcW(kp7E$eK(1H&jrzR0c?QG>F%HM zLF!R^s9BqrWNrVJ^jdAVq!S_5|2|(N@PaaL{coGIw*Q|)^tSByC~3{tYh6Aq6ZqMk z*}S`&WVHXI2Bp*gM^lqB(*MiEx?+fQlHXuG|5G#<|NSpKiLZ`#{jVDA>96W9noGMq z5L(gZ#=HKTJ^eNN7tJLO>j5>0sp4J#E<-%~SM6Ukx8my=#LMxn|M^2b{Z;)%bLl@x z-|3Qp@vr|pPk+t-OCQ;X?xo=xr~;E<1$+(%;7WR5<)JAwgXS<4UVx9`Q%Hql(3SZ5 zY3Kvb!4!BE-iK}Q74ZF#JPLC5xY@&&^jXra39mZ5A^EpEc5|)Z$|Em%_C$5#rtd zKRC?u|Em8N&BfkdgykT1Z`r$L>sp=AEHrfHY*_yzhUUw`^Jv=`DWm_N>glig|0MC{ zU111_Pag#4@jT1MS*=F{$Kl41Wfo{vFI%EsEP(f5A?${);UfChm%?q(7G}abuo6Cj-S9hb zZ}$GZpS}Ot`_qTN-+yn9z4BE6)zZEY!0zviW~-V)K^nl}L;Bmd2L8norO|er^4SmT9I&1=^m_8TAlX>7c9rjc0x3kf`5D zIq`q_5uW`k{l6r!-~WSMAa?u>h#lV!TG+2;T=oINcL?;TEtJ%ov|DRLhF;)KpXn%!L4_+s|6n4S4@CW3hUR(k7p#fNRr7!7$ zFcoINOn41;!#+5kx_{Gqn_i#(qP#}GIDPQ+r&>Q%tMya&K4nKN+KM`R6njkH%Hc=( z?UE5!+1c-;u7Z=;$czuQt&G)c*F#>e#GXYUua)O4=KO)SwW4*8l5CCs9;LLS!cB4% zDA=J&kuGxXJFd*8UR_`eY|a#@TZ@W~w}n0+`aKJ;fY#h=8JBH;WQ*24RM_=@>(gts z*X7zw`#&w$DOZy?wQ%6J%p3o^<*Z%*mmpe;y^5{w19l!dC$03L&-upj;hmlDC3 zfwJ&098O`S0IyZ@pP5X{uzr9IOJFN1GE+G48>ZbTd0 z7&M>IO>J*OXU>N8Khn^e>wk^2u zt#BLEfYvYwHo-x-hQ3ub=m-5_0K5RBU^=Xa4e%9w13$t~kU*P!5imZqdCC5Hv-WS^ zKX3o2LB?xVm;H^F?4QM}(UMvFmFG3Tc=^VNF_{@JnI-f4YFE$Us|(GbW%c|{L}EM4#sMM~pu0>(?U7z(|u)<`|b+>JqW<4$vP&f6;z7 z98}F*%hXf%+!->X?g*XMd+vXB+z{+LU?z*<-ajs#j{EpovIMNqZ+@Sz=*}cQ45JTT z%t;n^k|msE#hr=1YzZ7>B+m19_O+B#w5yn{+s*>kS<1<0Yn(5A>%TPyToAH4pPxQFVe|76}6;NhaDs{({ozmW6g&+@UhER(H$ z8*w9@KgpY0|5JVP$>tEf#he2F)GUw#9t>u%AQf67^&B6`5RSYP5&z0Fj74OzT+TtG7awYkA2N{ub^$yJ6$<>`Y zf2T%j&KkIgk)!>2X~)}J)hM-)*!n$iFFd8%X-p_$>|Ce^}d23l@@dO{Gz$)<1L?V`SYguaIH>sR|W5)W?HBjZBOV-b@doKo$Lg$ zB2c&>HuxGKai`|UkCPH-Xy{*-?!;NX1UzQ?CyBm2K-%r6VFGB~{W`WPB(w#cT1GA9 zGOb%|*!3T)(<@mtv-X+S_shgN{S5a1bBNwnthSAGFIBu&Av5ZZGoN->lZ^4-fiI@h z_@k*w8FBuZSXT_80{p*dz8zBFKE}r5UH?)HCZ@~z$Ep6(NBahjLlyL|3irdq@C0;+ z7hp7ufq&p%I0!Bi4f*$ZDyahZ2e)s0Z zQx^|g+-G;U-D%H;-@UnM+NOrPee~?clw0Za|H|6Ercm86>4?$mF-N_%{vTVjZm&@L zOETDtE-f~%>Tc<5X?y|NX+EKw>h85ndZzpyRL>TWPCm_1pyB^ldG>G27o$d7z@9i1NwjL z>96{Kzi6(;YgTMFmzeBrkl8uSCumK+mdO(Be={EKo#`z}*0KL2iM=hYx02og$KV8% zW|aIAxD;-H`XDoGU7;I10V(h;?1v-(iE?B6e% zw}tMo^BmCsk?EfPs{W$6+CQ`UV^&}64fi_+s zxizl(BI(7j1ipjs;c{kPuK>+2Xw7~n)nl|%GuocUA@^v-w2XR)lV)k>g+tb5&o|_@ zf12@s3;u84X4u33vf?v43%|{jECF{?#wyU^viSFrN4-sJd?Wk~nF6*Jv&K<54f2sO66cpRO8EQdoco_PE76WRT zI;w2i8elD>mI|KEZ&;$a2A|tMi@q7`hBqJA(y7hOv(jm`Z{?Dwb|;^A$`vFQDjc{i z^Wwiue5QW}*Z(Gn{-XOsAli3;$Dlhr1O1>s425Cv9880^U^%RW|G@#s&fK~masVs% zc5K+O^7!JnUYUUP_i5Ll-JLhdoYa)d);o5Eg_`T|WNWx;>*HIP+}OKI&%kxGy77AF zI>>82hJ2tu$X{-QQ@&_&ho0qg;#qdwo;&cqEqmXDoM&>2<*MGsyz~E$ zMh^zr+W-3H&Q^PtP|gtc6U2v^XH~BtS!~_X*wXi7wu!D^fM|OVL|2zFfYyYE&NNf) z&t*nEgmlnIK!h{z{cxWDDu1%iX#anUg(t~K{GW-w!ua&`_21`N_If?~-T)h!Y25_- z;28X=e&nfY=777*Hno_RQFnOhaGyYO6f0qkCm)N82yPw59;1x7TwhM{)d`RUxu6&n zhik#A6Q7da20P(K>crhp1vY?HUw$V22VBhjYDqY<^T@I}JLgOp->;MWYcamzT@9IP zKIO;uUvgpY{Fmnc?3hv8N^24Ee7v*ST0|^~*VfK%M}yi{I~tU&k)uH=Wu7QD1-J^XhRSd^RDt_J9_74w z<>tkk7th~3<$3efga4X=8$V%{!M>e(AITGZe^ov#M1F@YdD{I-;JAWZf$iJuv4S)! z5BUBD*=m=yz2mYj7X{g>m9?X4*&6QeEiSgMYQBd2A89)0MJv%!G^_>>KTMPH@-a2ejfcuBv^qe1_(@s%#Bs|M|tHMR!YEOV3x)b20pB zNhtq&<3H-B5xaEM<3A$1tKLyg(O7JE6?_d@zyDwF^%!{gc!?GLqWwzv2EK*;@B{o8 z{)T_xJXT*7gA#Bt+z8d+yqP{--Win9Nvo!4D^ktv>E`&5Fr ztoCOe$^RD+n-}dFYw9uJMeg1>t1^BV~;LrcR?&vix;0BRkR>_H-q-D5HwrQn)m;O8c8zjzw4Wx z{VV-HzqHqu{_m6i02aZguow2h0XPgt;1Bo<{)Y2dOV@IxCet-sw`lh?*a>>gd<)_Oforj4C)Yz`jmltZ4dYqrQzHbSq;Zin5K zHJ{w;qAdMEDHY|YE?1)+6|RHEC!~(o2J7jWt2!dJ^v+rnDQPK?Uu<60-O@GzJrhCm z2Ut?X!~3LSdNN86(w5>47@(d7dS-or8W7-ftZ?tu3Hg10>TSL{Eh*sY~?1=80* z185A7z-)LQcEVn`klEFIP#h}2Rd6>{ffV=__QMfKg+Jg=NO+B7As-Zml3+f!VadGx z^ES`hKWFUFIb-Lf&UxnXXC80!%;QZTI<-pVN$$|1Xi==~x#G_tesYKU)!wsmb{yQG zT_wAAJ9WNTtpP!{?z(rowpB_M?4Xq;l6JO+GgPrm?MVBIG>>OT=2A_=p3g4nroUwR z06C{+*>XKvp6fA3+tjTWi;X8kc@X`th5DeyfLf-GDoo{&8FfeSbQJO5yRop^b20u) z5Us`D#n$J5=I>jn!f6?G$C*#Nt4YIUC2{Aup8r?;zh5+e9mL1y#ZSk({(0W^^jGy4 z&80oJ+HGmGrM=Ee`>fRkE%wtgkEjkHbVemjAdGw1)_@$^^k|4I~{t(dG7 z>1wJpmDJ27YDQ1%{#QQiAYWZ%ej_Q@8JjdFZ>3-Lr&)7lOPuqgW_-%Tn*Pi zMYt30f_hLN8bCv63+>=3cp9F85ik;_!E#suTVWfdz_+j;PQV|K%xux|@3$QPEb_0l zMj7+>XUFH;$7~Vdga6nuy!2Q?MEmAwMqjPDyNWzDXHdMBqZBqgj~!d;@h zrTZ$k<4K*`Ts`lM`GAa!-^b9hjStpvNNOt0 zvh`0ol($W=?E6tP7yB05ZmKG#X|Ktx?NJX2lh&^)V=aYIOaANC5`WNd2(|scS~p0i z)t9tdlEd=l1JPZ4x%ly{r2q7HJui_F{jGL;4!TRbEp2vI(*L3Pp8grp--^-Jqr1dt z5}#!y{V!PH>7Nn(t$6%LbeDKsV)3k`|0kaQ8EXHnzE^&9m%f+uyRwr0{onKKKO^>U z^|LFXyY#W8f1Q=|zj2|be@67T##5T0yNspCI7(L1|3^>%4EcX+-0WF&SI5oLQwLlO z2%Pr*|LgC2_MZX!Pq4;FH=@6ck;?dJdbAHS=A-U9?fN(Sz|%hi`sc95!w;joH6DHw zW8&$l182hi3oi2X&w&03GXHTSh~_s#9jFUkpeu}n@h}(OhC^@|jzA)-vU5TiC<_lj zb!Y{xVE_z-+3*^C3@hPF_zJSI7B@TaX%jDy2FH?G}yc;{i`w~{nB zMEt$VI9T<3A=1qI)UCfOX+?%CyiBhLY!hVlQ;J^Rm4{HNOZRJ0cx*XHr6t14^rKjHL04ayEi;psB58G^e2L(N1903PbHWgkGR6kQv43lfYyjHJ7@3TZZ3P)4%WxeywL#E5 zd#LzNFBR7RS6%Mup8@?7t+CLAx#k$CjDOaG#~~}||F5TiM*M%G=sX@Kz#Py*_UdX7 zXFtqO-RL%5kZ%|MB*B;Mp4{0P)3d(wMebS-HcD(nb|0?rEIR|1?aqh#Wv;{0|7Fsz zfz*R}@GfkDFW@j7fmApSzda) zp1-Qy&u`z0BTo$oH>cu$_Qkm;M|%>1{bU0fwevAUBzxt5BX93Dw%Sh_2tHG^wOoO) z*IG=p7M(+Btoeh`nd+(|sG?@>3Yjr?U7mDG3RDhW&T_*t7rBX8)lgt(;=V zmdy@Lv(XRue-RvmBJZ#t2Ewy22sT4e?z?FZAA+BIFzUe!_z`YnP5yA$2jy9xTLJ!5 ztAUnDRrh3KMtf5&lcHY1S(e$SUYnCPY{huKO`c%ihFJOHD?&USN8F)lX@<=e5uf}% zh~~>+KOBIg@E80GInZ0`!NpJp>On)0deIVEK}UET`ojQF>&r0mZ@^ag1%BM~=|bl< z^T&}t?iu;x$iaQuR&Q(gk&4M3K4FcCh5gP))BS~h3IoIyMSs7>uQQrYb`SJVpox7zKX zrmOwT8*OV4^|fbgQU7kZwGZ`Aen(UPw4=gZE>C%uD~j;cvdp&w-^JpaEp2*}9tD<0 znv>Kr-#ea&+q%_E4K;JKnn_FRacTZmcLePLyd-x3ms=Tq1aPe{`^1ypw?6t>mdVz> zO{o5r(RxS)^d*Wd$~`nWkl22+pF2Ct zVd>t3`)GPXUziLt;A8jWLAVs21A&f;>GJE!J{wvHreBQ3pUE%kpOCo6CBLM!MF<6%0?g4ysb zd=Eds0XPUbS<_b-ia>d&02QGUn9q4#y?^!od5}6~OwVRDc-Ee_Imvisw- zwx1?6iqt=%$M3qwmA*b5S7$zXI_>JrXEkwkl8?}Pv)f=P8?H0xi~^^-%ND`p)=+@Z(3O`V{A)~HrA0wGx#?F7<-&3$guy_p8m!&;L_s% zB#F=3gQfWx(vx5vY=jG0g+Bo%!He)ROo3&v2{ywH_!)kIU*QBKvwFKW)Pa$3BK5@n z&Ae9cKQaHc;XUT}Xxd|b)8S3)X%ok3m4e7cqCYztv|~hVE770iql*+>M3qE}+Iii5 z)0Z77rd^{QDY|$^jy0d(zlA$-?7!NM|I!&ha#l0m)1pc~TQy%q?Ay|OE$OTu|Fv2h z0}mfJ@t-9z>%biMBj{TmAS7nLJw7A%_Gb_ziZ<9}^W=*)v^qo8Hf9i*da$AZpqOE_h&bvIIm|DV3zvwt=ImpI+hyD{nJ@ECN3 zeeezB=T5wfpb*>)4?rDg3eBK7w1BR#_hXKOFCjZCGLzsUCstLYb{&Y#$S zeuw_^o2E82wpHJ63ME z09Sp6vwHj=s&*Lu|L<7Tjw}@aiRPAnxBR%}zYpWXv*E9`I-xb}+K`{NCv-+@^0iDr zEy|Skf9l2A>O&Xk4?|%XJO|IiGFT2P;5#@7qgj192FAki zlpV)+9N(~b>afMb7H{a^qko6`9r{=Eh5fOk`ej`%a$;FK7M2LvT`a7{(st~tZDoX9 z%iH|{*&2D)KuVcsh-TKn#kuXzK)X7&nyW;#UXcCC$X0isj2izJ6q~nne-QoR-TtR+ zjzQ~?h3sE6w`|?AYs;oLv2TB1ozMcivpfD1^D)PNjFb`o-}Sy|GX@ZsI@DyfBo7Q4k0IKUD27?Ej@Lp8YHK?-$M0m~9vN-7sM_ z>%m|)XmvvK|DAlwPctR$)AoeUoEi-#N3s4p=%~L0j;7alQjY(>`3p~fMgJs;(_evU zuoRZT7T5th!Kx2EsRz%)c(CfmQqmv8UicPDtl@q?cmjICZaBDe*_?x8=Ik8Ow|(D7 z`_{cZ?d3e#MMQEJw`x5CJEGUN5)Dhfpz%BGS&99%Bkh=5+e(kXIbVbxf!%wMBO;Wo z7;s*9Ys^`57PI+3#Iqx2kxpLT1z*XFlz&CK>Jji(5VaulRqzXnr@;f?el; z{x@#(^jGy4&83~*1AC$DTGoTWzi<*RV7c z^2Ep!PoC&BvPBJEcS*xxk7|lU_Syw|R8u4j8^_tlOG*29wwh}lw5?G^huV>LY$sbI z#8kv~QmUZwo3gHBI~V7+V>=gR`3%gJ@lw|QWCG&e!eZZLK(xCO?t`;0{(EwJ47~O^ z8UKmimc4#JTJ!aB82?rLPYh~O78?H-|2zubg1N97et?5eoqMHf!h_HnIze~n0X?A~ z425AZ5hlSqFb^(doqb-&2gW=gbAg-JtuS7TX1wr3lPXPK&^r$JwyAj+;5X%W1Rrbr zV;5!d$J%*a`(qd9<~r_bcE7u8#`DS z^NT=quYqpyZvX%B>|c%l{Gz#KzNw>txklF|J$kd+*r+MdqQW_L&Btk zj{1senNh!)%Fee6h&d@e0VDqJxx@4SO8+m((t03zkAtZ&4RU=(tPcgz->L_9kiG|+ zK}%>0z2P}{6aEi2!&bPQb(UAcO>i@8gw3!MegavCdEQ3e6K299NMLPFB3uCZp*GZk z&HjM8 z^>VTA%b+rdzV|{?IH-PNTBeS=CnE#G`l#Kn{VohW{##&Y46OECjQ)IJ^>^K&q1sm@A}u<4_RQdA z{Qh9Z2P3u)>a2|zS-J-KpH)!O&y74nY>v#jC>z*wrvENg#~iN>+kdCz|LQ$TNjGBx z<~bvl4AQSxd+a`*pB3>O_I1gT;r@bRV$*}*xisB;Xr$^kjQn%(2kU14gj`>6A3yx8 zDwCc}s(OM0YG$9B(e{MSFnaC$Xc=eo*`7+y=*m~A>ZZSQQpOl)o{--0e6h(p;Vu{g zL&4JgJJP8z1>Jv#6JXVYQq+Mma2MPS_d|2&0K?&jEgx-p{gnwLCk&qO)PydLy4>5S z%gtR-z#j3^#gAlH0zHI1mZqJ> zwP%u}+KX7V%!Bt}IqZQi;RpB^^0IEW1T=$*Fa@^3PWUhU1vOY(R}&gQBWMAAVJHlP=V3U! z058KdSOc5j036=Aap%TW2CMjAQcLD8nZ;{t|Bg#K^1quV+>U+3%Z6YZ2(Q5$cpct=MerdoruXT>PZ!P{IdkNvPj+ooB~UhZd3w(>SmAbD&qY~cEp6nC zsK^|Hixd^fD1($SM+G9qkZ#UqjuJ+U+gwJZ@|~1nugd;QBfVo@vF(Ro45-?cXPsmP zSO=fL4mb=yt72-IAJjcsCPm%D`#3)-PLf+nLS|N|d$f$YBdev8GdkX9jP2!Myxv^t z7}Pr&J}~GW!*Avm`&|zk;0pA;5-jZ>A>9lH!&|Txtom?-^ijyOgXgJl zzafeBHu<0gTnRV8UCh7Mm6=k3nwq z$`a#$Veb|1_H_&*ytoP55W_+O~nVZ?t|e-ne+k%h+pM03k$=fn@k zyZ)bh`m6E3Uo^Mc=*P|h{kx`k_OJH;MRO|#d;c8J|AKEl{Z;)%^AF(vpe$>n*=rgf9ZRP_B){*`d5Vf&Z%J4P%?Z}O^}2RVIE@>-wPoC0fV-KD zyBBIeP52!yV9qT++zxj@Rp_iX)S&$8E-z5dGBSH>=Th16JKXt$xV zaaj|~I*kAAR_~tfe@ASU&Gj{dZ<-grBxAZxNmV^_>zX3dG)>K|V{7+ww%S)E`iG4b z?Dq<+Ddt$Tww0Qqy-Se#WsXH_N4cxlzS82O$HSW-dcO;w!9n%&)-rX}J(WXd)E&Xo z`HhzNfp)C9!3 z7yl{opItBBRe(ub3LLF0GW_o5%BT_t(#Oo9Eh zjP?%g<0JNY?N9O2LN%RF(cUV@Tm9QYV)LSR1sDr)VEiZrS(}(yD!1 zwyo6(ZQMU}MqAdw=E9ZNeob05pxc0E z-RgH6ut<4oz{QQBh{u+UCl!=!9h16RWTn2#T8uZ#jK zd+jz+wp)idD~+$I`&*E$|1Uwb78@7)z8gBIA8&(@8FfcY(l|UC zfCt)1b!guvgv8W^$_&!Pey`V4g{}NC6lM=7X8@Mgg znu~0>tJBw9gl{YSqPh6;2Vf2KR{zY_Av5ZZvmouZc04b&qb$A8Os^doZ~@seOMgQs z-TwWexwP9G!MFdM(0{e3zwQ71qPfIr5{GHF-{veZ>u4^)RSaO*e{%vx&;HH6LUz%; z9y|gwz-8PQjWU^E)|K{V|NrSDhWODx|0kNBWConSKeq9o-_pNvS{>-j|2<$ROoVAL z6JCY+umIM=I>`PVpCuH6!od1mI&!Oa9A7ePOwVT3u5UKRls?MUVa{r)2rIrguemx* zj)+jUVzROR%3Os-S=Xz>T%6lp9p<7eqrzIR((b99nf_ybv3W~h(Xa?ifQc|0J_5}r zXhVLjRof0du&3>*rXHhZj2+2N*N;FbY}e0R4=Epd@bN!mk1_r?MZV&~=>HYrxsy15 z&;CvSpJZu19-S@Smy-S%_QHO6kq>w}{2#1`?aZNm3swG0zX3WzFBl2?z|R#QF#qwa`BT5sXIUbHsYP>}B>>E3;mUJHLK*Q(OK#N_i%`FmnI zR$Sqd)7bfsI9BWz&Be~YgGt!7=IdLjABC1tcbxgOyV_y+|K2~Q)Bi_PJ2DdgWupFu zPyzja(cJRy+wke}u79DQJpGmVKfh>hwdWmZyW>Fru)SM6Ukmze!a$WAOSvAY$wTQR%l6SV$c=uB%hDhORn+oK*5Wjg4+ zZxZM#eMCe5-;a9wEBYr%pKJ;&0O^}8frILYuVw0}d$hG5>JFD_cTt-W(@#E@PSv6r zwac^#z04fbD<>az`qzKt5WW8as~>(B{qUO592hEfezfqvwzHkDX;m{fhs>BeGBmRv z*0VSKIRh@^xHPMNpvy8p8g_+Pn)$gpG{|`|Stoi@rW>qj(3q&*j zKPBnbLcYXFN$Q7Sq!VYduuA{QGKL6l8W>~+TbbXq6>KuHsjN{8Siu&s z*B%i}-*{+Iv3b${P52SCIbbb_(=ve)o#DK`{v)(^cwPnw7yM#06q)NCGSEzP%pr zhG+r||Nq=^&;Bj@&n~)49Jdkj5~FEjJzAen%S5YA=9YJ*y*dB?DzTO~|L@qpXl}*p zFOr@NDex^Eg+vz1X>~#yvD0EgZI718)b$_dUAHX6`VR@f&GA1YHC%$fyMOoeSI7UP zUp5dX!X&Wjz&E6=dXSSkkQ*+8^6(gRhxPC^+|EGJUC=7Dn1P8B6fuK)Npy%rwL41cB# z{`fO-{y)fAv0pT|?7Aaq&DU#9zLrsI$(c__(f-dkkxu^~P3_1)=RY&`H-rl4|BL40 z&wD^8e0IF+f9D^b{!07r7tO79`%T*Fc-Q|MPk(LwhiGoaX?L(TE)Mh$yZ&dupPv1z z_Al{yF?25pcR~$l3Gcx|*aKg~G58tEv3}}uxB{BMqtFxvgL(hQk_E2}@7cU(g9Q&P zxFdXMU`O&=^c+0$W^0g0o2cK3b;Cyel$7NlW>g%u8h)uF`q6{a{ZA6#-5Ca@`S20^zXcc$c7C)>9cMd_3=Nr4cLYzn>$Mr{zncG@POJ6Se}%t; zESnFX0dv&j3nW60#^|5vkWxCcZxWbz@V{l+F)PhwSwiGV&K;bd7d4qvL}? zR%+DtWmBIuB(5g(N{xWMcC@37GurL9tltP5N!Z70D?RM3@^1EMrhQh~8t$y6#nvsY z^P}a(a2;qqUmN<-GBs4w)-vjjsMCdH06xT-tCy zZSc)-4(OlE5V-gJ7xVsqzi5673=h=4joJ`ofwZgMgt7i-p{KvS{v*5SF7f$z*b6S> zzO$=BxYFJn|Jg(Y@2&r__4kYBRzIx6&)f?NyI?o`5B>qIPIObHce(yYJLW<41X@Pj zK|0y#1VnriOwf@a=>VD|K}O0L|LL5~(_b0?OEUW2)Pa*AU)ER;+Ch7;>cdXbR-O2l zbaocdUkv4-8MJ^7um*NPX%^UC0X3lkG&|0{WLuZb8MExjkiJj!ZCAHl-KzW&>Zfx)eTBMdycf=x{KP_YvN6@}as2;vCR+uMy zIxRe!6&`W3{zo*oY`HmU&DU!~Jz6Ffn!A4wceTUt{~KaaJ2KGui}m^+(Omp@M<|7_ zj(7bZ^T)tDWFh@UbE~~>N;@6z`k$A>(_e}I{Gz$UXPw~}=%spH&E;zuwbYqUN8$hH zdityLzkW;mFVOx!@GJP_9vNr{qhSKP1#=+<_QQ9OEGw;{5LAYH;XddF{or{R0ps8Z z$lc#O9(Kv>(NB+ly6ckGOUN|3w-LYX`Kt8Kf(423gU9r|m$?#1;^VOW2}(*&q@eLT zQb}_?P{eZv_B-sqJmQi3HRc`UQqrF3v$x9q0rpn=tg52HZr-Lh}XwzWE;jpc;Sq-SI(0~N{j_|G9vf7Smd ziSI54w}bd_@!!KiTk@l2>ZmfsX#LNK#B^G%xBe&W;t5}ZXl=F8n@I15!=Tx`=K8ga zw&X+0sI?^ObRlQxH#-NP#`Pc7a>k%0Wu*TX68kac3h(EbL|A4b7AkbC83!rL$p-iH+U7JS@C zlN}mEGiVM^!PD>z^nt$c3QUD*Fdg23_u&)R401otFK`lk+?#PN82yBlRfkyyxF}@@ zue7Cr{de@I2XQ!MRmziH1JwwYC)S?Z_A4hUCa-<>f*m^_ZE&Yk%HA4e{oga^Y3u(a z5)ZxpFJP~I-r%zvvNc?!?#ym<%vUbR)oQ&0JN}og;m%q{Y+kgl2xFnQ`axi>-^DXTt{10sZgF<>_xd z|HUtwTYmiQK>f8fKw9Q3u=#*Xa5V;C*#B3a{?7Uj(OlZ(58z*L8TSW#_-8olN_%tt z&+oM0-ufR$f6?5Ey{<`%zZ&!ZBk(TwO3eqYPH00uomAnqr2~ya?$L^A8TF7b>7Zjh z66l$0eauvLmI*l9obP{{oZHi18UIU?c>6l22$kSIr~(gzRSyo5{t+IfE_8)?VAYKs zr1!!3+@W;=9NBqf3ww1F= z-i-Uho;9#JeKic_Nr{kP2!a5zwZY=X*+IvSRTQT6g?$-414z0VEclQoFgGjDC z!gcsA68rub{sWeldC*ZC^3yVzH2$}*dYRDQ>2y<`bXx7XxTYuB89a-)ZZbi%wrsZ` z=|a#FT0t)u4P)UQm)zFnv{XgUyRrPT|A%ck+_y25+;mNfo7RYDPKdXT^uEp~6}Dtx z+g3EWL(lR#op$4nE3;YcXtHRv4tBvF*bC=#``qPl8{7vSp$80wiLe8{hTkB8o2drD zU>E{(VLohtuOJ2X!;kP29Nd2JvsFus*KB^zUiH$dQBRHPGHTWKN0eF-KIyJ)qeVC8 z%jHwE{$A%>8D(qOPtiVJiwM{nF7AkURKE8?`;5 zGjcN5Eqnb7Vo;B=PRp*3iG+%-bBRy>N74Hhw5|{1U?!}GJ#ZBMfIq>i6IHpFN}h<& z02)D4Xa>*12$%?yU>dvwpTG}W)-L~G`HbZw_&s2G&yGDCK*yfTt1kb*7p9$Z{C324 z()Qb$pS8ET^X87X4d-g*s^om8Uk+O`-F`osKBaTjZX>B;V%xdVr8Hau*T4kOx_nwj zGvvyuy=xhDNAR?}8v2q%?Z3prwli%)H#1}UAalCWv=IrSwQ8r+$$tf3!*5VP?w5n> z;ReX+{lE7v^jZmJ{4cloWYPK%9EPLtD~R?d!H4GAAumW>NCv47WuY8g1J{AnkBU$U zPJFj@<<_|?=Wd-eckA4NbGy!Me4=V_7ocx-7uUP*snM(Y{;GW8qkPV0dQCSarHs3C zy6^YNyN7K#GPQ45Kx|rcYYNTaS?-S-1moZhVA$9B(L#Q0Pw0#m|7#iNyk5%fClHNq zcN0116nniHZh>m>05k!sKJ+F%7e0nh;UwguUKE0|P!8I`<1i4c`tv5~gFDTsfS!Hj z)m(lx==(sVKQ+oo&81Bee@RKZNA0@b?VLZtSb8-5?uyCA)tJ}hHdgbw^N-QY68H*= zjf+^)^@Ee9z`vH>;Hd+iTEcxn_&Yy7kub zht99&$He3;=DALMsd8V$$5 zboc;5$NbuqjW#h8I`g1f6|{`HgY@Zc>x}uI3-iUmJ7k?vq9n0bOYcRbm%0EgiS zq{2}+28rB1nG=dYF(?itp%h#XH$W9Qp1ObY{?+@JzA^5NCtFl+Vg45&aHuRTm}!B% zd*_KXgKf9EF3QsCN~wazZ`ZA^-JZI52U=(srLb%%Wyd164QpP}wqe;C&W1y*kO<;G zPMx_(7W)?6m%$;(`tASxSXg-6wEv>HW#?kgb>Mke4cWL?E<3yhbHQahpfw6EGohOc zZBHnV5qy?FjLcjUW~2=N-zC52|CRBd^Tb!z0@1xLG=PTC4%)-hFbt$l42KtB6ub(v z-~(6%QeW1GnI0;r=N}wJkLQcpF`5+k%ZeKNb)tK#1pZb5-cFp7!MEXEK zO1|(FIW@m(#*Lb{jiz-uTUv~0S~<$fr(k{-5#nNBY5D96!lh6KDnS)^6D+&`g7gkZ zX@g;G^-XwxzL;JzqCsIt0T4jwmNBL zqm0yDyaVmIi&A#`{ba8joZ4r%-?BAq`)!w$dK$K*{Z3kqW~WE%S?5*=Bd2IPAYR~QVSgyledn_fbHdXrLW_ujC)og#Y zt+diwo@33JRofa(R76>G^hC}ODgG-a_APo|0F@z2^#8x_-Y}?#H`a>ePL6?v$4ji> z7tJj@w(Ru}w*3jYxKX+!#FPJDAB)W#J)s7#@Kp&;`1} zKo|tWVFbJaQ{i1$03X6)*a(~8Ye<3b;lJ=F`~`N8G4-97`**07p#{jg%^A{B}JaOrOxjxw#JQ+~Jy$%xQ0ev@EB#Kc&RJMdxzBqb%aU|933t`G3FL z_2hoh+_G=6WwBea(F?grJ}@&hPhzXg>9sF!Oh%De+yKF7SIv`Z6X)SNZ|Jqjj z^O3EQTR=1ZcTv`Nk=XV!*b1WSZn#NwR8`V4nKb?vHm3hH7FHWCF?}>6C<&sqWyk$V z4}c4KMnYaF36;R5@7G*+=!`bxqh*XL7tPf>zCGw#M=^MF{!dK79RC?w*z^C&{9lsz z>>khy`hryl#*rQm^I;{dhaccLoCF{BA}3q`R(-jV^li`?xT(Gd^15x?OTN z8%0~;e*CAf*th6*J9L7qKmJp;NDM4IUfK%3Xl~i-a?+Zw*XDn;OfYjjbKCU!pHPv6 z5&wS|i`tQO;(xzrz5+JDh4|`t*MD%)7w}7AF7s$??`GrBN6QSq- zv^}9S+LB)_6W~Cii3Eejf6}0v@t=`0?ElhYp8jh5C-K?EP!ev2Tc8F!2u;DN6WvMo zfYC4=-h#QX7XAarz^XR~zg{zM@{q3cS~sb~s|wuOMAeqq%A6kyjJPBpDh|`i28q_p zI7}-=B;=eiLa&+FO3FqkWyeR;7Z+%Y4ADL>v7Wg#B%hgmMe#kYcwND z7w_vUAog7ZNmH@{ zyR~C?WlcMFQ5HLvYmWZPwWGofIR<(@yN@i^9_~PAtWru^J=y|d^Oo+TNo#Gr7V>Ku zMW?eMsmK4Jyo1sHKU>^u|CRRNFPe{msqoD?pnvHSp8l%-qPgYo#kY$e{}?XhIT?B3 z9he82U(ka4(6fA@_h=^wopJA4g2w;iu+8}2NE!D3f1dv8_dn&5_IesjhXt@4ME{L& z5DvlLa6WawsuQJ1UkYWQ65Im!K`m$okHK&l4+pj_e|_87Uh?~~R*%$vq}BbcYPWji zc4>{wQCMmDtk%d!>y~=kZ%*DWB?79Y0*})6b5xK~+01X7&xQzus3|S(cNH}i5{+iV zYcLny2FvERlim%vxTF4jxB!a6#c(;ahsU5h^oBk#3todcun@Mu7jWX}i3499*tTNZ ziiO*Fy|M6(@oy{~Va*Qn$(b>hs2?GALq{oP)|6#5@eYBt(2lX>Sa&g&l(g$B z_0hZ=TdTG9S*7l}J8NJq53sI9T4v*pDxXtm>|PZ!O&S%PS7+4p6nwRT<$`L#@NMbFe$cQwgq z|4WvPK~2g+2HU2&>niic$feI>i|d07}UA$ZMML6yq6ZMh6{h}crTi`Rz|_ic+bT< z1ja~qWS0JT&y36hqd>FAqiyBAwLGz+*>};l+MkzfjSwGG)?EMLqO31j>|69)28SR^ zjQ@v?6;H;(!s8`Y^o!<);RLkBZZ%)8g?w7Zw&b%S5k>sBqO|A#760!S%`N}^2I)7U zG51J60v%xhXmvts*w60vUtE9S{D0F+JpC2@lccSl1D5tnNiT!nAt%~j54S-hu|69428$p|wEv-I_>8fle_~Bhp5d(C{)eg^M*nZwrJny+{J&o`xBT~5(y36Bd!!e@ zdvG6jQ&s`3UWASTw16OVMms_1jQeU8wEr&-FtGh^QpVF?(La~8(W61Mp9UYnQrHAv z!(os*@h23aJ`{xta0gTesVj})DR>%2!ZdgntU9!o^g7rNKSK%XQ#rU74sKsDYx@gN z2L9KnbtB_-N)4cG%}6V%MGoPb_GnsBMkuuNp8kl0tBI!lH`hVAY5z-#29}L?BHayM zh1X#hd=mULWIJ^OG!cy1-pM&vCSa~X}u$bSA_ou!( ztlJ~^;Q{OjaC+sRBSkH?Htx!rpPY-b=qRNM8o#5TS#qguT8sZnWB@$C)ZF%bQqfSI68`DvLts!eKBep*H?6+G>()iz?fzhdFF<0Ym` zj^_JcMQh96e;}>3^IFKKWr8burnb7PNk;tlQCYA3SK5ESXm0uU?D*(-*S~Q&Pk&W^ z(cBMt;hA$l|35tawfSGs+=|;IM%w~ixJSM#^n(eY`2?-EruFbb-}0~R(Qf(IGDhB(rrh1v%S>dWqH-$ok1!1!;6r@uP>d!EE!Kf=Ety1&Bx z@l)X~_yj(Ki@E#qDyRXSU_Pvbjj#!J!Cpv(AK^mkPF^Sr<)9NteR={0!en>{CQ!Ft zff?`~ECj309sX|LciZLl@%9hp^{Vlk)QcZJOXlehpZKujhXK{W%m#@BwQrMV|1(by z(IUepr5Z3cDDh;Z$j~eq7+K1ZE{^A&wV0Kd)I6X34*Sj=DP`=n0pztGbN#5KV z?#yMyz8Aw1*aVxwkF8raUx0KGC<+zeD!3Ewg{sgR`oUay2e!f2aAeQNZ;pTSxdA;p zTdx5vYX;RT_(nTk7p=nm1qlH>wD+;9RnVTGRF}a1Uyo}%FQN4R zb4$Bi5vs!dPz{5|x07sj;tTuzX}PuQ z32gN_${f#^>$8uFCRU-W*-MZ!M2KrDs@F(a*S!S!8zP+BS6E^*OY09vi^gkVFNoGZ zTNWn&b32Xm6FRfYDdh8MnLE@yKA#cgUvzB@Eb^R<@(mvt+B@bHjm4IWW2a)jS3-ZW zSyd@5qXh-p9xbCDpV?_6{@WUhdX!b;zfh5Lf@p2|>UE?)fnuzYEe_@3ZV27xYfZM+ zFo(`)Q~p{epql(Yd)EPo)%X7Iix6d#ts)h&Wn`6%lpQK7iHz)7R%wu|5=tal$ttsC zWM?Ik)iSbo_I&^6l2zKjZ}t8D-bNmKt^Y76TY}h~h07L*p0z?7i0W9dkOOS2=cm+W5 zQ7`s@y*xlMz*~S`fJuN^0AhH4Ex-l6woe^(Vg5%t#&fv=oj<^HSw zY#mtqejUb7!~<}ti$47O;(pg;CM>i;*u?R!Cez- zYc>~85OV&87XDv}C(vP;y8-wCkb?HQ!q%^F^(zj()?k1CcPmE;zN8&<_P^qHXve?D z{-eV(U$EEvLHYndBtR5EDnJ>)3Y}Qt*#CY0-<0b&SciX){IOvl%?+?1`(q%j2XGP~ z901mXa)6frH2_Tj7+?-H34kNOSpY8pAAl_U1v496o2!Z!eC+!?~c(yL=oTc)-GH@wtCP3;)mfe^};N0C0Qu23TS1SGfEY2fyFP zf9;Mx;Qz~ihj#pX>_0jz^X0ZX5FD@jXtTG1y)l3(KoGzR-B=L^teF4xod+vsd41=> zCrYqtuOBwk^S%D16l`kefB)H-=Qmh~e~6dC|X3_YiczAr>&-~rV2KgWcrafU`QHge$OsV071m)hOk;JmNNuo2g?~90G2Z>+ZE3ZzVl$k6NQfD zZM|jrLH+WBisc6@zA0G#&5DD(<=^~+2WiXiGoQxAhse)QBPq_qqpU~i8PHA~pOM9c zAizW%@XrY0N&sIRZU<(s!mpcrng=W3r+Kil%-g&96qtW=d?^oBoz>^b&RIuNSXOud za9gAU*a)x*U^BoD09F7gfPDbR0dxVp0K5Sn0i*-G0(cED4KM?M2jW&mAxRy69!n&0acJb46vXJt|09Ru%Hu(ApH#MKm)*n&b$Tb z5r8Rx;jZ_MuPYlX3+GXoRoPgWRhS+V;v3^?XOOOaI18>Gu)}cGfEAtpy;E?Eak^va zn1)n8O?4Bnk8Nz05a* zv|mDyCI{FAzy!bozzM(wu+XmegR~5Q62L)#g?6qA(nkSw0rUV)09XK60@wq%09*j@ z1n>gz1_%JS1rQ1l1u!z$*;G^2S=3aNot#}%6>-D;M#P{?r%O$ht);Dztj>Zyd<@SwnCI|{Z^79|M6i~w20F6j|5ww6Rdokk1?%O!Hhy*$ zxbsq(i&9)g17KYy(|4m&TA~L~4<9d4zv5d^#;5IJ#rNQ9xJ>x!`LdU{7Dm`6e=45^ zdE5oxMFYG5s0Nq>m;#^y^Kxkc*Z|l8lmS!#Q~|62;CZ{g0M`IQ0B!@s0K@{60<19Z zD-Ooz?brPF-}%j7R)ysZolDR823XzUm;<7Q+u4Gg(?I$$Kn6fIKpsE|Kp8+4z=9ss zfb<~15Wob$f?mvkGzO3_DF7M327m=!VFl?0ec=RYetW7oZOS0p?wz0gwXxYYh7eLjRoy%Ui-+ z`V;#Pw<{F@Re(bP3-UGv=>?hFgY<&jFM#xd>^(vH2EeB}kOJ~D0Ty(j2&5|j7IdN- zq!;u80{XC^8*4!t2Y?6w))O)SMu4pVL+^)Pg0Z2>)R?HCE0sZ4f-0|6Ub%GT0*uoJ z7r^!soV;`e+DNL8ZV$&{@Ky}OyK>SPkOF`vfHpj01X^njA;sbuL?D+4yb3da1ss*b zS1#IHaQ0GppXcs5>9KS!DB}n0!J^ezVN*V}=L;4H*oFCi4Se=zcI@j`4%mfNuY$HO z@EPXuHUNGA0RUM5IRG7i;{dt<7Xe%W?f`@VWB_CW)B)53GyuE;_y=3cis)*^!9RG@ z?<~6Sv$pn!Gd>+Df=86*M~YyDo{RrT7Ua$b~J|>&niUHd(XFot`^SNKN zJBw$0@qU2P)$cT4=Epm&TW*nHdj((fxn+W{S~co!rhzhM__ISp!d17h5=y9eGpgQ&qpH%Sm%?ECIR8G^AoZosO1qqS;Ag3 zxC4Uv{8uvZYHwLTOh5A!K78ejk5Ll^_{;FLrRoB_B5bP!{}!_;CB z?f?r&V(f+`y(WhQY%vjNV2cD9*pLpS2+2W8kQQ_jGKEe6z#oDE_;T<$qX=OQm~(+E z*E}vKQR76Aw9}upVh$6hBtt?nARjJu@7$>nbX$UEKpfe7$}Nmrw{2(Q=Gn>1$1fnZ zS6o6;YM->yL1h)yLx+#(9M{!5p>JSlX=QC=Yj^7Ox$_q;y1HF*zv_F<&p+V$jler$ z;SrHhccbqmCMBn&ragR=o|~6nP+0V|xTNYu^~;*ty84Eu=9c%ZZS5VMU4tKnhDS!n z#wRAhbD{a_fbD#ZmemKaAAyE}fsTO%*9U=S3s;s9W6dTGOrqTiSlZ^qn>o*7lkB+? z`XFa5J(uDD=`o8+95M#(rY(bTmF8;neHC)~sTwU-Xt6%X2522R=*$qJ6G9@;WaJqf zXd84J?LWC&4{`F}@wx9Mek=+Li!Z`T$-}?HkyBJ2A-V>lPru`DgA*3LH3EHK+HnSx ztJ{trEjdka`uTe9o+4UIeF9ocIcexzuMH9!dMh&3p_BwoX=G22dCyLg96sY1|8Ca4 z2njjjusjj2HAO-~El4Qq0@+bIqckLRYY+*&+jE(H!{XHndKi~kP713k4RfC{+vxQv z#~Bb;MQMi-6!>2bJzK9jd7^OI;zYnt~JH3NrLsMfBbG^c3F-e9Cj zles@wU{smul*BP**H99r1ObylokCr^lj_t=dnZeq-d)xnbiEK(O@Q%GAePx3m57=4v6F>!Nt zireBgV@eGV)nVRO@6;vU-VsymEa-D-*KO+epxycWjZA8qC5utj_PTT@KN^sn8OSU`e#FtQuBF%P!;KN`I!7)|>ywH5kC?PZj|scKu*Q{#^l38^I_S>~8mK=3k(&t4NaxW`rFx1-JIxaJGemLj z;zvUIZRxR6C6My5C?+Z4YQ*EF3UZ@a>W7Du9TmIk&psm(J2w)0PvL@i|E9>wVY)n# z1{E>xox(fMoIFONnJeI&kzJEfCZSdn^7=@KWoI0-V$#dlV;L_@jy+2C(Pi*V#7@|( zU~Z##AzhRNnlmb=`U}|H8}QHUz|IKh74XG>)3Dd#oysuj3)kFdNiVed863h|yuEI* z4RR+k@Onnp^P*Adob$GPn|Y59=ir#Co~-dLZOc=Z7CmFcrSu%dEctDZNfhg@;((u} zWKb$4#N1AUJ==ZE&lf#=;6x$*hH(|Cg7~N%1^19pX=cvr>+yyU1(bQkw{ttn4S2Vh zc(w%l^By&rEEK7JG;tgWS=D-8%8~VoIjA3gquVcEevagilz-frYf(5Lp>b7#qWZ~+ zt`o8r|#LLtVbKy`JdnQKZ-4d`x}r)?Et9X*je z?m8=~iiAcAvi(j!AHOo-+NPe}P1j}AedU=z!&;MgdY895{SBxLH+%RCyZ9O~-xtst zSIuHWLZPP0Na%@8Y}SMQ6Fw7%oO3ef1He#*aE7M*JrTha98WoI3h zn92>DIj`GqW#2(y&9)XTS|xH+Tklw+AK%g4!#uao-^M>UPCuv#?sd9MUZa#@lDZ7z zImfe#(SWLl5A39BJZoAdRD`=;Bp8n{W3#bD95#*;9ZV0ph!vU;Y!Ib)@=!QJ6v0O! zv<2YNbJ_Z9Q> z+_)r46^MV{dIrcbKV)CVW37I1!i`Qm*SeKl^9y}XsBOq$quob4ct;9TfFmc?DO2*{ z#9_}k2kt>9PpW;6x8JesuMv0iqw`efI2e_Dq2$?aKR>slyJ_6w&#o1v4INNx&I)U! z(d5&+63@Nw`r6w%!6ChDq49#6L_A^Ej}-j8)2(C_UmU55OQ8|a4XkNG4~u>#?$C3m zxu^Vc%0{4OSRvC!gVUPO!1;iflo4EyA|nbRwb~9&<|y3VHNojPvQNjUj=VIrj+3^Z zQEc9S`O)>Hr|nmbI<>KLEQ)l{{NyPow_Z%iv`+1q)M#x&LXjL~vF$1Z?3tG+Oa zts5qmP!$Qb+7rHY;IVt5#-U5bta?e0yqarN-Sz7@1*l2617!}M9qsSw92A=+cEs*% zh`ncB%0N{i$1h+~)EYR^W3r*HWU&6GgsRZhhtkX3Y&pazncXUi+0;KFk9IB)O1N8} zbN5iWfi3yfx4Ly5rwI(W1Cv;T_bCMiX$0B|%Z*?o@BVh&ciX(Kgy zPiLEwH+S3B5nBrYBHg%3tCsoRS5PDp@3p)Wb0_WPBK#?OkZFTDEB+=!kg$l=Qa~k@x z?P)SE2Af|LEa^}tIepbYDU|;)gP^9-h1{Hy@Y*P)ZqV9@&gIN*@Rsw4d6sAO0l&q; z683|4qQ$OF-{dWexPFXAHBJ1zvE7mpu*z$e^mo!NoqaGHne~XeU3{Q+RdyO^WF4~pGAxc}*fk{;pEd1URwuy|OAs9*;&;-{daTIUObfmb%;~7yf z^I9dM z*NEI!*d-|`HSxlxWEVc2-4_n3L(JvInTL-LpOF-|Zs;OEO(;sMJIK~pdvV&xlqFkl z{|O3T^bQCAY)F@Cl&Q!R(GC*34rtvYo9`&44WU7r5u36zy> zmwZ+D1M4K8XD6oRU*ZlnaM*EA1_=>O6_zLQH}eY#ySjfEqUPmx47U}})xf`Rl*a>_ z33{_>LutN+VK(8j>+wz%k$WHXR8Ung%Wj{$-amk?zU@ukm`;tudTuZAvCFc zO?s+Rq|nZ6H%yng7-Kj(=^4&T@Gy_X9h*nN_j3AChor&&#)C)1ugz{8I4{}rAh?!H zXB_L^6u$H*yWQBb|}S{b7=Gbsn{)+%J-NS*jL0S@=}rd4q?C*M?UPV@g!- zoE^2`dLJSxWIo;fte{2c=KG87bpvB~Eb1!sgagAG75L6{b`gDoE%BQD!$brktbT2d zdU5?_jwQurID%Qljz#efo5mkmIy>WeHXkN@tCUK02kojoX0PT&V#@auj1FGTdF&A~ z&FnGa=|ZpRf@ZKr66D7@^br(d>j<#NZnKJ$CUbD;H+!z_QZ^4%k&2d8eB*1`eZYri zE;6HF*~ip-g|xQvqP}5^qWMdX%@;OnJlMQ>a$5nz)wTOdRf>G1a=7#52l1!RBp{*g z%g#)99i;Hf9y!;DRM*zh2X$moBB7^9=(cn1d*^2l@||%srk?;n$d+u}k;2>nU+ z$sKsi6=?NcXGI?-JwkWl-?kgW>9~_}r>?JIjJMU^8rQ;?{S>tQQ?&1R((;W;9i{_z z9tgL;s1#C_@%kBWc*qU2*Hgk7#0p7LKKUP><|vRP5!c}*BPO~BqY4to^O`Xu9G=M} z%W9{o58TZ#AK@4AbQC7?P7DuFqS%#mA^xgdEE3X{yJ+5E&{B||zbXDib2+tR0bRk! zRuMnD8(v$|dRo$y4oOB22cHtZ0L{fq)NBPv2yu9PEH{8m@lM>Zp=QHL3(r~5U5oFi zxN(v(-n*h)F}LMFwF!%<4u|@yM_zk{l+Wgn4p&~in$Aw2{P1c`+xA9_tA#m!XLB%= z40!agI=pk`NZC9xup+wh&+RHCGTSA^XIhh`MYzBIpgFH>M-MaN0_0>jZF;43Jg!;N zcb8{{^O#Cj2-YxPfC zZJU>@phCsED+kWDSTmJKV$j|>--DH-MV>|TPG5wWbvSPGGkdrAiV(vqn~o`8YVkHR z>STZDC%~j=PhM9CJigwK{`yLe+kMKwr7Jx4J91qv~_<6c}+{ZZEElS@MLQ=}` zcFRkKpb+-g#X(aXq(eMvmd0Zu7Nt$(+6IMG770&FW1r=!soM1&vn$4n$EL64uJLfc z#FsopXJcSd%o9W;`&idbK5>%PQcI4kK~)4hj=V7Zn$07VyJZWy_OT(Ni{y)ZK+8r# z@jIsPg$`~M;cP`hhmeqxmb#baopB@3v79}NgtV?8A%P;o>FuZ2n>bxzdqXEZp=7o& zUV^`VCVu45$t7ESLPV>|e#X8EN#q`Rg=QV5sKSn0PTr=?*Oo4KqJ5p}ykLE>(avn8fpsW61 z^rn<)Xm`#qXJGZfk>RJpcV|3!ng@(dlXwT8QO^$_rpTE&lA&I8>zutKn_}x-#l{$9M`ZXzs3Yb($ zL`Pb^^qCt=Kx0h15Jk)_rHhVu`X0v??Z`skYfhW!I2}%oTb_#Ss->*)!!3_`W?MpV zJpT<6!W(JPF?+^o=~XtvbI+|l98e8%^sMZp!_K1B9QQq( zmXUe3xoVSfSr11W;q>Om><$o5`S>oW`(o#W+D6CZ<7Uh?Z>dsMn?AkN<8W59^Gcs{ zBCS`TYyORX2KCFh5`Oq$L>neJ5_q|Sqtza>;wtqD49M(RP%Rv2kgegBq=s12rOa1D zonvcR`$e7PW&$67sQ*xtX0>-zm@y%gQ^5>4@5^Idb*j|Dlk*Z}V&xvsdD+7`fP`L%_hm=eG(He6Jl#`#HC>oIJGeoMuYsy3$$k;cQp-aafL3F)qF^hyi^iW6{(=8qK~gRpw+v+ z`OO*~S9!aX{kkp0@#Of&I3{b?tEXQZ@wyP5^cpC&q@B{Cda`>-29{}b856!DnlIa5 zmfkCBs*d13J?b52kY1kn+Ca5tOg>HGQXWpzHQfyc%w+lp&wSFQM%O@(R)M0su)9TH z7`!2`vQrYf`d(f5%Fz>ag-0o-yG3+l#~#!rQ7d{g-RwKZJG3JB-2uN>lN?VdC6TAPYgu z7W^kOY=!--VUdZ4j2MnT)bdxFtsSqmp`v0E?tV$U?e)Zl!dr;J4d?pa(iq7G^5smN zP)OfN`-nz_a<=*#8DJ6`|#Qy z+u@D|ygaGN>`dBlx!dWY-9sOIb=!oUsfRbj@gA$53OQiMRoH;&i2Zz_P<0s*-}B=Sgf48~C$4d}-x*&-?~RsXAae+584Jto z;6>r%rly8N%xwxsl&Vwk+FuG24;1<~$uR8@5l{Adi$U$fuhS)Wda&!D-#*KIfm?J8 zlgQ<L8mksydQC-*wx1maz{Ff zY3^PwS;<1+shJ0m>VT(XJ@tLT%J z4>5C&uvI8y%9D&{U3-hL1;4((V3P0@i;nlfR83DP&-oosM^%}@en)Jx6TXH=jQavC zY9>8nYhJJ&^uyl#IBZ+71}Bc6CJlxLoRsVzphns%3-p;uyGl z9HR%eHjkuvvxr{|S6pvHI&qD9t?SqYhgml4j>mDHI?T7S)y0^NAj^%vd zxf`tN#H1O{r@-)&3h5w0c#6=0q-_b;Wvgp0+nizvN#0S^kmJ2~w~0XX%{CD)=5tf~ z-Zcq7FjGHe9i0%`nT;>Z&?(Fez8tXB;X7Z>OeO}LJpO19`#+3rhRt!vuAd`8!M;Nh?~wS z@o?TSjXmJk?#k9wIs9-352o7$@wTZrFQ(3xdxiUo-=Mn-Z^}--l{;#`|6v2On|fH7 zwW5IgyW7$PAIG-qF8EnTof%4u0YNivh*%9l0ta2K{s>y=_zFmK-2W4ptF)HAz9sxnVTPgQA3SPCm8 z=$B10k8)I#v5|GFu+|NooGuvKRPDUksHB{}PeN1o$>fI&G5ICyE|@SHSglgjg!zEh zd`qzx!(_jrv#3!Lg4VF(#z_Y%EvaLtReM^GnP#zf;HGV-dYpZqFjYidl~zC{baU9F zu~vur`%89?+-b|eE!So>CCrq_R~nl-+cq-TyH}B(z_NS1HoO93aIB})fOEfP6K=(3 zO{vtNt5F^Hs$r5`SF7%9%6l**=Pt+?6F*xpn$9Z%p zlL;f5G7X3F8d`5fM@d({ekDaC#aD56lCVHrYI{L}=PtTCx1d34T9yH?o3XF1T)uwR ze5Qt)aKe4Uu=)-E!>o(NZ)n!&vCr2XLE}QtMZJcC8!^b1FJsuQG3XRwZA~v+AIOa{ za4(9S&d;H3b6Bv%bVYrl1A|$efSL5O;%pU5e>fZy z#k+Pp{Y{FC7dLxE%V=;rmg<@X$QKEo*v?>BI?SICA=}m7th}*l>xN4I?LKpHyf zi}pK}3Se+Tz^4S8AIrz6O~z%Z)lQrneKv@5*^w%5P^3)#bqS*Lj40OZWcld{4I%Zi zwn;h`)kHnV^;1;(k=+H8k@vc<#~G(?AGXn+;nLJJ*tJ{oF)LNbQPqopP9mt7j|3s{ zO7%rw%G%f+2^;-CkiKEq)|lwg!r;UsyMwt7_vOZq{IyU#!FZL+QuB8WVECek!Do z-h%I*MU_6#?!gxJ(8Ggg5p7s)WRB`mp zfU0X>#{Onz8G-V(tc{O#&TGD}Nj6CLRglEf@w|wS&y>{T_}Ccd$z>89Y`1$@wBOc&Y3qed}YF5Onx zi6dva$+Oz4v9?-5dLQ>uF1g2nxsz}E>PzG>E^lV<+5I$%Aw2fU48gckbDhJ9M$cW! zmvXL07S{^Ko;w$kfHj!lpRZ_jkMCxbmGy{Q?isFW@ksGFebvd6jpTi|^}Xzmn>h>I z-B&0rgIV z2!pf@wNWfIydOGCUi5~}&ryQXJ2CRk z(Kq$QHO<2BXZ)vG-_ng6uYWz*lV1I*;-nXNj)>o3)NYBBF><8Gc+GKpQnPE?|z)@)AYl{46IeKQu-Bf8o2s%!jhVXtNC#hr=!<} zUIUs*pXt!FXuz?6g-JaNW)qwb*o%YJ;8bQyJ7r7l^eDlIDVrtpK$}C0=7Sp}SUm^g z+U?@JUo)Fv)v{>{8;v^YU8{(WDdPU9@;Fezpyb8mY+{36gP2z=cXv0`P58d1d={~- zTIK34_Cu57{6m#~-s}O5WEV}*BSTN8&b;uLwTs^xuxBbn`e7+?@rgB-l@2tGGgP@Y zS+c&yDX;oV@>t&~X;|Ieb2V9EsBZcNp;rKp>W2<1oqpn8HCnsKGlEk^A*9@LYhP1| zrH_s~cHGha;3#9ETDZqIwP$i&f-47K$IvuK-!m?maUkunUDoTFq?)!~^!y+?7H`)k zJsDOXm!nK#DjQMXBTQS%jf4mcitQuR83uUGtLgUaq8#2{?eVg?!n>e}S=&&++V4hW zrU+%y^=$~>{nQ+SNdn>exLh1h&}%OC@D&t?rwMu&h)`QOa(3-EW~%Rh!#5BXmA5tVVV{Y22ihqe{ zDUSYTYX*k?DP2n1S&4x7oMwCM2S$z`)Q_4a;vpfoqE^kfzZq;}_8!en^TSxGWBkyhl(P-J8*Sl{qXbr|Y z$kM2|GE-~Z7>jao&8Lx7vZhdh`AXkUD!tXOYEPdmz39_zk$@Kx7A<@!l{BU942WO=pQ zUgXtGOkNO8Oq<%{$2+%#uhCE`*|Gn? zr(fRdQEaC@?RDOMG$Ew0iH6@&QqPD&eT->n-m|RBCNgTUb>+S>LZjKbqxX*b)uM#^ zV=?Eq@LS|lJ(xg3gGPml);@3adfPq9h!J2lmhR#cX%F67V68Wn(M7BcF54q@K?A6mIY#+ zOA*?Gi7Ti;w=JY!$Gs$DV*4Z3qENpk73K}H9;p_!&&T7tPMO3r+-jgk0qnI(v zIQ32al=>!eaS5hkLPeGl@A_ByT%EI=-upP6@4SmabG@yr_+<8ae9szH%Aib&4-Oh< zFQ`)`7;8`P>U42WMRhp`+q<^jc@CnJD%yg_3I73d{z8Eng#&JFV* zQB~@GD7J>OE=t9yQR~tWsa;eTLX_^n?6V`&4XzhVtzO;0O}TuGWdvK?{;;FuHoh#^ zHbHF{PwE4PIS%`^l&Y5xrDbk&kQyKQp}{o%Y`3fpX@jokdDhdu$A2f`kV@YtqI0D3 zUT3+C6|t&hyKGYA>yF8|nh-|{ueJs;YIl1Ab1kK{+r{^h6V$7hp0HhC{y0&F7*j&Y z1v|3Junhls#d%>4@4{KE`!CVi8f?x#RG8+(OZhM!$m%CaY}Mygb)Q^*&3b34tm!@SEnp;C2o^nwKMVkpvoo@!Yp>Xn?*P^W44U7a$9GGLivz$M+jp@qv65L`olCdI+j(3U-3;d+vz%&<+s^sc?q6s#;1HUG1)upyNzG! zZ0^?QHVM*Rv?P9a+>Da~;?7$+K?A`^$YWZ@AiT4Zi-P)8JQiV;g)>e`rhe5qUrN;z z9nKeqjhylAobvW1%HKbdPiX$GdiGR8K+x;=?=u+WD( zpVsp0bKDzZH6u5aaB}+L5$FS3!>+tq%7lw?DY#N&H!)9K!+R_%62OylUD3jov%}F8 zTPIA6q*JDoXFP;m)y?S+31Yamr@d)n_#MvME?$LeMr~z>yYJ(7Yu0GV6ynK6Pq>on z=%^7>$~)uKR*$d|fMC%81of#QN3;t)9n-ZqIC$VOnB&WNn zB?lrpcpgpe&9-u6Zk1WIeZo>R@zZ!GQ|wJi>I&RgD*`N&tsKmpsVPSjOU7hxaseON z1i~XQbe-Vk>gu50ux0rA{g=YZ#Z6nN&y9Z&p&YWLq#kGoeXNlOqrrBJp&*z(I$n_* zsCs$$aPk|yH@fb-KcosD7jhHwG?0#sE)Cpl*j|uLDvWFN7WYwLbIsQ5Do*JN%J;WH z`~oH?6z#c}RC$9h`P6>2a2IU)2l35&=6hIp0Xdr3wGKK?sSP`>U3C#PkjmX!UBhFP zdo|;F)yXN5#@gH+Tun1Rw~W&Q%PvoqUew%BFKIZyap;kX0RD~z`c;I+N&YEc_FE^M51^+P?|!7(jU!%?&ABPP-zr-DN3Ceqv9 z8Xdkx<#ubke3M@xHcvNWU^*9lBs~3<1j3WWIs)}&(K@gW(s}k(p4>r8#pz@peek>j z7>N$dVhY|8QyoE8ks?gyz*pr&r{DZGNAOiKR)60ihiA^W6VM4S`59;gM(8|@H^1=N zXd)%lC}=&9BsQp<^Z3ez{2R-Sgf)u-!|Is;fdTfwIOtd84n8ecmKv$ZdmS-3YZUnd#0rUQyMPfy0|* z*;U+y#Yiv5xZmwk@v9GC2m$x{N>rAdZ7~yLUgxsSu*D-tqo&>0E~L@M^`6E)^T=|> zTNM=nnHmtE~R+`n&ZG>ng2Y-RN{HQXF8UoX2$7`FXK95^};CuML}t5oz6eg{Z-))PFQU{`Kx=`)h$) z@jNzC6IkFMk>mDNG2i{vkt!;0ONwU(v9z8)UvBdGqmR%j5;*z}3vC2O3Be$K1xG6M z+6cyRO6UU;qL{`Tu)P$1sv-Zd1~21&Q{S!CXKT_sbqrrrjcqC5D&g_2*OM=^3N}AP zdzn**&d;WZXI9_$>Cx<-%;w;Cj8PGOV|qvk=f#vF&4s6>leRr6F-hLN>-B25N8HFn z?na-AJ-ye#HFR8c)OaA(f3Wmk>CPx7OVau&KP0tOnM~$hZUJR=-Bv_SxEahXG!-!h4;;fiP7SV4m~emV<gW z;Zgxtk@dTJ!^$@y5;fs&HeLsx(s|nEaR-1|o753SJ&&7@w)ad%2A00EXP@;;EZ3h5`8BQ>8qz+M&uF81|Mr$ESh$7L_pEWCvBqkHV<#a*W+(w`A}xIp)y{n5Dadq42o zL10-n@W#(U@V1W*!~?NI9MDdP8{&fa0r|G~kcP{14xp<4@N7 zkKUZ~PuBd8-WmFpT2)lQA3* z=%@cdh5Ms7|AS!^4;Uu-gKYnUVH6J-8iJr%h>QWTfdR2k4_-e6;SWKZY#AzdYy$-#VK9Qt*HXka zuuDT?%&%DTU3-#Ia5Cvm1W(Nx2!YPG2JFFFCjRgq4IK~%_UIVslelKzPsJ%Kc%7=J zKUx1@_!WvDt_jS}d48^*Le*qmC;vW~{D$FHMMxL?t`s-K3*tnu-WW*n&;9)tqqg$q z+`yJ{fxq8m)aGx#G|CMRIP(3jXnCRL^UUU$t2I2_{@dBk3nb4EZp=AXb00v!(u!m7 zQ-P(=M(Rd_01R)ivenl=@QP!5!6^$Xjv?SHz=~sBV8t;akaUJb!Lus}K>#6<2n2ZT z2_l9z55fzXiL3$HU_Zag7)YJ@jSXO45}bfc_@M_ZiLWtZ-KP7*9q1kC;IyxPK4k{l zPcj3D;!iPy53uBWm=XHJ%z#%(`x(suH#(E?iQ{cRg1`f9KJdr^!3F~D9OZnlApxOt zBmTMHgxkxw7+2)LaRi7f&Qa=dBg|?@KmtskTQ7d#XCchEPd!E=;C5Q@I# zaV5zpihrQ>{C55Ojdq_G*m~IY!_f!q1HNwRe%1-D`AJg;F8j>X&GoT9nmSS(*wpE4 zSm;;%lcsKY$Ac7S-qh)A_)$~$4eEbE)xL+mZ<{(4eQ+1~OY%kW4{%`BA%~Si4F4Ve zeHv(eqka5if!6o5?&Tv#s|PaQpdMv?7DtLu{973aevkfrZXHprXE_)DzuC@DyLo?` z^+9Rp|7Y#|n)F{|!~KdN^55OBc#S>ZuLvUlm43x4)c=n>`ED<5^rifj{W28)zQm_5<^L@IzUE!Dv0(4A;o|`P$$xf$-fq+40KGQWym#5~ z!vploBezwk{{`=Y{vGrI+5AAi3`O6k^8K8DDDC{d$oIdgonMpwDzVvb8PocbXZo=* ztyQT11>3ZI%;QV+ecrZEk?;Hn@y~ir6#r2CTh{MJwcj6zef|df_ciabN^JIzd6)0; z-9P4CR-yhEwDWuD`!Vl=q7N1MuIN>wwDS|&`8C@#Z#(5FmieB4&Ni`ZUhI`GM3(Xt zzdq)>g8E;P{&&##W437(?xN`XL_5D7`J&{zG_4e+e}DsDn2UpsYQMiWZ4<@6Kfu4Q zd1u18C$h-jxiQ7Q~9F!_Z9E*Q~LL{);%8` zy8WKkeSE2PFT~z%ztp-{q5fA|_jl0uW7>(L?^F4r`1h68jncngi+%o+{QH{qo*#EC z{XO#;mPO_Z^BGG22lE+LQ2#5|`#b3SG3&DmcTx2Hz{ttpz`t*|o)zP^|J=Cacg(gy zJrntgZ~PAaecrmiCjH>Gzi*~7I{#AS8@%>^G1GVz>VL&HeFuG?bN6ebrYQP;ZKg4b zfB$~|J^Ey^h0o`5u84j9Ip4#w8JHaKa_#qrZK*Kn`}E+2SoSj@4lp1P{^sxsTOZ&4 zCYl=)tS4G{V)xIH-c31AI?>0`zDD6aP=fBf)&Mz3Z;kPX8uzV8U!h%>EeraKcRjB{9-sa*Y`hKJJKk53(!tJP1f#*eZL|KviQ|4r_GEha=+F5su; zrow~K-T$Ot_}iubx6{|S+893i{Ces7bIY-Kf-M@l0rA4aq&W+Px$-w!Xkj&jvD^bJ z<^7y5&|G>bum5p@7EgE&ewoF9c^9?()@IA_&pzI0Cp1`f)BUVYF>iJnlR7d)w)rw8x{MYVxK?rG!kWfP}T=!eNfit&sd+oJ+}Rtb^Q3Vr@v=S z7dyJeHB%PWvHi7mF;=1eS7yR|2YsJM=5tE~e|eqV987qPat?xpSEK*2HC>kf;Cc?` z`~u}11YcchcW(XD?< zm^nc$1p*ea_yvpiJ^TaBW${%f)YPTrU6}uBDEqb6y^3}G=UVr7ggQU=fOr+^f2H+& z2YsKno^OO=ese#0a2xNphGKpnfA!!t-mipWpOXFytX&>ZtV-PHeEmi!h9Yj!ihpX) z|7y;m;zW`qc`c>B?TP;eTlY0jf{GJiX~OZue>INyS}$f5>HmVeK*h1BegJ4qiyrRV zFHWyw9Z_-YvY+nybjb5_?tUYVMGYy!hJAS)yXtu7=Y0J}H1;W9f5B@0*N8)fil|U= z`FOz3t=9S-ZyGPZ$+rV>RH*Z9XMze9QC)$B5wAZH%6_e*iV79s!1`lH^`98>od1FpsE7&`mq&4^ zP!TkbZ;zU+63YH##`=54^p=b8-w}v^Ezm%5cUhqE9o?|c9U_Xm%jYL7aP!ms&pvNJ zaTmqi`7r9=XLtYh0R~jtLba`bYuiHUE=qU*t-AZS`-Q)qK2+QPC)@ttF8#lqK9qK% zwDa$Msy@H_qSXAIekTK{y%YGb`1qY1R)?x1U`hdSaFQGCA{EeycPg2=d~IZ zFZqWO@cn=1=ST1NqvykQS>WmKtk>UuFHTGdA_qhkqC-#(KpVg)044?mkpVCQ>;#Yi zPyx^bumQLT;1BSn>)>*?fi>ysz>6mx06$yK|M%!UZ8wMu-;T!9FV{y%E|kwGon&MuQXp z)UnTMf}J+VJ_4c5m3(Bbl6S&|Q!g)AW1(znKp#4w2Mz4u zdJnGSS*2{Qxw6q$mW@U^Pc`N8g6Aow1m`b(WTD+hKOjQ^FW0SE$fbfL08exw6Tll? zz?b85TQjH$aN7cKTW^kAW+1IQ_Z@te1;jdcxgumBI)qFDOKL8aw+_&+4IKkq)&`}S ztdJTtv;j7t+%=dG0tfx%d}0mA(T20pAY4!?3uLr$7?}h1&?1d%AtlHFl(i9*Y5-XR zO0~h6XwaOxKzpE#Kqeo+ejTv7^U_`q$%8Xj*5|}r8Ss_sA$(|kAvS~-M8LNR0qYVU zv|aQLgh22uD}w;=X%c80TwhpYmN>Ii5(EiCbUjWWg z{NjZKR7ifN*954Co3JRfd9If8EjrYu1LWa?I_d&h>VSK}wa@|EAwUCMV^R>;&9y@k zxb106lK_1qnJa;Hj!%6&a64Gy6Wro(ATkL5`~aj00D<#KfmG1I9_H(OTUt6Fz8VMG z0M2hjXaWG0gl6&YKxYx$b2@cQ9Hs*gIt6&K12PBDo8#LnIm83?*J6^f^IF(ChDai%3Bk->er)Ons<8?i=?^2**gngUYi35p3UOpCdKH*i9 zd}yUSHjHOx&ft|>E4(J}zI27JrC6-C6+QJn%^{(_)`{y3%|6NLB0FVcrNyw%B|W)X z-E((BqL^$CYssUnBdp=fmqN=A=N_D(xZk0v%Tvj7_wB|HxE@4#_t#z0syMNcVn_Plq6VIKQB*ZvEc=PDf)s`ALU*sw+Th_Qt^LQUJ8^*2+!qUwGnSA>c4 z82fy9|5&upZ2aYkrfcmR$pY_T<~56nlgj6cK9}&J=y-BV>xTyhr1` z?-nDI^4QC6$>lgFu(XO}nDAaZVDSD}=v}rJrA(i@y7#JSpQk*WsF?v(`Mjb=r~Jvz zfeN|l*8_E!)ayn`ocVwSYBbU6~9{-bMn^CdW`)pL$-M$J@k@C6>lN9)-k$ zD0dg#t_pco+uCxD_t`s=mpAvXKg<$@wccH~oYgIDz=0)->TZJ+bpgxCrXtd|PCN4A zWKZIE`<_}VIMToLxe%A~W@AkX^UF>BEN?U)F&GM6mQ?L{F}A%~yqlR32aRua;%n-% zxEnykdIh)=vek&^fXVJTTydF-mn3V$q{uo;+i9ah5r4aLLa9l{HogSAw(}br*N4UG z9hdlEasd0<`|ACnKK`_n9dWLTl=`=KBkLeJ}sr9|l&VFu2!zwQ*ajoECD=WKgLa)OW9qdg)HM z`9XL7HncRc`{sppk9cnQ5Zc%V^x2b4x~&OVPan80+%l+Mziv?B@aAzBEN*izLgPoz zUtAAfHz?L5Kw^%-KVq_P=25|$81ZCZ3WWy+DoNcj*=4L;Tf>5^A2! zFhis_A8KZ*cE7Z6#yKNFPi{~mKGta)7#ts>%_D!>VDL?xZHz8@!bF0JnaR|L6OE+X zM*58#r)Ho#`XF5}MP*kJql#AXgjwD_!ML$I!lpKc%5DwL_DkM7pKr-lS??cKzKK|3 zEOsEWf27;5YyxLZ(xwoN#LH_h7oEQdPQ+l7f9&E`eT*SK(QDL<`Sn|0 z5HP-7SKuLL8LikAaBo0sR?s?Pvx^OTRqPhFqq)3+40NvEM4ZGs#w-WA41JPR6J$d& z1RXz2b-sv-l5;AbREb~rScpW<`Z2GF<50yATX44X)L?m%>5KT1W^61%NQGx7hicNcI`ZSTUzpOF-lHV^|4 zu@DOk6bn%d48&Fx5fKAaFhRub!tU2l50?_3aliqeQlf zOC41?8}`;mzk9aCXL*Ue$*e3aA{>+i}Wa zhan5IKkw;O!S3KLN89*~*)xrb&pZD}{P4o}i%n^qc3AytAJUJqjk;w1z_G~a!S

)pL86 zD+c|~=3VAID7r{ZmrYwoOe);V$kd~JgDUB5#Or)cs~P!Uwte~ZYuA@%jy>M)i8wsy z+Q_+=v)TQ=apuGg)-9U)Mzyuto_>St6-~9@?(8}L#C7ui4TW9C9liCY_WH7i)_>eF z;Ys^PzkeS3rs|CHoA0i7zq)+M(WteZpOtYKYqqz~t)9!*Kg+uK)JF5+r)q55bn8>o zu#CIsH92^qcke0AJ{L=*wQgo%X;#6luV>7~aW&?bj_|yDY*@z&AC|OQ5Pg}DE6t6gB`Pup_0yjnN>#{gt!UX-fPrQ9vPn+blIHT2@ z;5q5ydXyYEv!Tzz(c3a*^fetiAaY0bvxcTK$A=iK_cd#`zR~h$z0+pi)i}fKZZ5CJ z&hIg)>9`~BCdIek<l7L9A2qPLozvR*R&ljP*LEK^(0Kcy zh35GhnyfKcWqCKl@S*oMZ>qYee9ZwZHf5>j_Hg(HgWIb&8E)y^e&(v>Hf~*NGzwex zaL-uVSq%?Q+qtA{lS)M=mTD5#)pFYIOlwE`oHFQT8$2mjlSoVR)3NzaRyk|$m?pPb zuG)vk?KvKzH+LsEz`4*haYW;rhB-4v+zAVcyzj~K9 z;X~S(&3iKFScS25ZJHjbvoY@F)DfEx2hF@195f)djl)5cn>YP4Ht<|n$^Gb(N{0g5 zp7p%$Q>lja(Yi<7{Jrb>Zkd?H)8+8aMlJ>G-*K;Z*urbmnf%fDu0D#p+@#IbOJy8Z zSXn$T9W#FMM>B)+bJjmRGSk^M{LsoXkC&cZn(eS=y4RDYL;BT=Zc}YT+m46Z_FGq~ z^;ZA8MqA!AH1!>Gce|f=qiHb@_FF%Gt{GH0QyKl^i*Do_*Wvvr`wJH7{nu3w)*FAo zYjlD18#~=g8@g#^)tJks`C@A2asL#sYe|oJ750SJH*J4?#rUY0O=q6?XZH%U4D^3{ zdd!3}(F0}=ef-ILUHH^p6%Q9|U%!$|z9OC1`}yxZYg>I&i>bTj-p_liR`yjJ%`_*w z`!9Jt`rUC)U#zTC?Z;2j zFLyC^9T>QMQs|Zm(+*k(6}eWp_2MkX9`WNkPTtjiFn{PMUKp4iDOchxadPYm(@J$8ACIV0=cahx;1{DCbeeBO2LIrGI7P4qse zZ7nRK&Wi2$T9d!7`CKB);E;kjGB*8EyL`mO{swtw|90d?&MAwl>mO>odGp@u-=eNb zQ-A?{Y6Dzemh4BzaJN>qKL(I;#P|K(g-Fk2?%< z6cHL8=IL12wUDEIa9B`y%h0e8Psh6cl?s$_w2z1k3~L$KE<7yQ)3JMSgk#w<=}k%p zdIYuW&)+)SH`@l&4+bN`A%T9q^ z3AlZ`f<+1yDps(4U}%`5eT$I7o{sGz3p?6JcH>#gwmfSQQbcVlqPFp@Wm}#F1_klq zhF2s#(Qe`IiCezCrl@vHQSEEov|HS?Timr<+_hVZX}1(}v~S%mG^{N#QV)*yt-{;+ zByP|?b1Ya!rJgz;plYQT+G8R%<1(JWhIDp=H)^9j24XcX;uFj$2JWbXPEcnPhhQ=G z<1OqsTj7bCXpCTV$8gNRDjdNLyoMp?B{HJ`ybz4un1T(soLj@ERqfALe)QR@e*U=C zK<8E!`j4+Q)wxxjA^GvOW;(Z8>D-!C=ho~xx7z63YNvB+Zk=26=-irD=hpl>x4P)u z>Z)_AI-CDv>J-t8;5PomF~iLPqzIYU@PrG?6QYbo*Vxq7)uFO@=yN*7fcl2h?YjXDmkgd<>o zgOH1W_QQuH{(=FVaw|ii`XulKbRdacNZ>K}OcGb*0Q0evM63mDsnnD82B^l}y z@!LeZ3(=jxPF@g6uY&Yxg+yk%iNtw`yrM^LSb=`DvdDQKk?mDEj_Sh(t{lsXR?&<8+adGub|y?QcL1gGAk-}R=itb<*aTl|+dqjrq6S3US`Q9TU`HzVVKEcfGDUtQ38Sb59 zcJU%%UuOF?k^VOrTil}5-4<#7fa88h`aKcp`%L8F3z2{~B9Gq@_YainPa=Eq>mUSX=4M&isha>7C*W2`jN zpTUh+MvWZHq>**D8hMyiBWvw7a?L>_MV&M#-4=u@k#N zvdUhNv_jI#K^y`}EJtw`=Wzu$aTg?|kc{#g@9`08X&lKS>5&1Mkpo0mh+4vUuOyG2 zsDR3-0e>_{3k0Jz+Mpf65rNK#!XS*pL`=d|%)~q_#!9TmR_wxF?8kAO#2KjZSXm~qo%rO#@mHao;W)BaWsHs`=vbYy1~Xrg$w)^#QlyrM zA3h_THf6XU{8JV%#k(!HlhwHWFq zZK7i=e@C<4#1lRGP;X$^k9vsG{ppVfkQW1~+qi_-LA-V_={|iBw1DG6rfw3s5AV(Nb;z}al`30lJ7Bqu*U`7QV-|dOQXY_L7v&docGFg%91Y}onC&C2 zu@=>SqdXoYU2p_>5Ale&!;~Q!&Zb9c`%cgvU@0!+!b#eLQyj}_`tmc>xwDjC7@ed3 zV-0ehr#xMt9$%*H;Uy|up>4TJS-M6&yiU1C@f-99Pz`cj&XM= zD|cyA?@`t;^gj8EW)G+nc#87z)En%E#Y58b5i6k(infnM7CvEo@|3m#O`lOOQ204{ zfR8Bgg7yiQ@cAX>=oR6^{xz?EL%)vTx1{SkUWZ3;c~3l``JHlrFIe%Bw&WB27Z!h^ zJ^D&H75Ys2;sx}@7qFK8_#qY>XvBd2*a=2P8u3I9`sKP4@A-HPRMaVNO4t znU1LjM&LS@nQO$DK07z&V67!#BbYwCmoC0#0F4EAa_rMrBw}UXV)yM$M z&7zS@$YH0E2{5Fuzm8>&j1iEBe*ZFJa%rTzvqlCYLvD>6fngrT1enTrAboy~)MG4g zxB!1=44}pVlgL`Cr%+Xd>LH6jb&w@k4yu7{z$R<~MO!G^vKe z9*T=lJcMGwnh?qXC<;PR5Q>11<*e!;pYa)FdBRtVuXi%ur0SAv^3r775uSIYIUa*&<|# zkR3vHNIsAyLY4?aGa);q5DKFRiozYmP#h)Tfl?@qGAN63C=V}Ggf}X|2USoN)leP2 zsEJyr4L{UDUDQK;G(bZ%LSr<6I(Hd>W(Y(OTA~#~5Q?^Fk1%vVM|46Yx}Yn%p*wmY z8a)w%UWi3+^g&DW2guUg8yA;|<>89e&40e8OjZ z!BI~9A4+b!V5!8yzv@k(Bq=!1UY7Prn!5TKmh)l=~bwWQ2?4VZCWJeCz!vT(P zLM}KXH`Hp{e8>+M6o4xVp)lN_&gm9M33#A1%Ag#|qawUf2|lQTYN(DHsEOL}gE}|t zk9uf;hG>MQ2tYG5M+*d@C0Zc_tmq3M@Ojh%@K%17j#88bVoE|&}>?%vz5a#-{drOf#FW z8KzpN`~I50x8=w1(@~%(2nlBzv-6k>E^3>@IH>)Z(`}Ss3px0ltd{oF6{@P#164l( zAA6;oWPf47W8zuRuM!&@tHxp$oX}14e7=KvX8c^0U-d>c`Cu-3&nvRsT=bvkVVg$M zK34&x{7EAw3GWkCN%^CC-z_SSdS4v>*S>EF+eYz%4HUKu6i)5?O7lJqM_g1)pHmm4 z7v1ts%^;czI%$5}OJ8w(QHcMIOxQL`Pd$uIcpjJVd~w2aUYmx0>JX%?LE>bd#d88@ zqx!K-c&?H}9a(c8SL-JpxoV2iP*sCC0w=cjA}aa`11;6OmXwkV7C7mb(A^qKS@x#V z!8{=~QpPIfe%yFvg{1wIP1=w4y^XWbJ<=@X&z9H}qAnuimBNpFC_e-KFB6JnYPboj47a7|&)#iN|=J zCZ_{*hn@L{u+^kl%CKz-J8?2CG3>IGTh$cmQ8H2w+cqKWM(nwQCNWJ^_tA&ru0xur z%283H5<_)}nuOt<1s$Nu@2?t*WI49Sj?Rcw-^*hORp3&Tj4-xnQ(bjV>gc`LgQ{Gy z$imU9?K=qCBB@MRQ$k7jgl)}v{GMhL$*e~-_pPa?$@laqM2L zy=v2}IPX_=!pKHr&(y8_kPwuHbJmfLu9+H<6HF2CW$Fh-QYWvS~tReAmAP#oq z+Tav>`krIizU5evq*km>iZoFN<>J`cbpZX>*OqKV9$D_L9yi-kZFaRAHoiuRmL~?RD;?_a^Vj(vk0K zyY^WAS}&O{Ay<=!t(L*4GL<~+^n{(bJm!0gua41=_@<2K_mZP7efnb$tQOOx4BL#b z)pqT1Tab1tY}HGq9=2IR*vZ?We1x%^k0X zw*S1gsh;EKy`*_Uypr$LMaNz(*sI$9^S%DHUQ&&Y6KjyR1uKxW*T2((T}?{UYkwh4 z)#9j>X=+LA)b^jpt}7!z)%UBik~Lw_{(bcOy&PKp5WB*EF?J`C##PDN=bH2bo|OAJ zS@_V%W6FA}+6QeNFOsw$ZP`xV>+7oH$;)$5-rMJw%X9Kx|3r!uQF*M6PTT9dr8qj( zC#!xy^~p9r>Gj={-ur8{P3=MTp=u9>c}$sh$;-W3AeXXaSWz<6_Mevw)$0#V>h)ic ziplr%y<}MZP}|f3yp(&jX0K}d&-Z$qF~&#A#|nEsfAHB)_g<}kD32wQ_L_QaODu`H z#`&J?Z$0A?b7}H>tMm(^1=!$4)JXOc}cj#7=GhdF=WUJ11f_hS)t+AA)p_hBEw6 zV$_1mlzYv{Ue)%W?^TV#W+#ooOenLu_sZp_jKNm_!Wc}Cz5n-)!PK!PKU!@Mq8)>! zNlF)Onf~t@gV`tTN884#F<2X@F_=e+Pw?6nP&JR?<=0lDF;IQoQ$9 z#&7yb?@d0&(okm9c5OWV+W3u$-xOmk107+fA#Al>8@3vYXvc4Vvz=AkcCuedNf|p6VyCwMJa%eaT9%QU8kcfE zw?FLZdu=oMAx-Uc?A4ULs_j4DtLhKb*|QL{KRbJ7%3k%J-{Di0>Zjv4HtpH7Eo{5V zk?*IU9PLE>lg^&iW$*vJBe#TUq-3Mplp{C)q;%2d`mc=KQpPG}uGb(1v}dvECGE$W z{#c!Uj71J4o?|j6eaA5t3n6{3=45lSBmSPNITF$}d8q0lAyt-=hid;V)Q*I@ijm!Q zYQ)hLt>pJm?SBYWU1XFpREKY&jv*gasQQ$?)I)XnA=E}XLUsHWsyfy_)LM0^#I#gv zt>X`&Hr5fUO2EW)Rma+!P-_z6)Ko*wnGkAg-lfegb&-@xfuiJ=>O5oN^cU*M2GT_B z@14`1?a%3lSg6adQl@w=_L&%q2-3!pvp+XT8&5SUrn7Y9`XN+x`ButMoxg=Tmb6i! z8gk63hwA)8sLgeRO0i9LthEWTD=kGkMg+Q#HTMsp2I>ek&$m!jKB`cs(DJ1oYMviL z4bl;6-fy9*GWUY?%tz?yQ~}qSu6ch5wWW?w^L-07j?h%7)i~bNL(P{EYVtDIDk;?D zWiJ0;EOTmq)?`BZRAZ6h;1kVSg3Kb*rSlOvC=v^>lS;GgKu_>{ZorL(T#6 z+gSAV7!0x2AaNbT_p`Slq^1$A#(he6D7mPX;iz*O+8Z zO!%80H|}c1>(!cylDxMl?^a=`_xy(;lV*sHpj^bqIl+fwUHshN+cnWY#%ELZJ1HLIEvi+O1u+EZIUxv((O zjYW4QSoKqy(MuWTO3JcmaRduHx3hTl6cy|xBX(<~G!3N^kh&ToiM^lCiBZ#9}0TyNx$GOC@B5~Y9 z9Q}yndg9oRICdnCuEcRYakM8s#Sn`H_zmx%jU(M-!m*7bQj;(l)8NhVRKaniA)@*) zgb~c~8o7vkZWKdtltvkNqbh2k4RH#?5RAZd?1#$Fqc{c?Z$A>PG5TOIYLR&A{KZZZ z@C9Ds9p2+3TuG-AXhNcn!e}hQ5uAqyWh4yY7>K3V099_(YAEidDm74;OmN2-Ou+Bp zPNOm%3($(pX@mYW%!4ojqv6aNpu8xIV%S7JZ9{!NMl{BD?165B$vidJZij_88* z*o*@>iP7ZsI9M@k&Vm*Q#$XJ`Y|O&}9Kl6A#Zbz}NIbw}RaU4EX0Sp=sQU39_LLzf z6hj%L&CT(_7QPsTiI|F+n2*KSjT2~4Kd&1f%?{vV#K(;yhlWAN6|-R6WOvsOdsDRpNhe)i&R@jx`lPi2ma?A)tPXv3g zkQm!=61hrIUh%9n@rSV|@5L_cgK8&d;U(TewV4&*k0uB}J47HF{V*KkFbk`(9lzl` zikBlTF#yA`8r!f3r|}tvCNR%)Pk)17?-DSP9ut1VvE_?a>i6Diane`_R9jQx!f-q7OzR4wG>m zx3RG*`GFGEcn^FKh!$vr7!1OA%*1SLz((xEZk)kc+`${X$M4XuPFf)?OpyVVQ4QX{ zls(i&W5i$(($?Vf6AHl_+i?_+@dfv5QWo$UdbLP1w18=C+6x@SX`J+~ISr360XoJ;Qj~DnI>FbiWut#B(hc8;yBaJZ~8?X;&a28MS6mMZt zpLPX~aDgidqX;}v7Iol{CI~`@4XQ3X{|7tPTY zUC{$k7>ovCT=0ah=#R;mjSS(W3;JLbmS7F+I*amem|1ms3JYM4~OuCbo$b+0cXs_Xg zYKX!>tiefKk0MWzBbqTLO2HR3&-!pxqG?Qj8ap&vtCferE^ABsTTM4%}G&mi+f_lc!!)r`8$fD4*bys zEfIp&2tzLn!8u$))?w@idEg3P)W=0UKs;nP?Hkg<5;n*HdlW`#48tg#!Z}<+JWNLL zdd$T_Y{L;8#RV9QB!6)M4-q_yG{g=Z!eJc430%ib+(A4(;xlA4#|ArOMQ(&68vAh$ z#o|b7sGCXTM*)<=RvgA_=#8cP;4D7CU>xIOn85;>kOg_*3Qv@QH)YnNPWRNJi=3afXigYrN}yk=NN-Y$UK!gi<~Hkk|+;v z_@N0}qBS}r7K1SyQ!o$9unOC85T|esckmQ%@c~BDI9Avp2MWL)W#I)s_@faTBN(CR zh8~DPZw$duOvN;8!8RPgVVuNS+`uEe#4Ef%6i44es5OhFCbiroqz-}DDQJg~NS&T2>fc$VlVRS_ldSL`cVg**> z1Ww{L-a<2*G6qXnp&$yu7c~%uaE!(nOu!UO#|+HIJS@X<9LEVfM6Nl62OrcyeS{zs zozV?_(I2A_hpAYA)mVpJ*o*VHg(rA{kN5(UxuhR5!4^))4G)xpH+71m-sHsLCs;0rYKspBw2ew2bQ zYQYb6(H(;^8PhNWv#}rN5s$}siWkVVfHSbtk)3+GNA-Yq6+*_7xmB-#WoN&YHs9jsDt(hLkvc3B7HFpo3RCZa2UsM9A^=^ znK1>9;S?_62C{9TO~obL#eEoWrGJAZY>^e&kP|oX3MIF3T<}IU496l2*v^;*6T7e<2XO}Ha1jsj4n{lZqu{fXdIG;)qy+-d5(}{lM{xrAc2j-rY7pHI9N~<-D1{0LL|1f2G=^dZW?})BU^lMg z4c_4c)ZJvPV1rD^ihRhAvM2{1)IuHjqY;`Q1fgh$X!L{I0m=``p%MmT7M5WpuHX&^ z9HKqIBuv8tyuus2#|Ic6Cf{KJD`Y@sxT7>Gzz4PAhq|bTrf7~xM4>nOVgN>BI%Z%t z7GNFL;~);<6fWT^uHg>u;RRmd13tm%2*(FYI3O?7UH0^m4p#6-2SlSUF5?r7j}b4l zMFd(Nr_DiEMBy;b;R3GXCLZ7+KHwAdPS6e^J0Z76UOKTd^Jc za0U+$kLP#=!&CGDC<-@}LK)P89~vS8z0n7w5r@fGgYDRl^SFQ;c!Ae=3%%2{2iOI# zGqfM54u6E6CB3j8#}Rdow8HH3jJL2I$8a2{a2gkJ8S!X(fw-eT;xO_eeGoQc7c`g1 zFF3;k{V^7^umUTw7yED&XYmk^@Cxb%nMO#9jEKb`9K$JG!Y$m!Bka0DULwa;$_P&5 z1|DA{Jz#vDa)j(~fFtrCA3U)N8*m3N@azVCE-Kul9-IQBmSH*8Vm%JR^BMa^2lPbi=k&?wjG-8f z8F+v%X!wHHAqH`{j#n^y$uYtm72%Cq@I!q>VG!c+6ir^SADqP{#N#bmyr!-r3=!yz zei(qE7=_uGi$z$Cz1W8nIEC}b`-U+G`eQ8K!r(3GfEBofCwPX>82yeu4+n7p*WPnX zc!bw@i?2xYJMAwrA{U%d5GCP*D)2`=G{#bFLZ%PA9t*Jy*YOO`@e!Yq<|FMUHsb{h zKT$_v1smAH9z{?TWl#>@sEn$ph8n1gRtP~SM4~(TVgQC?7G^`;Y;OhDVjFg1KQ7}6 z?js&g@d;mX;xl;%!!NWU$d588i>j!O+A#h~KZA;BfQD#^R%i=*(QqphjnqdYG)8-b zp*x}wiy@eTsaSwTSdI-ij3YRYi@1h+_<)Zvu*Y-9l~L$&-^ZsIYX!o*M`rpSoQ$c7wnL{2y( z4~oJawNM*P5P%@GM+bC5PxQhxOvfTD!AfkxR&2*X97a4I;tk&66HJUq2b{(Qq&3z^ zI?TgLtj2Miz-3&;Ev!qck#!MPEw~vY23Zn!jDxyqi3XSY*_Q4YH#Ej^I3+WFo!L5wUQ}Oc}viY`{hw!BJenRm9^RzTlKCOq^RNXwa2^+-*2EY|))e&ke@ZD|qyC@# zHQK)?-=>}?e_hJw+V}iVc+?*2|Ab@t_ZWUGlmDj4ziIMs8Thvh{HMx*wjcZ7y1)Na zy#C$Kuh&^^+0#D$Z%#wI6l2f-&2jy!ar^%@P5x8m{Xemv|HM9$m#6>q{*v$e-{*;K z{J(kd>wVq-jk2T7C-wMWwSRrWN+XHC)w4uKJSIN-*?#nx$YJgvE$aj@I4jT5#h!ER zIDnIw;lz1%+`)aQ>s>72g-Tcmb=|1CcI6}j@~}n+>e`j5P}i)Shq_iJLtf@eQ4i|c zlrd1(q=e#pe&%AJu00tGb*+d=5$0hK0CjE1AgF6YWK?|t0_-Y-#T2rsq z)2p@eYMs1VORm+t2y^=P;>2So?XqEtNC&@m#yZp)ts}MZ&q``3G=|W z66b5xoT{2nRdb(e-c!vHs`)`RccA76H7}#)Uevscnp;uxDr&Ao&6B9P5H$~?<~r0ohnmAs z^A~E4Ld{R8IS4iXpym|Re1e)YQ1b=pY?u##I;XGB=c{w?v#=BD{JT2$uFkuwbM5Lp zyE>Py&ZBG3l{e3EPVL;Z_Iz~Wx#Xg>cmMjYNdwiJ{Qr`c z1}WnDy${v)rD>RNP(K!+3rP6Fo4!W0qflRVqOO%zUudGPmp+z>HUgC#nWx5Gyn(v5 zv@ND%3DmWu&rmBj{SOM!FV@BCytJW(X%CCAzMvTEGw=>|i_MX-}Z8$6Ez;J)SM5U?I}`&<>*x8siI0>GNW$vMvF8aT2D!w7pQ* zy)}Wl?rkvCb#GQs*Sj6V_?onXP}j9(!Xli3lON$DjKrG?b^Y1}%V5fuLwqHQ*o8hH>Uy6g_#K715x4Ff zAGTvZ)O9?Aa1{@+I-33k>N=fesMm`)VJc=qU1xI#y<^EgZ12r6A$uRn7aI0uT?H=S z3)FQr!*LkuTAGRj7^k7eK*okp*TYP~Y23iqL5yhzlfI~qz8HliLl{RuU9a*Uw}w*o zhLK+=h+uTW#Nm{8sOv`_!f^zDN8VA4i*O{4u@Z8PB`**+p8P|)iL5_@x_%-AhLcGz z_@fc#V-0Ripv2y)t;a2aRXB$+dpIt9*w20rFup=%sP(C< zpw_3}#l?e+7oi<{{dR=-L9OewhFZ_r4{9B!CDc03NUX&!v^dQgEgZyAyu$BL>kHM| z!ZK$`E2#B^gOTGrbqd8Vu&xF(FdsKzc#$%ID%gv|@Vi95V(ewo32L36TH`nV3hgV@ zy1ttTy~gJl486{pLwte34dRVpjKD%X!&g+eN!Zwf!?<~iINxTBjQmjR@YEW-c6W#` z)VjM3D0`21q3M0DZ@~$iht&h(gy!ghPtc3!HHgE7hy4E$^$+TcO4YhJwINDyfhF~;~;03ZgXN(H9&TS@^z928K;uUos{oc~v;L|(O@;&X+2l59a zKQd;=;?I;1sC7^#xCOJXqyd~6Q!dr8&IT5G8YvC6ey9Vs;SgRi&dk9$vnKp85>v6v zh`D~K^*QO02kscj8k{Y7g!gE~TAKk_k3H~a4NFJN!hEQ8Ef1l`xYGh*tX-LmLpTm| z#-Ul!i?u3Ca2YpX&svoNSjt+G>q+ZLDzT1aIkwU)`h)vjxLpXvnxP;62h)+oG#Fx9m4elrgk0jN2^haF;qZL}C6Cx3XKIo6( z7=clkfa#cp1z3p1*nl0_gF`riQ#g&YIENQ_h2QZ3pP-+M&k)FntjGpe6hvLrLjyEK zbF@GNBGCn1p}vqd7I7GZ>6n38Sb}9(i*4A6LpY41ID@OWf%|xXhj<5#GhfgPGgu%a zGQk#EP!#Sch0-X4vZ#mpXo>)YqXVK44Yh`*canZQDr!xQ`a<2wn1bb4fpyr1o!Eu_ zIF3_L-}0(KTA08J>I-?DksAe32*pqmzNmp9w1jpojyfY{z&KZphLfMAQvXjW@$V_O z{`>zQQ|jL|`FWbC&zk?H(|;}v5^K8d{b_5y$`AEieezBDd01cF&yVeikLvlqek~33 zME!(g0oH>-ZNk@-tFsD)j2SiJHZ*DZw1I>>f|>9s6xut14K`&Yh!Gfx@tAY3OMb$2iIHLU7x zY58+=WhL4`eeJ5cQ<}Q7hq|jrJ(QlnRT(Ic3h+S{ygtO)5EwFgcY^lnpRSCy$6-7s zKz&VX6KJnOQD4;>4eeDM_KaE`kQ;g6f&wTCH_YQqPX-P(R-b*%Obq*@i5d=#ffa{o6+aUqPV)F>_jx<+v$*{*98w}|b3Hi|pL-*t`R zKC=C&*5)8^z7!?`jmc6uL%nN7$}w6c^9-KO4nmW(1{c6z9qI|5rzG z+BQ7#QMJ^nE!RH&adh{6yXu9%|1a^m_O*$BOKsctSABoKc5f*kwXgkak7~a^&TOhJ ziF^9@@$aXDkqj!gKzyeA?fhz<@}GtH8r@01aE47}%D;Ryw~%SYoiaopEpkBD-^#>X z%G%<=Q#W0I`|32hi0&{sW8&jo{f$? zmi;DvNeFnR0Rhi6XwLs@R@Em^eFD`_8DMy-0cJ=Du!Hfp06Q2HUc*Ei!M0KV0x4>3f6WB`Ye-PE!jNbx_%}8Le8UJBaYuSDau$C{{-@=@{-u_#dDL#qy_J4L^9!|@AC$Rw?u3F~dw9I#Yp)l*H zCI7a}|F+B&i^LB6XZ`KpXqm-A^}2~at&^UnPw2c=VU9>qm?OUYp`T5|`&<*o0Q#eT z494V71{L3%w=}#zF^n{EsfD4&5!Dk%Jm1C<)xRbWBTZ5mlIn5j9zG-wL+__?(LHKN z9)|u;xae+4lZRpS)3~VemK2w7BY{80#W*R9 z)JL=bbO4|&yvZK_S(}+GAnnt?KbqBsknHjAk7knvqJ8?$Mzh*bl07CK-QO9_CJRLS zG%+yUrL`u5_S6UN+7OaG{<8reO8Fq*JH(lK)?w zf=m8?b-FJ3|JCWj?>`F)snc@b|IZ>zP78kj|AIna|MW*4(I0=lepTD7B%{B}d_764 zZvd7x=<9lJCB~1c=F#<+7qv{`^&DeTIy(6_a#!Fo&+^>d3a*XLT4Ny3Obuj7 zWdp)8kcv?T^2yv###S+u&ovAsb6z8f%x@%(${NYcZbmZM+*qn+G?vV{jYU(C>D#Kt zl0C>+@`M=6v$SbNzj9j1-Xg86<13e{$CyYfYg3t9(p1itHkF#`(#hPc=_J2LIy6MFy!*BZKs;nL$c+$RG|e8DwHWMrj$5QI4Br zl5n$3Qnx}TXrLBY)u$5tzY-LhSTUneji#!d=A`9AQ zk@1v`110RFZAClz&D&0N|*SYX`ot%26C_9mTn@qdY3&C}}(# zWudR5TsFuloo#Z;zObAUZ|5YJt2)U?KPRy+lS@J^ouy|9XK7p2S%%egmage@OShuA zWp2+rd~si1sa+?pwBQ?nwsy=bLGJlvT(f+#0h`^;S-hNp~-_LL!-GBUVi z8Cg)b3}5b1MxOKSJP#X|l}D}0N}tfOk|jquY2a2)W)>(fQJ&?chI0j(RkDKIX@AV$Dv6I}CF$9^l9UUmEJeaA%R^S{cX9EN*~NUM1FQCLd-zDh`aV*%k&nCz z@sW(JePmO(j|5s*kt&(0FlApw8ar2!gBhzz>mpU9UeBs5;H)M&GgX(5ZL7;bYhMYl z^_9LwedYHCzS1htSJp@P$~D6plHRPQ)HA6imE3Ddp1idsU)|agZRaPyXY&(hA3xcN z>FxYvP>wnh?^{OvaT;l5lO1tKPvcaf@OwZ9mj^=D3dGfW8Y9>Ll+apNQ_y);qzUk#f^B{SewWWOK zn_8l(v=q@_X@hkyTpBWB*pP3$5f(Xe+7Jwv`Nz z36YisTFVyGP_bwnD$iSnN}q0_5*HmReNEcPq55s)M#DC8p?X_!ZqZg8tlLSg0`0^i zu$`>z)s8Q(YcJlm?PZ`_d(kf!CM~?fu7NHNb2V&LCFnl|eoOEPql7G*j~9is@jS1>}#R*jI1(GkQaLR#BK zN~_|LVpS?qyembDXQN1&-Xv1yb&Ql!T_WXj-p&&4(OClgI?J(UouyCtE^@M47ul1g zt8D4eReA<=lZ@frq-9h$S!d8)3fgp+j(kf=fzsXOW%=&1Ow&UKX6+#ZxMgA*>nNF= zB})8qM@f91C@Ja@B?0B5WM$`je5$cnmuJ{NKYAV6(iY8 z#E7e3j6~Osk>QnkiCv9eQp~@X6qHzb=^QHs3&hIv7O}F!xVIGX=`9P}_m%=B`bc!c zKH_EHS28r}%eu;b(%Gt?Ty^Ltk(K+&smA@JNB4fB7u8Q{Gs*A>-vYzLmlu=L9Ls)fLB0CrOsj zNz$X!BspR>S)Sr_smbD7eX?9=Hd)H%nj#y#r-)1N6q%qmP4?JMlVgRZiN5u8c~yM6 ze5yTNCU%-GX-sE`k=YC>SY(FuFEvB5ww)oZ+s%-Nxo1k<{4-@qU7SN|HklVEu$fOJl-@O(`C}mbGe+Yv|MJ?SS|x&mdm!B zE4WB&g&aUf{}mD)yh5g0tdv(-R?2C+l`_PBrR46kQugOuCDqHVlI#svNl=qjV%B~Y zH%?qFUO}s6kn+lR_>HrCu{1jBahdMzU6v3?6h7cx~`WT zrPfQopbe6@^#-vs+9)qeZIn3UO_DC}CRtE?llbUwmI_9jrBI>G5?5-o1ozr3yN$Pq zwfz<`DZE9T>THqnfm@_q$QJ3#UFjF3+se%?w@P`ttrAmYt5o#eD)(A$75|8>;@fSj zEYGk_8fMuhl^bjmtG3%DZO?7uRAIY}blxHB3h$7+Rd>kZPCI04<(=}b*-p6_vQyHg z-6ip+yJULiUDC49E=d=-ORih)mSF$gGP(Y4ITx^793pqiZ1X+x&~}f^>$XS2toKS} z_Pw&d&R&@rvsW6M?vrB<`{an;Z<4d{Z(_mS*#qtOOX*tsML%Z0)XQ-|`uZJ^$+-{8 z?m7o$a^OL^mi~}L*c=io=R@LO`jDKiaY&{F9Fh;Ahh%)7!?K7wuCJ+eSjN{mEDbvz zmMF&~lD6#;^74orsdZF-YkO3jU5-geiDQ!9`xqC(9g{vSkBLR$;}Y$8T-Mh=&Y0r3 zY~ar1tBReF-@;BvIqpFI*7>BEa_8`srB2H1f~Um2G`OpGLyOaVnc`_V?{!+@E1eeG z+NWi1{xfp6*cs6nos~L%XJvQevyw5>Ik}$aoS3+tlT1C&adXV`vNZd7=~w1FW5Dw= z!Q+CgYJEZ0Wx6OQs$Z1dJub@PG?!@CE=#S3mnC10D^jAs70FidifpTPMXt2GB8FjC zWPjePGS>5|G%bHs%D1>Gi-WJqtkA1+&f=PEx4R~R`LD@b*K2a5*m*)23aQDGoPejnfS|Qt*aoLT|`@{hPAg>89*+zbQtg zZ%V(0HzmO4mRu`&OY(T!l6*n8q-3vK5@mE->Xp1Lq4jTzli?jX>w>y>#Ln%mG_QSE zjMCquExspZqwk5c(R~^2bzcUA+?QEh9*90y+;p&rm+hFJH(sigjh8)D;$>J!yzGmK zm-96rib>Ol(hg61K9ncincB_vk(9N5EIw@>%MiUMGSu;jOltN-N_TxCU3xwd-||mo zl=oAy@_8!e^`44-x2JL|=X3c|=edN0KIe->U&z4xFC>HK3n|d}h0Hg3DaNiZWxD4} zNn`g)HWYd#{XAcZrsgZz+xC@Qto&N`XMZEva=wvS?r&spu{ZLd(i=I}?#=&c?>?ZS zxYhuUUse{8pb`^_CDApS#8?mkQEZ@KAr`7)M+8I^Sf$tt_TGE%y_cAxMvW=P#Kgpy zXiU*W6H_cP21VNYf3qx2Jwf-qciuT~=bRsRc9{9@%$?a~?%lZ;RiSj63U^Oa(YW3u zO-=DlQ}u$<)W9BTDx%_D72|nV+4sDw7Sz6{X2;x94&Lc1Gb~+Ygr}>$4eqOZI9xSD z?W&QX8aB>QOG7f$lqQ)f$Ujrv2+33}8BzJ-=uCCBe3mL(KTBu*< zRu|f3t4U$mYII_@I^QfuU5U)$pUvf{L&h8xoJ7wcCUYlknU@B$93Puh9;7^FddSnX z)bv{BWw}f`CsV5Zx9=s=t=5N4b0k1X)k!6&0Whi|{FJ3qkv31JwPbC=Xg>?jOA|ty zD)QBqPG?s_Z>!gpu+4E%m%0yC$9RlZo3xCW3ste)Fh;8+Wl$YShmZ!7Z@h{iXL_a* zN2m098=Xx_JDuL#VqxBSiIplpLKx#YUiD$iI9|ECXt_USXRDLxk-4`}iR91iCEK*h z-A1o%qO+YWUl$gbR_D(9vX==eS*5V;F!r_$c_yoV#EE1L?`B%gB}GV2VEoa|{ z95>YwEkZ2O67!Co(HP2T$6Fv@ZT5lV4I{q<#SLq2n#5`?oaflsD4ngIGPf(CllG5? zPQDzml$Gh&4XOjL$e8wR10}aow{ptb*xPv1iFy*A0UD0`ce!^891WTOmubMh^HaPF(ZNlji#E~XC? zN7i%XbIg(FSsmG3NBY4y@_Wa^8(CjjS_0Ypxp1JMi%xP1FHKy1J3XME)AqUcbZgju z!IfoE{DNK*+9{qxDqHf0iqphEJ&@~X54bASVaO>x2|2CwZ`S@~T_cr0Id`C4B*b)J z-aslRxDKf_z&9c-L+VFbmb5*o6KNy$1;79Wl|?++$yc! zPNFqfN?m=Rtc#J5Hh>9`c7qv^hexH6QkslbKdI9+_4lbM07J$Dqv_cvi~PmLPR_X}1Wyo}3bA)I!Q3 z%3?XB(Aea-=)9#YXv$JkzNU5vS-BA;z7OQJ%**kF2m1Skw{7p=v0aF^OxiG7u>1~% zmPjgbyeKJ2rODkKS5hh0s*rdx$-K0agwt?uq`|v^a#i(}r}AK+QZHV4^1FiZq*2+D zcnylg>tkU&k0SAUSQyW}NW9J##&atYFVMnxG$R!{AFo*$&owWevldt0r%!%hrg-MpEY_Ak-u3Ju;psm-{~H=qB}dspHG{f~)u$zu+2v#c%i>f8aW9;3jV2Pu#{|xP!luhP$|jblgVSB~KS94W?|SBy2U4g12_@~RvdOJ4E6j#PO#<2h6S zW6LW>4^@mOuNX&OF^0Ti+;~+L22=yhb*egQfKe$GBTUNJ-Kr*DMJ?2ZJQq|KE~p1r z)Q20~;eiJ5L_^5)Ltbc%CTNOgXbx}qpaojO7xJ7@E3`%%w1qzyWkR)sv~_ksM+6`c zK?p_&UK_sFOjlPIMKg6Ox1|SacNPrOoF$jrB zLNZb?7(*}=!!R5pFcPCM8e@=(u^5N(n1G3xgvpqKshEc8n1Pv?h1rmYYUg4e=3@aC zVi6W&36^3RmSY80Vii_n4c1~E)?))UViPuF3$|h#wqpl&Vi$H}5B6do_TvCv$3eV- zH}MwU#vvTW5xj$=IELeR7box@-p2>{5GU~wPT^yGg46gEpWzHX#}_z@FL4fE;cJ}7 z1$={x_!i&cd;EY)xQrk16RzMYe#S4jhF|d;e#alUjvKg%Tlf>V@fYskZ=~Ta?jarb zk%3HPAsac6hG(AR=7DhD|FDG~b|?vZl!61Ef+M7@y$s622~VROp24#y4`)1w3aE%m zs0_Mrs~1oORbfCiyoljWj zj|rHFNtlc&n2Kqbjv1JVS(uGEn2ULsj|EtWMOcg_Sc+_(MP5$20xPi!tFZ=au@398 z0UNOio3RC3u?^d?13R$`yRip*u@C!k0I%a9-oTr93vc5P4&w;k!BHH;alDHYcn|O6 z1AK^+_z0))F+Ra*e2UL-2A|^#oW++ohp+H8&f@~U!9{$F@9;f-z$IM9kN62!a1}q} z7hJ=y_zl0~4_wC$+{7*XiQD)KcknmTa2NNGj{C?!CbE!?97v<3JdCPC3D`mpJCuYy zO2Gk7!4aiV24&%dr%?{ifMFR_c{t-aR6s>kLS;OU7f=ONVE|nb)QhN&8h8mW;}tN( zgL)OUP#bl?a1Y7__27#7aDzKM&;WA1H6(2WFEj>2MDP!Kc>kk0yy1fuXbE5Vp%q$# z;UrXB_~SLSLwj^UM+6`cK?p_&LeUAG5r!`4if-tR9_Wc)2uE*3pbsJug=q9e4EiA! z{V@P>h(`j97>GegL=uvbg25O9hQd(8FdQQ=5~DC0V~~om7>DtgfQgud$(Vwvn1<kbOSbG6Eh$(b61xhyGSWmif*SWI;* zlGfUb+#hppWaY^7??+fMHFuexl5S2B1y_5ldHOEoUi={o~21wcuLdm@k zb?fNjZ2G`DWyoJ5*`c`kdvg4HrIN@LM(HO~^YLy#rb|G4>(zQ>{J)>jL{vi7#OOk1SFly}$+R6|4QS%APf1O+?PNrat$W!FVA8&se zk-sgW0WgvcV+*524M?_%E6rIEVhwQo?ttIj(AzfuDK0t-Cqs{jB1 diff --git a/src/hal/components/cros/docs/cROS_manual.pdf b/src/hal/components/cros/docs/cROS_manual.pdf deleted file mode 100644 index 7ef6c77357b135caa17b9ab63953f1119df268f0..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 319161 zcmb5VWmH_v(l9y8O+HzOTbMbSTL=omxqWiAFmZtM%5hgwul!tvZuQXgC60z&4psWyvqK} z{zp%y%lCoKU3H%a&hs|flb2fB`1nK=+cX@lwQ2HN?O2(=~yq&*}+g=~X5165K&OejU!j(fr>?`)K{eGu&VF{%x@GaQ}V!Pape#iJhXg`E zfkDH(uK*A}<^fT_Q2+q2bkKhW{688*N<>|0at*Y9mY4dM4f_(#bX$7duHby;AJiOd z78$sW2v2ji$3x>3iRk#B1=>C6@`pfyA8);(&T&4j-jqLWC^?eA9Jz|M^3m z3j_wURx81}JuN`Ed@a>d|U6Zdbjm6?!egiRdmD%f_y?sl9v!#Qx zy;@$z0^JP&3ip?@V^>YNMd{Oh$+P_J@9hV7VwltGKY1=AN5~n(RtcSO@qCmGjC`)U zXLn5^lLS}3Ag~~k_PKC>dfXkW-j*^gs!78eKEprSSMv*GiakBfP*!w_jwt7R!n8P)Cx@9*x)7tIIO&5I;E4^< zN1<++H?q-#+-}rBWyyPYaTFon(LD)GADh&ItIVSBjDDa^!cJy#WY-}%+C^zu0tijS zmm)O$$FtSHEtUmjbG!yPDd{~OZC~X4+yiX6DVD0e+Oh67GYU@XnG;-1V%I}(b>rhN zM#@3CeZlKla!l}yDosW1iO;eNc%y4J&g#r(saGye1KTCj*KVsKYBLo3g}{^fw49@C zk`djk66`iMWKxlYH3zpeC)Jq1Eq4@3&A^0VU**G%W4L(i%LC`*J1237z)K}LqkGy z!yxA49g6*{g<*R{FSYFRrz-P}yb9=w#bt(psxOdY^u;E4MAfQDAnm1%4UQ)pC7zeA z&O;`lM14J)1Wj%6=pwkchhzsZgzG>7PJd%NK8)0$06oof^08Px_(jpz3 z3m9c)*wdt+jd!J!Vp5)-*o;8$D%s#|-|;c6C@lgC|2eqkTDz`cJw8li{UQZ2Io-cQ z<>mSfHPn=72&-qiB1Mb_w@v!{f!;MQ!NcNKbr|RQ1DgN)0{tQMSW!MiA`lD+4g>rU zjeqAt0>S^vJOBy^6`dIk0}Gp#okIkZgp8D&MOf6>`9l=Ke27ReNZ>nQbEdJPnYPzC z-9-ENkZY2s(|i18!YR+q?-3;gWee5K= zUt&pAKG($Jn)aMelEjvuqQ<`I&Y2L>LuEtpCog|9Bycrut{9whHbJgakMytc^31!u z5ZK=E?Q4IC^qM zuYDDZ29CT{i3LQHB41F8JNKW2!$N&kZoGSa+HyT4!o>NXe*%$i3w;C8%}S>4+Y_>e zRv%o6O>4@U=C0qcm?pzxZ?;We9<{rO;RRXU5|PrfEABpa6BkWF8@90p+=Of8HGpUD zG1tpmCR>FGjAzhg=8Jo>P6nKrIO$!hPO3I4r(Hk!HA-}G{X!S-IpN}=ZvV9E5M4ZP z6TzY>2+X!<3a>wanTPi64mG}Li%j{MlHpp$DRur4pzqT7sMg2bf#V>-(h z53dqW`|=1b5A=ej(v8`aQO?YoztJm!=dZ?fMva<5XB2Yp zJWj3{&n(PPZReqQ2fVc8cwI4&5SB|~hZVsL!Tbx7cYr~gpN|prxDXYC_T!Ij*j9X% z-IsZH0yg5NbzRF*T6rr{X`6Z0efb4s;IXHsmhC~OoK2|*1FQ=dT*Iz?_Z<)H3knAi z+N0vRN21L9=SqDBX_=he49Pw1bA?gr1)E@5EA_>W=}%JyF!d=3^_O1y(G$98u;Kf7TBsDJTEdn|@>bAZVX=IG(!pRO^}I zVX{nXZ;^B$Qbm7*%ibWJl`7L^8S=}WHr*KF@Tt_H&zv6^aut^C7PVp!I>;4_N>OLxG0`f+Nm_W1fNE;wMM%!x`p zG}g`kF|th;Gm_nfTGv9IwrXF+kF!xNGi)|cm(69P{#cWjH$9`%5xo%y zi$JZ}MAF}e(D%=o$sv;D;eN$x=S<1QBA1f1w@%3si*~R74-A$o)xJ6Bw%OQM13tf7 zC)3^mvv-z~C z3N6K55lRfykmeLi`q7IY8PFn~$~&~kx(_WwQs`X0DERDYQ-xf5960Hg>9D1hH%KTm zG#qQ0s^`lN?D%cwYqta%1c5kfq8X7~fUwJi14C8EF~RN&0EFKYi!(kp_)M+v2n#ZM z6X8JzRtv?|tSP*!@kZ8}=??sVx8UeS+t@)xeIS-mSyG*1Z{z9l`~%yoM|kxF|0{1i zUhSmmR0g`|f+PEGJWcxjVp+ev;`~W6?UjI7+7jjG!McPD5OM;mkVFx@jDRPUS*ZIj z?gWy?rpT3>rHRj0JcOlL#C$)eKF7yD(N5ycI@IrZT+b3}#L+>yPq{vQDZ$*$FWW#jFC4zv%oUkL0S0E8S53Efc} z!BUlpzO&GhVZa!KZHA-H=MSMC7%_@cnC()xIDIQC^h3ib#v-w7(5aliDB{RMn$ic3 zTZYzD=NZQRN_qs9(ixW~R^Sh03|Ej=RgE%k7mkIi*C#ab6fZHIY#raZ;rwacnWwL; zO0mA@uhpi<8ur{%n6m9bHJ{y0=7fQ-#Da;X%szVDM2=}Kn8?)U>aE&l|KXfD2DlYo zMm2LPTP-=u^P@zBt|y$mSKvI5n8KRw^NP!1b*%J%4cEKwqnAiK>(3XG6fwuL`@~1( znB5e4{-v3ezQ&v=xHGdb#WQLLn?n84XGPN8!pf-#G7<#+&!7@F21u-Ho6f}O%mMk} z&DQm_{SYeLK!>&>yZdK-bS0{=zFm>??0U*!{2OP5O);XQHKIOKOewU=a!Yh+X_Y|> zulTmSQRLXEdU|tQzLlL9q1%;I#B(m<-okJ04q`N``vFXS&tBS_>+|k(rRBtm7kDtO zge|D<^+;MUE$l|AAy8wWdf16p<9Un-s%ZS5=U<;B;CDv#B7N$2*2L~;Q#mR*KVDYu zjwwcq|5|aKKs-Bf@K#x2o2#e%9OwVD*D*58%y>n;umbDjo@{Qoe%{BMBMqtE6R)vmrFg4 zU4Wt#qEjSI>0V^#ZdD(fv|{`?kF))`YV(T$ zHnfXVTawg<&H*{1)G;_Sgakx596fM*jHOUr`<_oMxcuReYE)-j8jZ^lMO@o*SdCu^ z(^5F0ENPZf-$=O7Jcp^jng9MZuLBi(zMd#yUL9Y=lumGARufRr8d!U4@-Tf~@pj!; zv>_3tkY31g7^|=C1_^(sO;nEioyN8=zL-R__qgb`5MCh>&L>pS5n~=Xe|?&r)N-Ge z7ooZ>QF)!)ph>^eV(dV^FGs1eMjYW4l0B;)yByUTSk0AlYchC+Qip0IBJUmFGp?+` zYVt=ZI>om)70KovpT=e{a*bDiYufdT3jTQT34WnHV@q1#6@nz)S-XeYIC;e6w%9#h zg~WFO9P2mMKs!{SVeTOv`T)nmelkz1dxJbo3&H4LAItK;hNs+QZdP6wTJoA2FC$X> zsU5~+P|APVXMX#cUXr1h5M%mS9It${jD;4?YVx-RiZ$<=HHCAkQjbROB~b|2nVLy_ z#BZ3K!sGe+ROhmT1xpLE+a>+ZCcSmhS7CzuGk%->=_DT69ya*E6E`_U)vsy2ms=g^ zY`5xj$Tsc%y%-I|F(rkwJbUXJbJq2JZoLr>T>F9=;y{n3@6*2~u@#DQH!Yd-D;cs+`! zK5{Ozc49j%`}C}<=sA}WyRAW~*lQ4<$e3^LW^9!zpJ4ly-Tx8T$Q zcC(baOJQT@?W=#Q_)tP0Dt^}lGyxBHRzHEH;m?Ofec#h+fxAVb=hH-g_o2=ox8_Rz z5aRY$n31E*?|^g@Vwly`HhyJu<57=<*h6fVzC(LD7m%Yf_nGh_ zGIUSvidFBbJs}cnZ#+LsJ}k~~b#^9q+}*}s)D%1Uo2K&Sb0CZ@6G~Z%MSY75+u4c! zV?O^pGX@_`<{;l*60&;eX|S$QG-SX!7%(9KY|Qa}+p_T7+u6Mj*7Toma1Y z(o88rPk&wi%#Te`kI9Uue$#)0Z1V$BJeU}rY+-N*e?7N9mYvltuMI|8`ftlM71e?E z3#=;&I2ZEeDJ&@i&qp4fF*1e5S3d`&SBFzWxq0O|<*45QqIJBAN?9v%Ts~E1M+(o= zHujcqd?KGLVERYQ5A$)CH)#2yaQIabL1#_d<}q50HhN2wz4liz8K zgirWkOc}h(_z+?%>}WU(?06w@B|JMiC}$>0fjzM&5gC!55puqb+>nmW8=P}$lq7f3ZCtP^+AUxXxKY!hv;lV9dzp77}!ymLk>gh_mtOdH`4E~9ifvCtPx8jJ{ zl|6E{;11Pn_OJgbl@~&VRYoh>~d^Sw2LoP)-!%?_(WxHOJq5TpOsynv#d8nUh|seAyfhKC!+P?$_%xP z6R&*s)b8|_yB3ZE3Da*&%@2aClfQ{c7Uc;Q$2UZ9EJog+)*%00G$dZFZZ~Sy_R+%L z#OaO8U~8Z7Xc;2yL+J(io3~-8Yt|wx73o7MA}V~Tbaf8P?eRfEHR?vbW_5N$Zhv&e z+W|6MaBUZ~TYsBxqyE$LO$j99z}LHLo2C6I$t0b4fn6mkC$pe%+88)HxySBpgCJ|3 zJONkaK%YbUmWb3oaEhnrD;;Jnl+%tA*tB^;1(Qf~#4Y{~aN11`jeOvtpo}+H_{CCq z|Fq!xhN!NKiS2TcXw(|#)<4a?)JHpRM<)SVp^M6gxk$1hS_~A)Wtm92Gc&Mk)Ik_u zezxQASKrll_y_kBxKN**KoDBDsDCP+VP5B7Ap%Zv85~scL)wS%5&2aa%?i3h>x;73 zvW{FJG&*+fGTjCbR)@EqnpxyCl8EMXHiwUxpl#*niLO$*O$ubFUX(<>xH}N^UBTIK zvl=pVo+zlBv;`NqbU8{pel@L^T#9LGS86m67)p=Vmm@HjI_ve0K{?uh)QLt{v-r3LLLPPPY{pCAwYH!7Ir2K>7t`ZJ8WzPo(-q8ZZnMRB1inn zz_ugCGu1yEza38Tp{Hsf6uoZQIlNO;3Vge)^wu=;9yq=nqJqoKl)Bo_tTsd7BrL4M zMhjcl`8>U)*u*097BF-bn(j(cRK-g}%m$1M~WA-y@HFaf3 z0$MHSyEN(df613oMf7lg^=I;P`-oo6J1zmkgFPyE;r7LYv540vv>gGKTr{cQaC)&i z;4}-I2_oJ&wDIIw{KTf49BPw+PUOrcC+dgNtKqbuaSa>*s~r5Dx$5`3ifkzyoK57J zIy_oFy6@v1QN}F46Xe$PhsokSva-&O6f4bK_prnEkUL1z zr^>saT>Ne=PW%W#V9fGzxZqqw!vz&1CuL6xoi;|!?u6iQDTC=$GNbj;xx-@h*!We6 z43r&sFf{4<_Q_Xhr2t&BJdUBL{^kgsYI1#qq3rE{A6olEy~#|hhW43GBS&gnr7cr@ z*-7_{Dv@f<)+H$%Tv*xUDiuR%WQ}2+4@-0c9qzmC#eG_l)u>gL>cH*Pt*+n&TuxjYl;iXx(FM zKyLO*7VfI;*BldsspTQmJ(2}VM3vj&{^sw1ZDswJuNMQ{_G}#22MyU}pgzL+yVQ3; zIC*rhBcUSbK;3#g@56fBl^l!{!)UxiAyC?=a%;Eco1+<a+b2>a1fXH6uI_}XI z5PV*M*)E4+W*Wzf;OLMM_h(?rI$LihKYYNAv1<5Yuw_wmw-1jf+5eez$|G`ye3LN? zCI(aTtI$=~hdN1%@?q@#xHv(6IL7|v8UsTGkf401k{}b8-~#64x}M9c>Fxg-hJ{Ez z48t679A-#R^eEsQUjgzd*{TD93`S#G*YhtwJ{m)sl7{gJ&$N)gb-JS{zwaGD6w>oA zjvLU23_tTK-%Mr6#O*hNDC{wkz7;5o=L(pTHZ;QdM9`~mW<>=zP*#$GU$_@tz!10k zw>pm_fg6`xTAJch03n;ECx4Ku2^F?Px-Z_|y$S>>8O?q)#xvj=PoaF>DXVZB)~r8e z0bZ$-w}mx=I!>?#htJ&tMGK*pQKR>8=5+FIM8iCB}-B5v6SN zwIvSr>@4F8*`=~Y+;!ABF@Sl+anTuscYw>uL+XkZ@uS;2KwQ)CVuI+Lx91(e@-j1d z-Gy#@jprMWNV~KJo`7SEiE&V?N-gf;tX&J2UnU&J^_7RaQoTLw@MR{qFs^)bJ0;;a z(j8uUz4C%1r!3zLOjH&n6hjEr3A?lC?MSy*H1(pVsH{wm=4n_E!7~{8rxsZ(#*knF zRy@N;UWklv^_0kbt7E~ixx4BU+=2#*Ygy5ma*|pm)#RmEq^2LRKa0517dkH{P`|95q|p@ z_~y?&?S6Cn!*K%jB~#ZuiV-pMQ-8W8MtEjpR)~AY|8yw+u0<`W`_+&6ycL--#d{e;w_wg+N_fY=+Wwk{8@1 zEkj*Z=|m_hF$mC7a&ZTLyMNHC0BUcY>}i}DA}6Dpqo>=nUVw>q9#R?7yB-%ie) zBX4@c?2AIhu(e3@6X{yvASsF-SagA{^r+SFui_HA475UH7+xKLM=YqBT#4syLy2Z~ zrtn)Tc_uBkEoZyg^lj+OTMTXsxSY`m(n8szW2G2Sd@Df?Tx_PPCoe@g-q++MCAX+$ zZCG0Y7|r1*fm-u#g$_Tc>cxuwSrf-2{&Z@{wj_Il-+J#_>p@>2`NNhpiJSiZwy{r` zpt0DfDj+V$xD7$d1nj!!(1x@BIY%9KCP=$`YE&&M@1FH7YHDKLK|UXL6*9#nRT#pz z8bnSW&yWP2q+`~;RW_H)VDVKF0Uu(ULWwue4^921?9eFshc{vX)UEjz{}_h8Bh`>k z9x|OviM)z+T$+uB&rhAaCf^-WINSrhaaKzxYka|}*^pA$LepUL`l_#HEV{5G+X@-FCb1*@c^EL*_#^#=!2!B!iELpb#TWcuM_hYQ zpWU&RA7W0?nsut?=Jt`+fxy} zm&q7?=y9W35Er~C0h1ZOS3R?V)q-;W*&YVn|5sAu+M-UNIz9~kqW)|~_a8Ar`sAuO zCKtbF9jAIsBQ0Jk-KcPeZhB$a5AR|pni=mwjAHe(cpCS8<9plZyw;?#|>CWxVVesMdBJH=v`oU-#dAG;CQHG)!9b|}C*;NZi+_3ua=z+YcA z%FQJI|6c2hjypaE10L9KCVx3G@!yycUwn*B6r2;hWGW1Se&8#gXt@~6f1BXBndGFg zJG*(NQJdsieto%F_5WV=BDl$aQ>Op%?!rUvqWmtSN&e;47l8!7n@NGc9~>3@bAEo~ zq;GdgGhSnSEs@^iSF}KCC>D!eYP#o*x zUy%9kvs+1}_^;Am*gEz+CxI6n?|}b(A%92zuk}MUqd@(zVgL1Td_*n%kCy|L1%*Tf zB=Vt}B^M~y_59bnf$|0kU@#7)@uurt?qcVFm3@)k=4ahT03So1O*L!|IMFSb@un)i z%}YX(M&MBX<%PB;;?^v!SI5wgREzg<%anvR$?$Nv2)l3*ozd@&DckiC5W)8i1X8@T zh1^S=X+69H{N%q$cmtSz>%*rVsAXOnm(%L&GDKCyoR3PxJlMgM{_TzVA{P)-#L|T6 z(J5MYLhjlGW{}%uW?$GJf>99RvG;Wa zAX{UR68*Rl{|}g-q2L-Pji){iazE^mMLh(~lnK2OgcLKNNbeyL;?f$}B$WR_a_WCb z4g`n&Pna4Q0QIk3*Te+{^g;5vp6Tt&E9L)qFi_qBA&q)$9xgAzjhWj=lnF13`Akb& z+mZJHlLuWEU6W_9(I;AKj-`jq`HoIF6pO=u;=K%$FzrtFS=sM1BwU0DOyUxSU$dWI zKuU3fZBK)h(j8i~nRM~hY2xaK0--vPeRBCLa%Voo6>y#zOK>EG$)#NvmA36K;Iopsb&k!ZNEwK;X@ znqphN36T9>ldGHqodz@h-1B>pSm139xDVEqrgRLf&z`xO4k28ny#dD?Aav(vRq zV=2G!-P`>c78s{S+gJnW$9}U^^Z~nKB5G2nq-f=R zONEG`GfIV`i`v|)ixul9cGF>v{uqsf+RD$bB%ddg#2(tMZ(V2@avbf(+)XZMnlV4D zjF}=1@Nm=1+iQT^qk$bPxegbDC`~(+w|*`Tx0fjUyI_q;YzWb&j<3LoKX1KGM7679 z-};fXuumA|jQ;%NZJ_&zANm{X`4L3)PxR17cq9M?6-2_qtYRYKQg9jkaX;J^o;G&= zAFl$K&<9Tn_F$8Jfnf*@%GNir*aN5jDhoh!(GGzDuiQ_k)uxagB2@=A{+BoRp*oCRQ*!(JogML?jjIQ9@*1Z$LP;U^CE zilYbuqk>_fq8w823W6mq6cv+(VHTwi(p6?g5fBMc`Q!~PMvlhXJ;4jI7aIqMrw9cA zluCocJOM}${!|i#o#Dif#!e@rbZj)G^}vizrhu5uIRqRjRG>nb2Pw`R%;alwH@55y zbz0odK4@H*3|!}LDDH-!5K0$7Ki4s!Ws9?1vbzAo7zhLcNTEbl#l%9Lk-(&xMEa3y zgR;Q03IRd%jH}dO!LrH#P7sJS=`y{-D5n=40s_OsxhCagva|R^QLq+JD^_OpoD*X&%y@W&rpiNLhEO(9%_4Y9_Kk%BcW05Svl@B?Szf-AJu(ibNlG$lVEDjHt6< zqx~bCcvI9|1za9y#kN2iceWxDO4wr*H69{5EjuhAC3b{H8HhW*7KH1(Fctye4xy3&NFJ!noIw;9 zvBjpY7!-{8Lqr~hO1GXH(m+@^qK_FMCWe}{TL~iQkupsfz;>=V?c z523}3K;a|-ETJa_WdOv`fR7%KH~_eQk=X?#O%(ZV5Vhd3g9>4`yWY0)R-y%mGhy=r{CG zY1CHejkph-Nh``rL=R`OBiv&E){KIMx52rzr+B;o5V%0dM-T+C9?ux-2!;apD8l_( z$;;2c;01!%iag+u7D<7Eg1LZ6qSP0GLNS!`hCxvBVB-M`YL~u)C9?ZG01=FJ5d_D< zNZAblHj2xIK>*6tR*e}&dPu?Hy5X{BNT4FszXJZKGD84iz^K9aj{($pAyW|Y!8`3J z)p?Lb78oM4(qVDp64XJC*i3ibL2=!|@5}#7HUj5=GY|fq{=xOp^Z%9pv92eVxXOvr zd8&0pU~JGy0~Qf{XNKGXsr@ywu$hw~Mj`lMrpw5cn>-SEBqq7iv&fiGZqH9_oGS-c zg?;tvs%v{QIsTXbY{N#hi>e=2ywMv@z!UQuTkU!={jAyRdxj7+-^Cnt`qJ_=4pr~G zB5%vpY-BLqZjxyBKA*xWMDw|=^{y-r!VGo1GkA|AgPc0f_dkc9i1Q;ykQZ9>T>aks zUyet%Gq0XzYkUWhkxeK2MSgz?t2x;cWEWtG+{_CkVw~(7A}V)@PzKXT(1(~K%U&88 zqxG=F=4|*5zS)O4@v_gB{aLur;a>R(*`I*utn*KcF%`#nTZ`Fu0c?4<=r=KEtpoaVyX==Xhv zj)8=Ntg$OR^q=9a=i+kh=rWro#|I+D5?;}{m!!8@qzf^=Kk2tZg{{mVN-oY*pY3uB z>r9;C-R2n@f72H^c)*W(jqbB7ibZ~J6s;AohvD>u_HfESB6b>v$IEuO15z|=ctICD z^b(QZ$bnh)9;nr!q-p7i1t;Ybj7EwF(605Y7v5y<+CsMMv8Hz2ZTPplNoiBHw``t< z^|4lB-869s4uu=ZK+`hAE}?9w9UZc($_wwh7M2cB=C&NrgQKWf5tSV%sy4o7-Wq|l;;T;hHtD|J4xb~oVK@` z1N$p%Y%4N&8%t<@k*bmZCL}6Z&8vjGVPb6q_wpQaX#eq$&|W1-0l86W3t!-bVLL#! z1<2y|VbzF&e1VZW`WjUIRgSf%K~7mqmVz6n~%<=bu`CB zL4jnVM>>E@xREH6E}gySGhOjID_Hu*)wx$^ZgLk7Lk*g;UbeVT4WlBhMYb~@B}r$P zrVHtT{EzlWYX6`k;kT)w_#6$*m^cFi=ahw?Hh{taCzZ8(bjsXY4BRN286nM9QQ=c6!=Vr2z)beF>EuacbgcUuE4Usu zyk030UPCO-?xP9Nemm2HmO>B z??ADrO=(m7$ptDa&U*saY=%7;cr{hksD!%k!)zrh7rGqU6(F)&8K5ej)^>VnkPUs$ zUhyG$)t*GFuE(D=-oE>-eHcm!>(Cs^7`y%4+=yCv1j3XnrGVk4dxW6dundx2nST^W zTPi^nCeG)^id?H?3-%bN!5Vb=gSyG$&)*&`TQEqBnp#1#YR^VNv*~-&yrPHci;b8m z*h=JjCXCWXK~pW)Y6?;){8_fx*Qw{y;2o9Iq7YSQ*`S`mCxkedj;Uvu{5siFGC!Il zJyP>p7J=+KVqIcPWqwtO_ z^k)feYG{x(^>F*1>7J~peQau>Gj2I+=}61s4*yceudV^*8Iam}jp~Rjb!^FZnQz&p zg;}(UjN`NHr=w9iL&oiSNu#_DKO{>lnp*S7_DwWWlrimKQS7DVQhY$gP_&n_nD8os zx~x{K;ShN}V-TilX_aC+2(s*78kcES^7_XL<2qCYe^@xETqC(-D$q8Gj{C&dvY8n) zV1nbFuqLNbgl7$y<5bCxrG)EJRnRp!$iq0yOK6O(xDR?&ECT#=PkrLa_`pM;A}DKVr>=dcLD!jysij2{7USBxSa+k@cVqpjh{ z<0}c79Lsx{MR3oTu4Eo+X{NlD--5Z;m>~4y_#>!EF4^TzZeV`47Qi@A5Zdhr_s$Mr ziKqMOvOEM%w+%GRXd2_hyp`=m>MAO|<(S>;x_(a64ze_na(%E8ygcbS|K)*Li@*n- z)zD$Y{Y$xLIA!rvGR4hhC!`EOp%>c3ny3%e_7lJU5%(7xA3fC+UKv<2a3RrUmO%$+ z0wJ=(ds#KoXf6!iE7e8e=_AuAK~t#tHx}u2y|qI!`^}WDsLT2L+EjuIs{Z~aZdhVN z+L5a_G|=BW+z7u}{9A6a935bGjaD_CB$*&f6-|E z>M#G~SF&(({U^ba=l`YVurU8$=sDkX$EtTZ5!N?rhFgVIJM71VQ9vzeP@ON8=8GpOxtZ%0&gkZ^@}`Mib2q?J+Vjje8IaKQAux zA7j&%n0thf9V3Y8bqm6o-}u@Oz4@3Ne?V7JuXDJXHFrjKg@r>-psyd*+$gOWUs&<9 z9Xh$j4|;`ux2}m`>Mz*})V1i%=h9zIs|a~*q^)E&nvcDI&ciLS>Z%)Q8^(|Sh}KjT zOPs9esQfmxD{IXB#d7Yn(cN%}L#hCr@2QwV^JSjGh;(0jQBba9+W`9;W;T*_WNs zAN3StCRl-jj6cG?BJ>D%X!01{x}!1!NgR7R`Fh7)@OfnfZ*xHR@+F{X=3U$-nlT*Bsc7a5>@l{Rl5 zVl%V5m5jy^<+2ve+1f~2;;VlNucMGV?qd;J@R+%|x&|zGCLn8Cucg+HA%oa06-Ix= zr?aI9h5ZB{6`s)eHnKzx^k@9+>GfMwpf@`>c^yA_0yfkQ{da865gCQpy0dzUT|C3L z;zmd%)bpO zv+^`$!3xPmNP=ew>DuWCXH z^sC4uQ6X)MzPSr+LCt;RZDVFA#~JRmi9=cnjTUbWM#R@hpvfS8rLpQBV&OMS5v|W0 zs4XKYE5`X~a6cc&+|(PgUAw$qJD^PMIYK%NExJPF8%pV<2NqP=eTuYFI7Ark$BJ^S zf{b6|Ds=fERT<0uBU2x8eVxf%5&c+li%k^RVpzXQ?XJ3ZaZtOn*-t!H;}jtqIb3!S zJRCr!j5c23)EQYPB?uqD(FP@IwJ>0r&U}Sv@p)=x;@0$3Pthi_FRr`hz3qb=% z@+hzd%^#**dA4Rx4G}suh^7IHb+kWWyfl?zsV2wAt6ZYP&m~VVaJ^YKe!AsV&;nta z;D;;HEe!@+p4~6I&!tDm`#@gzHlfim^2_^ar0ViFCtNtU6()o`c6L@ZFslLVW*Cs~XrX~)>8&g%feO5hV z`1K5O7mw2QatAgWI~NH%l6uo(&onS6xS4GsX?DLpqV`L_iK|0w(*z4owfh|Dl|E!f zAkg+d6j(35EHJ4H^NCH0Z8OmAEESn5jtjF)g?DfHX<)HuUu3bIcui=Pwp4u^(haSp zaQFHoC6ALO`30jcD8BUP=s~)q!ese)73GGCU7`VFNxACz4e)e5!%vcvK)xU`&%VcR zG`qiKkHNVTaoP<|NzCtZzOM=u_k5UKrm5U%8vLhP2vK#pcA|f!)-r7@ZUvNO^d!?I z#|)4Zn#NkGGNI1WrzgWaFxZ^`XYhNyIx?Lm#)>9Mm0?Q}4R@_?F{XW>dw<{5y4=1! zJ+=dWY}|(cMYKhl3G-l;&S06DBMMCqTi>w`&=OhP7d&simf3*K)o~%aHtU)!DbF6V z7MW;6W6xhd?@dOXqg}6o_qE2{VeW)y5G{4HC|6I2aZm_K9}hc7n40pb{@H8|K>T9G3?uwc*4EbbF`+coemj$mckL8|3K`Ab$t%xIkP7mJ&6qlKHB z0LWpBSgM@k_h<1_xze5 zgwiA!w$o2PXezZr=doi8mPy!AD`#M(I*iTFSEslIAlmYr(Z|UV00VyJIOgi8A8>l+12RF z|6%MMV?^ziwcWOD+qQeQZQIst+qP}nwmsXnZCl@b*E#!JC)q1m=kJq|N=E9(bB(II z?(%ZeHuNmjQyUQEYOPxOJ_N)ry$uK_f4K$>VzM$jNqT#e?%+M-WV(qApkO&Hhxdi= zXZQNNI9 z^;kzkF1{bo+<_yb>ZAuBUxIs2!w-NL$#^3VD2t8FBFeZSkM=@opJo4g`RzMuHNFjL zeYorf5Q&m)^!w7p>fF)?gco*09acsgUo0C^ozgwZsg|uA=o?gX@a|SAZD^i8WUcHA+>&yGpDGJW6<6N?V+S_@S88tI| z%?o|_lu-1G2G{Z2juPHi{@Y8}7IA4Y-Ofw|Q`W5akW{{U_OqOpHb8#5=<|D`s@|7}jf`2WY9 zWa)2t>Yxp==R@tai=W#|3J3;x!N}DQ)FDk8*b~A*iY;=Oga5g~m+y&4BZ)%m`tgwn zqpvz4LS$J@q~TS<2+rl_wW-zg=lx+PW(3RC!$IsohJ*L5Fq|B^_Y8z@@a(3R@0zZ*lb+de z8ZLB|(Us-k>}lmU^hpP;J^-?_$*;X!vxjuBej%<0msSPOYW>GG$7QCxncj2IZa`d^N4-&_VFB3k7#K-&UAMRO3ex0LODg;1r4euTHkrv+2Os%0<5{f1`++DY-h zh6{O`(E&@ZuIR*Di4#V$LP&|)1GxgA5TJQiIoD_&+3~)DI9Z@8aAoVa3ki0d`%+%L zxi|p@*9X0;8kNFnlAGY;%(H^W!gkS=ghHZm_>I5Zx)XOwo+AcqCXSwaqW46=O8S)<>~JeS^a)Y6}(bGA_AZb^abj@M=q zJ}oqDCXi$79e02b+qEc8ZC1-HO6#riw1-W6Ya*0F05`esoTJPKO#lx!Mee@Olf*a| zr#+W!1VmUoy7@)4styIxl_FD%D@+}DmTU?(1HRnL@@7wLqa(elDUWTwROXb{jYVP) zGaugcxeEl#!B)W4S`Rf{$kC1Qx9J}T(`|}>ZUjFOU9^(aRZ=U|mem3|(YLyILsUZn1*$mH#^(VEgEk`0b9f=@IeDl za_CGtO<%Ppt$&-4y^LG3BHmM)BkNod9Vq4dsXG{YyU1k>g%jHR>2dwQ-^;6R0EMEb0+Bo5kwiUW}#+))Ho-DrPkXma$+$2ODO-$cy4M4in_-(dHh@xhe zusS(sT)85uI7jj~xCciD{@G%QdCglFZ3cCmQz7Sot*XqRc;KVYtYQ>}%w-$Cmv_qp zrMXPXn5lCXX&AZ|DffzKL8&!+teAvt*zqW*Gd%&jchAZz8Ho(bR+2K`wC57Ml#s(E ze->JTIh!gT|JR4px3dO67;xzlA{?k~gm6x(!+u9*fVNK{ zR}BH2Qhob0RmWB9mg($v;{>VlvO0xl!X0#30PnDuq}lJ`k3wu&I2TW67=NL0Oi6Iz z=!&}2U&%;@n&dtnyI`8#4W;gp;n`;>CFSkYn(G6=2)(|eQhDCOVeEicdTfAQntOqk zw0F+U8}IODRO(y>g9hz>F%M7X7Tkj95x{SA{Fd=c5ZUFr=(rk4B;C7si`0?nr-!!y zbhWv7n#)Bpl)@l{6wTxnxqErqRFhnOZC+x8O4)PHj&|NX1E2%Eh2FVII(+*`i6tf+ zJNs#$N9tr;{w2c})pt_w_$9@9ApJkPxFMS^W>Ln{0iR>#{42i;1_;eyQep4IkS7kB z*&F`8^Qx4XN8oYlrq{0)>qTqD<4@6zS%05^3v>oVg`|ifq_8ZTzP>}l^`970EfM~H zMDrMLHsn6~WKa^Oyek+=NWPl82w(<2OXvkGQo+_SwLIE1rS3sRsb2;ZNpKEsA}>43 ztH8LAF=#y;Mfr0J&Cq!zaZa=zsSHWz)<*Z&%3(ntYnVR)QCkCPGPxvAyON5%97vjk z5;~h%^S+SIqkxLX5jK}q<4-b8>2Cu7m;|4*HOC*8?f- zpa|`_;%3ZR*<%4ET_lm}CAQF>ybQZ(ZXjY4B8o+pDCPa0?S(mR|u-k;1fSLWU~AVs!CGgY7p?NI&0#+*5(S{tprCfwp_Qxy7E z8tY_?xY#bs%>rOVLHD@d#TIpG4O|;R@rf7mZ z;g% zptCx}V_PCm2CH51wxex^y7g?pcIVxpZG@a1JwY#c^B2)?tQ;`;GZ!v}L_l5~V&yLB zrLKYmspRy`Xl?cke%mDGh<@q>tR1p1&-&vmF1Er}duzmhH=H^?R!+*CiM9G#@ z?v7x23n5g@DoA8X)eup33Ra^@n9p*I{Ed%6G`uZM%fimn`(T` zh#kE7V2Y`AUq#2@GU@z$-gU~+cWhtyMh9oT4{x<9QDFPg31cWjA{QacWN2Ab!D)u1 zb6&Nfl{O^HW(U}v2c;9`3z0xKm&8~|!#pOJe|Rj;gq?y~k16h6YCL}1QONp1qDG0R zkzg4-o<@Y&ukgHIcAl@TX*yLhyU44YPzloU+n$e&mA{~>Es|W2}juOb!;M#=OHP( zT_;X0+qyA%$ds&o^}IQrYJAwGHu{la{3%}6hNyknYg^c%TEHRp@Xy%4EpKdA()DqP*r$b+uBoIRoW8-Ft5{`gX?rC2l;<`iSG z_9xt-ZP)I9oA3eKxV+~ufPbwzIXEf3xqSAnqkNM7C+z;eW^^1Z4FAS%*8g2C{ijd- z-@3xXno_ZcEr>lYwXivZE52EO2ib4ww+DZGzD~~Gt_}zM z*WMfPx4(aV?y11%g|z1Wcwp+)+F_>7MboP}W1iF%TI1yuY&qlYYl^ z>ZVtAjs@T`G3*PN-0G|AgtzB(a?OB<^KRO zmZvW#b}O$?4U5_CMahAjv6C*I-fO*&PJqR;rc0DyYee0zUhHi9@NDhK@>M6vU6ss`kOOz_jVj<1=FEZFIy$3{Wv;moh zE>2ZVr`)80VQ@+cm@6=V*qrrIo$7Po{@x%c7u_Lr1MJ=7hLX+&lq<z9TIq!5~nN}m#3m2U(6fD+Rsv`%ju?p#5HL@b(hGyJl75dd;;U&;08v3 zGut!dpP~}FOUOv;CH2_Kk9|sI%q(Iy4|-DABM&}TdBVu`O!YIRp^8m7PPFGo86S&k z7txaQr${BjafXlC+0L0u#DoMysWR!1YSDo)p4UIz!znLrP7BP`PJ}8u{VM<)Ra>c{ zjIt&giRKi5%WRp5J#^jABsQ$K(ujK);Sa={HYJAOl6pfJjb? z86;mVj?cyN5$^cTTIJe>6xN4C33&u{pZNOebe%CQ-Z$b44vKC2_&KbN5gE6|B|DM!(%SU1rNG&Jroh z*WR8QIujkqW5OtGy}+2*^AIdL%Er3$qy~Mg=Qw8?T7B91MkXl=>~#dAJNH5gADx9V zRFvh3%s-Kkj>_$2s^Clz-k%#f%^9zGXPI3cuiusxtgl?Lu#G!5fI;G8fuL;Kyv~qN)v_O`vJ{@N^3JYxxVG?!3!zxoBlS zELKRO+!uvNVePG}?0&H%^CO6ywT=AlShwr0V6(K01fRmm9Ik+)EG&sh`2*|9o8H8p zku5>-g?MRQabD|m%w<&02A-^d@Bn$NP!DM3l%KBd;$73ObbnO5?eRHBXkDaKY__df zVF#7n2BpNd*4{dKiLg4y;bxvsti>>+?Cxz76&io%-9spiM{w z>e4b##U&4HMXWCF;pLs6?$hjI9AW2~&VE<8q3KMk*h-H9_c@LSAwKp6fh?ZcqERi~ z?yXnTn9@hh;S^QkrKh6f=UDuEEiOUN)>*eZ!dznYZVY(@Q8peuTZ0iV6Ufw*P{a^0 zYWtm%L=mU%F#@HB1xa(58CkOPxrAyW(^435lk=|;!-tYW42#ioyEQKbzoe9b#%0oP z`vF@hF^mDMuN4hwgx}eunjaGEJUX03C($ec=ba56HHR z&(=(e!QWIIN?FJpmG0veJKMw89@`rNR@tgLG!)!JR{Lg7^fu+fryw_rcnof&4gBdp= zX$3Lts6ME~8k7y~ZrSKx5XMUEyrUEXFMC@i#H(K?Xc6mA^T|mdWBpY~>WZ8V)?TPA zibxzGo4&H8wua0y@L589z3*VZ`V;#L)R3mz3^vT|u-qT|`LfA%#rLmj=zQGU!k}?54C?_P)RAN%qUaux3oQ*z%o$ z)Z-Psz@JHt+m+WIQs8p)%uSA@eU<3xFfs`cdyy)?QX7MyKtZ0D1OJjdD*KL8sRED> za1e2kew#vL1v4#c7}-?`uvXb0`J1z7jYjpk8d~afho|(R&#hptGV^DXUmk91q+6ko z%jzu8)DK`b^w+@w2t;UN)plrG#tc~%nPQ{Sn~5#n zpsv^7_uZLbqb}rB?b!(bQ=#S@-HxCWU&WGHMbZu3qCfNHgxBM3jbsb2L`^?L-J|yF zE`8PHdSXx!;x*Z`BQaD#GOmE&Ry#FYpe1Mu^N>J-XJ&~i7iF+20}i5ioX2Hn z3e*gi7_s%8DDVR$qpyVKOG);|x4)@9ed@D+w3g7vx-{~P-|F+H5p)?R32Fys&iwSLC`wQ+ z_E~*lf;m{|NI3selsK;{{mJ&pT`bz2Aopcca(c%p<@oUtCndik~8(X#qVqL&PMXf3K)IT`}s@LO^Iv^ zEA;3#DYZX2czUI|p|Du9wJSeg1;5W78PSiL2l!0i`usiU-V3g=N*{GVagJ=Yjk7xF z@V^T#?->uK{WAafXd5rxBO-`n_Ay0QW<9%ab$RWo4>QI0n?jt|WW_Kzu0Z;nnDnI8 zx%t*xq%RF`BDm^Wmpj}?sbTq~liQjUrfY*XHoS&bSdfkX^4)oe~d!~xRL>_ZURGo-Q3$CWjV6x5i#&NMWE|&c9P|c3aA7E%u!D*R+2#vjR!`r zhWcS4t5*#{Ay%q6FH1^IGc#P$4W}t$pSkacGbuUp_n_J9pfmvE@8~JYHof1$DMv0= z)P=>FU@23ou3oT)eXw1+uco?UQD6la8rUEekKqLgZ^dY8Jd1LknWZ zRRvpON{`l46DdAmop5p4g5eF%d?=*7YTm)Kx>8>8w)y!4*amy|@m{XChryQ5gusKd zkLzoYWC@Q(o=Q@g08*&wuBJzr%q~Ep9xUJyv((*#t~$g{0&kqj!|w#TrzkseoX2*^ z5_-ykmGFVz_#l+(bxc`iuKhE)PVutpw+^WYac}@mlng65@VX~nEOasNSDuKlr_Udw z_Q0p$AV*cBkT!T zlY2*x9~?3azSQRs7E~mk$sMrcaWl*QizY*7V_dYHj_0X;taW%qEI-*UtXnCUIUAl0 z3RWsN(oiocs|=tw85yHZ6bU8`W|d+oxbzqLYYT&&Aji;l`OMigBzAB}@eF7^J%p@u z7(g~hf4{cGfWJ-FtwIH~oHyN_jf8`u{Xan)qmh686I9AjFCmM6ikDs1G!<|ueOl~8 z)HW7JC&M^k zKRWjWOg{&qYixg{EDJcjBOh5RC#s^ugD)`xbtUSx zI1nwNGlqGiBkm)M62({+ML3(Sm47qt{WWWYLqVL`@^qU3FUuvC340p54yf=HmL?}% zv$?t*`rrnRi7UubO|Ks7i1qPjWitb=q6d>7ta_v|Q-d8z!#1FQMYm9;8d3iOxyVES z4=mmZ1*~e*reIg>Xn7Qsz5NpCIs3xcr&0zzU}c$)Fs;Rj%1Wq2H>ME{0lx~1$*3{Z zHTX0@!Sx8^Sx~>ybAUw3a31@sM^M#fo3A{RVV<2O?Va}4jWIX&u((}G%b<&JfMqsQ z-?WJ_F$5ans&4@3GnPz$4x|sqiw%Myat=pYleNcoBlWEtGQmx+?j?b`wQEb5H@7>L zDiD5MR+jl>v;T(2`CW(Y5cVDZ_tYOp0H3RQY@4HdD!rGRUp|=CL&Du1I-h_C)i39= zL*!F<*iFY80nM@7P}-~v?G^^DCnl5U-e;x?j+(6bzXX6E{84S5vN~`HSuX)4oWbpN zD61tO)8`JP;SZiN_B%ZJphHY}ET<|m@PD_{rrx8ju8P`3k0>B)S6_4XL$E^Z7xQ~{ z5`8IwDpsdpf|)c6*?aL_$W~u3Mv!|LVuKH~NU!fRHzCWA5 zz|=ZQQ9D`_pVb$uRM|b@giw<)^{qM-#gOuE$(zQ&kUq^0mtb=usx6PP)y8S|&VSoY z-0kCn3^xUiuqN^^Xp}3ty^{wb$Hoo8E^@Prd(Ke7T!gYOFTHa3vp#5CrNZXZnx;=i zfG2jN`nrKm+WNFY)l!87YDL`mW)ISXt!LXilIUW$%;#ytR!6a{EoCBfJ+}ETDo!tG zJFqo8VFzr?>yr{B-HR+zekghe_O5gV6K0#k+CP$U`~lCd`-2;bv$T@@DJ!FQFQZUX zkAHp|b0xQXWA>ItqDQS|F^#U%{>kT0n&bPQaQ1)1uS{(0|JGJ8G5#-{W%@^M@!xRv zZ;`i>njL97S8wj3@2*Tzk*^QKx)m2a6Ud8*#yb{={xA&`8DZ3y(`x3BouFv4o=i}^3Z^!A!ac8b~$H%?Xt1G%oziz?c z{Fl$=w+lX(-}S*sA=V-cfTRpd$F2I79DZ-l*01dz`WOJ{H_Q&;B4!^Bs><3@S1I3o zz9%uRBaeZ?`BK*m^&@gSWjBw~d_5^F&% zm_Rz8Di#dqT{a*d(ypyC1l_7;U~AcAD@QDl#mQ}Qx*$tvj5#73U~?9<#4kX|X$F>= z^phmJ(p{LNMKp=1@JTzdgZ*4j>2MyUzZ`fuKaUT`VEjc2$>f1W>|ViKN;wXeG-E=}t3J&pQm-zIK@mpHQJMWRsT~2+xB#$< z8A)D!8(Rlz#BfpKcageoB~iWZO0`a5!P#)9@%K1VTCmUogNnMO$U7nSnV`C=vcwO$F+?{)DE(CM?T@RTzyGtQyU2o(v$3N;H*Q>i z`vcB2{4_IeEPP<*?yyBWcYZD7{^&+Qyi>ws|JCR^l$-MCiCET+R!+b@yn5I`lF4(>4WROzs+D}bY8F^L-uTW&sj z+yf>R5XxY<1kU_Ng`TS%$kEBtC75N$*ugaHv)P=MS79hiwSB#Bs5p;7LnEH$7alpS zCTdXXd1qaZX~MixLPO#mhUmE$O>~Gzn6Dyv^=o zNt59~tq>C`QbJQx4YJOPpp}&#Icgs*Z_%=ca=j)ek*TZ`I}8aG(KvB7mWp#tP8YJE z&A`E-AE>M9UcH8qBfDpE^vp0G4)g%!hsj@DPjF$*P34*CgI_E^e=aVl&b;)+{j9@s z5kSyt9P2WjHWBd~ttCWE%ILw86q&|)L85J8g_B>&3Wl~lSLVnPbY)E4c&*Z<+zQmC5CJ@ zr)vI{XaWVdDG?Rq0`N7`xe#iRsn$EL%OdhZI8}VJnRnnj#2^bS(Akoe_nj&g%kVZo zJ`@uuJ;qg$u9ud5SNt)vF|*Uf8T@&mwLE1|w{?T-vNM;c*SR7Xl{(Hf4ro4E4MmaYCBdFby zL=A0i8IlvdaROa1OF@*)cUj*tcF6CfxJ^qZh43k=GPXVtvo`%$DDZbH`GYXz9K^c- zwXCx>*qg^s803*bX0Mg(dp(I zxQsfy;=7j1w|i!KK%;UxGu@oWr0lwHk9}HKk8&n#J36nYQ<%Xq`N@EggHr_MhIK=- ziT!I_@khyo3)NLuQwBMTn0k-1!r@Q$N~mtt%n@_&nRP?hff9UAz$VS~cISz2UiELq z0-1vk5Zs!LJW!vlU*!l(?x#i_{@a0wKT~GEH8#6P!Suu%k_F`*RLO zA_;`jGu$rAo*adhW74x(HN}H<1$K{d zBh;)zCK?}#O}cQrg^?TT(zctd_>9vA1mk{ z-F{Epeqm$#!QJTjb}+Wr-H9#-*JERb2PIn@>+Xu;L3bmzXp?ots6DaP{P=aFf|YAc zfG)~QPR*)0i)DB^weP0JNAoYL_#-=DEQzGuGCu7~!1w=MB7i3|bz37pF&oR5GI83w zT5dnR#FPKMB-dRYd-}B44M<*O%OMM|mpV~}=fS*xb{G75{3q;1jW{)MIJ{7ddB299 zulMJ+p}W_s+|pmJtU(zX6h|^krZ%MB&?63_ia|=%b&^T7Ic_mW=fi{8 zRox$87eJOwc^fLq;huo`n`w2|#{H+8jTqP^yK;boh1GmuSBkc2jG+?Z%Nh98Xn2?F zD5@B%5Vd&^wQ)Fttzt2+F9XOpxtk~ev}}|zhwo;`TG+Z$EY@(c3Wz5aNA)+-AZH+k zI`V!_9qGoYTj9gNSTqvs6mF<6G!=S7;5!M3kLBVUK%xSbxsT!@<@X%Et0O4pV_!P` zWype}$*U?N;_sVa#=3O0RW4qY=Y3)>mh=GoJ?y$o%yN()38)j|7Y=o?>(S4WB_-Tn+h zW?x>-9SVCtR9cHF7!fqnE|iOh*;HN}5Fv{&D%2tUY?888x5TqK-GtUF)WBRMDejGe1}S__Z4Yg<#FLfHB{uR3q+5ijjwT!)G^>`7=uX{mjB1wQLvgrxeZkl znlaM){g7?&+o*<~i{J0gxtxm+~AYG8WAOd)34U2d21=xV-L#2hC@}+WpFH43-`1&U@FE5f|T|Kym3`Y z(H!9Iw0+j`=v_T0on#t_~9l3Ex96L$fnIt4^ZheJ5Ky2y>3 zQIakQz)~Jl3z?aq1K%ArF7r~es815)Y;dV^^Q9FP_|zf@^O?F9!M5`8R^s#n1uHsU zEn^X&H>M6@F^(H`)sKd!vlR+|s-Yb(#jemxp5*>hBRb2`#aChITAa-F^nlcz((>%z zo|Vs#oD<|`!{9_gK8^(bFxz=F56PtZtJ9Y}BqsyfE$AE|aRsi${q<(yC{VS3#*vsz z5!nfUA@~3}SJU)dqFyeSoPr8;VUn0k0!SI=MFVja#KOc7UbX;5EPBvFyZ85Iz`hm+ zvxw3$?p89>S5`PiQ;}g(A5@E+hfJzJ8x@4rDXE%f>qhmZiq=khGTuTB1@Kh1pfLnn zl_D29R79LwTk_=B8z!_<{Sa3DxvQ{6Mc-2T>5iPKKhM2NZ({0gDn&}KS|+M9E$!E5 zn4snh6kz;D(8J1{K8ONL<`Jg1WdxfusBi`Un%+vAc8Q}$p1BXlt%|q{t&`Mr*G$CO zvt$!pPg78_jzF6mf_b3&9QnJIs~{9RBr8MRX0z!%3c&X6i4%qmb-^x19()%LnjGymxf>p17DEL8rZdz-bw2603Y z(>k@f@vv>?`e5hN=R6%C2azij?-1Xi18c8m5#yz`X~m86i7zW zR&XN~UZZVGifo1?J%l?wPfrBmyjC`c+8L*)CT8*My!>?ZMjAXHj0r<$$wGNLkIwPk-LF$=Ua zEym6A>2Hp(@0#P>&!^Hw+-ApeJj37;o8_g@(8Yq$mbQv%BJ~D;Y>J#|4A(c%HG=LP zlDw6vjiZr5$*5|~jXHFGE&)TEV78NOx$T(17?9~5Rz38275H6cDPL1B?Vn4z3Kyq2iV-4FZD~I;fijIL6IQPPPlX|~_on@r-sOAkN|E^Vu-|$skjJ=O zjD8Rjxi8n%pi+-Qq-?7Ju7l31(c-|l7E#foT}t2l4Ins|R_pq*rm>^On`u-!#3HeVq3zcSNM19?XSacwU%pAKuWy$j;#+pV7bIynw^$}lJ^5=k;}R8i z?e}ULrkI@v--jFSnW3Z}$rhg~n{cMK>5*kU5G!~GMeO}F7Tl*24&uLz8S0nMZJnMH zzGC|d!9DyZ2KxuC{-1#*7RLYEBWC)a;q!lQQ)c?7ZT#OD?CP&_^6#8^&rI!DJHMg= z`~+A4fPEH@hnBt{Uc~l0fCt5F_v1|}%2N{w_geNZxM4$b<`$Z}yZBFWv8>(OlMQ@u zdxY1_^hpUkQP|SE+qaX}n_kY(>6hD`6TO||msW{gTC0);C%2!o+bbble!Oj8&cLp& zpJx|e-;X8_fFO&ipFNu4BM7tnVIbE}3wpbd4UwGSJ$P4vS=)mn^Sg$&d3@*^y~V5h zgAdoKg~cE!b>2LtYaZ-FWZt(Z+T1Xt0e&P8OnGtFo(>#vq+@}73x?FfN9eo#u>CFj z@Q=V>i^)F)ONK~A7&omT)k$aN1wlON$yEFmVVGzL0Cu4MCp1Vu3GCs${ve%Zmp=lS@LID8F-v{YHUmm z%3Z65z8f51G20|b$vK~9;q0*PPZUWE3Cf1Y8~vgU$W0g`Sf9wA z5t>k~qU)OivnPeOc`oCfGulFVZBq+5SMUytsb4ZlbUL8wY`mDC1Ae_0o%SiySMaxaKZg?FXuRx3y`AVhCvY~F zm*0z4?zB0tATm$|O@`J6SAzmm_4ARu2p4aE1-CMrq^xSKVu}25T`^CNhd*}StJ_lQ zJ{pxjGQ_&`yy};ClPrX1sMDaqjU1VMRC0(5LJH=T>a+@yYRT4P4g*Xdh|9FLqbP*7 z0PgX-XbEB(46-PBOv#Agm-Y$N*63d@BHe2ubU6G&O&4`jx%zzEb>c>?fX%Gk_eLeA zyf~PTQp9L~sNnP}}?{2myxEN@9tcL`&^s znX)v@sg~Q70k)V(jas&0kFpgBevcUC2)vh=H>87FWJ$Ec1dUQ%91T8I9~-hE{`9qt z;D%Af8S-&ANy>B}m(wzb)^X$a>;+Z*AUKMmOYLqKSGQDIjFw@aFn^eX(^_#+3{>@l z;6q-1{?2cc2XQmy9igC8U5@9$49<)#GfC0)ikI6xX;FP+-ee0LdVSjCC_*%Kuj0B^ zel)39s$C*X=nLe*Gn23=z3R#HKe#tohnWRHA8(UESon_wF}uK?8>o5brdG+yn{@Z_ z_Kt&?+-yYs5roMoWYvmEinPkG%2~wThS0KIO8xmc^`cS$dUQ@CphsSz2AK{!C@|P| z*?IitW_NFEGQbyXL5Fx^i}dp0Uqo~t!E=1w8sK90wvOl6sp2b!pYC33rLj}EDM7tu9hbDn05PO@o*f?@Nwdpa}rRF(Y zLMj60YV+PiR~V+LEW~wBGhtiTuBX;#_#Tm3m~7%%n%l%E^O2tonsy4U0k#h?@%U`X z4@BdMrpjp?bM|r?X)rn=*OsG?AX#EmNv`rf3+F5cE8I_(JkYRXT8`?(j}oSL7bhPftGN=&^dix zQ{!WtUKzQpqK%@c&eu7(9NW)qMA24Vh;9IU}j9Ji=C=Evs`&ZTJ66OH(5cW=dE zk2S2a1HJA>{fXG59@BB{`BOUVi%%=Mjp86&FxJ?& z_-;Z5@!o=|-a;nL1C&q1lkM$vCXeU=TA-NO9DQ(*>)qS%kxWlpVo;tw&;|U>`?D^H=ZWHl_XHyd0 z`6yBUZJHV>oFhgCDSkV^@D8ksGJEg_a`g|qc&86nl_x+no2m)0UCLReoi8BcmZj5w zf}a18z%em${=4wR{Ew5)KPdEX=wbe+x%l6pXG>ekmZTl_@3=^rqo2KGEfuOhM%~tx z&~HGnF@t!m2FQRlfHkRaABym#Uy>CqC-njdF#1}w@%x!H4OYwGjh`R)wI({M@63bCAVrY3}YtJ6Abl+)^%;2Rc0&iOURiVcWPpjNxR07rO7#w9lTpa~%x)YdGoqEY8Z zq(l|8bS0hHYAX$606%26MqPyU-Q|^n?s%X3{rLu;x^XgrUkdbsT`mfp6{iXl9#JZ% zAZ*!{GFXlUr7<%5#Ap#m0!P(E@j`S!)8w$*{;eN=2l`f6*pEVpOOrYp`P37H}K~>T>WUhQu`6mfLm%LH@KTVWayi@zq(Tc_U}@JD9`uL zSj;;65N(*e%nEe~`))?LKSaRw4fXwQRstk74c9A`mzzOG_K2xXp{gsyfUaBzo7EeG zY(`t01J0z|y31ybnu)3;FpfkZVnk6lkbAtKY1wv8%4wLWO+@W+$Ky1zNe-71l(57L z52tLq`GWFEMIS3q26{e}gv_*`_a^znIp`s1l=@uFP!0^>3u7EiY1NUvdK{}A9-#p` zSk?nl(_+2$N9VK_%_>7-iM8X8DJ1t&QtYA}YYjOdNNNIby2iFvKye~Re=Yw}E{Qxr zZTCRbYPN|yvBmjyl<43fML8hrkx*RX0BV5n6SE&5potc=8NTtDRI{YXt8j7?vmJo< zm2ldUr3E!2Luo#O2wFzDl;dxZL-@S{@8gOe*;Z}Eslvm%g@Pn+xn zBdZgHp;tKkRpJ6uTU-fN=j;buDOQWG+D9O(w7ZM1aG|c{vx3t0fgBUi1jHqwwK)6# zbrco%x>OH~EnpveCZWA7Mb*%qaX zW~FW0woz%@Mx|}rwr$(CtxDUrjhp9m_w9~&9dX~UwPNnM#+(ZS-yXsc1)+jE2)vf7 z6+qZGGS~Zg3iZcwfUeSQ>_R(%s4D%Y_}dn1gFjUJ#fJ$pZvZ)VxsWTxLT{77BdAnu z^kmQ(_AE2iqFwG!I-G`$F`6#&*3{1{4^{>??J-Fr4xD*&pm_|gzG^EnlzS4cJhi4iKj zJLAme-nX^N3iM>CMgUY-26*7THf-prbJSXmk68}n!d5e!j_PmbKB-JFRb;(?3%2jNr+n???IN_5yBvvb;uK%={0Wa z0%&zHaJ)HKsDKWtQHkr*RyrO}#IBN6rH@--Hov$zo?16pu^bqTI?-R{KMkd=j99>6 z@Tg3uXJ9-i*JOo)K3W!7o{kcQ0af4*fyGo19;#a?27;+wHbb?GafR!&LuMX%%ckq_@37Hhu*6kQ9}WRt~RlSctrc8k|}&iQ^)b=MseN1vJ?h1;{?Xu5`t`L*;YbyartcLLWR?-TH5)_j^Q(JP&_^>^v9 z3HgOl4LB}XD!qSxb7{dCn z#6J#L6HW?GWv}y(4mBHD3Rx3U{ZU`9br|XG0lPpri(Np}%?=4N=T)qixvEbLLG5e$ zm(Vu)_V>Nmo4U4Gs63hz$8f-l3h9%lmgXOLmNt3f5f=naV}Gl2{~hE zx&~qpf4nBi>Cw57>c|MIGArsWd6H`l`}2Q*xGGA(*OJC+!+qgX^r0b-SF?69j)JVJvUVJ@ec2ja}(Y2f11Ru7MvJ{@FPN8ROwR|>=$ z%l!$<#6QoLH@dvW%GDLW7h*$CpDR=(Rz`jS83z%X#=2W;x=h=x*_nka_@ z){6guQI@)b&pZA@Pg8nN#Bt}z_*22{4Xm#aV$_PTa|$}H&sEF|EHBSj|L1o(aTX|B zf00U!VWNV7Eb%)XNPUtMH#J6Mse1xdf~ewji0rC{5bpFRZD_?krlnM)<@luraKlK5 zGE0ahF{x1f(3t~@Xh>j*xQCh9Z7hs^CX#m(#);9u#Nw_p>30@xgiHgXJ;?0kL{o_C zrslf@o~OiV;~h4_UP8dVSj*5jj`}9#dUCmR8nC)3p0?`f-zF7>q;K}*aIbB`e{4zQ zGKPtO59WLIgX3FWCzM8ziO7osFDh-yyz>=p<&(=}jz*+3z1FO6&CH!RBm)_`>O;s5 zm(|$y#T+V$>2JT|*H$HmJf)pp>!VmQ0X*`urZ)4OJPat*{M5>w$hg}x-EHMk_?i#S z6~(R1KFn+e8}+MOta>g5Q;naRP-WQ9^|4jpFVy5hFavwpZ8MOSej^{&4+`Os)z1<5 zMM(@$=OAj*;>a*pC2WqC-2zo0vSVnzX_BM*@-Owl6wNKZI!QuAhIbw(F7@JcmFxom z^H1utw)Y!ID=$YyF)zY$cyNJt*BQSeKce%rrf3AthfGUDExu2)rxm7%h5%n?6C5|) zy5j_y@J^7F)Wl#1(AxVRX^>PnDqy0Sbr^iu|6)3x$H}a0z3`&i_nf(RdDdjge*HDH znT~VIL1vv;4n5!P5KA_7PMq?L>pR&j%IZ03)kgcQ*ws{ZpN-m-mV;VF5MF@~<_0Qn zYuloNI?I{xqg>{6HA^6Wbc+HffhujM^$rgTZELLnn#|5< z&ie9kE9U}~-D@S_P#<+gxz&omDjsx`{_kM(asaoki13e}#KScf^G6L*X(7MRg-Y2G zyqLt|u8p;Sq3=# zSxKJm@*u2)_iIIVj##je6n;18%?4lJm{j4iSczD2KQ{}`)ygO9gMHk?iV92xI@U;!@2oln#vnxhS6=(0j_&2DDsqgrgy zQlQgYyeEnEYlh^UDWyDOJ2c3xNl_aL6PAHI`F&V+U<$?~^yrni)ox0gdmkC-Nxrze z8q4;K>9oy(^=5sr&530{a=IYkLb+K7gOqn|(YT305qT*f%KItW0h_jEI?~O5E^&_p z7ZW#mI-hw)(AzLG3-sMwLgyy+)$ZLlJK?N}4JOY2(|bXV;Ng!>4)1rkKrqsK*{9hXQ#%_TSrn0r=gI)?T)mI#z@ad75<)@4T1(U8Wu zsnd$;8&%aGY4_FLA@`2-0jt>)I98(^2=<5*xtLylCAHCG7CUxpzLHK&U z#n>eO_io1={*lW$s2r(@wl!_B@TE#Rd>&QskC1B9*0QzjkVl=Ji2ug|u0?3)Y_)$( zp1Xn{3`NSgIIMEm9eiI3l#W;e8~Cvm1F z>LMH{=@AC_{89=B5#b53*yaieN-zj=7lB);z0_f5Zz-W~7r*9PM1s%gkEyYP)xeEx z`PX`cxfOMtX&UQ>R9ID%4M4%9Ev0ol?TCgah2;T-T@Fdz+TQ3;;dI~Ls7U&Lp5Bq6 zDbomgieU1w$POD091C$79L{5K$ z-3H=mK3`21Ym!$0f`P1NC5P3M<7Db16!8X@`@KZkWTz+gI{$O9b{+hrV=W6S{etQ% zk%b7;V0s0d{T7el{N5&QpHHlGbd7XPd&HJ}_53W=K2?BS(=9)fqS;->du}RQ6)Y6k zu=7dH0}ndxTd8P&ERl88#%svirz~O)hrk^tWL>;T>yt%xE!ijh4hemOs#dPP?;mwA=&RuDGH0*rDmJ!z@qkUyh1J;$va5z*FM@`H^zN-$3o* zt&SkK=U&QfZd@3q$-7n`AHHHB$K4au(q{`h+etVRIl2rXw-8xgiRLwMk7cWpmU&{3 zz*LejWI{evvTXI;uAmHbw#QR|6-VYJeNDJiXu~RrsOMu%C-x-pA;UwzH)QGDrCr%s zaF;jlS5$`i(CHYMDq8f0q8PFk6c!!n8dYd_uZy(;N>~gCi(?^wAypDMAXHQ@sB0&J zAXts}<^*3?K)zL0pHbkis)lPHN)r zsx)MD8jiqAI&W3ps&97JOyaw1{7qtryqenT?iZ|?3`#(mXz1WHw8f$(D}pyx8_%@T za@WLHimX%QsH_v}1! zkRR6q6a;(`_+rczUhyq3$!G|s+X$1=H57&=*bw8)q5y6{R9!2pSIM>ZK+`)28MmKC{_+96Yy7 zwFEamc_ao{wJTw4{Y!rf8c7#VLkLOnbnw)H0}?q3l%AISIV3Vp1QDv$(fvU|546{M z@(KJwf+gBZZ7P?SQN-jY+Hfa%`+SAP@Jw=_LVZ(ZiZlc=%L6+?`*^+Cdd8hhC2V~f z1*KgT^VLyARL|o!VD|LLm5?SBC$pVdsX4xR37$LlI(r4(A6v4Qo&RSWb4&VpP2q7Q z)ImtVlX`b|jVBH>?Vu&6n_(aRf=D+FiN~DJ1r*PQ=)?hn`!`V61*HCobhH;^^kT#O zrZHTvNXzKj-q0k?aGYIWYh=#nWM8ev$JB%mS9*Bp$`?|Hbx8IuC?Z2dzgVHk8!@ld zfX)8eL`Js9bfAt}*M~O?Y|wpjqEb_nZqox$H5EA^nZST^f?hk+}Zt z!d=>eh1$iN3+4qdRmo`zw;iD4bSmV}_*R}J&V7o}?ePLSO@#tdeqsJ7l1fUQk;JI0Cj#K*5P=NpRI>6IG)IZ7_j z-xc~H9DCxVWp~vzW!C^);+S|Uuz}=Ejjz?e0BA`|@paVkEvB&1#C<^@ah7^DFYnnw zQkY_E#1Q~ci2IEn23u1lkj0@_9O5Mfz_?36{kHgcK6L^#VX8mobOfUgDm5($Q2BLj z%w%>tGwcjt>19BTmduY$BX>QHUS*qMWEgHi93LXR!ZSy3ysX%%k_3CH0A1`>oZbyX za>6iNV=j5nS2;{ze+z`Ayx7| zW0b01%;_-J+A|7wAr5-=Uos)73bTa)ln6l;TWBw%^gt%TVh}#GViOA%Pclz#>uHM zM7}39s)3q~irz%h&tEOQz7{B0slBEl9Ylkt-mfrj@Xz<#+>UMSWiz6i&MsecSV^_| zb4?-hK(3LI9LLy#$`5ncE9|Psto+Lyt^YEIb$L?}YrPC7%x}AXQ3Ns-8Axa?6|hL8 zAGT8ohqum4j-l2oyQb8IRkTI)T=$j$I$NFZWtLeX-}?^^U9tPg4s4S2SZ-(HsShmb ziqOy6@UEw2<`oBGOkg(k{)Z^;T1Rh6P;^50KO62iBKwE!JzA#(L8AH?g#6D_u9vW1TPL1qpklFhR1wIY$mh(SNNcawIJUI#-CF8f zm5%llqRrC-Y#=}(BOJ39`33f|ppK1GS;l=K z;DjP)rhiq-Y?IXait?o)p|>o+_pb8Xf&Tqc)lLYJ(_Nnir008XJLt^A6>BZF+snro%YSnkfZbXt?a zAuX>MajF>IsNb*6n{Sk7|K3?G1tiJSU2RR{>*`w=f>lb=QwGQg2p+ zZ4k6xnj!kiTPyX}s<4-S8XlS4@1n>o>69Q441NeAs|wZ#-I2r37F}P%f|*B{VS_mI zd!Wvea^_CVX;t3ZtVrB|V^D-^xNwIY&v0MG zK^^Z(wwJXq=?>XenDxU=ZznJY$smuH2xnJziY*(Tw3Md{<-X>-udkj4}BRfW~_ zdLaI`{?HlD0)c*b*d4BGCF&r`2tN%NQ7FJm7uQ%^E2sKs>1&x_s$u>k+045@he0z1 zDG{X$ZD77l=EWJ7S;zeRlQf7`IT{!W(FhS{UHYwC@-cTx!?kCv3dYK~=ifsrW~}jQ zyP!fhm#7xudGe5s#1?Amv~S~(onvW$DmtS$Qv(VHw_8VG(ix(9H!j7u? ztzdkYtR0(P0sS+1&XNo{J zLR#e2fDI6N@ak*_cVdwQSxS$3$t8S4CEklGZbSCFx3`RUlrmw=sNl^p!z6|H`cJ@f za#Seq6j za7y?1>Lj`DD2%bS-(3uvPnS9<6a9xs{xc!jRJ_RloN@lXi_&P&$5eIBV|MOC_hHLu;!y4zty>#*eirHB$^w4< z3+2#$O&N62PnFm-hLJZ6$d`p=ZBY4YX@IVIms?h9)9Fd25qYILYTvTUBjjO+u^g8o z?M#HXQ6_SYErj>D^4D_(?(pbWMcHY|lFO4Oy!wh})-*Zh8<tDwvOfWZGm>ixeU z@Fv!THF4YJ)*ty}kc-gxawWh>(6vwYgJxepZ2ygTY4{7!QJuT@JYoexlBCPD$0K<3 zeY57l=K`T*%WS>ZnbYkEb!cym{Wr^$&4K&wkGGcj9B;KRC&rl$IAiU%Z=cV1ib0be zOH_EWw_~SY-xu&UucskMz|YubQyekxgZjhwuMr$N>KXfeXYvXFCCm64$nDgeYAnhL z4@6;?h=4oAe<0i72`!zD>j{&E@H#J~52*#O)zR|MJX5 zvx~V)10^>u=Ry>0%yCs}d=)k(Tc=Xu3;(4()!q#0D^`x>{IWl5!mAYe!s6y03^crZ zb6(>n6;?}lv-t-K+d{1zUY79o__AF=(;c}t(~)A@I5%im2)tb>bUPgPbbj7C`bawB z8&5~c^$H0H(82E-<1O`9G%XPzU!w4myg;&jxHWBIHZD4Qzmp+2SaZdWR~{u3Gdy*Y zAmEB;eo#SK%*ZBt17+p_UWKsuq%7DLU)&vkf(UFGPIuy^WH$@(TD108d5Sgu!iX+^ zGi;M-rxr(~*Q7xp$9Dd@)(JGx7OR9(F%4$4yrm$1&VfYEFIyCl|hcKx)JMqbHc2{Nr z;W3n8-?QI<9FlbOIQ{HJ4-6R;sr z6#3%V>I>6EgsA|`;)_UdAyZfg+6%*cwGR6&@M-W@k%vi9)Rut#s=@<(Ez-fs?}imM zBM@nv)8XNQIvQD)nGt>nuW2Z`%A3^(`%=b|LI9we!Dw<|7@3hEyX}W9BSwX*0OUd3 z%e6w)ptK8XK(vhqEko*wE1D7B8(O*fdsrB|f*6^ZXSYfOu>5sXur8l54c2xQo?`3> z56u^{BA}dSGdzK2v2hf=tI#hb_{S%IbzA0{E{TATQ8m6Ct z!bqwacQDbdAA9?=c(9s5@5|p{cJ`uYQVN60vUjm+uLty;-Xh;7JtV67vBq`EG-VFB zLWkIRSJKU6OaRC#{k<*GTwx-~3($c!v}#BBqKW=!=mN+B+OUYO=ym}*>L}@m%e|w<=z3jAkPW-xUtSN(q5LC z1@$!(pUN4;^a-@#?rxEzy{RnYJIOzn;N|QFG>FM(bvzq#s4dcmBv)wFxwoDH185jC z^}u4RZs%kS2Wj6JqY4QoE^EHCGnu=~Q0->z}7ctgVQEk6f5*@gG$|Z@UM{qgA zt=qDH&P9Jhp?lhlM1yU1kHnj#baM=W@oH3XrF4%+CvrKWUTD5QNOI$igBp9gCeneo zL9J=Z%4GdYFa?wHqE{Ii%r9OKL9;$1x!=V6I7S3UQ##eJnNo26L*nPmS}<*33X>-^ zTGdQ?+j&l!tu$s+IN{ebIJC;a?TLRIfcM9EdPvr0@-jMS?hS-j`~KZNWMn;~t37nP z=yMrZouRw$WG)hvrLIN%dZr35Abwpw9Km^$Js;m`i5<4F(-d?p=e1LOYFay6_^?kI z6Io%x8OR*HBOAK9LCn#i<5 zwF)R_!C=)??Kz929iJ_vE^>DiOc%Foe(>d&bdoAFkjF&lX7 zxUw(I(r+K|Hi}2qT#z|7xV-fkbuK(Oq0Ry|);Q@+Z$!AosFeUzc^rY2FQSWk!Mt-Z zs}(Q|ywWUbA^1KcaXPc|h&(G%f6@29E>wZ_cnXr4QqkvbV1+0PKYxHbL+e{n9iu_9 zriV;|lm7Xv^UL}Pw6LikHNxWRpeT(B1juqm3*+5YnJDpk zSZU3?crH^6d?EZhsm)Tl=Kf)*dAfNn0IQ-u`+@)KQ)<_XqIAvk0bEy~Iv2iJ*1LB9 zv$iVwo2>CzkuE!u+Z8q>Mw1BC`o0D35}xRS-|1d4*_XLr3aPwP5CRT3#9V$#y3c4tj)$WhyUA zSXqNLEe5tCE!m>DRMf&-Q+aqPAn4Y24lgfUyHvV$+2Oqd>7t)-d>}aVRSA%ABg?Fs zr;_hJ%*NyENCmICOS?;)uZ#4TX0GH|8A05S1$Vk;oyQ*l3_3`odGwEFhS_tf4%9kS z(2yeiVO9pEw)Ls>%4+R(8NQA@s=TcK(9HLDYtG5ZuH;%vfhX~#fc8mHHEw3V#)|2k z>$J6h7Lbnjbsp0Y;IjGM)TEt%nMFD%xJzqQrTsVgAJx28edAmehO(T8D0i%-d#WQ5 zr^3f&V<=tODl_1_5coqjd2}2L74aXkRdayNJ*?xGy00gIlEStos{5nlS1gn; z)WK~&95Q$F67L;{QeRKd1Ft8qo}qXw3;k#r=N)%gQD1jwokZ<1RVI3;Sq6{ zua*d;t6Ylipkrzzp6uP1paiP78SH`ehXs1rp!O3Ibejtrl(!QXOjE}pf|UK+3eveu z(Bxsd_iPj6r>Wa6eks$;W-54Xkqi*weK^cdt+I9Lz~=p`@TXyHSQ}r8)tL66u*6ze zMMH|B036;Jg??8bxFW~%2(JU3gOl_j8?87)x>!pie-2yZzEY8 z-}5=uoR=QZX>azsJt+%}`TrY=`mZ!5CMNp-E`Z7OUv-538$~hy|7oK6(~YUZpbcT` zT{ZKu*GVaUIu3Z)w&t>9ly){&c$Ah2B9AO~>)^WNfxJv&?$)3k3^Dz-iMmW_rbZ%0 zUR3SN;pI6WHE4UO7WZ+F2K)2w@^~P}N8`&$PShq#BjxM5r3X)oeQn6pfmyP4#OYVW zcK2uJ$KCv|Opf3=lgJ^vO2wf=$X{LTV66`|Y3UAPKCkV%14YB(FyL>kVaX+aVBnx8 zCYWaF)loj!ghWQxI)anhrq;zBl-o^G+9b`*5~=gl6NcVSTG_;Zaas7{PuP2=4QXV7 z!sdkvWG12r#rK*k36A24gRJ8Mf3666W7LyLkgQh^Sco95t3#Bm%c~&nJT%+MdvxQO zk`yr6ckBCgoJX%IuiN>&SxGM3)DW4@Vv!_GM>ByJ2&?P%mF zjlW4Qt7zmQJK~@SEAMWi)P>d?VC7JoSoT{8(XbpWgf4ee&PeiL9O@^rz`ztoB%d-C zkEx$-T&b05SqY6o#rkJ;zc;A}7;}1zf0obVnbHqv`Dd%p5ioU9Ix8aVMB=sbej|{7 zEUN~kamC_^F^R>^Y{kbh^=ZvYXQxsJe>ST?OcBU2MH)s6&M-n1EdYY(;ae>>QD*nX zVf`#u*yowH(vTMhBNH`0^S2Rg-TD;2!bA`WD>FSe$I}bf!TnUX&_nd3-P%LR!k!oUv$2T8Oagg7}K3%*LzRLfw#8vi7$( zTgBZ+N7USJf7_OaGBdivrh^qDNP!*h#6`&*CU;}CZ2gBpz-NM3pmk<>rTX7Rj$-RK z7K3C=`P16J(MD%(!%`@EaD^ZgePdGCs?f0}n)7=1d4?^(HG(~QrJ{jO@jpXmt6)OlS*W+tDtJ~>_uIQT%yel4D$|rJ{+cyl~VzKM9xR`%iFA%$xSFexDg@3uG>}^R8yK} zs7#y_@7YC~ckXB$v$-&)jKYSOFDE!3Cv_8%wT5!~NkUZ~_jtb+ECgxG&nTezx3O{C zg2lB7)h>r<#O*TX9`hiGWA!HR%%fHG3utt z-oXT1FXVI#=OlmqGPA!8X9Ok+DvB`6b(e(QvO0%~Q$eNbtz<_of%7TRTJt5yD#icXqZ9_0adX?m$|U; z%dS-Z@VJ~hJVlDuf(1+`sza1uh>!nG;U4eQ-<)M`3QXCO=cqGfN8Vr<#*4e1 zak+tRYD2Lp&k0tInbTeW4#kI&ARk`%Tj`kGY?0xG@zKvgq8Pk9B8`zFqGEuCk;~m$ zKCvNno6{{}S(Je&+K80vVCcpqy4Uv{!$UBzni`NFErxvkRv0wU#maFuFKx1rUlBpa zzt4o;XJMmQM0PbqqB_?<&Rs~v!rpRbFUtfGsg9ywQh+r6seg}Qw{pBVLg0?xixMAi zg3)x)TF@*{047QMW;^mS0EJ<`6zU7@W__eUsTL#3Y3^HuQn!2Brq- z9{Q;fxeiRI;*s#hE*)l`=+?Xsli{5m)w~i(w{VB2RzTKQW<&U(#FMcE5eu908l{{e z4V+Bk-cMb)-;znd`S54W9LwCBY;u%wjV*bh+zc?yY8VI2+)prRVYZ>rfu5_tYhf+P zXy>dgj22r)(NCqYnb3@|Z-IpNv3$iND0`Ki$Rxw108->$ZFi6@!e8)I8QoAa5T8y-N)&?8IGuIC?JW_`+T1;HBx2is#+rH%W_B z9XuT7f~yRx7Q;U9uML7Qm}VLF60jHW87ptUyl>D2+)rBg6Nab~52c@j%{VUa;TN=W z04zgw-@S(NZTuJWl;V-mi!A``(i;e;O-IjsD6h1`pRo!U4eY_-`0NzY{eVtQK14i-ltr5Zl&xPWXWj=zM#ob z7E;NfUyn1=^tu}2M+9N#z0C_JGbu@%S=Ec6y*FL+O!j2v)<3zoX^x@jHf_!tFveaj zj1I0t$rx?fw~Mt3EbQCgCxp747OIa?SEU|c4W@@!+9bZqBI7-pW5Ti+z(wttGA?fg z*kxE6^L2b|C`kgh$2$pxTP z6y`OAfXj)JL$$kP@-l&O?v;Ov$dZ(80 z5kieBQEvh1*ynJt95`%3;Lr3V_uDA3E{)=_ysXa8WJJ>$9oaz3=N*|5UU<;vJ_3am$4k$yPW@mD+riHVU8!;xv)CKujq_jepD)+FB3I zEk>s#Y6yfQ=GXV+i9|C9uN<_Rq@2zr$IFrus7?P8k6=P_W)ExdYIAD}sg~I{ebqw!GzTl`=Qi^o08W zIKd!~I=yxD?ND-n(UwglIM zC;BPtI_<|umh(1fxDG-<6jn3b7z&k3*WM3~x4V4VzrQU*P;4*5tF5n%d!gZAgf_>` zmm=c;2E-*eY|-{+Ew*?ryiILoqcZnCt4cXxC; zS$41g?1384Uv&-4ZrA_G7hHRipcvI_eD~U|=b?;sJ&|3Ld^=~Oh;atpgbKBcex><9 z5-nq*JM-t~I-y&Ae$w?)JE4J2xpI;Dd&ctd)dk)oydPRuGQkm>HFCa$wQjYwJ(qch2 zUrY>T&wby4mY{onE+j=)aIX~e!-By~*1#UxmTNJYML_;u-^tBV{I+xW{n*z|gNbFN z^|YuD_gXYjU-ACiTXJs-ilHO7RNYYgMba#J4PJDK%iZ0>0XHCF<$4T9sMlWA*tM4o z6zBFmNNh z6JKU!-^(`c-@YEtd2_;`S|8MkUr=QB#XJ8Ca`_jK{2Kzv!1~|Fh57&6N0#}YsL219 z|ECIjBAoc&5J;1O#>eqQU}-@oqsP9MpkH{AHcxA*I04Ul+b%*U2nY(s%Fopp?d6H_ zdqu7i;|z4puh)mXKi;1Yjr+H~WaJA;4e!tQ!x`!6}!?l$qdb%YSKRR)jJ%h=4M#6{z_BG zF5(hA>e(g-K$sCGVX78Cv@1z_4Sku=wQ#{H?baCvviw6X)GJAI9feFw3|=a-74Rrx z%UWo|f2T&+Yb<0kW1C;~do?={YIYm!c9$LrPeD}Xp8w32Q)cNK9%k~0D5U_xA_Fe4 zQ^1P<3458cLM*q903A!dR-+_CsSO;7&RK z$2v1DpEX`ze>-5xA?%DnUVDF*3~x+j-+kXs=JkFVYCn9R@o{rHy3FumxrN~Y^jcld z@L<8kyQ#T(qUpl)tqT5le9xhMz6Ree5_-GsK-iF+@A3-BhC8bR6> z?F<5kMmhtJ!;$OJ0KYi`^;unOx&AIMu)6)F2SQ;cF~wE8Oi}M5jsCYj!y}KhBSt`? z{<*~l0c=OgMt80SCPVciY7**~ju8RomcL4nOn3&9Bm!Ju|ogay!P0Q4I*XmKM*W(jon3;?>h*jYA81v4u`G}*0!Vn74 zCatORSbb|R!n}fnz1iBt#pkBFpDp1=3nj1*e2C3cdnG^U2;#4^$GAjR1aGWQhU& zQag`?F$QYvXt;^0h7bc_=~M2ROTgm!m7Mkg&pbN~K+Qe7XXql2<{BR$hsWCeMZYx?VHy=p)g2p?*NgjN$y|zJ{Xbd7Ym! z?+w9*-GX<4ANh>x=`vs??XNe}x<1Z3M(jC?R2?$1K>fCGrM%aLJ|YBsEJt+w*Cs>2 zP8ESvyb)y57+SFr_|w3>d(jPySR|MA4p#6|;i0_I^6H zi?5H7Eyd|2I$}vsF^X>%9l?N>Ei=Dw-Rt}$lhp7S+ZmIPT!~suJ;Cj?1!FENM7O}$jr;N~u_{&4xFOe@=WNF&|hluo_z|-h9 z=v!L)C7EjPP;x={wUaekW#8FH)c&DqpCIu0(7v#H2B%*dHU~@a4vI3%n+l%T+ z6i$$_}=ys)4Er9B@(oL8J7_t2;%PZgO{}K8V=Go1b zq8V~g3GHBu9>8SzNo&1O&qf?p2Zeib4ro_MnW6uA=jljPa3GZ%_{~-+aivNBL2lek z-eKmV4?xsCzQ>jXU&9+%1bWeg_i*F*#7Db1y9>+drDLe++-WJp^Ueh=<`- z`f$1900f~L;U6WXg6kr^MJ-_noik<|1q~tHM|RHh#F(&;9cCK^4k0N(MuqLk$7Xgo zA@ULJrRsu7Xy=oBHEs5xh8tNlz-qjhS(ANK0LJm8e#LXx3po`~$Y7BesV`NDku@#B zvCTDctdz`Z(v4F&9_utWdb(oQUexNEnmUf`JbKEm$vG2zge@C850R7!O@T(ncpMfk zQ;19A%xQ-?`+I^gU*L`WT%TF6elFg(fseO@hKPYs8Sw_$zrdHeze0vGdLEfBo(?0) zNd0X;Ut$UK#E4^J=vk{c2BfRrS)I$7Ilp>CXlN{YuhKueTt299?M>~)s+`!6EiY70 zASF*riM+h2BE8ZNnSD6Ukewg7q`1*#y}6lKdJ|f2 z{hFRC!RTqAgiK=7gnvGf+3__<7(9GOcmtp$8a+_D@ntz;sjpE|Z}nc~n;!G{8VAX> zSb35%F=3pEsGUk`C}QxX_Q5QS=uA}zaOY(D=}9&p;JyWvE>Eli788!|XHGSZ>ps2T z_b0gbc~cM{^f{8XiQTqRrowvW=*e+I1ej3U7M>R+lM=;ZZwWTjtrC@|n@|u#W4`_0c`U zyCCQ3a>Hfg#7c=0uy7YQ;1z46%?FMu3sqgyrt3>k1^E*5YSfyxUiv$S6QtARQ4QJ8 z$$2tKkUBjXwNKWXVGizQ<{c?0ySFJJ@!-XZeBUg@7} zX!@d+!_O~LnJDvKvAUwoUE9nwQJ%+%9@9w}o+{N-2TV@_u&tU*b6#X&yP zM;`XmW*iV4oEhY|$`zX10=}4yaPxtw*~Q^DZ5fDS6iNpG*L|BmaY1k?^Yi!eV(KV6 zOv$!G*iHTh(GQ7qm1cp5%Y8x)oYW#0kTbtb?7f{7DtfnnGHmSYCI^jIb^?PfpZWKl z3EwcgqrStI*J?(D@80^)O{tp&-}2$tfRUa|6|&{@M3kti(37P=r_61B6jz&;4k@|k z+sDn2OMf&@mv&$zGDB=$zp+^)ZyrJU!MI^*<<7;YNl4smO#B`m>GQ+S)2JRMXlJ!xoKK!b1C2Mts$jutYvR z`cPHGb%r=afyh02#JbfUquOT)5B5pYx(cj0&>CsVeB2FbNiO; z;n=oq+c@!wZQHhOCnvUTCnvUT+qU2QZufn=Kio0;|Ml5x*I3UOwQJQ}RW;|-b=^3X zcDW>;@LjN^)4^$RWON^tt2o4BdwpRtI^sSFV%E#VM+eU`>HX8}4un?eI!t5JgVLEU zQKN%aLw-kP`P3Gpc_eB5^e)n=+HXDyN>fY$gp4ixGpubLq&{}0Fh2JJCP)wwgEQP* z0dBlDEyCuGboz-aYOSTa{qV2JdJea=!;{V2qXgQ`*XbM zPgPfThFsD-vB) zZlN}a?ljzkQ5rWsp*WkDbS)nO|cnl;FRMn z-I0%(_hpKFeNpLjtq2OFU$TT-!1=zq5?zwiQP6v{z(KQxkd7qUS||~9a0&8pzt)%> zB?{N4&5yAZ`Yz_Rum{CH7+e><3Ji@Df6dQ*52IDSFn@J3`K;ssS=tP)d;>rQwx#?h zl>YCLL}pIL{~=Fg`8TEF|Ee$kzsOEcVvk!`wb^g$+AjgSJaBm6n8AGB5p@6x{m@rW zkOX-GxBUq0Nhl;Kk)ArW@VX+xH2=sBDAOk`oGH_P^M07hM+SUv9O+CVseWRm;CJJG z-HqQ}2YN(X%GsQLf4vM7TGgEI?L0S|8S7r=tG+z$w;Vp}!f~Z9`z0=E{2C`?&JEeN znO!UX#@sRS^GItX`&~)J7*RNh6aZG-jqXGUU^?Cc3PuQz1B1jwFB*>qLZ>tc&QF`Mdb01Iv$4Tt)HFuL=S-CYp$@APwX@nz%y2 zNCvSxh=v)Ie5^+lZFyEIujdvLQV(A{=~oFXd65M$o80N5{81qdiMA;Y2^U-|%5R~b z1TX~pvy5n~qJRVfgO~6n5F!%frNXo5QwhWvhb{HP|4@Z`LIK^yO==6C-Q6 zEI>+!mcJTmp!D^fS;iB6W-O!1mkUcp7&xiGW|}Mg5^eOq(8C*Ih_+?mFKr?$Ut4)t z{EJ;MJ%j;WFsZfE&C+#3{gF9ofieKF{a{?Ry-S5DwFhGK7#?f|daXQ=@Be{aIME#K zISgy%9XV_;s%2aHOq4gjd?(viwLcNi2#$Z%I-aRD>W>8UK=I}%6q@KMZD$->NsXh< z-!7)^W9(2;MW`7}=`_KrZYM<2!lP`%Hx%REAx;!t><-SGt0=>n=9Z`4b~N&fU5_RC zaPd!Kv0dUC-^zGTIlIvBjRldc9dH)dJbO^O8Vw;cr8U2Pog{fv23pWOMSsF~7F(8& zU@RmPU<`-1OM=qu20c)0FEGGZ0`H-+#X+MenuBzs)Gf-0b&)fH2-Q5JBf$~83@AK^z1fzFh6uYC)WwOGcr8^o*yrTnB1592idZj7@&iWdUMOaS znG!SxJsuHYMePD3f&#zjKqLT!U$k#hes+79q8V+WaPRe5StZ>QGc^R zHQvk|vB51#^bSxfAjCmWu8@IZTRUrKRA}sjK#BKLeX*1#(G;RAs2?Rp1JROT!c@4M z&lvrJ0;(U;D`j(x!W+{ojc|me>>oB(E3kNv19Qfo%e@4~2i0&hbHoNme*c96av;cT zjsId+alVFg8Au@a8*7=zfR)-13~ZA4gc6e}2ZGW8%m1_4lQDAqe>MwE@sHWHs;pi8UxZ)^qMWBB zrElGo2-5X1pV0+-8^l+bS_AW;rj2o)>kqOo*la#jwDNlNc(8=c@LjAs_EtdM`XyU* zDmM^*e!SSlAw(h3`}A84$HURSAQ!6)tWpeCK)%fvZL#*#0zKKVirT6A`!A7;m#e;N zT%lL142zu@Z2tV~BDNWmGeH%PQmGa%ecA3~e>1G!9#+X!lB+uwhQv2UL09bc+Nw;0 z4(I6Z8IYcZoRbWar;N(ak$}nV$rIYCW?9xpq!tFPMfZGip(9~7c!_D`o%E9uQ$-8V zRNa&=SM}tz^eU{=RNTDyA4I}UdqvPRyb+hs&9s9DECVHqmteC8Q$_zMW2l8jqbjKm z+&oAdX;?hdEFj27eo_t1=+ii~a|N2NNEDmsP7}!xuNCH5m{bb2EBKKM5X~bMn)-)^ zaW%6m*+mrzrc}9il^aP_5`!)=x}b!o-McTAZ}0#pZcOUhXoMG#2il^rzIS z%q73!^85Ehw+Kj}SFuD_Z_buk0<^_7E8t-bswZbL8A=@;&tF%+@7BcUU&xNUpiA&)p@wke8PJwwuZeCx(RMLaM#!}r4LyDmo!kfs7Six zECGI}xaV>LxOQe%i)9%`1DoAZID6WBeJ!S_r|+2x?Px1|31> z1PH%nO6@$6x@!GpbT{n(BV7WxJ>J_SxbE*%V23uZqjLq>*{G4J?G@BE|$4BUz%(d+Sp{fZw0PSLR0zJ zX}_6ocYbRMV0d6XL(Iuh-06MoZ3gQx5-w}aQ9fvE7{asNrn#Ryo&d8sniKAkWg}Rii+{ zk1TdaMz^h?u0lL>N%V3fB~*cTy*rR}PO`(BHnqZaEA9yVpfYuOZ-as7k-AZJ9Pn65 zh&)8YYL>eKredlDU3o#JWI=$ES;^V51pQn>%dgew>eY zU0UFgbvqWb?Z4`^Rb85a zei__h?`Vx<@g%DgjD8EoB5^D&m_t!_FUtPVVw88}tpRl>G(^K7I}PXE)B90}E_p>3 zJ1|Kuu`iEq_ZP>35H#xw3crc)>PAvvNg|kW;wmD#35nMZ@p=Dl&wB#iV#5{h>dt3U z3Bp*w$0^tQ={%e-6Q5cCz>DDf?M-gA^_3^~GIN(AS6yC_jXJKLUDIq$I=AU1duhal z)%W0KxaXamDXGpR=MXu9?b-2?oN0OkRp((1wWx@8GnzCtJ7}MR$H^AJa%pafS~X*c_zUIy4D^f&r2GSvkOBfvQ`Z$)Alhjrp5|IqVG}N)Jcxjq(B} z4p`F18#c>jDFomkEe;5{Ic<={cUIDU+(Eiw?a+P+wvwkP=?!cR=9DrbX44bFa2N}E zJ5WiMrsM&i{P3M{E%lDc<~}$^7?ZKMqLlBm`se$WvP4dy% z6}dvEG?-a=#4S_%H1`+dhrp`5!R3Sb$vqV2pDNeLIBp4Ss^Xg+rPc`W^3ijm@d5Pg zQMbB{h>RaLLtG0FT-;MWHrO*%H>^h0OD`02!mbc{j+>BVu!|(NS%g~2Kw4Gi*KuqX zCa7biBo+;7gP6o_4+`A{0&ymK67jIdqHgqp-_YUnQ3V0kpB=vWgnHsg1Ca?crN0A| z%xw0tc8s9(r?+O!rmtS+d)UQSDCPtc1{#(DSauQ!U;!m@+}n}HH|9!z@15RNPHc10=a7ZBGT1kG=RXW&Y9z#@O?Xp~wu+;qndWHz&sZS&9&2D&@ z#ePmU|Lq2Js;tNMSxQefn7ZCN$d8!LT1+boyQFMbAuGF|qigB1bv|#Me^_UQ;a@98 z?cCVItP*!tz&H5_-uYCTAZm|ye~64kg%P=p3CyqBu6+#E(n6^bR|-?uw=ul~*IOT6 zJJtH~;htgf(!246SI9~X2|J4l=#)oU$ohE613ifis0dANU8;KSd5D-|(CJAupjW_&!!`Np}3wINtU_ zy52+u-!Gdw02bsJi@BP<*^;03AH&3h}Yqx9*r*92k`;>^sa@_AiYBTGT zkQ!Fp^{F&nx$VX2c7L=l&^sTCDW3KQB>A|27;@(2eZU*o07v~NeE08pJ7yME_J5xL zQ;~&Y`8Rv)|H60ww3Pl|RoMTY!qfR@3U6b!f&^8Mg~6o-z73QiPAKjln%Fc@7y8#v zIrxae!n1ntL|6ovA)6-Qh@WQk`u3B@?eUrl4OFkSr%QvSlLyl3?oaR6d)agP$aZ1$ zh32Pj-}hmpr%27qD*!Mbs|pu%XuI=iep;@Tah>PN2D=z*6OJn24W>|P&m}I%A2p6g zgNBe?{{vf4O^9F1Z_dFYBayIZ9_;A!Dv0oO9xVXcc18^IXx~~$gs9CMgnFt$v~KL3 zz@yg2N}G}`g-MoSoKqgsdFXC1(a2e?qB45D1glhgy}?gF;Lro&#m!qkudK}vKvqMM zKccNc1JOaqe4zOykBMujkwBtF!@uFI=QRWKcRLsbKo4y9FLe$HeP(j@C|Z+m_uSgU zHS6st9TL#;IK-g|Q|gNFw9afz^}39L{>v<}#X6N^K+DV)-Hyv>xZ?pn_KV(qY$L(< zPDL1aJo51jzo4d_%o#2a8fCc~byF)<=7Odj{AIcwl*|K|!Y`SJ5V#IV%R=R15oS2% z57ihc2l?A+0_dr|VXva|wlRTD|9xUOsmC&6c=GNlfkJq5S~hzt9N+q9;2L6J(b{Z? zKw$*Bn2wd5c(IELyPIr~e(g0DPm`4f?TM;C>pGwp;S9e)hRJU3SJtdTCw_4V%LWM$ zanPj^$n;S`Cxe@p%F1`eZlR#V@8d<{n{o!!i=*pM+Tns1dnfg(#5FwhZa)9yTx0SB z2=+T9ef4TIz{qW0*K-@3#&PN3{D@?2iuYD8)`)5KLr~cwWk4t8!0Mb~Fyqw%hpwkI z#KpqLSSRv(#VGVj(+!Y10p&IJvBreUexRI@BOOHix02;|B1k=wY5{hzFBG?RD5FW! z1@;}H%#BRh+&ZTZoZ?3G)gLL(r0pb%HM=A4+e`d^K`GRipL22+BR!OLcXWfc>BqFU zzneC;k%P8~^&m7O7%&H_*8oOzv!mPjPEDQX&n&W$_1=tYZjux=)i4E)uz&2AMPXo6yY!ps-TkT*2roauxjPR_0nGsgZ)vijzD38xHR3} ztYER?j*HPy?zRb-SKb2Ej%T<@?issmkYuu#no&W;f-O}jqD>@>M2n`(z>K7&8kooI zq~31+{0rSq;bk_dq&jG-$PvHS;EAZ)rbm`iI6YyqHovGL6D5uif!3}pkM?nemGg+} z!$N4%cH&7$8i`&MB*ud2-mC3H<^>UIQ0GbzmsOc8e-(2^N7E4@R+36G2pnS<`0a@( z7Iz`)ZEA9>ZIoQ)1dSlFhCW6uFJxWH?ZjQxq+E038!Ag*!YKUs_j8&ELSYW$HD4JB zJd!*FZa;WEfkf2hH!MCZXeZzc1|t*l5oNrR~Y7YXTn@Ejt&24>Za~A`zDI zBfBP#j3rCaUczoWt5|I3=Gk$$&`aFh)((PVM0~15VA&C^jpUGG*Rm@fVe78_YyfPJ zwg(=R!^cX!gLkvxZ;r`H(SfBvo@Su96XS{Y8AeGL^UBr9DMw{4mNDamiodFI@2-90 z9;=SUt-K2kM{eNy+pBO?ZK=QXfch5{2iYY7IV0TA-^YZgvwr9mvYLV6ZLsey-4{5X zdp>y6e>(sbGbaOG)545W?ENXc{2Nt&cxu=OCkZ)tB1-R9dD=yPm(Kee<~UY_eLeaVV{&w@UP?3E#N6q;b+R>0HsvWDF}1q@vv> z)9k${OCImC&S9H6oKyLgoP@GM8a9 z$oFtpVh;BkmPvXm8%0WZqc@uVanl}JbDq*KEx4kR#8dfR2D82AMnLAW9}Ww8Z_47` z-b+&6b^$)O**IjsSUPH}8-R7w=INz`csDDtx~Q_*!UI^N#_)1#;8wP@%d^f40zqk6 z4CZ^M$_V1QYt*aKBh0G4V8Xd^Q?}$=+p0CrN~tJSq;q>?gZydS(Re>9lF_&0VDi`B zkCIj6d@Al%OHuju7wYtj9`<{2fl-k1iC`!aN3aj~22pjjYm?Mo66dP0v9>qsm_@GV z>LPldfXbX61C`V{zuo#b76`PTrS%FtsKAA%Yd!uA z&{~>#fX?)D?6?ee2DI4qsWxv0c*cnT()yv}=9ZqD&!2>y??WQDYTExfCSBl~T{9QB zK`tablR5OM*KP7TwR=Xzx!g?m`@yi5i-U>zRxE76?dkVEIDJugj4pVURw*&4ldOzy zNgU$hk@y67pJB(n`=PdL``E4SVb|?g7ZbRpAr^7L|q2Fcv%RnTNr;Gh)xhRJ6KQ;xDHKTre3@W(Wy z_Etc?tain|*n;A-qmKG;GJ0|@ZLh%XFSR7AY%UIS3;!0J32qykL zAVw*Vf{^4c=RxP^Alm}$ElUJ=>WkOnfibfo`;3y1rP~XX9AnECAUpzm{&&9YKdPzV z4X!c9I5g85mN#y}yi9a3Uo324{lj@xPn7mNR*)xo&aL5WvZvDX{b-DuaIlG-XSE9S zmw`kFLbiP32$#})f_qu#6k!B65{u0Ou z-KK<^ee*pIcVo-@&p}hTW*FLkGR!t>K@Y#|AH;<34lB34n-N#P zaM9@BnCK7>`vIHfb1^%u&N6v7{o|ZW`I|3eJKz0A!kR+9?jTgh7<6g{px6HuM0RrRLTao!NUOYrChYI7Y+noH4nWYX)e_Mb1oBb8C{LF(I6%Rn za_Q9IG5&d_H(jCw{U^?qpLR_`%A=D9P45ZIN)9 z+w*nP3W6cO#@UEK>4@ecy}AKb(GPebbeg?p;7$73v8tgmZ0&H?r# zJaA3{fPwMW8&2qY0JKpyo6O}8(9GCAKNAut;^%JnkH7?Yib-#26Y`xV5tF}hzdBL} zcUo$L3$znL4K;k;hj;1uX}>vB4ew@yj6dGj`Qh!k-$+}-sC&8I9>hYSQ*?c})C^!i|TX^>V_H!DHAjBq>Ya4(Fe^lv3Af{>fXd{GSLO>u;!CLbm zj=qEt)1UWr!zD$pn3Xm`*U-8JigSnwR?8EbS%;BMNfh5ecZ~wF1Rnw@{L&=Pt&%=q z#5o;Fhe?}`^43xog&Lyr_rsayxk*K@_`ByLZPYu)515SFPL@A<&L!-7K2UxfP!KWc z7w2#OU)Ce5Ne{4+h(L*xIXXxxW4$F`X4^ztEWe2D#V>*z)MW!7{@NM!xe&W*OQ?G5 zndtxRoXlm9=qZ8NXz*VsdZHej(0|(NUD#ag!u^A+GA{vsnPIVI$6zx_7ybZD)|W8x z)v#3b&MkO_OFBHf3@Cu&1^RJ&em$PUw+~e7^?3&eFr{_#4ZHL3c-?;)X(CkToIB04 zsn0NY>g&ypPnI@5+J)^J;M#h~Ka{gI*8zEy=vUF^--}thI{h(7(wenJB%PZywUghH z)WaMts$nE+`@%{LhcptHq@ymxf*^wTI|=ZhbLU$RB`uMu1V%l_w0->4R@ZmQ;LR^$ zQDb$-g1;&l-trQ5erS>_U?++w>$1plte60ig_kg^<(HB5E|FwAs&!tloVatqs!aIc zbU`JRr-pb6U~@okPA0Jc`4ev2rsM^&lYa&CQ&e*s!G$?q?~?@aOJpGPmy132m#8H@ z$f_m!3OKR23{by^M<7*%SpF(tJ!`}T^LMSxT42TP(O5-OV%=rr*h=*waXjDcyy`X0 zc`!Gsu*7VsZXxJImzd+PTvX)*fD~T(Dw@??YlWw@+rRcJBnR*ACEls<40^Zki|%&w z+5Jd^uxqts%nnr<;9fO9$8`?blh@9|GAq@$8` zs|^`e9wS*rU~-8aOF;lEdooZ*b!LT@aQfmvH!L17CnEHBB76{gjVNZWO9SYn6izzU ztyewy(w_5gs6#M~4a&t-@zs&T+qj9fO?eD@X-xsTYexE4wI(npv#CC{mg#;e6OXpb zr@Q%0RwDpC>eJ`B1shaH2>)Kc-2WEMFIJ6$+st6m~WmlAD2*GM*!JVuRC{ zz1w@16{P<=k2NMT)i8&(2Wu=;Wr~8zszuS2*Y6<=9zryZ>R#>f>);GsBiv;m0B4y< zF$u8|Q}mfSI{yQtBplb$JqOZ-h#E}VUxK(10}{ZuJO2(O!Rj}cPL%kR^(wRG1AyY* zK$oME?H5WOsl~M^%Hi|!6SOFyX3M3C_cUU#6xmc>EIWGI6^#Pva3tj=65I{Op&+ya zC+Vq?N5Jp&UsGbre>Q@wcn{4t*a&-3E^=J~psLM4aJO5cDUn$H4?2mj|R2E6Q7{_o3QC z(gs~HQ{tr32$y%Mlqg(}?XZ_2GfX#s75tH3aRg~chc6-Euh&n54gZl}iPz_sDdK#I zu~IV;GDv(GkLg6`Z#IUAAjVE!jawo4s!zPSJa2{RwJ_>u!bZyI%i;al@>x$N42$~L z8$Wt3$CK{_a9i5w|7Bu+aJat^*Ht3EZNT1;k!1<&%3LOkI`|Vxc#+ER%tcl~hifYQ zL&u*$BAl_Iy#&)-bBL!Sr`xzV2PwxhoneouxYzSk8RUzRzOCfeOLK_Enf^VThqD;= znP5Xk|3TaVX>P=intH?*TwZgDg05Ufo;XrNLv>Y1gi2h=6wMf)_!2a41w0*99sqtY zpF|#4UCAh_aoNHE-b_WVDFqwh941il(|MCTbf<{N2nj0|K((7R2( zvkI~I!s3Vhrg4`yMxZ#^wKFODsZ~2%E-4{j&J~W$eKm%K6BKsjkiRZAHoTW)%Vd{< z&GA;9rCqZl(^ES%Y1U(g?6G3#SL&8&{sa( zB&{A|zq0nWQrobum((eJBwsimn#zzPzVldbI+o9F*STCBbl43lvD%lrl(3p|+LV)2 zf2TZG7!`1CwsB-NeIg;sZ^jSSEbwe47H2fD$&CLfn^2(gO&=0})6p~j<~2Va8lWyg z)vmc;aYlw+9|asUQEG|pUcYp?W?{d|-n4!?94eu%V)JXCR%5RZEW^ZhZX|=lm`?ZS zZ|%5Zr*(Z6RMYMB8#4fzzCGY}?Tf5lbaCpI*L0;fy$<@_X1o+eDQWlN)!=D)^0$%r zAn3U)S07Ekp>HjkeWl zKFSq}o`4NG4j!$ssJ``{yq1cmucQN=i`uSXsFAVhvXAViYvu~NI6YJa+u*g!_~KQq z=J#9cFnRb+fQvu&Ih(eWytmNs?Zsp87zcwMNau_1us8Ilt-3Ex!RDs}OYdEh?G-mY z;?y>hiezcHERJ>0m4QO;tJFADZpGJczZ#xc7Ooaj%lM~G=ygmL30%yx@Z#N~WSvsZ z7^Q63< zb-uD@Z*4U@YW);^JPD0tQK!^e*F=T8f1OKS8c{O5-|d{geoS+AX~=MU-9dhAc~XNF z6tAg3`Ib}a@}yPE;aaTuESj=zC$Or<=o`Rslee{c^|9Xy2;}7Y5t5XL%bV1>!R~z( z0OAd+2V-lI2%a5vj4i>DR^TT@S$|5A*<+NcUJ+!ryw_C^39I7kLZ)al**^W7HuoCG z>o}@1FXtN#e&1zY^DNN^tJQpYjYI);BABTDFi!Pkn-eRj-wi`D$R#B<56tBa@TUaX z?XNjL=>^qGA`2l+{uZ=}iFJfwY2yQ5@D}E(s*JE*0lD`>jJa)dksWrlvpnCV6UJj& zgraTQm?Q@F=Szq@`rHlYU(Y%|nQUgfcF&QD$B&!z`{!&_kGVt)opEh=nm-wLrQJTk zegMx8Rm^+t?Lp04RXnJ4=e|iJ3k8Nl-7Rwq*%+K*L0Va2+smQ?dxx$ z;{Gq3u=vALNZfY2r8|2JT6rVQrUwy6`x>_n1m*zYFm4l|heO~vdR-D>6t8R~SD?th8E2G8Dyo-ps86lD=CKrC=6XTbL-omQE8cl+VkBXNJpcfoJ~nJE}Eg? zvKv|QZdBl~^Uwlr$$L@;27$p`EV`U|b%r1*`;jxOf{l!D$Pw5tFOv#Dbbwc1x; zA?(XESq`G9KaR|AfT(4B(15Ym$n=P}YM#&KC6pk9adm1TgifI^ZW#Qt zG&}Z4p0lYeg;)T)=jjCjFNwZ8dr^d1Z8pPw6rSpI{FZPJY@oLMQud}0-=R(@IqB>QSS1*TS7oR0CiHz_>Y9C2& z2>7Mm_^oY?Z!(x8o)!y+6?MC-0s27!>RqYAe!4fA!*6JVp`?$|(0pdr-|SH=emK9} z$hZJO*V3lPY3C`_+WC&{2!xFYW>f^JNjmK@>x88Lfab}CyLiy@wzz0?*9C4m;{wd# zCJFev(0S8B{T+E+!NT_c`^37*sxOCZc4{fVBT^O}n|HcbV9G?e@Y$R9iyK`78ix8e zFBGtU^IU}K*Nz;rab8nTZC|c`WZQu%E>3bHS#)IRl>{8O%6hF8H`}t0GDt+9uvik+&r<2#hPlMsmu!GZY^Fqi65dAQg}_NFOzIO zMDX6DdSaEHs=v=MQxQc<;y?>}uQq0sGDtsQ$mRqqGE>~;?2*E-PBA{1LwaE*ZGmA| zs=n$u6JIn%#fT6N**PO1BxQuV$e1q85!J=86OTHzBkffm*K(CciaIxc!Vb(;A%S#l z@{IIsBlY!4wA#uLY=e@a66IE_H_!!og-rw2^FW|Nkgt>08K4+%hwd5uFXPE@^CO<& z8}UL6Jf!TEB%tJQ+&ZpmdY0oA{#cr?O87IxOkfeK1$XGA1$7UYb5WVP?@k)1dp0}JsttLSnom{@EI|zTryM@-7c0T5uTW~DsqeNnHEq2VIe;N zesM+UUH9@+!hB73&?*(LnzF^O+P+TwvMNW$Dfz@J&&8GnO`kbd8C_>^MnC$~z(H+C z^~GzdiJj!lS4ME1-F4w(X+a2;lo``N9G&J7f7^AA%=o8?TA=U>r*s*pP%_;XS)Uv= z;wYKCs!_mKqvX;p2V#{;Ir%J{`IHcetZsn{>7FjK-j z3(nsC-K^=gXr;?e52R|B!H(+Jz%Px|WReSpm5u7!5tG`M^$RaQ+ctd*{Y@~`Q(cs~ z*QFYAq&txN&|sD|6dVPKen-CvUi~e>$s&)8ZGdVl-MGVe*y}!9xxhqv3ko(3z{%3U zp#r>%>L{P3DPcdEn~PmYp9&tIsU?_ox?SinlQ& z3daZuY2qU}usF(J@~E#n3;=`!&a z`#Pd%LeW?|a;hNbt8v?Qt48qtTgikoIz51XuBc->?}twp>tsO+gWp5pvy+heCU2^C z=bBkxnQM4n=Z4tcGSS_rwAbC+YZ-TB-lZvOps@m7HU+3pIxRrjUkfL(sb#a3ckC34 z+7F(2(`Asa-kv3vodxU(jKI~BqXgS*bLqBsp8UUa%BI!IOztco_|oO7t$%sh1Rd z3_#!d(knne5RMDDr-lia>4E^Mk?{9f`0wKAzyi66=Cr;&7K&``8daF8Y%P)~h^M_J z-3IlNP058l*dlmk^MrC}n=Djc`Gs8k8aK=h)WPooUDuMlXBiw-T}uit_IpDZiT;=H zWgK_5M!bsP%oo_k4IP*(sI1%YwA!aSN9vEO>gv(c52uVmM`k=d4VS>msj2Jm>=Ds2 zO<|Vp$>jda3^dn5SI_I}dv4L-*oSp)QAWcRv*9>9h~L~PSXN)6hKTVdsy^T5fNK-X zJcJ*W_GFaoc^R-)Os;_WhRNVnY4p71pAnlnYsz39G(Db`NCmv5g!}%8U#m_6IsAnQ3z;NbcX#%)~Lm)$yMP%YW$>M_|{|KS~Sch z1rTq^a8|AKwkS7MlPo*@THdgCyQg1omSF}C1;+;l_q-kJnYwomjy>Imt?XmZLu(Yv z{M({EEo>1&*=$R_MKh&U41kqY#CSliq*}{eaEEdhJQ5uOMw}vixk)fy=ekWT;X=#s z63}F!Jl0xo8hd8usK8jaKklZ>EFBA1G*nPS;-6HA$M_)IifEQYc76iisec?-5?v@( zJUi2;kMSBF+Cv&nwMq4J=dv$A?Qc1qhSJ$9`gAqk^B!qJL|s*2mcrJ`KMd#{{{N|o1lFI@sXkopx zLKN_Y&eO9HFGYO9+wSSAfD4#e2I(y+pK$$}1^l>ehb~$Z#mA(zO-+iPK7O*U@7m{Q z=wUZ^6Z{f&%LBjfr~7eBr#27A2Z%KS{>2u--rZ|xmWs#qSbneUFgkL+=H^z8&_tx8 zFtV3$8u8H}C(%k&>th(9wM^KH=T79Q*$sdB`&~2it=Hf6r}ew1nnT$durgZP9A7V% z^+X>QD3mX`w^P7{0*Bf%Y0$?vkk~UnX!pm`zWKmmV`l|2I^N-u5VphZFkufPC7+W$ z!!`Y;sRyLj7YI96loi#!=yvIG8FiFA9&fdR)NrYtiMe}WNSf9W6qP>|d@y0!Ag)Ki zcdQA=L4jma#ffP}LiV!XZtvq)56$l^O<~%zE^|bsCR~4zOIF}GYRVPaL^uP0R(`a> z%{$4Qbft(X(@=B6RfC06ghpppKkjq{MTV#8BV(t%9Qx@coivFF#IO@3Lj{TRPN6z*_Nw{|OIJn?%;Z zf`YgdJD}Uw{Sw&o?jQW@C$y)ru~Zh>?j^lEaFBxpR1mWI!K6#^}=L!qZ9*7Mv0E zmMSQfWA(Petm=hE7S9oD7Z3e5!+@?;?wT|XSjweZDSE8F<2`&QbgPuisg`Z{DrYiA zS7C>C*rRrWG>f)yQCUPm6YdN+tu~`ySBkYCoz;L6e3(Lqh7A3e3ow?R%k9KQrfRO} z_?{Khw?|(gUyc)TAlU()$i09kS^POp78DQ_`*ltFF;IG$6{jP=ybg}dLB&Sb%`h$e zu-CLQ?ibjoRfR{?{%{?{?y8W#Nq?EDCao?MH^lTCvsVg4NnQ*mz9iaeW>?}Z|9TH*?)Q(1s zH@mPDZR_Gs3KGEg{&Cu@QvNK_Qe{ZtOx~+Oda@oQjz7WGi}=j7Tiw1O_}CVFl0Hskp*}H-kGr;W>nlcdPaG}&qK{;|(b0t@L;f)_zzH-B77^`j`| ze)dc>Z<~`ck>nO4p3Ifm)bI-|0cerPGGBXW^&mcV5yXVu3<9`E#*kywcmqC=?NYg`@0v$jxF7}jiTg5E9Wa_bITj%>ijuV)AsZ!^E|TadgYuGdci{H z`Z04gr0bze=URbt>(Adj$CuxLe4L<{A!FDyLXlYlJwA#JT~5eX$x|iqWua}o^4<9X z^ct3dGsW;PnnCb(1K;#(1}$xAO}I^U)K8YoX?ujl&qV6qw4R2Rws-pvkbWiSXNpFEy3z;isGJy3@Ls?>=t+&v?y+(N|&#jqv2kjzsIG67aD{F9dvYHfx6T)Tn}3 zNZ~@9k%kB3Tx+$o^e=FxU;goA?X`3p52b&6o)n^z;L$Fx$q8{rnE<__sT@2I$C`xq zN4Nn^LshjE4G$=Vgvu$h5fv&4yb9!BZhx0@64L$2e9@_b@UQQ~GyICW=MjN^Hu}gr z*SuzuRLLEMcSZ7nAkD}e*s`wU<;HO4`80QkwS?@1T$b~=MTp_a!0ca1SR&~07s?R9 z>EH|r*w53FMBK{@?sL=u9T;hYiy+%t5;sow7jX$3#j4YUySRdh#T7i}B_8C6%+(SE zN79ErlDtewC7Pl~L>oHcp`Baw`>+uX9yT|HsZVEaVAC*y;S3U?^O$ z99Q^q5b&1gtbzrw8wKvWyYU&f7w5mZihy%WF~~^M8-xvyyvb4#c|~j|3t(oSmR}@# ze}!XC;^x;=fX}OJKj2;ymq_cHi^7WMJym58zthPeo&jDMW#%oC+%^fTGu1BE91gTV zz&;~B2Ppbtuj@nmR zGTBYdmI<`Dfkxl0k*b418OSlE)#}}GdxqX#9Zsb9*{krashx}wM`kj#i%F5*ZJP?9 z(_uRdZ9pkx0?=sDgfkx6C!unyYcAO-%E+=gjw+M#%!Z$F3_{-1AZag9XR)#Y-SREk&s@7*3nH5b~sNaPGR8ExpK{0a!&GAGKBT96QN3W18KBW*L zu&BgxB}9lbu!#aOYzbr`(e@BK=@?i%3>!te3ec@kKWjyh3tk~HI8_!7QI6Pd3o0IE z)H=g|Kn*OmVO`=~)>m?LT`ETUz)JZ1VYTP%j?dnRp z2fy-YI|(Lc#}zayLjpnk`ey9xUyGt{69XAb5j?R-h_YH4EFQmI{XRcM;dAEzwH@vM*G{+#e^QdI*#C9*Q~EW`L#T9Y-k~eWnll5yqQ|(4_|8qhPT*SfW?nUgmvoy z|5+yp`JR5x64d-u`8v9;@!cvrB>X%aG}xcz`5`oD*?2nO2g8Ymrq74TTs&_f8ywF-h?S{;jJy^q)}4 zzeiJ;8JYg4Aj9?#yZQe|*f&RK_AHCWb~5246Wg|J+qP{d6Wg|JCllMYZS!S*_nvp| zd+V-s{`*$%>aTZIS5;TnF8sfNTg-oBx&G6wWCBylhNAW2QitvcbT=%IF%1v~SnM3- zxYEiO2bHU_T?2*YJ7eX$=A2l8pc%L2Q6aC+wdEXvw@+w-WfbkZ_p=jiK(37UqX>s2 zT-4n4noLOd2krZjZd5$=3}9zne5a0{e1273h456z3CNUf@=bCFFcIKt-Vh-NH)9o0#o@!6d2R>+ z+|F{<^#sj(6n#}A+aWMoRwMty4Q70A#K z$GG-(%(CbSpoi-v?m7h=8XD7gy@I)$X)tji3q!HszwbdF)pO+n3fK&0W%p!6gshAU zzFxqP+=;{A&Oxi~lI;&o>UySVLxg9&HXuoTP6`Ch9=)IN=XD!7=4Hywj+Wmf;h4P_ z*W&e*-&usBRF4x%h}x&Nv3h&o-Xjp?lZ8*(C?&_Fa6(7WK%!cYKxn*L{th7MdAYjs zS!M3Ji_6|J`6LWOrvo(X8zhvgDA4jA2_`-rslnS-3Wy1NLQn7PFq`U+fuVT}nn@K^Zj2n}-guIN z*{iLV6T3W8a%28&XIC}fMG;6DNVVc=u)lUfLM&->PG`pusESYm#;J-pT$bE~s>0(q z;J!9B#_ol;jZS_qt}d3m0nV+R!^OU@>?1EFiJhP0+3Y8G3wEYH)}s|JE@2NMf`))4 zt`6$zJh?E`7J04BpKiEyd3*((p;44l>cLAjK2i#JM9fsv!XcQtgi^83l8Xk4TV#MA zGJM*oIH&bFGyaQukDGi>q|3U=&$?H(>^QSuIe0UQC5#ji~wcy)4wp7g=&1Ba;FTPgAeol zU4_=PZMh+~MX`Qo)~Y1&Y2UQ@$zaPr?-Hv;N0bUE=-za(u!FK04Kcz+YwKR_P0Jam zG}E&E{LOix?PwV1(u$L;)x@_@FaF^IYzF{d7EkIz;-o(BKse$+H>jskvL3%8{YJu= z6wc4GY&tk1hIfm_9&KThYN;OFAiVvsd-=-(oVrN*RewJRf}WKy8FZufoFn1Pw~@6hJIyUr53-HczO>}3$^ZMb?;3S9OM_^TyXn+!aOW>XKpwLvGG9bBP6csU zR8mcc%8h%>?Zr(ZEX%AZqfXySwfv-4@YZiI4+&eHpsC<)I}oj7HZW&!b}&W~M+~M@ zE0N!9Mi^wMlu&>gswK)PF5LJNZW!Pis$yS&l_lQjd2L&i)H$!mie}!81V}?A4q~~| z&a`iY_hgj{_Cy0zqRsEh*dj2{Xr!`$fJ*IVgW8u?bSZn+dMm=^*Cd48VEE-r5?CGi zaAq6sN3*ji=6^Fy(PeP`k3}#4G1;?#8o~IY#c7#3Dykps9~iZ0f5Om12HV&jU9h7l z{5m8N&J{|dDb{ZW=hs|W+|yZ?HmChaB5O?lSStf+#9O6?P8U}!3X2Ho{aK~VH=`Ur zQ=8q7Uo5&`gA*}J{ABrfM5BGS?7pBJmXg^}$=szcw%VE#*3s~9oxyY7wJ^jv9%%0} zDpvC>xS6E*SP{{89~ywvS5-_k8VWKAcW!dn&=7l3I%qNSRdb1{ecF~Dx!L*ATytV1 zy=B4VPLWoV($4I$Yg%@?3RzWtGz$OJpdpeFN>L9}6qW>nQ@%#!3m&1RKbs*M;r~nl zOXrp#_x$zD+VAkW_0n-iGCJSk49TZlz=f;vl~_1cPL9a zfRC|i&|343d+rOd>QudXnhCej!mgRp;+%2ArID)Bs-ZE-DyaJ5aPZ@1_2LG|!ppR& znt1az*w|9q1xW%ZTIUDa56kRx9aoNuG2t$#x79a&b%wzW(83NoF3nW@OEPx&O&kg< zJ$2G0lUSxg$pv#em7;Qj;7je|Omv&|jPrtbRqK3ZIHdEv=DCrjCoP$f1&iJGtME1~ zK04-cZ#UZ7?)%8A@v$HEmgV+> z*T>J*RjFn&A5|8Iia4l`+2gxEx4-tO+clKnTFa)>ld7Kq@yaYDphew7^8KOktu4cde+QfP`7Q>!{S}e{`c^XtXtc#ZNQ*Rg z^C@N32QdRPXOCZjL}`qT!QOFJW~I|7Q=y&c%Kfa2iwn8!N7svy>5%?j_gE*<$}A zO~H>w_j*ME(AJoZ(sJt$;GpkwRC)^y_FMahEp`2nKztvPm4za%{7py^!OCN*vCb(> zxYxYb8dT@UNbgKfP{S38do2l=X!af<-`4!Etfuaav^_P@COZcp!R>=D(8iPS!~exN z`^V<`YlhJ?Ff;#s{`WT&^S^3H{o7^C{5OE+KN)8WS|d?E+wC`&ROVj+%}GXnit?KB zo+hDgYVrD_1-m>p>WPBAGkymqyQ<@|iO)U#~S@DYf=tgE_42AS}DH7@v|=yS`uzBm(FqTHvhn#m8;a5X7^-O@`l^3kg13HYv81Ll`#rX#@?_^W z_M`IA>-TpNZuyU{ii={^a_CH&?l~I`>oiO~kLMbtY2~ldcgX5rkMCG=$}vn|DKFSB z(FAeorXbnJ$<5@}7UVH}Xp5qf+2Ho%r>s_I1W2lx`gwR;PHsZ0qLn`x(FR z|H%%i{WR;YF4Isqcqw+#=Kh>uTL?16zhYK{wI1{6gZ<2RJT)~p}Javv4rw^1!3&o z^UP(0GEPjA1k|{fHVAkl3ExBy+5)aIZzc!DeyY0Kf;W*A+^ba#^yu%3uD%Gc(}WRF zh^tvb0uqt3imW4YV@Mq-QJ}+F2ZaJ%o9=D0=XoFBgx0gi_&_i3tM$ei)iZOF2vG@v zQyde8L49y0M60&E7mWB?S*T;NV_$^xr;S5xf<6DW5P_Ol?!0)gWgTRZQX+%DWz%PU zuv0fgi{GWmZpq=}GqkLaXhhPn_R<*Rir+l2^+6-4ezdOBdmUO zl*GPC;&h&xxr!^!i`(}FS967Qzg|9jIAV+hmpPY#h5^F5RAIjv!?V|*TkY3{p!XG} z;Q{tiMKu@Vo@J{^{M8gpI{fjw&se(4*xOn2SJC2TA}AZ?G2zjhDP4`JP2_{|IBnD1 zDGzxrdV6b%Rp7N6Oi4B^aR@h#2#&#=#L>o)kxSPz&Dj<&!Pi zR=u7=M6S$i$T2>a#Fb$u%S^s2Kv{~+Y%#xv)srzm_@CQ`ShYb{OB$Gr77i(6S*43- zZO>_mKVK%`Ig~jYbA;iXBivNkcY+#T>bL7tU!AR9Y;{~IBw|^$9+v`ncxJ?P1js8i zXLF`kt@l+aflne@&RtnFz|+ZWiijjri3=C(GK-Dp8bExf{hxbJ-euF zBsTLJ-412bXvnT0lq>eT>}h@?C~jDoAZ1P>XaGt=l zee{zwRh?8nGj5Al*9f7y5Y)Y8V>M#Db8l1{^6i2t{WutBH&fr*tC95HT@|2yVSK0{6iB#K|Y(sY(8Q}_zSx?LT&7sq(R^3#34p`!ruTcGz3RGT=O#q zy?FQv82!&b!clRWFAvzsF?dCmuPYCqgHrO%A=~+wsFB|v<3EWyL!XqOa7hxF%|x9r zcG~^bUMOBge}Kjo1j=tG8B(yw4hWnq9W!bxKgfi_wnM8B!e48u+1}>6Lo% zCq$CJ?M$2Dv1NzxMql{z^4XydK1Xh04H;1av-L1nDf&7xxvorTNUjkqqILSyx5x*% z^Y?`Q=5|RBuV<=#gznIJ?$VfLEC+0#j6dr z=hHCmcq7R|5X`i;Sd;c=05kMs+`Go4=hEK-pjitJ={UF}y;2* zecBa)^t`A&W^yu^_PoBwSES$4q=QcoW{Yw5Q1JONd{dO67do)u|-f5FaPz<`FNvqY*f>j^*UG+12 z+|=s`q`>sKQwlUpM`~ZCa9jqa_q_K5e7Li_I{vo*rLmywRF5eXoPSP(TiJij{jwb| z!+ymdHfKG9pXyQfRt-*V`M1i8)laK)q!ueyR!qbZpTb^gD$g6N*ci3_O2N`aZLDk= zP3tkZ9L?hzLyNcbTg{QRwJ5&5Ywi`K#lnnz2D>&5RIvaeCt~=%M*ZEw&nU77ddNO& zw%;`ve}W?N?~LG0*xQ!%ECE!6Fc;TOF6kp?_dD8DiQ~(W>OJs*&)}2iT;orBB-c*YU3HF&*NS`15P*4TGLi>1lP0JvzTe$Jm~71sA}K+G(|| z*<3%Gc|PzxY)d#v_G!(W_$=ge0i55w9~k)k*Dc9HSyZ3~2JMH!rUdKd%?JXoAVh*O z6j0w8M$IFSDj#SI`>AZWuV$gjLoIKi&Ghs|uf$jDyh_)#0_0JM(`l9t(9!8;{A|!B zRh3$CxwS&8FdLOPy^5eM^ZLcVB~DRJ2s@BRcCjs3ypE5|PGS)hkf4ZkY$UVo7mL97 zD88;d>US)`4tun?Fmt7k$GoY?GW)VOaV4>`HGpxV&@-@6rA0{r0x6O8 z$oHiFhEaUx;1mw2p6{XWOOF#`VuYpoR~-kkkXmca@46;gP(Bg8-rdo`SGvpM6k@X4wnzsk)?`P9iyo$^FfFJ-f5Rv7 z>;4x3G~)%;Ds>aS2|%;}s)U+=)ZAgFMekUu@OQRLC3G3(H~#rf-`~Y;o2HKP3!KII z@Vi@ip<;_8dZdnBI^9J<6;TY}6Dp#iR~=U4_NQ^GJ%=2OYYHj{mNjLD27Zy0V*W5Y@yDL6F5D;MH16jY6S?N>^+}3 z^55~NW-ku2?@aqiI+B{Y14PaKm*vFmbMTiahW*?#QjtN#$bIEez%;=}?Ub3QiL@LG zal@-nT$_3v2Dgnh3BseGN^l2%k(NrMVJH3Ftqr~7Hld+^n9XrU?|Z8&Ibbri9D_PD ztP7ti$^=ZHIng2nP@H-)A;{5%5S>L-EL@k-wuQL3luOih92qD}1p52pnlL9fm6&+R zBH|!?athcRgK8354y4ZFa?c{fjQ=6n zBf>ufGd!-KJMomwu0rWox8DqB0Ga|FD7RQY?j}f4`iEeX0Wu-~5DW*~l{IoUjrSh|(;Af*88I7`mJox~v$wj2L)^ z4+y-eFc+i&8KeLiBmo&DFxh;BIX-rYzp-}cZJXDxEEgM{tcRXp|087~hQwzM0E`KQ z`v`G(GqQwBY@8@gAaKfjH2!SlMcB4Ml2+LV{_mVVh+os!`r^=g6MIS8@N96AkFmO2 zLat<_5YJ4vOz;l_bfT-_J9M;Va!ftdy?OP=%Lppx1J~#h^SmhA;LD}$zVes?cAe-r ze{3+@0pBs>QigP!=-d~+=ja#i!1YvXzpE|lLpyTaU>2J+s7LwoKXMI**(Lqy9V zI#&X=yGJvStULh*X#+&-fjo! z8njFD-C@%RV2xJcf!>udZ(lGNO|TR06mSR(o5j@6@trFL+g{w<|C;{H$X>0Uuz(Pw zof3<-WgI2-eTvdagrDEPT0)p>yei^1*>{r{NU>UKzdKxlSaxTDFwwwsdfc9E`Y;>) zyZVOjha<_Pf4N47IzJtq#sgb~8sP)CsB{|?Pgw*tz zbsuOk8G@@L&_PQ1>bbWaFa}ih1~tJGT*37*gM7BX0#tl3gJ-ht0fWJD>Td!}@L&KF zc}0#AblB2cNOWG_wI72glezAe=HKj(7a!@Tufhyz(ls};X`At-tsCx}wW2=Lg0-bs z+ud|8ku^8L=$?enLBXX?vyaY)&fxAmP!0!!Ix%xjuInvbPE)iuczxTUm`kgE@7HtI zj(ln#UP>ix73DL5dSvNg$IsnWG>=5D304u)$v7m$H(aW9xr#<$GNYw(^Arfl)Ky~k zHJ1uX7iJob;etEImOIjT>BH^(GE5!ZU+3I1p$}kAEriUdf0z7T>0lhU-FS{S1Bh+R zURU2F&a6(2G+rJMEczDMK;0;-v^PUKId~f3owKj2ufjdy%(o&GcF*GZm6IKo?ubqW z*FP=8AJ)%e`H8a3Lcb9n{{kVtxz_+PYcfB-WlaaXgf1a7c)}aUG%zRk_uwQ@2l)A* zE8}iq&o34Sy_mdY`_$?6#`8-(I2T4255`?5c0BC>aPW>{y*&DF@*0WF%0&PKL zW=BROS{+@2B1D@+XP2*WDn3rk8>r-(-27PIm(yh0ptqDim253KA>Yv6NdE<$hF=5; zbp7GQ;J^SIE!@FOXnAZ(sM}>ND9IA*+EHqB>9{i!nrxF2sX1QS<7gzJ+PBfV6Fg^| zIYgTqFr#T@M)^Do%@D|8LKp#6MtN;IFM-?V8zl^MP)^W4Enq*fS!WhPpiRyP=E#~B z{sU$-nK~42o$T1UEQN#WFOI-^{Kbg9w??3Pkj+8C9yqoHAko57o6yMYn2?L9-a#Pc zUERO*tmRvxKz~IM6G_*^QIjMcxxidA`?F!vIZ;Z&>DuX5r`%KWz^A?&T$rH#iX_G! z>&Yl1Dxw&#bY_fNx!S;qOesSl#$gc>vxAZlb%k5e8z$JBfpFtFjI(W*X<1`9;4BS0 zSz@$&bYxuJPZMir@@|<~Ti*w4r^AVScu>@d&i>5ol-+!Oaedr`>2Lz7mK^95CJ$Nq zT0eV4+ypw=8aU^s0j*}Lw1yI`KCgUtBY9OFG4Eb|+5SLH+V1RvS?shyFZ(gmnF&Gz z0oHu^0{FT~FNI9+3TwJS1=c0xg2F&B-+QuEu+gXD9ZfOzdZ_spl?;6b^h>x*-v+dB zFFOm-Q#jQBi8TLM!}COkJ3TyWGEisCl6$FjtoT;*8FHs|5qgyLy^X$4!QG|&D(K0m zSIg{E^Bv=)_3{Y&`ZrzH{AU4lm8Bfo>(~BV2QYGB#yEI^hWsBlwWWo0L2arASwhhV`&&Po0 z@Bhtd`mba{CI$w^e>+Y8RhaRAI8FbC-u@@2X{CmQ4Ph&6*HU%K9gy=qHKZw{Zu12| ztCc=(h>*0{chN){-_)_s4_PEKx#&~tb`P-Ygs@$a6XXGrUILth*P~mnALYIzSm*ib zow3p0A2(YE**ukB8iEOxJl+~j0CiUJv{xEG(a|ANo@@JbC4ixUTQNQ}9B&&~>q4)4 zceod~l68F4>x>7pEsPaGzpN)nr^ki-yn`x3P5l+?V}+fF7GNxlTpcHd(Fl}<>L5}K z@=_&3Yp>%={QbxjdvhqOxkb)cBMCK#!+OZ&V&VFKG0F`t4hs7jQ!6zwxoZ$f;9o0` z@G~e8Q>a<)ho_RSVRlv$L6a^7Q4&EZ(NEt#JM@dj&%PDB_4=vjA+*nhBMAzUFOzO_ zCggxdEyV>At1l4p$rkuUa!v|Brfs`s;G)6c67bn=hvu8uG+Jc@BEb8`8zT+_ zChdqMQ7GQ%56QM)KTC+9YB@Ryzmqy|LCTM>x`kE2}2(twE`rX*RE&g|C4nyzi^ERxnImN9H{R#?VIPQ!mW5xl#Rt^K{ABFCA?H(+K39?)NGu^evyfd_NK*X{ zr%3Q35eg>%?($IRjTnYz$W*OzpHcHj?|4j+=8PDoLdW!j{vKuLh>QvWn~CiyT($$C zWF=syIEQr#C>Wyjr9vlom#rR5Gnht|X&ppbrIn=d`+rDMG0Vx_yW;Lz82XQ$A;X3a z+!rvIpYL4F&af@}G`azl1TeL_R?jp^x7F0R&x(_(<}lp@9fGRKcl?~7?{Ea;N~`L+ zROuJ_ZJX_fOxm8B4HP9#| zbh{F?FX7c|w^@Cf&(&|u%}X22>czztAzxq?uTq{r07(V;vV$@NpnJ3rLOoNx-7%JNlNV2ej`uda|FHxULv_TypeDkOC+}Fzvc$V7n;=ssF zkMoo0&YmbyXWg|Q7QN#uY$fa1q7uF?%0WDh4;Ncxg3m@@9)j>za4l9H-b$>mr14F9D#$2D9ta4Z8k&~>AYNoE%z9PqUvIgsfhSDHh1Uc-a6OA;9F@1ht zd?L0@=lCg%{+33yk@BFVBfL$f?U{ebv5Sol_KK5seCBB_AEywrHa+ND$7F(r&qZt% z9O$G5gcnN;^i_6&?+hb)ixl5}2Xd!J5q+;Sfka}yiZ2X6{P0X{7+EL70`$H1Nju!` z)6yS3iK}ss3E`8Ss(a~om2x`7&fcO%TqNIBj8+CA>C2ftgIy)`2!z`QO!vlCy4YT; zpxF2uJl|j(Qe_=@_)NgUA~3-g14om?HsprVjEsyNM&GiiOI%HMFfBN6AK}{ri&ma^ zfh>I%7Sc>BOiia6<`yi`-yIxePf_#k4g%ve+jn~n)AycS8V^IMFtEh9C7x`?V`#4V zg|f;WB>*;r7sXF#2E*cPa}5WMJUaO#5am|f;?m_Eyq6x zH)879!=y3*i3WM=7ImW6>GXQqks63WGeLL46}s#*zxC3INyFQ%7 z9>O%LjW#J9j$pL=$Cva0!AoFVwY4IC$`%<#2V1ZVnUjz7S53o{7vb`EhQ$@r&ZMh^ zSza9)c5N)MLD5>KBCWsL^{18QlJVO%-5znZna#lLDY~Kz9lbBdplIh%DcBO$S%r@8Rg9Jjx=u_qVgi zt_LBc{d%awvZV=t%lHLFO3^eD?8m zd`tbz@n_$;{{pVjp8Lm2ba1pc(zE)vz4))ZEP5t}e;2f4{#Tj1e_N3(e?ukzlNGt4 z!4^%?ia7Z|!LF@qsC;9}j0=%cOkiuQ3jiL>aJ5RiNnMlveAVuX7M-O2vEPr*JNL7$ zDtdp(Fj4rY-s{xPlbED`)_qsSu^3v;U^pE8d^2F1W?WR9I=9uar;6Vn151 z4|^b3e$5~*pN}`&H}-cTC_WS*-~5xp{^Doi0@;>OHPY8sUql9N-f)bnz8pkR9<=#u z$f~a>7CXf3YglSS*i0qb$cgd|nVHGh`JY+`^Ff9l>+X?1ivF!iLXLTvqMz2k=gZWY5t?^x0~$*m3xb zx0aS}o%plC-G=c@Y14eX#ATVTAG`ySOT6M-__+CWAe$XW;#BX;EnNv=BV7VsnUKi< z9CiDAu;s+iyz^fAO+s#4S&X`ZFERAM-Vt=#~BH6bgc0 z{~2pIUD{iTwHarZ;laEDw1g%FBJ+DC?rYGM=;kW;Gl4*tDige@i%k$Af$qy{{yUSo zg6?%noj#JYfSa0WY14ue^{X_u?rYw6m%!qL@mluPn0j##!Ah}C;UoYF)MCW4@O~mu z6Zv-oBfAiPWL;7yhUM?TMn}vP{2TT0EuIrQHi8|s!Xs!QU54(G4mSHxJYs5?~fujs$U&M^@#{#^5afPqO?_`Hk zG+1a`?MfCwNxH!Md$UwRnG~q!l*@hUiaoN*G)K%SqAW`VxL0hQc?m5*2JkJO8a->U zEb9k<)q@|UnzNwbe?G{{Il7O~zD`zWTpA_lDEz2(0Jc*m|CAKRXig(#_tsFn`oT?- z(N!1AN+L&<&lMlEbeu6Ta1`huK_7oua6}j%G^9DE)|!UwQilAAV4UPwHWosQ;)ujv zSU;|r9X}e+?z3#$w=1x#HiRw3_bl}F2h94N^%&lFH2d>-Sv?aZ)v6qVARn7}^Zuty zh5TIWllYM`woDNtYVS<4w15UpHD5P2t21QqngT8*UqJ@bUTT66hCgIBjxTxv9H909 zRbb0D*J_Akjn~0&YOX9SLdnKK9v~kdu%Evxh|dj3rk|L}t)GZ#+Z@k^$0$;c27Kv! zDBp<5GnS6-=8+#}N6dju{@zPsQUqmrl^QqTCM}6X3iub^ok#{Dnskcn1-J=?x0!p6 zzw-F?j4B8j(MQx6nWSyaH0$0G=IHq;vnelsP4A_=%t&IDk&E|_TLAB1YU_Ie7lh%4 zf-1MtIfX}KCodA*utOB+BO3I;-0DZxh*BRnHjb`|0&2@(lK7Q$KhDiKnK@QrXp)^@XqiqbZOU~-18V^Lc72LIZZaxz`gAx7 ztvy?DP@hd8j55-9r}RjjK65F%4?bPIfMv|UjGQzPZ%rs)+DRJHcr4iUE?6r zM0tpaRr;mZU%+Irx#^ESBly zG!XZOLc^_XE=nAe(ebubF%Oesf71yoF@(i7DuZ{Njd zDf@m5+5%BtH=)|04URg(Z@otAcIsqUJFWYp(=b0yDps(Q4r%cl{&emqpfxc%YTkUM z<{!i45B-+!`Y*>_~|g6*-1vwr#>>F*iw*zy?3X3vF z3?GO#^)JFoTH9J_`lPfrIvExl zdo)<<+6P#a-+O4jy+|o2QLQXON=pAM`_T(|jc0WXi2cY&p}}vxBF4Qj$E6abtn|0SJdSpf8*6Zc?;0&FT>-xhu+4!gzB;_g{7DlGKes4!8(_?@q(h$cw(dTJlL1 zz=YbYqkPR|%*WM?r%kicNnxjdng!GC3CG!u z97m}PHAMfImv-ZD`Ndkdd@y1jkLGcRpA#^>sl4T%x^#K<-L)ro({(BaK0T^gLSe<+ zs(%UohNd=(q_|-2hX_SGd!cF=b<~L}wQiHknv#8+{LIc3<8EXLCKjKhwzJzCe*L2SxuEFXKOE-#?+5 zjBNkTn_>A|-T3e5|F94LMr-~j`|v>Xzr2i3f4z*C3ED(Vy6807weyZh?`g~p^?@Cwy{rpM}i1#s>5OopVr$bnP>j+>}eF@^s+Rc}%m z?I>bW^Va4l!pUyZ^qg8l8#86`Cz9hVHJza93oI9$-Ai515X7%$2jV*1;u7CAOGM3= zZc#m*r0(o#kfB?X3?*Y)M9buaKi{op1~Cgb73HWNCcJH?CV+80?|5bXSY|QZWrHB! zki~zsB1s6ofgYG1{a(!2KCNhm^p5-Bb#3|uF=vUQm=jstgi54w?#YWC=3-`1@(MO$0|WF;t526G<>u>*M`+W~!}+{*Mtbf7Bn5C|fXI)NOn##Rlt zl-ydNJ6;5rprEfEVPdYgv@utO5-pv?8Y-iASWv&P=GlN67>O?KI%#pF7k&0YKe*k&z&$jm+E*?9o zU)`TCGgUJ1hjx)N*ME|E#?RutG_^j3-uAO`X7?}`3!2^!_M(fMqbqt+8wo#HC|t`6 zyxe^gpX&6LmyN2799}xKmk1q6;OW99`7FauThM{aerc()EWdUH7f1VSYg^=WvI6g< ztGiX+&h^(Um9avcmB&7q*imAA)%03Bl=_c0eWxm2oZuPL4HTZeDrdGWyUd%K3ZPys zj$+8V%AL3s(*-;c^CXjBQH26+(VR8jZT8E&!D*vD9U3$38E~zI#0VhXZML1H`hc8~ zDWfAf{J{I88zZEpNIa7nXgD!Y#l@suumL)YD5#+^;Z@ZgFQ(PUglM4Cn}_BQ>#U^G z10C^mfVaq!Ya6p&!m0h;_8qvMr(FRUfpvPnGa0b5j+T6p^+{VW$++9y_=O0lx644hb8#@Y zGi2O@x%z?;aFG2p#xuks7tkNhrg7myT zQd&$c;ihu)iPz-gdFDc8X*p?B5{mPs8@XD3uwH4Sm;7D?GvLuo5Xt2mS40;qm8GrZ z#mgf^8pP2xh0Hrf{znBhLN|<*rMy^nsFn1#Re1cInelJ~?Fcp~Q!a7#6@}bHak^v* z(i=a16375eY1Ja++ChEGbRAJU{R*vSqplnp*blT5=gtW3{t=021NE{M| znL|%8&{bD+3$ePwN#yxl*W@PSbQ1sXKdmGhNNyl0R2)IZb-kVx#5y7TVY*jZDKxT} zt56_tLv2+FOZIp1N}++`8M5r3`?&6Lkik5@x;(2#J?XBX&)+I-6grOQmN-HE!ZI8C zYBw>DZ&U%t)ebdvJ8nX7OyqmvP!l<$m_2V+=-Ka;2iL%)#Ml=7vs~R!#igw13Y!iS zFNeSuo0%`=+-dk?Pfw#CQ% zunFE9?DWA2vKDk_8|Fpp4bQ~EYc!#ibIz?P#jA?yr$#4!GC5rwnG+Mtii7Ebfh4|11=lVg<68~$;xB4kQ>DJ1(n&pI>kPg_>L6AP}t#JHWbQvvDE0d`Pu6yK2 z?nW%~R{0qrDnz$SCO@9b@mPN~0PaOK_v=?qa0;4%7IIn%mb_TeGPP@4*QHrHkb1{U z0f5i)j(PbHu8+F1$DW<8_95!TFLa0sS=RYX9F6?$psqoObNiJBiI2s*(Mc&THbf`0 z^@oC>@!P5+tkM*6%;ICg8bWHx!;|bm>N=0EMjgqno{ryI(Y3IL>snv72AX&A8ZY{w zp8-gWXA4?mOXRm6S^JFuuSMW_S}Z?Mdf5Ed`1fo8B#2i%slEjf(WQ(Y;X9Q%qUx_G z^ivG6elxB-2^JEik$7NbmyuW&x*wKNUgUKgRKv~uVw~Zbt;Sy#Z(1Z4`i7icjN6@S zp<3^P)pIbSC5Ts^x~gCk0$ml&LcthEePvk|{h!A$)$reAxHeKlfQzA*R)rWdgYf$m zX5z_#x$yX1`PosfW?k1TDanS^%SZ@SM-zrMxTZSKF~f>zNUEU!N{U=owi~)a>wgQ1 zj@K_4{L^P71DjwX2I) z(lI5X#B-3qGui2yEeyyYxo#|u{1cB7j2v31G;%#8%25x4!_Fom%nX>c3OsoMJ}e8H z@6%K!c0&X-(vIB31u*R@K*0+fG<%@54)BpXgZ%5lmn-40Jmhl?K7R|mu}x#eYdWd3 zWoPFL8TeIl{OBg$j<@ECyGnYeJ|N_8;yn8j+J7e7YRnx6ikv(}{hne8cJ-jobtqmGK(J;z_Le zBr|`wvj5=nmK4i0wo+BAh(XmvaOkoIbeWY zys1mPUz)IqTq&R{b<5O(hOFLNgu+m$iV?CM!6H}ga}Kx!y9 zr_)Ql>TLQ|$O2<}7EW_)MJJYnts7N=+*l)D0-LDut)iT&Y6I~rO(oNvuUxXT+7L(V zJk4Kt=XrkpZbGFBA)#26gQZuRL7GJ4fi@t$AyVt&@j=L1oYvQN^-_wFf%w5|I5;8f z36rl|=gDhi#?E%64COQ@v-7IKIY9lBoG-b{)SpB&nEqw;68?izu8Vv(LP0n4{5Ww( zsGko_cYk>?EQr%D`%WXXMkmk&%eB%LSLgJgi~+Aw<&?zCpY5hTaPq0Vt(PM{xF(pf zE6aOH;X9>fb`l{D&7{u{RK-Ih8y$aluar1*(>++StGyHLaR;Q&C^&xa5bJ`zBMEdRXLw9I( z5|mHU&Z2XhAZxR;x2=pPWD&K8+okU(4;r&R=0gO~kGMNRm{*J-)<5{uP<;r{hF;~1 zYE+fhyhhtMGIoAWuXL*@RelIj(ZWfivtD*EQi4~X&$7{KHt)PhlAn{C&q|BK6yY*GaP@;c@vl^ zXcq=xa8vGrMyI!UYD%u96~B7vdc6AWst^*m3*n_RT#XTdK0(dc^aSIr5=d-AelMFY zJ0^45ob|Y{#*B@W^sMFk2=V)m?jKv%HV#bvOLuzCSLY---^48wJ`Er3=U@CI#yYp% zFnjE-;yJ2cfCU)8ul~3D+P{+K7#L~U{;!vxOdoZ=lG3y02YmNZBxaM}@qp zjJx?2#F3gA;?iy}kK{Ik0^Q5rFItE1ZUAjae0h_JCy1I2N^cs5d+YgCFd=md-XM;o zE^)3N{0ix&F{XQ_`1vPaBd$5x?BzfvWb32m{X!QfUPnrG@ayaL=#RVSy~I5vStZ9| z_a|bE?aN6>0K*C0yjahcm^2ce{R#QNOG$&ypSVz64MQSp18LEv$q@U#Je{en z>Sa5pA{lOli6CIdXYUT`d+sBLzAOh7-=8Zw~L(vsyn+yn>sr*VqVM>M}T0;|n~&J5O~mTt9~-{^ajBGFa>@wznLbpGB_>Mpv0 zGdw~(qoMpZR^V{#-_X04Y+pjlRKN?bY~naJrF3GQ0h?VDYe<-(WzAU9p|&L}N6}%1 z`|fDfw{Bk4wx@~+!m}Vlwyp>a{e-i+JEn@IrkW-lW|8^wC%V*h3THXAhiUnnP!;Bd zD3*T!{Aq~xH!N{QMLxt|2p%oT(?BPI9q+R?6|egZ)d*_0>^*UpQ#2G`xsMN?`oKG? zStlz`)E|A>s{E!j6%ZcetKokoi`K4cOr8Wq-c%HcoI62kR4C1@l^o4o-VToA|4I}r zjrlSWOGXqLEyY(=sX>SglpHBX@WM1g41nAQ5_Sj~5kZ>N10)Xql|4BbBLGINA&bSn1%m|mWMz1+BmVdHO@hAHj zB`Ci(d37w{G8nQnwZ&~C_%82qW=aaU%lFh_$7F8FHq7KI2fkGM6<|j!agv^m<91-8 z8xilAl6oOHjX?UhoVZ2~T(YQyF$REbamtx-leq-Q`&Do$!$Gp@?~0ceC=m(^cA}zO z2Ibo<7^O4Q+AWgnAg))#S`4HBvo@JAg=D0+8abW}iM9t&{COQ092pHxec-@y;mR8N zJa58lI#u+NKhaBUBxuek;8wG~ef|AXU8bDMFU~@SgM`eW2)o9YM1s8H#2+;8F(K0G zv>8eudE~|{jDy(K5JV=Z%zzsVSw`Sgj|gdIhzE4pIW}iPrLZ)u06l-#`~gruq5esf zd282KZQu6#b;J7|HI1&8)vzGm@BAUrFd8h|R9KFJ9a=tB&qS#~P<4I%{CH@X#<`p- zS9SDVB|F{QvV{a=NFb02tqZ*{5_t!RV#+WTq|au*=D!&fRzK7VBM1lbYSnpghTd-J zWXU|!O>|{S(EJFi45Mbsk%3AB!JBf!C;c!v%ee^f0gHkWuYFw& z=pmVY72a$fAdWma3Ofp0|5c@ugUXlw5Q^C=>yG_UDlS$80*&$VEr&&+tV`e2ig~b< zu`6hz{>Lq~9cN(Ek|{=)&SQiehv%vxfXQA5k)$3&X^_y;uoiyi8H~!Xv^ykimI^rg zcW|FlKb~#wDxm4Wl*xAxLd@iXUT7;=MYl!iba29KT=Fk%RuB1OB1O4l8IJK2d_rk7rlf%+SmSq}ry}G& zt7ORm)p;{}l0b}2sh^T>p31x8B+v1akTWDAB~<#tSEiwnU9;oBdr}NgHUu+OMHiHl zjj+||6bY0pF)C6FK)PdS>=I3smjc6J53BaZ5?18CuDRQo6T~%+MNYi2Q%L3*aSDg}mj3v9OkfYMaSl|Ykg-*yW zMzVcn&>=}%YIgQ#r{y)nUTM&IXk#u6q;CtqkWe4(Xg|aV5o_O8SWdhpG9Zg;OfMi? zpskK5M0&DCEy)W7;5%Y`h*Nhsc66061}Y*_P@A?{&0>Y9pKFiXvD8@1O6o&|@F;1q zt*{Ao91H9I6t>U~%{XYide()W`n38xuv=J9k^Q-_))C3Sv293bscIUp zLiDz3Hnv`X0CzUFZszPK0v32DPsz6yx?u6<$vm))e(|4m(j~V=50co;H?1?c5_ZOF ztFiF#kR;84+yqu{%ZXs9rqbQ9OliBhRr`hfUQ024SFf;AAZ&CI^l1p^?65E3T=O*0 zKqbKBC0l#^EE)gEcCBeqbgRdLN#!yEyC&BY15`VV;ow^CQ0Hl__c#0T{BYS3Rd*tw zQTL%vw6ooy?Bm_$|Jg=u11(CL_gW_%&Kc&k!IRRQz=}S7l)r7so-TTSZPnQ^Vf9#O zbD{Mo<&*oWq4Hc6Ff4c~J8urJjz*WB)xzHt(-WOowr7b^&3`lyLzOKKTjZnXf|j<=EA&XV5X|;Q-Nr+)P2;?PO1HkF>kILKl=dtn?h_ zQ5m#YiACS<-TA`fTQ^^Vh01c}On!JA>DY8l37bemSw~^a3653`;}!$IRxL9zbm3cP ziO4$t=34LWsX8IF7dZcOepIiN8$YUG4lU)7%;9xDsT{7_YNx~zzsvQw-dtUI+1cNC zpBSBb$3Sy_tyI&O?Uo8>iQ=}GKU0-pZH`;_bHqGwY*fkH1#3v=YN2M^5^x~U6mpwa zfr8iPr2cH^H6#QX2wZU~ezZyIpc7;7zl)J(Dg93MjjDzb`p9pt_3i*NBnwvWUtEnM zHl)6Rl<5{!l;7#BD%C1-3mfH~M|-t zNi0f&LpA3vv90PeXhyQ0%!3m8Te)}4XiY94sGWvi=uBjGE~ zd^O0=+C>5-I@;~p1>qS&T1Nb71jbAOK*zdpqqhoQ*Q1~SDpKEWQXK`mO!1ihPjxb` zg$x(^h3>?zRfdZm{;p@H3*5RGSj*3g%X-9jL`t?(PA063RHVf5z3RktTN~(GTguj_ zM%(s)Lg1-ep{mX)7$5VqU(mxvZR79aQZu$m;X8Bl8;1AS z+w;hrJGN&wn9o#A4;SsXKYD@|Ef3c1H#Ql)g~bLqbud$u?w>}!D-z`ND{*Si@heJ0 z;RpM1&#`!7KHt+PLp7;6kH9DL`GpgiWT9szhzY9bN3dxHOiouC`}o>iC@vder=Jz+7nZN#-PMPC4GVc-T03Trgvci93gtPsd`_0I~#`GWT z|8M8B{{#E~L+|;Yuzwf!|MocBy9bgGktj$91Ojdzp`G1m2J|5Ju~q`W=zK?f!& zw5dxUd#{;OGu1vctTn+x8@->G(6}|ICj%8mF&alGq?ndi9Y3g*E@HKwf+mYG{5Fyb zpe=})8$XuW;PDhHNG3@+5p8TMx1n&`K?{P~mV#l*F#$8Y>xX~_($4VyeaH;DW`|sE zra~90HxF$9C>X?f$TiJfY+CQZDdIz+2R+bcJ^{f^PiNbs@ea;7+Bb%$2jIOaDRVHY z|Bl>^Y4r<%H_2M}ZuL+f$_ZEEc7Cx*6N$w1!~BLPP$2vKA9nOd zC?;6sMUW7(W%?F~reRN>E@?*Op;{9BFmi;h+p3IgA7mDL1Md|3S5`#lHwSDzdPsMCn0Ah8r zw4F|lz2EUn=bUw{dw1kX8Wyioj?+nNWbVLJA@% zA15g!jd3-?fGSkHH>joz;ikyToMz$5Jhj&ELFTAxQs6BnPzAxJCv(g;YaRP=6X%-| zo~9{|0Dgp7{m@$Vrp^cK3agU!(bvtwGdj{Fw0&;-o0(3=1JoeO<4nusmf{W+2U1@c zJ%!<2D)d{}gp}sucOSKTwhm5U;q$tSQmttqmpvzkD)-&3WZ&(q`?8%#NRZW)oDZ|n z7KBxq&=SRtBzju0VZuX&l5u=hth)MB-V1q8-Ny+iEW*JNn6U^yNWFO2E!ogqB zA^?o@*b#r$FYuDGM{e>sWJG&KO9G)}hIGhd>6b`j5+ESg`8zkba0p%XUF^T4P5N##YaU91|d zJxsWRqz|j)w69hKjRPUzv(!FY5MDBdUIdM6P7C7G;W$Q*obB;RG1!N9e1b%v{j0&_XI zGK}$66nzN8?s;wc|GIBh>Fe@8pWmNFuMgW`Z^+RNTVR7K#F5vsA+=4$%nZvXlI;Kk z<85v?Rx211qS_aWXn>M_Y^YjaNzxC6h@}e3^LEB=d8+<{tqCjPJXznp{ByX?563cG zbe9wKgF#>`00nT_n6s#`Wg z$D*tja#+Fb@h-9q;t*k&qAAJFSj}5Hnx-h}FUbH^+GnqFuL846enwyNa3X779$h{i zyRd~sByQ0t+cj@p9q>kMx!*Fb?=j{t$*$TW*pfBU+Vozq5o6PJIcUCn5i&1+0d*#tp(2H}22 z9vm^Kd}E1X-M^H>HwrGOtBt4xfS$A-Ru*Qv@BeEZse+{hog>T9y$okyB9X6)y6h#t zL44^$}Bm-hWY8aj#g0hOcJ zArVVP4lQ-3z1+C6**(qBfmfJZsoGbbrLM?HGhk7VDH|b@uupoLt&tXmJLB)Th;iXx z@>%RP&{Ncy55X$L*9_=7Xnj~WkVFW{|I+i*o23JeQ5Q? z;3jL|w$bmvtB~WBSbMhlXVzGL>yAC+0^fF6o@nA*8^8N3%os_>&j(rYFd82Pz2al9ABYLXfsl%AjGbBzAST)bsh%_1M|b!kEShIoO=3LOEG&>RtG;$s&_7Hx$w>HSFzUX?~{L zl(sTu?TpvJObc#Wgkx5QqnAD)7TuwXlXC^g;-e$M>Dt55A+xpaFSRZZbTeL|ws^!? zTNRQAMC$njmDzP*>QRS=T#BmdIYfq~M^U-27wf+8cM!OH)z#^a)JSNEb2;UBg_8ODTgf zClo=bZcXE7S;r|Fv;#p2MCt%hYsR&R!A0y7(7wDtJAz8*-%p?V^l9JY(HneUU-~h0 zi*=td(-DdhGdnT9oFD24a#lS8diAlJNlQH4%gT1e-X$Lu=+Q6QN-cA)(=5>e83S-R zhELO63L|epk#~SJxu3vUvCetYf4+co118Pl%AZbh6xyxswN}+F#zQIKbUfjqXRFLomGP&C%S=n;LZYW)VhxktT;h#h*&>m%?} zg;?$f-vR>l3#OX_{^_Kcge#X%v`*xTx5ea#x!{Li>LE4J11yq^+?`Rl=bd|iwIz1> zM5ljM-7CDk$ky#BJ~xN%$!G)3B(PVC8uU89Wiqv#@{&I7vKWeje%b&n)x{Ohdvu#L zUsdSNL_jsCb5Vx}9&O|0&6+B%(&2%L-kC0z5P{lemo@wM$2mvz=MP!??+3BG(jS_(_TiPm+v9_?W8#3Eh19c;Cedci0-Z14 z=WEH?4w-Au@K90N_jjsqR>~!>EHv>4tdEda*fStvi5y>cr`9zxEZ9fq-@bs;6*ufU zxGRLnyqCbqT)v!T>dS=Ycbm7DmNQp6z%m&;zRwSU%#F>zy109N*?6D8>>o$8w%(mu zxY_|nw{Ux)q37Z`2VSp?!Gk+nBp^D}h32)BFw?=(90Gk-#$nvDJ*`yJZJA6D%Ir(!1mE z-ag*%8xOBj_EmCAE~J0#*~!qvw8BLN?twn;g<~*+Li@6@Y>Ok|g1@cEAyQ$c1l1Ra zSyi@?$n-RY{mlcmdswAGNpeOL(69nj0q2ZSalukq2-*w70|M;#>I!M{)|7|g?9r_D zwF!iY;9et0p(MHEStyu6s(>_+_YLed2>(dUQ<6iV;{w|NP8HMVpvSToLIP3GcKT^b z!IESQ6JdTgkeW8?zwsPXGFN0_K{3aQQG(0Nn8&Hc^U+)!O92`tDhWuC-mOw%9ma}& z33|siL5UHS0ry+ymy%Tnq6E+eP-A9EIhe-zH=G*mi+5|U60a}>+n}~ zi?1e-A}K86F2@|Pc7@D~1^KOTM6hI_vKhZcA2-a^U~97NlmOO`&7Wm|5X)3?cu5rf zw46n*%X<)Yx{4ChgVfK!J+O;J1G*4124)tSp5fV_G4L7bAWP7D8W1uQ@^rYoF^$fD z=R;8Y&YAZ}R8ee7NLCc`h|L;x0d{~^Bk|bQmx!Le+SF@|gsB2n0GGKnQ>O<4jK)W& zfPTFxZm%@mNV50|Bx9?|DVe?hL55p!)`8OU8xEF9?bVTi2<1>Q84t`*5=CK=`ocf5 z_!UAT5SFn&cd}$37r#t91LN2NQSQ_^Ce{x7>c~g~MZ&q&-MDPElOnLwYq2xW6HrUA zL)c&5k_PI&$&`L2`JO_1I9Z(hvWD`lAwO~%W$P7X*Q7~BJ=zi$UOwB97xMDljR`Pm7iGXY1k$0 z=h-F)zuR{-wSO?;J0}g__5sQ~FFy=@c?D*eO|{{(pg|l2QD*IUB3N{p-~i#laRNIl ztvcO~I70I~Om*3PiowRjdB&QkX(Oa6;)ja8MLMu-cOij@ucBw(II zi-f(ZabyJ$#lgD=@``Zco|$iT^nV17y|tnTvyK`sM>bU?rdZ2CL;WCMQ3s`;@^gX3ujWga-3rf)h#Qm)fctI={q6pN1#EVG+UF#RfDK|w2LeqB{oz6Q3s->WmYUO&>f8;yE~l)@gj>w8 zp)WE^+afz;+Xy+DW#vn`GAJ&MM*rpV66l;($%wfn%hYColShRNH;eJj3F2L!gI9rt z%!OoQBA%j%6*V$}VFcX&%i)VO_55;g|BAcyc^rYgU?2VP?s)b&qP%QUJ`z=CGgMjz z8hjf7-VQQp1G-0!z+%HpFl>B+jrp2W-PVd)M3iw34E;L*^>odX8XepED=L@KD?}S9 zw+@bDwZGDm|E>)XdsLWUz`~(o*aCc$^4@f(nkL{e!O}jH(NAtbt5&V1;(Xnz?P8`% zYxoTQc4?1gvLno-Q7vJngJTl4XMXGp&)3&+L1%4On&Dda>hIOQTnT)1nQOuJ z-^v=A)>WOla|Siknici#Qgv5RY#045_npO*cigP{J?I13HUK!k#igutXE3=d~Q{RJ(Bu&q{ z7L`Z}NJ^`-$ovXl1<(-~3Y~$rpCsX`kQ(7Spxozk9X4rY*L$D7SyU}hOha#8iRoXX zu^r2d?2IUV!c-eQp~dX+c4;e+^7Np1)uBxHT2Xc|-7L0ene`I?_kNG`Hq znzQ^ODzaE_QwDediG(`gDyTv{HLV4|SzGTqU7p@^*DmWiL+73TZhx2f8b+Ucs;dk| zRHyICWrZcQv!2?|Y_JhfPS=obu`V6Yj-;u~*5#~gIH6W_ca`uA2d(?RMSABhDc)e! zD$h4r0a{dNaq2hqbV#|GuIyKH6vz6=l*3MZ>44F)Sf z%wr+=IiE=g+lhbtaqaUN2rDhHWNHPM^~%W{Eja zQOt2;{o0AxsG*c*Zvi=Zdk4YOUI0+>{LRT5hb#d}|MvxCyu5wjpxR;!W(~)5Lm7m) za}AIT+&hjjVoZULnT)ihfYWOSwLg-M<0tqH#)bB?4^d!qEm|)yqu%srmhSloFiCEN zH6r;0=H0$Ez1%RgRsz2zG(kc4`i*qAh_G0b89EgZPu7w@W*tFI;7b2G>@;xCJ?ok^_8-}!vh{_}?uk|$K>s6y;pZNiblGF^}vLT1enm)ol! zGhzrz^DYQoGHAN^Kc(og8}Hxq?kogPKL(zHN)0q42KlT<&!$GmZ)m00;QbD$O-;A8 z=k%WvdX)nP>A=aR@Yp~boDWURotnWZWi?fjIEczCk2+=0HK^hH&CNGkl%?elPfg^V z2}3fr+bbQU<>ll$`4He|&XjQYuplNdwggCkpuc zG{;_lr4khW*T{5fZ!*B6!bqtr+r?;06av}Nh_G@!RwFK&T}FASkz8yURW~h;=$b1~ zBW~nZ>{)ap(L@Dm&$e6rt&wh(7u)M|x8|*s*T&DaQKo?J;R;l~j;#vRo7YaC8_OXw zcaWO`-Kx$Lh=}2RBM1#5cFveLz#6rT!KaAnk(X#W#c@C`ELYJ26QisN1LtSvB2^`V z&+ZY5*E#8Lb`yux-97gQ@uBxiHlMeF~{>D`uXXGUw-fnxWE(4dd%WFa3VPW}7dUts6W z9sTN1CG1b^t;uOS`*usyzo)D0dNY#}7!W$8;P*7DugChf8!PYl|Ii|F!FS=bIZso{ zfsR{iBTltwX_8+Ha82~gtxXv;_?YJDMARSO454Et(LIAtaJoi%d>lj1e_idfx!*r_ z8#|oyZ_;}A=sWmQ+~hnP7N9elZ+XAHKA|&4Og{I-VU9iU-3o%~-}K&mUjAxxC!>3ZDCh=J> zaRr*IDpmfhWa%8T$5-MDZz=zBNb0&QRBI16Frz2u`)TrWP&nNeGYl7?mt)VT?)C9{ezQSGR@J{&BoR0xAj`=65le5YSHdqY#+F^OcaArZn*(C=@^)#L z`?vTi75=#lXL4M7S4ksnF{$C+AKrY5oI0G&P~5-2t4g zHTEKRuI_DQsE;o1Fp!=sq^w1F+w}xI8l^H@nn1pVdn5cu$<^peWg}*CUkd;mOZl2P za#D&Nz@)R#C1NsGL-~_JIwlQPu;9K(p=&E<>JEaU7kY+khxz9rYut9c(QCyZ3E(hPvnWA zUZQu@Te>dX9B|?olv+K6_HIJ&9QkyuiX|bG6dx^==xI)h#&%LTnI-n&rQmo2+_sI| zIG6&4#zy0yZzjw=P)2sfD{>D;o|mBz3JSXN~;G?sLW^= zjjds)(w*xeq!y!#S2XUMhG_i4H-eTmKy9@A5KO!|SNe}kgBA3%d zglBKR(Q*L>PsY1jqzz>1pXl4!tGzvpS_yxQ0Euh|H0$9d=^WbX>6|p&G^TKx)wClp zYc$X9d#9<}4X{L*1AMBb;0Ay~yMk_U#b^s3n-qH|v-*y=JKzypZ#3!i z4@WF+WG#L8h%_*q032532LHrCkyOs`-k{Usp+yS%;L<%ImsSp}Q`s|w7HacK=fJbe zHD%=ALHSB+Iv2pUckj?ONJFfx2$_CHfa$lY5F`Wg8p(%JVGEzG23kY)T?sAVirU+o z2_RClhQk`1>9gkX#No=^%>GWJt@Ih&B#I^n9yGI;9-`vm!$huXi+5Ov&@o~yn6XW9 zz+yI3?hHZP%9od=pCcykUkhtHkr`Ukh$ZG2lzU*dGe_^I=a+TmPi5m+N70ZDS1TXK zy|4!)sZ6`4I3QFL8W4tgk=o8TfL3CLIA~x??H-ce)Ji%AM`;p7CWNe;EScw>o;D6P zvgY@qeC{w!&4eVRW`%eV`z`R?4KLws69n~yG)vXG^LPmM65$#)Eh#t~lO^ed=WOLU z&NjM>tIk7661x~U6cV?RW|d{QN~(~tJ@};89Nm$P80>dj+Ankjjnwfs!*)K32>_r@$T9mt^ zUi{ucYV(`M-y8m$v#0=5Y1i&z$SiaYZLoo&!u${#43aKipYvdECe~3p>1+jg*Y21< zMq4w*%>26rgp?fu)Tf6DB`qG9gV91KCme&gGbE4!Eq4C$(a3FSL2GOEzJjrtrMC+vPyD zEwE(_1rV#}nMWgp_o)@+EaT$Y{xuNm`*aE!UB9ASK(&E5Iv>jHTsfQ9`NezOTw)#* zBx*{~FB+Szt6-K98y-ka&T3j!t^9^x)Yf$vst0;Jpqxth{49`;bX}(+_1eXANm>`pBHJeOU%IHS(n#wg1Q9#DZk)RaB zUh18zC<8=-2@r`pERo#7C=)v6v{oikihJW#N$xEK2py}?HC9urv{Zsi`3hymlvCr4 zGb8*=A|6bfgk(unh*eiAxHXmt)m~YPj4Z`QQ-&DtS}HT@iP$@Y2AMHZu>>feAvIm2|Ic0B4HJiiK7bz_+&t>|J(!{|cd zxTgDMwi&zoRC2pjU!Kg%;FEVJ8`J@dua9EBxk(0?EUnFlE&jRS%#3Kz1yOqYqtdS_ zY9E~J7jH;8jYm$M*WapbDww0|=FG|+DiN`PjZ0sh(}yuL$%~zar&i^R3z|slh`F^F zVP(TNq;A~iAw1pD@-{(de>BjkGY{#d94A@MtIfpedcYS7)ASTQQ~=Gh_|5q>FU*j@%ap>cV4PY^3@W!C(hyp5r(ke#xi8wIWI|c#;Bs< zYbP$6&^{v{%6m22gX>*{-jsP$%NRL!9en2aS16~XgI^r!>7<;wN`$E#wbv5Xkiz3h zHYmO23VvN1-1;A8^gabhc6(ad#ge^!tuz{b4jvvUqP){dZVR;iH^ljGWdI`!+kf-| z?EiKJ`aclo|IM`aOPq-qqMx~l|H!+yD0rlRV1T!5o`2-sSfd8G}SANX8fI-F%`3rT@`9(OrT_w+Gc zM+5B3T~Gk$UcME2kF~wedR9y@rVWLh(Le%ujoIBMUp50kUjwAkS2P7my^IVv5@qPZOv55M$=aiXQ-BVSVBXs{b~zP zZNLo!-xj85u4XfPPCPLB+Oy`ti3Mx^wmUmXX`fyy^s{)v)Tdh%Nxq%g?Fq3WrFM1bo!ou*R0>AR&T~1^7BrIHr9)aU zg$ip*35vm^8a&?=(>uTr3;M+C`Kqx^`Mh$UB3C0^V>%4~AFV%fg};fOmUTY0O@3}$ zn&5rHG_Bqwlp&jKl`H*9$i+EdvC$0Ca+B~t{)40vUULFTd-b8r@epUG%~kh7-|b|z zhP|}3ZI+l&JCZ!GC^n)hEDC7&LPmusaVZ{zP^gtsqnGm`^&vDHy{_3-|DZ60-qePh zK^=^AFGJr_JD;&Rp7N>jD({;1Wk=KTlIA(U_!De5>dTb&-b}@#3(MM&tpK`p_{XpG zh!Qjo`!pwPZaDCdHXkftRmcmjrK-p(W9z0Xo7zmn3-;h*fl!PlZJNSA0=1L_$%bCY z+dF~1gAFi@1KX}c`3|KR7)ET=GEDqx?J&@A$Y7Z6L7_g~3kyQJ%asoIH)TzJ@`{jo zoNHWZLF&E~QpzLBqLmTAwqWm_iE=Ch+Rp8k``t4AZyZe+UtFtK-T<|sq-5)H&g>neD@dAmKU0&H0Wa)#Q1qV z8BL7coN0QKo?@1gRql5M1|Y)wmY9~}WgSF!$(t*Q;e~lhr>;?II>f=&?+Mia@v0i2 z_5ev;F(Hi3K|k^`tX2@Byy=TyEjRM5Qa_LKz3b_prOb_Jc{xa_D8@xxHX^;xGg(JG ztji8=pC_|hi|&vH)k7)C=J6crs;qi<*sg|X=1mhsjIgq20sN{3P#i@WpjN}Rg1}(q zvG6|;(Kp(&6rup0_R-4xjTO@0+6zTsKPX*5Kx?VXf*75-ADfBi5|#tp$Ty2xG*VOr zv7OFrB=$&(!Yj1g7C`_UZCi@kt!>TbR$|Yk zE*^@OyX^^3PXh9W@Q&D*6q$>;9u@^WrKCBc_m0yO)to17g#CJz(^O`U0#e5W*h1%g zG@Wvge!n3X`z4}J7T@?gp(7XHMR7Y#a)g=dC?EGIRDpVJvh9l5w2wl89dTHUAO$U( zHHo~0K}JU5LfFgVJGHx#=#52P5*fxU`RT{LG8|!}0z|w4>JN2>?)@`vveACX z`&F&QOTrnk;+^Kh7i06#<*@6L>o43TCv{PXw|W>P?{h4b{uYI5c3K6Y^Yupaiu;7m zGkM-aTz_Cxt-If^4B4s2H$V zo5WG-i*jJFrmW^%38j_XHXQCsl`aCcu|ggF)fVby@ExRd+=bR$z37TNam~Cd^-P$} ziI(ir@w@96x1*4fdbQ|HeSdQt&1jfjQn;9i{dcuCQy7KhXG1=%qr$#3|A>~PhOJ^u zhNYpgs$sg4>(|`zFaS$LH~1zm1k?81w}x2KbpJ|bRR*+KOdM^;uYG&9BEEwv8;d=Z z_W=aG60^x?R4j~N!E&HeDgq~W;Q(;d@%V|oU)-qgpO6GL{v(fW>7XO~6S}g&XhDF< zoWq;ezHI67QVZn{g`xou`4cN_19q}2t8^+Ljq-P1QHY4obY-_MAjrRACj)%xMX(0#cIs9P-7*UF18B? z99dyipt@R}yenEpRNP{uyNXY0s=JzTwSGAVbnNNRY6k8bA@ z`Ksv{vX#S6;%sU+BwfAu@ji8)r22GoAyhHcGSPaDj?r!5;JO~X0iG~hFKaFJ0&mKD26rRw#uZLQL!GmOm$@Y zaC9g;*Tfs%!1S)( z0p#j(5I~AtW{!UfG?Sx;2{67(bB?Yb;W&%6hyH1gUdXYNqdiLaWvA(9uMYyzv*Z>w zkpG*ifWZPtakU`hSR2okK;A=wxDOg66*xBl67UP-1$Sktb8(<52x&1hKaygFj3IHe z$fRIRx^~5bQiWV2?b*5x-pf|MUG(vU73S7&6*j0YSUa69LjEykGy;edE#`b_nk#~W z(2*J#p~jWPy6z#8g5y%;TTz`~^Ea4kVlYc1m%d)iyRAu9Z*P8o1HNq$GTSmKvb#Rq zYcoQy90FV(Gs>L6$tO~KH`Bca!#k;03hBEGL18gpx+I0nVpFeClM7;&o9RjbR`5;g zZ(B80+*UA?z;3W1sdNq7_qf&HHV67>Wy>w@*~^Bxcv`Y0qc8WQ)q@|S!3nsNP4%Yd z#~>BIo3CVQnbiQ=tIkk3;=(c^A;e+BzQWSeOcRv^ z;ocRb(6GqSlF!gQF?}LG8M(c#UH3o{ehSEln8($k#Cf(i>|1Yc2Zc7^nD#xEBT-%X zL{|t+l*^IdIZ8`ugY4K6mJvRWm!IO$PC%GHC^V$7_*AWDG2 zr2+zBOgPTx8o;nzvNndS*UaABQT2x(18`h8NEonjT!^WQ>qkf5l^Qc9k7_5}1J`b% zke0NOF`a#%l6ah2Qa`%F-W2yYChZMor`e)AB;bSB&Cy#v60uXOa*Q2dE)i9yFTo>qvppL%>$+V>>%bIKHT%K%g@^zOBbJum$8GxT=cJ4 zaj0nRuUOQQBc4Fj2m2Ra!y`MD|5ic&TVKP(&hQ_G9LGO)x&L2>G>(5rbN^G%W(Y?n z*0}6aw`V_FEdvfO7F@h4rM;sazO9N`5w2o0K?Y??bX@oD*{GM5^`_iXH2lxs2=hwEUEQB74#+2{X+MzX&~9{ z+WF7Sk78I6WU$4Yu~ANa8Anp%QXzDlt1?FHN;K31NNY zS_eamh!FCI4Z}vicP%1xqS8k14S+CI2mlmAT>*biRZ&O_W#*CI9Xj$bGh8TFqD@h@ zRS0k5!*nTsI8tg0!c5ZZt6kT5V^?Z~NrQ_Ike~l8#mbi*VupMX!#Ao)QRE|(xDlVy zUygAP?O?=br2^=pG860u+;`|jO<5txpzY)ylE-CF1q$M*g)u!kqL;A_N%mjI>CFW) z;-q$ZbQYmUC_)O~=X%Y2ULLjIm*+J-+&{8R^mprRz0HO(Q?@aX6KU$6uI8POiQJyA z+q>JQSqGO?V|@n={Cn$$u6DVTi%h3!;G``sFRGi4uKu0OjqJKLJUYE^|3WW&k)mK} z(lo31f!0(ym4F#Sna|St=G^m=Cel^;y+8hJRU@`WHZ1>8eQ`4k`*b%_1s94_=@5Oe zn>HunU;|VzBdncUp~8m==lFh>yij-%w*cDnR$eVIq{q-xR9QZTJfKXXT?klsm@^ik z*}JxUlAY1a7pClHUkA1pw%TKIcv+IUc(4h_5;X$Tslel_1mP z?@wSA5Q%wvT~I65tlv0Zi=&@bkQUH@p9Y?DP1^6f-JK%$G6gi@dJpAGPjd`={562H ztYL@4Uj<73Vw^Qo!d!iXEbL3;wk2N0q+ci|%^X3T7Rw)?0Ch`C+1RC_B+~k}@awz$l&r$hxe*@@+@U{wE1C6u<)A;{ z9Q0EqR<*b<3;#quG*{l#FN2tdpA-UQfeA(pYOJCv7g_w0NR$j@7T~uG;6|a1Mwq9e z%J+2@FP(viPew60Ub=AXlDz1A5G(1e&{9l%$ZV>#QBx--h%vlzj9VoBWDPY#OKvg9 zk20){m;5jz-)iv(^sGVAPEmeok3lKkZ-s^__nrJ>yESp3jxKFMEZCZbZGxd zyhYWqvPn-7+70f)vBfSspdDq)ZxZ{IP{ATvAnPmXOM+l<%nh)j!ZVrasEozzelpXT zqK)J?{L9#!;gn11X1r}_F2>yYBrio2Bt3bc#U?v6Mq7C^d9Fi=Gp;emvC?Ps>)glc zav1kg)lw3~4*zgh#>FltzH$d;-T#NMcZ|;L*|vvc8=a)1j%_@#(XnmYwr$(C(Xl(W zZQIV9-#O#_&waT650DX=UezTJ5Y=YcmiRqP1EJi|K6EUYl26 z1Z*MZ1-kyKIreF?+*LkNM1!Ka<_m^7agFj@b3J*$y0kC)%{aHzss*T>RD;`!yE+0H z*CDt`BiEfcULH#(_@}mI2w53|cG2uLwcte{L>xADPzSO-QO93FdKy`MR*y*k+QJdL z0&z_lG&O8rfhCAvOL18F3B$5YdCi5_@=L|yk(Ow;i))%oB79*h_us$G=E4W2FE`x) z@8LCt<>{8f%)4v5Ybd(fO+$P)RIX}SsgCkRX*jWx+Zp7?MztUfUri0I!&+;jC3Tjn z#c0uWyt;2B9o82bYOaR#ky($>poA5ZF{}F<$+?z4sNKLbs~=M~)JCH-Ct#(f zO3E*Qs$UQbFt-TI7(fvp4_>XWl^ld%#|KWBE1%#OGd6;mo{0lDEF&{>0Aq)N60?ep z5-8Ra=(7rPDK!tUl*J~dHiNOErft4k;PqfVc%WVN5KB*&(gHskW3(<9lLLX)Q!ojj zlBj36@odsvrQ!BN5e@yxiHgb+Va#bVL@m{rN2%AAN*!rrktlhZSZ#TZNdp;|#71q7 zQIZiBN$=u$i(@myR~(QD2NpkpYl79nfRrhWi5$@=b6I^kpAZ)0HuVtkD-YNBNzlYv zgd-+MrOVWZ<|EzEuKw}h=c2dO1$~l5l7$Y$+!ih6biTp~zbYxRfj_>Caeixg#bcxMVbj!no!AFdFunaO%SFaZ!&cOCjhlZ-UsuV5TfJgo%*>xh&$ZR zv1tz+L4a26XjC>%@YTjz*oQJ&hOoS~6FzfH4vX$)|FhP~sm)d3N^S>63J-m&Rpgq@ z8|OJKKJS4kMuFM60{Ze622uCX?)X}lEsnN9Y!ibiy*3j;x0=SgBozaz zcU_}KXj2TEdn3_@VO86K!N$;&qKrl;kMC|)RH)zQ5ueVHY)E z5Dmwz<3&@7--+VSx~sH;?ZAnyWeYv!wATSEK9*wUAG4syIpW0LekWdhucm_&4Lkm# zZw@T(T(pPixCxmOi`&Xcib>9?n>hPZUjr%!g#ovE!vd^^<{$ONiG@3cTdOwR?U@Rl z7PiS%7YS|tp-^u>ZamWq9vSQXp|)Kn0Xu8ra7G@}Q(NzYV|;$bb~4=7D|_qi*voe1 z5PJ~wM3V&jH%F9bV?88lR}xig6o0KIWX}p8^vj!I(XI?`igK7KMY&cqkVHxiX1ZKT3>Eto zy{?55!_yrc{PcJH>MsZ&U^)M9JWOlm?~<(%#UJf!IERTXhXRYbA)yaDU4j2k zox@Cj0GAzD9+gN!KMcBQ~=pJ$b$eX%6;wK!gzynsQ%6n(XODD(Jm={jk|8VkR9 zjW8}}R-Nxk0@>fkZ{c|ZmKshtpzu1S-Z%ER+HMW%_*ki_gW3Y^27~i^icSfFBjFTUn zXXtruw{}ASoY9klZtj14aCg5rS{hKZMKBQljJjOea7NcQljH5IbWzXqUpPmt3{v}Cx{c-F&U-!gXBUS!dzd_ zc>!t+$cO%x!!C{@x}FM4DlICsF(kAXvLO2ajqoj2nFJK?yU))xlHRSZB8eo1BV2C0 za8o&jIW9*l@}`)qvGzv&X!gXsR>}wdf`4yz9Y@xe0!b=Bk1Mj)n&Yg6Wc%f0dgt;5 z9xtLYCpeO`&3#dt?6x42cCBOwSZ;%gl*XvN&10Wgizi&{RoBZY+xm!oV73(BQ(*?8 z)H>x!54#dEVY1~ULatmz<;))7dBPZ}0yn4EC%f`w5aVnrbnRHbQ5g!UEV=$IF4N+{ z+@)+7YLw4~7MsKBtzz=y*z6CWak)Or^Nwf57$`f^ef5Uoyc29#J;e#101&i=-b_|a zOpKumscrfG35g-Bc=X6b$e}7K2FnW1$f}{M$ARWBwtHBmNfIQt=z2mcmq~joyenSf z=}}>hnS(THgNc)Q@tDest*cgkL6N3lh>Xe6+I%g%xN+E}#Y6zo=#GS*OnF{fX|V8i zip^=Ij;?u1-=-rjdTXK9yg|$=rm+y3N*Q1ab+)V5=Hkw}C29Lz*V*B+vog-Gd6t!R ztWs_49rmKy*ml)$dc|e+5xzSOK+^}NCCtsdIF`7aBk8(ob5RioRu!jVNkM zWy@s>nLVr8;cT(GVB-;&(7NRKHpyXhtvAAT#jNH$Q&?G2qTrJ&RknbG@tX2ltuj0` z9`0bdo$%u#4X#WWE_dJcEd0yQw(i&>&UD&V=7ae%`}DTy{%Dua0Y2UCJw1N<1Z-68 z?gVn(^p@#frO6wkjZaV0l1k0JxtDvs$Jkb~`@sK}nLQ|zLRCf!c^-U1p}qe2JVaiY z1Qo89eAdF=&5~De`tUKmEvPK`xc#7i^Lz1+f{OFrj4*mh7t8C8Ri-4S|HZ^)XZdekoc%xUdjGA9v;RZR{J&6rm8L`t(Z783=H2`*kJaRX;XyB( z-(uH5;P-2XX;#2`CI97njz%J1sA%Le1s3(7N*ni1&LhNaKGC4{>G3+H{{wVu_eah~ ze1O;F82-2L?I7h~i|j5_kkJn7>theUq>($s$mxe^^ED0zb~g9)ck$f5e&m}!o(8~r zE(^i-jHw98xr5;vU}_mUn$KoWU?zZZWba2@v;tB^@lRaFlSYw}tL1l=5dUG?t>R4K zCo=Y3wui<>{uz{}QuP~GPS%<$cHC|+rv&1=Rw>+$WOaaT<8~KxZ$iLmgc-1L5lpR&mYX@-O&Z}=Kzh>XdZ%~4)08RUsLhA(yyDA1DUJA zuauZAACJ1;=LRnZ8@9v+^Dh3AyRFoIGj5C_B2z!$b*%qDdVwR8Dp|r5#msXUp8Te3Y%zEJ z(o5XnKp_jZCRC6UZ8;vz>44C1&QbvUxAO|Aaz zAStlEcZV?XTax}?Jqz1+AU?V%cQDS~X$u_3d$~$EKuuF2_t#X<)5f$xLuAiMYeD7h z6;2TAPEu(@4#80SHH*2=6^M&#Ptw}xkj;@9IIPArHHbJIYkK$z?-q>#ThC!T1wu%1FSxas22P$_`UVazChyO0sAjsgrJ zy$q@95@Wd=l5h?zRmb3GLE*aU@Jzjm(W;_2P^9;J4Vz}}`6{J?^GUdWZWV`^k|YaE zRnF^#^Y_|0QK#bRdHg$VGA1wLD?sJs*f<{!xchjb{bW zMJ*TE!N$!;Py9%Nq}co^A3-WOGYiGsMFR5bvXpJu3|5zfV^pN{VaLmHj&p0kxLumO zPW*LB^2S5O6$d4Xi31Uzv9Ckzx-B|T3poJguhnoT-(ja|f*q4ASdZ+VAies*N7~&k zCRpSjfW}$8MFDV&V|gS}A3FJfBX>B6W$S=C35c~AV2nC1Axj$$9)2Xe97>Jdq}kK# zWsh<*g5pdI?#@vRAL2V3X_w(N7nn_0h+PbVst+5@8g4dGnhGRBVu*ozQ4am(5jis9 zFncG-+J|(aWj6qHUqQJgU!;nC#&W)s?oM0j<`eBtwneUe2+$ZH zfScI0M>XYFZK(xYOEGjc_@+jbYMWM>%%0a7xP@A+<>TQ*SH44TC0~T9wsD0MdL`W2 za6gA-=~IZ4DiV+454vrdKXi5$_?laAu!>Edjo`ga2)_eoB9DICu7MCciK2V2kA3Dh zYw731MZDRCF`h%%X{xuL)FtneX@t3FueY({C5_rwT=p=}QVPYnW z*IIQBR6TFcFBubTM*O=}oI@gr|eYJ0d`GvSlP6pY|uQQaP8&Wxai(HAX^Oy4tSU+D)F z=dJF2)xr$`Io$E2uxn?$99`}ATwdJ;P7q%6pB+2N@w)2U9x>6z{j$N_6r6TT^&vh* zU$Eu%LFOq`{c;9GPjOe6I7&7U^V4R_OM;+n)2lRZT&dMibe!Ej+qVAUKvzIi_*BsBou#p&r)W*Q^1Zt7}>B?IRV}0;r<|%S*je-&;6hc%jWifiDGJl8k zxy}^UxRu}_=8SPQN`O50A+w2LO_jWPU8XJ0;){^p0TWQAjpnoF$(5bj14%Tm@{vX< z2e3OFy}#gxg%rh!```(Ret2}7vt*4UqhGCco9|Y+=F}KaIem=8^xJ_6MbMP$uU&fq zeEYM(vWv52-2?%Gij{Hci-H;oljRr4hnJ0b3CWc(w`H+&lhr zf+MMXW)IY*o*R+R9u}UHxVF}n^LO~?Gft#O5o-E0Rs)q^c<2zP;$FCSXoytGCIyvFr>Tj>?@l!-gK6KzPe z&Ub5K%y{otb|YXj+_x12LB>716t%3TT31?&i5ZiA4#>%k&@cZpb-Y6jcwQbgbB1Ul zhrQe%Z;)2ZhjGz*t3j?f-+t1FiW`*h4s4CC4-c}Kb+5$ynCtZ!)(2XPBPzI3>bBeYGy%icYimvC=oYrzl)L*u}K z1m@HM@7g}KsEm(1lEObXTzj#MXxrjbe=9h4 z%cQZ zYWaF?>{D%R0oNN0dj?-76u25J8VQPTQ;bK?fWUPq?S@OyKB5Pe>BQoRAiUOq8-gl7 zBH5VEVJWVgLdNi|gc2v=tY9ED`>O7s$}>0K+74WGa<Kg) zRj{1vP~^=TdgT%BW;H`7X$>!_2&F=z^kUeKGTTlH(@gK{*&Pp+?S6h1w(<0@4SG75 z!2D|W5j!%eF2AQAykoP`V5?E?`eHzX>U8oiRl44X)84vvez7TL}dv%%W zJnf{9|MfD0dhA|y!>Qv#{EqNLGK?j*YPoGDVcq7XDY)Z6^j&mxoT~z&{y>A*J${ge zmn0uX$}^*PM-RsgC~Dd+TzNnGp1Kx-0`58PO)@FXK#)Q`+x`2TL{fZ32+6}no9gm-DE!0;tkEF^G0MFC47wAf8>AXY(B}~PPFN%){sOOx{0`v^#O9u^cx7q zjRDBc^rHwOE4}2Lm`0F9_pGZ3v8T|TP5NvS1qCTp!?V2z~IApD>#Y13YM@Jdoqi!v~-;ndSr0 z-w?H30=6z@LMXN&^PnxME7)}(LJ=|-BApEbeWMtrVT`M$JU#fTZChE6wgV!Lp;A(J zUcZ|0+A#%^4q!pfS65AQta%huz$E}@_K}}sVL(@SS^fPKfy&|~Zgbi|8bL1(y9eo) z@1J1~DQA?*A9E^WK5HR)5ZKDvhXo_^W?r6XBd6$-epsv|qnBho)Qj3+NQQtRsb<|e zfpd}je$GOqnwt#u>ITf~J~3SM&UKmBesbTN3~imoDu-`P)U=WI0f4cUrn_wA3Q7ymF}-^g4w)(Y-t9 z9f}d2@9oT{IklLB72PcDV7faz01E0{`qt^fG^*IJx;?-#w5skdpc_QM=hJl>_K_Mg zY!FaJoGob!=b&K=zz@AdR;xuw41 zeU`Ue2Z3Jh8?$rrgV%`<#573QKAT@g$3H!7o{ntzy}PwSbl9!i03(|dr%ywjmnuF8!I}I3r>XCz z@P7$HIsbZ1WLI;(MkT$yR#^|g@MsLYZeUjv$ygF2P z+u(oOAHhB}=gjHy*{x>pW4IH0$2iqV^DMsH!|ED;d|{j0Dih3JOwr1N#P||j&hfx@ zrAxB91NTh^4SqF&La2PXk_ORfZ6Unt9OgeA3;?p|K}cRg!#LVFWM#-(xqbbJR`(PY z#IXuQzkK1yRB9~oh3F;;$hRNbfgZhUz(67h0-4-)`p&D2BCc{FAguu0H5zsqo@6m}m^86BtcU~CAp%)R zy^9)Z01gZ+H1#a^7b;VMqNyRXhM!MP_^;~QV_S?~qc&i|$(h&+O(=CLJ*kFTJ-5!m zD`8Zd3cl=d*%u1(M6)3Mp|AWf3L5#M?vN4Yh)fuJBkh^$ZxMDn1mSm?9JSbC?6}ta zMWLk0`5R43C2o>5RBT|Pse@PTEzOmFaHy9(+dMKyGUTOcZpw`8*@yeh`+WVq4FPB} zI)777LNKm}vUUbFryd6_hcXQPn@DF}YTmOrzb-yxO5QkU>b6<1G(~(3wRHnByc&Mc zE~u2H`;4+_NjO#|MRQ1L7`IR0!DTCaN-E?nQ&tFRoI+8pgOq?nluk&<=S~(yp zfeLn&t*3$}XuwreNo|QbBt;=*O<~>EP$?|&?!weH11c*{JUWQ192ut#+X7uuLRs!5 zm;iP5Z4#CUZYIaiTX_|giY~eie{o03Fc{3AP`5owbjbnJZtcsz2A(Vo*C0>63=il1 zXbc(y9FEWkEJ~}kUg0+AbNnN)v(6SkrD{7wIBxK}zX-8x{hBxTx+ZX`53RD7UdsT5 zGu)ImeOZ?AhLSp3D;7D-)&UQvgT=qn+?S#aqMI+8LFJ;_fEw>b3?xIV;CG`;_Bf(( zVeZl>z>UFVdI5Y~fCL*=_U`7?l-obea}Nii}2^BCHNHlv@%&TL*aE1@|mOVDUmWB1)7>Tyl|Qtvu=J=b}}BSkOmgm zy!zPA)Zq(69I=u94d@)XIc%;Y5+1JgjMlOIealocQoRx+K?TLBdxU|Vk5Ny@`d=Yg zmB!-Y6NkX_zZK!%DvQBV2|k6n`xT#dyF~6qnkc!@pULUT#goZdom|h-NTLLmuroiP zIUc-H8QX+xV|D-Ta~c&lZ^kS(Xcyrh@n%tkH13@GZ)(|_6I_rM^O#Y%;|3Hw( z>}AoHuou9MNtbNbl+bb?1udeutl76slx~#8DMnx;hI=?1v%60VIwq~UVN)w?*WlW7u36&n+AoUUS zJb6@L4YmXm(;YbxYr)z&Dtl|;gmuJ@q|?~q4MXWXFPkDt78>mJth86!6jqv-Lh>Ii zrC{Z-7(L}b7qI3L)(YzfuX z-KowIA`a>>uqi+CFzzUWORaiM#i}L_*a7F0L!?a>1b)P=b^IXeOql_p8tO62AWDOF zRL^2osvyoHu@06zi4k?gXHn8SNTZPWVYALsrMu%llMF@0eR?VZug4(O8R;%RXN~~7Y zPOfIt5P-DhM*d+y8IpXl>@+2lj1{%>cR9Sc_9Qli6+#LdylAvIY7l|$arIg1EjafO z+dtigxp8z{0b2rha~oT4j)2ap@bfXXx}Enliw zzKJ4s@n0hbpVz<3G0?1{u?lB=yI?7<@g2#LiqbB#>UZuRe<8071cxb8ON4%Dmkb z0%UTp&Pumfw0+`F+_-rhSYRoIi7tqQdV910H=z5iUiO9q{=D8q*+$VE&ByUJ^Q_K( zrhBj__Xz%}hk>v+*qs=xBO+yZXQczfa0xX6VI4u7KQF$JP04U9ck{Lm;}S(FAo1C_ zsgjwah}FTxZ3tV+ddRmH=)6BV zP)`b?$ZkksifuFrb=1K6KX&7Di6< zd~eWw!SV|CLz;|oI2>QotaY_xcyR=WK3vXCbSj-Pom9hxbl-dYZ5pJQ+9Y6~S>im! z74sFXqM9e~wfB&z(wxqU;$wBYp{36G=zav7Njvz-WpwW)VeJt8iyl8L@Kj8gvqphv zV^%A``0+Q#y7Eb@muTA*R;ffLqPgNS#H!r{t ztNt@5ZrFd>d8MEeg>HzdV|??jv~L-#Ka;wPd<)=tIHz}kj_Gi^+lDWNi*LF2EwQIQ zgPNgLHjSs-L0rcqD5v^Le^r-SeJZswz9d`duhsiz=x>0|9qD z!mkwPQ8fW>VOAo^JeIjsaL}~PoDNoGRlt`P>C37hsDg2HFz&ZiEZklPlB*q zuewuFZ&-3(rkzKfrcmGNm8tM;xAgbH{9j zY{7npR5F~XpJH@>GU9G+wPE|IB2&lYyh{2sw|ql0k8T}nBjhizeWtN@X%)dd>HVcH zp&bdL#ZFA)(>GPgxSV72;gRuM%ZxBq_g4bsZdr^Mha|$=0PpjvykDr_G11dT zi(|bgihmB|KEZHjc1S-2gHuX3;(`JJ_#p{CtE=hg@+S;qooOF)I4KCZRN%k!7o!Y| zVI|tiFr-E1hX(xSzBrj;AZ)=YXJGITBCon3%aUaay3hh7_{DI~MuAoJ zQ^!32<^95(FSvuR+wwubk;9J(xUL7K2$n~?B*B`SDu(WZQh0OdC%qj{#i$mhl&uDH zJQtVH2dU@P#sMT~$NkJEFCv(VU!Q?hr~p}*uPYsydxV3OpfAIdDIc!10}7LpK?O-( zyMd1&Y*LYX2cVuJDr6V;bbE2dNrsCv_QWwb5B4L$f%9MQ**b|nU2|WO6WeILBLRx- z*CXB^s1fqZ$DoTb;S)Q+U8g?}yVY}>om{`Yb+zS!t1ELh&Zf=$)&7Deeas@PX3B&#$Q_ugxE(ZF42+RK$?%L4Mu%T)}_Fk%4J_3?=)4Bxl2VRSZ zy{D=2<3aNpEfOYx9CjPonu^WDLZG9P;Njty#Sv0>XMe)F#g>BrZfBih$ zcSop0D$ufQ)%R{MX_~l77*R7MR1xljqfq36t7Z@L4l091&2`ku{2YScxalWd|fo&uU%1a>#jsa@9j1% z`T$3FyN3F#hkt8PV(FR2;|sI-$~;&84yf59=RPV)usWB-o}w~w}pJ5j5NG65BhxSPfx~!;-G)r^|2&Uh4)#% z&jdnNL?a!?UnTBS6$(ExzhHrev~46%O4(^YJ>(-~g; z4og}sezG!D)4Ub1yrrGuT6H>s*EOfR5%L~smNB&_#dY8{jY>d&%3U%Zxo2r894M&= ztuD}2Jt+Oq|2?)vkdFiW$jgUi%X9vIcOQHW3*p=*Ne^WQ6oWjkdUNg;FUBcf-2K}) z<)oW4P%!z$X1b4w+Q_$HwBbO_j=2GB1X0+K8}xYW-6KbFOeTdGa1zUw-wa%=??%(hLOuaKdRY*;X9#5U^Y#d2KBE2FUchK{G7qz z*>PO;*wi21!X5$6G4EGvhUN+5%qIbOXdMaCoi@Ycl+#ThA`s2b^2LWqJ+HJH&ru;cHObOBUTTX6? zU_y=sv$r<=dRDAJpu^FRRlptAet+eqdXs3a6F98ebm;i1i`*P+&2Se>jNhp0f&WzL z)|5bmK6*@ea9v&?;~b}MG@m8Ojwy5-#p<^ny}%4;ivBT!(y)lK?|%pu8J;NJ*?@1b zWN)1EFzpxIFtlz-JtacjCjYbvT(%w?)aka|SDCB0|#u%D;D7h^aI9%7KX# zME$~0YAlF)Aj@CNUgZjCpB79k8V`IaAeQuEVzvoQTKHWe%XlsO1Y7sm+nS;NxFNWz z^j>=|TKPk_@|5KAP2lEiv+joL9PCSyVM8E*yS{_FrgW{(a1%P*@2v8q($%gybAg0| z`t#|b+TKoTd#EbU_nA$19UL?m^gNVhx6N62P zHrjZmj=j=}04cNIvPEq!gY4S<;-|l{p};r{`w<#9=ypE@gQOESEYJx$Y3LVo#2PA? z)e_P)j7lVfIzkr)nHCLd20mJ9Is!V3@j)Y!T$D>P{9Pi?<$TjcgIyz4&af>OBe^f$ z&bmMCMEkiGSF7AROpnn$D%6Q$yhdV`GpS09-lE=P43@lD<5hk6O5NP_WmJxQNh`*t zaxx;Ugt1;tBdoPX;NUBqi|chcE=irZ^3arT!HK!fsk+-m(fS1jj1T@qdyG(LK~`94 zR`OKaf`L}6%qq0atM34LjjyF2xpVlk*g)%VxLT4{9ZZPyFzRe=i5JCEp$2{~TepC- z5@Px*YYLpBN5pb}mnTM<5J4K>!d=v)whGm2yjNuSM`?Aeii`L}btV6e6y&G@vFSxF zwmrn0Gc7+r-NTCLmy`$@T(n`g0BWKJLGkB~6$a<+*0s>XmCr_*Sxqmz%{GE6VD8T7 zRn*#>F>N;FhZRu1&0P`r3>lnaJ#rhU4geD11o7k?%MP)o0)w6E$qD!P2i^}p+nq~X zA5|n(4hvg+t2(DKfE@g7+1DiFbu7v{r_^@|>l0gF!BktuJ=poN=;#8y$|2GwXN~<) z8V)X=;{&38qAfm$)uvqZX&$uAuEKiP1qm=;R^FdFn1wNCDMX($J8iQrJk~_IRqJ(o z(dd@C7j0<9Jpk@F?Xs>M=%ORhK;+fF^;!2c{oNM1j|d4lwrd854m`&zP74rxwfRS@ zUCJ2H{_JLBpz8U`gXAg-+c|_vpEvZtLQCMWmOl^n28vU`)l?mW8@xVr1CT(^ec2WoR&YD zdUDJQ;(NOq`r!IR1-+g*(O`T#<1B-)-*O?lfrLotDLXBudB`-;|E=uCQA-c|kTq6+ z7Q0{}Mt_>O+b%7N3b4l_7R@HjmtjO?#D?sHh?kp@4pMHYIT)@-Yef2Pc~~DXg@-9j zxTaa8lx1BkO>|rI-~Fo8n~3ziUx_#1?9tnz2)A#M)+ry?K6TVA>aw-?WW&zqSiZKx@G* zTRv(44F@l=?LYY5-px>*5#S1VjZoDoH9Z3PIIB@O9GhWFy< z5UJWC;lR)A3rdL$bQi$b+w}- zg2*;D+CY5~t}S;IJtX3wi=G+mnQg4(tjAa+9pH#E7%q ztohC@qsMvx@iKCN?c;WCB>mz1?$_*HIV4d5;CBvZq?zq_Xa*6L3+CK*>P*`~$2miB zF9VV9#!@N-=MbjCtEk=Uy}P=gF2=X}@tIBuuV--?edp(`Y40yrri_Q%*Rzv@hkFO( zz;USBX!ad&YR#yiL9X5eH`Yb2Ilz=LgG#%F4^4Rb%R>N2J(={DWqo@ilR~ zZZaUy$pWBh6U;mo{}*~7&_7pDJ*X?zk+5enQ#pH>6Kxb|u(kSyOQaN^cSP10Z=sFO z7_Q8Zx4QTzH+tU8c&rU^`%`+`Y2VF~A|L`*nJ32X*>-wEYO<4_2ooJ%oACyTO{R~5+IbRURF42*d<7gy zFr?(HAL!9^4P4>A&}h#!pepQPEETeeJ*ISbeA@ADd$UhT^)%Sb#Gz}C&lLr7rQHt< zQ3Na@>DfvFnZd+DTc`@t6L?3cM-~#53&Q!4Qm}Wm9GfkYV42e0t(}25bU)K;zF{7+ zwH_WgD#7fBX1rg(@`besWz+%%rBBCYbs1mp!R=(wp(0BV8xw+1_k`baF$e^+jD=b% zhx%kP7V^!=-AIpT?7OQ%MVZ0~OXM0OR~i|!jBG+pcKyHvM34#T?kFZ6g_STin>9;$ z)Q3lhyHFaaa>=2ECo?uHd1_oh^Vh0z5r7O5%Ob~`&?OiWn=AC0)78IcyG+P8WZgzG zN{A4fTT~HJK4{budGLXeg=~RP8fm59x~bp(y78ImqeN8w3}qUTWG|o?zl(>grZ1+u+717?&ag@p9KQg*RzG4ozU5^wyWTKl6YLAAeFn zKzvhrybF&!9~6D@Fo4Q+rj^cmi1y-m>E|uQMnyr-6a%5$L*Mjr8c;xvn7)55@Rb0$ z_!Qa7;;4lPKWP7L*A&@ zE(aXxJU=c3BuK8FSnBHPE* zH{yX_z0x}Ec}@-N-AIkL)Wf?-T^;{HNYOO71JH6Mtlpyxh_^1gbU)DPcOB1kGxJIb zOvn3Ri)i|lNW;(%Pky*E^gvv` z0wluB?bzo>jDceEDO9W!{#BL49{0CN4$954Yk?IDw4i*FD{Wa@@@RcC6kZofKg z%}4JB&c)*6xb+0w`o0Gbb4yr9bCJq370a&GrD^DEWq{dZ>E&5iB~G-Qoo#$K)W9z* zdd$JnV4}{jE6=Y`c(U!aiyijz)722S(%$Jcuat_6X;@OOC{-3D%jWApm!hRhsx4Z6 z6$orLYyxmlslt2Anaoj-?@`V?D7q1mC$fI`V(0Hxd+&srw;I;kZ$T<%)~!X^w6>U7ZQ8#iLmqkIDrhkGYr=ofi`qYO4;uMCZg}E*Y+yR4QOy zfjs=h?=PDxxIChO0*!|2P!WuhMU25qyoM8L786d$Ov7ySPUw+%R9A*Ek9*`_3-`^d zES7CFl&ROHQP`_Q3%W^f{m;}=zRWc9$V=?`7+X%%S`L+kSBr1#fx83qcU)@#kIxzBd^nj%(e>sX9*rmS~^ZqOJK#0NV5s^8n2D9O#l@q-H!TP zJ4JUdJNvi1ir5eWvZ2jOB~h{yCGohW9S+lLu1?MNM6G5O8tK8#Fy{- zshCNi_GTj_(DJv5{Tpk2#JW=au=h5s>;?!{HgaKahTP}#<+v4Z$nnsv=!UslyN9I- z|EV*CH3(gtt-mhwfsuY6KYw2eVWohA*lQ|mp$!;%5EoMpYy32!%-)^jTk?fV`4evF zZ0+2?gR5zC`BUe=gKJS-@%M@uS(ZJYZngwfZQto3q&F$Yokwn7RSBo4T8*wy#@Q1mIW9xYP*f{NY^Lk31SW)+O3wk3 zTV>WLi)6gwbaTX9YavLoowP3ws2b0z`FTAQiU{ax#N}wKx{h;cWG*5mA>=?_spSq5 z=B$ppNOLD@4mFW`P`sxc2ESMANmK7;2%&B;i}vwrJB`rlYHU!Vg?XGW0nKjCdL2dy zI8v{UyYlCIImcxxUa8y?&t9;WH(0S0H>4qA!2={|Zh!?S(G&kZ<{B-1e0_lBfw|yN zt9v0wNG|MB2!#LEmk9!imYFhAoBmSklQC(8hAA9a6K{TsHhrCWq0d^Be$er!AoqO+ z;(=3!2iah|P~pSn>*BBjc4V3u#&3}dZf3d2;Dj>^AvEy7U zIfMKA-1~vozyYEzrPx7J)ZIl>)Y(M?gH6$QSGg}__?0iDL~FQX&;_D`=Ep^&mNQ6Y z_A{Qe`)`Hr+rIgn^sT7G+--rDmgT^Vd8Y<8^_uPM&U~1AYlm}evlqLr2AkE7olBJ8 zY-fQ#$tWGEs^iE#5qJ@|9=B^dpBwn@b3_WoaU-~VV+7y%5-|L>}o zf&M=RkN@+o_aEBL|CRe5X#5Y&iZgK?u@Z1FuuJ>DMJq`A$s1t3(*F{zxW}SB*uKnY zQ!_>Y7iT0t-w?!;%r2PMlzd%&GL?x1ZJnDx8LJz2>J;nvtbQDXoNRG6=@{3X&wsu@ z;B$6g9*t0RyCkq$Y%ks6f$ZJiA#xTZcMb|eb<^21F!)R4L}T6Urp}mRIBV*TN8lEO z|Hz*-zSV}@w85Hu)1AE^LxakJ!VZOkKsPcoKxzCXqR%Ax?hq0VG*;?LwmZ?P{8lR} z-y=_WfT^mVpIUhZXZhE%57JQd*I^$S`53ktygxGpDl2v9rUOl6=np6u)1&A`K3o1O zFSfj-2gE>wkm3dd9)7je-`>)ghO8*F5Q*RMI0!@5Mn$eiei{ZWIdu>v01q<2inac7RF0XJi_c&h#IU<0EfAX*Klj~we%2-&sCH^QX0F&kI-v^qw=bSgJ+ zwM7d((6Nz_PeT3yx@2vhI(e8)m%vmEpBF*xFP7XvbmQKGLyOQXCvM~O``dj9BCi>@ z*Qckfv|4IR&ev+E&ybf$2vhS-;T$UzID_Tencm`G3Np|>gcs}wDCO92w%H& zhkH=A(;S~Q=XUHp?(^c7OBKq<#>%88ISesAZ|@C_F3r6SO`X`U_&T0)EO{oD*pk?< ze>=e8BZfWEa{Hkd86PCAL$eV$==_!pBV=BfOqK9^Fwx(!-_xBm6y>reBi=Wkm2*bg zyIB7SOrKx|avtR_i53BZ`ChLje-e*ZqI|fLHF7wjILng8;SY$2rty$WF5(hAUTlz= zAw0Yctc7c^EB%l}zg`8aihMjRlsT2cdMK9Sx{v2swHOH(7ZScpb|gu6>|iY1xceRJ zjSoF*L+Q<0k-(8*E5GCG20!BynG19VrP!&n$m=N}(1$LnYM}TV?q~_(w2-eNwZQMC zkO##fP!4+90yr+qOp09Wp)OD|%48B6ZI;#O^Jgqu`t9krx#m1{oTFSA4x3V7A*j!k zrJDKfoGxcrOd@vw2*!Q?1? z8vgZ-$Oha7u`9oSW6O#Ra-whA2PL7^qBVl}krZ31>BDIc2lDi;%^%^=s-kd;{v{XB zllr3!Oq4Kgn+bjHXuk(Vsjc;=1qB8Z>oeGZPnXQ+vE(d+NpMIRTtT2=Ab{F_?LbfV z_EjFnYR0Mp$?}li@q41OsAg7wKrc%&BHLpN|8|a&Lg*KHr?XOwDF^FUeh?b!rf!<2 z7-c*c@}ose`t&PU6HVZPRf8A%U6xQ?o*S)3W#oJ@C$Ll^H;NA1GslOVKg@27n`Akc z#7hA*RZz|t0J9(Kq^VGGN+mO{|I}>AltfSTeQ4tm2KO$ZQHhO+qP}nw(V86waU2lo&9#) z{a)-B=ieN2_8220W@e6-JzHY)So7R!pf30h_fWPW5Jwi8W@| z8?-7m*?Hb*yI;+Sl1C26IVdDr=6Dbx?0Null|b3RH~;mf`EJCE7QAFn=n){#>wR_a zUGI7x%4FS})phW}c#KY%#(T>(sBwCX9+-GOin`*4vG<3o$IKMbBZwhK8V%JaWG8Bw z6g$)r_n5mX`_b~{H^gImhMXEEZXox|0V71^Cb|R};ayb}4AqGZ0X)pn_1#P#FZaZs zFLq&W%A2VRwf@m=#@dKu+GbzAzio-{9v)_8!@9-~I zfumoPNxO2h@AJh6@`ne&E1T1w8$|XOltk=YL3XPLw)gtZrc!>J|DGsw6DJ%Jwgv^I z3SKce10*Mb%rO)dP3hAxVTva+Wc-O>u@oXG13>|e00s)31#o+LO@2Vhe&(OINiwlU zYP|}PM4#oWNkkEqHIXC&n0i1^6Ns@=*CGDUuom1uK?L#>L|6q!V_iw4+HbAF{)}1$ z$}i+PBlDck;_DUqV$k6#d!>q4BTC5RV{8A#PXs{=T3l9~#vR>Cz@J4L z>N@39&stXpP@?m|Qmht7kXe{&is^BFo)>LUsVeJRHe#_j7@8CMRHiH`iyntiO*2Ur zHue`G5_faOyon~;;9A^U8T~P$cVhvkIW@pa`yrWcSNp(i-iU?#D!eDFkJv7yERBm;3R8{uX!UezM0l!p`VrpKVraQz61Z&K;0)dK}d+dgk`DyR+Lm zp->TaeZEz`Ku@=-1mZ=lqgHRl%E;_>SKX>l7F$PULF7NPN-f(r9&SC4LW(wS+!t?B z!+xiD1pU&oL=)B;3MniGv}yEpg?Iv6q2m9_khDoibxB^hgnHrRo!cLyct+xQZQTqT z5s;n#c!D${SYx{j0{@T8G=SL8BmB5bJ~Jsd-mp>sL{iNvnRdsBSW*t(&y5p^e+RBh zafaieBh#7(wUt%p`4!J)#N)&nBVw-6^+);NO!ibzUy@0>5G2=FE zu5HmV=icn?eEWkTXT6+?IR|slU;D9`3)Zar>beWdlDDDtQu5_$#s~Qz(iJ!u*3Q8x zZu%8`{J0WR6^T9EIITdL^Ptj)`-%DOR8-xox+zX0`!o#vIjiQY~ z%D98~@3f)4A59+@c`^EbCulx*cE0UzH|@2%ob6j!G!IuV;0X@yFJyle4$QCchvfp} zPDP<20fOpt-pr|1Id*xW&HJBN0M~R77|evn-miJ%LApF`OoT z8>w>Wu5e_;o_OM+V%LLcsQU9vuvdskoW0 z$VrO*?jPK=b*`B&=d*^IUz#`};FW^%q=>M-BjVb6eX-8g`C@v%Pg2Ccx#Z>|;gIw! z_nKV=5$Y^z3nGB6vY@0NUqU%!Qog`Pi$l=%C{r!LuLm=ZHZK{uc?me?lOF0Klsf}1 zV{<#<**sz+^?i*(F-rHM5AfkPHwvXe&oVr2gxK_~@;8@`ZX^SU;5W2CkLXGAZolI8&tJD$H@Z+=u9%evT9_HFUC&(3vedO-)Do zELw1_|T6K>- z;v0~WuZ?Ia65=Xfoyq|zr&=JT=cqBnVCC?6u1a4+5H+ng^&yHWOv2oElSqjm)4#3`JL6%Xe@e0gWbh2NO@f+0A!(iR#=)F)@d_&`klXZu%!H$*;5V6Cno+H7HYldqQLHF&KHE} zWWy@$+E#<$PWQ83T5Yb2vs-fW9k^8Ms!-D;Zz+{DRci~;o+Kj4wU6*y*5pF62qc7^ z320@+97${-0qF0K4LZS&2+{(6t*i#NySWiA(+fWMC49G3mXb`=KH8L@2C{qqNinJY zibReP3X^y%CNKEJzR)AlBwmYvoooe6etI6&2BVBTuq${Nx`@w$1a`g@->uOH0MI0qZn}VeUPh5#DLE+_rzq zzzVg0_PYqM=x1Abdc-4&Z9mhJmWpvZ+kV)j+TmBa)_1?XTIvXp9!C9UbMtmpZr3wl zJXSd88kx^K!Ey@YM{#bM$e}8;r+pDEpzmIezkg$e?QGDw zi-}J2xVn;Mg;KJ6H#B({7|}y3Ver`>@I1;F1Iu;^1EpP#8 zJiz@Bi&d$23sd;dkRJMY2|Fc_qu8!|iE5s;kn>$Cj6L}SiU3>iIeXFupINa(<5-Z%wLE_#0W^(_ zN}fq4IYf_HU8c(#|DH4c+awvMwk<34^`H{z_se!LhWWImJPI@Bx|O5{r3*6w!R!U@ zCND-Um_F2}bdka7Hv-B#AV4!7U+ukf8-@YoZlu+Fbqy}f4x0sJWN#9Smx2Xk1FAJ0voAG04Hv_U zUXu@N7vW`~#HVvN@~*>|_zvvWnBeFbwdsECC16EpxSG}sy%YMzsRz|3T#_fC;+yj2 zD8it9N&D^VDF!u_f+SJuW$Z$eQEKFrj_Ll}kO6nVD2m|{r-G@oT&SR%dzd^D@h+!) z4K^um_vE%h@OGcQYohvu{RjJ$lL{^S7=b8Ax3qrD+*`g2lW6lG&R-M#D{ryPkjxeT z1FRNCHsEu9{;Z8Pk{6uChk6 zpnORIVfhSA3J*O^F>AdS$Wp895JFACR0@2qvU|@NK8DKdr~nRFQI-&z-^jw`WlzV_ z1Z4e-vp<|y5%sLFb&y9&Az6^;f^)boG=GZLL!MH(5c2&@X7A6~ zE>@v$>8o#^Xn`~o4UlZ+I+OMEwmroam`)Ga40)o!DcF;u4&oUct>HHicHK+v z!=wd*Ya?kMQX90%oNj|3mzJ1h-un~3R5x>`ChI)Z(JBh{sX|sxxzNf&nvp}OqCb^y zd#h3N`-ZBhpk{n69*dsQ5XSJ=ii0$GZTLy$p2=gm+cYUz8*=SDkJdY*0{83h(dXbb zvtME2*`iiWi@MzXVo%e-vBx#{@&P4&75M}=Ei|P^UXUeL2GJ@hjNTzvNcv2 zDVjTfo|pEqw_99(8zBUkVaT5O(Na778)#JN;lG`{rKkHh+nbS&{XZH5^#90F|9?m^ z^#2fL{wpfMMgCYQ?8NCH{IY(3Cci%A5Xj>bDkiuYL-PPz zl@rK#c;%JUyU+KY7i$ACkh~rueVGR@A1}B*o@N(yeKEc~6>1wZlW@*;=(hFuA1hWi_rXZaN&i*GPv!Qy`JOBD z{Jc1;ZQKcOrA+=VhwdTJ4%F$vPrj5rL=hqZ43nH$B#y8nm(3AHfGpym&e*ziv=qfL z?!qr~f1bC_HmI$0%(qmOzbfSS=p+esboTsMtj2IR6bjtBR^%kvWLH6rpZk)aG62zTY!YXA^I?WBKz=K^ok$Z}nVW!LSdKB#XgOvD3d^$^fPv5M0wXFcj(B z+Sj0jdxGjo`skP>5DFg)S{6;*Z*Kg7j7xDwc$}zRvB7fNjT8cj+tktag*cy7V)a*> z#TC>DC!gWD8sB^vQfv@X9!?5`O$HQslvr3-t^P#@RK569TL*?)s{bq1g9|5<6q_1D z&y%1bC8|2qAp!TJ5Hgr%@Ad+ZJoO z&TiKG?+*s!R>MpZ$CV}VJE3=SXc>zRovHLaK3R2xo8%KT2Ov=o8TC;N^{&)Jiz7a} zE|z|U<&?ph7J?FlonQ>miG*tH>CM$fi@B!IuU{3sWBS}L=Gm(3$Sn-%l(z8m0H2Jd z_u81@^EwI#=v{tplA#3M5lV>w^-$jXr)|Vp!P*MXLBRGZ4|F2E=!R5HD2{=)`QE_w zsxLQD3z68YAkN#sT#q2aUZX)~76~v>Y1)}Gh;X$)PB47Yk6bpt>G968diR75Hz6g- zt-snL`Kl%{+>*k=>!;G(I9lR$u9Fxmv{h^ZtQm-4uhk4GCXn7kgQC5eV_T|^u1x!vT(~+^(_3OsV1O(FLElee9k|gEkOouCKW{*Zq!g%hRph zIIN_QWmZSzq{y3Cy);!ad&k!lT<}@eXogoi^ z;rqijF=Q%0wTt(bKSY?VuQeDo!{r?8R-ZRe$q){PHrJOMQc4KG=@!~?-b&5q>JoU( zt$4ivtSr69eweg62Mi@sE~Y&{m5CIp*jWPtPSr%zc0PZs>P zXLXRfU4XHCv46s-JPpiLrroI7@@>s=CO znL}OT5(R?JK}7R=Izehct}_mYH$1Qt+;rc8h=~e#pDx!?O7XKFe~#tbNo)Gf?0*BG z?XgS1VdM~jM-u?sx?{Lec>fSMB+1R7>0QaZg<-ZE+>;krBi$R5jI?wdm4ZUVsnE@) z;ueq6tB%a^#1YKnh}L!!b}gQR7@K&hM4D#ppn-V`;y3BEe?3Sp|a9~K@feLY)H#FQTE<=8_y`kvs5p??f zZvdDy@R0D*_Z0o?qNSh6DhiQ_6s+;_1}8AMF7?-@1A`FyW%QN#`n0I)(P160j()xL zAA}79hwjyq0?wnEVfC1AnzIsV+iGu7V39u%KnaV6)Eee=H0#!72X6)iqEQR5@Q>;3 zvL25IWGg~HYiI-?Na}!8FYGU*k=pK2YldhN8|_n&*v+&AZ#$W?Cu1;8T5WE&sYCU9 zk*1g{IlbQK@Ey*?Diri#D2ne)ZZra8vsYH`uVO^%*q&1-b?;6QOzPAxfclx7-}|dC zn%j1IX&0BI2^<_fb!bESKF#%A6KSc-V=1cP6JwVd8{3S768c1tA#;>;m4W=hDPgf> zBUl`XOgvubK}vqKF|RzFT928K6Aq;^=a1e90AvvZ`3M=rC>J zVDKCzA(2#Wh6dLg0he%p)hY`vQ#EvB9(yz3;`E@?irtiLkIa@5z zAv}dYjbgf5KGr=oWQqZgC06DD z0=!lKC!i*&Jn#)>ak73XDj-Zo7F&b@=B=g0l7&oTJNc@a;ggCqo{)z@7-YmCkQ;z} zZEd;P;1jfGm%!}O%9C~r9#w(!apu&Z9i^e3M(A4IJ{r~MeS_ON@JPBL2aQ(mS6$jS zEn#Kc9Zh9)*YyblbG~;wxM6{x9M_QdnPdr|(!>QL8TyCmCov$7niTA0vL6i=qfkd4 zEc8dK>^{2DQ++u3zl0Jh)u0E9luO^H%mSCs%Ih=u92wUMb!Ri1MvU=Vs$;kp@Qj+U zk9Vs}__HxN08)J_5I&4){tD2NAWAvSZD@y&?4 zC5n{2N@DeJnP|Btrc|j1To9i$T8>^u7<%oAf%UNxU}o`=#WyquGV69F3#uL{YGmE z^AFPL{iLYn8BuEU)!s(IYqHrM{P23sCk`;zg>9`zSqM7N;_%Q ze6!VWf5tDGY~XZ4aLlrB7--{i*IXTKFrsNLvi_7Rv}x-!8PxP186GWle{#O8k8R68 zG;!A|OLqAU(0-sT_1~H*|5LI2Q^hdSv;SXRA$t0M%MJZUQ-%H?Ce;5{+;nS9$5OUJ z_rBMrIr=#Zdq;!OL9O9%^4VIWp5l*@TO%OdAdAtwJ_|||2#GUZc-toG)jy3P@CqyN zPN;~c`EY(Pl#A~rYHrEP_7BiY^?p6y-q@CW<1W#!skfK=e*Y~U?83S_L}jBvJOw@L z$ALzW;q`HxqrlS6VS2$jG$L}E$*aR2U}A?85x#>B*42geZ_4h@(fMZlyz+b@s1>I9dJ1zOD`RH8P1y-*ZT6o`lvlotrR|Yg`$dtY}W2 zyE7(^CNUK4d8S=Te9cD?<(+onr)9>{7olkcwYz1ty{r|h6eBo4=4JRFW^;@L?g=(2#TnEe8=)R3$?5Kvq2I0pFQ zj;IoU9U}(F>h=Bfbg;9h9btP@argSn0>WtoOWa!w3S@`jf+5?IW_0MHD-jsuB1oqR z%!OrFmWrRBlab1N;Yg#OJRCzJXSxrrKTIDM@>V4Atzo}eN5DuEAQz26#W~+17)LP| zCu6|aD4$6;2**k%D4wB){{TpbtgC*8LHqWgB@9Ew~E{4 z(ZSjXYgwp;cf(04pQ)i)%3)$`984J+0WOMN>Kr~!Hw*T%WhiIGrQZA<UEI%6OK zQmEb#e%=5f=H4wURskycS05vP=f^!c%L6I!$;a|jNB-N{shXt zyZsG6#yqzqaAK2jNoL%I&Yjp@RH>WoG%)2yFCL}PkKGk+;<@90qIEX(67vlJ)$z-j zOt+qe`6xJUf?e4PTU~#2e#UKVpRy5K%UP$*r_xzmP}KekTDDnKL&u7C& zTEaF_4=kzBMfEi3D18jHCj8p|`k=v64Oo{Kf!=?nWZZz;m-@jJ^HbjoX8tn4$?zAk zJ!82#e1mPD{+S5ru}8X9-9Y9-iBl$hp~*zWO+N(0!#n53MYohd-pMkn$&tv~KI9^m zh}gLTV?&?W9!{`nDhpv-w|;g}W>HcTwG=HSXqo|OLfhF>0-qSp?W8||$8LCdu#;wHq$PhrarhV+ge5o*((4b&IsKft2U7Gj zP{%F}04b6T4&CrAG2D?D7N3&y$IQ&u6Q)=-2}DZ7_Sgx3C;~+NBuhOBV4yjRc|iQ^ z4qdl$Cn(}8f0L;S^Rv&jf{h)FB9b^-nVhV3!<8{r>xnJ;g|SF1$UD(9;tztY#1H#Z z4*i>me#}@GOt+1}qyI6Zg@`eQ%Q!Ug$&s^TDvimTBe^SX3*&2k($kYC$e}jGgqf%{ z#J9NiAAghS&TfrU(CqL6J?e*c3J{GZ`i9@h;G%-I4)BnoZ(Z_VX2{ z<1FfacLqU4tdqvsT9zXe!&g?!6)fCsGj?WUNTzT|ER?}AGWdHfHIFmO$o0K@ZpxjA zO*TS^_$hJv7_#Ixh_XpB#Xzq zB((R%sbsFS;*J%iwK>)1SYd|&{g*@Qk>!SAV1PkcY0MKo>TFQ29%8ZvIsfk8cps%$po5~osx5G8Hass5Mnw8bqAK!8H zdB|e20+pIgA$0x_o5~20VsPzxS!I9>4@+2#sGi}5rlNwPP?*i#_Cppa&+Y2*fB=<^ zk~yf?MU^al*=n4L15SsWKvGew6mCbS$j#qzx`X0(b!-j26FW60na{S3!{8Xoyy=>& z_?ow$pt|ejCcbrxC27>ODb{sLdO=j@U~{4n z3pZ>4IJa^eI&rZz9L2S+(p^;B?JE8Ew`8g0u`^hVzj}o9Bs{NH^{QNmLu*LRD~NYL zRwx2vz?J}}i!PuPx*Y{FNitThI^l2zUC^FkrbC#w*&>cqIuA8yN(xIUlHwXP+{YqY z47DJN8RE=1HMcxF4f@lh-3Z^zYUCX!LGMW6WwYz*El3wOq9aII2zaG2ABX8VEg*Y( z0h^v}qH`o;)_;3ZL|O7U;xD)N^#bI{*1jCR@0ZjIG>z{3j_Se_3ahI><)p}E?U4&v zp38*!Ah5~8FJcWlV$Oc-OEq-$5(h!i%DQ(F8HttK>2phf`2eLPPrlH;#GFR=akU>H zyP95qvllUxS!iUGK#Q~d+s&>2K+Y)2Ognx$6oz85wv*T8AU$hGg^l85!w;XMQaZW> zR)@1s5e38A#3$~m2Bu5?r#W1IVcA5oK^euwf#detNAF)rnxptsL&nj~e;U}w1G zN;2t{u-Y(h!W~Js`d73y3#uiSSMd3zy(bll%NniJA@+Ff&~$gWy%-akL@-o1QCwoY<9Z`hx6JZIWZavr1*3cP{! ziv&C2BM(<<`m$cSXMrcmfQP%AO0|%k(+}KoZN;XV-yKwun|KWkr)Vk&IY2ab?rKUTSiB{0^l3 zLX#*w!cQ7`mB$bWUrZ#%5zitqEHdE%&@U#9yk`!;5$^40R}(0Fxf%248Kuz*NFjjn z23xsD%iwFpr__kF8AVA7zcj`d z_5ur2=+FdoyP8kuk?W1hOnfau(S&L~@708{DG%Navy%$|yTjjFkh^P3%oD0yeQJ?b zi=bS1KExRANCx|-4|{hTq$rRvEh>zdC+9*?FU9YJ5=?`WQ35-wEN-FG{v+yPt$3z9(f@kfSMMp!yt1J%0L zx>-BpEdd+IXM$&x-q0P|kfE3MUk5wk-(+7GLx|zJfmdjHKD-sc;X0rycK{qQI{xcB zLbipxaPR)FCkN0k3~I%=y0NyyyaT+$y(74NH1MA@ddG~+br!ITzZ^O;USS+5tG*Dt za*ehPwoSeUyaw*Az&JzyOl*&itkFLay_3!iy|xR~1Zj?2?;_--EBMg&hVTyk+-@=# znIf1aU-hNYt=X;me23(oMaB!Z2lpGz1;A5JXqUTBiP16*4$-8>_=$m) zL3H;W?E&)G`Dgkil-;$Sx0N5Yerf~en&g_znqbnOaUIBn?qKgU?{M#6@7Pa9Pq3!r zT@!oYE#xijEx|4L>(!58FI2CcS=5;ZJ zBmwd?xkoWUZuH))Dnj#I*?%v%9JHdwKKOO8b10NOhFxktDo z&x8?5DVbBhn z!rS@P>~G8&e{VtVLp*Xm`h0k1<-DW3l+rtKnmx)t!hGSsaULV$2DlpbV+Qw4ZUH>` zU=!%nqR*_;u*K5c;$;s=o`Sc_4>{-qx`J{ZAKE)caL^x_S!yh_2#qqeX@b>c7+8=Sg^Ol(AMiPjD&HZD-=x7fY+; zvH;tt1-;OTI`gn-$1)R;+lKTB((R9a>4)7s@J5kVigVZnUxy0 zx#@SpAg?AHl5X>>))~m{4f_pIjFbO$;#aR+;Od}OZoX|Ei<&-+MPI1A_CA?g1#(%) zCu^K52}^!38}K*4sm5*)h8#!5?swSF^ful2v`L}0(!CgQzHhoMo$^zHF61`40UnY# z=vpD;yJa%nl%onzROOjmRr=O0Dz?t1qPDU+Jl?p}%*3nNiAQ5M;@PUlCZ*QNv%1YH za+Jn#v$=~VjAjWWw-{r(iQ2!*&0_Svl2H%uDOBWm*1F&v0*o9Q1_(~e*|CU;8e|zx zJS9+RSGog9iD~ypBrM{|Wes?`6@SI+Q7yd|(6;L;;CC@Cy`&W`p@LHtUjTyt0(f@w z;VpA;S`?w##9J6TZ*qiKXj=e|M;NfDgm;o*UAvwNyz#5JMNdbW862u?R&FPT47$_f1$GWtculPIBF{zLh1gJlrSPS zQXT&c_HHgJJ#ru~d7^%FhSJ^!s<4M0o;%0hhHurxk7E)M+~g!72>}gIHc%|2S!WfV?yP_cklBD|5SP28fP*H;uQq=td zxzU2z(r_Eq3#nY8O=P`ZQWLPGGri(u!eOKM!QU^z40?IyKr<@)6uEhe45>W^7Fs(W z&oB3@hqguN`aaqd)ALmuG>nBTR+V2cW@cqGq|4CFP{AVzovg1!iMD20?qTj`Wvwm7 z3i*cNhVP1o&a6!RBpkP0K;XZ_n%rMPFja&gW9isv!VIO7vjW^`W(cCkhUp#3L3n2B zWc7I!1oh5@X8g_;C&DL?OJXTgWt9pj{G?BI7^B<%e-uqJh4+=Yoz zY?2{$p}4Y{ndM673gKFDJr`ZBYv?oiE%TXi&Hcr_H|wz|SfNY*O~O_BmL7vEcZFD^ zTz^bY!cj2Kk(W@NWmXOMhq}DGT&ki!QtNueqQkM_raG44^&(q&aZX26q}pt zo~xGY>&^5i%sM(G-@U@`2wqp~{kdltY>b}gIeY`sRz9l@koV@ozy?Jr`ywkj=dw0> zm|x2LiqRD#FFoO?;G1VA^LeOF8q^{fVDDFADGY(J_*s79TQYQD>pAi0T)S8<4Ba!0 zGRK6ZE*4Eb+4@i?F{ghy0`x%=^s#v=2RTv;D-y0fEPtjWw4X(Vr@1jQ@f2e(El^4) zD<>-+s47!MgYDnw`+QSQyVgF^HR3?eTDQ3n zUGIH75JDJga+3e zkXE8F7tFAS)LRL~=!;JEx9~X+^mB~Ru^J4j#7&Sj#4}v^3y0B7S30xVivWj$W{p!8{MgRLlQp2lSu7P)IUFLWQ@mfopL z%1rGPW0M-T{Armd4x%xr*8jh)(wJ@|TTDVDoa@qiG#wO$SV$vn5L2up8jX zTCmAkFv+?CrjJj1BMC2>RddQ~g`Mi&KTcu3Vh=@c;-6`n#SQz!QwI4mjSegLpYnTU z@1WDO&b5}?bKW#QC99?Ai&hR69&;p^Q~ZTjZCAvYmV`M2Y?To!V8528q8nz+>X^_p zprZQC`|0eKk}-xFsh28?B*kwM;xns&LB}`JMJ(6N=)gk9kBos@ST(4G;Imfti6X|9 zBE}*kt+tcFtn5&yH`ppoSNhers0vkyFQWOADY+B4toQ&f})J5Ei zGIrxlY9RVl6_-JxVhJJX0MSqepz!7~ZpFA6!Xe)9o&6B-(CU=nN<^`WBBCppANe$MA@O)tC5Ee)*o42{yD|A$@dZbvZ#8k8^Bq+)@&xv%T7702T4*F%vGZKm7|B^RbdsnzSp_vQ2ICKa`S$v zpm}COEoUhfQ|Gql;$7?8JS47Cw-olmc5z$Zoy<^+ROS6}3gi1Yisf5pQs*Kk%93IE z=u0DCrc`#vr;Xiq^7CMqT2CB(%FD8ujLux;(i{&n2W4WDY_tbM zqIGI!%aIa5%jsQKR7Vu&-&PZ@+&hbq-RZeA_8IAk%B-!g@$_HKl(DTZwwSk>Ag-ooOs-sjJd^vPnDI zw{Hv3$UA@=xK?F~b%$57pchBFe{VdmQPaQ?5SIE1=ELe6al-8dxigu2)c%L-#WO^h zBE%a}O9ATM>Ivj`5|L7rj6kH|AFc6zaoSofYvtreG(`NFkj+c5tU=s&Pwd-z)PX?< zHg^&yv6=S3Q43(5!PW<0E=?nICMPrx>MRlKC3mN(>5ib7@`dUp`VM*%xfx{Pwsduo zxkJ2WaqX&WWk-u=nn`}h#`%WH`?RB?;KsB#+(tY6NsW;8=um&h)OHK3()>eZt^vffjG&r0Z9ilNNKj`(^*3ON!4h zBWW{wJHj4z>WfP*69>*i557TZJgMDM;l?(?7Ulrk`xc0p%rmGit}e2r&0Qm0%Xw8R zJx#B2hNlbzJKS6HEYUR!HBBn*{}ihvO1cKEcARBbB14P7rH{m0&b`yGr_6_KvCa-O zO6k}=Mkg~4R7@P1l2CjHCA_CckmGeMhz5m`0Np8fGA58PvATB~@2WE5H8u6c;B|my zW=7Pu$$dng4`Swo`NI3U99mpv)FCZv!FPOZOj~>2bQk?L4P<53Tdj^d)4CZ%b>+Pc zB)#SYhuQ{z*B*SuAL&qa@fz$}_u`zb08Nz}SIW3Z_jvG(G9$udaMa*jSgTTr))1?I zvt|)`GOAk?n~XuUJN*_fDprvWR-^Dz306J{RbxD;JTj$_)(x-ANW^>=eA$%-LvlQZ zi_@mRIU7p}r5&M}{n& zUm;?cSUkS1AEid{M48R0^#fl{eN5icA6QFPvu_}8Dz|0bcb?kg7+8&KtkaFtyJyym zB2S0hQye_o6R{6}MSH$-k9gMzri5M|q!hX|`-=dS^DFS$)Pq4c{w*41rWCU*WG~|l zVGoloY#6Q*X2>Hzzn!}TMmsP>Yal{Pz#<^&-bIXj1eqfR4py4C30zQcywEnNBzGqb}g4<7}TrLM$WnzC`0 z@)qz`_>9kZF4J7)DgN2iyq=F3T<1)93U~rrsOh>C77K*LslVW(YUO~tRd|Pv|=Qy5v%c5zx;Z0yx+lq6xjjdJ86A+bA%ix3vBUxczAn&uW?Ue@nKENKd<^F(!_41 z*u?=b+@+IncoKzyCwE;cRsxQ|HuuJ*eo~AB&90N zIM+RnuhkK4tm*ylf~4b!Cp4ohjIZv9Jv0lJs7)*pD1dD8tzXR3?)utu&P03a8&i?4f&T zdKkZ)ytsN0Q$Nmb_jr3ghY(BCRAv9Hb0Van-o)%+f7^8ZTDXhp9YzcvYM<@J%P~^l zm)+9khk0cq(Pgs1k>t&IX!Meig13%A3Y|s=f7r_^tJB7vc2|7vkXmLvjr^8ZtD{(3JFa%vsfGJ=HK!Y2FUy;Bmv@Z+nIDDJEK-fi0FoUW_Mn+Jz$Fe!2=ikf;;&`6Br9|bye_Lf~>yVsH1-)*#C ziHU!ZNnjj7-7QW;fB$kBBag`|)R}tfgydcyEJ+KeUp|PP5*Ktq?yBk{E!SOiLmPL@ zOhKds3k7w~iw&}5(%{UldIu^M_Y)z6#z-*s<5E~e_2ZJE_=_v5AV%Z@LGgbmd&ekC z!e(8t&}Ca)wr$(oW!tvdW!tv9Y;@VS*=5^P{hc%So;7pl$LzH--gqM;GIy>WnQLc8 z#`BnefZU>)XD3CcKzA9J&&(K-`1pm;V7%U=VUtr*q6MVu$x2R_iP(?s3v?3(f`aZ& z!R#JG5nb7(7>xSZUrn}XJ3H9F^J_g`z8;5S-r6yKij5_!={7Y|_|e65vDgcP;KuqP z`r-CuyN*9_tAeGLXhI*=Zr|*09j$%6N`EXZ)kj6>t15ae&%e2S+QK}0{KhB$S;8Oi zu`-*c%@x}3k!74PA-iEqUb`2=jg`4Mp-$`CKsXpC6M`Bg6QxqkWO*ZIBQ$P4YEp&t zdo``oS<)#5>Mt)0pntMTak@sSTA8&>T_vV`q;qy6O|q|+z33V1OpL{v-@xpgq*UcB z&^6WQjRQpzTs@$AH0x8Fx#*$B*j-c`9u-+tEs2VgS7dgQvZ4GSDR0a)6V9T0`P^ws z%VD|q(f6wW3?sSg-SNH4i_@<^$7=X|-qg%Vyba1EGBwplAQZqe2epnWm>Q}knm%fnp@IvNCf{#BwJUQDNPY`Ty4%$6I6^Z zzQ=lTit47)agpaxu_Q?4$or?DM_%rqm_1qNF;_8ZQ ze7{wt(}|j_p6&&`94xZtZWO{@xfvI>9R3(bLC{G=b^(Ec1}ImZIJkx-21A152RS)wqXi;Qd61(!VhF4nQUw4(%1RQ9ZJP^-Q;0xAm*ixiP6X*KPJXZz!ijUH(SH=xP*7A>+ z*61eEtAnfI)^zL1cf(_ngH*N>k_J29H+6z<&Q{Fj<|CjjJTGKV-z+%Sg3`WH4E2zxHqI>@H}5U$`~9bD#V=wi2P z(Pd@UlLXiOScUcxm11ZL(W%hOP)dAlQGW0=_)z|CWqNK=Mw3{T1rWp=3)SmXvSPu( z*N$z%XvA_Z>odH{l4U?!c+J{6>J_Px+W1*99kXZWc#q1E#!m8ZhrE_nBYx(qz}0ez~$SkNgt%Ci=8^ zpKNzQxoX%ijP-WA&wrC-W5GSC$xm<2lHSuEsZ8JWj^h85d%!PMvfS}(aJ)lUWxUxF>oNPKDXECJaK)Zl|zN^$`!r)`aWi)%@CH;8pmTHe2f*DUc z)R4Yf6@|eVxk6@?*hFdNbNMO&m_7PC4VbulXt=m@GCdNO(65~?ALXyiv^Tc}=_y=1 zY4YImY5sHh8|~P|zMPY}?iL>E9oJ{kqNMg!tf}2NJ2BNe`ea;ciS*CGnM@OHGFNRh z&urCjnk-dNi9dz96-qW8A-&pJ=mbJzg=^c!Wu~+})UmtFQr>F1Ze*o9)%=J0sLLo8 z2XFaacm5ReaTC+eLr`(qd0g3pZJ|I+i=i+*ZxxoQCcW9%|8^yMZ$`+rH;#c{d{m!K*~};jh|o{Q3RB3>4ri^2IurYo?`J$ljAnvV{z|eWlng{@zuJh5CE>B_`~god+TRqNemI}BY*kw?*CpJl z+MkhTmwwjk1>-46aCgJ!TP^tO+~|z!WWN2pIChj9%9}I!%LHT&e%@%lackXh7pbZ1 zqCFIxf5XI8-fbaFIq_DjJQJsiJ{?tuSARv5S+`5t45;!O?J-_;r*GJ;^m@yIC*NB;h)}8bfx+I`Iwg5c}edRa7$=#=ejyNEU3yt7e zsT;s4RG2SPcASRUnluPltWd_zUJ9{^3U;a7%z#zS^AW5J0ywBnNu~y34_H-?6i*6y ztPy0CZbmL>7VUmlZ!!#i1O09fH2$h*rX??1^tvDP)R;cOg>R7r6l+K(W*cgZGooqM zL}-oHZ7`N#?he?lmQf3<2kK`H(rw=LPx(iN(d=mZmSDxmHCTz-wEmTEl$8zC{+R*| zYZeklJtL#rGf#XtxP=PQ_6+F6ophA&5k&qpqZNJ5Uun~8LFDs0I?RhwxC{tHh}67X-(_`M8LB++|#X-UGX3@K=8dIB7ny-?Lu=KAmm= z58Eucr_j%J1Jw0gF1m4d{ldH{?vc$bMa7vvg^NgMQkm|;`s7DEff|{3auyvsSyI&b zsLU4>^k`D+CCDwnQWgopow!n#%|)A}6vBz7oqj6T$bo8#We^D`2Qd1JsVrJr3m}J8 z!C2&vgbjueBfFmD;tyXMdtbQH=Gt?UlWo*MnJEQPly^`a-m;S>dhLB3qom`a-4C)# zbqAK%6%aZL9DF<=Rae#ee|Ym1)cRj|`zq_@C%Y6KorlHT$4L>SC#E#Y$###GtRl84 zhdUH~my^cpTPgAS|6=6y4)yo+H9r&I^zb?SfHj6<78}E`V~mxl0$pyzAbQeDb}QT_ z&E@c+OEsq+17MUgp(DeQ7zIp=IbHm{#`|0uSk>CU8vH`%$MHK$DDg_~giw9N^FBEC*(TLB z#&?g8{_RHl`oUp{Vi$Wlsj?7L{{`FZ2PThmM zS-*C<(HrJ(3*mRY1a#7VR)>#YufOfZ2BwV2$NYpC4U-6x9*$3Q7gkMG^Y;r5l`sm+ zs?Q_n2i3G2f!ngi3=*ds+rNGODI0G~$MK75^YGY_Y@+!Nw_-AbzhTG(#8laGqEB*P zgf(Pcv5|-b+A=25{m4#_n^nM%IQahhT_*dJ z6+JX2uS|#bQ9k?jrOx@V4s-yIae1JZ|M^q?Q=L}AM$j8fljS3*S?~4F_w~g2hCUDR zNxTtzjCP`br2nDNl44K>tUW+h+$-OYHVG%+4)@*%;@S>_N&%$@k2Z)61e-jH7Hxi3 zu6JGD9uh8Udq|n&GL(^ar!wD;zW2>P$9qzce&6Joqai_O_=DSSxAt%vs;fY7ql~H2 z7QkQQs|_fDJ9gh>ZFl(40k=M8B1#-|?#;6G)mR;+Vd1t2zGlfMQifw+bp()$kQccT zyMYn$FN*l}rtoPxlZBojq`p+p$K$+dK~)EORs}-{;G){d4Y&Nc4}h7Q!N8X(#@>MRhTLNR!+2Buo5$z0ouO=hfH(yOVQ za67VKcRnHak_~dU3a45Kf}@a643ecEBeA*DDjdLh;L0OlfcMH8v;X~FJN7Y9(ew1| zI}3Y%)^ltX|CYGldzJ|h#EAJ3tTI#UR6@ZJnk9!2yeQH$

7;(LRQX`PV^(lHqu{ zNj<$OoTmk=B(yr?Lnc!%H+KlqP!2|~W zz9EY0s`zdSM)s^lnI&aS@PrNqjx*8~+IR9RccUK~_WgiQ&;ViZD;NajU5z43^4r7b zpU20qJ`WK-4^J@gmjlul8iE`GdbNNyNp;afelXB4j|E?4XFZQf)U5I228mT=Ed0SQR{>UQRs6ciWJcY)k7Afcf%a`m03Nf|3#@RNaAd5GjQN6KQ8VvkxKhb1<*s7R7E9gWTqvImp z?Eqc6DqtonrdO;HcJ5LY#TiBFbLlCFMcgz>upLhNB#vl$qQZs+tlk5xWG-!$gb9Pc zfZH=f)Y3*5i~fdNOL)aiZ8G*_L)s!_Q@1@D`zfs{iV{^v6hHcb(L+0hlq(Y{kRREq zc!HM|g^S9rJ0tM6`6w4;C*{~n-LDoa$kxnAowQI^iX~-}O%taZXhL83OPWeQ%8j!Y zNVJHrO8(kIm|1F6eJf9PRl20JMoH{$^0TBju8N#u=Y_bz&_cBpj(FE9UrtN{a8Wj} zwLhA!WfqZ>(-a*44$A4!9TfB2k(9e<6I~m{D(UhGJEgL~MNT%Ttp9>s)FdA~DK?|G zh~7+MYD-LC!cDKIalCDz&NwO9KwRG_$*L~eKFQiEjBGBcW`8ZlDgTmwvr<`F5i!He zOR(jhX6rYnzL%FuS#dr4CJ`>TkzN3A2CO~;IwIv>5{s6gu14?hR650OSstiW*)Dh! z3w4x<37x@;cLR^&6-JQPMR zfG1$nXZ5}NxdjVaGD>b-N2#d#m+HYh`Qj#t>U;dqh3Yy0FAF*vZ0tezT{8}R$XO-UV{>koz;${sZQd1EYb9IJmy-E=+O)6%pp}Tl#4cNpX2-*+2TLBj?*6-xVsA? ze8qZ<&x+zS9eI?lu5pj z%d6BH;?L(idE>mAac4Bg7CT?*Ep8Z}i(Y2^M$RX7ON@HbELZL5DGKVQANBJ5>x*3t zKU2o;lyxMexS{rtmbBzNuqJi%e#)7%pnfb}Z~KMs zz)gEtTVwy};2Ix1hG$b>QyfXNp&M);MmqXAM4GJ+bU5P{^uT@SxV zoi@mz2|_FgMkn5ok66K32I~l{G;;pc&~fSK$1E3v<|F9|c-mdV?b>}*uu}iNs5KMQ z&|aNfh6|-OmX+4Fl+vR{e*jy@)MxNj~21=W`h=$>S?aQIg_HH4T4=$ z&?Z%Ia~ChhPvXw{TDY|GZ?w>EsBU6FxzL@TSZ8KA;a$W_EO$QacKb59SnO^nQJQ%a zHPx2(l=5b9M>C-WJ)+OFEh#<6H8#^PpkG2i*u;CTeNi4M>A5=<@nc`Yv`EI=QbL!V zElI%TmmQ=o(WR<3w_r%+U$Rkdtm5OH12y)Qr|VDiX1)n^;c}b01{Y^^&*&FQ=Z8Qe&W0NJ0-?!W6vl+M`Yzz6jvXb!U5452>A42U8W3J0>UuUQm9anaJh0(}>{g3L@bV zeAwZsA1~pyF5s7B+r-~q$jk#k9?*OnrP}STkzgM@K3fHu5Apw60#>Ie?`@VacS69z zKL<*e#@(ILjgyFNty zXIF-y4w}TK&xn0Oj)HQ()tys=$E9T;&a%c`TV`%=|NFNz<4WIBPEA3Kxtf`xTGG2u zWkJnnG-3UX(A*0>GHeKUKD&&hYL#|JqwPt-X_K^CO`PhIBnG{Xa)+U8wQ`rci_OLD z8k*Guc9*FQM3{?`rO1E?c`wB|ffT7{0I?XmFl_p{$2`1+_%zV>Gs-1!$C4uoixmBz z3u1yj_V4Y$yp3)2Kgm+mDOiCu#33j0%uqPR1h9Bxu?(|=2D19{S{dS<^;%h>J#8U4 z1r-KORAP=H4*0EV=l{pbKLebEC zKWt-4Y*)}+mbGymKA_W=k2nIkgO~j1+FgR=$gaKA2BCBXSP?hoH#bApLH!=%J59cm zbi+@k<=xaXtYDr)Bg6?5rP$J!-%j}~idXU~%H|E)QGj4`5qsqx;bKrz+R)K2>*X)2 za0^Xh(G?Xsq8B-=YxMDS8t}5gg?ySf>Ah+Sd23I`y{GgB9DK&)1Wd7U2(J_E{weBs z9eaerGiQRu%d=Ss-Q}!_64H!G8a#5j!J2z@Cb2k z@i#I_yn@+ZtCTzO0&}107;fx>TfnIY&y&i` zP|w`Eq{#C#AJ&Tlp~XLQns)E6y%T48M_A<%m8qegz4ry!_OZ;{n%fXAiC=Z7B6-t z!&@^L`QL6a8CAu%m|n|ejK#_|Y;`U9;1sYCb>p*86tJfTc7h#w4F09Xji$?@W>MtD7*pxNbuOpfETeII1 zB6OgfYACb_Wy3#^HN(CC*ahIRW=Og9d#d>B4zBe=d8}A2#YpAX7;I7`7r;efCY-@W z9ULU9TCIpyB^F?6@VXNtR2mM(E5+WEP=l5-d3}^=N@v-F_@2sJLPg+ z3Y&D=lYHbxy;wI!SGW5UX317A_7F0MvS^v|&L$Am1x4bEqI1z(=4VS;@z}swh6xQS zv+jxM5TA3t)(A;qW=8^D7$4$S2Amb(RQ?j7;@-ztZ)X$itn})Y6v2d`JDL z1kUs*A0nkbu12{W0y#OMD@Tkb#*6q=vWGgQeXG+waBzQw8RQt}1Vs+@cMe7&(WSAc zJ1B!a;LO+tdHy_IRlvu&`P^V0ii#Ej>)d88Xi1!8nvVVvf!do`3>2db_IpLTvLVeZ z-An;n<_4#XIo~&gGk__N555WI6h#L0S57gMV{6o1dFla3iXQ$2ObeGjCYWZpr^57@3zOhET_}=n7O}R818^ja3!DAD_AFd zT9N!F% zYWV&_6q^18VE;t0QX`NkjKV)rOz__PZ!ian0MGj;q-KFo;RODPqyk_HZ2y7)r4a!_ z&|p|prsB8&A#i9c3S+7No>PEF80P&GQi`ysg986VQUNAKL_k5Bl7s*uaPSXQ2)81rh-eaDb+eQ~d7{c#u-+-~4+t zy=L&Le=QYoM3#^P8I_d!?~#C0Qu5!NfV141S1S0A|LXa#dYCa=ieHaEW^#bmPz1aJ z=W_D?vH}Iq4EYm*qcY}&DMnx_4E#rzLIVA#Qw%aFz=Hl0fCz&;Oz?lA1hs#`9wHAh z;;T*ZGghTvI~YgBzKyBFfv(yuAHkvvrAzRPde%VG4~AFb%&wr)!Uv41^nu`Z9{X>h z3#x~*`vAbUsyPTo9m<3_36BCIp9lPq12tZVUeE(VDv&afnWL~(iHCxZ3=iXFcmP0T zgsKV+5j^~BALJ{|UP`~{R9Q^^gQ6APnc3$u@JL)?2RXnDEx00$pbF;)|2MY1AQ?sG zv;_X)ZtCsz4bQteUNTJjRLBN#1+|~g0A=T+QjpGtp}X2 zL;RAe@NjQ#78;m$J`D#dxufNd7T$T2E< zC7N^fkNya0Fl0pH%vzK+pjJ#ANOdDkdzUeZ@u2EH>@$%GzKp79~R?uw;j+-uXsHo}Vs@O1DF=9vGnknb@|CqK}*u$T(` zM4B$nDTT}|gJNfj{rSMs`%BsYLxVVHP{Sx`kZT$~_I&hFSwbG^rJ!xpV;m-j2w9+b zbn#BG+e5=>U}!lx)}SGaJO>RSEHw;f@TEY00?{n|q(H?)FITsbfTz(`e8 zpU~5VPSB%mfhJRg%yD}_VQn~)nBU_Sj#*_m!-OhraF+hc3gh5&E<#(NGKifSChNmOSN43BoqX$YUgY3j$kQOk0zMHFw=7AiHV z08v*tpDWtSHq#ImabayWmKki3rRC2fPb{Mg%em=?B^KZ{iwN)~Yao^+St$iavb#d_4FddeiGcJ#J!-Jdx*wV9QOQWsu@SY-=UlA{-os+;UA ztB1y5WK<=|tVpa>RP^*)Gho%?n<_>%7mcf_+9GOHa0p$uXWZ3;(b;I*Su|pqG~rh4 zr7K!2@;4Vc{4wvJy=$G!P8s2Mg(YyzBTl|MmOLqp%ktXuGGM=9@P)x|dnPD^r?T(5 z%7?EBO-6ofnr`=%_ho4F7=Q*waQyckzdc*(W_%O0^Ff;z|K5|xu|BX_r>|t+0 zFJ@=!EMnqh@(Xb%Z5 z`d1t>G6}j*nFYW9ig1K-l*ve7Pp#RIO);Io@P6A;{IJ1hjwUb7Ze{ktci{m+QzNak zBblJa9>*DxTnNtbhWd59j#==yKGj86-`a&g*#xn>t;?gXa^>fOb8skucOCS3o}J;H zqTa@NL0>!&yXyq?0T@= z265j5&%n+j-!Z&axmDt2N-bupyx?^dgTp2ew9D^{9|D0G#(Dcf7Dz+B?Qt#XJFH8R)iTi((3jPlqE!Kb7TK^{;t^Wn%z|6$V#_>N> z`NGG;TY2%}S!avwEpwEFInib-&P0d|C%`)F09aC;h~!*Eaug99c{B$oP}N>^R!URr z30>1o8#B;QfOer^N42cFgLbB%AZ4sY)JDbB_o({`IdHvYd;98b`D$P0vg2se>$2+` z#on=K@TL*!^#Q4)Shh!VO(cU!$^OQ=U z{9aizwbi{`#~-2~PgQw!8t%;7`dZ_*>kA^Zj>W$L27o+haz}fj1qEi8&|3>`NmB{r;7HI&I3O!xq^JZ z0lq6FXeE;G4_zZ}X+Fk`(J@?BB%Sbg^^*e1V!jlO!NCUYKmB%865lJZke1WoiV|Q9kpkD3d_I!IcN6ZGP3?>xKX{bWeX$7Sf>D!UMF(BYTj)tACFT8Kk zsr|{J?-{`D+487oAI!-Y*6L8m_rRWy4jdiB-m|^p{D^%ae-Gs^3e2k^ zl|4>9CTxVc0QQ70+%9aH?GSL`)D5Ta@9CA_lfOA45fF=qDoT8h5*fw2#mEpboA+qU z+!DIVo1W#jm$PM38~?~!AM7GMfFU2SKi^u>+i%#5+*2FyL-u3(lzB(E=^;j`2L{8U zKxhJ~?J&?i3rLJD(CdF9@Wky3|F<1C!BfQMJyh*Lvz<0~KHGlYXR=PLFVyXj+`%4V z(>;>#qL_t5v;$4YxSs`R_dHx9hxUWotZsxY31|B?hmpJ7i9PXt{@X+R1G#%WsM>vO zP?Gf7qr7)V>eALO=kg9t8+h`u7=v2-xdeE{{ingR&^x2qy@_4|TzFz{GrlunJ>q^p zPPYX8g#5(B{rmh1=<9IE4d}fA_IM*pG~SHeKlFer-Eq26wxiv41>aFWX??NqhkAx< zci68P-!a|^Ke_otDQ=MXM+pe)e>KKETz92x$k>v*#bF4@!N>O>usQ~26*1w8!dP&6 zlg~&xGi{3C7lgKnOo49H?5jGQ`R_kDytQuI_Z)qW5$pa%d6waetv$5cdAfmUzl*vsCdALN&^r4(MH;)QA$`NNmRqvW?E`pnswt?Q3- zEyjbeeoeOFzd`cLchn^jYeUDFd6X1^Q6@hyvObG1%(ftnqt*A-=pAs+F?7u1%$gIn zw3FK_#UI)s(0)6E>r5XVj}2=Hyd5+?TY8UpT%;DDwSSGtgEmP3(YAd9fo?~VegN?- zdkGO3?ghUolXH;KiVP2Gw;Wi}6Y~j~T+lPG>-p1FlzSP0`<7`U|8CUdnB|~00Ctok z5;aeXC;Qc&S0^UXAgAi0GLKYq6kaL(aX&X9nFT^GB660TBE=p4 zI;mf<(@q*pMKUaANbPrweUIjM7VDCfHG;6UaZz%3}wc;9^wn-1Ni== zR*;HhrY+XK_}F1BKyH?dNn&bedc^D)<5+Q~Z(S0xM^@QcNxctu@XB?~flCLTD>~0h zkea^OPdssNI1c}55Ml6X4|Rp!OMw0Z<4bEU+!y3^Z#hqq4uTgQ+`?8^wG-XdAGe!Z z@bL@y^$+NHe-QkIzmF%x@i=cDy)Ia_1IX#NU~b=57jBmrNbH=*cC&%f~*&>JL?!Ey>2Y;|44WD}7mZ6h34 zvF1w-8{VQAP=AS|vWTKlRB&RDSFl)^kF}*y>C=5dmsA*4^rxw-lI20Ycu2W~Ap5jY z31balOzHl(j;&zAPCAx^%PIe(-Rx=iG^INP?q)COcJD5%bDafeEFUp@OT6UY*-j z)=ou=Xg)F>xkovPC0$5i)E7)(7oE*&3J5qb zc42RY8(kbL@rYysnH@`t3b9u?g=b%5F}skg1HJ9R|=R3>XSu^O5x3 zIu)!ZD3n$u0Da7V6`iRv@Fu+GLDaAG?9Yox>63NtKNo%N3QQ@1C1_$;KqQr|GTl~? z-Sbv$Feo!^^#Fi}T^VI~Wn(7Shx+|=R1$I-f2NRojX4d|)#GL_bbSntOgfd71-G+0 z%62MUPDNP8LR_Nu-&Lf8;b7$jC9$0fj#LMJzt&*ITCA3x*AE4bp8Z7`X%1~MLzp1c zR3l)Gf$%h8i>{%er=5}){!vc>R<{WKTL~pOv*ctM+=<}2+6G+ILZtA|Gv$)h_!Jv+ z2`Vl=*VAXG)8%yjRC_o`mo{5@w%cxr>|du-O9644HRZ(o1#8PSRJ8-f;lV8jzrlY0g6FkhoDj+m|P^pZfz-GPaU>H#x_r*d_KION^ zFh%xn`l#1mNMFbzXmSVQZWVAnlq@ohR*8v4k z(6_J&Hrx?ign4asZ_nxb4F^M)Ir2DWIk2N#Cp0z(FhcxEPqCDJ(Pa)a@TgvMZ6fSQ zV2UG5F)Gom7u6z#Y@XNb0!fT`Z<$A?e{>bIQ;;o3uyyVV$4ZhkQ z-H7bQNEkIoPIA;1+*V$8*7G@ayhPr1=?0XeYtTNbj9JEXD8VbuGGlJHpg^nFpbO@c zo7b-}fw8ujXXVM!A}yRIT=WqJ&>*qFtOvLD5(=WD=Ie5hUiT5ys3aySXPr(Zt%fM~ z(gE&m$BxsjC2?@%$2NP5aug_>kyUbFgVhar{_t)>F+rmiinPXzO%3hWGmAk96>wWv zJ7}DkxRbeh|0-UoSrKo^I?f-o@|{`H^IJtwU49anKpv1bG~0?9G+frXg|E@^wYXZ3 z^s0%r4%wv#v(<*%kPz%xxJ^RMva%ugF5N$Cr6x$9aCK8(v?pC03)V;TWxeZ<0r_U} z>_(N4{+c*6#?D^}J*R`fCMi^oq#x0Ehwoetz&-DtLu(A9KvCP1yJjPVCU%H|*=>d| z_(khZFQObnWt@5uk-e5UpTHDP5gFkQ-MQ@s5T~DUcW_LZ_*De5G7Es*;Be}!gP@aB z1AAx40Rk}4+hY~hOAoa%4WpBLJfAmcQx+HgF6DZR4Z!%?iWTR*T~PEoH*)H4Z2Mrw zP*bz({dn=pc^SsX|FiFUzMZkea8*CO=KI?li@Wd`A}1@voCj-Kfu1ly+ac$~`$=YA z=82alSKn1L?BIbYmmT8JJ$b=mt7f6PJQOfVftDGMB@Hk6h*WznJymbYIi6TbI^8nu zMAt&wLhn>VSb6REI*-80#6=sDj8>mkW zQf8Q3p54{Wtw9IctW1bOd^ZQe-1xHFr z36R|A?53&7H8rUsG=d9FxOyTZD?JXE&f8|{Bt}PWNYa{pVnIPNUg05^&wNP|nzh0y zk)ujm?gTx*XK|o_E$5ZM6D^e2SPFMA72v}o?1B+&4}e?_eibHh2IR~q{)Gy7-Rjy0 zi-Xk(c=88N6=X_tKCaKVxEV`CIY`WKW&oZ|=1fH39`^w4l}jxJX`^CWJN$SHrB5KE zijLldJRn5{0d(VV?vaBZKtJWBtlraLO|TnN#_`grQRudTgf_~9HehiOq%h$sjMaqO z#)Bgh*aG_ikr`v6|G0oxF5o@Hh}Cs0t~__kKC_eRqH{gx(vut=^Ee76sxw;lq>6w^ zT`W-tqzuP6tg3C)Su6UiyXctUOji*u*CyT#xlVuT&}3_`VvW$!>TxkU`Aop1n7|!J z)5S?3DCw7eJaa9R@!vexoq^B0G*@1b%W4ObWF}0os7SHB(dw4cGyb_!!X~ju?412- zv`GCRonJ5Mktz^n=`f~2NgvC~qI;`+UySewuxYc&b`L*8{(9J(PU?_-TFXIxk_s(i zFYf=_LW3WMFP$6tmGZg!N%YXjPkGQeC5ry4_Uva;5^=Cuxibk3{&i=x7(}pBGhBpc z1ZG6jIu{64a6%fM-dL68K%<7@bFzO*s!eV{GMYeyn<6LK$s+OnUr|SB#xkCcxhv`K zM|^6LM_mgv>35|KS$2&uT1Cp;$9#ckVX8S-iNry_7Y`z4MlYMa9Qr0=u+)hAKM!fCcS(e!;+)ya6K^Ws2od zrZ*&g1`OuG*-Ci)#t$xyTE_Z^Q*e>lTDD{l^NE}5g>7gTNbnRAQ~$v&$T9to*GiV& zBHD38`gkqx63AMeRxA$fw?UoSLAly442-YGu9c(by^s+6mutMzlRaub?DjjkbB^1k z=BLlI$&^8;zl)W%UC_>JAirA41p~d^i(k_;eo5#cizqtU(@82QosymsKP$fA-hkeK zrgI0#aquObz}yBDfo`>!Tr!@aGzaYg!?@|~`8STgrv+0#EI>Y`Uo|*r-Gq(;eIC@1|;eXnD+C5?E9OXjEwUXr3# zsXD73;F5??sdSUeMtHN8YWDQbuuMN$WR0}7^%QDXnpKuMWT!xxA_r>IP^*AlMNp?G+ zxadzmSek>A0ZUd(SXz4oU`AvD(E`Zfr^1}>wQzlm7`41la zm&j}Rp-qhLm`=YAt6vt!F_HZR3kr*@brviQCPDzq* zH)uJIm$%}35rwtAtwdXThiLm?V=@Z52R&o;gDoOk7>=<)Qf)@2rKSi*IyQ2!qEfSD zJahI-_^K{pq_JW!;MpG$NQwr1Dl^G2_F_qhBDn zJG4AL?Qa|$w9%W0dMvJ1x{*IXesu;n0Ll#q2INs6W& zFEAQ}?T@BJfqNp|jp@W;zP?XizblCD-l+ctb89xyZUayS&HgxTJ~I5M9V)4E0k0oW z!>!aDI`2jJ<4pjLGDisBkVkC!2gIo>T4F<}UO}D0u~!XBee7<)7#xSNjTSwed_bO) zz4&Jz4dh+85R!cYvlh)SDKg1DP9oqAlau8lXs*K@XkcOsg-vOiP#STX`!F!#adiJZ z^eLvWbv%I0iWoRBu>n4>j3knZ1#hu);No7sW>xdlN_<|6SK2KvC8%1cE2(xpYbXoJRgU}TGh%3J;QD6APai8hF~s5hu{^4P*9P}9wmL{9iX=E*`&h7XLWkQ5Hm z91FrAW5M+6!@vZQ;xm))=c`$uvkX*6_i@A_EK)3w2rT#WlFdWXz)}vDfCFCSS}Ncb zt7(8GYd}x!5=3NMx~xUI2<E1#DdaD*I@m73ady^a?lT^Oo_=} z!BXkDut3w4hoobio2s76GTgx6r2ClA`5MTYZzb%u(n4wrXEPaz-kJ7nIM+S%+381K zdK}s-rLQbzs@go69m#Qi<5kyx_C$B%+BXI_C$o4cHR%HS){AA)r**?Ou`8HS!KJNj zTQjJoa$J7DVA0q5>_B;Ej|h=&0i`LWpa4(xryvjeAc!69xzadGBkUS^AzXrYEe1Y6 z7O51hXo(9eu&|#=F)icI)A>Yh6*uu_hzN5k$w1bMLGG`4ALOZPNsfMUgBPG@%nbY) z`=5Lm%w)O=Oooc{*EoAYAh>^$vSklNwcOyzDalk;2fE4FX-6+!hhdo z{f#X(nk=4sQ0rpj8+RsNpw+M;2#&#FAmqYzFn~Zv)HQUKB@AChR`VwaJfYK(^4kmP z14s(kM2v7)Z&Et|$sw^-kYrDGosg@}ig5(R1^&EM($)PjZj%G*1n2l!vh8MmC}%%| zoa6i_tPLUWt+8$Zy41Zhdwm`t5`w4T@}VDI={KwM_+vPLJJ5sK_Wz>nE`TEI`32z* zFu>sM4ucKu?(R0YySuyV;O_43?hXSq?(Xh14$E`*zIAtRz2EMBRb7?jbdn>T(^V&( z{{3}hH3uf1t~MWijjlXyhn8ixqiI+(USWxS*)|J!lV$2)NEgJH#KjVNL4X|#KOKm7 zls+1-(}F!yI;FM_HXG~!Hvs6G1l5xY+@3$aT5i$YQaJ}-e^F<>vA@Oij@+{?=G(DB z^P%35d4jvw_$UstTxUb^VNsV^y0%1NMgCfw9-8}Tpj47kb7+Br9=qZcH<~i-P}cuj zsGI`f@1Svp~p|Xd8>d>ea*M-2@ag?S5%{9T3U>*9BXa9;K4tM$H$2mi76OXB$uDL+I z%YLY+o5Sp=8=^ZE5xZX#)i5~=Ri|%(i`_SDyFxM3R!2~FyJ~2A`mdma6#)Qyda?K8 zy=dr`b9!O{yJKWfC`7Pi|H-<9NQFMe<}LjGlQ~7R=UGfz&N-HjoAM!yho|AFHV@tu z@58;6!}WJ|pZ7o61dlQ*I*zzTra>6QgP(Zk^2pHJcMDRapww)nH$I`dvW^k74OY$? zC=Lm(=}%gkF1PNggz>6#c2)r=d*>*wtQE*)My)-CQW#x2$@ zZ=DAsYj?M6%`N+ESy$I=7G*{ z!^Du2k?NE7a(}~l3UygFQ6pA!q361cS*?3-p2rngC&x#p>g)UFog(|2uxk<_5$x32@8$&fbuI%Dt>Qe_<qHY6dP6P~+>47&20tB(dpQ>*->H}YF|Gza*82xRs-&&$${ zAxX+mHZ)MW5~`}RQSXeeMT?#CD1F)RE&H^uBKysBwe=ZcB)bDP@+`H{by!zLIcLwf zqKWAvTYlgZWQ+ptp{%>`yKOw(N<#90v8HBx_EC02sx+=G(WSucuHR%LXj(eCye&NA zSaAWFg`~>6%tw{asp}!(agdcrYL*rokq2s4zz5_Q9@iXIMB2^q$)!n2{KmOj{MfyO zo`9<%D22W1p&B^HM$li(B#HXQJ+TyYiQKHdx}Fgf-JsTv%c|w#o-W+iT7;voFm#v8 zlb$c1sIOtabyhxcGCge1XOz3hR#b`D$Q?^t;%~&Xh-=0-yS*s+W_HsUyN<&wdQSyX0``9fNXZ*!;g?S4 zvXCt>Qguz~mROCVeD49AOUT`)M5mjVH7pcXd{BZo&-0u>qGV5cP`%TLh;qxGOZQ6z zwhA~~JzhNE-756dOsk$0@M`1t5&7QOWAJ?6;T4kHUI>b)@=DwqQA$&NBF?^z{97iF zkQm5#ARYfKp%|SRhLUin=*WrZ_Qyp$vxKvD*d830SNGVpcSk*!(Zu3N@@1yRCbI)a zXat7}5Y&z%$95{#Ga+EGsX(%0X7@~bJw(7%BM^!a4cz2rFng3s!KTg>|Z=Ixjt|W9ID+NneTG>u@<&M+CJIIFG3mj z@^dP<@(AwpclZ+vZ#5*M)(n<275ch{?}-g1V8AN85c^Z`##9 zrv0GZarp}t;8wS6p#!Kwp^IBwt0^|9Iv}=h{%xCXsw1IMrWPRC#^4p)wytPiBWeKu zYyURLi$Qg~@s|q&PEx>14SM+?7W#ru*TJtdf(esIisox{khFwa05LK><)J{m#)F=n z4dsYc_&22PUYPjsi0?cS0rZ+w&r8*C=w9;yG=ahjJSNN~C|WHAxneq;2E&J3DXE+? zgZGwQ3;kn@z(=)9=y^zJb#%EHvWHV&QqCpn<&fM;P8h4J!Xk{?}j ztQoFJ*cS<|bALEa)GlXBG65!Blj}c|+0bJ! zi$X5u&4MuRU?9*R56jw6DZk$=eGAXNV1ok*IyT3UXP>~j=Gfn_Oug8Y> z>My^%|K2A**W7#09Tfv6>-lBK65OFL@>FMAWNb48%q|(@sKb(&24TylRenVxjfk`K zquR$e#?sQ$9i_lv{Gpj`>W5gPxp%p7Iq|vw=EL8*_4XYP6=6DXSK!3?9^EIfQ(|jh z=SUZA6Ll+5rvbM)tM_WoH5g$Kp%Cx{2=bN6N#|vX*;`I$t5qBRE@g8#X@b8i4SLBX zESM=wjV|JgD9$0-%D8!c=+4iaMk<2Z`xjA9LL?zrq-u1+5mevSZG0BDC#;1Zac~M$ z%Lm`~aF1J`j?en&@fPxfRBn=ZHRjCixRw12GMh}6w@O_yoXC9v_KsU@;E+BxvwUQ9l0US~S z6fywEfmpuOzm33<0R=u2F_$MWqaMlU!4AYR z30(}xw)r+@m;3uZ!oyNU5--c3$`NoqCEoQ@nH#{p$;E4|VBq}{a{oHrTnKjT?bQE) z^+NvMvXX5%C84!DZVeuhmHfNAl*2UfY9AnZ z26r70?a*-zA>Fb|K}HP027=*le`MdDSw^sv9gzCG!F-No9aKP01YK3!g8}yzYJ_nRc%gu2oye|J*kP(^bqb1s^cb@3FSS5AojYUPStGV*$T{rn* zDx=9BXxu>escd^uXm~YvqrE1+h>-)dQhSyyd@Npu+FmvY)2<-I9^*H>u9=^ba;TQ+ zW;{rrm*MDyS=R!ODneJ4aZnz=-*z5bf^;S4%XxFTUF(x;ppoF!Vh6H#b>CD&s8&=Axt7_oc@RI-+?GGGY=!S51QHyuqd#A#PW(={!N|e{-%Vq8 zL?ka8D-$WRuuwhSnJ*~wviJsyk)9JPCws8xwMS{&jRby(Lq4}kQe^a_<}T0%rqwEp zeY+oYdSIL8M;LfZ0T?wiY;7rhZ+`zbt}K`DcDS8+TC%7WZzpQ9p-Hl)0`m3F@W)L2 zgxb;G@cB787#zFs_>o2j)pZneeXulo9*z~<7RF=92qqspL;$2~MDL*l-Dx;e*B`JZ zEybjsvN>IyG*9?Hh}EyPva!Olw`4$wGfBDt$EHZdM=?hbv>TJlpjXts42aUI=5U% z+If&0p-Wdr@{mH04W%7@O~kXd)Nd3ERsz9CWzn%iEf;VS6+34fm5C@PQ;jR|Sg2C| z0{2m)1PmFD9E}Va7aR!>ep7DzZq2)o3xI@fdzoe7&@NIfv7Oi%!H||IlgLSK8T|_> z!~uQ)bFvkVZQ8K1A_WkVrd7uihy8kD~-Z0|H}W5NrI zhj zmvM!0%|W6_BkZo}LZV`3xXt*Ku#ga148Ax&2FG-T9<>0<-W4v|U(!_fe;v4k z2m4RVhxb)P(0Lxk$nD-%X~&gxckV1`ThAqNxsV}Vtv>M7P+x2|+Po{$!#E-#i%gfR zyam5tbm$?=D6*6nvmC;kH`&3h2GyX;rd5i#Ech(T;^mKF|8!r(GKg~KX+`&}BFmv% zIc+g5&{D#`3n>lO*b*X;B(E$v5BR3CF7R{nCp0q4A3(m)EXbW>a-hnG%3apt$aKQ& zUNL!Pzw;qZ6(2@(m}cRcOn*`F-^z?&g7A!@Ga11@O=I2YRTztZQY8DG%*&nQ%;TDr z%us@%mD-wQTlx1ZjW~j~K#ahdELhSe3#e5P38?_~1r+uT1wPVM=z_+f#~9&H%x%~p z#BIoF;tPf@G2~cp^;Gkjq{$@c^_$2oh@s%V6Sp+zoht!L1F+OtmGib`4f22SxxXEH z@XM4BN?jvK-&jup^CvwZ95P!>H3Ivxc4TA;nJE<%ap`VvY>bU99Uy+9bRA5j}oxb=xa&aW9$jy zNQ_73)Xrc^5Kpd*8>8fdvV#=6;;2645&$VpsWpaUT>ErynG)N5@w!aUS~iE5HirXV zwHCHrbpg*nPE0TzjQ-whlKoEzA%VLbz*f;i4!Y$9pFq50Ns3EpyMB&43~B z>*tDk({#h*bvP@3aj82U+%PizW2( zCJ{Q*r;R(yG@DQ!z^C9{Vf+j{%a(BbQ)W846=TatOq&~~KFB}pE@LXtRp=N<7H5A_qwh|?k5_`F^;aP_xG)iUv4@p_K#u!Hsw4G9 zlRl(GEaF|)i7>4Yw-|aU0hhyk-xQ>Q{%z2to)1s>qDDgD^ul277u#vEF`sKkkycJz zgBqWH$Jo1*8>7fuY}@7hfj5B`ffWYPesLF&yIxEOu{g#Bu(-P;yC-JQI(~;c#;q7N zN!9&5{I0t+`$zXKa~Fqe)bGuZ3iH6uXTl&M%F$#$)q>TN|jX2|Wi5nS_r1)9=ZYIGwqPK^q3QJ2G$4xTs33TMb&0H7; z7NG%)7EGE1zn{b!FGtpjFuB%~D~yM_A11J9hGYZ| zsyccia*<5$3H$+u_&L*mgjgg~Av^9vCprT0K)$5Xx^;HdVSmEbZsKfoO)=JCp9sJj zwa(A5v%~I~FB@{WsxFiZy%54zWRt`ageSb|R9`x@Ws{L%JFxP-_d} ztiM3F6g(oVA@(D+3pHu>jw4Bz|H$FNi+pLw`Q}jx(#mMhTPvEe$mB4tuqPZQ@7`!3 ziapH|bj~}B3-pky4ZjjnKOzeIYEL6ncL$ONXLIau`q7C4{J1fpq>Up*rpom= z)}?cx99YSijQ&NB=!n$1nDpBQF~;SiHIUt`%WX!t8pVMY=6^?VIs+3#Vph$pg)~uKCWjS9&6mn z_g-2UHL;(6JV%Xy2CMw$;l2DYX>Y9-HjF5D__rv_G6@1>1Jxvqls_MW)<7o{tZ_Z> zHtr{FyEPx!w$?=35w}Uf4CUDH`S3V-Zy#KbIBbG-ON za-w()HKdPX_8z^E*PbORk5DdBK#1bDKC9}*ecZzh`^n?wW_@TbFIPOJLVa}UeSEE0 zg_R~8G9tHz7FWYj3>&5scXX*>32!oWmGHrb5&S)8df(Bshad8#h^{c^rk(tjzIFOp z3Ax5X7w+)$LXO`FFvx|hLl|Q~qnVNSeA9-1o|I=rS*;BAV%RUQg_{fp;>q)2tS%-* zJiW`ia`g~?z|PZ!0Aok^{5{lsnbCPd00V&dwPS|t>yco&Ah|)RqTh3Bb4-Zs@3_>H zYt4MCF&v9yEgZH^)>5sD)-H5*wvrPvZ_MTbvkU;aPLM#=0(mHyRjegF0D8VeZqla_ z9%QF}krX>nGx&E!jJaxg6O*OuZ_A`0Qm|q(4p}xm_^5tW9D0Ph5pM|UT_3c~-1`hCG>lQZQCWz%)37m4Fm!4T$E9gLxQh#&xRGyx)ac-CoH+Mf;}xxS#&X zzAPBocKtmTy@=t#1H=ST3AbJ^cl~$|iylbZz7Pa;e$g$%SFZt%5g-}zO^*H`84}Cx zB_Nv9sX=t1zI(ovTw8wD_kykoAZrT#q0cRPD}K#R04T6S`D)c^J^b;6E)j@{JgX~M zIP~20iwj0Qc*@NR7B!=@-L4ToeVR^>&-g_E7sSNN^Y1_zD{t>I2-q{2nIL!}zOl5g zYzFtHUmjq;>YwaeB*?%ES&wmVZTA=hSjdA6R=@v)42S+#xs2zl0uH9>SJqz>7ITs9 zy6CI!p)kwxgd5AMMQ?A#OdGb9|Lw%g9c-$QNCDl99#zKcF(fSJA+JWsNOe*Uy&eUC z6q{P;xpW=(>)-{dr|!NFC#*}sVm`%sCO~xLakH>l^x}?>c}oPMVDYq-z+oOkxwEi` z&k&@W^z!?fwL!ctx90hPh1d&$VQ2Oq!{V{%b80^L@<>xk6mZv-L!A8(*=i-$=)qkiho z77&`wZ&eTzEBr3`GPDxFuu11+WMG-ptCVI_a}EIV$tuj~>Z~CnTCg zTXQCzvku5%taPM;Qrv3jaesU)S%mp}K@Io|(N9Qk+NWfEK=TBFjXUl8o#-}qc@^Xy zknuR?Jt-2u5qTUF@AHwWh~NCX+dB09gSnF^^NS`+CU3#gb8Q54D`h3mYX_!PXPRAA zU|otO6*`31A}wfFJ&Q&`lTjC%7nn@DlT&|dWHD{8_G^_IlFToxP3)+0w1K^5Y{s0L zF;Q|23wa)?3iBCuYzT|n5c8`U3k{a%Y?viw?1h)p;|NiSoZ$na?uTzQ(0aje zP>Tn(%6spOGPG%hE@p;NiD4!MHMOERmMhf{qsKP>H)~D!&X-i!3;NcbCWb8c*rLug zb@=+$(H^zD%m$78#Oa8HdsV( zuq{ewQUso*T1PH9eQ_p2ik>|oOOW28%+-y8)sGuiiIubLlRr-@rCqT-8LiBMd7~7Y zvoAy|@{rP-fE7^+no((|TuBdh$uNIG5*~_EvJWgP`Zi<@$*QnFeqCIimjf*nSBn)M zCm9(E<2*x;`cGGzKBj6p9b8mh-nw;$UAk$y4GhDIRiVn*B`H}X8A$VR2SS;~^&8yW z0|)6=ROzr<`8Um~2_4<~-uX401=?Fz-XV+Au1YmT#->drI&0Nc;+R!)t7pF(d@`C= zzgBO@bW884q%=z{!UGJ<&f#TD>tAYz!|@Gan?s#wf#w&X6!@oIQVDr3GPdwDD!J+t zyrnn@$*qVl##^9pmW3fS(~U}8dW~kDWpKGJew>YH5oWGkX5nODFA*59h96hawW0}_ zcwf3mqI&Mhk-to(KQs8xm_BFuacX`zExPIPiaaA)tu(9pp4#7VTWK06+(v!XRO-UfYpMyRmCM- z(}&V-Uro5I70dGzG!42q{j5=Q$vH>7EZ(^0zq1uBD!mXTVq zC-eJ^>G{z?2)7SPPlyZAo|=&`ovG{1(uk&s8b6u86w73(QrEm%8hpW#twvrhYlS06 zk>!4k)X;8|UX(I#md@OXRe*&>o{qz4V^b*$=8&rv8B`oZ(9>q83x@M_0!Pg{A5TzMraD zNB1qFnx9Fl7SZ=Yq4#_06SBPvJoC!fBkG?t19vkg*pwR=A)*VBaES43kH%TnV&(J*p;Ch97MMdPY|4gmMUDRT3 z+~$IB&x_Fe1LlWr1rnuq0>{G zY^*jrIa59q_8{S@Ao)x7&=|5t=Gsi7RQwpcrjZfaj$flpL1Hmn6McVQ?;im({wFCj zt!k1dt1Ij8tEexWW8V+qD#jdT?03MNB)HrT_($v8g2W1g?eST95-7JcF zaia@`w;!i(wIac#Qhky;Q&RiEUzkL@5k}J4MI7}xP&4oW9uO{0)%&0hjm2o{2nuDM z9hNFQDz!D0ZLe;|5=TlWo{FZnq6V7vj1iA3b88zoD#w8e&g4EPzdr{1xJZw7W44r(r0JQqrdMdZxbu$S(#m(YYl5K zVUAvc38wp(bqN`JTN2}WS0VMBTViqQPZ z?QutgEu`D7U%ZZxRp}`PcdKp`UTyay$r_JXTtI>oSVPh;f?LjO6!UY%tD5w~zdVij zhQpOkLLWADsWMDW>HsBO%ro1pqOS;DKR5hhW@|mSyk>e{r(@V@VphkTUwH~1H`|JJ zqB9hxy)f}qLwyxQDHKCr*3C;sPXCboqtcTg41!vTu2vdFuGJc$=0}a?gPzW*YU+CuyasjzUvx?JTkV2ZUh`SilFdRCAyti4w!6<+7R#kE+j?wJ z7gdY4>AfVLsykLBNhlrtRs~dMvj9w*(~g?HX@<5At4%C^{}GIJZ;tyOLQ^ag0>zkd zH`_G6I;C7`%#^^o-OwE4RQG*n+6Z+KqgfX+H&pdrm{=MaCK1AH5gCHvSZll)sqvgE z6q78=GL>CKZ-+Q?jwkMrSMBJaccj6oj`Um$X-2-06yF9?3T(E+NtwV2&d8-d8k-?c z?+Q_YaCD#&(`KSjMjg*a6HNyi8T}F*3TvgEKW!^-?0hv(y*Cqf5_}g=#iN6GvMSf9 zo7WM%bvaK0#}Y@UxiV?&ySQ;oaqXR&eax_?wX1J%Y4ajbg_Q_XctCS*rJ-IGLs165 zseg(kS6-FI+Va9$e~(|6vtvy_-rq;9yt*lZwi6{ve56tJv0S`{752F6 zP+TsQArL>%2Cl88$PrB~G<)=PaTHry+uQmD7D0s(6kBga9gU4!j8ICca?;y$DOHBr zO1|qYqhmoF-$w7;vw#GXe4QqawYZlE#F>5glz6jgMu2H$z+|q zH93k#X0ef0 zHM?M?>SYU+LgYP*BQ1tBY@IbpBp!Gxw;89FkqzkJZ`HOo4^f!+oG;89a*zGakc!7D zFF(CgFAF<%Y=qaYdS5^zXL4v7$J69p@Nt(T98B-d}Uh&s%m48@A2gTdTvse0irnN;Sq^8DTS&p0iN3 zJQCh=Ccf8pdk9gjll)`uZ0!vkG_Ptl6$3uxY{w*q$p(Ioz)GCGsjNI>lN}wAIULz5 zwJKI;Dz)9UX;gz!YB463uK%RKaj(vMgIvCgOlUPc?wC63#;Z8&*|}p|SgPmb4mrWp zVx|tI0+>Hqvp0!m)+&BQHTPw&JUNT5vAgNG=48g1j^LfY0CUOjnTB27AzAaqnfgqI zf6|Q){bcd@(0vF6auM9j#ReQZ-^1l>h!-)rqDRxHK%Pwaha0LlH+Z!BXx41NYn7VD z)#0caOGc=)%^fT{{;67Kpp2UmJ|psm4bPWsT@+Y_rGyU`f-lIPhH=?d_$V$hr&(XB z@0k`TxyocLa(ma|#IyEy`i{IH`G+~b_==ix=B}T?10VMExs)1n8TvRWc2S7ai211L zusCS9Ujl>^tlXrRJ!7XGW0pu4er;v^@DuiYJj-1tSeNCYUJM zb#ufROLFli%*Srn%z8b!z15iM(t$Q5=99%2%wt2<8#mtjSs>1eb@0EXr~UY@GU>~n zViLUe0n?>noq@l_AHI6U3voi7r4%O?w7UpxL{mid*dBi&k0Pe4|K!Vi&!SWgREDFS z-6n=pVOC?XyA;8kwfM3f@0}2c_wiXQ$G+lniUXlNMe4PeY2#`7(c|W!oQds!z!yv~ zl*P$HK*)T(Leym!lN}iCycjh=&AAG_V|Qb}{}z{=@eu#cv3(jkDEW(M;p;-0!xVPK z{%pjnRvWhsg4#`5 z_`UUQmR~P(_!1-34xh7Xl$fZ=2HxNNjj_3G@X#Rgm+nBz1tK>R(m0$|-EIoFhyr{N?`89+_FmS%~F zG9M0gI{4k$Ab^uWAS*RjR$88%tQ6({x)~xAlsZ&-l=`Njr>xYN?s(0n- z?-QhB1f$GnlrFFlTM?xYRJ<)VAQluhR0V#W8M{85D+%^9UV_Yn_cW*xf7V1Cw`#fU z`|r~O7JZs^r|KA~TQm?P2w7{j77lt9sGGW^!e7vy@peB9pgUIgkN8*K&+I!4wpA@EEyTUXc5D zkPbDWr|urCu7s}8Mm}Ak_x_m~?+MiGa82={-`K#xo7O`e#)QB=0D*zH_}8CY?Be*V z2cOg}Q=j^mxk0Yae52ZDzAUD(dJ8+BhJAlOR6*%peKVAfeT&iPmH+tap8fsM1$&8> zt_tLG@L!Yn^0aCD9BCtj1(%0?w9Gv@T@U>VmVySmbh1>-1seDSUuuo8c^(jp9)^{aajY{beYd+ zS{%_v*hpE1p)(O?FUsR|`Bc}t&DiXcN-QXMgohf#{c&;R3UBd%r#f;`VPT%{N49j+ z+vxNQWmJc7bAh6SMWA$%7$8A7ar4>LcjKT2SZIi`?wX{gLN|ZPDb@pfO@#`!K39uQq zyIqu?(^U5)Mo=umlUNA_$*}ZqNt?0UAQ-HIf&LGeY^-L}Be2Mo3Lw+M;+3Iypqqb{ zfObFlav$`94Kfsx_$YmmD?rmsm0f^xv{ApV4vxSTf!mOgzpx^*=r7r1@>*Kx=DF&( zaq2!A5N=gkR8IJGNCX}$8t0eRMtHjxl&|MPEehHkEej6L5pcr|oPS%FTlm!nyhuPtk~8oZ8P5% zfNYY|JLiy|{eg31&QFxeP;ne6U88~4-z7Bka`gGFgs|HB-LQa z63hRi2_^d9O{o6~J^hyq#mfG_(oifc9RHJsVqjol`4XO(=>K;biiYX`8Hf6(=D)?E zSpETZ{+qGG`d_Cb{q@x8WJbuu%JKgThXTE``WciMOIUX-RP_GM0D)4GVKf%pR~G>L zA2t*a@6~UF?#$8pWyrUwz(Nm6+qN;-y;Bo>7rJHBjS;n7^)G9F3-;>Rowjwu6bx+B z$uRfh8(Rx2oeNq=0e;Q(_A{MI;KiiOIq&#zRK_c_ci%S?B!5*@L7^j{yG6;7%eS0z zV3W(=_xeliG2LzNEdC(o0h=qQ0>Q>lya(^6;AR6 z5J!UdV%e)=Ne+K-kWC@)2OR-MDVZisCouNKrslFCcj@_gZ`ET)$)^ZnrKaZ8O%D;D z`=!$&*!$eEu6d6sSNQrg#y-?TI-hf5SNKE-+McEMCv$Gk3HDqifO1MkK)Gq%dS4CV z248Xb48DV}>b8mJ1WMWOhWD^5d;>fK7oX%#>T6ZoP5Qd;je+v6mfBSEcM1sG7mpA= zua4N{mH9_FKZ9En{_XH&`>)UK_rGYe{{tEIe;SSd7LHMviKkdJOssHHXU+w=X|FfSjfC~Db z^q=y-gQWhs8~+<5_0Nd@Z-bouzb@8)4swRC+wK1XNij0AGSdHVkd!Bk7wW>oQx*r) zYwCuTOM(?4f4qd?k3Ld~z+CP`nNbK(qVO>Y0jWu{4x&F2E*l$�K&(AOr*|q`ZIT zcTuF`6lGAGntwNnTUOjt=~{n9eej7&{8G^oL$21j8dhX}pbjlPzJ9vabGcsfJaz)w zOrtTOoS(wdDv9_rWvsLH-bpb4Bte~`KT_r!iHQN28@GnIpCCPwj&|yrrlxf%h`pxu z*6Nn3j*}HMDuO|pP<1|CLq|8!ZAVdmen>o{RLfcRg_lWoWFvM&0`@dliK$F639JT> z7`;9`EOW<{4lxq~0h-rsrIQVePoCWxy+=zo>Nby!&zw52%DZ<8f+k%kut6lMu{g3w zimzna2q9j5hXwrtUc91V%K?tKI`@XddC|OggkhXreH3XEP|3m-G?W}e*^(by&vr@@ z1S_FSQu|Q1-yVPPvh3Znur~y6Ni4?0XFfx<#H{to4Y!GnD%?T{0%N_egZNvYji9HH z9pv}N7^Y-gh@wxlpxLx*P{QO%&yg*Hd3S1SbF_X21-}ahA|4j^3hff#Aic?k=G;o` zE5}Q`w6;j3a>EexqAt@ULZRQXelfYDuj1erQOvCIRYqvH@%`Z9Y&{V*Lup;RJ`Myr zAX-}h>%&d{%j^RiV#rY*!t1+Hlgn@8O)+m->)&mTXn28El%LmH6`Y7+;%dDY$;l5^ZS86&h|GWgje0U@NTESCUjPz2iwEu{7;` z*tTRUf^l+xu^(bTvAQ_n{pDGZ=u!;!Wr2c)2qaZ9dCv@OMb;&LY+8zyn2fCElKAGT z^$uhQFVUUE73XU_5sZuTnxjYOR!cw=bp>%3EQNz!e3)OGF^R!9$=T2KtM zwGXiu%CSe?RWmk=hF>B&-qhslhpx{PZ&`J5m<~^>Y(wuDO;wMFWZw^(!oOm;D(NtT zVC?auF513u(}g6;Q|g(W9;iB6O1;{-3f9q|SFE2O zHU63lz#R?1^=WYk0w}9=0pvaxUJ_KR_y{#*i~E3k7d-TF>FGRJ_6h7C>zAx7iqg@n~Qh(DXL z@fYc4Jav$0-lNIPczmTmp%IA6aWSz<0z3)qqLbd+JnMNUo>!4L34F9R>rCb;<|!>b zCvo_M0q!|=rF8RjbJj88!{IxWqVQsgv0%zzphOu86Pd~3-)Hr`yG4mJ>9dJ5 z+%*YKWb{IyYJ^CYu@HuO)R8E&QSQOd4RS>MuskM z-L1kl!*Pwr_sRW^Rv!oqN7=A36-hRp?ZUx#<0L-{E>lX;YR)BNvwthbVdF~^NT7^a z39-{|M?K8JV5e!v2r3!1(bQ?lY{ zrUYiDyj+todv%JW+=4VMHMqHW%;=06r8B`rey7f?9x*s$x=Lo>U8* z=k20q&AInk6(7~rFM>kTw_b*mXN(#vi+vNjUa0lR;bQXA4+G$+PssYDBZMMtZgZ>z zvp|5dQv{1aASP-|1n0sJvv1ntJrJcdH5?Dw(@~t_M1~U69NEhpdz=FvGau~az>Kyd z(MR%!G+R8j#VMR~h)0{8qJiH(X+=godHobn^j6IHi9 zi7O=)^KV^I&7TP{p)a=^B(Jyp^C_;ZrA~W1D?nRr^%MsOCTHU$+6ghPRq;3`5z|?e z3@8t3@8Jy(2~)z5+n=ZH)U-1$w%wvpO%-I%eMZN{@RGanyVfNB|9 zOvsLSt5+TNHWaXuHVIU9c(q_3Cae>QpDE`qhegKeqF6NhHwx`Oe`Iu0p3hr9aHY!% zCX7%ciy2J=h=y1|4ty?TA_;JmUb2NoWc5V7vED~mH(0Y?dwI-*XNPD)Nf zLZU(@f$Ono6F*EK4)hk&BU#cc_gT~nQu6O-mZme9UyqDr{H^hh)Ha; zprUferMMDf8>;5Ysg1Fp#u>hZlNRYRuUnk#U4|$e$GmB2#*ZaUf&L-IU}-w%2v8?A zlo?KjO;?`A`wetMCojX@(>OrJKRwI1nRkAbjZ=E9_B`F+6uWBOs~fp&-BWN&{~Ydt zz`yfoWt{R;O%toT;_`AM@R>2~FWE%7tG$&P=;R{X;Yz-kCBu2d`O34^%O)OIm>Q46L13f% zyv`VZOtfrs*4k)C7#aps^%h#GcoXF)J%1loww=fr;ThEj@8w}uu;-qCV4s0tcZTH5seMWYefG?GAr%GZ>BnMoon_)lU*I!~{oe z>M-+^e$#i;60;Ss^k;7i-`AoO$=%eYX=G&0Tui+COpMLvGx$S(WLWG)D655Mc8kwb zY^iBYZ@`&~*CQol#}z+Uf7I6J_yeQoFMP6b)KZ!0euG=Hl?YbpB%O0~)M_Wdf$EX_ zTvp5zV4O@>qC#gf*6VRBKYuP;VDZRj^kaQ?T_9S%jl13S4P+pgK{7|<0h`15v`Ja+ z2BYABplEAE)@DgU&W&f><{-!I0%+$Ojh!v*T_2d`U2uIawfI0D=i{h5TU8Jx5uPFt zzrm93UZ8SMD8|t+rxK^!ZsXmOx3)~bS`JbKKhPOH6(ORmAuuw#$6By`z)>>}k=pM4yaPee5HJN(3{4}Pqxm7vI8r_-np z9tJ1pWoAf9En+KSaCS0p7llHCfv2*qBnycBiq1t{^sJpSHDi?{)W8kr{gK0!j_wdV zPWS41zWmWid$5_FTpUFXy_14dk-Zku?ulk;$+oySX0}$6sZ=KmAz=yzBAh|UPuY#S zDo|_7SZpQP+81QIsmBy=UxuzADTQLypj2pY;;)X!#F!k#s-lTL#mUM~W$6K@vh<{U zCTR*mRa7H$C`{}?j1KUq)Vt7v&WV8fnX>QD_V#^rPG(w;UYdR!Yp&JYi$is)B@9n6 zbUCe4r$xs%<`UnV4E>K9VYK9wKj$Fk8_nHsHugMt#&x=Q`&>^r1xrtjbJ7N6m+ z{x}r2to7XWI`cWax6App8dZ(f%gx~goI$Xjk4bC5?E-ns{kqef#;F{03htnm;5#Mc zySWnbiqQx9LrLQxWvoeyqAw@X9XYKg*X0eH+$8on#_gt}ti$!9CT>Zl2MkPB(zwTg z@j;0s!V;;9F#27$v-TMlz(2OFRPc-+wb>-gm;eF$u2yU`UZ9 zM&Kl*ONTW{Fk&vHoA_FhskT+v*Q6IZ>3tA@Cvi`n_K~`-Y7mQrZMPnPXYyS_1UFP z{dBkEnYK>zeCO)m3h64|<*s8W=gxBn!=QZ>M}Rz4cubFRchc%;j!+qsB$_3Xr8xE6 z1e=t@3*QK;&7hf9Gskk2DLSQIn&a?g3M4zbKbll`XEZxTQ9beEIC_a=9`s{>~$b(~o6vJHy z^oRGG_ds8-9=Yd>zMhYZ+-bXfjO4ki5zXAkL5D?*{gib}sjlZxRnn=KFe7X=-}98M zw~=GD7HRcDxsgh#Frr2N0hBfwi5S~5ILo>yWMG0x-5<4Q&05fIzwb{c`=kuv+*HM} zmd9$CgiHMJZDCV=UbmHYsg?Pb26Rwoj|{D*!H-D5ltTG+ah(O_c>1Y_W1BpmsHR@@ z`rwKk6^D+enKx!1m`_hH^6FqdC=6)JAlD&-n~dGFb}A>$`#o6j$PR@dpuqgxl;ma6 z><<@Sw5!G?RtOkiDeNQc_<;ULyBN;hk4J55SoY6SR&={gC7eHl(%DPHoRz`43&RkV z16$L>Wbv}_=6gYfwvIEGmBCu6o$5H6;grv+tIMK6`&&dJvh>74B4bx(hS7RW`BCl$ zwMXWLz0ZQy)Z5ENQ6h}1K1HmHaj2eZI5^_XU1B&Hqp+`gtL#LsA@4laRzx&Rb4_(g zg;QD;QPzX>cOM-7da9#6m88USR^usu{zMdyMrk{g@gYYCd1bSPYRzo^CQNB;9&bYZ zMZNUW-#rbT)56bbea76raG^fa%st&LP5aQ8sc)EO+h(fp^|ZQcu6qdcH#=4i*z&h8 zdRWCb;T*2f{p~^_l~c~Ml^L;>^OC`)brb{o73{*nzAv%t)Crg0F&D2)R6ZdidJrh^SiOA?nQo6AOHWs**ix`^7v`G zUAAr8Rav%e+qS!G+qPX@>auOStGaC4TfdpRcV~9Lb9U~{KQbdTPb5x!@|=i#pLic{ zcwCH8XYGzaI>WVXbl6~GzPe3Z=%z6;qHXAy6iT?#0(e#78xvA0F+8%nQEKZTt5ov5 z2a+7Sc;}W9p6CXCT918+(Y=Re4(&5BtT5T*jFV$+)n7bv@p8qxzN+f3c3jlDH~Ws3z6Gu zZ3SQK!jvV&o{O$l!y ziN03B4cj~{Qj(q&?5B(6z=oG#cK*LK=Sz;2(p*z+n zv&}cNxP)vb5hU0+h8x}S7lwBS&_A(?9>b>8an43gi-CQA6|WDK*lSoxClW4sV}p=5 z$$U>FV}n_!br2+gutrXa8q_e4fNs(NUGB14#ax*s+*&W?4Qv9-Y^*O*;s!9$cDT_* zLT!HCT2L^wN@-|qi$^=7mP5zN_IX+Cp=C_|+0Ksy&rUt^wn{GS{9MG_rMXim$>jJI zj7J7*a~oP^V>EU}ga-goBI6&V0}7sOWKk+2pi)Npvr)(tO{5<<%%KcA_S5M zqYD8Xg`8^0X9%pjnrMT;(Od(>fr*9H(;}G4^Y9G#;&eqV9;CJ*6tU>;N1|_QFUFl+ z-R9io5@zf#YNkUKs2Q^*hx`;VRu&xAL~*;4H4tMmXv}+n?uBM(pdoGx1nFsI-eg5+ ze1C8V?W%St8Bpe&+hAD4J<4GiOIJd|o5o?E%`Q$I`<$ zu4*M+xhcJW>eN`lr;^3IijvSR9Q|_RhdeWcGOpG z)~>DA(AW_xzO-sl6;|D;EE+{`w>p{}7)hLu9EW~F4pY(FsTIl|)v3`TUP)eTr#W0y zPhA{meQ0J~a-|a{EipF2_acZY!B#Z76vG z92PxVskpC|5r9gV7B1{YSD1Bnn4+RKUV?VXsFjGV>A~&8}6lUDRw8`O@;|-KOHn#ik7qdJmj*6K?i&|vg6t;~WF`>SYCY~%pRp&^TE61AdZEaC7 zQ43-$uM|Gf$Tj|*>WuK@o=ZadRu)#7!_zEUA%7Se{lwdfj8lt7Qa?sZQt9HlZAmH- zyZ%U(Ml#Q^hIGh^>_vH5YCgXGukxbQe8%_PO=~i$TD3@oZK4C@+@o|-zA7il`EjzB z&T%qJ$d2=_o5pW)`_bQfi!A9rBUL6*jd?7u+axhGP3E?YDx(-ln*1dxSthZ=WUl{Z zDfuGNBd?Mx(I_!b-j!6eisZ$+l7y<8E0JH-l{7{JZ|q1JPthC?nKCFO&Skh?{1f#1 z_OV}_FRWkOC}d`KeP;kV+2B+joQi3KC_1W0#7F_E6(_x09NF%i9pw%Af zL?E0z_<_-euVr9>G!aRf`x6|g6zS#_NpAQ9sQA**df;Y}iPmtR(tl;iTm#R;4QY=t z`X@-V71J$ehzuSuFLS6_i*O2Sp=RJXzTmWZyijK+_B(VigFx$#En&)-xC=O@HWY!>CgttEdG!xs`G|pGiKxMJ0b1V%p|m^CII&OZr+DA{m&ClaXe^J?{Q(^gz2f>X?0Ve>?>erwV-)hrJO)Vk({E=}U3nZ3kmDj~kNR@}8Mri_nP z;$MovQwj{$#_iqMN5B>Vej_j`)uB7q#;ZnKSLQs;^{jd1E6idgx{d9^jU85WDVe9n zmHgb8`aotMZY})fqUlmZ{u(5iaWAY4*NnK8rDSmC!H!Iud6}5F55j0(s30L~9_l`8 zBfF{(UFRGOV`iyEUR~$-X5Oqj_;KWUgN6A}yiGBN5%nqMzL}1Tm7?>5g80U$7J3Em z;wG=&X9hvx9EpZ8ywN#FH!7j;#G^s@U_IlpIYKc0$u!nCPk$Ig=1t z8p{zDnU==RH^`!qn+qnAX?1k~nLb6AteEG2|Z2p(|4aR|enubc7H9J5zJnZiV!INb$ZwUuvN5QUzrK zsW4(k*|6}j$ak~Sw52>G6VsMqZ?(>mD@e3Vdv7`j6NHv!&;~It$_Sf-7v+20M502k z5mzm$2Lb4LAiGavGG+*SI!0|C<#iS73Ny$vn&(&sW!cqh-7$ZRU-ootkPIo1csY49 zMZkmst}R_E6uRqcg-T^y_GUbutnyZ-zS!8=iku}}55sFbTW5t7&9vCPj|lA;^t_xo#0kLi)(aHUfdC6BewbEN5kbHTQ- zt9S=G&vmmAj|^;VXJi0 zsbfvNs1^&-x$y$m`bS7h?ocCG=D2Qbzcfp95Y7*-;EwJe^jgh6@abj=kfi-5cG~^bcl}1f~UaWECugpaj z9qL%E_mbAMzpz7ejpnaSKX$4RG5(wpM`6KX7*z>RS31f#G7PRsb#?$a+Ku6-7x!53 zX2_bLRZmoR=-H$2+T(5uolb{eFqaB+R2d@`r_pzn>2~wVYElU=p+nJK1S1V#E%_5!Qom9qhk8OCjtPyKtPa1>As-3ilOejZORnal_1e$}#=Wsh$2yLO6 zFkYNk=oF;v;O6_zB@U>J->8&>Mwyi+v##ah82#$7)8(HUeI_|YCt5=luU)4oDURzo1180( zU9c7HfIMo$b{uN3=7odkuXNN(FsY_(PDidN%^s;nyJQ0PqH}yfnzTj@7p8J#v$OU- zOe*u64u!o=-`N2p&V0SQnNz{(&{KA7am7~WF+A#8%8LSYTH{ZZ3P%n0ayT?0E*L#l z_qJ;tYoja9guYJR`~BEDY~v*h?>{Hu6U{d%{+S_1%&RSi4!<-XY$bUcjjp(Enl~iu zSLc2kH>bv_b^SUOp89jjt?0dUX8O>k1riETmH49y1SBX$l0rKs=R~l^9U1dBSDeag zb?pkX7>zf|sCBV@e&G9?(D4Ma&3Fq*mUUr)uX)rdotW%}t7Ry>Z~-*9{4VN$?^t{t zx!{bFtP@gCNTdjNehN=+30~-q&j4i3#(?4M!$r0yjJOK072d3D2deS!_#$n=v9?t6 zxirv6+R{$6h^^vc=aV(MBOhD4-)k4l|D=6TKLiyMedO z&dy48Q^l5P)R91Y*x@ZEaM8}EEvqNWpV3Hu_0*xRs7JbDe%e2#6cbULr>&?biv84n zqrOimMtYNCis(-7PIO82?Za45PaLJVpc(qCS}V?Vit&+h%c3TYPeEPJFI( zO(SJ)>Hs*o??s)^OPbHs;+FN*xur=O@ws`j-VM2D6xTVVNn(GW-PLvGo1ja6vh9|! z&rm%<>Jvzx^xo#0L0TFl8t%%sr?%ttpYs8yB z)8qcV%GTW4wQqM2{3-p0V4oVWR|J(P+gBmNkUz`ex4eEUcwcdJYC~R8UPFLw(T8rA z^3F@29PP=yN9U8}ncDq3cRuhPvhmhPa@-(tdm}-@@KFw4agz1XS0BzzmevoxmMv zKji|aj&$?RyAKbAO-yxFaQn3g&K|7iJwR ztPtQ2d;#5L*0%&r`#U9a;;gq$V8Tix6k!Vb8TFcawpgGDI2Y@?)wJ6%v;31187=1@I*thP5vQ zi;)Z{vd0WL3hg-)DiT`OND&w{GvA;=k+#0ygoGn3qKYI4I)@8-*m?#+hyPv48g4_i^H<~>lZLm zNgCciGF$&bj{b!da(6fw0jRC84}v~&LE2mepUBoiYuBOx7t z?OO)P&iQ@C%)$qe}K za)h7I3=5*5Za+zpg`_0oHSP=MeSiE4L#@D^_!meRNAV33)=%?X9$ST$Tgk;>r5-QI zR)&N;17V^r^cDc2(S&2HUe0NQw+oBbgAD6sM!mp%9te%X3FE?25C9@S6mU&AtLrhNhin`BVl`!U) zN|f}4dIWQu7mRRjgy0rlN|v4Z_D40vwW*B**F1&>>S@7$?xYm zzF+SjFu#BMiTr2G@81UQf1AO7@I|uyUwx6xjEqdo{|WQsfih58YJQsXUUhP_#|0w7#uW zaM@#(LJjSv=x1G1Q!{5@qsmSex)r3Z)#v)SKHl`9o2liEXTt1)aNh&Nq(Y@dpL)~F*U`asl5 zKJV~L=#TF@!;C_|@V}G16t5-5os;|Z-)Qc$A%{^L^~E8mzdZd(%!>Z&%OIpjuTRx> z!%es?N89RG(Fq}`QM^{1=8Z1ie0wo(=S1Lv_KP8{RuP@5ilfP3(6XzFhq$W*poJ|N zhZ{eW=sub@!HsC7@Hm#1bn$G{be{5i6k|+&0wV#RK4Wp(uMO9vu}LFSicOnMnocQW z(P&K=ANe+q#5Q$WAy*}r^li+y0*2^{To|32m%NK^Nt$;ae$X?mlSXyNrq;CfzVu!^ z`6tR63@!jAX!ZrUm%^p`*aj&ziKn}U&0 zZF0+)b%WP|Qv;`Uwi2VTgiZQ`PvGTdHf>^&{$PGfgp>4gEGPIWD#{IA+`k z_~)ky6DcVhDO4K;ZkujxZl@Dr8{QtJwvy@y8&p#cP3b0h)>;m~Iv~QlLnm4C(qYbhjzrk~eOGC&<6guAr3_XEBjn@S}`T?lwt~ z{3v=8lcNZE6MKxAq;62twyvsK;>*Ad7kUX$e~4Lox$40i<5DF>IpKVc+eVo&-xvfF*Gy4PJ^fJV!;xl&I2@uEOnA+}PQ7Z4bGuzqT%6e*QIbSW zE=OBKM^hqdvhiGP+qNQ8Tq2s3?;xd#o^aPt;9|7%d_Ip`PRYYKUw<`+WW6^ib~ZS? zAo-DTl~0FF0brXJ3Npu_w!hZj)Y5D;o=26``lF)CSJ7#6yMKH8P3@*k(zqsbNTsFG zGF((HN;S>1rhrwIgnBq1$?(fkH|8^C=qP2U=u*kjR)VW+-DVl~_&u|IC`os?M5?%4 ztZ=adE|z@wv%l70{${KCbmMhSS53L!1E@X2y4p_ zCQl44Ev&Xh8Ex65Lt&G}=5I9Mo22EmMW%KpG<;<@XFZ5-h!71ieK0*rrm}d)mX^E; zUV}q9oc`r+=zasb{TN2D_1P6h@R_^FU_1ZzAp0uondg%9PV><_diDw=>FXnsSQ62$ z-XpkO?o~oJuVtCjBsw_w0HLpFRwz>z&z;E^Vs3InN;6eaohV%_zMm*gZreD%Vf9;s zeX6{eQ}tl2xRbVrqodkPY$u8Y^Jkpa++<8lgoa1+wQ3Vf?M|Mi?B#WZlCh|@V;~5{-uRqNzQk)q2w;^}6?K{8arDeu@Bob3Cxj_y-qN`zUQM{0Q6ZNa z%(eD|n8yMPj$=B5B*U2o`0J#6Kg2$1>gL#wwIATb;E6L**q6Qz2*C2FIPUC=t+HEv zAN6i>v)do;V~C|lndx1Tj6x7+Y3A#`&$u#0-*V%7@tqGE>*j_T87JBIOsBgHJt39( zB%DgbPtjvxY8-_%VzTcRnjhq(ve&4*$(mr&g}Q%+0^cI+6#)keJJKE#ag$2-&* zzKNUs73ex?eE97LjX#>a;V>fWVHx2p4t!t2J(6t|r3Q?=n8?lX9=68sqjuTE)`-w! z3&tk$;DcqV-luY=N?1a%jG!~un&fhV_e)K)jWEYDFpfZHs5nJ%P7|k);Y{6dC@J59 zIA;r^C-d0H-dSNJqi61U&-)DO=V7b zhOG(t(PBqgSoIEP3(Dg)BET|Ygheu^p)V1ivD)MDg1X|1yH=fnSPS>@wJ2Qh!x}}k zsOj>Bvxi}yFBHneSWFU_?DTB`^@Mo{11%BA^tx2_9rw@rv)hgjXELl1EBaB6VxOAL zus%L7PXZM5DZ_;Z70Ny}Xgl+}x1!F@AkGR4`ohKULWq5|bQPkSp&99s)u}BZ*hnfZ ztJQHJ;c~P_Vub>@mf09_FXNc~2yr6^0StGdLO^D57H6`G$UMP);BY}DjRk-qyd~uf zBCYvTmqR4K<`7t1`Ux1Dj3`o*XHoh&R1e9 zMAhE~m*wb%1w*sbZ0ImdtQ?d5$E+fd55u!YzJP#n9!h}ds76m}ebNXDbTCpj$lz-N zmzE}s*qkS8KPhV&F%yGw(OmpU9WnnYoCi3F7GvpfD-Yk-4=zjaGD<`Z_fZTSp}UoA zYSO-^Oo>tM<2g{QIs&x2C-`9iJ;;f1g00~|+v6b0JzLnrPSSn7|2+yz|CN-($> zDilznAR_G0ES#coiWgaSnMoRR@i` ziqZ0EL~3XDe0bv)!uFM`yE_482qR2S4uQhT6&e7$%m|7~OukMKh*hwYm>%8f6$~`f zYqX9iDSkmEM1%;Zm{9Jqg1eo6h^%_ z;z8)XuVispC{vjz=)3~uZcIa_3I4LfRs@{Zz(R{yIxO#k-Jx_{XvSa#d?7-s>~h$< zWM31EaXOO>To_Z=;ucVhJ!U(JDPKNS*?kq$a-CR0A^K!JR9w4?i`SaLtTl8tatibg zEL`0F5+(jt`cl8`rX)4a-!h0pDPYEG&*ciAa?C?RMiGy4-`4zbgAfSx$a1*nR)r|s z>gL9QsdjGdKQ!MWL}#}~di5hG^QfYbrPU5cJ!4~<>`T#{iA`7{ok#d5spYltb3B?g zWOMPT7*5Z2o!Cpe^%JA%65R_qh+0nW%#E~W4(|0;p~16bmqxcEr81m|c5bjr$?=op z!@*Q8ovA1XEB#{|_jdJli>ie`0RTk~jK)U#j)@B!y=6Q1U*EfWwbOPG^A>^)c+;_J z7*39~uKCCkaS-w9OrR2AuE zZlZA43!`FqLel33a3Mw-XF<1coL@-PXGI0T06@@-CL9aIVMc!1WR2P4#_VFUPRnJ5 zYwaQSBHFWGT;h{v*Vr-KS(gBMQ2b+KY0{ECy@-z zv`i81CCn6wri)jkqM|dR``tib>1A|EXtgMKu5n73SsAp-v@Cc(%Qecg&l{)&a5Rrr z%rAvT_vC3tmR^*)Pg0~&96%t@tPzO`=B?F=$nDjND77MUi*81qDu;jZ zjv29QA8a8)24pm#N&!XejTd`v@`FFRHL^7DHN=##bCS?VxD2}iD zLPVhmQScMKSsLEMrxTGYsz*pLDIDdS4Kq6kLjda?qlypkO1;SBJ<4wx*at0v)0;ik zofQ&qy#A1D?!a~{V0Pkar7!?s&{DQ2wO*-TIcykYdoOh*_z93qFA|EF`gS__(_h(Y zAm{tNG17fEVPM*f`!_a$kplYmf|Pan$Ek}RBt%yY#DekUa3b@5xv+PZ?Na`+3LEAU z-Nt(h!&m*xey+yb*rv+)XcZo!Bv<>H3>I9ovZDpbdo9@DFx3v)<)5EA4$>1}p?_{! zs^=3ZJ$v6ib>Fc=7nFr$EXK1DtV6W(k>87u)n?`ZNwk!XW$eoWts*#34`8(%4n{57 z8zcGq6Bm0C1(lsf^=abGS7A&>L$J{-g8;D1E4?>KkH+xL!GlJv8h=k?MzCJDGEAo?|4A z)k&Mje3PKSNW&x|B#@8Y#fXelsPJZ&7-TLH=EC`SH-**N)I&Tzct}6&?ovw%3DjS< zVRHkii;a5Ogfa?jw|)5+>@}GuA0*Z}_1Q$W99VNqMC#P1O4x8vUZAm&!o)b0#=%YjvN{S0ou2A-@% z*lK&3%2p*)p|rr^F!(qxQd;{hS(uI*$so4_nmB5M?jiBviu|5=CM?Y{yAAE-X*aVXUXUQE5U(;%Tyo-p8nafCO zHGDxcQ_-LPv$hJRDCl5Vfri~s+A=D(NH0^~$v}C%egb3pI*DdLhU4b9B zZ~E+Z42|6m4)?tLtn8p%R(|h6X+QQvu&f&ZS`{-ce&ST@ZR%~ZeMroGNB zdfym5?L2QOIh57O78VVcMWS%l>?Lv11k5n(-)SBU})b;eeK~#8|n{xkUF)J*j zCusZd+Ci_^6v>ve(U>#$-Jx6^w(^pN=O0UHZtDdapgmMOl==WghYHm_+p*S0r7NzJ z<_!(|)rjB5MXB;CT@|MT933(4h4Z_T^MdBkKN{)JDo;n16D<>lEw*23%@tJc7*t#A*E+2$kX8hqf`@V?0 zmA(pQWLJ_&R7+V;VB=OZiXg~pvV zKFF_9E|c9#QbkD%>V@LR7}?p~$Yca9b;+;VWhiUvQU}^*`X>&*uXZ}5hu_*1zg3y- z(#a-nX~UOx+1Z!Kyn(KonPB{Dy5!VG=d)Ia`|cUX6s;2?;;tD=SM3yy6C(We6N7SE zX%W8R^eAtm?J_E>2AoQ|w8gB|wj&=&E8K;~ukU-Ocyq#@;^)R^cyj}fu}Zx8aoz4B z{32J!5-$AfTZ+3Tyzeni79(N!!Xp%2($2)(eD|$fd|};}!PS%{6EBI{DbD{HiM#?orSmninvRo2i3;(zle#niV)UPSD7xC-S`CoXiQhLg-wM$n=?( zBIP-oR&9LHIq|0)>WtD2Z9=3ZpAsDs3mW;US@I5O#Xn4M50~1{DYGK>Q*LjXVPlhDNrS*O~B6nV2NF9JNyuYe9kJs&8 zS7|*TY-t^&IX(~F4UIEM?Yw-1jNc8__aHv(}u7g2`=_@$EeMYur4( zSG99P{_;!fxdQ%cCq%qQt(>QFfJFJPAs+txP9Fq1CBpDdfY*2m1bXiWe@6A@_7DCw zTUB3XEVnCa1vhQO6`!{8THsa&4BzNU-|7K5JYN-H{_G!LwF3w)&)x9c=wV)npSe!0I6aqJKem^4YEQ21xi%{X@VIQ2cJ!$>YX{uzCe*u&25es( zw3EDPuuqpcym1t}b)mNV>+FYivdZ7t%X;a!Y}!218brgg*z45nw!ql+$?dya)Ydw& z+1SU}PwJtHjFaHmm;1y-HZ|5<5bXpOaWK6}Flt9i&eD2UZqClQbi&+PYf-z^AXk?A z)m>Y~!s?E%P4Q}%cci&=9)`3IsPVcrVDx8qr0{Aj{AcWgzc;oM9z9KvG?h-_05~4~ z@=hU#H$D6S7i`UTCG1JPFAePQcuF6~;5ZR>*eLjOHT+JzEYunyW|#mj_-?&iSTonN z>+#b*^#|N+Sx_AifI$_2*NNIG+fbH9ovU+Pl3z=uxENC})&q!E^@y1of{Jq?xSf~?-;m#U@`Y*fz6xDgGi(53$g_rxHzs>=3PoE zu*Gy($fK62A?~@vf*y?Q5h|rDSGKcTx04)Db`FAhPDWX_Dzy1wAC{g*zOXTqw^rDT zD5L$QiK!mlWR7z6WDcw|LGsZokEtBfWSdOxpZKat`p`95Ct><*KI*LL%=m@GO{32$dpV)hDw>*%1}x$fw=^>^n8$IkE@Lbfjo^)k=)*&*+i=al>CHo z4!UHt1^`dis-^@pq(|A%4~kdK@%vQaI3)00s3ac}8bA3ChUkl#6T$sXStRf z)xHk-0#Cl|?N7YnllU6pRG}1FJXC0@B-#<6P6I*OsMs>~Tw=|D{eULrTRo2$m-Vk$ zf?G$JYot$=_%CovGt2aU%r^gLD-Sy(E8~BN(Eq!oJVO6_gia@9Z)5UbmoEo&j?zD%q0Q5X6A?)I3UR=SPc3`uE=xr9|hvQ>mD2o*dTfy(osi^NBFI=LU{S zO?tMRd}z)WhX%hs!?jSXp;Zqu1h+-=*~im5HyfdDY2sqxto^U-oAo=} zW@rDd!D0PZMGfP3*3J2kx%K~9PxEgZ_^)vu>HUP(e%B?>DvJ@JMuC9n6EN z$n0XVT67{C(pbeD?TG^f0YS-f*;%@FMuD)e=CoWpID5Qs!XYB6{A+hjw#!@X4JffCztm3 z@u$zHKPGGl$(#lcLqeVm8fs}&q_ot;I1P1GrM1=BDGqi?spHfhg-o`~@IICDOg0|I9@UU7F}!20(pXt1{r=X2J>o~jcV(3y&68rk2`RWhSr5&dRmPF7%Z49S*uyQSmRDW$}#W>>X}A2S07Mzo;7O1 z*9E0pjlLAK|EpG-%bDHDb8XC7u z8{SfIHd}P;a}RMZKFO?Q#>NungX^Kd%Y{Q1u+@pQe$VKMhBs`b6KY)_%rFwibmVyc z7e*irw?P;oSJ3OrvkC9@-uIr=6R0aManN0{N4MikvF1~L3z z#!qxTgqokYelZYO5YhT*3q$8dTZT#JIGa3|5q-Q^US{ftXEJPW7A^d5XXj@w-ZX!C zrx<~4%OM`-kv3qiJdp52{lvC=xsy45*z$tNhg@~Ro$^P^19MemL}iSm(`wJ?jdYJg zZQ5p>WGrQDFn2VXu37qP(<%`HrS$2;87$x~Lc4yUdfw^XLq<>9_2xd!>es`(IU?mo zAML*UWcoe|bVSgLVi?2_tmBJ*`U~>R`!4>D5o(+qjrU45Mr-OjzBzhssAsTc9NQ3{ zGc;|!(ty#x-r%*$zBa>|EA1${39w`OozNi=fLc9Y;QLX0w}0A47UT~5$)%i*mB-26 z$a*(1>6H=J*nZx3P9YHynIBVQ0oHUwas822WIf00PY@@7!!MLqz+gD}1@6;l1m`9k z1>&-qS@35e|KQGnMi1VP=7#$CYwB6C6NW!1Z=`3iZg=JS>N@P%8eI4cW~UCl+yK52 zw|fWvP_Z=c1?1gl;5{hj7gb9zohi=h&^4=iy?X!C=D6>uZ$DFZ4$YyNV^(V#TD4Ke zF8Yw$5KN>xsy?O1K_G5i`nS`)zK*45AA6O)31rGZ;}Pc2T?X$H>Wt+U_~RO_LoL)$KwO+44;JM$g8$3gHIK>=o?)F?DWhL- zC%p^FCzp)Z;2(KM`ConfLvHz- zSB5sklTL%bz|E*U!z@D-{ZO>ig}j;mo>(=Z*$Yzfgy)IZv8)t9Z}#!ID`8F=Uym98 zr0@b?kLWqlJJ2)bC_hP?%@Xb9CGCy(3J{+J8i#FQ>dmAsHc;~avh4+$5ivW&@kUw* z;kvOs&%61MLHYJY*)XsmZ705jS{q3hj68y{TdJv5B^1i-?2Q`?@(aWH$Iu`u)$YU7 z_(~Z(+CGZ3u;NV8Dq*q^6vE`YPye`VL9;l4y$5pp>wK-LlbSJZ+UPu2G?}y+rb)59 zN9aidgPOV)b>=`yv)inR#|Ojsj(ac=^xIjxZKCyIQR?sr=Q5Otvs9RugCiI6R zJkM|3n!tCxcm<5YsDRHp0d)|BRxn=wEhBuxOT(bal*y<`4;hNrA3nX^lTttFJ;OR8 zH-gOyAh}?)ccxMprGS23o}O=9w}5_Eo-u$>gLTgMX816?&Hg1SzXGy&gBy1|mNsP( zN+e}vTU=RMR#8_?O-o5d#VCP+Q5t&}(c?jn2^JP)`upU;Yu9(+vTCaqmBEe$=b58c zq(+5?J7JKViJ04}aUMr^d1SQ1GEGT%;=pav6gRH1$sn{m_{;QlXs9Wjqoo*>EG;VqViZjbXL*jx?@)WZeFoNol?3bza11BnJB}Zvq;!FqC)TeiQyL0RIePMJ?a8hfDR#Q{-f}Q-U1LZKoTTsGX~ec4 z>b1#RxkM8;8xEj<`Hg7aiEqOt);FV_`kQAQ4-RRQ_gB@0%4Ks&2mfYDYo5LuN$aB# zy(QbP+P^x3lqckjQjhq^yl%Z7r>jCAm~H)iUzC9p)Uj!2ss9M+B#-!eGM51Uk=?Vu z=3=jl+ty-YGNuDe5_NcsO5$RF%4y^hkm=;V#fW{3*)2|_l`Wub4*x_;P6sUy9(;_E z7CD>vV)$H2jqIXQKqp<5kNIvrbglBmjF~X)D|@#~=M6VTG4n)I=&iAN3kM7fN$_T6 zBj-ggj&;x@2uk{sIois>5Asr;{O#@zDzZG&P^KUT4xpm_0{e7LEHdk30e)}m zWi5-{&W}k7(&;K7BuzNi^HTldq$MKF(-EI6*Yd~`HI{P}cDZ^K&z%)^3CrKj0h`_# zwvO`X>uU4d6XC8wQC)p966@#KA>7UHs`m3#(|iVl!bUUuY@?!aqPWKp{Ln{ z@66B%anJBXS71D3AVkuryE`wc-;Yf6)D(o8)qrfOZ+Fnw_Xf*pwGs~eYbp%9VPs4h zfbBuMGWnUgzv^<4exH7L^1FW4fPPpe$LBGXZk_3g92jD~!PH12yVdk2^)cZ5kjyXv z!F~$$*l1&3&?JI?oC;_-bNCh6ZR_p0zHI*heCy#^m*JKZs#+;6;4_O54HTN> zBhT+B$iQClBR1~_GkG#?jri5G!Ir0~D@al2?_!kum5GnS*u~%r`P);Rh4^7E;4Weu z?v^-}mCz6-Ei(8nCM%RxlFZrQ8S5@8UoCcj=+##hllDl?=mbJ1Ewws+ZPDyu~~3#(4I$b@pL z;lDUZ)&cy$l4sg3IQxMMtB;}QrkVKVc0I`SIrz)?dTt^1Cg{h?&B(JcrB7kaC8i&$ zU5Vs$gt1vvW2VlK=oC&#yJVwJgoTUMBs(`@!CzR&>1dhM>tayQPfb0j8L?eWqE@ub zIc738+5UvTtZUrsW`jqX z{h*1wkIKkTYf60Id6)I!w;}gb? zSxTX!MRE|E^mG-J3`H}8#_MyXcv@VzK}}_>=Z~dHOLuL_=3se}x3J>^$EV--Nv= zB3h}EMK!ZBdZ`p!Ij3TaG$T_M1(2HF{q2-(q!}7=1T#DQnO2_q^G(G^Pc)=*kXw+o zk6XNiQb~Oofl*#!c!0Il%t%;P)3st>MJxPU3+NL}RDmhaH&@@1kxnB}dps z(pSg8lhJW-NP~)ySaEGQ7JJzslWqyh_SZs_f`x!2mf^$&%0N@Fsdm|x6=jCZq?=2y z;g+>47w9Mb7Tk8h2EV*2TAItYN+=l)$#eqMW~f}|Krs*uLX>8?`X*KOA?l^C;0=;N zWx!p)MY{Kq7negzAH9BLk@!!^Y86j?f;l==G!PJy>2SO~BC_o@_c}cFpZe z35J$L{_@A}>+z%{WFm`PslKF@P<30Tk)J$3Cs3EU%)S4+)XHjx$IF3%uq&!fHIui% z_}Ug)f176}O@o|52fZq3?Fbl>@tM%zI>rDknT7pA80uCwbQtu7Eh~3eDN1yuwg1&W z#iIcBXydjhB;viH^Y&E2xQTC0qh-8>x?0>Lhe3Ho z#Clv!R4s+y$_r2(og6k`@CQX^k!l`0M5qReX$!IVDgOsN=%{o6Rg%W8Yw2)rzXKs6 zbXcdMih|ThQMrYYrlO*yW_f<~W_1x7)`C`%Vi{ZpuUleErVbFsp3@&Y=eF~f_z^SN zA&Jjo5hFuDLb#}Kg#{^gMEHRz+g}F5+1VOm=nX_@5^ImaQpi1pnicE12TJ4Siuv`x zK$ZUiML@d0iqX|t%t&2}8Le#qACWZOVI00A8ktkRAMZd>$T9SkZ7yTPvZAuZWo+31 zbFbv9v?64|qP9n~S;O!eQBwrK4o$yCsSzp;;JeUfnXwaMSxabz&<23V2%B#^2DCy* zgwPA6mSYfMLNHfwg-MEr*6cgG{dJAj0f@boax9d{v9o~2yKH%li1O@Ke1+mCFI-_P zbR`u(;a_ke^)TW5H4S7mIysiiPPsWY2@>J7;wmFw2a*_Mgg!@A`%pOh)h?Slxb1ObCvxU$E*Fb3EcfKYYO}_ygM6uk85n!yWhh=}*wN&jKfMD9oS_deJ5+ zKO~e0k}y+PD)b3I6I50MKi_5x8I*d>e1#&UcX)-H?e>tDeUTZ!H$CN58uVI(58y@c zJOeaomp@hRcM??FVrm?eEoow-9)3I|NBMRu@~9NCYNCHt|=X4eJ@ zdj-p8M8uI!t{h3`=!U)gu~#mXX30`*<@7fm-A^GauAs5o9ftQMAo^Rm~ z@+>b1qd6#Hp8tY#{wUYHek{-Y8{wHlVqpYBD8B^Em(G_vU_zza z%_+UVa0bIVW39S%M*v?Jcs{@c+$mcyM#z6h44Yzs*mW^BCL~`N#R1mhAUeHuVuc+y4QZm8oZ>>p9{F1o=Ka(xb@z5&X{3ynv>(s z#q7(kVLjoJ*FkU3h2G9X6?lns)+4I>0*@53F;z5B%k^0|xX<%!wq54F!FIFzQPmFH zeeNgHdsWXEciQ&2_xWElo+!3!v4Hb2dx!ZZH*;a(mcs3Yj~E{EY8VZK;EIz9yMIhzy)$6S3nQx_Q@vL|+1 z)P?F{y=g6yJtQ_LdI0&Mh&B7X>=D6jl-OhPFgwd-JCJ5n=7xBf+GF%yZuHstPrrHV zBS+q^xw2*R<|R8t4e!(%m)<$!o?WuyRr|BaPp_E1VcqHjOE=zed-nxTnfNOwEU(tO zEM~3AoqyNT!FLFu;a`~fmfD$TuAH}sjQwU9`!tNbfJF4B)TwpcZf8oEam-9+Df1HZ zlJz&j2bK?n4?X|w`XumAhe7b>d&`(A{}j)efrXy41Kpmkz{Q@cJ-2&r_dl&Lt#|DA zzQDdU#Wi4>;~?OvDp`Ns<@z8r=RVv-GOiqQ1}4;L~@42T3oxshP!PCZHGXJ zayB8D|JW!Jns)3M8LwkU=~%%~5axVrjCkDTuz@9he2>Q#@G}EB-xC=Eupn^IF@|u4 zDF>-!^@A%o`H0zT8l163k}4 zWFGds`ZUPA|G8xA6OUa3d$uZ>%j;M+C#$=pt`)Wm(?|(1)2f83mUaR{cM`W z?EB^cd|>A@t|WBpAlR5kQ5qTsET;|Q9QNUydd40ZTxj-~1ub*0&So?VipW3bM+KAK zu6W346ihn6n(F~;>HuqW$cfL8sw@u9VRJgzSMjU5cbPvb{-kPrF4x8E8U?YLmX0nkIFGv;uv|2q6PeQ=3ra&D|%7*#KzgA4Nt|9y!ZCTZ2 zj)5BW7i@XqSJ*n|qa8awA{T!0`OI-_KKMK~XO92!?*BS`*PTZW10Eg99%eHzHskSM zr7LZX-X_L!CTc~U;~x_rtNeM@>(*Dt{g?GU$9oNb9QUR5XxZoEPFhcteK*dcvnm~m z2F0wyQ;HlRqX{;7uQ}@bye;Cwp}*uwY7T#%vtJWA{;B0 z>g&qfE|XDZ*H@#mlA~L+u9+U_UIAFA;*6svO2osuBtTLtz>dwho!bF#D#DqIcJAsqO z_(5669vnP|$bh#&FlYvzF0=As7bBIr)fU++UR62HB6_3NsI$QyK((Ig0In1vu#AJG zqtO*WPFF*Hjjt|%J?`4Nsz5~m*_zDMbvWV?A9jb2GI{pY+RC)vXR@CmCn$O2!L)9$ zRRWUi4ms+)Rl`_Dyc=uVs2isWFh~PZtJVfV6~QMLZ-*2F;|T(dw*ktGw}I;8CG|kh zkU~!rvayk16aoS6mYwEdS5X%`b)wVkF}`&NxhFf<Vh+Td-r`i&YGEd`N}ijx$y}$=>s8jo!{*c>f%+^$@029 zugU5PUv$Q~4|arXMz{ZQu-WW|fuhC>#!pYD#j=%kU7Lum*9Cg%Jiyl~+$B}Mt9)0@ zyr6tR&D^WrrQD@v*Qz$Fn5C*^>SZ2wyXSr-b75dN-ovooz^VX)Fvl={&|0at#bgiI z8T&-RZg&YGOCn9JhSb8MR4QE*O33a)BQj#+#GJ=a)=YR5<)ch=VNN}%Dj}JM@`=Ue zuFx-8$hDeayOt9dR2;i@4*I-+r!+a-K+hT z_HFG^t>Qf6<;I(h4;Ww4y{A(;)mTM5BphBR+0`7UQim{a)7Z^qO{h&F=o16@UdimQ zjYL$n7$c=VDClfgas&8b$(Bl~HDWCI67uq1(c9xa=v8Wkf{q`B}eOsi|qte zIY8N{;|Gt)$~xgAr(1kRw@0hfxitZ#_2^}L4+enravL5pgv_>6)sRu9UXBm}$zGuy zSharboR=zX2Hs^5|GxIdC#Z?HoscBEgc^5mOINTjnH;NYJbFwX1OC{* z@Nxbt-^s2#ebIi9`2`^JlX`=QiOes~{%L1CJi?2KyeEd2?4EMS&W|6({xLp~sgJF{ z=y$!D!Dm0Z`gJ;Z-J@I~cfEbv?eDyE`>pS=OKxAVukqwo-SuxC0vg!Qs5mRL3Dh39q({#)$gN-mZt0_hogC#qMd@ioWwQs%IqS&>nN8=P z56@I(v)oPW5|qcLr2F$6ap&diBaZu>1I#|h9;X^HJhREM&GDq;Imcl~#-ZN9^fQMT zR?VsHF3x4oV-j4RJ?^aHs_bWQXV~X)^KA3&^M(0&%kjC~O8W}u3SmXwh1~h}+Z?wz zA7CEk9e1NMIB)56nvuR30K{?YM{^Usc>&ZN%aaU_|fBk8V=N3Yu;JTS{IK_eN6sg3{v7$}EaiZO31F@NhkmwR@E||p@!$`%|Weh3D zkaCQ~o6DF1{Gen|h>Pvm{%ad|qpem!rXY*TNwmAFx{M~1auQxcW2k|~@H~y7G7?J` zhqFu}*~>R6wka5-Xi+eVXYt1>ZCJ-Rc2wYxwz>I1k{EP-D)57CZr3sS z=7~?qP2`fL9U)ulPd-rx*bBhmu`3jXN%c?o7a(?x03?R!il4lYO#bVMskXMYYfsNR zJ(F+&JfipS%~K0%jvrCH@VcOiW#jA+3(mJtdF5a^=2(l}{uHMSn(ub8p_g zY+&0e;=B5k$i6(xcn6PS)wvsH9v!-wvXl04!u&+zO5 zQ>%BCX_dFnv>9#2o0-k*%_ie?^)~fA>PNl%K`yC1gnN0jCw3`x>H+-tQydr4$rB!u zQKxjz7DS8H=(y330K*nZW`ABYx1c1l6@YO4TNNxt1bQ|!f68wzR=t3@wE0i<@o#nXP++vWN-&_9?(rK(y zty4lP@k;ag*7HMGTCWW4Ge2wHA9^YDe#l7nuasEKB~~p@*F+c$X|ut=n?qW&#S`*x z_GAC;AwNhVPbi!$CiBUKMby@CO1hH5!s3!pvc#&9r|A@m+vQ0*4MuJoWev_ECw8W- zPN&Tpa$8ICBP4fqJf05626! z{(eerR16+%Tg!h6XgV|@B!&Tn{yx{%J|ULh0DzoMD8Q)%c&ZVx5-Q%kGV ze4VkI%msqqZLm8&rGgR9!!xOH{3PJ!ZH0qhT}q{< zm*S8LXvku)YbX)5Fgs-eGB{Mw0#JaSpGxi9QlJRbc#k9FJ!@{ zvk?33_oS<+S6DH(@22*-Qfyq@i}^iWkIpdLEso^OH#!%bbH+JWm0b4O6>oBZTGIP} z4!B*O*-fp-As{vL<8Es6Q5o07+l)l*bnH zxEat@FOalYr`2M$&+7+%n36mQrKD)NDb~B^;1_cUV_WF5*_^JB`Ba;p*UC1%0UH)-3_FKkM8jke?|+Hju@em`&0gl#Q@e;={r7hF&vZW#RP z)Y&@*&4)364md;ss>DCvhl1G`r0HU?AqivHu%LW-@%my`m8>bAQhc_1esQ1JmpZ@v zn)3VeA1OW*dpGd5csTZM!PhafDW)l&5SSP|KXqAPOX~W-F9MIIUJ+jnewsA+4`jbX z8e}3oL!adJdK2%{kYzZKR+Lil?o+!gBwcGyn(cOr+2=|H#QbDX87fGH1BIv}CJ2_8 zizz_;22_JbUvs5=r&k`Dl0Mn@`s4 zKI`F)nb#O?X3AJr^YTy5zNft5PC7x2ozIoep3zk``!vB@Kx|&W- zmsu>sQj@XUEY6%)G{(f3wz@~+dZR^9tvTnMH7dbk)JGqZ@ZWkdgJ*_yF0)quo6Nv{ z_h$xP)oaayE`+CK5XJ?Wlgy@EYZDqq;}#gf&5;&sEJ~+?S16&iS}imh+G|1qXs_FM z-+nx8sJ+B{ec~uxeE`iJ(^?zD49^W(%FCE|EVm@u;$SxYU~B){o5^M#59tMH?W_2V z`|ihQ{03U<(uFclW#H9jvR)ZzGanV=r=(|+nvhK z2P4bo@whE!v!5w|z9@);NhO)cU z6S{^uKk*KoAc`FY2T#z&nK@gkjV=@cDkiq=h$TBlCvRK(ueI?1MMo!3tN6M)mAtO9 z4F}m%mpcwo?5XP@SvJhF7Y_b@?awJke~E-mxR-hh@q_q|CYt>+q2nDlejfM;Uy*s^ z6n)@V!^6#J#(3AP>^^I`Dq=vTp`(eHyNBFb|Us|%K< zmzG{&xG1r<^y&6Ao#8imGPUKo@Ywe^R&gW*HrBjMv=E-aMg zw~v`drqlk!paQ-N^7|NdEgt267gDm9$m^*huAYU{s0{w= zlnrsm74JU&{r8W*d&O(lT=SdXTyxE9%q!HxaB|=5ajA3isIh(K z+!`;72AjcjimQXvc&dz&={?5po@k3o#j$WqYU)mGN^DEqlXxt4I6h}`;|*j5W~y}N7N3cuYQKI4qbe)b&Et!KfwTm-zh2!{@U zJ@_38044=^FIkG~$$lq=Hh7LIKlOeQ_y_Zi@*B^0f$v3)j^UKpqYGT-*{)QQiLd)4 z9@%+2V=tu>Up`BIk!)@i;DQ7yC@4a{e6yCSMur4cl6~exn4~AgN<*ZVe2F5^c*3zn z!WFYxi*lM!(2sF9+J+btnAc=ct-C~U`_*bfeBIRbs?Z3&|OZSoRib50%(TRQUWN=5l877#o5)q_1xTrkP+HtPMdy|ewf zpU%2!1)WflGq{GqaXXJMdHHT-60vM?<=(NWr z7w6_YDnANXJ(fV!Q|_7I*_YJjTjB%RzexOg_s`rw0Qu@0-8TmqjcMQqwbY6+T59o# zmTHL!E+&lpQHv?kNc`R|P(>3_lNWp4=>qIG^FrabBMyk%J~%pKQb*(wqPA#+sI(DD zV}bzmT4IXkRy6+G3S}Ev6G~R4ow}g!R0WZE2&2JawXh$HRH}GJ>4l#^{>MorQ)kr7 z&HMxF+wPtE$WJrx;v<>9v9jj%tIqmqw90LrJ?s32rFRnwsU_!~uuD)FFOrHM#gAGZ zv$CRA)QNi0AR0xJ$k(7+TxF@TE@xJlJ8hleC!x?IR*MwCvaiq%hAVGK8(72iVV9x2 z+}a!}b+dmbpLiit}#ANxM_GZWF2s^`(0=xzLa z?_0ia(YN?ppEio(zPLYDHOV{A`;dR1{~h!We#iF({@iDn@5g#dEY^EW*klTr7*oP( zGFhxXeIP3H!O)Bl6H3HFp=it(NS9MeD=DogDJie;rF9CLR+n;WwSx2M$l#L7Tv#67 z1RY+Vr!Y_EpUD|XCYgvQlX-Dp;Xrn)`BdVM}WW^9;*{wjq0KEDmJ5V8Dp{%9-^ z@cX^K7$)f{UazOBl40#J4^x4r=FdHR@Rb)ZJ z`&+DstcR^%TRE#x{i|+Q&Or&vsaxQWxj=-<|J&9=vMrfh>*7Ck6FrvZl3gY8(q}R~ zPzP`w-IFvpya{Ot@zl9pL`m0=^X|3(SFSGd7$D?2IwcDj`Qz>`JgirpR)o2GapqZ` za{0_}sGF;tgulfNxcaw5)h(MSyT!;H&UOo4#2h~97|V$UQ_MRg9-U9Hcm`ncX29ZR zrc}DwQjTkaRpIhxN#EjVDQv1bOTWm`R(MubyMB#hO<{Z0-G$dxJs29W3d7R4fkLgAiJ z5(`7_-uQAKyoub`%Cq*YPZ)my$FLeWm0gzn!n6}EU{wNyL>LLvj=C&E*d~y%B{ylF z0Eywuww#Jb)xKk+1A=NqkokE@C~4#+X8{DZyhc7`@i+2Dg98G+tueO*t2M_C>4GYY zybsQj+XqKucoO2zcf-Rp4bPK!*Vrmd@Q<=XT2E>Auaed1Y&4t9-bS;+P6FQEh@l?I z3RyuH+-xQR?=sos`XL4z;q#}-!at+a)sU-PUhCKT1KZTbWK@)%GNE$X3=8n!+wnG8 zzW$AbmuK$JJbXD#pYXeRTPfa>xiXNOVQ~k}!Q=6`a|lG|Uqoi!;NY-5_grS23>zIv zhQ*%^={uP=eDARC!=ibB^R0mMUa;d{(ImL98Wia0GeIWjeQxlvJKsugR3HPOcW z+T;Y&Bwnpa3!*pZiF)!q>98q3souyx6L~q!P826c&l4{fw-nth-Y@PI)hV^tAtVv< zIT*Dfi9v%Z5{+y)&sZYHeTkSO9#aELE-oo?s2PVk95v|!`n0}L-=bfn@7AmI1Nc%Y zQ4mFD-prWH*O{L;ziB>VK5kZ;-DUASKc)_)<3N)_>6DFQeRp-a4S3&@-I5A z>rUMc(Y#UeM^YA>R%eYRqxrUi0vy$b3vkMkC_oWitU%t4OqUVLtA^;}thVSF9ZThq zr`K|LV-1#6=CmzyPVXX$7oxfr!{7Dd{g+-a->h+ zySHY3H(O?NdoQ?SQTonvW^Y|WJ-QWP@2bj{%T7AZSXC{Z-vFJk8ODA-jC~MwNmXW@ zvD{+u>4F}AIShb@)hPeSFg}gV?JT!ieQqYE^SBwEnykNYhY|K$Ekfx2jiZihAe+o` z)0(t@_+viJ+7?J_ZQgd)vNLtQsk4i3!%oeOx-P2?W_m87@)-0?(u zx8q=%lbwP(Lo(+Jbct zNS+BG zZi8lj5hF3zF>DL-Cc~JRMNButGS4sv(0DWjzlZ4h`L?yxpargwBqKF^tiiF|q8HhN zpB`gu{oCt1|NZk1erWWm90WPt@G2@48&1Lfhc&kHPw?ji6ul2}TST>^7+JSjkJ@cmc}i^O^i)Eaoc+4B#%Q&duPME*y!u%TX-qM@UB)SSSY8_SfF5m+>~SF{HsW zG&?kJYK~}*YZRLDSga5g@C8i40B(>R(P#u{Su@j`wtQ_lZecA##gy(LiYDI1+@v8- zcJ{+1OOC;umOV&$AK`*+@6-(gqq$=;Ido$6|Hyj;PNGPxW)hq;{g zbNi@eioI0feE=7@ZD(zVf<<)5;GCwPXy!7U6}F%14Auj0;u2suBHm6}z9>f)pO9depP?^e!%48KB6+lKp*a9XH1L8;+z}-*rycs9}EP-xG zG5JC%uQgYVV(;#Iz)Rdt!JY8F4I~f~BY8?CH~tLx?&8 zFxP!RWz*kD_(A3&!VTER>0;S2qABwy!d&o%%vCfF0|ub%VGePRBLfofNh$lP=|zFD zeCqhr^$-3ZmJ^N>g7Rg@ANW65-gCU~`ZND$i^|QrEp~^)#l39#r|DZOdza>B{e8^C ziib7#>3^gAjaq#vbG70c^=3UFv;8J!yFy*5tW=k3>hv{ysio9W<5K4{Nqw4+TB44$ ztCmqcV>-z1vh1?%viCaVXd@RyQZ6_@DnU*ol>jzC_Un zIknYf;E>80~=R_klTPZ*hY6X%ZG=MQ=y28nbR_f z+ng9XdmUP@nOHF2?uM8j$bPrm;%an2EE#Q{Mu*GcYjlx71LRA#punv!NURvhzBioK zSxEXMvK#`OC9A zzsIWkpZyCn@uAE|J8&yDViTUfBlGdYZ{UfUS3mqB^FE$Hv>!gVo*{`_C9evpTt(k<>nf9`Z4Io2>otrFNdT*?oGuBa~0FyeQTaV-VQwi(_xbxL7O! z5-30E3nh_`sy(i56*j2?s%{mlQY9Fb3N#=cgM$LOIoancb@_aP%NKMx?F`0#AR{mc zTwn$Hoi3Zx>2f;qV<90Hw8ivnOcx3U^?DtHfFA-|iWRwLxcXhkU7U*uES-~yrR|IC z2kopK(z~-x?A(tpMGof8-6=ZfNu7_Mwo%_>8!^%A#;)i6mk!9&mh7IkUiSyr>aNZC zA7v{zSdz1Maz4mYbD8_sXIh(_HiNnKg4Ofo~L|8ayB^x*~ab(U!7Xled>Wl=ndXeT8bvs^ZR4MjL07 zp13V(D~O>fMa^Z+6_cx{*0e>=j?Rs@70oK0Q#P-1LG{9#Wyxj5E6NsEK2daU*^bI* zi=HaoRn}j*ujZxXOGTj~eI-!TQ#INmb%cN?b``5j2|=_b^>KBgAW>bO*O*^ZP*XiQ zGAVgYWNZA=}5jmhf@Z!WsM;(qi%^6liGtN&T_ZP~XK|E%#=R94q; zWu--IDi#9s)i1A8^(_ZJ0K9zeF5Pm~0KQ7HvQcLhC^IF|gfqd0HOA#BG=ST7;bS6M zD(5OsN6E%(F;$e>ky4~Klt*JiEpU+`6PR)m{@cN$gyA=)iA3cGj~*31aX~~e!uXRH zMwm?7$V@0+EAOYA7_JjcjfKnnmd0cr1cij@RQMq%BSD4D!#9?Zpu!L3Dr`<8Bq6A< zISc?nkhB30!KV;@D{fP$=%R?ylHsKtbk&5sT2h`as*Ew^ge4HhU>(I2m~}m~D(26r z3)W2X>Mc&KeM(hD{!J5#&*-SPY0NI?fd{_^egQlq^X4CiIYw}{S#+i?POC}h5`_xA z*{pEcZ2T1oJmqu3LoyF#{+jtF^B}Wq3@2gG=FG+1BkX37&S`w+K9tOUyUSFaAe&38 zOw~`I2R%mU^Jjd|w!EPdq9< znqcM0bmQpBba^+N&i+bnBb>bPZ6?|HHgr@La>ol?wH9)$m9Ze`1-q!0QO1Ml#*iw4 z1&ttX)?$1|Z6_B?q#9+L<_I?~Z(G!jJlc9)*s`4K8^9-r?Q!;*B?4`{q=9Y zNzT>4{hXCKjG}0xG$pEzX%c?6)whbjjo%mH)B@J3eF0v!3D}EtiRJQ@N~1-#CEd`>yA5w*zlg8wzVidZSceiATc9dUxIRH6XZNjgwn`C zS!0gXAy_}OYG`<-8$k9;-u1-ff`xOcYh6BfxZY>hnw{=(D~l5|8as+zlVDX4Xt`QXgPcy*g^c~lSI+5wVq6}tpAGZ#oV2h0HrW2rLFGcz8tV>=L9 zj8|K_gWci9&A-OK=6`GXZSeK*>m|P``&F|EbaQlDi2Wf~H+64|vU{7ly+zsW&D7#r zQ!QVswboi{gSBF3*z|&5BVQkB<^Zx4@6}cDE6?HgS1V*S5ePbsux_M~8gfx!8?dr+B()ptz!N$a(#n zQ=He2<(${w?zV(U{4v=n5AlEP^3*k)=Khw#dGZKQ1pu?w;cNhGu{1O{0q4dfXs!gNjY-g42}~Q4pxFkT z8MqTE?RT53F?>npk%$%Ue?sOpmtha~ zEhE!KpOCu|{9uBy+aMu^FBERjQb3g zq1U9Sq@SVKthiKvN%0Q-F8!Y5uafU1wN5pdY_E_LdCCe=F)kipxTg>b6@uY5fF;RI z&ghBcg;2CD5%eKs5rx76mr|qFhR6mpZ3P(NqWcg%Jnm-6khV+q9{Zbi&MuU%--ln% z?E{)d9TIi?r&P78BkKYNkJ2?YWBu1{r#P~W`ALsA-;!*V|TU= zFTSD7*FNNiOk?VO7^E8}Uwev#Pd#?+hAT?#E}PnV>&i77@Kw}3VHlh=cC+xNRd+hn z4vWRfI=d!p8k%2jg}%KVbo6n|WU0+n;Hq#pMM}#{D=Q`i7c{L1b~SAXUevTj+R}8p zw7u!crUOl{m0C@xqI5#(yfV%dN>)rPYbu{x{6gcerRST}o{*=wGt^mpQ`wUR4_AC1 z`bWV(Dzqiz5Gp=(J%{WGHX<+fmWp0)K=cXn0vv}}a9sgYRDcT#t}8AmC@S{l7bDrj zLB;jhWy-qY1t9X;oEb54s8CUijU{tybV9+VGO2=V92Y7^7UU0xfJKH-usFntxG0t% zj^!s3LUAZ0268K#t14p+jg4wGACoj{G{9W2Cm3{TfbY$JS{yg77>$dS?8gtG5U_Hm zG^2QNaZfReilyQi#q5#dj76atd`b&W{!Rzd)L z43UXHB#j(nNlvjuI%NqWL2lX`j*Ii7+Ks&a(?l%Jy+o0WVW2{DjocuQ4G6Hm*!vG*L0k& zo$fr8PmV5UR4fMWQh>{(OKo00EP20ie-}|S3s+b-+t`375IHl#=0_GAmRVOvUU&YF z<%H*WM4ie9shp>^8P&F+C6ymHX%$=)6%<6G5nD7GiG(B3a75mD<8gbu?ugZ5vsx`0 zwK^iNfGLPZ!d?Z+b6YKDjl!soAd9AeL(u`2l`LkJWr13aRFP>O(efr ztA{lqa&dr0MzRRkIJCcX48Y0y6O<+UWpalblEId@QRIB3(cB@0D#g&+@c6J15zQTu z;ly(Z7MXRw7$tM*I=1%cx!@=Q0e8_Py?|WiGn8k&Izb^?3TJrN*^qi;5z|O71F{$cm z@eYwm^NoB9&ra4)jLg-w>E}is&^-`&M!8?F;lj>vOdk)&A{7y31**OdfoSkml&1-2 zDm9hhl8VBTl60Z3T&oVm`2s8UJ6#P37gXd2d^{WU)Ws^&v5Mv8<(xGbHL{)o+$o7R zo0Z9ravJ~g!omVS#>m|ekD1f~HKP{lujoEyOC4Q7$PXRD&`6oc(UGln)w%Jb$8?Qx z>gFg~PVg^`*>%^XMLd`kitGuL%6@?oaOGvc*c)+#9AU>8qk<4W(V+{O+@?BbS;c9q zOX)s5j_kuT5ATqr>IIixd~nefZ(Q4Q)nBfD?P`_7NGRFt#LBlW>^pE)1x6p8`O|r_ zM;+fprtR^r%x&crGp^fp)g4=~VoP^1s3!jB0)o>wr>o=Iw)1X(>sv7oSJG9!PO>Eu zF}Nz5&mz#p4WL=im(0%_-!%T7KVtkdf86*FUgfk7;74{BnXqX9yFmc#LZ<)Wf9c=` zutPHF%8b7=fkOO&fRwqzs({Nw*Pl2+jCzpz0LnWM#+(hp0-ELUjF}y+YMD_9!C4Ej zI`fGgFkV!$OJ-EoPAAvk>zSgE#c$VJiQStaXA*Jb*Pxovw^GeTs*5xiRsRORE-GsC z=N7F9uSj30`dPzOO%JPn(eUdAZ6uwL%F{JcTWn5^GE!7r9IkEx2B#sLxdx=-^78F; zH&;njaX4ID96=fz+-!>AB2E*oX7iQ9pouM95Rb>83-Cc|KYSn$>hN*c= zZ?@STf<3-aINjaIUUztOn#A}Bl|J85e$&|-Sp;J2sk zJ!Qx4L@ho^S2nP8MZ+kVd3tC$YUb@xD?$)zV&CVcAv3yM3LR9wrT?3r)oL^mWVRtt zo6TwhV}P{?YZsUaQ$^|pxJic}L3-vEq{Yu5GxI1?LyVb6B|X}gR$`@KK{GA{W)i$C ziG(zE$-B~yl9g^~*m*$q@l$n$ET|*^9jIYRj&r&O`~Jy3ht4d7SnbKbhXk|E!A#01 z^@3Tj;(pw31waoJ`&ss5_A>?fWju2B&lS>m%Y&icpueEM=s7p%vnLDYB|8{}&d9ku zMw@Gk>n41M`VQTV@q3bY7Cemajqha+YWM5+C*RQiCTZP>?+-G?wgTdI^M?Hc*+18rOY()CJ1cmV%F z;u8r6AFF3Sa@RLHS{#gH03VZdrD8q*QL0`el>B@@{h^c8Fz+RpPtW5_e>!yb<1uQ5 z78MkG0%kj>_FF^=d2Mb97phVcDpJHG95DMN?6E^opiWDun1g`W4MQueEcF{bWzR~W2;&zf}CwX>~H^z0* zigV_#%=vTMOsI>xa+EuAjBC=`dG3)NJC2;YYGHoOySLo>PEEq_^Ywi{zvsLS_c$Nh zy!o-mH*I>H*;@MG;+sGC;O0dSmX%kZwPeeoLtB>2sQGi(_B&TCx$(w~s{6kC)~tKz zA@Zv?z|KGCz5{yOxyz#VkV!q0S&7sdkK~zQnc?9yrv1#rNRPKm8fw%&r%^K`M*%q& zYNbA>&i$I1%Hpx@XWj>lU-2|jXw-Uvu^nJ80qSuwzm>FTh1rZ(ki$)$WqObo{T%;R z9#l|P=|NiZYWBus$Nrbitz?M<9i`J&Gbx6TN@cD)WhK*JY#NiGRSVpA-!B~Alw35I zXx;|31~euM$TT-lNEj92MWH6eC&>Je`Ld2r7&w)=;IApB zQf#(gA@Ea+!^R5oM0>?l0@kLtOURjXLlA<9T;mN)q~qr?lZRe>%S!qbvH;(A*aips zmUviio7BSEzgz5~0Zv{A^kzYz=GXB<65Tf&31^e(Xm^%0y?v(?p8n@k_gKBv1~Z7(Ae-u&X!Jv(zN>;fW)op*DY3i`gsCiV1zL4L@e z>w5ygh~3@Q0$bAF(p9yUmF=p2%0|onzRB*zlc(rHhW_e?^{(S|XcBZznXRx>aF`Hl#KaKuwUS#Noe5E9sjNnZ zR~aR8en6{WjUBgFlDy1HM<=vw0p+G?yK?wi^sl5Ff70k7Jc@$!Q1Gz!3-hB{&gqSK zLc*c#Drwht|9$bwtDP9H$$6B!E+;4q%YGqM?}mHBaq?6qMQZe(x#g}tRH8l9J=H7w zBGqf1@#JZ#ZY$&TWWwtbsgtNwaZdgutDa%Ipr7Z) z{!iFzl5a#)py5S4Ms2BL{g7_lBNOaAp=rPFa!Uzm&^jtzugus|tK!^SdfDi9L3DxnkhHM`iH{ z2s|`y-hF~VJX1G8*~FBxZf_FV?}gq#8T~~_+@=;)Z5jiYO+HeaQcKYJsL!Kw-g<}B zbJh#elOkFN6}~6~I=;W+Hlhb{z)ZIA{*BNf>oOChN3AIvL3Vva&lW;hBOB5v6yz7O zw^_zfx*)&c?AH+jb_k7^Rp#tTndkXH+1SIHddRWDczA|L2A@ta_9wR6N`H}h80aeq zY1rVPP_)72yBy_L6n=hoSp=8kd+j_ml~45ZgTsw~E@AmzCbR2-6+0#Xxm88gTE%f% zn}~6+@PIVaf-{_L6s0q2(=YylC6yM${s}B`FJ!;D%yZ-RyLJs~O0^He-DWXHL4^Ug z-VDte4Y2*P6`ofX=qLI66>-U2?Y3#{at@hh(9+>|w`jihf!KZW8VHT{ z$>K^r-#DN}cgA??&4{xBWPQ)^0Nx`^b1ZQqc0+z+bhBs{>M7zz7UpT!M9_s=;<>r; z#kDC-Nu+Nrn(?UGBkU}RV&+CCuOmYSWWCi>=0 z;99LHS(r$oiNz@5*Jm)f!YO}g@=U2+WJ?V3C>f<#UXEJYT53m~9kuP4Di%Gfz#llU z=uGNhg2tM*yZt{Xwg$ir(1Z{vv8WgkkM>FmuI8OHaQgEWz+X4x2ehhP7jNjD%1(f^ zJuc^=@eFx!;IGBSp1EWrmf=vNa9nZ%Vof5k`@j(U={~e*R)FsG4e47g=~7plz=6YG2R;Ng`w1c^L;%zi&c?JHjK+;O8hgI9r#s&Eqyb1 z0x7DdSGJZv3p3tcpYib>Gw%2bb(4yvFZ{>amTJ#U}uHL#;eofnYZ#K3Q8mo45XWb_Yh;JySX4jw*f^91n zOxbK}C46ZVwSp=)3UEA^$ny#`h4_Z>WjbmHgmtpHjfGx5fB!kPiU$wV?;e}(&S96k zm;G|75O$bRI#)=pB}QE^Yv(t$FH9{u2g63-VxkJ)f>Axcrkxd)zUPF3E>490D1DKtnVofpq+iax=)LpAfUkm3uC(gU zv@2fA69x$Nu*(*!r{vCrCwMushxA%UxvWhtCNyCpv)*kdZq6v&4Jn@IHLKf1YP|4@LS?=Hk%Y&qcNm~PK&_B2RAo7ABp&?0KZ~SV%#v6O{wptY4Xk0~ zlq-9H$A~rTPuB&>bxKeVtC<~fB|fXzvWDxLnZTv=!wFp ze0TvTGS)W^u2fJ_{c@fqEnpD!BfrCu`Qf_Zh9I$Vg0|}q!Y$+9u_`DYRBGbQGMWZX zF9|wayLrJ=>kY`^jY*hw?vIudlqC6Fq&^k`jsJGWpdhonPykaE?9X*wN14ji*OeE& z7qmL|dd;(57fNPQ)esQNxyl;BpzQSKJsGfC8~sYqUi21in_OMjy{s22Iy^jbMl`Y)tH!D(4U*P&pCn3gNpxJXd9;~ii#PLan-u2nRa;D+ZxSCbQ z1LN*TNcK6~MdkcF0WmlhS#rU?Z^G3+o}_+JW3>g27mGYF=$U_Hs4IEH*Z9mg1rPKq z4e!%nSIMqv2i}}J0i%dY{e5o4pm@3m(`cv7#EK_!AF(5zTo&;&3i2~K3B(cIzZ^zT z-MB$yq7XDfFwHq*@)@{jD#+w;0oj7xakj|6r{oYmI0rc?%K{dL8_)~y_8@*LcI+is z3Q28U7{&9nK#HDBvMS^%`vmw=Dg~I*C+E*f6YrP+WTRtWE{}VgZO{3M2=i`?phXWL zu9YxH$TvCPYKcamST(F*T5-8x#8Q;FOIzl4UA#sHGDT$2Zdb@#6}Yw%)l_(KDug4- zU0&Qb*ittFq zMTkG76^##IFH$D%JvDBD$>)hVE_Rd3tqV4DKx09g7V4I-y(i8wCD?%P8#EztGrev> zsQuHUnHvyAeB?X#OL#!KqzM5?=8LM!lRG)3WFyuU#?u+T(XV|A@eJkv(>{#mop2Kh zRN5X! zCG*_=zB+LM73M=lxbtjbI^ zo~DcFXPlM%PnT?jm24y*TLP{?XxFBD`0LY{Se`hl;Gvlpe1bX`N+Qnwnc~Q$q+l%< zZj^mP6uwIdzLX3)cL0j|7ZyKC*zzgJ$3mhCNwy*eSnJqieEMqO>lh}VW_WA!qH_%i zlZNWPVX#xo&IOUpSHwGxa7SRm`%j|CxCzDBVEh91&$Bq#cm&R@u)tQQ8i-?LLZ=!G zeB|T4$4Bl^6YOS8^cNNH%eGSrQCgA z3;2U?)u)vR2n<6%G{>>jXI{xd)i}EXdaV!^RWtzuL;OszeR#`72ZneQI^HN@Y;~@O zpT*U3KD>b;H{O5Fy=Mg*MV>7XA(@zNCjV0e?0^1k{?jcv_Oq^=y#HBTO#Y0ZbwCYm z+-rlluHVCr^H7p75*o(M#Nt-wl{Ipb%~g_BSwg6h_}#TGIohDOB!8+UY6LqjlH3d7 z>joJr)DXGGP0HlV5ekX*&KFA2@VbjNQG5xGfQvSQX4zG0`QlvjTICL3k$fYPSM$m= zIAzm-eh0Rw8ND*C{6pfFRkuE{56y$%MACZeWHb>&iKAcvT(;M32vk7z>7W&KhzL%g zaI^a1UqL6d#*WTw#IsjP$~=qcnLwVjsS#PWT@BYsS?H4Vo z)p5vuRj@#Z15@}4hKTay8fhue#j|g9!eEEYGM~~gn$M9j9r1v}BT|VrG z=Kn+|8Az@+1p19*l%#LXv1_+x9>_sy4h&=Wfe7AUf|`gM)@7VKM4uH0I&(?TEh_=G zG*l~e3(T;eE|<9XQ`K4sz7#W8M9+TL(BzUF$3s}6V{Tlz9{0Iky+i4T4nzV zEEr$s6C}2>Cp~XVj3qcu1u9z62{sa3ROY%OZCjGRX`-&#JplhpHQ?khTiIE z0N|DVxaQKrYEVZSX0@fuaG6GoF45d7ayPYjU&lWDHyZrsgEPwEUtw z9zX|W7pPmu;q@iNeC8ECmIF~dNG2h`TP~o>lGB}=os6jiZr)w_>_Q-F7j=KsCpX-w zG>b^cp6Gz^gXAtcQr|rKu&P*-^DBVRvKpl$!RN{|-(%S8xgt^ua2|JM#l^(Z6r$*j z+y4`V9+}@_13_D6kkt&0$ZovDmTFBTCh0WH4au>TeC_=z3x8*xUF0N(b1Kz(ng3T* zCZ|8$z9J!AXLWx5Bj9a5^QFpegiX?FIkx;Je4~kR&>o(IIhT{F$xGVKbYaixJs17g zY&F5E3i~D>F@wCn^C7FbP|VDR*Y|3@mW_+T#=EzpHblA)O~B~GOd59UV~w�(4Wjn#x@ z?Phb8X*c`}ck#);vD6cPc+T=CSDjv*>u)6EshM4rd}*;6%4D-%InzBscf4=C2NIXB zE@m>gv3w+xeyKXuoL*jCtgSLhU22ikU8$|A&iuQ6x_B$5))Cr=+KcO6t+}{S@3f1* zPPnqjgjwu%d*|v}LAW`++G30>xUs?TC!*je%r#Rn_uGq?=L5gvb+hH?`uaD*oTXM2 z$K}?0Wb40Fg#AUpQ~uS(ADD|`3RPqZ!>B*%`CIvu#4(e8P5Bm(=PiA;0_m=7P%(o9 zN%fazmz!J)nx7a zgQlfgg0Q@56PLcW#fQ5JP(G8^B6#NfVmijlN^&eYCXWjsJKpPsKdDL=Kv-Ylef&I6 zZ2fGJjZ<}YdRi)7%ww~p*v!A^8YZMn1MAOY=0r?H+C>~W-cEjEeHvY&qVQb$M**6}%86uBm{#~itvi#YJ~FEI1e zu)B@>*XW{aHHjrIlDNUcFBEN4$lpYscKBQ8trJwIFF95g)cDUzq*x$gE3B;v2Mr{V zHR;&9_WP>4bD7qh<&F+w-~4Q5tiy`qlcV!OQiU#vPC6;d0M2t`dxgQ7iT!}gV&4|H zj^?6j=W&E*o0mFZCKL;p!h(`t?3d+Ji_PAbXL(yyjl+`(VSEltz99P!b}OZjg4A_U zexcK37uWkLN>E_y-i7{pr&7p$pW>YjB{9zoz{KF|i^=tMT#U6ry^?NQXMS>L*vwEX zR||_~vNaFWT3emB?;?B7uN!^=Zbc&Aw;O3byy0m1Ezj#8aZY`kaN|L^w~Z|4g!m@c zM&~W+EFaremI>jcnh2juDh3rLLLP4CV#J!2#VZ)A=xOO0=w~brWJAaD zc6RF6;TU8X`bU0UpIlo7-%FKT7+_FH7dnsUyTFAh^Kh~K`2GzkBx(uF*c|&uwunkD zmy10PcJJS>xfk&FC~h<_jNWKfzGr^-wI95*@4K=uF&wr_C2uht=OPmg|Izf{D<5hT z4Q%R@aBK3TA5$GM@}5N|kwTQGDI&V&xc!$(mJ^fM&MGa^x3>;|V zER2$~y!V`gTug8v4vGLokgfe5HBtiWm2CcY!R_S_>kW3-cPFT8-z(vC518;9T1Mr2 zAuo<27ESR@^R?2T1gqYt*ZJ|=XMNAiJ)cn*$y=xm8#?#G+36}zw;g%M=STHu|6+}f z`#g-S9KrL=gaIHWG$PQx&3UmPDc%>Wq$0ojJ~!jMjhtqRvc_G!{31ks9lo*^Q~Qo9 z1kY+6+P9$zQ*&@S*roa< zI?k*fc~PYwI%IM?$7iMl3MMqukFrDVVuIKsT4Y(h@&(7ku_UNh__5Silx{m!SAls& zB;Qzqyph^t_F}@hBg46)!sWm4t!}M=FB+xQ!}ZGjb1w_2*aS%4)3&RB+6K{3mH>hT zFpRa}-wC#>18oBY?iBx!i2tn2-_(RHJlT?SoziesbS6ep_~)lw>x%ZvhL1i-Cde8_ zH)2~^REW-l8T$Nt523OR_%BgCu>&j9V9l|9a5IK$z?u1k;O<6f^TB(tuZL2_Qblvo=R@f?F6DLj$2Wex0`^ME$+2#;cT8i=90nOYztP9!UiF~`t1qHwp z|7+Pio5E0w{Lc_-Uba}k{T&}igS*gPp|=%6MDWBGQ1ji1Uu}cCthYe~k8K3+e|jyJ zvSdkQ*&{~UMtKtL8M=yJhp&V8%OGV1UYX2rWVH7jzyJ*?^B2@v+f(AE-5&+pyPUdG zfS+OV!yQu&F1g!@p<%1I1a|rsDP2>nzPUy_;sCGQ&$*zu4Cy}7gs{{2uoh{6#EfDV z=WlA{p9Q(Ykvdm`v9XiW4n)W@7I=BNsFSNe(KPcRY8oy>!DtY?KE@*@Z}+xdpiD5N z)ek@VVw@o*rePm?MJ!$PZ^L|Se7~Tw_j6!4VMW4JdFcvOtIKdoDb*Uw6xDJ@RQlD7 z)I@mMYhj%1)Gfv|}8UrboT^c3+S^%BzaD$dB6>6-Ms_LXeja7~=#wSiM~aic zX)xvh2ni;9WNw!I&SbO#Psa?Jss4*bSmG#@RH72Gn3y(o3BQvz{zsP*^>Z*#^3H6q znRID~^A#0DAtG2;{}cpIybT}GhsnRCz1DTto>=uhF7wrq^pnsWr(-NasMBtG{2imo z9ck;Kmw5Ljy>GWaQ-zuL&F=$x@Ba7YW9Cs1eElJ)UGxO6%la25P`ugK=k!AQ{3#~J zb6pKky_&lD4w=}PFmgTIMlJLX*+fdyL=)#OB)@UT>$ym2iQ$Xe?rQ~ zb+nzTCl&eK)(`l@Cy3HQ?GhdS(b2)TwfVZay1MEv58~^fEc)<%S?H5!DhM23m-6$j zmsZ%<0c&R49@p7!O}n=E)6~?cupvI%MiRI3{Eb!liM4^mYM{e}yZtWfIp(5Q>>7Ih zu4>RlhX{SPIYk_Wmgs>rGSJD~CtLikUd zxVXKC4ih^Y851`T9~n0XHyIl%JL~^Or{v{eM_~EqQb0QwGA0gIRsH(6RlJ5EgMZOUJ9uN6}t6nh*$kA2&48_%e^^>?lutk4`FA#LK* zM}70utgPF4xjy4ohl-qvaE*5LxVQ|9$^4>!R*~e|B zFkL4tlYvqi-!rq^_k?chN)zd{@>AafJdxh8PmtfhC0>(2jQvB;t3uPCDJ{xV(NHVLF}wYNHz#Re`Uk5v16i?Ur3{)}%pBvM_WgVXj#Znrv- zuD3d`IMIF)e*Di&UOPt*tZjRX1kX&*$45<%=R|LL*3+alxP$Kz3&jtx-tM>InSMjZ z5&bk#9EX!iaXb%}+R^YrDYrh|!&7%xpk*!;3d!S5`&{iW^8Zsx@v#5rL-!x4!=mD9 z?D9_@sXDm=|D{#j$Qk%ga{R9u^gm)fd3gUD*Yh8?=l|n@@c$V1@7MePI$rNzh3P*9 zjg0MI6i*p}Mb%!-&hj5k_-y(|KK?I7`3HgfkGKd5vizH(eDZSt>GnUU%D?qsBje=d z;^Y3W8gt&^>7zRT#OpKBQ9o&VFcH5TpW0GSol3fMeG}&?}IO&{qlW~}Fdv9YS#XlFWNm-$}xN;X24(fm=zm*B8vFtC>*LNK+ z1&P1%ijTx-xK(Q7>bITKz?E*du$!w5CMEh;ZthF{cmd0uxIDRE_@mG?e~f-oFG^fk zPQDu$M@moTOI`q3zIdnq#M76bAA1qxVlQ?0Yas?`;Ey;%^UX`|G*NF->w6-cUKiBN z>+^!^`F#I@R%|Cm?yEu@KhG_R7`ews_?~ZPd2z>S-uKbA*o~jDp459rk5nT3Kj^=u z83CO_l?xXvT(!I)%{fzC7n*OapceZFF)DX)eT&KcU1_{}IXK8-z0T?H552_In;q$c|{7 z>FcLhchmxd?nQRyfnYqBuQ*?;ciEPumi`KdaBo*Wg3j1WK1fQ8e)DmvhvVd=Pc^Ac z*JkISVNCJ6AU{(-qi`E;EZ}LddeRZ6y>6Kr zP;p1JO<6wQ7Pn!w6SW4ARcpUU2DHzBIpEiqOH{PDiTRwZNFDTkLI*%-3X}XU(pAVi zDMjSD2J9R^%r~S@XlGyjR)?1+)>W^|)VyIyn%-!F|tlpZzWR z%mSs|7vD;%T#@EPET$V2laol}h*5mWXofR8w${7WFF3hD-lpzG966?b%SqOV6kZ!} zxD)#5_-g%XeN@SG)+h>x@VD<_;%noP_#Dz^8>Hk3)yhfJG~B>cP+~KtK?G@c+ZFmN z>;be}+!@5OlnKM2-Pb*5xhVtv-{EDsVG^vJB4Xat;2tliVAJ5XEIpQwoz-l(?CDFC zt3&!D`h6Fsq{f@EdS70KEu3zvO?aY&c<;xS(oMRE3B#Tvhr*35m~U^tG(hPgQ#{Ds z3YFXyA2Hp+-|ymk2)z?@V)~+t=tlNr}dj zWP?uMgP(EUQWvB^Nkqx?!QUo)CM_n;16g?9erM;kT@d@%vk zF)ChXS?9LgWEwN-NAIn3U+5FTwPJx7e3Cj`hP$`a3NBj0;L_bIa}4>m=?;&Z9{o*D5{J>YiB^EtIG22vFK$3P|5HGd)5-kmz0)5;eaGq`v?^ zSx0tJhOC^B>ad{hgfn{OQ>UQl#IAXB${Ewi6^h9pyn+)zs4KhG=elS9Q=6;53jTK@V4a494Jw{bcY$BPQZ(@R*s}W%0c-D3VajWA) zco~H_C9w311n(4227;Re-#W-`wf&nu*~192Q?M?D0NqQ+S;-mSGODQoPU)NqV!Rh#=qZR9;7pl38JGh;;bfwwKIB?UQN+2Fj#1dj+-# zgDs$rPLsPeKmm?V;J4=(TMiJ;;0IH*&sfL+)DHMb)Ep`BLGFAUms(?VtAHyMaNq`+ zk%_?NiWFIYuV$(t%r`)c0i1~wzHxxeVUO$!#Pjr0@R@Om2`W{RRLUZ&$g}TnSY0!r zF$L%8>`Rwh+{g9s`=~Y4DH1h72t`y{5(b z`!bV{e@USM&b5JqSY*LlW$ZLRU4CJ9c*U~H=LX()mi-yyEvVH{O^nA zSFLr-#C*LQ(o)}3&(k!q&ZjNN%R)7~#y1;bSBp#tjBFYcz3~c0g4FK>-s-Jhh~Kpe z;NNU&{xnv0UFA_V);qOt6lpe)PU>zOY{a~BJR_rPLyZ?0K4WvdBw`_Th54YgMr~pW zwZEMP6KQW?OgBEQYP0CfO#b0U#~Xm_&YZVzI|=dq0-u;}#=Ok^WQo$&vCtVtGkm_< zboJK8qRswPn*1myrwH3V3T578lZ~9yCGRy&5*`+Nkf4ArQnqd&n zqeaR-_DgHUaNPZsIj3Yq!qOV_?)p#Zh*#GDn73C1679ObsHDpB?e|U%z7l*`!P(SE z_IFJ^tQE%^lQzo}YVkhTeqDk?PfzFBU+~;ME(cYdwF`+PVj&6MQf}ek5b>dc4^ut- zDQ8GgFxwR;LIOj$<%^G18Cxx5`F21%9&kA?in|ydm8I+`z881#N*%U}qNC)~o9@#X zY~%U+e30LMNTofe;#}`J97x|AUl*|4n7-cOh~$8TTkVD~G(rbHkTT|V+gj^Lbw2uc z-L9TKHLC@eNw0U&&OYwh?IMqk?#lD&k%Ml*KC2~U+^7IAp>L)*;Ae6aF7;2URzEGY zcPy-gRhUG2QHiwRr{y=@QRuwQf-A3&zGRp|`M4$v)~mF+vG6!CYE|qj)k(p}5`|{ub>^H3*KcT4fU`X}19k|VEdw2*)w#b*ieQP^Z_In=b zk5oP%d8`ge^e?54rDgAuWDkC}XqSnPPTI+V&sq&LXNFx&7K6I;sb#-Tn%^Gjlw~m~ zw~*&FpkG=HyW$2ad%(lv8oIw3r4RV-R0lWiu^-;8xHz3i*Qz{wTgr z#q>|u>(0c_u6(79c^wXpNWNvTc<_QJOnjD65OcUysBlA{vh@c*y$5_xufnC9W$5HS zhP5&Tq?D)QL_Cv+Swx~fZKFEDDPJ>*hV`89^ZD7&ZY^mdZkIJcrX-Omkl5P?mUQ_W zr3fNl@pYqUi_2XSAOg+%RO@_SY(zx7y+`jCdTKlO{jzTCMY^@MPVcK$l91w1qKVs? ziv8~w)4^CYc1Q5wbVGtf7bICB8KyaVDiyPW1Y6FqA~+GAKB=%7R)uF+k|^_Q<>zrv zYsY1t6lymulVt~v6l%13T*?xie(Oc?m_HooX}NXRZWczp7y@BWr9;X?8J+ci&A&zh zovy!FaSn09!$|06FYp{C9}C>++#$Tp%3@aB{~km34xT_*b&jPMcRQgOKmqZj#FJ=7 zA?={vCsq3|k!%ZFN2?+`_f5Te7MF*mw;252G<_Flt>A{uL&IrBI%uq6g zl@XsP5sGA#*1_Z3;WIL2;tqGDgz%61_V|42=nTcRJ2Mk+Z0+_(V*6n~bjW@Pq(V#9 zr>(53u1zichZg(0c7SwYd=Z#wZA_ZUgr6p?-!iu4E9-M?@&k2y)CP`yvc|S+2rS!%Gjji|Sx`y$mjh?e9VcX7 z(d$9Mh17I&e<#QD|gSP*#uec5QFM>u+d+>~tu zl8XUO(~}{i*~T!~_EsAUT_(?R_v!bWyc@%ONiUu6&IgDR3lY({&UmvmPhao7qhp** zK}(xsZ?T>RS6K%XZ{gIy^tWGY;d?P9qzmt?r%J(2$o0z8Ra)g;y>n)2n(h3BvkbFN z!DGf^!Dz7M=ewet3LM!G+Dj?@3Z8}*t*R9#V0+C0?O{LjzpDN29=`xHM+N|4{YKoq#=paRu_>@l}wg}KVh^E{E!Ywve#E;mdjO}@nS zJS3^J&+Drw!p^7n=9dZSe*S%TAiEZ?$D~!^R=57%s=z3w7(s|7f7DwA?AuAAgJSXT zw#o%W)YewOorAyYKS{AJKDQEF%H0vB@kDH>bQ!p4KWMF$^n7nou4Ein_&solH5OeC z5u)DO5XOnoJxd0u{xmvu8?^ilUFu!bk6u_n*j#5?NWvh8?G?3afO4^p-&Hrc#*&FY zd41P`S~ii#(q86O`(?HHz2ldNsvo+!C!su)jg54!e6T4xu-kf`4?~t5^>-{;mYu`d z<3{gJfg}RCuSX7hA^NAEUW2#~0=Z{vlOwtGY^-p#Jz6&4+h!JELzot35c+&f6(V5t zTG6%;DmWx)B{jLC!q}ByoNShSjv9gK=U4JmCKWHMriH8uAaWyeV`MSTBEq|2ojR@D ze5aP4USSP9Uv0tpca#!PbF#!nL-JMU@!Xy#lQuY>+oG4 zy3d#Rs+};3xU7qV+e>a=qdn~l=&$grsz!C_ty!s2qqM-40D3;7`Q>KOH z81~#(RzG)= z3dj8eOw_~+<;o1RI>{sM=8e;6iywV%J#siQ56ZYkYnUZdHXvb-?nm|)q{0-_gk|bP ztl}Kiy2t~w1L|i8fpQPu`Ge&w*3GBU0WP7+rjAt?g_SK1s(>b_h`TF7mQG(k)X-ct!m=@a-{y?n(V+dYCZQ}r3eEtIy#tso*XifyMiTcVVLbz#Y-ZXCmLJ;Z zPx8gXS_VySzIr4+-2ei9`E*fb@bxuj(~? zXE@<~c7xjxgD8fgxT-~p*^rRMgjgAgbm1&E3M)R+)r$jIA$?Yj-}fVD2g4tFiNpa3 zIN}G!fh@EZ!=Mq+sQFE(rP{EMIaUu;^ssI)G;N49In8aTqH6rD1AO#0RJ~A3a9;++ z7n=f?Sz>T4Sks1Y1*t!*w`AZ`5QZCI-r+Bx$W!bdDV%dljdhGD>&{nj&#+$LAf<$$ zAZu$)M>vyG`)znJ6Cx&~{MzFu^z;3z6%?wsrCm~{rx`P#wR+2K`iC-iOmQkreX&DB zu^-16#s~=LSR&Q3*m~Evi9H^|ohXn@@L14y2r9p`Oqwa2vK8N%dV4UBS??}8W}$v? z->zOt<;L`yM^e0|1_a`Au9#{TDA~s2&V_NxEtAJnr15A_nUVP3?Q+HLKwupsbiU-X>@U>e4LsBRkTx zu*tjrh?dD5nrsb>K-W(~SAY`;7BPW-U^~7^)q!dgQLw*G4*+8Q3j!}q zfy*L`L~{&ffU|Zrvh6fb&$)T5RS)LKL+~B(W?J_7Gs*Ixy=D2w^_31^JH=k<;j!dd zB_t+C6$3piEo<*ju2fyu-FRw&~mecStN2yYPRf4Du5bS*m66EbTv+XO-DTh!brRPPP-+|<#8z2bQERZ> z-djK8Pg6fyKl?#3iTe8CVxjk)ubc~ZO;ZAnPB)b@H^=!g-nT5D{s3zzDtPI+V3?^{ z+3tF1YbPOt5b;8or@k+0J$Q+^AlC32(&Sp4l4oCRDS%+g`Uf$EHC`q;yv>+QGmb}6 z!=km?NPEVDToGS`7UF1C$NRD+!31&~+SUu?@UC`+YZyd&poI=pgw|!T-~ls-2~Hi7 zbUlUUBDtJ%oF@ehoN@Q#n1CFRmQ2G$LU3kn9n!OpGr;mxx@qgOX#KbSUIaO3T9_Xu zGVC?ePRPuyhInUysmlcg6f%fZbZm(*XvIRtJ8!GUAIGhMN!fGq*fx$Grv>?%yYoevn@M^9EFByQ61Z zA$wXjVaCtOYLdP%Dyq9vW}DT9y?B`<(QM&JSW~|i5-P>;EtgbJ)fDrEgO)LEN)3sT z*FjbK(@dDZB2gM-u=IZ!vf>5&tdc%dt-aE;!d!;N7+0%hEC?Hs81b`~bzJ&=YyLvE zkQP8cBfcURDrh)T&}^Mbpum#21oMhvXsuJrXDu(!9MbSeK^@-n|zNuEOPIHA}jI4qO_Jv1o%m2vzh%jKfvU z#gQ?peEII1#N(yWUL!**73ODp%eJD3=q}1B4}Y#)M|A?5ZiG2I%sp8yjW&{#Sl$Q{ zCC<&i%bgFX72TgDJ{g=~X%lnLrylHcrTW=>3S7qR_XW%x56zIb^qvjXp49PjB?mjh z)VgP*>Z#?Bn!dNh+~Cm82RG|0av+p17ORaFH&5~@>s{>H6;1ojjW=hqact`pRiR0e z3;%|54K*c~y7eyrs`6}WC4)BK&khP!3pS(8;)*rJ>hSDiUj6;I--B+p9o;DaI* z6U8V@y)X+hOu!w@g(vqnPCdJenA$buDxiy^75SyGoQLACk3EFcM>ej#Hb{irg7}7$fjx#@#g@1y;NedBR0A zLUHOpL^qibUS=|eX})J(?8N6`NGhGk!Z`FI>(duA2FU1MfjN1jjrp72Q+`h;K}VmA zxB6a$!12BJqM*M$dw4V-FD*!VItGLW%t$_+6(?2*CGxpFPtaTEx(mHI6fmAW{uF0qX-s;Snca4wB(99EoHfYT7=5yNCz-$bmHfnmMT3={ST>RGUE6#b zdGNJ)Ec|jDx|vKjX)g6Bp6Vj3RAIKn2udzHY}zAP!V!Qal@Ej@MerYGJ%e%op8HRWcM&ipFF zyN1k;VuVIkH;`1s7vx9OBh-r39hhn=vEu)Zm(kN(_I}x_jx&p;fs^3M=ln((G8XVEuZXZCYbH4mW=ee~ z9;tO9o>%cRs= zsTE<0Re@P8>|ytQLXuVt5d{(wvPl%hc^os{{P1PeeLur^uAqB*e02g_>Ztw5z{sx8 z&qXBm7(~es4>UrkxuDiq2tL6Glr8su7P~AS3_fJuWa?`ad3p72_GO#|&W?z3(*ZSn zEo}}Vtjap~3e9Pw>R&LBWn*R#7K)Zir(!*9TfDoz{S8Vu`IT0QC#Kuja0(+C-@F#uvyj@LaaMLw34I4ofAozvZldnf@i~@p$W?t&OTIlo@wVt*8*j zqYPg*>=4-|9O1pUn)aGbOUiUYF24!RfAo4DpH|}Pyan=hrSnVuxBM%*(!ot2d_QY}A zQ95;a$oO=_5@GTPks->g*kNJUJg$Z|7Eg=>tlG~%KqW?LeC{dV*0BGIMK#lb63Oa4 z5WjrR+69@>N!T066ZY%1iA49(Z;jqCu93cBrYFCwY4iqzyHXq&1OB8kM#g3AiIMx{ zq0yBn3k8HMWelrg7-^jbnL|H_i}w7GeAo%@Jru8+H%vBQurw2k+(h=>FYhP#AX{0N zo<4FP$Pg8x?i-K2i;x_%kD?juY{5EYzz-`PcRe0`+I7JFbo7Yp)u_s(B+SC;r`rmG zH`|{yb$U7rw|ce~?(pm=+~wI-IOG{B)D)?P%7@ZSnv_=SljWqe86qYDJVb3?D=Mc= zt9q8%H5Cssx*~(VMnCx()iV*DlS$M+!KzW7u`Thq&r{Ygp{EjbCV9pzr7(6-EGpvH zj2eMhu2fBvasj^#2OxUCkn4_xQ&>5}>D7e2vdtRh3`HuL3bm)Jy~y_ve{HP(g@P z5D2A0w1Uu3=wxUtBx`}CLSJYnEEmW-A@WWLY8pVXLU?tcikn8YFQL?~Y1lAr_q3sD z$ETf|CV!1aSo$!!G?tzwRHu{Egqj)2X?-*2KK1qKqF0bru!@sX(p>^vQ`ceJuPYcUaO0SLKXlFk^P&HAT3NH zO;uDe_8ED>N?xGdWrH`Yq@a})w308hk}s5WvlIkOpA%$wnux1(O;k}k3z|j-Xjg52 ze(S6z`U=@JY!;Fq>neKYTrNzXUjMw$?{fuHH0t*UW)}%*iM5EF zq_QX&jYcEYjb9s05r)BRiw8`O+R+taqA(bnDaOT(FOjF<2S=Y`FTfV0MiGI-NOlb2 z=MeKP^bAsBvJFl$ zpSwXmyN~@kNZwxvK8;Urh7~M$8og7$MzeN0f`xHPd&jkv1!zWaCDBlXUfQCe1W3EOU^#1hIq>=qu7J2Ji}yyyQUZkJOyCxyzs#ql;ueWq*i*s0eQqD)f0w5u?=EbY4@DX@+UgGw6NE z^OErQ*V^!_vN%gE{2r$yR4qWdnQ3kZnh^=Y(8Nc9O>5U~sX zf*c|4@{1}`7oIo{lIbq5yUt<@nJiXk++~d#3cP+Lx{ASyxYwmM=tG;agYb8axG3>! zc7Y6YNd1~x)}=m7gc*-Iu5KXWW=W%|#l)CijFPpY#ltu7%lJ;di zMLBW5ERh@X;~eJ{{kT4mpBF6f%ku;9EQq9Mla%ABjB?y)QH--Xpn%k5W%*jW8VJ4D=qE0QA=2coL4)IzBV5@`NoEtJz;=dh!j0}-#w z`KjYdm(s2%_6C3m^?78F{Rv*+8+jJ-9MAK|d09V(kT$5R~(lzUQWQT-1A)>}SN_N1sPM_e0xsU1OND;zalFSdg{RJUk zDD2Mj;1WN0`JRXeC%wfPv>=Evjv2vFB2?^`6$Rsd=-?@sVTu;!)9IT61+nhVb} zm?LhU&}lUx?&v0{m;m$@BU`wI#ETKSDcmTwjR45xJ>pLqpwy2kwVi?vv5O%yV>Yof5KaMy1K3Ib#qqt7=^wrO`5U2+5EW z7L>JnZYQJh+2j>9HDi*d(o$0@$z(L?ba#Mx?yJ|+`)+)&x3mfdXb&#ky$hG_$#=ne z+IRJyo;`H_h54XE_ADx8o)OH3kS8=$%9_x`0ksVNnaTW0?3LJwvuYD_Y`r)w#|q;` z#U-iIQZ&ho37#PrxLj}=Qcf$IFzuc|`g$~B0e)Ue5pBj{MHkGx6mZv@N~0kY0%r(% zg6UvGa9MCya44uGNfMO>2bt>xAMo0!)*p2;T9qN!lN--v$8vG*bXfgyYF^z1JaZ7Q z7c_-}BUKm!FGdX9La2~w67(UyG*K#)HkP)O%1xz%*tQ=Bfgk@OWF;y%M=2Ta(ms%LOZz~wE$suzwzMxJ**4MrlL!|9so_L(H{rv; zj|oW<@i<6OJ1A-9f|6#Agacf*C{9*UJ)a8^YM}NNqOMtha!ANYK_ojv8=P3QloTx` zMN3K1Qc|>(6f1@DQqf381wrkJT;vTe@l=v=g-i;SQmBwZIU!LOvA!UsJ;ZYp&qcs_ ziKmiGJ7iL*ltP6R(i%*}285LM5YJ7#^BQcx;@jwzlw!tn^vXtyXs_WlmWb9Fl3fOn z5WbVq61qJzZ9=Kc-b>cJqrQDq<-zf#6|l7a=jiL5lqR8i~sY4#^T@UXOLHxSm<4Axw+_N|4WJ2y`Os3$oqih0pGoeA6pLk z4Nklv@->LGx#3rI`>@WM|Cd=+a}KZOtJzh6y~J3LcVZ! zVSnMNLOCiNDr5@D=lW)(h6-_EVWQKz%Q|FbcU$`*IBu1BtUIhv5EYA+2x3;Ecu|83 zN0U~eB4zD!q^wp;O_tQIOq(<9g?0|)3T7&u8`Tr%lQK=x!^#jL0Xhx|3sYo!Mo^nA z7PHyn_xVVwq9|c0DoXgg2_ZL_v{3H?M@m72ZGcs;ygv>07qIaF z*|tJr&_ts42v-uPoVvNY2cpdpYFa@pd$R(5zy_Zm`VArPHcnNgszPMeYyzo=N_}3k znj)$fva{`#zMonI>h-p?MSzq=fLJa;a!Ak+(rzj3meMXM?UK^@QaWEshop2!O6N)G zJSpv$K+_P?l=935OFEB?kDqux>PhdBQ1C&)1ZlWM^mG2o4kp zGzOAL5%GLP(N;t}H}PD=%N3I#vABqrD{Ah*%O_qQ@y=^I`G4@!)Dvk&VwB1_MLy^u zN_}ehI-d_dir8*G1r0%@^Te0vk|NUG-P4SkdwRONyYYmsWIR$PEgQw9q!O1*(wvp$ z;pa|G676?8rq}<4#45hP4OJxaaednNHeNjXIGs1>X2;09>mxa$BK(!nW8$2W7b0gY zwvPQ#wm|kV)2%aZ%(=n(2dMrvy%?Pv&wuLsIS3=E|yBKT#RN8`WgoLx3<&A9$UC73nRWt$}+S_g3vM zdD-w{$uYy5CGS;#k@H!#zM4QOfp?q)X7x0Cjx4*}&ijJN9OBvecrnDNSDCVdqeW82 zz#y&`_*9;kO~)Ey%VM3e9kHR9A|AuBpM@Af)2M0Du$n>44zMaj%4&>xv(%nIJix%K z!-Bn-;7W16&5_(yjEe<6nDi9Ki<#m<=6XR>Uo22){+dK7G5AQxn%-$I!9yKY4m+hRJcQYs5r`(Ktx)Lc8iq{mZ;`2F= zAHWGjHIG#wJOi`p9wzKXvRrQNt{U!+K*lf#)x#&!!_CBu8Dhq9bLEUJ4Pk5%V&;IG zX?}3Bq3?vHFWJH@WF+(7v&?EF#e@x}pp3aNo|faa$15u0fg-2gb?1^>mz0-Q=im0| zkJ>)_dBe7{{Cu*24CPv_Mpy5Jo!6&Ie0bVpU2DJbyTuPCc~iP>dwn|8#CZfEuOc+l zYH)g%>~6l}_x?a$y6*n^w9UBE8cMDz56!XYr`LaX-MyDyNl&3%50cG=AlcZ_b%J)1 z)Hzv_JimO2&XF(G#Nt-1lJrhaBS-iLJ%>T0#TJkeWt_v2QPz#`lBKEm_tna|Bj?e~ ziRO$Gga=;|fiHQ4{W_{(E<1pXKwQR|DFdHQv37STnM1rXAUSfF#Y;%93?kO(&!R(y z-$Z2>)s8D68mQZHZax!1#FV~5M(c^$u_f7kV{=k*HJYg*i(U8dckeL{Ok zE7xlEaYBOAvu(+EJci6zFgJoQx!c@tmYdC7fdCH*3Sdo8!9^oD<}!HnT!q^e^8mhk zJbjDXlFh`7i0Kea5o`f0tcbZ>Bzu7iyGZtrxC#r$RbV~ma;t2bir9h30gt;lsCY&H zbMD2+%ds>1(=b1O8B={L@}1aY`bW5D^n1B|k*8z7)IZO?5P3DGGe+DoZ=^6*5~+&K zj9eJIO@AYIb7Xr=cd>pccX4ECY^DARZe`?(*nRqYIE_tj=PZ$&SiOEWr--TMreX;Y z$yAl#W~MlPZVFn+$(*H;;<+hL1nXropP>|wmEzJ;8C}24P00jbDA^#xoUC7VSjN)j z9%o9*L>vB`q)ax==X288yfL-Ixy$*4^N{m3r`!orLP^|6^ZuB1Hfxsev{8AkoT`Fad0xw7pW6 zr<{_+!O+Bcj*<;d;}VaC9jN)S0k#+^(c7>r$t!CPQqeK#)$_MdZ1RXg}#(146V8)co@J=rKO5G^sGM(Hm?g{P?Cqu9> zJ;c36*iCZMiL>s=ywfMRs$qKS44D3j6Lh1ciW^RMi?%ky>EYpw+yT-hIl&72zK5s{ zzCuALF@1@%6RtjPZPPyG*w}L7FMs*a;-PeTfjP8w`O0~?O0v1pPu*gefAbzsc{Dyv z>8Os)m-ZZph2&ba=k_J}y-zFPf_{VLBDx=F1OLnB& zc8#_{ui~tCu9|+)f?(x0*Bv{;`Y=(w;`)ln-FL2C{I zyn##h8<;auy@<89ASrhDDym1Z^-QQ;yTAR;v^sU_AzFKcfaiWWUhJ zoq~ZIWR!x2H*Py+!lrG=M~Vo`%-BrwIld8L8M2^8)Q9?!9O~V@*S~d#J>npg;T&mp z+F>M{hn)@@CEMo1;?}vTVw*}_t3ha&R%+s$P2_P*w1RY5p@#?-$Og7xi%D4!;q2sK z4DpO0r9PE8YdA?QM-Oj4iTWTHQDHA8hL(I9j_-xz=AhY^H^~Ua@5k^v$t(r1gjxOj z1-4>93J0Ms-{@m+jy~q1S|4*=I$>Y<1%6TQsTApLq=q>p(q4tz05~vOO6&VEv!Zn(+cSc3^wwgUxz3p(t1oM3*6ytStW#MEl$FR5ve+zQfH@0qN8_8Df96I`h$lM8fvY$@h74@KyBT+j zr^ij=kpo6N8E%`z?Uf2M5%;O4(Wk%<*WY~MgP;82rw<;z;@3F$yVusns>1eIj$`?+ z=cN?mB5 zPM6j0B4-c;BbSrTabvgAZnG(coKRBv9J2sfkqSSI^eC6PMaV)IUY&I*b0GGq`sA852&Rk^WtqH4OZKO6-yRAp9YLaBE&oGTZhbKQu}$67@IzFA z`E&bVkaGnX;amZRH}?b#ZEhbx&+P-qxswj6Hqnh3jZ4V!Cv%7=p6?Ll8bIovj44U> zRIw)IO<}VUvvKT4%RSM*l;)l!B8H$5A+Z^$o->>^GRxwz3w(OTeI+jbkU zVN_3yqO@Hu%(=F@j=NZ(`+N3pg|=gipTQ5HLX^gDiQ1U%;pXlU$LVJKa5E4fwIUGZ z4%>v_0dj|UfZSFg9)FD}sR~%U#z3OP5BAzTM2-7~6jEb4LRC*e$oUKv`pFbVXe-j) z+(TbBP+7&3M)i7g+R#V5j6{P@ko|Y+Nt#^)A#148AZRD3RXjgMSBS7uL_I+qnyg(k zn|+hlf>@eGGK7aukA9~(nXZ2t$Zggqr)B`r{j@G!ni>tyvnPNIYX}+sQppS|%JW72 z;%EVWOB7vT?RRFmXm=eWv+dZgVcs|cQcMAg0_R*CAp%+`QWjYixh}FJ@^VDskKjj* zuSDNQZ{oMOH;u;}$DHqnKX+&@ajat27*{$r;48zMqT7sLbKDrYDSBV{?r4_WnB`E1 z<XLODcijshlR3_F6( zU_mIH;>sP{I89Z!B+7)zbu%a&DZtT4gb}iFM$8`9LOlhD9ujf{YWQi&0;C7=NI8BD zE;>sjbAk(j%aM$Dbp-jd6z7Utgx(B7)_Io1gx*9#0fE~w(Tqsc7-5(}X04#%j2392 zBT`U6lDyMlfs#hLnQ99AkSXdz268GK`XvtGATuIp)jqFR1MP4eF_P`>~3CdyOOcjnekj$`T-%3xE z)6M+@C@P-BiAr_=G@Kg}Lzp+BQ%>k`;yYpSov`#ySb8Tcy%V1DPWr?!QKCpH6bO(} zB1V4He)iMvm={x~06lZu{Jiv$OnRUe(ek_m`AQly&jHOO-S_>LeRO#)gA7-EIupWK+HmAto5+7G$ zC53I&q?OJn3hK#n&|(wCn~bHna+eibGsUcqh-9S|k<7H{L9cGQ_b#1f?*@6MT-tMY z7KC}PTzu?|EleS^l#p03nZ#^hCjwXwW zM&CL5i_uTR1;rDp;E6Ns7^we`vV*`0r(+Jo)0bvE)QG?+z@(;J&8^zj%(yasv5#*EYeLD;jzBSrv{J>oE@A) zVn|Nu^g%8)?$l3Q?tO?kHi?JXL<3ssitkB!9imsT;mkhx#71}w@`>Ftu~!Zgv7paD zug4%Z%nsn4PQk>5Cs{Ec%hbk&lLZAvCO1z8-05lNVshi{*x{!P*-89)k)2+6Vbma4 zXfkTSoSjsEo*-9P(8Gg74@8XY31R^UsuEX`fK&k|tg^E#lcsI`|a^#KxaoI5y)p&S{lWtv7SO%BONw7AK32@iZCXdR zEyp2q+Oi#CThJjhC4~G+kbl8T2t?E+UP5s45ChMEu-%a0?7UsDAGIrYpdE?Zif$|3 zg?1(GE4r`vC+H`M9~J$m_z*gjIBq{>=h};|D82~_+);FY@%M{OtAn3ib;WD zyA2B197Ty@8%_udQ@av4K@2QlA%JW}#VUi{1LI^@*^<_zGwEQ8;I&T#Esm=4^YX}{ zX*;otX-Uywp%^rZ!E7<8qQTx7Gm|6~%E~K}GP}jzX1~+E&wj#w+OD+N1NKGs8)?Q1 z_BZURfISHrqy$MS?J$<{LGj9%c;+~6Jf4m-acTv~_j!+c8RX^YwP-T0P>}Kt;tvEa zd7c|&9GpRzQJXXnyObRU>@ma*jKNXz^1~TzHqpeAlQERPSB;PzHHWDp-qW2SCuEm| zgOSgw(x*?KFjmBy={ek<2>fC3NG`cmCKBnECUYq&3E41@lNG2mlT$&#PA;;TowgI^ zsQ1>S69RaMcy=l=$A}>Vc!+pnRz5Li01pw*E(y`}kt<{8fQVOY*P6(6UM9!`kCwDA!`LRu-Nv)j_^cm?CS8eBT9&T=1GZt)pX$@) zDY^juq%NJVoBPoKn;7}dgpruMJPhn&=$en_e*g((+>V z&x40UU*IvLMU54VI;aRau$wUkt)XeSBvg-Qg)YV$vHmC?G9R@Jk)z=WMI}kj?{NP-hYLkRa z%I|sEclH97m@>kVZV+>(~hy3Iy}g{~W-#B74L^RJ`5ICr)ITqr@Vq z>bJW}yo$zFK~DX?e3lEN_<5&{mTalJtPKf+Y87&%WGG;?h0v zEtvdYhDjUbL@!ce&g|)A_R0Os>Tmq?N97gi`a|{UKu*K_t^4~LFC==9pOT&NN1wx2 zjo!>05_iW73witK(|F$KUR*03J};<{jnn`SZUfFS06e$^KPhmAfG5D5$H2u%H@~&a zxhALNtbr&M6_=Et3CXB_T%Y!DPvxBVfP#3XeJ~L4XI_f9>%CFCDG)6&`J*LiU?j^> zCz2Tu`R_7FWoNi35nv+3;C_&4dNL4&Sd{1txPYf5UeZ!>xI|VW2Rieem}5K!pey2L8;C0BoV!!&XGKbAIE<}8QYvl=2XIbZj>IY zBiaRW&5O7(NNGq?H~2*y@=M1J#4wwNd&Cn086`tNEE$kW2BeaKDJ3(K2l^LRz?{!i zU$WGH=ht*PubE`1|8u(4_vyazy9o^Vi_tgEnr=paL5%b}M&A;R^y|~aMjxXZSV8_w z_FGhimkO?>St|=)$$BOD%g|e@xAbpCju)zxdSx)Ac)d_&CPqb0e<)d+H9KpG@m|%V zs{L8}LrS&Iu6rB!7_mLl>Ls&3h#>LsdKF#T{UEcqH+lC@pbI#4<~oV-+Y6}8Gq%H= zh3)a;Bu28bWN9>ksKDtV>LzjuJT5qMlj(Rvd|A9Relk85SH$D~sA&*a@AKyw*qlMk zf<(ncVcBPL+gxs&-5w_xrQHn-lW9aPfO>=CCC$h=J)hj&tVIRX*GP9aYp{mg-CVu? z#wm9+by}kEWbRpPLpN`?=duLBU1#tnOURL#5~2+(iTaWxktB&^)Mwh|#Rjj(yUe@G zJLHvsj2rp(Tncp zjlk++m{2;o^C?Y@yiXI;?2NfJp`(PuzWK0xm(=gm{TFRR&c1i#2wOIooVC z+sraWVK5LZ<`MxmW={lgz9tmFetjsw78y$dn2VdE0hFgL2;g9rFM#w0dNWj{Dk4p? zL|f23e$Lq|-32Ax-#yML%#sDB-)r<{VlPl;020XYWU95*?+lgP**WqsVH3lD*wR?K zvUBuPy!_gdy!&^K{_Tj!6pq|Jb5n8U1Iy>nEOCcZRHOKh>VoRhi_W|o%uP7EAuyp@6Z2MShh`mqxy-wCn6{0C)A(G zKU1rk@|(hIBkUZNUa&fo`lyi_obV7wwh+=6%xUmrgWu!d;b;B+xND9@9-pJkaz4Xc zj%-+uLd@lRS!W4P(nd%}A+2>3-*Esh6WKVqkC)t~J}li=NS1%y!xi+fE*&2_Jjqy5 zj>lw`Y1Bb^$Zh5$C^y>~!G)@51SJ%q2=~Egs@0Q*^USH*+c&qb+`M^Z`)2!X zt5)51sUG&HMv@?x4(Em>n-lzJY%0V3d`1wXJfEV`XfMd`tF~72VY!V312y zd~-axiCiW(w_K?gv{^~L-U2vhDDo6BMMaT0<#G>5421uXbrV#%&z_S+`Gd^m1JkD2 zhz$lgyRYW5lPsG~CdnK3qcY&P3^v;&?OhB2@Ywu8=kk&?H=% zbb?hhw4hh`)4a0jjC6IFGIQot>_hgpOP6l`<5PdVa^|$K%M;D3TC;a|^ZeM_lCrA8 zmCF3aq757FoNv=-JHu6%-Td-%@6BRkcfd4_8L3EpCmzd2xCpZ&I=q;b;|> zMduaNEW6mD)K*1nYnC0mx8YWTYwH2mfF@B8=hAD@glTh9TllU1t$}NTvY@uW9|_C} z+#GmO^MY16SF;#(p!UG>;12XzP-*gUelEZTkNS@Kj|PqgRmApx@APz%o&hjargnJ` zXC2LA`!raA*+G0_FJ@Uhh(F$^@CT4>kkRhr=D6kbc+ZSENg7_bkcI<`QtClw-hPbc zC{?NZE8RktLLw{n zO&n4#&D`s$z>nac2EFmJGfSE9hRPe(t*;B&^hJSib?0xc{n0>dhuZZ(n}c!YgslZhnj zkPnhRpUI4kz;e{4oTNJG1j)c;R1smG9LVhFj&r9tB{_@vyt%J9WiY4BTg^Mnhs-C< zV`gRCjLpyUlqKkkQ^YJVIc^qMx7Sbh8Hrh7#5oM(2ac?j$&J(}jM7yPrpfDK2%MRv za5MRr8xf4B9LH1;Gd+orB~6f@S&0kcqFG*jI$@hLdg;$cFT217i#(i7BynH_%nOnf zR>pH@jO=LifOUSHXDOuvsYFjIPu@s(TT=ta}5!qk9j1sQVQCQ>QfrQB+r8 zyBIa;&}5li0XbMZL~O>@L^t*Xk@8HP}41 zxQ!jNVcT?|w^6~W@?jn-XIhv(W+$_o>1PfzN`@R5w>z>e)3qK=Li2>?kmfbbNsU6| zuuC_A17)4AqL;vooFG1>tE%(~ki_1Tc3JIW*AgR19X+l+Zks^1&KaT^g{%(-vW%J2 zPtC!MB_HceVXtVvmNLq-@ptgA#BI{cnC{V=NoTT+NA9I-rUyXmxq}W#D}D*Y{|iPJ z$^I(caq+Z}I_5ZS9dVFbF8-eQds3-2DO}7Wjsv!rZEqL68T)I&d$IS5RGi7<;!Sav z%v3;bx=0cc)ktr;$RRgfSPE3*CM#C<{F86yAKd;sxtfP5x!4l%<|y5t@*YL8*P`KY zn_8teeZsjto;;51mFiF@x&6B8Wzc^%k-j0YZkOqV@XR2O6IG))P4Y^^5Mi z;BO@VZ?nE}`Tb4wb_?dvndO=5EpTID;rbh29mK9g(QK0WSMhp_h4VwSJ_6A>mSln0bt`e78kt*s@gQyX*c&_?NE&DiuXEY_ajgzB6 z%0~SqhGnr9{VGFeOu>;OjQd@|q@DxM6j#SJacx`|&*F`|i8u2WKAX2T>lbm0W6kj^ z^eefQu`A+(`UCOj@hh%ZJ->**Q2l%Sw(A|wyVd`UdH6g&pD*CUe1wn26Y-*Wal9m+ zjHh&oyhMJYAQ4VP647)bU6d|Pm!y;FlrF7KYtpbRPG=>I2~)zHuq3h*)&_Nhra{}F zYseDvgnXeu2n&%JQLCxI5GY(?xTNrw!g~veE#aAcg)&YSKR9*%9AbJYf} z+M{=S^sabe47=ijLBwW?2|@XU{oiC}HevdA)+rl1_SprHBi(05jcW4S)cU{b**YnF-l6oqo;FtyD zSiG9!$oc@XIg;&j#VsyZ+!Kq%~unHvy?KLatzm2kc0VqKq4 zKM0ieCvm+sd^$25aXLmun(e2Z!`z5uFGUW55sBussPG^YZDgAs9k<}8^ z;igwEOBKdQ6;_Dosqq-`^gga44vr)Ln2D>15kp39lA@iQcP5n*Gn;>PK&=EzW{EcG zf1cUOq|5fD4N_(pUtC?Cu7AHSU0L}*M=L7R_3zcEr&r-cqt8@LOV>YJpSEPh4m};q zvZU*usZUo|GFObQz!h}&Gg85fjxv=I@&AY5UHAs3B1%OzU#`*q=RezxxxQE>B;g^# z__eVimPIF#7I^{L53yPf(BKV)Lw683V_`;z(TvD{s|s4A1p~Qvo?URk1q+B=c@gWE0|6gWv6szEvEG5Fnv*25a+9&`cBl2-g0 zlrKiZ_;*Z)tXN*JNGYeo@6)Oc>R)K~Y3sD#)-BH}%vx$#Vz?3T_Z8zWO`n@*S^k>+ zy#0hT;k+x?=90OdbU)=;;TiQ_>~{vfAKVpsI{$YC*GIk^JskU4;nDcL2}k08itZ}D zy<~cFRjQ(Fec4^~%PXJ-Ed6c<7Ef_>2|~&p=9!SkK;AOhRR~GXN2$*uan=}>;tW!e zrdwf=fTdB^X4Oh@2FbD-r8tXwT!%^w>!o-W zyVUTY6xX9d)0fZ^EF-k1YEXVSQyV7_;S6qliV z?*`yc4bMISH9Qxe*dm6Wp_ISUP;?N$Sn#Qvs-{Ol)aT$vGM7UN` z-q$I`N%?DO+(FBKUy76RPtkY|!2#c&q&VP!ZZsvuT{{CTjd8QXIxt-$Uau!U_CONpV>wuA==v9*6e(e@^4{yZj$X zagzT9iR)gRj>$vrY8s!1I?;M? zy;3P2&4g4B#EIVu`De~RW2WOzsh=nCeI@-af>AS|B~j^nCy&WyskR9Hnw2{`)OJSYA z$2)1SbWwkzPkLz0ofJnl(6;x|wutqKz27O}X{NoswC`3!YNBV_pwtR@YaKjWPwU9w zX(9bu0=W{nN*wAVzlD?fNNjB({nm9*f|OqewZ_5kg)*(QR+8F)mPr_#!H)IxI}69( z`F|GHW{N3d+4f0L=A#CnkvhP&I>6Q%8gGDH9{kzhUr6(7A!i}@1efLj@2iF1c{Fzs z%0e3I2-UBl*xh^1OpwVJ`9c@vTwM}9ZyDzs{~7g8jNTfF7jL9kl%b(5v{y4Q$ro>$ z#7j2PcSStPv`56Nb+o+L5<=}A6ers0EYVK$R?%A8$1#K8aFYaWH9X%$%XLCKGF&Qy z+h5%?8U8KuqjpLIYowO-P;6X7^H)#~wT0q)4;{tx^;f6#U7eI;Ow_Vjs`GsC+9ZBU z@NhY$DY4bdrIA@L)qK7Y%A;R3wWmbBb1qh#3wg~rXWc|;ZzHAG4ym^{(As*x5_Tcw zAM0uP4YZ~$=Zu9oPE%(Rk<+x&7Io2{T|>FyivMWLc?lQSQ?9TcQWJ2=Y}iJ7TI3fZ zKk1nyH$=xv_e{cD@%wuJZBLQbt&{lR1kaqPFT>w9OyOD+9CD&$r!?yj9=e>?yMaO% zzejA>Wb7t0Ji!LB|3#|mlJFygrDx;NzxtvH{HUYfe(uPT9wzm7Lw-B0JM%RY^GFZn z+mpwj=dAvosF{3UC!Gu0q`6`f<-40F@qzzj^fUE|G)ZW6lQbF=G?uCF+;J3pOZ_k=p;y=DPp=%$Ru= zKdbk~=fg!?y4qK^u4v~U;}@-N=jU~<@9c#ver9J+S7%Ra@0!l_d{@VcLVi|jZ|lD) z6epGO3p+bDlAH~E-FkShsG=ef1FyJ{uj%OE7pz&ex_1M=pnXGo&!+aanVlPZ*0lHV z^V>Ji=>lXVRpc%41%6?BNBatB!U`U`k~D

4a@U83H_zfMc%V`xW>GyT6 zU(w#x3zaS205$Y>^6l$3cC_}kPi}bU#@?=ty|g7g?a*1h8=`z~>vAXu&F_RhCH3~U zuUNgFHm0p}#m05*>w9UR7Ow8??W&B&H*eltxK8T2!WEtC;;Vbtb;Q^8u5Vq}9$&X% z*$S~c*0*miBv1asw>GzTKyEvIWq!k=y4iIzYZlct%;y_s^B305tewBGmamz!ptg2i z?fgYqnk>zt)oV7;PoslD;(!ZXJ)K=J&RYm>oM(9H=dA%O-ngNijL??OjpWT0otr3@ zY+T<4eMIVkzFN0|ROG#7Y@|(-rfX(qW@cu_HZxP3sSRzWHdC9Knb~b-W@hF#GrLS> zuiu&P?C8vynIrArl~QplDzfybsv`3NRm3$v?p~!c*G&}BQgdeQK%K2J%H3yMsmI-P zpFII98^4`1j}y4#FMa9rhF4Y<=AgF#4!&I%Vzjor4d zhjW7aNo;9Wp6ww~h{i#@M8%(f-+=Yw4ll4OQ8C66?g3Qf??D%v2yt-rsncO|5I)M3ie9>Rkdm2Jj z%t#)ML|8!OBA1%nV?i_zJXIxfPyM^Z@8pw8Ox4Z@7cR79A|nl1uj%c6p^5HZA~3%h zt>kRgqaNAbPdd=gN{IJbw449l`Banby=_{___PY4N@+(CLv`l2o*u6Cx!z20rr}<@ zNp>*HnF(DofSgLoAd2Ux0XDL-Vpj_gcwjPirFE;tzCc+xl=D#cc2YWZEm*wcQo zs2V*VrRWj+ZAq{DZt6Zcd-Kf*D5xeoCQyI+)fPOxUR9?}K2BreXq>M5Z0bJ6;K4y_ z51rEDbD|q1cbZZ#Ff#95D>;*fRV%@6+W!m6ZHzHi4%HMi#-DV6<-UC4{tLGHBxh z$R5W!W>j$1aSTl>W$K z$9xRM)P5VgyQRM`$jLqn!DS7tn=D z*&F`{H49<8o%sJ5xi}pGGXeX3l)bRKczv|HV|q6SjDvam7w0D!JWzLrVVx`a59i9S zt~5YCgzW>2385K=ptae74}WVStNX!xWT@}G&|f3(a%7D=uZG*oz(K^op?Bk7W44`2 zm7&AM-|ilvUKl@3CNTHvvDp)WQ}RN-%$I6+QvNwFeXt-V8#LPm=Y4+{r#Bn-#sx~9 z4P188UnwWx;DUfFQ0&G6)0tJikjM?MNJe{1%37N=o)gOLGGCp#bFNN!~d}Qqj5yGux3vpcST)wo=v#F_0fB#(dlC>pSsG+d-f}%*jAK;+`Dh0Lnf8!t^x?*h$?3P@X7*yVlOu zdf~2ru;GD^6mT50_@9Hdpl=#GI-O)|?QXcS_fSB~_hR=p`I8?SiSqXnf{pYyr1o}! zwH4Kv@0vCyw?P>UU4VYmOQ-L%19iu2tqkyQX6xGJ3fzFOXP-I+TS2UW-h|3n0JSG< zeca`k8~jS2{~@dEN4fZ~K;wsO*}nqm(}MNm8LS}6upo4x zB9xFQr67MnNRa+9ilBp_A_O2`CJQVa+@A;J9|MU34e}G@AM*ykN>6q3o?5Q;5-v)BeSah zlHul%tz3?myPEUI6JI~SEo4j9kv`lNo8#0jQz>t%zaCf(?51i)1IlJW2I~!W5P2LP zh$^^9Md`n*4ahe{j4%Hm1t|{@`*#H;;Z**||5A`5D7b&L7n>6#3kr@J(UA@m3cL<; zcfGeMNKYo05qt%#26NLX!Ojdt5coa|XKPChf8t13~KL(B*iRCZIKL+F9XMIhjjUx5GMG9esUV z_?Lp=h-3WYe<_G*c>ljE%y$HpfBY{6jUojLYJ(D<)Q`>+(dPgri63vU=5+U#K z*If6vU;-Ua1IJ)PrzwV`p#pXJ>d+ewmVE%)1%Z2K^rKe9Fb|EWY-p+~kWWi2`|@N0 zPq^_lclH%JKe{8ox+8DOZuUtoFT^>om_2_;WA+ZBks0bTO3eE{Z%foF8(H@>WchAV ztZ%RTjo25D0QPIX!`M14l{=EJ#%JJ<;D;%6}?5n%l zXXc=^k?JS$qOc-xqTERGSI(=wK4)CWV$kyfr~{`D;1tvW9AW@``w&HdYgfT5rw^ID z7J(f3qt74UGz$FicL#;2gzBV)(M0lyBuQi4&4v}75_0IjSGvwafQOnP?i9>1@;(;? zZo|z4Q8@HAL<-G&d3B;Hb#Px;qcsiO)v-@jCMF} zMC0B}<4%>tPd=V)d^jEFWGT|NP`G)js;f?gSvp_oLKiJttXgJ9Cl#WO<)r11lyzKB z!-Gb1($k+f$1{SGn?KTp5~P7pAyYC?gw0z#cak%sDe6ERo zk}T^tHX2>!2D~Xa&uvPwli^=DzY)S8KQVV&ohFEuwD&kH>l69D62e>E-Bgu)phP2? zK-`+pGKqAmMgT;8Vlcw)6oBUOZ9(I8kif(EHsIN?T3K<`pk0Q2Rhr8wLTm#(8MC}H?uj`nKm21CdQ9-H%CTbi=D3B^|jvgJ@hPxfCSMk@JE^XOT-ez z4?E&P@sB4R;KF;uvmiWI_FJbPx@Tvvur2bsdWY3mj)K@*RRCmvpswF}RY@0OI%n`JUh0KPKCS{gGx|VR zlqR_LPQt!jhVfy0i5gnkQIk=svvTN7;WvEivy~Ui2g?BRSQE}VygL|}T!DGGJ* z9{UXLV-jC=uR+n~Ibm>6*sOas{`1e59mCg#J>lmUCE0?>dqeNb44uSJ8Wm|Ta{OK9 z`k$R--$aM3f|BMsNX0%`lWHhyDc9M;?0&X*q<`H=`9*qRX=<1)i7#v6+&MDi_3SZ% zG0O@oN8*SU@i`$9sCR=#f?AWQyP}Xj@s$<9I-G35K=$!pD;bYmteeq zIBEaJn&jr;=KN>IzbK8I|G`xI-@v^8K(qY^Yf|R_%bH{qbNFHQ-vB2$xJVeeSy@Q9 zzaV+6Y;3ImXTZs?H~u%kN$!7GaQ_Jp$Mqi{4lZ8i{~zFF;)MMM3yLJ*6^>~^ zlH`_>(x0^V7W}3FdGQo_6zviwoH8H<`{V6yT0<`n9zSex@&=)Aj63Yo_~iIqnqb2; zZK;j89mjrOuvs*_znGZKPJN~2zCPjeX3~3;$O_BP6>Barv z?t}%*zF63iQFzsRFgbPjSyS92d7bisb(``*#K{O0-1awkr%e4ij?SxhdC%sjBGJvA z>TMFMt)G-c698wyV^m~Z|2x@N583QB>#R7+o*gHOE1Lf@{ki`mH~x2;{|{Wq|1sPD zOE@Ga7aX&ewV8{Rv)=y=aL4}&;`q;2@}D4%|18P>v`YUW#r|iA<3AwW{}JNI$@aC8 z{J$WMKu;g^ku9E>nTwjn_NIg?y%fzBv+QPu4K}FSx+(Ksq!v;nu4Yi^Af*}#jAIfA zr(G_Dub{z=Si#*7{jbx_G58Z7Rgx!9xuS&T=cIdA z7j9Vj{OXREdObfKHzF-CLe41`b(XdkwwqaBN0d<4R}f#Ixti_HzgkF8iCU#49dkF? zUFbjlVqD}z01R>io)8e+NOpL{n0>Pk08j2uAB?R}wT?j98bp|Z*!Mo5wrmY;M*KIO zbp3qwa!y^vQf>?!1&hcf3wI1YgU*fp#~1KEiq0wpl9-1}(&b7*snOP{B z_vlXX(by}|w|M02k)acF7q}_$*nGUj;486)xy%!k`Mk?#=x|X(^{^Zf-wQE}jF3s~ zz$@wN0(uqUS3$lYu`*B~)Omi6*J7kR4f_Y_GvZ0gExNL7fXy8DoXlo4b|J*zv9bz5 z0QEJ#aL6PL$H%~}`|erg31F4m7iTJ9_3c|&pcVBi+k9c=9AJc5OzxKzq8EWtn6QNX zVuZc9Yx7lnZ?0GYp^P z(5dbi=wKm8MhsQ~*Ju}3S0rL<8rE>oKKsaZ*y6~XQ5npmK}Lw1#H@ZJ-c}4zz_tf| zwxr*XY}T;RkkY{Q$a4?nwa5nEtc45W?y^=sfJk~K54{x!Tg_q|q03y#xDn&-DL?4u zz3_y?zIW;wAr$EOr$B$R~?XOvEgl)t>KzoyRa_RxsNQ#JV^#o1%rXDw~^N zr!lk_OtzG8sM|Z#E3o$Dx)sK^r?^|`ST>!>B7HeE_4?A00RVn)sjz_U^9f}Opim)IhUN6^TG*W7IpB0+y=lt(0LK! z{{$J?jouD@6V5MDalP?Q@=AO~?26nT(NEgU5=;crysbSO4uU03h5D{WYa3)tegHsv~td^XtcR|Se{4r;`+ zNbT9%GRS?`#;-g;rXZY9+|;CAhnNG+vHf}CKcsF*9a2xjL(Zm>rcCCB*urbD%a69z<(}_wav*;hSK((-LR2b; z8*cTV8GE&Q9caJslgT%>1GYC_&zFPZ&rm+(-LN|`*I0glz7E1crN8?(-blH@J9^K8 z??NBsAMlVO@^c_lzI5Lx%Md!P896g;im>_|2$#Tn3@dWi!N?zYT$A`jyA<%Pgb52b zPJ~{4Uo#&j9%fFIa3g-R!Inhm3|E=jn{AuFDZ&A1KW1gVNs zX15*)ACxF;Z#^?cdV0N2nY;#`3m+KHu3eA~rMiLz8|74GY9(9pf4*}(>Yaf-LJx=Bjn;Bx8NT> z#GW9<>L{fNaN2&PxOdYRRK;C2qI(fpG`fG6H!> z6f1nTN$Qo&OR=!pP~63)8V7H5

{t8+$i)3|=(0fm1fuy|cJzN{geE~+qN$m0Wyv+99P#xa1#pD#`q@j~6Ks*#_tn4;m>vhQNQ-Yd z(%$L0;FncIJ$CPmX2BmfBFNRCOkP-a5}$}bt}vkx)wPhty;~g@X2&wXEarY}Y{wt4 zYT|3C@i#EOQCa^d1k%!jJQA2C_7W|H%1QO4)M7a`%Aqj{0jy|@5b-|$o#V$=&(wxl z;}#u83$*~z+WkhnCfV}13GMRWeA=jj$8DYJ_iuTw!LT03lfUC_9q=g{6=bXk_eiam z56z{XHS2KvJY_&-Yk>MPq*|(J48M81;WUN{e7aJJUyQR;iHGQGwLxz;&O4{xM|oMh zMWT=+e{v3P*|!GNwp7*bOFPmV3A5QwymmcSyDD!P7Z)t+ZIp*g&KZZG708B1r*(CO z)p;SZV@1t=yf(+gm(Xi2uB>fc9cjBjtTcPCM3G!i zqS{F?1lkOZM18ZB*1l!+!S=*=q4(qQvbnY^B+y;JQ*rg+U$4<;6>H&4v^*iZ*=$F> z=d@%@Wy*K@bs4dx=6w0E?Ink;#00q7nu;Z8H5ppe=f2I%%#T4G7p`=@X7ISNn+-vX z^z^`Ls`MO!*@i?u81s&EAMY|KZOJKWG@ObZ?PBjb2N-U_ftzmd2V&W|6W6T;qwibB zRp;z#YXCtS~sqq&Gx%1fpAsP6~Uw6g~B zuoRx?sTl93Asun{HaxAy;FvqrAaD8TXSbi=XrTP2aIWx9{clh3;#Zol4(Uj<$pNw7 z-m_A7-n1b0uLS|OhqQJt80EQd^kgbvl!^XOFX*@N-^_~2!a-p4;K||avPca}P3IU3 z7sFdp_euS)#1&>+?52`oPkdT35w5EX!F0i~$;Gv*Mh)(*qJc1IXz@p=70Cx#8KbOB zl)U2TyooI>UZ63#m`$kr;S4M z8mAY`M9&wTX>*aVSQg)t@-{<|E=hW=IEd%eXG_oX4&7sr3Q>G%geH0`{-vx)6*t`~ zxLv^bSjN~89A6NL;vN6(KZL4Z_XwqRI2?%&%!tJ zgS&I)mn9^()DWUAzSxj2kYbYN@oxi% z>K_Ux1oBFGI#1X;T8Vv@gnh(kI&Dq1jS8YCd9L+h^X= z+NBv8QowSETz)9Ir*h^h%A7g@p&|9vTHRAeRch?V-}~n?j?}@fG4QeYgLH~Sms_GP z-%?Nj&Y!-F)*DVai0wbc#WtYQ+fBvHcQSGmWQO@?&Py5u?_@VxSwC7$-u9eyF5O6X zk!W3rT2>Tfnm+P&O7~2|7a-O5a5U*QvxGy%WN*k&5n3YLu?_qL?N7mF@{k8G6+5C0 zk?A?l;EGghsZkJ$T#G?#0jl+hvdKxwqBbnM#ePvUMHytn^wV9pnSeweU?2u(t;_Ej zT^_%kR|8?4T^Ob~2;hgV*3K&P_VXn<^5*QjmTZq{{!Z@GV=SS)PodxTZk;>Lr=pPT zLukxx!=l~;sk;0`$R7clXjQLY`pOmsq~^n_vsJpd9&sHL)GB`>;>b2L&J+-fi`Exu zjzYH1QtFAz{PH}901=Gl-cJkGV zcJL07+f|d=khT>>A&(QpFM6Q}yTF>!zm2{VSB-5y(i(2L z#--S*ci7_Pam;eHs?8cjS>t$a0m=d@>ImQVm`&`A>U9~vE*A28lN?_kWzP@=hhMq3 zuG>BjO127J0`e}F{%YFQx@pVVO8-p%j<{a+GZkT?;4P??bM>nlc;1Hvwi+0^jx&-?rS~r`cw-UIsgC9+mVktHcbbc6P|S^U^VlWA@?n>BA8M zmEk3v=-$28FB|g!-RjNi-_jiXty&q3;cw6VZ7IH!*&U8d*w}?bG9XgYG#7t)vy)s; zH<1_+CkVoivycWuDUjs9v!Jzoc!BvUx4pAJpWYDVLZhdWpyNq7^ zy;gaC;5Q@lUja7Hw}{o6I9en`EUsfE^3vG$6rSy)g-*O#=&Y?nTTop{w=?3ys&E-- z*f4nb^EOZ=ho$~ZSBwsl($evHN)7QoF5%^Ph_J-2vYULLdX^dy<}n%da1MYA+z*vs zG#Ongo>?kh8X$kTf%!!>em*963|Wm8Va?7_8hTtN19=SDg_lzp%C^hEHfe^&G=Vm9kxiIQSe*)TRh{If zP?l%}xY(yCl9!#T>6KV43Hn5cA+kdU8j`oF9gB2E#;xOx>z(uUqd^)WuN(oUf)sKVh+u!e^geb5}E);d#1We1ZOI6 zCMz5YtX^bL28;GBgV)(!7;Kt5=bD0b$&A2XZ;1wo&Viw|6FoIJ$%KiqG~Lp`0KUTZ zJes&{3sw`Q28`x74sz<7ZOe zG(1Wt`Q)5sno04UEPwNxE5lppBu2euBh{wqy7jtcfdV!hT&NijEq;oM%$m@eH(N#` zESm@9TKGkjJe{|!so5-1!PuONXzn6R1{Gp7M^@;{>@ladxGq`P^x`6)t@8=~>G`!0 z%QAI=13bjBKdB>x`CH`-=-|US8*JdBpj7Vx%w9Oe9{Z5YW*{A%vJrfZ!EF_Wam1PJ z)%i>i?isWTZ-BN(+I*%8Y+V#yR+NYb&A<^DE$l@by*CrHU-75`t&^71t?cY5yVP_> z-CcWB26i8TG*i%{MGyycrA5CcTb-!H*ryKgTUNJ#VNMemClea7H1Hf;Y&V{%6j;!eS@qV#rFj z>u41c^vZ!xP@$L+H)muQaQuv-u}l{nI~RyhFB$&q+F=;uWCox!#=~qBBX|lC3bOK1`{RBjWEYN}#y=($8w|)`tBpp~M>;q6U)7laINV=g{#G z&-ZN~&WwEV{uXTEZ|7~1&$YpN`@SR&tJ+eAncq1?t!OL33Al@+u3Th1b$E^c6mhTS z5zi2>8VZ#z9vj#%;+x6@c^yUlFiRP!h%4*j(oznHe#j>VmT}=RR5HUDC1>*sTLE5t zsHQ7z`!AtO_#3+oPY994=6n1v5(js%gl2XBK#fh}Rq_=f+D4{PROZH%QfDvLu1$w1L-5D<0F4X`fV0$+F8=7|WxKD=Nm4ng3BjPJ^Jepj^yr zDxuHV?q zX`LW)@}@~OIC^qo>VSnHN)r49j(e5ZY z=-|>Jyb8Zjk{cRs@~77U8uVa|()fL2w!v|<0!!57LWqh@9U|tLnFhSJlW{A2#eDTe z&&gj2t)$N^Tq^ID8yl4!Dh?{bDsbCOyUn^Ab7}#TwHOrujByOS6zQC{R#8?hUhXYr zLp@&iSEob1L#7vnh|HvQ$`X8qq)IPW&Lujz{P_wG9?e&QW^zo`=v+P|0Rl4#Oj4od zagI_ORTVIgYPt;UAVxYV4RcZn{;Ek1T}?eN2ABT&1u>oOgTE}t?)77I`4JTnY+ZHv zIGA!_g)(IRr#R;C>IfscK$>FBa#=aGnSIyMu=krhFa@X=gAdZrKe?#y$H2un;r&?Yzll}JR}L5B#gl|Hmv&< zv`A6Kd7|Y1LrN4#ymNAYdj{1qT$9v|mndLN5@${$K#sH4KZv~nk=?TwvPRRm^)sO?jbZ5%2`p!wglL?lQ_ zGNe>YK|R&rz|bjF8XN%03P!)G+24a=Q<-a?pvxqBb8*)Eruwasy85dQ)rTv#n}(8~ zx2;UK>LXsb?G|+$THBjj?KjwVa+-*g8rck0@$|R|+@_)~#=Cv@8^JUUG=3^=Ww#cO zL@&5VV2eTfirwFPFGIHikPo=p$``xb5U#O+{z?UlrurV8hl34k1i@EKV7i4 zp@`Na-BZtCN=olnyrikdbjfjCJ{QEA1xU6(YO%-rmi(G$~^y z`4Gvzf?jnzd&XZss8x!{gPo&)n?S;<#KfRNs-=z{q+^j9fLMD)Ca19Z(%ZRxhn@Ay z1dq%xlTas<6hs5}G3j3)j2~;@=)aUqfdc2@yhTfu+Y6QMo}Zn{&I5XA_livDdr^Ia{G^%JS=R9GG}IPhRSxPj$dsPXEy;4m3D)A?hjjWj6Ya0!Ok&qLi|65ad7##bcr1FVop2DA(mOW_Uf8KIDQ+$FiLhB^ zj9pueuc*Mvgt~S z3i1;2ShR4=-`|ncD=V3PYcRGVqYuH-q^uH3q|jy3$r#u&W@lH|#B6XdMKTr*@Qo4X zE-_TnJiPqSkh9+FTjQFh7gK=ScEV5JkY0gfv?on!Zr^YrH0Sk>9Wr0(5Y(L8CU;+R zwXZdlR?)Vo^E&9NCyl#0{Ym!19nh#$o06T7oibO?)x9DJ5eS*>!ri8~#evFBqf+Ld zO8Q5{pY&6T*CkToT@BaghK(M9)Q8q)WA%QF5;RhGFM=vj{nzbZe~s#0a+#hTqe&xi z_gZJYM=Vy~TP9Y;?dtFEGwPW4c4pEJxguw`$ia9T7Mz9(DoY`N^Q)F_GoLXI8({DF z74x9?k5-kXGQ(=!>W@mun=LEYKnU|jZ95Js!vG*iNwkaw#s(yGN#ZQLs)jDs5}krW z;N1f?x!ncBpPqoE!e>*`@gOTgC5#vh)esod(D-M#3Pu z@On&*S^LwX%%Y!B*_7Y=3==UgoakH;_=dRKDm7;}2{DlQ87wLYlB~v}I3~G)qNy z(q_R}U;sq&{2@eg1+6N9DyM0|3zH)z-L}7a5(r`lJS)%TU%UFzIwms-zCj?4x=5mEN6#3%d3Ci9Lwew6mH(@hD~ z%l1xW(olU!kb*j*X@gj$2imhY>3Bu|#hc=MwJVuQnL^T$V;`-S_?yhnV=#<_q<(oH zmQ1$bFicrsC(dCHV#Lhu4FkOr`4s)#U#zsJ(Rd=0f2Y%8D~21ZlzVd1PkbY_-C`k( zX#3a6dZakC18_K1XiHZOc;i}Y?&rMAk8N*DS#Q1dtRn^HG9F^W;|oUV-#nRPkU?S~ z%#Oxvr=UNgPrWvQK#4@DNU2TAnv|#9uWlB_kj#+G_a{>dv8jDT% z?N2G%F*(Kz`L=>Ql5Cv)S>?0T2c2(qL1s_rPVCdYQXTc`H5M&6o4B$@p*!ALREVfr z^ni}`ao|Rh{31-0#B_TB51#9sEVgR91{w7nzMg`3i8$4|IUbivNo>D@>YOhcGv%Ih z!@~T!hh2<=1PI3Ps~*gRzi=MTAc!)eRonv0eMAXQd$UMzJR}fQ2PZTNlzH&gQ=j7Z z9gU?Q`U|v(JmeKQ*6v3)7771+mJcM*K~d@Mm=-|_yHtXWVqANtk}$)4CcVec(z`IF zWtGgPDiSoYZX{{#{-?Euw}~XMBK}I_+(O~=VoB@v0#eD>6^XPy7x?;B_uCn>Q+__h zQR8>{CQRLEjrDy)pqGogdrt>9=+ThP5L~iO^?>r(Op`fP8EF8`qRy<&1-7HVyT7j{ z@kcyDJ}IE#NlTPiILMbdK6QW1Bi+2*w5erj{%qCc)cs;<#~5cBQIBZAs1(UF_OHYK zb=^*bFAe|sGr1qAUeTfe&4af{>610Vl}$y<<*_bM3|#zd7231nRgLM~0V{SwR-ETtfG}Tu}5E?%}8y zaa_npcU^Z%!!O_<7RGP&T>cemIBZK;9SU)JBqkV(f;NmIw?Un7)cg0J;!NY3d3Hr(LMQ<;{t!P#Jqli$BP9Ry^|-@jjPKF>nq2zR_?4$WCBX3w8z&#?j{ zq@SvEv<)L_1)c~Qeq9q*t+kNE!QiyCX5B9f+&#|N>~Fw^Y6gseh0?`C16BVZy*XCa zu5e%Q@A6+K-gHwDQ=-gB&*l{ssUy(SIU6&i!*}_zV*J(AE2q5Rn7l6=Py`Cp@>^B! zWnb|nYnbDtn0%f0V6c!3VTlf?ll*<=2n`X!)*sxTdX(|~An|G<; z?VBESLm8nNjv3J=P_Hre#2aJ5sd6TCyhH@Sf7(TaP+Jbzf#xj6QdbFxtRbXMQ1-$0 zQRs8n>8hwKH8Sf&+SWHU{p`qcT_+$#WQyAqWvhsYC^6qMeXoxaPttJQ*izZfXt?n4 zd}p{4Wfm8ff`CRVlGTk1A|u1DSg1Vo=y{LJO1M@v_=RZQvFal6t;bKcl0_DV^VUOQ zYNCKx2?hcid!N(>(p-w80?R{|O?_3RSuIesK#H6rng)q`Pg5WYd3QlOLi*<^E5{&% zj!X{QvY6Q##3?yhvLu3ip*Jff?pqYwj+~7UGG3y3JyH5>msr4e>kuR%Q{y_--EFB*}}c|j(W%$uf(Xni9Rz2LfJMYW%Z-A$xF_iP<-HH!FU9 zHxs4(Qs4}vVR-agl{qaA_ z%X)6@QB3s7p7O(C3wG3hFcoyCTT^^VdvcXR!a;;C5P{zJi z$Mog$AY(k?CcSoUoeml56~U3Bq?>aBVJ!wVqgf2C=EXz%JF5d(NM!czkl-A~-D`r6 z3|wKj)ilbI+IPD$qsI1Z5#PKsIx)aBcmNXWfTU>X-zo~P51dpPCtZ>tG9+@7pUxdW zr|WN&G`YzVQqy3l>Tr4?l?Az}!F$Pca=WNA6>n{*k6|X2Vg++`kGS~);Z>h|c3$?n zrE`ZGquXfI&RRp zFE$-ORx{W?^v1Zxpo+k7KmI8D)PBG|$qXeDG+;Vdy6@|0muQwCk*?z;j5BI5@rkO1 zA6exn&Vw=LA7e#u)bLGV!;wUsSMcyMmo_?K&;D+UiFqQ3d0^&g@h3;0Mros9O!?li zT#K*@7V0lm97)RGJm?4t&zcBfk|jnspb00?HHjl`F^topui8n}()gX|l;?oTm-m}j zmHJ5hDM(igxXLv_${v2hika}ZZ(P@{7BlejxkS+YhrsK6sW+Dlmzj6H@Wcm62hmPI zrQ;=j*zuJw*_+tYT4BpBnSM~M&nuRgUg)qNyweVpx+o)d)oVv#+T|c#!3*Y?=3ML3Rn1k+HO}nfUmNHKiuHv_< zHmkR*+hjWU?;!3aA{Hvu&Qx97Sv0hD(-e0hFikzEui?T>%W*xRG^Qx3Wi02DU6h!! zb8-rE)EXt)M2twmnoZ%66n>mQVi3(>wlQHg=*3bKotvK zXxt?)^pB1E{^~J~y%u`v9t4h2qm_#7FumFzs2{$J2Zq$c@t@^igxzGO_4Bh1Q;UAH zM)~@aWCY<;KT+z|sesNmRoQA}42*#0s(Fku^^EX}ZvzkqCM1u4S%Zu6P1f=0;?!oo z_np!Xtzz~Ds9a#S247JUKIS0Zw~My}TpI0@)`3~)xR$}-6_RkRPb0T-x57gsU&Z)o z*;s!a`XQtL`Uv0(V_N=Y-qoQAvNV>lg>ThR4Wvw@bH7o(}Kg zpJDkuZ=Xb}@nF&j)^(eF%#Nk$J0WUl|iWVNT-x3WS?fRh&$BRg3|&XA8hdXMd}l$9u2Ko3-JcJ_HktBm0dbnGXYVl5%{zG>RNJZW_o% zA!7QgJ1(K8>{8v|#pQWTt&IJGaEPssW))pQ)2BD0$NbmAYff^r?@;QWD3dqm&gEEM z;V1Ml&2qX&;aFY_5vf@ujX<(GKL3zjWq+`P_h+4#nD1h4Xy95SZla)GghX)J$SS?P ziiAOW;?UX!>g$>P9yC$Ew^EVe_u_BLZ%$e8?V}ZRwWkfj8y|G+j)$kgVjgvvR;P7% z6U>4jMw_#mviF%#+?i$y?FOH~=%^vk?=jk*+X%3(Qw~tH4>s7);^if%RLM|#?fXYQ zyJno~>2x&ghY?y15GF`e(Gicd{U_7nhp?ncIpSc$#n_Cwa53U`-%*{1mB0!;TvyZ5 z55%WP6420t*1nUZG=0ZoTH9<2@BGX8G`|`NdbQ|5bGh7gXH!mTRnb2){S#|c(XWFf zt?%k^?;^IpXXaxm7bs~giTNuv_a~qJT%GW~!$l37tyNDhm-9B*{;JW9rFmyS`0ult zNMaS|k(|QvHd;=QSR_&@NL^)KP6eD53w>N!=0leE9oil})+z!`!LnZ>fV>aTm5)i4 z5Hi!LRZZtg*Ghve{n(n3M-s}rs8#tJ}pZN z>o?5Irg{jC!CPOUxKV$h(K;!6TEjucJzVA%0_K(O8DMhaw6|*qm%5qTY%2%f=5;3XPj1)^(M=US9r}NQu{bJ`-D54sy|JZiu!^)%&Vz)OU zctL~3IQ~oC8=^Qusqj+(gE&U*b~$2_X4`F1;pm(kp84T1F~k+58NSVzEmw|5UCw9; zLnjHO9rGb5SSXPcA)>MZEkK){wLy*y22T4f!$^@26SzfjjhRB(00jP$`22e6O6Aw9 z^D04`g#Rf*GfQ7-K&mX-iwVc_L24IDR-}`zk|2^gcLe6iL^6Ck&@OMtm94y__eW8m zJuXb2b^=enAOy#3iOocO0DXP(cm<2Ymq&@x8Ty7Wl55%LVGd7e-ZDGB8gW&8O;~#$ zzW!Fnuu!#f&+dz-bVT8NSWw6nIP4yMz=cu)AA;L*2f)8Zo^^G}wuy1Y?vkVQ^zZwnhZPE1khJ zNg3Jim5o8eZ8`F!(s^>yjS~~CnoH0p8Lu)4d!~$Y-o$rBugmxpKg21mrzbb`r=AU4 zQ|nmFs165?Dy+jrJ7_E+YbnEUiuNl()J>yvii$ZhQ>~mTt+4zdoJAor=wP-TcrzPd z)it6YwKQT+*%*KSUSs$B(~EI~5ALfr8W0F61pSfa#^!30s}0k6icuFe6}S7Lm8SmpE_{M2Ww#= zFy8U9zMI(oQG;h~VQYn=Klo|wgy>cdzY>0DW*A3tf3Omn666cB@AJMMf1X_H#)g`rJ)OilW~$ zkJ?5Q`!0VbINga_MIFth-!=Bh_qcFf`f;Rb<}{g&8mh|``d9VO%u_e7+SAGh!Dh>o zW9znvWft{4g7)+j3bU+f%P_?hg2D?=g?*k+e~N>=TbjphRs%B#7RRI--e#pPBZYG5 z(_j=TJV>yvqG0c%IHV4jB|ce8bdi=gty`?BrdEa|^BTX1_O>|Nm*6yZ=7)ePRw!_{ zJ>Z!$x-1Bwokz8Ej!C1=S5E|&<)zro6QMk6cdRM_M>B1y4z|(_sr-(|e6%_rLdGE1 z$ZgoelWi^!a9HG=_j@wdpa%C^xc&{Bzd5Vpy5!oyy?QFgPssaZaC^V?RnPs3etrY~ zhb+pNngh{JB5=*SaIFUcAgJPa@!Kz}^BKJJ;DwheNHBf>XrAI14C9DvU}2Kr);l%2 zAOxN`d%Al}*{vh{k-5*RHEuDJ&^awKeX zzon=|+MWTPzH-B0TcaWtlM#toKPj#WupyPvva$fpPrX>VURA1pI!7Z504RYE+i=M_*7c8-y86$+zzbKdNlrtr34~5L}Yny3kGH-E+r`Y=LH3MMkdlj zmNj28tWGA2YwEtp+?v8o9s1b%)UxZ0>+Wcn+vhf^6=l=2RfcJ9vx2b)m!&t%rzm6P ztZec8j8fu`HkPFq$E;)_LUoBvG|lE#B7EGT4HR{(-qz5twDP*_mSw!j<@1@vm3Mo4 z<3f6lGLmh$Dh`Rj5*I;Qu}~N-&l*>j*?m6yw59l}d!ugAjEku8PqXvrlK?wGP?{Ox zONwb^#v{Yc_}yHA<^RQ&Q|w3@fNs z#gO8Vf>wOQQwC9c;Q1zZubUcjV>0EDo92+XTPt5~n3$iCnV zBjAw3+eDgG%lxGQRUqk4vn4!EsKHCg%XnO@jPswv+kOy|S|YSRWt=3WqLLk)7|)cc z{L;j}BnK75CYK*~3fls+_ciXlvhSAOr{J^qS_RMYA*eRFAiE#lp2Ge$uzl zq9^-Sre|l-zl716hUTJXU`99SO-=U(?k}TJj9W!*DcZ7_LPgTJbkVImQ(fM;s;7K& zpm)_iW}oCn^G%k0>Af>LuLSDjgPewE$E z8&h1mpv<*O^0;hXS+yOR=rZ>_JF?bZ+J`7N7z*UFAwSlOY9pvaBove&SMK*8^1tM# z{iFC{L9-$P!>HfyKIDFh>>|h&6+pVrmikqxMR?7M<>L*Bw4}iN?gnNs&37qt?Pw5*~*;>8_Ql zPU3CIjTMJM=Z$<4ArwqpOn0vTEEfUcpfA#Yu8#wCx1T7ox%Jdz?~~Ql)!ey$(0uzj zxal0Y+7-`|Z5UKbb3bUy#EkvyoBsFwh8CqO<{AT|2A5HhK;7S*c-EiGtqkqR4GOi3{*lu_dSf?9&5a+y>rlQ1e3^E{pl z6h(&D3QDmttL7xW1LK^K+gByZxK#@- zkWB&ai<`r$Gl{%u26PZzMkfN~?s$YDGbs?&YPH2l)Zc;u8L-i-^XlAboPq$;CypN+ zdzsq2>0s7}FWixR0IwaQZ@73H^~l(2GOy)9h>3_RqoR1>2^0qmy|Xf%h;OzG+6Nui zg?i(6JJ{`(=X@tazqJ3-@e7~S8sy@kK*pa5P75XDD}!CZ-uO^l`CWu zED8a*Q8HGhM>fuuvFW|jVY{3pY|e66^@CcUU|{k?N;T9Rs_uFY<~d_Jk+$Tg3h*34EY9vND3 zZ-W7(j`r*39Uh zRFz8YbGe3jtmSbl=ziyUnmT@&mp-5F#q^PQD=(Pi@RusE! zc3qp>5;t9%pWi2D7e%VF{Kd5&ot^3RmZ;g~{^eh$Zo4<)&d<^7#?R8{=(`Y4RSI(b zV#Nx}Dl2Uz3q+J@59#eTW4gdvVDrj73b&5e`z*Ybw^hnBib_4%RZ(f1Cz~s;Q`B4P zt#fT%vWH~%%fD)Sz`oz}Bzi)2Q2sUR*KAMNUz8n_A6Fc=JZC*=d)|J^^Oofk#V3}} zY_a`v>=Bnyw3dotq&OEk!?|$w>|7WO=0dMG7wU9kC$ z=JrB1O{LvS>81Q~_Xfun$#%zXYOigt<5Btv`2p`4uNHZ+cn+A+WR?}EDVipXP{+Fq zc$(n>cN{cpVI+{c|0vJ94JYxZ$U++g;F$Qo5cOYfH)7C9JOkPB>|;aXB7{-=S3x0o zvEXe5f)AwS)8iaVFc5<$)j+yRc_te%tjZv3Ih56`v`qize~eWK~-EXtd5&xr;f z4BU`RQ{*@3AaVi2)R$kLsNBHx=sTRflHNiI+2?>+H_PM}C$f`dV#0_+`W(~S7iQNt zWBG%-mfv*6!1il;;$B-Y(Xe3i;fHVC_yT4m^Pf5%eE62p4abLq<%>%k5zbwDc;~fm zRmNCKOZ2|a$O==*9mqt^1QWJ%0sM98ap^Jkk1j?MAUYr)*i3I>Zl-rLkI_%d*x4+u zluaz3wwP6jv6~PFUD8akcE4QdEOtqTB~+_qNODL*OFmMW5VH7GDo)*`?o|(~nIZ5- z)HG6aYF?d$=qdGSHLE6=Qk7P>`d`j(H=I=UCv?1XL^KA_zeH(6R-RU}0iJg9*ru>J zkkz78Ib?9EJRgZ*df9upyS(m|6AJuXd!yN4<(>0=Lxs|&|MW4|g% zz4p?`x9kNVv#bE9J`7Tv1%=Q#p=h&wi(;$h2Kl@G5B*Xpjd#%3FxQxNnVBkC2!v0s zHDs02{AwANjpEwld;pM3i%0S8M=eOgk7DUjtr`QE3Iy6gRkEQXp@ICkxJ9$Zh98Ckh$;6I0=02S*)# zxxUchbWl>AUmftvy=$%rpF)M;2Zw(t#7Qm?u}d9Ui=Ey{(ZyP8~WL^QgfH@xn)OSxn#Xx z-T1Zd?P)(V^3-ilU3nB29>;UCE5_<}FK+$q%*5C4d-@+dp&(?h@Ijy;O7x8rh#5a~ z)L@?>Av7%l5vvSK=py+{B&b`}2h`ukuT$^fcc?RJ=tQi z46T-cv>^E}ph&aR(*GjoI*j7`j~`HCrBx+4NqvB5>JI{70mqPK)CU*|^8)ogQssM^ zBP2jH1z#X5Wh30VNOcvNq};6`ONeqy#-Y=IxB?k<6Qfb^N7T==RlWG`?5%ytC8D*V*W9)-LCk z7OZeytzE~hcW&W^nBAJa+TGl4{T}Bnt_QUba`)>Vbe_Ws>@4R*=}o#b*-sL<(j$TbSP+~hFZO)?-PGNZ1*ZrtY7 za%9WC!=vL0biF!E><${Ttx`uh3n?X{}AL_ zxQ9BZ5p<(e@@S(IJ4dOH1r1MjJD=Ckn#cJKd&NPr+QwSWS;*{i%ehNaa2HyLiHR%< z5qDFzklDB0k{w^jY;*o@ZpSNCY!&>ARDDv<1^pJg#g(gM8cf^@kixk+F>!`WNzqS^ z{m+%2X=}1ems(RZ@UOl2olMK(u@A4vgf_kZG5+q`jX_s}_4~D!`_#??3|5zq@iaaq2 z8tT(P+njioa06>mX3Q4H^iqoeo|Sl7r^ys%tJt~hldM$WS1~JPtIR7c8)SpJLH$F@ zuWIhsJ)?X^^Sb19^J|uO&F@;y@SifDnvF(W$XF$Iqt#?J7g|`kS!q!gmRe_9_n7zb zti?)EW}8)Im8xkgC6QW0bAW*X&3C;ZmlG=z*-br4rvw!zvF)?s{nkTP%6gJcAsQ$t zrc_R#sDc_v|Etlk+R$UzX<$G@Uy5J)nk2;vv~uJXq=%qO_jKeP%}XV zT5{WLG3`zxl%tERw`_uVf-K?&)u5bydiBLKxDE3UZ|c||@LS(}@ZhhL^B((j242&> ze3lJMvY+|!Onl#y*FUzo@5Fb1HoR`#*N$b+S8&BKcrjr3z?A2sYU*nOw_S(TR(YTj zZ$n$%61|I1NsSgX_ks*pBSeH6x`t4JU%0%bThKP$HJ;wcHL(Xg_j@11k8w}BpY%NG zeJu85;(71${^tWHD~?yc#=UBPm47XBs^%^ITl}ZW^ED2Ag5&ib-WLhQ6N%|uQlI3f zxyypd$ZVw6qZ+;@S#!FE`ECpk#;!~3itI@+wUK63vzwNCtzOfN>Y4^yZBVK&h+|*8 z(|yo=FwVsJ1dh4+1P3(1qpwCdrmvQAtX*p3?RHx*7P1F3)s^;)1T%JtRd2Cd10KIU zP?0FNSHwIV2;i{-jAMyvyinlc}f(1_PAMYL!YQvh~%tI)cbH zJ02(TmADo6;t_lbpTXy`0*_Mv5VUptV!i`3#}ec@;GsOD)OU^xHTSp)X#u7FLJL5^ z*_KOGC2y<4x#ebr5s1_JB=WynnKISa=1eIqWN$2V43Qb(5BJrsIG(^^Phtk$(kw18 z>vDHQdJ?TQjCgcOKR_ChH??bnT@_T31(X0UrzTVX9|#6j#tVoX2j`t*$^?=oaY?{& zQ^pfAk;5m>95H1I$f{MPxLcee`)ZZ?OiZTFczAuLf^5IlW`emuPqa)eS=~>9NG_NG z!C`IYGE3l;B?T?wHgh~dAIr;fW;z;6rw~w}phWFRPp+iNm6}*gBW3A2?A^Y3<=Dwe zhtV!)lkaEG#`It^FCvv*YnjeHU9_e(h2idFb7ynBB;Szr@)_-AKd$HiqKqmeV}FeGWs z3%dwy3?sxm2iV4=n*}?EII_qD3!dfFI%+Gmm%pEXl0U(#KpDh$2q{fR*;49CCna~; zX}8BzZr4rsC|q{V?d4s(xL1Qle{kqH%HgGG8F~hHQ=`-?g3@I6fHwQEyR9X^%o!Ng z1#!6)*~xjfC6lul!H9!4VWvN4+%X2E()_Fzu{<#MxCcLbe`2LDwYGaXjoY=7uW z>+x1LJM6FE2k8wv`6%>Fk+A*ZGmBlug7{{TO6Gx#`yB8A4z!4qN7`gWP(31%7)hX3 z+eV2zDz~A45TJxWYv4fOOn?dKNU~-%>OnivK6C&{kkx-lL0%T>d#cH-vQa7VpgIrSE8eE`(m`MdOALiF!y9_)%XIz7< z3genfYlDkKS5$Ppp;TWk8e((>lqj&4bGh}CW%)gjWOirXZG$UYAG+?rTYkFcyJ{Yh9Vq}MsNIvz|uX?Z63eDX|Mw#?cq?PYh$cFKpOL+n1;KDpfI zvKP8NewRJs_R8EEjZ1Eqv2KcTN$sqIvs2g$qF`YP9gM_Lj3dhxsGkaRqR|LtEI3$b z|G?oWl*ylw$)wLzlQj&8<=94+X1BzjiAG$p1l-VVdxp0Q_V?_xeQ{H|H+>*YrxEAz z`yMVma{}+4IQER3KLPT=*|ASrTFB~ILfJkBu!A5g3QV9#KrzDSxN{#P?yoo(Iof2y718;mV8)|oWL)n^@K{KD2u_OD8-^>Z^bl>&P zkU#tL4NXREey;?vI+&0fy(z3x+LeVjb9ZxZagr_EmV({feTMsuuiIZQ{23>+==24J zPMS61-L_ktR7fUu*%3(XF1y<8HM^~@kVd1XtRa&L$sARUdW`g(p4TV!f?lGZ>&=^m z1uy2kxYtXJ7PQw*7NI}vX=|T)!iQ)M7T3P#wq&2v5k$^eer-;zk@FV>oLa{+Y%_va zsBsnj8 zeSLJBY`bE+b*pXAzAd^(vB!EJx=((u?ViZ<=yd7trCzyQ7KucQiWFEh)>(;#FItkf z@C9VN)n-c;DGH!KG!hYQdyyizrN|~{6tXCUR)t(9^X9F5LGi30Xl*dz%@k@&%^**b z<+1jC3jChpJeiK}Rs2aoD|S@N8|ACzw0wt*Se6STZ)=IUct1#n`&LJBB3d1#qSjRD zlVqKOXfy6#a26Q;*ahJHEq!D8wF#hZM{>M>a;>>+%2L(*#K^zo|Cd&y;yz-zh|+Y+p_G zIj!5unN###{$OFSK3jtS5f16)N;PO37PqGQ;-+1<)J2O@Chd&o{nSxc+^gbLx$}?c z{{h{j4E;k$R0u_BMFm-PqV;P7Llwgn%t*zliqjQzM2ed#S}S@w9cCtCjZ2Tv5(91@un#7Kw4 z>cFhb#uiRpFVZK@p!5Oq>Aqm8$fYHZQ&xnOUv@IUf}R^{7WUc=8bwmb&M*ok3ffhd zEWUc4F*7S$Io(@e(YkELga+#+cZ{vMrheI#!c*B7mh%?z1PX2;uDj=|L}_EzA+Gi? zC@PlG({nprL2S4Vc=KN1%~ljZDSVA^vKeD}%9Sb#_N1=y3@L|H&c+2lc!+HCWy{h9`R8@^G!OY|q&{!#Uz=ELBJC1F-A4_2ZKt|*bz$&ktxWJHfMi&BMD5+i{@U05T> zS|rDU=#miU@LGXGX(3IK15Xa5&!m~O_XWtH@eEW1!iFMg7Raz?Hg zNznI2PDZ{Pd6)ia2_vG6F6#^Wq}I6j42ij#C?J? zXtH?hWc~~cpcM3<)m~eD=hYy6eALjBO*hW-XO~?%1!LkwjO^R1n?rSJo2h2=D<)cQZnd7a(sGPghSAFO=(t``DVa*G(PbPm(jZY62}+lCwU*LaEf0M? z55QwB;^CS43EM5<6Na2=NfcTq=MDhoHm&QEv#ipuFDR~TD4V%%U-s#!XJ3;+EiaH) zriy0`tX_ARv`#<%Tgf-cTs!`Tpi^k2E=te2u}P-PJZZwBq0o{uBc{?hGh#GRnll{o z-qQckf95*xrcatrSf00yxIbkjPga(7C*Sjj53S++`Xm8RJ!5!_8^3B6phSSsP&rA&o!7MiP( z_@!YsBnufsCJA!$?xeewtP?{LY)2p`*|i#(+OBb#op#sg_-;XGVx_!{Wj)XnCX-Pj zl@cpJ+GHjx)LmMwMuR9;D*w!k&3{Om1kJMBU_Glz%rzH#t++*K+AEH(QsRsuSc2i#`MDlX(2&aV{g1 z8OVW$?;JN|6rtQ{kW+MNGHEn9D;J!iAY+`LGD=InIme$4XM_GM6XdLOXHZ2~Rm5>K z77~^95|w1WU+pgL{On82msb|Jyr2flL#0aXg# zdvuTMsGC)Lbd=&gxencj4HzMX{0WVxNs6UI1xv2V8N&0242im&s0bLLJye=Q<1Kqf{kAd4QbB~Hcf z>)iKH_RYUtyMKY(+Hjquqo`r+SF&5*%6>nKH~H&7!W&+FYh>?ZqyHv-D-4f{t z#!XZ!MNw9nd=wWSb^l~#ZmUR6tIwUiHnG-q0a3VeT2>C<-8#1s9MzhggGaN!!Jh2Z zKsrAqx@JSRi=xGL-X!=0`2jiAD#zmIlL8oV5+*>=`f52Q&Bd4+kxFunC4u0C#!7fe zuVhHV43U|=0}?FRokWK~bAoohfQx~dEP(;f3BL<2Oq`CCTQkuzrK!1=_O%$GnQ3qh z(9Su~&fiEj{5{(!?I5ejfG>@R>w8`oDp%r_)XKt@&JB12wV`l>Qo{VTOc4Zk8ua;DE)w+s6btqGstemTC z8z-+96(sXGm?bq)!C3IMq1`)ud zD%@dha$;w~-{alsrM&yR*lYF2ZOQ!toJ zsR~jl6_7KFTxCuL{Z?f~LMRGY6tt?8O=}(4;c`V2iMWBIDnLQh>4?S`XJR-Ob2oH+d+z%?BD=Y*uNF}De7IAWW zVxO0C{QMCG+0OO%=j0hPIcOT<9~_0yNX)%*hpt*J*tu#8NrelGXaoktkdTGAtOdL> z600OL^bm$aT2kBjLhQn7$WV(Z7aT$%$5?)vwnTM%~hh*t|0dKqJ3_A~p{`!!E8qYS&>jMe7N>f*8{v_jitr0tAZW6)m3EYiNm zoYu1F6OOAfI>}LKKA}V+c~;?6G8(PcM>7R9&Cp7U(PEXxtfsY;hG_y8kW{Or+-fb> zCNZVZK2Oa+8pKdDgeZ;U`w4e$QsbmrQ1_~7wJl+;HaCLWrHTW&q%dVQnIFla4sf>x z7syHkV(tR@iGv^cphAC@=uL?4`Gw890cCc)V!@Z$6$!|r{s;&ZBD`zHPYZH@ZaN8` zc-Wa5B3gl5`AlR(MOJUgysZD? z&%9B79hu7?89$4M=@Fy^4PfSB+B%B;LZNzH*|7D1m68gGB}cJo1tTrUONWgIjFj;O z>_-;#6OrA5G;<-hVmv3yL@rxbIQ4)XH#t@ZTr^QLlZ5n0?xdY~&6r_kE;f@u5V3@Q z6}WFb`kv6&JNv-w)3eXaW(>0*b_iun5T*1kyVC9PkV9NNr7nBi?WuR!XNX53E5xr) z*xg>g%N}!k(=PjTw->7P`h4~o)2Azy3Mw9pIUIJG-r%7G4}Q;sc~8>Q>p9>#?K$I- zdPXT;uyM0nXP=r)^Rw~ndcP;#RN7ifl|DR+996i0yC5D?$dOfYmzivGb~RDsn;52C zhY+dg^1}-!PcOX0hu!~L>C4}O4^msilMRzpooH*$9eFtRLUM7yI<{BLD*oJIh{=e{ zLj4@yv_7Z)nYo&di+5kr1@Tw2?Nc|z7<$TVRm8_XY|X}4&dF4}7g<8ZTH>Gc;{SXQdB+mXwqAg%tFpi3uj=4TP|wooMQ;wq=}X1JVrh2CIE&C8Y; zDhdlMt8SUQ>&Lj@JDJu%W%|ZoM|JOkM+c@|Ngw)b?TQkI-_I#Ci}2p=#=rdFL+s~y zhi@!_zuB;?>cwxLIF&-QxGw#Dz#J!vpbDy4NUlUHoqNzN&ONF7ZC?#OV|ym}q3w^s zKO|HY=$hd6)B`2=rw;m_O1)!yC-_cC!Bmb?e>kdLS4NoZQHQ6Lgu?HP=F*hlj)G@( zmX-+K5P0^&(mG$Ae~;~5{I>5Gso(oq#)tjt5{@=X?Y07^$!7`~lkt*z-@MZ0c!hOk z@E%IXA&}4TN?&VbZ{<+sfl8SzX)9?$G{@R}&X6_1NGaNBb~dJN@jc*sH^uUmLS<8B zJJn9NN?N6@Y-@6hbig)X?{yCP27=dwZj#(-4E1Bp5ncT~kUfkJio- zL!kiD=dX5Pg)`}(97~yqt0G!VGPwG*USJL|6aw!ILyNRtq1Kfm2J_^j3HZmyqaIHC zaHjGF{A18k+QhS9#5MP1RYs(xkI47Mx%KwNWW9Za>=`5LZO@5Br2o?N3t7A=uHP`{ zmVBEhptB}X>J3?(n6=rhc1kJ@`23VV6$n`ZDV$)7Q`qYar07yyoT7vFa0(|S@f7kG zdQ!+)LZ?$8nQ~Q;s;RO@QSPfGeYn3L_*&njF%c0kH|3A2{QN3?ao>y7pImhy7mz*%U0j8VCxg6&wA{SZfpF?5=J6wBICbh zcL4lns3!!gT&iYOGLZWfilj8d>NOTaMwS0^$x)fS)J}#OLI+WX$01TCPbcQdo0%2z z$K}$1G$M;CL#mJ=WDDDiLc!uPX~tHXoGqD+PV8cClkIiflDH+gr}!)E{mMHHcUtbZ z-Dkfmba&#e@z@BT*=mdB}GLl8IsvMoEq%SqzuO27NFT z%4GD2QYsWO87rUmu=F4x&(IMeU7SNb5tRb5WNET^sCc-TZY*A1+*;f#UYsdDUo0#B zK=uduBBlM9O?i^ykq!StP?GcQPpfFv7ljTiiIestN$_&9^Zq96KV%~t}O|v`FGo1yI?9HL<53)b>WjDrD1@+VL zCzf8p8~xqJ^fwbjf^wpwxTQXV25^7r)A^{gOH0_xqC` z@0Z3{YIpytl7K<2mq{ThoZ30&qCUDVX%5LWe!#Es-;e)M@)Y1#5bqTl_Uf^IAI6l> znBGTWeIbQ|RLoFe*k-todXE~XScAu-=ZHw=aTAfuLlesEEg;{n@apwCOi>=ar$Ddw zct-Kp1a0sctdPqwWw*)na+-W|LeS_J>vTMq~}81rF#AbciFg$)H_ zIk!9M!KXafV-2chAiW%P|`o_;l2sqaBo>#xy2h@ZsI<74_C;D2EKpD0Yshs~%jf^#}9HU0!qXvsRI zqcG5VBEp+9Sa1-1?}H=xz%B;Itr;w{hzpa5da5_+GZbeO58yN8)KiLlZbuCMeykuv z370;Wgr_J0sD*ap$%QnzgR#Az-$W%YUa;d{dOG>c1%8Lvc6=9aum>8G*%0~q#kA?u z3a3ftUu0>`goWp`JxtxjZ%uyf;!u6mASW{6mE*ssr%5(|tPw{a9FxnmNs1!cZbdR( znn+eB8T~B>urALrk*K~+U07~2J7g-mUujVnBDsw%#459* z5X&6kO`Z%)1o%GeEz2zc%gb%XsoSNPE~cj8)l0aRQYzVS>90|qA}3f666H&iH*9r?=tm`{ugsM*p^r2uKN5W4t-e2 zX>!6p{_)S#IaoZk@hO0wQqVD%D41Ds${MtVD9*xJDZWf7Yc1O*>$UV+w-pUrhOHx( z5vwwm*rMF6q%CD}TT@wY*=@`>m@{RJioRKSs*Ii^bGq!7|MBR_yq~vJJfGvJcs>U{ ziV)o(M2a7Xk`LlYLs422^2l+-<@D%uP5NOyrPVj;DY7H!7dS;#QvhoN zt^gI7o9@^l^nkTYKd0AI#g{ zQ(-XJX~~A{k^@(4{^2+N^m6vp-2p$oWo@-P5GeI;%(l&_y!frZ9sT-ux|UnQMlaDZ z-XA~9+QgG~%>a@23cb8v@KZ>Tssz17a60vRsnRaEh1#@Rs7~)ETgA2RM)y95Fg@;< z+~WX@?`Q=h6flwk#`lbr*4Sv=M?N&xV|>Xt4vN`3v>+dvdRw;#&M%EbOlTAs9L;{p1WTY53 z;<1kn1$`lZJeiz@h2#}@1-sI@BGi-|kPff|p@E{_2KS~`) zzEb#B;h7}AOS+rg9ilM{xRRT^a0Pg~3x(o#l$*wIS_U%+BEdxC)ikVIISjPTlU zFa%eiSeO$bHYp3S!GJ~L;xGc8+exN9%qEW@lRv|YYn$}yH2Dal5Nr~^$T%EigQL`a zM-xO;BU=h1wy`RkCI88X$u-G&J6mQqlb^d}@?sgDT$~h{B85dR?K31Fl@hm9MMRD) zA*;!O72^<*xrk@UeU^vK1!Q^fVO3^g&k&KvL_rG#vd@1$D)0qjSino4#_!r}9gC{6 zCmexA(Xms6wPtUdnV44)pz557#_8CO6;*|0WgvFNm$i+JWuKYgsknx!=qT|j{QhXv zcU86lf9sB@4BJJmK#FmC%ozWZ8;Ff=BUf z&zj9hZ6Q0gY*_nTZjm6ZAH|)59)*SQP?#3NtzjZ)F=0&hdby#Ppm<3^DXgK(PMA8E zJ7Q`qx6iOTzljU@v1l~SkElo^i|js`P8Hz%j(|7dQoGlnLM>SsD}xxXbgsd69e5tO zf9g)IiGz$<%w&GFeBx_JIp#|NZ^9EAX8u5h?iFCqW8b09Uq5%`)+hHq{n>wIw_krXb>fDFZNJ@i;NhQcT^1#0 zic?K=ujCH22>nsG-MD0zc2(I4%}chUvz}Y@rtLekeq;ODELpi^nx;ZqX`2>EmzT{- zEs_-!avrX#pr&AEL2c9?t)Fh6Ucb=3uzt0Db^UhBfMcL``IK?wNC+CMEjH^0+ou_IP zKPPzgrH$9I2;HZC%UJ4qD|lboTh-P4{N(&M=bxU>%-^**ci1^u3OhD-VGIc5g$uyE z@TCJf#Bd^-Kl3|6bdXESgYriYi4qQ*JJllhLEPYF3x~?eNq(NJ=8n4vPS#_>|IJuD z#XeU&EiI4M&GfiqGjCduDs9-_5OX@`&InJZjJ~AZ?=;3$l4;Soew)LS42SKlGp3a_ zTw556IoZ%;m2v)S2uW)6AeVS~RyflBwMi36&Qbq{*4eLj}0N8c&Sk!K3aa zEAvy(=f=ni<*}ChRC)CRQ9M0&X)#d#+hy1|c@9Ww3U~5GR5K zyLVBmIt8UtrHq=w(-&0Cy!vLzhH$)NnpdsSPLC$*2bXqyBM_*(a)v{r;ig53=kzb_ z`UV;wpXdclqxlFik0N+yqHa?NQ$=(bMR1GQSJ_wS#p2qa1iDq2{*e8l_>+mz#A}HU z6Vhumo6UPQyUdKm>IfptXx*|RmF0Mm;8W(73lDw|?h8*{~mVsuRGzym=X{LW?T zh>mKl$>OP9N90qWB`y7A+w!@y@J~8wYLE4`{Hs+(l7Ept&{?fesud=asij7W0UX6Q=N zC%b4b^#Z_;8rjf8&y8ANv;Iv*t3XX7Ut$o$Sh5t8dCa18DH)8CQF{eHj%=}WlG5`F8^1tpZ3sET+bW5{IJl$+AAXr--; z_F~_5tF^kavUq8G_Ln$xO;o6yRvf$|`!4xZ`JD0Br0wwDG}3|gSBz?Kz2F5Ul%}4P zKBjt-d(!_{^vUE2>2cKw?u7q%^h8qi6&VfMA&5u?7!_a)u;nr$dPbRjmO?*EF^Zdy zGcaX&10e8S`vY4zU z5ZL{N{#0>UA*%7sD8!iv#HNKIX3!Z6kw@Yx#5N_w9CC=I6D5VXIsjgItg;YI4}n*p z)fZx33Z6-AFGNlmc$0IvQ>O2VCS&4v%ZTM?QZAkN^p7z&8}%* z>AmP{rhj#D>F&Fq*wT1YW7Dm(QjI2^&*(~eN{V^<$=SCrcxrI&p2o&Kvy?y^ zl1aB@!#`RikDmGRH(y--Ojp<8jzaJF-i@H~xFbyAty5AZ>@gP-mbAmY``5n7fmX`fm-Xwofu~b>CQmHBRW9px% z$2EQ0{aj4LPbWABp}h=8J8M{V?8<+)!Lsd|SyCsc`9&WzFTQDhw6ZWL9N< zmHEds^|aGfFHEnVzG(WdW~`{bN*wJ*v=V4Z66h$|MV>%4Kw5sNwE$Hi!#tZ_iO9)v z zT#jz0{*sR?B+KY+`M47G%YKxPtI%4xkdLdS&&nUm$2I6mZsjB#@8pi>;}~gm>3p0* ztgbE}r%^`NkdHG^=B9jHf>gRY^KmKC>GtR2Eb7*c?)MP~7r6XbEOWudA#&F)kq^Vl5QoQZYRy##u2vBgSQ7 ze6ARmlQFV3=i@L&w&nRajFD|~J`Q7KyD=Y!F|z$NABQor8}e}&BYPwthcU7*$j4!f z9R7S9#>ml-kHZ)_ew>fP7agMx8QLPv!UmejGT_MH`#4_z-+$h%5BgRc)`c5%!74N-OjN8RBxwaLG zWu6q{E-`*gjQhm$-xlLVV*E`p9uwogCUIG={U3?(+_U~h;wmvcLW^c9Q#f6cC@c0Q^RH_F8dV zNejt+olxt5*!K1L+Tvf(rW_3Qz;j8PdZFz0e_5(igk4f@tJtb_lh5tSV@6Db#X+$R z-H@_9-w*xbTaj9$kg`lHKPaa2phJ=VBR#oEd?x~FF3ezJ9! z@RGEnU+g_zd>4}2_PKtXsD0vkVzj1?cy1i$i|y^mkI^QvCkLR;HnHA*@lDp|-(joR z)^_koy@O&(n^;$eSSP`-O=3I92oc;QW!C4*3;+hK5yz?zV!3|k7W-}uT(ygE*D1DZ zQ$Acf1tPYJZRm!&NnIO7{20vF+Aj9Y0Q}a9SjJBo({{0E+NRKqT-zpkBsXg7#C~cM z@9ns3><7eW=OBw0$EQP#Z5Dg2Q|y<24-3J34|R!kv`@i^HDc+14<|Vc_~-GP+QYd% ze7ys7WX~2inoMllwr$&XI_AXo#F|Vzu`{vljx({%iEZ;`{`cN@@AuZXUazjMs;$~} z&e?nQ>eFZcR+1c!iiX)mB{-wiV<*+}6&n=#+Z48~>`+clM|O;>Vx6r!w>!}~(}VLF zNblBJBhd=0Uv4JPbMW&J5}6;oUPtcwJ`jhT9k|p&GV%2vq#(V63$X0+Qu z4Wks%3ze6d@AbroM^1|dek4q~=MfDYsN7GGg~=~kaCHcgT%xo+**c_m#7B2=b=bb7 zZ)1=@43U5kCda6Sm`X<)@_+a~#*8Uv7M1cKI-d+T1ZC1bJoN8pwB7ckc%byq<{a9g ztI;OO9XiaT)R~h<6dS0*mGFu$YdBHxoZ}j406rN@QHt@!`^4Y7y7Lw9a`x$}B0(>MFf{$T^nU!XjkK?zyBka3V1P<^Tdu52OMMUN{t4-OB;~hEmxH zx6!FUuLP5d;n*?tji&+h$Ls8otAv2Cr~rRwEkY@;!&XieL?HREhDhIhwL&?->(`kV zxf_1gA+@HDS?%4yBj+?`DVWuM%)2Oo)#PH*j|`TBn;LGBp+yBR#ykQ76QzSkrynm5 zO4DfhiN9WoyU!?EcQ+r@o25D01f^z~=DB*9pu`{B$uZ=oI^-yPa=$7DZP4Fymj=A; zzU;1WV6<`Vmzq_5xM;&5R@oGaHLYrPc3@6D z*aL(Np^+`zw{{u61kA6CL4bgami{P}&ebt09b7(~?X<%Bk=i z#`8Z8IAVY?w4%pfAR(6ztIowSrG>1S{H7Tn8Wjnr#=4*tv!0@kAZz^CylVS6$aErQ zx@z6X*bSF!dT_6@v%T2O*-fdq*u}d2dW;i7-qlnLY%T`c)s$PJZLY2p9-OQ|uD&CM zq876HJ1IR_B(Qc*u7f{dSh5addr~-c6}4vo<{Itawm^U-F1hsqa+X60RWVs$ue}qM zuldCLPwRM&I32buwcCf?S z;^T#CTd+GmjktMT6Y&=)g#T*DHnrT0voh701(;plsIN`@G$C_LMo`;W3~V?3wz&j` zfW5S~vBAS!9uzJ}!SkchP+>}_w-k&6IeHo0s`vjdx26e#pRl(Xui{C4Bp zmv_MX=ze%Ueip23bpz5`Us?}rc5!scfTmMex66QRH_B{xUWZ3+&AK=`--3c{?we|= zb~tee_Q`ANsk#UCAKzblY;bG1OTdNld34(q;B9X7aQ8^WI)}tIn4MqeXdxi%Xl=%W zwL8PrpR7JP0{tD*bo+G;vC?p3ei{a>)r%O4%GTSP6FvfAWvQv^EWJu03yi1a@B$nj zJ0v_J)onL0D#R$#qTZXIaF>Awo$Y{OD2Tm>W&n>drBl0lsN!&B+6kPq8h$a zqT9h|(!$x&XA(0WeOLJB9N12SN`DP!7ap%I8oGnt;^wjpFZ~2ppWVr-QH}Qke3J-L z-=V<*w&-xvm+k8;15)sPVM0i22G`MF?jjCI_Z25&pfotk!!Jh2kT2~Z;<+|f7KD7o zAdX!s-zX9dSiIQ!S05Sd)-xm$W;mIrc)0J*xq;}+sLbXs{d(Pedo&~<#&|+gtIHb@ z#MhG!6JkuL*Z``H)rIM4iA(G$AYd0pC1H)}zEWCFaIxgPD#pM|8fz4#10Q>A+U$#$ ziOHlU@l?j5s|P{_)ns6s^Li(WFzO1D*nE4|WT16dIAcKE5|PWIp*K%ZsPHLd{}9Mb zgU7wY2v8)TYu)5e;GIAQsLl&tvn!|=3G?M-wbDqBwELIreW&=O3|}cLOnxKen%5cZ zL@e9UX74jBEjmuG>oE6R2U}h$+Mt651kHpma#)KXYA_l}Kf|A{w_)pG$Z1lT?7sDz zIXPL=Jnoe`cXiNb$ym-s1q8Z%VB!pGkZ$s$Ac^Rlqs+_aV;HrOKdb;}lg~fT4gK=W(0-4;_J3*h;bp3i-X*_as;buuxujYAo=9VXo4P#addZd| z5ID6JpfM_nVhMdV-1+9sA4HTFAA!Z8Tn5uT33@$438%5y&cIy=A^P&|vekk}emXu#$!W+}HgYf&q z(PG}O-KNFZ5;qFvKM#ioafGdhJGZsI6SW1n1~6qfxeij;SzkWbTiCWHf=!i?-nds_ z?1@^$wy+B!HX#^6tKti%lxCI0_>dBQoo-Qe-+fz!`XM5BSWN5(c6CH`SRM~`-Wh}c z0`;fzHp}GE4cZRbA7Lg7S@6pu`C1OpA6YcvLs;<5zlBr86Cl`uHFJi%0k%lq!57dO zx{tv*8UqLIQ9td$o@(uoP<8m(UdXde8s8% z3}eV@NTt4U7sT}l@$i{o%7riYz?~Sft}27t#h^<5dl%w}K9qS?Fzcd7HTe;!=LX2~ z2#6rm4=5=yAd)aJ@fPJ$DUgq$XcPRMxdm)DY6JESJ52K|uUZh*88ohvFC9g;ET?!; zR15BQ<+B7(f?rD2F6@^GWSi@}pGjxvE_|^g8sv>PC6&xrqNNBNxNQUma6d#LwqClE zgUZ2%rp}~%VHRb)Nmi)Ef(9ECS7a{C++|bE2o`|iDY=w5U*hIwS=1pQdaLF&S~@f~ z=!g<#Gc!}D_JTTO7?1KiW?rmTB&ZH;+TslrqKhBvl4e8x6dq^v zM)>C_RAH?#b3;CRaunX&YHVrtSyhBpwZP7xJVIA-I~IT9lZ(RWTjynBj#!guz?JGs zOKc@(QaV;{dEd0o3};6ANI%_>L+K+sbHvGVB4vVp@J9w_Kxx^usP-k}%4QWYio)?+ zdO0d=S+ShKbAQ9Vq`FRcdewAPW%=mPwjfm^eo(~2qRv!Km{&E-R|4-$S% z!&ZAYMYl%q2`E(_Vr1#&w>bylV=iT~miaAm$3-saNYKsXNwTAt0e)1nr#bT{_KsIJ zDO$A{j+?;KD#&Icd;~Wku&2q0UoN#`CeP51mi@PbzE5B#yfk}XxSWVcg6Jfin^C76 zDas(5arK!L?&&*an4~TOgI9Vy0#6`3pz+13LpGhgdF~J%1;hlzq#)dCr$gl_jflLn zzL)=@#^?^h6(V_lh9n?e?sHD8NfouUO*x;0mBjFc%W5 z9%@j4jta)_UswhibQ)i_jQpLT)Ehe}0Kc?t_U_9&dUORsfOEgcf`(~7)}?-!{)dv_ zD_eGScGx9<$O*A8yoRw*2tY{HO@QWudhG*5KLEkYtnihrM5e<*D4VV*fFod2Oe)`Y zs}zYa(muRc@LT@2+JykB09>gDVdQz>7M)c7I`S#sHWfn0v$RHhxn%ev36aD|6Pb{N zF00T-Kaa8tS4h1J*CJ75Li_P=FiEE~6bBY?k(m*!8Z3^;SQmn;C?=hV4igN-T?{-s zDSwtZqsk>uj_ZiL(})M}5{c)EkBSoqjmbnTsepkMGNg9@hc6wQtSzrh{13lptB7)l z*WLQ>EL_oFV%z<9{Wk8(9MGBgws|oKl0^pPrd42xTnNGx2?OiU1#lL2<_iHgR%yTmas?b zLo0$A;*QKS<;m2B(OJ{vX-&i3`hJ(X&((wxJ(;m|gl|VBlc8S7!?D_xHmRRj9YSUtlts>iX^yJl!#T5@WfEH~e8I<~WN}icA<@Lut;9%?{y7$p)wVHS3 zgzpo%dE;5pv68IWUdI6kN4M`LoX=l^^$~@4s;$UR0A`g}%75>y0`KT@;VOJ}1xVbU zW!CI?0>OhJd(44365wGz_g?Vx9HGd<+Kz^Q*d`6x;pI|Ala0#)D zK>osVT><&ednjb6-;#qqJrXZn6aQyk+IXYZes4jND(K^sjR)5xyHh8d_-^Wclkc#9 zf=G8$(kE7Nvd4Y|F8*-Te7}t}rV-WfjBp)ij``CJd-EH)KWWZqp^FJVG>N!hUFG)I z0Y*kfaV_H+WLrq~O%c|?-x|(8L38S>v|CLIZ3@f$#jPrBF}Xg!UVhA1A-ANIZB*oY zm$_@6m9GHYN@VN2T-eA@0M{Kyd73`n{Com0{4WCG{Hh5~dr>>0^Zt`L{I?e0DEXlS z?%cN7xWu48cAXt>K%T@dzkxco4oPrI*=WsqD)T7_>dOT?fE+-id}+qc1owk_%?WxT zX!!sg%EjveqOBEd{YXIPl^i-|FmT(A97hp#rFR{kHft+1LGNv9Y<0XE(bontV`*OW>zj@7G`En111?)?^fi^ z?JZrch&ebonLZU<%|AEt)!5Zs+}zZ`%=}a7--pLD(;#X5M+P%`=mS5*=#dF{7pJFW z6zQHNsrfV!$1D`_R-rTkaWDP#{ax#j-7iLqTjTRiUqMi;_MLa`Te&Mc!c{fxYdIlT zmzG!}7rbkimU}S^@{4D88LekvCx@D5|HkZ2SIDYU%lC2(P2bpu^U>GeIh^aBh&grJ zCu^atL{(3-vFkL3If6w;S}mc*!=qn2C)yn&T?DyiQ}!`o&%6cQuiSzB*`}kwpobxJ z=V@E9WcSlvKbsY1u09U4@;d#4@;>9N(7cz6?1!&BfR&tad^rVrhS4%zDNJ*R9?jMT zw;?xBtr68ATk~cpp9Q>2xF>|y5yPzdKhKCA<`;CHL%2{I_4^?HWGByo@LXyM)3%39 zo7tL%3kyX_gyrgoa44RmFd=Fsd@@fBb5i7H+_QXp$oTJ1vxAMCl zGQ@wYWPO9Mw=d{~Olx7l@o~Myaw`aUO}n}(sz%RYnMe~F65H?}bK%FmWu9RJEfx_9 z0Upe=D;~}OH+dR}&i`*w#m)NPOYZ-T9ws$66W70?r0(oy{ts2u*v0(swfp}UMAptO zu3}ck&VOS=-uT~RE-qLmEo(DZD;GU3E-qp&R#sv*ZXRNGb~a)*Ha22b4rXHRf6qCX znTh{4)=#;Ac%S`$XrJ=1f9Zea|MLFQIXOB1vG*Aw|LQ+!{|E1X^grcc|H}Qf@gJN2 z+yC|JKfJ$VPL_Z9^0)sx|40Ad{{3U;(~p1l^6AUpHUHE8qxW}zf9?HS_ph(8JfF0` z^Z(_2CYFCK^dG^$PW=`5r!jLAbAK8)_*dTeZ%!j-`6ubA!ZN8lXxLl-%?O{QzsckO zlTXqI5~MZ|2M;&b;5Y!4>U6C&I`P()K6Nm&kB?m z;>1XfS~b!)(#si|j~ab9NB&74L%&?-zWke>(}$Uy8(EX82iaYkUT*L=y$DQ593Cnk z9rp zL{hT}sgl?H30K>fQjW!R6x}YOXX>MnpougTj$J^AZ;Q*&)IiXmeL9j4%w6Ez`s%{5 zcjH+sbR~9>@7s@s=$EKU*lxfT`)+9V0>RPT>^aTv55&BDpJ#8V@NBU^Iw6c??67`d zM_uU$iV`FN$RwjX%3*)R-r+&Yq_d4NWRrCZ=8q606VvLo?cBp>_wLO++5G$sp>scS zK4^eJo_jX9tjpCTOrrI`a7Wl)fQ9CSf5qAlQMOeJr8-U{O>Nj~0JuR+?;+D6=PkXB zq>zQSdic$hWMQ}ct_VD%R5^3FzI3D;HD2fx?OX)D6~!|dXTDeo$Q#CD0i7dwr364q zqydsg1>u>NHzH-cE3!%$*llpAV)cfMse-8+lQP-$VQp~`3F9!*+uAQ#v2D|) z+))y~pbc-l_0g@F%YB6A0mZ*HJJ|F%U+`7)$hQn52?M>}(BHmv!e5sSAv`XQ?VYw< zuPn=b+aNy<(b11Nml~S#(T;3f`gW$>EzyE*;SQ7C547}^k68X~Z;V9akV%mC#tQqb zioJlnVJ)mmbTOA|%IOyBmWw@!^IOQ%nkzha(;2qcj)hE*aI zHS9L>L~ko|`^}YP2i!$p^VZ8H+~dO|`g^L#_du9ls=*{?x-_^ny0qrhSRDd96WgWe z<<;e!dLmlMq|i$*^{19=gLxV^@tc_;s3Jgy2%7<0^EiGg1Ehg^;Qn&=zN!_>;hJom zGv9IeB*;pnJ7}ys*bopg-32-(uXrkYt24ImK;xEAmg%VjCV@MqXd2!^I9s+NFjm}0 zP`U`S;TJ@)NYl&#sxL(s;U;Jf*ki^c;nv0E<>VF9<;f$$E8!Rn3rta%4kVk68KY79 zpZYXw(<&o-qaWP|#becJT4_SeM0NPK#$KG!m8Q*g0F#H>f>V_Hl+$W-6^R&=R+IMa z@Ti=gu@pKjYA(9fBrfU;6@R%Wzr6rzB^pbO=8|&_g_8IDg;Ke>nB%+SlX;}_9Mum+ zf%4%mel{S-E|@K?dY2GU9N*O_`cKq}XFZN8AHUA*I3Jaz6G|*H(^sr19Z!F&bmhRH<};8(M-N zzGB|+7FZyH4sHtLW=wl61;4A+f_8$v3RSH^@mE?J_QkR?G1G}80z@8Il=AqdMUn=8 z2^;Wv=(I78QVV8C=Hn=vip&hgzF=doBmGbr0VmoYV*`K37RB{$rz1U~r>gD09U zEXQbkfIF1(JIEuc(|zwUU$Vjo_cM)Q48ft!rJ?=Sww>(R7`hQ5D4#C|!w^^QRhm0H z%t;iUlp~4NY?5=aGw>Q^4`t6ISx5yVH@e3L=^OgTH^sg(k}b}y0l+j=?ulHtw{wrb z&9|t%nlLvug;TZ?Ov_xs%fRKW<`fVl&OmagycD|W7$c>JKHO`yTgLs%(8Kmaj2e8b zB`>`1)q(z+c{jqx!H1xiKeYZhQCmy-qk{?hMJTj<#|gTM$mv9QJtd#|L?x!W#IH}P z((iy~f;ei-LyBPNgQh4!A(FhSml8o+$o+LHcNj|zUw(pGGyRfl0CyM$^Q7E*jWCwG zmc~(UE|2vB@7`-;^`%l5iSET@4M{!Km~i%Y;9~xEnt2KS!cu36iWiVNYF;Cn4of2` zB^k3g3Ks4<(JOMzlb(u?4i_do2sH5Fg|!ohuzB>TX0-xy;wvleEpMe{r7Ep9WWidS zvR^bat{sksxY%e4m9*d-EJP(7tga*kb(s*JrMgv~Rqt{tsgb+y7qukj0WbM6nB)@H zc)GdJkm($H1Q;~W*|r#v059tj8SJclN3hTbwIygrVi1H7UXKG+WAtWdw}Lx9zK8in z`p|R46Su~^l?KN6XvC9SkYi9>x6o%qQ!R`lke#w}?lIyRlnC5$XogC>yO5u#u#(Ht z2DUF&J8dy;a-D&~YNQ_Mi%p?T-}9#uR9rIib9H&gO3K{T6~a;4et(@p=>4-c?8l|( zXeB4NHbrRT(CN5Cy}BYkSkrI3_N$<3y?D*CZmPA>ea8*Xw)3=$*CR!^?&aQq>N}x+ zvy<`XZ59QEDX0c>(mRHiW8*`-z{v6m3?uga^rD!8M_g=GVm6Zcg#8r6`S^sNcD*H> z4dipJ4tj7Al%|{1yq4t5KjTRVGXx@8k3;-UxWc{^%x@^Goy?5&xi?&7#yiDQdrnlz z>=N$NqnaXRjIkOqGlI|DxS{7Cg6q0X@LRIvQ8#=+H&M4j`<4(xlB$LiIb&`G(8I~? z0icCx7Mu~sSE<<9jS5x;B_-Y$j6h?d-gkxzgNdp+WEsrRrj(Gb`9ypZSK8M=I6l~# z_Vi`PB%{r*f=K%kqrncy{JqZ8vqo^d11|l|uLQ+&bwxcM{2uCycsG}PgcY2b(e3&)Qe>G6%$^rw9WlBr#?TlpRAdGUjfG@jzhs(KT2z7GjE z-71mh87Z`)n?aooHDT59B|Tqy1_sa2zSN@`B|~W&(Dp)1vM+x?;7&L>Mcfr zIZ{zS{+kQZvRpURy}3=pE_85?S}B!wjFp(|fD8RhDaNGn?sQ4gB+Kb+3#7Gi?aH?` zr}MXwqY;j^qmi&9S#<>F2NrRNCs4U=)Iex5h-YmhDk03|Lo#w|%3*8F3DGF1t<#^; zU?#!3dGmX9B5$&ts=w5a^5v6aX!F}j1Z}iO7fu$A`CR$j7LVoIUG5JAsyy`{g0>;n zbI=23I;(vF#cVKwKFqslzCVnZ+Zee#f`&1h&7blumSC&cu>C9UtmAOY7g@$)H3S<0 zm!ZKVF}eii@U}5yy7+#aL1Z`uUXHwdlJ${XEJd_AI1`reW!Eo$*&*xk83Yz}PE9>l z@T&@4&Z@{F%kj)RQ{>m#Vdiuoa!I({C{pwD$!crpT3&54CJaiNiqr5#V}FX$TaWff zWQ{a#UdH1-&>pCw(1+IM^dfw!pU|xN4dY@njQPGZNQ6>Q{F3;o%#I7IbE;^m5>KYA zudc1Ds;l}{CVplcq;xzgEH%GE0n8R5FYfHmnNHQpb^mRCf4*aw%mui%lhY4tN2$He zsI((M(&;*j-}?h&n)R8yxdbVq+SGcFq3l+gTorJ|HtcB#*+g{2zAzlEzdU~yxVT-$ zskeolJgmn}ms08+>U9F{{~|qwz0Uv+-P-57ul8ui&0cOnB_UTv?j{|oLE0N8pFE#G zt0-(unMBG^Y~DbcVal8u(-F@yxWgxh?m#r4(Vwc!An*us@GY1j=p+czBoLn5n}M zfovghfkp4MAiG1BDKqP4($<3d7((EAQCVyFEDN%Nt*z82FYrS9f}eSh*+=ss=wuKwn^EpUwrCAZYN0rA8eloo zo|x|N=(}K;KZ?7;t zA+DA?f0q%%FqR72)HR^c9WC#-(kzp`y+P(#D}AMvFB_^nSDyzjM<#G@w{j^w?V4Fd zfCMBM515HWWf`$afJt5jttK9~l}vVIvQO)k%g`Qw(>3NX>{>HmVet`lMfWa?!Dz{! z63Df<1gRdBsVRohq}bnOM9D)D-#^|({c70DWc-sJ$+0l5AQbBe!X{4E^UVQWO~9KDI>NTs{-U<%sx?l}vjHlp3@uIeDu&Xp^3Z(*f!A4^ZGY_R zJ)mChvV%?&B+i;M%k`7cCQPsUZ714El$9O*{WoA^xs^T4{d;<JWCf9d3 zo`~Mod$F*x+UpifulOPXmDvKnsEvrkMSO2q&>{B)Ysy+!d_Nz4Krj>=tLe?9=2}aB za@-d$+9tSL#igV^jwuC+9FsNMNzPQbYQvo@V?n$bQT~m;ZF>M3rkb1q9S51HBbAMWW_31G7)j4)kpEg?_N zf2Ru8(?&;B_ZIKDKGq0_DIz(TT@ zvnHL@YS4PL!?C@xP5?Wq4jO2O;#WeP_(J#Nc=XO4OskR#4H8HA`M`-OCo0ibB&yJ7 z=+Pwo?&+5ao0J;M=()U;lW&H9K<2d1_n%42P{rN&8$(a6_!#co28`$(DGzE77v6yn zDi1`kH#{H_U2#=Y^v`tV<0cl?jb7ym8YL<}MlNR4GUCr=I&f$%_o?p7l}eP#bnd|Y zRp$!u>!YEn3E?0j^))e|N>9~j(!C+lquk5cIB@#?M7{^<$HBaFHc4J1CFBaj7b|h~ zmHEhCUlT`q;=d2fQzB{_k|NZ&>68#-YJ=ia7>(C`gzurz<9uEIQ`eq3!z=s~w8mN+ z+JDa1R6)m1!SD*VapG|Fw-dCso%GM~609MPHY>rGEhb=FYgL>3o@(d_PKEtveff&a z8}B7YZJn+R6;56@wj>m7jm3#LjQEx|wiw-&E@|&|X8|z*PFAMrI#df*jZTGs*n`66 z+36stdhaG+GX47T8cUPWmW1r5Y_mfAye}IRr_tnj+L3o-MHk?0#Ik@i5~|a7G?F;O zMsAKSmsz=_)VtD@hQCqByNhY)1=wTui=}CC;fv;A5M8o`m$k^-NlqaOcqBOat~>)e zKns;=S`VyED2g+bjL)UsAf6h@@;A31B-R*XqgBjxy@W<;_}-QruAXa0K~dsW`3a)| zff=PFcRL_0HjYid7>_G^Nc2QPj+5wx!h`Lyv*+#0Y%AAPG4%yloK0TBj-zl<; zq^L2pLpFWtUe;D_vY@KCJqPLo27Yi|bRL*&=+3zkgIBa5(w8VKN9rGz701H_;9-#= zbbVMk*0*6{0&&S0RCWS-5r-l#q%2-RorNP4jl~mnf|GC?L1pkWj2cNTSvk^u#>2tEeQG|2(&E1^S(9uCZxBL|b>W zEwu3T!5Xvowi^>>MO3r!@Kh*u6@B)1@Hw&q*O3D&Gy4o)oaGlH3;}K9=K3Gy*cGU^ zPu?n%$50K{iP^?fH8Qs}cTxyAh84=h^$A+6BdOy@*ntC5>`Uc#;&$4=)I6aT4>h#p%N^p*BVo)Wl=TD@J^xbwutmKXR8)zpOG1 zpjCHT{BY*Lcz2V^V5~iK2l#tNnt1QWHyv3nQux_T3^xt*tX>L+LmOl| z1LkjzvY?Rh1Et1gUgNLn_kxf8VKm~4jL>t8Dcc}e#-yJQJhRcZj~kn+mz8j_kYovl0y$UGsJ5>=`mvgD(x!tzX$g` zAq;zBu#^1C1i}RLb1bWmDf<-G3@4+<;5zJM+a{MRKkN0#bS}0m`X=fHXQ;RYpq{PAVL%2->L$ z)pD|#$giX5k-_fmNUv~yCjOeBE(ASXOf-!rtAD1dSK&CLB9Y%=2vM$xIDyh>RPYJs z&e~<=6jf*<=y^LBga#q+AE-U0J*AaPT{H4RQ-I-q@B}$MWtGaF$xMxq{mSt+BiM4^ z8#5FK+I9tAhET}YEIiAC1whcZCq2MNri-VQV2D$OGn)&7C^14G@86S`p(3R=IbxMr zl!*f(kGs7H#Cy(RjYsXaxzst%A*s-l#fR3jk?BAW=R?$@wlcy-&yT8-pq@EhTwO$- z4{N|LKX^X-ri4A$-S=15Zeq8M`?tR+eq{B4{ee0~f+B(;Y7`=9VstZV6AOAzC!IsY z%I^Mx2ohbgMG=rAv8T|qvIT!dqUHZ~yLs3HA5fZWH4s*b+o1GSC2Zti{3O>ACs6-gg6Ff!tSpA6fUAMTWResE0a5Hax%OMI* z^mXUH|XKf{A zQoYdAZIjP#nayqq8cXcS*hr#tNQsWetGHn5#+!*d!cx2@%S&_S_I{7(E%_c&u>y}5VV?^{1M z1|QLAO?7_v>%Bo(6FMhkYwW_|JQJ zj=A_Kjcniyf7EF%QcWNQTES-IJcM0QSK!da3g^J@I5q zct60fn+|0Gw7*Bck-J0>9dp|befEXYs@(d{<35p6puC)Qk44HP9?wRIW@=y5PCal6?Y{;!_@Jz|D6ZX#|EgbcHLw4o`Q9=SE&+iiLdANx_N$o6y zXf28jd0-3V=!FR%5$#@+piDXOA{NlpFKmv3;2Vr#{7{)6HM+^uEi1>dx_8$$pEbW} zd;>8)@gC7~i$OYhDE5MGFEgMfLZFb6j1~Ww+1S~*lyNmuxlw;sn;geA?6VTLA{OcS zo_X3DEXFyJ?ZUU5OKPjtG~(ngNo}cBM+b~Rp2GhS{=LgTLa@@xLM8X`b<6Bmmrx6AhkE#tl`>AxwN-OVtw?( z+;W5b7ub!AnW0x@PXkwg0!S=19Ojk_89unT8(wfA0 zE0;GGgb$zgxi!mZ+V2tZc^+Z=h`TvYafZWUnIw37)rP>4G;w9YMCfNL)SmZ437u|3 zfokW3NqpgPZbV*7nt05tdID+{F%BK~3HP!|j?>CjPaVbk?KG>rA)6SVdaZ+WyJ(C2 zX#SrSwAvmU+FGmr>pf|DzYVms34TlO_=I6X$evi-o@M7!;%mZ8oSmvXbw#JW38+=B z{m@n?eMTOfhqtxF#;nC=^S(zOQQj7lui8H1)dp2mnl>fAds(Ep*-mkq?4x&tW}9bg z^tq}ZIU_n(KPU6|;}9(~zC5`b!;J?F8kHwwxxK5#EQism2snTZE|fdUlv0iL{k+&C z>k&_#gcNRovhDRXr>0&{kJ91B-mj-j)yjsBjCQn61%TIJ!!f|z+~vjbrc%R{)b;{& zmhvt=4qah6%Cs~XM<(i4T}LGxy>40G`2(0ke-h~nw)r}@nzW848?$D3v6=ob_Y|%a z5D&k&=Re^&HY)D^yW-m(tW9O~X+wMXZ zKJKBP9vIhvLc9S*t0h%5-IIsp!(kP5CjI5}t>iCSp>!Mu<^88x*|S;8$J(A$b&TiQ z#jO-ZKO-!0E>W#+BHdbl@rLUlNxE)MnPFQn9cgH*xl}72E0T5UP`#fy`V%Ga#$qGH zj314Lt(vWx^^)zHLaq8bgB2Q;9KI#0-xhk4(o1mYsca)+S-mq|)XOO?K5X&f1B;9Z z9jm-O1RJq`IZIn_RHR&6v}Px+ipXAv-|bmuvhz@K^mNad7o6N66h)=ApOTp8)Z`Iy z=r>x-o(u0V#8d`HR+QlYS{H3%U$*hkFtx_zEi`+-DtXs~4V#qQsI{9Ab(r~`JtJh` zW>f*~_M^_xm*QDRSEC%dzFOzDc}u2*#n7JGi$WZ1WjsU}Lji3h7d0#M7Z%1H>l#sV z3}M|-EYbUmKlYsbS~Ww_T)DbFf0PMK-kKz%z2_RMUIv)gAZoC9dfzWTx-pq@WGI6S zc%fl!ZHJFdrgJQ1SR>t73DB zNtEbTdghS|Yj;th9KH>9&h1Ok?n5XK(1Fq)kb3iCvHdXKe~}Tn$rUhj(I;v8Z4KC43w<|f zkku|I7B5s;Kgj^}zJtJ8`QGW0{-TX*n?_t8XGNbJ+q8Nb) zzXD#LwcvtQE$3HQ_ZXbzM^6`|xAr&MG>Lq$3G6-CKsqLLx|AY&>MjYt+|*x6(t27L zL<~bI!mGpS9f0^%4{lZT$gSLc_K`T_c4HTv3OR0I(P_Fc)b7%X8Oq{fHI zcQ9SHY3egP9NZt;F5HQng-$eq=l-K_wHa>h5@;#TY#r&-87@z2X7+AXq#o9al{~#t zslNdzvNq+7J`IyL7+=W0-;!q^@>6M^Rhsmd{iH2ACV_JI96qhXt1q|gT*8z3g-=fq zDtjH#r1eylFsv0dB55A2ns-aj$Tukv2v-%Klgc4~mvbH)P@P8Ry}~c-dh@2{C|yN4 zX*!Iie@#q?8Fe&UY*g!*9L(300L@8zdqw|1hM)z*8*Oxl9fvq$j*&(}Fho0s0$@j; zmKrUf1P;W)2Q9<~=fO73#kR;75S;`eC(~d)P4Y^Q+Cmc^I!J=;!hBKL^yDW6m`g>O zYsoc@-(am-y6UxI7E#$m%l6XJnrZSs{~EMIYgwYOSnOwIEuh2H$l-CxEkO}<69 zXf0H=VWcqWX{n}TT?YXf#3SC?>d?&dff0Y0ST>CbO_%BWD(MSmPqsiRZgr7(W4UoG zDf?P#6sw0byBVY^Wfn`KofNjVu$ycS6A4UZ*?PiaQ*G*LsJXw`NRo$H*{`=RyDJ-8LuN2zp0a+MUIOJ*NwH zUi28{H~BQm@94COAoh;C+;WWh*b^tLK0Ng>bZiES#k!TM=A@kuVD{vj^yzrx6|Kv+ zTJqZROd#OC$ZWgF0r>ST@yeWrt(+1xw_$av{?k^9PPcC+@YD^OeE;U$bcV&CYs(mr*lhR15CYLII{I5l2S88qQMY{x4&|F(}s4) z{u+FAU=yOo(ua9{C>+T0S)>?Wh|c5tj)W77ljBHRv7m}@?czT;K-GZz(An_;MN*H8Pu@Bp z6m+2xfAR{KJ=I^$iOud!*o2)w#DN;IGCzeuilm970N={Pwe#aot0k932PRvA5gU%o zktQq6<{8zIi%l@id@6su*>eI^qI^Q^c8PROQbLOLm`ig46*t7eFkME#yl`7J1*s2U zL_y|3U9EW7TX0~SDtJ7n<<}(Q;P)m+a&%+$1uJ(=Ma%+LiJ&Gc%qSd^DD5vg+@tt8 zz{=PGQ`h=WaDh*pr69lB>0L9`dCL{k^`jNEte&A zZ)a$p2mDME56yZQ<=*O4p)X$4&J5!ssQ|wg=A=M&vM)_%G(5&?NqJVgBBOag)eQNF zMOez+LaayCg>OfFlS}+U9B{WSd6n#3nl;(c+(ExU|7}{1?-W!7#p-b-tSned(B*}% zU5$g=w0$-OC!_10&i(!50pFk8W)*6IFx4-39mCxpg}s!80R;mL(!pQG|;FN#pF%-b2wKrIgav)Lu&4C|W8n6%`rni8K_IP$4Z1k@iq23i;p9IVa9J zz3=;dukZc;_~D$c?)$l}``Y()kH>t@Le3|6@>9R~hrHt-^1i_aUhaR^HEgHp^CAwR zf4b|{EAIXM-PiYb?XveN@M$m-C#Y$iTf^kP!-sGhufCC?@yKv}qBNe6r@N)Y{ar%+ zbnoPsX^ue?xc}p)+Sb;5#>N!_d>@WCC6~Is7MRfy){#BzGm(?az0H3p|A9VkW1^h* zyo8OZcvDW}ZG_zfUyu8@)F6aP9^(n#&o`A+-7|-Buy40V9?CFlF-q36K*PdnZCgf# zK7?(L$lJOlhtIrN)%hXg$Bm0zyJF7uDTmPS#?Yr*YH8;i4H-rmD|_kbW}7`_G<_h9 ze0nO6!AEQIo5@jDpWO7mBKKt7XURGKm3lqf4NuqTolHeuddBYTaOaflLl(nT*H&t7 zaGRablt{k#$MK@KC%hBJ7e3FuLkYPo9YxME#nefO<_uFial^B;8tBLMt9Tj7}#fR*fT(~+~XPVq_I<2_z zkEi90_J`Yia~u5?r{a$!MtV+{9gvEWDsSsdLEqNgyV<@y0*jD!O}FP<$t>-V|4005 z|17&pN@sCGV(qcd*H%>X3=_hcZpNOxcI=s?Y26TybjnK>XRKeHpzW>Gvajv%DygF@ zEP5nm1bY>#kKP;Uim3_XaqaZ9z7-jMV|Uu~Tg8zLyckK34H*rg>XTTNXsig)wI4^NIi&5xo zJ>xjt>8|xDQYXLEWwow2gou1Eu)&MF;qFr{RO0Z=#i&iPd*MTy{Bz8rZ+^HL!hiOS zTh4k7uMXJg|6xHs1$bumP zuj#pS`mL&jYLBVJyK>Px-a@nW-}1a1lfnx*^L*re%X|W6FTYeem4kZ!D0lziIq6`< z(jrCn36nZ~)48u6T{B7d>x`L9Ii8(6bKDq#bQ{n^!#=K=WAvjtqVnb0m#}X^JBHM& zZmd|#x!-a)q{J|sG2tvzt;3^eX;^30BgV)#E)B=OOg(t2>xAAPQtyb&8ha=x*Jw9p zI=x~v<3{nl+cCA4w^ca`CmRTb$(P?RO0UO9FD7(3YTicpyg7q7{`8n(;NHj@_U%my zL%c%$Cpd=to-@xx=>*87bma-Nef9n_Cutr&`FiNA#7(`N#n0=kJ0`r>WK}OTcP5;7 zGH^fPKlyT^FqCay6? zy$(LM8_&2Yv;V-Eul)xn#V2##Bq~by1a6UxY3UmNYE>)Oz0F6qL1!l>eBFS7n*5xC z)=GW2lQ`zhi=li1E!(ckwA$qdd65#@AXPGKkz3dRos zpwe^o6t7j^72Lzv^ujoNu$L}u2;gH_9g@fSoFQmk*2ZG`jqi?XjbYkWKRLBe3BM{r zXZZ1+YBO5_qxjO#TyxE(K7R}=T()Jv?d|!LQWo*PewD0=M(?`RoOoR2H@atep9IUy z(iERr_O9_Ox$|*MW_Mdk-zo33j+n!KqNfXyurpE|8kgUl+o>C#c-bM(zlh(!=*65A zua>H_@OgXY_ZOWmr88=7ce;MN;N#&Fiz;bvPu?#-XdzVYQJtmYtIWlM2==m@`Pgvw zSa~#3s#mVKvTQ={u!OJM9_z+Shh7~BsHrGEQXbKPb@R~nfrD!lXY)?1Hd=TnSn7E5 zt<9NBm!)~lOAucB0yBsH7++hVI%gAQ_I@(B7qeY1;DG2zzoetrkZDQ9n2+GQ3r`bI z1eP3o@o_eKZ@P4>i|N3PW8*e>*_JO%&UyGJ}{TU;CMrik?uynst zc6&sf@*z&sGtTAT9$hrHb_$rf^m&E2UxCN?#LPsOQR#8bFDu_$*Jw38ZL+aWNuB>( znNp{EIl}qSx9;4guTBGtYr`2;#^&kbhyChB@*L+fwy#*wJ(q7d(I_WpV{lUcVqR<3 zx!cp-)&|n%=iCShmO7^*+%%60PGu9?1`luZ!(%r1A+2_&9x*aFwZ7=_;p#-VAS>r* z|HZw*=Q+KZdB2=};HETkE|oS-4B6*^AbX_RTb2rW^P&dww((cl#~D zx_o)~L!FsD@-M-s>0)<~89N5d*F(shL;t2z69M0UE& zn;+W0FNWpB<@+{D6LR&)y-rd?cW-Yw?i>@<+htK>?K8VGH{;X0fiUgqjPo@)(MNRk zZz?6s*ZDlplD2-(lN6mv?34)4`?^iAKH8$FB^O4x$(9~$&yxV!qG_lgo zBG<(? z#M*v=KC-3%tQ1eG5u5*n8N4Fr%9~GjEAAZIS}S*O;lRoVi5$%4n~*o6>K_oeB2Z3V z_(O@E@3)i=++`|ob4hvdrqR5M=R#+e>22#<{P#3YsGm6e?Vgd<&Pmr>OlQ|WN{vE2 z_V)01^mn_Ds}Ks<|8?Td&VWsWV{Q{(1&z%444nMKUv}xQe6at{=i?#=7tqQ*{+Lh3 zL*4oXu|3X$kIP$I4!rN$5pB)Xy}ka()Ww9jyP{8a?cP1?#rF2wvkyXnuOj?k3Z0Ls z98xM63zqoO+;)7kf45MR-y=58C>|b3BgdO(JbJ%bm|IS%b-yqOh?jJB*HC=UJL^## z-+e9VV!-Da8XLWM^AOl70&DUl?< zNBV~$zHHp*Y1h&p zaLNF8t5+Xf4%!eWa!&OQe0{_)6Vt$mi-x+&`WipSl##MMDWc}n?D*lCnc9q|hrGL= z=4s!6-;Uxj9iOuLkm9nvIzm%q&wQ+qeo{}xNz|Kee%bnnGjY2r@J-H&jGLmAQp)1a z3tnW3!|hDnSD@iCa9&^9^q_JO;{k8y+dYTkV835a`Fdx zv%rVDr^`>*u~(`Kv$r36TGF|H&sfz4>65v&4Wm&_Q`k@b;^1{{@m(YVL8)_i=REN)3+aut_&EC+9~Z9d$_r8UF^<` zmK3pT)^dC6n5VnJ^0roeo1@vKw>y)YMbjkL_hsEq3qv?9P9A&HZk^EMO#k|hXvbJi zmVeaP8S%Z?y6zR#p7i_p`-69-<4rkjO0J53JNUii{@N*vuNKva#kyKtl%$sFIzcJk zOD{2>asotlpRK5pt8CjPW|jWYRYT0A>)zpOOgWd5QvBLDz2jbP6P;T(Iua`qB^E8^ zHuGX}BHXVv=W)bKy*=rcGP~2FuC6vM_I}N7J@r)c(?b(2e3pABa`&Wq8@JeX4lsz%bgE>-VvyvPiQ=FKWIA1b>n_m%M~u= zS7V9KMHuY4r!O%vnAAxJuU9y=>OT8O;T0#AjN`T(HYaSiR?V(J-BN2lwu-JNjE#}M z9ag#d#fs4F?^&NqGRQLM>JyHI-g9hV5nek|Wq*Y|>%k{Ppjlupmx1ZJ)XJN0Z~3(t z9LssJc|`i26tk{H+|)5tU3Tex+wqd(_Sai{>!RKAcef=^Za{4@wHn~}30G)2X(%a2 zf4td?<%44g^$84W0Lx9-B_`B@pBr$<*NnA43kizk|=kDQv5LOLjN zx*88yl}3EYVYaX=5lt|s-z9q6DoI5!L6!R;tn9rJ_rdKU@5Q+f@@KwZ$$c>0>B&oE zmIBtkgxOv<+arc~G)XNx@kt)v1u55oK>b_w2(?Qw-Tim;!}kg|eOs>+yeV{a=Xve$ zO~OqanR)CD9w|Gy4_-;ijH(t9Ht?wstzq0a+ofoZi8(u%#2g|j-J70A-@uZBvoZ{F zm6_Bv)a<{sA=*4TH!;zCsIJT~T%n>zk~>_sJi>~rfAm`vbBKR=V4``n^L1_TtBYa{DQ1g50-emTgx3bb&EM zu5Z;@Y^sqD?w#)%(`Ic~&t653$Y%1&Y@0b<2=ev3B?yowqKGi=y`l_Ol5uMmQewnqM4r_SAZEw3>sZ?O-|CIFN#@5;) zMJd~hCtKz3oqhXJ@zCy%m7(WeFYfBxEt{DP?=4v6cqN^svFV(S0e$|-f~&paNi{24 zm=%XkF1js_r+hC9XevrHO0|2<;IAc`^m3tzdsOOxuQGOAN3DUr*;9B&{9@`8{gW@y z>KRO`ThrWNu52)Em+*emCOV@vSCWc2b>h#pt(Y?6Dhp{y)KTxecC}4mk6&h&agR;2 zz_6Y~q^9i%U02Na&Aaz{B)u6=JK&hStyHrldiATv{Ckve@saiZTvrOWmk%gDravR6 z{A5khAJgYu3L+X$4;|ZZ!9d7Sa0RAuY*%!dO~SiGtR`)-=KFqFm2Tq5LAgJ!@mw z@^yn>?s~@@yvF%z!N^+mw@SG8-J9!+!E2KD-}CJ+vItTgOGCTA>^;H0qR?Pq#c*+y zt2LYK*62~wDojwPfo~As`i7!s$CYC{4>n{T>@4{v+QIrFU+@|G4gbzvXsx>m*Zt?) z?nMi_-8eh~0FnRJ^yqh1usNZig=v*$T~Wks5Ri{@^Rm=VxZB_YVdtrNV?f=v zTS~h!BawZZuVrNbwoWkFKJ!IlkzbOl;fP&QqX|!8%$$nvmf-2uH38Q)40P`KG8sF5 zcU06d!M~s3TGpi!t_;+8LUB%9L%u$rm;9WUrDw>2=^C33)~gAIU0b4*c~{@M5G!4O z|MiXB(Ue4&(lNDC`JwAAQS~Uk>lFt_>^QTBGsQ!!EVFtd0FZS)^$(}eiR$XoR zq3w4SiZ0HH9ZK%swsxSc{{DQz_oj1q&h9zdD3ro&#bWr_H*dk0p*XLO|AUdRVC#PE z^0m8iLt@?dca~kT%1di|kQtPaqo%shWn603J^zsrWzciCiu2>+!+aM%J^kJz;-`Ah zq#$0f`;VD`9S4pK)(W6{?wT3oH9u*5zN6#ixi$8NyNl^H3Vr)~jCXCayLWvLkLt(! z8hlZPgZnzZJ74W7(=@%CaePs#@#*+W&&RvP^RMNsk#)J6m?(NvsWqYLtwFA|M0Nhh zLn-c!)^akYWn&ujH{P>0`9+EqSZ>ZqP8|_8eIhlXtIJ4ue2kv+ zsj87(=Zyn?viu=I+8@4XHCYCfyGyi*=L%Z(@+a#Ie66b7@$K%Sz$L9Gqr)AeZx08i zCZsPbU*J**vz-N)$4j#cuz_XzcBkTzqR00w^77BU%$|b zi4tjZyHTCdL71S1P4(UK?wSls9mD5zs}tKjC%aFtFHBA(`BDX-obSCMfHoIRudB$ zV~yA1Z0d@5uN%dgQZlR1%$%JgYz&Mu zn=t}o$F^?ld>LY5?Dt~aCphn@`cP=>gFdm4;YIT+SN)XRW1U93V>bKxy~vQOK4nr) z*F7Fmea;kS=5i-nOa|YM=-R8n(MkUzJ|d){?MYmB87#1$9bUUhFWYimSn zA@^o5>h#J>@NY6KDlGLAiczlnu%d_$#?iSCj}wx((uWeDuMxA`y4v(p>|#)&yb|ls zJuSRTj)^UuCu=Fo$`>DCb{$E4Lz=8CrFtfIV@~7TBdZ8r8V>YPTQkK6QCLs-y-xZtv6@(aqu0h?)XyuGS06;4|M)7VE!DFT9ajYAj2g$ByJNtCUh40oiCc{o?r;8r{Q9NqUl6xcjQ038a z9&^X?JYN;Z?myf{L;tWEYfSmxH2YNIeGvxBTxN}aZo(v56k+(JK` ztin8HS61Z`=Fa&_UgN!j`qruhl>5rk2;b7{f`Wpv$(&~`p6wcGy zX!uxq_W&gYaT9enW8+86c;{20mFv+E>W|61wyL@9X8Q_dEsbsWqWGM z-4%xgBr7hII668{Zuev!kdtoJgY!MB zZKuhmA~ERD>OUr*jVcn@3pGN zJwn8%y=k_tjPCw^tn2$tlg2>Y z+Gq65ZbPWWbIvWc8?5It@}`n33oZ>45c@|L1ZSjUdb2K zw7im`wW`+Qbak7(J7Z{1P?d1>s*v;7vug`y7<*4_o=!?uu#VK&^<@fg6Y;!#_;_Kq zPF7!gV45leGmjN(x&{ODh*htW9}Ipnz*baOp8hCn)Tmb2mN%6ls~P+bO+QNtVYB#y zlb`LYSFPi|7vECro^I}(n9cthzdF)wqPmtx>I=AeQ*EqCmtksb8*El-K}r2YbVXGK zgZ-+jGZn{C26s4Xzihq|cy?>hnh2Xo8?SUl^=(Hb)J)IJT|&g68=Y$lEW$97a|&~M zXOQ)CH4Qa|-D7k4il*LY%2|asb(Gc?`zrPtTU<{6z&*FuQ>uWjbyt<^)PPMpUwo*% zH4>47**uYp$m4)NTW7|^iazR&i&B{zv}oQC`3YXeNw_+J_gmvCzrf)4J}D&C|ID|O zY316!K~oc3Uv7-C?6MWPob|lmlj6AWm(;4M?-}XIeCHE3rTro7x1%AVYQ_HE6+e`>QYX)(2?XQwzzLt32es5@jW?dHcjOXy32L}wlF@HfCF$JxBdxYETE|XvC(MrY4 zg21kEL6!i*bpQ{@+yj}AL&$`CB7EM#~3qF4~&!j(e|q6~Sf%b9|LrdJ(c>D$TBq=;XqprL6D zgX^TQ?kJMKzikzr%ufC$1yM!YpghiM1%^1bp0F#bbseiCl`1gttW&1oeQTyRkG&#% zI}Tsjk?3D|W>a$!JwM;L`NYQEANY#aPMXi<`EB%z_q!vrIm&R$W3i2;F~aASJJ>Fi z*nTPUD|V|?3!80qG$@%eVz&C|&Hv;qm z6?3=f@4dC}#=cuU`n!1b#cs*IGJO^BO5ws81EE*1j_F?WF1?=Mvnk1!?X4~Yb0^bO z0ORo@YxcJs=*rXqW7tHx&xW_EjA5`wA+}JqkF&;AKr;duR-f-zVjGN;4rROZp;!LU zwufK%;ZL0(J~az4WJVn}WIzc=_OiU==!-MAMoV*B@3a?5G7!FyyW&Ewj>wUqgxu!* zBS8TxgV|OcJ96Dyg0APu#!0KH9nCfuisif1_J3dinF0_$zp;N7)SH zL+|h?N<6#BKL9YUJf$*_GnLgsu{~QuM^&mQ5wJuB9TsR=H z*7=3^C0j<`@9fwLHf+Fdhx<4Of5yt7V4V$YsNh;T`l#;)&7YNrARyG1`2U1AK%kp7-$QxK|gb(ve~O)n=m=Uzq#RtgAeEsnM!+ zYC4GYpiCpsE6t&^X{+#_mk;vkB@TQ#kBR16sO5=`{&)_vQK{fW)`(EFdz60i_PwQV zYq*Mdy{>RwEAq(Np&AgTo1%_txc-VW?P%vrK|XzTtnUkrs10q8IS!ox=Q3C&9z@tl zyfqZv)_kk>sIK~E&C9p9jGj``!zQpu*Iu*e=w01-p`NavZC*O6MxK6H7`@qVWAx|z zb6eEET5R=*_SVpZHS9cpOhrFl_=#bp*>MX*(0B3H^!1(T>s6*q)aLDmKBeDUci%kn zyUyk2H;Ba$2laE)&KQTj@2&8auZO=?U)a!!B^5PY{J=gcH?e6iJd zzO`Mb?6az4k)IT~*k)RqZb%q7>*g9r1bGN)Zq;4e5>RYn@ydlKuJ+1vJ2cM) z)vM-c^$;$p`LII#eB*tSM={FRVEMt-Ta=EfF3hi>x92KikLm<(oxbIk7@1IOu=j>d z6SvseTpoR#Y2FmP_K02eZB?Py&9}s=hrSGk@62=RzOHL!yN<6oNAY&6#BsW5;hAge zB3qp=R~2SF!kA76wrx}58Z@1;lF%5T>0@9te=T8yLj zk`6f7i1U4Y%MEu<_N_gSm_SuluH7r!_4D7y&m+2@mfX8mmozS5*Xhmdh3xhijCSq1 zTePluvo89;BZsx<7I}7g3vp%r)hrDXhfjvHDowqp(bs2xVdS3#3woY)P5qKUMrWA% z7|Wf6?XL=OIvLJ;=xR@{=h6{acV$7afk38wNuEBlN{dzrzFbzzhBwFcjSJdjfIKifj0JA zno73Z<>oe-e(sZ-74Th!aR(UE9g3_pjfFIfU#3eb4PQ%_xq7X-?-8$Dy6=vgrdy1B zxg4{ME2loFBFfzz^6%UcZ@E`w=l^Vz)i;|ck8fWMtqlBCZ49Ct8|n@17v$3Kr5?!T zx0sE~UwCY2{}AJzr9UY8;5HAAul0&x|0f1J$;KfMGcmCq`IqcMZ3ZfPZFH+n-2J4n zrka)i#C^LKHY4P*Q2ATznA1FZYj*CvrKRJLaJwr}g3&wmA?suPEgOv$HJhW$a_o0~ za5}LUJcw=-wTC91}uRN-nl`^8B*Uk*!I0h zUKALc)|SVwlR6*=P>3bX@&xc3I2k+}h}~aeCF8KF2lKmEk|Kr#kb>(J}A2P*ZgJfFwS* z@{RF#XUVMOdw+TrC3G_(bipeUj=^FmZbikA;s5D^SG7OA;8je~*}?L!Zh6K0xaAcE zu4IKHF~4}rD-!b;x4hz^D;Q~RWyJm*5BXL`6b}9;H!@P*!Dtfu%qaxUd~#A+6^^SCGs-r(7?R2le6iVh-aqf{pygx%J$6; z&Q3=^>9#nT^kxn-(ZkO&+?VDQbI)nokCMUBeLhf}YgF1TG1_zCtKj`y4h9wb$0EKC z<|#ZZB?#Py0$9Du!GybOuYW7$3_t8T{t;N^E%%!WY=UEUMMFjdfVL+WpVtzVw-aj^_1r`F?7lFi=aTLvH)F zx$}r4`GKLIuD8p{G+Q|41~-Mf&7#&T_GWve%h#x(PUiOVeok((yPS8X!uf;#9j8B4 zP~1(+g8k;I=f13sa#BMVBrng)Salx*HU_2o<_3bm`(z;$^1f-;*yo zWrQwTC4VR0BMN;dwxhVnbm?d4{?Q-*pi5FQSVriARp|GBcK;}3lQj2_LTXK;>ChF& zkVON_RYkxMScrk(?onWBSm@qa@D&9vE+v0O!w?wqR}A<{Zi@vC$X~%d%s6rbJork4 zGE!flymUu2gb-X;3;L37S(Y%=K6=c`!X3ah9MLf19jL&(By?PzE%sZv8%yXYt4bWO z@^%NmHn*`-B7JC(J`_!;ko+7IQ3-TFdkGzLSLpg@1e!#uo|T)khpUB^8z2B8Nl#+0 zm8G3Ibh9*cTQ!D&f#K0e6A3j}XAc)Z(|&N%uDO$&3-lBV9|@)X;6iCnI}0m4HAM*x zm^-+IS5{U+2@D7zaf1qriVk|t=7znLbYic-=9S@7?riR5T0Xd-&r-%JzH$y3q<+2(Ji zi8f-O{>_l_YdTJfD-?V}|C<5e*Jwi1dV6Bf`@OOw#al`~{eIVuCPq~Xnm`8syLB8x zOtvU#`u%gho$x7*x&ER!6X(;ASn6t z`~5hUHj&5uem_o{T2t@|_itwHzZP3?v?)4q(*JvPob=u%3O+4+0oDI9|Nb>Tk>346 zNz?E5QC1($)noa}(FsUno=ro~a3p;lQD``~~RaH0~O#m-< zRDr{BXz&jSz5^H*`VN*puqYB73$=mZDqzh3yob%*m?&xq!$U?eJVqP=!(j;G0FlX$ zg#ePjme>cD6ae|OUXery6~S;SqA-9EG(d;}1c0wZ0Wh`<58;woMLYXL5Gec&NiV2b1s+b4jWQqsVihDSCGsbbD;D>IEMhh8Cs{ZYLL3Bw zzaon$v?U2Hb2yZN?a#=9BnNQ`yF?a565tXfO^D1Nss?@_eqU<)lZ!OuqR|L(K+|85 zOJv*OL7+&!{*+uwk`zd!E@?ke0f9w8oLv(3((fRvAi^N7Q!4=t`Cy$77|LIg zPh{{C`NXbNx(Y%5sjfmKkqw(j8lukpy5W$sCtRCrja{8Pbx2M54t3&cD)YlBXq-1GqpF zVM&m`A&2PQfeAup2nihW>_jdBZW1}Rqy*&O;~-8hnJ(mL$-gJUp#BiIm!3i8T}TU@ zKe>!WVE<0viHs#F2$GoSQ3gX2>7q(dXn;(+!)mljz- zOT(hg9l^VlY0VgVyD=2vk#Lf`g~XHK-&_lAhiY)wF>cym*0DxHN$pGO_5$B>ktEBJl(&K`o7vn42tVCE5GXCIMih+TRZu(l=t>xXirB zLtA18@F>KgLi+dr>a9q4kbu1qzW?1l5Q~Hq>_nijRL-1+ofJt561-fUdb&h{{Egl( zGk>y15hV^eWk~v9Ttrs>*Wr=0MenCZK-nr~Jfax_MWTWv&N`_b_kT!K(Ep+lVDMm) z0G@{i+6A!&(l#h_AW9a}JfK~~+ziq(2rtAQ8a6Glju;@o@0T(OD6=5;g+6~|62zpJ zLfR+{nrhlZ!zzj-i0slSayk;^Z={VTlRzYcoMsXw3pscSPX`Tw2>(w$9S(4U!fl4PrVke;P@zrJ&eF{oMw~>#h$a9d#%Qo146So{y2qQ$#3Yoo#oxx!x^Ere?c zFDUFR=@c#ArAaUq-v7ZTbw=%;tYkt?@~dJ z3h%G^Dbi>mTuXQ{h#xvdi+8CGN`?2={1gce7V|@=Xz?zUi>dJbnx7)!g>Wt5 z#SzFl^;flXDy+Zer-)Gqivx#3hzSNU*&t&j=1)IGyR>>hh4k0l6!1JCJ%bh_&`1RM z9}B2JEsd495=BaEp%_nqAu%YR18@^6#J}dJkRUe%rV6HVD3D3vVOZo((NYw1kzm4# zfuTSK1PqlL@2~kOD47D;8wLkztY|!x<=}7#@ue_CAzlOyWLlsJVmant^Hd1g5+c5=avS zqHZAoJD_MqAQna`@uDH0_Rswlm~VoCfDyxxVALR=q;Qjz$ty{@5I88!r55tC4YdDV zLPthS^jKunlpbrDUjHfTU-MaH)EG$0WYkznDgTT~e>3V|b6aH8a6BYpGQ+_t$PWek zuTcM*??R#=eMW$^0}XTxoCE_dWhwaoiKKyo1MX6pGjUJwKUXmX@Y2w6I6SZw1P%px zY#dm~`pYpRQ6N&&qW(1xhQ@+1!mwc34*^4C34jbNNcVmOie*ZMgq#cIn1PIF%u31J z-Q2<11}szCK!?-F6Kx4?7b_E;IYB5$r3pr`0PkcYY~>`Tzke6y z(mj&u2ouG!JQ5Bbqu}9g>+DKis7FGJ?@F##=I%d0n5D&2P&fTq*#gbLxjst|3#+BB zP=$=R>I3R7OI0msd7W5)0c9cL@5pr*sFZ{P^%o>4j-wDuC;@Af&E2hpl_f#(9tTdl zfdg=8u080brYrp=Tvz#a@#Y@vbyW^B_3R)?u*Al21N-2c(G88iV_A#;;|@NyeP0m3D_QG zJJ1&g)te|`K*4}M(!#)SY4`{F5}*)C-4}&|%3stl911#*K?y?vrY_SV6b^-@9S0hm z@uYbk=nL{A>gU1HK=o<)hX$3LWi%sE&>m1qTHt6j7Fsi*hT-r~6sCj$ydVXoh5>T} zjHZDBZGn6kO9?_=b`g%l(Kwlhmz?c#S922N()57rh z<$eQ&U#7b_JYkvNz$4JK&qKk{z%A0?z$3tXh!%z>(Ap~Ki-68tP|=LXz-fI1Fg7@C zJJ1(J>$9PD(9w6Q=MlhpY8t+PzUXCs4YY$Q=hS`CAU&XlfxcL%icQ;&RuA9=9Ew)& zL0=qn9-q1|4unQ(7;v!2<^CB>ptTRE9ql-Pms}=u6!4r-m7WqWXa_tcEsQ{1LZWO3 z`U1zXyd6{yp=^f$$I&q~ehBnkW(!a|+E{@A7D$T&^j&5PP&-;a0}l&LOQ~o9eQ~t9 z3$>%wU7#7r<-Q$*r?o+-9W4&f6TMtUSoku$pdED9iHcX?`U&XJJ|zsys6i@D4MTu- z%X9+@m^8cxf?OU~K^TM%`cvTm`)QZyGXm07+I9d)>ra8;pv02;c?i@pzYF%ELTBJ; z+F@v81q!4p&_My3c6i!22-*>r@rsx!()I;rPOAqXsazIAf#9HxG1PdG7^vJy1H&%M z8Gt~Rz07KEnWB@}i9|vlg9|G-`>lFe8JSr`H zs6UZ88V>CXr5*DFmaHC4ixz3oR(|0vL{#_aMUr znG;Pru&4`NfI`(5z-V=0yASU*Q2HdI8!)nT!3J-;{`Aztxp6n9PRuFiAE9ksZ)&ul>V0C1u$9} z0oS>Vr$`(gWbQOHgH(ALUIGCmFf{D|3_3eUMGJv|qSbpOfq? z!mub>`GUBBTIOHCZieNt7Q5V^g2a=S_b6z0AI&%b3_+XU0~kmsY3TzsSQ`BU?QqNd zE|~t(=4K#STQ(mAFchr~0vMK7p8*VHl(e)Ez*JxvY?;haZ~}%l?gJQpdF+9MC`$7@ z1bR7rAiPn|f8AZp?HsIJq5GKj+xc2SH3|tmXJ@eEl2|DMCnGeRtewG6fS1tqP8!NE zW0*AX3wV?w2Cjrt1aVpkt*oqyS5QPM;8hgx1eBtJEaQJ}0_~OnWg;b8D+_x!4@Z~; zIC?=qV&QmYJVHfLMG221fO#cGNdd@5Raq5{hl)gQ?&hxU#BvG>cvMhN5foI>R%QG@ DWBTWq diff --git a/src/hal/components/cros/include/cros.h b/src/hal/components/cros/include/cros.h deleted file mode 100644 index c6c034f6..00000000 --- a/src/hal/components/cros/include/cros.h +++ /dev/null @@ -1,27 +0,0 @@ -/*! \file cros.h - * \brief Top header file of cROS - * - * This cROS library is an upgrade from the version of 27/apr/2015. - * It now supports service calls (service clients). - * \author Nico and others (cROS 0.9) - * \author Richard R. Carrillo (of the Service-client implementation). Aging in Vision and Action lab, Institut de la Vision, Sorbonne University, Paris, France. - * \date 10 Oct 2017 - */ - -#ifndef INCLUDE_CROS_H_ -#define INCLUDE_CROS_H_ - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "cros_api.h" -#include "cros_log.h" -#include "cros_clock.h" - -#endif /* INCLUDE_CROS_H_ */ - -#ifdef __cplusplus -} -#endif diff --git a/src/hal/components/cros/include/cros_api.h b/src/hal/components/cros/include/cros_api.h deleted file mode 100644 index c12ec625..00000000 --- a/src/hal/components/cros/include/cros_api.h +++ /dev/null @@ -1,407 +0,0 @@ -#ifndef _CROS_API_H_ -#define _CROS_API_H_ - -#include "xmlrpc_params.h" -#include "cros_node.h" -#include "cros_message.h" -#include "cros_err_codes.h" - -#define CROS_INFINITE_TIMEOUT ~0UL - -typedef enum CrosTransportType -{ - CROS_TRANSPORT_TCPROS, - CROS_TRANSPORT_UPDROS -} CrosTransportType; - -typedef enum CrosTransportDirection -{ - CROS_TRANSPORT_DIRECTION_IN, - CROS_TRANSPORT_DIRECTION_OUT, - CROS_TRANSPORT_DIRECTION_BOTH -} CrosTransportDirection; - -typedef struct LookupNodeResult LookupNodeResult; -typedef struct GetPublishedTopicsResult GetPublishedTopicsResult; -typedef struct GetTopicTypesResult GetTopicTypesResult; -typedef struct GetSystemStateResult GetSystemStateResult; -typedef struct GetUriResult GetUriResult; -typedef struct LookupServiceResult LookupServiceResult; -typedef struct GetBusStatsResult GetBusStatsResult; -typedef struct GetBusInfoResult GetBusInfoResult; -typedef struct GetMasterUriResult GetMasterUriResult; -typedef struct ShutdownResult ShutdownResult; -typedef struct GetPidResult GetPidResult; -typedef struct GetSubscriptionsResult GetSubscriptionsResult; -typedef struct GetPublicationsResult GetPublicationsResult; - -struct LookupNodeResult -{ - int code; - char *status; - char *uri; -}; - -struct TopicTypePair; - -struct GetPublishedTopicsResult -{ - int code; - char *status; - struct TopicTypePair *topics; - size_t topic_count; -}; - -struct GetTopicTypesResult -{ - int code; - char *status; - struct TopicTypePair *topics; - size_t topic_count; -}; - -struct ProviderState; - -struct GetSystemStateResult -{ - int code; - char *status; - struct ProviderState *publishers; - size_t pub_count; - struct ProviderState *subscribers; - size_t sub_count; - struct ProviderState *service_providers; - size_t svc_count; -}; - -struct GetUriResult -{ - int code; - char *status; - char *master_uri; -}; - -struct LookupServiceResult -{ - int code; - char *status; - char *service_result; -}; - -struct PubConnectionData; - -struct TopicPubStats -{ - char *topic_name; - size_t message_data_sent; - struct PubConnectionData *datas; - size_t datas_count; -}; - -struct SubConnectionData; - -struct TopicSubStats -{ - char *topic_name; - struct SubConnectionData *datas; - size_t datas_count; -}; - -struct ServiceStats -{ - size_t num_requests; - size_t bytes_received; - size_t bytes_sent; -}; - -struct BusStats -{ - struct TopicPubStats *pub_stats; - size_t pub_stats_count; - struct TopicSubStats *sub_stats; - size_t sub_stats_count; - struct ServiceStats service_stats; -}; - -struct GetBusStatsResult -{ - int code; - char *status; - struct BusStats stats; -}; - -struct BusInfo; - -struct GetBusInfoResult -{ - int code; - char *status; - struct BusInfo *bus_infos; - size_t bus_infos_count; -}; - -struct GetMasterUriResult -{ - int code; - char *status; - char *master_uri; -}; - -struct ShutdownResult -{ - int code; - char *status; - int ignore; -}; - -struct GetPidResult -{ - int code; - char *status; - int server_process_pid; -}; - -struct GetSubscriptionsResult -{ - int code; - char *status; - struct TopicTypePair *topic_list; - size_t topic_count; -}; - -struct GetPublicationsResult -{ - int code; - char *status; - struct TopicTypePair *topic_list; - size_t topic_count; -}; - -struct DeleteParamResult -{ - int code; - char *status; - int ignore; -}; - -struct SetParamResult -{ - int code; - char *status; - int ignore; -}; - -struct GetParamResult -{ - int code; - char *status; - XmlrpcParam *value; -}; - -struct SearchParamResult -{ - int code; - char *status; - char *found_key; -}; - -struct HasParamResult -{ - int code; - char *status; - int has_param; -}; - -struct GetParamNamesResult -{ - int code; - char *status; - char **parameter_names; - size_t parameter_count; -}; - -struct TopicTypePair -{ - char *topic; - char *type; -}; - -struct ProviderState -{ - char *provider_name; - char **users; - size_t user_count; -}; - -struct PubConnectionData -{ - int connection_id; - size_t bytes_sent; - size_t num_sent; - int connected; -}; - -struct SubConnectionData -{ - int connection_id; - size_t bytes_received; - int drop_estimate; - int connected; -}; - -struct BusInfo -{ - int connectionId; - int destinationId; - CrosTransportDirection direction; - CrosTransportType transport; - char *topic; - int connected; -}; - -typedef struct LookupNodeResult LookupNodeResult; -typedef struct GetPublishedTopicsResult GetPublishedTopicsResult; -typedef struct GetTopicTypesResult GetTopicTypesResult; -typedef struct GetSystemStateResult GetSystemStateResult; -typedef struct GetUriResult GetUriResult; -typedef struct LookupServiceResult LookupServiceResult; -typedef struct GetBusStatsResult GetBusStatsResult; -typedef struct GetBusInfoResult GetBusInfoResult; -typedef struct GetMasterUriResult GetMasterUriResult; -typedef struct ShutdownResult ShutdownResult; -typedef struct GetPidResult GetPidResult; -typedef struct GetSubscriptionsResult GetSubscriptionsResult; -typedef struct GetPublicationsResult GetPublicationsResult; -typedef struct DeleteParamResult DeleteParamResult; -typedef struct SetParamResult SetParamResult; -typedef struct GetParamResult GetParamResult; -typedef struct SearchParamResult SearchParamResult; -typedef struct HasParamResult HasParamResult; -typedef struct GetParamNamesResult GetParamNamesResult; - -typedef void (*LookupNodeCallback)(int callid, LookupNodeResult *result, void *context); -typedef void (*GetPublishedTopicsCallback)(int callid, GetPublishedTopicsResult *result, void *context); -typedef void (*GetTopicTypesCallback)(int callid, GetTopicTypesResult *result, void *context); -typedef void (*GetSystemStateCallback)(int callid, GetSystemStateResult *result, void *context); -typedef void (*GetUriCallback)(int callid, GetUriResult *result, void *context); -typedef void (*LookupServiceCallback)(int callid, LookupServiceResult *result, void *context); -typedef void (*GetBusStatsCallback)(int callid, GetBusStatsResult *result, void *context); -typedef void (*GetBusInfoCallback)(int callid, GetBusInfoResult *result, void *context); -typedef void (*GetMasterUriCallback)(int callid, GetMasterUriResult *result, void *context); -typedef void (*RequestShutdownCallback)(int callid, ShutdownResult *result, void *context); -typedef void (*GetPidCallback)(int callid, GetPidResult *result, void *context); -typedef void (*GetSubscriptionsCallback)(int callid, GetSubscriptionsResult *result, void *context); -typedef void (*GetPublicationsCallback)(int callid, GetPublicationsResult *result, void *context); -typedef void (*DeleteParamCallback)(int callid, DeleteParamResult *result, void *context); -typedef void (*SetParamCallback)(int callid, SetParamResult *result, void *context); -typedef void (*GetParamCallback)(int callid, GetParamResult *result, void *context); -typedef void (*SearchParamCallback)(int callid, SearchParamResult *result, void *context); -typedef void (*HasParamCallback)(int callid, HasParamResult *result, void *context); -typedef void (*GetParamNamesCallback)(int callid, GetParamNamesResult *result, void *context); - -typedef uint8_t CallbackResponse; -typedef CallbackResponse (*ServiceCallerApiCallback)(cRosMessage *request, cRosMessage *response, int call_resp_flag, void *context); -typedef CallbackResponse (*ServiceProviderApiCallback)(cRosMessage *request, cRosMessage *response, void *context); -typedef CallbackResponse (*SubscriberApiCallback)(cRosMessage *message, void *context); -/*! \brief Application-defined callback function which is called by the library */ -typedef CallbackResponse (*PublisherApiCallback)(cRosMessage *message, void *context); - -// Transfer data from message buffer (context_) of the Publisher/Service caller to the output ROS packet buffer (buffer) -cRosErrCodePack cRosNodeSerializeOutgoingMessage(DynBuffer *buffer, void *context_); -// Transfer data from packet buffer (buffer) of the Service caller to the input mesage buffer (context_) -cRosErrCodePack cRosNodeDeserializeIncomingPacket(DynBuffer *buffer, void *context_); - -// Intermediary functions that call the user callback functions -// context is a structure (object) opaque for the caller function -cRosErrCodePack cRosNodeSubscriberCallback(void *context_); -cRosErrCodePack cRosNodePublisherCallback(void *context_); -cRosErrCodePack cRosNodeServiceCallerCallback(int call_resp_flag, void* contex_); -cRosErrCodePack cRosNodeServiceProviderCallback(void *context_); -void cRosNodeStatusCallback(CrosNodeStatusUsr *status, void* context_); - -// Master api: register/unregister methods -cRosErrCodePack cRosApiRegisterServiceCaller(CrosNode *node, const char *service_name, const char *service_type, int loop_period, ServiceCallerApiCallback callback, NodeStatusApiCallback status_callback, void *context, int persistent, int tcp_nodelay, int *svcidx_ptr); -void cRosApiReleaseServiceCaller(CrosNode *node, int svcidx); -cRosErrCodePack cRosApiRegisterServiceProvider(CrosNode *node, const char *service_name, const char *service_type, ServiceProviderApiCallback callback, NodeStatusApiCallback status_callback, void *context, int *svcidx_ptr); -cRosErrCodePack cRosApiUnregisterServiceProvider(CrosNode *node, int svcidx); -void cRosApiReleaseServiceProvider(CrosNode *node, int svcidx); -cRosErrCodePack cRosApiRegisterSubscriber(CrosNode *node, const char *topic_name, const char *topic_type, SubscriberApiCallback callback, NodeStatusApiCallback status_callback, void *context, int tcp_nodelay, int *subidx_ptr); -cRosErrCodePack cRosApiUnregisterSubscriber(CrosNode *node, int subidx); -void cRosApiReleaseSubscriber(CrosNode *node, int subidx); -cRosErrCodePack cRosApiRegisterPublisher(CrosNode *node, const char *topic_name, const char *topic_type, int loop_period, PublisherApiCallback callback, NodeStatusApiCallback status_callback, void *context, int *pubidx_ptr); -cRosErrCodePack cRosApiUnregisterPublisher(CrosNode *node, int pubidx); -void cRosApiReleasePublisher(CrosNode *node, int pubidx); - -// Master api: name service and system state -cRosErrCodePack cRosApiLookupNode(CrosNode *node, const char *node_name, LookupNodeCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiGetPublishedTopics(CrosNode *node, const char *subgraph, GetPublishedTopicsCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiGetTopicTypes(CrosNode *node, GetTopicTypesCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiGetSystemState(CrosNode *node, GetSystemStateCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiGetUri(CrosNode *node, GetUriCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiLookupService(CrosNode *node, const char *service, LookupServiceCallback callback, void *context, int *caller_id_ptr); - -// Slave API - -/*! - * \param if host == NULL, it will contact ROS Master - * \param So, host and port are only used if host != NULL - */ -cRosErrCodePack cRosApiGetBusStats(CrosNode *node, const char* host, int port, GetBusStatsCallback callback, void *context, int *caller_id_ptr); - -/*! - * \param if host == NULL, it will contact ROS Master - * \param So, host and port are only used if host != NULL - */ -cRosErrCodePack cRosApiGetBusInfo(CrosNode *node, const char* host, int port, GetBusInfoCallback callback, void *context, int *caller_id_ptr); - -/*! - * \param if host == NULL, it will contact ROS Master - * \param So, host and port are only used if host != NULL - */ -cRosErrCodePack cRosApiGetMasterUri(CrosNode *node, const char* host, int port, GetMasterUriCallback callback, void *context, int *caller_id_ptr); - -/*! - * \param if host == NULL, it will contact ROS Master - * \param So, host and port are only used if host != NULL - */ -cRosErrCodePack cRosApiShutdown(CrosNode *node, const char* host, int port, const char *msg, GetMasterUriCallback callback, void *context, int *caller_id_ptr); - -/*! - * \param if host == NULL, it will contact ROS Master - * \param So, host and port are only used if host != NULL - */ -cRosErrCodePack cRosApiGetPid(CrosNode *node, const char* host, int port, GetPidCallback callback, void *context, int *caller_id_ptr); - -/*! - * \param if host == NULL, it will contact ROS Master - * \param So, host and port are only used if host != NULL - */ -cRosErrCodePack cRosApiGetSubscriptions(CrosNode *node, const char* host, int port, GetSubscriptionsCallback callback, void *context, int *caller_id_ptr); - -/*! - * \param if host == NULL, it will contact ROS Master - * \param So, host and port are only used if host != NULL - */ -cRosErrCodePack cRosApiGetPublications(CrosNode *node, const char* host, int port, GetSubscriptionsCallback callback, void *context, int *caller_id_ptr); - -// Parameter Server API: subscribe/unsubscribe params -cRosErrCodePack cRosApiSubscribeParam(CrosNode *node, const char *key, NodeStatusApiCallback callback, void *context, int *paramsubidx_ptr); -cRosErrCodePack cRosApiUnsubscribeParam(CrosNode *node, int paramsubidx); - -// Parameter Server API: other methods -cRosErrCodePack cRosApiDeleteParam(CrosNode *node, const char *key, DeleteParamCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiSetParam(CrosNode *node, const char *key, XmlrpcParam *value, SetParamCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiGetParam(CrosNode *node, const char *key, GetParamCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiSearchParam(CrosNode *node, const char *key, SearchParamCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiHasParam(CrosNode *node, const char *key, HasParamCallback callback, void *context, int *caller_id_ptr); -cRosErrCodePack cRosApiGetParamNames(CrosNode *node, GetParamNamesCallback callback, void *context, int *caller_id_ptr); - -// Message polling -cRosErrCodePack cRosNodeReceiveTopicMsg(CrosNode *node, int subidx, cRosMessage *msg, unsigned char *buff_overflow, unsigned long time_out); -cRosErrCodePack cRosNodeQueueTopicMsg( CrosNode *node, int pubidx, cRosMessage *msg ); -cRosErrCodePack cRosNodeSendTopicMsg(CrosNode *node, int pubidx, cRosMessage *msg, unsigned long time_out); -cRosErrCodePack cRosNodeServiceCall(CrosNode *node, int svcidx, cRosMessage *req_msg, cRosMessage *resp_msg, unsigned long time_out); -cRosMessage *cRosApiCreatePublisherMessage(CrosNode *node, int pubidx); -cRosMessage *cRosApiCreateServiceCallerRequest(CrosNode *node, int svcidx); - -#endif // _CROS_API_H_ diff --git a/src/hal/components/cros/include/cros_api_call.h b/src/hal/components/cros/include/cros_api_call.h deleted file mode 100644 index dfe5fb29..00000000 --- a/src/hal/components/cros/include/cros_api_call.h +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef _CROS_API_CALL_H_ -#define _CROS_API_CALL_H_ - -#include "cros_node_api.h" -#include "xmlrpc_params_vector.h" - -typedef void (*ResultCallback)(int callid, void *result, void *context); -typedef void * (*FetchResultCallback)(XmlrpcParamVector *response); -typedef void (*FreeResultCallback)(void *result); - -typedef struct RosApiCall RosApiCall; -typedef struct ApiCallNode ApiCallNode; -typedef struct ApiCallQueue ApiCallQueue; - -struct RosApiCall -{ - int id; //! Progressive id of the call - int user_call; //! 1 = user api call, 0 = internal call - CrosApiMethod method; //! ROS api method - XmlrpcParamVector params; //! Method arguments - char *host; //! Host to contact for the api - int port; //! Tcp port of the host to contact for the api - int provider_idx; //! Provider (sub, pub, service provider or service caller) index - ResultCallback result_callback; //! Response callback - void *context_data; //! Result callback context - FetchResultCallback fetch_result_callback; //! Callback to fetch the result - FreeResultCallback free_result_callback; -}; - -struct ApiCallNode -{ - RosApiCall *call; - ApiCallNode* next; -}; - -struct ApiCallQueue -{ - ApiCallNode* head; - ApiCallNode* tail; - size_t count; -}; - -RosApiCall * newRosApiCall(void); -void freeRosApiCall(RosApiCall *call); - -void initApiCallQueue(ApiCallQueue *queue); -int enqueueApiCall(ApiCallQueue *queue, RosApiCall* apiCall); -RosApiCall * peekApiCallQueue(ApiCallQueue *queue); -RosApiCall * dequeueApiCall(ApiCallQueue *queue); -void releaseApiCallQueue(ApiCallQueue *queue); -size_t getQueueCount(ApiCallQueue *queue); -int isQueueEmpty(ApiCallQueue *queue); - -#endif // _CROS_API_CALL_H_ diff --git a/src/hal/components/cros/include/cros_api_internal.h b/src/hal/components/cros/include/cros_api_internal.h deleted file mode 100644 index ca078deb..00000000 --- a/src/hal/components/cros/include/cros_api_internal.h +++ /dev/null @@ -1,138 +0,0 @@ -#ifndef _CROS_NODE_INTERNAL_H_ -#define _CROS_NODE_INTERNAL_H_ - -#include "cros_node.h" - -/*! \brief Register a new topic to be published by a node. - * \param n Pointer to CrosNode structure that has previously been created with cRosNodeCreate - * \param message_definition Full text of message definition (output of gendeps --cat) - * \param topic_name The published topic namespace - * \param topic_type The published topic data type (e.g., std_msgs/String, ...) - * \param md5sum The MD5 sum of the message typedef - * \param loop_period Period (in msec) for publication cycle - * \param data_context Pointer to user data than will be passed to the callback function as context information. Can be NULL - * \return Returns the index of the created publisher on success, -1 on failure (e.g., the maximum number of publisher topics has been reached) - */ -int cRosNodeRegisterPublisher(CrosNode *n, const char *message_definition, const char *topic_name, - const char *topic_type, const char *md5sum, int loop_period, void *data_context); - -/*! \brief Register the node in roscore as topic subscriber. - * \param n Pointer to CrosNode structure that has previously been created with cRosNodeCreate - * \param message_definition Full text of message definition - * \param topic_name The published topic namespace - * \param topic_type The published topic data type (e.g., std_msgs/String, ...) - * \param md5sum The MD5 sum of the message typedef - * \param data_context Pointer to user data than will be passed to the callback function as context information. Can be NULL - * \param tcp_nodelay If this parameter is 1, the publisher is asked to disable the Nagle algorithm for the socket, - * so small packets are sent immediately, reducing the latency but increasing the bandwidth usage. - * \return Returns the index of the created subscriber on success, -1 on failure (e.g., the maximum number of subscriber topics has been reached) - */ -int cRosNodeRegisterSubscriber(CrosNode *n, const char *message_definition, const char *topic_name, const char *topic_type, - const char *md5sum, void *data_context, int tcp_nodelay); - -/*! \brief Register the service provider in roscore - * \param n Pointer to CrosNode structure that has previously been created with cRosNodeCreate - * \param service_name The published service namespace - * \param service_type The published service data type (e.g., roscpp_tutorials/TwoInts) - * \param md5sum The MD5 sum of the message typedef - * \param data_context Pointer to user data than will be passed to the callback function as context information. Can be NULL - * \return Returns the index of the created service provider on success, -1 on failure (e.g., the maximum number of service providers has been reached) - */ -int cRosNodeRegisterServiceProvider(CrosNode *n, const char *service_name, - const char *service_type, const char *md5sum, void *data_context); - -/*! \brief Register the service caller (not in roscore) - * \param n Pointer to CrosNode structure that has previously been created with cRosNodeCreate - * \param service_name The published service namespace - * \param service_type The published service data type (e.g., roscpp_tutorials/TwoInts) - * \param md5sum The MD5 sum of the message typedef - * \param data_context Pointer to user data than will be passed to the callback function as context information. Can be NULL - * \param persistent If this parameter is 1, the RPCROS connection is kept opened for multiple calls. - * This reduces bandwidth usage and latency. Otherwise the parameter value should be to 0. - * \param tcp_nodelay If this parameter is 1, the service provider is asked to disable the Nagle algorithm for the socket, - * so small packets are sent immediately, reducing the latency but increasing the bandwidth usage. - * \return Returns the index of the created service caller on success, -1 on failure (e.g., the maximum number of services has been reached) - */ -int cRosNodeRegisterServiceCaller(CrosNode *n, const char *message_definition, const char *service_name, - const char *service_type, const char *md5sum, int loop_period, - void *data_context, int persistent, int tcp_nodelay); - -/*! \brief Unregister the topic subscriber - * - * \param subidx Index of the subscriber - */ -int cRosNodeUnregisterSubscriber(CrosNode *node, int subidx); - -/*! \brief Unregister the topic publisher - * - * \param pubidx Index of the topic publisher - */ -int cRosNodeUnregisterPublisher(CrosNode *node, int pubidx); - -/*! \brief Unregister the service provider - * - * \param serviceidx Index of the service provider - */ -int cRosNodeUnregisterServiceProvider(CrosNode *node, int serviceidx); - - -/*! \brief Frees the memory of a publisher node - * - * \param node Pointer to the publisher node to be freed - */ -void cRosNodeReleasePublisher(PublisherNode *node); - -/*! \brief Frees the memory of a subscriber node - * - * \param node Pointer to the subscriber node to be freed - */ -void cRosNodeReleaseSubscriber(SubscriberNode *node); - -/*! \brief Frees the memory of a service-provider node - * - * \param node Pointer to the service node to be freed - */ -void cRosNodeReleaseServiceProvider(ServiceProviderNode *node); - -/*! \brief Frees the memory of a service-caller node - * - * \param node Pointer to the service node to be freed - */ -void cRosNodeReleaseServiceCaller(ServiceCallerNode *node); - -/*! \brief Frees the memory of a parameter subscription - * - * \param node Pointer to the parameter subscription to be freed - */ -void cRosNodeReleaseParameterSubscrition(ParameterSubscription *subscription); - -/*! \brief Search for a Tcpros client proc that is currently not assigned - * to any subscriber and assign it to the specified subscriber - * - * \param node Pointer to CrosNode structure that has previously been created with cRosNodeCreate - * \param subidx Index of the subscriber - * \return Returns the found Tcpros client index on success or -1 on failure (e.g., No Tcpros client available) - */ -int cRosNodeRecruitTcprosClientProc(CrosNode *node, int subidx); - -/*! \brief Search for a Tcpros client proc that is currently assigned - * to the specified subscriber and is connected to the specified hostname and port - * - * subidx, tcpros_hostname and tcpros_port set the search condition for the Tcpros client proc - * \param node Pointer to CrosNode structure that has previously been created with cRosNodeCreate - * \param subidx Index of the subscriber or -1 for any subscriber - * \param tcpros_hostname Pointer to the hostname string or NULL for any hostname - * \param tcpros_port Port number of -1 for any port - * \return Returns the found Tcpros client index on success or -1 on failure (e.g., No Tcpros client matching the search criteria) - */ -int cRosNodeFindFirstTcprosClientProc(CrosNode *node, int subidx, const char *tcpros_hostname, int tcpros_port); - -void restartAdversing(CrosNode* node); -int enqueueRequestTopic(CrosNode *node, int subidx, const char *host, int port); -int enqueueMasterApiCall(CrosNode *node, RosApiCall *call); -int enqueueSlaveApiCall(CrosNode *node, RosApiCall *call, const char *host, int port); - -void getMsgFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *topic_type); -void getSrvFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *service_type); - -#endif // _CROS_NODE_INTERNAL_H_ diff --git a/src/hal/components/cros/include/cros_clock.h b/src/hal/components/cros/include/cros_clock.h deleted file mode 100644 index c513bba2..00000000 --- a/src/hal/components/cros/include/cros_clock.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef _CROS_CLOCK_H_ -#define _CROS_CLOCK_H_ - -// declaration of struct timeval: -#ifdef _WIN32 -# include -#else -# include -#endif - -#include - -/*! \defgroup cros_clock cROS clock - * - * Utility functions for time management - */ - -/*! \addtogroup cros_clock - * @{ - */ - -/*! \brief Return the current time, expressed as seconds and microseconds since the Epoch - * - * \return The current time - */ -struct timeval cRosClockGetTimeSecUsec( void ); - -/*! \brief Return the current time, expressed as milliseconds since the Epoch - * - * \return The current time - */ -uint64_t cRosClockGetTimeMs( void ); - -/*! \brief Convert an interval expressed as milliseconds in a timeval structure, - * that express the same interval as seconds and microseconds - * - * \return The time interval express with a timeval structure - */ -struct timeval cRosClockGetTimeVal( uint64_t msec ); - -/*! \brief Get a high-precision real-time time stamp from the system. - * Time stamps can be substracted to obtain time-stamp differences, which - * can be converted to us using cRosClockGetTimeStamp() - * - * \return The time interval expressed in arbitrary units - */ -int64_t cRosClockGetTimeStamp(void); - -/*! \brief Convert a time stamp in arbitrary units to us. - * These time stamps can be obtained by using the cRosClockGetTimeStamp() function. - * Time differences obtained substracting these time stamps can be also converted. - * - * \return The time stamp (or difference) in microseconds. - */ -double cRosClockTimeStampToUSec(int64_t time_stamp); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/cros_defs.h b/src/hal/components/cros/include/cros_defs.h deleted file mode 100644 index f57d73eb..00000000 --- a/src/hal/components/cros/include/cros_defs.h +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef _CROS_DEFS_H_ -#define _CROS_DEFS_H_ - -#include -#include "cros_node.h" - -/*! \defgroup cros_defs Cros defintions */ - -/*! \addtogroup cros_defs - * @{ - */ - -#ifndef CROS_DEBUG_LEVEL -# define CROS_DEBUG_LEVEL 1 -#endif - -#if CROS_DEBUG_LEVEL >= 1 -# define PRINT_INFO(...) fprintf(cRosOutStreamGet(),__VA_ARGS__) -#else -# define PRINT_INFO(...) -#endif - -#if CROS_DEBUG_LEVEL >= 2 -# define PRINT_DEBUG(...) printf(__VA_ARGS__) -#else -# define PRINT_DEBUG(...) -#endif - -#if CROS_DEBUG_LEVEL >= 3 -# define PRINT_VDEBUG(...) printf(__VA_ARGS__) -#else -# define PRINT_VDEBUG(...) -#endif - -#if CROS_DEBUG_LEVEL >= 4 -# define PRINT_VVDEBUG(...) printf(__VA_ARGS__) -#else -# define PRINT_VVDEBUG(...) -#endif - -#define PRINT_ERROR(...) fprintf(cRosOutStreamGet(), __VA_ARGS__) - -#define FLUSH_PRINT() fflush(cRosOutStreamGet()) - -// macros for detecting and converting endianness - -// We ignore the existence of mixed endianness -#ifdef _WIN32 -# define LITTLE_ENDIAN_ARC 1 -#else -# ifdef __APPLE__ -# ifndef __BIG_ENDIAN__ -# define LITTLE_ENDIAN_ARC 1 -# endif -# else -# include -# if __BYTE_ORDER == __LITTLE_ENDIAN -# define LITTLE_ENDIAN_ARC 1 -# endif -# endif -#endif - -// We only support processors with sizeof(char)==1 -#if LITTLE_ENDIAN_ARC -# define HOST_TO_ROS_UINT32(val) (val) -#else -# define HOST_TO_ROS_UINT32(val) (((val)>>24)&0x000000FFUL) | \ - (((val)<<8)&0X00FF0000UL) | \ - (((val)>>8)&0X0000FF00UL) | \ - (((val)<<24)&0XFF000000UL) -#endif - -#define ROS_TO_HOST_UINT32 HOST_TO_ROS_UINT32 - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/cros_err_codes.h b/src/hal/components/cros/include/cros_err_codes.h deleted file mode 100644 index c4ee397d..00000000 --- a/src/hal/components/cros/include/cros_err_codes.h +++ /dev/null @@ -1,162 +0,0 @@ -/// \file cros_err_codes.h -/// \brief This header file define the codes for the error messages of the cROS library. -/// The functions for managing these codes are declared as well. -/// -/// \author Richard R. Carrillo, Aging in Vision and Action lab, Institut de la Vision, Sorbonne University, Paris, France. - -#ifndef __CROS_ERR_CODES_H__ -#define __CROS_ERR_CODES_H__ -#include - -///////////////////////////////// ERROR messages ///////////////////////////////// -//! Definition of all the error codes. One code per line. Up to 255 error codes -#define ERROR_CODE_LIST_DEF \ - MSG_COD_ELEM(CROS_NO_ERR, "Operation completed successfully") \ - MSG_COD_ELEM(CROS_UNSPECIFIED_ERR, "An unspecified error occurred") \ - MSG_COD_ELEM(CROS_MEM_ALLOC_ERR, "Error allocating memory") \ - MSG_COD_ELEM(CROS_BAD_PARAM_ERR, "An invalid value has been specified for at least one of the input parameters") \ - MSG_COD_ELEM(CROS_OPEN_MSG_FILE_ERR, "The message file definition the cannot be opened") \ - MSG_COD_ELEM(CROS_READ_MSG_FILE_ERR, "Error reading the message definition file") \ - MSG_COD_ELEM(CROS_LOAD_MSG_FILE_ERR, "Error loading or processing the message definition file") \ - MSG_COD_ELEM(CROS_OPEN_SVC_FILE_ERR, "The file defining the service cannot be opened") \ - MSG_COD_ELEM(CROS_READ_SVC_FILE_ERR, "Error reading the service definition file") \ - MSG_COD_ELEM(CROS_UNREG_TIMEOUT_ERR, "The unregistration from the ROS master was abandoned before it finished because it was taking too long") \ - MSG_COD_ELEM(CROS_SELECT_FD_ERR, "An error ocurred while monitoring the socket file descriptors (select() function)") \ - MSG_COD_ELEM(CROS_MANY_PARAM_ERR, "The maximum number of paramter subscriptions has been reached") \ - MSG_COD_ELEM(CROS_PARAM_SUB_IND_ERR, "The provided parameter subscriber index does not corresponds to a valid subscriber to be unsubscribed") \ - MSG_COD_ELEM(CROS_TOPIC_PUB_IND_ERR, "The provided topic publisher index does not corresponds to a valid publisher to be unregistered") \ - MSG_COD_ELEM(CROS_TOPIC_SUB_IND_ERR, "The provided topic subscriber index does not corresponds to a valid subscriber to be unregistered") \ - MSG_COD_ELEM(CROS_SVC_FILE_DELIM_ERR, "The delimiter string between resquest and response definition could not be found in the service definition file") \ - MSG_COD_ELEM(CROS_FILE_ENTRY_TYPE_ERR, "The definition file contains an entry that specifies and incorrect data type") \ - MSG_COD_ELEM(CROS_LOAD_SVC_FILE_REQ_ERR, "Error loading the service request definition in the file") \ - MSG_COD_ELEM(CROS_LOAD_SVC_FILE_RES_ERR, "Error loading the service response definition in the file") \ - MSG_COD_ELEM(CROS_CREATE_CUSTOM_MSG_ERR, "Error loading the specified custom message definition file or creating a message of its type") \ - MSG_COD_ELEM(CROS_FILE_ENTRY_NO_SEP_ERR, "The definition file contains an entry that is syntactically incorrect (a white space is expected between data type and data name)") \ - MSG_COD_ELEM(CROS_DEPACK_INSUFF_DAT_ERR, "Error decoding a received packet: The length of the packet is too small for the expected data type") \ - MSG_COD_ELEM(CROS_DEPACK_NO_MSG_DEF_ERR, "Error decoding a received packet: The definition of the custom message does not corresponds to the fields of the message to send") \ - MSG_COD_ELEM(CROS_TOP_PUB_CALLBACK_ERR, "The callback function specified for a topic publisher returned a non-zero value") \ - MSG_COD_ELEM(CROS_TOP_SUB_CALLBACK_ERR, "The callback function specified for a topic subscriber returned a non-zero value") \ - MSG_COD_ELEM(CROS_SVC_REQ_CALLBACK_ERR, "The callback function specified for a service client returned a non-zero value when generating the service request") \ - MSG_COD_ELEM(CROS_SVC_RES_CALLBACK_ERR, "The callback function specified for a service client returned a non-zero value when generating the service response") \ - MSG_COD_ELEM(CROS_SVC_SER_CALLBACK_ERR, "The callback function specified for a service server returned a non-zero value") \ - MSG_COD_ELEM(CROS_SVC_RES_OK_BYTE_ERR, "The response received from the service server contains an 'ok' byte codifying a value different from true (1)") \ - MSG_COD_ELEM(CROS_RCV_TOP_TIMEOUT_ERR, "The specified timeout was up while waiting for a topic message") \ - MSG_COD_ELEM(CROS_SEND_TOP_TIMEOUT_ERR, "The specified timeout was up while waiting for space in the message transmission queue") \ - MSG_COD_ELEM(CROS_CALL_SVC_TIMEOUT_ERR, "The specified timeout was up while waiting for the service call response") \ - MSG_COD_ELEM(CROS_CALL_INI_TIMEOUT_ERR, "The specified timeout was up while the service caller was waiting for the previous call to finish") \ - MSG_COD_ELEM(CROS_XMLRPC_CLI_CONN_ERR, "An error occurred when an XMLRPC client process was trying to establish the connection to the target address") \ - MSG_COD_ELEM(CROS_XMLRPC_CLI_REFUS_ERR, "An XMLRPC client process could not establish the connection to the target address because the connection was refused") \ - MSG_COD_ELEM(CROS_XMLRPC_CLI_WRITE_ERR, "An error arised when the XMLRPC client process tried to write the request on the socket") \ - MSG_COD_ELEM(CROS_XMLRPC_CLI_READ_ERR, "An error arised when the XMLRPC client process tried to read the response from the socket") \ - MSG_COD_ELEM(CROS_TCPROS_CLI_CONN_ERR, "A TCPROS client process could not establish the connection to the target address due to an error") \ - MSG_COD_ELEM(CROS_TCPROS_CLI_REFUS_ERR, "An TCPROS client process could not establish the connection to the target address because the connection was refused") \ - MSG_COD_ELEM(CROS_RPCROS_CLI_CONN_ERR, "A RPCROS client process could not establish the connection to the target address due to an error") \ - MSG_COD_ELEM(CROS_RPCROS_CLI_REFUS_ERR, "An RPCROS client process could not establish the connection to the target address because the connection was refused") \ - MSG_COD_ELEM(CROS_SOCK_OPEN_TIMEOUT_ERR, "The specified timeout was up while waiting for the specified port to be open") \ - MSG_COD_ELEM(CROS_SOCK_OPEN_CONN_ERR, "An error occurred when the specified target port was tried to be connected (target address could not be resolved?)") \ - MSG_COD_ELEM(CROS_EXTRACT_MSG_INT_ERR, "An internal error occurred when sending an inmediate message: The message could not be extracted from the queue") \ - MSG_COD_ELEM(LAST_ERR_LIST_CODE, "") // Sentinel code used to mark the last element of the global error list - -#define CROS_SUCCESS_ERR_PACK 0U //! Function return value indicating success - -// Declare the enum data type used for encoding error codes and declare the error code for errors (CROS_NO_ERR,...) -#define MSG_COD_ELEM(code,msg) code, -enum cRosErrorCode { ERROR_CODE_LIST_DEF }; -#undef MSG_COD_ELEM - -typedef enum cRosErrorCode cRosErrCode; -// Declare an entry type of the message list variable that will contain the global list of error messages -struct cRosErrCodeListElem -{ - cRosErrCode code; - const char *msg; -}; - -//! Error data type returned by many cROS library functions. It is a pack that can contain up to 4 accumulated error codes (uint8_t) -typedef uint32_t cRosErrCodePack; - - -/*! \brief Locates the message string corresponding to the specified error code in the global error message list. - * - * The function locates the message string describing the specified error code (a single error code, not a pack). - * \param err_code number of the error - * \return Pointer to the message string. If no message is found for the specified code, it returns NULL - */ -const char *cRosGetErrCodeStr(cRosErrCode err_code); - -/*! \brief Add a new error code to the specified error code pack. - * - * This function is intended to add more information (a new error code) to an error code pack. - * If the new error code is CROS_NO_ERR (0), nothing is added, that is, the prev_err_pack is returned. - * parameter list. - * \param prev_err_pack is the original error code pack. - * \param err_code number of the new error. - * \return the new error pack (containing the specified err_code if err_code was not CROS_NO_ERR) - */ -cRosErrCodePack cRosAddErrCode(cRosErrCodePack prev_err_pack, cRosErrCode err_code); - -/*! \brief Add a new error code to the specified non-empty error code pack. - * - * This function is intended to add more information (a new error code) to an error code pack that already contains errors. - * If the specified error code pack does not contain any error, this function returns the same error code pack specified in the - * parameter list. - * \param prev_err_pack is the original error code pack. - * \param err_code number of the new error. If this code is CROS_NO_ERR, prev_err_pack is returned - * \return the new error pack (containing the specified err_code if prev_err_pack was not initially CROS_SUCCESS_ERR_PACK) - */ -cRosErrCodePack cRosAddErrCodeIfErr(cRosErrCodePack prev_err_pack, cRosErrCode err_code); - -/*! \brief Remove the last error code inserted in the specified error code pack. - * - * \param err_pack the original error code pack. - * \return the new error code pack with the last code removed, or the original pack if no error code was contained in it. - */ -cRosErrCodePack cRosRemoveLastErrCode(cRosErrCodePack prev_err_pack); - -/*! \brief Obtain the last error code inserted in the specified error code pack. - * - * \param err_pack the error code pack. - * \return the last error code of the pack or CROS_NO_ERR (0) if no error is codified in the pack - */ -cRosErrCode cRosGetLastErrCode(cRosErrCodePack err_pack); - -/*! \brief Compile the error codes contained in two error code packs into a single error code pack. - * - * This function is intended to add new error codes to an error code pack that already may contain errors. - * If the specified error code packs do not contain any error, this function returns an empty code pack (CROS_SUCCESS_ERR_PACK). - * \param prev_err_pack_0 is the first error code pack. - * \param prev_err_pack_1 is the second error code pack. - * \return the new error pack joining the two error packs - */ -cRosErrCodePack cRosAddErrCodePackIfErr(cRosErrCodePack prev_err_pack_0, cRosErrCodePack prev_err_pack_1); - -/*! \brief Composes and print the error message corresponding to the specified error code pack. - * - * The function search for the error strings indicated in the specified error pack. The composed message is printed - * in the output file stream. The error pack can contain up to 4 error codes, so up to 4 message string can be printed. - * Additionally the text string specified by fmt_str is printed before the error strings. This string can be used to - * supply the user with context information about the error. - * \param err structure encoding the occurred error (usually returned by a failing function). - * \param fmt_str string that specifies how subsequent arguments are printed. It has the same format as printf - * function. - * \return the number of characters written in the output stream. - */ -int cRosPrintErrCodePack(cRosErrCodePack err_cod_pack, const char *fmt_str, ...); - -/*! \brief Composes the error message corresponding to the specified error code pack and writes it to the specified buffer. - * - * The function search for the error strings indicated in the specified error pack. The error pack can contain up to 4 error - * codes, so up to 4 message string can be printed. - * Additionally the text string specified by fmt_str is printed before the error strings. This string can be used to - * supply the user with context information about the error. - * If not enough space if available in the output buffer the output is truncated and a '\0' is added at the end of the buffer. - * \param out_str_buf Pointer to the buffer where the output string will be stored. - * \param out_str_buf_len Length of the output buffer. - * \param err structure encoding the occurred error (usually returned by a failing function). - * \param fmt_str string that specifies how subsequent arguments are printed. It has the same format as printf - * function. - * \return the number of characters that would be written to the output buffer if it had enough space. - */ -int cRosErrCodePackStr(char *out_str_buf, size_t out_str_buf_len, cRosErrCodePack err_cod_pack, const char *fmt_str, ...); - -#endif /* __CROS_ERR_CODES_H__ */ diff --git a/src/hal/components/cros/include/cros_gentools.h b/src/hal/components/cros/include/cros_gentools.h deleted file mode 100644 index d9df0c45..00000000 --- a/src/hal/components/cros/include/cros_gentools.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef _CROS_GENTOOLS_H_ -#define _CROS_GENTOOLS_H_ - -/*! \defgroup cros_gentools cROS message generation tool - * - * Implemenation of the ROS message generation tool and - * dependency resolutor (gentools and gendeps) - */ - -/*! \addtogroup cros_gentool - * @{ - */ - -/*! \brief Generate md5 hash of files - * - * \param filename Full path of the message/service file - */ -char* cRosGentoolsMD5(char* filename); - -/*! \brief Generate SHA1 hash of files - * - * \param filename Full path of the message/service file - * - * \return Returns 1 on success, 0 on failure - */ -int cRosGentoolsSHA1(char* filename); - -/*! \brief Generate concatenated list of files - * - * \param filename Full path of the message/service file - * - */ -int cRosGentoolsFulltext(char* filename); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/cros_log.h b/src/hal/components/cros/include/cros_log.h deleted file mode 100644 index d99531a2..00000000 --- a/src/hal/components/cros/include/cros_log.h +++ /dev/null @@ -1,67 +0,0 @@ -/*! \file cros_log.h - * \brief This file declares the function and macros (ROS_INFO, ROS_DEBUG, ROS_WARN, ROS_ERROR and ROS_FATAL) - * for printing messages using the ROS log. These messages are sent through the /rosout topic to the /rosout node. - * - * These macros must not be confused with the macros for printing messages localy (either in a local log file or local console) which - * are defined in cros_defs.h: PRINT_INFO, PRINT_DEBUG, PRINT_VDEBUG, PRINT_VVDEBUG and PRINT_ERROR - */ - -#ifndef _CROS_LOG_H_ -#define _CROS_LOG_H_ - -#include -#include - -#define PRINT_LOG(node,log_level,...) \ - cRosLogPrint(node,\ - log_level,\ - __FILE__,\ - __FUNCTION__,\ - __LINE__,\ - __VA_ARGS__) - -#define ROS_INFO(node,...) PRINT_LOG(node, CROS_LOGLEVEL_INFO, __VA_ARGS__) -#define ROS_DEBUG(node,...) PRINT_LOG(node, CROS_LOGLEVEL_DEBUG, __VA_ARGS__) -#define ROS_WARN(node,...) PRINT_LOG(node, CROS_LOGLEVEL_WARN, __VA_ARGS__) -#define ROS_ERROR(node,...) PRINT_LOG(node, CROS_LOGLEVEL_ERROR, __VA_ARGS__) -#define ROS_FATAL(node,...) PRINT_LOG(node, CROS_LOGLEVEL_FATAL, __VA_ARGS__) - -typedef struct CrosLog CrosLog; - -struct CrosNode; // We forward declare CrosNode struct since it is used by cRosLogPrint() before it is declared - -struct CrosLog -{ - uint8_t level; //! debug level - char* name; //! name of the node - char* msg; //! message - char* file; //! file the message came from - char* function; //! function the message came from - uint32_t line; //! line the message came from - char** pubs; //! topic names that the node publishes - uint32_t secs; - uint32_t nsecs; - size_t n_pubs; -}; - -typedef enum CrosLogLevel //!Logging levels -{ - CROS_LOGLEVEL_DEBUG = 1, - CROS_LOGLEVEL_INFO = 2, - CROS_LOGLEVEL_WARN = 4, - CROS_LOGLEVEL_ERROR = 8, - CROS_LOGLEVEL_FATAL = 16 -} CrosLogLevel; - -CrosLog *cRosLogNew(void); -void cRosLogFree(CrosLog *log); -int stringToLogLevel(const char* level_str, CrosLogLevel *level_num); -const char *LogLevelToString(CrosLogLevel log_level); -void cRosLogPrint(struct CrosNode *node, - CrosLogLevel level, // debug level - const char *file, // file the message came from - const char *function, // function the message came from - uint32_t line, - const char *msg, ...); - -#endif //_CROS_LOG_H diff --git a/src/hal/components/cros/include/cros_message.h b/src/hal/components/cros/include/cros_message.h deleted file mode 100644 index ab748985..00000000 --- a/src/hal/components/cros/include/cros_message.h +++ /dev/null @@ -1,209 +0,0 @@ -#ifndef _CROS_MESSAGE_H_ -#define _CROS_MESSAGE_H_ - -#include "dyn_buffer.h" -#include "cros_err_codes.h" - -/*! \defgroup cros_message cROS TCPROS - * - * Implemenation of the TCPROS protocol for topic message exchanges - */ - -/*! \addtogroup cros_message - * @{ - */ - -typedef enum CrosMessageType -{ - CROS_CUSTOM_TYPE = 0, - CROS_STD_MSGS_INT8, - CROS_STD_MSGS_UINT8, - CROS_STD_MSGS_INT16, - CROS_STD_MSGS_UINT16, - CROS_STD_MSGS_INT32, - CROS_STD_MSGS_UINT32, - CROS_STD_MSGS_INT64, - CROS_STD_MSGS_UINT64, - CROS_STD_MSGS_FLOAT32, - CROS_STD_MSGS_FLOAT64, - CROS_STD_MSGS_STRING, - CROS_STD_MSGS_BOOL, - CROS_STD_MSGS_TIME, - CROS_STD_MSGS_DURATION, - CROS_STD_MSGS_HEADER, - // deprecated - CROS_STD_MSGS_CHAR, - CROS_STD_MSGS_BYTE -} CrosMessageType; - -typedef struct cRosMessageField cRosMessageField; -typedef struct cRosMessage cRosMessage; - -struct cRosMessageField -{ - char *name; - - union data_t - { - uint8_t opaque[8]; - int8_t as_int8; - uint8_t as_uint8; - int16_t as_int16; - uint16_t as_uint16; - int32_t as_int32; - uint32_t as_uint32; - int64_t as_int64; - uint64_t as_uint64; - float as_float32; - double as_float64; - char *as_string; - cRosMessage *as_msg; - int8_t *as_int8_array; - uint8_t *as_uint8_array; - int16_t *as_int16_array; - uint16_t *as_uint16_array; - int32_t *as_int32_array; - uint32_t *as_uint32_array; - int64_t *as_int64_array; - uint64_t *as_uint64_array; - float *as_float32_array; - double *as_float64_array; - char **as_string_array; - cRosMessage **as_msg_array; - void *as_array; - } data; - int size; - int is_const; - int is_array; - int is_fixed_array; - int array_size; - int array_capacity; - CrosMessageType type; - char *type_s; -}; - -typedef struct t_msgDef cRosMessageDef; - -struct cRosMessage -{ - cRosMessageField **fields; - cRosMessageDef *msgDef; - char *md5sum; - int n_fields; -}; - -cRosMessage * cRosMessageNew(void); - -void cRosMessageInit(cRosMessage *message); - -cRosErrCodePack cRosMessageDefBuild(cRosMessageDef **msg_def_ptr, const char *msg_root_dir, const char *msg_type); - -cRosErrCodePack cRosMessageNewBuild(const char *msg_root_dir, const char *msg_type, cRosMessage **new_msg_ptr); - -void cRosMessageFieldsPrint(cRosMessage *msg, int n_indent); - -int cRosMessageFieldCopy(cRosMessageField *new_field, cRosMessageField *orig_field); - -int cRosMessageFieldsCopy(cRosMessage *m_dst, cRosMessage *m_src); - -cRosMessage *cRosMessageCopyWithoutDef(cRosMessage *m_src); - -cRosMessage *cRosMessageCopy(cRosMessage *m_src); - -cRosErrCodePack cRosMessageBuildFromDef(cRosMessage **message, cRosMessageDef *msg_def ); - -void cRosMessageFree(cRosMessage *message); - -void cRosMessageFieldsFree(cRosMessage *message); - -void cRosMessageRelease(cRosMessage *message); - -cRosMessageField * cRosMessageFieldNew(void); - -void cRosMessageFieldInit(cRosMessageField *field); - -void cRosMessageFieldRelease(cRosMessageField *field); - -void cRosMessageFieldFree(cRosMessageField *field); - -cRosMessageField * cRosMessageGetField(cRosMessage *message, const char *field); - -int cRosMessageSetFieldValueString(cRosMessageField *field, const char *value); - -int cRosMessageFieldArrayPushBackInt8(cRosMessageField *field, int8_t val); - -int cRosMessageFieldArrayPushBackInt16(cRosMessageField *field, int16_t val); - -int cRosMessageFieldArrayPushBackInt32(cRosMessageField *field, int32_t val); - -int cRosMessageFieldArrayPushBackInt64(cRosMessageField *field, int64_t val); - -int cRosMessageFieldArrayPushBackUInt8(cRosMessageField *field, uint8_t val); - -int cRosMessageFieldArrayPushBackUInt16(cRosMessageField *field, uint16_t val); - -int cRosMessageFieldArrayPushBackUInt32(cRosMessageField *field, uint32_t val); - -int cRosMessageFieldArrayPushBackUInt64(cRosMessageField *field, uint64_t val); - -int cRosMessageFieldArrayPushBackFloat32(cRosMessageField *field, float val); - -int cRosMessageFieldArrayPushBackFloat64(cRosMessageField *field, double val); - -int cRosMessageFieldArrayPushBackString(cRosMessageField *field, const char *val); - -int cRosMessageFieldArrayPushBackZero(cRosMessage *msg, int n_field); - -int cRosMessageFieldArrayPushBackMsg(cRosMessageField *field, cRosMessage *msg); - -cRosMessage *cRosMessageFieldArrayRemoveLastMsg(cRosMessageField *field); - -int8_t * cRosMessageFieldArrayAtInt8(cRosMessageField *field, int position); - -int16_t * cRosMessageFieldArrayAtInt16(cRosMessageField *field, int position); - -int32_t * cRosMessageFieldArrayAtInt32(cRosMessageField *field, int position); - -int64_t * cRosMessageFieldArrayAtInt64(cRosMessageField *field, int position); - -uint8_t * cRosMessageFieldArrayAtUInt8(cRosMessageField *field, int position); - -uint16_t * cRosMessageFieldArrayAtUInt16(cRosMessageField *field, int position); - -uint32_t * cRosMessageFieldArrayAtUInt32(cRosMessageField *field, int position); - -uint64_t * cRosMessageFieldArrayAtUInt64(cRosMessageField *field, int position); - -float * cRosMessageFieldArrayAtFloat32(cRosMessageField *field, int position); - -double * cRosMessageFieldArrayAtFloat64(cRosMessageField *field, int position); - -char *cRosMessageFieldArrayAtStringGet(cRosMessageField *field, int position); - -int cRosMessageFieldArrayAtStringSet(cRosMessageField *field, int position, const char *val); - -cRosMessage *cRosMessageFieldArrayAtMsgGet(cRosMessageField *field, int position); - -int cRosMessageFieldArrayAtMsgSet(cRosMessageField *field, int position, cRosMessage *val); - -int cRosMessageFieldArrayClear(cRosMessageField *field); - -size_t cRosMessageSize(cRosMessage *message); - -cRosErrCodePack cRosMessageSerialize(cRosMessage *message, DynBuffer *buffer); - -cRosErrCodePack cRosMessageDeserialize(cRosMessage *message, DynBuffer *buffer); - -CrosMessageType getMessageType(const char *type); - -const char * getMessageTypeString(CrosMessageType type); - -const char * getMessageTypeDeclaration(CrosMessageType type); - -size_t getMessageTypeSizeOf(CrosMessageType type); - -int isBuiltinMessageType(CrosMessageType type); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/cros_message_internal.h b/src/hal/components/cros/include/cros_message_internal.h deleted file mode 100644 index efb799ec..00000000 --- a/src/hal/components/cros/include/cros_message_internal.h +++ /dev/null @@ -1,119 +0,0 @@ -#ifndef _CROS_MESSAGE_INTERNAL_H_ -#define _CROS_MESSAGE_INTERNAL_H_ - -#include "dyn_string.h" -#include "cros_err_codes.h" - -static const char* FILEEXT_MSG = "msg"; - -// e.g. std_msgs/String -static const char* CHAR_SEP = "/"; - -// character that designates a constant assignment rather than a field -static const char* CHAR_CONST = "="; - -// char that denotes the comment line start -static const char* CHAR_COMMENT = "#"; - -static const char* HEADER_DEFAULT_PACK = "std_msgs"; -static const char* HEADER_DEFAULT_NAME = "Header"; -static const char* HEADER_DEFAULT_TYPE = "std_msgs/Header"; - -static const char* HEADER_DEFAULT_TYPEDEF = -"# Standard metadata for higher-level stamped data types.\n" -"# This is generally used to communicate timestamped data \n" -"# in a particular coordinate frame.\n" -"# \n" -"# sequence ID: consecutively increasing ID \n" -"uint32 seq\n" -"#Two-integer timestamp that is expressed as:\n" -"# * stamp.secs: seconds (stamp_secs) since epoch\n" -"# * stamp.nsecs: nanoseconds since stamp_secs\n" -"# time-handling sugar is provided by the client library\n" -"time stamp\n" -"#Frame this data is associated with\n" -"# 0: no frame\n" -"string frame_id\n\n"; - - -struct t_msgFieldDef -{ - CrosMessageType type; - char* type_s; - char* name; - int is_array; - int array_size; - struct t_msgFieldDef* prev; - struct t_msgFieldDef* next; - cRosMessageDef* child_msg_def; // Stores the definition of a type defined trough a custom message file -}; - -typedef struct t_msgFieldDef msgFieldDef; - -struct t_msgConst -{ - CrosMessageType type; - char* type_s; - char* name; - char* value; - struct t_msgConst* prev; - struct t_msgConst* next; -}; - -typedef struct t_msgConst msgConst; - -struct t_msgDef -{ - char* name; - char* package; - char* root_dir; - char* plain_text; - msgFieldDef* fields; - msgFieldDef* first_field; - msgConst* constants; - msgConst* first_const; -}; - -typedef struct t_msgDef cRosMessageDef; - -struct t_msgDep -{ - cRosMessageDef* msg; - struct t_msgDep* prev; - struct t_msgDep* next; -}; - -typedef struct t_msgDep msgDep; - -cRosErrCodePack getFileDependenciesMsg(char* filename, cRosMessageDef* msg, msgDep* deps); - -// Compute full text of message, including text of embedded -// types. The text of the main msg is listed first. Embedded -// msg files are denoted first by an 80-character '=' separator, -// followed by a type declaration line,'MSG: pkg/type', followed by -// the text of the embedded type. -char* computeFullTextMsg(cRosMessageDef* msg, msgDep* deps); - -cRosErrCodePack getDependenciesMsg(cRosMessageDef* msg, msgDep* msgDeps); - -void cRosMD5Readable(unsigned char* data, DynString* output); - -cRosErrCodePack getMD5Txt(cRosMessageDef* msg, DynString* buffer); - -cRosErrCodePack initCrosMsg(cRosMessageDef* msg); - -void initMsgConst(msgConst *msg); - -void initCrosDep(msgDep* dep); - -void initFieldDef(msgFieldDef* field); - -cRosErrCodePack loadFromStringMsg(char* text, cRosMessageDef* msg); - -cRosErrCodePack loadFromFileMsg(char* filename, cRosMessageDef* msg); - -cRosErrCodePack cRosMessageDefCopy(cRosMessageDef** ptr_new_msg_def, cRosMessageDef* orig_msg_def ); - -void cRosMessageDefFree(cRosMessageDef *msgDef); - -#endif // _CROS_MESSAGE_INTERNAL_H_ diff --git a/src/hal/components/cros/include/cros_message_queue.h b/src/hal/components/cros/include/cros_message_queue.h deleted file mode 100644 index 6db796b9..00000000 --- a/src/hal/components/cros/include/cros_message_queue.h +++ /dev/null @@ -1,122 +0,0 @@ -/*! \file cros_message_queue.h - * \brief This header file declares the cRosMessageQueue type and associated management functions - * - * cRosMessageQueue implements a queue of TCPROS-protocol messages. This queue behaves as a - * FIFO (First In First Out). - * This queue only stores the fields of the messages ('fields' fields of the struct), not the message definition (msgDef field). - * \author Richard R. Carrillo. Aging in Vision and Action lab, Institut de la Vision, Sorbonne University, Paris, France. - * \date 31 Oct 2017 - */ - -#ifndef _CROS_MESSAGE_QUEUE_H_ -#define _CROS_MESSAGE_QUEUE_H_ - -#include "cros_message.h" - - -#define MAX_QUEUE_LEN 10 //! Maximum number of messages that can be hold in the queue - -struct cRosMessageQueue -{ - cRosMessage msgs[MAX_QUEUE_LEN]; //! Content of the queue - unsigned int length; //! Number of messages currently in the queue - unsigned int first_msg_ind; //! Index of the oldest message in the queue (the one that was inserted first) -}; - -typedef struct cRosMessageQueue cRosMessageQueue; - -/*! \brief Initializes a queue. - * - * This function must be called before using a queue for the first time. - * \param q Pointer to the queue. - */ -void cRosMessageQueueInit(cRosMessageQueue *q); - -/*! \brief Empty queue. - * - * This function removes all the messages in a queue. - * \param q Pointer to the queue. - */ -void cRosMessageQueueClear(cRosMessageQueue *q); - -/*! \brief Deletes the content of a queue and free all its allocated memory. - * - * This function frees the memory allocated for its content so cRosMessageQueueInit() must be called before - * using the queue again. - * \param q Pointer to the queue. - */ -void cRosMessageQueueRelease(cRosMessageQueue *q); - -/*! \brief Calculates the free space still available in the queue. - * - * \return Number of messages that can still be added to the queue without causing an overflow. - */ -unsigned int cRosMessageQueueVacancies(cRosMessageQueue *q); - -/*! \brief Calculates the number of messages stored in the queue. - * - * \return Number of messages that can be removed from the queue. - */ -unsigned int cRosMessageQueueUsage(cRosMessageQueue *q); - -/*! \brief Add a new message at the end of the queue. - * - * This function adds a new element (message) at the end of the queue. The fields of the message pointed by m will be copied - * in internal memory of the queue, so the message pointed by m can be freed after being added. - * \param q Pointer to the queue. - * \param m Pointer to the message to be added. - * \return 0 on success, otherwise an error code: -1 = error allocating memory, -2 = No free space to add a new element. - */ -int cRosMessageQueueAdd(cRosMessageQueue *q, cRosMessage *m); - -/*! \brief Extract the first message of the queue. - * - * This function removes a element (message) at the start of the queue. The fields of the message at the start of the queue will be copied - * to the message pointed by m, so the message pointed by m should be freed independently after being used. - * \param q Pointer to the queue. - * \param m Pointer to the message where the fields of the removed message are copied. - * \return 0 on success, otherwise an error code: -1 = error allocating memory, -2 = No messages in the queue. If an error occurs, the - * message is not removed from the queue. - */ -int cRosMessageQueueExtract(cRosMessageQueue *q, cRosMessage *m); - -/*! \brief Get a copy of the first message from the queue. - * - * This function obtains the element (message) at the start of the queue. The fields of the message at the start of the queue will be copied - * to the message pointed by m, so the message pointed by m should be freed independently after being used. - * The queue is not modified. - * \param q Pointer to the queue. - * \param m Pointer to the message where the fields of the removed message are copied. - * \return 0 on success, otherwise an error code: -1 = error allocating memory, -2 = No messages in the queue. - */ -int cRosMessageQueueGet(cRosMessageQueue *q, cRosMessage *m); - -/*! \brief Delete the first message from the queue. - * - * This function removes a element (message) at the start of the queue. - * \param q Pointer to the queue. - * \return 0 on success, otherwise an error code: -2 = No messages in the queue. - */ -int cRosMessageQueueRemove(cRosMessageQueue *q); - -/*! \brief Get a reference of the first message from the queue. - * - * This function obtains a pointer to the element (message) at the start of the queue (oldest element). The fields of the message at the - * start of the queue will not be copied, so if the message queue is modified after obtaining this pointer, the pointer become invalid. - * The queue is not modified. - * \param q Pointer to the queue. - * \return Pointer to the first message. If the first message could not be obtained, NULL is returned. - */ -cRosMessage *cRosMessageQueuePeekFirst(cRosMessageQueue *q); - -/*! \brief Get a reference of the last message from the queue. - * - * This function obtains a pointer to the element (message) at the end of the queue (newest element). The fields of this message - * of the queue will not be copied, so if the message queue is modified after obtaining this pointer, the pointer become invalid. - * The queue is not modified by this function. - * \param q Pointer to the queue. - * \return Pointer to the last message. If the last message could not be obtained, NULL is returned. - */ -cRosMessage *cRosMessageQueuePeekLast(cRosMessageQueue *q); - -#endif // _CROS_MESSAGE_QUEUE_H_ diff --git a/src/hal/components/cros/include/cros_node.h b/src/hal/components/cros/include/cros_node.h deleted file mode 100644 index 56def9cb..00000000 --- a/src/hal/components/cros/include/cros_node.h +++ /dev/null @@ -1,344 +0,0 @@ -/*! \file cros_node.h - * \brief This file declares the CrosNode structure, which codifies a ROS node in cROS. It also declares - * associated substructures, functions and macros. - * - * This ROS node can implement at the same time several: - * - Topic subscribers - * - Topic publishers - * - Service providers (servers) - * - Service callers (clients) - * - Parameter subscriber - */ - -#ifndef _CROS_NODE_H_ -#define _CROS_NODE_H_ - -#include -#include - -#include "cros_log.h" -#include "xmlrpc_process.h" -#include "tcpros_process.h" -#include "cros_api_call.h" -#include "cros_message_queue.h" -#include "cros_err_codes.h" - -/*! \defgroup cros_node cROS Node */ - -/*! \addtogroup cros_node - * @{ - */ - -/*! Max num published topics */ -#define CN_MAX_PUBLISHED_TOPICS 5 - -/*! Max num subscribed topics */ -#define CN_MAX_SUBSCRIBED_TOPICS 5 - -/*! Max num service providers */ -#define CN_MAX_SERVICE_PROVIDERS 8 - -/*! Max num service callers */ -#define CN_MAX_SERVICE_CALLERS 8 - -/*! Max num parameter subscriptions */ -#define CN_MAX_PARAMETER_SUBSCRIPTIONS 20 - -/*! Max num serving XMLRPC connections */ -#define CN_MAX_XMLRPC_SERVER_CONNECTIONS 5 - -/*! Max num serving TCPROS connections */ -#define CN_MAX_TCPROS_SERVER_CONNECTIONS 5 - -/*! Max num serving RPCROS connections */ -#define CN_MAX_RPCROS_SERVER_CONNECTIONS CN_MAX_SERVICE_PROVIDERS - -/*! - * Max num XMLRPC connections against another subscribed nodes - * (first connection index reserved to roscore) - * */ -#define CN_MAX_XMLRPC_CLIENT_CONNECTIONS 1 + CN_MAX_SUBSCRIBED_TOPICS - -/*! - * Max num TCPROS connections against another subscribed nodes - * */ -#define CN_MAX_TCPROS_CLIENT_CONNECTIONS CN_MAX_SUBSCRIBED_TOPICS - -/*! - * Max num RPCROS connections against other service-providing nodes - * (service calls are one to one, so, one TcprosProcess per ServiceCallerNode) - * */ -#define CN_MAX_RPCROS_CLIENT_CONNECTIONS CN_MAX_SERVICE_CALLERS - -/*! Node automatic XMLRPC ping cycle period (in msec) */ -#define CN_PING_LOOP_PERIOD 1000 - -/*! Maximum I/O operations timeout (in msec) */ -#define CN_IO_TIMEOUT 3000 - -/*! Maximum time that the node will wait for unregistering all publishers, subscribers, servicer providers... in the ROS master (in msec) */ -#define CN_UNREGISTRATION_TIMEOUT 3000 - -typedef struct PublisherNode PublisherNode; -typedef struct SubscriberNode SubscriberNode; -typedef struct ServiceProviderNode ServiceProviderNode; -typedef struct ServiceCallerNode ServiceCallerNode; -typedef struct ParameterSubscription ParameterSubscription; - -typedef enum CrosNodeStatus -{ - // TODO: this is a work in progress - CROS_STATUS_NONE = 0, - CROS_STATUS_PUBLISHER_UNREGISTERED, - CROS_STATUS_SUBSCRIBER_UNREGISTERED, - CROS_STATUS_SERVICE_PROVIDER_UNREGISTERED, - CROS_STATUS_PARAM_UNSUBSCRIBED, - CROS_STATUS_PARAM_SUBSCRIBED, - CROS_STATUS_PARAM_UPDATE, -} CrosNodeStatus; - -typedef struct CrosNodeStatusUsr -{ - // FIXME: this is a work in progress - // int callid; // This may be useful to track register/unregister - CrosNodeStatus state; // May be useful to understand what the node, or the particular sub/pub/svc is doing - int provider_idx; - const char *xmlrpc_host; - int xmlrpc_port; - const char *parameter_key; - XmlrpcParam *parameter_value; -} CrosNodeStatusUsr; - -/*! \brief Callback to communicate publisher or subscriber status */ -typedef void (*NodeStatusApiCallback)(CrosNodeStatusUsr *status, void* context); - -/*! Structure that define a published topic */ -struct PublisherNode -{ - char *topic_name; //! The published topic name - char *topic_type; //! The published topic data type (e.g., std_msgs/String, ...) - char *md5sum; //! The MD5 sum of the message type - char *message_definition; //! Full text of message definition (output of gendeps --cat) - int tcpros_id_list[CN_MAX_TCPROS_SERVER_CONNECTIONS+1]; //! List of node->tcpros_server_proc IDs allocated for this publisher. The last element of the list is always -1 (sentinel) - void *context; - int loop_period; //! Period (in msec) for publication cycle - uint64_t wake_up_time; //! The time for the next automatic message publication (in msec, since the Epoch) - cRosMessageQueue msg_queue; //! Messages on this topic wait in this queue to be send for every process -}; - -/*! Structure that define a subscribed topic */ -struct SubscriberNode -{ - char *message_definition; //! Full text of message definition (output of gendeps --cat) - char *topic_name; //! The subscribed topic name - char *topic_type; //! The subscribed topic data type (e.g., std_msgs/String, ...) - char *md5sum; //! The MD5 sum of the message type - unsigned char tcp_nodelay; //! If 1, the publisher should set TCP_NODELAY on the socket, if possible - void *context; //! Pointer to an internal library structure that stores received messages and its type - cRosMessageQueue msg_queue; //! Each time a message on this topic is received it is queued here - unsigned char msg_queue_overflow; //! If 1, the subscriber tried to insert a message in the queue but it was full -}; - -struct ServiceProviderNode -{ - char *service_name; - char *service_type; - char *servicerequest_type; - char *serviceresponse_type; - char *md5sum; - void *context; -}; - -struct ServiceCallerNode -{ - char *service_name; - char *service_type; - char *servicerequest_type; - char *serviceresponse_type; - char *md5sum; - char *message_definition; //! Full text of message definition (output of gendeps --cat) - int rpcros_id; //! Index of the node->service_callers allocated for this ServiceCallerNode - char *service_host; //! The hostname of the service provider. - int service_port; //! The host port of the the service provider. - unsigned char persistent; //! If 1, the service RPCROS connection should be kept open for multiple requests - unsigned char tcp_nodelay; //! If 1, the service caller should set TCP_NODELAY on the socket, if possible - void *context; - int loop_period; //! Period (in msec) for service-call cycle - uint64_t wake_up_time; //! The time for the next automatic service call (in msec, since the Epoch) - cRosMessageQueue msg_queue; //! Service requests and service responses for this service wait in this queue to be send -}; - -struct ParameterSubscription -{ - char *parameter_key; - XmlrpcParam parameter_value; - void *context; - NodeStatusApiCallback status_api_callback; -}; - -/*! \brief CrosNode object. Don't modify its internal members: use the related functions instead */ -typedef struct CrosNode CrosNode; -struct CrosNode -{ - char *name; //! The node name: it is the absolute name, i.e. it includes the namespace - char *host; //! The node host (ipv4, e.g. 192.168.0.2) - unsigned short xmlrpc_port; //! The node port for the XMLRPC protocol - unsigned short tcpros_port; //! The node port for the TCPROS protocol - unsigned short rpcros_port; //! The node port for the RPCROS protocol - - int pid; //! cROS node process ID - int roscore_pid; //! Roscore PID - - char *roscore_host; //! The roscore host (ipv4, e.g. 192.168.0.1) - unsigned short roscore_port; //! The roscore port - - char *message_root_path; //! Directory with the message register - - CrosLogLevel log_level; - int rosout_pub_idx; //! Index of the publisher of the /rosout topic for ROS log messages - - uint64_t xmlrpc_master_wake_up_time; //! The time (in msec, since the Epoch) for the next automatic operation cycle of the xmlrpc_client_proc[0] (xmlrpc master-node client proc) - - uint32_t log_last_id; //! Sequence number of the last transmitted rosout log message - - unsigned int next_call_id; - ApiCallQueue master_api_queue; - ApiCallQueue slave_api_queue; - - //! Manage connections for XMLRPC calls from this node to others - XmlrpcProcess xmlrpc_client_proc[CN_MAX_XMLRPC_CLIENT_CONNECTIONS]; - XmlrpcProcess xmlrpc_listner_proc; //! Accept new XMLRPC connections from roscore or other nodes - /*! Manage connections for XMLRPC calls from roscore or other nodes to this node */ - XmlrpcProcess xmlrpc_server_proc[CN_MAX_XMLRPC_SERVER_CONNECTIONS]; - - //! Manage connections for TCPROS calls from this node to others - TcprosProcess tcpros_client_proc[CN_MAX_TCPROS_CLIENT_CONNECTIONS]; - TcprosProcess tcpros_listner_proc; //! Accept new TCPROS connections from roscore or other nodes - - /*! Manage connections for TCPROS between this and other nodes */ - TcprosProcess tcpros_server_proc[CN_MAX_TCPROS_SERVER_CONNECTIONS]; - - //! Manage connections for RPCROS calls from this node to others - TcprosProcess rpcros_client_proc[CN_MAX_RPCROS_CLIENT_CONNECTIONS]; - TcprosProcess rpcros_listner_proc; //! Accept new TCPROS connections from roscore or other nodes - - /*! Manage connections for RPCROS between this and other nodes */ - TcprosProcess rpcros_server_proc[CN_MAX_RPCROS_SERVER_CONNECTIONS]; - - PublisherNode pubs[CN_MAX_PUBLISHED_TOPICS]; //! All the published topic, defined by PublisherNode structures - SubscriberNode subs[CN_MAX_SUBSCRIBED_TOPICS]; //! All the subscribed topic, defined by PublisherNode structures - ServiceProviderNode service_providers[CN_MAX_SERVICE_PROVIDERS]; //! All the provided services to register - ServiceCallerNode service_callers[CN_MAX_SERVICE_CALLERS]; //! All the services to call - ParameterSubscription paramsubs[CN_MAX_PARAMETER_SUBSCRIPTIONS]; - - int n_pubs; //! Number of node's published topics - int n_subs; //! Number of node's subscribed topics - int n_service_providers; //! Number of registered services to provide - int n_service_callers; //! Number of services to call - int n_paramsubs; -}; - -/*! \brief Resolve the namespace of the resource name - * - * \param node the CrosNode which is owner of the resource. If NULL the resource is a node as well. - * \param resource_name the name of the resource. - * - * \return A string with the resource name. - */ -char *cRosNamespaceBuild(CrosNode *node, const char *resource_name); - -void cRosGetMsgFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *topic_type); - -/*! \brief Dynamically create a CrosNode instance. This is the right way to create a CrosNode object. - * Once finished, the CrosNode should be released using cRosNodeDestroy() - * - * \param node_name The node name: it is the absolute name, i.e. it should includes the namespace - * \param node_host The node host (ipv4, e.g. 192.168.0.2) - * \param roscore_host The roscore host (ipv4, e.g. 192.168.0.1) - * \param roscore_port The roscore port - * \param select_timeout_ms Max timeout for the select() in ms. NULL defaults to UINT64_MAX - * - * \return A pointer to the new CrosNode on success, NULL on failure - */ -CrosNode *cRosNodeCreate(const char *node_name, const char *node_host, const char *roscore_host, unsigned short roscore_port, - const char *message_root_path); - -/*! \brief Unregister from ROS master and release all the internal allocated memory for a CrosNode - * object previously crated with cRosNodeCreate() - * - * \param n A pointer to the CrosNode object to be released - * \return CROS_SUCCESS_ERR_PACK (0) on success. Otherwise an error code pack containing one or more error codes - */ -cRosErrCodePack cRosNodeDestroy( CrosNode *n ); - -/*! \brief Perform a loop of the cROS node main cycle - * - * \param n A pointer to a CrosNode object (e.g., created with cRosNodeCreate()) - * \param max_timeout Maximum time in milliseconds that this function will take to finish (it may finish before). - * - * cRosNodeDoEventsLoop() perform a file-based event loop: check the read and write operation availability - * for considered sockets, start new write and read actions, open new connections, close dropped connections, - * manage the internal state of the node (i.e., advertise new topics, ...) - * - * cRosNodeDoEventsLoop() should be used inside a cycle, e.g.: - * - * while(1) - * { - * // If you want, do here something - * cRosNodeDoEventsLoop( node ); - * } - * \return CROS_SUCCESS_ERR_PACK (0) on success - */ -cRosErrCodePack cRosNodeDoEventsLoop( CrosNode *n, uint64_t max_timeout ); - -/*! \brief Run the cROS node for a specific time while the exit flag provided by the user is 0 - * - * This function repeatedly calls cRosNodeDoEventsLoop() while these three conditions are met: - * No error occurs, the exit_flag variable is 0 and the timeout is not reached. - * \param n A pointer to a CrosNode object (e.g., created with cRosNodeCreate()) - * \param time_out Time in milliseconds that this function will block running the node. If CROS_INFINITE_TIMEOUT - * is specified, the function will not finish until an error occurs or the exit flag becomes different - * from 0. - * \param exit_flag Pointer to an unsigned char variable: the function will exit if this variable becomes - * different from zero. If this pointer is NULL, the node will be run until the timeout is reached or an - * an error occurs. - * \return CROS_SUCCESS_ERR_PACK (0) on success - */ -cRosErrCodePack cRosNodeStart( CrosNode *n, unsigned long time_out, unsigned char *exit_flag ); - -XmlrpcParam *cRosNodeGetParameterValue( CrosNode *n, const char *key); -/*! @}*/ - -/*! \brief Waits until a network port is open is a host address. - * - * This function repeatedly tries to connect to the specified port until it succeed, an error occurs or the - * specified timeout is reached. When the connection is established it is closed immediately. - * \param host_addr The target address - * \param host_port The target port - * \param time_out Maximum time in milliseconds that this function will block trying to connect. If - * CROS_INFINITE_TIMEOUT is specified, the function will not finish until an error occurs or the - * the port connection is established. - * \return CROS_SUCCESS_ERR_PACK if the port could be connected. CROS_SOCK_OPEN_CONN_ERR is an error - * occurred when trying to connect or CROS_SOCK_OPEN_TIMEOUT_ERR if the timeout was reached before - * the port could be connected. - */ -cRosErrCodePack cRosWaitPortOpen(const char *host_addr, unsigned short host_port, unsigned long time_out); - -/*! \brief Set the file stream used when printing messages locally, that is, through macros: - * PRINT_INFO, PRINT_DEBUG, PRINT_VDEBUG, PRINT_VVDEBUG and PRINT_ERROR. - * - * There are two types of message printing: - * - local message printing (which are printed to a file or to stdout depending on the file stream specified - * when calling this function). - * - ROS log messages. These messages are not affected by this function. - * \param new_stream The file stream used. It must be a valid file stream or NULL if stdout must be used. - */ -void cRosOutStreamSet(FILE *new_stream); - -/*! \brief Get the file stream used for printing local messages. - * - * \return The file stream used. If NULL was specified when calling cRosOutStreamSet(), stdout is returned. - */ -FILE *cRosOutStreamGet(void); - -#endif diff --git a/src/hal/components/cros/include/cros_node_api.h b/src/hal/components/cros/include/cros_node_api.h deleted file mode 100644 index 3e9cc211..00000000 --- a/src/hal/components/cros/include/cros_node_api.h +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef _CROS_NODE_API_H_ -#define _CROS_NODE_API_H_ - -#define CROS_TRANSPORT_TCPROS_STRING "TCPROS" -#define CROS_TRANSPORT_UPDROS_STRING "UDPROS" - -typedef enum -{ - CROS_API_NONE = 0, - CROS_API_REGISTER_SERVICE, - CROS_API_UNREGISTER_SERVICE, - CROS_API_UNREGISTER_PUBLISHER, - CROS_API_UNREGISTER_SUBSCRIBER, - CROS_API_REGISTER_PUBLISHER, - CROS_API_REGISTER_SUBSCRIBER, - CROS_API_LOOKUP_NODE, - CROS_API_GET_PUBLISHED_TOPICS, - CROS_API_GET_TOPIC_TYPES, - CROS_API_GET_SYSTEM_STATE, - CROS_API_GET_URI, - CROS_API_LOOKUP_SERVICE, - CROS_API_GET_BUS_STATS, - CROS_API_GET_BUS_INFO, - CROS_API_GET_MASTER_URI, - CROS_API_SHUTDOWN, - CROS_API_GET_PID, - CROS_API_GET_SUBSCRIPTIONS, - CROS_API_GET_PUBLICATIONS, - CROS_API_PARAM_UPDATE, - CROS_API_PUBLISHER_UPDATE, - CROS_API_REQUEST_TOPIC, - CROS_API_DELETE_PARAM, - CROS_API_SET_PARAM, - CROS_API_GET_PARAM, - CROS_API_SEARCH_PARAM, - CROS_API_SUBSCRIBE_PARAM, - CROS_API_UNSUBSCRIBE_PARAM, - CROS_API_HAS_PARAM, - CROS_API_GET_PARAM_NAMES -} CrosApiMethod; - -/*! \defgroup cros_api cROS APIs - * - * Internal implemenation (via XMLRPC protocol) of the ROS - * Master, Parameter Server and Slave APIs - * NOTE: this are cROS internal functions, usually you don't need to use them. - */ - -/*! \addtogroup cros_api - * @{ - */ - -struct CrosNode; - -/*! \brief Prepare a XMLRPC message for a ROS XMLRPC protocol function call, - * This function modify the internal member of the XmlrpcProcess client_proc - * of a cROS node. The function to call is chosen checking the currente - * node's internal state (enum CrosNodeState) , - * - * \param n Pointer to the CrosNode object - * \param client_idx The client id that manage the request - */ -void cRosApiPrepareRequest(struct CrosNode *n, int client_idx ); - - -/*! \brief Parse a XMLRPC response generated from a previous API function call ( see cRosApiPrepareRequest() ), - * eventually changing the internal state of the node n - * - * \param n Ponter to the CrosNode object - * \param client_idx The client id that manage the response - * - * \return Returns 0 on success, -1 on failure - */ -int cRosApiParseResponse(struct CrosNode *n, int client_idx ); - -/*! \brief Parse a XMLRPC request (i.e., an API function call generated from roscore or anther node), given - * the XmlrpcProcess server_proc with index server_idx. - * Eventually this function perfomrs the requested actions, eventually changing the internal state of the node n. - * Moreover, it prepares the related XMLRPC response. - * - * \param n Ponter to the CrosNode object - * \param server_idx Index of the XmlrpcProcess ( xmlrpc_server_proc[server_idx] ) to be considered for the parsing and - * the response generation - * - * \return Returns 0 on success, -1 on failure - */ -int cRosApiParseRequestPrepareResponse(struct CrosNode *n, int server_idx ); - -const char * getMethodName(CrosApiMethod method); -CrosApiMethod getMethodCode(const char *method); -int isRosMasterApi(CrosApiMethod method); -int isRosSlaveApi(CrosApiMethod method); - -struct CrosNodeStatusUsr; - -void initCrosNodeStatus(struct CrosNodeStatusUsr *status); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/cros_service.h b/src/hal/components/cros/include/cros_service.h deleted file mode 100644 index 67404d61..00000000 --- a/src/hal/components/cros/include/cros_service.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef _CROS_SERVICE_H_ -#define _CROS_SERVICE_H_ - -#include "cros_node.h" -#include "cros_message.h" -#include "cros_err_codes.h" - -/*! \defgroup cros_service cROS TCPROS - * - * Implementation of the TCPROS protocol for service message exchanges - */ - -/*! \addtogroup cros_service - * @{ - */ - -struct cRosService -{ - cRosMessage *request; - cRosMessage *response; - char* md5sum; -}; - -typedef struct cRosService cRosService; - -cRosService * cRosServiceNew(); -cRosErrCodePack cRosServiceInit(cRosService* service); -cRosErrCodePack cRosServiceBuild(cRosService* service, const char* filepath); -void cRosServiceRelease(cRosService* service); -void cRosServiceFree(cRosService* service); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/cros_service_internal.h b/src/hal/components/cros/include/cros_service_internal.h deleted file mode 100644 index 192f81d8..00000000 --- a/src/hal/components/cros/include/cros_service_internal.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef _CROS_SERVICE_INTERNAL_H_ -#define _CROS_SERVICE_INTERNAL_H_ - -#include "cros_message.h" -#include "cros_message_internal.h" -#include "cros_err_codes.h" - -static const char* FILEEXT_SRV = "srv"; - -// string that denotes the separation between request/response service parts -static const char* SRV_DELIMITER = "---"; - -struct t_srvDef -{ - char* name; - char* package; - char* root_dir; - char* plain_text; - cRosMessageDef* request; - cRosMessageDef* response; -}; - -typedef struct t_srvDef cRosSrvDef; - -cRosErrCodePack cRosServiceBuildInner(cRosMessage **request, cRosMessage **response, char **message_definition, char *md5sum, const char *srv_filepath); -cRosErrCodePack initCrosSrv(cRosSrvDef* srv); - -cRosErrCodePack getFileDependenciesSrv(char* filename, cRosSrvDef* srv, msgDep* deps); - -// Compute full text of service, including text of embedded -// types. The text of the main srv is listed first. Embedded -// srv files are denoted first by an 80-character '=' separator, -// followed by a type declaration line,'MSG: pkg/type', followed by -// the text of the embedded type. -char* computeFullTextSrv(cRosSrvDef* srv, msgDep* deps); - -cRosErrCodePack loadFromFileSrv(const char *filename, cRosSrvDef* srv); - -void cRosServiceDefFree(cRosSrvDef* service_def); - -#endif // _CROS_SERVICE_INTERNAL_H_ diff --git a/src/hal/components/cros/include/cros_tcpros.h b/src/hal/components/cros/include/cros_tcpros.h deleted file mode 100644 index f030758e..00000000 --- a/src/hal/components/cros/include/cros_tcpros.h +++ /dev/null @@ -1,137 +0,0 @@ -#ifndef _CROS_TCPROS_H_ -#define _CROS_TCPROS_H_ - -#include "cros_node.h" - -typedef enum -{ - TCPROS_PARSER_ERROR = 0, - TCPROS_PARSER_HEADER_INCOMPLETE, - TCPROS_PARSER_DATA_INCOMPLETE, - TCPROS_PARSER_DONE -} TcprosParserState; - -enum -{ - TCPROS_OK_BYTE_FAIL = 0, - TCPROS_OK_BYTE_SUCCESS = 1 -}; - -/*! \brief Parse a TCPROS header sent initially from a subscriber - * - * \param n Ponter to the CrosNode object - * \param server_idx Index of the TcprosProcess ( tcpros_server_proc[server_idx] ) to be considered for the parsing - * - * \return Returns TCPROS_PARSER_DONE if the header is successfully parsed, - * TCPROS_PARSER_HEADER_INCOMPLETE if the header is incomplete, - * or TCPROS_PARSER_ERROR on failure - */ -TcprosParserState cRosMessageParseSubcriptionHeader( CrosNode *n, int server_idx ); - -/*! \brief Parse a TCPROS header sent from a publisher - * - * \param n Ponter to the CrosNode object - * \param client_idx Index of the TcprosProcess ( tcpros_client_proc[server_idx] ) to be considered for the parsing - * - * \return Returns TCPROS_PARSER_DONE if the header is successfully parsed, - * TCPROS_PARSER_HEADER_INCOMPLETE if the header is incomplete, - * or TCPROS_PARSER_ERROR on failure - */ -TcprosParserState cRosMessageParsePublicationHeader( CrosNode *n, int client_idx ); - -/*! \brief Prepare a TCPROS header to be initially sent to a publisher - * - * \param n Ponter to the CrosNode object - * \param client_idx Index of the TcprosProcess ( tcpros_client_proc[server_idx] ) to be considered - */ -void cRosMessagePrepareSubcriptionHeader( CrosNode *n, int client_idx ); - -/*! \brief Prepare a TCPROS header to be initially sent back to a subscriber - * - * \param n Ponter to the CrosNode object - * \param server_idx Index of the TcprosProcess ( tcpros_server_proc[server_idx] ) to be considered - */ -void cRosMessagePreparePublicationHeader( CrosNode *n, int server_idx ); - -/*! \brief Prepare a TCPROS message (with data) to be sent to a subscriber - * - * \param n Ponter to the CrosNode object - * \param server_idx Index of the TcprosProcess ( tcpros_server_proc[server_idx] ) to be considered - * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code - */ -cRosErrCodePack cRosMessagePreparePublicationPacket( CrosNode *n, int server_idx ); - -/*! \brief Read the TCPROS message (with data) received from the publisher - * - * \param n Ponter to the CrosNode object - * \param client_idx Index of the TcprosProcess ( tcpros_client_proc[server_idx] ) to be considered for the parsing - * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code - */ -cRosErrCode cRosMessageParsePublicationPacket( CrosNode *n, int client_idx ); - -/*! \brief Parse a RCPROS header sent from a service caller - * - * \param n Ponter to the CrosNode object - * \param server_idx Index of the TcprosProcess ( rpcros_server_proc[server_idx] ) to be considered for the parsing - * - * \return Returns TCPROS_PARSER_DONE if the header is successfully parsed, - * or TCPROS_PARSER_ERROR on failure - */ -TcprosParserState cRosMessageParseServiceCallerHeader( CrosNode *n, int server_idx); - -/*! \brief Parse a RCPROS header sent by a service provider - * - * \param n Ponter to the CrosNode object - * \param client_idx Index of the TcprosProcess ( rpcros_server_proc[server_idx] ) to be considered for the parsing - * - * \return Returns TCPROS_PARSER_DONE if the header is successfully parsed, - * or TCPROS_PARSER_ERROR on failure - */ -TcprosParserState cRosMessageParseServiceProviderHeader( CrosNode *n, int client_idx); - -/*! \brief Prepare a RCPROS header to be sent to a service provider - * - * \param n Ponter to the CrosNode object - * \param client_idx Index of the TcprosProcess ( rpcros_client_proc[client_idx] ) to be considered - */ -void cRosMessagePrepareServiceCallHeader( CrosNode *n, int client_idx); - -/*! \brief Prepare a RCPROS packet to be sent to a service provider - * - * \param n Ponter to the CrosNode object - * \param client_idx Index of the TcprosProcess ( rpcros_client_proc[client_idx] ) to be considered - * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code - */ -cRosErrCodePack cRosMessagePrepareServiceCallPacket( CrosNode *n, int client_idx ); - -/*! \brief Prepare a RCPROS header to be initially sent back to a service caller - * - * \param n Ponter to the CrosNode object - * \param server_idx Index of the TcprosProcess ( rpcros_server_proc[server_idx] ) to be considered - */ -void cRosMessagePrepareServiceProviderHeader( CrosNode *n, int server_idx); - -/*! \brief Read the RPCROS packet (with data) received from the service provider - * - * \param n Ponter to the CrosNode object - * \param client_idx Index of the TcprosProcess ( rpcros_client_proc[client_idx] ) to be considered for the parsing - * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code - */ -cRosErrCodePack cRosMessageParseServiceResponsePacket( CrosNode *n, int client_idx ); - -/*! \brief Prepare a RCPROS response to be sent back to a service caller - * - * \param n Ponter to the CrosNode object - * \param server_idx Index of the TcprosProcess ( rpcros_server_proc[server_idx] ) to be considered - * \return CROS_SUCCESS_ERR_PACK on success, otherwise an error code - */ -cRosErrCodePack cRosMessagePrepareServiceResponsePacket( CrosNode *n, int server_idx); - -/*! \brief Prepare a RCPROS header to be initially sent to a service provider - * - * \param n Ponter to the CrosNode object - * \param server_idx Index of the TcprosProcess ( rpcros_client_proc[client_idx] ) to be considered - */ -void cRosMessagePrepareServiceCallerHeader(CrosNode *n, int server_idx); - -#endif // _CROS_TCPROS_H_ diff --git a/src/hal/components/cros/include/dyn_buffer.h b/src/hal/components/cros/include/dyn_buffer.h deleted file mode 100644 index f51ba5a1..00000000 --- a/src/hal/components/cros/include/dyn_buffer.h +++ /dev/null @@ -1,249 +0,0 @@ -#ifndef _DYN_BUFFER_H_ -#define _DYN_BUFFER_H_ - -#include - -/*! \defgroup dyn_buffer Dynamic buffer */ - -/*! \addtogroup dyn_buffer - * @{ - */ - -/*! \brief Dynamic buffer object. Don't modify its internal members: use - * the related functions instead */ -typedef struct DynBuffer DynBuffer; -struct DynBuffer -{ - size_t size; //! Current buffer size - size_t pos_offset; //! Current position indicator - size_t max; //! Max buffer size - unsigned char *data; //! buffer data -}; - -/*! \brief Initialize a dynamic buffer - * - * \param d_buf Pointer to a DynBuffer object to be initialized - */ -void dynBufferInit( DynBuffer *d_buf ); - -/*! \brief Release a dynamic buffer - * - * \param d_buf Pointer to a DynBuffer object to be released - */ -void dynBufferRelease( DynBuffer *d_buf ); - -/*! \brief Append a copy of the n bytes pointed by new_buf to the end of the dynamic buffer pointed by d_buf - * - * \param d_buf Pointer to a DynBuffer object - * \param new_str Pointer to the buffer to be appended - * \param n Number of the bytes to be copied - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackBuf( DynBuffer *d_buf, const unsigned char *new_buf, size_t n ); - -/*! \brief Replace the content of the dynamic buffer starting from current position indicator with the content - * of the buffer cont_buf. - * - * If the length of cont_buf (cont_buf_len) is shorter that the remaining data in the dynamic buffer, the - * dynamic buffer is truncated so that the end to the end of the dynamic buffer if occupied by pointed by cont_buf. - * If the length of remaining content in the dynamic buffer is shorter that cont_buf the dynamic buffer content is - * increased. - * \param d_buf Pointer to a DynBuffer object - * \param cont_buf Pointer to the buffer to write in the dynamic buffer - * \param cont_buf_len Length of the buffer cont_buf - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferReplaceContent( DynBuffer *d_buf, const unsigned char *cont_buf, size_t cont_buf_len ); - -/*! \brief Append a 8 bit signed integer - * to the end of the dynamic buffer pointed by d_buf - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackInt8( DynBuffer *d_buf, int8_t val ); - -/*! \brief Append a 16 bit signed integer (in binary format) - * to the end of the dynamic buffer pointed by d_buf - * WARNING: The byte order depends on the endianess of your platform - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackInt16( DynBuffer *d_buf, int16_t val ); - -/*! \brief Append a 32 bit signed integer (in binary format) - * to the end of the dynamic buffer pointed by d_buf - * WARNING: The byte order depends on the endianess of your platform - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackInt32( DynBuffer *d_buf, int32_t val ); - -/*! \brief Append a 64 bit signed integer (in binary format) - * to the end of the dynamic buffer pointed by d_buf - * WARNING: The byte order depends on the endianess of your platform - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackInt64( DynBuffer *d_buf, int64_t val ); - -/*! \brief Append a 8 bit signed integer - * to the end of the dynamic buffer pointed by d_buf - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackUInt8( DynBuffer *d_buf, uint8_t val ); - -/*! \brief Append a 16 bit unsigned integer (in binary format) - * to the end of the dynamic buffer pointed by d_buf - * WARNING: The byte order depends on the endianess of your platform - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackUInt16( DynBuffer *d_buf, uint16_t val ); - -/*! \brief Append a 32 bit unsigned integer (in binary format) - * to the end of the dynamic buffer pointed by d_buf - * WARNING: The byte order depends on the endianess of your platform - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackUInt32( DynBuffer *d_buf, uint32_t val ); - -/*! \brief Append a 64 bit unsigned integer (in binary format) - * to the end of the dynamic buffer pointed by d_buf - * WARNING: The byte order depends on the endianess of your platform - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackUInt64( DynBuffer *d_buf, uint64_t val ); - -/*! \brief Append a single precision floating point value (in binary format) - * to the end of the dynamic buffer pointed by d_buf - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackFloat32( DynBuffer *d_buf, float val ); - -/*! \brief Append a double precision floating point value (in binary format) - * to the end of the dynamic buffer pointed by d_buf - * - * \param d_buf Pointer to a DynBuffer object - * \param val The value to be appended - * - * \return The new dynamic bufer size, or -1 on failure - */ -int dynBufferPushBackFloat64( DynBuffer *d_buf, double val ); - -/*! \brief Clear a dynamic buffer (the internal memory IS NOT released) - * - * \param d_buf Pointer to a DynBuffer object - */ -void dynBufferClear( DynBuffer *d_buf ); - -/*! \brief Get the current dynamic buffer size - * - * \param d_buf Pointer to a DynBuffer object - * - * \return The current dynamic buffer size - */ -size_t dynBufferGetSize( DynBuffer *d_buf ); - -/*! \brief Get a const pointer to the internal buffer data - * - * \param d_buf Pointer to a DynBuffer object - * - * \return The const pointer to the internal buffer data - */ -const unsigned char *dynBufferGetData( DynBuffer *d_buf ); - -/*! \brief Move the position indicator, adding offset to the current position - * The position indicator may be exploited to mark alreadey "used" bytes - * - * \param d_buf Pointer to a DynBuffer object - * \param offset Offset in bytes added to the position indicator - */ -void dynBufferMovePoseIndicator( DynBuffer *d_buf, int offset ); - -/*! \brief Move the position indicator, setting the current position - * The position indicator may be exploited to mark alreadey "used" bytes - * - * \param d_str Pointer to a DynBuffer object - * \param pos The new the position indicator (in bytes) - */ -void dynBufferSetPoseIndicator( DynBuffer *d_buf, size_t pos ); - -/*! \brief Reset the position indicator - * The position indicator may be exploited to mark alreadey "used" bytes - * - * \param d_buf Pointer to a DynBuffer object - */ -void dynBufferRewindPoseIndicator( DynBuffer *d_buf ); - -/*! \brief Get the current offset of the position indicator - * - * \param d_buf Pointer to a DynBuffer object - - * \return Offset in bytes of the position indicator - */ -size_t dynBufferGetPoseIndicatorOffset( DynBuffer *d_buf ); - -/*! \brief Get a const pointer to the internal buffer data, starting from the current position indicator - * - * \param d_buf Pointer to a DynBuffer object - * - * \return The const pointer to the internal buffer data - */ -const unsigned char *dynBufferGetCurrentData( DynBuffer *d_buf ); - -/*! \brief Copy data from the dynamic buffer starting from current position indicator to a buffer. - * - * If the amount of data to copy is larger than the available data in the dynamic buffer, an error is returned and - * nothing is done. - * \param d_buf Pointer to a DynBuffer object - * \param cont_buf Pointer to the buffer where the data is copied - * \param cont_buf_len Length of the data to copy - * - * \return 0 on success, or -1 on failure - */ -int dynBufferGetCurrentContent ( unsigned char *cont_buf, DynBuffer *d_buf, size_t cont_buf_len ); - -/*! \brief Get the remaining dynamic buffer size, starting from the current position indicator - * - * \param d_buf Pointer to a DynBuffer object - * - * \return The remaining dynamic buffer size - */ -size_t dynBufferGetRemainingDataSize( DynBuffer *d_buf ); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/dyn_string.h b/src/hal/components/cros/include/dyn_string.h deleted file mode 100644 index 82fc3363..00000000 --- a/src/hal/components/cros/include/dyn_string.h +++ /dev/null @@ -1,165 +0,0 @@ -#ifndef _DYN_STRING_H_ -#define _DYN_STRING_H_ - -/*! \defgroup dyn_string Dynamic string */ - -/*! \addtogroup dyn_string - * @{ - */ - -/*! \brief Dynamic string object. Don't modify its internal members: use - * the related functions instead */ -typedef struct DynString DynString; -struct DynString -{ - int len; //! Current string length not including the \0 character at the end of the string - int pos_offset; //! Movable current string position indicator (cursor) that indicates the start of a last string chunk - int max; //! Max string size not including the space for \0 character at the end of the string - char *data; //! Pointer to the \0-terminated string data -}; - -/*! \brief Initialize a dynamic string - * - * \param d_str Pointer to a DynString object to be initialized - */ -void dynStringInit( DynString *d_str ); - -/*! \brief Release a dynamic string - * \param d_str Pointer to a DynString object to be released - */ -void dynStringRelease( DynString *d_str ); - -/*! \brief Append a copy of the string pointed by new_str to the end of the dynamic string pointed by d_str - * - * \param d_str Pointer to a DynString object - * \param new_str Pointer to the new string to be appended - * - * \return The current dynamic string length, not including the terminating null byte, or -1 on failure - */ -int dynStringPushBackStr( DynString *d_str, const char *new_str ); - -/*! \brief Append a copy of the first n characters of the - * string pointed by new_str to the end of the dynamic string pointed by d_str - * - * \param d_str Pointer to a DynString object - * \param new_str Pointer to the new string to be appended - * \param n Number of characters to be copied - * - * \return The current dynamic string length, not including the terminating null byte, or -1 on failure - */ -int dynStringPushBackStrN( DynString *d_str, const char *new_str, int n ); - -/*! \brief Replace the content of the dynamic string pointed by d_str by the first n characters of the - * character array pointed by new_str - * - * \param d_str Pointer to a DynString object - * \param new_str Pointer to the string that will be the new d_str content - * \param n Number of characters of the inserted in d_str - * - * \return The current dynamic string length, not including the terminating null byte, or -1 on failure - */ -int dynStringReplaceWithStrN ( DynString *d_str, const char *new_str, int n ); - -/*! \brief Append a character to the end of the dynamic string pointed by d_str - * - * \param d_str Pointer to a DynString object - * \param c The character to be appended - * - * \return The current dynamic string length, not including the terminating null byte, or -1 on failure - */ -int dynStringPushBackChar( DynString *d_str, const char c ); - -/*! \brief Copy the string pointed by new_str (not including the terminating null byte) - * inside the dynamic string pointed by d_str, starting from the position pos - * - * \param d_str Pointer to a DynString object - * \param new_str Pointer to the string to be copied - * \param pos Starting position for the copy, must be smaller than the current dynamic string length - * - * \return The new dynamic string length, not including the terminating null byte, or -1 on failure - */ -int dynStringPatch( DynString *d_str, const char *new_str, int pos ); - -/*! \brief Clear a dynamic string (the internal memory IS NOT released) - * - * \param d_str Pointer to a DynString object - */ -void dynStringClear( DynString *d_str ); - -/*! \brief Truncates a DynString object. If more characters than available are tried to be removed, the string just becomes empty. - * The offset index of the DynString is moved left the number of positions removed from left up to pointing to 0. If - * characters are removed from the right, the offset is adjusted to limit its right position to the \0 character at most. - * - * \param d_str Pointer to a DynString object - * \param rem_left Number of character to remove from the beggning of the string - * \param rem_right Number of character to remove from the end of the string - */ -void dynStringReduce ( DynString *d_str, int rem_left, int rem_right); - -/*! \brief Get the current dynamic string length, not including the terminating null byte - * - * \param d_str Pointer to a DynString object - * - * \return The new dynamic string length, not including the terminating null byte - */ -int dynStringGetLen( DynString *d_str ); - -/*! \brief Get a const pointer to the internal string data - * - * \param d_str Pointer to a DynString object - * - * \return The const pointer to the internal string data - */ -const char *dynStringGetData( DynString *d_str ); - -/*! \brief Move the position indicator, adding offset to the current position - * The position indicator may be exploited to mark already "used" characters - * - * \param d_str Pointer to a DynString object - * \param offset Offset in bytes added to the position indicator - */ -void dynStringMovePoseIndicator( DynString *d_str, int offset ); - -/*! \brief Move the position indicator, setting the current position - * The position indicator may be exploited to mark already "used" characters - * - * \param d_str Pointer to a DynString object - * \param pos The new the position indicator (in bytes) - */ -void dynStringSetPoseIndicator( DynString *d_str, int pos ); - -/*! \brief Reset the position indicator - * The position indicator may be exploited to mark already "used" characters - * - * \param d_str Pointer to a DynString object - */ -void dynStringRewindPoseIndicator( DynString *d_str ); - -/*! \brief Get the current offset of the position indicator - * - * \param d_str Pointer to a DynString object - - * \return Offset in bytes of the position indicator - */ -int dynStringGetPoseIndicatorOffset( DynString *d_str ); - -/*! \brief Get a const pointer to the internal string data, starting from the current position indicator - * - * \param d_buf Pointer to a DynString object - * - * \return The const pointer to the internal buffer data - */ -const char *dynStringGetCurrentData( DynString *d_str ); - -/*! \brief Get the remaining dynamic string length starting from the current position indicator, - * not including the terminating null byte - * - * \param d_str Pointer to a DynString object - * - * \return The remaining dynamic string length - */ -int dynStringGetRemainingDataSize( DynString *d_str ); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/md5.h b/src/hal/components/cros/include/md5.h deleted file mode 100644 index 7faca7d9..00000000 --- a/src/hal/components/cros/include/md5.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * This is an OpenSSL-compatible implementation of the RSA Data Security, Inc. - * MD5 Message-Digest Algorithm (RFC 1321). - * - * Homepage: - * http://openwall.info/wiki/people/solar/software/public-domain-source-code/md5 - * - * Author: - * Alexander Peslyak, better known as Solar Designer - * - * This software was written by Alexander Peslyak in 2001. No copyright is - * claimed, and the software is hereby placed in the public domain. - * In case this attempt to disclaim copyright and place the software in the - * public domain is deemed null and void, then the software is - * Copyright (c) 2001 Alexander Peslyak and it is hereby released to the - * general public under the following terms: - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted. - * - * There's ABSOLUTELY NO WARRANTY, express or implied. - * - * See md5.c for more information. - */ - -#ifndef _MD5_H -#define _MD5_H - -/* Any 32-bit or wider unsigned integer data type will do */ -typedef unsigned int MD5_u32plus; - -typedef struct { - MD5_u32plus lo, hi; - MD5_u32plus a, b, c, d; - unsigned char buffer[64]; - MD5_u32plus block[16]; -} MD5_CTX; - -extern void MD5_Init(MD5_CTX *ctx); -extern void MD5_Update(MD5_CTX *ctx, const void *data, unsigned long size); -extern void MD5_Final(unsigned char *result, MD5_CTX *ctx); - -#endif diff --git a/src/hal/components/cros/include/tcpip_socket.h b/src/hal/components/cros/include/tcpip_socket.h deleted file mode 100644 index 0985c85a..00000000 --- a/src/hal/components/cros/include/tcpip_socket.h +++ /dev/null @@ -1,304 +0,0 @@ -#ifndef _TCPIP_SOCKET_H_ -#define _TCPIP_SOCKET_H_ - -// _WIN32 is defined when compiling 32 bit and 64 bit applications, so _WIN64 does not have to be checked here -#ifdef _WIN32 -# include -# define FN_SOCKET_ERROR SOCKET_ERROR -# define FN_INVALID_SOCKET INVALID_SOCKET - -# define FN_ERROR_INVALID_PARAMETER ERROR_INVALID_PARAMETER -# define MAX_HOST_NAME_LEN 256 -#else -# include -# include -# ifndef __USE_POSIX -# define __USE_POSIX -# endif -# include -# define MAX_HOST_NAME_LEN _POSIX_HOST_NAME_MAX -# define FN_SOCKET_ERROR (-1) -# define FN_INVALID_SOCKET (-1) - -# define FN_ERROR_INVALID_PARAMETER ENOSPC -# define MAX_HOST_NAME_LEN _POSIX_HOST_NAME_MAX -#endif - -#include "dyn_string.h" -#include "dyn_buffer.h" - -/*! \defgroup tcpip_socket TcpIp socket */ - -/*! \addtogroup tcpip_socket - * @{ - */ - -typedef enum -{ - TCPIPSOCKET_FAILED = 0, - TCPIPSOCKET_IN_PROGRESS, - TCPIPSOCKET_DISCONNECTED, - TCPIPSOCKET_DONE, - - TCPIPSOCKET_UNKNOWN, - TCPIPSOCKET_REFUSED -} TcpIpSocketState; - -/*! \brief TcpIpSocket object. Don't modify directly its internal members: use - * the related functions instead */ -typedef struct TcpIpSocket TcpIpSocket; -struct TcpIpSocket -{ - int fd; //! File descriptor for the new socket - struct sockaddr_in rem_addr; //! Adress of the (remote) host to which this socket is connected or address to which the socket is binded (if it is listening) - unsigned char open; //! It is 1 if the socket has been already created (successful socket() funciton call). Otherwise it is 0 - unsigned char connected; //! It is 1 if the socket is connected (inbound or outbound). Otherwise it is 0 - unsigned char listening; //! It is 1 if the socket is already in the listening state (ready to accept connections). Otherwise it is 0 - unsigned char is_nonblocking; //! It is 1 if the socket has been configured as non blocking. Otherwise it is 0 -}; - -/*! \brief Initialize the TcpIpSocket object with default values - * - * \param s Pointer to a TcpIpSocket object - */ -void tcpIpSocketInit( TcpIpSocket *s ); - -/*! \brief Open a TCP/IP4 socket - * - * \param s Pointer to a TcpIpSocket object - * - * \return Returns 1 on success, 0 on failure - */ -int tcpIpSocketOpen( TcpIpSocket *s ); - -/*! \brief Close a socket - * - * \param s Pointer to a TcpIpSocket object - * - * \return Returns 1 on success, 0 on failure - */ -int tcpIpSocketClose( TcpIpSocket *s ); - -/*! \brief Set non-blocking operation for a TCP/IP4 socket - * - * \param s Pointer to a TcpIpSocket object - * - * \return Returns 1 on success, 0 on failure - */ -int tcpIpSocketSetNonBlocking( TcpIpSocket *s ); - -/*! \brief Set no-delay operation for a socket, that is, disables the Nagle's algorithm. - * This option is intended for applications that require low latency on every packet sent - * - * \param s Pointer to a TcpIpSocket object - * - * \return Returns 1 on success, 0 on failure - */ -int tcpIpSocketSetNoDelay ( TcpIpSocket *s ); - -/*! \brief Set a TCP/IP4 socket to be re-bound immediately without timeout - * - * \param s Pointer to a TcpIpSocket object - * - * \return Returns 1 on success, 0 on failure - */ -int tcpIpSocketSetReuse ( TcpIpSocket *s ); - -/*! \brief Set a TCP/IP4 socket to prevent disconnection - * - * \param s Pointer to a TcpIpSocket object - * \param idle The interval between the last data packet sent and the first keepalive probe - * \param interval The interval between subsequential keepalive probes - * \param count The number of unacknowledged probes to send before considering the connection dead - * - * \return Returns 1 on success, 0 on failure - */ -int tcpIpSocketSetKeepAlive( TcpIpSocket *s, unsigned int idle, unsigned int interval, unsigned int count ); - -/*! \brief Connect a TCP/IP4 socket to a server - * - * \param s Pointer to a TcpIpSocket object - * \param host The server address - * \param port The server port - - * \return Returns TCPIPSOCKET_DONE on success, - * TCPIPSOCKET_IN_PROGRESS if the connection is not yet completed, - * or TCPIPSOCKET_FAILED on failure - */ -TcpIpSocketState tcpIpSocketConnect( TcpIpSocket *s, const char *host, unsigned short port ); - -/*! \brief Checks if a network port is open is a host address. - * - * This function tries to connect to a target port and reports the success. If the connection - * is established it is closed immediately. - * \param host_addr The target address - * \param host_port The target port - - * \return Returns TCPIPSOCKET_DONE if the port is open, - * TCPIPSOCKET_REFUSED if the connection was refused (e.g. the target port is closed), - * or TCPIPSOCKET_FAILED on failure - */ -TcpIpSocketState tcpIpSocketCheckPort ( const char *host_addr, unsigned short host_port ); - -/*! \brief Bind and listen for TCP/IP4 socket connections - * - * \param s Pointer to a TcpIpSocket object - * \param host The address to be bound to the socket - * \param port The port to be bound to the socket - * \param backlog the maximum length of the listen queue - * - * \return Returns 1 on success, 0 on failure - */ -int tcpIpSocketBindListen( TcpIpSocket *s, const char *host, unsigned short port, int backlog ); - -/*! \brief Accept a new connection on a TCP/IP4 socket - * - * \param s Pointer to a TcpIpSocket object used to accept new connection - * (it should be opened and listening) - * \param new_s Pointer to a TcpIpSocket object used to return the accepted connection - * - * \return Returns TCPIPSOCKET_DONE on success, - * TCPIPSOCKET_IN_PROGRESS if the connection is not yet completed, - * or TCPIPSOCKET_FAILED on failure - */ -TcpIpSocketState tcpIpSocketAccept( TcpIpSocket *s, TcpIpSocket *new_s ); - -/*! \brief Shutdown a connectd TCP/IP4 socket to a server - * - * \param s Pointer to a TcpIpSocket object - * - * \return Returns 1 on success, 0 on failure - */ -int tcpIpSocketDisconnect( TcpIpSocket *s ); - -/*! \brief Send a binary message on a connected socket - * - * \param s Pointer to a TcpIpSocket object - * \param d_buf The dynamic buffer to be written - * - * \return Returns TCPIPSOCKET_DONE on success, - * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) - * if the write operation is not yet completed, - * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, - * or TCPIPSOCKET_FAILED on failure - */ -TcpIpSocketState tcpIpSocketWriteBuffer( TcpIpSocket *s, DynBuffer *d_buf ); - -/*! \brief Send a string on a connected socket - * - * \param s Pointer to a TcpIpSocket object - * \param d_str The dynamic string to be written - * - * \return Returns TCPIPSOCKET_DONE on success, - * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) - * if the write operation is not yet completed, - * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, - * or TCPIPSOCKET_FAILED on failure - */ -TcpIpSocketState tcpIpSocketWriteString( TcpIpSocket *s, DynString *d_str ); - -/*! \brief Receive a binary message from a connected socket - * - * \param s Pointer to a TcpIpSocket object - * \param d_buf Pointer to the input dynamic string - * - * \return Returns TCPIPSOCKET_DONE on success, - * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) - * if the read operation would block, - * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, - * or TCPIPSOCKET_FAILED on failure - */ -TcpIpSocketState tcpIpSocketReadBufferEx( TcpIpSocket *s, DynBuffer *d_buf, size_t length, size_t *reads); - -/*! \brief Receive a binary message from a connected socket - * - * \param s Pointer to a TcpIpSocket object - * \param d_buf Pointer to the input dynamic string - * - * \return Returns TCPIPSOCKET_DONE on success, - * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) - * if the read operation would block, - * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, - * or TCPIPSOCKET_FAILED on failure - */ -TcpIpSocketState tcpIpSocketReadBuffer( TcpIpSocket *s, DynBuffer *d_buf ); - -/*! \brief Receive a string from a connected socket - * - * \param s Pointer to a TcpIpSocket object - * \param d_str Pointer to the input dynamic string - * - * \return Returns TCPIPSOCKET_DONE on success, - * TCPIPSOCKET_IN_PROGRESS (only if the socket is non-blocking) - * if the read operation would block, - * TCPIPSOCKET_DISCONNECTED if the socket has been disconnectd, - * or TCPIPSOCKET_FAILED on failure - */ -TcpIpSocketState tcpIpSocketReadString( TcpIpSocket *s, DynString *d_str ); - -/*! \brief Return the file descriptor associated with the TcpIpSocket object - * - * \param s A TcpIpSocket object - * - * \return Returns the file descriptor - */ -int tcpIpSocketGetFD( TcpIpSocket *s ); - -/*! \brief Return the current (local) port to which the TcpIpSocket object is associated. - * - * \param s A TcpIpSocket object - * - * \return Returns the TCP/IP port number. - */ -unsigned short tcpIpSocketGetPort( TcpIpSocket *s ); - -/*! \brief Return the (remote) port to which the TcpIpSocket object is connected, or the - * port to which the socket is binded in case of being a listening port. - * - * \param s A TcpIpSocket object. - * - * \return Returns the TCP/IP port number. - */ -unsigned short tcpIpSocketGetRemotePort( TcpIpSocket *s ); - -/*! \brief Return the (remote) address to which the TcpIpSocket object is connected, or the - * address to which the socket is binded in case of being a listening port. - * - * \param s A TcpIpSocket object. - * - * \return Returns a pointer to the address string or NULL if the address string could not be obtained. - */ -const char *tcpIpSocketGetRemoteAddress( TcpIpSocket *s ); - -/*! \brief Check several file descriptors simultaneously waiting until at least one of them is ready - * for reading, writing or atteding an exceptional condition. That is, the select() function is called. - * - * \return Returns the error code - */ -int tcpIpSocketSelect( int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds, uint64_t time_out ); - -/*! \brief Initiate the use of the TcpIpSocket library. - * This function must be called when before using the tcpIpSocketStartUp library functions. - * - * \return Returns 0 on success or an error code in case of failure. - */ -int tcpIpSocketStartUp( void ); - -/*! \brief Terminate the use of the TcpIpSocket library. - * This function must be called when the application called tcpIpSocketStartUp and it does not need to use the library anymore to free resources. - * - * \return Returns 0 on success or FN_SOCKET_ERROR in case of failure. - */ -int tcpIpSocketCleanUp( void ); - -/*! \brief Get the error code returned by the last socket-function call that failed. - * So this code is significant only when the return value of the call indicated an error. - * - * \return Returns the number of file descriptors that this function has set sets in the fd_set variables or -1 on error. - */ -int tcpIpSocketGetError( void ); - - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/tcpros_process.h b/src/hal/components/cros/include/tcpros_process.h deleted file mode 100644 index e3866159..00000000 --- a/src/hal/components/cros/include/tcpros_process.h +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef _TCPROS_PROCESS_H_ -#define _TCPROS_PROCESS_H_ - -#include "tcpip_socket.h" - -/*! \defgroup tcpros_process TCPROS process */ - -/*! \addtogroup tcpros_process - * @{ - */ - -typedef enum -{ - TCPROS_PROCESS_STATE_IDLE, // 0 - TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING, // 1 - TCPROS_PROCESS_STATE_CONNECTING, // 2 - TCPROS_PROCESS_STATE_READING_HEADER_SIZE, // 3 - TCPROS_PROCESS_STATE_READING_HEADER, // 4 - TCPROS_PROCESS_STATE_WRITING_HEADER, // 5 - TCPROS_PROCESS_STATE_WAIT_FOR_WRITING, // 6 - TCPROS_PROCESS_STATE_START_WRITING, // 7 - TCPROS_PROCESS_STATE_READING_SIZE, // 8 - TCPROS_PROCESS_STATE_READING, // 9 - TCPROS_PROCESS_STATE_WRITING // A -} TcprosProcessState; - -/*! \brief The TcprosProcess object represents a client or server connection used to manage - * peer to peer TCPROS connections between nodes. It is internally used to emulate the - * "process descriptor" in a multi-task system (here used in a mono task system), including - * the process file descriptors (i.e., a socket), process memory and the state. - * NOTE: this is a cROS internal object, usually you don't need to use it. - */ -typedef struct TcprosProcess TcprosProcess; -struct TcprosProcess -{ - TcprosProcessState state; //! The state of the process. When it is TCPROS_PROCESS_STATE_WAIT_FOR_WRITING the publisher/caller waits until a new message must be sent (periodic or immediate sending) - TcpIpSocket socket; //! The socket used for the TCPROS or RPCROS communication - DynString topic; //! The name of the topic - DynString service; //! The name of the service - DynString type; //! The message/service type - DynString servicerequest_type; //! The service request type - DynString serviceresponse_type; //! The service response type - DynString md5sum; //! The MD5 sum of the message type - DynString caller_id; //! The name of subscriber or service caller - unsigned char latching; //! If 1, the publisher is sending latched messages. Otherwise 0 - unsigned char tcp_nodelay; //! If 1, the publisher should set TCP_NODELAY on the socket, if possible. Otherwise 0 - unsigned char persistent; //! If 1, the service connection should be kept open for multiple requests. Otherwise it should be 0 - DynBuffer packet; //! The incoming/outgoing TCPROS packet - uint64_t last_change_time; //! Last state change time (in ms) - int topic_idx; //! Index used to associate the process to a publisher or a subscriber - int service_idx; //! Index used to associate the process to a service provider or a service client - size_t left_to_recv; //! Remaining to receive - uint8_t ok_byte; //! 'ok' byte send by a service provider in response to the last service request - int probe; //! The current session is a probing one - int sub_tcpros_port; //! Port (obtained from a publisher node) to which the process must connect - char *sub_tcpros_host; //! Host (obtained from a publisher node) to which the process must connect -}; - - -/*! \brief Initialize an TcprosProcess object, starting to allocate memory - * and settings default values for the objects' fields - * - * \param s Pointer to TcprosProcess object to be initialized - */ -void tcprosProcessInit( TcprosProcess *p ); - -/*! \brief Release all the internally allocated memory of an TcprosProcess object - * - * \param s Pointer to TcprosProcess object to be released - */ -void tcprosProcessRelease( TcprosProcess *p ); - -/*! \brief Clear internal i/o data buffer (packet) of a TcprosProcess object - * - * \param s Pointer to TcprosProcess object - */ -void tcprosProcessClear( TcprosProcess *p ); - -/*! \brief Reset the state and clear all the content of a TcprosProcess object (the - * internal memory IS NOT released, so we can continue using the process) - * - * \param s Pointer to TcprosProcess object - */ -void tcprosProcessReset( TcprosProcess *p ); - -/*! \brief Change the internal state of an TcprosProcess object, and update its timer - * - * \param s Pointer to TcprosProcess object - * \param state The new state - */ -void tcprosProcessChangeState( TcprosProcess *p, TcprosProcessState state ); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/tcpros_tags.h b/src/hal/components/cros/include/tcpros_tags.h deleted file mode 100644 index bd27b0e7..00000000 --- a/src/hal/components/cros/include/tcpros_tags.h +++ /dev/null @@ -1,99 +0,0 @@ -#ifndef _TCPROS_TAGS_H_ -#define _TCPROS_TAGS_H_ - -/*! \defgroup tcpros_tags TCPROS tags */ - -/*! \addtogroup tcpros_tags - * @{ - */ - -/*! \brief Structure that contains a TCPROS tag string and the related string length - */ -typedef struct -{ - char *str; - int dim; -}TcprosTagStrDim; - -static TcprosTagStrDim TCPROS_CALLERID_TAG = { "callerid=", 9 }; -static TcprosTagStrDim TCPROS_TOPIC_TAG = { "topic=", 6 }; -static TcprosTagStrDim TCPROS_TYPE_TAG = { "type=", 5 }; -static TcprosTagStrDim TCPROS_MD5SUM_TAG = { "md5sum=", 7 }; -static TcprosTagStrDim TCPROS_MESSAGE_DEFINITION_TAG = { "message_definition=", 19 }; -static TcprosTagStrDim TCPROS_SERVICE_TAG = { "service=", 8 }; -static TcprosTagStrDim TCPROS_SERVICE_REQUESTTYPE_TAG = { "request_type=", 13 }; -static TcprosTagStrDim TCPROS_SERVICE_RESPONSETYPE_TAG = { "response_type=", 14 }; -static TcprosTagStrDim TCPROS_TCP_NODELAY_TAG = { "tcp_nodelay=", 12 }; -static TcprosTagStrDim TCPROS_LATCHING_TAG = { "latching=", 9 }; // WARNING Not implemented -static TcprosTagStrDim TCPROS_PERSISTENT_TAG = { "persistent=", 11 }; // WARNING Not implemented -static TcprosTagStrDim TCPROS_PROBE_TAG = { "probe=", 6 }; -static TcprosTagStrDim TCPROS_ERROR_TAG = { "error=", 6 }; -static TcprosTagStrDim TCPROS_EMPTY_MD5SUM_TAG = { "md5sum=*", 8 }; - -enum -{ - TCPROS_CALLER_ID_FLAG = 0x1, - TCPROS_TOPIC_FLAG = 0x2, - TCPROS_TYPE_FLAG = 0x4, - TCPROS_MD5SUM_FLAG = 0x8, - TCPROS_MESSAGE_DEFINITION_FLAG = 0x10, - TCPROS_SERVICE_FLAG = 0x20, - TCPROS_TCP_NODELAY_FLAG = 0x40, - TCPROS_LATCHING_FLAG = 0x80, - TCPROS_PERSISTENT_FLAG = 0x100, - TCPROS_ERROR_FLAG = 0x200, - TCPROS_DATA_FLAG = 0x400, - TCPROS_PROBE_FLAG = 0x800, - TCPROS_EMPTY_MD5SUM_FLAG = 0x1000, - TCPROS_SERVICE_REQUESTTYPE_FLAG = 0x2000, - TCPROS_SERVICE_RESPONSETYPE_FLAG = 0x4000 -}; - -// http://wiki.ros.org/ROS/TCPROS mentions message_definition as compulsory but -// it's not sent by roscpp subscribers -static const uint32_t TCPROS_SUBCRIPTION_HEADER_FLAGS = // TCPROS_MESSAGE_DEFINITION_FLAG | - TCPROS_CALLER_ID_FLAG | - TCPROS_TOPIC_FLAG | - TCPROS_MD5SUM_FLAG | - TCPROS_TYPE_FLAG; - -// http://wiki.ros.org/ROS/TCPROS doesn't mention message_definition, caller_id -// or the topic as compulsory but they are sent by roscpp publishers -static const uint32_t TCPROS_PUBLICATION_HEADER_FLAGS = TCPROS_TYPE_FLAG | - TCPROS_MD5SUM_FLAG; -/* -static const uint32_t TCPROS_PUBLICATION_PACKET_FLAGS = TCPROS_CALLER_ID_FLAG | - TCPROS_TOPIC_FLAG | - TCPROS_TYPE_FLAG | - TCPROS_MD5SUM_FLAG | - TCPROS_MESSAGE_DEFINITION_FLAG | - TCPROS_DATA_FLAG; -*/ -static const uint32_t TCPROS_SERVICECALL_HEADER_FLAGS = TCPROS_CALLER_ID_FLAG | - TCPROS_SERVICE_FLAG | - TCPROS_MD5SUM_FLAG | - TCPROS_PERSISTENT_FLAG; - -static const uint32_t TCPROS_SERVICECALL_MATLAB_HEADER_FLAGS = TCPROS_CALLER_ID_FLAG | - TCPROS_TYPE_FLAG | - TCPROS_MESSAGE_DEFINITION_FLAG | - TCPROS_SERVICE_FLAG | - TCPROS_MD5SUM_FLAG | - TCPROS_TCP_NODELAY_FLAG | - TCPROS_PERSISTENT_FLAG; - -static const uint32_t TCPROS_SERVICEPROBE_HEADER_FLAGS = TCPROS_CALLER_ID_FLAG | - TCPROS_SERVICE_FLAG | - TCPROS_PROBE_FLAG | - TCPROS_EMPTY_MD5SUM_FLAG; - -static const uint32_t TCPROS_SERVICEPROBE_MATLAB_HEADER_FLAGS = TCPROS_CALLER_ID_FLAG | - TCPROS_TYPE_FLAG | - TCPROS_SERVICE_FLAG | - TCPROS_EMPTY_MD5SUM_FLAG; - -static const uint32_t TCPROS_SERVICEPROVISION_HEADER_FLAGS = TCPROS_TYPE_FLAG | - TCPROS_MD5SUM_FLAG; -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/xmlrpc_params.h b/src/hal/components/cros/include/xmlrpc_params.h deleted file mode 100644 index bc8ba0e1..00000000 --- a/src/hal/components/cros/include/xmlrpc_params.h +++ /dev/null @@ -1,280 +0,0 @@ -#ifndef _XMLRPC_PARAMS_H_ -#define _XMLRPC_PARAMS_H_ - -#include -#include "dyn_string.h" - -/*! \defgroup xmlrpc_param XMLRPC parameters */ - -/*! \addtogroup xmlrpc_param - * @{ - */ - -typedef enum -{ - XMLRPC_PARAM_UNKNOWN = 0, - XMLRPC_PARAM_BOOL, - XMLRPC_PARAM_INT, - XMLRPC_PARAM_DOUBLE, - XMLRPC_PARAM_STRING, - XMLRPC_PARAM_ARRAY, - XMLRPC_PARAM_DATETIME, /* WARNING: Currently unsupported */ - XMLRPC_PARAM_BINARY, /* WARNING: Currently unsupported */ - XMLRPC_PARAM_STRUCT -}XmlrpcParamType; - -/*! \brief Struct used to store a input/oputput XMLRPC param. - * To modify its internal members, you could use the related functions - */ -typedef struct XmlrpcParam XmlrpcParam; - -struct XmlrpcParam -{ - XmlrpcParamType type; //! Param type - char *member_name; - union - { - uint8_t opaque[8]; - int as_bool; - int32_t as_int; - double as_double; - char *as_string; - XmlrpcParam *as_array; // or struct - void* as_time; /* WARNING: Currently unsupported */ - void* as_binary; /* WARNING: Currently unsupported */ - } data; //! Param data - int array_n_elem; //! Used only if type is XMLRPC_PARAM_ARRAY: it stores the array size - int array_max_elem; //! Used only if type is XMLRPC_PARAM_ARRAY: it stores the current max size -}; - -/*! \brief Return an XMLRPC parameter as a boolena value (i.e., an - * unisigned char value either 0 : false or 1 : true ). - * No type control are performed. If the XMLRPC parameter - * is not boolean, return value is undefined - * - * \param param Pointer to a XMLRPC parameter - * - * \return An integer value ( 0 : false, 1 : true ) - */ -int xmlrpcParamGetBool( XmlrpcParam *param ); - -/*! \brief Return an XMLRPC parameter as a integer value. - * No type control are performed. If the XMLRPC parameter - * is not integer, return value is undefined - * - * \param param Pointer to a XMLRPC parameter - * - * \return The integer value - */ -int32_t xmlrpcParamGetInt( XmlrpcParam *param ); - -/*! \brief Return an XMLRPC parameter as a double value. - * No type control are performed. If the XMLRPC parameter - * is not double, return value is undefined - * - * \param param Pointer to a XMLRPC parameter - * - * \return The double value - */ -double xmlrpcParamGetDouble( XmlrpcParam *param ); - -/*! \brief Return an XMLRPC parameter as a string value. - * No type control are performed. If the XMLRPC parameter - * is not a string, return value is undefined - * - * \param param Pointer to a XMLRPC parameter - * - * \return Pointer to the string, passed as a pointer to the internal memory - */ -char *xmlrpcParamGetString( XmlrpcParam *param ); - -/*! \brief Setup a XMLRPC unknown parameter (e.g., in case of errors) - * - * \param param Pointer to a XMLRPC parameter - */ -void xmlrpcParamSetUnknown( XmlrpcParam *param ); - -/*! \brief Setup a XMLRPC boolean parameter with the given value - * - * \param param Pointer to a XMLRPC parameter - * \param val An integer value (0 : false, true otherwise) - */ -void xmlrpcParamSetBool( XmlrpcParam *param, int val ); - -/*! \brief Setup a XMLRPC integer parameter with the given value - * - * \param param Pointer to a XMLRPC parameter - * \param val An integer value - */ -void xmlrpcParamSetInt( XmlrpcParam *param, int32_t val ); - -/*! \brief Setup a XMLRPC double floating point parameter with the given value - * - * \param param Pointer to a XMLRPC parameter - * \param val An double value - */ -void xmlrpcParamSetDouble( XmlrpcParam *param, double val ); - -/*! \brief Setup a XMLRPC string parameter with the given string. - * The string is copied inside a dynamically allocated memory - * - * \param param Pointer to a XMLRPC parameter - * \param val Pointer to a string - * \return 0 on success, -1 on memory allocation error - */ -int xmlrpcParamSetString( XmlrpcParam *param, const char *val ); - -/*! \brief As xmlrpcParamSetString(), but limits the string length to n characters - * - * \param param Pointer to a XMLRPC parameter - * \param val Pointer to a string - * \param n The string length - * \return 0 on success, -1 on memory allocation error - */ -int xmlrpcParamSetStringN( XmlrpcParam *param, const char *val, int n ); - -/*! \brief Setup an empty array XMLRPC parameter, starting to allocate internal memory - * - * \param param Pointer to a XMLRPC parameter - */ -int xmlrpcParamSetArray( XmlrpcParam *param ); - -/*! \brief Setup an empty struct XMLRPC parameter, starting to allocate internal memory - * - * \param param Pointer to a XMLRPC parameter - */ -int xmlrpcParamSetStruct( XmlrpcParam *param ); - -/*! \brief Return the data type for a given XMLRPC parameter - * - * \param param Pointer to a XMLRPC parameter - * - * \return The data type - */ -XmlrpcParamType xmlrpcParamGetType( XmlrpcParam *param ); - -/*! \brief Return the size of a XMLRPC array parameter - * - * \param param Pointer to a XMLRPC parameter - * - * \return The array size, or -1 if the XMLRPC parameter is not an array - */ -int xmlrpcParamArrayGetSize( XmlrpcParam *param ); - -/*! \brief Return the idx-th element of a XMLRPC array parameter - * - * \param param Pointer to a XMLRPC parameter - * \param idx The element index - * - * \return Pointer to the XMLRPC parameter, or NULL - * if the XMLRPC parameter is not an array or if idx exceeds the array size - */ -XmlrpcParam * xmlrpcParamArrayGetParamAt( XmlrpcParam *param, int idx ); - -/*! \brief Append to an array XMLRPC parameter a bool parameter - * - * \param param Pointer to an array XMLRPC parameter - * \param val An integer value (false if it is equal to 0, true otherwise) - */ -XmlrpcParam * xmlrpcParamArrayPushBackBool( XmlrpcParam *param, int val ); - -/*! \brief Append to an array XMLRPC parameter an integer parameter - * - * \param param Pointer to an array XMLRPC parameter - * \param val An integer value - */ -XmlrpcParam * xmlrpcParamArrayPushBackInt( XmlrpcParam *param, int32_t val ); - -/*! \brief Append to an array XMLRPC parameter a double parameter - * - * \param param Pointer to an array XMLRPC parameter - * \param val An double value - */ -XmlrpcParam * xmlrpcParamArrayPushBackDouble( XmlrpcParam *param, double val ); - -/*! \brief Append to an array XMLRPC parameter a string parameter - * - * \param param Pointer to an array XMLRPC parameter - * \param val Pointer to a string - */ -XmlrpcParam * xmlrpcParamArrayPushBackString( XmlrpcParam *param, const char *val ); - -/*! \brief As xmlrpcParamArrayPushBackString(), but limits the string length to n characters - * - * \param param Pointer to an array XMLRPC parameter - * \param val Pointer to a string - * \param n The string length - */ -XmlrpcParam * xmlrpcParamArrayPushBackStringN( XmlrpcParam *param, const char *val, int n ); - -/*! \brief Append to an array XMLRPC parameter an empty array parameter - * - * \param param Pointer to an array XMLRPC parameter - * - * \return A pointer to the new pushed array XMLRPC parameter, or NULL on failure - */ -XmlrpcParam * xmlrpcParamArrayPushBackArray( XmlrpcParam *param ); - -/*! \brief Append to an array XMLRPC parameter an empty struct parameter - * - * \param param Pointer to an array XMLRPC parameter - * - * \return A pointer to the new pushed struct XMLRPC parameter, or NULL on failure - */ -XmlrpcParam * xmlrpcParamArrayPushBackStruct ( XmlrpcParam *param ); - -XmlrpcParam * xmlrpcParamStructGetParam( XmlrpcParam *param, const char *name ); -XmlrpcParam * xmlrpcParamStructPushBackBool( XmlrpcParam *param, const char *name, int val ); -XmlrpcParam * xmlrpcParamStructPushBackInt( XmlrpcParam *param, const char *name, int32_t val ); -XmlrpcParam * xmlrpcParamStructPushBackDouble( XmlrpcParam *param, const char *name, double val ); -XmlrpcParam * xmlrpcParamStructPushBackString( XmlrpcParam *param, const char *name, const char *val ); -XmlrpcParam * xmlrpcParamStructPushBackStringN( XmlrpcParam *param, const char *name, const char *val, int n ); -XmlrpcParam * xmlrpcParamStructPushBackArray( XmlrpcParam *param, const char *name ); -XmlrpcParam * xmlrpcParamStructPushBackStruct ( XmlrpcParam *param, const char *name ); - -XmlrpcParam * xmlrpcParamNew(void); - -void xmlrpcParamFree( XmlrpcParam *param ); - -void xmlrpcParamInit( XmlrpcParam *param ); - -/*! \brief Release internal data dynamically allocated (e.g., string and arrays) - * - * \param param Pointer to a XMLRPC parameter - */ -void xmlrpcParamRelease( XmlrpcParam *param ); - -/*! \brief Append to a dynamic string the parameters given in input, converted in XML - * - * \param param Pointer to the param to be converted in XML - * \param message Pointer to the dynamic string where the parameter will be appended - */ -void xmlrpcParamToXml( XmlrpcParam *param, DynString *message ); - -/*! \brief Look for a XMLRPC parameter inside a dynamic string, and store in a XmlrpcParam object - * - * \param message Pointer to the dynamic string to be parsed - * \param param Pointer to the output parameter - * - * \return Returns 1 on success, 0 on failure - */ -int xmlrpcParamFromXml( DynString *message, XmlrpcParam *param ); - -/*! \brief Print XMLRPC parameter to stdout in human readable form - * - * \param param Pointer to the output parameter - */ -void xmlrpcParamPrint( XmlrpcParam *param ); - -XmlrpcParam * xmlrpcParamClone( XmlrpcParam *param ); - -int xmlrpcParamCopy(XmlrpcParam *dest, XmlrpcParam *source); - -// Functions for internal library use -static void paramPrint( XmlrpcParam *param, char *head, int is_struct_member); - -static void paramArrayPrint( XmlrpcParam *param, char *head, int is_struct_member); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/xmlrpc_params_vector.h b/src/hal/components/cros/include/xmlrpc_params_vector.h deleted file mode 100644 index a5e8fa84..00000000 --- a/src/hal/components/cros/include/xmlrpc_params_vector.h +++ /dev/null @@ -1,126 +0,0 @@ -#ifndef _XMLRPC_PARAMS_VECTOR_H_ -#define _XMLRPC_PARAMS_VECTOR_H_ - -#include "xmlrpc_params.h" - -/*! \defgroup xmlrpc_param_vector XMLRPC parameters vector */ - -/*! \addtogroup xmlrpc_param_vector - * @{ - */ - -/*! \brief Dynamic vector object for XMLRPC parameters. Don't modify its internal members directly: - * use the related functions instead */ -typedef struct XmlrpcParamVector XmlrpcParamVector; -struct XmlrpcParamVector -{ - int size; //! Current vector size - int max; //! Max vector size - XmlrpcParam *data; //! buffer data -}; - -/*! \brief Initialize a dynamic vector - * - * \param p_vec Pointer to a XmlrpcParamVector object to be initialized - */ -void xmlrpcParamVectorInit( XmlrpcParamVector *p_vec ); - -/*! \brief Release a dynamic vector. It also release all the internal - * data dynamically allocated (e.g., string and arrays) calling - * the xmlrpcParamReleaseData() function - * - * \param p_vec Pointer to a XmlrpcParamVector object to be be released - */ -void xmlrpcParamVectorRelease( XmlrpcParamVector *p_vec ); - -/*! \brief Append a new XMLRPC boolean parameter to the vector. - * - * \param p_vec Pointer to a XmlrpcParamVector object - * \param val An integer value (false if it is equalt to 0, true otherwise) - * - * \return The new dynamic vector size, or -1 on failure - */ -int xmlrpcParamVectorPushBackBool( XmlrpcParamVector *p_vec, int val ); - -/*! \brief Append a new XMLRPC integer parameter to the vector. - * - * \param p_vec Pointer to a XmlrpcParamVector object - * \param val An integer value - * - * \return The new dynamic vector size, or -1 on failure - */ -int xmlrpcParamVectorPushBackInt( XmlrpcParamVector *p_vec, int32_t val ); - -/*! \brief Append a new XMLRPC double floating point parameter to the vector. - * - * \param p_vec Pointer to a XmlrpcParamVector object - * \param val A double value - * - * \return The new dynamic vector size, or -1 on failure - */ -int xmlrpcParamVectorPushBackDouble( XmlrpcParamVector *p_vec, double val ); - -/*! \brief Append a new XMLRPC string parameter to the vector. - * The string is copied inside a dynamically allocated memory - * - * \param p_vec Pointer to a XmlrpcParamVector object - * \param val Pointer to a string - * - * \return The new dynamic vector size, or -1 on failure - */ -int xmlrpcParamVectorPushBackString( XmlrpcParamVector *p_vec, const char *val ); - -/*! \brief Append a new (empty) XMLRPC array parameter to the vector, - * and start to allocate internal memory for the array - * - * \param p_vec Pointer to a XmlrpcParamVector object - * - * \return The new dynamic vector size, or -1 on failure - */ -int xmlrpcParamVectorPushBackArray( XmlrpcParamVector *p_vec ); - -/*! \brief Append a new (empty) XMLRPC struct parameter to the vector, - * and start to allocate internal memory - * - * \param p_vec Pointer to a XmlrpcParamVector object - * - * \return The new dynamic vector size, or -1 on failure - */ -int xmlrpcParamVectorPushBackStruct( XmlrpcParamVector *p_vec ); - -/*! \brief Append a new XMLRPC parameter to the vector. - * Warning: data as string and arrays ARE NOT copied, i.e. only references are copyed. - * - * \param p_vec Pointer to a XmlrpcParamVector object - * \param param Pointer to the XMLRPC parameter to be appended - * - * \return The new dynamic vector size, or -1 on failure - */ -int xmlrpcParamVectorPushBack( XmlrpcParamVector *p_vec, XmlrpcParam *param ); - -/*! \brief Get the current dynamic vector size - * - * \param p_vec Pointer to a XmlrpcParamVector object - * - * \return The current dynamic vector size - */ -int xmlrpcParamVectorGetSize( XmlrpcParamVector *p_vec ); - -/*! \brief Return a reference to a XMLRPC parameter - * - * \param p_vec Pointer to a XmlrpcParamVector object - * \param pos Index of the parameter - * - * \return Pointer to the pos-th XMLRPC parameter - */ -XmlrpcParam *xmlrpcParamVectorAt( XmlrpcParamVector *p_vec, int pos ); - -/*! \brief Print a vector of XMLRPC parameters to stdout in human readable form - * - * \param p_vec Pointer to a XmlrpcParamVector object - */ -void xmlrpcParamVectorPrint( XmlrpcParamVector *p_vec ); - -/*! @}*/ - -#endif \ No newline at end of file diff --git a/src/hal/components/cros/include/xmlrpc_process.h b/src/hal/components/cros/include/xmlrpc_process.h deleted file mode 100644 index 9d35fc86..00000000 --- a/src/hal/components/cros/include/xmlrpc_process.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef _XMLRPC_PROCESS_H_ -#define _XMLRPC_PROCESS_H_ - -#include "tcpip_socket.h" -#include "xmlrpc_protocol.h" -#include "cros_api_call.h" - -/*! \defgroup xmlrpc_process XMLRPC process */ - -/*! \addtogroup xmlrpc_process - * @{ - */ - -typedef enum -{ - XMLRPC_PROCESS_STATE_IDLE, - XMLRPC_PROCESS_STATE_CONNECTING, - XMLRPC_PROCESS_STATE_READING, - XMLRPC_PROCESS_STATE_WRITING -}XmlrpcProcessState; - -/*! \brief The XmlrpcProcess object represents a client or server connection between - * the node and the roscore node or other nodes. It is internally used to emulate the - * "precess descriptor" in a multitask system (here used in a mono task system), including - * the process file descriptors (i.e., a socket), proecess memory and the state. - * NOTE: this is a cROS internal object, usually you don't need to use it. - */ -typedef struct XmlrpcProcess XmlrpcProcess; -struct XmlrpcProcess -{ - RosApiCall *current_call; - XmlrpcProcessState state; //! The state - TcpIpSocket socket; //! The socket used for the XMLRPC communication - XmlrpcMessageType message_type; //! The incoming/outgoing XMLRPC message type - DynString method; //! The incoming/outgoing XMLRPC method - XmlrpcParamVector params; //! The incoming/outgoing XMLRPC response - XmlrpcParamVector response; //! The incoming/outgoing XMLRPC response - /*! The incoming/outgoing XMLRPC message - * (e.g., generated using generateXmlrpcMessage() ) */ - DynString message; - uint64_t last_change_time; //! Last state change time (in ms) - char host[256]; - int port; -}; - - -/*! \brief Initialize an XmlrpcProcess object, starting to allocate memory - * and settins default values for the objects' fields - * - * \param s Pointer to XmlrpcProcess object to be initialized - */ -void xmlrpcProcessInit( XmlrpcProcess *p ); - -/*! \brief Release all the internally allocated memory of an XmlrpcProcess object - * - * \param s Pointer to XmlrpcProcess object to be released - */ -void xmlrpcProcessRelease( XmlrpcProcess *p ); - -/*! \brief Clear internal message buffer of an XmlrpcProcess object - * - * \param s Pointer to XmlrpcProcess object - */ -void xmlrpcProcessClear( XmlrpcProcess *p); - -/*! \brief Reset the state and internal data of an XmlrpcProcess object (the internal memory IS NOT released) - * - * \param s Pointer to XmlrpcProcess object - */ -void xmlrpcProcessReset( XmlrpcProcess *p); - -/*! \brief Change the internal state of an XmlrpcProcess object, and update its timer - * - * \param s Pointer to XmlrpcProcess object - * \param state The new state - */ -void xmlrpcProcessChangeState( XmlrpcProcess *p, XmlrpcProcessState state ); - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/include/xmlrpc_protocol.h b/src/hal/components/cros/include/xmlrpc_protocol.h deleted file mode 100644 index f999dd38..00000000 --- a/src/hal/components/cros/include/xmlrpc_protocol.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef _XMLRPC_PROTOCOL_H_ -#define _XMLRPC_PROTOCOL_H_ - -#include "xmlrpc_params_vector.h" -#include "dyn_string.h" - -/*! \defgroup xmlrpc_protocol XMLRPC protocol */ - -/*! \addtogroup xmlrpc_protocol - * @{ - */ - -typedef enum -{ - XMLRPC_MESSAGE_REQUEST, - XMLRPC_MESSAGE_RESPONSE, - XMLRPC_MESSAGE_UNKNOWN -}XmlrpcMessageType; - -typedef enum -{ - XMLRPC_PARSER_ERROR, - XMLRPC_PARSER_INCOMPLETE, - XMLRPC_PARSER_DONE -}XmlrpcParserState; - - -/*! \brief Generate a XMLRPC over HTTP message and store it into a dynamic string - * - * \param host The hostname where the master runs - * \param port The port where the master runs - * \param type The message type (XMLRPC_MESSAGE_REQUEST or XMLRPC_MESSAGE_RESPONSE ) - * \param method The RPC method to invoke ( used only if type == XMLRPC_MESSAGE_REQUEST ) - * \param params Vector of arguments to the RPC call - * \param message Pointer to the (output) dynamic string that will contain the generated message - */ -void generateXmlrpcMessage( const char*host, unsigned short port, XmlrpcMessageType type, - const char *method, XmlrpcParamVector *params, DynString *message ); - -/*! \brief Parse a XMLRPC over HTTP message - * - * \param message Pointer to the input dynamic string that will contain the message to be parsed - * \param type The message type (XMLRPC_MESSAGE_REQUEST or XMLRPC_MESSAGE_RESPONSE ) - * \param method The RPC method to invoke ( used only if type == XMLRPC_MESSAGE_REQUEST ) - * \param response Output vector of XMLRPC parameters with the set of arguments to the RPC call - * - * \return XMLRPC_PARSER_DONE if the message has ben successfully, - * XMLRPC_PARSER_INCOMPLETE if the message is incomplete, - * XMLRPC_PARSER_ERROR on failure - */ -XmlrpcParserState parseXmlrpcMessage(DynString *message, XmlrpcMessageType *type, - DynString *method, XmlrpcParamVector *response, - char host[256], int *port); - -/*! @}*/ -#endif \ No newline at end of file diff --git a/src/hal/components/cros/include/xmlrpc_tags.h b/src/hal/components/cros/include/xmlrpc_tags.h deleted file mode 100644 index 638289c1..00000000 --- a/src/hal/components/cros/include/xmlrpc_tags.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef _XMLRPC_TAGS_H_ -#define _XMLRPC_TAGS_H_ - -/*! \defgroup xmlrpc_tags XMLRPC tags */ - -/*! \addtogroup xmlrpc_tags - * @{ - */ - -/*! \brief Structure that contains a XMLRPC tag string and the related string length - */ -typedef struct -{ - char *str; - int dim; -}XmlrpcTagStrDim; - -static XmlrpcTagStrDim XMLRPC_VERSION = { "Custom XMLRPC", 13 }; -static XmlrpcTagStrDim XMLRPC_MESSAGE_BEGIN = { "", 21 }; -static XmlrpcTagStrDim XMLRPC_MESSAGE_END = { "", 0 }; -static XmlrpcTagStrDim XMLRPC_REQUEST_BEGIN = { "", 12 }; -static XmlrpcTagStrDim XMLRPC_REQUEST_END = { "", 13 }; -static XmlrpcTagStrDim XMLRPC_METHODNAME_BEGIN = { "", 12 }; -static XmlrpcTagStrDim XMLRPC_METHODNAME_END = { "", 13 }; -static XmlrpcTagStrDim XMLRPC_RESPONSE_BEGIN = { "", 16 }; -static XmlrpcTagStrDim XMLRPC_RESPONSE_END = { "", 17 }; - -static XmlrpcTagStrDim XMLRPC_PARAMS_TAG = { "", 8 }; -static XmlrpcTagStrDim XMLRPC_PARAMS_ETAG = { "", 9 }; -static XmlrpcTagStrDim XMLRPC_PARAM_TAG = { "", 7 }; -static XmlrpcTagStrDim XMLRPC_PARAM_ETAG = { "", 8 }; -static XmlrpcTagStrDim XMLRPC_FAULT_TAG = { "", 7 }; -static XmlrpcTagStrDim XMLRPC_FAULT_ETAG = { "", 8 }; - -static XmlrpcTagStrDim XMLRPC_VALUE_TAG = { "", 7 }; -static XmlrpcTagStrDim XMLRPC_VALUE_ETAG = { "", 8 }; - -static XmlrpcTagStrDim XMLRPC_BOOLEAN_TAG = { "", 9 }; -static XmlrpcTagStrDim XMLRPC_BOOLEAN_ETAG = { "", 10 }; -static XmlrpcTagStrDim XMLRPC_DOUBLE_TAG = { "", 8 }; -static XmlrpcTagStrDim XMLRPC_DOUBLE_ETAG = { "", 9 }; -static XmlrpcTagStrDim XMLRPC_I4_TAG = { "", 4 }; -static XmlrpcTagStrDim XMLRPC_I4_ETAG = { "", 5 }; -static XmlrpcTagStrDim XMLRPC_INT_TAG = { "", 5 }; -static XmlrpcTagStrDim XMLRPC_INT_ETAG = { "", 6 }; -static XmlrpcTagStrDim XMLRPC_STRING_TAG = { "", 8 }; -static XmlrpcTagStrDim XMLRPC_STRING_ETAG = { "", 9 }; -static XmlrpcTagStrDim XMLRPC_DATETIME_TAG = { "", 18 }; -static XmlrpcTagStrDim XMLRPC_DATETIME_ETAG = { "", 19 }; -static XmlrpcTagStrDim XMLRPC_BASE64_TAG = { "", 8 }; -static XmlrpcTagStrDim XMLRPC_BASE64_ETAG = { "", 9 }; -static XmlrpcTagStrDim XMLRPC_ARRAY_TAG = { "", 7 }; -static XmlrpcTagStrDim XMLRPC_ARRAY_ETAG = { "", 8 }; -static XmlrpcTagStrDim XMLRPC_DATA_TAG = { "", 6 }; // start-tag -static XmlrpcTagStrDim XMLRPC_DATA_ETAG = { "", 7 }; // end-tag -static XmlrpcTagStrDim XMLRPC_DATA_NTAG = { "", 7 }; // empty-element tag (no content) -static XmlrpcTagStrDim XMLRPC_STRUCT_TAG = { "", 8 }; -static XmlrpcTagStrDim XMLRPC_STRUCT_ETAG = { "", 9 }; -static XmlrpcTagStrDim XMLRPC_STRUCT_NTAG = { "", 9 }; -static XmlrpcTagStrDim XMLRPC_MEMBER_TAG = { "", 8 }; -static XmlrpcTagStrDim XMLRPC_MEMBER_ETAG = { "", 9 }; -static XmlrpcTagStrDim XMLRPC_NAME_TAG = { "", 6 }; -static XmlrpcTagStrDim XMLRPC_NAME_ETAG = { "", 7 }; - -/*! @}*/ - -#endif diff --git a/src/hal/components/cros/master_python_source/defusedxml/ElementTree.py b/src/hal/components/cros/master_python_source/defusedxml/ElementTree.py deleted file mode 100644 index b1504e4a..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/ElementTree.py +++ /dev/null @@ -1,143 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defused xml.etree.ElementTree facade -""" -from __future__ import print_function, absolute_import - -import sys -import warnings -from xml.etree.ElementTree import TreeBuilder as _TreeBuilder -from xml.etree.ElementTree import parse as _parse -from xml.etree.ElementTree import tostring - -from .common import PY3 - -if PY3: - import importlib -else: - from xml.etree.ElementTree import XMLParser as _XMLParser - from xml.etree.ElementTree import iterparse as _iterparse - from xml.etree.ElementTree import ParseError - - -from .common import ( - DTDForbidden, - EntitiesForbidden, - ExternalReferenceForbidden, - _generate_etree_functions, -) - -__origin__ = "xml.etree.ElementTree" - - -def _get_py3_cls(): - """Python 3.3 hides the pure Python code but defusedxml requires it. - - The code is based on test.support.import_fresh_module(). - """ - pymodname = "xml.etree.ElementTree" - cmodname = "_elementtree" - - pymod = sys.modules.pop(pymodname, None) - cmod = sys.modules.pop(cmodname, None) - - sys.modules[cmodname] = None - pure_pymod = importlib.import_module(pymodname) - if cmod is not None: - sys.modules[cmodname] = cmod - else: - sys.modules.pop(cmodname) - sys.modules[pymodname] = pymod - - _XMLParser = pure_pymod.XMLParser - _iterparse = pure_pymod.iterparse - ParseError = pure_pymod.ParseError - - return _XMLParser, _iterparse, ParseError - - -if PY3: - _XMLParser, _iterparse, ParseError = _get_py3_cls() - -_sentinel = object() - - -class DefusedXMLParser(_XMLParser): - def __init__( - self, - html=_sentinel, - target=None, - encoding=None, - forbid_dtd=False, - forbid_entities=True, - forbid_external=True, - ): - # Python 2.x old style class - _XMLParser.__init__(self, target=target, encoding=encoding) - if html is not _sentinel: - # the 'html' argument has been deprecated and ignored in all - # supported versions of Python. Python 3.8 finally removed it. - if html: - raise TypeError("'html=True' is no longer supported.") - else: - warnings.warn( - "'html' keyword argument is no longer supported. Pass " - "in arguments as keyword arguments.", - category=DeprecationWarning, - ) - - self.forbid_dtd = forbid_dtd - self.forbid_entities = forbid_entities - self.forbid_external = forbid_external - if PY3: - parser = self.parser - else: - parser = self._parser - if self.forbid_dtd: - parser.StartDoctypeDeclHandler = self.defused_start_doctype_decl - if self.forbid_entities: - parser.EntityDeclHandler = self.defused_entity_decl - parser.UnparsedEntityDeclHandler = self.defused_unparsed_entity_decl - if self.forbid_external: - parser.ExternalEntityRefHandler = self.defused_external_entity_ref_handler - - def defused_start_doctype_decl(self, name, sysid, pubid, has_internal_subset): - raise DTDForbidden(name, sysid, pubid) - - def defused_entity_decl( - self, name, is_parameter_entity, value, base, sysid, pubid, notation_name - ): - raise EntitiesForbidden(name, value, base, sysid, pubid, notation_name) - - def defused_unparsed_entity_decl(self, name, base, sysid, pubid, notation_name): - # expat 1.2 - raise EntitiesForbidden(name, None, base, sysid, pubid, notation_name) # pragma: no cover - - def defused_external_entity_ref_handler(self, context, base, sysid, pubid): - raise ExternalReferenceForbidden(context, base, sysid, pubid) - - -# aliases -# XMLParse is a typo, keep it for backwards compatibility -XMLTreeBuilder = XMLParse = XMLParser = DefusedXMLParser - -parse, iterparse, fromstring = _generate_etree_functions( - DefusedXMLParser, _TreeBuilder, _parse, _iterparse -) -XML = fromstring - - -__all__ = [ - "ParseError", - "XML", - "XMLParse", - "XMLParser", - "XMLTreeBuilder", - "fromstring", - "iterparse", - "parse", - "tostring", -] diff --git a/src/hal/components/cros/master_python_source/defusedxml/__init__.py b/src/hal/components/cros/master_python_source/defusedxml/__init__.py deleted file mode 100644 index 6f3b7584..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/__init__.py +++ /dev/null @@ -1,62 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defuse XML bomb denial of service vulnerabilities -""" -from __future__ import print_function, absolute_import - -from .common import ( - DefusedXmlException, - DTDForbidden, - EntitiesForbidden, - ExternalReferenceForbidden, - NotSupportedError, - _apply_defusing, -) - - -def defuse_stdlib(): - """Monkey patch and defuse all stdlib packages - - :warning: The monkey patch is an EXPERIMETNAL feature. - """ - defused = {} - - from . import cElementTree - from . import ElementTree - from . import minidom - from . import pulldom - from . import sax - from . import expatbuilder - from . import expatreader - from . import xmlrpc - - xmlrpc.monkey_patch() - defused[xmlrpc] = None - - for defused_mod in [ - cElementTree, - ElementTree, - minidom, - pulldom, - sax, - expatbuilder, - expatreader, - ]: - stdlib_mod = _apply_defusing(defused_mod) - defused[defused_mod] = stdlib_mod - - return defused - - -__version__ = "0.6.0" - -__all__ = [ - "DefusedXmlException", - "DTDForbidden", - "EntitiesForbidden", - "ExternalReferenceForbidden", - "NotSupportedError", -] diff --git a/src/hal/components/cros/master_python_source/defusedxml/cElementTree.py b/src/hal/components/cros/master_python_source/defusedxml/cElementTree.py deleted file mode 100644 index fdc761ed..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/cElementTree.py +++ /dev/null @@ -1,40 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defused xml.etree.cElementTree -""" -from __future__ import absolute_import - -from xml.etree.cElementTree import TreeBuilder as _TreeBuilder -from xml.etree.cElementTree import parse as _parse -from xml.etree.cElementTree import tostring - -# iterparse from ElementTree! -from xml.etree.ElementTree import iterparse as _iterparse - -from .ElementTree import DefusedXMLParser -from .common import _generate_etree_functions - -__origin__ = "xml.etree.cElementTree" - - -# XMLParse is a typo, keep it for backwards compatibility -XMLTreeBuilder = XMLParse = XMLParser = DefusedXMLParser - -parse, iterparse, fromstring = _generate_etree_functions( - DefusedXMLParser, _TreeBuilder, _parse, _iterparse -) -XML = fromstring - -__all__ = [ - "XML", - "XMLParse", - "XMLParser", - "XMLTreeBuilder", - "fromstring", - "iterparse", - "parse", - "tostring", -] diff --git a/src/hal/components/cros/master_python_source/defusedxml/common.py b/src/hal/components/cros/master_python_source/defusedxml/common.py deleted file mode 100644 index 7e4bb0bd..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/common.py +++ /dev/null @@ -1,134 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Common constants, exceptions and helpe functions -""" -import sys -import xml.parsers.expat - -PY3 = sys.version_info[0] == 3 - -# Fail early when pyexpat is not installed correctly -if not hasattr(xml.parsers.expat, "ParserCreate"): - raise ImportError("pyexpat") # pragma: no cover - - -class DefusedXmlException(ValueError): - """Base exception - """ - - def __repr__(self): - return str(self) - - -class DTDForbidden(DefusedXmlException): - """Document type definition is forbidden - """ - - def __init__(self, name, sysid, pubid): - super(DTDForbidden, self).__init__() - self.name = name - self.sysid = sysid - self.pubid = pubid - - def __str__(self): - tpl = "DTDForbidden(name='{}', system_id={!r}, public_id={!r})" - return tpl.format(self.name, self.sysid, self.pubid) - - -class EntitiesForbidden(DefusedXmlException): - """Entity definition is forbidden - """ - - def __init__(self, name, value, base, sysid, pubid, notation_name): - super(EntitiesForbidden, self).__init__() - self.name = name - self.value = value - self.base = base - self.sysid = sysid - self.pubid = pubid - self.notation_name = notation_name - - def __str__(self): - tpl = "EntitiesForbidden(name='{}', system_id={!r}, public_id={!r})" - return tpl.format(self.name, self.sysid, self.pubid) - - -class ExternalReferenceForbidden(DefusedXmlException): - """Resolving an external reference is forbidden - """ - - def __init__(self, context, base, sysid, pubid): - super(ExternalReferenceForbidden, self).__init__() - self.context = context - self.base = base - self.sysid = sysid - self.pubid = pubid - - def __str__(self): - tpl = "ExternalReferenceForbidden(system_id='{}', public_id={})" - return tpl.format(self.sysid, self.pubid) - - -class NotSupportedError(DefusedXmlException): - """The operation is not supported - """ - - -def _apply_defusing(defused_mod): - assert defused_mod is sys.modules[defused_mod.__name__] - stdlib_name = defused_mod.__origin__ - __import__(stdlib_name, {}, {}, ["*"]) - stdlib_mod = sys.modules[stdlib_name] - stdlib_names = set(dir(stdlib_mod)) - for name, obj in vars(defused_mod).items(): - if name.startswith("_") or name not in stdlib_names: - continue - setattr(stdlib_mod, name, obj) - return stdlib_mod - - -def _generate_etree_functions(DefusedXMLParser, _TreeBuilder, _parse, _iterparse): - """Factory for functions needed by etree, dependent on whether - cElementTree or ElementTree is used.""" - - def parse(source, parser=None, forbid_dtd=False, forbid_entities=True, forbid_external=True): - if parser is None: - parser = DefusedXMLParser( - target=_TreeBuilder(), - forbid_dtd=forbid_dtd, - forbid_entities=forbid_entities, - forbid_external=forbid_external, - ) - return _parse(source, parser) - - def iterparse( - source, - events=None, - parser=None, - forbid_dtd=False, - forbid_entities=True, - forbid_external=True, - ): - if parser is None: - parser = DefusedXMLParser( - target=_TreeBuilder(), - forbid_dtd=forbid_dtd, - forbid_entities=forbid_entities, - forbid_external=forbid_external, - ) - return _iterparse(source, events, parser) - - def fromstring(text, forbid_dtd=False, forbid_entities=True, forbid_external=True): - parser = DefusedXMLParser( - target=_TreeBuilder(), - forbid_dtd=forbid_dtd, - forbid_entities=forbid_entities, - forbid_external=forbid_external, - ) - parser.feed(text) - return parser.close() - - return parse, iterparse, fromstring diff --git a/src/hal/components/cros/master_python_source/defusedxml/expatbuilder.py b/src/hal/components/cros/master_python_source/defusedxml/expatbuilder.py deleted file mode 100644 index 7bfc57e4..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/expatbuilder.py +++ /dev/null @@ -1,107 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defused xml.dom.expatbuilder -""" -from __future__ import print_function, absolute_import - -from xml.dom.expatbuilder import ExpatBuilder as _ExpatBuilder -from xml.dom.expatbuilder import Namespaces as _Namespaces - -from .common import DTDForbidden, EntitiesForbidden, ExternalReferenceForbidden - -__origin__ = "xml.dom.expatbuilder" - - -class DefusedExpatBuilder(_ExpatBuilder): - """Defused document builder""" - - def __init__( - self, options=None, forbid_dtd=False, forbid_entities=True, forbid_external=True - ): - _ExpatBuilder.__init__(self, options) - self.forbid_dtd = forbid_dtd - self.forbid_entities = forbid_entities - self.forbid_external = forbid_external - - def defused_start_doctype_decl(self, name, sysid, pubid, has_internal_subset): - raise DTDForbidden(name, sysid, pubid) - - def defused_entity_decl( - self, name, is_parameter_entity, value, base, sysid, pubid, notation_name - ): - raise EntitiesForbidden(name, value, base, sysid, pubid, notation_name) - - def defused_unparsed_entity_decl(self, name, base, sysid, pubid, notation_name): - # expat 1.2 - raise EntitiesForbidden(name, None, base, sysid, pubid, notation_name) # pragma: no cover - - def defused_external_entity_ref_handler(self, context, base, sysid, pubid): - raise ExternalReferenceForbidden(context, base, sysid, pubid) - - def install(self, parser): - _ExpatBuilder.install(self, parser) - - if self.forbid_dtd: - parser.StartDoctypeDeclHandler = self.defused_start_doctype_decl - if self.forbid_entities: - # if self._options.entities: - parser.EntityDeclHandler = self.defused_entity_decl - parser.UnparsedEntityDeclHandler = self.defused_unparsed_entity_decl - if self.forbid_external: - parser.ExternalEntityRefHandler = self.defused_external_entity_ref_handler - - -class DefusedExpatBuilderNS(_Namespaces, DefusedExpatBuilder): - """Defused document builder that supports namespaces.""" - - def install(self, parser): - DefusedExpatBuilder.install(self, parser) - if self._options.namespace_declarations: - parser.StartNamespaceDeclHandler = self.start_namespace_decl_handler - - def reset(self): - DefusedExpatBuilder.reset(self) - self._initNamespaces() - - -def parse(file, namespaces=True, forbid_dtd=False, forbid_entities=True, forbid_external=True): - """Parse a document, returning the resulting Document node. - - 'file' may be either a file name or an open file object. - """ - if namespaces: - build_builder = DefusedExpatBuilderNS - else: - build_builder = DefusedExpatBuilder - builder = build_builder( - forbid_dtd=forbid_dtd, forbid_entities=forbid_entities, forbid_external=forbid_external - ) - - if isinstance(file, str): - fp = open(file, "rb") - try: - result = builder.parseFile(fp) - finally: - fp.close() - else: - result = builder.parseFile(file) - return result - - -def parseString( - string, namespaces=True, forbid_dtd=False, forbid_entities=True, forbid_external=True -): - """Parse a document from a string, returning the resulting - Document node. - """ - if namespaces: - build_builder = DefusedExpatBuilderNS - else: - build_builder = DefusedExpatBuilder - builder = build_builder( - forbid_dtd=forbid_dtd, forbid_entities=forbid_entities, forbid_external=forbid_external - ) - return builder.parseString(string) diff --git a/src/hal/components/cros/master_python_source/defusedxml/expatreader.py b/src/hal/components/cros/master_python_source/defusedxml/expatreader.py deleted file mode 100644 index 890e1d16..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/expatreader.py +++ /dev/null @@ -1,61 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defused xml.sax.expatreader -""" -from __future__ import print_function, absolute_import - -from xml.sax.expatreader import ExpatParser as _ExpatParser - -from .common import DTDForbidden, EntitiesForbidden, ExternalReferenceForbidden - -__origin__ = "xml.sax.expatreader" - - -class DefusedExpatParser(_ExpatParser): - """Defused SAX driver for the pyexpat C module.""" - - def __init__( - self, - namespaceHandling=0, - bufsize=2 ** 16 - 20, - forbid_dtd=False, - forbid_entities=True, - forbid_external=True, - ): - _ExpatParser.__init__(self, namespaceHandling, bufsize) - self.forbid_dtd = forbid_dtd - self.forbid_entities = forbid_entities - self.forbid_external = forbid_external - - def defused_start_doctype_decl(self, name, sysid, pubid, has_internal_subset): - raise DTDForbidden(name, sysid, pubid) - - def defused_entity_decl( - self, name, is_parameter_entity, value, base, sysid, pubid, notation_name - ): - raise EntitiesForbidden(name, value, base, sysid, pubid, notation_name) - - def defused_unparsed_entity_decl(self, name, base, sysid, pubid, notation_name): - # expat 1.2 - raise EntitiesForbidden(name, None, base, sysid, pubid, notation_name) # pragma: no cover - - def defused_external_entity_ref_handler(self, context, base, sysid, pubid): - raise ExternalReferenceForbidden(context, base, sysid, pubid) - - def reset(self): - _ExpatParser.reset(self) - parser = self._parser - if self.forbid_dtd: - parser.StartDoctypeDeclHandler = self.defused_start_doctype_decl - if self.forbid_entities: - parser.EntityDeclHandler = self.defused_entity_decl - parser.UnparsedEntityDeclHandler = self.defused_unparsed_entity_decl - if self.forbid_external: - parser.ExternalEntityRefHandler = self.defused_external_entity_ref_handler - - -def create_parser(*args, **kwargs): - return DefusedExpatParser(*args, **kwargs) diff --git a/src/hal/components/cros/master_python_source/defusedxml/lxml.py b/src/hal/components/cros/master_python_source/defusedxml/lxml.py deleted file mode 100644 index 1320ca52..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/lxml.py +++ /dev/null @@ -1,155 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""DEPRECATED Example code for lxml.etree protection - -The code has NO protection against decompression bombs. -""" -from __future__ import print_function, absolute_import - -import threading -import warnings - -from lxml import etree as _etree - -from .common import DTDForbidden, EntitiesForbidden, NotSupportedError - -LXML3 = _etree.LXML_VERSION[0] >= 3 - -__origin__ = "lxml.etree" - -tostring = _etree.tostring - - -warnings.warn( - "defusedxml.lxml is no longer supported and will be removed in a " "future release.", - category=DeprecationWarning, - stacklevel=2, -) - - -class RestrictedElement(_etree.ElementBase): - """A restricted Element class that filters out instances of some classes - """ - - __slots__ = () - # blacklist = (etree._Entity, etree._ProcessingInstruction, etree._Comment) - blacklist = _etree._Entity - - def _filter(self, iterator): - blacklist = self.blacklist - for child in iterator: - if isinstance(child, blacklist): - continue - yield child - - def __iter__(self): - iterator = super(RestrictedElement, self).__iter__() - return self._filter(iterator) - - def iterchildren(self, tag=None, reversed=False): - iterator = super(RestrictedElement, self).iterchildren(tag=tag, reversed=reversed) - return self._filter(iterator) - - def iter(self, tag=None, *tags): - iterator = super(RestrictedElement, self).iter(tag=tag, *tags) - return self._filter(iterator) - - def iterdescendants(self, tag=None, *tags): - iterator = super(RestrictedElement, self).iterdescendants(tag=tag, *tags) - return self._filter(iterator) - - def itersiblings(self, tag=None, preceding=False): - iterator = super(RestrictedElement, self).itersiblings(tag=tag, preceding=preceding) - return self._filter(iterator) - - def getchildren(self): - iterator = super(RestrictedElement, self).__iter__() - return list(self._filter(iterator)) - - def getiterator(self, tag=None): - iterator = super(RestrictedElement, self).getiterator(tag) - return self._filter(iterator) - - -class GlobalParserTLS(threading.local): - """Thread local context for custom parser instances - """ - - parser_config = { - "resolve_entities": False, - # 'remove_comments': True, - # 'remove_pis': True, - } - - element_class = RestrictedElement - - def createDefaultParser(self): - parser = _etree.XMLParser(**self.parser_config) - element_class = self.element_class - if self.element_class is not None: - lookup = _etree.ElementDefaultClassLookup(element=element_class) - parser.set_element_class_lookup(lookup) - return parser - - def setDefaultParser(self, parser): - self._default_parser = parser - - def getDefaultParser(self): - parser = getattr(self, "_default_parser", None) - if parser is None: - parser = self.createDefaultParser() - self.setDefaultParser(parser) - return parser - - -_parser_tls = GlobalParserTLS() -getDefaultParser = _parser_tls.getDefaultParser - - -def check_docinfo(elementtree, forbid_dtd=False, forbid_entities=True): - """Check docinfo of an element tree for DTD and entity declarations - - The check for entity declarations needs lxml 3 or newer. lxml 2.x does - not support dtd.iterentities(). - """ - docinfo = elementtree.docinfo - if docinfo.doctype: - if forbid_dtd: - raise DTDForbidden(docinfo.doctype, docinfo.system_url, docinfo.public_id) - if forbid_entities and not LXML3: - # lxml < 3 has no iterentities() - raise NotSupportedError("Unable to check for entity declarations " "in lxml 2.x") - - if forbid_entities: - for dtd in docinfo.internalDTD, docinfo.externalDTD: - if dtd is None: - continue - for entity in dtd.iterentities(): - raise EntitiesForbidden(entity.name, entity.content, None, None, None, None) - - -def parse(source, parser=None, base_url=None, forbid_dtd=False, forbid_entities=True): - if parser is None: - parser = getDefaultParser() - elementtree = _etree.parse(source, parser, base_url=base_url) - check_docinfo(elementtree, forbid_dtd, forbid_entities) - return elementtree - - -def fromstring(text, parser=None, base_url=None, forbid_dtd=False, forbid_entities=True): - if parser is None: - parser = getDefaultParser() - rootelement = _etree.fromstring(text, parser, base_url=base_url) - elementtree = rootelement.getroottree() - check_docinfo(elementtree, forbid_dtd, forbid_entities) - return rootelement - - -XML = fromstring - - -def iterparse(*args, **kwargs): - raise NotSupportedError("defused lxml.etree.iterparse not available") diff --git a/src/hal/components/cros/master_python_source/defusedxml/minidom.py b/src/hal/components/cros/master_python_source/defusedxml/minidom.py deleted file mode 100644 index 78033b6c..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/minidom.py +++ /dev/null @@ -1,63 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defused xml.dom.minidom -""" -from __future__ import print_function, absolute_import - -from xml.dom.minidom import _do_pulldom_parse -from . import expatbuilder as _expatbuilder -from . import pulldom as _pulldom - -__origin__ = "xml.dom.minidom" - - -def parse( - file, parser=None, bufsize=None, forbid_dtd=False, forbid_entities=True, forbid_external=True -): - """Parse a file into a DOM by filename or file object.""" - if parser is None and not bufsize: - return _expatbuilder.parse( - file, - forbid_dtd=forbid_dtd, - forbid_entities=forbid_entities, - forbid_external=forbid_external, - ) - else: - return _do_pulldom_parse( - _pulldom.parse, - (file,), - { - "parser": parser, - "bufsize": bufsize, - "forbid_dtd": forbid_dtd, - "forbid_entities": forbid_entities, - "forbid_external": forbid_external, - }, - ) - - -def parseString( - string, parser=None, forbid_dtd=False, forbid_entities=True, forbid_external=True -): - """Parse a file into a DOM from a string.""" - if parser is None: - return _expatbuilder.parseString( - string, - forbid_dtd=forbid_dtd, - forbid_entities=forbid_entities, - forbid_external=forbid_external, - ) - else: - return _do_pulldom_parse( - _pulldom.parseString, - (string,), - { - "parser": parser, - "forbid_dtd": forbid_dtd, - "forbid_entities": forbid_entities, - "forbid_external": forbid_external, - }, - ) diff --git a/src/hal/components/cros/master_python_source/defusedxml/pulldom.py b/src/hal/components/cros/master_python_source/defusedxml/pulldom.py deleted file mode 100644 index e3b10a46..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/pulldom.py +++ /dev/null @@ -1,41 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defused xml.dom.pulldom -""" -from __future__ import print_function, absolute_import - -from xml.dom.pulldom import parse as _parse -from xml.dom.pulldom import parseString as _parseString -from .sax import make_parser - -__origin__ = "xml.dom.pulldom" - - -def parse( - stream_or_string, - parser=None, - bufsize=None, - forbid_dtd=False, - forbid_entities=True, - forbid_external=True, -): - if parser is None: - parser = make_parser() - parser.forbid_dtd = forbid_dtd - parser.forbid_entities = forbid_entities - parser.forbid_external = forbid_external - return _parse(stream_or_string, parser, bufsize) - - -def parseString( - string, parser=None, forbid_dtd=False, forbid_entities=True, forbid_external=True -): - if parser is None: - parser = make_parser() - parser.forbid_dtd = forbid_dtd - parser.forbid_entities = forbid_entities - parser.forbid_external = forbid_external - return _parseString(string, parser) diff --git a/src/hal/components/cros/master_python_source/defusedxml/sax.py b/src/hal/components/cros/master_python_source/defusedxml/sax.py deleted file mode 100644 index b2786f74..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/sax.py +++ /dev/null @@ -1,60 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defused xml.sax -""" -from __future__ import print_function, absolute_import - -from xml.sax import InputSource as _InputSource -from xml.sax import ErrorHandler as _ErrorHandler - -from . import expatreader - -__origin__ = "xml.sax" - - -def parse( - source, - handler, - errorHandler=_ErrorHandler(), - forbid_dtd=False, - forbid_entities=True, - forbid_external=True, -): - parser = make_parser() - parser.setContentHandler(handler) - parser.setErrorHandler(errorHandler) - parser.forbid_dtd = forbid_dtd - parser.forbid_entities = forbid_entities - parser.forbid_external = forbid_external - parser.parse(source) - - -def parseString( - string, - handler, - errorHandler=_ErrorHandler(), - forbid_dtd=False, - forbid_entities=True, - forbid_external=True, -): - from io import BytesIO - - if errorHandler is None: - errorHandler = _ErrorHandler() - parser = make_parser() - parser.setContentHandler(handler) - parser.setErrorHandler(errorHandler) - parser.forbid_dtd = forbid_dtd - parser.forbid_entities = forbid_entities - parser.forbid_external = forbid_external - - inpsrc = _InputSource() - inpsrc.setByteStream(BytesIO(string)) - parser.parse(inpsrc) - - -def make_parser(parser_list=[]): - return expatreader.create_parser() diff --git a/src/hal/components/cros/master_python_source/defusedxml/xmlrpc.py b/src/hal/components/cros/master_python_source/defusedxml/xmlrpc.py deleted file mode 100644 index fbc674da..00000000 --- a/src/hal/components/cros/master_python_source/defusedxml/xmlrpc.py +++ /dev/null @@ -1,153 +0,0 @@ -# defusedxml -# -# Copyright (c) 2013 by Christian Heimes -# Licensed to PSF under a Contributor Agreement. -# See https://www.python.org/psf/license for licensing details. -"""Defused xmlrpclib - -Also defuses gzip bomb -""" -from __future__ import print_function, absolute_import - -import io - -from .common import DTDForbidden, EntitiesForbidden, ExternalReferenceForbidden, PY3 - -if PY3: - __origin__ = "xmlrpc.client" - from xmlrpc.client import ExpatParser - from xmlrpc import client as xmlrpc_client - from xmlrpc import server as xmlrpc_server - from xmlrpc.client import gzip_decode as _orig_gzip_decode - from xmlrpc.client import GzipDecodedResponse as _OrigGzipDecodedResponse -else: - __origin__ = "xmlrpclib" - from xmlrpclib import ExpatParser - import xmlrpclib as xmlrpc_client - - xmlrpc_server = None - from xmlrpclib import gzip_decode as _orig_gzip_decode - from xmlrpclib import GzipDecodedResponse as _OrigGzipDecodedResponse - -try: - import gzip -except ImportError: # pragma: no cover - gzip = None - - -# Limit maximum request size to prevent resource exhaustion DoS -# Also used to limit maximum amount of gzip decoded data in order to prevent -# decompression bombs -# A value of -1 or smaller disables the limit -MAX_DATA = 30 * 1024 * 1024 # 30 MB - - -def defused_gzip_decode(data, limit=None): - """gzip encoded data -> unencoded data - - Decode data using the gzip content encoding as described in RFC 1952 - """ - if not gzip: # pragma: no cover - raise NotImplementedError - if limit is None: - limit = MAX_DATA - f = io.BytesIO(data) - gzf = gzip.GzipFile(mode="rb", fileobj=f) - try: - if limit < 0: # no limit - decoded = gzf.read() - else: - decoded = gzf.read(limit + 1) - except IOError: # pragma: no cover - raise ValueError("invalid data") - f.close() - gzf.close() - if limit >= 0 and len(decoded) > limit: - raise ValueError("max gzipped payload length exceeded") - return decoded - - -class DefusedGzipDecodedResponse(gzip.GzipFile if gzip else object): - """a file-like object to decode a response encoded with the gzip - method, as described in RFC 1952. - """ - - def __init__(self, response, limit=None): - # response doesn't support tell() and read(), required by - # GzipFile - if not gzip: # pragma: no cover - raise NotImplementedError - self.limit = limit = limit if limit is not None else MAX_DATA - if limit < 0: # no limit - data = response.read() - self.readlength = None - else: - data = response.read(limit + 1) - self.readlength = 0 - if limit >= 0 and len(data) > limit: - raise ValueError("max payload length exceeded") - self.stringio = io.BytesIO(data) - gzip.GzipFile.__init__(self, mode="rb", fileobj=self.stringio) - - def read(self, n): - if self.limit >= 0: - left = self.limit - self.readlength - n = min(n, left + 1) - data = gzip.GzipFile.read(self, n) - self.readlength += len(data) - if self.readlength > self.limit: - raise ValueError("max payload length exceeded") - return data - else: - return gzip.GzipFile.read(self, n) - - def close(self): - gzip.GzipFile.close(self) - self.stringio.close() - - -class DefusedExpatParser(ExpatParser): - def __init__(self, target, forbid_dtd=False, forbid_entities=True, forbid_external=True): - ExpatParser.__init__(self, target) - self.forbid_dtd = forbid_dtd - self.forbid_entities = forbid_entities - self.forbid_external = forbid_external - parser = self._parser - if self.forbid_dtd: - parser.StartDoctypeDeclHandler = self.defused_start_doctype_decl - if self.forbid_entities: - parser.EntityDeclHandler = self.defused_entity_decl - parser.UnparsedEntityDeclHandler = self.defused_unparsed_entity_decl - if self.forbid_external: - parser.ExternalEntityRefHandler = self.defused_external_entity_ref_handler - - def defused_start_doctype_decl(self, name, sysid, pubid, has_internal_subset): - raise DTDForbidden(name, sysid, pubid) - - def defused_entity_decl( - self, name, is_parameter_entity, value, base, sysid, pubid, notation_name - ): - raise EntitiesForbidden(name, value, base, sysid, pubid, notation_name) - - def defused_unparsed_entity_decl(self, name, base, sysid, pubid, notation_name): - # expat 1.2 - raise EntitiesForbidden(name, None, base, sysid, pubid, notation_name) # pragma: no cover - - def defused_external_entity_ref_handler(self, context, base, sysid, pubid): - raise ExternalReferenceForbidden(context, base, sysid, pubid) - - -def monkey_patch(): - xmlrpc_client.FastParser = DefusedExpatParser - xmlrpc_client.GzipDecodedResponse = DefusedGzipDecodedResponse - xmlrpc_client.gzip_decode = defused_gzip_decode - if xmlrpc_server: - xmlrpc_server.gzip_decode = defused_gzip_decode - - -def unmonkey_patch(): - xmlrpc_client.FastParser = None - xmlrpc_client.GzipDecodedResponse = _OrigGzipDecodedResponse - xmlrpc_client.gzip_decode = _orig_gzip_decode - if xmlrpc_server: - xmlrpc_server.gzip_decode = _orig_gzip_decode diff --git a/src/hal/components/cros/master_python_source/rosgraph/__init__.py b/src/hal/components/cros/master_python_source/rosgraph/__init__.py deleted file mode 100644 index 44bc101e..00000000 --- a/src/hal/components/cros/master_python_source/rosgraph/__init__.py +++ /dev/null @@ -1,56 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. - -import sys - -from . rosenv import get_master_uri, ROS_MASTER_URI, ROS_NAMESPACE, ROS_HOSTNAME, ROS_IP, ROS_IPV6 -from . masterapi import Master, MasterFailure, MasterError, MasterException -from . masterapi import is_online as is_master_online - -# bring in names submodule -from . import names - -def myargv(argv=None): - """ - Remove ROS remapping arguments from sys.argv arguments. - - :returns: copy of sys.argv with ROS remapping arguments removed, ``[str]`` - """ - if argv is None: - argv = sys.argv - return [a for a in argv if not names.REMAP in a] - -__all__ = ['myargv', - 'get_master_uri', 'ROS_MASTER_URI', 'ROS_NAMESPACE', 'ROS_HOSTNAME', 'ROS_IP', 'ROS_IPV6', - 'Master', 'MasterFailure', 'MasterError', 'MasterException', - 'is_master_online'] - diff --git a/src/hal/components/cros/master_python_source/rosgraph/impl/__init__.py b/src/hal/components/cros/master_python_source/rosgraph/impl/__init__.py deleted file mode 100644 index 0212c3f0..00000000 --- a/src/hal/components/cros/master_python_source/rosgraph/impl/__init__.py +++ /dev/null @@ -1,34 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id: __init__.py 5735 2009-08-20 21:31:27Z sfkwc $ - diff --git a/src/hal/components/cros/master_python_source/rosgraph/impl/graph.py b/src/hal/components/cros/master_python_source/rosgraph/impl/graph.py deleted file mode 100644 index 5f78df97..00000000 --- a/src/hal/components/cros/master_python_source/rosgraph/impl/graph.py +++ /dev/null @@ -1,582 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -from __future__ import print_function - -""" -Data structures and library for representing ROS Computation Graph state. -""" - -import sys -import time -import itertools -import random -import logging -import traceback -try: - from xmlrpc.client import ServerProxy -except ImportError: - from xmlrpclib import ServerProxy -import socket - -import rosgraph.masterapi - -logger = logging.getLogger('rosgraph.graph') - -_ROS_NAME = '/rosviz' - -def topic_node(topic): - """ - In order to prevent topic/node name aliasing, we have to remap - topic node names. Currently we just prepend a space, which is - an illegal ROS name and thus not aliased. - @return str: topic mapped to a graph node name. - """ - return ' ' + topic -def node_topic(node): - """ - Inverse of topic_node - @return str: undo topic_node() operation - """ - return node[1:] - -class BadNode(object): - """ - Data structure for storing info about a 'bad' node - """ - - ## no connectivity - DEAD = 0 - ## intermittent connectivity - WONKY = 1 - - def __init__(self, name, type, reason): - """ - @param type: DEAD | WONKY - @type type: int - """ - self.name =name - self.reason = reason - self.type = type - -class EdgeList(object): - """ - Data structure for storing Edge instances - """ - __slots__ = ['edges_by_start', 'edges_by_end'] - def __init__(self): - # in order to make it easy to purge edges, we double-index them - self.edges_by_start = {} - self.edges_by_end = {} - - def __iter__(self): - return itertools.chain(*[v for v in self.edges_by_start.values()]) - - def has(self, edge): - return edge in self - - def __contains__(self, edge): - """ - @return: True if edge is in edge list - @rtype: bool - """ - key = edge.key - return key in self.edges_by_start and \ - edge in self.edges_by_start[key] - - def add(self, edge): - """ - Add an edge to our internal representation. not multi-thread safe - @param edge: edge to add - @type edge: Edge - """ - # see note in __init__ - def update_map(map, key, edge): - if key in map: - l = map[key] - if not edge in l: - l.append(edge) - return True - else: - return False - else: - map[key] = [edge] - return True - - updated = update_map(self.edges_by_start, edge.key, edge) - updated = update_map(self.edges_by_end, edge.rkey, edge) or updated - return updated - - def add_edges(self, start, dest, direction, label=''): - """ - Create Edge instances for args and add resulting edges to edge - list. Convenience method to avoid repetitve logging, etc... - @param edge_list: data structure to add edge to - @type edge_list: EdgeList - @param start: name of start node. If None, warning will be logged and add fails - @type start: str - @param dest: name of start node. If None, warning will be logged and add fails - @type dest: str - @param direction: direction string (i/o/b) - @type direction: str - @return: True if update occured - @rtype: bool - """ - - # the warnings should generally be temporary, occuring of the - # master/node information becomes stale while we are still - # doing an update - updated = False - if not start: - logger.warn("cannot add edge: cannot map start [%s] to a node name", start) - elif not dest: - logger.warn("cannot add edge: cannot map dest [%s] to a node name", dest) - else: - for args in edge_args(start, dest, direction, label): - updated = self.add(Edge(*args)) or updated - return updated - - def delete_all(self, node): - """ - Delete all edges that start or end at node - @param node: name of node - @type node: str - """ - def matching(map, pref): - return [map[k] for k in map.keys() if k.startswith(pref)] - - pref = node+"|" - edge_lists = matching(self.edges_by_start, pref) + matching(self.edges_by_end, pref) - for el in edge_lists: - for e in el: - self.delete(e) - - def delete(self, edge): - # see note in __init__ - def update_map(map, key, edge): - if key in map: - edges = map[key] - if edge in edges: - edges.remove(edge) - return True - update_map(self.edges_by_start, edge.key, edge) - update_map(self.edges_by_end, edge.rkey, edge) - -class Edge(object): - """ - Data structure for representing ROS node graph edge - """ - - __slots__ = ['start', 'end', 'label', 'key', 'rkey'] - def __init__(self, start, end, label=''): - self.start = start - self.end = end - self.label = label - self.key = "%s|%s"%(self.start, self.label) - # reverse key, indexed from end - self.rkey = "%s|%s"%(self.end, self.label) - - def __ne__(self, other): - return self.start != other.start or self.end != other.end - def __str__(self): - return "%s->%s"%(self.start, self.end) - def __eq__(self, other): - return self.start == other.start and self.end == other.end - -def edge_args(start, dest, direction, label): - """ - compute argument ordering for Edge constructor based on direction flag - @param direction str: 'i', 'o', or 'b' (in/out/bidir) relative to \a start - @param start str: name of starting node - @param start dest: name of destination node - """ - edge_args = [] - if direction in ['o', 'b']: - edge_args.append((start, dest, label)) - if direction in ['i', 'b']: - edge_args.append((dest, start, label)) - return edge_args - - -class Graph(object): - """ - Utility class for polling ROS statistics from running ROS graph. - Not multi-thread-safe - """ - - def __init__(self, node_ns='/', topic_ns='/'): - self.master = rosgraph.masterapi.Master(_ROS_NAME) - - self.node_ns = node_ns or '/' - self.topic_ns = topic_ns or '/' - - # ROS nodes - self.nn_nodes = set([]) - # ROS topic nodes - self.nt_nodes = set([]) - - # ROS nodes that aren't responding quickly - self.bad_nodes = {} - import threading - self.bad_nodes_lock = threading.Lock() - - # ROS services - self.srvs = set([]) - # ROS node->node transport connections - self.nn_edges = EdgeList() - # ROS node->topic connections - self.nt_edges = EdgeList() - # ROS all node->topic connections, including empty - self.nt_all_edges = EdgeList() - - # node names to URI map - self.node_uri_map = {} # { node_name_str : uri_str } - # reverse map URIs to node names - self.uri_node_map = {} # { uri_str : node_name_str } - - # time we last contacted master - self.last_master_refresh = 0 - self.last_node_refresh = {} - - # time we last communicated with master - # seconds until master data is considered stale - self.master_stale = 5.0 - # time we last communicated with node - # seconds until node data is considered stale - self.node_stale = 5.0 #seconds - - - def set_master_stale(self, stale_secs): - """ - @param stale_secs: seconds that data is considered fresh - @type stale_secs: double - """ - self.master_stale = stale_secs - - def set_node_stale(self, stale_secs): - """ - @param stale_secs: seconds that data is considered fresh - @type stale_secs: double - """ - self.node_stale = stale_secs - - def _master_refresh(self): - """ - @return: True if nodes information was updated - @rtype: bool - """ - logger.debug("master refresh: starting") - updated = False - try: - val = self.master.getSystemState() - except rosgraph.masterapi.MasterException as e: - print("Unable to contact master", str(e), file=sys.stderr) - logger.error("unable to contact master: %s", str(e)) - return False - - pubs, subs, srvs = val - - nodes = [] - nt_all_edges = self.nt_all_edges - nt_nodes = self.nt_nodes - for state, direction in ((pubs, 'o'), (subs, 'i')): - for topic, l in state: - if topic.startswith(self.topic_ns): - nodes.extend([n for n in l if n.startswith(self.node_ns)]) - nt_nodes.add(topic_node(topic)) - for node in l: - updated = nt_all_edges.add_edges( - node, topic_node(topic), direction) or updated - - nodes = set(nodes) - - srvs = set([s for s, _ in srvs]) - purge = None - if nodes ^ self.nn_nodes: - purge = self.nn_nodes - nodes - self.nn_nodes = nodes - updated = True - if srvs ^ self.srvs: - self.srvs = srvs - updated = True - - if purge: - logger.debug("following nodes and related edges will be purged: %s", ','.join(purge)) - for p in purge: - logger.debug('purging edges for node %s', p) - self.nn_edges.delete_all(p) - self.nt_edges.delete_all(p) - self.nt_all_edges.delete_all(p) - - logger.debug("master refresh: done, updated[%s]", updated) - return updated - - def _mark_bad_node(self, node, reason): - try: - # bad nodes are updated in a separate thread, so lock - self.bad_nodes_lock.acquire() - if node in self.bad_nodes: - self.bad_nodes[node].type = BadNode.DEAD - else: - self.bad_nodes[node] = (BadNode(node, BadNode.DEAD, reason)) - finally: - self.bad_nodes_lock.release() - - def _unmark_bad_node(self, node, reason): - """ - Promotes bad node to 'wonky' status. - """ - try: - # bad nodes are updated in a separate thread, so lock - self.bad_nodes_lock.acquire() - bad = self.bad_nodes[node] - bad.type = BadNode.WONKY - finally: - self.bad_nodes_lock.release() - - def _node_refresh_businfo(self, node, api, bad_node=False): - """ - Retrieve bus info from the node and update nodes and edges as appropriate - @param node: node name - @type node: str - @param api: XML-RPC proxy - @type api: ServerProxy - @param bad_node: If True, node has connectivity issues and - should be treated differently - @type bad_node: bool - """ - try: - logger.debug("businfo: contacting node [%s] for bus info", node) - - # unmark bad node, though it stays on the bad list - if bad_node: - self._unmark_bad_node(node) - # Lower the socket timeout as we cannot abide by slow HTTP timeouts. - # If a node cannot meet this timeout, it goes on the bad list - # TODO: override transport instead. - old_timeout = socket.getdefaulttimeout() - if bad_node: - #even stricter timeout for bad_nodes right now - socket.setdefaulttimeout(0.2) - else: - socket.setdefaulttimeout(1.0) - - code, msg, bus_info = api.getBusInfo(_ROS_NAME) - - socket.setdefaulttimeout(old_timeout) - except Exception as e: - # node is (still) bad - self._mark_bad_node(node, str(e)) - code = -1 - msg = traceback.format_exc() - - updated = False - if code != 1: - logger.error("cannot get stats info from node [%s]: %s", node, msg) - else: - # [[connectionId1, destinationId1, direction1, transport1, ...]... ] - for info in bus_info: - # #3579 bad node, ignore - if len(info) < 5: - continue - - connection_id = info[0] - dest_id = info[1] - direction = info[2] - transport = info[3] - topic = info[4] - if len(info) > 5: - connected = info[5] - else: - connected = True #backwards compatibility - - if connected and topic.startswith(self.topic_ns): - # blindly add as we will be able to catch state change via edges. - # this currently means we don't cleanup topics - self.nt_nodes.add(topic_node(topic)) - - # update node->topic->node graph edges - updated = self.nt_edges.add_edges(node, topic_node(topic), direction) or updated - - # update node->node graph edges - if dest_id.startswith('http://'): - #print("FOUND URI", dest_id) - dest_name = self.uri_node_map.get(dest_id, None) - updated = self.nn_edges.add_edges(node, dest_name, direction, topic) or updated - else: - #TODO: anyting to do here? - pass - return updated - - def _node_refresh(self, node, bad_node=False): - """ - Contact node for stats/connectivity information - @param node: name of node to contact - @type node: str - @param bad_node: if True, node has connectivity issues - @type bad_node: bool - @return: True if node was successfully contacted - @rtype bool - """ - # TODO: I'd like for master to provide this information in - # getSystemState() instead to prevent the extra connection per node - updated = False - uri = self._node_uri_refresh(node) - try: - if uri: - api = ServerProxy(uri) - updated = self._node_refresh_businfo(node, api, bad_node) - except KeyError as e: - logger.warn('cannot contact node [%s] as it is not in the lookup table'%node) - return updated - - def _node_uri_refresh(self, node): - try: - uri = self.master.lookupNode(node) - except: - msg = traceback.format_exc() - logger.warn("master reported error in node lookup: %s"%msg) - return None - # update maps - self.node_uri_map[node] = uri - self.uri_node_map[uri] = node - return uri - - def _node_uri_refresh_all(self): - """ - Build self.node_uri_map and self.uri_node_map using master as a - lookup service. This will make N requests to the master for N - nodes, so this should only be used sparingly - """ - for node in self.nn_nodes: - self._node_uri_refresh(node) - - def bad_update(self): - """ - Update loop for nodes with bad connectivity. We box them separately - so that we can maintain the good performance of the normal update loop. - Once a node is on the bad list it stays there. - """ - last_node_refresh = self.last_node_refresh - - # nodes left to check - try: - self.bad_nodes_lock.acquire() - # make copy due to multithreading - update_queue = self.bad_nodes.values()[:] - finally: - self.bad_nodes_lock.release() - - # return value. True if new data differs from old - updated = False - # number of nodes we checked - num_nodes = 0 - - start_time = time.time() - while update_queue: - # figure out the next node to contact - next = update_queue.pop() - # rate limit talking to any particular node - if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale): - updated = self._node_refresh(next.name, True) or updated - # include in random offset (max 1/5th normal update interval) - # to help spread out updates - last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0) - num_nodes += 1 - - # small yield to keep from torquing the processor - time.sleep(0.01) - end_time = time.time() - #print("Update (bad nodes) took %ss for %s nodes"%((end_time-start_time), num_nodes)) - logger.debug("ROS stats (bad nodes) update took %ss"%(end_time-start_time)) - return updated - - def update(self): - """ - Update all the stats. This method may take a while to complete as it will - communicate with all nodes + master. - """ - - last_node_refresh = self.last_node_refresh - - # nodes left to check - update_queue = None - # True if there are still more stats to fetch this cycle - work_to_do = True - # return value. True if new data differs from old - updated = False - # number of nodes we checked - num_nodes = 0 - - start_time = time.time() - while work_to_do: - - # each time through the loop try to talk to either the master - # or a node. stop when we have talked to everybody. - - # get a new node list from the master - if time.time() > (self.last_master_refresh + self.master_stale): - updated = self._master_refresh() - if self.last_master_refresh == 0: - # first time we contact the master, also refresh our full lookup tables - self._node_uri_refresh_all() - - self.last_master_refresh = time.time() - # contact the nodes for stats - else: - # initialize update_queue based on most current nodes list - if update_queue is None: - update_queue = list(self.nn_nodes) - # no nodes left to contact - elif not update_queue: - work_to_do = False - # contact next node - else: - # figure out the next node to contact - next = update_queue.pop() - # rate limit talking to any particular node - if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale): - updated = self._node_refresh(next) or updated - # include in random offset (max 1/5th normal update interval) - # to help spread out updates - last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0) - num_nodes += 1 - - # small yield to keep from torquing the processor - time.sleep(0.01) - end_time = time.time() - #print("Update took %ss for %s nodes"%((end_time-start_time), num_nodes)) - logger.debug("ROS stats update took %ss"%(end_time-start_time)) - return updated - diff --git a/src/hal/components/cros/master_python_source/rosgraph/masterapi.py b/src/hal/components/cros/master_python_source/rosgraph/masterapi.py deleted file mode 100644 index 7e391b9e..00000000 --- a/src/hal/components/cros/master_python_source/rosgraph/masterapi.py +++ /dev/null @@ -1,481 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id: masterapi.py 9672 2010-05-11 21:57:40Z kwc $ -""" -Python adapter for calling ROS Master API. While it is trivial to call the -Master directly using XML-RPC, this API provides a safer abstraction in the event -the Master API is changed. -""" - -try: - from xmlrpc.client import ServerProxy # Python 3.x -except ImportError: - from xmlrpclib import ServerProxy # Python 2.x - -from . names import make_caller_id -from . rosenv import get_master_uri -from . network import parse_http_host_and_port - -class MasterException(Exception): - """ - Base class of ROS-master related errors. - """ - pass - -class MasterFailure(MasterException): - """ - Call to Master failed. This generally indicates an internal error - in the Master and that the Master may be in an inconsistent state. - """ - pass - -class MasterError(MasterException): - """ - Master returned an error code, which indicates an error in the - arguments passed to the Master. - """ - pass - -# backwards compat -ROSMasterException = MasterException -Error = MasterError -Failure = MasterFailure - -def is_online(master_uri=None): - """ - @param master_uri: (optional) override environment's ROS_MASTER_URI - @type master_uri: str - @return: True if Master is available - """ - return Master('rosgraph', master_uri=master_uri).is_online() - -class Master(object): - """ - API for interacting with the ROS master. Although the Master is - relatively simple to interact with using the XMLRPC API, this - abstraction layer provides protection against future updates. It - also provides a streamlined API with builtin return code checking - and caller_id passing. - """ - - def __init__(self, caller_id, master_uri=None): - """ - :param caller_id: name of node to use in calls to master, ``str`` - :param master_uri: (optional) override default ROS master URI, ``str`` - :raises: :exc:`ValueError` If ROS master uri not set properly - """ - - if master_uri is None: - master_uri = get_master_uri() - self._reinit(master_uri) - - self.caller_id = make_caller_id(caller_id) #resolve - if self.caller_id[-1] == '/': - self.caller_id = self.caller_id[:-1] - - def _reinit(self, master_uri): - """ - Internal API for reinitializing this handle to be a new master - - :raises: :exc:`ValueError` If ROS master uri not set - """ - if master_uri is None: - raise ValueError("ROS master URI is not set") - # #1730 validate URL for better error messages - try: - parse_http_host_and_port(master_uri) - except ValueError: - raise ValueError("invalid master URI: %s"%(master_uri)) - - self.master_uri = master_uri - self.handle = ServerProxy(self.master_uri) - - def is_online(self): - """ - Check if Master is online. - - NOTE: this is not part of the actual Master API. This is a convenience function. - - @param master_uri: (optional) override environment's ROS_MASTER_URI - @type master_uri: str - @return: True if Master is available - """ - try: - self.getPid() - return True - except: - return False - - def _succeed(self, args): - """ - Check master return code and return the value field. - - @param args: master return value - @type args: (int, str, XMLRPCLegalValue) - @return: value field of args (master return value) - @rtype: XMLRPCLegalValue - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - code, msg, val = args - if code == 1: - return val - elif code == -1: - raise Error(msg) - else: - raise Failure(msg) - - ################################################################################ - # PARAM SERVER - - def deleteParam(self, key): - """ - Parameter Server: delete parameter - @param key: parameter name - @type key: str - @return: 0 - @rtype: int - """ - return self._succeed(self.handle.deleteParam(self.caller_id, key)) - - def setParam(self, key, value): - """ - Parameter Server: set parameter. NOTE: if value is a - dictionary it will be treated as a parameter tree, where key - is the parameter namespace. For example::: - {'x':1,'y':2,'sub':{'z':3}} - - will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it - will replace all existing parameters in the key parameter - namespace with the parameters in value. You must set - parameters individually if you wish to perform a union update. - - @param key: parameter name - @type key: str - @param value: parameter value. - @type value: XMLRPCLegalValue - @return: 0 - @rtype: int - """ - return self._succeed(self.handle.setParam(self.caller_id, key, value)) - - def getParam(self, key): - """ - Retrieve parameter value from server. - @param key: parameter to lookup. If key is a namespace, - getParam() will return a parameter tree. - @type key: str - getParam() will return a parameter tree. - - @return: parameterValue. If key is a namespace, - the return value will be a dictionary, where each key is a - parameter in that namespace. Sub-namespaces are also - represented as dictionaries. - @rtype: XMLRPCLegalValue - """ - return self._succeed(self.handle.getParam(self.caller_id, key)) - - def searchParam(self, key): - """ - Search for parameter key on parameter server. Search starts in caller's namespace and proceeds - upwards through parent namespaces until Parameter Server finds a matching key. - - searchParam's behavior is to search for the first partial match. - For example, imagine that there are two 'robot_description' parameters:: - - /robot_description - /robot_description/arm - /robot_description/base - /pr2/robot_description - /pr2/robot_description/base - - If I start in the namespace /pr2/foo and search for - 'robot_description', searchParam will match - /pr2/robot_description. If I search for 'robot_description/arm' - it will return /pr2/robot_description/arm, even though that - parameter does not exist (yet). - - @param key: parameter key to search for. - @type key: str - @return: foundKey - @rtype: str - """ - return self._succeed(self.handle.searchParam(self.caller_id, key)) - - def subscribeParam(self, caller_api, key): - """ - Retrieve parameter value from server and subscribe to updates to that param. See - paramUpdate() in the Node API. - @param key: parameter to lookup. - @type key: str - @param caller_api: API URI for paramUpdate callbacks. - @type caller_api: str - @return: parameterValue. parameterValue is an empty dictionary if the parameter has not been set yet. - @rtype: XMLRPCLegalValue - """ - return self._succeed(self.handle.subscribeParam(self.caller_id, caller_api, key)) - - def unsubscribeParam(self, caller_api, key): - """ - Retrieve parameter value from server and subscribe to updates to that param. See - paramUpdate() in the Node API. - @param key: parameter to lookup. - @type key: str - @param caller_api: API URI for paramUpdate callbacks. - @type caller_api: str - @return: numUnsubscribed. If numUnsubscribed is zero it means that the caller was not subscribed to the parameter. - @rtype: int - """ - return self._succeed(self.handle.unsubscribeParam(self.caller_id, caller_api, key)) - - def hasParam(self, key): - """ - Check if parameter is stored on server. - @param key: parameter to check - @type key: str - @return: [code, statusMessage, hasParam] - @rtype: [int, str, bool] - """ - return self._succeed(self.handle.hasParam(self.caller_id, key)) - - def getParamNames(self): - """ - Get list of all parameter names stored on this server. - This does not adjust parameter names for caller's scope. - - @return: [code, statusMessage, parameterNameList] - @rtype: [int, str, [str]] - """ - return self._succeed(self.handle.getParamNames(self.caller_id)) - - - ################################################################################ - - def getPid(self): - """ - Get the PID of this server - @return: serverProcessPID - @rtype: int - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.getPid(self.caller_id)) - - def getUri(self): - """ - Get the URI of this Master - @return: masterUri - @rtype: str - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.getUri(self.caller_id)) - - def registerService(self, service, service_api, caller_api): - """ - Register the caller as a provider of the specified service. - @param service str: Fully-qualified name of service - @param service_api str: Service URI - @param caller_api str: XML-RPC URI of caller node - @return: ignore - @rtype: int - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.registerService(self.caller_id, service, service_api, caller_api)) - - def lookupService(self, service): - """ - Lookup all provider of a particular service. - @param service: fully-qualified name of service to lookup. - @type: service: str - @return (int, str, str): (code, message, serviceUrl). service URL is provides - and address and port of the service. Fails if there is no provider. - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.lookupService(self.caller_id, service)) - - - def unregisterService(self, service, service_api): - """ - Unregister the caller as a provider of the specified service. - @param service: Fully-qualified name of service - @type service: str - @param service_api: API URI of service to unregister. Unregistration will only occur if current - registration matches. - @type service_api: str - @return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1). - If this is zero it means that the caller was not registered as a service provider. - The call still succeeds as the intended final state is reached. - @rtype: (int, str, int) - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.unregisterService(self.caller_id, service, service_api)) - - - def registerSubscriber(self, topic, topic_type, caller_api): - """ - Subscribe the caller to the specified topic. In addition to receiving - a list of current publishers, the subscriber will also receive notifications - of new publishers via the publisherUpdate API. - @param topic str: Fully-qualified name of topic to subscribe to. - @param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name. - @type topic_type: str - @param caller_api: XML-RPC URI of caller node for new publisher notifications - @type caller_api: str - @return: (code, message, publishers). Publishers is a list of XMLRPC API URIs - for nodes currently publishing the specified topic. - @rtype: (int, str, list(str)) - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.registerSubscriber(self.caller_id, topic, topic_type, caller_api)) - - - def unregisterSubscriber(self, topic, caller_api): - """ - Unregister the caller as a publisher of the topic. - @param topic: Fully-qualified name of topic to unregister. - @type topic: str - @param caller_api: API URI of service to unregister. Unregistration will only occur if current - @type caller_api: str - registration matches. - @return: (code, statusMessage, numUnsubscribed). - If numUnsubscribed is zero it means that the caller was not registered as a subscriber. - The call still succeeds as the intended final state is reached. - @rtype: (int, str, int) - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.unregisterSubscriber(self.caller_id, topic, caller_api)) - - def registerPublisher(self, topic, topic_type, caller_api): - """ - Register the caller as a publisher the topic. - @param topic: Fully-qualified name of topic to register. - @type topic: str - @param topic_type: Datatype for topic. Must be a - package-resource name, i.e. the .msg name. - @type topic_type: str - @param caller_api str: ROS caller XML-RPC API URI - @type caller_api: str - @return: subscriberApis. - List of current subscribers of topic in the form of XMLRPC URIs. - @rtype: [str] - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.registerPublisher(self.caller_id, topic, topic_type, caller_api)) - - def unregisterPublisher(self, topic, caller_api): - """ - Unregister the caller as a publisher of the topic. - @param topic: Fully-qualified name of topic to unregister. - @type topic: str - @param caller_api str: API URI of service to - unregister. Unregistration will only occur if current - registration matches. - @type caller_api: str - @return: numUnregistered. - If numUnregistered is zero it means that the caller was not registered as a publisher. - The call still succeeds as the intended final state is reached. - @rtype: int - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.unregisterPublisher(self.caller_id, topic, caller_api)) - - def lookupNode(self, node_name): - """ - Get the XML-RPC URI of the node with the associated - name/caller_id. This API is for looking information about - publishers and subscribers. Use lookupService instead to lookup - ROS-RPC URIs. - @param node: name of node to lookup - @type node: str - @return: URI - @rtype: str - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.lookupNode(self.caller_id, node_name)) - - def getPublishedTopics(self, subgraph): - """ - Get list of topics that can be subscribed to. This does not return topics that have no publishers. - See L{getSystemState()} to get more comprehensive list. - @param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace - is resolved relative to the caller's namespace. Use '' to specify all names. - @type subgraph: str - @return: [[topic1, type1]...[topicN, typeN]] - @rtype: [[str, str],] - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.getPublishedTopics(self.caller_id, subgraph)) - - def getTopicTypes(self): - """ - Retrieve list topic names and their types. - - New in ROS 1.2. - - @rtype: (int, str, [[str,str]] ) - @return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs. - """ - return self._succeed(self.handle.getTopicTypes(self.caller_id)) - - def getSystemState(self): - """ - Retrieve list representation of system state (i.e. publishers, subscribers, and services). - @rtype: [[str,[str]], [str,[str]], [str,[str]]] - @return: systemState - - System state is in list representation:: - [publishers, subscribers, services]. - - publishers is of the form:: - [ [topic1, [topic1Publisher1...topic1PublisherN]] ... ] - - subscribers is of the form:: - [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] - - services is of the form:: - [ [service1, [service1Provider1...service1ProviderN]] ... ] - - @raise rosgraph.masterapi.Error: if Master returns ERROR. - @raise rosgraph.masterapi.Failure: if Master returns FAILURE. - """ - return self._succeed(self.handle.getSystemState(self.caller_id)) diff --git a/src/hal/components/cros/master_python_source/rosgraph/names.py b/src/hal/components/cros/master_python_source/rosgraph/names.py deleted file mode 100644 index b5465eb1..00000000 --- a/src/hal/components/cros/master_python_source/rosgraph/names.py +++ /dev/null @@ -1,329 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id: names.py 14589 2011-08-07 18:30:21Z kwc $ - -""" -Library for manipulating ROS Names. See U{http://ros.org/wiki/Names}. -""" - -import os -import sys - -from .rosenv import ROS_NAMESPACE - -#TODO: why are these here? -MSG_EXT = '.msg' -SRV_EXT = '.srv' - -SEP = '/' -GLOBALNS = '/' -PRIV_NAME = '~' -REMAP = ":=" -ANYTYPE = '*' - -if sys.hexversion > 0x03000000: #Python3 - def isstring(s): - return isinstance(s, str) #Python 3.x -else: - def isstring(s): - """ - Small helper version to check an object is a string in a way that works - for both Python 2 and 3 - """ - return isinstance(s, basestring) #Python 2.x - -def get_ros_namespace(env=None, argv=None): - """ - @param env: environment dictionary (defaults to os.environ) - @type env: dict - @param argv: command-line arguments (defaults to sys.argv) - @type argv: [str] - @return: ROS namespace of current program - @rtype: str - """ - #we force command-line-specified namespaces to be globally scoped - if argv is None: - argv = sys.argv - for a in argv: - if a.startswith('__ns:='): - return make_global_ns(a[len('__ns:='):]) - if env is None: - env = os.environ - return make_global_ns(env.get(ROS_NAMESPACE, GLOBALNS)) - -def make_caller_id(name): - """ - Resolve a local name to the caller ID based on ROS environment settings (i.e. ROS_NAMESPACE) - - @param name: local name to calculate caller ID from, e.g. 'camera', 'node' - @type name: str - @return: caller ID based on supplied local name - @rtype: str - """ - return make_global_ns(ns_join(get_ros_namespace(), name)) - -def make_global_ns(name): - """ - Convert name to a global name with a trailing namespace separator. - - @param name: ROS resource name. Cannot be a ~name. - @type name: str - @return str: name as a global name, e.g. 'foo' -> '/foo/'. - This does NOT resolve a name. - @rtype: str - @raise ValueError: if name is a ~name - """ - if is_private(name): - raise ValueError("cannot turn [%s] into a global name"%name) - if not is_global(name): - name = SEP + name - if name[-1] != SEP: - name = name + SEP - return name - -def is_global(name): - """ - Test if name is a global graph resource name. - - @param name: must be a legal name in canonical form - @type name: str - @return: True if name is a globally referenced name (i.e. /ns/name) - @rtype: bool - """ - return name and name[0] == SEP - -def is_private(name): - """ - Test if name is a private graph resource name. - - @param name: must be a legal name in canonical form - @type name: str - @return bool: True if name is a privately referenced name (i.e. ~name) - """ - return name and name[0] == PRIV_NAME - -def namespace(name): - """ - Get the namespace of name. The namespace is returned with a - trailing slash in order to favor easy concatenation and easier use - within the global context. - - @param name: name to return the namespace of. Must be a legal - name. NOTE: an empty name will return the global namespace. - @type name: str - @return str: Namespace of name. For example, '/wg/node1' returns '/wg/'. The - global namespace is '/'. - @rtype: str - @raise ValueError: if name is invalid - """ - "map name to its namespace" - if name is None: - raise ValueError('name') - if not isstring(name): - raise TypeError('name') - if not name: - return SEP - elif name[-1] == SEP: - name = name[:-1] - return name[:name.rfind(SEP)+1] or SEP - -def ns_join(ns, name): - """ - Join a namespace and name. If name is unjoinable (i.e. ~private or - /global) it will be returned without joining - - @param ns: namespace ('/' and '~' are both legal). If ns is the empty string, name will be returned. - @type ns: str - @param name str: a legal name - @return str: name concatenated to ns, or name if it is - unjoinable. - @rtype: str - """ - if is_private(name) or is_global(name): - return name - if ns == PRIV_NAME: - return PRIV_NAME + name - if not ns: - return name - if ns[-1] == SEP: - return ns + name - return ns + SEP + name - -def load_mappings(argv): - """ - Load name mappings encoded in command-line arguments. This will filter - out any parameter assignment mappings. - - @param argv: command-line arguments - @type argv: [str] - @return: name->name remappings. - @rtype: dict {str: str} - """ - mappings = {} - for arg in argv: - if REMAP in arg: - try: - src, dst = [x.strip() for x in arg.split(REMAP)] - if src and dst: - if len(src) > 1 and src[0] == '_' and src[1] != '_': - #ignore parameter assignment mappings - pass - else: - mappings[src] = dst - except: - #TODO: remove - sys.stderr.write("ERROR: Invalid remapping argument '%s'\n"%arg) - return mappings - - -################################################################################ -# NAME VALIDATORS - -import re - -#~,/, or ascii char followed by (alphanumeric, _, /) -NAME_LEGAL_CHARS_P = re.compile('^[\~\/A-Za-z][\w\/]*$') -def is_legal_name(name): - """ - Check if name is a legal ROS name for graph resources - (alphabetical character followed by alphanumeric, underscore, or - forward slashes). This constraint is currently not being enforced, - but may start getting enforced in later versions of ROS. - - @param name: Name - @type name: str - """ - # should we enforce unicode checks? - if name is None: - return False - # empty string is a legal name as it resolves to namespace - if name == '': - return True - m = NAME_LEGAL_CHARS_P.match(name) - return m is not None and m.group(0) == name and not '//' in name - -BASE_NAME_LEGAL_CHARS_P = re.compile('^[A-Za-z][\w]*$') #ascii char followed by (alphanumeric, _) -def is_legal_base_name(name): - """ - Validates that name is a legal base name for a graph resource. A base name has - no namespace context, e.g. "node_name". - """ - if name is None: - return False - m = BASE_NAME_LEGAL_CHARS_P.match(name) - return m is not None and m.group(0) == name - -def canonicalize_name(name): - """ - Put name in canonical form. Extra slashes '//' are removed and - name is returned without any trailing slash, e.g. /foo/bar - @param name: ROS name - @type name: str - """ - if not name or name == SEP: - return name - elif name[0] == SEP: - return '/' + '/'.join([x for x in name.split(SEP) if x]) - else: - return '/'.join([x for x in name.split(SEP) if x]) - -def resolve_name(name, namespace_, remappings=None): - """ - Resolve a ROS name to its global, canonical form. Private ~names - are resolved relative to the node name. - - @param name: name to resolve. - @type name: str - @param namespace_: node name to resolve relative to. - @type namespace_: str - @param remappings: Map of resolved remappings. Use None to indicate no remapping. - @return: Resolved name. If name is empty/None, resolve_name - returns parent namespace_. If namespace_ is empty/None, - @rtype: str - """ - if not name: #empty string resolves to parent of the namespace_ - return namespace(namespace_) - - name = canonicalize_name(name) - if name[0] == SEP: #global name - resolved_name = name - elif is_private(name): #~name - # #3044: be careful not to accidentally make rest of name global - resolved_name = canonicalize_name(namespace_ + SEP + name[1:]) - else: #relative - resolved_name = namespace(namespace_) + name - - #Mappings override general namespace-based resolution - # - do this before canonicalization as remappings are meant to - # match the name as specified in the code - if remappings and resolved_name in remappings: - return remappings[resolved_name] - else: - return resolved_name - -def script_resolve_name(script_name, name): - """ - Name resolver for scripts. Supports :envvar:`ROS_NAMESPACE`. Does not - support remapping arguments. - - :param name: name to resolve, ``str`` - :param script_name: name of script. script_name must not - contain a namespace., ``str`` - :returns: resolved name, ``str`` - """ - if not name: #empty string resolves to namespace - return get_ros_namespace() - #Check for global name: /foo/name resolves to /foo/name - if is_global(name): - return name - #Check for private name: ~name resolves to /caller_id/name - elif is_private(name): - return ns_join(make_caller_id(script_name), name[1:]) - return get_ros_namespace() + name - -def anonymous_name(id): - """ - Generate a ROS-legal 'anonymous' name - - @param id: prefix for anonymous name - @type id: str - """ - import socket, random - name = "%s_%s_%s_%s"%(id, socket.gethostname(), os.getpid(), random.randint(0, sys.maxsize)) - # RFC 952 allows hyphens, IP addrs can have '.'s, both - # of which are illegal for ROS names. For good - # measure, screen ipv6 ':'. - name = name.replace('.', '_') - name = name.replace('-', '_') - return name.replace(':', '_') - diff --git a/src/hal/components/cros/master_python_source/rosgraph/network.py b/src/hal/components/cros/master_python_source/rosgraph/network.py deleted file mode 100644 index b6670048..00000000 --- a/src/hal/components/cros/master_python_source/rosgraph/network.py +++ /dev/null @@ -1,419 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id: network.py 15125 2011-10-06 02:51:15Z kwc $ - -""" -Network APIs for ROS-based systems, including IP address and ROS -TCP header libraries. Because ROS-based runtimes must respect the -ROS_IP and ROS_HOSTNAME environment variables, ROS-specific APIs -are necessary for correctly retrieving local IP address -information. -""" - -import logging -import os -import socket -import struct -import sys -import platform - -try: - from cStringIO import StringIO #Python 2.x - python3 = 0 -except ImportError: - from io import BytesIO #Python 3.x - python3 = 1 - -try: - import urllib.parse as urlparse -except ImportError: - import urlparse - -from .rosenv import ROS_IP, ROS_HOSTNAME, ROS_IPV6 - -SIOCGIFCONF = 0x8912 -SIOCGIFADDR = 0x8915 -if platform.system() == 'FreeBSD': - SIOCGIFADDR = 0xc0206921 - if platform.architecture()[0] == '64bit': - SIOCGIFCONF = 0xc0106924 - else: - SIOCGIFCONF = 0xc0086924 - -logger = logging.getLogger('rosgraph.network') - -def parse_http_host_and_port(url): - """ - Convenience routine to handle parsing and validation of HTTP URL - port due to the fact that Python only provides easy accessors in - Python 2.5 and later. Validation checks that the protocol and host - are set. - - :param url: URL to parse, ``str`` - :returns: hostname and port number in URL or 80 (default), ``(str, int)`` - :raises: :exc:`ValueError` If the url does not validate - """ - if not url: - raise ValueError('not a valid URL') - p = urlparse.urlparse(url) - if not p.scheme or not p.hostname: - raise ValueError('not a valid URL') - port = p.port if p.port else 80 - return p.hostname, port - -def _is_unix_like_platform(): - """ - :returns: true if the platform conforms to UNIX/POSIX-style APIs - @rtype: bool - """ - return platform.system() in ['Linux', 'FreeBSD', 'Darwin'] - -def get_address_override(): - """ - :returns: ROS_IP/ROS_HOSTNAME override or None, ``str`` - :raises: :exc:`ValueError` If ROS_IP/ROS_HOSTNAME/__ip/__hostname are invalidly specified - """ - # #998: check for command-line remappings first - # TODO IPV6: check for compatibility - for arg in sys.argv: - if arg.startswith('__hostname:=') or arg.startswith('__ip:='): - try: - _, val = arg.split(':=') - return val - except: #split didn't unpack properly - raise ValueError("invalid ROS command-line remapping argument '%s'"%arg) - - # check ROS_HOSTNAME and ROS_IP environment variables, which are - # aliases for each other - if ROS_HOSTNAME in os.environ: - hostname = os.environ[ROS_HOSTNAME] - if hostname == '': - msg = 'invalid ROS_HOSTNAME (an empty string)' - sys.stderr.write(msg + '\n') - logger.warn(msg) - else: - parts = urlparse.urlparse(hostname) - if parts.scheme: - msg = 'invalid ROS_HOSTNAME (protocol ' + ('and port ' if parts.port else '') + 'should not be included)' - sys.stderr.write(msg + '\n') - logger.warn(msg) - elif hostname.find(':') != -1: - # this can not be checked with urlparse() - # since it does not extract the port for a hostname like "foo:1234" - msg = 'invalid ROS_HOSTNAME (port should not be included)' - sys.stderr.write(msg + '\n') - logger.warn(msg) - return hostname - elif ROS_IP in os.environ: - ip = os.environ[ROS_IP] - if ip == '': - msg = 'invalid ROS_IP (an empty string)' - sys.stderr.write(msg + '\n') - logger.warn(msg) - elif ip.find('://') != -1: - msg = 'invalid ROS_IP (protocol should not be included)' - sys.stderr.write(msg + '\n') - logger.warn(msg) - elif ip.find('.') != -1 and ip.rfind(':') > ip.rfind('.'): - msg = 'invalid ROS_IP (port should not be included)' - sys.stderr.write(msg + '\n') - logger.warn(msg) - elif ip.find('.') == -1 and ip.find(':') == -1: - msg = 'invalid ROS_IP (must be a valid IPv4 or IPv6 address)' - sys.stderr.write(msg + '\n') - logger.warn(msg) - return ip - return None - -def is_local_address(hostname): - """ - :param hostname: host name/address, ``str`` - :returns True: if hostname maps to a local address, False otherwise. False conditions include invalid hostnames. - """ - try: - if use_ipv6(): - reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, 0, 0, socket.SOL_TCP)] - else: - reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, socket.AF_INET, 0, socket.SOL_TCP)] - except socket.error: - return False - local_addresses = ['localhost'] + get_local_addresses() - # 127. check is due to #1260 - if ([ip for ip in reverse_ips if (ip.startswith('127.') or ip == '::1')] != []) or (set(reverse_ips) & set(local_addresses) != set()): - return True - return False - -def get_local_address(): - """ - :returns: default local IP address (e.g. eth0). May be overriden by ROS_IP/ROS_HOSTNAME/__ip/__hostname, ``str`` - """ - override = get_address_override() - if override: - return override - addrs = get_local_addresses() - if len(addrs) == 1: - return addrs[0] - for addr in addrs: - # pick first non 127/8 address - if not addr.startswith('127.') and not addr == '::1': - return addr - else: # loopback - if use_ipv6(): - return '::1' - else: - return '127.0.0.1' - -# cache for performance reasons -_local_addrs = None -def get_local_addresses(): - """ - :returns: known local addresses. Not affected by ROS_IP/ROS_HOSTNAME, ``[str]`` - """ - # cache address data as it can be slow to calculate - global _local_addrs - if _local_addrs is not None: - return _local_addrs - - local_addrs = None - if _is_unix_like_platform(): - # unix-only branch - v4addrs = [] - v6addrs = [] - import netifaces - for iface in netifaces.interfaces(): - try: - ifaddrs = netifaces.ifaddresses(iface) - except ValueError: - # even if interfaces() returns an interface name - # ifaddresses() might raise a ValueError - # https://bugs.launchpad.net/ubuntu/+source/netifaces/+bug/753009 - continue - if socket.AF_INET in ifaddrs: - v4addrs.extend([addr['addr'] for addr in ifaddrs[socket.AF_INET]]) - if socket.AF_INET6 in ifaddrs: - v6addrs.extend([addr['addr'] for addr in ifaddrs[socket.AF_INET6]]) - if use_ipv6(): - local_addrs = v6addrs + v4addrs - else: - local_addrs = v4addrs - else: - # cross-platform branch, can only resolve one address - if use_ipv6(): - local_addrs = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, 0, 0, socket.SOL_TCP)] - else: - local_addrs = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, socket.AF_INET, 0, socket.SOL_TCP)] - _local_addrs = local_addrs - return local_addrs - -def use_ipv6(): - return ROS_IPV6 in os.environ and os.environ[ROS_IPV6] == 'on' - -def get_bind_address(address=None): - """ - :param address: (optional) address to compare against, ``str`` - :returns: address TCP/IP sockets should use for binding. This is - generally 0.0.0.0, but if \a address or ROS_IP/ROS_HOSTNAME is set - to localhost it will return 127.0.0.1, ``str`` - """ - if address is None: - address = get_address_override() - if address and (address == 'localhost' or address.startswith('127.') or address == '::1' ): - #localhost or 127/8 - if use_ipv6(): - return '::1' - elif address.startswith('127.'): - return address - else: - return '127.0.0.1' #loopback - else: - if use_ipv6(): - return '::' - else: - return '0.0.0.0' - -# #528: semi-complicated logic for determining XML-RPC URI -def get_host_name(): - """ - Determine host-name for use in host-name-based addressing (e.g. XML-RPC URIs): - - if ROS_IP/ROS_HOSTNAME is set, use that address - - if the hostname returns a non-localhost value, use that - - use whatever L{get_local_address()} returns - """ - hostname = get_address_override() - if not hostname: - try: - hostname = socket.gethostname() - except: - pass - if not hostname or hostname == 'localhost' or hostname.startswith('127.'): - hostname = get_local_address() - return hostname - -def create_local_xmlrpc_uri(port): - """ - Determine the XMLRPC URI for local servers. This handles the search - logic of checking ROS environment variables, the known hostname, - and local interface IP addresses to determine the best possible - URI. - - :param port: port that server is running on, ``int`` - :returns: XMLRPC URI, ``str`` - """ - #TODO: merge logic in rosgraph.xmlrpc with this routine - # in the future we may not want to be locked to http protocol nor root path - return 'http://%s:%s/'%(get_host_name(), port) - - -## handshake utils ########################################### - -class ROSHandshakeException(Exception): - """ - Exception to represent errors decoding handshake - """ - pass - -def decode_ros_handshake_header(header_str): - """ - Decode serialized ROS handshake header into a Python dictionary - - header is a list of string key=value pairs, each prefixed by a - 4-byte length field. It is preceeded by a 4-byte length field for - the entire header. - - :param header_str: encoded header string. May contain extra data at the end, ``str`` - :returns: key value pairs encoded in \a header_str, ``{str: str}`` - """ - (size, ) = struct.unpack(' header_len: - raise ROSHandshakeException("Incomplete header. Expected %s bytes but only have %s"%((size+4), header_len)) - - d = {} - start = 4 - while start < size: - (field_size, ) = struct.unpack(' size: - raise ROSHandshakeException("Invalid line length in handshake header: %s"%size) - line = header_str[start-field_size:start] - - #python3 compatibility - if python3 == 1: - line = line.decode() - - idx = line.find("=") - if idx < 0: - raise ROSHandshakeException("Invalid line in handshake header: [%s]"%line) - key = line[:idx] - value = line[idx+1:] - d[key.strip()] = value - return d - -def read_ros_handshake_header(sock, b, buff_size): - """ - Read in tcpros header off the socket \a sock using buffer \a b. - - :param sock: socket must be in blocking mode, ``socket`` - :param b: buffer to use, ``StringIO`` for Python2, ``BytesIO`` for Python 3 - :param buff_size: incoming buffer size to use, ``int`` - :returns: key value pairs encoded in handshake, ``{str: str}`` - :raises: :exc:`ROSHandshakeException` If header format does not match expected - """ - header_str = None - while not header_str: - d = sock.recv(buff_size) - if not d: - raise ROSHandshakeException("connection from sender terminated before handshake header received. %s bytes were received. Please check sender for additional details."%b.tell()) - b.write(d) - btell = b.tell() - if btell > 4: - # most likely we will get the full header in the first recv, so - # not worth tiny optimizations possible here - bval = b.getvalue() - (size,) = struct.unpack('= size: - header_str = bval - - # memmove the remnants of the buffer back to the start - leftovers = bval[size+4:] - b.truncate(len(leftovers)) - b.seek(0) - b.write(leftovers) - header_recvd = True - - # process the header - return decode_ros_handshake_header(bval) - -def encode_ros_handshake_header(header): - """ - Encode ROS handshake header as a byte string. Each header - field is a string key value pair. The encoded header is - prefixed by a length field, as is each field key/value pair. - key/value pairs a separated by a '=' equals sign. - - FORMAT: (4-byte length + [4-byte field length + field=value ]*) - - :param header: header field keys/values, ``dict`` - :returns: header encoded as byte string, ``bytes`` - """ - str_cls = str if python3 else unicode - - # encode all unicode keys in the header. Ideally, the type of these would be specified by the api - encoded_header = {} - for k, v in header.items(): - if isinstance(k, str_cls): - k = k.encode('utf-8') - if isinstance(v, str_cls): - v = v.encode('utf-8') - encoded_header[k] = v - - fields = [k + b"=" + v for k, v in sorted(encoded_header.items())] - s = b''.join([struct.pack(' (3, 2): - # Dummy last argument to match Python3 return type - return co.co_filename, f.f_lineno, func_name, None - else: - return co.co_filename, f.f_lineno, func_name - -logging.setLoggerClass(RospyLogger) - -def renew_latest_logdir(logfile_dir): - log_dir = os.path.dirname(logfile_dir) - latest_dir = os.path.join(log_dir, 'latest') - if os.path.lexists(latest_dir): - if not os.path.islink(latest_dir): - return False - os.remove(latest_dir) - os.symlink(logfile_dir, latest_dir) - return True - -def configure_logging(logname, level=logging.INFO, filename=None, env=None): - """ - Configure Python logging package to send log files to ROS-specific log directory - :param logname str: name of logger, ``str`` - :param filename: filename to log to. If not set, a log filename - will be generated using logname, ``str`` - :param env: override os.environ dictionary, ``dict`` - :returns: log file name, ``str`` - :raises: :exc:`LoggingException` If logging cannot be configured as specified - """ - if env is None: - env = os.environ - - logname = logname or 'unknown' - log_dir = rospkg.get_log_dir(env=env) - - # if filename is not explicitly provided, generate one using logname - if not filename: - log_filename = os.path.join(log_dir, '%s-%s.log'%(logname, os.getpid())) - else: - log_filename = os.path.join(log_dir, filename) - - logfile_dir = os.path.dirname(log_filename) - if not os.path.exists(logfile_dir): - try: - makedirs_with_parent_perms(logfile_dir) - except OSError: - # cannot print to screen because command-line tools with output use this - if os.path.exists(logfile_dir): - # We successfully created the logging folder, but could not change - # permissions of the new folder to the same as the parent folder - sys.stderr.write("WARNING: Could not change permissions for folder [%s], make sure that the parent folder has correct permissions.\n"%logfile_dir) - else: - # Could not create folder - sys.stderr.write("WARNING: cannot create log directory [%s]. Please set %s to a writable location.\n"%(logfile_dir, ROS_LOG_DIR)) - return None - elif os.path.isfile(logfile_dir): - raise LoggingException("Cannot save log files: file [%s] is in the way"%logfile_dir) - - # the log dir itself should not be symlinked as latest - if logfile_dir != log_dir: - if sys.platform not in ['win32']: - try: - success = renew_latest_logdir(logfile_dir) - if not success: - sys.stderr.write("INFO: cannot create a symlink to latest log directory\n") - except OSError as e: - sys.stderr.write("INFO: cannot create a symlink to latest log directory: %s\n" % e) - - config_file = os.environ.get('ROS_PYTHON_LOG_CONFIG_FILE') - if not config_file: - # search for logging config file in ROS_HOME, ROS_ETC_DIR or relative - # to the rosgraph package directory. - rosgraph_d = rospkg.RosPack().get_path('rosgraph') - for config_dir in [os.path.join(rospkg.get_ros_home(), 'config'), - rospkg.get_etc_ros_dir(), - os.path.join(rosgraph_d, 'conf')]: - for extension in ('conf', 'yaml'): - f = os.path.join(config_dir, - 'python_logging{}{}'.format(os.path.extsep, - extension)) - if os.path.isfile(f): - config_file = f - break - if config_file is not None: - break - - if config_file is None or not os.path.isfile(config_file): - # logging is considered soft-fail - sys.stderr.write("WARNING: cannot load logging configuration file, logging is disabled\n") - logging.getLogger(logname).setLevel(logging.CRITICAL) - return log_filename - - # pass in log_filename as argument to pylogging.conf - os.environ['ROS_LOG_FILENAME'] = log_filename - if config_file.endswith(('.yaml', '.yml')): - with open(config_file) as f: - dict_conf = yaml.safe_load(f) - dict_conf.setdefault('version', 1) - logging.config.dictConfig(dict_conf) - else: - # #3625: disabling_existing_loggers=False - logging.config.fileConfig(config_file, disable_existing_loggers=False) - - return log_filename - - -def makedirs_with_parent_perms(p): - """ - Create the directory using the permissions of the nearest - (existing) parent directory. This is useful for logging, where a - root process sometimes has to log in the user's space. - :param p: directory to create, ``str`` - """ - p = os.path.abspath(p) - parent = os.path.dirname(p) - # recurse upwards, checking to make sure we haven't reached the - # top - if not os.path.exists(p) and p and parent != p: - makedirs_with_parent_perms(parent) - s = os.stat(parent) - os.mkdir(p) - - # if perms of new dir don't match, set anew - s2 = os.stat(p) - if s.st_uid != s2.st_uid or s.st_gid != s2.st_gid: - os.chown(p, s.st_uid, s.st_gid) - if s.st_mode != s2.st_mode: - os.chmod(p, s.st_mode) - -_logging_to_rospy_names = { - 'DEBUG': ('DEBUG', '\033[32m'), - 'INFO': ('INFO', None), - 'WARNING': ('WARN', '\033[33m'), - 'ERROR': ('ERROR', '\033[31m'), - 'CRITICAL': ('FATAL', '\033[31m') -} -_color_reset = '\033[0m' -_defaultFormatter = logging.Formatter() - -class RosStreamHandler(logging.Handler): - def __init__(self, colorize=True, stdout=None, stderr=None): - super(RosStreamHandler, self).__init__() - self._stdout = stdout or sys.stdout - self._stderr = stderr or sys.stderr - self._colorize = colorize - try: - from rospy.rostime import get_time, is_wallclock - self._get_time = get_time - self._is_wallclock = is_wallclock - except ImportError: - self._get_time = None - self._is_wallclock = None - - def emit(self, record): - level, color = _logging_to_rospy_names[record.levelname] - record_message = _defaultFormatter.format(record) - msg = os.environ.get( - 'ROSCONSOLE_FORMAT', '[${severity}] [${time}]: ${message}') - msg = msg.replace('${severity}', level) - msg = msg.replace('${message}', str(record_message)) - msg = msg.replace('${walltime}', '%f' % time.time()) - msg = msg.replace('${thread}', str(record.thread)) - msg = msg.replace('${logger}', str(record.name)) - msg = msg.replace('${file}', str(record.pathname)) - msg = msg.replace('${line}', str(record.lineno)) - msg = msg.replace('${function}', str(record.funcName)) - try: - from rospy import get_name - node_name = get_name() - except ImportError: - node_name = '' - msg = msg.replace('${node}', node_name) - time_str = '%f' % time.time() - if self._get_time is not None and not self._is_wallclock(): - time_str += ', %f' % self._get_time() - msg = msg.replace('${time}', time_str) - msg += '\n' - if record.levelno < logging.WARNING: - self._write(self._stdout, msg, color) - else: - self._write(self._stderr, msg, color) - - def _write(self, fd, msg, color): - if self._colorize and color and hasattr(fd, 'isatty') and fd.isatty(): - msg = color + msg + _color_reset - fd.write(msg) diff --git a/src/hal/components/cros/master_python_source/rosgraph/xmlrpc.py b/src/hal/components/cros/master_python_source/rosgraph/xmlrpc.py deleted file mode 100644 index c8ff8022..00000000 --- a/src/hal/components/cros/master_python_source/rosgraph/xmlrpc.py +++ /dev/null @@ -1,304 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id: xmlrpc.py 15336 2011-11-07 20:43:00Z kwc $ - -from __future__ import print_function - -""" -Common XML-RPC for higher-level libraries running XML-RPC libraries in -ROS. In particular, this library provides common handling for URI -calculation based on ROS environment variables. - -The common entry point for most libraries is the L{XmlRpcNode} class. -""" - -import errno -import logging -import select -import socket - -try: - import _thread -except ImportError: - import thread as _thread - -import traceback - -try: - from xmlrpc.server import SimpleXMLRPCServer, SimpleXMLRPCRequestHandler #Python 3.x -except ImportError: - from SimpleXMLRPCServer import SimpleXMLRPCServer #Python 2.x - from SimpleXMLRPCServer import SimpleXMLRPCRequestHandler #Python 2.x - -try: - import socketserver -except ImportError: - import SocketServer as socketserver - -import rosgraph.network - -def isstring(s): - """Small helper version to check an object is a string in a way that works - for both Python 2 and 3 - """ - try: - return isinstance(s, basestring) - except NameError: - return isinstance(s, str) - -class SilenceableXMLRPCRequestHandler(SimpleXMLRPCRequestHandler): - - protocol_version = 'HTTP/1.1' - - def log_message(self, format, *args): - if 0: - SimpleXMLRPCRequestHandler.log_message(self, format, *args) - -class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, SimpleXMLRPCServer): - """ - Adds ThreadingMixin to SimpleXMLRPCServer to support multiple concurrent - requests via threading. Also makes logging toggleable. - """ - - daemon_threads = True - - def __init__(self, addr, log_requests=1): - """ - Overrides SimpleXMLRPCServer to set option to allow_reuse_address. - """ - # allow_reuse_address defaults to False in Python 2.4. We set it - # to True to allow quick restart on the same port. This is equivalent - # to calling setsockopt(SOL_SOCKET,SO_REUSEADDR,1) - self.allow_reuse_address = True - # Increase request_queue_size to handle issues with many simultaneous - # connections in OSX 10.11 - self.request_queue_size = min(socket.SOMAXCONN, 128) - if rosgraph.network.use_ipv6(): - logger = logging.getLogger('xmlrpc') - # The XMLRPC library does not support IPv6 out of the box - # We have to monipulate private members and duplicate - # code from the constructor. - # TODO IPV6: Get this into SimpleXMLRPCServer - SimpleXMLRPCServer.__init__(self, addr, SilenceableXMLRPCRequestHandler, log_requests, bind_and_activate=False) - self.address_family = socket.AF_INET6 - self.socket = socket.socket(self.address_family, self.socket_type) - logger.info('binding ipv6 xmlrpc socket to' + str(addr)) - # TODO: set IPV6_V6ONLY to 0: - # self.socket.setsockopt(socket.IPPROTO_IPV6, socket.IPV6_V6ONLY, 0) - self.server_bind() - self.server_activate() - logger.info('bound to ' + str(self.socket.getsockname()[0:2])) - else: - SimpleXMLRPCServer.__init__(self, addr, SilenceableXMLRPCRequestHandler, log_requests) - - def handle_error(self, request, client_address): - """ - override ThreadingMixin, which sends errors to stderr - """ - if logging and traceback: - logger = logging.getLogger('xmlrpc') - if logger: - logger.error(traceback.format_exc()) - -#class ForkingXMLRPCServer(socketserver.ForkingMixIn, SimpleXMLRPCServer): -# """ -# Adds ThreadingMixin to SimpleXMLRPCServer to support multiple concurrent -# requests via forking. Also makes logging toggleable. -# """ -# def __init__(self, addr, request_handler=SilenceableXMLRPCRequestHandler, log_requests=1): -# SimpleXMLRPCServer.__init__(self, addr, request_handler, log_requests) - - -class XmlRpcHandler(object): - """ - Base handler API for handlers used with XmlRpcNode. Public methods will be - exported as XML RPC methods. - """ - - def _ready(self, uri): - """ - callback into handler to inform it of XML-RPC URI - """ - pass - - def _shutdown(self, reason): - """ - callback into handler to inform it of shutdown - """ - pass - -class XmlRpcNode(object): - """ - Generic XML-RPC node. Handles the additional complexity of binding - an XML-RPC server to an arbitrary port. - XmlRpcNode is initialized when the uri field has a value. - """ - - def __init__(self, port=0, rpc_handler=None, on_run_error=None): - """ - XML RPC Node constructor - :param port: port to use for starting XML-RPC API. Set to 0 or omit to bind to any available port, ``int`` - :param rpc_handler: XML-RPC API handler for node, `XmlRpcHandler` - :param on_run_error: function to invoke if server.run() throws - Exception. Server always terminates if run() throws, but this - enables cleanup routines to be invoked if server goes down, as - well as include additional debugging. ``fn(Exception)`` - """ - super(XmlRpcNode, self).__init__() - - self.handler = rpc_handler - self.uri = None # initialize the property now so it can be tested against, will be filled in later - self.server = None - if port and isstring(port): - port = int(port) - self.port = port - self.is_shutdown = False - self.on_run_error = on_run_error - - def shutdown(self, reason): - """ - Terminate i/o connections for this server. - - :param reason: human-readable debug string, ``str`` - """ - self.is_shutdown = True - if self.server: - server = self.server - handler = self.handler - self.handler = self.server = self.port = self.uri = None - if handler: - handler._shutdown(reason) - if server: - server.socket.close() - server.server_close() - - def start(self): - """ - Initiate a thread to run the XML RPC server. Uses thread.start_new_thread. - """ - _thread.start_new_thread(self.run, ()) - - def set_uri(self, uri): - """ - Sets the XML-RPC URI. Defined as a separate method as a hood - for subclasses to bootstrap initialization. Should not be called externally. - - :param uri: XMLRPC URI, ``str`` - """ - self.uri = uri - - def run(self): - try: - self._run() - except Exception as e: - if self.is_shutdown: - pass - elif self.on_run_error is not None: - self.on_run_error(e) - else: - raise - - # separated out for easier testing - def _run_init(self): - logger = logging.getLogger('xmlrpc') - try: - log_requests = 0 - port = self.port or 0 #0 = any - - bind_address = rosgraph.network.get_bind_address() - logger.info("XML-RPC server binding to %s:%d" % (bind_address, port)) - - self.server = ThreadingXMLRPCServer((bind_address, port), log_requests) - self.port = self.server.server_address[1] #set the port to whatever server bound to - if not self.port: - self.port = self.server.socket.getsockname()[1] #Python 2.4 - - assert self.port, "Unable to retrieve local address binding" - - # #528: semi-complicated logic for determining XML-RPC URI - # - if ROS_IP/ROS_HOSTNAME is set, use that address - # - if the hostname returns a non-localhost value, use that - # - use whatever rosgraph.network.get_local_address() returns - uri = None - override = rosgraph.network.get_address_override() - if override: - uri = 'http://%s:%s/'%(override, self.port) - else: - try: - hostname = socket.gethostname() - if hostname and not hostname == 'localhost' and not hostname.startswith('127.') and hostname != '::': - uri = 'http://%s:%s/'%(hostname, self.port) - except: - pass - if not uri: - uri = 'http://%s:%s/'%(rosgraph.network.get_local_address(), self.port) - self.set_uri(uri) - - logger.info("Started XML-RPC server [%s]", self.uri) - - self.server.register_multicall_functions() - self.server.register_instance(self.handler) - - except socket.error as e: - if e.errno == errno.EADDRINUSE: - msg = "ERROR: Unable to start XML-RPC server, port %s is already in use"%self.port - else: - msg = "ERROR: Unable to start XML-RPC server: %s" % e.strerror - logger.error(msg) - print(msg) - raise #let higher level catch this - - if self.handler is not None: - self.handler._ready(self.uri) - logger.info("xml rpc node: starting XML-RPC server") - - def _run(self): - """ - Main processing thread body. - :raises: :exc:`socket.error` If server cannot bind - - """ - self._run_init() - while not self.is_shutdown: - try: - self.server.serve_forever() - except (IOError, select.error) as e: - # check for interrupted call, which can occur if we're - # embedded in a program using signals. All other - # exceptions break _run. - if self.is_shutdown: - pass - elif e.errno != errno.EINTR: - self.is_shutdown = True - logging.getLogger('xmlrpc').error("serve forever IOError: %s, %s"%(e.errno, e.strerror)) - diff --git a/src/hal/components/cros/master_python_source/rosmaster/__init__.py b/src/hal/components/cros/master_python_source/rosmaster/__init__.py deleted file mode 100644 index a56ceb75..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/__init__.py +++ /dev/null @@ -1,38 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2010, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -from .main import rosmaster_main - -from .master import DEFAULT_MASTER_PORT - diff --git a/src/hal/components/cros/master_python_source/rosmaster/exceptions.py b/src/hal/components/cros/master_python_source/rosmaster/exceptions.py deleted file mode 100644 index 0120de16..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/exceptions.py +++ /dev/null @@ -1,39 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2010, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -""" -Exceptions for rosmaster package. -""" - -class InternalException(Exception): pass diff --git a/src/hal/components/cros/master_python_source/rosmaster/main.py b/src/hal/components/cros/master_python_source/rosmaster/main.py deleted file mode 100644 index ae94dd18..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/main.py +++ /dev/null @@ -1,139 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -"""Command-line handler for ROS zenmaster (Python Master)""" - -import logging -import os -import sys -import time -import optparse - -import rosmaster.master -from rosmaster.master_api import NUM_WORKERS - -def configure_logging(): - """ - Setup filesystem logging for the master - """ - filename = 'master.log' - # #988 __log command-line remapping argument - import rosgraph.names - import rosgraph.roslogging - mappings = rosgraph.names.load_mappings(sys.argv) - if '__log' in mappings: - logfilename_remap = mappings['__log'] - filename = os.path.abspath(logfilename_remap) - _log_filename = rosgraph.roslogging.configure_logging('rosmaster', logging.DEBUG, filename=filename) - -def rosmaster_main(argv=sys.argv, stdout=sys.stdout, env=os.environ): - parser = optparse.OptionParser(usage="usage: zenmaster [options]") - parser.add_option("--core", - dest="core", action="store_true", default=False, - help="run as core") - parser.add_option("-p", "--port", - dest="port", default=0, - help="override port", metavar="PORT") - parser.add_option("-w", "--numworkers", - dest="num_workers", default=NUM_WORKERS, type=int, - help="override number of worker threads", metavar="NUM_WORKERS") - parser.add_option("-t", "--timeout", - dest="timeout", - help="override the socket connection timeout (in seconds).", metavar="TIMEOUT") - parser.add_option("--master-logger-level", - dest="master_logger_level", default=False, type=str, - help="set rosmaster.master logger level ('debug', 'info', 'warn', 'error', 'fatal')") - - options, args = parser.parse_args(argv[1:]) - - # only arg that zenmaster supports is __log remapping of logfilename - for arg in args: - if not arg.startswith('__log:='): - parser.error("unrecognized arg: %s"%arg) - configure_logging() - - port = rosmaster.master.DEFAULT_MASTER_PORT - if options.port: - port = int(options.port) - - if not options.core: - print(""" - - -ACHTUNG WARNING ACHTUNG WARNING ACHTUNG -WARNING ACHTUNG WARNING ACHTUNG WARNING - - -Standalone zenmaster has been deprecated, please use 'roscore' instead - - -ACHTUNG WARNING ACHTUNG WARNING ACHTUNG -WARNING ACHTUNG WARNING ACHTUNG WARNING - - -""") - - logger = logging.getLogger("rosmaster.main") - logger.info("initialization complete, waiting for shutdown") - - if options.timeout is not None and float(options.timeout) >= 0.0: - logger.info("Setting socket timeout to %s" % options.timeout) - import socket - socket.setdefaulttimeout(float(options.timeout)) - - if options.master_logger_level: - levels = {'debug': logging.DEBUG, 'info': logging.INFO, 'warn': logging.WARN, 'error': logging.ERROR, 'fatal': logging.FATAL} - if options.master_logger_level.lower() in levels.keys(): - logger.info("set rosmaster.master logger level '{}'".format(options.master_logger_level)) - rosmaster.master_api.LOG_API = True - logging.getLogger("rosmaster.master").setLevel(levels[options.master_logger_level.lower()]) - else: - logger.error("--master-logger-level received unknown option '{}'".format(options.master_logger_level)) - - try: - logger.info("Starting ROS Master Node") - master = rosmaster.master.Master(port, options.num_workers) - master.start() - - import time - while master.ok(): - time.sleep(.1) - except KeyboardInterrupt: - logger.info("keyboard interrupt, will exit") - finally: - logger.info("stopping master...") - master.stop() - -if __name__ == "__main__": - main() diff --git a/src/hal/components/cros/master_python_source/rosmaster/master.py b/src/hal/components/cros/master_python_source/rosmaster/master.py deleted file mode 100644 index fc8652ac..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/master.py +++ /dev/null @@ -1,89 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -""" -ROS Master. - -This module integrates the lower-level implementation modules into a -single interface for running and stopping the ROS Master. -""" - -import logging -import time - -import rosgraph.xmlrpc - -import rosmaster.master_api - -DEFAULT_MASTER_PORT=11311 #default port for master's to bind to - -class Master(object): - - def __init__(self, port=DEFAULT_MASTER_PORT, num_workers=rosmaster.master_api.NUM_WORKERS): - self.port = port - self.num_workers = num_workers - - def start(self): - """ - Start the ROS Master. - """ - self.handler = None - self.master_node = None - self.uri = None - - handler = rosmaster.master_api.ROSMasterHandler(self.num_workers) - master_node = rosgraph.xmlrpc.XmlRpcNode(self.port, handler) - master_node.start() - - # poll for initialization - while not master_node.uri: - time.sleep(0.0001) - - # save fields - self.handler = handler - self.master_node = master_node - self.uri = master_node.uri - - logging.getLogger('rosmaster.master').info("Master initialized: port[%s], uri[%s]", self.port, self.uri) - - def ok(self): - if self.master_node is not None: - return self.master_node.handler._ok() - else: - return False - - def stop(self): - if self.master_node is not None: - self.master_node.shutdown('Master.stop') - self.master_node = None diff --git a/src/hal/components/cros/master_python_source/rosmaster/master_api.py b/src/hal/components/cros/master_python_source/rosmaster/master_api.py deleted file mode 100644 index c5312618..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/master_api.py +++ /dev/null @@ -1,889 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ -""" -ROS Master API. - -L{ROSMasterHandler} provides the API implementation of the -Master. Python allows an API to be introspected from a Python class, -so the handler has a 1-to-1 mapping with the actual XMLRPC API. - -API return convention: (statusCode, statusMessage, returnValue) - - - statusCode: an integer indicating the completion condition of the method. - - statusMessage: a human-readable string message for debugging - - returnValue: the return value of the method; method-specific. - -Current status codes: - - - -1: ERROR: Error on the part of the caller, e.g. an invalid parameter - - 0: FAILURE: Method was attempted but failed to complete correctly. - - 1: SUCCESS: Method completed successfully. - -Individual methods may assign additional meaning/semantics to statusCode. -""" - -from __future__ import print_function - -import os -import sys -import logging -import threading -import time -import traceback - -from rosgraph.xmlrpc import XmlRpcHandler - -import rosgraph.names -from rosgraph.names import resolve_name -import rosmaster.paramserver -import rosmaster.threadpool - -from rosmaster.util import xmlrpcapi -from rosmaster.registrations import RegistrationManager -from rosmaster.validators import non_empty, non_empty_str, not_none, is_api, is_topic, is_service, valid_type_name, valid_name, empty_or_valid_name, ParameterInvalid - -NUM_WORKERS = 3 #number of threads we use to send publisher_update notifications - -# Return code slots -STATUS = 0 -MSG = 1 -VAL = 2 - -_logger = logging.getLogger("rosmaster.master") - -LOG_API = False - -def mloginfo(msg, *args): - """ - Info-level master log statements. These statements may be printed - to screen so they should be user-readable. - @param msg: Message string - @type msg: str - @param args: arguments for msg if msg is a format string - """ - #mloginfo is in core so that it is accessible to master and masterdata - _logger.info(msg, *args) - -def mlogwarn(msg, *args): - """ - Warn-level master log statements. These statements may be printed - to screen so they should be user-readable. - @param msg: Message string - @type msg: str - @param args: arguments for msg if msg is a format string - """ - #mloginfo is in core so that it is accessible to master and masterdata - _logger.warn(msg, *args) - if args: - print("WARN: " + msg % args) - else: - print("WARN: " + str(msg)) - - -def apivalidate(error_return_value, validators=()): - """ - ROS master/slave arg-checking decorator. Applies the specified - validator to the corresponding argument and also remaps each - argument to be the value returned by the validator. Thus, - arguments can be simultaneously validated and canonicalized prior - to actual function call. - @param error_return_value: API value to return if call unexpectedly fails - @param validators: sequence of validators to apply to each - arg. None means no validation for the parameter is required. As all - api methods take caller_id as the first parameter, the validators - start with the second param. - @type validators: sequence - """ - def check_validates(f): - try: - func_code = f.__code__ - func_name = f.__name__ - except AttributeError: - func_code = f.func_code - func_name = f.func_name - assert len(validators) == func_code.co_argcount - 2, "%s failed arg check"%f #ignore self and caller_id - def validated_f(*args, **kwds): - if LOG_API: - _logger.debug("%s%s", func_name, str(args[1:])) - #print "%s%s"%(func_name, str(args[1:])) - if len(args) == 1: - _logger.error("%s invoked without caller_id paramter" % func_name) - return -1, "missing required caller_id parameter", error_return_value - elif len(args) != func_code.co_argcount: - return -1, "Error: bad call arity", error_return_value - - instance = args[0] - caller_id = args[1] - def isstring(s): - """Small helper version to check an object is a string in - a way that works for both Python 2 and 3 - """ - try: - return isinstance(s, basestring) - except NameError: - return isinstance(s, str) - if not isstring(caller_id): - _logger.error("%s: invalid caller_id param type", func_name) - return -1, "caller_id must be a string", error_return_value - - newArgs = [instance, caller_id] #canonicalized args - try: - for (v, a) in zip(validators, args[2:]): - if v: - try: - newArgs.append(v(a, caller_id)) - except ParameterInvalid as e: - _logger.error("%s: invalid parameter: %s", func_name, str(e) or 'error') - return -1, str(e) or 'error', error_return_value - else: - newArgs.append(a) - - if LOG_API: - retval = f(*newArgs, **kwds) - _logger.debug("%s%s returns %s", func_name, args[1:], retval) - return retval - else: - code, msg, val = f(*newArgs, **kwds) - if val is None: - return -1, "Internal error (None value returned)", error_return_value - return code, msg, val - except TypeError as te: #most likely wrong arg number - _logger.error(traceback.format_exc()) - return -1, "Error: invalid arguments: %s"%te, error_return_value - except Exception as e: #internal failure - _logger.error(traceback.format_exc()) - return 0, "Internal failure: %s"%e, error_return_value - try: - validated_f.__name__ = func_name - except AttributeError: - validated_f.func_name = func_name - validated_f.__doc__ = f.__doc__ #preserve doc - return validated_f - return check_validates - -def publisher_update_task(api, topic, pub_uris): - """ - Contact api.publisherUpdate with specified parameters - @param api: XML-RPC URI of node to contact - @type api: str - @param topic: Topic name to send to node - @type topic: str - @param pub_uris: list of publisher APIs to send to node - @type pub_uris: [str] - """ - msg = "publisherUpdate[%s] -> %s %s" % (topic, api, pub_uris) - mloginfo(msg) - start_sec = time.time() - try: - #TODO: check return value for errors so we can unsubscribe if stale - ret = xmlrpcapi(api).publisherUpdate('/master', topic, pub_uris) - msg_suffix = "result=%s" % ret - except Exception as ex: - msg_suffix = "exception=%s" % ex - raise - finally: - delta_sec = time.time() - start_sec - mloginfo("%s: sec=%0.2f, %s", msg, delta_sec, msg_suffix) - - -def service_update_task(api, service, uri): - """ - Contact api.serviceUpdate with specified parameters - @param api: XML-RPC URI of node to contact - @type api: str - @param service: Service name to send to node - @type service: str - @param uri: URI to send to node - @type uri: str - """ - mloginfo("serviceUpdate[%s, %s] -> %s",service, uri, api) - xmlrpcapi(api).serviceUpdate('/master', service, uri) - -################################################### -# Master Implementation - -class ROSMasterHandler(object): - """ - XML-RPC handler for ROS master APIs. - API routines for the ROS Master Node. The Master Node is a - superset of the Slave Node and contains additional API methods for - creating and monitoring a graph of slave nodes. - - By convention, ROS nodes take in caller_id as the first parameter - of any API call. The setting of this parameter is rarely done by - client code as ros::msproxy::MasterProxy automatically inserts - this parameter (see ros::client::getMaster()). - """ - - def __init__(self, num_workers=NUM_WORKERS): - """ctor.""" - - self.uri = None - self.done = False - - self.thread_pool = rosmaster.threadpool.MarkedThreadPool(num_workers) - # pub/sub/providers: dict { topicName : [publishers/subscribers names] } - self.ps_lock = threading.Condition(threading.Lock()) - - self.reg_manager = RegistrationManager(self.thread_pool) - - # maintain refs to reg_manager fields - self.publishers = self.reg_manager.publishers - self.subscribers = self.reg_manager.subscribers - self.services = self.reg_manager.services - self.param_subscribers = self.reg_manager.param_subscribers - - self.topics_types = {} #dict { topicName : type } - - # parameter server dictionary - self.param_server = rosmaster.paramserver.ParamDictionary(self.reg_manager) - - def _shutdown(self, reason=''): - if self.thread_pool is not None: - self.thread_pool.join_all(wait_for_tasks=False, wait_for_threads=False) - self.thread_pool = None - self.done = True - - def _ready(self, uri): - """ - Initialize the handler with the XMLRPC URI. This is a standard callback from the XmlRpcNode API. - - @param uri: XML-RPC URI - @type uri: str - """ - self.uri = uri - - def _ok(self): - return not self.done - - ############################################################################### - # EXTERNAL API - - @apivalidate(0, (None, )) - def shutdown(self, caller_id, msg=''): - """ - Stop this server - @param caller_id: ROS caller id - @type caller_id: str - @param msg: a message describing why the node is being shutdown. - @type msg: str - @return: [code, msg, 0] - @rtype: [int, str, int] - """ - if msg: - print("shutdown request: %s" % msg, file=sys.stdout) - else: - print("shutdown requst", file=sys.stdout) - self._shutdown('external shutdown request from [%s]: %s'%(caller_id, msg)) - return 1, "shutdown", 0 - - @apivalidate('') - def getUri(self, caller_id): - """ - Get the XML-RPC URI of this server. - @param caller_id str: ROS caller id - @return [int, str, str]: [1, "", xmlRpcUri] - """ - return 1, "", self.uri - - - @apivalidate(-1) - def getPid(self, caller_id): - """ - Get the PID of this server - @param caller_id: ROS caller id - @type caller_id: str - @return: [1, "", serverProcessPID] - @rtype: [int, str, int] - """ - return 1, "", os.getpid() - - - ################################################################ - # PARAMETER SERVER ROUTINES - - @apivalidate(0, (non_empty_str('key'),)) - def deleteParam(self, caller_id, key): - """ - Parameter Server: delete parameter - @param caller_id: ROS caller id - @type caller_id: str - @param key: parameter name - @type key: str - @return: [code, msg, 0] - @rtype: [int, str, int] - """ - try: - key = resolve_name(key, caller_id) - self.param_server.delete_param(key, self._notify_param_subscribers) - mloginfo("-PARAM [%s] by %s",key, caller_id) - return 1, "parameter %s deleted"%key, 0 - except KeyError as e: - return -1, "parameter [%s] is not set"%key, 0 - - @apivalidate(0, (non_empty_str('key'), not_none('value'))) - def setParam(self, caller_id, key, value): - """ - Parameter Server: set parameter. NOTE: if value is a - dictionary it will be treated as a parameter tree, where key - is the parameter namespace. For example::: - {'x':1,'y':2,'sub':{'z':3}} - - will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it - will replace all existing parameters in the key parameter - namespace with the parameters in value. You must set - parameters individually if you wish to perform a union update. - - @param caller_id: ROS caller id - @type caller_id: str - @param key: parameter name - @type key: str - @param value: parameter value. - @type value: XMLRPCLegalValue - @return: [code, msg, 0] - @rtype: [int, str, int] - """ - key = resolve_name(key, caller_id) - self.param_server.set_param(key, value, self._notify_param_subscribers, caller_id) - mloginfo("+PARAM [%s] by %s",key, caller_id) - return 1, "parameter %s set"%key, 0 - - @apivalidate(0, (non_empty_str('key'),)) - def getParam(self, caller_id, key): - """ - Retrieve parameter value from server. - @param caller_id: ROS caller id - @type caller_id: str - @param key: parameter to lookup. If key is a namespace, - getParam() will return a parameter tree. - @type key: str - getParam() will return a parameter tree. - - @return: [code, statusMessage, parameterValue]. If code is not - 1, parameterValue should be ignored. If key is a namespace, - the return value will be a dictionary, where each key is a - parameter in that namespace. Sub-namespaces are also - represented as dictionaries. - @rtype: [int, str, XMLRPCLegalValue] - """ - try: - key = resolve_name(key, caller_id) - return 1, "Parameter [%s]"%key, self.param_server.get_param(key) - except KeyError as e: - return -1, "Parameter [%s] is not set"%key, 0 - - @apivalidate(0, (non_empty_str('key'),)) - def searchParam(self, caller_id, key): - """ - Search for parameter key on parameter server. Search starts in caller's namespace and proceeds - upwards through parent namespaces until Parameter Server finds a matching key. - - searchParam's behavior is to search for the first partial match. - For example, imagine that there are two 'robot_description' parameters:: - - /robot_description - /robot_description/arm - /robot_description/base - /pr2/robot_description - /pr2/robot_description/base - - If I start in the namespace /pr2/foo and search for - 'robot_description', searchParam will match - /pr2/robot_description. If I search for 'robot_description/arm' - it will return /pr2/robot_description/arm, even though that - parameter does not exist (yet). - - @param caller_id str: ROS caller id - @type caller_id: str - @param key: parameter key to search for. - @type key: str - @return: [code, statusMessage, foundKey]. If code is not 1, foundKey should be - ignored. - @rtype: [int, str, str] - """ - search_key = self.param_server.search_param(caller_id, key) - if search_key: - return 1, "Found [%s]"%search_key, search_key - else: - return -1, "Cannot find parameter [%s] in an upwards search"%key, '' - - @apivalidate(0, (is_api('caller_api'), non_empty_str('key'),)) - def subscribeParam(self, caller_id, caller_api, key): - """ - Retrieve parameter value from server and subscribe to updates to that param. See - paramUpdate() in the Node API. - @param caller_id str: ROS caller id - @type caller_id: str - @param key: parameter to lookup. - @type key: str - @param caller_api: API URI for paramUpdate callbacks. - @type caller_api: str - @return: [code, statusMessage, parameterValue]. If code is not - 1, parameterValue should be ignored. parameterValue is an empty dictionary if the parameter - has not been set yet. - @rtype: [int, str, XMLRPCLegalValue] - """ - key = resolve_name(key, caller_id) - try: - # ps_lock has precedence and is required due to - # potential self.reg_manager modification - self.ps_lock.acquire() - val = self.param_server.subscribe_param(key, (caller_id, caller_api)) - finally: - self.ps_lock.release() - return 1, "Subscribed to parameter [%s]"%key, val - - @apivalidate(0, (is_api('caller_api'), non_empty_str('key'),)) - def unsubscribeParam(self, caller_id, caller_api, key): - """ - Retrieve parameter value from server and subscribe to updates to that param. See - paramUpdate() in the Node API. - @param caller_id str: ROS caller id - @type caller_id: str - @param key: parameter to lookup. - @type key: str - @param caller_api: API URI for paramUpdate callbacks. - @type caller_api: str - @return: [code, statusMessage, numUnsubscribed]. - If numUnsubscribed is zero it means that the caller was not subscribed to the parameter. - @rtype: [int, str, int] - """ - key = resolve_name(key, caller_id) - try: - # ps_lock is required due to potential self.reg_manager modification - self.ps_lock.acquire() - retval = self.param_server.unsubscribe_param(key, (caller_id, caller_api)) - finally: - self.ps_lock.release() - return 1, "Unsubscribe to parameter [%s]"%key, 1 - - - @apivalidate(False, (non_empty_str('key'),)) - def hasParam(self, caller_id, key): - """ - Check if parameter is stored on server. - @param caller_id str: ROS caller id - @type caller_id: str - @param key: parameter to check - @type key: str - @return: [code, statusMessage, hasParam] - @rtype: [int, str, bool] - """ - key = resolve_name(key, caller_id) - if self.param_server.has_param(key): - return 1, key, True - else: - return 1, key, False - - @apivalidate([]) - def getParamNames(self, caller_id): - """ - Get list of all parameter names stored on this server. - This does not adjust parameter names for caller's scope. - - @param caller_id: ROS caller id - @type caller_id: str - @return: [code, statusMessage, parameterNameList] - @rtype: [int, str, [str]] - """ - return 1, "Parameter names", self.param_server.get_param_names() - - ################################################################################## - # NOTIFICATION ROUTINES - - def _notify(self, registrations, task, key, value, node_apis): - """ - Generic implementation of callback notification - @param registrations: Registrations - @type registrations: L{Registrations} - @param task: task to queue - @type task: fn - @param key: registration key - @type key: str - @param value: value to pass to task - @type value: Any - """ - # cache thread_pool for thread safety - thread_pool = self.thread_pool - if not thread_pool: - return - - try: - for node_api in node_apis: - # use the api as a marker so that we limit one thread per subscriber - thread_pool.queue_task(node_api, task, (node_api, key, value)) - except KeyError: - _logger.warn('subscriber data stale (key [%s], listener [%s]): node API unknown'%(key, s)) - - def _notify_param_subscribers(self, updates): - """ - Notify parameter subscribers of new parameter value - @param updates [([str], str, any)*]: [(subscribers, param_key, param_value)*] - @param param_value str: parameter value - """ - # cache thread_pool for thread safety - thread_pool = self.thread_pool - if not thread_pool: - return - - for subscribers, key, value in updates: - # use the api as a marker so that we limit one thread per subscriber - for caller_id, caller_api in subscribers: - self.thread_pool.queue_task(caller_api, self.param_update_task, (caller_id, caller_api, key, value)) - - def param_update_task(self, caller_id, caller_api, param_key, param_value): - """ - Contact api.paramUpdate with specified parameters - @param caller_id: caller ID - @type caller_id: str - @param caller_api: XML-RPC URI of node to contact - @type caller_api: str - @param param_key: parameter key to pass to node - @type param_key: str - @param param_value: parameter value to pass to node - @type param_value: str - """ - mloginfo("paramUpdate[%s]", param_key) - code, _, _ = xmlrpcapi(caller_api).paramUpdate('/master', param_key, param_value) - if code == -1: - try: - # ps_lock is required due to potential self.reg_manager modification - self.ps_lock.acquire() - # reverse lookup to figure out who we just called - matches = self.reg_manager.reverse_lookup(caller_api) - for m in matches: - retval = self.param_server.unsubscribe_param(param_key, (m.id, caller_api)) - finally: - self.ps_lock.release() - - def _notify_topic_subscribers(self, topic, pub_uris, sub_uris): - """ - Notify subscribers with new publisher list - @param topic: name of topic - @type topic: str - @param pub_uris: list of URIs of publishers. - @type pub_uris: [str] - """ - self._notify(self.subscribers, publisher_update_task, topic, pub_uris, sub_uris) - - ################################################################################## - # SERVICE PROVIDER - - @apivalidate(0, ( is_service('service'), is_api('service_api'), is_api('caller_api'))) - def registerService(self, caller_id, service, service_api, caller_api): - """ - Register the caller as a provider of the specified service. - @param caller_id str: ROS caller id - @type caller_id: str - @param service: Fully-qualified name of service - @type service: str - @param service_api: Service URI - @type service_api: str - @param caller_api: XML-RPC URI of caller node - @type caller_api: str - @return: (code, message, ignore) - @rtype: (int, str, int) - """ - try: - self.ps_lock.acquire() - self.reg_manager.register_service(service, caller_id, caller_api, service_api) - mloginfo("+SERVICE [%s] %s %s", service, caller_id, caller_api) - finally: - self.ps_lock.release() - return 1, "Registered [%s] as provider of [%s]"%(caller_id, service), 1 - - @apivalidate('', (is_service('service'),)) - def lookupService(self, caller_id, service): - """ - Lookup all provider of a particular service. - @param caller_id str: ROS caller id - @type caller_id: str - @param service: fully-qualified name of service to lookup. - @type: service: str - @return: (code, message, serviceUrl). service URL is provider's - ROSRPC URI with address and port. Fails if there is no provider. - @rtype: (int, str, str) - """ - try: - self.ps_lock.acquire() - service_url = self.services.get_service_api(service) - finally: - self.ps_lock.release() - if service_url: - return 1, "rosrpc URI: [%s]"%service_url, service_url - else: - return -1, "no provider", '' - - @apivalidate(0, ( is_service('service'), is_api('service_api'))) - def unregisterService(self, caller_id, service, service_api): - """ - Unregister the caller as a provider of the specified service. - @param caller_id str: ROS caller id - @type caller_id: str - @param service: Fully-qualified name of service - @type service: str - @param service_api: API URI of service to unregister. Unregistration will only occur if current - registration matches. - @type service_api: str - @return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1). - If this is zero it means that the caller was not registered as a service provider. - The call still succeeds as the intended final state is reached. - @rtype: (int, str, int) - """ - try: - self.ps_lock.acquire() - retval = self.reg_manager.unregister_service(service, caller_id, service_api) - mloginfo("-SERVICE [%s] %s %s", service, caller_id, service_api) - return retval - finally: - self.ps_lock.release() - - ################################################################################## - # PUBLISH/SUBSCRIBE - - @apivalidate([], ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api'))) - def registerSubscriber(self, caller_id, topic, topic_type, caller_api): - """ - Subscribe the caller to the specified topic. In addition to receiving - a list of current publishers, the subscriber will also receive notifications - of new publishers via the publisherUpdate API. - @param caller_id: ROS caller id - @type caller_id: str - @param topic str: Fully-qualified name of topic to subscribe to. - @param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name. - @type topic_type: str - @param caller_api: XML-RPC URI of caller node for new publisher notifications - @type caller_api: str - @return: (code, message, publishers). Publishers is a list of XMLRPC API URIs - for nodes currently publishing the specified topic. - @rtype: (int, str, [str]) - """ - #NOTE: subscribers do not get to set topic type - try: - self.ps_lock.acquire() - self.reg_manager.register_subscriber(topic, caller_id, caller_api) - - # ROS 1.1: subscriber can now set type if it is not already set - # - don't let '*' type squash valid typing - if not topic in self.topics_types and topic_type != rosgraph.names.ANYTYPE: - self.topics_types[topic] = topic_type - - mloginfo("+SUB [%s] %s %s",topic, caller_id, caller_api) - pub_uris = self.publishers.get_apis(topic) - finally: - self.ps_lock.release() - return 1, "Subscribed to [%s]"%topic, pub_uris - - @apivalidate(0, (is_topic('topic'), is_api('caller_api'))) - def unregisterSubscriber(self, caller_id, topic, caller_api): - """ - Unregister the caller as a subscriber of the topic. - @param caller_id: ROS caller id - @type caller_id: str - @param topic: Fully-qualified name of topic to unregister. - @type topic: str - @param caller_api: API URI of service to unregister. Unregistration will only occur if current - registration matches. - @type caller_api: str - @return: (code, statusMessage, numUnsubscribed). - If numUnsubscribed is zero it means that the caller was not registered as a subscriber. - The call still succeeds as the intended final state is reached. - @rtype: (int, str, int) - """ - try: - self.ps_lock.acquire() - retval = self.reg_manager.unregister_subscriber(topic, caller_id, caller_api) - mloginfo("-SUB [%s] %s %s",topic, caller_id, caller_api) - return retval - finally: - self.ps_lock.release() - - @apivalidate([], ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api'))) - def registerPublisher(self, caller_id, topic, topic_type, caller_api): - """ - Register the caller as a publisher the topic. - @param caller_id: ROS caller id - @type caller_id: str - @param topic: Fully-qualified name of topic to register. - @type topic: str - @param topic_type: Datatype for topic. Must be a - package-resource name, i.e. the .msg name. - @type topic_type: str - @param caller_api str: ROS caller XML-RPC API URI - @type caller_api: str - @return: (code, statusMessage, subscriberApis). - List of current subscribers of topic in the form of XMLRPC URIs. - @rtype: (int, str, [str]) - """ - #NOTE: we need topic_type for getPublishedTopics. - try: - self.ps_lock.acquire() - self.reg_manager.register_publisher(topic, caller_id, caller_api) - # don't let '*' type squash valid typing - if topic_type != rosgraph.names.ANYTYPE or not topic in self.topics_types: - self.topics_types[topic] = topic_type - pub_uris = self.publishers.get_apis(topic) - sub_uris = self.subscribers.get_apis(topic) - self._notify_topic_subscribers(topic, pub_uris, sub_uris) - mloginfo("+PUB [%s] %s %s",topic, caller_id, caller_api) - sub_uris = self.subscribers.get_apis(topic) - finally: - self.ps_lock.release() - return 1, "Registered [%s] as publisher of [%s]"%(caller_id, topic), sub_uris - - - @apivalidate(0, (is_topic('topic'), is_api('caller_api'))) - def unregisterPublisher(self, caller_id, topic, caller_api): - """ - Unregister the caller as a publisher of the topic. - @param caller_id: ROS caller id - @type caller_id: str - @param topic: Fully-qualified name of topic to unregister. - @type topic: str - @param caller_api str: API URI of service to - unregister. Unregistration will only occur if current - registration matches. - @type caller_api: str - @return: (code, statusMessage, numUnregistered). - If numUnregistered is zero it means that the caller was not registered as a publisher. - The call still succeeds as the intended final state is reached. - @rtype: (int, str, int) - """ - try: - self.ps_lock.acquire() - retval = self.reg_manager.unregister_publisher(topic, caller_id, caller_api) - if retval[VAL]: - self._notify_topic_subscribers(topic, self.publishers.get_apis(topic), self.subscribers.get_apis(topic)) - mloginfo("-PUB [%s] %s %s",topic, caller_id, caller_api) - finally: - self.ps_lock.release() - return retval - - ################################################################################## - # GRAPH STATE APIS - - @apivalidate('', (valid_name('node'),)) - def lookupNode(self, caller_id, node_name): - """ - Get the XML-RPC URI of the node with the associated - name/caller_id. This API is for looking information about - publishers and subscribers. Use lookupService instead to lookup - ROS-RPC URIs. - @param caller_id: ROS caller id - @type caller_id: str - @param node: name of node to lookup - @type node: str - @return: (code, msg, URI) - @rtype: (int, str, str) - """ - try: - self.ps_lock.acquire() - node = self.reg_manager.get_node(node_name) - if node is not None: - retval = 1, "node api", node.api - else: - retval = -1, "unknown node [%s]"%node_name, '' - finally: - self.ps_lock.release() - return retval - - @apivalidate(0, (empty_or_valid_name('subgraph'),)) - def getPublishedTopics(self, caller_id, subgraph): - """ - Get list of topics that can be subscribed to. This does not return topics that have no publishers. - See L{getSystemState()} to get more comprehensive list. - @param caller_id: ROS caller id - @type caller_id: str - @param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace - is resolved relative to the caller's namespace. Use '' to specify all names. - @type subgraph: str - @return: (code, msg, [[topic1, type1]...[topicN, typeN]]) - @rtype: (int, str, [[str, str],]) - """ - try: - self.ps_lock.acquire() - # force subgraph to be a namespace with trailing slash - if subgraph and subgraph[-1] != rosgraph.names.SEP: - subgraph = subgraph + rosgraph.names.SEP - #we don't bother with subscribers as subscribers don't report topic types. also, the intended - #use case is for subscribe-by-topic-type - retval = [[t, self.topics_types[t]] for t in self.publishers.iterkeys() if t.startswith(subgraph)] - finally: - self.ps_lock.release() - return 1, "current topics", retval - - @apivalidate([]) - def getTopicTypes(self, caller_id): - """ - Retrieve list topic names and their types. - @param caller_id: ROS caller id - @type caller_id: str - @rtype: (int, str, [[str,str]] ) - @return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs. - """ - try: - self.ps_lock.acquire() - retval = list(self.topics_types.items()) - finally: - self.ps_lock.release() - return 1, "current system state", retval - - @apivalidate([[],[], []]) - def getSystemState(self, caller_id): - """ - Retrieve list representation of system state (i.e. publishers, subscribers, and services). - @param caller_id: ROS caller id - @type caller_id: str - @rtype: (int, str, [[str,[str]], [str,[str]], [str,[str]]]) - @return: (code, statusMessage, systemState). - - System state is in list representation:: - [publishers, subscribers, services]. - - publishers is of the form:: - [ [topic1, [topic1Publisher1...topic1PublisherN]] ... ] - - subscribers is of the form:: - [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] - - services is of the form:: - [ [service1, [service1Provider1...service1ProviderN]] ... ] - """ - edges = [] - try: - self.ps_lock.acquire() - retval = [r.get_state() for r in (self.publishers, self.subscribers, self.services)] - finally: - self.ps_lock.release() - return 1, "current system state", retval diff --git a/src/hal/components/cros/master_python_source/rosmaster/paramserver.py b/src/hal/components/cros/master_python_source/rosmaster/paramserver.py deleted file mode 100644 index d29b4d30..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/paramserver.py +++ /dev/null @@ -1,402 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. - -from threading import RLock - -from rosgraph.names import ns_join, GLOBALNS, SEP, is_global, is_private, canonicalize_name - -def _get_param_names(names, key, d): - """ - helper recursive routine for getParamNames() - @param names: list of param names to append to - @type names: [str] - @param d: parameter tree node - @type d: dict - @param key: parameter key for tree node d - @type key: str - """ - - #TODOXXX - for k,v in d.items(): - if type(v) == dict: - _get_param_names(names, ns_join(key, k), v) - else: - names.append(ns_join(key, k)) - -class ParamDictionary(object): - - def __init__(self, reg_manager): - """ - ctor. - @param subscribers: parameter subscribers - @type subscribers: Registrations - """ - self.lock = RLock() - self.parameters = {} - self.reg_manager = reg_manager - - def get_param_names(self): - """ - Get list of all parameter names stored on this server. - - @return: [code, statusMessage, parameterNameList] - @rtype: [int, str, [str]] - """ - try: - self.lock.acquire() - param_names = [] - _get_param_names(param_names, '/', self.parameters) - finally: - self.lock.release() - return param_names - - def search_param(self, ns, key): - """ - Search for matching parameter key for search param - key. Search for key starts at ns and proceeds upwards to - the root. As such, search_param should only be called with a - relative parameter name. - - search_param's behavior is to search for the first partial match. - For example, imagine that there are two 'robot_description' parameters: - - - /robot_description - - /robot_description/arm - - /robot_description/base - - - /pr2/robot_description - - /pr2/robot_description/base - - If I start in the namespace /pr2/foo and search for - 'robot_description', search_param will match - /pr2/robot_description. If I search for 'robot_description/arm' - it will return /pr2/robot_description/arm, even though that - parameter does not exist (yet). - - @param ns: namespace to begin search from. - @type ns: str - @param key: Parameter key. - @type key: str - @return: key of matching parameter or None if no matching - parameter. - @rtype: str - """ - if not key or is_private(key): - raise ValueError("invalid key") - if not is_global(ns): - raise ValueError("namespace must be global") - if is_global(key): - if self.has_param(key): - return key - else: - return None - - # there are more efficient implementations, but our hiearchy - # is not very deep and this is fairly clean code to read. - - # - we only search for the first namespace in the key to check for a match - key_namespaces = [x for x in key.split(SEP) if x] - key_ns = key_namespaces[0] - - # - corner case: have to test initial namespace first as - # negative indices won't work with 0 - search_key = ns_join(ns, key_ns) - if self.has_param(search_key): - # resolve to full key - return ns_join(ns, key) - - namespaces = [x for x in ns.split(SEP) if x] - for i in range(1, len(namespaces)+1): - search_key = SEP + SEP.join(namespaces[0:-i] + [key_ns]) - if self.has_param(search_key): - # we have a match on the namespace of the key, so - # compose the full key and return it - full_key = SEP + SEP.join(namespaces[0:-i] + [key]) - return full_key - return None - - def get_param(self, key): - """ - Get the parameter in the parameter dictionary. - - @param key: parameter key - @type key: str - @return: parameter value - """ - try: - self.lock.acquire() - val = self.parameters - if key != GLOBALNS: - # split by the namespace separator, ignoring empty splits - namespaces = [x for x in key.split(SEP)[1:] if x] - for ns in namespaces: - if not type(val) == dict: - raise KeyError(val) - val = val[ns] - return val - finally: - self.lock.release() - - def set_param(self, key, value, notify_task=None, caller_id=None): - """ - Set the parameter in the parameter dictionary. - - @param key: parameter key - @type key: str - @param value: parameter value - @param notify_task: function to call with - subscriber updates. updates is of the form - [(subscribers, param_key, param_value)*]. The empty dictionary - represents an unset parameter. - @type notify_task: fn(updates) - @param caller_id: the caller id - @type caller_id: str - """ - try: - self.lock.acquire() - if key == GLOBALNS: - if type(value) != dict: - raise TypeError("cannot set root of parameter tree to non-dictionary") - self.parameters = value - else: - namespaces = [x for x in key.split(SEP) if x] - # - last namespace is the actual key we're storing in - value_key = namespaces[-1] - namespaces = namespaces[:-1] - d = self.parameters - # - descend tree to the node we're setting - for ns in namespaces: - if not ns in d: - new_d = {} - d[ns] = new_d - d = new_d - else: - val = d[ns] - # implicit type conversion of value to namespace - if type(val) != dict: - d[ns] = val = {} - d = val - - d[value_key] = value - - # ParamDictionary needs to queue updates so that the updates are thread-safe - if notify_task: - updates = compute_param_updates(self.reg_manager.param_subscribers, key, value, caller_id) - if updates: - notify_task(updates) - finally: - self.lock.release() - - - def subscribe_param(self, key, registration_args): - """ - @param key: parameter key - @type key: str - @param registration_args: additional args to pass to - subscribers.register. First parameter is always the parameter - key. - @type registration_args: tuple - """ - if key != SEP: - key = canonicalize_name(key) + SEP - try: - self.lock.acquire() - # fetch parameter value - try: - val = self.get_param(key) - except KeyError: - # parameter not set yet - val = {} - self.reg_manager.register_param_subscriber(key, *registration_args) - return val - finally: - self.lock.release() - - - def unsubscribe_param(self, key, unregistration_args): - """ - @param key str: parameter key - @type key: str - @param unregistration_args: additional args to pass to - subscribers.unregister. i.e. unregister will be called with - (key, *unregistration_args) - @type unregistration_args: tuple - @return: return value of subscribers.unregister() - """ - if key != SEP: - key = canonicalize_name(key) + SEP - return self.reg_manager.unregister_param_subscriber(key, *unregistration_args) - - def delete_param(self, key, notify_task=None): - """ - Delete the parameter in the parameter dictionary. - @param key str: parameter key - @param notify_task fn(updates): function to call with - subscriber updates. updates is of the form - [(subscribers, param_key, param_value)*]. The empty dictionary - represents an unset parameter. - """ - try: - self.lock.acquire() - if key == GLOBALNS: - raise KeyError("cannot delete root of parameter tree") - else: - # key is global, so first split is empty - namespaces = [x for x in key.split(SEP) if x] - # - last namespace is the actual key we're deleting - value_key = namespaces[-1] - namespaces = namespaces[:-1] - d = self.parameters - # - descend tree to the node we're setting - for ns in namespaces: - if type(d) != dict or not ns in d: - raise KeyError(key) - else: - d = d[ns] - - if not value_key in d: - raise KeyError(key) - else: - del d[value_key] - - # ParamDictionary needs to queue updates so that the updates are thread-safe - if notify_task: - updates = compute_param_updates(self.reg_manager.param_subscribers, key, {}) - if updates: - notify_task(updates) - finally: - self.lock.release() - - def has_param(self, key): - """ - Test for parameter existence - - @param key: parameter key - @type key: str - @return: True if parameter set, False otherwise - @rtype: bool - """ - try: - # more efficient implementations are certainly possible, - # but this guarantees correctness for now - self.get_param(key) - return True - except KeyError: - return False - -def _compute_all_keys(param_key, param_value, all_keys=None): - """ - Compute which subscribers should be notified based on the parameter update - @param param_key: key of updated parameter - @type param_key: str - @param param_value: value of updated parameter - @param all_keys: (internal use only) list of parameter keys - to append to for recursive calls. - @type all_keys: [str] - @return: list of parameter keys. All keys will be canonicalized with trailing slash. - @rtype: [str] - """ - if all_keys is None: - all_keys = [] - for k, v in param_value.items(): - new_k = ns_join(param_key, k) + SEP - all_keys.append(new_k) - if type(v) == dict: - _compute_all_keys(new_k, v, all_keys) - return all_keys - -def compute_param_updates(subscribers, param_key, param_value, caller_id_to_ignore=None): - """ - Compute subscribers that should be notified based on the parameter update - @param subscribers: parameter subscribers - @type subscribers: Registrations - @param param_key: parameter key - @type param_key: str - @param param_value: parameter value - @type param_value: str - @param caller_id_to_ignore: the caller to ignore - @type caller_id_to_ignore: str - """ - - # logic correct for both updates and deletions - - if not subscribers: - return [] - - # end with a trailing slash to optimize startswith check from - # needing an extra equals check - if param_key != SEP: - param_key = canonicalize_name(param_key) + SEP - - # compute all the updated keys - if type(param_value) == dict: - all_keys = _compute_all_keys(param_key, param_value) - else: - all_keys = None - - updates = [] - - # subscriber gets update if anything in the subscribed namespace is updated or if its deleted - for sub_key in subscribers.iterkeys(): - ns_key = sub_key - if ns_key[-1] != SEP: - ns_key = sub_key + SEP - if param_key.startswith(ns_key): - node_apis = subscribers[sub_key] - if caller_id_to_ignore is not None: - node_apis = [ - (caller_id, caller_api) - for (caller_id, caller_api) in node_apis - if caller_id != caller_id_to_ignore] - updates.append((node_apis, param_key, param_value)) - elif all_keys is not None and ns_key.startswith(param_key) \ - and not sub_key in all_keys: - # parameter was deleted - node_apis = subscribers[sub_key] - updates.append((node_apis, sub_key, {})) - - # add updates for exact matches within tree - if all_keys is not None: - # #586: iterate over parameter tree for notification - for key in all_keys: - if key in subscribers: - # compute actual update value - sub_key = key[len(param_key):] - namespaces = [x for x in sub_key.split(SEP) if x] - val = param_value - for ns in namespaces: - val = val[ns] - - updates.append((subscribers[key], key, val)) - - return updates - diff --git a/src/hal/components/cros/master_python_source/rosmaster/registrations.py b/src/hal/components/cros/master_python_source/rosmaster/registrations.py deleted file mode 100644 index 6cbfddf2..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/registrations.py +++ /dev/null @@ -1,473 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -from rosmaster.util import remove_server_proxy -from rosmaster.util import xmlrpcapi -import rosmaster.exceptions - -"""Data structures for representing registration data in the Master""" - -class NodeRef(object): - """ - Container for node registration information. Used in master's - self.nodes data structure. This is effectively a reference - counter for the node registration information: when the - subscriptions and publications are empty the node registration can - be deleted. - """ - def __init__(self, id, api): - """ - ctor - @param api str: node XML-RPC API - """ - self.id = id - self.api = api - self.param_subscriptions = [] - self.topic_subscriptions = [] - self.topic_publications = [] - self.services = [] - - def clear(self): - """ - Delete all state from this NodeRef except for the api location - """ - self.param_subscriptions = [] - self.topic_subscriptions = [] - self.topic_publications = [] - self.services = [] - - def is_empty(self): - """ - @return: True if node has no active registrations - """ - return sum((len(x) for x in - [self.param_subscriptions, - self.topic_subscriptions, - self.topic_publications, - self.services,])) == 0 - - def add(self, type_, key): - if type_ == Registrations.TOPIC_SUBSCRIPTIONS: - if not key in self.topic_subscriptions: - self.topic_subscriptions.append(key) - elif type_ == Registrations.TOPIC_PUBLICATIONS: - if not key in self.topic_publications: - self.topic_publications.append(key) - elif type_ == Registrations.SERVICE: - if not key in self.services: - self.services.append(key) - elif type_ == Registrations.PARAM_SUBSCRIPTIONS: - if not key in self.param_subscriptions: - self.param_subscriptions.append(key) - else: - raise rosmaster.exceptions.InternalException("internal bug") - - def remove(self, type_, key): - if type_ == Registrations.TOPIC_SUBSCRIPTIONS: - if key in self.topic_subscriptions: - self.topic_subscriptions.remove(key) - elif type_ == Registrations.TOPIC_PUBLICATIONS: - if key in self.topic_publications: - self.topic_publications.remove(key) - elif type_ == Registrations.SERVICE: - if key in self.services: - self.services.remove(key) - elif type_ == Registrations.PARAM_SUBSCRIPTIONS: - if key in self.param_subscriptions: - self.param_subscriptions.remove(key) - else: - raise rosmaster.exceptions.InternalException("internal bug") - -# NOTE: I'm not terribly happy that this task has leaked into the data model. need -# to refactor to get this back into masterslave. - -def shutdown_node_task(api, caller_id, reason): - """ - Method to shutdown another ROS node. Generally invoked within a - separate thread as this is used to cleanup hung nodes. - - @param api: XML-RPC API of node to shutdown - @type api: str - @param caller_id: name of node being shutdown - @type caller_id: str - @param reason: human-readable reason why node is being shutdown - @type reason: str - """ - try: - xmlrpcapi(api).shutdown('/master', reason) - except: - pass #expected in many common cases - remove_server_proxy(api) - -class Registrations(object): - """ - All calls may result in access/modifications to node registrations - dictionary, so be careful to guarantee appropriate thread-safeness. - - Data structure for storing a set of registrations (e.g. publications, services). - The underlying data storage is the same except for services, which have the - constraint that only one registration may be active for a given key. - """ - - TOPIC_SUBSCRIPTIONS = 1 - TOPIC_PUBLICATIONS = 2 - SERVICE = 3 - PARAM_SUBSCRIPTIONS = 4 - - def __init__(self, type_): - """ - ctor. - @param type_: one of [ TOPIC_SUBSCRIPTIONS, - TOPIC_PUBLICATIONS, SERVICE, PARAM_SUBSCRIPTIONS ] - @type type_: int - """ - if not type_ in [ - Registrations.TOPIC_SUBSCRIPTIONS, - Registrations.TOPIC_PUBLICATIONS, - Registrations.SERVICE, - Registrations.PARAM_SUBSCRIPTIONS ]: - raise rosmaster.exceptions.InternalException("invalid registration type: %s"%type_) - self.type = type_ - ## { key: [(caller_id, caller_api)] } - self.map = {} - self.service_api_map = None - - def __bool__(self): - """ - @return: True if there are registrations - """ - return len(self.map) != 0 - - def __nonzero__(self): - """ - @return: True if there are registrations - """ - return len(self.map) != 0 - - def iterkeys(self): - """ - Iterate over registration keys - @return: iterator for registration keys - """ - return self.map.keys() - - def get_service_api(self, service): - """ - Lookup service API URI. NOTE: this should only be valid if type==SERVICE as - service Registrations instances are the only ones that track service API URIs. - @param service: service name - @type service: str - @return str: service_api for registered key or None if - registration is no longer valid. - @type: str - """ - if self.service_api_map and service in self.service_api_map: - caller_id, service_api = self.service_api_map[service] - return service_api - return None - - def get_apis(self, key): - """ - Only valid if self.type != SERVICE. - @param key: registration key (e.g. topic/service/param name) - @type key: str - @return: caller_apis for registered key, empty list if registration is not valid - @rtype: [str] - """ - return [api for _, api in self.map.get(key, [])] - - def __contains__(self, key): - """ - Emulate mapping type for has_key() - """ - return key in self.map - - def __getitem__(self, key): - """ - @param key: registration key (e.g. topic/service/param name) - @type key: str - @return: (caller_id, caller_api) for registered - key, empty list if registration is not valid - @rtype: [(str, str),] - """ - # unlike get_apis, returns the caller_id to prevent any race - # conditions that can occur if caller_id/caller_apis change - # due to a new node. - return self.map.get(key, []) - - def has_key(self, key): - """ - @param key: registration key (e.g. topic/service/param name) - @type key: str - @return: True if key is registered - @rtype: bool - """ - return key in self.map - - def get_state(self): - """ - @return: state in getSystemState()-friendly format [ [key, [callerId1...callerIdN]] ... ] - @rtype: [str, [str]...] - """ - retval = [] - for k in self.map.keys(): - retval.append([k, [id for id, _ in self.map[k]]]) - return retval - - def register(self, key, caller_id, caller_api, service_api=None): - """ - Add caller_id into the map as a provider of the specified - service (key). caller_id must not have been previously - registered with a different caller_api. - - Subroutine for managing provider map data structure (essentially a multimap). - @param key: registration key (e.g. topic/service/param name) - @type key: str - @param caller_id: caller_id of provider - @type caller_id: str - @param caller_api: API URI of provider - @type caller_api: str - @param service_api: (keyword) ROS service API URI if registering a service - @type service_api: str - """ - map = self.map - if key in map and not service_api: - providers = map[key] - if not (caller_id, caller_api) in providers: - providers.append((caller_id, caller_api)) - else: - map[key] = providers = [(caller_id, caller_api)] - - if service_api: - if self.service_api_map is None: - self.service_api_map = {} - self.service_api_map[key] = (caller_id, service_api) - elif self.type == Registrations.SERVICE: - raise rosmaster.exceptions.InternalException("service_api must be specified for Registrations.SERVICE") - - def unregister_all(self, caller_id): - """ - Remove all registrations associated with caller_id - @param caller_id: caller_id of provider - @type caller_id: str - """ - map = self.map - # fairly expensive - dead_keys = [] - for key in map: - providers = map[key] - # find all matching entries - to_remove = [(id, api) for id, api in providers if id == caller_id] - # purge them - for r in to_remove: - providers.remove(r) - if not providers: - dead_keys.append(key) - for k in dead_keys: - del self.map[k] - if self.type == Registrations.SERVICE and self.service_api_map: - del dead_keys[:] - for key, val in self.service_api_map.items(): - if val[0] == caller_id: - dead_keys.append(key) - for k in dead_keys: - del self.service_api_map[k] - - def unregister(self, key, caller_id, caller_api, service_api=None): - """ - Remove caller_id from the map as a provider of the specified service (key). - Subroutine for managing provider map data structure, essentially a multimap - @param key: registration key (e.g. topic/service/param name) - @type key: str - @param caller_id: caller_id of provider - @type caller_id: str - @param caller_api: API URI of provider - @type caller_api: str - @param service_api: (keyword) ROS service API URI if registering a service - @type service_api: str - @return: for ease of master integration, directly returns unregister value for - higher-level XMLRPC API. val is the number of APIs unregistered (0 or 1) - @rtype: code, msg, val - """ - # if we are unregistering a topic, validate against the caller_api - if service_api: - # validate against the service_api - if self.service_api_map is None: - return 1, "[%s] is not a provider of [%s]"%(caller_id, key), 0 - if self.service_api_map.get(key, None) != (caller_id, service_api): - return 1, "[%s] is no longer the current service api handle for [%s]"%(service_api, key), 0 - else: - del self.service_api_map[key] - del self.map[key] - # caller_api is None for unregister service, so we can't validate as well - return 1, "Unregistered [%s] as provider of [%s]"%(caller_id, key), 1 - elif self.type == Registrations.SERVICE: - raise rosmaster.exceptions.InternalException("service_api must be specified for Registrations.SERVICE") - else: - providers = self.map.get(key, []) - if (caller_id, caller_api) in providers: - providers.remove((caller_id, caller_api)) - if not providers: - del self.map[key] - return 1, "Unregistered [%s] as provider of [%s]"%(caller_id, key), 1 - else: - return 1, "[%s] is not a known provider of [%s]"%(caller_id, key), 0 - -class RegistrationManager(object): - """ - Stores registrations for Master. - - RegistrationManager is not threadsafe, so access must be externally locked as appropriate - """ - - def __init__(self, thread_pool): - """ - ctor. - @param thread_pool: thread pool for queueing tasks - @type thread_pool: ThreadPool - """ - self.nodes = {} - self.thread_pool = thread_pool - - self.publishers = Registrations(Registrations.TOPIC_PUBLICATIONS) - self.subscribers = Registrations(Registrations.TOPIC_SUBSCRIPTIONS) - self.services = Registrations(Registrations.SERVICE) - self.param_subscribers = Registrations(Registrations.PARAM_SUBSCRIPTIONS) - - - def reverse_lookup(self, caller_api): - """ - Get a NodeRef by caller_api - @param caller_api: caller XML RPC URI - @type caller_api: str - @return: nodes that declare caller_api as their - API. 99.9% of the time this should only be one node, but we - allow for multiple matches as the master API does not restrict - this. - @rtype: [NodeRef] - """ - matches = [n for n in self.nodes.items() if n.api == caller_api] - if matches: - return matches - - def get_node(self, caller_id): - return self.nodes.get(caller_id, None) - - def _register(self, r, key, caller_id, caller_api, service_api=None): - # update node information - node_ref, changed = self._register_node_api(caller_id, caller_api) - node_ref.add(r.type, key) - # update pub/sub/service indicies - if changed: - self.publishers.unregister_all(caller_id) - self.subscribers.unregister_all(caller_id) - self.services.unregister_all(caller_id) - self.param_subscribers.unregister_all(caller_id) - r.register(key, caller_id, caller_api, service_api) - - def _unregister(self, r, key, caller_id, caller_api, service_api=None): - node_ref = self.nodes.get(caller_id, None) - if node_ref != None: - retval = r.unregister(key, caller_id, caller_api, service_api) - # check num removed field, if 1, unregister is valid - if retval[2] == 1: - node_ref.remove(r.type, key) - if node_ref.is_empty(): - del self.nodes[caller_id] - else: - retval = 1, "[%s] is not a registered node"%caller_id, 0 - return retval - - def register_service(self, service, caller_id, caller_api, service_api): - """ - Register service provider - @return: None - """ - self._register(self.services, service, caller_id, caller_api, service_api) - def register_publisher(self, topic, caller_id, caller_api): - """ - Register topic publisher - @return: None - """ - self._register(self.publishers, topic, caller_id, caller_api) - def register_subscriber(self, topic, caller_id, caller_api): - """ - Register topic subscriber - @return: None - """ - self._register(self.subscribers, topic, caller_id, caller_api) - def register_param_subscriber(self, param, caller_id, caller_api): - """ - Register param subscriber - @return: None - """ - self._register(self.param_subscribers, param, caller_id, caller_api) - - def unregister_service(self, service, caller_id, service_api): - caller_api = None - return self._unregister(self.services, service, caller_id, caller_api, service_api) - - def unregister_subscriber(self, topic, caller_id, caller_api): - return self._unregister(self.subscribers, topic, caller_id, caller_api) - def unregister_publisher(self, topic, caller_id, caller_api): - return self._unregister(self.publishers, topic, caller_id, caller_api) - def unregister_param_subscriber(self, param, caller_id, caller_api): - return self._unregister(self.param_subscribers, param, caller_id, caller_api) - - def _register_node_api(self, caller_id, caller_api): - """ - @param caller_id: caller_id of provider - @type caller_id: str - @param caller_api: caller_api of provider - @type caller_api: str - @return: (registration_information, changed_registration). changed_registration is true if - caller_api is differet than the one registered with caller_id - @rtype: (NodeRef, bool) - """ - node_ref = self.nodes.get(caller_id, None) - - bumped_api = None - if node_ref is not None: - if node_ref.api == caller_api: - return node_ref, False - else: - bumped_api = node_ref.api - self.thread_pool.queue_task(bumped_api, shutdown_node_task, - (bumped_api, caller_id, "new node registered with same name")) - - node_ref = NodeRef(caller_id, caller_api) - self.nodes[caller_id] = node_ref - return (node_ref, bumped_api != None) - - diff --git a/src/hal/components/cros/master_python_source/rosmaster/threadpool.py b/src/hal/components/cros/master_python_source/rosmaster/threadpool.py deleted file mode 100644 index 6d195dca..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/threadpool.py +++ /dev/null @@ -1,228 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -""" -Internal threadpool library for zenmaster. - -Adapted from U{http://aspn.activestate.com/ASPN/Cookbook/Python/Recipe/203871} - -Added a 'marker' to tasks so that multiple tasks with the same -marker are not executed. As we are using the thread pool for i/o -tasks, the marker is set to the i/o name. This prevents a slow i/o -for gobbling up all of our threads -""" - -import threading, logging, traceback -from time import sleep - -class MarkedThreadPool(object): - - """Flexible thread pool class. Creates a pool of threads, then - accepts tasks that will be dispatched to the next available - thread.""" - - def __init__(self, numThreads): - - """Initialize the thread pool with numThreads workers.""" - - self.__threads = [] - self.__resizeLock = threading.Condition(threading.Lock()) - self.__taskLock = threading.Condition(threading.Lock()) - self.__tasks = [] - self.__markers = set() - self.__isJoining = False - self.set_thread_count(numThreads) - - def set_thread_count(self, newNumThreads): - - """ External method to set the current pool size. Acquires - the resizing lock, then calls the internal version to do real - work.""" - - # Can't change the thread count if we're shutting down the pool! - if self.__isJoining: - return False - - self.__resizeLock.acquire() - try: - self.__set_thread_count_nolock(newNumThreads) - finally: - self.__resizeLock.release() - return True - - def __set_thread_count_nolock(self, newNumThreads): - - """Set the current pool size, spawning or terminating threads - if necessary. Internal use only; assumes the resizing lock is - held.""" - - # If we need to grow the pool, do so - while newNumThreads > len(self.__threads): - newThread = ThreadPoolThread(self) - self.__threads.append(newThread) - newThread.start() - # If we need to shrink the pool, do so - while newNumThreads < len(self.__threads): - self.__threads[0].go_away() - del self.__threads[0] - - def get_thread_count(self): - """@return: number of threads in the pool.""" - self.__resizeLock.acquire() - try: - return len(self.__threads) - finally: - self.__resizeLock.release() - - def queue_task(self, marker, task, args=None, taskCallback=None): - - """Insert a task into the queue. task must be callable; - args and taskCallback can be None.""" - - if self.__isJoining == True: - return False - if not callable(task): - return False - - self.__taskLock.acquire() - try: - self.__tasks.append((marker, task, args, taskCallback)) - return True - finally: - self.__taskLock.release() - - def remove_marker(self, marker): - """Remove the marker from the currently executing tasks. Only one - task with the given marker can be executed at a given time""" - if marker is None: - return - self.__taskLock.acquire() - try: - self.__markers.remove(marker) - finally: - self.__taskLock.release() - - def get_next_task(self): - - """ Retrieve the next task from the task queue. For use - only by ThreadPoolThread objects contained in the pool.""" - - self.__taskLock.acquire() - try: - retval = None - for marker, task, args, callback in self.__tasks: - # unmarked or not currently executing - if marker is None or marker not in self.__markers: - retval = (marker, task, args, callback) - break - if retval: - # add the marker so we don't do any similar tasks - self.__tasks.remove(retval) - if marker is not None: - self.__markers.add(marker) - return retval - else: - return (None, None, None, None) - finally: - self.__taskLock.release() - - def join_all(self, wait_for_tasks = True, wait_for_threads = True): - """ Clear the task queue and terminate all pooled threads, - optionally allowing the tasks and threads to finish.""" - - # Mark the pool as joining to prevent any more task queueing - self.__isJoining = True - - # Wait for tasks to finish - if wait_for_tasks: - while self.__tasks != []: - sleep(.1) - - # Tell all the threads to quit - self.__resizeLock.acquire() - try: - self.__set_thread_count_nolock(0) - self.__isJoining = True - - # Wait until all threads have exited - if wait_for_threads: - for t in self.__threads: - t.join() - del t - - # Reset the pool for potential reuse - self.__isJoining = False - finally: - self.__resizeLock.release() - - - -class ThreadPoolThread(threading.Thread): - """ - Pooled thread class. - """ - - threadSleepTime = 0.1 - - def __init__(self, pool): - """Initialize the thread and remember the pool.""" - threading.Thread.__init__(self) - self.daemon = True #don't block program exit - self.__pool = pool - self.__isDying = False - - def run(self): - """ - Until told to quit, retrieve the next task and execute - it, calling the callback if any. - """ - while self.__isDying == False: - marker, cmd, args, callback = self.__pool.get_next_task() - # If there's nothing to do, just sleep a bit - if cmd is None: - sleep(ThreadPoolThread.threadSleepTime) - else: - try: - try: - result = cmd(*args) - finally: - self.__pool.remove_marker(marker) - if callback is not None: - callback(result) - except Exception as e: - logging.getLogger('rosmaster.threadpool').error(traceback.format_exc()) - - def go_away(self): - """ Exit the run loop next time through.""" - self.__isDying = True diff --git a/src/hal/components/cros/master_python_source/rosmaster/util.py b/src/hal/components/cros/master_python_source/rosmaster/util.py deleted file mode 100644 index ee3a4848..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/util.py +++ /dev/null @@ -1,90 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -""" -Utility routines for rosmaster. -""" - -try: - from urllib.parse import urlparse -except ImportError: - from urlparse import urlparse -try: - from xmlrpc.client import ServerProxy -except ImportError: - from xmlrpclib import ServerProxy - -from defusedxml.xmlrpc import monkey_patch -monkey_patch() -del monkey_patch - -import errno -import socket - -_proxies = {} #cache ServerProxys -def xmlrpcapi(uri): - """ - @return: instance for calling remote server or None if not a valid URI - @rtype: xmlrpc.client.ServerProxy - """ - if uri is None: - return None - uriValidate = urlparse(uri) - if not uriValidate[0] or not uriValidate[1]: - return None - if not uri in _proxies: - _proxies[uri] = ServerProxy(uri) - close_half_closed_sockets() - return _proxies[uri] - - -def close_half_closed_sockets(): - if not hasattr(socket, 'TCP_INFO'): - return - for proxy in _proxies.values(): - transport = proxy("transport") - if transport._connection and transport._connection[1] is not None and transport._connection[1].sock is not None: - try: - state = transport._connection[1].sock.getsockopt(socket.SOL_TCP, socket.TCP_INFO) - except socket.error as e: # catch [Errno 92] Protocol not available - if e.args[0] is errno.ENOPROTOOPT: - return - raise - if state == 8: # CLOSE_WAIT - transport.close() - - -def remove_server_proxy(uri): - if uri in _proxies: - del _proxies[uri] diff --git a/src/hal/components/cros/master_python_source/rosmaster/validators.py b/src/hal/components/cros/master_python_source/rosmaster/validators.py deleted file mode 100644 index 417f6ab9..00000000 --- a/src/hal/components/cros/master_python_source/rosmaster/validators.py +++ /dev/null @@ -1,199 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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 Willow Garage, Inc. 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. -# -# Revision $Id$ - -"""Internal-use Python decorators for parameter validation""" - -from rosgraph.names import resolve_name, ANYTYPE - -TYPE_SEPARATOR = '/' -ROSRPC = "rosrpc://" - -def isstring(s): - """Small helper version to check an object is a string in a way that works - for both Python 2 and 3 - """ - try: - return isinstance(s, basestring) - except NameError: - return isinstance(s, str) - - -class ParameterInvalid(Exception): - """Exception that is raised when a parameter fails validation checks""" - def __init__(self, message): - self._message = message - - def __str__(self): - return str(self._message) - -def non_empty(param_name): - """Validator that checks that parameter is not empty""" - def validator(param, context): - if not param: - raise ParameterInvalid("ERROR: parameter [%s] must be specified and non-empty"%param_name) - return param - return validator - -def non_empty_str(param_name): - """Validator that checks that parameter is a string and non-empty""" - def validator(param, context): - if not param: - raise ParameterInvalid("ERROR: parameter [%s] must be specified and non-empty"%param_name) - elif not isstring(param): - raise ParameterInvalid("ERROR: parameter [%s] must be a string"%param_name) - return param - return validator - -def not_none(param_name): - """Validator that checks that parameter is not None""" - def validator(param, context): - if param is None: - raise ParameterInvalid("ERROR: parameter [%s] must be specified"%param_name) - return param - return validator - - -# Validators ###################################### - -def is_api(paramName): - """ - Validator that checks that parameter is a valid API handle - (i.e. URI). Both http and rosrpc are allowed schemes. - """ - def validator(param_value, callerId): - if not param_value or not isstring(param_value): - raise ParameterInvalid("ERROR: parameter [%s] is not an XMLRPC URI"%paramName) - if not param_value.startswith("http://") and not param_value.startswith(ROSRPC): - raise ParameterInvalid("ERROR: parameter [%s] is not an RPC URI"%paramName) - #could do more fancy parsing, but the above catches the major cases well enough - return param_value - return validator - -def is_topic(param_name): - """ - Validator that checks that parameter is a valid ROS topic name - """ - def validator(param_value, caller_id): - v = valid_name_validator_resolved(param_name, param_value, caller_id) - if param_value == '/': - raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) - return v - return validator - -def is_service(param_name): - """Validator that checks that parameter is a valid ROS service name""" - def validator(param_value, caller_id): - v = valid_name_validator_resolved(param_name, param_value, caller_id) - if param_value == '/': - raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) - return v - return validator - -def empty_or_valid_name(param_name): - """ - empty or valid graph resource name. - Validator that resolves names unless they an empty string is supplied, in which case - an empty string is returned. - """ - def validator(param_value, caller_id): - if not isstring(param_value): - raise ParameterInvalid("ERROR: parameter [%s] must be a string"%param_name) - if not param_value: - return '' - #return resolve_name(param_value, namespace(caller_id)) - return resolve_name(param_value, caller_id) - return validator - -def valid_name_validator_resolved(param_name, param_value, caller_id): - if not param_value or not isstring(param_value): - raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) - #TODO: actual validation of chars - # I added the colon check as the common error will be to send an URI instead of name - if ':' in param_value or ' ' in param_value: - raise ParameterInvalid("ERROR: parameter [%s] contains illegal chars"%param_name) - #return resolve_name(param_value, namespace(caller_id)) - return resolve_name(param_value, caller_id) -def valid_name_validator_unresolved(param_name, param_value, caller_id): - if not param_value or not isstring(param_value): - raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) - #TODO: actual validation of chars - # I added the colon check as the common error will be to send an URI instead of name - if ':' in param_value or ' ' in param_value: - raise ParameterInvalid("ERROR: parameter [%s] contains illegal chars"%param_name) - return param_value - -def valid_name(param_name, resolve=True): - """ - Validator that resolves names and also ensures that they are not empty - @param param_name: name - @type param_name: str - @param resolve: if True/omitted, the name will be resolved to - a global form. Otherwise, no resolution occurs. - @type resolve: bool - @return: resolved parameter value - @rtype: str - """ - def validator(param_value, caller_id): - if resolve: - return valid_name_validator_resolved(param_name, param_value, caller_id) - return valid_name_validator_unresolved(param_name, param_value, caller_id) - return validator - -def global_name(param_name): - """ - Validator that checks for valid, global graph resource name. - @return: parameter value - @rtype: str - """ - def validator(param_value, caller_id): - if not param_value or not isstring(param_value): - raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) - #TODO: actual validation of chars - if not is_global(param_value): - raise ParameterInvalid("ERROR: parameter [%s] must be a globally referenced name"%param_name) - return param_value - return validator - -def valid_type_name(param_name): - """validator that checks the type name is specified correctly""" - def validator(param_value, caller_id): - if param_value == ANYTYPE: - return param_value - if not param_value or not isstring(param_value): - raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) - if not len(param_value.split(TYPE_SEPARATOR)) == 2: - raise ParameterInvalid("ERROR: parameter [%s] is not a valid package resource name"%param_name) - #TODO: actual validation of chars - return param_value - return validator diff --git a/src/hal/components/cros/master_python_source/runmaster.py b/src/hal/components/cros/master_python_source/runmaster.py deleted file mode 100644 index 6a4afc4f..00000000 --- a/src/hal/components/cros/master_python_source/runmaster.py +++ /dev/null @@ -1,25 +0,0 @@ -"""!@brief This Python script runs the ROS Master node, which is needed to use a ROS network. - -This script does not requires that a ROS distribution is installed. - -@date 5 Mar 2019 -""" - -#import sys -#sys.path.append('.') -import rosmaster - -try: - input = raw_input -except NameError: - pass - -ros_master=rosmaster.master.Master() -ros_master.start() - -ros_master_ok=ros_master.ok() -print("Master is running: {}".format(ros_master_ok)) - -input("Press Enter to shutdown ROS Master and exit...") - -ros_master.stop() diff --git a/src/hal/components/cros/msvs_projects/cros.sln b/src/hal/components/cros/msvs_projects/cros.sln deleted file mode 100644 index 3d77542d..00000000 --- a/src/hal/components/cros/msvs_projects/cros.sln +++ /dev/null @@ -1,99 +0,0 @@ - -Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 15 -VisualStudioVersion = 15.0.28307.421 -MinimumVisualStudioVersion = 10.0.40219.1 -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "cros_library", "cros_library.vcxproj", "{57EB7322-CD61-4FD1-B595-C511C4648A37}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "sample_listener", "sample_listener\sample_listener.vcxproj", "{A5BAFDD4-D98D-4922-9F36-928F7C6C738A}" - ProjectSection(ProjectDependencies) = postProject - {57EB7322-CD61-4FD1-B595-C511C4648A37} = {57EB7322-CD61-4FD1-B595-C511C4648A37} - EndProjectSection -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "sample_talker", "sample_talker\sample_talker.vcxproj", "{231D4B94-3DAF-4FF5-AA68-770FA7901E2A}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "parameters_test", "parameters_test\parameters_test.vcxproj", "{5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}" -EndProject -Project("{888888A0-9F3D-457C-B088-3A5042F75D52}") = "ros_master_python", "ros_master_python.pyproj", "{539A16EC-20A8-4148-9A97-542B39A0A62F}" -EndProject -Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "performance_test", "performance_test\performance_test.vcxproj", "{5ABA6561-E322-4334-83E4-F3FEE2965133}" -EndProject -Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Solution Items", "Solution Items", "{748C592B-7E5D-4A0F-BD18-3EB7C39C92FC}" - ProjectSection(SolutionItems) = preProject - readme.txt = readme.txt - EndProjectSection -EndProject -Global - GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|Any CPU = Debug|Any CPU - Debug|x64 = Debug|x64 - Debug|x86 = Debug|x86 - Release|Any CPU = Release|Any CPU - Release|x64 = Release|x64 - Release|x86 = Release|x86 - EndGlobalSection - GlobalSection(ProjectConfigurationPlatforms) = postSolution - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|Any CPU.ActiveCfg = Debug|Win32 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|x64.ActiveCfg = Debug|x64 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|x64.Build.0 = Debug|x64 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|x86.ActiveCfg = Debug|Win32 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Debug|x86.Build.0 = Debug|Win32 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|Any CPU.ActiveCfg = Release|Win32 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|x64.ActiveCfg = Release|x64 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|x64.Build.0 = Release|x64 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|x86.ActiveCfg = Release|Win32 - {57EB7322-CD61-4FD1-B595-C511C4648A37}.Release|x86.Build.0 = Release|Win32 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|Any CPU.ActiveCfg = Debug|Win32 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|x64.ActiveCfg = Debug|x64 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|x64.Build.0 = Debug|x64 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|x86.ActiveCfg = Debug|Win32 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Debug|x86.Build.0 = Debug|Win32 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|Any CPU.ActiveCfg = Release|Win32 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|x64.ActiveCfg = Release|x64 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|x64.Build.0 = Release|x64 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|x86.ActiveCfg = Release|Win32 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A}.Release|x86.Build.0 = Release|Win32 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|Any CPU.ActiveCfg = Debug|Win32 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|x64.ActiveCfg = Debug|x64 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|x64.Build.0 = Debug|x64 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|x86.ActiveCfg = Debug|Win32 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Debug|x86.Build.0 = Debug|Win32 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|Any CPU.ActiveCfg = Release|Win32 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|x64.ActiveCfg = Release|x64 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|x64.Build.0 = Release|x64 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|x86.ActiveCfg = Release|Win32 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A}.Release|x86.Build.0 = Release|Win32 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|Any CPU.ActiveCfg = Debug|Win32 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|x64.ActiveCfg = Debug|x64 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|x64.Build.0 = Debug|x64 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|x86.ActiveCfg = Debug|Win32 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Debug|x86.Build.0 = Debug|Win32 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|Any CPU.ActiveCfg = Release|Win32 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|x64.ActiveCfg = Release|x64 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|x64.Build.0 = Release|x64 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|x86.ActiveCfg = Release|Win32 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26}.Release|x86.Build.0 = Release|Win32 - {539A16EC-20A8-4148-9A97-542B39A0A62F}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {539A16EC-20A8-4148-9A97-542B39A0A62F}.Debug|x64.ActiveCfg = Debug|Any CPU - {539A16EC-20A8-4148-9A97-542B39A0A62F}.Debug|x86.ActiveCfg = Debug|Any CPU - {539A16EC-20A8-4148-9A97-542B39A0A62F}.Release|Any CPU.ActiveCfg = Release|Any CPU - {539A16EC-20A8-4148-9A97-542B39A0A62F}.Release|x64.ActiveCfg = Release|Any CPU - {539A16EC-20A8-4148-9A97-542B39A0A62F}.Release|x86.ActiveCfg = Release|Any CPU - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|Any CPU.ActiveCfg = Debug|Win32 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|x64.ActiveCfg = Debug|x64 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|x64.Build.0 = Debug|x64 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|x86.ActiveCfg = Debug|Win32 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Debug|x86.Build.0 = Debug|Win32 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|Any CPU.ActiveCfg = Release|Win32 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|x64.ActiveCfg = Release|x64 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|x64.Build.0 = Release|x64 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|x86.ActiveCfg = Release|Win32 - {5ABA6561-E322-4334-83E4-F3FEE2965133}.Release|x86.Build.0 = Release|Win32 - EndGlobalSection - GlobalSection(SolutionProperties) = preSolution - HideSolutionNode = FALSE - EndGlobalSection - GlobalSection(ExtensibilityGlobals) = postSolution - SolutionGuid = {B97C4EE5-3641-461A-9398-88A34183EB84} - EndGlobalSection -EndGlobal diff --git a/src/hal/components/cros/msvs_projects/cros_library.vcxproj b/src/hal/components/cros/msvs_projects/cros_library.vcxproj deleted file mode 100644 index 477ecb12..00000000 --- a/src/hal/components/cros/msvs_projects/cros_library.vcxproj +++ /dev/null @@ -1,207 +0,0 @@ - - - - - Debug - Win32 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 15.0 - {57EB7322-CD61-4FD1-B595-C511C4648A37} - Win32Proj - 10.0.17763.0 - - - - StaticLibrary - true - v141 - - - StaticLibrary - false - v141 - - - StaticLibrary - true - v141 - - - StaticLibrary - false - v141 - - - - - - - - - - - - - - - - - - - - - true - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - cros - - - true - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - cros - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - cros - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - cros - - - - _CRT_SECURE_NO_WARNINGS;_CRT_NONSTDC_NO_DEPRECATE;%(PreprocessorDefinitions) - MultiThreadedDebugDLL - Level3 - ProgramDatabase - Disabled - - - MachineX86 - true - Windows - - - xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ - - - Copy ROS message definition files to working directory - - - - - _CRT_SECURE_NO_WARNINGS;_CRT_NONSTDC_NO_DEPRECATE;%(PreprocessorDefinitions) - MultiThreadedDLL - Level3 - ProgramDatabase - - - MachineX86 - true - Windows - true - true - - - xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ - - - Copy ROS message definition files to working directory - - - - - _CRT_SECURE_NO_WARNINGS;_CRT_NONSTDC_NO_DEPRECATE;%(PreprocessorDefinitions) - - - xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ - - - Copy ROS message definition files to working directory - - - - - _CRT_SECURE_NO_WARNINGS;_CRT_NONSTDC_NO_DEPRECATE;%(PreprocessorDefinitions) - - - xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ - - - Copy ROS message definition files to working directory - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/cros_library.vcxproj.filters b/src/hal/components/cros/msvs_projects/cros_library.vcxproj.filters deleted file mode 100644 index 7eabdd86..00000000 --- a/src/hal/components/cros/msvs_projects/cros_library.vcxproj.filters +++ /dev/null @@ -1,168 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;hm;inl;inc;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav - - - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - Source Files - - - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - Header Files - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj b/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj deleted file mode 100644 index 19da4d62..00000000 --- a/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj +++ /dev/null @@ -1,159 +0,0 @@ - - - - - Debug - Win32 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 15.0 - {5BB1A885-23C3-4B9A-99BB-1B7FEDE48E26} - parameterstest - 10.0.17763.0 - - - - Application - true - v141 - MultiByte - - - Application - false - v141 - true - MultiByte - - - Application - true - v141 - MultiByte - - - Application - false - v141 - true - MultiByte - - - - - - - - - - - - - - - - - - - - - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - - Level3 - Disabled - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - Console - - - - - Level3 - Disabled - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - Console - - - - - Level3 - MaxSpeed - true - true - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - true - true - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - Console - - - - - Level3 - MaxSpeed - true - true - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - true - true - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - Console - - - - - {57eb7322-cd61-4fd1-b595-c511c4648a37} - - - - - - - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj.filters b/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj.filters deleted file mode 100644 index 5a5df6c7..00000000 --- a/src/hal/components/cros/msvs_projects/parameters_test/parameters_test.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Archivos de origen - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj b/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj deleted file mode 100644 index 53720019..00000000 --- a/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj +++ /dev/null @@ -1,159 +0,0 @@ - - - - - Debug - Win32 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 15.0 - {5ABA6561-E322-4334-83E4-F3FEE2965133} - performancetest - 10.0.17763.0 - - - - Application - true - v141 - MultiByte - - - Application - false - v141 - true - MultiByte - - - Application - true - v141 - MultiByte - - - Application - false - v141 - true - MultiByte - - - - - - - - - - - - - - - - - - - - - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - - Level3 - Disabled - true - true - _MBCS;_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) - - - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;ws2_32.lib;%(AdditionalDependencies) - Console - - - - - Level3 - Disabled - true - true - _MBCS;_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) - - - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;ws2_32.lib;%(AdditionalDependencies) - Console - - - - - Level3 - MaxSpeed - true - true - true - true - _MBCS;_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) - - - true - true - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;ws2_32.lib;%(AdditionalDependencies) - Console - - - - - Level3 - MaxSpeed - true - true - true - true - _MBCS;_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS;%(PreprocessorDefinitions) - - - true - true - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;ws2_32.lib;%(AdditionalDependencies) - Console - - - - - - - - {57eb7322-cd61-4fd1-b595-c511c4648a37} - - - - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj.filters b/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj.filters deleted file mode 100644 index a446c2cc..00000000 --- a/src/hal/components/cros/msvs_projects/performance_test/performance_test.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Archivos de origen - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/readme.txt b/src/hal/components/cros/msvs_projects/readme.txt deleted file mode 100644 index 54cc727b..00000000 --- a/src/hal/components/cros/msvs_projects/readme.txt +++ /dev/null @@ -1,50 +0,0 @@ -This is the readme file for the cROS solution of MS Visual Studio. ------------------------------------------------------------------- - -This solution comprises the following projects: -- cros_library: it creates the cros.lib static library, which is used by cROS programs. -- ros_master_python: it creates a ROS master. Since one ROS master is required to have a ROS network working, this program (or other ROS master) must be run before executing any ROS program. -- sample_listener and sample_talker: they create sample_listener.exe and sample_talker.exe, which can be run together to try the operation of a topic publisher, a topic subscriber, a service provider and a service client. -- parameter_test: it checks the parameter subscription and parameter-value update. It must be used together with a ROS program able to update and check parameter values. -- performance_test: this program measures the communication performance of cROS. - -Requirements ------------- -This solution has been designed for Visual Studio 2017. -Windows 10 version 1703 or later ir required to set the socket options TCP_KEEPIDLE, TCP_KEEPINTVL and TCP_KEEPCNT. - -Creation of new projects ------------------------- -If you want to create a new (empty) project in the cros solution for building a new cROS program, the following project options must be set: - -* In the new-project tree: -In references: - Add reference: cros_library -In files: - Add source (and header) files. - -* In the new-project properties: - First select the option: In configuration: "All configurations" and in platform: "All platforms". So the following option changes are applied to all configurations and platforms. -In General: - Output folder: $(SolutionDir)$(Platform)\$(Configuration)\ - Intermediate folder: $(Platform)\$(Configuration)\ -In Debug: - Working directory: $(SolutionDir)$(Platform)\$(Configuration)\ -In VC++ directories: - Directory of header files: add: $(SolutionDir)..\include -In C/C++ -> Preprocessor: - Preprocessor definitions: Add: _CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS -In Linker -> Input: Add: ;ws2_32.lib -In Linker -> System: Set Subsystem to Console - -In Build events -> Post build events (only in cros_library project): - Command line: xcopy /E /Y $(SolutionDir)..\samples\rosdb $(TargetDir)rosdb\ - Description: Copy ROS message definition files to working directory - -You may also want to disable Win32 Control-C exception catching when debugging, so that this event is catched by the cROS program being run. - -cROS library verbose option ---------------------------- -When debugging the debug and information messages of the cROS library can be helpful. They can be activated by changing the value of CROS_DEBUG_LEVEL in cros_defs.h. -The default debug level value for console messages is 1 (#define CROS_DEBUG_LEVEL 1). -The maximum value (the most vebose level) is 4. diff --git a/src/hal/components/cros/msvs_projects/ros_master_python.pyproj b/src/hal/components/cros/msvs_projects/ros_master_python.pyproj deleted file mode 100644 index 4bb39a49..00000000 --- a/src/hal/components/cros/msvs_projects/ros_master_python.pyproj +++ /dev/null @@ -1,62 +0,0 @@ - - - - Debug - 2.0 - {539a16ec-20a8-4148-9a97-542b39a0a62f} - ..\master_python_source\ - runmaster.py - - . - . - {888888a0-9f3d-457c-b088-3a5042f75d52} - Standard Python launcher - - - - - - 10.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj b/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj deleted file mode 100644 index 447b758a..00000000 --- a/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj +++ /dev/null @@ -1,191 +0,0 @@ - - - - - Debug - Win32 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 15.0 - {A5BAFDD4-D98D-4922-9F36-928F7C6C738A} - samplelistener - 10.0.17763.0 - - - - Application - true - v141 - MultiByte - - - Application - false - v141 - true - MultiByte - - - Application - true - v141 - MultiByte - - - Application - false - v141 - true - MultiByte - - - - - - - - - - - - - - - - - - - - - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - - Level3 - MaxSpeed - true - true - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - true - true - Console - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - - - - - - - - - - - - - Level3 - Disabled - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - Console - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - - - - - - - - - - - - - Level3 - Disabled - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - Console - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - - - - - - - - - - - - - Level3 - MaxSpeed - true - true - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - true - true - Console - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - - - - - - - - - - - - - - - - {57eb7322-cd61-4fd1-b595-c511c4648a37} - - - - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj.filters b/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj.filters deleted file mode 100644 index 9b304868..00000000 --- a/src/hal/components/cros/msvs_projects/sample_listener/sample_listener.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Archivos de origen - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj b/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj deleted file mode 100644 index dbbdd978..00000000 --- a/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj +++ /dev/null @@ -1,191 +0,0 @@ - - - - - Debug - Win32 - - - Release - Win32 - - - Debug - x64 - - - Release - x64 - - - - 15.0 - {231D4B94-3DAF-4FF5-AA68-770FA7901E2A} - sampletalker - 10.0.17763.0 - - - - Application - true - v141 - MultiByte - - - Application - false - v141 - true - MultiByte - - - Application - true - v141 - MultiByte - - - Application - false - v141 - true - MultiByte - - - - - - - - - - - - - - - - - - - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - $(SolutionDir)$(Platform)\$(Configuration)\ - $(Platform)\$(Configuration)\ - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - $(VC_IncludePath);$(WindowsSDK_IncludePath);$(SolutionDir)..\include - - - - Level3 - Disabled - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - - - - - - - - - Console - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - - - - - Level3 - Disabled - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - - - - - - - - - Console - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - - - - - Level3 - MaxSpeed - true - true - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - true - true - Console - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - - - - - - - - - - - - - Level3 - MaxSpeed - true - true - true - true - _MBCS;%(PreprocessorDefinitions);_CRT_NONSTDC_NO_DEPRECATE;_CRT_SECURE_NO_WARNINGS - - - true - true - Console - kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies);ws2_32.lib - - - - - - - - - - - - - {57eb7322-cd61-4fd1-b595-c511c4648a37} - - - - - - - - - \ No newline at end of file diff --git a/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj.filters b/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj.filters deleted file mode 100644 index f0da3499..00000000 --- a/src/hal/components/cros/msvs_projects/sample_talker/sample_talker.vcxproj.filters +++ /dev/null @@ -1,22 +0,0 @@ - - - - - {4FC737F1-C7A5-4376-A066-2A32D752A2FF} - cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx - - - {93995380-89BD-4b04-88EB-625FBE52EBFB} - h;hh;hpp;hxx;hm;inl;inc;ipp;xsd - - - {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} - rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms - - - - - Archivos de origen - - - \ No newline at end of file diff --git a/src/hal/components/cros/resources/README b/src/hal/components/cros/resources/README deleted file mode 100644 index bd35d6fe..00000000 --- a/src/hal/components/cros/resources/README +++ /dev/null @@ -1,3 +0,0 @@ -* rosgraph_msgs/Log.msg: standard message type for ROS logs -* ROS_QUIKSTART: some sample official ROS command samples -* ros_testbed/: Sample ROS package definition used for tests diff --git a/src/hal/components/cros/resources/ROS-QUICKSTART b/src/hal/components/cros/resources/ROS-QUICKSTART deleted file mode 100644 index 2611f0d9..00000000 --- a/src/hal/components/cros/resources/ROS-QUICKSTART +++ /dev/null @@ -1,29 +0,0 @@ -# Launch a ROS master node -$ roscore - -# Enumerate active topics -$ rostopic list - -# Enumerate publishers and subscribers on the topic /test -$ rostopic info /test - -# Register a publisher on the topic /test, publishing a "Hello world" string with frequency 1 -$ rostopic pub /test std_msgs/String "Hello world" -r1 - -# Register an echo subscriber on the topic /test -$ rostopic echo /test - -# Run a ROS node registered with catkin -$ rosrun - -# Run sample "gripperstatus_talker" node -$ rosrun gripping_robot gripperstatus_talker - -# Request a service -$ rosservice call - -# Sample service "add_two_ints" request -$ rosservice call /add_two_ints 1 2 - -# Sample publisher with internal fields defined -$ rostopic pub /test cros_testbed/Test "{header: { frame_id: /base_link}}" diff --git a/src/hal/components/cros/resources/cROS_logo.jpg b/src/hal/components/cros/resources/cROS_logo.jpg deleted file mode 100644 index d88d7fc19b4df926e60418e883c95dca19c71044..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 13108 zcmeHtXH-*NxaL8WDnzREsGxL^COtusF1<-t5u^x6M+k%>y(tK21R`C!^bXQ{?}!3H zdP}IGX7cs#-np}8?);oJhZRm%vJdQczwLRRy>U~xdElC=qKYCwaOo1Df&T$;-vBBS zAcJ6%kbnxfL`6VIMS%Mbya9+VUm?13nTY5L5iv0l2^keR87V0l9q8IsDn@!HCI)(j z>&#%To6M}Y*{(C(5@f&4&BMpf$9z*r^e(R`7cU<#0WmQ#87UbpIXNva3j+%a?|=8g zwGpHP0KpZ4-!I_Li{KI=eqzKVq-5mRfJ+2~gqJQ8Ub%AlGJd2l{yX3@)fMWSLidSi zw4V~+a;6myc$Y@P{-C6pPG=a+A@a;6kd%!6Is+rq?K_-Y+&rRU_rxV6rQ{zfC@Lwd zsOswJ8yG$|GPZnfW&Og&*3Q+<-NV!CwRg~);E>R;@QAqh_X&wfA3h?|GcvQXbH3!} zm6nxPR902j)V8#?wRd!Ob@z;nj*U-DPEF4&pca2EEw8Mutz&lg_74t^u*W9^0O23z z;D7&N(7)`#FF(Ph%a;i+6aQup!6i@pi;(K_m779D)c3WCpE}dr5)L4teef==q?wdm zLs2h2T)ynFrcr?sswNAkUd80zke`oFpn^Pg9ogCI_h zZJ%Ipy-^H35!Y9nSd;Tz>cEIo%U*dnmp`6vI0Oe=hh2>=$40avwQt-iKgn`hqI_=t z6&pXlaYX8Xk`+tdw~%!`Tek|$XGOKFt~(SxsSUTPOt>8#BG3MusXM!hk^jcOT@9cm zKVNMyX)}hr70I*nNDR98q)MN6_kE=ydGh#RJCg9*l1ZDl5BTs7QRQRf!Oaran{Us| z(4qyjUJ>oC=WB9l3%qb%%I9uOnvpPQ{#P>*qp!7=Lkf58dh~Kwhs>D$1BEFSyT-O*Z3@&Z z)UqZ&>sb^9+}RN92gw+-0X7{p{O5(!NqR$UsBC9k{Py z5j&7a#=tVlI7^b~wMs&n zaaJk)a=^ZMmp1oqvfF9N>@N1*5)$EJ{^ND*MZCD~nRR1{0?VS`cXu2B6@kxLBYA*R zhsm?Eete`NB(;dYJ2tH>JDqM0_xMBp9u%WTaQMCW`1sSssi@26I~5aR%tqqh=S&rU zrpxQBwQmQmSS{>CvE3W`p$qLy1Fbp^e1m{i_?;Ds_6`zA~+ zZoS(5c<^+}SH#@?WvQC6<5KPsBx2N^TwS9_x&eZcjz6PW`c0##_@P>yQtyQ@whXmH z*M-6XdWpRF1Wmj~9R?*kSbMUjJIz>g?^UT9M~<+xBo44yt}RgR0ra2c@F+vZ2S)yVnjpS> z)^JFD>iXK%W%G!pmtClvuWh-GR%a@X>Z^&4U383IOi2`fVtdj79jiHhjsx5#qr;muV)FHD zGCyu4v}yf5hEko=C&!%d(`v~gnAv$$;nblQJn2+1Nqi5?VmN>- z4F-b2`T=+I@;rVa4A{l>cisToqo zBKE+a_Y!TH?QTUSLIUG$dA~|oU=14uaX^}rJ`VWiQP5bTcKr7EF}$T713xGCF1{eF z06)$;>yrGhJ_3g}3U;1F{%e$x;%N)w{EgL)o6qVscR|uixdz#XPpzMoCP+NFzc2Rr z&1aAvmK|e`|6ksHT4uc*CoFxJhlBLV4It~I#SRC&B!YZdMh-c0 znA+qJ;x_wF!l=1aP#ekX}xCaL>T~goLms z_gTY=0!Zc9($FTT#*BGZa|!ErMWQMnNIzSqAh!h5ie${G#WLJ$v=d@$-PCnPjYkSA zSf#8G6AOeHxdRHY&2-dee`LiCe3%g<&+IX0<^cLlQhvn&c~qx8qpZo~sxWU3`TdEM zS~86;EVJ>rlY616&$$jtEwbiAS=UOO{n}HKW|vOOO9@&`C!8S_4ephzCuU0vI3Qby zzF@`R8a5@=+P$)gNXGM3K+=O0k(vAD6ChRvcQ7yg#(Y!E)>z_D4$LFUG0pNzV_MIE zv@v?SB(00aaMZD2YM(~Y5SrrlAd?ea7tvw%INU+Nx~wJ!HDMTGGbJp=LMMt)Hwrl1 z*<8cOU(ooHtj!eUx%YA~4{AzEId(1~jyD~zPxWA?KPei@##O8StIhTUca)lz3lU?9r8${=gwsrCGm@l41q@P+nQSe4$jr zYxzo!`L&Qo7|y_h9gumq6FkS1PPeTj?fKJnl47DPviCm_P~+z1DN|G0+PT&_~qdBPZ4lyMJXlvS@m9&Tx2)au&gMcCohz?2rru0 zFWh6;u{!%qv*$PT%`yT;8uez)@>jWlnZ`hW3j7;_*vlGsG3+4s&y=7}I4O*Gk^Hsa zWKR2)MNr~09ohW3eRd2_7H0-c#EW(?tqeJQ9!#T_HRrd>zIR`OMY*xb_~fdr*F>nW zkX`3Zldx0pp*+SEQT(Q8UpkjuE&Hp`^Svb`dfqVTqs~%pUoE0})#vxi0y_?@-@^nI<7&;PzwqAIa-oYn=osunU%2b(9H)n${W4Ks!R{Qy^ zL8Y_IB=uLlhm3r-s;WGjpu$Gb#h?e1Lt$giBG<(Fxxah4Os09~*kou#-)+ub&4>03 zo{+$@vdYrno8^HLmiL48Oab10hX$kP`~FG_?lbEQ-c)jIRv5wg$&fR9C%z_YHGZu? zon^Xa&hxZcL$j+^s+vCOrn(S*Y`}bjNi)-Ctz$8%DNFm^)6(KEVmm{{OvB6J@dlEJ zDW##N(yF#_(G*)4f1WxHpg!&BFOx(`%0faTcK2AsMSU|p6LF zbFpip8u(D;H<KY@rEEJd=hPN+yEF}719R!WE#7D zJBwg9u{Ex6IAt`-i0Y?qC3E_vI3J=d^AH-QU&nV~G4R&AS?Z{}`3kkNv*6@&;)7x> ztdI=>qx{utBuAeG6zIx@gltgJr0e?52m|F@4V@qHt{acuvi^?D82MCqw%*ZA4u{y- zE$TRr3*8F7WmaM8bK#z?4CKHSh`O*yJR8 z@@gX!4(P%GpW&0g-~%OXa5D2C6J(~^kZvhAF*&52dD+4sWElI}gq^a;^Lj?~W%h-V@{zM4IS z)YE8}DZg#o?P}JhEu)#DEGIbLz~r=-Vp~x}S+D{1a*d}|rsyyA&IgjvXkQ3*Y-wha zE~9i=a8aCQ0S>Uz3~`>ZU2LyyyIJ%C)z9MF4sJi-geG`>jJaL4xmcz9SSfcu<+VaE z@H<{&5p2f+1V-4SmLz(e^um2R^O=_}(HD5SRV7j(kp*LZ;tq zVjjxs2Q<^|568vT-eoVZj+bpk)i|#xHh?Q|EDf33(N1AxaKZtCFu#^mMWhdG=&^)Wp;Zt-STrt0`Qq$di zAg0>Uj;G+iuOY?Tgy@KZU9rVV@M8h>8f8C_e>z*t&F!B~xqES$S!dp0K%EXwuIZ zLP-i4Ix^pT)Hr_W`zzM|OhiIx8+7+l6AmciKg&zQ+<*!%;ef|DK-a=F#x)&lk0+KF z0XV?o9S)G{q&nmFrf~FoC-?)bdaV2EFTDgrab9%r_+=w=`}01Tjot(Wb1YB^SKSw$ z5;s17M>B;jYV_|q5}YO*FM{0Bw!jZ}!h5)c1z0%p}Y+C4)Qp;TKeM z!6!>7$n;O}9jK0JFf>hUI5=mECzD&6eQ!LRBhE!r*Fx)wBlfy{d|RsEh+EkElG+q~ z|BKb7COLbaaoH4cfi7D!WX6epl$7Hqd#jgePphIW<%pjMS@3*!+?@Tb1pbLkAR^~Q z^*jj8X#R>%HixabX~Iulae&8m?SJ(0?A>qP7etZp;tG1tW*qzJj znPgbyr0(kdi-ZOJG%3ds#oTPS@wdNg3776v1?SL+Dq9f}bp0apykz_r_xe4Wz{0SX zSl8Ft>yu0q3FQ4b1D-wCp11wL0h9-emahhcU1R#rMM7MA+_^d_^k>u*mSb=5Om~bT z9~mb5Mh~xnNgCPAvGGP#^$h{2k(&;En?lP}XSeF^R@>?&{Os(1al9nMdR^mF{oK6o zlMG7dQJag39dLZiU-9tw3X7b{#~lvSNOHvlPzLq&W1(Vkk(6AMIstOsEh)76gTF$H48RD^ zd{;7)UkGpbzPENnDzGExr+JR3+nwH>sB(LF2)R?7CM@f7_;{Qmw7rh})vKzKZgM{| z6MXS4y4oS0KKCrG=12jC`Km=6=QnIYsTm`~BOr=x9^Vv32wvK@f z$Q*64MS=Qp#WGaOt3u6ck*0_N^i1evlESq1{XtA{ZrLn8sFl2QA5YWgmD##hf$NXs zq2}W&vHJQfljq9#86V zcaRMm+*x*i%$n0O2-ulJFtqpXX_p%F5<0sm_2RKXY=tYznJR<1p(9~%2t;91>Bl;> zxnFosh_$f%={1n!!Jf#c`>tLbD zuhzcMQ#z9%R}^nDteG0x7j(|s*>BMjeZyDNT=Mfjo?!uX=0EjB$G17(UFKsU$S6NMrGuiE&l(uL=~xGcW{!iN;C&(Ex(NDq zF4(ankda;b*q-BgwMC-t#UyspgPEBJvN2A>HG6!$Bb5MiJ!tzUtGxz7qWSoRcI`d3 z8dfON;o39qaq|sf+l<7*Hhj?{$__$NnB1>3Nx5}_8nId(uA!sJVa!*dT&eRSKTH~b zY0e9$n~B`;3-PLc3o`XAzE^2=TSCdc~nD7k;=)S}Jb z*p-~gJ$fL^_ex5XpzK&5wZzc#64_vG6$P_^9XJST{lh{B!HcBpj#W^uj6-P99q|k5Jp53F@|d5OS>8%ou4{T@*7XupTvZUaE$zw}w1~TLKxHj> ztN3d7lt~X1vtT};U*4$b4no*Zia@f?Ba&a6bHvsNRd`JoVMMci<<0N^+p|g?_)g7> zVHVdcv)p^a3lqd-taHEg^D-R!z=~|n6vRMV^PnrM;Gs!~AXa#v#S{8)W2yg-zEYW% zGh=se>&kh#;k7EgcZ7t;qv|fgl`f z5K(zr=ECAJtHNJ0@76)iX5~U!`RTLA1(WQ>Np-!CkE8>R!VZh#+vxwP{C@(h!T%)} zm_dsrGlfRE`AC;jn+&>y;q?#ATV;iuOb$h@CdgN@fqsPFa6mXk)%l)<^uf$rpM6iz z1#OZ)4p=30zyYw~-#%mc0$ot?0$upun>*?Un-!il_e8<3pOK&wRzd|AhWD%a#fZX= zX}C@cV|dmU^EyqteMa8=xPdnWy;0jWO5zr)Yc zDMQK1sCgseHr;JJ4o_;q0rahlnpf6N+7up^-k`a0K}!4+oC2%x)<{sIcT5=`pJ4xu z!Y*5U*Xc_UZ#}B>GD5JN--nJgyv#ivF;{ej=oZ`bo$wwCjnNCzZx{$n?Y%r}Ke{;v zzh&Rk&%Ui_ZuRYCw_aZBpXx4Ce25q`=v*KwVROFS9m8?5qXU<-FC$>HTmASF#%!Vd z%-e8t>6ugG-Bt1`ZsxkCuT+&#Q$sc>gS1;2HAhRZ>97CPIR?Mm=+sb@i7rt^_II}o~G0adBi5p43*;=xv86hK5 z;ANrZBkR&(Z}fiNiA8*9UufWffMH?dL&tn7`2H^PLxA)71Pj%1#IfDj56SArknSI>SFG>;VLJh9 zAiD_56-H44j9{C(-HI8;FU#GX9Wsp&y^PMwVE`wYm%=z+O-a2{9q4W{ zaMcfAL-;X|7rCiBj$HJ}vkY~SuvhQr=ZKSXvwva?3Aq)Ot8OOh+iUeINvl)l1viIX z@4jSlmd5%DO8>?J=k?)4?zg|=#((7z5(Pdcu)qP6^cVVC*tg)1E;yh7o?Bd`BsmNd z!vT0cAcq5r@su{jGVFr3fv5_qoU|RA1S40ngrFVE+of5gA z#bmtswuuEoCN}N;r0zHS(HloQ)8d`-3K-Ew)}|zS@7R;%RC?{fbrJ03$B$aeq2(s& z^C#fvhHvcf6C{4?H#WmKenDq85LvT-tTRt}?tvVR_E_3mDyZ*93}K%&Mz!r+T*kif zazgnowxC!(4mrh-{a}Ih`G6~ z^nIl|0cPYA!w~#Dsex_@S^~xIhNIgRqy_xNHxhU0esFNjmqsme_=#!o^G=bE-}rOA zT~j5{jk z5#uWS0hH|Ik*t+5nR0u+$os-JU*A(Edywk_zlGYnD&M$Y$~Bt|1hM_S zf$Uo(ttClV9`R?!;tbWEPSi;J`(&h!-SZHoLV%Gi-T!K=_Ma{J-G&^I(wEPK>|(2X!ka_+eh0I$PwUC6}?1kU{5J4|Y9bLdCjJD3m)bQ`-uu6L-X~+@?E$-0xRi?pPz!bh;Tm7BfFNrl_hS`b0#t4$*S*b9=f+^K#^^yPNmBayT*z zzMWsg>Y}D#FXp4A^~0M#WOq->6a)n!i-|GCiETP=c@m#;+}sbsTKq-M6tWcxZ46Id z&NFO(wu$k1{t7PjMWsVk8PwlIy{2p9eUX-Jn!!<7Ci-05l(6~itNr>~zXp$5ZqU$R z_zX9-@>G7>)l83D1xj;Bf_hitXc&df7G9&anf?;EEVK7k0{Q?C%==KQQu<7*{ps7W z%0)VVMlKnmGR+~&w(9!iNP)r7O-~OAfXq2EzLapU6@`qn!W2ilQ#?H)$@Kl2cEQm3 z1DxQi$P`p(G!*{mJ=KKL>QzmEu7wO?^1t3NQ_bhz@| zhwjz;r`S1DI0e>FXW~|+xTfv(54VV!Yrq$^Bkv`bGiKGNuuYv%HW?F?lQ6Rna`XG0 z#hxk&4sV*D4eR**yaVqt&U-y`Wh+t0RtuV!ISn8CD~?V?H~Wzdqj=O!uF0&F8mabK zVH)Y3do}GnJ~G)GU<-d2aAJIgsSLoMCU}$dkc@c7ug{b1)RV=prr4gRiLc?&GHlVj zbdRhhMbj{qpJcujT-kUlbrHV-sGu&Nuv)rNCGGX(|_Ln*BMAC_B zlFl;A*B)YhSJR*F`l`Dac07o$+&sBj94aKSx_2hDJs_fv#e4fm+MR+&d4V$|dROkK z$ZJ{kBxAa;S)bNPBWrpS!Zs<21FqS2`(&ANpv9fXzz?LE_PNKR#!-W@?F+n5?WEij zK4v#SnaVzJ5yY#p2+5R5W!jL72z?`*2-%cq@6N1}iEW3nxcLgNt2yvY@BEP1lYc65 zZacnT@E0C~CjwSiEf=k$Mvcyrshd=gv+`hM;ij ze{l5wHk^)=jDB z1IczvOq7(+Neq*NR+u|MB-qx);mL~6WCFx$+t z&+yXM;551eJv*MaI*JrVAHw=#ir(aNcd?fgKJ?8wm4+ErU?7oBX&W>J-s5?jIxB46 zF>ibhi;*1*tdg!u*7l61M?8Cfbe5}4)b=x;p`srTSk0ICx6)1HI*Y#^<-fTt>C(nr za*P8o_8mIdm5wSzS>RWAewF5ShZI~G&U9ib2C`It*rKtyX#ZJh8eVeo<^=A)BPRJa z-(of;*?p`WdEzFmAM+Pq@Ol-$6$3o{D{Hf1h$czP^uxh{H| zvkp)330+Db!lRQBwj0%>GUE0*#@%IGo4o>3TMUbs<@&Rhj3`<|-HmGbD@;>42KMt7 z;d^vmIqL*+;71n&#%D93!A@@9Kay8OKI1zeQSdSHRpOFp3|*Fa8W_#;$gUZtW}!Rw zZz-*T3#N?S`)`S9l1*Wa*Q+q0Z^S6reu3r`VU_ILP)pq^yWh!k}LNwP=m`ana2Zy-Xk9T&u3 z#|Z^zJl<4mp9xb|>|zlD2}(3>+c6_w8MLlYz?I7<-FyngwZp9{Y97R7x)bG5JKL$f zf?UQnp|?*Lj>3LbB$%s1bkr+4B&_x!2m95gkL(rCA3ZO4{OBP0kD1Gzr~T;sElj8hu}lMUcCIB#y!-{|pu)8j4$QOyN@WMkZa7!yVYj!^{+v%|0uurR#{% zcZoAzFDrOo+p?EMjLd5ki0Q}i$<}^6m!xt)3+IqWv1$w(Z9;mOLs?Sc8#g}yoq_|R z&eUx)*M>zHSzZ?rQ{}k8$$A=WMuOs*3}39mgZJ&>ATPgGl~S#*#%)y2b7L2;i<6RP zfBic7l%ewv`}?zFOU{KEi)>tg#`ag`M0%;HUSw}N8ySYmiqlQ)3)kzkpw=c5yEj&0 zhOZsOQpPHegvwnN+sz(739`61s?m2|l%hNqk!&`4Gt+1TT2>Y6D0udit;iAwJp5`P zJ@}wqb@8C+>0n|(Tn$#bsEqlvE~mtE}_D!zD^{T8S=Di>T6vY+eX z+VAa``;0cef;P-2EYv$FHFJ+c87wVLXqdF%WRqhDg4ghSf@C=0qPl9gSQShSkvr>6 zI=4Jm_jQpRh+HT>i#;Je_ftCCU{OM6t%}v0JZy6F1n9xv|>CEl@4`0V2`rQk+S%R>;P^LXs zCKmJxo?{)99Hw~PNyCHbC)mLjgkj{qQTY$+A660fcPk;!Wy55nZB+|$dl9$nR@$5B zAmsKx$!id(3C^YwVbwvVU!U>#1%Ei9^FdzO@JEUA*J_tC14-CRick;!U|5t%&E_PN zFiXG68UFWifpWIWEu3Q$V)iX@^&duFobE{I%bQ3Kr@c`sb66_3)tdz}=kgozvOcXRcaWcLyREWJ zZa37z$zpQGP7wPyHSQWjK;-En^CP~j8zf9;{sjRa#w!l@1T4Oi=e-)pWw#iWY&NkH z?}eQA2+cE7anA0)lR7w<#BfDqf6J6k7_0JA3TzUHI@4$PqHj9)VU7ZJb=d9umu)fL zaHk&ErD_6^{Tem!YK%X4De7>kGF+?3S41x*r5obtG;PrR0f|)+ki?%#)Pi<2Xl|+C0B)ABA@s`T zpl$j_B~u(W@U`)T)%%-oD5hkk#x#YKQHu`E_rVV-0lMYHzf=l>&_xT18Lz5V@tJ#{ zp{_2UJS}fix{#b)<5ksXmCO|gv|9;)xAGXlhq|3{fFCh-GUGfSy2Sa8dd9Ce@i0ki z_CCtzJ%W=m>ITa-7m%^0XuN^*{VreToYvbF(e*T{)`=1=bt6hs^FI_HG@K4{XF&zZfGNso}BQ&@7D5Rz6TO@p5>o?2xXUt{tq$02s16Y-|6= abJ~~wQEL9v2KK)&;NM*D{|^Ch)Bgj_b*Bpe diff --git a/src/hal/components/cros/resources/cros_testbed/.gitignore b/src/hal/components/cros/resources/cros_testbed/.gitignore deleted file mode 100755 index b25c15b8..00000000 --- a/src/hal/components/cros/resources/cros_testbed/.gitignore +++ /dev/null @@ -1 +0,0 @@ -*~ diff --git a/src/hal/components/cros/resources/cros_testbed/BUILD b/src/hal/components/cros/resources/cros_testbed/BUILD deleted file mode 100755 index 6def7d22..00000000 --- a/src/hal/components/cros/resources/cros_testbed/BUILD +++ /dev/null @@ -1,17 +0,0 @@ -BUILD -===== - -Create a catkin environment following the guide at: - - http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment - -Assume the catkin workspace is located at "~/catkin_ws", link -the current folder in the catkin source folder, for example: - - $ ln -s ~/itr-cros/cros_testbed ~/catkin_ws/src - -To compile and install do the following: - - $ cd ~/catkin_ws - $ catkin_make - $ catkin_make install diff --git a/src/hal/components/cros/resources/cros_testbed/CMakeLists.txt b/src/hal/components/cros/resources/cros_testbed/CMakeLists.txt deleted file mode 100755 index eb2b3001..00000000 --- a/src/hal/components/cros/resources/cros_testbed/CMakeLists.txt +++ /dev/null @@ -1,160 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(cros_testbed) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -####################################### -## Declare ROS messages and services ## -####################################### - -## Generate messages in the 'msg' folder -add_message_files( - FILES - DoubleVector.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - add_two_ints.srv - ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) -generate_messages() - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES cros_testbed -# CATKIN_DEPENDS roscpp rospy std_msgs message_runtime -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(include - ${catkin_INCLUDE_DIRS} -) - -add_executable(clock_listener src/clock_listener.cpp) -target_link_libraries(clock_listener ${catkin_LIBRARIES}) - -add_executable(counter_listener src/counter_listener.cpp) -target_link_libraries(counter_listener ${catkin_LIBRARIES}) - -add_executable(vector_listener src/vector_listener.cpp) -target_link_libraries(vector_listener ${catkin_LIBRARIES}) - -add_executable(vector_talker src/vector_talker.cpp) -target_link_libraries(vector_talker ${catkin_LIBRARIES}) - -add_executable(add_two_ints src/add_two_ints.cpp) -target_link_libraries(add_two_ints ${catkin_LIBRARIES}) - -add_executable(test_services_list src/test_services.cpp) -target_link_libraries(test_services_list ${catkin_LIBRARIES}) - -add_executable(std_msgs_talker src/std_msgs_talker.cpp) -target_link_libraries(std_msgs_talker ${catkin_LIBRARIES}) - -add_executable(std_msgs_listener src/std_msgs_listener.cpp) -target_link_libraries(std_msgs_listener ${catkin_LIBRARIES}) - -## Declare a cpp library -# add_library(cros_testbed -# src/${PROJECT_NAME}/cros_testbed.cpp -# ) - -## Declare a cpp executable -# add_executable(cros_testbed_node src/cros_testbed_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -add_dependencies(clock_listener cros_testbed_generate_messages_cpp) -add_dependencies(counter_listener cros_testbed_generate_messages_cpp) -add_dependencies(vector_listener cros_testbed_generate_messages_cpp) -add_dependencies(vector_talker cros_testbed_generate_messages_cpp) -add_dependencies(add_two_ints cros_testbed_generate_messages_cpp) -add_dependencies(test_services_list cros_testbed_generate_messages_cpp) -add_dependencies(std_msgs_talker cros_testbed_generate_messages_cpp) -add_dependencies(std_msgs_listener cros_testbed_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(cros_testbed_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS cros_testbed cros_testbed_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_cros_testbed.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/hal/components/cros/resources/cros_testbed/include/.keep b/src/hal/components/cros/resources/cros_testbed/include/.keep deleted file mode 100755 index e69de29b..00000000 diff --git a/src/hal/components/cros/resources/cros_testbed/msg/DoubleVector.msg b/src/hal/components/cros/resources/cros_testbed/msg/DoubleVector.msg deleted file mode 100755 index 7353eb1f..00000000 --- a/src/hal/components/cros/resources/cros_testbed/msg/DoubleVector.msg +++ /dev/null @@ -1 +0,0 @@ -float64[] val diff --git a/src/hal/components/cros/resources/cros_testbed/package.xml b/src/hal/components/cros/resources/cros_testbed/package.xml deleted file mode 100755 index cfddf4bb..00000000 --- a/src/hal/components/cros/resources/cros_testbed/package.xml +++ /dev/null @@ -1,60 +0,0 @@ - - - cros_testbed - 0.0.0 - The cros_testbed package - - - - - albe - - - - - - TODO - - - - - - - - - - - - - - - - - - - message_generation - - - - message_runtime - message_runtime - - - catkin - rospy - roscpp - std_msgs - rospy - roscpp - std_msgs - - - - - - - - - - - diff --git a/src/hal/components/cros/resources/cros_testbed/src/add_two_ints.cpp b/src/hal/components/cros/resources/cros_testbed/src/add_two_ints.cpp deleted file mode 100755 index d4ef647b..00000000 --- a/src/hal/components/cros/resources/cros_testbed/src/add_two_ints.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "ros/ros.h" -#include "cros_testbed/add_two_ints.h" - -bool add(cros_testbed::add_two_ints::Request &req, - cros_testbed::add_two_ints::Response &res) -{ - res.sum = req.a + req.b; - ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b); - ROS_INFO("sending back response: [%ld]", (long int)res.sum); - return true; -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "add_two_ints_server"); - ros::NodeHandle n; - - ros::ServiceServer service = n.advertiseService("add_two_ints", add); - ROS_INFO("Ready to add two ints."); - ros::spin(); - - return 0; -} diff --git a/src/hal/components/cros/resources/cros_testbed/src/clock_listener.cpp b/src/hal/components/cros/resources/cros_testbed/src/clock_listener.cpp deleted file mode 100755 index 62134d35..00000000 --- a/src/hal/components/cros/resources/cros_testbed/src/clock_listener.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" - -/** - * This tutorial demonstrates simple receipt of messages over the ROS system. - */ -void chatterCallback(const std_msgs::String::ConstPtr& msg) -{ - ROS_INFO("I heard: [%s]", msg->data.c_str()); -} - -int main(int argc, char **argv) -{ - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. For programmatic - * remappings you can use a different version of init() which takes remappings - * directly, but for most command-line programs, passing argc and argv is the easiest - * way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ - ros::init(argc, argv, "clock_listener"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - ros::NodeHandle n; - - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called chatterCallback. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ - ros::Subscriber sub = n.subscribe("topic_clock", 1000, chatterCallback); - - /** - * ros::spin() will enter a loop, pumping callbacks. With this version, all - * callbacks will be called from within this thread (the main one). ros::spin() - * will exit when Ctrl-C is pressed, or the node is shutdown by the master. - */ - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/src/hal/components/cros/resources/cros_testbed/src/counter_listener.cpp b/src/hal/components/cros/resources/cros_testbed/src/counter_listener.cpp deleted file mode 100755 index 0db818ab..00000000 --- a/src/hal/components/cros/resources/cros_testbed/src/counter_listener.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" - -/** - * This tutorial demonstrates simple receipt of messages over the ROS system. - */ -void chatterCallback(const std_msgs::String::ConstPtr& msg) -{ - ROS_INFO("I heard: [%s]", msg->data.c_str()); -} - -int main(int argc, char **argv) -{ - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. For programmatic - * remappings you can use a different version of init() which takes remappings - * directly, but for most command-line programs, passing argc and argv is the easiest - * way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ - ros::init(argc, argv, "counter_listener"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - ros::NodeHandle n; - - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called chatterCallback. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ - ros::Subscriber sub = n.subscribe("topic_counter", 1000, chatterCallback); - - /** - * ros::spin() will enter a loop, pumping callbacks. With this version, all - * callbacks will be called from within this thread (the main one). ros::spin() - * will exit when Ctrl-C is pressed, or the node is shutdown by the master. - */ - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/src/hal/components/cros/resources/cros_testbed/src/std_msgs_listener.cpp b/src/hal/components/cros/resources/cros_testbed/src/std_msgs_listener.cpp deleted file mode 100755 index 43ee8cbc..00000000 --- a/src/hal/components/cros/resources/cros_testbed/src/std_msgs_listener.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -void chatter1(const std_msgs::Bool::ConstPtr& msg) -{ - ROS_INFO("I heard: [%s]", msg->data ? "true" : "false"); -} - -void chatter2(const std_msgs::Byte::ConstPtr& msg) -{ - ROS_INFO("I heard: [%i]", msg->data); -} - -void chatter3(const std_msgs::Char::ConstPtr& msg) -{ - ROS_INFO("I heard: [%i]", msg->data); -} - -void chatter4(const std_msgs::Duration::ConstPtr& msg) -{ - ROS_INFO("I heard: sec=[%i] nsec=[%i]", msg->data.sec, msg->data.nsec); -} - - -void chatter5(const std_msgs::Header::ConstPtr& msg) -{ - ROS_INFO("I heard: seq = [%i], time = [%i, %i], frame_id = [%s]", msg->seq, msg->stamp.sec, msg->stamp.nsec, msg->frame_id.c_str()); -} - -void chatter6(const std_msgs::Int16::ConstPtr& msg) -{ - ROS_INFO("I heard: [%i]", msg->data); -} - -void chatter7(const std_msgs::Int32::ConstPtr& msg) -{ - ROS_INFO("I heard: [%i]", msg->data); -} - -void chatter8(const std_msgs::Int64::ConstPtr& msg) -{ - ROS_INFO("I heard: [%lli]", msg->data); -} - -void chatter9(const std_msgs::Int8::ConstPtr& msg) -{ - ROS_INFO("I heard: [%i]", msg->data); -} - -void chatter10(const std_msgs::Time::ConstPtr& msg) -{ - ROS_INFO("I heard: sec=[%i] nsec=[%i]", msg->data.sec, msg->data.nsec); -} - -void chatter11(const std_msgs::UInt16::ConstPtr& msg) -{ - ROS_INFO("I heard: [%i]", msg->data); -} - -void chatter12(const std_msgs::UInt32::ConstPtr& msg) -{ - ROS_INFO("I heard: [%i]", msg->data); -} - -void chatter13(const std_msgs::UInt64::ConstPtr& msg) -{ - ROS_INFO("I heard: [%lli]", msg->data); -} - -void chatter14(const std_msgs::UInt8::ConstPtr& msg) -{ - ROS_INFO("I heard: [%i]", msg->data); -} - -void chatter15(const std_msgs::Float32::ConstPtr& msg) -{ - ROS_INFO("I heard: [%f]", msg->data); -} - -void chatter16(const std_msgs::Float64::ConstPtr& msg) -{ - ROS_INFO("I heard: [%f]", msg->data); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "std_msgs_listener"); - - ros::NodeHandle n; - ros::Subscriber sub1 = n.subscribe("bool", 1000, chatter1); - ros::Subscriber sub2 = n.subscribe("byte", 1000, chatter2); - ros::Subscriber sub3 = n.subscribe("char", 1000, chatter3); - ros::Subscriber sub4 = n.subscribe("duration", 1000, chatter4); - ros::Subscriber sub5 = n.subscribe("header", 1000, chatter5); - ros::Subscriber sub6 = n.subscribe("/int16", 1000, chatter6); - ros::Subscriber sub7 = n.subscribe("/int32", 1000, chatter7); - ros::Subscriber sub8 = n.subscribe("/int64", 1000, chatter8); - ros::Subscriber sub9 = n.subscribe("/int8", 1000, chatter9); - ros::Subscriber sub10 = n.subscribe("/time", 1000, chatter10); - ros::Subscriber sub11 = n.subscribe("/uint16", 1000, chatter11); - ros::Subscriber sub12 = n.subscribe("/uint32", 1000, chatter12); - ros::Subscriber sub13 = n.subscribe("/uint64", 1000, chatter13); - ros::Subscriber sub14 = n.subscribe("/uint8", 1000, chatter14); - ros::Subscriber sub15 = n.subscribe("/float32", 1000, chatter15); - ros::Subscriber sub16 = n.subscribe("/float64", 1000, chatter16); - - ros::spin(); - - return 0; -} diff --git a/src/hal/components/cros/resources/cros_testbed/src/std_msgs_talker.cpp b/src/hal/components/cros/resources/cros_testbed/src/std_msgs_talker.cpp deleted file mode 100755 index 5e4bdd58..00000000 --- a/src/hal/components/cros/resources/cros_testbed/src/std_msgs_talker.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "std_msgs_talker"); - ros::NodeHandle n; - ros::Publisher chatter1 = n.advertise("bool", 1000); - ros::Publisher chatter2 = n.advertise("byte", 1000); - ros::Publisher chatter3 = n.advertise("char", 1000); - ros::Publisher chatter4 = n.advertise("duration", 1000); - ros::Publisher chatter5 = n.advertise("header", 1000); - ros::Publisher chatter6 = n.advertise("int16", 1000); - ros::Publisher chatter7 = n.advertise("int32", 1000); - ros::Publisher chatter8 = n.advertise("int64", 1000); - ros::Publisher chatter9 = n.advertise("int8", 1000); - ros::Publisher chatter10 = n.advertise("time", 1000); - ros::Publisher chatter11 = n.advertise("uint16", 1000); - ros::Publisher chatter12 = n.advertise("uint32", 1000); - ros::Publisher chatter13 = n.advertise("uint64", 1000); - ros::Publisher chatter14 = n.advertise("uint8", 1000); - ros::Publisher chatter15 = n.advertise("float32", 1000); - ros::Publisher chatter16 = n.advertise("float64", 1000); - ros::Rate loop_rate(1); - std::cout<getServerURI()< - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "test_services_list"); - - /*if (argc != 3) - { - ROS_INFO("usage: add_two_ints_client X Y"); - return 1; - }*/ - - ros::NodeHandle n; - ros::ServiceClient client; - - //Test add_two_ints service - client = n.serviceClient("add_two_ints"); - - cros_testbed::add_two_ints srv_add_two_ints; - srv_add_two_ints.request.a = 3; - srv_add_two_ints.request.b = 5; - - if (client.call(srv_add_two_ints)) - { - ROS_INFO("Sum: %ld", (long int)srv_add_two_ints.response.sum); - } - else - { - ROS_ERROR("Failed to call service add_two_ints"); - return 1; - } - - return 0; -} diff --git a/src/hal/components/cros/resources/cros_testbed/src/vector_listener.cpp b/src/hal/components/cros/resources/cros_testbed/src/vector_listener.cpp deleted file mode 100755 index 7603a0eb..00000000 --- a/src/hal/components/cros/resources/cros_testbed/src/vector_listener.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include "cros_testbed/DoubleVector.h" - - -void chatterCallback(const cros_testbed::DoubleVector::ConstPtr& msg) -{ - for(int i = 0; i < msg->val.size(); i++) - ROS_INFO("I heard: [%f]", msg->val[i]); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "vector_listener"); - ros::NodeHandle n; - ros::Subscriber sub = n.subscribe("double_vector", 1000, chatterCallback); - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/src/hal/components/cros/resources/cros_testbed/src/vector_talker.cpp b/src/hal/components/cros/resources/cros_testbed/src/vector_talker.cpp deleted file mode 100755 index be00b75b..00000000 --- a/src/hal/components/cros/resources/cros_testbed/src/vector_talker.cpp +++ /dev/null @@ -1,87 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/Float64.h" -#include "ros/xmlrpc_manager.h" -#include "cros_testbed/DoubleVector.h" - -#include - -/** - * This tutorial demonstrates simple sending of messages over the ROS system. - */ -int main(int argc, char **argv) -{ - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. For programmatic - * remappings you can use a different version of init() which takes remappings - * directly, but for most command-line programs, passing argc and argv is the easiest - * way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ - ros::init(argc, argv, "vector_talker"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - ros::NodeHandle n; - - /** - * The advertise() function is how you tell ROS that you want to - * publish on a given topic name. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. After this advertise() call is made, the master - * node will notify anyone who is trying to subscribe to this topic name, - * and they will in turn negotiate a peer-to-peer connection with this - * node. advertise() returns a Publisher object which allows you to - * publish messages on that topic through a call to publish(). Once - * all copies of the returned Publisher object are destroyed, the topic - * will be automatically unadvertised. - * - * The second parameter to advertise() is the size of the message queue - * used for publishing messages. If messages are published more quickly - * than we can send them, the number here specifies how many messages to - * buffer up before throwing some away. - */ - ros::Publisher chatter_pub = n.advertise("double_vector", 1000); - - ros::Rate loop_rate(1); - std::cout<getServerURI()<() call, as was done - * in the constructor above. - */ - chatter_pub.publish(msg); - - ros::spinOnce(); - - loop_rate.sleep(); - ++count; - } - - - return 0; -} \ No newline at end of file diff --git a/src/hal/components/cros/resources/cros_testbed/srv/add_two_ints.srv b/src/hal/components/cros/resources/cros_testbed/srv/add_two_ints.srv deleted file mode 100755 index 43158244..00000000 --- a/src/hal/components/cros/resources/cros_testbed/srv/add_two_ints.srv +++ /dev/null @@ -1,4 +0,0 @@ -int32 a -int32 b ---- -int32 sum diff --git a/src/hal/components/cros/resources/rosgraph_msgs/Log.msg b/src/hal/components/cros/resources/rosgraph_msgs/Log.msg deleted file mode 100644 index 9a9597c2..00000000 --- a/src/hal/components/cros/resources/rosgraph_msgs/Log.msg +++ /dev/null @@ -1,19 +0,0 @@ -## -## Severity level constants -## -byte DEBUG=1 #debug level -byte INFO=2 #general level -byte WARN=4 #warning level -byte ERROR=8 #error level -byte FATAL=16 #fatal/critical level -## -## Fields -## -Header header -byte level -string name # name of the node -string msg # message -string file # file the message came from -string function # function the message came from -uint32 line # line the message came from -string[] topics # topic names that the node publishes diff --git a/src/hal/components/cros/samples/CMakeLists.txt b/src/hal/components/cros/samples/CMakeLists.txt deleted file mode 100644 index e2e710d7..00000000 --- a/src/hal/components/cros/samples/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -file(COPY rosdb DESTINATION ${EXECUTABLE_OUTPUT_PATH}) - -add_executable(parameters-test parameters-test.c) -target_link_libraries(parameters-test cros) - -add_executable(api-test api-test.c) -target_link_libraries(api-test cros) - -add_executable(ros-i-trajectory-test ros-i-trajectory-test.c) -target_link_libraries(ros-i-trajectory-test cros) - -add_executable(talker talker.c) -target_link_libraries(talker cros) - -add_executable(listener listener.c) -target_link_libraries(listener cros) - -add_executable(performance-test performance-test.cpp) -target_link_libraries(performance-test cros m) diff --git a/src/hal/components/cros/samples/api-test.c b/src/hal/components/cros/samples/api-test.c deleted file mode 100644 index bfa0fb7d..00000000 --- a/src/hal/components/cros/samples/api-test.c +++ /dev/null @@ -1,402 +0,0 @@ -#include -#include -#include -#include -#include - -#ifdef _WIN32 -# define WIN32_LEAN_AND_MEAN -# include -# include - -# define DIR_SEPARATOR_STR "\\" -#else -# include -# include -# include - -# define DIR_SEPARATOR_STR "/" -#endif - -#include "cros.h" - -// TODO Signal handler - -static void printHelp( char *cmd_name ) -{ - printf("Usage: %s [OPTION] ... \n", cmd_name); - printf("Options:\n"); - printf("\t-name Set the node name (default: /test_node)\n"); - printf("\t-host Set the node host (default: 127.0.0.1)\n"); - printf("\t-chost Set the roscore host (default: 127.0.0.1)\n"); - printf("\t-cport Set the roscore port (default: 11311)\n"); - printf("\t-sub run as a subscriber (default mode)\n"); - printf("\t-pub run as a publisher\n"); - printf("\t-h Print this help\n"); -} - -CrosNode *node; - -static CallbackResponse gripperstatus_sub_callback(cRosMessage *message, void* data_context) -{ - CallbackResponse cb_resp; - cRosMessageField *PowerStatus_field = cRosMessageGetField(message, "PowerStatus"); - cRosMessageField *ClampStatus_field = cRosMessageGetField(message, "ClampStatus"); - cRosMessageField *AlarmCodes_field = cRosMessageGetField(message, "AlarmCodes"); - cRosMessageField *Configuration_field = cRosMessageGetField(message, "Configuration"); - - uint32_t PowerStatus = PowerStatus_field->data.as_uint32; - uint32_t NAlarmCodes = AlarmCodes_field->array_size; - uint32_t Configuration = Configuration_field->data.as_uint32; - uint32_t ClampStatus[4]; - ClampStatus[0] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 0); - ClampStatus[1] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 1); - ClampStatus[2] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 2); - ClampStatus[3] = *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 3); - - uint32_t *AlarmCodes = (uint32_t *)malloc(NAlarmCodes*sizeof(uint32_t)); - if (AlarmCodes != NULL) - { - uint32_t it; - for (it = 0; it < NAlarmCodes; it++) - { - AlarmCodes[it] = *cRosMessageFieldArrayAtUInt32(AlarmCodes_field, it); - } - - printf("GripperStatus: PowerStatus: %d\n", PowerStatus); - printf("GripperStatus: ClampStatus: [%d %d %d %d]\n", ClampStatus[0], ClampStatus[1], - ClampStatus[2], ClampStatus[3]); - printf("GripperStatus: AlarmCodes: ["); - int first = 1; - - for (it = 0; it < NAlarmCodes; it++) - { - if (first) - first = 0; - else - printf(" "); - - printf("%d", AlarmCodes[it]); - } - printf("]\n"); - printf("GripperStatus: Configuration: %d\n", Configuration); - fflush(stdout); - - free(AlarmCodes); - cb_resp = 0; //success - } - else - { - ROS_ERROR(node, "Error allocating memory for AlarmCodes\n"); - cb_resp = 1; //fail - } - - return(cb_resp); -} - -static CallbackResponse gripperjoints_sub_callback(cRosMessage *message, void* context) -{ - cRosMessageField *Position_field = cRosMessageGetField(message, "Position"); - double Position[9]; - Position[0] = *cRosMessageFieldArrayAtFloat64(Position_field, 0); - Position[1] = *cRosMessageFieldArrayAtFloat64(Position_field, 1); - Position[2] = *cRosMessageFieldArrayAtFloat64(Position_field, 2); - Position[3] = *cRosMessageFieldArrayAtFloat64(Position_field, 3); - Position[4] = *cRosMessageFieldArrayAtFloat64(Position_field, 4); - Position[5] = *cRosMessageFieldArrayAtFloat64(Position_field, 5); - Position[6] = *cRosMessageFieldArrayAtFloat64(Position_field, 6); - Position[7] = *cRosMessageFieldArrayAtFloat64(Position_field, 7); - Position[8] = *cRosMessageFieldArrayAtFloat64(Position_field, 8); - - printf("GripperJoints: Position: [%f %f %f %f %f %f %f %f %f]\n", Position[0], Position[1], Position[2], - Position[3], Position[4], Position[5], Position[6], Position[7], Position[8]); - - fflush(stdout); - return 0; -} - -static CallbackResponse callback_pub_gripperstatus(cRosMessage *message, void* data_context) -{ - cRosMessageField *PowerStatus_field = cRosMessageGetField(message, "PowerStatus"); - cRosMessageField *ClampStatus_field = cRosMessageGetField(message, "ClampStatus"); - cRosMessageField *AlarmCodes_field = cRosMessageGetField(message, "AlarmCodes"); - cRosMessageField *Configuration_field = cRosMessageGetField(message, "Configuration"); - - PowerStatus_field->data.as_uint32 = 1; - - *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 0) = 0; - *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 1) = 1; - *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 2) = 2; - *cRosMessageFieldArrayAtUInt32(ClampStatus_field, 3) = 3; - - cRosMessageFieldArrayClear(AlarmCodes_field); - cRosMessageFieldArrayPushBackUInt32(AlarmCodes_field, 1); - cRosMessageFieldArrayPushBackUInt32(AlarmCodes_field, 8); - - Configuration_field->data.as_uint32 = 3; - - return 0; -} - -static CallbackResponse callback_pub_gripperjoints(cRosMessage *message, void* context) -{ - cRosMessageField *Position_field = cRosMessageGetField(message, "Position"); - - *cRosMessageFieldArrayAtFloat64(Position_field, 0) = 8; - *cRosMessageFieldArrayAtFloat64(Position_field, 1) = 7; - *cRosMessageFieldArrayAtFloat64(Position_field, 2) = 6; - *cRosMessageFieldArrayAtFloat64(Position_field, 3) = 5; - *cRosMessageFieldArrayAtFloat64(Position_field, 4) = 4; - *cRosMessageFieldArrayAtFloat64(Position_field, 5) = 3; - *cRosMessageFieldArrayAtFloat64(Position_field, 6) = 2; - *cRosMessageFieldArrayAtFloat64(Position_field, 7) = 1; - *cRosMessageFieldArrayAtFloat64(Position_field, 8) = 0; - - return 0; -} - -/* - * Service callbacks definition - */ - -static CallbackResponse callback_srv_close_gripper(cRosMessage *request, cRosMessage *response, void* context) -{ - srand((unsigned int)time(NULL)); - uint8_t response_val = rand() % 2; - - cRosMessageField *closed_field = cRosMessageGetField(response, "closed"); - closed_field->data.as_uint8 = response_val; - - printf("Service close_clamps. Arguments: none . Response %d \n", response_val); - - fflush(stdout); - return 0; -} - -static CallbackResponse callback_srv_open_gripper(cRosMessage *request, cRosMessage *response, void* context) -{ - srand((unsigned int)time(NULL)); - uint8_t response_val = rand() % 2; - - cRosMessageField *opened_field = cRosMessageGetField(response, "opened"); - opened_field->data.as_uint8 = response_val; - - printf("Service open_clamps. Arguments: none . Response %d \n", response_val); - fflush(stdout); - - return 0; -} - -static CallbackResponse callback_srv_rest(cRosMessage *request, cRosMessage *response, void* context) -{ - srand((unsigned int)time(NULL)); - uint8_t response_val = rand() % 2; - - cRosMessageField *parked_field = cRosMessageGetField(response, "parked"); - parked_field->data.as_uint8 = response_val; - - printf("Service park. Arguments: none . Response %d \n", response_val); - fflush(stdout); - - return 0; -} - -static CallbackResponse callback_srv_reconfigure(cRosMessage *request, cRosMessage *response, void* context) -{ - cRosMessageField *id_field = cRosMessageGetField(request, "id"); - int64_t reconfigure_id; - reconfigure_id = id_field->data.as_int64; - - srand((unsigned int)time(NULL)); - uint8_t response_val = rand() % 2; - - cRosMessageField *reconfigured_field = cRosMessageGetField(response, "reconfigured"); - reconfigured_field->data.as_uint8 = response_val; - - printf("Service reconfigure. Arguments: %lld. Response %u \n", (long long)reconfigure_id, response_val); - fflush(stdout); - - return 0; -} - -static CallbackResponse callback_srv_move_arm(cRosMessage *request, cRosMessage *response, void* context) -{ - cRosMessageField *arm_field = cRosMessageGetField(request, "arm"); - cRosMessageField *rad_field = cRosMessageGetField(request, "rad"); - cRosMessageField *velocity_field = cRosMessageGetField(request, "velocity"); - - int32_t arm = arm_field->data.as_int32; - int32_t rad = arm_field->data.as_int32; - int32_t velocity = arm_field->data.as_int32; - - srand((unsigned int)time(NULL)); - uint8_t response_val = rand() % 2; - - cRosMessageField *positioned_field = cRosMessageGetField(response, "positioned"); - positioned_field->data.as_uint8 = response_val; - - printf("Service reconfigure. Arguments: {arm: %d, rad: %d, velocity: %d} . Response %d \n", arm, rad, velocity, response_val); - fflush(stdout); - - return 0; -} - -static CallbackResponse callback_srv_transfer(cRosMessage *request, cRosMessage *response, void* context) -{ - srand((unsigned int)time(NULL)); - uint8_t response_val = rand() % 2; - - cRosMessageField *transfered_field = cRosMessageGetField(response, "transfered"); - transfered_field->data.as_uint8 = response_val; - - printf("Service transfer. Arguments: none . Response %d \n", response_val); - fflush(stdout); - - return 0; -} - -int main(int argc, char **argv) -{ - const char *def_pub_node_name = "/Gripper_pub"; - const char *def_sub_node_name = "/Gripper_sub"; - const char *node_name; - const char *default_host = "127.0.0.1", - *node_host = default_host, - *roscore_host = default_host; - unsigned short roscore_port = 11311; - int run_as_publisher = 0; - - node_name = NULL; - int i; - for(i = 1; i < argc; i++) - { - if( strcmp(argv[i],"-name") == 0) - node_name = argv[++i]; - else if (strcmp(argv[i], "-sub") == 0) - run_as_publisher = 0; - else if (strcmp(argv[i], "-pub") == 0) - run_as_publisher = 1; - else if (strcmp(argv[i], "-h") == 0) - { - printHelp(argv[0]); - return(EXIT_SUCCESS); - } - else if( strcmp(argv[i],"-host") == 0) - node_host = argv[++i]; - else if( strcmp(argv[i],"-chost") == 0) - roscore_host = argv[++i]; - else if( strcmp(argv[i],"-cport") == 0) - { - int i_port = atoi(argv[++i]); - if( i_port < 0 || i_port > USHRT_MAX ) - { - fprintf(stderr,"Invalid port value %d.\n",i_port); - return(EXIT_FAILURE); - } - roscore_port = (unsigned short)i_port; - } - else - { - fprintf(stderr,"Invalid option %s\n",argv[i]); - return(EXIT_FAILURE); - } - } - - if (node_name == NULL) // Node name has not been specified, so use the default one - { - if (run_as_publisher) - node_name = def_pub_node_name; - else - node_name = def_sub_node_name; - } - - printf("Running node \"%s\" with host : %s, roscore host : %s and roscore port : %d\n", - node_name, node_host, roscore_host, roscore_port ); - printf("To set a different node/host/port, take a look at the options: "); - printf("%s -h\n", argv[0]); - - char path[4096]; - getcwd(path, sizeof(path)); - strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); - - srand((unsigned int)time(NULL)); - - node = cRosNodeCreate(node_name, node_host, roscore_host, roscore_port, path); - - cRosErrCodePack err_cod; - ROS_INFO(node, "cROS Node created!\n"); - - if (run_as_publisher) - { - ROS_INFO(node, "Example running as publisher\n"); - - // Topics - err_cod = cRosApiRegisterPublisher(node, "/gripperstatus", "gripping_robot/GripperStatus", 1000, - callback_pub_gripperstatus, NULL, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - err_cod = cRosApiRegisterPublisher(node, "/gripperjoints", "gripping_robot/GripperJoints", 1000, - callback_pub_gripperjoints, NULL, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - // Services - err_cod = cRosApiRegisterServiceProvider(node, "/close_clamps", "gripping_robot/CloseGripper", - callback_srv_close_gripper, NULL, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - err_cod = cRosApiRegisterServiceProvider(node, "/open_clamps", "gripping_robot/OpenGripper", - callback_srv_open_gripper, NULL, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - err_cod = cRosApiRegisterServiceProvider(node, "/park_configuration", "gripping_robot/RestPosition", - callback_srv_rest, NULL, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - err_cod = cRosApiRegisterServiceProvider(node, "/reconfigure", "gripping_robot/Reconfigure", - callback_srv_reconfigure, NULL, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - err_cod = cRosApiRegisterServiceProvider(node, "/transfer", "gripping_robot/Transfer", - callback_srv_transfer, NULL, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - err_cod = cRosApiRegisterServiceProvider(node, "/move_arm", "gripping_robot/MoveArm", - callback_srv_move_arm, NULL, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - } - else - { - ROS_INFO(node, "Example running as subscriber\n"); - - err_cod = cRosApiRegisterSubscriber(node, "/gripperstatus", "gripping_robot/GripperStatus", - gripperstatus_sub_callback, NULL, NULL, 0, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - err_cod = cRosApiRegisterSubscriber(node, "/gripperjoints", "gripping_robot/GripperJoints", - gripperjoints_sub_callback, NULL, NULL, 0, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - } - - unsigned char exit_flag = 0; - - err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned error code(s)"); - - // All done: free memory and unregister from ROS master - err_cod=cRosNodeDestroy( node ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); - - return EXIT_SUCCESS; -} diff --git a/src/hal/components/cros/samples/listener.c b/src/hal/components/cros/samples/listener.c deleted file mode 100644 index 494ff676..00000000 --- a/src/hal/components/cros/samples/listener.c +++ /dev/null @@ -1,223 +0,0 @@ -/*! \file listener.c - * \brief The file is an example of cROS usage implementing a subscriber and service provider, which - * can be used together with talker to prove the operation of message transmissions and service - * calls between two nodes. - * - * It creates a subscriber to the topic /chatter. Each time a message of this topic is - * published a string is received and the callback function callback_sub() is executed. - * This node also implements a provider of the service /sum. Each time a the service is called - * two 64bit integers are received, the callback function callback_srv_add_two_ints() is executed, - * this function computes the sum of them and this result is sent back to service caller. - * To exit safely press Ctrl-C or 'kill' the process once. If this actions are repeated, the process - * will be finished immediately. - */ -#include -#include -#include - -#ifdef _WIN32 -# define WIN32_LEAN_AND_MEAN -# include -# include - -# define DIR_SEPARATOR_STR "\\" -#else -# include -# include -# include - -# define DIR_SEPARATOR_STR "/" -#endif - -#include "cros.h" - -#define ROS_MASTER_PORT 11311 -#define ROS_MASTER_ADDRESS "127.0.0.1" - -CrosNode *node; //! Pointer to object storing the ROS node. This object includes all the ROS node state variables -static unsigned char exit_flag = 0; //! ROS node loop exit flag. When set to 1 the cRosNodeStart() function exits - -// This callback will be invoked when the subscriber receives a message -static CallbackResponse callback_sub(cRosMessage *message, void* data_context) -{ - cRosMessageField *data_field = cRosMessageGetField(message, "data"); - if(data_field != NULL) - ROS_INFO(node, "I heard: [%s]\n", data_field->data.as_string); - - return 0; // 0=success -} - -// This callback will be invoked when the service provider receives a service call -static CallbackResponse callback_srv_add_two_ints(cRosMessage *request, cRosMessage *response, void* data_context) -{ - cRosMessageField *a_field = cRosMessageGetField(request, "a"); - cRosMessageField *b_field = cRosMessageGetField(request, "b"); - //cRosMessageFieldsPrint(request, 0); - - if(a_field != NULL && a_field != NULL) - { - int64_t a = a_field->data.as_int64; - int64_t b = b_field->data.as_int64; - - int64_t response_val = a+b; - - cRosMessageField *sum_field = cRosMessageGetField(response, "sum"); - if(sum_field != NULL) - { - sum_field->data.as_int64 = response_val; - ROS_INFO(node,"Service add 2 ints. Args: {a: %lld, b: %lld}. Resp: %lld\n", (long long)a, (long long)b, (long long)response_val); - } - } - - return 0; // 0=success -} - -// Ctrl-C-and-'kill' event/signal handler: (this code is no strictly necessary for a simple example and can be removed) -#ifdef _WIN32 -// This callback function will be called when the console process receives a CTRL_C_EVENT or -// CTRL_CLOSE_EVENT signal. -// Function set_signal_handler() should be called before calling cRosNodeStart() to set function -// exit_deamon_handler() as the handler of these signals. -// These functions are declared as 'static' to allow the declaration of other (independent) functions with -// the same name in this project. -static BOOL WINAPI exit_deamon_handler(DWORD sig) -{ - BOOL sig_handled; - - switch(sig) - { - case CTRL_C_EVENT: - case CTRL_CLOSE_EVENT: - SetConsoleCtrlHandler(exit_deamon_handler, FALSE); // Remove the handler - printf("Signal %u received: exiting safely.\n", sig); - exit_flag = 1; // Cause the exit of cRosNodeStart loop (safe exit) - sig_handled = TRUE; // Indicate that this signal is handled by this function - break; - default: - sig_handled = FALSE; // Indicate that this signal is not handled by this functions, so the next handler function of the list will be called - break; - } - return(sig_handled); -} - -// Sets the signal handler functions of CTRL_C_EVENT and CTRL_CLOSE_EVENT: exit_deamon_handler -static DWORD set_signal_handler(void) - { - DWORD ret; - - if(SetConsoleCtrlHandler(exit_deamon_handler, TRUE)) - ret=0; // Success setting the control handler - else - { - ret=GetLastError(); - printf("Error setting termination signal handler. Error code=%u\n",ret); - } - return(ret); - } -#else -struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); - -// This callback function will be called when the main process receives a SIGINT or -// SIGTERM signal. -// Function set_signal_handler() should be called to set this function as the handler of -// these signals -static void exit_deamon_handler(int sig) -{ - printf("Signal %i received: exiting safely.\n", sig); - sigaction(SIGINT, &old_int_signal_handler, NULL); - sigaction(SIGTERM, &old_term_signal_handler, NULL); - exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) -} - -// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler -static int set_signal_handler(void) - { - int ret; - struct sigaction act; - - memset (&act, '\0', sizeof(act)); - - act.sa_handler = exit_deamon_handler; - // If the signal handler is invoked while a system call or library function call is blocked, - // then the we want the call to be automatically restarted after the signal handler returns - // instead of making the call fail with the error EINTR. - act.sa_flags=SA_RESTART; - if(sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) - ret=0; - else - { - ret=errno; - printf("Error setting termination signal handler. errno=%d\n",ret); - } - return(ret); - } -#endif - -int main(int argc, char **argv) -{ - char path[4097]; // We need to tell our node where to find the .msg files that we'll be using - const char *node_name; - int subidx; // Index (identifier) of the created subscriber - cRosErrCodePack err_cod; - - if(argc>1) - node_name=argv[1]; - else - node_name="/listener"; // Default node name if no command-line parameters are specified - getcwd(path, sizeof(path)); - strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); - - printf("Using the following path for message definitions: %s\n", path); - // Create a new node and tell it to connect to roscore in the usual place - node = cRosNodeCreate(node_name, "127.0.0.1", ROS_MASTER_ADDRESS, ROS_MASTER_PORT, path); - if( node == NULL ) - { - printf("cRosNodeCreate() failed; is this program already being run?"); - return EXIT_FAILURE; - } - - err_cod = cRosWaitPortOpen(ROS_MASTER_ADDRESS, ROS_MASTER_PORT, 0); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "Port %s:%hu cannot be opened: ROS Master does not seems to be running", ROS_MASTER_ADDRESS, ROS_MASTER_PORT); - return EXIT_FAILURE; - } - - // Create a subscriber to topic /chatter of type "std_msgs/String" and supply a callback for received messages (callback_sub) - err_cod = cRosApiRegisterSubscriber(node, "/chatter", "std_msgs/String", callback_sub, NULL, NULL, 0, &subidx); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosApiRegisterSubscriber() failed; did you run this program one directory above 'rosdb'?"); - cRosNodeDestroy( node ); - return EXIT_FAILURE; - } - - // Create a service provider named /sum of type "roscpp_tutorials/TwoInts" and supply a callback for received calls - err_cod = cRosApiRegisterServiceProvider(node,"/sum","roscpp_tutorials/TwoInts", callback_srv_add_two_ints, NULL, NULL, NULL); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosApiRegisterServiceProvider() failed; did you run this program one directory above 'rosdb'?"); - cRosNodeDestroy( node ); - return EXIT_FAILURE; - } - - ROS_INFO(node, "Node %s created with XMLRPC port: %i, TCPROS port: %i and RPCROS port: %i\n", node->name, node->xmlrpc_port, node->tcpros_port, node->rpcros_port); - - // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed - set_signal_handler(); - - // Run the main loop until exit_flag is 1 - err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); - - // Free memory and unregister - err_cod=cRosNodeDestroy( node ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); - return EXIT_FAILURE; - } - - return EXIT_SUCCESS; -} diff --git a/src/hal/components/cros/samples/parameters-test.c b/src/hal/components/cros/samples/parameters-test.c deleted file mode 100644 index 8c745197..00000000 --- a/src/hal/components/cros/samples/parameters-test.c +++ /dev/null @@ -1,249 +0,0 @@ -/*! \file parameters-test.c - * \brief The file is an example of cROS usage implementing a parameter subscriber. This sample - * can be used together with rosparam or MATLAB to prove the operation of parameter sharing - * between two nodes. If MATLAB is used, it must not be used as ROS master node since it seems that - * it does not notify parameter updates. - * - * It creates a parameter subscriber to the key /testparam. Each time this parameter is updated by - * other node the master node informs about it and the callback function getNodeStatusCallback() is executed. - * This function checks if the parameters /testparam/x, /testparam/y and /testparam/z have been updated. - * When all of these parameters are updated by other node, the function changes their value to - * {x: 5, y: hello, z: [0,1,2,4]} and continues checking again when all the them change. - * - With ROS core it can be tested by running: - * $ rosparam set /testparam "{x: 3, y: ciao, z: [1,2,3]}" - * - With MATLAB R2015a (or later) it can be tested by running: - * >> rosinit - * >> rosparam('set', '/testparam/x', int32(3)) - * >> rosparam('set', '/testparam/y', 'ciao') - * >> rosparam('set', '/testparam/z', {int32(1),int32(2),int32(3)}) - * >> rosparam('get', '/testparam') - * To exit safely press Ctrl-C or 'kill' the process once. If this actions are repeated, the process - * will be finished immediately. - */ - -#include -#include -#include - -#ifdef _WIN32 -# define WIN32_LEAN_AND_MEAN -# include -# include - -# define DIR_SEPARATOR_STR "\\" -#else -# include -# include -# include - -# define DIR_SEPARATOR_STR "/" -#endif - -#include "cros_log.h" -#include "cros_node.h" -#include "cros_api.h" -#include "cros_clock.h" - -CrosNode *node; - -int paramx_updated = 0; -int paramy_updated = 0; -int paramz_updated = 0; -static unsigned char exit_flag = 0; //! ROS-node loop exit flag. When it is set to 1 the cRosNodeStart() function exits - -static void getParamNamesCallback(int callid, GetParamNamesResult *result, void *context) -{ - // OK -} - -static void setParamCallback(int callid, SetParamResult *result, void *context) -{ - // OK -} - -static void getNodeStatusCallback(CrosNodeStatusUsr *status, void* context) -{ -/* - XmlrpcParam *param0; - XmlrpcParam *param1; - XmlrpcParam *param2; - - if (status->parameter_value->array_n_elem == 3) - { - param0 = &status->parameter_value->data.as_array[0]; - param1 = &status->parameter_value->data.as_array[1]; - param2 = &status->parameter_value->data.as_array[2]; - } -*/ - - if (status->state == CROS_STATUS_PARAM_UPDATE) - { - ROS_INFO(node, "Subscribed parameter updated: %s\n", status->parameter_key); - - if (strcmp(status->parameter_key, "/testparam/x/") == 0) - paramx_updated = 1; - - if (strcmp(status->parameter_key, "/testparam/y/") == 0) - paramy_updated = 1; - - if (strcmp(status->parameter_key, "/testparam/z/") == 0) - paramz_updated = 1; - - if (paramx_updated && paramy_updated && paramz_updated) - { - cRosErrCodePack err_cod; - - XmlrpcParam param; - xmlrpcParamInit(¶m); - xmlrpcParamSetStruct(¶m); - - xmlrpcParamStructPushBackInt(¶m, "x", 5); - xmlrpcParamStructPushBackString(¶m, "y", "hello"); - XmlrpcParam *array = xmlrpcParamStructPushBackArray(¶m, "z"); - xmlrpcParamArrayPushBackInt(array, 0); - xmlrpcParamArrayPushBackInt(array, 1); - xmlrpcParamArrayPushBackInt(array, 2); - xmlrpcParamArrayPushBackInt(array, 4); - - ROS_INFO(node, "x, y, and z parameter values changed: Modifying their value\n"); - err_cod = cRosApiSetParam(node, "/testparam", ¶m, setParamCallback, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosApiSetParam() failed"); - - paramx_updated = 0; - paramy_updated = 0; - paramz_updated = 0; - xmlrpcParamRelease(¶m); - } - } -} - -#ifdef _WIN32 -// This callback function will be called when the console process receives a CTRL_C_EVENT or -// CTRL_CLOSE_EVENT signal. -// Function set_signal_handler() should be called to set function exit_deamon_handler() as the handler of -// these signals -static BOOL WINAPI exit_deamon_handler(DWORD sig) -{ - BOOL sig_handled; - - switch (sig) - { - case CTRL_C_EVENT: - case CTRL_CLOSE_EVENT: - SetConsoleCtrlHandler(exit_deamon_handler, FALSE); // Remove the handler - printf("Signal %u received: exiting safely.\n", sig); - exit_flag = 1; // Cause the exit of cRosNodeStart loop (safe exit) - sig_handled = TRUE; // Indicate that this signal is handled by this function - break; - default: - sig_handled = FALSE; // Indicate that this signal is not handled by this functions, so the next handler function of the list will be called - break; - } - return(sig_handled); -} - -// Sets the signal handler functions of CTRL_C_EVENT and CTRL_CLOSE_EVENT: exit_deamon_handler -static DWORD set_signal_handler(void) -{ - DWORD ret; - - if (SetConsoleCtrlHandler(exit_deamon_handler, TRUE)) - ret = 0; // Success setting the control handler - else - { - ret = GetLastError(); - printf("Error setting termination signal handler. Error code=%u\n", ret); - } - return(ret); -} -#else -struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); - -// This callback function will be called when the main process receives a SIGINT or -// SIGTERM signal. -// Function set_signal_handler() should be called to set this function as the handler of -// these signals -static void exit_deamon_handler(int sig) -{ - printf("Signal %i received: exiting safely.\n", sig); - sigaction(SIGINT, &old_int_signal_handler, NULL); - sigaction(SIGTERM, &old_term_signal_handler, NULL); - exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) -} - -// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler -static int set_signal_handler(void) -{ - int ret; - struct sigaction act; - - memset(&act, '\0', sizeof(act)); - - act.sa_handler = exit_deamon_handler; - // If the signal handler is invoked while a system call or library function call is blocked, - // then the we want the call to be automatically restarted after the signal handler returns - // instead of making the call fail with the error EINTR. - act.sa_flags = SA_RESTART; - if (sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) - ret = 0; - else - { - ret = errno; - printf("Error setting termination signal handler. errno=%d\n", ret); - } - return(ret); -} -#endif - -int main(int argc, char **argv) -{ - const char *default_node_name = "/Gripper", - *node_name = default_node_name; - const char *default_host = "127.0.0.1", - *node_host = default_host, - *roscore_host = default_host; - unsigned short roscore_port = 11311; - char path[4097]; - cRosErrCodePack err_cod; - - getcwd(path, sizeof(path)); - strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); - node = cRosNodeCreate(node_name, node_host, roscore_host, roscore_port, path); - - err_cod = cRosApiSubscribeParam(node, "/testparam", getNodeStatusCallback, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosApiSubscribeParam() failed"); - cRosNodeDestroy( node ); - return EXIT_FAILURE; - } - - printf("Node XMLRPC port: %i\n", node->xmlrpc_port); - - err_cod = cRosApiGetParamNames(node, getParamNamesCallback, NULL, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosApiGetParamNames() failed"); - cRosNodeDestroy( node ); - return EXIT_FAILURE; - } - - // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed - set_signal_handler(); - - // Run the main loop until exit_flag is 1 - err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); - - // Free memory and unregister - err_cod=cRosNodeDestroy( node ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unsubscribing in ROS master"); - return EXIT_FAILURE; - } - - return EXIT_SUCCESS; -} diff --git a/src/hal/components/cros/samples/performance-test.cpp b/src/hal/components/cros/samples/performance-test.cpp deleted file mode 100644 index 778ad725..00000000 --- a/src/hal/components/cros/samples/performance-test.cpp +++ /dev/null @@ -1,583 +0,0 @@ -/*! \file performance-tesp.cpp - * \brief This file implements a cROS program for measuring the performance of the cROS library. - * - * To exit safely press Ctrl-C or 'kill' the process once. If this actions are repeated, the process - * will be finished immediately. - */ -#include -#include -#include -#include - -#ifdef _WIN32 -# define WIN32_LEAN_AND_MEAN -# include -# include - -# define DIR_SEPARATOR_STR "\\" -#else -# include -# include -# include - -# define DIR_SEPARATOR_STR "/" -#endif - -#include "cros.h" - -#define ROS_MASTER_PORT 11311 -#define ROS_MASTER_ADDRESS "127.0.0.1" - -CrosNode *node; //! Pointer to object storing the ROS node. This object includes all the ROS-node state variables -static unsigned char exit_flag = 0; //! ROS-node loop exit flag. When set to 1 the cRosNodeStart() function exits - -double **Time_stamps; -size_t N_measure_exper = 0; -size_t N_measure_rep = 0; -size_t Msg_sizes[]={1, 1, 1024/10, 1024*10, 1024*1024, 1024*1024*100}; -size_t Measure_reps[]={1000000, 1000000, 100000, 100000, 1000, 100}; - -#define TOTAL_MEASURE_EXPERS (sizeof(Measure_reps)/sizeof(size_t)) -#define MAX_MEASURE_REPS 1000000 -#define MAX_MSG_SIZE (1024*1024*100) - -double **allocate_heter_2d_array(size_t n_sub_arrays, const size_t *sub_array_sizes) -{ - double **array_2d; - size_t n_total_elems, cur_sub_array_ind; - - n_total_elems=0; - for(cur_sub_array_ind=0;cur_sub_array_inddata.as_string)); - //ROS_INFO(node, "I heard: [%s]\n", data_field->data.as_string); - } - - N_measure_rep++; - - if(N_measure_rep >= Measure_reps[N_measure_exper]) - { - N_measure_rep=0; - N_measure_exper++; - //printf("*** %lu **\n", N_measure_exper); - } - - if(N_measure_exper>=TOTAL_MEASURE_EXPERS) - exit_flag = 1; - - return 0; // 0=success -} - -static CallbackResponse callback_pub(cRosMessage *message, void *data_context) -{ - static int pub_count = 0; - char buf[1024]; - // We need to index into the message structure and then assign to fields - cRosMessageField *data_field; - - data_field = cRosMessageGetField(message, "data"); - if(data_field) - { - snprintf(buf, sizeof(buf), "periodic hello world %d", pub_count); - if(cRosMessageSetFieldValueString(data_field, buf) == 0) - { - ROS_INFO(node, "%s\n", buf); - } - pub_count+=10; - } - - return 0; // 0=success -} - -// This callback will be invoked when the service provider receives a service call -static CallbackResponse callback_service_provider(cRosMessage *request, cRosMessage *response, void* data_context) -{ - Time_stamps[N_measure_exper][N_measure_rep++] = cRosClockTimeStampToUSec(cRosClockGetTimeStamp()); - if(N_measure_rep >= Measure_reps[N_measure_exper]) - { - N_measure_rep=0; - N_measure_exper++; - } - - cRosMessageField *name_field = cRosMessageGetField(request, "name"); - if(name_field != NULL) - { - cRosMessageField *ok_field = cRosMessageGetField(response, "ok"); - if(ok_field != NULL) - { - ok_field->data.as_uint8 = 1; - //ROS_INFO(node,"Service Resp: %hu\n", (unsigned short)ok_field->data.as_uint8); - } - } - - if(N_measure_exper>=TOTAL_MEASURE_EXPERS) - exit_flag = 1; - - return 0; // 0=success -} - -static CallbackResponse callback_service_caller(cRosMessage *request, cRosMessage *response, int call_resp_flag, void* context) -{ - static int call_count = 0; - - if(!call_resp_flag) // Check if this callback function has been called to provide the service call arguments or to collect the response - { - cRosMessageField *a_field = cRosMessageGetField(request, "a"); - cRosMessageField *b_field = cRosMessageGetField(request, "b"); - if(a_field != NULL && b_field != NULL) - { - a_field->data.as_int64=10; - b_field->data.as_int64=call_count; - ROS_INFO(node, "Service add 2 ints call arguments: {a: %lld, b: %lld}\b\n", (long long)a_field->data.as_int64, (long long)b_field->data.as_int64); - } - } - else // Service call response available - { - cRosMessageField *sum_field = cRosMessageGetField(response, "sum"); - if(sum_field != NULL) - ROS_INFO(node, "Service add 2 ints response: %lld (call_count: %i)\n", (long long)sum_field->data.as_int64, call_count++); - } - - if(call_count > 10) exit_flag=1; - return 0; // 0=success -} - - -// Ctrl-C-and-'kill' event/signal handler: (this code is no strictly necessary for a simple example and can be removed) -#ifdef _WIN32 -// This callback function will be called when the console process receives a CTRL_C_EVENT or -// CTRL_CLOSE_EVENT signal. -// Function set_signal_handler() should be called before calling cRosNodeStart() to set function -// exit_deamon_handler() as the handler of these signals. -// These functions are declared as 'static' to allow the declaration of other (independent) functions with -// the same name in this project. -static BOOL WINAPI exit_deamon_handler(DWORD sig) -{ - BOOL sig_handled; - - switch(sig) - { - case CTRL_C_EVENT: - case CTRL_CLOSE_EVENT: - SetConsoleCtrlHandler(exit_deamon_handler, FALSE); // Remove the handler - printf("Signal %u received: exiting safely.\n", sig); - exit_flag = 1; // Cause the exit of cRosNodeStart loop (safe exit) - sig_handled = TRUE; // Indicate that this signal is handled by this function - break; - default: - sig_handled = FALSE; // Indicate that this signal is not handled by this functions, so the next handler function of the list will be called - break; - } - return(sig_handled); -} - -// Sets the signal handler functions of CTRL_C_EVENT and CTRL_CLOSE_EVENT: exit_deamon_handler -static DWORD set_signal_handler(void) - { - DWORD ret; - - if(SetConsoleCtrlHandler(exit_deamon_handler, TRUE)) - ret=0; // Success setting the control handler - else - { - ret=GetLastError(); - printf("Error setting termination signal handler. Error code=%u\n",ret); - } - return(ret); - } -#else -struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); - -// This callback function will be called when the main process receives a SIGINT or -// SIGTERM signal. -// Function set_signal_handler() should be called to set this function as the handler of -// these signals -static void exit_deamon_handler(int sig) -{ - printf("Signal %i received: exiting safely.\n", sig); - sigaction(SIGINT, &old_int_signal_handler, NULL); - sigaction(SIGTERM, &old_term_signal_handler, NULL); - exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) -} - -// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler -static int set_signal_handler(void) - { - int ret; - struct sigaction act; - - memset (&act, '\0', sizeof(act)); - - act.sa_handler = exit_deamon_handler; - // If the signal handler is invoked while a system call or library function call is blocked, - // then the we want the call to be automatically restarted after the signal handler returns - // instead of making the call fail with the error EINTR. - act.sa_flags=SA_RESTART; - if(sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) - ret=0; - else - { - ret=errno; - printf("Error setting termination signal handler. errno=%d\n",ret); - } - return(ret); - } -#endif - -cRosErrCodePack run_topic_subscriber(void) -{ - cRosErrCodePack err_cod; - int subidx; // Index (identifier) of the subscriber to be created - - // Create a subscriber to topic /chatter of type "std_msgs/String" and supply a callback for received messages (callback_sub) - err_cod = cRosApiRegisterSubscriber(node, "/chatter", "std_msgs/String", callback_sub, NULL, NULL, 0, &subidx); - if (err_cod == CROS_SUCCESS_ERR_PACK) - { - // Run the main loop until exit_flag is 1 - err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); - } - else - { - cRosPrintErrCodePack(err_cod, "cRosApiRegisterSubscriber() failed."); - } - return err_cod; -} - -cRosErrCodePack run_service_provider(void) -{ - cRosErrCodePack err_cod; - - // Create a service provider named /bulk of type "controller_manager_msgs/LoadController.srv" and supply a callback for received calls - err_cod = cRosApiRegisterServiceProvider(node,"/bulk","controller_manager_msgs/LoadController", callback_service_provider, NULL, NULL, NULL); - if(err_cod == CROS_SUCCESS_ERR_PACK) - { - // Run the main loop until exit_flag is 1 - err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); - } - else - { - cRosPrintErrCodePack(err_cod, "cRosApiRegisterServiceProvider() failed; did you run this program one directory above 'rosdb'?"); - } - - return err_cod; -} - -cRosErrCodePack run_topic_publisher(void) -{ - cRosErrCodePack err_cod; - cRosMessage *msg; - cRosMessageField *data_field; - int pubidx; // Index (identifier) of the publisher to be created - - // Create a publisher to topic /chatter of type "std_msgs/String" - err_cod = cRosApiRegisterPublisher(node, "/chatter", "std_msgs/String", -1, NULL, NULL, NULL, &pubidx); // callback_pub - if (err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosApiRegisterPublisher() failed; did you run this program one directory above 'rosdb'?"); - cRosNodeDestroy(node); - return err_cod; - } - - msg = cRosApiCreatePublisherMessage(node, pubidx); - // Popullate msg - data_field = cRosMessageGetField(msg, "data"); - if (data_field != NULL) - { - static char buf[MAX_MSG_SIZE+1]; - int pub_count; - - memset(buf,' ',MAX_MSG_SIZE+1); - for (pub_count = 0; pub_count < TOTAL_MEASURE_EXPERS; pub_count++) - buf[Msg_sizes[pub_count]]='\0'; // Previously set end of string for all message sizes - - - - data_field->data.as_string = buf; - - printf("Publishing strings...\n"); - //cRosMessageSetFieldValueString(data_field, buf); - err_cod = cRosNodeStart( node, 200, &exit_flag ); - for (pub_count = 0; pub_count < TOTAL_MEASURE_EXPERS && err_cod == CROS_SUCCESS_ERR_PACK && exit_flag == 0; pub_count++) - { - int rep_count; - printf("Publishing %lu bytes %lu times\n", strlen(buf),Measure_reps[pub_count]); - //cRosMessageSetFieldValueString(data_field, buf); - for(rep_count=0;rep_countdata.as_string = NULL; // Prevent cRosMessageFree from trying to free the char array - - } - else - printf("Error accessing message fields\n"); - - cRosMessageFree(msg); - - if (err_cod == CROS_SUCCESS_ERR_PACK) - { - err_cod = cRosNodeStart( node, 20000, &exit_flag ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); - } - - return err_cod; -} - -cRosErrCodePack run_service_caller(void) -{ - cRosErrCodePack err_cod; - cRosMessage *msg_req, msg_res; - cRosMessageField *name_field; - int calleridx; // Index (identifier) of the service caller to be created - - - // Create a service caller named /bulk of type "controller_manager_msgs/LoadController.srv" and request that the associated callback - err_cod = cRosApiRegisterServiceCaller(node,"/bulk","controller_manager_msgs/LoadController", -1, NULL, NULL, NULL, 1, 1, &calleridx); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosApiRegisterServiceCaller() failed; did you run this program one directory above 'rosdb'?"); - cRosNodeDestroy( node ); - return err_cod; - } - - - msg_req = cRosApiCreateServiceCallerRequest(node, calleridx); - cRosMessageInit(&msg_res); - - name_field = cRosMessageGetField(msg_req, "name"); - if (name_field != NULL) - { - static char buf[MAX_MSG_SIZE+1]={0}; - int call_count = 0; - - printf("Calling service...\n"); - - err_cod = cRosNodeStart( node, 200, &exit_flag ); - for (call_count = 0; call_count < TOTAL_MEASURE_EXPERS && err_cod == CROS_SUCCESS_ERR_PACK && exit_flag == 0; call_count++) - { - int rep_count; - //snprintf(buf, sizeof(buf), "hello world %d", call_count); - memset(buf,' ',Msg_sizes[call_count]); - - cRosMessageSetFieldValueString(name_field, buf); - - for(rep_count=0;rep_countdata.as_uint8); - //err_cod = cRosNodeStart( node, 1000, &exit_flag ); - } - else - cRosPrintErrCodePack(err_cod, "cRosNodeServiceCall() failed: service call not made"); - } - } - - } - else - printf("Error accessing message fields\n"); - - - cRosMessageFree(msg_req); - cRosMessageRelease(&msg_res); - - printf("End of service call.\n"); - - // Run the main loop until exit_flag is 1 - if (err_cod == CROS_SUCCESS_ERR_PACK) - { - err_cod = cRosNodeStart( node, 200, &exit_flag ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); - } - - return err_cod; -} - - -int main(int argc, char **argv) -{ - char path[4097]; // We need to tell our node where to find the .msg files that it will be using - const char *node_name; - - cRosErrCodePack err_cod; - int op_mode; - - if(getcwd(path, sizeof(path)) == NULL) - { - perror("Could not get current directory"); - return EXIT_FAILURE; - } - - strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); - - printf("PATH ROSDB: %s\n", path); - - printf("Press s for subscriber, p for publisher, r for service server or c for service client: "); - op_mode = getchar(); - if (op_mode == 's') - node_name = "/node_sub"; - else if (op_mode == 'r') - node_name = "/node_server"; - else if (op_mode == 'p') - node_name = "/node_pub"; - else if (op_mode == 'c') - node_name = "/node_caller"; - else - { - printf("Invalid option\n"); - return EXIT_FAILURE; - } - - Time_stamps = allocate_heter_2d_array(TOTAL_MEASURE_EXPERS, Measure_reps); - if(Time_stamps == NULL) - { - printf("Cannot allocate memory for the time stamps\n"); - return EXIT_FAILURE; - } - - // Create a new node and tell it to connect to roscore in the usual place - node = cRosNodeCreate(node_name, "127.0.0.1", ROS_MASTER_ADDRESS, ROS_MASTER_PORT, path); - if( node == NULL ) - { - printf("cRosNodeCreate() failed; is this program already being run?"); - free_heter_2d_array(Time_stamps); - return EXIT_FAILURE; - } - - err_cod = cRosWaitPortOpen(ROS_MASTER_ADDRESS, ROS_MASTER_PORT, 0); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "Port %s:%hu cannot be opened: ROS Master does not seems to be running", ROS_MASTER_ADDRESS, ROS_MASTER_PORT); - free_heter_2d_array(Time_stamps); - return EXIT_FAILURE; - } - - printf("Node RPCROS port: %i\n", node->rpcros_port); - - // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed - set_signal_handler(); - - if (op_mode == 's') - { - err_cod = run_topic_subscriber(); - } - else if (op_mode == 'r') - { - err_cod = run_service_provider(); - } - else if (op_mode == 'p') - { - err_cod = run_topic_publisher(); - } - else if (op_mode == 'c') - { - err_cod = run_service_caller(); - } - - - printf("Unregistering in ROS master\n"); - // Free memory and unregister - err_cod=cRosNodeDestroy( node ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); - free_heter_2d_array(Time_stamps); - return EXIT_FAILURE; - } - - printf("Node end. Current N_measure_exper: %lu N_measure_rep: %lu.\n", N_measure_exper, N_measure_rep); - - if(op_mode == 's' || op_mode == 'r') - { - store_heter_2d_array("times.txt", Time_stamps, TOTAL_MEASURE_EXPERS, Measure_reps); - } - - free_heter_2d_array(Time_stamps); - return EXIT_SUCCESS; -} diff --git a/src/hal/components/cros/samples/plot_performance.m b/src/hal/components/cros/samples/plot_performance.m deleted file mode 100644 index 5219d1e6..00000000 --- a/src/hal/components/cros/samples/plot_performance.m +++ /dev/null @@ -1,28 +0,0 @@ -times={}; -fd = fopen('../build/bin/times.txt','r'); -n_measure_expers=0; -while(~feof(fd)) - n_measure_expers=n_measure_expers+1; - tline = fgetl(fd); - times{n_measure_expers} = sscanf(tline,'%f')'; -end -fclose(fd) -clear(tline) - -diff_times = cellfun(@diff, times, 'UniformOutput', false); - -x_axis = 1:n_measure_expers; -mean_times = cellfun(@mean, diff_times); -stddev_times = cellfun(@std, diff_times); -%bar(x_axis, mean_times) -%hold on, -h = errorbar(x_axis, mean_times, stddev_times); -set(gca,'YScale','log') -%ylabel('\mus/msg') -c_axis=axis; -axis([0.9 n_measure_expers+0.1 c_axis(3) c_axis(4)]) - -figure -for n_hist=1:n_measure_expers - subplot(n_measure_expers,1,n_hist), hist(diff_times{n_hist}, length(diff_times{n_hist})/5) -end \ No newline at end of file diff --git a/src/hal/components/cros/samples/ros-i-trajectory-test.c b/src/hal/components/cros/samples/ros-i-trajectory-test.c deleted file mode 100644 index a34c6602..00000000 --- a/src/hal/components/cros/samples/ros-i-trajectory-test.c +++ /dev/null @@ -1,219 +0,0 @@ -// -// Created by nico on 26/05/16. -// - -#include -#include -#include -#include - -#ifdef _WIN32 -# define WIN32_LEAN_AND_MEAN -# include -# include - -# define DIR_SEPARATOR_STR "\\" -#else -# include -# include -# include - -# define DIR_SEPARATOR_STR "/" -#endif - -#include "cros.h" - -CrosNode *node; -static unsigned char exit_flag = 0; //! ROS-node loop exit flag. When it is set to 1 the cRosNodeStart() function exits - -static void printHelp( char *cmd_name ) -{ - printf("Usage: %s [OPTION] ... \n", cmd_name); - printf("Options:\n"); - printf("\t-name Set the node name (default: /test_node)\n"); - printf("\t-host Set the node host (default: 127.0.0.1)\n"); - printf("\t-chost Set the roscore host (default: 127.0.0.1)\n"); - printf("\t-cport Set the roscore port (default: 11311)\n"); - printf("\t-h Print this help\n"); -} - -static CallbackResponse jointstates_sub_callback(cRosMessage *message, void* data_context) -{ - int joints_size, joint_ind; - - cRosMessageField *field_name = cRosMessageGetField(message, "name"); - cRosMessageField *field_position = cRosMessageGetField(message, "position"); - cRosMessageField *field_velocity = cRosMessageGetField(message, "velocity"); - cRosMessageField *field_effort = cRosMessageGetField(message, "effort"); - - joints_size = field_name->array_size; - - printf("Joint names:"); - for(joint_ind = 0; joint_ind < joints_size; joint_ind++) - { - char* joint_name; - joint_name = cRosMessageFieldArrayAtStringGet(field_name, joint_ind); - printf(" %s", joint_name); - } - printf("\n"); - - return 0; -} - -#ifdef _WIN32 -// This callback function will be called when the console process receives a CTRL_C_EVENT or -// CTRL_CLOSE_EVENT signal. -// Function set_signal_handler() should be called to set function exit_deamon_handler() as the handler of -// these signals -static BOOL WINAPI exit_deamon_handler(DWORD sig) -{ - BOOL sig_handled; - - switch (sig) - { - case CTRL_C_EVENT: - case CTRL_CLOSE_EVENT: - SetConsoleCtrlHandler(exit_deamon_handler, FALSE); // Remove the handler - printf("Signal %u received: exiting safely.\n", sig); - exit_flag = 1; // Cause the exit of cRosNodeStart loop (safe exit) - sig_handled = TRUE; // Indicate that this signal is handled by this function - break; - default: - sig_handled = FALSE; // Indicate that this signal is not handled by this functions, so the next handler function of the list will be called - break; - } - return(sig_handled); -} - -// Sets the signal handler functions of CTRL_C_EVENT and CTRL_CLOSE_EVENT: exit_deamon_handler -static DWORD set_signal_handler(void) -{ - DWORD ret; - - if (SetConsoleCtrlHandler(exit_deamon_handler, TRUE)) - ret = 0; // Success setting the control handler - else - { - ret = GetLastError(); - printf("Error setting termination signal handler. Error code=%u\n", ret); - } - return(ret); -} -#else -struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); - -// This callback function will be called when the main process receives a SIGINT or -// SIGTERM signal. -// Function set_signal_handler() should be called to set this function as the handler of -// these signals -static void exit_deamon_handler(int sig) -{ - printf("Signal %i received: exiting safely.\n", sig); - sigaction(SIGINT, &old_int_signal_handler, NULL); - sigaction(SIGTERM, &old_term_signal_handler, NULL); - exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) -} - -// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler -static int set_signal_handler(void) -{ - int ret; - struct sigaction act; - - memset(&act, '\0', sizeof(act)); - - act.sa_handler = exit_deamon_handler; - // If the signal handler is invoked while a system call or library function call is blocked, - // then the we want the call to be automatically restarted after the signal handler returns - // instead of making the call fail with the error EINTR. - act.sa_flags = SA_RESTART; - if (sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) - ret = 0; - else - { - ret = errno; - printf("Error setting termination signal handler. errno=%d\n", ret); - } - return(ret); -} -#endif - -int main(int argc, char **argv) -{ - const char *default_node_name = "/robot_listener", - *node_name = default_node_name; - const char *default_host = "127.0.0.1", - *node_host = default_host, - *roscore_host = default_host; - unsigned short roscore_port = 11311; - - if(argc == 2) - { - printHelp( argv[0] ); - return EXIT_SUCCESS; - } - - // srand (time(NULL)); - - int i = 1; - for( ; i < argc - 1; i+=2) - { - if( strcmp(argv[i],"-name") == 0) - node_name = argv[i+1]; - else if( strcmp(argv[i],"-host") == 0) - node_host = argv[i+1]; - else if( strcmp(argv[i],"-chost") == 0) - roscore_host = argv[i+1]; - else if( strcmp(argv[i],"-cport") == 0) - { - int i_port = atoi(argv[i+1]); - if( i_port < 0 || i_port > USHRT_MAX ) - { - fprintf(stderr,"Invalid port %d\n",i_port); - exit(EXIT_FAILURE); - } - roscore_port = (unsigned short)i_port; - } - else - { - fprintf(stderr,"Invalid option %s\n",argv[i]); - exit(EXIT_FAILURE); - } - } - - printf("Running node \"%s\" with host : %s, roscore host : %s and roscore port : %d\n", - node_name, node_host, roscore_host, roscore_port ); - printf("To set a different node/host/port, take a look at the options: "); - printf("%s -h\n", argv[0]); - - char path[4096]; - getcwd(path, sizeof(path)); - strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); - node = cRosNodeCreate(node_name, node_host, roscore_host, roscore_port, path); - - cRosErrCodePack err_cod; - ROS_INFO(node, "cROS Node created!\n"); - - err_cod = cRosApiRegisterSubscriber(node, "/arm/joint_states", "sensor_msgs/JointState", - jointstates_sub_callback, NULL, NULL, 0, NULL); - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - if (err_cod != CROS_SUCCESS_ERR_PACK) - return EXIT_FAILURE; - - // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed - set_signal_handler(); - - // Run the main loop until exit_flag is 1 - err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); - - // All done: free memory and unregister from ROS master - err_cod=cRosNodeDestroy( node ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); - - return EXIT_SUCCESS; -} diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/CloseGripper.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/CloseGripper.srv deleted file mode 100644 index 6045b07b..00000000 --- a/src/hal/components/cros/samples/rosdb/gripping_robot/CloseGripper.srv +++ /dev/null @@ -1,3 +0,0 @@ ---- -bool closed - diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/GripperJoints.msg b/src/hal/components/cros/samples/rosdb/gripping_robot/GripperJoints.msg deleted file mode 100644 index 1bc7c0b7..00000000 --- a/src/hal/components/cros/samples/rosdb/gripping_robot/GripperJoints.msg +++ /dev/null @@ -1 +0,0 @@ -float64[9] Position diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/GripperStatus.msg b/src/hal/components/cros/samples/rosdb/gripping_robot/GripperStatus.msg deleted file mode 100644 index 95c32b04..00000000 --- a/src/hal/components/cros/samples/rosdb/gripping_robot/GripperStatus.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint32 PowerStatus -uint32[4] ClampStatus -uint32[] AlarmCodes -uint32 Configuration diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/MoveArm.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/MoveArm.srv deleted file mode 100644 index 5e9e3f61..00000000 --- a/src/hal/components/cros/samples/rosdb/gripping_robot/MoveArm.srv +++ /dev/null @@ -1,5 +0,0 @@ -int32 arm -int32 rad -int32 velocity ---- -bool positioned diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/OpenGripper.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/OpenGripper.srv deleted file mode 100644 index 08078611..00000000 --- a/src/hal/components/cros/samples/rosdb/gripping_robot/OpenGripper.srv +++ /dev/null @@ -1,3 +0,0 @@ ---- -bool opened - diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/Reconfigure.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/Reconfigure.srv deleted file mode 100644 index 897ceaf8..00000000 --- a/src/hal/components/cros/samples/rosdb/gripping_robot/Reconfigure.srv +++ /dev/null @@ -1,4 +0,0 @@ -int64 id ---- -bool reconfigured - diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/RestPosition.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/RestPosition.srv deleted file mode 100644 index 988d5c0e..00000000 --- a/src/hal/components/cros/samples/rosdb/gripping_robot/RestPosition.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -bool parked diff --git a/src/hal/components/cros/samples/rosdb/gripping_robot/Transfer.srv b/src/hal/components/cros/samples/rosdb/gripping_robot/Transfer.srv deleted file mode 100644 index 979b249f..00000000 --- a/src/hal/components/cros/samples/rosdb/gripping_robot/Transfer.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -bool transfered diff --git a/src/hal/components/cros/samples/rosdb/roscpp/GetLoggers.srv b/src/hal/components/cros/samples/rosdb/roscpp/GetLoggers.srv deleted file mode 100644 index 855e6bc0..00000000 --- a/src/hal/components/cros/samples/rosdb/roscpp/GetLoggers.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -Logger[] loggers \ No newline at end of file diff --git a/src/hal/components/cros/samples/rosdb/roscpp/Logger.msg b/src/hal/components/cros/samples/rosdb/roscpp/Logger.msg deleted file mode 100644 index bf45c288..00000000 --- a/src/hal/components/cros/samples/rosdb/roscpp/Logger.msg +++ /dev/null @@ -1,2 +0,0 @@ -string name -string level \ No newline at end of file diff --git a/src/hal/components/cros/samples/rosdb/roscpp/SetLoggerLevel.srv b/src/hal/components/cros/samples/rosdb/roscpp/SetLoggerLevel.srv deleted file mode 100644 index 29841db3..00000000 --- a/src/hal/components/cros/samples/rosdb/roscpp/SetLoggerLevel.srv +++ /dev/null @@ -1,3 +0,0 @@ -string logger -string level ---- \ No newline at end of file diff --git a/src/hal/components/cros/samples/rosdb/roscpp_tutorials/TwoInts.srv b/src/hal/components/cros/samples/rosdb/roscpp_tutorials/TwoInts.srv deleted file mode 100755 index 3a68808e..00000000 --- a/src/hal/components/cros/samples/rosdb/roscpp_tutorials/TwoInts.srv +++ /dev/null @@ -1,4 +0,0 @@ -int64 a -int64 b ---- -int64 sum diff --git a/src/hal/components/cros/samples/rosdb/rosgraph_msgs/Log.msg b/src/hal/components/cros/samples/rosdb/rosgraph_msgs/Log.msg deleted file mode 100644 index 9a9597c2..00000000 --- a/src/hal/components/cros/samples/rosdb/rosgraph_msgs/Log.msg +++ /dev/null @@ -1,19 +0,0 @@ -## -## Severity level constants -## -byte DEBUG=1 #debug level -byte INFO=2 #general level -byte WARN=4 #warning level -byte ERROR=8 #error level -byte FATAL=16 #fatal/critical level -## -## Fields -## -Header header -byte level -string name # name of the node -string msg # message -string file # file the message came from -string function # function the message came from -uint32 line # line the message came from -string[] topics # topic names that the node publishes diff --git a/src/hal/components/cros/samples/rosdb/sensor_msgs/JointState.msg b/src/hal/components/cros/samples/rosdb/sensor_msgs/JointState.msg deleted file mode 100644 index f67f4bcf..00000000 --- a/src/hal/components/cros/samples/rosdb/sensor_msgs/JointState.msg +++ /dev/null @@ -1,26 +0,0 @@ -# This is a message that holds data to describe the state of a set of torque controlled joints. -# -# The state of each joint (revolute or prismatic) is defined by: -# * the position of the joint (rad or m), -# * the velocity of the joint (rad/s or m/s) and -# * the effort that is applied in the joint (Nm or N). -# -# Each joint is uniquely identified by its name -# The header specifies the time at which the joint states were recorded. All the joint states -# in one message have to be recorded at the same time. -# -# This message consists of a multiple arrays, one for each part of the joint state. -# The goal is to make each of the fields optional. When e.g. your joints have no -# effort associated with them, you can leave the effort array empty. -# -# All arrays in this message should have the same size, or be empty. -# This is the only way to uniquely associate the joint name with the correct -# states. - - -Header header - -string[] name -float64[] position -float64[] velocity -float64[] effort diff --git a/src/hal/components/cros/samples/rosdb/std_msgs/String.msg b/src/hal/components/cros/samples/rosdb/std_msgs/String.msg deleted file mode 100644 index ae721739..00000000 --- a/src/hal/components/cros/samples/rosdb/std_msgs/String.msg +++ /dev/null @@ -1 +0,0 @@ -string data diff --git a/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectory.msg b/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectory.msg deleted file mode 100644 index 99998b91..00000000 --- a/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectory.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -string[] joint_names -JointTrajectoryPoint[] points \ No newline at end of file diff --git a/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectoryPoint.msg b/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectoryPoint.msg deleted file mode 100644 index 55fd3c0a..00000000 --- a/src/hal/components/cros/samples/rosdb/trajectory_msgs/JointTrajectoryPoint.msg +++ /dev/null @@ -1,9 +0,0 @@ -# Each trajectory point specifies either positions[, velocities[, accelerations]] -# or positions[, effort] for the trajectory to be executed. -# All specified values are in the same order as the joint names in JointTrajectory.msg - -float64[] positions -float64[] velocities -float64[] accelerations -float64[] effort -duration time_from_start diff --git a/src/hal/components/cros/samples/talker.c b/src/hal/components/cros/samples/talker.c deleted file mode 100644 index b7a6da99..00000000 --- a/src/hal/components/cros/samples/talker.c +++ /dev/null @@ -1,152 +0,0 @@ -/*! \file talker.c - * \brief This file is an example of cROS usage implementing a publisher and a service caller, which - * can be used together with listener to prove the operation of message transmissions and service - * calls between two nodes. - * - * It creates a publisher to the topic /chatter. Each 100ms the callback function callback_pub() is - * executed composing a string that is sent to the subscriber through a message of this topic. - * This node also implements a caller of the service /sum. Each 200ms the service is called: - * First the callback function callback_srv_add_two_ints is executed with call_resp_flag parameter set to 0, - * in this way two 64bit integers are generated by the function, which are sent to the service provider as - * service call parameters. The result of the service call (sum of the integers) is sent to the service - * caller and the callback function is executed again receiving the result with the call_resp_flag - * parameter set to 1. - * When the number of service calls or published messages is 10 the ROS node exits and the program finishes. - */ - -#include -#include -#include - -#ifdef _WIN32 -//# define WIN32_LEAN_AND_MEAN -//# include -# include - -# define DIR_SEPARATOR_STR "\\" -#else -# include - -# define DIR_SEPARATOR_STR "/" -#endif - -#include "cros.h" -#include "cros_clock.h" - -#define ROS_MASTER_PORT 11311 -#define ROS_MASTER_ADDRESS "127.0.0.1" - -CrosNode *node; //! Pointer to object storing the ROS node. This object includes all the ROS node state variables -unsigned char exit_flag = 0; //! ROS node loop exit flag. When set to 1 the cRosNodeStart() function exits - -// This callback will be invoked when it's our turn to publish a new message -static CallbackResponse callback_pub(cRosMessage *message, void* data_context) -{ - static int pub_count = 0; - char buf[1024]; - // We need to index into the message structure and then assign to fields - cRosMessageField *data_field; - - data_field = cRosMessageGetField(message, "data"); - if(data_field) - { - snprintf(buf, sizeof(buf), "hello world %d", pub_count); - if(cRosMessageSetFieldValueString(data_field, buf) == 0) - { - ROS_INFO(node, "%s\n", buf); - } - } - - if(++pub_count > 10) exit_flag=1; - - return 0; // 0=success -} - -static CallbackResponse callback_srv_add_two_ints(cRosMessage *request, cRosMessage *response, int call_resp_flag, void* context) -{ - static int call_count = 0; - - if(!call_resp_flag) // Check if this callback function has been called to provide the service call arguments or to collect the response - { - cRosMessageField *a_field = cRosMessageGetField(request, "a"); - cRosMessageField *b_field = cRosMessageGetField(request, "b"); - if(a_field != NULL && b_field != NULL) - { - a_field->data.as_int64=10; - b_field->data.as_int64=call_count; - ROS_INFO(node, "Service add 2 ints call arguments: {a: %lld, b: %lld}\b\n", (long long)a_field->data.as_int64, (long long)b_field->data.as_int64); - } - } - else // Service call response available - { - cRosMessageField *sum_field = cRosMessageGetField(response, "sum"); - if(sum_field != NULL) - ROS_INFO(node, "Service add 2 ints response: %lld (call_count: %i)\n", (long long)sum_field->data.as_int64, call_count++); - } - - if(call_count > 10) exit_flag=1; - return 0; // 0=success -} - -int main(int argc, char **argv) -{ - // We need to tell our node where to find the .msg files that we'll be using - char path[4097]; - const char *node_name; - cRosErrCodePack err_cod; - int pubidx, svcidx; - - if(argc>1) - node_name=argv[1]; - else - node_name="/talker"; // Default node name if no command-line parameters are specified - getcwd(path, sizeof(path)); - strncat(path, "/rosdb", sizeof(path) - strlen(path) - 1); - // Create a new node and tell it to connect to roscore in the usual place - node = cRosNodeCreate(node_name, "127.0.0.1", ROS_MASTER_ADDRESS, ROS_MASTER_PORT, path); - - // Create a publisher to topic /chatter of type "std_msgs/String" and request that the associated callback be invoked every 100ms (10Hz) - err_cod = cRosApiRegisterPublisher(node, "/chatter","std_msgs/String", 1000, callback_pub, NULL, NULL, &pubidx); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosApiRegisterPublisher() failed; did you run this program one directory above 'rosdb'?"); - cRosNodeDestroy( node ); - return EXIT_FAILURE; - } - - // Create a service caller named /sum of type "roscpp_tutorials/TwoInts" and request that the associated callback be invoked every 200ms (5Hz) - err_cod = cRosApiRegisterServiceCaller(node,"/sum","roscpp_tutorials/TwoInts", 2000, callback_srv_add_two_ints, NULL, NULL, 1, 1, &svcidx); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosApiRegisterServiceCaller() failed; did you run this program one directory above 'rosdb'?"); - cRosNodeDestroy( node ); - return EXIT_FAILURE; - } - - printf("Node TCPROS port: %i\n", node->tcpros_port); - - // Run the main loop - uint64_t start_time, end_time; - unsigned long elapsed_time; - - start_time = cRosClockGetTimeMs(); - err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); - end_time = cRosClockGetTimeMs(); - - elapsed_time = (unsigned long)(end_time - start_time); - - printf("Elapsed time: %lums\n", elapsed_time); - if(err_cod != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); - - - // All done: free memory and unregister from ROS master - err_cod=cRosNodeDestroy( node ); - if(err_cod != CROS_SUCCESS_ERR_PACK) - { - cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); - return EXIT_FAILURE; - } - - return EXIT_SUCCESS; -} diff --git a/src/hal/components/cros/src/cros_api.c b/src/hal/components/cros/src/cros_api.c deleted file mode 100644 index 7d71a324..00000000 --- a/src/hal/components/cros/src/cros_api.c +++ /dev/null @@ -1,2305 +0,0 @@ -#include -#include -#include // for PATH_MAX - -#include "cros_defs.h" -#include "cros_api.h" -#include "cros_api_internal.h" -#include "cros_message_internal.h" -#include "cros_service.h" -#include "cros_service_internal.h" -#include "cros_message_queue.h" -#include "xmlrpc_process.h" - -#ifdef _WIN32 -# define OS_MAX_PATH _MAX_PATH -#else -# define OS_MAX_PATH PATH_MAX -#endif - -static LookupNodeResult * fetchLookupNodeResult(XmlrpcParamVector *response); -static GetPublishedTopicsResult * fetchGetPublishedTopicsResult(XmlrpcParamVector *response); -static GetTopicTypesResult * fetchGetTopicTypesResult(XmlrpcParamVector *response); -static GetSystemStateResult * fetchGetSystemStateResult(XmlrpcParamVector *response); -static GetUriResult * fetchGetUriResult(XmlrpcParamVector *response); -static LookupServiceResult * fetchLookupServiceResult(XmlrpcParamVector *response); -static GetBusStatsResult * fetchGetBusStatsResult(XmlrpcParamVector *response); -static GetBusInfoResult * fetchGetBusInfoResult(XmlrpcParamVector *response); -static GetMasterUriResult * fetchGetMasterUriResult(XmlrpcParamVector *response); -static ShutdownResult * fetchShutdownResult(XmlrpcParamVector *response); -static GetPidResult * fetchGetPidResult(XmlrpcParamVector *response); -static GetSubscriptionsResult * fetchGetSubscriptionsResult(XmlrpcParamVector *response); -static GetPublicationsResult * fetchGetPublicationsResult(XmlrpcParamVector *response); -static DeleteParamResult * fetchDeleteParamResult(XmlrpcParamVector *response); -static SetParamResult * fetchSetParamResult(XmlrpcParamVector *response); -static GetParamResult * fetchGetParamResult(XmlrpcParamVector *response); -static SearchParamResult * fetchSearchParamResult(XmlrpcParamVector *response); -static HasParamResult * fetchHasParamResult(XmlrpcParamVector *response); -static GetParamNamesResult * fetchGetParamNamesResult(XmlrpcParamVector *response); - -static void freeLookupNodeResult(LookupNodeResult *result); -static void freeGetPublishedTopicsResult(GetPublishedTopicsResult *result); -static void freeGetTopicTypesResult(GetTopicTypesResult *result); -static void freeGetSystemStateResult(GetSystemStateResult *result); -static void freeGetUriResult(GetUriResult *result); -static void freeLookupServiceResult(LookupServiceResult *result); -static void freeGetBusStatsResult(GetBusStatsResult *result); -static void freeGetBusInfoResult(GetBusInfoResult *result); -static void freeGetMasterUriResult(GetMasterUriResult *result); -static void freeShutdownResult(ShutdownResult *result); -static void freeGetPidResult(GetPidResult *result); -static void freeGetSubscriptionsResult(GetSubscriptionsResult *result); -static void freeGetPublicationsResult(GetPublicationsResult *result); -static void freeDeleteParamResult(DeleteParamResult *result); -static void freeSetParamResult(SetParamResult *result); -static void freeGetParamResult(GetParamResult *result); -static void freeSearchParamResult(SearchParamResult *result); -static void freeHasParamResult(HasParamResult *result); -static void freeGetParamNamesResult(GetParamNamesResult *result); - -typedef enum ProviderType -{ - CROS_SUBSCRIBER, - CROS_PUBLISHER, - CROS_SERVICE_CALLER, - CROS_SERVICE_PROVIDER -} ProviderType; - -typedef struct ProviderContext -{ - ProviderType type; //! Type of role: provider, subscriber, service provider or service caller - cRosMessage *incoming; //! Message that has been received - cRosMessage *outgoing; //! Message that will be sent - char *message_definition; - char *md5sum; - void *api_callback; //! The application-defined callback function called to generate outgoing data or to handle the received data - NodeStatusApiCallback status_api_callback; //! The application-defined callback function called when the state of the role has chnaged - cRosMessageQueue *msg_queue; //! It is just a reference to the queue declared in node. For the publisher: it is msgs to send. For the subscriber: it is msgs received. For the svc caller: it is first svc request and then svc response - void *context; //! Context parameter specified by the application and that will be passed to the application-defined callback functions -} ProviderContext; - -static void initProviderContext(ProviderContext *context) -{ - context->type=-1; - context->incoming=NULL; - context->outgoing=NULL; - context->message_definition=NULL; - context->md5sum=NULL; - context->status_api_callback=NULL; - context->api_callback=NULL; - context->msg_queue=NULL; - context->context=NULL; -} - -static void freeProviderContext(ProviderContext *context) -{ - if(context != NULL) - { - cRosMessageFree(context->incoming); - cRosMessageFree(context->outgoing); - free(context->message_definition); - free(context->md5sum); - free(context); - } -} - -static cRosErrCodePack newProviderContext(const char *provider_path, ProviderType type, ProviderContext **context_ptr) -{ - cRosErrCodePack ret_err; - - ret_err = CROS_SUCCESS_ERR_PACK; // Default return value: success - - ProviderContext *context = (ProviderContext *)malloc(sizeof(ProviderContext)); - if (context == NULL) - return CROS_MEM_ALLOC_ERR; - - initProviderContext(context); - context->type = type; - context->md5sum = (char *)calloc(sizeof(char), 33);// 32 chars + '\0'; - if (context->md5sum == NULL) - { - free(context); - return CROS_MEM_ALLOC_ERR; - } - - switch(type) - { - case CROS_SUBSCRIBER: - { - ret_err = cRosMessageNewBuild(NULL, provider_path, &context->incoming); - if (ret_err == CROS_SUCCESS_ERR_PACK) - { - strcpy(context->md5sum, context->incoming->md5sum); - context->message_definition = strdup(context->incoming->msgDef->plain_text); // Alloc new mem so that it can be freed independently - } - break; - } - case CROS_PUBLISHER: - { - ret_err = cRosMessageNewBuild(NULL, provider_path, &context->outgoing); - if (ret_err == CROS_SUCCESS_ERR_PACK) - { - strcpy(context->md5sum, context->outgoing->md5sum); - context->message_definition = strdup(context->outgoing->msgDef->plain_text); - } - break; - } - case CROS_SERVICE_PROVIDER: - { - ret_err = cRosServiceBuildInner(&context->incoming, &context->outgoing, NULL, context->md5sum, provider_path); - break; - } - case CROS_SERVICE_CALLER: - { - ret_err = cRosServiceBuildInner(&context->outgoing, &context->incoming, &context->message_definition, context->md5sum, provider_path); - break; - } - default: - { - PRINT_ERROR ( "newProviderContext() : Unknown ProviderType specified\n" ); - ret_err = CROS_BAD_PARAM_ERR; - } - } - - if(ret_err == CROS_SUCCESS_ERR_PACK) - *context_ptr = context; // No error occurred: return the created context - else - freeProviderContext(context); // An error occurred: free the allocated memory and return the error code - - return ret_err; -} - -cRosErrCodePack cRosNodeSerializeOutgoingMessage(DynBuffer *buffer, void *context_) -{ - cRosErrCodePack ret_err; - ProviderContext *context = (ProviderContext *)context_; - - ret_err = cRosMessageSerialize(context->outgoing, buffer); - return(ret_err); -} - -cRosErrCodePack cRosNodeDeserializeIncomingPacket(DynBuffer *buffer, void *context_) -{ - cRosErrCodePack ret_err; - ProviderContext *context = (ProviderContext *)context_; - - ret_err = cRosMessageDeserialize(context->incoming, buffer); - return(ret_err); -} - -cRosErrCodePack cRosNodePublisherCallback(void *context_) -{ - cRosErrCodePack ret_err; - ProviderContext *context = (ProviderContext *)context_; - - ret_err = CROS_SUCCESS_ERR_PACK; // Default return value - - if(cRosMessageQueueUsage(context->msg_queue) > 0) // An inmediate message is waiting to be sent - { - if(cRosMessageQueueExtract(context->msg_queue, context->outgoing) != 0) - ret_err = CROS_EXTRACT_MSG_INT_ERR; - } - else // It is time to send a periodic message - { - CallbackResponse ret_cb; - - // Cast to the appropriate public api callback and invoke it on the user context - PublisherApiCallback publisher_user_callback = (PublisherApiCallback)context->api_callback; - if(publisher_user_callback != NULL) - { - ret_cb = publisher_user_callback(context->outgoing, context->context); // Use the callback if it is available and no msg is waiting in the queue - if(ret_cb != 0) - ret_err = CROS_TOP_PUB_CALLBACK_ERR; - } - } - - if(ret_err != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(ret_err, "cRosNodePublisherCallback() failed encoding the packet to send"); - - return ret_err; -} - -cRosErrCodePack cRosNodeSubscriberCallback(void *context_) -{ - cRosErrCodePack ret_err; - ProviderContext *context = (ProviderContext *)context_; - cRosMessageQueueAdd(context->msg_queue, context->incoming); - - // Cast to the appropriate public api callback and invoke it on the user context - SubscriberApiCallback subs_user_callback_fn = (SubscriberApiCallback)context->api_callback; - if(subs_user_callback_fn != NULL) - { - CallbackResponse ret_cb = subs_user_callback_fn(context->incoming, context->context); - if(ret_cb == 0) - ret_err = CROS_SUCCESS_ERR_PACK; - else - ret_err = CROS_TOP_SUB_CALLBACK_ERR; - } - else - ret_err = CROS_SUCCESS_ERR_PACK; - - return ret_err; -} - -cRosErrCodePack cRosNodeServiceCallerCallback(int call_resp_flag, void* contex_) -{ - cRosErrCodePack ret_err; - CallbackResponse ret_cb; - ServiceCallerApiCallback svc_call_user_callback_fn; - ProviderContext *context = (ProviderContext *)contex_; - - svc_call_user_callback_fn = (ServiceCallerApiCallback)context->api_callback; - - if(call_resp_flag) // Process service response - { - // If msg_queue is empty, this is a periodic call (send msg response to callback fn), otherwise this is a non-periodic call (put msg response in the queue) - if(cRosMessageQueueUsage(context->msg_queue) == 0) // Periodic service call - { - if(svc_call_user_callback_fn != NULL) - { - ret_cb = svc_call_user_callback_fn(context->outgoing, context->incoming, call_resp_flag, context->context); - if(ret_cb != 0) // The callback indicated and error in the return value when processing the service response - ret_err=CROS_SVC_RES_CALLBACK_ERR; - else - ret_err = CROS_SUCCESS_ERR_PACK; - } - else - ret_err = CROS_SUCCESS_ERR_PACK; - } - else // Non-periodic service call - { - if(cRosMessageQueueAdd(context->msg_queue, context->incoming) == 0) // Add response msg to the queue (in case the svc was called non periodically) - ret_err = CROS_SUCCESS_ERR_PACK; // A new message has been aded to the queue to later store the received response - else - ret_err = CROS_MEM_ALLOC_ERR; - } - if(ret_err != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(ret_err, "cRosNodeServiceCallerCallback() failed decoding the received service response packet"); - } - else // Generate service request - { - // If msg_queue is empty, this is a periodic call (get the msg request from the callback fn), otherwise this is a non-periodic call (get msg request from the queue) - if(cRosMessageQueueUsage(context->msg_queue) == 0) // Periodic service call - { - if(svc_call_user_callback_fn != NULL) - { - ret_cb = svc_call_user_callback_fn(context->outgoing, context->incoming, call_resp_flag, context->context); - if(ret_cb == 0) // The callback returned success when generating the service request: send the service request - ret_err = CROS_SUCCESS_ERR_PACK; - else // The callback indicated and error in the return value when generating the service request - ret_err = CROS_SVC_REQ_CALLBACK_ERR; - } - else - ret_err = CROS_SUCCESS_ERR_PACK; - } - else // Non-periodic service call - ret_err = (cRosMessageQueueGet(context->msg_queue, context->outgoing) == 0)?CROS_SUCCESS_ERR_PACK:CROS_EXTRACT_MSG_INT_ERR; // Copy the message request from the queue msg - - if(ret_err != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(ret_err, "cRosNodeServiceCallerCallback() failed getting the service request packet"); - } - - return ret_err; -} - -cRosErrCodePack cRosNodeServiceProviderCallback(void *context_) -{ - cRosErrCodePack ret_err; - ProviderContext *context = (ProviderContext *)context_; - - ServiceProviderApiCallback serviceProviderApiCallback = (ServiceProviderApiCallback)context->api_callback; - CallbackResponse ret_cb = serviceProviderApiCallback(context->incoming, context->outgoing, context->context); - - if(ret_cb == 0) - ret_err = CROS_SUCCESS_ERR_PACK; - else - ret_err = CROS_SVC_SER_CALLBACK_ERR; - - return ret_err; -} - -void cRosNodeStatusCallback(CrosNodeStatusUsr *status, void* context_) -{ - ProviderContext *context = (ProviderContext *)context_; - if(context->status_api_callback != NULL) // If the application defined a status callback function, call it - context->status_api_callback(status, context->context); -} - -cRosErrCodePack cRosApiRegisterServiceCaller(CrosNode *node, const char *service_name, const char *service_type, int loop_period, - ServiceCallerApiCallback callback, NodeStatusApiCallback status_callback, void *context, int persistent, int tcp_nodelay, int *svcidx_ptr) -{ - cRosErrCodePack ret_err; - char path[OS_MAX_PATH]; - ProviderContext *nodeContext = NULL; - int svcidx; - - if(loop_period >= 0 && callback == NULL) - return CROS_BAD_PARAM_ERR; - - getSrvFilePath(node, path, OS_MAX_PATH, service_type); - ret_err = newProviderContext(path, CROS_SERVICE_CALLER, &nodeContext); - if (ret_err == CROS_SUCCESS_ERR_PACK) - { - nodeContext->api_callback = callback; - nodeContext->status_api_callback = status_callback; - nodeContext->context = context; - - // NB: Pass the private ProviderContext to the private api, not the user context - svcidx = cRosNodeRegisterServiceCaller(node, nodeContext->message_definition, service_name, service_type, nodeContext->md5sum, - loop_period, nodeContext, persistent, tcp_nodelay); - if(svcidx >= 0) // Success - { - if(svcidx_ptr != NULL) - *svcidx_ptr = svcidx; // Return the index of the created service caller - nodeContext->msg_queue = &node->service_callers[svcidx].msg_queue; - } - else - ret_err=CROS_MEM_ALLOC_ERR; - } - return ret_err; -} - -void cRosApiReleaseServiceCaller(CrosNode *node, int svcidx) -{ - ServiceCallerNode *svc = &node->service_callers[svcidx]; - ProviderContext *context = (ProviderContext *)svc->context; - freeProviderContext(context); - cRosNodeReleaseServiceCaller(svc); -} - -cRosErrCodePack cRosApiRegisterServiceProvider(CrosNode *node, const char *service_name, const char *service_type, - ServiceProviderApiCallback callback, NodeStatusApiCallback status_callback, void *context, int *svcidx_ptr) -{ - cRosErrCodePack ret_err; - char path[OS_MAX_PATH]; - ProviderContext *nodeContext = NULL; - int svcidx; - - if (callback == NULL) - return CROS_BAD_PARAM_ERR; - - getSrvFilePath(node, path, OS_MAX_PATH, service_type); - ret_err = newProviderContext(path, CROS_SERVICE_PROVIDER, &nodeContext); - if (ret_err == CROS_SUCCESS_ERR_PACK) - { - nodeContext->api_callback = callback; - nodeContext->status_api_callback = status_callback; - nodeContext->context = context; - - // NB: Pass the private ProviderContext to the private api, not the user context - svcidx = cRosNodeRegisterServiceProvider(node, service_name, service_type, nodeContext->md5sum, nodeContext); - if(svcidx >= 0) // Success - { - if(svcidx_ptr != NULL) - *svcidx_ptr = svcidx; // Return the index of the created service provider - } - else - ret_err=CROS_MEM_ALLOC_ERR; - } - return ret_err; -} - -cRosErrCodePack cRosApiUnregisterServiceProvider(CrosNode *node, int svcidx) -{ - int ret_err; - if (svcidx < 0 || svcidx >= CN_MAX_SERVICE_PROVIDERS) - return CROS_BAD_PARAM_ERR; - - ServiceProviderNode *service = &node->service_providers[svcidx]; - if (service->service_name == NULL) - return CROS_TOPIC_SUB_IND_ERR; - - ret_err = cRosNodeUnregisterServiceProvider(node, svcidx); - - return (ret_err != -1)? CROS_SUCCESS_ERR_PACK: CROS_UNSPECIFIED_ERR; -} - -void cRosApiReleaseServiceProvider(CrosNode *node, int svcidx) -{ - ServiceProviderNode *svc = &node->service_providers[svcidx]; - ProviderContext *context = (ProviderContext *)svc->context; - freeProviderContext(context); - cRosNodeReleaseServiceProvider(svc); -} - -cRosErrCodePack cRosApiRegisterSubscriber(CrosNode *node, const char *topic_name, const char *topic_type, - SubscriberApiCallback callback, NodeStatusApiCallback status_callback, void *context, int tcp_nodelay, int *subidx_ptr) -{ - cRosErrCodePack ret_err; - char path[OS_MAX_PATH]; - ProviderContext *nodeContext = NULL; - int subidx; - - cRosGetMsgFilePath(node, path, OS_MAX_PATH, topic_type); - ret_err = newProviderContext(path, CROS_SUBSCRIBER, &nodeContext); - if (ret_err == CROS_SUCCESS_ERR_PACK) - { - nodeContext->api_callback = callback; - nodeContext->status_api_callback = status_callback; - nodeContext->context = context; - - // NB: Pass the private ProviderContext to the private api, not the user context - subidx = cRosNodeRegisterSubscriber(node, nodeContext->message_definition, topic_name, topic_type, - nodeContext->md5sum, nodeContext, tcp_nodelay); - if(subidx >= 0) // Success - { - nodeContext->msg_queue = &node->subs[subidx].msg_queue; // Allow the callback functions to access the msg queue - if(subidx_ptr != NULL) - *subidx_ptr = subidx; // Return the index of the created service caller - } - else - ret_err=CROS_MEM_ALLOC_ERR; - } - return ret_err; -} - -cRosErrCodePack cRosApiUnregisterSubscriber(CrosNode *node, int subidx) -{ - int ret_err; - if (subidx < 0 || subidx >= CN_MAX_SUBSCRIBED_TOPICS) - return CROS_BAD_PARAM_ERR; - - SubscriberNode *sub = &node->subs[subidx]; - if (sub->topic_name == NULL) - return CROS_TOPIC_SUB_IND_ERR; - - ret_err = cRosNodeUnregisterSubscriber(node, subidx); - - return (ret_err != -1)? CROS_SUCCESS_ERR_PACK: CROS_UNSPECIFIED_ERR; -} - -void cRosApiReleaseSubscriber(CrosNode *node, int subidx) -{ - SubscriberNode *sub = &node->subs[subidx]; - ProviderContext *context = (ProviderContext *)sub->context; - freeProviderContext(context); - cRosNodeReleaseSubscriber(sub); -} - -cRosErrCodePack cRosApiRegisterPublisher(CrosNode *node, const char *topic_name, const char *topic_type, int loop_period, - PublisherApiCallback callback, NodeStatusApiCallback status_callback, void *context, int *pubidx_ptr) -{ - cRosErrCodePack ret_err; - char path[OS_MAX_PATH]; - ProviderContext *nodeContext = NULL; - int pubidx; - - if(loop_period >= 0 && callback == NULL) - return CROS_BAD_PARAM_ERR; - - cRosGetMsgFilePath(node, path, OS_MAX_PATH, topic_type); - ret_err = newProviderContext(path, CROS_PUBLISHER, &nodeContext); - if (ret_err == CROS_SUCCESS_ERR_PACK) - { - nodeContext->api_callback = callback; - nodeContext->status_api_callback = status_callback; - nodeContext->context = context; - - // NB: Pass the private ProviderContext to the private api, not the user context - pubidx = cRosNodeRegisterPublisher(node, nodeContext->message_definition, topic_name, topic_type, - nodeContext->md5sum, loop_period, nodeContext); - if(pubidx >= 0) // Success - { - // Allow the callback functions to access the msg queue and send-now flag - nodeContext->msg_queue = &node->pubs[pubidx].msg_queue; - if(pubidx_ptr != NULL) - *pubidx_ptr = pubidx; // Return the index of the created service caller - } - else - ret_err=CROS_MEM_ALLOC_ERR; - } - return ret_err; -} - -cRosErrCodePack cRosApiUnregisterPublisher(CrosNode *node, int pubidx) -{ - int ret_err; - if (pubidx < 0 || pubidx >= CN_MAX_PUBLISHED_TOPICS) - return CROS_BAD_PARAM_ERR; - - PublisherNode *pub = &node->pubs[pubidx]; - if (pub->topic_name == NULL) - return CROS_TOPIC_PUB_IND_ERR; - - ret_err = cRosNodeUnregisterPublisher(node, pubidx); - - return (ret_err != -1)? CROS_SUCCESS_ERR_PACK: CROS_UNSPECIFIED_ERR; -} - -void cRosApiReleasePublisher(CrosNode *node, int pubidx) -{ - PublisherNode *pub = &node->pubs[pubidx]; - ProviderContext *context = (ProviderContext *)pub->context; - freeProviderContext(context); - cRosNodeReleasePublisher(pub); -} - -cRosErrCodePack cRosApicRosApiLookupNode(CrosNode *node, const char *node_name, LookupNodeCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApicRosApiLookupNode() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_LOOKUP_NODE; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchLookupNodeResult; - call->free_result_callback = (FreeResultCallback)freeLookupNodeResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, node_name); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetPublishedTopics(CrosNode *node, const char *subgraph, GetPublishedTopicsCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetPublishedTopics() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_PUBLISHED_TOPICS; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetPublishedTopicsResult; - call->free_result_callback = (FreeResultCallback)freeGetPublishedTopicsResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, subgraph == NULL ? "" : subgraph); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetTopicTypes(CrosNode *node, GetTopicTypesCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetTopicTypes() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_TOPIC_TYPES; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetTopicTypesResult; - call->free_result_callback = (FreeResultCallback)freeGetTopicTypesResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetSystemState(CrosNode *node, GetSystemStateCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetSystemState() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_SYSTEM_STATE; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetSystemStateResult; - call->free_result_callback = (FreeResultCallback)freeGetSystemStateResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetUri(CrosNode *node, GetUriCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetUri() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_URI; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetUriResult; - call->free_result_callback = (FreeResultCallback)freeGetUriResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiLookupService(CrosNode *node, const char *service, LookupServiceCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiLookupService() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_LOOKUP_SERVICE; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchLookupServiceResult; - call->free_result_callback = (FreeResultCallback)freeLookupServiceResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, service); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetBusStats(CrosNode *node, const char* host, int port, - GetBusStatsCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetBusStats() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_BUS_STATS; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetBusStatsResult; - call->free_result_callback = (FreeResultCallback)freeGetBusStatsResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueSlaveApiCall(node, call, host, port); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetBusInfo(CrosNode *node, const char* host, int port, - GetBusInfoCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetBusInfo() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_BUS_INFO; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetBusInfoResult; - call->free_result_callback = (FreeResultCallback)freeGetBusInfoResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueSlaveApiCall(node, call, host, port); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetMasterUri(CrosNode *node, const char* host, int port, - GetMasterUriCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_MASTER_URI; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetMasterUriResult; - call->free_result_callback = (FreeResultCallback)freeGetMasterUriResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueSlaveApiCall(node, call, host, port); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiShutdown(CrosNode *node, const char* host, int port, const char *msg, - GetMasterUriCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiShutdown() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_SHUTDOWN; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchShutdownResult; - call->free_result_callback = (FreeResultCallback)freeShutdownResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, msg == NULL ? "" : msg); - - caller_id = enqueueSlaveApiCall(node, call, host, port); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetPid(CrosNode *node, const char* host, int port, - GetPidCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetPid() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_PID; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetPidResult; - call->free_result_callback = (FreeResultCallback)freeGetPidResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueSlaveApiCall(node, call, host, port); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetSubscriptions(CrosNode *node, const char* host, int port, - GetSubscriptionsCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_SUBSCRIPTIONS; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetSubscriptionsResult; - call->free_result_callback = (FreeResultCallback)freeGetSubscriptionsResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueSlaveApiCall(node, call, host, port); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetPublications(CrosNode *node, const char* host, int port, - GetSubscriptionsCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetPublications() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_PUBLICATIONS; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetPublicationsResult; - call->free_result_callback = (FreeResultCallback)freeGetPublicationsResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueSlaveApiCall(node, call, host, port); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiDeleteParam(CrosNode *node, const char *key, DeleteParamCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiDeleteParam() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_DELETE_PARAM; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchDeleteParamResult; - call->free_result_callback = (FreeResultCallback)freeDeleteParamResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, key); - - caller_id=enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiSetParam(CrosNode *node, const char *key, XmlrpcParam *value, SetParamCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id, vec_size; - - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiSetParam() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - XmlrpcParam param; - int rc = xmlrpcParamCopy(¶m, value); - if (rc == -1) - { - PRINT_ERROR ( "cRosApiSetParam() : Can't allocate memory\n"); - freeRosApiCall(call); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_SET_PARAM; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchSetParamResult; - call->free_result_callback = (FreeResultCallback)freeSetParamResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, key); - vec_size = xmlrpcParamVectorPushBack(&call->params, ¶m); - if (vec_size == -1) - { - freeRosApiCall(call); - xmlrpcParamRelease(¶m); - return CROS_MEM_ALLOC_ERR; - } - - caller_id=enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetParam(CrosNode *node, const char *key, GetParamCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetParam() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_PARAM; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetParamResult; - call->free_result_callback = (FreeResultCallback)freeGetParamResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, key); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiSearchParam(CrosNode *node, const char *key, SearchParamCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiSearchParam() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_SEARCH_PARAM; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchSearchParamResult; - call->free_result_callback = (FreeResultCallback)freeSearchParamResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, key); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiHasParam(CrosNode *node, const char *key, HasParamCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiHasParam() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_HAS_PARAM; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchHasParamResult; - call->free_result_callback = (FreeResultCallback)freeHasParamResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - xmlrpcParamVectorPushBackString(&call->params, key); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosApiGetParamNames(CrosNode *node, GetParamNamesCallback callback, void *context, int *caller_id_ptr) -{ - int caller_id; - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosApiGetParamNames() : Can't allocate memory\n"); - return CROS_MEM_ALLOC_ERR; - } - - call->method = CROS_API_GET_PARAM_NAMES; - call->result_callback = (ResultCallback)callback; - call->context_data = context; - call->fetch_result_callback = (FetchResultCallback)fetchGetParamNamesResult; - call->free_result_callback = (FreeResultCallback)freeGetParamNamesResult; - - xmlrpcParamVectorPushBackString(&call->params, node->name); - - caller_id = enqueueMasterApiCall(node, call); - if(caller_id_ptr != NULL) - *caller_id_ptr = caller_id; - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -XmlrpcParam *GetMethodResponseStatus(XmlrpcParamVector *response, int *status_code, char **status_msg) -{ - XmlrpcParam *resp_param; - - resp_param = xmlrpcParamVectorAt(response, 0); - if (resp_param != NULL) - { - XmlrpcParam *status_code_param; - status_code_param = xmlrpcParamArrayGetParamAt(resp_param, 0); // resp_param must be a vector or structure in order to be accessed by index - if (status_code_param != NULL) - { - if( status_code_param->type == XMLRPC_PARAM_INT ) - { - XmlrpcParam *status_msg_param; - - *status_code = status_code_param->data.as_int; - status_msg_param = xmlrpcParamArrayGetParamAt(resp_param, 1); - if (status_msg_param != NULL) - { - if( status_msg_param->type == XMLRPC_PARAM_STRING ) - *status_msg = strdup(status_msg_param->data.as_string); - else - *status_msg = NULL; // status_msg = NULL: No human-readable string describing the returned status has been found - } - else - *status_msg = NULL; // The array or structure returned in the ROS master response does not contain a second element (the human-readable string describing the status) - } - else - { - PRINT_ERROR ( "GetMethodResponseStatus() : The status code integer cannot be found in the responsed returned by the ROS master.\n"); - resp_param = NULL; - } - } - else - { - PRINT_ERROR ( "GetMethodResponseStatus() : The responsed returned by the ROS master does not contain a non-empty array or structure.\n"); - resp_param = NULL; - } - } - else - PRINT_ERROR ( "GetMethodResponseStatus() : The ROS master returned an XML response with an unexpected format.\n"); - - return(resp_param); -} - -LookupNodeResult * fetchLookupNodeResult(XmlrpcParamVector *response) -{ - LookupNodeResult *ret = (LookupNodeResult *)calloc(1, sizeof(LookupNodeResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchLookupNodeResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeLookupNodeResult(ret); - return NULL; - } - - XmlrpcParam* uri = xmlrpcParamArrayGetParamAt(array, 2); - ret->uri = strdup(uri->data.as_string); - if (ret->uri == NULL) - { - freeLookupNodeResult(ret); - return NULL; - } - - return ret; -} - -GetPublishedTopicsResult * fetchGetPublishedTopicsResult(XmlrpcParamVector *response) -{ - int it; - - GetPublishedTopicsResult *ret = (GetPublishedTopicsResult *)calloc(1, sizeof(GetPublishedTopicsResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetPublishedTopicsResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetPublishedTopicsResult(ret); - return NULL; - } - - XmlrpcParam* topics = xmlrpcParamArrayGetParamAt(array, 2); - ret->topics = (struct TopicTypePair *)calloc(topics->array_n_elem, sizeof(struct TopicTypePair)); - if (ret->topics == NULL) - goto clean; - ret->topic_count = topics->array_n_elem; - - for (it = 0; it < topics->array_n_elem; it++) - { - struct TopicTypePair *pair = &ret->topics[it]; - XmlrpcParam* pair_xml = xmlrpcParamArrayGetParamAt(topics, it); - XmlrpcParam* topic = xmlrpcParamArrayGetParamAt(pair_xml, 0); - pair->topic = (char *)malloc(strlen(topic->data.as_string) + 1); - if (pair->topic == NULL) - goto clean; - strcpy(pair->topic, topic->data.as_string); - - XmlrpcParam* type = xmlrpcParamArrayGetParamAt(pair_xml, 1); - pair->type = (char *)malloc(strlen(type->data.as_string) + 1); - if (pair->type == NULL) - goto clean; - strcpy(pair->type, type->data.as_string); - } - - return ret; - -clean: - freeGetPublishedTopicsResult(ret); - return NULL; -} - -GetTopicTypesResult * fetchGetTopicTypesResult(XmlrpcParamVector *response) -{ - int it; - - GetTopicTypesResult *ret = (GetTopicTypesResult *)calloc(1, sizeof(GetTopicTypesResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetTopicTypesResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetTopicTypesResult(ret); - return NULL; - } - - XmlrpcParam* topics = xmlrpcParamArrayGetParamAt(array, 2); - ret->topics = (struct TopicTypePair *)calloc(topics->array_n_elem, sizeof(struct TopicTypePair)); - if (ret->topics == NULL) - goto clean; - ret->topic_count = topics->array_n_elem; - - for (it = 0; it < topics->array_n_elem; it++) - { - struct TopicTypePair *pair = &ret->topics[it]; - XmlrpcParam* pair_xml = xmlrpcParamArrayGetParamAt(topics, it); - XmlrpcParam* topic = xmlrpcParamArrayGetParamAt(pair_xml, 0); - pair->topic = (char *)malloc(strlen(topic->data.as_string) + 1); - if (pair->topic == NULL) - goto clean; - strcpy(pair->topic, topic->data.as_string); - - XmlrpcParam* type = xmlrpcParamArrayGetParamAt(pair_xml, 1); - pair->type = (char *)malloc(strlen(type->data.as_string) + 1); - if (pair->type == NULL) - goto clean; - strcpy(pair->type, type->data.as_string); - } - - return ret; - -clean: - freeGetTopicTypesResult(ret); - return NULL; -} - -GetSystemStateResult * fetchGetSystemStateResult(XmlrpcParamVector *response) -{ - int it1; - - GetSystemStateResult *ret = (GetSystemStateResult *)calloc(1, sizeof(GetSystemStateResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetSystemStateResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetSystemStateResult(ret); - return NULL; - } - - if (array->array_n_elem < 3) - return ret; - - XmlrpcParam* publishers = xmlrpcParamArrayGetParamAt(array, 2); - ret->publishers = (struct ProviderState *)calloc(publishers->array_n_elem, sizeof(struct ProviderState)); - if (ret->publishers == NULL) - goto clean; - ret->pub_count = publishers->array_n_elem; - - for (it1 = 0; it1 < publishers->array_n_elem; it1++) - { - int it2; - - struct ProviderState *state = &ret->publishers[it1]; - XmlrpcParam* state_xml = xmlrpcParamArrayGetParamAt(publishers, it1); - XmlrpcParam* name = xmlrpcParamArrayGetParamAt(state_xml, 0); - state->provider_name = (char *)malloc(strlen(name->data.as_string) + 1); - if (state->provider_name == NULL) - goto clean; - strcpy(state->provider_name, name->data.as_string); - - XmlrpcParam* users_xml = xmlrpcParamArrayGetParamAt(state_xml, 1); - state->users = (char **)calloc(1, users_xml->array_n_elem * sizeof(char *)); - if (state->users == NULL) - goto clean; - - for (it2 = 0; it2 < users_xml->array_n_elem; it2++) - { - char *user = state->users[it2]; - XmlrpcParam* user_xml = xmlrpcParamArrayGetParamAt(users_xml, it2); - user = (char *)malloc(strlen(user_xml->data.as_string) + 1); - if (user == NULL) - goto clean; - strcpy(user, user_xml->data.as_string); - } - } - - if (array->array_n_elem < 4) - return ret; - - XmlrpcParam* subscribers = xmlrpcParamArrayGetParamAt(array, 3); - ret->subscribers = (struct ProviderState *)calloc(subscribers->array_n_elem, sizeof(struct ProviderState)); - if (ret->subscribers == NULL) - goto clean; - ret->sub_count = subscribers->array_n_elem; - - for (it1 = 0; it1 < subscribers->array_n_elem; it1++) - { - int it2; - - struct ProviderState *state = &ret->subscribers[it1]; - XmlrpcParam* state_xml = xmlrpcParamArrayGetParamAt(subscribers, it1); - XmlrpcParam* name = xmlrpcParamArrayGetParamAt(state_xml, 0); - state->provider_name = (char *)malloc(strlen(name->data.as_string) + 1); - if (state->provider_name == NULL) - goto clean; - strcpy(state->provider_name, name->data.as_string); - - XmlrpcParam* users_xml = xmlrpcParamArrayGetParamAt(state_xml, 1); - state->users = (char **)calloc(users_xml->array_n_elem, sizeof(char *)); - if (state->users == NULL) - goto clean; - - for (it2 = 0; it2 < users_xml->array_n_elem; it2++) - { - char *user = state->users[it2]; - XmlrpcParam* user_xml = xmlrpcParamArrayGetParamAt(users_xml, it2); - user = (char *)malloc(strlen(user_xml->data.as_string) + 1); - if (user == NULL) - goto clean; - strcpy(user, user_xml->data.as_string); - } - } - - if (array->array_n_elem < 5) - return ret; - - XmlrpcParam* services = xmlrpcParamArrayGetParamAt(array, 4); - ret->service_providers = (struct ProviderState *)calloc(services->array_n_elem, sizeof(struct ProviderState)); - if (ret->service_providers == NULL) - goto clean; - ret->svc_count = services->array_n_elem; - - for (it1 = 0; it1 < services->array_n_elem; it1++) - { - int it2; - - struct ProviderState *state = &ret->service_providers[it1]; - XmlrpcParam* state_xml = xmlrpcParamArrayGetParamAt(services, it1); - XmlrpcParam* name = xmlrpcParamArrayGetParamAt(state_xml, 0); - state->provider_name = (char *)malloc(strlen(name->data.as_string) + 1); - if (state->provider_name == NULL) - goto clean; - strcpy(state->provider_name, name->data.as_string); - - XmlrpcParam* users_xml = xmlrpcParamArrayGetParamAt(state_xml, 1); - state->users = (char **)calloc(users_xml->array_n_elem, sizeof(char *)); - if (state->users == NULL) - goto clean; - - for (it2 = 0; it2 < users_xml->array_n_elem; it2++) - { - char *user = state->users[it2]; - XmlrpcParam* user_xml = xmlrpcParamArrayGetParamAt(users_xml, it2); - user = (char *)malloc(strlen(user_xml->data.as_string) + 1); - if (user == NULL) - goto clean; - strcpy(user, user_xml->data.as_string); - } - } - - return ret; - -clean: - freeGetSystemStateResult(ret); - return NULL; -} - -GetUriResult * fetchGetUriResult(XmlrpcParamVector *response) -{ - GetUriResult *ret = (GetUriResult *)calloc(1, sizeof(GetUriResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetUriResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetUriResult(ret); - return NULL; - } - - XmlrpcParam* uri = xmlrpcParamArrayGetParamAt(array, 2); - ret->master_uri = strdup(uri->data.as_string); - if (ret == NULL) - { - freeGetUriResult(ret); - return NULL; - } - - return ret; -} - -LookupServiceResult * fetchLookupServiceResult(XmlrpcParamVector *response) -{ - LookupServiceResult *ret = (LookupServiceResult *)calloc(1, sizeof(LookupServiceResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchLookupServiceResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeLookupServiceResult(ret); - return NULL; - } - - XmlrpcParam* service = xmlrpcParamArrayGetParamAt(array, 2); - ret->service_result = strdup(service->data.as_string); - if (ret == NULL) - { - freeLookupServiceResult(ret); - return NULL; - } - - return ret; -} - -GetBusStatsResult * fetchGetBusStatsResult(XmlrpcParamVector *response) -{ - int it1; - - GetBusStatsResult *ret = (GetBusStatsResult *)calloc(1, sizeof(GetBusStatsResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetBusStatsResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetBusStatsResult(ret); - return NULL; - } - - if (array->array_n_elem < 3) - return ret; - - XmlrpcParam* pubs_stats = xmlrpcParamArrayGetParamAt(array, 2); - ret->stats.pub_stats = (struct TopicPubStats *)calloc(pubs_stats->array_n_elem, sizeof(struct TopicPubStats)); - if (ret->stats.pub_stats == NULL) - goto clean; - ret->stats.pub_stats_count = pubs_stats->array_n_elem; - - for (it1 = 0; it1 < pubs_stats->array_n_elem; it1++) - { - int it2; - - struct TopicPubStats *pub_stats = &ret->stats.pub_stats[it1]; - XmlrpcParam* pub_stats_xml = xmlrpcParamArrayGetParamAt(pubs_stats, it1); - if (pub_stats_xml->array_n_elem < 3) - goto clean; - - XmlrpcParam* name_xml = xmlrpcParamArrayGetParamAt(pub_stats_xml, 0); - pub_stats->topic_name = (char *)malloc(strlen(name_xml->data.as_string) + 1); - if (pub_stats->topic_name == NULL) - goto clean; - strcpy(pub_stats->topic_name, name_xml->data.as_string); - - XmlrpcParam* message_data_sent = xmlrpcParamArrayGetParamAt(pub_stats_xml, 1); - pub_stats->message_data_sent = (size_t)message_data_sent->data.as_int; - - XmlrpcParam* pub_datas = xmlrpcParamArrayGetParamAt(pub_stats_xml, 2); - pub_stats->datas = (struct PubConnectionData *)calloc(pub_datas->array_n_elem, sizeof(struct PubConnectionData)); - if (pub_stats->datas == NULL) - goto clean; - pub_stats->datas_count = pub_datas->array_n_elem; - - for (it2 = 0; it2 < pub_datas->array_n_elem; it2++) - { - struct PubConnectionData *pub_data = &pub_stats->datas[it2]; - XmlrpcParam* pub_data_xml = xmlrpcParamArrayGetParamAt(pub_datas, it2); - XmlrpcParam* connection_id = xmlrpcParamArrayGetParamAt(pub_data_xml, 0); - XmlrpcParam* bytes_sent = xmlrpcParamArrayGetParamAt(pub_data_xml, 1); - XmlrpcParam* num_sent = xmlrpcParamArrayGetParamAt(pub_data_xml, 2); - XmlrpcParam* connected = xmlrpcParamArrayGetParamAt(pub_data_xml, 3); - pub_data->connection_id = connection_id->data.as_int; - pub_data->bytes_sent = (size_t)bytes_sent->data.as_int; - pub_data->num_sent = (size_t)num_sent->data.as_int; - pub_data->connected = connected->data.as_int; - } - } - - if (array->array_n_elem < 4) - return ret; - - XmlrpcParam* subs_stats = xmlrpcParamArrayGetParamAt(array, 3); - ret->stats.sub_stats = (struct TopicSubStats *)calloc(subs_stats->array_n_elem, sizeof(struct TopicSubStats)); - if (ret->stats.sub_stats == NULL) - goto clean; - ret->stats.sub_stats_count = subs_stats->array_n_elem; - - for (it1 = 0; it1 < subs_stats->array_n_elem; it1++) - { - int it2; - - struct TopicSubStats *sub_stats = &ret->stats.sub_stats[it1]; - XmlrpcParam *sub_stats_xml = xmlrpcParamArrayGetParamAt(subs_stats, it1); - if (sub_stats_xml->array_n_elem < 3) - goto clean; - - XmlrpcParam *name_xml = xmlrpcParamArrayGetParamAt(sub_stats_xml, 0); - sub_stats->topic_name = (char *)malloc(strlen(name_xml->data.as_string) + 1); - if (sub_stats->topic_name == NULL) - goto clean; - strcpy(sub_stats->topic_name, name_xml->data.as_string); - - XmlrpcParam* sub_datas = xmlrpcParamArrayGetParamAt(sub_stats_xml, 1); - sub_stats->datas = (struct SubConnectionData *)calloc(sub_datas->array_n_elem, sizeof(struct SubConnectionData)); - if (sub_stats->datas == NULL) - goto clean; - sub_stats->datas_count = sub_datas->array_n_elem; - - for (it2 = 0; it2 < sub_datas->array_n_elem; it2++) - { - struct SubConnectionData *sub_data = &sub_stats->datas[it2]; - XmlrpcParam *sub_data_xml = xmlrpcParamArrayGetParamAt(sub_datas, it2); - XmlrpcParam *connection_id = xmlrpcParamArrayGetParamAt(sub_data_xml, 0); - XmlrpcParam *bytes_received = xmlrpcParamArrayGetParamAt(sub_data_xml, 1); - XmlrpcParam *drop_estimate = xmlrpcParamArrayGetParamAt(sub_data_xml, 2); - XmlrpcParam *connected = xmlrpcParamArrayGetParamAt(sub_data_xml, 3); - sub_data->connection_id = connection_id->data.as_int; - sub_data->bytes_received = (size_t)bytes_received->data.as_int; - sub_data->drop_estimate = drop_estimate->data.as_int; - sub_data->connected = connected->data.as_bool; - } - } - - if (array->array_n_elem < 5) - return ret; - - XmlrpcParam *services_stats = xmlrpcParamArrayGetParamAt(array, 4); - XmlrpcParam *numRequests = xmlrpcParamArrayGetParamAt(services_stats, 0); - XmlrpcParam *bytesReceived = xmlrpcParamArrayGetParamAt(services_stats, 1); - XmlrpcParam *bytesSent = xmlrpcParamArrayGetParamAt(services_stats, 2); - - ret->stats.service_stats.num_requests = (size_t)numRequests->data.as_int; - ret->stats.service_stats.bytes_received = (size_t)bytesReceived->data.as_int; - ret->stats.service_stats.bytes_sent = (size_t)bytesSent->data.as_int; - - return ret; - -clean: - freeGetBusStatsResult(ret); - return NULL; -} - -GetBusInfoResult * fetchGetBusInfoResult(XmlrpcParamVector *response) -{ - int it; - - GetBusInfoResult *ret = (GetBusInfoResult *)calloc(1, sizeof(GetBusInfoResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetBusInfoResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetBusInfoResult(ret); - return NULL; - } - - if (array->array_n_elem < 3) - return ret; - - XmlrpcParam* businfos = xmlrpcParamArrayGetParamAt(array, 2); - ret->bus_infos = (struct BusInfo *)calloc(businfos->array_n_elem, sizeof(struct BusInfo)); - if (ret->bus_infos == NULL) - goto clean; - ret->bus_infos_count = businfos->array_n_elem; - - for (it = 0; it < businfos->array_n_elem; it++) - { - struct BusInfo *businfo = &ret->bus_infos[it]; - XmlrpcParam *businfo_xml = xmlrpcParamArrayGetParamAt(businfos, it); - - XmlrpcParam *connectionId = xmlrpcParamArrayGetParamAt(businfo_xml, 0); - XmlrpcParam *destinationId = xmlrpcParamArrayGetParamAt(businfo_xml, 1); - XmlrpcParam *direction = xmlrpcParamArrayGetParamAt(businfo_xml, 2); - XmlrpcParam *transport = xmlrpcParamArrayGetParamAt(businfo_xml, 3); - XmlrpcParam *topic = xmlrpcParamArrayGetParamAt(businfo_xml, 4); - XmlrpcParam *connected = xmlrpcParamArrayGetParamAt(businfo_xml, 5); - - businfo->topic = (char *)malloc(strlen(topic->data.as_string) + 1); - if (businfo->topic == NULL) - goto clean; - strcpy(businfo->topic, topic->data.as_string); - businfo->connectionId = connectionId->data.as_int; - businfo->destinationId = connectionId->data.as_int; - switch (connectionId->data.as_string[0]) - { - case 'i': - businfo->direction = CROS_TRANSPORT_DIRECTION_IN; - break; - case 'o': - businfo->direction = CROS_TRANSPORT_DIRECTION_OUT; - break; - case 'b': - businfo->direction = CROS_TRANSPORT_DIRECTION_BOTH; - break; - default: - goto clean; - } - businfo->transport = (CrosTransportType)connectionId->data.as_int; - businfo->connected = connectionId->data.as_int; - } - - return ret; - -clean: - freeGetBusInfoResult(ret); - return NULL; -} - -GetMasterUriResult * fetchGetMasterUriResult(XmlrpcParamVector *response) -{ - GetMasterUriResult *ret = (GetMasterUriResult *)calloc(1, sizeof(GetMasterUriResult)); - if (ret == NULL) - return ret; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetMasterUriResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetMasterUriResult(ret); - return NULL; - } - - XmlrpcParam* uri = xmlrpcParamArrayGetParamAt(array, 2); - ret->master_uri = strdup(uri->data.as_string); - if (ret->master_uri == NULL) - { - freeGetMasterUriResult(ret); - return NULL; - } - - return ret; -} - -ShutdownResult * fetchShutdownResult(XmlrpcParamVector *response) -{ - ShutdownResult *ret = (ShutdownResult *)calloc(1, sizeof(ShutdownResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchShutdownResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeShutdownResult(ret); - return NULL; - } - - XmlrpcParam *ignore = xmlrpcParamArrayGetParamAt(array, 2); - if(ignore != NULL) - ret->ignore = ignore->data.as_bool; - else - { - PRINT_ERROR ( "fetchShutdownResult() : The ROS master response does not contain the 'ignore' parameter.\n" ); - freeShutdownResult(ret); - return NULL; - } - - return ret; -} - -GetPidResult * fetchGetPidResult(XmlrpcParamVector *response) -{ - GetPidResult *ret = (GetPidResult *)calloc(1, sizeof(GetPidResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetPidResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetPidResult(ret); - return NULL; - } - - XmlrpcParam* roscore_pid_param = xmlrpcParamArrayGetParamAt(array, 2); - if(roscore_pid_param != NULL) - ret->server_process_pid = roscore_pid_param->data.as_int; - else - { - PRINT_ERROR ( "fetchGetPidResult() : The ROS master response does not contain the requested PID.\n" ); - freeGetPidResult(ret); - return NULL; - } - - return ret; -} - -GetSubscriptionsResult * fetchGetSubscriptionsResult(XmlrpcParamVector *response) -{ - int it; - - GetSubscriptionsResult *ret = (GetSubscriptionsResult *)calloc(1, sizeof(GetSubscriptionsResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetSubscriptionsResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetSubscriptionsResult(ret); - return NULL; - } - - XmlrpcParam* topics = xmlrpcParamArrayGetParamAt(array, 2); - ret->topic_list = (struct TopicTypePair *)calloc(topics->array_n_elem, sizeof(struct TopicTypePair)); - if (ret->topic_list == NULL) - goto clean; - ret->topic_count = topics->array_n_elem; - - for (it = 0; it < topics->array_n_elem; it++) - { - struct TopicTypePair *pair = &ret->topic_list[it]; - XmlrpcParam* pair_xml = xmlrpcParamArrayGetParamAt(topics, it); - XmlrpcParam* topic = xmlrpcParamArrayGetParamAt(pair_xml, 0); - pair->topic = (char *)malloc(strlen(topic->data.as_string) + 1); - if (pair->topic == NULL) - goto clean; - strcpy(pair->topic, topic->data.as_string); - - XmlrpcParam* type = xmlrpcParamArrayGetParamAt(pair_xml, 1); - pair->type = (char *)malloc(strlen(type->data.as_string) + 1); - if (pair->type == NULL) - goto clean; - strcpy(pair->type, type->data.as_string); - } - - return ret; - -clean: - freeGetSubscriptionsResult(ret); - return NULL; -} - -GetPublicationsResult * fetchGetPublicationsResult(XmlrpcParamVector *response) -{ - int it; - - GetPublicationsResult *ret = (GetPublicationsResult *)calloc(1, sizeof(GetPublicationsResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetPublicationsResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetPublicationsResult(ret); - return NULL; - } - - XmlrpcParam* topics = xmlrpcParamArrayGetParamAt(array, 2); - ret->topic_list = (struct TopicTypePair *)calloc(topics->array_n_elem, sizeof(struct TopicTypePair)); - if (ret->topic_list == NULL) - goto clean; - ret->topic_count = topics->array_n_elem; - - for (it = 0; it < topics->array_n_elem; it++) - { - struct TopicTypePair *pair = &ret->topic_list[it]; - XmlrpcParam* pair_xml = xmlrpcParamArrayGetParamAt(topics, it); - XmlrpcParam* topic = xmlrpcParamArrayGetParamAt(pair_xml, 0); - pair->topic = (char *)malloc(strlen(topic->data.as_string) + 1); - if (pair->topic == NULL) - goto clean; - strcpy(pair->topic, topic->data.as_string); - - XmlrpcParam* type = xmlrpcParamArrayGetParamAt(pair_xml, 1); - pair->type = (char *)malloc(strlen(type->data.as_string) + 1); - if (pair->type == NULL) - goto clean; - strcpy(pair->type, type->data.as_string); - } - - return ret; - -clean: - freeGetPublicationsResult(ret); - return NULL; -} - -static DeleteParamResult * fetchDeleteParamResult(XmlrpcParamVector *response) -{ - DeleteParamResult *ret = (DeleteParamResult *)calloc(1, sizeof(DeleteParamResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchDeleteParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeDeleteParamResult(ret); - return NULL; - } - - XmlrpcParam* ignore = xmlrpcParamArrayGetParamAt(array, 2); - if(ignore != NULL) - ret->ignore = ignore->data.as_bool; - else - { - PRINT_ERROR ( "fetchDeleteParamResult() : The ROS master response does not contain the 'ignore' parameter.\n" ); - freeDeleteParamResult(ret); - return NULL; - } - - return ret; -} - -static SetParamResult * fetchSetParamResult(XmlrpcParamVector *response) -{ - SetParamResult *ret = (SetParamResult *)calloc(1, sizeof(SetParamResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchSetParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeSetParamResult(ret); - return NULL; - } - - XmlrpcParam* ignore = xmlrpcParamArrayGetParamAt(array, 2); - if(ignore != NULL) - ret->ignore = ignore->data.as_bool; - else - { - PRINT_ERROR ( "fetchSetParamResult() : The ROS master response does not contain the 'ignore' parameter.\n" ); - freeSetParamResult(ret); - return NULL; - } - - return ret; -} - -static GetParamResult * fetchGetParamResult(XmlrpcParamVector *response) -{ - GetParamResult *ret = (GetParamResult *)calloc(1, sizeof(GetParamResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetParamResult(ret); - return NULL; - } - - XmlrpcParam* value = xmlrpcParamArrayGetParamAt(array, 2); - if( value != NULL ) - ret->value = xmlrpcParamClone(value); - else - { - if(ret->code != 1) // Only if ret->code is not 1, parameterValue can be ignored. - { - PRINT_ERROR ( "fetchGetParamResult() : The ROS master response does not contain the 'parameterValue' parameter.\n" ); - freeGetParamResult(ret); - return NULL; - } - } - - return ret; -} - -static SearchParamResult * fetchSearchParamResult(XmlrpcParamVector *response) -{ - SearchParamResult *ret = (SearchParamResult *)calloc(1, sizeof(SearchParamResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchSearchParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeSearchParamResult(ret); - return NULL; - } - - XmlrpcParam* found_key = xmlrpcParamArrayGetParamAt(array, 2); - if(found_key != NULL) - { - ret->found_key = strdup(found_key->data.as_string); - if(ret->found_key == NULL) - { - PRINT_ERROR ( "fetchSearchParamResult() : Error allocating memory for the 'foundKey' parameter.\n" ); - freeSearchParamResult(ret); - return NULL; - } - } - else - { - PRINT_ERROR ( "fetchSearchParamResult() : The ROS master response does not contain the 'foundKey' parameter.\n" ); - freeSearchParamResult(ret); - return NULL; - } - - return ret; -} - -static HasParamResult * fetchHasParamResult(XmlrpcParamVector *response) -{ - HasParamResult *ret = (HasParamResult *)calloc(1, sizeof(HasParamResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchHasParamResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeHasParamResult(ret); - return NULL; - } - - XmlrpcParam* has_param = xmlrpcParamArrayGetParamAt(array, 2); - if(has_param != NULL) - ret->has_param = has_param->data.as_bool; - else - { - PRINT_ERROR ( "fetchHasParamResult() : The ROS master response does not contain the 'has_param' parameter.\n" ); - freeHasParamResult(ret); - return NULL; - } - - return ret; -} - -static GetParamNamesResult * fetchGetParamNamesResult(XmlrpcParamVector *response) -{ - int it; - - GetParamNamesResult *ret = (GetParamNamesResult *)calloc(1, sizeof(GetParamNamesResult)); - if (ret == NULL) - return NULL; - - XmlrpcParam *array; - array = GetMethodResponseStatus(response, &ret->code, &ret->status); - if (array == NULL) - { - free(ret); - return NULL; - } - if(ret->code == -1 || ret->code == 0) - { - PRINT_ERROR ( "fetchGetParamNamesResult() : The ROS master returned a (%s) %i status code.\n", (ret->code==-1)?"ERROR":"FAILURE" ,ret->code); - freeGetParamNamesResult(ret); - return NULL; - } - - XmlrpcParam* param_names = xmlrpcParamArrayGetParamAt(array, 2); - ret->parameter_names = (char **)calloc(param_names->array_n_elem, sizeof(char *)); - if (ret->parameter_names== NULL) - goto clean; - - for (it = 0; it < param_names->array_n_elem; it++) - { - ret->parameter_names[it] = (char *)malloc(strlen(param_names->data.as_array[it].data.as_string) + 1); - if (ret->parameter_names[it] == NULL) - goto clean; - - strcpy(ret->parameter_names[it], param_names->data.as_array[it].data.as_string); - } - ret->parameter_count = (size_t)param_names->array_n_elem; - - return ret; - -clean: - freeGetParamNamesResult(ret); - return NULL; -} - -cRosMessage *cRosApiCreatePublisherMessage(CrosNode *node, int pubidx) -{ - cRosMessage *new_msg; - ProviderContext *pub_context; - PublisherNode *pub; - - if (pubidx < 0 || pubidx >= CN_MAX_PUBLISHED_TOPICS) - return NULL; - - pub = &node->pubs[pubidx]; - if (pub->topic_name == NULL) - return NULL; - - pub_context = pub->context; - new_msg = cRosMessageCopy(pub_context->outgoing); - - return new_msg; -} - -cRosMessage *cRosApiCreateServiceCallerRequest(CrosNode *node, int svcidx) -{ - cRosMessage *new_msg; - ServiceCallerNode *svc_caller; - ProviderContext *caller_context; - - if (svcidx < 0 || svcidx >= CN_MAX_SERVICE_CALLERS) - return NULL; - - svc_caller = &node->service_callers[svcidx]; - if (svc_caller->service_name == NULL) - return NULL; - - caller_context = svc_caller->context; - new_msg = cRosMessageCopy(caller_context->outgoing); - - return new_msg; -} - - -void freeLookupNodeResult(LookupNodeResult *result) -{ - free(result->status); - free(result->uri); - free(result); -} - -void freeGetPublishedTopicsResult(GetPublishedTopicsResult *result) -{ - size_t it; - - free(result->status); - for (it = 0; it < result->topic_count; it++) - { - free(result->topics[it].topic); - free(result->topics[it].type); - } - free(result->topics); - free(result); -} - -void freeGetTopicTypesResult(GetTopicTypesResult *result) -{ - size_t it; - - free(result->status); - for (it = 0; it < result->topic_count; it++) - { - free(result->topics[it].topic); - free(result->topics[it].type); - } - free(result->topics); - free(result); -} - -void freeGetSystemStateResult(GetSystemStateResult *result) -{ - size_t it1; - - free(result->status); - for (it1 = 0; it1 < result->sub_count; it1++) - { - size_t it2; - - free(result->publishers[it1].provider_name); - for (it2 = 0; it2 < result->publishers[it1].user_count; it2++) - free(result->publishers[it1].users[it2]); - free(result->publishers[it1].users); - } - free(result->publishers); - - for (it1 = 0; it1 < result->sub_count; it1++) - { - size_t it2; - - free(result->subscribers[it1].provider_name); - for (it2 = 0; it2 < result->subscribers[it1].user_count; it2++) - free(result->subscribers[it1].users[it2]); - free(result->subscribers[it1].users); - } - free(result->subscribers); - - for (it1 = 0; it1 < result->svc_count; it1++) - { - size_t it2; - - free(result->service_providers[it1].provider_name); - for (it2 = 0; it2 < result->service_providers[it1].user_count; it2++) - free(result->service_providers[it1].users[it2]); - free(result->service_providers[it1].users); - } - free(result->service_providers); - - free(result); -} - -void freeGetUriResult(GetUriResult *result) -{ - free(result->status); - free(result->master_uri); - free(result); -} - -void freeLookupServiceResult(LookupServiceResult *result) -{ - free(result->status); - free(result->service_result); - free(result); -} - -void freeGetBusStatsResult(GetBusStatsResult *result) -{ - size_t it1; - - free(result->status); - for (it1 = 0; it1 < result->stats.pub_stats_count; it1++) - free(result->stats.pub_stats[it1].topic_name); - free(result->stats.pub_stats); - - for (it1 = 0; it1 < result->stats.sub_stats_count; it1++) - free(result->stats.sub_stats[it1].topic_name); - free(result->stats.sub_stats); - - free(result); -} - -void freeGetBusInfoResult(GetBusInfoResult *result) -{ - size_t it; - - free(result->status); - for (it = 0; it < result->bus_infos_count; it++) - free(result->bus_infos[it].topic); - free(result->bus_infos); - free(result); -} - -void freeGetMasterUriResult(GetMasterUriResult *result) -{ - free(result->status); - free(result->master_uri); - free(result); -} - -void freeShutdownResult(ShutdownResult *result) -{ - free(result->status); - free(result); -} - -void freeGetPidResult(GetPidResult *result) -{ - free(result->status); - free(result); -} - -void freeGetSubscriptionsResult(GetSubscriptionsResult *result) -{ - size_t it; - - free(result->status); - for (it = 0; it < result->topic_count; it++) - { - free(result->topic_list[it].topic); - free(result->topic_list[it].type); - } - free(result->topic_list); - free(result); -} - -void freeGetPublicationsResult(GetPublicationsResult *result) -{ - size_t it; - - free(result->status); - for (it = 0; it < result->topic_count; it++) - { - free(result->topic_list[it].topic); - free(result->topic_list[it].type); - } - free(result->topic_list); - free(result); -} - -static void freeDeleteParamResult(DeleteParamResult *result) -{ - free(result->status); - free(result); -} - -static void freeSetParamResult(SetParamResult *result) -{ - free(result->status); - free(result); -} - -static void freeGetParamResult(GetParamResult *result) -{ - free(result->status); - xmlrpcParamFree(result->value); - free(result); -} - -static void freeSearchParamResult(SearchParamResult *result) -{ - free(result->status); - free(result->found_key); - free(result); -} - -static void freeHasParamResult(HasParamResult *result) -{ - free(result->status); - free(result); -} - -static void freeGetParamNamesResult(GetParamNamesResult *result) -{ - size_t it; - - free(result->status); - for (it = 0; it < result->parameter_count; it++) - free(result->parameter_names[it]); - free(result->parameter_names); - free(result); -} - diff --git a/src/hal/components/cros/src/cros_api_call.c b/src/hal/components/cros/src/cros_api_call.c deleted file mode 100644 index 90b04cb9..00000000 --- a/src/hal/components/cros/src/cros_api_call.c +++ /dev/null @@ -1,112 +0,0 @@ -#include -#include - -#include "cros_api_call.h" -#include "cros_defs.h" - -RosApiCall * newRosApiCall(void) -{ - RosApiCall *ret = (RosApiCall *)malloc(sizeof(RosApiCall)); - ret->id = -1; - ret->user_call = 0; - ret->provider_idx = -1; - ret->host = NULL; - ret->port = -1; - xmlrpcParamVectorInit(&ret->params); - ret->result_callback = NULL; - ret->context_data = NULL; - ret->fetch_result_callback = NULL; - ret->free_result_callback = NULL; - return ret; -} - -void freeRosApiCall(RosApiCall *call) -{ - xmlrpcParamVectorRelease(&call->params); - if (call->host != NULL) - free(call->host); - free(call); -} - -void initApiCallQueue(ApiCallQueue *queue) -{ - queue->tail = NULL; - queue->head = NULL; - queue->count = 0; -} - -RosApiCall * peekApiCallQueue(ApiCallQueue *queue) -{ - if (queue->head == NULL) - return NULL; - - return queue->head->call; -} - -int enqueueApiCall(ApiCallQueue *queue, RosApiCall* apiCall) -{ - ApiCallNode *node = (ApiCallNode *)malloc(sizeof(ApiCallNode)); - - if(node == NULL) - { - PRINT_ERROR("enqueueApiCall() : Can't enqueue call\n"); - return -1; - } - - node->call = apiCall; - node->next = NULL; - - if(queue->head == NULL) - { - queue->head = node; - queue->tail = node; - } - else - { - queue->tail->next = node; - queue->tail = node; - } - - queue->count++; - - return 0; -} - -RosApiCall * dequeueApiCall(ApiCallQueue *queue) -{ - ApiCallNode* head = queue->head; - queue->head = head->next; - if(queue->head == NULL) - queue->tail = queue->head; - - RosApiCall *call = head->call; - free(head); - - queue->count--; - - return call; -} - -void releaseApiCallQueue(ApiCallQueue *queue) -{ - ApiCallNode *current = queue->head; - while(current != NULL) - { - freeRosApiCall(current->call); - ApiCallNode *next = current->next; - free(current); - current = next; - } - - initApiCallQueue(queue); -} - -size_t getQueueCount(ApiCallQueue *queue) -{ - return queue->count; -} - -int isQueueEmpty(ApiCallQueue *queue) -{ - return queue->count == 0; -} diff --git a/src/hal/components/cros/src/cros_clock.c b/src/hal/components/cros/src/cros_clock.c deleted file mode 100644 index 68260c26..00000000 --- a/src/hal/components/cros/src/cros_clock.c +++ /dev/null @@ -1,122 +0,0 @@ -#include - -#ifdef _WIN32 -# define WIN32_LEAN_AND_MEAN -# include -# include -#else -# include -# include -#endif - -#include "cros_defs.h" -#include "cros_clock.h" - -struct timeval cRosClockGetTimeSecUsec( void ) -{ - struct timeval time_since_epoch; - int ret_val; - - PRINT_VVDEBUG ( "cRosClockGetTimeSecUsec()\n" ); - -#ifdef _WIN32 - const uint64_t epoch_filetime = 116444736000000000ULL; // FILETIME on Jan 1 1970 00:00:00 - FILETIME cur_filetime; - SYSTEMTIME cur_system_time; - - GetSystemTime(&cur_system_time); - - if(SystemTimeToFileTime(&cur_system_time, &cur_filetime) != 0) // If Success: - { - ULARGE_INTEGER cur_filetime_large; - - // It is recomended that the calculations are done using a ULARGE_INTEGER - cur_filetime_large.LowPart = cur_filetime.dwLowDateTime; - cur_filetime_large.HighPart = cur_filetime.dwHighDateTime; - - time_since_epoch.tv_sec = (long) ((cur_filetime_large.QuadPart - epoch_filetime) / 10000000L); - time_since_epoch.tv_usec = (long) (cur_system_time.wMilliseconds * 1000); - ret_val = 0; - } - else - ret_val = -1; -#else - ret_val = gettimeofday(&time_since_epoch, NULL); -#endif - - if(ret_val == -1) // Failure obtaning the time: set default values: - { - time_since_epoch.tv_sec = 0; - time_since_epoch.tv_usec = 0; - } - - return(time_since_epoch); -} - -uint64_t cRosClockGetTimeMs( void ) -{ - uint64_t ms_since_epoch; - struct timeval tv; - - PRINT_VVDEBUG ( "cRosClockGetTimeMs()\n" ); - - tv = cRosClockGetTimeSecUsec(); - ms_since_epoch = (uint64_t)tv.tv_sec*1000 + (uint64_t)tv.tv_usec/1000; - - return(ms_since_epoch); -} - -struct timeval cRosClockGetTimeVal( uint64_t msec ) -{ - PRINT_VVDEBUG ( "cRosClockGetTimeVal() msec: %lu\n", msec ); - struct timeval tv; - if (msec > ( LONG_MAX * 1000ULL )) - { - // Given timespan would overflow timeval - tv.tv_sec = LONG_MAX; - tv.tv_usec = 999999L; - } - else - { - tv.tv_sec = (long)(msec / 1000); - tv.tv_usec = (long)(msec - tv.tv_sec * 1000ULL ) * 1000; - } - - return tv; -} - -int64_t cRosClockGetTimeStamp(void) -{ - int64_t time_stamp_ret; -#ifdef _WIN32 - LARGE_INTEGER time_stamp_init; - QueryPerformanceCounter(&time_stamp_init); - time_stamp_ret = time_stamp_init.QuadPart; -#else - struct timespec time_stamp_init; - int fn_ret_val; - fn_ret_val = clock_gettime(CLOCK_MONOTONIC, &time_stamp_init); - if (fn_ret_val == 0) // clock_gettime success - time_stamp_ret = time_stamp_init.tv_sec * 1000000000LL + time_stamp_init.tv_nsec; - else - time_stamp_ret = 0; -#endif - return(time_stamp_ret); -} - -double cRosClockTimeStampToUSec(int64_t time_stamp) -{ - double time_sec; -#ifdef _WIN32 - LARGE_INTEGER counter_freq; - BOOL fn_ret_val; - fn_ret_val = QueryPerformanceFrequency(&counter_freq); - if (fn_ret_val) // QueryPerformanceFrequency success - time_sec = (time_stamp * 1.0e6L) / counter_freq.QuadPart; - else - time_sec = 0; -#else - time_sec = time_stamp / 1.0e3; -#endif - return(time_sec); -} diff --git a/src/hal/components/cros/src/cros_err_codes.c b/src/hal/components/cros/src/cros_err_codes.c deleted file mode 100644 index fc768d9c..00000000 --- a/src/hal/components/cros/src/cros_err_codes.c +++ /dev/null @@ -1,163 +0,0 @@ -#include -#include -#include -#include "cros_node.h" -#include "cros_err_codes.h" - -// Populate the error message list global variable using the messages defined before (ERROR_CODE_LIST_DEF) but -// using the format defined by MSG_COD_ELEM for them. -// This an array of structures where the error messages are searched for. -#define MSG_COD_ELEM(code,msg) {code,msg}, -struct cRosErrCodeListElem CRosErrCodeList[]= -{ - ERROR_CODE_LIST_DEF - {LAST_ERR_LIST_CODE, NULL} // Last value of the list not used -}; -#undef MSG_COD_ELEM - -const char *cRosGetErrCodeStr(cRosErrCode err_code) -{ - int msg_ind; - const char *ret_msg; - - // Search for a message in the error message list according to the specified message code - ret_msg=NULL; - for(msg_ind=0;CRosErrCodeList[msg_ind].code!=LAST_ERR_LIST_CODE && ret_msg==NULL;msg_ind++) - { - if(CRosErrCodeList[msg_ind].code == err_code) // We have found a message with the specified code - ret_msg = CRosErrCodeList[msg_ind].msg; // Use the found message as return value - } - return ret_msg; -} - -cRosErrCodePack cRosAddErrCode(cRosErrCodePack prev_err_pack, cRosErrCode err_code) -{ - cRosErrCodePack new_err_pack; - - if(err_code == CROS_NO_ERR) // If no new error: - new_err_pack = prev_err_pack; // do not add any error code to the pack - else // if we have some new error code - new_err_pack = (prev_err_pack << 8) | err_code; // add more info to the error pack: add the new code - - return new_err_pack; -} - -cRosErrCodePack cRosAddErrCodeIfErr(cRosErrCodePack prev_err_pack, cRosErrCode err_code) -{ - cRosErrCodePack new_err_pack; - - if(prev_err_pack == CROS_SUCCESS_ERR_PACK) // If no error stored in the error pack - new_err_pack = prev_err_pack; // do not add any error code to the pack - else // if we had some error code in the pack - new_err_pack = cRosAddErrCode(prev_err_pack, err_code); // add more info to the error pack: add the new code - - return new_err_pack; -} - -cRosErrCodePack cRosRemoveLastErrCode(cRosErrCodePack prev_err_pack) -{ - cRosErrCodePack new_err_pack; - - new_err_pack = prev_err_pack >> 8; // Remove the last error code from the error pack (8 least-significant bits) - - return new_err_pack; -} - -cRosErrCode cRosGetLastErrCode(cRosErrCodePack err_pack) -{ - cRosErrCode last_err_code; - - last_err_code = err_pack & 0xFFU; // Get the last error code from the error pack (8 least-significant bits) - - return last_err_code; -} - -cRosErrCodePack cRosAddErrCodePackIfErr(cRosErrCodePack prev_err_pack_0, cRosErrCodePack prev_err_pack_1) -{ - cRosErrCodePack new_err_code_pack; - cRosErrCode curr_err_cod; - - // First, add the error codes from prev_err_pack_0 to the output - new_err_code_pack = prev_err_pack_0; - // Then, add the error codes from prev_err_pack_1 to the output: - // Iterate throughout all the errors contained in prev_err_pack_1 - while((curr_err_cod=cRosGetLastErrCode(prev_err_pack_1)) != CROS_NO_ERR) - { - new_err_code_pack = cRosAddErrCode(new_err_code_pack, curr_err_cod); - prev_err_pack_1 = cRosRemoveLastErrCode(prev_err_pack_1); // Pass to the next error code - } - - return new_err_code_pack; -} - -int cRosPrintErrCodePack(cRosErrCodePack err_cod_pack, const char *fmt_str, ...) -{ - int n_prn_chars; - const char *msg_str; - cRosErrCode curr_err_cod; - FILE *msg_out = cRosOutStreamGet(); - - n_prn_chars=0; - - if(fmt_str != NULL) - { - va_list arg_list; - - va_start(arg_list, fmt_str); - n_prn_chars+=vfprintf(msg_out, fmt_str, arg_list); - va_end(arg_list); - } - - // Iterate throughout all the errors contained in the pack (up to 4) - while((curr_err_cod=cRosGetLastErrCode(err_cod_pack)) != CROS_NO_ERR) - { - // Print error info included in err - msg_str = cRosGetErrCodeStr(curr_err_cod); - if(msg_str != NULL) // The error code has been found in the list - n_prn_chars+=fprintf(msg_out, ". Err %u: %s", curr_err_cod, msg_str); - else - n_prn_chars+=fprintf(msg_out, ". Error code %u (The description string for this error code has not been found).", curr_err_cod); - - err_cod_pack = cRosRemoveLastErrCode(err_cod_pack); // Pass to the next error code - } - n_prn_chars+=fprintf(msg_out, "\n"); - return(n_prn_chars); -} - -int cRosErrCodePackStr(char *out_str_buf, size_t out_str_buf_len, cRosErrCodePack err_cod_pack, const char *fmt_str, ...) -{ - int n_prn_chars; - const char *msg_str; - cRosErrCode curr_err_cod; - - n_prn_chars=0; - if(out_str_buf_len == 0) - return(n_prn_chars); - - out_str_buf[0]='\0'; - - if(fmt_str != NULL) - { - va_list arg_list; - - va_start(arg_list, fmt_str); - n_prn_chars+=vsnprintf(out_str_buf+strlen(out_str_buf), out_str_buf_len-strlen(out_str_buf), fmt_str, arg_list); - va_end(arg_list); - } - - // Iterate throughout all the errors contained in the pack (up to 4) - while((curr_err_cod=cRosGetLastErrCode(err_cod_pack)) != CROS_NO_ERR) - { - // Print error info included in err - msg_str = cRosGetErrCodeStr(curr_err_cod); - if(msg_str != NULL) // The error code has been found in the list - n_prn_chars+=snprintf(out_str_buf+strlen(out_str_buf), out_str_buf_len-strlen(out_str_buf), ". Err %u: %s", curr_err_cod, msg_str); - else - n_prn_chars+=snprintf(out_str_buf+strlen(out_str_buf), out_str_buf_len-strlen(out_str_buf), ". Error code %u (The description string for this error code has not been found).", curr_err_cod); - - err_cod_pack = cRosRemoveLastErrCode(err_cod_pack); // Pass to the next error code - } - - return(n_prn_chars); -} - diff --git a/src/hal/components/cros/src/cros_gentools.c b/src/hal/components/cros/src/cros_gentools.c deleted file mode 100644 index f58d2be5..00000000 --- a/src/hal/components/cros/src/cros_gentools.c +++ /dev/null @@ -1,93 +0,0 @@ -#include -#include -#include - -#include "cros_gentools.h" -#include "cros_message.h" -#include "cros_message_internal.h" -#include "cros_service.h" -#include "cros_service_internal.h" -#include "md5.h" - -char* cRosGentoolsMD5(char* filename) -{ - char* filename_tokenized = (char *)malloc(strlen(filename)+1); - strcpy(filename_tokenized, filename); - strtok(filename_tokenized, "."); - char* file_ext = strtok(NULL,"."); - - if(strcmp(file_ext,FILEEXT_MSG) == 0) - { - char *md5sum; - cRosMessage *msg=NULL; - cRosMessageNewBuild(NULL, filename, &msg); - if(msg != NULL) - { - md5sum = (char *)calloc(strlen(msg->md5sum)+1,sizeof(char)); - if(md5sum != NULL) - strcpy(md5sum,msg->md5sum); - cRosMessageFree(msg); - } - else - md5sum=NULL; - return md5sum; - } - - if(strcmp(file_ext,FILEEXT_SRV) == 0) - { - cRosService srv; - cRosServiceInit(&srv); - cRosServiceBuild(&srv,filename); - char *md5sum = (char *)calloc(strlen(srv.md5sum)+1,sizeof(char)); - if(md5sum != NULL) - strcpy(md5sum,srv.md5sum); - cRosServiceRelease(&srv); - return md5sum; - } - - free(filename_tokenized); - return NULL; -} - -int cRosGentoolsSHA1(char* filename) -{ - //FILE * fp = fopen(filename, "r"); - //fclose(fp); - return 0; -} - -int cRosGentoolsFulltext(char* filename) -{ - char* full_text = NULL;; - - char* filename_tokenized = (char *)malloc(strlen(filename)+1); - strcpy(filename_tokenized, filename); - strtok(filename_tokenized, "."); - char* file_ext = strtok(NULL,"."); - - if(strcmp(file_ext,FILEEXT_MSG) == 0) - { - msgDep messageDependencies; - cRosMessageDef msg; - initCrosMsg(&msg); - initCrosDep(&messageDependencies); - getFileDependenciesMsg(filename, &msg, &messageDependencies); - full_text = computeFullTextMsg(&msg, &messageDependencies); - } - - if(strcmp(file_ext,FILEEXT_SRV) == 0) - { - msgDep messageDependencies; - cRosSrvDef srv; - initCrosSrv(&srv); - initCrosDep(&messageDependencies); - getFileDependenciesSrv(filename, &srv, &messageDependencies); - full_text = computeFullTextSrv(&srv, &messageDependencies); - } - - printf("%s",full_text); - - free(filename_tokenized); - //free(full_text); - return 1; -} diff --git a/src/hal/components/cros/src/cros_log.c b/src/hal/components/cros/src/cros_log.c deleted file mode 100644 index e1e3b8c9..00000000 --- a/src/hal/components/cros/src/cros_log.c +++ /dev/null @@ -1,239 +0,0 @@ -#include -#include -#include -#include - -#ifdef _WIN32 -# define strcasecmp _stricmp // This is the POSIX verion of strnicmp -#endif - -#include "cros_log.h" -#include "cros_defs.h" -#include "cros_node.h" -#include "cros_clock.h" -#include "cros_message.h" -#include "cros_api.h" - -CrosLog *cRosLogNew(void) -{ - CrosLog *ret = (CrosLog *)malloc(sizeof(CrosLog)); - ret->file = NULL; - ret->level = CROS_LOGLEVEL_INFO; - ret->function = NULL; - ret->msg = NULL; - ret->name = NULL; - ret->pubs = NULL; - ret->n_pubs = 0; - return ret; -} - -void cRosLogFree(CrosLog *log) -{ - free(log->file); - free(log->function); - free(log->msg); - free(log->name); - size_t i; - for(i = 0; i < log->n_pubs; i++) - { - free(log->pubs[i]); - } - free(log->pubs); - free(log); -} - -int stringToLogLevel(const char* level_str, CrosLogLevel *level_num) -{ - int ret; - ret = 0; // Default return value: success - if(strcasecmp("Info",level_str) == 0) - { - *level_num = CROS_LOGLEVEL_INFO; - } - else if(strcasecmp("Debug",level_str) == 0) - { - *level_num = CROS_LOGLEVEL_DEBUG; - } - else if(strcasecmp("Warn",level_str) == 0) - { - *level_num = CROS_LOGLEVEL_WARN; - } - else if(strcasecmp("Error",level_str) == 0) - { - *level_num = CROS_LOGLEVEL_ERROR; - } - else if(strcasecmp("Fatal",level_str) == 0) - { - *level_num = CROS_LOGLEVEL_FATAL; - } - else - { - ret = -1; // Unknown input level string: return error - } - return ret; -} - -const char *LogLevelToString(CrosLogLevel log_level) -{ - char *ret; - - switch(log_level) - { - case CROS_LOGLEVEL_INFO: - ret = "INFO"; - break; - case CROS_LOGLEVEL_DEBUG: - ret = "DEBUG"; - break; - case CROS_LOGLEVEL_WARN: - ret = "WARN"; - break; - case CROS_LOGLEVEL_ERROR: - ret = "ERROR"; - break; - case CROS_LOGLEVEL_FATAL: - ret = "FATAL"; - break; - default: - ret = "UNKNOWN"; - } - return ret; -} - -cRosMessage *cRosLogToMessage(CrosNode* node, CrosLog* log) -{ - cRosMessage *message; - size_t pub_ind; - - message = cRosApiCreatePublisherMessage(node, node->rosout_pub_idx); - if(message != NULL) - { - cRosMessageField* header_field = cRosMessageGetField(message, "header"); - cRosMessage* header_msg = header_field->data.as_msg; - cRosMessageField* seq_id = cRosMessageGetField(header_msg, "seq"); - seq_id->data.as_uint32 = node->log_last_id++; - cRosMessageField* time_field = cRosMessageGetField(header_msg, "stamp"); - cRosMessage* time_msg = time_field->data.as_msg; - cRosMessageField* time_secs = cRosMessageGetField(time_msg, "secs"); - time_secs->data.as_uint32 = log->secs; - cRosMessageField* time_nsecs = cRosMessageGetField(time_msg, "nsecs"); - time_nsecs->data.as_uint32 = log->nsecs; - cRosMessageSetFieldValueString(cRosMessageGetField(header_msg, "frame_id"), "0"); - - - cRosMessageField* level = cRosMessageGetField(message, "level"); - level->data.as_uint8 = log->level; - - cRosMessageField* name = cRosMessageGetField(message, "name"); // name of the node - cRosMessageSetFieldValueString(name, node->name); - - cRosMessageField* msg = cRosMessageGetField(message, "msg"); // message - cRosMessageSetFieldValueString(msg, log->msg); - - cRosMessageField* file = cRosMessageGetField(message, "file"); // file the message came from - cRosMessageSetFieldValueString(file, log->file); - - cRosMessageField* function = cRosMessageGetField(message, "function"); // function the message came from - cRosMessageSetFieldValueString(function, log->function); - - cRosMessageField* line = cRosMessageGetField(message, "line"); // line the message came from - line->data.as_uint32 = log->line; - - cRosMessageField* topics = cRosMessageGetField(message, "topics"); // topic names that the node publishes - - cRosMessageFieldArrayClear(topics); - for(pub_ind = 0; pub_ind < log->n_pubs; pub_ind++) - cRosMessageFieldArrayPushBackString(topics, log->pubs[pub_ind]); - } - return(message); -} - -void cRosLogPrint(CrosNode* node, - CrosLogLevel level, // debug level - const char* file, // file the message came from - const char* function, // function the message came from - uint32_t line, - const char* msg_fmt_str, ...) // message -{ - // Only print the message localy if its priority level is equal or higher than the current log - // priority set for this ROS node or we cannot find out the node's prioroty level - if(node == NULL || level >= node->log_level) - { - struct timeval wall_time; - int msg_str_size; - va_list msg_str_args; - - wall_time = cRosClockGetTimeSecUsec(); - - fprintf(cRosOutStreamGet(), "[%s] [%d,%ld] ", LogLevelToString(level), (int)wall_time.tv_sec, (long)wall_time.tv_usec*1000); - va_start(msg_str_args, msg_fmt_str); - msg_str_size = vfprintf(cRosOutStreamGet(), msg_fmt_str, msg_str_args); - va_end(msg_str_args); - - // Only print the message in rosout if its priority level is equal or higher than the node's current log priority - if(node != NULL) - { - int pub_ind; - - CrosLog* log = cRosLogNew(); - - log->secs = wall_time.tv_sec; - log->nsecs = (uint32_t)wall_time.tv_usec*1000; - - log->level = level; - - log->file = strdup(file); // We need to make a string memory copy since it is freed later in the cRosLogFree() call - - log->function = strdup(function); - - log->line = line; - - log->n_pubs = node->n_pubs; - log->pubs = (char **)calloc(log->n_pubs,sizeof(char*)); - - for(pub_ind = 0; pub_ind < node->n_pubs; pub_ind++) - { - if(node->pubs[pub_ind].topic_name != NULL) - log->pubs[pub_ind] = strdup(node->pubs[pub_ind].topic_name); - else - log->pubs[pub_ind] = NULL; - } - - log->msg = (char *)malloc((msg_str_size + 1)*sizeof(char)); - - // We need to use the argument list twice (the 1st time to call vfprintf and the 2nd time to call vsprintf), so we start the arg list again here - va_start(msg_str_args, msg_fmt_str); - vsprintf(log->msg, msg_fmt_str, msg_str_args); - va_end(msg_str_args); - - cRosMessage *topic_msg; - topic_msg = cRosLogToMessage(node, log); - if(topic_msg != NULL) - { - cRosErrCodePack err_cod; - int rosout_pub_idx = node->rosout_pub_idx; - - //err_cod = cRosNodeSendTopicMsg(node, rosout_pub_idx, topic_msg, 0); - err_cod = cRosNodeQueueTopicMsg(node, rosout_pub_idx, topic_msg); - if (err_cod != CROS_SUCCESS_ERR_PACK) - { - // Check if the rossout publisher has any TCP process associated, that is, check if a node is subscribed to this topic - int srv_proc_ind, subs_node; - subs_node = 0; - for(srv_proc_ind=0;srv_proc_indtcpros_server_proc[srv_proc_ind].topic_idx == rosout_pub_idx) - subs_node = 1; // there is a subscriber - // Only print the error message if there is a subscriber node, that is, if there is a node that should receive the rosout message - if(subs_node == 1) - cRosPrintErrCodePack(err_cod, "cRosLogPrint() : A message of /rosout topic could not be sent"); - } - cRosMessageFree(topic_msg); - } - else - { - PRINT_ERROR ( "cRosLogPrint() : A message of /rosout topic could not be created to be sent.\n" ); - } - cRosLogFree(log); - } - } -} diff --git a/src/hal/components/cros/src/cros_message.c b/src/hal/components/cros/src/cros_message.c deleted file mode 100644 index 00c18b6b..00000000 --- a/src/hal/components/cros/src/cros_message.c +++ /dev/null @@ -1,3280 +0,0 @@ -#include -#include -#include - -#include "cros_message.h" -#include "cros_message_internal.h" -#include "cros_defs.h" -#include "md5.h" - -#ifdef _WIN32 -# define SPLIT_REGEX "\\." -# define DIR_SEPARATOR_CHAR '\\' -# define DIR_SEPARATOR_STR "\\" -// strtok_s is the Windows implementation of strtok_r -# define strtok_r strtok_s -#else -# define SPLIT_REGEX "/." -# define DIR_SEPARATOR_CHAR '/' -# define DIR_SEPARATOR_STR "/" -#endif - - -static void *arrayFieldValueAt(cRosMessageField *field, int position, size_t element_size); -static const char *getMessageTypeDeclarationConst(msgConst *msgConst); -static const char *getMessageTypeDeclarationField(msgFieldDef *fieldDef); - -char *base_msg_type(const char* type) -{ - // """ - // Compute the base data type, e.g. for arrays, get the underlying array item type - // @param type_: ROS msg type (e.g. 'std_msgs/String') - // @type type_: str - // @return: base type - // @rtype: str - // """ - char* base = NULL; - int char_count = 0; - const char* iterator = type; - - while( *iterator != '\0' && *iterator != '[') - { - char_count++; - iterator++; - } - - base = (char *)malloc(char_count + 1); - memcpy(base, type, char_count); - base[char_count] = '\0'; - - return base; -} - -int is_valid_msg_type(char* type_statement) -{ - //@return: True if the name is a syntatically legal message type name - // @rtype: bool - //if not x or len(x) != len(x.strip()): - // return False - char* iterator = type_statement; - while(*iterator != '\0') - { - if(*iterator == ' ') - { - return 0; - } - iterator ++; - } - -// base = base_msg_type(x) - char* base = base_msg_type(type_statement); - // if not roslib.names.is_legal_resource_name(base): - // return False - int slash_found = 0; - char* base_itr = base; - - while(*base_itr != '\0') - { - if((*base_itr == ']') || - (*base_itr == DIR_SEPARATOR_CHAR && slash_found)) - { - free(base); - return 0; - } - if(*base_itr == DIR_SEPARATOR_CHAR) - slash_found = 1; - base_itr++; - } - - // TODO Check that the array index is a number - - free(base); - return 1; -} - -int is_array_type(char* type_statement, int* size) -{ - //@return: True if the name is a syntactically legal message type name - // @rtype: bool - //if not x or len(x) != len(x.strip()): - // return False - char* type_it = type_statement; - char* array_size; - char* num_start = NULL; - int count = 0; - - while(*type_it != '\0' && *type_it != ']' && *type_it!= ' ') - { - if(num_start != NULL) - count++; - - if(*type_it == '[') - num_start = type_it; - - type_it++; - } - - if(num_start == NULL) - return 0; - - array_size = (char *)calloc(count + 1, sizeof(char)); - memcpy(array_size, num_start + 1, count); - char *endptr; - long int res = strtol(array_size, &endptr, 10); - if (endptr == array_size) - *size = -1; - else - *size = (int)res; - free(array_size); - - return 1; -} - -int containsDep(msgDep* iterator, char* depName) -{ - char* dep = (char *)malloc(strlen(depName)+1); - memcpy(dep,depName, strlen(depName)+1); - char* pack = dep; - char* name = dep; - while(*name != DIR_SEPARATOR_CHAR) name++; - *(name++) = '\0'; - - int found = 0; - //rollback - while((iterator)->prev != NULL) iterator = iterator->prev; - while(iterator->next != NULL && iterator->msg != NULL) - { - if(strcmp(iterator->msg->package, pack) == 0 && - strcmp(iterator->msg->name, name) == 0) - { - found = 1; - break; - } - iterator = iterator->next; - } - - free(dep); - return found; -} - -//Add the list of message types that spec depends on to depends. -cRosErrCodePack getDependenciesMsg(cRosMessageDef* msg, msgDep* msgDeps) -{ - cRosErrCodePack ret_err = CROS_SUCCESS_ERR_PACK; // Default return value - - //move on until you reach the head - while(msgDeps->next != NULL) msgDeps = msgDeps->next; - msgDep* currentDep = msgDeps ; - - msgFieldDef* fields = msg->first_field; - - while(fields->next != NULL && ret_err == CROS_SUCCESS_ERR_PACK) - { - msgFieldDef* currentField = fields; - - if(!isBuiltinMessageType(currentField->type)) - { - char *base_type = base_msg_type(currentField->type_s); - if(containsDep(msgDeps, base_type)) - { - fields = fields->next; - free(base_type); - continue; - } - - currentDep->msg = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); - // special mapping for header - if(currentField->type == CROS_STD_MSGS_HEADER) - { - //have to re-names Header - currentDep->msg->package = (char *)malloc(strlen(HEADER_DEFAULT_PACK) + 1); - currentDep->msg->package[0] = '\0'; - strcpy(currentDep->msg->package,HEADER_DEFAULT_PACK); - currentDep->msg->name = (char *)malloc(strlen(HEADER_DEFAULT_NAME) + 1); - currentDep->msg->name[0] = '\0'; - strcpy(currentDep->msg->name,HEADER_DEFAULT_NAME); - - currentDep->msg->plain_text = (char *)malloc(strlen(HEADER_DEFAULT_TYPEDEF) + 1); - memcpy(currentDep->msg->plain_text,"\0",1); - strcpy(currentDep->msg->plain_text, HEADER_DEFAULT_TYPEDEF); - msgDep *next = (msgDep *)malloc(sizeof(msgDep)); - next->msg = NULL; - next->prev = currentDep; - currentDep->next = next; - currentDep = next; - } - else - { - - if(strstr(base_type,"/") == NULL) - { - char* trail = msg->package; - currentDep->msg->name = (char *)malloc(strlen(trail) + strlen(base_type) + 1 + 1); - memcpy(currentDep->msg->name, trail, strlen(trail) + 1); - strcat(currentDep->msg->name, "/"); - strcat(currentDep->msg->name, base_type); - } - else - { - - char* dep = (char *)malloc(strlen(base_type)+1); - memcpy(dep,base_type, strlen(base_type)+1); - char* pack = dep; - char* name = dep; - while(*name != DIR_SEPARATOR_CHAR) name++; - *(name++) = '\0'; - currentDep->msg->name = name; - currentDep->msg->package = pack; - } - - currentDep->msg->fields = (msgFieldDef *)calloc(1, sizeof(msgFieldDef)); - initFieldDef(currentDep->msg->fields); - currentDep->msg->first_field = currentDep->msg->fields; - currentDep->msg->constants = (msgConst *)malloc(sizeof(msgConst)); - currentDep->msg->first_const = currentDep->msg->constants; - - char* path = (char *)malloc(strlen(msg->root_dir) + 1 + - strlen(currentDep->msg->package) + 1 + - strlen(currentDep->msg->name) + 4 + 1); - memcpy(path, msg->root_dir, strlen(msg->root_dir) + 1); - strcat(path, DIR_SEPARATOR_STR); - strcat(path, currentDep->msg->package); - strcat(path, DIR_SEPARATOR_STR); - strcat(path,currentDep->msg->name); - strcat(path,".msg"); - - ret_err = loadFromFileMsg(path,currentDep->msg); - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - msgDep *next = (msgDep *)malloc(sizeof(msgDep)); - next->msg = NULL; - next->prev = currentDep; - currentDep->next = next; - currentDep = next; - ret_err = getDependenciesMsg(currentDep->prev->msg, currentDep); - } - } - - free(base_type); - } - fields = fields->next; - } - return ret_err; -} - -// Compute dependencies of the specified message file -cRosErrCodePack getFileDependenciesMsg(char* filename, cRosMessageDef* msg, msgDep* deps) -{ - cRosErrCodePack ret_err; - ret_err = loadFromFileMsg(filename, msg); - if(ret_err == CROS_SUCCESS_ERR_PACK) - ret_err = getDependenciesMsg(msg,deps); - return ret_err; -} - -// Compute full text of message, including text of embedded -// types. The text of the main msg is listed first. Embedded -// msg files are denoted first by an 80-character '=' separator, -// followed by a type declaration line,'MSG: pkg/type', followed by -// the text of the embedded type. -char *computeFullTextMsg(cRosMessageDef* msg, msgDep* deps) -{ - char* full_text = NULL; - char* msg_tag = "MSG: "; - int full_size = 0; - char separator[81]; separator[80] = '\0'; - int i; - for(i = 0; i < 80; i++) - { - separator[i] = '='; - } - - full_size += strlen(msg->plain_text); - - while(deps->next != NULL) - { - full_size = strlen(deps->msg->plain_text) + strlen(separator) + strlen(msg_tag) + 3/*New lines*/; - deps = deps->next; - } - - //rollback - while(deps->prev != NULL) deps = deps->prev; - full_text = (char *)malloc(full_size + 1); - memcpy(full_text,msg->plain_text,strlen(msg->plain_text) + 1); - while(deps->next != NULL) - { - strcat(full_text, "\n"); - strcat(full_text, separator); - strcat(full_text, "\n"); - strcat(full_text, msg_tag); - strcat(full_text, deps->msg->package); - strcat(full_text, "/"); - strcat(full_text, deps->msg->name); - strcat(full_text, "\n"); - strcat(full_text, deps->msg->plain_text); - deps = deps->next; - } - return full_text; -} - -cRosErrCodePack initCrosMsg(cRosMessageDef* msg) -{ - cRosErrCodePack ret_err; - if(msg != NULL) - { - msg->constants = (msgConst *)malloc(sizeof(msgConst)); - msg->fields = (msgFieldDef *)calloc(1, sizeof(msgFieldDef)); - if(msg->constants != NULL && msg->fields != NULL) - { - initMsgConst(msg->constants); - msg->first_const = msg->constants; - initFieldDef(msg->fields); - msg->first_field = msg->fields; - msg->name = NULL; - msg->package = NULL; - msg->plain_text = NULL; - msg->root_dir = NULL; - ret_err = CROS_SUCCESS_ERR_PACK; - } - else - { - free(msg->constants); - free(msg->fields); - ret_err = CROS_MEM_ALLOC_ERR; - } - } - else - ret_err = CROS_BAD_PARAM_ERR; - return ret_err; -} - -void initMsgConst(msgConst *msg) -{ - msg->type = CROS_CUSTOM_TYPE; - msg->type_s = NULL; - msg->name = NULL; - msg->value = NULL; - msg->prev = NULL; - msg->next = NULL; -} - -void initCrosDep(msgDep* dep) -{ - dep->msg = NULL; - dep->prev = NULL; - dep->next = NULL; -} - -void initFieldDef(msgFieldDef* field) -{ - field->array_size = -1; - field->is_array = 0; - field->name = NULL; - field->type = CROS_CUSTOM_TYPE; - field->type_s = NULL; - field->next = NULL; - field->prev = NULL; - field->child_msg_def = NULL; -} - -unsigned char *getMD5Msg(cRosMessageDef* msg) -{ - DynString buffer; - cRosErrCodePack ret_err; - unsigned char *result; - - result = (unsigned char *)malloc(16); - if(result == NULL) - return(NULL); - - dynStringInit(&buffer); - msgConst* const_it = msg->first_const; - - while(const_it->next != NULL) - { - const char *type_decl = getMessageTypeDeclarationConst(const_it); - dynStringPushBackStr(&buffer,type_decl); - dynStringPushBackStr(&buffer," "); - dynStringPushBackStr(&buffer,const_it->name); - dynStringPushBackStr(&buffer,"="); - dynStringPushBackStr(&buffer,const_it->value); - dynStringPushBackStr(&buffer,"\n"); - const_it = const_it->next; - } - - msgFieldDef* fields_it = msg->first_field; - - ret_err = CROS_SUCCESS_ERR_PACK; - while(ret_err == CROS_SUCCESS_ERR_PACK && fields_it->next != NULL) - { - const char *type_decl = getMessageTypeDeclarationField(fields_it); - - if(isBuiltinMessageType(fields_it->type)) - { - if(fields_it->is_array) - { - dynStringPushBackStr(&buffer, type_decl); - dynStringPushBackStr(&buffer,"["); - if(fields_it->array_size != -1) - { - char num[15]; - sprintf(num, "%d",fields_it->array_size); - dynStringPushBackStr(&buffer,num); - } - dynStringPushBackStr(&buffer,"]"); - } - else - { - dynStringPushBackStr(&buffer, type_decl); - } - - dynStringPushBackStr(&buffer," "); - dynStringPushBackStr(&buffer,fields_it->name); - dynStringPushBackStr(&buffer,"\n"); - } - else if(fields_it->type == CROS_STD_MSGS_HEADER) - { - cRosMessageDef *msg_header; - char *header_text; - unsigned char *res; - - msg_header = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); - if(msg_header != NULL) - { - initCrosMsg(msg_header); - header_text = (char *)malloc(strlen(HEADER_DEFAULT_TYPEDEF) + 1); - if(header_text != NULL) - { - memcpy(header_text,HEADER_DEFAULT_TYPEDEF,strlen(HEADER_DEFAULT_TYPEDEF) + 1); - loadFromStringMsg(header_text, msg_header); - free(header_text); - res = getMD5Msg(msg_header); - cRosMessageDefFree(msg_header); - if(res != NULL) - { - cRosMD5Readable(res, &buffer); - free(res); - dynStringPushBackStr(&buffer," "); - dynStringPushBackStr(&buffer,fields_it->name); - dynStringPushBackStr(&buffer,"\n"); - } - else - { - dynStringRelease(&buffer); - return NULL; - } - } - else - { - cRosMessageDefFree(msg_header); - dynStringRelease(&buffer); - return NULL; - } - } - else - { - dynStringRelease(&buffer); - return NULL; - } - } - else - { - cRosMessage *msg_fn; - - ret_err = cRosMessageBuildFromDef(&msg_fn, fields_it->child_msg_def); // it is faster than ret_err = cRosMessageNewBuild(msg->root_dir, type_decl, &msg_fn) - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - char *md5sum = strdup(msg_fn->md5sum); - cRosMessageFree(msg_fn); - if(md5sum != NULL) - { - dynStringPushBackStr(&buffer, md5sum); - dynStringPushBackStr(&buffer," "); - dynStringPushBackStr(&buffer,fields_it->name); - dynStringPushBackStr(&buffer,"\n"); - free(md5sum); - } - else - ret_err = CROS_MEM_ALLOC_ERR; - } - - } - fields_it = fields_it->next; - } - - if(dynStringGetLen(&buffer) == 0) - dynStringPushBackStr(&buffer,"\n"); - - MD5_CTX md5_t; - MD5_Init(&md5_t); - MD5_Update(&md5_t, dynStringGetData(&buffer), dynStringGetLen(&buffer) - 1); - MD5_Final(result, &md5_t); - dynStringRelease(&buffer); - - return (ret_err == CROS_SUCCESS_ERR_PACK)? result : NULL; -} - -cRosErrCodePack getMD5Txt(cRosMessageDef* msg_def, DynString* buffer) -{ - cRosErrCodePack ret_err; - - msgConst* const_it = msg_def->first_const; - - ret_err = CROS_SUCCESS_ERR_PACK; - while(ret_err == CROS_SUCCESS_ERR_PACK && const_it->next != NULL) - { - const char *type_decl = getMessageTypeDeclarationConst(const_it); - dynStringPushBackStr(buffer,type_decl); - dynStringPushBackStr(buffer," "); - dynStringPushBackStr(buffer,const_it->name); - dynStringPushBackStr(buffer,"="); - dynStringPushBackStr(buffer,const_it->value); - dynStringPushBackStr(buffer,"\n"); - const_it = const_it->next; - } - - msgFieldDef* fields_it = msg_def->first_field; - - while(ret_err == CROS_SUCCESS_ERR_PACK && fields_it->next != NULL) - { - const char *type_decl = getMessageTypeDeclarationField(fields_it); - - if(isBuiltinMessageType(fields_it->type)) - { - dynStringPushBackStr(buffer, type_decl); - if(fields_it->is_array) - { - dynStringPushBackStr(buffer,"["); - if(fields_it->array_size != -1) - { - char num[15]; - sprintf(num, "%d",fields_it->array_size); - dynStringPushBackStr(buffer,num); - } - dynStringPushBackStr(buffer,"]"); - } - - dynStringPushBackStr(buffer," "); - dynStringPushBackStr(buffer,fields_it->name); - dynStringPushBackStr(buffer,"\n"); - } - else if(fields_it->type == CROS_STD_MSGS_HEADER) - { - char *header_text; - unsigned char *res; - cRosMessageDef *msg_header; - - msg_header = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); - if(msg_header != NULL) - { - initCrosMsg(msg_header); - header_text = (char *)malloc(strlen(HEADER_DEFAULT_TYPEDEF) + 1); - if(header_text != NULL) - { - memcpy(header_text,HEADER_DEFAULT_TYPEDEF,strlen(HEADER_DEFAULT_TYPEDEF) + 1); - loadFromStringMsg(header_text, msg_header); - free(header_text); - res = getMD5Msg(msg_header); - if(res != NULL) - { - cRosMD5Readable(res, buffer); - free(res); - dynStringPushBackStr(buffer," "); - dynStringPushBackStr(buffer,fields_it->name); - dynStringPushBackStr(buffer,"\n"); - } - else - ret_err = CROS_MEM_ALLOC_ERR; - } - else - ret_err = CROS_MEM_ALLOC_ERR; - cRosMessageDefFree(msg_header); - } - else - ret_err = CROS_MEM_ALLOC_ERR; - } - else - { - char *msg_name_str; - cRosMessage *msg_fn; - - msg_name_str = base_msg_type(type_decl); - ret_err = cRosMessageBuildFromDef(&msg_fn, fields_it->child_msg_def); // it is faster than ret_err = cRosMessageNewBuild(msg_def->root_dir, msg_name_str, &msg_fn) - free(msg_name_str); - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - dynStringPushBackStr(buffer, msg_fn->md5sum); - cRosMessageFree(msg_fn); - - dynStringPushBackStr(buffer," "); - dynStringPushBackStr(buffer,fields_it->name); - dynStringPushBackStr(buffer,"\n"); - } - - } - fields_it = fields_it->next; - } - - if(dynStringGetLen(buffer) == 0) - dynStringPushBackStr(buffer,""); // Ensure that the returned DynString has allocated memory - else - dynStringReduce (buffer, 0, 1); // Remove the ending \n character which must not be processed by the MD5 algorithm - - return ret_err; -} - -void cRosMD5Readable(unsigned char* data, DynString* output) -{ - char val[5]; - int i; - for(i = 0; i < 16; i++) - { - snprintf(val, 4, "%02x", (unsigned char) data[i]); - dynStringPushBackStr(output,val); - } -} - -void cRosMessageInit(cRosMessage *message) -{ - message->fields = NULL; - message->n_fields = 0; - message->msgDef = NULL; - - message->md5sum = (char *)calloc(33, sizeof(char)); // 32 chars + '\0'; -} - -cRosMessage * cRosMessageNew(void) -{ - cRosMessage *ret = (cRosMessage *)calloc(1, sizeof(cRosMessage)); - - if (ret == NULL) - return NULL; - - cRosMessageInit(ret); - - if (ret->md5sum == NULL) - { - free(ret); - return NULL; - } - - return ret; -} - -void cRosMessageFieldInit(cRosMessageField *field) -{ - if(field != NULL) - { - field->is_const = 0; - field->name = NULL; - field->type = CROS_CUSTOM_TYPE; - field->type_s = NULL; - field->size = -1; - field->is_array = 0; - field->is_fixed_array = 0; - field->array_size = -1; - field->array_capacity = -1; - memset(field->data.opaque, 0, sizeof(field->data.opaque)); - } -} - -// Load message specification from a string: -// types, names, constants -cRosErrCodePack loadFromStringMsg(char* text, cRosMessageDef* msg) -{ - char* new_line = NULL; - char* new_line_saveptr = NULL; - const char* delimiter = "\n"; - - msg->plain_text = strdup(text); - if(msg->plain_text == NULL) - return CROS_MEM_ALLOC_ERR; - - new_line = strtok_r(text, delimiter, &new_line_saveptr); - while(new_line != NULL) - { - char* iterator = new_line; - int comment_char_found = 0; - int char_count = 0; // Number of characters in the line before the comment - - //strip comments and new line - while(((char)*iterator) != '\0') - { - if(((char)*iterator) == *CHAR_COMMENT) - { - comment_char_found = 1; - break; - } - char_count++; - iterator++; - } - - // ignore empty lines - if(comment_char_found) - { - if(char_count == 0) - { - new_line = strtok_r(NULL, delimiter, &new_line_saveptr); - continue; - } - else - { - //remove spaces between the message entry and the comment - while(*(new_line + char_count - 1) == ' ') - { - char_count--; - } - } - } - - char* msg_entry = (char *)calloc(char_count + 1, sizeof(char)); // type/name - strncpy(msg_entry, new_line, char_count); - char* entry_type; - char* entry_name; - - char* msg_entry_itr = msg_entry; - - while(*msg_entry_itr != '\0' && *(msg_entry_itr++) != ' '); // Skip character at the beginning of the line until a space is found - if(*(msg_entry_itr - 1) != ' ') // Error no space found - { - free(msg_entry); - return CROS_FILE_ENTRY_NO_SEP_ERR; - } - *(msg_entry_itr - 1) = '\0'; // Convert the found space into a \0 - - entry_name = (char *)calloc(strlen(msg_entry_itr) + 1, sizeof(char)); - strcpy(entry_name, msg_entry_itr); - entry_type = base_msg_type(msg_entry); // Return the type of the entry in a new allocated memory string - - char* filled_count = msg_entry_itr; - - while(*filled_count != '\0') // ??? Any purpose here - { - if(*filled_count == ' ') - { - msg_entry_itr = filled_count + 1; - while(*msg_entry_itr != '\0' && *msg_entry_itr == ' ' ) - msg_entry_itr++; - *filled_count = *msg_entry_itr; - if(*msg_entry_itr != '\0') - *msg_entry_itr = ' '; - } - else - { - filled_count ++; - } - } - - if(!is_valid_msg_type(entry_type)) - { - free(entry_name); - free(entry_type); - free(msg_entry); - return CROS_FILE_ENTRY_TYPE_ERR; - } - - char* const_char_ptr = strpbrk(entry_name, CHAR_CONST); - - if( const_char_ptr != NULL) // The entry is a constant definition - { - char* entry_const_val; - if(strcmp(entry_type, "string") == 0) - { - // String constants - // Strings contain anything to the right of the equals sign, - // there are no comments allowed - - char* entry_name_saveptr = NULL; - strtok_r(entry_name,CHAR_CONST,&entry_name_saveptr); - entry_const_val = strtok_r(NULL,CHAR_CONST,&entry_name_saveptr); - } - else - { - //Number constants - char* entry_name_saveptr = NULL; - strtok_r(entry_name,CHAR_CONST,&entry_name_saveptr); - entry_const_val = strtok_r(NULL,CHAR_CONST,&entry_name_saveptr); - } - - // TODO: try to cast number values - - msgConst* current = msg->constants; - - current->name = entry_name; - current->type = getMessageType(entry_type); - if (current->type == CROS_CUSTOM_TYPE) - { - current->type_s = entry_type; - entry_type = NULL; - } - current->value = strdup(entry_const_val); // strdup() will allocate a memory buffer that is independent of entry_name memory buffer so that it can freed independently as well - current->next = (msgConst *)malloc(sizeof(msgConst)); - msgConst* next = current->next; - initMsgConst(next); - next->prev = current; - msg->constants = next; - } - else // The entry is a data type declaration - { - msgFieldDef* current = msg->fields; - - current->name = entry_name; - current->type = getMessageType(entry_type); - if (current->type == CROS_CUSTOM_TYPE) - { - cRosErrCodePack ret_err; - - if(strstr(entry_type,"/") == NULL) - { - current->type_s = (char *)calloc(strlen(msg->package)+ 1 + strlen(entry_type) + 1, sizeof(char)); - memcpy(current->type_s, msg->package, strlen(msg->package)+ 1); - strcat(current->type_s,"/"); - strcat(current->type_s,entry_type); - } - else - { - current->type_s = entry_type; - entry_type = NULL; // Indicate that this buffer must not be freed later since it is used for the current msg def field - } - // Recursively load the msg definition of the custom type and store it into current->child_msg_def - ret_err = cRosMessageDefBuild(¤t->child_msg_def, msg->root_dir, current->type_s); - if(ret_err != CROS_SUCCESS_ERR_PACK) - { - free(current->type_s); - current->type_s = NULL; - free(entry_name); - current->name = NULL; - free(entry_type); - free(msg_entry); - return ret_err; - } - } - - int array_size; - if(is_array_type(msg_entry, &array_size)) - { - current->is_array = 1; - current->array_size = array_size; - } - - current->next = (msgFieldDef *)malloc(sizeof(msgFieldDef)); - msgFieldDef* next = current->next; - initFieldDef(next); - next->prev = current; - msg->fields = next; - } - - if (entry_type != NULL) - free(entry_type); - - free(msg_entry); - - new_line = strtok_r(NULL, delimiter, &new_line_saveptr); - } - - return CROS_SUCCESS_ERR_PACK; -} - -cRosErrCodePack loadFromFileMsg(char* filename, cRosMessageDef* msg) -{ - cRosErrCodePack ret_err; - size_t f_read_bytes; - char* file_tokenized; - FILE *f; - char* token_pack = NULL; - char* token_root = NULL; - char* token_name = NULL; - - f = fopen(filename, "rb"); - if (f == NULL) - return CROS_OPEN_MSG_FILE_ERR; - - // Load the entire message definition file in a buffer - fseek(f, 0, SEEK_END); - long fsize = ftell(f); - fseek(f, 0, SEEK_SET); - char *msg_text = (char *)malloc(fsize + 1); - if(msg_text == NULL) - { - fclose(f); - return CROS_MEM_ALLOC_ERR; - } - - f_read_bytes = fread(msg_text, 1, fsize, f); - fclose(f); - - if(f_read_bytes != (size_t)fsize) - { - free(msg_text); - return CROS_READ_MSG_FILE_ERR; - } - - msg_text[fsize] = '\0'; - - file_tokenized = (char *)calloc(strlen(filename)+1, sizeof(char)); - if(file_tokenized == NULL) - { - free(msg_text); - return CROS_MEM_ALLOC_ERR; - } - strcpy(file_tokenized, filename); - char* tok = strtok(file_tokenized,SPLIT_REGEX); - - while(tok != NULL) - { - if(strcmp(tok, "msg") != 0) - { - token_root = token_pack; - token_pack = token_name; - token_name = tok ; - } - tok = strtok(NULL,SPLIT_REGEX); - } - - //build up the root path - char* it = file_tokenized; - while(it != token_root) - { - if(*it == '\0') - *it=DIR_SEPARATOR_CHAR; - it++; - } - - msg->root_dir = (char*) malloc ((strlen(file_tokenized)+1) * sizeof(char)); // msg->root_dir[0] = '\0'; - msg->package = (char*) malloc ((strlen(token_pack)+1) * sizeof(char)); // msg->package[0] = '\0'; - msg->name = (char*) malloc ((strlen(token_name)+1) * sizeof(char)); // msg->name[0] = '\0'; - if(msg->root_dir != NULL && msg->package != NULL && msg->name != NULL) - { - strcpy(msg->root_dir,file_tokenized); - strcpy(msg->package,token_pack); - strcpy(msg->name,token_name); - ret_err = loadFromStringMsg(msg_text, msg); - ret_err = cRosAddErrCodeIfErr(ret_err, CROS_LOAD_MSG_FILE_ERR); // If an error occurred (the file could not be loaded), add more info (CROS_LOAD_MSG_FILE_ERR error code) - } - else - { - free(msg->root_dir); - free(msg->package); - free(msg->name); - ret_err = CROS_MEM_ALLOC_ERR; - } - - free(msg_text); - free(file_tokenized); - - return ret_err; -} - -cRosMessage *build_time_field(void) -{ - cRosMessage* time_msg = cRosMessageNew(); - if(time_msg != NULL) - { - time_msg->fields = (cRosMessageField **)calloc(2,sizeof(cRosMessageField*)); - if(time_msg->fields != NULL) - { - time_msg->n_fields = 2; - - cRosMessageField* sec = cRosMessageFieldNew(); - cRosMessageField* nsec = cRosMessageFieldNew(); - if(sec != NULL && nsec != NULL) - { - //int32 sec - sec->name = strdup("secs"); - sec->type = CROS_STD_MSGS_UINT32; - sec->size = getMessageTypeSizeOf(sec->type); - time_msg->fields[0] = sec; - - //int32 nsec - nsec->name = strdup("nsecs"); - nsec->type = CROS_STD_MSGS_UINT32; - nsec->size = getMessageTypeSizeOf(nsec->type); - time_msg->fields[1] = nsec; - - if(sec->name == NULL || nsec->name == NULL) - { - cRosMessageFree(time_msg); - time_msg = NULL; - } - } - else - { - cRosMessageFieldFree(sec); - cRosMessageFieldFree(nsec); - cRosMessageFree(time_msg); - time_msg = NULL; - } - } - else - { - cRosMessageFree(time_msg); - time_msg = NULL; - } - } - - return time_msg; -} - -cRosMessage *build_duration_field(void) -{ - cRosMessage* durat_msg = cRosMessageNew(); - if(durat_msg != NULL) - { - durat_msg->fields = (cRosMessageField **)calloc(2,sizeof(cRosMessageField*)); - if(durat_msg->fields != NULL) - { - durat_msg->n_fields = 2; - - cRosMessageField* sec = cRosMessageFieldNew(); - cRosMessageField* nsec = cRosMessageFieldNew(); - if(sec != NULL && nsec != NULL) - { - //int32 sec - sec->name = strdup("secs"); - sec->type = CROS_STD_MSGS_INT32; - sec->size = getMessageTypeSizeOf(sec->type); - durat_msg->fields[0] = sec; - - //int32 nsec - nsec->name = strdup("nsecs"); - nsec->type = CROS_STD_MSGS_INT32; - nsec->size = getMessageTypeSizeOf(nsec->type); - durat_msg->fields[1] = nsec; - - if(sec->name == NULL || nsec->name == NULL) - { - cRosMessageFree(durat_msg); - durat_msg = NULL; - } - } - else - { - cRosMessageFieldFree(sec); - cRosMessageFieldFree(nsec); - cRosMessageFree(durat_msg); - durat_msg = NULL; - } - } - else - { - cRosMessageFree(durat_msg); - durat_msg = NULL; - } - } - - return durat_msg; -} - -cRosMessage *build_header_field(void) -{ - cRosMessage* header_msg = cRosMessageNew(); - if(header_msg != NULL) - { - header_msg->fields = (cRosMessageField **)calloc(3,sizeof(cRosMessageField*)); - if(header_msg->fields != NULL) - { - header_msg->n_fields = 3; - - cRosMessageField* sequence_id = cRosMessageFieldNew(); - cRosMessageField* time_stamp = cRosMessageFieldNew(); - cRosMessageField* frame_id = cRosMessageFieldNew(); - if(sequence_id != NULL && time_stamp != NULL && frame_id != NULL) - { - //uint32 seq - sequence_id->name = strdup("seq"); - sequence_id->type = CROS_STD_MSGS_UINT32; - sequence_id->size = getMessageTypeSizeOf(sequence_id->type); - header_msg->fields[0] = sequence_id; - - // time stamp - // Two-integer timestamp that is expressed as: - // * stamp.secs: seconds (stamp_secs) since epoch - // * stamp.nsecs: nanoseconds since stamp_secs - time_stamp->name = strdup("stamp"); - time_stamp->type = CROS_STD_MSGS_TIME; - time_stamp->data.as_msg = build_time_field(); - header_msg->fields[1] = time_stamp; - - //string frame_id - frame_id->name = strdup("frame_id"); - frame_id->type = CROS_STD_MSGS_STRING; - header_msg->fields[2] = frame_id; - if(sequence_id->name == NULL || time_stamp->name == NULL || frame_id->name == NULL) - { - cRosMessageFree(header_msg); - header_msg = NULL; - } - } - else - { - cRosMessageFieldFree(frame_id); - cRosMessageFieldFree(time_stamp); - cRosMessageFieldFree(sequence_id); - cRosMessageFree(header_msg); - header_msg = NULL; - } - } - else - { - cRosMessageFree(header_msg); - header_msg = NULL; - } - } - - return header_msg; -} - -cRosErrCodePack cRosMessageDefBuild(cRosMessageDef **msg_def_ptr, const char *msg_root_dir, const char *msg_type) -{ - cRosErrCodePack ret; - cRosMessageDef* msg_def = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); - if(msg_def == NULL) - return CROS_MEM_ALLOC_ERR; - - ret = initCrosMsg(msg_def); - if(ret == CROS_SUCCESS_ERR_PACK) - { - char* msg_file_path; - if(msg_root_dir != NULL) // If msg_root_dir parameter is not NULL, the file path is composed, otherwise, msg_type is used directly as file path - { - msg_file_path = (char *)malloc(strlen(msg_root_dir) + strlen(DIR_SEPARATOR_STR) + strlen(msg_type) + strlen(FILEEXT_MSG) + 2); - if(msg_file_path != NULL) - { - strcpy(msg_file_path, msg_root_dir); - strcat(msg_file_path, DIR_SEPARATOR_STR); - strcat(msg_file_path, msg_type); - strcat(msg_file_path, "."); - strcat(msg_file_path, FILEEXT_MSG); - } - else - ret = CROS_MEM_ALLOC_ERR; - } - else - msg_file_path = (char *)msg_type; - if(ret == CROS_SUCCESS_ERR_PACK) - { - ret = loadFromFileMsg(msg_file_path, msg_def); - if(msg_root_dir != NULL) - free(msg_file_path); - - if (ret == CROS_SUCCESS_ERR_PACK) - *msg_def_ptr = msg_def; - else - cRosMessageDefFree(msg_def); - } - } - else - free(msg_def); - - return ret; -} - -int cRosMessageFieldCopy(cRosMessageField* new_field, cRosMessageField* orig_field) -{ - int ret; - if(new_field != NULL && orig_field != NULL) - { - new_field->size = orig_field->size; - new_field->is_const = orig_field->is_const; - new_field->is_array = orig_field->is_array; - new_field->is_fixed_array = orig_field->is_fixed_array; - new_field->array_size = orig_field->array_size; - new_field->array_capacity = orig_field->array_capacity; - new_field->type = orig_field->type; - new_field->type_s = (orig_field->type_s != NULL)? strdup(orig_field->type_s):NULL; - new_field->name = (orig_field->name != NULL)? strdup(orig_field->name):NULL; - if((new_field->type_s == NULL && orig_field->type_s != NULL) || (new_field->name == NULL && orig_field->name != NULL)) - { - free(new_field->type_s); - free(new_field->name); - ret=-1; - } - else - { - ret=0; // Default return value - if(!orig_field->is_array) - { - switch (orig_field->type) - { - case CROS_STD_MSGS_INT8: - case CROS_STD_MSGS_UINT8: - case CROS_STD_MSGS_INT16: - case CROS_STD_MSGS_UINT16: - case CROS_STD_MSGS_INT32: - case CROS_STD_MSGS_UINT32: - case CROS_STD_MSGS_INT64: - case CROS_STD_MSGS_UINT64: - case CROS_STD_MSGS_FLOAT32: - case CROS_STD_MSGS_FLOAT64: - case CROS_STD_MSGS_BOOL: - case CROS_STD_MSGS_CHAR: - case CROS_STD_MSGS_BYTE: - { - new_field->data = orig_field->data; - break; - } - case CROS_STD_MSGS_STRING: - { - new_field->data.as_string = (orig_field->data.as_string != NULL)? strdup(orig_field->data.as_string) : NULL; - if(orig_field->data.as_string != NULL && new_field->data.as_string == NULL) - ret=-1; // Error allocating memory for string - break; - } - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_HEADER: - case CROS_CUSTOM_TYPE: - { - if(!orig_field->is_array) - { - new_field->data.as_msg = cRosMessageCopyWithoutDef(orig_field->data.as_msg); - if(new_field->data.as_msg == NULL) - ret=-1; - } - break; - } - } - } - else - { - // Allocate the array - if(orig_field->type == CROS_STD_MSGS_STRING) // The array is encoded as a pointer to pointers - new_field->data.as_array = calloc(orig_field->array_capacity, sizeof(char *)); // String pointers - else if(orig_field->type == CROS_STD_MSGS_TIME || orig_field->type == CROS_STD_MSGS_DURATION || - orig_field->type == CROS_STD_MSGS_HEADER || orig_field->type == CROS_CUSTOM_TYPE) - new_field->data.as_array = calloc(orig_field->array_capacity, sizeof(cRosMessage *)); // Message pointers - else // The array is encoded as a pointer to elements - new_field->data.as_array = calloc(orig_field->array_capacity, orig_field->size); // Element pointers - if(new_field->data.as_array != NULL) // If the array was allocated, copy the elements - { - switch (orig_field->type) - { - case CROS_STD_MSGS_STRING: - { - int n_str; - for(n_str=0;n_strarray_size && ret==0;n_str++) - { - new_field->data.as_string_array[n_str] = (orig_field->data.as_string_array[n_str] != NULL)?strdup(orig_field->data.as_string_array[n_str]):NULL; - if(new_field->data.as_string_array[n_str] == NULL && orig_field->data.as_string_array[n_str] != NULL) - ret=-1; - } - if(ret != 0) // Error allocating memory for strings: free previously allocated string memory - for(n_str=0;n_strarray_size && ret==0;n_str++) - free(new_field->data.as_string_array[n_str]); - break; - } - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_HEADER: - case CROS_CUSTOM_TYPE: - { - int n_msg; - for(n_msg=0;n_msgarray_size && ret==0;n_msg++) - { - new_field->data.as_msg_array[n_msg] = cRosMessageCopyWithoutDef(orig_field->data.as_msg_array[n_msg]); - if(new_field->data.as_msg_array[n_msg] == NULL && orig_field->data.as_msg_array[n_msg] != NULL) - ret=-1; - } - if(ret != 0) // Error allocating memory for messages: free previously allocated message memory - for(n_msg=0;n_msgarray_size && ret==0;n_msg++) - cRosMessageFree(new_field->data.as_msg_array[n_msg]); - break; - } - default: - { - memcpy(new_field->data.as_array, orig_field->data.as_array, orig_field->size * orig_field->array_size); - break; - } - } - if(ret != 0) - { - free(new_field->data.as_array); - } - } - else - ret=-1; // Error allocating the array - } - if(ret != 0) // Free memory to leave the message in a determined state - { - free(new_field->type_s); - free(new_field->name); - } - } - } - else - ret=-1; - return ret; -} - -void printNSpaces(int n_spaces) -{ - int spc_ind; - for(spc_ind=0;spc_indname!=NULL)?msg_field->name:"NULL", msg_field->size, msg_field->is_const, \ - msg_field->is_array, msg_field->is_fixed_array, msg_field->array_size, msg_field->array_capacity, \ - (msg_field->type_s!=NULL)?msg_field->type_s:"NULL", getMessageTypeDeclaration(msg_field->type)); - switch(msg_field->type) - { - case CROS_STD_MSGS_INT8: - { - if(!msg_field->is_array) - fprintf(msg_out,"%hi", (short)msg_field->data.as_int8); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%hi,", (short)msg_field->data.as_int8_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_UINT8: - { - if(!msg_field->is_array) - fprintf(msg_out,"%hu", (unsigned short)msg_field->data.as_uint8); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%hu,", (unsigned short)msg_field->data.as_uint8_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_INT16: - { - if(!msg_field->is_array) - fprintf(msg_out,"%hi", msg_field->data.as_int16); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%hi,", msg_field->data.as_int16_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_UINT16: - { - if(!msg_field->is_array) - fprintf(msg_out,"%hu", msg_field->data.as_uint16); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%hu,", msg_field->data.as_uint16_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_INT32: - { - if(!msg_field->is_array) - fprintf(msg_out,"%li", (long)msg_field->data.as_int32); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%li,", (long)msg_field->data.as_int32_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_UINT32: - { - if(!msg_field->is_array) - fprintf(msg_out,"%lu", (long unsigned)msg_field->data.as_uint32); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%lu,", (long unsigned)msg_field->data.as_uint32_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_INT64: - { - if(!msg_field->is_array) - fprintf(msg_out,"%lli", (long long)msg_field->data.as_int64); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%lli,", (long long)msg_field->data.as_int64_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_UINT64: - { - if(!msg_field->is_array) - fprintf(msg_out,"%llu", (long long unsigned)msg_field->data.as_uint64); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%llu,", (long long unsigned)msg_field->data.as_uint64_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_FLOAT32: - { - if(!msg_field->is_array) - fprintf(msg_out,"%f", msg_field->data.as_float32); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%f,", msg_field->data.as_float32_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_FLOAT64: - { - if(!msg_field->is_array) - fprintf(msg_out,"%f", msg_field->data.as_float64); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%f,", msg_field->data.as_float64_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_BOOL: - { - if(!msg_field->is_array) - fprintf(msg_out,"%hu", (unsigned short)msg_field->data.as_uint8); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%hu,", (unsigned short)msg_field->data.as_uint8_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_CHAR: - { - if(!msg_field->is_array) - fprintf(msg_out,"%hu", (unsigned short)msg_field->data.as_uint8); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%hu,", (unsigned short)msg_field->data.as_uint8_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_BYTE: - { - if(!msg_field->is_array) - fprintf(msg_out,"%hi", (short)msg_field->data.as_int8); - else - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"%hi,", (short)msg_field->data.as_int8_array[elem_ind]); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - break; - } - case CROS_STD_MSGS_STRING: - { - if(!msg_field->is_array) - fprintf(msg_out,"'%s'", (msg_field->data.as_string!=NULL)?msg_field->data.as_string:"NULL"); - else - { - if(msg_field->data.as_string_array != NULL) - { - for(elem_ind=0;elem_ind < msg_field->array_size && elem_ind < MAX_NUM_MSG_ELEM_PRINT;elem_ind++) - fprintf(msg_out,"'%s',", (msg_field->data.as_string_array[elem_ind]!=NULL)?msg_field->data.as_string_array[elem_ind]:"NULL"); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - else - fprintf(msg_out,"NULL"); - } - break; - } - default: - { - break; - } - } - // The value of these last types are printed after the switch statement because they share the same code - if(msg_field->type == CROS_STD_MSGS_TIME || msg_field->type == CROS_STD_MSGS_DURATION || - msg_field->type == CROS_STD_MSGS_HEADER || msg_field->type == CROS_CUSTOM_TYPE) - { - if(!msg_field->is_array) - cRosMessageFieldsPrint(msg_field->data.as_msg, n_indent); - else - { - fprintf(msg_out,"\n"); - for(elem_ind=0;elem_ind < msg_field->array_size;elem_ind++) - cRosMessageFieldsPrint(msg_field->data.as_msg_array[elem_ind], n_indent); - if(msg_field->array_size > MAX_NUM_MSG_ELEM_PRINT) - fprintf(msg_out,"..."); - } - } - else - fprintf(msg_out,"\n"); - } - else - fprintf(msg_out,"Field NULL\n"); -} - -// This function prints the MD5 and all the fields (fields field struct) of one message (msg) to the output file stream. -void cRosMessageFieldsPrint(cRosMessage *msg, int n_indent) -{ - printNSpaces(n_indent); - if(msg != NULL) - { - fprintf(cRosOutStreamGet(), "MsgAt 0x%p: MD5:'%s' N.Flds:%i Flds:%s\n", msg, (msg->md5sum != NULL)? msg->md5sum: "NULL", msg->n_fields, (msg->fields != NULL)?"":"NULL"); - if(msg->fields != NULL) - { - int field_ind; - // Print all the filed in source message while no error occurs - for(field_ind=0;field_indn_fields;field_ind++) - cRosMessageFieldPrint(msg->fields[field_ind], n_indent+1); - } - } - else - fprintf(cRosOutStreamGet(), "MsgAt NULL\n"); -} - -// This function copies the MD5 and all the fields (fields field struct) from one message (m_src) to another (m_dst). -// It if expected than the fields field of m_dst is NULL. -int cRosMessageFieldsCopy(cRosMessage *m_dst, cRosMessage *m_src) -{ - int ret; - - if(m_src != NULL && m_src != NULL) - { - if(m_src->md5sum != NULL) // If source message has a valid MD5 field, copy it - { - if(m_dst->md5sum != NULL) - { - strcpy(m_dst->md5sum, m_src->md5sum); - ret=0; - } - else - { - m_dst->md5sum = strdup(m_src->md5sum); - ret = (m_dst->md5sum != NULL)?0:-1; - } - } - else - { - free(m_dst->md5sum); - m_dst->md5sum=NULL; - ret=0; - } - } - else - ret=-1; - if(ret == 0) // If no error copying MD5 field, continue - { - // Remove previous fields from destination message - cRosMessageFieldsFree(m_dst); - // Create a new field array in destination message - m_dst->fields = (cRosMessageField **)calloc(m_src->n_fields, sizeof(cRosMessageField*)); - if(m_dst->fields != NULL) - { - int field_ind; - - m_dst->n_fields = m_src->n_fields; - // Copy all the filed in source message while no error occurs - for(field_ind=0;field_indn_fields && ret==0;field_ind++) - { - cRosMessageField *new_field = cRosMessageFieldNew(); - if(new_field != NULL) - { - ret = cRosMessageFieldCopy(new_field, m_src->fields[field_ind]); - if(ret == 0) - m_dst->fields[field_ind] = new_field; - else - free(new_field); - } - else - ret=-1; - } - if(ret != 0) // Error copying fields - { - // Destroy already created fields - cRosMessageFieldsFree(m_dst); - } - } - else - ret=-1; - } - return ret; -} - -cRosMessage *cRosMessageCopyWithoutDef(cRosMessage *m_src) -{ - cRosMessage *m_dst; - if(m_src != NULL) - { - m_dst = cRosMessageNew(); - if(m_dst != NULL) - { - if(cRosMessageFieldsCopy(m_dst, m_src) != 0) - { - // Error copying fields: free the new message - cRosMessageFree(m_dst); - m_dst=NULL; // Return NULL (error indicator) - } - } - } - else - m_dst = NULL; - return m_dst; -} - -cRosMessage *cRosMessageCopy(cRosMessage *m_src) -{ - cRosMessage *m_dst; - if(m_src != NULL) - { - m_dst = cRosMessageNew(); - if(m_dst != NULL) - { - if(cRosMessageFieldsCopy(m_dst, m_src) == 0) - { - cRosErrCodePack ret_err; - ret_err = cRosMessageDefCopy(&m_dst->msgDef, m_src->msgDef ); - if(ret_err != CROS_SUCCESS_ERR_PACK) - { - cRosMessageFree(m_dst); - m_dst=NULL; // Return NULL (error indicator) - } - } - else - { - // Error copying fields: free the new message - cRosMessageFree(m_dst); - m_dst=NULL; // Return NULL (error indicator) - } - } - } - else - m_dst = NULL; - return m_dst; -} - -cRosErrCodePack cRosFieldDefCopy(msgFieldDef* new_field_def, msgFieldDef* orig_field_def ) -{ - cRosErrCodePack ret; - if(new_field_def != NULL && orig_field_def != NULL) - { - new_field_def->type = orig_field_def->type; - new_field_def->is_array = orig_field_def->is_array; - new_field_def->array_size = orig_field_def->array_size; - new_field_def->next = NULL; - new_field_def->prev = NULL; - new_field_def->type_s = (orig_field_def->type_s != NULL)? strdup(orig_field_def->type_s):NULL; - new_field_def->name = (orig_field_def->name != NULL)? strdup(orig_field_def->name):NULL; - if((new_field_def->type_s == NULL && orig_field_def->type_s != NULL) || (new_field_def->name == NULL && orig_field_def->name != NULL)) - ret=CROS_MEM_ALLOC_ERR; - else - ret = cRosMessageDefCopy(&new_field_def->child_msg_def, orig_field_def->child_msg_def ); - - if(ret != CROS_SUCCESS_ERR_PACK) - { - free(new_field_def->type_s); - free(new_field_def->name); - new_field_def->type_s = NULL; - new_field_def->name = NULL; - } - } - else - ret=CROS_BAD_PARAM_ERR; - return ret; -} - -cRosErrCodePack cRosConstDefCopy(msgConst* new_const_def, msgConst* orig_const_def ) -{ - cRosErrCodePack ret; - if(new_const_def != NULL && orig_const_def != NULL) - { - new_const_def->type = orig_const_def->type; - new_const_def->next = NULL; - new_const_def->prev = NULL; - new_const_def->type_s = (orig_const_def->type_s != NULL)? strdup(orig_const_def->type_s):NULL; - new_const_def->name = (orig_const_def->name != NULL)? strdup(orig_const_def->name):NULL; - new_const_def->value = (orig_const_def->value != NULL)? strdup(orig_const_def->value):NULL; - if((new_const_def->type_s == NULL && orig_const_def->type_s != NULL) || - (new_const_def->name == NULL && orig_const_def->name != NULL) || - (new_const_def->value == NULL && orig_const_def->value != NULL)) - { - free(new_const_def->type_s); - free(new_const_def->name); - free(new_const_def->value); - ret=CROS_MEM_ALLOC_ERR; - } - else - ret=CROS_SUCCESS_ERR_PACK; - } - else - ret=CROS_BAD_PARAM_ERR; - return ret; -} - -// Make a copy of a message definition struct allocating new memory for it, so that all fields can be freed independently from the originals -cRosErrCodePack cRosMessageDefCopy(cRosMessageDef** ptr_new_msg_def, cRosMessageDef* orig_msg_def ) -{ - cRosErrCodePack ret; - - if(orig_msg_def == NULL) - { - *ptr_new_msg_def = NULL; - return CROS_SUCCESS_ERR_PACK; - } - - *ptr_new_msg_def = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); - if(*ptr_new_msg_def != NULL) - { - initCrosMsg(*ptr_new_msg_def); - (*ptr_new_msg_def)->name = (orig_msg_def->name != NULL)? strdup(orig_msg_def->name):NULL; // NULL should not be passed to strdup() - (*ptr_new_msg_def)->package = (orig_msg_def->package != NULL)? strdup(orig_msg_def->package):NULL; - (*ptr_new_msg_def)->root_dir = (orig_msg_def->root_dir != NULL)? strdup(orig_msg_def->root_dir):NULL; - (*ptr_new_msg_def)->plain_text = (orig_msg_def->plain_text != NULL)? strdup(orig_msg_def->plain_text):NULL; - - ret=CROS_SUCCESS_ERR_PACK; // Default return value: no error - // Copy the first field - if(orig_msg_def->first_field != NULL) - { - msgFieldDef *new_field_itr, *orig_field_itr; - - orig_field_itr = orig_msg_def->first_field; - new_field_itr = (*ptr_new_msg_def)->first_field; - - ret=cRosFieldDefCopy(new_field_itr, orig_field_itr); // Copy the first msg def field - - // Copy the rest of message definition fields - while(orig_field_itr->next != NULL && ret == CROS_SUCCESS_ERR_PACK) - { - new_field_itr->next = (msgFieldDef *)malloc(sizeof(msgFieldDef)); - if(new_field_itr->next != NULL) - { - initFieldDef(new_field_itr->next); - ret=cRosFieldDefCopy(new_field_itr->next, orig_field_itr->next); - if(ret == CROS_SUCCESS_ERR_PACK) - { - new_field_itr->next->prev = new_field_itr; - new_field_itr = new_field_itr->next; - orig_field_itr = orig_field_itr->next; - } - else // Error allocating memory: discard last filed and exit the loop - { - free(new_field_itr->next); - new_field_itr->next = NULL; - } - } - else - ret=CROS_MEM_ALLOC_ERR; // Error allocating memory: exit the loop - } - (*ptr_new_msg_def)->fields=new_field_itr; // Last valid field - } - else - { - free((*ptr_new_msg_def)->first_field); - (*ptr_new_msg_def)->first_field=NULL; - (*ptr_new_msg_def)->fields=NULL; - } - - // Copy the first constant - if(orig_msg_def->first_const != NULL) - { - msgConst *new_const_itr, *orig_const_itr; - - orig_const_itr = orig_msg_def->first_const; - new_const_itr = (*ptr_new_msg_def)->first_const; - - ret=cRosConstDefCopy(new_const_itr, orig_const_itr); - - // Copy message definition constants - while(orig_const_itr->next != NULL && ret == CROS_SUCCESS_ERR_PACK) - { - new_const_itr->next = (msgConst *)malloc(sizeof(msgConst)); - if(new_const_itr->next != NULL) - { - initMsgConst(new_const_itr->next); - ret=cRosConstDefCopy(new_const_itr->next, orig_const_itr->next); - if(ret == CROS_SUCCESS_ERR_PACK) - { - new_const_itr->next->prev = new_const_itr; - new_const_itr = new_const_itr->next; - orig_const_itr = orig_const_itr->next; - } - else // Error allocating memory: discard last filed and exit the loop - { - free(new_const_itr->next); - new_const_itr->next = NULL; - } - } - else - ret=CROS_MEM_ALLOC_ERR; // Error allocating memory: exit the loop - } - (*ptr_new_msg_def)->constants = new_const_itr; // Last valid constant - - } - else - { - free((*ptr_new_msg_def)->first_const); - (*ptr_new_msg_def)->first_const=NULL; - (*ptr_new_msg_def)->constants=NULL; - } - if(ret != CROS_SUCCESS_ERR_PACK) // Error making the msg. def. copy: free all the memory that we have allocated - { - cRosMessageDefFree(*ptr_new_msg_def); - *ptr_new_msg_def = NULL; - } - } - else - ret=CROS_MEM_ALLOC_ERR; - return(ret); -} - -cRosErrCodePack cRosMessageNewBuild(const char *msg_root_dir, const char *msg_type, cRosMessage **new_msg_ptr) -{ - cRosErrCodePack ret_err; - - if(new_msg_ptr != NULL) - { - cRosMessageDef *msg_def; - ret_err = cRosMessageDefBuild(&msg_def, msg_root_dir, msg_type); - if(ret_err == CROS_SUCCESS_ERR_PACK) // Check if we got any error while building message definition - { - ret_err = cRosMessageBuildFromDef(new_msg_ptr, msg_def); // Copy msg_def into message->msgDef and build the message (original msg_def can be freed after the copy) - cRosMessageDefFree(msg_def); - } - } - else - ret_err = CROS_BAD_PARAM_ERR; - return ret_err; -} - -cRosErrCodePack cRosMessageBuildFromDef(cRosMessage** message_ptr, cRosMessageDef* msg_def ) -{ - cRosErrCodePack ret; - cRosMessage* message; - - ret = CROS_SUCCESS_ERR_PACK; // Default return value: success - - message = cRosMessageNew(); - if(message == NULL) - return CROS_MEM_ALLOC_ERR; - - DynString output; - dynStringInit(&output); - - cRosMessageDefFree(message->msgDef); // Just in case there was a previous message definition in the message - cRosMessageDefCopy(&message->msgDef, msg_def ); - - unsigned char* res = getMD5Msg(msg_def); - cRosMD5Readable(res, &output); - free(res); - strcpy(message->md5sum, dynStringGetData(&output)); - dynStringRelease(&output); - - msgFieldDef* field_def_itr = msg_def->first_field; - while(field_def_itr->next != NULL) - { - message->n_fields++; - field_def_itr = field_def_itr->next; - } - - message->fields = (cRosMessageField **)calloc(message->n_fields, sizeof(cRosMessageField*)); - - field_def_itr = msg_def->first_field; - cRosMessageField** msg_field_itr = message->fields; - - while(field_def_itr->next != NULL && ret == CROS_SUCCESS_ERR_PACK) - { - *msg_field_itr = (cRosMessageField *)malloc(sizeof(cRosMessageField)); - cRosMessageField* field = *msg_field_itr; - cRosMessageFieldInit(field); - - field->type = field_def_itr->type; - field->name = (char *)calloc(strlen(field_def_itr->name)+1,sizeof(char)); - strcpy(field->name, field_def_itr->name); - if (field_def_itr->type_s != NULL) - { - field->type_s = (char *)calloc(strlen(field_def_itr->type_s)+1,sizeof(char)); - strcpy(field->type_s, field_def_itr->type_s); - } - - field->is_array = field_def_itr->is_array; - if(field_def_itr->is_array) - { - if (field_def_itr->array_size == -1) // Variable-length array - { - field->array_size = 0; - field->array_capacity = 1; // If we don't now the array length, allocate memory for at least one element - } - else // Fixed-length array - { - field->array_size = field_def_itr->array_size; - field->array_capacity = field_def_itr->array_size; - field->is_fixed_array = 1; - } - } - switch (field->type) - { - case CROS_STD_MSGS_INT8: - case CROS_STD_MSGS_UINT8: - case CROS_STD_MSGS_INT16: - case CROS_STD_MSGS_UINT16: - case CROS_STD_MSGS_INT32: - case CROS_STD_MSGS_UINT32: - case CROS_STD_MSGS_INT64: - case CROS_STD_MSGS_UINT64: - case CROS_STD_MSGS_FLOAT32: - case CROS_STD_MSGS_FLOAT64: - case CROS_STD_MSGS_BOOL: - case CROS_STD_MSGS_CHAR: - case CROS_STD_MSGS_BYTE: - { - field->size = getMessageTypeSizeOf(field->type); - if(field_def_itr->is_array) - { - field->data.as_array = calloc(field->array_capacity,field->size); - if(field->data.as_array == NULL) - ret=CROS_MEM_ALLOC_ERR; - } - break; - } - case CROS_STD_MSGS_STRING: - { - if(field_def_itr->is_array) - { - field->data.as_string_array = (char **)calloc(field->array_capacity,sizeof(char *)); - if(field->data.as_string_array == NULL) - ret=CROS_MEM_ALLOC_ERR; - } - break; - } - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_HEADER: - case CROS_CUSTOM_TYPE: - { - cRosMessage *new_msg; - if(field_def_itr->is_array) - { - field->data.as_msg_array = (cRosMessage **)calloc(field->array_capacity,sizeof(cRosMessage *)); - if(field->data.as_msg_array != NULL) - { - int msg_ind; - - for(msg_ind=0;msg_indarray_size && ret == CROS_SUCCESS_ERR_PACK;msg_ind++) - { - new_msg = NULL; - if(field->type == CROS_STD_MSGS_TIME) - new_msg = build_time_field(); - else if(field->type == CROS_STD_MSGS_DURATION) - new_msg = build_duration_field(); - else if(field->type == CROS_STD_MSGS_HEADER) - new_msg = build_header_field(); - else if(field->type == CROS_CUSTOM_TYPE) - { - ret = cRosMessageBuildFromDef(&new_msg, field_def_itr->child_msg_def); // faster alternative to ret = cRosMessageNewBuild(msg_def->root_dir, field_def_itr->type_s, &new_msg) - ret = cRosAddErrCodeIfErr(ret, CROS_CREATE_CUSTOM_MSG_ERR); // If the message could not be created, add more info to the error code pack - } - - if(new_msg != NULL) // New message created: insert it into the array - cRosMessageFieldArrayAtMsgSet(field, msg_ind, new_msg); - else // Error creating the new message - { - if(field->type == CROS_STD_MSGS_TIME || field->type == CROS_STD_MSGS_DURATION || field->type == CROS_STD_MSGS_HEADER) - ret = CROS_MEM_ALLOC_ERR; - } - } - } - else - ret = CROS_MEM_ALLOC_ERR; - } - else - { - if(field->type == CROS_STD_MSGS_TIME) - field->data.as_msg = build_time_field(); - else if(field->type == CROS_STD_MSGS_DURATION) - field->data.as_msg = build_duration_field(); - else if(field->type == CROS_STD_MSGS_HEADER) - field->data.as_msg = build_header_field(); - else if(field->type == CROS_CUSTOM_TYPE) - { - ret = cRosMessageBuildFromDef(&field->data.as_msg, field_def_itr->child_msg_def); - ret = cRosAddErrCodeIfErr(ret, CROS_CREATE_CUSTOM_MSG_ERR); - } - - if(field->data.as_msg == NULL && (field->type == CROS_STD_MSGS_TIME || field->type == CROS_STD_MSGS_DURATION || field->type == CROS_STD_MSGS_HEADER)) - ret = CROS_MEM_ALLOC_ERR; - } - break; - } - } - msg_field_itr++; - field_def_itr = field_def_itr->next; - } - - if(ret == CROS_SUCCESS_ERR_PACK) - *message_ptr = message; - else - cRosMessageFree(message); - - return ret; -} - -void cRosMessageConstDefFree(msgConst* msg_const) -{ - if(msg_const != NULL) - { - free(msg_const->name); - msg_const->name = NULL; - free(msg_const->type_s); - msg_const->type_s = NULL; - free(msg_const->value); - msg_const->value = NULL; - free(msg_const); // Assignment to NULL not needed - } -} - -void cRosMessageFieldDefFree(msgFieldDef* msg_field) -{ - if(msg_field != NULL) - { - cRosMessageDefFree(msg_field->child_msg_def); - free(msg_field->name); - msg_field->name = NULL; - free(msg_field->type_s); - msg_field->type_s = NULL; // Assignment to NULL not needed - free(msg_field); - } -} - -void cRosMessageDefFree(cRosMessageDef *msgDef) -{ - if (msgDef == NULL) - return; - - free(msgDef->name); - msgDef->name = NULL; - free(msgDef->package); - msgDef->package = NULL; - free(msgDef->root_dir); - msgDef->root_dir = NULL; - free(msgDef->plain_text); - msgDef->plain_text = NULL; - - msgConst* it_const = msgDef->first_const; - while(it_const != NULL) - { - msgConst* next_const = it_const->next; - cRosMessageConstDefFree(it_const); - it_const = next_const; - } - msgDef->first_const = NULL; - msgDef->constants = NULL; - - msgFieldDef* it_field = msgDef->first_field; - while(it_field != NULL) - { - msgFieldDef* next_field=it_field->next; - cRosMessageFieldDefFree(it_field); - it_field = next_field; - } - msgDef->first_field = NULL; - msgDef->fields = NULL; - free(msgDef); // All the previous pointer assignments to NULL make no sense if we free the cRosMessageDef struct -} - -void cRosMessageFieldsFree(cRosMessage *message) -{ - int i; - - for(i = 0; i < message->n_fields; i++) - cRosMessageFieldFree(message->fields[i]); - free(message->fields); - message->fields = NULL; - message->n_fields = 0; -} - -void cRosMessageRelease(cRosMessage *message) -{ - cRosMessageFieldsFree(message); - - cRosMessageDefFree(message->msgDef); - message->msgDef = NULL; - - free(message->md5sum); - message->md5sum = NULL; -} - -void cRosMessageFree(cRosMessage *message) -{ - if (message == NULL) - return; - - cRosMessageRelease(message); - free(message); -} - -cRosMessageField * cRosMessageFieldNew(void) -{ - cRosMessageField *ret = (cRosMessageField *)calloc(1, sizeof(cRosMessageField)); - cRosMessageFieldInit(ret); - return ret; -} - -void cRosMessageFieldRelease(cRosMessageField *field) -{ - free(field->name); - field->name = NULL; - - free(field->type_s); - field->type_s = NULL; - - if(field->is_array) - { - if(field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_STRING && - field->type != CROS_STD_MSGS_TIME && field->type != CROS_STD_MSGS_DURATION && - field->type != CROS_STD_MSGS_HEADER) - { - free(field->data.as_array); - field->data.as_array = NULL; - } - else - { - int elem_ind; - for(elem_ind = 0; elem_ind < field->array_size; elem_ind++) - { - if(field->type == CROS_STD_MSGS_STRING) - free(field->data.as_string_array[elem_ind]); - else - cRosMessageFree(field->data.as_msg_array[elem_ind]); - } - free(field->data.as_array); - field->data.as_array = NULL; - } - } - else - { - if(field->type == CROS_STD_MSGS_STRING) - { - free(field->data.as_string); - field->data.as_string=NULL; - } - else if(field->type == CROS_CUSTOM_TYPE || field->type == CROS_STD_MSGS_TIME || - field->type == CROS_STD_MSGS_DURATION || field->type == CROS_STD_MSGS_HEADER) - { - cRosMessageFree(field->data.as_msg); - field->data.as_msg=NULL; - } - } -} - -void cRosMessageFieldFree(cRosMessageField *field) -{ - if (field == NULL) - return; - - cRosMessageFieldRelease(field); - free(field); -} - -cRosMessageField *cRosMessageGetField(cRosMessage *message, const char *field_name) -{ - cRosMessageField* matching_field = NULL; - - int i; - for(i = 0; i < message->n_fields; i++) - { - cRosMessageField* curr_field = message->fields[i]; - if(strcmp(curr_field->name, field_name) == 0) - { - matching_field = curr_field; - break; - } - } - - return matching_field; -} - -int cRosMessageSetFieldValueString(cRosMessageField* field, const char* value) -{ - int ret; - if (field->type != CROS_STD_MSGS_STRING) - return -1; - - size_t str_len = strlen(value); - field->data.as_string = (char *)realloc(field->data.as_string, sizeof(char)*(str_len+1)); - if(field->data.as_string != NULL) - { - strcpy(field->data.as_string, value); - field->size = (int)str_len; - ret=0; // Success - } - else - ret=-1; - return ret; -} - -int arrayFieldValuesPushBack(cRosMessageField *field, const void* data, int element_size, int n_new_elements) -{ - if(field == NULL || !field->is_array || field->is_fixed_array) - return -1; - - if(field->array_capacity < field->array_size + n_new_elements) - { - void *new_location; - size_t new_arr_cap; - - new_arr_cap = (field->array_size + n_new_elements) * 3 / 2; - new_location = realloc(field->data.as_array, new_arr_cap * element_size); // If field->data.as_array is NULL, realloc() behaves as malloc() - if(new_location != NULL) - { - field->data.as_array = new_location; - field->array_capacity = (int)new_arr_cap; - } - else - { - return -1; - } - } - - memcpy((void *)((char *)field->data.as_array + field->array_size*element_size), data, element_size * n_new_elements); - field->array_size += n_new_elements; - return 0; -} - -int arrayFieldValuePushBack(cRosMessageField *field, const void* data, int element_size) -{ - return arrayFieldValuesPushBack(field, data, element_size, 1); -} - -int cRosMessageFieldArrayPushBackInt8(cRosMessageField *field, int8_t val) -{ - if(field->type != CROS_STD_MSGS_INT8) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_INT8)); -} - -int cRosMessageFieldArrayPushBackInt16(cRosMessageField *field, int16_t val) -{ - if(field->type != CROS_STD_MSGS_INT16) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_INT16)); -} - -int cRosMessageFieldArrayPushBackInt32(cRosMessageField *field, int32_t val) -{ - if(field->type != CROS_STD_MSGS_INT32) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_INT32)); -} - -int cRosMessageFieldArrayPushBackInt64(cRosMessageField *field, int64_t val) -{ - if(field->type != CROS_STD_MSGS_INT64) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_INT64)); - -} - -int cRosMessageFieldArrayPushBackUInt8(cRosMessageField *field, uint8_t val) -{ - if(field->type != CROS_STD_MSGS_UINT8) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_UINT8)); -} - -int cRosMessageFieldArrayPushBackUInt16(cRosMessageField *field, uint16_t val) -{ - if(field->type != CROS_STD_MSGS_UINT16) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_UINT16)); -} - -int cRosMessageFieldArrayPushBackUInt32(cRosMessageField *field, uint32_t val) -{ - if(field->type != CROS_STD_MSGS_UINT32) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_UINT32)); -} - -int cRosMessageFieldArrayPushBackUint64(cRosMessageField *field, uint64_t val) -{ - if(field->type != CROS_STD_MSGS_UINT64) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_UINT64)); -} - -int cRosMessageFieldArrayPushBackFloat32(cRosMessageField *field, float val) -{ - if(field->type != CROS_STD_MSGS_FLOAT32) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_FLOAT32)); -} - -int cRosMessageFieldArrayPushBackFloat64(cRosMessageField *field, double val) -{ - if(field->type != CROS_STD_MSGS_FLOAT64) - return -1; - - return arrayFieldValuePushBack(field, &val, getMessageTypeSizeOf(CROS_STD_MSGS_FLOAT64)); -} - -int cRosMessageFieldArrayPushBackString(cRosMessageField *field, const char* val) -{ - if(field->type != CROS_STD_MSGS_STRING) - return -1; - - if(!field->is_array || field->is_fixed_array) - return -1; - - if(field->array_capacity == field->array_size) - { - char** new_location; - new_location = (char **)realloc(field->data.as_string_array, 2 * field->array_capacity * sizeof(char*)); - if(new_location != NULL) - { - field->data.as_string_array = new_location; - field->array_capacity *= 2; - } - else - { - return -1; - } - } - int ret; - char* element_val; - ret=0; - if(val != NULL) - { - element_val = (char *)calloc(strlen(val) + sizeof(char), 1); - if(element_val != NULL) - strcpy(element_val, val); - else - ret=-1; - } - else - element_val = NULL; - if(ret == 0) - { - field->data.as_string_array[field->array_size] = element_val; - field->array_size ++; - } - return ret; -} - -// Add a new blank element at the end of the array in field n_field of message msg -int cRosMessageFieldArrayPushBackZero(cRosMessage* msg, int n_field) -{ - cRosMessageField *field; - CrosMessageType elem_type; - size_t elem_size; - - if(n_field >= msg->n_fields || n_field < 0) - return -1; - - field = msg->fields[n_field]; - - if(!field->is_array || field->is_fixed_array) - return -1; - - elem_type = field->type; - elem_size = getMessageTypeSizeOf(elem_type); - if(field->array_capacity == field->array_size) - { - void *new_location; - new_location = realloc(field->data.as_array, 2 * field->array_capacity * elem_size); - if(new_location != NULL) - { - field->data.as_array = new_location; - field->array_capacity *= 2; - } - else - { - return -1; - } - } - - switch(elem_type) - { - case CROS_STD_MSGS_INT8: - case CROS_STD_MSGS_UINT8: - case CROS_STD_MSGS_INT16: - case CROS_STD_MSGS_UINT16: - case CROS_STD_MSGS_INT32: - case CROS_STD_MSGS_UINT32: - case CROS_STD_MSGS_INT64: - case CROS_STD_MSGS_UINT64: - case CROS_STD_MSGS_FLOAT32: - case CROS_STD_MSGS_FLOAT64: - case CROS_STD_MSGS_BOOL: - case CROS_STD_MSGS_CHAR: - case CROS_STD_MSGS_BYTE: - case CROS_STD_MSGS_STRING: - { - char *elem_array = field->data.as_array; - memset(elem_array + elem_size*field->array_size, 0, elem_size); - break; - } - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_HEADER: - case CROS_CUSTOM_TYPE: - { - if(msg->msgDef != NULL) - { - msgFieldDef* field_def_itr; - int field_ind; - cRosMessage **msg_array; - cRosErrCodePack ret_err; - - // Look for the definition corresponding to the specified field - field_def_itr = msg->msgDef->first_field; - for(field_ind=0;field_ind < n_field && field_def_itr != NULL;field_ind++) - field_def_itr = field_def_itr->next; - - if(field_def_itr == NULL) - return -1; // msg definition not found - - msg_array = field->data.as_msg_array; - ret_err = cRosMessageBuildFromDef(&msg_array[field->array_size], field_def_itr->child_msg_def); // instead of cRosMessageNewBuild(msg->msgDef->root_dir, field->type_s, &msg_array[field->array_size]); - if(ret_err != CROS_SUCCESS_ERR_PACK) - return -1; // Error creating the message - } - else - return -1; // msgDef not available in current message, we don't know the type of the message to build - } - default: - break; - } - - field->array_size ++; - return 0; -} - -int cRosMessageFieldArrayPushBackMsg(cRosMessageField *field, cRosMessage* msg) -{ - if(field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_TIME && - field->type != CROS_STD_MSGS_DURATION && field->type != CROS_STD_MSGS_HEADER) - return -1; - - if(!field->is_array || field->is_fixed_array) - return -1; - - if(field->array_capacity == field->array_size) - { - cRosMessage **new_location; - new_location = (cRosMessage **)realloc(field->data.as_msg_array, 2 * field->array_capacity * sizeof(cRosMessage*)); - if(new_location != NULL) - { - field->data.as_msg_array = new_location; - field->array_capacity *= 2; - } - else - { - return -1; - } - } - - field->data.as_msg_array[field->array_size] = msg; - field->array_size ++; - return 0; -} - -cRosMessage *cRosMessageFieldArrayRemoveLastMsg(cRosMessageField *field) -{ - if(field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_TIME && - field->type != CROS_STD_MSGS_DURATION && field->type != CROS_STD_MSGS_HEADER) - return NULL; - - if(!field->is_array || field->is_fixed_array) - return NULL; - - cRosMessage *msg; - - if(field->array_size > 0) - { - msg = field->data.as_msg_array[field->array_size-1]; - field->array_size--; - } - else - msg=NULL; // Array empty - - return msg; -} - -int8_t *cRosMessageFieldArrayAtInt8(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_INT8) - return NULL; - - return (int8_t *)arrayFieldValueAt(field, position, sizeof(int8_t)); -} - -int16_t *cRosMessageFieldArrayAtInt16(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_INT16) - return NULL; - - return (int16_t *)arrayFieldValueAt(field, position, sizeof(int16_t)); -} - -int32_t *cRosMessageFieldArrayAtInt32(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_INT32) - return NULL; - - return (int32_t *)arrayFieldValueAt(field, position, sizeof(int32_t)); -} - -int64_t *cRosMessageFieldArrayAtInt64(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_INT64) - return NULL; - - return (int64_t *)arrayFieldValueAt(field, position, sizeof(int64_t)); -} - -uint8_t *cRosMessageFieldArrayAtUInt8(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_UINT8) - return NULL; - - return (uint8_t *)arrayFieldValueAt(field, position, sizeof(uint8_t)); -} - -uint16_t *cRosMessageFieldArrayAtUInt16(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_UINT16) - return NULL; - - return (uint16_t *)arrayFieldValueAt(field, position, sizeof(uint16_t)); -} - -uint32_t *cRosMessageFieldArrayAtUInt32(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_UINT32) - return NULL; - - return (uint32_t *)arrayFieldValueAt(field, position, sizeof(uint32_t)); -} - -uint64_t *cRosMessageFieldArrayAtUInt64(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_UINT64) - return NULL; - - return (uint64_t *)arrayFieldValueAt(field, position, sizeof(uint64_t)); -} - -float *cRosMessageFieldArrayAtFloat32(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_FLOAT32) - return NULL; - - return (float *)arrayFieldValueAt(field, position, sizeof(float)); -} - -double *cRosMessageFieldArrayAtFloat64(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_FLOAT64) - return NULL; - - return (double *)arrayFieldValueAt(field, position, sizeof(double)); -} - -cRosMessage *cRosMessageFieldArrayAtMsgGet(cRosMessageField *field, int position) -{ - if(field == NULL || (field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_TIME && - field->type != CROS_STD_MSGS_DURATION && field->type != CROS_STD_MSGS_HEADER) || !field->is_array) - return NULL; - - if(position >= field->array_size) - return NULL; - - cRosMessage* msg = field->data.as_msg_array[position]; - return msg; -} - -int cRosMessageFieldArrayAtMsgSet(cRosMessageField *field, int position, cRosMessage* msg) -{ - int ret; - if((field->type != CROS_CUSTOM_TYPE && field->type != CROS_STD_MSGS_TIME && - field->type != CROS_STD_MSGS_DURATION && field->type != CROS_STD_MSGS_HEADER) || !field->is_array) - return -1; - - if(position > field->array_size) - return -1; - - if(position == field->array_size) - ret = cRosMessageFieldArrayPushBackMsg(field, msg); - else - { - cRosMessageFree(field->data.as_msg_array[position]); - field->data.as_msg_array[position] = msg; - ret = 0; - } - - return ret; -} - -char *cRosMessageFieldArrayAtStringGet(cRosMessageField *field, int position) -{ - if(field->type != CROS_STD_MSGS_STRING || !field->is_array) - return NULL; - - char* str = (char*) field->data.as_string_array[position]; - return str; -} - -int cRosMessageFieldArrayAtStringSet(cRosMessageField *field, int position, const char* val) -{ - int ret; - if(field->type != CROS_STD_MSGS_STRING || !field->is_array) - return -1; - - char *current = field->data.as_string_array[position]; - ret = 0; // Default return value: success - if(val != NULL) - { - current = (char *)realloc(current, strlen(val) + sizeof(char)); // Realloc buffer for the string - if(current != NULL) - strcpy(current, val); - else - ret=-1; - } - else - { - if(current != NULL) - { - free(current); - current = NULL; - } - } - - field->data.as_string_array[position] = current; - - return ret; -} - -int cRosMessageFieldArrayClear(cRosMessageField *field) -{ - int n_elem; - if(!field->is_array || field->is_fixed_array) - return -1; - - for(n_elem=0;n_elemarray_size;n_elem++) - { - switch(field->type) - { - case CROS_STD_MSGS_STRING: - { - free(field->data.as_string_array[n_elem]); - break; - } - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_HEADER: - case CROS_CUSTOM_TYPE: - { - cRosMessageFree(field->data.as_msg_array[n_elem]); - break; - } - default: - break; - } - } - - field->array_size = 0; - - return 0; -} - -void *arrayFieldValueAt(cRosMessageField *field, int position, size_t size) -{ - if(!field->is_array) - return NULL; - - if (position < 0 || position > field->array_size) - return NULL; - - return((void *)((char *)field->data.as_array + (position * size))); // sizeof(char) is 1 (byte), so we cast to char * before calculating the array field position -} - -size_t cRosMessageFieldSize(cRosMessageField* field) -{ - size_t single_size = 0; - - if (isBuiltinMessageType(field->type)) - { - single_size = field->size; - } - else - { - single_size = cRosMessageSize(field->data.as_msg); - } - - size_t ret = 0; - if(field->is_array) - { - ret = single_size * field->array_size; - } - else - { - ret = single_size; - } - return ret; -} - -size_t cRosMessageSize(cRosMessage* message) -{ - size_t ret = 0; - cRosMessageField** fields_it = message->fields; - int i; - for( i = 0; i < message->n_fields; i++) - { - cRosMessageField* field = *fields_it; - ret += cRosMessageFieldSize(field); - fields_it++; - } - - return ret;// + sizeof(uint32_t); -} - -cRosErrCodePack cRosMessageSerialize(cRosMessage *message, DynBuffer* buffer) -{ - cRosErrCodePack ret_err; - int field_ind; - - ret_err = CROS_SUCCESS_ERR_PACK; // default error value: success - for (field_ind = 0; field_ind < message->n_fields && ret_err == CROS_SUCCESS_ERR_PACK; field_ind++) - { - cRosMessageField *field = message->fields[field_ind]; - - if(field->is_array && !field->is_fixed_array) - ret_err = (dynBufferPushBackInt32(buffer, field->array_size) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - - switch (field->type) - { - case CROS_STD_MSGS_INT8: - case CROS_STD_MSGS_UINT8: - case CROS_STD_MSGS_INT16: - case CROS_STD_MSGS_UINT16: - case CROS_STD_MSGS_INT32: - case CROS_STD_MSGS_UINT32: - case CROS_STD_MSGS_INT64: - case CROS_STD_MSGS_UINT64: - case CROS_STD_MSGS_FLOAT32: - case CROS_STD_MSGS_FLOAT64: - case CROS_STD_MSGS_BOOL: - case CROS_STD_MSGS_CHAR: - case CROS_STD_MSGS_BYTE: - { - size_t size = getMessageTypeSizeOf(field->type); - if (field->is_array) - ret_err = (dynBufferPushBackBuf(buffer, field->data.as_array, size * field->array_size) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - else - ret_err = (dynBufferPushBackBuf(buffer, field->data.opaque, size) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - break; - } - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_HEADER: - case CROS_CUSTOM_TYPE: - { - if(field->is_array) - { - int elem_ind; - for(elem_ind = 0; elem_ind < field->array_size && ret_err == CROS_SUCCESS_ERR_PACK; elem_ind++) - { - cRosMessage *arr_elem_msg; - arr_elem_msg = cRosMessageFieldArrayAtMsgGet(field, elem_ind); - if(arr_elem_msg != NULL) - ret_err = cRosMessageSerialize(arr_elem_msg, buffer); - } - } - else - { - if(field->data.as_msg != NULL) - ret_err = cRosMessageSerialize(field->data.as_msg, buffer); - } - break; - } - case CROS_STD_MSGS_STRING: - { - if(field->is_array) - { - int elem_ind; - for(elem_ind = 0; elem_ind < field->array_size && ret_err == CROS_SUCCESS_ERR_PACK; elem_ind++) - { - const char* arr_elem_str; - size_t arr_elem_str_len; - arr_elem_str = cRosMessageFieldArrayAtStringGet(field, elem_ind); - if(arr_elem_str != NULL) - arr_elem_str_len = strlen(arr_elem_str); - else - arr_elem_str_len = 0; - dynBufferPushBackInt32(buffer, arr_elem_str_len); - ret_err = (dynBufferPushBackBuf(buffer, (unsigned char *)arr_elem_str, arr_elem_str_len) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - } - } - else - { - if(field->data.as_string != NULL) - { - size_t str_len = strlen(field->data.as_string); - dynBufferPushBackInt32(buffer, str_len); - ret_err = (dynBufferPushBackBuf(buffer,(unsigned char*)field->data.as_string, str_len) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - } - else - ret_err = (dynBufferPushBackInt32(buffer, 0) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - } - break; - } - default: - { - ret_err = CROS_BAD_PARAM_ERR; - break; - } - } - } - return ret_err; -} - -// In this function we assume that the message is already build according to its definition. -// Only when receiving a variable-length array, new elements if the message field may need to be created -cRosErrCodePack cRosMessageDeserialize(cRosMessage *message, DynBuffer* buffer) -{ - int field_ind; - cRosErrCodePack ret_err; - - ret_err = CROS_SUCCESS_ERR_PACK; // default error value: no error - - msgFieldDef* field_def_itr = (message->msgDef != NULL)? message->msgDef->first_field : NULL; // Keep track of the message definition (if available) corresponding to the current message field in case we need to build a msg of type custom - - for (field_ind = 0; field_ind < message->n_fields && ret_err == CROS_SUCCESS_ERR_PACK; field_ind++) - { - cRosMessageField *field = message->fields[field_ind]; - - switch (field->type) - { - case CROS_STD_MSGS_INT8: - case CROS_STD_MSGS_UINT8: - case CROS_STD_MSGS_INT16: - case CROS_STD_MSGS_UINT16: - case CROS_STD_MSGS_INT32: - case CROS_STD_MSGS_UINT32: - case CROS_STD_MSGS_INT64: - case CROS_STD_MSGS_UINT64: - case CROS_STD_MSGS_FLOAT32: - case CROS_STD_MSGS_FLOAT64: - case CROS_STD_MSGS_BOOL: - case CROS_STD_MSGS_CHAR: - case CROS_STD_MSGS_BYTE: - { - size_t elem_size = getMessageTypeSizeOf(field->type); - if (field->is_array) - { - if (field->is_fixed_array) - { - ret_err = (dynBufferGetCurrentContent( field->data.as_array, buffer, elem_size * field->array_size ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: memcpy(field->data.as_array, dynBufferGetCurrentData(buffer), size * field->array_size); - dynBufferMovePoseIndicator(buffer, elem_size * field->array_size); - } - else - { - uint32_t array_n_elems; - cRosMessageFieldArrayClear(field); - ret_err = (dynBufferGetCurrentContent( (unsigned char *)&array_n_elems, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: array_n_elems = *((uint32_t*)dynBufferGetCurrentData(buffer)); - dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - if(dynBufferGetRemainingDataSize(buffer) >= array_n_elems*elem_size) - { - ret_err = (arrayFieldValuesPushBack(field, (const void *)dynBufferGetCurrentData(buffer), elem_size, array_n_elems) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - dynBufferMovePoseIndicator(buffer, array_n_elems*elem_size); - } - else - ret_err = CROS_DEPACK_INSUFF_DAT_ERR; // Not enough data available in the packet buffer - } - } - } - else - { - ret_err = (dynBufferGetCurrentContent( field->data.opaque, buffer, elem_size ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: memcpy(field->data.opaque, dynBufferGetCurrentData(buffer), elem_size); - dynBufferMovePoseIndicator(buffer, elem_size); - } - - break; - } - case CROS_STD_MSGS_STRING: - { - if (field->is_array) - { - uint32_t curr_array_size; - if(field->is_fixed_array) // If it is a fixed array, use the fixed array size - curr_array_size = field->array_size; - else // Otherwise, obtain the number of elements of the received array - { - cRosMessageFieldArrayClear(field); // Remove previous strings from the array to start added the new ones one by one - ret_err = (dynBufferGetCurrentContent( (unsigned char *)&curr_array_size, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: curr_array_size = *((uint32_t*)dynBufferGetCurrentData(buffer)); - dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); - } - - uint32_t elem_ind; - for(elem_ind = 0; elem_ind < curr_array_size && ret_err == CROS_SUCCESS_ERR_PACK; elem_ind++) - { - uint32_t element_size; - ret_err = (dynBufferGetCurrentContent( (unsigned char *)&element_size, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: element_size = *((uint32_t*)dynBufferGetCurrentData(buffer)); - dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - char* tmp_string = (char *)calloc(element_size + 1, sizeof(char)); - if(tmp_string != NULL) - { - ret_err = (dynBufferGetCurrentContent( (unsigned char *)tmp_string, buffer, element_size ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: memcpy(tmp_string, dynBufferGetCurrentData(buffer), element_size); - if(field->is_fixed_array) - ret_err = (cRosMessageFieldArrayAtStringSet(field, elem_ind, tmp_string) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - else - ret_err = (cRosMessageFieldArrayPushBackString(field, tmp_string) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - dynBufferMovePoseIndicator(buffer, element_size); - free(tmp_string); - } - else - ret_err=CROS_MEM_ALLOC_ERR; - } - } - } - else - { - uint32_t curr_data_size; - ret_err = (dynBufferGetCurrentContent( (unsigned char *)&curr_data_size, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; // equiv. to: curr_data_size = *((uint32_t*)dynBufferGetCurrentData(buffer)); - dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); - field->data.as_string = (char *)realloc(field->data.as_string, (curr_data_size + 1) * sizeof(char)); // If field->data.as_string was NULL previously, realloc behaves as malloc - if(field->data.as_string != NULL) - { - dynBufferGetCurrentContent( (unsigned char *)field->data.as_string, buffer, curr_data_size ); // equiv. to: memcpy(field->data.as_string, dynBufferGetCurrentData(buffer), curr_data_size); - field->data.as_string[curr_data_size] = '\0'; - dynBufferMovePoseIndicator(buffer, curr_data_size); - } - else - ret_err=CROS_MEM_ALLOC_ERR; - } - break; - } - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_HEADER: - case CROS_CUSTOM_TYPE: - { - if (field->is_array) - { - uint32_t received_arr_siz; - uint32_t msg_ind; - - if(field->is_fixed_array) // If it is a fixed array, we use the fixed array size - received_arr_siz = field->array_size; - else // Otherwise, obtain the number of array elements from the received packet - { - ret_err = (dynBufferGetCurrentContent( (unsigned char *)&received_arr_siz, buffer, sizeof(uint32_t) ) >= 0)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - dynBufferMovePoseIndicator(buffer, sizeof(uint32_t)); - - // Adapt the array size of the message field to the received array length - for(msg_ind = field->array_size;msg_ind < received_arr_siz && ret_err == CROS_SUCCESS_ERR_PACK;msg_ind++) // received more elements than the available messages: create more - { - cRosMessage *new_msg; - new_msg = NULL; - if(field->type == CROS_STD_MSGS_TIME) - new_msg = build_time_field(); - else if(field->type == CROS_STD_MSGS_DURATION) - new_msg = build_duration_field(); - else if(field->type == CROS_STD_MSGS_HEADER) - new_msg = build_header_field(); - else if(field->type == CROS_CUSTOM_TYPE) - { - if(field_def_itr != NULL) - ret_err = cRosMessageBuildFromDef(&new_msg, field_def_itr->child_msg_def); // instead of cRosMessageNewBuild(message->msgDef->root_dir, field->type_s, &new_msg); - else - ret_err =CROS_DEPACK_NO_MSG_DEF_ERR; - } - - if(new_msg != NULL) - cRosMessageFieldArrayPushBackMsg(field, new_msg); // new_msg is now used in the array so it cannot be freed independently - else - { - if(ret_err == CROS_SUCCESS_ERR_PACK) - ret_err=CROS_MEM_ALLOC_ERR; - } - } - - while(field->array_size > (int)received_arr_siz) // received less elements than the available messages: delete - cRosMessageFree(cRosMessageFieldArrayRemoveLastMsg(field)); - } - - for(msg_ind = 0;(int)msg_ind < field->array_size && ret_err == CROS_SUCCESS_ERR_PACK;msg_ind++) - { - cRosMessage *curr_msg; - curr_msg = cRosMessageFieldArrayAtMsgGet(field, msg_ind); - ret_err = cRosMessageDeserialize(curr_msg, buffer); - } - } - else - ret_err = cRosMessageDeserialize(field->data.as_msg, buffer); - break; - } - default: - { - ret_err=CROS_BAD_PARAM_ERR; - break; - } - } - field_def_itr = (field_def_itr != NULL)? field_def_itr->next : NULL; - } - return ret_err; -} - -const char *getMessageTypeDeclarationConst(msgConst *msgConst) -{ - if (msgConst->type_s == NULL) - return getMessageTypeDeclaration(msgConst->type); - else - return msgConst->type_s; -} - -const char *getMessageTypeDeclarationField(msgFieldDef *fieldDef) -{ - if (fieldDef->type_s == NULL) - return getMessageTypeDeclaration(fieldDef->type); - else - return fieldDef->type_s; -} - -int isBuiltinMessageType(CrosMessageType type) -{ - switch(type) - { - case CROS_STD_MSGS_INT8: - case CROS_STD_MSGS_UINT8: - case CROS_STD_MSGS_INT16: - case CROS_STD_MSGS_UINT16: - case CROS_STD_MSGS_INT32: - case CROS_STD_MSGS_UINT32: - case CROS_STD_MSGS_INT64: - case CROS_STD_MSGS_UINT64: - case CROS_STD_MSGS_FLOAT32: - case CROS_STD_MSGS_FLOAT64: - case CROS_STD_MSGS_BOOL: - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_CHAR: - case CROS_STD_MSGS_BYTE: - case CROS_STD_MSGS_STRING: - return 1; - default: - return 0; - } -} - -size_t getMessageTypeSizeOf(CrosMessageType type) -{ - switch(type) - { - case CROS_STD_MSGS_INT8: - case CROS_STD_MSGS_UINT8: - return sizeof(int8_t); - case CROS_STD_MSGS_INT16: - case CROS_STD_MSGS_UINT16: - return sizeof(int16_t); - case CROS_STD_MSGS_INT32: - case CROS_STD_MSGS_UINT32: - return sizeof(int32_t); - case CROS_STD_MSGS_INT64: - case CROS_STD_MSGS_UINT64: - return sizeof(int64_t); - case CROS_STD_MSGS_FLOAT32: - return sizeof(float); - case CROS_STD_MSGS_FLOAT64: - return sizeof(double); - case CROS_STD_MSGS_BOOL: - return sizeof(int8_t); - case CROS_STD_MSGS_STRING: - return sizeof(char *); - case CROS_STD_MSGS_TIME: - case CROS_STD_MSGS_DURATION: - case CROS_STD_MSGS_HEADER: - case CROS_CUSTOM_TYPE: - return sizeof(cRosMessage *); - // deprecated - case CROS_STD_MSGS_CHAR: - case CROS_STD_MSGS_BYTE: - return sizeof(char); - default: - PRINT_ERROR ( "getMessageTypeSizeOf() : Invalid CrosMessageType specified\n" ); - return 0; - } -} - -CrosMessageType getMessageType(const char *type_s) -{ - if (strcmp(type_s, "int8") == 0 || strcmp(type_s, "std_msgs/Int8") == 0) - return CROS_STD_MSGS_INT8; - else if (strcmp(type_s, "uint8") == 0 || strcmp(type_s, "std_msgs/UInt8") == 0) - return CROS_STD_MSGS_UINT8; - else if (strcmp(type_s, "int16") == 0 || strcmp(type_s, "std_msgs/Int16") == 0) - return CROS_STD_MSGS_INT16; - else if (strcmp(type_s, "uint16") == 0 || strcmp(type_s, "std_msgs/UInt16") == 0) - return CROS_STD_MSGS_UINT16; - else if (strcmp(type_s, "int32") == 0 || strcmp(type_s, "std_msgs/Int32") == 0) - return CROS_STD_MSGS_INT32; - else if (strcmp(type_s, "uint32") == 0 || strcmp(type_s, "std_msgs/UInt32") == 0) - return CROS_STD_MSGS_UINT32; - else if (strcmp(type_s, "int64") == 0 || strcmp(type_s, "std_msgs/Int64") == 0) - return CROS_STD_MSGS_INT64; - else if (strcmp(type_s, "uint64") == 0 || strcmp(type_s, "std_msgs/UInt64") == 0) - return CROS_STD_MSGS_UINT64; - else if (strcmp(type_s, "float32") == 0 || strcmp(type_s, "std_msgs/Float32") == 0) - return CROS_STD_MSGS_FLOAT32; - else if (strcmp(type_s, "float64") == 0 || strcmp(type_s, "std_msgs/Float64") == 0) - return CROS_STD_MSGS_FLOAT64; - else if (strcmp(type_s, "string") == 0 || strcmp(type_s, "std_msgs/String") == 0) - return CROS_STD_MSGS_STRING; - else if (strcmp(type_s, "bool") == 0 || strcmp(type_s, "std_msgs/Bool") == 0) - return CROS_STD_MSGS_BOOL; - else if (strcmp(type_s, "time") == 0 || strcmp(type_s, "std_msgs/Time") == 0) - return CROS_STD_MSGS_TIME; - else if (strcmp(type_s, "duration") == 0 || strcmp(type_s, "std_msgs/Duration") == 0) - return CROS_STD_MSGS_DURATION; - else if (strcmp(type_s, "char") == 0 || strcmp(type_s, "std_msgs/Char") == 0) // deprecated - return CROS_STD_MSGS_CHAR; - else if (strcmp(type_s, "byte") == 0 || strcmp(type_s, "std_msgs/Byte") == 0) // deprecated - return CROS_STD_MSGS_BYTE; - else if (strcmp(type_s, "std_msgs/Header") == 0 || (strcmp(type_s, "Header") == 0) - || (strcmp(type_s, "roslib/Header") == 0)) - return CROS_STD_MSGS_HEADER; - else - return CROS_CUSTOM_TYPE; -} - -const char *getMessageTypeString(CrosMessageType type) -{ - switch(type) - { - case CROS_STD_MSGS_INT8: - return "std_msgs/UInt8"; - case CROS_STD_MSGS_UINT8: - return "std_msgs/UInt8"; - case CROS_STD_MSGS_INT16: - return "std_msgs/Int16"; - case CROS_STD_MSGS_UINT16: - return "std_msgs/UInt16"; - case CROS_STD_MSGS_INT32: - return "std_msgs/Int32"; - case CROS_STD_MSGS_UINT32: - return "std_msgs/UInt32"; - case CROS_STD_MSGS_INT64: - return "std_msgs/Int64"; - case CROS_STD_MSGS_UINT64: - return "std_msgs/UInt64"; - case CROS_STD_MSGS_FLOAT32: - return "std_msgs/Float32"; - case CROS_STD_MSGS_FLOAT64: - return "std_msgs/Float64"; - case CROS_STD_MSGS_STRING: - return "std_msgs/String"; - case CROS_STD_MSGS_BOOL: - return "std_msgs/Bool"; - case CROS_STD_MSGS_TIME: - return "std_msgs/Time"; - case CROS_STD_MSGS_DURATION: - return "std_msgs/Duration"; - case CROS_STD_MSGS_CHAR: // deprecated - return "std_msgs/Char"; - case CROS_STD_MSGS_BYTE: // deprecated - return "std_msgs/Byte"; - case CROS_STD_MSGS_HEADER: - return "std_msgs/Header"; - default: - PRINT_ERROR ( "getMessageTypeString() : Invalid CrosMessageType specified\n" ); - return NULL; - } -} - -const char *getMessageTypeDeclaration(CrosMessageType type) -{ - switch(type) - { - case CROS_STD_MSGS_INT8: - return "int8"; - case CROS_STD_MSGS_UINT8: - return "uint8"; - case CROS_STD_MSGS_INT16: - return "int16"; - case CROS_STD_MSGS_UINT16: - return "uint16"; - case CROS_STD_MSGS_INT32: - return "int32"; - case CROS_STD_MSGS_UINT32: - return "uint32"; - case CROS_STD_MSGS_INT64: - return "int64"; - case CROS_STD_MSGS_UINT64: - return "uint64"; - case CROS_STD_MSGS_FLOAT32: - return "float32"; - case CROS_STD_MSGS_FLOAT64: - return "float64"; - case CROS_STD_MSGS_STRING: - return "string"; - case CROS_STD_MSGS_BOOL: - return "bool"; - case CROS_STD_MSGS_TIME: - return "time"; - case CROS_STD_MSGS_DURATION: - return "duration"; - case CROS_STD_MSGS_CHAR: // deprecated - return "char"; - case CROS_STD_MSGS_BYTE: // deprecated - return "byte"; - case CROS_STD_MSGS_HEADER: - return "header"; - case CROS_CUSTOM_TYPE: - return "custom"; - default: - PRINT_ERROR ( "getMessageTypeString() : Invalid CrosMessageType specified\n" ); - return NULL; - } -} diff --git a/src/hal/components/cros/src/cros_message_queue.c b/src/hal/components/cros/src/cros_message_queue.c deleted file mode 100644 index 4a3d45e5..00000000 --- a/src/hal/components/cros/src/cros_message_queue.c +++ /dev/null @@ -1,138 +0,0 @@ -#include -#include - -#include "cros_message_queue.h" - -void cRosMessageQueueInit(cRosMessageQueue *q) -{ - unsigned int msg_ind; - // Initialize all messages in the queue so that the inserted messages only need to be copied over these ones - for(msg_ind=0;msg_indmsgs[msg_ind]); - q->length = 0; - q->first_msg_ind = 0; -} - -void cRosMessageQueueClear(cRosMessageQueue *q) -{ - // Empty the queue - while(q->length > 0) - { - // Delete fields from message to remove - cRosMessageFieldsFree(&q->msgs[q->first_msg_ind]); - // The queue is internally implemented as a circular buffer - q->first_msg_ind = (q->first_msg_ind + 1) % MAX_QUEUE_LEN; - q->length--; - } - q->first_msg_ind = 0; -} - -unsigned int cRosMessageQueueVacancies(cRosMessageQueue *q) -{ - return MAX_QUEUE_LEN - q->length; -} - -unsigned int cRosMessageQueueUsage(cRosMessageQueue *q) -{ - return q->length; -} - -void cRosMessageQueueRelease(cRosMessageQueue *q) -{ - unsigned int msg_ind; - cRosMessageQueueClear(q); - // Release the memory of all container messages in the queue - for(msg_ind=0;msg_indmsgs[msg_ind]); -} - -int cRosMessageQueueAdd(cRosMessageQueue *q, cRosMessage *m) -{ - int ret; - if(q->length < MAX_QUEUE_LEN) - { - unsigned int next_msg_pos; - // The queue is internally implemented as a circular buffer - next_msg_pos = (q->first_msg_ind + q->length) % MAX_QUEUE_LEN; - ret = cRosMessageFieldsCopy(&q->msgs[next_msg_pos], m); - q->length++; - } - else - ret=-2; - - return ret; -} - -int cRosMessageQueueGet(cRosMessageQueue *q, cRosMessage *m) -{ - int ret; - if(q->length > 0) - { - cRosMessage *msg_to_peek; - msg_to_peek = &q->msgs[q->first_msg_ind]; - ret = cRosMessageFieldsCopy(m, msg_to_peek); - } - else - ret=-2; - - return ret; -} - -int cRosMessageQueueRemove(cRosMessageQueue *q) -{ - int ret; - - if(q->length > 0) - { - cRosMessage *msg_to_remove; - msg_to_remove = &q->msgs[q->first_msg_ind]; - // Delete fields from removed message - cRosMessageFieldsFree(msg_to_remove); - // The queue is internally implemented as a circular buffer - q->first_msg_ind = (q->first_msg_ind + 1) % MAX_QUEUE_LEN; - q->length--; - ret=0; - } - else - ret=-2; - - return ret; -} - -int cRosMessageQueueExtract(cRosMessageQueue *q, cRosMessage *m) -{ - int ret; - - ret = cRosMessageQueueGet(q, m); - if(ret == 0) - ret = cRosMessageQueueRemove(q); - - return ret; -} - -cRosMessage *cRosMessageQueuePeekFirst(cRosMessageQueue *q) -{ - cRosMessage *first_msg; - if(q->length > 0) - first_msg = &q->msgs[q->first_msg_ind]; - else - first_msg = NULL; - - return first_msg; -} - -cRosMessage *cRosMessageQueuePeekLast(cRosMessageQueue *q) -{ - cRosMessage *last_msg; - if(q->length > 0) - { - unsigned int last_msg_pos; - // The queue is internally implemented as a circular buffer - last_msg_pos = (q->first_msg_ind + q->length - 1) % MAX_QUEUE_LEN; - last_msg = &q->msgs[last_msg_pos]; - } - else - last_msg = NULL; - - return last_msg; -} diff --git a/src/hal/components/cros/src/cros_node.c b/src/hal/components/cros/src/cros_node.c deleted file mode 100644 index e04ec739..00000000 --- a/src/hal/components/cros/src/cros_node.c +++ /dev/null @@ -1,4005 +0,0 @@ -#include -#include -#include -#include - -#ifdef _WIN32 -# define WIN32_LEAN_AND_MEAN // This define speeds up the build process by excluding some parts of the Windows header -# include -#else -# include -#endif - -#include "cros_node.h" -#include "cros_api_internal.h" -#include "cros_api.h" -#include "cros_message.h" -#include "cros_clock.h" -#include "cros_defs.h" -#include "cros_node_api.h" -#include "cros_tcpros.h" -#include "tcpip_socket.h" -#include "cros_log.h" - -static void initPublisherNode(PublisherNode *node); -static void initSubscriberNode(SubscriberNode *node); -static void initServiceProviderNode(ServiceProviderNode *node); -static void initServiceCallerNode(ServiceCallerNode *node); -static void initParameterSubscrition(ParameterSubscription *subscription); -static int enqueueSubscriberAdvertise(CrosNode *node, int subidx); -static int enqueuePublisherAdvertise(CrosNode *node, int pubidx); -static int enqueueServiceAdvertise(CrosNode *node, int servivceidx); -static int enqueueServiceLookup(CrosNode *node, int serviceidx); -static int enqueueParameterSubscription(CrosNode *node, int parameteridx); -static int enqueueParameterUnsubscription(CrosNode *node, int parameteridx); -static void getIdleXmplrpcClients(CrosNode *node, int array[], size_t *count); -static int enqueueSlaveApiCallInternal(CrosNode *node, RosApiCall *call); -static int enqueueMasterApiCallInternal(CrosNode *node, RosApiCall *call); -static void printNodeProcState( CrosNode *n ); - -FILE *Msg_output = NULL; //! The pointer to file stream used to print local messages (except debug messages). If it is NULL (default value), stdout is used. - -static int openXmlrpcClientSocket( CrosNode *n, int i ) -{ - int ret; - if( !tcpIpSocketOpen( &(n->xmlrpc_client_proc[i].socket) ) || - !tcpIpSocketSetReuse( &(n->xmlrpc_client_proc[i].socket) ) || - !tcpIpSocketSetNonBlocking( &(n->xmlrpc_client_proc[i].socket) ) ) - { - PRINT_ERROR("openXmlrpcClientSocket() at index %d failed", i); - ret=-1; - } - else - { - ret=0; // success - } - return(ret); -} - -static int openTcprosClientSocket( CrosNode *n, int i ) -{ - int ret; - if( !tcpIpSocketOpen( &(n->tcpros_client_proc[i].socket) ) || - !tcpIpSocketSetReuse( &(n->tcpros_client_proc[i].socket) ) || - !tcpIpSocketSetNonBlocking( &(n->tcpros_client_proc[i].socket) ) ) - { - PRINT_ERROR("openTcprosClientSocket() at index %d failed", i); - ret=-1; - } - else - { - ret=0; // success - } - return(ret); -} - -static int openRpcrosClientSocket( CrosNode *n, int i ) -{ - int ret; - if( !tcpIpSocketOpen( &(n->rpcros_client_proc[i].socket) ) || - !tcpIpSocketSetReuse( &(n->rpcros_client_proc[i].socket) ) || - !tcpIpSocketSetNonBlocking( &(n->rpcros_client_proc[i].socket) ) ) - { - PRINT_ERROR("openRpcrosClientSocket() at index %d failed", i); - ret=-1; - } - else - { - ret=0; // success - } - return(ret); -} - -static int openXmlrpcListnerSocket( CrosNode *n ) -{ - int ret; - if( !tcpIpSocketOpen( &(n->xmlrpc_listner_proc.socket) ) || - !tcpIpSocketSetReuse( &(n->xmlrpc_listner_proc.socket) ) || - !tcpIpSocketSetNonBlocking( &(n->xmlrpc_listner_proc.socket) ) || - !tcpIpSocketBindListen( &(n->xmlrpc_listner_proc.socket), n->host, 0, CN_MAX_XMLRPC_SERVER_CONNECTIONS ) ) - { - PRINT_ERROR("openXmlrpcListnerSocket() failed"); - ret=-1; - } - else - { - n->xmlrpc_port = tcpIpSocketGetPort( &(n->xmlrpc_listner_proc.socket) ); - PRINT_VDEBUG ( "openXmlrpcListnerSocket () : Accepting xmlrpc connections at port %d\n", n->xmlrpc_port ); - ret=0; // success - } - return(ret); -} - -static int openRpcrosListnerSocket( CrosNode *n ) -{ - int ret; - if( !tcpIpSocketOpen( &(n->rpcros_listner_proc.socket) ) || - !tcpIpSocketSetReuse( &(n->rpcros_listner_proc.socket) ) || - !tcpIpSocketSetNonBlocking( &(n->rpcros_listner_proc.socket) ) || - !tcpIpSocketBindListen( &(n->rpcros_listner_proc.socket), n->host, 0, CN_MAX_RPCROS_SERVER_CONNECTIONS ) ) - { - PRINT_ERROR("openRpcrosListnerSocket() failed"); - ret=-1; - } - else - { - n->rpcros_port = tcpIpSocketGetPort( &(n->rpcros_listner_proc.socket) ); - PRINT_VDEBUG ( "openRpcrosListnerSocket() : Accepting rcpros connections at port %d\n", n->rpcros_port ); - ret=0; // success - } - return(ret); -} - -static int openTcprosListnerSocket( CrosNode *n ) -{ - int ret; - if( !tcpIpSocketOpen( &(n->tcpros_listner_proc.socket) ) || - !tcpIpSocketSetReuse( &(n->tcpros_listner_proc.socket) ) || - !tcpIpSocketSetNonBlocking( &(n->tcpros_listner_proc.socket) ) || - !tcpIpSocketBindListen( &(n->tcpros_listner_proc.socket), n->host, 0, CN_MAX_TCPROS_SERVER_CONNECTIONS ) ) - { - PRINT_ERROR("openTcprosListnerSocket() failed"); - ret=-1; - } - else - { - n->tcpros_port = tcpIpSocketGetPort( &(n->tcpros_listner_proc.socket) ); - PRINT_VDEBUG ( "openTcprosListnerSocket() : Accepting tcpros connections at port %d\n", n->tcpros_port ); - ret=0; // success - } - return(ret); -} - -static void closeTcprosProcess(TcprosProcess *process) -{ - tcpIpSocketClose(&process->socket); - tcprosProcessReset(process); -} - -static void closeXmlrpcProcess(XmlrpcProcess *process) -{ - tcpIpSocketClose(&process->socket); - xmlrpcProcessReset(process); - xmlrpcProcessChangeState(process, XMLRPC_PROCESS_STATE_IDLE); -} - -// This method is used to communicate that some api calls at least attempted -// to complete, like in the case of unregistration when a gracefully shutdown -// is requested -static void handleApiCallAttempt(CrosNode *node, RosApiCall *call) -{ - CrosNodeStatusUsr status; - - initCrosNodeStatus(&status); - status.provider_idx = call->provider_idx; - - switch(call->method) - { - case CROS_API_UNREGISTER_PUBLISHER: - { - if (call->provider_idx == -1) - break; - - PublisherNode *pub = &node->pubs[call->provider_idx]; - status.state = CROS_STATUS_PUBLISHER_UNREGISTERED; - cRosNodeStatusCallback(&status, pub->context); // calls the publisher application-defined callback function (if specified when creating the publisher) - - // Finally release publisher - cRosApiReleasePublisher(node, call->provider_idx); - initPublisherNode(pub); - call->provider_idx = -1; - break; - } - case CROS_API_UNREGISTER_SUBSCRIBER: - { - if (call->provider_idx == -1) - break; - - SubscriberNode *sub = &node->subs[call->provider_idx]; - status.state = CROS_STATUS_SUBSCRIBER_UNREGISTERED; - cRosNodeStatusCallback(&status, sub->context); - - // Finally release subscriber - cRosApiReleaseSubscriber(node, call->provider_idx); - initSubscriberNode(sub); - call->provider_idx = -1; - break; - } - case CROS_API_UNREGISTER_SERVICE: - { - if (call->provider_idx == -1) - break; - - ServiceProviderNode *service = &node->service_providers[call->provider_idx]; - status.state = CROS_STATUS_SERVICE_PROVIDER_UNREGISTERED; - cRosNodeStatusCallback(&status, service->context); - - // Finally release service provider - cRosApiReleaseServiceProvider(node, call->provider_idx); - initServiceProviderNode(service); - call->provider_idx = -1; - break; - } - case CROS_API_UNSUBSCRIBE_PARAM: - { - if (call->provider_idx == -1) - break; - - ParameterSubscription *subscription = &node->paramsubs[call->provider_idx]; - status.state = CROS_STATUS_PARAM_UNSUBSCRIBED; - status.parameter_key = subscription->parameter_key; - subscription->status_api_callback(&status, subscription->context); - - // Finally release parameter subscription - cRosNodeReleaseParameterSubscrition(subscription); - initParameterSubscrition(subscription); - call->provider_idx = -1; - break; - } - default: - { - break; - } - } -} - -static void cleanApiCallState(CrosNode *node, RosApiCall *call) -{ - switch(call->method) - { - case CROS_API_REQUEST_TOPIC: - { - // transitory xmlrpc client processes are checked and cleared one by one in the subscriber unregistration function - break; - } - default: - { - break; - } - } -} - -static void handleXmlrpcClientError(CrosNode *node, int i) -{ - XmlrpcProcess *proc = &node->xmlrpc_client_proc[i]; - RosApiCall *call = proc->current_call; - - switch (call->method) - { - case CROS_API_REGISTER_SERVICE: - case CROS_API_REGISTER_PUBLISHER: - case CROS_API_REGISTER_SUBSCRIBER: - case CROS_API_SUBSCRIBE_PARAM: - { - proc->current_call = NULL; - enqueueApiCall(&node->master_api_queue, call); - break; - } - default: - { - ResultCallback callback = call->result_callback; - if (callback != NULL) - { - // Notifies an error with a NULL result - callback(call->id, NULL, call->context_data); - } - break; - } - } - - handleApiCallAttempt(node, call); - cleanApiCallState(node, call); - closeXmlrpcProcess(proc); -} - -static void handleTcprosClientError(CrosNode *n, int i) -{ - TcprosProcess *process = &n->tcpros_client_proc[i]; - closeTcprosProcess(process); - // CHECK-ME Riaccoda register subscriber? -} - -static void handleXmlrpcServerError(CrosNode *n, int i) -{ - XmlrpcProcess *process = &n->xmlrpc_server_proc[i]; - closeXmlrpcProcess(process); -} - -static void handleTcprosServerError(CrosNode *n, int proc_idx) -{ - int list_elem; - TcprosProcess *process = &n->tcpros_server_proc[proc_idx]; - PublisherNode *pub = &n->pubs[process->topic_idx]; - - // Look for the tcpros_server_proc index (proc_idx) in the publisher tcpros_server_proc list (to remove it) - for(list_elem=0;pub->tcpros_id_list[list_elem]!=-1 && pub->tcpros_id_list[list_elem] != proc_idx;list_elem++); - if(pub->tcpros_id_list[list_elem] != proc_idx) // tcpros_server_proc index (proc_idx) found - { - // Remove index - for(;pub->tcpros_id_list[list_elem]!=-1;list_elem++) - pub->tcpros_id_list[list_elem] = pub->tcpros_id_list[list_elem+1]; - } - else - PRINT_ERROR("handleTcprosServerError() : TcprosProcess index %i has not been found in Publisher %i", proc_idx, process->topic_idx); - - closeTcprosProcess(process); -} - -static void handleRpcrosClientError(CrosNode *n, int i) -{ - TcprosProcess *process = &n->rpcros_client_proc[i]; - closeTcprosProcess(process); -} - -static void handleRpcrosServerError(CrosNode *n, int i) -{ - TcprosProcess *process = &n->rpcros_server_proc[i]; - closeTcprosProcess(process); -} - -static cRosErrCodePack xmlrpcClientConnect(CrosNode *n, int i) -{ - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "xmlrpcClientConnect()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; - XmlrpcProcess *client_proc = &(n->xmlrpc_client_proc[i]); - - PRINT_VDEBUG ( "xmlrpcClientConnect() : Connecting\n" ); - - if( !client_proc->socket.connected ) - { - TcpIpSocketState conn_state; - RosApiCall *xml_call; - - if(!client_proc->socket.open) - openXmlrpcClientSocket(n, i); - - xml_call = client_proc->current_call; - if(xml_call == NULL) - { - PRINT_ERROR ( "xmlrpcClientConnect() : Invalid XMLRPC call\n" ); - return CROS_UNSPECIFIED_ERR; - } - - if (i == 0 || isRosMasterApi(xml_call->method)) - { - conn_state = tcpIpSocketConnect( &(client_proc->socket), - n->roscore_host, n->roscore_port ); - } - else // slave api or client xmlrpc to be invoked from a subscriber - { - conn_state = tcpIpSocketConnect(&client_proc->socket, - xml_call->host, xml_call->port); - } - - if( conn_state == TCPIPSOCKET_DONE) - { - xmlrpcProcessChangeState( client_proc, XMLRPC_PROCESS_STATE_WRITING ); - } - else if( conn_state == TCPIPSOCKET_IN_PROGRESS ) - { - PRINT_VDEBUG ( "xmlrpcClientConnect() : Wait: connection is established asynchronously\n" ); // Not connected yet - } - else if( conn_state == TCPIPSOCKET_REFUSED ) - { - PRINT_ERROR("xmlrpcClientConnect() : Connection of XMLRPC client number %i was refused (is the target port not open?)\n", i); - handleXmlrpcClientError( n, i); - ret_err = CROS_XMLRPC_CLI_REFUS_ERR; - } - else - { - if( conn_state != TCPIPSOCKET_FAILED ) - PRINT_ERROR ( "tcprosClientConnect() : invalid connection state\n" ); - PRINT_ERROR("xmlrpcClientConnect() : XMLRPC client number %i can't connect\n", i); - handleXmlrpcClientError( n, i); - ret_err = CROS_XMLRPC_CLI_CONN_ERR; - } - } - - return ret_err; -} - -static cRosErrCodePack doWithXmlrpcClientSocket(CrosNode *n, int i) -{ - cRosErrCodePack ret_err; - XmlrpcProcess *client_proc; - PRINT_VVDEBUG ( "doWithXmlrpcClientSocket()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; - client_proc = &(n->xmlrpc_client_proc[i]); - - switch( client_proc->state ) - { - case XMLRPC_PROCESS_STATE_CONNECTING: - { - ret_err = xmlrpcClientConnect(n, i); - break; - } - case XMLRPC_PROCESS_STATE_WRITING: - { - PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : writing. Xmlrpc client index: %d\n", i); - - cRosApiPrepareRequest( n, i ); - - TcpIpSocketState sock_state = tcpIpSocketWriteString( &(client_proc->socket), - &(client_proc->message) ); - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - { - // NB: Node release is done by handleApiCallAttempt when the ros master response is received - xmlrpcProcessClear(client_proc); - xmlrpcProcessChangeState( client_proc, XMLRPC_PROCESS_STATE_READING ); - break; - } - - case TCPIPSOCKET_IN_PROGRESS: - { - PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : Write in progress...\n" ); - break; - } - - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - { - PRINT_ERROR("doWithXmlrpcClientSocket() : Unexpected failure writing request\n"); - handleXmlrpcClientError( n, i ); - ret_err = CROS_XMLRPC_CLI_WRITE_ERR; - break; - } - } - break; - } - case XMLRPC_PROCESS_STATE_READING: - { - PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : Reading. Xmlrpc client index: %d\n", i); - TcpIpSocketState sock_state = tcpIpSocketReadString( &client_proc->socket, - &client_proc->message ); - XmlrpcParserState parser_state = XMLRPC_PARSER_INCOMPLETE; - - int disconnected = 0; - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - { - parser_state = parseXmlrpcMessage( &client_proc->message, - &client_proc->message_type, - NULL, - &client_proc->response, - client_proc->host, - &client_proc->port); - break; - } - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - { - parser_state = parseXmlrpcMessage( &client_proc->message, - &client_proc->message_type, - NULL, - &client_proc->response, - client_proc->host, - &client_proc->port); - disconnected = 1; - break; - } - - case TCPIPSOCKET_FAILED: - default: - { - PRINT_ERROR("doWithXmlrpcClientSocket() : Unexpected failure reading response\n" ); - handleXmlrpcClientError( n, i ); - ret_err = CROS_XMLRPC_CLI_READ_ERR; - break; - } - } - - switch ( parser_state ) - { - case XMLRPC_PARSER_DONE: - { - PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : Done with no error\n" ); - - int rc = cRosApiParseResponse( n, i ); - handleApiCallAttempt(n, client_proc->current_call); - if (rc != 0) - { - handleXmlrpcClientError( n, i ); - } - else - { - cleanApiCallState(n, client_proc->current_call); - closeXmlrpcProcess(client_proc); - } - break; - } - - case XMLRPC_PARSER_INCOMPLETE: - { - if (disconnected) - handleXmlrpcClientError( n, i ); - break; - } - - case XMLRPC_PARSER_ERROR: - default: - { - handleXmlrpcClientError( n, i ); - break; - } - } - break; - } - default: - { - PRINT_VDEBUG ( "doWithXmlrpcClientSocket() : Invalid XMLRPC process state\n" ); - } - } - return ret_err; -} - -static cRosErrCodePack doWithXmlrpcServerSocket( CrosNode *n, int i ) -{ - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "doWithXmlrpcServerSocket()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; - XmlrpcProcess *server_proc = &(n->xmlrpc_server_proc[i]); - - if( server_proc->state == XMLRPC_PROCESS_STATE_READING ) - { - PRINT_VDEBUG ( "doWithXmlrpcServerSocket() : Reading. Xmlrpc server index: %d\n", i ); - TcpIpSocketState sock_state = tcpIpSocketReadString( &(server_proc->socket), - &(server_proc->message) ); - XmlrpcParserState parser_state = XMLRPC_PARSER_INCOMPLETE; - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - parser_state = parseXmlrpcMessage( &server_proc->message, - &server_proc->message_type, - &server_proc->method, - &server_proc->params, - server_proc->host, - &server_proc->port); - break; - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - xmlrpcProcessReset( &(n->xmlrpc_server_proc[i]) ); - xmlrpcProcessChangeState( &(n->xmlrpc_server_proc[i]), XMLRPC_PROCESS_STATE_IDLE ); - tcpIpSocketClose( &(n->xmlrpc_server_proc[i].socket) ); - break; - case TCPIPSOCKET_FAILED: - default: - PRINT_ERROR("doWithXmlrpcServerSocket() : Unexpected failure reading request\n"); - handleXmlrpcServerError( n, i ); - break; - } - - switch ( parser_state ) - { - case XMLRPC_PARSER_DONE: - { - - PRINT_VDEBUG ( "doWithXmlrpcServerSocket() : Done reading and parsing with no error\n" ); - int rc = cRosApiParseRequestPrepareResponse( n, i ); - if (rc == -1) - { - handleXmlrpcServerError( n, i ); - break; - } - xmlrpcProcessChangeState( server_proc, XMLRPC_PROCESS_STATE_WRITING ); - break; - } - case XMLRPC_PARSER_INCOMPLETE: - break; - - case XMLRPC_PARSER_ERROR: - default: - { - handleXmlrpcServerError( n, i ); - break; - } - } - } - else if( server_proc->state == XMLRPC_PROCESS_STATE_WRITING ) - { - PRINT_VDEBUG ( "doWithXmlrpcServerSocket() : writing. Xmlrpc server index %d \n", i ); - TcpIpSocketState sock_state = tcpIpSocketWriteString( &(server_proc->socket), - &(server_proc->message) ); - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - xmlrpcProcessReset( server_proc ); - xmlrpcProcessChangeState( server_proc, XMLRPC_PROCESS_STATE_READING ); - break; - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - xmlrpcProcessReset( server_proc ); - xmlrpcProcessChangeState( server_proc, XMLRPC_PROCESS_STATE_IDLE ); - tcpIpSocketClose( &(server_proc->socket) ); - break; - - case TCPIPSOCKET_FAILED: - default: - PRINT_ERROR("doWithXmlrpcServerSocket() : Unexpected failure writing response\n"); - handleXmlrpcServerError( n, i ); - break; - } - } - return ret_err; -} - -static cRosErrCodePack tcprosClientConnect( CrosNode *n, int client_idx) -{ - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "tcprosClientConnect()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; - TcprosProcess *client_proc = &(n->tcpros_client_proc[client_idx]); - - if(!client_proc->socket.open) - openTcprosClientSocket(n, client_idx); - - tcprosProcessClear( client_proc ); // clear packet buffer and variable indicating bytes left to receive (left_to_recv) - TcpIpSocketState conn_state = tcpIpSocketConnect( &(client_proc->socket), - client_proc->sub_tcpros_host, client_proc->sub_tcpros_port ); - switch (conn_state) - { - case TCPIPSOCKET_DONE: - { - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WRITING_HEADER ); - break; - } - case TCPIPSOCKET_IN_PROGRESS: - { - PRINT_VDEBUG ( "tcprosClientConnect() : Wait: connection is established asynchronously\n" ); - // Wait: connection is established asynchronously - break; - } - case TCPIPSOCKET_REFUSED: - { - PRINT_ERROR("tcprosClientConnect() : Connection of TCPROS client number %i was refused (is the target port not open?)\n", client_idx); - handleTcprosClientError( n, client_idx); - ret_err = CROS_TCPROS_CLI_REFUS_ERR; - break; - } - default: - { - PRINT_ERROR ( "tcprosClientConnect() : invalid connection state\n" ); - // no break execute next case too - } - case TCPIPSOCKET_FAILED: - { - PRINT_ERROR ( "tcprosClientConnect() : An error occurred when TCPROS client number %i was trying to connect\n", client_idx); - handleTcprosClientError( n, client_idx); - ret_err = CROS_TCPROS_CLI_CONN_ERR; - break; - } - } - - return ret_err; -} - -static cRosErrCodePack doWithTcprosClientSocket( CrosNode *n, int client_idx) -{ - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "doWithTcprosClientSocket()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; - TcprosProcess *client_proc = &(n->tcpros_client_proc[client_idx]); - - switch ( client_proc->state ) - { - case TCPROS_PROCESS_STATE_CONNECTING: - { - ret_err = tcprosClientConnect( n, client_idx); - break; - } - case TCPROS_PROCESS_STATE_WRITING_HEADER: - { - cRosMessagePrepareSubcriptionHeader( n, client_idx); - - TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(client_proc->socket), - &(client_proc->packet) ); - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - tcprosProcessClear( client_proc ); - client_proc->left_to_recv = sizeof(uint32_t); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_HEADER_SIZE ); - - break; - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - closeTcprosProcess( client_proc ); - break; - - case TCPIPSOCKET_FAILED: - default: - handleTcprosClientError( n, client_idx ); - break; - } - break; - } - case TCPROS_PROCESS_STATE_READING_HEADER_SIZE: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), - &(client_proc->packet), - client_proc->left_to_recv, - &n_reads); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - client_proc->left_to_recv -= n_reads; - if (client_proc->left_to_recv == 0) - { - const unsigned char *data = dynBufferGetCurrentData(&client_proc->packet); - uint32_t header_size; - header_size = ROS_TO_HOST_UINT32(*(uint32_t *)data); - tcprosProcessClear( client_proc ); - client_proc->left_to_recv = header_size; - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_HEADER); - goto read_header; - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - handleTcprosClientError( n, client_idx ); - break; - } - - break; - } - read_header: - case TCPROS_PROCESS_STATE_READING_HEADER: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), - &(client_proc->packet), - client_proc->left_to_recv, - &n_reads); - TcprosParserState parser_state = TCPROS_PARSER_ERROR; - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - client_proc->left_to_recv -= n_reads; - if (client_proc->left_to_recv == 0) - { - parser_state = cRosMessageParsePublicationHeader( n, client_idx ); - break; - } - parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; - break; - case TCPIPSOCKET_IN_PROGRESS: - parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; - break; - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - handleTcprosClientError( n, client_idx ); - break; - } - - switch ( parser_state ) - { - case TCPROS_PARSER_DONE: - tcprosProcessClear( client_proc ); - client_proc->left_to_recv = sizeof(uint32_t); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_SIZE ); - break; - case TCPROS_PARSER_HEADER_INCOMPLETE: - break; - case TCPROS_PARSER_ERROR: - default: - handleTcprosClientError( n, client_idx ); - break; - } - break; // To avoid blocking ??? - } - case TCPROS_PROCESS_STATE_READING_SIZE: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), - &(client_proc->packet), - client_proc->left_to_recv, - &n_reads); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - client_proc->left_to_recv -= n_reads; - if (client_proc->left_to_recv == 0) - { - const unsigned char *data = dynBufferGetCurrentData(&client_proc->packet); - uint32_t msg_size = 0; - msg_size = ROS_TO_HOST_UINT32(*(uint32_t *)data); - tcprosProcessClear( client_proc ); - client_proc->left_to_recv = msg_size; - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING); - goto read_msg; - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - handleTcprosClientError( n, client_idx ); - break; - } - break; - } - read_msg: - case TCPROS_PROCESS_STATE_READING: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), - &(client_proc->packet), - client_proc->left_to_recv, - &n_reads); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - client_proc->left_to_recv -= n_reads; - if (client_proc->left_to_recv == 0) - { - ret_err = cRosMessageParsePublicationPacket(n, client_idx); - tcprosProcessClear( client_proc ); - client_proc->left_to_recv = sizeof(uint32_t); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_SIZE ); - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - handleTcprosClientError( n, client_idx ); - break; - } - break; - } - default: - { - PRINT_ERROR( "doWithTcprosClientSocket() : Invalid TCPROS process state\n" ); - } - - } - return ret_err; -} - -static cRosErrCodePack doWithTcprosServerSocket( CrosNode *n, int i ) -{ - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "doWithTcprosServerSocket()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; // Default return value - TcprosProcess *server_proc = &(n->tcpros_server_proc[i]); - - if( server_proc->state == TCPROS_PROCESS_STATE_READING_HEADER) - { - PRINT_VDEBUG ( "doWithTcprosServerSocket() : Reading header. Tcpros server index %d \n", i ); - tcprosProcessClear( server_proc ); - TcpIpSocketState sock_state = tcpIpSocketReadBuffer( &(server_proc->socket), - &(server_proc->packet) ); - TcprosParserState parser_state = TCPROS_PARSER_ERROR; - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - parser_state = cRosMessageParseSubcriptionHeader( n, i ); - break; - - case TCPIPSOCKET_IN_PROGRESS: - parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; - break; - - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - PRINT_INFO( "doWithTcprosServerSocket() : Client disconnected?\n" ); - handleTcprosServerError( n, i ); - break; - } - - switch ( parser_state ) - { - case TCPROS_PARSER_DONE: - - PRINT_VDEBUG ( "doWithTcprosServerSocket() : Done reading and parsing with no error\n" ); - tcprosProcessClear( server_proc ); - cRosMessagePreparePublicationHeader( n, i ); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING ); // Proceed to write the header - break; - case TCPROS_PARSER_HEADER_INCOMPLETE: - break; - - case TCPROS_PARSER_ERROR: - default: - PRINT_INFO( "doWithTcprosServerSocket() : Parser error\n" ); - handleTcprosServerError( n, i ); - break; - } - } - else if( server_proc->state == TCPROS_PROCESS_STATE_START_WRITING || - server_proc->state == TCPROS_PROCESS_STATE_WRITING ) // It is time to write a message or header - { - PRINT_VDEBUG ( "doWithTcprosServerSocket() : writing. Tcpros server index: %d \n", i ); - if( server_proc->state == TCPROS_PROCESS_STATE_START_WRITING ) // Start to publish a message - { - tcprosProcessClear( server_proc ); - ret_err = cRosMessagePreparePublicationPacket( n, i ); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING ); - } - TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(server_proc->socket), - &(server_proc->packet) ); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - PRINT_VDEBUG ( "doWithTcprosServerSocket() : Done writing with no error\n" ); - tcprosProcessClear( server_proc ); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WAIT_FOR_WRITING ); // Wait before publishing a new message - break; - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - PRINT_INFO( "doWithTcprosServerSocket() : Client disconnected?\n" ); - handleTcprosServerError(n, i); - break; - } - } - return ret_err; -} - -static cRosErrCodePack rpcrosClientConnect(CrosNode *n, int client_idx) -{ - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "rpcrosClientConnect()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; - TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); - - if(!client_proc->socket.open) - openRpcrosClientSocket(n, client_idx); - - ServiceCallerNode *service_caller = &(n->service_callers[client_proc->service_idx]); - tcprosProcessClear( client_proc ); - TcpIpSocketState conn_state = tcpIpSocketConnect( &(client_proc->socket), service_caller->service_host, service_caller->service_port ); - switch (conn_state) - { - case TCPIPSOCKET_DONE: - { - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WRITING_HEADER ); - break; - } - case TCPIPSOCKET_IN_PROGRESS: - { - PRINT_VDEBUG ( "rpcrosClientConnect() : Wait: connection is established asynchronously\n" ); - // Wait: connection is established asynchronously - break; - } - case TCPIPSOCKET_REFUSED: - { - PRINT_ERROR("xmlrpcClientConnect() : Connection of RPCROS client number %i was refused (is the target port not open?)\n", client_idx); - handleRpcrosClientError( n, client_idx); - ret_err = CROS_RPCROS_CLI_REFUS_ERR; - break; - } - default: - { - PRINT_ERROR ( "rpcrosClientConnect() : invalid connection state\n" ); - // no break execute next case too - } - case TCPIPSOCKET_FAILED: - { - PRINT_ERROR ( "rpcrosClientConnect() : An error occurred when RPCROS client number %i was trying to connect\n", client_idx); - handleRpcrosClientError( n, client_idx); - ret_err = CROS_RPCROS_CLI_CONN_ERR; - break; - } - } - - return ret_err; -} - -static cRosErrCodePack doWithRpcrosClientSocket(CrosNode *n, int client_idx) -{ - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "doWithRpcrosClientSocket()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; - TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); - - switch ( client_proc->state ) - { - case TCPROS_PROCESS_STATE_CONNECTING: - { - ret_err = rpcrosClientConnect(n, client_idx); - break; - } - case TCPROS_PROCESS_STATE_WRITING_HEADER: - { - cRosMessagePrepareServiceCallHeader(n, client_idx); - - TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(client_proc->socket), &(client_proc->packet) ); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - tcprosProcessClear( client_proc ); - client_proc->left_to_recv = sizeof(uint32_t); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_HEADER_SIZE ); - - break; - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - closeTcprosProcess( client_proc ); - break; - - case TCPIPSOCKET_FAILED: - default: - handleRpcrosClientError( n, client_idx ); - break; - } - break; - } - case TCPROS_PROCESS_STATE_READING_HEADER_SIZE: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), - &(client_proc->packet), - client_proc->left_to_recv, - &n_reads); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - client_proc->left_to_recv -= n_reads; - if (client_proc->left_to_recv == 0) - { - const unsigned char *data = dynBufferGetCurrentData(&client_proc->packet); - uint32_t header_size; - header_size = ROS_TO_HOST_UINT32(*(uint32_t *)data); - tcprosProcessClear( client_proc ); - client_proc->left_to_recv = header_size; - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_HEADER); - goto read_header; - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - tcpIpSocketClose(&client_proc->socket); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); - break; - case TCPIPSOCKET_FAILED: - default: - handleRpcrosClientError( n, client_idx ); - break; - } - - break; - } - read_header: - case TCPROS_PROCESS_STATE_READING_HEADER: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), - &(client_proc->packet), - client_proc->left_to_recv, - &n_reads); - TcprosParserState parser_state = TCPROS_PARSER_ERROR; - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - client_proc->left_to_recv -= n_reads; - if (client_proc->left_to_recv == 0) - { - parser_state = cRosMessageParseServiceProviderHeader( n, client_idx ); - break; - } - parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; - break; - case TCPIPSOCKET_IN_PROGRESS: - parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; - break; - case TCPIPSOCKET_DISCONNECTED: - tcpIpSocketClose(&client_proc->socket); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); - break; - case TCPIPSOCKET_FAILED: - default: - handleRpcrosClientError( n, client_idx ); - break; - } - - switch ( parser_state ) - { - case TCPROS_PARSER_DONE: - tcprosProcessClear( client_proc ); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_WRITING ); - break; - case TCPROS_PARSER_HEADER_INCOMPLETE: - break; - case TCPROS_PARSER_ERROR: - default: - handleRpcrosClientError( n, client_idx ); - break; - } - break; - } - - case TCPROS_PROCESS_STATE_START_WRITING: - { - tcprosProcessClear( client_proc ); - ret_err = cRosMessagePrepareServiceCallPacket(n, client_idx); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WRITING ); - } - - case TCPROS_PROCESS_STATE_WRITING: - { - PRINT_VDEBUG ( "doWithRpcrosClientSocket() : writing. Rpcros client index: %d \n", client_idx ); - TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(client_proc->socket), - &(client_proc->packet) ); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - PRINT_VDEBUG ( "doWithRpcrosClientSocket() : Done writing with no error\n" ); - tcprosProcessClear( client_proc ); // Clears only packet and left_to_recv vars - client_proc->left_to_recv = sizeof(uint32_t) + sizeof(uint8_t); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING_SIZE ); - break; - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - tcpIpSocketClose(&client_proc->socket); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); - break; - case TCPIPSOCKET_FAILED: - default: - PRINT_INFO( "doWithRpcrosClientSocket() : Client disconnected\n" ); - handleRpcrosClientError(n, client_idx); - break; - } - break; - } - - case TCPROS_PROCESS_STATE_READING_SIZE: // Read 'ok' byte and response-packet size field - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), - &(client_proc->packet), - client_proc->left_to_recv, - &n_reads); - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - client_proc->left_to_recv -= n_reads; - if (client_proc->left_to_recv == 0) - { - const uint8_t *data = (const uint8_t *)dynBufferGetCurrentData(&client_proc->packet); - uint32_t msg_size; - client_proc->ok_byte = *data; - msg_size = ROS_TO_HOST_UINT32(*(uint32_t *)(data+1)); - tcprosProcessClear( client_proc ); - client_proc->left_to_recv = msg_size; - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_READING); - goto read_msg; - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - tcpIpSocketClose(&client_proc->socket); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); - break; - case TCPIPSOCKET_FAILED: - default: - handleRpcrosClientError( n, client_idx ); - break; - } - break; - } - read_msg: - case TCPROS_PROCESS_STATE_READING: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(client_proc->socket), - &(client_proc->packet), - client_proc->left_to_recv, - &n_reads); - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - client_proc->left_to_recv -= n_reads; - if (client_proc->left_to_recv == 0) - { - ret_err = cRosMessageParseServiceResponsePacket(n, client_idx); - if(client_proc->persistent) - { - tcprosProcessClear( client_proc ); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_WRITING ); - } - else - { - tcprosProcessClear( client_proc ); - tcpIpSocketClose( &(client_proc->socket) ); - openRpcrosClientSocket(n, client_idx); - // The master should be checked before contacting the service provider again, - // that is, before going to state TCPROS_PROCESS_STATE_CONNECTING again. - // Services are stateless (unless the persistent parameter is set to 1) - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_IDLE ); - if(enqueueServiceLookup(n, client_proc->service_idx) == -1) - handleRpcrosClientError( n, client_idx ); - } - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - tcpIpSocketClose(&client_proc->socket); - tcprosProcessChangeState( client_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); - break; - case TCPIPSOCKET_FAILED: - default: - handleRpcrosClientError( n, client_idx ); - break; - } - break; - } - default: - { - PRINT_ERROR ( "doWithRpcrosClientSocket() : Invalid RPC ROS process state\n" ); - } - - } - return ret_err; -} - -static cRosErrCodePack doWithRpcrosServerSocket(CrosNode *n, int i) -{ - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "doWithRpcrosServerSocket()\n" ); - - ret_err = CROS_SUCCESS_ERR_PACK; - TcprosProcess *server_proc = &(n->rpcros_server_proc[i]); - - switch (server_proc->state) - { - case TCPROS_PROCESS_STATE_READING_HEADER_SIZE: - { - size_t n_reads; - if (server_proc->left_to_recv == 0) - server_proc->left_to_recv = sizeof(uint32_t); - - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(server_proc->socket), - &(server_proc->packet), - server_proc->left_to_recv, - &n_reads); - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - server_proc->left_to_recv -= n_reads; - if (server_proc->left_to_recv == 0) - { - const unsigned char *data = dynBufferGetCurrentData(&server_proc->packet); - uint32_t header_size; - header_size = ROS_TO_HOST_UINT32(*(uint32_t *)data); - tcprosProcessClear( server_proc ); - server_proc->left_to_recv = header_size; - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING_HEADER); - goto read_header; - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - closeTcprosProcess( server_proc ); - break; - case TCPIPSOCKET_FAILED: - default: - handleRpcrosServerError( n, i ); - break; - } - - break; - } - read_header: - case TCPROS_PROCESS_STATE_READING_HEADER: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(server_proc->socket), - &(server_proc->packet), - server_proc->left_to_recv, - &n_reads); - TcprosParserState parser_state = TCPROS_PARSER_ERROR; - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - server_proc->left_to_recv -= n_reads; - if (server_proc->left_to_recv == 0) - { - parser_state = cRosMessageParseServiceCallerHeader( n, i ); - break; - } - parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; - break; - case TCPIPSOCKET_IN_PROGRESS: - parser_state = TCPROS_PARSER_HEADER_INCOMPLETE; - break; - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - handleRpcrosServerError( n, i ); - break; - } - - switch ( parser_state ) - { - case TCPROS_PARSER_DONE: - tcprosProcessClear( server_proc ); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING_HEADER ); - PRINT_VDEBUG("doWithRpcrosServerSocket() : Header parsed. Probing session: %d. Rpcros server index: %d \n", server_proc->probe, i); - break; - case TCPROS_PARSER_HEADER_INCOMPLETE: - break; - case TCPROS_PARSER_ERROR: - default: - handleRpcrosServerError( n, i ); - break; - } - - break; - } - case TCPROS_PROCESS_STATE_WRITING_HEADER: - { - PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Writing header. RpcrosServer index: %d \n", i ); - - cRosMessagePrepareServiceProviderHeader( n, i ); - - TcpIpSocketState sock_state = tcpIpSocketWriteBuffer( &(server_proc->socket), - &(server_proc->packet) ); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Done writing header with no error. RpcrosServer index: %d \n", i); - if(server_proc->probe) - { - if(server_proc->persistent) - { - tcprosProcessClear( server_proc ); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING_HEADER_SIZE); - server_proc->left_to_recv = sizeof(uint32_t); - } - else - closeTcprosProcess( server_proc ); - } - else - { - tcprosProcessClear( server_proc ); - server_proc->left_to_recv = sizeof(uint32_t); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING_SIZE ); - } - break; - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - closeTcprosProcess( server_proc ); - break; - - case TCPIPSOCKET_FAILED: - default: - handleRpcrosServerError( n, i ); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_IDLE ); - break; - } - - break; - } - case TCPROS_PROCESS_STATE_READING_SIZE: - { - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(server_proc->socket), - &(server_proc->packet), - server_proc->left_to_recv, - &n_reads); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - server_proc->left_to_recv -= n_reads; - if (server_proc->left_to_recv == 0) - { - const uint32_t *data = (const uint32_t *)dynBufferGetCurrentData(&server_proc->packet); - uint32_t msg_size; - msg_size = ROS_TO_HOST_UINT32(*data); - tcprosProcessClear( server_proc ); - if (msg_size == 0) - { - PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Done reading size with no error\n" ); - ret_err = cRosMessagePrepareServiceResponsePacket(n, i); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING); - goto write_msg; - } - else - { - server_proc->left_to_recv = msg_size; - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING); - goto read_msg; - } - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - case TCPIPSOCKET_FAILED: - default: - handleRpcrosServerError( n, i ); - break; - } - break; - } - read_msg: - case TCPROS_PROCESS_STATE_READING: - { - PRINT_VDEBUG ( "doWithRpcrosServerSocket() : reading. RpcrosServer index: %d\n", i ); - - size_t n_reads; - TcpIpSocketState sock_state = tcpIpSocketReadBufferEx( &(server_proc->socket), - &(server_proc->packet), - server_proc->left_to_recv, - &n_reads); - - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - server_proc->left_to_recv -= n_reads; - if (server_proc->left_to_recv == 0) - { - PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Done reading with no error\n" ); - ret_err = cRosMessagePrepareServiceResponsePacket(n, i); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_WRITING ); - } - break; - case TCPIPSOCKET_IN_PROGRESS: - break; - case TCPIPSOCKET_DISCONNECTED: - closeTcprosProcess( server_proc ); - break; - case TCPIPSOCKET_FAILED: - default: - handleRpcrosServerError( n, i ); - break; - } - - break; - } - write_msg: - case TCPROS_PROCESS_STATE_WRITING: - { - PRINT_VDEBUG ( "doWithRpcrosServerSocket() : writing. Rpcros server index %d \n", i ); - - TcpIpSocketState sock_state; - - //If the rpc response is empty, e.g. log procedures - //if(server_proc->packet.size > 0) - //{ - sock_state = tcpIpSocketWriteBuffer( &(server_proc->socket), &(server_proc->packet) ); - //} - //else - //{ - //sock_state = TCPIPSOCKET_DONE; - //} - switch ( sock_state ) - { - case TCPIPSOCKET_DONE: - PRINT_VDEBUG ( "doWithRpcrosServerSocket() : Done writing with no error\n" ); - if(server_proc->persistent) - { - server_proc->left_to_recv = sizeof(uint32_t); - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_READING_SIZE); - } - else - closeTcprosProcess( server_proc ); - break; - - case TCPIPSOCKET_IN_PROGRESS: - break; - - case TCPIPSOCKET_DISCONNECTED: - closeTcprosProcess( server_proc ); - break; - - case TCPIPSOCKET_FAILED: - default: - handleRpcrosServerError( n, i ); - break; - } - - break; - } - default: - { - PRINT_ERROR ( "doWithRpcrosServerSocket() : Invalid RPCROS server process state\n" ); - } - } - return ret_err; -} - -/* - * Callback functions for the service callS of the logging mechanism - */ -static CallbackResponse callback_srv_set_logger_level(cRosMessage *request, cRosMessage *response, void* context) -{ - cRosMessageField* level = cRosMessageGetField(request, "level"); - const char* level_str = level->data.as_string; - CrosNode* node = (CrosNode*) context; - if(stringToLogLevel(level_str, &node->log_level) == 0) - PRINT_INFO( "callback_srv_set_logger_level() : Node log priority level changed to %s\n", LogLevelToString(node->log_level)); - return 0; -} - -static CallbackResponse callback_srv_get_loggers(cRosMessage *request, cRosMessage *response, void* context) -{ - const char *log_level_str; - cRosMessage *logger_msg; - cRosMessageField *loggers_field, *level_field; - CrosNode *node = (CrosNode*) context; - - loggers_field = cRosMessageGetField(response, "loggers"); - - logger_msg = cRosMessageFieldArrayAtMsgGet(loggers_field, 0); - if(logger_msg == NULL) // If the message array is empty (it is the first time that the callback fn is called), create a new message (array element) - { - cRosMessageField *logger_field; - - cRosMessageNewBuild(node->message_root_path, "roscpp/Logger", &logger_msg); - logger_field = cRosMessageGetField(logger_msg, "name"); - cRosMessageSetFieldValueString(logger_field, "ros.cros_node"); - - cRosMessageFieldArrayPushBackMsg(loggers_field, logger_msg); - } - level_field = cRosMessageGetField(logger_msg, "level"); - log_level_str = LogLevelToString(node->log_level); - cRosMessageSetFieldValueString(level_field, log_level_str); - - return 0; -} - -int checkNamespaceFormat(const char* name_space) -{ - const char* it_ns = name_space; - - if(*it_ns == '~') - it_ns++; - - while(*it_ns != '\0') - { - if(*it_ns == '/') - { - if(*(it_ns + 1) == '\0' || - !isalpha(*(it_ns + 1))) - return 0; - it_ns++; - } - else - { - if(!isalnum(*it_ns) && *it_ns != '_') - return 0; - } - it_ns++; - } - - return 1; -} - -char* cRosNamespaceBuild(CrosNode* node, const char* resource_name) -{ - char *resolved_name = NULL; - - if(!checkNamespaceFormat(resource_name)) - return NULL; - - if(node == NULL) - { - if(resource_name[0] == '/') - { - resolved_name = (char *)calloc(strlen(resource_name) + 1, sizeof(char)); - strncat(resolved_name, resource_name,strlen(resource_name)); - } - else - { - resolved_name = (char *)calloc(strlen(resource_name) + strlen("/") + 1, sizeof(char)); - strncat(resolved_name, "/",strlen("/")); - strncat(resolved_name, resource_name,strlen(resource_name)); - } - } - else - { - char* node_name = node->name; - switch(resource_name[0]) - { - case '/': - { - //global namespace - if(node != NULL || 1) - { - resolved_name = (char *)calloc(strlen(resource_name) + 1, - sizeof(char)); - strncat(resolved_name,resource_name,strlen(resource_name)); - } - break; - } - case '~': - { - //private namespace - if(node != NULL || 1) - { - resolved_name = (char *)calloc(strlen(node_name) + - strlen("/") + - strlen(resource_name) + 1, - sizeof(char)); - strncat(resolved_name,node_name,strlen(node_name)); - strncat(resolved_name,"/",strlen("/")); - strncat(resolved_name,resource_name + 1,strlen(resource_name) - 1 ); - } - break; - } - default: - { - // the resource has a name that is not global or private - if(node != NULL || 1) - { - char *node_namespace = (char *)calloc(strlen(node_name) + 1, sizeof(char)); - strcpy(node_namespace, node_name); - char *it = node_namespace + strlen(node_name); - while(*(it--) != '/'); - *(it + 2) = '\0'; - resolved_name = (char *)calloc(strlen(node_namespace) + strlen(resource_name) + 1,sizeof(char)); - strncat(resolved_name,node_namespace,strlen(node_namespace)); - strncat(resolved_name,resource_name,strlen(resource_name)); - } - break; - } - } - } - return resolved_name; -} - -int cRosInitializeLibrary(void) -{ - int ret; - ret = tcpIpSocketStartUp(); // Initialize the socket library (it's mandatory on Windows) - // Enable escape character processing for console messages so that debug messages can use colors - // This processing is supported on Windows 10 build 16257 and later (and on Linux). -#ifdef _WIN32 - HANDLE stdout_handle = GetStdHandle(STD_OUTPUT_HANDLE); - if (stdout_handle != INVALID_HANDLE_VALUE) - { - DWORD console_mode = 0; - if (GetConsoleMode(stdout_handle, &console_mode)) - { - // Set the console output mode to process virtual-terminal sequences - if (!SetConsoleMode(stdout_handle, console_mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING)) - PRINT_ERROR("cRosInitializeLibrary() : Can't set console-output mode (Error code: %lu).\n", GetLastError()); - } - else - PRINT_ERROR("cRosInitializeLibrary() : Can't obatin console-output mode (Error code: %lu).\n", GetLastError()); - } - else - PRINT_ERROR("cRosInitializeLibrary() : Can't obatin console handle to configure output mode (Error code: %lu).\n", GetLastError()); -#endif - return(ret); -} - -CrosNode *cRosNodeCreate (const char *node_name, const char *node_host, const char *roscore_host, unsigned short roscore_port, - const char *message_root_path) -{ - CrosNode *new_n; // Value to be returned by this function. NULL on failure - PRINT_VVDEBUG ( "cRosNodeCreate()\n" ); - - if(node_name == NULL || node_host == NULL || roscore_host == NULL ) - { - PRINT_ERROR ( "cRosNodeCreate() : NULL parameters\n" ); - return NULL; - } - - if(cRosInitializeLibrary() != 0) - return NULL; // Initialization of the library failed: exit - - new_n = ( CrosNode * ) malloc ( sizeof ( CrosNode ) ); - - if ( new_n == NULL ) - { - PRINT_ERROR ( "cRosNodeCreate() : Can't allocate memory\n" ); - return NULL; - } - - new_n->name = new_n->host = new_n->roscore_host = NULL; - - new_n->name = cRosNamespaceBuild(NULL, node_name); - new_n->host = ( char * ) malloc ( ( strlen ( node_host ) + 1 ) *sizeof ( char ) ); - new_n->roscore_host = ( char * ) malloc ( ( strlen ( roscore_host ) + 1 ) *sizeof ( char ) ); - new_n->message_root_path = ( char * ) malloc ( ( strlen ( message_root_path ) + 1 ) *sizeof ( char ) ); - - if (new_n->name == NULL || new_n->host == NULL - || new_n->roscore_host == NULL || new_n->message_root_path == NULL ) - { - PRINT_ERROR ( "cRosNodeCreate() : Can't allocate memory\n" ); - cRosNodeDestroy ( new_n ); - return NULL; - } - - strcpy ( new_n->host, node_host ); - strcpy ( new_n->roscore_host, roscore_host ); - strcpy ( new_n->message_root_path, message_root_path ); - - new_n->log_level = CROS_LOGLEVEL_INFO; - new_n->xmlrpc_port = 0; - new_n->tcpros_port = 0; - new_n->roscore_port = roscore_port; - new_n->roscore_pid = -1; - - xmlrpcProcessInit( &(new_n->xmlrpc_listner_proc) ); - - new_n->next_call_id = 0; - initApiCallQueue(&new_n->master_api_queue); - initApiCallQueue(&new_n->slave_api_queue); - - new_n->xmlrpc_master_wake_up_time = 0; - - int i, fn_ret; - for (i = 0 ; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++) - xmlrpcProcessInit( &(new_n->xmlrpc_server_proc[i]) ); - - for ( i = 0 ; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++) - xmlrpcProcessInit( &(new_n->xmlrpc_client_proc[i]) ); - - tcprosProcessInit( &(new_n->tcpros_listner_proc) ); - - for ( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++) - tcprosProcessInit( &(new_n->tcpros_server_proc[i]) ); - - for ( i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++) - tcprosProcessInit( &(new_n->tcpros_client_proc[i]) ); - - tcprosProcessInit( &(new_n->rpcros_listner_proc) ); - - for ( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++) - tcprosProcessInit( &(new_n->rpcros_server_proc[i]) ); - - for ( i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++) - tcprosProcessInit( &(new_n->rpcros_client_proc[i]) ); - - for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS; i++) - initPublisherNode(&new_n->pubs[i]); - new_n->n_pubs = 0; - - for ( i = 0; i < CN_MAX_SUBSCRIBED_TOPICS; i++) - initSubscriberNode(&new_n->subs[i]); - new_n->n_subs = 0; - - for ( i = 0; i < CN_MAX_SERVICE_PROVIDERS; i++) - initServiceProviderNode(&new_n->service_providers[i]); - new_n->n_service_providers = 0; - - for ( i = 0; i < CN_MAX_SERVICE_CALLERS; i++) - initServiceCallerNode(&new_n->service_callers[i]); - new_n->n_service_callers = 0; - - for ( i = 0; i < CN_MAX_PARAMETER_SUBSCRIPTIONS; i++) - initParameterSubscrition(&new_n->paramsubs[i]); - new_n->n_paramsubs = 0; - -#ifdef _WIN32 - new_n->pid = (int)GetCurrentProcessId(); -#else - new_n->pid = (int)getpid(); -#endif - - fn_ret = 0; - for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS && fn_ret == 0; i++) - { - fn_ret = openXmlrpcClientSocket( new_n, i ); - } - - for(i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS && fn_ret == 0; i++) - { - fn_ret = openTcprosClientSocket( new_n, i ); - } - - for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS && fn_ret == 0; i++) - { - fn_ret = openRpcrosClientSocket( new_n, i ); - } - if(fn_ret == 0) - fn_ret = openXmlrpcListnerSocket( new_n ); - if(fn_ret == 0) - fn_ret = openTcprosListnerSocket( new_n ); - if(fn_ret == 0) - fn_ret = openRpcrosListnerSocket( new_n ); - - if(fn_ret != 0) - { - PRINT_ERROR ( "cRosNodeCreate() : socket could not be opened\n" ); - cRosNodeDestroy ( new_n ); - return NULL; - } - - new_n-> log_last_id = 0; - - /* - * Registering logging callback - */ - - cRosErrCodePack ret_err; - - // Create a publisher of the topic /rosout of type "rosgraph_msgs/Log". - // The messages of this topic will not be periodically sent but on demand (loop_period = -1). - ret_err = cRosApiRegisterPublisher(new_n,"/rosout","rosgraph_msgs/Log", -1, NULL, NULL, new_n, &new_n->rosout_pub_idx); - if (ret_err != CROS_SUCCESS_ERR_PACK) - { - PRINT_ERROR ( "cRosNodeCreate(): Error registering rosout.\n"); - cRosPrintErrCodePack(ret_err, "cRosNodeCreate()"); - } - - ret_err = cRosApiRegisterServiceProvider(new_n,"~get_loggers","roscpp/GetLoggers", - callback_srv_get_loggers, NULL, (void *)new_n, NULL); - if (ret_err != CROS_SUCCESS_ERR_PACK) - { - PRINT_ERROR ( "cRosNodeCreate(): Error registering loggers.\n"); - cRosPrintErrCodePack(ret_err, "cRosNodeCreate()"); - } - - ret_err = cRosApiRegisterServiceProvider(new_n,"~set_logger_level","roscpp/SetLoggerLevel", - callback_srv_set_logger_level, NULL, (void *)new_n, NULL); - if (ret_err != CROS_SUCCESS_ERR_PACK) - { - PRINT_ERROR ( "cRosNodeCreate(): Error registering loggers.\n"); - cRosPrintErrCodePack(ret_err, "cRosNodeCreate()"); - } - - return new_n; -} - -int cRosUnregistrationCompleted(CrosNode *n) -{ - int i, unreg_finished; - - unreg_finished = 1; - - for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS && unreg_finished == 1; i++) - if(n->pubs[i].topic_name != NULL) - unreg_finished = 0; - - for ( i = 0; i < CN_MAX_SUBSCRIBED_TOPICS && unreg_finished == 1; i++) - if(n->subs[i].topic_name != NULL) - unreg_finished = 0; - - for ( i = 0; i < CN_MAX_SERVICE_PROVIDERS && unreg_finished == 1; i++) - if(n->service_providers[i].service_name != NULL) - unreg_finished = 0; - - for ( i = 0; i < CN_MAX_PARAMETER_SUBSCRIPTIONS && unreg_finished == 1; i++) - if(n->paramsubs[i].parameter_key != NULL) - unreg_finished = 0; - - return(unreg_finished); -} - -int cRosOutputQueuesEmpty(CrosNode *n) -{ - int i, queues_empty; - - queues_empty = 1; - - for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS && queues_empty == 1; i++) - if(n->pubs[i].topic_name != NULL && cRosMessageQueueUsage(&n->pubs[i].msg_queue) > 0) - queues_empty = 0; - - for ( i = 0; i < CN_MAX_SERVICE_CALLERS && queues_empty == 1; i++) - if(n->service_callers[i].service_name != NULL && cRosMessageQueueUsage(&n->service_callers[i].msg_queue) > 0) - queues_empty = 0; - - return(queues_empty); -} - -// This function continues to run the cROS node until the specified function returns true (1) -cRosErrCodePack cRosNodeWaitUntilFnRetTrue(CrosNode *n, int (*fn_to_check)(CrosNode *n)) -{ - int unreg_incomp; - cRosErrCodePack ret_err = CROS_SUCCESS_ERR_PACK; - uint64_t start_time, elapsed_time; - - start_time = cRosClockGetTimeMs(); - while( (unreg_incomp = !fn_to_check(n)) && (elapsed_time=cRosClockGetTimeMs()-start_time) <= CN_UNREGISTRATION_TIMEOUT && ret_err == CROS_SUCCESS_ERR_PACK) - ret_err=cRosNodeDoEventsLoop( n, CN_UNREGISTRATION_TIMEOUT - elapsed_time); - - if(ret_err == CROS_SUCCESS_ERR_PACK && unreg_incomp) - ret_err = CROS_UNREG_TIMEOUT_ERR; // the unregistration process could not be completed - - return ret_err; -} - -void cRosNodePauseAllCallersPublishers(CrosNode *n) -{ - int i; - - for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS; i++) - if(n->pubs[i].topic_name != NULL) - n->pubs[i].loop_period = -1; - - for ( i = 0; i < CN_MAX_SERVICE_CALLERS; i++) - if(n->service_callers[i].service_name != NULL) - n->service_callers[i].loop_period = -1; -} - -cRosErrCodePack cRosNodeUnregisterAll(CrosNode *n) -{ - int i; - cRosErrCodePack ret_err=CROS_SUCCESS_ERR_PACK; - - for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS && ret_err == CROS_SUCCESS_ERR_PACK; i++) - if(n->pubs[i].topic_name != NULL) - ret_err = cRosApiUnregisterPublisher(n, i); - - for ( i = 0; i < CN_MAX_SUBSCRIBED_TOPICS && ret_err == CROS_SUCCESS_ERR_PACK; i++) - if(n->subs[i].topic_name != NULL) - ret_err = cRosApiUnregisterSubscriber(n, i); - - for ( i = 0; i < CN_MAX_SERVICE_PROVIDERS && ret_err == CROS_SUCCESS_ERR_PACK; i++) - if(n->service_providers[i].service_name != NULL) - ret_err = cRosApiUnregisterServiceProvider(n, i); - - for ( i = 0; i < CN_MAX_PARAMETER_SUBSCRIPTIONS && ret_err == CROS_SUCCESS_ERR_PACK; i++) - if(n->paramsubs[i].parameter_key != NULL) - ret_err = cRosApiUnsubscribeParam(n, i); - - return ret_err; -} - -cRosErrCodePack cRosNodeDestroy ( CrosNode *n ) -{ - cRosErrCodePack ret_err; - - PRINT_VVDEBUG ( "cRosNodeDestroy()\n" ); - - if ( n == NULL ) - return CROS_BAD_PARAM_ERR; - - cRosNodeWaitUntilFnRetTrue(n, cRosOutputQueuesEmpty); // Wait until all pendind topic messages and service call have been sent - - cRosNodePauseAllCallersPublishers(n); - ret_err = cRosNodeUnregisterAll(n); - if(ret_err == CROS_SUCCESS_ERR_PACK) - ret_err = cRosNodeWaitUntilFnRetTrue(n, cRosUnregistrationCompleted); // Wait until all roles have been unsuscribed - - xmlrpcProcessRelease( &(n->xmlrpc_listner_proc) ); - - releaseApiCallQueue(&n->master_api_queue); - releaseApiCallQueue(&n->slave_api_queue); - - int i; - for (i = 0; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++) - xmlrpcProcessRelease( &(n->xmlrpc_server_proc[i]) ); - - for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++) - xmlrpcProcessRelease( &(n->xmlrpc_client_proc[i]) ); - - tcprosProcessRelease( &(n->tcpros_listner_proc) ); - - for ( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++) - tcprosProcessRelease( &(n->tcpros_server_proc[i]) ); - - for ( i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++) - tcprosProcessRelease( &(n->tcpros_client_proc[i]) ); - - for ( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++) - tcprosProcessRelease( &(n->rpcros_server_proc[i]) ); - - for ( i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++) - tcprosProcessRelease( &(n->rpcros_client_proc[i]) ); - - if ( n->name != NULL ) free ( n->name ); - if ( n->host != NULL ) free ( n->host ); - if ( n->roscore_host != NULL ) free ( n->roscore_host ); - - for ( i = 0; i < CN_MAX_PUBLISHED_TOPICS; i++) - cRosApiReleasePublisher(n, i); - - for ( i = 0; i < CN_MAX_SUBSCRIBED_TOPICS; i++) - cRosApiReleaseSubscriber(n, i); - - for ( i = 0; i < CN_MAX_SERVICE_PROVIDERS; i++) - cRosApiReleaseServiceProvider(n, i); - - for ( i = 0; i < CN_MAX_SERVICE_CALLERS; i++) - cRosApiReleaseServiceCaller(n, i); - - for ( i = 0; i < CN_MAX_PARAMETER_SUBSCRIPTIONS; i++) - cRosNodeReleaseParameterSubscrition(&n->paramsubs[i]); - - tcpIpSocketCleanUp(); - - return ret_err; -} - - int cRosNodeRegisterPublisher (CrosNode *node, const char *message_definition, const char *topic_name, - const char *topic_type, const char *md5sum, int loop_period, void *data_context) -{ - PRINT_VVDEBUG ( "cRosNodeRegisterPublisher()\n" ); - - if ( node->n_pubs >= CN_MAX_PUBLISHED_TOPICS ) - { - PRINT_ERROR ( "cRosNodeRegisterPublisher() : Can't register a new publisher: \ - reached the maximum number of published topics\n"); - return -1; - } - - char *pub_message_definition = ( char * ) malloc ( ( strlen ( message_definition ) + 1 ) * sizeof ( char ) ); - char *pub_topic_name = cRosNamespaceBuild(node, topic_name); - char *pub_topic_type = ( char * ) malloc ( ( strlen ( topic_type ) + 1 ) * sizeof ( char ) ); - char *pub_md5sum = ( char * ) malloc ( ( strlen ( md5sum ) + 1 ) * sizeof ( char ) ); - - if ( pub_message_definition == NULL || pub_topic_name == NULL || - pub_topic_type == NULL || pub_md5sum == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterPublisher() : Can't allocate memory\n" ); - return -1; - } - - strcpy ( pub_message_definition, message_definition ); - strcpy ( pub_topic_type, topic_type ); - strcpy ( pub_md5sum, md5sum ); - - PRINT_INFO ( "Publishing topic %s type %s \n", pub_topic_name, pub_topic_type ); - - int pubidx = -1; - int it = 0; - for (; it < CN_MAX_PUBLISHED_TOPICS; it++) - { - if (node->pubs[it].topic_name == NULL) - { - pubidx = it; - break; - } - } - - PublisherNode *pub = &node->pubs[pubidx]; - pub->message_definition = pub_message_definition; - pub->topic_name = pub_topic_name; - pub->topic_type = pub_topic_type; - pub->md5sum = pub_md5sum; - - pub->loop_period = loop_period; - pub->wake_up_time = 0; - pub->context = data_context; - cRosMessageQueueClear(&pub->msg_queue); - - node->n_pubs++; - - int rc = enqueuePublisherAdvertise(node, pubidx); - if (rc == -1) - return -1; - - return pubidx; -} - -int cRosNodeRegisterServiceProvider(CrosNode *node, const char *service_name, - const char *service_type, const char *md5sum, void *data_context) -{ - PRINT_VVDEBUG ( "cRosNodeRegisterServiceProvider()\n" ); - - if (node->n_service_providers >= CN_MAX_SERVICE_PROVIDERS) - { - PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't register a new service provider: \ - reached the maximum number of service providers\n"); - return -1; - } - - char *srv_service_name = cRosNamespaceBuild(node, service_name); - char *srv_service_type = ( char * ) malloc ( ( strlen ( service_type ) + 1 ) * sizeof ( char ) ); - char *srv_servicerequest_type = ( char * ) malloc ( ( strlen ( service_type ) + strlen("Request") + 1 ) * sizeof ( char ) ); - char *srv_serviceresponse_type = ( char * ) malloc ( ( strlen ( service_type ) + strlen("Response") + 1 ) * sizeof ( char ) ); - char *srv_md5sum = ( char * ) malloc ( ( strlen ( md5sum ) + 1 ) * sizeof ( char ) ); - - if ( srv_service_name == NULL || srv_service_type == NULL || - srv_md5sum == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't allocate memory\n" ); - return -1; - } - - strncpy ( srv_service_type, service_type, strlen ( service_type ) + 1 ); - strncpy ( srv_servicerequest_type, service_type, strlen ( service_type ) + 1 ); - strncat ( srv_servicerequest_type, "Request", strlen("Request") + 1 ); - strncpy ( srv_serviceresponse_type, service_type, strlen ( service_type ) + 1 ); - strncat ( srv_serviceresponse_type, "Response", strlen("Response") +1 ); - strncpy ( srv_md5sum, md5sum, strlen(md5sum) + 1 ); - - PRINT_INFO ( "Registering service provider %s type %s \n", srv_service_name, srv_service_type); - - int serviceidx = -1; - int it = 0; - for (; it < CN_MAX_SERVICE_PROVIDERS; it++) - { - if (node->service_providers[it].service_name == NULL) - { - serviceidx = it; - break; - } - } - - ServiceProviderNode *service = &(node->service_providers[serviceidx]); - - service->service_name = srv_service_name; - service->service_type = srv_service_type; - service->servicerequest_type = srv_servicerequest_type; - service->serviceresponse_type = srv_serviceresponse_type; - service->md5sum = srv_md5sum; - service->context = data_context; - - node->n_service_providers++; - - int rc = enqueueServiceAdvertise(node, serviceidx); - if (rc == -1) - return -1; - - return serviceidx; -} - -int cRosNodeRecruitTcprosClientProc(CrosNode *node, int subidx) -{ - int ret; // Return value: -1 on error, or the recruited proc index on success - - SubscriberNode *sub; - TcprosProcess *client_proc; - - // Look for a free Tcpros client proc - int clientidx; - client_proc=NULL; - sub = &node->subs[subidx]; - ret=-1; // Default return value (No free Tcpros client proc is available) - for(clientidx=0;clientidxtcpros_client_proc[clientidx]; - if(client_proc->topic_idx == -1) // A free Tcpros client proc has been found - { - client_proc->topic_idx = subidx; - client_proc->tcp_nodelay = (unsigned char)sub->tcp_nodelay; - ret=clientidx; // Exit loop - } - } - return ret; -} - -int cRosNodeFindFirstTcprosClientProc(CrosNode *node, int subidx, const char *tcpros_hostname, int tcpros_port) -{ - int ret; // Return value: -1 on error, or the found proc index on success - int clientidx; - - // Look for the first Tcpros client proc that was recruited for subidx subscriber and a specific publisher host and port - ret=-1; - for(clientidx=0;clientidxtcpros_client_proc[clientidx]; - if((subidx == -1 || cur_cli->topic_idx == subidx) && - (tcpros_port == -1 || cur_cli->sub_tcpros_port == tcpros_port) && - (tcpros_hostname == NULL || strcmp(cur_cli->sub_tcpros_host,tcpros_hostname)==0) - ) - ret=clientidx; - } - return ret; -} - -int cRosNodeRegisterSubscriber(CrosNode *node, const char *message_definition, const char *topic_name, - const char *topic_type, const char *md5sum, void *data_context, int tcp_nodelay) -{ - PRINT_VVDEBUG ( "cRosNodeRegisterSubscriber()\n" ); - - if (node->n_subs >= CN_MAX_SUBSCRIBED_TOPICS) - { - PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't register a new subscriber: \ - reached the maximum number of published topics\n"); - return -1; - } - - char *pub_message_definition = ( char * ) malloc ( ( strlen ( message_definition ) + 1 ) * sizeof ( char ) ); - char *pub_topic_name = cRosNamespaceBuild(node, topic_name); - char *pub_topic_type = ( char * ) malloc ( ( strlen ( topic_type ) + 1 ) * sizeof ( char ) ); - char *pub_md5sum = ( char * ) malloc ( ( strlen ( md5sum ) + 1 ) * sizeof ( char ) ); - - if ( pub_topic_name == NULL || pub_topic_type == NULL ) - { - PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n" ); - return -1; - } - - strcpy ( pub_message_definition, message_definition ); - strcpy ( pub_topic_name, topic_name ); - strcpy ( pub_topic_type, topic_type ); - strcpy ( pub_md5sum, md5sum ); - - PRINT_INFO ( "Subscribing to topic %s type %s \n", pub_topic_name, pub_topic_type ); - - int subidx = node->n_subs; - int it = 0; - for (; it < CN_MAX_SUBSCRIBED_TOPICS; it++) - { - if (node->subs[it].topic_name == NULL) - { - subidx = it; - break; - } - } - - SubscriberNode *sub = &node->subs[subidx]; - sub->message_definition = pub_message_definition; - sub->topic_name = pub_topic_name; - sub->topic_type = pub_topic_type; - sub->md5sum = pub_md5sum; - sub->context = data_context; - sub->tcp_nodelay = (unsigned char)tcp_nodelay; - sub->msg_queue_overflow = 0; - cRosMessageQueueClear(&sub->msg_queue); - - node->n_subs++; - - int rc = enqueueSubscriberAdvertise(node, subidx); - if (rc == -1) - return -1; - - return subidx; -} - -int cRosNodeRegisterServiceCaller(CrosNode *node, const char *message_definition, const char *service_name, - const char *service_type, const char *md5sum, int loop_period, - void *data_context, int persistent, int tcp_nodelay) -{ - PRINT_VVDEBUG ( "cRosNodeRegisterServiceCaller()\n" ); - - if (node->n_service_callers >= CN_MAX_SERVICE_CALLERS) - { - PRINT_ERROR ( "cRosNodeRegisterServiceCaller() : Can't register a new service caller: \ - reached the maximum number of service callers\n"); - return -1; - } - - char *srv_message_definition = ( char * ) malloc ( ( strlen ( message_definition ) + 1 ) * sizeof ( char ) ); - char *srv_service_name = cRosNamespaceBuild(node, service_name); - char *srv_service_type = ( char * ) malloc ( ( strlen ( service_type ) + 1 ) * sizeof ( char ) ); - char *srv_servicerequest_type = ( char * ) malloc ( ( strlen ( service_type ) + strlen("Request") + 1 ) * sizeof ( char ) ); - char *srv_serviceresponse_type = ( char * ) malloc ( ( strlen ( service_type ) + strlen("Response") + 1 ) * sizeof ( char ) ); - char *srv_md5sum = ( char * ) malloc ( ( strlen ( md5sum ) + 1 ) * sizeof ( char ) ); - - if ( srv_service_name == NULL || srv_service_type == NULL || - srv_md5sum == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterServiceCaller() : Can't allocate memory\n" ); - return -1; - } - - strcpy ( srv_message_definition, message_definition ); - strncpy ( srv_service_type, service_type, strlen ( service_type ) + 1 ); - strncpy ( srv_servicerequest_type, service_type, strlen ( service_type ) + 1 ); - strncat ( srv_servicerequest_type, "Request", strlen("Request") + 1 ); - strncpy ( srv_serviceresponse_type, service_type, strlen ( service_type ) + 1 ); - strncat ( srv_serviceresponse_type, "Response", strlen("Response") +1 ); - strncpy ( srv_md5sum, md5sum, strlen(md5sum) + 1 ); - - PRINT_INFO ( "Starting service caller %s type %s \n", srv_service_name, srv_service_type); - - int serviceidx = -1; - int it = 0; - for (; it < CN_MAX_SERVICE_CALLERS; it++) - { - if (node->service_callers[it].service_name == NULL) - { - serviceidx = it; - break; - } - } - - ServiceCallerNode *service = &(node->service_callers[serviceidx]); - service->message_definition = srv_message_definition; - service->service_name = srv_service_name; - service->service_type = srv_service_type; - service->servicerequest_type = srv_servicerequest_type; - service->serviceresponse_type = srv_serviceresponse_type; - service->md5sum = srv_md5sum; - service->context = data_context; - service->loop_period = loop_period; - service->wake_up_time = 0; // cRosClockGetTimeMs() + 10; - service->persistent = (unsigned char)persistent; - service->tcp_nodelay = (unsigned char)tcp_nodelay; - - int clientidx = serviceidx; // node->service_callers[0] is assigned node->rpcros_client_proc[0] and so on - service->rpcros_id = clientidx; - - TcprosProcess *client_proc = &node->rpcros_client_proc[clientidx]; - client_proc->service_idx = serviceidx; - client_proc->persistent = (unsigned char)persistent; - client_proc->tcp_nodelay = (unsigned char)tcp_nodelay; - - node->n_service_callers++; - - int rc = enqueueServiceLookup(node, serviceidx); - if (rc == -1) - return -1; - - return serviceidx; -} - - -int cRosNodeUnregisterSubscriber(CrosNode *node, int subidx) -{ - int client_tcpros_ind, client_xmlrpc_ind; - if (subidx < 0 || subidx >= CN_MAX_SUBSCRIBED_TOPICS) - return -1; - - SubscriberNode *sub = &node->subs[subidx]; - if (sub->topic_name == NULL) - return -1; - - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeUnRegisterSubscriber() : Can't allocate memory\n"); - return -1; - } - - while((client_tcpros_ind=cRosNodeFindFirstTcprosClientProc(node, subidx, NULL, -1)) != -1) - { - TcprosProcess *tcprosProc = &node->tcpros_client_proc[client_tcpros_ind]; - closeTcprosProcess(tcprosProc); - } - - // Check if any xmlrpc_client_proc is working for the subscriber being unregistered and if so, close them - for(client_xmlrpc_ind=0;client_xmlrpc_ind < CN_MAX_XMLRPC_CLIENT_CONNECTIONS;client_xmlrpc_ind++) - { - XmlrpcProcess *xmlrpcProc = &node->xmlrpc_client_proc[client_xmlrpc_ind]; - if(xmlrpcProc->state != XMLRPC_PROCESS_STATE_IDLE && xmlrpcProc->current_call != NULL && - xmlrpcProc->current_call->method == CROS_API_REQUEST_TOPIC && xmlrpcProc->current_call->provider_idx == subidx) - closeXmlrpcProcess(xmlrpcProc); - } - - XmlrpcProcess *coreproc = &node->xmlrpc_client_proc[0]; - if (coreproc->current_call != NULL - && coreproc->current_call->method == CROS_API_REGISTER_SUBSCRIBER - && coreproc->current_call->provider_idx == subidx) - { - // Delist current registration - closeXmlrpcProcess(coreproc); - } - - call->method = CROS_API_UNREGISTER_SUBSCRIBER; - call->provider_idx = subidx; - - xmlrpcParamVectorPushBackString( &call->params, node->name); - xmlrpcParamVectorPushBackString( &call->params, sub->topic_name ); - char node_uri[256]; - snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); - xmlrpcParamVectorPushBackString( &call->params, node_uri ); - - // NB: Node release is done in handleApiCallAttempt() - - if(enqueueMasterApiCallInternal(node, call) < 0) - return -1; - else - return 0; -} - -int cRosNodeUnregisterPublisher(CrosNode *node, int pubidx) -{ - int list_elem; - if (pubidx < 0 || pubidx >= CN_MAX_PUBLISHED_TOPICS) - return -1; - - PublisherNode *pub = &node->pubs[pubidx]; - if (pub->topic_name == NULL) - return -1; - - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeUnRegisterPublisher() : Can't allocate memory\n"); - return -1; - } - - for(list_elem=0;pub->tcpros_id_list[list_elem]!=-1;list_elem++) - { - TcprosProcess *tcprosProc = &node->tcpros_server_proc[pub->tcpros_id_list[list_elem]]; - closeTcprosProcess(tcprosProc); - } - - XmlrpcProcess *coreproc = &node->xmlrpc_client_proc[0]; - if (coreproc->current_call != NULL - && coreproc->current_call->method == CROS_API_REGISTER_PUBLISHER - && coreproc->current_call->provider_idx == pubidx) - { - // Delist current registration - closeXmlrpcProcess(coreproc); - } - - call->method = CROS_API_UNREGISTER_PUBLISHER; - call->provider_idx = pubidx; - - xmlrpcParamVectorPushBackString( &call->params, node->name); - xmlrpcParamVectorPushBackString( &call->params, pub->topic_name ); - char node_uri[256]; - snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); - xmlrpcParamVectorPushBackString( &call->params, node_uri ); - - // NB: Node release is done in handleApiCallAttempt() - - if(enqueueMasterApiCallInternal(node, call) < 0) - return -1; - else - return 0; -} - -int cRosNodeUnregisterServiceProvider(CrosNode *node, int serviceidx) -{ - if (serviceidx < 0 || serviceidx >= CN_MAX_SERVICE_PROVIDERS) - return -1; - - ServiceProviderNode *svc = &node->service_providers[serviceidx]; - if (svc->service_name == NULL) - return -1; - - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeUnregisterServiceProvider() : Can't allocate memory\n"); - return -1; - } - - XmlrpcProcess *coreproc = &node->xmlrpc_client_proc[0]; - if (coreproc->current_call != NULL - && coreproc->current_call->method == CROS_API_REGISTER_SERVICE - && coreproc->current_call->provider_idx == serviceidx) - { - // Delist current registration - closeXmlrpcProcess(coreproc); - } - - call->method = CROS_API_UNREGISTER_SERVICE; - call->provider_idx = serviceidx; - // 3 parameters are expected for this method - xmlrpcParamVectorPushBackString( &call->params, node->name); - xmlrpcParamVectorPushBackString( &call->params, svc->service_name); - char uri[256]; - snprintf( uri, 256, "rosrpc://%s:%d/", node->host, node->rpcros_port); - xmlrpcParamVectorPushBackString( &call->params, uri ); -// snprintf( uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); -// xmlrpcParamVectorPushBackString( &call->params, uri ); - - // NB: Node release is done in handleApiCallAttempt() - - if(enqueueMasterApiCallInternal(node, call) < 0) - return -1; - else - return 0; -} - -cRosErrCodePack cRosApiSubscribeParam(CrosNode *node, const char *key, NodeStatusApiCallback callback, void *context, int *paramsubidx_ptr) -{ - PRINT_VVDEBUG ( "cRosApiSubscribeParam()\n" ); - PRINT_INFO ( "Subscribing to parameter %s\n", key); - - if (node->n_paramsubs >= CN_MAX_PARAMETER_SUBSCRIPTIONS) - { - PRINT_ERROR ( "cRosApiSubscribeParam() : Can't register a new parameter subscription: \ - reached the maximum number of parameters\n"); - return CROS_MANY_PARAM_ERR; - } - - char *parameter_key = ( char * ) malloc ( ( strlen ( key ) + 1 ) * sizeof ( char ) ); - if (parameter_key == NULL) - { - PRINT_ERROR ( "cRosApiSubscribeParam() : Can't allocate memory\n" ); - return CROS_MEM_ALLOC_ERR; - } - - strcpy (parameter_key, key); - - int paramsubidx = -1; // This value should never be used - int it; - for (it = 0; it < CN_MAX_PARAMETER_SUBSCRIPTIONS; it++) - { - if (node->paramsubs[it].parameter_key == NULL) - { - paramsubidx = it; - break; - } - } - - ParameterSubscription *sub = &node->paramsubs[paramsubidx]; - sub->parameter_key = parameter_key; - sub->context = context; - sub->status_api_callback = callback; - - node->n_paramsubs++; - - int callid = enqueueParameterSubscription(node, paramsubidx); - if (callid == -1) - { - free(parameter_key); - sub->parameter_key = NULL; - node->n_paramsubs--; - return CROS_MEM_ALLOC_ERR; - } - - if(paramsubidx_ptr != NULL) - *paramsubidx_ptr = paramsubidx; - - return CROS_SUCCESS_ERR_PACK; -} - -cRosErrCodePack cRosApiUnsubscribeParam(CrosNode *node, int paramsubidx) -{ - int caller_id; - - if (paramsubidx < 0 || paramsubidx >= CN_MAX_PARAMETER_SUBSCRIPTIONS) - return CROS_BAD_PARAM_ERR; - - ParameterSubscription *sub = &node->paramsubs[paramsubidx]; - if (sub->parameter_key == NULL) - return CROS_PARAM_SUB_IND_ERR; - - XmlrpcProcess *coreproc = &node->xmlrpc_client_proc[0]; - if (coreproc->current_call != NULL - && coreproc->current_call->method == CROS_API_SUBSCRIBE_PARAM - && coreproc->current_call->provider_idx == paramsubidx) - { - // Delist current registration - closeXmlrpcProcess(coreproc); - } - - caller_id = enqueueParameterUnsubscription(node, paramsubidx); - - return (caller_id != -1)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; -} - -cRosErrCodePack cRosNodeTriggerPublishersWriting( CrosNode *n, uint64_t cur_time ) -{ - cRosErrCodePack ret_err; - int pub_idx; - - ret_err = CROS_SUCCESS_ERR_PACK; // Default return value: success - // Check whether it is time to send a new topic message and trigger the corresponding TcprosProcesses - for(pub_idx = 0; pub_idx < CN_MAX_PUBLISHED_TOPICS; pub_idx++) - { - PublisherNode *cur_pub = &n->pubs[pub_idx]; - if(cur_pub->topic_name != NULL) // Is this publisher active? - { - if((cur_pub->loop_period >= 0 && cur_pub->wake_up_time <= cur_time) || cRosMessageQueueUsage(&cur_pub->msg_queue) > 0) // Is it time to publish a message (periodic or immediate)? - { - int list_elem, all_procs_ready; - // Check whether all tcpProcess are ready to start writing a new message (or idle) - all_procs_ready = 1; - for(list_elem=0;cur_pub->tcpros_id_list[list_elem]!=-1;list_elem++) - { - TcprosProcess *server_proc = &n->tcpros_server_proc[cur_pub->tcpros_id_list[list_elem]]; - if(server_proc->state != TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) // server_proc->state != TCPROS_PROCESS_STATE_IDLE && - { - all_procs_ready = 0; - break; - } - } - if(all_procs_ready && cur_pub->tcpros_id_list[0]!=-1) // All associated processes are ready to write and there is at least one associated process - { - if(cRosMessageQueueUsage(&cur_pub->msg_queue) == 0) // There is no immediate message waiting, so a periodic message must be sent - cur_pub->wake_up_time = cur_time + cur_pub->loop_period; - - // The next function will store the next message to be sent in cur_pub->context->outgoing - ret_err = cRosNodePublisherCallback(cur_pub->context); // Calls the publisher application-defined callback - - // Make all the waiting processes start writing - for(list_elem=0;cur_pub->tcpros_id_list[list_elem]!=-1;list_elem++) - { - TcprosProcess *server_proc = &n->tcpros_server_proc[cur_pub->tcpros_id_list[list_elem]]; - // if(server_proc->state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) - tcprosProcessChangeState( server_proc, TCPROS_PROCESS_STATE_START_WRITING ); - } - } - } - } - } - return(ret_err); -} - - -cRosErrCodePack cRosNodeTriggerServiceCallersWriting( CrosNode *n, uint64_t cur_time ) -{ - cRosErrCodePack ret_err; - int caller_idx; - - ret_err = CROS_SUCCESS_ERR_PACK; // Default return value: success - // Check whether it is time to make a service call, and if so, trigger the corresponding TcprosProcesses - for(caller_idx = 0; caller_idx < CN_MAX_SERVICE_CALLERS; caller_idx++) - { - ServiceCallerNode *cur_caller = &n->service_callers[caller_idx]; - if(cur_caller->service_name != NULL) // Is this caller active? - { - if((cur_caller->loop_period >= 0 && cur_caller->wake_up_time <= cur_time) || cRosMessageQueueUsage(&cur_caller->msg_queue) == 1) // Is it time to make a call (periodic or immediate)? - { - // Check whether the corresponding process is ready to start making a new call - TcprosProcess *caller_proc = &n->rpcros_client_proc[cur_caller->rpcros_id]; - if(caller_proc->state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) // caller_proc->state == TCPROS_PROCESS_STATE_IDLE || - { - if(cRosMessageQueueUsage(&cur_caller->msg_queue) == 0) // There is no immediate call waiting, so a periodic call will be made - cur_caller->wake_up_time = cur_time + cur_caller->loop_period; - - // Now the service-call parameters are stored in cur_caller->context->outgoing - ret_err = cRosNodeServiceCallerCallback( 0, cur_caller->context); // calls the service-caller application-defined callback function to generate the service request - - //if(caller_proc->state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) - { - tcprosProcessChangeState( caller_proc, TCPROS_PROCESS_STATE_START_WRITING ); - } - } - } - } - } - return(ret_err); -} - -#define NODE_STATUS_STR_MAX_LEN (CN_MAX_XMLRPC_CLIENT_CONNECTIONS+CN_MAX_XMLRPC_SERVER_CONNECTIONS+CN_MAX_TCPROS_CLIENT_CONNECTIONS+CN_MAX_TCPROS_SERVER_CONNECTIONS+CN_MAX_RPCROS_CLIENT_CONNECTIONS+CN_MAX_RPCROS_SERVER_CONNECTIONS+6*3+3*4+2) -void printNodeProcState( CrosNode *n ) -{ - static char prev_stat_str[NODE_STATUS_STR_MAX_LEN]="\0"; - char stat_str[NODE_STATUS_STR_MAX_LEN]; - int i; - - sprintf(stat_str, "XL%X XC",n->xmlrpc_listner_proc.state); - for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++ ) - sprintf(stat_str+strlen(stat_str), "%X",n->xmlrpc_client_proc[i].state); - - sprintf(stat_str+strlen(stat_str), " XS"); - for( i = 0; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++ ) - sprintf(stat_str+strlen(stat_str), "%X",n->xmlrpc_server_proc[i].state); - - sprintf(stat_str+strlen(stat_str), " TL%X TC",n->tcpros_listner_proc.state); - for(i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++ ) - sprintf(stat_str+strlen(stat_str), "%X",n->tcpros_client_proc[i].state); - - sprintf(stat_str+strlen(stat_str), " TS"); - for( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++ ) - sprintf(stat_str+strlen(stat_str), "%X",n->tcpros_server_proc[i].state); - - sprintf(stat_str+strlen(stat_str), " RL%X RC",n->rpcros_listner_proc.state); - for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++ ) - sprintf(stat_str+strlen(stat_str), "%X",n->rpcros_client_proc[i].state); - - sprintf(stat_str+strlen(stat_str), " RS"); - for( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++ ) - sprintf(stat_str+strlen(stat_str), "%X",n->rpcros_server_proc[i].state); - - if (strcmp(prev_stat_str, stat_str) != 0) // If the node status has changed: - { - // Print a compact string indicating the state of all the node processed for debug purposes - PRINT_DEBUG("Node status: %s\n", stat_str); - strcpy(prev_stat_str, stat_str); - } -} - -uint64_t cRosNodeCalculateSelectTimeout(CrosNode *n, uint64_t max_timeout) -{ - uint64_t wakeup_timeout, select_timeout, cur_time; - int pub_idx, svc_idx; - - select_timeout = max_timeout; - cur_time = cRosClockGetTimeMs(); - - if( n->xmlrpc_master_wake_up_time > cur_time ) - wakeup_timeout = n->xmlrpc_master_wake_up_time - cur_time; - else - wakeup_timeout = 0; - - if( wakeup_timeout < select_timeout ) - select_timeout = wakeup_timeout; - - for (pub_idx = 0;pub_idx < CN_MAX_PUBLISHED_TOPICS;pub_idx++) // < n_pubs? - { - PublisherNode *cur_pub = &n->pubs[pub_idx]; - if(cur_pub->topic_name != NULL && cur_pub->loop_period >= 0) // Is this publisher active and automatically publishing messages? - { - if( cur_pub->wake_up_time > cur_time ) // Is not it time to publish a new message yet? - wakeup_timeout = cur_pub->wake_up_time - cur_time; - else - wakeup_timeout = 0; - - if( wakeup_timeout < select_timeout ) - select_timeout = wakeup_timeout; - } - } - - for (svc_idx = 0;svc_idx < CN_MAX_SERVICE_CALLERS;svc_idx++) // service_callers[svc_idx]; - if(cur_svc_caller->service_name != NULL && cur_svc_caller->loop_period >= 0) // Is this service caller active and automatically publishing messages? - { - if( cur_svc_caller->wake_up_time > cur_time ) // Is not it time to make a service call yet? - wakeup_timeout = cur_svc_caller->wake_up_time - cur_time; - else - wakeup_timeout = 0; - - if( wakeup_timeout < select_timeout ) - { - //printf("[i:%i cur time: %lu wake:%lu new timeout: %lu prev timeout:%lu]", i, cur_time, cur_svc_caller->wake_up_time, wakeup_timeout, select_timeout); - select_timeout = wakeup_timeout; - } - } - } - - return(select_timeout); -} - -cRosErrCodePack cRosNodeDoEventsLoop( CrosNode *n, uint64_t max_timeout ) -{ - cRosErrCodePack ret_err, new_errors; - uint64_t cur_time, select_timeout; - int nfds = -1; - fd_set r_fds, w_fds, err_fds; - int i; - - PRINT_VVDEBUG ( "cRosNodeDoEventsLoop ()\n" ); - - if(n == NULL) - return CROS_BAD_PARAM_ERR; - - #if CROS_DEBUG_LEVEL >= 2 - PRINT_DEBUG("*"); - FLUSH_PRINT(); - printNodeProcState( n ); - #endif - - cur_time = cRosClockGetTimeMs(); - - ret_err = cRosNodeTriggerPublishersWriting( n, cur_time ); - - new_errors = cRosNodeTriggerServiceCallersWriting( n, cur_time ); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - - FD_ZERO( &r_fds ); - FD_ZERO( &w_fds ); - FD_ZERO( &err_fds ); - - int xmlrpc_listner_fd = tcpIpSocketGetFD( &(n->xmlrpc_listner_proc.socket) ); - int tcpros_listner_fd = tcpIpSocketGetFD( &(n->tcpros_listner_proc.socket) ); - int rpcros_listner_fd = tcpIpSocketGetFD( &(n->rpcros_listner_proc.socket) ); - - XmlrpcProcess *coreproc = &n->xmlrpc_client_proc[0]; - if (coreproc->state == XMLRPC_PROCESS_STATE_IDLE && !isQueueEmpty(&n->master_api_queue)) - { - RosApiCall *call = dequeueApiCall(&n->master_api_queue); - coreproc->current_call = call; - xmlrpcProcessChangeState( coreproc, XMLRPC_PROCESS_STATE_CONNECTING ); - } - - size_t idle_client_count; - int idle_clients[CN_MAX_XMLRPC_CLIENT_CONNECTIONS]; - getIdleXmplrpcClients(n, idle_clients, &idle_client_count); - - int next_idle_client_idx = 0; - while (!isQueueEmpty(&n->slave_api_queue) && next_idle_client_idx != idle_client_count) - { - int idle_client_idx = idle_clients[next_idle_client_idx]; - RosApiCall *call = dequeueApiCall(&n->slave_api_queue); - - XmlrpcProcess *proc = &n->xmlrpc_client_proc[idle_client_idx]; - proc->current_call = call; - xmlrpcProcessChangeState( proc, XMLRPC_PROCESS_STATE_CONNECTING ); - - next_idle_client_idx++; - } - - /* If active (not idle state), add to the tcpIpSocketSelect() the XMLRPC clients */ - for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++) - { - fd_set *fdset = NULL; - if( n->xmlrpc_client_proc[i].state == XMLRPC_PROCESS_STATE_CONNECTING ) - { - new_errors = xmlrpcClientConnect(n, i); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - fdset = &w_fds; // tcpIpSocketSelect() will acknowledge the socket connection completion in the file descriptors checked for writing - } - else if( n->xmlrpc_client_proc[i].state == XMLRPC_PROCESS_STATE_WRITING ) - fdset = &w_fds; - else if( n->xmlrpc_client_proc[i].state == XMLRPC_PROCESS_STATE_READING ) - fdset = &r_fds; - - if (fdset != NULL) - { - int xmlrpc_client_fd = tcpIpSocketGetFD( &(n->xmlrpc_client_proc[i].socket) ); - if(xmlrpc_client_fd != -1) - { - if( xmlrpc_client_fd > nfds ) - nfds = xmlrpc_client_fd; - - FD_SET( xmlrpc_client_fd, fdset); - FD_SET( xmlrpc_client_fd, &err_fds); - } - } - } - - //printf("FD_SET COUNT. R: %d W: %d\n", r_count, w_count); - - /* Add to the tcpIpSocketSelect() the active XMLRPC servers */ - int next_xmlrpc_server_i = -1; - for( i = 0; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++ ) - { - int server_fd = tcpIpSocketGetFD( &(n->xmlrpc_server_proc[i].socket) ); - - if( next_xmlrpc_server_i < 0 && - n->xmlrpc_server_proc[i].state == XMLRPC_PROCESS_STATE_IDLE ) - { - next_xmlrpc_server_i = i; - } - else if( n->xmlrpc_server_proc[i].state == XMLRPC_PROCESS_STATE_READING ) - { - FD_SET( server_fd, &r_fds); - FD_SET( server_fd, &err_fds); - if( server_fd > nfds ) nfds = server_fd; - } - else if( n->xmlrpc_server_proc[i].state == XMLRPC_PROCESS_STATE_WRITING ) - { - FD_SET( server_fd, &w_fds); - FD_SET( server_fd, &err_fds); - if( server_fd > nfds ) nfds = server_fd; - } - } - - /* If one XMLRPC server is active at least, add to the tcpIpSocketSelect() the listener socket */ - if( next_xmlrpc_server_i >= 0) - { - if(xmlrpc_listner_fd != -1) // If the listener socket is still opened, add it to the tcpIpSocketSelect() file descriptors - { - FD_SET( xmlrpc_listner_fd, &r_fds); - FD_SET( xmlrpc_listner_fd, &err_fds); - if( xmlrpc_listner_fd > nfds ) nfds = xmlrpc_listner_fd; - } - } - - /* - * - * TCPROS PROCESSES tcpIpSocketSelect() MANAGEMENT - * - */ - - /* If active (not idle state), add to the tcpIpSocketSelect() the TCPROS clients */ - int next_tcpros_client_i = -1; // Unused ??? - for(i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++) - { - TcprosProcess *client_proc = &(n->tcpros_client_proc[i]); - int tcpros_client_fd; - - if( next_tcpros_client_i < 0 && - i != 0 && //the zero-index is reserved to the roscore communications - client_proc->state == TCPROS_PROCESS_STATE_IDLE ) - { - next_tcpros_client_i = i; - } - else if(client_proc->state == TCPROS_PROCESS_STATE_CONNECTING) - { - new_errors = tcprosClientConnect(n, i); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - - tcpros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); - FD_SET( tcpros_client_fd, &w_fds); - FD_SET( tcpros_client_fd, &err_fds); - if( tcpros_client_fd > nfds ) nfds = tcpros_client_fd; - } - else if(client_proc->state == TCPROS_PROCESS_STATE_WRITING_HEADER) - { - tcpros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); - FD_SET( tcpros_client_fd, &w_fds); - FD_SET( tcpros_client_fd, &err_fds); - if( tcpros_client_fd > nfds ) nfds = tcpros_client_fd; - } - else if(client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE || - client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER || - client_proc->state == TCPROS_PROCESS_STATE_READING_SIZE || - client_proc->state == TCPROS_PROCESS_STATE_READING) - { - tcpros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); - FD_SET( tcpros_client_fd, &r_fds); - FD_SET( tcpros_client_fd, &err_fds); - if( tcpros_client_fd > nfds ) nfds = tcpros_client_fd; - } - } - - /* Add to the tcpIpSocketSelect() the active TCPROS servers */ - int next_tcpros_server_i = -1; // Index of the first server that is idle (ready to be activated). -1 = no one is idle - for( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++ ) - { - int server_fd = tcpIpSocketGetFD( &(n->tcpros_server_proc[i].socket) ); - - if( next_tcpros_server_i < 0 && - n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_IDLE ) - { - next_tcpros_server_i = i; - } - else if( n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER ) - { - FD_SET( server_fd, &r_fds); - FD_SET( server_fd, &err_fds); - if( server_fd > nfds ) nfds = server_fd; - } - else if( n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_START_WRITING || - n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_WRITING ) - { - FD_SET( server_fd, &w_fds); - FD_SET( server_fd, &err_fds); - if( server_fd > nfds ) nfds = server_fd; - } - else if( n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING ) - { - FD_SET( server_fd, &err_fds); - if( server_fd > nfds ) nfds = server_fd; - } - } - - /* If one TCPROS server is available at least, add to the tcpIpSocketSelect() the listener socket */ - if( next_tcpros_server_i >= 0) - { - if(tcpros_listner_fd != -1) // If the listener socket is still opened - { - FD_SET( tcpros_listner_fd, &r_fds); - FD_SET( tcpros_listner_fd, &err_fds); - if( tcpros_listner_fd > nfds ) nfds = tcpros_listner_fd; - } - } - - - /* - * - * RPCROS PROCESSES tcpIpSocketSelect() MANAGEMENT - * - */ - - /* If active (not idle state), add to the tcpIpSocketSelect() the TCPROS clients */ - for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++) - { - int rpcros_client_fd; - - rpcros_client_fd = tcpIpSocketGetFD( &(n->rpcros_client_proc[i].socket) ); - if(n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_CONNECTING) - { - new_errors = rpcrosClientConnect(n, i); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - - rpcros_client_fd = tcpIpSocketGetFD( &(n->rpcros_client_proc[i].socket) ); // update file descriptor after connecting - FD_SET( rpcros_client_fd, &w_fds); - FD_SET( rpcros_client_fd, &err_fds); - if( rpcros_client_fd > nfds ) nfds = rpcros_client_fd; - } - else if(n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_WRITING_HEADER || - n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_START_WRITING || - n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_WRITING) - { - FD_SET( rpcros_client_fd, &w_fds); - FD_SET( rpcros_client_fd, &err_fds); - if( rpcros_client_fd > nfds ) nfds = rpcros_client_fd; - } - else if(n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE || - n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER || - n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING_SIZE || - n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING) - { - FD_SET( rpcros_client_fd, &r_fds); - FD_SET( rpcros_client_fd, &err_fds); - if( rpcros_client_fd > nfds ) nfds = rpcros_client_fd; - } - else if(n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) - { - FD_SET( rpcros_client_fd, &err_fds); - if( rpcros_client_fd > nfds ) nfds = rpcros_client_fd; - } - } - - - /* Add to the tcpIpSocketSelect() the active RPCROS servers */ - int next_rpcros_server_i = -1; - - for( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++ ) - { - int server_fd = tcpIpSocketGetFD( &(n->rpcros_server_proc[i].socket) ); - - if( next_rpcros_server_i < 0 && - n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_IDLE ) - { - next_rpcros_server_i = i; - } - else if (n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE || - n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER || - n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_SIZE || - n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_READING) - { - FD_SET( server_fd, &r_fds); - FD_SET( server_fd, &err_fds); - if( server_fd > nfds ) nfds = server_fd; - } - else if( n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_WRITING_HEADER || - n->rpcros_server_proc[i].state == TCPROS_PROCESS_STATE_WRITING ) - { - FD_SET( server_fd, &w_fds); - FD_SET( server_fd, &err_fds); - if( server_fd > nfds ) nfds = server_fd; - } - } - - /* If one RPCROS server is available at least, add to the tcpIpSocketSelect() the listner socket */ - if( next_rpcros_server_i >= 0) - { - if(rpcros_listner_fd != -1) // If the listener socket is still opened - { - FD_SET( rpcros_listner_fd, &r_fds); - FD_SET( rpcros_listner_fd, &err_fds); - if( rpcros_listner_fd > nfds ) nfds = rpcros_listner_fd; - } - } - - if (nfds + 1 == 0) - { - PRINT_VDEBUG("cRosNodeDoEventsLoop() : Warning: tcpIpSocketSelect() is being called with no file descriptors to monitor.\n"); - } - - select_timeout = cRosNodeCalculateSelectTimeout(n, max_timeout); - - // The node waits here until the specified file descriptors become ready for the corresponding I/O operation or the timeout is up - // ------------------------------------------------------------------------------------------------------------------------------ - int n_set = tcpIpSocketSelect(nfds + 1, &r_fds, &w_fds, &err_fds, select_timeout); - - cur_time = cRosClockGetTimeMs(); // Update current time after select() - if (n_set == -1) - { - PRINT_ERROR("cRosNodeDoEventsLoop() : tcpIpSocketSelect() function failed.\n"); - ret_err = CROS_SELECT_FD_ERR; - } - else if( n_set == 0 ) - { - PRINT_VDEBUG ("cRosNodeDoEventsLoop() : tcpIpSocketSelect() finished due to timeout (parameter: %llu ms) or it was interrupted\n", (long long unsigned)select_timeout); - - XmlrpcProcess *rosproc = &n->xmlrpc_client_proc[0]; - if(n->xmlrpc_master_wake_up_time <= cur_time ) // It's time to wakeup, ping master, and maybe look up in master for pending services - { - PRINT_VDEBUG("cRosNodeDoEventsLoop() : It is ime to wake up the Master XML RPC process. Current time: %lu Wake up time: %lu\n", cur_time, n->xmlrpc_master_wake_up_time); - if(rosproc->state == XMLRPC_PROCESS_STATE_IDLE) - { - // Prepare to ping roscore ... - PRINT_VDEBUG("cRosNodeDoEventsLoop() : Sending ping to ROS Master\n"); - - RosApiCall *call = newRosApiCall(); - if (call != NULL) - { - call->method = CROS_API_GET_PID; - int rc = xmlrpcParamVectorPushBackString(&call->params, n->name); - if(rc >= 0) - { - rosproc->message_type = XMLRPC_MESSAGE_REQUEST; - generateXmlrpcMessage( n->roscore_host, n->roscore_port, rosproc->message_type, - getMethodName(call->method), &call->params, &rosproc->message ); - - rosproc->current_call = call; - xmlrpcProcessChangeState(rosproc, XMLRPC_PROCESS_STATE_CONNECTING ); - } - else - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : Can't allocate memory\n"); - ret_err=CROS_MEM_ALLOC_ERR; - } - - // The ROS master does not warn us when a new service is registered, so we have to - // continuously check for the required service - for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++ ) - { - TcprosProcess *client_proc = &(n->rpcros_client_proc[i]); - if( client_proc->state == TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING) - { - tcprosProcessChangeState(client_proc, TCPROS_PROCESS_STATE_IDLE); - enqueueServiceLookup(n, client_proc->service_idx); - } - } - n->xmlrpc_master_wake_up_time = cur_time + CN_PING_LOOP_PERIOD; // The process completed doing what it should, so wake up again CN_PING_LOOP_PERIOD milliseconds later - } - else - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : Can't allocate memory\n"); - ret_err=CROS_MEM_ALLOC_ERR; - } - } - else - n->xmlrpc_master_wake_up_time = cur_time + CN_PING_LOOP_PERIOD/50; // The process is busy, so try to wake up again soon (CN_PING_LOOP_PERIOD/50 milliseconds later) to do what is pending - } - if( rosproc->state != XMLRPC_PROCESS_STATE_IDLE && cRosClockGetTimeMs() - rosproc->last_change_time > CN_IO_TIMEOUT ) // last_change_time is updated when changing process state - { - // Timeout between I/O operations... close the socket and re-advertise - PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : XMLRPC client I/O timeout\n"); - handleXmlrpcClientError( n, 0 ); - } - - - for( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS && ret_err==CROS_SUCCESS_ERR_PACK; i++ ) - { - if( (n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER || - n->tcpros_server_proc[i].state == TCPROS_PROCESS_STATE_WRITING ) && - cur_time - n->tcpros_server_proc[i].last_change_time > CN_IO_TIMEOUT ) - { - // Timeout between I/O operations - PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : TCPROS server I/O timeout\n"); - handleTcprosServerError( n, i ); - } - } - - for( i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS && ret_err==CROS_SUCCESS_ERR_PACK; i++ ) - { - if( (n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_READING_HEADER || // Add more states to the condition??? - n->rpcros_client_proc[i].state == TCPROS_PROCESS_STATE_WRITING ) && - cur_time - n->rpcros_client_proc[i].last_change_time > CN_IO_TIMEOUT ) - { - // Timeout between I/O operations - PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : TCPROS server I/O timeout\n"); - handleTcprosServerError( n, i ); - } - } - } - else - { - PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : tcpIpSocketSelect() finished with num. fd set: %i (timeout parameter was: %llu ms)\n", n_set, (long long unsigned)select_timeout); - for(i = 0; i < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; i++ ) - { - XmlrpcProcess *client_proc; - int xmlrpc_client_fd; - - client_proc = &n->xmlrpc_client_proc[i]; - xmlrpc_client_fd = tcpIpSocketGetFD( &client_proc->socket ); - if( client_proc->state != XMLRPC_PROCESS_STATE_IDLE && FD_ISSET(xmlrpc_client_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : XMLRPC client socket error\n" ); - handleXmlrpcClientError( n, i ); - } - /* Check what is the socket unblocked by tcpIpSocketSelect(), and start the requested operations */ - else if( ( client_proc->state == XMLRPC_PROCESS_STATE_CONNECTING && FD_ISSET(xmlrpc_client_fd, &w_fds) ) || - ( client_proc->state == XMLRPC_PROCESS_STATE_WRITING && FD_ISSET(xmlrpc_client_fd, &w_fds) ) || - ( client_proc->state == XMLRPC_PROCESS_STATE_READING && FD_ISSET(xmlrpc_client_fd, &r_fds) ) ) - { - new_errors = doWithXmlrpcClientSocket( n, i );; - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - } - } - - if ( next_xmlrpc_server_i >= 0 && xmlrpc_listner_fd != -1) // Check that there is an available free (idle) xmlrpx process and that the listener socket is still open - { - if( FD_ISSET( xmlrpc_listner_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : XMLRPC server listener-socket error\n" ); - } - else if( FD_ISSET( xmlrpc_listner_fd, &r_fds) ) - { - PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : XMLRPC server listener-socket ready\n" ); - if( tcpIpSocketAccept( &(n->xmlrpc_listner_proc.socket), - &(n->xmlrpc_server_proc[next_xmlrpc_server_i].socket) ) == TCPIPSOCKET_DONE && - tcpIpSocketSetReuse( &(n->xmlrpc_server_proc[next_xmlrpc_server_i].socket) ) && - tcpIpSocketSetNonBlocking( &(n->xmlrpc_server_proc[next_xmlrpc_server_i].socket ) ) ) - - xmlrpcProcessChangeState( &(n->xmlrpc_server_proc[next_xmlrpc_server_i]), XMLRPC_PROCESS_STATE_READING ); - } - } - - for( i = 0; i < CN_MAX_XMLRPC_SERVER_CONNECTIONS; i++ ) - { - XmlrpcProcess *server_proc; - int server_fd; - - server_proc = &n->xmlrpc_server_proc[i]; - server_fd = tcpIpSocketGetFD( &server_proc->socket ); - if( server_proc->state != XMLRPC_PROCESS_STATE_IDLE && FD_ISSET(server_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : XMLRPC server socket error\n" ); - tcpIpSocketClose( &(n->xmlrpc_server_proc[i].socket) ); - xmlrpcProcessChangeState( &(n->xmlrpc_server_proc[next_xmlrpc_server_i]), XMLRPC_PROCESS_STATE_IDLE ); // maybe server_proc instead of &(n->xmlrpc_server_proc[next_xmlrpc_server_i]) ??? - } - else if( ( server_proc->state == XMLRPC_PROCESS_STATE_WRITING && FD_ISSET(server_fd, &w_fds) ) || - ( server_proc->state == XMLRPC_PROCESS_STATE_READING && FD_ISSET(server_fd, &r_fds) ) ) - { - new_errors = doWithXmlrpcServerSocket( n, i ); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - } - } - - for(i = 0; i < CN_MAX_TCPROS_CLIENT_CONNECTIONS; i++ ) - { - TcprosProcess *client_proc = &(n->tcpros_client_proc[i]); - int tcpros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); - - if( client_proc->state != TCPROS_PROCESS_STATE_IDLE && FD_ISSET(tcpros_client_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : XMLRPC client socket error\n" ); - handleTcprosClientError( n, i ); - } - - if( (client_proc->state == TCPROS_PROCESS_STATE_CONNECTING && FD_ISSET(tcpros_client_fd, &w_fds) ) || // tcpIpSocketSelect() indicates connection completion through write-fd - ( client_proc->state == TCPROS_PROCESS_STATE_WRITING_HEADER && FD_ISSET(tcpros_client_fd, &w_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_READING_SIZE && FD_ISSET(tcpros_client_fd, &r_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_READING && FD_ISSET(tcpros_client_fd, &r_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE && FD_ISSET(tcpros_client_fd, &r_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER && FD_ISSET(tcpros_client_fd, &r_fds) ) ) - { - new_errors = doWithTcprosClientSocket( n, i ); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - } - } - - if ( next_tcpros_server_i >= 0 && tcpros_listner_fd != -1) - { - if( FD_ISSET( tcpros_listner_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : TCPROS listener-socket error\n" ); - } - else if( FD_ISSET( tcpros_listner_fd, &r_fds) ) - { - PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : TCPROS listener ready\n" ); - if( tcpIpSocketAccept( &(n->tcpros_listner_proc.socket), - &(n->tcpros_server_proc[next_tcpros_server_i].socket) ) == TCPIPSOCKET_DONE && - tcpIpSocketSetReuse( &(n->tcpros_server_proc[next_tcpros_server_i].socket) ) && - tcpIpSocketSetNonBlocking( &(n->tcpros_server_proc[next_tcpros_server_i].socket ) ) && - tcpIpSocketSetKeepAlive( &(n->tcpros_server_proc[next_tcpros_server_i].socket ), 60, 10, 9 ) ) - { - tcprosProcessChangeState( &(n->tcpros_server_proc[next_tcpros_server_i]), TCPROS_PROCESS_STATE_READING_HEADER ); // A TCPROS process has been activated to attend the connection - } - } - } - - for( i = 0; i < CN_MAX_TCPROS_SERVER_CONNECTIONS; i++ ) - { - TcprosProcess *server_proc; - int server_fd; - - server_proc = &n->tcpros_server_proc[i]; - server_fd = tcpIpSocketGetFD( &server_proc->socket ); - if( server_proc->state != TCPROS_PROCESS_STATE_IDLE && FD_ISSET(server_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : TCPROS server socket error\n" ); - tcpIpSocketClose( &(server_proc->socket) ); - tcprosProcessChangeState( &(n->tcpros_server_proc[next_tcpros_server_i]), TCPROS_PROCESS_STATE_IDLE ); - } - else if( ( server_proc->state == TCPROS_PROCESS_STATE_READING_HEADER && FD_ISSET(server_fd, &r_fds) ) || - ( server_proc->state == TCPROS_PROCESS_STATE_START_WRITING && FD_ISSET(server_fd, &w_fds) ) || - ( server_proc->state == TCPROS_PROCESS_STATE_WRITING && FD_ISSET(server_fd, &w_fds) ) ) - { - new_errors = doWithTcprosServerSocket( n, i ); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - } - } - - for(i = 0; i < CN_MAX_RPCROS_CLIENT_CONNECTIONS; i++ ) - { - TcprosProcess *client_proc = &(n->rpcros_client_proc[i]); - int rpcros_client_fd = tcpIpSocketGetFD( &(client_proc->socket) ); - - if( client_proc->state != TCPROS_PROCESS_STATE_IDLE && FD_ISSET(rpcros_client_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : RPCROS client socket error\n" ); - handleRpcrosClientError( n, i ); - } - - if( ( client_proc->state == TCPROS_PROCESS_STATE_CONNECTING && FD_ISSET(rpcros_client_fd, &w_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_WRITING_HEADER && FD_ISSET(rpcros_client_fd, &w_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_READING_SIZE && FD_ISSET(rpcros_client_fd, &r_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_READING && FD_ISSET(rpcros_client_fd, &r_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE && FD_ISSET(rpcros_client_fd, &r_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_READING_HEADER && FD_ISSET(rpcros_client_fd, &r_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_START_WRITING && FD_ISSET(rpcros_client_fd, &w_fds) ) || - ( client_proc->state == TCPROS_PROCESS_STATE_WRITING && FD_ISSET(rpcros_client_fd, &w_fds) ) ) - { - new_errors = doWithRpcrosClientSocket( n, i ); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - } - } - - if ( next_rpcros_server_i >= 0 && rpcros_listner_fd != -1) - { - if( FD_ISSET( rpcros_listner_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : TCPROS listener-socket error\n" ); - } - else if( next_rpcros_server_i >= 0 && FD_ISSET( rpcros_listner_fd, &r_fds) ) - { - PRINT_VDEBUG ( "cRosNodeDoEventsLoop() : TCPROS listener ready\n" ); - if( tcpIpSocketAccept( &(n->rpcros_listner_proc.socket), - &(n->rpcros_server_proc[next_rpcros_server_i].socket) ) == TCPIPSOCKET_DONE && - tcpIpSocketSetReuse( &(n->rpcros_server_proc[next_rpcros_server_i].socket) ) && - tcpIpSocketSetNonBlocking( &(n->rpcros_server_proc[next_rpcros_server_i].socket ) ) && - tcpIpSocketSetKeepAlive( &(n->rpcros_server_proc[next_rpcros_server_i].socket ), 60, 10, 9 ) ) - { - tcprosProcessChangeState( &(n->rpcros_server_proc[next_rpcros_server_i]), TCPROS_PROCESS_STATE_READING_HEADER_SIZE ); - } - } - } - - for( i = 0; i < CN_MAX_RPCROS_SERVER_CONNECTIONS; i++ ) - { - TcprosProcess *server_proc = &(n->rpcros_server_proc[i]); - int server_fd = tcpIpSocketGetFD( &(server_proc->socket) ); - - if( server_proc->state != TCPROS_PROCESS_STATE_IDLE && FD_ISSET(server_fd, &err_fds) ) - { - PRINT_ERROR ( "cRosNodeDoEventsLoop() : TCPROS server socket error\n" ); - tcpIpSocketClose( &(server_proc->socket) ); - tcprosProcessChangeState( &(n->rpcros_server_proc[next_rpcros_server_i]), TCPROS_PROCESS_STATE_IDLE ); - } - else if( ( server_proc->state == TCPROS_PROCESS_STATE_READING_HEADER_SIZE && FD_ISSET(server_fd, &r_fds) ) || - ( server_proc->state == TCPROS_PROCESS_STATE_READING_HEADER && FD_ISSET(server_fd, &r_fds) ) || - ( server_proc->state == TCPROS_PROCESS_STATE_READING_SIZE && FD_ISSET(server_fd, &r_fds) ) || - ( server_proc->state == TCPROS_PROCESS_STATE_READING && FD_ISSET(server_fd, &r_fds) ) || - ( server_proc->state == TCPROS_PROCESS_STATE_WRITING_HEADER && FD_ISSET(server_fd, &w_fds) ) || - ( server_proc->state == TCPROS_PROCESS_STATE_WRITING && FD_ISSET(server_fd, &w_fds) ) ) - { - new_errors = doWithRpcrosServerSocket( n, i ); - ret_err = cRosAddErrCodePackIfErr(ret_err, new_errors); - } - } - } - return ret_err; -} - -cRosErrCodePack cRosNodeStart( CrosNode *n, unsigned long time_out, unsigned char *exit_flag ) -{ - uint64_t start_time, elapsed_time; - cRosErrCodePack ret_err; - PRINT_VVDEBUG ( "cRosNodeStart ()\n" ); - - start_time = cRosClockGetTimeMs(); - ret_err = CROS_SUCCESS_ERR_PACK; - while(ret_err == CROS_SUCCESS_ERR_PACK && (exit_flag==NULL || !(*exit_flag)) && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) - ret_err = cRosNodeDoEventsLoop( n, (time_out == CROS_INFINITE_TIMEOUT)? UINT64_MAX : time_out-elapsed_time); - - return ret_err; -} - -cRosErrCodePack cRosNodeReceiveTopicMsg( CrosNode *node, int subidx, cRosMessage *msg, unsigned char *buff_overflow, unsigned long time_out ) -{ - cRosErrCodePack ret_err; - SubscriberNode *subs_node; - uint64_t start_time, elapsed_time = 0; // Initialized just to avoid a compiler warning - PRINT_VVDEBUG ( "cRosNodeReceiveTopicMsg ()\n" ); - - if(subidx < 0 || subidx >= CN_MAX_SUBSCRIBED_TOPICS) - return CROS_BAD_PARAM_ERR; - - subs_node = &node->subs[subidx]; - if(subs_node->topic_name == NULL) - return CROS_BAD_PARAM_ERR; - - if(buff_overflow != NULL) - *buff_overflow = subs_node->msg_queue_overflow; - subs_node->msg_queue_overflow = 0; // Reset overflow flag - - start_time = cRosClockGetTimeMs(); - ret_err = CROS_SUCCESS_ERR_PACK; // default return value - // While the buffer is empty and the timeout is not reached wait - while(cRosMessageQueueUsage(&subs_node->msg_queue) == 0 && ret_err == CROS_SUCCESS_ERR_PACK && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) - { - ret_err = cRosNodeDoEventsLoop ( node, time_out - elapsed_time); - } - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - if(cRosMessageQueueUsage(&subs_node->msg_queue) > 0) // If no error and there is at least one message in the queue, get the message - ret_err = (cRosMessageQueueExtract(&subs_node->msg_queue, msg) == 0)? CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - else - ret_err = CROS_RCV_TOP_TIMEOUT_ERR; - } - return ret_err; -} - -cRosErrCodePack cRosNodeQueueTopicMsg( CrosNode *node, int pubidx, cRosMessage *msg ) -{ - cRosErrCodePack ret_err; - PublisherNode *pub_node; - - pub_node = &node->pubs[pubidx]; - { - if(cRosMessageQueueVacancies(&pub_node->msg_queue) > 0) // If no error and there is space in the queue, put the new message - { - if(cRosMessageQueueAdd(&pub_node->msg_queue, msg) == 0) - ret_err = CROS_SUCCESS_ERR_PACK; - else - ret_err = CROS_MEM_ALLOC_ERR; - } - else - ret_err = CROS_SEND_TOP_TIMEOUT_ERR; - } - return ret_err; -} - -cRosErrCodePack cRosNodeSendTopicMsg( CrosNode *node, int pubidx, cRosMessage *msg, unsigned long time_out ) -{ - cRosErrCodePack ret_err; - PublisherNode *pub_node; - uint64_t start_time, elapsed_time = 0; // Initialized just to avoid a compiler warning - PRINT_VVDEBUG ( "cRosNodeSendTopicMsg ()\n" ); - - if(pubidx < 0 || pubidx >= CN_MAX_PUBLISHED_TOPICS) - return CROS_BAD_PARAM_ERR; - - pub_node = &node->pubs[pubidx]; - if(pub_node->topic_name == NULL) - return CROS_BAD_PARAM_ERR; - - start_time = cRosClockGetTimeMs(); - ret_err = CROS_SUCCESS_ERR_PACK; // default return value - // While the buffer is full and the timeout is not reached wait - while(cRosMessageQueueVacancies(&pub_node->msg_queue) == 0 && ret_err == CROS_SUCCESS_ERR_PACK && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) - { - ret_err = cRosNodeDoEventsLoop ( node, time_out - elapsed_time); - } - - if(ret_err == CROS_SUCCESS_ERR_PACK) - cRosNodeQueueTopicMsg( node, pubidx, msg ); - - return ret_err; -} - - -cRosErrCodePack cRosNodeServiceCall( CrosNode *node, int svcidx, cRosMessage *req_msg, cRosMessage *resp_msg, unsigned long time_out) -{ - cRosErrCodePack ret_err; - ServiceCallerNode *caller_node; - TcprosProcess *svc_client_proc; - uint64_t start_time, elapsed_time = 0; // Initialized just to avoid a compiler warning - PRINT_VVDEBUG ( "cRosNodeServiceCall ()\n" ); - - if(svcidx < 0 || svcidx >= CN_MAX_SERVICE_CALLERS) - return CROS_BAD_PARAM_ERR; - - caller_node = &node->service_callers[svcidx]; - if(caller_node->service_name == NULL) - return CROS_BAD_PARAM_ERR; - - svc_client_proc = &node->rpcros_client_proc[caller_node->rpcros_id]; - - start_time = cRosClockGetTimeMs(); - // Wait until the RPCROS process has finished the current call and the timeout is not reached wait - ret_err = CROS_SUCCESS_ERR_PACK; - while(svc_client_proc->state != TCPROS_PROCESS_STATE_WAIT_FOR_WRITING && ret_err == CROS_SUCCESS_ERR_PACK && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) - { - ret_err = cRosNodeDoEventsLoop ( node, time_out - elapsed_time); - } - - if(ret_err == CROS_SUCCESS_ERR_PACK && svc_client_proc->state != TCPROS_PROCESS_STATE_WAIT_FOR_WRITING) - ret_err = CROS_CALL_INI_TIMEOUT_ERR; - if(ret_err != CROS_SUCCESS_ERR_PACK) - return ret_err; // If an error occurred or the process is not in TCPROS_PROCESS_STATE_WAIT_FOR_WRITING state, exit - - if(cRosMessageQueueUsage(&caller_node->msg_queue) > 0) // Unfinished service call? - { - PRINT_ERROR ( "cRosNodeServiceCall () : The service caller is not ready to make a new call. Overwriting previous state and trying anyway...\n" ); - cRosMessageQueueClear(&caller_node->msg_queue); - } - - if(cRosMessageQueueAdd(&caller_node->msg_queue, req_msg) != 0) // Put service-call request msg in the queue - ret_err = CROS_MEM_ALLOC_ERR; - - // Wait while the buffer is full and the timeout is not reached - while(cRosMessageQueueUsage(&caller_node->msg_queue) < 2 && ret_err == CROS_SUCCESS_ERR_PACK && (time_out == CROS_INFINITE_TIMEOUT || (elapsed_time=cRosClockGetTimeMs()-start_time) <= time_out)) - { - ret_err = cRosNodeDoEventsLoop ( node, time_out - elapsed_time); - } - - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - if(cRosMessageQueueUsage(&caller_node->msg_queue) > 1) // If no error and the response is in the buffer - { - cRosMessageQueueRemove(&caller_node->msg_queue); // Remove request msg from queue - if(resp_msg != NULL) - { - int queue_ret_val; - queue_ret_val = cRosMessageQueueExtract(&caller_node->msg_queue, resp_msg); // Extract response msg from queue - if(queue_ret_val == 0) - ret_err = CROS_SUCCESS_ERR_PACK; - else - ret_err = CROS_MEM_ALLOC_ERR; - } - else // The user is not interested in the service response, just remove the msg from queue - cRosMessageQueueRemove(&caller_node->msg_queue); - } - else - ret_err = CROS_CALL_SVC_TIMEOUT_ERR; - } - - return ret_err; -} - -int enqueueSubscriberAdvertise(CrosNode *node, int subidx) -{ - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n"); - return -1; - } - - call->provider_idx= subidx; - call->method = CROS_API_REGISTER_SUBSCRIBER; - - SubscriberNode *sub = &node->subs[subidx]; - xmlrpcParamVectorPushBackString( &call->params, node->name ); - xmlrpcParamVectorPushBackString( &call->params, sub->topic_name ); - xmlrpcParamVectorPushBackString( &call->params, sub->topic_type ); - char node_uri[256]; - snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); - xmlrpcParamVectorPushBackString( &call->params, node_uri ); - - return enqueueMasterApiCallInternal(node, call); -} - -int enqueuePublisherAdvertise(CrosNode *node, int pubidx) -{ - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterPublisher() : Can't allocate memory\n"); - return -1; - } - - call->provider_idx= pubidx; - call->method = CROS_API_REGISTER_PUBLISHER; - - PublisherNode *publiser = &node->pubs[pubidx]; - xmlrpcParamVectorPushBackString( &call->params, node->name); - xmlrpcParamVectorPushBackString( &call->params, publiser->topic_name ); - xmlrpcParamVectorPushBackString( &call->params, publiser->topic_type ); - char node_uri[256]; - snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); - xmlrpcParamVectorPushBackString( &call->params, node_uri ); - - return enqueueMasterApiCallInternal(node, call); -} - -int enqueueServiceAdvertise(CrosNode *node, int serviceidx) -{ - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't allocate memory\n"); - return -1; - } - - call->provider_idx = serviceidx; - call->method = CROS_API_REGISTER_SERVICE; - - ServiceProviderNode *service = &node->service_providers[serviceidx]; - xmlrpcParamVectorPushBackString( &call->params, node->name ); - xmlrpcParamVectorPushBackString( &call->params, service->service_name ); - char uri[256]; - snprintf( uri, 256, "rosrpc://%s:%d/", node->host, node->rpcros_port); - xmlrpcParamVectorPushBackString( &call->params, uri ); - snprintf( uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); - xmlrpcParamVectorPushBackString( &call->params, uri ); - - return enqueueMasterApiCallInternal(node, call); -} - -int enqueueServiceLookup(CrosNode *node, int serviceidx) -{ - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "enqueueServiceLookup() : Can't allocate memory\n"); - return -1; - } - - call->provider_idx= serviceidx; - call->method = CROS_API_LOOKUP_SERVICE; - - ServiceCallerNode *service = &node->service_callers[serviceidx]; - xmlrpcParamVectorPushBackString( &call->params, node->name ); - xmlrpcParamVectorPushBackString( &call->params, service->service_name ); - - return enqueueMasterApiCallInternal(node, call); -} - -int enqueueParameterSubscription(CrosNode *node, int parameteridx) -{ - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't allocate memory\n"); - return -1; - } - - call->provider_idx = parameteridx; - call->method = CROS_API_SUBSCRIBE_PARAM; - - ParameterSubscription *subscrition = &node->paramsubs[parameteridx]; - xmlrpcParamVectorPushBackString( &call->params, node->name ); - char node_uri[256]; - snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); - xmlrpcParamVectorPushBackString( &call->params, node_uri ); - xmlrpcParamVectorPushBackString( &call->params, subscrition->parameter_key); - - return enqueueMasterApiCallInternal(node, call); -} - -int enqueueParameterUnsubscription(CrosNode *node, int parameteridx) -{ - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterServiceProvider() : Can't allocate memory\n"); - return -1; - } - - call->method = CROS_API_UNSUBSCRIBE_PARAM; - call->provider_idx = parameteridx; - - ParameterSubscription *subscrition = &node->paramsubs[parameteridx]; - xmlrpcParamVectorPushBackString( &call->params, node->name); - char node_uri[256]; - snprintf( node_uri, 256, "http://%s:%d/", node->host, node->xmlrpc_port); - xmlrpcParamVectorPushBackString( &call->params, node_uri ); - xmlrpcParamVectorPushBackString( &call->params, subscrition->parameter_key ); - - // NB: Node release is done in handleApiCallAttempt() - return enqueueMasterApiCallInternal(node, call); -} - -int enqueueRequestTopic(CrosNode *node, int subidx, const char *host, int port) -{ - RosApiCall *call = newRosApiCall(); - if (call == NULL) - { - PRINT_ERROR ( "cRosNodeRegisterSubscriber() : Can't allocate memory\n"); - return -1; - } - - call->provider_idx = subidx; - call->method = CROS_API_REQUEST_TOPIC; - - SubscriberNode *sub = &node->subs[subidx]; - - CrosNodeStatusUsr status; - initCrosNodeStatus(&status); - status.provider_idx = subidx; - status.xmlrpc_host = host; - status.xmlrpc_port = port; - cRosNodeStatusCallback(&status, sub->context); // calls the subscriber-status application-defined callback function (if specified when creating the subscriber). Undocumented status callback? - - xmlrpcParamVectorPushBackString(&call->params, node->name ); - xmlrpcParamVectorPushBackString(&call->params, sub->topic_name ); - xmlrpcParamVectorPushBackArray(&call->params); - XmlrpcParam* array_param = xmlrpcParamVectorAt(&call->params,2); - xmlrpcParamArrayPushBackArray(array_param); - xmlrpcParamArrayPushBackString(xmlrpcParamArrayGetParamAt(array_param,0),CROS_TRANSPORT_TCPROS_STRING); - - return enqueueSlaveApiCall(node, call, host, port); -} - -void restartAdversing(CrosNode* n) -{ - int it; - for(it = 0; it < n->n_pubs; it++) - { - if (n->pubs[it].topic_name == NULL) - continue; - - enqueuePublisherAdvertise(n, it); - } - - for(it = 0; it < n->n_subs; it++) - { - if (n->subs[it].topic_name == NULL) - continue; - - enqueueSubscriberAdvertise(n, it); - } - - for(it = 0; it < n->n_service_providers; it++) - { - if (n->service_providers[it].service_name == NULL) - continue; - - enqueueServiceAdvertise(n, it); - } -} - -void initPublisherNode(PublisherNode *pub) -{ - pub->message_definition = NULL; - pub->topic_name = NULL; - pub->topic_type = NULL; - pub->md5sum = NULL; - pub->context = NULL; - pub->tcpros_id_list[0] = -1; // Empty list of TcprosProcess indices - pub->loop_period = -1; // Publication paused - pub->wake_up_time = 0; - cRosMessageQueueInit(&pub->msg_queue); -} - -void initSubscriberNode(SubscriberNode *sub) -{ - sub->message_definition = NULL; - sub->topic_name = NULL; - sub->topic_type = NULL; - sub->md5sum = NULL; - sub->context = NULL; - sub->tcp_nodelay = 0; - sub->msg_queue_overflow = 0; - cRosMessageQueueInit(&sub->msg_queue); -} - -void initServiceProviderNode(ServiceProviderNode *srv_prov) -{ - srv_prov->service_name = NULL; - srv_prov->service_type = NULL; - srv_prov->md5sum = NULL; - srv_prov->context = NULL; - srv_prov->servicerequest_type = NULL; - srv_prov->serviceresponse_type = NULL; -} - -void initServiceCallerNode(ServiceCallerNode *srv_caller) -{ - srv_caller->service_name = NULL; - srv_caller->service_type = NULL; - srv_caller->service_host = NULL; - srv_caller->service_port = -1; - srv_caller->md5sum = NULL; - srv_caller->message_definition = NULL; - srv_caller->rpcros_id = -1; - srv_caller->context = NULL; - srv_caller->servicerequest_type = NULL; - srv_caller->serviceresponse_type = NULL; - srv_caller->persistent = 0; - srv_caller->tcp_nodelay = 0; - srv_caller->loop_period = -1; // Calling paused - srv_caller->wake_up_time = 0; - cRosMessageQueueInit(&srv_caller->msg_queue); -} - -void initParameterSubscrition(ParameterSubscription *subscription) -{ - subscription->parameter_key = NULL; - xmlrpcParamInit(&subscription->parameter_value); - subscription->status_api_callback = NULL; - subscription->context = NULL; -} - -void cRosNodeReleasePublisher(PublisherNode *node) -{ - free(node->message_definition); - free(node->topic_name); - free(node->topic_type); - free(node->md5sum); - cRosMessageQueueRelease(&node->msg_queue); -} - -void cRosNodeReleaseSubscriber(SubscriberNode *node) -{ - free(node->message_definition); - free(node->topic_name); - free(node->topic_type); - free(node->md5sum); - cRosMessageQueueRelease(&node->msg_queue); -} - -void cRosNodeReleaseServiceProvider(ServiceProviderNode *node) -{ - free(node->service_name); - free(node->service_type); - free(node->servicerequest_type); - free(node->serviceresponse_type); - free(node->md5sum); -} - -void cRosNodeReleaseServiceCaller(ServiceCallerNode *node) -{ - free(node->service_name); - free(node->service_type); - free(node->servicerequest_type); - free(node->serviceresponse_type); - free(node->md5sum); - free(node->message_definition); - free(node->service_host); -} - -void initCrosNodeStatus(CrosNodeStatusUsr *status) -{ - status->state = CROS_STATUS_NONE; - status->xmlrpc_port = -1; - status->xmlrpc_host = NULL; - status->provider_idx = -1; - status->parameter_key = NULL; - status->parameter_value = NULL; -} - -void cRosNodeReleaseParameterSubscrition(ParameterSubscription *subscription) -{ - free(subscription->parameter_key); - xmlrpcParamRelease(&subscription->parameter_value); -} - -void getIdleXmplrpcClients(CrosNode *node, int idle_clients[], size_t *idle_client_count) -{ - int client_it = 1; // client_it = 0 is for roscore only - int idle_it = 0; - *idle_client_count = 0; - for(; client_it < CN_MAX_XMLRPC_CLIENT_CONNECTIONS; client_it++) - { - if (node->xmlrpc_client_proc[client_it].state == XMLRPC_PROCESS_STATE_IDLE) - { - idle_clients[idle_it] = client_it; - (*idle_client_count)++; - idle_it++; - } - } -} - -int enqueueMasterApiCall(CrosNode *node, RosApiCall *call) -{ - call->user_call = 1; - return enqueueSlaveApiCallInternal(node, call); -} - -int enqueueSlaveApiCall(CrosNode *node, RosApiCall *call, const char *host, int port) -{ - call->user_call = 1; - if (host == NULL) - { - call->host = node->roscore_host; - call->port = node->roscore_port; - } - else - { - call->host = (char *)malloc(strlen(host) + 1); - if (call->host == NULL) - { - PRINT_ERROR("enqueueSlaveApiCall() : Not enough memory\n"); - return -1; - } - - strcpy(call->host, host); - call->port = port; - } - - return enqueueSlaveApiCallInternal(node, call); -} - -int enqueueMasterApiCallInternal(CrosNode *node, RosApiCall *call) -{ - int callid = (int)node->next_call_id; - call->id = callid; - int rc = enqueueApiCall(&node->master_api_queue, call); - if (rc == -1) - return -1; - - node->next_call_id++; - return callid; -} - -int enqueueSlaveApiCallInternal(CrosNode *node, RosApiCall *call) -{ - int callid = (int)node->next_call_id; - call->id = callid; - int rc = enqueueApiCall(&node->slave_api_queue, call); - if (rc == -1) - return -1; - - node->next_call_id++; - return callid; -} - -void cRosGetMsgFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *topic_type) -{ - snprintf(buffer, bufsize, "%s/%s.msg", node->message_root_path, topic_type); -} - -void getSrvFilePath(CrosNode *node, char *buffer, size_t bufsize, const char *service_type) -{ - snprintf(buffer, bufsize, "%s/%s.srv", node->message_root_path, service_type); -} - -XmlrpcParam * cRosNodeGetParameterValue( CrosNode *node, const char *key) -{ - int it = 0; - for (it = 0 ; it < node->n_paramsubs; it++) - { - if (node->paramsubs[it].parameter_key == NULL) - continue; - - if (strcmp(node->paramsubs[it].parameter_key, key) == 0) - return &node->paramsubs[it].parameter_value; - } - - return NULL; -} - -#define MAX_PORT_OPEN_CHECK_PERIOD 1000 //! Maximum time to wait in ms until the target port is checked again by cRosWaitPortOpen() -cRosErrCodePack cRosWaitPortOpen(const char *host_addr, unsigned short host_port, unsigned long time_out) -{ - uint64_t start_time, elapsed_time; - cRosErrCodePack ret_err; - TcpIpSocketState socket_state; - PRINT_VVDEBUG ( "cRosWaitPortOpen ()\n" ); - - start_time = cRosClockGetTimeMs(); - do - { - elapsed_time = cRosClockGetTimeMs()-start_time; - socket_state = tcpIpSocketCheckPort (host_addr, host_port); - // socket_state can be TCPIPSOCKET_FAILED, TCPIPSOCKET_DONE, TCPIPSOCKET_IN_PROGRESS or TCPIPSOCKET_REFUSED - if(socket_state == TCPIPSOCKET_REFUSED) - { - unsigned int pause_ms; - if(time_out == CROS_INFINITE_TIMEOUT || time_out-elapsed_time > MAX_PORT_OPEN_CHECK_PERIOD) - pause_ms = MAX_PORT_OPEN_CHECK_PERIOD; - else - pause_ms = (unsigned int)(time_out-elapsed_time); -#ifdef _WIN32 - Sleep(pause_ms); -#else - usleep(pause_ms*1000); -#endif - } - } - while(socket_state == TCPIPSOCKET_REFUSED && (time_out == CROS_INFINITE_TIMEOUT || elapsed_time <= time_out)); - // We contnue iterating if the connection is refused and the time is not out - if(socket_state == TCPIPSOCKET_FAILED) - ret_err = CROS_SOCK_OPEN_CONN_ERR; - else if(socket_state == TCPIPSOCKET_REFUSED) - ret_err = CROS_SOCK_OPEN_TIMEOUT_ERR; - else // socket_state == TCPIPSOCKET_DONE or TCPIPSOCKET_IN_PROGRESS - ret_err = CROS_SUCCESS_ERR_PACK; // Port is ready - - return ret_err; -} - -FILE *cRosOutStreamGet(void) -{ - return((Msg_output == NULL)?stdout:Msg_output); -} - -void cRosOutStreamSet(FILE *new_stream) -{ - Msg_output = new_stream; -} - diff --git a/src/hal/components/cros/src/cros_node_api.c b/src/hal/components/cros/src/cros_node_api.c deleted file mode 100644 index 1f4973e9..00000000 --- a/src/hal/components/cros/src/cros_node_api.c +++ /dev/null @@ -1,1275 +0,0 @@ -#include -#include -#include - -#ifdef _WIN32 -# include -# include -# define strtok_r strtok_s -#else -# include -# include -# include -#endif - -#include "cros_node_api.h" -#include "cros_api.h" -#include "cros_api_internal.h" -#include "cros_defs.h" -#include "xmlrpc_params.h" -#include "tcpip_socket.h" - -int lookup_host (const char *host, char *ip_addr_buff, size_t ip_addr_buff_size) -{ - struct addrinfo hints, *res, *res_it; - int errcode; - int ret; - - memset (&hints, 0, sizeof (hints)); - hints.ai_family = PF_UNSPEC; - hints.ai_socktype = SOCK_STREAM; - hints.ai_flags |= AI_CANONNAME; - - errcode = getaddrinfo (host, NULL, &hints, &res); - if (errcode != 0) - { - PRINT_ERROR ("lookup_host() : getaddrinfo failed and returned the error code:: %i", errcode); - return -1; - } - - ret=0; // Default return value=0: no error - res_it = res; - while (res_it != NULL && ret == 0) - { - void *src_addr = NULL; - switch (res_it->ai_family) - { - case AF_INET: - src_addr = &((struct sockaddr_in *) res_it->ai_addr)->sin_addr; - break; -#ifdef AF_INET6 - case AF_INET6: - src_addr = &((struct sockaddr_in6 *) res_it->ai_addr)->sin6_addr; - break; -#endif - default: - break; - } - if (src_addr != NULL) - { - if(inet_ntop (res_it->ai_family, src_addr, ip_addr_buff, ip_addr_buff_size) != NULL) - { - PRINT_VVDEBUG ("IPv%d address: %s (%s)\n", res_it->ai_family == PF_INET6 ? 6 : 4, - ip_addr_buff, res_it->ai_canonname); - res_it = res_it->ai_next; - } - else - { - int fn_error_code; - - fn_error_code = tcpIpSocketGetError(); - if(fn_error_code == FN_ERROR_INVALID_PARAMETER) - PRINT_ERROR ("lookup_host() : buffer for host address too small"); - else - PRINT_ERROR ("lookup_host() : error executing inet_ntop(). Error code = %i", fn_error_code); - ret = -1; - } - } - else - { - PRINT_ERROR ("lookup_host() : unsupported ai_family from getaddrinfo()"); - ret = -1; - } - } - freeaddrinfo(res); - return ret; -} - -// TODO Improve this -static int checkResponseValue( XmlrpcParamVector *params ) -{ - XmlrpcParam *param = xmlrpcParamVectorAt( params, 0 ); - - // TODO - if( param == NULL || - ( xmlrpcParamGetType( param ) != XMLRPC_PARAM_INT && - xmlrpcParamGetType( param ) != XMLRPC_PARAM_ARRAY ) ) - { - return 0; - } - - int res = 0; - - if( xmlrpcParamGetType( param ) == XMLRPC_PARAM_INT ) - res = param->data.as_int; - else if ( xmlrpcParamGetType( param ) == XMLRPC_PARAM_ARRAY && - param->array_n_elem > 0 ) - { - XmlrpcParam *array_param = param->data.as_array; - if( xmlrpcParamGetType( &( array_param[0]) ) != XMLRPC_PARAM_INT ) - res = 0; - else - res = array_param[0].data.as_int; - } - - return res; -} - -void cRosApiPrepareRequest( CrosNode *n, int client_idx ) -{ - PRINT_VVDEBUG ( "cRosApiPrepareRequest()\n" ); - - XmlrpcProcess *client_proc = &(n->xmlrpc_client_proc[client_idx]); - - client_proc->message_type = XMLRPC_MESSAGE_REQUEST; - - if(client_proc->current_call == NULL) - { - PRINT_ERROR ( "cRosApiPrepareRequest() : Invalid call corresponding to XmlrpcProcess\n" ); - return; - } - - RosApiCall *call = client_proc->current_call; - if(client_idx == 0) //requests managed by the xmlrpc client connected to roscore - { - generateXmlrpcMessage(n->roscore_host, n->roscore_port, XMLRPC_MESSAGE_REQUEST, - getMethodName(call->method), &call->params, &client_proc->message); - } - else // client_idx > 0 - { - generateXmlrpcMessage(call->host, call->port, XMLRPC_MESSAGE_REQUEST, - getMethodName(call->method), &call->params, &client_proc->message); - } -} - - -/* - * XMLRPC Return values are lists in the format: [statusCode, statusMessage, value] - * - * statusCode (int): An integer indicating the completion condition of the method. Current values: - * -1: ERROR: Error on the part of the caller, e.g. an invalid parameter. In general, this means that the master/slave did not attempt to execute the action. - * 0: SUCCESS: Method completed successfully. - * Individual methods may assign additional meaning/semantics to statusCode. - * - * statusMessage (str): a human-readable string describing the return status - * - * value (anytype): return value is further defined by individual API calls. - */ -int cRosApiParseResponse( CrosNode *n, int client_idx ) -{ - PRINT_VVDEBUG ( "cRosApiParseResponse()\n" ); - XmlrpcProcess *client_proc = &(n->xmlrpc_client_proc[client_idx]); - int ret = -1; - - if( client_proc->current_call == NULL ) - { - PRINT_ERROR ( "cRosApiParseResponse() : Invalid current call for XMLRPC process\n" ); - return ret; - } - - RosApiCall *call = client_proc->current_call; - if(client_idx == 0 && call->user_call == 0) // xmlrpc client connected to roscore (master) - { - if( client_proc->message_type != XMLRPC_MESSAGE_RESPONSE ) - { - PRINT_ERROR ( "cRosApiParseResponse() : Not a response message \n" ); - return ret; - } - // xmlrpcParamVectorPrint(&client_proc->response); //// - - switch (call->method) - { - case CROS_API_REGISTER_PUBLISHER: - { - PRINT_VDEBUG ( "cRosApiParseResponse() : registerPublisher response \n" ); - if( checkResponseValue( &client_proc->response ) ) - ret = 0; - - break; - } - case CROS_API_REGISTER_SERVICE: - { - PRINT_VDEBUG ( "cRosApiParseResponse() : registerService response \n" ); - - if( checkResponseValue( &client_proc->response ) ) - ret = 0; - - break; - } - case CROS_API_REGISTER_SUBSCRIBER: - { - PRINT_VDEBUG ( "cRosApiParseResponse() : registerSubscriber response \n" ); - - //Get the next subscriber without a topic host - if(call->provider_idx == -1) - { - PRINT_ERROR ( "cRosApiParseResponse() : Invalid provider index in call CROS_API_REGISTER_SUBSCRIBER for XMLRPC process\n" ); - return ret; - } - int subidx = call->provider_idx; - - if(checkResponseValue( &client_proc->response ) ) - { - ret = 0; - - //Check if there is some publishers for the subscription - XmlrpcParam *param = xmlrpcParamVectorAt(&client_proc->response, 0); - XmlrpcParam *array = xmlrpcParamArrayGetParamAt(param,2); - int available_pubs_n = xmlrpcParamArrayGetSize(array); - - if (available_pubs_n > 0) - { - int i ; - for (i = 0; i < available_pubs_n; i++) - { - XmlrpcParam* pub_host = xmlrpcParamArrayGetParamAt(array, i); - char* pub_host_string = xmlrpcParamGetString(pub_host); - //manage string for exploit informations - //removing the 'http://' and the last '/' - int dirty_string_len = strlen(pub_host_string); - char* clean_string = (char *)calloc(dirty_string_len-8+1,sizeof(char)); - if (clean_string != NULL) - { - char topic_host_addr[MAX_HOST_NAME_LEN+1]; - int topic_host_port; - strncpy(clean_string,pub_host_string+7,dirty_string_len-8); - char *progress = NULL; - char *hostname = strtok_r(clean_string,":",&progress); - - int rc = lookup_host(hostname, topic_host_addr, sizeof(topic_host_addr)); - if (rc == 0) - { - topic_host_port = atoi(strtok_r(NULL,":",&progress)); - if(enqueueRequestTopic(n, subidx, topic_host_addr, topic_host_port) == -1) - ret=-1; - } - else - ret=-1; - free(clean_string); - } - else - ret=-1; - } - } - } - break; - } - case CROS_API_LOOKUP_SERVICE: - { - PRINT_VDEBUG ( "cRosApiParseResponse() : Lookup Service response\n" ); - - //Get the next service caller without a topic host - if(call->provider_idx == -1) - { - PRINT_ERROR ( "cRosApiParseResponse() : Invalid provider index in call CROS_API_LOOKUP_SERVICE for XMLRPC process\n" ); - return ret; - } - - int srvcalleridx = call->provider_idx; - ServiceCallerNode* requesting_service_caller = &(n->service_callers[srvcalleridx]); - - TcprosProcess* rpcros_proc = &n->rpcros_client_proc[requesting_service_caller->rpcros_id]; - rpcros_proc->service_idx = call->provider_idx; - - if(checkResponseValue( &client_proc->response ) == 1) - { - ret = 0; - //Check if there is some publishers for the subscription - XmlrpcParam *param = xmlrpcParamVectorAt(&client_proc->response, 0); - if(xmlrpcParamArrayGetSize(param) >= 3) - { - XmlrpcParam *service_prov_host = xmlrpcParamArrayGetParamAt(param,2); - char* service_prov_host_string = xmlrpcParamGetString(service_prov_host); - //manage string for exploit informations - //removing the 'rosrpc://' and the last '/' - int dirty_string_len = strlen(service_prov_host_string); - char* clean_string = (char *)calloc(dirty_string_len-10+1,sizeof(char)); - if (clean_string != NULL) - { - strncpy(clean_string,service_prov_host_string+9,dirty_string_len-10); - char * progress = NULL; - char* hostname = strtok_r(clean_string,":",&progress); - if(requesting_service_caller->service_host == NULL) - { - requesting_service_caller->service_host = (char *)calloc(MAX_HOST_NAME_LEN+1,sizeof(char)); //deleted in cRosNodeDestroy - } - - if (requesting_service_caller->service_host != NULL) - { - int rc = lookup_host(hostname, requesting_service_caller->service_host, (MAX_HOST_NAME_LEN+1)*sizeof(char)); - if (rc == 0) - { - requesting_service_caller->service_port = atoi(strtok_r(NULL,":",&progress)); - - //need to be checked because maybe the connection went down suddenly. - if(!rpcros_proc->socket.open) - { - tcpIpSocketOpen(&(rpcros_proc->socket)); - } - - PRINT_VDEBUG( "cRosApiParseResponse() : Lookup Service response [tcp port: %d]\n", requesting_service_caller->service_port); - - //set the process to open the socket with the desired host - tcprosProcessChangeState(rpcros_proc, TCPROS_PROCESS_STATE_CONNECTING); - } - else - ret=-1; - } - else - ret=-1; - free(clean_string); - } - else - ret=-1; - } - else - { - PRINT_ERROR ( "cRosApiParseResponse() : Invalid response from ROS master when Looking up services (Not enough parameters in response)\n"); - ret=-1; - } - } - else - { - tcprosProcessChangeState(rpcros_proc, TCPROS_PROCESS_STATE_WAIT_FOR_CONNECTING); - } - - break; - } - case CROS_API_SUBSCRIBE_PARAM: - { - int paramsubidx = call->provider_idx; - ParameterSubscription *subscription = &n->paramsubs[paramsubidx]; - - if(checkResponseValue( &client_proc->response ) ) - { - XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response,0); - // XmlrpcParam *status_msg = xmlrpcParamArrayGetParamAt(array, 1); - XmlrpcParam *value = xmlrpcParamArrayGetParamAt(array, 2); - - XmlrpcParam copy; - int rc = xmlrpcParamCopy(©, value); - if (rc < 0) - break; - - subscription = &n->paramsubs[paramsubidx]; - - CrosNodeStatusUsr status; - initCrosNodeStatus(&status); - status.state = CROS_STATUS_PARAM_SUBSCRIBED; - status.provider_idx = paramsubidx; - status.parameter_key = subscription->parameter_key; - status.parameter_value = value; - subscription->status_api_callback(&status, subscription->context); // calls the parameter-subscriber application-defined status callback function (if specified when creating the publisher). - - ret = 0; - xmlrpcParamRelease(&subscription->parameter_value); - subscription->parameter_value = copy; - } - - break; - } - case CROS_API_GET_PID: - { - PRINT_VDEBUG ( "cRosApiParseResponse() : get PID response \n" ); - - if( checkResponseValue( &client_proc->response ) ) - { - ret = 0; - XmlrpcParam* roscore_pid_param = - xmlrpcParamArrayGetParamAt(xmlrpcParamVectorAt(&client_proc->response,0),2); - - if(n->roscore_pid == -1) - { - n->roscore_pid = roscore_pid_param->data.as_int; - } - else if (n->roscore_pid != roscore_pid_param->data.as_int) - { - n->roscore_pid = roscore_pid_param->data.as_int; - restartAdversing(n); - } - } - else - { - restartAdversing(n); - } - - xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); - break; - } - case CROS_API_UNREGISTER_SERVICE: - { - PRINT_VDEBUG ( "cRosApiParseResponse() : unregister service response \n" ); - - if( checkResponseValue( &client_proc->response ) ) - { - ret = 0; - XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response,0); - XmlrpcParam* status_msg = xmlrpcParamArrayGetParamAt(array, 1); - XmlrpcParam* unreg_num = xmlrpcParamArrayGetParamAt(array, 2); - } - - xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); - break; - } - case CROS_API_UNREGISTER_PUBLISHER: - { - PRINT_VDEBUG ( "cRosApiParseResponse() : unregister publisher response \n" ); - if( checkResponseValue( &client_proc->response ) ) - { - ret = 0; - XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response,0); - XmlrpcParam* status_msg = xmlrpcParamArrayGetParamAt(array, 1); - XmlrpcParam* unreg_num = xmlrpcParamArrayGetParamAt(array, 2); - } - - xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); - break; - } - case CROS_API_UNREGISTER_SUBSCRIBER: - { - PRINT_VDEBUG ( "cRosApiParseResponse() : unregister subscriber response \n" ); - if( checkResponseValue( &client_proc->response ) ) - { - ret = 0; - XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response, 0); - XmlrpcParam* status_msg = xmlrpcParamArrayGetParamAt(array, 1); - XmlrpcParam* unreg_num = xmlrpcParamArrayGetParamAt(array, 2); - } - - xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); - break; - } - case CROS_API_UNSUBSCRIBE_PARAM: - { - if( checkResponseValue( &client_proc->response ) ) - { - ret = 0; - XmlrpcParam *array = xmlrpcParamVectorAt(&client_proc->response, 0); - XmlrpcParam* status_msg = xmlrpcParamArrayGetParamAt(array, 1); - XmlrpcParam* unreg_num = xmlrpcParamArrayGetParamAt(array, 2); - } - - xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); - break; - } - default: - { - PRINT_ERROR ( "cRosApiParseResponse() : Invalid call method for XML RPC process connected to ROS master\n" ); - ret=-1; - } - } - } - else // client_idx > 0 || user_call = 1 - { - switch (call->method) - { - case CROS_API_LOOKUP_NODE: - case CROS_API_GET_PUBLISHED_TOPICS: - case CROS_API_GET_TOPIC_TYPES: - case CROS_API_GET_SYSTEM_STATE: - case CROS_API_GET_URI: - case CROS_API_LOOKUP_SERVICE: - case CROS_API_GET_BUS_STATS: - case CROS_API_GET_BUS_INFO: - case CROS_API_GET_MASTER_URI: - case CROS_API_SHUTDOWN: - case CROS_API_GET_PID: - case CROS_API_GET_SUBSCRIPTIONS: - case CROS_API_GET_PUBLICATIONS: - case CROS_API_DELETE_PARAM: - case CROS_API_SET_PARAM: - case CROS_API_GET_PARAM: - case CROS_API_SEARCH_PARAM: - case CROS_API_HAS_PARAM: - case CROS_API_GET_PARAM_NAMES: - { - ret = 0; - - // xmlrpcParamVectorPrint(&client_proc->response); //// - - ResultCallback callback = call->result_callback; - if (callback != NULL) - { - void *result = call->fetch_result_callback(&client_proc->response); - callback(call->id, result, call->context_data); - if (result != NULL) - call->free_result_callback(result); - } - - break; - } - case CROS_API_REQUEST_TOPIC: - { - if( checkResponseValue( &client_proc->response ) ) - { - int client_tcpros_ind; - char tcpros_host[MAX_HOST_NAME_LEN+1]; - ret = 0; - - XmlrpcParam* param_array = xmlrpcParamVectorAt(&client_proc->response,0); - XmlrpcParam* nested_array = xmlrpcParamArrayGetParamAt(param_array,2); - XmlrpcParam* tcp_host = xmlrpcParamArrayGetParamAt(nested_array,1); - XmlrpcParam* tcp_port = xmlrpcParamArrayGetParamAt(nested_array,2); - - RosApiCall *call = client_proc->current_call; - int sub_ind = call->provider_idx; - - int tcp_port_print = tcp_port->data.as_int; - SubscriberNode* sub = &n->subs[sub_ind]; - - PRINT_VDEBUG( "cRosApiParseResponse() : requestTopic response [tcp port: %d]\n", tcp_port_print); - xmlrpcProcessChangeState(client_proc,XMLRPC_PROCESS_STATE_IDLE); - - int rc = lookup_host(tcp_host->data.as_string, tcpros_host, sizeof(tcpros_host)); - if (rc == 0) - { - // Check if a Tcpros client is already connected to the received hostname and port for the current subscriber node - client_tcpros_ind = cRosNodeFindFirstTcprosClientProc(n, sub_ind, tcpros_host, tcp_port_print); - if(client_tcpros_ind == -1) // No Tcpros client is already connected to this hostname and port for the current subscriber node, recruit one: - { - client_tcpros_ind = cRosNodeRecruitTcprosClientProc(n, sub_ind); - if(client_tcpros_ind != -1) // A Tcpros client has been recruited to be used - { - TcprosProcess* tcpros_proc = &n->tcpros_client_proc[client_tcpros_ind]; - tcpros_proc->topic_idx = sub_ind; - - // need to be checked because maybe the connection went down suddenly. - if(!tcpros_proc->socket.open) - tcpIpSocketOpen(&(tcpros_proc->socket)); - - // set the process to open the socket with the desired host - tcpros_proc->sub_tcpros_host = (char *)realloc(tcpros_proc->sub_tcpros_host, strlen(tcpros_host)+sizeof(char)); // If already allocated, realloc() does nothing - if (tcpros_proc->sub_tcpros_host != NULL) - { - strcpy(tcpros_proc->sub_tcpros_host, tcpros_host); - tcpros_proc->sub_tcpros_port = tcp_port_print; - tcprosProcessChangeState(tcpros_proc, TCPROS_PROCESS_STATE_CONNECTING); - // printf("HOST: %s:%i\n",tcpros_proc->sub_tcpros_host,tcpros_proc->sub_tcpros_port); - } - else - { - PRINT_ERROR ( "cRosApiParseResponse() : Not enough memory allocating the publisher hostname string\n"); - ret=-1; - } - } - else - { - PRINT_ERROR ( "cRosApiParseResponse() : No TCPROS client process is available to be used to subscribe to the topic. Allocate more TCPROS client processes.\n"); - ret=-1; - } - } - } - else - { - PRINT_ERROR ( "cRosApiParseResponse() : Cannot resolve the publisher hostname received in response of REQUEST_TOPIC\n"); - ret=-1; - } - } - break; - } - default: - { - PRINT_ERROR ( "cRosApiParseResponse() : Invalid call method for XML RPC process connected to ROS node\n" ); - ret=-1; - } - } - } - - return ret; -} - -// return value is different from 0 only when a response message cannot be generated -int cRosApiParseRequestPrepareResponse( CrosNode *n, int server_idx ) -{ - int ret; - PRINT_VDEBUG ( "cRosApiParseRequestPrepareResponse()\n" ); - - XmlrpcProcess *server_proc = &(n->xmlrpc_server_proc[server_idx]); - - if( server_proc->message_type != XMLRPC_MESSAGE_REQUEST) - { - PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Not a request message \n" ); - return -1; - } - - ret=0; // Default return value - server_proc->message_type = XMLRPC_MESSAGE_RESPONSE; - - XmlrpcParamVector params; - xmlrpcParamVectorInit(¶ms); - - CrosApiMethod method = getMethodCode(dynStringGetData(&server_proc->method)); - switch (method) - { - case CROS_API_GET_PID: - { - xmlrpcParamVectorPushBackArray(¶ms); - XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); - xmlrpcParamArrayPushBackInt(array, 1); - xmlrpcParamArrayPushBackString(array, ""); - xmlrpcParamArrayPushBackInt(array, n->pid); - break; - } - case CROS_API_PUBLISHER_UPDATE: - { - PRINT_VDEBUG("publisherUpdate()\n"); - // TODO Store the subscribed node name - XmlrpcParam *caller_id_param, *topic_param, *publishers_param; - -#if CROS_DEBUG_LEVEL >= 3 - xmlrpcParamVectorPrint( &server_proc->params ); -#endif - - caller_id_param = xmlrpcParamVectorAt(&server_proc->params, 0); - topic_param = xmlrpcParamVectorAt( &server_proc->params, 1); - publishers_param = xmlrpcParamVectorAt(&server_proc->params, 2); - - if( xmlrpcParamGetType( caller_id_param ) != XMLRPC_PARAM_STRING || - xmlrpcParamGetType( topic_param ) != XMLRPC_PARAM_STRING || - xmlrpcParamGetType( publishers_param ) != XMLRPC_PARAM_ARRAY ) - { - PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Wrong publisherUpdate message \n" ); - xmlrpcParamVectorPushBackString( ¶ms, "Unable to resolve hostname" ); - break; - } - else - { - int array_size; - char *topic_name_param; - XmlrpcParam *uri; - int sub_idx = -1; - int uri_found = 0; - int i; - - topic_name_param = xmlrpcParamGetString( topic_param ); - array_size = xmlrpcParamArrayGetSize( publishers_param ); - - for(i = 0; i < n->n_subs; i++) - { - if (n->subs[i].topic_name == NULL) - continue; - - if( strcmp( topic_name_param, n->subs[i].topic_name ) == 0) - { - sub_idx = i; - break; - } - } - - if(array_size > 0) - { - uri = xmlrpcParamArrayGetParamAt ( publishers_param, 0 ); - if(xmlrpcParamGetType(uri) == XMLRPC_PARAM_STRING) - { - uri_found = 1; - } - } - - // The condition to be met is: - // Topic name must be registered and - // either some node is unregistering the topic or - // the host is found and the tcpros port is not assigned - if (sub_idx != -1 && (array_size == 0 || uri_found)) - { - // Subscriber that is still waiting for a tcpros connection found - publishers_param = xmlrpcParamVectorAt(&server_proc->params, 2); - int available_pubs_n = xmlrpcParamArrayGetSize(publishers_param); - - { - int pub_host_ind; - // Iterate through all the hosts while no error occurs - for(pub_host_ind=0;pub_host_indparams), 0); - topic_param = xmlrpcParamVectorAt( &(server_proc->params), 1); - protocols_param = xmlrpcParamVectorAt( &(server_proc->params), 2); - - if( xmlrpcParamGetType( node_param ) != XMLRPC_PARAM_STRING || - xmlrpcParamGetType( topic_param ) != XMLRPC_PARAM_STRING || - xmlrpcParamGetType( protocols_param ) != XMLRPC_PARAM_ARRAY ) - { - PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Wrong requestTopic message \n" ); - xmlrpcParamVectorPushBackString( ¶ms, "Wrong requestTopic message" ); - break; - } - else - { - int array_size = xmlrpcParamArrayGetSize( protocols_param ); - XmlrpcParam *proto, *proto_name; - int i = 0, topic_found = 0, protocol_found = 0; - - for( i = 0 ; i < n->n_pubs; i++) - { - PublisherNode *pub = &n->pubs[i]; - if (pub->topic_name == NULL) - continue; - - if( strcmp( xmlrpcParamGetString( topic_param ), pub->topic_name ) == 0) - { - topic_found = 1; - if (strlen(server_proc->host) != 0) - { - CrosNodeStatusUsr status; - initCrosNodeStatus(&status); - status.xmlrpc_host = server_proc->host; - status.xmlrpc_port = server_proc->port; - cRosNodeStatusCallback(&status, pub->context); // calls the publisher-status application-defined callback function (if specified when creating the publisher). Undocumented status callback? - } - - break; - } - } - - for( i = 0 ; i < array_size; i++) - { - proto = xmlrpcParamArrayGetParamAt ( protocols_param, i ); - - if( xmlrpcParamGetType( proto ) == XMLRPC_PARAM_ARRAY && - ( proto_name = xmlrpcParamArrayGetParamAt ( proto, 0 ) ) != NULL && - xmlrpcParamGetType( proto_name ) == XMLRPC_PARAM_STRING && - strcmp( xmlrpcParamGetString( proto_name ), CROS_TRANSPORT_TCPROS_STRING) == 0 ) - { - protocol_found = 1; - break; - } - } - - if( topic_found && protocol_found) - { - xmlrpcParamVectorPushBackArray(¶ms); - XmlrpcParam *array1 = xmlrpcParamVectorAt(¶ms, 0); - xmlrpcParamArrayPushBackInt(array1, 1); - xmlrpcParamArrayPushBackString(array1, ""); - XmlrpcParam* array2 = xmlrpcParamArrayPushBackArray(array1); - xmlrpcParamArrayPushBackString( array2, CROS_TRANSPORT_TCPROS_STRING ); - xmlrpcParamArrayPushBackString( array2, n->host ); - xmlrpcParamArrayPushBackInt( array2, n->tcpros_port ); - } - else - { - PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Topic or protocol for requestTopic not supported\n" ); - xmlrpcParamVectorPushBackString( ¶ms, "Topic or protocol for requestTopic not supported"); - break; - } - } - - break; - } - case CROS_API_GET_BUS_STATS: - { - // CHECK-ME What to answer here? - xmlrpcParamVectorPushBackArray(¶ms); - XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); - xmlrpcParamArrayPushBackInt(array, 0); - xmlrpcParamArrayPushBackString(array, ""); - break; - } - case CROS_API_GET_BUS_INFO: - { - int proc_idx; - char tcpros_url_msg[MAX_HOST_NAME_LEN+60]; - XmlrpcParam *ret_parm_arr, *ret_businfo_arr, *ret_connect_arr; - - // Answer the transport/topic connection information - xmlrpcParamVectorPushBackArray(¶ms); - ret_parm_arr = xmlrpcParamVectorAt(¶ms, 0); - if(ret_parm_arr != NULL) - { - xmlrpcParamArrayPushBackInt(ret_parm_arr, 1); - xmlrpcParamArrayPushBackString(ret_parm_arr, ""); - ret_businfo_arr = xmlrpcParamArrayPushBackArray(ret_parm_arr); - if(ret_businfo_arr == NULL) - ret=-1; - } - else - ret=-1; - - for(proc_idx=0;proc_idxtcpros_client_proc[proc_idx]; - if(cur_cli_proc->topic_idx != -1) - { - ret_connect_arr = xmlrpcParamArrayPushBackArray(ret_businfo_arr); - if(ret_connect_arr != NULL) - { - xmlrpcParamArrayPushBackInt(ret_connect_arr, proc_idx); - snprintf(tcpros_url_msg, sizeof(tcpros_url_msg), "http://%s:%hu/", tcpIpSocketGetRemoteAddress(&cur_cli_proc->socket), tcpIpSocketGetRemotePort(&cur_cli_proc->socket)); - // This URL could also be the name of the node that receives the topic messages - xmlrpcParamArrayPushBackString(ret_connect_arr, tcpros_url_msg); - // In this case, this node is the subscriber, so the connection direction marked as inbound. This means that data (messages) is transmitted - // from other node to this node: published -> subscriber (although the connection is established from this node to other node: subscriber -> publisher) - xmlrpcParamArrayPushBackString(ret_connect_arr, "i"); - xmlrpcParamArrayPushBackString(ret_connect_arr, "TCPROS"); - xmlrpcParamArrayPushBackString(ret_connect_arr, dynStringGetData(&cur_cli_proc->topic)); - xmlrpcParamArrayPushBackInt(ret_connect_arr, 1); - snprintf(tcpros_url_msg, sizeof(tcpros_url_msg), "TCPROS connection on port %hu to [%s:%hu on socket %i]", tcpIpSocketGetPort(&cur_cli_proc->socket), cur_cli_proc->sub_tcpros_host, cur_cli_proc->sub_tcpros_port, tcpIpSocketGetFD(&cur_cli_proc->socket)); - // example of tcpros_url_msg sniffed from a ROS subscriber node (/rosout): TCPROS connection on port 55636 to [host_name:49463 on socket 14] - // 55636 is the TCPROS local port in this (subscriber) node that is connected to the listening port of the other (remote) node (publisher) - // host_name is the address of the other (remote node), which is the publisher. In this case it is also a local address - // 49463 is the listening port of the other (remote) node, which is the publisher node - // 14 is the local file despcriptor of this TCPROS connection, that is a file descriptor of the /rousout node process - xmlrpcParamArrayPushBackString(ret_connect_arr, tcpros_url_msg); - } - else - ret=-1; - } - } - - for(proc_idx=0;proc_idxtcpros_server_proc[proc_idx]; - if(cur_ser_proc->topic_idx != -1) - { - ret_connect_arr = xmlrpcParamArrayPushBackArray(ret_businfo_arr); - if(ret_connect_arr != NULL) - { - xmlrpcParamArrayPushBackInt(ret_connect_arr, proc_idx); - snprintf(tcpros_url_msg, sizeof(tcpros_url_msg), "http://%s:%hu/", tcpIpSocketGetRemoteAddress(&cur_ser_proc->socket), tcpIpSocketGetRemotePort(&cur_ser_proc->socket)); - xmlrpcParamArrayPushBackString(ret_connect_arr, tcpros_url_msg); - // In this case, this node is the publisher, so the connection direction is marked as outbound. This means that data (messages) is transmitted - // from this node to other node: published -> subscriber (although the connection is established from other node to this node: subscriber -> publisher) - xmlrpcParamArrayPushBackString(ret_connect_arr, "o"); - xmlrpcParamArrayPushBackString(ret_connect_arr, "TCPROS"); - xmlrpcParamArrayPushBackString(ret_connect_arr, dynStringGetData(&cur_ser_proc->topic)); - xmlrpcParamArrayPushBackInt(ret_connect_arr, 1); - snprintf(tcpros_url_msg, sizeof(tcpros_url_msg), "TCPROS connection on port %hu to [%s:%hu on socket %i]", tcpIpSocketGetPort(&cur_ser_proc->socket), tcpIpSocketGetRemoteAddress(&cur_ser_proc->socket), tcpIpSocketGetRemotePort(&cur_ser_proc->socket), tcpIpSocketGetFD(&cur_ser_proc->socket)); - // example of tcpros_url_msg sniffed from a ROS publisher node (/turtlesim): TCPROS connection on port 49463 to [127.0.0.1:55636 on socket 26] - // 49463 is the TCPROS local listening port of this (publisher) node, which received the incoming connection - // 127.0.0.1 is the address of the other (remote node). In this case it is also a local address - // 55636 is the other-(remote)-node port, which corresponds to the socket created for this TCPROS connection - // 26 is the local file despcriptor of this TCPROS connection - // So this XML call returns information about the TCPROS connection between publisher and subscriber, although this information is tramsitted through the XMLRPC port connetion - xmlrpcParamArrayPushBackString(ret_connect_arr, tcpros_url_msg); - } - else - ret=-1; - } - } - -// xmlrpcParamVectorPrint( ¶ms ); - break; - } - case CROS_API_GET_MASTER_URI: - { - xmlrpcParamVectorPushBackArray(¶ms); - XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); - xmlrpcParamArrayPushBackInt(array, 1); - xmlrpcParamArrayPushBackString(array, ""); - char node_uri[256]; - snprintf(node_uri, 256, "http://%s:%d/", n->roscore_host, n->roscore_port); - xmlrpcParamArrayPushBackString(array, node_uri); - break; - } - case CROS_API_SHUTDOWN: - { - xmlrpcParamVectorPushBackArray(¶ms); - XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); - xmlrpcParamArrayPushBackInt(array, 1); - xmlrpcParamArrayPushBackString(array, ""); - xmlrpcParamArrayPushBackInt(array, 1); - PRINT_INFO ( "cRosApiParseRequestPrepareResponse() : Received shutdown request through ROS slave API. This call will be ignored.\n" ); - break; - } - case CROS_API_PARAM_UPDATE: - { - ParameterSubscription* subscription = NULL; - - XmlrpcParam *node_param = xmlrpcParamVectorAt(&server_proc->params, 0); - XmlrpcParam *key_param = xmlrpcParamVectorAt(&server_proc->params, 1); - XmlrpcParam *value_param = xmlrpcParamVectorAt(&server_proc->params, 2); - if (xmlrpcParamGetType(key_param) != XMLRPC_PARAM_STRING) - { - PRINT_ERROR ( "cRosApiParseRequestPrepareResponse() : Wrong paramUpdate message\n" ); - goto PrepareResponse; - } - - int paramsubidx = -1; - char *parameter_key = xmlrpcParamGetString(key_param); - int it = 0; - for (it = 0 ; it < n->n_paramsubs; it++) - { - if (n->paramsubs[it].parameter_key == NULL) - continue; - - if (strncmp(parameter_key, n->paramsubs[it].parameter_key, strlen(n->paramsubs[it].parameter_key)) == 0) - { - paramsubidx = it; - - break; - } - } - - if (paramsubidx != -1) - { - subscription = &n->paramsubs[it]; - CrosNodeStatusUsr status; - initCrosNodeStatus(&status); - status.state = CROS_STATUS_PARAM_UPDATE; - status.provider_idx = it; - status.parameter_key = parameter_key; - status.parameter_value = value_param; - subscription->status_api_callback(&status, subscription->context); // calls the parameter-subscriber-status application-defined callback function (if specified when creating the subscriber). - - XmlrpcParam param; - int rc = xmlrpcParamCopy(¶m, value_param); - if (rc != -1) - { - xmlrpcParamRelease(&subscription->parameter_value); - subscription->parameter_value = param; - } - } - - PrepareResponse: - xmlrpcParamVectorPushBackArray(¶ms); - XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); - xmlrpcParamArrayPushBackInt(array, 1); - xmlrpcParamArrayPushBackString(array, ""); - if (subscription == NULL) - xmlrpcParamArrayPushBackInt(array, 1); - else - xmlrpcParamArrayPushBackInt(array, 0); - break; - } - case CROS_API_GET_SUBSCRIPTIONS: - { - xmlrpcParamVectorPushBackArray(¶ms); - XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); - xmlrpcParamArrayPushBackInt(array, 1); - xmlrpcParamArrayPushBackString(array, ""); - XmlrpcParam* param_array = xmlrpcParamArrayPushBackArray(array); - - int i = 0; - for(i = 0; i < n->n_subs; i++) - { - XmlrpcParam* sub_array = xmlrpcParamArrayPushBackArray(param_array); - xmlrpcParamArrayPushBackString(sub_array, n->subs[i].topic_name); - xmlrpcParamArrayPushBackString(sub_array, n->subs[i].topic_type); - } - - break; - } - case CROS_API_GET_PUBLICATIONS: - { - xmlrpcParamVectorPushBackArray(¶ms); - XmlrpcParam *array = xmlrpcParamVectorAt(¶ms, 0); - xmlrpcParamArrayPushBackInt(array, 1); - xmlrpcParamArrayPushBackString(array, ""); - XmlrpcParam* param_array = xmlrpcParamArrayPushBackArray(array); - - int i = 0; - for(i = 0; i < n->n_pubs; i++) - { - XmlrpcParam* sub_array = xmlrpcParamArrayPushBackArray(param_array); - xmlrpcParamArrayPushBackString(sub_array, n->pubs[i].topic_name); - xmlrpcParamArrayPushBackString(sub_array, n->pubs[i].topic_type); - } - - break; - } - default: - { - PRINT_ERROR("cRosApiParseRequestPrepareResponse() : Unknown method \n Message : \n %s", - dynStringGetData( &(server_proc->message))); - xmlrpcParamVectorPushBackString( ¶ms, "Unknown method"); - break; - } - } - - xmlrpcProcessClear(server_proc); - generateXmlrpcMessage(NULL, 0, server_proc->message_type, - dynStringGetData(&server_proc->method), ¶ms, &server_proc->message); // host and port are not used in XMLRPC_MESSAGE_RESPONSE - xmlrpcParamVectorRelease(¶ms); - - return ret; -} - -const char * getMethodName(CrosApiMethod method) -{ - switch (method) - { - case CROS_API_NONE: - return NULL; - case CROS_API_REGISTER_SERVICE: - return "registerService"; - case CROS_API_UNREGISTER_SERVICE: - return "unregisterService"; - case CROS_API_REGISTER_SUBSCRIBER: - return "registerSubscriber"; - case CROS_API_UNREGISTER_SUBSCRIBER: - return "unregisterSubscriber"; - case CROS_API_REGISTER_PUBLISHER: - return "registerPublisher"; - case CROS_API_UNREGISTER_PUBLISHER: - return "unregisterPublisher"; - case CROS_API_LOOKUP_NODE: - return "lookupNode"; - case CROS_API_GET_PUBLISHED_TOPICS: - return "getPublishedTopics"; - case CROS_API_GET_TOPIC_TYPES: - return "getTopicTypes"; - case CROS_API_GET_SYSTEM_STATE: - return "getSystemState"; - case CROS_API_GET_URI: - return "getUri"; - case CROS_API_LOOKUP_SERVICE: - return "lookupService"; - case CROS_API_GET_BUS_STATS: - return "getBusStats"; - case CROS_API_GET_BUS_INFO: - return "getBusInfo"; - case CROS_API_GET_MASTER_URI: - return "getMasterUri"; - case CROS_API_SHUTDOWN: - return "shutdown"; - case CROS_API_GET_PID: - return "getPid"; - case CROS_API_GET_SUBSCRIPTIONS: - return "getSubscriptions"; - case CROS_API_GET_PUBLICATIONS: - return "getPublications"; - case CROS_API_PARAM_UPDATE: - return "paramUpdate"; - case CROS_API_PUBLISHER_UPDATE: - return "publisherUpdate"; - case CROS_API_REQUEST_TOPIC: - return "requestTopic"; - case CROS_API_DELETE_PARAM: - return "deleteParam"; - case CROS_API_SET_PARAM: - return "setParam"; - case CROS_API_GET_PARAM: - return "getParam"; - case CROS_API_SEARCH_PARAM: - return "searchParam"; - case CROS_API_SUBSCRIBE_PARAM: - return "subscribeParam"; - case CROS_API_UNSUBSCRIBE_PARAM: - return "unsubscribeParam"; - case CROS_API_HAS_PARAM: - return "hasParam"; - case CROS_API_GET_PARAM_NAMES: - return "getParamNames"; - default: - PRINT_ERROR( "getMethodName() : Invalid CrosApiMethod value specified\n" ); - return NULL; - } -} - -CrosApiMethod getMethodCode(const char *method) -{ - if (strcmp(method, "registerService") == 0) - return CROS_API_REGISTER_SERVICE; - else if (strcmp(method, "unregisterService") == 0) - return CROS_API_UNREGISTER_SERVICE; - else if (strcmp(method, "registerSubscriber") == 0) - return CROS_API_REGISTER_SUBSCRIBER; - else if (strcmp(method, "unregisterSubscriber") == 0) - return CROS_API_UNREGISTER_SUBSCRIBER; - else if (strcmp(method, "registerPublisher") == 0) - return CROS_API_REGISTER_PUBLISHER; - else if (strcmp(method, "unregisterPublisher") == 0) - return CROS_API_UNREGISTER_PUBLISHER; - else if (strcmp(method, "lookupNode") == 0) - return CROS_API_LOOKUP_NODE; - else if (strcmp(method, "getPublishedTopics") == 0) - return CROS_API_GET_PUBLISHED_TOPICS; - else if (strcmp(method, "getTopicTypes") == 0) - return CROS_API_GET_TOPIC_TYPES; - else if (strcmp(method, "getSystemState") == 0) - return CROS_API_GET_SYSTEM_STATE; - else if (strcmp(method, "getUri") == 0) - return CROS_API_GET_URI; - else if (strcmp(method, "lookupService") == 0) - return CROS_API_LOOKUP_SERVICE; - else if (strcmp(method, "getBusStats") == 0) - return CROS_API_GET_BUS_STATS; - else if (strcmp(method, "getBusInfo") == 0) - return CROS_API_GET_BUS_INFO; - else if (strcmp(method, "getMasterUri") == 0) - return CROS_API_GET_MASTER_URI; - else if (strcmp(method, "shutdown") == 0) - return CROS_API_SHUTDOWN; - else if (strcmp(method, "getPid") == 0) - return CROS_API_GET_PID; - else if (strcmp(method, "getSubscriptions") == 0) - return CROS_API_GET_SUBSCRIPTIONS; - else if (strcmp(method, "getPublications") == 0) - return CROS_API_GET_PUBLICATIONS; - else if (strcmp(method, "paramUpdate") == 0) - return CROS_API_PARAM_UPDATE; - else if (strcmp(method, "publisherUpdate") == 0) - return CROS_API_PUBLISHER_UPDATE; - else if (strcmp(method, "requestTopic") == 0) - return CROS_API_REQUEST_TOPIC; - else if (strcmp(method, "deleteParam") == 0) - return CROS_API_DELETE_PARAM; - else if (strcmp(method, "setParam") == 0) - return CROS_API_SET_PARAM; - else if (strcmp(method, "getParam") == 0) - return CROS_API_GET_PARAM; - else if (strcmp(method, "searchParam") == 0) - return CROS_API_SEARCH_PARAM; - else if (strcmp(method, "subscribeParam") == 0) - return CROS_API_SUBSCRIBE_PARAM; - else if (strcmp(method, "unsubscribeParam") == 0) - return CROS_API_UNSUBSCRIBE_PARAM; - else if (strcmp(method, "hasParam") == 0) - return CROS_API_HAS_PARAM; - else if (strcmp(method, "getParamNames") == 0) - return CROS_API_GET_PARAM_NAMES; - else - return CROS_API_NONE; -} - -int isRosMasterApi(CrosApiMethod method) -{ - switch (method) - { - case CROS_API_NONE: - return 0; - case CROS_API_REGISTER_SERVICE: - case CROS_API_UNREGISTER_SERVICE: - case CROS_API_REGISTER_SUBSCRIBER: - case CROS_API_UNREGISTER_SUBSCRIBER: - case CROS_API_REGISTER_PUBLISHER: - case CROS_API_UNREGISTER_PUBLISHER: - case CROS_API_LOOKUP_NODE: - case CROS_API_GET_PUBLISHED_TOPICS: - case CROS_API_GET_TOPIC_TYPES: - case CROS_API_GET_SYSTEM_STATE: - case CROS_API_GET_URI: - case CROS_API_LOOKUP_SERVICE: - return 1; - case CROS_API_GET_BUS_STATS: - case CROS_API_GET_BUS_INFO: - case CROS_API_GET_MASTER_URI: - case CROS_API_SHUTDOWN: - case CROS_API_GET_PID: - case CROS_API_GET_SUBSCRIPTIONS: - case CROS_API_GET_PUBLICATIONS: - case CROS_API_PARAM_UPDATE: - case CROS_API_PUBLISHER_UPDATE: - case CROS_API_REQUEST_TOPIC: - return 0; - case CROS_API_DELETE_PARAM: - case CROS_API_SET_PARAM: - case CROS_API_GET_PARAM: - case CROS_API_SEARCH_PARAM: - case CROS_API_SUBSCRIBE_PARAM: - case CROS_API_UNSUBSCRIBE_PARAM: - case CROS_API_HAS_PARAM: - case CROS_API_GET_PARAM_NAMES: - return 1; - default: - PRINT_ERROR ( "isRosMasterApi() : Invalid CrosApiMethod value specified\n" ); - return -1; - } -} - -int isRosSlaveApi(CrosApiMethod method) -{ - switch (method) - { - case CROS_API_NONE: - return 0; - case CROS_API_REGISTER_SERVICE: - case CROS_API_UNREGISTER_SERVICE: - case CROS_API_REGISTER_SUBSCRIBER: - case CROS_API_UNREGISTER_SUBSCRIBER: - case CROS_API_REGISTER_PUBLISHER: - case CROS_API_UNREGISTER_PUBLISHER: - case CROS_API_LOOKUP_NODE: - case CROS_API_GET_PUBLISHED_TOPICS: - case CROS_API_GET_TOPIC_TYPES: - case CROS_API_GET_SYSTEM_STATE: - case CROS_API_GET_URI: - case CROS_API_LOOKUP_SERVICE: - return 0; - case CROS_API_GET_BUS_STATS: - case CROS_API_GET_BUS_INFO: - case CROS_API_GET_MASTER_URI: - case CROS_API_SHUTDOWN: - case CROS_API_GET_PID: - case CROS_API_GET_SUBSCRIPTIONS: - case CROS_API_GET_PUBLICATIONS: - case CROS_API_PARAM_UPDATE: - case CROS_API_PUBLISHER_UPDATE: - case CROS_API_REQUEST_TOPIC: - return 1; - case CROS_API_DELETE_PARAM: - case CROS_API_SET_PARAM: - case CROS_API_GET_PARAM: - case CROS_API_SEARCH_PARAM: - case CROS_API_SUBSCRIBE_PARAM: - case CROS_API_UNSUBSCRIBE_PARAM: - case CROS_API_HAS_PARAM: - case CROS_API_GET_PARAM_NAMES: - return 0; - default: - PRINT_ERROR ( "isRosSlaveApi() : Invalid CrosApiMethod value specified\n" ); - return -1; - } -} diff --git a/src/hal/components/cros/src/cros_service.c b/src/hal/components/cros/src/cros_service.c deleted file mode 100644 index d39e7fbb..00000000 --- a/src/hal/components/cros/src/cros_service.c +++ /dev/null @@ -1,433 +0,0 @@ -#include -#include -#include -#include - -#include "cros_service.h" -#include "cros_service_internal.h" -#include "cros_message_internal.h" -#include "tcpros_tags.h" -#include "cros_defs.h" -#include "md5.h" - -cRosErrCodePack initCrosSrv(cRosSrvDef* srv) -{ - cRosErrCodePack ret_err; - - if(srv == NULL) - return CROS_BAD_PARAM_ERR; - - srv->request = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); - srv->response = (cRosMessageDef *)malloc(sizeof(cRosMessageDef)); - if(srv->request != NULL && srv->response != NULL) - { - ret_err = initCrosMsg(srv->request); - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - ret_err = initCrosMsg(srv->response); - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - srv->name = NULL; - srv->package = NULL; - srv->plain_text = NULL; - srv->root_dir = NULL; - ret_err = CROS_SUCCESS_ERR_PACK; - } - else - { - cRosMessageDefFree(srv->request); - free(srv->request); - free(srv->response); - ret_err = CROS_MEM_ALLOC_ERR; - } - } - else - { - free(srv->request); - free(srv->response); - ret_err = CROS_MEM_ALLOC_ERR; - } - } - else - { - free(srv->request); - free(srv->response); - ret_err = CROS_MEM_ALLOC_ERR; - } - return ret_err; -} - -cRosErrCodePack loadFromFileSrv(const char *filename, cRosSrvDef *srv) -{ - cRosErrCodePack ret_err; - size_t f_read_bytes; - - char* file_tokenized = (char* )malloc(strlen(filename)+sizeof(char)); - if(file_tokenized == NULL) - return CROS_MEM_ALLOC_ERR; - - file_tokenized[0] = '\0'; - strcpy(file_tokenized, filename); - char* token_pack = NULL; - char* token_root = NULL; - char* token_name = NULL; - - FILE *f = fopen(filename, "rb"); - - if(f != NULL) - { - ret_err=CROS_SUCCESS_ERR_PACK; - - fseek(f, 0, SEEK_END); - long fsize = ftell(f); - fseek(f, 0, SEEK_SET); - char *srv_req; - char *srv_res; - char *srv_text = (char *)malloc(fsize + 1); - if(srv_text == NULL) - { - fclose(f); - free(file_tokenized); - return CROS_MEM_ALLOC_ERR; - } - - f_read_bytes = fread(srv_text, 1, fsize, f); - fclose(f); - - if(f_read_bytes != (size_t)fsize) - { - free(file_tokenized); - free(srv_text); - return CROS_READ_SVC_FILE_ERR; - } - - - srv_text[fsize] = '\0'; - srv->plain_text = strdup(srv_text); // equiv. to malloc() + memcpy() - if(srv->plain_text == NULL) - { - free(srv_text); - free(file_tokenized); - return CROS_MEM_ALLOC_ERR; - } - - //splitting msg_text into the request response parts - srv_res = strstr(srv_text, SRV_DELIMITER); - if(srv_res == NULL) // The delimiter must be present in the service definition - { - free(srv_text); - free(file_tokenized); - return CROS_SVC_FILE_DELIM_ERR; - } - - //if the srv has some request parameters - if(srv_res != srv_text) - { - //split before the first delim char - *(srv_res - 1) = '\0'; - srv_req = srv_text; - } - else - srv_req = ""; // The service has no request parameter - - srv_res += strlen(SRV_DELIMITER); // skip the delimiter - if(*srv_res == '\n') - srv_res++; // skip the new line char - - char* tok = strtok(file_tokenized,"/."); - - while(tok != NULL) - { - if(strcmp(tok, "srv") != 0) - { - token_root = token_pack; - token_pack = token_name; - token_name = tok ; - } - tok = strtok(NULL,"/."); - } - - //build up the root path - char* it = file_tokenized; - while(it != token_root) - { - if(*it == '\0') - *it='/'; - it++; - } - - srv->root_dir = (char*) malloc (strlen(file_tokenized)+sizeof(char)); srv->root_dir[0] = '\0'; - srv->package = (char*) malloc (strlen(token_pack)+sizeof(char)); srv->package[0] = '\0'; - srv->name = (char*) malloc (strlen(token_name)+sizeof(char)); srv->name[0] = '\0'; - if(srv->root_dir != NULL && srv->package != NULL && srv->name != NULL) - { - strcpy(srv->root_dir,file_tokenized); - strcpy(srv->package,token_pack); - strcpy(srv->name,token_name); - } - else - ret_err=CROS_MEM_ALLOC_ERR; - - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - srv->request->package = strdup(srv->package); - srv->request->root_dir = strdup(srv->root_dir); - if(srv->request->package != NULL && srv->request->root_dir != NULL) - { - ret_err=loadFromStringMsg(srv_req, srv->request); - ret_err=cRosAddErrCodeIfErr(ret_err, CROS_LOAD_SVC_FILE_REQ_ERR); // If loadFromStringMsg() failed, add more info to the error-code pack, indicating the context - } - else - ret_err=CROS_MEM_ALLOC_ERR; - } - - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - srv->response->package = strdup(srv->package); - srv->response->root_dir = strdup(srv->root_dir); - if(srv->response->package != NULL && srv->response->root_dir != NULL) - { - ret_err=loadFromStringMsg(srv_res, srv->response); - ret_err=cRosAddErrCodeIfErr(ret_err, CROS_LOAD_SVC_FILE_RES_ERR); // If loadFromStringMsg() failed, add more another error code to the to the error-code pack indicating the context - } - else - ret_err=CROS_MEM_ALLOC_ERR; - } - - if(ret_err != CROS_SUCCESS_ERR_PACK) // An error occurred, free allocated memory before exiting - { - free(srv->response->root_dir); - free(srv->response->package); - free(srv->request->root_dir); - free(srv->request->package); - free(srv->root_dir); - free(srv->package); - free(srv->name); - srv->name = NULL; - srv->package = NULL; - srv->root_dir = NULL; - srv->request->package = NULL; - srv->request->root_dir = NULL; - srv->response->package = NULL; - srv->response->root_dir = NULL; - } - - free(srv_text); - } - else - ret_err=CROS_OPEN_SVC_FILE_ERR; - - free(file_tokenized); - - return ret_err; -} - -// Compute dependencies of the specified service file -cRosErrCodePack getFileDependenciesSrv(char* filename, cRosSrvDef* srv, msgDep* deps) -{ - cRosErrCodePack ret_err; - ret_err = loadFromFileSrv(filename, srv); - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - ret_err = getDependenciesMsg(srv->request,deps); - if(ret_err == CROS_SUCCESS_ERR_PACK) - ret_err = getDependenciesMsg(srv->response,deps); - } - return ret_err; -} - -// Compute full text of service, including text of embedded -// types. The text of the main srv is listed first. Embedded -// srv files are denoted first by an 80-character '=' separator, -// followed by a type declaration line,'MSG: pkg/type', followed by -// the text of the embedded type. -char* computeFullTextSrv(cRosSrvDef* srv, msgDep* deps) -{ - char* full_text = NULL; - char* msg_tag = "MSG: "; - int full_size = 0; - char separator[81]; - separator[80] = '\0'; - - memset(&separator,'=', 80); - full_size += strlen(srv->plain_text); - - while(deps->next != NULL) - { - full_size = strlen(deps->msg->plain_text) + strlen(separator) + strlen(msg_tag) + 3/*New lines*/; - deps = deps->next; - } - - //rollback - while(deps->prev != NULL) deps = deps->prev; - full_text = (char *)malloc(full_size + 1); - memcpy(full_text,srv->plain_text,strlen(srv->plain_text) + 1); - - while(deps->next != NULL) - { - strcat(full_text, "\n"); - strcat(full_text, separator); - strcat(full_text, "\n"); - strcat(full_text, msg_tag); - strcat(full_text, deps->msg->package); - strcat(full_text, "/"); - strcat(full_text, deps->msg->name); - strcat(full_text, "\n"); - strcat(full_text, deps->msg->plain_text); - deps = deps->next; - } - return full_text; -} - -cRosService * cRosServiceNew() -{ - cRosService *ret_svc = (cRosService *)calloc(1, sizeof(cRosService)); - if(ret_svc != NULL) - cRosServiceInit(ret_svc); - return ret_svc; -} - -cRosErrCodePack cRosServiceInit(cRosService* service) -{ - cRosErrCodePack ret_err; - - service->request = NULL; - service->response = NULL; - service->md5sum = (char *)malloc(33*sizeof(char));// 32 chars + '\0'; - - ret_err = (service->md5sum != NULL)?CROS_SUCCESS_ERR_PACK:CROS_MEM_ALLOC_ERR; - - return ret_err; -} - -cRosErrCodePack cRosServiceBuild(cRosService* service, const char* filepath) -{ - return cRosServiceBuildInner(&service->request, &service->response, NULL, service->md5sum, filepath); -} - -cRosErrCodePack cRosServiceBuildInner(cRosMessage **request_ptr, cRosMessage **response_ptr, char **message_definition, char *md5sum, const char *srv_filepath) -{ - cRosErrCodePack ret_err; - - cRosSrvDef* srv = (cRosSrvDef *)malloc(sizeof(cRosSrvDef)); - if(srv == NULL) - return CROS_MEM_ALLOC_ERR; - - ret_err = initCrosSrv(srv); - if(ret_err != CROS_SUCCESS_ERR_PACK) - { - free(srv); - return ret_err; - } - - ret_err = loadFromFileSrv(srv_filepath, srv); - if (ret_err != CROS_SUCCESS_ERR_PACK) - { - cRosServiceDefFree(srv); - return ret_err; - } - - DynString buffer; - dynStringInit(&buffer); - - MD5_CTX md5_t; - MD5_Init(&md5_t); - - if(srv->request->plain_text != NULL) - { - getMD5Txt(srv->request, &buffer); - MD5_Update(&md5_t, dynStringGetData(&buffer), dynStringGetLen(&buffer)); - dynStringClear(&buffer); - } - - if(srv->response->plain_text != NULL) - { - getMD5Txt(srv->response, &buffer); - MD5_Update(&md5_t, dynStringGetData(&buffer), dynStringGetLen(&buffer)); - } - dynStringRelease(&buffer); - - unsigned char* result = (unsigned char *)malloc(16); - MD5_Final(result, &md5_t); - DynString output; - dynStringInit(&output); - cRosMD5Readable(result,&output); - free(result); - - strcpy(md5sum, dynStringGetData(&output)); - dynStringRelease(&output); - - if(srv->request->plain_text != NULL) - { - ret_err = cRosMessageBuildFromDef(request_ptr, srv->request); - } - - if(ret_err == CROS_SUCCESS_ERR_PACK && srv->response->plain_text != NULL) - { - ret_err = cRosMessageBuildFromDef(response_ptr, srv->response); - } - - if(ret_err == CROS_SUCCESS_ERR_PACK && srv->plain_text != NULL && message_definition != NULL) - { - *message_definition=(char *)malloc((strlen(srv->plain_text)+1)*sizeof(char)); - if(*message_definition != NULL) - strcpy(*message_definition,srv->plain_text); - else - ret_err = CROS_MEM_ALLOC_ERR; - } - - cRosServiceDefFree(srv); - - return ret_err; -} - -void cRosServiceDefFree(cRosSrvDef* service_def) -{ - if(service_def != NULL) - { - free(service_def->name); - free(service_def->package); - free(service_def->plain_text); - free(service_def->root_dir); - cRosMessageDefFree(service_def->request); - cRosMessageDefFree(service_def->response); - free(service_def); - } -} - -void cRosServiceFree(cRosService* service) -{ - cRosServiceRelease(service); - free(service); -} - -void cRosServiceRelease(cRosService* service) -{ - cRosMessageFree(service->request); - cRosMessageFree(service->response); - free(service->md5sum); - service->md5sum = NULL; -} - -static uint32_t getLen( DynBuffer *pkt ) -{ - uint32_t len; - const unsigned char *data = dynBufferGetCurrentData(pkt); - len = ROS_TO_HOST_UINT32(*(uint32_t *)data); - dynBufferMovePoseIndicator(pkt,sizeof(uint32_t)); - - return len; -} - -static uint32_t pushBackField( DynBuffer *pkt, TcprosTagStrDim *tag, const char *val ) -{ - size_t val_len = strlen( val ); - uint32_t out_len, field_len = tag->dim + val_len; - //PRINT_VDEBUG("pushBackField() : filed : %s field_len ; %d\n", tag->str, field_len); - out_len = HOST_TO_ROS_UINT32( field_len ); - dynBufferPushBackUInt32( pkt, out_len ); - dynBufferPushBackBuf( pkt, (const unsigned char*)tag->str, tag->dim ); - dynBufferPushBackBuf( pkt, (const unsigned char*)val, val_len ); - - return field_len + sizeof( uint32_t ); -} diff --git a/src/hal/components/cros/src/cros_tcpros.c b/src/hal/components/cros/src/cros_tcpros.c deleted file mode 100644 index 50aca5cb..00000000 --- a/src/hal/components/cros/src/cros_tcpros.c +++ /dev/null @@ -1,1049 +0,0 @@ -#include -#include -#include -#include - -#include "cros_api.h" -#include "cros_tcpros.h" -#include "cros_defs.h" -#include "tcpros_tags.h" -#include "tcpros_process.h" -#include "dyn_buffer.h" -#include "cros_log.h" - -static uint32_t getLen( DynBuffer *pkt ) -{ - uint32_t len; - const unsigned char *data = dynBufferGetCurrentData(pkt); - len = ROS_TO_HOST_UINT32(*(uint32_t *)data); - dynBufferMovePoseIndicator(pkt,sizeof(uint32_t)); - - return len; -} - -static uint32_t pushBackField( DynBuffer *pkt, TcprosTagStrDim *tag, const char *val ) -{ - size_t val_len = strlen( val ); - uint32_t out_len, field_len = tag->dim + val_len; - //PRINT_VDEBUG("pushBackField() : filed : %s field_len ; %d\n", tag->str, field_len); - out_len = HOST_TO_ROS_UINT32( field_len ); - dynBufferPushBackUInt32( pkt, out_len ); - dynBufferPushBackBuf( pkt, (const unsigned char*)tag->str, tag->dim ); - dynBufferPushBackBuf( pkt, (const unsigned char*)val, val_len ); - - return field_len + sizeof( uint32_t ); -} - -static void printPacket( DynBuffer *pkt, int print_data ) -{ - /* Save position indicator: it will be restored */ - int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( pkt ); - dynBufferRewindPoseIndicator ( pkt ); - - uint32_t bytes_to_read = getLen( pkt ); - - fprintf(cRosOutStreamGet(),"Header len %d\n",bytes_to_read); - while ( bytes_to_read > 0) - { - uint32_t field_len = getLen( pkt ); - const char *field = (const char *)dynBufferGetCurrentData( pkt ); - if( field_len ) - { - fwrite ( field, 1, field_len, stdout ); - fprintf(cRosOutStreamGet(),"\n" ); - dynBufferMovePoseIndicator( pkt, field_len ); - } - bytes_to_read -= ( sizeof(uint32_t) + field_len ); - } - - if( print_data ) - { - bytes_to_read = getLen( pkt ); - - fprintf(cRosOutStreamGet(),"Data len %d\n",bytes_to_read); - while ( bytes_to_read > 0) - { - uint32_t field_len = getLen( pkt ); - const char *field = (const char *)dynBufferGetCurrentData( pkt ); - if( field_len ) - { - fwrite ( field, 1, field_len, stdout ); - fprintf(cRosOutStreamGet(),"\n"); - dynBufferMovePoseIndicator( pkt, field_len ); - } - bytes_to_read -= ( sizeof(uint32_t) + field_len ); - } - } - - /* Restore position indicator */ - dynBufferSetPoseIndicator ( pkt, initial_pos_idx ); -} - -static TcprosParserState readSubcriptionHeader( TcprosProcess *p, uint32_t *flags ) -{ - PRINT_VVDEBUG("readSubcriptioHeader()\n"); - DynBuffer *packet = &(p->packet); - uint32_t bytes_to_read = getLen( packet ); - size_t packet_len = dynBufferGetSize( packet ); - - if( bytes_to_read > packet_len - sizeof( uint32_t ) ) - return TCPROS_PARSER_HEADER_INCOMPLETE; - - *flags = 0x0; - - PRINT_VDEBUG("readSubcriptioHeader() : Header len=%d\n",bytes_to_read); - - while ( bytes_to_read > 0) - { - uint32_t field_len = getLen( packet ); - - PRINT_VDEBUG("readSubcriptioHeader() : Field len=%d\n",field_len); - - const char *field = (const char *)dynBufferGetCurrentData( packet ); - if( field_len ) - { - if ( field_len > (uint32_t)TCPROS_CALLERID_TAG.dim && - strncmp ( field, TCPROS_CALLERID_TAG.str, TCPROS_CALLERID_TAG.dim ) == 0 ) - { - field += TCPROS_CALLERID_TAG.dim; - - dynStringReplaceWithStrN( &(p->caller_id), field, - field_len - TCPROS_CALLERID_TAG.dim ); - *flags |= TCPROS_CALLER_ID_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TOPIC_TAG.dim && - strncmp ( field, TCPROS_TOPIC_TAG.str, TCPROS_TOPIC_TAG.dim ) == 0 ) - { - field += TCPROS_TOPIC_TAG.dim; - - dynStringReplaceWithStrN( &(p->topic), field, - field_len - TCPROS_TOPIC_TAG.dim ); - *flags |= TCPROS_TOPIC_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TYPE_TAG.dim && - strncmp ( field, TCPROS_TYPE_TAG.str, TCPROS_TYPE_TAG.dim ) == 0 ) - { - field += TCPROS_TYPE_TAG.dim; - - dynStringReplaceWithStrN( &(p->type), field, - field_len - TCPROS_TYPE_TAG.dim ); - *flags |= TCPROS_TYPE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_MD5SUM_TAG.dim && - strncmp ( field, TCPROS_MD5SUM_TAG.str, TCPROS_MD5SUM_TAG.dim ) == 0 ) - { - field += TCPROS_MD5SUM_TAG.dim; - - dynStringReplaceWithStrN( &(p->md5sum), field, - field_len - TCPROS_MD5SUM_TAG.dim ); - *flags |= TCPROS_MD5SUM_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_MESSAGE_DEFINITION_TAG.dim && - strncmp ( field, TCPROS_MESSAGE_DEFINITION_TAG.str, TCPROS_MESSAGE_DEFINITION_TAG.dim ) == 0 ) - { - *flags |= TCPROS_MESSAGE_DEFINITION_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TCP_NODELAY_TAG.dim && - strncmp ( field, TCPROS_TCP_NODELAY_TAG.str, TCPROS_TCP_NODELAY_TAG.dim ) == 0 ) - { - field += TCPROS_TCP_NODELAY_TAG.dim; - p->tcp_nodelay = (*field == '1')?1:0; - *flags |= TCPROS_TCP_NODELAY_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_LATCHING_TAG.dim && - strncmp ( field, TCPROS_LATCHING_TAG.str, TCPROS_LATCHING_TAG.dim ) == 0 ) - { - PRINT_INFO("readSubcriptioHeader() WARNING : TCPROS_LATCHING_TAG not implemented\n"); - field += TCPROS_LATCHING_TAG.dim; - p->latching = (*field == '1')?1:0; - *flags |= TCPROS_LATCHING_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_ERROR_TAG.dim && - strncmp ( field, TCPROS_ERROR_TAG.str, TCPROS_ERROR_TAG.dim ) == 0 ) - { - PRINT_INFO("readSubcriptioHeader() WARNING : TCPROS_ERROR_TAG not implemented\n"); - *flags |= TCPROS_ERROR_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else - { - PRINT_ERROR("readSubcriptioHeader() : unknown field\n"); - *flags = 0x0; - break; - } - } - - bytes_to_read -= ( sizeof(uint32_t) + field_len ); - } - - return TCPROS_PARSER_DONE; - -} - -static TcprosParserState readPublicationHeader( TcprosProcess *p, uint32_t *flags ) -{ - PRINT_VVDEBUG("readPublicationHeader()\n"); - DynBuffer *packet = &(p->packet); - size_t bytes_to_read = dynBufferGetSize( packet ); - - *flags = 0x0; - - PRINT_VDEBUG("readPublicationHeader() : Header len=%lu\n", (long unsigned)bytes_to_read); - - while ( bytes_to_read > 0) - { - uint32_t field_len = getLen( packet ); - - PRINT_VDEBUG("readPublicationHeader() : Field len=%d\n",field_len); - - const char *field = (const char *)dynBufferGetCurrentData( packet ); - if( field_len ) - { - // http://wiki.ros.org/ROS/TCPROS doesn't mention it but it's sent anyway in ros groovy - if ( field_len > (uint32_t)TCPROS_MESSAGE_DEFINITION_TAG.dim && - strncmp ( field, TCPROS_MESSAGE_DEFINITION_TAG.str, TCPROS_MESSAGE_DEFINITION_TAG.dim ) == 0 ) - { - *flags |= TCPROS_MESSAGE_DEFINITION_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } else if ( field_len > (uint32_t)TCPROS_CALLERID_TAG.dim && - strncmp ( field, TCPROS_CALLERID_TAG.str, TCPROS_CALLERID_TAG.dim ) == 0 ) - { - field += TCPROS_CALLERID_TAG.dim; - - dynStringReplaceWithStrN( &(p->caller_id), field, - field_len - TCPROS_CALLERID_TAG.dim ); - *flags |= TCPROS_CALLER_ID_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TYPE_TAG.dim && - strncmp ( field, TCPROS_TYPE_TAG.str, TCPROS_TYPE_TAG.dim ) == 0 ) - { - field += TCPROS_TYPE_TAG.dim; - - dynStringReplaceWithStrN( &(p->type), field, - field_len - TCPROS_TYPE_TAG.dim ); - *flags |= TCPROS_TYPE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_MD5SUM_TAG.dim && - strncmp ( field, TCPROS_MD5SUM_TAG.str, TCPROS_MD5SUM_TAG.dim ) == 0 ) - { - field += TCPROS_MD5SUM_TAG.dim; - - dynStringReplaceWithStrN( &(p->md5sum), field, - field_len - TCPROS_MD5SUM_TAG.dim ); - *flags |= TCPROS_MD5SUM_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_LATCHING_TAG.dim && - strncmp ( field, TCPROS_LATCHING_TAG.str, TCPROS_LATCHING_TAG.dim ) == 0 ) - { - PRINT_INFO("readPublicationHeader() WARNING : TCPROS_LATCHING_TAG not implemented\n"); - field += TCPROS_LATCHING_TAG.dim; - p->latching = (*field == '1')?1:0; - *flags |= TCPROS_LATCHING_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TOPIC_TAG.dim && - strncmp ( field, TCPROS_TOPIC_TAG.str, TCPROS_TOPIC_TAG.dim ) == 0 ) - { - // http://wiki.ros.org/ROS/TCPROS doesn't mention it but it's sent anyway in ros groovy - field += TCPROS_TOPIC_TAG.dim; - - dynStringReplaceWithStrN( &(p->topic), field, - field_len - TCPROS_TOPIC_TAG.dim ); - *flags |= TCPROS_TOPIC_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_ERROR_TAG.dim && - strncmp ( field, TCPROS_ERROR_TAG.str, TCPROS_ERROR_TAG.dim ) == 0 ) - { - PRINT_INFO("readPublicationHeader() WARNING : TCPROS_ERROR_TAG not implemented\n"); - *flags |= TCPROS_ERROR_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TCP_NODELAY_TAG.dim && - strncmp ( field, TCPROS_TCP_NODELAY_TAG.str, TCPROS_TCP_NODELAY_TAG.dim ) == 0 ) - { - field += TCPROS_TCP_NODELAY_TAG.dim; - p->tcp_nodelay = (*field == '1')?1:0; - *flags |= TCPROS_TCP_NODELAY_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else - { - PRINT_ERROR("readPublicationHeader() : unknown field\n"); - *flags = 0x0; - break; - } - } - - bytes_to_read -= ( sizeof(uint32_t) + field_len ); - } - - return TCPROS_PARSER_DONE; -} - -TcprosParserState cRosMessageParseSubcriptionHeader( CrosNode *n, int server_idx ) -{ - PRINT_VVDEBUG("cRosMessageParseSubcriptionHeader()\n"); - - TcprosProcess *server_proc = &(n->tcpros_server_proc[server_idx]); - DynBuffer *packet = &(server_proc->packet); - - // Save position indicator: it will be restored - int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( packet ); - dynBufferRewindPoseIndicator ( packet ); - - uint32_t header_flags; - TcprosParserState ret = readSubcriptionHeader( server_proc, &header_flags ); - if( ret != TCPROS_PARSER_DONE ) - return ret; - - if( TCPROS_SUBCRIPTION_HEADER_FLAGS != ( header_flags&TCPROS_SUBCRIPTION_HEADER_FLAGS) ) - { - PRINT_ERROR("cRosMessageParseSubcriptionHeader() : There are missing fields in the received header.\n"); - ret = TCPROS_PARSER_ERROR; - } - else - { - int topic_found = 0; - int i = 0; - for( i = 0 ; i < n->n_pubs; i++) - { - PublisherNode *pub = &n->pubs[i]; - if (pub->topic_name == NULL) - continue; - - //printf("cRosMessageParseSubcriptionHeader() : checking if the topic in received header is what we expect: Topic name: %s Topic type: %s MD5: %s.\n", pub->topic_name, pub->topic_type, pub->md5sum); - - if( strcmp(pub->topic_name, dynStringGetData(&(server_proc->topic))) == 0 && - (strcmp(pub->topic_type, dynStringGetData(&(server_proc->type))) == 0 || strcmp(dynStringGetData(&(server_proc->type)), "*") == 0) && - (strcmp(pub->md5sum, dynStringGetData(&(server_proc->md5sum))) == 0 || strcmp(dynStringGetData(&(server_proc->md5sum)), "*") == 0)) - { - int list_elem; - - topic_found = 1; - server_proc->topic_idx = i; // Assign a topic (publisher index) to the TCPROS process - // Add the TcprosProcess index to the Publisher - for(list_elem=0;pub->tcpros_id_list[list_elem]!=-1;list_elem++); // Locate the list end - pub->tcpros_id_list[list_elem] = server_idx; - pub->tcpros_id_list[list_elem+1] = -1; // Set a new list end (sentinel) - break; - } - } - - if( ! topic_found ) - { - PRINT_ERROR("cRosMessageParseSubcriptionHeader() : Wrong service, type or md5sum in the received header. " - "Received: Topic name: %s Topic type: %s MD5: %s.\n", \ - dynStringGetData(&(server_proc->topic)), dynStringGetData(&(server_proc->type)), dynStringGetData(&(server_proc->md5sum))); - server_proc->topic_idx = -1; - ret = TCPROS_PARSER_ERROR; - } - else - { - if(server_proc->tcp_nodelay) - tcpIpSocketSetNoDelay(&server_proc->socket); - } - } - - // Restore position indicator - dynBufferSetPoseIndicator ( packet, initial_pos_idx ); - - return ret; -} - -TcprosParserState cRosMessageParsePublicationHeader( CrosNode *n, int client_idx ) -{ - PRINT_VVDEBUG("cRosMessageParsePublicationHeader()\n"); - - TcprosProcess *client_proc = &(n->tcpros_client_proc[client_idx]); - DynBuffer *packet = &(client_proc->packet); - - /* Save position indicator: it will be restored */ - int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( packet ); - dynBufferRewindPoseIndicator ( packet ); - - uint32_t header_flags; - TcprosParserState ret = readPublicationHeader( client_proc, &header_flags ); - if( ret != TCPROS_PARSER_DONE ) - return ret; - - if( TCPROS_PUBLICATION_HEADER_FLAGS != ( header_flags&TCPROS_PUBLICATION_HEADER_FLAGS) ) - { - PRINT_ERROR("cRosMessageParsePublicationHeader() : Missing fields\n"); - ret = TCPROS_PARSER_ERROR; - } - else - { - int subscriber_found = 0; - int i = 0; - for( i = 0 ; i < n->n_subs; i++) - { - SubscriberNode *sub = &n->subs[i]; - if (sub->topic_name == NULL) - continue; - - if( strcmp(sub->topic_type, dynStringGetData(&(client_proc->type))) == 0 && - strcmp(sub->md5sum, dynStringGetData(&(client_proc->md5sum))) == 0) - { - subscriber_found = 1; - break; - } - } - - if( ! subscriber_found ) - { - PRINT_ERROR("cRosMessageParsePublicationHeader() : Wrong topic, type or md5sum\n"); - ret = TCPROS_PARSER_ERROR; - } - else - { - if(client_proc->tcp_nodelay) - tcpIpSocketSetNoDelay(&client_proc->socket); // Not necessary really because subscribers do not write massage packets (only read) - } - } - - /* Restore position indicator */ - dynBufferSetPoseIndicator ( packet, initial_pos_idx ); - - return ret; -} - -void cRosMessagePrepareSubcriptionHeader( CrosNode *n, int client_idx ) -{ - PRINT_VVDEBUG("cRosMessagePrepareSubcriptionHeader()\n"); - - TcprosProcess *client_proc = &(n->tcpros_client_proc[client_idx]); - int sub_idx = client_proc->topic_idx; - DynBuffer *packet = &(client_proc->packet); - uint32_t header_len = 0, header_out_len = 0; - dynBufferPushBackUInt32( packet, header_out_len ); - - header_len += pushBackField( packet, &TCPROS_MESSAGE_DEFINITION_TAG, n->subs[sub_idx].message_definition ); - header_len += pushBackField( packet, &TCPROS_CALLERID_TAG, n->name ); - header_len += pushBackField( packet, &TCPROS_TOPIC_TAG, n->subs[sub_idx].topic_name ); - header_len += pushBackField( packet, &TCPROS_MD5SUM_TAG, n->subs[sub_idx].md5sum ); - header_len += pushBackField( packet, &TCPROS_TYPE_TAG, n->subs[sub_idx].topic_type ); - if(n->subs[sub_idx].tcp_nodelay) - header_len += pushBackField( packet, &TCPROS_TCP_NODELAY_TAG, "1" ); - - header_out_len= HOST_TO_ROS_UINT32( header_len ); - uint32_t *header_len_p = (uint32_t *)dynBufferGetData( packet ); - *header_len_p = header_out_len; -} - -cRosErrCodePack cRosMessageParsePublicationPacket( CrosNode *n, int client_idx ) -{ - cRosErrCodePack ret_err; - SubscriberNode *sub_node; - TcprosProcess *client_proc; - DynBuffer *packet; - void *data_context; - - client_proc = &(n->tcpros_client_proc[client_idx]); - packet = &(client_proc->packet); - sub_node = &n->subs[client_proc->topic_idx]; - data_context = sub_node->context; - - if(cRosMessageQueueVacancies(&sub_node->msg_queue) == 0) - sub_node->msg_queue_overflow = 1; // No space in the queue for the new message - - ret_err = cRosNodeDeserializeIncomingPacket(packet, data_context); - if(ret_err == CROS_SUCCESS_ERR_PACK) - ret_err = cRosNodeSubscriberCallback(data_context); // Calls the subscriber application-defined callback - else - cRosPrintErrCodePack(ret_err, "cRosNodeSubscriberCallback() failed decoding the received packet"); - - return ret_err; -} - -void cRosMessagePreparePublicationHeader( CrosNode *n, int server_idx ) -{ - PRINT_VVDEBUG("cRosMessagePreparePublicationHeader()\n"); - - TcprosProcess *server_proc = &(n->tcpros_server_proc[server_idx]); - int pub_idx = server_proc->topic_idx; - DynBuffer *packet = &(server_proc->packet); - uint32_t header_len = 0, header_out_len = 0; - dynBufferPushBackUInt32( packet, header_out_len ); - - // http://wiki.ros.org/ROS/TCPROS doesn't mention to send message_definition and topic_name - // but they are sent anyway in ros groovy - header_len += pushBackField( packet, &TCPROS_MESSAGE_DEFINITION_TAG, n->pubs[pub_idx].message_definition ); - header_len += pushBackField( packet, &TCPROS_CALLERID_TAG, n->name ); - header_len += pushBackField( packet, &TCPROS_LATCHING_TAG, "1" ); - header_len += pushBackField( packet, &TCPROS_MD5SUM_TAG, n->pubs[pub_idx].md5sum ); - header_len += pushBackField( packet, &TCPROS_TOPIC_TAG, n->pubs[pub_idx].topic_name ); - header_len += pushBackField( packet, &TCPROS_TYPE_TAG, n->pubs[pub_idx].topic_type ); - header_len += pushBackField( packet, &TCPROS_TCP_NODELAY_TAG, (server_proc->tcp_nodelay)?"1":"0" ); - - header_out_len = HOST_TO_ROS_UINT32( header_len ); - uint32_t *header_len_p = (uint32_t *)dynBufferGetData( packet ); - *header_len_p = header_out_len; -} - -cRosErrCodePack cRosMessagePreparePublicationPacket( CrosNode *node, int server_idx ) -{ - cRosErrCodePack ret_err; - PublisherNode *pub_node; - TcprosProcess *server_proc; - int pub_idx; - DynBuffer *packet; - uint32_t *packet_data_size_ptr; - uint32_t packet_size; - PRINT_VVDEBUG("cRosMessagePreparePublicationPacket()\n"); - - server_proc = &(node->tcpros_server_proc[server_idx]); - pub_idx = server_proc->topic_idx; - packet = &(server_proc->packet); - dynBufferPushBackUInt32( packet, 0 ); // Placeholder for packet size - - pub_node = &node->pubs[pub_idx]; - - ret_err = cRosNodeSerializeOutgoingMessage(packet, pub_node->context); - - packet_size = (uint32_t)dynBufferGetSize(packet) - sizeof(uint32_t); - packet_data_size_ptr = (uint32_t *)dynBufferGetData(packet); - *packet_data_size_ptr = packet_size; - - return ret_err; -} - -static TcprosParserState readServiceCallHeader( TcprosProcess *p, uint32_t *flags ) -{ - PRINT_VVDEBUG("readServiceCallHeader()\n"); - DynBuffer *packet = &(p->packet); - size_t bytes_to_read = dynBufferGetSize( packet ); - - *flags = 0x0; - - PRINT_VDEBUG("readServiceCallHeader() : Header len=%lu\n", (long unsigned)bytes_to_read); - - while ( bytes_to_read > 0) - { - uint32_t field_len = getLen( packet ); - - PRINT_VDEBUG("readServiceCallHeader() : Field len=%d\n",field_len); - - const char *field = (const char *)dynBufferGetCurrentData( packet ); - - if( field_len ) - { - if ( field_len > (uint32_t)TCPROS_CALLERID_TAG.dim && - strncmp ( field, TCPROS_CALLERID_TAG.str, TCPROS_CALLERID_TAG.dim ) == 0 ) - { - field += TCPROS_CALLERID_TAG.dim; - - dynStringReplaceWithStrN( &(p->caller_id), field, - field_len - TCPROS_CALLERID_TAG.dim ); - *flags |= TCPROS_CALLER_ID_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TYPE_TAG.dim && - strncmp ( field, TCPROS_TYPE_TAG.str, TCPROS_TYPE_TAG.dim ) == 0 ) - { - field += TCPROS_TYPE_TAG.dim; - - dynStringReplaceWithStrN( &(p->type), field, - field_len - TCPROS_TYPE_TAG.dim ); - *flags |= TCPROS_TYPE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len >= (uint32_t)TCPROS_EMPTY_MD5SUM_TAG.dim && - strncmp ( field, TCPROS_EMPTY_MD5SUM_TAG.str, TCPROS_EMPTY_MD5SUM_TAG.dim ) == 0 ) - { - *flags |= TCPROS_EMPTY_MD5SUM_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_MD5SUM_TAG.dim && - strncmp ( field, TCPROS_MD5SUM_TAG.str, TCPROS_MD5SUM_TAG.dim ) == 0 ) - { - field += TCPROS_MD5SUM_TAG.dim; - - dynStringReplaceWithStrN( &(p->md5sum), field, - field_len - TCPROS_MD5SUM_TAG.dim ); - *flags |= TCPROS_MD5SUM_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_SERVICE_TAG.dim && - strncmp ( field, TCPROS_SERVICE_TAG.str, TCPROS_SERVICE_TAG.dim ) == 0 ) - { - field += TCPROS_SERVICE_TAG.dim; - - dynStringReplaceWithStrN( &(p->service), field, - field_len - TCPROS_SERVICE_TAG.dim ); - *flags |= TCPROS_SERVICE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_PERSISTENT_TAG.dim && - strncmp ( field, TCPROS_PERSISTENT_TAG.str, TCPROS_PERSISTENT_TAG.dim ) == 0 ) - { - //PRINT_INFO("readPublicationHeader() WARNING : TCPROS_PERSISTENT_TAG not implemented\n"); - field += TCPROS_PERSISTENT_TAG.dim; - p->persistent = (*field == '1')?1:0; - *flags |= TCPROS_PERSISTENT_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_PROBE_TAG.dim && - strncmp ( field, TCPROS_PROBE_TAG.str, TCPROS_PROBE_TAG.dim ) == 0 ) - { - //PRINT_INFO("readServiceCallHeader() WARNING : TCPROS_PROBE_TAG implemented only\n"); - field += TCPROS_PROBE_TAG.dim; - p->probe = (*field == '1')?1:0; - *flags |= TCPROS_PROBE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_ERROR_TAG.dim && - strncmp ( field, TCPROS_ERROR_TAG.str, TCPROS_ERROR_TAG.dim ) == 0 ) - { - PRINT_INFO("readServiceCallHeader() WARNING : TCPROS_ERROR_TAG not implemented\n"); - *flags |= TCPROS_ERROR_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_MESSAGE_DEFINITION_TAG.dim && - strncmp ( field, TCPROS_MESSAGE_DEFINITION_TAG.str, TCPROS_MESSAGE_DEFINITION_TAG.dim ) == 0 ) - { - PRINT_INFO("readServiceCallHeader() WARNING : TCPROS_MESSAGE_DEFINITION_TAG not implemented\n"); - *flags |= TCPROS_MESSAGE_DEFINITION_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TCP_NODELAY_TAG.dim && - strncmp ( field, TCPROS_TCP_NODELAY_TAG.str, TCPROS_TCP_NODELAY_TAG.dim ) == 0 ) - { - field += TCPROS_TCP_NODELAY_TAG.dim; - p->tcp_nodelay = (*field == '1')?1:0; - *flags |= TCPROS_TCP_NODELAY_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else - { - PRINT_ERROR("readServiceCallHeader() : unknown field\n"); - *flags = 0x0; - break; - } - } - bytes_to_read -= ( sizeof(uint32_t) + field_len ); - } - - return TCPROS_PARSER_DONE; -} - -TcprosParserState cRosMessageParseServiceCallerHeader( CrosNode *n, int server_idx) -{ - PRINT_VVDEBUG("cRosMessageParseServiceCallerHeader()\n"); - - TcprosProcess *server_proc = &(n->rpcros_server_proc[server_idx]); - DynBuffer *packet = &(server_proc->packet); - - /* Save position indicator: it will be restored */ - int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( packet ); - dynBufferRewindPoseIndicator ( packet ); - - uint32_t header_flags; - TcprosParserState ret = readServiceCallHeader( server_proc, &header_flags ); - if( ret != TCPROS_PARSER_DONE ) - return ret; - - int service_found = 0; - - if( header_flags == ( header_flags & TCPROS_SERVICECALL_HEADER_FLAGS) || header_flags == ( header_flags & TCPROS_SERVICECALL_MATLAB_HEADER_FLAGS) ) - { - int svc_name_match = 0; - int i = 0; - for( i = 0 ; i < n->n_service_providers; i++) - { - if( strcmp( n->service_providers[i].service_name, dynStringGetData(&(server_proc->service))) == 0) - { - svc_name_match = 1; - if(strcmp( n->service_providers[i].md5sum, dynStringGetData(&(server_proc->md5sum))) == 0) - { - service_found = 1; - server_proc->service_idx = i; - break; - } - } - } - if( ! service_found ) - { - if(svc_name_match == 0) - PRINT_ERROR("cRosMessageParseServiceCallerHeader() : Received a service call header specifying a unknown service name\n"); - else - PRINT_ERROR("cRosMessageParseServiceCallerHeader() : Received a service call header specifying a known service name with a wrong MD5 sum\n"); - } - } - else if( header_flags == ( header_flags & TCPROS_SERVICEPROBE_HEADER_FLAGS) || header_flags == ( header_flags & TCPROS_SERVICEPROBE_MATLAB_HEADER_FLAGS) ) - { - int i = 0; - for( i = 0 ; i < n->n_service_providers; i++) - { - if( strcmp( n->service_providers[i].service_name, dynStringGetData(&(server_proc->service))) == 0) - { - service_found = 1; - server_proc->service_idx = i; - break; - } - } - if( ! service_found ) - PRINT_ERROR("cRosMessageParseServiceCallerHeader() : Received a service probe header specifying a unknown service name\n"); - } - else - { - PRINT_ERROR("cRosMessageParseServiceCallerHeader() : Received a service call header missing fields\n"); - ret = TCPROS_PARSER_ERROR; - } - - if( ! service_found ) - { - server_proc->service_idx = -1; - ret = TCPROS_PARSER_ERROR; - } - else - { - if(server_proc->tcp_nodelay) - tcpIpSocketSetNoDelay(&server_proc->socket); - } - - /* Restore position indicator */ - dynBufferSetPoseIndicator ( packet, initial_pos_idx ); - - return ret; -} - -static TcprosParserState readServiceProvisionHeader( TcprosProcess *p, uint32_t *flags ) -{ - PRINT_VVDEBUG("readServiceProvisionHeader()\n"); - DynBuffer *packet = &(p->packet); - size_t bytes_to_read = dynBufferGetSize( packet ); - - *flags = 0x0; - - PRINT_VDEBUG("readServiceProvisionHeader() : Header len=%lu\n", (long unsigned)bytes_to_read); - - while ( bytes_to_read > 0) - { - uint32_t field_len = getLen( packet ); - - PRINT_VDEBUG("readServiceProvisionHeader() : Field len=%d\n",field_len); - - const char *field = (const char *)dynBufferGetCurrentData( packet ); - - if( field_len ) - { - if ( field_len > (uint32_t)TCPROS_CALLERID_TAG.dim && - strncmp ( field, TCPROS_CALLERID_TAG.str, TCPROS_CALLERID_TAG.dim ) == 0 ) - { - field += TCPROS_CALLERID_TAG.dim; - - dynStringReplaceWithStrN( &(p->caller_id), field, - field_len - TCPROS_CALLERID_TAG.dim ); - *flags |= TCPROS_CALLER_ID_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TYPE_TAG.dim && - strncmp ( field, TCPROS_TYPE_TAG.str, TCPROS_TYPE_TAG.dim ) == 0 ) - { - field += TCPROS_TYPE_TAG.dim; - - dynStringReplaceWithStrN( &(p->type), field, - field_len - TCPROS_TYPE_TAG.dim ); - *flags |= TCPROS_TYPE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len >= (uint32_t)TCPROS_EMPTY_MD5SUM_TAG.dim && - strncmp ( field, TCPROS_EMPTY_MD5SUM_TAG.str, TCPROS_EMPTY_MD5SUM_TAG.dim ) == 0 ) - { - *flags |= TCPROS_EMPTY_MD5SUM_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_MD5SUM_TAG.dim && - strncmp ( field, TCPROS_MD5SUM_TAG.str, TCPROS_MD5SUM_TAG.dim ) == 0 ) - { - field += TCPROS_MD5SUM_TAG.dim; - - dynStringReplaceWithStrN( &(p->md5sum), field, - field_len - TCPROS_MD5SUM_TAG.dim ); - *flags |= TCPROS_MD5SUM_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_SERVICE_TAG.dim && - strncmp ( field, TCPROS_SERVICE_TAG.str, TCPROS_SERVICE_TAG.dim ) == 0 ) - { - field += TCPROS_SERVICE_TAG.dim; - - dynStringReplaceWithStrN( &(p->service), field, field_len - TCPROS_SERVICE_TAG.dim ); - *flags |= TCPROS_SERVICE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_SERVICE_REQUESTTYPE_TAG.dim && - strncmp ( field, TCPROS_SERVICE_REQUESTTYPE_TAG.str, TCPROS_SERVICE_REQUESTTYPE_TAG.dim ) == 0 ) - { - field += TCPROS_SERVICE_REQUESTTYPE_TAG.dim; - dynStringReplaceWithStrN( &(p->servicerequest_type), field, field_len - TCPROS_SERVICE_REQUESTTYPE_TAG.dim ); - *flags |= TCPROS_SERVICE_REQUESTTYPE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_SERVICE_RESPONSETYPE_TAG.dim && - strncmp ( field, TCPROS_SERVICE_RESPONSETYPE_TAG.str, TCPROS_SERVICE_RESPONSETYPE_TAG.dim ) == 0 ) - { - field += TCPROS_SERVICE_RESPONSETYPE_TAG.dim; - dynStringReplaceWithStrN( &(p->serviceresponse_type), field, field_len - TCPROS_SERVICE_RESPONSETYPE_TAG.dim ); - *flags |= TCPROS_SERVICE_RESPONSETYPE_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_ERROR_TAG.dim && - strncmp ( field, TCPROS_ERROR_TAG.str, TCPROS_ERROR_TAG.dim ) == 0 ) - { - PRINT_INFO("readServiceProvisionHeader() WARNING : TCPROS_ERROR_TAG not implemented\n"); - *flags |= TCPROS_ERROR_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_MESSAGE_DEFINITION_TAG.dim && - strncmp ( field, TCPROS_MESSAGE_DEFINITION_TAG.str, TCPROS_MESSAGE_DEFINITION_TAG.dim ) == 0 ) - { - PRINT_INFO("readServiceProvisionHeader() WARNING : TCPROS_MESSAGE_DEFINITION_TAG not implemented\n"); - *flags |= TCPROS_MESSAGE_DEFINITION_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else if ( field_len > (uint32_t)TCPROS_TCP_NODELAY_TAG.dim && - strncmp ( field, TCPROS_TCP_NODELAY_TAG.str, TCPROS_TCP_NODELAY_TAG.dim ) == 0 ) - { - field += TCPROS_TCP_NODELAY_TAG.dim; - p->tcp_nodelay = (*field == '1')?1:0; - *flags |= TCPROS_TCP_NODELAY_FLAG; - dynBufferMovePoseIndicator( packet, field_len ); - } - else - { - PRINT_ERROR("readServiceProvisionHeader() : unknown field\n"); - *flags = 0x0; - break; - } - } - bytes_to_read -= ( sizeof(uint32_t) + field_len ); - } - - return TCPROS_PARSER_DONE; -} - -TcprosParserState cRosMessageParseServiceProviderHeader( CrosNode *n, int client_idx ) -{ - PRINT_VVDEBUG("cRosMessageParseServiceProviderHeader()\n"); - - TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); - DynBuffer *packet = &(client_proc->packet); - - /* Save position indicator: it will be restored */ - int initial_pos_idx = dynBufferGetPoseIndicatorOffset ( packet ); - dynBufferRewindPoseIndicator ( packet ); - - uint32_t header_flags; - TcprosParserState ret = readServiceProvisionHeader( client_proc, &header_flags ); - if( ret != TCPROS_PARSER_DONE ) - return ret; - - if( TCPROS_SERVICEPROVISION_HEADER_FLAGS != ( header_flags&TCPROS_SERVICEPROVISION_HEADER_FLAGS) ) - { - PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Missing fields\n"); - ret = TCPROS_PARSER_ERROR; - } - else - { - ServiceCallerNode *svc_caller = &n->service_callers[client_proc->service_idx]; - - if (header_flags&TCPROS_SERVICE_FLAG && strcmp(svc_caller->service_name, dynStringGetData(&(client_proc->service))) != 0) - { - PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong service name from service provider\n"); - ret = TCPROS_PARSER_ERROR; - } - if(strcmp(svc_caller->md5sum, dynStringGetData(&(client_proc->md5sum))) != 0) - { - PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong MD5 sum from service provider\n"); - ret = TCPROS_PARSER_ERROR; - } - if(strcmp(svc_caller->service_type, dynStringGetData(&(client_proc->type))) != 0) - { - PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong service type from service provider\n"); - ret = TCPROS_PARSER_ERROR; - } - if(header_flags&TCPROS_SERVICE_REQUESTTYPE_FLAG && strcmp(svc_caller->servicerequest_type, dynStringGetData(&(client_proc->servicerequest_type))) != 0) - { - PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong service request type from service provider\n"); - ret = TCPROS_PARSER_ERROR; - } - if(header_flags&TCPROS_SERVICE_RESPONSETYPE_FLAG && strcmp(svc_caller->serviceresponse_type, dynStringGetData(&(client_proc->serviceresponse_type))) != 0) - { - PRINT_ERROR("cRosMessageParseServiceProviderHeader() : Wrong service response type from service provider\n"); - ret = TCPROS_PARSER_ERROR; - } - if(client_proc->tcp_nodelay) - tcpIpSocketSetNoDelay(&client_proc->socket); - } - - /* Restore position indicator */ - dynBufferSetPoseIndicator ( packet, initial_pos_idx ); - - return ret; -} - -void cRosMessagePrepareServiceCallHeader(CrosNode *n, int client_idx) -{ - PRINT_VVDEBUG("cRosMessagePrepareServiceCallHeader()\n"); - - TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); - int srv_idx = client_proc->service_idx; - DynBuffer *packet = &(client_proc->packet); - uint32_t header_len = 0, header_out_len = 0; - dynBufferPushBackUInt32( packet, header_out_len ); - - // Same format as MATLAB second header (not the probe one) - header_len += pushBackField( packet, &TCPROS_SERVICE_TAG, n->service_callers[srv_idx].service_name ); - header_len += pushBackField( packet, &TCPROS_MESSAGE_DEFINITION_TAG, n->service_callers[srv_idx].message_definition ); - header_len += pushBackField( packet, &TCPROS_CALLERID_TAG, n->name ); - header_len += pushBackField( packet, &TCPROS_MD5SUM_TAG, n->service_callers[srv_idx].md5sum ); - if(client_proc->persistent) - header_len += pushBackField( packet, &TCPROS_PERSISTENT_TAG, "1" ); - if(client_proc->tcp_nodelay) - header_len += pushBackField( packet, &TCPROS_TCP_NODELAY_TAG, "1" ); - - //header_len += pushBackField( packet, &TCPROS_SERVICE_REQUESTTYPE_TAG, n->service_callers[srv_idx].servicerequest_type ); - //header_len += pushBackField( packet, &TCPROS_SERVICE_RESPONSETYPE_TAG, n->service_callers[srv_idx].serviceresponse_type ); - header_len += pushBackField( packet, &TCPROS_TYPE_TAG, n->service_callers[srv_idx].service_type ); - - header_out_len = HOST_TO_ROS_UINT32( header_len ); - uint32_t *header_len_p = (uint32_t *)dynBufferGetData( packet ); - *header_len_p = header_out_len; -} - -cRosErrCodePack cRosMessagePrepareServiceCallPacket( CrosNode *n, int client_idx ) -{ - cRosErrCodePack ret_err; - - PRINT_VVDEBUG("cRosMessagePrepareServiceCallPacket()\n"); - TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); - int svc_idx = client_proc->service_idx; - DynBuffer *packet = &(client_proc->packet); - dynBufferPushBackUInt32( packet, 0 ); // Placehoder for packet size - - void* data_context = n->service_callers[svc_idx].context; - ret_err = cRosNodeSerializeOutgoingMessage(packet, data_context); // Serialize the call outgoing message into the outgoing packet - - uint32_t data_size = (uint32_t)dynBufferGetSize(packet) - sizeof(uint32_t); - uint32_t *packet_data_size_ptr = (uint32_t *)dynBufferGetData(packet); - *packet_data_size_ptr = data_size; - - return ret_err; -} - -cRosErrCodePack cRosMessageParseServiceResponsePacket( CrosNode *n, int client_idx ) -{ - cRosErrCodePack ret_err; - - TcprosProcess *client_proc = &(n->rpcros_client_proc[client_idx]); - DynBuffer *packet = &(client_proc->packet); - if(client_proc->ok_byte == TCPROS_OK_BYTE_SUCCESS) - { - int svc_idx = client_proc->service_idx; - void* data_context = n->service_callers[svc_idx].context; - - ret_err = cRosNodeDeserializeIncomingPacket(packet, data_context); // Deserialize the message response - - if(ret_err == CROS_SUCCESS_ERR_PACK) - ret_err = cRosNodeServiceCallerCallback(1, data_context); // Call the service-caller application-defined callback function to process the service response - } - else - { - size_t n_char; - PRINT_ERROR("cRosMessageParseServiceResponsePacket() : Error in service call response. 'ok' byte=%i. Error message='",client_proc->ok_byte); - for(n_char=0;n_charrpcros_server_proc[server_idx]); - int srv_idx = server_proc->service_idx; - DynBuffer *packet = &(server_proc->packet); - uint32_t header_len = 0, header_out_len = 0; - dynBufferPushBackUInt32( packet, header_out_len ); - - // http://wiki.ros.org/ROS/TCPROS doesn't mention to send message_definition and topic_name - // but they are sent anyway in ros groovy - header_len += pushBackField( packet, &TCPROS_CALLERID_TAG, n->name ); - header_len += pushBackField( packet, &TCPROS_MD5SUM_TAG, n->service_providers[srv_idx].md5sum ); - - //if(server_proc->probe) - //{ - header_len += pushBackField( packet, &TCPROS_SERVICE_REQUESTTYPE_TAG, n->service_providers[srv_idx].servicerequest_type ); - header_len += pushBackField( packet, &TCPROS_SERVICE_RESPONSETYPE_TAG, n->service_providers[srv_idx].serviceresponse_type ); - header_len += pushBackField( packet, &TCPROS_TYPE_TAG, n->service_providers[srv_idx].service_type ); - //} - - header_out_len = HOST_TO_ROS_UINT32( header_len ); - uint32_t *header_len_p = (uint32_t *)dynBufferGetData( packet ); - *header_len_p = header_out_len; -} - -cRosErrCodePack cRosMessagePrepareServiceResponsePacket( CrosNode *n, int server_idx) -{ - cRosErrCodePack ret_err; - uint8_t ok_byte; // OK field (byte size) of the service response packet - - PRINT_VVDEBUG("cRosMessageParseServiceArgumentsPacket()\n"); - TcprosProcess *server_proc = &(n->rpcros_server_proc[server_idx]); - DynBuffer *packet = &(server_proc->packet); - int srv_idx = server_proc->service_idx; - void* service_context = n->service_providers[srv_idx].context; - DynBuffer service_response; - dynBufferInit(&service_response); - - ret_err = cRosNodeDeserializeIncomingPacket(packet, service_context); // prepare the context incoming message used by the user callback function - if(ret_err == CROS_SUCCESS_ERR_PACK) - ret_err = cRosNodeServiceProviderCallback(service_context); // calls the service-provider application-defined callback function - else - cRosPrintErrCodePack(ret_err, "cRosMessagePrepareServiceResponsePacket() failed decoding the received packet"); - - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - ret_err = cRosNodeSerializeOutgoingMessage(&service_response, service_context); // Create the output packet from outgoing message in the context - if(ret_err != CROS_SUCCESS_ERR_PACK) - cRosPrintErrCodePack(ret_err, "cRosNodeServiceProviderCallback() failed encoding the packet to send"); - } - - dynBufferClear(packet); // clear packet buffer - - if(ret_err == CROS_SUCCESS_ERR_PACK) - { - ok_byte = TCPROS_OK_BYTE_SUCCESS; - dynBufferPushBackBuf( packet, &ok_byte, sizeof(uint8_t) ); - dynBufferPushBackUInt32( packet, dynBufferGetSize(&service_response)); // data size field - dynBufferPushBackBuf( packet, dynBufferGetData(&service_response), dynBufferGetSize(&service_response)); // Response data - } - else - { - ok_byte = TCPROS_OK_BYTE_FAIL; - dynBufferPushBackBuf( packet, &ok_byte, sizeof(uint8_t) ); - dynBufferPushBackUInt32( packet, 0); // Serialize an error string of size 0: Just add the data size field - } - - dynBufferRelease(&service_response); - - return ret_err; -} diff --git a/src/hal/components/cros/src/dyn_buffer.c b/src/hal/components/cros/src/dyn_buffer.c deleted file mode 100644 index 8acad319..00000000 --- a/src/hal/components/cros/src/dyn_buffer.c +++ /dev/null @@ -1,249 +0,0 @@ -#include -#include - -#include "dyn_buffer.h" -#include "cros_defs.h" -#include "cros_log.h" - -enum { DYNBUFFER_INIT_SIZE = 256, DYNBUFFER_GROW_RATE = 2 }; - -void dynBufferInit ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferInit()\n" ); - - d_buf->data = NULL; - d_buf->size = 0; - d_buf->pos_offset = 0; - d_buf->max = 0; -} - -void dynBufferRelease ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferRelease()\n" ); - - if ( d_buf->data != NULL ) - free ( d_buf->data ); - - d_buf->size = 0; - d_buf->pos_offset = 0; - d_buf->max = 0; -} - -int dynBufferPushBackBuf ( DynBuffer *d_buf, const unsigned char *new_buf, size_t n ) -{ - PRINT_VVDEBUG ( "dynBufferPushBackBuf()\n" ); - - if (new_buf == NULL && n > 0) // If n == 0, the function accepts NULL as new_buf since nothing have to be appended - { - PRINT_ERROR ( "dynBufferPushBackBuf() : Invalid function argument values: new buffer content must be different from NULL and no shorter than 0\n" ); - return -1; - } - - if ( d_buf->data == NULL ) - { - PRINT_VVDEBUG ( "dynBufferPushBackBuf() : allocate memory for the first time\n" ); - d_buf->data = ( unsigned char * ) malloc ( DYNBUFFER_INIT_SIZE * sizeof ( unsigned char ) ); - - if ( d_buf->data == NULL ) - { - PRINT_ERROR ( "dynBufferPushBackBuf() : Can't allocate memory\n" ); - return -1; - } - - d_buf->size = 0; - d_buf->max = DYNBUFFER_INIT_SIZE; - } - - while ( d_buf->size + n > d_buf->max ) - { - PRINT_VVDEBUG ( "dynBufferPushBackBuf() : reallocate memory\n" ); - unsigned char *new_d_buf = ( unsigned char * ) realloc ( d_buf->data, ( DYNBUFFER_GROW_RATE * d_buf->max ) * - sizeof ( unsigned char ) ); - if ( new_d_buf == NULL ) - { - PRINT_ERROR ( "dynBufferPushBackBuf() : Can't allocate more memory\n" ); - return -1; - } - d_buf->max *= DYNBUFFER_GROW_RATE; - d_buf->data = new_d_buf; - } - - if(n>0) - { - memcpy ( ( void * ) ( d_buf->data + d_buf->size ), ( void * ) new_buf, n ); - d_buf->size += n; - } - - return d_buf->size; -} - -int dynBufferReplaceContent( DynBuffer *d_buf, const unsigned char *cont_buf, size_t cont_buf_len ) -{ - size_t ret_val; - - d_buf->size = d_buf->pos_offset; // Discard content beyond current position index (it will be replaced by cont_buf) - ret_val = dynBufferPushBackBuf(d_buf, cont_buf, cont_buf_len); - - return ret_val; -} - -int dynBufferPushBackInt8( DynBuffer *d_buf, int8_t val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( int8_t ) ); -} - -int dynBufferPushBackInt16( DynBuffer *d_buf, int16_t val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( int16_t ) ); -} - -int dynBufferPushBackInt32 ( DynBuffer *d_buf, int32_t val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( int32_t ) ); -} - -int dynBufferPushBackInt64( DynBuffer *d_buf, int64_t val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( int64_t ) ); -} - -int dynBufferPushBackUInt8( DynBuffer *d_buf, uint8_t val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( uint8_t ) ); -} - -int dynBufferPushBackUInt16( DynBuffer *d_buf, uint16_t val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( uint16_t ) ); -} - -int dynBufferPushBackUInt32 ( DynBuffer *d_buf, uint32_t val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( uint32_t ) ); -} - -int dynBufferPushBackUInt64( DynBuffer *d_buf, uint64_t val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( uint64_t ) ); -} - -int dynBufferPushBackFloat32( DynBuffer *d_buf, float val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( float ) ); -} - -int dynBufferPushBackFloat64( DynBuffer *d_buf, double val ) -{ - PRINT_VVDEBUG ( "dynBufferPushBack()\n" ); - - return dynBufferPushBackBuf ( d_buf, ( unsigned char * ) ( &val ), sizeof ( double ) ); -} - -void dynBufferClear ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferClear()\n" ); - - if ( d_buf->data == NULL ) - return; - - d_buf->size = 0; - d_buf->pos_offset = 0; -} - -size_t dynBufferGetSize ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferGetSize()\n" ); - - return d_buf->size; -} - -const unsigned char *dynBufferGetData ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferGetData()\n" ); - - return ( const unsigned char * ) d_buf->data; -} - -void dynBufferMovePoseIndicator ( DynBuffer *d_buf, int offset ) -{ - PRINT_VVDEBUG ( "dynBufferMovePoseIndicator()\n" ); - - size_t curr = d_buf->pos_offset; - if (offset > 0 && curr >= (d_buf->size - offset)) - d_buf->pos_offset = d_buf->size; - else if ( offset < 0 && (size_t)(-offset) >= curr) - d_buf->pos_offset = 0; - else - d_buf->pos_offset += (size_t)offset; -} - -void dynBufferSetPoseIndicator( DynBuffer *d_buf, size_t pos ) -{ - PRINT_VVDEBUG ( "dynBufferSetPoseIndicator()\n" ); - - if ( pos > d_buf->size ) - d_buf->pos_offset = d_buf->size; - else - d_buf->pos_offset = pos; -} - -void dynBufferRewindPoseIndicator ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferRewindPoseIndicator()\n" ); - - d_buf->pos_offset = 0; -} - -size_t dynBufferGetPoseIndicatorOffset ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferGetPoseIndicatorOffset()\n" ); - - return d_buf->pos_offset; -} - -const unsigned char *dynBufferGetCurrentData ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferGetCurrentData()\n" ); - - return ( const unsigned char * ) ( d_buf->data + d_buf->pos_offset ); -} - -int dynBufferGetCurrentContent ( unsigned char *cont_buf, DynBuffer *d_buf, size_t cont_buf_len ) -{ - int ret_err; - - if( d_buf->pos_offset + cont_buf_len > d_buf->size ) // There is not enough content in the dynamic buffer - ret_err=-1; - else - { - memcpy(cont_buf, d_buf->data + d_buf->pos_offset, cont_buf_len); - ret_err=0; - } - - return ret_err; -} - -size_t dynBufferGetRemainingDataSize ( DynBuffer *d_buf ) -{ - PRINT_VVDEBUG ( "dynBufferGetRemainingDataSize()\n" ); - - return d_buf->size - d_buf->pos_offset; -} diff --git a/src/hal/components/cros/src/dyn_string.c b/src/hal/components/cros/src/dyn_string.c deleted file mode 100644 index 5c3aad1c..00000000 --- a/src/hal/components/cros/src/dyn_string.c +++ /dev/null @@ -1,286 +0,0 @@ -#include -#include - -#include "dyn_string.h" -#include "cros_defs.h" -#include "cros_log.h" - -enum { DYNSTRING_INIT_SIZE = 256, DYNSTRING_GROW_RATE = 2 }; - -void dynStringInit ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "dynStringInit()\n" ); - - d_str->data = NULL; - d_str->len = 0; - d_str->pos_offset = 0; - d_str->max = 0; -} - -void dynStringRelease ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "dynStringRelease()\n" ); - - if ( d_str->data != NULL ) - { - free ( d_str->data ); - d_str->data = NULL; - } - d_str->len = 0; - d_str->pos_offset = 0; - d_str->max = 0; -} - -int dynStringPushBackStr ( DynString *d_str, const char *new_str ) -{ - PRINT_VVDEBUG ( "dynStringPushBackStr()\n" ); - - return dynStringPushBackStrN ( d_str, new_str, strlen ( new_str ) ); -} - -int dynStringPushBackStrN ( DynString *d_str, const char *new_str, int n ) -{ - PRINT_VVDEBUG ( "dynStringPushBackStrN()\n" ); - - if ( new_str == NULL ) - { - PRINT_ERROR ( "dynStringPushBackStrN() : Invalid new string\n" ); - return -1; - } - - if ( d_str->data == NULL ) - { - PRINT_VVDEBUG ( "dynStringPushBackStrN() : allocate memory for the first time\n" ); - d_str->data = ( char * ) malloc ( (DYNSTRING_INIT_SIZE + n + 1) * sizeof ( char ) ); // Reserve one char at the end of the array (+1) to store the \0 terminating char - - if ( d_str->data == NULL ) - { - PRINT_ERROR ( "dynStringPushBackStrN() : Can't allocate memory\n" ); - return -1; - } - - d_str->len = 0; - d_str->max = DYNSTRING_INIT_SIZE + n; - } - else - if(d_str->len + n > d_str->max) - { - PRINT_VVDEBUG ( "dynStringPushBackStrN() : reallocate memory\n" ); - char *n_d_str = ( char * ) realloc ( d_str->data, ( DYNSTRING_GROW_RATE * d_str->max + n + 1) * sizeof ( char ) ); - if ( n_d_str == NULL ) - { - PRINT_ERROR ( "dynStringPushBackStrN() : Can't reallocate more memory\n" ); - return -1; - } - d_str->max = DYNSTRING_GROW_RATE * d_str->max + n; - d_str->data = n_d_str; - } - - memcpy ( ( void * ) ( d_str->data + d_str->len ), ( void * ) new_str, ( size_t ) n ); - d_str->len += n; - d_str->data[d_str->len] = '\0'; - - return d_str->len; -} - -int dynStringReplaceWithStrN ( DynString *d_str, const char *new_str, int n ) -{ - int new_len; - - d_str->len=0; // Clear old content - d_str->pos_offset=0; - new_len = dynStringPushBackStrN ( d_str, new_str, n ); - - return new_len; -} - -int dynStringPushBackChar ( DynString *d_str, const char c ) -{ - PRINT_VVDEBUG ( "dynStringPushBackChar()\n" ); - - if ( d_str->data == NULL ) - { - PRINT_VVDEBUG ( "dynStringPushBackChar() : allocate memory for the first time\n" ); - d_str->data = ( char * ) malloc ( (DYNSTRING_INIT_SIZE + 2) * sizeof ( char ) ); - - if ( d_str->data == NULL ) - { - PRINT_ERROR ( "dynStringPushBackChar() : Can't allocate memory\n" ); - return -1; - } - - d_str->data[0] = '\0'; - d_str->len = 0; - d_str->max = DYNSTRING_INIT_SIZE + 1; - } - else - if ( d_str->len + 1 > d_str->max ) // If there is not enogh space in the array to store a new char, allocate more meory - { - PRINT_VVDEBUG ( "dynStringPushBackChar() : reallocate memory\n" ); - char *n_d_str = ( char * ) realloc ( d_str->data, ( DYNSTRING_GROW_RATE * d_str->max + 2) * - sizeof ( char ) ); - if ( n_d_str == NULL ) - { - PRINT_ERROR ( "dynStringPushBackChar() : Can't allocate more memory\n" ); - return -1; - } - d_str->max = DYNSTRING_GROW_RATE * d_str->max + 1; - d_str->data = n_d_str; - } - - d_str->data[d_str->len] = c; - d_str->len += 1; - d_str->data[d_str->len] = '\0'; - - return d_str->len; -} - -int dynStringPatch ( DynString *d_str, const char *new_str, int pos ) -{ - PRINT_VVDEBUG ( "dynStringPatch()\n" ); - - if ( new_str == NULL ) - { - PRINT_ERROR ( "dynStringPatch() : Invalid new string\n" ); - return -1; - } - - if ( d_str->data == NULL ) - { - PRINT_ERROR ( "dynStringPatch() : Empty dynamic string (NULL data)\n" ); - return -1; - } - - if ( pos > d_str->len ) - { - PRINT_ERROR ( "dynStringPatch() : The starting position for the copy must be smaller \ - or equal to the current dynamic string lenght\n" ); - return -1; - } - - int new_str_len = strlen ( new_str ); - int str_final_len = pos + new_str_len; - - if ( str_final_len > d_str->max ) - { - PRINT_VVDEBUG ( "dynStringPatch() : reallocate memory\n" ); - char *n_d_str = ( char * ) realloc ( d_str->data, ( DYNSTRING_GROW_RATE * d_str->max + new_str_len + 1) * - sizeof ( char ) ); - if ( n_d_str == NULL ) - { - PRINT_ERROR ( "dynStringPatch() : Can't allocate more memory\n" ); - return -1; - } - d_str->max = DYNSTRING_GROW_RATE * d_str->max + new_str_len; - d_str->data = n_d_str; - } - - memcpy ( ( void * ) ( d_str->data + pos ), ( void * ) new_str, ( size_t ) new_str_len ); - - if ( str_final_len > d_str->len ) - { - d_str->data[str_final_len] = '\0'; - d_str->len = str_final_len; - } - - return d_str->len; -} - -void dynStringClear ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "dynStringClear()\n" ); - - if ( d_str->data == NULL ) - return; - - d_str->data[0] = '\0'; - d_str->pos_offset = 0; - d_str->len = 0; -} - -void dynStringReduce ( DynString *d_str, int rem_left, int rem_right) -{ - PRINT_VVDEBUG ( "dynStringReduce()\n" ); - - if ( d_str->data == NULL ) - return; - - if (rem_left > d_str->len) - rem_left = d_str->len; - if (rem_left + rem_right > d_str->len) - rem_right = d_str->len - rem_left; - - d_str->len -= rem_left + rem_right; - memmove(d_str->data, d_str->data+rem_left, d_str->len); - d_str->data[d_str->len] = '\0'; - d_str->pos_offset -= rem_left; - if(d_str->pos_offset < 0) - d_str->pos_offset = 0; - else - if(d_str->pos_offset > d_str->len) - d_str->pos_offset = d_str->len; -} - -int dynStringGetLen ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "dynStringGetLen()\n" ); - - return d_str->len; -} - -const char *dynStringGetData ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "getDynStringBuf()\n" ); - - return ( const char * ) d_str->data; -} - -void dynStringMovePoseIndicator ( DynString *d_str, int offset ) -{ - PRINT_VVDEBUG ( "dynStringMovePoseIndicator()\n" ); - - d_str->pos_offset += offset; - if ( d_str->pos_offset < 0 ) - d_str->pos_offset = 0; - if ( d_str->pos_offset > d_str->len ) - d_str->pos_offset = d_str->len; -} - -void dynStringSetPoseIndicator ( DynString *d_str, int pos ) -{ - PRINT_VVDEBUG ( "dynStringSetPoseIndicator()\n" ); - - d_str->pos_offset = pos; - if ( d_str->pos_offset < 0 ) - d_str->pos_offset = 0; - if ( d_str->pos_offset > d_str->len ) - d_str->pos_offset = d_str->len; -} - -void dynStringRewindPoseIndicator ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "dynStringRewindPoseIndicator()\n" ); - - d_str->pos_offset = 0; -} - -int dynStringGetPoseIndicatorOffset ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "dynStringGetPoseIndicatorOffset()\n" ); - - return d_str->pos_offset; -} - -const char *dynStringGetCurrentData ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "dynStringGetCurrentData()\n" ); - - return ( const char * ) ( d_str->data + d_str->pos_offset ); -} - -int dynStringGetRemainingDataSize ( DynString *d_str ) -{ - PRINT_VVDEBUG ( "dynStringGetRemainingDataSize()\n" ); - - return d_str->len - d_str->pos_offset; -} diff --git a/src/hal/components/cros/src/md5.c b/src/hal/components/cros/src/md5.c deleted file mode 100644 index e7bb9a37..00000000 --- a/src/hal/components/cros/src/md5.c +++ /dev/null @@ -1,296 +0,0 @@ -/* - * This is an OpenSSL-compatible implementation of the RSA Data Security, Inc. - * MD5 Message-Digest Algorithm (RFC 1321). - * - * Homepage: - * http://openwall.info/wiki/people/solar/software/public-domain-source-code/md5 - * - * Author: - * Alexander Peslyak, better known as Solar Designer - * - * This software was written by Alexander Peslyak in 2001. No copyright is - * claimed, and the software is hereby placed in the public domain. - * In case this attempt to disclaim copyright and place the software in the - * public domain is deemed null and void, then the software is - * Copyright (c) 2001 Alexander Peslyak and it is hereby released to the - * general public under the following terms: - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted. - * - * There's ABSOLUTELY NO WARRANTY, express or implied. - * - * (This is a heavily cut-down "BSD license".) - * - * This differs from Colin Plumb's older public domain implementation in that - * no exactly 32-bit integer data type is required (any 32-bit or wider - * unsigned integer data type will do), there's no compile-time endianness - * configuration, and the function prototypes match OpenSSL's. No code from - * Colin Plumb's implementation has been reused; this comment merely compares - * the properties of the two independent implementations. - * - * The primary goals of this implementation are portability and ease of use. - * It is meant to be fast, but not as fast as possible. Some known - * optimizations are not included to reduce source code size and avoid - * compile-time configuration. - */ - -#ifndef HAVE_OPENSSL - -#include - -#include "md5.h" - -/* - * The basic MD5 functions. - * - * F and G are optimized compared to their RFC 1321 definitions for - * architectures that lack an AND-NOT instruction, just like in Colin Plumb's - * implementation. - */ -#define F(x, y, z) ((z) ^ ((x) & ((y) ^ (z)))) -#define G(x, y, z) ((y) ^ ((z) & ((x) ^ (y)))) -#define H(x, y, z) (((x) ^ (y)) ^ (z)) -#define H2(x, y, z) ((x) ^ ((y) ^ (z))) -#define I(x, y, z) ((y) ^ ((x) | ~(z))) - -/* - * The MD5 transformation for all four rounds. - */ -#define STEP(f, a, b, c, d, x, t, s) \ - (a) += f((b), (c), (d)) + (x) + (t); \ - (a) = (((a) << (s)) | (((a) & 0xffffffff) >> (32 - (s)))); \ - (a) += (b); - -/* - * SET reads 4 input bytes in little-endian byte order and stores them - * in a properly aligned word in host byte order. - * - * The check for little-endian architectures that tolerate unaligned - * memory accesses is just an optimization. Nothing will break if it - * doesn't work. - */ -#if defined(__i386__) || defined(__x86_64__) || defined(__vax__) -#define SET(n) \ - (*(MD5_u32plus *)&ptr[(n) * 4]) -#define GET(n) \ - SET(n) -#else -#define SET(n) \ - (ctx->block[(n)] = \ - (MD5_u32plus)ptr[(n) * 4] | \ - ((MD5_u32plus)ptr[(n) * 4 + 1] << 8) | \ - ((MD5_u32plus)ptr[(n) * 4 + 2] << 16) | \ - ((MD5_u32plus)ptr[(n) * 4 + 3] << 24)) -#define GET(n) \ - (ctx->block[(n)]) -#endif - -/* - * This processes one or more 64-byte data blocks, but does NOT update - * the bit counters. There are no alignment requirements. - */ -static const void *body(MD5_CTX *ctx, const void *data, unsigned long size) -{ - const unsigned char *ptr; - MD5_u32plus a, b, c, d; - MD5_u32plus saved_a, saved_b, saved_c, saved_d; - - ptr = (const unsigned char *)data; - - a = ctx->a; - b = ctx->b; - c = ctx->c; - d = ctx->d; - - do { - saved_a = a; - saved_b = b; - saved_c = c; - saved_d = d; - -/* Round 1 */ - STEP(F, a, b, c, d, SET(0), 0xd76aa478, 7) - STEP(F, d, a, b, c, SET(1), 0xe8c7b756, 12) - STEP(F, c, d, a, b, SET(2), 0x242070db, 17) - STEP(F, b, c, d, a, SET(3), 0xc1bdceee, 22) - STEP(F, a, b, c, d, SET(4), 0xf57c0faf, 7) - STEP(F, d, a, b, c, SET(5), 0x4787c62a, 12) - STEP(F, c, d, a, b, SET(6), 0xa8304613, 17) - STEP(F, b, c, d, a, SET(7), 0xfd469501, 22) - STEP(F, a, b, c, d, SET(8), 0x698098d8, 7) - STEP(F, d, a, b, c, SET(9), 0x8b44f7af, 12) - STEP(F, c, d, a, b, SET(10), 0xffff5bb1, 17) - STEP(F, b, c, d, a, SET(11), 0x895cd7be, 22) - STEP(F, a, b, c, d, SET(12), 0x6b901122, 7) - STEP(F, d, a, b, c, SET(13), 0xfd987193, 12) - STEP(F, c, d, a, b, SET(14), 0xa679438e, 17) - STEP(F, b, c, d, a, SET(15), 0x49b40821, 22) - -/* Round 2 */ - STEP(G, a, b, c, d, GET(1), 0xf61e2562, 5) - STEP(G, d, a, b, c, GET(6), 0xc040b340, 9) - STEP(G, c, d, a, b, GET(11), 0x265e5a51, 14) - STEP(G, b, c, d, a, GET(0), 0xe9b6c7aa, 20) - STEP(G, a, b, c, d, GET(5), 0xd62f105d, 5) - STEP(G, d, a, b, c, GET(10), 0x02441453, 9) - STEP(G, c, d, a, b, GET(15), 0xd8a1e681, 14) - STEP(G, b, c, d, a, GET(4), 0xe7d3fbc8, 20) - STEP(G, a, b, c, d, GET(9), 0x21e1cde6, 5) - STEP(G, d, a, b, c, GET(14), 0xc33707d6, 9) - STEP(G, c, d, a, b, GET(3), 0xf4d50d87, 14) - STEP(G, b, c, d, a, GET(8), 0x455a14ed, 20) - STEP(G, a, b, c, d, GET(13), 0xa9e3e905, 5) - STEP(G, d, a, b, c, GET(2), 0xfcefa3f8, 9) - STEP(G, c, d, a, b, GET(7), 0x676f02d9, 14) - STEP(G, b, c, d, a, GET(12), 0x8d2a4c8a, 20) - -/* Round 3 */ - STEP(H, a, b, c, d, GET(5), 0xfffa3942, 4) - STEP(H2, d, a, b, c, GET(8), 0x8771f681, 11) - STEP(H, c, d, a, b, GET(11), 0x6d9d6122, 16) - STEP(H2, b, c, d, a, GET(14), 0xfde5380c, 23) - STEP(H, a, b, c, d, GET(1), 0xa4beea44, 4) - STEP(H2, d, a, b, c, GET(4), 0x4bdecfa9, 11) - STEP(H, c, d, a, b, GET(7), 0xf6bb4b60, 16) - STEP(H2, b, c, d, a, GET(10), 0xbebfbc70, 23) - STEP(H, a, b, c, d, GET(13), 0x289b7ec6, 4) - STEP(H2, d, a, b, c, GET(0), 0xeaa127fa, 11) - STEP(H, c, d, a, b, GET(3), 0xd4ef3085, 16) - STEP(H2, b, c, d, a, GET(6), 0x04881d05, 23) - STEP(H, a, b, c, d, GET(9), 0xd9d4d039, 4) - STEP(H2, d, a, b, c, GET(12), 0xe6db99e5, 11) - STEP(H, c, d, a, b, GET(15), 0x1fa27cf8, 16) - STEP(H2, b, c, d, a, GET(2), 0xc4ac5665, 23) - -/* Round 4 */ - STEP(I, a, b, c, d, GET(0), 0xf4292244, 6) - STEP(I, d, a, b, c, GET(7), 0x432aff97, 10) - STEP(I, c, d, a, b, GET(14), 0xab9423a7, 15) - STEP(I, b, c, d, a, GET(5), 0xfc93a039, 21) - STEP(I, a, b, c, d, GET(12), 0x655b59c3, 6) - STEP(I, d, a, b, c, GET(3), 0x8f0ccc92, 10) - STEP(I, c, d, a, b, GET(10), 0xffeff47d, 15) - STEP(I, b, c, d, a, GET(1), 0x85845dd1, 21) - STEP(I, a, b, c, d, GET(8), 0x6fa87e4f, 6) - STEP(I, d, a, b, c, GET(15), 0xfe2ce6e0, 10) - STEP(I, c, d, a, b, GET(6), 0xa3014314, 15) - STEP(I, b, c, d, a, GET(13), 0x4e0811a1, 21) - STEP(I, a, b, c, d, GET(4), 0xf7537e82, 6) - STEP(I, d, a, b, c, GET(11), 0xbd3af235, 10) - STEP(I, c, d, a, b, GET(2), 0x2ad7d2bb, 15) - STEP(I, b, c, d, a, GET(9), 0xeb86d391, 21) - - a += saved_a; - b += saved_b; - c += saved_c; - d += saved_d; - - ptr += 64; - } while (size -= 64); - - ctx->a = a; - ctx->b = b; - ctx->c = c; - ctx->d = d; - - return ptr; -} - -void MD5_Init(MD5_CTX *ctx) -{ - ctx->a = 0x67452301; - ctx->b = 0xefcdab89; - ctx->c = 0x98badcfe; - ctx->d = 0x10325476; - - ctx->lo = 0; - ctx->hi = 0; -} - -void MD5_Update(MD5_CTX *ctx, const void *data, unsigned long size) -{ - MD5_u32plus saved_lo; - unsigned long used, available; - - saved_lo = ctx->lo; - if ((ctx->lo = (saved_lo + size) & 0x1fffffff) < saved_lo) - ctx->hi++; - ctx->hi += size >> 29; - - used = saved_lo & 0x3f; - - if (used) { - available = 64 - used; - - if (size < available) { - memcpy(&ctx->buffer[used], data, size); - return; - } - - memcpy(&ctx->buffer[used], data, available); - data = (const unsigned char *)data + available; - size -= available; - body(ctx, ctx->buffer, 64); - } - - if (size >= 64) { - data = body(ctx, data, size & ~(unsigned long)0x3f); - size &= 0x3f; - } - - memcpy(ctx->buffer, data, size); -} - -void MD5_Final(unsigned char *result, MD5_CTX *ctx) -{ - unsigned long used, available; - - used = ctx->lo & 0x3f; - - ctx->buffer[used++] = 0x80; - - available = 64 - used; - - if (available < 8) { - memset(&ctx->buffer[used], 0, available); - body(ctx, ctx->buffer, 64); - used = 0; - available = 64; - } - - memset(&ctx->buffer[used], 0, available - 8); - - ctx->lo <<= 3; - ctx->buffer[56] = ctx->lo; - ctx->buffer[57] = ctx->lo >> 8; - ctx->buffer[58] = ctx->lo >> 16; - ctx->buffer[59] = ctx->lo >> 24; - ctx->buffer[60] = ctx->hi; - ctx->buffer[61] = ctx->hi >> 8; - ctx->buffer[62] = ctx->hi >> 16; - ctx->buffer[63] = ctx->hi >> 24; - - body(ctx, ctx->buffer, 64); - - result[0] = ctx->a; - result[1] = ctx->a >> 8; - result[2] = ctx->a >> 16; - result[3] = ctx->a >> 24; - result[4] = ctx->b; - result[5] = ctx->b >> 8; - result[6] = ctx->b >> 16; - result[7] = ctx->b >> 24; - result[8] = ctx->c; - result[9] = ctx->c >> 8; - result[10] = ctx->c >> 16; - result[11] = ctx->c >> 24; - result[12] = ctx->d; - result[13] = ctx->d >> 8; - result[14] = ctx->d >> 16; - result[15] = ctx->d >> 24; - - memset(ctx, 0, sizeof(*ctx)); -} - -#endif diff --git a/src/hal/components/cros/src/tcpip_socket.c b/src/hal/components/cros/src/tcpip_socket.c deleted file mode 100644 index e9c562b7..00000000 --- a/src/hal/components/cros/src/tcpip_socket.c +++ /dev/null @@ -1,844 +0,0 @@ -#include -#include -#include - -#include "tcpip_socket.h" -#include "cros_defs.h" -#include "cros_log.h" -#include "cros_clock.h" - -#ifdef _WIN32 -# define WIN32_LEAN_AND_MEAN // speed up the build process by excluding parts of the Windows header -# include -# include -# include // Ws2_32.lib must be used when linking -// We redefine the socket function codes using constants (FN_...) that will have the same name on Windows and Linux, -// so, in the functions below we do not have to implement different source code for each operating system. -# define FN_EISCONN WSAEISCONN -# define FN_EINPROGRESS WSAEINPROGRESS -# define FN_EALREADY WSAEALREADY -# define FN_ECONNREFUSED WSAECONNREFUSED -# define FN_EWOULDBLOCK WSAEWOULDBLOCK -# define FN_EAGAIN WSAEWOULDBLOCK // Windows does not have a different error code for EAGAIN, so we use EWOULDBLOCK -# define FN_ENOTCONN WSAENOTCONN -# define FN_ECONNRESET WSAECONNRESET -# define FN_EINTR WSAEINTR - -# define FN_SHUT_RDWR SD_BOTH -typedef int fn_socklen_t; -#else -# include -# include -# include -# include -# include -# include -# include -# define closesocket close - -// connect()/accept()/send()/recv()/select() error codes: -# define FN_EISCONN EISCONN -# define FN_EINPROGRESS EINPROGRESS -# define FN_EALREADY EALREADY -# define FN_ECONNREFUSED ECONNREFUSED -# define FN_EWOULDBLOCK EWOULDBLOCK -# define FN_EAGAIN EAGAIN -# define FN_ENOTCONN ENOTCONN -# define FN_ECONNRESET ECONNRESET -# define FN_EINTR EINTR -// shutdown() how mode: -# define FN_SHUT_RDWR SHUT_RDWR -typedef socklen_t fn_socklen_t; -#endif - -#define TCPIP_SOCKET_READ_BUFFER_SIZE 2048 -// Definitions for debug messages only. -// Console virtual-terminal color sequences (supported on Linux and on Windows 10 build 16257 and later): -#define ANSI_COLOR_GREEN "\x1b[32m" -#define ANSI_COLOR_YELLOW "\x1b[33m" -#define ANSI_COLOR_MAGENTA "\x1b[35m" -#define ANSI_COLOR_CYAN "\x1b[36m" -#define ANSI_COLOR_RESET "\x1b[0m" - -// This global variable is incremented for each call to tcpIpSocketStartUp() that has been successfuly executed, -// and it decremented after each successful call to tcpIpSocketCleanUp() -int socket_lib_initialized = 0; // Default value: 0 = library not initialized - -void tcpIpSocketInit ( TcpIpSocket *s ) -{ - PRINT_VVDEBUG ( "tcpIpSocketInit()\n" ); - s->fd = FN_INVALID_SOCKET; - memset ( & ( s->rem_addr ), 0, sizeof ( struct sockaddr_in ) ); - s->open = 0; - s->connected = 0; - s->listening = 0; - s->is_nonblocking = 0; -} - -int tcpIpSocketOpen ( TcpIpSocket *s ) -{ - int ret_success; - - PRINT_VVDEBUG ( "tcpIpSocketOpen()\n" ); - if ( s->open ) - return(1); - - s->fd = socket ( AF_INET, SOCK_STREAM, IPPROTO_TCP ); - if ( s->fd == FN_INVALID_SOCKET ) - { - PRINT_ERROR ( "tcpIpSocketOpen() : Can't open a socket. Error code: %i\n", tcpIpSocketGetError()); - ret_success = 0; - } - else - { - PRINT_VDEBUG ( "tcpIpSocketOpen(): Created socket FD: %i\n", s->fd); - s->open = 1; - ret_success = 1; - } - - return(ret_success); -} - -int tcpIpSocketClose ( TcpIpSocket *s ) -{ - int ret_success; - - PRINT_VVDEBUG ( "tcpIpSocketClose()\n"); - - if ( !s->open ) - return(1); - - if ( s->fd != FN_INVALID_SOCKET ) - { - int close_ret_val; - - PRINT_VDEBUG ( "tcpIpSocketClose(): Closing socket FD: %i\n", s->fd); - close_ret_val = closesocket( s->fd ); - ret_success = (close_ret_val != FN_SOCKET_ERROR); - } - else - { - PRINT_ERROR ( "tcpIpSocketClose(): Invalid file descriptor: %i\n", s->fd); - ret_success = 0; - } - tcpIpSocketInit ( s ); - - return(ret_success); -} - -int tcpIpSocketSetNonBlocking ( TcpIpSocket *s ) -{ - int ret_fn_ctl; - - PRINT_VVDEBUG ( "tcpIpSocketSetNonBlocking()\n" ); - - if ( !s->open ) - { - PRINT_ERROR ( "tcpIpSocketSetNonBlocking() : Socket not opened\n" ); - return(0); - } - -#ifdef _WIN32 - u_long enable_non_blocking = 1; - ret_fn_ctl = ioctlsocket(s->fd, FIONBIO, &enable_non_blocking); -#else - int prev_flags; - prev_flags = fcntl( s->fd, F_GETFL, 0 ); - if(prev_flags < 0) - { - PRINT_ERROR ( "tcpIpSocketSetNonBlocking() : fcntl() failed getting the socket flags\n" ); - prev_flags = 0; // Try to continue anyway - } - ret_fn_ctl = fcntl ( s->fd, F_SETFL, prev_flags | O_NONBLOCK ); -#endif - - if(ret_fn_ctl == 0) - { - s->is_nonblocking = 1; - return(1); - } - else - { - PRINT_ERROR ( "tcpIpSocketSetNonBlocking() : fcntl()/ ioctlsocket() failed configuring socket as non blocking. Error code: %i\n", tcpIpSocketGetError()); - return(0); - } -} - -int tcpIpSocketSetNoDelay ( TcpIpSocket *s ) -{ - PRINT_VVDEBUG ( "tcpIpSocketSetNoDelay()\n" ); - - if ( !s->open ) - { - PRINT_ERROR ( "tcpIpSocketSetNoDelay() : Socket not opened\n" ); - return(0); - } - - int enable_no_delay = 1; - int ret = setsockopt ( s->fd, IPPROTO_TCP, TCP_NODELAY, (const void *)&enable_no_delay, sizeof(enable_no_delay) ); - - if ( ret == 0 ) - { - return(1); - } - else - { - PRINT_ERROR ( "tcpIpSocketSetNoDelay() : setsockopt() with TCP_NODELAY failed. System error code: %i \n", tcpIpSocketGetError()); - return(0); - } -} - -int tcpIpSocketSetReuse ( TcpIpSocket *s ) -{ - PRINT_VVDEBUG ( "tcpIpSocketSetReuse()\n" ); - - if ( !s->open ) - { - PRINT_ERROR ( "tcpIpSocketSetReuse() : Socket not opened\n" ); - return(0); - } - - int enable_reuse_addr = 1; - if ( setsockopt ( s->fd, SOL_SOCKET, SO_REUSEADDR, (const char*)&enable_reuse_addr, sizeof(enable_reuse_addr) ) != 0 ) - { - PRINT_ERROR ( "tcpIpSocketSetReuse() : setsockopt() with SO_REUSEADDR option failed. System error code: %i \n", tcpIpSocketGetError()); - return(0); - } - return(1); -} - -int tcpIpSocketSetKeepAlive ( TcpIpSocket *s, unsigned int idle, unsigned int interval, unsigned int count ) -{ - PRINT_VVDEBUG ( "tcpIpSocketSetKeepAlive()\n" ); - - if ( !s->open ) - { - PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : Socket not opened\n" ); - return(0); - } - - int sock_opt_val = 1; - if ( setsockopt ( s->fd, SOL_SOCKET, SO_KEEPALIVE, (const char *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) - { - PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with SO_KEEPALIVE option failed. System error code: %i \n", tcpIpSocketGetError()); - return(0); - } - - // TCP_KEEPIDLE on Linux is equivalent to TCP_KEEPALIVE option on OS X - // see https://www.winehq.org/pipermail/wine-devel/2015-July/108583.html - sock_opt_val = idle; -#ifdef __APPLE__ - if ( setsockopt( s->fd, IPPROTO_TCP, TCP_KEEPALIVE, (void *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) - { - PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with TCP_KEEPALIVE option failed \n" ); - return 0; - } -#else - if ( setsockopt ( s->fd, IPPROTO_TCP, TCP_KEEPIDLE, (const void *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) - { - PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with SO_KEEPALIVE option failed. System error code: %i \n", tcpIpSocketGetError()); - return(0); - } -#endif - - sock_opt_val = interval; - if ( setsockopt ( s->fd, IPPROTO_TCP, TCP_KEEPINTVL, (const void *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) - { - PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with TCP_KEEPINTVL option failed. System error code: %i \n", tcpIpSocketGetError()); - return(0); - } - - // On Windows this option is available since Windows 10 only - sock_opt_val = count; - if ( setsockopt ( s->fd, IPPROTO_TCP, TCP_KEEPCNT, (const void *)&sock_opt_val, sizeof ( sock_opt_val ) ) != 0 ) - { - PRINT_ERROR ( "tcpIpSocketSetKeepAlive() : setsockopt() with TCP_KEEPCNT option failed. System error code: %i \n", tcpIpSocketGetError()); - return(0); - } - - return(1); -} - -TcpIpSocketState tcpIpSocketConnect ( TcpIpSocket *s, const char *host_addr, unsigned short host_port ) -{ - int connect_ret; - struct sockaddr_in adr; - int fn_error_code; - - PRINT_VVDEBUG ( "tcpIpSocketConnect():\n" ); - - if ( !s->open ) - { - PRINT_ERROR ( "tcpIpSocketConnect() : Socket not opened\n" ); - return TCPIPSOCKET_FAILED; - } - - if( s->connected ) - return TCPIPSOCKET_DONE; - - memset ( &adr, 0, sizeof ( struct sockaddr_in ) ); - - adr.sin_family = AF_INET; - adr.sin_port = htons ( host_port ); - if ( inet_pton ( AF_INET, host_addr, &adr.sin_addr ) <= 0 ) - { - PRINT_ERROR ( "tcpIpSocketConnect() : Invalid network address: %s. It cannot be converted to a binary address. System error code: %i \n", host_addr, tcpIpSocketGetError()); - s->connected = 0; - return TCPIPSOCKET_FAILED; - } - - connect_ret = connect ( s->fd, ( struct sockaddr * ) &adr, sizeof ( struct sockaddr ) ); - fn_error_code = tcpIpSocketGetError(); - if ( connect_ret == FN_SOCKET_ERROR && fn_error_code != FN_EISCONN ) // The connection is not established so far - { - if ( s->is_nonblocking && - ( fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EALREADY || fn_error_code == FN_EWOULDBLOCK) ) - { - PRINT_VDEBUG ( "tcpIpSocketConnect() : Connection in progress to %s:%i through FD:%i\n", host_addr, host_port, s->fd); - return TCPIPSOCKET_IN_PROGRESS; - } - else - { - s->connected = 0; - if(fn_error_code == FN_ECONNREFUSED) - { - PRINT_ERROR ( "tcpIpSocketConnect() : Connection to %s:%i through FD:%i was refused\n", host_addr, host_port, s->fd); - return TCPIPSOCKET_REFUSED; - } - else - { - PRINT_ERROR ( "tcpIpSocketConnect() : Connection to %s:%i through FD:%i failed due to error code: %i\n", host_addr, host_port, s->fd, fn_error_code); - return TCPIPSOCKET_FAILED; - } - } - } - PRINT_DEBUG (ANSI_COLOR_YELLOW"tcpIpSocketConnect() : connection established to %s:%i through FD:%i\n"ANSI_COLOR_RESET, host_addr, host_port, s->fd); - - s->rem_addr = adr; - s->connected = 1; - - return TCPIPSOCKET_DONE; -} - -int tcpIpSocketDisconnect ( TcpIpSocket *s ) -{ - PRINT_VVDEBUG ( "tcpIpSocketDisconnect()\n" ); - - if ( !s->connected ) - return 1; - - s->connected = 0; - if ( shutdown ( s->fd, FN_SHUT_RDWR ) != 0 ) - { - PRINT_ERROR ( "tcpIpSocketDisconnect() : shutdown failed. System error code: %i \n", tcpIpSocketGetError()); - return 0; - } - return 1; -} - -TcpIpSocketState tcpIpSocketCheckPort ( const char *host_addr, unsigned short host_port ) -{ - TcpIpSocketState port_open, fn_ret; - TcpIpSocket socket_struct; - - tcpIpSocketInit ( &socket_struct ); - fn_ret = tcpIpSocketOpen ( &socket_struct ); - if(fn_ret) - { - TcpIpSocketState socket_stat; - socket_stat = tcpIpSocketConnect ( &socket_struct, host_addr, host_port ); - if ( socket_stat == TCPIPSOCKET_IN_PROGRESS ) - port_open = TCPIPSOCKET_FAILED; - else - { - port_open = socket_stat; - if ( socket_stat == TCPIPSOCKET_DONE ) - tcpIpSocketDisconnect ( &socket_struct ); - } - - tcpIpSocketClose ( &socket_struct ); - } - else - port_open = TCPIPSOCKET_FAILED; - - return port_open; -} - -int tcpIpSocketBindListen( TcpIpSocket *s, const char *host_addr, unsigned short port, int backlog ) -{ - PRINT_VVDEBUG ( "tcpIpSocketBindListen()\n" ); - - if ( !s->open ) - { - PRINT_ERROR ( "tcpIpSocketBindListen() : Socket not opened\n" ); - return 0; - } - - if ( !s->listening ) - { - struct sockaddr_in adr; - - memset ( &adr, 0, sizeof ( struct sockaddr_in ) ); - - adr.sin_family = AF_INET; - adr.sin_port = htons ( port ); - adr.sin_addr.s_addr = htonl(INADDR_ANY); - - if ( inet_pton ( AF_INET, host_addr, &adr.sin_addr ) <= 0 ) - { - PRINT_ERROR ( "tcpIpSocketBindListen() : Invalid network address: %s. It cannot be converted to a binary address. System error code: %i \n", host_addr, tcpIpSocketGetError()); - return 0; - } - - - if ( bind ( s->fd, ( struct sockaddr * ) &adr, sizeof ( struct sockaddr ) ) == FN_SOCKET_ERROR ) - { - PRINT_ERROR ( "tcpIpSocketBindListen() : Socket bind failed. System error code: %i \n", tcpIpSocketGetError()); - return 0; - } - - if ( listen ( s->fd, backlog ) == FN_SOCKET_ERROR ) - { - PRINT_ERROR ( "tcpIpSocketBindListen() : Socket listen failed. System error code: %i \n", tcpIpSocketGetError()); - return 0; - } - - s->rem_addr = adr; - s->listening = 1; - } - - return 1; -} - -TcpIpSocketState tcpIpSocketAccept ( TcpIpSocket *s, TcpIpSocket *new_s ) -{ - PRINT_VVDEBUG ( "tcpIpSocketAccept()\n" ); - - TcpIpSocketState state = TCPIPSOCKET_DONE; - - if ( !s->open ) - { - PRINT_ERROR ( "tcpIpSocketAccept() : Failed: Socket is not opened\n" ); - return TCPIPSOCKET_FAILED; - } - - if ( !s->listening ) - { - PRINT_ERROR ( "tcpIpSocketAccept() : Failed: Socket is not listening\n" ); - return TCPIPSOCKET_FAILED; - } - - struct sockaddr_in new_adr; - socklen_t new_adr_len = sizeof(struct sockaddr); - - int new_fd = accept ( s->fd, ( struct sockaddr * ) &new_adr, &new_adr_len ); - - if ( new_fd == FN_INVALID_SOCKET ) - { - int fn_error_code; - - fn_error_code = tcpIpSocketGetError(); - if ( s->is_nonblocking && - ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) - { - PRINT_VDEBUG ( "tcpIpSocketAccept() : Connection accept in progress from port %i, new FD:%i\n", ntohs( new_adr.sin_port ), new_fd); - state = TCPIPSOCKET_IN_PROGRESS; - } - else - { - PRINT_ERROR ( "tcpIpSocketAccept() : accept() failed. Error code: %d\n", fn_error_code ); - return TCPIPSOCKET_FAILED; - } - } - else - PRINT_DEBUG (ANSI_COLOR_YELLOW"tcpIpSocketAccept() : Accepted connecion from port %i, new FD:%i\n"ANSI_COLOR_RESET, ntohs(new_adr.sin_port), new_fd); - - if( new_s->open ) - tcpIpSocketClose ( new_s ); - - new_s->fd = new_fd; - new_s->rem_addr = new_adr; - new_s->open = 1; - new_s->connected = 1; - - return state; -} - -void printTransmissionBuffer(const char *buffer, const char *msg_info, char *msg_color_seq, int msg_fd, int buf_len) -{ - int i; - PRINT_DEBUG("%s", msg_color_seq); - PRINT_DEBUG("%s (%i bytes FD: %i) ["ANSI_COLOR_RESET, msg_info, buf_len, msg_fd); - for(i=0;iconnected ) - { - PRINT_ERROR ( "tcpIpSocketWriteBuffer() : Socket not connected\n" ); - return TCPIPSOCKET_FAILED; - } - - #if CROS_DEBUG_LEVEL >= 2 - printTransmissionBuffer(data, "tcpIpSocketWriteBuffer() : Buffer", ANSI_COLOR_MAGENTA, s->fd, data_size); - #endif - while ( data_size > 0 ) - { - int n_written , fn_error_code; - - n_written = send ( s->fd, data, data_size, 0 ); - fn_error_code = tcpIpSocketGetError(); - if ( n_written > 0 ) - { - dynBufferMovePoseIndicator ( d_buf, n_written ); - data = (const char *)dynBufferGetCurrentData ( d_buf ); - data_size = dynBufferGetRemainingDataSize ( d_buf ); - } - else if ( s->is_nonblocking && - ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) - { - PRINT_VDEBUG ( "tcpIpSocketWriteBuffer() : write in progress, %d remaining bytes\n", data_size ); - return TCPIPSOCKET_IN_PROGRESS; - } - else if ( fn_error_code == FN_ENOTCONN || fn_error_code == FN_ECONNRESET ) - { - PRINT_VDEBUG ( "tcpIpSocketWriteBuffer() : socket disconnected\n" ); - s->connected = 0; - return TCPIPSOCKET_DISCONNECTED; - } - else - { - PRINT_ERROR ( "tcpIpSocketWriteBuffer() : Write failed. Error code: %i\n", fn_error_code); - return TCPIPSOCKET_FAILED; - } - } - - return TCPIPSOCKET_DONE; -} - -TcpIpSocketState tcpIpSocketWriteString ( TcpIpSocket *s, DynString *d_str ) -{ - PRINT_VVDEBUG ( "tcpIpSocketWriteString()\n" ); - - const char *data = dynStringGetCurrentData ( d_str ); - int data_size = dynStringGetRemainingDataSize ( d_str ); - - if ( !s->connected ) - { - PRINT_ERROR ( "tcpIpSocketWriteString() : Socket not connected\n" ); - return TCPIPSOCKET_FAILED; - } - #if CROS_DEBUG_LEVEL >= 2 - printTransmissionBuffer(data, "tcpIpSocketWriteString() : Buffer", ANSI_COLOR_MAGENTA, s->fd, data_size); - #endif - while ( data_size > 0 ) - { - int n_written , fn_error_code; - - n_written = send ( s->fd, data, data_size, 0 ); - fn_error_code = tcpIpSocketGetError(); - if ( n_written > 0 ) - { - dynStringMovePoseIndicator ( d_str, n_written ); - data = dynStringGetCurrentData ( d_str ); - data_size = dynStringGetRemainingDataSize ( d_str ); - } - else if ( s->is_nonblocking && - ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) - { - PRINT_VDEBUG ( "tcpIpSocketWriteString() : write in progress, %d remaining bytes\n", data_size ); - return TCPIPSOCKET_IN_PROGRESS; - } - else if ( fn_error_code == FN_ENOTCONN || fn_error_code == FN_ECONNRESET ) - { - PRINT_VDEBUG ( "tcpIpSocketWriteString() : socket disconnectd\n" ); - s->connected = 0; - return TCPIPSOCKET_DISCONNECTED; - } - else - { - PRINT_ERROR ( "tcpIpSocketWriteString() : Write failed. Error code: %i\n", fn_error_code); - return TCPIPSOCKET_FAILED; - } - } - - return TCPIPSOCKET_DONE; -} - -TcpIpSocketState tcpIpSocketReadBuffer ( TcpIpSocket *s, DynBuffer *d_buf ) -{ - size_t n_read; - return tcpIpSocketReadBufferEx(s, d_buf, TCPIP_SOCKET_READ_BUFFER_SIZE, &n_read); -} - -TcpIpSocketState tcpIpSocketReadBufferEx( TcpIpSocket *s, DynBuffer *d_buf, size_t max_size, size_t *n_reads) -{ - int recv_ret, fn_error_code; - char *read_buf; - - PRINT_VVDEBUG ( "tcpIpSocketReadBufferEx()\n" ); - - *n_reads = 0; - if ( !s->connected ) - { - PRINT_ERROR ( "tcpIpSocketReadBufferEx() : Socket not connected\n" ); - return TCPIPSOCKET_FAILED; - } - - read_buf = (char *)malloc(max_size); - if (read_buf == NULL) - { - PRINT_ERROR("tcpIpSocketReadBufferEx() : Out of memory allocating %lu bytes before reading from socket", (unsigned long)max_size); - return TCPIPSOCKET_FAILED; - } - - TcpIpSocketState state = TCPIPSOCKET_UNKNOWN; - recv_ret = recv ( s->fd, read_buf, max_size, 0); - fn_error_code = tcpIpSocketGetError(); - if ( recv_ret == 0 ) - { - PRINT_VDEBUG ( "tcpIpSocketReadBufferEx() : socket disconnectd\n" ); - s->connected = 0; - state = TCPIPSOCKET_DISCONNECTED; - } - else if ( recv_ret > 0 ) - { - PRINT_VDEBUG ( "tcpIpSocketReadBufferEx() : read %d bytes \n", recv_ret ); - #if CROS_DEBUG_LEVEL >= 2 - printTransmissionBuffer((const char *)read_buf, "tcpIpSocketReadBufferEx() : Buffer", ANSI_COLOR_CYAN, s->fd, recv_ret); - #endif - - dynBufferPushBackBuf ( d_buf, (const unsigned char *)read_buf, recv_ret ); - state = TCPIPSOCKET_DONE; - *n_reads = recv_ret; - } - else if ( s->is_nonblocking && - ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) - { - PRINT_VDEBUG ( "tcpIpSocketReadBufferEx() : read in progress\n" ); - state = TCPIPSOCKET_IN_PROGRESS; - } - else if ( fn_error_code == FN_ENOTCONN || fn_error_code == FN_ECONNRESET ) - { - PRINT_VDEBUG ( "tcpIpSocketReadBufferEx() : socket disconnectd\n" ); - s->connected = 0; - state = TCPIPSOCKET_DISCONNECTED; - } - else - { - PRINT_ERROR ( "tcpIpSocketReadBufferEx() : Read through socket failed. Error code: %i\n", fn_error_code); - state = TCPIPSOCKET_FAILED; - } - - free(read_buf); - - return state; -} - -TcpIpSocketState tcpIpSocketReadString ( TcpIpSocket *s, DynString *d_str ) -{ - int recv_ret, fn_error_code; - - PRINT_VVDEBUG ( "tcpIpSocketReadString()\n" ); - - if ( !s->connected ) - { - PRINT_ERROR ( "tcpIpSocketReadString() : Socket not connected\n" ); - return TCPIPSOCKET_FAILED; - } - - char read_buf[TCPIP_SOCKET_READ_BUFFER_SIZE]; - - TcpIpSocketState state = TCPIPSOCKET_UNKNOWN; - - recv_ret = recv ( s->fd, read_buf, TCPIP_SOCKET_READ_BUFFER_SIZE , 0 ); - fn_error_code = tcpIpSocketGetError(); - if ( recv_ret == 0 ) - { - PRINT_VDEBUG ( "tcpIpSocketReadString() : socket disconnectd\n" ); - s->connected = 0; - state = TCPIPSOCKET_DISCONNECTED; - } - else if ( recv_ret > 0 ) - { - PRINT_VDEBUG ( "tcpIpSocketReadString() : read %d bytes \n", recv_ret ); - #if CROS_DEBUG_LEVEL >= 2 - printTransmissionBuffer(read_buf, "tcpIpSocketReadString() : Buffer", ANSI_COLOR_CYAN, s->fd, recv_ret); - #endif - - dynStringPushBackStrN ( d_str, read_buf, recv_ret ); - state = TCPIPSOCKET_DONE; - } - else if ( s->is_nonblocking && - ( fn_error_code == FN_EWOULDBLOCK || fn_error_code == FN_EINPROGRESS || fn_error_code == FN_EAGAIN ) ) - { - PRINT_VDEBUG ( "tcpIpSocketReadString() : read in progress\n" ); - state = TCPIPSOCKET_IN_PROGRESS; - } - else if ( fn_error_code == FN_ENOTCONN || fn_error_code == FN_ECONNRESET ) - { - PRINT_VDEBUG ( "tcpIpSocketReadString() : socket disconnectd\n" ); - s->connected = 0; - state = TCPIPSOCKET_DISCONNECTED; - } - else - { - PRINT_ERROR ( "tcpIpSocketReadString() : Read failed. Error code: %i\n", fn_error_code); - state = TCPIPSOCKET_FAILED; - } - - return state; -} - -int tcpIpSocketGetFD ( TcpIpSocket *s ) -{ - return s->fd; -} - -unsigned short tcpIpSocketGetPort( TcpIpSocket *s ) -{ - struct sockaddr_in addr; - int fn_error_code; - unsigned short ret_addr_port; - fn_socklen_t len_adr = sizeof ( addr ); - if ( getsockname ( s->fd, (struct sockaddr *)&addr, &len_adr ) == 0 ) - ret_addr_port = ntohs ( ((struct sockaddr_in *)&addr)->sin_port ); - else - { - fn_error_code = tcpIpSocketGetError(); - PRINT_ERROR ( "tcpIpSocketConnect() : getsockname() failed obtaining the socket local port due to error code: %i\n", fn_error_code); - } - - return ret_addr_port; -} - -unsigned short tcpIpSocketGetRemotePort( TcpIpSocket *s ) -{ - return ntohs( s->rem_addr.sin_port ); -} - -const char *tcpIpSocketGetRemoteAddress( TcpIpSocket *s ) -{ - char host_addr_buff[MAX_HOST_NAME_LEN+1]; - const char *ret_host_addr; - - ret_host_addr = inet_ntop(s->rem_addr.sin_family, &s->rem_addr.sin_addr, host_addr_buff, sizeof(host_addr_buff)); - - return ret_host_addr; -} - -int tcpIpSocketSelect( int nfds, fd_set *readfds, fd_set *writefds, fd_set *exceptfds, uint64_t time_out ) -{ - struct timeval timeout_tv; - int nfds_set; - - PRINT_VVDEBUG( "tcpIpSocketSelect(): nfds: %i\n", nfds); - - timeout_tv = cRosClockGetTimeVal( time_out ); - - nfds_set = select(nfds, readfds, writefds, exceptfds, &timeout_tv); - - if(nfds_set == FN_SOCKET_ERROR)// Using the definition SOCKET_ERROR is not needed to check the return value, but it is recommended on Windows - { - int socket_err_num; - - socket_err_num = tcpIpSocketGetError(); - - if(socket_err_num == FN_EINTR) - { - PRINT_VDEBUG("tcpIpSocketSelect() : select() returned EINTR error code\n"); - nfds_set = 0; - } - else - { - PRINT_ERROR("tcpIpSocketSelect() : select() call failed. Error code: %i\n", socket_err_num); - nfds_set = -1; - } - } - - return(nfds_set); -} - -#define REQUESTED_WS_HIGH_VER 2 -#define REQUESTED_WS_LOW_VER 0 -int tcpIpSocketStartUp( void ) -{ - int ret_val; - - if(socket_lib_initialized == 0) // If the library is not aready initialized: - { -#ifdef _WIN32 - WORD ws_ver_requested; - WSADATA ws_ver_obtained; - - ws_ver_requested = MAKEWORD(REQUESTED_WS_HIGH_VER, REQUESTED_WS_LOW_VER); // Codify the requested version of Winsock - ret_val = WSAStartup(ws_ver_requested, &ws_ver_obtained); // It returns 0 on success, the same as tcpIpSocketStartUp() - if(ret_val == 0) // If success, check the obtained version - { - // Ckeck that WinSock supports the requested version - if(LOBYTE(ws_ver_obtained.wVersion) != REQUESTED_WS_HIGH_VER || HIBYTE(ws_ver_obtained.wVersion) != REQUESTED_WS_LOW_VER) - PRINT_ERROR("tcpIpSocketStartUp(): Could not find the required version of Winsock: %i.%i.\n", REQUESTED_WS_HIGH_VER, REQUESTED_WS_LOW_VER); - } - else - PRINT_ERROR ( "tcpIpSocketStartUp(): Loading of Winsock failed with error code: %i.\n", ret_val); -#else - // Ignore the SIGPIPE signal. That is, prevent the process from being terminated when it tries to write to a closed socket - signal(SIGPIPE, SIG_IGN); - // In Windows this default behavior does not occur, so nothing has to be done - ret_val = 0; // The socket library does not have to be initialized on Linux or OS X -#endif - if(ret_val == 0) - socket_lib_initialized++; - } - else - ret_val = 0; - - return(ret_val); -} - -int tcpIpSocketCleanUp( void ) -{ - int ret_val; - - if(socket_lib_initialized != 0) // If the library is initialized: - { -#ifdef _WIN32 - ret_val = WSACleanup(); -#else - ret_val = 0; // The socket library does not need to be initialized on Linux or OS X -#endif - if(ret_val == 0) - socket_lib_initialized--; - } - else - ret_val = 0; - - return(ret_val); -} - -int tcpIpSocketGetError( void ) -{ - int socket_err_num; - -#ifdef _WIN32 - socket_err_num = WSAGetLastError(); -#else - socket_err_num = errno; -#endif - - return(socket_err_num); -} diff --git a/src/hal/components/cros/src/tcpros_process.c b/src/hal/components/cros/src/tcpros_process.c deleted file mode 100644 index 1607ec9e..00000000 --- a/src/hal/components/cros/src/tcpros_process.c +++ /dev/null @@ -1,79 +0,0 @@ -#include "tcpros_process.h" -#include "cros_clock.h" -#include - -void tcprosProcessInit( TcprosProcess *p ) -{ - p->state = TCPROS_PROCESS_STATE_IDLE; - tcpIpSocketInit( &(p->socket) ); - dynStringInit( &(p->topic) ); - dynStringInit( &(p->caller_id) ); - dynStringInit( &(p->service) ); - dynStringInit( &(p->type) ); - dynStringInit( &(p->servicerequest_type) ); - dynStringInit( &(p->serviceresponse_type) ); - dynStringInit( &(p->md5sum) ); - dynBufferInit( &(p->packet) ); - p->latching = p->tcp_nodelay = p->persistent = 0; - p->probe = 0; - p->last_change_time = 0; - p->topic_idx = -1; - p->service_idx = -1; - p->ok_byte = 0; - p->left_to_recv = 0; - p->sub_tcpros_host = NULL; - p->sub_tcpros_port = -1; -} - -void tcprosProcessRelease( TcprosProcess *p ) -{ - if( p->socket.connected ) - tcpIpSocketDisconnect( &(p->socket) ); - - dynStringRelease( &(p->topic) ); - dynStringRelease( &(p->service) ); - dynStringRelease( &(p->caller_id) ); - dynStringRelease( &(p->type) ); - dynStringRelease( &(p->servicerequest_type) ); - dynStringRelease( &(p->serviceresponse_type) ); - dynStringRelease( &(p->md5sum) ); - dynBufferRelease( &(p->packet) ); - free(p->sub_tcpros_host); -} - -void tcprosProcessClear( TcprosProcess *p) -{ - dynBufferClear( &(p->packet) ); - p->left_to_recv = 0; -} - -void tcprosProcessReset( TcprosProcess *p) -{ - tcprosProcessClear( p ); - - dynStringClear( &(p->topic) ); - dynStringClear( &(p->caller_id) ); - dynStringClear( &(p->service) ); - dynStringClear( &(p->type) ); - dynStringClear( &(p->servicerequest_type) ); - dynStringClear( &(p->serviceresponse_type) ); - dynStringClear( &(p->md5sum) ); - p->latching = 0; - p->tcp_nodelay = 0; - p->persistent = 0; - p->probe = 0; - p->topic_idx = -1; - p->service_idx = -1; - p->ok_byte = 0; - free(p->sub_tcpros_host); - p->sub_tcpros_host = NULL; - p->sub_tcpros_port = -1; - - tcprosProcessChangeState( p, TCPROS_PROCESS_STATE_IDLE ); -} - -void tcprosProcessChangeState( TcprosProcess *p, TcprosProcessState state ) -{ - p->state = state; - p->last_change_time = cRosClockGetTimeMs(); -} diff --git a/src/hal/components/cros/src/xmlrpc_params.c b/src/hal/components/cros/src/xmlrpc_params.c deleted file mode 100644 index 56573733..00000000 --- a/src/hal/components/cros/src/xmlrpc_params.c +++ /dev/null @@ -1,1454 +0,0 @@ -#include -#include -#include -#include - -#include "xmlrpc_params.h" -#include "xmlrpc_tags.h" -#include "cros_defs.h" -#include "cros_log.h" - -enum { XMLRPC_ARRAY_INIT_SIZE = 4, XMLRPC_ARRAY_GROW_RATE = 2 }; - -typedef enum ParamContainerType -{ - PARAM_CONTAINER_NONE = 0, - PARAM_CONTAINER_ARRAY = 1, - PARAM_CONTAINER_STRUCT = 2 -} ParamContainerType; - -static int paramFromXml ( DynString *message, XmlrpcParam *param, ParamContainerType container); -static int paramValueFromXml ( DynString *message, XmlrpcParam *param, ParamContainerType container); -static int structMemberFromXml ( DynString *message, XmlrpcParam *param); -static int arrayFromXml ( DynString *message, XmlrpcParam *param); -static int structFromXml ( DynString *message, XmlrpcParam *param); -static XmlrpcParam * arrayAddElem ( XmlrpcParam *param ); -static int paramSetMemberName ( XmlrpcParam *param, const char *name ); - - -static void boolToXml ( unsigned char val, DynString *message ) -{ - dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); - dynStringPushBackStr ( message, XMLRPC_BOOLEAN_TAG.str ); - dynStringPushBackStr ( message, val != 0?"1":"0" ); - dynStringPushBackStr ( message, XMLRPC_BOOLEAN_ETAG.str ); - dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); -} - -static void intToXml ( int val, DynString *message ) -{ - dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); - dynStringPushBackStr ( message, XMLRPC_INT_TAG.str ); - char num_str[22]; - snprintf ( num_str, 22, "%d", val ); - dynStringPushBackStr ( message, num_str ); - dynStringPushBackStr ( message, XMLRPC_INT_ETAG.str ); - dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); -} - -static void doubleToXml ( double val, DynString *message ) -{ - setlocale ( LC_NUMERIC, "" ); // Set decimal-point and thousands-separator character to the implementation-defined native environment - dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); - dynStringPushBackStr ( message, XMLRPC_DOUBLE_TAG.str ); - char num_str[26]; - snprintf ( num_str, 26, "%.17g", val ); - dynStringPushBackStr ( message, num_str ); - dynStringPushBackStr ( message, XMLRPC_DOUBLE_ETAG.str ); - dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); -} - -static void stringToXml ( char *val, DynString *message ) -{ - dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); - dynStringPushBackStr ( message, XMLRPC_STRING_TAG.str ); - - int str_len = strlen ( val ); - int i = 0; - for ( i = 0; i < str_len; i++ ) - { - char c = val[i]; - if ( c == '<' ) - dynStringPushBackStr ( message, "<" ); - else if ( c == '>' ) - dynStringPushBackStr ( message, ">" ); - else if ( c == '&' ) - dynStringPushBackStr ( message, "&" ); - else if ( c == '\'' ) - dynStringPushBackStr ( message, "'" ); - else if ( c == '\"' ) - dynStringPushBackStr ( message, """ ); - else - dynStringPushBackChar ( message, c ); - } - - dynStringPushBackStr ( message, XMLRPC_STRING_ETAG.str ); - dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); -} - -static void structToXml ( XmlrpcParam *val, DynString *message ) -{ - dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); - dynStringPushBackStr ( message, XMLRPC_STRUCT_TAG.str ); - - int i; - for ( i = 0; i < val->array_n_elem; i++ ) - xmlrpcParamToXml ( & ( val->data.as_array[i] ), message ); - - dynStringPushBackStr ( message, XMLRPC_STRUCT_ETAG.str ); - dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); -} - -static void arrayToXml ( XmlrpcParam *val, DynString *message ) -{ - dynStringPushBackStr ( message, XMLRPC_VALUE_TAG.str ); - dynStringPushBackStr ( message, XMLRPC_ARRAY_TAG.str ); - dynStringPushBackStr ( message, XMLRPC_DATA_TAG.str ); - - int i; - for ( i = 0; i < val->array_n_elem; i++ ) - xmlrpcParamToXml ( & ( val->data.as_array[i] ), message ); - - dynStringPushBackStr ( message, XMLRPC_DATA_ETAG.str ); - dynStringPushBackStr ( message, XMLRPC_ARRAY_ETAG.str ); - dynStringPushBackStr ( message, XMLRPC_VALUE_ETAG.str ); -} - -static void timeToXml ( void *val, DynString *message ) -{ - PRINT_ERROR ( "timeToXml() : ERROR: Not yet implemented!\n" ); -} - -static void binaryToXml ( void *val, DynString *message ) -{ - PRINT_ERROR ( "binaryToXml() : ERROR: Not yet implemented!\n" ); -} - -int paramFromXml (DynString *message, XmlrpcParam *param, ParamContainerType container) -{ - PRINT_VVDEBUG ( "paramFromXml(), is_array : %s \n", (container == PARAM_CONTAINER_ARRAY)?"TRUE":"FALSE" ); - - int rc; - const char *c = dynStringGetCurrentData ( message ); - int len = dynStringGetLen ( message ); - int i = dynStringGetPoseIndicatorOffset ( message ); - const char *param_begin = NULL; - const char *param_end = NULL; - for (; i < len; i++, c++ ) - { - if ((container == PARAM_CONTAINER_NONE || container == PARAM_CONTAINER_ARRAY) - && len - i >= XMLRPC_VALUE_TAG.dim - && strncmp ( c, XMLRPC_VALUE_TAG.str, XMLRPC_VALUE_TAG.dim ) == 0 ) - { - c += XMLRPC_VALUE_TAG.dim; - i += XMLRPC_VALUE_TAG.dim; - param_begin = c; - break; - } - else if (container == PARAM_CONTAINER_STRUCT && len - i >= XMLRPC_MEMBER_TAG.dim - && strncmp ( c, XMLRPC_MEMBER_TAG.str, XMLRPC_MEMBER_TAG.dim ) == 0 ) - { - c += XMLRPC_MEMBER_TAG.dim; - i += XMLRPC_MEMBER_TAG.dim; - param_begin = c; - break; - } - else if (container == PARAM_CONTAINER_ARRAY && len - i >= XMLRPC_DATA_ETAG.dim - && strncmp ( c, XMLRPC_DATA_ETAG.str, XMLRPC_DATA_ETAG.dim ) == 0 ) - { - PRINT_VVDEBUG ( "paramFromXml() : reach end of array\n" ); - return -1; - } - else if (container == PARAM_CONTAINER_STRUCT && len - i >= XMLRPC_STRUCT_ETAG.dim - && strncmp ( c, XMLRPC_STRUCT_ETAG.str, XMLRPC_STRUCT_ETAG.dim ) == 0 ) - { - PRINT_VVDEBUG ( "paramFromXml() : reach end of struct\n" ); - return -1; - } - } - - if (param_begin == NULL) - { - PRINT_ERROR ( "paramFromXml() : no param tag found\n" ); - return -1; - } - - dynStringSetPoseIndicator ( message, i); - - switch (container) - { - case PARAM_CONTAINER_NONE: - case PARAM_CONTAINER_ARRAY: - { - rc = paramValueFromXml(message, param, container); - break; - } - case PARAM_CONTAINER_STRUCT: - { - param = arrayAddElem (param); - if (param == NULL) - return -1; - - rc = structMemberFromXml(message, param); - break; - } - default: - { - PRINT_ERROR("paramFromXml() : Invalid ParamContainerType specified\n"); - return -1; - } - } - - if (rc < 0) - return rc; - - c = dynStringGetCurrentData ( message ); - i = dynStringGetPoseIndicatorOffset ( message ); - - for (; i < len; i++, c++ ) - { - switch (container) - { - case PARAM_CONTAINER_NONE: - case PARAM_CONTAINER_ARRAY: - { - if ( len - i >= XMLRPC_VALUE_ETAG.dim && - strncmp ( c, XMLRPC_VALUE_ETAG.str, XMLRPC_VALUE_ETAG.dim ) == 0 ) - { - param_end = c; - c += XMLRPC_VALUE_ETAG.dim; - i += XMLRPC_VALUE_ETAG.dim; - goto paramFromXml_exit; - } - break; - } - case PARAM_CONTAINER_STRUCT: - { - if ( len - i >= XMLRPC_MEMBER_ETAG.dim && - strncmp ( c, XMLRPC_MEMBER_ETAG.str, XMLRPC_MEMBER_ETAG.dim ) == 0 ) - { - param_end = c; - c += XMLRPC_MEMBER_ETAG.dim; - i += XMLRPC_MEMBER_ETAG.dim; - goto paramFromXml_exit; - } - break; - } - default: - { - break; // Error - } - } - } - -paramFromXml_exit: - if ( param_end == NULL ) - { - PRINT_ERROR ( "arrayFromXml() : no param end tag found\n" ); - return -1; - } - - dynStringSetPoseIndicator ( message, i); - - return 0; -} - -int arrayFromXml(DynString *message, XmlrpcParam *param) -{ - PRINT_VVDEBUG ( "arrayFromXml()\n" ); - - const char *c = dynStringGetCurrentData ( message ); - int len = dynStringGetLen ( message ); - int i = dynStringGetPoseIndicatorOffset ( message ); - const char *data_init = NULL; - const char *data_end = NULL; - - for ( ; i < len; i++, c++ ) - { - if ( len - i >= XMLRPC_DATA_TAG.dim && - strncmp ( c, XMLRPC_DATA_TAG.str, XMLRPC_DATA_TAG.dim ) == 0 ) - { - c += XMLRPC_DATA_TAG.dim; - i += XMLRPC_DATA_TAG.dim; - data_init = c; - break; - } - else if ( len - i >= XMLRPC_DATA_NTAG.dim && - strncmp ( c, XMLRPC_DATA_NTAG.str, XMLRPC_DATA_NTAG.dim ) == 0 ) // Empty array: start-tag and end-tag codified in a single tag: empty-element tag - { - c += XMLRPC_DATA_NTAG.dim; - i += XMLRPC_DATA_NTAG.dim; - data_init = c; - data_end = c; - break; - } - } - - if ( data_init == NULL ) - { - PRINT_ERROR ( "arrayFromXml() : no data start-tag found in array\n" ); - return -1; - } - - if ( data_end == NULL ) - { - dynStringSetPoseIndicator ( message, i); - - while (paramFromXml (message, param, PARAM_CONTAINER_ARRAY ) != -1); - - c = dynStringGetCurrentData ( message ); - i = dynStringGetPoseIndicatorOffset ( message ); - - for ( ; i < len; i++, c++ ) - { - if ( len - i >= XMLRPC_DATA_ETAG.dim && - strncmp ( c, XMLRPC_DATA_ETAG.str, XMLRPC_DATA_ETAG.dim ) == 0 ) - { - data_end = c; - c += XMLRPC_DATA_ETAG.dim; - i += XMLRPC_DATA_ETAG.dim; - break; - } - } - - if ( data_end == NULL ) - { - PRINT_ERROR ( "arrayFromXml() : no data end-tag found in array\n" ); - return -1; - } - } - - dynStringSetPoseIndicator ( message, i ); - - return 0; -} - -int structFromXml(DynString *message, XmlrpcParam *param) -{ - PRINT_VVDEBUG ( "structFromXml()\n" ); - - while (paramFromXml (message, param, PARAM_CONTAINER_STRUCT ) != -1); - - return 0; -} - -int paramValueFromXml (DynString *message, XmlrpcParam *param, ParamContainerType container) -{ - int ret = 0; // Default return value=0 - const char *c = dynStringGetCurrentData ( message ); - int len = dynStringGetLen ( message ); - int i = dynStringGetPoseIndicatorOffset ( message ); - - XmlrpcParamType p_type = XMLRPC_PARAM_UNKNOWN; - const char *value_init = NULL; - const char *type_init = NULL; - const char *type_end = NULL; - - for ( ; i < len; i++, c++ ) - { - if ( len - i >= XMLRPC_BOOLEAN_TAG.dim && - strncmp ( c, XMLRPC_BOOLEAN_TAG.str, XMLRPC_BOOLEAN_TAG.dim ) == 0 ) - { - c += XMLRPC_BOOLEAN_TAG.dim; - i += XMLRPC_BOOLEAN_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_BOOL; - break; - } - else if ( len - i >= XMLRPC_I4_TAG.dim && - strncmp ( c, XMLRPC_I4_TAG.str, XMLRPC_I4_TAG.dim ) == 0 ) - { - c += XMLRPC_I4_TAG.dim; - i += XMLRPC_I4_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_INT; - break; - } - else if ( len - i >= XMLRPC_INT_TAG.dim && - strncmp ( c, XMLRPC_INT_TAG.str, XMLRPC_INT_TAG.dim ) == 0 ) - { - c += XMLRPC_INT_TAG.dim; - i += XMLRPC_INT_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_INT; - break; - } - else if ( len - i >= XMLRPC_DOUBLE_TAG.dim && - strncmp ( c, XMLRPC_DOUBLE_TAG.str, XMLRPC_DOUBLE_TAG.dim ) == 0 ) - { - c += XMLRPC_DOUBLE_TAG.dim; - i += XMLRPC_DOUBLE_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_DOUBLE; - break; - } - else if ( len - i >= XMLRPC_STRING_TAG.dim && - strncmp ( c, XMLRPC_STRING_TAG.str, XMLRPC_STRING_TAG.dim ) == 0 ) - { - c += XMLRPC_STRING_TAG.dim; - i += XMLRPC_STRING_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_STRING; - break; - } - else if ( len - i >= XMLRPC_ARRAY_TAG.dim && - strncmp ( c, XMLRPC_ARRAY_TAG.str, XMLRPC_ARRAY_TAG.dim ) == 0 ) - { - c += XMLRPC_ARRAY_TAG.dim; - i += XMLRPC_ARRAY_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_ARRAY; - break; - } - else if ( len - i >= XMLRPC_DATETIME_TAG.dim && - strncmp ( c, XMLRPC_DATETIME_TAG.str, XMLRPC_DATETIME_TAG.dim ) == 0 ) - { - PRINT_ERROR ( "paramFromXml() : ERROR: Tag %s not yet implemented!\n", XMLRPC_DATETIME_TAG.str ); - ret=-1; - - c += XMLRPC_DATETIME_TAG.dim; - i += XMLRPC_DATETIME_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_DATETIME; - break; - } - else if ( len - i >= XMLRPC_BASE64_TAG.dim && - strncmp ( c, XMLRPC_BASE64_TAG.str, XMLRPC_BASE64_TAG.dim ) == 0 ) - { - PRINT_ERROR ( "paramFromXml() : ERROR: Tag %s not yet implemented!\n", XMLRPC_BASE64_TAG.str ); - ret=-1; - - c += XMLRPC_BASE64_TAG.dim; - i += XMLRPC_BASE64_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_BINARY; - break; - } - else if ( len - i >= XMLRPC_STRUCT_TAG.dim && - strncmp ( c, XMLRPC_STRUCT_TAG.str, XMLRPC_STRUCT_TAG.dim ) == 0 ) - { - c += XMLRPC_STRUCT_TAG.dim; - i += XMLRPC_STRUCT_TAG.dim; - type_init = c; - p_type = XMLRPC_PARAM_STRUCT; - break; - } - else if ( len - i >= XMLRPC_STRUCT_NTAG.dim && - strncmp ( c, XMLRPC_STRUCT_NTAG.str, XMLRPC_STRUCT_NTAG.dim ) == 0 ) // Empty-structure tag - { - c += XMLRPC_STRUCT_NTAG.dim; - i += XMLRPC_STRUCT_NTAG.dim; - type_init = c; - type_end = c; // Indicate that the struct end has been found, so it is an empty structure - p_type = XMLRPC_PARAM_STRUCT; - break; - } - else if ( len - i >= XMLRPC_VALUE_ETAG.dim && - strncmp ( c, XMLRPC_VALUE_ETAG.str, XMLRPC_VALUE_ETAG.dim ) == 0 ) - { - value_init = dynStringGetCurrentData ( message ); - i = dynStringGetPoseIndicatorOffset ( message ); - break; - } - } - - PRINT_VVDEBUG("paramFromXml() : Param type %d \n", p_type ); - - // Update pose indicator (useful in case it is an array parameter) - dynStringSetPoseIndicator ( message, i ); - - const char *str_init = NULL; - int str_len = 0; - - switch (p_type) - { - case XMLRPC_PARAM_BOOL: - { - int val; - if ( sscanf ( c, "%d", &val ) == 1 ) - { - if (container == PARAM_CONTAINER_ARRAY) - xmlrpcParamArrayPushBackBool ( param, val ); - else - xmlrpcParamSetBool ( param, val ); - } - else - { - PRINT_ERROR ( "paramFromXml() : not valid value\n" ); - xmlrpcParamSetUnknown ( param ); - return -1; - } - break; - } - case XMLRPC_PARAM_INT: - { - int val; - if ( sscanf ( c, "%d", &val ) == 1 ) - { - if (container == PARAM_CONTAINER_ARRAY) - xmlrpcParamArrayPushBackInt ( param, val ); - else - xmlrpcParamSetInt ( param, val ); - } - else - { - PRINT_ERROR ( "paramFromXml() : not valid value\n" ); - xmlrpcParamSetUnknown ( param ); - return -1; - } - break; - } - case XMLRPC_PARAM_DOUBLE: - { - double val; - if ( sscanf ( c, "%lf", &val ) == 1 ) - { - if (container == PARAM_CONTAINER_ARRAY) - xmlrpcParamArrayPushBackDouble ( param, val ); - else - xmlrpcParamSetDouble ( param, val ); - } - else - { - PRINT_ERROR ( "paramFromXml() : not valid value\n" ); - xmlrpcParamSetUnknown ( param ); - return -1; - } - break; - } - case XMLRPC_PARAM_STRING: - { - str_init = type_init; - break; - } - case XMLRPC_PARAM_ARRAY: - { - if (container == PARAM_CONTAINER_ARRAY) - // Pushed new param array: start to parse for this new element - param = xmlrpcParamArrayPushBackArray ( param ); - else - xmlrpcParamSetArray ( param ); - - if (param == NULL) - return -1; - - int rc = arrayFromXml(message, param); - if (rc < 0) - return rc; - - break; - } - case XMLRPC_PARAM_DATETIME: - { - // TODO Implement me! - break; - } - case XMLRPC_PARAM_BINARY: - { - // TODO Implement me! - break; - } - case XMLRPC_PARAM_STRUCT: - { - if (container == PARAM_CONTAINER_ARRAY) - // Pushed new param array: start to parse for this new element - param = xmlrpcParamArrayPushBackStruct ( param ); - else - xmlrpcParamSetStruct ( param ); - - if (param == NULL) - return -1; - - if(type_end == NULL) // If it is not an empty structure, so parse the xml text - { - int rc = structFromXml(message, param); - if (rc < 0) - return rc; - } - - break; - } - default: - { - // Maybe string without and tabs - str_init = value_init; - break; - } - } - - c = dynStringGetCurrentData ( message ); - i = dynStringGetPoseIndicatorOffset ( message ); - for ( ; i < len && type_end == NULL; c++ ) - { - if ( p_type == XMLRPC_PARAM_BOOL && len - i >= XMLRPC_BOOLEAN_ETAG.dim && - strncmp ( c, XMLRPC_BOOLEAN_ETAG.str, XMLRPC_BOOLEAN_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_BOOLEAN_ETAG.dim; - i += XMLRPC_BOOLEAN_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_INT && len - i >= XMLRPC_I4_ETAG.dim && - strncmp ( c, XMLRPC_I4_ETAG.str, XMLRPC_I4_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_I4_ETAG.dim; - i += XMLRPC_I4_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_INT && len - i >= XMLRPC_INT_ETAG.dim && - strncmp ( c, XMLRPC_INT_ETAG.str, XMLRPC_INT_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_INT_ETAG.dim; - i += XMLRPC_INT_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_STRING && len - i >= XMLRPC_STRING_ETAG.dim && - strncmp ( c, XMLRPC_STRING_ETAG.str, XMLRPC_STRING_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_STRING_ETAG.dim; - i += XMLRPC_STRING_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_ARRAY && len - i >= XMLRPC_ARRAY_ETAG.dim && - strncmp ( c, XMLRPC_ARRAY_ETAG.str, XMLRPC_ARRAY_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_ARRAY_ETAG.dim; - i += XMLRPC_ARRAY_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_STRUCT && len - i >= XMLRPC_STRUCT_ETAG.dim && - strncmp ( c, XMLRPC_STRUCT_ETAG.str, XMLRPC_STRUCT_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_STRUCT_ETAG.dim; - i += XMLRPC_STRUCT_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_DATETIME && len - i >= XMLRPC_DATETIME_ETAG.dim && - strncmp ( c, XMLRPC_DATETIME_ETAG.str, XMLRPC_DATETIME_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_DATETIME_ETAG.dim; - i += XMLRPC_DATETIME_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_BINARY && len - i >= XMLRPC_BASE64_ETAG.dim && - strncmp ( c, XMLRPC_BASE64_ETAG.str, XMLRPC_BASE64_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_BASE64_ETAG.dim; - i += XMLRPC_BASE64_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_STRUCT && len - i >= XMLRPC_STRUCT_ETAG.dim && - strncmp ( c, XMLRPC_STRUCT_ETAG.str, XMLRPC_STRUCT_ETAG.dim ) == 0 ) - { - type_end = c; - - c += XMLRPC_STRUCT_ETAG.dim; - i += XMLRPC_STRUCT_ETAG.dim; - } - else if ( p_type == XMLRPC_PARAM_UNKNOWN && len - i >= XMLRPC_VALUE_ETAG.dim && - strncmp ( c, XMLRPC_VALUE_ETAG.str, XMLRPC_VALUE_ETAG.dim ) == 0 ) - { - type_end = c; - - // End value tag is feeded outside - } - else - { - str_len++; - i++; // Only increase the string index if an end tag has not been found - } - } - - // Update pose indicator (useful in case it is an array parameter) - dynStringSetPoseIndicator ( message, i ); - - if ( type_end == NULL ) - { - PRINT_ERROR ( "paramFromXml() : no end type tag found\n" ); - return -1; - } - - if( p_type == XMLRPC_PARAM_STRING || p_type == XMLRPC_PARAM_UNKNOWN ) - { - if (container == PARAM_CONTAINER_ARRAY) - xmlrpcParamArrayPushBackStringN ( param, str_init, str_len ); - else - xmlrpcParamSetStringN ( param, str_init, str_len ); - } - - return ret; -} - -int structMemberFromXml ( DynString *message, XmlrpcParam *param) -{ - int rc; - int len = dynStringGetLen ( message ); - const char *c = dynStringGetCurrentData ( message ); - int i = dynStringGetPoseIndicatorOffset ( message ); - - const char *name_begin = NULL; - const char *name_end = NULL; - for ( ; i < len; i++, c++ ) - { - if ( len - i >= XMLRPC_NAME_TAG.dim && - strncmp ( c, XMLRPC_NAME_TAG.str, XMLRPC_NAME_TAG.dim ) == 0 ) - { - c += XMLRPC_NAME_TAG.dim; - i += XMLRPC_NAME_TAG.dim; - name_begin = c; - break; - } - } - - for ( ; i < len; i++, c++ ) - { - if ( len - i >= XMLRPC_NAME_ETAG.dim && - strncmp ( c, XMLRPC_NAME_ETAG.str, XMLRPC_NAME_ETAG.dim ) == 0 ) - { - name_end = c; - c += XMLRPC_NAME_ETAG.dim; - i += XMLRPC_NAME_ETAG.dim; - break; - } - } - - if (name_begin == NULL || name_end == NULL) - { - PRINT_ERROR ( "paramMemberFromXml() : no name type tag found\n" ); - return -1; - } - - size_t name_len = name_end - name_begin; - param->member_name = (char *)malloc(name_len + 1); - if (param->member_name == NULL) - { - PRINT_ERROR ( "paramMemberFromXml() : Can't allocate memory\n" ); - return -1; - } - - memcpy(param->member_name, name_begin, name_len); - param->member_name[name_len] = '\0'; - - const char *value_begin = NULL; - const char *value_end = NULL; - for ( ; i < len; i++, c++ ) - { - if ( len - i >= XMLRPC_VALUE_TAG.dim && - strncmp ( c, XMLRPC_VALUE_TAG.str, XMLRPC_VALUE_TAG.dim ) == 0 ) - { - c += XMLRPC_VALUE_TAG.dim; - i += XMLRPC_VALUE_TAG.dim; - value_begin = c; - break; - } - } - - if (value_begin == NULL) - { - PRINT_ERROR ( "paramMemberFromXml() : no value tag found\n" ); - return -1; - } - - dynStringSetPoseIndicator ( message, i); - - rc = paramValueFromXml(message, param, PARAM_CONTAINER_STRUCT ); - if (rc < 0) - return rc; - - c = dynStringGetCurrentData ( message ); - i = dynStringGetPoseIndicatorOffset ( message ); - - for ( ; i < len; i++, c++ ) - { - if ( len - i >= XMLRPC_VALUE_ETAG.dim && - strncmp ( c, XMLRPC_VALUE_ETAG.str, XMLRPC_VALUE_ETAG.dim ) == 0 ) - { - value_end = c; - c += XMLRPC_VALUE_ETAG.dim; - i += XMLRPC_VALUE_ETAG.dim; - break; - } - } - - if ( value_end == NULL ) - { - PRINT_ERROR ( "paramMemberFromXml() : no value end tag found\n" ); - return -1; - } - - dynStringSetPoseIndicator ( message, i); - - return 0; -} - -XmlrpcParam * arrayAddElem ( XmlrpcParam *param ) -{ - if ( param->type != XMLRPC_PARAM_ARRAY && param->type != XMLRPC_PARAM_STRUCT ) - { - PRINT_ERROR ( "arrayAddElem() : Not array or struct type param \n" ); - return NULL; - } - - if ( param->array_n_elem == param->array_max_elem ) - { - PRINT_VVDEBUG ( "arrayAddElem() : reallocate memory\n" ); - XmlrpcParam *new_param = ( XmlrpcParam * ) realloc ( param->data.as_array, - ( XMLRPC_ARRAY_GROW_RATE * param->array_max_elem ) * sizeof ( XmlrpcParam ) ); - if ( new_param == NULL ) - { - PRINT_ERROR ( "arrayAddElem() : Can't allocate more memory\n" ); - return NULL; - } - param->array_max_elem *= XMLRPC_ARRAY_GROW_RATE; - param->data.as_array = new_param; - } - - XmlrpcParam *ret = ¶m->data.as_array[param->array_n_elem++]; - xmlrpcParamInit(ret); - return ret; -} - - -int xmlrpcParamGetBool( XmlrpcParam *param ) -{ - return param->data.as_bool?1:0; -} - -int32_t xmlrpcParamGetInt( XmlrpcParam *param ) -{ - return param->data.as_int; -} - -double xmlrpcParamGetDouble( XmlrpcParam *param ) -{ - return param->data.as_double; -} -char *xmlrpcParamGetString( XmlrpcParam *param ) -{ - return param->data.as_string; -} - -void xmlrpcParamSetUnknown ( XmlrpcParam *param ) -{ - PRINT_VVDEBUG ( "xmlrpcParamSetUnknown()\n" ); - - param->type = XMLRPC_PARAM_UNKNOWN; -} - -void xmlrpcParamSetBool ( XmlrpcParam *param, int val ) -{ - PRINT_VVDEBUG ( "xmlrpcSetBool()\n" ); - - param->type = XMLRPC_PARAM_BOOL; - param->data.as_bool = ( ( val != 0 ) ?1:0 ); - PRINT_VDEBUG( "xmlrpcSetBool() : Set: %s\n", param->data.as_bool?"TRUE":"FALSE" ); -} - -void xmlrpcParamSetInt ( XmlrpcParam *param, int32_t val ) -{ - PRINT_VVDEBUG ( "xmlrpcSetInt()\n" ); - - param->type = XMLRPC_PARAM_INT; - param->data.as_int = val; - PRINT_VDEBUG( "xmlrpcSetInt() : Set: %d\n", param->data.as_int ); -} - -void xmlrpcParamSetDouble ( XmlrpcParam *param, double val ) -{ - PRINT_VVDEBUG ( "xmlrpcSetDouble()\n" ); - - param->type = XMLRPC_PARAM_DOUBLE; - param->data.as_double = val; - PRINT_VDEBUG ( "xmlrpcSetDouble() : Set: %f\n", param->data.as_double ); -} - -int xmlrpcParamSetString ( XmlrpcParam *param, const char *val ) -{ - PRINT_VVDEBUG ( "xmlrpcSetString()\n" ); - - int str_len = strlen ( val ); - return xmlrpcParamSetStringN ( param, val, str_len ); -} - -int xmlrpcParamSetStringN ( XmlrpcParam *param, const char *val, int n ) -{ - PRINT_VVDEBUG ( "xmlrpcSetStringN()\n" ); - - param->type = XMLRPC_PARAM_STRING; - param->data.as_string = ( char * ) malloc ( ( n + 1 ) *sizeof ( char ) ); - if ( param->data.as_string == NULL ) - { - PRINT_ERROR ( "xmlrpcSetStringN() : Can't allocate memory\n" ); - return -1; - } - int i = 0; - const char *c = val; - for( ; i < n; i++, c++) - { - if ( n - i >= 4 && strncmp ( c, "<", 4 ) == 0 ) - param->data.as_string[i] = '<'; - else if ( n - i >= 4 && strncmp ( c, ">", 4 ) == 0 ) - param->data.as_string[i] = '>'; - else if ( n - i >= 5 && strncmp ( c, "&", 5 ) == 0 ) - param->data.as_string[i] = '&'; - else if ( n - i >= 6 && strncmp ( c, "'", 6 ) == 0 ) - param->data.as_string[i] = '\''; - else if ( n - i >= 6 && strncmp ( c, """, 6 ) == 0 ) - param->data.as_string[i] = '\"'; - else - param->data.as_string[i] = *c; - } - param->data.as_string[i] = '\0'; - - PRINT_VDEBUG ( "xmlrpcSetStringN() : Set: %s\n", param->data.as_string ); - return 0; -} - -int xmlrpcParamArrayGetSize( XmlrpcParam *param ) -{ - if( param->type == XMLRPC_PARAM_ARRAY) - return param->array_n_elem; - else - return -1; -} - -XmlrpcParam *xmlrpcParamArrayGetParamAt( XmlrpcParam *param, int idx ) -{ - if( param->type == XMLRPC_PARAM_ARRAY || param->type == XMLRPC_PARAM_STRUCT ) - { - if( idx >= 0 && idx < param->array_n_elem ) - return &(param->data.as_array[idx]); - else - { - PRINT_ERROR ( "xmlrpcParamArrayGetParamAt() : The index to access the array (or structure) element must be between 0 and the number of elements-1.\n" ); - return NULL; - } - } - else - { - PRINT_ERROR ( "xmlrpcParamArrayGetParamAt() : Only elements of a structure or an array can be accessed through index.\n" ); - return NULL; - } -} - -int xmlrpcParamSetArray ( XmlrpcParam *param ) -{ - PRINT_VVDEBUG ( "xmlrpcSetArray()\n" ); - param->type = XMLRPC_PARAM_ARRAY; - - param->data.as_array = ( XmlrpcParam * ) malloc ( XMLRPC_ARRAY_INIT_SIZE*sizeof ( XmlrpcParam ) ); - if ( param->data.as_array == NULL ) - { - PRINT_ERROR ( "xmlrpcSetArray() : Can't allocate memory\n" ); - param->array_max_elem = param->array_n_elem = 0; - return -1; - } - param->array_n_elem = 0; - param->array_max_elem = XMLRPC_ARRAY_INIT_SIZE; - return 0; -} - -int xmlrpcParamSetStruct( XmlrpcParam *param ) -{ - PRINT_VVDEBUG ( "xmlrpcSetArray()\n" ); - param->type = XMLRPC_PARAM_STRUCT; - - param->data.as_array = ( XmlrpcParam * ) malloc ( XMLRPC_ARRAY_INIT_SIZE*sizeof ( XmlrpcParam ) ); - if ( param->data.as_array == NULL ) - { - PRINT_ERROR ( "xmlrpcSetArray() : Can't allocate memory\n" ); - param->array_max_elem = param->array_n_elem = 0; - return -1; - } - param->array_n_elem = 0; - param->array_max_elem = XMLRPC_ARRAY_INIT_SIZE; - return 0; -} - - -XmlrpcParamType xmlrpcParamGetType ( XmlrpcParam *param ) -{ - return param->type; -} - -XmlrpcParam * xmlrpcParamArrayPushBackBool ( XmlrpcParam *param, int val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackBool()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - xmlrpcParamSetBool ( new_param, val ); - return new_param; -} - -XmlrpcParam * xmlrpcParamArrayPushBackInt ( XmlrpcParam *param, int32_t val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackInt()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - xmlrpcParamSetInt ( new_param, val ); - return new_param; -} - -XmlrpcParam * xmlrpcParamArrayPushBackDouble ( XmlrpcParam *param, double val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackDouble()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - xmlrpcParamSetDouble ( new_param, val ); - return new_param; -} - -XmlrpcParam * xmlrpcParamArrayPushBackString ( XmlrpcParam *param, const char *val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackString()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - xmlrpcParamSetString ( new_param, val ); - return new_param; -} - -XmlrpcParam * xmlrpcParamArrayPushBackStringN ( XmlrpcParam *param, const char *val, int n ) -{ - PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackStringN()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - xmlrpcParamSetStringN ( new_param, val, n ); - return new_param; -} - -XmlrpcParam *xmlrpcParamArrayPushBackArray ( XmlrpcParam *param ) -{ - PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackArray()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - xmlrpcParamSetArray ( new_param ); - return new_param; -} - -XmlrpcParam * xmlrpcParamArrayPushBackStruct ( XmlrpcParam *param ) -{ - PRINT_VVDEBUG ( "xmlrpcParamArrayPushBackStruct()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - xmlrpcParamSetStruct ( new_param ); - return new_param; -} - -XmlrpcParam * xmlrpcParamStructGetParam( XmlrpcParam *param, const char *name ) -{ - if ( param->type != XMLRPC_PARAM_STRUCT ) - { - PRINT_ERROR ( "xmlrpcParamStructGetParam() : Not a struct type param \n" ); - return NULL; - } - - int it = 0; - for (; it < param->array_n_elem; it++) - { - XmlrpcParam *param_arr = ¶m->data.as_array[it]; - if (strcmp(param_arr->member_name, name) == 0) - return param_arr; - } - - return NULL; -} - -XmlrpcParam * xmlrpcParamStructPushBackBool( XmlrpcParam *param, const char *name, int val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamStructPushBackBool()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - paramSetMemberName(new_param, name); - xmlrpcParamSetBool ( new_param, val ); - return new_param; -} - -XmlrpcParam * xmlrpcParamStructPushBackInt( XmlrpcParam *param, const char *name, int32_t val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamStructPushBackInt()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - paramSetMemberName(new_param, name); - xmlrpcParamSetInt ( new_param, val ); - return new_param; -} - -XmlrpcParam * xmlrpcParamStructPushBackDouble( XmlrpcParam *param, const char *name, double val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamStructPushBackDouble()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - paramSetMemberName(new_param, name); - xmlrpcParamSetDouble ( new_param, val ); - return new_param; -} - -XmlrpcParam * xmlrpcParamStructPushBackString( XmlrpcParam *param, const char *name, const char *val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamStructPushBackString()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - paramSetMemberName(new_param, name); - xmlrpcParamSetString ( new_param, val ); - return new_param; -} - -XmlrpcParam * xmlrpcParamStructPushBackStringN( XmlrpcParam *param, const char *name, const char *val, int n ) -{ - PRINT_VVDEBUG ( "xmlrpcParamStructPushBackStringN()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - paramSetMemberName(new_param, name); - xmlrpcParamSetStringN ( new_param, val, n ); - return new_param; -} - -XmlrpcParam * xmlrpcParamStructPushBackArray( XmlrpcParam *param, const char *name ) -{ - PRINT_VVDEBUG ( "xmlrpcParamStructPushBackArray()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - paramSetMemberName(new_param, name); - xmlrpcParamSetArray ( new_param ); - return new_param; -} - -XmlrpcParam * xmlrpcParamStructPushBackStruct ( XmlrpcParam *param, const char *name ) -{ - PRINT_VVDEBUG ( "xmlrpcParamStructPushBackStruct()\n" ); - XmlrpcParam *new_param = arrayAddElem ( param ); - if ( new_param == NULL ) - return NULL; - - paramSetMemberName(new_param, name); - xmlrpcParamSetStruct ( new_param ); - return new_param; -} - -int paramSetMemberName ( XmlrpcParam *param, const char *name ) -{ - param->member_name = (char *)malloc(strlen(name) + 1); - if (param->member_name == NULL) - return -1; - strcpy(param->member_name, name); - - return 0; -} - -void xmlrpcParamInit( XmlrpcParam *param ) -{ - param->type = XMLRPC_PARAM_UNKNOWN; - param->member_name = NULL; - memset(param->data.opaque, 0, sizeof(param->data.opaque)); - param->array_n_elem = -1; - param->array_max_elem = -1; -} - -void xmlrpcParamRelease ( XmlrpcParam *param ) -{ - PRINT_VVDEBUG ( "xmlrpcParamReleaseData()\n" ); - - free(param->member_name); - - switch ( param->type ) - { - case XMLRPC_PARAM_BOOL: - case XMLRPC_PARAM_INT: - case XMLRPC_PARAM_DOUBLE: - break; - - case XMLRPC_PARAM_STRING: - if ( param->data.as_string != NULL ) - { - free ( param->data.as_string ); - param->data.as_string = NULL; - } - break; - - case XMLRPC_PARAM_ARRAY: - case XMLRPC_PARAM_STRUCT: - if ( param->data.as_array != NULL ) - { - int i; - for ( i = 0; i< param->array_n_elem; i++ ) - xmlrpcParamRelease ( & ( param->data.as_array[i] ) ); - free ( param->data.as_array ); - param->data.as_array = NULL; - } - param->array_n_elem = 0; - param->array_max_elem = 0; - break; - - case XMLRPC_PARAM_DATETIME: /* WARNING: Currently unsupported */ - PRINT_ERROR ( "xmlrpcParamReleaseData() : ERROR: Parameter type datetime not yet supported\n" ); - break; - case XMLRPC_PARAM_BINARY: /* WARNING: Currently unsupported */ - PRINT_ERROR ( "xmlrpcParamReleaseData() : ERROR: Parameter type binary not yet supported\n" ); - break; - case XMLRPC_PARAM_UNKNOWN: - break; - default: - PRINT_VVDEBUG ( "xmlrpcParamReleaseData() : Unknown parameter \n" ); - break; - } -} - -void xmlrpcParamToXml ( XmlrpcParam *param, DynString *message ) -{ - PRINT_VVDEBUG ( "xmlrpcParamToXml()\n" ); - - int struct_member = 0; - if (param->member_name != NULL) - { - dynStringPushBackStr ( message, XMLRPC_MEMBER_TAG.str ); - dynStringPushBackStr ( message, XMLRPC_NAME_TAG.str ); - dynStringPushBackStr ( message, param->member_name ); - dynStringPushBackStr ( message, XMLRPC_NAME_ETAG.str ); - struct_member = 1; - } - - switch ( param->type ) - { - case XMLRPC_PARAM_BOOL: - boolToXml ( param->data.as_bool, message ); - break; - case XMLRPC_PARAM_INT: - intToXml ( param->data.as_int, message ); - break; - case XMLRPC_PARAM_DOUBLE: - doubleToXml ( param->data.as_double, message ); - break; - case XMLRPC_PARAM_STRING: - stringToXml ( param->data.as_string, message ); - break; - case XMLRPC_PARAM_ARRAY: - arrayToXml ( param, message ); - break; - case XMLRPC_PARAM_STRUCT: - structToXml ( param, message ); - break; - case XMLRPC_PARAM_DATETIME: - timeToXml ( param->data.as_time, message ); - break; - case XMLRPC_PARAM_BINARY: - binaryToXml ( param->data.as_binary, message ); - break; - case XMLRPC_PARAM_UNKNOWN: - break; - default: - PRINT_ERROR ( "xmlrpcParamToXml() : Unsupported type\n" ); - break; - } - - if (struct_member) - dynStringPushBackStr ( message, XMLRPC_MEMBER_ETAG.str ); -} - -int xmlrpcParamFromXml ( DynString *message, XmlrpcParam *param ) -{ - PRINT_VVDEBUG ( "xmlrpcParamFromXml()\n" ); - - /* Save position indicator: it will be restored */ - int initial_pos_idx = dynStringGetPoseIndicatorOffset ( message ); - dynStringRewindPoseIndicator ( message ); - - int ret = paramFromXml ( message, param, PARAM_CONTAINER_NONE ); - - /* Restore position indicator */ - dynStringSetPoseIndicator ( message, initial_pos_idx ); - - return ret; -} - -static void paramArrayPrint( XmlrpcParam *param, char *head, int is_struct_member) -{ - int elem_ind; - - if ( param->data.as_array != NULL) - { - for ( elem_ind = 0; elem_ind < param->array_n_elem; elem_ind++ ) - { - char elem_head[20]; - snprintf(elem_head, 20, "Elem [%d]",elem_ind); - paramPrint( & ( param->data.as_array[elem_ind] ), elem_head , is_struct_member); - } - } - else - fprintf(cRosOutStreamGet(),"Empty array/struct\n"); -} - -static void paramPrint( XmlrpcParam *param, char *head, int is_struct_member) -{ - fprintf(cRosOutStreamGet(),"%s ", head); - - if(is_struct_member) - fprintf(cRosOutStreamGet(),"Member name: [%s] ", (param->member_name != NULL)? param->member_name:"NULL"); - - switch ( param->type ) - { - case XMLRPC_PARAM_BOOL: - fprintf(cRosOutStreamGet(),"Type : boolean Value : [%s]\n", param->data.as_bool?"TRUE":"FALSE" ); - break; - case XMLRPC_PARAM_INT: - fprintf(cRosOutStreamGet(),"Type : integer Value : [%d]\n", param->data.as_int ); - break; - case XMLRPC_PARAM_DOUBLE: - fprintf(cRosOutStreamGet(),"Type : double Value : [%f]\n", param->data.as_double ); - break; - case XMLRPC_PARAM_STRING: - fprintf(cRosOutStreamGet(),"Type : string Value : [%s]\n", (param->data.as_string != NULL)?param->data.as_string:"NULL" ); - break; - case XMLRPC_PARAM_ARRAY: - fprintf(cRosOutStreamGet(),"Type : array\n======== Array start ========\n"); - paramArrayPrint( param, head, 0); - fprintf(cRosOutStreamGet(),"========= Array end =========\n\n"); - break; - case XMLRPC_PARAM_STRUCT: - fprintf(cRosOutStreamGet(),"Type : struct\n======== Struct start ========\n"); - paramArrayPrint( param, head, 1); - fprintf(cRosOutStreamGet(),"========= Struct end =========\n\n"); - break; - - case XMLRPC_PARAM_DATETIME: /* WARNING: Currently unsupported */ - PRINT_ERROR ( "\nxmlrpcParamPrint() : Printing of parameter type datetime is not supported yet\n" ); - break; - case XMLRPC_PARAM_BINARY: /* WARNING: Currently unsupported */ - PRINT_ERROR ( "\nxmlrpcParamPrint() : Printing of parameter type binary is not supported.\n" ); - break; - default: - PRINT_ERROR ( "\nxmlrpcParamPrint() : Unknown parameter type\n" ); - break; - } -} - -void xmlrpcParamPrint( XmlrpcParam *param ) -{ - paramPrint( param, "XMLRPC parameter.", 0 ); // last parameter = 0 means that this parameter is not the member of a structure -} - -XmlrpcParam * xmlrpcParamNew(void) -{ - XmlrpcParam *ret = (XmlrpcParam *)malloc(sizeof(XmlrpcParam)); - if (ret == NULL) - return NULL; - - xmlrpcParamInit(ret); - - return ret; -} - -void xmlrpcParamFree( XmlrpcParam *param ) -{ - if (param == NULL) - return; - - xmlrpcParamRelease(param); - free(param); -} - -XmlrpcParam * xmlrpcParamClone(XmlrpcParam *source) -{ - XmlrpcParam *ret = (XmlrpcParam *)malloc(sizeof(XmlrpcParam)); - if (ret == NULL) - return NULL; - - int rc = xmlrpcParamCopy(ret, source); - if (rc == -1) - { - free(ret); - return NULL; - } - - return ret; -} - -int xmlrpcParamCopy(XmlrpcParam *dest, XmlrpcParam *source) -{ - int ret_val; - - memcpy(dest, source, sizeof(XmlrpcParam)); - if (source->member_name != NULL) - { - dest->member_name = (char *)malloc(strlen(source->member_name) + 1); - if(dest->member_name != NULL) - strcpy(dest->member_name, source->member_name); - else - return -1; - } - - ret_val = 0; // Default return value = 0 (success) - switch ( source->type ) - { - case XMLRPC_PARAM_BOOL: - case XMLRPC_PARAM_INT: - case XMLRPC_PARAM_DOUBLE: - break; - case XMLRPC_PARAM_STRING: - dest->data.as_string = (char *)malloc(strlen(source->data.as_string) + 1); - if (dest->data.as_string != NULL) - strcpy(dest->data.as_string, source->data.as_string); - else - ret_val = -1; // Failure allocating memory - break; - case XMLRPC_PARAM_ARRAY: - case XMLRPC_PARAM_STRUCT: - dest->data.as_array = (XmlrpcParam *)calloc(source->array_n_elem, sizeof(XmlrpcParam)); - if (dest->data.as_array != NULL) - { - int it; - for (it = 0; it < source->array_n_elem && ret_val == 0; it++) - ret_val = xmlrpcParamCopy(&dest->data.as_array[it], &source->data.as_array[it]); - } - else - ret_val = -1; - break; - case XMLRPC_PARAM_DATETIME: - case XMLRPC_PARAM_BINARY: - PRINT_ERROR ( "xmlrpcParamToXml() : Unsupported type in source XmlrpcParam (binary or datetime)\n" ); - ret_val = -1; - break; - case XMLRPC_PARAM_UNKNOWN: - break; - default: - PRINT_ERROR ( "xmlrpcParamToXml() : Unsupported type in source XmlrpcParam\n" ); - ret_val = -1; - } - - if(ret_val != 0) // If failure, free param before exit - xmlrpcParamRelease(dest); - - return(ret_val); -} diff --git a/src/hal/components/cros/src/xmlrpc_params_vector.c b/src/hal/components/cros/src/xmlrpc_params_vector.c deleted file mode 100644 index 86a6314a..00000000 --- a/src/hal/components/cros/src/xmlrpc_params_vector.c +++ /dev/null @@ -1,171 +0,0 @@ -#include - -#include "xmlrpc_params_vector.h" -#include "cros_defs.h" -#include "cros_log.h" - -enum { XMLRPC_VECTOR_INIT_SIZE = 4, XMLRPC_VECTOR_GROW_RATE = 2 }; - -void xmlrpcParamVectorInit ( XmlrpcParamVector *p_vec ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorInit()\n" ); - - p_vec->data = NULL; - p_vec->size = 0; - p_vec->max = 0; -} - -void xmlrpcParamVectorRelease ( XmlrpcParamVector *p_vec ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorRelease()\n" ); - - if ( p_vec->data == NULL ) - return; - - int i; - for ( i = 0; i < p_vec->size; i++ ) - xmlrpcParamRelease ( & ( p_vec->data[i] ) ); - - free ( p_vec->data ); - p_vec->data = NULL; - - p_vec->size = p_vec->max = 0; -} - -int xmlrpcParamVectorPushBackBool ( XmlrpcParamVector *p_vec, int val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackBool()\n" ); - - XmlrpcParam param; - xmlrpcParamInit(¶m); - xmlrpcParamSetBool ( ¶m, val ); - return xmlrpcParamVectorPushBack ( p_vec, ¶m ); -} - -int xmlrpcParamVectorPushBackInt ( XmlrpcParamVector *p_vec, int32_t val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackInt()\n" ); - - XmlrpcParam param; - xmlrpcParamInit(¶m); - xmlrpcParamSetInt ( ¶m, val ); - return xmlrpcParamVectorPushBack ( p_vec, ¶m ); -} - -int xmlrpcParamVectorPushBackDouble ( XmlrpcParamVector *p_vec, double val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackDouble()\n" ); - - XmlrpcParam param; - xmlrpcParamInit(¶m); - xmlrpcParamSetDouble ( ¶m, val ); - return xmlrpcParamVectorPushBack ( p_vec, ¶m ); -} - -int xmlrpcParamVectorPushBackString ( XmlrpcParamVector *p_vec, const char *val ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackString()\n" ); - - XmlrpcParam param; - xmlrpcParamInit(¶m); - xmlrpcParamSetString ( ¶m, val ); - return xmlrpcParamVectorPushBack ( p_vec, ¶m ); -} - -int xmlrpcParamVectorPushBackArray ( XmlrpcParamVector *p_vec ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackArray()\n" ); - - XmlrpcParam param; - xmlrpcParamInit(¶m); - xmlrpcParamSetArray ( ¶m ); - return xmlrpcParamVectorPushBack ( p_vec, ¶m ); -} - -int xmlrpcParamVectorPushBackStruct ( XmlrpcParamVector *p_vec ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBackStruct()\n" ); - - XmlrpcParam param; - xmlrpcParamInit(¶m); - xmlrpcParamSetStruct ( ¶m ); - return xmlrpcParamVectorPushBack ( p_vec, ¶m ); -} - -int xmlrpcParamVectorPushBack ( XmlrpcParamVector *p_vec, XmlrpcParam *param ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBack()\n" ); - - if ( param == NULL ) - { - PRINT_ERROR ( "xmlrpcParamVectorPushBack() : Invalid new param\n" ); - return -1; - } - - if ( p_vec->data == NULL ) - { - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBack() : allocate memory for the first time\n" ); - p_vec->data = ( XmlrpcParam * ) malloc ( XMLRPC_VECTOR_INIT_SIZE * sizeof ( XmlrpcParam ) ); - - if ( p_vec->data == NULL ) - { - PRINT_ERROR ( "xmlrpcParamVectorPushBack() : Can't allocate memory\n" ); - return -1; - } - - p_vec->size = 0; - p_vec->max = XMLRPC_VECTOR_INIT_SIZE; - } - - while ( p_vec->size == p_vec->max ) - { - PRINT_VVDEBUG ( "xmlrpcParamVectorPushBack() : reallocate memory\n" ); - XmlrpcParam *new_p_vec = ( XmlrpcParam * ) realloc ( p_vec->data, - ( XMLRPC_VECTOR_GROW_RATE* p_vec->max ) * sizeof ( XmlrpcParam ) ); - if ( new_p_vec == NULL ) - { - PRINT_ERROR ( "xmlrpcParamVectorPushBack() : Can't allocate more memory\n" ); - return -1; - } - p_vec->max *= XMLRPC_VECTOR_GROW_RATE; - p_vec->data = new_p_vec; - } - - p_vec->data[p_vec->size] = *param; - p_vec->size += 1; - - return p_vec->size; -} - -int xmlrpcParamVectorGetSize ( XmlrpcParamVector *p_vec ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorGetSize()\n" ); - - return p_vec->size; -} - -XmlrpcParam *xmlrpcParamVectorAt ( XmlrpcParamVector *p_vec, int pos ) -{ - PRINT_VVDEBUG ( "xmlrpcParamVectorAt()\n" ); - - if ( pos < 0 || pos >= p_vec->size ) - { - PRINT_ERROR ( "xmlrpcParamVectorAt() : index out of range\n" ); - return NULL; - } - - return & ( p_vec->data[pos] ); -} - -void xmlrpcParamVectorPrint( XmlrpcParamVector *p_vec ) -{ - int elem_ind; - - PRINT_VDEBUG ( "XMLRPC parameter vector (size : %d):\n", p_vec->size); - - for(elem_ind = 0 ; elem_ind < p_vec->size; elem_ind++) - { - PRINT_VDEBUG ( "Vec. elem [%i]: ", elem_ind ); - xmlrpcParamPrint( xmlrpcParamVectorAt( p_vec, elem_ind ) ); - } -} diff --git a/src/hal/components/cros/src/xmlrpc_process.c b/src/hal/components/cros/src/xmlrpc_process.c deleted file mode 100644 index 3f3e895e..00000000 --- a/src/hal/components/cros/src/xmlrpc_process.c +++ /dev/null @@ -1,62 +0,0 @@ -#include -#include - -#include "xmlrpc_process.h" -#include "cros_clock.h" - -void xmlrpcProcessInit( XmlrpcProcess *p ) -{ - p->current_call = NULL; - p->state = XMLRPC_PROCESS_STATE_IDLE; - tcpIpSocketInit( &(p->socket) ); - p->message_type = XMLRPC_MESSAGE_UNKNOWN; - dynStringInit( &(p->method) ); - dynStringInit( &(p->message) ); - xmlrpcParamVectorInit( &(p->params) ); - xmlrpcParamVectorInit( &(p->response) ); - p->last_change_time = 0; - memset(p->host, 0, sizeof(p->host)); - p->port = -1; -} - -void xmlrpcProcessRelease( XmlrpcProcess *p ) -{ - if( p->socket.connected ) - tcpIpSocketDisconnect( &(p->socket) ); - - if (p->current_call != NULL) - freeRosApiCall(p->current_call); - - tcpIpSocketClose( &(p->socket) ); - dynStringRelease( &(p->method) ); - dynStringRelease( &(p->message) ); - xmlrpcParamVectorRelease( &(p->params) ); - xmlrpcParamVectorRelease( &(p->response) ); -} - -void xmlrpcProcessClear( XmlrpcProcess *p) -{ - dynStringClear(&p->message); -} - -void xmlrpcProcessReset( XmlrpcProcess *p) -{ - xmlrpcProcessClear(p); - - if (p->current_call != NULL) - freeRosApiCall(p->current_call); - - p->current_call = NULL; - dynStringClear(&p->method); - xmlrpcParamVectorRelease(&p->params); - xmlrpcParamVectorRelease(&p->response); - p->message_type = XMLRPC_MESSAGE_UNKNOWN; - memset(p->host, 0, sizeof(p->host)); - p->port = -1; -} - -void xmlrpcProcessChangeState( XmlrpcProcess *p, XmlrpcProcessState state ) -{ - p->state = state; - p->last_change_time = cRosClockGetTimeMs(); -} diff --git a/src/hal/components/cros/src/xmlrpc_protocol.c b/src/hal/components/cros/src/xmlrpc_protocol.c deleted file mode 100644 index 96744e56..00000000 --- a/src/hal/components/cros/src/xmlrpc_protocol.c +++ /dev/null @@ -1,410 +0,0 @@ -#include -#include -#include -#include - -#ifdef _WIN32 -# define strtok_r strtok_s -# define strncasecmp _strnicmp // This is the POSIX verion of strnicmp -#endif - -#include "xmlrpc_protocol.h" -#include "xmlrpc_tags.h" -#include "cros_defs.h" -#include "cros_log.h" - -static XmlrpcParserState parseXmlrpcMessageParams ( const char *params_body, int params_body_len, - XmlrpcParamVector *params ) -{ - PRINT_VVDEBUG ( "parseXmlrpcMessageParams()\n" ); - - int i = 0; - const char *c = params_body; - int found_params = 0; - int found_fault = 0; - - for ( ; i < params_body_len; i++, c++ ) - { - if ( params_body_len - i >= XMLRPC_PARAMS_TAG.dim && - strncmp ( c, XMLRPC_PARAMS_TAG.str, XMLRPC_PARAMS_TAG.dim ) == 0 ) - { - c += XMLRPC_PARAMS_TAG.dim; - i += XMLRPC_PARAMS_TAG.dim; - found_params = 1; - break; - } - else - if ( params_body_len - i >= XMLRPC_FAULT_TAG.dim && - strncmp ( c, XMLRPC_FAULT_TAG.str, XMLRPC_FAULT_TAG.dim ) == 0 ) - { - c += XMLRPC_FAULT_TAG.dim; - i += XMLRPC_FAULT_TAG.dim; - found_fault = 1; - break; - } - } - - if ( !found_params && !found_fault) - { - PRINT_ERROR ( "parseXmlrpcMessageParams() : neither tag nor tag have been found in the ROS master XML response.\n" ); - return XMLRPC_PARSER_ERROR; - } - - if ( found_params ) - { - DynString param_str; - dynStringInit ( ¶m_str ); - int param_str_len = 0; - const char *param_init = NULL; - - for ( ; i < params_body_len; i++, c++ ) - { - if ( params_body_len - i >= XMLRPC_PARAMS_ETAG.dim && - strncmp ( c, XMLRPC_PARAMS_ETAG.str, XMLRPC_PARAMS_ETAG.dim ) == 0 ) - { - c += XMLRPC_PARAMS_ETAG.dim; - i += XMLRPC_PARAMS_ETAG.dim; - break; - } - - if ( param_init == NULL ) - { - if ( params_body_len - i >= XMLRPC_PARAM_TAG.dim && - strncmp ( c, XMLRPC_PARAM_TAG.str, XMLRPC_PARAM_TAG.dim ) == 0 ) - { - c += XMLRPC_PARAM_TAG.dim - 1; - i += XMLRPC_PARAM_TAG.dim - 1; - param_init = c + 1; - param_str_len = 0; - } - } - else - { - if ( params_body_len - i >= XMLRPC_PARAM_ETAG.dim && - strncmp ( c, XMLRPC_PARAM_ETAG.str, XMLRPC_PARAM_ETAG.dim ) == 0 ) - { - if ( param_str_len ) - { - dynStringReplaceWithStrN ( ¶m_str, param_init, param_str_len ); - - XmlrpcParam param; - xmlrpcParamInit(¶m); - - if ( xmlrpcParamFromXml ( ¶m_str, ¶m ) == -1 || - xmlrpcParamVectorPushBack ( params, ¶m ) < 0 ) - return XMLRPC_PARSER_ERROR; - } - - c += XMLRPC_PARAM_ETAG.dim - 1; - i += XMLRPC_PARAM_ETAG.dim - 1; - param_init = NULL; - param_str_len = 0; - } - else - param_str_len++; - } - } - dynStringRelease ( ¶m_str ); - } - else // Found tag - { - DynString fault_str; - dynStringInit ( &fault_str ); - int fault_str_len = 0; - const char *fault_init = c; - - for ( ; i < params_body_len; i++, c++ ) - { - if ( params_body_len - i >= XMLRPC_FAULT_ETAG.dim && - strncmp ( c, XMLRPC_FAULT_ETAG.str, XMLRPC_FAULT_ETAG.dim ) == 0 ) - { - if ( fault_str_len ) - { - dynStringReplaceWithStrN ( &fault_str, fault_init, fault_str_len ); - - XmlrpcParam param; - xmlrpcParamInit(¶m); - - if ( xmlrpcParamFromXml ( &fault_str, ¶m ) == -1 || - xmlrpcParamVectorPushBack ( params, ¶m ) < 0 ) - return XMLRPC_PARSER_ERROR; - } - - c += XMLRPC_FAULT_ETAG.dim - 1; - i += XMLRPC_FAULT_ETAG.dim - 1; - fault_init = NULL; - fault_str_len = 0; - } - else - fault_str_len++; - } - dynStringRelease ( &fault_str ); - } - - return XMLRPC_PARSER_DONE; -} - -static XmlrpcParserState parseXmlrpcMessageBody ( const char *body, int body_len, XmlrpcMessageType *type, - DynString *method, XmlrpcParamVector *params ) -{ - PRINT_VVDEBUG ( "parseXmlrpcMessageBody()\n" ); - - *type = XMLRPC_MESSAGE_UNKNOWN; - int i = 0; - const char *c = body; - - for ( ; i < body_len; i++, c++ ) - { - if ( body_len - i >= XMLRPC_REQUEST_BEGIN.dim && - strncmp ( c, XMLRPC_REQUEST_BEGIN.str, XMLRPC_REQUEST_BEGIN.dim ) == 0 ) - { - c += XMLRPC_REQUEST_BEGIN.dim; - i += XMLRPC_REQUEST_BEGIN.dim; - *type = XMLRPC_MESSAGE_REQUEST; - break; - } - else if ( body_len - i >= XMLRPC_RESPONSE_BEGIN.dim && - strncmp ( c, XMLRPC_RESPONSE_BEGIN.str, XMLRPC_RESPONSE_BEGIN.dim ) == 0 ) - { - c += XMLRPC_RESPONSE_BEGIN.dim; - i += XMLRPC_RESPONSE_BEGIN.dim; - *type = XMLRPC_MESSAGE_RESPONSE; - break; - } - } - - if ( *type == XMLRPC_MESSAGE_UNKNOWN ) - { - PRINT_ERROR ( "parseXmlrpcMessageBody() : unknown message type\n" ); - return XMLRPC_PARSER_ERROR; - } - - if ( *type == XMLRPC_MESSAGE_REQUEST ) - { - const char *method_name_init = NULL, *method_name_end = NULL; - int method_name_size = 0; - for ( ; i < body_len; i++, c++ ) - { - if ( body_len - i >= XMLRPC_METHODNAME_BEGIN.dim && - strncmp ( c, XMLRPC_METHODNAME_BEGIN.str, XMLRPC_METHODNAME_BEGIN.dim ) == 0 ) - { - c += XMLRPC_METHODNAME_BEGIN.dim; - i += XMLRPC_METHODNAME_BEGIN.dim; - method_name_init = c; - break; - } - } - - if ( method_name_init == NULL ) - { - PRINT_ERROR ( "parseXmlrpcMessageBody() : missing method name\n" ); - return XMLRPC_PARSER_ERROR; - } - - for ( ; i < body_len; i++, c++ ) - { - if ( body_len - i >= XMLRPC_METHODNAME_END.dim && - strncmp ( c, XMLRPC_METHODNAME_END.str, XMLRPC_METHODNAME_END.dim ) == 0 ) - { - method_name_end = c - 1; - c += XMLRPC_METHODNAME_END.dim; - i += XMLRPC_METHODNAME_END.dim; - break; - } - else - method_name_size++; - } - - if ( method_name_end == NULL ) - { - PRINT_ERROR ( "parseXmlrpcMessageBody() : missing method name\n" ); - return XMLRPC_PARSER_ERROR; - } - - if ( dynStringPushBackStrN(method, method_name_init, method_name_size ) < 0 ) - return XMLRPC_PARSER_ERROR; - } - - if( *type == XMLRPC_MESSAGE_REQUEST ) - PRINT_VDEBUG("Received request message, method : %s\n", dynStringGetData( method )); - else if ( *type == XMLRPC_MESSAGE_RESPONSE ) - PRINT_VDEBUG("Received response message\n"); - - return parseXmlrpcMessageParams ( c, body_len - i, params ); -} - -void generateXmlrpcMessage ( const char*host, unsigned short port, XmlrpcMessageType type, - const char *method, XmlrpcParamVector *params, DynString *message ) -{ - PRINT_VVDEBUG ( "generateXmlrpcMessage()\n" ); - - dynStringClear ( message ); - - if( type == XMLRPC_MESSAGE_REQUEST ) - { - dynStringPushBackStr ( message, "POST " ); - dynStringPushBackStr ( message, "/" ); // uri - dynStringPushBackStr ( message, " HTTP/1.1\r\n" ); - dynStringPushBackStr ( message, "User-Agent: " ); - dynStringPushBackStr ( message, XMLRPC_VERSION.str ); - dynStringPushBackStr ( message, "\r\nHost: " ); - if(host != NULL) - { - char port_str[50]; - dynStringPushBackStr ( message, host ); - snprintf ( port_str, 50, ":%d", port ); - dynStringPushBackStr ( message, port_str ); - } - dynStringPushBackStr ( message, "\r\n" ); - } - else if( type == XMLRPC_MESSAGE_RESPONSE ) - { - dynStringPushBackStr ( message, "HTTP/1.1 200 OK\r\n" ); - dynStringPushBackStr ( message, "Server: " ); - dynStringPushBackStr ( message, XMLRPC_VERSION.str ); - dynStringPushBackStr ( message, "\r\n" ); - } - else - { - PRINT_ERROR ( "generateXmlrpcMessage() : Unknown message type\n" ); - } - - dynStringPushBackStr ( message, "Content-Type: text/xml\r\nContent-length: " ); - - int content_len_init = dynStringGetLen ( message ); - - dynStringPushBackStr ( message, " \r\n\r\n" ); - int content_init = dynStringGetLen ( message ); - - dynStringPushBackStr ( message, XMLRPC_MESSAGE_BEGIN.str ); - - if ( type == XMLRPC_MESSAGE_REQUEST ) - { - dynStringPushBackStr ( message, XMLRPC_REQUEST_BEGIN.str ); - dynStringPushBackStr ( message, XMLRPC_METHODNAME_BEGIN.str ); - dynStringPushBackStr ( message, method ); - dynStringPushBackStr ( message, XMLRPC_METHODNAME_END.str ); - } - else if ( type == XMLRPC_MESSAGE_RESPONSE ) - { - dynStringPushBackStr ( message, XMLRPC_RESPONSE_BEGIN.str ); - } - int n_params = xmlrpcParamVectorGetSize ( params ); - if ( n_params > 0 ) - { - int i = 0; - dynStringPushBackStr ( message, XMLRPC_PARAMS_TAG.str ); - - for ( i = 0; i < n_params; i++ ) - { - dynStringPushBackStr ( message, XMLRPC_PARAM_TAG.str ); - xmlrpcParamToXml ( xmlrpcParamVectorAt ( params, i ), message ); - dynStringPushBackStr ( message, XMLRPC_PARAM_ETAG.str ); - } - dynStringPushBackStr ( message, XMLRPC_PARAMS_ETAG.str ); - } - - if ( type == XMLRPC_MESSAGE_REQUEST ) - dynStringPushBackStr ( message, XMLRPC_REQUEST_END.str ); - else if ( type == XMLRPC_MESSAGE_RESPONSE ) - dynStringPushBackStr ( message, XMLRPC_RESPONSE_END.str ); - - dynStringPushBackStr ( message, XMLRPC_MESSAGE_END.str ); - - int content_end = dynStringGetLen ( message ); - int content_len = content_end - content_init; - - // Avoid writing numbers with more than the 10 digits - if(content_len > 9999999999L) - { - content_len = (INT_MAX < 9999999999L)?INT_MAX:9999999999L; - PRINT_VVDEBUG ( "generateXmlrpcMessage(): POST content too long. Trimming to %d.\n", content_len ); - } - char content_len_str[12]; - snprintf ( content_len_str, 12, "%010d", content_len ); - - dynStringPatch ( message, content_len_str, content_len_init ); -} - -XmlrpcParserState parseXmlrpcMessage(DynString *message, XmlrpcMessageType *type, - DynString *method, XmlrpcParamVector *params, - char host[256], int *port) -{ - PRINT_VVDEBUG ( "parseXmlrpcMessage()\n" ); - - int msg_len = dynStringGetLen ( message ); - char *msg = ( char * ) dynStringGetData ( message ); - char *body_len_init = NULL; - char *body_init = NULL; - char *host_init = NULL; - - int i; - for ( i = 0; i < msg_len; i++, msg++ ) - { - if ( msg_len - i >= 16 && strncasecmp ( msg, "Content-length: ", 16 ) == 0 ) - { - body_len_init = msg + 16; - } - if ( msg_len - i >= 6 && strncasecmp ( msg, "Host: ", 16 ) == 0 ) - { - host_init = msg + 6; - } - else if ( msg_len - i >= 4 && strncmp ( msg, "\r\n\r\n", 4 ) == 0 ) - { - body_init = msg + 4; - break; - } - else if ( msg_len - i >= 2 && strncmp ( msg, "\n\n", 2 ) == 0 ) - { - body_init = msg + 2; - break; - } - } - - if ( body_init == NULL ) - { - PRINT_VDEBUG ( "parseXmlrpcMessage() : message incomplete\n" ); - return XMLRPC_PARSER_INCOMPLETE; - } - else if ( body_len_init == NULL ) - { - PRINT_ERROR ( "parseXmlrpcMessage() : Content-length not present\n" ); - return XMLRPC_PARSER_ERROR; - } - - int body_len = 0; - if ( sscanf ( body_len_init,"%d", &body_len ) != 1 ) - { - PRINT_ERROR ( "parseXmlrpcMessage() : Content-length not valid\n" ); - return XMLRPC_PARSER_ERROR; - } - - if ( body_len > (int)strlen ( body_init ) ) - { - PRINT_VDEBUG ( "parseXmlrpcMessage() : message incomplete\n" ); - return XMLRPC_PARSER_INCOMPLETE; - } - - if (host_init == NULL) - { - memset(host, 0, sizeof(256)); - *port = -1; - } - else - { - char temp_host[256]; - snprintf(temp_host, 256, "%s", host_init); // Temporary copy - char full_host[256]; - sscanf(full_host, "%s", temp_host); // Remove spaces and newlines - strncpy(temp_host,full_host+7,strlen(full_host)-8); // Remove 'http://' and '/' - char * progress = NULL; - char* host_ = strtok_r(temp_host,":",&progress); - strcpy(host, host_); - *port = atoi(strtok_r(NULL,":",&progress)); - } - - PRINT_VDEBUG ( "parseXmlrpcMessage() : body len : %d\n", body_len ); - - return parseXmlrpcMessageBody ( body_init, body_len, type, method, params ); - -} From d10bdb1cafe2fe921a4a11d1401b676afcd4d76a Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 02:47:37 -0500 Subject: [PATCH 03/22] cROS instalation added to make --- make | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/make b/make index 9db7c1d0..beaac810 100755 --- a/make +++ b/make @@ -11,6 +11,11 @@ cd /opt/hal-core/src/ ./configure --disable-gtk --with-realtime=uspace ./make && sudo make setuid +# Comopile cROS and copy shared library +mkdir /opt/hal-core/src/hal/components/cros/build +cd /opt/hal-core/src/hal/components/cros/build && cmake .. +chmod +x /opt/hal-core/src/hal/components/cros/make +cd /opt/hal-core/src/hal/components/cros/ && ./make # Compile test component: chmod +x /opt/hal-core/src/hal/components/test/make From 89d90b5df00bda890613d33c4bc8a9f5076644fd Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 02:55:31 -0500 Subject: [PATCH 04/22] Shared library added to be linked in halcmd --- make | 7 ++++--- src/make | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/make b/make index beaac810..a894868d 100755 --- a/make +++ b/make @@ -11,11 +11,12 @@ cd /opt/hal-core/src/ ./configure --disable-gtk --with-realtime=uspace ./make && sudo make setuid -# Comopile cROS and copy shared library +# Compile cROS and copy shared library mkdir /opt/hal-core/src/hal/components/cros/build cd /opt/hal-core/src/hal/components/cros/build && cmake .. -chmod +x /opt/hal-core/src/hal/components/cros/make -cd /opt/hal-core/src/hal/components/cros/ && ./make +chmod +x /opt/hal-core/src/hal/components/cros/build/make +cd /opt/hal-core/src/hal/components/cros/build/ && ./make && \ +cp /opt/hal_core/src/hal/components/cros/build/libcros.so /opt/hal_core/lib/ # Compile test component: chmod +x /opt/hal-core/src/hal/components/test/make diff --git a/src/make b/src/make index 83087734..064ab042 100755 --- a/src/make +++ b/src/make @@ -54,7 +54,7 @@ gcc -c -I. -Irtapi -Ihal -Os -fwrapv -g -Wall -DULAPI -std=gnu99 -fgnu89-inline # Creating shared library libhalcore.so.0 gcc -L/opt/hal-core/lib -Wl,-rpath,/opt/hal-core/lib -Wl,-soname,libhalcore.so.0 -shared -o ../lib/libhalcore.so.0 objects/hal/hal_lib.o objects/rtapi/uspace_ulapi.o -pthread -lrt # Linking halcmd -gcc -L/opt/hal-core/lib -Wl,-rpath,/opt/hal-core/lib -o ../bin/halcmd objects/hal/utils/halcmd.o objects/hal/utils/halcmd_commands.o objects/hal/utils/halcmd_main.o objects/hal/utils/halcmd_completion.o ../lib/libhalcore.so.0 -lreadline +gcc -L/opt/hal-core/lib -Wl,-rpath,/opt/hal-core/lib -o ../bin/halcmd objects/hal/utils/halcmd.o objects/hal/utils/halcmd_commands.o objects/hal/utils/halcmd_main.o objects/hal/utils/halcmd_completion.o ../lib/libcros.so ../lib/libhalcore.so.0 -lreadline ln -sf libhalcore.so.0 ../lib/libhalcore.so # Compiling module_helper/module_helper.c From b7adb36cb84f70fb29e2ae1c56152674541aafac Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 03:36:44 -0500 Subject: [PATCH 05/22] make and runtest updated --- make | 14 +++++++++----- runtest | 16 +++++++++++++++- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/make b/make index a894868d..4dd7a270 100755 --- a/make +++ b/make @@ -12,11 +12,15 @@ cd /opt/hal-core/src/ ./make && sudo make setuid # Compile cROS and copy shared library -mkdir /opt/hal-core/src/hal/components/cros/build -cd /opt/hal-core/src/hal/components/cros/build && cmake .. -chmod +x /opt/hal-core/src/hal/components/cros/build/make -cd /opt/hal-core/src/hal/components/cros/build/ && ./make && \ -cp /opt/hal_core/src/hal/components/cros/build/libcros.so /opt/hal_core/lib/ +if [ -f /opt/hal-core/src/hal/components/cros/build/libcros.so ]; then + echo "Exist!" +else + mkdir /opt/hal-core/src/hal/components/cros/build + cd /opt/hal-core/src/hal/components/cros/build && cmake .. + chmod +x /opt/hal-core/src/hal/components/cros/build/make + cd /opt/hal-core/src/hal/components/cros/build/ && ./make + cp /opt/hal-core/src/hal/components/cros/build/libcros.so /opt/hal-core/lib/ +fi # Compile test component: chmod +x /opt/hal-core/src/hal/components/test/make diff --git a/runtest b/runtest index e55f71e8..9cb525a1 100755 --- a/runtest +++ b/runtest @@ -1,10 +1,24 @@ #!/usr/bin/bash +# Startup hal-core cd /opt/hal-core/scripts/ && . ./rip-environment - cd /opt/hal-core/bin +halcmd stop +halcmd loadrt threads name1=base-thread fp1=0 period1=1000000 +#name2=servo-thread period2=1 + +# Unix command to load the ethercat .xml config +/opt/hal-core/rtlib/./lcec_conf /opt/hal-core/rtlib/ethercat-conf.xml & +halcmd loadrt lcec +#halcmd loadrt test + +halcmd addf lcec.read-all base-thread +halcmd addf lcec.write-all base-thread + +halcmd start + halcmd show # To clean the hal environment : From f7228638f46055e6b7c88c7907acc93cea47c70d Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 04:22:51 -0500 Subject: [PATCH 06/22] README installation process updated --- Readme.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Readme.md b/Readme.md index 4842ad87..5b8b1ff9 100644 --- a/Readme.md +++ b/Readme.md @@ -19,6 +19,7 @@ To install: Install hal-core: $ git clone https://github.com/grotius-cnc/hal_core.git /opt/hal-core + $ sudo chown -R $USER:$USER /opt $ /opt/hal-core/./make To run: From 20c5a715f88c3145e380e2352aab2967ec004fb3 Mon Sep 17 00:00:00 2001 From: Jaime R <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 12:24:53 +0300 Subject: [PATCH 07/22] Update Readme.md --- Readme.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Readme.md b/Readme.md index 5b8b1ff9..0a9da8e2 100644 --- a/Readme.md +++ b/Readme.md @@ -13,12 +13,10 @@ A hal environment can be used as platform to run realtime applications like: Packages needed for the installation (at the bottom): -https://github.com/grotius-cnc/hal_core/tree/main/src#readme - To install: Install hal-core: - $ git clone https://github.com/grotius-cnc/hal_core.git /opt/hal-core + $ git clone https://github.com/jjrbfi/hal-core.git /opt/hal-core $ sudo chown -R $USER:$USER /opt $ /opt/hal-core/./make From 04a402c422000e80db48b55f6f9eab03686711e9 Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 04:38:16 -0500 Subject: [PATCH 08/22] Readme updated --- Readme.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Readme.md b/Readme.md index 0a9da8e2..dc88f8cb 100644 --- a/Readme.md +++ b/Readme.md @@ -20,6 +20,9 @@ To install: $ sudo chown -R $USER:$USER /opt $ /opt/hal-core/./make +Before run: +Modify config/hal.yaml file with your Hardware configuration and then run hal_config.py to create our .xml + To run: $ /opt/hal-core/./runtest From 2cd4f9e6e85dd940479ce1f49318232eba291460 Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 04:42:28 -0500 Subject: [PATCH 09/22] Readme updated --- Readme.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Readme.md b/Readme.md index dc88f8cb..3484fe5f 100644 --- a/Readme.md +++ b/Readme.md @@ -19,6 +19,7 @@ To install: $ git clone https://github.com/jjrbfi/hal-core.git /opt/hal-core $ sudo chown -R $USER:$USER /opt $ /opt/hal-core/./make + $ sudo chown -R $USER:$USER /opt/hal-core Before run: Modify config/hal.yaml file with your Hardware configuration and then run hal_config.py to create our .xml From 24c5d0dc970d37707920e996f7b024b1ba2d7eaf Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Mon, 27 Mar 2023 04:45:19 -0500 Subject: [PATCH 10/22] Readme updated --- Readme.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Readme.md b/Readme.md index 3484fe5f..0e6a19c7 100644 --- a/Readme.md +++ b/Readme.md @@ -22,7 +22,9 @@ To install: $ sudo chown -R $USER:$USER /opt/hal-core Before run: -Modify config/hal.yaml file with your Hardware configuration and then run hal_config.py to create our .xml +1. Modify **config/hal.yaml** file with your Hardware configuration and then run **hal_config.py** to create our .xml +2. Run ROScore and keep it in the backgroundwith: ```roscore &``` + To run: From f851ccc50dc90d84de5d9480ebde307764c56163 Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Tue, 28 Mar 2023 02:50:06 -0500 Subject: [PATCH 11/22] ln -s instead copy same content & chmod +x to new files --- make | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/make b/make index 4dd7a270..dd14c297 100755 --- a/make +++ b/make @@ -5,12 +5,20 @@ chmod +x /opt/hal-core/runtest chmod +x /opt/hal-core/src/clean chmod +x /opt/hal-core/src/make chmod +x /opt/hal-core/src/configure +chmod +x /opt/hal-core/scripts/halrun +chmod +x /opt/hal-core/scripts/realtime # Compile hal-core cd /opt/hal-core/src/ ./configure --disable-gtk --with-realtime=uspace ./make && sudo make setuid +# Set user able to insert kernel modules +chown 777 -R /opt/hal-core/bin/rtapi_app +chown 777 -R /opt/hal-core/bin/module_helper +chmod 777 /opt/hal-core/bin/rtapi_app +chmod 777 /opt/hal-core/bin/module_helper + # Compile cROS and copy shared library if [ -f /opt/hal-core/src/hal/components/cros/build/libcros.so ]; then echo "Exist!" @@ -19,7 +27,9 @@ else cd /opt/hal-core/src/hal/components/cros/build && cmake .. chmod +x /opt/hal-core/src/hal/components/cros/build/make cd /opt/hal-core/src/hal/components/cros/build/ && ./make - cp /opt/hal-core/src/hal/components/cros/build/libcros.so /opt/hal-core/lib/ + ln -s /opt/hal-core/src/hal/components/cros/build/libcros.so /opt/hal-core/lib/ + ln -s /opt/hal-core/src/hal/components/cros/samples/rosdb/ /opt/hal-core/ + ln -s /opt/hal-core/src/hal/components/cros/include/ /opt/hal-core/src/cros fi # Compile test component: From 867839b887e7e146b80ed0ff9e3423c8ea87be86 Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Wed, 29 Mar 2023 04:35:53 -0500 Subject: [PATCH 12/22] ROS publisher added and -r flag to run it from halcmd To run ROS publisher which will run the commands recevied, run: halcmd -r --- src/hal/utils/halcmd_main.c | 135 +++++++++++++++++++++++++++++++++++- 1 file changed, 133 insertions(+), 2 deletions(-) diff --git a/src/hal/utils/halcmd_main.c b/src/hal/utils/halcmd_main.c index 16a55b3e..194cb1b4 100644 --- a/src/hal/utils/halcmd_main.c +++ b/src/hal/utils/halcmd_main.c @@ -59,6 +59,19 @@ #include #include +// cROS +#include "cros.h" +#include +#include +#include + +#define DIR_SEPARATOR_STR "/" +#define ROS_MASTER_PORT 11311 +#define ROS_MASTER_ADDRESS "127.0.0.1" + +CrosNode *node; //! Pointer to object storing the ROS node. This object includes all the ROS node state variables +static unsigned char exit_flag = 0; //! ROS node loop exit flag. When set to 1 the cRosNodeStart() function exits + static int get_input(FILE *srcfile, char *buf, size_t bufsize); static void print_help_general(int showR); static int release_HAL_mutex(void); @@ -71,10 +84,124 @@ static char *prompt_continue = "halcmd+: "; #define MAX_EXTEND_LINES 20 + +/*********************************************************************** +* cROS FUNCTION DEFINITIONS * +************************************************************************/ + +// This callback will be invoked when the subscriber receives a message +static CallbackResponse callback_sub(cRosMessage *message, void* data_context) +{ + int retval; + char *tokens[MAX_TOK+1]; + + cRosMessageField *data_field = cRosMessageGetField(message, "data"); + if(data_field != NULL) + ROS_INFO(node, "I heard: [%s]\n", data_field->data.as_string); + halcmd_startup(1); + // remove comments, do var substitution, and tokenise + retval = halcmd_preprocess_line(data_field->data.as_string, tokens); + // Run the command + retval = halcmd_parse_cmd(tokens); +} + +struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); + +// This callback function will be called when the main process receives a SIGINT or +// SIGTERM signal. +// Function set_signal_handler() should be called to set this function as the handler of +// these signals +static void exit_deamon_handler(int sig) +{ + printf("Signal %i received: exiting safely.\n", sig); + sigaction(SIGINT, &old_int_signal_handler, NULL); + sigaction(SIGTERM, &old_term_signal_handler, NULL); + exit_flag = 1; // Indicate the exit of cRosNodeStart loop (safe exit) +} + +// Sets the signal handler functions of SIGINT and SIGTERM: exit_deamon_handler +static int set_signal_handler(void) + { + int ret; + struct sigaction act; + + memset (&act, '\0', sizeof(act)); + + act.sa_handler = exit_deamon_handler; + // If the signal handler is invoked while a system call or library function call is blocked, + // then the we want the call to be automatically restarted after the signal handler returns + // instead of making the call fail with the error EINTR. + act.sa_flags=SA_RESTART; + if(sigaction(SIGINT, &act, &old_int_signal_handler) == 0 && sigaction(SIGTERM, &act, &old_term_signal_handler) == 0) + ret=0; + else + { + ret=errno; + printf("Error setting termination signal handler. errno=%d\n",ret); + } + return(ret); + } + + /*********************************************************************** * LOCAL FUNCTION DEFINITIONS * ************************************************************************/ +int cros_main(){ + char path[4097]; // We need to tell our node where to find the .msg files that we'll be using + const char *node_name; + int subidx; // Index (identifier) of the created subscriber + cRosErrCodePack err_cod; + + node_name="/listener"; // Default node name if no command-line parameters are specified + getcwd(path, sizeof(path)); + strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); + + printf("Using the following path for message definitions: %s\n", path); + // Create a new node and tell it to connect to roscore in the usual place + node = cRosNodeCreate(node_name, "127.0.0.1", ROS_MASTER_ADDRESS, ROS_MASTER_PORT, path); + if( node == NULL ) + { + printf("cRosNodeCreate() failed; is this program already being run?"); + return EXIT_FAILURE; + } + + err_cod = cRosWaitPortOpen(ROS_MASTER_ADDRESS, ROS_MASTER_PORT, 0); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "Port %s:%hu cannot be opened: ROS Master does not seems to be running", ROS_MASTER_ADDRESS, ROS_MASTER_PORT); + return EXIT_FAILURE; + } + + // Create a subscriber to topic /chatter of type "std_msgs/String" and supply a callback for received messages (callback_sub) + err_cod = cRosApiRegisterSubscriber(node, "/chatter", "std_msgs/String", callback_sub, NULL, NULL, 0, &subidx); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosApiRegisterSubscriber() failed; did you run this program one directory above 'rosdb'?"); + cRosNodeDestroy( node ); + return EXIT_FAILURE; + } + + ROS_INFO(node, "Node %s created with XMLRPC port: %i, TCPROS port: %i and RPCROS port: %i\n", node->name, node->xmlrpc_port, node->tcpros_port, node->rpcros_port); + + // Function exit_deamon_handler() will be called when Ctrl-C is pressed or kill is executed + set_signal_handler(); + + // Run the main loop until exit_flag is 1 + err_cod = cRosNodeStart( node, CROS_INFINITE_TIMEOUT, &exit_flag ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + cRosPrintErrCodePack(err_cod, "cRosNodeStart() returned an error code"); + + // Free memory and unregister + err_cod=cRosNodeDestroy( node ); + if(err_cod != CROS_SUCCESS_ERR_PACK) + { + cRosPrintErrCodePack(err_cod, "cRosNodeDestroy() failed; Error unregistering from ROS master"); + return EXIT_FAILURE; + } + return EXIT_SUCCESS;; +} + int main(int argc, char **argv) { int c, fd; @@ -97,7 +224,7 @@ int main(int argc, char **argv) keep_going = 0; /* start parsing the command line, options first */ while(1) { - c = getopt(argc, argv, "+RCfi:kqQsvVhe"); + c = getopt(argc, argv, "+RCfri:kqQsvVhe"); if(c == -1) break; switch(c) { case 'R': @@ -147,6 +274,9 @@ int main(int argc, char **argv) case 'f': filemode = 1; break; + case 'r': + cros_main(); + break; case 'C': cl = getenv("COMP_LINE"); cw = getenv("COMP_POINT"); @@ -276,7 +406,6 @@ int main(int argc, char **argv) if (!extend_ct) { elineptr = (char*)raw_buf; } extend_ct = 0; if (prompt == prompt_continue) { prompt = prompt_interactive; } - /* remove comments, do var substitution, and tokenise */ retval = halcmd_preprocess_line(elineptr, tokens); if(echo_mode) { @@ -390,6 +519,7 @@ static void print_help_general(int showR) printf(" -e echo the commands from stdin to stderr\n"); printf(" -f [filename] Read commands from 'filename', not command\n"); printf(" line. If no filename, read from stdin.\n"); + printf(" -r Start ROS listener (roscore have to be running)\n"); #ifndef NO_INI printf(" -i filename Open .ini file 'filename', allow commands\n"); printf(" to get their values from ini file.\n"); @@ -445,6 +575,7 @@ static int get_input(FILE *srcfile, char *buf, size_t bufsize) { } #endif + void halcmd_output(const char *format, ...) { va_list ap; va_start(ap, format); From 008d83c19be49bed2bdc3d38b1c854f0127266ef Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Wed, 29 Mar 2023 04:57:03 -0500 Subject: [PATCH 13/22] Compilation order changed in order to get the .so file before halcmd compiles --- make | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/make b/make index dd14c297..40778f35 100755 --- a/make +++ b/make @@ -8,30 +8,29 @@ chmod +x /opt/hal-core/src/configure chmod +x /opt/hal-core/scripts/halrun chmod +x /opt/hal-core/scripts/realtime -# Compile hal-core -cd /opt/hal-core/src/ -./configure --disable-gtk --with-realtime=uspace -./make && sudo make setuid - -# Set user able to insert kernel modules -chown 777 -R /opt/hal-core/bin/rtapi_app -chown 777 -R /opt/hal-core/bin/module_helper -chmod 777 /opt/hal-core/bin/rtapi_app -chmod 777 /opt/hal-core/bin/module_helper - # Compile cROS and copy shared library if [ -f /opt/hal-core/src/hal/components/cros/build/libcros.so ]; then echo "Exist!" else mkdir /opt/hal-core/src/hal/components/cros/build cd /opt/hal-core/src/hal/components/cros/build && cmake .. - chmod +x /opt/hal-core/src/hal/components/cros/build/make cd /opt/hal-core/src/hal/components/cros/build/ && ./make ln -s /opt/hal-core/src/hal/components/cros/build/libcros.so /opt/hal-core/lib/ ln -s /opt/hal-core/src/hal/components/cros/samples/rosdb/ /opt/hal-core/ ln -s /opt/hal-core/src/hal/components/cros/include/ /opt/hal-core/src/cros fi +# Compile hal-core +cd /opt/hal-core/src/ +./configure --disable-gtk --with-realtime=uspace +./make && sudo make setuid + +# Set user able to insert kernel modules +chown 777 -R /opt/hal-core/bin/rtapi_app +chown 777 -R /opt/hal-core/bin/module_helper +chmod 777 /opt/hal-core/bin/rtapi_app +chmod 777 /opt/hal-core/bin/module_helper + # Compile test component: chmod +x /opt/hal-core/src/hal/components/test/make chmod +x /opt/hal-core/src/hal/components/test/runtest From 6f1dd89adb5fa897a34a1d3ed99ae384766b9ebb Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Wed, 29 Mar 2023 05:02:43 -0500 Subject: [PATCH 14/22] make reference fixed --- make | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/make b/make index 40778f35..7c9e1a31 100755 --- a/make +++ b/make @@ -14,7 +14,7 @@ if [ -f /opt/hal-core/src/hal/components/cros/build/libcros.so ]; then else mkdir /opt/hal-core/src/hal/components/cros/build cd /opt/hal-core/src/hal/components/cros/build && cmake .. - cd /opt/hal-core/src/hal/components/cros/build/ && ./make + cd /opt/hal-core/src/hal/components/cros/build/ && make ln -s /opt/hal-core/src/hal/components/cros/build/libcros.so /opt/hal-core/lib/ ln -s /opt/hal-core/src/hal/components/cros/samples/rosdb/ /opt/hal-core/ ln -s /opt/hal-core/src/hal/components/cros/include/ /opt/hal-core/src/cros From fc0e87d73343dcb93f8bb90ad3aec1ae173c3ba7 Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Wed, 29 Mar 2023 05:08:40 -0500 Subject: [PATCH 15/22] Included cros headers --- src/make | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/make b/src/make index 064ab042..64db8c94 100755 --- a/src/make +++ b/src/make @@ -35,7 +35,7 @@ gcc -c -I. -Irtapi -Ihal -Os -fwrapv -g -Wall -DULAPI -std=gnu99 -fgnu89-inline -MP -MD -MF "objects/hal/utils/halcmd_commands.d" -MT "objects/hal/utils/halcmd_commands.o" \ hal/utils/halcmd_commands.c -o objects/hal/utils/halcmd_commands.o # Compiling hal/utils/halcmd_main.c -gcc -c -I. -Irtapi -Ihal -Os -fwrapv -g -Wall -DULAPI -std=gnu99 -fgnu89-inline -Werror=implicit-function-declaration -g -O2 \ +gcc -c -I. -Icros -Irtapi -Ihal -Os -fwrapv -g -Wall -DULAPI -std=gnu99 -fgnu89-inline -Werror=implicit-function-declaration -g -O2 \ -MP -MD -MF "objects/hal/utils/halcmd_main.d" -MT "objects/hal/utils/halcmd_main.o" \ hal/utils/halcmd_main.c -o objects/hal/utils/halcmd_main.o # Compiling hal/utils/halcmd_completion.c From 22a190f7de2fdead6af012536256cf18e17efe9c Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Wed, 29 Mar 2023 05:21:34 -0500 Subject: [PATCH 16/22] .vscode removed and logo added --- .vscode/settings.json | 10 ---------- img/logo.png | Bin 0 -> 35563 bytes 2 files changed, 10 deletions(-) delete mode 100644 .vscode/settings.json create mode 100644 img/logo.png diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 772e47ee..00000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,10 +0,0 @@ -{ - "ros.distro": "noetic", - "python.autoComplete.extraPaths": [ - "/opt/ros/noetic/lib/python3/dist-packages" - ], - "python.analysis.extraPaths": [ - "/opt/ros/noetic/lib/python3/dist-packages" - ], - "cmake.configureOnOpen": false -} \ No newline at end of file diff --git a/img/logo.png b/img/logo.png new file mode 100644 index 0000000000000000000000000000000000000000..446b464efa68d78550b736d3901e6934d2055b54 GIT binary patch literal 35563 zcma%ibx_n_xHsJ$(%mA`ONVr)NDI>49kK|5bV)8DASof;AyP`Wbc1wC$36V++&|x$ zml;PzclUhHInSq_hX@Tdd29@F3^+JAY()hbEjT#%OzEj z#Qz}04gI$!WQ)8@9I=}hFySlGUqJqBm+`Z$-80qJ?BAod-$&$t;x|6Wiq z5Y1=`b-Ncsk1}MQq?6@Y%}&2BmtY*yA&L|HTFY3Ecc;?I5%}(g&{FDkY#=uR-QwRh z(&LxMr9BRFoMD9U!A^LwOQxA<0T@_^8{R{ms?Ta{>u?3UvA)Bnjmy?-zp$DJ5yv6t zgdb?JsiwM0=y51LvJRb~_AS*9I&&=@ z{_J$E1b<0GZ*>dK{MOVN9yyrm7cw?V!cJwY$0`)-4)5|Uryqru@`_*MivTI-5n&#g zGu9+U^dn}#j(oMNcq3V`m|w_=PTLPbNh}TogtvG&E)u2K^Mq2t%f5+`PZi?hWt3 zW1t=7U;TP}e5;|6*k&Uz+;`C24|DDA6PGvnvVE)x-ilWOcQm0h2s$Xflxhe~&c{x@ zH4MI^nyQ^k9fg1VCP4Zq?AEnOe2y7n@cTr!@Bp*)wnw;^Es{?`0EGBXi{>;TF<4+Ywu{;_DF#-qy0iJ-hmDjM`?S zy}#c6>|%`jo~U@|$5Du;D$h=44B|!YeOcnPRGu~BH>JUt0$SDWj>I?Z41@EXUwHDQHe@N-C z8N-QXk_IhWNbr1cI5gMz&WghY*5gne0XC$wP4S?z7~U~^6l=q; zh9GLT7MshrNww)&P04JOI0d{j%b>2|V4>LkPPg7*p%wV|B{K+FaF1j;i9}1st%dtX z9M6iM6b;)PowA@Fx0m8$%!9nZD`1qnofLIKq&}tL;bTc`%JA>axM3;&w6Kd?ElX}# zHiBGUFLRw&o`>`IwELU`Dan-&bd*uE9^_LWo4&yhqb$7Rh7F3Bv7`Rk&K;5<`d2b5 z@+Y|{SU|M)=Gm2Ekhmhw#Q0{hRZfLV#F8oHTw9$&Lw-hw=qgQTfOACeP`iV?r`(@i z*)tlE&zjzo27;w*mF%L<+^~n)kh5`6$JEmNHOVajb;Nr6BUmH|2qdLlKg=KpaD@mt zU*sBF%y7A8%pm2bCtGDQ|A_pkZ5%E=g}yJCK^8SFF5csGRXb;WpkG8;*p^G?&y!0U zsZUd#y1$nZ8dj_z*>-O~3OVbNoSc-*J`M{#*9J#C_cKr^#*TU6AObmgi3## zl<;m3<#v;^ai+Yj$i3&;O0JaJHF1HDNLL*%GaTyDR8z06@46Aow}s_1p`F(laoG@X znxv(du@X7cdz#zucQe~>wb_sB%$sPz!DIkr^e;wd5&8MY$>V^{JQ%zGgk@InV*AsQ zX+V5MTO(uN!_tBIr6zUzV+G0Zh%5J3G&Bz!eaa8-!w;z2FWEwBz@f!#x0*ffb4)R_ zwW9Bzj_%8aH=iNXt!d%o;e|4c@>1@uCI$NYxM|lT2JM$;vAtdo%RV1{fwH_^C@Q4) zH9}cYQW7s#a^{$ijB3L?2vaPeqoRe{c1cR}3XSz_bZ{mSFMI3A3KXV=s}MRGS{MFb z@!ZUldIov&Ky{}+=YJPC#0UJdZF(b?;$C|ZjGmp#{tWuXP$-l(6BC(1G{9s}+Jslk z2ochX`{#g7{GxUloWqashYQ~^EBNnnla>E1bcH$z{MRjMlWYQy_Y=C@Dj56SHDrT( zp?RQ&5RTkVIb=8xE(vqJkdTm8(FD2Nx}?xYKJMe(j20B};FN5FM7kM z0Uz+8`?+%aj*7r7mh=$J(gTZ{zZhN8#8Y(VEuIFEC3kk)H?J%3Y3+l7tx>noiVxg@ z<-zAX$I7&W0~ZX?qF@POgn(r^&kP#^>5ouJPPWX1Fm?oR#F()x3;t03;wth61R7

FesQX^vS>#{?pg8V;h(surrAN={nrWL7vMld2XuH6~KSRl4YUJg+O`eajNvuj_ecS2#R6*wP+#6-b=uabqL~33M=7E?!QMrf zk>cYW;~Z|e1SsD5(AddYsW*}xVHS}6Lots8D1O34T9liy+4`z%o$X&vpo>aSi2x~k z%e5go|F9J)7f#*puMK~!0EDee`>Su~7YHQg_(6-B67kx8kgnL*+eOX3t`s08fg3^a zUd?e^AM128ZCWy2KoGU%hKUnPM?#^_p}Yg(%SGS;a^I>9j$F8oW?_H!vGJ~oyA}Qx zhqi4rNXEbF@20HY)H^K8k!rOVu zI4@e7pMvJ}?7{?Lf0`mX$Yqk001xk2QjjHJ|0l&+LS@p3&f8j{Ma=nGniIpQ*>gKi ztix@3xdB+GazPP$RoYA)ptT^@_*n9*rh1XmlBmEdY*q>x6hwIVk&xdIpj-L}$Z6YE z{yg{5eed%v?zZ_Kf{6E<6*U0Gi>1lb+seZ~#F7169j~HDS$Sal&uV^IaKoxKw}%eh zjaVKuyxpOyi|90Xcvp1z!zO`OZ4Hjg2(9HG8he4*WAKEz~U1kufnl#l)K+gl3m~)F;K8-s#vm6>0al|^vNbkR6prY@< zuavw*TSHv2i}E%8hAVJl_Zqh%LoI^{&K*c_e43jVC|l#cO)GXQ!eFlzB_z~G;7gv> zunhnPtlNL=#yuk~MENo-U6UKwnv;!o_}YZV2V6EsZrB^3>e->IozBOUoRicY6Y6@T zY+)3^G{OsN^m&Tx8(Hp6z@>wugQN`hRjH9lT zQk>44be*}#p)e>rcyJH(9M*mTy-g1^H|nTQFxK3J zZt>@3_{iqan#<#3{?hkjHVr3aw_rxj_riFxO~m~7iO^2mXNiJH?LQJGtG?|fk%32A z4u9xH@ZXtZFC!*3=Y~bzwQ?SPNl81z;sgife|!ALDSuD!`y-(94vDymk1i|fJ|r*N zX3Zc&#NP3pxMXHLXOut9AjEJ-{<$(iQ<&c_iCTEek5G=-KO7tFu{?^3cw+DPx5X{3}6(ZF2QBp>~UsB~z92GrvoptM_KhQ(*~KE3?XuBiBBsph_@(hRyI< zeJdmwX=%~qzs#VPiHfG`;YgJqtf}vRu^(Y4tJ{z^S$VD=z9U-c_&Lg&bMW1o9SLAc z7x7M5Glw_eIUflEW9;m2eiwdDjj!u!&s3>1vSuQze7T3+`{?-<>lF3v zm%SS)p6^oNf00_eq~Y)kVtgX>VU+N> zE(^<&>wxXHCGn3rCy_F*!O}VaylHwz$N`gt$ZtK@*ZI7U@czl7Xq5iAWDLJ?#k2%} zQJ#!OhEWB+N1&Xbh^_>iSp2@w4YJ}(9|X73z~Y6xvmAT2K-`PKE^kHZ`Jh*{WEzb! zz0!30R~}puNnC0NaZgQ+IIy-EdGhGoHkaMQFME=^!Wxm?@eH(RMe8vc$ooju!el-w zfNs=ioF8&y#Z$+)vF~YDY|-EBuo%(#FKD9W3uMLnEnnN4s;U}*>2%arXNUlw@#cY%%Re=wYbb}I>D8|vxd=3 z4UvkrlsVU>&CK@vw}^c7wPnf1@Z~n(N?ESISLi@loQq(Zj5J#;pXH(Utv)*DsFGAG)qtPw#QgdvrtL-n1x+P?~riph<{@fooJ!WD`Y^`Ow$o)ujc zJXR1uqeZEU9U|haSiTLW2d7tEsfG^(ohCSAiPvXEMM&A$#Y<2G1PPl)ck=#=)k{pU zBgH)q!t1$NoPnScQ~uhn-D$m!<8V%hu^Zpxj6?WVrM;qIll6`?9ev#mQ!u`6)Ty5#GhRr{|iEbTw$9Vz~GV@CM4S7|H~Zx{$b}E}6bS z7I$ZDJJN`noTa=}B#OuQnk%fQ^3$JlBo!2I-G5u-5z6`M{VPcxyV(yF@rUbPF()5f zv;qRJ4%AtC6RoSkhp_;6#1|+7o(CXQ-IZle;wcM}?elD9=30r4o%aj(EC6Skof5b3 z!?g#tuc&8pqJ{e!2jgzt7L@Cr)4Kflxk#8%se|2SyF7;(IOm@`a)yD?8nR8!#n#F3 zo}u~96XV$2^j2u@AZVqaq#kp8HqIce@{Te`O5}T4PV+V<+By2_{^jarjKXKXvEdoF z{@Yd?_x?Wdjy-3VqJD?!+-bv;P?Nnr&T~V&N`~nz3{!iWqtkD!3qYXUX^weF*p0pl~l>E-$F@Exx-53Be2XYU(vEsM`MM%J3{LL|m87Oa8uB$fv6JQI3G zpB)1C>R;=xf2X~V3ko?drETx?Tfz5l0C(bXQH)eDMzTl-vt|J8Tzo2W)3u^`O#{xgD) z5*yzs_-5rr??<p(vv(py#I4lB10Mc(HOZf8w8-`ZQqyR zCL2>Ka#>mYe)3KIf{Nld@Gd4;1@Dw$_snBL=X^AOMsuf_6WQZbT; zEUlzeW}K?e7tQWI_t}?c#csvb(y^QNlSoy08^9a>e|>cdg)`y=LnRyVLNbEsdN!A@vLPTtoDW+FU&9VjwW=+P-^QQ5tRbi7Dp48$#SiPwu1oJ5dXMMJy$9>1x< zL6*okq)jCI?KL?i<=|wAwxXIEilin{Vq#+LjGCZ{$YYM6>xaTZX1L$Kf7{sEt#wBb zQVO|!9g4KnU$SnPe}pfCSL!Z#veUFKtN79G=@SnFS64pZ^$KcgR_A}moFaM%S3Uj( zT3Rw$T3YTecjWy3tu&)kDLdrTrk_dwq+Jm1NoFPM85L((Bl9*dqhe#3#l^EfeuSeqU45@yx{H|f1P?A5P zpr9ZjGgHOQjc06ZY>^%_2tRi3kwHH8yLlhZ?|}hhVs)H0T^^|?Zx4m$eA?>R_wpC) z3`Hj-<+Z}HwY7cE6qTJA-7+H|OSw9sE>TcXfuoqXOAHjBXRQ2xuj=#-bx=%vt4qYh z!^jwFJ5k6Tc;g`@CFPf8aaZowezVK|^lMv}Js9=l9R7cs^S(HPxRV}QR`#M$Jx9&O z<@@rlJORbRd+X%noZ_s28tNXu%5;ga!$ z?(aK}0_2u9 z>8!3goHhInh85kvpf%7K|BG2f1lHuf?L`}w{iUW|v^>9P**k1<^0l44eMxC4Mk3=* zU#IYj9*M*+bhp*KmHKY+fxFEgcvw$Dz?(TLu8xkPI!yDa;Tno?Feon9g`&)o10V8*C(5=cg(M@PBe6GOSZ3UOIiIH zZeC{~9B=GDuJ((PBT8_q>#2((@auHm5>p@Hj$NqbWWbE1bp>PLR9 z1<>*w6i)Aaii_JmUj6yUp?*Mf`)#HgQYE-N(fT!3o=TA6%S6V6v@``REdqXieoA4F zPjM0tX@CFPR99E`4-Q(4e^r!`k#RZytC<0VX+tT3bKr`o7?VDdBPNH~h0U`O#erKI z{19!lvKv9j!eibWEv`$noxh8Yi78z)q50-b*oy%Y_Q+ELyYJhl+~&RU+1VBNQ6kl@ zfi#qqDt30y-@SWBDe*p2%>UxM<2-~}Jv;hsB?~y!!9f*(A>bC|wYQ6tdsb|-6#sia z-u3e*D00Ol%WE82&EZ;Y}UXu-V#Na}U8H1u$P+jO)0 zE#lKBT2|Ii@Q5h!DJi`I%K_5v?!1VoI2+A-73)1DCX4`8UxlE!?oO64=6b#NiSMDq z`#S>8`yJHxZ0nwu5+Oj=!h%t|NX@f-$gARKR^pDz4r}CiuA`9=C9m}` z{)bKBySsLYNlA)IN^2Xxk^pqUyUaT_;M36H78g8=jEVv*((BY94>$6q_h0v?Z~6de zJFlZ10E`tC6%i6noSl?PG{5pcYx`kpI{M|+7nBe$T72)&@$_9+M+zdMZa_q2JQir- zxHMZ8Er)q!WzpT;GUYZ#3JPdv+aojeOk7;?ot=_O%F5_CI9^UO^oogt#qKKq9ptcj z7rcVHx`Z=P&vmJv)lf!3o0^i+sp62w#nP-7V5IJz9!(Pybtk8no}QkcQ&JGV+leawME98qy6rqoM%;CFTCaLQKc6 zH6wy_&QN}bKU@fih;7`u+f(z=^lJzqj*Bx9f4Nc8g zrlz#YDKBiL1y@&B;ktW!2?z_gbDOm=*YKZM^`V$TR+C;n znsjvvZeel~ponZLqKB5&IC8n)hYufYY;AL^suI9%G=z@NX2!a+v9f*v-{|U+0&pJ@ z6T?JL{~)Lr9=? z0ftcrE)LG&6!f(-nr(kPo!*OQu~Z_x-{1Wd*5N(ab~c(+c`s?)c8xLVB2n<_4rE315zgl#_ zslkwP(c zhKIK_^Itr zpw`X|Lx9aM%6aa?Mq;gujPRg&WZ)7#PzNVDF*LDNC=sz&-Z0q0}YLM zIh8DYCEZ~ptD5wD`8Wb{^4|<`qEap{TqXf0$S?utEl^b%!mjjmbitvap*ZvS#Kcp* zV+_2!gixSh>TDZa*X7<+z8wOZA{;SJB!vSg049PPIvFv_;0HOrrs9In=-Akn06wBA zg%bb#(c9hIdpbdd-lp21(+dG3#ePMVFT)f~?OxjY6D-v()`^%H7;egHk$HGzWQrCR zUoZ6Ey`w`$Mm7rkHx*6FEv3z@mgm~;bw7A5&dI6H07Iqrfr6vx>guZ99L<&9*xalF zcn+|YZ^8iVwXR7Ze~tABGcRvaaWQMoWm|oH%G5XA8n;a)AnUSZjGb#{m%QUgtX%Cu zD+ceF#0+Y!d2sy|$iejVUx4{V;+1bV0~A~*s4KQwE*Ph>JB=jVhyBlqCJI%}!E;e5 z^eo;2r2Fo^tpu9Uj5;;o#>K>J0&X<}FqBnF zw(YhxNY)V_yy$c}N4fo)Pi>^EVg(E8q4T`rs3pH*E?+DmOUOO!=Ekp371j%owuO-# zHq;Af;!m5u00##LBQx{q@lRYrBBIbmr=!m{nT(U;<5F^RVOEJ)*w_UnCFqKY3(wmC z4~|bv0FgrvwhnA?oXiGSLlrpgnYb(+Mcq$^@=r>*(xPSxfc9WIZ@7I$6H3eCq2`na z1Wp9idRuaz5zR!QfRlEhIwh2kkr63NtqA|McLe(6b@BMPR-sx}DZZL@VeQ>r0EgGs zxH9J^;GBsqcSglsu&CXwFbbzH&CTSBiJu}O-t|V4DJ04S28yw$XK#Rq$`tm*_%>B4 zMBONDV2}aXCL|^XJY*3rxqNzhiqp+7P;IRFSxZ)P3w_G{K~fMKEwjOjPcgCm`}_gE z#KTEk)74rOzm%SIsvKRzSfOi`t4hl zLHS&jppQoRX1)E)%jN8^Op5jM2MliY>Ml% zm8+{;dFWJNkQnEV`-}Y>qepfOiJ;?GgV)z80ILCR0UQP^daa}LuFjzD*V#oV5h*Fq zAPTU8HhQ|FAVFWS5{18y0dN%mH5=wKxcGiGu344dOOFt zreZ8#o)8~DHzA5q(Xn5yXoB14*z`#)@mi_CMlAKW{*LAMSE~TF0eG7s#!;qfs4Sq>?VG(yXK6^q83Y)= @}y1Fw~Z%sG;j#;SX z4A-Xf{>p^Jpp#{7hkn*Tq-THrd=4)*k5yqb49N)9nCN4!ba*`-g*xMUZ2!jFiWWhSzjy!>9C&k97s3Gbm6n!1DN2AnrKQhfsl|W@iGt)F+U4yH zax?>_0`@Q^EDQxeM&o*{NSdH)o}ApUqg$ebe;%-EYgK*Jdw4Z`Hlwt1v6Qo6rVAX2 zjJ8M<4`*PTE%ce0m% z5T*a%n`+$KdVQ9jnGWEU5%AsDuU|cscn(l|0cP0Q*$rom8z~mH-MBVV_Vx8#%kUgD z(9zMo8u$dwGxgS&FMT@_oz6R36)5s7>f_|(WPNBjdF6Vvrz=x;bF*;Fr>CdW(IEzG zoI&nJpaxXze1r3h*$1Go2eZ7FN=nFe^ITt6$V>ny3ar@>%(=aUEgIBa3?6`l-2?|e zbD&0m{RK|8Q>4E-KovMHaAGrP+W?INfUo(SoD6R_d3QI!xxL#HmA;Qg>A@5Q6ztVk z;V>3vEnUN?p{alkDX!s>%%gB@ifPBZF{gGY91t@?E-RAqmIbUv^``J5_0C1b#R?3R z&MVO!u1rSXrha=b2libY%xe;T1o%|%ysQSaFc5YxOxiPngQ!g(ab@C`)r_~)XMm|o z+<(^j_GWB!R1zpNkP}P}x<=-4xvt3odYw<}TS%V>xL&w6*3F>-Og~Yij#x&t#rKq3 zWaF7P%riV$jFOUE;2A61ZzAt+FA=!}A@#nid9tsfDm$KnjD|CIHRQDjJfjYGxjT`Z zi!2Oz*L<%JAs;@up%8hK9XywIEwZEDiyuo|g?d&^PuCFUS z!mYj$NA*a>2%=wf^W65}YOCrJ=7s%RC|JjbMO@ ziJ8C{0sPaspBS##IUdlGj%%L646w?MhYUdN_8+H!!u&Nj$S5e7<~Uz(n}bWvhZgd~ zMNP%FFP7@3Srx6KR(c*Qtf!pu>5dU9!GUrV0u>9;v>Tf&PxM0KF+U1?RSE?f@QVl$ zS~vfie9^!F%M$U{vZ%6g1pY}Cu+AGleK$=Ze0=;&ud?&1I;e?J?#1NR z3o)Dk+1DfJrapEAoH}>32dKEi{l1NQll8lY@joo8&MTMGw z(A|}*<66Mq(OlqJ-2b*5kwQFdC_b($*&Sx0EC|7aAf@;xWa_nUQcljuSH&a^qK~fY zy(d${S>&q<2cLCr1!+6;$;x@Bv_S*~4k2{P&T^^MSM=`qCl~~>X&TH7T^G&H2{=Wk z2{;Z;p`!)<9hey&yGhpa>6kNtC;1h{**-eW?|W(i%Iw|>ayI4e`1o*7^z2#Q#7=wd ztb^3zlYV8K;Q!?zD||p4Ti=M}Umqt;MnF?(73FokSY_V#30O?w=NQOnnAin_JIPxZ zGer+xp@bmLrjumk{nVv z4CTDM>LGf{iC@zv_Ps=w1J1AknwD$K`aB>3#P5!!%igeGF8On&8hfS7@u?9s%sNa?PoMTl zJS2pm;@UBNw;sxnm6MZ01#Ah_+WuVK0H?)zncN=eA=zyX)t7{!Bx@Zu(GdS!${2RyS<}A#*;i%uo3FtYgS(A~IdbIM;e4dkF00 z*p7!2X0_0-t+Nq{na}Y_4$Vs3>)*mND&cbs>m7bLKo|y2#akA$*`36mLQqlun#pCV z7Tywm2%pW8*+H_JxwJHe?)KK!7NkSWbq(o>7pih!sWu!{S4^j*;JC9@{Kojw-kzP7 zMtT`K5$mxayWf(S`Ay1hd8s7exT|&=NQaw$dq7<-1YRxiT>D?6=q9LP^5VXEtp=)ma~kV|@*cPT=U{UJU&6YPo+S^X9*_ z7EN$!Fm%Zvcu`291*@M!%?yh3BfZ>9q#Qy4FH8k)pxrKMD*R*j$vTK!G_|!Stf-5d zn=?klZ=-?Miw73KamctBKF3u$4DX2fm>1+~g}l}a1S9(mcSXE`vy*)4>XdoY9XO9Av=Vd4E{u0Ff4s*2;KqLYA zKUZrDEL=7(VaAl*X5Gx&355x#hGpE1h%;y+p?b9wQ*os;d|WU;0bs>`pB zct**J#*Q3_C|I-{95Fy{G|&}T!LXIz(WP`r#p=91KAEfA*bbCdRn5|!KZ^>d+1izj z>K`7qvQ@)@J8^XdVB=kCXY|Xh3|5AylblSo<+?w7e+FG_@pcChE$G}d-X6Bhvmz)a z>eX01RpC>7h0h@O-RHzib;<)=aLaWi$l2DA`ATOn^~AurTxZdXfq?Td`GAM(Z3AYs z6=fNWC-ftEp+(7#QdQh?IKbxQ#e5U zSRhYku(bS4Bh>W}c*|O?$Lc1tD1uML#%*1yBA20}@>!;rG$M^PqNc~y^vsEZX%ho> z`RCf@z(CxnC1qLcfpkRlQuO5HM2~V7Ypxr!PFol-vZ4#)g9rb5R&m+nV0gue#Kp_k z*u`g%7VjhG7cZj2Y2G)6pdWw=nJ|0(vh7?8B-Wy(PaYej<+D}hs9&D>!$iDaCL|=Z zRhTZMPhwzT=ry?&yg_B*Lp z(lxP7hM;1TcY}0zfjI39zUm3WtK^=c6?V*t5|#ei(Xe+|_>^5pe9% z^7-_QgkyUAP%t%q#kA1M%I$hHm2j@Y6ov#4xb{TJT}N)1NCIH&p^FhvJPJUDfV>cx z)&lI=yItLmH4wG|Ips2A?2S)CvYp@nJkcM}W-3z8kxOKJGGK+GwGCvjHtsr4OSKw< z^1D5gN3)})wv3P-&l%&sWZk#FK;dRuB*iftWj{?eJ;ac%=r`}ULRRUiU=nBy%yajW z*@>)4?rkc=|ALSCo}ZLCzdOUBxJG`tD~|%BicJobm$9c`{};k4EEGCXhoOm&pMi08 zI>70>S{bx?J3e0jRX~N_zdg3hR=ApT%2SH59ba|+GM?MO>TEg0=a{p+?CuIJ$MFic zvh4w-NG*&0-5)L~X=z0T1s7#LIQckg9mO6+!2=#sk*J8KH+M+<@)^9ukHDVI_C{Z9 zB&oZtbi#*WQ}m7(C`Uv`pDRE>fb6i?oV>MV#i(co!kD+Iq{l@pb3lB8dXL$v7W3NI zJ8Zjr3+_%lV?@W}{n4=Waap4($XU;W@^%t{^G%42kF_osk{C91d?*i3!8ra zQhXJU=JbqjHc+(OUcVa<2=}=i67b!cja`4|j|$z*Aa9?m`zC!C!@er0-CxVunUa$e z2XxMl56B=mw?n*WIcgs`TxVR`;yMiIiVD_7nTu}jLR>KdKP4YgR^>#_WO(6fPXOC3}JM}b2rC@Cd{ z0B-X1oL>2xK`Y)bDdaARLm=|6A5BzBydLQCV~k;;L>{fn;n{g-bvik(*sui)NirsO<@EniO)_!Dx6-u0~x^N ztVC6Fa|RLb!$}2pT3RTmsf!Hs@|w++A0e$Ly%YvrqqVhsPk_bm-{6r_s;s2s1Gwb+ z;r0lANv=@}6D-?X!Id>b_TjI}8Q)oojqrWtRnOp5s?0=hl4IUrW(1F6`=Y#;7vxEY zfqoz#=w7nzPwR`E3ApfUcEsCCW6N02CJ}XNVG&71Uv<(b? zgEA@p_DwpG@hSHQIjD$F?scMBudX%>(DPByqX=k#{5kLf4MhF+03qOS@xg&OZsn^m z6SBXtxzj>fc;RFWi0>Y=>0E2n$}?ts`0K?_TJo94CW(Y}|D3Ek`^1Fso%2t9}LP+}_vH z%Jgf5nP93gNJ456%H}|I^bb>7e*UTL=%J;IIgnUD9Ou3wCkYQ(gHF@fTA7FNRMS&4 z7~F&ezyuchUe8}??#Tg>fjbqjh2wj>Z!t7lN0-qW=v8Vzb;%Y%z=rO&Oc-NGm>m?+ zJNwc;aWFK2;QAv*L_`FTwsuQa*?2&hf5K--1D`ZoTKKYmWCYa|bb2$`92c8CpFsB~ zwm>6O2I#DV?Ux7RABl|kpeM0ETLT)Ee|gVlPuBZ<59(%W0~|ohaT6Rh3$O@a$^#_t zpyQ8V3gXla(czmISZA6L9(dWJ!jJ*56?CiKd`kw>I~Mf1;J6&;_$r!rpHutqb3^OU z4(L|Y_FT4@|JBuc9Dfn?shOK|zi$l(P8AsQJdWk}w;WHJ z4}^Z8)#$n$cnN~^LSkLQ-KX1fU1|4bg{25O98Zah`!aznoz-f_e(p=41wG7SMMz}Uw}MmzacT^xK*(!g zYwulJz+3zH0HR;0K@?!-A$CWA@Q$Afb8~}nCGO%uX2_sO0Lt3J(z4DK>+etjh+hSk ze5B`>gxt4|&nMKu#K$K{X(3=55YU|siyVQ5BZ}iPFqVZ)G9u{Oc(G4o$t3!*6ty$2 zRoz54mY9>y82SRDe@0CyLVWkSK1Ddl(+=ywkRpg5Iny$k;5S6gEH`)GK!PmIdvZPq zmu9_}Zs`>ad z7CHfntYm`$L;a-l?5<~e<7ndER$kigrVkI=g0ZdE>n)f;$-%r+ON@24Zx{$@X%!O- zZNNAOm?t3ujR<-WF~GwC>$Cw{JvM}Y*%T8~EPn*c7B@{)<12~)>5%DeA()NeUw~)e z(P$^^?-HS+BEwQ`_d!XJMV-wUHAooM3fYXFX}XnkFmkF~^vVLW!@vC_KZRr2wILT9 zx>g4xd5jBladqXmreCLSqHB>8NTJIL_Hlu|5)*rx=JEAyO6f95*~B^HZ>!@I5I6(9 zJ53`aEa(r@6;KA#k)804uZMul@!7BH6ZG0A1>pVEs!&W~w%ypoAaT#|B16bQ9La6| zhk4H1zlLXo=QDaTIm2#oc2&;92LBf+IrQ*8J;eq+Pp zO}T-ry*;~mUo04C_>MShQVrStwv5AEDHXiK&&r|nSUF;tb;Mb=MliG3j z)II029;Tf7y{qwjXlMxZWfEYp0V?l>A0Uru{F@hbHIefm0|EzFpyxwjmQcAEwPg8e zI=xdMfSo{lDMNTsY0kl!7a44ttxEET<;H|DgtIk_c|YF`p}_p}G=&Lb_7Zti=$$>$ z;+6>|RkON|-a0U?le5FTXK%x7_8%C?t!c>o7D_3>&sb-6(1>`w0F43E$)-%u_?Rx! z&rcZ26mlmObopXP@5)3}!%Z1r+G{}YQK#I~@LXv@Y*5uw^>J{MBiD9?{Q}oDg91bB&MQ~iR~Z;cZQ=?>K7g_Y^K5y!x$scXka_EWzz@VuMZypi9R`0v|M52gGet!N}0Q}rNkYl;1>4B?_n*TQokR{UQ|8w<64^LCW zuk6atYD=k*A&Kt|(PjN_dGi{T2S8q@g@A0$_Uy2LDr!$b&1ciMPa;r>^d?wYd0GQMO+sXc2%V4}x6nsZ9o!Y=Wvo zuSfu7>eH+tlX413C4T}1a^1uE{s)M=g;}2fZde6T+zg?V#4wDMJ`~(3Au|ng`>iX( zsj{o4*nNGi8QqT?2mTR><``t&bsrcLE?<3$Y9JFk75-MCqj%PnwV-Rj-;*Tzn%+NF zWmrd;M^16uXsr0Tx~6IZ#)du-^OCuA<%~PH4!-4fMjay~Bi6v3d@U++2l)vE(aX=T z`B}@_2pBVs1!!+_77OFIf7Wq5^s1KrAMFs>vQLq1iq z=V^U()V%j{I8$gMDXm~0p9u@hl6g?Ht(~^ogmp*e?7a?$L+`-H>8$y7AbS2G2Ho5O z(m}1F5*jM82%B-JuUccEuy4pLXGs#3p|`JAxnSZU%V*LGQ)-jviw$HGDvZ}HCWvxp z6T|7L+@su;d)2%kXJ$q}^VYvymvJK7m{K zYc4mavkn(U=&CyWJhQz1BY8I~{5-xNG1wo?S+-|}%C7C>O+Eot>L080V#@RRb=j<$ z^#O_3oA+ksn=FtRs8(3%EX=%%&b(GerSa=?I?jl~W}H)uDFN^vQ~x5@B^%!YSFoZzp5cgQjV#GIbiIyW}+qn%Rbkjeb)T}<-^)tf)391hph z)0wl&8uRaJX7AjZM5w2$k#+#o9@7$NtlQre+GV#XGjH>hj9`^; zxWR5;XE21ySvGiOt>Kx@JB>UdS$ z(#^|SYhYl&9E6SrkHAje9eQ_|U+m9$eKZzJNJEcl6T`LfQq6MjN1rZ;y+^bB=JMf;gwlg_Lu>^o|c z+?ta4M^F;^We=w!{AbFB^vQVi359nEFBw;>xH3P?;NX#!JYNM@(Uu` zKRmpD4GgSdjNK*z=wUbA zoeq!zZD=@SV=`n5l`}i)pzPYpf>8T)?N$zO&C2eeje|*tQA3NfPWlMz)EvX3%QQ5- zGYMfg-L9iIbLsx_ytoTx43`zq8fJ(DVN9QRi!$FzO_6agnk;Y3KSo17Nnk0%RS=e5 zfN7zpD*>DVP;Hq^q+#Gr7S)8tJf2L1Id&^Wck(8_e_Bt@3->5jUZUmxP- zu7yi;wOe9st>k7J?S|O z^iYv{s3ra?UXRTGqd4Y*8Q8iQH&p~o0c={+GMz`H$kzxV0+xIQ$p8omY4K->K8tep zfZTI7_SHwh&W;myFS1is<*K{ihZs)sa&dp#ll1xE*v3_F2JX;fyS?YEF1{+G`aHr# zN~TsFa0K&}=G7GB%1gYlSF=oyiD!sC@kLoJsr)nRqjbD zw&TAca{oNCdU-w#fa*zZ-CB#!7Ja=>4@iq307*Z6=%8nut#ht=V*_F~Ls(}IkY{_G zZ{^=(XGb%ht#$0rkj?-Y>Z`;+BmTZiFu%LrF{Q!n|)`r?>?EB66%ZtT_sJ*qyEEmx%*1s-IO>cez zmMTX5;r{6WNuBhr^i+7N^~+9Aal(HCd7Z-?hM$>E?uu!K3=_WXa?bEj2+w=e_)Ktc zFRe@%?$5j@hQOktyqxcNcdjrta;^lkEhIRY*J^~>VlcHAQZe*H0Ik>x>0^yf0S@yT zkb%#iKO>AMOHDwm8nL>%D)eb73ht88N${wSz4D{8-9-CiXp}v+<$M2G-8{o^lNAHh zZV+S2@<8z5;^p$&)V>$x+#fxTC8>OUwr%&w{N-rsy z1{lECcTsT;UC}fV8u>W@m;nx_X<(q{?cJovqwyYT7=Yf0g^5X?i9CPWkxPTS5_s)> z=rS!urvN5J+6F*FYmb|K)Uj4qA(ZKvub|Ll&b==|oy+{)_MbMZDB*>j(Z+R+`}!aC zcwM^dJAen=QLi5re8Fv_cg$Fs+w(U3k;~K6*6P8FU!OA>`Om+wq6UcNFtSAsMk?#e zP{`gR3nKoE5nTROK;aW7s=9vWg-YB9p@9n}Px*vT>h>QkL>GoOd`1N7gzFyedixBC z4tWeR{3v7iFx`dI!E$1^yK_bvH!?!rlVRciQ|OAey=8l;d9S6)TJZD@;peB*M+^n- z&SjUfHMA6{wyXkOjT6Hc2#$s9R;-IxbWT820nnqzZKL?yIt3&$nWd#fa#=)d@hUYU zaV8*oOXM}f%+ARHgeUy?*v+tat0bUZqPn_z87P((2!;7KD?k#~pU8^_1gs(_t)QEK z0qbNoWcwZHi~i#wffGf~p%wbv91 z@Dc!*)Sw&>&Ch?CVW+A0#Smp`cD9ISNlC%v^7Ie#<^cs-+TD$=t`_?Il7H|6z>ih_sZP&f(n)^tt%$=e=r6thmf6A~{u}15s-?1K z9p*$o4jK~b{0>voxQl9UEUd&?f9m1-Q26}TjRRh9p*cihplM9?>*$HXI`c%?%|Vu{0H`PZ9Sp^qLt@`FYj6$t}FkU6SW0xd1+ z4Gehcjv!4ig+N0TcE7&vHj6}3ndKplI!G)C=wQ5!P0NzQ;`Vj~X#BSr=Q(a!S$rV^ z5;6FPs4^V5%B*T}exIQ%9-NU& zdRFdUzmcK?6$i*TTfq$!17JMB!4k*Slzq~gW@meK$`^+D@!guFMGOM{2T>Q5*E*%f zD2SaKSFi?4f4b?AnylFP_+U8BM%km&)8TZ!m&Azu4e4tNh~;9jTWlnelRN?K{S%-; zU}R^{`u`NA{v^Hk#`U1w=eNugRjlP@Q?I73{+B#$Pc$_HAP25)Y?!GlVvU=-&>y)| zf4Yh;9PqNhl0!q(H6tkMWX#P)XuS`A*gFec|2t_$K|pug4VW3yC;-xm%|UIJxU8zG zTYz`m2R4p7Yb#kc)5ciF)m0FrX3@?YgDAs_HGf`9KysqW%F42M_?IeNfdAQBpr0I; z+A!u05_#7RM|sWadgj(%-23qWE4Q`mhW$@;ArmG)X?=BDorkA04~J!@^{t} zJ(_$e{4NyF6b7F;~=OEgJExk+iUGtek3X!#V;;IM z`63yJ#T9{M5TPcRot@Nu$}J8~0LVasIKo>OO5 zyT>$lU+v{Xo^0C zf4&$o(YzOl*%~#;iS;}r)JhB#WS?K=+@jiuGG3_F-|o(U;L)(enn76bG<>)ed(M$k z-%oHxko!QGNLEUSRoBqO!62sg(TW5@P($cFTy@h?LVZF@PYhi0a_jT#%1TnkSZt(n z4wg9tM31s06d{rAl>I&hXpi3&#QNyySB;Lyp+#X*ouKh@&yyj=kw0s*DwRK_i~`ac zIarT_hxc>)n`rJ@=(WInRiUDRDJT`XyY`AI3tCyy6KBZ{toRpEYl6u;P@9}|Q`TX^(l3ak$ zlp7!qfRF|EX|GO}zrY2B62n(i+L=lJ&(f~+b~%`J8-fh0?IBhLPxb6XCQ5VMVc zASaV^3gH3T((*l?-Sx3PwA9VA+LQ@>o#K0qfeKQOsuT z)g;q^Cr1Z%G%dg^&AulfyMk!iL#IuRRVAv!!g$WA;oL2sBE?yaD*??SYNDDSMH-`u2Hl3eTRaP#7YJ|PSpbpjD-CZq_2Sq*v&xDh$nlu|= zG6KLckOn1qp^x?TBZ8gh=#R_hUAc~z=ug;DOxM}yxrN!Ur=5oti({+g1I(U#v-TU< ztgxBJUF)A1qj=vlQY&*n(6g{~EqXDN;8fdA1pr${h}2qv(lP z1aAon^uwlW$9xJh{jT}E{-~gOM?nDkLJnd)EJV)Vd|f%0a;XEkWwaa4@_99G;RL~| zaf_1}eJ1OARNBN;e%_^iQnJ>TmOPeAhrj$fOhM92f9H-u4fhkbx~weO8n)*jeF>C& z$MQZ9pW(^9xxAltp~@BySI$ZiG}90q0yH(Ns0a^ioFfW(t~;f`B_g>1gE}V+(=u@Q z_HV|Mg%;1KE-*LWnX~c}qI{V3$QVx7+sMcc2p=aJtz`q>BNNLER(Lo_AGEs1KcX;@3g}7kH9EERRQcoNE6956OxB8kb{)T43PllQ7vNB>w zQ=!ex(g2Ll4lHl72|?(Csx5hJ)@Zz^Kub$2Pj&b#U-}9M;j&v+bqz5EMHuKfLC(zJ!#2dWG~5+)Ohl-^u#w`GxRGi2 zQ$1#J@y*_$ut;JfMa>JwH$OmazEO^#fxAjDI7#!cKM6NMuz{ zPmc$D0&3|JQ`WH-A@dUxUXi^8)i3(UprK|E5+Z|~i-Gt(*KiwAYH`+LF{a{`7`pLq zqMhVKs%~7_!B&y6Urj%*>V04$L&=!s78X7hZfTh{c`JB)TxzRh`qo%kS!M3u58@)# z<4Q0C<_5wV5Ha$$wnbC17S&(odoQo1UV;Un&KWk!&$^MpG;!g@adqJ;o$at-_oTJ) zoBnN|9yr(rO&tj7XROgXrrO*xw5!E2zpO!nel|h3Mc2U07Iavc2iZtMi?rNm^$^f?e{#(O^<{d_-Xpd*{Nn>)0g0OLPQus%sW& zW|&WAsihl7D8+%$2rK=S?h5xl1hs3Gm-#bw`cH|l9=AqMIPC3xGo0yqrVs*#L}I{^ z{9!M`)6Ks*P-i8b9?O+P!^Y$r>DOUUdH7HXt`ComjPGDhtgNY70mGMw`z|%h{RXom zXNiUjnrz0}p6Oq~Q-rMjl&b#Y8!?MI{^W?lDdfIk8i}kAcxPGqhYH_Z+jY*NW`ZeD z3MPKAp%hUk`n{Q2Vo4&0|c$|ay7#M{9Ok7re8xtgNCit#czIuOjiS~h4xWp{mil>;p+8>e#!7{&L=|U)Ii}p3kn5r zK_Wn%gqk^U4VwD{X>d#itK_x;*-(t@KY#B2ka@^8a=DFaVgLR=3_4YX5qEv^PJuyx+>v)xKo{mLA<6x^YG~KaHqtG0@#H}1D9VC|I6iq{PdCB zLX^s+n~hCPoa-89PgGHNzht49XN8{E+*|WNV{NNSbTWwivn-&FKF}7S&6`2^PH! z^pY#DNXp2O>>YS~e|*|(Nv5Gx9ubT2>G3JTV$m@M^%dC=eM?chY(huREjR)(=0Z9t&C6SNu6 zgaQw-|4uOaj!=E*SIJ+(!qyaI? z%=PMYpyxqHGy)MeCw2;+)J+2|XyQB#jXt{MA04HG1<9cCH8CiSJCC*}P%P0XeNGw{ zmeRd;{gs&mLPN1Y>K_HxM8jb;1V`g2{zDG8+1(dsH)(q$v%2O3ch+mivt=IJFpIE| zVDM*;82L_CcBaV(ia*jSHr*p3iYRkfl7q$sX~~c2HP@*?)2Wb;3U;K1L6$KRfoOl| zsggsUA3YCgp)Sn-x@=QT4^CI#v%crU;B?v+WRd^NKO~V;n9z)Ta%T4?u;uXr_=0jX1&j*R(3%5E74r2f_vsrc2<1>k2Ed&dgSmCG(&{F38&c3HsU?VW z^757?BtqYZM7n&?z$w zF1cYb@s{gJb@~PLnVFfH2tWho6jzA-4b-ShWV1g6CYn$)tB77bzlWLOTratLohSSI z<_-WA0?=iR4CrlHS8ZO*Bu=b3iSqID{{iETX?qC1R$1n`r#J8ltnBQgS4NUE22j7W z1>=e!txe21@EWhrCz=;`cjs;TdV5o2ipr)Y8sx4%JJ>)$ zxX(Ibfqdc}%=pLzzwO3Q2B^@gcU>Pod-ZB4#gxknSr_4+Ox~{yG_=A5cgdDz+FV9^^4r;Z{9j5-&8~_A2*GBi>(yDAzNRVgn;4!~N4K{!aM-519b& z3YOmOtMR|JDEWYIO4j%~TA)SGs zrou#~ObrbJWFMU2`;THs;s6iS0}Wx{SKdiSL&QVQc1aYp`N+i|n1%BXgl)(a3D^s= zg@e#nEN(GzMNUFO5|fzt7KrO5-|G|KKTt>8*uOvhA6fueW|Az}Z7R8%HTOtw08vi~ z;C_f^K~P2p9W1DHH z#_^J>wc+}8Ln*%&{GfTC=FxhSXSrL8Hq>~>Yo$(v0+k2~Q9W6>xeo&xlTZbL>Go}S zcpIcuq<9VH(8w^x(hCTX0#!tGBY6MtFbEdbHSLCWNg=~m^ws#5$4Pk;#|!+Fm7fLR z2)bnSicRTBLbyaK${^1`+^<8adjw)YENN+JWcN@@oZcV~1C0V;b_A61OTWenPeBH_ zw6zrmI+nJ90el)78tfZ40wAOz)#!ldw!$|7!Pwp1otp(Ske!I{KM6=+Sz3iir5aKQ z1dmTkOWU;WUN5!bR0lw%zYYg@i-cgjPi5ATy1J#UZ3pbyGLHibu&ZUm%S94(noph( zlaOSymmV*L(q-q!i~j|uelT1YKr)y=Ky<6C^-xDR)V%;j<|*@S{HT_Kj4@lTn^Jy6 zk1$b_+qc@|KYkyK@6r+Gz)(Utl^XFkX%AWIy_-?FNlh?wa(MC&3JOR&hkBpb;(8sf z&Rr$;ThPQ>O~kcx_Ksjw#~To~UkxslmZe5|&y9}WgfP|q} zUhVl3Mkxj&4J$8Snq9IAlfLP5ztM_k%^KD5-=`E@U^j_M@ zgKthS-=G^HXIiNRdc1zUiSJ;%j&)c!%29S&0V2iiL8_KE8|Zp#-F7IquOOI4!5)Cj zgXQ7jaoSGz&rjRP0|T~osA5hlv@W~=)lo~lCfXQG8?BS1Kz7mYy9Sqk-uVLl%JW)+ zc6M&Ab>+`t{m5Z%smU}C6B8Qp8ITSxAZh_HH3MTDz>B}{=!6o`jD+DMAoV>xN%8RV za(6Eb7Voar;#f9b5tAazEpscj3lbsweN(Z~w=md$@KnmiYqp#BO#R7N04#q zp+_mA!R|-mA%s$=^vJODg+^EgGJa5b!F{q9!yu20j^;Gh?#YWN2<2}xG&uaT4u|OM6MAy_i>vk4 z8!82M^>NrADKN-1Fi5&}dpanyckmK1T%)o1WBZFzYbx~%fhvSDnShwXm;=bx_I5&P zJV;(|j>fcKuf=P-F!c7XZ)}9ZhBo;h52L5hHEmid&8n3VJ<`<^~<;)ulxL+ku)<`HRuLSy}UcK1?d5wcSI z`>4h%W*?oYc(Q(|i4iLW_2zeeh^-djOHvbGR->K#@T@=Ji7Gw;P#u3j3?|DMi>!0M zHXT#?z;*4jSJv0_w9ONhgoSIQf)z7@feh6_Ot-xvC{77OsaRmz8&cqcWCBD*MPcgLPhCOqbnYJ=DY&=@ zAX_>Z(&moKh+OaobqBYm%}}v>zVp)Rofh1|_3M>i3o2{rlBUp+ZgHX^IJkToi&Vf!NwFCMHIzkGQnT6M5ILV=&;G`+it>JTR0c6M_XW-C9?v5U^kj5DDnF zYhAWTkmC-aMmGjJ0~-%72uZUpXYUEdtK{CK2J{NVCeW$1_x0hz4MI}{D^u%pB?e~T zrHzgJ3RC3BArQ$xqR5bOcXpPCVHV)yf2`?)3;Z6DWaPF%z|CHTe;$J2K4!PbiE;Tq znust8gtm`Vs*|HJM_0`!jD#rKC{+R>=aSfEzW&Nnk*loC`miZI<0}mj!BaM>rsijA zc;yupF!}41tBrt&2)Qrh86>F+Y&N~PIMvkDR3eCm02uLu@?cwqlKdt|OYNves)#@E zg~-Yah@m-yARiwer1A@v9DoF`7aE(mutaKUVz(B2uO(@(RN22rd&&M@F~HG4o`${F zlp;Ly+DU`xLR<|-ox!Jy+67~)m18tY$b zL%jqy>v;C}MPc2TC!f#dYnYdWicA17TJJ5$6P6*pyatg8F)eL098Dkxp-*dSMFm_D z_CJ)}tuW}(E35;`b>x5*kcIenuWYIFn{z<5P`_7y8Ktl6+dWz!7W)~SOY`09!q(SWCHWm&&XmWkfyc?7zdlmV6!juFy8FvZV{DCiiZaf0YbN^ zWm3BBfPF^7O3-mQZjIc5VlJz`UQheA-NHBTE86ab1v;o2VWY~z7@W(?OV8YUdh zJyvaB`9pN`W+349#z$CoJ+6GMyrbV8DOw%ZN;d_(s_hZ<9`#z2u64!rw$vQc~E2gcEj@i6?MaQ4_<5SXuSo2h0WOEn=5D* z5Wk1gx=M{(Q4uWw?VxABqx#DKV6&^(ZqVI6O)Hucs|Cn;+c}O@Q}J!ZmunqXK%W0m z3MHmi)feGQ!h<9ZcIh9kI>&4xfW9(3+fs4>h?Vb~wUT_?0Vk$l`}YSs)Yx&0A`E@H zI|2*GOQ}}QB5$!*m*LgAW#~s0ESV`gOzKHSx|$ItVc_ZInDRm&jS7dj5bge<-P)%; zw{O%o<`6?RFCHu8?V9hn3VW9wznhL05f`5+p*mhy-_Z*yv3bl9E4%|o8ccVk7nXKBy>%2?O7K$Kh4e>Adbb|M=O|){`~P*$t}NVZIxNx*@=R%kGSk2Pua+r>zk9>Djtf543T=Q?&QIk-=tS8XfaoKChUsk3bYIVjHf%dlrRIfWF zCa;i;IkmG1`W9H}5q^Mm&M|P6Ksv%iIDjSt6#)WSg%UwDp6&17#5Pmyt5&^c8N!Um znpj%G0FMwrPmd{=b#!%D>`nze`ijQcEJFrt}2yAhbh> zgi;tvNd$t<=ad0q3}yGCg9!AC^?To7kX1vfd&TWm_{e0P4ihRyPX9OyT8CS$#ArMo z9rvfQ3ckez{uG@)OQjKan~V=O3zIN7J3B+NVydbYeF8#~v3+aEl*-7+2s2eb z6^imhaOBV_MZo2Ob7KnL@@KCd>-p}e-fHr!pl`k?rNs74Km?9_E$h8|t*}IPx5=V& ztrsV~C{w-_QE?^A*m=Q>SzedTM+Qxvx8UvEjpsF9Gxy$r_39AU!6IZ9H}i^FJggce z6&IRn^v`+aOhnNk>*FH@rcL9Tg)!2?yVBBh$kV5;UNr-EIMv|JjWGVZ+B^5fhHnO9 z90L)y1E`f6aF0uGx1LQun6pM~FQUZqhexwx6R^AMSqEI9;~w4-^LoD4qSVp}MTF}Z zkDak zn>T3`9T0Xdlx9B;NmD_f zf3*fk(3B*Nb~e4+(FnQ8R<)clT20n8oSDl^kY{atqYjNxEOPO1LvwrRSMShJs4}yh zQ@Po!ndHR$=8OO(K6K~PGc$mv?RVUKNG~Qv3F!gJ5InF$yuHr|Xzc2$=-0&<{Q*Jr z?t(e1CdJ3@^JHmoK)cpI5>UmOuySw&spQHzIhD%?$)ODgi*Ah0j&QF+Hf4Oi)|n6n+X zSbz3IA-PuU_c<825Ncp%CX(hS=QDq6v>npEe{U)_g_NyzyJ5I9PCpt2`U3QQ{{kgDim#4?C^WGD-iu8wXs;C zn;OXHD!Yu|A}6<)L;yYBeKB^3N49xS1dtbadPude!K(AX1%Zu@3Sar@-MsC^iUBK; z<53klB8CbthinW^v1=!5UM={AYdqH?WD<#9)`$c6nv=5=_()=EYTKu!ep#HLxIl6% zkdMsyl2jw2-@SYHWM~B;v#h2Sz!WTm2uJTu_tk&mt|QWUM#&%=9hkF+kU}MZK|inJ z57~t6XN{XS@kiX8L^9#!=4xs-_v`ah-gfa^U8~goD*QdH3l=JU)}ogqXx4(v5s2F zC4UE4WGK-gd-0GH%ssIs2kv77Dt}P9kc&DoOI=?&!^}`>$hb*KNnAA=p-J8M?oEj7 zh$<>LG0<$(N64f-bI^&xBcR=87>DkPG^3GPx#{;MOmPvA+~Yd5sM6t+wM3JLBzbq zVsB-@?M^~hVnfMBO}hw*rugpvo6aDTuB{WyM@Xuu@NM+H0Oo`~n(u8U0T_q6!jb!j zx6;O4x7mdHa{xbtK`LQ@+`{Bn5<2fgVyN8=a2ayN)pY37Bm~sP2H#GIJ`Gd03go!RJOYWoJxss{b^@CFTkLxb556`Wd9BviWG)fvgu2DfK)+e{TIICDTWQyq8`4Dz zp}L2?MXkKSkg4rv#Er8xyhTUPZv>Cu=E!_#o2UeE$wR3#bf|uK0S7C4wIGE|gPt~b z8T`}q2bOO8iRCPo{%RuWuANuN_;*{l@mo+KmBRB{ht^dy9c$Zf}^uBi!$QAW_wkyiaLMYF|fUgIOaPAqj*GPqeP0gklB)moJgesmlBwfz-|RN zi22ib0Sg-|D_IkhEDb-z=B=a>XEB6JHU`g`^;dYYLJ1L{Td6r-H=!Zt8Nh75^}#fozm96^R-kc2EqHpPOvl#A zC8ZeF9;8$F>*_W(HqbrpT8u%VLk0r`;D13dG(-qGnKqyrkxJ~hoy$h8XpzG_4kFah zFf2i)dA$^T8#zqm&7aQO$Z>`a-8AkZ1FkwS+^LB!zd`g$vU{Y>I&_p#N zp?#?~NEP2$@?G3!&xUPD@J)=18)H;>WM`vK?PN;6ZQ(}yH(#7D?EGoOh|bz@Wv5#i z=oZoW$BUKl>ZdWBsYDRQ-)DSWQ^gcnmHorgMHUSbIVke1xox^)*4l#P(PZrHOQy&O zQf}mQee;2a3?@c{*O>S}dEpfOuf;hzXf^h8a^T|>6B8T$@dG7EEdcM6!owU3kbbps$PrbU3=;l$3(699j-&xQ&sR z8+eW9tA!6|wk{TAcAh_e><=nsM9W?AX&&y@xmufsq3xvhf8^Bsd%R51#Eac3I(yW{ zxZOoVLhh_(lr7ulbVTT~wdfXBmPnkn8pA3 zXa4(l{_7+Dci*|MJzIc--FD`1K+?u<>QZxEHD)BQzawZ(P1Qp94pVzE$7?>zGT=*N z40?Bxh9Zv>TceT&uT_SA)x?j)n(p0i`l^2@Si>KVrm2#9+uHp!RV`bEXmf;?#rHtoMG#pdV5mWqZNdFp#7zr0g8xi|u1pSao$5z>wA zNqzatSUF}Cn0+ch+L>>t@-|h)0j?<9(xj?Bt>m~BsgB-#xL2J70B}Xct?oPWtZ8T^ zpgz7*Qu8;y|MF#8Iqg6>_0Ki)WY>YPmG`gcMsggSu!oJLnw%RKS76kq5f*7NctB5XyqvZR(WS$K`_H%4{qh9;|(R>RfuNkLB< zQ0iP3W)N%jtgy>iNnDbmmGGM_|8~>{orU`jUGh1L+>`N{OXsttoK#I0We4oGXygw+ zU5%^SSM?;#u2mS{d;5F1iX~hh{m0=d%EcR>Q`VqQ-xOf~1s&n|?60BRq6&e}G{3ps zo^D`sxxa~0X6woLfn}x=sFHHoUx%@0m84iZ%O!;lxTaPcu<2`YRMSSm- zylgZ@i}mj18qW)ouIjZf32r%4O=B@nM(2V&Z2W#45s{Hk)21M&5|>@FEiNE$C&~HQ;8r?-)Vt z^OzMU8lT(TG4&5IH@=51rlCW>wX<35E9ITz*~FjJC@0o*w-N?3ca>R^UnoWXYlM}@ zlxCdXJfW&6H!Npt|8%_TF$mjCj>qXsZ*xlaY2{-kJJ^KSX6uO-VLy$w1-aV}2uE`z zc;mktzM>jqD2N|mor`HxaS0j`71}<;Y^!4VFlbWs{JH699Gt!^Edi#})&#X(dDs;> z`}>4~z3L+g+X3wRCOb~Vot$lO1ALyO-K(?KAJ-NYyZRL89$mh*jQjR{qNv{6o6y;^ zr_Xdvg(NaDAf&NKO6llw-!e{|5K(*nVlPHpC>vWQ0e3*>=XZ+_zU6rB6Eu9^)jzRu z|Gbk@d8^v3xXuGdH_ThYE!Gu&83Gv9be`=_0%Mlmh&_#d@JS&f_AVQoF?>(ra%2L*ZOKB_sl40{BML< z%`X`@7nBpOD&2LLYSZTlFvx2_$Nkeuz&yc=it0J>nT08m)Ax%plZgV$cn`dB*D^Sq zdxR`M(#RSP3dOZYVK7w|Tp!I{!rM;%{ZZ3E%*^4ZeK>o0p-gF-wpC9bDulZ5= zoZ79WNurBPul6nwyl~Fg)}0Jmng2jzF-Hp@8S|=iZ{hvM?Qp-dJ{?`h@oI;DVlRZZ z-ryki3rYtRjmyPkDl zg${V0hSvWSgzdmrccm~Ir>pc=7p>eu+2O6!=c#6j<|eV*w=*aMmv4qn5#1i&j znb>;7p7Qv(m!G4MkroBVRB*4;bx4wAV!J!~vgqW-e*rhI_B$gQZm`1#+h`ey>G#U_pvw0Aw>%`mb@ zE~ePMA6fWevNBDo633(Lc~_7YUPA&??9-tiP`276s)3%EU#$3dZ19t zZBeH@J^bw~!z>=q6{m~lg^%YKKScP>?ey)#Cz)z!Tms?D=fq)+l0+&SiA{q&iA0vF z;oo?BA6_sD9H~1r1z8q0M6X{mO@9D4;~y=bGcpLhdPS!cDaXP`6_V{rDMv{V+>tS! z=DUSJUvy^fsoGaDMe`*{aq$H2@YicyPnUkw6TPw%3RYt9kfO;EN*&3&AFgj+@s{TZ zG2~J3UF{vSzyzPUiXL2!Nd2Lr<#*4>S%(|%Z&B>4s!%xe#i55fh}yc2hAornGDif6 z;*#DEz`3L-%6`!rlN||2P$;3vntMZuMC=_x!AfYrQeZ;Ch;_5e^ zXR(lnlJwAITTvjl)KvR&ojf5frc5HV=yJNq8mLQjlc3B=Ifq0KJXN~Ur|wUEe*}f_ z2#ygT(EFcT{1r{9w+O*UThp^CZ0b_kgtV;oFAak!zg*>u7yoss{K7V~NQd8OMq$fz zfg1eR{u0(NC+mMn7%vOdS$1;RxXs75!t(B>2%NuQDOB&&igkx`TU243h(^^a?8@*Z zB>@?|k}pMJJB1RmIGnKx3WbqhmSk2sjw3BG{bj0P>SL&oC$qTe4dgwr zUC%;UwJIi#AWAN$g<=WjzQSuZ@gA(EHqqXjiIg1?x2vDeXRqhDySk&r)W_rRSsg=RiYF+g5tCoecmn2f)7 zaQ3t97pLTKp}*aS_fsLU(tAkFnJ4s(9d*?;CQ`fG786;#Pa05el8$H}|NI^IF~s#w ztl+D({6}J;N2%$Sp5>hpwBu{$Vx7~}$?Xh9Od_^iirfrABh06FB&}V`h$B<2okIJW zm!Fbm2zmTXrIY&3JYfel=Ci*IWx0P=Tk66JAyr%HLvdMA7*q(S;U`V6OZYc`^w3g~ zqdp)v%puN~c@$pqvZOMFU}#(`RBy`G zx$jFu4;g;#iw6+0(8n%>^nWW>{fv5vqpdeY5T+ht^`wFb>L?1LA9d+lc47h{fT`u)fSeNpnGdsNjz3hFrw7S zP`iG8nXjqiZ>W0l>qKCKP8L?WokNG~`ZYz!S83Znm+oDc}ZJ6;~qq_~#nQv(qi^OXzoy<`rVR128ZqSu(9z-x`Psyq^xDwt>QtIm(J|b{^%!1j>YUl`onl!U095 zA?c(ebFjHH?cIFZ^$F^tRlh745*OFqESL5VUzu;z zJy*{fcCHGZFw72*lyo%=Yh_m53QK$UIE~8rpD6+ej5qk}yO8pUq!rtRh>Hd7_D5G>>miy?yK@ zX(q!1RY5?hDmy*h>cK>}HR3B_Wqgj3Wl2@xAM5{3e`s;=k&plvtJr%4{q|Rx$PPZnI9JS`bDWsohNcfc&OGXV z{zh)B+R{ygjI~6J1cq|DZF0+M!mQB-^VhB8zdr_7UizboK(!S>k6?BdJuQ_F#9pVv z$*H2CJJgk|Yqp*cJ1R9^?CMl^#fo;hfft5{ykd~a5=zYP-H-=6){*?nSA3T3LkF@p xssH&g{W1w|`B;@*VE};q|F8eIH-P!}8tV Date: Wed, 29 Mar 2023 13:29:56 +0300 Subject: [PATCH 17/22] Readme updated --- Readme.md | 56 ++++++++++++++++++++++++++++++++++--------------------- 1 file changed, 35 insertions(+), 21 deletions(-) diff --git a/Readme.md b/Readme.md index 0e6a19c7..4c9ad1bb 100644 --- a/Readme.md +++ b/Readme.md @@ -1,33 +1,47 @@ +

+ + ROS + LinuxCNC -Hal_core is a lightweight hal environment. -The installed size is approx 6.8Mib + +

+

ROS + LinuxCNC HAL

-A hal environment can be used as platform to run realtime applications like: +Hal-core is a lightweight HAL environment from LinuxCNC. +The installed size is approx 7Mib - motion controllers - robots - cnc-machines - parport, ethercat applications - research and development - scientific projects +### A hal environment can be used as platform to run realtime applications like: -Packages needed for the installation (at the bottom): + Motion controllers + Robots + CNC-machines + Parport, EtherCAT applications + Research and development + Scientific projects -To install: +### Install hal-core: - Install hal-core: - $ git clone https://github.com/jjrbfi/hal-core.git /opt/hal-core - $ sudo chown -R $USER:$USER /opt - $ /opt/hal-core/./make - $ sudo chown -R $USER:$USER /opt/hal-core + $ git clone --recursive https://github.com/jjrbfi/hal-core.git /opt/hal-core + $ cd /op/hal-core/ + $ sudo ./make -Before run: +### Before run: 1. Modify **config/hal.yaml** file with your Hardware configuration and then run **hal_config.py** to create our .xml 2. Run ROScore and keep it in the backgroundwith: ```roscore &``` -To run: +### To run: - $ /opt/hal-core/./runtest - -https://user-images.githubusercontent.com/44880102/129791198-ab705999-23ca-4004-a5f5-f0bd3357b47e.mp4 + Setup HAL + $ ./runtest + + Run ROS listener + $ halcmd -r + + +### Requirements: +```bash +apt-get install -y build-essential +apt-get install -y libudev-dev +apt-get install -y libboost-all-dev +apt-get install -y libreadline-dev +``` From f51ed198bea53059c5580c2aa8aaa26deaff173e Mon Sep 17 00:00:00 2001 From: Jaime R <71790456+jjrbfi@users.noreply.github.com> Date: Wed, 29 Mar 2023 14:00:08 +0300 Subject: [PATCH 18/22] Update Readme.md --- Readme.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Readme.md b/Readme.md index 4c9ad1bb..ce7bbf23 100644 --- a/Readme.md +++ b/Readme.md @@ -23,6 +23,7 @@ The installed size is approx 7Mib $ git clone --recursive https://github.com/jjrbfi/hal-core.git /opt/hal-core $ cd /op/hal-core/ $ sudo ./make + $ sudo chown -R $USER:$USER /opt/hal-core/ ### Before run: 1. Modify **config/hal.yaml** file with your Hardware configuration and then run **hal_config.py** to create our .xml From f23c19afba8269562127f92177e674f18768dc6c Mon Sep 17 00:00:00 2001 From: Jaime R <71790456+jjrbfi@users.noreply.github.com> Date: Wed, 29 Mar 2023 14:03:33 +0300 Subject: [PATCH 19/22] Update Readme.md --- Readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Readme.md b/Readme.md index ce7bbf23..c1a6ac85 100644 --- a/Readme.md +++ b/Readme.md @@ -27,7 +27,7 @@ The installed size is approx 7Mib ### Before run: 1. Modify **config/hal.yaml** file with your Hardware configuration and then run **hal_config.py** to create our .xml -2. Run ROScore and keep it in the backgroundwith: ```roscore &``` +2. Run ROScore and keep it in the background with: ```roscore &``` ### To run: From 4a48c69b574a1068ee8c714601808ff9b28907e1 Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Thu, 30 Mar 2023 02:57:10 -0500 Subject: [PATCH 20/22] path to rosdb fixed to run the command anywhere --- make | 1 - src/hal/utils/halcmd_main.c | 9 +++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/make b/make index 7c9e1a31..762e444e 100755 --- a/make +++ b/make @@ -16,7 +16,6 @@ else cd /opt/hal-core/src/hal/components/cros/build && cmake .. cd /opt/hal-core/src/hal/components/cros/build/ && make ln -s /opt/hal-core/src/hal/components/cros/build/libcros.so /opt/hal-core/lib/ - ln -s /opt/hal-core/src/hal/components/cros/samples/rosdb/ /opt/hal-core/ ln -s /opt/hal-core/src/hal/components/cros/include/ /opt/hal-core/src/cros fi diff --git a/src/hal/utils/halcmd_main.c b/src/hal/utils/halcmd_main.c index 194cb1b4..453bfc9a 100644 --- a/src/hal/utils/halcmd_main.c +++ b/src/hal/utils/halcmd_main.c @@ -148,14 +148,15 @@ static int set_signal_handler(void) ************************************************************************/ int cros_main(){ - char path[4097]; // We need to tell our node where to find the .msg files that we'll be using + //char path[4097]; // We need to tell our node where to find the .msg files that we'll be using + char *path="/opt/hal-core/src/hal/components/cros/samples/rosdb/"; const char *node_name; int subidx; // Index (identifier) of the created subscriber cRosErrCodePack err_cod; node_name="/listener"; // Default node name if no command-line parameters are specified - getcwd(path, sizeof(path)); - strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); + //getcwd(path, sizeof(path)); + //strncat(path, DIR_SEPARATOR_STR"rosdb", sizeof(path) - strlen(path) - 1); printf("Using the following path for message definitions: %s\n", path); // Create a new node and tell it to connect to roscore in the usual place @@ -276,7 +277,7 @@ int main(int argc, char **argv) break; case 'r': cros_main(); - break; + break; case 'C': cl = getenv("COMP_LINE"); cw = getenv("COMP_POINT"); From 59e049e5ee412955cc661124bad587beb95f41dc Mon Sep 17 00:00:00 2001 From: Jaime Roque <71790456+jjrbfi@users.noreply.github.com> Date: Thu, 30 Mar 2023 03:43:22 -0500 Subject: [PATCH 21/22] Return when success added --- src/hal/utils/halcmd_main.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/hal/utils/halcmd_main.c b/src/hal/utils/halcmd_main.c index 453bfc9a..df2bcf56 100644 --- a/src/hal/utils/halcmd_main.c +++ b/src/hal/utils/halcmd_main.c @@ -103,6 +103,8 @@ static CallbackResponse callback_sub(cRosMessage *message, void* data_context) retval = halcmd_preprocess_line(data_field->data.as_string, tokens); // Run the command retval = halcmd_parse_cmd(tokens); + + return 0; // 0=success } struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time); From 084d99c6aa1b2f4cf0ba87302228ab457c922d74 Mon Sep 17 00:00:00 2001 From: Jaime R <71790456+jjrbfi@users.noreply.github.com> Date: Tue, 28 Nov 2023 13:29:43 +0200 Subject: [PATCH 22/22] Last push from local added --- src/hal/utils/halcmd_main.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/hal/utils/halcmd_main.c b/src/hal/utils/halcmd_main.c index 194cb1b4..2570f484 100644 --- a/src/hal/utils/halcmd_main.c +++ b/src/hal/utils/halcmd_main.c @@ -97,12 +97,14 @@ static CallbackResponse callback_sub(cRosMessage *message, void* data_context) cRosMessageField *data_field = cRosMessageGetField(message, "data"); if(data_field != NULL) + { ROS_INFO(node, "I heard: [%s]\n", data_field->data.as_string); halcmd_startup(1); // remove comments, do var substitution, and tokenise retval = halcmd_preprocess_line(data_field->data.as_string, tokens); // Run the command retval = halcmd_parse_cmd(tokens); + } } struct sigaction old_int_signal_handler, old_term_signal_handler; //! Structures codifying the original handlers of SIGINT and SIGTERM signals (e.g. used when pressing Ctrl-C for the second time);