From bed7b864ef018f1cada8655d52e259d321068924 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Thu, 14 Feb 2019 17:41:50 -0500 Subject: [PATCH 1/4] Switch to rclcpp logging and improve messages --- include/ros1_bridge/factory.hpp | 66 ++++++++++++----------- include/ros1_bridge/factory_interface.hpp | 3 +- src/bridge.cpp | 4 +- 3 files changed, 40 insertions(+), 33 deletions(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index d8bc2f39..9749cde8 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -20,6 +20,7 @@ #include #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 &>( boost::bind( &Factory::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::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( topic_name, callback, qos, nullptr, true); } @@ -146,20 +144,23 @@ class Factory : public FactoryInterface const ros::MessageEvent & 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::SharedPtr typed_ros2_pub; typed_ros2_pub = std::dynamic_pointer_cast>(ros2_pub); if (!typed_ros2_pub) { - throw std::runtime_error("Invalid type for publisher"); + throw std::runtime_error("Invalid type " + ros2_type_name + " for ROS2 publisher " + + ros2_pub->get_topic_name()); } const boost::shared_ptr & connection_header = ros1_msg_event.getConnectionHeaderPtr(); if (!connection_header) { - printf(" dropping message without connection header\n"); + RCLCPP_WARN(logger, "Dropping ROS1 message %s without connection header", + ros1_type_name.c_str()); return; } @@ -174,9 +175,8 @@ class Factory : public FactoryInterface auto ros2_msg = std::make_shared(); 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)", + RCLCPP_INFO_ONCE(logger, + "Passing message from ROS1 %s to ROS2 %s (showing msg only once per type)", ros1_type_name.c_str(), ros2_type_name.c_str()); 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 ROS2 %s to ROS1 %s (showing msg only once per type)", + ros2_type_name.c_str(), ros1_type_name.c_str()); ros1_pub.publish(ros1_msg); } @@ -241,34 +241,37 @@ class ServiceFactory : public ServiceFactoryInterface using ROS2Response = typename ROS2_T::Response; void forward_2_to_1( - ros::ServiceClient client, const std::shared_ptr, + ros::ServiceClient & client, rclcpp::Logger logger, const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { ROS1_T srv; + auto service_name = client.getService(); 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 ROS1 service " + service_name); } } bool forward_1_to_2( - rclcpp::ClientBase::SharedPtr cli, + rclcpp::ClientBase::SharedPtr cli, rclcpp::Logger logger, const ROS1Request & request1, ROS1Response & response1) { + auto service_name = std::string(cli->get_service_name()); auto client = std::dynamic_pointer_cast>(cli); if (!client) { - fprintf(stderr, "Failed to get the client.\n"); + RCLCPP_ERROR(logger, "Failed to get ROS2 client " + service_name); return false; } auto request2 = std::make_shared(); translate_1_to_2(request1, *request2); - while (!client->wait_for_service(std::chrono::seconds(1))) { + while (!client->wait_for_service(std::chrono::seconds(5))) { if (!rclcpp::ok()) { - fprintf(stderr, "Client was interrupted while waiting for the service. Exiting.\n"); + RCLCPP_ERROR(logger, "Interrupted while waiting for ROS2 service " + service_name); return false; } + RCLCPP_WARN(logger, "Waiting for ROS2 service " + service_name + "..."); } auto timeout = std::chrono::seconds(5); auto future = client->async_send_request(request2); @@ -277,7 +280,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 ROS2 service " + service_name); return false; } return true; @@ -289,7 +292,9 @@ class ServiceFactory : public ServiceFactoryInterface ServiceBridge1to2 bridge; bridge.client = ros2_node->create_client(name); auto m = &ServiceFactory::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(name, f); return bridge; } @@ -306,7 +311,8 @@ class ServiceFactory : public ServiceFactoryInterface const std::shared_ptr, std::shared_ptr)> 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(name, f); return bridge; } diff --git a/include/ros1_bridge/factory_interface.hpp b/include/ros1_bridge/factory_interface.hpp index 870947e3..595e559d 100755 --- a/include/ros1_bridge/factory_interface.hpp +++ b/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 diff --git a/src/bridge.cpp b/src/bridge.cpp index 25eb74f3..2e154b24 100755 --- a/src/bridge.cpp +++ b/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, From 4dc78d51c2b2a50c1a33e048ff1ff072b83d1939 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Fri, 1 Mar 2019 13:33:05 -0500 Subject: [PATCH 2/4] Review changes --- include/ros1_bridge/factory.hpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index 9749cde8..bb956875 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -152,15 +152,15 @@ class Factory : public FactoryInterface std::dynamic_pointer_cast>(ros2_pub); if (!typed_ros2_pub) { - throw std::runtime_error("Invalid type " + ros2_type_name + " for ROS2 publisher " + + throw std::runtime_error( + "Invalid type " + ros2_type_name + " for ROS 2 publisher " + ros2_pub->get_topic_name()); } const boost::shared_ptr & connection_header = ros1_msg_event.getConnectionHeaderPtr(); if (!connection_header) { - RCLCPP_WARN(logger, "Dropping ROS1 message %s without connection header", - ros1_type_name.c_str()); + RCLCPP_WARN(logger, "Dropping ROS 1 message %s without connection header", ros1_type_name); return; } @@ -175,9 +175,9 @@ class Factory : public FactoryInterface auto ros2_msg = std::make_shared(); convert_1_to_2(*ros1_msg, *ros2_msg); - RCLCPP_INFO_ONCE(logger, - "Passing message from ROS1 %s to ROS2 %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); } @@ -207,9 +207,9 @@ class Factory : public FactoryInterface ROS1_T ros1_msg; convert_2_to_1(*ros2_msg, ros1_msg); - RCLCPP_INFO_ONCE(logger, - "Passing message from ROS2 %s to ROS1 %s (showing msg only once per type)", - ros2_type_name.c_str(), ros1_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,7 +241,7 @@ class ServiceFactory : public ServiceFactoryInterface using ROS2Response = typename ROS2_T::Response; void forward_2_to_1( - ros::ServiceClient & client, rclcpp::Logger logger, const std::shared_ptr, + ros::ServiceClient client, rclcpp::Logger logger, const std::shared_ptr, const std::shared_ptr request, std::shared_ptr response) { ROS1_T srv; @@ -250,7 +250,7 @@ class ServiceFactory : public ServiceFactoryInterface if (client.call(srv)) { translate_1_to_2(srv.response, *response); } else { - throw std::runtime_error("Failed to get response from ROS1 service " + service_name); + throw std::runtime_error("Failed to get response from ROS 1 service " + service_name); } } @@ -261,17 +261,17 @@ class ServiceFactory : public ServiceFactoryInterface auto service_name = std::string(cli->get_service_name()); auto client = std::dynamic_pointer_cast>(cli); if (!client) { - RCLCPP_ERROR(logger, "Failed to get ROS2 client " + service_name); + RCLCPP_ERROR(logger, "Failed to get ROS 2 client %s", service_name); return false; } auto request2 = std::make_shared(); translate_1_to_2(request1, *request2); - while (!client->wait_for_service(std::chrono::seconds(5))) { + while (!client->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { - RCLCPP_ERROR(logger, "Interrupted while waiting for ROS2 service " + service_name); + RCLCPP_ERROR(logger, "Interrupted while waiting for ROS 2 service %s", service_name); return false; } - RCLCPP_WARN(logger, "Waiting for ROS2 service " + service_name + "..."); + RCLCPP_WARN(logger, "Waiting for ROS2 service %s...", service_name); } auto timeout = std::chrono::seconds(5); auto future = client->async_send_request(request2); @@ -280,7 +280,7 @@ class ServiceFactory : public ServiceFactoryInterface auto response2 = future.get(); translate_2_to_1(*response2, response1); } else { - RCLCPP_ERROR(logger, "Failed to get response from ROS2 service " + service_name); + RCLCPP_ERROR(logger, "Failed to get response from ROS 2 service %s", service_name); return false; } return true; From 2678029b091eb05324e2a45b7d69ddae1ff03d5e Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 5 Mar 2019 10:25:03 -0500 Subject: [PATCH 3/4] Recreate changes --- include/ros1_bridge/factory.hpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index bb956875..b3e725bb 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -245,12 +245,11 @@ class ServiceFactory : public ServiceFactoryInterface const std::shared_ptr request, std::shared_ptr response) { ROS1_T srv; - auto service_name = client.getService(); 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 1 service " + service_name); + throw std::runtime_error("Failed to get response from ROS 1 service " + client.getService()); } } @@ -258,20 +257,19 @@ class ServiceFactory : public ServiceFactoryInterface rclcpp::ClientBase::SharedPtr cli, rclcpp::Logger logger, const ROS1Request & request1, ROS1Response & response1) { - auto service_name = std::string(cli->get_service_name()); auto client = std::dynamic_pointer_cast>(cli); if (!client) { - RCLCPP_ERROR(logger, "Failed to get ROS 2 client %s", service_name); + RCLCPP_ERROR(logger, "Failed to get ROS 2 client %s", cli->get_service_name()); return false; } auto request2 = std::make_shared(); translate_1_to_2(request1, *request2); while (!client->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { - RCLCPP_ERROR(logger, "Interrupted while waiting for ROS 2 service %s", service_name); + RCLCPP_ERROR(logger, "Interrupted while waiting for ROS 2 service %s", cli->get_service_name()); return false; } - RCLCPP_WARN(logger, "Waiting for ROS2 service %s...", service_name); + 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); @@ -280,7 +278,7 @@ class ServiceFactory : public ServiceFactoryInterface auto response2 = future.get(); translate_2_to_1(*response2, response1); } else { - RCLCPP_ERROR(logger, "Failed to get response from ROS 2 service %s", service_name); + RCLCPP_ERROR(logger, "Failed to get response from ROS 2 service %s", cli->get_service_name()); return false; } return true; From 4848722d88dd6eddcb3ab5de5c1a84d8d62e7342 Mon Sep 17 00:00:00 2001 From: Paul Bovbel Date: Tue, 5 Mar 2019 12:13:03 -0500 Subject: [PATCH 4/4] Fix lint --- include/ros1_bridge/factory.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/ros1_bridge/factory.hpp b/include/ros1_bridge/factory.hpp index b3e725bb..6be91b3e 100755 --- a/include/ros1_bridge/factory.hpp +++ b/include/ros1_bridge/factory.hpp @@ -266,7 +266,8 @@ class ServiceFactory : public ServiceFactoryInterface translate_1_to_2(request1, *request2); while (!client->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { - RCLCPP_ERROR(logger, "Interrupted while waiting for ROS 2 service %s", cli->get_service_name()); + 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());