diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000..e43b0f9889 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.DS_Store diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 55be36708e..e7841542d3 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -129,13 +129,15 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) } // update the time even on the first loop to account for time spent in the first call // to this->server_is_ready() - std::chrono::nanoseconds time_to_wait = timeout - (std::chrono::steady_clock::now() - start); - if (timeout > std::chrono::nanoseconds(0) && time_to_wait < std::chrono::nanoseconds(0)) { + std::chrono::nanoseconds time_to_wait = + timeout > std::chrono::nanoseconds(0) ? + timeout - (std::chrono::steady_clock::now() - start) : + std::chrono::nanoseconds::max(); + if (time_to_wait < std::chrono::nanoseconds(0)) { // Do not allow the time_to_wait to become negative when timeout was originally positive. // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while. time_to_wait = std::chrono::nanoseconds(0); } - // continue forever if timeout is negative, otherwise continue until out of time_to_wait do { if (!rclcpp::ok(this->context_)) { return false; @@ -156,8 +158,11 @@ ClientBase::wait_for_service_nanoseconds(std::chrono::nanoseconds timeout) return true; } // server is not ready, loop if there is time left - time_to_wait = timeout - (std::chrono::steady_clock::now() - start); - } while (time_to_wait > std::chrono::nanoseconds(0) || timeout < std::chrono::nanoseconds(0)); + if (timeout > std::chrono::nanoseconds(0)) { + time_to_wait = timeout - (std::chrono::steady_clock::now() - start); + } + // if timeout is negative, time_to_wait will never reach zero + } while (time_to_wait > std::chrono::nanoseconds(0)); return false; // timeout exceeded while waiting for the server to be ready } diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index bdd0c25f21..0ce4b4a948 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include #include @@ -25,6 +27,7 @@ #include #include +#include #include #include #include @@ -57,6 +60,22 @@ class ClientBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); + /// Return true if there is an action server that is ready to take goal requests. + RCLCPP_ACTION_PUBLIC + bool + action_server_is_ready() const; + + /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. + template + bool + wait_for_action_server( + std::chrono::duration timeout = std::chrono::duration(-1)) + { + return wait_for_action_server_nanoseconds( + std::chrono::duration_cast(timeout) + ); + } + // ------------- // Waitables API @@ -107,11 +126,17 @@ class ClientBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & options); + /// Wait for action_server_is_ready() to become true, or until the given timeout is reached. + RCLCPP_ACTION_PUBLIC + bool + wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout); + // ----------------------------------------------------- // API for communication between ClientBase and Client<> using ResponseCallback = std::function response)>; @@ -242,12 +267,13 @@ class Client : public ClientBase Client( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rcl_action_client_options_t client_options = rcl_action_client_get_default_options() ) : ClientBase( - node_base, node_logging, action_name, + node_base, node_graph, node_logging, action_name, rosidl_typesupport_cpp::get_action_type_support_handle(), client_options) { diff --git a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp index e473b6b8a6..3cf7960a33 100644 --- a/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp +++ b/rclcpp_action/include/rclcpp_action/client_goal_handle_impl.hpp @@ -19,6 +19,8 @@ #include +#include "rclcpp/logging.hpp" +#include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/exceptions.hpp" namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index 730083f24e..9f1e33a0ad 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -63,7 +63,11 @@ create_client( }; std::shared_ptr> action_client( - new Client(node->get_node_base_interface(), node->get_node_logging_interface(), name), + new Client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + name), deleter); node->get_node_waitables_interface()->add_waitable(action_client, group); diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index c8b3de30d2..e3f1198dcc 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -34,11 +34,13 @@ class ClientBaseImpl public: ClientBaseImpl( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) - : node_handle(node_base->get_shared_rcl_node_handle()), + : node_graph_(node_graph), + node_handle(node_base->get_shared_rcl_node_handle()), logger(node_logging->get_logger().get_child("rclcpp_acton")), random_bytes_generator(std::random_device{} ()) { @@ -96,6 +98,8 @@ class ClientBaseImpl bool is_cancel_response_ready{false}; bool is_result_response_ready{false}; + rclcpp::Context::SharedPtr context_; + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; std::shared_ptr client_handle{nullptr}; std::shared_ptr node_handle{nullptr}; rclcpp::Logger logger; @@ -117,11 +121,13 @@ class ClientBaseImpl ClientBase::ClientBase( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, const rosidl_action_type_support_t * type_support, const rcl_action_client_options_t & client_options) -: pimpl_(new ClientBaseImpl(node_base, node_logging, action_name, type_support, client_options)) +: pimpl_(new ClientBaseImpl( + node_base, node_graph, node_logging, action_name, type_support, client_options)) { } @@ -129,6 +135,84 @@ ClientBase::~ClientBase() { } +bool +ClientBase::action_server_is_ready() const +{ + bool is_ready; + rcl_ret_t ret = rcl_action_server_is_available( + this->pimpl_->node_handle.get(), + this->pimpl_->client_handle.get(), + &is_ready); + if (RCL_RET_NODE_INVALID == ret) { + const rcl_node_t * node_handle = this->pimpl_->node_handle.get(); + if (node_handle && !rcl_context_is_valid(node_handle->context)) { + // context is shutdown, do a soft failure + return false; + } + } + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "rcl_action_server_is_available failed"); + } + return is_ready; +} + +bool +ClientBase::wait_for_action_server_nanoseconds(std::chrono::nanoseconds timeout) +{ + auto start = std::chrono::steady_clock::now(); + // make an event to reuse, rather than create a new one each time + auto node_ptr = pimpl_->node_graph_.lock(); + if (!node_ptr) { + throw rclcpp::exceptions::InvalidNodeError(); + } + auto event = node_ptr->get_graph_event(); + // check to see if the server is ready immediately + if (this->action_server_is_ready()) { + return true; + } + if (timeout == std::chrono::nanoseconds(0)) { + // check was non-blocking, return immediately + return false; + } + // update the time even on the first loop to account for time spent in the first call + // to this->server_is_ready() + std::chrono::nanoseconds time_to_wait = + timeout > std::chrono::nanoseconds(0) ? + timeout - (std::chrono::steady_clock::now() - start) : + std::chrono::nanoseconds::max(); + if (time_to_wait < std::chrono::nanoseconds(0)) { + // Do not allow the time_to_wait to become negative when timeout was originally positive. + // Setting time_to_wait to 0 will allow one non-blocking wait because of the do-while. + time_to_wait = std::chrono::nanoseconds(0); + } + do { + if (!rclcpp::ok(this->pimpl_->context_)) { + return false; + } + // Limit each wait to 100ms to workaround an issue specific to the Connext RMW implementation. + // A race condition means that graph changes for services becoming available may trigger the + // wait set to wake up, but then not be reported as ready immediately after the wake up + // (see https://github.com/ros2/rmw_connext/issues/201) + // If no other graph events occur, the wait set will not be triggered again until the timeout + // has been reached, despite the service being available, so we artificially limit the wait + // time to limit the delay. + node_ptr->wait_for_graph_change( + event, std::min(time_to_wait, std::chrono::nanoseconds(RCL_MS_TO_NS(100)))); + // Because of the aforementioned race condition, we check if the service is ready even if the + // graph event wasn't triggered. + event->check_and_clear(); + if (this->action_server_is_ready()) { + return true; + } + // server is not ready, loop if there is time left + if (timeout > std::chrono::nanoseconds(0)) { + time_to_wait = timeout - (std::chrono::steady_clock::now() - start); + } + // if timeout is negative, time_to_wait will never reach zero + } while (time_to_wait > std::chrono::nanoseconds(0)); + return false; // timeout exceeded while waiting for the server to be ready +} + rclcpp::Logger ClientBase::get_logger() { diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index d06d627685..ac787a4bcc 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -217,11 +217,17 @@ ServerBase::execute_goal_request_received() &request_header, message.get()); - if (RCL_RET_OK != ret) { + pimpl_->goal_request_ready_ = false; + + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - pimpl_->goal_request_ready_ = false; GoalID uuid = get_goal_id_from_goal_request(message.get()); convert(uuid, &goal_info); @@ -295,7 +301,14 @@ ServerBase::execute_cancel_request_received() &request_header, request.get()); - if (RCL_RET_OK != ret) { + pimpl_->cancel_request_ready_ = false; + + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } @@ -361,7 +374,14 @@ ServerBase::execute_result_request_received() ret = rcl_action_take_result_request( pimpl_->action_server_.get(), &request_header, result_request.get()); - if (RCL_RET_OK != ret) { + pimpl_->result_request_ready_ = false; + + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); }