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_) {