From 2f8715d1038121ba60fb09497dcec596466c42a7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 9 Dec 2022 11:17:14 +0100 Subject: [PATCH 01/24] 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 14466691f..d2a7cfb1d 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 02/24] 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 f8f424c03..b032c725d 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 000000000..a5ac2f066 --- /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 d2a7cfb1d..873e08e90 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 a91de83a7..5d8bf9d0e 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 03/24] 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 b032c725d..f8f424c03 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 a5ac2f066..000000000 --- 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 873e08e90..d2a7cfb1d 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 5d8bf9d0e..a91de83a7 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 From ae332d2f22dcb7f677a93f5c07161ce733c31713 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 9 Dec 2022 17:19:53 +0100 Subject: [PATCH 04/24] 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 f8f424c03..b032c725d 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 000000000..a5ac2f066 --- /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 d2a7cfb1d..873e08e90 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 a91de83a7..5d8bf9d0e 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 dd2e8cbb11370d6bf9e08370b0cce700bca2142f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 12 Dec 2022 10:40:14 +0100 Subject: [PATCH 05/24] revert timeout --- tf2_ros/include/tf2_ros/buffer_server.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 873e08e90..c5945b558 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -86,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(5); + action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(0); server_ = rclcpp_action::create_server( node, ns, From 590647ba2893d409f489acaaf261b2889d340c96 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 12 Dec 2022 10:40:23 +0100 Subject: [PATCH 06/24] Revert "set result_timeout to 0" This reverts commit 2f8715d1038121ba60fb09497dcec596466c42a7. --- tf2_ros/include/tf2_ros/buffer_server.h | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index c5945b558..4ddd70ddb 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -85,16 +85,12 @@ 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), - action_server_ops - ); + std::bind(&BufferServer::acceptedCB, this, std::placeholders::_1)); service_server_ = rclcpp::create_service(node->get_node_base_interface(), node->get_node_services_interface(), ns, std::bind( From 8a8dec25a4c29d8c317ca4f93840f42394039e95 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:47:37 +0100 Subject: [PATCH 07/24] Update tf2_msgs/srv/LookupTransform.srv Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_msgs/srv/LookupTransform.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_msgs/srv/LookupTransform.srv b/tf2_msgs/srv/LookupTransform.srv index a5ac2f066..368614545 100644 --- a/tf2_msgs/srv/LookupTransform.srv +++ b/tf2_msgs/srv/LookupTransform.srv @@ -4,7 +4,7 @@ string source_frame builtin_interfaces/Time source_time builtin_interfaces/Duration timeout -#Advanced API +# Advanced API builtin_interfaces/Time target_time string fixed_frame From 593692c747445dee2fe62276350efc87dbdc3b2f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:47:44 +0100 Subject: [PATCH 08/24] Update tf2_msgs/srv/LookupTransform.srv Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_msgs/srv/LookupTransform.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_msgs/srv/LookupTransform.srv b/tf2_msgs/srv/LookupTransform.srv index 368614545..35ca6be5c 100644 --- a/tf2_msgs/srv/LookupTransform.srv +++ b/tf2_msgs/srv/LookupTransform.srv @@ -8,7 +8,7 @@ builtin_interfaces/Duration timeout 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 --- From 6d0f6795151ee123b57f0d8c8010fe6191ac165a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:47:51 +0100 Subject: [PATCH 09/24] Update tf2_msgs/srv/LookupTransform.srv Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_msgs/srv/LookupTransform.srv | 1 - 1 file changed, 1 deletion(-) diff --git a/tf2_msgs/srv/LookupTransform.srv b/tf2_msgs/srv/LookupTransform.srv index 35ca6be5c..ba83d36a2 100644 --- a/tf2_msgs/srv/LookupTransform.srv +++ b/tf2_msgs/srv/LookupTransform.srv @@ -14,4 +14,3 @@ bool advanced --- geometry_msgs/TransformStamped transform tf2_msgs/TF2Error error - From 68dfed0f20041fac75f2cda7a2b83f33005126bb Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:48:03 +0100 Subject: [PATCH 10/24] Update tf2_ros/include/tf2_ros/buffer_server.h Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_ros/include/tf2_ros/buffer_server.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 4ddd70ddb..899eea27f 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -92,13 +92,13 @@ class BufferServer std::bind(&BufferServer::cancelCB, this, std::placeholders::_1), std::bind(&BufferServer::acceptedCB, this, std::placeholders::_1)); - 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 - ); + 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)); From d22398c717d5fbb39007ccf072e50c1c2ebc48ec Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:48:15 +0100 Subject: [PATCH 11/24] Update tf2_ros/src/buffer_server.cpp Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_ros/src/buffer_server.cpp | 50 +++++++++++++++++------------------ 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index 5d8bf9d0e..be582fa89 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -143,32 +143,32 @@ rclcpp_action::CancelResponse BufferServer::cancelCB(GoalHandle gh) return rclcpp_action::CancelResponse::REJECT; } -void BufferServer::serviceCB(const std::shared_ptr request, - std::shared_ptr response) +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(); - } - + // 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( From 371691efe7be299b942db318350b426eccbb2610 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:48:25 +0100 Subject: [PATCH 12/24] Update tf2_ros/src/buffer_server.cpp Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_ros/src/buffer_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index be582fa89..ad15e7612 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -275,7 +275,8 @@ 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) +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) { From bf1c7ea2e8e2fe255b627229c53d4ed9348882dd Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:48:30 +0100 Subject: [PATCH 13/24] Update tf2_ros/src/buffer_server.cpp Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_ros/src/buffer_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index ad15e7612..a5debb73d 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -250,7 +250,8 @@ bool BufferServer::canTransform(const std::shared_ptradvanced) { - return buffer_.canTransform(request->target_frame, request->source_frame, source_time_point, nullptr); + return buffer_.canTransform( + request->target_frame, request->source_frame, source_time_point, nullptr); } tf2::TimePoint target_time_point = tf2_ros::fromMsg(request->target_time); From b92cc63a1c6b1196cc65669fc8fbe1dd2cc7d7ff Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:48:36 +0100 Subject: [PATCH 14/24] Update tf2_msgs/srv/LookupTransform.srv Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_msgs/srv/LookupTransform.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_msgs/srv/LookupTransform.srv b/tf2_msgs/srv/LookupTransform.srv index ba83d36a2..6617069b5 100644 --- a/tf2_msgs/srv/LookupTransform.srv +++ b/tf2_msgs/srv/LookupTransform.srv @@ -1,4 +1,4 @@ -#Simple API +# Simple API string target_frame string source_frame builtin_interfaces/Time source_time From f54674e3b7df483583971310525a80b4d63951eb Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:48:46 +0100 Subject: [PATCH 15/24] Update tf2_ros/include/tf2_ros/buffer_server.h Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_ros/include/tf2_ros/buffer_server.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 899eea27f..1f2ad2990 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -113,8 +113,9 @@ class BufferServer }; TF2_ROS_PUBLIC - void serviceCB(const std::shared_ptr request, - std::shared_ptr response); + void serviceCB( + const std::shared_ptr request, + std::shared_ptr response); TF2_ROS_PUBLIC rclcpp_action::GoalResponse goalCB( From a8d461377d3b4f53081d4b8d286c89b7218e587d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 13:48:55 +0100 Subject: [PATCH 16/24] Update tf2_ros/include/tf2_ros/buffer_server.h Co-authored-by: Chris Lalancette Signed-off-by: Tony Najjar --- tf2_ros/include/tf2_ros/buffer_server.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 1f2ad2990..4482196c1 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -140,7 +140,8 @@ class BufferServer geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh); TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped lookupTransform(const std::shared_ptr request); + geometry_msgs::msg::TransformStamped lookupTransform( + const std::shared_ptr request); const tf2::BufferCoreInterface & buffer_; rclcpp::Logger logger_; From e19de6d333f3808d5ca149e71815e3f82c9ffdf1 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 15:18:25 +0100 Subject: [PATCH 17/24] simplification --- tf2_ros/include/tf2_ros/buffer_server.h | 4 -- tf2_ros/src/buffer_server.cpp | 83 ++++++++++++------------- 2 files changed, 40 insertions(+), 47 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 4482196c1..5b7c8d00a 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -139,10 +139,6 @@ class BufferServer 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_; diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index bb06bb268..0e46cd818 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -47,6 +47,46 @@ namespace tf2_ros { + +void BufferServer::serviceCB( + const std::shared_ptr request, + std::shared_ptr 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 lock(mutex_); @@ -142,34 +182,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) { @@ -275,19 +287,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 From 271626c830b13118758441a4b5b900fa38463c5a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 9 Nov 2023 15:53:25 +0100 Subject: [PATCH 18/24] fix indentation --- tf2_ros/src/buffer_server.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index 0e46cd818..79aaaf57d 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -56,9 +56,9 @@ void BufferServer::serviceCB( 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); + 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( @@ -185,6 +185,8 @@ rclcpp_action::CancelResponse BufferServer::cancelCB(GoalHandle gh) rclcpp_action::GoalResponse BufferServer::goalCB( const rclcpp_action::GoalUUID & uuid, std::shared_ptr 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 From a7802a78b97c0f8e4afd5c5e92b137f2256bbe4b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 13 Nov 2023 12:39:17 +0100 Subject: [PATCH 19/24] add python buffer client changes --- tf2_ros_py/tf2_ros/buffer_client.py | 45 ++++++++++++----------------- 1 file changed, 18 insertions(+), 27 deletions(-) diff --git a/tf2_ros_py/tf2_ros/buffer_client.py b/tf2_ros_py/tf2_ros/buffer_client.py index a8d38022f..ff467e847 100644 --- a/tf2_ros_py/tf2_ros/buffer_client.py +++ b/tf2_ros_py/tf2_ros/buffer_client.py @@ -39,19 +39,18 @@ 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') @@ -60,12 +59,12 @@ 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,7 +109,7 @@ def lookup_transform( else: raise TypeError('Must pass a rclpy.time.Time object.') - goal = LookupTransform.Goal() + goal = LookupTransform.Request() goal.target_frame = target_frame goal.source_frame = source_frame goal.source_time = source_time.to_msg() @@ -140,7 +139,7 @@ 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 = LookupTransform.Request() goal.target_frame = target_frame goal.source_frame = source_frame goal.source_time = source_time.to_msg() @@ -206,26 +205,24 @@ def can_transform_full( 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(): + 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(goal) + future.add_done_callback(unblock) def unblock_by_timeout(): - nonlocal send_goal_future, goal, event + nonlocal future, goal, event clock = Clock() start_time = clock.now() timeout = Duration.from_msg(goal.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,20 +239,14 @@ 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(): + if not 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) + response = future.result() + + return self.__process_result(response) - def __process_result(self, result: LookupTransformResult) -> TransformStamped: + def __process_result(self, result: LookupTransform.Response) -> 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: @@ -277,4 +268,4 @@ def __process_result(self, result: LookupTransformResult) -> TransformStamped: def destroy(self) -> None: """Cleanup resources associated with this BufferClient.""" - self.action_client.destroy() + self.service_client.destroy() From 43085e7e39756b9f99caf68e1f6639ace66dcdd9 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 13 Nov 2023 13:48:20 +0100 Subject: [PATCH 20/24] remove action interface --- tf2_ros/include/tf2_ros/buffer_server.h | 52 +------ tf2_ros/src/buffer_server.cpp | 188 +----------------------- 2 files changed, 4 insertions(+), 236 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 5b7c8d00a..76ed620f6 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -49,49 +49,35 @@ #include "tf2_ros/visibility_control.h" #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 { -/** \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 LookupTransformService = tf2_msgs::srv::LookupTransform; - using GoalHandle = std::shared_ptr>; 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, - 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)); - service_server_ = rclcpp::create_service( node->get_node_base_interface(), node->get_node_services_interface(), @@ -99,53 +85,21 @@ class BufferServer 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"); } private: - struct GoalInfo - { - 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( - 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); - TF2_ROS_PUBLIC bool canTransform(const std::shared_ptr request); - TF2_ROS_PUBLIC - geometry_msgs::msg::TransformStamped lookupTransform(GoalHandle gh); - 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_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index 79aaaf57d..2d94928f3 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -52,7 +52,7 @@ void BufferServer::serviceCB( const std::shared_ptr request, std::shared_ptr response) { - // TODO: implement retrying + timeout + // TODO: implement timeout try { // check whether we need to use the advanced or simple api if (request->advanced) { @@ -87,176 +87,6 @@ void BufferServer::serviceCB( } } -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) -{ - 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 - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; -} - -void BufferServer::acceptedCB(GoalHandle gh) -{ - 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(); - } - - RCLCPP_DEBUG(logger_, "Transform available immediately for new goal"); - gh->succeed(result); - return; - } - - std::unique_lock lock(mutex_); - active_goals_.push_back(goal_info); -} - -bool BufferServer::canTransform(GoalHandle gh) -{ - const auto goal = gh->get_goal(); - - tf2::TimePoint source_time_point = tf2_ros::fromMsg(goal->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); - } - - tf2::TimePoint target_time_point = tf2_ros::fromMsg(goal->target_time); - return buffer_.canTransform( - goal->target_frame, target_time_point, - 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); @@ -273,20 +103,4 @@ bool BufferServer::canTransform(const std::shared_ptrsource_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 - 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); -} - } // namespace tf2_ros From e05f43f558603a83e2441270195a4fae5cea6879 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 13 Nov 2023 13:56:37 +0100 Subject: [PATCH 21/24] Fix formatting in buffer server code --- tf2_ros/include/tf2_ros/buffer_server.h | 2 +- tf2_ros/src/buffer_server.cpp | 11 ++++------- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer_server.h b/tf2_ros/include/tf2_ros/buffer_server.h index 76ed620f6..0fe373782 100644 --- a/tf2_ros/include/tf2_ros/buffer_server.h +++ b/tf2_ros/include/tf2_ros/buffer_server.h @@ -78,7 +78,7 @@ class BufferServer : buffer_(buffer), logger_(node->get_logger()) { - service_server_ = rclcpp::create_service( + service_server_ = rclcpp::create_service( node->get_node_base_interface(), node->get_node_services_interface(), ns, diff --git a/tf2_ros/src/buffer_server.cpp b/tf2_ros/src/buffer_server.cpp index 2d94928f3..cf8f102af 100644 --- a/tf2_ros/src/buffer_server.cpp +++ b/tf2_ros/src/buffer_server.cpp @@ -52,20 +52,17 @@ void BufferServer::serviceCB( const std::shared_ptr request, std::shared_ptr response) { - // TODO: implement 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 { + } else { response->transform = buffer_.lookupTransform( - request->target_frame, request->source_frame, - tf2_ros::fromMsg(request->source_time)); + 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(); @@ -84,7 +81,7 @@ void BufferServer::serviceCB( } catch (const tf2::TransformException & ex) { response->error.error = response->error.TRANSFORM_ERROR; response->error.error_string = ex.what(); - } + } } bool BufferServer::canTransform(const std::shared_ptr request) From 3770bf7963baa2d1889b29cec2313a908626e37a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 13 Nov 2023 14:27:16 +0100 Subject: [PATCH 22/24] Remove LookupTransform action and update BufferClient naming --- tf2_msgs/CMakeLists.txt | 1 - tf2_msgs/action/LookupTransform.action | 17 ---------- tf2_ros_py/tf2_ros/buffer_client.py | 44 +++++++++++++------------- 3 files changed, 22 insertions(+), 40 deletions(-) delete mode 100644 tf2_msgs/action/LookupTransform.action diff --git a/tf2_msgs/CMakeLists.txt b/tf2_msgs/CMakeLists.txt index b5ab8ec74..c461125e7 100644 --- a/tf2_msgs/CMakeLists.txt +++ b/tf2_msgs/CMakeLists.txt @@ -20,7 +20,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "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/action/LookupTransform.action b/tf2_msgs/action/LookupTransform.action deleted file mode 100644 index 27d35d4bb..000000000 --- a/tf2_msgs/action/LookupTransform.action +++ /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_py/tf2_ros/buffer_client.py b/tf2_ros_py/tf2_ros/buffer_client.py index ff467e847..c26ead9aa 100644 --- a/tf2_ros_py/tf2_ros/buffer_client.py +++ b/tf2_ros_py/tf2_ros/buffer_client.py @@ -53,7 +53,7 @@ from tf2_msgs.srv import LookupTransform # Used for documentation purposes only -LookupTransformGoal = TypeVar('LookupTransformGoal') +LookupTransformRequest = TypeVar('LookupTransformRequest') LookupTransformResult = TypeVar('LookupTransformResult') @@ -109,14 +109,14 @@ def lookup_transform( else: raise TypeError('Must pass a rclpy.time.Time object.') - goal = LookupTransform.Request() - 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( @@ -139,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.Request() - 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( @@ -204,7 +204,7 @@ def can_transform_full( except tf2.TransformException: return False - def __process_goal(self, goal: LookupTransformGoal) -> TransformStamped: + 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() @@ -213,14 +213,14 @@ def unblock(future): nonlocal event event.set() - future = self.service_client.call_async(goal) + future = self.service_client.call_async(request) future.add_done_callback(unblock) def unblock_by_timeout(): - nonlocal 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 future.done() and not event.is_set(): if clock.now() > start_time + timeout + timeout_padding: @@ -240,7 +240,7 @@ def unblock_by_timeout(): # This shouldn't happen, but could in rare cases where the server hangs if not 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.") + 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() From f1507054cf4e806c2ab802829ea5bf33fac3f708 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 13 Nov 2023 14:29:57 +0100 Subject: [PATCH 23/24] reaname result to response --- tf2_ros_py/tf2_ros/buffer_client.py | 42 ++++++++++++++--------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/tf2_ros_py/tf2_ros/buffer_client.py b/tf2_ros_py/tf2_ros/buffer_client.py index c26ead9aa..8f6f2b1b9 100644 --- a/tf2_ros_py/tf2_ros/buffer_client.py +++ b/tf2_ros_py/tf2_ros/buffer_client.py @@ -54,7 +54,7 @@ # Used for documentation purposes only LookupTransformRequest = TypeVar('LookupTransformRequest') -LookupTransformResult = TypeVar('LookupTransformResult') +LookupTransformResponse = TypeVar('LookupTransformResponse') class BufferClient(tf2_ros.BufferInterface): @@ -244,26 +244,26 @@ def unblock_by_timeout(): response = future.result() - return self.__process_result(response) - - def __process_result(self, result: LookupTransform.Response) -> 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 + 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.""" From 84cff94c840b21ca1dcfd314b048e184617d98ef Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 13 Nov 2023 16:59:53 +0100 Subject: [PATCH 24/24] draft of cpp buffer client --- tf2_ros/include/tf2_ros/buffer_client.h | 26 +++-- tf2_ros/src/buffer_client.cpp | 136 +++++++++--------------- 2 files changed, 65 insertions(+), 97 deletions(-) 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/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(