diff --git a/ros_babel_fish/CMakeLists.txt b/ros_babel_fish/CMakeLists.txt index 7067279..88ffa9a 100644 --- a/ros_babel_fish/CMakeLists.txt +++ b/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 $ $ ) +ament_target_dependencies(any_subscriber geometry_msgs) add_executable(troll_node examples/troll_node.cpp) target_link_libraries(troll_node ${PROJECT_NAME}) diff --git a/ros_babel_fish/examples/any_subscriber.cpp b/ros_babel_fish/examples/any_subscriber.cpp index 7eabf3b..a14d178 100644 --- a/ros_babel_fish/examples/any_subscriber.cpp +++ b/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 +#include +#include #include #include #include -#include -#include /*! * 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(); + 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(); + std::cout << "Point { x: " << point_msg->x << ", y: " << point_msg->y << ", z: " << point_msg->z + << " }" << std::endl; +} template 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(); - std::cout << "Time { seconds: " << time.seconds() << " }"; + std::cout << "Time { seconds: " << time.seconds() << " }" << std::endl; return; } if ( compound.isDuration() ) { auto duration = compound.value(); - 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(); diff --git a/ros_babel_fish/include/ros_babel_fish/messages/compound_message.hpp b/ros_babel_fish/include/ros_babel_fish/messages/compound_message.hpp index 6a8f024..37a505f 100644 --- a/ros_babel_fish/include/ros_babel_fish/messages/compound_message.hpp +++ b/ros_babel_fish/include/ros_babel_fish/messages/compound_message.hpp @@ -82,6 +82,41 @@ class CompoundMessage final : public Message std::shared_ptr type_erased_message(); + /*! + * Cast this compound message to a message type. + * Example: + * \code{.cpp} + * using geometry_msgs::msg::Point; + * Point point = compound.message<Point>(); + * \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 + std::shared_ptr message() + { + if ( rosidl_generator_traits::name() != name() ) { + throw BabelFishException( "Tried to cast compound message of type " + name() + + " to incompatible message type " + + rosidl_generator_traits::name() + "!" ); + } + return std::static_pointer_cast( type_erased_message() ); + } + + //! @copydoc message() + template + std::shared_ptr message() const + { + if ( rosidl_generator_traits::name() != name() ) { + throw BabelFishException( "Tried to cast compound message of type " + name() + + " to incompatible message type " + + rosidl_generator_traits::name() + "!" ); + } + return std::static_pointer_cast( type_erased_message() ); + } + //! Creates a copy of this compound message CompoundMessage clone() const; diff --git a/ros_babel_fish/include/ros_babel_fish/messages/message.hpp b/ros_babel_fish/include/ros_babel_fish/messages/message.hpp index e146d25..b40e8a8 100644 --- a/ros_babel_fish/include/ros_babel_fish/messages/message.hpp +++ b/ros_babel_fish/include/ros_babel_fish/messages/message.hpp @@ -48,10 +48,8 @@ class Message template T value() const { - auto result = std::dynamic_pointer_cast( 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 T &as() { - T *result = dynamic_cast( this ); + auto result = dynamic_cast( 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( this ); - if ( result == nullptr ) + if ( result == nullptr ) { throw BabelFishException( "Tried to cast message to incompatible type!" ); + } return *result; } diff --git a/ros_babel_fish/package.xml b/ros_babel_fish/package.xml index 1b09ceb..d00d324 100644 --- a/ros_babel_fish/package.xml +++ b/ros_babel_fish/package.xml @@ -16,14 +16,14 @@ ament_cmake ament_index_cpp + geometry_msgs rclcpp rclcpp_action rcpputils rosidl_runtime_cpp rosidl_typesupport_cpp rosidl_typesupport_introspection_cpp - action_tutorials_interfaces - geometry_msgs + example_interfaces ament_cmake_gtest ament_cmake_clang_format ament_cmake_cppcheck @@ -32,6 +32,7 @@ geometry_msgs ros_babel_fish_test_msgs std_msgs + ament_cmake diff --git a/ros_babel_fish/test/message.cpp b/ros_babel_fish/test/message.cpp index 8c32ef6..d298dff 100644 --- a/ros_babel_fish/test/message.cpp +++ b/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(), BabelFishException ); + EXPECT_NO_THROW( auto pose = + ( *msg )["pose"].as().message() ); + { auto pose = ( *msg )["pose"].as(); msg.reset();