示例#1
0
/* Tests the rcl_system_time_point_now() function.
 */
TEST_F(TestTimeFixture, test_rcl_system_time_point_now) {
  assert_no_realloc_begin();
  rcl_ret_t ret;
  // Check for invalid argument error condition (allowed to alloc).
  ret = rcl_system_time_point_now(nullptr);
  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
  rcl_reset_error();
  assert_no_malloc_begin();
  assert_no_free_begin();
  // Check for normal operation (not allowed to alloc).
  rcl_system_time_point_t now = {0};
  ret = rcl_system_time_point_now(&now);
  assert_no_malloc_end();
  assert_no_realloc_end();
  assert_no_free_end();
  stop_memory_checking();
  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
  EXPECT_NE(now.nanoseconds, 0u);
  // Compare to std::chrono::system_clock time (within a second).
  now = {0};
  ret = rcl_system_time_point_now(&now);
  {
    std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
    auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
    int64_t now_ns_int = now_ns.count();
    int64_t now_diff = now.nanoseconds - now_ns_int;
    const int k_tolerance_ms = 1000;
    EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "system_clock differs";
  }
}
示例#2
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;
}
示例#3
0
/* Tests the rcl_steady_time_point_now() function.
 */
TEST_F(TestTimeFixture, test_rcl_steady_time_point_now) {
  assert_no_realloc_begin();
  rcl_ret_t ret;
  // Check for invalid argument error condition (allowed to alloc).
  ret = rcl_steady_time_point_now(nullptr);
  EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
  rcl_reset_error();
  assert_no_malloc_begin();
  assert_no_free_begin();
  // Check for normal operation (not allowed to alloc).
  rcl_steady_time_point_t now = {0};
  ret = rcl_steady_time_point_now(&now);
  assert_no_malloc_end();
  assert_no_realloc_end();
  assert_no_free_end();
  stop_memory_checking();
  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
  EXPECT_NE(now.nanoseconds, 0u);
  // Compare to std::chrono::steady_clock difference of two times (within a second).
  now = {0};
  ret = rcl_steady_time_point_now(&now);
  std::chrono::steady_clock::time_point now_sc = std::chrono::steady_clock::now();
  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
  // Wait for a little while.
  std::this_thread::sleep_for(std::chrono::milliseconds(100));
  // Then take a new timestamp with each and compare.
  rcl_steady_time_point_t later;
  ret = rcl_steady_time_point_now(&later);
  std::chrono::steady_clock::time_point later_sc = std::chrono::steady_clock::now();
  EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
  int64_t steady_diff = later.nanoseconds - now.nanoseconds;
  int64_t sc_diff =
    std::chrono::duration_cast<std::chrono::nanoseconds>(later_sc - now_sc).count();
  const int k_tolerance_ms = 1;
  EXPECT_LE(llabs(steady_diff - sc_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "steady_clock differs";
}
示例#4
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;
}