diff --git a/include/ur_rtde_driver/comm/package_serializer.h b/include/ur_rtde_driver/comm/package_serializer.h new file mode 100644 index 0000000..e885ec1 --- /dev/null +++ b/include/ur_rtde_driver/comm/package_serializer.h @@ -0,0 +1,84 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- htobeGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Tristan Schnell schnell@fzi.de + * \date 2019-04-10 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED +#define UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED + +#include +#include + +namespace ur_driver +{ +namespace comm +{ +class PackageSerializer +{ +public: + template + static size_t serialize(uint8_t* buffer, T val) + { + size_t size = sizeof(T); + T tmp = encode(val); + std::memcpy(buffer, &tmp, size); + return size; + } + +private: + template + static T encode(T val) + { + return val; + } + static uint16_t encode(uint16_t val) + { + return htobe16(val); + } + static uint32_t encode(uint32_t val) + { + return htobe32(val); + } + static uint64_t encode(uint64_t val) + { + return htobe64(val); + } + static int16_t encode(int16_t val) + { + return htobe16(val); + } + static int32_t encode(int32_t val) + { + return htobe32(val); + } + static int64_t encode(int64_t val) + { + return htobe64(val); + } +}; + +} // namespace comm +} // namespace ur_driver +#endif // UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED diff --git a/include/ur_rtde_driver/rtde/package_header.h b/include/ur_rtde_driver/rtde/package_header.h index 88c268d..31c52a2 100644 --- a/include/ur_rtde_driver/rtde/package_header.h +++ b/include/ur_rtde_driver/rtde/package_header.h @@ -19,6 +19,7 @@ #include #include #include "ur_rtde_driver/types.h" +#include "ur_rtde_driver/comm/package_serializer.h" namespace ur_driver { @@ -51,6 +52,15 @@ public: return be16toh(*(reinterpret_cast<_package_size_type*>(buf))); } + static size_t serializeHeader(uint8_t* buffer, PackageType package_type, uint16_t payload_length) + { + uint16_t header_size = sizeof(_package_size_type) + sizeof(PackageType); + uint16_t size = header_size+payload_length; + comm::PackageSerializer::serialize(buffer, size); + comm::PackageSerializer::serialize(buffer + sizeof(size), package_type); + return header_size; + } + private: _package_size_type package_size_; PackageType package_type_; diff --git a/include/ur_rtde_driver/rtde/request_protocol_version.h b/include/ur_rtde_driver/rtde/request_protocol_version.h index 6ddf5b9..bef297e 100644 --- a/include/ur_rtde_driver/rtde/request_protocol_version.h +++ b/include/ur_rtde_driver/rtde/request_protocol_version.h @@ -29,6 +29,7 @@ #define UR_RTDE_DRIVER_REQUEST_PROTOCOL_VERSION_H_INCLUDED #include "ur_rtde_driver/rtde/rtde_package.h" +#include "ur_rtde_driver/rtde/package_header.h" namespace ur_driver { @@ -52,7 +53,13 @@ public: RequestProtocolVersionRequest() = default; virtual ~RequestProtocolVersionRequest() = default; + static size_t generateSerializedRequest(uint8_t* buffer, uint16_t version); + uint16_t protocol_version_; + +private: + static const uint16_t PAYLOAD_SIZE = sizeof(uint16_t); + static const PackageType PACKAGE_TYPE = PackageType::RTDE_REQUEST_PROTOCOL_VERSION; }; } // namespace rtde_interface diff --git a/src/rtde/request_protocol_version.cpp b/src/rtde/request_protocol_version.cpp index 65e5b80..2d6c41a 100644 --- a/src/rtde/request_protocol_version.cpp +++ b/src/rtde/request_protocol_version.cpp @@ -39,7 +39,19 @@ bool RequestProtocolVersion::parseWith(comm::BinParser& bp) } std::string RequestProtocolVersion::toString() const { - return "accepted: " + accepted_; + std::stringstream ss; + ss << "accepted: " << static_cast(accepted_); + return ss.str(); +} + +size_t RequestProtocolVersionRequest::generateSerializedRequest(uint8_t* buffer, uint16_t version) +{ + size_t size = 0; + size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE); + + size += comm::PackageSerializer::serialize(buffer + size, version); + + return size; } } // namespace rtde_interface } // namespace ur_driver