[Foxy] Handle exception on deserializing ROS message (#603)

* Handle exception on deserializing ROS message
* Capture cdr exceptions (#505)
* Capturing fastcdr exceptions.

Signed-off-by: Erki Suurjaak <erki@lap.ee>
Co-authored-by: Miguel Company <miguelcompany@eprosima.com>

rmw_fastrtps_cpp/src/type_support_common.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <fastcdr/exceptions/Exception.h>
+
 #include <string>
 
 #include "rmw/error_handling.h"
@@ -88,20 +90,27 @@ bool TypeSupport::deserializeROSmessage(
   assert(ros_message);
   assert(impl);
 
-  // Deserialize encapsulation.
-  deser.read_encapsulation();
-
-  // If type is not empty, deserialize message
-  if (has_data_) {
-    auto callbacks = static_cast<const message_type_support_callbacks_t *>(impl);
-    return callbacks->cdr_deserialize(deser, ros_message);
+  try {
+    // Deserialize encapsulation.
+    deser.read_encapsulation();
+
+    // If type is not empty, deserialize message
+    if (has_data_) {
+      auto callbacks = static_cast<const message_type_support_callbacks_t *>(impl);
+      return callbacks->cdr_deserialize(deser, ros_message);
+    }
+
+    // Otherwise, consume dummy byte
+    uint8_t dump = 0;
+    deser >> dump;
+    (void)dump;
+  } catch (const eprosima::fastcdr::exception::Exception &) {
+    RMW_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Fast CDR exception deserializing message of type %s.",
+      getName());
+    return false;
   }
 
-  // Otherwise, consume dummy byte
-  uint8_t dump = 0;
-  deser >> dump;
-  (void)dump;
-
   return true;
 }
 

rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp

@@ -17,6 +17,8 @@
 
 #include <fastcdr/FastBuffer.h>
 #include <fastcdr/Cdr.h>
+#include <fastcdr/exceptions/Exception.h>
+
 #include <cassert>
 #include <string>
 #include <vector>
@@ -803,7 +805,9 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
         {
           auto sub_members = static_cast<const MembersType *>(member->members_->data);
           if (!member->is_array_) {
-            deserializeROSmessage(deser, sub_members, field);
+            if (!deserializeROSmessage(deser, sub_members, field)) {
+              return false;
+            }
           } else {
             size_t array_size = 0;
 
@@ -826,11 +830,14 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
               return false;
             }
             for (size_t index = 0; index < array_size; ++index) {
-              deserializeROSmessage(
-                deser, sub_members,
-                get_subros_message(
-                  member, field, index, member->array_size_,
-                  member->is_upper_bound_));
+              if (!deserializeROSmessage(
+                  deser, sub_members,
+                  get_subros_message(
+                    member, field, index, member->array_size_,
+                    member->is_upper_bound_)))
+              {
+                return false;
+              }
             }
           }
         }
@@ -972,16 +979,23 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
   assert(ros_message);
   assert(members_);
 
-  // Deserialize encapsulation.
-  deser.read_encapsulation();
+  try {
+    // Deserialize encapsulation.
+    deser.read_encapsulation();
+
+    (void)impl;
+    if (members_->member_count_ != 0) {
+      return TypeSupport::deserializeROSmessage(deser, members_, ros_message);
+    }
 
-  (void)impl;
-  if (members_->member_count_ != 0) {
-    TypeSupport::deserializeROSmessage(deser, members_, ros_message);
-  } else {
     uint8_t dump = 0;
     deser >> dump;
     (void)dump;
+  } catch (const eprosima::fastcdr::exception::Exception &) {
+    RMW_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Fast CDR exception deserializing message of type %s.",
+      getName());
+    return false;
   }
 
   return true;

rmw_fastrtps_shared_cpp/src/rmw_request.cpp

@@ -96,22 +96,22 @@ __rmw_take_request(
   if (request.buffer_ != nullptr) {
     eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
       eprosima::fastcdr::Cdr::DDS_CDR);
-    info->request_type_support_->deserializeROSmessage(
-      deser, ros_request, info->request_type_support_impl_);
-
-    // Get header
-    rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array(
-      request.sample_identity_.writer_guid(),
-      request_header->request_id.writer_guid);
-    request_header->request_id.sequence_number =
-      ((int64_t)request.sample_identity_.sequence_number().high) <<
-      32 | request.sample_identity_.sequence_number().low;
-    request_header->source_timestamp = request.sample_info_.sourceTimestamp.to_ns();
-    request_header->received_timestamp = request.sample_info_.receptionTimestamp.to_ns();
+    if (info->request_type_support_->deserializeROSmessage(
+        deser, ros_request, info->request_type_support_impl_))
+    {
+      // Get header
+      rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array(
+        request.sample_identity_.writer_guid(),
+        request_header->request_id.writer_guid);
+      request_header->request_id.sequence_number =
+        ((int64_t)request.sample_identity_.sequence_number().high) <<
+        32 | request.sample_identity_.sequence_number().low;
+      request_header->source_timestamp = request.sample_info_.sourceTimestamp.to_ns();
+      request_header->received_timestamp = request.sample_info_.receptionTimestamp.to_ns();
+      *taken = true;
+    }
 
     delete request.buffer_;
-
-    *taken = true;
   }
 
   return RMW_RET_OK;

rmw_fastrtps_shared_cpp/src/rmw_response.cpp

@@ -59,16 +59,17 @@ __rmw_take_response(
       *response.buffer_,
       eprosima::fastcdr::Cdr::DEFAULT_ENDIAN,
       eprosima::fastcdr::Cdr::DDS_CDR);
-    info->response_type_support_->deserializeROSmessage(
-      deser, ros_response, info->response_type_support_impl_);
-
-    request_header->source_timestamp = response.sample_info_.sourceTimestamp.to_ns();
-    request_header->received_timestamp = response.sample_info_.receptionTimestamp.to_ns();
-    request_header->request_id.sequence_number =
-      ((int64_t)response.sample_identity_.sequence_number().high) <<
-      32 | response.sample_identity_.sequence_number().low;
-
-    *taken = true;
+    if (info->response_type_support_->deserializeROSmessage(
+        deser, ros_response, info->response_type_support_impl_))
+    {
+      request_header->source_timestamp = response.sample_info_.sourceTimestamp.to_ns();
+      request_header->received_timestamp = response.sample_info_.receptionTimestamp.to_ns();
+      request_header->request_id.sequence_number =
+        ((int64_t)response.sample_identity_.sequence_number().high) <<
+        32 | response.sample_identity_.sequence_number().low;
+
+      *taken = true;
+    }
   }
 
   return RMW_RET_OK;