From 0e6fd30a8c4ccb1743f93ca1beee7cefe3b25ce9 Mon Sep 17 00:00:00 2001 From: dennis-adlink <45659984+dennis-adlink@users.noreply.github.com> Date: Thu, 19 Sep 2019 11:56:26 +0200 Subject: [PATCH] Validation in deserializer (#36) * Validation in Deserializer Added validation in CDR deserialization: max buffer length is checked when deserializing fields and strings are checked for null-terminator (except for wstrings, which are serialized without null-terminator). Signed-off-by: Dennis Potman * Catch exceptions in serdata functions In serdata functions rmw_print, rmw_to_sample and rmw_from_sample catch exceptions so that correct return code is given when functions are called from ddsi. Signed-off-by: Dennis Potman * Improve deserialisation validation Refactored the deserialisation validation functions so that sequence length is checked more properly and protection against overflows. Renamed source files for exceptions so that it conforms to ros2 / google c++ style guide. Signed-off-by: Dennis Potman --- README.md | 8 +- rmw_cyclonedds_cpp/CMakeLists.txt | 2 + .../rmw_cyclonedds_cpp/TypeSupport_impl.hpp | 12 +- .../deserialization_exception.hpp | 37 +++ .../include/rmw_cyclonedds_cpp/exception.hpp | 40 +++ .../include/rmw_cyclonedds_cpp/serdes.hpp | 81 ++++-- .../src/deserialization_exception.cpp | 41 ++++ rmw_cyclonedds_cpp/src/exception.cpp | 42 ++++ rmw_cyclonedds_cpp/src/rmw_node.cpp | 38 +-- rmw_cyclonedds_cpp/src/serdata.cpp | 230 ++++++++++-------- rmw_cyclonedds_cpp/src/serdes.cpp | 8 +- 11 files changed, 379 insertions(+), 160 deletions(-) mode change 100644 => 100755 README.md mode change 100644 => 100755 rmw_cyclonedds_cpp/CMakeLists.txt mode change 100644 => 100755 rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp create mode 100755 rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/deserialization_exception.hpp create mode 100755 rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/exception.hpp create mode 100755 rmw_cyclonedds_cpp/src/deserialization_exception.cpp create mode 100755 rmw_cyclonedds_cpp/src/exception.cpp mode change 100644 => 100755 rmw_cyclonedds_cpp/src/rmw_node.cpp mode change 100644 => 100755 rmw_cyclonedds_cpp/src/serdata.cpp diff --git a/README.md b/README.md old mode 100644 new mode 100755 index 152471d..a8d47c4 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ with [*Eclipse Cyclone DDS*](https://github.com/eclipse-cyclonedds/cyclonedds) a implementation. ## Getting, building and using it - + All it takes to get Cyclone DDS support into ROS2 is to clone this repository into the ROS2 workspace source directory, and then run colcon build in the usual manner: @@ -34,9 +34,3 @@ There are a number of known limitations: * Cyclone DDS does not yet implement DDS Security. Consequently, there is no support for security in this RMW implementation either. - -* Deserialization only handles native format (it doesn't do any byte swapping). This is pure - laziness, adding it is trivial. - -* Deserialization assumes the input is valid and will do terrible things if it isn't. Again, pure - laziness, it's just adding some bounds checks and other validation code. diff --git a/rmw_cyclonedds_cpp/CMakeLists.txt b/rmw_cyclonedds_cpp/CMakeLists.txt old mode 100644 new mode 100755 index 9e823ad..dfb5cfc --- a/rmw_cyclonedds_cpp/CMakeLists.txt +++ b/rmw_cyclonedds_cpp/CMakeLists.txt @@ -56,6 +56,8 @@ add_library(rmw_cyclonedds_cpp src/serdes.cpp src/graphrhc.cpp src/u16string.cpp + src/exception.cpp + src/deserialization_exception.cpp ) target_link_libraries(rmw_cyclonedds_cpp CycloneDDS::ddsc) diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp old mode 100644 new mode 100755 index b963ab9..1135aff --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp @@ -654,9 +654,7 @@ inline size_t get_submessage_array_deserialize( size_t max_align) { (void)member; - uint32_t vsize = 0; - // Deserialize length - deser >> vsize; + uint32_t vsize = deser.deserialize_len(1); auto vector = reinterpret_cast *>(field); if (call_new) { new(vector) std::vector; @@ -677,9 +675,7 @@ inline size_t get_submessage_array_deserialize( size_t) { (void)member; - // Deserialize length - uint32_t vsize = 0; - deser >> vsize; + uint32_t vsize = deser.deserialize_len(1); auto tmparray = static_cast(field); rosidl_generator_c__void__Sequence__init(tmparray, vsize, sub_members_size); subros_message = reinterpret_cast(tmparray->data); @@ -786,7 +782,7 @@ void print_field(const M * member, cycprint & deser, T & dummy) if (member->array_size_ && !member->is_upper_bound_) { deser.printA(&dummy, member->array_size_); } else { - int32_t dsize = deser.get32(); + int32_t dsize = deser.get_len(1); deser.printA(&dummy, dsize); } deser.print_constant("}"); @@ -857,7 +853,7 @@ bool TypeSupport::printROSmessage( if (member->array_size_ && !member->is_upper_bound_) { array_size = member->array_size_; } else { - array_size = deser.get32(); + array_size = deser.get_len(1); } deser.print_constant("{"); for (size_t index = 0; index < array_size; ++index) { diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/deserialization_exception.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/deserialization_exception.hpp new file mode 100755 index 0000000..7150029 --- /dev/null +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/deserialization_exception.hpp @@ -0,0 +1,37 @@ +// Copyright 2018 to 2019 ADLINK Technology +// +// 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. + +#ifndef RMW_CYCLONEDDS_CPP__DESERIALIZATION_EXCEPTION_HPP_ +#define RMW_CYCLONEDDS_CPP__DESERIALIZATION_EXCEPTION_HPP_ + +#include "rmw_cyclonedds_cpp/exception.hpp" + +namespace rmw_cyclonedds_cpp +{ + +class DeserializationException : public Exception +{ +public: + explicit DeserializationException(const char * const & message); + DeserializationException(const DeserializationException & ex); + DeserializationException & operator=(const DeserializationException & ex); + + virtual ~DeserializationException() throw(); + + static const char * const DEFAULT_MESSAGE; +}; + +} // namespace rmw_cyclonedds_cpp + +#endif // RMW_CYCLONEDDS_CPP__DESERIALIZATION_EXCEPTION_HPP_ diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/exception.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/exception.hpp new file mode 100755 index 0000000..048684a --- /dev/null +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/exception.hpp @@ -0,0 +1,40 @@ +// Copyright 2018 to 2019 ADLINK Technology +// +// 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. + +#ifndef RMW_CYCLONEDDS_CPP__EXCEPTION_HPP_ +#define RMW_CYCLONEDDS_CPP__EXCEPTION_HPP_ + +#include +#include + +namespace rmw_cyclonedds_cpp +{ + +class Exception : public std::exception +{ +public: + virtual ~Exception() throw(); + virtual const char * what() const throw(); + +protected: + explicit Exception(const char * const & message); + Exception(const Exception & ex); + Exception & operator=(const Exception & ex); + + std::string m_message; +}; + +} // namespace rmw_cyclonedds_cpp + +#endif // RMW_CYCLONEDDS_CPP__EXCEPTION_HPP_ diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp index dfbe01c..b93d66f 100755 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp @@ -20,10 +20,15 @@ #include #include +#include #include #include #include +#include "rmw_cyclonedds_cpp/deserialization_exception.hpp" + +using rmw_cyclonedds_cpp::DeserializationException; + class cycser { public: @@ -159,17 +164,10 @@ private: class cycdeserbase { public: - explicit cycdeserbase(const char * data_); + explicit cycdeserbase(const char * data_, size_t lim_); cycdeserbase() = delete; protected: - inline void align(size_t a) - { - if ((pos % a) != 0) { - pos += a - (pos % a); - } - } - inline uint16_t bswap2u(uint16_t x) { return (uint16_t) ((x >> 8) | (x << 8)); @@ -197,8 +195,32 @@ protected: return (int64_t) bswap8u((uint64_t) x); } + inline void align(size_t a) + { + if ((pos % a) != 0) { + pos += a - (pos % a); + if (pos > lim) { + throw DeserializationException("invalid data size"); + } + } + } + inline void validate_size(size_t count, size_t sz) + { + assert(sz == 1 || sz == 2 || sz == 4 || sz == 8); + if (count > (lim - pos) / sz) { + throw DeserializationException("invalid data size"); + } + } + inline void validate_str(size_t sz) + { + if (sz > 0 && data[pos + sz - 1] != '\0') { + throw DeserializationException("string data is not null-terminated"); + } + } + const char * data; size_t pos; + size_t lim; bool swap_bytes; }; @@ -231,6 +253,7 @@ public: #define DESER8(T) DESER(T, ) #define DESER(T, fn_swap) inline void deserialize(T & x) { \ align(sizeof(x)); \ + validate_size(1, sizeof(x)); \ x = *reinterpret_cast(data + pos); \ if (swap_bytes) {x = fn_swap(x);} \ pos += sizeof(x); \ @@ -258,30 +281,36 @@ public: { deserialize(*reinterpret_cast(&x)); } - inline uint32_t deserialize32() + inline uint32_t deserialize_len(size_t el_sz) { - uint32_t sz; deserialize(sz); return sz; + uint32_t sz; + deserialize(sz); + validate_size(sz, el_sz); + return sz; } inline void deserialize(char * & x) { - const uint32_t sz = deserialize32(); + const uint32_t sz = deserialize_len(sizeof(char)); + validate_str(sz); x = reinterpret_cast(malloc(sz)); memcpy(x, data + pos, sz); pos += sz; } inline void deserialize(std::string & x) { - const uint32_t sz = deserialize32(); + const uint32_t sz = deserialize_len(sizeof(char)); if (sz == 0) { x = std::string(""); } else { + validate_str(sz); x = std::string(data + pos, sz - 1); } pos += sz; } inline void deserialize(std::wstring & x) { - const uint32_t sz = deserialize32(); + const uint32_t sz = deserialize_len(sizeof(wchar_t)); + // wstring is not null-terminated in cdr x = std::wstring(reinterpret_cast(data + pos), sz); pos += sz * sizeof(wchar_t); } @@ -290,6 +319,7 @@ public: #define DESER_A(T, fn_swap) inline void deserializeA(T * x, size_t cnt) { \ if (cnt > 0) { \ align(sizeof(T)); \ + validate_size(cnt, sizeof(T)); \ if (swap_bytes) { \ for (size_t i = 0; i < cnt; i++) { \ x[i] = fn_swap(*reinterpret_cast(data + pos)); \ @@ -331,13 +361,13 @@ public: template inline void deserialize(std::vector & x) { - const uint32_t sz = deserialize32(); + const uint32_t sz = deserialize_len(1); x.resize(sz); deserializeA(x.data(), sz); } inline void deserialize(std::vector & x) { - const uint32_t sz = deserialize32(); + const uint32_t sz = deserialize_len(sizeof(unsigned char)); x.resize(sz); for (size_t i = 0; i < sz; i++) { x[i] = ((data + pos)[i] != 0); @@ -349,9 +379,6 @@ public: { deserializeA(x.data(), x.size()); } - -private: - size_t lim; // ignored for now ... better provide correct input }; class cycprint : cycdeserbase @@ -388,6 +415,7 @@ public: #define PRNT8(T, F) PRNT(T, F, ) #define PRNT(T, F, fn_swap) inline void print(T & x) { \ align(sizeof(x)); \ + validate_size(1, sizeof(x)); \ x = *reinterpret_cast(data + pos); \ if (swap_bytes) {x = fn_swap(x);} \ prtf(&buf, &bufsize, F, x); \ @@ -413,6 +441,7 @@ public: { union { uint32_t u; float f; } tmp; align(sizeof(x)); + validate_size(1, sizeof(x)); tmp.u = *reinterpret_cast(data + pos); if (swap_bytes) {tmp.u = bswap4u(tmp.u);} static_cast(tmp.u); @@ -423,24 +452,28 @@ public: { union { uint64_t u; double f; } tmp; align(sizeof(x)); + validate_size(1, sizeof(x)); tmp.u = *reinterpret_cast(data + pos); if (swap_bytes) {tmp.u = bswap8u(tmp.u);} static_cast(tmp.u); prtf(&buf, &bufsize, "%f", tmp.f); pos += sizeof(x); } - inline uint32_t get32() + inline uint32_t get_len(size_t el_sz) { uint32_t sz; align(sizeof(sz)); + validate_size(1, sizeof(sz)); sz = *reinterpret_cast(data + pos); if (swap_bytes) {sz = bswap4u(sz);} pos += sizeof(sz); + validate_size(sz, el_sz); return sz; } inline void print(char * & x) { - const uint32_t sz = get32(); + const uint32_t sz = get_len(sizeof(char)); + validate_str(sz); const int len = (sz == 0) ? 0 : (sz > INT32_MAX) ? INT32_MAX : static_cast(sz - 1); static_cast(x); prtf(&buf, &bufsize, "\"%*.*s\"", len, len, static_cast(data + pos)); @@ -448,7 +481,8 @@ public: } inline void print(std::string & x) { - const uint32_t sz = get32(); + const uint32_t sz = get_len(sizeof(char)); + validate_str(sz); const int len = (sz == 0) ? 0 : (sz > INT32_MAX) ? INT32_MAX : static_cast(sz - 1); static_cast(x); prtf(&buf, &bufsize, "\"%*.*s\"", len, len, static_cast(data + pos)); @@ -456,7 +490,8 @@ public: } inline void print(std::wstring & x) { - const uint32_t sz = get32(); + const uint32_t sz = get_len(sizeof(wchar_t)); + // wstring is not null-terminated in cdr x = std::wstring(reinterpret_cast(data + pos), sz); prtf(&buf, &bufsize, "\"%ls\"", x.c_str()); pos += sz * sizeof(wchar_t); @@ -476,7 +511,7 @@ public: template inline void print(std::vector & x) { - const uint32_t sz = get32(); + const uint32_t sz = get_len(1); printA(x.data(), sz); } template diff --git a/rmw_cyclonedds_cpp/src/deserialization_exception.cpp b/rmw_cyclonedds_cpp/src/deserialization_exception.cpp new file mode 100755 index 0000000..ab1fa4b --- /dev/null +++ b/rmw_cyclonedds_cpp/src/deserialization_exception.cpp @@ -0,0 +1,41 @@ +// Copyright 2018 to 2019 ADLINK Technology +// +// 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. + +#include "rmw_cyclonedds_cpp/deserialization_exception.hpp" + +using rmw_cyclonedds_cpp::DeserializationException; + +DeserializationException::DeserializationException(const char * const & message) +: Exception(message) +{ +} + +DeserializationException::DeserializationException(const DeserializationException & ex) +: Exception(ex) +{ +} + +DeserializationException & DeserializationException::operator=(const DeserializationException & ex) +{ + if (this != &ex) { + Exception::operator=(ex); + } + return *this; +} + +DeserializationException::~DeserializationException() throw() +{ +} + +const char * const DeserializationException::DEFAULT_MESSAGE = "Invalid data"; diff --git a/rmw_cyclonedds_cpp/src/exception.cpp b/rmw_cyclonedds_cpp/src/exception.cpp new file mode 100755 index 0000000..5d0a58f --- /dev/null +++ b/rmw_cyclonedds_cpp/src/exception.cpp @@ -0,0 +1,42 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +#include "rmw_cyclonedds_cpp/exception.hpp" + +using rmw_cyclonedds_cpp::Exception; + +Exception::Exception(const char * const & message) +: m_message(message) +{ +} + +Exception::Exception(const Exception & ex) +: m_message(ex.m_message) +{ +} + +Exception & Exception::operator=(const Exception & ex) +{ + m_message = ex.m_message; + return *this; +} + +Exception::~Exception() throw() +{ +} + +const char * Exception::what() const throw() +{ + return m_message.c_str(); +} diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp old mode 100644 new mode 100755 index 2aa2627..908fc91 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -713,31 +713,37 @@ extern "C" rmw_ret_t rmw_deserialize( const rosidl_message_type_support_t * type_support, void * ros_message) { - cycdeser sd(serialized_message->buffer, serialized_message->buffer_length); bool ok; - const rosidl_message_type_support_t * ts; - if ((ts = - get_message_typesupport_handle(type_support, - rosidl_typesupport_introspection_c__identifier)) != nullptr) - { - auto members = - static_cast(ts->data); - MessageTypeSupport_c msgts(members); - ok = msgts.deserializeROSmessage(sd, ros_message, nullptr); - } else { + try { + cycdeser sd(serialized_message->buffer, serialized_message->buffer_length); + const rosidl_message_type_support_t * ts; if ((ts = get_message_typesupport_handle(type_support, - rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) + rosidl_typesupport_introspection_c__identifier)) != nullptr) { auto members = - static_cast(ts->data); - MessageTypeSupport_cpp msgts(members); + static_cast(ts->data); + MessageTypeSupport_c msgts(members); ok = msgts.deserializeROSmessage(sd, ros_message, nullptr); } else { - RMW_SET_ERROR_MSG("rmw_serialize: type support trouble"); - return RMW_RET_ERROR; + if ((ts = + get_message_typesupport_handle(type_support, + rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) + { + auto members = + static_cast(ts->data); + MessageTypeSupport_cpp msgts(members); + ok = msgts.deserializeROSmessage(sd, ros_message, nullptr); + } else { + RMW_SET_ERROR_MSG("rmw_serialize: type support trouble"); + return RMW_RET_ERROR; + } } + } catch (rmw_cyclonedds_cpp::Exception & e) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("rmw_serialize: %s", e.what()); + ok = false; } + return ok ? RMW_RET_OK : RMW_RET_ERROR; } diff --git a/rmw_cyclonedds_cpp/src/serdata.cpp b/rmw_cyclonedds_cpp/src/serdata.cpp old mode 100644 new mode 100755 index bc6f9d3..ff7c8ea --- a/rmw_cyclonedds_cpp/src/serdata.cpp +++ b/rmw_cyclonedds_cpp/src/serdata.cpp @@ -166,44 +166,52 @@ static struct ddsi_serdata * serdata_rmw_from_sample( enum ddsi_serdata_kind kind, const void * sample) { - const struct sertopic_rmw * topic = static_cast(topiccmn); - struct serdata_rmw * d = rmw_serdata_new(topic, kind); - if (kind != SDK_DATA) { - /* ROS2 doesn't do keys, so SDK_KEY is trivial */ - } else if (!topic->is_request_header) { - cycser sd(d->data); - if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(sample, sd); - } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(sample, sd); + try { + const struct sertopic_rmw * topic = static_cast(topiccmn); + struct serdata_rmw * d = rmw_serdata_new(topic, kind); + if (kind != SDK_DATA) { + /* ROS2 doesn't do keys, so SDK_KEY is trivial */ + } else if (!topic->is_request_header) { + cycser sd(d->data); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + (void) typed_typesupport->serializeROSmessage(sample, sd); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + (void) typed_typesupport->serializeROSmessage(sample, sd); + } + } else { + /* The "prefix" lambda is there to inject the service invocation header data into the CDR + stream -- I haven't checked how it is done in the official RMW implementations, so it is + probably incompatible. */ + const cdds_request_wrapper_t * wrap = static_cast(sample); + auto prefix = [wrap](cycser & ser) {ser << wrap->header.guid; ser << wrap->header.seq;}; + cycser sd(d->data); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix); + } } - } else { - /* The "prefix" lambda is there to inject the service invocation header data into the CDR - stream -- I haven't checked how it is done in the official RMW implementations, so it is - probably incompatible. */ - const cdds_request_wrapper_t * wrap = static_cast(sample); - auto prefix = [wrap](cycser & ser) {ser << wrap->header.guid; ser << wrap->header.seq;}; - cycser sd(d->data); - if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix); - } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix); + /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector + when copying data to network. Should fix Cyclone to handle that more elegantly. */ + while (d->data.size() % 4) { + d->data.push_back(0); } + return d; + } catch (rmw_cyclonedds_cpp::Exception & e) { + RMW_SET_ERROR_MSG(e.what()); + return nullptr; + } catch (std::runtime_error & e) { + RMW_SET_ERROR_MSG(e.what()); + return nullptr; } - /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector - when copying data to network. Should fix Cyclone to handle that more elegantly. */ - while (d->data.size() % 4) { - d->data.push_back(0); - } - return d; } struct ddsi_serdata * serdata_rmw_from_serialized_message( @@ -257,42 +265,51 @@ static bool serdata_rmw_to_sample( const struct ddsi_serdata * dcmn, void * sample, void ** bufptr, void * buflim) { - static_cast(bufptr); // unused - static_cast(buflim); // unused - const struct serdata_rmw * d = static_cast(dcmn); - const struct sertopic_rmw * topic = static_cast(d->topic); - assert(bufptr == NULL); - assert(buflim == NULL); - if (d->kind != SDK_DATA) { - /* ROS2 doesn't do keys in a meaningful way yet */ - } else if (!topic->is_request_header) { - cycdeser sd(static_cast(d->data.data()), d->data.size()); - if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage(sd, sample); - } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage(sd, sample); - } - } else { - /* The "prefix" lambda is there to inject the service invocation header data into the CDR - stream -- I haven't checked how it is done in the official RMW implementations, so it is - probably incompatible. */ - cdds_request_wrapper_t * const wrap = static_cast(sample); - auto prefix = [wrap](cycdeser & ser) {ser >> wrap->header.guid; ser >> wrap->header.seq;}; - cycdeser sd(static_cast(d->data.data()), d->data.size()); - if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage(sd, wrap->data, prefix); - } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage(sd, wrap->data, prefix); + try { + static_cast(bufptr); // unused + static_cast(buflim); // unused + const struct serdata_rmw * d = static_cast(dcmn); + const struct sertopic_rmw * topic = static_cast(d->topic); + assert(bufptr == NULL); + assert(buflim == NULL); + if (d->kind != SDK_DATA) { + /* ROS2 doesn't do keys in a meaningful way yet */ + } else if (!topic->is_request_header) { + cycdeser sd(static_cast(d->data.data()), d->data.size()); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->deserializeROSmessage(sd, sample); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->deserializeROSmessage(sd, sample); + } + } else { + /* The "prefix" lambda is there to inject the service invocation header data into the CDR + stream -- I haven't checked how it is done in the official RMW implementations, so it is + probably incompatible. */ + cdds_request_wrapper_t * const wrap = static_cast(sample); + auto prefix = [wrap](cycdeser & ser) {ser >> wrap->header.guid; ser >> wrap->header.seq;}; + cycdeser sd(static_cast(d->data.data()), d->data.size()); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->deserializeROSmessage(sd, wrap->data, prefix); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->deserializeROSmessage(sd, wrap->data, prefix); + } } + } catch (rmw_cyclonedds_cpp::Exception & e) { + RMW_SET_ERROR_MSG(e.what()); + return false; + } catch (std::runtime_error & e) { + RMW_SET_ERROR_MSG(e.what()); + return false; } + return false; } @@ -322,41 +339,50 @@ static bool serdata_rmw_eqkey(const struct ddsi_serdata * a, const struct ddsi_s static size_t serdata_rmw_print( const struct ddsi_sertopic * tpcmn, const struct ddsi_serdata * dcmn, char * buf, size_t bufsize) { - const struct serdata_rmw * d = static_cast(dcmn); - const struct sertopic_rmw * topic = static_cast(tpcmn); - if (d->kind != SDK_DATA) { - /* ROS2 doesn't do keys in a meaningful way yet */ - return static_cast(snprintf(buf, bufsize, ":k:{}")); - } else if (!topic->is_request_header) { - cycprint sd(buf, bufsize, static_cast(d->data.data()), d->data.size()); - if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - return typed_typesupport->printROSmessage(sd); - } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - return typed_typesupport->printROSmessage(sd); - } - } else { - /* The "prefix" lambda is there to inject the service invocation header data into the CDR - stream -- I haven't checked how it is done in the official RMW implementations, so it is - probably incompatible. */ - cdds_request_wrapper_t wrap; - auto prefix = [&wrap](cycprint & ser) { - ser >> wrap.header.guid; ser.print_constant(","); ser >> wrap.header.seq; - }; - cycprint sd(buf, bufsize, static_cast(d->data.data()), d->data.size()); - if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - return typed_typesupport->printROSmessage(sd, prefix); - } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - return typed_typesupport->printROSmessage(sd, prefix); + try { + const struct serdata_rmw * d = static_cast(dcmn); + const struct sertopic_rmw * topic = static_cast(tpcmn); + if (d->kind != SDK_DATA) { + /* ROS2 doesn't do keys in a meaningful way yet */ + return static_cast(snprintf(buf, bufsize, ":k:{}")); + } else if (!topic->is_request_header) { + cycprint sd(buf, bufsize, static_cast(d->data.data()), d->data.size()); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->printROSmessage(sd); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->printROSmessage(sd); + } + } else { + /* The "prefix" lambda is there to inject the service invocation header data into the CDR + stream -- I haven't checked how it is done in the official RMW implementations, so it is + probably incompatible. */ + cdds_request_wrapper_t wrap; + auto prefix = [&wrap](cycprint & ser) { + ser >> wrap.header.guid; ser.print_constant(","); ser >> wrap.header.seq; + }; + cycprint sd(buf, bufsize, static_cast(d->data.data()), d->data.size()); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->printROSmessage(sd, prefix); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->printROSmessage(sd, prefix); + } } + } catch (rmw_cyclonedds_cpp::Exception & e) { + RMW_SET_ERROR_MSG(e.what()); + return false; + } catch (std::runtime_error & e) { + RMW_SET_ERROR_MSG(e.what()); + return false; } + return false; } #endif diff --git a/rmw_cyclonedds_cpp/src/serdes.cpp b/rmw_cyclonedds_cpp/src/serdes.cpp index d1af832..51a4723 100755 --- a/rmw_cyclonedds_cpp/src/serdes.cpp +++ b/rmw_cyclonedds_cpp/src/serdes.cpp @@ -32,9 +32,10 @@ cycser::cycser(std::vector & dst_) dst.push_back(0); } -cycdeserbase::cycdeserbase(const char * data_) +cycdeserbase::cycdeserbase(const char * data_, size_t lim_) : data(data_), pos(0), + lim(lim_), swap_bytes(false) { /* Get the endianness byte (skip unused first byte in data[0]) */ @@ -48,13 +49,12 @@ cycdeserbase::cycdeserbase(const char * data_) } cycdeser::cycdeser(const void * data_, size_t size_) -: cycdeserbase(static_cast(data_)), - lim(size_ - 4) +: cycdeserbase(static_cast(data_), size_ - 4) { } cycprint::cycprint(char * buf_, size_t bufsize_, const void * data_, size_t size_) -: cycdeserbase(static_cast(data_)), +: cycdeserbase(static_cast(data_), size_ - 4), buf(buf_), bufsize(bufsize_) {