Skip to content

Commit

Permalink
Added a method to get the actual underlying message from a compound m…
Browse files Browse the repository at this point in the history
…essage.

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

(cherry picked from commit b89cce1)
  • Loading branch information
StefanFabian committed Jan 6, 2025
1 parent 89109d4 commit 1c65ced
Show file tree
Hide file tree
Showing 6 changed files with 85 additions and 15 deletions.
2 changes: 2 additions & 0 deletions ros_babel_fish/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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})
Expand Down
42 changes: 35 additions & 7 deletions ros_babel_fish/examples/any_subscriber.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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:
Expand All @@ -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() );
}
Expand All @@ -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 )
Expand Down Expand Up @@ -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>();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
11 changes: 5 additions & 6 deletions ros_babel_fish/include/ros_babel_fish/messages/message.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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!" );
}

/*!
Expand All @@ -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;
Expand All @@ -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;
}

Expand Down
5 changes: 3 additions & 2 deletions ros_babel_fish/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
<build_depend>example_interfaces</build_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_clang_format</test_depend>
<test_depend>ament_cmake_cppcheck</test_depend>
Expand All @@ -32,6 +32,7 @@
<test_depend>geometry_msgs</test_depend>
<test_depend>ros_babel_fish_test_msgs</test_depend>
<test_depend>std_msgs</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
5 changes: 5 additions & 0 deletions ros_babel_fish/test/message.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down

0 comments on commit 1c65ced

Please sign in to comment.