From ebe9d2b9f381b9a2a4de49a0dc876ab884c21f1f Mon Sep 17 00:00:00 2001
From: Dirk Thomas <dirk-thomas@users.noreply.github.com>
Date: Thu, 5 Oct 2017 17:05:00 -0700
Subject: [PATCH] print bridged type names

---
 include/ros1_bridge/factory.hpp | 44 +++++++++++++++++++++++----------
 resource/pkg_factories.cpp.em   |  2 +-
 2 files changed, 32 insertions(+), 14 deletions(-)

diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp
index 770546b5..bcdbc798 100644
--- a/include/ros1_bridge/factory.hpp
+++ b/include/ros1_bridge/factory.hpp
@@ -26,8 +26,6 @@
 
 #include "ros1_bridge/factory_interface.hpp"
 
-#define _ros1_bridge_type_to_name(x) #x
-
 namespace ros1_bridge
 {
 
@@ -35,6 +33,14 @@ template<typename ROS1_T, typename ROS2_T>
 class Factory : public FactoryInterface
 {
 public:
+  Factory(
+    const std::string & ros1_type_name, const std::string & ros2_type_name)
+  : ros1_type_name_(ros1_type_name),
+    ros2_type_name_(ros2_type_name)
+  {
+    fprintf(stderr, "Factory(%s, %s)\n", ros1_type_name_.c_str(), ros2_type_name_.c_str());
+  }
+
   ros::Publisher
   create_ros1_publisher(
     ros::NodeHandle node,
@@ -70,7 +76,9 @@ class Factory : public FactoryInterface
     ops.datatype = ros::message_traits::datatype<ROS1_T>();
     ops.helper = ros::SubscriptionCallbackHelperPtr(
       new ros::SubscriptionCallbackHelperT<const ros::MessageEvent<ROS1_T const> &>(
-        boost::bind(&Factory<ROS1_T, ROS2_T>::ros1_callback, _1, ros2_pub)));
+        boost::bind(
+          &Factory<ROS1_T, ROS2_T>::ros1_callback,
+          _1, ros2_pub, ros1_type_name_, ros2_type_name_)));
     return node.subscribe(ops);
   }
 
@@ -83,10 +91,13 @@ class Factory : public FactoryInterface
   {
     rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data;
     custom_qos_profile.depth = queue_size;
+    const std::string & ros1_type_name = ros1_type_name_;
+    const std::string & ros2_type_name = ros2_type_name_;
     // TODO(wjwwood): use a lambda until create_subscription supports std/boost::bind.
     auto callback =
-      [this, ros1_pub](const typename ROS2_T::SharedPtr msg) {
-        return this->ros2_callback(msg, ros1_pub);
+      [this, ros1_pub, ros1_type_name, ros2_type_name](const typename ROS2_T::SharedPtr msg) {
+        return this->ros2_callback(
+          msg, ros1_pub, ros1_type_name, ros2_type_name);
       };
     return node->create_subscription<ROS2_T>(
       topic_name, callback, custom_qos_profile, nullptr, true);
@@ -96,7 +107,9 @@ class Factory : public FactoryInterface
   static
   void ros1_callback(
     const ros::MessageEvent<ROS1_T const> & ros1_msg_event,
-    rclcpp::publisher::PublisherBase::SharedPtr ros2_pub)
+    rclcpp::publisher::PublisherBase::SharedPtr ros2_pub,
+    const std::string & ros1_type_name,
+    const std::string & ros2_type_name)
   {
     typename rclcpp::publisher::Publisher<ROS2_T>::SharedPtr typed_ros2_pub;
     typed_ros2_pub =
@@ -125,23 +138,25 @@ class Factory : public FactoryInterface
     auto ros2_msg = std::make_shared<ROS2_T>();
     convert_1_to_2(*ros1_msg, *ros2_msg);
     RCUTILS_LOG_INFO_ONCE_NAMED(
-      "ros1_bridge", "Passing message from ROS 1 "
-      _ros1_bridge_type_to_name(ROS1_T) " to ROS 2 "
-      _ros1_bridge_type_to_name(ROS2_T) " (showing msg only once per type)");
+      "ros1_bridge",
+      "Passing message from ROS 1 %s to ROS 2 %s (showing msg only once per type)",
+      ros1_type_name.c_str(), ros2_type_name.c_str());
     typed_ros2_pub->publish(ros2_msg);
   }
 
   static
   void ros2_callback(
     typename ROS2_T::SharedPtr ros2_msg,
-    ros::Publisher ros1_pub)
+    ros::Publisher ros1_pub,
+    const std::string & ros1_type_name,
+    const std::string & ros2_type_name)
   {
     ROS1_T ros1_msg;
     convert_2_to_1(*ros2_msg, ros1_msg);
     RCUTILS_LOG_INFO_ONCE_NAMED(
-      "ros1_bridge", "Passing message from ROS 2 "
-      _ros1_bridge_type_to_name(ROS2_T) " to ROS 1 "
-      _ros1_bridge_type_to_name(ROS1_T) " (showing msg only once per type)");
+      "ros1_bridge",
+      "Passing message from ROS 2 %s to ROS 1 %s (showing msg only once per type)",
+      ros1_type_name.c_str(), ros2_type_name.c_str());
     ros1_pub.publish(ros1_msg);
   }
 
@@ -158,6 +173,9 @@ class Factory : public FactoryInterface
   convert_2_to_1(
     const ROS2_T & ros2_msg,
     ROS1_T & ros1_msg);
+
+  std::string ros1_type_name_;
+  std::string ros2_type_name_;
 };
 
 template<class ROS1_T, class ROS2_T>
diff --git a/resource/pkg_factories.cpp.em b/resource/pkg_factories.cpp.em
index ee450338..d79ed2a1 100644
--- a/resource/pkg_factories.cpp.em
+++ b/resource/pkg_factories.cpp.em
@@ -58,7 +58,7 @@ get_factory_@(ros2_package_name)(const std::string & ros1_type_name, const std::
         @(m.ros1_msg.package_name)::@(m.ros1_msg.message_name),
         @(m.ros2_msg.package_name)::msg::@(m.ros2_msg.message_name)
       >
-    >();
+    >("@(m.ros1_msg.package_name)/@(m.ros1_msg.message_name)", ros2_type_name);
   }
 @[end for]@
   return std::shared_ptr<FactoryInterface>();