Added a method to get the actual underlying message from a compound message.

Usage:
using geometry_msgs::msg::Point;
Point point = compound["position"].as<CompoundMessage>().message<Point>();

(cherry picked from commit b89cce1)

Author: StefanFabian

ros_babel_fish/CMakeLists.txt

@@ -17,6 +17,7 @@ endif ()
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rcpputils REQUIRED)
@@ -69,6 +70,7 @@ target_include_directories(any_subscriber PRIVATE
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
+ament_target_dependencies(any_subscriber geometry_msgs)
 
 add_executable(troll_node examples/troll_node.cpp)
 target_link_libraries(troll_node ${PROJECT_NAME})

ros_babel_fish/examples/any_subscriber.cpp

@@ -2,11 +2,11 @@
 // Licensed under the MIT license. See LICENSE file in the project root for full license information.
 
 #include <chrono>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/point32.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <ros_babel_fish/babel_fish.hpp>
 #include <ros_babel_fish/macros.hpp>
-#include <thread>
-#include <utility>
 
 /*!
  * The following example demonstrates how to subscribe to a topic of any type,
@@ -28,9 +28,12 @@ class AnySubscriber : public rclcpp::Node
   void init()
   {
     fish = ros_babel_fish::BabelFish::make_shared();
+    RCLCPP_INFO( this->get_logger(), "Started. Waiting for topic '%s' to become available.",
+                 topic_.c_str() );
     subscription_ = fish->create_subscription(
         *this, topic_, 10,
         [this]( ros_babel_fish::CompoundMessage::SharedPtr msg ) { topic_callback( msg ); } );
+    RCLCPP_INFO( this->get_logger(), "Subscribed to topic '%s'", topic_.c_str() );
   }
 
 private:
@@ -43,7 +46,7 @@ class AnySubscriber : public rclcpp::Node
     try {
       std::cout << "Type: " << msg->name() << std::endl;
       dumpMessageContent( *msg );
-      std::cout << std::endl << "---" << std::endl;
+      std::cout << "---" << std::endl;
     } catch ( ros_babel_fish::BabelFishException &ex ) {
       RCLCPP_ERROR( this->get_logger(), "Got a BabelFishException during translation: %s", ex.what() );
     }
@@ -70,6 +73,24 @@ int main( int argc, char *argv[] )
 }
 
 // Here's where the dumping happens
+void printPoint( const ros_babel_fish::CompoundMessage &point )
+{
+  // This method demonstrates how to get a known message type out of a CompoundMessage
+  // Note that this is only possible if the CompoundMessage actually contains a message of the given type
+  // Otherwise, a BabelFishException will be thrown
+  // Also, this message is a reference to the original message, so any modifications to either will affect the other
+  auto point_msg = point.message<geometry_msgs::msg::Point>();
+  std::cout << "Point { x: " << point_msg->x << ", y: " << point_msg->y << ", z: " << point_msg->z
+            << " }" << std::endl;
+}
+
+void printPoint32( const ros_babel_fish::CompoundMessage &point )
+{
+  // Again for Point32 so it can be tested with geometry_msgs/msg/Polygon
+  auto point_msg = point.message<geometry_msgs::msg::Point32>();
+  std::cout << "Point { x: " << point_msg->x << ", y: " << point_msg->y << ", z: " << point_msg->z
+            << " }" << std::endl;
+}
 
 template<typename T>
 void printToStdOut( const T &val )
@@ -134,20 +155,27 @@ void dumpMessageContent( const ros_babel_fish::Message &message, const std::stri
     // We can either just dump the message or demonstrate the time conversion
     if ( compound.isTime() ) {
       rclcpp::Time time = compound.value<rclcpp::Time>();
-      std::cout << "Time { seconds: " << time.seconds() << " }";
+      std::cout << "Time { seconds: " << time.seconds() << " }" << std::endl;
       return;
     }
     if ( compound.isDuration() ) {
       auto duration = compound.value<rclcpp::Duration>();
-      std::cout << "Duration { seconds: " << duration.seconds() << " }";
+      std::cout << "Duration { seconds: " << duration.seconds() << " }" << std::endl;
+      return;
+    }
+    if ( compound.name() == "geometry_msgs/msg/Point" ) {
+      printPoint( compound );
+      return;
+    }
+    if ( compound.name() == "geometry_msgs/msg/Point32" ) {
+      printPoint32( compound );
       return;
     }
     std::cout << std::endl;
     for ( size_t i = 0; i < compound.keys().size(); ++i ) {
       std::cout << prefix << compound.keys()[i] << ": ";
       dumpMessageContent( *compound.values()[i], prefix + "  " );
-      if ( i != compound.keys().size() - 1 )
-        std::cout << std::endl;
+      std::cout << std::endl;
     }
   } else if ( message.type() == MessageTypes::Array ) {
     auto &base = message.as<ArrayMessageBase>();

ros_babel_fish/include/ros_babel_fish/messages/compound_message.hpp

@@ -82,6 +82,41 @@ class CompoundMessage final : public Message
 
   std::shared_ptr<void> type_erased_message();
 
+  /*!
+   * Cast this compound message to a message type.
+   * Example:
+   * \code{.cpp}
+   * using geometry_msgs::msg::Point;
+   * Point point = compound.message&lt;Point&gt;();
+   * \endcode
+   *
+   * @tparam T The type of the message to cast to.
+   * @throws BabelFishException If the message is not of the target type.
+   * @return The message casted to the target type.
+   */
+  template<typename T>
+  std::shared_ptr<T> message()
+  {
+    if ( rosidl_generator_traits::name<T>() != name() ) {
+      throw BabelFishException( "Tried to cast compound message of type " + name() +
+                                " to incompatible message type " +
+                                rosidl_generator_traits::name<T>() + "!" );
+    }
+    return std::static_pointer_cast<T>( type_erased_message() );
+  }
+
+  //! @copydoc message()
+  template<typename T>
+  std::shared_ptr<const T> message() const
+  {
+    if ( rosidl_generator_traits::name<T>() != name() ) {
+      throw BabelFishException( "Tried to cast compound message of type " + name() +
+                                " to incompatible message type " +
+                                rosidl_generator_traits::name<T>() + "!" );
+    }
+    return std::static_pointer_cast<const T>( type_erased_message() );
+  }
+
   //! Creates a copy of this compound message
   CompoundMessage clone() const;
 

ros_babel_fish/include/ros_babel_fish/messages/message.hpp

@@ -48,10 +48,8 @@ class Message
   template<typename T>
   T value() const
   {
-    auto result = std::dynamic_pointer_cast<T>( data_ );
-    if ( !result )
-      throw BabelFishException( "Invalid cast!" );
-    return *result;
+    // Fallback to the default implementation which throws an exception if no specialization is available
+    static_assert( false && "Invalid type for value!" );
   }
 
   /*!
@@ -69,7 +67,7 @@ class Message
   template<typename T>
   T &as()
   {
-    T *result = dynamic_cast<T *>( this );
+    auto result = dynamic_cast<T *>( this );
     if ( result == nullptr )
       throw BabelFishException( "Tried to cast message to incompatible type!" );
     return *result;
@@ -80,8 +78,9 @@ class Message
   const T &as() const
   {
     auto result = dynamic_cast<const T *>( this );
-    if ( result == nullptr )
+    if ( result == nullptr ) {
       throw BabelFishException( "Tried to cast message to incompatible type!" );
+    }
     return *result;
   }
 

ros_babel_fish/package.xml

@@ -16,14 +16,14 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>ament_index_cpp</depend>
+  <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rcpputils</depend>
   <depend>rosidl_runtime_cpp</depend>
   <depend>rosidl_typesupport_cpp</depend>
   <depend>rosidl_typesupport_introspection_cpp</depend>
   <build_depend>action_tutorials_interfaces</build_depend>
-  <build_depend>geometry_msgs</build_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_cppcheck</test_depend>

ros_babel_fish/test/message.cpp

@@ -285,6 +285,11 @@ TEST( MessageTest, compoundMessage )
   }
   ASSERT_TRUE( shared_pointer_alive );
 
+  // Obtain a known subtype
+  EXPECT_THROW( auto pose = msg->message<geometry_msgs::msg::Pose>(), BabelFishException );
+  EXPECT_NO_THROW( auto pose =
+                       ( *msg )["pose"].as<CompoundMessage>().message<geometry_msgs::msg::Pose>() );
+
   {
     auto pose = ( *msg )["pose"].as<CompoundMessage>();
     msg.reset();