Skip to content

Commit

Permalink
Add wait to ROS1 server and improve logging
Browse files Browse the repository at this point in the history
  • Loading branch information
@paulbovbel
paulbovbel committed Feb 16, 2019
1 parent 961ddee commit ad5d401
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 32 deletions.
70 changes: 41 additions & 29 deletions include/ros1_bridge/factory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <string>

#include "rmw/rmw.h"
#include "rclcpp/rclcpp.hpp"

// include ROS 1 message event
#include "ros/message.h"
Expand Down Expand Up @@ -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;
Expand All @@ -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);
}

Expand All @@ -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);
}
Expand All @@ -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 ROS2 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 ROS1 message %s without connection header",
ros1_type_name.c_str());
return;
}

Expand All @@ -174,9 +175,8 @@ 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)",
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);
}
Expand All @@ -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) {
Expand All @@ -206,9 +207,8 @@ 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)",
RCLCPP_INFO_ONCE(logger,
"Passing message from ROS2 %s to ROS1 %s (showing msg only once per type)",
ros1_type_name.c_str(), ros2_type_name.c_str());
ros1_pub.publish(ros1_msg);
}
Expand Down Expand Up @@ -241,34 +241,43 @@ 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;
auto service_name = client.getService();
translate_2_to_1(*request, srv.request);
while (!client.waitForExistence(ros::Duration(5))) {
if (!ros::ok()) {
throw std::runtime_error("Interrupted while waiting for ROS1 service " + service_name);
}
RCLCPP_WARN(logger, "Waiting for ROS1 service " + service_name + "...");
}
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<rclcpp::Client<ROS2_T>>(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<ROS2Request>();
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);
Expand All @@ -277,7 +286,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;
Expand All @@ -289,7 +298,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;
}
Expand All @@ -306,7 +317,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;
}
Expand Down
3 changes: 2 additions & 1 deletion include/ros1_bridge/factory_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions src/bridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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,
Expand Down

0 comments on commit ad5d401

Please sign in to comment.