diff --git a/rmw_cyclonedds_cpp/src/TypeSupport2.cpp b/rmw_cyclonedds_cpp/src/TypeSupport2.cpp index 00bf6626..a35fe90a 100644 --- a/rmw_cyclonedds_cpp/src/TypeSupport2.cpp +++ b/rmw_cyclonedds_cpp/src/TypeSupport2.cpp @@ -18,6 +18,10 @@ #include #include +#include "rcutils/error_handling.h" +#include "rosidl_runtime_c/message_type_support_struct.h" +#include "rosidl_runtime_c/service_type_support_struct.h" + namespace rmw_cyclonedds_cpp { class ROSIDLC_StructValueType : public StructValueType @@ -68,25 +72,44 @@ class ROSIDLCPP_StructValueType : public StructValueType std::unique_ptr make_message_value_type(const rosidl_message_type_support_t * mts) { - if (auto ts_c = mts->func(mts, TypeGeneratorInfo::get_identifier())) { + if (auto ts_c = + get_message_typesupport_handle( + mts, + TypeGeneratorInfo::get_identifier())) + { auto members = static_cast *>(ts_c->data); return std::make_unique(members); + } else { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + + if (auto ts_cpp = + get_message_typesupport_handle( + mts, + TypeGeneratorInfo::get_identifier())) + { + auto members = static_cast *>(ts_cpp->data); + return std::make_unique(members); + } else { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + + throw std::runtime_error( + std::string("Type support not from this implementation. Got:\n") + + " " + prev_error_string.str + "\n" + + " " + error_string.str + "\n" + + "while fetching it"); + } } - if (auto ts_cpp = - mts->func(mts, TypeGeneratorInfo::get_identifier())) - { - auto members = static_cast *>(ts_cpp->data); - return std::make_unique(members); - } - throw std::runtime_error( - "could not identify message typesupport " + std::string(mts->typesupport_identifier)); } std::pair, std::unique_ptr> make_request_response_value_types(const rosidl_service_type_support_t * svc_ts) { if (auto tsc = - svc_ts->func(svc_ts, TypeGeneratorInfo::get_identifier())) + get_service_typesupport_handle( + svc_ts, + TypeGeneratorInfo::get_identifier())) { auto typed = static_cast::MetaService *>(tsc->data); @@ -94,21 +117,32 @@ make_request_response_value_types(const rosidl_service_type_support_t * svc_ts) std::make_unique(typed->request_members_), std::make_unique(typed->response_members_) }; + } else { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + + if (auto tscpp = + get_service_typesupport_handle( + svc_ts, + TypeGeneratorInfo::get_identifier())) + { + auto typed = + static_cast::MetaService *>(tscpp->data); + return { + std::make_unique(typed->request_members_), + std::make_unique(typed->response_members_) + }; + } else { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + + throw std::runtime_error( + std::string("Service type support not from this implementation. Got:\n") + + " " + prev_error_string.str + "\n" + + " " + error_string.str + "\n" + + "while fetching it"); + } } - - if (auto tscpp = - svc_ts->func(svc_ts, TypeGeneratorInfo::get_identifier())) - { - auto typed = - static_cast::MetaService *>(tscpp->data); - return { - std::make_unique(typed->request_members_), - std::make_unique(typed->response_members_) - }; - } - - throw std::runtime_error( - "Unidentified service type support: " + std::string(svc_ts->typesupport_identifier)); } ROSIDLC_StructValueType::ROSIDLC_StructValueType(