diff --git a/tf2_msgs/CMakeLists.txt b/tf2_msgs/CMakeLists.txt index 2661fc85d..c461125e7 100644 --- a/tf2_msgs/CMakeLists.txt +++ b/tf2_msgs/CMakeLists.txt @@ -19,7 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/TF2Error.msg" "msg/TFMessage.msg" "srv/FrameGraph.srv" - "action/LookupTransform.action" + "srv/LookupTransform.srv" DEPENDENCIES builtin_interfaces geometry_msgs ADD_LINTER_TESTS ) diff --git a/tf2_msgs/action/LookupTransform.action b/tf2_msgs/srv/LookupTransform.srv similarity index 78% rename from tf2_msgs/action/LookupTransform.action rename to tf2_msgs/srv/LookupTransform.srv index 27d35d4bb..6617069b5 100644 --- a/tf2_msgs/action/LookupTransform.action +++ b/tf2_msgs/srv/LookupTransform.srv @@ -1,17 +1,16 @@ -#Simple API +# Simple API string target_frame string source_frame builtin_interfaces/Time source_time builtin_interfaces/Duration timeout -#Advanced API +# Advanced API builtin_interfaces/Time target_time string fixed_frame -#Whether or not to use the advanced API +# Whether or not to use the advanced API bool advanced --- geometry_msgs/TransformStamped transform tf2_msgs/TF2Error error ---- diff --git a/tf2_ros/include/tf2_ros/buffer_client.h b/tf2_ros/include/tf2_ros/buffer_client.h index ba2818115..3a93efc48 100644 --- a/tf2_ros/include/tf2_ros/buffer_client.h +++ b/tf2_ros/include/tf2_ros/buffer_client.h @@ -46,8 +46,7 @@ #include "tf2/time.h" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_msgs/action/lookup_transform.hpp" +#include "tf2_msgs/srv/lookup_transform.hpp" namespace tf2_ros { @@ -104,7 +103,7 @@ class UnexpectedResultCodeException : public LookupTransformGoalException } }; -/** \brief Action client-based implementation of the tf2_ros::BufferInterface abstract data type. +/** \brief Service client-based implementation of the tf2_ros::BufferInterface abstract data type. * * BufferClient uses actions to coordinate waiting for available transforms. * @@ -113,7 +112,7 @@ class UnexpectedResultCodeException : public LookupTransformGoalException class BufferClient : public BufferInterface { public: - using LookupTransformAction = tf2_msgs::action::LookupTransform; + using LookupTransformService = tf2_msgs::srv::LookupTransform; /** \brief BufferClient constructor * \param node The node to add the buffer client to @@ -127,10 +126,16 @@ class BufferClient : public BufferInterface const std::string ns, const double & check_frequency = 10.0, const tf2::Duration & timeout_padding = tf2::durationFromSec(2.0)) - : check_frequency_(check_frequency), + : node_(node), + check_frequency_(check_frequency), timeout_padding_(timeout_padding) { - client_ = rclcpp_action::create_client(node, ns); + service_client_ = rclcpp::create_client( + node->get_node_base_interface(), + node->get_node_services_interface(), + ns, + rmw_qos_profile_services_default, + nullptr); } virtual ~BufferClient() = default; @@ -244,17 +249,18 @@ class BufferClient : public BufferInterface TF2_ROS_PUBLIC bool waitForServer(const tf2::Duration & timeout = tf2::durationFromSec(0)) { - return client_->wait_for_action_server(timeout); + return service_client_->wait_for_service(timeout); } private: geometry_msgs::msg::TransformStamped - processGoal(const LookupTransformAction::Goal & goal) const; + processRequest(const LookupTransformService::Request::SharedPtr & request) const; geometry_msgs::msg::TransformStamped - processResult(const LookupTransformAction::Result::SharedPtr & result) const; + processResponse(const LookupTransformService::Response::SharedPtr & response) const; - rclcpp_action::Client::SharedPtr client_; + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr service_client_; double check_frequency_; tf2::Duration timeout_padding_; }; diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 14466691f..0fe373782 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -49,83 +49,57 @@ #include "tf2_ros/visibility_control.h" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "rclcpp/create_timer.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" -#include "tf2_msgs/action/lookup_transform.hpp" +#include "rclcpp/qos.hpp" +#include "tf2_msgs/srv/lookup_transform.hpp" namespace tf2_ros { -/** \brief Action server for the action-based implementation of tf2::BufferCoreInterface. +/** \brief Service server for the service-based implementation of tf2::BufferCoreInterface. * * Use this class with a tf2_ros::TransformListener in the same process. * You can use this class with a tf2_ros::BufferClient in a different process. */ class BufferServer { - using LookupTransformAction = tf2_msgs::action::LookupTransform; - using GoalHandle = std::shared_ptr>; + using LookupTransformService = tf2_msgs::srv::LookupTransform; public: /** \brief Constructor * \param buffer The Buffer that this BufferServer will wrap. * \param node The node to add the buffer server to. - * \param ns The namespace in which to look for action clients. - * \param check_period How often to check for changes to known transforms (via a timer event). + * \param ns The namespace in which to look for service clients. */ template BufferServer( const tf2::BufferCoreInterface & buffer, NodePtr node, - const std::string & ns, - tf2::Duration check_period = tf2::durationFromSec(0.01)) + const std::string & ns) : buffer_(buffer), logger_(node->get_logger()) { - server_ = rclcpp_action::create_server( - node, + service_server_ = rclcpp::create_service( + node->get_node_base_interface(), + node->get_node_services_interface(), ns, - std::bind(&BufferServer::goalCB, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&BufferServer::cancelCB, this, std::placeholders::_1), - std::bind(&BufferServer::acceptedCB, this, std::placeholders::_1)); - - check_timer_ = rclcpp::create_timer( - node, node->get_clock(), check_period, std::bind(&BufferServer::checkTransforms, this)); + std::bind(&BufferServer::serviceCB, this, std::placeholders::_1, std::placeholders::_2), + rmw_qos_profile_services_default, + nullptr); RCLCPP_DEBUG(logger_, "Buffer server started"); } private: - struct GoalInfo - { - GoalHandle handle; - tf2::TimePoint end_time; - }; - - TF2_ROS_PUBLIC - rclcpp_action::GoalResponse goalCB( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - - TF2_ROS_PUBLIC - void acceptedCB(GoalHandle gh); - - TF2_ROS_PUBLIC - rclcpp_action::CancelResponse cancelCB(GoalHandle gh); - - TF2_ROS_PUBLIC - void checkTransforms(); - TF2_ROS_PUBLIC - bool canTransform(GoalHandle gh); + void serviceCB( + const std::shared_ptr request, + std::shared_ptr response); TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh); + bool canTransform(const std::shared_ptr request); const tf2::BufferCoreInterface & buffer_; rclcpp::Logger logger_; - rclcpp_action::Server::SharedPtr server_; - std::list active_goals_; - std::mutex mutex_; - rclcpp::TimerBase::SharedPtr check_timer_; + rclcpp::Service::SharedPtr service_server_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/buffer_client.cpp b/tf2_ros/src/buffer_client.cpp index 32702ab32..1a7c6a496 100644 --- a/tf2_ros/src/buffer_client.cpp +++ b/tf2_ros/src/buffer_client.cpp @@ -51,15 +51,15 @@ geometry_msgs::msg::TransformStamped BufferClient::lookupTransform( const tf2::TimePoint & time, const tf2::Duration timeout) const { - // populate the goal message - LookupTransformAction::Goal goal; - goal.target_frame = target_frame; - goal.source_frame = source_frame; - goal.source_time = tf2_ros::toMsg(time); - goal.timeout = tf2_ros::toMsg(timeout); - goal.advanced = false; - - return processGoal(goal); + // populate the request + LookupTransformService::Request::SharedPtr request; + request->target_frame = target_frame; + request->source_frame = source_frame; + request->source_time = tf2_ros::toMsg(time); + request->timeout = tf2_ros::toMsg(timeout); + request->advanced = false; + + return processRequest(request); } geometry_msgs::msg::TransformStamped BufferClient::lookupTransform( @@ -70,109 +70,71 @@ geometry_msgs::msg::TransformStamped BufferClient::lookupTransform( const std::string & fixed_frame, const tf2::Duration timeout) const { - // populate the goal message - LookupTransformAction::Goal goal; - goal.target_frame = target_frame; - goal.source_frame = source_frame; - goal.source_time = tf2_ros::toMsg(source_time); - goal.timeout = tf2_ros::toMsg(timeout); - goal.target_time = tf2_ros::toMsg(target_time); - goal.fixed_frame = fixed_frame; - goal.advanced = true; - - return processGoal(goal); + // populate the request + LookupTransformService::Request::SharedPtr request; + request->target_frame = target_frame; + request->source_frame = source_frame; + request->source_time = tf2_ros::toMsg(source_time); + request->timeout = tf2_ros::toMsg(timeout); + request->target_time = tf2_ros::toMsg(target_time); + request->fixed_frame = fixed_frame; + request->advanced = true; + + return processRequest(request); } -geometry_msgs::msg::TransformStamped BufferClient::processGoal( - const LookupTransformAction::Goal & goal) const +geometry_msgs::msg::TransformStamped BufferClient::processRequest( + const LookupTransformService::Request::SharedPtr & request) const { - if (!client_->wait_for_action_server(tf2_ros::fromMsg(goal.timeout))) { - throw tf2::ConnectivityException("Failed find available action server"); + if (!service_client_->wait_for_service(tf2_ros::fromMsg(request->timeout))) { + throw tf2::ConnectivityException("Failed find available service server"); } - auto goal_handle_future = client_->async_send_goal(goal); + auto response_future = service_client_->async_send_request(request); - const std::chrono::milliseconds period(static_cast((1.0 / check_frequency_) * 1000)); - bool ready = false; - bool timed_out = false; - tf2::TimePoint start_time = tf2::get_now(); - while (rclcpp::ok() && !ready && !timed_out) { - ready = (std::future_status::ready == goal_handle_future.wait_for(period)); - timed_out = tf2::get_now() > start_time + tf2_ros::fromMsg(goal.timeout) + timeout_padding_; - } - - if (timed_out) { + // Wait for the result + if (rclcpp::spin_until_future_complete( + node_->get_node_base_interface(), + response_future, tf2_ros::fromMsg(request->timeout)) == rclcpp::FutureReturnCode::SUCCESS) + { + return processResponse(response_future.get()); + } else { throw tf2::TimeoutException( - "Did not receive the goal response for the goal sent to " - "the action server. Something is likely wrong with the server."); - } - - auto goal_handle = goal_handle_future.get(); - if (!goal_handle) { - throw GoalRejectedException("Goal rejected by action server"); - } - - auto result_future = client_->async_get_result(goal_handle); - - ready = false; - while (rclcpp::ok() && !ready && !timed_out) { - ready = (std::future_status::ready == result_future.wait_for(period)); - timed_out = tf2::get_now() > start_time + tf2_ros::fromMsg(goal.timeout) + timeout_padding_; + "Did not receive the response for the request sent to " + "the service server. Something is likely wrong with the server."); } - - if (timed_out) { - throw tf2::TimeoutException( - "Did not receive the result for the goal sent to " - "the action server. Something is likely wrong with the server."); - } - - auto wrapped_result = result_future.get(); - - switch (wrapped_result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - break; - case rclcpp_action::ResultCode::ABORTED: - throw GoalAbortedException("LookupTransform action was aborted"); - case rclcpp_action::ResultCode::CANCELED: - throw GoalCanceledException("LookupTransform action was canceled"); - default: - throw UnexpectedResultCodeException("Unexpected result code returned from server"); - } - - // process the result for errors and return it - return processResult(wrapped_result.result); } -geometry_msgs::msg::TransformStamped BufferClient::processResult( - const LookupTransformAction::Result::SharedPtr & result) const +geometry_msgs::msg::TransformStamped BufferClient::processResponse( + const LookupTransformService::Response::SharedPtr & response) const { // if there's no error, then we'll just return the transform - if (result->error.error != result->error.NO_ERROR) { + if (response->error.error != response->error.NO_ERROR) { // otherwise, we'll have to throw the appropriate exception - if (result->error.error == result->error.LOOKUP_ERROR) { - throw tf2::LookupException(result->error.error_string); + if (response->error.error == response->error.LOOKUP_ERROR) { + throw tf2::LookupException(response->error.error_string); } - if (result->error.error == result->error.CONNECTIVITY_ERROR) { - throw tf2::ConnectivityException(result->error.error_string); + if (response->error.error == response->error.CONNECTIVITY_ERROR) { + throw tf2::ConnectivityException(response->error.error_string); } - if (result->error.error == result->error.EXTRAPOLATION_ERROR) { - throw tf2::ExtrapolationException(result->error.error_string); + if (response->error.error == response->error.EXTRAPOLATION_ERROR) { + throw tf2::ExtrapolationException(response->error.error_string); } - if (result->error.error == result->error.INVALID_ARGUMENT_ERROR) { - throw tf2::InvalidArgumentException(result->error.error_string); + if (response->error.error == response->error.INVALID_ARGUMENT_ERROR) { + throw tf2::InvalidArgumentException(response->error.error_string); } - if (result->error.error == result->error.TIMEOUT_ERROR) { - throw tf2::TimeoutException(result->error.error_string); + if (response->error.error == response->error.TIMEOUT_ERROR) { + throw tf2::TimeoutException(response->error.error_string); } - throw tf2::TransformException(result->error.error_string); + throw tf2::TransformException(response->error.error_string); } - return result->transform; + return response->transform; } bool BufferClient::canTransform( diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index 37f8a9be9..cf8f102af 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -47,188 +47,57 @@ namespace tf2_ros { -void BufferServer::checkTransforms() -{ - std::unique_lock lock(mutex_); - for (std::list::iterator it = active_goals_.begin(); it != active_goals_.end(); ) { - GoalInfo & info = *it; - - // we want to lookup a transform if the time on the goal - // has expired, or a transform is available - if (canTransform(info.handle)) { - auto result = std::make_shared(); - - // try to populate the result, catching exceptions if they occur - try { - result->transform = lookupTransform(info.handle); - - RCLCPP_DEBUG( - logger_, - "Can transform for goal %s", - rclcpp_action::to_string(info.handle->get_goal_id()).c_str()); - - info.handle->succeed(result); - } catch (const tf2::ConnectivityException & ex) { - result->error.error = result->error.CONNECTIVITY_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } catch (const tf2::LookupException & ex) { - result->error.error = result->error.LOOKUP_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } catch (const tf2::ExtrapolationException & ex) { - result->error.error = result->error.EXTRAPOLATION_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } catch (const tf2::InvalidArgumentException & ex) { - result->error.error = result->error.INVALID_ARGUMENT_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } catch (const tf2::TimeoutException & ex) { - result->error.error = result->error.TIMEOUT_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } catch (const tf2::TransformException & ex) { - result->error.error = result->error.TRANSFORM_ERROR; - result->error.error_string = ex.what(); - info.handle->abort(result); - } - - } else if (info.end_time < tf2::get_now()) { - // Timeout - auto result = std::make_shared(); - info.handle->abort(result); - } else { - ++it; - } - - // Remove goal if it has terminated - if (!info.handle->is_active()) { - it = active_goals_.erase(it); - } - } -} - -rclcpp_action::CancelResponse BufferServer::cancelCB(GoalHandle gh) -{ - RCLCPP_DEBUG( - logger_, - "Cancel request for goal %s", - rclcpp_action::to_string(gh->get_goal_id()).c_str()); - - std::unique_lock lock(mutex_); - // we need to find the goal in the list and remove it... also setting it as canceled - // if its not in the list, we won't do anything since it will have already been set - // as completed - auto goal_to_cancel_it = std::find_if( - active_goals_.begin(), active_goals_.end(), [&gh](const auto & info) { - return info.handle == gh; - }); - if (goal_to_cancel_it != active_goals_.end()) { - RCLCPP_DEBUG( - logger_, - "Accept cancel request for goal %s", - rclcpp_action::to_string(gh->get_goal_id()).c_str()); - goal_to_cancel_it->handle->canceled(std::make_shared()); - active_goals_.erase(goal_to_cancel_it); - return rclcpp_action::CancelResponse::ACCEPT; - } - - RCLCPP_DEBUG( - logger_, - "Reject cancel request for goal %s", - rclcpp_action::to_string(gh->get_goal_id()).c_str()); - return rclcpp_action::CancelResponse::REJECT; -} - -rclcpp_action::GoalResponse BufferServer::goalCB( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) -{ - (void)uuid; - (void)goal; - // accept all goals we get - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -void BufferServer::acceptedCB(GoalHandle gh) +void BufferServer::serviceCB( + const std::shared_ptr request, + std::shared_ptr response) { - RCLCPP_DEBUG( - logger_, - "New goal accepted with ID %s", - rclcpp_action::to_string(gh->get_goal_id()).c_str()); - // if the transform isn't immediately available, we'll push it onto our list to check - // along with the time that the goal will end - GoalInfo goal_info; - goal_info.handle = gh; - goal_info.end_time = tf2::get_now() + tf2_ros::fromMsg(gh->get_goal()->timeout); - - // we can do a quick check here to see if the transform is valid - // we'll also do this if the end time has been reached - if (canTransform(gh) || goal_info.end_time <= tf2::get_now()) { - auto result = std::make_shared(); - try { - result->transform = lookupTransform(gh); - } catch (const tf2::ConnectivityException & ex) { - result->error.error = result->error.CONNECTIVITY_ERROR; - result->error.error_string = ex.what(); - } catch (const tf2::LookupException & ex) { - result->error.error = result->error.LOOKUP_ERROR; - result->error.error_string = ex.what(); - } catch (const tf2::ExtrapolationException & ex) { - result->error.error = result->error.EXTRAPOLATION_ERROR; - result->error.error_string = ex.what(); - } catch (const tf2::InvalidArgumentException & ex) { - result->error.error = result->error.INVALID_ARGUMENT_ERROR; - result->error.error_string = ex.what(); - } catch (const tf2::TimeoutException & ex) { - result->error.error = result->error.TIMEOUT_ERROR; - result->error.error_string = ex.what(); - } catch (const tf2::TransformException & ex) { - result->error.error = result->error.TRANSFORM_ERROR; - result->error.error_string = ex.what(); + 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)); } - - RCLCPP_DEBUG(logger_, "Transform available immediately for new goal"); - gh->succeed(result); - return; + } 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(); } - - std::unique_lock lock(mutex_); - active_goals_.push_back(goal_info); } -bool BufferServer::canTransform(GoalHandle gh) +bool BufferServer::canTransform(const std::shared_ptr request) { - const auto goal = gh->get_goal(); - - tf2::TimePoint source_time_point = tf2_ros::fromMsg(goal->source_time); + tf2::TimePoint source_time_point = tf2_ros::fromMsg(request->source_time); // check whether we need to used the advanced or simple api - if (!goal->advanced) { - return buffer_.canTransform(goal->target_frame, goal->source_frame, source_time_point, nullptr); + if (!request->advanced) { + return buffer_.canTransform( + request->target_frame, request->source_frame, source_time_point, nullptr); } - tf2::TimePoint target_time_point = tf2_ros::fromMsg(goal->target_time); + tf2::TimePoint target_time_point = tf2_ros::fromMsg(request->target_time); return buffer_.canTransform( - goal->target_frame, target_time_point, - goal->source_frame, source_time_point, goal->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 - if (!goal->advanced) { - return buffer_.lookupTransform( - goal->target_frame, goal->source_frame, - tf2_ros::fromMsg(goal->source_time)); - } - - return buffer_.lookupTransform( - goal->target_frame, tf2_ros::fromMsg(goal->target_time), - goal->source_frame, tf2_ros::fromMsg(goal->source_time), goal->fixed_frame); + request->target_frame, target_time_point, + request->source_frame, source_time_point, request->fixed_frame, nullptr); } } // namespace tf2_ros diff --git a/tf2_ros_py/tf2_ros/buffer_client.py b/tf2_ros_py/tf2_ros/buffer_client.py index a8d38022f..8f6f2b1b9 100644 --- a/tf2_ros_py/tf2_ros/buffer_client.py +++ b/tf2_ros_py/tf2_ros/buffer_client.py @@ -39,33 +39,32 @@ from geometry_msgs.msg import TransformStamped from rclpy.node import Node -from rclpy.action.client import ActionClient from rclpy.duration import Duration from rclpy.time import Time from rclpy.clock import Clock from time import sleep - import builtin_interfaces.msg import tf2_py as tf2 import tf2_ros import threading import warnings +from rclpy.callback_groups import ReentrantCallbackGroup -from tf2_msgs.action import LookupTransform +from tf2_msgs.srv import LookupTransform # Used for documentation purposes only -LookupTransformGoal = TypeVar('LookupTransformGoal') -LookupTransformResult = TypeVar('LookupTransformResult') +LookupTransformRequest = TypeVar('LookupTransformRequest') +LookupTransformResponse = TypeVar('LookupTransformResponse') class BufferClient(tf2_ros.BufferInterface): """ - Action client-based implementation of BufferInterface. + Service client-based implementation of BufferInterface. """ def __init__( self, node: Node, - ns: str, + ns: str = "tf2_buffer_server", check_frequency: float = 10.0, timeout_padding: Duration = Duration(seconds=2.0) ) -> None: @@ -79,7 +78,7 @@ def __init__( """ tf2_ros.BufferInterface.__init__(self) self.node = node - self.action_client = ActionClient(node, LookupTransform, action_name=ns) + self.service_client = node.create_client(LookupTransform, ns, callback_group=ReentrantCallbackGroup()) self.check_frequency = check_frequency self.timeout_padding = timeout_padding @@ -110,14 +109,14 @@ def lookup_transform( else: raise TypeError('Must pass a rclpy.time.Time object.') - goal = LookupTransform.Goal() - goal.target_frame = target_frame - goal.source_frame = source_frame - goal.source_time = source_time.to_msg() - goal.timeout = timeout.to_msg() - goal.advanced = False + request = LookupTransform.Request() + request.target_frame = target_frame + request.source_frame = source_frame + request.source_time = source_time.to_msg() + request.timeout = timeout.to_msg() + request.advanced = False - return self.__process_goal(goal) + return self.__process_request(request) # lookup, advanced api def lookup_transform_full( @@ -140,16 +139,16 @@ def lookup_transform_full( :param timeout: Time to wait for the target frame to become available. :return: The transform between the frames. """ - goal = LookupTransform.Goal() - goal.target_frame = target_frame - goal.source_frame = source_frame - goal.source_time = source_time.to_msg() - goal.timeout = timeout.to_msg() - goal.target_time = target_time.to_msg() - goal.fixed_frame = fixed_frame - goal.advanced = True + request = LookupTransform.Request() + request.target_frame = target_frame + request.source_frame = source_frame + request.source_time = source_time.to_msg() + request.timeout = timeout.to_msg() + request.target_time = target_time.to_msg() + request.fixed_frame = fixed_frame + request.advanced = True - return self.__process_goal(goal) + return self.__process_request(request) # can, simple api def can_transform( @@ -205,27 +204,25 @@ def can_transform_full( except tf2.TransformException: return False - def __process_goal(self, goal: LookupTransformGoal) -> TransformStamped: - # TODO(sloretz) why is this an action client? Service seems more appropriate. - if not self.action_client.server_is_ready(): + def __process_request(self, request: LookupTransformRequest) -> TransformStamped: + if not self.service_client.wait_for_service(timeout_sec=1.0): raise tf2.TimeoutException("The BufferServer is not ready.") - event = threading.Event() def unblock(future): nonlocal event event.set() - send_goal_future = self.action_client.send_goal_async(goal) - send_goal_future.add_done_callback(unblock) + future = self.service_client.call_async(request) + future.add_done_callback(unblock) def unblock_by_timeout(): - nonlocal send_goal_future, goal, event + nonlocal future, request, event clock = Clock() start_time = clock.now() - timeout = Duration.from_msg(goal.timeout) + timeout = Duration.from_msg(request.timeout) timeout_padding = self.timeout_padding - while not send_goal_future.done() and not event.is_set(): + while not future.done() and not event.is_set(): if clock.now() > start_time + timeout + timeout_padding: break # TODO(Anyone): We can't use Rate here because it would never expire @@ -242,39 +239,33 @@ def unblock_by_timeout(): event.wait() # This shouldn't happen, but could in rare cases where the server hangs - if not send_goal_future.done(): - raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.") - - # Raises if future was given an exception - goal_handle = send_goal_future.result() - - if not goal_handle.accepted: - raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with accepted status. Something is likely wrong with the server.") - - response = self.action_client._get_result(goal_handle) - - return self.__process_result(response.result) - - def __process_result(self, result: LookupTransformResult) -> TransformStamped: - if result == None or result.error == None: - raise tf2.TransformException("The BufferServer returned None for result or result.error! Something is likely wrong with the server.") - if result.error.error != result.error.NO_ERROR: - if result.error.error == result.error.LOOKUP_ERROR: - raise tf2.LookupException(result.error.error_string) - if result.error.error == result.error.CONNECTIVITY_ERROR: - raise tf2.ConnectivityException(result.error.error_string) - if result.error.error == result.error.EXTRAPOLATION_ERROR: - raise tf2.ExtrapolationException(result.error.error_string) - if result.error.error == result.error.INVALID_ARGUMENT_ERROR: - raise tf2.InvalidArgumentException(result.error.error_string) - if result.error.error == result.error.TIMEOUT_ERROR: - raise tf2.TimeoutException(result.error.error_string) - - raise tf2.TransformException(result.error.error_string) - - return result.transform + if not future.done(): + raise tf2.TimeoutException("The LookupTransform request sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.") + + response = future.result() + + return self.__process_response(response) + + def __process_response(self, response: LookupTransform.Response) -> TransformStamped: + if response == None or response.error == None: + raise tf2.TransformException("The BufferServer returned None for response or response.error! Something is likely wrong with the server.") + if response.error.error != response.error.NO_ERROR: + if response.error.error == response.error.LOOKUP_ERROR: + raise tf2.LookupException(response.error.error_string) + if response.error.error == response.error.CONNECTIVITY_ERROR: + raise tf2.ConnectivityException(response.error.error_string) + if response.error.error == response.error.EXTRAPOLATION_ERROR: + raise tf2.ExtrapolationException(response.error.error_string) + if response.error.error == response.error.INVALID_ARGUMENT_ERROR: + raise tf2.InvalidArgumentException(response.error.error_string) + if response.error.error == response.error.TIMEOUT_ERROR: + raise tf2.TimeoutException(response.error.error_string) + + raise tf2.TransformException(response.error.error_string) + + return response.transform def destroy(self) -> None: """Cleanup resources associated with this BufferClient.""" - self.action_client.destroy() + self.service_client.destroy()