S Morita / mbed-mros2

Porting mros2 as an Mbed library.

Dependents:   mbed-os-example-mros2 example-mbed-mros2-sub-pose example-mbed-mros2-pub-twist example-mbed-mros2-mturtle-teleop

embeddedRTPS/include/rtps/common/Locator.h@7:c80f65422d99, 2022-03-19 (annotated)

Committer:
smoritaemb
Date:
Sat Mar 19 09:23:37 2022 +0900
Revision:
7:c80f65422d99
Parent:
0:580aba13d1a1 
Merge test_assortment_of_msgs branch.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 0:580aba13d1a1 1 /*
smoritaemb 0:580aba13d1a1 2 The MIT License
smoritaemb 0:580aba13d1a1 3 Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
smoritaemb 0:580aba13d1a1 4 Permission is hereby granted, free of charge, to any person obtaining a copy
smoritaemb 0:580aba13d1a1 5 of this software and associated documentation files (the "Software"), to deal
smoritaemb 0:580aba13d1a1 6 in the Software without restriction, including without limitation the rights
smoritaemb 0:580aba13d1a1 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
smoritaemb 0:580aba13d1a1 8 copies of the Software, and to permit persons to whom the Software is
smoritaemb 0:580aba13d1a1 9 furnished to do so, subject to the following conditions:
smoritaemb 0:580aba13d1a1 10 The above copyright notice and this permission notice shall be included in
smoritaemb 0:580aba13d1a1 11 all copies or substantial portions of the Software.
smoritaemb 0:580aba13d1a1 12 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
smoritaemb 0:580aba13d1a1 13 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
smoritaemb 0:580aba13d1a1 14 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
smoritaemb 0:580aba13d1a1 15 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
smoritaemb 0:580aba13d1a1 16 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
smoritaemb 0:580aba13d1a1 17 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
smoritaemb 0:580aba13d1a1 18 THE SOFTWARE
smoritaemb 0:580aba13d1a1 19
smoritaemb 0:580aba13d1a1 20 This file is part of embeddedRTPS.
smoritaemb 0:580aba13d1a1 21
smoritaemb 0:580aba13d1a1 22 Author: i11 - Embedded Software, RWTH Aachen University
smoritaemb 0:580aba13d1a1 23 */
smoritaemb 0:580aba13d1a1 24
smoritaemb 0:580aba13d1a1 25 #ifndef RTPS_LOCATOR_T_H
smoritaemb 0:580aba13d1a1 26 #define RTPS_LOCATOR_T_H
smoritaemb 0:580aba13d1a1 27
smoritaemb 0:580aba13d1a1 28 #include "rtps/communication/UdpDriver.h"
smoritaemb 0:580aba13d1a1 29 #include "rtps/utils/udpUtils.h"
smoritaemb 0:580aba13d1a1 30 #include "ucdr/microcdr.h"
smoritaemb 0:580aba13d1a1 31
smoritaemb 0:580aba13d1a1 32 #include <array>
smoritaemb 0:580aba13d1a1 33
smoritaemb 0:580aba13d1a1 34 namespace rtps {
smoritaemb 0:580aba13d1a1 35 enum class LocatorKind_t : int32_t {
smoritaemb 0:580aba13d1a1 36 LOCATOR_KIND_INVALID = -1,
smoritaemb 0:580aba13d1a1 37 LOCATOR_KIND_RESERVED = 0,
smoritaemb 0:580aba13d1a1 38 LOCATOR_KIND_UDPv4 = 1,
smoritaemb 0:580aba13d1a1 39 LOCATOR_KIND_UDPv6 = 2
smoritaemb 0:580aba13d1a1 40 };
smoritaemb 0:580aba13d1a1 41
smoritaemb 0:580aba13d1a1 42 const uint32_t LOCATOR_PORT_INVALID = 0;
smoritaemb 0:580aba13d1a1 43 const std::array<uint8_t, 16> LOCATOR_ADDRESS_INVALID = {
smoritaemb 0:580aba13d1a1 44 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
smoritaemb 0:580aba13d1a1 45
smoritaemb 0:580aba13d1a1 46 struct Locator {
smoritaemb 0:580aba13d1a1 47 LocatorKind_t kind = LocatorKind_t::LOCATOR_KIND_INVALID;
smoritaemb 0:580aba13d1a1 48 uint32_t port = LOCATOR_PORT_INVALID;
smoritaemb 0:580aba13d1a1 49 std::array<uint8_t, 16> address =
smoritaemb 0:580aba13d1a1 50 LOCATOR_ADDRESS_INVALID; // TODO make private such that kind and address
smoritaemb 0:580aba13d1a1 51 // always match?
smoritaemb 0:580aba13d1a1 52
smoritaemb 0:580aba13d1a1 53 static Locator createUDPv4Locator(uint8_t a, uint8_t b, uint8_t c, uint8_t d,
smoritaemb 0:580aba13d1a1 54 uint32_t port) {
smoritaemb 0:580aba13d1a1 55 Locator locator;
smoritaemb 0:580aba13d1a1 56 locator.kind = LocatorKind_t::LOCATOR_KIND_UDPv4;
smoritaemb 0:580aba13d1a1 57 locator.address = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, a, b, c, d};
smoritaemb 0:580aba13d1a1 58 locator.port = port;
smoritaemb 0:580aba13d1a1 59 return locator;
smoritaemb 0:580aba13d1a1 60 }
smoritaemb 0:580aba13d1a1 61
smoritaemb 0:580aba13d1a1 62 void setInvalid() { kind = LocatorKind_t::LOCATOR_KIND_INVALID; }
smoritaemb 0:580aba13d1a1 63
smoritaemb 0:580aba13d1a1 64 bool isValid() const { return kind != LocatorKind_t::LOCATOR_KIND_INVALID; }
smoritaemb 0:580aba13d1a1 65
smoritaemb 0:580aba13d1a1 66 bool readFromUcdrBuffer(ucdrBuffer &buffer) {
smoritaemb 0:580aba13d1a1 67 if (ucdr_buffer_remaining(&buffer) < sizeof(Locator)) {
smoritaemb 0:580aba13d1a1 68 return false;
smoritaemb 0:580aba13d1a1 69 } else {
smoritaemb 0:580aba13d1a1 70 ucdr_deserialize_array_uint8_t(&buffer, reinterpret_cast<uint8_t *>(this),
smoritaemb 0:580aba13d1a1 71 sizeof(Locator));
smoritaemb 0:580aba13d1a1 72 return true;
smoritaemb 0:580aba13d1a1 73 }
smoritaemb 0:580aba13d1a1 74 }
smoritaemb 0:580aba13d1a1 75
smoritaemb 0:580aba13d1a1 76 bool serializeIntoUdcrBuffer(ucdrBuffer &buffer) {
smoritaemb 0:580aba13d1a1 77 if (ucdr_buffer_remaining(&buffer) < sizeof(Locator)) {
smoritaemb 0:580aba13d1a1 78 return false;
smoritaemb 0:580aba13d1a1 79 } else {
smoritaemb 0:580aba13d1a1 80 ucdr_serialize_array_uint8_t(&buffer, reinterpret_cast<uint8_t *>(this),
smoritaemb 0:580aba13d1a1 81 sizeof(Locator));
smoritaemb 0:580aba13d1a1 82 }
smoritaemb 0:580aba13d1a1 83 return true;
smoritaemb 0:580aba13d1a1 84 }
smoritaemb 0:580aba13d1a1 85
smoritaemb 0:580aba13d1a1 86 ip4_addr_t getIp4Address() const {
smoritaemb 0:580aba13d1a1 87 return transformIP4ToU32(address[12], address[13], address[14],
smoritaemb 0:580aba13d1a1 88 address[15]);
smoritaemb 0:580aba13d1a1 89 }
smoritaemb 0:580aba13d1a1 90
smoritaemb 0:580aba13d1a1 91 inline bool isSameSubnet() const {
smoritaemb 0:580aba13d1a1 92 return UdpDriver::isSameSubnet(getIp4Address());
smoritaemb 0:580aba13d1a1 93 }
smoritaemb 0:580aba13d1a1 94
smoritaemb 0:580aba13d1a1 95 } __attribute__((packed));
smoritaemb 0:580aba13d1a1 96
smoritaemb 0:580aba13d1a1 97 inline Locator getBuiltInUnicastLocator(ParticipantId_t participantId) {
smoritaemb 0:580aba13d1a1 98 return Locator::createUDPv4Locator(
smoritaemb 0:580aba13d1a1 99 Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], Config::IP_ADDRESS[2],
smoritaemb 0:580aba13d1a1 100 Config::IP_ADDRESS[3], getBuiltInUnicastPort(participantId));
smoritaemb 0:580aba13d1a1 101 }
smoritaemb 0:580aba13d1a1 102
smoritaemb 0:580aba13d1a1 103 inline Locator getBuiltInMulticastLocator() {
smoritaemb 0:580aba13d1a1 104 return Locator::createUDPv4Locator(239, 255, 0, 1, getBuiltInMulticastPort());
smoritaemb 0:580aba13d1a1 105 }
smoritaemb 0:580aba13d1a1 106
smoritaemb 0:580aba13d1a1 107 inline Locator getUserUnicastLocator(ParticipantId_t participantId) {
smoritaemb 0:580aba13d1a1 108 return Locator::createUDPv4Locator(
smoritaemb 0:580aba13d1a1 109 Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], Config::IP_ADDRESS[2],
smoritaemb 0:580aba13d1a1 110 Config::IP_ADDRESS[3], getUserUnicastPort(participantId));
smoritaemb 0:580aba13d1a1 111 }
smoritaemb 0:580aba13d1a1 112
smoritaemb 0:580aba13d1a1 113 inline Locator getUserMulticastLocator() {
smoritaemb 0:580aba13d1a1 114 return Locator::createUDPv4Locator(
smoritaemb 0:580aba13d1a1 115 Config::IP_ADDRESS[0], Config::IP_ADDRESS[1], Config::IP_ADDRESS[2],
smoritaemb 0:580aba13d1a1 116 Config::IP_ADDRESS[3], getUserMulticastPort());
smoritaemb 0:580aba13d1a1 117 }
smoritaemb 0:580aba13d1a1 118
smoritaemb 0:580aba13d1a1 119 inline Locator getDefaultSendMulticastLocator() {
smoritaemb 0:580aba13d1a1 120 return Locator::createUDPv4Locator(239, 255, 0, 1, getBuiltInMulticastPort());
smoritaemb 0:580aba13d1a1 121 }
smoritaemb 0:580aba13d1a1 122 } // namespace rtps
smoritaemb 0:580aba13d1a1 123
smoritaemb 0:580aba13d1a1 124 #endif // RTPS_LOCATOR_T_H