From 2f8715d1038121ba60fb09497dcec596466c42a7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 9 Dec 2022 11:17:14 +0100 Subject: [PATCH 1/3] set result_timeout to 0 --- tf2_ros/include/tf2_ros/buffer_server.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 14466691f3..d2a7cfb1d9 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -82,12 +82,16 @@ class BufferServer : buffer_(buffer), logger_(node->get_logger()) { + rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); + action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(0); server_ = rclcpp_action::create_server( node, 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)); + std::bind(&BufferServer::acceptedCB, this, std::placeholders::_1), + action_server_ops + ); check_timer_ = rclcpp::create_timer( node, node->get_clock(), check_period, std::bind(&BufferServer::checkTransforms, this)); From edebd79c1474bcd865464c211d330e00a542e098 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 9 Dec 2022 17:19:53 +0100 Subject: [PATCH 2/3] Add TF buffer service --- tf2_msgs/CMakeLists.txt | 1 + tf2_msgs/srv/LookupTransform.srv | 17 +++++++ tf2_ros/include/tf2_ros/buffer_server.h | 24 +++++++++- tf2_ros/src/buffer_server.cpp | 59 ++++++++++++++++++++++++- 4 files changed, 99 insertions(+), 2 deletions(-) create mode 100644 tf2_msgs/srv/LookupTransform.srv diff --git a/tf2_msgs/CMakeLists.txt b/tf2_msgs/CMakeLists.txt index f8f424c039..b032c725d5 100644 --- a/tf2_msgs/CMakeLists.txt +++ b/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 diff --git a/tf2_msgs/srv/LookupTransform.srv b/tf2_msgs/srv/LookupTransform.srv new file mode 100644 index 0000000000..a5ac2f0662 --- /dev/null +++ b/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 + diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index d2a7cfb1d9..873e08e90f 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/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>; public: @@ -83,7 +86,7 @@ class BufferServer logger_(node->get_logger()) { rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); - action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(0); + action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(5); server_ = rclcpp_action::create_server( node, ns, @@ -93,6 +96,14 @@ class BufferServer action_server_ops ); + service_server_ = + rclcpp::create_service(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"); @@ -104,6 +115,10 @@ class BufferServer GoalHandle handle; tf2::TimePoint end_time; }; + + TF2_ROS_PUBLIC + void serviceCB(const std::shared_ptr request, + std::shared_ptr response); TF2_ROS_PUBLIC rclcpp_action::GoalResponse goalCB( @@ -121,12 +136,19 @@ class BufferServer TF2_ROS_PUBLIC bool canTransform(GoalHandle gh); + TF2_ROS_PUBLIC + bool canTransform(const std::shared_ptr request); + TF2_ROS_PUBLIC geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh); + TF2_ROS_PUBLIC + geometry_msgs::msg::TransformStamped lookupTransform(const std::shared_ptr request); + const tf2::BufferCoreInterface & buffer_; rclcpp::Logger logger_; rclcpp_action::Server::SharedPtr server_; + rclcpp::Service::SharedPtr service_server_; std::list active_goals_; std::mutex mutex_; rclcpp::TimerBase::SharedPtr check_timer_; diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index a91de83a71..5d8bf9d0ed 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/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 request, + std::shared_ptr 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 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 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 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 From 6530c38d5d52fb145940a04e65f7503c03391961 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 12 Dec 2022 10:33:04 +0100 Subject: [PATCH 3/3] Revert "Add TF buffer service" This reverts commit edebd79c1474bcd865464c211d330e00a542e098. --- tf2_msgs/CMakeLists.txt | 1 - tf2_msgs/srv/LookupTransform.srv | 17 ------- tf2_ros/include/tf2_ros/buffer_server.h | 24 +--------- tf2_ros/src/buffer_server.cpp | 59 +------------------------ 4 files changed, 2 insertions(+), 99 deletions(-) delete mode 100644 tf2_msgs/srv/LookupTransform.srv diff --git a/tf2_msgs/CMakeLists.txt b/tf2_msgs/CMakeLists.txt index b032c725d5..f8f424c039 100644 --- a/tf2_msgs/CMakeLists.txt +++ b/tf2_msgs/CMakeLists.txt @@ -18,7 +18,6 @@ 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 diff --git a/tf2_msgs/srv/LookupTransform.srv b/tf2_msgs/srv/LookupTransform.srv deleted file mode 100644 index a5ac2f0662..0000000000 --- a/tf2_msgs/srv/LookupTransform.srv +++ /dev/null @@ -1,17 +0,0 @@ -#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 - diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 873e08e90f..d2a7cfb1d9 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -51,10 +51,8 @@ #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 { @@ -66,7 +64,6 @@ namespace tf2_ros class BufferServer { using LookupTransformAction = tf2_msgs::action::LookupTransform; - using LookupTransformService = tf2_msgs::srv::LookupTransform; using GoalHandle = std::shared_ptr>; public: @@ -86,7 +83,7 @@ class BufferServer logger_(node->get_logger()) { rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); - action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(5); + action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(0); server_ = rclcpp_action::create_server( node, ns, @@ -96,14 +93,6 @@ class BufferServer action_server_ops ); - service_server_ = - rclcpp::create_service(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"); @@ -115,10 +104,6 @@ class BufferServer GoalHandle handle; tf2::TimePoint end_time; }; - - TF2_ROS_PUBLIC - void serviceCB(const std::shared_ptr request, - std::shared_ptr response); TF2_ROS_PUBLIC rclcpp_action::GoalResponse goalCB( @@ -136,19 +121,12 @@ class BufferServer TF2_ROS_PUBLIC bool canTransform(GoalHandle gh); - TF2_ROS_PUBLIC - bool canTransform(const std::shared_ptr request); - TF2_ROS_PUBLIC geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh); - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped lookupTransform(const std::shared_ptr request); - const tf2::BufferCoreInterface & buffer_; rclcpp::Logger logger_; rclcpp_action::Server::SharedPtr server_; - rclcpp::Service::SharedPtr service_server_; std::list active_goals_; std::mutex mutex_; rclcpp::TimerBase::SharedPtr check_timer_; diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index 5d8bf9d0ed..a91de83a71 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -143,34 +143,6 @@ rclcpp_action::CancelResponse BufferServer::cancelCB(GoalHandle gh) return rclcpp_action::CancelResponse::REJECT; } -void BufferServer::serviceCB(const std::shared_ptr request, - std::shared_ptr 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 goal) { @@ -244,26 +216,11 @@ bool BufferServer::canTransform(GoalHandle gh) goal->source_frame, source_time_point, goal->fixed_frame, nullptr); } -bool BufferServer::canTransform(const std::shared_ptr 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 use the advanced or simple api + // check whether we need to used the advanced or simple api if (!goal->advanced) { return buffer_.lookupTransform( goal->target_frame, goal->source_frame, @@ -275,18 +232,4 @@ 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 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