示例#1
0
文件: test_rcl.cpp 项目: younata/rcl
/* Tests the rcl_get_instance_id() and rcl_ok() functions.
 */
TEST_F(TestRCLFixture, test_rcl_get_instance_id_and_ok) {
  rcl_ret_t ret;
  // Instance id should be 0 before rcl_init().
  EXPECT_EQ(0u, rcl_get_instance_id());
  ASSERT_FALSE(rcl_ok());
  // It should still return 0 after an invalid init.
  ret = rcl_init(1, nullptr, rcl_get_default_allocator());
  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
  EXPECT_EQ(0u, rcl_get_instance_id());
  ASSERT_FALSE(rcl_ok());
  // A non-zero instance id should be returned after a valid init.
  {
    FakeTestArgv test_args;
    ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
    EXPECT_EQ(RCL_RET_OK, ret);
    ASSERT_TRUE(rcl_ok());
  }
  // And it should be allocation free.
  uint64_t first_instance_id;
  assert_no_malloc_begin();
  assert_no_realloc_begin();
  assert_no_free_begin();
  first_instance_id = rcl_get_instance_id();
  assert_no_malloc_end();
  assert_no_realloc_end();
  assert_no_free_end();
  EXPECT_NE(0u, first_instance_id);
  EXPECT_EQ(first_instance_id, rcl_get_instance_id());  // Repeat calls should return the same.
  EXPECT_EQ(true, rcl_ok());
  // Calling after a shutdown should return 0.
  ret = rcl_shutdown();
  EXPECT_EQ(ret, RCL_RET_OK);
  EXPECT_EQ(0u, rcl_get_instance_id());
  ASSERT_FALSE(rcl_ok());
  // It should return a different value after another valid init.
  {
    FakeTestArgv test_args;
    ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
    EXPECT_EQ(RCL_RET_OK, ret);
    ASSERT_TRUE(rcl_ok());
  }
  EXPECT_NE(0u, rcl_get_instance_id());
  EXPECT_NE(first_instance_id, rcl_get_instance_id());
  ASSERT_TRUE(rcl_ok());
  // Shutting down a second time should result in 0 again.
  ret = rcl_shutdown();
  EXPECT_EQ(ret, RCL_RET_OK);
  EXPECT_EQ(0u, rcl_get_instance_id());
  ASSERT_FALSE(rcl_ok());
}
示例#2
0
/*
 * Expected usage:
 *
 *    rcl_node_t node = rcl_get_zero_initialized_node();
 *    rcl_node_options_t * node_ops = rcl_node_get_default_options();
 *    rcl_ret_t ret = rcl_node_init(&node, "node_name", node_ops);
 *    // ... error handling and then use the node, but finally deinitialize it:
 *    ret = rcl_node_fini(&node);
 *    // ... error handling for rcl_node_fini()
*/
int main(int argc, char ** argv)
{
    rcl_ret_t ret1 = rcl_init(argc, argv, rcl_get_default_allocator());
//    std::cout << "ret1: " << ret1 << std::endl;

    rcl_node_t node_ptr;
    node_ptr = rcl_get_zero_initialized_node();
    const char * name = "ros_test_node_victor";
    rcl_node_options_t node_options = rcl_node_get_default_options();
    rcl_ret_t ret = rcl_node_init(&node_ptr, name, &node_options);
    
    rcl_strings_t node_names;

    node_names = rcl_get_zero_initialized_strings();

    rcl_ret_t ret3 = rcl_get_node_names(&node_names);
    //std::cout << "ret: " << ret3 << std::endl;

    for(unsigned int i = 0; i < node_names.count; i++){
        std::cout << "->/" << node_names.data[i] << std::endl;
    }

    rcl_topic_names_and_types_t topic_names_and_types;
    topic_names_and_types.topic_count = 0;
    topic_names_and_types.topic_names = NULL;
    topic_names_and_types.type_names = NULL;
    rcl_ret_t result = rcl_get_topic_names_and_types(&node_ptr, &topic_names_and_types);
    std::cout << "result: " << result << std::endl;
    for(unsigned int i = 0; i < topic_names_and_types.topic_count; i++){
        std::cout << topic_names_and_types.topic_names[i] << std::endl;

    }

}
示例#3
0
void
GraphListener::start_if_not_started()
{
  std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
  if (is_shutdown_.load()) {
    throw GraphListenerShutdownError();
  }
  if (!is_started_) {
    // Initialize the wait set before starting.
    rcl_ret_t ret = rcl_wait_set_init(
      &wait_set_,
      0,  // number_of_subscriptions
      2,  // number_of_guard_conditions
      0,  // number_of_timers
      0,  // number_of_clients
      0,  // number_of_services
      rcl_get_default_allocator());
    if (RCL_RET_OK != ret) {
      throw_from_rcl_error(ret, "failed to initialize wait set");
    }
    // Register an on_shutdown hook to shtudown the graph listener.
    // This is important to ensure that the wait set is finalized before
    // destruction of static objects occurs.
    std::weak_ptr<GraphListener> weak_this = shared_from_this();
    rclcpp::utilities::on_shutdown([weak_this]() {
          auto shared_this = weak_this.lock();
          if (shared_this) {
            shared_this->shutdown();
          }
        });
    // Start the listener thread.
    listener_thread_ = std::thread(&GraphListener::run, this);
    is_started_ = true;
  }
}
示例#4
0
/* Tests the default allocator.
 */
TEST_F(TestAllocatorFixture, test_default_allocator_normal) {
#if defined(WIN32)
  printf("Allocator tests disabled on Windows.\n");
  return;
#endif  // defined(WIN32)
  ASSERT_NO_MALLOC(
    rcl_allocator_t allocator = rcl_get_default_allocator();
  )
示例#5
0
文件: service.c 项目: dhood/rcl
rcl_service_options_t
rcl_service_get_default_options()
{
  static rcl_service_options_t default_options;
  // Must set the allocator and qos after because they are not a compile time constant.
  default_options.qos = rmw_qos_profile_services_default;
  default_options.allocator = rcl_get_default_allocator();
  return default_options;
}
示例#6
0
void
rclcpp::utilities::init(int argc, char * argv[])
{
  g_is_interrupted.store(false);
  if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
    throw std::runtime_error(
      std::string("failed to initialize rmw implementation: ") + rcl_get_error_string_safe());
    // *INDENT-ON*
  }
#ifdef HAS_SIGACTION
  struct sigaction action;
  memset(&action, 0, sizeof(action));
  sigemptyset(&action.sa_mask);
  action.sa_sigaction = ::signal_handler;
  action.sa_flags = SA_SIGINFO;
  ssize_t ret = sigaction(SIGINT, &action, &old_action);
  if (ret == -1)
#else
  ::old_signal_handler = std::signal(SIGINT, ::signal_handler);
  // NOLINTNEXTLINE(readability/braces)
  if (::old_signal_handler == SIG_ERR)
#endif
  {
    const size_t error_length = 1024;
    // NOLINTNEXTLINE(runtime/arrays)
    char error_string[error_length];
#ifndef _WIN32
#ifdef _GNU_SOURCE
    char * msg = strerror_r(errno, error_string, error_length);
    if (msg != error_string) {
      strncpy(error_string, msg, error_length);
      msg[error_length - 1] = '\0';
    }
#else
    int error_status = strerror_r(errno, error_string, error_length);
    if (error_status != 0) {
      throw std::runtime_error("Failed to get error string for errno: " + std::to_string(errno));
    }
#endif
#else
    strerror_s(error_string, error_length, errno);
#endif
    // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
    throw std::runtime_error(
      std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") +
      error_string);
    // *INDENT-ON*
  }
}
示例#7
0
 void SetUp()
 {
   stop_memory_checking();
   rcl_ret_t ret;
   ret = rcl_init(0, nullptr, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
   this->node_ptr = new rcl_node_t;
   *this->node_ptr = rcl_get_zero_initialized_node();
   const char * name = "node_name";
   rcl_node_options_t node_options = rcl_node_get_default_options();
   ret = rcl_node_init(this->node_ptr, name, &node_options);
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
   set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
   set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
   set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
   start_memory_checking();
 }
示例#8
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;
}
示例#9
0
class CLASSNAME (TestMessagesFixture, RMW_IMPLEMENTATION) : public ::testing::Test
{
public:
  rcl_context_t * context_ptr;
  rcl_node_t * node_ptr;
  void SetUp()
  {
    rcl_ret_t ret;
    {
      rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
      ret = rcl_init_options_init(&init_options, rcl_get_default_allocator());
      ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
      OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
        EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str;
      });
      this->context_ptr = new rcl_context_t;
      *this->context_ptr = rcl_get_zero_initialized_context();
      ret = rcl_init(0, nullptr, &init_options, this->context_ptr);
      ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
    }
示例#10
0
class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing::Test
{
public:
  rcl_node_t * node_ptr;
  void SetUp()
  {
    stop_memory_checking();
    rcl_ret_t ret;
    ret = rcl_init(0, nullptr, rcl_get_default_allocator());
    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
    this->node_ptr = new rcl_node_t;
    *this->node_ptr = rcl_get_zero_initialized_node();
    const char * name = "node_name";
    rcl_node_options_t node_options = rcl_node_get_default_options();
    ret = rcl_node_init(this->node_ptr, name, &node_options);
    ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
    set_on_unexpected_malloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";});
    set_on_unexpected_realloc_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";});
    set_on_unexpected_free_callback([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";});
    start_memory_checking();
  }
示例#11
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;
}
示例#12
0
int main(int argc, char ** argv)
{
  int main_ret = 0;
  {
    if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
      fprintf(stderr, "Error in rcl init: %s\n", rcl_get_error_string_safe());
      return -1;
    }
    rcl_node_t node = rcl_get_zero_initialized_node();
    const char * name = "node_name";
    rcl_node_options_t node_options = rcl_node_get_default_options();
    if (rcl_node_init(&node, name, &node_options) != RCL_RET_OK) {
      fprintf(stderr, "Error in node init: %s\n", rcl_get_error_string_safe());
      return -1;
    }
    auto node_exit = make_scope_exit([&main_ret, &node]() {
      if (rcl_node_fini(&node) != RCL_RET_OK) {
        fprintf(stderr, "Error in node fini: %s\n", rcl_get_error_string_safe());
        main_ret = -1;
      }
    });

    const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
      example_interfaces, srv, AddTwoInts);
    const char * topic = "add_two_ints";

    rcl_client_t client = rcl_get_zero_initialized_client();
    rcl_client_options_t client_options = rcl_client_get_default_options();
    rcl_ret_t ret = rcl_client_init(&client, &node, ts, topic, &client_options);
    if (ret != RCL_RET_OK) {
      fprintf(stderr, "Error in client init: %s\n", rcl_get_error_string_safe());
      return -1;
    }

    auto client_exit = make_scope_exit([&client, &main_ret, &node]() {
      if (rcl_client_fini(&client, &node)) {
        fprintf(stderr, "Error in client fini: %s\n", rcl_get_error_string_safe());
        main_ret = -1;
      }
    });

    // Initialize a request.
    example_interfaces__srv__AddTwoInts_Request client_request;
    example_interfaces__srv__AddTwoInts_Request__init(&client_request);
    client_request.a = 1;
    client_request.b = 2;
    int64_t sequence_number;

    if (rcl_send_request(&client, &client_request, &sequence_number)) {
      fprintf(stderr, "Error in send request: %s\n", rcl_get_error_string_safe());
      return -1;
    }

    if (sequence_number != 1) {
      fprintf(stderr, "Got invalid sequence number\n");
      return -1;
    }

    example_interfaces__srv__AddTwoInts_Request__fini(&client_request);

    // Initialize the response owned by the client and take the response.
    example_interfaces__srv__AddTwoInts_Response client_response;
    example_interfaces__srv__AddTwoInts_Response__init(&client_response);

    if (!wait_for_client_to_be_ready(&client, 1000, 100)) {
      fprintf(stderr, "Client never became ready\n");
      return -1;
    }
    rmw_request_id_t header;
    if (rcl_take_response(&client, &header, &client_response) != RCL_RET_OK) {
      fprintf(stderr, "Error in send response: %s\n", rcl_get_error_string_safe());
      return -1;
    }

    example_interfaces__srv__AddTwoInts_Response__fini(&client_response);
  }

  return main_ret;
}
示例#13
0
文件: test_rcl.cpp 项目: younata/rcl
/* Tests the rcl_init(), rcl_ok(), and rcl_shutdown() functions.
 */
TEST_F(TestRCLFixture, test_rcl_init_and_ok_and_shutdown) {
  rcl_ret_t ret;
  // A shutdown before any init has been called should fail.
  ret = rcl_shutdown();
  EXPECT_EQ(RCL_RET_NOT_INIT, ret);
  rcl_reset_error();
  ASSERT_FALSE(rcl_ok());
  // If argc is not 0, but argv is, it should be an invalid argument.
  ret = rcl_init(42, nullptr, rcl_get_default_allocator());
  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
  rcl_reset_error();
  ASSERT_FALSE(rcl_ok());
  // If either the allocate or deallocate function pointers are not set, it should be invalid arg.
  rcl_allocator_t invalid_allocator = rcl_get_default_allocator();
  invalid_allocator.allocate = nullptr;
  ret = rcl_init(0, nullptr, invalid_allocator);
  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
  rcl_reset_error();
  ASSERT_FALSE(rcl_ok());
  invalid_allocator.allocate = rcl_get_default_allocator().allocate;
  invalid_allocator.deallocate = nullptr;
  ret = rcl_init(0, nullptr, invalid_allocator);
  EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
  rcl_reset_error();
  ASSERT_FALSE(rcl_ok());
  // If the malloc call fails (with some valid arguments to copy), it should be a bad alloc.
  {
    FakeTestArgv test_args;
    rcl_allocator_t failing_allocator = rcl_get_default_allocator();
    failing_allocator.allocate = &failing_malloc;
    failing_allocator.deallocate = failing_free;
    failing_allocator.reallocate = failing_realloc;
    ret = rcl_init(test_args.argc, test_args.argv, failing_allocator);
    EXPECT_EQ(RCL_RET_BAD_ALLOC, ret);
    rcl_reset_error();
    ASSERT_FALSE(rcl_ok());
  }
  // If argc is 0 and argv is nullptr and the allocator is valid, it should succeed.
  ret = rcl_init(0, nullptr, rcl_get_default_allocator());
  EXPECT_EQ(RCL_RET_OK, ret);
  ASSERT_TRUE(rcl_ok());
  // Then shutdown should work.
  ret = rcl_shutdown();
  EXPECT_EQ(ret, RCL_RET_OK);
  ASSERT_FALSE(rcl_ok());
  // Valid argc/argv values and a valid allocator should succeed.
  {
    FakeTestArgv test_args;
    ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
    EXPECT_EQ(RCL_RET_OK, ret);
    ASSERT_TRUE(rcl_ok());
  }
  // Then shutdown should work.
  ret = rcl_shutdown();
  EXPECT_EQ(RCL_RET_OK, ret);
  ASSERT_FALSE(rcl_ok());
  // A repeat call to shutdown should not work.
  ret = rcl_shutdown();
  EXPECT_EQ(RCL_RET_NOT_INIT, ret);
  rcl_reset_error();
  ASSERT_FALSE(rcl_ok());
  // Repeat, but valid, calls to rcl_init() should fail.
  {
    FakeTestArgv test_args;
    ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
    EXPECT_EQ(RCL_RET_OK, ret);
    ASSERT_TRUE(rcl_ok());
    ret = rcl_init(test_args.argc, test_args.argv, rcl_get_default_allocator());
    EXPECT_EQ(RCL_RET_ALREADY_INIT, ret);
    rcl_reset_error();
    ASSERT_TRUE(rcl_ok());
  }
  // But shutdown should still work.
  ret = rcl_shutdown();
  EXPECT_EQ(ret, RCL_RET_OK);
  ASSERT_FALSE(rcl_ok());
}
示例#14
0
int main(int argc, char ** argv)
{
  int main_ret = 0;
  {
    if (rcl_init(argc, argv, rcl_get_default_allocator()) != RCL_RET_OK) {
      fprintf(stderr, "Error in rcl init: %s\n", rcl_get_error_string_safe());
      return -1;
    }
    rcl_node_t node = rcl_get_zero_initialized_node();
    const char * name = "node_name";
    rcl_node_options_t node_options = rcl_node_get_default_options();
    if (rcl_node_init(&node, name, &node_options) != RCL_RET_OK) {
      fprintf(stderr, "Error in node init: %s\n", rcl_get_error_string_safe());
      return -1;
    }
    auto node_exit = make_scope_exit([&main_ret, &node]() {
      if (rcl_node_fini(&node) != RCL_RET_OK) {
        fprintf(stderr, "Error in node fini: %s\n", rcl_get_error_string_safe());
        main_ret = -1;
      }
    });

    const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
      example_interfaces, srv, AddTwoInts);
    const char * topic = "add_two_ints";

    rcl_service_t service = rcl_get_zero_initialized_service();
    rcl_service_options_t service_options = rcl_service_get_default_options();
    rcl_ret_t ret = rcl_service_init(&service, &node, ts, topic, &service_options);
    if (ret != RCL_RET_OK) {
      fprintf(stderr, "Error in service init: %s\n", rcl_get_error_string_safe());
      return -1;
    }

    auto service_exit = make_scope_exit([&main_ret, &service, &node]() {
      if (rcl_service_fini(&service, &node)) {
        fprintf(stderr, "Error in service fini: %s\n", rcl_get_error_string_safe());
        main_ret = -1;
      }
    });

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    // Initialize a response.
    example_interfaces__srv__AddTwoInts_Response service_response;
    example_interfaces__srv__AddTwoInts_Response__init(&service_response);
    auto response_exit = make_scope_exit([&service_response]() {
      example_interfaces__srv__AddTwoInts_Response__fini(&service_response);
    });

    // Block until a client request comes in.

    if (!wait_for_service_to_be_ready(&service, 1000, 100)) {
      fprintf(stderr, "Service never became ready\n");
      return -1;
    }

    // Take the pending request.
    example_interfaces__srv__AddTwoInts_Request service_request;
    example_interfaces__srv__AddTwoInts_Request__init(&service_request);
    auto request_exit = make_scope_exit([&service_request]() {
      example_interfaces__srv__AddTwoInts_Request__fini(&service_request);
    });
    rmw_request_id_t header;
    // TODO(jacquelinekay) May have to check for timeout error codes
    if (rcl_take_request(&service, &header, &service_request) != RCL_RET_OK) {
      fprintf(stderr, "Error in take_request: %s\n", rcl_get_error_string_safe());
      return -1;
    }

    // Sum the request and send the response.
    service_response.sum = service_request.a + service_request.b;
    if (rcl_send_response(&service, &header, &service_response) != RCL_RET_OK) {
      fprintf(stderr, "Error in send_response: %s\n", rcl_get_error_string_safe());
      return -1;
    }
    // Our scope exits should take care of fini for everything
  }
  return main_ret;
}