Buffer server service

tf2_msgs/CMakeLists.txt

@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/TF2Error.msg"
   "msg/TFMessage.msg"
   "srv/FrameGraph.srv"
+  "srv/LookupTransform.srv"
   "action/LookupTransform.action"
   DEPENDENCIES builtin_interfaces geometry_msgs
   ADD_LINTER_TESTS

tf2_msgs/srv/LookupTransform.srv

@@ -0,0 +1,16 @@
+# Simple API
+string target_frame
+string source_frame
+builtin_interfaces/Time source_time
+builtin_interfaces/Duration timeout
+
+# Advanced API
+builtin_interfaces/Time target_time
+string fixed_frame
+
+# Whether or not to use the advanced API
+bool advanced
+
+---
+geometry_msgs/TransformStamped transform
+tf2_msgs/TF2Error error

tf2_ros/include/tf2_ros/buffer_server.h

@@ -51,8 +51,10 @@
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "rclcpp/create_timer.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "tf2_msgs/action/lookup_transform.hpp"
+#include "tf2_msgs/srv/lookup_transform.hpp"
 
 namespace tf2_ros
 {
@@ -64,6 +66,7 @@ namespace tf2_ros
 class BufferServer
 {
   using LookupTransformAction = tf2_msgs::action::LookupTransform;
+  using LookupTransformService = tf2_msgs::srv::LookupTransform;
   using GoalHandle = std::shared_ptr<rclcpp_action::ServerGoalHandle<LookupTransformAction>>;
 
 public:
@@ -89,6 +92,14 @@ class BufferServer
       std::bind(&BufferServer::cancelCB, this, std::placeholders::_1),
       std::bind(&BufferServer::acceptedCB, this, std::placeholders::_1));
 
+    service_server_ =  rclcpp::create_service<LookupTransformService>(
+      node->get_node_base_interface(),
+      node->get_node_services_interface(),
+      ns,
+      std::bind(&BufferServer::serviceCB, this, std::placeholders::_1, std::placeholders::_2),
+      rmw_qos_profile_services_default,
+      nullptr);
+
     check_timer_ = rclcpp::create_timer(
       node, node->get_clock(), check_period, std::bind(&BufferServer::checkTransforms, this));
     RCLCPP_DEBUG(logger_, "Buffer server started");
@@ -100,6 +111,11 @@ class BufferServer
     GoalHandle handle;
     tf2::TimePoint end_time;
   };
+
+  TF2_ROS_PUBLIC
+  void serviceCB(
+    const std::shared_ptr<LookupTransformService::Request> request,
+    std::shared_ptr<LookupTransformService::Response> response);
 
   TF2_ROS_PUBLIC
   rclcpp_action::GoalResponse goalCB(
@@ -117,12 +133,16 @@ class BufferServer
   TF2_ROS_PUBLIC
   bool canTransform(GoalHandle gh);
 
+  TF2_ROS_PUBLIC
+  bool canTransform(const std::shared_ptr<LookupTransformService::Request> request);
+
   TF2_ROS_PUBLIC
   geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh);
 
   const tf2::BufferCoreInterface & buffer_;
   rclcpp::Logger logger_;
   rclcpp_action::Server<LookupTransformAction>::SharedPtr server_;
+  rclcpp::Service<LookupTransformService>::SharedPtr service_server_;
   std::list<GoalInfo> active_goals_;
   std::mutex mutex_;
   rclcpp::TimerBase::SharedPtr check_timer_;

tf2_ros/src/buffer_server.cpp

@@ -47,6 +47,46 @@
 
 namespace tf2_ros
 {
+
+void BufferServer::serviceCB(
+  const std::shared_ptr<LookupTransformService::Request> request,
+  std::shared_ptr<LookupTransformService::Response> response)
+{
+  // TODO: implement retrying + timeout
+  try {
+    // check whether we need to use the advanced or simple api
+    if (request->advanced) {
+      response->transform = buffer_.lookupTransform(
+        request->target_frame, tf2_ros::fromMsg(request->target_time),
+        request->source_frame, tf2_ros::fromMsg(request->source_time), request->fixed_frame);
+    }
+    else {
+      response->transform = buffer_.lookupTransform(
+      request->target_frame, request->source_frame,
+      tf2_ros::fromMsg(request->source_time));
+    }
+
+  } catch (const tf2::ConnectivityException & ex) {
+    response->error.error = response->error.CONNECTIVITY_ERROR;
+    response->error.error_string = ex.what();
+  } catch (const tf2::LookupException & ex) {
+    response->error.error = response->error.LOOKUP_ERROR;
+    response->error.error_string = ex.what();
+  } catch (const tf2::ExtrapolationException & ex) {
+    response->error.error = response->error.EXTRAPOLATION_ERROR;
+    response->error.error_string = ex.what();
+  } catch (const tf2::InvalidArgumentException & ex) {
+    response->error.error = response->error.INVALID_ARGUMENT_ERROR;
+    response->error.error_string = ex.what();
+  } catch (const tf2::TimeoutException & ex) {
+    response->error.error = response->error.TIMEOUT_ERROR;
+    response->error.error_string = ex.what();
+  } catch (const tf2::TransformException & ex) {
+    response->error.error = response->error.TRANSFORM_ERROR;
+    response->error.error_string = ex.what();
+  } 
+}
+
 void BufferServer::checkTransforms()
 {
   std::unique_lock<std::mutex> lock(mutex_);
@@ -145,6 +185,8 @@ rclcpp_action::CancelResponse BufferServer::cancelCB(GoalHandle gh)
 rclcpp_action::GoalResponse BufferServer::goalCB(
   const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const LookupTransformAction::Goal> goal)
 {
+  RCLCPP_WARN_ONCE(logger_, "You are using the deprecated action interface of the tf2_ros::BufferServer. \
+   Please use the service interface instead.");
   (void)uuid;
   (void)goal;
   // accept all goals we get
@@ -215,11 +257,27 @@ bool BufferServer::canTransform(GoalHandle gh)
     goal->source_frame, source_time_point, goal->fixed_frame, nullptr);
 }
 
+bool BufferServer::canTransform(const std::shared_ptr<LookupTransformService::Request> request)
+{
+  tf2::TimePoint source_time_point = tf2_ros::fromMsg(request->source_time);
+
+  // check whether we need to used the advanced or simple api
+  if (!request->advanced) {
+    return buffer_.canTransform(
+      request->target_frame, request->source_frame, source_time_point, nullptr);
+  }
+
+  tf2::TimePoint target_time_point = tf2_ros::fromMsg(request->target_time);
+  return buffer_.canTransform(
+    request->target_frame, target_time_point,
+    request->source_frame, source_time_point, request->fixed_frame, nullptr);
+}
+
 geometry_msgs::msg::TransformStamped BufferServer::lookupTransform(GoalHandle gh)
 {
   const auto goal = gh->get_goal();
 
-  // check whether we need to used the advanced or simple api
+  // check whether we need to use the advanced or simple api
   if (!goal->advanced) {
     return buffer_.lookupTransform(
       goal->target_frame, goal->source_frame,