diff --git a/ros1_foxglove_bridge/src/service_utils.cpp b/ros1_foxglove_bridge/src/service_utils.cpp index 0cc47ce..0f4eadb 100644 --- a/ros1_foxglove_bridge/src/service_utils.cpp +++ b/ros1_foxglove_bridge/src/service_utils.cpp @@ -2,26 +2,46 @@ #include #include +#include +#include #include -#include +#include +#include #include namespace foxglove_bridge { +/** + * Looks up the service server host & port and opens a TCP connection to it to retrieve the header + * which contains the service type. + * + * The implementation is similar to how ROS does it under the hood when creating a service server + * link: + * https://github.com/ros/ros_comm/blob/845f74602c7464e08ef5ac6fd9e26c97d0fe42c9/clients/roscpp/src/libros/service_manager.cpp#L246-L261 + * https://github.com/ros/ros_comm/blob/845f74602c7464e08ef5ac6fd9e26c97d0fe42c9/clients/roscpp/src/libros/service_server_link.cpp#L114-L130 + */ std::string retrieveServiceType(const std::string& serviceName, std::chrono::milliseconds timeout) { - auto link = ros::ServiceManager::instance()->createServiceServerLink(serviceName, false, "*", "*", - {{"probe", "1"}}); - if (!link) { - throw std::runtime_error("Failed to create service link"); - } else if (!link->getConnection()) { - throw std::runtime_error("Failed to get service link connection"); + std::string srvHost; + uint32_t srvPort; + if (!ros::ServiceManager::instance()->lookupService(serviceName, srvHost, srvPort)) { + throw std::runtime_error("Failed to lookup service " + serviceName); + } + + auto transport = + boost::make_shared(&ros::PollManager::instance()->getPollSet()); + auto connection = boost::make_shared(); + ros::ConnectionManager::instance()->addConnection(connection); + connection->initialize(transport, false, ros::HeaderReceivedFunc()); + + if (!transport->connect(srvHost, srvPort)) { + throw std::runtime_error("Failed to connect to service server of service " + serviceName); } std::promise promise; auto future = promise.get_future(); - link->getConnection()->setHeaderReceivedCallback( + connection->setHeaderReceivedCallback( [&promise](const ros::ConnectionPtr& conn, const ros::Header& header) { std::string serviceType; if (header.getValue("type", serviceType)) { @@ -35,10 +55,18 @@ std::string retrieveServiceType(const std::string& serviceName, std::chrono::mil return true; }); + ros::M_string header; + header["service"] = serviceName; + header["md5sum"] = "*"; + header["callerid"] = ros::this_node::getName(); + header["persistent"] = "0"; + header["probe"] = "1"; + connection->writeHeader(header, [](const ros::ConnectionPtr&) {}); + if (future.wait_for(timeout) != std::future_status::ready) { - // Drop connection here, removes the link from the service manager instance and avoids - // that the header-received callback is called after the promise has already been destroyed. - link->getConnection()->drop(ros::Connection::DropReason::Destructing); + // Drop the connection here to prevent that the header-received callback is called after the + // promise has already been destroyed. + connection->drop(ros::Connection::DropReason::Destructing); throw std::runtime_error("Timed out when retrieving service type"); }