Buffer server service

tf2_msgs/CMakeLists.txt

@@ -18,6 +18,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,17 @@
+#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,10 @@ 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 +132,19 @@ 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);
 
+  TF2_ROS_PUBLIC
+  geometry_msgs::msg::TransformStamped lookupTransform(const std::shared_ptr<LookupTransformService::Request> request);
+
   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

@@ -143,6 +143,34 @@ rclcpp_action::CancelResponse BufferServer::cancelCB(GoalHandle gh)
   return rclcpp_action::CancelResponse::REJECT;
 }
 
+void BufferServer::serviceCB(const std::shared_ptr<LookupTransformService::Request> request,
+          std::shared_ptr<LookupTransformService::Response> response)
+{
+    // TODO: implement retrying + timeout
+    try {
+      response->transform = lookupTransform(request);
+    } 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();
+    }
+
+}
+
 rclcpp_action::GoalResponse BufferServer::goalCB(
   const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const LookupTransformAction::Goal> goal)
 {
@@ -216,11 +244,26 @@ 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,
@@ -232,4 +275,18 @@ geometry_msgs::msg::TransformStamped BufferServer::lookupTransform(GoalHandle gh
     goal->source_frame, tf2_ros::fromMsg(goal->source_time), goal->fixed_frame);
 }
 
+geometry_msgs::msg::TransformStamped BufferServer::lookupTransform(const std::shared_ptr<LookupTransformService::Request> request)
+{
+  // check whether we need to use the advanced or simple api
+  if (!request->advanced) {
+    return buffer_.lookupTransform(
+      request->target_frame, request->source_frame,
+      tf2_ros::fromMsg(request->source_time));
+  }
+
+  return buffer_.lookupTransform(
+    request->target_frame, tf2_ros::fromMsg(request->target_time),
+    request->source_frame, tf2_ros::fromMsg(request->source_time), request->fixed_frame);
+}
+
 }  // namespace tf2_ros