@INPROCEEDINGS{fabian2021hri,
  author = {Stefan Fabian and Oskar von Stryk},
  title = {Open-Source Tools for Efficient ROS and ROS2-based 2D Human-Robot Interface Development},
  year = {2021},
  booktitle = {2021 European Conference on Mobile Robots (ECMR)},
}

NodeHandle nh;
BabelFish fish;
// Subscribe topic /pose
ros::Subscriber sub = nh.subscribe<ros_babel_fish::BabelFishMessage>( topic, 1, &callback );

/* ... */

void callback( const ros_babel_fish::BabelFishMessage::ConstPtr &msg )
{
  std::cout << "Message received!" << std::endl;
  std::cout << "Data Type:" << msg->dataType() << std::endl; // geometry_msgs/Pose
  std::cout << "MD5:" << msg->md5Sum() << std::endl; // e45d45a5a1ce597b249e23fb30fc871f
  std::cout << "Message Definition:" << std::endl;
  std::cout << msg->definition() << std::endl;
  std::cout << "Message Content:" << std::endl;
  TranslatedMessage::Ptr translated = fish->translateMessage( msg );
  auto &compound = translated->translated_message->as<CompoundMessage>();
  std::cout << "Position: " << compound["position"]["x"].value<double>() << ", " << compound["position"]["y"].value<double>() << ", "
            << compound["position"]["z"].value<double>() << std::endl;
  std::cout << "Orientation: " << compound["orientation"]["w"].value<double>() << ", " << compound["orientation"]["x"].value<double>() << ", "
            << compound["orientation"]["y"].value<double>() << ", " << compound["orientation"]["z"].value<double>() << std::endl;
};

NodeHandle nh;
BabelFish fish;
// Advertise a publisher on topic /pose
ros::Publisher pub_pose = fish.advertise( nh, "geometry_msgs/Pose", "/pose", 1, true );

Message::Ptr message = fish.createMessage( "geometry_msgs/Pose" );
auto &compound = message->as<CompoundMessage>();
compound["position"].as<CompoundMessage>()["x"].as<ValueMessage<double>>().setValue(1.1);
// or using convenience methods
compound["position"]["y"].as<ValueMessage<double>>().setValue(2.4);
// or using even more convenience methods
compound["position"]["z"] = 3.6;
// Be careful with your types here. Casting to a wrong type will throw an exception!
// The as<ValueMessage<T>> method is also a bit faster because the convenience method
// will automatically convert to the right type and perform bound and compatibility checks.
// This makes it more robust but comes with a little overhead.
// Note that assigning a double to a float field will always throw an exception because
// the float may not have the required resolution.
// Assigning, e.g., an int to a uint8 field will only throw if the int is out of bounds (0-255)
// otherwise a warning will be printed because uint8 is not compatible with all possible values
// of int. This warning can be disabled as a compile option. 

compound["orientation"]["w"] = 0.384;
compound["orientation"]["x"] = -0.003;
compound["orientation"]["y"] = -0.876;
compound["orientation"]["z"] = 0.292;

BabelFishMessage::Ptr translated_message = fish.translateMessage( message );
pub_pose.publish( translated_message );

BabelFish fish;
MessageExtractor extractor(fish);
// We want the position of a pose stamped
SubMessageLocation location = extractor.retrieveLocationForPath( "geometry_msgs/PoseStamped", "pose.position" );
BabelFishMessage::ConstPtr msg = ros::topic::waitForMessage<BabelFishMessage>( "/topic" );
TranslatedMessage::Ptr translated = extractor.extractMessage( msg, location );
auto &compound = translated->translated_message->as<CompoundMessage>();
std::cout << "Position: " << compound["x"].value<double>() << ", " << compound["y"].value<double>() << ", "
          << compound["z"].value<double>() << std::endl;

// You can also extract primitives (but you need to make sure you extract the right type or the extractor will throw!)
SubMessageLocation location_of_x = extractor.retrieveLocationForPath( "geometry_msgs/PoseStamped", "pose.position.x" );
std::cout << "Position X: " << extractor.extractValue<double>( msg, location_of_x ) << std::endl;