示例#1
0
bool
wait_for_client_to_be_ready(
  rcl_client_t * client,
  size_t max_tries,
  int64_t period_ms)
{
  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, rcl_get_default_allocator());
  if (ret != RCL_RET_OK) {
    fprintf(stderr, "Error in wait set init: %s\n", rcl_get_error_string_safe());
    return false;
  }
  auto wait_set_exit = make_scope_exit([&wait_set]() {
    if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) {
      fprintf(stderr, "Error in wait set fini: %s\n", rcl_get_error_string_safe());
      throw std::runtime_error("error while waiting for client");
    }
  });
  size_t iteration = 0;
  do {
    ++iteration;
    if (rcl_wait_set_clear_clients(&wait_set) != RCL_RET_OK) {
      fprintf(stderr, "Error in wait_set_clear_clients: %s\n", rcl_get_error_string_safe());
      return false;
    }
    if (rcl_wait_set_add_client(&wait_set, client) != RCL_RET_OK) {
      fprintf(stderr, "Error in wait_set_add_client: %s\n", rcl_get_error_string_safe());
      return false;
    }
    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
    if (ret == RCL_RET_TIMEOUT) {
      continue;
    }
    if (ret != RCL_RET_OK) {
      fprintf(stderr, "Error in wait: %s\n", rcl_get_error_string_safe());
      return false;
    }
    for (size_t i = 0; i < wait_set.size_of_clients; ++i) {
      if (wait_set.clients[i] && wait_set.clients[i] == client) {
        return true;
      }
    }
  } while (iteration < max_tries);
  return false;
}
示例#2
0
void
wait_for_service_to_be_ready(
  rcl_service_t * service,
  size_t max_tries,
  int64_t period_ms,
  bool & success)
{
  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator());
  ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
  auto wait_set_exit = make_scope_exit([&wait_set]() {
    stop_memory_checking();
    rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
  });
  size_t iteration = 0;
  do {
    ++iteration;
    ret = rcl_wait_set_clear_services(&wait_set);
    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
    ret = rcl_wait_set_add_service(&wait_set, service);
    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
    ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
    if (ret == RCL_RET_TIMEOUT) {
      continue;
    }
    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
    for (size_t i = 0; i < wait_set.size_of_services; ++i) {
      if (wait_set.services[i] && wait_set.services[i] == service) {
        success = true;
        return;
      }
    }
  } while (iteration < max_tries);
  success = false;
}
示例#3
0
void
GraphListener::run_loop()
{
  while (true) {
    // If shutdown() was called, exit.
    if (is_shutdown_.load()) {
      return;
    }
    rcl_ret_t ret;
    {
      // This "barrier" lock ensures that other functions can acquire the
      // node_graph_interfaces_mutex_ after waking up rcl_wait.
      std::lock_guard<std::mutex> nodes_barrier_lock(node_graph_interfaces_barrier_mutex_);
      // This is ownership is passed to nodes_lock in the next line.
      node_graph_interfaces_mutex_.lock();
    }
    // This lock is released when the loop continues or exits.
    std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);

    // Resize the wait set if necessary.
    if (wait_set_.size_of_guard_conditions < (node_graph_interfaces_.size() + 2)) {
      ret = rcl_wait_set_resize_guard_conditions(&wait_set_, node_graph_interfaces_.size() + 2);
      if (RCL_RET_OK != ret) {
        throw_from_rcl_error(ret, "failed to resize wait set");
      }
    }
    // Clear the wait set's guard conditions.
    ret = rcl_wait_set_clear_guard_conditions(&wait_set_);
    if (RCL_RET_OK != ret) {
      throw_from_rcl_error(ret, "failed to clear wait set");
    }
    // Put the interrupt guard condition in the wait set.
    ret = rcl_wait_set_add_guard_condition(&wait_set_, &interrupt_guard_condition_);
    if (RCL_RET_OK != ret) {
      throw_from_rcl_error(ret, "failed to add interrupt guard condition to wait set");
    }
    // Put the shutdown guard condition in the wait set.
    ret = rcl_wait_set_add_guard_condition(&wait_set_, shutdown_guard_condition_);
    if (RCL_RET_OK != ret) {
      throw_from_rcl_error(ret, "failed to add shutdown guard condition to wait set");
    }
    // Put graph guard conditions for each node into the wait set.
    for (const auto node_ptr : node_graph_interfaces_) {
      // Only wait on graph changes if some user of the node is watching.
      if (node_ptr->count_graph_users() == 0) {
        continue;
      }
      // Add the graph guard condition for the node to the wait set.
      auto graph_gc = node_ptr->get_graph_guard_condition();
      if (!graph_gc) {
        throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
      }
      ret = rcl_wait_set_add_guard_condition(&wait_set_, graph_gc);
      if (RCL_RET_OK != ret) {
        throw_from_rcl_error(ret, "failed to add graph guard condition to wait set");
      }
    }

    // Wait for: graph changes, interrupt, or shutdown/SIGINT
    ret = rcl_wait(&wait_set_, -1);  // block for ever until a guard condition is triggered
    if (RCL_RET_TIMEOUT == ret) {
      throw std::runtime_error("rcl_wait unexpectedly timed out");
    }
    if (RCL_RET_OK != ret) {
      throw_from_rcl_error(ret, "failed to wait on wait set");
    }

    bool shutdown_guard_condition_triggered = false;
    // Check to see if the shutdown guard condition has been triggered.
    for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
      if (shutdown_guard_condition_ == wait_set_.guard_conditions[i]) {
        shutdown_guard_condition_triggered = true;
      }
    }
    // Notify nodes who's guard conditions are set (triggered).
    for (const auto node_ptr : node_graph_interfaces_) {
      auto graph_gc = node_ptr->get_graph_guard_condition();
      if (!graph_gc) {
        throw_from_rcl_error(RCL_RET_ERROR, "failed to get graph guard condition");
      }
      for (size_t i = 0; i < wait_set_.size_of_guard_conditions; ++i) {
        if (graph_gc == wait_set_.guard_conditions[i]) {
          node_ptr->notify_graph_change();
        }
      }
      if (shutdown_guard_condition_triggered) {
        // If shutdown, then notify the node of this as well.
        node_ptr->notify_shutdown();
      }
    }
  }  // while (true)
}