search

ROBOTIS-GIT / ros2arduino

This library helps the Arduino board communicate with the ROS2 using XRCE-DDS.
Apache License 2.0
218 stars 43 forks source link

UDP Error with ESP32 #40

Closed Nyarn-WTF closed 4 years ago

Nyarn-WTF commented 4 years ago

Hello.

Do you know how to solve Serialization Error?

[1585565543.693715] error    | InputMessage.cpp   | log_error                | deserialization error  | buffer: 
0000: 12 00 15 00 80

Log of MicroXRCEAgent udp -p 50000 -v 5

[1585565377.806545] info     | UDPServerLinux.cpp | init                     | running...             | port: 50000
[1585565377.806875] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 5
[1585565386.816984] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 148
[1585565489.217035] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 161
[1585565526.388516] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 24
[1585565526.388740] info     | Root.cpp           | create_client            | create                 | client_key: 0xAABBCCDD, session_id: 0x81
[1585565526.388876] info     | UDPServerBase.cpp  | on_create_client         | session established    | client_key: 0xAABBCCDD, address: 192.168.0.5:47138
[1585565526.389089] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 19
[1585565526.389726] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 24
[1585565526.390014] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 19
[1585565526.390921] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 24
[1585565526.391164] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 19
[1585565526.397325] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 92
[1585565526.405395] debug    | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0xAABBCCDD, participant_id: 0x001(1)
[1585565526.405659] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 14
[1585565526.406720] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.448114] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.448427] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.550214] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.550444] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.710746] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.751156] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.751537] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.754546] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.755890] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 24
[1585565526.756041] debug    | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0xAABBCCDD, publisher_id: 0x001(3), participant_id: 0x001(1)
[1585565526.756253] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 14
[1585565526.756311] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.757689] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.760046] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.761250] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 24
[1585565526.761392] debug    | ProxyClient.cpp    | create_subscriber        | subscriber created     | client_key: 0xAABBCCDD, subscriber_id: 0x001(4), participant_id: 0x001(1)
[1585565526.761571] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 14
[1585565526.761638] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.773095] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.774128] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 120
[1585565526.774466] debug    | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0xAABBCCDD, topic_id: 0x005(2), participant_id: 0x001(1)
[1585565526.774641] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 14
[1585565526.774697] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.778140] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.779304] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 168
[1585565526.780362] debug    | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0xAABBCCDD, datawriter_id: 0x001(5), publisher_id: 0x001(3)
[1585565526.780538] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 14
[1585565526.780665] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.786724] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.787970] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 120
[1585565526.788477] debug    | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0xAABBCCDD, topic_id: 0x014(2), participant_id: 0x001(1)
[1585565526.788682] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 14
[1585565526.788751] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.791973] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.793276] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 172
[1585565526.794063] debug    | ProxyClient.cpp    | create_datareader        | datareader created     | client_key: 0xAABBCCDD, datareader_id: 0x001(6), subscriber_id: 0x001(4)
[1585565526.794313] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 14
[1585565526.794411] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.797745] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 13
[1585565526.800867] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 40
[1585565526.801340] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.801519] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.823034] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.823262] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.823439] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.841715] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.841966] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.842125] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.862498] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.862635] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.862713] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.884190] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.884626] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.884889] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.903686] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.903806] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.903900] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.925575] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.925684] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.925781] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.944740] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.944959] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.945120] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.965496] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.965726] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.965942] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565526.994352] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565526.994461] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565526.994580] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.006737] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 20
[1585565527.006934] debug    | DataWriter.cpp     | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 5
[1585565527.007044] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.043987] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 40
[1585565527.044123] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.060395] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 60
[1585565527.060533] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.081579] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 80
[1585565527.081713] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.101302] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 100
[1585565527.101390] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.166950] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 113
[1585565527.167056] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.615070] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 126
[1585565527.615144] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 139
[1585565527.615175] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 152
[1585565527.615329] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.615432] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565527.615508] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.218624] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 165
[1585565528.218879] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.231726] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 178
[1585565528.232087] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.340701] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 191
[1585565528.340982] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.409201] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 204
[1585565528.409410] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.639222] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 217
[1585565528.639305] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 230
[1585565528.639499] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.639569] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.849346] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 243
[1585565528.849752] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.851281] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 256
[1585565528.851600] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565528.915866] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 269
[1585565528.916053] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.349860] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 282
[1585565529.350034] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 295
[1585565529.350077] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 308
[1585565529.350217] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.350287] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.350322] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.417789] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 321
[1585565529.418263] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.663316] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 334
[1585565529.663389] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 347
[1585565529.663711] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.663783] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.873116] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 360
[1585565529.873197] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 373
[1585565529.873369] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.873442] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565529.942222] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 386
[1585565529.942494] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.009111] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 399
[1585565530.009364] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.079007] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 412
[1585565530.081310] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.172733] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 425
[1585565530.172981] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.273994] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 438
[1585565530.274211] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.482613] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 451
[1585565530.482919] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 464
[1585565530.483236] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.483312] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.580255] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 477
[1585565530.581036] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.681325] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 490
[1585565530.681733] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.892336] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 503
[1585565530.892454] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 516
[1585565530.892671] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.892761] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565530.986503] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 529
[1585565530.986922] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.199382] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 542
[1585565531.199480] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 555
[1585565531.199632] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.199751] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.293638] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 568
[1585565531.294058] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.361083] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 581
[1585565531.361335] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.609065] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 594
[1585565531.609245] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 607
[1585565531.609503] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.609616] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.701252] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 620
[1585565531.701520] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.802207] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 633
[1585565531.802416] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565531.916201] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 646
[1585565531.916581] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565532.008606] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 659
[1585565532.016531] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565532.109723] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 672
[1585565532.109866] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565532.208168] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 685
[1585565532.208343] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565532.325681] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 698
[1585565532.325848] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565532.411759] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 711
[1585565532.411897] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565532.477744] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 724
[1585565532.478001] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565533.656995] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 737
[1585565533.657100] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 750
[1585565533.657134] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 763
[1585565533.657273] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565533.657350] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565533.657402] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565534.578354] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 776
[1585565534.578634] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 789
[1585565534.578812] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565534.578996] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565534.596183] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 802
[1585565534.596574] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565535.602533] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 815
[1585565535.602651] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 828
[1585565535.602688] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 841
[1585565535.603100] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565535.603194] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565535.603249] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565535.675592] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 854
[1585565535.675828] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565535.769826] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 867
[1585565535.770157] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565535.870890] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 880
[1585565535.871242] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565535.973118] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 893
[1585565535.973386] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565536.216853] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 919
[1585565536.217121] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565536.296499] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 932
[1585565536.296730] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565536.380743] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 945
[1585565536.381081] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565536.482514] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 958
[1585565536.482769] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565536.626192] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 971
[1585565536.626593] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565537.650114] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 984
[1585565537.650447] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565537.703151] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 997
[1585565537.703478] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565537.855258] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1010
[1585565537.855543] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565538.161041] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1023
[1585565538.161392] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565538.349967] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1036
[1585565538.350053] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1049
[1585565538.350315] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565538.350359] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565538.418119] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1062
[1585565538.418446] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565538.674527] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1075
[1585565538.674651] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1088
[1585565538.674926] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565538.675028] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565539.593669] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1101
[1585565539.593975] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565539.597932] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1114
[1585565539.598241] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565539.806004] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1127
[1585565539.806133] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1140
[1585565539.806486] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565539.806620] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565539.877542] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1153
[1585565539.877656] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565540.614060] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1166
[1585565540.614249] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1179
[1585565540.614653] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565540.614780] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565540.616034] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1192
[1585565540.616471] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565540.684446] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1205
[1585565540.684854] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565540.759639] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1218
[1585565540.760021] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565541.031265] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1244
[1585565541.031988] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565541.235453] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1257
[1585565541.235624] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565541.288376] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1270
[1585565541.288730] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565541.368766] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1283
[1585565541.369149] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565541.645697] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1309
[1585565541.646848] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565541.741830] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1322
[1585565541.742521] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565542.669458] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1335
[1585565542.669642] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1348
[1585565542.669976] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1361
[1585565542.669847] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565542.670638] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565542.671099] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565542.976717] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1374
[1585565542.977103] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565542.999058] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1387
[1585565542.999461] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565543.168706] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1400
[1585565543.168983] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565543.200725] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1413
[1585565543.201051] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565543.301561] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1426
[1585565543.301858] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565543.489150] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1439
[1585565543.489425] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1452
[1585565543.489714] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565543.489817] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565543.693380] debug    | UDPServerLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xAABBCCDD, len: 1460
[1585565543.693638] debug    | UDPServerLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xAABBCCDD, len: 13
[1585565543.693715] error    | InputMessage.cpp   | log_error                | deserialization error  | buffer: 
0000: 12 00 15 00 80

Source code of ESP32(M5StickC)

#include <M5StickC.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <ros2arduino.h>
#include <ros2wr.hpp>
#include <ESPmDNS.h>

const char *ssid = "foo";
const char *password = "bar";

WiFiUDP udp;
const char *host = "cat";

ROS2WR<std_msgs::String, geometry_msgs::Twist> *node;

void MainThread(void*);

void setup(){
    M5.begin();
    M5.Lcd.setTextSize(3);
    M5.Lcd.setRotation(3);
    //Wire.begin(0,26,10000);
    pinMode(10, OUTPUT);

    WiFi.begin(ssid, password);
    while(WiFi.status() != WL_CONNECTED);
    MDNS.begin("esp32.local");
    IPAddress ipa = MDNS.queryHost(host);
    String ipaa = ipa.toString();
    if(ipaa == "0.0.0.0")esp_restart();
    Serial.println(ipaa);

    while(!ros2::init(&udp, ipaa.c_str(), (uint16_t) 50000)){
        delay(1);
    }
    Serial.println("ros start");
    //Serial.begin(1500000);
    //ros2::init(&Serial);

    node = new ROS2WR<std_msgs::String, geometry_msgs::Twist>(50, "m5/pub", "cmd_vel");

    xTaskCreatePinnedToCore(MainThread, "MainThread", 1024*32, NULL, 2, NULL, 1);
}

void loop(){
    Serial.println("Spin");
    delay(1);
    ros2::spin(node);
}

void BrinkLED();

void MainThread(void *pvParameters){
    geometry_msgs::Twist smsg;
    std_msgs::String pmsg;
    uint64_t n = 0;
    while(1){
        do{
            delay(1);
            Serial.printf("%lld\n", n);
            n++;
        }while(!node->getSubscribeMsg(&smsg));
        BrinkLED();
        Serial.println("ok");
        sprintf(pmsg.data, "%lld\n", n);
        node->setPublishMsg(&pmsg);
        delay(1);
    }
}

void BrinkLED(){
    static bool stat = true;
    digitalWrite(10, (int)stat);
    stat = stat ? false : true;
}

uint8_t I2CWrite1Byte( uint8_t Addr ,  uint8_t Data )
{
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    Wire.write(Data);
    return Wire.endTransmission();
}

uint8_t I2CWritebuff( uint8_t Addr,  uint8_t* Data, uint16_t Length )
{
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    for (int i = 0; i < Length; i++)
    {
        Wire.write(Data[i]);
    }
    return Wire.endTransmission();
}

int16_t speed_buff[4] = {0};
int8_t  speed_sendbuff[4] = {0};
uint32_t count = 0;
uint8_t IIC_ReState = I2C_ERROR_NO_BEGIN;

uint8_t Setspeed( int16_t Vtx, int16_t Vty, int16_t Wt)
{
    Wt = ( Wt > 100 )  ? 100 :  Wt;
    Wt = ( Wt < -100 ) ? -100 : Wt;

    Vtx = ( Vtx > 100 )  ? 100 :  Vtx;
    Vtx = ( Vtx < -100 ) ? -100 : Vtx;
    Vty = ( Vty > 100 )  ? 100 :  Vty;
    Vty = ( Vty < -100 ) ? -100 : Vty;

    Vtx = ( Wt != 0 ) ? Vtx *  (100- abs(Wt)) / 100 : Vtx;
    Vty = ( Wt != 0 ) ? Vty *  (100- abs(Wt)) / 100 : Vty;

    speed_buff[0] = Vty - Vtx - Wt ;
    speed_buff[1] = Vty + Vtx + Wt ;
    speed_buff[3] = Vty - Vtx + Wt ;
    speed_buff[2] = Vty + Vtx - Wt ;

    for (int i = 0; i < 4; i++)
    {
        speed_buff[i] = ( speed_buff[i] > 100 )  ? 100 :  speed_buff[i];
        speed_buff[i] = ( speed_buff[i] < -100 ) ? -100 : speed_buff[i];
        speed_sendbuff[i] = speed_buff[i];
    }
    return I2CWritebuff(0x00,(uint8_t*)speed_sendbuff,4);
}

ROS2WR

#ifndef _ROS2WR_
#define _ROS2WR_

#include <Arduino.h>
#include <ros2arduino.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <string.h>

//if QUEUE_SIZE > 2, you can not use xQueueOverwrite()!!!
//also use xQueueSend()
#define QUEUE_SIZE 3

template<typename MsgTx, typename MsgRx>
class ROS2WR : public ros2::Node{
private:
  ros2::Publisher<MsgTx> *_publisher;
  QueueHandle_t command_q, status_q;

  static void subsclibed(MsgRx* msg, void *arg){
    (void)(arg);
    static ROS2WR<MsgTx, MsgRx> *_this = ROS2WR<MsgTx, MsgRx>::thisPtr;
    xQueueSend(_this->command_q, msg, 0);
  }

  static void publishing(MsgTx* msg, void *arg){
    (void)(arg);
    static ROS2WR<MsgTx, MsgRx> *_this = ROS2WR<MsgTx, MsgRx>::thisPtr;
    MsgTx qmsg;
    if(xQueueReceive(_this->status_q, &qmsg, 0) == pdTRUE){
      memcpy(msg, &qmsg, sizeof(qmsg));
    }
  }

public:
  static ROS2WR<MsgTx, MsgRx>* thisPtr;

  ROS2WR(uint32_t pubFreq, String pubTopicName, String subTopicName):Node("esp32"){
    this->thisPtr = this;
    command_q = xQueueCreate(QUEUE_SIZE, sizeof(MsgRx));
    status_q = xQueueCreate(QUEUE_SIZE, sizeof(MsgTx));
    this->_publisher = this->createPublisher<MsgTx>(pubTopicName.c_str());
    this->createWallFreq(pubFreq, (ros2::CallbackFunc)this->publishing, nullptr, _publisher);
    this->createSubscriber<MsgRx>(subTopicName.c_str(), (ros2::CallbackFunc)this->subsclibed, nullptr);
  }

  void setPublishMsg(MsgTx *msg){
      xQueueSend(this->status_q, msg, 0);
  }

  bool getSubscribeMsg(MsgRx *msg){
      if (xQueueReceive(this->command_q, msg, 0) == pdTRUE) return true;
      else return false;
  }
};

template <typename MsgTx, typename MsgRx>
ROS2WR<MsgTx, MsgRx>* ROS2WR<MsgTx, MsgRx>::thisPtr;

#endif
Nyarn-WTF commented 4 years ago

Solved! mDNS was the cause. I could do it with a Static IP.