Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Buffer server service #574

Open
wants to merge 25 commits into
base: rolling
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion tf2_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Expand Down
Original file line number Diff line number Diff line change
@@ -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
---
26 changes: 16 additions & 10 deletions tf2_ros/include/tf2_ros/buffer_client.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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.
*
Expand All @@ -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
Expand All @@ -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<LookupTransformAction>(node, ns);
service_client_ = rclcpp::create_client<LookupTransformService>(
node->get_node_base_interface(),
node->get_node_services_interface(),
ns,
rmw_qos_profile_services_default,
nullptr);
}

virtual ~BufferClient() = default;
Expand Down Expand Up @@ -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<LookupTransformAction>::SharedPtr client_;
rclcpp::Node::SharedPtr node_;
rclcpp::Client<LookupTransformService>::SharedPtr service_client_;
double check_frequency_;
tf2::Duration timeout_padding_;
};
Expand Down
60 changes: 17 additions & 43 deletions tf2_ros/include/tf2_ros/buffer_server.h
Original file line number Diff line number Diff line change
Expand Up @@ -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<rclcpp_action::ServerGoalHandle<LookupTransformAction>>;
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<typename NodePtr>
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<LookupTransformAction>(
node,
service_server_ = rclcpp::create_service<LookupTransformService>(
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<const LookupTransformAction::Goal> 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<LookupTransformService::Request> request,
std::shared_ptr<LookupTransformService::Response> response);

TF2_ROS_PUBLIC
geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh);
bool canTransform(const std::shared_ptr<LookupTransformService::Request> request);

const tf2::BufferCoreInterface & buffer_;
rclcpp::Logger logger_;
rclcpp_action::Server<LookupTransformAction>::SharedPtr server_;
std::list<GoalInfo> active_goals_;
std::mutex mutex_;
rclcpp::TimerBase::SharedPtr check_timer_;
rclcpp::Service<LookupTransformService>::SharedPtr service_server_;
};

} // namespace tf2_ros
Expand Down
136 changes: 49 additions & 87 deletions tf2_ros/src/buffer_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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<int>((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(
Expand Down
Loading