From 124ebd00b7056dd8fa1f67db92d79cf55a59498d Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 13 Apr 2023 13:37:43 +0300 Subject: [PATCH 01/19] Bridge plugin sources --- Cargo.toml | 29 + DEFAULT_CONFIG.json5 | 74 ++ LICENSE | 277 ++++ README.md | 3 +- rust-toolchain | 1 + zenoh-dragon.png | Bin 0 -> 33031 bytes zplugin-ros1/Cargo.toml | 82 ++ zplugin-ros1/build.rs | 21 + .../examples/bridge_with_external_master.rs | 31 + .../examples/bridge_with_own_master.rs | 35 + zplugin-ros1/examples/ros1_pub.rs | 78 ++ zplugin-ros1/examples/ros1_service.rs | 75 ++ zplugin-ros1/examples/ros1_sub.rs | 74 ++ zplugin-ros1/src/config.rs | 160 +++ zplugin-ros1/src/lib.rs | 103 ++ zplugin-ros1/src/ros_to_zenoh_bridge.rs | 119 ++ .../ros_to_zenoh_bridge/abstract_bridge.rs | 258 ++++ .../ros_to_zenoh_bridge/aloha_declaration.rs | 124 ++ .../ros_to_zenoh_bridge/aloha_subscription.rs | 223 ++++ .../src/ros_to_zenoh_bridge/bridge_type.rs | 21 + .../ros_to_zenoh_bridge/bridges_storage.rs | 272 ++++ .../src/ros_to_zenoh_bridge/discovery.rs | 301 +++++ .../src/ros_to_zenoh_bridge/environment.rs | 82 ++ .../src/ros_to_zenoh_bridge/ros1_client.rs | 87 ++ .../ros1_to_zenoh_bridge_impl.rs | 245 ++++ .../src/ros_to_zenoh_bridge/topic_bridge.rs | 131 ++ .../src/ros_to_zenoh_bridge/topic_mapping.rs | 76 ++ .../ros_to_zenoh_bridge/topic_utilities.rs | 39 + .../src/ros_to_zenoh_bridge/zenoh_client.rs | 124 ++ zplugin-ros1/tests/aloha_declaration_test.rs | 331 +++++ zplugin-ros1/tests/discovery_test.rs | 343 +++++ zplugin-ros1/tests/endurance_test.rs_ | 145 +++ zplugin-ros1/tests/ping_pong_test.rs | 1156 +++++++++++++++++ zplugin-ros1/tests/query_performance_test.rs_ | 202 +++ 34 files changed, 5321 insertions(+), 1 deletion(-) create mode 100644 Cargo.toml create mode 100644 DEFAULT_CONFIG.json5 create mode 100644 LICENSE create mode 100644 rust-toolchain create mode 100644 zenoh-dragon.png create mode 100644 zplugin-ros1/Cargo.toml create mode 100644 zplugin-ros1/build.rs create mode 100644 zplugin-ros1/examples/bridge_with_external_master.rs create mode 100644 zplugin-ros1/examples/bridge_with_own_master.rs create mode 100644 zplugin-ros1/examples/ros1_pub.rs create mode 100644 zplugin-ros1/examples/ros1_service.rs create mode 100644 zplugin-ros1/examples/ros1_sub.rs create mode 100644 zplugin-ros1/src/config.rs create mode 100644 zplugin-ros1/src/lib.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs create mode 100644 zplugin-ros1/tests/aloha_declaration_test.rs create mode 100644 zplugin-ros1/tests/discovery_test.rs create mode 100644 zplugin-ros1/tests/endurance_test.rs_ create mode 100644 zplugin-ros1/tests/ping_pong_test.rs create mode 100644 zplugin-ros1/tests/query_performance_test.rs_ diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..96309bd --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,29 @@ +# +# Copyright (c) 2022 ZettaScale Technology +# +# This program and the accompanying materials are made available under the +# terms of the Eclipse Public License 2.0 which is available at +# http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +# which is available at https://www.apache.org/licenses/LICENSE-2.0. +# +# SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +# +# Contributors: +# ZettaScale Zenoh Team, +# +[workspace] +members = [ + "zplugin-ros1", +] + +[profile.release] +debug = false +lto = "fat" +codegen-units = 1 +opt-level = 3 +panic = "abort" + +#[[bench]] +#name = "benchmarks" +#harness = false +#path = "tests/benchmarks.rs" \ No newline at end of file diff --git a/DEFAULT_CONFIG.json5 b/DEFAULT_CONFIG.json5 new file mode 100644 index 0000000..7c57ff9 --- /dev/null +++ b/DEFAULT_CONFIG.json5 @@ -0,0 +1,74 @@ +//// +//// This file presents the default configuration used by `zplugin-ros1` plugin. +//// The "ros1" JSON5 object below can be used as such in the "plugins" part of a config file for the zenoh router (zenohd). +//// +{ + plugins: { + //// + //// ROS1 bridge related configuration + //// All settings are optional and are unset by default - uncomment the ones you want to set + //// + ros1: { + //// + //// ROS_MASTER_URI: A URI of the ROS1 Master to connect to, the defailt is http://localhost:11311/ + //// + // ROS_MASTER_URI: "http://localhost:11311/", + + //// + //// ROS_HOSTNAME: A hostname to send to ROS1 Master, the default is system's hostname + //// + // ROS_HOSTNAME: "hostname", + + //// + //// ROS_NAME: A bridge node's name for ROS1, the default is ros1_to_zenoh_bridge + //// + // ROS_NAME: "ros1_to_zenoh_bridge", + }, + + //// + //// REST API configuration (active only if this part is defined) + //// + // rest: { + // //// + // //// The HTTP port number (for all network interfaces). + // //// You can bind on a specific interface setting a ":" string. + // //// + // http_port: 8000, + // }, + }, + + //// + //// zenoh related configuration (see zenoh documentation for more details) + //// + + //// + //// id: The identifier (as hex-string) that zenoh-bridge-ros1 must use. If not set, a random UUIDv4 will be used. + //// WARNING: this id must be unique in your zenoh network. + // id: "A00001", + + //// + //// mode: The bridge's mode (peer or client) + //// + //mode: "client", + + //// + //// Which endpoints to connect to. E.g. tcp/localhost:7447. + //// By configuring the endpoints, it is possible to tell zenoh which router/peer to connect to at startup. + //// + connect: { + endpoints: [ + // "/:" + ] + }, + + //// + //// Which endpoints to listen on. E.g. tcp/localhost:7447. + //// By configuring the endpoints, it is possible to tell zenoh which are the endpoints that other routers, + //// peers, or client can use to establish a zenoh session. + //// + listen: { + endpoints: [ + // "/:" + ] + }, +} diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..e48e096 --- /dev/null +++ b/LICENSE @@ -0,0 +1,277 @@ +Eclipse Public License - v 2.0 + + THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE + PUBLIC LICENSE ("AGREEMENT"). ANY USE, REPRODUCTION OR DISTRIBUTION + OF THE PROGRAM CONSTITUTES RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT. + +1. DEFINITIONS + +"Contribution" means: + + a) in the case of the initial Contributor, the initial content + Distributed under this Agreement, and + + b) in the case of each subsequent Contributor: + i) changes to the Program, and + ii) additions to the Program; + where such changes and/or additions to the Program originate from + and are Distributed by that particular Contributor. A Contribution + "originates" from a Contributor if it was added to the Program by + such Contributor itself or anyone acting on such Contributor's behalf. + Contributions do not include changes or additions to the Program that + are not Modified Works. + +"Contributor" means any person or entity that Distributes the Program. + +"Licensed Patents" mean patent claims licensable by a Contributor which +are necessarily infringed by the use or sale of its Contribution alone +or when combined with the Program. + +"Program" means the Contributions Distributed in accordance with this +Agreement. + +"Recipient" means anyone who receives the Program under this Agreement +or any Secondary License (as applicable), including Contributors. + +"Derivative Works" shall mean any work, whether in Source Code or other +form, that is based on (or derived from) the Program and for which the +editorial revisions, annotations, elaborations, or other modifications +represent, as a whole, an original work of authorship. + +"Modified Works" shall mean any work in Source Code or other form that +results from an addition to, deletion from, or modification of the +contents of the Program, including, for purposes of clarity any new file +in Source Code form that contains any contents of the Program. Modified +Works shall not include works that contain only declarations, +interfaces, types, classes, structures, or files of the Program solely +in each case in order to link to, bind by name, or subclass the Program +or Modified Works thereof. + +"Distribute" means the acts of a) distributing or b) making available +in any manner that enables the transfer of a copy. + +"Source Code" means the form of a Program preferred for making +modifications, including but not limited to software source code, +documentation source, and configuration files. + +"Secondary License" means either the GNU General Public License, +Version 2.0, or any later versions of that license, including any +exceptions or additional permissions as identified by the initial +Contributor. + +2. GRANT OF RIGHTS + + a) Subject to the terms of this Agreement, each Contributor hereby + grants Recipient a non-exclusive, worldwide, royalty-free copyright + license to reproduce, prepare Derivative Works of, publicly display, + publicly perform, Distribute and sublicense the Contribution of such + Contributor, if any, and such Derivative Works. + + b) Subject to the terms of this Agreement, each Contributor hereby + grants Recipient a non-exclusive, worldwide, royalty-free patent + license under Licensed Patents to make, use, sell, offer to sell, + import and otherwise transfer the Contribution of such Contributor, + if any, in Source Code or other form. This patent license shall + apply to the combination of the Contribution and the Program if, at + the time the Contribution is added by the Contributor, such addition + of the Contribution causes such combination to be covered by the + Licensed Patents. The patent license shall not apply to any other + combinations which include the Contribution. No hardware per se is + licensed hereunder. + + c) Recipient understands that although each Contributor grants the + licenses to its Contributions set forth herein, no assurances are + provided by any Contributor that the Program does not infringe the + patent or other intellectual property rights of any other entity. + Each Contributor disclaims any liability to Recipient for claims + brought by any other entity based on infringement of intellectual + property rights or otherwise. As a condition to exercising the + rights and licenses granted hereunder, each Recipient hereby + assumes sole responsibility to secure any other intellectual + property rights needed, if any. For example, if a third party + patent license is required to allow Recipient to Distribute the + Program, it is Recipient's responsibility to acquire that license + before distributing the Program. + + d) Each Contributor represents that to its knowledge it has + sufficient copyright rights in its Contribution, if any, to grant + the copyright license set forth in this Agreement. + + e) Notwithstanding the terms of any Secondary License, no + Contributor makes additional grants to any Recipient (other than + those set forth in this Agreement) as a result of such Recipient's + receipt of the Program under the terms of a Secondary License + (if permitted under the terms of Section 3). + +3. REQUIREMENTS + +3.1 If a Contributor Distributes the Program in any form, then: + + a) the Program must also be made available as Source Code, in + accordance with section 3.2, and the Contributor must accompany + the Program with a statement that the Source Code for the Program + is available under this Agreement, and informs Recipients how to + obtain it in a reasonable manner on or through a medium customarily + used for software exchange; and + + b) the Contributor may Distribute the Program under a license + different than this Agreement, provided that such license: + i) effectively disclaims on behalf of all other Contributors all + warranties and conditions, express and implied, including + warranties or conditions of title and non-infringement, and + implied warranties or conditions of merchantability and fitness + for a particular purpose; + + ii) effectively excludes on behalf of all other Contributors all + liability for damages, including direct, indirect, special, + incidental and consequential damages, such as lost profits; + + iii) does not attempt to limit or alter the recipients' rights + in the Source Code under section 3.2; and + + iv) requires any subsequent distribution of the Program by any + party to be under a license that satisfies the requirements + of this section 3. + +3.2 When the Program is Distributed as Source Code: + + a) it must be made available under this Agreement, or if the + Program (i) is combined with other material in a separate file or + files made available under a Secondary License, and (ii) the initial + Contributor attached to the Source Code the notice described in + Exhibit A of this Agreement, then the Program may be made available + under the terms of such Secondary Licenses, and + + b) a copy of this Agreement must be included with each copy of + the Program. + +3.3 Contributors may not remove or alter any copyright, patent, +trademark, attribution notices, disclaimers of warranty, or limitations +of liability ("notices") contained within the Program from any copy of +the Program which they Distribute, provided that Contributors may add +their own appropriate notices. + +4. COMMERCIAL DISTRIBUTION + +Commercial distributors of software may accept certain responsibilities +with respect to end users, business partners and the like. While this +license is intended to facilitate the commercial use of the Program, +the Contributor who includes the Program in a commercial product +offering should do so in a manner which does not create potential +liability for other Contributors. Therefore, if a Contributor includes +the Program in a commercial product offering, such Contributor +("Commercial Contributor") hereby agrees to defend and indemnify every +other Contributor ("Indemnified Contributor") against any losses, +damages and costs (collectively "Losses") arising from claims, lawsuits +and other legal actions brought by a third party against the Indemnified +Contributor to the extent caused by the acts or omissions of such +Commercial Contributor in connection with its distribution of the Program +in a commercial product offering. The obligations in this section do not +apply to any claims or Losses relating to any actual or alleged +intellectual property infringement. In order to qualify, an Indemnified +Contributor must: a) promptly notify the Commercial Contributor in +writing of such claim, and b) allow the Commercial Contributor to control, +and cooperate with the Commercial Contributor in, the defense and any +related settlement negotiations. The Indemnified Contributor may +participate in any such claim at its own expense. + +For example, a Contributor might include the Program in a commercial +product offering, Product X. That Contributor is then a Commercial +Contributor. If that Commercial Contributor then makes performance +claims, or offers warranties related to Product X, those performance +claims and warranties are such Commercial Contributor's responsibility +alone. Under this section, the Commercial Contributor would have to +defend claims against the other Contributors related to those performance +claims and warranties, and if a court requires any other Contributor to +pay any damages as a result, the Commercial Contributor must pay +those damages. + +5. NO WARRANTY + +EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, AND TO THE EXTENT +PERMITTED BY APPLICABLE LAW, THE PROGRAM IS PROVIDED ON AN "AS IS" +BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR +IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR CONDITIONS OF +TITLE, NON-INFRINGEMENT, MERCHANTABILITY OR FITNESS FOR A PARTICULAR +PURPOSE. Each Recipient is solely responsible for determining the +appropriateness of using and distributing the Program and assumes all +risks associated with its exercise of rights under this Agreement, +including but not limited to the risks and costs of program errors, +compliance with applicable laws, damage to or loss of data, programs +or equipment, and unavailability or interruption of operations. + +6. DISCLAIMER OF LIABILITY + +EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, AND TO THE EXTENT +PERMITTED BY APPLICABLE LAW, NEITHER RECIPIENT NOR ANY CONTRIBUTORS +SHALL HAVE ANY LIABILITY FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION LOST +PROFITS), 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 OR DISTRIBUTION OF THE PROGRAM OR THE +EXERCISE OF ANY RIGHTS GRANTED HEREUNDER, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + +7. GENERAL + +If any provision of this Agreement is invalid or unenforceable under +applicable law, it shall not affect the validity or enforceability of +the remainder of the terms of this Agreement, and without further +action by the parties hereto, such provision shall be reformed to the +minimum extent necessary to make such provision valid and enforceable. + +If Recipient institutes patent litigation against any entity +(including a cross-claim or counterclaim in a lawsuit) alleging that the +Program itself (excluding combinations of the Program with other software +or hardware) infringes such Recipient's patent(s), then such Recipient's +rights granted under Section 2(b) shall terminate as of the date such +litigation is filed. + +All Recipient's rights under this Agreement shall terminate if it +fails to comply with any of the material terms or conditions of this +Agreement and does not cure such failure in a reasonable period of +time after becoming aware of such noncompliance. If all Recipient's +rights under this Agreement terminate, Recipient agrees to cease use +and distribution of the Program as soon as reasonably practicable. +However, Recipient's obligations under this Agreement and any licenses +granted by Recipient relating to the Program shall continue and survive. + +Everyone is permitted to copy and distribute copies of this Agreement, +but in order to avoid inconsistency the Agreement is copyrighted and +may only be modified in the following manner. The Agreement Steward +reserves the right to publish new versions (including revisions) of +this Agreement from time to time. No one other than the Agreement +Steward has the right to modify this Agreement. The Eclipse Foundation +is the initial Agreement Steward. The Eclipse Foundation may assign the +responsibility to serve as the Agreement Steward to a suitable separate +entity. Each new version of the Agreement will be given a distinguishing +version number. The Program (including Contributions) may always be +Distributed subject to the version of the Agreement under which it was +received. In addition, after a new version of the Agreement is published, +Contributor may elect to Distribute the Program (including its +Contributions) under the new version. + +Except as expressly stated in Sections 2(a) and 2(b) above, Recipient +receives no rights or licenses to the intellectual property of any +Contributor under this Agreement, whether expressly, by implication, +estoppel or otherwise. All rights in the Program not expressly granted +under this Agreement are reserved. Nothing in this Agreement is intended +to be enforceable by any entity that is not a Contributor or Recipient. +No third-party beneficiary rights are created under this Agreement. + +Exhibit A - Form of Secondary Licenses Notice + +"This Source Code may also be made available under the following +Secondary Licenses when the conditions for such availability set forth +in the Eclipse Public License, v. 2.0 are satisfied: {name license(s), +version(s), and exceptions or additional permissions here}." + + Simply including a copy of this Agreement, including this Exhibit A + is not sufficient to license the Source Code under Secondary Licenses. + + If it is not possible or desirable to put the notice in a particular + file, then You may include the notice in a location (such as a LICENSE + file in a relevant directory) where a recipient would be likely to + look for such a notice. + + You may add additional accurate notices of copyright ownership. diff --git a/README.md b/README.md index 7cc9575..f5d935d 100644 --- a/README.md +++ b/README.md @@ -1 +1,2 @@ -# zenoh-plugin-ros1 \ No newline at end of file +# zenoh-plugin-ros1 +Plugin to integrate ROS1 to Zenoh diff --git a/rust-toolchain b/rust-toolchain new file mode 100644 index 0000000..870bbe4 --- /dev/null +++ b/rust-toolchain @@ -0,0 +1 @@ +stable \ No newline at end of file diff --git a/zenoh-dragon.png b/zenoh-dragon.png new file mode 100644 index 0000000000000000000000000000000000000000..6507046b2eb7b721661c657255c42f7749a4fefd GIT binary patch literal 33031 zcmX`SW0WXO4=y^kXOC^$wr$(CZQHi3J+^Jzwte>d-FwcDtd&(=U6o3DrIIJ<2zgmC zSSU;=0001332|XX002Phe`E~-_HV?MBp3q#00hHANJ!pFNKDAi+RjnQ-oVI2%*58j z(ZWbkOb`HoJtjs)#~fSb4{xWF84>Y}k~MRPb2%R@32c%ob3t>5-}W}EZK?$82H~0U z%gs#s_zXK!=XZgeh`cw$Mb@98-d2Bn@@M!>-e$TKD3XmOd~)9Aw;=hmsM5;}E~X5N zj)onR8}G-Q+gz`kcC%a1v-V2$)Pz5PsF}4iXp9F=VR7L+RPn)K`DkDTY6>`1Iu+7D zta8w9En2%m173RT%iBMBHLapJDEJO|hD|5s(BW}_cV#M*u7I8&33z-Lnz%29XRQqU zLv)^P`$nsSPQ1P`7N$qk2aDmLDoU2&X$Wys!*8N6pA7XYdigSzmz8J7g@#!!+xZRl zw+26XHvQ%yk2Zs4vl)53!M6t5pHwU)r~3*+Qk98$qF)dPRyIC=vG+Dpvl0;bz~Z-d zGV4kaZ-uq1H^D1=@``I(A_;s?gIOP`4`{+?*-j^v`q!!|lZLbVU58p(G#?(gK>8v& zv@-8U9Y1Z9avx5QFIX}sy*z}!KDs=_Uwj<%IIhpV&mMzQX~4fq(vpKvhJwQV3Pc57 zP7i(>wj>dNF)T5UCH!a;i{P~A4s~ow&t|1y(Abvnq);zE0u=Lqg`1)r{p&QqsuZGQ zc^Snb?s{;IbV4EO8$x}jv$ff0?#`N4Kk*UT$|;J_IF)cnDS z)GIJW!#eHbe&B4*nC|ZxNd6fc(+hfUn`x#r4&x0(ZN&VFtwvomxvvV*HbUm}rDg-- zBgMc+S`V7lWt4bHa9{)^^oZn^c}c=A zqLkKFTCJvr`x{C$N%0A))BQD~*Pwk)&8G1803)YEZRTC?{ql?Ph3d<)c+c-h?Y`A< zRCB7cQF`#oTHVP_1=9xX5cO_8V>n_M=h5N*NDW@pH+4MYwC+iVTf>ePPP94=^uzL9 z6+<>gxcVvRDXf{go6Lv&d!&2XXToRNXU3;r2PlDSGUu>opVl7FNpIxE@)1*W*9+AH zz18eI)v8*OfXEsJ7u9iqbzi}N@qr8m!bnhIEg-e#@)I?WE|&)f&Is?M>{GjkRuwSf zvjONy=0`GkTR_k+>h_j~DlYsjhkl*Ye>jVq^j_ttrZm;bj4z%0fOFh9>*1y~#U_Mfj{ zH&K@`m5~9U{KrE80ESrrfc)bC{}JXt0ssKY2l~GwfYkZG|Btu+?_rVFTQUFuKY)a= zfU-N_l@GYT^6>ZXR99=K8v9;s>{4eDdE}ZE48BN$RXUME5nwdLXuKm-Y^w+^^!O^3AkS-Qy}pfeN(WDc1s6C$+Gray|8 zw^RWB&EqFP005%)uiu5z)YPoB2$n09zNL^(f|$8yfLv_uPhRAB2VDg7p$hycK#+$Z z2N4v;Hy`*Yr(6u}>P}o{D~TTxDR*TsFm{a_X#BFfDQEis*2Dx7Kwg<~@zF*HvAs*_ zMj&OE>ZFWG&}avBdwD<+)lvGl)E}S#!hBH7{j9r7;g|I^RpgmbB*{&0r)@8=9Mlp` zT$C;`*&C9}SnKcln`Zipz<;vlBR~KIB2P=!FQzH$ZH(-R4YS+DqIXla|!D_A#S8kA~aNHak7i{a_Q6-EG0Z04JirfeXGT}v?7mgmaR<|01^qTjr!%xLV)9eGHgqGremNP|!fHuI{ z_!3|#u690RM8eyJqR{}6#Bje?CffV=%Q(@%6tCDG4GtrrtPG<5aI+$Z2uXz;C%sLnoEZ zO56}GTurSR&wc!+D2wka@3rwm@qgx;CPYvj;QYIP7%B@)S96$*#y_!;HND(H04t>d zMshH51&C(D)p1-jO%tZ(hVpL*WwHF>0xkMI_KPb!!?xBHCW9_gaicJUwz1fp&Gj>PrhX=676TYoYoGU zvM0ht(4wS`X!x{kzvBq|l9~kth#{bWdMWirq^;qd0_849BCy^IVl*ja89aPuMz&u2 z2@hWp#ehta{N@7{ul+5S)i#+Av+7vQ0cW3#mD+tb(8p9WW-N(qc?e}xe*lrv_KNVB@GzU}sHYtb`37M(EK1QWwR zBH+O%ESu0;@8q1&^p1FO2;ty*VgS9h*WR$OqvsP;(NCc9#O8EYiiZ1jkBPKYy70_y z)h>OTyaNs^+U8P;90Ab<2-h0hC7G&nSvzbwH-0r?zRou5A)AVT*h-T|LIe)TNx^8S z{wcFQ?oBNQUp`np1DZpF;f|P}ADCmQOJd%>5Y%Z+506iOeE?tNzT5z zZf2#-xV_z8InqMeKR}R?{K1e>@z+siKK&tY$hagBzya9q*vG(QYq00p+?iUO?K=1k zNetHWTP$WVS=}FTn2UO5n4O4!ZEb>yDXm5ccUe-cxEKl?q=}SkNbg`1Ix_honhj%c zb2IhF8vP-fK%n*s+&#bKlzGD!fKnPHn)_Ax%+1$lS!=U&4*h_}5gYH?RkmBeJfVxL zv>Hk@d$bv8pOt^G6YJ@fw*)NP7xdnN)1i=8A%xY#o@KTlqb4RnisX#q(cc{L?NJXf z*5XgymSp^hrb&AwI6P*d@;=Q_ow!B^=zf@~jXSWRT(UpJlw->2w?(A~-7UtG_t^rz zeBv;$fUE36__K)+?g1R-@-~VCm=ya?3KWp&&8=lTtv&He6$({HPiwH*OLYQKB#nY;?am2i?8(Pw=$` z2(B=2IrSgzq7Hekl^`m@HP?eE?cx(8PkOv;>56V)sFB=5E<72+No!bmRGKsd7(;|r zXPK4S?<`P>U5FA8!Xx1>iScFsDz6RGL^mKho>1-$64F?-O#9{NsjGClU}*R=CmNXz7O`@=?}q7LZj0=59Jt{Q{%wc-+z{DtlSK7z}@ zM!UTM@RNEIfe8+@<^)ieVg`&wF?4Y_pCOo|^8kMAc~D{o6|Gd$dhR02i+x zoM+_;rR{lLFxDap9t)bVjv53pCXfOdHRRk2LCz1@ga)-twIBy~v4|HzOvZOqsUKN# zb0K1@{YxjXq-mW;v^guf?}n}5cM~;F%F=Ymdwpn7@@>O9j$SB-MM}0Hz9~fT2s*)^ zPP_Z3r~)M2Q1B`#d8jv152-`(Gapt*T2fm*UalMBNQS1JD@!vt33L(#iLidFO89?I8nLGP`o*5Pl#c1Y}65gBBz4u(n&Px_-~<$qTSyCA-EDMSI|X4=daz`zI7lLkTYgN(A2<2;6T zjX9Iiu~gMv92HpWts!p-gJOgU!y!G)6hx=scNXO*cT$IdxbtTQDeDJJJ}o5((w~5I zq|_nlR-p+(Lnm^k(A@!DxR7*Ss7a078>Z6g%hnr_JF)pkDlgPooo^@8R+|3IUPhQ- zPIX*9qI`L~8q5X#Ke&uPt8?{w>v}CZPK>>OynS&+n(E&p* z+G8c(y#~q&4=Y0qkpcBfF&os%={md{Qxj)^LOnzYmn7w_R_>NjVS<1m?YXmG`}c|wg`SlmhA$ZpLozqXVN_+7@Ih; zi;wnvOUTCU2)7257h@ksl^1F{rf8Hyh+AY}jp&MI8|me7inb3zqRbJ;;5}*#l(ym4 zIZ+SRTi4K8h$klU(BwGtSwWmHwc@F^uF;%}Ca5TZAirXFeu(23yD6EYk=W8Gj}yi4 zYa^}%sK_lTCe4O!K-!x3ApQ_joa2|<0?!g$cjgkfFz3n@&nujZp#e5ib@5m2>pLsR zm2VwH))PAx!fU~XEN1PBYs_ivdxq(@7wfFvSiYsnZ%_UXXnK)lp5R(iUV8s3NOi|M zh^cFt=?6iInhI2M5|>c-gIa8sV4bg#!Sfx~wpFYe5y~6A0S#`5y$W>2`2KjVhtCqm zhe$F?AcRYSvFmZ&0k&PZa6Cy(T1s^sERxRC{&sDrgLc(^W&~}TxSUmwO`yp`dQT^&GOV1E>Oqz) zFOevT{V)Yo6-DBbfE*|eB)aT67_`Fj%y8HVeb(~)(GaO%5~jkka)JRmRP8royj~kJ zu|!9wIYZ{f>#d$JQkYDnRf`P>C^w7LiUBd22SFTHf&**_K1VnK@&i5%bXt|^(&a#l zhFBQSIAi+@?4aDcpkeh0JEB*V=~{c7NfJ9z8i_;^Kt=PboxYC}+7O!g6oy|_5?P&C>`f<-ht#833B(|TUkg!XzA^zsz1<6K zXFTOnH7_{jDU!}O5|10i>kE?0f;+|Ce;djc+HD}INs?3uF%$!L`LS;0$U%~Fc62H> z{pu0P>d9PbL$NAj#PxT^=WYwQNVle3UdpyGz^m~T5iMAS-p(U5rfhojVl@lMaWCzv zCMoGr_Fl1CpVi-@E#HY2zM za?~c-l8q7`CSrY-dYCgBwH|5Tq@dlee2kJ|lVM&G2N|-;cw}OKsI(hm;G)zN6uD+n zyg>5C%X~$U(f6Xf>E+(VEk&73mqs}P88M(7c?CZAS8F!ZQ;no)omSxz1i5a~{YwqQ z3m@b6fr{pH__YQfM%V)2dGBd9j*--LlYm4N@7h#>@+Q9okTK3@o89xY68Tgp#Mqg0T=jrO<$b zT-|j5&K_RKPZ9VR0SQ*9gchEJ>RjzD+K_uO)8*aOSB0crN^?vOK1vk3K}t`@{0(pO zE85wY8VR9>hAEn&UlKknX04&7DfGY>P!xPA4GmT|srGrL^B0RG;;P*H^43HU5)$%rPbs97T82d1Me#6c@bhJYs3X@YLLMi^@y6d9LaY~T z63s}&G_i?BfDM9l_etmbVT-9BgX3lcmWq4#a?KGeAVi|-1r0k_mhx|t%FP}ux);?l zq-0o-6tKXHa-nZq(7qWiph<{ z<;*bm9nCUiJR4?-Te3FdzGZ)lseH}M))<~1WyGC+dosG(4csh|RFi(_NN6Q==R_tl zl4JzuOfZe4+!pA5I)7d^QvfZHtCuW{5yHb4w$Bp3g{t>^ReDlt?p2K^ZkSX!x|ojf zuoYjE&+8LtuleQU^B=hWf=ZYq%*2;FGF}TO9X}r03*?o+aTtshM1r8&8^Atb!nmyj zwbR>OPdlyVGFUxdX6E}TnZj3=2_pgi=B@!q$}`_?OO=o71+aMCnrk&lEJu zFEDk&p0BpkoI~>{m$6!;f((BVmzOTPV3K+`ADbC}*A7xtdD}&fR-7tc1U`zK>z3l) z`3}f5tQ~D+O-_c{0ycQ;V3&z1_M!j@{0BQw`ux<$igy2?SvJK*hwM!FgV1}*%Q+5) z_|OD4ZsmbMN*gtQk6w-oP4W?Z#x0j+FLWaU`}Ne+ba8*eNdj%cI9R7xWY)1QB|TT& z;OJb+VNym5=Cgnduo7o;-x_JD3?_irC;J&!n}U=Cljxmuq#r}CHdyGfa3~)RMHbdO zdbLHOv7M_o)ubxaKb^~h3o`H_Ou0CjVm-tia8&6sg}>ZjDb@XO+Co)BCC2{;t-wWI z+z#$oGICNZsW!~NkF|dN4H_M907J7#Glg##z}y0R|4jR&$*NXv8HU0ZPLI$x==65b zZ%cj;IbQqppy0B1HZmD@SKG6wLa&JBS04(!JneG9C1bUwv73w; zmalI>Jl4_;t1IOagCSHqakcHjetNx_eNBA-T@$PaENAgUa2BkToXTgX;y7D!PIZ~serH?BnXRn5Bxhw3N+Ei&$p8XXf8_VvsA=SfW zFa2{(1Bf!fV=*gY5I!lRx~V|X$2o2juqgzdQ}#Laacoe%B%OmGSdM&ROmuG?|0Y~z3% zxO>u;LpdoY9)XOPWv(E*PjgC$eODAzXEx%U!DaQ=?lKZog)yGfn+Tyk_!u`=C)&)5 zGS}W*`p}B}tTR%V@IlcjXp;DOsQx3)n8J$3ps#sbTlT`U4@Cr|lLY*oA+N zH}NQc_LCR!ioC__hpw*8n`Oz1s@kkoa7*xfx4xq&rorI;4VEm$IeqvsPKmSa z`93$2vVFMJ!H8HXuUE#PA;F@4dGZ$fU8QT4u{Hln5-LrC{2P7ZiP}m;jGTB`JEZ#v zbb%AbWyJB|gsHWg!_3F2U+KIPI+4Ty;_s?y@QcU1-p?{!D&QL@xIwCRkZeDqqKYLmxM*u_%FGa|~VAqncy;(+sI zYX!xR!`HH2`QsYR0A*H7DVpIOh`Qd~jzyJ{p!fol87NhyQ`tpwl}Xqes^r6v_~oQs zh3K!c@?5aI{|lbmsYpDSUyx&5A3Z8byop{q~Ed~o|?aQ)E)`htO zl#iXO%B(j1wfl$xZ?X^JKG1~y2XN;feyFn8D+tn=4Uvmx(i&>pvbUfj zTeRepAde%J*UK4RNoI9b`J|c41AzM zzK*UZO?|V&*}=6=Od+eD##^1Dov&frqT8imL4!irBn~~oeCDpl-n|i!bxIs6So42t zyL^rsgsY$P7Zf`e*WbhNEAxaDudrTCxe}XUe)N*ClH9r&UI>xYHTgq|}F zLS+7F^+rE@KM?Sy@|Cg4HH;8GdVl#EdcUCRkxc$V5reMT_ zmU2x47Bs0O1GqYjn5r6fbn-$79oL{&z)9q|*=i4wnIjf3xrj03o{k%U zjfN7-hhF1?Czn9qu_xMO8=nAvj)k>%cI~0yehF@$JW6iBFw+&HiL~V{ma z6o?RD&_%V$Pi?@>e=CuzE*Sh@G6zHtTNK*t-l!R;+!|jT0-5yFA$xL)k_;cG%-bNo zN@k#7c~z{9tq_b7MH-RB%@N?dOeYJ4?9OmRt=~IPp_ESl8(H?+iOwM28Qq60IftJ5 zXS~_heAuI1WKoRRoJE%EuiU`G3bz}A%;&mA!E(rwfWN}Z@mOwfa{WW(gr{QXThUxI zpMSMyX|=0R$D;vto&qmjuY1Sn40XR(z1%=$bM`?LioS)?2cx3c1p)rvP2!Vmm*;7U zK7{bOo43O$vY{zQ3Uv=fWIK$(!1p;8{9T7i#R&P34^>5DwmT>$2_68)w`0 z=nsu7sIiAo<%J>;fU+9b^OeB}b;k&E25mq7>kWK?W?saN%bvlSO0-phdlkom5811A z_8%?@6J!hLtWxTXMw0K7@r{2cNiZ+O89>lOgLw{mXFNho2Imy?Mv$^L@2|x}H2Ub*fVikky5w1!L$rz zMp<9|LXBP8S%A|8oBRb@>q-_4c?cqAX6UHtU^sKoMXVF*s~HupMc%+Rii)ezTjZr1 zLnd^_P+IOiV5oB!$b_2sV0ku?=|OmeDrkVL$Rg037kTnavQyOPTiEN@8eq364QO&1 zor#XUC9I<`uD1x($JQ0_@T|bmwOh4;iiYKUjT{AK<-I?P_T7@;wIRr+s;OR-$%ubk zV`g!$lo=H;THm(Ih*th*4yM4B2BgPqVjrG03<`a zRwB`ylVV3B?`FmySewGd7I{Dd3xc2J;3%j!1*4y9PWwIrwAuheaLJ`Zhy12Q`eOj? z1Jg9W5*zAogns@aC7%Ad3@bPfwgv5cg#!Knwq}#j$?ssg+n&K3@P4ROBl%M5=kX|@X!lFI|TiwfQ%ZT4yb4yb| zD|$nHI0QRCd!xA>hj$9<+{z9LhZ)3&yI9!j*!;5vYcRRDw8vWNFpy!Ph-Jq4;E?5% zUK=-T3hNMqNIxf@_C{{#dw9N1GpR zVzzOgj<}M)(8vl)%L=;XOJ~Gvp^S)lp+Z1LC4-yqcMYU7rqnKSZ2h^A;Rnkx4?#Rv zxzNopI}v$L%WXx+KT(+wc`VF{IT^J+(<*(_V{)I2*g#Mv3?vg9W@W=bP`v}ncXR1H zUq+zPKaZGGt=1S}C&q!;G8k3dh8A0h^JS%M5 z4o8*9Qp09ynauKRmt~qaGfVXF`0eB6qKdy7PYAusZoV1O>afcHUJtV?-8|?#9Lm7J zLx$I-DJ=dN{?7c&p*O}K>qz%%GoP3okO!TCE8>_ z*aFVDYSh5c(W+X4>I}}~h}wqt>_tJjocE;|GzpZwK9u2%);d?-*E#>XfCM>AMXbdn ziI%C~d97EpP}2vx436IX+PgRgC+(Ozf8&-Ii9L);rY=X7Ra73s`ry1{^{9X+{i5OPz~b%%6KesgLa4UQDEe^=_?8cp zjH@-Xv|WJK!=l+E5^QY+As#7>ev7UR_ApH97X3cX{3z?oL!FAWV$&dai7)dYnaj|z z^Q-Wj&j?c|oYygzu(I@wc%(h8c_W|-cbYC!Q9A4AC#H;+77DzcIi1s9<|j0P;MvyF}WT9s8lE@}9t z1gAHnZReVC?H!tv-;rs3#PiI))D}~TW5voR2~*V=yptx-AaYN(m%!sq4r^kFc$}2L zOv!nDas$BXUE0m&7v&x)izBPdsX{(suz-~ug4jp{Z_H@5Qr3apKyi~L_<}6Z=zmWl zbOcj#P)Rf#Nd#*<51kpE&HGLi`crwUF>0#H<2#DrkIlB2?4I>)M>$FjU`AfnzuWPB z`UGt3;XNjFLMo=TeC=tu8ecn_2N9LU)<*2@v-xEa2DifCe)Y_`cmB}!xW4BQ^$K#k z2txqs%WOaQOR2A+)zeT==7q8?ZJ({-q$r~lghr|~I*O((u&&v?CUpHZ!{7-^Y`>9+-hJfh$Htm+*#MI`_Aqe}ra^bY#MiJsEoO!t!G0_wLn<)b?2{cRBW`)3g z`vrva;UzAvWc+O7o^+BXqDc)6T)cZ@>!ylHT*H^>k~)XfKq(qspPAZL7&uXZU-p`` zF~~Lvudn2M@QfYgf!G$j{9j&F*q&P;-8+TJNxoU>ZnJln41En9^+x@a8ur*&Wjp7UYCRMx7cwmNKbvr-{ zN&0dBBdt}$Nlgl)eLiDt>7YNvrATPJM?!N|boU$HJv;jWlM{~`Ye+Q!1s>^J%wW5- z+bPiE6vAvT84R2ZTEYe2=EuIh_DxMtDu7&og47j_Y9XN|c@3zgIx_2}?W*Qk&H~jY z6q;!;F@koYlJ0gHcPtqJ1nBAv?VUxprWa7Z_alb`1sDM!4o%0`IoIs9$jNbgtAL4V zK`{_1*qZtClm=&A>q)dCw}B+&XA%H}7ZYF?7hGdJFNC5eQlLd5i;61&U8UaqYi4K9 z^9SmGuQlDS^kT+>CJZ)gkwdkIP(f33papb_n`W{Q!>AD`bN)-2lAFmJghD^ZwAMqtGh0x6efA4;I?rgp<83|QytbgcsAL>rL}9XZ zE#`dJO+xeDng34GjU&(2fA~!P`aeDWX#g0+irxiyehv@T0)QC?CJLb1wg{4vWjGLz zUk?G;TtwD}yLx9JH-i(9IQ+Xs$kHJWWEnZ839w8^Qwb?~D_izKq$9eWy)JmRXe%=0 z{nM}nW13LP$}`2!Vr z`Nx9LZl=~X91X(XKd13($#!b zP&u*g%OL{MCL~%4HlB6?f7-n*v)CdwPwoLJqPkP+?>)4)O1Ae?(7wn3n@P`9AmG9+ z!F%WB1xJ3SYKT7CN|W&%mAi~%hd8AZd_QBK6zPCpa~@xrM_k&De*FuHeu?j#4m*Z0 z1KTA_rKEw`Bo58{P0`&{ll0XgcQ3c`xrjba-TJBY8y?(#LuY5aDZ7?uD*Rh;I(^~O zDN5XWM+OBcXnUm|RhKxdBw7te9QFg6#053|LjacSgDh!=CQj0AKq8A@a_Ih&%cwD( z#Fa+PZBs_dL6my#Y=(t7X)tsZlsku~>qL;ypsU}V8^7QQP{oSRoJGud$shQjCpq<3 zk_Uh55Af)!jOk9V)A&}7uDg#f#lQok)E%0X=}%{nKG_d^QK*DC=r=>dnB>ZqCjw~Q z;350Xa|MwaSl~$hf|gVE%~M=_=;*LCegZN>b%}eJOD}#D#c}VO1J?|Lfl#_LqAgD1U}EwZv4+uWjR_}m5-VM< zOB)AF3S1S3abTM+_iYXLfE;%uoGVWu8L@^(B~2cTq)dey{GGl_4oJg$Nh}2YF+1#y z2LS_r2y5B*-r&#vKeUHV)I!T!2}k`|m}X#Xmezaa&Duv!iSvI>&j z@h`$)x6qshPwokR6Wd3HxNTza?P4U9>xf@fBTlv}cz{b8=5-J+D}bW9ANpht;K@ZW z*d@e*vpRh9YlUpH?^#MCueo`7ukGx_Y3v)2kc4`VJmj8ce{_n^dbXQywAt6_+Hx5a z8_`V|pZ@|Vp7P7PKIf=|7LN0*{bF8Vm`R7Ah8V)CUAC=*>rYD(-cE<%&)@ihK2=XW zWWq3sho%CCZs@)BrVg~p(4}Q$r4BQF^7SzK*kiqBzJcrjIdE^syN5e>fRvcX)QDAs zlH^ADFs5sZOzKWTYJY^khZ|+9FZO36ulS{M!KnuI$&!g7)ko&2OqzGtk|lUQ{%|+0 z2G|&?XySm!c#nPVXZZJLeK)2z^{`XJCk_Alk<^hR9BcIWc4PXF1d3_+b_`5FFPWUn zw&(xK#8Q~k6$+4zO!uJcz6_H7eV?m%;a_{%t9gN`{Ue)7B^wUYw%)#(>eHxQmJRZV z#6uFX^g+=|ha6h-NA5|+{oj?qkZO=8{1il-IRUN*DY_M{g%t*})W=PflgK&lPT{TI z33bphR6vr1?@9lqZxv<7@ zn_ZxE&tsdYMQ_72L`eCCAi)U2T{9Edp%cBF=h~eDt#QMAQGBB+STbV6Av|O=6l0raZk3_rN{&B9W|yAXD+tj-;nZQfHhaP0`tHZEn|hlw zTLbB6>wfr+Eogga0?zaEijoCSm7WwJyTF%TP+2T?#0#w-7z;Z$M2prv(iD{NrYbnm zYxEe8HVq5hc?x7~v53z4vs*H!&@S!3)7J?)i|(w}$&7@1XAWhH?x36PL6mbUa{bQ>F&y)(>RJDO2 zJL0+I#*R6*@6NuBd-s)p8pVlZL{&a;&ER8hI=)*_Y3dU;bm7puJz53l3d=Kj6J_WBoQbVoKeWRJG8*N_rd` z95P#dUWnDThVKl7>aPQmg)CzWQ$So9sLfFE7uwwpVM0g3^ZH4QuyC|4kghjhVc!$I`a*>zckm)$?=iIjiPXJMx;Cj zG5vG1{a2s^_{jS|hr|jcfgXbNI>Ix1`U|CCN${UR5pfs;aPpLc&N8szz=QDNgGbm% zzxWgHO&%g)GljM1O!^;l)yr=B=VW4M#U#~<98_G;r6@|Wr1=%J3(PucNGXJss+_de z9N_}7=+a~BS^n^II%eTedsHIofI(ppsV>|l9OfZm71P*Dz4>1Wbg8pj*wUKg=w#;* zR)4}NlcER(njw>w#FaA%D*-%fNemPo5t>Xuhzt~x8f$kzu>p@N16f7-gkF@1I(&go zfuPIFzP&&HHIJ~sCRLtF2&t3A)m=?7Jqu2?OZSNaq>)EZzR|`QY^#oLdR+p^6wC%g zfmiV^Db;4Hij_mShlNMyjI!&*tEs=J%Lt8V*B$oCBg?J5LCUGG^8UWc!lhLq+%S<2 zyj>Ym&ox&R=#hUTDj@}0S65@>-B}Bt^lW3KpI-!s(If+vx5*&XS07SbN1O2UT4gEt zxw_sbhq1LDymm=E6?4g`4_xjGvH(S;pJ_JKqpzJ*Yf{$@O@?;(+e z2?azI_m8sC;W`b_&iw2CVshyf_$r77U}^dWmF!eR|1GG4z+x8+Q$)QhE#~W1+sT5|>6bXIK!J(-S=C0_>>@GYA*J%;Zd@SO8+ap-Qr zE-b{A)B~gFw5*6!KXv{dfjSrE$^B-*TuI5b>~#gSKY99&jImKSoLgb#7z6rq(7dsu zbIL)$Je*}8Fs#INtI4DFs0y6hQWHeB5SJb6|03~_A^=iuE6Y;$V`g-!2xLsQ~?lu&_<&{cS)xj22xs^e@5K zMRwt(HFVpJhli-LZ^z4c**e543v|Z~hh+Y%d4IbEh*+AiC3Ce|(SW4IJvBqA;>7cvnZr*gor4lZ$A{y`tG?{RI_5^7tt^WPBt&~qKqDb#J9Yncw1hkXH6 zvPv?ymggz;9SaRYpCMSF*QM{M*j5wE{;wME^J;1C^e?End#$fTY6Y^bQP@fVOep)o6Liw^M*)n*i_8Kz1|6U7mMP zCPo_&RKkx1J9_pdgin-HI? zX`dCOOiFEwf%LAG&J-Nd`?M&hU`i&^j6fEPa?G&6r$*@R^e^`9TAPb^e8jL^bG@Yc zzaJE&gL|M7QLKu_D19;4)g-#Cz}cXY5?aJd@_WI z!ompJQyV#-bFl#=$DzEWI2*(+p<$zWC0A64W1^Dk5mo!QmPPDSf4SmSxI|RTT#Md5 zY4XSf-VRJ!V_+<=@zFM(?WJO1$>QaksywQ*WGHVvBIUX*LGQLW-?JyJa!+@>iX5Jcz8BsB?MNLxQSY5dsDoAhb)iKWfPR3K zj&V`>soXENO{WYs{TXEkz&T7j8EAI)Ih!l^z$Jj0`sxWBMXf=Op?pMBO`i}~!<{C9 zkGmj9sER;^N5u}r%DdABECGBzmLqZh<~Y=_z+ZF3OlIyu}D1GSo5kS->y*aT+Syzo5I62pdT}n-9S!}_6 z+9$XoD@gKa@D^idEiEgZ0fvy``(l#1^k*rUg=nC^wKIkHiv((I4pFH!E1F*Ev{{tz z=Ew7}W4-_XRQFawVeGBW4;KAC(S3mUwcc_sj~MHnA-I72nP_`;Yx09o;{Sz;7p0qoBrEgS!U zoTajw>Wnm&VQ`9&il&%o5JF0A@)|X4vOjmtJ*a)eC!vN4KfwN}bUn)cf$D!$N7sZz z%#A|P{?EKiD#r&3BU4wt{maQ%8!yGX0w8b|QVZ|DRMYyoU}b>uZQV5oK*(x@I;-V| zXLDbM-(|Cm?-^#P%3usbgC$hec#4BJFuOPJzs%59GX^Ldsmg@5@gRaS$mcy*kr?2Z)M!mUp+;KxlbSe-R{~nw zUQ*_Qbl9<Bqjm#$;t@fz4F_bwk(qtJWan%%c`W1 zD1h`D7Ul~!p%uBs@4`d6v~I^v_WwUP7l?Or0vTGAcD_i)&NQWlTtt%&y;_|SY%vU)GIOZL z5l%ho;<1!Oz_Y9dWW8EWs3`Q7v}7`B%d8V6`1}Q2L!&sq)V5V=0W|~-#Cs0;09Vls zHteGcb8zIGGYvD;+_Me?o4?|L$HCc8#|*iY1{{d@#2UZr_3}%VKYRNHnyp?cz{r3A zok{BTm>GV$){j%$NChKRu}}ePz_X|ZT^bu~#n!(8s{6%R<)=Le&}y81Sa;V|X-6e8 zRrpYW5kP~~w;Ix}!T3ZL=DccOMR^3L9D*c1K&1h0?t@h)im&bblUiPcPdp`9h=}XI z)|chM=Dp<~N-^{W$|Nb$766H*76q2?B)g7|-^E)#d9e{S#4cQ*8cej3GY6oe{=T9( z?pa*mk{J;BOsk9bZ_wXT6$S$=ka(KpTc`b64s_lnE&Q@)#fOJq)(9D@$xaWmX75ns zQ}TQpF|MTu9#bYpmW0qEqxnO{jnt%mL~%luD2trau+xV{k1;n0_?W!4Bu$=F^)OH^ z8^8ghMNJ(&Ze`#;uX@)V)U0@+h%;AK`P}V#C8d9E`TH$-Oau$Wgn_fWrtfC}r#4Ud zq{`9Oezgc++e1npCGh)FpF`1<==e%CjGX|)4JEjMJl7EIB z<}0};LIj$T)5NZL8A>8n->dk93Xl*@SB5|; zpYSB4)^$hqt2M9*v92!{OLre--#Uix)kKs_zy?MTZD-c(Uv3E9r{*Njqv!(|5SWAL zmdtD_{EiaK`rU@qPgE??!G4MM{4_++ZAL7gPEvexZ#tWCE zusowVi7eL0nlU=DD$=*F+UKy<=`wbFFbScYh5;rSAMj68sC|@~5x?Q5?*&?35W$w( zp&@qIr;FsvaT;3QNO{Ef42ZXt`%ni?Knx{$Q@M;HKui}8OT@R=PCv;HNcCe36;&2t z16u@oNwNx2k z)d&@oZC+iBK7@~yMlmviiUO%$ban`Ne?z5?UBB#;m)t=1_*ht9qV9K@B2+;6zh>_P zH^$Ss^VIz_#R{UAI$pW~=J1^GI=1OU@u{-Fq7f=5hl*r0Y|z}01MGDOA2B{lwFdX`vo7m25pPbOI^|E-WXX zrQt3WSGO2Xvgnu!ilQ+b2jHfze_R83tA&qEUT9*~A0-@EEu?qFE0c(FyZwamOtG8WcaQ zxwM~ZWGsS!qy79df6^DubZXLe*Z}bP{4Q?1QpX!uUpxVt>c}+W&EX~aW<{a84j|z%C@p)m`nZtQfI^EqVnE!# zh)dQeRfutz){Hj%#4Q|wXTz;|!?mytT2yzDWva}vi%h3(xWyKw<^wBnjOMX(8z7L~ z%*5A9p(+3t!92~rM8j{pY#Sb&gTFnmf)w}`M3X~q0gjwyq`e>8W>S;ZX2qtxv{91i z4g)J>MX!G9yD14R+9dg)kuvgXPbmlJD|euboP?8b53UFYXm{N(%pY+6}~X$hOy^(6_rI-kUIo~=zZAI*N{vJdDjLApPE2u{wyi9~YBKhsY*Xa>p! zgNKpdD2#-y{wxGVlBNsuiE!}RDC{v9!^1U9qQxw5;jLRD0FG2N6`b8(NkX9PlFwc8rvI%J=q}x}c1din*N3Fyy#N?cmot(sjt;l+p%cQbQh-kft z!ZqArOgs>`++Z_T3}JPu({w71Llv{Gj?3sB#1|B5 z`zrfF`a#dVJtZs}V1KZLbAq>Wp7b`bIK|AHwMoSDR*l{4EyT#fy44<*uTy>7zqFhY z?(w6q7iHA@`wk6MmcElxMJ($#@3n=^NdrGfx7+69o%#a`$xj);wcyGSlHm3Mm*>LW zYvxY1NYQ;yjCLx5T-*cXttd#Ub%oFVvLlp>|wTc(9DGnHX|Z{io>TQ zbLaDidN=poJ{#t;&!mSDUL?NvhPZ1`P;kYLV!ehh>8d#EV)%;c*CC%p^%R$i=aQ+_ zB6G$CR&mEWY31*$EP8Bx(B{tUgUZD3P94Jo=!KGsZJs?;a0(Dj!)B_oAc$0n+IVB? zbVIPY5vCpkppnkP+y*X6IqTjZ!En@oju8R+A*`O8JZpkE?Fw@??B#}r_)P^=O4I;} zGI&86rG72Bm;O5K`VpBii`fWF=B)j^t4Sj*Pv>X5Fr&b0zJ##l&oE>74DN8yJjHB?7jUr~oXadHm0cBsOn#EBCG04%@8GNjDFtnKpYodf4; z3SkJmAvef@hDMD&ats@8EoiGluEPX&YPqGn>7GJ3Q&5|SpeSl5N!7)8Cl_Brtlp6} z_^$uqYh+QL#ngWc){B+Fi`8N4;L=U#Ee4Im1uVD`FUl?dw*8rPQP{U^#xbBC2esyS zFj7mIQ<4iUqQT^~Q`sCY*yoka$g7Q$n1fi_R)BmR8W7%b-J&`B`$#)ZY17p_S2DisWtLYrE|pr9cZwnp7_r@=K}|9_7bCxeyjfIres57m~ZFVCXm{;F^@~z2Wjgiz>{Y?eoLo&@F41BQt&S#Z|Yh$$x?NPm!*s4&gT>IpP}B`8IEQt*AZIC6$`{RL19E%kYkFyGY^R)|O8XzzsUbET+$ z(ff2ofTVAB;~XNRQAc(C;m5D=%t}>BtsMSrsiTS-Di95j$WDg`0ZZzaivJy2v|K%y zUEO1qo?VzJV4JJh$9`gWCvb3YjWno4h>6)pmEOz~eAB>h6|(`g+qB#+nl~@|4KyO+ zL#dH~Zy<6tN*rKF4iUHcNjILI-V`*?X57-MsBpoYM%-kkq#&sRp9R%!enOBK03`2|F;!&1iNWOk`-ouC#yu2l+kGL=PWyr0u|vORliOTF)e! z{z-Q1sYGiNBA%fndQ`3-H4}ODe@LD79c+id8===J*pRY|+^kO=KgVL86OJds~ zGnX~U!lr|%CikcOKN6FKE`;1_Sl~dRFe8T~i-3v~n-iqr{>fmHxX%nO7YZrOZ^!?t z*sj^11jehVmA*o)m4D~8sgv5Ozk{(54D4t4FAbE@ScK=dxo;^%wO!e%!khS|2A}Ue z;zvlk3}6q(p?n+_g)}W&O7p+VkwGw=CQhLhR%y?jA0Zb^(xf*Y7NnLVE5cMT?kUDs zPfSdhm?qR+_BTBN7vPOZnZDT&dB_$bAd8R#ELrvoDoJ=tJlkvHe&5N&S^@I!%qDrz zN7U&nvW+5QV=68Qo6(;x46w4efTBGT5nNAS|C#v;thYYW0^vSoW*r{ad%WWDXHXk9 z%%DnKgI5PDeS>Oo!-pzfGAfI=CG7d% z+p`2J1dXr~%?#t|5=6FH~JtpG?}l-X5EpXiIwUwSs#2A2vDDBBcmP17y6}6 zs`4MP107%xrqEXP0>ER%qHX)2vMF_ZMQILMXeb@L+inu8p^liYhSb#soK&zcuw=T} z5u0P62eOJvXH!Fz56gYtSvaRY zeuC{{+)|17-{ztQ(D*|#=VBb$ea)m82H?a_+X|V^88Nkci)2Ss*!_Y zCAI(Y#{LI2`w#l|A9(KnL%MyR^nX7@c7#agRT>~x!=ZpEh!(hVOln_2urZNYMsBeh z3j}%sG5u_Y+>pLjYi*`dk5hkiv{LrL(&tdIh6Xi?8vr z%{q1dWvJaX#%fa3Fqj+3nv6CF#_0+M|63{9+TTu6X8aFyra`0tTgMbb=Z@T$)5FxD zlaOTgx8$L+Q1ll?u=tiu!mBgw zK;f3dqfGY>57St-*1Y#-B{{NkB8%?|f2)49#QYye*`np30v)e>u!!#WhKdR1Nv_Og zJpH76Ub6|N>3L?-YTSpIpS+p4ST;T~P)@l&EJ9sFRsDUx9ALq82*dI>4SGK|j?h8| z8(e4vx&jR-Jf*B0Ka^3_blBsmvV?k&ASk#AUnw*1KMubPB{j{TT+Cc=_~p)psrsUP z@k&v@(SHI02F^V7&OI;^&}ktJ&I+0TKEA-@=%rB54MkxrQeuUOPyxxQeNll5-g%bN zZD8gn#Rf2qA6^2XlihA!NQpzp!mpRsE|FenQR2Re1OA$G8-`Pp79E zJ29h0ZLtk70E2F|%-rW;(-gtuBgJLec{^MO;msSyv8LW}&&tn8*7j+VG74h_MRC!W zT_;Q$h1{2pi~Y!OPFEu`E1knO7Sl2dsTn6-iceToBE0$r9Qzx9qc3frBd-s48?ig2 z9~Wsm7J7=B@=^_i!AWoo)T>D6Sx{>6E-OJ2TZzu{J>>Ua6z2XIbhoGkiF<-%=nl#2 zL8G03`fLQ_v}nbs-kuY95({-9Pqst0!b~&DNh#<%I~iB}@TF65hQl@J!6QtW6`z1x zKQSE>?GFC=*-dj_U%9I32It>-X{ciVM4&I@J_Fhkn@UMtDC?XRECO259tJ<1?m^1C zu$I2WIbISRyLO_Z+s?l~%Rj9mQCMGxA+cndvXAZlF_&QnIWCD#zmBxIfeX6!GEXCt z-!5X%%0{3qDX7;G7L4oloW^I7PmY@b+(To-wDFK$RnEW26;+BYeov;n|>O>G)8*`$u5ijXxePn?!HvO z54LEWAyMJwhqZZGFZNL$yK@DHIog0Bu>z^DhSfmejaqqQao3=b8JO;R{^% zE7oP|t7~VHQ#_bn^VwL68Fc6Y#iKn26irtW<}A()ro77H?XSgJx1-=@P2{V;jppNf zq(dqPY6z>x{N!)f6I=f=1aibhqeoSI|s z7k?x=eE!m8g4cYxDm%Bzdc%ZvE%$H6lH1XV;p;+YU^@OD0+Th_Y&BgmHaw&FpMg2v zECyXAGr(0#UHg%~ZnRPi0g6;pjRlg=!iPOkGk#u=CG@TlIGz*gg4%8@uv18r#2FPb z?JrW+0>*t;ti1r=RHfFQS5^1rtQYRp9pC&8hfPMm(Pj2oyS$K`@CnI(F=A;;Idh+> z&=oy>b>VU1sW)5K6G%{3bz4MMG46?AGUb|}h`G3CV}DWhF6^5(`T|Sn)`zQ=f7D#z zg-u`Zrf65)*ioG&-@4eKyHf#V^}}=Z^UM5mdR17b#H6JWcWBE%Wc5vkB@8Up+$FB4 z*~i)tnI0wr$(sD_eD`aT&=w$m_#s`KJC1waEhz4T>`jU&m|S6kydkg@)-(O{1b z-uVC*R$5oU=~n$+#>Lx(f*8|Bj?Y?Owt&JcpJXn1$~bdgq&Pg4s7@$ChwV-p<2fzTS|t4@D&N7T;@|;tO+coO zY9b)52=@n*J}+n8w`I9~{QIzfNR=-r$m<4)TAUD6ml4nPL%r68za5BML)n$qRF|A3 z5b9tv?-6Z!4F30aeq8*Zs5t#ZpT5qh-QbOE-?SB2Iwhk2>oo+xQr@h~7y~7Vgc%ce zI{md9*0eG8s{b(c1+hWjFR78P;ta@qW_7?uWrC{H#Tgf%?~*A`s(aJ#css@-v_rtq zC=0muby#~zAZZKLao8#ozbZfXQ3&GBnqm%`WN{z(xNtKk{G@%or9n36B6rj(M74?f z9O!SCKZU{`7vf%@w0Q073RQ-FLt{>6_!ySs+~a7R1hI(UCBC#vA)a*=eO2R2c7^if z720^@^o=UHhbbcX9JtW-I0C6!$q`?MzBXuV9t)U4o#`*HAQM@{V_GM?f3zhSmdRQd zXFd!shAXS+FH}qi;GK*#Fc`H2SnYr4gV06owYJmK)cZ}*o2FsQM*e@7I zoM=VlW3(9_x%(*6#C-jLPUXvmjqW>vt)g+5yOy(prTPn ztuwmkDDbgEBEEoSq1JucimGIUCHPlsOT8JXYc>0C6XIf^^CL&ZOW!k5g?$taIdTWS z|2CV>lP-g29g9Tl>yCr@h~~H~$^wMsEJ{$E>;7eyx+;aT=JavO!(T6w{B-!`{xhF%VcYuOv|na-Az5tuXKkyZvZ}SOl;lBxtTxpJc;3`NE2Pg4KBQd{lIL8I>U1^W-Xm2DCd0C{V*K5 z9(ONc9}Dr0)z|&c(0E-S{V>C|xRMvlOU<>R$27~;(b~~wTWE4@vvclWls6MH^y$pP zUdd``gqS|C<%BT<(NtH_;W!uFS|6Vl(n*wOhtW}cu}r<4+Ira9HrX@S%zgU>?W&4_ z4+W$vhUcME=6h;kS?cs>?#+zGXs0I=gGZ3DAklRZT$@y>~+BKdA;-O-<~JE!&OmweECE?h991Dr^i4nYa6Ck?kc)7H#U6Ya?u{3w+zX?NIkeO~DD}%f zk@R{;!dbAY5Th%IuE1m$;oP8G{tF#GXXlCskpR9yv{lknoltwEvZ~viLFhtvu!0ZF zc~@{hwcd6?+GFV|&hwD-DM7tb`tdpf5zcd%TY)tqO(vKo$LMC-9PjB|6R-H3bp%H{ zF_p?@#NH~INAeZ55JI%#XZ#YpnV(3GVy7&0br3UL!3yC5Fe&jJ2G!wSP}=GP;G8}D z?dbsF@h6hnh!)oKE)71yGtg1v>-k$$!`GKmd3p(5&Qeu2g&vc54y3&$z0wK-*s9Gm zJN~H)c9L%WMm4(s9LjahhN9qId^djkMG*wPVMc+D?Gta)jN8TJEpl{k>;#4Wt#b3{ zHh*hga`E}FUco&`0Ce=!zGr{?H?r;)l5awZijnXi%fa!esdZ)V2w7gMvcg!71b9HU zhJ~MfDLUIBxUI=ypu3d*$?mYjqdEP%Xc z=cYnL(WswAkOrk!2Nvj)(6u?9MInagpYn_bq*7?$!d}&S-zrGd5%4_mB38ZA)j@8#f|rdf~(5`eLPd?W?wPfgR^| z5S(=Phwe<(%JT0&DwRpa&e^#y=-2Ulb|aE;ED8^ z$9McBxo}o`R4o9a4oQ0v08^Sb$l#K_=gZ7y=DXUQMf;|ul7cMj48bdssFbojyEPYF z2-->ZD|^*D_Zm9toH(7BFD27Mjne0-!Ye8%s1u>aR#uH1OdXs8A%<{%3zUI^n>WLwafbH4TqZ}JcK z90FT8Rcjjz5)0z;>`#kMr`6@kC34wZuEBsYJjQ^0_@VRtB+ClT8AofyCH2(>_8s{H zq0k#V2JhWYzd)%~WFs`QPwxJdLHa?sR@Y7I{({*TD(x$unUme!EKIV~&T6}M(@ko7 zIPQi9pFY;iavX2tV}zv7g;9jl0q#;)aD*x<}&WDlu{O6&eD0XB3hAijJq6 z$CXw6J1_UJxnNujgJfhIROf|L@aoC4AhMK|wAAFJ2PJUB^3}rzG=hBn?~+<%SK3Q+ zH#I70J(MWvy%G*vR9rL|Kuap@zSR*LycO z!E2~p`PzD3gvJCF#L!1d&mm_@+!-t$0z88L%JP9^lZJS(1eZWlKPW}>Dk?K#0spdC z>Qzk$9;IQO)l==FHRvF>Ums0njG+!zM)`d-^ECg=-32^tUHcKa$WY{_8^!>Ix?Sx&8s@rEh_qF<3p+q~Twcs(_g(7Z8(r7TR zT}wL~tMG^>Crc}`l9=kP|W6sD){S`tVA0*gW3n zaiHCyzasLr5cyQ%J|DSV-ruHw{SfG&51P94Iu@=DX!Rqe!czu&%Rx0(B|d@ew2ie% z^cTnh-sJ~8KnB2XMDd{~u#IRRS6fp_W7NNH8dH0y;2L^Z>Jf?4{xqck~b9OV7^rTPs_*LM) z{FD7e`=l#q{%V$^hXMDC5WV%FUIRi2hLt4KJ(y83!k*B*rlFaL zvZ2`AvCdymRXy+;{eyELt0;u!cQU87Sw{A;m9u2Tuz*A3V`20s93RTS;l!w+2ALuP z0lRAl1`KU->I3vKXY}=7x!h*PLKgB86$7=kb0xZ1E+<8*n>W7?#6$@w@O_m6LNw?$ zQc%guQQE(JquIWl;zB|hLA|oxAHxH(_k?|r?>&##CS^i$*cq4<`+pP z0}}dNYST$VD z8{vE8ciHHeEds)hFGZFxJuy(=4~16fy^oQMW>2mWAhord6gIa!o{RKKn-&G9IApD+ z2B*|#)eiRa#nK;rIl}3%x{{gbk{1+-SXsyd6B8Yotpr3? zMZ&!hK156FEehhv#8r0rUZff=G&9?!%@t|aV1ZnfbV7jEMseAcT1FLpK&srrYYmDB@MM?QZEB*PtR7ERMfp`6Cs;ClE< zRE20FK{U<7{x}O;uMgof(D!R7ysj3jKN#EwSRW+z64&c`LU_Jd zI}Bw|#EXEYG%Vy;6r=%p`Wq4;PhgG6E}P?bswedlm26;K(VS;Fes(|zq#x|COCRQ3 z)1e`e(GEYH!2mJ2EZnaDi8P|_3zAZqbti3`)`?4`qZ8SNv9=stOQsk4N7=|{Bavso zNSYt?wPi&{E~GtBK2|tKjG1|;K0wp;oZ&%#Whns3kXwn1W8(Q`FSIXgoLc$ZR|v>! zMRhAc(fZ_geW91xKmr?-j#Q}%B{>owyxvZ{;`$G5XYEf}gLOI2Wu~i^vqr?WU!$B) z&yI4@~l1niAaart6V~?--hx5+8%E>C!EDLek1K~DGTUd1%-BE1PxtfTt zEilGUz{&tiHfLe!3ThI=KPXOuBGLX#f2)3fo*pyc8aYVJ#RwN3n3`^>g=yE2gdgyD z3x|^p-WRS%6(u|&k|z7q@+8R_aSgg{w#<@z?Z0{nOy~(5Db+=fn;aw~;2U>!#2HKg zMYfpW;Sds?{=nlf+Y-giy@N$%^7*+Uc5#E4;gGiq(1oGb!cJI0#b3ib&?i^jW}90J z;c&^o_5sT?txUvVg(6Oci82wQe@1J}NKx2s5#2?pfM*{-;+4Tmd27D0Fy#5e;=yPe z!MA;6igar~94CI=#G$p(bFe<45CfG*cy<4{r4{;+&bHv1#{ z=QKXO%HKIAh;s|@?XADOD1)o66yqme!b4PinTE6k&va(GtCEZ3m>1DC{dLX#*|9K` zjFqn)LuWJ3&B2Z)h>TVM)@7pRK+2iC82@8VTsOjjvIPof0`~)3nt|a(wbZFH&36Xd z7jv3~0)n`&QJhS-99Sf5AG?#*yEc#A!8tpxpSY2q)3%nHg1WKTSdu#BapgH~=Qu}I z5n_5l>5qSI%k;Gg<+r}y>l^&rm@$$j@X`}k%RT^l2`7U?1FL@kb}m_3@-=gZb^9!J zF5@8JdONswac0sa4oq*JUk~l7T5fUH7kNzK)Ajp;vLoj7Yn)!WhfP8FU*63Ut@n%H zj~m4OB<8va7{xuTRGtDdNQ6cT1~T(t=_mX{E?XenrCtoh57)abU`i^GsXj=8yXj$D zTZ!v0Ue9&)B|9-2yAGO;YE27eLK>8v(n?uI$y}@u1POIsT1?mDkLnflFE_{Cgpv3k zZY;ArP$06k1FfwMKncbk( zvJ%>GK5C>}<~8~b!-^=sb+j7|q{R#SKg2T-mK9;Lk9ZLrmW@7?WvMbuK+Q7@#GstP z>PJGvnO_)Z!eg@C^dz+QuO`y2d>WV`_NESy&eCCX_P;z0BE`f5yWioUB;S8k z+Z(AI1Hx;pC8wtKdV`LIheNq_N1vdaUv@4PxmU6GY5z|3@=^YIPs&|+J%DQWL5^Xw z4ZSpqu){f4AUpA)Bw7x}ip_CZDn2^0FX2hu^ktR|dfIaNy1NtZ+y|Kq1a6RGT^Q$m!Vbe|9jTsh~y8TKI z5rtb|CS9)cEK10CWf&mO-Oor7lMndkUAg7IiGQ+&*WQbC$}toCw3JUmas^$FTQW|w z7gAo^zmmBFPG}d$&pp?j#8er0b=8I}w!LK4?21>uypwQC^vMISzvO(n){0;kJU^vt|MqzaDc*KcqW)OVAFRdd)w^8B&cyZZ9M_nw!w~Oq_p7eMS`ln>V6)fNM0VAH^dL$kR-`iX;pmooYa}Zs^t>1WJ4C$Q3 zjVRX;`giAN8$GEE60ZE&up~~Z?M;OucMIvwPu*W)*gL92gYD}uzJ}AZbKLX4i-Lc+ zdngM;WaASOI0;dsw&vUTZbBdPLjjU=O z$9N|~8Uv?ASxG4W8R=3Zk*wBZgWh!}8SF6xQA&sjGwKgA9D4MsWQ{yW!dhY)-^Uq9 z4aW0Iz*F2cMf372LhH@Jq8KOFuI8D9zBG;bP!BwSUNk6gOEr=z0sc>Ddmkoebrxf6 zijZg$6U}*@4xBR7G2I6fjg{pm#&caeV$67-ojxpC5lZNIgmpc5C~x?OavaX1imdAB z*$6H9u4SvL$K8BMuT2%y3}@|c#oSc2N`qSyBy%=A#Io2i{-CM#kd90t&{n*I4p`Bz z`h|lc7*(KGlN2yknGs_7I&YEN_M0=EXPuTrVS1e4P*X7`- z0W=*y>yBl&e(op-{JtPLUbj19(+3dF@t^psDz15U$5|Og+Z``oA0&a|gqYne;$)vA zO$lwh1vLd8&#CXe=}B-Ov@9O0O}orO5ID^2LpO4P9lQ7-wXc(N)tsKOKIQy z0Hc$!#o@Z!+TG7w!Dg9`>j7*{4$bW%Sv^TITk^jRYMYm<1Se}|D&8|z!P(_n=b~MVrAC73x@zf+vy!u-qlgd-*ppM2 z^jI#7gk|hbspVgwzyTX?*>(@PfDd@3Y46i#>9tIo_a4*$%|CQ4FLWM!lTI}TE;p7I z%DD^+eClz8iVo&LQ8ZO&T*?s}tW+s=R6D@jwPk~nuHx$k z8zQ(u3<4k*Qd=+sUw>)m4a^>Dh>&);nhfrhV$^K&PbgKYG;>YMi@VM#ysx78VuFNd z8X$YuFH}*^iE;-Csq7VxNnQ)xYK@&}k*21k%ybpGt(9gw;ERNEA6H@*2`%4b((C>V zPZ8MN9<2e{MZCz#Zi!KTOGeaTc1vbn8|UJr^nEzEOVzG=6ZVkN$bTld9XH9-C8n8s z6r)|6YjhkMb0&>NJ}&U85L#{Qg9=Gl;q)PJMI!OX=UJ*KT6IiW-=f2n0a{T-9T~kD zLcP8;Dfcy$ZWvFT`g(|e7`7C+lo5d0);d~ZEHy6(-8{Pk)Qf;N`~0!g1V8$cx? zY_Dd&1WDp`7R;TK{7U9%)|-Md870nCh9wwjW2xWQhJ(-h(UUAh>)TfrmixYo*=W~T z1;04$G%9M0NykqI$^H9GtD5`k@A(a&f}?hwQNbPv87y`B&gfA4y8bqG+)uhhh0b^2 zR)9#baNSq5oc-hWnW}Obdz2e@j8k60@)YO%M6h*oY)!>r@r1?7A*GLRwRU*DyO=IrK8qqF10@KBi56ZnU-% zCB&0LC#=br^F2xzf^v>EmSG-UELOsnG}*;5PP$CDKDMv zVj+SCRWRJS$fhcqdzIV^u_VE~U}9g!yhbHv$jmnT_j9*DA@KWK+OJ#?CJ4m~#T3Eu zdV{%~s}7jAK0ig0HzPtYD7>UuIGd<#NZ;{@ZlfueJne6{`4b%Ty6pd;K@i_P*Y*?Y zg{lWi;$nBOPRCDF>gl{b-Al>lczq4IKON}vB$BmIE869T)MCg=aalr4)>~nvncXjs zwYi?{Zz5bRW_wQ^(6?~jf@f^hs1!w;-%ab?(-Uj{b0Ts7^9&3#3;F(uw!7P;a=k?L=cBk;pw!p|M_`4`9KR<=?} zV$I*=q zwUTb$&gu2_2}l~b$#lByE9!y&%yT4Z2;c)kdZ0RR2&_P92kWgJy7P4*AfdbWypu`s z)rN-}pZmjknYJBTIB%Y{A8JL>;E4e1z1P815@Rg>S0;iG#lMx76(84r1-js+ zypJkD%d~*N>}dAR9(1sawd8_kl;8()x1yP1C+^c4*muKL4A-sI1`|En!Ev*^cF3lq zZ;4Y0cX58!`1IF2r&a;R@{F7ft=0?9%Iy8xb+mj@5s73Y!JmM=xCQSnX!EGKnL#>H zeYl}o;Y=mAc49|0!JJNh9mVy00~Gl+!VkM`b)Z?1x|-?&{&AKrF@;KLfqve~WSqdmg>VG1n9XjarxlL*vjCgOAOW>Uh0)qCfm7~xG|njUdHyR zN6wrfxm*8rM=b7u>|2N#NdSMN$@E5msHo%uW=XO@fqWx%m#hR%Ajv{8RDCiiGYSG2 v6lidOU=NW2G6Mh`0{;J@!)o2$UvVZ9lXpsN?cwACcPNYi6An5-9 +# +[package] +name = "zplugin-ros1" +version = "0.0.1" +authors = [ + "Dmitrii Bannov ", +] +edition = "2021" +repository = "https://github.com/ZettaScaleLabs/zenoh-plugin-ros1" +homepage = "http://zenoh.io" +license = " EPL-2.0 OR Apache-2.0" +categories = ["network-programming"] +description = "Zenoh plugin for ROS1" + +[lib] +name = "zplugin_ros1" +crate-type = ["cdylib", "rlib"] + +[features] +no_mangle = ["zenoh-plugin-trait/no_mangle"] +default = ["no_mangle"] + +[dependencies] +cyclors = "0.1.5" +derivative = "2.2.0" +env_logger = "0.9.1" +flume = "0.10.14" +futures = "0.3.24" +git-version = "0.3.5" +hex = "0.4.3" +lazy_static = "1.4.0" +log = "0.4.17" +regex = "1.6.0" +serde = "1.0.147" +serde_json = "1.0.85" +async-global-executor = "2.3.1" +rand = "0.8.5" + +# zenoh = { version = "0.6.0-beta.1", features = ["unstable"] } +# zenoh = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } +zenoh = { path = "../../../zenoh/zenoh", features = ["unstable"] } + +# zenoh-ext = { version = "0.6.0-beta.1", features = ["unstable"] } +# zenoh-ext = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } +zenoh-ext = { path = "../../../zenoh/zenoh-ext", features = ["unstable"] } + +# zenoh-core = { version = "0.6.0-beta.1" } +# zenoh-core = { git = "https://github.com/eclipse-zenoh/zenoh.git" } +zenoh-core = { path = "../../../zenoh/commons/zenoh-core" } + +# zenoh-plugin-trait = { version = "0.6.0-beta.1", default-features = false } +# zenoh-plugin-trait = { git = "https://github.com/eclipse-zenoh/zenoh.git", default-features = false } +zenoh-plugin-trait = { path = "../../../zenoh/plugins/zenoh-plugin-trait", default-features = false } + +# rosrust = "0.9" +# rosrust = { git = "https://github.com/ZettaScaleLabs/rosrust.git", branch = "feature/fix_bugs" } +rosrust = { path = "../../rosrust/rosrust/rosrust" } + +[dev-dependencies] +strum = "0.24" +strum_macros = "0.24" +serial_test = "0.10.0" +multiset = "0.0.5" + +[dependencies.async-std] +version = "=1.12.0" +features = ["unstable", "attributes"] + +[build-dependencies] +rustc_version = "0.4" diff --git a/zplugin-ros1/build.rs b/zplugin-ros1/build.rs new file mode 100644 index 0000000..8e34100 --- /dev/null +++ b/zplugin-ros1/build.rs @@ -0,0 +1,21 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// +fn main() { + // Add rustc version to zenohd + let version_meta = rustc_version::version_meta().unwrap(); + println!( + "cargo:rustc-env=RUSTC_VERSION={}", + version_meta.short_version_string + ); +} diff --git a/zplugin-ros1/examples/bridge_with_external_master.rs b/zplugin-ros1/examples/bridge_with_external_master.rs new file mode 100644 index 0000000..b887d50 --- /dev/null +++ b/zplugin-ros1/examples/bridge_with_external_master.rs @@ -0,0 +1,31 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; + +#[async_std::main] +async fn main() { + // initiate logging + env_logger::init(); + + // create bridge with ROS1 master + // In this example the bridge will connect to master specified by ROS_MASTER_URI env variable (default http://localhost:11311/) + print!("Starting Bridge..."); + #[allow(unused_variables)] + let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()).await; + println!(" OK!"); + + println!("Running bridge, press Ctrl+C to exit..."); + async_std::task::sleep(core::time::Duration::MAX).await; +} diff --git a/zplugin-ros1/examples/bridge_with_own_master.rs b/zplugin-ros1/examples/bridge_with_own_master.rs new file mode 100644 index 0000000..3844cf8 --- /dev/null +++ b/zplugin-ros1/examples/bridge_with_own_master.rs @@ -0,0 +1,35 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; + +#[async_std::main] +async fn main() { + // initiate logging + env_logger::init(); + + // create bridge with ROS1 master + // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. + // In this example the bridge will start ROS1 master by itself. + print!("Starting Bridge..."); + #[allow(unused_variables)] + let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) + .await + .with_ros1_master() + .await; + println!(" OK!"); + + println!("Running bridge, press Ctrl+C to exit..."); + async_std::task::sleep(core::time::Duration::MAX).await; +} diff --git a/zplugin-ros1/examples/ros1_pub.rs b/zplugin-ros1/examples/ros1_pub.rs new file mode 100644 index 0000000..c831953 --- /dev/null +++ b/zplugin-ros1/examples/ros1_pub.rs @@ -0,0 +1,78 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use async_std; +use zenoh_core::AsyncResolve; + +use rosrust; +use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; + +#[async_std::main] +async fn main() { + // initiate logging + env_logger::init(); + + // create bridge with ROS1 master + // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. + // In this example the bridge will start ROS1 master by itself. + print!("Starting Bridge..."); + #[allow(unused_variables)] + let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) + .await + .with_ros1_master() + .await; + println!(" OK!"); + + // create ROS1 node and publisher + print!("Creating ROS1 Node..."); + let ros1_node = rosrust::api::Ros::new("ROS1_test_node").unwrap(); + println!(" OK!"); + print!("Creating ROS1 Publisher..."); + let ros1_publisher = ros1_node + .publish::("/some/ros/topic/", 0) + .unwrap(); + println!(" OK!"); + + // create Zenoh session and subscriber + print!("Creating Zenoh Session..."); + let zenoh_session = zenoh::open(zenoh::config::default()) + .res_async() + .await + .unwrap() + .into_arc(); + println!(" OK!"); + print!("Creating Zenoh Subscriber..."); + #[allow(unused_variables)] + let zenoh_subscriber = zenoh_session + .declare_subscriber("some/ros/topic") + .callback(|data| println!("Zenoh Subscriber: got data!")) + .res_async() + .await + .unwrap(); + println!(" OK!"); + + // here bridge will expose our test ROS topic to Zenoh so that our subscriber will get data published within it + println!("Running bridge, press Ctrl+C to exit..."); + + // run test loop publishing data to ROS topic... + let working_loop = move || { + let data: Vec = (0..10).collect(); + loop { + println!("ROS Publisher: publishing data..."); + ros1_publisher.send(rosrust::RawMessage(data.clone())).unwrap(); + std::thread::sleep(core::time::Duration::from_secs(1)); + } + }; + async_std::task::spawn_blocking(working_loop).await; +} diff --git a/zplugin-ros1/examples/ros1_service.rs b/zplugin-ros1/examples/ros1_service.rs new file mode 100644 index 0000000..59dfde4 --- /dev/null +++ b/zplugin-ros1/examples/ros1_service.rs @@ -0,0 +1,75 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zenoh::prelude::SplitBuffer; +use zenoh_core::AsyncResolve; + +use rosrust; +use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; + +#[async_std::main] +async fn main() { + // initiate logging + env_logger::init(); + + // create bridge with ROS1 master + // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. + // In this example the bridge will start ROS1 master by itself. + print!("Starting Bridge..."); + #[allow(unused_variables)] + let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) + .await + .with_ros1_master() + .await; + println!(" OK!"); + + // create ROS1 node and service + print!("Creating ROS1 Node..."); + let ros1_node = rosrust::api::Ros::new("ROS1_test_node").unwrap(); + println!(" OK!"); + print!("Creating ROS1 Service..."); + #[allow(unused_variables)] + let ros1_service = ros1_node + .service::("/some/ros/topic/", |query| -> rosrust::ServiceResult {println!("ROS Service: got query, sending reply..."); Ok(query) }) + .unwrap(); + println!(" OK!"); + + // create Zenoh session + print!("Creating Zenoh Session..."); + let zenoh_session = zenoh::open(zenoh::config::default()) + .res_async() + .await + .unwrap() + .into_arc(); + println!(" OK!"); + + // here bridge will expose our test ROS topic to Zenoh so that our ROS1 subscriber will get data published in Zenoh + println!("Running bridge, press Ctrl+C to exit..."); + + // run test loop querying Zenoh... + let data: Vec = (0..10).collect(); + loop { + println!("Zenoh: sending query..."); + let reply = zenoh_session.get("some/ros/topic").with_value(data.clone()).res_async().await.unwrap(); + let result = reply.recv_async().await; + match result { + Ok(val) => { + println!("Zenoh: got reply!"); + assert!(data == val.sample.unwrap().value.payload.contiguous().to_vec() ); + } + Err(_) => {} + } + async_std::task::sleep(core::time::Duration::from_secs(1)).await; + } +} diff --git a/zplugin-ros1/examples/ros1_sub.rs b/zplugin-ros1/examples/ros1_sub.rs new file mode 100644 index 0000000..613923d --- /dev/null +++ b/zplugin-ros1/examples/ros1_sub.rs @@ -0,0 +1,74 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zenoh_core::AsyncResolve; + +use rosrust; +use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; + +#[async_std::main] +async fn main() { + // initiate logging + env_logger::init(); + + // create bridge with ROS1 master + // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. + // In this example the bridge will start ROS1 master by itself. + print!("Starting Bridge..."); + #[allow(unused_variables)] + let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) + .await + .with_ros1_master() + .await; + println!(" OK!"); + + // create ROS1 node and subscriber + print!("Creating ROS1 Node..."); + let ros1_node = rosrust::api::Ros::new("ROS1_test_node").unwrap(); + println!(" OK!"); + print!("Creating ROS1 Subscriber..."); + #[allow(unused_variables)] + let ros1_subscriber = ros1_node + .subscribe("/some/ros/topic/", 0, |msg: rosrust::RawMessage| {println!("ROS Subscriber: got message!")}) + .unwrap(); + println!(" OK!"); + + // create Zenoh session and publisher + print!("Creating Zenoh Session..."); + let zenoh_session = zenoh::open(zenoh::config::default()) + .res_async() + .await + .unwrap() + .into_arc(); + println!(" OK!"); + print!("Creating Zenoh Publisher..."); + let zenoh_publisher = zenoh_session + .declare_publisher("some/ros/topic") + .congestion_control(zenoh::publication::CongestionControl::Block) + .res_async() + .await + .unwrap(); + println!(" OK!"); + + // here bridge will expose our test ROS topic to Zenoh so that our ROS1 subscriber will get data published in Zenoh + println!("Running bridge, press Ctrl+C to exit..."); + + // run test loop publishing data to Zenoh... + let data: Vec = (0..10).collect(); + loop { + println!("Zenoh Publisher: publishing data..."); + zenoh_publisher.put(data.clone()).res_async().await.unwrap(); + async_std::task::sleep(core::time::Duration::from_secs(1)).await; + } +} diff --git a/zplugin-ros1/src/config.rs b/zplugin-ros1/src/config.rs new file mode 100644 index 0000000..904c8d5 --- /dev/null +++ b/zplugin-ros1/src/config.rs @@ -0,0 +1,160 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// +use regex::Regex; +use serde::{de, Deserialize, Deserializer}; +use std::env; +use std::time::Duration; +use zenoh::prelude::*; + +pub const DEFAULT_DOMAIN: u32 = 0; +pub const DEFAULT_GROUP_LEASE_SEC: f64 = 3.0; +pub const DEFAULT_FORWARD_DISCOVERY: bool = false; +pub const DEFAULT_RELIABLE_ROUTES_BLOCKING: bool = true; +pub const DEFAULT_DDS_LOCALHOST_ONLY: bool = false; + +#[derive(Deserialize, Debug)] +#[serde(deny_unknown_fields)] +pub struct Config { + #[serde(default)] + pub scope: Option, + #[serde(default = "default_domain")] + pub domain: u32, + #[serde(default)] + pub group_member_id: Option, + #[serde( + default = "default_group_lease", + deserialize_with = "deserialize_group_lease" + )] + pub group_lease: Duration, + #[serde(default, deserialize_with = "deserialize_regex")] + pub allow: Option, + #[serde(default, deserialize_with = "deserialize_regex")] + pub deny: Option, + #[serde(default, deserialize_with = "deserialize_max_frequencies")] + pub max_frequencies: Vec<(Regex, f32)>, + #[serde(default)] + pub generalise_subs: Vec, + #[serde(default)] + pub generalise_pubs: Vec, + #[serde(default = "default_forward_discovery")] + pub forward_discovery: bool, + #[serde(default = "default_reliable_routes_blocking")] + pub reliable_routes_blocking: bool, + #[serde(default = "default_localhost_only")] + pub localhost_only: bool, + #[serde(default)] + __required__: bool, + #[serde(default, deserialize_with = "deserialize_paths")] + __path__: Vec, +} + +fn default_domain() -> u32 { + if let Ok(s) = env::var("ROS_DOMAIN_ID") { + s.parse::().unwrap_or(DEFAULT_DOMAIN) + } else { + DEFAULT_DOMAIN + } +} + +fn default_group_lease() -> Duration { + Duration::from_secs_f64(DEFAULT_GROUP_LEASE_SEC) +} + +fn deserialize_group_lease<'de, D>(deserializer: D) -> Result +where + D: Deserializer<'de>, +{ + let secs: f64 = Deserialize::deserialize(deserializer)?; + Ok(Duration::from_secs_f64(secs)) +} + +fn deserialize_paths<'de, D>(deserializer: D) -> Result, D::Error> +where + D: Deserializer<'de>, +{ + struct V; + impl<'de> serde::de::Visitor<'de> for V { + type Value = Vec; + fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { + write!(formatter, "a string or vector of strings") + } + fn visit_str(self, v: &str) -> Result + where + E: de::Error, + { + Ok(vec![v.into()]) + } + fn visit_seq(self, mut seq: A) -> Result + where + A: de::SeqAccess<'de>, + { + let mut v = if let Some(l) = seq.size_hint() { + Vec::with_capacity(l) + } else { + Vec::new() + }; + while let Some(s) = seq.next_element()? { + v.push(s); + } + Ok(v) + } + } + deserializer.deserialize_any(V) +} + +fn deserialize_regex<'de, D>(deserializer: D) -> Result, D::Error> +where + D: Deserializer<'de>, +{ + let s: String = Deserialize::deserialize(deserializer)?; + Regex::new(&s) + .map(Some) + .map_err(|e| de::Error::custom(format!("Invalid regex 'allow={}': {}", s, e))) +} + +fn deserialize_max_frequencies<'de, D>(deserializer: D) -> Result, D::Error> +where + D: Deserializer<'de>, +{ + let strs: Vec = Deserialize::deserialize(deserializer)?; + let mut result: Vec<(Regex, f32)> = Vec::with_capacity(strs.len()); + for s in strs { + let i = s + .find('=') + .ok_or_else(|| de::Error::custom(format!("Invalid 'max_frequency': {}", s)))?; + let regex = Regex::new(&s[0..i]).map_err(|e| { + de::Error::custom(format!("Invalid regex for 'max_frequency': '{}': {}", s, e)) + })?; + let frequency: f32 = s[i + 1..].parse().map_err(|e| { + de::Error::custom(format!( + "Invalid float value for 'max_frequency': '{}': {}", + s, e + )) + })?; + result.push((regex, frequency)); + } + Ok(result) +} + +fn default_forward_discovery() -> bool { + DEFAULT_FORWARD_DISCOVERY +} + +fn default_reliable_routes_blocking() -> bool { + DEFAULT_RELIABLE_ROUTES_BLOCKING +} + +fn default_localhost_only() -> bool { + env::var("ROS_LOCALHOST_ONLY").as_deref() == Ok("1") +} diff --git a/zplugin-ros1/src/lib.rs b/zplugin-ros1/src/lib.rs new file mode 100644 index 0000000..0c7341a --- /dev/null +++ b/zplugin-ros1/src/lib.rs @@ -0,0 +1,103 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// +#![recursion_limit = "1024"] + +use ros_to_zenoh_bridge::Ros1ToZenohBridge; +use ros_to_zenoh_bridge::environment::Environment; +use std::sync::{ + Arc +}; +use zenoh::plugins::{Plugin, RunningPluginTrait, ValidationFunction, ZenohPlugin}; +use zenoh::prelude::r#async::*; +use zenoh::runtime::Runtime; +use zenoh_core::{bail, Result as ZResult}; + +pub mod ros_to_zenoh_bridge; + +// The struct implementing the ZenohPlugin and ZenohPlugin traits +pub struct Ros1Plugin {} + +// declaration of the plugin's VTable for zenohd to find the plugin's functions to be called +zenoh_plugin_trait::declare_plugin!(Ros1Plugin); + +impl ZenohPlugin for Ros1Plugin {} +impl Plugin for Ros1Plugin { + type StartArgs = Runtime; + type RunningPlugin = zenoh::plugins::RunningPlugin; + + // A mandatory const to define, in case of the plugin is built as a standalone executable + const STATIC_NAME: &'static str = "ros1"; + + // The first operation called by zenohd on the plugin + fn start(name: &str, runtime: &Self::StartArgs) -> ZResult { + let config = runtime.config.lock(); + let self_cfg = config.plugin(name).unwrap().as_object().unwrap(); + + // run through the bridge's config options and fill them from plugins config + let plugin_configuration_entries = Environment::env(); + for entry in plugin_configuration_entries.iter() { + match self_cfg.get(entry.name) { + Some(v) => { entry.set(v); }, + None => {}, + } + } + + std::mem::drop(config); + + // return a RunningPlugin to zenohd + Ok(Box::new(RunningPlugin::new(name.clone(), runtime.clone()))) + } +} + + +// The RunningPlugin struct implementing the RunningPluginTrait trait +struct RunningPlugin { + _name: String, + _bridge: Option +} +impl RunningPluginTrait for RunningPlugin { + // Operation returning a ValidationFunction(path, old, new)-> ZResult>> + // this function will be called each time the plugin's config is changed via the zenohd admin space + fn config_checker(&self) -> ValidationFunction { + Arc::new(move |_path, _old, _new| { + bail!("Reconfiguration at runtime is not allowed!"); + }) + } + + // Function called on any query on admin space that matches this plugin's sub-part of the admin space. + // Thus the plugin can reply its contribution to the global admin space of this zenohd. + fn adminspace_getter<'a>( + &'a self, + _selector: &'a Selector<'a>, + _plugin_status_key: &str, + ) -> ZResult> { + Ok(Vec::new()) + } +} + +impl RunningPlugin { + fn new(name: &str, runtime: Runtime) -> Self { + let bridge = async_std::task::block_on( async { + // create a zenoh Session that shares the same Runtime than zenohd + let session = zenoh::init(runtime.clone()).res().await.unwrap().into_arc(); + let bridge = ros_to_zenoh_bridge::Ros1ToZenohBridge::new_with_external_session(session).await; + return bridge; + }); + + return Self { + _name: name.to_string(), + _bridge: Some(bridge) + }; + } +} \ No newline at end of file diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge.rs new file mode 100644 index 0000000..3dd43e2 --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge.rs @@ -0,0 +1,119 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use async_std::{task::JoinHandle}; + +use zenoh_core::AsyncResolve; +use zenoh; + +use std::{sync::{ + Arc, + atomic::{AtomicBool, Ordering::Relaxed} +}, process::Command}; + +use self::{ros1_to_zenoh_bridge_impl::work_cycle}; + +pub mod zenoh_client; +pub mod ros1_client; +pub mod discovery; +pub mod topic_bridge; +pub mod abstract_bridge; +pub mod bridge_type; + +mod topic_utilities; +mod topic_mapping; +mod bridges_storage; + +pub mod environment; +pub mod ros1_to_zenoh_bridge_impl; +pub mod aloha_declaration; +pub mod aloha_subscription; + +pub struct Ros1ToZenohBridge { + flag: Arc, + task_handle: Box>, + rosmaster: Option +} +impl Ros1ToZenohBridge { + pub async fn new_with_own_session(config: zenoh::config::Config) -> Self { + let session = zenoh::open(config).res_async().await.unwrap().into_arc(); + return Self::new_with_external_session(session).await; + } + + pub async fn new_with_external_session(session: Arc) -> Self { + let flag = Arc::new(AtomicBool::new(true)); + Self { + flag: flag.clone(), + task_handle: Box::new(async_std::task::spawn(Self::run(session, flag))), + rosmaster: None + } + } + + pub async fn with_ros1_master(mut self) -> Self { + assert!(self.rosmaster.is_none()); + + self.rosmaster = Some( + Command::new("rosmaster") + .stdout(std::process::Stdio::piped()) + .stderr(std::process::Stdio::piped()) + .spawn() + .unwrap(), + ); + + return self; + } + + pub async fn without_ros1_master(mut self) -> Self { + assert!(self.rosmaster.is_some()); + + if self.rosmaster.is_some() { + self.rosmaster.take().unwrap().kill().unwrap(); + self.rosmaster = None; + } + else { + let mut kill_master = Command::new("killall").arg("rosmaster").spawn().unwrap(); + kill_master.wait().unwrap(); + } + + return self; + } + + pub async fn async_await(&mut self) { + self.task_handle.as_mut().await; + } + + pub async fn stop(&mut self) { + self.flag.store(false, Relaxed); + self.async_await().await; + } + + pub async fn run( + session: Arc, + flag: Arc + ) { + work_cycle( + session, + flag, + |_v| {}, + |_status| {} + ) + .await; + } +} +impl Drop for Ros1ToZenohBridge { + fn drop(&mut self) { + self.flag.store(false, Relaxed); + } +} + diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs new file mode 100644 index 0000000..4c3152d --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs @@ -0,0 +1,258 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::sync::Arc; + +use log::{info, debug, error}; + +use zenoh::{plugins::ZResult, prelude::SplitBuffer}; +use zenoh_core::{SyncResolve, AsyncResolve}; + +use super::{ros1_client, zenoh_client, topic_utilities::make_zenoh_key, bridge_type::BridgeType}; + +pub struct AbstractBridge { + _impl: BridgeIml +} + +impl AbstractBridge { + pub async fn new(b_type: BridgeType, + topic: &rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: &Arc) -> ZResult { + let _impl = { + match b_type { + BridgeType::Publisher => {BridgeIml::Pub(Ros1ToZenoh::new(topic, ros1_client, zenoh_client).await?)}, + BridgeType::Subscriber => {BridgeIml::Sub(ZenohToRos1::new(topic, ros1_client, zenoh_client).await?)}, + BridgeType::Service => {BridgeIml::Service(Ros1ToZenohService::new(topic, ros1_client, zenoh_client).await?)}, + BridgeType::Client => {BridgeIml::Client(Ros1ToZenohClient::new(topic, ros1_client, zenoh_client.clone()).await?)}, + } + }; + return Ok(Self {_impl}); + } +} + +enum BridgeIml { + Client(Ros1ToZenohClient), + Service(Ros1ToZenohService), + Pub(Ros1ToZenoh), + Sub(ZenohToRos1) +} + +struct Ros1ToZenohClient { + _service: rosrust::Service +} +impl Ros1ToZenohClient { + async fn new(topic: &rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: Arc)-> ZResult { + info!("Creating ROS1 -> Zenoh Client bridge for topic {}, datatype {}", topic.name, topic.datatype); + + let zenoh_key = make_zenoh_key(&topic).to_string(); + match ros1_client.service::(topic, + move |q| -> rosrust::ServiceResult { + return Self::on_query(&zenoh_key, q, zenoh_client.as_ref()); + }) { + Ok(service) => { + return Ok(Ros1ToZenohClient{ + _service: service + }); + } + Err(e) => { + zenoh_core::bail!("Ros error: {}", e.to_string()) + } + } + } + +//PRIVATE: + fn on_query(key: &str, query: rosrust::RawMessage, zenoh_client: & zenoh_client::ZenohClient) -> rosrust::ServiceResult { + return async_std::task::block_on(Self::do_zenoh_query(key, query, zenoh_client)); + } + + async fn do_zenoh_query(key: &str, query: rosrust::RawMessage, zenoh_client: & zenoh_client::ZenohClient) -> rosrust::ServiceResult { + match zenoh_client.make_query_sync(key, query.0).await { + Ok(reply) => { + match reply.recv_async().await { + Ok(r) => { + match r.sample { + Ok(value) => { + let data = value.payload.contiguous().to_vec(); + debug!("Zenoh -> ROS1: sending {} bytes!", data.len()); + return Ok(rosrust::RawMessage(data)); + } + Err(e) => { + error!("ROS1 -> Zenoh Client: received Zenoh Query with error: {}", e); + let error = e.to_string(); + return Err(error); + } + } + } + Err(e) => { + error!("ROS1 -> Zenoh Client: error while receiving reply to Zenoh Query: {}", e); + let error = e.to_string(); + return Err(error); + } + } + } + Err(e) => { + error!("ROS1 -> Zenoh Client: error while creating Zenoh Query: {}", e); + let error = e.to_string(); + return Err(error); + } + } + } +} + + +struct Ros1ToZenohService { + _queryable: zenoh::queryable::Queryable<'static, ()> +} +impl Ros1ToZenohService { + async fn new<'b>(topic: & rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: &'b zenoh_client::ZenohClient)-> ZResult { + info!("Creating ROS1 -> Zenoh Service bridge for topic {}, datatype {}", topic.name, topic.datatype); + + match ros1_client.client(topic) { + Ok(client) => { + let client_in_arc = Arc::new(client); + let queryable = zenoh_client.make_queryable(make_zenoh_key(topic), + move |query| + { + async_std::task::spawn(Self::on_query(client_in_arc.clone(), query)); + }).await?; + return Ok(Ros1ToZenohService { _queryable: queryable }); + } + Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) } + } + } + +//PRIVATE: + async fn on_query(ros1_client: Arc>, query: zenoh::queryable::Query) { + match query.value() { + Some(val) => { + let payload = val.payload.contiguous().to_vec(); + debug!("ROS1 -> Zenoh Service: got query of {} bytes!", payload.len()); + Self::process_query(ros1_client, query, payload).await; + } + None => { + error!("ROS1 -> Zenoh Service: got query without value!"); + } + } + } + + async fn process_query(ros1_client: Arc>, query: zenoh::queryable::Query, payload: Vec) { + // rosrust is synchronous, so we will use spawn_blocking. If there will be an async mode some day for the rosrust, + // than reply_to_query can be refactored to async very easily + let res = async_std::task::spawn_blocking(move || { + return ros1_client.req(&rosrust::RawMessage(payload)); + }).await; + match Self::reply_to_query(res, &query).await { + Ok(_) => {}, + Err(e) => { + error!("ROS1 -> Zenoh Service: error replying to query on {}: {}", query.key_expr(), e); + } + } + } + + async fn reply_to_query(res: rosrust::error::tcpros::Result>, + query: &zenoh::queryable::Query) -> ZResult<()> { + match res { + Ok(reply) => { + match reply { + Ok(reply_message) => { + debug!("ROS1 -> Zenoh Service: got reply of {} bytes!", reply_message.0.len()); + query.reply(Ok(zenoh::prelude::Sample::new(query.key_expr().clone(), reply_message.0))).res_async().await?; + } + Err(e) => { + error!("ROS1 -> Zenoh Service: got reply from ROS1 Service with error: {}", e); + query.reply(Err(zenoh::prelude::Value::from(e))).res_async().await?; + } + } + } + Err(e) => { + error!("ROS1 -> Zenoh Service: error while sending request to ROS1 Service: {}", e); + let error = e.to_string(); + query.reply(Err(zenoh::prelude::Value::from(error))).res_async().await?; + } + } + return Ok(()); + } +} + + + +struct Ros1ToZenoh { + _subscriber: rosrust::Subscriber +} +impl Ros1ToZenoh { + async fn new<'b>(topic: & rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: &'b zenoh_client::ZenohClient)-> ZResult { + info!("Creating ROS1 -> Zenoh bridge for topic {}, datatype {}", topic.name, topic.datatype); + + let publisher = zenoh_client.publish(make_zenoh_key(topic)).await?; + match ros1_client.subscribe(topic, + move |msg: rosrust::RawMessage| + { + debug!("ROS1 -> Zenoh: sending {} bytes!", msg.0.len()); + match publisher.put(msg.0).res_sync() { + Ok(_) => {}, + Err(e) => { + error!("ROS1 -> Zenoh: error publishing: {}", e); + } + } + }) + { + Ok(subscriber) => { + return Ok(Ros1ToZenoh { _subscriber: subscriber }); + } + Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) } + } + } +} + + +struct ZenohToRos1 { + _subscriber: zenoh::subscriber::Subscriber<'static, ()> +} +impl ZenohToRos1 { + async fn new(topic: & rosrust::api::Topic, + ros1_client: & ros1_client::Ros1Client, + zenoh_client: &Arc)-> ZResult { + info!("Creating Zenoh -> ROS1 bridge for topic {}, datatype {}", topic.name, topic.datatype); + + match ros1_client.publish(topic) { + Ok(publisher) => { + let publisher_in_arc = Arc::new(publisher); + let subscriber = zenoh_client.subscribe( + make_zenoh_key(topic), + move |sample| { + let publisher_in_arc_cloned = publisher_in_arc.clone(); + async_std::task::spawn_blocking(move || { + let data = sample.value.payload.contiguous().to_vec(); + debug!("Zenoh -> ROS1: sending {} bytes!", data.len()); + match publisher_in_arc_cloned.send(rosrust::RawMessage(data)) { + Ok(_) => {}, + Err(e) => { + error!("Zenoh -> ROS1: error publishing: {}", e); + } + } + }); + }).await?; + return Ok(ZenohToRos1 { _subscriber: subscriber}) + } + Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) } + } + } +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs new file mode 100644 index 0000000..82923ab --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs @@ -0,0 +1,124 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zenoh::buffers::ZBuf; + +use std::sync::Arc; +use std::sync::atomic::{AtomicUsize, AtomicBool}; +use std::time::Duration; + +use zenoh::{prelude::r#async::*}; +use zenoh::Session; + +pub struct AlohaDeclaration { + monitor_running: Arc +} +impl Drop for AlohaDeclaration { + fn drop(&mut self) { + self.monitor_running.store(false, std::sync::atomic::Ordering::Relaxed); + } +} +impl AlohaDeclaration { + pub fn new(session: Arc, + key: OwnedKeyExpr, + beacon_period: Duration) -> Self + { + let monitor_running = Arc::new(AtomicBool::new(true)); + async_std::task::spawn(Self::aloha_monitor_task( + beacon_period, + monitor_running.clone(), + key, + session)); + return Self {monitor_running}; + } + +//PRIVATE: + async fn aloha_monitor_task(beacon_period: Duration, + monitor_running: Arc, + key: OwnedKeyExpr, + session: Arc) + { + let beacon_task_flag = Arc::new(AtomicBool::new(false)); + + let remote_beacons = Arc::new(AtomicUsize::new(0)); + let rb = remote_beacons.clone(); + let _beacon_listener = session.declare_subscriber(key.clone()) + .allowed_origin(Locality::Remote) + .reliability(Reliability::BestEffort) + .callback(move |_| { + rb.fetch_add(1, std::sync::atomic::Ordering::SeqCst); + }) + .res_async().await.unwrap(); + + let mut sending_beacons = true; + Self::start_beacon_task(beacon_period.clone(), + key.clone(), + session.clone(), + beacon_task_flag.clone()).await; + + while monitor_running.load(std::sync::atomic::Ordering::Relaxed) { + async_std::task::sleep(beacon_period).await; + match remote_beacons.fetch_and(0, std::sync::atomic::Ordering::SeqCst) { + 0 => { + if !sending_beacons { + // start publisher in ALOHA style... + let period_ns = beacon_period.as_nanos(); + let aloha_wait: u128 = rand::random::() % period_ns; + async_std::task::sleep(Duration::from_nanos(aloha_wait.try_into().unwrap())).await; + if remote_beacons.load(std::sync::atomic::Ordering::SeqCst) == 0 { + Self::start_beacon_task(beacon_period.clone(), + key.clone(), + session.clone(), + beacon_task_flag.clone()).await; + sending_beacons = true; + } + } + } + _ => { + if sending_beacons { + if rand::random::() { + Self::stop_beacon_task(beacon_task_flag.clone()).await; + sending_beacons = false; + } + } + } + } + } + Self::stop_beacon_task(beacon_task_flag).await; + } + + async fn start_beacon_task(beacon_period: Duration, key: OwnedKeyExpr, session: Arc, running: Arc) { + running.store(true, std::sync::atomic::Ordering::SeqCst); + async_std::task::spawn(Self::aloha_publishing_task(beacon_period, key, session, running)); + } + + async fn stop_beacon_task(running: Arc) { + running.store(false, std::sync::atomic::Ordering::Relaxed); + } + + async fn aloha_publishing_task(beacon_period: Duration, key: OwnedKeyExpr, session: Arc, running: Arc) { + let publisher = session.declare_publisher(key) + .allowed_destination(Locality::Remote) + .congestion_control(CongestionControl::Drop) + .priority(Priority::Background) + .res_async().await.unwrap(); + + while running.load(std::sync::atomic::Ordering::Relaxed) { + let _res = + publisher.put(zenoh::value::Value::new(ZBuf::default())).res_async().await; + async_std::task::sleep(beacon_period).await; + } + } + +} \ No newline at end of file diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs new file mode 100644 index 0000000..dfe2a9d --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs @@ -0,0 +1,223 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::{sync::{Arc, atomic::{AtomicBool, Ordering::Relaxed}}, time::Duration, collections::{BTreeMap, btree_map::Entry::*}, cell::Cell}; + +use flume::Receiver; +use futures::{Future, FutureExt, select, pin_mut}; +use log::error; +use zenoh::{prelude::r#async::*, plugins::ZResult}; + + +struct AlohaResource { + activity: AtomicBool +} +impl AlohaResource { + fn new() -> Self { + Self { + activity: AtomicBool::new(true) + } + } + + pub fn update(&mut self) { + self.activity.store(true, Relaxed); + } + + pub fn reset(&mut self) { + self.activity.store(false, Relaxed); + } + + pub fn is_active(&self) -> bool { + return self.activity.load(Relaxed); + } +} + +pub struct AlohaSubscription { + task_running: Arc +} + +impl Drop for AlohaSubscription { + fn drop(&mut self) { + self.task_running.store(false, Relaxed); + } +} +impl AlohaSubscription { + pub async fn new(session: Arc, + key: OwnedKeyExpr, + beacon_period: Duration, + on_resource_declared: F, + on_resource_undeclared: F) -> ZResult + where + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + let task_running = Arc::new(AtomicBool::new(true)); + + async_std::task::spawn(AlohaSubscription::task( + task_running.clone(), + key, + beacon_period, + session, + on_resource_declared, + on_resource_undeclared + )); + + return Ok(Self{task_running}); + } + +//PRIVATE: + async fn task(task_running: Arc, + key: OwnedKeyExpr, + beacon_period: Duration, + session: Arc, + on_resource_declared: F, + on_resource_undeclared: F) -> ZResult<()> + where + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + let mut accumulating_resources = Cell::new(BTreeMap::::new()); + let subscriber = session.declare_subscriber(key).res_async().await?; + + Self::accumulating_task( + task_running, + beacon_period*3, + &mut accumulating_resources, + &subscriber, + on_resource_declared, + on_resource_undeclared).await; + + return Ok(()); + } + + async fn listening_task<'a, F>(task_running: Arc, + accumulating_resources: &mut Cell>, + subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver> , + on_resource_declared: &F) + where + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + while task_running.load(Relaxed) { + match subscriber.recv_async().await { + Ok(val) => { + match accumulating_resources.get_mut().entry(val.key_expr.to_string()) { + Occupied(mut val) => { + val.get_mut().update(); + } + Vacant(entry) => { + entry.insert(AlohaResource::new()); + on_resource_declared(val.key_expr).await; + } + } + } + Err(e) => { + error!("Listening error: {}", e); + } + } + } + } + + async fn accumulating_task<'a, F>(task_running: Arc, + accumulate_period: Duration, + accumulating_resources: &mut Cell>, + subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver> , + on_resource_declared: F, + on_resource_undeclared: F) + where + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + while task_running.load(Relaxed) { + accumulating_resources.get_mut().iter_mut().for_each(|val| { + val.1.reset(); + }); + + { + let listen = + Self::listening_task( + task_running.clone(), + accumulating_resources, + &subscriber, + &on_resource_declared).fuse(); + let listen_timeout = async_std::task::sleep(accumulate_period).fuse(); + pin_mut!(listen, listen_timeout); + select! { + () = listen => { + + }, + () = listen_timeout => { + + } + }; + } + + for (key, val) in accumulating_resources.get_mut().iter() { + if !val.is_active() { + unsafe {on_resource_undeclared(zenoh::key_expr::KeyExpr::from_str_uncheckend(key)).await}; + } + } + + accumulating_resources.get_mut().retain(|_key, val| { + return val.is_active(); + }); + } + } +} + + +pub struct AlohaSubscriptionBuilder +{ + session: Arc, + key: OwnedKeyExpr, + beacon_period: Duration, + + on_resource_declared: Option Box + Unpin + Send + Sync> + Send + Sync + 'static>>, + on_resource_undeclared: Option Box + Unpin + Send + Sync> + Send + Sync + 'static>> +} + +impl AlohaSubscriptionBuilder +{ + pub fn new(session: Arc, + key: OwnedKeyExpr, + beacon_period: Duration) -> Self { + return Self { session, + key, + beacon_period, + on_resource_declared: None, + on_resource_undeclared: None }; + } + + pub fn on_resource_declared(mut self, on_resource_declared: F) -> Self + where + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + self.on_resource_declared = Some(Box::new(on_resource_declared)); + return self; + } + + pub fn on_resource_undeclared(mut self, on_resource_undeclared: F) -> Self + where + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + self.on_resource_undeclared = Some(Box::new(on_resource_undeclared)); + return self; + } + + pub async fn build(self) -> ZResult { + return AlohaSubscription::new( + self.session, + self.key, + self.beacon_period, + self.on_resource_declared.unwrap_or(Box::new(|_dummy|{ Box::new(Box::pin(async {})) })), + self.on_resource_undeclared.unwrap_or(Box::new(|_dummy|{ Box::new(Box::pin(async {})) })), + ).await; + } +} \ No newline at end of file diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs new file mode 100644 index 0000000..795acfb --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs @@ -0,0 +1,21 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +#[derive(Clone, Copy, PartialEq, Eq)] +pub enum BridgeType { + Publisher, + Subscriber, + Service, + Client +} \ No newline at end of file diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs new file mode 100644 index 0000000..f64ec86 --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs @@ -0,0 +1,272 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::{collections::{HashMap, hash_map::Entry, HashSet}, sync::Arc}; + +use super::{topic_bridge::{TopicBridge, self}, bridge_type::BridgeType, ros1_client, discovery::LocalResources, zenoh_client, topic_mapping::Ros1TopicMapping, ros1_to_zenoh_bridge_impl::BridgeStatus}; + + +struct Bridges { + publisher_bridges: HashMap, + subscriber_bridges: HashMap, + service_bridges: HashMap, + client_bridges: HashMap +} +impl Bridges { + fn new() -> Self { + Self { + publisher_bridges: HashMap::new(), + subscriber_bridges: HashMap::new(), + service_bridges: HashMap::new(), + client_bridges: HashMap::new() + } + } + + fn container_mut(&mut self, b_type: BridgeType) -> &mut HashMap { + match b_type { + BridgeType::Publisher => &mut self.publisher_bridges, + BridgeType::Subscriber => &mut self.subscriber_bridges, + BridgeType::Service => &mut self.service_bridges, + BridgeType::Client => &mut self.client_bridges, + } + } + + fn status(&self) -> BridgeStatus { + let fill = |status: &mut (usize, usize), bridges: &HashMap| { + for (_topic, bridge) in bridges.iter() { + status.0 += 1; + if bridge.is_bridging() { + status.1 += 1; + } + } + }; + + let mut result = BridgeStatus::default(); + fill(&mut result.ros_clients, &self.client_bridges); + fill(&mut result.ros_publishers, &self.publisher_bridges); + fill(&mut result.ros_services, &self.service_bridges); + fill(&mut result.ros_subscribers, &self.subscriber_bridges); + return result; + } + + fn clear(&mut self) { + self.publisher_bridges.clear(); + self.subscriber_bridges.clear(); + self.service_bridges.clear(); + self.client_bridges.clear(); + } +} + + +struct Access<'a> { + container: &'a mut HashMap, + b_type: BridgeType, + ros1_client: &'a Arc, + zenoh_client: &'a Arc, + declaration_interface: &'a Arc +} + +impl<'a> Access<'a> { + fn new(b_type: BridgeType, + container: &'a mut HashMap, + ros1_client: &'a Arc, + zenoh_client: &'a Arc, + declaration_interface: &'a Arc) -> Self { + Self { + container, + b_type, + ros1_client, + zenoh_client, + declaration_interface } + } +} + + +pub struct ComplementaryElementAccessor<'a> { + access: Access<'a> +} + +impl<'a> ComplementaryElementAccessor<'a> { + fn new(b_type: BridgeType, + container: &'a mut HashMap, + ros1_client: &'a Arc, + zenoh_client: &'a Arc, + declaration_interface: &'a Arc) -> Self { + Self { access: Access::new(b_type, container, ros1_client, zenoh_client, declaration_interface) } + } + + pub async fn complementary_entity_lost(&mut self, topic: rosrust::api::Topic) { + match self.access.container.entry(topic) { + Entry::Occupied(mut val) => { + let bridge = val.get_mut(); + if bridge.set_has_complementary_in_zenoh(false).await && !bridge.is_actual() { + val.remove_entry(); + } + }, + Entry::Vacant(_) => {}, + } + } + + pub async fn complementary_entity_discovered(&mut self, topic: rosrust::api::Topic) { + match self.access.container.entry(topic) { + Entry::Occupied(mut val) => { + val.get_mut().set_has_complementary_in_zenoh(true).await; + } + Entry::Vacant(val) => { + let key = val.key().clone(); + val.insert(TopicBridge::new( + key, + self.access.b_type, + self.access.declaration_interface.clone(), + self.access.ros1_client.clone(), + self.access.zenoh_client.clone(), + topic_bridge::BridgingMode::Automatic + )).set_has_complementary_in_zenoh(true).await; + } + } + } +} + + +pub struct ElementAccessor<'a> { + access: Access<'a> +} + +impl<'a> ElementAccessor<'a> { + fn new(b_type: BridgeType, + container: &'a mut HashMap, + ros1_client: &'a Arc, + zenoh_client: &'a Arc, + declaration_interface: &'a Arc) -> Self { + Self { access: Access::new(b_type, container, ros1_client, zenoh_client, declaration_interface) } + } + + async fn receive_ros1_state(&mut self, + part_of_ros_state: &mut HashSet) -> bool { + let mut smth_changed = false; + // Run through bridges and actualize their state based on ROS1 state, removing corresponding entries from ROS1 state. + // As a result of this cycle, part_of_ros_state will contain only new topics that doesn't have corresponding bridge. + for (topic, bridge) in self.access.container.iter_mut() { + smth_changed |= bridge.set_present_in_ros1(part_of_ros_state.take(topic).is_some()).await; + } + + // erase all non-actual bridges + { + let size_before_retain = self.access.container.len(); + self.access.container.retain(|_topic, bridge| {return bridge.is_actual();}); + smth_changed |= size_before_retain != self.access.container.len(); + } + + // run through the topics and create corresponding bridges + for ros_topic in part_of_ros_state.iter() { + match self.access.container.entry(ros_topic.clone()) { + Entry::Occupied(_val) => { + debug_assert!(false); // that shouldn't happen + } + Entry::Vacant(val) => { + let inserted = val.insert(TopicBridge::new( + ros_topic.clone(), + self.access.b_type, + self.access.declaration_interface.clone(), + self.access.ros1_client.clone(), + self.access.zenoh_client.clone(), + topic_bridge::BridgingMode::Automatic)); + inserted.set_present_in_ros1(true).await; + smth_changed = true; + } + } + } + return smth_changed; + } +} + + +pub struct TypeAccessor<'a> { + storage: &'a mut BridgesStorage +} +impl<'a> TypeAccessor<'a> { + fn new(storage: &'a mut BridgesStorage) -> Self { Self { storage } } + + pub fn complementary_for(&'a mut self, b_type: BridgeType) -> ComplementaryElementAccessor<'a> { + let b_type = match b_type { + BridgeType::Publisher => BridgeType::Subscriber, + BridgeType::Subscriber => BridgeType::Publisher, + BridgeType::Service => BridgeType::Client, + BridgeType::Client => BridgeType::Service + }; + ComplementaryElementAccessor::new( + b_type, + self.storage.bridges.container_mut(b_type), + &self.storage.ros1_client, + &self.storage.zenoh_client, + &self.storage.declaration_interface + ) + } + + pub fn for_type(&'a mut self, b_type: BridgeType) -> ElementAccessor<'a> { + ElementAccessor::new( + b_type, + self.storage.bridges.container_mut(b_type), + &self.storage.ros1_client, + &self.storage.zenoh_client, + &self.storage.declaration_interface + ) + } +} + + + +pub struct BridgesStorage { + bridges: Bridges, + + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc +} +impl BridgesStorage { + pub fn new(ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc) -> Self { + Self { + bridges: Bridges::new(), + + ros1_client, + zenoh_client, + declaration_interface + } + } + + pub fn bridges(&mut self) -> TypeAccessor { + TypeAccessor::new(self) + } + + pub async fn receive_ros1_state (&mut self, + ros1_state: &mut Ros1TopicMapping) -> bool { + + let mut smth_changed = self.bridges().for_type(BridgeType::Publisher).receive_ros1_state(&mut ros1_state.published).await; + smth_changed |= self.bridges().for_type(BridgeType::Service).receive_ros1_state(&mut ros1_state.serviced).await; + smth_changed |= self.bridges().for_type(BridgeType::Subscriber).receive_ros1_state(&mut ros1_state.subscribed).await; + return smth_changed; + } + + pub fn status(&self) -> BridgeStatus { + return self.bridges.status(); + } + + pub fn clear(&mut self) { + return self.bridges.clear(); + } +} + + diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs new file mode 100644 index 0000000..5ce9efc --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs @@ -0,0 +1,301 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use futures::Future; +use log::error; +use rosrust; + +use std::sync::Arc; +use std::time::Duration; + +use zenoh::prelude::r#async::*; + +use std::str; + +use super::aloha_declaration::AlohaDeclaration; +use super::aloha_subscription::{AlohaSubscription, AlohaSubscriptionBuilder}; +use super::bridge_type::BridgeType; +use super::topic_utilities::{make_zenoh_key, make_topic}; + +zenoh::kedefine!( + pub discovery_format: "ros1_discovery_info/${discovery_namespace:*}/${resource_class:*}/${data_type:*}/${bridge_namespace:*}/${topic:**}", +); +// example: +// ros1_discovery_info/discovery_namespace/publishers|subscribers|services|clients/data_type/bridge_namespace/some/ros/topic +// where +// discovery_namespace - namespace to isolate different discovery pools. Would be * by default ( == global namespace) +// bridge_namespace - namespace to prefix bridge's resources. Would be * by default ( == global namespace) +// publishers|subscribers|services|clients - one of + +const ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS: &str = "pub"; +const ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS: &str = "sub"; +const ROS1_DISCOVERY_INFO_SERVICES_CLASS: &str = "srv"; +const ROS1_DISCOVERY_INFO_CLIENTS_CLASS: &str = "cl"; + + + +struct RemoteResources { + _subscriber: Option +} +impl RemoteResources { + async fn new(session: Arc, + discovery_namespace: String, + on_discovered: F, + on_lost: F) -> Self + where + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + let subscriber: Option; + + // make proper discovery keyexpr + let mut formatter = discovery_format::formatter(); + let discovery_keyexpr = zenoh::keformat!( + formatter, + discovery_namespace = discovery_namespace, + resource_class = "*", + data_type = "*", + bridge_namespace = "*", + topic = "*/**" ).unwrap(); + + let _on_discovered = Arc::new(on_discovered); + let _on_lost = Arc::new(on_lost); + + let subscription = AlohaSubscriptionBuilder::new(session, + discovery_keyexpr.clone(), + Duration::from_secs(1)) + .on_resource_declared(move |key| { Box::new( Box::pin( Self::process(key.into_owned(), _on_discovered.clone() ) ) ) }) + .on_resource_undeclared(move |key| { Box::new( Box::pin( Self::process(key.into_owned(), _on_lost.clone() ) ) ) }) + .build().await; + + match subscription { + Ok(s) => { subscriber = Some(s); } + Err(e) => { + error!("ROS1 Discovery: error creating querying subscriber: {}", e); + subscriber = None; + } + } + + return Self { _subscriber: subscriber }; + } + +// PRIVATE: + async fn process(data: KeyExpr<'_>, callback: Arc) + where + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + match Self::parse_format(&data, &callback).await { + Ok(_) => {} + Err(e) => { + error!("ROS1 Discovery: entry {}: processing error: {}", data.as_str(), e); + debug_assert!(false); + } + } + } + + async fn parse_format(data: &KeyExpr<'_>, callback: &Arc) -> Result<(), String> + where + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + { + let discovery = discovery_format::parse(&data).map_err(|err| err.to_string() )?; + return Self::handle_format(discovery, callback).await; + } + + async fn handle_format(discovery: discovery_format::Parsed<'_>, callback: &Arc) -> Result<(), String> + where + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + { + //let discovery_namespace = discovery.discovery_namespace().ok_or("No discovery_namespace present!")?; + let datatype = discovery.data_type().ok_or("No data_type present!")?; + let resource_class = discovery.resource_class().ok_or("No resource_class present!")?.to_string(); + //let bridge_namespace = discovery.bridge_namespace().ok_or("No bridge_namespace present!")?.to_string(); + let topic = discovery.topic().ok_or("No topic present!")?; + + let ros1_topic = make_topic(datatype, topic)?; + + let b_type; + match resource_class.as_str() { + ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS => { b_type = BridgeType::Publisher; } + ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS => { b_type = BridgeType::Subscriber; } + ROS1_DISCOVERY_INFO_SERVICES_CLASS => { b_type = BridgeType::Service; } + ROS1_DISCOVERY_INFO_CLIENTS_CLASS => { b_type = BridgeType::Client; } + _ => { return Err("unexpected resource class!".to_string()); } + }; + + callback(b_type, ros1_topic).await; + return Ok(()); + } +} + + + +pub struct LocalResource { + _declaration: AlohaDeclaration +} +impl LocalResource { + async fn new( + discovery_namespace: &str, + bridge_namespace: &str, + resource_class: &str, + topic: &rosrust::api::Topic, + session: Arc) -> LocalResource { + + // make proper discovery keyexpr + let mut formatter = discovery_format::formatter(); + let discovery_keyexpr = zenoh::keformat!( + formatter, + discovery_namespace = discovery_namespace, + resource_class = resource_class, + data_type = topic.datatype.clone(), + bridge_namespace = bridge_namespace, + topic = make_zenoh_key(topic)).unwrap(); + + let _declaration = AlohaDeclaration::new( + session, + discovery_keyexpr, + Duration::from_secs(1)); + + Self { _declaration } + } +} + + + +pub struct LocalResources { + session: Arc, + discovery_namespace: String, + bridge_namespace: String +} +impl LocalResources { + pub fn new( + discovery_namespace: String, + bridge_namespace: String, + session: Arc) -> LocalResources { + + return Self{ + session, + discovery_namespace, + bridge_namespace + }; + } + + pub async fn declare_with_type(&self, topic: &rosrust::api::Topic, b_type: BridgeType) -> LocalResource { + match b_type { + BridgeType::Publisher => {self.declare_publisher(topic).await} + BridgeType::Subscriber => {self.declare_subscriber(topic).await} + BridgeType::Service => {self.declare_service(topic).await} + BridgeType::Client => {self.declare_client(topic).await} + } + } + + pub async fn declare_publisher(&self, topic: &rosrust::api::Topic ) -> LocalResource { + self.declare(topic, ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS).await + } + + pub async fn declare_subscriber(&self, topic: &rosrust::api::Topic ) -> LocalResource { + self.declare(topic, ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS).await + } + + pub async fn declare_service(&self, topic: &rosrust::api::Topic ) -> LocalResource { + self.declare(topic, ROS1_DISCOVERY_INFO_SERVICES_CLASS).await + } + + pub async fn declare_client(&self, topic: &rosrust::api::Topic ) -> LocalResource { + self.declare(topic, ROS1_DISCOVERY_INFO_CLIENTS_CLASS).await + } + +//PRIVATE: + pub async fn declare(&self, topic: &rosrust::api::Topic, resource_class: &str) -> LocalResource { + LocalResource::new( + &self.discovery_namespace, + &self.bridge_namespace, + resource_class, + topic, + self.session.clone()).await + } +} + + + +pub struct Discovery { + _remote_resources: RemoteResources, + local_resources: LocalResources, +} +impl Discovery { + pub async fn new( discovery_namespace: String, + bridge_namespace: String, + session: Arc, + on_discovered: F, + on_lost: F) -> Self + where + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + return Self { + _remote_resources: RemoteResources::new(session.clone(), discovery_namespace.clone(), on_discovered, on_lost).await, + local_resources: LocalResources::new(discovery_namespace, bridge_namespace, session) + }; + } + + pub fn local_resources(&self) -> &LocalResources { + return &self.local_resources; + } +} + + + +pub struct DiscoveryBuilder +{ + discovery_namespace: String, + bridge_namespace: String, + session: Arc, + + on_discovered: Option Box + Unpin + Send + Sync> + Send + Sync + 'static>>, + on_lost: Option Box + Unpin + Send + Sync> + Send + Sync + 'static>> +} + +impl DiscoveryBuilder +{ + pub fn new(discovery_namespace: String, + bridge_namespace: String, + session: Arc) -> Self { + Self { discovery_namespace, + bridge_namespace, + session, + on_discovered: None, + on_lost: None } + } + + pub fn on_discovered(&mut self, on_discovered: F) -> &mut Self + where + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + self.on_discovered = Some(Box::new(on_discovered)); + return self; + } + pub fn on_lost(&mut self, on_lost: F) -> &mut Self + where + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + { + self.on_lost = Some(Box::new(on_lost)); + return self; + } + + pub async fn build(self) -> Discovery { + return Discovery::new( + self.discovery_namespace, + self.bridge_namespace, + self.session, + self.on_discovered.unwrap_or(Box::new(|_, _|{ Box::new(Box::pin(async {})) })), + self.on_lost.unwrap_or(Box::new(|_, _|{ Box::new(Box::pin(async {})) }))).await; + } +} \ No newline at end of file diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs new file mode 100644 index 0000000..b594398 --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs @@ -0,0 +1,82 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::str::FromStr; +use rosrust::api::resolve::*; + +#[derive(Clone)] +pub struct Entry<'a> { + pub name: &'a str, + pub default: String +} +impl<'a> Entry<'a> { + fn new(name: &'a str, default: Tvar) -> Entry<'a> + where + Tvar: ToString + { + return Entry{name, default: default.to_string()}; + } + + pub fn get(&self) -> Tvar + where + Tvar: FromStr + std::convert::From + { + match std::env::var(self.name) { + Ok(val) => { + match val.parse::() { + Ok(val) => return val, + Err(..) => {} + }; + } + Err(..) => {} + }; + return self.default.clone().into(); + } + + pub fn set(&self, value: Tvar) + where + Tvar: ToString + { + std::env::set_var(self.name, value.to_string()); + } +} + + +pub struct Environment {} +impl Environment { + pub fn ros_master_uri() -> Entry<'static> { + return Entry::new("ROS_MASTER_URI", master()); + } + + pub fn ros_hostname() -> Entry<'static> { + return Entry::new("ROS_HOSTNAME", hostname()); + } + + pub fn ros_name() -> Entry<'static> { + return Entry::new("ROS_NAME", name("ros1_to_zenoh_bridge")); + } + + pub fn ros_namespace() -> Entry<'static> { + return Entry::new("ROS_NAMESPACE", namespace()); + } + + pub fn env() -> Vec> { + return [ + Self::ros_master_uri(), + Self::ros_hostname(), + Self::ros_name(), + Self::ros_namespace() + ].to_vec(); + } +} \ No newline at end of file diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs new file mode 100644 index 0000000..e685de3 --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs @@ -0,0 +1,87 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use log::{debug}; +use rosrust; + +pub struct Ros1Client { + ros: rosrust::api::Ros +} + +impl Ros1Client { +// PUBLIC + pub fn new(name: &str) -> Ros1Client { + Ros1Client{ + ros: rosrust::api::Ros::new(name).unwrap() + } + } + + pub fn subscribe(&self, topic: &rosrust::api::Topic, callback: F) -> rosrust::api::error::Result + where + T: rosrust::Message, + F: Fn(T) + Send + 'static, + { + self.ros.subscribe( + &topic.name, + 0, + callback) + } + + pub fn publish(&self, topic: &rosrust::api::Topic) -> rosrust::api::error::Result> { + self.ros.publish(&topic.name, 0) + } + + pub fn client(&self, topic: &rosrust::api::Topic) -> rosrust::api::error::Result> { + self.ros.client::(&topic.name) + } + + pub fn service(&self, topic: &rosrust::api::Topic, handler: F) -> rosrust::api::error::Result + where + T: rosrust::ServicePair, + F: Fn(T::Request) -> rosrust::ServiceResult + Send + Sync + 'static, + { + self.ros.service::(&topic.name, handler) + } + + pub fn state(&self) -> rosrust::api::error::Response { + self.filter(self.ros.state()) + } + + pub fn topic_types(&self) -> rosrust::api::error::Response> { + self.ros.topics() + } + + // PRIVATE + /** + * Filter out topics, which are published\subscribed\serviced only by the bridge itself + */ + fn filter(&self, mut state: rosrust::api::error::Response) -> rosrust::api::error::Response { + debug!("system state before filter: {:#?}", state); + match state.as_mut() { + Ok(value) => { + let name = self.ros.name(); + + let retain_lambda = |x: &rosrust::api::TopicData| + return x.connections.len() > 1 || (x.connections.len() == 1 && x.connections[0] != name); + + value.publishers.retain(retain_lambda); + value.subscribers.retain(retain_lambda); + value.services.retain(retain_lambda); + } + Err(_) => {} + } + debug!("system state after filter: {:#?}", state); + return state; + } +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs new file mode 100644 index 0000000..91d5e67 --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs @@ -0,0 +1,245 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use async_std::{sync::{Mutex, MutexGuard}}; +use futures::{FutureExt, select, pin_mut}; + +use zenoh; + +use std::{sync::{ + Arc, + atomic::{AtomicBool, Ordering::Relaxed} +}}; + +use log::{debug, error}; + +use crate::ros_to_zenoh_bridge::{bridges_storage::BridgesStorage, topic_mapping, zenoh_client, ros1_client, discovery::LocalResources, environment::Environment}; + +use super::{discovery::DiscoveryBuilder, ros1_client::Ros1Client}; + + + +#[derive(PartialEq, Clone, Copy)] +pub enum RosStatus { + Unknown, + Synchronizing, + Ok, + Error +} + +#[derive(Default, PartialEq, Eq, Clone, Copy)] +pub struct BridgeStatus { + pub ros_publishers: (usize, usize), + pub ros_subscribers: (usize, usize), + pub ros_services: (usize, usize), + pub ros_clients: (usize, usize) +} + +pub async fn work_cycle(session: Arc, + flag: Arc, + ros_status_callback: RosStatusCallback, + statistics_callback: BridgeStatisticsCallback) +where + RosStatusCallback: Fn(RosStatus), + BridgeStatisticsCallback: Fn(BridgeStatus) + +{ + let ros1_client = Arc::new(ros1_client::Ros1Client::new(Environment::ros_name().get::().as_str() )); + let zenoh_client = Arc::new(zenoh_client::ZenohClient::new(session.clone())); + + let local_resources = Arc::new(LocalResources::new( + "*".to_string(), + "*".to_string(), + session.clone())); + + let bridges = Arc::new(Mutex::new(BridgesStorage::new( + ros1_client.clone(), + zenoh_client, + local_resources + ))); + + + let mut bridge = RosToZenohBridge::new(ros_status_callback, + statistics_callback); + + let discovery = make_discovery(session.clone(), bridges.clone()).fuse(); + + let run = bridge.run( + ros1_client, + bridges, + flag + ).fuse(); + + pin_mut!(discovery, run); + + select! { + _ = discovery => {}, + _ = run => {} + }; + +} + +async fn make_discovery<'a>(session: Arc, + bridges: Arc>) { + let mut builder = DiscoveryBuilder::new( + "*".to_string(), + "*".to_string(), + session); + + let (add_tx, add_rx) = flume::unbounded(); + let (rem_tx, rem_rx) = flume::unbounded(); + + let add_tx = Arc::new(add_tx); + let rem_tx = Arc::new(rem_tx); + + builder + .on_discovered(move |b_type, topic| { + let add_tx = add_tx.clone(); + Box::new( Box::pin( + async move { + match add_tx.send_async((b_type, topic)).await { + Ok(_) => {}, + Err(e) => { + error!("Error posting topic discoery to channel: {}", e); + } + } + } + )) + }) + .on_lost(move |b_type, topic|{ + let rem_tx = rem_tx.clone(); + Box::new( Box::pin( + async move { + match rem_tx.send_async((b_type, topic)).await { + Ok(_) => {}, + Err(e) => { + error!("Error posting topic loss to channel: {}", e); + } + } + } + )) + }); + + let _discovery = builder.build().await; + + loop { + select! { + add = add_rx.recv_async().fuse() => { + match add { + Ok((b_type, topic)) => { + bridges.lock().await.bridges().complementary_for(b_type).complementary_entity_discovered(topic).await; + } + Err(_) => todo!(), + } + } + rem = rem_rx.recv_async().fuse() => { + match rem { + Ok((b_type, topic)) => { + bridges.lock().await.bridges().complementary_for(b_type).complementary_entity_lost(topic).await; + } + Err(_) => todo!(), + } + } + } + } +} + + + +struct RosToZenohBridge +where + RosStatusCallback: Fn(RosStatus), + BridgeStatisticsCallback: Fn(BridgeStatus) +{ + ros_status: RosStatus, + ros_status_callback: RosStatusCallback, + + statistics_callback: BridgeStatisticsCallback +} + +impl RosToZenohBridge +where + RosStatusCallback: Fn(RosStatus), + BridgeStatisticsCallback: Fn(BridgeStatus) +{ +// PUBLIC + pub fn new(ros_status_callback: RosStatusCallback, + statistics_callback: BridgeStatisticsCallback) -> Self { + RosToZenohBridge { + ros_status: RosStatus::Unknown, + ros_status_callback, + + statistics_callback + } + } + + pub async fn run(&mut self, + ros1_client: Arc, + bridges: Arc>, + flag: Arc) { + while flag.load(Relaxed) { + let cl = ros1_client.clone(); + let ros1_state = async_std::task::spawn_blocking( + move || { topic_mapping::Ros1TopicMapping::topic_mapping(cl.as_ref()) } + ).await; + + debug!("ros state: {:#?}", ros1_state); + + let mut locked = bridges.lock().await; + match ros1_state { + Ok(mut ros1_state_val) => { + self.transit_ros_status(RosStatus::Ok); + + if locked.receive_ros1_state(&mut ros1_state_val).await { + self.report_bridge_statistics(&locked).await; + async_std::task::sleep(core::time::Duration::from_millis(100)).await; + } + else { + self.report_bridge_statistics(&locked).await; + async_std::task::sleep(core::time::Duration::from_millis(200)).await; + } + } + Err(e) => { + error!("Error reading ROS state: {}", e); + + self.transit_ros_status(RosStatus::Error); + Self::cleanup(&mut locked); + self.report_bridge_statistics(&locked).await; + + async_std::task::sleep(core::time::Duration::from_millis(500)).await; + } + } + } + } + +// PRIVATE + fn transit_ros_status(&mut self, new_ros_status: RosStatus) { + if self.ros_status != new_ros_status { + self.ros_status = new_ros_status; + (self.ros_status_callback)(self.ros_status); + } + } + + async fn report_bridge_statistics(&self, locked: &MutexGuard<'_, BridgesStorage>) { + (self.statistics_callback)(locked.status()); + } + + fn cleanup(locked: &mut MutexGuard<'_, BridgesStorage>) { + locked.clear(); + } +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs new file mode 100644 index 0000000..e4ff5af --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs @@ -0,0 +1,131 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::sync::Arc; +use log::error; +use super::{ros1_client, zenoh_client, discovery::{LocalResources, LocalResource}, bridge_type::BridgeType, abstract_bridge::AbstractBridge}; + + +#[derive(PartialEq, Eq)] +pub enum BridgingMode { + Lazy, + Automatic +} + +pub struct TopicBridge { + topic: rosrust::api::Topic, + b_type: BridgeType, + ros1_client: Arc, + zenoh_client: Arc, + + briging_mode: BridgingMode, + required_on_ros1_side: bool, + required_on_zenoh_side: bool, + + declaration_interface: Arc, + declaration: Option, + + bridge: Option +} + +impl TopicBridge { + pub fn new(topic: rosrust::api::Topic, + b_type: BridgeType, + declaration_interface: Arc, + ros1_client: Arc, + zenoh_client: Arc, + briging_mode: BridgingMode) -> Self { + return Self { + topic, + b_type, + ros1_client, + zenoh_client, + briging_mode, + required_on_ros1_side: false, + required_on_zenoh_side: false, + declaration_interface, + declaration: None, + bridge: None + }; + } + + pub async fn set_present_in_ros1(&mut self, present: bool) -> bool { + let recalc = self.required_on_ros1_side != present; + self.required_on_ros1_side = present; + if recalc { + self.recalc_state().await; + } + return recalc; + } + + pub async fn set_has_complementary_in_zenoh(&mut self, present: bool) -> bool { + let recalc = self.required_on_zenoh_side != present; + self.required_on_zenoh_side = present; + if recalc { + self.recalc_state().await; + } + return recalc; + } + + pub fn is_bridging(&self) -> bool { + return self.bridge.is_some(); + } + + pub fn is_actual(&self) -> bool { + match self.briging_mode { + BridgingMode::Lazy => { + return self.required_on_ros1_side || self.required_on_zenoh_side; + } + BridgingMode::Automatic => { + return self.required_on_ros1_side; + } + } + } + +//PRIVATE: + async fn recalc_state(&mut self) { + self.recalc_declaration().await; + self.recalc_bridging().await; + } + + async fn recalc_declaration(&mut self) { + match (self.required_on_ros1_side, &self.declaration) { + (true, None) => { self.declaration = Some(self.declaration_interface.declare_with_type(&self.topic, self.b_type).await); } + (false, Some(_)) => { self.declaration = None; } + (_, _) => {} + } + } + + async fn recalc_bridging(&mut self) { + let is_discovered_client = self.b_type == BridgeType::Client && self.required_on_zenoh_side; + let is_required = self.required_on_ros1_side && (self.briging_mode == BridgingMode::Automatic || self.required_on_zenoh_side); + + if is_required || is_discovered_client { + self.create_bridge().await; + } + else { + self.bridge = None; + } + } + + async fn create_bridge(&mut self) { + match AbstractBridge::new(self.b_type, &self.topic, &self.ros1_client, &self.zenoh_client).await { + Ok(val) => {self.bridge = Some(val);} + Err(e) => { + self.bridge = None; + error!("Errr creating bridge: {}", e); + } + } + } +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs new file mode 100644 index 0000000..12f5d8c --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs @@ -0,0 +1,76 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::collections::HashSet; + +use log::{debug}; +use crate::ros_to_zenoh_bridge::ros1_client; + +#[derive(Debug)] +pub struct Ros1TopicMapping { + pub published: HashSet, + pub subscribed: HashSet, + pub serviced: HashSet, + pub clients: HashSet +} +impl Ros1TopicMapping { + pub fn topic_mapping(ros1_client: &ros1_client::Ros1Client) -> rosrust::api::error::Response { + match ros1_client.state() { + Ok(state_val) => { + match ros1_client.topic_types() { + Ok(topics_val) => { + debug!("topics: {:#?}", topics_val); + return Ok(Ros1TopicMapping::new(&state_val, &topics_val)); + } + Err(e) => { return Err(e); } + }; + } + Err(e) => { return Err(e); } + }; + } + +// PRIVATE: + fn new(state: &rosrust::api::SystemState, + topics: &Vec) -> Ros1TopicMapping { + let mut result = Ros1TopicMapping{ + published: HashSet::new(), + subscribed: HashSet::new(), + serviced: HashSet::new(), + clients: HashSet::new() + }; + + Ros1TopicMapping::fill(&mut result.subscribed, &state.subscribers, topics); + Ros1TopicMapping::fill(&mut result.published, &state.publishers, topics); + Ros1TopicMapping::fill(&mut result.serviced, &state.services, topics); + + return result; + } + + fn fill(dst: &mut HashSet, + data: &Vec, + topics: &Vec) { + + for item in data.iter() { + let topic = topics.iter().find(|x| x.name == item.name); + if topic.is_none() { + debug!("Unable to find datatype for topic {}", item.name); + dst.insert(rosrust::api::Topic{name: item.name.clone(), datatype: "*".to_string()}); + } + else { + let t = topic.unwrap(); + dst.insert(t.clone()); + } + } + } +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs new file mode 100644 index 0000000..06ab889 --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs @@ -0,0 +1,39 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zenoh::prelude::keyexpr; + +pub fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { + return topic.name.trim_start_matches('/') + .trim_end_matches('/'); +} + +pub fn make_topic(datatype: &keyexpr, topic_name: &keyexpr) -> Result { + match datatype.as_str() { + "*" | "**" => { return Err("incorrect datatype!".into()) }, + _ => {} + } + + match topic_name.as_str() { + "*" | "**" => { return Err("incorrect topic name!".into()) }, + _ => {} + } + + let mut name = topic_name.to_string(); + name.insert(0, '/'); + return Ok(rosrust::api::Topic{ + name, + datatype: datatype.to_string() + }); +} \ No newline at end of file diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs new file mode 100644 index 0000000..bc21fb9 --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs @@ -0,0 +1,124 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::sync::Arc; + +use log::{debug, info}; + +use zenoh::Session; +use zenoh::prelude::r#async::*; +pub use zenoh_core::zresult::ZResult; + +pub struct ZenohClient { + session: Arc +} + +impl ZenohClient { +// PUBLIC + pub fn new(session: Arc) -> ZenohClient { + ZenohClient { + session + } + } + + pub async fn subscribe(&self, key_expr: &str, callback: C) -> ZResult> + where + C: Fn(Sample) + Send + Sync + 'static + { + debug!("Creating Subscriber on {}", key_expr); + + match self.make_keyexpr(key_expr).await { + Ok(opt_keyexpr) => { + return self.session.declare_subscriber(opt_keyexpr) + .callback(callback) + .allowed_origin(Locality::Remote).res_async().await; + } + Err(e) => { return Err(e); } + } + } + + pub async fn publish(&self, key_expr: &str) -> ZResult> { + debug!("Creating Publisher on {}", key_expr); + + match self.make_keyexpr(key_expr).await { + Ok(opt_keyexpr) => { + return self.session.declare_publisher(opt_keyexpr) + .allowed_destination(Locality::Remote) + .congestion_control(CongestionControl::Block).res_async().await; + } + Err(e) => { return Err(e); } + } + } + + pub async fn make_queryable(&self, key_expr: &str, callback: Callback) -> ZResult > + where + Callback: Fn(zenoh::queryable::Query) + Send + Sync + 'static, + { + info!("Creating Queryable on {}", key_expr); + + match self.make_keyexpr(key_expr).await { + Ok(opt_keyexpr) => { + return self.session.declare_queryable(opt_keyexpr) + .allowed_origin(Locality::Remote) + .callback(callback).res_async().await; + } + Err(e) => { return Err(e); } + } + } + + pub async fn make_query( + &self, + key_expr: &str, + callback: Callback, + data: Vec) -> ZResult<()> + where + Callback: Fn(zenoh::query::Reply) + Send + Sync + 'static, + { + debug!("Creating Query on {}", key_expr); + + match self.make_keyexpr(key_expr).await { + Ok(opt_keyexpr) => { + return self.session.get(opt_keyexpr) + .with_value(data) + .callback(callback).res_async().await; + } + Err(e) => { return Err(e); } + } + } + + pub async fn make_query_sync( + &self, + key_expr: &str, + data: Vec) -> ZResult> + { + debug!("Creating Query on {}", key_expr); + + match self.make_keyexpr(key_expr).await { + Ok(opt_keyexpr) => { + return self.session.get(opt_keyexpr) + .with_value(data) + .allowed_destination(Locality::Remote) + .res_async().await; + } + Err(e) => { return Err(e); } + } + } + +// PRIVATE + async fn make_keyexpr(&self, key_expr: &str) -> ZResult> { + debug!("Create optimized keyexpr {}", key_expr); + + self.session.declare_keyexpr(key_expr.to_string()).res_async().await + } +} \ No newline at end of file diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs new file mode 100644 index 0000000..be5f4f2 --- /dev/null +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -0,0 +1,331 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::{sync::{Arc, Mutex, atomic::AtomicUsize}, time::Duration, collections::HashSet, str::FromStr}; + +use async_std::prelude::FutureExt; +use serial_test::serial; +use zenoh::{Session, OpenBuilder, plugins::ZResult}; +use zenoh_core::{AsyncResolve, SyncResolve}; +use zplugin_ros1::ros_to_zenoh_bridge::{aloha_declaration, aloha_subscription}; + +const TIMEOUT: Duration = Duration::from_secs(30); + +fn session_builder() -> OpenBuilder { + return zenoh::open(zenoh::config::peer()); +} + +fn declaration_builder<'a>(session: Arc, beacon_period: Duration) -> aloha_declaration::AlohaDeclaration { + return aloha_declaration::AlohaDeclaration::new( + session, + zenoh::key_expr::OwnedKeyExpr::from_str("key").unwrap(), + beacon_period); +} + +fn subscription_builder(session: Arc, beacon_period: Duration) -> aloha_subscription::AlohaSubscriptionBuilder { + return aloha_subscription::AlohaSubscriptionBuilder::new( + session, + zenoh::key_expr::OwnedKeyExpr::from_str("key").unwrap(), + beacon_period); +} + +fn make_session() -> Arc { + return session_builder().res_sync().unwrap().into_arc(); +} + +fn make_subscription(session: Arc, beacon_period: Duration) -> aloha_subscription::AlohaSubscription { + return async_std::task::block_on(subscription_builder(session, beacon_period).build()).unwrap(); +} + + +#[test] +#[serial(ROS1)] +fn discovery_instantination_one_instance() { + let session = make_session(); + let _declaration = declaration_builder(session.clone(), Duration::from_secs(1)); + let _subscription = make_subscription(session, Duration::from_secs(1)); +} + +#[test] +#[serial(ROS1)] +fn discovery_instantination_many_instances() { + let mut sessions = Vec::new(); + let mut declarations = Vec::new(); + let mut subscriptions = Vec::new(); + for _i in 0..10 { + let session = make_session(); + sessions.push(session.clone()); + declarations.push(declaration_builder(session.clone(), Duration::from_secs(1))); + } + + for session in sessions.iter() { + subscriptions.push(make_subscription(session.clone(), Duration::from_secs(1))); + } +} + + + + + + + + +pub struct PPCMeasurement<'a> { + _subscriber: zenoh::subscriber::Subscriber<'a, ()>, + ppc: Arc, + measurement_period: Duration +} +impl<'a> PPCMeasurement<'a> { + pub async fn new(session: &'a Session, + key: String, + measurement_period: Duration) -> ZResult> + { + let ppc = Arc::new(AtomicUsize::new(0)); + let p = ppc.clone(); + let subscriber = session.declare_subscriber(key) + .callback(move |_val| { + p.fetch_add(1, std::sync::atomic::Ordering::SeqCst); + }) + .res_async().await?; + + return Ok(Self{_subscriber: subscriber, ppc, measurement_period}); + } + + pub async fn measure_ppc(&self) -> usize { + self.ppc.store(0, std::sync::atomic::Ordering::SeqCst); + async_std::task::sleep(self.measurement_period).await; + self.ppc.load(std::sync::atomic::Ordering::SeqCst) + } +} + + + + + + + + + +struct DeclarationCollector { + resources: Arc>>>, + + to_be_declared: Arc>>>, + to_be_undeclared: Arc>>> +} +impl DeclarationCollector { + fn new() -> Self { + return Self { + resources: Arc::new(Mutex::new(HashSet::new())), + to_be_declared: Arc::new(Mutex::new(HashSet::new())), + to_be_undeclared: Arc::new(Mutex::new(HashSet::new())) + }; + } + + pub fn use_builder<'b>(&self, mut builder: aloha_subscription::AlohaSubscriptionBuilder) -> aloha_subscription::AlohaSubscriptionBuilder { + + let r = self.resources.clone(); + let r2 = r.clone(); + + let declared = self.to_be_declared.clone(); + let undeclared = self.to_be_undeclared.clone(); + + builder = builder + .on_resource_declared(move |k| { + assert!(declared.lock().unwrap().remove(&k.clone().into_owned())); + assert!(r.lock().unwrap().insert(k.into_owned())); + Box::new(Box::pin(async {})) + }) + .on_resource_undeclared(move |k| { + assert!(undeclared.lock().unwrap().remove(&k.clone().into_owned())); + assert!(r2.lock().unwrap().remove(&k.into_owned())); + Box::new(Box::pin(async move {})) + }); + + return builder; + } + + pub fn arm(&mut self, + declared: HashSet>, + undeclared: HashSet>) { + *self.to_be_declared.lock().unwrap() = declared; + *self.to_be_undeclared.lock().unwrap() = undeclared; + } + + pub async fn wait(&self, expected: HashSet>) { + while !self.to_be_declared.lock().unwrap().is_empty() || + !self.to_be_undeclared.lock().unwrap().is_empty() || + expected != *self.resources.lock().unwrap() + { + async_std::task::sleep(core::time::Duration::from_millis(1)).await; + } + } +} + + + + +#[derive(Default)] +struct State { + pub declarators_count: usize +} +impl State { + pub fn declarators(mut self, declarators_count: usize) -> Self { + self.declarators_count = declarators_count; + self + } +} + +async fn test_state_transition<'a>(beacon_period: Duration, + declaring_sessions: &mut Vec>, + declarations: &mut Vec, + collector: &mut DeclarationCollector, + ppc_measurer: &'a PPCMeasurement<'a>, + state: &State) { + + let ke =zenoh::key_expr::KeyExpr::from_str("key").unwrap().into_owned(); + let mut result: HashSet = HashSet::new(); + let mut undeclared: HashSet = HashSet::new(); + let mut declared: HashSet = HashSet::new(); + + match (declarations.len(), state.declarators_count) { + (0,0) => {} + (0, _) => { + result.insert(ke.clone()); + declared.insert(ke.clone()); + } + (_, 0) => { + undeclared.insert(ke.clone()); + } + (_, _) => { + result.insert(ke.clone()); + } + } + + collector.arm(declared, undeclared); + + while declarations.len() > state.declarators_count { + declarations.pop(); + } + + while declarations.len() < state.declarators_count { + if declaring_sessions.len() <= declarations.len() { + declaring_sessions.push(session_builder().res_async().await.unwrap().into_arc()); + } + declarations.push(declaration_builder(declaring_sessions[declarations.len()].clone(), beacon_period)); + } + + collector.wait(result).await; + async_std::task::sleep(beacon_period).await; + while ppc_measurer.measure_ppc().await != { let mut res = 1; if state.declarators_count == 0 { res=0; } res} {} +} + +async fn run_aloha(beacon_period: Duration, scenario: Vec) { + let mut declaring_sessions: Vec> = Vec::new(); + let mut declarations: Vec = Vec::new(); + + let mut collector = DeclarationCollector::new(); + let subscription_session = session_builder().res_async().await.unwrap().into_arc(); + let _subscriber = collector.use_builder(subscription_builder(subscription_session.clone(), beacon_period)).build().await.unwrap(); + let ppc_measurer = PPCMeasurement::new(&subscription_session, + "key".to_string(), + Duration::from_millis(100)).await.unwrap(); + for scene in scenario { + println!("Transiting State: {}", scene.declarators_count); + test_state_transition(beacon_period, + &mut declaring_sessions, + &mut declarations, + &mut collector, + &ppc_measurer, + &scene).timeout(TIMEOUT).await.expect("Timeout waiting state transition!"); + } +} + + +#[test] +#[serial(Aloha)] +fn aloha_declare_one() { + async_std::task::block_on(run_aloha( Duration::from_millis(100), + [ + State::default().declarators(1) + ].into_iter().collect() + )); +} + +#[test] +#[serial(Aloha)] +fn aloha_declare_many() { + async_std::task::block_on(run_aloha( Duration::from_millis(100), + [ + State::default().declarators(10) + ].into_iter().collect() + )); +} + +#[test] +#[serial(Aloha)] +fn aloha_declare_many_one_many() { + async_std::task::block_on(run_aloha( Duration::from_millis(100), + [ + State::default().declarators(10), + State::default().declarators(1), + State::default().declarators(10) + ].into_iter().collect() + )); +} + +#[test] +#[serial(Aloha)] +fn aloha_declare_one_zero_one() { + async_std::task::block_on(run_aloha( Duration::from_millis(100), + [ + State::default().declarators(1), + State::default().declarators(0), + State::default().declarators(1) + ].into_iter().collect() + )); +} + +#[test] +#[serial(Aloha)] +fn aloha_declare_many_zero_many() { + async_std::task::block_on(run_aloha( Duration::from_millis(100), + [ + State::default().declarators(10), + State::default().declarators(0), + State::default().declarators(10) + ].into_iter().collect() + )); +} + +#[test] +#[serial(Aloha)] +fn aloha_many_scenarios() { + async_std::task::block_on(run_aloha( Duration::from_millis(100), + [ + State::default().declarators(1), + State::default().declarators(10), + State::default().declarators(1), + State::default().declarators(10), + State::default().declarators(1), + State::default().declarators(10), + State::default().declarators(0), + State::default().declarators(1), + State::default().declarators(10), + State::default().declarators(1), + State::default().declarators(0), + State::default().declarators(10), + State::default().declarators(1), + ].into_iter().collect() + )); +} diff --git a/zplugin-ros1/tests/discovery_test.rs b/zplugin-ros1/tests/discovery_test.rs new file mode 100644 index 0000000..61a051d --- /dev/null +++ b/zplugin-ros1/tests/discovery_test.rs @@ -0,0 +1,343 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::{sync::{Arc, Mutex}, time::Duration}; + +use async_std::prelude::FutureExt; +use serial_test::serial; +use zenoh::{Session, OpenBuilder, config::ModeDependentValue}; +use zenoh_core::{AsyncResolve, SyncResolve}; +use zplugin_ros1::ros_to_zenoh_bridge::discovery; +use multiset::HashMultiSet; + +const TIMEOUT: Duration = Duration::from_secs(10); + +fn session_builder() -> OpenBuilder { + let mut config = zenoh::config::peer(); + config.timestamping.set_enabled(Some(ModeDependentValue::Unique(true))).unwrap(); + return zenoh::open(config); +} + +fn discovery_builder(session: Arc) -> discovery::DiscoveryBuilder +{ + return discovery::DiscoveryBuilder::new( + "*".to_string(), + "*".to_string(), + session); +} + +fn make_session() -> Arc { + session_builder().res_sync().unwrap().into_arc() +} + +fn make_discovery(session: Arc) -> discovery::Discovery +{ + return async_std::task::block_on(discovery_builder(session).build()); +} + +#[test] +#[serial(ROS1)] +fn discovery_instantination_one_instance() { + let session = make_session(); + let _discovery = make_discovery(session); +} + +#[test] +#[serial(ROS1)] +fn discovery_instantination_many_instances() { + let mut sessions = Vec::new(); + for _i in 0..10 { + sessions.push(make_session()); + } + + let mut discoveries = Vec::new(); + for session in sessions.iter() { + discoveries.push(make_discovery(session.clone())); + } +} + + + +struct DiscoveryCollector { + publishers: Arc>>, + subscribers: Arc>>, + services: Arc>>, + clients: Arc>> +} +impl DiscoveryCollector { + fn new() -> Self { + return Self { publishers: Arc::new(Mutex::new(HashMultiSet::new())), + subscribers: Arc::new(Mutex::new(HashMultiSet::new())), + services: Arc::new(Mutex::new(HashMultiSet::new())), + clients: Arc::new(Mutex::new(HashMultiSet::new())) }; + } + + pub fn use_builder<'a>(&self, mut builder: discovery::DiscoveryBuilder) -> discovery::DiscoveryBuilder { + let p = self.publishers.clone(); + let s = self.subscribers.clone(); + let srv = self.services.clone(); + let cli = self.clients.clone(); + + let p1 = self.publishers.clone(); + let s1 = self.subscribers.clone(); + let srv1 = self.services.clone(); + let cli1 = self.clients.clone(); + + builder + .on_discovered(move |b_type, topic| { + let container = match b_type { + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Publisher => { &p }, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Subscriber => { &s }, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Service => { &srv }, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Client => { &cli }, + }; + container.lock().unwrap().insert(topic); + Box::new(Box::pin(async {})) + }) + .on_lost(move |b_type, topic| { + let container = match b_type { + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Publisher => { &p1 }, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Subscriber => { &s1 }, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Service => { &srv1 }, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Client => { &cli1 }, + }; + container.lock().unwrap().remove(&topic); + Box::new(Box::pin(async {})) + }); + + return builder; + } + + pub async fn wait_publishers(&self, expected: HashMultiSet) { + Self::wait(&self.publishers, expected).await; + } + + pub async fn wait_subscribers(&self, expected: HashMultiSet) { + Self::wait(&self.subscribers, expected).await; + } + + pub async fn wait_services(&self, expected: HashMultiSet) { + Self::wait(&self.services, expected).await; + } + + pub async fn wait_clients(&self, expected: HashMultiSet) { + Self::wait(&self.clients, expected).await; + } + +// PRIVATE: + async fn wait(container: &Arc>>, + expected: HashMultiSet) { + while expected != *container.lock().unwrap() { + async_std::task::sleep(core::time::Duration::from_millis(1)).await; + } + } +} + + + +async fn generate_topics(count: usize, duplication: usize, stage: usize) -> HashMultiSet { + let mut result = HashMultiSet::new(); + for number in 0..count { + for _dup in 0..duplication { + result.insert(rosrust::api::Topic{name: format!("name_{}_{}",number, stage), datatype: "some".to_string()}); + } + } + return result; +} + +#[derive(Default)] +struct State { + publishers_count: usize, publishers_duplication: usize, + subscribers_count: usize, subscribers_duplication: usize, + services_count: usize, services_duplication: usize, + clients_count: usize, clients_duplication: usize, + stage: usize +} +impl State { + pub fn publishers(mut self, publishers_count: usize, publishers_duplication: usize) -> Self { + self.publishers_count = publishers_count; + self.publishers_duplication = publishers_duplication; + self + } + pub fn subscribers(mut self, subscribers_count: usize, subscribers_duplication: usize) -> Self { + self.subscribers_count = subscribers_count; + self.subscribers_duplication = subscribers_duplication; + self + } + pub fn services(mut self, services_count: usize, services_duplication: usize) -> Self { + self.services_count = services_count; + self.services_duplication = services_duplication; + self + } + pub fn clients(mut self, clients_count: usize, clients_duplication: usize) -> Self { + self.clients_count = clients_count; + self.clients_duplication = clients_duplication; + self + } + //pub fn stage(mut self, stage: usize) -> Self { + // self.stage = stage; + // self + //} + async fn summarize(&self) -> (HashMultiSet, + HashMultiSet, + HashMultiSet, + HashMultiSet) + { + return ( + generate_topics(self.publishers_count, self.publishers_duplication, self.stage).await, + generate_topics(self.subscribers_count, self.subscribers_duplication, self.stage).await, + generate_topics(self.services_count, self.services_duplication, self.stage).await, + generate_topics(self.clients_count, self.clients_duplication, self.stage).await, + ); + } +} + +async fn test_state_transition(src_discovery: &discovery::Discovery, + rcv: &DiscoveryCollector, + state: &State) { + let (publishers, + subscribers, + services, + clients) = state.summarize().await; + + let mut _pub_entities = Vec::new(); + for publisher in publishers.iter() { + _pub_entities.push(src_discovery.local_resources().declare_publisher(publisher).await); + } + + let mut _sub_entities = Vec::new(); + for subscriber in subscribers.iter() { + _sub_entities.push(src_discovery.local_resources().declare_subscriber(subscriber).await); + } + + let mut _srv_entities = Vec::new(); + for service in services.iter() { + _srv_entities.push(src_discovery.local_resources().declare_service(service).await); + } + + let mut _cl_entities = Vec::new(); + for client in clients.iter() { + _cl_entities.push(src_discovery.local_resources().declare_client(client).await); + } + + rcv.wait_publishers(publishers).await; + rcv.wait_subscribers(subscribers).await; + rcv.wait_services(services).await; + rcv.wait_clients(clients).await; +} + +async fn run_discovery(scenario: Vec) { + let src_session = session_builder().res_async().await.unwrap().into_arc(); + let src_discovery = discovery_builder(src_session).build().await; + + let rcv = DiscoveryCollector::new(); + let rcv_session = session_builder().res_async().await.unwrap().into_arc(); + let _rcv_discovery = rcv.use_builder(discovery_builder(rcv_session)).build().await; + + for scene in scenario { + test_state_transition(&src_discovery, + &rcv, + &scene).timeout(TIMEOUT).await.expect("Timeout waiting state transition!"); + } +} + +#[test] +#[serial(ROS1)] +fn discover_single_publisher() { + async_std::task::block_on(run_discovery([State::default().publishers(1, 1)].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_single_subscriber() { + async_std::task::block_on(run_discovery([State::default().subscribers(1, 1)].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_single_service() { + async_std::task::block_on(run_discovery([State::default().services(1, 1)].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_single_client() { + async_std::task::block_on(run_discovery([State::default().clients(1, 1)].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_single_transition() { + async_std::task::block_on(run_discovery([ + State::default().publishers(1, 1), + State::default().subscribers(1, 1), + State::default().services(1, 1), + State::default().clients(1, 1), + ].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_single_transition_with_zero_state() { + async_std::task::block_on(run_discovery([ + State::default().publishers(1, 1), + State::default(), + State::default().subscribers(1, 1), + State::default(), + State::default().services(1, 1), + State::default(), + State::default().clients(1, 1), + ].into_iter().collect())); +} + + + +#[test] +#[serial(ROS1)] +fn discover_multiple_publishers() { + async_std::task::block_on(run_discovery([State::default().publishers(100, 1)].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_multiple_subscribers() { + async_std::task::block_on(run_discovery([State::default().subscribers(100, 1)].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_multiple_services() { + async_std::task::block_on(run_discovery([State::default().services(100, 1)].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_multiple_clients() { + async_std::task::block_on(run_discovery([State::default().clients(100, 1)].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_multiple_transition() { + async_std::task::block_on(run_discovery([ + State::default().publishers(100, 1), + State::default().subscribers(100, 1), + State::default().services(100, 1), + State::default().clients(100, 1), + ].into_iter().collect())); +} +#[test] +#[serial(ROS1)] +fn discover_multiple_transition_with_zero_state() { + async_std::task::block_on(run_discovery([ + State::default().publishers(100, 1), + State::default(), + State::default().subscribers(100, 1), + State::default(), + State::default().services(100, 1), + State::default(), + State::default().clients(100, 1), + ].into_iter().collect())); +} \ No newline at end of file diff --git a/zplugin-ros1/tests/endurance_test.rs_ b/zplugin-ros1/tests/endurance_test.rs_ new file mode 100644 index 0000000..6ac1746 --- /dev/null +++ b/zplugin-ros1/tests/endurance_test.rs_ @@ -0,0 +1,145 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zenoh::{prelude::SplitBuffer, queryable::Queryable}; +use zenoh_core::SyncResolve; +use zenoh::prelude::r#async::*; +use zenoh_core; + +use async_std; + +use std::{ + sync::{ + atomic::{Ordering::Relaxed, AtomicU64}, + Arc + }, + time +}; + + + +async fn pps_loop(counter: Arc) { + loop { + counter.store(0, Relaxed); + async_std::task::sleep(time::Duration::from_secs(1)).await; + let pps = counter.load(Relaxed); + print!("PPS: {}\n", pps); + //assert!(pps > 0); + } +} + +async fn query_loop(session: Arc, name: String, data: Arc>, counter: Arc) { + loop { + let query = session.get(name.clone()) + .with_value(data.as_ref().clone()) + .res_async().await; + match query { + Ok(result) => { + match result.recv_async().await { + Ok(res) => { + let returned_data = res.sample.unwrap().value.payload.contiguous().to_vec(); + assert!(data.as_ref().eq(&returned_data)); + counter.fetch_add(1, Relaxed); + } + Err(..) => {} + } + } + Err(..) => {} + } + } +} + +async fn run_querying(instances: u32) { + // make session + let session = zenoh::open(zenoh::config::default()) + .res_sync() + .unwrap() + .into_arc(); + + // make names + let mut names = Vec::new(); + for i in 0..instances { + let name = format!("some/key/expr{}", i); + names.push(name); + } + + // make counter + let counter = Arc::new(AtomicU64::new(0)); + + // make data + let array: Vec = (0..100).collect(); + let data = Arc::new(array); + + // make loops as tasks + let mut loops = Vec::new(); + for name in names { + loops.push(async_std::task::spawn(query_loop(session.clone(), name, data.clone(), counter.clone()))); + } + + // spawn pps task + async_std::task::spawn(pps_loop(counter)); + + // do work + futures::future::join_all(loops).await; +} + + +async fn echo(q: zenoh::queryable::Query) { + let payload = q.value().unwrap().payload.contiguous().to_vec(); + q.reply(Ok(zenoh::prelude::Sample::new(q.key_expr().clone(), payload))).res_async().await.unwrap(); +} + +async fn run_queryable(instances: u32) -> Vec>> { + // make session + let session = zenoh::open(zenoh::config::default()) + .res_sync() + .unwrap() + .into_arc(); + + // make names + let mut names = Vec::new(); + for i in 0..instances { + let name = format!("some/key/expr{}", i); + names.push(name); + } + + // make queryables + let mut queryables = Vec::new(); + for name in &names { + let queryable = session.declare_queryable(name) + .callback( |q| { async_std::task::spawn(echo(q));} ).res_async(); + queryables.push(queryable); + } + + return futures::future::join_all(queryables).await; +} + + +fn run_check_zenoh_query(instances: u32) { + print!("Starting queryables...."); + let queryables = async_std::task::block_on(run_queryable(instances)); + print!("...started"); + print!("Starting polling...."); + async_std::task::block_on(run_querying(instances)); +} + +//#[test] +fn check_zenoh_query() { + run_check_zenoh_query(1); +} + +//#[test] +fn check_zenoh_query_100() { + run_check_zenoh_query(100); +} diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs new file mode 100644 index 0000000..7b77182 --- /dev/null +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -0,0 +1,1156 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use strum_macros::Display; +use zenoh::{prelude::SplitBuffer}; +use zenoh_core::SyncResolve; + +use std::{ + str::FromStr, + sync::{ + atomic::{AtomicBool, Ordering::*, AtomicU64}, + Arc, Mutex, RwLock, + }, collections::HashSet, +}; + +use zplugin_ros1::ros_to_zenoh_bridge::ros1_to_zenoh_bridge_impl::{BridgeStatus, RosStatus, work_cycle}; +use zplugin_ros1::ros_to_zenoh_bridge::discovery::{LocalResource, LocalResources}; +use zplugin_ros1::ros_to_zenoh_bridge::{ros1_client, zenoh_client}; + +use log::{debug, error}; +use rosrust::{RawMessage, Client, Duration}; +use std::sync::atomic::AtomicUsize; +use zenoh::prelude::r#async::*; +use zenoh_core; + +use std::process::Command; +use std::{thread, time}; + +use serial_test::serial; + +async fn wait(waiter: Waiter, timeout: core::time::Duration) -> bool +where + Waiter: Fn() -> bool, +{ + let cycles = 1000; + let millis = timeout.as_millis() / cycles + 1; + + for i in 0..cycles { + async_std::task::sleep(core::time::Duration::from_millis( + millis.try_into().unwrap(), + )).await; + if waiter() { + return true; + } + } + return false; +} + +struct RunningBridge { + flag: Arc, + + ros_status: Arc>, + + bridge_status: Arc>, +} +impl RunningBridge { + pub fn new(config: zenoh::config::Config) -> RunningBridge { + let result = RunningBridge { + flag: Arc::new(AtomicBool::new(true)), + ros_status: Arc::new(Mutex::new(RosStatus::Unknown)), + bridge_status: Arc::new(Mutex::new(BridgeStatus::default())), + }; + async_std::task::spawn(Self::run( + config, + result.flag.clone(), + result.ros_status.clone(), + result.bridge_status.clone(), + )); + return result; + } + + async fn run( + config: zenoh::config::Config, + flag: Arc, + ros_status: Arc>, + bridge_status: Arc>, + ) { + let session = zenoh::open(config).res_async().await.unwrap().into_arc(); + work_cycle( + session, + flag, + move |v| { + let mut val = ros_status.lock().unwrap(); + *val = v; + }, + move |status| { + let mut my_status = bridge_status.lock().unwrap(); + *my_status = status; + }, + ) + .await; + } + + pub async fn assert_ros_error(&self) { + self.assert_status(RosStatus::Error).await; + } + pub async fn assert_ros_ok(&self) { + self.assert_status(RosStatus::Ok).await; + } + pub async fn assert_status(&self, status: RosStatus) { + assert!(self.wait_ros_status(status, core::time::Duration::from_secs(10)).await); + } + pub async fn wait_ros_status( + &self, + status: RosStatus, + timeout: core::time::Duration, + ) -> bool { + return wait( + move || { + let val = self.ros_status.lock().unwrap(); + return *val == status; + }, + timeout, + ).await; + } + + pub async fn assert_bridge_empy(&self) { + self.assert_bridge_status(|| BridgeStatus::default()).await; + } + pub async fn assert_bridge_status BridgeStatus>(&self, status: F) { + assert!(self.wait_bridge_status(status, core::time::Duration::from_secs(120)).await); + } + pub async fn wait_bridge_status BridgeStatus>( + &self, + status: F, + timeout: core::time::Duration, + ) -> bool { + return wait( + move || { + let val = self.bridge_status.lock().unwrap(); + return *val == (status)(); + }, + timeout, + ).await; + } +} +impl Drop for RunningBridge { + fn drop(&mut self) { + self.flag.store(false, Relaxed); + } +} + +#[test] +#[serial(ROS1)] +fn env_checks_no_master_init_and_exit_immed() { + let ros_env = ROSEnvironment::new(); + let bridge = RunningBridge::new(zenoh::config::Config::default()); + async_std::task::block_on(bridge.assert_ros_error()); + async_std::task::block_on(bridge.assert_bridge_empy()); +} + +#[test] +#[serial(ROS1)] +fn env_checks_no_master_init_and_wait() { + let ros_env = ROSEnvironment::new(); + let bridge = RunningBridge::new(zenoh::config::Config::default()); + async_std::task::block_on(bridge.assert_ros_error()); + async_std::task::block_on(bridge.assert_bridge_empy()); + thread::sleep(time::Duration::from_secs(1)); + async_std::task::block_on(bridge.assert_ros_error()); + async_std::task::block_on(bridge.assert_bridge_empy()); +} + +#[test] +#[serial(ROS1)] +fn env_checks_with_master_init_and_exit_immed() { + let ros_env = ROSEnvironment::new().with_master(); + let bridge = RunningBridge::new(zenoh::config::Config::default()); + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); +} + +#[test] +#[serial(ROS1)] +fn env_checks_with_master_init_and_wait() { + let ros_env = ROSEnvironment::new().with_master(); + let bridge = RunningBridge::new(zenoh::config::Config::default()); + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); + thread::sleep(time::Duration::from_secs(1)); + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); +} + +#[test] +#[serial(ROS1)] +fn env_checks_with_master_init_and_loose_master() { + let mut ros_env = Some(ROSEnvironment::new().with_master()); + let bridge = RunningBridge::new(zenoh::config::Config::default()); + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); + thread::sleep(time::Duration::from_secs(1)); + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); + ros_env = None; + async_std::task::block_on(bridge.assert_ros_error()); + async_std::task::block_on(bridge.assert_bridge_empy()); + thread::sleep(time::Duration::from_secs(1)); + async_std::task::block_on(bridge.assert_ros_error()); + async_std::task::block_on(bridge.assert_bridge_empy()); +} + +#[test] +#[serial(ROS1)] +fn env_checks_with_master_init_and_wait_for_master() { + let mut ros_env = ROSEnvironment::new(); + let bridge = RunningBridge::new(zenoh::config::Config::default()); + async_std::task::block_on(bridge.assert_ros_error()); + async_std::task::block_on(bridge.assert_bridge_empy()); + thread::sleep(time::Duration::from_secs(1)); + async_std::task::block_on(bridge.assert_ros_error()); + async_std::task::block_on(bridge.assert_bridge_empy()); + ros_env = ros_env.with_master(); + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); + thread::sleep(time::Duration::from_secs(1)); + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); +} + +#[test] +#[serial(ROS1)] +fn env_checks_with_master_init_and_reconnect_many_times_to_master() { + let mut ros_env = ROSEnvironment::new(); + let bridge = RunningBridge::new(zenoh::config::Config::default()); + for i in 0..20 { + async_std::task::block_on(bridge.assert_ros_error()); + async_std::task::block_on(bridge.assert_bridge_empy()); + ros_env = ros_env.with_master(); + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); + ros_env = ros_env.without_master(); + } +} + +struct ZenohPublisher { + inner: Arc>, +} +struct ROS1Publisher { + inner: Arc>>, +} +struct ZenohQuery { + inner: Arc, + key: String, + running: Arc, + cycles: Arc, +} +impl ZenohQuery { + fn new(inner: Arc, key: String, cycles: Arc) -> Self { + let running = Arc::new(AtomicBool::new(true)); + return Self { + inner, + key, + running, + cycles, + }; + } + + async fn query_loop( + inner: Arc, + key: String, + running: Arc, + data: Vec, + cycles: Arc, + ) { + while running.load(Relaxed) { + let query = inner.make_zenoh_query_sync(key.as_str(), data.clone()).await; + match query.recv_async().await { + Ok(reply) => { + match reply.sample { + Ok(value) => { + let returned_data = value.payload.contiguous().to_vec(); + assert!(data.eq(&returned_data)); + cycles.fetch_add(1, SeqCst); + } + Err(e) => { + error!("ZenohQuery: got reply with error: {}", e); + break; + } + } + } + Err(e) => { + error!("ZenohQuery: failed to get reply with error: {}", e); + break; + } + } + } + } +} +impl Drop for ZenohQuery { + fn drop(&mut self) { + self.running.store(false, Relaxed); + } +} + +struct ROS1Client { + running: Arc, + cycles: Arc, + ros1_client: Arc>> +} +impl ROS1Client { + fn new(inner: Arc, key: String, cycles: Arc) -> Self { + let running = Arc::new(AtomicBool::new(true)); + let ros1_client = Arc::new(inner.make_ros_client(&key)); + return Self { + running, + cycles, + ros1_client + }; + } + + fn query_loop( + running: Arc, + data: Vec, + cycles: Arc, + ros1_client: Arc>>) { + + ros1_client.data.probe(Duration::from_seconds(10).into() ).unwrap(); + + while running.load(Relaxed) { + match ros1_client.data.req(&RawMessage(data.clone())) { + Ok(reply) => { + match reply { + Ok(msg) => { + assert!(data.eq(&msg.0)); + cycles.fetch_add(1, SeqCst); + }, + Err(e) => { + error!("ROS1Client: got reply with error: {}", e); + break; + } + } + }, + Err(e) => { + error!("ROS1Client: failed to send request with error: {}", e); + break; + } + } + } + } +} +impl Drop for ROS1Client { + fn drop(&mut self) { + self.running.store(false, Relaxed); + } +} + +trait Publisher { + fn put(&self, data: Vec); + fn ready(&self) -> bool { + return true; + } +} +impl Publisher for ZenohPublisher { + fn put(&self, data: Vec) { + let inner = self.inner.clone(); + async_std::task::spawn_blocking(move || inner.put(data).res_sync().unwrap() ); + } +} +impl Publisher for ROS1Publisher { + fn put(&self, data: Vec) { + let inner = self.inner.clone(); + async_std::task::spawn_blocking( move || inner.data.send(rosrust::RawMessage(data)).unwrap() ); + } + + fn ready(&self) -> bool { + return self.inner.data.subscriber_count() != 0; + } +} +impl Publisher for ZenohQuery { + fn put(&self, data: Vec) { + async_std::task::spawn(Self::query_loop( + self.inner.clone(), + self.key.clone(), + self.running.clone(), + data, + self.cycles.clone(), + )); + } +} +impl Publisher for ROS1Client { + fn put(&self, data: Vec) { + let running = self.running.clone(); + let cycles = self.cycles.clone(); + let ros1_client = self.ros1_client.clone(); + + async_std::task::spawn_blocking(|| Self::query_loop( + running, + data, + cycles, + ros1_client + )); + } +} + +struct ZenohSubscriber { + inner: zenoh::subscriber::Subscriber<'static, ()>, +} +struct ZenohQueryable { + inner: zenoh::queryable::Queryable<'static, ()>, +} +struct ROS1Subscriber { + inner: RAIICounter, +} +struct ROS1Service { + inner: RAIICounter, +} + +trait Subscriber { + fn ready(&self) -> bool { + return true; + } +} +impl Subscriber for ZenohSubscriber {} +impl Subscriber for ZenohQueryable {} +impl Subscriber for ROS1Subscriber { + fn ready(&self) -> bool { + return self.inner.data.publisher_count() > 0 && self.inner.data.publisher_uris().len() > 0; + } +} +impl Subscriber for ROS1Service {} + + + +struct PubSub { + key: String, + publisher: Box, + subscriber: Box, + discovery_resource: LocalResource +} + +struct PingPong { + pub_sub: PubSub, + + cycles: Arc, +} +impl PingPong { + pub async fn run(self, pps_measurements: u32) { + self.start().await; + self.check_pps(pps_measurements).await; + } + +// PRIVATE: + async fn new_ros1_to_zenoh_service(key: &str, backend: Arc) -> PingPong { + let cycles = Arc::new(AtomicUsize::new(0)); + let ros1_service = backend + .make_ros_service(key, |q| -> rosrust::ServiceResult { + debug!( + "PingPong: got query of {} bytes from Zenoh to ROS1!", + q.0.len() + ); + Ok(q) // echo the request back! + }); + + let discovery_resource = backend.local_resources.declare_client(&BridgeChecker::make_topic(key)).await; + let zenoh_query = ZenohQuery::new(backend, key.to_string(), cycles.clone()); + + return PingPong { + pub_sub: PubSub { + key: key.to_string(), + publisher: Box::new(zenoh_query), + subscriber: Box::new(ROS1Service { + inner: ros1_service, + }), + discovery_resource, + }, + cycles, + }; + } + + async fn new_ros1_to_zenoh_client(key: &str, backend: Arc) -> PingPong { + let cycles = Arc::new(AtomicUsize::new(0)); + + let discovery_resource = backend.local_resources.declare_service(&BridgeChecker::make_topic(key)).await; + let zenoh_queryable = backend.make_zenoh_queryable(key, |q| { + async_std::task::spawn( async move { + let key = q.key_expr().clone(); + let val = q.value().unwrap().clone(); + q.reply(Ok(Sample::new(key, val))).res_async().await; + }); + }).await; + + return PingPong { + pub_sub: PubSub { + key: key.to_string(), + publisher: Box::new(ROS1Client::new(backend, key.to_string(), cycles.clone())), + subscriber: Box::new(ZenohQueryable{ inner: zenoh_queryable }) , + discovery_resource, + }, + cycles, + }; + } + + async fn new_ros1_to_zenoh(key: &str, backend: Arc) -> PingPong { + let cycles = Arc::new(AtomicUsize::new(0)); + let ros1_pub = Arc::new(backend.make_ros_publisher(key)); + + let discovery_resource = backend.local_resources.declare_subscriber(&BridgeChecker::make_topic(key)).await; + + let c = cycles.clone(); + let rpub = ros1_pub.clone(); + let zenoh_sub = backend + .make_zenoh_subscriber(key, move |msg| { + let data = msg.value.payload.contiguous().to_vec(); + debug!( + "PingPong: transferring {} bytes from Zenoh to ROS1!", + data.len() + ); + rpub.data.send(rosrust::RawMessage(data)).unwrap(); + c.fetch_add(1, Relaxed); + }) + .await; + + return PingPong { + pub_sub: PubSub { + key: key.to_string(), + publisher: Box::new(ROS1Publisher { inner: ros1_pub }), + subscriber: Box::new(ZenohSubscriber { inner: zenoh_sub }), + discovery_resource + }, + cycles, + }; + } + + async fn new_zenoh_to_ros1(key: &str, backend: Arc) -> PingPong { + let cycles = Arc::new(AtomicUsize::new(0)); + let zenoh_pub = Arc::new(backend.make_zenoh_publisher(key).await); + + let discovery_resource = backend.local_resources.declare_publisher(&BridgeChecker::make_topic(key)).await; + + let c = cycles.clone(); + let zpub = zenoh_pub.clone(); + let ros1_sub = backend.make_ros_subscriber(key, move |msg: rosrust::RawMessage| { + debug!( + "PingPong: transferring {} bytes from ROS1 to Zenoh!", + msg.0.len() + ); + zpub.put(msg.0).res_sync().unwrap(); + c.fetch_add(1, Relaxed); + }); + + return PingPong { + pub_sub: PubSub { + key: key.to_string(), + publisher: Box::new(ZenohPublisher { inner: zenoh_pub }), + subscriber: Box::new(ROS1Subscriber { inner: ros1_sub }), + discovery_resource + }, + cycles, + }; + } + + async fn start(&self) { + self.wait_for_pub_sub_ready().await; + self.start_ping_pong().await; + } + + async fn start_ping_pong(&self) { + debug!("Starting ping-pong!"); + let mut data = Vec::new(); + data.reserve(TestEnvironment::data_size() as usize); + for i in 0..TestEnvironment::data_size() { + data.push((i % 255) as u8); + } + self.pub_sub.publisher.put(data.clone()); + } + + async fn check_pps(&self, pps_measurements: u32) { + for i in 0..pps_measurements { + let pps = self.measure_pps().await; + print!("PPS #{}: {} \t Key: {}\n", i, pps, self.pub_sub.key); + assert!(pps > 0.0); + } + } + + async fn measure_pps(&self) -> f64 { + debug!("Starting measure PPS...."); + + let duration_milliseconds = TestEnvironment::pps_measure_period_ms(); + + let mut result = 0.0; + let mut duration: u64 = 0; + + self.cycles.store(0, Relaxed); + while !(result > 0.0 || duration >= 10000) { + async_std::task::sleep(core::time::Duration::from_millis(duration_milliseconds)).await; + duration += duration_milliseconds; + result += self.cycles.load(Relaxed) as f64; + } + debug!("...finished measure PPS!"); + return result * 1000.0 / (duration as f64); + } + + async fn wait_for_pub_sub_ready(&self) { + assert!(Self::wait(move || { + return self.pub_sub.publisher.ready() && self.pub_sub.subscriber.ready(); + }, core::time::Duration::from_secs(30)).await); + async_std::task::sleep(time::Duration::from_secs(1)).await; + } + + async fn wait(waiter: Waiter, timeout: core::time::Duration) -> bool + where + Waiter: Fn() -> bool, + { + let cycles = 1000; + let micros = timeout.as_micros() / cycles; + + for i in 0..cycles { + async_std::task::sleep(core::time::Duration::from_micros( + micros.try_into().unwrap(), + )).await; + if waiter() { + return true; + } + } + return false; + } +} + +struct ROSEnvironment { + rosmaster: Option, +} +impl Drop for ROSEnvironment { + fn drop(&mut self) { + if self.rosmaster.is_some() { + self.rosmaster.take().unwrap().kill().unwrap(); + } + } +} +impl ROSEnvironment { + pub fn new() -> Self { + let mut kill_master = Command::new("killall").arg("rosmaster").spawn().unwrap(); + kill_master.wait().unwrap(); + + return ROSEnvironment { rosmaster: None }; + } + + pub fn with_master(mut self) -> Self { + assert!(self.rosmaster.is_none()); + + self.rosmaster = Some( + Command::new("rosmaster") + .stdout(std::process::Stdio::piped()) + .stderr(std::process::Stdio::piped()) + .spawn() + .unwrap(), + ); + + return self; + } + + pub fn without_master(mut self) -> Self { + assert!(self.rosmaster.is_some()); + + if self.rosmaster.is_some() { + self.rosmaster.take().unwrap().kill().unwrap(); + self.rosmaster = None; + } + + return self; + } +} + +struct TestEnvironment { + pub bridge: RunningBridge, + pub checker: Arc, + _ros_env: ROSEnvironment, +} +impl TestEnvironment { + pub fn new() -> TestEnvironment { + // start environment for ROS + let ros_env = ROSEnvironment::new().with_master(); + + // start bridge + let bridge = RunningBridge::new(zenoh::config::Config::default()); + + // start checker's engine + let checker = Arc::new(BridgeChecker::new()); + + // this will wait for the bridge to have some expected initial state and serves two purposes: + // - asserts on the expected state + // - performs wait and ensures that everything is properly connected and negotiated within the bridge + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); + + return TestEnvironment { + bridge, + checker, + _ros_env: ros_env, + }; + } + + pub fn many_count() -> u32 { + return Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 40); + } + + pub fn pps_measurements() -> u32 { + return Self::env_var("TEST_ROS1_TO_ZENOH_PPS_ITERATIONS", 100); + } + + pub fn pps_measure_period_ms() -> u64 { + return Self::env_var("TEST_ROS1_TO_ZENOH_PPS_PERIOD_MS", 1); + } + + pub fn data_size() -> u32 { + return Self::env_var("TEST_ROS1_TO_ZENOH_DATA_SIZE", 16); + } + + pub async fn assert_bridge_status_synchronized(&self) { + self.bridge.assert_bridge_status(|| self.checker.expected_bridge_status.read().unwrap().clone()).await; + } + + // PRIVATE + fn env_var(key: &str, default: Tvar) -> Tvar + where + Tvar: FromStr, + { + match std::env::var(key) { + Ok(val) => { + match val.parse::() { + Ok(val) => return val, + Err(..) => {} + }; + } + Err(..) => {} + }; + return default; + } +} + +struct RAIICounter +where + T: Sized +{ + pub data: T, + on_destroy: Box +} + +impl RAIICounter +where + T: Sized +{ + fn new(data: T, on_destroy: F) -> Self + where + F: Fn() + Sync + Send + 'static + { + Self { data, on_destroy: Box::new(on_destroy) } + } +} + +impl Drop for RAIICounter +where + T: Sized +{ + fn drop(&mut self) { + (self.on_destroy)(); + } +} + +struct BridgeChecker { + ros_client: ros1_client::Ros1Client, + zenoh_client: zenoh_client::ZenohClient, + local_resources: LocalResources, + + pub expected_bridge_status: Arc>, +} +impl BridgeChecker { + // PUBLIC + pub fn new() -> BridgeChecker { + let session = zenoh::open(config::peer()) + .res_sync() + .unwrap() + .into_arc(); + return BridgeChecker { + ros_client: ros1_client::Ros1Client::new("test_ros_node"), + zenoh_client: zenoh_client::ZenohClient::new(session.clone()), + local_resources: LocalResources::new("*".to_string(), "*".to_string(), session), + expected_bridge_status: Arc::new(RwLock::new(BridgeStatus::default())), + }; + } + + pub async fn make_zenoh_subscriber( + &self, + name: &str, + callback: C, + ) -> zenoh::subscriber::Subscriber<'static, ()> + where + C: Fn(Sample) + Send + Sync + 'static, + { + return self + .zenoh_client + .subscribe(Self::make_zenoh_key(&Self::make_topic(name)), callback) + .await + .unwrap(); + } + + pub async fn make_zenoh_publisher(&self, name: &str) -> zenoh::publication::Publisher<'static> { + return self + .zenoh_client + .publish(Self::make_zenoh_key(&Self::make_topic(name))) + .await + .unwrap(); + } + + pub async fn make_zenoh_queryable( + &self, + name: &str, + callback: Callback, + ) -> zenoh::queryable::Queryable<'static, ()> + where + Callback: Fn(zenoh::queryable::Query) + Send + Sync + 'static, + { + return self + .zenoh_client + .make_queryable(Self::make_zenoh_key(&Self::make_topic(name)), callback) + .await + .unwrap(); + } + + pub async fn make_zenoh_query(&self, name: &str, callback: Callback, data: Vec) + where + Callback: Fn(zenoh::query::Reply) + Send + Sync + 'static, + { + return self + .zenoh_client + .make_query( + Self::make_zenoh_key(&Self::make_topic(name)), + callback, + data, + ) + .await + .unwrap(); + } + + pub async fn make_zenoh_query_sync( + &self, + name: &str, + data: Vec, + ) -> flume::Receiver { + return self + .zenoh_client + .make_query_sync(Self::make_zenoh_key(&Self::make_topic(name)), data) + .await + .unwrap(); + } + + pub fn make_ros_publisher(&self, name: &str) -> RAIICounter> { + let status = self.expected_bridge_status.clone(); + status.write().unwrap().ros_publishers.0 += 1; + status.write().unwrap().ros_publishers.1 += 1; + return RAIICounter::new(self.ros_client.publish(&Self::make_topic(name)).unwrap(), + move || { + status.write().unwrap().ros_publishers.0 -= 1; + status.write().unwrap().ros_publishers.1 -= 1; + }); + } + + pub fn make_ros_subscriber( + &self, + name: &str, + callback: F + ) -> RAIICounter + where + T: rosrust::Message, + F: Fn(T) + Send + 'static, + { + let status = self.expected_bridge_status.clone(); + status.write().unwrap().ros_subscribers.0 += 1; + status.write().unwrap().ros_subscribers.1 += 1; + return RAIICounter::new(self.ros_client.subscribe(&Self::make_topic(name), callback) + .unwrap(), + move || { + status.write().unwrap().ros_subscribers.0 -= 1; + status.write().unwrap().ros_subscribers.1 -= 1; + }); + } + + pub fn make_ros_client( + &self, + name: &str, + ) -> RAIICounter> + { + let status = self.expected_bridge_status.clone(); + status.write().unwrap().ros_clients.0 += 1; + status.write().unwrap().ros_clients.1 += 1; + return RAIICounter::new(self.ros_client.client(&Self::make_topic(name)).unwrap(), + move || { + status.write().unwrap().ros_clients.0 -= 1; + status.write().unwrap().ros_clients.1 -= 1; + }); + } + + pub fn make_ros_service( + &self, + name: &str, + handler: F, + ) -> RAIICounter + where + F: Fn(rosrust::RawMessage) -> rosrust::ServiceResult + + Send + + Sync + + 'static, + { + let status = self.expected_bridge_status.clone(); + status.write().unwrap().ros_services.0 += 1; + status.write().unwrap().ros_services.1 += 1; + return RAIICounter::new(self.ros_client.service::(&Self::make_topic(name), handler).unwrap(), + move || { + status.write().unwrap().ros_services.0 -= 1; + status.write().unwrap().ros_services.1 -= 1; + }); + } + + // PRIVATE + fn make_topic(name: &str) -> rosrust::api::Topic { + return rosrust::api::Topic { + name: name.to_string(), + datatype: "some_datatype".to_string(), + }; + } + + fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { + return topic.name.trim_start_matches('/').trim_end_matches('/'); + } +} + +#[test] +#[serial(ROS1)] +fn init_with_ros() { + let ros_env = ROSEnvironment::new().with_master(); + let bridge = RunningBridge::new(zenoh::config::Config::default()); + let checker = BridgeChecker::new(); + + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_empy()); + + let ros_publisher = checker.make_ros_publisher("/some/ros/topic"); + + async_std::task::block_on(bridge.assert_ros_ok()); + async_std::task::block_on(bridge.assert_bridge_status(|| *checker.expected_bridge_status.read().unwrap())); +} + +#[derive(PartialEq, Eq, Hash, Display)] +enum Mode { + Ros1ToZenoh, + ZenohToRos1, + Ros1Service, + Ros1Client, + + FastRun +} +static UNIQUE_NUMBER: AtomicU64 = AtomicU64::new(0); +async fn ping_pong_duplex_parallel_many_( + env: & TestEnvironment, + number: u32, + mode: std::collections::HashSet) +{ + zenoh_core::zasync_executor_init!(); + + let make_keyexpr = |i: u32, mode: Mode| -> String { + return format!("/some/key/expr{}_{}_{}", i, mode.to_string(), UNIQUE_NUMBER.fetch_add(1, SeqCst)); + }; + + // create scenarios + let mut ping_pongs = Vec::new(); + for i in 0..number { + if mode.contains(&Mode::Ros1ToZenoh) { + ping_pongs.push(PingPong::new_ros1_to_zenoh(make_keyexpr(i, Mode::Ros1ToZenoh).as_str(), env.checker.clone()).await); + } + if mode.contains(&Mode::ZenohToRos1) { + ping_pongs.push(PingPong::new_zenoh_to_ros1(make_keyexpr(i, Mode::ZenohToRos1).as_str(), env.checker.clone()).await); + } + if mode.contains(&Mode::Ros1Service) { + ping_pongs.push(PingPong::new_ros1_to_zenoh_service(make_keyexpr(i, Mode::Ros1Service).as_str(), env.checker.clone()).await); + } + if mode.contains(&Mode::Ros1Client) { + ping_pongs.push(PingPong::new_ros1_to_zenoh_client(make_keyexpr(i, Mode::Ros1Client).as_str(), env.checker.clone()).await); + } + } + + // pass scenarios to runners + let mut vec = Vec::new(); + for ping_pong in ping_pongs { + if mode.contains(&Mode::FastRun) { + vec.push(ping_pong.run(1)); + } + else { + vec.push(ping_pong.run(TestEnvironment::pps_measurements())); + } + } + + // run + env.assert_bridge_status_synchronized().await; + futures::future::join_all(vec).await; + env.assert_bridge_status_synchronized().await; +} + +#[test] +#[serial(ROS1)] +fn ping_pong_zenoh_to_ros1() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::ZenohToRos1]))); +} +#[test] +#[serial(ROS1)] +fn ping_pong_zenoh_to_ros1_many() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::ZenohToRos1]))); +} + +#[test] +#[serial(ROS1)] +fn ping_pong_ros1_to_zenoh() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1ToZenoh]))); +} +#[test] +#[serial(ROS1)] +fn ping_pong_ros1_to_zenoh_many() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1ToZenoh]))); +} + +#[test] +#[serial(ROS1)] +fn ping_pong_ros1_service() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1Service]))); +} +#[test] +#[serial(ROS1)] +fn ping_pong_ros1_service_many() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1Service]))); +} + +#[test] +#[serial(ROS1)] +fn ping_pong_ros1_client() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1Client]))); +} +#[test] +#[serial(ROS1)] +fn ping_pong_ros1_client_many() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1Client]))); +} + +#[test] +#[serial(ROS1)] +fn ping_pong_all_sequential() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::ZenohToRos1]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1ToZenoh]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1Service]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1Client]))); +} +#[test] +#[serial(ROS1)] +fn ping_pong_all_sequential_many() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::ZenohToRos1]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1ToZenoh]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1Service]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1Client]))); +} + +#[test] +#[serial(ROS1)] +fn ping_pong_all_parallel() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::ZenohToRos1, Mode::Ros1ToZenoh, Mode::Ros1Service, Mode::Ros1Client]))); +} + +#[test] +#[serial(ROS1)] +fn ping_pong_all_parallel_many() { + let env = TestEnvironment::new(); + futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::ZenohToRos1, Mode::Ros1ToZenoh, Mode::Ros1Service, Mode::Ros1Client]))); +} + +async fn main_work(env: &TestEnvironment, main_work_finished: Arc) { + assert!(!main_work_finished.load(Relaxed)); + ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::ZenohToRos1, Mode::Ros1ToZenoh, Mode::Ros1Service, Mode::Ros1Client])).await; + main_work_finished.store(true, Relaxed); +} +async fn parallel_subwork(env: &TestEnvironment, main_work_finished: Arc) { + while !main_work_finished.load(Relaxed) { + ping_pong_duplex_parallel_many_(& env, 10, HashSet::from([Mode::ZenohToRos1, Mode::Ros1ToZenoh, Mode::Ros1Service, Mode::Ros1Client, Mode::FastRun])).await; + } +} +async fn parallel_subworks(env: &TestEnvironment, main_work_finished: Arc, concurrent_subwork_count: u32) { + let mut subworks = Vec::new(); + for i in 0..concurrent_subwork_count { + subworks.push(parallel_subwork(env, main_work_finished.clone())); + } + futures::future::join_all(subworks).await; +} +#[test] +#[serial(ROS1)] +fn ping_pong_all_overlap_one() { + let env = TestEnvironment::new(); + let main_work_finished = Arc::new(AtomicBool::new(false)); + + let main_work = main_work(&env, main_work_finished.clone()); + let parallel_subworks = parallel_subworks(&env, main_work_finished.clone(), 1); + async_std::task::block_on(futures::future::join(main_work, parallel_subworks)); +} +#[test] +#[serial(ROS1)] +fn ping_pong_all_overlap_many() { + let env = TestEnvironment::new(); + let main_work_finished = Arc::new(AtomicBool::new(false)); + + let main_work = main_work(&env, main_work_finished.clone()); + let parallel_subworks = parallel_subworks(&env, main_work_finished.clone(), TestEnvironment::many_count()); + async_std::task::block_on(futures::future::join(main_work, parallel_subworks)); +} + + +// there were some issues with rosrust service, so there is a test to check it +async fn check_query(checker: & BridgeChecker) { + let name = "/some/key/expr"; + let queryable = checker + .make_ros_service(name, |q| { + print!("got query!\n"); + Ok(q) + }); + + let ros_client = checker.make_ros_client(name); + let data: Vec = (0..50).collect(); + let result = ros_client.data.req(&rosrust::RawMessage(data.clone())) + .unwrap() + .unwrap(); + assert!(data.eq(&result.0)); +} + +#[test] +#[serial(ROS1)] +fn check_query_() { + let ros_env = ROSEnvironment::new().with_master(); + let checker = BridgeChecker::new(); + futures::executor::block_on(check_query(&checker)); +} diff --git a/zplugin-ros1/tests/query_performance_test.rs_ b/zplugin-ros1/tests/query_performance_test.rs_ new file mode 100644 index 0000000..b2e5fe1 --- /dev/null +++ b/zplugin-ros1/tests/query_performance_test.rs_ @@ -0,0 +1,202 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use zenoh::{prelude::SplitBuffer, queryable::Queryable}; +use zenoh_core::SyncResolve; +use zenoh::prelude::r#async::*; +use zenoh_core; + +use serial_test::serial; + +use async_std; + +use std::{ + sync::{ + atomic::{Ordering::{Relaxed, SeqCst}, AtomicU64, AtomicBool}, + Arc + }, + time +}; + + +async fn probe_loop(session: Arc, name: String, data: Arc>, counter: Arc) { + loop { + let query = session.get(name.clone()) + .with_value(data.as_ref().clone()) + .res_async().await; + match query { + Ok(result) => { + match result.recv_async().await { + Ok(res) => { + match(res.sample) { + Ok(sample) => { + break; + } + Err(e) => {} + } + } + Err(e) => {} + } + } + Err(e) => {} + } + } +} + +async fn query_loop(session: Arc, name: String, data: Arc>, counter: Arc, works: Arc) { + while works.load(Relaxed) { + let query = session.get(name.clone()) + .with_value(data.as_ref().clone()) + .res_async().await; + match query { + Ok(result) => { + match result.recv_async().await { + Ok(res) => { + match(res.sample) { + Ok(sample) => { + let returned_data = sample.value.payload.contiguous().to_vec(); + assert!(data.as_ref().eq(&returned_data)); + counter.fetch_add(1, SeqCst); + } + Err(e) => { + print!("Error: {}", e); + } + } + } + Err(e) => { + print!("Error: {}", e); + } + } + } + Err(e) => { + print!("Error: {}", e); + } + } + } +} + +async fn echo(q: zenoh::queryable::Query) { + let payload = q.value().unwrap().payload.contiguous().to_vec(); + q.reply(Ok(zenoh::prelude::Sample::new(q.key_expr().clone(), payload))).res_async().await.unwrap(); +} + +async fn pair_measure_loop(querying_session: Arc, + data: Arc>, + counter: Arc, + names: Vec, + queryables_per_session: u32, + queryables_per_measure: u32) { + let mut queryables = Vec::new(); + let mut loops = Vec::new(); + let mut i=0; + let works = Arc::new(AtomicBool::new(true)); + + let mut queryable_session = zenoh::open(zenoh::config::default()).res_sync() + .unwrap() + .into_arc(); + + for name in names { + i += 1; + + // create new session + if i % queryables_per_session == 0 { + queryable_session = zenoh::open(zenoh::config::default()) + .res_sync() + .unwrap() + .into_arc(); + } + + // create queryable + queryables.push(queryable_session.declare_queryable(name.clone()) + .callback( |q| { async_std::task::spawn(echo(q));} ).res_async().await.unwrap()); + + // recently created queryable may become available later, so we probe it here + probe_loop(querying_session.clone(), name.clone(), data.clone(), counter.clone()).await; + + // create query loop + loops.push(async_std::task::spawn(query_loop(querying_session.clone(), name, data.clone(), counter.clone(), works.clone()))); + + // each queryables_per_measure we do perform measurement + if i % queryables_per_measure == 0 { + // warm up + async_std::task::sleep(time::Duration::from_millis(100)).await; + + // bench + counter.store(0, Relaxed); + async_std::task::sleep(time::Duration::from_secs(1)).await; + let pps = counter.load(Relaxed); + print!("{}, {}\n", loops.len(), pps); + } + } + + works.store(false, Relaxed); + futures::future::join_all(loops).await; +} + +async fn run_pairs(instances: u32, + queryables_per_session: u32, + queryables_per_measure: u32) { + // make session + let querying_session = zenoh::open(zenoh::config::default()) + .res_sync() + .unwrap() + .into_arc(); + + // make names + let mut names = Vec::new(); + for i in 0..instances { + let name = format!("some/key/expr{}", i); + names.push(name); + } + + // make counter + let counter = Arc::new(AtomicU64::new(0)); + + // make data + let array: Vec = (0..100).collect(); + let data = Arc::new(array); + + pair_measure_loop(querying_session, + data, + counter, + names, + queryables_per_session, + queryables_per_measure).await; +} + +fn run_check_zenoh_query(instances: u32, + queryables_per_session: u32, + queryables_per_measure: u32) { + async_std::task::block_on(run_pairs(instances, + queryables_per_session, + queryables_per_measure)); +} + +//#[test] +//#[serial] +fn test_query_perf_session_per_1_queryable() { + run_check_zenoh_query(100, 1, 10); +} + +//#[test] +//#[serial] +fn test_query_perf_session_per_10_queryables() { + run_check_zenoh_query(1000, 10, 10); +} + +//#[test] +//#[serial] +fn test_query_perf_single_session() { + run_check_zenoh_query(1000, 3000, 10); +} \ No newline at end of file From dc8ad114beacb7630bb15ed1b175549975e13610 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Thu, 13 Apr 2023 13:43:03 +0300 Subject: [PATCH 02/19] Remove unnecessary files --- zplugin-ros1/tests/endurance_test.rs_ | 145 ------------- zplugin-ros1/tests/query_performance_test.rs_ | 202 ------------------ 2 files changed, 347 deletions(-) delete mode 100644 zplugin-ros1/tests/endurance_test.rs_ delete mode 100644 zplugin-ros1/tests/query_performance_test.rs_ diff --git a/zplugin-ros1/tests/endurance_test.rs_ b/zplugin-ros1/tests/endurance_test.rs_ deleted file mode 100644 index 6ac1746..0000000 --- a/zplugin-ros1/tests/endurance_test.rs_ +++ /dev/null @@ -1,145 +0,0 @@ -// -// Copyright (c) 2022 ZettaScale Technology -// -// This program and the accompanying materials are made available under the -// terms of the Eclipse Public License 2.0 which is available at -// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 -// which is available at https://www.apache.org/licenses/LICENSE-2.0. -// -// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 -// -// Contributors: -// ZettaScale Zenoh Team, -// - -use zenoh::{prelude::SplitBuffer, queryable::Queryable}; -use zenoh_core::SyncResolve; -use zenoh::prelude::r#async::*; -use zenoh_core; - -use async_std; - -use std::{ - sync::{ - atomic::{Ordering::Relaxed, AtomicU64}, - Arc - }, - time -}; - - - -async fn pps_loop(counter: Arc) { - loop { - counter.store(0, Relaxed); - async_std::task::sleep(time::Duration::from_secs(1)).await; - let pps = counter.load(Relaxed); - print!("PPS: {}\n", pps); - //assert!(pps > 0); - } -} - -async fn query_loop(session: Arc, name: String, data: Arc>, counter: Arc) { - loop { - let query = session.get(name.clone()) - .with_value(data.as_ref().clone()) - .res_async().await; - match query { - Ok(result) => { - match result.recv_async().await { - Ok(res) => { - let returned_data = res.sample.unwrap().value.payload.contiguous().to_vec(); - assert!(data.as_ref().eq(&returned_data)); - counter.fetch_add(1, Relaxed); - } - Err(..) => {} - } - } - Err(..) => {} - } - } -} - -async fn run_querying(instances: u32) { - // make session - let session = zenoh::open(zenoh::config::default()) - .res_sync() - .unwrap() - .into_arc(); - - // make names - let mut names = Vec::new(); - for i in 0..instances { - let name = format!("some/key/expr{}", i); - names.push(name); - } - - // make counter - let counter = Arc::new(AtomicU64::new(0)); - - // make data - let array: Vec = (0..100).collect(); - let data = Arc::new(array); - - // make loops as tasks - let mut loops = Vec::new(); - for name in names { - loops.push(async_std::task::spawn(query_loop(session.clone(), name, data.clone(), counter.clone()))); - } - - // spawn pps task - async_std::task::spawn(pps_loop(counter)); - - // do work - futures::future::join_all(loops).await; -} - - -async fn echo(q: zenoh::queryable::Query) { - let payload = q.value().unwrap().payload.contiguous().to_vec(); - q.reply(Ok(zenoh::prelude::Sample::new(q.key_expr().clone(), payload))).res_async().await.unwrap(); -} - -async fn run_queryable(instances: u32) -> Vec>> { - // make session - let session = zenoh::open(zenoh::config::default()) - .res_sync() - .unwrap() - .into_arc(); - - // make names - let mut names = Vec::new(); - for i in 0..instances { - let name = format!("some/key/expr{}", i); - names.push(name); - } - - // make queryables - let mut queryables = Vec::new(); - for name in &names { - let queryable = session.declare_queryable(name) - .callback( |q| { async_std::task::spawn(echo(q));} ).res_async(); - queryables.push(queryable); - } - - return futures::future::join_all(queryables).await; -} - - -fn run_check_zenoh_query(instances: u32) { - print!("Starting queryables...."); - let queryables = async_std::task::block_on(run_queryable(instances)); - print!("...started"); - print!("Starting polling...."); - async_std::task::block_on(run_querying(instances)); -} - -//#[test] -fn check_zenoh_query() { - run_check_zenoh_query(1); -} - -//#[test] -fn check_zenoh_query_100() { - run_check_zenoh_query(100); -} diff --git a/zplugin-ros1/tests/query_performance_test.rs_ b/zplugin-ros1/tests/query_performance_test.rs_ deleted file mode 100644 index b2e5fe1..0000000 --- a/zplugin-ros1/tests/query_performance_test.rs_ +++ /dev/null @@ -1,202 +0,0 @@ -// -// Copyright (c) 2022 ZettaScale Technology -// -// This program and the accompanying materials are made available under the -// terms of the Eclipse Public License 2.0 which is available at -// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 -// which is available at https://www.apache.org/licenses/LICENSE-2.0. -// -// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 -// -// Contributors: -// ZettaScale Zenoh Team, -// - -use zenoh::{prelude::SplitBuffer, queryable::Queryable}; -use zenoh_core::SyncResolve; -use zenoh::prelude::r#async::*; -use zenoh_core; - -use serial_test::serial; - -use async_std; - -use std::{ - sync::{ - atomic::{Ordering::{Relaxed, SeqCst}, AtomicU64, AtomicBool}, - Arc - }, - time -}; - - -async fn probe_loop(session: Arc, name: String, data: Arc>, counter: Arc) { - loop { - let query = session.get(name.clone()) - .with_value(data.as_ref().clone()) - .res_async().await; - match query { - Ok(result) => { - match result.recv_async().await { - Ok(res) => { - match(res.sample) { - Ok(sample) => { - break; - } - Err(e) => {} - } - } - Err(e) => {} - } - } - Err(e) => {} - } - } -} - -async fn query_loop(session: Arc, name: String, data: Arc>, counter: Arc, works: Arc) { - while works.load(Relaxed) { - let query = session.get(name.clone()) - .with_value(data.as_ref().clone()) - .res_async().await; - match query { - Ok(result) => { - match result.recv_async().await { - Ok(res) => { - match(res.sample) { - Ok(sample) => { - let returned_data = sample.value.payload.contiguous().to_vec(); - assert!(data.as_ref().eq(&returned_data)); - counter.fetch_add(1, SeqCst); - } - Err(e) => { - print!("Error: {}", e); - } - } - } - Err(e) => { - print!("Error: {}", e); - } - } - } - Err(e) => { - print!("Error: {}", e); - } - } - } -} - -async fn echo(q: zenoh::queryable::Query) { - let payload = q.value().unwrap().payload.contiguous().to_vec(); - q.reply(Ok(zenoh::prelude::Sample::new(q.key_expr().clone(), payload))).res_async().await.unwrap(); -} - -async fn pair_measure_loop(querying_session: Arc, - data: Arc>, - counter: Arc, - names: Vec, - queryables_per_session: u32, - queryables_per_measure: u32) { - let mut queryables = Vec::new(); - let mut loops = Vec::new(); - let mut i=0; - let works = Arc::new(AtomicBool::new(true)); - - let mut queryable_session = zenoh::open(zenoh::config::default()).res_sync() - .unwrap() - .into_arc(); - - for name in names { - i += 1; - - // create new session - if i % queryables_per_session == 0 { - queryable_session = zenoh::open(zenoh::config::default()) - .res_sync() - .unwrap() - .into_arc(); - } - - // create queryable - queryables.push(queryable_session.declare_queryable(name.clone()) - .callback( |q| { async_std::task::spawn(echo(q));} ).res_async().await.unwrap()); - - // recently created queryable may become available later, so we probe it here - probe_loop(querying_session.clone(), name.clone(), data.clone(), counter.clone()).await; - - // create query loop - loops.push(async_std::task::spawn(query_loop(querying_session.clone(), name, data.clone(), counter.clone(), works.clone()))); - - // each queryables_per_measure we do perform measurement - if i % queryables_per_measure == 0 { - // warm up - async_std::task::sleep(time::Duration::from_millis(100)).await; - - // bench - counter.store(0, Relaxed); - async_std::task::sleep(time::Duration::from_secs(1)).await; - let pps = counter.load(Relaxed); - print!("{}, {}\n", loops.len(), pps); - } - } - - works.store(false, Relaxed); - futures::future::join_all(loops).await; -} - -async fn run_pairs(instances: u32, - queryables_per_session: u32, - queryables_per_measure: u32) { - // make session - let querying_session = zenoh::open(zenoh::config::default()) - .res_sync() - .unwrap() - .into_arc(); - - // make names - let mut names = Vec::new(); - for i in 0..instances { - let name = format!("some/key/expr{}", i); - names.push(name); - } - - // make counter - let counter = Arc::new(AtomicU64::new(0)); - - // make data - let array: Vec = (0..100).collect(); - let data = Arc::new(array); - - pair_measure_loop(querying_session, - data, - counter, - names, - queryables_per_session, - queryables_per_measure).await; -} - -fn run_check_zenoh_query(instances: u32, - queryables_per_session: u32, - queryables_per_measure: u32) { - async_std::task::block_on(run_pairs(instances, - queryables_per_session, - queryables_per_measure)); -} - -//#[test] -//#[serial] -fn test_query_perf_session_per_1_queryable() { - run_check_zenoh_query(100, 1, 10); -} - -//#[test] -//#[serial] -fn test_query_perf_session_per_10_queryables() { - run_check_zenoh_query(1000, 10, 10); -} - -//#[test] -//#[serial] -fn test_query_perf_single_session() { - run_check_zenoh_query(1000, 3000, 10); -} \ No newline at end of file From 3897b4787ec3063865fc2d7ffa267d8c356e80d2 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 14 Apr 2023 12:15:44 +0300 Subject: [PATCH 03/19] Review fixes --- zplugin-ros1/Cargo.toml | 24 +++---- .../examples/bridge_with_external_master.rs | 3 +- .../examples/bridge_with_own_master.rs | 7 +- zplugin-ros1/examples/ros1_pub.rs | 6 +- zplugin-ros1/examples/ros1_service.rs | 4 +- zplugin-ros1/examples/ros1_sub.rs | 3 +- zplugin-ros1/src/lib.rs | 26 ++++---- zplugin-ros1/src/ros_to_zenoh_bridge.rs | 66 ++++++++++++------- .../ros_to_zenoh_bridge/bridges_storage.rs | 41 ++++++------ .../src/ros_to_zenoh_bridge/environment.rs | 18 ++--- zplugin-ros1/tests/ping_pong_test.rs | 2 +- 11 files changed, 102 insertions(+), 98 deletions(-) diff --git a/zplugin-ros1/Cargo.toml b/zplugin-ros1/Cargo.toml index 91a69c7..429636b 100644 --- a/zplugin-ros1/Cargo.toml +++ b/zplugin-ros1/Cargo.toml @@ -33,40 +33,36 @@ no_mangle = ["zenoh-plugin-trait/no_mangle"] default = ["no_mangle"] [dependencies] -cyclors = "0.1.5" -derivative = "2.2.0" env_logger = "0.9.1" flume = "0.10.14" futures = "0.3.24" git-version = "0.3.5" -hex = "0.4.3" lazy_static = "1.4.0" log = "0.4.17" -regex = "1.6.0" serde = "1.0.147" serde_json = "1.0.85" async-global-executor = "2.3.1" rand = "0.8.5" # zenoh = { version = "0.6.0-beta.1", features = ["unstable"] } -# zenoh = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } -zenoh = { path = "../../../zenoh/zenoh", features = ["unstable"] } +zenoh = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } +# zenoh = { path = "../../../zenoh/zenoh", features = ["unstable"] } # zenoh-ext = { version = "0.6.0-beta.1", features = ["unstable"] } -# zenoh-ext = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } -zenoh-ext = { path = "../../../zenoh/zenoh-ext", features = ["unstable"] } +zenoh-ext = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } +# zenoh-ext = { path = "../../../zenoh/zenoh-ext", features = ["unstable"] } # zenoh-core = { version = "0.6.0-beta.1" } -# zenoh-core = { git = "https://github.com/eclipse-zenoh/zenoh.git" } -zenoh-core = { path = "../../../zenoh/commons/zenoh-core" } +zenoh-core = { git = "https://github.com/eclipse-zenoh/zenoh.git" } +# zenoh-core = { path = "../../../zenoh/commons/zenoh-core" } # zenoh-plugin-trait = { version = "0.6.0-beta.1", default-features = false } -# zenoh-plugin-trait = { git = "https://github.com/eclipse-zenoh/zenoh.git", default-features = false } -zenoh-plugin-trait = { path = "../../../zenoh/plugins/zenoh-plugin-trait", default-features = false } +zenoh-plugin-trait = { git = "https://github.com/eclipse-zenoh/zenoh.git", default-features = false } +# zenoh-plugin-trait = { path = "../../../zenoh/plugins/zenoh-plugin-trait", default-features = false } # rosrust = "0.9" -# rosrust = { git = "https://github.com/ZettaScaleLabs/rosrust.git", branch = "feature/fix_bugs" } -rosrust = { path = "../../rosrust/rosrust/rosrust" } +rosrust = { git = "https://github.com/ZettaScaleLabs/rosrust.git", branch = "feature/fix_bugs" } +# rosrust = { path = "../../rosrust/rosrust/rosrust" } [dev-dependencies] strum = "0.24" diff --git a/zplugin-ros1/examples/bridge_with_external_master.rs b/zplugin-ros1/examples/bridge_with_external_master.rs index b887d50..1fa192b 100644 --- a/zplugin-ros1/examples/bridge_with_external_master.rs +++ b/zplugin-ros1/examples/bridge_with_external_master.rs @@ -12,6 +12,7 @@ // ZettaScale Zenoh Team, // +use std::future; use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; #[async_std::main] @@ -27,5 +28,5 @@ async fn main() { println!(" OK!"); println!("Running bridge, press Ctrl+C to exit..."); - async_std::task::sleep(core::time::Duration::MAX).await; + future::pending::<()>().await; } diff --git a/zplugin-ros1/examples/bridge_with_own_master.rs b/zplugin-ros1/examples/bridge_with_own_master.rs index 3844cf8..676ccf3 100644 --- a/zplugin-ros1/examples/bridge_with_own_master.rs +++ b/zplugin-ros1/examples/bridge_with_own_master.rs @@ -12,6 +12,8 @@ // ZettaScale Zenoh Team, // +use std::future; + use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; #[async_std::main] @@ -26,10 +28,9 @@ async fn main() { #[allow(unused_variables)] let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) .await - .with_ros1_master() - .await; + .with_ros1_master(); println!(" OK!"); println!("Running bridge, press Ctrl+C to exit..."); - async_std::task::sleep(core::time::Duration::MAX).await; + future::pending::<()>().await; } diff --git a/zplugin-ros1/examples/ros1_pub.rs b/zplugin-ros1/examples/ros1_pub.rs index c831953..9f5cf8c 100644 --- a/zplugin-ros1/examples/ros1_pub.rs +++ b/zplugin-ros1/examples/ros1_pub.rs @@ -12,10 +12,7 @@ // ZettaScale Zenoh Team, // -use async_std; use zenoh_core::AsyncResolve; - -use rosrust; use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; #[async_std::main] @@ -30,8 +27,7 @@ async fn main() { #[allow(unused_variables)] let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) .await - .with_ros1_master() - .await; + .with_ros1_master(); println!(" OK!"); // create ROS1 node and publisher diff --git a/zplugin-ros1/examples/ros1_service.rs b/zplugin-ros1/examples/ros1_service.rs index 59dfde4..c4916d8 100644 --- a/zplugin-ros1/examples/ros1_service.rs +++ b/zplugin-ros1/examples/ros1_service.rs @@ -15,7 +15,6 @@ use zenoh::prelude::SplitBuffer; use zenoh_core::AsyncResolve; -use rosrust; use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; #[async_std::main] @@ -30,8 +29,7 @@ async fn main() { #[allow(unused_variables)] let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) .await - .with_ros1_master() - .await; + .with_ros1_master(); println!(" OK!"); // create ROS1 node and service diff --git a/zplugin-ros1/examples/ros1_sub.rs b/zplugin-ros1/examples/ros1_sub.rs index 613923d..83dd4f5 100644 --- a/zplugin-ros1/examples/ros1_sub.rs +++ b/zplugin-ros1/examples/ros1_sub.rs @@ -29,8 +29,7 @@ async fn main() { #[allow(unused_variables)] let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) .await - .with_ros1_master() - .await; + .with_ros1_master(); println!(" OK!"); // create ROS1 node and subscriber diff --git a/zplugin-ros1/src/lib.rs b/zplugin-ros1/src/lib.rs index 0c7341a..c500f61 100644 --- a/zplugin-ros1/src/lib.rs +++ b/zplugin-ros1/src/lib.rs @@ -42,7 +42,9 @@ impl Plugin for Ros1Plugin { // The first operation called by zenohd on the plugin fn start(name: &str, runtime: &Self::StartArgs) -> ZResult { let config = runtime.config.lock(); - let self_cfg = config.plugin(name).unwrap().as_object().unwrap(); + let self_cfg = config + .plugin(name).ok_or("No plugin in the config!")? + .as_object().ok_or("Unable to get cfg objet!")?; // run through the bridge's config options and fill them from plugins config let plugin_configuration_entries = Environment::env(); @@ -56,14 +58,13 @@ impl Plugin for Ros1Plugin { std::mem::drop(config); // return a RunningPlugin to zenohd - Ok(Box::new(RunningPlugin::new(name.clone(), runtime.clone()))) + Ok(Box::new(RunningPlugin::new(runtime)?)) } } // The RunningPlugin struct implementing the RunningPluginTrait trait struct RunningPlugin { - _name: String, _bridge: Option } impl RunningPluginTrait for RunningPlugin { @@ -74,7 +75,7 @@ impl RunningPluginTrait for RunningPlugin { bail!("Reconfiguration at runtime is not allowed!"); }) } - + // Function called on any query on admin space that matches this plugin's sub-part of the admin space. // Thus the plugin can reply its contribution to the global admin space of this zenohd. fn adminspace_getter<'a>( @@ -87,17 +88,16 @@ impl RunningPluginTrait for RunningPlugin { } impl RunningPlugin { - fn new(name: &str, runtime: Runtime) -> Self { - let bridge = async_std::task::block_on( async { + fn new(runtime: &Runtime) -> ZResult { + let bridge: ZResult = async_std::task::block_on( async { // create a zenoh Session that shares the same Runtime than zenohd - let session = zenoh::init(runtime.clone()).res().await.unwrap().into_arc(); - let bridge = ros_to_zenoh_bridge::Ros1ToZenohBridge::new_with_external_session(session).await; - return bridge; + let session = zenoh::init(runtime.clone()).res().await?.into_arc(); + let bridge = ros_to_zenoh_bridge::Ros1ToZenohBridge::new_with_external_session(session); + return Ok(bridge); }); - return Self { - _name: name.to_string(), - _bridge: Some(bridge) - }; + return Ok(Self { + _bridge: Some(bridge?) + }); } } \ No newline at end of file diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge.rs index 3dd43e2..58abefb 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge.rs @@ -12,15 +12,18 @@ // ZettaScale Zenoh Team, // -use async_std::{task::JoinHandle}; +use async_std::{task::JoinHandle, process::Child}; +use log::error; use zenoh_core::AsyncResolve; use zenoh; use std::{sync::{ Arc, atomic::{AtomicBool, Ordering::Relaxed} -}, process::Command}; +}}; + +use async_std::process::Command; use self::{ros1_to_zenoh_bridge_impl::work_cycle}; @@ -43,15 +46,15 @@ pub mod aloha_subscription; pub struct Ros1ToZenohBridge { flag: Arc, task_handle: Box>, - rosmaster: Option + rosmaster: Option } impl Ros1ToZenohBridge { pub async fn new_with_own_session(config: zenoh::config::Config) -> Self { let session = zenoh::open(config).res_async().await.unwrap().into_arc(); - return Self::new_with_external_session(session).await; + return Self::new_with_external_session(session); } - pub async fn new_with_external_session(session: Arc) -> Self { + pub fn new_with_external_session(session: Arc) -> Self { let flag = Arc::new(AtomicBool::new(true)); Self { flag: flag.clone(), @@ -60,33 +63,46 @@ impl Ros1ToZenohBridge { } } - pub async fn with_ros1_master(mut self) -> Self { + pub fn with_ros1_master(mut self) -> Self { assert!(self.rosmaster.is_none()); - - self.rosmaster = Some( - Command::new("rosmaster") - .stdout(std::process::Stdio::piped()) - .stderr(std::process::Stdio::piped()) - .spawn() - .unwrap(), - ); - - return self; + match Command::new("rosmaster") + .stdout(std::process::Stdio::piped()) + .stderr(std::process::Stdio::piped()) + .spawn() { + Ok(child) => { + self.rosmaster = Some(child); + }, + Err(e) => { + error!("Error starting rosmaster: {}", e); + }, + } + self } pub async fn without_ros1_master(mut self) -> Self { assert!(self.rosmaster.is_some()); - if self.rosmaster.is_some() { - self.rosmaster.take().unwrap().kill().unwrap(); - self.rosmaster = None; - } - else { - let mut kill_master = Command::new("killall").arg("rosmaster").spawn().unwrap(); - kill_master.wait().unwrap(); + match &mut self.rosmaster { + Some(child) => { + if child.kill().is_ok() { + if let Err(e) = child.status().await { + error!("Error stopping child rosmaster: {}", e); + } + } + self.rosmaster = None; + } + None => { + match Command::new("killall").arg("rosmaster").spawn() { + Ok(mut child) => { + if let Err(e) = child.status().await { + error!("Error stopping foreign rosmaster: {}", e); + } + }, + Err(e) => error!("Error executing killall command to stop foreign rosmaster: {}", e) + } + } } - - return self; + self } pub async fn async_await(&mut self) { diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs index f64ec86..62ba836 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs @@ -72,17 +72,17 @@ impl Bridges { struct Access<'a> { container: &'a mut HashMap, b_type: BridgeType, - ros1_client: &'a Arc, - zenoh_client: &'a Arc, - declaration_interface: &'a Arc + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc } impl<'a> Access<'a> { fn new(b_type: BridgeType, container: &'a mut HashMap, - ros1_client: &'a Arc, - zenoh_client: &'a Arc, - declaration_interface: &'a Arc) -> Self { + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc) -> Self { Self { container, b_type, @@ -100,9 +100,9 @@ pub struct ComplementaryElementAccessor<'a> { impl<'a> ComplementaryElementAccessor<'a> { fn new(b_type: BridgeType, container: &'a mut HashMap, - ros1_client: &'a Arc, - zenoh_client: &'a Arc, - declaration_interface: &'a Arc) -> Self { + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc) -> Self { Self { access: Access::new(b_type, container, ros1_client, zenoh_client, declaration_interface) } } @@ -146,9 +146,9 @@ pub struct ElementAccessor<'a> { impl<'a> ElementAccessor<'a> { fn new(b_type: BridgeType, container: &'a mut HashMap, - ros1_client: &'a Arc, - zenoh_client: &'a Arc, - declaration_interface: &'a Arc) -> Self { + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc) -> Self { Self { access: Access::new(b_type, container, ros1_client, zenoh_client, declaration_interface) } } @@ -193,10 +193,11 @@ impl<'a> ElementAccessor<'a> { pub struct TypeAccessor<'a> { - storage: &'a mut BridgesStorage + storage: &'a mut BridgesStorage, + _v: bool } impl<'a> TypeAccessor<'a> { - fn new(storage: &'a mut BridgesStorage) -> Self { Self { storage } } + fn new(storage: &'a mut BridgesStorage) -> Self { Self { storage, _v: false } } pub fn complementary_for(&'a mut self, b_type: BridgeType) -> ComplementaryElementAccessor<'a> { let b_type = match b_type { @@ -208,9 +209,9 @@ impl<'a> TypeAccessor<'a> { ComplementaryElementAccessor::new( b_type, self.storage.bridges.container_mut(b_type), - &self.storage.ros1_client, - &self.storage.zenoh_client, - &self.storage.declaration_interface + self.storage.ros1_client.clone(), + self.storage.zenoh_client.clone(), + self.storage.declaration_interface.clone() ) } @@ -218,9 +219,9 @@ impl<'a> TypeAccessor<'a> { ElementAccessor::new( b_type, self.storage.bridges.container_mut(b_type), - &self.storage.ros1_client, - &self.storage.zenoh_client, - &self.storage.declaration_interface + self.storage.ros1_client.clone(), + self.storage.zenoh_client.clone(), + self.storage.declaration_interface.clone() ) } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs index b594398..fc34599 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs @@ -25,23 +25,19 @@ impl<'a> Entry<'a> { where Tvar: ToString { - return Entry{name, default: default.to_string()}; + Entry{name, default: default.to_string()} } pub fn get(&self) -> Tvar where Tvar: FromStr + std::convert::From { - match std::env::var(self.name) { - Ok(val) => { - match val.parse::() { - Ok(val) => return val, - Err(..) => {} - }; + if let Ok(val) = std::env::var(self.name) { + if let Ok(val) = val.parse::() { + return val; } - Err(..) => {} - }; - return self.default.clone().into(); + } + self.default.clone().into() } pub fn set(&self, value: Tvar) @@ -53,7 +49,7 @@ impl<'a> Entry<'a> { } -pub struct Environment {} +pub struct Environment; impl Environment { pub fn ros_master_uri() -> Entry<'static> { return Entry::new("ROS_MASTER_URI", master()); diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 7b77182..16af627 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -702,7 +702,7 @@ impl TestEnvironment { } pub fn many_count() -> u32 { - return Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 40); + return Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 10); } pub fn pps_measurements() -> u32 { From 990a27ee444773f7d44adeae13f01d0bdeb997f8 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 14 Apr 2023 12:30:05 +0300 Subject: [PATCH 04/19] Used autoformating Cargo fmt --all --- .../examples/bridge_with_own_master.rs | 2 +- zplugin-ros1/examples/ros1_pub.rs | 6 +- zplugin-ros1/examples/ros1_service.rs | 21 +- zplugin-ros1/examples/ros1_sub.rs | 6 +- zplugin-ros1/src/lib.rs | 33 +- zplugin-ros1/src/ros_to_zenoh_bridge.rs | 72 ++- .../ros_to_zenoh_bridge/abstract_bridge.rs | 342 +++++++----- .../ros_to_zenoh_bridge/aloha_declaration.rs | 109 ++-- .../ros_to_zenoh_bridge/aloha_subscription.rs | 214 +++++--- .../src/ros_to_zenoh_bridge/bridge_type.rs | 4 +- .../ros_to_zenoh_bridge/bridges_storage.rs | 190 ++++--- .../src/ros_to_zenoh_bridge/discovery.rs | 333 ++++++++---- .../src/ros_to_zenoh_bridge/environment.rs | 23 +- .../src/ros_to_zenoh_bridge/ros1_client.rs | 52 +- .../ros1_to_zenoh_bridge_impl.rs | 150 +++--- .../src/ros_to_zenoh_bridge/topic_bridge.rs | 67 ++- .../src/ros_to_zenoh_bridge/topic_mapping.rs | 49 +- .../ros_to_zenoh_bridge/topic_utilities.rs | 17 +- .../src/ros_to_zenoh_bridge/zenoh_client.rs | 129 +++-- zplugin-ros1/tests/aloha_declaration_test.rs | 260 +++++---- zplugin-ros1/tests/discovery_test.rs | 326 ++++++----- zplugin-ros1/tests/ping_pong_test.rs | 509 ++++++++++++------ 22 files changed, 1818 insertions(+), 1096 deletions(-) diff --git a/zplugin-ros1/examples/bridge_with_own_master.rs b/zplugin-ros1/examples/bridge_with_own_master.rs index 676ccf3..23667e3 100644 --- a/zplugin-ros1/examples/bridge_with_own_master.rs +++ b/zplugin-ros1/examples/bridge_with_own_master.rs @@ -23,7 +23,7 @@ async fn main() { // create bridge with ROS1 master // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. - // In this example the bridge will start ROS1 master by itself. + // In this example the bridge will start ROS1 master by itself. print!("Starting Bridge..."); #[allow(unused_variables)] let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) diff --git a/zplugin-ros1/examples/ros1_pub.rs b/zplugin-ros1/examples/ros1_pub.rs index 9f5cf8c..69c06d3 100644 --- a/zplugin-ros1/examples/ros1_pub.rs +++ b/zplugin-ros1/examples/ros1_pub.rs @@ -22,7 +22,7 @@ async fn main() { // create bridge with ROS1 master // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. - // In this example the bridge will start ROS1 master by itself. + // In this example the bridge will start ROS1 master by itself. print!("Starting Bridge..."); #[allow(unused_variables)] let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) @@ -66,7 +66,9 @@ async fn main() { let data: Vec = (0..10).collect(); loop { println!("ROS Publisher: publishing data..."); - ros1_publisher.send(rosrust::RawMessage(data.clone())).unwrap(); + ros1_publisher + .send(rosrust::RawMessage(data.clone())) + .unwrap(); std::thread::sleep(core::time::Duration::from_secs(1)); } }; diff --git a/zplugin-ros1/examples/ros1_service.rs b/zplugin-ros1/examples/ros1_service.rs index c4916d8..8026c39 100644 --- a/zplugin-ros1/examples/ros1_service.rs +++ b/zplugin-ros1/examples/ros1_service.rs @@ -24,7 +24,7 @@ async fn main() { // create bridge with ROS1 master // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. - // In this example the bridge will start ROS1 master by itself. + // In this example the bridge will start ROS1 master by itself. print!("Starting Bridge..."); #[allow(unused_variables)] let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) @@ -39,7 +39,13 @@ async fn main() { print!("Creating ROS1 Service..."); #[allow(unused_variables)] let ros1_service = ros1_node - .service::("/some/ros/topic/", |query| -> rosrust::ServiceResult {println!("ROS Service: got query, sending reply..."); Ok(query) }) + .service::( + "/some/ros/topic/", + |query| -> rosrust::ServiceResult { + println!("ROS Service: got query, sending reply..."); + Ok(query) + }, + ) .unwrap(); println!(" OK!"); @@ -58,13 +64,18 @@ async fn main() { // run test loop querying Zenoh... let data: Vec = (0..10).collect(); loop { - println!("Zenoh: sending query..."); - let reply = zenoh_session.get("some/ros/topic").with_value(data.clone()).res_async().await.unwrap(); + println!("Zenoh: sending query..."); + let reply = zenoh_session + .get("some/ros/topic") + .with_value(data.clone()) + .res_async() + .await + .unwrap(); let result = reply.recv_async().await; match result { Ok(val) => { println!("Zenoh: got reply!"); - assert!(data == val.sample.unwrap().value.payload.contiguous().to_vec() ); + assert!(data == val.sample.unwrap().value.payload.contiguous().to_vec()); } Err(_) => {} } diff --git a/zplugin-ros1/examples/ros1_sub.rs b/zplugin-ros1/examples/ros1_sub.rs index 83dd4f5..7a927b6 100644 --- a/zplugin-ros1/examples/ros1_sub.rs +++ b/zplugin-ros1/examples/ros1_sub.rs @@ -24,7 +24,7 @@ async fn main() { // create bridge with ROS1 master // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. - // In this example the bridge will start ROS1 master by itself. + // In this example the bridge will start ROS1 master by itself. print!("Starting Bridge..."); #[allow(unused_variables)] let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) @@ -39,7 +39,9 @@ async fn main() { print!("Creating ROS1 Subscriber..."); #[allow(unused_variables)] let ros1_subscriber = ros1_node - .subscribe("/some/ros/topic/", 0, |msg: rosrust::RawMessage| {println!("ROS Subscriber: got message!")}) + .subscribe("/some/ros/topic/", 0, |msg: rosrust::RawMessage| { + println!("ROS Subscriber: got message!") + }) .unwrap(); println!(" OK!"); diff --git a/zplugin-ros1/src/lib.rs b/zplugin-ros1/src/lib.rs index c500f61..8bfe453 100644 --- a/zplugin-ros1/src/lib.rs +++ b/zplugin-ros1/src/lib.rs @@ -13,11 +13,9 @@ // #![recursion_limit = "1024"] -use ros_to_zenoh_bridge::Ros1ToZenohBridge; use ros_to_zenoh_bridge::environment::Environment; -use std::sync::{ - Arc -}; +use ros_to_zenoh_bridge::Ros1ToZenohBridge; +use std::sync::Arc; use zenoh::plugins::{Plugin, RunningPluginTrait, ValidationFunction, ZenohPlugin}; use zenoh::prelude::r#async::*; use zenoh::runtime::Runtime; @@ -43,18 +41,22 @@ impl Plugin for Ros1Plugin { fn start(name: &str, runtime: &Self::StartArgs) -> ZResult { let config = runtime.config.lock(); let self_cfg = config - .plugin(name).ok_or("No plugin in the config!")? - .as_object().ok_or("Unable to get cfg objet!")?; + .plugin(name) + .ok_or("No plugin in the config!")? + .as_object() + .ok_or("Unable to get cfg objet!")?; // run through the bridge's config options and fill them from plugins config let plugin_configuration_entries = Environment::env(); for entry in plugin_configuration_entries.iter() { match self_cfg.get(entry.name) { - Some(v) => { entry.set(v); }, - None => {}, + Some(v) => { + entry.set(v); + } + None => {} } } - + std::mem::drop(config); // return a RunningPlugin to zenohd @@ -62,10 +64,9 @@ impl Plugin for Ros1Plugin { } } - // The RunningPlugin struct implementing the RunningPluginTrait trait struct RunningPlugin { - _bridge: Option + _bridge: Option, } impl RunningPluginTrait for RunningPlugin { // Operation returning a ValidationFunction(path, old, new)-> ZResult>> @@ -75,7 +76,7 @@ impl RunningPluginTrait for RunningPlugin { bail!("Reconfiguration at runtime is not allowed!"); }) } - + // Function called on any query on admin space that matches this plugin's sub-part of the admin space. // Thus the plugin can reply its contribution to the global admin space of this zenohd. fn adminspace_getter<'a>( @@ -89,15 +90,15 @@ impl RunningPluginTrait for RunningPlugin { impl RunningPlugin { fn new(runtime: &Runtime) -> ZResult { - let bridge: ZResult = async_std::task::block_on( async { + let bridge: ZResult = async_std::task::block_on(async { // create a zenoh Session that shares the same Runtime than zenohd let session = zenoh::init(runtime.clone()).res().await?.into_arc(); let bridge = ros_to_zenoh_bridge::Ros1ToZenohBridge::new_with_external_session(session); return Ok(bridge); }); - + return Ok(Self { - _bridge: Some(bridge?) + _bridge: Some(bridge?), }); } -} \ No newline at end of file +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge.rs index 58abefb..b224145 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge.rs @@ -12,41 +12,41 @@ // ZettaScale Zenoh Team, // -use async_std::{task::JoinHandle, process::Child}; +use async_std::{process::Child, task::JoinHandle}; use log::error; -use zenoh_core::AsyncResolve; use zenoh; +use zenoh_core::AsyncResolve; -use std::{sync::{ +use std::sync::{ + atomic::{AtomicBool, Ordering::Relaxed}, Arc, - atomic::{AtomicBool, Ordering::Relaxed} -}}; +}; use async_std::process::Command; -use self::{ros1_to_zenoh_bridge_impl::work_cycle}; +use self::ros1_to_zenoh_bridge_impl::work_cycle; -pub mod zenoh_client; -pub mod ros1_client; -pub mod discovery; -pub mod topic_bridge; pub mod abstract_bridge; pub mod bridge_type; +pub mod discovery; +pub mod ros1_client; +pub mod topic_bridge; +pub mod zenoh_client; -mod topic_utilities; -mod topic_mapping; mod bridges_storage; +mod topic_mapping; +mod topic_utilities; -pub mod environment; -pub mod ros1_to_zenoh_bridge_impl; pub mod aloha_declaration; pub mod aloha_subscription; +pub mod environment; +pub mod ros1_to_zenoh_bridge_impl; pub struct Ros1ToZenohBridge { flag: Arc, task_handle: Box>, - rosmaster: Option + rosmaster: Option, } impl Ros1ToZenohBridge { pub async fn new_with_own_session(config: zenoh::config::Config) -> Self { @@ -59,7 +59,7 @@ impl Ros1ToZenohBridge { Self { flag: flag.clone(), task_handle: Box::new(async_std::task::spawn(Self::run(session, flag))), - rosmaster: None + rosmaster: None, } } @@ -68,13 +68,14 @@ impl Ros1ToZenohBridge { match Command::new("rosmaster") .stdout(std::process::Stdio::piped()) .stderr(std::process::Stdio::piped()) - .spawn() { + .spawn() + { Ok(child) => { self.rosmaster = Some(child); - }, + } Err(e) => { error!("Error starting rosmaster: {}", e); - }, + } } self } @@ -91,16 +92,17 @@ impl Ros1ToZenohBridge { } self.rosmaster = None; } - None => { - match Command::new("killall").arg("rosmaster").spawn() { - Ok(mut child) => { - if let Err(e) = child.status().await { - error!("Error stopping foreign rosmaster: {}", e); - } - }, - Err(e) => error!("Error executing killall command to stop foreign rosmaster: {}", e) + None => match Command::new("killall").arg("rosmaster").spawn() { + Ok(mut child) => { + if let Err(e) = child.status().await { + error!("Error stopping foreign rosmaster: {}", e); + } } - } + Err(e) => error!( + "Error executing killall command to stop foreign rosmaster: {}", + e + ), + }, } self } @@ -114,17 +116,8 @@ impl Ros1ToZenohBridge { self.async_await().await; } - pub async fn run( - session: Arc, - flag: Arc - ) { - work_cycle( - session, - flag, - |_v| {}, - |_status| {} - ) - .await; + pub async fn run(session: Arc, flag: Arc) { + work_cycle(session, flag, |_v| {}, |_status| {}).await; } } impl Drop for Ros1ToZenohBridge { @@ -132,4 +125,3 @@ impl Drop for Ros1ToZenohBridge { self.flag.store(false, Relaxed); } } - diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs index 4c3152d..dae6569 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs @@ -14,31 +14,41 @@ use std::sync::Arc; -use log::{info, debug, error}; +use log::{debug, error, info}; use zenoh::{plugins::ZResult, prelude::SplitBuffer}; -use zenoh_core::{SyncResolve, AsyncResolve}; +use zenoh_core::{AsyncResolve, SyncResolve}; -use super::{ros1_client, zenoh_client, topic_utilities::make_zenoh_key, bridge_type::BridgeType}; +use super::{bridge_type::BridgeType, ros1_client, topic_utilities::make_zenoh_key, zenoh_client}; pub struct AbstractBridge { - _impl: BridgeIml + _impl: BridgeIml, } impl AbstractBridge { - pub async fn new(b_type: BridgeType, - topic: &rosrust::api::Topic, - ros1_client: &ros1_client::Ros1Client, - zenoh_client: &Arc) -> ZResult { - let _impl = { + pub async fn new( + b_type: BridgeType, + topic: &rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: &Arc, + ) -> ZResult { + let _impl = { match b_type { - BridgeType::Publisher => {BridgeIml::Pub(Ros1ToZenoh::new(topic, ros1_client, zenoh_client).await?)}, - BridgeType::Subscriber => {BridgeIml::Sub(ZenohToRos1::new(topic, ros1_client, zenoh_client).await?)}, - BridgeType::Service => {BridgeIml::Service(Ros1ToZenohService::new(topic, ros1_client, zenoh_client).await?)}, - BridgeType::Client => {BridgeIml::Client(Ros1ToZenohClient::new(topic, ros1_client, zenoh_client.clone()).await?)}, + BridgeType::Publisher => { + BridgeIml::Pub(Ros1ToZenoh::new(topic, ros1_client, zenoh_client).await?) + } + BridgeType::Subscriber => { + BridgeIml::Sub(ZenohToRos1::new(topic, ros1_client, zenoh_client).await?) + } + BridgeType::Service => BridgeIml::Service( + Ros1ToZenohService::new(topic, ros1_client, zenoh_client).await?, + ), + BridgeType::Client => BridgeIml::Client( + Ros1ToZenohClient::new(topic, ros1_client, zenoh_client.clone()).await?, + ), } }; - return Ok(Self {_impl}); + return Ok(Self { _impl }); } } @@ -46,27 +56,32 @@ enum BridgeIml { Client(Ros1ToZenohClient), Service(Ros1ToZenohService), Pub(Ros1ToZenoh), - Sub(ZenohToRos1) + Sub(ZenohToRos1), } struct Ros1ToZenohClient { - _service: rosrust::Service + _service: rosrust::Service, } impl Ros1ToZenohClient { - async fn new(topic: &rosrust::api::Topic, - ros1_client: &ros1_client::Ros1Client, - zenoh_client: Arc)-> ZResult { - info!("Creating ROS1 -> Zenoh Client bridge for topic {}, datatype {}", topic.name, topic.datatype); + async fn new( + topic: &rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: Arc, + ) -> ZResult { + info!( + "Creating ROS1 -> Zenoh Client bridge for topic {}, datatype {}", + topic.name, topic.datatype + ); let zenoh_key = make_zenoh_key(&topic).to_string(); - match ros1_client.service::(topic, - move |q| -> rosrust::ServiceResult { - return Self::on_query(&zenoh_key, q, zenoh_client.as_ref()); - }) { + match ros1_client.service::( + topic, + move |q| -> rosrust::ServiceResult { + return Self::on_query(&zenoh_key, q, zenoh_client.as_ref()); + }, + ) { Ok(service) => { - return Ok(Ros1ToZenohClient{ - _service: service - }); + return Ok(Ros1ToZenohClient { _service: service }); } Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) @@ -74,38 +89,51 @@ impl Ros1ToZenohClient { } } -//PRIVATE: - fn on_query(key: &str, query: rosrust::RawMessage, zenoh_client: & zenoh_client::ZenohClient) -> rosrust::ServiceResult { + //PRIVATE: + fn on_query( + key: &str, + query: rosrust::RawMessage, + zenoh_client: &zenoh_client::ZenohClient, + ) -> rosrust::ServiceResult { return async_std::task::block_on(Self::do_zenoh_query(key, query, zenoh_client)); } - async fn do_zenoh_query(key: &str, query: rosrust::RawMessage, zenoh_client: & zenoh_client::ZenohClient) -> rosrust::ServiceResult { + async fn do_zenoh_query( + key: &str, + query: rosrust::RawMessage, + zenoh_client: &zenoh_client::ZenohClient, + ) -> rosrust::ServiceResult { match zenoh_client.make_query_sync(key, query.0).await { - Ok(reply) => { - match reply.recv_async().await { - Ok(r) => { - match r.sample { - Ok(value) => { - let data = value.payload.contiguous().to_vec(); - debug!("Zenoh -> ROS1: sending {} bytes!", data.len()); - return Ok(rosrust::RawMessage(data)); - } - Err(e) => { - error!("ROS1 -> Zenoh Client: received Zenoh Query with error: {}", e); - let error = e.to_string(); - return Err(error); - } - } + Ok(reply) => match reply.recv_async().await { + Ok(r) => match r.sample { + Ok(value) => { + let data = value.payload.contiguous().to_vec(); + debug!("Zenoh -> ROS1: sending {} bytes!", data.len()); + return Ok(rosrust::RawMessage(data)); } Err(e) => { - error!("ROS1 -> Zenoh Client: error while receiving reply to Zenoh Query: {}", e); + error!( + "ROS1 -> Zenoh Client: received Zenoh Query with error: {}", + e + ); let error = e.to_string(); return Err(error); } + }, + Err(e) => { + error!( + "ROS1 -> Zenoh Client: error while receiving reply to Zenoh Query: {}", + e + ); + let error = e.to_string(); + return Err(error); } - } + }, Err(e) => { - error!("ROS1 -> Zenoh Client: error while creating Zenoh Query: {}", e); + error!( + "ROS1 -> Zenoh Client: error while creating Zenoh Query: {}", + e + ); let error = e.to_string(); return Err(error); } @@ -113,37 +141,51 @@ impl Ros1ToZenohClient { } } - struct Ros1ToZenohService { - _queryable: zenoh::queryable::Queryable<'static, ()> + _queryable: zenoh::queryable::Queryable<'static, ()>, } impl Ros1ToZenohService { - async fn new<'b>(topic: & rosrust::api::Topic, - ros1_client: &ros1_client::Ros1Client, - zenoh_client: &'b zenoh_client::ZenohClient)-> ZResult { - info!("Creating ROS1 -> Zenoh Service bridge for topic {}, datatype {}", topic.name, topic.datatype); + async fn new<'b>( + topic: &rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: &'b zenoh_client::ZenohClient, + ) -> ZResult { + info!( + "Creating ROS1 -> Zenoh Service bridge for topic {}, datatype {}", + topic.name, topic.datatype + ); match ros1_client.client(topic) { Ok(client) => { let client_in_arc = Arc::new(client); - let queryable = zenoh_client.make_queryable(make_zenoh_key(topic), - move |query| - { - async_std::task::spawn(Self::on_query(client_in_arc.clone(), query)); - }).await?; - return Ok(Ros1ToZenohService { _queryable: queryable }); + let queryable = zenoh_client + .make_queryable(make_zenoh_key(topic), move |query| { + async_std::task::spawn(Self::on_query(client_in_arc.clone(), query)); + }) + .await?; + return Ok(Ros1ToZenohService { + _queryable: queryable, + }); + } + Err(e) => { + zenoh_core::bail!("Ros error: {}", e.to_string()) } - Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) } } } -//PRIVATE: - async fn on_query(ros1_client: Arc>, query: zenoh::queryable::Query) { + //PRIVATE: + async fn on_query( + ros1_client: Arc>, + query: zenoh::queryable::Query, + ) { match query.value() { - Some(val) => { + Some(val) => { let payload = val.payload.contiguous().to_vec(); - debug!("ROS1 -> Zenoh Service: got query of {} bytes!", payload.len()); - Self::process_query(ros1_client, query, payload).await; + debug!( + "ROS1 -> Zenoh Service: got query of {} bytes!", + payload.len() + ); + Self::process_query(ros1_client, query, payload).await; } None => { error!("ROS1 -> Zenoh Service: got query without value!"); @@ -151,108 +193,150 @@ impl Ros1ToZenohService { } } - async fn process_query(ros1_client: Arc>, query: zenoh::queryable::Query, payload: Vec) { + async fn process_query( + ros1_client: Arc>, + query: zenoh::queryable::Query, + payload: Vec, + ) { // rosrust is synchronous, so we will use spawn_blocking. If there will be an async mode some day for the rosrust, // than reply_to_query can be refactored to async very easily let res = async_std::task::spawn_blocking(move || { return ros1_client.req(&rosrust::RawMessage(payload)); - }).await; + }) + .await; match Self::reply_to_query(res, &query).await { - Ok(_) => {}, + Ok(_) => {} Err(e) => { - error!("ROS1 -> Zenoh Service: error replying to query on {}: {}", query.key_expr(), e); + error!( + "ROS1 -> Zenoh Service: error replying to query on {}: {}", + query.key_expr(), + e + ); } } } - async fn reply_to_query(res: rosrust::error::tcpros::Result>, - query: &zenoh::queryable::Query) -> ZResult<()> { + async fn reply_to_query( + res: rosrust::error::tcpros::Result>, + query: &zenoh::queryable::Query, + ) -> ZResult<()> { match res { - Ok(reply) => { - match reply { - Ok(reply_message) => { - debug!("ROS1 -> Zenoh Service: got reply of {} bytes!", reply_message.0.len()); - query.reply(Ok(zenoh::prelude::Sample::new(query.key_expr().clone(), reply_message.0))).res_async().await?; - } - Err(e) => { - error!("ROS1 -> Zenoh Service: got reply from ROS1 Service with error: {}", e); - query.reply(Err(zenoh::prelude::Value::from(e))).res_async().await?; - } + Ok(reply) => match reply { + Ok(reply_message) => { + debug!( + "ROS1 -> Zenoh Service: got reply of {} bytes!", + reply_message.0.len() + ); + query + .reply(Ok(zenoh::prelude::Sample::new( + query.key_expr().clone(), + reply_message.0, + ))) + .res_async() + .await?; } - } + Err(e) => { + error!( + "ROS1 -> Zenoh Service: got reply from ROS1 Service with error: {}", + e + ); + query + .reply(Err(zenoh::prelude::Value::from(e))) + .res_async() + .await?; + } + }, Err(e) => { - error!("ROS1 -> Zenoh Service: error while sending request to ROS1 Service: {}", e); + error!( + "ROS1 -> Zenoh Service: error while sending request to ROS1 Service: {}", + e + ); let error = e.to_string(); - query.reply(Err(zenoh::prelude::Value::from(error))).res_async().await?; + query + .reply(Err(zenoh::prelude::Value::from(error))) + .res_async() + .await?; } } return Ok(()); } } - - struct Ros1ToZenoh { - _subscriber: rosrust::Subscriber + _subscriber: rosrust::Subscriber, } impl Ros1ToZenoh { - async fn new<'b>(topic: & rosrust::api::Topic, - ros1_client: &ros1_client::Ros1Client, - zenoh_client: &'b zenoh_client::ZenohClient)-> ZResult { - info!("Creating ROS1 -> Zenoh bridge for topic {}, datatype {}", topic.name, topic.datatype); + async fn new<'b>( + topic: &rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: &'b zenoh_client::ZenohClient, + ) -> ZResult { + info!( + "Creating ROS1 -> Zenoh bridge for topic {}, datatype {}", + topic.name, topic.datatype + ); let publisher = zenoh_client.publish(make_zenoh_key(topic)).await?; - match ros1_client.subscribe(topic, - move |msg: rosrust::RawMessage| - { - debug!("ROS1 -> Zenoh: sending {} bytes!", msg.0.len()); - match publisher.put(msg.0).res_sync() { - Ok(_) => {}, - Err(e) => { - error!("ROS1 -> Zenoh: error publishing: {}", e); - } - } - }) - { + match ros1_client.subscribe(topic, move |msg: rosrust::RawMessage| { + debug!("ROS1 -> Zenoh: sending {} bytes!", msg.0.len()); + match publisher.put(msg.0).res_sync() { + Ok(_) => {} + Err(e) => { + error!("ROS1 -> Zenoh: error publishing: {}", e); + } + } + }) { Ok(subscriber) => { - return Ok(Ros1ToZenoh { _subscriber: subscriber }); + return Ok(Ros1ToZenoh { + _subscriber: subscriber, + }); + } + Err(e) => { + zenoh_core::bail!("Ros error: {}", e.to_string()) } - Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) } } } } - struct ZenohToRos1 { - _subscriber: zenoh::subscriber::Subscriber<'static, ()> + _subscriber: zenoh::subscriber::Subscriber<'static, ()>, } impl ZenohToRos1 { - async fn new(topic: & rosrust::api::Topic, - ros1_client: & ros1_client::Ros1Client, - zenoh_client: &Arc)-> ZResult { - info!("Creating Zenoh -> ROS1 bridge for topic {}, datatype {}", topic.name, topic.datatype); + async fn new( + topic: &rosrust::api::Topic, + ros1_client: &ros1_client::Ros1Client, + zenoh_client: &Arc, + ) -> ZResult { + info!( + "Creating Zenoh -> ROS1 bridge for topic {}, datatype {}", + topic.name, topic.datatype + ); match ros1_client.publish(topic) { Ok(publisher) => { let publisher_in_arc = Arc::new(publisher); - let subscriber = zenoh_client.subscribe( - make_zenoh_key(topic), - move |sample| { - let publisher_in_arc_cloned = publisher_in_arc.clone(); - async_std::task::spawn_blocking(move || { - let data = sample.value.payload.contiguous().to_vec(); - debug!("Zenoh -> ROS1: sending {} bytes!", data.len()); - match publisher_in_arc_cloned.send(rosrust::RawMessage(data)) { - Ok(_) => {}, - Err(e) => { - error!("Zenoh -> ROS1: error publishing: {}", e); - } - } - }); - }).await?; - return Ok(ZenohToRos1 { _subscriber: subscriber}) + let subscriber = zenoh_client + .subscribe(make_zenoh_key(topic), move |sample| { + let publisher_in_arc_cloned = publisher_in_arc.clone(); + async_std::task::spawn_blocking(move || { + let data = sample.value.payload.contiguous().to_vec(); + debug!("Zenoh -> ROS1: sending {} bytes!", data.len()); + match publisher_in_arc_cloned.send(rosrust::RawMessage(data)) { + Ok(_) => {} + Err(e) => { + error!("Zenoh -> ROS1: error publishing: {}", e); + } + } + }); + }) + .await?; + return Ok(ZenohToRos1 { + _subscriber: subscriber, + }); + } + Err(e) => { + zenoh_core::bail!("Ros error: {}", e.to_string()) } - Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) } } } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs index 82923ab..76a1431 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs @@ -14,58 +14,64 @@ use zenoh::buffers::ZBuf; +use std::sync::atomic::{AtomicBool, AtomicUsize}; use std::sync::Arc; -use std::sync::atomic::{AtomicUsize, AtomicBool}; use std::time::Duration; -use zenoh::{prelude::r#async::*}; +use zenoh::prelude::r#async::*; use zenoh::Session; pub struct AlohaDeclaration { - monitor_running: Arc + monitor_running: Arc, } impl Drop for AlohaDeclaration { fn drop(&mut self) { - self.monitor_running.store(false, std::sync::atomic::Ordering::Relaxed); + self.monitor_running + .store(false, std::sync::atomic::Ordering::Relaxed); } } impl AlohaDeclaration { - pub fn new(session: Arc, - key: OwnedKeyExpr, - beacon_period: Duration) -> Self - { + pub fn new(session: Arc, key: OwnedKeyExpr, beacon_period: Duration) -> Self { let monitor_running = Arc::new(AtomicBool::new(true)); async_std::task::spawn(Self::aloha_monitor_task( - beacon_period, - monitor_running.clone(), + beacon_period, + monitor_running.clone(), key, - session)); - return Self {monitor_running}; + session, + )); + return Self { monitor_running }; } -//PRIVATE: - async fn aloha_monitor_task(beacon_period: Duration, - monitor_running: Arc, - key: OwnedKeyExpr, - session: Arc) - { + //PRIVATE: + async fn aloha_monitor_task( + beacon_period: Duration, + monitor_running: Arc, + key: OwnedKeyExpr, + session: Arc, + ) { let beacon_task_flag = Arc::new(AtomicBool::new(false)); let remote_beacons = Arc::new(AtomicUsize::new(0)); let rb = remote_beacons.clone(); - let _beacon_listener = session.declare_subscriber(key.clone()) + let _beacon_listener = session + .declare_subscriber(key.clone()) .allowed_origin(Locality::Remote) .reliability(Reliability::BestEffort) .callback(move |_| { rb.fetch_add(1, std::sync::atomic::Ordering::SeqCst); }) - .res_async().await.unwrap(); + .res_async() + .await + .unwrap(); let mut sending_beacons = true; - Self::start_beacon_task(beacon_period.clone(), - key.clone(), - session.clone(), - beacon_task_flag.clone()).await; + Self::start_beacon_task( + beacon_period.clone(), + key.clone(), + session.clone(), + beacon_task_flag.clone(), + ) + .await; while monitor_running.load(std::sync::atomic::Ordering::Relaxed) { async_std::task::sleep(beacon_period).await; @@ -75,12 +81,18 @@ impl AlohaDeclaration { // start publisher in ALOHA style... let period_ns = beacon_period.as_nanos(); let aloha_wait: u128 = rand::random::() % period_ns; - async_std::task::sleep(Duration::from_nanos(aloha_wait.try_into().unwrap())).await; + async_std::task::sleep(Duration::from_nanos( + aloha_wait.try_into().unwrap(), + )) + .await; if remote_beacons.load(std::sync::atomic::Ordering::SeqCst) == 0 { - Self::start_beacon_task(beacon_period.clone(), - key.clone(), - session.clone(), - beacon_task_flag.clone()).await; + Self::start_beacon_task( + beacon_period.clone(), + key.clone(), + session.clone(), + beacon_task_flag.clone(), + ) + .await; sending_beacons = true; } } @@ -98,27 +110,46 @@ impl AlohaDeclaration { Self::stop_beacon_task(beacon_task_flag).await; } - async fn start_beacon_task(beacon_period: Duration, key: OwnedKeyExpr, session: Arc, running: Arc) { + async fn start_beacon_task( + beacon_period: Duration, + key: OwnedKeyExpr, + session: Arc, + running: Arc, + ) { running.store(true, std::sync::atomic::Ordering::SeqCst); - async_std::task::spawn(Self::aloha_publishing_task(beacon_period, key, session, running)); + async_std::task::spawn(Self::aloha_publishing_task( + beacon_period, + key, + session, + running, + )); } - async fn stop_beacon_task(running: Arc) { + async fn stop_beacon_task(running: Arc) { running.store(false, std::sync::atomic::Ordering::Relaxed); } - async fn aloha_publishing_task(beacon_period: Duration, key: OwnedKeyExpr, session: Arc, running: Arc) { - let publisher = session.declare_publisher(key) + async fn aloha_publishing_task( + beacon_period: Duration, + key: OwnedKeyExpr, + session: Arc, + running: Arc, + ) { + let publisher = session + .declare_publisher(key) .allowed_destination(Locality::Remote) .congestion_control(CongestionControl::Drop) .priority(Priority::Background) - .res_async().await.unwrap(); + .res_async() + .await + .unwrap(); while running.load(std::sync::atomic::Ordering::Relaxed) { - let _res = - publisher.put(zenoh::value::Value::new(ZBuf::default())).res_async().await; + let _res = publisher + .put(zenoh::value::Value::new(ZBuf::default())) + .res_async() + .await; async_std::task::sleep(beacon_period).await; } } - -} \ No newline at end of file +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs index dfe2a9d..387127f 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs @@ -12,22 +12,29 @@ // ZettaScale Zenoh Team, // -use std::{sync::{Arc, atomic::{AtomicBool, Ordering::Relaxed}}, time::Duration, collections::{BTreeMap, btree_map::Entry::*}, cell::Cell}; +use std::{ + cell::Cell, + collections::{btree_map::Entry::*, BTreeMap}, + sync::{ + atomic::{AtomicBool, Ordering::Relaxed}, + Arc, + }, + time::Duration, +}; use flume::Receiver; -use futures::{Future, FutureExt, select, pin_mut}; +use futures::{pin_mut, select, Future, FutureExt}; use log::error; -use zenoh::{prelude::r#async::*, plugins::ZResult}; - +use zenoh::{plugins::ZResult, prelude::r#async::*}; struct AlohaResource { - activity: AtomicBool + activity: AtomicBool, } impl AlohaResource { - fn new() -> Self { - Self { - activity: AtomicBool::new(true) - } + fn new() -> Self { + Self { + activity: AtomicBool::new(true), + } } pub fn update(&mut self) { @@ -44,7 +51,7 @@ impl AlohaResource { } pub struct AlohaSubscription { - task_running: Arc + task_running: Arc, } impl Drop for AlohaSubscription { @@ -53,63 +60,88 @@ impl Drop for AlohaSubscription { } } impl AlohaSubscription { - pub async fn new(session: Arc, - key: OwnedKeyExpr, - beacon_period: Duration, - on_resource_declared: F, - on_resource_undeclared: F) -> ZResult + pub async fn new( + session: Arc, + key: OwnedKeyExpr, + beacon_period: Duration, + on_resource_declared: F, + on_resource_undeclared: F, + ) -> ZResult where - F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn( + zenoh::key_expr::KeyExpr, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { let task_running = Arc::new(AtomicBool::new(true)); async_std::task::spawn(AlohaSubscription::task( - task_running.clone(), + task_running.clone(), key, beacon_period, session, on_resource_declared, - on_resource_undeclared + on_resource_undeclared, )); - - return Ok(Self{task_running}); + + return Ok(Self { task_running }); } -//PRIVATE: - async fn task(task_running: Arc, - key: OwnedKeyExpr, - beacon_period: Duration, - session: Arc, - on_resource_declared: F, - on_resource_undeclared: F) -> ZResult<()> + //PRIVATE: + async fn task( + task_running: Arc, + key: OwnedKeyExpr, + beacon_period: Duration, + session: Arc, + on_resource_declared: F, + on_resource_undeclared: F, + ) -> ZResult<()> where - F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn( + zenoh::key_expr::KeyExpr, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { let mut accumulating_resources = Cell::new(BTreeMap::::new()); let subscriber = session.declare_subscriber(key).res_async().await?; Self::accumulating_task( - task_running, - beacon_period*3, - &mut accumulating_resources, + task_running, + beacon_period * 3, + &mut accumulating_resources, &subscriber, on_resource_declared, - on_resource_undeclared).await; + on_resource_undeclared, + ) + .await; return Ok(()); } - async fn listening_task<'a, F>(task_running: Arc, - accumulating_resources: &mut Cell>, - subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver> , - on_resource_declared: &F) - where - F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + async fn listening_task<'a, F>( + task_running: Arc, + accumulating_resources: &mut Cell>, + subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver>, + on_resource_declared: &F, + ) where + F: Fn( + zenoh::key_expr::KeyExpr, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { while task_running.load(Relaxed) { match subscriber.recv_async().await { Ok(val) => { - match accumulating_resources.get_mut().entry(val.key_expr.to_string()) { + match accumulating_resources + .get_mut() + .entry(val.key_expr.to_string()) + { Occupied(mut val) => { val.get_mut().update(); } @@ -126,14 +158,20 @@ impl AlohaSubscription { } } - async fn accumulating_task<'a, F>(task_running: Arc, - accumulate_period: Duration, - accumulating_resources: &mut Cell>, - subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver> , - on_resource_declared: F, - on_resource_undeclared: F) - where - F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + async fn accumulating_task<'a, F>( + task_running: Arc, + accumulate_period: Duration, + accumulating_resources: &mut Cell>, + subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver>, + on_resource_declared: F, + on_resource_undeclared: F, + ) where + F: Fn( + zenoh::key_expr::KeyExpr, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { while task_running.load(Relaxed) { accumulating_resources.get_mut().iter_mut().for_each(|val| { @@ -141,12 +179,13 @@ impl AlohaSubscription { }); { - let listen = - Self::listening_task( + let listen = Self::listening_task( task_running.clone(), accumulating_resources, &subscriber, - &on_resource_declared).fuse(); + &on_resource_declared, + ) + .fuse(); let listen_timeout = async_std::task::sleep(accumulate_period).fuse(); pin_mut!(listen, listen_timeout); select! { @@ -161,9 +200,12 @@ impl AlohaSubscription { for (key, val) in accumulating_resources.get_mut().iter() { if !val.is_active() { - unsafe {on_resource_undeclared(zenoh::key_expr::KeyExpr::from_str_uncheckend(key)).await}; + unsafe { + on_resource_undeclared(zenoh::key_expr::KeyExpr::from_str_uncheckend(key)) + .await + }; } - } + } accumulating_resources.get_mut().retain(|_key, val| { return val.is_active(); @@ -172,32 +214,46 @@ impl AlohaSubscription { } } - -pub struct AlohaSubscriptionBuilder -{ +pub struct AlohaSubscriptionBuilder { session: Arc, key: OwnedKeyExpr, beacon_period: Duration, - - on_resource_declared: Option Box + Unpin + Send + Sync> + Send + Sync + 'static>>, - on_resource_undeclared: Option Box + Unpin + Send + Sync> + Send + Sync + 'static>> + + on_resource_declared: Option< + Box< + dyn Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, + >, + >, + on_resource_undeclared: Option< + Box< + dyn Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, + >, + >, } -impl AlohaSubscriptionBuilder -{ - pub fn new(session: Arc, - key: OwnedKeyExpr, - beacon_period: Duration) -> Self { - return Self { session, - key, - beacon_period, - on_resource_declared: None, - on_resource_undeclared: None }; +impl AlohaSubscriptionBuilder { + pub fn new(session: Arc, key: OwnedKeyExpr, beacon_period: Duration) -> Self { + return Self { + session, + key, + beacon_period, + on_resource_declared: None, + on_resource_undeclared: None, + }; } pub fn on_resource_declared(mut self, on_resource_declared: F) -> Self where - F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { self.on_resource_declared = Some(Box::new(on_resource_declared)); return self; @@ -205,7 +261,10 @@ impl AlohaSubscriptionBuilder pub fn on_resource_undeclared(mut self, on_resource_undeclared: F) -> Self where - F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { self.on_resource_undeclared = Some(Box::new(on_resource_undeclared)); return self; @@ -214,10 +273,13 @@ impl AlohaSubscriptionBuilder pub async fn build(self) -> ZResult { return AlohaSubscription::new( self.session, - self.key, - self.beacon_period, - self.on_resource_declared.unwrap_or(Box::new(|_dummy|{ Box::new(Box::pin(async {})) })), - self.on_resource_undeclared.unwrap_or(Box::new(|_dummy|{ Box::new(Box::pin(async {})) })), - ).await; + self.key, + self.beacon_period, + self.on_resource_declared + .unwrap_or(Box::new(|_dummy| Box::new(Box::pin(async {})))), + self.on_resource_undeclared + .unwrap_or(Box::new(|_dummy| Box::new(Box::pin(async {})))), + ) + .await; } -} \ No newline at end of file +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs index 795acfb..98d4e95 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/bridge_type.rs @@ -17,5 +17,5 @@ pub enum BridgeType { Publisher, Subscriber, Service, - Client -} \ No newline at end of file + Client, +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs index 62ba836..ab62532 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs @@ -12,28 +12,41 @@ // ZettaScale Zenoh Team, // -use std::{collections::{HashMap, hash_map::Entry, HashSet}, sync::Arc}; - -use super::{topic_bridge::{TopicBridge, self}, bridge_type::BridgeType, ros1_client, discovery::LocalResources, zenoh_client, topic_mapping::Ros1TopicMapping, ros1_to_zenoh_bridge_impl::BridgeStatus}; - +use std::{ + collections::{hash_map::Entry, HashMap, HashSet}, + sync::Arc, +}; + +use super::{ + bridge_type::BridgeType, + discovery::LocalResources, + ros1_client, + ros1_to_zenoh_bridge_impl::BridgeStatus, + topic_bridge::{self, TopicBridge}, + topic_mapping::Ros1TopicMapping, + zenoh_client, +}; struct Bridges { publisher_bridges: HashMap, subscriber_bridges: HashMap, service_bridges: HashMap, - client_bridges: HashMap + client_bridges: HashMap, } impl Bridges { fn new() -> Self { - Self { + Self { publisher_bridges: HashMap::new(), subscriber_bridges: HashMap::new(), service_bridges: HashMap::new(), - client_bridges: HashMap::new() + client_bridges: HashMap::new(), } } - fn container_mut(&mut self, b_type: BridgeType) -> &mut HashMap { + fn container_mut( + &mut self, + b_type: BridgeType, + ) -> &mut HashMap { match b_type { BridgeType::Publisher => &mut self.publisher_bridges, BridgeType::Subscriber => &mut self.subscriber_bridges, @@ -43,7 +56,8 @@ impl Bridges { } fn status(&self) -> BridgeStatus { - let fill = |status: &mut (usize, usize), bridges: &HashMap| { + let fill = |status: &mut (usize, usize), + bridges: &HashMap| { for (_topic, bridge) in bridges.iter() { status.0 += 1; if bridge.is_bridging() { @@ -68,42 +82,53 @@ impl Bridges { } } - struct Access<'a> { container: &'a mut HashMap, b_type: BridgeType, - ros1_client: Arc, + ros1_client: Arc, zenoh_client: Arc, - declaration_interface: Arc + declaration_interface: Arc, } impl<'a> Access<'a> { - fn new(b_type: BridgeType, - container: &'a mut HashMap, - ros1_client: Arc, - zenoh_client: Arc, - declaration_interface: Arc) -> Self { - Self { + fn new( + b_type: BridgeType, + container: &'a mut HashMap, + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc, + ) -> Self { + Self { container, b_type, ros1_client, zenoh_client, - declaration_interface } + declaration_interface, + } } } - pub struct ComplementaryElementAccessor<'a> { - access: Access<'a> + access: Access<'a>, } impl<'a> ComplementaryElementAccessor<'a> { - fn new(b_type: BridgeType, - container: &'a mut HashMap, - ros1_client: Arc, - zenoh_client: Arc, - declaration_interface: Arc) -> Self { - Self { access: Access::new(b_type, container, ros1_client, zenoh_client, declaration_interface) } + fn new( + b_type: BridgeType, + container: &'a mut HashMap, + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc, + ) -> Self { + Self { + access: Access::new( + b_type, + container, + ros1_client, + zenoh_client, + declaration_interface, + ), + } } pub async fn complementary_entity_lost(&mut self, topic: rosrust::api::Topic) { @@ -113,8 +138,8 @@ impl<'a> ComplementaryElementAccessor<'a> { if bridge.set_has_complementary_in_zenoh(false).await && !bridge.is_actual() { val.remove_entry(); } - }, - Entry::Vacant(_) => {}, + } + Entry::Vacant(_) => {} } } @@ -131,40 +156,57 @@ impl<'a> ComplementaryElementAccessor<'a> { self.access.declaration_interface.clone(), self.access.ros1_client.clone(), self.access.zenoh_client.clone(), - topic_bridge::BridgingMode::Automatic - )).set_has_complementary_in_zenoh(true).await; + topic_bridge::BridgingMode::Automatic, + )) + .set_has_complementary_in_zenoh(true) + .await; } } } } - pub struct ElementAccessor<'a> { - access: Access<'a> + access: Access<'a>, } impl<'a> ElementAccessor<'a> { - fn new(b_type: BridgeType, - container: &'a mut HashMap, - ros1_client: Arc, - zenoh_client: Arc, - declaration_interface: Arc) -> Self { - Self { access: Access::new(b_type, container, ros1_client, zenoh_client, declaration_interface) } + fn new( + b_type: BridgeType, + container: &'a mut HashMap, + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc, + ) -> Self { + Self { + access: Access::new( + b_type, + container, + ros1_client, + zenoh_client, + declaration_interface, + ), + } } - async fn receive_ros1_state(&mut self, - part_of_ros_state: &mut HashSet) -> bool { + async fn receive_ros1_state( + &mut self, + part_of_ros_state: &mut HashSet, + ) -> bool { let mut smth_changed = false; // Run through bridges and actualize their state based on ROS1 state, removing corresponding entries from ROS1 state. - // As a result of this cycle, part_of_ros_state will contain only new topics that doesn't have corresponding bridge. + // As a result of this cycle, part_of_ros_state will contain only new topics that doesn't have corresponding bridge. for (topic, bridge) in self.access.container.iter_mut() { - smth_changed |= bridge.set_present_in_ros1(part_of_ros_state.take(topic).is_some()).await; + smth_changed |= bridge + .set_present_in_ros1(part_of_ros_state.take(topic).is_some()) + .await; } // erase all non-actual bridges { let size_before_retain = self.access.container.len(); - self.access.container.retain(|_topic, bridge| {return bridge.is_actual();}); + self.access.container.retain(|_topic, bridge| { + return bridge.is_actual(); + }); smth_changed |= size_before_retain != self.access.container.len(); } @@ -181,37 +223,39 @@ impl<'a> ElementAccessor<'a> { self.access.declaration_interface.clone(), self.access.ros1_client.clone(), self.access.zenoh_client.clone(), - topic_bridge::BridgingMode::Automatic)); + topic_bridge::BridgingMode::Automatic, + )); inserted.set_present_in_ros1(true).await; smth_changed = true; } - } + } } return smth_changed; } } - pub struct TypeAccessor<'a> { storage: &'a mut BridgesStorage, - _v: bool + _v: bool, } impl<'a> TypeAccessor<'a> { - fn new(storage: &'a mut BridgesStorage) -> Self { Self { storage, _v: false } } + fn new(storage: &'a mut BridgesStorage) -> Self { + Self { storage, _v: false } + } pub fn complementary_for(&'a mut self, b_type: BridgeType) -> ComplementaryElementAccessor<'a> { let b_type = match b_type { BridgeType::Publisher => BridgeType::Subscriber, BridgeType::Subscriber => BridgeType::Publisher, BridgeType::Service => BridgeType::Client, - BridgeType::Client => BridgeType::Service + BridgeType::Client => BridgeType::Service, }; ComplementaryElementAccessor::new( b_type, self.storage.bridges.container_mut(b_type), self.storage.ros1_client.clone(), self.storage.zenoh_client.clone(), - self.storage.declaration_interface.clone() + self.storage.declaration_interface.clone(), ) } @@ -221,30 +265,30 @@ impl<'a> TypeAccessor<'a> { self.storage.bridges.container_mut(b_type), self.storage.ros1_client.clone(), self.storage.zenoh_client.clone(), - self.storage.declaration_interface.clone() + self.storage.declaration_interface.clone(), ) } } - - pub struct BridgesStorage { bridges: Bridges, - ros1_client: Arc, + ros1_client: Arc, zenoh_client: Arc, - declaration_interface: Arc + declaration_interface: Arc, } impl BridgesStorage { - pub fn new(ros1_client: Arc, - zenoh_client: Arc, - declaration_interface: Arc) -> Self { - Self { + pub fn new( + ros1_client: Arc, + zenoh_client: Arc, + declaration_interface: Arc, + ) -> Self { + Self { bridges: Bridges::new(), ros1_client, zenoh_client, - declaration_interface + declaration_interface, } } @@ -252,12 +296,22 @@ impl BridgesStorage { TypeAccessor::new(self) } - pub async fn receive_ros1_state (&mut self, - ros1_state: &mut Ros1TopicMapping) -> bool { - - let mut smth_changed = self.bridges().for_type(BridgeType::Publisher).receive_ros1_state(&mut ros1_state.published).await; - smth_changed |= self.bridges().for_type(BridgeType::Service).receive_ros1_state(&mut ros1_state.serviced).await; - smth_changed |= self.bridges().for_type(BridgeType::Subscriber).receive_ros1_state(&mut ros1_state.subscribed).await; + pub async fn receive_ros1_state(&mut self, ros1_state: &mut Ros1TopicMapping) -> bool { + let mut smth_changed = self + .bridges() + .for_type(BridgeType::Publisher) + .receive_ros1_state(&mut ros1_state.published) + .await; + smth_changed |= self + .bridges() + .for_type(BridgeType::Service) + .receive_ros1_state(&mut ros1_state.serviced) + .await; + smth_changed |= self + .bridges() + .for_type(BridgeType::Subscriber) + .receive_ros1_state(&mut ros1_state.subscribed) + .await; return smth_changed; } @@ -269,5 +323,3 @@ impl BridgesStorage { return self.bridges.clear(); } } - - diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs index 5ce9efc..eefd2ed 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs @@ -20,13 +20,13 @@ use std::sync::Arc; use std::time::Duration; use zenoh::prelude::r#async::*; - + use std::str; use super::aloha_declaration::AlohaDeclaration; use super::aloha_subscription::{AlohaSubscription, AlohaSubscriptionBuilder}; use super::bridge_type::BridgeType; -use super::topic_utilities::{make_zenoh_key, make_topic}; +use super::topic_utilities::{make_topic, make_zenoh_key}; zenoh::kedefine!( pub discovery_format: "ros1_discovery_info/${discovery_namespace:*}/${resource_class:*}/${data_type:*}/${bridge_namespace:*}/${topic:**}", @@ -43,61 +43,93 @@ const ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS: &str = "sub"; const ROS1_DISCOVERY_INFO_SERVICES_CLASS: &str = "srv"; const ROS1_DISCOVERY_INFO_CLIENTS_CLASS: &str = "cl"; - - struct RemoteResources { - _subscriber: Option + _subscriber: Option, } impl RemoteResources { - async fn new(session: Arc, - discovery_namespace: String, - on_discovered: F, - on_lost: F) -> Self + async fn new( + session: Arc, + discovery_namespace: String, + on_discovered: F, + on_lost: F, + ) -> Self where - F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { let subscriber: Option; // make proper discovery keyexpr let mut formatter = discovery_format::formatter(); let discovery_keyexpr = zenoh::keformat!( - formatter, - discovery_namespace = discovery_namespace, - resource_class = "*", + formatter, + discovery_namespace = discovery_namespace, + resource_class = "*", data_type = "*", bridge_namespace = "*", - topic = "*/**" ).unwrap(); + topic = "*/**" + ) + .unwrap(); let _on_discovered = Arc::new(on_discovered); let _on_lost = Arc::new(on_lost); - let subscription = AlohaSubscriptionBuilder::new(session, - discovery_keyexpr.clone(), - Duration::from_secs(1)) - .on_resource_declared(move |key| { Box::new( Box::pin( Self::process(key.into_owned(), _on_discovered.clone() ) ) ) }) - .on_resource_undeclared(move |key| { Box::new( Box::pin( Self::process(key.into_owned(), _on_lost.clone() ) ) ) }) - .build().await; + let subscription = AlohaSubscriptionBuilder::new( + session, + discovery_keyexpr.clone(), + Duration::from_secs(1), + ) + .on_resource_declared(move |key| { + Box::new(Box::pin(Self::process( + key.into_owned(), + _on_discovered.clone(), + ))) + }) + .on_resource_undeclared(move |key| { + Box::new(Box::pin(Self::process(key.into_owned(), _on_lost.clone()))) + }) + .build() + .await; match subscription { - Ok(s) => { subscriber = Some(s); } - Err(e) => { - error!("ROS1 Discovery: error creating querying subscriber: {}", e); + Ok(s) => { + subscriber = Some(s); + } + Err(e) => { + error!("ROS1 Discovery: error creating querying subscriber: {}", e); subscriber = None; } } - return Self { _subscriber: subscriber }; + return Self { + _subscriber: subscriber, + }; } -// PRIVATE: + // PRIVATE: async fn process(data: KeyExpr<'_>, callback: Arc) where - F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { match Self::parse_format(&data, &callback).await { Ok(_) => {} Err(e) => { - error!("ROS1 Discovery: entry {}: processing error: {}", data.as_str(), e); + error!( + "ROS1 Discovery: entry {}: processing error: {}", + data.as_str(), + e + ); debug_assert!(false); } } @@ -105,19 +137,31 @@ impl RemoteResources { async fn parse_format(data: &KeyExpr<'_>, callback: &Arc) -> Result<(), String> where - F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + F: Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync>, { - let discovery = discovery_format::parse(&data).map_err(|err| err.to_string() )?; + let discovery = discovery_format::parse(&data).map_err(|err| err.to_string())?; return Self::handle_format(discovery, callback).await; } - async fn handle_format(discovery: discovery_format::Parsed<'_>, callback: &Arc) -> Result<(), String> + async fn handle_format( + discovery: discovery_format::Parsed<'_>, + callback: &Arc, + ) -> Result<(), String> where - F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + F: Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync>, { //let discovery_namespace = discovery.discovery_namespace().ok_or("No discovery_namespace present!")?; let datatype = discovery.data_type().ok_or("No data_type present!")?; - let resource_class = discovery.resource_class().ok_or("No resource_class present!")?.to_string(); + let resource_class = discovery + .resource_class() + .ok_or("No resource_class present!")? + .to_string(); //let bridge_namespace = discovery.bridge_namespace().ok_or("No bridge_namespace present!")?.to_string(); let topic = discovery.topic().ok_or("No topic present!")?; @@ -125,11 +169,21 @@ impl RemoteResources { let b_type; match resource_class.as_str() { - ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS => { b_type = BridgeType::Publisher; } - ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS => { b_type = BridgeType::Subscriber; } - ROS1_DISCOVERY_INFO_SERVICES_CLASS => { b_type = BridgeType::Service; } - ROS1_DISCOVERY_INFO_CLIENTS_CLASS => { b_type = BridgeType::Client; } - _ => { return Err("unexpected resource class!".to_string()); } + ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS => { + b_type = BridgeType::Publisher; + } + ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS => { + b_type = BridgeType::Subscriber; + } + ROS1_DISCOVERY_INFO_SERVICES_CLASS => { + b_type = BridgeType::Service; + } + ROS1_DISCOVERY_INFO_CLIENTS_CLASS => { + b_type = BridgeType::Client; + } + _ => { + return Err("unexpected resource class!".to_string()); + } }; callback(b_type, ros1_topic).await; @@ -137,10 +191,8 @@ impl RemoteResources { } } - - pub struct LocalResource { - _declaration: AlohaDeclaration + _declaration: AlohaDeclaration, } impl LocalResource { async fn new( @@ -148,101 +200,124 @@ impl LocalResource { bridge_namespace: &str, resource_class: &str, topic: &rosrust::api::Topic, - session: Arc) -> LocalResource { - + session: Arc, + ) -> LocalResource { // make proper discovery keyexpr let mut formatter = discovery_format::formatter(); let discovery_keyexpr = zenoh::keformat!( - formatter, - discovery_namespace = discovery_namespace, - resource_class = resource_class, + formatter, + discovery_namespace = discovery_namespace, + resource_class = resource_class, data_type = topic.datatype.clone(), bridge_namespace = bridge_namespace, - topic = make_zenoh_key(topic)).unwrap(); - - let _declaration = AlohaDeclaration::new( - session, - discovery_keyexpr, - Duration::from_secs(1)); - - Self { _declaration } - } -} + topic = make_zenoh_key(topic) + ) + .unwrap(); + let _declaration = + AlohaDeclaration::new(session, discovery_keyexpr, Duration::from_secs(1)); + Self { _declaration } + } +} pub struct LocalResources { session: Arc, discovery_namespace: String, - bridge_namespace: String + bridge_namespace: String, } impl LocalResources { pub fn new( discovery_namespace: String, bridge_namespace: String, - session: Arc) -> LocalResources { - - return Self{ + session: Arc, + ) -> LocalResources { + return Self { session, discovery_namespace, - bridge_namespace + bridge_namespace, }; } - pub async fn declare_with_type(&self, topic: &rosrust::api::Topic, b_type: BridgeType) -> LocalResource { + pub async fn declare_with_type( + &self, + topic: &rosrust::api::Topic, + b_type: BridgeType, + ) -> LocalResource { match b_type { - BridgeType::Publisher => {self.declare_publisher(topic).await} - BridgeType::Subscriber => {self.declare_subscriber(topic).await} - BridgeType::Service => {self.declare_service(topic).await} - BridgeType::Client => {self.declare_client(topic).await} + BridgeType::Publisher => self.declare_publisher(topic).await, + BridgeType::Subscriber => self.declare_subscriber(topic).await, + BridgeType::Service => self.declare_service(topic).await, + BridgeType::Client => self.declare_client(topic).await, } } - pub async fn declare_publisher(&self, topic: &rosrust::api::Topic ) -> LocalResource { - self.declare(topic, ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS).await + pub async fn declare_publisher(&self, topic: &rosrust::api::Topic) -> LocalResource { + self.declare(topic, ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS) + .await } - pub async fn declare_subscriber(&self, topic: &rosrust::api::Topic ) -> LocalResource { - self.declare(topic, ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS).await + pub async fn declare_subscriber(&self, topic: &rosrust::api::Topic) -> LocalResource { + self.declare(topic, ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS) + .await } - pub async fn declare_service(&self, topic: &rosrust::api::Topic ) -> LocalResource { - self.declare(topic, ROS1_DISCOVERY_INFO_SERVICES_CLASS).await + pub async fn declare_service(&self, topic: &rosrust::api::Topic) -> LocalResource { + self.declare(topic, ROS1_DISCOVERY_INFO_SERVICES_CLASS) + .await } - pub async fn declare_client(&self, topic: &rosrust::api::Topic ) -> LocalResource { + pub async fn declare_client(&self, topic: &rosrust::api::Topic) -> LocalResource { self.declare(topic, ROS1_DISCOVERY_INFO_CLIENTS_CLASS).await } -//PRIVATE: - pub async fn declare(&self, topic: &rosrust::api::Topic, resource_class: &str) -> LocalResource { + //PRIVATE: + pub async fn declare( + &self, + topic: &rosrust::api::Topic, + resource_class: &str, + ) -> LocalResource { LocalResource::new( - &self.discovery_namespace, - &self.bridge_namespace, - resource_class, + &self.discovery_namespace, + &self.bridge_namespace, + resource_class, topic, - self.session.clone()).await + self.session.clone(), + ) + .await } } - - pub struct Discovery { _remote_resources: RemoteResources, local_resources: LocalResources, } impl Discovery { - pub async fn new( discovery_namespace: String, - bridge_namespace: String, - session: Arc, - on_discovered: F, - on_lost: F) -> Self + pub async fn new( + discovery_namespace: String, + bridge_namespace: String, + session: Arc, + on_discovered: F, + on_lost: F, + ) -> Self where - F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { return Self { - _remote_resources: RemoteResources::new(session.clone(), discovery_namespace.clone(), on_discovered, on_lost).await, - local_resources: LocalResources::new(discovery_namespace, bridge_namespace, session) + _remote_resources: RemoteResources::new( + session.clone(), + discovery_namespace.clone(), + on_discovered, + on_lost, + ) + .await, + local_resources: LocalResources::new(discovery_namespace, bridge_namespace, session), }; } @@ -251,40 +326,72 @@ impl Discovery { } } - - -pub struct DiscoveryBuilder -{ +pub struct DiscoveryBuilder { discovery_namespace: String, bridge_namespace: String, session: Arc, - on_discovered: Option Box + Unpin + Send + Sync> + Send + Sync + 'static>>, - on_lost: Option Box + Unpin + Send + Sync> + Send + Sync + 'static>> + on_discovered: Option< + Box< + dyn Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, + >, + >, + on_lost: Option< + Box< + dyn Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, + >, + >, } -impl DiscoveryBuilder -{ - pub fn new(discovery_namespace: String, - bridge_namespace: String, - session: Arc) -> Self { - Self { discovery_namespace, - bridge_namespace, - session, - on_discovered: None, - on_lost: None } +impl DiscoveryBuilder { + pub fn new( + discovery_namespace: String, + bridge_namespace: String, + session: Arc, + ) -> Self { + Self { + discovery_namespace, + bridge_namespace, + session, + on_discovered: None, + on_lost: None, + } } pub fn on_discovered(&mut self, on_discovered: F) -> &mut Self where - F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { self.on_discovered = Some(Box::new(on_discovered)); return self; } pub fn on_lost(&mut self, on_lost: F) -> &mut Self where - F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + Send + Sync + 'static + F: Fn( + BridgeType, + rosrust::api::Topic, + ) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static, { self.on_lost = Some(Box::new(on_lost)); return self; @@ -293,9 +400,13 @@ impl DiscoveryBuilder pub async fn build(self) -> Discovery { return Discovery::new( self.discovery_namespace, - self.bridge_namespace, - self.session, - self.on_discovered.unwrap_or(Box::new(|_, _|{ Box::new(Box::pin(async {})) })), - self.on_lost.unwrap_or(Box::new(|_, _|{ Box::new(Box::pin(async {})) }))).await; + self.bridge_namespace, + self.session, + self.on_discovered + .unwrap_or(Box::new(|_, _| Box::new(Box::pin(async {})))), + self.on_lost + .unwrap_or(Box::new(|_, _| Box::new(Box::pin(async {})))), + ) + .await; } -} \ No newline at end of file +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs index fc34599..78665a2 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs @@ -12,25 +12,28 @@ // ZettaScale Zenoh Team, // -use std::str::FromStr; use rosrust::api::resolve::*; +use std::str::FromStr; #[derive(Clone)] pub struct Entry<'a> { pub name: &'a str, - pub default: String + pub default: String, } impl<'a> Entry<'a> { fn new(name: &'a str, default: Tvar) -> Entry<'a> where - Tvar: ToString + Tvar: ToString, { - Entry{name, default: default.to_string()} + Entry { + name, + default: default.to_string(), + } } pub fn get(&self) -> Tvar where - Tvar: FromStr + std::convert::From + Tvar: FromStr + std::convert::From, { if let Ok(val) = std::env::var(self.name) { if let Ok(val) = val.parse::() { @@ -42,13 +45,12 @@ impl<'a> Entry<'a> { pub fn set(&self, value: Tvar) where - Tvar: ToString + Tvar: ToString, { std::env::set_var(self.name, value.to_string()); } } - pub struct Environment; impl Environment { pub fn ros_master_uri() -> Entry<'static> { @@ -72,7 +74,8 @@ impl Environment { Self::ros_master_uri(), Self::ros_hostname(), Self::ros_name(), - Self::ros_namespace() - ].to_vec(); + Self::ros_namespace(), + ] + .to_vec(); } -} \ No newline at end of file +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs index e685de3..ce39f9f 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs @@ -12,41 +12,52 @@ // ZettaScale Zenoh Team, // -use log::{debug}; +use log::debug; use rosrust; pub struct Ros1Client { - ros: rosrust::api::Ros + ros: rosrust::api::Ros, } impl Ros1Client { -// PUBLIC + // PUBLIC pub fn new(name: &str) -> Ros1Client { - Ros1Client{ - ros: rosrust::api::Ros::new(name).unwrap() + Ros1Client { + ros: rosrust::api::Ros::new(name).unwrap(), } } - pub fn subscribe(&self, topic: &rosrust::api::Topic, callback: F) -> rosrust::api::error::Result + pub fn subscribe( + &self, + topic: &rosrust::api::Topic, + callback: F, + ) -> rosrust::api::error::Result where T: rosrust::Message, F: Fn(T) + Send + 'static, { - self.ros.subscribe( - &topic.name, - 0, - callback) + self.ros.subscribe(&topic.name, 0, callback) } - pub fn publish(&self, topic: &rosrust::api::Topic) -> rosrust::api::error::Result> { + pub fn publish( + &self, + topic: &rosrust::api::Topic, + ) -> rosrust::api::error::Result> { self.ros.publish(&topic.name, 0) } - pub fn client(&self, topic: &rosrust::api::Topic) -> rosrust::api::error::Result> { + pub fn client( + &self, + topic: &rosrust::api::Topic, + ) -> rosrust::api::error::Result> { self.ros.client::(&topic.name) } - pub fn service(&self, topic: &rosrust::api::Topic, handler: F) -> rosrust::api::error::Result + pub fn service( + &self, + topic: &rosrust::api::Topic, + handler: F, + ) -> rosrust::api::error::Result where T: rosrust::ServicePair, F: Fn(T::Request) -> rosrust::ServiceResult + Send + Sync + 'static, @@ -57,8 +68,8 @@ impl Ros1Client { pub fn state(&self) -> rosrust::api::error::Response { self.filter(self.ros.state()) } - - pub fn topic_types(&self) -> rosrust::api::error::Response> { + + pub fn topic_types(&self) -> rosrust::api::error::Response> { self.ros.topics() } @@ -66,14 +77,19 @@ impl Ros1Client { /** * Filter out topics, which are published\subscribed\serviced only by the bridge itself */ - fn filter(&self, mut state: rosrust::api::error::Response) -> rosrust::api::error::Response { + fn filter( + &self, + mut state: rosrust::api::error::Response, + ) -> rosrust::api::error::Response { debug!("system state before filter: {:#?}", state); match state.as_mut() { Ok(value) => { let name = self.ros.name(); - let retain_lambda = |x: &rosrust::api::TopicData| - return x.connections.len() > 1 || (x.connections.len() == 1 && x.connections[0] != name); + let retain_lambda = |x: &rosrust::api::TopicData| { + return x.connections.len() > 1 + || (x.connections.len() == 1 && x.connections[0] != name); + }; value.publishers.retain(retain_lambda); value.subscribers.retain(retain_lambda); diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs index 91d5e67..d29e6ca 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs @@ -12,30 +12,31 @@ // ZettaScale Zenoh Team, // -use async_std::{sync::{Mutex, MutexGuard}}; -use futures::{FutureExt, select, pin_mut}; +use async_std::sync::{Mutex, MutexGuard}; +use futures::{pin_mut, select, FutureExt}; use zenoh; -use std::{sync::{ +use std::sync::{ + atomic::{AtomicBool, Ordering::Relaxed}, Arc, - atomic::{AtomicBool, Ordering::Relaxed} -}}; +}; use log::{debug, error}; -use crate::ros_to_zenoh_bridge::{bridges_storage::BridgesStorage, topic_mapping, zenoh_client, ros1_client, discovery::LocalResources, environment::Environment}; +use crate::ros_to_zenoh_bridge::{ + bridges_storage::BridgesStorage, discovery::LocalResources, environment::Environment, + ros1_client, topic_mapping, zenoh_client, +}; use super::{discovery::DiscoveryBuilder, ros1_client::Ros1Client}; - - #[derive(PartialEq, Clone, Copy)] pub enum RosStatus { Unknown, Synchronizing, Ok, - Error + Error, } #[derive(Default, PartialEq, Eq, Clone, Copy)] @@ -43,44 +44,40 @@ pub struct BridgeStatus { pub ros_publishers: (usize, usize), pub ros_subscribers: (usize, usize), pub ros_services: (usize, usize), - pub ros_clients: (usize, usize) + pub ros_clients: (usize, usize), } -pub async fn work_cycle(session: Arc, - flag: Arc, - ros_status_callback: RosStatusCallback, - statistics_callback: BridgeStatisticsCallback) -where +pub async fn work_cycle( + session: Arc, + flag: Arc, + ros_status_callback: RosStatusCallback, + statistics_callback: BridgeStatisticsCallback, +) where RosStatusCallback: Fn(RosStatus), - BridgeStatisticsCallback: Fn(BridgeStatus) - + BridgeStatisticsCallback: Fn(BridgeStatus), { - let ros1_client = Arc::new(ros1_client::Ros1Client::new(Environment::ros_name().get::().as_str() )); + let ros1_client = Arc::new(ros1_client::Ros1Client::new( + Environment::ros_name().get::().as_str(), + )); let zenoh_client = Arc::new(zenoh_client::ZenohClient::new(session.clone())); let local_resources = Arc::new(LocalResources::new( "*".to_string(), "*".to_string(), - session.clone())); + session.clone(), + )); let bridges = Arc::new(Mutex::new(BridgesStorage::new( ros1_client.clone(), zenoh_client, - local_resources + local_resources, ))); - - let mut bridge = RosToZenohBridge::new(ros_status_callback, - statistics_callback); + let mut bridge = RosToZenohBridge::new(ros_status_callback, statistics_callback); let discovery = make_discovery(session.clone(), bridges.clone()).fuse(); - let run = bridge.run( - ros1_client, - bridges, - flag - ).fuse(); + let run = bridge.run(ros1_client, bridges, flag).fuse(); pin_mut!(discovery, run); @@ -88,16 +85,11 @@ where _ = discovery => {}, _ = run => {} }; - } -async fn make_discovery<'a>(session: Arc, - bridges: Arc>) { - let mut builder = DiscoveryBuilder::new( - "*".to_string(), - "*".to_string(), - session); - +async fn make_discovery<'a>(session: Arc, bridges: Arc>) { + let mut builder = DiscoveryBuilder::new("*".to_string(), "*".to_string(), session); + let (add_tx, add_rx) = flume::unbounded(); let (rem_tx, rem_rx) = flume::unbounded(); @@ -105,40 +97,36 @@ async fn make_discovery<'a>(session: Arc, let rem_tx = Arc::new(rem_tx); builder - .on_discovered(move |b_type, topic| { - let add_tx = add_tx.clone(); - Box::new( Box::pin( - async move { + .on_discovered(move |b_type, topic| { + let add_tx = add_tx.clone(); + Box::new(Box::pin(async move { match add_tx.send_async((b_type, topic)).await { - Ok(_) => {}, + Ok(_) => {} Err(e) => { error!("Error posting topic discoery to channel: {}", e); } } - } - )) - }) - .on_lost(move |b_type, topic|{ - let rem_tx = rem_tx.clone(); - Box::new( Box::pin( - async move { + })) + }) + .on_lost(move |b_type, topic| { + let rem_tx = rem_tx.clone(); + Box::new(Box::pin(async move { match rem_tx.send_async((b_type, topic)).await { - Ok(_) => {}, + Ok(_) => {} Err(e) => { error!("Error posting topic loss to channel: {}", e); } } - } - )) - }); + })) + }); - let _discovery = builder.build().await; + let _discovery = builder.build().await; loop { select! { add = add_rx.recv_async().fuse() => { match add { - Ok((b_type, topic)) => { + Ok((b_type, topic)) => { bridges.lock().await.bridges().complementary_for(b_type).complementary_entity_discovered(topic).await; } Err(_) => todo!(), @@ -146,57 +134,58 @@ async fn make_discovery<'a>(session: Arc, } rem = rem_rx.recv_async().fuse() => { match rem { - Ok((b_type, topic)) => { + Ok((b_type, topic)) => { bridges.lock().await.bridges().complementary_for(b_type).complementary_entity_lost(topic).await; } Err(_) => todo!(), } } } - } + } } - - -struct RosToZenohBridge +struct RosToZenohBridge where RosStatusCallback: Fn(RosStatus), - BridgeStatisticsCallback: Fn(BridgeStatus) + BridgeStatisticsCallback: Fn(BridgeStatus), { ros_status: RosStatus, ros_status_callback: RosStatusCallback, - statistics_callback: BridgeStatisticsCallback + statistics_callback: BridgeStatisticsCallback, } -impl RosToZenohBridge +impl + RosToZenohBridge where RosStatusCallback: Fn(RosStatus), - BridgeStatisticsCallback: Fn(BridgeStatus) + BridgeStatisticsCallback: Fn(BridgeStatus), { -// PUBLIC - pub fn new(ros_status_callback: RosStatusCallback, - statistics_callback: BridgeStatisticsCallback) -> Self { + // PUBLIC + pub fn new( + ros_status_callback: RosStatusCallback, + statistics_callback: BridgeStatisticsCallback, + ) -> Self { RosToZenohBridge { ros_status: RosStatus::Unknown, ros_status_callback, - statistics_callback + statistics_callback, } } - pub async fn run(&mut self, - ros1_client: Arc, - bridges: Arc>, - flag: Arc) { + pub async fn run( + &mut self, + ros1_client: Arc, + bridges: Arc>, + flag: Arc, + ) { while flag.load(Relaxed) { let cl = ros1_client.clone(); - let ros1_state = async_std::task::spawn_blocking( - move || { topic_mapping::Ros1TopicMapping::topic_mapping(cl.as_ref()) } - ).await; + let ros1_state = async_std::task::spawn_blocking(move || { + topic_mapping::Ros1TopicMapping::topic_mapping(cl.as_ref()) + }) + .await; debug!("ros state: {:#?}", ros1_state); @@ -208,8 +197,7 @@ where if locked.receive_ros1_state(&mut ros1_state_val).await { self.report_bridge_statistics(&locked).await; async_std::task::sleep(core::time::Duration::from_millis(100)).await; - } - else { + } else { self.report_bridge_statistics(&locked).await; async_std::task::sleep(core::time::Duration::from_millis(200)).await; } @@ -227,7 +215,7 @@ where } } -// PRIVATE + // PRIVATE fn transit_ros_status(&mut self, new_ros_status: RosStatus) { if self.ros_status != new_ros_status { self.ros_status = new_ros_status; diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs index e4ff5af..5fe539a 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs @@ -12,15 +12,19 @@ // ZettaScale Zenoh Team, // -use std::sync::Arc; +use super::{ + abstract_bridge::AbstractBridge, + bridge_type::BridgeType, + discovery::{LocalResource, LocalResources}, + ros1_client, zenoh_client, +}; use log::error; -use super::{ros1_client, zenoh_client, discovery::{LocalResources, LocalResource}, bridge_type::BridgeType, abstract_bridge::AbstractBridge}; - +use std::sync::Arc; #[derive(PartialEq, Eq)] pub enum BridgingMode { Lazy, - Automatic + Automatic, } pub struct TopicBridge { @@ -36,17 +40,19 @@ pub struct TopicBridge { declaration_interface: Arc, declaration: Option, - bridge: Option + bridge: Option, } impl TopicBridge { - pub fn new(topic: rosrust::api::Topic, - b_type: BridgeType, - declaration_interface: Arc, - ros1_client: Arc, - zenoh_client: Arc, - briging_mode: BridgingMode) -> Self { - return Self { + pub fn new( + topic: rosrust::api::Topic, + b_type: BridgeType, + declaration_interface: Arc, + ros1_client: Arc, + zenoh_client: Arc, + briging_mode: BridgingMode, + ) -> Self { + return Self { topic, b_type, ros1_client, @@ -56,7 +62,7 @@ impl TopicBridge { required_on_zenoh_side: false, declaration_interface, declaration: None, - bridge: None + bridge: None, }; } @@ -93,35 +99,52 @@ impl TopicBridge { } } -//PRIVATE: + //PRIVATE: async fn recalc_state(&mut self) { self.recalc_declaration().await; self.recalc_bridging().await; } - + async fn recalc_declaration(&mut self) { match (self.required_on_ros1_side, &self.declaration) { - (true, None) => { self.declaration = Some(self.declaration_interface.declare_with_type(&self.topic, self.b_type).await); } - (false, Some(_)) => { self.declaration = None; } + (true, None) => { + self.declaration = Some( + self.declaration_interface + .declare_with_type(&self.topic, self.b_type) + .await, + ); + } + (false, Some(_)) => { + self.declaration = None; + } (_, _) => {} } } async fn recalc_bridging(&mut self) { let is_discovered_client = self.b_type == BridgeType::Client && self.required_on_zenoh_side; - let is_required = self.required_on_ros1_side && (self.briging_mode == BridgingMode::Automatic || self.required_on_zenoh_side); + let is_required = self.required_on_ros1_side + && (self.briging_mode == BridgingMode::Automatic || self.required_on_zenoh_side); if is_required || is_discovered_client { self.create_bridge().await; - } - else { + } else { self.bridge = None; } } async fn create_bridge(&mut self) { - match AbstractBridge::new(self.b_type, &self.topic, &self.ros1_client, &self.zenoh_client).await { - Ok(val) => {self.bridge = Some(val);} + match AbstractBridge::new( + self.b_type, + &self.topic, + &self.ros1_client, + &self.zenoh_client, + ) + .await + { + Ok(val) => { + self.bridge = Some(val); + } Err(e) => { self.bridge = None; error!("Errr creating bridge: {}", e); diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs index 12f5d8c..51e6e38 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs @@ -14,18 +14,20 @@ use std::collections::HashSet; -use log::{debug}; use crate::ros_to_zenoh_bridge::ros1_client; +use log::debug; #[derive(Debug)] pub struct Ros1TopicMapping { pub published: HashSet, pub subscribed: HashSet, pub serviced: HashSet, - pub clients: HashSet + pub clients: HashSet, } impl Ros1TopicMapping { - pub fn topic_mapping(ros1_client: &ros1_client::Ros1Client) -> rosrust::api::error::Response { + pub fn topic_mapping( + ros1_client: &ros1_client::Ros1Client, + ) -> rosrust::api::error::Response { match ros1_client.state() { Ok(state_val) => { match ros1_client.topic_types() { @@ -33,21 +35,27 @@ impl Ros1TopicMapping { debug!("topics: {:#?}", topics_val); return Ok(Ros1TopicMapping::new(&state_val, &topics_val)); } - Err(e) => { return Err(e); } - }; + Err(e) => { + return Err(e); + } + }; + } + Err(e) => { + return Err(e); } - Err(e) => { return Err(e); } }; - } + } -// PRIVATE: - fn new(state: &rosrust::api::SystemState, - topics: &Vec) -> Ros1TopicMapping { - let mut result = Ros1TopicMapping{ + // PRIVATE: + fn new( + state: &rosrust::api::SystemState, + topics: &Vec, + ) -> Ros1TopicMapping { + let mut result = Ros1TopicMapping { published: HashSet::new(), subscribed: HashSet::new(), serviced: HashSet::new(), - clients: HashSet::new() + clients: HashSet::new(), }; Ros1TopicMapping::fill(&mut result.subscribed, &state.subscribers, topics); @@ -57,17 +65,20 @@ impl Ros1TopicMapping { return result; } - fn fill(dst: &mut HashSet, - data: &Vec, - topics: &Vec) { - + fn fill( + dst: &mut HashSet, + data: &Vec, + topics: &Vec, + ) { for item in data.iter() { let topic = topics.iter().find(|x| x.name == item.name); if topic.is_none() { debug!("Unable to find datatype for topic {}", item.name); - dst.insert(rosrust::api::Topic{name: item.name.clone(), datatype: "*".to_string()}); - } - else { + dst.insert(rosrust::api::Topic { + name: item.name.clone(), + datatype: "*".to_string(), + }); + } else { let t = topic.unwrap(); dst.insert(t.clone()); } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs index 06ab889..34e5647 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs @@ -15,25 +15,24 @@ use zenoh::prelude::keyexpr; pub fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { - return topic.name.trim_start_matches('/') - .trim_end_matches('/'); + return topic.name.trim_start_matches('/').trim_end_matches('/'); } pub fn make_topic(datatype: &keyexpr, topic_name: &keyexpr) -> Result { match datatype.as_str() { - "*" | "**" => { return Err("incorrect datatype!".into()) }, + "*" | "**" => return Err("incorrect datatype!".into()), _ => {} } match topic_name.as_str() { - "*" | "**" => { return Err("incorrect topic name!".into()) }, + "*" | "**" => return Err("incorrect topic name!".into()), _ => {} } let mut name = topic_name.to_string(); name.insert(0, '/'); - return Ok(rosrust::api::Topic{ - name, - datatype: datatype.to_string() - }); -} \ No newline at end of file + return Ok(rosrust::api::Topic { + name, + datatype: datatype.to_string(), + }); +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs index bc21fb9..b90a599 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs @@ -16,35 +16,43 @@ use std::sync::Arc; use log::{debug, info}; -use zenoh::Session; use zenoh::prelude::r#async::*; +use zenoh::Session; pub use zenoh_core::zresult::ZResult; pub struct ZenohClient { - session: Arc + session: Arc, } impl ZenohClient { -// PUBLIC + // PUBLIC pub fn new(session: Arc) -> ZenohClient { - ZenohClient { - session - } + ZenohClient { session } } - pub async fn subscribe(&self, key_expr: &str, callback: C) -> ZResult> + pub async fn subscribe( + &self, + key_expr: &str, + callback: C, + ) -> ZResult> where - C: Fn(Sample) + Send + Sync + 'static + C: Fn(Sample) + Send + Sync + 'static, { debug!("Creating Subscriber on {}", key_expr); - + match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - return self.session.declare_subscriber(opt_keyexpr) - .callback(callback) - .allowed_origin(Locality::Remote).res_async().await; + Ok(opt_keyexpr) => { + return self + .session + .declare_subscriber(opt_keyexpr) + .callback(callback) + .allowed_origin(Locality::Remote) + .res_async() + .await; + } + Err(e) => { + return Err(e); } - Err(e) => { return Err(e); } } } @@ -52,73 +60,104 @@ impl ZenohClient { debug!("Creating Publisher on {}", key_expr); match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - return self.session.declare_publisher(opt_keyexpr) - .allowed_destination(Locality::Remote) - .congestion_control(CongestionControl::Block).res_async().await; + Ok(opt_keyexpr) => { + return self + .session + .declare_publisher(opt_keyexpr) + .allowed_destination(Locality::Remote) + .congestion_control(CongestionControl::Block) + .res_async() + .await; + } + Err(e) => { + return Err(e); } - Err(e) => { return Err(e); } } } - pub async fn make_queryable(&self, key_expr: &str, callback: Callback) -> ZResult > + pub async fn make_queryable( + &self, + key_expr: &str, + callback: Callback, + ) -> ZResult> where Callback: Fn(zenoh::queryable::Query) + Send + Sync + 'static, { info!("Creating Queryable on {}", key_expr); match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - return self.session.declare_queryable(opt_keyexpr) - .allowed_origin(Locality::Remote) - .callback(callback).res_async().await; + Ok(opt_keyexpr) => { + return self + .session + .declare_queryable(opt_keyexpr) + .allowed_origin(Locality::Remote) + .callback(callback) + .res_async() + .await; + } + Err(e) => { + return Err(e); } - Err(e) => { return Err(e); } } } pub async fn make_query( - &self, + &self, key_expr: &str, callback: Callback, - data: Vec) -> ZResult<()> + data: Vec, + ) -> ZResult<()> where Callback: Fn(zenoh::query::Reply) + Send + Sync + 'static, { debug!("Creating Query on {}", key_expr); match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - return self.session.get(opt_keyexpr) - .with_value(data) - .callback(callback).res_async().await; + Ok(opt_keyexpr) => { + return self + .session + .get(opt_keyexpr) + .with_value(data) + .callback(callback) + .res_async() + .await; + } + Err(e) => { + return Err(e); } - Err(e) => { return Err(e); } } } pub async fn make_query_sync( - &self, + &self, key_expr: &str, - data: Vec) -> ZResult> - { + data: Vec, + ) -> ZResult> { debug!("Creating Query on {}", key_expr); match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - return self.session.get(opt_keyexpr) - .with_value(data) - .allowed_destination(Locality::Remote) - .res_async().await; + Ok(opt_keyexpr) => { + return self + .session + .get(opt_keyexpr) + .with_value(data) + .allowed_destination(Locality::Remote) + .res_async() + .await; + } + Err(e) => { + return Err(e); } - Err(e) => { return Err(e); } } } -// PRIVATE + // PRIVATE async fn make_keyexpr(&self, key_expr: &str) -> ZResult> { debug!("Create optimized keyexpr {}", key_expr); - - self.session.declare_keyexpr(key_expr.to_string()).res_async().await + + self.session + .declare_keyexpr(key_expr.to_string()) + .res_async() + .await } -} \ No newline at end of file +} diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs index be5f4f2..997b730 100644 --- a/zplugin-ros1/tests/aloha_declaration_test.rs +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -12,11 +12,16 @@ // ZettaScale Zenoh Team, // -use std::{sync::{Arc, Mutex, atomic::AtomicUsize}, time::Duration, collections::HashSet, str::FromStr}; +use std::{ + collections::HashSet, + str::FromStr, + sync::{atomic::AtomicUsize, Arc, Mutex}, + time::Duration, +}; use async_std::prelude::FutureExt; use serial_test::serial; -use zenoh::{Session, OpenBuilder, plugins::ZResult}; +use zenoh::{plugins::ZResult, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; use zplugin_ros1::ros_to_zenoh_bridge::{aloha_declaration, aloha_subscription}; @@ -26,29 +31,40 @@ fn session_builder() -> OpenBuilder { return zenoh::open(zenoh::config::peer()); } -fn declaration_builder<'a>(session: Arc, beacon_period: Duration) -> aloha_declaration::AlohaDeclaration { +fn declaration_builder<'a>( + session: Arc, + beacon_period: Duration, +) -> aloha_declaration::AlohaDeclaration { return aloha_declaration::AlohaDeclaration::new( session, zenoh::key_expr::OwnedKeyExpr::from_str("key").unwrap(), - beacon_period); + beacon_period, + ); } -fn subscription_builder(session: Arc, beacon_period: Duration) -> aloha_subscription::AlohaSubscriptionBuilder { +fn subscription_builder( + session: Arc, + beacon_period: Duration, +) -> aloha_subscription::AlohaSubscriptionBuilder { return aloha_subscription::AlohaSubscriptionBuilder::new( session, zenoh::key_expr::OwnedKeyExpr::from_str("key").unwrap(), - beacon_period); + beacon_period, + ); } fn make_session() -> Arc { return session_builder().res_sync().unwrap().into_arc(); } -fn make_subscription(session: Arc, beacon_period: Duration) -> aloha_subscription::AlohaSubscription { - return async_std::task::block_on(subscription_builder(session, beacon_period).build()).unwrap(); +fn make_subscription( + session: Arc, + beacon_period: Duration, +) -> aloha_subscription::AlohaSubscription { + return async_std::task::block_on(subscription_builder(session, beacon_period).build()) + .unwrap(); } - #[test] #[serial(ROS1)] fn discovery_instantination_one_instance() { @@ -74,32 +90,32 @@ fn discovery_instantination_many_instances() { } } - - - - - - - pub struct PPCMeasurement<'a> { _subscriber: zenoh::subscriber::Subscriber<'a, ()>, ppc: Arc, - measurement_period: Duration + measurement_period: Duration, } impl<'a> PPCMeasurement<'a> { - pub async fn new(session: &'a Session, - key: String, - measurement_period: Duration) -> ZResult> - { + pub async fn new( + session: &'a Session, + key: String, + measurement_period: Duration, + ) -> ZResult> { let ppc = Arc::new(AtomicUsize::new(0)); let p = ppc.clone(); - let subscriber = session.declare_subscriber(key) - .callback(move |_val| { - p.fetch_add(1, std::sync::atomic::Ordering::SeqCst); - }) - .res_async().await?; - - return Ok(Self{_subscriber: subscriber, ppc, measurement_period}); + let subscriber = session + .declare_subscriber(key) + .callback(move |_val| { + p.fetch_add(1, std::sync::atomic::Ordering::SeqCst); + }) + .res_async() + .await?; + + return Ok(Self { + _subscriber: subscriber, + ppc, + measurement_period, + }); } pub async fn measure_ppc(&self) -> usize { @@ -109,97 +125,93 @@ impl<'a> PPCMeasurement<'a> { } } - - - - - - - - struct DeclarationCollector { resources: Arc>>>, to_be_declared: Arc>>>, - to_be_undeclared: Arc>>> + to_be_undeclared: Arc>>>, } impl DeclarationCollector { - fn new() -> Self { + fn new() -> Self { return Self { resources: Arc::new(Mutex::new(HashSet::new())), to_be_declared: Arc::new(Mutex::new(HashSet::new())), - to_be_undeclared: Arc::new(Mutex::new(HashSet::new())) + to_be_undeclared: Arc::new(Mutex::new(HashSet::new())), }; } - pub fn use_builder<'b>(&self, mut builder: aloha_subscription::AlohaSubscriptionBuilder) -> aloha_subscription::AlohaSubscriptionBuilder { - + pub fn use_builder<'b>( + &self, + mut builder: aloha_subscription::AlohaSubscriptionBuilder, + ) -> aloha_subscription::AlohaSubscriptionBuilder { let r = self.resources.clone(); let r2 = r.clone(); let declared = self.to_be_declared.clone(); let undeclared = self.to_be_undeclared.clone(); - + builder = builder - .on_resource_declared(move |k| { - assert!(declared.lock().unwrap().remove(&k.clone().into_owned())); - assert!(r.lock().unwrap().insert(k.into_owned())); - Box::new(Box::pin(async {})) - }) - .on_resource_undeclared(move |k| { - assert!(undeclared.lock().unwrap().remove(&k.clone().into_owned())); - assert!(r2.lock().unwrap().remove(&k.into_owned())); - Box::new(Box::pin(async move {})) - }); + .on_resource_declared(move |k| { + assert!(declared.lock().unwrap().remove(&k.clone().into_owned())); + assert!(r.lock().unwrap().insert(k.into_owned())); + Box::new(Box::pin(async {})) + }) + .on_resource_undeclared(move |k| { + assert!(undeclared.lock().unwrap().remove(&k.clone().into_owned())); + assert!(r2.lock().unwrap().remove(&k.into_owned())); + Box::new(Box::pin(async move {})) + }); return builder; } - pub fn arm(&mut self, - declared: HashSet>, - undeclared: HashSet>) { + pub fn arm( + &mut self, + declared: HashSet>, + undeclared: HashSet>, + ) { *self.to_be_declared.lock().unwrap() = declared; *self.to_be_undeclared.lock().unwrap() = undeclared; } pub async fn wait(&self, expected: HashSet>) { - while !self.to_be_declared.lock().unwrap().is_empty() || - !self.to_be_undeclared.lock().unwrap().is_empty() || - expected != *self.resources.lock().unwrap() + while !self.to_be_declared.lock().unwrap().is_empty() + || !self.to_be_undeclared.lock().unwrap().is_empty() + || expected != *self.resources.lock().unwrap() { async_std::task::sleep(core::time::Duration::from_millis(1)).await; } } } - - - #[derive(Default)] struct State { - pub declarators_count: usize + pub declarators_count: usize, } impl State { - pub fn declarators(mut self, declarators_count: usize) -> Self { + pub fn declarators(mut self, declarators_count: usize) -> Self { self.declarators_count = declarators_count; - self + self } } -async fn test_state_transition<'a>(beacon_period: Duration, - declaring_sessions: &mut Vec>, - declarations: &mut Vec, - collector: &mut DeclarationCollector, - ppc_measurer: &'a PPCMeasurement<'a>, - state: &State) { - - let ke =zenoh::key_expr::KeyExpr::from_str("key").unwrap().into_owned(); +async fn test_state_transition<'a>( + beacon_period: Duration, + declaring_sessions: &mut Vec>, + declarations: &mut Vec, + collector: &mut DeclarationCollector, + ppc_measurer: &'a PPCMeasurement<'a>, + state: &State, +) { + let ke = zenoh::key_expr::KeyExpr::from_str("key") + .unwrap() + .into_owned(); let mut result: HashSet = HashSet::new(); let mut undeclared: HashSet = HashSet::new(); let mut declared: HashSet = HashSet::new(); - + match (declarations.len(), state.declarators_count) { - (0,0) => {} + (0, 0) => {} (0, _) => { result.insert(ke.clone()); declared.insert(ke.clone()); @@ -211,7 +223,7 @@ async fn test_state_transition<'a>(beacon_period: Duration, result.insert(ke.clone()); } } - + collector.arm(declared, undeclared); while declarations.len() > state.declarators_count { @@ -222,12 +234,21 @@ async fn test_state_transition<'a>(beacon_period: Duration, if declaring_sessions.len() <= declarations.len() { declaring_sessions.push(session_builder().res_async().await.unwrap().into_arc()); } - declarations.push(declaration_builder(declaring_sessions[declarations.len()].clone(), beacon_period)); + declarations.push(declaration_builder( + declaring_sessions[declarations.len()].clone(), + beacon_period, + )); } collector.wait(result).await; async_std::task::sleep(beacon_period).await; - while ppc_measurer.measure_ppc().await != { let mut res = 1; if state.declarators_count == 0 { res=0; } res} {} + while ppc_measurer.measure_ppc().await != { + let mut res = 1; + if state.declarators_count == 0 { + res = 0; + } + res + } {} } async fn run_aloha(beacon_period: Duration, scenario: Vec) { @@ -236,82 +257,105 @@ async fn run_aloha(beacon_period: Duration, scenario: Vec) { let mut collector = DeclarationCollector::new(); let subscription_session = session_builder().res_async().await.unwrap().into_arc(); - let _subscriber = collector.use_builder(subscription_builder(subscription_session.clone(), beacon_period)).build().await.unwrap(); - let ppc_measurer = PPCMeasurement::new(&subscription_session, - "key".to_string(), - Duration::from_millis(100)).await.unwrap(); + let _subscriber = collector + .use_builder(subscription_builder( + subscription_session.clone(), + beacon_period, + )) + .build() + .await + .unwrap(); + let ppc_measurer = PPCMeasurement::new( + &subscription_session, + "key".to_string(), + Duration::from_millis(100), + ) + .await + .unwrap(); for scene in scenario { println!("Transiting State: {}", scene.declarators_count); - test_state_transition(beacon_period, - &mut declaring_sessions, - &mut declarations, - &mut collector, - &ppc_measurer, - &scene).timeout(TIMEOUT).await.expect("Timeout waiting state transition!"); + test_state_transition( + beacon_period, + &mut declaring_sessions, + &mut declarations, + &mut collector, + &ppc_measurer, + &scene, + ) + .timeout(TIMEOUT) + .await + .expect("Timeout waiting state transition!"); } } - #[test] #[serial(Aloha)] fn aloha_declare_one() { - async_std::task::block_on(run_aloha( Duration::from_millis(100), - [ - State::default().declarators(1) - ].into_iter().collect() + async_std::task::block_on(run_aloha( + Duration::from_millis(100), + [State::default().declarators(1)].into_iter().collect(), )); } #[test] #[serial(Aloha)] fn aloha_declare_many() { - async_std::task::block_on(run_aloha( Duration::from_millis(100), - [ - State::default().declarators(10) - ].into_iter().collect() + async_std::task::block_on(run_aloha( + Duration::from_millis(100), + [State::default().declarators(10)].into_iter().collect(), )); } #[test] #[serial(Aloha)] fn aloha_declare_many_one_many() { - async_std::task::block_on(run_aloha( Duration::from_millis(100), + async_std::task::block_on(run_aloha( + Duration::from_millis(100), [ State::default().declarators(10), State::default().declarators(1), - State::default().declarators(10) - ].into_iter().collect() + State::default().declarators(10), + ] + .into_iter() + .collect(), )); } #[test] #[serial(Aloha)] fn aloha_declare_one_zero_one() { - async_std::task::block_on(run_aloha( Duration::from_millis(100), + async_std::task::block_on(run_aloha( + Duration::from_millis(100), [ State::default().declarators(1), State::default().declarators(0), - State::default().declarators(1) - ].into_iter().collect() + State::default().declarators(1), + ] + .into_iter() + .collect(), )); } #[test] #[serial(Aloha)] fn aloha_declare_many_zero_many() { - async_std::task::block_on(run_aloha( Duration::from_millis(100), + async_std::task::block_on(run_aloha( + Duration::from_millis(100), [ State::default().declarators(10), State::default().declarators(0), - State::default().declarators(10) - ].into_iter().collect() + State::default().declarators(10), + ] + .into_iter() + .collect(), )); } #[test] #[serial(Aloha)] fn aloha_many_scenarios() { - async_std::task::block_on(run_aloha( Duration::from_millis(100), + async_std::task::block_on(run_aloha( + Duration::from_millis(100), [ State::default().declarators(1), State::default().declarators(10), @@ -326,6 +370,8 @@ fn aloha_many_scenarios() { State::default().declarators(0), State::default().declarators(10), State::default().declarators(1), - ].into_iter().collect() + ] + .into_iter() + .collect(), )); } diff --git a/zplugin-ros1/tests/discovery_test.rs b/zplugin-ros1/tests/discovery_test.rs index 61a051d..4de0589 100644 --- a/zplugin-ros1/tests/discovery_test.rs +++ b/zplugin-ros1/tests/discovery_test.rs @@ -12,37 +12,38 @@ // ZettaScale Zenoh Team, // -use std::{sync::{Arc, Mutex}, time::Duration}; +use std::{ + sync::{Arc, Mutex}, + time::Duration, +}; use async_std::prelude::FutureExt; +use multiset::HashMultiSet; use serial_test::serial; -use zenoh::{Session, OpenBuilder, config::ModeDependentValue}; +use zenoh::{config::ModeDependentValue, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; use zplugin_ros1::ros_to_zenoh_bridge::discovery; -use multiset::HashMultiSet; const TIMEOUT: Duration = Duration::from_secs(10); fn session_builder() -> OpenBuilder { let mut config = zenoh::config::peer(); - config.timestamping.set_enabled(Some(ModeDependentValue::Unique(true))).unwrap(); + config + .timestamping + .set_enabled(Some(ModeDependentValue::Unique(true))) + .unwrap(); return zenoh::open(config); } -fn discovery_builder(session: Arc) -> discovery::DiscoveryBuilder -{ - return discovery::DiscoveryBuilder::new( - "*".to_string(), - "*".to_string(), - session); +fn discovery_builder(session: Arc) -> discovery::DiscoveryBuilder { + return discovery::DiscoveryBuilder::new("*".to_string(), "*".to_string(), session); } fn make_session() -> Arc { session_builder().res_sync().unwrap().into_arc() } -fn make_discovery(session: Arc) -> discovery::Discovery -{ +fn make_discovery(session: Arc) -> discovery::Discovery { return async_std::task::block_on(discovery_builder(session).build()); } @@ -67,23 +68,26 @@ fn discovery_instantination_many_instances() { } } - - struct DiscoveryCollector { publishers: Arc>>, subscribers: Arc>>, services: Arc>>, - clients: Arc>> + clients: Arc>>, } impl DiscoveryCollector { - fn new() -> Self { - return Self { publishers: Arc::new(Mutex::new(HashMultiSet::new())), - subscribers: Arc::new(Mutex::new(HashMultiSet::new())), - services: Arc::new(Mutex::new(HashMultiSet::new())), - clients: Arc::new(Mutex::new(HashMultiSet::new())) }; + fn new() -> Self { + return Self { + publishers: Arc::new(Mutex::new(HashMultiSet::new())), + subscribers: Arc::new(Mutex::new(HashMultiSet::new())), + services: Arc::new(Mutex::new(HashMultiSet::new())), + clients: Arc::new(Mutex::new(HashMultiSet::new())), + }; } - pub fn use_builder<'a>(&self, mut builder: discovery::DiscoveryBuilder) -> discovery::DiscoveryBuilder { + pub fn use_builder<'a>( + &self, + mut builder: discovery::DiscoveryBuilder, + ) -> discovery::DiscoveryBuilder { let p = self.publishers.clone(); let s = self.subscribers.clone(); let srv = self.services.clone(); @@ -95,26 +99,26 @@ impl DiscoveryCollector { let cli1 = self.clients.clone(); builder - .on_discovered(move |b_type, topic| { - let container = match b_type { - zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Publisher => { &p }, - zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Subscriber => { &s }, - zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Service => { &srv }, - zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Client => { &cli }, - }; - container.lock().unwrap().insert(topic); - Box::new(Box::pin(async {})) - }) - .on_lost(move |b_type, topic| { - let container = match b_type { - zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Publisher => { &p1 }, - zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Subscriber => { &s1 }, - zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Service => { &srv1 }, - zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Client => { &cli1 }, - }; - container.lock().unwrap().remove(&topic); - Box::new(Box::pin(async {})) - }); + .on_discovered(move |b_type, topic| { + let container = match b_type { + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Publisher => &p, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Subscriber => &s, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Service => &srv, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Client => &cli, + }; + container.lock().unwrap().insert(topic); + Box::new(Box::pin(async {})) + }) + .on_lost(move |b_type, topic| { + let container = match b_type { + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Publisher => &p1, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Subscriber => &s1, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Service => &srv1, + zplugin_ros1::ros_to_zenoh_bridge::bridge_type::BridgeType::Client => &cli1, + }; + container.lock().unwrap().remove(&topic); + Box::new(Box::pin(async {})) + }); return builder; } @@ -135,22 +139,29 @@ impl DiscoveryCollector { Self::wait(&self.clients, expected).await; } -// PRIVATE: - async fn wait(container: &Arc>>, - expected: HashMultiSet) { + // PRIVATE: + async fn wait( + container: &Arc>>, + expected: HashMultiSet, + ) { while expected != *container.lock().unwrap() { async_std::task::sleep(core::time::Duration::from_millis(1)).await; } } } - - -async fn generate_topics(count: usize, duplication: usize, stage: usize) -> HashMultiSet { +async fn generate_topics( + count: usize, + duplication: usize, + stage: usize, +) -> HashMultiSet { let mut result = HashMultiSet::new(); for number in 0..count { for _dup in 0..duplication { - result.insert(rosrust::api::Topic{name: format!("name_{}_{}",number, stage), datatype: "some".to_string()}); + result.insert(rosrust::api::Topic { + name: format!("name_{}_{}", number, stage), + datatype: "some".to_string(), + }); } } return result; @@ -158,72 +169,103 @@ async fn generate_topics(count: usize, duplication: usize, stage: usize) -> Hash #[derive(Default)] struct State { - publishers_count: usize, publishers_duplication: usize, - subscribers_count: usize, subscribers_duplication: usize, - services_count: usize, services_duplication: usize, - clients_count: usize, clients_duplication: usize, - stage: usize + publishers_count: usize, + publishers_duplication: usize, + subscribers_count: usize, + subscribers_duplication: usize, + services_count: usize, + services_duplication: usize, + clients_count: usize, + clients_duplication: usize, + stage: usize, } impl State { - pub fn publishers(mut self, publishers_count: usize, publishers_duplication: usize) -> Self { + pub fn publishers(mut self, publishers_count: usize, publishers_duplication: usize) -> Self { self.publishers_count = publishers_count; self.publishers_duplication = publishers_duplication; - self + self } - pub fn subscribers(mut self, subscribers_count: usize, subscribers_duplication: usize) -> Self { + pub fn subscribers(mut self, subscribers_count: usize, subscribers_duplication: usize) -> Self { self.subscribers_count = subscribers_count; self.subscribers_duplication = subscribers_duplication; - self + self } - pub fn services(mut self, services_count: usize, services_duplication: usize) -> Self { + pub fn services(mut self, services_count: usize, services_duplication: usize) -> Self { self.services_count = services_count; self.services_duplication = services_duplication; - self + self } - pub fn clients(mut self, clients_count: usize, clients_duplication: usize) -> Self { + pub fn clients(mut self, clients_count: usize, clients_duplication: usize) -> Self { self.clients_count = clients_count; self.clients_duplication = clients_duplication; - self + self } - //pub fn stage(mut self, stage: usize) -> Self { + //pub fn stage(mut self, stage: usize) -> Self { // self.stage = stage; - // self + // self //} - async fn summarize(&self) -> (HashMultiSet, - HashMultiSet, - HashMultiSet, - HashMultiSet) - { + async fn summarize( + &self, + ) -> ( + HashMultiSet, + HashMultiSet, + HashMultiSet, + HashMultiSet, + ) { return ( - generate_topics(self.publishers_count, self.publishers_duplication, self.stage).await, - generate_topics(self.subscribers_count, self.subscribers_duplication, self.stage).await, + generate_topics( + self.publishers_count, + self.publishers_duplication, + self.stage, + ) + .await, + generate_topics( + self.subscribers_count, + self.subscribers_duplication, + self.stage, + ) + .await, generate_topics(self.services_count, self.services_duplication, self.stage).await, generate_topics(self.clients_count, self.clients_duplication, self.stage).await, ); } } -async fn test_state_transition(src_discovery: &discovery::Discovery, - rcv: &DiscoveryCollector, - state: &State) { - let (publishers, - subscribers, - services, - clients) = state.summarize().await; +async fn test_state_transition( + src_discovery: &discovery::Discovery, + rcv: &DiscoveryCollector, + state: &State, +) { + let (publishers, subscribers, services, clients) = state.summarize().await; let mut _pub_entities = Vec::new(); for publisher in publishers.iter() { - _pub_entities.push(src_discovery.local_resources().declare_publisher(publisher).await); + _pub_entities.push( + src_discovery + .local_resources() + .declare_publisher(publisher) + .await, + ); } let mut _sub_entities = Vec::new(); for subscriber in subscribers.iter() { - _sub_entities.push(src_discovery.local_resources().declare_subscriber(subscriber).await); + _sub_entities.push( + src_discovery + .local_resources() + .declare_subscriber(subscriber) + .await, + ); } let mut _srv_entities = Vec::new(); for service in services.iter() { - _srv_entities.push(src_discovery.local_resources().declare_service(service).await); + _srv_entities.push( + src_discovery + .local_resources() + .declare_service(service) + .await, + ); } let mut _cl_entities = Vec::new(); @@ -243,101 +285,135 @@ async fn run_discovery(scenario: Vec) { let rcv = DiscoveryCollector::new(); let rcv_session = session_builder().res_async().await.unwrap().into_arc(); - let _rcv_discovery = rcv.use_builder(discovery_builder(rcv_session)).build().await; + let _rcv_discovery = rcv + .use_builder(discovery_builder(rcv_session)) + .build() + .await; for scene in scenario { - test_state_transition(&src_discovery, - &rcv, - &scene).timeout(TIMEOUT).await.expect("Timeout waiting state transition!"); + test_state_transition(&src_discovery, &rcv, &scene) + .timeout(TIMEOUT) + .await + .expect("Timeout waiting state transition!"); } } #[test] #[serial(ROS1)] fn discover_single_publisher() { - async_std::task::block_on(run_discovery([State::default().publishers(1, 1)].into_iter().collect())); + async_std::task::block_on(run_discovery( + [State::default().publishers(1, 1)].into_iter().collect(), + )); } #[test] #[serial(ROS1)] fn discover_single_subscriber() { - async_std::task::block_on(run_discovery([State::default().subscribers(1, 1)].into_iter().collect())); + async_std::task::block_on(run_discovery( + [State::default().subscribers(1, 1)].into_iter().collect(), + )); } #[test] #[serial(ROS1)] fn discover_single_service() { - async_std::task::block_on(run_discovery([State::default().services(1, 1)].into_iter().collect())); + async_std::task::block_on(run_discovery( + [State::default().services(1, 1)].into_iter().collect(), + )); } #[test] #[serial(ROS1)] fn discover_single_client() { - async_std::task::block_on(run_discovery([State::default().clients(1, 1)].into_iter().collect())); + async_std::task::block_on(run_discovery( + [State::default().clients(1, 1)].into_iter().collect(), + )); } #[test] #[serial(ROS1)] fn discover_single_transition() { - async_std::task::block_on(run_discovery([ - State::default().publishers(1, 1), - State::default().subscribers(1, 1), - State::default().services(1, 1), - State::default().clients(1, 1), - ].into_iter().collect())); + async_std::task::block_on(run_discovery( + [ + State::default().publishers(1, 1), + State::default().subscribers(1, 1), + State::default().services(1, 1), + State::default().clients(1, 1), + ] + .into_iter() + .collect(), + )); } #[test] #[serial(ROS1)] fn discover_single_transition_with_zero_state() { - async_std::task::block_on(run_discovery([ - State::default().publishers(1, 1), - State::default(), - State::default().subscribers(1, 1), - State::default(), - State::default().services(1, 1), - State::default(), - State::default().clients(1, 1), - ].into_iter().collect())); + async_std::task::block_on(run_discovery( + [ + State::default().publishers(1, 1), + State::default(), + State::default().subscribers(1, 1), + State::default(), + State::default().services(1, 1), + State::default(), + State::default().clients(1, 1), + ] + .into_iter() + .collect(), + )); } - - #[test] #[serial(ROS1)] fn discover_multiple_publishers() { - async_std::task::block_on(run_discovery([State::default().publishers(100, 1)].into_iter().collect())); + async_std::task::block_on(run_discovery( + [State::default().publishers(100, 1)].into_iter().collect(), + )); } #[test] #[serial(ROS1)] fn discover_multiple_subscribers() { - async_std::task::block_on(run_discovery([State::default().subscribers(100, 1)].into_iter().collect())); + async_std::task::block_on(run_discovery( + [State::default().subscribers(100, 1)].into_iter().collect(), + )); } #[test] #[serial(ROS1)] fn discover_multiple_services() { - async_std::task::block_on(run_discovery([State::default().services(100, 1)].into_iter().collect())); + async_std::task::block_on(run_discovery( + [State::default().services(100, 1)].into_iter().collect(), + )); } #[test] #[serial(ROS1)] fn discover_multiple_clients() { - async_std::task::block_on(run_discovery([State::default().clients(100, 1)].into_iter().collect())); + async_std::task::block_on(run_discovery( + [State::default().clients(100, 1)].into_iter().collect(), + )); } #[test] #[serial(ROS1)] fn discover_multiple_transition() { - async_std::task::block_on(run_discovery([ - State::default().publishers(100, 1), - State::default().subscribers(100, 1), - State::default().services(100, 1), - State::default().clients(100, 1), - ].into_iter().collect())); + async_std::task::block_on(run_discovery( + [ + State::default().publishers(100, 1), + State::default().subscribers(100, 1), + State::default().services(100, 1), + State::default().clients(100, 1), + ] + .into_iter() + .collect(), + )); } #[test] #[serial(ROS1)] fn discover_multiple_transition_with_zero_state() { - async_std::task::block_on(run_discovery([ - State::default().publishers(100, 1), - State::default(), - State::default().subscribers(100, 1), - State::default(), - State::default().services(100, 1), - State::default(), - State::default().clients(100, 1), - ].into_iter().collect())); -} \ No newline at end of file + async_std::task::block_on(run_discovery( + [ + State::default().publishers(100, 1), + State::default(), + State::default().subscribers(100, 1), + State::default(), + State::default().services(100, 1), + State::default(), + State::default().clients(100, 1), + ] + .into_iter() + .collect(), + )); +} diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 16af627..04488d1 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -13,23 +13,26 @@ // use strum_macros::Display; -use zenoh::{prelude::SplitBuffer}; +use zenoh::prelude::SplitBuffer; use zenoh_core::SyncResolve; use std::{ + collections::HashSet, str::FromStr, sync::{ - atomic::{AtomicBool, Ordering::*, AtomicU64}, + atomic::{AtomicBool, AtomicU64, Ordering::*}, Arc, Mutex, RwLock, - }, collections::HashSet, + }, }; -use zplugin_ros1::ros_to_zenoh_bridge::ros1_to_zenoh_bridge_impl::{BridgeStatus, RosStatus, work_cycle}; use zplugin_ros1::ros_to_zenoh_bridge::discovery::{LocalResource, LocalResources}; +use zplugin_ros1::ros_to_zenoh_bridge::ros1_to_zenoh_bridge_impl::{ + work_cycle, BridgeStatus, RosStatus, +}; use zplugin_ros1::ros_to_zenoh_bridge::{ros1_client, zenoh_client}; use log::{debug, error}; -use rosrust::{RawMessage, Client, Duration}; +use rosrust::{Client, Duration, RawMessage}; use std::sync::atomic::AtomicUsize; use zenoh::prelude::r#async::*; use zenoh_core; @@ -49,7 +52,8 @@ where for i in 0..cycles { async_std::task::sleep(core::time::Duration::from_millis( millis.try_into().unwrap(), - )).await; + )) + .await; if waiter() { return true; } @@ -109,27 +113,30 @@ impl RunningBridge { self.assert_status(RosStatus::Ok).await; } pub async fn assert_status(&self, status: RosStatus) { - assert!(self.wait_ros_status(status, core::time::Duration::from_secs(10)).await); + assert!( + self.wait_ros_status(status, core::time::Duration::from_secs(10)) + .await + ); } - pub async fn wait_ros_status( - &self, - status: RosStatus, - timeout: core::time::Duration, - ) -> bool { + pub async fn wait_ros_status(&self, status: RosStatus, timeout: core::time::Duration) -> bool { return wait( move || { let val = self.ros_status.lock().unwrap(); return *val == status; }, timeout, - ).await; + ) + .await; } pub async fn assert_bridge_empy(&self) { self.assert_bridge_status(|| BridgeStatus::default()).await; } pub async fn assert_bridge_status BridgeStatus>(&self, status: F) { - assert!(self.wait_bridge_status(status, core::time::Duration::from_secs(120)).await); + assert!( + self.wait_bridge_status(status, core::time::Duration::from_secs(120)) + .await + ); } pub async fn wait_bridge_status BridgeStatus>( &self, @@ -142,7 +149,8 @@ impl RunningBridge { return *val == (status)(); }, timeout, - ).await; + ) + .await; } } impl Drop for RunningBridge { @@ -275,21 +283,21 @@ impl ZenohQuery { cycles: Arc, ) { while running.load(Relaxed) { - let query = inner.make_zenoh_query_sync(key.as_str(), data.clone()).await; + let query = inner + .make_zenoh_query_sync(key.as_str(), data.clone()) + .await; match query.recv_async().await { - Ok(reply) => { - match reply.sample { - Ok(value) => { - let returned_data = value.payload.contiguous().to_vec(); - assert!(data.eq(&returned_data)); - cycles.fetch_add(1, SeqCst); - } - Err(e) => { - error!("ZenohQuery: got reply with error: {}", e); - break; - } + Ok(reply) => match reply.sample { + Ok(value) => { + let returned_data = value.payload.contiguous().to_vec(); + assert!(data.eq(&returned_data)); + cycles.fetch_add(1, SeqCst); } - } + Err(e) => { + error!("ZenohQuery: got reply with error: {}", e); + break; + } + }, Err(e) => { error!("ZenohQuery: failed to get reply with error: {}", e); break; @@ -307,7 +315,7 @@ impl Drop for ZenohQuery { struct ROS1Client { running: Arc, cycles: Arc, - ros1_client: Arc>> + ros1_client: Arc>>, } impl ROS1Client { fn new(inner: Arc, key: String, cycles: Arc) -> Self { @@ -316,7 +324,7 @@ impl ROS1Client { return Self { running, cycles, - ros1_client + ros1_client, }; } @@ -324,22 +332,23 @@ impl ROS1Client { running: Arc, data: Vec, cycles: Arc, - ros1_client: Arc>>) { - - ros1_client.data.probe(Duration::from_seconds(10).into() ).unwrap(); + ros1_client: Arc>>, + ) { + ros1_client + .data + .probe(Duration::from_seconds(10).into()) + .unwrap(); while running.load(Relaxed) { match ros1_client.data.req(&RawMessage(data.clone())) { - Ok(reply) => { - match reply { - Ok(msg) => { - assert!(data.eq(&msg.0)); - cycles.fetch_add(1, SeqCst); - }, - Err(e) => { - error!("ROS1Client: got reply with error: {}", e); - break; - } + Ok(reply) => match reply { + Ok(msg) => { + assert!(data.eq(&msg.0)); + cycles.fetch_add(1, SeqCst); + } + Err(e) => { + error!("ROS1Client: got reply with error: {}", e); + break; } }, Err(e) => { @@ -365,13 +374,15 @@ trait Publisher { impl Publisher for ZenohPublisher { fn put(&self, data: Vec) { let inner = self.inner.clone(); - async_std::task::spawn_blocking(move || inner.put(data).res_sync().unwrap() ); + async_std::task::spawn_blocking(move || inner.put(data).res_sync().unwrap()); } } impl Publisher for ROS1Publisher { fn put(&self, data: Vec) { let inner = self.inner.clone(); - async_std::task::spawn_blocking( move || inner.data.send(rosrust::RawMessage(data)).unwrap() ); + async_std::task::spawn_blocking(move || { + inner.data.send(rosrust::RawMessage(data)).unwrap() + }); } fn ready(&self) -> bool { @@ -395,12 +406,7 @@ impl Publisher for ROS1Client { let cycles = self.cycles.clone(); let ros1_client = self.ros1_client.clone(); - async_std::task::spawn_blocking(|| Self::query_loop( - running, - data, - cycles, - ros1_client - )); + async_std::task::spawn_blocking(|| Self::query_loop(running, data, cycles, ros1_client)); } } @@ -431,13 +437,11 @@ impl Subscriber for ROS1Subscriber { } impl Subscriber for ROS1Service {} - - struct PubSub { key: String, publisher: Box, subscriber: Box, - discovery_resource: LocalResource + discovery_resource: LocalResource, } struct PingPong { @@ -451,11 +455,11 @@ impl PingPong { self.check_pps(pps_measurements).await; } -// PRIVATE: + // PRIVATE: async fn new_ros1_to_zenoh_service(key: &str, backend: Arc) -> PingPong { let cycles = Arc::new(AtomicUsize::new(0)); - let ros1_service = backend - .make_ros_service(key, |q| -> rosrust::ServiceResult { + let ros1_service = + backend.make_ros_service(key, |q| -> rosrust::ServiceResult { debug!( "PingPong: got query of {} bytes from Zenoh to ROS1!", q.0.len() @@ -463,7 +467,10 @@ impl PingPong { Ok(q) // echo the request back! }); - let discovery_resource = backend.local_resources.declare_client(&BridgeChecker::make_topic(key)).await; + let discovery_resource = backend + .local_resources + .declare_client(&BridgeChecker::make_topic(key)) + .await; let zenoh_query = ZenohQuery::new(backend, key.to_string(), cycles.clone()); return PingPong { @@ -482,20 +489,27 @@ impl PingPong { async fn new_ros1_to_zenoh_client(key: &str, backend: Arc) -> PingPong { let cycles = Arc::new(AtomicUsize::new(0)); - let discovery_resource = backend.local_resources.declare_service(&BridgeChecker::make_topic(key)).await; - let zenoh_queryable = backend.make_zenoh_queryable(key, |q| { - async_std::task::spawn( async move { - let key = q.key_expr().clone(); - let val = q.value().unwrap().clone(); - q.reply(Ok(Sample::new(key, val))).res_async().await; - }); - }).await; + let discovery_resource = backend + .local_resources + .declare_service(&BridgeChecker::make_topic(key)) + .await; + let zenoh_queryable = backend + .make_zenoh_queryable(key, |q| { + async_std::task::spawn(async move { + let key = q.key_expr().clone(); + let val = q.value().unwrap().clone(); + q.reply(Ok(Sample::new(key, val))).res_async().await; + }); + }) + .await; return PingPong { pub_sub: PubSub { key: key.to_string(), publisher: Box::new(ROS1Client::new(backend, key.to_string(), cycles.clone())), - subscriber: Box::new(ZenohQueryable{ inner: zenoh_queryable }) , + subscriber: Box::new(ZenohQueryable { + inner: zenoh_queryable, + }), discovery_resource, }, cycles, @@ -506,7 +520,10 @@ impl PingPong { let cycles = Arc::new(AtomicUsize::new(0)); let ros1_pub = Arc::new(backend.make_ros_publisher(key)); - let discovery_resource = backend.local_resources.declare_subscriber(&BridgeChecker::make_topic(key)).await; + let discovery_resource = backend + .local_resources + .declare_subscriber(&BridgeChecker::make_topic(key)) + .await; let c = cycles.clone(); let rpub = ros1_pub.clone(); @@ -527,7 +544,7 @@ impl PingPong { key: key.to_string(), publisher: Box::new(ROS1Publisher { inner: ros1_pub }), subscriber: Box::new(ZenohSubscriber { inner: zenoh_sub }), - discovery_resource + discovery_resource, }, cycles, }; @@ -537,7 +554,10 @@ impl PingPong { let cycles = Arc::new(AtomicUsize::new(0)); let zenoh_pub = Arc::new(backend.make_zenoh_publisher(key).await); - let discovery_resource = backend.local_resources.declare_publisher(&BridgeChecker::make_topic(key)).await; + let discovery_resource = backend + .local_resources + .declare_publisher(&BridgeChecker::make_topic(key)) + .await; let c = cycles.clone(); let zpub = zenoh_pub.clone(); @@ -555,7 +575,7 @@ impl PingPong { key: key.to_string(), publisher: Box::new(ZenohPublisher { inner: zenoh_pub }), subscriber: Box::new(ROS1Subscriber { inner: ros1_sub }), - discovery_resource + discovery_resource, }, cycles, }; @@ -603,9 +623,15 @@ impl PingPong { } async fn wait_for_pub_sub_ready(&self) { - assert!(Self::wait(move || { - return self.pub_sub.publisher.ready() && self.pub_sub.subscriber.ready(); - }, core::time::Duration::from_secs(30)).await); + assert!( + Self::wait( + move || { + return self.pub_sub.publisher.ready() && self.pub_sub.subscriber.ready(); + }, + core::time::Duration::from_secs(30) + ) + .await + ); async_std::task::sleep(time::Duration::from_secs(1)).await; } @@ -619,7 +645,8 @@ impl PingPong { for i in 0..cycles { async_std::task::sleep(core::time::Duration::from_micros( micros.try_into().unwrap(), - )).await; + )) + .await; if waiter() { return true; } @@ -644,7 +671,7 @@ impl ROSEnvironment { kill_master.wait().unwrap(); return ROSEnvironment { rosmaster: None }; - } + } pub fn with_master(mut self) -> Self { assert!(self.rosmaster.is_none()); @@ -718,7 +745,9 @@ impl TestEnvironment { } pub async fn assert_bridge_status_synchronized(&self) { - self.bridge.assert_bridge_status(|| self.checker.expected_bridge_status.read().unwrap().clone()).await; + self.bridge + .assert_bridge_status(|| self.checker.expected_bridge_status.read().unwrap().clone()) + .await; } // PRIVATE @@ -740,28 +769,31 @@ impl TestEnvironment { } struct RAIICounter -where - T: Sized +where + T: Sized, { pub data: T, - on_destroy: Box + on_destroy: Box, } impl RAIICounter where - T: Sized + T: Sized, { fn new(data: T, on_destroy: F) -> Self where - F: Fn() + Sync + Send + 'static + F: Fn() + Sync + Send + 'static, { - Self { data, on_destroy: Box::new(on_destroy) } + Self { + data, + on_destroy: Box::new(on_destroy), + } } } impl Drop for RAIICounter where - T: Sized + T: Sized, { fn drop(&mut self) { (self.on_destroy)(); @@ -778,10 +810,7 @@ struct BridgeChecker { impl BridgeChecker { // PUBLIC pub fn new() -> BridgeChecker { - let session = zenoh::open(config::peer()) - .res_sync() - .unwrap() - .into_arc(); + let session = zenoh::open(config::peer()).res_sync().unwrap().into_arc(); return BridgeChecker { ros_client: ros1_client::Ros1Client::new("test_ros_node"), zenoh_client: zenoh_client::ZenohClient::new(session.clone()), @@ -859,17 +888,19 @@ impl BridgeChecker { let status = self.expected_bridge_status.clone(); status.write().unwrap().ros_publishers.0 += 1; status.write().unwrap().ros_publishers.1 += 1; - return RAIICounter::new(self.ros_client.publish(&Self::make_topic(name)).unwrap(), - move || { - status.write().unwrap().ros_publishers.0 -= 1; - status.write().unwrap().ros_publishers.1 -= 1; - }); + return RAIICounter::new( + self.ros_client.publish(&Self::make_topic(name)).unwrap(), + move || { + status.write().unwrap().ros_publishers.0 -= 1; + status.write().unwrap().ros_publishers.1 -= 1; + }, + ); } pub fn make_ros_subscriber( - &self, - name: &str, - callback: F + &self, + name: &str, + callback: F, ) -> RAIICounter where T: rosrust::Message, @@ -878,34 +909,31 @@ impl BridgeChecker { let status = self.expected_bridge_status.clone(); status.write().unwrap().ros_subscribers.0 += 1; status.write().unwrap().ros_subscribers.1 += 1; - return RAIICounter::new(self.ros_client.subscribe(&Self::make_topic(name), callback) - .unwrap(), - move || { - status.write().unwrap().ros_subscribers.0 -= 1; - status.write().unwrap().ros_subscribers.1 -= 1; - }); + return RAIICounter::new( + self.ros_client + .subscribe(&Self::make_topic(name), callback) + .unwrap(), + move || { + status.write().unwrap().ros_subscribers.0 -= 1; + status.write().unwrap().ros_subscribers.1 -= 1; + }, + ); } - pub fn make_ros_client( - &self, - name: &str, - ) -> RAIICounter> - { + pub fn make_ros_client(&self, name: &str) -> RAIICounter> { let status = self.expected_bridge_status.clone(); status.write().unwrap().ros_clients.0 += 1; status.write().unwrap().ros_clients.1 += 1; - return RAIICounter::new(self.ros_client.client(&Self::make_topic(name)).unwrap(), - move || { - status.write().unwrap().ros_clients.0 -= 1; - status.write().unwrap().ros_clients.1 -= 1; - }); + return RAIICounter::new( + self.ros_client.client(&Self::make_topic(name)).unwrap(), + move || { + status.write().unwrap().ros_clients.0 -= 1; + status.write().unwrap().ros_clients.1 -= 1; + }, + ); } - pub fn make_ros_service( - &self, - name: &str, - handler: F, - ) -> RAIICounter + pub fn make_ros_service(&self, name: &str, handler: F) -> RAIICounter where F: Fn(rosrust::RawMessage) -> rosrust::ServiceResult + Send @@ -915,11 +943,15 @@ impl BridgeChecker { let status = self.expected_bridge_status.clone(); status.write().unwrap().ros_services.0 += 1; status.write().unwrap().ros_services.1 += 1; - return RAIICounter::new(self.ros_client.service::(&Self::make_topic(name), handler).unwrap(), - move || { - status.write().unwrap().ros_services.0 -= 1; - status.write().unwrap().ros_services.1 -= 1; - }); + return RAIICounter::new( + self.ros_client + .service::(&Self::make_topic(name), handler) + .unwrap(), + move || { + status.write().unwrap().ros_services.0 -= 1; + status.write().unwrap().ros_services.1 -= 1; + }, + ); } // PRIVATE @@ -948,7 +980,9 @@ fn init_with_ros() { let ros_publisher = checker.make_ros_publisher("/some/ros/topic"); async_std::task::block_on(bridge.assert_ros_ok()); - async_std::task::block_on(bridge.assert_bridge_status(|| *checker.expected_bridge_status.read().unwrap())); + async_std::task::block_on( + bridge.assert_bridge_status(|| *checker.expected_bridge_status.read().unwrap()), + ); } #[derive(PartialEq, Eq, Hash, Display)] @@ -958,34 +992,63 @@ enum Mode { Ros1Service, Ros1Client, - FastRun + FastRun, } -static UNIQUE_NUMBER: AtomicU64 = AtomicU64::new(0); +static UNIQUE_NUMBER: AtomicU64 = AtomicU64::new(0); async fn ping_pong_duplex_parallel_many_( - env: & TestEnvironment, - number: u32, - mode: std::collections::HashSet) -{ + env: &TestEnvironment, + number: u32, + mode: std::collections::HashSet, +) { zenoh_core::zasync_executor_init!(); let make_keyexpr = |i: u32, mode: Mode| -> String { - return format!("/some/key/expr{}_{}_{}", i, mode.to_string(), UNIQUE_NUMBER.fetch_add(1, SeqCst)); + return format!( + "/some/key/expr{}_{}_{}", + i, + mode.to_string(), + UNIQUE_NUMBER.fetch_add(1, SeqCst) + ); }; // create scenarios let mut ping_pongs = Vec::new(); for i in 0..number { if mode.contains(&Mode::Ros1ToZenoh) { - ping_pongs.push(PingPong::new_ros1_to_zenoh(make_keyexpr(i, Mode::Ros1ToZenoh).as_str(), env.checker.clone()).await); + ping_pongs.push( + PingPong::new_ros1_to_zenoh( + make_keyexpr(i, Mode::Ros1ToZenoh).as_str(), + env.checker.clone(), + ) + .await, + ); } if mode.contains(&Mode::ZenohToRos1) { - ping_pongs.push(PingPong::new_zenoh_to_ros1(make_keyexpr(i, Mode::ZenohToRos1).as_str(), env.checker.clone()).await); + ping_pongs.push( + PingPong::new_zenoh_to_ros1( + make_keyexpr(i, Mode::ZenohToRos1).as_str(), + env.checker.clone(), + ) + .await, + ); } if mode.contains(&Mode::Ros1Service) { - ping_pongs.push(PingPong::new_ros1_to_zenoh_service(make_keyexpr(i, Mode::Ros1Service).as_str(), env.checker.clone()).await); + ping_pongs.push( + PingPong::new_ros1_to_zenoh_service( + make_keyexpr(i, Mode::Ros1Service).as_str(), + env.checker.clone(), + ) + .await, + ); } if mode.contains(&Mode::Ros1Client) { - ping_pongs.push(PingPong::new_ros1_to_zenoh_client(make_keyexpr(i, Mode::Ros1Client).as_str(), env.checker.clone()).await); + ping_pongs.push( + PingPong::new_ros1_to_zenoh_client( + make_keyexpr(i, Mode::Ros1Client).as_str(), + env.checker.clone(), + ) + .await, + ); } } @@ -994,8 +1057,7 @@ async fn ping_pong_duplex_parallel_many_( for ping_pong in ping_pongs { if mode.contains(&Mode::FastRun) { vec.push(ping_pong.run(1)); - } - else { + } else { vec.push(ping_pong.run(TestEnvironment::pps_measurements())); } } @@ -1010,98 +1072,205 @@ async fn ping_pong_duplex_parallel_many_( #[serial(ROS1)] fn ping_pong_zenoh_to_ros1() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::ZenohToRos1]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([Mode::ZenohToRos1]), + )); } #[test] #[serial(ROS1)] fn ping_pong_zenoh_to_ros1_many() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::ZenohToRos1]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([Mode::ZenohToRos1]), + )); } #[test] #[serial(ROS1)] fn ping_pong_ros1_to_zenoh() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1ToZenoh]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([Mode::Ros1ToZenoh]), + )); } #[test] #[serial(ROS1)] fn ping_pong_ros1_to_zenoh_many() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1ToZenoh]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([Mode::Ros1ToZenoh]), + )); } #[test] #[serial(ROS1)] fn ping_pong_ros1_service() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1Service]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([Mode::Ros1Service]), + )); } #[test] #[serial(ROS1)] fn ping_pong_ros1_service_many() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1Service]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([Mode::Ros1Service]), + )); } #[test] #[serial(ROS1)] fn ping_pong_ros1_client() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1Client]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([Mode::Ros1Client]), + )); } #[test] #[serial(ROS1)] fn ping_pong_ros1_client_many() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1Client]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([Mode::Ros1Client]), + )); } #[test] #[serial(ROS1)] fn ping_pong_all_sequential() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::ZenohToRos1]))); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1ToZenoh]))); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1Service]))); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::Ros1Client]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([Mode::ZenohToRos1]), + )); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([Mode::Ros1ToZenoh]), + )); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([Mode::Ros1Service]), + )); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([Mode::Ros1Client]), + )); } #[test] #[serial(ROS1)] fn ping_pong_all_sequential_many() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::ZenohToRos1]))); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1ToZenoh]))); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1Service]))); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::Ros1Client]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([Mode::ZenohToRos1]), + )); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([Mode::Ros1ToZenoh]), + )); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([Mode::Ros1Service]), + )); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([Mode::Ros1Client]), + )); } #[test] #[serial(ROS1)] fn ping_pong_all_parallel() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, 1, HashSet::from([Mode::ZenohToRos1, Mode::Ros1ToZenoh, Mode::Ros1Service, Mode::Ros1Client]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + 1, + HashSet::from([ + Mode::ZenohToRos1, + Mode::Ros1ToZenoh, + Mode::Ros1Service, + Mode::Ros1Client, + ]), + )); } #[test] #[serial(ROS1)] fn ping_pong_all_parallel_many() { let env = TestEnvironment::new(); - futures::executor::block_on(ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::ZenohToRos1, Mode::Ros1ToZenoh, Mode::Ros1Service, Mode::Ros1Client]))); + futures::executor::block_on(ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([ + Mode::ZenohToRos1, + Mode::Ros1ToZenoh, + Mode::Ros1Service, + Mode::Ros1Client, + ]), + )); } async fn main_work(env: &TestEnvironment, main_work_finished: Arc) { assert!(!main_work_finished.load(Relaxed)); - ping_pong_duplex_parallel_many_(& env, TestEnvironment::many_count(), HashSet::from([Mode::ZenohToRos1, Mode::Ros1ToZenoh, Mode::Ros1Service, Mode::Ros1Client])).await; + ping_pong_duplex_parallel_many_( + &env, + TestEnvironment::many_count(), + HashSet::from([ + Mode::ZenohToRos1, + Mode::Ros1ToZenoh, + Mode::Ros1Service, + Mode::Ros1Client, + ]), + ) + .await; main_work_finished.store(true, Relaxed); } async fn parallel_subwork(env: &TestEnvironment, main_work_finished: Arc) { while !main_work_finished.load(Relaxed) { - ping_pong_duplex_parallel_many_(& env, 10, HashSet::from([Mode::ZenohToRos1, Mode::Ros1ToZenoh, Mode::Ros1Service, Mode::Ros1Client, Mode::FastRun])).await; + ping_pong_duplex_parallel_many_( + &env, + 10, + HashSet::from([ + Mode::ZenohToRos1, + Mode::Ros1ToZenoh, + Mode::Ros1Service, + Mode::Ros1Client, + Mode::FastRun, + ]), + ) + .await; } } -async fn parallel_subworks(env: &TestEnvironment, main_work_finished: Arc, concurrent_subwork_count: u32) { +async fn parallel_subworks( + env: &TestEnvironment, + main_work_finished: Arc, + concurrent_subwork_count: u32, +) { let mut subworks = Vec::new(); for i in 0..concurrent_subwork_count { subworks.push(parallel_subwork(env, main_work_finished.clone())); @@ -1125,25 +1294,29 @@ fn ping_pong_all_overlap_many() { let main_work_finished = Arc::new(AtomicBool::new(false)); let main_work = main_work(&env, main_work_finished.clone()); - let parallel_subworks = parallel_subworks(&env, main_work_finished.clone(), TestEnvironment::many_count()); + let parallel_subworks = parallel_subworks( + &env, + main_work_finished.clone(), + TestEnvironment::many_count(), + ); async_std::task::block_on(futures::future::join(main_work, parallel_subworks)); } - // there were some issues with rosrust service, so there is a test to check it -async fn check_query(checker: & BridgeChecker) { +async fn check_query(checker: &BridgeChecker) { let name = "/some/key/expr"; - let queryable = checker - .make_ros_service(name, |q| { - print!("got query!\n"); - Ok(q) - }); + let queryable = checker.make_ros_service(name, |q| { + print!("got query!\n"); + Ok(q) + }); let ros_client = checker.make_ros_client(name); let data: Vec = (0..50).collect(); - let result = ros_client.data.req(&rosrust::RawMessage(data.clone())) - .unwrap() - .unwrap(); + let result = ros_client + .data + .req(&rosrust::RawMessage(data.clone())) + .unwrap() + .unwrap(); assert!(data.eq(&result.0)); } From cf319195cfe99126378481e30298e15df654f19a Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 14 Apr 2023 13:14:05 +0300 Subject: [PATCH 05/19] Fixed all clippy warnings --- zplugin-ros1/examples/ros1_service.rs | 4 +- zplugin-ros1/examples/ros1_sub.rs | 1 - zplugin-ros1/src/lib.rs | 13 +- zplugin-ros1/src/ros_to_zenoh_bridge.rs | 2 +- .../ros_to_zenoh_bridge/abstract_bridge.rs | 41 ++-- .../ros_to_zenoh_bridge/aloha_declaration.rs | 14 +- .../ros_to_zenoh_bridge/aloha_subscription.rs | 49 ++-- .../ros_to_zenoh_bridge/bridges_storage.rs | 16 +- .../src/ros_to_zenoh_bridge/discovery.rs | 76 ++---- .../src/ros_to_zenoh_bridge/ros1_client.rs | 22 +- .../src/ros_to_zenoh_bridge/topic_bridge.rs | 18 +- .../src/ros_to_zenoh_bridge/topic_mapping.rs | 55 ++--- .../ros_to_zenoh_bridge/topic_utilities.rs | 4 +- .../src/ros_to_zenoh_bridge/zenoh_client.rs | 45 ++-- zplugin-ros1/tests/aloha_declaration_test.rs | 29 ++- zplugin-ros1/tests/discovery_test.rs | 20 +- zplugin-ros1/tests/ping_pong_test.rs | 231 ++++++++---------- 17 files changed, 269 insertions(+), 371 deletions(-) diff --git a/zplugin-ros1/examples/ros1_service.rs b/zplugin-ros1/examples/ros1_service.rs index 8026c39..f263b39 100644 --- a/zplugin-ros1/examples/ros1_service.rs +++ b/zplugin-ros1/examples/ros1_service.rs @@ -77,7 +77,9 @@ async fn main() { println!("Zenoh: got reply!"); assert!(data == val.sample.unwrap().value.payload.contiguous().to_vec()); } - Err(_) => {} + Err(e) => { + println!("Zenoh got error: {}", e); + } } async_std::task::sleep(core::time::Duration::from_secs(1)).await; } diff --git a/zplugin-ros1/examples/ros1_sub.rs b/zplugin-ros1/examples/ros1_sub.rs index 7a927b6..24bf6bd 100644 --- a/zplugin-ros1/examples/ros1_sub.rs +++ b/zplugin-ros1/examples/ros1_sub.rs @@ -14,7 +14,6 @@ use zenoh_core::AsyncResolve; -use rosrust; use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; #[async_std::main] diff --git a/zplugin-ros1/src/lib.rs b/zplugin-ros1/src/lib.rs index 8bfe453..9225a72 100644 --- a/zplugin-ros1/src/lib.rs +++ b/zplugin-ros1/src/lib.rs @@ -49,11 +49,8 @@ impl Plugin for Ros1Plugin { // run through the bridge's config options and fill them from plugins config let plugin_configuration_entries = Environment::env(); for entry in plugin_configuration_entries.iter() { - match self_cfg.get(entry.name) { - Some(v) => { - entry.set(v); - } - None => {} + if let Some(v) = self_cfg.get(entry.name) { + entry.set(v); } } @@ -94,11 +91,11 @@ impl RunningPlugin { // create a zenoh Session that shares the same Runtime than zenohd let session = zenoh::init(runtime.clone()).res().await?.into_arc(); let bridge = ros_to_zenoh_bridge::Ros1ToZenohBridge::new_with_external_session(session); - return Ok(bridge); + Ok(bridge) }); - return Ok(Self { + Ok(Self { _bridge: Some(bridge?), - }); + }) } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge.rs index b224145..fb402dd 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge.rs @@ -51,7 +51,7 @@ pub struct Ros1ToZenohBridge { impl Ros1ToZenohBridge { pub async fn new_with_own_session(config: zenoh::config::Config) -> Self { let session = zenoh::open(config).res_async().await.unwrap().into_arc(); - return Self::new_with_external_session(session); + Self::new_with_external_session(session) } pub fn new_with_external_session(session: Arc) -> Self { diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs index dae6569..7aacb69 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/abstract_bridge.rs @@ -48,7 +48,7 @@ impl AbstractBridge { ), } }; - return Ok(Self { _impl }); + Ok(Self { _impl }) } } @@ -73,16 +73,14 @@ impl Ros1ToZenohClient { topic.name, topic.datatype ); - let zenoh_key = make_zenoh_key(&topic).to_string(); + let zenoh_key = make_zenoh_key(topic).to_string(); match ros1_client.service::( topic, move |q| -> rosrust::ServiceResult { return Self::on_query(&zenoh_key, q, zenoh_client.as_ref()); }, ) { - Ok(service) => { - return Ok(Ros1ToZenohClient { _service: service }); - } + Ok(service) => Ok(Ros1ToZenohClient { _service: service }), Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) } @@ -109,7 +107,7 @@ impl Ros1ToZenohClient { Ok(value) => { let data = value.payload.contiguous().to_vec(); debug!("Zenoh -> ROS1: sending {} bytes!", data.len()); - return Ok(rosrust::RawMessage(data)); + Ok(rosrust::RawMessage(data)) } Err(e) => { error!( @@ -117,7 +115,7 @@ impl Ros1ToZenohClient { e ); let error = e.to_string(); - return Err(error); + Err(error) } }, Err(e) => { @@ -126,7 +124,7 @@ impl Ros1ToZenohClient { e ); let error = e.to_string(); - return Err(error); + Err(error) } }, Err(e) => { @@ -135,7 +133,7 @@ impl Ros1ToZenohClient { e ); let error = e.to_string(); - return Err(error); + Err(error) } } } @@ -163,9 +161,9 @@ impl Ros1ToZenohService { async_std::task::spawn(Self::on_query(client_in_arc.clone(), query)); }) .await?; - return Ok(Ros1ToZenohService { + Ok(Ros1ToZenohService { _queryable: queryable, - }); + }) } Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) @@ -200,10 +198,9 @@ impl Ros1ToZenohService { ) { // rosrust is synchronous, so we will use spawn_blocking. If there will be an async mode some day for the rosrust, // than reply_to_query can be refactored to async very easily - let res = async_std::task::spawn_blocking(move || { - return ros1_client.req(&rosrust::RawMessage(payload)); - }) - .await; + let res = + async_std::task::spawn_blocking(move || ros1_client.req(&rosrust::RawMessage(payload))) + .await; match Self::reply_to_query(res, &query).await { Ok(_) => {} Err(e) => { @@ -258,7 +255,7 @@ impl Ros1ToZenohService { .await?; } } - return Ok(()); + Ok(()) } } @@ -286,11 +283,9 @@ impl Ros1ToZenoh { } } }) { - Ok(subscriber) => { - return Ok(Ros1ToZenoh { - _subscriber: subscriber, - }); - } + Ok(subscriber) => Ok(Ros1ToZenoh { + _subscriber: subscriber, + }), Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) } @@ -330,9 +325,9 @@ impl ZenohToRos1 { }); }) .await?; - return Ok(ZenohToRos1 { + Ok(ZenohToRos1 { _subscriber: subscriber, - }); + }) } Err(e) => { zenoh_core::bail!("Ros error: {}", e.to_string()) diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs index 76a1431..f0ee4c9 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs @@ -39,7 +39,7 @@ impl AlohaDeclaration { key, session, )); - return Self { monitor_running }; + Self { monitor_running } } //PRIVATE: @@ -66,7 +66,7 @@ impl AlohaDeclaration { let mut sending_beacons = true; Self::start_beacon_task( - beacon_period.clone(), + beacon_period, key.clone(), session.clone(), beacon_task_flag.clone(), @@ -87,7 +87,7 @@ impl AlohaDeclaration { .await; if remote_beacons.load(std::sync::atomic::Ordering::SeqCst) == 0 { Self::start_beacon_task( - beacon_period.clone(), + beacon_period, key.clone(), session.clone(), beacon_task_flag.clone(), @@ -98,11 +98,9 @@ impl AlohaDeclaration { } } _ => { - if sending_beacons { - if rand::random::() { - Self::stop_beacon_task(beacon_task_flag.clone()).await; - sending_beacons = false; - } + if sending_beacons && rand::random::() { + Self::stop_beacon_task(beacon_task_flag.clone()).await; + sending_beacons = false; } } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs index 387127f..e5a2f79 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs @@ -46,10 +46,15 @@ impl AlohaResource { } pub fn is_active(&self) -> bool { - return self.activity.load(Relaxed); + self.activity.load(Relaxed) } } +pub type TCallback = dyn Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static; + pub struct AlohaSubscription { task_running: Arc, } @@ -86,7 +91,7 @@ impl AlohaSubscription { on_resource_undeclared, )); - return Ok(Self { task_running }); + Ok(Self { task_running }) } //PRIVATE: @@ -119,7 +124,7 @@ impl AlohaSubscription { ) .await; - return Ok(()); + Ok(()) } async fn listening_task<'a, F>( @@ -182,7 +187,7 @@ impl AlohaSubscription { let listen = Self::listening_task( task_running.clone(), accumulating_resources, - &subscriber, + subscriber, &on_resource_declared, ) .fuse(); @@ -207,9 +212,9 @@ impl AlohaSubscription { } } - accumulating_resources.get_mut().retain(|_key, val| { - return val.is_active(); - }); + accumulating_resources + .get_mut() + .retain(|_key, val| val.is_active()); } } } @@ -219,33 +224,19 @@ pub struct AlohaSubscriptionBuilder { key: OwnedKeyExpr, beacon_period: Duration, - on_resource_declared: Option< - Box< - dyn Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> - + Send - + Sync - + 'static, - >, - >, - on_resource_undeclared: Option< - Box< - dyn Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> - + Send - + Sync - + 'static, - >, - >, + on_resource_declared: Option>, + on_resource_undeclared: Option>, } impl AlohaSubscriptionBuilder { pub fn new(session: Arc, key: OwnedKeyExpr, beacon_period: Duration) -> Self { - return Self { + Self { session, key, beacon_period, on_resource_declared: None, on_resource_undeclared: None, - }; + } } pub fn on_resource_declared(mut self, on_resource_declared: F) -> Self @@ -256,7 +247,7 @@ impl AlohaSubscriptionBuilder { + 'static, { self.on_resource_declared = Some(Box::new(on_resource_declared)); - return self; + self } pub fn on_resource_undeclared(mut self, on_resource_undeclared: F) -> Self @@ -267,11 +258,11 @@ impl AlohaSubscriptionBuilder { + 'static, { self.on_resource_undeclared = Some(Box::new(on_resource_undeclared)); - return self; + self } pub async fn build(self) -> ZResult { - return AlohaSubscription::new( + AlohaSubscription::new( self.session, self.key, self.beacon_period, @@ -280,6 +271,6 @@ impl AlohaSubscriptionBuilder { self.on_resource_undeclared .unwrap_or(Box::new(|_dummy| Box::new(Box::pin(async {})))), ) - .await; + .await } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs index ab62532..82cc523 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs @@ -71,7 +71,7 @@ impl Bridges { fill(&mut result.ros_publishers, &self.publisher_bridges); fill(&mut result.ros_services, &self.service_bridges); fill(&mut result.ros_subscribers, &self.subscriber_bridges); - return result; + result } fn clear(&mut self) { @@ -204,9 +204,9 @@ impl<'a> ElementAccessor<'a> { // erase all non-actual bridges { let size_before_retain = self.access.container.len(); - self.access.container.retain(|_topic, bridge| { - return bridge.is_actual(); - }); + self.access + .container + .retain(|_topic, bridge| bridge.is_actual()); smth_changed |= size_before_retain != self.access.container.len(); } @@ -230,7 +230,7 @@ impl<'a> ElementAccessor<'a> { } } } - return smth_changed; + smth_changed } } @@ -312,14 +312,14 @@ impl BridgesStorage { .for_type(BridgeType::Subscriber) .receive_ros1_state(&mut ros1_state.subscribed) .await; - return smth_changed; + smth_changed } pub fn status(&self) -> BridgeStatus { - return self.bridges.status(); + self.bridges.status() } pub fn clear(&mut self) { - return self.bridges.clear(); + self.bridges.clear() } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs index eefd2ed..000f7e8 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs @@ -106,9 +106,9 @@ impl RemoteResources { } } - return Self { + Self { _subscriber: subscriber, - }; + } } // PRIVATE: @@ -142,8 +142,8 @@ impl RemoteResources { rosrust::api::Topic, ) -> Box + Unpin + Send + Sync>, { - let discovery = discovery_format::parse(&data).map_err(|err| err.to_string())?; - return Self::handle_format(discovery, callback).await; + let discovery = discovery_format::parse(data).map_err(|err| err.to_string())?; + Self::handle_format(discovery, callback).await } async fn handle_format( @@ -167,27 +167,18 @@ impl RemoteResources { let ros1_topic = make_topic(datatype, topic)?; - let b_type; - match resource_class.as_str() { - ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS => { - b_type = BridgeType::Publisher; - } - ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS => { - b_type = BridgeType::Subscriber; - } - ROS1_DISCOVERY_INFO_SERVICES_CLASS => { - b_type = BridgeType::Service; - } - ROS1_DISCOVERY_INFO_CLIENTS_CLASS => { - b_type = BridgeType::Client; - } + let b_type = match resource_class.as_str() { + ROS1_DISCOVERY_INFO_PUBLISHERS_CLASS => BridgeType::Publisher, + ROS1_DISCOVERY_INFO_SUBSCRIBERS_CLASS => BridgeType::Subscriber, + ROS1_DISCOVERY_INFO_SERVICES_CLASS => BridgeType::Service, + ROS1_DISCOVERY_INFO_CLIENTS_CLASS => BridgeType::Client, _ => { return Err("unexpected resource class!".to_string()); } }; callback(b_type, ros1_topic).await; - return Ok(()); + Ok(()) } } @@ -232,11 +223,11 @@ impl LocalResources { bridge_namespace: String, session: Arc, ) -> LocalResources { - return Self { + Self { session, discovery_namespace, bridge_namespace, - }; + } } pub async fn declare_with_type( @@ -309,7 +300,7 @@ impl Discovery { + Sync + 'static, { - return Self { + Self { _remote_resources: RemoteResources::new( session.clone(), discovery_namespace.clone(), @@ -318,41 +309,26 @@ impl Discovery { ) .await, local_resources: LocalResources::new(discovery_namespace, bridge_namespace, session), - }; + } } pub fn local_resources(&self) -> &LocalResources { - return &self.local_resources; + &self.local_resources } } +pub type TCallback = dyn Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> + + Send + + Sync + + 'static; + pub struct DiscoveryBuilder { discovery_namespace: String, bridge_namespace: String, session: Arc, - on_discovered: Option< - Box< - dyn Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync> - + Send - + Sync - + 'static, - >, - >, - on_lost: Option< - Box< - dyn Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync> - + Send - + Sync - + 'static, - >, - >, + on_discovered: Option>, + on_lost: Option>, } impl DiscoveryBuilder { @@ -381,7 +357,7 @@ impl DiscoveryBuilder { + 'static, { self.on_discovered = Some(Box::new(on_discovered)); - return self; + self } pub fn on_lost(&mut self, on_lost: F) -> &mut Self where @@ -394,11 +370,11 @@ impl DiscoveryBuilder { + 'static, { self.on_lost = Some(Box::new(on_lost)); - return self; + self } pub async fn build(self) -> Discovery { - return Discovery::new( + Discovery::new( self.discovery_namespace, self.bridge_namespace, self.session, @@ -407,6 +383,6 @@ impl DiscoveryBuilder { self.on_lost .unwrap_or(Box::new(|_, _| Box::new(Box::pin(async {})))), ) - .await; + .await } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs index ce39f9f..1b0d9fe 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs @@ -82,22 +82,18 @@ impl Ros1Client { mut state: rosrust::api::error::Response, ) -> rosrust::api::error::Response { debug!("system state before filter: {:#?}", state); - match state.as_mut() { - Ok(value) => { - let name = self.ros.name(); + if let Ok(value) = state.as_mut() { + let name = self.ros.name(); - let retain_lambda = |x: &rosrust::api::TopicData| { - return x.connections.len() > 1 - || (x.connections.len() == 1 && x.connections[0] != name); - }; + let retain_lambda = |x: &rosrust::api::TopicData| { + x.connections.len() > 1 || (x.connections.len() == 1 && x.connections[0] != name) + }; - value.publishers.retain(retain_lambda); - value.subscribers.retain(retain_lambda); - value.services.retain(retain_lambda); - } - Err(_) => {} + value.publishers.retain(retain_lambda); + value.subscribers.retain(retain_lambda); + value.services.retain(retain_lambda); } debug!("system state after filter: {:#?}", state); - return state; + state } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs index 5fe539a..9fb9d72 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs @@ -52,7 +52,7 @@ impl TopicBridge { zenoh_client: Arc, briging_mode: BridgingMode, ) -> Self { - return Self { + Self { topic, b_type, ros1_client, @@ -63,7 +63,7 @@ impl TopicBridge { declaration_interface, declaration: None, bridge: None, - }; + } } pub async fn set_present_in_ros1(&mut self, present: bool) -> bool { @@ -72,7 +72,7 @@ impl TopicBridge { if recalc { self.recalc_state().await; } - return recalc; + recalc } pub async fn set_has_complementary_in_zenoh(&mut self, present: bool) -> bool { @@ -81,21 +81,17 @@ impl TopicBridge { if recalc { self.recalc_state().await; } - return recalc; + recalc } pub fn is_bridging(&self) -> bool { - return self.bridge.is_some(); + self.bridge.is_some() } pub fn is_actual(&self) -> bool { match self.briging_mode { - BridgingMode::Lazy => { - return self.required_on_ros1_side || self.required_on_zenoh_side; - } - BridgingMode::Automatic => { - return self.required_on_ros1_side; - } + BridgingMode::Lazy => self.required_on_ros1_side || self.required_on_zenoh_side, + BridgingMode::Automatic => self.required_on_ros1_side, } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs index 51e6e38..dd4edcf 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs @@ -29,28 +29,19 @@ impl Ros1TopicMapping { ros1_client: &ros1_client::Ros1Client, ) -> rosrust::api::error::Response { match ros1_client.state() { - Ok(state_val) => { - match ros1_client.topic_types() { - Ok(topics_val) => { - debug!("topics: {:#?}", topics_val); - return Ok(Ros1TopicMapping::new(&state_val, &topics_val)); - } - Err(e) => { - return Err(e); - } - }; - } - Err(e) => { - return Err(e); - } - }; + Ok(state_val) => match ros1_client.topic_types() { + Ok(topics_val) => { + debug!("topics: {:#?}", topics_val); + Ok(Ros1TopicMapping::new(&state_val, &topics_val)) + } + Err(e) => Err(e), + }, + Err(e) => Err(e), + } } // PRIVATE: - fn new( - state: &rosrust::api::SystemState, - topics: &Vec, - ) -> Ros1TopicMapping { + fn new(state: &rosrust::api::SystemState, topics: &[rosrust::api::Topic]) -> Ros1TopicMapping { let mut result = Ros1TopicMapping { published: HashSet::new(), subscribed: HashSet::new(), @@ -62,25 +53,27 @@ impl Ros1TopicMapping { Ros1TopicMapping::fill(&mut result.published, &state.publishers, topics); Ros1TopicMapping::fill(&mut result.serviced, &state.services, topics); - return result; + result } fn fill( dst: &mut HashSet, - data: &Vec, - topics: &Vec, + data: &[rosrust::api::TopicData], + topics: &[rosrust::api::Topic], ) { for item in data.iter() { let topic = topics.iter().find(|x| x.name == item.name); - if topic.is_none() { - debug!("Unable to find datatype for topic {}", item.name); - dst.insert(rosrust::api::Topic { - name: item.name.clone(), - datatype: "*".to_string(), - }); - } else { - let t = topic.unwrap(); - dst.insert(t.clone()); + match topic { + None => { + debug!("Unable to find datatype for topic {}", item.name); + dst.insert(rosrust::api::Topic { + name: item.name.clone(), + datatype: "*".to_string(), + }); + } + Some(val) => { + dst.insert(val.clone()); + } } } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs index 34e5647..c819c62 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs @@ -31,8 +31,8 @@ pub fn make_topic(datatype: &keyexpr, topic_name: &keyexpr) -> Result { - return self - .session + self.session .declare_subscriber(opt_keyexpr) .callback(callback) .allowed_origin(Locality::Remote) .res_async() - .await; - } - Err(e) => { - return Err(e); + .await } + Err(e) => Err(e), } } @@ -61,17 +58,14 @@ impl ZenohClient { match self.make_keyexpr(key_expr).await { Ok(opt_keyexpr) => { - return self - .session + self.session .declare_publisher(opt_keyexpr) .allowed_destination(Locality::Remote) .congestion_control(CongestionControl::Block) .res_async() - .await; - } - Err(e) => { - return Err(e); + .await } + Err(e) => Err(e), } } @@ -87,17 +81,14 @@ impl ZenohClient { match self.make_keyexpr(key_expr).await { Ok(opt_keyexpr) => { - return self - .session + self.session .declare_queryable(opt_keyexpr) .allowed_origin(Locality::Remote) .callback(callback) .res_async() - .await; - } - Err(e) => { - return Err(e); + .await } + Err(e) => Err(e), } } @@ -114,17 +105,14 @@ impl ZenohClient { match self.make_keyexpr(key_expr).await { Ok(opt_keyexpr) => { - return self - .session + self.session .get(opt_keyexpr) .with_value(data) .callback(callback) .res_async() - .await; - } - Err(e) => { - return Err(e); + .await } + Err(e) => Err(e), } } @@ -137,17 +125,14 @@ impl ZenohClient { match self.make_keyexpr(key_expr).await { Ok(opt_keyexpr) => { - return self - .session + self.session .get(opt_keyexpr) .with_value(data) .allowed_destination(Locality::Remote) .res_async() - .await; - } - Err(e) => { - return Err(e); + .await } + Err(e) => Err(e), } } diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs index 997b730..cfc7e18 100644 --- a/zplugin-ros1/tests/aloha_declaration_test.rs +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -28,41 +28,40 @@ use zplugin_ros1::ros_to_zenoh_bridge::{aloha_declaration, aloha_subscription}; const TIMEOUT: Duration = Duration::from_secs(30); fn session_builder() -> OpenBuilder { - return zenoh::open(zenoh::config::peer()); + zenoh::open(zenoh::config::peer()) } -fn declaration_builder<'a>( +fn declaration_builder( session: Arc, beacon_period: Duration, ) -> aloha_declaration::AlohaDeclaration { - return aloha_declaration::AlohaDeclaration::new( + aloha_declaration::AlohaDeclaration::new( session, zenoh::key_expr::OwnedKeyExpr::from_str("key").unwrap(), beacon_period, - ); + ) } fn subscription_builder( session: Arc, beacon_period: Duration, ) -> aloha_subscription::AlohaSubscriptionBuilder { - return aloha_subscription::AlohaSubscriptionBuilder::new( + aloha_subscription::AlohaSubscriptionBuilder::new( session, zenoh::key_expr::OwnedKeyExpr::from_str("key").unwrap(), beacon_period, - ); + ) } fn make_session() -> Arc { - return session_builder().res_sync().unwrap().into_arc(); + session_builder().res_sync().unwrap().into_arc() } fn make_subscription( session: Arc, beacon_period: Duration, ) -> aloha_subscription::AlohaSubscription { - return async_std::task::block_on(subscription_builder(session, beacon_period).build()) - .unwrap(); + async_std::task::block_on(subscription_builder(session, beacon_period).build()).unwrap() } #[test] @@ -111,11 +110,11 @@ impl<'a> PPCMeasurement<'a> { .res_async() .await?; - return Ok(Self { + Ok(Self { _subscriber: subscriber, ppc, measurement_period, - }); + }) } pub async fn measure_ppc(&self) -> usize { @@ -133,14 +132,14 @@ struct DeclarationCollector { } impl DeclarationCollector { fn new() -> Self { - return Self { + Self { resources: Arc::new(Mutex::new(HashSet::new())), to_be_declared: Arc::new(Mutex::new(HashSet::new())), to_be_undeclared: Arc::new(Mutex::new(HashSet::new())), - }; + } } - pub fn use_builder<'b>( + pub fn use_builder( &self, mut builder: aloha_subscription::AlohaSubscriptionBuilder, ) -> aloha_subscription::AlohaSubscriptionBuilder { @@ -162,7 +161,7 @@ impl DeclarationCollector { Box::new(Box::pin(async move {})) }); - return builder; + builder } pub fn arm( diff --git a/zplugin-ros1/tests/discovery_test.rs b/zplugin-ros1/tests/discovery_test.rs index 4de0589..bba8a0e 100644 --- a/zplugin-ros1/tests/discovery_test.rs +++ b/zplugin-ros1/tests/discovery_test.rs @@ -32,11 +32,11 @@ fn session_builder() -> OpenBuilder { .timestamping .set_enabled(Some(ModeDependentValue::Unique(true))) .unwrap(); - return zenoh::open(config); + zenoh::open(config) } fn discovery_builder(session: Arc) -> discovery::DiscoveryBuilder { - return discovery::DiscoveryBuilder::new("*".to_string(), "*".to_string(), session); + discovery::DiscoveryBuilder::new("*".to_string(), "*".to_string(), session) } fn make_session() -> Arc { @@ -44,7 +44,7 @@ fn make_session() -> Arc { } fn make_discovery(session: Arc) -> discovery::Discovery { - return async_std::task::block_on(discovery_builder(session).build()); + async_std::task::block_on(discovery_builder(session).build()) } #[test] @@ -76,15 +76,15 @@ struct DiscoveryCollector { } impl DiscoveryCollector { fn new() -> Self { - return Self { + Self { publishers: Arc::new(Mutex::new(HashMultiSet::new())), subscribers: Arc::new(Mutex::new(HashMultiSet::new())), services: Arc::new(Mutex::new(HashMultiSet::new())), clients: Arc::new(Mutex::new(HashMultiSet::new())), - }; + } } - pub fn use_builder<'a>( + pub fn use_builder( &self, mut builder: discovery::DiscoveryBuilder, ) -> discovery::DiscoveryBuilder { @@ -120,7 +120,7 @@ impl DiscoveryCollector { Box::new(Box::pin(async {})) }); - return builder; + builder } pub async fn wait_publishers(&self, expected: HashMultiSet) { @@ -164,7 +164,7 @@ async fn generate_topics( }); } } - return result; + result } #[derive(Default)] @@ -212,7 +212,7 @@ impl State { HashMultiSet, HashMultiSet, ) { - return ( + ( generate_topics( self.publishers_count, self.publishers_duplication, @@ -227,7 +227,7 @@ impl State { .await, generate_topics(self.services_count, self.services_duplication, self.stage).await, generate_topics(self.clients_count, self.clients_duplication, self.stage).await, - ); + ) } } diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 04488d1..4b9821c 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -35,7 +35,6 @@ use log::{debug, error}; use rosrust::{Client, Duration, RawMessage}; use std::sync::atomic::AtomicUsize; use zenoh::prelude::r#async::*; -use zenoh_core; use std::process::Command; use std::{thread, time}; @@ -49,7 +48,7 @@ where let cycles = 1000; let millis = timeout.as_millis() / cycles + 1; - for i in 0..cycles { + for _i in 0..cycles { async_std::task::sleep(core::time::Duration::from_millis( millis.try_into().unwrap(), )) @@ -58,7 +57,7 @@ where return true; } } - return false; + false } struct RunningBridge { @@ -81,7 +80,7 @@ impl RunningBridge { result.ros_status.clone(), result.bridge_status.clone(), )); - return result; + result } async fn run( @@ -119,18 +118,18 @@ impl RunningBridge { ); } pub async fn wait_ros_status(&self, status: RosStatus, timeout: core::time::Duration) -> bool { - return wait( + wait( move || { let val = self.ros_status.lock().unwrap(); - return *val == status; + *val == status }, timeout, ) - .await; + .await } pub async fn assert_bridge_empy(&self) { - self.assert_bridge_status(|| BridgeStatus::default()).await; + self.assert_bridge_status(BridgeStatus::default).await; } pub async fn assert_bridge_status BridgeStatus>(&self, status: F) { assert!( @@ -143,14 +142,14 @@ impl RunningBridge { status: F, timeout: core::time::Duration, ) -> bool { - return wait( + wait( move || { let val = self.bridge_status.lock().unwrap(); - return *val == (status)(); + *val == (status)() }, timeout, ) - .await; + .await } } impl Drop for RunningBridge { @@ -162,7 +161,7 @@ impl Drop for RunningBridge { #[test] #[serial(ROS1)] fn env_checks_no_master_init_and_exit_immed() { - let ros_env = ROSEnvironment::new(); + let _ros_env = ROSEnvironment::new(); let bridge = RunningBridge::new(zenoh::config::Config::default()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); @@ -171,7 +170,7 @@ fn env_checks_no_master_init_and_exit_immed() { #[test] #[serial(ROS1)] fn env_checks_no_master_init_and_wait() { - let ros_env = ROSEnvironment::new(); + let _ros_env = ROSEnvironment::new(); let bridge = RunningBridge::new(zenoh::config::Config::default()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); @@ -183,7 +182,7 @@ fn env_checks_no_master_init_and_wait() { #[test] #[serial(ROS1)] fn env_checks_with_master_init_and_exit_immed() { - let ros_env = ROSEnvironment::new().with_master(); + let _ros_env = ROSEnvironment::new().with_master(); let bridge = RunningBridge::new(zenoh::config::Config::default()); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); @@ -192,7 +191,7 @@ fn env_checks_with_master_init_and_exit_immed() { #[test] #[serial(ROS1)] fn env_checks_with_master_init_and_wait() { - let ros_env = ROSEnvironment::new().with_master(); + let _ros_env = ROSEnvironment::new().with_master(); let bridge = RunningBridge::new(zenoh::config::Config::default()); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); @@ -204,14 +203,14 @@ fn env_checks_with_master_init_and_wait() { #[test] #[serial(ROS1)] fn env_checks_with_master_init_and_loose_master() { - let mut ros_env = Some(ROSEnvironment::new().with_master()); + let mut _ros_env = Some(ROSEnvironment::new().with_master()); let bridge = RunningBridge::new(zenoh::config::Config::default()); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); - ros_env = None; + _ros_env = None; async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -222,14 +221,14 @@ fn env_checks_with_master_init_and_loose_master() { #[test] #[serial(ROS1)] fn env_checks_with_master_init_and_wait_for_master() { - let mut ros_env = ROSEnvironment::new(); + let mut _ros_env = ROSEnvironment::new(); let bridge = RunningBridge::new(zenoh::config::Config::default()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); - ros_env = ros_env.with_master(); + _ros_env = _ros_env.with_master(); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -242,7 +241,7 @@ fn env_checks_with_master_init_and_wait_for_master() { fn env_checks_with_master_init_and_reconnect_many_times_to_master() { let mut ros_env = ROSEnvironment::new(); let bridge = RunningBridge::new(zenoh::config::Config::default()); - for i in 0..20 { + for _i in 0..20 { async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); ros_env = ros_env.with_master(); @@ -267,12 +266,12 @@ struct ZenohQuery { impl ZenohQuery { fn new(inner: Arc, key: String, cycles: Arc) -> Self { let running = Arc::new(AtomicBool::new(true)); - return Self { + Self { inner, key, running, cycles, - }; + } } async fn query_loop( @@ -321,11 +320,11 @@ impl ROS1Client { fn new(inner: Arc, key: String, cycles: Arc) -> Self { let running = Arc::new(AtomicBool::new(true)); let ros1_client = Arc::new(inner.make_ros_client(&key)); - return Self { + Self { running, cycles, ros1_client, - }; + } } fn query_loop( @@ -368,7 +367,7 @@ impl Drop for ROS1Client { trait Publisher { fn put(&self, data: Vec); fn ready(&self) -> bool { - return true; + true } } impl Publisher for ZenohPublisher { @@ -386,7 +385,7 @@ impl Publisher for ROS1Publisher { } fn ready(&self) -> bool { - return self.inner.data.subscriber_count() != 0; + self.inner.data.subscriber_count() != 0 } } impl Publisher for ZenohQuery { @@ -411,28 +410,28 @@ impl Publisher for ROS1Client { } struct ZenohSubscriber { - inner: zenoh::subscriber::Subscriber<'static, ()>, + _inner: zenoh::subscriber::Subscriber<'static, ()>, } struct ZenohQueryable { - inner: zenoh::queryable::Queryable<'static, ()>, + _inner: zenoh::queryable::Queryable<'static, ()>, } struct ROS1Subscriber { inner: RAIICounter, } struct ROS1Service { - inner: RAIICounter, + _inner: RAIICounter, } trait Subscriber { fn ready(&self) -> bool { - return true; + true } } impl Subscriber for ZenohSubscriber {} impl Subscriber for ZenohQueryable {} impl Subscriber for ROS1Subscriber { fn ready(&self) -> bool { - return self.inner.data.publisher_count() > 0 && self.inner.data.publisher_uris().len() > 0; + self.inner.data.publisher_count() > 0 && !self.inner.data.publisher_uris().is_empty() } } impl Subscriber for ROS1Service {} @@ -441,7 +440,7 @@ struct PubSub { key: String, publisher: Box, subscriber: Box, - discovery_resource: LocalResource, + _discovery_resource: LocalResource, } struct PingPong { @@ -473,17 +472,17 @@ impl PingPong { .await; let zenoh_query = ZenohQuery::new(backend, key.to_string(), cycles.clone()); - return PingPong { + PingPong { pub_sub: PubSub { key: key.to_string(), publisher: Box::new(zenoh_query), subscriber: Box::new(ROS1Service { - inner: ros1_service, + _inner: ros1_service, }), - discovery_resource, + _discovery_resource: discovery_resource, }, cycles, - }; + } } async fn new_ros1_to_zenoh_client(key: &str, backend: Arc) -> PingPong { @@ -498,22 +497,22 @@ impl PingPong { async_std::task::spawn(async move { let key = q.key_expr().clone(); let val = q.value().unwrap().clone(); - q.reply(Ok(Sample::new(key, val))).res_async().await; + let _ = q.reply(Ok(Sample::new(key, val))).res_async().await; }); }) .await; - return PingPong { + PingPong { pub_sub: PubSub { key: key.to_string(), publisher: Box::new(ROS1Client::new(backend, key.to_string(), cycles.clone())), subscriber: Box::new(ZenohQueryable { - inner: zenoh_queryable, + _inner: zenoh_queryable, }), - discovery_resource, + _discovery_resource: discovery_resource, }, cycles, - }; + } } async fn new_ros1_to_zenoh(key: &str, backend: Arc) -> PingPong { @@ -539,15 +538,15 @@ impl PingPong { }) .await; - return PingPong { + PingPong { pub_sub: PubSub { key: key.to_string(), publisher: Box::new(ROS1Publisher { inner: ros1_pub }), - subscriber: Box::new(ZenohSubscriber { inner: zenoh_sub }), - discovery_resource, + subscriber: Box::new(ZenohSubscriber { _inner: zenoh_sub }), + _discovery_resource: discovery_resource, }, cycles, - }; + } } async fn new_zenoh_to_ros1(key: &str, backend: Arc) -> PingPong { @@ -570,15 +569,15 @@ impl PingPong { c.fetch_add(1, Relaxed); }); - return PingPong { + PingPong { pub_sub: PubSub { key: key.to_string(), publisher: Box::new(ZenohPublisher { inner: zenoh_pub }), subscriber: Box::new(ROS1Subscriber { inner: ros1_sub }), - discovery_resource, + _discovery_resource: discovery_resource, }, cycles, - }; + } } async fn start(&self) { @@ -599,7 +598,7 @@ impl PingPong { async fn check_pps(&self, pps_measurements: u32) { for i in 0..pps_measurements { let pps = self.measure_pps().await; - print!("PPS #{}: {} \t Key: {}\n", i, pps, self.pub_sub.key); + println!("PPS #{}: {} \t Key: {}", i, pps, self.pub_sub.key); assert!(pps > 0.0); } } @@ -619,15 +618,13 @@ impl PingPong { result += self.cycles.load(Relaxed) as f64; } debug!("...finished measure PPS!"); - return result * 1000.0 / (duration as f64); + result * 1000.0 / (duration as f64) } async fn wait_for_pub_sub_ready(&self) { assert!( Self::wait( - move || { - return self.pub_sub.publisher.ready() && self.pub_sub.subscriber.ready(); - }, + move || { self.pub_sub.publisher.ready() && self.pub_sub.subscriber.ready() }, core::time::Duration::from_secs(30) ) .await @@ -642,7 +639,7 @@ impl PingPong { let cycles = 1000; let micros = timeout.as_micros() / cycles; - for i in 0..cycles { + for _i in 0..cycles { async_std::task::sleep(core::time::Duration::from_micros( micros.try_into().unwrap(), )) @@ -651,7 +648,7 @@ impl PingPong { return true; } } - return false; + false } } @@ -670,7 +667,7 @@ impl ROSEnvironment { let mut kill_master = Command::new("killall").arg("rosmaster").spawn().unwrap(); kill_master.wait().unwrap(); - return ROSEnvironment { rosmaster: None }; + ROSEnvironment { rosmaster: None } } pub fn with_master(mut self) -> Self { @@ -684,7 +681,7 @@ impl ROSEnvironment { .unwrap(), ); - return self; + self } pub fn without_master(mut self) -> Self { @@ -695,7 +692,7 @@ impl ROSEnvironment { self.rosmaster = None; } - return self; + self } } @@ -721,32 +718,32 @@ impl TestEnvironment { async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); - return TestEnvironment { + TestEnvironment { bridge, checker, _ros_env: ros_env, - }; + } } pub fn many_count() -> u32 { - return Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 10); + Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 10) } pub fn pps_measurements() -> u32 { - return Self::env_var("TEST_ROS1_TO_ZENOH_PPS_ITERATIONS", 100); + Self::env_var("TEST_ROS1_TO_ZENOH_PPS_ITERATIONS", 100) } pub fn pps_measure_period_ms() -> u64 { - return Self::env_var("TEST_ROS1_TO_ZENOH_PPS_PERIOD_MS", 1); + Self::env_var("TEST_ROS1_TO_ZENOH_PPS_PERIOD_MS", 1) } pub fn data_size() -> u32 { - return Self::env_var("TEST_ROS1_TO_ZENOH_DATA_SIZE", 16); + Self::env_var("TEST_ROS1_TO_ZENOH_DATA_SIZE", 16) } pub async fn assert_bridge_status_synchronized(&self) { self.bridge - .assert_bridge_status(|| self.checker.expected_bridge_status.read().unwrap().clone()) + .assert_bridge_status(|| *self.checker.expected_bridge_status.read().unwrap()) .await; } @@ -755,16 +752,12 @@ impl TestEnvironment { where Tvar: FromStr, { - match std::env::var(key) { - Ok(val) => { - match val.parse::() { - Ok(val) => return val, - Err(..) => {} - }; + if let Ok(val) = std::env::var(key) { + if let Ok(val) = val.parse::() { + return val; } - Err(..) => {} - }; - return default; + } + default } } @@ -811,12 +804,12 @@ impl BridgeChecker { // PUBLIC pub fn new() -> BridgeChecker { let session = zenoh::open(config::peer()).res_sync().unwrap().into_arc(); - return BridgeChecker { + BridgeChecker { ros_client: ros1_client::Ros1Client::new("test_ros_node"), zenoh_client: zenoh_client::ZenohClient::new(session.clone()), local_resources: LocalResources::new("*".to_string(), "*".to_string(), session), expected_bridge_status: Arc::new(RwLock::new(BridgeStatus::default())), - }; + } } pub async fn make_zenoh_subscriber( @@ -827,19 +820,17 @@ impl BridgeChecker { where C: Fn(Sample) + Send + Sync + 'static, { - return self - .zenoh_client + self.zenoh_client .subscribe(Self::make_zenoh_key(&Self::make_topic(name)), callback) .await - .unwrap(); + .unwrap() } pub async fn make_zenoh_publisher(&self, name: &str) -> zenoh::publication::Publisher<'static> { - return self - .zenoh_client + self.zenoh_client .publish(Self::make_zenoh_key(&Self::make_topic(name))) .await - .unwrap(); + .unwrap() } pub async fn make_zenoh_queryable( @@ -850,26 +841,10 @@ impl BridgeChecker { where Callback: Fn(zenoh::queryable::Query) + Send + Sync + 'static, { - return self - .zenoh_client + self.zenoh_client .make_queryable(Self::make_zenoh_key(&Self::make_topic(name)), callback) .await - .unwrap(); - } - - pub async fn make_zenoh_query(&self, name: &str, callback: Callback, data: Vec) - where - Callback: Fn(zenoh::query::Reply) + Send + Sync + 'static, - { - return self - .zenoh_client - .make_query( - Self::make_zenoh_key(&Self::make_topic(name)), - callback, - data, - ) - .await - .unwrap(); + .unwrap() } pub async fn make_zenoh_query_sync( @@ -877,24 +852,23 @@ impl BridgeChecker { name: &str, data: Vec, ) -> flume::Receiver { - return self - .zenoh_client + self.zenoh_client .make_query_sync(Self::make_zenoh_key(&Self::make_topic(name)), data) .await - .unwrap(); + .unwrap() } pub fn make_ros_publisher(&self, name: &str) -> RAIICounter> { let status = self.expected_bridge_status.clone(); status.write().unwrap().ros_publishers.0 += 1; status.write().unwrap().ros_publishers.1 += 1; - return RAIICounter::new( + RAIICounter::new( self.ros_client.publish(&Self::make_topic(name)).unwrap(), move || { status.write().unwrap().ros_publishers.0 -= 1; status.write().unwrap().ros_publishers.1 -= 1; }, - ); + ) } pub fn make_ros_subscriber( @@ -909,7 +883,7 @@ impl BridgeChecker { let status = self.expected_bridge_status.clone(); status.write().unwrap().ros_subscribers.0 += 1; status.write().unwrap().ros_subscribers.1 += 1; - return RAIICounter::new( + RAIICounter::new( self.ros_client .subscribe(&Self::make_topic(name), callback) .unwrap(), @@ -917,20 +891,20 @@ impl BridgeChecker { status.write().unwrap().ros_subscribers.0 -= 1; status.write().unwrap().ros_subscribers.1 -= 1; }, - ); + ) } pub fn make_ros_client(&self, name: &str) -> RAIICounter> { let status = self.expected_bridge_status.clone(); status.write().unwrap().ros_clients.0 += 1; status.write().unwrap().ros_clients.1 += 1; - return RAIICounter::new( + RAIICounter::new( self.ros_client.client(&Self::make_topic(name)).unwrap(), move || { status.write().unwrap().ros_clients.0 -= 1; status.write().unwrap().ros_clients.1 -= 1; }, - ); + ) } pub fn make_ros_service(&self, name: &str, handler: F) -> RAIICounter @@ -943,7 +917,7 @@ impl BridgeChecker { let status = self.expected_bridge_status.clone(); status.write().unwrap().ros_services.0 += 1; status.write().unwrap().ros_services.1 += 1; - return RAIICounter::new( + RAIICounter::new( self.ros_client .service::(&Self::make_topic(name), handler) .unwrap(), @@ -951,15 +925,15 @@ impl BridgeChecker { status.write().unwrap().ros_services.0 -= 1; status.write().unwrap().ros_services.1 -= 1; }, - ); + ) } // PRIVATE fn make_topic(name: &str) -> rosrust::api::Topic { - return rosrust::api::Topic { + rosrust::api::Topic { name: name.to_string(), datatype: "some_datatype".to_string(), - }; + } } fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { @@ -970,14 +944,14 @@ impl BridgeChecker { #[test] #[serial(ROS1)] fn init_with_ros() { - let ros_env = ROSEnvironment::new().with_master(); + let _ros_env = ROSEnvironment::new().with_master(); let bridge = RunningBridge::new(zenoh::config::Config::default()); let checker = BridgeChecker::new(); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); - let ros_publisher = checker.make_ros_publisher("/some/ros/topic"); + let _ros_publisher = checker.make_ros_publisher("/some/ros/topic"); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on( @@ -1003,12 +977,12 @@ async fn ping_pong_duplex_parallel_many_( zenoh_core::zasync_executor_init!(); let make_keyexpr = |i: u32, mode: Mode| -> String { - return format!( + format!( "/some/key/expr{}_{}_{}", i, - mode.to_string(), + mode, UNIQUE_NUMBER.fetch_add(1, SeqCst) - ); + ) }; // create scenarios @@ -1238,7 +1212,7 @@ fn ping_pong_all_parallel_many() { async fn main_work(env: &TestEnvironment, main_work_finished: Arc) { assert!(!main_work_finished.load(Relaxed)); ping_pong_duplex_parallel_many_( - &env, + env, TestEnvironment::many_count(), HashSet::from([ Mode::ZenohToRos1, @@ -1253,7 +1227,7 @@ async fn main_work(env: &TestEnvironment, main_work_finished: Arc) { async fn parallel_subwork(env: &TestEnvironment, main_work_finished: Arc) { while !main_work_finished.load(Relaxed) { ping_pong_duplex_parallel_many_( - &env, + env, 10, HashSet::from([ Mode::ZenohToRos1, @@ -1272,7 +1246,7 @@ async fn parallel_subworks( concurrent_subwork_count: u32, ) { let mut subworks = Vec::new(); - for i in 0..concurrent_subwork_count { + for _i in 0..concurrent_subwork_count { subworks.push(parallel_subwork(env, main_work_finished.clone())); } futures::future::join_all(subworks).await; @@ -1284,7 +1258,7 @@ fn ping_pong_all_overlap_one() { let main_work_finished = Arc::new(AtomicBool::new(false)); let main_work = main_work(&env, main_work_finished.clone()); - let parallel_subworks = parallel_subworks(&env, main_work_finished.clone(), 1); + let parallel_subworks = parallel_subworks(&env, main_work_finished, 1); async_std::task::block_on(futures::future::join(main_work, parallel_subworks)); } #[test] @@ -1294,19 +1268,16 @@ fn ping_pong_all_overlap_many() { let main_work_finished = Arc::new(AtomicBool::new(false)); let main_work = main_work(&env, main_work_finished.clone()); - let parallel_subworks = parallel_subworks( - &env, - main_work_finished.clone(), - TestEnvironment::many_count(), - ); + let parallel_subworks = + parallel_subworks(&env, main_work_finished, TestEnvironment::many_count()); async_std::task::block_on(futures::future::join(main_work, parallel_subworks)); } // there were some issues with rosrust service, so there is a test to check it async fn check_query(checker: &BridgeChecker) { let name = "/some/key/expr"; - let queryable = checker.make_ros_service(name, |q| { - print!("got query!\n"); + let _queryable = checker.make_ros_service(name, |q| { + println!("got query!"); Ok(q) }); @@ -1323,7 +1294,7 @@ async fn check_query(checker: &BridgeChecker) { #[test] #[serial(ROS1)] fn check_query_() { - let ros_env = ROSEnvironment::new().with_master(); + let _ros_env = ROSEnvironment::new().with_master(); let checker = BridgeChecker::new(); futures::executor::block_on(check_query(&checker)); } From 245be3ee559db68a0ae85ba3746f7bdcba591de6 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 14 Apr 2023 16:48:53 +0300 Subject: [PATCH 06/19] Some review fixes.... --- zplugin-ros1/src/config.rs | 160 ------------------------------------- zplugin-ros1/src/lib.rs | 6 +- 2 files changed, 3 insertions(+), 163 deletions(-) delete mode 100644 zplugin-ros1/src/config.rs diff --git a/zplugin-ros1/src/config.rs b/zplugin-ros1/src/config.rs deleted file mode 100644 index 904c8d5..0000000 --- a/zplugin-ros1/src/config.rs +++ /dev/null @@ -1,160 +0,0 @@ -// -// Copyright (c) 2022 ZettaScale Technology -// -// This program and the accompanying materials are made available under the -// terms of the Eclipse Public License 2.0 which is available at -// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 -// which is available at https://www.apache.org/licenses/LICENSE-2.0. -// -// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 -// -// Contributors: -// ZettaScale Zenoh Team, -// -use regex::Regex; -use serde::{de, Deserialize, Deserializer}; -use std::env; -use std::time::Duration; -use zenoh::prelude::*; - -pub const DEFAULT_DOMAIN: u32 = 0; -pub const DEFAULT_GROUP_LEASE_SEC: f64 = 3.0; -pub const DEFAULT_FORWARD_DISCOVERY: bool = false; -pub const DEFAULT_RELIABLE_ROUTES_BLOCKING: bool = true; -pub const DEFAULT_DDS_LOCALHOST_ONLY: bool = false; - -#[derive(Deserialize, Debug)] -#[serde(deny_unknown_fields)] -pub struct Config { - #[serde(default)] - pub scope: Option, - #[serde(default = "default_domain")] - pub domain: u32, - #[serde(default)] - pub group_member_id: Option, - #[serde( - default = "default_group_lease", - deserialize_with = "deserialize_group_lease" - )] - pub group_lease: Duration, - #[serde(default, deserialize_with = "deserialize_regex")] - pub allow: Option, - #[serde(default, deserialize_with = "deserialize_regex")] - pub deny: Option, - #[serde(default, deserialize_with = "deserialize_max_frequencies")] - pub max_frequencies: Vec<(Regex, f32)>, - #[serde(default)] - pub generalise_subs: Vec, - #[serde(default)] - pub generalise_pubs: Vec, - #[serde(default = "default_forward_discovery")] - pub forward_discovery: bool, - #[serde(default = "default_reliable_routes_blocking")] - pub reliable_routes_blocking: bool, - #[serde(default = "default_localhost_only")] - pub localhost_only: bool, - #[serde(default)] - __required__: bool, - #[serde(default, deserialize_with = "deserialize_paths")] - __path__: Vec, -} - -fn default_domain() -> u32 { - if let Ok(s) = env::var("ROS_DOMAIN_ID") { - s.parse::().unwrap_or(DEFAULT_DOMAIN) - } else { - DEFAULT_DOMAIN - } -} - -fn default_group_lease() -> Duration { - Duration::from_secs_f64(DEFAULT_GROUP_LEASE_SEC) -} - -fn deserialize_group_lease<'de, D>(deserializer: D) -> Result -where - D: Deserializer<'de>, -{ - let secs: f64 = Deserialize::deserialize(deserializer)?; - Ok(Duration::from_secs_f64(secs)) -} - -fn deserialize_paths<'de, D>(deserializer: D) -> Result, D::Error> -where - D: Deserializer<'de>, -{ - struct V; - impl<'de> serde::de::Visitor<'de> for V { - type Value = Vec; - fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { - write!(formatter, "a string or vector of strings") - } - fn visit_str(self, v: &str) -> Result - where - E: de::Error, - { - Ok(vec![v.into()]) - } - fn visit_seq(self, mut seq: A) -> Result - where - A: de::SeqAccess<'de>, - { - let mut v = if let Some(l) = seq.size_hint() { - Vec::with_capacity(l) - } else { - Vec::new() - }; - while let Some(s) = seq.next_element()? { - v.push(s); - } - Ok(v) - } - } - deserializer.deserialize_any(V) -} - -fn deserialize_regex<'de, D>(deserializer: D) -> Result, D::Error> -where - D: Deserializer<'de>, -{ - let s: String = Deserialize::deserialize(deserializer)?; - Regex::new(&s) - .map(Some) - .map_err(|e| de::Error::custom(format!("Invalid regex 'allow={}': {}", s, e))) -} - -fn deserialize_max_frequencies<'de, D>(deserializer: D) -> Result, D::Error> -where - D: Deserializer<'de>, -{ - let strs: Vec = Deserialize::deserialize(deserializer)?; - let mut result: Vec<(Regex, f32)> = Vec::with_capacity(strs.len()); - for s in strs { - let i = s - .find('=') - .ok_or_else(|| de::Error::custom(format!("Invalid 'max_frequency': {}", s)))?; - let regex = Regex::new(&s[0..i]).map_err(|e| { - de::Error::custom(format!("Invalid regex for 'max_frequency': '{}': {}", s, e)) - })?; - let frequency: f32 = s[i + 1..].parse().map_err(|e| { - de::Error::custom(format!( - "Invalid float value for 'max_frequency': '{}': {}", - s, e - )) - })?; - result.push((regex, frequency)); - } - Ok(result) -} - -fn default_forward_discovery() -> bool { - DEFAULT_FORWARD_DISCOVERY -} - -fn default_reliable_routes_blocking() -> bool { - DEFAULT_RELIABLE_ROUTES_BLOCKING -} - -fn default_localhost_only() -> bool { - env::var("ROS_LOCALHOST_ONLY").as_deref() == Ok("1") -} diff --git a/zplugin-ros1/src/lib.rs b/zplugin-ros1/src/lib.rs index 9225a72..18b74c7 100644 --- a/zplugin-ros1/src/lib.rs +++ b/zplugin-ros1/src/lib.rs @@ -54,7 +54,7 @@ impl Plugin for Ros1Plugin { } } - std::mem::drop(config); + drop(config); // return a RunningPlugin to zenohd Ok(Box::new(RunningPlugin::new(runtime)?)) @@ -63,7 +63,7 @@ impl Plugin for Ros1Plugin { // The RunningPlugin struct implementing the RunningPluginTrait trait struct RunningPlugin { - _bridge: Option, + _bridge: Ros1ToZenohBridge, } impl RunningPluginTrait for RunningPlugin { // Operation returning a ValidationFunction(path, old, new)-> ZResult>> @@ -95,7 +95,7 @@ impl RunningPlugin { }); Ok(Self { - _bridge: Some(bridge?), + _bridge: bridge?, }) } } From 4043a26b7c3cfd27c3bc2c3597dd38c938689d9c Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 17 Apr 2023 13:40:00 +0300 Subject: [PATCH 07/19] Review issues: fix tests, move rosmaster ctrl to separate file --- zplugin-ros1/Cargo.toml | 2 +- .../examples/bridge_with_external_master.rs | 3 +- .../examples/bridge_with_own_master.rs | 16 +++-- zplugin-ros1/examples/ros1_pub.rs | 16 +++-- zplugin-ros1/examples/ros1_service.rs | 16 +++-- zplugin-ros1/examples/ros1_sub.rs | 16 +++-- zplugin-ros1/src/lib.rs | 4 +- zplugin-ros1/src/ros_to_zenoh_bridge.rs | 54 +------------- .../ros_to_zenoh_bridge/ros1_master_ctrl.rs | 70 +++++++++++++++++++ zplugin-ros1/tests/aloha_declaration_test.rs | 13 +++- zplugin-ros1/tests/discovery_test.rs | 24 +++++-- zplugin-ros1/tests/rosmaster_test.rs | 65 +++++++++++++++++ 12 files changed, 205 insertions(+), 94 deletions(-) create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/ros1_master_ctrl.rs create mode 100644 zplugin-ros1/tests/rosmaster_test.rs diff --git a/zplugin-ros1/Cargo.toml b/zplugin-ros1/Cargo.toml index 429636b..64e1ac8 100644 --- a/zplugin-ros1/Cargo.toml +++ b/zplugin-ros1/Cargo.toml @@ -18,7 +18,7 @@ authors = [ "Dmitrii Bannov ", ] edition = "2021" -repository = "https://github.com/ZettaScaleLabs/zenoh-plugin-ros1" +repository = "https://github.com/eclipse-zenoh/zenoh-plugin-ros1" homepage = "http://zenoh.io" license = " EPL-2.0 OR Apache-2.0" categories = ["network-programming"] diff --git a/zplugin-ros1/examples/bridge_with_external_master.rs b/zplugin-ros1/examples/bridge_with_external_master.rs index 1fa192b..a2d3d5b 100644 --- a/zplugin-ros1/examples/bridge_with_external_master.rs +++ b/zplugin-ros1/examples/bridge_with_external_master.rs @@ -23,8 +23,7 @@ async fn main() { // create bridge with ROS1 master // In this example the bridge will connect to master specified by ROS_MASTER_URI env variable (default http://localhost:11311/) print!("Starting Bridge..."); - #[allow(unused_variables)] - let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()).await; + let _bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()).await; println!(" OK!"); println!("Running bridge, press Ctrl+C to exit..."); diff --git a/zplugin-ros1/examples/bridge_with_own_master.rs b/zplugin-ros1/examples/bridge_with_own_master.rs index 23667e3..6fe9b89 100644 --- a/zplugin-ros1/examples/bridge_with_own_master.rs +++ b/zplugin-ros1/examples/bridge_with_own_master.rs @@ -14,21 +14,23 @@ use std::future; -use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; +use zplugin_ros1::ros_to_zenoh_bridge::{ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge}; #[async_std::main] async fn main() { // initiate logging env_logger::init(); - // create bridge with ROS1 master // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. - // In this example the bridge will start ROS1 master by itself. - print!("Starting Bridge..."); - #[allow(unused_variables)] - let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) + // start ROS1 master... + print!("Starting ROS1 Master..."); + Ros1MasterCtrl::with_ros1_master() .await - .with_ros1_master(); + .expect("Error starting rosmaster!"); + + // create bridge + print!("Starting Bridge..."); + let _bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()).await; println!(" OK!"); println!("Running bridge, press Ctrl+C to exit..."); diff --git a/zplugin-ros1/examples/ros1_pub.rs b/zplugin-ros1/examples/ros1_pub.rs index 69c06d3..aa408b2 100644 --- a/zplugin-ros1/examples/ros1_pub.rs +++ b/zplugin-ros1/examples/ros1_pub.rs @@ -13,21 +13,23 @@ // use zenoh_core::AsyncResolve; -use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; +use zplugin_ros1::ros_to_zenoh_bridge::{ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge}; #[async_std::main] async fn main() { // initiate logging env_logger::init(); - // create bridge with ROS1 master // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. - // In this example the bridge will start ROS1 master by itself. - print!("Starting Bridge..."); - #[allow(unused_variables)] - let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) + // start ROS1 master... + print!("Starting ROS1 Master..."); + Ros1MasterCtrl::with_ros1_master() .await - .with_ros1_master(); + .expect("Error starting rosmaster!"); + + // create bridge + print!("Starting Bridge..."); + let _bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()).await; println!(" OK!"); // create ROS1 node and publisher diff --git a/zplugin-ros1/examples/ros1_service.rs b/zplugin-ros1/examples/ros1_service.rs index f263b39..6555101 100644 --- a/zplugin-ros1/examples/ros1_service.rs +++ b/zplugin-ros1/examples/ros1_service.rs @@ -15,21 +15,23 @@ use zenoh::prelude::SplitBuffer; use zenoh_core::AsyncResolve; -use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; +use zplugin_ros1::ros_to_zenoh_bridge::{ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge}; #[async_std::main] async fn main() { // initiate logging env_logger::init(); - // create bridge with ROS1 master // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. - // In this example the bridge will start ROS1 master by itself. - print!("Starting Bridge..."); - #[allow(unused_variables)] - let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) + // start ROS1 master... + print!("Starting ROS1 Master..."); + Ros1MasterCtrl::with_ros1_master() .await - .with_ros1_master(); + .expect("Error starting rosmaster!"); + + // create bridge + print!("Starting Bridge..."); + let _bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()).await; println!(" OK!"); // create ROS1 node and service diff --git a/zplugin-ros1/examples/ros1_sub.rs b/zplugin-ros1/examples/ros1_sub.rs index 24bf6bd..0b5f299 100644 --- a/zplugin-ros1/examples/ros1_sub.rs +++ b/zplugin-ros1/examples/ros1_sub.rs @@ -14,21 +14,23 @@ use zenoh_core::AsyncResolve; -use zplugin_ros1::ros_to_zenoh_bridge::Ros1ToZenohBridge; +use zplugin_ros1::ros_to_zenoh_bridge::{ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge}; #[async_std::main] async fn main() { // initiate logging env_logger::init(); - // create bridge with ROS1 master // You need to have ros1 installed within your system and have "rosmaster" command available, otherwise this code will fail. - // In this example the bridge will start ROS1 master by itself. - print!("Starting Bridge..."); - #[allow(unused_variables)] - let bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()) + // start ROS1 master... + print!("Starting ROS1 Master..."); + Ros1MasterCtrl::with_ros1_master() .await - .with_ros1_master(); + .expect("Error starting rosmaster!"); + + // create bridge + print!("Starting Bridge..."); + let _bridge = Ros1ToZenohBridge::new_with_own_session(zenoh::config::default()).await; println!(" OK!"); // create ROS1 node and subscriber diff --git a/zplugin-ros1/src/lib.rs b/zplugin-ros1/src/lib.rs index 18b74c7..a2beb99 100644 --- a/zplugin-ros1/src/lib.rs +++ b/zplugin-ros1/src/lib.rs @@ -94,8 +94,6 @@ impl RunningPlugin { Ok(bridge) }); - Ok(Self { - _bridge: bridge?, - }) + Ok(Self { _bridge: bridge? }) } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge.rs index fb402dd..2972aa7 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge.rs @@ -12,9 +12,8 @@ // ZettaScale Zenoh Team, // -use async_std::{process::Child, task::JoinHandle}; +use async_std::task::JoinHandle; -use log::error; use zenoh; use zenoh_core::AsyncResolve; @@ -23,8 +22,6 @@ use std::sync::{ Arc, }; -use async_std::process::Command; - use self::ros1_to_zenoh_bridge_impl::work_cycle; pub mod abstract_bridge; @@ -36,17 +33,17 @@ pub mod zenoh_client; mod bridges_storage; mod topic_mapping; -mod topic_utilities; pub mod aloha_declaration; pub mod aloha_subscription; pub mod environment; +pub mod ros1_master_ctrl; pub mod ros1_to_zenoh_bridge_impl; +pub mod topic_utilities; pub struct Ros1ToZenohBridge { flag: Arc, task_handle: Box>, - rosmaster: Option, } impl Ros1ToZenohBridge { pub async fn new_with_own_session(config: zenoh::config::Config) -> Self { @@ -59,52 +56,7 @@ impl Ros1ToZenohBridge { Self { flag: flag.clone(), task_handle: Box::new(async_std::task::spawn(Self::run(session, flag))), - rosmaster: None, - } - } - - pub fn with_ros1_master(mut self) -> Self { - assert!(self.rosmaster.is_none()); - match Command::new("rosmaster") - .stdout(std::process::Stdio::piped()) - .stderr(std::process::Stdio::piped()) - .spawn() - { - Ok(child) => { - self.rosmaster = Some(child); - } - Err(e) => { - error!("Error starting rosmaster: {}", e); - } - } - self - } - - pub async fn without_ros1_master(mut self) -> Self { - assert!(self.rosmaster.is_some()); - - match &mut self.rosmaster { - Some(child) => { - if child.kill().is_ok() { - if let Err(e) = child.status().await { - error!("Error stopping child rosmaster: {}", e); - } - } - self.rosmaster = None; - } - None => match Command::new("killall").arg("rosmaster").spawn() { - Ok(mut child) => { - if let Err(e) = child.status().await { - error!("Error stopping foreign rosmaster: {}", e); - } - } - Err(e) => error!( - "Error executing killall command to stop foreign rosmaster: {}", - e - ), - }, } - self } pub async fn async_await(&mut self) { diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_master_ctrl.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_master_ctrl.rs new file mode 100644 index 0000000..cb4377c --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_master_ctrl.rs @@ -0,0 +1,70 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use async_std::{ + process::{Child, Command}, + sync::Mutex, +}; +use log::error; +use zenoh::plugins::ZResult; +use zenoh_core::bail; + +static ROSMASTER: Mutex> = Mutex::new(None); + +pub struct Ros1MasterCtrl; +impl Ros1MasterCtrl { + pub async fn with_ros1_master() -> ZResult<()> { + let mut locked = ROSMASTER.lock().await; + assert!(locked.is_none()); + match Command::new("rosmaster") + .stdout(std::process::Stdio::piped()) + .stderr(std::process::Stdio::piped()) + .spawn() + { + Ok(child) => { + let _ = locked.insert(child); + Ok(()) + } + Err(e) => { + bail!("Error starting rosmaster: {}", e); + } + } + } + + pub async fn without_ros1_master() { + let mut locked = ROSMASTER.lock().await; + assert!(locked.is_some()); + match locked.take() { + Some(mut child) => match child.kill() { + Ok(_) => { + if let Err(e) = child.status().await { + error!("Error stopping child rosmaster: {}", e); + } + } + Err(e) => error!("Error sending kill cmd to child rosmaster: {}", e), + }, + None => match Command::new("killall").arg("rosmaster").spawn() { + Ok(mut child) => { + if let Err(e) = child.status().await { + error!("Error stopping foreign rosmaster: {}", e); + } + } + Err(e) => error!( + "Error executing killall command to stop foreign rosmaster: {}", + e + ), + }, + } + } +} diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs index cfc7e18..a287818 100644 --- a/zplugin-ros1/tests/aloha_declaration_test.rs +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -14,6 +14,7 @@ use std::{ collections::HashSet, + net::SocketAddr, str::FromStr, sync::{atomic::AtomicUsize, Arc, Mutex}, time::Duration, @@ -28,7 +29,13 @@ use zplugin_ros1::ros_to_zenoh_bridge::{aloha_declaration, aloha_subscription}; const TIMEOUT: Duration = Duration::from_secs(30); fn session_builder() -> OpenBuilder { - zenoh::open(zenoh::config::peer()) + let mut config = zenoh::config::peer(); + config + .scouting + .multicast + .set_address(Some(SocketAddr::from_str("224.0.0.224:16000").unwrap())) + .unwrap(); + zenoh::open(config) } fn declaration_builder( @@ -65,7 +72,7 @@ fn make_subscription( } #[test] -#[serial(ROS1)] +#[serial(Aloha)] fn discovery_instantination_one_instance() { let session = make_session(); let _declaration = declaration_builder(session.clone(), Duration::from_secs(1)); @@ -73,7 +80,7 @@ fn discovery_instantination_one_instance() { } #[test] -#[serial(ROS1)] +#[serial(Aloha)] fn discovery_instantination_many_instances() { let mut sessions = Vec::new(); let mut declarations = Vec::new(); diff --git a/zplugin-ros1/tests/discovery_test.rs b/zplugin-ros1/tests/discovery_test.rs index bba8a0e..a10fae1 100644 --- a/zplugin-ros1/tests/discovery_test.rs +++ b/zplugin-ros1/tests/discovery_test.rs @@ -13,6 +13,8 @@ // use std::{ + net::SocketAddr, + str::FromStr, sync::{Arc, Mutex}, time::Duration, }; @@ -20,14 +22,19 @@ use std::{ use async_std::prelude::FutureExt; use multiset::HashMultiSet; use serial_test::serial; -use zenoh::{config::ModeDependentValue, OpenBuilder, Session}; +use zenoh::{config::ModeDependentValue, prelude::keyexpr, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; -use zplugin_ros1::ros_to_zenoh_bridge::discovery; +use zplugin_ros1::ros_to_zenoh_bridge::{discovery, topic_utilities::make_topic}; const TIMEOUT: Duration = Duration::from_secs(10); fn session_builder() -> OpenBuilder { let mut config = zenoh::config::peer(); + config + .scouting + .multicast + .set_address(Some(SocketAddr::from_str("224.0.0.224:16001").unwrap())) + .unwrap(); config .timestamping .set_enabled(Some(ModeDependentValue::Unique(true))) @@ -158,10 +165,15 @@ async fn generate_topics( let mut result = HashMultiSet::new(); for number in 0..count { for _dup in 0..duplication { - result.insert(rosrust::api::Topic { - name: format!("name_{}_{}", number, stage), - datatype: "some".to_string(), - }); + unsafe { + let topic = make_topic( + keyexpr::from_str_unchecked("some"), + keyexpr::from_str_unchecked(format!("name_{}_{}", number, stage).as_str()), + ) + .unwrap(); + + result.insert(topic); + } } } result diff --git a/zplugin-ros1/tests/rosmaster_test.rs b/zplugin-ros1/tests/rosmaster_test.rs new file mode 100644 index 0000000..4a378a1 --- /dev/null +++ b/zplugin-ros1/tests/rosmaster_test.rs @@ -0,0 +1,65 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::time::Duration; + +use serial_test::serial; +use zplugin_ros1::ros_to_zenoh_bridge::ros1_master_ctrl::Ros1MasterCtrl; + +#[test] +#[serial(ROS1)] +fn start_and_stop_master() { + async_std::task::block_on(async { + Ros1MasterCtrl::with_ros1_master() + .await + .expect("Error starting rosmaster!"); + + Ros1MasterCtrl::without_ros1_master().await; + }); +} + +#[test] +#[serial(ROS1)] +fn start_and_stop_master_and_check_connectivity() { + // start rosmaster + async_std::task::block_on(async { + Ros1MasterCtrl::with_ros1_master() + .await + .expect("Error starting rosmaster!"); + }); + + // start ros1 client + let ros1_client = + rosrust::api::Ros::new("start_and_stop_master_and_check_connectivity_client").unwrap(); + + // wait for correct status from rosmaster.... + let mut has_rosmaster = false; + for _i in 0..1000 { + if ros1_client.state().is_ok() { + has_rosmaster = true; + break; + } + std::thread::sleep(Duration::from_millis(10)); + } + + // stop rosmaster + async_std::task::block_on(async { + Ros1MasterCtrl::without_ros1_master().await; + }); + + // check if there was a status from rosmaster... + if !has_rosmaster { + panic!("cannot see rosmaster!"); + } +} From 5116a480d1562c905017e4d9e294093e8c2467ee Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 17 Apr 2023 20:24:54 +0300 Subject: [PATCH 08/19] Fixes for review, fix tests interference --- .../src/ros_to_zenoh_bridge/topic_bridge.rs | 2 +- .../ros_to_zenoh_bridge/topic_utilities.rs | 2 +- .../src/ros_to_zenoh_bridge/zenoh_client.rs | 90 ++++++------------- zplugin-ros1/tests/aloha_declaration_test.rs | 6 +- zplugin-ros1/tests/ping_pong_test.rs | 73 +++++++++------ 5 files changed, 79 insertions(+), 94 deletions(-) diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs index 9fb9d72..3b4739c 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs @@ -143,7 +143,7 @@ impl TopicBridge { } Err(e) => { self.bridge = None; - error!("Errr creating bridge: {}", e); + error!("Error creating bridge: {}", e); } } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs index c819c62..e32bec1 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs @@ -15,7 +15,7 @@ use zenoh::prelude::keyexpr; pub fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { - return topic.name.trim_start_matches('/').trim_end_matches('/'); + topic.name.trim_start_matches('/').trim_end_matches('/') } pub fn make_topic(datatype: &keyexpr, topic_name: &keyexpr) -> Result { diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs index a8384b2..1ef1d33 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs @@ -40,33 +40,23 @@ impl ZenohClient { { debug!("Creating Subscriber on {}", key_expr); - match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - self.session - .declare_subscriber(opt_keyexpr) - .callback(callback) - .allowed_origin(Locality::Remote) - .res_async() - .await - } - Err(e) => Err(e), - } + self.session + .declare_subscriber(key_expr) + .callback(callback) + .allowed_origin(Locality::Remote) + .res_async() + .await } pub async fn publish(&self, key_expr: &str) -> ZResult> { debug!("Creating Publisher on {}", key_expr); - match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - self.session - .declare_publisher(opt_keyexpr) - .allowed_destination(Locality::Remote) - .congestion_control(CongestionControl::Block) - .res_async() - .await - } - Err(e) => Err(e), - } + self.session + .declare_publisher(key_expr.to_owned()) + .allowed_destination(Locality::Remote) + .congestion_control(CongestionControl::Block) + .res_async() + .await } pub async fn make_queryable( @@ -79,17 +69,12 @@ impl ZenohClient { { info!("Creating Queryable on {}", key_expr); - match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - self.session - .declare_queryable(opt_keyexpr) - .allowed_origin(Locality::Remote) - .callback(callback) - .res_async() - .await - } - Err(e) => Err(e), - } + self.session + .declare_queryable(key_expr) + .allowed_origin(Locality::Remote) + .callback(callback) + .res_async() + .await } pub async fn make_query( @@ -103,17 +88,13 @@ impl ZenohClient { { debug!("Creating Query on {}", key_expr); - match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - self.session - .get(opt_keyexpr) - .with_value(data) - .callback(callback) - .res_async() - .await - } - Err(e) => Err(e), - } + self.session + .get(key_expr) + .with_value(data) + .callback(callback) + .allowed_destination(Locality::Remote) + .res_async() + .await } pub async fn make_query_sync( @@ -123,25 +104,10 @@ impl ZenohClient { ) -> ZResult> { debug!("Creating Query on {}", key_expr); - match self.make_keyexpr(key_expr).await { - Ok(opt_keyexpr) => { - self.session - .get(opt_keyexpr) - .with_value(data) - .allowed_destination(Locality::Remote) - .res_async() - .await - } - Err(e) => Err(e), - } - } - - // PRIVATE - async fn make_keyexpr(&self, key_expr: &str) -> ZResult> { - debug!("Create optimized keyexpr {}", key_expr); - self.session - .declare_keyexpr(key_expr.to_string()) + .get(key_expr) + .with_value(data) + .allowed_destination(Locality::Remote) .res_async() .await } diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs index a287818..6d410b9 100644 --- a/zplugin-ros1/tests/aloha_declaration_test.rs +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -73,7 +73,7 @@ fn make_subscription( #[test] #[serial(Aloha)] -fn discovery_instantination_one_instance() { +fn aloha_instantination_one_instance() { let session = make_session(); let _declaration = declaration_builder(session.clone(), Duration::from_secs(1)); let _subscription = make_subscription(session, Duration::from_secs(1)); @@ -81,11 +81,11 @@ fn discovery_instantination_one_instance() { #[test] #[serial(Aloha)] -fn discovery_instantination_many_instances() { +fn aloha_instantination_many_instances() { let mut sessions = Vec::new(); let mut declarations = Vec::new(); let mut subscriptions = Vec::new(); - for _i in 0..10 { + for _ in 0..10 { let session = make_session(); sessions.push(session.clone()); declarations.push(declaration_builder(session.clone(), Duration::from_secs(1))); diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 4b9821c..2286f22 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -14,7 +14,7 @@ use strum_macros::Display; use zenoh::prelude::SplitBuffer; -use zenoh_core::SyncResolve; +use zenoh_core::{zresult::ZResult, bail, SyncResolve}; use std::{ collections::HashSet, @@ -162,7 +162,7 @@ impl Drop for RunningBridge { #[serial(ROS1)] fn env_checks_no_master_init_and_exit_immed() { let _ros_env = ROSEnvironment::new(); - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); } @@ -171,7 +171,7 @@ fn env_checks_no_master_init_and_exit_immed() { #[serial(ROS1)] fn env_checks_no_master_init_and_wait() { let _ros_env = ROSEnvironment::new(); - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -183,7 +183,7 @@ fn env_checks_no_master_init_and_wait() { #[serial(ROS1)] fn env_checks_with_master_init_and_exit_immed() { let _ros_env = ROSEnvironment::new().with_master(); - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); } @@ -192,7 +192,7 @@ fn env_checks_with_master_init_and_exit_immed() { #[serial(ROS1)] fn env_checks_with_master_init_and_wait() { let _ros_env = ROSEnvironment::new().with_master(); - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -204,7 +204,7 @@ fn env_checks_with_master_init_and_wait() { #[serial(ROS1)] fn env_checks_with_master_init_and_loose_master() { let mut _ros_env = Some(ROSEnvironment::new().with_master()); - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -222,7 +222,7 @@ fn env_checks_with_master_init_and_loose_master() { #[serial(ROS1)] fn env_checks_with_master_init_and_wait_for_master() { let mut _ros_env = ROSEnvironment::new(); - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -240,7 +240,7 @@ fn env_checks_with_master_init_and_wait_for_master() { #[serial(ROS1)] fn env_checks_with_master_init_and_reconnect_many_times_to_master() { let mut ros_env = ROSEnvironment::new(); - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); for _i in 0..20 { async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); @@ -274,6 +274,28 @@ impl ZenohQuery { } } + async fn make_query(inner: &Arc, key: &str, data: &Vec) -> ZResult<()> { + let query = inner.make_zenoh_query_sync(key, data.clone()).await; + match query.recv_async().await { + Ok(reply) => match reply.sample { + Ok(value) => { + let returned_data = value.payload.contiguous().to_vec(); + if data.eq(&returned_data) { + Ok(()) + } else { + bail!("ZenohQuery: data is not equal! \n Sent data: {:?} \nReturned data: {:?}", data, returned_data); + } + } + Err(e) => { + bail!("ZenohQuery: got reply with error: {}", e); + } + }, + Err(e) => { + bail!("ZenohQuery: failed to get reply with error: {}", e); + } + } + } + async fn query_loop( inner: Arc, key: String, @@ -282,24 +304,12 @@ impl ZenohQuery { cycles: Arc, ) { while running.load(Relaxed) { - let query = inner - .make_zenoh_query_sync(key.as_str(), data.clone()) - .await; - match query.recv_async().await { - Ok(reply) => match reply.sample { - Ok(value) => { - let returned_data = value.payload.contiguous().to_vec(); - assert!(data.eq(&returned_data)); - cycles.fetch_add(1, SeqCst); - } - Err(e) => { - error!("ZenohQuery: got reply with error: {}", e); - break; - } - }, + match Self::make_query(&inner, &key, &data).await { + Ok(_) => { + cycles.fetch_add(1, SeqCst); + } Err(e) => { - error!("ZenohQuery: failed to get reply with error: {}", e); - break; + error!("{}", e); } } } @@ -398,6 +408,15 @@ impl Publisher for ZenohQuery { self.cycles.clone(), )); } + + fn ready(&self) -> bool { + let data = (0..10).collect(); + async_std::task::block_on( + async move { + Self::make_query(&self.inner, &self.key, &data).await + } + ).is_ok() + } } impl Publisher for ROS1Client { fn put(&self, data: Vec) { @@ -707,7 +726,7 @@ impl TestEnvironment { let ros_env = ROSEnvironment::new().with_master(); // start bridge - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); // start checker's engine let checker = Arc::new(BridgeChecker::new()); @@ -945,7 +964,7 @@ impl BridgeChecker { #[serial(ROS1)] fn init_with_ros() { let _ros_env = ROSEnvironment::new().with_master(); - let bridge = RunningBridge::new(zenoh::config::Config::default()); + let bridge = RunningBridge::new(zenoh::config::peer()); let checker = BridgeChecker::new(); async_std::task::block_on(bridge.assert_ros_ok()); From f2439e1a70419eb692896747169eedd4fc73cac1 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 18 Apr 2023 11:09:42 +0300 Subject: [PATCH 09/19] Fix review issues, some improvements --- .../ros_to_zenoh_bridge/aloha_declaration.rs | 16 +-- .../ros_to_zenoh_bridge/aloha_subscription.rs | 56 +++----- .../src/ros_to_zenoh_bridge/discovery.rs | 41 ++---- .../ros1_to_zenoh_bridge_impl.rs | 126 ++++++++---------- zplugin-ros1/tests/aloha_declaration_test.rs | 44 +++--- zplugin-ros1/tests/discovery_test.rs | 9 +- zplugin-ros1/tests/ping_pong_test.rs | 117 +++++++++------- 7 files changed, 184 insertions(+), 225 deletions(-) diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs index f0ee4c9..1784eca 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_declaration.rs @@ -70,11 +70,9 @@ impl AlohaDeclaration { key.clone(), session.clone(), beacon_task_flag.clone(), - ) - .await; + ); while monitor_running.load(std::sync::atomic::Ordering::Relaxed) { - async_std::task::sleep(beacon_period).await; match remote_beacons.fetch_and(0, std::sync::atomic::Ordering::SeqCst) { 0 => { if !sending_beacons { @@ -91,24 +89,24 @@ impl AlohaDeclaration { key.clone(), session.clone(), beacon_task_flag.clone(), - ) - .await; + ); sending_beacons = true; } } } _ => { if sending_beacons && rand::random::() { - Self::stop_beacon_task(beacon_task_flag.clone()).await; + Self::stop_beacon_task(beacon_task_flag.clone()); sending_beacons = false; } } } + async_std::task::sleep(beacon_period).await; } - Self::stop_beacon_task(beacon_task_flag).await; + Self::stop_beacon_task(beacon_task_flag.clone()); } - async fn start_beacon_task( + fn start_beacon_task( beacon_period: Duration, key: OwnedKeyExpr, session: Arc, @@ -123,7 +121,7 @@ impl AlohaDeclaration { )); } - async fn stop_beacon_task(running: Arc) { + fn stop_beacon_task(running: Arc) { running.store(false, std::sync::atomic::Ordering::Relaxed); } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs index e5a2f79..4a43370 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs @@ -14,7 +14,7 @@ use std::{ cell::Cell, - collections::{btree_map::Entry::*, BTreeMap}, + collections::{hash_map::Entry::*, HashMap}, sync::{ atomic::{AtomicBool, Ordering::Relaxed}, Arc, @@ -50,7 +50,7 @@ impl AlohaResource { } } -pub type TCallback = dyn Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> +pub type TCallback = dyn Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send> + Send + Sync + 'static; @@ -73,9 +73,7 @@ impl AlohaSubscription { on_resource_undeclared: F, ) -> ZResult where - F: Fn( - zenoh::key_expr::KeyExpr, - ) -> Box + Unpin + Send + Sync> + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send> + Send + Sync + 'static, @@ -104,14 +102,12 @@ impl AlohaSubscription { on_resource_undeclared: F, ) -> ZResult<()> where - F: Fn( - zenoh::key_expr::KeyExpr, - ) -> Box + Unpin + Send + Sync> + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send> + Send + Sync + 'static, { - let mut accumulating_resources = Cell::new(BTreeMap::::new()); + let mut accumulating_resources = Cell::new(HashMap::::new()); let subscriber = session.declare_subscriber(key).res_async().await?; Self::accumulating_task( @@ -129,33 +125,26 @@ impl AlohaSubscription { async fn listening_task<'a, F>( task_running: Arc, - accumulating_resources: &mut Cell>, + accumulating_resources: &mut Cell>, subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver>, on_resource_declared: &F, ) where - F: Fn( - zenoh::key_expr::KeyExpr, - ) -> Box + Unpin + Send + Sync> + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send> + Send + Sync + 'static, { while task_running.load(Relaxed) { match subscriber.recv_async().await { - Ok(val) => { - match accumulating_resources - .get_mut() - .entry(val.key_expr.to_string()) - { - Occupied(mut val) => { - val.get_mut().update(); - } - Vacant(entry) => { - entry.insert(AlohaResource::new()); - on_resource_declared(val.key_expr).await; - } + Ok(val) => match accumulating_resources.get_mut().entry(val.key_expr.into()) { + Occupied(mut val) => { + val.get_mut().update(); } - } + Vacant(entry) => { + on_resource_declared(entry.key().into()).await; + entry.insert(AlohaResource::new()); + } + }, Err(e) => { error!("Listening error: {}", e); } @@ -166,14 +155,12 @@ impl AlohaSubscription { async fn accumulating_task<'a, F>( task_running: Arc, accumulate_period: Duration, - accumulating_resources: &mut Cell>, + accumulating_resources: &mut Cell>, subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver>, on_resource_declared: F, on_resource_undeclared: F, ) where - F: Fn( - zenoh::key_expr::KeyExpr, - ) -> Box + Unpin + Send + Sync> + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send> + Send + Sync + 'static, @@ -205,10 +192,7 @@ impl AlohaSubscription { for (key, val) in accumulating_resources.get_mut().iter() { if !val.is_active() { - unsafe { - on_resource_undeclared(zenoh::key_expr::KeyExpr::from_str_uncheckend(key)) - .await - }; + on_resource_undeclared(key.into()).await; } } @@ -241,7 +225,7 @@ impl AlohaSubscriptionBuilder { pub fn on_resource_declared(mut self, on_resource_declared: F) -> Self where - F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send> + Send + Sync + 'static, @@ -252,7 +236,7 @@ impl AlohaSubscriptionBuilder { pub fn on_resource_undeclared(mut self, on_resource_undeclared: F) -> Self where - F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send + Sync> + F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send> + Send + Sync + 'static, diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs index 000f7e8..7d5c950 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/discovery.rs @@ -54,10 +54,7 @@ impl RemoteResources { on_lost: F, ) -> Self where - F: Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync> + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send> + Send + Sync + 'static, @@ -114,10 +111,7 @@ impl RemoteResources { // PRIVATE: async fn process(data: KeyExpr<'_>, callback: Arc) where - F: Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync> + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send> + Send + Sync + 'static, @@ -137,10 +131,7 @@ impl RemoteResources { async fn parse_format(data: &KeyExpr<'_>, callback: &Arc) -> Result<(), String> where - F: Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync>, + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send>, { let discovery = discovery_format::parse(data).map_err(|err| err.to_string())?; Self::handle_format(discovery, callback).await @@ -151,10 +142,7 @@ impl RemoteResources { callback: &Arc, ) -> Result<(), String> where - F: Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync>, + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send>, { //let discovery_namespace = discovery.discovery_namespace().ok_or("No discovery_namespace present!")?; let datatype = discovery.data_type().ok_or("No data_type present!")?; @@ -292,10 +280,7 @@ impl Discovery { on_lost: F, ) -> Self where - F: Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync> + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send> + Send + Sync + 'static, @@ -317,7 +302,7 @@ impl Discovery { } } -pub type TCallback = dyn Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send + Sync> +pub type TCallback = dyn Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send> + Send + Sync + 'static; @@ -346,12 +331,9 @@ impl DiscoveryBuilder { } } - pub fn on_discovered(&mut self, on_discovered: F) -> &mut Self + pub fn on_discovered(mut self, on_discovered: F) -> Self where - F: Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync> + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send> + Send + Sync + 'static, @@ -359,12 +341,9 @@ impl DiscoveryBuilder { self.on_discovered = Some(Box::new(on_discovered)); self } - pub fn on_lost(&mut self, on_lost: F) -> &mut Self + pub fn on_lost(mut self, on_lost: F) -> Self where - F: Fn( - BridgeType, - rosrust::api::Topic, - ) -> Box + Unpin + Send + Sync> + F: Fn(BridgeType, rosrust::api::Topic) -> Box + Unpin + Send> + Send + Sync + 'static, diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs index d29e6ca..ebf91a8 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs @@ -12,8 +12,7 @@ // ZettaScale Zenoh Team, // -use async_std::sync::{Mutex, MutexGuard}; -use futures::{pin_mut, select, FutureExt}; +use async_std::sync::Mutex; use zenoh; @@ -29,7 +28,10 @@ use crate::ros_to_zenoh_bridge::{ ros1_client, topic_mapping, zenoh_client, }; -use super::{discovery::DiscoveryBuilder, ros1_client::Ros1Client}; +use super::{ + discovery::{Discovery, DiscoveryBuilder}, + ros1_client::Ros1Client, +}; #[derive(PartialEq, Clone, Copy)] pub enum RosStatus { @@ -73,75 +75,46 @@ pub async fn work_cycle( local_resources, ))); - let mut bridge = RosToZenohBridge::new(ros_status_callback, statistics_callback); - - let discovery = make_discovery(session.clone(), bridges.clone()).fuse(); - - let run = bridge.run(ros1_client, bridges, flag).fuse(); + let _discovery = make_discovery(session.clone(), bridges.clone()).await; - pin_mut!(discovery, run); - - select! { - _ = discovery => {}, - _ = run => {} - }; + let mut bridge = RosToZenohBridge::new(ros_status_callback, statistics_callback); + bridge.run(ros1_client, bridges, flag).await; } -async fn make_discovery<'a>(session: Arc, bridges: Arc>) { - let mut builder = DiscoveryBuilder::new("*".to_string(), "*".to_string(), session); - - let (add_tx, add_rx) = flume::unbounded(); - let (rem_tx, rem_rx) = flume::unbounded(); - - let add_tx = Arc::new(add_tx); - let rem_tx = Arc::new(rem_tx); +async fn make_discovery<'a>( + session: Arc, + bridges: Arc>, +) -> Discovery { + let bridges2 = bridges.clone(); + let builder = DiscoveryBuilder::new("*".to_string(), "*".to_string(), session); builder .on_discovered(move |b_type, topic| { - let add_tx = add_tx.clone(); + let bridges = bridges.clone(); Box::new(Box::pin(async move { - match add_tx.send_async((b_type, topic)).await { - Ok(_) => {} - Err(e) => { - error!("Error posting topic discoery to channel: {}", e); - } - } + bridges + .lock() + .await + .bridges() + .complementary_for(b_type) + .complementary_entity_discovered(topic) + .await; })) }) .on_lost(move |b_type, topic| { - let rem_tx = rem_tx.clone(); + let bridges = bridges2.clone(); Box::new(Box::pin(async move { - match rem_tx.send_async((b_type, topic)).await { - Ok(_) => {} - Err(e) => { - error!("Error posting topic loss to channel: {}", e); - } - } + bridges + .lock() + .await + .bridges() + .complementary_for(b_type) + .complementary_entity_lost(topic) + .await; })) - }); - - let _discovery = builder.build().await; - - loop { - select! { - add = add_rx.recv_async().fuse() => { - match add { - Ok((b_type, topic)) => { - bridges.lock().await.bridges().complementary_for(b_type).complementary_entity_discovered(topic).await; - } - Err(_) => todo!(), - } - } - rem = rem_rx.recv_async().fuse() => { - match rem { - Ok((b_type, topic)) => { - bridges.lock().await.bridges().complementary_for(b_type).complementary_entity_lost(topic).await; - } - Err(_) => todo!(), - } - } - } - } + }) + .build() + .await } struct RosToZenohBridge @@ -189,26 +162,35 @@ where debug!("ros state: {:#?}", ros1_state); - let mut locked = bridges.lock().await; match ros1_state { Ok(mut ros1_state_val) => { self.transit_ros_status(RosStatus::Ok); - if locked.receive_ros1_state(&mut ros1_state_val).await { - self.report_bridge_statistics(&locked).await; - async_std::task::sleep(core::time::Duration::from_millis(100)).await; - } else { - self.report_bridge_statistics(&locked).await; - async_std::task::sleep(core::time::Duration::from_millis(200)).await; + let smth_changed; + { + let mut locked = bridges.lock().await; + smth_changed = locked.receive_ros1_state(&mut ros1_state_val).await; + self.report_bridge_statistics(&locked); } + + async_std::task::sleep({ + if smth_changed { + core::time::Duration::from_millis(100) + } else { + core::time::Duration::from_millis(200) + } + }) + .await; } Err(e) => { error!("Error reading ROS state: {}", e); self.transit_ros_status(RosStatus::Error); - Self::cleanup(&mut locked); - self.report_bridge_statistics(&locked).await; - + { + let mut locked = bridges.lock().await; + Self::cleanup(&mut locked); + self.report_bridge_statistics(&locked); + } async_std::task::sleep(core::time::Duration::from_millis(500)).await; } } @@ -223,11 +205,11 @@ where } } - async fn report_bridge_statistics(&self, locked: &MutexGuard<'_, BridgesStorage>) { + fn report_bridge_statistics(&self, locked: &BridgesStorage) { (self.statistics_callback)(locked.status()); } - fn cleanup(locked: &mut MutexGuard<'_, BridgesStorage>) { + fn cleanup(locked: &mut BridgesStorage) { locked.clear(); } } diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs index 6d410b9..96ffad9 100644 --- a/zplugin-ros1/tests/aloha_declaration_test.rs +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -22,7 +22,7 @@ use std::{ use async_std::prelude::FutureExt; use serial_test::serial; -use zenoh::{plugins::ZResult, OpenBuilder, Session}; +use zenoh::{plugins::ZResult, prelude::OwnedKeyExpr, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; use zplugin_ros1::ros_to_zenoh_bridge::{aloha_declaration, aloha_subscription}; @@ -132,10 +132,10 @@ impl<'a> PPCMeasurement<'a> { } struct DeclarationCollector { - resources: Arc>>>, + resources: Arc>>, - to_be_declared: Arc>>>, - to_be_undeclared: Arc>>>, + to_be_declared: Arc>>, + to_be_undeclared: Arc>>, } impl DeclarationCollector { fn new() -> Self { @@ -158,14 +158,22 @@ impl DeclarationCollector { builder = builder .on_resource_declared(move |k| { - assert!(declared.lock().unwrap().remove(&k.clone().into_owned())); - assert!(r.lock().unwrap().insert(k.into_owned())); - Box::new(Box::pin(async {})) + let declared = declared.clone(); + let r = r.clone(); + let k_owned = OwnedKeyExpr::from(k); + Box::new(Box::pin(async move { + assert!(declared.lock().unwrap().remove::(&k_owned)); + assert!(r.lock().unwrap().insert(k_owned)); + })) }) .on_resource_undeclared(move |k| { - assert!(undeclared.lock().unwrap().remove(&k.clone().into_owned())); - assert!(r2.lock().unwrap().remove(&k.into_owned())); - Box::new(Box::pin(async move {})) + let undeclared = undeclared.clone(); + let r2 = r2.clone(); + let k_owned = OwnedKeyExpr::from(k); + Box::new(Box::pin(async move { + assert!(undeclared.lock().unwrap().remove(&k_owned)); + assert!(r2.lock().unwrap().remove(&k_owned)); + })) }); builder @@ -173,14 +181,14 @@ impl DeclarationCollector { pub fn arm( &mut self, - declared: HashSet>, - undeclared: HashSet>, + declared: HashSet, + undeclared: HashSet, ) { *self.to_be_declared.lock().unwrap() = declared; *self.to_be_undeclared.lock().unwrap() = undeclared; } - pub async fn wait(&self, expected: HashSet>) { + pub async fn wait(&self, expected: HashSet) { while !self.to_be_declared.lock().unwrap().is_empty() || !self.to_be_undeclared.lock().unwrap().is_empty() || expected != *self.resources.lock().unwrap() @@ -209,12 +217,10 @@ async fn test_state_transition<'a>( ppc_measurer: &'a PPCMeasurement<'a>, state: &State, ) { - let ke = zenoh::key_expr::KeyExpr::from_str("key") - .unwrap() - .into_owned(); - let mut result: HashSet = HashSet::new(); - let mut undeclared: HashSet = HashSet::new(); - let mut declared: HashSet = HashSet::new(); + let ke = zenoh::key_expr::OwnedKeyExpr::from_str("key").unwrap(); + let mut result: HashSet = HashSet::new(); + let mut undeclared: HashSet = HashSet::new(); + let mut declared: HashSet = HashSet::new(); match (declarations.len(), state.declarators_count) { (0, 0) => {} diff --git a/zplugin-ros1/tests/discovery_test.rs b/zplugin-ros1/tests/discovery_test.rs index a10fae1..f645dc4 100644 --- a/zplugin-ros1/tests/discovery_test.rs +++ b/zplugin-ros1/tests/discovery_test.rs @@ -91,10 +91,7 @@ impl DiscoveryCollector { } } - pub fn use_builder( - &self, - mut builder: discovery::DiscoveryBuilder, - ) -> discovery::DiscoveryBuilder { + pub fn use_builder(&self, builder: discovery::DiscoveryBuilder) -> discovery::DiscoveryBuilder { let p = self.publishers.clone(); let s = self.subscribers.clone(); let srv = self.services.clone(); @@ -125,9 +122,7 @@ impl DiscoveryCollector { }; container.lock().unwrap().remove(&topic); Box::new(Box::pin(async {})) - }); - - builder + }) } pub async fn wait_publishers(&self, expected: HashMultiSet) { diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 2286f22..07b7202 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -13,14 +13,15 @@ // use strum_macros::Display; -use zenoh::prelude::SplitBuffer; -use zenoh_core::{zresult::ZResult, bail, SyncResolve}; +use zenoh::{config::ModeDependentValue, prelude::SplitBuffer}; +use zenoh_core::{bail, zresult::ZResult, SyncResolve}; use std::{ collections::HashSet, + net::SocketAddr, str::FromStr, sync::{ - atomic::{AtomicBool, AtomicU64, Ordering::*}, + atomic::{AtomicBool, AtomicU16, AtomicU64, Ordering::*}, Arc, Mutex, RwLock, }, }; @@ -32,7 +33,7 @@ use zplugin_ros1::ros_to_zenoh_bridge::ros1_to_zenoh_bridge_impl::{ use zplugin_ros1::ros_to_zenoh_bridge::{ros1_client, zenoh_client}; use log::{debug, error}; -use rosrust::{Client, Duration, RawMessage}; +use rosrust::{Client, RawMessage}; use std::sync::atomic::AtomicUsize; use zenoh::prelude::r#async::*; @@ -343,27 +344,37 @@ impl ROS1Client { cycles: Arc, ros1_client: Arc>>, ) { - ros1_client - .data - .probe(Duration::from_seconds(10).into()) - .unwrap(); - while running.load(Relaxed) { - match ros1_client.data.req(&RawMessage(data.clone())) { - Ok(reply) => match reply { - Ok(msg) => { - assert!(data.eq(&msg.0)); - cycles.fetch_add(1, SeqCst); - } - Err(e) => { - error!("ROS1Client: got reply with error: {}", e); - break; + match Self::make_query(&data, &ros1_client) { + Ok(_) => { + cycles.fetch_add(1, SeqCst); + } + Err(e) => { + error!("{}", e); + } + } + } + } + + fn make_query( + data: &Vec, + ros1_client: &Arc>>, + ) -> ZResult<()> { + match ros1_client.data.req(&RawMessage(data.clone())) { + Ok(reply) => match reply { + Ok(msg) => { + if data.eq(&msg.0) { + Ok(()) + } else { + bail!("ROS1Client: data is not equal! \n Sent data: {:?} \nReturned data: {:?}", data, msg.0); } - }, + } Err(e) => { - error!("ROS1Client: failed to send request with error: {}", e); - break; + bail!("ROS1Client: got reply with error: {}", e); } + }, + Err(e) => { + bail!("ROS1Client: failed to send request with error: {}", e); } } } @@ -412,10 +423,9 @@ impl Publisher for ZenohQuery { fn ready(&self) -> bool { let data = (0..10).collect(); async_std::task::block_on( - async move { - Self::make_query(&self.inner, &self.key, &data).await - } - ).is_ok() + async move { Self::make_query(&self.inner, &self.key, &data).await }, + ) + .is_ok() } } impl Publisher for ROS1Client { @@ -426,6 +436,11 @@ impl Publisher for ROS1Client { async_std::task::spawn_blocking(|| Self::query_loop(running, data, cycles, ros1_client)); } + + fn ready(&self) -> bool { + let data = (0..10).collect(); + Self::make_query(&data, &self.ros1_client).is_ok() + } } struct ZenohSubscriber { @@ -715,6 +730,7 @@ impl ROSEnvironment { } } +static TEST_PORT: AtomicU16 = AtomicU16::new(17000); struct TestEnvironment { pub bridge: RunningBridge, pub checker: Arc, @@ -722,14 +738,16 @@ struct TestEnvironment { } impl TestEnvironment { pub fn new() -> TestEnvironment { + let port = TEST_PORT.fetch_add(1, SeqCst); + // start environment for ROS let ros_env = ROSEnvironment::new().with_master(); // start bridge - let bridge = RunningBridge::new(zenoh::config::peer()); + let bridge = RunningBridge::new(Self::config(port)); // start checker's engine - let checker = Arc::new(BridgeChecker::new()); + let checker = Arc::new(BridgeChecker::new(Self::config(port))); // this will wait for the bridge to have some expected initial state and serves two purposes: // - asserts on the expected state @@ -745,7 +763,7 @@ impl TestEnvironment { } pub fn many_count() -> u32 { - Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 10) + Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 40) } pub fn pps_measurements() -> u32 { @@ -778,6 +796,22 @@ impl TestEnvironment { } default } + + fn config(port: u16) -> zenoh::config::Config { + let mut config = zenoh::config::peer(); + config + .scouting + .multicast + .set_address(Some( + SocketAddr::from_str(format!("224.0.0.224:{}", port).as_str()).unwrap(), + )) + .unwrap(); + config + .timestamping + .set_enabled(Some(ModeDependentValue::Unique(true))) + .unwrap(); + config + } } struct RAIICounter @@ -821,8 +855,8 @@ struct BridgeChecker { } impl BridgeChecker { // PUBLIC - pub fn new() -> BridgeChecker { - let session = zenoh::open(config::peer()).res_sync().unwrap().into_arc(); + pub fn new(config: zenoh::config::Config) -> BridgeChecker { + let session = zenoh::open(config).res_sync().unwrap().into_arc(); BridgeChecker { ros_client: ros1_client::Ros1Client::new("test_ros_node"), zenoh_client: zenoh_client::ZenohClient::new(session.clone()), @@ -960,24 +994,6 @@ impl BridgeChecker { } } -#[test] -#[serial(ROS1)] -fn init_with_ros() { - let _ros_env = ROSEnvironment::new().with_master(); - let bridge = RunningBridge::new(zenoh::config::peer()); - let checker = BridgeChecker::new(); - - async_std::task::block_on(bridge.assert_ros_ok()); - async_std::task::block_on(bridge.assert_bridge_empy()); - - let _ros_publisher = checker.make_ros_publisher("/some/ros/topic"); - - async_std::task::block_on(bridge.assert_ros_ok()); - async_std::task::block_on( - bridge.assert_bridge_status(|| *checker.expected_bridge_status.read().unwrap()), - ); -} - #[derive(PartialEq, Eq, Hash, Display)] enum Mode { Ros1ToZenoh, @@ -1287,8 +1303,7 @@ fn ping_pong_all_overlap_many() { let main_work_finished = Arc::new(AtomicBool::new(false)); let main_work = main_work(&env, main_work_finished.clone()); - let parallel_subworks = - parallel_subworks(&env, main_work_finished, TestEnvironment::many_count()); + let parallel_subworks = parallel_subworks(&env, main_work_finished, 10); async_std::task::block_on(futures::future::join(main_work, parallel_subworks)); } @@ -1314,6 +1329,6 @@ async fn check_query(checker: &BridgeChecker) { #[serial(ROS1)] fn check_query_() { let _ros_env = ROSEnvironment::new().with_master(); - let checker = BridgeChecker::new(); + let checker = BridgeChecker::new(zenoh::config::peer()); futures::executor::block_on(check_query(&checker)); } From 205bba76f6025b16cc2f08005cd0309ada72f845 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 18 Apr 2023 13:22:38 +0300 Subject: [PATCH 10/19] Added new settings, brushed-up modules visibility for test and non-test builds --- DEFAULT_CONFIG.json5 | 20 ++++- zplugin-ros1/Cargo.toml | 7 +- zplugin-ros1/src/lib.rs | 2 +- zplugin-ros1/src/ros_to_zenoh_bridge.rs | 48 +++++++++--- .../ros_to_zenoh_bridge/bridges_storage.rs | 14 ++-- .../src/ros_to_zenoh_bridge/environment.rs | 75 +++++++++++++------ .../ros1_to_zenoh_bridge_impl.rs | 19 +++-- .../src/ros_to_zenoh_bridge/topic_bridge.rs | 3 +- .../src/ros_to_zenoh_bridge/zenoh_client.rs | 1 + 9 files changed, 134 insertions(+), 55 deletions(-) diff --git a/DEFAULT_CONFIG.json5 b/DEFAULT_CONFIG.json5 index 7c57ff9..46e9052 100644 --- a/DEFAULT_CONFIG.json5 +++ b/DEFAULT_CONFIG.json5 @@ -20,9 +20,27 @@ // ROS_HOSTNAME: "hostname", //// - //// ROS_NAME: A bridge node's name for ROS1, the default is ros1_to_zenoh_bridge + //// ROS_NAME: A bridge node's name for ROS1, the default is "ros1_to_zenoh_bridge" //// // ROS_NAME: "ros1_to_zenoh_bridge", + + //// + //// ROS_BRIDGING_MODE: When to bridge topics: + //// "Automatic"(default) - bridge topics once they are declared locally + //// "Lazy" - bridge topics once they are declared both locally and required remotely through discovery + //// Warn: this setting is ignored for local ROS1 clients, as they require a tricky discovery mechanism + //// + // ROS_BRIDGING_MODE: "Automatic", + + //// + //// ROS_MASTER_POLLING_INTERVAL: An interval how to poll the ROS1 master for status + //// Bridge polls ROS1 master to get information on local topics, as this is the only way to keep + //// this info updated. This is the interval of this polling. The default is "100ms" + //// + //// Takes a string such as 100ms, 2s, 5m + //// The string format is [0-9]+(ns|us|ms|[smhdwy]) + //// + // ROS_MASTER_POLLING_INTERVAL: "100ms", }, //// diff --git a/zplugin-ros1/Cargo.toml b/zplugin-ros1/Cargo.toml index 64e1ac8..9c063f5 100644 --- a/zplugin-ros1/Cargo.toml +++ b/zplugin-ros1/Cargo.toml @@ -31,6 +31,7 @@ crate-type = ["cdylib", "rlib"] [features] no_mangle = ["zenoh-plugin-trait/no_mangle"] default = ["no_mangle"] +test = [] [dependencies] env_logger = "0.9.1" @@ -43,6 +44,9 @@ serde = "1.0.147" serde_json = "1.0.85" async-global-executor = "2.3.1" rand = "0.8.5" +strum = "0.24" +strum_macros = "0.24" +duration-string = "0.3.0" # zenoh = { version = "0.6.0-beta.1", features = ["unstable"] } zenoh = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } @@ -65,10 +69,9 @@ rosrust = { git = "https://github.com/ZettaScaleLabs/rosrust.git", branch = "fea # rosrust = { path = "../../rosrust/rosrust/rosrust" } [dev-dependencies] -strum = "0.24" -strum_macros = "0.24" serial_test = "0.10.0" multiset = "0.0.5" +zplugin-ros1 = { path = ".", features = ["test"]} [dependencies.async-std] version = "=1.12.0" diff --git a/zplugin-ros1/src/lib.rs b/zplugin-ros1/src/lib.rs index a2beb99..49457e7 100644 --- a/zplugin-ros1/src/lib.rs +++ b/zplugin-ros1/src/lib.rs @@ -50,7 +50,7 @@ impl Plugin for Ros1Plugin { let plugin_configuration_entries = Environment::env(); for entry in plugin_configuration_entries.iter() { if let Some(v) = self_cfg.get(entry.name) { - entry.set(v); + entry.set(v.to_string()); } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge.rs index 2972aa7..d38e294 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge.rs @@ -24,22 +24,47 @@ use std::sync::{ use self::ros1_to_zenoh_bridge_impl::work_cycle; -pub mod abstract_bridge; +#[cfg(feature = "test")] +pub mod aloha_declaration; +#[cfg(feature = "test")] +pub mod aloha_subscription; +#[cfg(feature = "test")] pub mod bridge_type; +#[cfg(feature = "test")] pub mod discovery; +#[cfg(feature = "test")] pub mod ros1_client; -pub mod topic_bridge; +#[cfg(feature = "test")] +pub mod ros1_to_zenoh_bridge_impl; +#[cfg(feature = "test")] +pub mod topic_utilities; +#[cfg(feature = "test")] pub mod zenoh_client; +#[cfg(not(feature = "test"))] +mod aloha_declaration; +#[cfg(not(feature = "test"))] +mod aloha_subscription; +#[cfg(not(feature = "test"))] +mod bridge_type; +#[cfg(not(feature = "test"))] +mod discovery; +#[cfg(not(feature = "test"))] +mod ros1_client; +#[cfg(not(feature = "test"))] +mod ros1_to_zenoh_bridge_impl; +#[cfg(not(feature = "test"))] +mod topic_utilities; +#[cfg(not(feature = "test"))] +mod zenoh_client; + +mod abstract_bridge; mod bridges_storage; +mod topic_bridge; mod topic_mapping; -pub mod aloha_declaration; -pub mod aloha_subscription; pub mod environment; pub mod ros1_master_ctrl; -pub mod ros1_to_zenoh_bridge_impl; -pub mod topic_utilities; pub struct Ros1ToZenohBridge { flag: Arc, @@ -59,18 +84,19 @@ impl Ros1ToZenohBridge { } } - pub async fn async_await(&mut self) { - self.task_handle.as_mut().await; - } - pub async fn stop(&mut self) { self.flag.store(false, Relaxed); self.async_await().await; } - pub async fn run(session: Arc, flag: Arc) { + //PRIVATE: + async fn run(session: Arc, flag: Arc) { work_cycle(session, flag, |_v| {}, |_status| {}).await; } + + async fn async_await(&mut self) { + self.task_handle.as_mut().await; + } } impl Drop for Ros1ToZenohBridge { fn drop(&mut self) { diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs index 82cc523..dbd176b 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/bridges_storage.rs @@ -18,13 +18,9 @@ use std::{ }; use super::{ - bridge_type::BridgeType, - discovery::LocalResources, - ros1_client, - ros1_to_zenoh_bridge_impl::BridgeStatus, - topic_bridge::{self, TopicBridge}, - topic_mapping::Ros1TopicMapping, - zenoh_client, + bridge_type::BridgeType, discovery::LocalResources, environment::Environment, ros1_client, + ros1_to_zenoh_bridge_impl::BridgeStatus, topic_bridge::TopicBridge, + topic_mapping::Ros1TopicMapping, zenoh_client, }; struct Bridges { @@ -156,7 +152,7 @@ impl<'a> ComplementaryElementAccessor<'a> { self.access.declaration_interface.clone(), self.access.ros1_client.clone(), self.access.zenoh_client.clone(), - topic_bridge::BridgingMode::Automatic, + Environment::bridging_mode().get(), )) .set_has_complementary_in_zenoh(true) .await; @@ -223,7 +219,7 @@ impl<'a> ElementAccessor<'a> { self.access.declaration_interface.clone(), self.access.ros1_client.clone(), self.access.zenoh_client.clone(), - topic_bridge::BridgingMode::Automatic, + Environment::bridging_mode().get(), )); inserted.set_present_in_ros1(true).await; smth_changed = true; diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs index 78665a2..39aec41 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/environment.rs @@ -12,69 +12,98 @@ // ZettaScale Zenoh Team, // +use duration_string::DurationString; use rosrust::api::resolve::*; -use std::str::FromStr; +use std::convert::From; +use std::time::Duration; +use std::{marker::PhantomData, str::FromStr}; + +use super::topic_bridge::BridgingMode; #[derive(Clone)] -pub struct Entry<'a> { +pub struct Entry<'a, Tvar> +where + Tvar: ToString + FromStr + Clone, +{ pub name: &'a str, - pub default: String, + pub default: Tvar, + marker: std::marker::PhantomData, } -impl<'a> Entry<'a> { - fn new(name: &'a str, default: Tvar) -> Entry<'a> - where - Tvar: ToString, - { +impl<'a, Tvar> Entry<'a, Tvar> +where + Tvar: ToString + FromStr + Clone, +{ + fn new(name: &'a str, default: Tvar) -> Entry<'a, Tvar> { Entry { name, - default: default.to_string(), + default, + marker: PhantomData, } } - pub fn get(&self) -> Tvar - where - Tvar: FromStr + std::convert::From, - { + pub fn get(&self) -> Tvar { if let Ok(val) = std::env::var(self.name) { if let Ok(val) = val.parse::() { return val; } } - self.default.clone().into() + self.default.clone() } - pub fn set(&self, value: Tvar) - where - Tvar: ToString, - { + pub fn set(&self, value: Tvar) { std::env::set_var(self.name, value.to_string()); } } +impl<'a> From> for Entry<'a, String> { + fn from(item: Entry<'a, BridgingMode>) -> Entry<'a, String> { + Entry::new(item.name, item.default.to_string()) + } +} + +impl<'a> From> for Entry<'a, String> { + fn from(item: Entry<'a, DurationString>) -> Entry<'a, String> { + Entry::new(item.name, item.default.to_string()) + } +} + pub struct Environment; impl Environment { - pub fn ros_master_uri() -> Entry<'static> { + pub fn ros_master_uri() -> Entry<'static, String> { return Entry::new("ROS_MASTER_URI", master()); } - pub fn ros_hostname() -> Entry<'static> { + pub fn ros_hostname() -> Entry<'static, String> { return Entry::new("ROS_HOSTNAME", hostname()); } - pub fn ros_name() -> Entry<'static> { + pub fn ros_name() -> Entry<'static, String> { return Entry::new("ROS_NAME", name("ros1_to_zenoh_bridge")); } - pub fn ros_namespace() -> Entry<'static> { + pub fn ros_namespace() -> Entry<'static, String> { return Entry::new("ROS_NAMESPACE", namespace()); } - pub fn env() -> Vec> { + pub fn bridging_mode() -> Entry<'static, BridgingMode> { + return Entry::new("ROS_BRIDGING_MODE", BridgingMode::Automatic); + } + + pub fn master_polling_interval() -> Entry<'static, DurationString> { + return Entry::new( + "ROS_MASTER_POLLING_INTERVAL", + DurationString::from(Duration::from_millis(100)), + ); + } + + pub fn env() -> Vec> { return [ Self::ros_master_uri(), Self::ros_hostname(), Self::ros_name(), Self::ros_namespace(), + Self::bridging_mode().into(), + Self::master_polling_interval().into(), ] .to_vec(); } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs index ebf91a8..1e5b288 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs @@ -16,9 +16,12 @@ use async_std::sync::Mutex; use zenoh; -use std::sync::{ - atomic::{AtomicBool, Ordering::Relaxed}, - Arc, +use std::{ + sync::{ + atomic::{AtomicBool, Ordering::Relaxed}, + Arc, + }, + time::Duration, }; use log::{debug, error}; @@ -59,7 +62,7 @@ pub async fn work_cycle( BridgeStatisticsCallback: Fn(BridgeStatus), { let ros1_client = Arc::new(ros1_client::Ros1Client::new( - Environment::ros_name().get::().as_str(), + Environment::ros_name().get().as_str(), )); let zenoh_client = Arc::new(zenoh_client::ZenohClient::new(session.clone())); @@ -153,6 +156,8 @@ where bridges: Arc>, flag: Arc, ) { + let poll_interval: Duration = Environment::master_polling_interval().get().into(); + while flag.load(Relaxed) { let cl = ros1_client.clone(); let ros1_state = async_std::task::spawn_blocking(move || { @@ -175,9 +180,9 @@ where async_std::task::sleep({ if smth_changed { - core::time::Duration::from_millis(100) + poll_interval / 2 } else { - core::time::Duration::from_millis(200) + poll_interval } }) .await; @@ -191,7 +196,7 @@ where Self::cleanup(&mut locked); self.report_bridge_statistics(&locked); } - async_std::task::sleep(core::time::Duration::from_millis(500)).await; + async_std::task::sleep(poll_interval).await; } } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs index 3b4739c..d1ca577 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_bridge.rs @@ -20,8 +20,9 @@ use super::{ }; use log::error; use std::sync::Arc; +use strum_macros::{Display, EnumString}; -#[derive(PartialEq, Eq)] +#[derive(PartialEq, Eq, EnumString, Clone, Display)] pub enum BridgingMode { Lazy, Automatic, diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs index 1ef1d33..36dc536 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/zenoh_client.rs @@ -44,6 +44,7 @@ impl ZenohClient { .declare_subscriber(key_expr) .callback(callback) .allowed_origin(Locality::Remote) + .reliability(Reliability::Reliable) .res_async() .await } From 342e1b9f87b0f04c6b0aa29e24815c48d6d79081 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 18 Apr 2023 13:26:59 +0300 Subject: [PATCH 11/19] Decrease default test load --- zplugin-ros1/tests/ping_pong_test.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 07b7202..0eb28e1 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -763,7 +763,7 @@ impl TestEnvironment { } pub fn many_count() -> u32 { - Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 40) + Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 10) } pub fn pps_measurements() -> u32 { From 144498e73eb7d79930b04db40ef9ac7d0db6a478 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 18 Apr 2023 14:39:38 +0300 Subject: [PATCH 12/19] Brush-up build system scripts, update LICENSE --- Cargo.lock | 4026 +++++++++++++++++++++++++++++++++++++++ Cargo.toml | 37 +- LICENSE | 186 +- zplugin-ros1/Cargo.toml | 79 +- 4 files changed, 4277 insertions(+), 51 deletions(-) create mode 100644 Cargo.lock diff --git a/Cargo.lock b/Cargo.lock new file mode 100644 index 0000000..9230a86 --- /dev/null +++ b/Cargo.lock @@ -0,0 +1,4026 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "addr2line" +version = "0.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a76fd60b23679b7d19bd066031410fb7e458ccc5e958eb5c325888ce4baedc97" +dependencies = [ + "gimli", +] + +[[package]] +name = "adler" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f26201604c87b1e01bd3d98f8d5d9a8fcbb815e8cedb41ffccbeb4bf593a35fe" + +[[package]] +name = "adler32" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aae1277d39aeec15cb388266ecc24b11c80469deae6067e17a1a7aa9e5c1f234" + +[[package]] +name = "aes" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "433cfd6710c9986c576a25ca913c39d66a6474107b406f34f91d4a8923395241" +dependencies = [ + "cfg-if", + "cipher", + "cpufeatures", +] + +[[package]] +name = "ahash" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2c99f64d1e06488f620f932677e24bc6e2897582980441ae90a671415bd7ec2f" +dependencies = [ + "cfg-if", + "once_cell", + "version_check 0.9.4", +] + +[[package]] +name = "aho-corasick" +version = "0.6.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "81ce3d38065e618af2d7b77e10c5ad9a069859b4be3c2250f674af3840d9c8a5" +dependencies = [ + "memchr", +] + +[[package]] +name = "aho-corasick" +version = "0.7.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cc936419f96fa211c1b9166887b38e5e40b19958e5b895be7c1f93adec7071ac" +dependencies = [ + "memchr", +] + +[[package]] +name = "alloc-no-stdlib" +version = "2.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cc7bb162ec39d46ab1ca8c77bf72e890535becd1751bb45f64c597edb4c8c6b3" + +[[package]] +name = "alloc-stdlib" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94fb8275041c72129eb51b7d0322c29b8387a0386127718b096429201a5d6ece" +dependencies = [ + "alloc-no-stdlib", +] + +[[package]] +name = "android_system_properties" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311" +dependencies = [ + "libc", +] + +[[package]] +name = "anyhow" +version = "1.0.70" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7de8ce5e0f9f8d88245311066a578d72b7af3e7088f32783804676302df237e4" + +[[package]] +name = "array-init" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d62b7694a562cdf5a74227903507c56ab2cc8bdd1f781ed5cb4cf9c9f810bfc" + +[[package]] +name = "ascii" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d92bec98840b8f03a5ff5413de5293bfcd8bf96467cf5452609f939ec6f5de16" + +[[package]] +name = "async-attributes" +version = "1.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a3203e79f4dd9bdda415ed03cf14dae5a2bf775c683a00f94e9cd1faf0f596e5" +dependencies = [ + "quote", + "syn 1.0.109", +] + +[[package]] +name = "async-channel" +version = "1.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf46fee83e5ccffc220104713af3292ff9bc7c64c7de289f66dae8e38d826833" +dependencies = [ + "concurrent-queue", + "event-listener", + "futures-core", +] + +[[package]] +name = "async-executor" +version = "1.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fa3dc5f2a8564f07759c008b9109dc0d39de92a88d5588b8a5036d286383afb" +dependencies = [ + "async-lock", + "async-task", + "concurrent-queue", + "fastrand", + "futures-lite", + "slab", +] + +[[package]] +name = "async-global-executor" +version = "2.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1b6f5d7df27bd294849f8eec66ecfc63d11814df7a4f5d74168a2394467b776" +dependencies = [ + "async-channel", + "async-executor", + "async-io", + "async-lock", + "blocking", + "futures-lite", + "once_cell", + "tokio", +] + +[[package]] +name = "async-io" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fc5b45d93ef0529756f812ca52e44c221b35341892d3dcc34132ac02f3dd2af" +dependencies = [ + "async-lock", + "autocfg", + "cfg-if", + "concurrent-queue", + "futures-lite", + "log 0.4.17", + "parking", + "polling", + "rustix", + "slab", + "socket2", + "waker-fn", +] + +[[package]] +name = "async-lock" +version = "2.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fa24f727524730b077666307f2734b4a1a1c57acb79193127dcc8914d5242dd7" +dependencies = [ + "event-listener", +] + +[[package]] +name = "async-process" +version = "1.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7a9d28b1d97e08915212e2e45310d47854eafa69600756fc735fb788f75199c9" +dependencies = [ + "async-io", + "async-lock", + "autocfg", + "blocking", + "cfg-if", + "event-listener", + "futures-lite", + "rustix", + "signal-hook", + "windows-sys 0.48.0", +] + +[[package]] +name = "async-rustls" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93b21a03b7c21702a0110f9f8d228763a533570deb376119042dabf33c37a01a" +dependencies = [ + "futures-io", + "rustls", + "webpki", +] + +[[package]] +name = "async-std" +version = "1.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "62565bb4402e926b29953c785397c6dc0391b7b446e45008b0049eb43cec6f5d" +dependencies = [ + "async-attributes", + "async-channel", + "async-global-executor", + "async-io", + "async-lock", + "async-process", + "crossbeam-utils", + "futures-channel", + "futures-core", + "futures-io", + "futures-lite", + "gloo-timers", + "kv-log-macro", + "log 0.4.17", + "memchr", + "once_cell", + "pin-project-lite", + "pin-utils", + "slab", + "wasm-bindgen-futures", +] + +[[package]] +name = "async-task" +version = "4.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ecc7ab41815b3c653ccd2978ec3255c81349336702dfdf62ee6f7069b12a3aae" + +[[package]] +name = "async-trait" +version = "0.1.68" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9ccdd8f2a161be9bd5c023df56f1b2a0bd1d83872ae53b71a84a12c9bf6e842" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.15", +] + +[[package]] +name = "atomic-waker" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1181e1e0d1fce796a03db1ae795d67167da795f9cf4a39c37589e85ef57f26d3" + +[[package]] +name = "atty" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8" +dependencies = [ + "hermit-abi 0.1.19", + "libc", + "winapi", +] + +[[package]] +name = "autocfg" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" + +[[package]] +name = "backtrace" +version = "0.3.67" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "233d376d6d185f2a3093e58f283f60f880315b6c60075b01f36b3b85154564ca" +dependencies = [ + "addr2line", + "cc", + "cfg-if", + "libc", + "miniz_oxide", + "object", + "rustc-demangle", +] + +[[package]] +name = "base64" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96434f987501f0ed4eb336a411e0631ecd1afa11574fe148587adc4ff96143c9" +dependencies = [ + "byteorder", + "safemem 0.2.0", +] + +[[package]] +name = "base64" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "489d6c0ed21b11d038c31b6ceccca973e65d73ba3bd8ecb9a2babf5546164643" +dependencies = [ + "byteorder", + "safemem 0.3.3", +] + +[[package]] +name = "base64" +version = "0.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e1b586273c5702936fe7b7d6896644d8be71e6314cfe09d3167c95f712589e8" + +[[package]] +name = "base64" +version = "0.21.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4a4ddaa51a5bc52a6948f74c06d20aaaddb71924eab79b8c97a8c556e942d6a" + +[[package]] +name = "base64ct" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8c3c1a368f70d6cf7302d78f8f7093da241fb8e8807c05cc9e51a125895a6d5b" + +[[package]] +name = "bincode" +version = "1.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1f45e9417d87227c7a56d22e471c6206462cba514c7590c09aff4cf6d1ddcad" +dependencies = [ + "serde", +] + +[[package]] +name = "bitflags" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aad18937a628ec6abcd26d1489012cc0e18c21798210f491af69ded9b881106d" + +[[package]] +name = "bitflags" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4efd02e230a02e18f92fc2735f44597385ed02ad8f831e7c1c1156ee5e1ab3a5" + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "block-buffer" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4152116fd6e9dadb291ae18fc1ec3575ed6d84c29642d97890f4b4a3417297e4" +dependencies = [ + "generic-array", +] + +[[package]] +name = "block-buffer" +version = "0.10.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71" +dependencies = [ + "generic-array", +] + +[[package]] +name = "blocking" +version = "1.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77231a1c8f801696fc0123ec6150ce92cffb8e164a02afb9c8ddee0e9b65ad65" +dependencies = [ + "async-channel", + "async-lock", + "async-task", + "atomic-waker", + "fastrand", + "futures-lite", + "log 0.4.17", +] + +[[package]] +name = "brotli" +version = "3.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1a0b1dbcc8ae29329621f8d4f0d835787c1c38bb1401979b49d13b0b305ff68" +dependencies = [ + "alloc-no-stdlib", + "alloc-stdlib", + "brotli-decompressor", +] + +[[package]] +name = "brotli-decompressor" +version = "2.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4b6561fd3f895a11e8f72af2cb7d22e08366bebc2b6b57f7744c4bda27034744" +dependencies = [ + "alloc-no-stdlib", + "alloc-stdlib", +] + +[[package]] +name = "buf_redux" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b953a6887648bb07a535631f2bc00fbdb2a2216f135552cb3f534ed136b9c07f" +dependencies = [ + "memchr", + "safemem 0.3.3", +] + +[[package]] +name = "bumpalo" +version = "3.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d261e256854913907f67ed06efbc3338dfe6179796deefc1ff763fc1aee5535" + +[[package]] +name = "byteorder" +version = "1.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610" + +[[package]] +name = "bytes" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "89b2fd2a0dcf38d7971e2194b6b6eebab45ae01067456a7fd93d5547a61b70be" + +[[package]] +name = "cache-padded" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1db59621ec70f09c5e9b597b220c7a2b43611f4710dc03ceb8748637775692c" + +[[package]] +name = "cc" +version = "1.0.79" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "50d30906286121d95be3d479533b458f87493b30a4b5f79a607db8f5d11aa91f" + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "chrono" +version = "0.4.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e3c5919066adf22df73762e50cffcde3a758f2a848b113b586d1f86728b673b" +dependencies = [ + "iana-time-zone", + "num-integer", + "num-traits", + "winapi", +] + +[[package]] +name = "chunked_transfer" +version = "1.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cca491388666e04d7248af3f60f0c40cfb0991c72205595d7c396e3510207d1a" + +[[package]] +name = "cipher" +version = "0.4.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "773f3b9af64447d2ce9850330c473515014aa235e6a783b02db81ff39e4a3dad" +dependencies = [ + "crypto-common", + "inout", +] + +[[package]] +name = "clap" +version = "3.2.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "71655c45cb9845d3270c9d6df84ebe72b4dad3c2ba3f7023ad47c144e4e473a5" +dependencies = [ + "atty", + "bitflags 1.3.2", + "clap_lex", + "indexmap", + "strsim", + "termcolor", + "textwrap", +] + +[[package]] +name = "clap_lex" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2850f2f5a82cbf437dd5af4d49848fbdfc27c157c3d010345776f952765261c5" +dependencies = [ + "os_str_bytes", +] + +[[package]] +name = "codespan-reporting" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3538270d33cc669650c4b093848450d380def10c331d38c768e34cac80576e6e" +dependencies = [ + "termcolor", + "unicode-width", +] + +[[package]] +name = "colored" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b3616f750b84d8f0de8a58bda93e08e2a81ad3f523089b05f1dffecab48c6cbd" +dependencies = [ + "atty", + "lazy_static", + "winapi", +] + +[[package]] +name = "concurrent-queue" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "62ec6771ecfa0762d24683ee5a32ad78487a3d3afdc0fb8cae19d2c5deb50b7c" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "const-oid" +version = "0.9.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "520fbf3c07483f94e3e3ca9d0cfd913d7718ef2483d2cfd91c0d9e91474ab913" + +[[package]] +name = "core-foundation" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "194a7a9e6de53fa55116934067c844d9d749312f75c6f6d0980e8c252f8c2146" +dependencies = [ + "core-foundation-sys", + "libc", +] + +[[package]] +name = "core-foundation-sys" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e496a50fda8aacccc86d7529e2c1e0892dbd0f898a6b5645b5561b89c3210efa" + +[[package]] +name = "cpufeatures" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "280a9f2d8b3a38871a3c8a46fb80db65e5e5ed97da80c4d08bf27fb63e35e181" +dependencies = [ + "libc", +] + +[[package]] +name = "crc32fast" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b540bd8bc810d3885c6ea91e2018302f68baba2129ab3e88f32389ee9370880d" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "crossbeam" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2801af0d36612ae591caa9568261fddce32ce6e08a7275ea334a06a4ad021a2c" +dependencies = [ + "cfg-if", + "crossbeam-channel", + "crossbeam-deque", + "crossbeam-epoch", + "crossbeam-queue", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-channel" +version = "0.5.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a33c2bf77f2df06183c3aa30d1e96c0695a313d4f9c453cc3762a6db39f99200" +dependencies = [ + "cfg-if", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-deque" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ce6fd6f855243022dcecf8702fef0c297d4338e226845fe067f6341ad9fa0cef" +dependencies = [ + "cfg-if", + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "46bd5f3f85273295a9d14aedfb86f6aadbff6d8f5295c4a9edb08e819dcf5695" +dependencies = [ + "autocfg", + "cfg-if", + "crossbeam-utils", + "memoffset 0.8.0", + "scopeguard", +] + +[[package]] +name = "crossbeam-queue" +version = "0.3.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d1cfb3ea8a53f37c40dea2c7bedcbd88bdfae54f5e2175d6ecaff1c988353add" +dependencies = [ + "cfg-if", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3c063cd8cc95f5c377ed0d4b49a4b21f632396ff690e8470c29b3359b346984b" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "crypto-common" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1bfb12502f3fc46cca1bb51ac28df9d618d813cdc3d2f25b9fe775a34af26bb3" +dependencies = [ + "generic-array", + "typenum", +] + +[[package]] +name = "ctor" +version = "0.1.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6d2301688392eb071b0bf1a37be05c469d3cc4dbbd95df672fe28ab021e6a096" +dependencies = [ + "quote", + "syn 1.0.109", +] + +[[package]] +name = "ctrlc" +version = "3.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbcf33c2a618cbe41ee43ae6e9f2e48368cd9f9db2896f10167d8d762679f639" +dependencies = [ + "nix", + "windows-sys 0.45.0", +] + +[[package]] +name = "cxx" +version = "1.0.94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f61f1b6389c3fe1c316bf8a4dccc90a38208354b330925bce1f74a6c4756eb93" +dependencies = [ + "cc", + "cxxbridge-flags", + "cxxbridge-macro", + "link-cplusplus", +] + +[[package]] +name = "cxx-build" +version = "1.0.94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "12cee708e8962df2aeb38f594aae5d827c022b6460ac71a7a3e2c3c2aae5a07b" +dependencies = [ + "cc", + "codespan-reporting", + "once_cell", + "proc-macro2", + "quote", + "scratch", + "syn 2.0.15", +] + +[[package]] +name = "cxxbridge-flags" +version = "1.0.94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7944172ae7e4068c533afbb984114a56c46e9ccddda550499caa222902c7f7bb" + +[[package]] +name = "cxxbridge-macro" +version = "1.0.94" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2345488264226bf682893e25de0769f3360aac9957980ec49361b083ddaa5bc5" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.15", +] + +[[package]] +name = "dashmap" +version = "5.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "907076dfda823b0b36d2a1bb5f90c96660a5bbcd7729e10727f07858f22c4edc" +dependencies = [ + "cfg-if", + "hashbrown 0.12.3", + "lock_api", + "once_cell", + "parking_lot_core", +] + +[[package]] +name = "deflate" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c86f7e25f518f4b81808a2cf1c50996a61f5c2eb394b2393bd87f2a4780a432f" +dependencies = [ + "adler32", + "gzip-header", +] + +[[package]] +name = "der" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1a467a65c5e759bce6e65eaf91cc29f466cdc57cb65777bd646872a8a1fd4de" +dependencies = [ + "const-oid", + "pem-rfc7468", + "zeroize", +] + +[[package]] +name = "digest" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d3dd60d1080a57a05ab032377049e0591415d2b31afd7028356dbf3cc6dcb066" +dependencies = [ + "generic-array", +] + +[[package]] +name = "digest" +version = "0.10.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8168378f4e5023e7218c89c891c0fd8ecdb5e5e4f18cb78f38cf245dd021e76f" +dependencies = [ + "block-buffer 0.10.4", + "const-oid", + "crypto-common", + "subtle", +] + +[[package]] +name = "dirs" +version = "5.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dece029acd3353e3a58ac2e3eb3c8d6c35827a892edc6cc4138ef9c33df46ecd" +dependencies = [ + "dirs-sys", +] + +[[package]] +name = "dirs-sys" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04414300db88f70d74c5ff54e50f9e1d1737d9a5b90f53fcf2e95ca2a9ab554b" +dependencies = [ + "libc", + "redox_users", + "windows-sys 0.45.0", +] + +[[package]] +name = "duration-string" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6fcc1d9ae294a15ed05aeae8e11ee5f2b3fe971c077d45a42fb20825fba6ee13" + +[[package]] +name = "either" +version = "1.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91" + +[[package]] +name = "env_logger" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a12e6657c4c97ebab115a42dcee77225f7f482cdd841cf7088c657a42e9e00e7" +dependencies = [ + "atty", + "humantime", + "log 0.4.17", + "regex 1.7.3", + "termcolor", +] + +[[package]] +name = "env_logger" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85cdab6a89accf66733ad5a1693a4dcced6aeff64602b634530dd73c1f3ee9f0" +dependencies = [ + "humantime", + "is-terminal", + "log 0.4.17", + "regex 1.7.3", + "termcolor", +] + +[[package]] +name = "errno" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4bcfec3a70f97c962c307b2d2c56e358cf1d00b558d74262b5f929ee8cc7e73a" +dependencies = [ + "errno-dragonfly", + "libc", + "windows-sys 0.48.0", +] + +[[package]] +name = "errno-dragonfly" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa68f1b12764fab894d2755d2518754e71b4fd80ecfb822714a1206c2aab39bf" +dependencies = [ + "cc", + "libc", +] + +[[package]] +name = "error-chain" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9435d864e017c3c6afeac1654189b06cdb491cf2ff73dbf0d73b0f292f42ff8" +dependencies = [ + "backtrace", +] + +[[package]] +name = "error-chain" +version = "0.12.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2d2f06b9cac1506ece98fe3231e3cc9c4410ec3d5b1f24ae1c8946f0742cdefc" +dependencies = [ + "backtrace", + "version_check 0.9.4", +] + +[[package]] +name = "event-listener" +version = "2.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0206175f82b8d6bf6652ff7d71a1e27fd2e4efde587fd368662814d6ec1d9ce0" + +[[package]] +name = "fastrand" +version = "1.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e51093e27b0797c359783294ca4f0a911c270184cb10f85783b118614a1501be" +dependencies = [ + "instant", +] + +[[package]] +name = "filetime" +version = "0.2.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5cbc844cecaee9d4443931972e1289c8ff485cb4cc2767cb03ca139ed6885153" +dependencies = [ + "cfg-if", + "libc", + "redox_syscall 0.2.16", + "windows-sys 0.48.0", +] + +[[package]] +name = "fixedbitset" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ce7134b9999ecaf8bcd65542e436736ef32ddca1b3e06094cb6ec5755203b80" + +[[package]] +name = "flume" +version = "0.10.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1657b4441c3403d9f7b3409e47575237dac27b1b5726df654a6ecbf92f0f7577" +dependencies = [ + "futures-core", + "futures-sink", + "nanorand", + "pin-project", + "spin 0.9.8", +] + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "form_urlencoded" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a9c384f161156f5260c24a097c56119f9be8c798586aecc13afbcbe7b7e26bf8" +dependencies = [ + "percent-encoding 2.2.0", +] + +[[package]] +name = "futures" +version = "0.1.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3a471a38ef8ed83cd6e40aa59c1ffe17db6855c18e3604d9c4ed8c08ebc28678" + +[[package]] +name = "futures" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "23342abe12aba583913b2e62f22225ff9c950774065e4bfb61a19cd9770fec40" +dependencies = [ + "futures-channel", + "futures-core", + "futures-executor", + "futures-io", + "futures-sink", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-channel" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "955518d47e09b25bbebc7a18df10b81f0c766eaf4c4f1cccef2fca5f2a4fb5f2" +dependencies = [ + "futures-core", + "futures-sink", +] + +[[package]] +name = "futures-core" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4bca583b7e26f571124fe5b7561d49cb2868d79116cfa0eefce955557c6fee8c" + +[[package]] +name = "futures-executor" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ccecee823288125bd88b4d7f565c9e58e41858e47ab72e8ea2d64e93624386e0" +dependencies = [ + "futures-core", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-io" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4fff74096e71ed47f8e023204cfd0aa1289cd54ae5430a9523be060cdb849964" + +[[package]] +name = "futures-lite" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49a9d51ce47660b1e808d3c990b4709f2f415d928835a17dfd16991515c46bce" +dependencies = [ + "fastrand", + "futures-core", + "futures-io", + "memchr", + "parking", + "pin-project-lite", + "waker-fn", +] + +[[package]] +name = "futures-macro" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "89ca545a94061b6365f2c7355b4b32bd20df3ff95f02da9329b34ccc3bd6ee72" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.15", +] + +[[package]] +name = "futures-sink" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f43be4fe21a13b9781a69afa4985b0f6ee0e1afab2c6f454a8cf30e2b2237b6e" + +[[package]] +name = "futures-task" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "76d3d132be6c0e6aa1534069c705a74a5997a356c0dc2f86a47765e5617c5b65" + +[[package]] +name = "futures-util" +version = "0.3.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "26b01e40b772d54cf6c6d721c1d1abd0647a0106a12ecaa1c186273392a69533" +dependencies = [ + "futures-channel", + "futures-core", + "futures-io", + "futures-macro", + "futures-sink", + "futures-task", + "memchr", + "pin-project-lite", + "pin-utils", + "slab", +] + +[[package]] +name = "generic-array" +version = "0.14.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a" +dependencies = [ + "typenum", + "version_check 0.9.4", +] + +[[package]] +name = "getrandom" +version = "0.2.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c85e1d9ab2eadba7e5040d4e09cbd6d072b76a557ad64e797c2cb9d4da21d7e4" +dependencies = [ + "cfg-if", + "js-sys", + "libc", + "wasi 0.11.0+wasi-snapshot-preview1", + "wasm-bindgen", +] + +[[package]] +name = "gimli" +version = "0.27.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ad0a93d233ebf96623465aad4046a8d3aa4da22d4f4beba5388838c8a434bbb4" + +[[package]] +name = "git-version" +version = "0.3.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f6b0decc02f4636b9ccad390dcbe77b722a77efedfa393caf8379a51d5c61899" +dependencies = [ + "git-version-macro", + "proc-macro-hack", +] + +[[package]] +name = "git-version-macro" +version = "0.3.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fe69f1cbdb6e28af2bac214e943b99ce8a0a06b447d15d3e61161b0423139f3f" +dependencies = [ + "proc-macro-hack", + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "glob" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d2fabcfbdc87f4758337ca535fb41a6d701b65693ce38287d856d1674551ec9b" + +[[package]] +name = "gloo-timers" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b995a66bb87bebce9a0f4a95aed01daca4872c050bfcb21653361c03bc35e5c" +dependencies = [ + "futures-channel", + "futures-core", + "js-sys", + "wasm-bindgen", +] + +[[package]] +name = "gzip-header" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95cc527b92e6029a62960ad99aa8a6660faa4555fe5f731aab13aa6a921795a2" +dependencies = [ + "crc32fast", +] + +[[package]] +name = "hashbrown" +version = "0.12.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" + +[[package]] +name = "hashbrown" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43a3c133739dddd0d2990f9a4bdf8eb4b21ef50e4851ca85ab661199821d510e" +dependencies = [ + "ahash", +] + +[[package]] +name = "heck" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95505c38b4572b2d910cecb0281560f54b440a19336cbbcb27bf6ce6adc6f5a8" + +[[package]] +name = "hermit-abi" +version = "0.1.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "62b467343b94ba476dcb2500d242dadbb39557df889310ac77c5d99100aaac33" +dependencies = [ + "libc", +] + +[[package]] +name = "hermit-abi" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee512640fe35acbfb4bb779db6f0d80704c2cacfa2e39b601ef3e3f47d1ae4c7" +dependencies = [ + "libc", +] + +[[package]] +name = "hermit-abi" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fed44880c466736ef9a5c5b5facefb5ed0785676d0c02d612db14e54f0d84286" + +[[package]] +name = "hex" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f24254aa9a54b5c858eaee2f5bccdb46aaf0e486a595ed5fd8f86ba55232a70" + +[[package]] +name = "hmac" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6c49c37c09c17a53d937dfbb742eb3a961d65a994e6bcdcf37e7399d0cc8ab5e" +dependencies = [ + "digest 0.10.6", +] + +[[package]] +name = "home" +version = "0.5.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "747309b4b440c06d57b0b25f2aee03ee9b5e5397d288c60e21fc709bb98a7408" +dependencies = [ + "winapi", +] + +[[package]] +name = "hostname" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3c731c3e10504cc8ed35cfe2f1db4c9274c3d35fa486e3b31df46f068ef3e867" +dependencies = [ + "libc", + "match_cfg", + "winapi", +] + +[[package]] +name = "http" +version = "0.2.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd6effc99afb63425aff9b05836f029929e345a6148a14b7ecd5ab67af944482" +dependencies = [ + "bytes", + "fnv", + "itoa", +] + +[[package]] +name = "httparse" +version = "1.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d897f394bad6a705d5f4104762e116a75639e470d80901eed05a860a95cb1904" + +[[package]] +name = "httpdate" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4a1e36c821dbe04574f602848a19f742f4fb3c98d40449f11bcad18d6b17421" + +[[package]] +name = "humantime" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" + +[[package]] +name = "hyper" +version = "0.10.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0a0652d9a2609a968c14be1a9ea00bf4b1d64e2e1f53a1b51b6fff3a6e829273" +dependencies = [ + "base64 0.9.3", + "httparse", + "language-tags", + "log 0.3.9", + "mime 0.2.6", + "num_cpus", + "time 0.1.45", + "traitobject", + "typeable", + "unicase 1.4.2", + "url 1.7.2", +] + +[[package]] +name = "iana-time-zone" +version = "0.1.56" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0722cd7114b7de04316e7ea5456a0bbb20e4adb46fd27a3697adb812cff0f37c" +dependencies = [ + "android_system_properties", + "core-foundation-sys", + "iana-time-zone-haiku", + "js-sys", + "wasm-bindgen", + "windows", +] + +[[package]] +name = "iana-time-zone-haiku" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0703ae284fc167426161c2e3f1da3ea71d94b21bedbcc9494e92b28e334e3dca" +dependencies = [ + "cxx", + "cxx-build", +] + +[[package]] +name = "idna" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38f09e0f0b1fb55fdee1f17470ad800da77af5186a1a76c026b679358b7e844e" +dependencies = [ + "matches", + "unicode-bidi", + "unicode-normalization", +] + +[[package]] +name = "idna" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e14ddfc70884202db2244c223200c204c2bda1bc6e0998d11b5e024d657209e6" +dependencies = [ + "unicode-bidi", + "unicode-normalization", +] + +[[package]] +name = "indexmap" +version = "1.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" +dependencies = [ + "autocfg", + "hashbrown 0.12.3", +] + +[[package]] +name = "inout" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a0c10553d664a4d0bcff9f4215d0aac67a639cc68ef660840afe309b807bc9f5" +dependencies = [ + "generic-array", +] + +[[package]] +name = "instant" +version = "0.1.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7a5bbe824c507c5da5956355e86a746d82e0e1464f65d862cc5e71da70e94b2c" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "io-lifetimes" +version = "1.0.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c66c74d2ae7e79a5a8f7ac924adbe38ee42a859c6539ad869eb51f0b52dc220" +dependencies = [ + "hermit-abi 0.3.1", + "libc", + "windows-sys 0.48.0", +] + +[[package]] +name = "ipnetwork" +version = "0.19.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1f84f1612606f3753f205a4e9a2efd6fe5b4c573a6269b2cc6c3003d44a0d127" +dependencies = [ + "serde", +] + +[[package]] +name = "is-terminal" +version = "0.4.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "adcf93614601c8129ddf72e2d5633df827ba6551541c6d8c59520a371475be1f" +dependencies = [ + "hermit-abi 0.3.1", + "io-lifetimes", + "rustix", + "windows-sys 0.48.0", +] + +[[package]] +name = "itertools" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473" +dependencies = [ + "either", +] + +[[package]] +name = "itoa" +version = "1.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "453ad9f582a441959e5f0d088b02ce04cfe8d51a8eaf077f12ac6d3e94164ca6" + +[[package]] +name = "js-sys" +version = "0.3.61" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "445dde2150c55e483f3d8416706b97ec8e8237c307e5b7b4b8dd15e6af2a0730" +dependencies = [ + "wasm-bindgen", +] + +[[package]] +name = "json5" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96b0db21af676c1ce64250b5f40f3ce2cf27e4e47cb91ed91eb6fe9350b430c1" +dependencies = [ + "pest", + "pest_derive", + "serde", +] + +[[package]] +name = "keccak" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3afef3b6eff9ce9d8ff9b3601125eec7f0c8cbac7abd14f355d053fa56c98768" +dependencies = [ + "cpufeatures", +] + +[[package]] +name = "keyed-set" +version = "0.4.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b79e110283e09081809ca488cf3a9709270c6d4d4c4a32674c39cc438366615a" +dependencies = [ + "hashbrown 0.13.2", +] + +[[package]] +name = "kv-log-macro" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0de8b303297635ad57c9f5059fd9cee7a47f8e8daa09df0fcd07dd39fb22977f" +dependencies = [ + "log 0.4.17", +] + +[[package]] +name = "language-tags" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a91d884b6667cd606bb5a69aa0c99ba811a115fc68915e7056ec08a46e93199a" + +[[package]] +name = "lazy_static" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" +dependencies = [ + "spin 0.5.2", +] + +[[package]] +name = "libc" +version = "0.2.141" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3304a64d199bb964be99741b7a14d26972741915b3649639149b2479bb46f4b5" + +[[package]] +name = "libloading" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b67380fd3b2fbe7527a606e18729d21c6f3951633d0500574c4dc22d2d638b9f" +dependencies = [ + "cfg-if", + "winapi", +] + +[[package]] +name = "libm" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "348108ab3fba42ec82ff6e9564fc4ca0247bdccdc68dd8af9764bbc79c3c8ffb" + +[[package]] +name = "link-cplusplus" +version = "1.0.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ecd207c9c713c34f95a097a5b029ac2ce6010530c7b49d7fea24d977dede04f5" +dependencies = [ + "cc", +] + +[[package]] +name = "linked-hash-map" +version = "0.5.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0717cef1bc8b636c6e1c1bbdefc09e6322da8a9321966e8928ef80d20f7f770f" + +[[package]] +name = "linux-raw-sys" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d59d8c75012853d2e872fb56bc8a2e53718e2cafe1a4c823143141c6d90c322f" + +[[package]] +name = "lock_api" +version = "0.4.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "435011366fe56583b16cf956f9df0095b405b82d76425bc8981c0e22e60ec4df" +dependencies = [ + "autocfg", + "scopeguard", +] + +[[package]] +name = "log" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e19e8d5c34a3e0e2223db8e060f9e8264aeeb5c5fc64a4ee9965c062211c024b" +dependencies = [ + "log 0.4.17", +] + +[[package]] +name = "log" +version = "0.4.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e" +dependencies = [ + "cfg-if", + "value-bag", +] + +[[package]] +name = "match_cfg" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ffbee8634e0d45d258acb448e7eaab3fce7a0a467395d4d9f228e3c1f01fb2e4" + +[[package]] +name = "matches" +version = "0.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2532096657941c2fea9c289d370a250971c689d4f143798ff67113ec042024a5" + +[[package]] +name = "md-5" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b5a279bb9607f9f53c22d496eade00d138d1bdcccd07d74650387cf94942a15" +dependencies = [ + "block-buffer 0.9.0", + "digest 0.9.0", + "opaque-debug", +] + +[[package]] +name = "memchr" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" + +[[package]] +name = "memoffset" +version = "0.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5de893c32cde5f383baa4c04c5d6dbdd735cfd4a794b0debdb2bb1b421da5ff4" +dependencies = [ + "autocfg", +] + +[[package]] +name = "memoffset" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d61c719bcfbcf5d62b3a09efa6088de8c54bc0bfcd3ea7ae39fcc186108b8de1" +dependencies = [ + "autocfg", +] + +[[package]] +name = "mime" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba626b8a6de5da682e1caa06bdb42a335aee5a84db8e5046a3e8ab17ba0a3ae0" +dependencies = [ + "log 0.3.9", +] + +[[package]] +name = "mime" +version = "0.3.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6877bb514081ee2a7ff5ef9de3281f14a4dd4bceac4c09388074a6b5df8a139a" + +[[package]] +name = "mime_guess" +version = "2.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4192263c238a5f0d0c6bfd21f336a313a4ce1c450542449ca191bb657b4642ef" +dependencies = [ + "mime 0.3.17", + "unicase 2.6.0", +] + +[[package]] +name = "miniz_oxide" +version = "0.6.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b275950c28b37e794e8c55d88aeb5e139d0ce23fdbbeda68f8d7174abdf9e8fa" +dependencies = [ + "adler", +] + +[[package]] +name = "mio" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b9d9a46eff5b4ff64b45a9e316a6d1e0bc719ef429cbec4dc630684212bfdf9" +dependencies = [ + "libc", + "log 0.4.17", + "wasi 0.11.0+wasi-snapshot-preview1", + "windows-sys 0.45.0", +] + +[[package]] +name = "multipart" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "00dec633863867f29cb39df64a397cdf4a6354708ddd7759f70c7fb51c5f9182" +dependencies = [ + "buf_redux", + "httparse", + "log 0.4.17", + "mime 0.3.17", + "mime_guess", + "quick-error", + "rand", + "safemem 0.3.3", + "tempfile", + "twoway", +] + +[[package]] +name = "multiset" +version = "0.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ce8738c9ddd350996cb8b8b718192851df960803764bcdaa3afb44a63b1ddb5c" + +[[package]] +name = "nanorand" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a51313c5820b0b02bd422f4b44776fbf47961755c74ce64afc73bfad10226c3" +dependencies = [ + "getrandom", +] + +[[package]] +name = "nix" +version = "0.26.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bfdda3d196821d6af13126e40375cdf7da646a96114af134d5f417a9a1dc8e1a" +dependencies = [ + "bitflags 1.3.2", + "cfg-if", + "libc", + "memoffset 0.7.1", + "pin-utils", + "static_assertions", +] + +[[package]] +name = "no-std-net" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43794a0ace135be66a25d3ae77d41b91615fb68ae937f904090203e81f755b65" + +[[package]] +name = "num-bigint-dig" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2399c9463abc5f909349d8aa9ba080e0b88b3ce2885389b60b993f39b1a56905" +dependencies = [ + "byteorder", + "lazy_static", + "libm", + "num-integer", + "num-iter", + "num-traits", + "rand", + "smallvec", + "zeroize", +] + +[[package]] +name = "num-integer" +version = "0.1.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "225d3389fb3509a24c93f5c29eb6bde2586b98d9f016636dff58d7c6f7569cd9" +dependencies = [ + "autocfg", + "num-traits", +] + +[[package]] +name = "num-iter" +version = "0.1.43" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7d03e6c028c5dc5cac6e2dec0efda81fc887605bb3d884578bb6d6bf7514e252" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd" +dependencies = [ + "autocfg", + "libm", +] + +[[package]] +name = "num_cpus" +version = "1.15.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0fac9e2da13b5eb447a6ce3d392f23a29d8694bff781bf03a16cd9ac8697593b" +dependencies = [ + "hermit-abi 0.2.6", + "libc", +] + +[[package]] +name = "num_threads" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2819ce041d2ee131036f4fc9d6ae7ae125a3a40e97ba64d04fe799ad9dabbb44" +dependencies = [ + "libc", +] + +[[package]] +name = "object" +version = "0.30.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ea86265d3d3dcb6a27fc51bd29a4bf387fae9d2986b823079d4986af253eb439" +dependencies = [ + "memchr", +] + +[[package]] +name = "once_cell" +version = "1.17.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b7e5500299e16ebb147ae15a00a942af264cf3688f47923b8fc2cd5858f23ad3" + +[[package]] +name = "opaque-debug" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "624a8340c38c1b80fd549087862da4ba43e08858af025b236e509b6649fc13d5" + +[[package]] +name = "openssl-probe" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff011a302c396a5197692431fc1948019154afc178baf7d8e37367442a4601cf" + +[[package]] +name = "ordered-float" +version = "3.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "13a384337e997e6860ffbaa83708b2ef329fd8c54cb67a5f64d421e0f943254f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "os_str_bytes" +version = "6.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ceedf44fb00f2d1984b0bc98102627ce622e083e49a5bacdb3e514fa4238e267" + +[[package]] +name = "parking" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "14f2252c834a40ed9bb5422029649578e63aa341ac401f74e719dd1afda8394e" + +[[package]] +name = "parking_lot" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3742b2c103b9f06bc9fff0a37ff4912935851bee6d36f3c02bcc755bcfec228f" +dependencies = [ + "lock_api", + "parking_lot_core", +] + +[[package]] +name = "parking_lot_core" +version = "0.9.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9069cbb9f99e3a5083476ccb29ceb1de18b9118cafa53e90c9551235de2b9521" +dependencies = [ + "cfg-if", + "libc", + "redox_syscall 0.2.16", + "smallvec", + "windows-sys 0.45.0", +] + +[[package]] +name = "paste" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f746c4065a8fa3fe23974dd82f15431cc8d40779821001404d10d2e79ca7d79" + +[[package]] +name = "pem-rfc7468" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d159833a9105500e0398934e205e0773f0b27529557134ecfc51c27646adac" +dependencies = [ + "base64ct", +] + +[[package]] +name = "percent-encoding" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "31010dd2e1ac33d5b46a5b413495239882813e0369f8ed8a5e266f173602f831" + +[[package]] +name = "percent-encoding" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "478c572c3d73181ff3c2539045f6eb99e5491218eae919370993b890cdbdd98e" + +[[package]] +name = "pest" +version = "2.5.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b1403e8401ad5dedea73c626b99758535b342502f8d1e361f4a2dd952749122" +dependencies = [ + "thiserror", + "ucd-trie", +] + +[[package]] +name = "pest_derive" +version = "2.5.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "be99c4c1d2fc2769b1d00239431d711d08f6efedcecb8b6e30707160aee99c15" +dependencies = [ + "pest", + "pest_generator", +] + +[[package]] +name = "pest_generator" +version = "2.5.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e56094789873daa36164de2e822b3888c6ae4b4f9da555a1103587658c805b1e" +dependencies = [ + "pest", + "pest_meta", + "proc-macro2", + "quote", + "syn 2.0.15", +] + +[[package]] +name = "pest_meta" +version = "2.5.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6733073c7cff3d8459fda0e42f13a047870242aed8b509fe98000928975f359e" +dependencies = [ + "once_cell", + "pest", + "sha2", +] + +[[package]] +name = "petgraph" +version = "0.6.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4dd7d28ee937e54fe3080c91faa1c3a46c06de6252988a7f4592ba2310ef22a4" +dependencies = [ + "fixedbitset", + "indexmap", +] + +[[package]] +name = "pin-project" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ad29a609b6bcd67fee905812e544992d216af9d755757c05ed2d0e15a74c6ecc" +dependencies = [ + "pin-project-internal", +] + +[[package]] +name = "pin-project-internal" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "069bdb1e05adc7a8990dce9cc75370895fbe4e3d58b9b73bf1aee56359344a55" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "pin-project-lite" +version = "0.2.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e0a7ae3ac2f1173085d398531c705756c94a4c56843785df85a60c1a0afac116" + +[[package]] +name = "pin-utils" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" + +[[package]] +name = "pkcs1" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eff33bdbdfc54cc98a2eca766ebdec3e1b8fb7387523d5c9c9a2891da856f719" +dependencies = [ + "der", + "pkcs8", + "spki", + "zeroize", +] + +[[package]] +name = "pkcs8" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9eca2c590a5f85da82668fa685c09ce2888b9430e83299debf1f34b65fd4a4ba" +dependencies = [ + "der", + "spki", +] + +[[package]] +name = "pnet" +version = "0.31.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0caaf5b11fd907ff15cf14a4477bfabca4b37ab9e447a4f8dead969a59cdafad" +dependencies = [ + "pnet_base", + "pnet_datalink", + "pnet_packet", + "pnet_transport", +] + +[[package]] +name = "pnet_base" +version = "0.31.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9d3a993d49e5fd5d4d854d6999d4addca1f72d86c65adf224a36757161c02b6" +dependencies = [ + "no-std-net", +] + +[[package]] +name = "pnet_datalink" +version = "0.31.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e466faf03a98ad27f6e15cd27a2b7cc89e73e640a43527742977bc503c37f8aa" +dependencies = [ + "ipnetwork", + "libc", + "pnet_base", + "pnet_sys", + "winapi", +] + +[[package]] +name = "pnet_macros" +version = "0.31.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "48dd52a5211fac27e7acb14cfc9f30ae16ae0e956b7b779c8214c74559cef4c3" +dependencies = [ + "proc-macro2", + "quote", + "regex 1.7.3", + "syn 1.0.109", +] + +[[package]] +name = "pnet_macros_support" +version = "0.31.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "89de095dc7739349559913aed1ef6a11e73ceade4897dadc77c5e09de6740750" +dependencies = [ + "pnet_base", +] + +[[package]] +name = "pnet_packet" +version = "0.31.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bc3b5111e697c39c8b9795b9fdccbc301ab696699e88b9ea5a4e4628978f495f" +dependencies = [ + "glob", + "pnet_base", + "pnet_macros", + "pnet_macros_support", +] + +[[package]] +name = "pnet_sys" +version = "0.31.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "328e231f0add6d247d82421bf3790b4b33b39c8930637f428eef24c4c6a90805" +dependencies = [ + "libc", + "winapi", +] + +[[package]] +name = "pnet_transport" +version = "0.31.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff597185e6f1f5671b3122e4dba892a1c73e17c17e723d7669bd9299cbe7f124" +dependencies = [ + "libc", + "pnet_base", + "pnet_packet", + "pnet_sys", +] + +[[package]] +name = "polling" +version = "2.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4be1c66a6add46bff50935c313dae30a5030cf8385c5206e8a95e9e9def974aa" +dependencies = [ + "autocfg", + "bitflags 1.3.2", + "cfg-if", + "concurrent-queue", + "libc", + "log 0.4.17", + "pin-project-lite", + "windows-sys 0.48.0", +] + +[[package]] +name = "ppv-lite86" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" + +[[package]] +name = "proc-macro-hack" +version = "0.5.20+deprecated" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc375e1527247fe1a97d8b7156678dfe7c1af2fc075c9a4db3690ecd2a148068" + +[[package]] +name = "proc-macro2" +version = "1.0.56" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b63bdb0cd06f1f4dedf69b254734f9b45af66e4a031e42a7480257d9898b435" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quick-error" +version = "1.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1d01941d82fa2ab50be1e79e6714289dd7cde78eba4c074bc5a4374f650dfe0" + +[[package]] +name = "quinn" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "445cbfe2382fa023c4f2f3c7e1c95c03dcc1df2bf23cebcb2b13e1402c4394d1" +dependencies = [ + "bytes", + "pin-project-lite", + "quinn-proto", + "quinn-udp", + "rustc-hash", + "rustls", + "thiserror", + "tokio", + "tracing", + "webpki", +] + +[[package]] +name = "quinn-proto" +version = "0.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67c10f662eee9c94ddd7135043e544f3c82fa839a1e7b865911331961b53186c" +dependencies = [ + "bytes", + "rand", + "ring", + "rustc-hash", + "rustls", + "rustls-native-certs", + "slab", + "thiserror", + "tinyvec", + "tracing", + "webpki", +] + +[[package]] +name = "quinn-udp" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "641538578b21f5e5c8ea733b736895576d0fe329bb883b937db6f4d163dbaaf4" +dependencies = [ + "libc", + "quinn-proto", + "socket2", + "tracing", + "windows-sys 0.42.0", +] + +[[package]] +name = "quote" +version = "1.0.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4424af4bf778aae2051a77b60283332f386554255d722233d09fbfc7e30da2fc" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "rand" +version = "0.8.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34af8d1a0e25924bc5b7c43c079c942339d8f0a8b57c39049bef581b46327404" +dependencies = [ + "libc", + "rand_chacha", + "rand_core", +] + +[[package]] +name = "rand_chacha" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +dependencies = [ + "getrandom", +] + +[[package]] +name = "redox_syscall" +version = "0.2.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb5a58c1855b4b6819d59012155603f0b22ad30cad752600aadfcb695265519a" +dependencies = [ + "bitflags 1.3.2", +] + +[[package]] +name = "redox_syscall" +version = "0.3.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "567664f262709473930a4bf9e51bf2ebf3348f2e748ccc50dea20646858f8f29" +dependencies = [ + "bitflags 1.3.2", +] + +[[package]] +name = "redox_users" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b033d837a7cf162d7993aded9304e30a83213c648b6e389db233191f891e5c2b" +dependencies = [ + "getrandom", + "redox_syscall 0.2.16", + "thiserror", +] + +[[package]] +name = "regex" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9329abc99e39129fcceabd24cf5d85b4671ef7c29c50e972bc5afe32438ec384" +dependencies = [ + "aho-corasick 0.6.10", + "memchr", + "regex-syntax 0.5.6", + "thread_local", + "utf8-ranges", +] + +[[package]] +name = "regex" +version = "1.7.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b1f693b24f6ac912f4893ef08244d70b6067480d2f1a46e950c9691e6749d1d" +dependencies = [ + "aho-corasick 0.7.20", + "memchr", + "regex-syntax 0.6.29", +] + +[[package]] +name = "regex-syntax" +version = "0.5.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7d707a4fa2637f2dca2ef9fd02225ec7661fe01a53623c1e6515b6916511f7a7" +dependencies = [ + "ucd-util", +] + +[[package]] +name = "regex-syntax" +version = "0.6.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f162c6dd7b008981e4d40210aca20b4bd0f9b60ca9271061b07f78537722f2e1" + +[[package]] +name = "ring" +version = "0.16.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3053cf52e236a3ed746dfc745aa9cacf1b791d846bdaf412f60a8d7d6e17c8fc" +dependencies = [ + "cc", + "libc", + "once_cell", + "spin 0.5.2", + "untrusted", + "web-sys", + "winapi", +] + +[[package]] +name = "ringbuffer-spsc" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fd1938faa63a2362ee1747afb2d10567d0fb1413b9cbd6198a8541485c4f773" +dependencies = [ + "array-init", + "cache-padded", +] + +[[package]] +name = "ros_message" +version = "0.1.0" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#a3fc4ceefa82a58250677b12cfe3c9df4ea239f0" +dependencies = [ + "array-init", + "hex", + "itertools", + "lazy_static", + "md-5", + "regex 1.7.3", + "serde", + "serde_derive", + "thiserror", +] + +[[package]] +name = "rosrust" +version = "0.9.10" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#a3fc4ceefa82a58250677b12cfe3c9df4ea239f0" +dependencies = [ + "byteorder", + "colored", + "crossbeam", + "ctrlc", + "error-chain 0.12.4", + "hostname", + "lazy_static", + "log 0.4.17", + "regex 1.7.3", + "ros_message", + "rosrust_codegen", + "serde", + "serde_derive", + "socket2", + "xml-rpc", + "yaml-rust", +] + +[[package]] +name = "rosrust_codegen" +version = "0.9.6" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#a3fc4ceefa82a58250677b12cfe3c9df4ea239f0" +dependencies = [ + "error-chain 0.12.4", + "hex", + "lazy_static", + "md-5", + "proc-macro2", + "quote", + "ros_message", + "syn 1.0.109", +] + +[[package]] +name = "rouille" +version = "3.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4f86e4c51a773f953f02bbab5fd049f004bfd384341d62da2a079aff812ab176" +dependencies = [ + "base64 0.13.1", + "brotli", + "chrono", + "deflate", + "filetime", + "multipart", + "num_cpus", + "percent-encoding 2.2.0", + "rand", + "serde", + "serde_derive", + "serde_json", + "sha1", + "threadpool", + "time 0.3.20", + "tiny_http", + "url 2.3.1", +] + +[[package]] +name = "rsa" +version = "0.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "094052d5470cbcef561cb848a7209968c9f12dfa6d668f4bca048ac5de51099c" +dependencies = [ + "byteorder", + "digest 0.10.6", + "num-bigint-dig", + "num-integer", + "num-iter", + "num-traits", + "pkcs1", + "pkcs8", + "rand_core", + "signature", + "smallvec", + "subtle", + "zeroize", +] + +[[package]] +name = "rustc-demangle" +version = "0.1.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d626bb9dae77e28219937af045c257c28bfd3f69333c512553507f5f9798cb76" + +[[package]] +name = "rustc-hash" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" + +[[package]] +name = "rustc_version" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" +dependencies = [ + "semver", +] + +[[package]] +name = "rustix" +version = "0.37.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85597d61f83914ddeba6a47b3b8ffe7365107221c2e557ed94426489fefb5f77" +dependencies = [ + "bitflags 1.3.2", + "errno", + "io-lifetimes", + "libc", + "linux-raw-sys", + "windows-sys 0.48.0", +] + +[[package]] +name = "rustls" +version = "0.20.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fff78fc74d175294f4e83b28343315ffcfb114b156f0185e9741cb5570f50e2f" +dependencies = [ + "log 0.4.17", + "ring", + "sct", + "webpki", +] + +[[package]] +name = "rustls-native-certs" +version = "0.6.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0167bac7a9f490495f3c33013e7722b53cb087ecbe082fb0c6387c96f634ea50" +dependencies = [ + "openssl-probe", + "rustls-pemfile", + "schannel", + "security-framework", +] + +[[package]] +name = "rustls-pemfile" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d194b56d58803a43635bdc398cd17e383d6f71f9182b9a192c127ca42494a59b" +dependencies = [ + "base64 0.21.0", +] + +[[package]] +name = "rustversion" +version = "1.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4f3208ce4d8448b3f3e7d168a73f5e0c43a61e32930de3bceeccedb388b6bf06" + +[[package]] +name = "ryu" +version = "1.0.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f91339c0467de62360649f8d3e185ca8de4224ff281f66000de5eb2a77a79041" + +[[package]] +name = "safemem" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e27a8b19b835f7aea908818e871f5cc3a5a186550c30773be987e155e8163d8f" + +[[package]] +name = "safemem" +version = "0.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef703b7cb59335eae2eb93ceb664c0eb7ea6bf567079d843e09420219668e072" + +[[package]] +name = "schannel" +version = "0.1.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "713cfb06c7059f3588fb8044c0fad1d09e3c01d225e25b9220dbfdcf16dbb1b3" +dependencies = [ + "windows-sys 0.42.0", +] + +[[package]] +name = "scopeguard" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d29ab0c6d3fc0ee92fe66e2d99f700eab17a8d57d1c1d3b748380fb20baa78cd" + +[[package]] +name = "scratch" +version = "1.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1792db035ce95be60c3f8853017b3999209281c24e2ba5bc8e59bf97a0c590c1" + +[[package]] +name = "sct" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d53dcdb7c9f8158937a7981b48accfd39a43af418591a5d008c7b22b5e1b7ca4" +dependencies = [ + "ring", + "untrusted", +] + +[[package]] +name = "security-framework" +version = "2.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a332be01508d814fed64bf28f798a146d73792121129962fdf335bb3c49a4254" +dependencies = [ + "bitflags 1.3.2", + "core-foundation", + "core-foundation-sys", + "libc", + "security-framework-sys", +] + +[[package]] +name = "security-framework-sys" +version = "2.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "31c9bb296072e961fcbd8853511dd39c2d8be2deb1e17c6860b1d30732b323b4" +dependencies = [ + "core-foundation-sys", + "libc", +] + +[[package]] +name = "semver" +version = "1.0.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bebd363326d05ec3e2f532ab7660680f3b02130d780c299bca73469d521bc0ed" + +[[package]] +name = "serde" +version = "1.0.160" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bb2f3770c8bce3bcda7e149193a069a0f4365bda1fa5cd88e03bca26afc1216c" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde-xml-rs" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c06881f4313eec67d4ecfcd8e14339f6042cfc0de4b1bd3ceae74c29d597f68" +dependencies = [ + "log 0.3.9", + "serde", + "xml-rs 0.3.6", +] + +[[package]] +name = "serde_bytes" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "defbb8a83d7f34cc8380751eeb892b825944222888aff18996ea7901f24aec88" +dependencies = [ + "serde", +] + +[[package]] +name = "serde_derive" +version = "1.0.160" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "291a097c63d8497e00160b166a967a4a79c64f3facdd01cbd7502231688d77df" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.15", +] + +[[package]] +name = "serde_json" +version = "1.0.96" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "057d394a50403bcac12672b2b18fb387ab6d289d957dab67dd201875391e52f1" +dependencies = [ + "itoa", + "ryu", + "serde", +] + +[[package]] +name = "serde_yaml" +version = "0.9.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9d684e3ec7de3bf5466b32bd75303ac16f0736426e5a4e0d6e489559ce1249c" +dependencies = [ + "indexmap", + "itoa", + "ryu", + "serde", + "unsafe-libyaml", +] + +[[package]] +name = "serial_test" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1c789ec87f4687d022a2405cf46e0cd6284889f1839de292cadeb6c6019506f2" +dependencies = [ + "dashmap", + "futures 0.3.28", + "lazy_static", + "log 0.4.17", + "parking_lot", + "serial_test_derive", +] + +[[package]] +name = "serial_test_derive" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b64f9e531ce97c88b4778aad0ceee079216071cffec6ac9b904277f8f92e7fe3" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "sha1" +version = "0.10.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f04293dc80c3993519f2d7f6f511707ee7094fe0c6d3406feb330cdb3540eba3" +dependencies = [ + "cfg-if", + "cpufeatures", + "digest 0.10.6", +] + +[[package]] +name = "sha2" +version = "0.10.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "82e6b795fe2e3b1e845bafcb27aa35405c4d47cdfc92af5fc8d3002f76cebdc0" +dependencies = [ + "cfg-if", + "cpufeatures", + "digest 0.10.6", +] + +[[package]] +name = "sha3" +version = "0.10.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54c2bb1a323307527314a36bfb73f24febb08ce2b8a554bf4ffd6f51ad15198c" +dependencies = [ + "digest 0.10.6", + "keccak", +] + +[[package]] +name = "shellexpand" +version = "3.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da03fa3b94cc19e3ebfc88c4229c49d8f08cdbd1228870a45f0ffdf84988e14b" +dependencies = [ + "dirs", +] + +[[package]] +name = "signal-hook" +version = "0.3.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "732768f1176d21d09e076c23a93123d40bba92d50c4058da34d45c8de8e682b9" +dependencies = [ + "libc", + "signal-hook-registry", +] + +[[package]] +name = "signal-hook-registry" +version = "1.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d8229b473baa5980ac72ef434c4415e70c4b5e71b423043adb4ba059f89c99a1" +dependencies = [ + "libc", +] + +[[package]] +name = "signature" +version = "1.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "74233d3b3b2f6d4b006dc19dee745e73e2a6bfb6f93607cd3b02bd5b00797d7c" +dependencies = [ + "digest 0.10.6", + "rand_core", +] + +[[package]] +name = "slab" +version = "0.4.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6528351c9bc8ab22353f9d776db39a20288e8d6c37ef8cfe3317cf875eecfc2d" +dependencies = [ + "autocfg", +] + +[[package]] +name = "smallvec" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a507befe795404456341dfab10cef66ead4c041f62b8b11bbb92bffe5d0953e0" + +[[package]] +name = "socket2" +version = "0.4.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "64a4a911eed85daf18834cfaa86a79b7d266ff93ff5ba14005426219480ed662" +dependencies = [ + "libc", + "winapi", +] + +[[package]] +name = "spin" +version = "0.5.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6e63cff320ae2c57904679ba7cb63280a3dc4613885beafb148ee7bf9aa9042d" + +[[package]] +name = "spin" +version = "0.9.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" +dependencies = [ + "lock_api", +] + +[[package]] +name = "spki" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67cf02bbac7a337dc36e4f5a693db6c21e7863f45070f7064577eb4367a3212b" +dependencies = [ + "base64ct", + "der", +] + +[[package]] +name = "static_assertions" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" + +[[package]] +name = "stop-token" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "af91f480ee899ab2d9f8435bfdfc14d08a5754bd9d3fef1f1a1c23336aad6c8b" +dependencies = [ + "async-channel", + "cfg-if", + "futures-core", + "pin-project-lite", +] + +[[package]] +name = "strsim" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" + +[[package]] +name = "strum" +version = "0.24.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "063e6045c0e62079840579a7e47a355ae92f60eb74daaf156fb1e84ba164e63f" + +[[package]] +name = "strum_macros" +version = "0.24.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e385be0d24f186b4ce2f9982191e7101bb737312ad61c1f2f984f34bcf85d59" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "rustversion", + "syn 1.0.109", +] + +[[package]] +name = "subtle" +version = "2.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6bdef32e8150c2a081110b42772ffe7d7c9032b606bc226c8260fd97e0976601" + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "2.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a34fcf3e8b60f57e6a14301a2e916d323af98b0ea63c599441eec8558660c822" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "tempfile" +version = "3.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9fbec84f381d5795b08656e4912bec604d162bff9291d6189a78f4c8ab87998" +dependencies = [ + "cfg-if", + "fastrand", + "redox_syscall 0.3.5", + "rustix", + "windows-sys 0.45.0", +] + +[[package]] +name = "termcolor" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "be55cf8942feac5c765c2c993422806843c9a9a45d4d5c407ad6dd2ea95eb9b6" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "textwrap" +version = "0.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "222a222a5bfe1bba4a77b45ec488a741b3cb8872e5e499451fd7d0129c9c7c3d" + +[[package]] +name = "thiserror" +version = "1.0.40" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "978c9a314bd8dc99be594bc3c175faaa9794be04a5a5e153caba6915336cebac" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "1.0.40" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9456a42c5b0d803c8cd86e73dd7cc9edd429499f37a3550d286d5e86720569f" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.15", +] + +[[package]] +name = "thread_local" +version = "0.3.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c6b53e329000edc2b34dbe8545fd20e55a333362d0a321909685a19bd28c3f1b" +dependencies = [ + "lazy_static", +] + +[[package]] +name = "threadpool" +version = "1.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d050e60b33d41c19108b32cea32164033a9013fe3b46cbd4457559bfbf77afaa" +dependencies = [ + "num_cpus", +] + +[[package]] +name = "time" +version = "0.1.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b797afad3f312d1c66a56d11d0316f916356d11bd158fbc6ca6389ff6bf805a" +dependencies = [ + "libc", + "wasi 0.10.0+wasi-snapshot-preview1", + "winapi", +] + +[[package]] +name = "time" +version = "0.3.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cd0cbfecb4d19b5ea75bb31ad904eb5b9fa13f21079c3b92017ebdf4999a5890" +dependencies = [ + "libc", + "num_threads", + "serde", + "time-core", +] + +[[package]] +name = "time-core" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2e153e1f1acaef8acc537e68b44906d2db6436e2b35ac2c6b42640fff91f00fd" + +[[package]] +name = "tiny_http" +version = "0.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "389915df6413a2e74fb181895f933386023c71110878cd0825588928e64cdc82" +dependencies = [ + "ascii", + "chunked_transfer", + "httpdate", + "log 0.4.17", +] + +[[package]] +name = "tinyvec" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "87cc5ceb3875bb20c2890005a4e226a4651264a5c75edb2421b52861a0a0cb50" +dependencies = [ + "tinyvec_macros", +] + +[[package]] +name = "tinyvec_macros" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1f3ccbac311fea05f86f61904b462b55fb3df8837a366dfc601a0161d0532f20" + +[[package]] +name = "token-cell" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f4a2b964fdb303b08a4eab04d7c1bad2bca33f8eee334ccd28802f1041c6eb87" +dependencies = [ + "paste", +] + +[[package]] +name = "tokio" +version = "1.27.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d0de47a4eecbe11f498978a9b29d792f0d2692d1dd003650c24c76510e3bc001" +dependencies = [ + "autocfg", + "bytes", + "libc", + "mio", + "num_cpus", + "pin-project-lite", + "socket2", + "tokio-macros", + "windows-sys 0.45.0", +] + +[[package]] +name = "tokio-macros" +version = "2.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "61a573bdc87985e9d6ddeed1b3d864e8a302c847e40d647746df2f1de209d1ce" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.15", +] + +[[package]] +name = "tokio-tungstenite" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54319c93411147bced34cb5609a80e0a8e44c5999c93903a81cd866630ec0bfd" +dependencies = [ + "futures-util", + "log 0.4.17", + "tokio", + "tungstenite", +] + +[[package]] +name = "tracing" +version = "0.1.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ce8c33a8d48bd45d624a6e523445fd21ec13d3653cd51f681abf67418f54eb8" +dependencies = [ + "cfg-if", + "pin-project-lite", + "tracing-attributes", + "tracing-core", +] + +[[package]] +name = "tracing-attributes" +version = "0.1.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4017f8f45139870ca7e672686113917c71c7a6e02d4924eda67186083c03081a" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "tracing-core" +version = "0.1.30" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24eb03ba0eab1fd845050058ce5e616558e8f8d8fca633e6b163fe25c797213a" +dependencies = [ + "once_cell", +] + +[[package]] +name = "traitobject" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "efd1f82c56340fdf16f2a953d7bda4f8fdffba13d93b00844c25572110b26079" + +[[package]] +name = "tungstenite" +version = "0.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30ee6ab729cd4cf0fd55218530c4522ed30b7b6081752839b68fcec8d0960788" +dependencies = [ + "base64 0.13.1", + "byteorder", + "bytes", + "http", + "httparse", + "log 0.4.17", + "rand", + "sha1", + "thiserror", + "url 2.3.1", + "utf-8", +] + +[[package]] +name = "twoway" +version = "0.1.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "59b11b2b5241ba34be09c3cc85a36e56e48f9888862e19cedf23336d35316ed1" +dependencies = [ + "memchr", +] + +[[package]] +name = "typeable" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1410f6f91f21d1612654e7cc69193b0334f909dcf2c790c4826254fbb86f8887" + +[[package]] +name = "typenum" +version = "1.16.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "497961ef93d974e23eb6f433eb5fe1b7930b659f06d12dec6fc44a8f554c0bba" + +[[package]] +name = "ucd-trie" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e79c4d996edb816c91e4308506774452e55e95c3c9de07b6729e17e15a5ef81" + +[[package]] +name = "ucd-util" +version = "0.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "abd2fc5d32b590614af8b0a20d837f32eca055edd0bbead59a9cfe80858be003" + +[[package]] +name = "uhlc" +version = "0.5.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d291a7454d390b753ef68df8145da18367e32883ec2fa863959f0aefb915cdb" +dependencies = [ + "hex", + "humantime", + "lazy_static", + "log 0.4.17", + "serde", + "spin 0.9.8", + "uuid", +] + +[[package]] +name = "unicase" +version = "1.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f4765f83163b74f957c797ad9253caf97f103fb064d3999aea9568d09fc8a33" +dependencies = [ + "version_check 0.1.5", +] + +[[package]] +name = "unicase" +version = "2.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "50f37be617794602aabbeee0be4f259dc1778fabe05e2d67ee8f79326d5cb4f6" +dependencies = [ + "version_check 0.9.4", +] + +[[package]] +name = "unicode-bidi" +version = "0.3.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92888ba5573ff080736b3648696b70cafad7d250551175acbaa4e0385b3e1460" + +[[package]] +name = "unicode-ident" +version = "1.0.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5464a87b239f13a63a501f2701565754bae92d243d4bb7eb12f6d57d2269bf4" + +[[package]] +name = "unicode-normalization" +version = "0.1.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c5713f0fc4b5db668a2ac63cdb7bb4469d8c9fed047b1d0292cc7b0ce2ba921" +dependencies = [ + "tinyvec", +] + +[[package]] +name = "unicode-width" +version = "0.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c0edd1e5b14653f783770bce4a4dabb4a5108a5370a5f5d8cfe8710c361f6c8b" + +[[package]] +name = "unsafe-libyaml" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1865806a559042e51ab5414598446a5871b561d21b6764f2eabb0dd481d880a6" + +[[package]] +name = "untrusted" +version = "0.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a156c684c91ea7d62626509bce3cb4e1d9ed5c4d978f7b4352658f96a4c26b4a" + +[[package]] +name = "unzip-n" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c2e7e85a0596447f0f2ac090e16bc4c516c6fe91771fb0c0ccf7fa3dae896b9c" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "url" +version = "1.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dd4e7c0d531266369519a4aa4f399d748bd37043b00bde1e4ff1f60a120b355a" +dependencies = [ + "idna 0.1.5", + "matches", + "percent-encoding 1.0.1", +] + +[[package]] +name = "url" +version = "2.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d68c799ae75762b8c3fe375feb6600ef5602c883c5d21eb51c09f22b83c4643" +dependencies = [ + "form_urlencoded", + "idna 0.3.0", + "percent-encoding 2.2.0", +] + +[[package]] +name = "utf-8" +version = "0.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09cc8ee72d2a9becf2f2febe0205bbed8fc6615b7cb429ad062dc7b7ddd036a9" + +[[package]] +name = "utf8-ranges" +version = "1.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7fcfc827f90e53a02eaef5e535ee14266c1d569214c6aa70133a624d8a3164ba" + +[[package]] +name = "uuid" +version = "1.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b55a3fef2a1e3b3a00ce878640918820d3c51081576ac657d23af9fc7928fdb" +dependencies = [ + "getrandom", +] + +[[package]] +name = "validated_struct" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "feef04c049b4beae3037a2a31b8da40d8cebec0b97456f24c7de0ede4ed9efed" +dependencies = [ + "json5", + "serde", + "serde_json", + "validated_struct_macros", +] + +[[package]] +name = "validated_struct_macros" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9d4444a980afa9ef0d29c2a3f4d952ec0495a7a996a9c78b52698b71bc21edb4" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", + "unzip-n", +] + +[[package]] +name = "value-bag" +version = "1.0.0-alpha.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2209b78d1249f7e6f3293657c9779fe31ced465df091bbd433a1cf88e916ec55" +dependencies = [ + "ctor", + "version_check 0.9.4", +] + +[[package]] +name = "vec_map" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191" + +[[package]] +name = "version_check" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "914b1a6776c4c929a602fafd8bc742e06365d4bcbe48c30f9cca5824f70dc9dd" + +[[package]] +name = "version_check" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" + +[[package]] +name = "waker-fn" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9d5b2c62b4012a3e1eca5a7e077d13b3bf498c4073e33ccd58626607748ceeca" + +[[package]] +name = "wasi" +version = "0.10.0+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1a143597ca7c7793eff794def352d41792a93c481eb1042423ff7ff72ba2c31f" + +[[package]] +name = "wasi" +version = "0.11.0+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9c8d87e72b64a3b4db28d11ce29237c246188f4f51057d65a7eab63b7987e423" + +[[package]] +name = "wasm-bindgen" +version = "0.2.84" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "31f8dcbc21f30d9b8f2ea926ecb58f6b91192c17e9d33594b3df58b2007ca53b" +dependencies = [ + "cfg-if", + "wasm-bindgen-macro", +] + +[[package]] +name = "wasm-bindgen-backend" +version = "0.2.84" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95ce90fd5bcc06af55a641a86428ee4229e44e07033963a2290a8e241607ccb9" +dependencies = [ + "bumpalo", + "log 0.4.17", + "once_cell", + "proc-macro2", + "quote", + "syn 1.0.109", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-futures" +version = "0.4.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f219e0d211ba40266969f6dbdd90636da12f75bee4fc9d6c23d1260dadb51454" +dependencies = [ + "cfg-if", + "js-sys", + "wasm-bindgen", + "web-sys", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.84" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4c21f77c0bedc37fd5dc21f897894a5ca01e7bb159884559461862ae90c0b4c5" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.84" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2aff81306fcac3c7515ad4e177f521b5c9a15f2b08f4e32d823066102f35a5f6" +dependencies = [ + "proc-macro2", + "quote", + "syn 1.0.109", + "wasm-bindgen-backend", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.84" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0046fef7e28c3804e5e38bfa31ea2a0f73905319b677e57ebe37e49358989b5d" + +[[package]] +name = "web-sys" +version = "0.3.61" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e33b99f4b23ba3eec1a53ac264e35a755f00e966e0065077d6027c0f575b0b97" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + +[[package]] +name = "webpki" +version = "0.22.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f095d78192e208183081cc07bc5515ef55216397af48b873e5edcd72637fa1bd" +dependencies = [ + "ring", + "untrusted", +] + +[[package]] +name = "webpki-roots" +version = "0.22.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6c71e40d7d2c34a5106301fb632274ca37242cd0c9d3e64dbece371a40a2d87" +dependencies = [ + "webpki", +] + +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-util" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178" +dependencies = [ + "winapi", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + +[[package]] +name = "windows" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e686886bc078bc1b0b600cac0147aadb815089b6e4da64016cbd754b6342700f" +dependencies = [ + "windows-targets 0.48.0", +] + +[[package]] +name = "windows-sys" +version = "0.42.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a3e1820f08b8513f676f7ab6c1f99ff312fb97b553d30ff4dd86f9f15728aa7" +dependencies = [ + "windows_aarch64_gnullvm 0.42.2", + "windows_aarch64_msvc 0.42.2", + "windows_i686_gnu 0.42.2", + "windows_i686_msvc 0.42.2", + "windows_x86_64_gnu 0.42.2", + "windows_x86_64_gnullvm 0.42.2", + "windows_x86_64_msvc 0.42.2", +] + +[[package]] +name = "windows-sys" +version = "0.45.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "75283be5efb2831d37ea142365f009c02ec203cd29a3ebecbc093d52315b66d0" +dependencies = [ + "windows-targets 0.42.2", +] + +[[package]] +name = "windows-sys" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" +dependencies = [ + "windows-targets 0.48.0", +] + +[[package]] +name = "windows-targets" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e5180c00cd44c9b1c88adb3693291f1cd93605ded80c250a75d472756b4d071" +dependencies = [ + "windows_aarch64_gnullvm 0.42.2", + "windows_aarch64_msvc 0.42.2", + "windows_i686_gnu 0.42.2", + "windows_i686_msvc 0.42.2", + "windows_x86_64_gnu 0.42.2", + "windows_x86_64_gnullvm 0.42.2", + "windows_x86_64_msvc 0.42.2", +] + +[[package]] +name = "windows-targets" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b1eb6f0cd7c80c79759c929114ef071b87354ce476d9d94271031c0497adfd5" +dependencies = [ + "windows_aarch64_gnullvm 0.48.0", + "windows_aarch64_msvc 0.48.0", + "windows_i686_gnu 0.48.0", + "windows_i686_msvc 0.48.0", + "windows_x86_64_gnu 0.48.0", + "windows_x86_64_gnullvm 0.48.0", + "windows_x86_64_msvc 0.48.0", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "597a5118570b68bc08d8d59125332c54f1ba9d9adeedeef5b99b02ba2b0698f8" + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91ae572e1b79dba883e0d315474df7305d12f569b400fcf90581b06062f7e1bc" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e08e8864a60f06ef0d0ff4ba04124db8b0fb3be5776a5cd47641e942e58c4d43" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b2ef27e0d7bdfcfc7b868b317c1d32c641a6fe4629c171b8928c7b08d98d7cf3" + +[[package]] +name = "windows_i686_gnu" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c61d927d8da41da96a81f029489353e68739737d3beca43145c8afec9a31a84f" + +[[package]] +name = "windows_i686_gnu" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "622a1962a7db830d6fd0a69683c80a18fda201879f0f447f065a3b7467daa241" + +[[package]] +name = "windows_i686_msvc" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "44d840b6ec649f480a41c8d80f9c65108b92d89345dd94027bfe06ac444d1060" + +[[package]] +name = "windows_i686_msvc" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4542c6e364ce21bf45d69fdd2a8e455fa38d316158cfd43b3ac1c5b1b19f8e00" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8de912b8b8feb55c064867cf047dda097f92d51efad5b491dfb98f6bbb70cb36" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ca2b8a661f7628cbd23440e50b05d705db3686f894fc9580820623656af974b1" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "26d41b46a36d453748aedef1486d5c7a85db22e56aff34643984ea85514e94a3" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7896dbc1f41e08872e9d5e8f8baa8fdd2677f29468c4e156210174edc7f7b953" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.42.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9aec5da331524158c6d1a4ac0ab1541149c0b9505fde06423b02f5ef0106b9f0" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1a515f5799fe4961cb532f983ce2b23082366b898e52ffbce459c86f67c8378a" + +[[package]] +name = "xml-rpc" +version = "0.0.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3764ec0b1224a569a4962c6f45663bcd734662dd3f3e6485c3b8ca81fa5e00a2" +dependencies = [ + "base64 0.6.0", + "error-chain 0.10.0", + "futures 0.1.31", + "hyper", + "lazy_static", + "regex 0.2.11", + "rouille", + "serde", + "serde-xml-rs", + "serde_bytes", + "serde_derive", + "xml-rs 0.6.1", +] + +[[package]] +name = "xml-rs" +version = "0.3.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7ec6c39eaa68382c8e31e35239402c0a9489d4141a8ceb0c716099a0b515b562" +dependencies = [ + "bitflags 0.7.0", +] + +[[package]] +name = "xml-rs" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e1945e12e16b951721d7976520b0832496ef79c31602c7a29d950de79ba74621" +dependencies = [ + "bitflags 0.9.1", +] + +[[package]] +name = "yaml-rust" +version = "0.4.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "56c1936c4cc7a1c9ab21a1ebb602eb942ba868cbd44a99cb7cdc5892335e1c85" +dependencies = [ + "linked-hash-map", +] + +[[package]] +name = "zenoh" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-global-executor", + "async-std", + "async-trait", + "base64 0.21.0", + "env_logger 0.10.0", + "event-listener", + "flume", + "form_urlencoded", + "futures 0.3.28", + "git-version", + "hex", + "lazy_static", + "log 0.4.17", + "ordered-float", + "petgraph", + "rand", + "regex 1.7.3", + "rustc_version", + "serde", + "serde_json", + "socket2", + "stop-token", + "uhlc", + "uuid", + "vec_map", + "zenoh-buffers", + "zenoh-cfg-properties", + "zenoh-codec", + "zenoh-collections", + "zenoh-config", + "zenoh-core", + "zenoh-crypto", + "zenoh-link", + "zenoh-macros", + "zenoh-plugin-trait", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", + "zenoh-transport", + "zenoh-util", +] + +[[package]] +name = "zenoh-buffers" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "zenoh-collections", +] + +[[package]] +name = "zenoh-cfg-properties" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "zenoh-result", +] + +[[package]] +name = "zenoh-codec" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "uhlc", + "zenoh-buffers", + "zenoh-protocol", +] + +[[package]] +name = "zenoh-collections" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" + +[[package]] +name = "zenoh-config" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "flume", + "json5", + "num_cpus", + "serde", + "serde_json", + "serde_yaml", + "validated_struct", + "zenoh-cfg-properties", + "zenoh-core", + "zenoh-protocol", + "zenoh-result", + "zenoh-util", +] + +[[package]] +name = "zenoh-core" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "lazy_static", + "zenoh-result", +] + +[[package]] +name = "zenoh-crypto" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "aes", + "hmac", + "rand", + "rand_chacha", + "sha3", + "zenoh-result", +] + +[[package]] +name = "zenoh-ext" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "bincode", + "env_logger 0.10.0", + "flume", + "futures 0.3.28", + "log 0.4.17", + "serde", + "zenoh", + "zenoh-core", + "zenoh-macros", + "zenoh-result", + "zenoh-sync", + "zenoh-util", +] + +[[package]] +name = "zenoh-keyexpr" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "hashbrown 0.13.2", + "keyed-set", + "rand", + "serde", + "token-cell", + "zenoh-result", +] + +[[package]] +name = "zenoh-link" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "async-trait", + "zenoh-cfg-properties", + "zenoh-config", + "zenoh-link-commons", + "zenoh-link-quic", + "zenoh-link-tcp", + "zenoh-link-tls", + "zenoh-link-udp", + "zenoh-link-unixsock_stream", + "zenoh-link-ws", + "zenoh-protocol", + "zenoh-result", +] + +[[package]] +name = "zenoh-link-commons" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "async-trait", + "flume", + "serde", + "typenum", + "zenoh-buffers", + "zenoh-cfg-properties", + "zenoh-codec", + "zenoh-protocol", + "zenoh-result", +] + +[[package]] +name = "zenoh-link-quic" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "async-trait", + "futures 0.3.28", + "log 0.4.17", + "quinn", + "rustls", + "rustls-native-certs", + "rustls-pemfile", + "webpki", + "zenoh-cfg-properties", + "zenoh-config", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", + "zenoh-util", +] + +[[package]] +name = "zenoh-link-tcp" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "async-trait", + "log 0.4.17", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", + "zenoh-util", +] + +[[package]] +name = "zenoh-link-tls" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-rustls", + "async-std", + "async-trait", + "futures 0.3.28", + "log 0.4.17", + "rustls-pemfile", + "webpki", + "webpki-roots", + "zenoh-cfg-properties", + "zenoh-config", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", + "zenoh-util", +] + +[[package]] +name = "zenoh-link-udp" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "async-trait", + "log 0.4.17", + "socket2", + "zenoh-buffers", + "zenoh-collections", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", + "zenoh-util", +] + +[[package]] +name = "zenoh-link-unixsock_stream" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "async-trait", + "futures 0.3.28", + "log 0.4.17", + "nix", + "uuid", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", +] + +[[package]] +name = "zenoh-link-ws" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "async-trait", + "futures-util", + "log 0.4.17", + "tokio", + "tokio-tungstenite", + "url 2.3.1", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", + "zenoh-util", +] + +[[package]] +name = "zenoh-macros" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "proc-macro2", + "quote", + "rustc_version", + "syn 1.0.109", + "unzip-n", + "zenoh-keyexpr", +] + +[[package]] +name = "zenoh-plugin-trait" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "libloading", + "log 0.4.17", + "serde_json", + "zenoh-macros", + "zenoh-result", + "zenoh-util", +] + +[[package]] +name = "zenoh-protocol" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "hex", + "rand", + "serde", + "uhlc", + "uuid", + "zenoh-buffers", + "zenoh-keyexpr", + "zenoh-result", +] + +[[package]] +name = "zenoh-result" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "anyhow", +] + +[[package]] +name = "zenoh-sync" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "event-listener", + "flume", + "futures 0.3.28", + "tokio", + "zenoh-buffers", + "zenoh-collections", + "zenoh-core", +] + +[[package]] +name = "zenoh-transport" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-executor", + "async-global-executor", + "async-std", + "async-trait", + "flume", + "log 0.4.17", + "paste", + "rand", + "ringbuffer-spsc", + "rsa", + "serde", + "zenoh-buffers", + "zenoh-cfg-properties", + "zenoh-codec", + "zenoh-collections", + "zenoh-config", + "zenoh-core", + "zenoh-crypto", + "zenoh-link", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", + "zenoh-util", +] + +[[package]] +name = "zenoh-util" +version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +dependencies = [ + "async-std", + "async-trait", + "clap", + "flume", + "futures 0.3.28", + "hex", + "home", + "humantime", + "lazy_static", + "libc", + "libloading", + "log 0.4.17", + "pnet", + "pnet_datalink", + "shellexpand", + "winapi", + "zenoh-core", + "zenoh-protocol", + "zenoh-result", +] + +[[package]] +name = "zeroize" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2a0956f1ba7c7909bfb66c2e9e4124ab6f6482560f6628b5aaeba39207c9aad9" + +[[package]] +name = "zplugin-ros1" +version = "0.7.0-rc" +dependencies = [ + "async-global-executor", + "async-std", + "duration-string", + "env_logger 0.9.3", + "flume", + "futures 0.3.28", + "git-version", + "lazy_static", + "log 0.4.17", + "multiset", + "rand", + "rosrust", + "rustc_version", + "serde", + "serde_json", + "serial_test", + "strum", + "strum_macros", + "zenoh", + "zenoh-core", + "zenoh-ext", + "zenoh-plugin-trait", + "zplugin-ros1", +] diff --git a/Cargo.toml b/Cargo.toml index 96309bd..4736607 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,14 +16,41 @@ members = [ "zplugin-ros1", ] +[workspace.package] +version = "0.7.0-rc" +authors = [ + "Dmitrii Bannov ", + "Luca Cominardi " +] +edition = "2021" +repository = "https://github.com/eclipse-zenoh/zenoh-plugin-ros1" +homepage = "http://zenoh.io" +license = " EPL-2.0 OR Apache-2.0" + +[workspace.dependencies] +env_logger = "0.9.1" +flume = "0.10.14" +futures = "0.3.24" +git-version = "0.3.5" +lazy_static = "1.4.0" +log = "0.4.17" +serde = "1.0.147" +serde_json = "1.0.85" +async-global-executor = "2.3.1" +rand = "0.8.5" +strum = "0.24" +strum_macros = "0.24" +duration-string = "0.3.0" +zenoh = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } +zenoh-ext = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } +zenoh-core = { git = "https://github.com/eclipse-zenoh/zenoh.git" } +zenoh-plugin-trait = { git = "https://github.com/eclipse-zenoh/zenoh.git", default-features = false } +rosrust = { git = "https://github.com/ZettaScaleLabs/rosrust.git", branch = "feature/fix_bugs" } +rustc_version = "0.4" + [profile.release] debug = false lto = "fat" codegen-units = 1 opt-level = 3 panic = "abort" - -#[[bench]] -#name = "benchmarks" -#harness = false -#path = "tests/benchmarks.rs" \ No newline at end of file diff --git a/LICENSE b/LICENSE index e48e096..8c3fbf6 100644 --- a/LICENSE +++ b/LICENSE @@ -1,3 +1,184 @@ +apache-2.0 +epl-2.0 + + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + OR + Eclipse Public License - v 2.0 THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF THIS ECLIPSE @@ -261,8 +442,8 @@ No third-party beneficiary rights are created under this Agreement. Exhibit A - Form of Secondary Licenses Notice -"This Source Code may also be made available under the following -Secondary Licenses when the conditions for such availability set forth +"This Source Code may also be made available under the following +Secondary Licenses when the conditions for such availability set forth in the Eclipse Public License, v. 2.0 are satisfied: {name license(s), version(s), and exceptions or additional permissions here}." @@ -275,3 +456,4 @@ version(s), and exceptions or additional permissions here}." look for such a notice. You may add additional accurate notices of copyright ownership. + diff --git a/zplugin-ros1/Cargo.toml b/zplugin-ros1/Cargo.toml index 9c063f5..fe71b0d 100644 --- a/zplugin-ros1/Cargo.toml +++ b/zplugin-ros1/Cargo.toml @@ -13,16 +13,14 @@ # [package] name = "zplugin-ros1" -version = "0.0.1" -authors = [ - "Dmitrii Bannov ", -] -edition = "2021" -repository = "https://github.com/eclipse-zenoh/zenoh-plugin-ros1" -homepage = "http://zenoh.io" -license = " EPL-2.0 OR Apache-2.0" +version = { workspace = true } +authors = { workspace = true } +edition = { workspace = true } +repository = { workspace = true } +homepage = { workspace = true } +license = { workspace = true } categories = ["network-programming"] -description = "Zenoh plugin for ROS1" +description = "Zenoh plugin for bidging ROS1" [lib] name = "zplugin_ros1" @@ -34,40 +32,25 @@ default = ["no_mangle"] test = [] [dependencies] -env_logger = "0.9.1" -flume = "0.10.14" -futures = "0.3.24" -git-version = "0.3.5" -lazy_static = "1.4.0" -log = "0.4.17" -serde = "1.0.147" -serde_json = "1.0.85" -async-global-executor = "2.3.1" -rand = "0.8.5" -strum = "0.24" -strum_macros = "0.24" -duration-string = "0.3.0" - -# zenoh = { version = "0.6.0-beta.1", features = ["unstable"] } -zenoh = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } -# zenoh = { path = "../../../zenoh/zenoh", features = ["unstable"] } - -# zenoh-ext = { version = "0.6.0-beta.1", features = ["unstable"] } -zenoh-ext = { git = "https://github.com/eclipse-zenoh/zenoh.git", features = ["unstable"] } -# zenoh-ext = { path = "../../../zenoh/zenoh-ext", features = ["unstable"] } - -# zenoh-core = { version = "0.6.0-beta.1" } -zenoh-core = { git = "https://github.com/eclipse-zenoh/zenoh.git" } -# zenoh-core = { path = "../../../zenoh/commons/zenoh-core" } - -# zenoh-plugin-trait = { version = "0.6.0-beta.1", default-features = false } -zenoh-plugin-trait = { git = "https://github.com/eclipse-zenoh/zenoh.git", default-features = false } -# zenoh-plugin-trait = { path = "../../../zenoh/plugins/zenoh-plugin-trait", default-features = false } - -# rosrust = "0.9" -rosrust = { git = "https://github.com/ZettaScaleLabs/rosrust.git", branch = "feature/fix_bugs" } -# rosrust = { path = "../../rosrust/rosrust/rosrust" } - +env_logger = { workspace = true } +flume = { workspace = true } +futures = { workspace = true } +git-version = { workspace = true } +lazy_static = { workspace = true } +log = { workspace = true } +serde = { workspace = true } +serde_json = { workspace = true } +async-global-executor = { workspace = true } +rand = { workspace = true } +strum = { workspace = true } +strum_macros = { workspace = true } +duration-string = { workspace = true } +zenoh = { workspace = true } +zenoh-ext = { workspace = true } +zenoh-core = { workspace = true } +zenoh-plugin-trait = { workspace = true } +rosrust = { workspace = true } + [dev-dependencies] serial_test = "0.10.0" multiset = "0.0.5" @@ -78,4 +61,12 @@ version = "=1.12.0" features = ["unstable", "attributes"] [build-dependencies] -rustc_version = "0.4" +rustc_version = { workspace = true } + +[package.metadata.deb] +name = "zenoh-plugin-ros1" +maintainer = "zenoh-dev@eclipse.org" +copyright = "2017, 2022 ZettaScale Technology Inc." +section = "net" +license-file = ["../LICENSE", "0"] +depends = "zenohd (=0.7.0-rc)" \ No newline at end of file From 88344b8f9104c12cbafe0584557515a5a671c958 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 18 Apr 2023 17:12:36 +0300 Subject: [PATCH 13/19] Isolate and parallelize the tests --- Cargo.lock | 6 +- zplugin-ros1/examples/ros1_pub.rs | 10 +- zplugin-ros1/examples/ros1_service.rs | 10 +- zplugin-ros1/examples/ros1_sub.rs | 10 +- zplugin-ros1/src/ros_to_zenoh_bridge.rs | 13 +- .../src/ros_to_zenoh_bridge/ros1_client.rs | 4 +- .../ros1_to_zenoh_bridge_impl.rs | 2 + .../src/ros_to_zenoh_bridge/test_helpers.rs | 61 +++++++++ zplugin-ros1/tests/aloha_declaration_test.rs | 40 +++--- zplugin-ros1/tests/discovery_test.rs | 57 +++----- zplugin-ros1/tests/ping_pong_test.rs | 125 ++++++++---------- zplugin-ros1/tests/rosmaster_test.rs | 7 +- 12 files changed, 191 insertions(+), 154 deletions(-) create mode 100644 zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs diff --git a/Cargo.lock b/Cargo.lock index 9230a86..6be6f4f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2245,7 +2245,7 @@ dependencies = [ [[package]] name = "ros_message" version = "0.1.0" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#a3fc4ceefa82a58250677b12cfe3c9df4ea239f0" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#3ff335ae909e494578d2c9b5d020c5ade58f39cf" dependencies = [ "array-init", "hex", @@ -2261,7 +2261,7 @@ dependencies = [ [[package]] name = "rosrust" version = "0.9.10" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#a3fc4ceefa82a58250677b12cfe3c9df4ea239f0" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#3ff335ae909e494578d2c9b5d020c5ade58f39cf" dependencies = [ "byteorder", "colored", @@ -2284,7 +2284,7 @@ dependencies = [ [[package]] name = "rosrust_codegen" version = "0.9.6" -source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#a3fc4ceefa82a58250677b12cfe3c9df4ea239f0" +source = "git+https://github.com/ZettaScaleLabs/rosrust.git?branch=feature/fix_bugs#3ff335ae909e494578d2c9b5d020c5ade58f39cf" dependencies = [ "error-chain 0.12.4", "hex", diff --git a/zplugin-ros1/examples/ros1_pub.rs b/zplugin-ros1/examples/ros1_pub.rs index aa408b2..de51d52 100644 --- a/zplugin-ros1/examples/ros1_pub.rs +++ b/zplugin-ros1/examples/ros1_pub.rs @@ -13,7 +13,9 @@ // use zenoh_core::AsyncResolve; -use zplugin_ros1::ros_to_zenoh_bridge::{ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge}; +use zplugin_ros1::ros_to_zenoh_bridge::{ + environment::Environment, ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge, +}; #[async_std::main] async fn main() { @@ -34,7 +36,11 @@ async fn main() { // create ROS1 node and publisher print!("Creating ROS1 Node..."); - let ros1_node = rosrust::api::Ros::new("ROS1_test_node").unwrap(); + let ros1_node = rosrust::api::Ros::new( + Environment::ros_name().get().as_str(), + Environment::ros_master_uri().get().as_str(), + ) + .unwrap(); println!(" OK!"); print!("Creating ROS1 Publisher..."); let ros1_publisher = ros1_node diff --git a/zplugin-ros1/examples/ros1_service.rs b/zplugin-ros1/examples/ros1_service.rs index 6555101..09f32ba 100644 --- a/zplugin-ros1/examples/ros1_service.rs +++ b/zplugin-ros1/examples/ros1_service.rs @@ -15,7 +15,9 @@ use zenoh::prelude::SplitBuffer; use zenoh_core::AsyncResolve; -use zplugin_ros1::ros_to_zenoh_bridge::{ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge}; +use zplugin_ros1::ros_to_zenoh_bridge::{ + environment::Environment, ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge, +}; #[async_std::main] async fn main() { @@ -36,7 +38,11 @@ async fn main() { // create ROS1 node and service print!("Creating ROS1 Node..."); - let ros1_node = rosrust::api::Ros::new("ROS1_test_node").unwrap(); + let ros1_node = rosrust::api::Ros::new( + Environment::ros_name().get().as_str(), + Environment::ros_master_uri().get().as_str(), + ) + .unwrap(); println!(" OK!"); print!("Creating ROS1 Service..."); #[allow(unused_variables)] diff --git a/zplugin-ros1/examples/ros1_sub.rs b/zplugin-ros1/examples/ros1_sub.rs index 0b5f299..deb2e27 100644 --- a/zplugin-ros1/examples/ros1_sub.rs +++ b/zplugin-ros1/examples/ros1_sub.rs @@ -14,7 +14,9 @@ use zenoh_core::AsyncResolve; -use zplugin_ros1::ros_to_zenoh_bridge::{ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge}; +use zplugin_ros1::ros_to_zenoh_bridge::{ + environment::Environment, ros1_master_ctrl::Ros1MasterCtrl, Ros1ToZenohBridge, +}; #[async_std::main] async fn main() { @@ -35,7 +37,11 @@ async fn main() { // create ROS1 node and subscriber print!("Creating ROS1 Node..."); - let ros1_node = rosrust::api::Ros::new("ROS1_test_node").unwrap(); + let ros1_node = rosrust::api::Ros::new( + Environment::ros_name().get().as_str(), + Environment::ros_master_uri().get().as_str(), + ) + .unwrap(); println!(" OK!"); print!("Creating ROS1 Subscriber..."); #[allow(unused_variables)] diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge.rs b/zplugin-ros1/src/ros_to_zenoh_bridge.rs index d38e294..0608172 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge.rs @@ -22,7 +22,7 @@ use std::sync::{ Arc, }; -use self::ros1_to_zenoh_bridge_impl::work_cycle; +use self::{environment::Environment, ros1_to_zenoh_bridge_impl::work_cycle}; #[cfg(feature = "test")] pub mod aloha_declaration; @@ -37,6 +37,8 @@ pub mod ros1_client; #[cfg(feature = "test")] pub mod ros1_to_zenoh_bridge_impl; #[cfg(feature = "test")] +pub mod test_helpers; +#[cfg(feature = "test")] pub mod topic_utilities; #[cfg(feature = "test")] pub mod zenoh_client; @@ -91,7 +93,14 @@ impl Ros1ToZenohBridge { //PRIVATE: async fn run(session: Arc, flag: Arc) { - work_cycle(session, flag, |_v| {}, |_status| {}).await; + work_cycle( + Environment::ros_master_uri().get().as_str(), + session, + flag, + |_v| {}, + |_status| {}, + ) + .await; } async fn async_await(&mut self) { diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs index 1b0d9fe..429e275 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_client.rs @@ -21,9 +21,9 @@ pub struct Ros1Client { impl Ros1Client { // PUBLIC - pub fn new(name: &str) -> Ros1Client { + pub fn new(name: &str, master_uri: &str) -> Ros1Client { Ros1Client { - ros: rosrust::api::Ros::new(name).unwrap(), + ros: rosrust::api::Ros::new(name, master_uri).unwrap(), } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs index 1e5b288..65bbf7d 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/ros1_to_zenoh_bridge_impl.rs @@ -53,6 +53,7 @@ pub struct BridgeStatus { } pub async fn work_cycle( + ros_master_uri: &str, session: Arc, flag: Arc, ros_status_callback: RosStatusCallback, @@ -63,6 +64,7 @@ pub async fn work_cycle( { let ros1_client = Arc::new(ros1_client::Ros1Client::new( Environment::ros_name().get().as_str(), + ros_master_uri, )); let zenoh_client = Arc::new(zenoh_client::ZenohClient::new(session.clone())); diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs new file mode 100644 index 0000000..fbdf395 --- /dev/null +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs @@ -0,0 +1,61 @@ +// +// Copyright (c) 2022 ZettaScale Technology +// +// This program and the accompanying materials are made available under the +// terms of the Eclipse Public License 2.0 which is available at +// http://www.eclipse.org/legal/epl-2.0, or the Apache License, Version 2.0 +// which is available at https://www.apache.org/licenses/LICENSE-2.0. +// +// SPDX-License-Identifier: EPL-2.0 OR Apache-2.0 +// +// Contributors: +// ZettaScale Zenoh Team, +// + +use std::sync::atomic::Ordering::SeqCst; +use std::{net::SocketAddr, str::FromStr, sync::atomic::AtomicU16}; +use zenoh::config::ModeDependentValue; + +pub struct IsolatedPort { + pub port: u16, +} +impl Default for IsolatedPort { + fn default() -> Self { + static TEST_PORT: AtomicU16 = AtomicU16::new(17000); + Self { + port: TEST_PORT.fetch_add(1, SeqCst), + } + } +} + +#[derive(Default)] +pub struct IsolatedConfig { + port: IsolatedPort, +} +impl IsolatedConfig { + pub fn peer(&self) -> zenoh::config::Config { + let mut config = zenoh::config::peer(); + config + .scouting + .multicast + .set_address(Some( + SocketAddr::from_str(format!("224.0.0.224:{}", self.port.port).as_str()).unwrap(), + )) + .unwrap(); + config + .timestamping + .set_enabled(Some(ModeDependentValue::Unique(true))) + .unwrap(); + config + } +} + +#[derive(Default)] +pub struct IsolatedROSMaster { + pub port: IsolatedPort, +} +impl IsolatedROSMaster { + pub fn master_uri(&self) -> String { + format!("http://localhost:{}/", self.port.port) + } +} diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs index 96ffad9..93e2b03 100644 --- a/zplugin-ros1/tests/aloha_declaration_test.rs +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -14,28 +14,22 @@ use std::{ collections::HashSet, - net::SocketAddr, str::FromStr, sync::{atomic::AtomicUsize, Arc, Mutex}, time::Duration, }; use async_std::prelude::FutureExt; -use serial_test::serial; use zenoh::{plugins::ZResult, prelude::OwnedKeyExpr, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; -use zplugin_ros1::ros_to_zenoh_bridge::{aloha_declaration, aloha_subscription}; +use zplugin_ros1::ros_to_zenoh_bridge::{ + aloha_declaration, aloha_subscription, test_helpers::IsolatedConfig, +}; const TIMEOUT: Duration = Duration::from_secs(30); -fn session_builder() -> OpenBuilder { - let mut config = zenoh::config::peer(); - config - .scouting - .multicast - .set_address(Some(SocketAddr::from_str("224.0.0.224:16000").unwrap())) - .unwrap(); - zenoh::open(config) +fn session_builder(cfg: &IsolatedConfig) -> OpenBuilder { + zenoh::open(cfg.peer()) } fn declaration_builder( @@ -60,8 +54,8 @@ fn subscription_builder( ) } -fn make_session() -> Arc { - session_builder().res_sync().unwrap().into_arc() +fn make_session(cfg: &IsolatedConfig) -> Arc { + session_builder(cfg).res_sync().unwrap().into_arc() } fn make_subscription( @@ -72,21 +66,20 @@ fn make_subscription( } #[test] -#[serial(Aloha)] fn aloha_instantination_one_instance() { - let session = make_session(); + let session = make_session(&IsolatedConfig::default()); let _declaration = declaration_builder(session.clone(), Duration::from_secs(1)); let _subscription = make_subscription(session, Duration::from_secs(1)); } #[test] -#[serial(Aloha)] fn aloha_instantination_many_instances() { + let cfg = IsolatedConfig::default(); let mut sessions = Vec::new(); let mut declarations = Vec::new(); let mut subscriptions = Vec::new(); for _ in 0..10 { - let session = make_session(); + let session = make_session(&cfg); sessions.push(session.clone()); declarations.push(declaration_builder(session.clone(), Duration::from_secs(1))); } @@ -210,6 +203,7 @@ impl State { } async fn test_state_transition<'a>( + cfg: &IsolatedConfig, beacon_period: Duration, declaring_sessions: &mut Vec>, declarations: &mut Vec, @@ -244,7 +238,7 @@ async fn test_state_transition<'a>( while declarations.len() < state.declarators_count { if declaring_sessions.len() <= declarations.len() { - declaring_sessions.push(session_builder().res_async().await.unwrap().into_arc()); + declaring_sessions.push(session_builder(cfg).res_async().await.unwrap().into_arc()); } declarations.push(declaration_builder( declaring_sessions[declarations.len()].clone(), @@ -264,11 +258,12 @@ async fn test_state_transition<'a>( } async fn run_aloha(beacon_period: Duration, scenario: Vec) { + let cfg = IsolatedConfig::default(); let mut declaring_sessions: Vec> = Vec::new(); let mut declarations: Vec = Vec::new(); let mut collector = DeclarationCollector::new(); - let subscription_session = session_builder().res_async().await.unwrap().into_arc(); + let subscription_session = session_builder(&cfg).res_async().await.unwrap().into_arc(); let _subscriber = collector .use_builder(subscription_builder( subscription_session.clone(), @@ -287,6 +282,7 @@ async fn run_aloha(beacon_period: Duration, scenario: Vec) { for scene in scenario { println!("Transiting State: {}", scene.declarators_count); test_state_transition( + &cfg, beacon_period, &mut declaring_sessions, &mut declarations, @@ -301,7 +297,6 @@ async fn run_aloha(beacon_period: Duration, scenario: Vec) { } #[test] -#[serial(Aloha)] fn aloha_declare_one() { async_std::task::block_on(run_aloha( Duration::from_millis(100), @@ -310,7 +305,6 @@ fn aloha_declare_one() { } #[test] -#[serial(Aloha)] fn aloha_declare_many() { async_std::task::block_on(run_aloha( Duration::from_millis(100), @@ -319,7 +313,6 @@ fn aloha_declare_many() { } #[test] -#[serial(Aloha)] fn aloha_declare_many_one_many() { async_std::task::block_on(run_aloha( Duration::from_millis(100), @@ -334,7 +327,6 @@ fn aloha_declare_many_one_many() { } #[test] -#[serial(Aloha)] fn aloha_declare_one_zero_one() { async_std::task::block_on(run_aloha( Duration::from_millis(100), @@ -349,7 +341,6 @@ fn aloha_declare_one_zero_one() { } #[test] -#[serial(Aloha)] fn aloha_declare_many_zero_many() { async_std::task::block_on(run_aloha( Duration::from_millis(100), @@ -364,7 +355,6 @@ fn aloha_declare_many_zero_many() { } #[test] -#[serial(Aloha)] fn aloha_many_scenarios() { async_std::task::block_on(run_aloha( Duration::from_millis(100), diff --git a/zplugin-ros1/tests/discovery_test.rs b/zplugin-ros1/tests/discovery_test.rs index f645dc4..bd24761 100644 --- a/zplugin-ros1/tests/discovery_test.rs +++ b/zplugin-ros1/tests/discovery_test.rs @@ -13,41 +13,30 @@ // use std::{ - net::SocketAddr, - str::FromStr, sync::{Arc, Mutex}, time::Duration, }; use async_std::prelude::FutureExt; use multiset::HashMultiSet; -use serial_test::serial; -use zenoh::{config::ModeDependentValue, prelude::keyexpr, OpenBuilder, Session}; +use zenoh::{prelude::keyexpr, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; -use zplugin_ros1::ros_to_zenoh_bridge::{discovery, topic_utilities::make_topic}; +use zplugin_ros1::ros_to_zenoh_bridge::{ + discovery, test_helpers::IsolatedConfig, topic_utilities::make_topic, +}; const TIMEOUT: Duration = Duration::from_secs(10); -fn session_builder() -> OpenBuilder { - let mut config = zenoh::config::peer(); - config - .scouting - .multicast - .set_address(Some(SocketAddr::from_str("224.0.0.224:16001").unwrap())) - .unwrap(); - config - .timestamping - .set_enabled(Some(ModeDependentValue::Unique(true))) - .unwrap(); - zenoh::open(config) +fn session_builder(cfg: &IsolatedConfig) -> OpenBuilder { + zenoh::open(cfg.peer()) } fn discovery_builder(session: Arc) -> discovery::DiscoveryBuilder { discovery::DiscoveryBuilder::new("*".to_string(), "*".to_string(), session) } -fn make_session() -> Arc { - session_builder().res_sync().unwrap().into_arc() +fn make_session(cfg: &IsolatedConfig) -> Arc { + session_builder(cfg).res_sync().unwrap().into_arc() } fn make_discovery(session: Arc) -> discovery::Discovery { @@ -55,18 +44,17 @@ fn make_discovery(session: Arc) -> discovery::Discovery { } #[test] -#[serial(ROS1)] fn discovery_instantination_one_instance() { - let session = make_session(); + let session = make_session(&IsolatedConfig::default()); let _discovery = make_discovery(session); } #[test] -#[serial(ROS1)] fn discovery_instantination_many_instances() { + let cfg = IsolatedConfig::default(); let mut sessions = Vec::new(); for _i in 0..10 { - sessions.push(make_session()); + sessions.push(make_session(&cfg)); } let mut discoveries = Vec::new(); @@ -207,10 +195,7 @@ impl State { self.clients_duplication = clients_duplication; self } - //pub fn stage(mut self, stage: usize) -> Self { - // self.stage = stage; - // self - //} + async fn summarize( &self, ) -> ( @@ -287,11 +272,13 @@ async fn test_state_transition( } async fn run_discovery(scenario: Vec) { - let src_session = session_builder().res_async().await.unwrap().into_arc(); + let cfg = IsolatedConfig::default(); + + let src_session = session_builder(&cfg).res_async().await.unwrap().into_arc(); let src_discovery = discovery_builder(src_session).build().await; let rcv = DiscoveryCollector::new(); - let rcv_session = session_builder().res_async().await.unwrap().into_arc(); + let rcv_session = session_builder(&cfg).res_async().await.unwrap().into_arc(); let _rcv_discovery = rcv .use_builder(discovery_builder(rcv_session)) .build() @@ -306,35 +293,30 @@ async fn run_discovery(scenario: Vec) { } #[test] -#[serial(ROS1)] fn discover_single_publisher() { async_std::task::block_on(run_discovery( [State::default().publishers(1, 1)].into_iter().collect(), )); } #[test] -#[serial(ROS1)] fn discover_single_subscriber() { async_std::task::block_on(run_discovery( [State::default().subscribers(1, 1)].into_iter().collect(), )); } #[test] -#[serial(ROS1)] fn discover_single_service() { async_std::task::block_on(run_discovery( [State::default().services(1, 1)].into_iter().collect(), )); } #[test] -#[serial(ROS1)] fn discover_single_client() { async_std::task::block_on(run_discovery( [State::default().clients(1, 1)].into_iter().collect(), )); } #[test] -#[serial(ROS1)] fn discover_single_transition() { async_std::task::block_on(run_discovery( [ @@ -348,7 +330,6 @@ fn discover_single_transition() { )); } #[test] -#[serial(ROS1)] fn discover_single_transition_with_zero_state() { async_std::task::block_on(run_discovery( [ @@ -366,35 +347,30 @@ fn discover_single_transition_with_zero_state() { } #[test] -#[serial(ROS1)] fn discover_multiple_publishers() { async_std::task::block_on(run_discovery( [State::default().publishers(100, 1)].into_iter().collect(), )); } #[test] -#[serial(ROS1)] fn discover_multiple_subscribers() { async_std::task::block_on(run_discovery( [State::default().subscribers(100, 1)].into_iter().collect(), )); } #[test] -#[serial(ROS1)] fn discover_multiple_services() { async_std::task::block_on(run_discovery( [State::default().services(100, 1)].into_iter().collect(), )); } #[test] -#[serial(ROS1)] fn discover_multiple_clients() { async_std::task::block_on(run_discovery( [State::default().clients(100, 1)].into_iter().collect(), )); } #[test] -#[serial(ROS1)] fn discover_multiple_transition() { async_std::task::block_on(run_discovery( [ @@ -408,7 +384,6 @@ fn discover_multiple_transition() { )); } #[test] -#[serial(ROS1)] fn discover_multiple_transition_with_zero_state() { async_std::task::block_on(run_discovery( [ diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 0eb28e1..19f0d40 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -13,23 +13,25 @@ // use strum_macros::Display; -use zenoh::{config::ModeDependentValue, prelude::SplitBuffer}; +use zenoh::prelude::SplitBuffer; use zenoh_core::{bail, zresult::ZResult, SyncResolve}; use std::{ collections::HashSet, - net::SocketAddr, str::FromStr, sync::{ - atomic::{AtomicBool, AtomicU16, AtomicU64, Ordering::*}, + atomic::{AtomicBool, AtomicU64, Ordering::*}, Arc, Mutex, RwLock, }, }; -use zplugin_ros1::ros_to_zenoh_bridge::discovery::{LocalResource, LocalResources}; use zplugin_ros1::ros_to_zenoh_bridge::ros1_to_zenoh_bridge_impl::{ work_cycle, BridgeStatus, RosStatus, }; +use zplugin_ros1::ros_to_zenoh_bridge::{ + discovery::{LocalResource, LocalResources}, + test_helpers::{IsolatedConfig, IsolatedROSMaster}, +}; use zplugin_ros1::ros_to_zenoh_bridge::{ros1_client, zenoh_client}; use log::{debug, error}; @@ -40,8 +42,6 @@ use zenoh::prelude::r#async::*; use std::process::Command; use std::{thread, time}; -use serial_test::serial; - async fn wait(waiter: Waiter, timeout: core::time::Duration) -> bool where Waiter: Fn() -> bool, @@ -69,13 +69,14 @@ struct RunningBridge { bridge_status: Arc>, } impl RunningBridge { - pub fn new(config: zenoh::config::Config) -> RunningBridge { + pub fn new(config: zenoh::config::Config, ros_master_uri: String) -> RunningBridge { let result = RunningBridge { flag: Arc::new(AtomicBool::new(true)), ros_status: Arc::new(Mutex::new(RosStatus::Unknown)), bridge_status: Arc::new(Mutex::new(BridgeStatus::default())), }; async_std::task::spawn(Self::run( + ros_master_uri, config, result.flag.clone(), result.ros_status.clone(), @@ -85,6 +86,7 @@ impl RunningBridge { } async fn run( + ros_master_uri: String, config: zenoh::config::Config, flag: Arc, ros_status: Arc>, @@ -92,6 +94,7 @@ impl RunningBridge { ) { let session = zenoh::open(config).res_async().await.unwrap().into_arc(); work_cycle( + ros_master_uri.as_str(), session, flag, move |v| { @@ -160,19 +163,19 @@ impl Drop for RunningBridge { } #[test] -#[serial(ROS1)] fn env_checks_no_master_init_and_exit_immed() { - let _ros_env = ROSEnvironment::new(); - let bridge = RunningBridge::new(zenoh::config::peer()); + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port); + let bridge = RunningBridge::new(IsolatedConfig::default().peer(), roscfg.master_uri()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); } #[test] -#[serial(ROS1)] fn env_checks_no_master_init_and_wait() { - let _ros_env = ROSEnvironment::new(); - let bridge = RunningBridge::new(zenoh::config::peer()); + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port); + let bridge = RunningBridge::new(IsolatedConfig::default().peer(), roscfg.master_uri()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -181,19 +184,20 @@ fn env_checks_no_master_init_and_wait() { } #[test] -#[serial(ROS1)] fn env_checks_with_master_init_and_exit_immed() { - let _ros_env = ROSEnvironment::new().with_master(); - let bridge = RunningBridge::new(zenoh::config::peer()); + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + let bridge = RunningBridge::new(IsolatedConfig::default().peer(), roscfg.master_uri()); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); } #[test] -#[serial(ROS1)] fn env_checks_with_master_init_and_wait() { - let _ros_env = ROSEnvironment::new().with_master(); - let bridge = RunningBridge::new(zenoh::config::peer()); + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + let bridge = RunningBridge::new(IsolatedConfig::default().peer(), roscfg.master_uri()); + async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -202,10 +206,10 @@ fn env_checks_with_master_init_and_wait() { } #[test] -#[serial(ROS1)] fn env_checks_with_master_init_and_loose_master() { - let mut _ros_env = Some(ROSEnvironment::new().with_master()); - let bridge = RunningBridge::new(zenoh::config::peer()); + let roscfg = IsolatedROSMaster::default(); + let mut _ros_env = Some(ROSEnvironment::new(roscfg.port.port).with_master()); + let bridge = RunningBridge::new(IsolatedConfig::default().peer(), roscfg.master_uri()); async_std::task::block_on(bridge.assert_ros_ok()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -220,10 +224,10 @@ fn env_checks_with_master_init_and_loose_master() { } #[test] -#[serial(ROS1)] fn env_checks_with_master_init_and_wait_for_master() { - let mut _ros_env = ROSEnvironment::new(); - let bridge = RunningBridge::new(zenoh::config::peer()); + let roscfg = IsolatedROSMaster::default(); + let mut _ros_env = ROSEnvironment::new(roscfg.port.port); + let bridge = RunningBridge::new(IsolatedConfig::default().peer(), roscfg.master_uri()); async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); thread::sleep(time::Duration::from_secs(1)); @@ -238,10 +242,10 @@ fn env_checks_with_master_init_and_wait_for_master() { } #[test] -#[serial(ROS1)] fn env_checks_with_master_init_and_reconnect_many_times_to_master() { - let mut ros_env = ROSEnvironment::new(); - let bridge = RunningBridge::new(zenoh::config::peer()); + let roscfg = IsolatedROSMaster::default(); + let mut ros_env = ROSEnvironment::new(roscfg.port.port); + let bridge = RunningBridge::new(IsolatedConfig::default().peer(), roscfg.master_uri()); for _i in 0..20 { async_std::task::block_on(bridge.assert_ros_error()); async_std::task::block_on(bridge.assert_bridge_empy()); @@ -687,6 +691,7 @@ impl PingPong { } struct ROSEnvironment { + ros_master_port: u16, rosmaster: Option, } impl Drop for ROSEnvironment { @@ -697,11 +702,11 @@ impl Drop for ROSEnvironment { } } impl ROSEnvironment { - pub fn new() -> Self { - let mut kill_master = Command::new("killall").arg("rosmaster").spawn().unwrap(); - kill_master.wait().unwrap(); - - ROSEnvironment { rosmaster: None } + pub fn new(ros_master_port: u16) -> Self { + ROSEnvironment { + rosmaster: None, + ros_master_port, + } } pub fn with_master(mut self) -> Self { @@ -709,6 +714,7 @@ impl ROSEnvironment { self.rosmaster = Some( Command::new("rosmaster") + .arg(format!("-p {}", self.ros_master_port).as_str()) .stdout(std::process::Stdio::piped()) .stderr(std::process::Stdio::piped()) .spawn() @@ -730,7 +736,6 @@ impl ROSEnvironment { } } -static TEST_PORT: AtomicU16 = AtomicU16::new(17000); struct TestEnvironment { pub bridge: RunningBridge, pub checker: Arc, @@ -738,16 +743,17 @@ struct TestEnvironment { } impl TestEnvironment { pub fn new() -> TestEnvironment { - let port = TEST_PORT.fetch_add(1, SeqCst); + let cfg = IsolatedConfig::default(); + let roscfg = IsolatedROSMaster::default(); // start environment for ROS - let ros_env = ROSEnvironment::new().with_master(); + let ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); // start bridge - let bridge = RunningBridge::new(Self::config(port)); + let bridge = RunningBridge::new(cfg.peer(), roscfg.master_uri()); // start checker's engine - let checker = Arc::new(BridgeChecker::new(Self::config(port))); + let checker = Arc::new(BridgeChecker::new(cfg.peer(), roscfg.master_uri().as_str())); // this will wait for the bridge to have some expected initial state and serves two purposes: // - asserts on the expected state @@ -796,22 +802,6 @@ impl TestEnvironment { } default } - - fn config(port: u16) -> zenoh::config::Config { - let mut config = zenoh::config::peer(); - config - .scouting - .multicast - .set_address(Some( - SocketAddr::from_str(format!("224.0.0.224:{}", port).as_str()).unwrap(), - )) - .unwrap(); - config - .timestamping - .set_enabled(Some(ModeDependentValue::Unique(true))) - .unwrap(); - config - } } struct RAIICounter @@ -855,10 +845,10 @@ struct BridgeChecker { } impl BridgeChecker { // PUBLIC - pub fn new(config: zenoh::config::Config) -> BridgeChecker { + pub fn new(config: zenoh::config::Config, ros_master_uri: &str) -> BridgeChecker { let session = zenoh::open(config).res_sync().unwrap().into_arc(); BridgeChecker { - ros_client: ros1_client::Ros1Client::new("test_ros_node"), + ros_client: ros1_client::Ros1Client::new("test_ros_node", ros_master_uri), zenoh_client: zenoh_client::ZenohClient::new(session.clone()), local_resources: LocalResources::new("*".to_string(), "*".to_string(), session), expected_bridge_status: Arc::new(RwLock::new(BridgeStatus::default())), @@ -1078,7 +1068,6 @@ async fn ping_pong_duplex_parallel_many_( } #[test] -#[serial(ROS1)] fn ping_pong_zenoh_to_ros1() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1088,7 +1077,6 @@ fn ping_pong_zenoh_to_ros1() { )); } #[test] -#[serial(ROS1)] fn ping_pong_zenoh_to_ros1_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1099,7 +1087,6 @@ fn ping_pong_zenoh_to_ros1_many() { } #[test] -#[serial(ROS1)] fn ping_pong_ros1_to_zenoh() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1109,7 +1096,6 @@ fn ping_pong_ros1_to_zenoh() { )); } #[test] -#[serial(ROS1)] fn ping_pong_ros1_to_zenoh_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1120,7 +1106,6 @@ fn ping_pong_ros1_to_zenoh_many() { } #[test] -#[serial(ROS1)] fn ping_pong_ros1_service() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1130,7 +1115,6 @@ fn ping_pong_ros1_service() { )); } #[test] -#[serial(ROS1)] fn ping_pong_ros1_service_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1141,7 +1125,6 @@ fn ping_pong_ros1_service_many() { } #[test] -#[serial(ROS1)] fn ping_pong_ros1_client() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1151,7 +1134,6 @@ fn ping_pong_ros1_client() { )); } #[test] -#[serial(ROS1)] fn ping_pong_ros1_client_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1162,7 +1144,6 @@ fn ping_pong_ros1_client_many() { } #[test] -#[serial(ROS1)] fn ping_pong_all_sequential() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1187,7 +1168,6 @@ fn ping_pong_all_sequential() { )); } #[test] -#[serial(ROS1)] fn ping_pong_all_sequential_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1213,7 +1193,6 @@ fn ping_pong_all_sequential_many() { } #[test] -#[serial(ROS1)] fn ping_pong_all_parallel() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1229,7 +1208,6 @@ fn ping_pong_all_parallel() { } #[test] -#[serial(ROS1)] fn ping_pong_all_parallel_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( @@ -1287,7 +1265,6 @@ async fn parallel_subworks( futures::future::join_all(subworks).await; } #[test] -#[serial(ROS1)] fn ping_pong_all_overlap_one() { let env = TestEnvironment::new(); let main_work_finished = Arc::new(AtomicBool::new(false)); @@ -1297,7 +1274,6 @@ fn ping_pong_all_overlap_one() { async_std::task::block_on(futures::future::join(main_work, parallel_subworks)); } #[test] -#[serial(ROS1)] fn ping_pong_all_overlap_many() { let env = TestEnvironment::new(); let main_work_finished = Arc::new(AtomicBool::new(false)); @@ -1326,9 +1302,12 @@ async fn check_query(checker: &BridgeChecker) { } #[test] -#[serial(ROS1)] fn check_query_() { - let _ros_env = ROSEnvironment::new().with_master(); - let checker = BridgeChecker::new(zenoh::config::peer()); + let roscfg = IsolatedROSMaster::default(); + let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); + let checker = BridgeChecker::new( + IsolatedConfig::default().peer(), + roscfg.master_uri().as_str(), + ); futures::executor::block_on(check_query(&checker)); } diff --git a/zplugin-ros1/tests/rosmaster_test.rs b/zplugin-ros1/tests/rosmaster_test.rs index 4a378a1..8f0ab8c 100644 --- a/zplugin-ros1/tests/rosmaster_test.rs +++ b/zplugin-ros1/tests/rosmaster_test.rs @@ -40,8 +40,11 @@ fn start_and_stop_master_and_check_connectivity() { }); // start ros1 client - let ros1_client = - rosrust::api::Ros::new("start_and_stop_master_and_check_connectivity_client").unwrap(); + let ros1_client = rosrust::api::Ros::new( + "start_and_stop_master_and_check_connectivity_client", + "http://localhost:11311/", + ) + .unwrap(); // wait for correct status from rosmaster.... let mut has_rosmaster = false; From 34e44d118476e0a77a301e084f2b6bc7d5a7fc6b Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Tue, 18 Apr 2023 17:57:18 +0300 Subject: [PATCH 14/19] Fixed rosmaster process management in tests --- zplugin-ros1/tests/ping_pong_test.rs | 32 ++++++++++++++++------------ 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 19f0d40..e92b8f4 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -712,26 +712,30 @@ impl ROSEnvironment { pub fn with_master(mut self) -> Self { assert!(self.rosmaster.is_none()); - self.rosmaster = Some( - Command::new("rosmaster") - .arg(format!("-p {}", self.ros_master_port).as_str()) - .stdout(std::process::Stdio::piped()) - .stderr(std::process::Stdio::piped()) - .spawn() - .unwrap(), - ); + let rosmaster_cmd = Command::new("rosmaster") + .arg(format!("-p {}", self.ros_master_port).as_str()) + .stdout(std::process::Stdio::piped()) + .stderr(std::process::Stdio::piped()) + .spawn(); + + match rosmaster_cmd { + Ok(val) => { + self.rosmaster = Some(val); + } + Err(e) => { + println!("Error while starting rosmaster: {}", e); + panic!("{}", e); + } + } self } pub fn without_master(mut self) -> Self { assert!(self.rosmaster.is_some()); - - if self.rosmaster.is_some() { - self.rosmaster.take().unwrap().kill().unwrap(); - self.rosmaster = None; - } - + let mut child = self.rosmaster.take().unwrap(); + child.kill().unwrap(); + child.wait().unwrap(); self } } From a0075fd6a18d0171c9ce2091041bc93a6defe582 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Wed, 19 Apr 2023 11:57:23 +0300 Subject: [PATCH 15/19] Update Cargo.lock --- Cargo.lock | 58 +++++++++++++++++++++++++++--------------------------- 1 file changed, 29 insertions(+), 29 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 6be6f4f..3dda91f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1476,9 +1476,9 @@ checksum = "0717cef1bc8b636c6e1c1bbdefc09e6322da8a9321966e8928ef80d20f7f770f" [[package]] name = "linux-raw-sys" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d59d8c75012853d2e872fb56bc8a2e53718e2cafe1a4c823143141c6d90c322f" +checksum = "3f508063cc7bb32987c71511216bd5a32be15bccb6a80b52df8b9d7f01fc3aa2" [[package]] name = "lock_api" @@ -2365,9 +2365,9 @@ dependencies = [ [[package]] name = "rustix" -version = "0.37.11" +version = "0.37.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "85597d61f83914ddeba6a47b3b8ffe7365107221c2e557ed94426489fefb5f77" +checksum = "722529a737f5a942fdbac3a46cee213053196737c5eaa3386d52e85b786f2659" dependencies = [ "bitflags 1.3.2", "errno", @@ -3563,7 +3563,7 @@ dependencies = [ [[package]] name = "zenoh" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-global-executor", "async-std", @@ -3610,7 +3610,7 @@ dependencies = [ [[package]] name = "zenoh-buffers" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "zenoh-collections", ] @@ -3618,7 +3618,7 @@ dependencies = [ [[package]] name = "zenoh-cfg-properties" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "zenoh-result", ] @@ -3626,7 +3626,7 @@ dependencies = [ [[package]] name = "zenoh-codec" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "uhlc", "zenoh-buffers", @@ -3636,12 +3636,12 @@ dependencies = [ [[package]] name = "zenoh-collections" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" [[package]] name = "zenoh-config" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "flume", "json5", @@ -3660,7 +3660,7 @@ dependencies = [ [[package]] name = "zenoh-core" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "lazy_static", @@ -3670,7 +3670,7 @@ dependencies = [ [[package]] name = "zenoh-crypto" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "aes", "hmac", @@ -3683,7 +3683,7 @@ dependencies = [ [[package]] name = "zenoh-ext" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "bincode", @@ -3703,7 +3703,7 @@ dependencies = [ [[package]] name = "zenoh-keyexpr" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "hashbrown 0.13.2", "keyed-set", @@ -3716,7 +3716,7 @@ dependencies = [ [[package]] name = "zenoh-link" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3736,7 +3736,7 @@ dependencies = [ [[package]] name = "zenoh-link-commons" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3753,7 +3753,7 @@ dependencies = [ [[package]] name = "zenoh-link-quic" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3777,7 +3777,7 @@ dependencies = [ [[package]] name = "zenoh-link-tcp" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3793,7 +3793,7 @@ dependencies = [ [[package]] name = "zenoh-link-tls" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-rustls", "async-std", @@ -3816,7 +3816,7 @@ dependencies = [ [[package]] name = "zenoh-link-udp" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3835,7 +3835,7 @@ dependencies = [ [[package]] name = "zenoh-link-unixsock_stream" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3853,7 +3853,7 @@ dependencies = [ [[package]] name = "zenoh-link-ws" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3873,7 +3873,7 @@ dependencies = [ [[package]] name = "zenoh-macros" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "proc-macro2", "quote", @@ -3886,7 +3886,7 @@ dependencies = [ [[package]] name = "zenoh-plugin-trait" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "libloading", "log 0.4.17", @@ -3899,7 +3899,7 @@ dependencies = [ [[package]] name = "zenoh-protocol" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "hex", "rand", @@ -3914,7 +3914,7 @@ dependencies = [ [[package]] name = "zenoh-result" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "anyhow", ] @@ -3922,7 +3922,7 @@ dependencies = [ [[package]] name = "zenoh-sync" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "event-listener", @@ -3937,7 +3937,7 @@ dependencies = [ [[package]] name = "zenoh-transport" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-executor", "async-global-executor", @@ -3967,7 +3967,7 @@ dependencies = [ [[package]] name = "zenoh-util" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#966f459c8d893fe56ca60268c16d67a37b25e94e" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", From d588c14d0d71c3c549fb940333df6550bdef211a Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Fri, 21 Apr 2023 13:01:02 +0300 Subject: [PATCH 16/19] Some improvements to tests --- .cargo/config.toml | 2 + Cargo.lock | 102 ++++++++---------- .../ros_to_zenoh_bridge/aloha_subscription.rs | 61 +++++------ .../src/ros_to_zenoh_bridge/test_helpers.rs | 2 +- zplugin-ros1/tests/aloha_declaration_test.rs | 27 ++--- zplugin-ros1/tests/ping_pong_test.rs | 65 ++++------- 6 files changed, 109 insertions(+), 150 deletions(-) create mode 100644 .cargo/config.toml diff --git a/.cargo/config.toml b/.cargo/config.toml new file mode 100644 index 0000000..f3a55b7 --- /dev/null +++ b/.cargo/config.toml @@ -0,0 +1,2 @@ +[env] +RUST_BACKTRACE = { value = "full", force = true } diff --git a/Cargo.lock b/Cargo.lock index 3dda91f..bec7f56 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -172,7 +172,7 @@ dependencies = [ "polling", "rustix", "slab", - "socket2", + "socket2 0.4.9", "waker-fn", ] @@ -1341,9 +1341,9 @@ dependencies = [ [[package]] name = "ipnetwork" -version = "0.19.0" +version = "0.20.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1f84f1612606f3753f205a4e9a2efd6fe5b4c573a6269b2cc6c3003d44a0d127" +checksum = "bf466541e9d546596ee94f9f69590f89473455f88372423e0008fc1a7daf100e" dependencies = [ "serde", ] @@ -1476,9 +1476,9 @@ checksum = "0717cef1bc8b636c6e1c1bbdefc09e6322da8a9321966e8928ef80d20f7f770f" [[package]] name = "linux-raw-sys" -version = "0.3.2" +version = "0.3.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3f508063cc7bb32987c71511216bd5a32be15bccb6a80b52df8b9d7f01fc3aa2" +checksum = "9b085a4f2cde5781fc4b1717f2e86c62f5cda49de7ba99a7c2eae02b61c9064c" [[package]] name = "lock_api" @@ -1930,30 +1930,32 @@ dependencies = [ [[package]] name = "pnet" -version = "0.31.0" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0caaf5b11fd907ff15cf14a4477bfabca4b37ab9e447a4f8dead969a59cdafad" +checksum = "cd959a8268165518e2bf5546ba84c7b3222744435616381df3c456fe8d983576" dependencies = [ + "ipnetwork", "pnet_base", "pnet_datalink", "pnet_packet", + "pnet_sys", "pnet_transport", ] [[package]] name = "pnet_base" -version = "0.31.0" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f9d3a993d49e5fd5d4d854d6999d4addca1f72d86c65adf224a36757161c02b6" +checksum = "872e46346144ebf35219ccaa64b1dffacd9c6f188cd7d012bd6977a2a838f42e" dependencies = [ "no-std-net", ] [[package]] name = "pnet_datalink" -version = "0.31.0" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e466faf03a98ad27f6e15cd27a2b7cc89e73e640a43527742977bc503c37f8aa" +checksum = "c302da22118d2793c312a35fb3da6846cb0fab6c3ad53fd67e37809b06cdafce" dependencies = [ "ipnetwork", "libc", @@ -1964,9 +1966,9 @@ dependencies = [ [[package]] name = "pnet_macros" -version = "0.31.0" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "48dd52a5211fac27e7acb14cfc9f30ae16ae0e956b7b779c8214c74559cef4c3" +checksum = "2a780e80005c2e463ec25a6e9f928630049a10b43945fea83207207d4a7606f4" dependencies = [ "proc-macro2", "quote", @@ -1976,18 +1978,18 @@ dependencies = [ [[package]] name = "pnet_macros_support" -version = "0.31.0" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "89de095dc7739349559913aed1ef6a11e73ceade4897dadc77c5e09de6740750" +checksum = "e6d932134f32efd7834eb8b16d42418dac87086347d1bc7d142370ef078582bc" dependencies = [ "pnet_base", ] [[package]] name = "pnet_packet" -version = "0.31.0" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bc3b5111e697c39c8b9795b9fdccbc301ab696699e88b9ea5a4e4628978f495f" +checksum = "8bde678bbd85cb1c2d99dc9fc596e57f03aa725f84f3168b0eaf33eeccb41706" dependencies = [ "glob", "pnet_base", @@ -1997,9 +1999,9 @@ dependencies = [ [[package]] name = "pnet_sys" -version = "0.31.0" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "328e231f0add6d247d82421bf3790b4b33b39c8930637f428eef24c4c6a90805" +checksum = "faf7a58b2803d818a374be9278a1fe8f88fce14b936afbe225000cfcd9c73f16" dependencies = [ "libc", "winapi", @@ -2007,9 +2009,9 @@ dependencies = [ [[package]] name = "pnet_transport" -version = "0.31.0" +version = "0.33.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ff597185e6f1f5671b3122e4dba892a1c73e17c17e723d7669bd9299cbe7f124" +checksum = "813d1c0e4defbe7ee22f6fe1755f122b77bfb5abe77145b1b5baaf463cab9249" dependencies = [ "libc", "pnet_base", @@ -2105,7 +2107,7 @@ checksum = "641538578b21f5e5c8ea733b736895576d0fe329bb883b937db6f4d163dbaaf4" dependencies = [ "libc", "quinn-proto", - "socket2", + "socket2 0.4.9", "tracing", "windows-sys 0.42.0", ] @@ -2276,7 +2278,7 @@ dependencies = [ "rosrust_codegen", "serde", "serde_derive", - "socket2", + "socket2 0.4.9", "xml-rpc", "yaml-rust", ] @@ -2323,9 +2325,9 @@ dependencies = [ [[package]] name = "rsa" -version = "0.7.2" +version = "0.8.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "094052d5470cbcef561cb848a7209968c9f12dfa6d668f4bca048ac5de51099c" +checksum = "55a77d189da1fee555ad95b7e50e7457d91c0e089ec68ca69ad2989413bbdab4" dependencies = [ "byteorder", "digest 0.10.6", @@ -2337,7 +2339,6 @@ dependencies = [ "pkcs8", "rand_core", "signature", - "smallvec", "subtle", "zeroize", ] @@ -2365,9 +2366,9 @@ dependencies = [ [[package]] name = "rustix" -version = "0.37.12" +version = "0.37.13" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "722529a737f5a942fdbac3a46cee213053196737c5eaa3386d52e85b786f2659" +checksum = "f79bef90eb6d984c72722595b5b1348ab39275a5e5123faca6863bf07d75a4e0" dependencies = [ "bitflags 1.3.2", "errno", @@ -2645,9 +2646,9 @@ dependencies = [ [[package]] name = "signature" -version = "1.6.4" +version = "2.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "74233d3b3b2f6d4b006dc19dee745e73e2a6bfb6f93607cd3b02bd5b00797d7c" +checksum = "5e1788eed21689f9cf370582dfc467ef36ed9c707f073528ddafa8d83e3b8500" dependencies = [ "digest 0.10.6", "rand_core", @@ -2678,6 +2679,16 @@ dependencies = [ "winapi", ] +[[package]] +name = "socket2" +version = "0.5.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6d283f86695ae989d1e18440a943880967156325ba025f05049946bff47bcc2b" +dependencies = [ + "libc", + "windows-sys 0.48.0", +] + [[package]] name = "spin" version = "0.5.2" @@ -2917,7 +2928,7 @@ dependencies = [ "mio", "num_cpus", "pin-project-lite", - "socket2", + "socket2 0.4.9", "tokio-macros", "windows-sys 0.45.0", ] @@ -3563,7 +3574,6 @@ dependencies = [ [[package]] name = "zenoh" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-global-executor", "async-std", @@ -3585,7 +3595,7 @@ dependencies = [ "rustc_version", "serde", "serde_json", - "socket2", + "socket2 0.5.2", "stop-token", "uhlc", "uuid", @@ -3610,7 +3620,6 @@ dependencies = [ [[package]] name = "zenoh-buffers" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "zenoh-collections", ] @@ -3618,7 +3627,6 @@ dependencies = [ [[package]] name = "zenoh-cfg-properties" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "zenoh-result", ] @@ -3626,7 +3634,6 @@ dependencies = [ [[package]] name = "zenoh-codec" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "uhlc", "zenoh-buffers", @@ -3636,12 +3643,10 @@ dependencies = [ [[package]] name = "zenoh-collections" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" [[package]] name = "zenoh-config" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "flume", "json5", @@ -3660,7 +3665,6 @@ dependencies = [ [[package]] name = "zenoh-core" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "lazy_static", @@ -3670,7 +3674,6 @@ dependencies = [ [[package]] name = "zenoh-crypto" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "aes", "hmac", @@ -3683,7 +3686,6 @@ dependencies = [ [[package]] name = "zenoh-ext" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "bincode", @@ -3703,7 +3705,6 @@ dependencies = [ [[package]] name = "zenoh-keyexpr" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "hashbrown 0.13.2", "keyed-set", @@ -3716,7 +3717,6 @@ dependencies = [ [[package]] name = "zenoh-link" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3736,7 +3736,6 @@ dependencies = [ [[package]] name = "zenoh-link-commons" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3753,7 +3752,6 @@ dependencies = [ [[package]] name = "zenoh-link-quic" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3777,7 +3775,6 @@ dependencies = [ [[package]] name = "zenoh-link-tcp" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3793,7 +3790,6 @@ dependencies = [ [[package]] name = "zenoh-link-tls" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-rustls", "async-std", @@ -3816,12 +3812,11 @@ dependencies = [ [[package]] name = "zenoh-link-udp" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", "log 0.4.17", - "socket2", + "socket2 0.5.2", "zenoh-buffers", "zenoh-collections", "zenoh-core", @@ -3835,7 +3830,6 @@ dependencies = [ [[package]] name = "zenoh-link-unixsock_stream" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3853,7 +3847,6 @@ dependencies = [ [[package]] name = "zenoh-link-ws" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", @@ -3873,7 +3866,6 @@ dependencies = [ [[package]] name = "zenoh-macros" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "proc-macro2", "quote", @@ -3886,7 +3878,6 @@ dependencies = [ [[package]] name = "zenoh-plugin-trait" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "libloading", "log 0.4.17", @@ -3899,7 +3890,6 @@ dependencies = [ [[package]] name = "zenoh-protocol" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "hex", "rand", @@ -3914,7 +3904,6 @@ dependencies = [ [[package]] name = "zenoh-result" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "anyhow", ] @@ -3922,7 +3911,6 @@ dependencies = [ [[package]] name = "zenoh-sync" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "event-listener", @@ -3937,7 +3925,6 @@ dependencies = [ [[package]] name = "zenoh-transport" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-executor", "async-global-executor", @@ -3967,7 +3954,6 @@ dependencies = [ [[package]] name = "zenoh-util" version = "0.7.0-rc" -source = "git+https://github.com/eclipse-zenoh/zenoh.git#3b140e0e4eb19d106fd8202136c1c62fd24c21db" dependencies = [ "async-std", "async-trait", diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs index 4a43370..f865731 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/aloha_subscription.rs @@ -13,7 +13,6 @@ // use std::{ - cell::Cell, collections::{hash_map::Entry::*, HashMap}, sync::{ atomic::{AtomicBool, Ordering::Relaxed}, @@ -22,8 +21,9 @@ use std::{ time::Duration, }; +use async_std::sync::Mutex; use flume::Receiver; -use futures::{pin_mut, select, Future, FutureExt}; +use futures::{Future, FutureExt, join}; use log::error; use zenoh::{plugins::ZResult, prelude::r#async::*}; @@ -107,25 +107,33 @@ impl AlohaSubscription { + Sync + 'static, { - let mut accumulating_resources = Cell::new(HashMap::::new()); + let accumulating_resources = Mutex::new(HashMap::::new()); let subscriber = session.declare_subscriber(key).res_async().await?; - Self::accumulating_task( + + let listen = Self::listening_task( + task_running.clone(), + &accumulating_resources, + &subscriber, + &on_resource_declared, + ) + .fuse(); + + let listen_timeout = Self::accumulating_task( task_running, beacon_period * 3, - &mut accumulating_resources, - &subscriber, - on_resource_declared, + &accumulating_resources, on_resource_undeclared, - ) - .await; + ).fuse(); + + join!(listen, listen_timeout); Ok(()) } async fn listening_task<'a, F>( task_running: Arc, - accumulating_resources: &mut Cell>, + accumulating_resources: &Mutex>, subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver>, on_resource_declared: &F, ) where @@ -136,7 +144,7 @@ impl AlohaSubscription { { while task_running.load(Relaxed) { match subscriber.recv_async().await { - Ok(val) => match accumulating_resources.get_mut().entry(val.key_expr.into()) { + Ok(val) => match accumulating_resources.lock().await.entry(val.key_expr.into()) { Occupied(mut val) => { val.get_mut().update(); } @@ -155,9 +163,7 @@ impl AlohaSubscription { async fn accumulating_task<'a, F>( task_running: Arc, accumulate_period: Duration, - accumulating_resources: &mut Cell>, - subscriber: &'a zenoh::subscriber::Subscriber<'a, Receiver>, - on_resource_declared: F, + accumulating_resources: &Mutex>, on_resource_undeclared: F, ) where F: Fn(zenoh::key_expr::KeyExpr) -> Box + Unpin + Send> @@ -166,38 +172,21 @@ impl AlohaSubscription { + 'static, { while task_running.load(Relaxed) { - accumulating_resources.get_mut().iter_mut().for_each(|val| { + accumulating_resources.lock().await.iter_mut().for_each(|val| { val.1.reset(); }); - { - let listen = Self::listening_task( - task_running.clone(), - accumulating_resources, - subscriber, - &on_resource_declared, - ) - .fuse(); - let listen_timeout = async_std::task::sleep(accumulate_period).fuse(); - pin_mut!(listen, listen_timeout); - select! { - () = listen => { - - }, - () = listen_timeout => { - - } - }; - } + async_std::task::sleep(accumulate_period).await; - for (key, val) in accumulating_resources.get_mut().iter() { + for (key, val) in accumulating_resources.lock().await.iter() { if !val.is_active() { on_resource_undeclared(key.into()).await; } } accumulating_resources - .get_mut() + .lock() + .await .retain(|_key, val| val.is_active()); } } diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs index fbdf395..ad6233b 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs @@ -21,7 +21,7 @@ pub struct IsolatedPort { } impl Default for IsolatedPort { fn default() -> Self { - static TEST_PORT: AtomicU16 = AtomicU16::new(17000); + static TEST_PORT: AtomicU16 = AtomicU16::new(20000); Self { port: TEST_PORT.fetch_add(1, SeqCst), } diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs index 93e2b03..39ea7ac 100644 --- a/zplugin-ros1/tests/aloha_declaration_test.rs +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -15,11 +15,11 @@ use std::{ collections::HashSet, str::FromStr, - sync::{atomic::AtomicUsize, Arc, Mutex}, + sync::{atomic::AtomicUsize, Arc}, time::Duration, }; -use async_std::prelude::FutureExt; +use async_std::{prelude::FutureExt, sync::Mutex}; use zenoh::{plugins::ZResult, prelude::OwnedKeyExpr, OpenBuilder, Session}; use zenoh_core::{AsyncResolve, SyncResolve}; use zplugin_ros1::ros_to_zenoh_bridge::{ @@ -155,8 +155,8 @@ impl DeclarationCollector { let r = r.clone(); let k_owned = OwnedKeyExpr::from(k); Box::new(Box::pin(async move { - assert!(declared.lock().unwrap().remove::(&k_owned)); - assert!(r.lock().unwrap().insert(k_owned)); + assert!(declared.lock().await.remove::(&k_owned)); + assert!(r.lock().await.insert(k_owned)); })) }) .on_resource_undeclared(move |k| { @@ -164,27 +164,28 @@ impl DeclarationCollector { let r2 = r2.clone(); let k_owned = OwnedKeyExpr::from(k); Box::new(Box::pin(async move { - assert!(undeclared.lock().unwrap().remove(&k_owned)); - assert!(r2.lock().unwrap().remove(&k_owned)); + let st = k_owned.to_string(); + assert!(undeclared.lock().await.remove(&k_owned)); + assert!(r2.lock().await.remove(&k_owned)); })) }); builder } - pub fn arm( + pub async fn arm( &mut self, declared: HashSet, undeclared: HashSet, ) { - *self.to_be_declared.lock().unwrap() = declared; - *self.to_be_undeclared.lock().unwrap() = undeclared; + *self.to_be_declared.lock().await = declared; + *self.to_be_undeclared.lock().await = undeclared; } pub async fn wait(&self, expected: HashSet) { - while !self.to_be_declared.lock().unwrap().is_empty() - || !self.to_be_undeclared.lock().unwrap().is_empty() - || expected != *self.resources.lock().unwrap() + while !self.to_be_declared.lock().await.is_empty() + || !self.to_be_undeclared.lock().await.is_empty() + || expected != *self.resources.lock().await { async_std::task::sleep(core::time::Duration::from_millis(1)).await; } @@ -230,7 +231,7 @@ async fn test_state_transition<'a>( } } - collector.arm(declared, undeclared); + collector.arm(declared, undeclared).await; while declarations.len() > state.declarators_count { declarations.pop(); diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index e92b8f4..c0b84c5 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -34,7 +34,7 @@ use zplugin_ros1::ros_to_zenoh_bridge::{ }; use zplugin_ros1::ros_to_zenoh_bridge::{ros1_client, zenoh_client}; -use log::{debug, error}; +use log::{debug, error, trace}; use rosrust::{Client, RawMessage}; use std::sync::atomic::AtomicUsize; use zenoh::prelude::r#async::*; @@ -636,7 +636,7 @@ impl PingPong { async fn check_pps(&self, pps_measurements: u32) { for i in 0..pps_measurements { let pps = self.measure_pps().await; - println!("PPS #{}: {} \t Key: {}", i, pps, self.pub_sub.key); + trace!("PPS #{}: {} \t Key: {}", i, pps, self.pub_sub.key); assert!(pps > 0.0); } } @@ -696,8 +696,10 @@ struct ROSEnvironment { } impl Drop for ROSEnvironment { fn drop(&mut self) { - if self.rosmaster.is_some() { - self.rosmaster.take().unwrap().kill().unwrap(); + if let Some(mut child) = self.rosmaster.take() { + if child.kill().is_ok() { + let _ = child.wait(); + } } } } @@ -773,7 +775,7 @@ impl TestEnvironment { } pub fn many_count() -> u32 { - Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 10) + Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 40) } pub fn pps_measurements() -> u32 { @@ -912,8 +914,10 @@ impl BridgeChecker { RAIICounter::new( self.ros_client.publish(&Self::make_topic(name)).unwrap(), move || { - status.write().unwrap().ros_publishers.0 -= 1; - status.write().unwrap().ros_publishers.1 -= 1; + if let Ok(mut locked) = status.write() { + locked.ros_publishers.0 -= 1; + locked.ros_publishers.1 -= 1; + } }, ) } @@ -935,8 +939,10 @@ impl BridgeChecker { .subscribe(&Self::make_topic(name), callback) .unwrap(), move || { - status.write().unwrap().ros_subscribers.0 -= 1; - status.write().unwrap().ros_subscribers.1 -= 1; + if let Ok(mut locked) = status.write() { + locked.ros_subscribers.0 -= 1; + locked.ros_subscribers.1 -= 1; + } }, ) } @@ -948,8 +954,10 @@ impl BridgeChecker { RAIICounter::new( self.ros_client.client(&Self::make_topic(name)).unwrap(), move || { - status.write().unwrap().ros_clients.0 -= 1; - status.write().unwrap().ros_clients.1 -= 1; + if let Ok(mut locked) = status.write() { + locked.ros_clients.0 -= 1; + locked.ros_clients.1 -= 1; + } }, ) } @@ -969,8 +977,10 @@ impl BridgeChecker { .service::(&Self::make_topic(name), handler) .unwrap(), move || { - status.write().unwrap().ros_services.0 -= 1; - status.write().unwrap().ros_services.1 -= 1; + if let Ok(mut locked) = status.write() { + locked.ros_services.0 -= 1; + locked.ros_services.1 -= 1; + } }, ) } @@ -1286,32 +1296,3 @@ fn ping_pong_all_overlap_many() { let parallel_subworks = parallel_subworks(&env, main_work_finished, 10); async_std::task::block_on(futures::future::join(main_work, parallel_subworks)); } - -// there were some issues with rosrust service, so there is a test to check it -async fn check_query(checker: &BridgeChecker) { - let name = "/some/key/expr"; - let _queryable = checker.make_ros_service(name, |q| { - println!("got query!"); - Ok(q) - }); - - let ros_client = checker.make_ros_client(name); - let data: Vec = (0..50).collect(); - let result = ros_client - .data - .req(&rosrust::RawMessage(data.clone())) - .unwrap() - .unwrap(); - assert!(data.eq(&result.0)); -} - -#[test] -fn check_query_() { - let roscfg = IsolatedROSMaster::default(); - let _ros_env = ROSEnvironment::new(roscfg.port.port).with_master(); - let checker = BridgeChecker::new( - IsolatedConfig::default().peer(), - roscfg.master_uri().as_str(), - ); - futures::executor::block_on(check_query(&checker)); -} From 59dd7cc2ba980e5385ac0d484267348ef5a6e585 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 24 Apr 2023 17:55:59 +0300 Subject: [PATCH 17/19] - some brush-up - remove old logo file - move common test code to test_helpers.rs - remove unnecessary topic_name and datatype restrictions --- Cargo.lock | 94 +++-- zenoh-dragon.png | Bin 33031 -> 0 bytes .../ros_to_zenoh_bridge/aloha_subscription.rs | 22 +- .../src/ros_to_zenoh_bridge/test_helpers.rs | 371 ++++++++++++++++- .../src/ros_to_zenoh_bridge/topic_mapping.rs | 2 - .../ros_to_zenoh_bridge/topic_utilities.rs | 10 - zplugin-ros1/tests/aloha_declaration_test.rs | 1 - zplugin-ros1/tests/ping_pong_test.rs | 372 +----------------- 8 files changed, 452 insertions(+), 420 deletions(-) delete mode 100644 zenoh-dragon.png diff --git a/Cargo.lock b/Cargo.lock index bec7f56..9087770 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -56,9 +56,9 @@ dependencies = [ [[package]] name = "aho-corasick" -version = "0.7.20" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cc936419f96fa211c1b9166887b38e5e40b19958e5b895be7c1f93adec7071ac" +checksum = "67fc08ce920c31afb70f013dcce1bfc3a3195de6a228474e45e1f145b36f8d04" dependencies = [ "memchr", ] @@ -428,9 +428,9 @@ dependencies = [ [[package]] name = "bumpalo" -version = "3.12.0" +version = "3.12.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0d261e256854913907f67ed06efbc3338dfe6179796deefc1ff763fc1aee5535" +checksum = "9b1ce199063694f33ffb7dd4e0ee620741495c32833cde5aa08f02a0bf96f0c8" [[package]] name = "byteorder" @@ -568,9 +568,9 @@ checksum = "e496a50fda8aacccc86d7529e2c1e0892dbd0f898a6b5645b5561b89c3210efa" [[package]] name = "cpufeatures" -version = "0.2.6" +version = "0.2.7" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "280a9f2d8b3a38871a3c8a46fb80db65e5e5ed97da80c4d08bf27fb63e35e181" +checksum = "3e4c1eaa2012c47becbbad2ab175484c2a84d1185b566fb2cc5b8707343dfe58" dependencies = [ "libc", ] @@ -821,7 +821,7 @@ dependencies = [ "atty", "humantime", "log 0.4.17", - "regex 1.7.3", + "regex 1.8.1", "termcolor", ] @@ -834,7 +834,7 @@ dependencies = [ "humantime", "is-terminal", "log 0.4.17", - "regex 1.7.3", + "regex 1.8.1", "termcolor", ] @@ -1439,9 +1439,9 @@ dependencies = [ [[package]] name = "libc" -version = "0.2.141" +version = "0.2.142" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3304a64d199bb964be99741b7a14d26972741915b3649639149b2479bb46f4b5" +checksum = "6a987beff54b60ffa6d51982e1aa1146bc42f19bd26be28b0586f252fccf5317" [[package]] name = "libloading" @@ -1476,9 +1476,9 @@ checksum = "0717cef1bc8b636c6e1c1bbdefc09e6322da8a9321966e8928ef80d20f7f770f" [[package]] name = "linux-raw-sys" -version = "0.3.3" +version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b085a4f2cde5781fc4b1717f2e86c62f5cda49de7ba99a7c2eae02b61c9064c" +checksum = "36eb31c1778188ae1e64398743890d0877fef36d11521ac60406b42016e8c2cf" [[package]] name = "lock_api" @@ -1751,9 +1751,9 @@ checksum = "ff011a302c396a5197692431fc1948019154afc178baf7d8e37367442a4601cf" [[package]] name = "ordered-float" -version = "3.6.0" +version = "3.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "13a384337e997e6860ffbaa83708b2ef329fd8c54cb67a5f64d421e0f943254f" +checksum = "2fc2dbde8f8a79f2102cc474ceb0ad68e3b80b85289ea62389b60e66777e4213" dependencies = [ "num-traits", ] @@ -1972,7 +1972,7 @@ checksum = "2a780e80005c2e463ec25a6e9f928630049a10b43945fea83207207d4a7606f4" dependencies = [ "proc-macro2", "quote", - "regex 1.7.3", + "regex 1.8.1", "syn 1.0.109", ] @@ -2021,9 +2021,9 @@ dependencies = [ [[package]] name = "polling" -version = "2.7.0" +version = "2.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4be1c66a6add46bff50935c313dae30a5030cf8385c5206e8a95e9e9def974aa" +checksum = "4b2d323e8ca7996b3e23126511a523f7e62924d93ecd5ae73b333815b0eb3dce" dependencies = [ "autocfg", "bitflags 1.3.2", @@ -2195,13 +2195,13 @@ dependencies = [ [[package]] name = "regex" -version = "1.7.3" +version = "1.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8b1f693b24f6ac912f4893ef08244d70b6067480d2f1a46e950c9691e6749d1d" +checksum = "af83e617f331cc6ae2da5443c602dfa5af81e517212d9d611a5b3ba1777b5370" dependencies = [ - "aho-corasick 0.7.20", + "aho-corasick 1.0.1", "memchr", - "regex-syntax 0.6.29", + "regex-syntax 0.7.1", ] [[package]] @@ -2215,9 +2215,9 @@ dependencies = [ [[package]] name = "regex-syntax" -version = "0.6.29" +version = "0.7.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f162c6dd7b008981e4d40210aca20b4bd0f9b60ca9271061b07f78537722f2e1" +checksum = "a5996294f19bd3aae0453a862ad728f60e6600695733dd5df01da90c54363a3c" [[package]] name = "ring" @@ -2254,7 +2254,7 @@ dependencies = [ "itertools", "lazy_static", "md-5", - "regex 1.7.3", + "regex 1.8.1", "serde", "serde_derive", "thiserror", @@ -2273,7 +2273,7 @@ dependencies = [ "hostname", "lazy_static", "log 0.4.17", - "regex 1.7.3", + "regex 1.8.1", "ros_message", "rosrust_codegen", "serde", @@ -2300,9 +2300,9 @@ dependencies = [ [[package]] name = "rouille" -version = "3.6.1" +version = "3.6.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4f86e4c51a773f953f02bbab5fd049f004bfd384341d62da2a079aff812ab176" +checksum = "3716fbf57fc1084d7a706adf4e445298d123e4a44294c4e8213caf1b85fcc921" dependencies = [ "base64 0.13.1", "brotli", @@ -2310,13 +2310,12 @@ dependencies = [ "deflate", "filetime", "multipart", - "num_cpus", "percent-encoding 2.2.0", "rand", "serde", "serde_derive", "serde_json", - "sha1", + "sha1_smol", "threadpool", "time 0.3.20", "tiny_http", @@ -2366,9 +2365,9 @@ dependencies = [ [[package]] name = "rustix" -version = "0.37.13" +version = "0.37.14" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f79bef90eb6d984c72722595b5b1348ab39275a5e5123faca6863bf07d75a4e0" +checksum = "d9b864d3c18a5785a05953adeed93e2dca37ed30f18e69bba9f30079d51f363f" dependencies = [ "bitflags 1.3.2", "errno", @@ -2595,6 +2594,12 @@ dependencies = [ "digest 0.10.6", ] +[[package]] +name = "sha1_smol" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae1a47186c03a32177042e55dbc5fd5aee900b8e0069a8d70fba96a9375cd012" + [[package]] name = "sha2" version = "0.10.6" @@ -3574,6 +3579,7 @@ dependencies = [ [[package]] name = "zenoh" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-global-executor", "async-std", @@ -3591,7 +3597,7 @@ dependencies = [ "ordered-float", "petgraph", "rand", - "regex 1.7.3", + "regex 1.8.1", "rustc_version", "serde", "serde_json", @@ -3620,6 +3626,7 @@ dependencies = [ [[package]] name = "zenoh-buffers" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "zenoh-collections", ] @@ -3627,6 +3634,7 @@ dependencies = [ [[package]] name = "zenoh-cfg-properties" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "zenoh-result", ] @@ -3634,6 +3642,7 @@ dependencies = [ [[package]] name = "zenoh-codec" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "uhlc", "zenoh-buffers", @@ -3643,10 +3652,12 @@ dependencies = [ [[package]] name = "zenoh-collections" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" [[package]] name = "zenoh-config" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "flume", "json5", @@ -3665,6 +3676,7 @@ dependencies = [ [[package]] name = "zenoh-core" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "lazy_static", @@ -3674,6 +3686,7 @@ dependencies = [ [[package]] name = "zenoh-crypto" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "aes", "hmac", @@ -3686,6 +3699,7 @@ dependencies = [ [[package]] name = "zenoh-ext" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "bincode", @@ -3705,6 +3719,7 @@ dependencies = [ [[package]] name = "zenoh-keyexpr" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "hashbrown 0.13.2", "keyed-set", @@ -3717,6 +3732,7 @@ dependencies = [ [[package]] name = "zenoh-link" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "async-trait", @@ -3736,6 +3752,7 @@ dependencies = [ [[package]] name = "zenoh-link-commons" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "async-trait", @@ -3752,6 +3769,7 @@ dependencies = [ [[package]] name = "zenoh-link-quic" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "async-trait", @@ -3775,6 +3793,7 @@ dependencies = [ [[package]] name = "zenoh-link-tcp" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "async-trait", @@ -3790,6 +3809,7 @@ dependencies = [ [[package]] name = "zenoh-link-tls" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-rustls", "async-std", @@ -3812,6 +3832,7 @@ dependencies = [ [[package]] name = "zenoh-link-udp" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "async-trait", @@ -3830,6 +3851,7 @@ dependencies = [ [[package]] name = "zenoh-link-unixsock_stream" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "async-trait", @@ -3847,6 +3869,7 @@ dependencies = [ [[package]] name = "zenoh-link-ws" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "async-trait", @@ -3866,6 +3889,7 @@ dependencies = [ [[package]] name = "zenoh-macros" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "proc-macro2", "quote", @@ -3878,6 +3902,7 @@ dependencies = [ [[package]] name = "zenoh-plugin-trait" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "libloading", "log 0.4.17", @@ -3890,6 +3915,7 @@ dependencies = [ [[package]] name = "zenoh-protocol" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "hex", "rand", @@ -3904,6 +3930,7 @@ dependencies = [ [[package]] name = "zenoh-result" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "anyhow", ] @@ -3911,6 +3938,7 @@ dependencies = [ [[package]] name = "zenoh-sync" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "event-listener", @@ -3925,6 +3953,7 @@ dependencies = [ [[package]] name = "zenoh-transport" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-executor", "async-global-executor", @@ -3954,6 +3983,7 @@ dependencies = [ [[package]] name = "zenoh-util" version = "0.7.0-rc" +source = "git+https://github.com/eclipse-zenoh/zenoh.git#eca888b410a0afb1df939393d4a304b7f926cd5e" dependencies = [ "async-std", "async-trait", diff --git a/zenoh-dragon.png b/zenoh-dragon.png deleted file mode 100644 index 6507046b2eb7b721661c657255c42f7749a4fefd..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 33031 zcmX`SW0WXO4=y^kXOC^$wr$(CZQHi3J+^Jzwte>d-FwcDtd&(=U6o3DrIIJ<2zgmC zSSU;=0001332|XX002Phe`E~-_HV?MBp3q#00hHANJ!pFNKDAi+RjnQ-oVI2%*58j z(ZWbkOb`HoJtjs)#~fSb4{xWF84>Y}k~MRPb2%R@32c%ob3t>5-}W}EZK?$82H~0U z%gs#s_zXK!=XZgeh`cw$Mb@98-d2Bn@@M!>-e$TKD3XmOd~)9Aw;=hmsM5;}E~X5N zj)onR8}G-Q+gz`kcC%a1v-V2$)Pz5PsF}4iXp9F=VR7L+RPn)K`DkDTY6>`1Iu+7D zta8w9En2%m173RT%iBMBHLapJDEJO|hD|5s(BW}_cV#M*u7I8&33z-Lnz%29XRQqU zLv)^P`$nsSPQ1P`7N$qk2aDmLDoU2&X$Wys!*8N6pA7XYdigSzmz8J7g@#!!+xZRl zw+26XHvQ%yk2Zs4vl)53!M6t5pHwU)r~3*+Qk98$qF)dPRyIC=vG+Dpvl0;bz~Z-d zGV4kaZ-uq1H^D1=@``I(A_;s?gIOP`4`{+?*-j^v`q!!|lZLbVU58p(G#?(gK>8v& zv@-8U9Y1Z9avx5QFIX}sy*z}!KDs=_Uwj<%IIhpV&mMzQX~4fq(vpKvhJwQV3Pc57 zP7i(>wj>dNF)T5UCH!a;i{P~A4s~ow&t|1y(Abvnq);zE0u=Lqg`1)r{p&QqsuZGQ zc^Snb?s{;IbV4EO8$x}jv$ff0?#`N4Kk*UT$|;J_IF)cnDS z)GIJW!#eHbe&B4*nC|ZxNd6fc(+hfUn`x#r4&x0(ZN&VFtwvomxvvV*HbUm}rDg-- zBgMc+S`V7lWt4bHa9{)^^oZn^c}c=A zqLkKFTCJvr`x{C$N%0A))BQD~*Pwk)&8G1803)YEZRTC?{ql?Ph3d<)c+c-h?Y`A< zRCB7cQF`#oTHVP_1=9xX5cO_8V>n_M=h5N*NDW@pH+4MYwC+iVTf>ePPP94=^uzL9 z6+<>gxcVvRDXf{go6Lv&d!&2XXToRNXU3;r2PlDSGUu>opVl7FNpIxE@)1*W*9+AH zz18eI)v8*OfXEsJ7u9iqbzi}N@qr8m!bnhIEg-e#@)I?WE|&)f&Is?M>{GjkRuwSf zvjONy=0`GkTR_k+>h_j~DlYsjhkl*Ye>jVq^j_ttrZm;bj4z%0fOFh9>*1y~#U_Mfj{ zH&K@`m5~9U{KrE80ESrrfc)bC{}JXt0ssKY2l~GwfYkZG|Btu+?_rVFTQUFuKY)a= zfU-N_l@GYT^6>ZXR99=K8v9;s>{4eDdE}ZE48BN$RXUME5nwdLXuKm-Y^w+^^!O^3AkS-Qy}pfeN(WDc1s6C$+Gray|8 zw^RWB&EqFP005%)uiu5z)YPoB2$n09zNL^(f|$8yfLv_uPhRAB2VDg7p$hycK#+$Z z2N4v;Hy`*Yr(6u}>P}o{D~TTxDR*TsFm{a_X#BFfDQEis*2Dx7Kwg<~@zF*HvAs*_ zMj&OE>ZFWG&}avBdwD<+)lvGl)E}S#!hBH7{j9r7;g|I^RpgmbB*{&0r)@8=9Mlp` zT$C;`*&C9}SnKcln`Zipz<;vlBR~KIB2P=!FQzH$ZH(-R4YS+DqIXla|!D_A#S8kA~aNHak7i{a_Q6-EG0Z04JirfeXGT}v?7mgmaR<|01^qTjr!%xLV)9eGHgqGremNP|!fHuI{ z_!3|#u690RM8eyJqR{}6#Bje?CffV=%Q(@%6tCDG4GtrrtPG<5aI+$Z2uXz;C%sLnoEZ zO56}GTurSR&wc!+D2wka@3rwm@qgx;CPYvj;QYIP7%B@)S96$*#y_!;HND(H04t>d zMshH51&C(D)p1-jO%tZ(hVpL*WwHF>0xkMI_KPb!!?xBHCW9_gaicJUwz1fp&Gj>PrhX=676TYoYoGU zvM0ht(4wS`X!x{kzvBq|l9~kth#{bWdMWirq^;qd0_849BCy^IVl*ja89aPuMz&u2 z2@hWp#ehta{N@7{ul+5S)i#+Av+7vQ0cW3#mD+tb(8p9WW-N(qc?e}xe*lrv_KNVB@GzU}sHYtb`37M(EK1QWwR zBH+O%ESu0;@8q1&^p1FO2;ty*VgS9h*WR$OqvsP;(NCc9#O8EYiiZ1jkBPKYy70_y z)h>OTyaNs^+U8P;90Ab<2-h0hC7G&nSvzbwH-0r?zRou5A)AVT*h-T|LIe)TNx^8S z{wcFQ?oBNQUp`np1DZpF;f|P}ADCmQOJd%>5Y%Z+506iOeE?tNzT5z zZf2#-xV_z8InqMeKR}R?{K1e>@z+siKK&tY$hagBzya9q*vG(QYq00p+?iUO?K=1k zNetHWTP$WVS=}FTn2UO5n4O4!ZEb>yDXm5ccUe-cxEKl?q=}SkNbg`1Ix_honhj%c zb2IhF8vP-fK%n*s+&#bKlzGD!fKnPHn)_Ax%+1$lS!=U&4*h_}5gYH?RkmBeJfVxL zv>Hk@d$bv8pOt^G6YJ@fw*)NP7xdnN)1i=8A%xY#o@KTlqb4RnisX#q(cc{L?NJXf z*5XgymSp^hrb&AwI6P*d@;=Q_ow!B^=zf@~jXSWRT(UpJlw->2w?(A~-7UtG_t^rz zeBv;$fUE36__K)+?g1R-@-~VCm=ya?3KWp&&8=lTtv&He6$({HPiwH*OLYQKB#nY;?am2i?8(Pw=$` z2(B=2IrSgzq7Hekl^`m@HP?eE?cx(8PkOv;>56V)sFB=5E<72+No!bmRGKsd7(;|r zXPK4S?<`P>U5FA8!Xx1>iScFsDz6RGL^mKho>1-$64F?-O#9{NsjGClU}*R=CmNXz7O`@=?}q7LZj0=59Jt{Q{%wc-+z{DtlSK7z}@ zM!UTM@RNEIfe8+@<^)ieVg`&wF?4Y_pCOo|^8kMAc~D{o6|Gd$dhR02i+x zoM+_;rR{lLFxDap9t)bVjv53pCXfOdHRRk2LCz1@ga)-twIBy~v4|HzOvZOqsUKN# zb0K1@{YxjXq-mW;v^guf?}n}5cM~;F%F=Ymdwpn7@@>O9j$SB-MM}0Hz9~fT2s*)^ zPP_Z3r~)M2Q1B`#d8jv152-`(Gapt*T2fm*UalMBNQS1JD@!vt33L(#iLidFO89?I8nLGP`o*5Pl#c1Y}65gBBz4u(n&Px_-~<$qTSyCA-EDMSI|X4=daz`zI7lLkTYgN(A2<2;6T zjX9Iiu~gMv92HpWts!p-gJOgU!y!G)6hx=scNXO*cT$IdxbtTQDeDJJJ}o5((w~5I zq|_nlR-p+(Lnm^k(A@!DxR7*Ss7a078>Z6g%hnr_JF)pkDlgPooo^@8R+|3IUPhQ- zPIX*9qI`L~8q5X#Ke&uPt8?{w>v}CZPK>>OynS&+n(E&p* z+G8c(y#~q&4=Y0qkpcBfF&os%={md{Qxj)^LOnzYmn7w_R_>NjVS<1m?YXmG`}c|wg`SlmhA$ZpLozqXVN_+7@Ih; zi;wnvOUTCU2)7257h@ksl^1F{rf8Hyh+AY}jp&MI8|me7inb3zqRbJ;;5}*#l(ym4 zIZ+SRTi4K8h$klU(BwGtSwWmHwc@F^uF;%}Ca5TZAirXFeu(23yD6EYk=W8Gj}yi4 zYa^}%sK_lTCe4O!K-!x3ApQ_joa2|<0?!g$cjgkfFz3n@&nujZp#e5ib@5m2>pLsR zm2VwH))PAx!fU~XEN1PBYs_ivdxq(@7wfFvSiYsnZ%_UXXnK)lp5R(iUV8s3NOi|M zh^cFt=?6iInhI2M5|>c-gIa8sV4bg#!Sfx~wpFYe5y~6A0S#`5y$W>2`2KjVhtCqm zhe$F?AcRYSvFmZ&0k&PZa6Cy(T1s^sERxRC{&sDrgLc(^W&~}TxSUmwO`yp`dQT^&GOV1E>Oqz) zFOevT{V)Yo6-DBbfE*|eB)aT67_`Fj%y8HVeb(~)(GaO%5~jkka)JRmRP8royj~kJ zu|!9wIYZ{f>#d$JQkYDnRf`P>C^w7LiUBd22SFTHf&**_K1VnK@&i5%bXt|^(&a#l zhFBQSIAi+@?4aDcpkeh0JEB*V=~{c7NfJ9z8i_;^Kt=PboxYC}+7O!g6oy|_5?P&C>`f<-ht#833B(|TUkg!XzA^zsz1<6K zXFTOnH7_{jDU!}O5|10i>kE?0f;+|Ce;djc+HD}INs?3uF%$!L`LS;0$U%~Fc62H> z{pu0P>d9PbL$NAj#PxT^=WYwQNVle3UdpyGz^m~T5iMAS-p(U5rfhojVl@lMaWCzv zCMoGr_Fl1CpVi-@E#HY2zM za?~c-l8q7`CSrY-dYCgBwH|5Tq@dlee2kJ|lVM&G2N|-;cw}OKsI(hm;G)zN6uD+n zyg>5C%X~$U(f6Xf>E+(VEk&73mqs}P88M(7c?CZAS8F!ZQ;no)omSxz1i5a~{YwqQ z3m@b6fr{pH__YQfM%V)2dGBd9j*--LlYm4N@7h#>@+Q9okTK3@o89xY68Tgp#Mqg0T=jrO<$b zT-|j5&K_RKPZ9VR0SQ*9gchEJ>RjzD+K_uO)8*aOSB0crN^?vOK1vk3K}t`@{0(pO zE85wY8VR9>hAEn&UlKknX04&7DfGY>P!xPA4GmT|srGrL^B0RG;;P*H^43HU5)$%rPbs97T82d1Me#6c@bhJYs3X@YLLMi^@y6d9LaY~T z63s}&G_i?BfDM9l_etmbVT-9BgX3lcmWq4#a?KGeAVi|-1r0k_mhx|t%FP}ux);?l zq-0o-6tKXHa-nZq(7qWiph<{ z<;*bm9nCUiJR4?-Te3FdzGZ)lseH}M))<~1WyGC+dosG(4csh|RFi(_NN6Q==R_tl zl4JzuOfZe4+!pA5I)7d^QvfZHtCuW{5yHb4w$Bp3g{t>^ReDlt?p2K^ZkSX!x|ojf zuoYjE&+8LtuleQU^B=hWf=ZYq%*2;FGF}TO9X}r03*?o+aTtshM1r8&8^Atb!nmyj zwbR>OPdlyVGFUxdX6E}TnZj3=2_pgi=B@!q$}`_?OO=o71+aMCnrk&lEJu zFEDk&p0BpkoI~>{m$6!;f((BVmzOTPV3K+`ADbC}*A7xtdD}&fR-7tc1U`zK>z3l) z`3}f5tQ~D+O-_c{0ycQ;V3&z1_M!j@{0BQw`ux<$igy2?SvJK*hwM!FgV1}*%Q+5) z_|OD4ZsmbMN*gtQk6w-oP4W?Z#x0j+FLWaU`}Ne+ba8*eNdj%cI9R7xWY)1QB|TT& z;OJb+VNym5=Cgnduo7o;-x_JD3?_irC;J&!n}U=Cljxmuq#r}CHdyGfa3~)RMHbdO zdbLHOv7M_o)ubxaKb^~h3o`H_Ou0CjVm-tia8&6sg}>ZjDb@XO+Co)BCC2{;t-wWI z+z#$oGICNZsW!~NkF|dN4H_M907J7#Glg##z}y0R|4jR&$*NXv8HU0ZPLI$x==65b zZ%cj;IbQqppy0B1HZmD@SKG6wLa&JBS04(!JneG9C1bUwv73w; zmalI>Jl4_;t1IOagCSHqakcHjetNx_eNBA-T@$PaENAgUa2BkToXTgX;y7D!PIZ~serH?BnXRn5Bxhw3N+Ei&$p8XXf8_VvsA=SfW zFa2{(1Bf!fV=*gY5I!lRx~V|X$2o2juqgzdQ}#Laacoe%B%OmGSdM&ROmuG?|0Y~z3% zxO>u;LpdoY9)XOPWv(E*PjgC$eODAzXEx%U!DaQ=?lKZog)yGfn+Tyk_!u`=C)&)5 zGS}W*`p}B}tTR%V@IlcjXp;DOsQx3)n8J$3ps#sbTlT`U4@Cr|lLY*oA+N zH}NQc_LCR!ioC__hpw*8n`Oz1s@kkoa7*xfx4xq&rorI;4VEm$IeqvsPKmSa z`93$2vVFMJ!H8HXuUE#PA;F@4dGZ$fU8QT4u{Hln5-LrC{2P7ZiP}m;jGTB`JEZ#v zbb%AbWyJB|gsHWg!_3F2U+KIPI+4Ty;_s?y@QcU1-p?{!D&QL@xIwCRkZeDqqKYLmxM*u_%FGa|~VAqncy;(+sI zYX!xR!`HH2`QsYR0A*H7DVpIOh`Qd~jzyJ{p!fol87NhyQ`tpwl}Xqes^r6v_~oQs zh3K!c@?5aI{|lbmsYpDSUyx&5A3Z8byop{q~Ed~o|?aQ)E)`htO zl#iXO%B(j1wfl$xZ?X^JKG1~y2XN;feyFn8D+tn=4Uvmx(i&>pvbUfj zTeRepAde%J*UK4RNoI9b`J|c41AzM zzK*UZO?|V&*}=6=Od+eD##^1Dov&frqT8imL4!irBn~~oeCDpl-n|i!bxIs6So42t zyL^rsgsY$P7Zf`e*WbhNEAxaDudrTCxe}XUe)N*ClH9r&UI>xYHTgq|}F zLS+7F^+rE@KM?Sy@|Cg4HH;8GdVl#EdcUCRkxc$V5reMT_ zmU2x47Bs0O1GqYjn5r6fbn-$79oL{&z)9q|*=i4wnIjf3xrj03o{k%U zjfN7-hhF1?Czn9qu_xMO8=nAvj)k>%cI~0yehF@$JW6iBFw+&HiL~V{ma z6o?RD&_%V$Pi?@>e=CuzE*Sh@G6zHtTNK*t-l!R;+!|jT0-5yFA$xL)k_;cG%-bNo zN@k#7c~z{9tq_b7MH-RB%@N?dOeYJ4?9OmRt=~IPp_ESl8(H?+iOwM28Qq60IftJ5 zXS~_heAuI1WKoRRoJE%EuiU`G3bz}A%;&mA!E(rwfWN}Z@mOwfa{WW(gr{QXThUxI zpMSMyX|=0R$D;vto&qmjuY1Sn40XR(z1%=$bM`?LioS)?2cx3c1p)rvP2!Vmm*;7U zK7{bOo43O$vY{zQ3Uv=fWIK$(!1p;8{9T7i#R&P34^>5DwmT>$2_68)w`0 z=nsu7sIiAo<%J>;fU+9b^OeB}b;k&E25mq7>kWK?W?saN%bvlSO0-phdlkom5811A z_8%?@6J!hLtWxTXMw0K7@r{2cNiZ+O89>lOgLw{mXFNho2Imy?Mv$^L@2|x}H2Ub*fVikky5w1!L$rz zMp<9|LXBP8S%A|8oBRb@>q-_4c?cqAX6UHtU^sKoMXVF*s~HupMc%+Rii)ezTjZr1 zLnd^_P+IOiV5oB!$b_2sV0ku?=|OmeDrkVL$Rg037kTnavQyOPTiEN@8eq364QO&1 zor#XUC9I<`uD1x($JQ0_@T|bmwOh4;iiYKUjT{AK<-I?P_T7@;wIRr+s;OR-$%ubk zV`g!$lo=H;THm(Ih*th*4yM4B2BgPqVjrG03<`a zRwB`ylVV3B?`FmySewGd7I{Dd3xc2J;3%j!1*4y9PWwIrwAuheaLJ`Zhy12Q`eOj? z1Jg9W5*zAogns@aC7%Ad3@bPfwgv5cg#!Knwq}#j$?ssg+n&K3@P4ROBl%M5=kX|@X!lFI|TiwfQ%ZT4yb4yb| zD|$nHI0QRCd!xA>hj$9<+{z9LhZ)3&yI9!j*!;5vYcRRDw8vWNFpy!Ph-Jq4;E?5% zUK=-T3hNMqNIxf@_C{{#dw9N1GpR zVzzOgj<}M)(8vl)%L=;XOJ~Gvp^S)lp+Z1LC4-yqcMYU7rqnKSZ2h^A;Rnkx4?#Rv zxzNopI}v$L%WXx+KT(+wc`VF{IT^J+(<*(_V{)I2*g#Mv3?vg9W@W=bP`v}ncXR1H zUq+zPKaZGGt=1S}C&q!;G8k3dh8A0h^JS%M5 z4o8*9Qp09ynauKRmt~qaGfVXF`0eB6qKdy7PYAusZoV1O>afcHUJtV?-8|?#9Lm7J zLx$I-DJ=dN{?7c&p*O}K>qz%%GoP3okO!TCE8>_ z*aFVDYSh5c(W+X4>I}}~h}wqt>_tJjocE;|GzpZwK9u2%);d?-*E#>XfCM>AMXbdn ziI%C~d97EpP}2vx436IX+PgRgC+(Ozf8&-Ii9L);rY=X7Ra73s`ry1{^{9X+{i5OPz~b%%6KesgLa4UQDEe^=_?8cp zjH@-Xv|WJK!=l+E5^QY+As#7>ev7UR_ApH97X3cX{3z?oL!FAWV$&dai7)dYnaj|z z^Q-Wj&j?c|oYygzu(I@wc%(h8c_W|-cbYC!Q9A4AC#H;+77DzcIi1s9<|j0P;MvyF}WT9s8lE@}9t z1gAHnZReVC?H!tv-;rs3#PiI))D}~TW5voR2~*V=yptx-AaYN(m%!sq4r^kFc$}2L zOv!nDas$BXUE0m&7v&x)izBPdsX{(suz-~ug4jp{Z_H@5Qr3apKyi~L_<}6Z=zmWl zbOcj#P)Rf#Nd#*<51kpE&HGLi`crwUF>0#H<2#DrkIlB2?4I>)M>$FjU`AfnzuWPB z`UGt3;XNjFLMo=TeC=tu8ecn_2N9LU)<*2@v-xEa2DifCe)Y_`cmB}!xW4BQ^$K#k z2txqs%WOaQOR2A+)zeT==7q8?ZJ({-q$r~lghr|~I*O((u&&v?CUpHZ!{7-^Y`>9+-hJfh$Htm+*#MI`_Aqe}ra^bY#MiJsEoO!t!G0_wLn<)b?2{cRBW`)3g z`vrva;UzAvWc+O7o^+BXqDc)6T)cZ@>!ylHT*H^>k~)XfKq(qspPAZL7&uXZU-p`` zF~~Lvudn2M@QfYgf!G$j{9j&F*q&P;-8+TJNxoU>ZnJln41En9^+x@a8ur*&Wjp7UYCRMx7cwmNKbvr-{ zN&0dBBdt}$Nlgl)eLiDt>7YNvrATPJM?!N|boU$HJv;jWlM{~`Ye+Q!1s>^J%wW5- z+bPiE6vAvT84R2ZTEYe2=EuIh_DxMtDu7&og47j_Y9XN|c@3zgIx_2}?W*Qk&H~jY z6q;!;F@koYlJ0gHcPtqJ1nBAv?VUxprWa7Z_alb`1sDM!4o%0`IoIs9$jNbgtAL4V zK`{_1*qZtClm=&A>q)dCw}B+&XA%H}7ZYF?7hGdJFNC5eQlLd5i;61&U8UaqYi4K9 z^9SmGuQlDS^kT+>CJZ)gkwdkIP(f33papb_n`W{Q!>AD`bN)-2lAFmJghD^ZwAMqtGh0x6efA4;I?rgp<83|QytbgcsAL>rL}9XZ zE#`dJO+xeDng34GjU&(2fA~!P`aeDWX#g0+irxiyehv@T0)QC?CJLb1wg{4vWjGLz zUk?G;TtwD}yLx9JH-i(9IQ+Xs$kHJWWEnZ839w8^Qwb?~D_izKq$9eWy)JmRXe%=0 z{nM}nW13LP$}`2!Vr z`Nx9LZl=~X91X(XKd13($#!b zP&u*g%OL{MCL~%4HlB6?f7-n*v)CdwPwoLJqPkP+?>)4)O1Ae?(7wn3n@P`9AmG9+ z!F%WB1xJ3SYKT7CN|W&%mAi~%hd8AZd_QBK6zPCpa~@xrM_k&De*FuHeu?j#4m*Z0 z1KTA_rKEw`Bo58{P0`&{ll0XgcQ3c`xrjba-TJBY8y?(#LuY5aDZ7?uD*Rh;I(^~O zDN5XWM+OBcXnUm|RhKxdBw7te9QFg6#053|LjacSgDh!=CQj0AKq8A@a_Ih&%cwD( z#Fa+PZBs_dL6my#Y=(t7X)tsZlsku~>qL;ypsU}V8^7QQP{oSRoJGud$shQjCpq<3 zk_Uh55Af)!jOk9V)A&}7uDg#f#lQok)E%0X=}%{nKG_d^QK*DC=r=>dnB>ZqCjw~Q z;350Xa|MwaSl~$hf|gVE%~M=_=;*LCegZN>b%}eJOD}#D#c}VO1J?|Lfl#_LqAgD1U}EwZv4+uWjR_}m5-VM< zOB)AF3S1S3abTM+_iYXLfE;%uoGVWu8L@^(B~2cTq)dey{GGl_4oJg$Nh}2YF+1#y z2LS_r2y5B*-r&#vKeUHV)I!T!2}k`|m}X#Xmezaa&Duv!iSvI>&j z@h`$)x6qshPwokR6Wd3HxNTza?P4U9>xf@fBTlv}cz{b8=5-J+D}bW9ANpht;K@ZW z*d@e*vpRh9YlUpH?^#MCueo`7ukGx_Y3v)2kc4`VJmj8ce{_n^dbXQywAt6_+Hx5a z8_`V|pZ@|Vp7P7PKIf=|7LN0*{bF8Vm`R7Ah8V)CUAC=*>rYD(-cE<%&)@ihK2=XW zWWq3sho%CCZs@)BrVg~p(4}Q$r4BQF^7SzK*kiqBzJcrjIdE^syN5e>fRvcX)QDAs zlH^ADFs5sZOzKWTYJY^khZ|+9FZO36ulS{M!KnuI$&!g7)ko&2OqzGtk|lUQ{%|+0 z2G|&?XySm!c#nPVXZZJLeK)2z^{`XJCk_Alk<^hR9BcIWc4PXF1d3_+b_`5FFPWUn zw&(xK#8Q~k6$+4zO!uJcz6_H7eV?m%;a_{%t9gN`{Ue)7B^wUYw%)#(>eHxQmJRZV z#6uFX^g+=|ha6h-NA5|+{oj?qkZO=8{1il-IRUN*DY_M{g%t*})W=PflgK&lPT{TI z33bphR6vr1?@9lqZxv<7@ zn_ZxE&tsdYMQ_72L`eCCAi)U2T{9Edp%cBF=h~eDt#QMAQGBB+STbV6Av|O=6l0raZk3_rN{&B9W|yAXD+tj-;nZQfHhaP0`tHZEn|hlw zTLbB6>wfr+Eogga0?zaEijoCSm7WwJyTF%TP+2T?#0#w-7z;Z$M2prv(iD{NrYbnm zYxEe8HVq5hc?x7~v53z4vs*H!&@S!3)7J?)i|(w}$&7@1XAWhH?x36PL6mbUa{bQ>F&y)(>RJDO2 zJL0+I#*R6*@6NuBd-s)p8pVlZL{&a;&ER8hI=)*_Y3dU;bm7puJz53l3d=Kj6J_WBoQbVoKeWRJG8*N_rd` z95P#dUWnDThVKl7>aPQmg)CzWQ$So9sLfFE7uwwpVM0g3^ZH4QuyC|4kghjhVc!$I`a*>zckm)$?=iIjiPXJMx;Cj zG5vG1{a2s^_{jS|hr|jcfgXbNI>Ix1`U|CCN${UR5pfs;aPpLc&N8szz=QDNgGbm% zzxWgHO&%g)GljM1O!^;l)yr=B=VW4M#U#~<98_G;r6@|Wr1=%J3(PucNGXJss+_de z9N_}7=+a~BS^n^II%eTedsHIofI(ppsV>|l9OfZm71P*Dz4>1Wbg8pj*wUKg=w#;* zR)4}NlcER(njw>w#FaA%D*-%fNemPo5t>Xuhzt~x8f$kzu>p@N16f7-gkF@1I(&go zfuPIFzP&&HHIJ~sCRLtF2&t3A)m=?7Jqu2?OZSNaq>)EZzR|`QY^#oLdR+p^6wC%g zfmiV^Db;4Hij_mShlNMyjI!&*tEs=J%Lt8V*B$oCBg?J5LCUGG^8UWc!lhLq+%S<2 zyj>Ym&ox&R=#hUTDj@}0S65@>-B}Bt^lW3KpI-!s(If+vx5*&XS07SbN1O2UT4gEt zxw_sbhq1LDymm=E6?4g`4_xjGvH(S;pJ_JKqpzJ*Yf{$@O@?;(+e z2?azI_m8sC;W`b_&iw2CVshyf_$r77U}^dWmF!eR|1GG4z+x8+Q$)QhE#~W1+sT5|>6bXIK!J(-S=C0_>>@GYA*J%;Zd@SO8+ap-Qr zE-b{A)B~gFw5*6!KXv{dfjSrE$^B-*TuI5b>~#gSKY99&jImKSoLgb#7z6rq(7dsu zbIL)$Je*}8Fs#INtI4DFs0y6hQWHeB5SJb6|03~_A^=iuE6Y;$V`g-!2xLsQ~?lu&_<&{cS)xj22xs^e@5K zMRwt(HFVpJhli-LZ^z4c**e543v|Z~hh+Y%d4IbEh*+AiC3Ce|(SW4IJvBqA;>7cvnZr*gor4lZ$A{y`tG?{RI_5^7tt^WPBt&~qKqDb#J9Yncw1hkXH6 zvPv?ymggz;9SaRYpCMSF*QM{M*j5wE{;wME^J;1C^e?End#$fTY6Y^bQP@fVOep)o6Liw^M*)n*i_8Kz1|6U7mMP zCPo_&RKkx1J9_pdgin-HI? zX`dCOOiFEwf%LAG&J-Nd`?M&hU`i&^j6fEPa?G&6r$*@R^e^`9TAPb^e8jL^bG@Yc zzaJE&gL|M7QLKu_D19;4)g-#Cz}cXY5?aJd@_WI z!ompJQyV#-bFl#=$DzEWI2*(+p<$zWC0A64W1^Dk5mo!QmPPDSf4SmSxI|RTT#Md5 zY4XSf-VRJ!V_+<=@zFM(?WJO1$>QaksywQ*WGHVvBIUX*LGQLW-?JyJa!+@>iX5Jcz8BsB?MNLxQSY5dsDoAhb)iKWfPR3K zj&V`>soXENO{WYs{TXEkz&T7j8EAI)Ih!l^z$Jj0`sxWBMXf=Op?pMBO`i}~!<{C9 zkGmj9sER;^N5u}r%DdABECGBzmLqZh<~Y=_z+ZF3OlIyu}D1GSo5kS->y*aT+Syzo5I62pdT}n-9S!}_6 z+9$XoD@gKa@D^idEiEgZ0fvy``(l#1^k*rUg=nC^wKIkHiv((I4pFH!E1F*Ev{{tz z=Ew7}W4-_XRQFawVeGBW4;KAC(S3mUwcc_sj~MHnA-I72nP_`;Yx09o;{Sz;7p0qoBrEgS!U zoTajw>Wnm&VQ`9&il&%o5JF0A@)|X4vOjmtJ*a)eC!vN4KfwN}bUn)cf$D!$N7sZz z%#A|P{?EKiD#r&3BU4wt{maQ%8!yGX0w8b|QVZ|DRMYyoU}b>uZQV5oK*(x@I;-V| zXLDbM-(|Cm?-^#P%3usbgC$hec#4BJFuOPJzs%59GX^Ldsmg@5@gRaS$mcy*kr?2Z)M!mUp+;KxlbSe-R{~nw zUQ*_Qbl9<Bqjm#$;t@fz4F_bwk(qtJWan%%c`W1 zD1h`D7Ul~!p%uBs@4`d6v~I^v_WwUP7l?Or0vTGAcD_i)&NQWlTtt%&y;_|SY%vU)GIOZL z5l%ho;<1!Oz_Y9dWW8EWs3`Q7v}7`B%d8V6`1}Q2L!&sq)V5V=0W|~-#Cs0;09Vls zHteGcb8zIGGYvD;+_Me?o4?|L$HCc8#|*iY1{{d@#2UZr_3}%VKYRNHnyp?cz{r3A zok{BTm>GV$){j%$NChKRu}}ePz_X|ZT^bu~#n!(8s{6%R<)=Le&}y81Sa;V|X-6e8 zRrpYW5kP~~w;Ix}!T3ZL=DccOMR^3L9D*c1K&1h0?t@h)im&bblUiPcPdp`9h=}XI z)|chM=Dp<~N-^{W$|Nb$766H*76q2?B)g7|-^E)#d9e{S#4cQ*8cej3GY6oe{=T9( z?pa*mk{J;BOsk9bZ_wXT6$S$=ka(KpTc`b64s_lnE&Q@)#fOJq)(9D@$xaWmX75ns zQ}TQpF|MTu9#bYpmW0qEqxnO{jnt%mL~%luD2trau+xV{k1;n0_?W!4Bu$=F^)OH^ z8^8ghMNJ(&Ze`#;uX@)V)U0@+h%;AK`P}V#C8d9E`TH$-Oau$Wgn_fWrtfC}r#4Ud zq{`9Oezgc++e1npCGh)FpF`1<==e%CjGX|)4JEjMJl7EIB z<}0};LIj$T)5NZL8A>8n->dk93Xl*@SB5|; zpYSB4)^$hqt2M9*v92!{OLre--#Uix)kKs_zy?MTZD-c(Uv3E9r{*Njqv!(|5SWAL zmdtD_{EiaK`rU@qPgE??!G4MM{4_++ZAL7gPEvexZ#tWCE zusowVi7eL0nlU=DD$=*F+UKy<=`wbFFbScYh5;rSAMj68sC|@~5x?Q5?*&?35W$w( zp&@qIr;FsvaT;3QNO{Ef42ZXt`%ni?Knx{$Q@M;HKui}8OT@R=PCv;HNcCe36;&2t z16u@oNwNx2k z)d&@oZC+iBK7@~yMlmviiUO%$ban`Ne?z5?UBB#;m)t=1_*ht9qV9K@B2+;6zh>_P zH^$Ss^VIz_#R{UAI$pW~=J1^GI=1OU@u{-Fq7f=5hl*r0Y|z}01MGDOA2B{lwFdX`vo7m25pPbOI^|E-WXX zrQt3WSGO2Xvgnu!ilQ+b2jHfze_R83tA&qEUT9*~A0-@EEu?qFE0c(FyZwamOtG8WcaQ zxwM~ZWGsS!qy79df6^DubZXLe*Z}bP{4Q?1QpX!uUpxVt>c}+W&EX~aW<{a84j|z%C@p)m`nZtQfI^EqVnE!# zh)dQeRfutz){Hj%#4Q|wXTz;|!?mytT2yzDWva}vi%h3(xWyKw<^wBnjOMX(8z7L~ z%*5A9p(+3t!92~rM8j{pY#Sb&gTFnmf)w}`M3X~q0gjwyq`e>8W>S;ZX2qtxv{91i z4g)J>MX!G9yD14R+9dg)kuvgXPbmlJD|euboP?8b53UFYXm{N(%pY+6}~X$hOy^(6_rI-kUIo~=zZAI*N{vJdDjLApPE2u{wyi9~YBKhsY*Xa>p! zgNKpdD2#-y{wxGVlBNsuiE!}RDC{v9!^1U9qQxw5;jLRD0FG2N6`b8(NkX9PlFwc8rvI%J=q}x}c1din*N3Fyy#N?cmot(sjt;l+p%cQbQh-kft z!ZqArOgs>`++Z_T3}JPu({w71Llv{Gj?3sB#1|B5 z`zrfF`a#dVJtZs}V1KZLbAq>Wp7b`bIK|AHwMoSDR*l{4EyT#fy44<*uTy>7zqFhY z?(w6q7iHA@`wk6MmcElxMJ($#@3n=^NdrGfx7+69o%#a`$xj);wcyGSlHm3Mm*>LW zYvxY1NYQ;yjCLx5T-*cXttd#Ub%oFVvLlp>|wTc(9DGnHX|Z{io>TQ zbLaDidN=poJ{#t;&!mSDUL?NvhPZ1`P;kYLV!ehh>8d#EV)%;c*CC%p^%R$i=aQ+_ zB6G$CR&mEWY31*$EP8Bx(B{tUgUZD3P94Jo=!KGsZJs?;a0(Dj!)B_oAc$0n+IVB? zbVIPY5vCpkppnkP+y*X6IqTjZ!En@oju8R+A*`O8JZpkE?Fw@??B#}r_)P^=O4I;} zGI&86rG72Bm;O5K`VpBii`fWF=B)j^t4Sj*Pv>X5Fr&b0zJ##l&oE>74DN8yJjHB?7jUr~oXadHm0cBsOn#EBCG04%@8GNjDFtnKpYodf4; z3SkJmAvef@hDMD&ats@8EoiGluEPX&YPqGn>7GJ3Q&5|SpeSl5N!7)8Cl_Brtlp6} z_^$uqYh+QL#ngWc){B+Fi`8N4;L=U#Ee4Im1uVD`FUl?dw*8rPQP{U^#xbBC2esyS zFj7mIQ<4iUqQT^~Q`sCY*yoka$g7Q$n1fi_R)BmR8W7%b-J&`B`$#)ZY17p_S2DisWtLYrE|pr9cZwnp7_r@=K}|9_7bCxeyjfIres57m~ZFVCXm{;F^@~z2Wjgiz>{Y?eoLo&@F41BQt&S#Z|Yh$$x?NPm!*s4&gT>IpP}B`8IEQt*AZIC6$`{RL19E%kYkFyGY^R)|O8XzzsUbET+$ z(ff2ofTVAB;~XNRQAc(C;m5D=%t}>BtsMSrsiTS-Di95j$WDg`0ZZzaivJy2v|K%y zUEO1qo?VzJV4JJh$9`gWCvb3YjWno4h>6)pmEOz~eAB>h6|(`g+qB#+nl~@|4KyO+ zL#dH~Zy<6tN*rKF4iUHcNjILI-V`*?X57-MsBpoYM%-kkq#&sRp9R%!enOBK03`2|F;!&1iNWOk`-ouC#yu2l+kGL=PWyr0u|vORliOTF)e! z{z-Q1sYGiNBA%fndQ`3-H4}ODe@LD79c+id8===J*pRY|+^kO=KgVL86OJds~ zGnX~U!lr|%CikcOKN6FKE`;1_Sl~dRFe8T~i-3v~n-iqr{>fmHxX%nO7YZrOZ^!?t z*sj^11jehVmA*o)m4D~8sgv5Ozk{(54D4t4FAbE@ScK=dxo;^%wO!e%!khS|2A}Ue z;zvlk3}6q(p?n+_g)}W&O7p+VkwGw=CQhLhR%y?jA0Zb^(xf*Y7NnLVE5cMT?kUDs zPfSdhm?qR+_BTBN7vPOZnZDT&dB_$bAd8R#ELrvoDoJ=tJlkvHe&5N&S^@I!%qDrz zN7U&nvW+5QV=68Qo6(;x46w4efTBGT5nNAS|C#v;thYYW0^vSoW*r{ad%WWDXHXk9 z%%DnKgI5PDeS>Oo!-pzfGAfI=CG7d% z+p`2J1dXr~%?#t|5=6FH~JtpG?}l-X5EpXiIwUwSs#2A2vDDBBcmP17y6}6 zs`4MP107%xrqEXP0>ER%qHX)2vMF_ZMQILMXeb@L+inu8p^liYhSb#soK&zcuw=T} z5u0P62eOJvXH!Fz56gYtSvaRY zeuC{{+)|17-{ztQ(D*|#=VBb$ea)m82H?a_+X|V^88Nkci)2Ss*!_Y zCAI(Y#{LI2`w#l|A9(KnL%MyR^nX7@c7#agRT>~x!=ZpEh!(hVOln_2urZNYMsBeh z3j}%sG5u_Y+>pLjYi*`dk5hkiv{LrL(&tdIh6Xi?8vr z%{q1dWvJaX#%fa3Fqj+3nv6CF#_0+M|63{9+TTu6X8aFyra`0tTgMbb=Z@T$)5FxD zlaOTgx8$L+Q1ll?u=tiu!mBgw zK;f3dqfGY>57St-*1Y#-B{{NkB8%?|f2)49#QYye*`np30v)e>u!!#WhKdR1Nv_Og zJpH76Ub6|N>3L?-YTSpIpS+p4ST;T~P)@l&EJ9sFRsDUx9ALq82*dI>4SGK|j?h8| z8(e4vx&jR-Jf*B0Ka^3_blBsmvV?k&ASk#AUnw*1KMubPB{j{TT+Cc=_~p)psrsUP z@k&v@(SHI02F^V7&OI;^&}ktJ&I+0TKEA-@=%rB54MkxrQeuUOPyxxQeNll5-g%bN zZD8gn#Rf2qA6^2XlihA!NQpzp!mpRsE|FenQR2Re1OA$G8-`Pp79E zJ29h0ZLtk70E2F|%-rW;(-gtuBgJLec{^MO;msSyv8LW}&&tn8*7j+VG74h_MRC!W zT_;Q$h1{2pi~Y!OPFEu`E1knO7Sl2dsTn6-iceToBE0$r9Qzx9qc3frBd-s48?ig2 z9~Wsm7J7=B@=^_i!AWoo)T>D6Sx{>6E-OJ2TZzu{J>>Ua6z2XIbhoGkiF<-%=nl#2 zL8G03`fLQ_v}nbs-kuY95({-9Pqst0!b~&DNh#<%I~iB}@TF65hQl@J!6QtW6`z1x zKQSE>?GFC=*-dj_U%9I32It>-X{ciVM4&I@J_Fhkn@UMtDC?XRECO259tJ<1?m^1C zu$I2WIbISRyLO_Z+s?l~%Rj9mQCMGxA+cndvXAZlF_&QnIWCD#zmBxIfeX6!GEXCt z-!5X%%0{3qDX7;G7L4oloW^I7PmY@b+(To-wDFK$RnEW26;+BYeov;n|>O>G)8*`$u5ijXxePn?!HvO z54LEWAyMJwhqZZGFZNL$yK@DHIog0Bu>z^DhSfmejaqqQao3=b8JO;R{^% zE7oP|t7~VHQ#_bn^VwL68Fc6Y#iKn26irtW<}A()ro77H?XSgJx1-=@P2{V;jppNf zq(dqPY6z>x{N!)f6I=f=1aibhqeoSI|s z7k?x=eE!m8g4cYxDm%Bzdc%ZvE%$H6lH1XV;p;+YU^@OD0+Th_Y&BgmHaw&FpMg2v zECyXAGr(0#UHg%~ZnRPi0g6;pjRlg=!iPOkGk#u=CG@TlIGz*gg4%8@uv18r#2FPb z?JrW+0>*t;ti1r=RHfFQS5^1rtQYRp9pC&8hfPMm(Pj2oyS$K`@CnI(F=A;;Idh+> z&=oy>b>VU1sW)5K6G%{3bz4MMG46?AGUb|}h`G3CV}DWhF6^5(`T|Sn)`zQ=f7D#z zg-u`Zrf65)*ioG&-@4eKyHf#V^}}=Z^UM5mdR17b#H6JWcWBE%Wc5vkB@8Up+$FB4 z*~i)tnI0wr$(sD_eD`aT&=w$m_#s`KJC1waEhz4T>`jU&m|S6kydkg@)-(O{1b z-uVC*R$5oU=~n$+#>Lx(f*8|Bj?Y?Owt&JcpJXn1$~bdgq&Pg4s7@$ChwV-p<2fzTS|t4@D&N7T;@|;tO+coO zY9b)52=@n*J}+n8w`I9~{QIzfNR=-r$m<4)TAUD6ml4nPL%r68za5BML)n$qRF|A3 z5b9tv?-6Z!4F30aeq8*Zs5t#ZpT5qh-QbOE-?SB2Iwhk2>oo+xQr@h~7y~7Vgc%ce zI{md9*0eG8s{b(c1+hWjFR78P;ta@qW_7?uWrC{H#Tgf%?~*A`s(aJ#css@-v_rtq zC=0muby#~zAZZKLao8#ozbZfXQ3&GBnqm%`WN{z(xNtKk{G@%or9n36B6rj(M74?f z9O!SCKZU{`7vf%@w0Q073RQ-FLt{>6_!ySs+~a7R1hI(UCBC#vA)a*=eO2R2c7^if z720^@^o=UHhbbcX9JtW-I0C6!$q`?MzBXuV9t)U4o#`*HAQM@{V_GM?f3zhSmdRQd zXFd!shAXS+FH}qi;GK*#Fc`H2SnYr4gV06owYJmK)cZ}*o2FsQM*e@7I zoM=VlW3(9_x%(*6#C-jLPUXvmjqW>vt)g+5yOy(prTPn ztuwmkDDbgEBEEoSq1JucimGIUCHPlsOT8JXYc>0C6XIf^^CL&ZOW!k5g?$taIdTWS z|2CV>lP-g29g9Tl>yCr@h~~H~$^wMsEJ{$E>;7eyx+;aT=JavO!(T6w{B-!`{xhF%VcYuOv|na-Az5tuXKkyZvZ}SOl;lBxtTxpJc;3`NE2Pg4KBQd{lIL8I>U1^W-Xm2DCd0C{V*K5 z9(ONc9}Dr0)z|&c(0E-S{V>C|xRMvlOU<>R$27~;(b~~wTWE4@vvclWls6MH^y$pP zUdd``gqS|C<%BT<(NtH_;W!uFS|6Vl(n*wOhtW}cu}r<4+Ira9HrX@S%zgU>?W&4_ z4+W$vhUcME=6h;kS?cs>?#+zGXs0I=gGZ3DAklRZT$@y>~+BKdA;-O-<~JE!&OmweECE?h991Dr^i4nYa6Ck?kc)7H#U6Ya?u{3w+zX?NIkeO~DD}%f zk@R{;!dbAY5Th%IuE1m$;oP8G{tF#GXXlCskpR9yv{lknoltwEvZ~viLFhtvu!0ZF zc~@{hwcd6?+GFV|&hwD-DM7tb`tdpf5zcd%TY)tqO(vKo$LMC-9PjB|6R-H3bp%H{ zF_p?@#NH~INAeZ55JI%#XZ#YpnV(3GVy7&0br3UL!3yC5Fe&jJ2G!wSP}=GP;G8}D z?dbsF@h6hnh!)oKE)71yGtg1v>-k$$!`GKmd3p(5&Qeu2g&vc54y3&$z0wK-*s9Gm zJN~H)c9L%WMm4(s9LjahhN9qId^djkMG*wPVMc+D?Gta)jN8TJEpl{k>;#4Wt#b3{ zHh*hga`E}FUco&`0Ce=!zGr{?H?r;)l5awZijnXi%fa!esdZ)V2w7gMvcg!71b9HU zhJ~MfDLUIBxUI=ypu3d*$?mYjqdEP%Xc z=cYnL(WswAkOrk!2Nvj)(6u?9MInagpYn_bq*7?$!d}&S-zrGd5%4_mB38ZA)j@8#f|rdf~(5`eLPd?W?wPfgR^| z5S(=Phwe<(%JT0&DwRpa&e^#y=-2Ulb|aE;ED8^ z$9McBxo}o`R4o9a4oQ0v08^Sb$l#K_=gZ7y=DXUQMf;|ul7cMj48bdssFbojyEPYF z2-->ZD|^*D_Zm9toH(7BFD27Mjne0-!Ye8%s1u>aR#uH1OdXs8A%<{%3zUI^n>WLwafbH4TqZ}JcK z90FT8Rcjjz5)0z;>`#kMr`6@kC34wZuEBsYJjQ^0_@VRtB+ClT8AofyCH2(>_8s{H zq0k#V2JhWYzd)%~WFs`QPwxJdLHa?sR@Y7I{({*TD(x$unUme!EKIV~&T6}M(@ko7 zIPQi9pFY;iavX2tV}zv7g;9jl0q#;)aD*x<}&WDlu{O6&eD0XB3hAijJq6 z$CXw6J1_UJxnNujgJfhIROf|L@aoC4AhMK|wAAFJ2PJUB^3}rzG=hBn?~+<%SK3Q+ zH#I70J(MWvy%G*vR9rL|Kuap@zSR*LycO z!E2~p`PzD3gvJCF#L!1d&mm_@+!-t$0z88L%JP9^lZJS(1eZWlKPW}>Dk?K#0spdC z>Qzk$9;IQO)l==FHRvF>Ums0njG+!zM)`d-^ECg=-32^tUHcKa$WY{_8^!>Ix?Sx&8s@rEh_qF<3p+q~Twcs(_g(7Z8(r7TR zT}wL~tMG^>Crc}`l9=kP|W6sD){S`tVA0*gW3n zaiHCyzasLr5cyQ%J|DSV-ruHw{SfG&51P94Iu@=DX!Rqe!czu&%Rx0(B|d@ew2ie% z^cTnh-sJ~8KnB2XMDd{~u#IRRS6fp_W7NNH8dH0y;2L^Z>Jf?4{xqck~b9OV7^rTPs_*LM) z{FD7e`=l#q{%V$^hXMDC5WV%FUIRi2hLt4KJ(y83!k*B*rlFaL zvZ2`AvCdymRXy+;{eyELt0;u!cQU87Sw{A;m9u2Tuz*A3V`20s93RTS;l!w+2ALuP z0lRAl1`KU->I3vKXY}=7x!h*PLKgB86$7=kb0xZ1E+<8*n>W7?#6$@w@O_m6LNw?$ zQc%guQQE(JquIWl;zB|hLA|oxAHxH(_k?|r?>&##CS^i$*cq4<`+pP z0}}dNYST$VD z8{vE8ciHHeEds)hFGZFxJuy(=4~16fy^oQMW>2mWAhord6gIa!o{RKKn-&G9IApD+ z2B*|#)eiRa#nK;rIl}3%x{{gbk{1+-SXsyd6B8Yotpr3? zMZ&!hK156FEehhv#8r0rUZff=G&9?!%@t|aV1ZnfbV7jEMseAcT1FLpK&srrYYmDB@MM?QZEB*PtR7ERMfp`6Cs;ClE< zRE20FK{U<7{x}O;uMgof(D!R7ysj3jKN#EwSRW+z64&c`LU_Jd zI}Bw|#EXEYG%Vy;6r=%p`Wq4;PhgG6E}P?bswedlm26;K(VS;Fes(|zq#x|COCRQ3 z)1e`e(GEYH!2mJ2EZnaDi8P|_3zAZqbti3`)`?4`qZ8SNv9=stOQsk4N7=|{Bavso zNSYt?wPi&{E~GtBK2|tKjG1|;K0wp;oZ&%#Whns3kXwn1W8(Q`FSIXgoLc$ZR|v>! zMRhAc(fZ_geW91xKmr?-j#Q}%B{>owyxvZ{;`$G5XYEf}gLOI2Wu~i^vqr?WU!$B) z&yI4@~l1niAaart6V~?--hx5+8%E>C!EDLek1K~DGTUd1%-BE1PxtfTt zEilGUz{&tiHfLe!3ThI=KPXOuBGLX#f2)3fo*pyc8aYVJ#RwN3n3`^>g=yE2gdgyD z3x|^p-WRS%6(u|&k|z7q@+8R_aSgg{w#<@z?Z0{nOy~(5Db+=fn;aw~;2U>!#2HKg zMYfpW;Sds?{=nlf+Y-giy@N$%^7*+Uc5#E4;gGiq(1oGb!cJI0#b3ib&?i^jW}90J z;c&^o_5sT?txUvVg(6Oci82wQe@1J}NKx2s5#2?pfM*{-;+4Tmd27D0Fy#5e;=yPe z!MA;6igar~94CI=#G$p(bFe<45CfG*cy<4{r4{;+&bHv1#{ z=QKXO%HKIAh;s|@?XADOD1)o66yqme!b4PinTE6k&va(GtCEZ3m>1DC{dLX#*|9K` zjFqn)LuWJ3&B2Z)h>TVM)@7pRK+2iC82@8VTsOjjvIPof0`~)3nt|a(wbZFH&36Xd z7jv3~0)n`&QJhS-99Sf5AG?#*yEc#A!8tpxpSY2q)3%nHg1WKTSdu#BapgH~=Qu}I z5n_5l>5qSI%k;Gg<+r}y>l^&rm@$$j@X`}k%RT^l2`7U?1FL@kb}m_3@-=gZb^9!J zF5@8JdONswac0sa4oq*JUk~l7T5fUH7kNzK)Ajp;vLoj7Yn)!WhfP8FU*63Ut@n%H zj~m4OB<8va7{xuTRGtDdNQ6cT1~T(t=_mX{E?XenrCtoh57)abU`i^GsXj=8yXj$D zTZ!v0Ue9&)B|9-2yAGO;YE27eLK>8v(n?uI$y}@u1POIsT1?mDkLnflFE_{Cgpv3k zZY;ArP$06k1FfwMKncbk( zvJ%>GK5C>}<~8~b!-^=sb+j7|q{R#SKg2T-mK9;Lk9ZLrmW@7?WvMbuK+Q7@#GstP z>PJGvnO_)Z!eg@C^dz+QuO`y2d>WV`_NESy&eCCX_P;z0BE`f5yWioUB;S8k z+Z(AI1Hx;pC8wtKdV`LIheNq_N1vdaUv@4PxmU6GY5z|3@=^YIPs&|+J%DQWL5^Xw z4ZSpqu){f4AUpA)Bw7x}ip_CZDn2^0FX2hu^ktR|dfIaNy1NtZ+y|Kq1a6RGT^Q$m!Vbe|9jTsh~y8TKI z5rtb|CS9)cEK10CWf&mO-Oor7lMndkUAg7IiGQ+&*WQbC$}toCw3JUmas^$FTQW|w z7gAo^zmmBFPG}d$&pp?j#8er0b=8I}w!LK4?21>uypwQC^vMISzvO(n){0;kJU^vt|MqzaDc*KcqW)OVAFRdd)w^8B&cyZZ9M_nw!w~Oq_p7eMS`ln>V6)fNM0VAH^dL$kR-`iX;pmooYa}Zs^t>1WJ4C$Q3 zjVRX;`giAN8$GEE60ZE&up~~Z?M;OucMIvwPu*W)*gL92gYD}uzJ}AZbKLX4i-Lc+ zdngM;WaASOI0;dsw&vUTZbBdPLjjU=O z$9N|~8Uv?ASxG4W8R=3Zk*wBZgWh!}8SF6xQA&sjGwKgA9D4MsWQ{yW!dhY)-^Uq9 z4aW0Iz*F2cMf372LhH@Jq8KOFuI8D9zBG;bP!BwSUNk6gOEr=z0sc>Ddmkoebrxf6 zijZg$6U}*@4xBR7G2I6fjg{pm#&caeV$67-ojxpC5lZNIgmpc5C~x?OavaX1imdAB z*$6H9u4SvL$K8BMuT2%y3}@|c#oSc2N`qSyBy%=A#Io2i{-CM#kd90t&{n*I4p`Bz z`h|lc7*(KGlN2yknGs_7I&YEN_M0=EXPuTrVS1e4P*X7`- z0W=*y>yBl&e(op-{JtPLUbj19(+3dF@t^psDz15U$5|Og+Z``oA0&a|gqYne;$)vA zO$lwh1vLd8&#CXe=}B-Ov@9O0O}orO5ID^2LpO4P9lQ7-wXc(N)tsKOKIQy z0Hc$!#o@Z!+TG7w!Dg9`>j7*{4$bW%Sv^TITk^jRYMYm<1Se}|D&8|z!P(_n=b~MVrAC73x@zf+vy!u-qlgd-*ppM2 z^jI#7gk|hbspVgwzyTX?*>(@PfDd@3Y46i#>9tIo_a4*$%|CQ4FLWM!lTI}TE;p7I z%DD^+eClz8iVo&LQ8ZO&T*?s}tW+s=R6D@jwPk~nuHx$k z8zQ(u3<4k*Qd=+sUw>)m4a^>Dh>&);nhfrhV$^K&PbgKYG;>YMi@VM#ysx78VuFNd z8X$YuFH}*^iE;-Csq7VxNnQ)xYK@&}k*21k%ybpGt(9gw;ERNEA6H@*2`%4b((C>V zPZ8MN9<2e{MZCz#Zi!KTOGeaTc1vbn8|UJr^nEzEOVzG=6ZVkN$bTld9XH9-C8n8s z6r)|6YjhkMb0&>NJ}&U85L#{Qg9=Gl;q)PJMI!OX=UJ*KT6IiW-=f2n0a{T-9T~kD zLcP8;Dfcy$ZWvFT`g(|e7`7C+lo5d0);d~ZEHy6(-8{Pk)Qf;N`~0!g1V8$cx? zY_Dd&1WDp`7R;TK{7U9%)|-Md870nCh9wwjW2xWQhJ(-h(UUAh>)TfrmixYo*=W~T z1;04$G%9M0NykqI$^H9GtD5`k@A(a&f}?hwQNbPv87y`B&gfA4y8bqG+)uhhh0b^2 zR)9#baNSq5oc-hWnW}Obdz2e@j8k60@)YO%M6h*oY)!>r@r1?7A*GLRwRU*DyO=IrK8qqF10@KBi56ZnU-% zCB&0LC#=br^F2xzf^v>EmSG-UELOsnG}*;5PP$CDKDMv zVj+SCRWRJS$fhcqdzIV^u_VE~U}9g!yhbHv$jmnT_j9*DA@KWK+OJ#?CJ4m~#T3Eu zdV{%~s}7jAK0ig0HzPtYD7>UuIGd<#NZ;{@ZlfueJne6{`4b%Ty6pd;K@i_P*Y*?Y zg{lWi;$nBOPRCDF>gl{b-Al>lczq4IKON}vB$BmIE869T)MCg=aalr4)>~nvncXjs zwYi?{Zz5bRW_wQ^(6?~jf@f^hs1!w;-%ab?(-Uj{b0Ts7^9&3#3;F(uw!7P;a=k?L=cBk;pw!p|M_`4`9KR<=?} zV$I*=q zwUTb$&gu2_2}l~b$#lByE9!y&%yT4Z2;c)kdZ0RR2&_P92kWgJy7P4*AfdbWypu`s z)rN-}pZmjknYJBTIB%Y{A8JL>;E4e1z1P815@Rg>S0;iG#lMx76(84r1-js+ zypJkD%d~*N>}dAR9(1sawd8_kl;8()x1yP1C+^c4*muKL4A-sI1`|En!Ev*^cF3lq zZ;4Y0cX58!`1IF2r&a;R@{F7ft=0?9%Iy8xb+mj@5s73Y!JmM=xCQSnX!EGKnL#>H zeYl}o;Y=mAc49|0!JJNh9mVy00~Gl+!VkM`b)Z?1x|-?&{&AKrF@;KLfqve~WSqdmg>VG1n9XjarxlL*vjCgOAOW>Uh0)qCfm7~xG|njUdHyR zN6wrfxm*8rM=b7u>|2N#NdSMN$@E5msHo%uW=XO@fqWx%m#hR%Ajv{8RDCiiGYSG2 v6lidOU=NW2G6Mh`0{;J@!)o2$UvVZ9lXpsN?cwACcPNYi6An5-9::new()); let subscriber = session.declare_subscriber(key).res_async().await?; - let listen = Self::listening_task( task_running.clone(), &accumulating_resources, @@ -124,7 +123,8 @@ impl AlohaSubscription { beacon_period * 3, &accumulating_resources, on_resource_undeclared, - ).fuse(); + ) + .fuse(); join!(listen, listen_timeout); @@ -144,7 +144,11 @@ impl AlohaSubscription { { while task_running.load(Relaxed) { match subscriber.recv_async().await { - Ok(val) => match accumulating_resources.lock().await.entry(val.key_expr.into()) { + Ok(val) => match accumulating_resources + .lock() + .await + .entry(val.key_expr.into()) + { Occupied(mut val) => { val.get_mut().update(); } @@ -172,9 +176,13 @@ impl AlohaSubscription { + 'static, { while task_running.load(Relaxed) { - accumulating_resources.lock().await.iter_mut().for_each(|val| { - val.1.reset(); - }); + accumulating_resources + .lock() + .await + .iter_mut() + .for_each(|val| { + val.1.reset(); + }); async_std::task::sleep(accumulate_period).await; diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs index ad6233b..92bf601 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs @@ -12,9 +12,19 @@ // ZettaScale Zenoh Team, // -use std::sync::atomic::Ordering::SeqCst; +use rosrust::RawMessage; +use std::process::Command; +use std::sync::atomic::AtomicBool; +use std::sync::atomic::Ordering::*; +use std::sync::{Arc, Mutex, RwLock}; use std::{net::SocketAddr, str::FromStr, sync::atomic::AtomicU16}; use zenoh::config::ModeDependentValue; +use zenoh::sample::Sample; +use zenoh_core::{AsyncResolve, SyncResolve}; + +use super::discovery::LocalResources; +use super::ros1_to_zenoh_bridge_impl::{work_cycle, BridgeStatus, RosStatus}; +use super::{ros1_client, zenoh_client}; pub struct IsolatedPort { pub port: u16, @@ -59,3 +69,362 @@ impl IsolatedROSMaster { format!("http://localhost:{}/", self.port.port) } } + +pub async fn wait(waiter: Waiter, timeout: core::time::Duration) -> bool +where + Waiter: Fn() -> bool, +{ + let cycles = 1000; + let millis = timeout.as_millis() / cycles + 1; + + for _i in 0..cycles { + async_std::task::sleep(core::time::Duration::from_millis( + millis.try_into().unwrap(), + )) + .await; + if waiter() { + return true; + } + } + false +} + +pub struct RunningBridge { + flag: Arc, + + ros_status: Arc>, + + bridge_status: Arc>, +} +impl RunningBridge { + pub fn new(config: zenoh::config::Config, ros_master_uri: String) -> RunningBridge { + let result = RunningBridge { + flag: Arc::new(AtomicBool::new(true)), + ros_status: Arc::new(Mutex::new(RosStatus::Unknown)), + bridge_status: Arc::new(Mutex::new(BridgeStatus::default())), + }; + async_std::task::spawn(Self::run( + ros_master_uri, + config, + result.flag.clone(), + result.ros_status.clone(), + result.bridge_status.clone(), + )); + result + } + + async fn run( + ros_master_uri: String, + config: zenoh::config::Config, + flag: Arc, + ros_status: Arc>, + bridge_status: Arc>, + ) { + let session = zenoh::open(config).res_async().await.unwrap().into_arc(); + work_cycle( + ros_master_uri.as_str(), + session, + flag, + move |v| { + let mut val = ros_status.lock().unwrap(); + *val = v; + }, + move |status| { + let mut my_status = bridge_status.lock().unwrap(); + *my_status = status; + }, + ) + .await; + } + + pub async fn assert_ros_error(&self) { + self.assert_status(RosStatus::Error).await; + } + pub async fn assert_ros_ok(&self) { + self.assert_status(RosStatus::Ok).await; + } + pub async fn assert_status(&self, status: RosStatus) { + assert!( + self.wait_ros_status(status, core::time::Duration::from_secs(10)) + .await + ); + } + pub async fn wait_ros_status(&self, status: RosStatus, timeout: core::time::Duration) -> bool { + wait( + move || { + let val = self.ros_status.lock().unwrap(); + *val == status + }, + timeout, + ) + .await + } + + pub async fn assert_bridge_empy(&self) { + self.assert_bridge_status(BridgeStatus::default).await; + } + pub async fn assert_bridge_status BridgeStatus>(&self, status: F) { + assert!( + self.wait_bridge_status(status, core::time::Duration::from_secs(120)) + .await + ); + } + pub async fn wait_bridge_status BridgeStatus>( + &self, + status: F, + timeout: core::time::Duration, + ) -> bool { + wait( + move || { + let val = self.bridge_status.lock().unwrap(); + *val == (status)() + }, + timeout, + ) + .await + } +} +impl Drop for RunningBridge { + fn drop(&mut self) { + self.flag.store(false, Relaxed); + } +} + +pub struct ROSEnvironment { + ros_master_port: u16, + rosmaster: Option, +} +impl Drop for ROSEnvironment { + fn drop(&mut self) { + if let Some(mut child) = self.rosmaster.take() { + if child.kill().is_ok() { + let _ = child.wait(); + } + } + } +} +impl ROSEnvironment { + pub fn new(ros_master_port: u16) -> Self { + ROSEnvironment { + rosmaster: None, + ros_master_port, + } + } + + pub fn with_master(mut self) -> Self { + assert!(self.rosmaster.is_none()); + + let rosmaster_cmd = Command::new("rosmaster") + .arg(format!("-p {}", self.ros_master_port).as_str()) + .stdout(std::process::Stdio::piped()) + .stderr(std::process::Stdio::piped()) + .spawn(); + + match rosmaster_cmd { + Ok(val) => { + self.rosmaster = Some(val); + } + Err(e) => { + println!("Error while starting rosmaster: {}", e); + panic!("{}", e); + } + } + + self + } + + pub fn without_master(mut self) -> Self { + assert!(self.rosmaster.is_some()); + let mut child = self.rosmaster.take().unwrap(); + child.kill().unwrap(); + child.wait().unwrap(); + self + } +} + +pub struct BridgeChecker { + ros_client: ros1_client::Ros1Client, + zenoh_client: zenoh_client::ZenohClient, + pub local_resources: LocalResources, + + pub expected_bridge_status: Arc>, +} +impl BridgeChecker { + // PUBLIC + pub fn new(config: zenoh::config::Config, ros_master_uri: &str) -> BridgeChecker { + let session = zenoh::open(config).res_sync().unwrap().into_arc(); + BridgeChecker { + ros_client: ros1_client::Ros1Client::new("test_ros_node", ros_master_uri), + zenoh_client: zenoh_client::ZenohClient::new(session.clone()), + local_resources: LocalResources::new("*".to_string(), "*".to_string(), session), + expected_bridge_status: Arc::new(RwLock::new(BridgeStatus::default())), + } + } + + pub async fn make_zenoh_subscriber( + &self, + name: &str, + callback: C, + ) -> zenoh::subscriber::Subscriber<'static, ()> + where + C: Fn(Sample) + Send + Sync + 'static, + { + self.zenoh_client + .subscribe(Self::make_zenoh_key(&Self::make_topic(name)), callback) + .await + .unwrap() + } + + pub async fn make_zenoh_publisher(&self, name: &str) -> zenoh::publication::Publisher<'static> { + self.zenoh_client + .publish(Self::make_zenoh_key(&Self::make_topic(name))) + .await + .unwrap() + } + + pub async fn make_zenoh_queryable( + &self, + name: &str, + callback: Callback, + ) -> zenoh::queryable::Queryable<'static, ()> + where + Callback: Fn(zenoh::queryable::Query) + Send + Sync + 'static, + { + self.zenoh_client + .make_queryable(Self::make_zenoh_key(&Self::make_topic(name)), callback) + .await + .unwrap() + } + + pub async fn make_zenoh_query_sync( + &self, + name: &str, + data: Vec, + ) -> flume::Receiver { + self.zenoh_client + .make_query_sync(Self::make_zenoh_key(&Self::make_topic(name)), data) + .await + .unwrap() + } + + pub fn make_ros_publisher(&self, name: &str) -> RAIICounter> { + let status = self.expected_bridge_status.clone(); + status.write().unwrap().ros_publishers.0 += 1; + status.write().unwrap().ros_publishers.1 += 1; + RAIICounter::new( + self.ros_client.publish(&Self::make_topic(name)).unwrap(), + move || { + if let Ok(mut locked) = status.write() { + locked.ros_publishers.0 -= 1; + locked.ros_publishers.1 -= 1; + } + }, + ) + } + + pub fn make_ros_subscriber( + &self, + name: &str, + callback: F, + ) -> RAIICounter + where + T: rosrust::Message, + F: Fn(T) + Send + 'static, + { + let status = self.expected_bridge_status.clone(); + status.write().unwrap().ros_subscribers.0 += 1; + status.write().unwrap().ros_subscribers.1 += 1; + RAIICounter::new( + self.ros_client + .subscribe(&Self::make_topic(name), callback) + .unwrap(), + move || { + if let Ok(mut locked) = status.write() { + locked.ros_subscribers.0 -= 1; + locked.ros_subscribers.1 -= 1; + } + }, + ) + } + + pub fn make_ros_client(&self, name: &str) -> RAIICounter> { + let status = self.expected_bridge_status.clone(); + status.write().unwrap().ros_clients.0 += 1; + status.write().unwrap().ros_clients.1 += 1; + RAIICounter::new( + self.ros_client.client(&Self::make_topic(name)).unwrap(), + move || { + if let Ok(mut locked) = status.write() { + locked.ros_clients.0 -= 1; + locked.ros_clients.1 -= 1; + } + }, + ) + } + + pub fn make_ros_service(&self, name: &str, handler: F) -> RAIICounter + where + F: Fn(rosrust::RawMessage) -> rosrust::ServiceResult + + Send + + Sync + + 'static, + { + let status = self.expected_bridge_status.clone(); + status.write().unwrap().ros_services.0 += 1; + status.write().unwrap().ros_services.1 += 1; + RAIICounter::new( + self.ros_client + .service::(&Self::make_topic(name), handler) + .unwrap(), + move || { + if let Ok(mut locked) = status.write() { + locked.ros_services.0 -= 1; + locked.ros_services.1 -= 1; + } + }, + ) + } + + pub fn make_topic(name: &str) -> rosrust::api::Topic { + rosrust::api::Topic { + name: name.to_string(), + datatype: "bool".to_string(), + } + } + + pub fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { + return topic.name.trim_start_matches('/').trim_end_matches('/'); + } +} + +pub struct RAIICounter +where + T: Sized, +{ + pub data: T, + on_destroy: Box, +} + +impl RAIICounter +where + T: Sized, +{ + fn new(data: T, on_destroy: F) -> Self + where + F: Fn() + Sync + Send + 'static, + { + Self { + data, + on_destroy: Box::new(on_destroy), + } + } +} + +impl Drop for RAIICounter +where + T: Sized, +{ + fn drop(&mut self) { + (self.on_destroy)(); + } +} diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs index dd4edcf..8d1f9ca 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_mapping.rs @@ -22,7 +22,6 @@ pub struct Ros1TopicMapping { pub published: HashSet, pub subscribed: HashSet, pub serviced: HashSet, - pub clients: HashSet, } impl Ros1TopicMapping { pub fn topic_mapping( @@ -46,7 +45,6 @@ impl Ros1TopicMapping { published: HashSet::new(), subscribed: HashSet::new(), serviced: HashSet::new(), - clients: HashSet::new(), }; Ros1TopicMapping::fill(&mut result.subscribed, &state.subscribers, topics); diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs index e32bec1..9490a49 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/topic_utilities.rs @@ -19,16 +19,6 @@ pub fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { } pub fn make_topic(datatype: &keyexpr, topic_name: &keyexpr) -> Result { - match datatype.as_str() { - "*" | "**" => return Err("incorrect datatype!".into()), - _ => {} - } - - match topic_name.as_str() { - "*" | "**" => return Err("incorrect topic name!".into()), - _ => {} - } - let mut name = topic_name.to_string(); name.insert(0, '/'); Ok(rosrust::api::Topic { diff --git a/zplugin-ros1/tests/aloha_declaration_test.rs b/zplugin-ros1/tests/aloha_declaration_test.rs index 39ea7ac..e9f68cf 100644 --- a/zplugin-ros1/tests/aloha_declaration_test.rs +++ b/zplugin-ros1/tests/aloha_declaration_test.rs @@ -164,7 +164,6 @@ impl DeclarationCollector { let r2 = r2.clone(); let k_owned = OwnedKeyExpr::from(k); Box::new(Box::pin(async move { - let st = k_owned.to_string(); assert!(undeclared.lock().await.remove(&k_owned)); assert!(r2.lock().await.remove(&k_owned)); })) diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index c0b84c5..39cc51b 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -21,147 +21,25 @@ use std::{ str::FromStr, sync::{ atomic::{AtomicBool, AtomicU64, Ordering::*}, - Arc, Mutex, RwLock, + Arc, }, }; -use zplugin_ros1::ros_to_zenoh_bridge::ros1_to_zenoh_bridge_impl::{ - work_cycle, BridgeStatus, RosStatus, +use zplugin_ros1::ros_to_zenoh_bridge::test_helpers::{ + BridgeChecker, ROSEnvironment, RunningBridge, }; use zplugin_ros1::ros_to_zenoh_bridge::{ - discovery::{LocalResource, LocalResources}, - test_helpers::{IsolatedConfig, IsolatedROSMaster}, + discovery::LocalResource, + test_helpers::{IsolatedConfig, IsolatedROSMaster, RAIICounter}, }; -use zplugin_ros1::ros_to_zenoh_bridge::{ros1_client, zenoh_client}; use log::{debug, error, trace}; use rosrust::{Client, RawMessage}; use std::sync::atomic::AtomicUsize; use zenoh::prelude::r#async::*; -use std::process::Command; use std::{thread, time}; -async fn wait(waiter: Waiter, timeout: core::time::Duration) -> bool -where - Waiter: Fn() -> bool, -{ - let cycles = 1000; - let millis = timeout.as_millis() / cycles + 1; - - for _i in 0..cycles { - async_std::task::sleep(core::time::Duration::from_millis( - millis.try_into().unwrap(), - )) - .await; - if waiter() { - return true; - } - } - false -} - -struct RunningBridge { - flag: Arc, - - ros_status: Arc>, - - bridge_status: Arc>, -} -impl RunningBridge { - pub fn new(config: zenoh::config::Config, ros_master_uri: String) -> RunningBridge { - let result = RunningBridge { - flag: Arc::new(AtomicBool::new(true)), - ros_status: Arc::new(Mutex::new(RosStatus::Unknown)), - bridge_status: Arc::new(Mutex::new(BridgeStatus::default())), - }; - async_std::task::spawn(Self::run( - ros_master_uri, - config, - result.flag.clone(), - result.ros_status.clone(), - result.bridge_status.clone(), - )); - result - } - - async fn run( - ros_master_uri: String, - config: zenoh::config::Config, - flag: Arc, - ros_status: Arc>, - bridge_status: Arc>, - ) { - let session = zenoh::open(config).res_async().await.unwrap().into_arc(); - work_cycle( - ros_master_uri.as_str(), - session, - flag, - move |v| { - let mut val = ros_status.lock().unwrap(); - *val = v; - }, - move |status| { - let mut my_status = bridge_status.lock().unwrap(); - *my_status = status; - }, - ) - .await; - } - - pub async fn assert_ros_error(&self) { - self.assert_status(RosStatus::Error).await; - } - pub async fn assert_ros_ok(&self) { - self.assert_status(RosStatus::Ok).await; - } - pub async fn assert_status(&self, status: RosStatus) { - assert!( - self.wait_ros_status(status, core::time::Duration::from_secs(10)) - .await - ); - } - pub async fn wait_ros_status(&self, status: RosStatus, timeout: core::time::Duration) -> bool { - wait( - move || { - let val = self.ros_status.lock().unwrap(); - *val == status - }, - timeout, - ) - .await - } - - pub async fn assert_bridge_empy(&self) { - self.assert_bridge_status(BridgeStatus::default).await; - } - pub async fn assert_bridge_status BridgeStatus>(&self, status: F) { - assert!( - self.wait_bridge_status(status, core::time::Duration::from_secs(120)) - .await - ); - } - pub async fn wait_bridge_status BridgeStatus>( - &self, - status: F, - timeout: core::time::Duration, - ) -> bool { - wait( - move || { - let val = self.bridge_status.lock().unwrap(); - *val == (status)() - }, - timeout, - ) - .await - } -} -impl Drop for RunningBridge { - fn drop(&mut self) { - self.flag.store(false, Relaxed); - } -} - #[test] fn env_checks_no_master_init_and_exit_immed() { let roscfg = IsolatedROSMaster::default(); @@ -690,58 +568,6 @@ impl PingPong { } } -struct ROSEnvironment { - ros_master_port: u16, - rosmaster: Option, -} -impl Drop for ROSEnvironment { - fn drop(&mut self) { - if let Some(mut child) = self.rosmaster.take() { - if child.kill().is_ok() { - let _ = child.wait(); - } - } - } -} -impl ROSEnvironment { - pub fn new(ros_master_port: u16) -> Self { - ROSEnvironment { - rosmaster: None, - ros_master_port, - } - } - - pub fn with_master(mut self) -> Self { - assert!(self.rosmaster.is_none()); - - let rosmaster_cmd = Command::new("rosmaster") - .arg(format!("-p {}", self.ros_master_port).as_str()) - .stdout(std::process::Stdio::piped()) - .stderr(std::process::Stdio::piped()) - .spawn(); - - match rosmaster_cmd { - Ok(val) => { - self.rosmaster = Some(val); - } - Err(e) => { - println!("Error while starting rosmaster: {}", e); - panic!("{}", e); - } - } - - self - } - - pub fn without_master(mut self) -> Self { - assert!(self.rosmaster.is_some()); - let mut child = self.rosmaster.take().unwrap(); - child.kill().unwrap(); - child.wait().unwrap(); - self - } -} - struct TestEnvironment { pub bridge: RunningBridge, pub checker: Arc, @@ -810,194 +636,6 @@ impl TestEnvironment { } } -struct RAIICounter -where - T: Sized, -{ - pub data: T, - on_destroy: Box, -} - -impl RAIICounter -where - T: Sized, -{ - fn new(data: T, on_destroy: F) -> Self - where - F: Fn() + Sync + Send + 'static, - { - Self { - data, - on_destroy: Box::new(on_destroy), - } - } -} - -impl Drop for RAIICounter -where - T: Sized, -{ - fn drop(&mut self) { - (self.on_destroy)(); - } -} - -struct BridgeChecker { - ros_client: ros1_client::Ros1Client, - zenoh_client: zenoh_client::ZenohClient, - local_resources: LocalResources, - - pub expected_bridge_status: Arc>, -} -impl BridgeChecker { - // PUBLIC - pub fn new(config: zenoh::config::Config, ros_master_uri: &str) -> BridgeChecker { - let session = zenoh::open(config).res_sync().unwrap().into_arc(); - BridgeChecker { - ros_client: ros1_client::Ros1Client::new("test_ros_node", ros_master_uri), - zenoh_client: zenoh_client::ZenohClient::new(session.clone()), - local_resources: LocalResources::new("*".to_string(), "*".to_string(), session), - expected_bridge_status: Arc::new(RwLock::new(BridgeStatus::default())), - } - } - - pub async fn make_zenoh_subscriber( - &self, - name: &str, - callback: C, - ) -> zenoh::subscriber::Subscriber<'static, ()> - where - C: Fn(Sample) + Send + Sync + 'static, - { - self.zenoh_client - .subscribe(Self::make_zenoh_key(&Self::make_topic(name)), callback) - .await - .unwrap() - } - - pub async fn make_zenoh_publisher(&self, name: &str) -> zenoh::publication::Publisher<'static> { - self.zenoh_client - .publish(Self::make_zenoh_key(&Self::make_topic(name))) - .await - .unwrap() - } - - pub async fn make_zenoh_queryable( - &self, - name: &str, - callback: Callback, - ) -> zenoh::queryable::Queryable<'static, ()> - where - Callback: Fn(zenoh::queryable::Query) + Send + Sync + 'static, - { - self.zenoh_client - .make_queryable(Self::make_zenoh_key(&Self::make_topic(name)), callback) - .await - .unwrap() - } - - pub async fn make_zenoh_query_sync( - &self, - name: &str, - data: Vec, - ) -> flume::Receiver { - self.zenoh_client - .make_query_sync(Self::make_zenoh_key(&Self::make_topic(name)), data) - .await - .unwrap() - } - - pub fn make_ros_publisher(&self, name: &str) -> RAIICounter> { - let status = self.expected_bridge_status.clone(); - status.write().unwrap().ros_publishers.0 += 1; - status.write().unwrap().ros_publishers.1 += 1; - RAIICounter::new( - self.ros_client.publish(&Self::make_topic(name)).unwrap(), - move || { - if let Ok(mut locked) = status.write() { - locked.ros_publishers.0 -= 1; - locked.ros_publishers.1 -= 1; - } - }, - ) - } - - pub fn make_ros_subscriber( - &self, - name: &str, - callback: F, - ) -> RAIICounter - where - T: rosrust::Message, - F: Fn(T) + Send + 'static, - { - let status = self.expected_bridge_status.clone(); - status.write().unwrap().ros_subscribers.0 += 1; - status.write().unwrap().ros_subscribers.1 += 1; - RAIICounter::new( - self.ros_client - .subscribe(&Self::make_topic(name), callback) - .unwrap(), - move || { - if let Ok(mut locked) = status.write() { - locked.ros_subscribers.0 -= 1; - locked.ros_subscribers.1 -= 1; - } - }, - ) - } - - pub fn make_ros_client(&self, name: &str) -> RAIICounter> { - let status = self.expected_bridge_status.clone(); - status.write().unwrap().ros_clients.0 += 1; - status.write().unwrap().ros_clients.1 += 1; - RAIICounter::new( - self.ros_client.client(&Self::make_topic(name)).unwrap(), - move || { - if let Ok(mut locked) = status.write() { - locked.ros_clients.0 -= 1; - locked.ros_clients.1 -= 1; - } - }, - ) - } - - pub fn make_ros_service(&self, name: &str, handler: F) -> RAIICounter - where - F: Fn(rosrust::RawMessage) -> rosrust::ServiceResult - + Send - + Sync - + 'static, - { - let status = self.expected_bridge_status.clone(); - status.write().unwrap().ros_services.0 += 1; - status.write().unwrap().ros_services.1 += 1; - RAIICounter::new( - self.ros_client - .service::(&Self::make_topic(name), handler) - .unwrap(), - move || { - if let Ok(mut locked) = status.write() { - locked.ros_services.0 -= 1; - locked.ros_services.1 -= 1; - } - }, - ) - } - - // PRIVATE - fn make_topic(name: &str) -> rosrust::api::Topic { - rosrust::api::Topic { - name: name.to_string(), - datatype: "some_datatype".to_string(), - } - } - - fn make_zenoh_key(topic: &rosrust::api::Topic) -> &str { - return topic.name.trim_start_matches('/').trim_end_matches('/'); - } -} - #[derive(PartialEq, Eq, Hash, Display)] enum Mode { Ros1ToZenoh, From 1489785c495a61cfb4a86efd6f187aeb36692cdb Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 24 Apr 2023 18:00:32 +0300 Subject: [PATCH 18/19] Update README.md --- README.md | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index f5d935d..a9ab342 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,21 @@ -# zenoh-plugin-ros1 -Plugin to integrate ROS1 to Zenoh + + + +[![Discussion](https://img.shields.io/badge/discussion-on%20github-blue)](https://github.com/eclipse-zenoh/roadmap/discussions) +[![Discord](https://img.shields.io/badge/chat-on%20discord-blue)](https://discord.gg/2GJ958VuHs) +[![License](https://img.shields.io/badge/License-EPL%202.0-blue)](https://choosealicense.com/licenses/epl-2.0/) +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) + +# Eclipse Zenoh +The Eclipse Zenoh: Zero Overhead Pub/sub, Store/Query and Compute. + +Zenoh (pronounce _/zeno/_) unifies data in motion, data at rest and computations. It carefully blends traditional pub/sub with geo-distributed storages, queries and computations, while retaining a level of time and space efficiency that is well beyond any of the mainstream stacks. + +Check the website [zenoh.io](http://zenoh.io) and the [roadmap](https://github.com/eclipse-zenoh/roadmap) for more detailed information. + +------------------------------- +# ROS1 plugin + +Under construction \ No newline at end of file From c67e2880c1a61b1c24233c5f8cb9924745f30da8 Mon Sep 17 00:00:00 2001 From: yellowhatter Date: Mon, 24 Apr 2023 19:01:32 +0300 Subject: [PATCH 19/19] Move test's params to test_helpers.rs --- .../src/ros_to_zenoh_bridge/test_helpers.rs | 32 ++++++++++ zplugin-ros1/tests/ping_pong_test.rs | 60 +++++-------------- 2 files changed, 47 insertions(+), 45 deletions(-) diff --git a/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs b/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs index 92bf601..9983cee 100644 --- a/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs +++ b/zplugin-ros1/src/ros_to_zenoh_bridge/test_helpers.rs @@ -428,3 +428,35 @@ where (self.on_destroy)(); } } + +pub struct TestParams; +impl TestParams { + pub fn many_count() -> u32 { + Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 10) + } + + pub fn pps_measurements() -> u32 { + Self::env_var("TEST_ROS1_TO_ZENOH_PPS_ITERATIONS", 100) + } + + pub fn pps_measure_period_ms() -> u64 { + Self::env_var("TEST_ROS1_TO_ZENOH_PPS_PERIOD_MS", 1) + } + + pub fn data_size() -> u32 { + Self::env_var("TEST_ROS1_TO_ZENOH_DATA_SIZE", 16) + } + + // PRIVATE + fn env_var(key: &str, default: Tvar) -> Tvar + where + Tvar: FromStr, + { + if let Ok(val) = std::env::var(key) { + if let Ok(val) = val.parse::() { + return val; + } + } + default + } +} diff --git a/zplugin-ros1/tests/ping_pong_test.rs b/zplugin-ros1/tests/ping_pong_test.rs index 39cc51b..7bad74f 100644 --- a/zplugin-ros1/tests/ping_pong_test.rs +++ b/zplugin-ros1/tests/ping_pong_test.rs @@ -18,7 +18,6 @@ use zenoh_core::{bail, zresult::ZResult, SyncResolve}; use std::{ collections::HashSet, - str::FromStr, sync::{ atomic::{AtomicBool, AtomicU64, Ordering::*}, Arc, @@ -26,7 +25,7 @@ use std::{ }; use zplugin_ros1::ros_to_zenoh_bridge::test_helpers::{ - BridgeChecker, ROSEnvironment, RunningBridge, + BridgeChecker, ROSEnvironment, RunningBridge, TestParams, }; use zplugin_ros1::ros_to_zenoh_bridge::{ discovery::LocalResource, @@ -504,8 +503,8 @@ impl PingPong { async fn start_ping_pong(&self) { debug!("Starting ping-pong!"); let mut data = Vec::new(); - data.reserve(TestEnvironment::data_size() as usize); - for i in 0..TestEnvironment::data_size() { + data.reserve(TestParams::data_size() as usize); + for i in 0..TestParams::data_size() { data.push((i % 255) as u8); } self.pub_sub.publisher.put(data.clone()); @@ -522,7 +521,7 @@ impl PingPong { async fn measure_pps(&self) -> f64 { debug!("Starting measure PPS...."); - let duration_milliseconds = TestEnvironment::pps_measure_period_ms(); + let duration_milliseconds = TestParams::pps_measure_period_ms(); let mut result = 0.0; let mut duration: u64 = 0; @@ -600,40 +599,11 @@ impl TestEnvironment { } } - pub fn many_count() -> u32 { - Self::env_var("TEST_ROS1_TO_ZENOH_MANY_COUNT", 40) - } - - pub fn pps_measurements() -> u32 { - Self::env_var("TEST_ROS1_TO_ZENOH_PPS_ITERATIONS", 100) - } - - pub fn pps_measure_period_ms() -> u64 { - Self::env_var("TEST_ROS1_TO_ZENOH_PPS_PERIOD_MS", 1) - } - - pub fn data_size() -> u32 { - Self::env_var("TEST_ROS1_TO_ZENOH_DATA_SIZE", 16) - } - pub async fn assert_bridge_status_synchronized(&self) { self.bridge .assert_bridge_status(|| *self.checker.expected_bridge_status.read().unwrap()) .await; } - - // PRIVATE - fn env_var(key: &str, default: Tvar) -> Tvar - where - Tvar: FromStr, - { - if let Ok(val) = std::env::var(key) { - if let Ok(val) = val.parse::() { - return val; - } - } - default - } } #[derive(PartialEq, Eq, Hash, Display)] @@ -709,7 +679,7 @@ async fn ping_pong_duplex_parallel_many_( if mode.contains(&Mode::FastRun) { vec.push(ping_pong.run(1)); } else { - vec.push(ping_pong.run(TestEnvironment::pps_measurements())); + vec.push(ping_pong.run(TestParams::pps_measurements())); } } @@ -733,7 +703,7 @@ fn ping_pong_zenoh_to_ros1_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([Mode::ZenohToRos1]), )); } @@ -752,7 +722,7 @@ fn ping_pong_ros1_to_zenoh_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([Mode::Ros1ToZenoh]), )); } @@ -771,7 +741,7 @@ fn ping_pong_ros1_service_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([Mode::Ros1Service]), )); } @@ -790,7 +760,7 @@ fn ping_pong_ros1_client_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([Mode::Ros1Client]), )); } @@ -824,22 +794,22 @@ fn ping_pong_all_sequential_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([Mode::ZenohToRos1]), )); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([Mode::Ros1ToZenoh]), )); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([Mode::Ros1Service]), )); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([Mode::Ros1Client]), )); } @@ -864,7 +834,7 @@ fn ping_pong_all_parallel_many() { let env = TestEnvironment::new(); futures::executor::block_on(ping_pong_duplex_parallel_many_( &env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([ Mode::ZenohToRos1, Mode::Ros1ToZenoh, @@ -878,7 +848,7 @@ async fn main_work(env: &TestEnvironment, main_work_finished: Arc) { assert!(!main_work_finished.load(Relaxed)); ping_pong_duplex_parallel_many_( env, - TestEnvironment::many_count(), + TestParams::many_count(), HashSet::from([ Mode::ZenohToRos1, Mode::Ros1ToZenoh,