Add a deserialize method to Subscription (#19)

* Add a deserialize method to Subscription to enable users to use a SerializedMessage and deserialize later.

* Catch exceptions when deserializing.

(cherry picked from commit 8fbbefc)

Author: StefanFabian

ros_babel_fish/include/ros_babel_fish/detail/babel_fish_subscription.hpp

@@ -71,6 +71,8 @@ class BabelFishSubscription : public rclcpp::SubscriptionBase
   void handle_dynamic_message( const rclcpp::dynamic_typesupport::DynamicMessage::SharedPtr &message,
                                const rclcpp::MessageInfo &message_info ) override;
 
+  bool deserialize( const rclcpp::SerializedMessage &serialized, CompoundMessage &out ) const;
+
 private:
   RCLCPP_DISABLE_COPY( BabelFishSubscription )
 

ros_babel_fish/src/detail/babel_fish_subscription.cpp

@@ -8,6 +8,7 @@
 
 #include <rcl/rcl.h>
 #include <rclcpp/node.hpp>
+#include <rclcpp/serialization.hpp>
 
 namespace ros_babel_fish
 {
@@ -135,6 +136,26 @@ MessageTypeSupport::ConstSharedPtr BabelFishSubscription::get_message_type_suppo
 
 std::string BabelFishSubscription::get_message_type() const { return type_support_->name; }
 
+bool BabelFishSubscription::deserialize( const rclcpp::SerializedMessage &serialized,
+                                         CompoundMessage &out ) const
+{
+  auto type_erased = createContainer( *type_support_ );
+  if ( !type_erased ) {
+    return false;
+  }
+
+  rclcpp::SerializationBase serializer( &type_support_->type_support_handle );
+  try {
+    serializer.deserialize_message( &serialized, type_erased.get() );
+  } catch ( const std::exception &e ) {
+    RBF2_ERROR_STREAM( "Failed to deserialize message of type '" << type_support_->name
+                                                                 << "': " << e.what() );
+    return false;
+  }
+  out = CompoundMessage( *type_support_, std::move( type_erased ) );
+  return true;
+}
+
 rclcpp::dynamic_typesupport::DynamicMessageType::SharedPtr
 BabelFishSubscription::get_shared_dynamic_message_type()
 {

ros_babel_fish/test/message_decoding.cpp

@@ -270,6 +270,33 @@ TEST_F( MessageDecodingTest, arrayTests )
   ASSERT_EQ( msg_subarrays_fixed.elementName(), "ros_babel_fish_test_msgs/msg/TestSubArray" );
 }
 
+TEST_F( MessageDecodingTest, deserializeSerializedMessage )
+{
+  using namespace std::chrono_literals;
+  rclcpp::GuardCondition::SharedPtr cond = std::make_shared<rclcpp::GuardCondition>();
+  rclcpp::WaitSet set;
+  set.add_guard_condition( cond );
+
+  std::shared_ptr<rclcpp::SerializedMessage> serialized_message;
+  auto subscription = fish.create_subscription(
+      *node, "/test_message_decoding/test_message", 1,
+      std::function<void( std::shared_ptr<rclcpp::SerializedMessage> )>(
+          [&serialized_message, &cond]( std::shared_ptr<rclcpp::SerializedMessage> msg ) {
+            serialized_message = std::move( msg );
+            cond->trigger();
+          } ) );
+  ASSERT_NE( subscription, nullptr );
+  ASSERT_EQ( set.wait( 5s ).kind(), rclcpp::WaitResultKind::Ready );
+  ASSERT_NE( serialized_message, nullptr );
+
+  CompoundMessage result;
+  ASSERT_TRUE( subscription->deserialize( *serialized_message, result ) );
+
+  ASSERT_TRUE( result.isValid() );
+  ASSERT_EQ( result.name(), "ros_babel_fish_test_msgs/msg/TestMessage" );
+  EXPECT_TRUE( MESSAGE_CONTENT_EQUAL( test_message, result ) );
+}
+
 int main( int argc, char **argv )
 {
   testing::InitGoogleTest( &argc, argv );