Switch to rclcpp logging and improve messages

include/ros1_bridge/factory.hpp

@@ -20,6 +20,7 @@
 #include <string>
 
 #include "rmw/rmw.h"
+#include "rclcpp/rclcpp.hpp"
 
 // include ROS 1 message event
 #include "ros/message.h"
@@ -76,7 +77,8 @@ class Factory : public FactoryInterface
     ros::NodeHandle node,
     const std::string & topic_name,
     size_t queue_size,
-    rclcpp::PublisherBase::SharedPtr ros2_pub)
+    rclcpp::PublisherBase::SharedPtr ros2_pub,
+    rclcpp::Logger logger)
   {
     // workaround for https://github.com/ros/roscpp_core/issues/22 to get the connection header
     ros::SubscribeOptions ops;
@@ -88,7 +90,7 @@ class Factory : public FactoryInterface
       new ros::SubscriptionCallbackHelperT<const ros::MessageEvent<ROS1_T const> &>(
         boost::bind(
           &Factory<ROS1_T, ROS2_T>::ros1_callback,
-          _1, ros2_pub, ros1_type_name_, ros2_type_name_)));
+          _1, ros2_pub, ros1_type_name_, ros2_type_name_, logger)));
     return node.subscribe(ops);
   }
 
@@ -113,15 +115,11 @@ class Factory : public FactoryInterface
     ros::Publisher ros1_pub,
     rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr)
   {
-    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, ros1_type_name, ros2_type_name,
-      ros2_pub](const typename ROS2_T::SharedPtr msg, const rmw_message_info_t & msg_info) {
-        return this->ros2_callback(
-          msg, msg_info, ros1_pub, ros1_type_name, ros2_type_name, ros2_pub);
-      };
+    std::function<
+      void(const typename ROS2_T::SharedPtr msg, const rmw_message_info_t & msg_info)> callback;
+    callback = std::bind(
+      &Factory<ROS1_T, ROS2_T>::ros2_callback, std::placeholders::_1, std::placeholders::_2,
+      ros1_pub, ros1_type_name_, ros2_type_name_, node->get_logger(), ros2_pub);
     return node->create_subscription<ROS2_T>(
       topic_name, callback, qos, nullptr, true);
   }
@@ -146,20 +144,23 @@ class Factory : public FactoryInterface
     const ros::MessageEvent<ROS1_T const> & ros1_msg_event,
     rclcpp::PublisherBase::SharedPtr ros2_pub,
     const std::string & ros1_type_name,
-    const std::string & ros2_type_name)
+    const std::string & ros2_type_name,
+    rclcpp::Logger logger)
   {
     typename rclcpp::Publisher<ROS2_T>::SharedPtr typed_ros2_pub;
     typed_ros2_pub =
       std::dynamic_pointer_cast<typename rclcpp::Publisher<ROS2_T>>(ros2_pub);
 
     if (!typed_ros2_pub) {
-      throw std::runtime_error("Invalid type for publisher");
+      throw std::runtime_error(
+              "Invalid type " + ros2_type_name + " for ROS 2 publisher " +
+              ros2_pub->get_topic_name());
     }
 
     const boost::shared_ptr<ros::M_string> & connection_header =
       ros1_msg_event.getConnectionHeaderPtr();
     if (!connection_header) {
-      printf("  dropping message without connection header\n");
+      RCLCPP_WARN(logger, "Dropping ROS 1 message %s without connection header", ros1_type_name);
       return;
     }
 
@@ -174,10 +175,9 @@ 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 %s to ROS 2 %s (showing msg only once per type)",
-      ros1_type_name.c_str(), ros2_type_name.c_str());
+    RCLCPP_INFO_ONCE(
+      logger, "Passing message from ROS 1 %s to ROS 2 %s (showing msg only once per type)",
+      ros1_type_name, ros2_type_name);
     typed_ros2_pub->publish(ros2_msg);
   }
 
@@ -188,6 +188,7 @@ class Factory : public FactoryInterface
     ros::Publisher ros1_pub,
     const std::string & ros1_type_name,
     const std::string & ros2_type_name,
+    rclcpp::Logger logger,
     rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr)
   {
     if (ros2_pub) {
@@ -206,10 +207,9 @@ class Factory : public FactoryInterface
 
     ROS1_T ros1_msg;
     convert_2_to_1(*ros2_msg, ros1_msg);
-    RCUTILS_LOG_INFO_ONCE_NAMED(
-      "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());
+    RCLCPP_INFO_ONCE(
+      logger, "Passing message from ROS 2 %s to ROS 1 %s (showing msg only once per type)",
+      ros2_type_name, ros1_type_name);
     ros1_pub.publish(ros1_msg);
   }
 
@@ -241,34 +241,36 @@ class ServiceFactory : public ServiceFactoryInterface
   using ROS2Response = typename ROS2_T::Response;
 
   void forward_2_to_1(
-    ros::ServiceClient client, const std::shared_ptr<rmw_request_id_t>,
+    ros::ServiceClient client, rclcpp::Logger logger, const std::shared_ptr<rmw_request_id_t>,
     const std::shared_ptr<ROS2Request> request, std::shared_ptr<ROS2Response> response)
   {
     ROS1_T srv;
     translate_2_to_1(*request, srv.request);
     if (client.call(srv)) {
       translate_1_to_2(srv.response, *response);
     } else {
-      throw std::runtime_error("Failed to get response from ROS service");
+      throw std::runtime_error("Failed to get response from ROS 1 service " + client.getService());
     }
   }
 
   bool forward_1_to_2(
-    rclcpp::ClientBase::SharedPtr cli,
+    rclcpp::ClientBase::SharedPtr cli, rclcpp::Logger logger,
     const ROS1Request & request1, ROS1Response & response1)
   {
     auto client = std::dynamic_pointer_cast<rclcpp::Client<ROS2_T>>(cli);
     if (!client) {
-      fprintf(stderr, "Failed to get the client.\n");
+      RCLCPP_ERROR(logger, "Failed to get ROS 2 client %s", cli->get_service_name());
       return false;
     }
     auto request2 = std::make_shared<ROS2Request>();
     translate_1_to_2(request1, *request2);
     while (!client->wait_for_service(std::chrono::seconds(1))) {
       if (!rclcpp::ok()) {
-        fprintf(stderr, "Client was interrupted while waiting for the service. Exiting.\n");
+        RCLCPP_ERROR(
+          logger, "Interrupted while waiting for ROS 2 service %s", cli->get_service_name());
         return false;
       }
+      RCLCPP_WARN(logger, "Waiting for ROS 2 service %s...", cli->get_service_name());
     }
     auto timeout = std::chrono::seconds(5);
     auto future = client->async_send_request(request2);
@@ -277,7 +279,7 @@ class ServiceFactory : public ServiceFactoryInterface
       auto response2 = future.get();
       translate_2_to_1(*response2, response1);
     } else {
-      fprintf(stderr, "Failed to get response from ROS2 service.\n");
+      RCLCPP_ERROR(logger, "Failed to get response from ROS 2 service %s", cli->get_service_name());
       return false;
     }
     return true;
@@ -289,7 +291,9 @@ class ServiceFactory : public ServiceFactoryInterface
     ServiceBridge1to2 bridge;
     bridge.client = ros2_node->create_client<ROS2_T>(name);
     auto m = &ServiceFactory<ROS1_T, ROS2_T>::forward_1_to_2;
-    auto f = std::bind(m, this, bridge.client, std::placeholders::_1, std::placeholders::_2);
+    auto f = std::bind(
+      m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1,
+      std::placeholders::_2);
     bridge.server = ros1_node.advertiseService<ROS1Request, ROS1Response>(name, f);
     return bridge;
   }
@@ -306,7 +310,8 @@ class ServiceFactory : public ServiceFactoryInterface
         const std::shared_ptr<ROS2Request>,
         std::shared_ptr<ROS2Response>)> f;
     f = std::bind(
-      m, this, bridge.client, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
+      m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1,
+      std::placeholders::_2, std::placeholders::_3);
     bridge.server = ros2_node->create_service<ROS2_T>(name, f);
     return bridge;
   }

include/ros1_bridge/factory_interface.hpp

@@ -73,7 +73,8 @@ class FactoryInterface
     ros::NodeHandle node,
     const std::string & topic_name,
     size_t queue_size,
-    rclcpp::PublisherBase::SharedPtr ros2_pub) = 0;
+    rclcpp::PublisherBase::SharedPtr ros2_pub,
+    rclcpp::Logger logger) = 0;
 
   virtual
   rclcpp::SubscriptionBase::SharedPtr

src/bridge.cpp

@@ -36,7 +36,7 @@ create_bridge_from_1_to_2(
     ros2_node, ros2_topic_name, publisher_queue_size);
 
   auto ros1_sub = factory->create_ros1_subscriber(
-    ros1_node, ros1_topic_name, subscriber_queue_size, ros2_pub);
+    ros1_node, ros1_topic_name, subscriber_queue_size, ros2_pub, ros2_node->get_logger());
 
   Bridge1to2Handles handles;
   handles.ros1_subscriber = ros1_sub;
@@ -78,7 +78,7 @@ create_bidirectional_bridge(
   const std::string & topic_name,
   size_t queue_size)
 {
-  printf("create bidirectional bridge for topic [%s]\n", topic_name.c_str());
+  RCLCPP_INFO(ros2_node->get_logger(), "create bidirectional bridge for topic " + topic_name);
   BridgeHandles handles;
   handles.bridge1to2 = create_bridge_from_1_to_2(
     ros1_node, ros2_node,