diff --git a/rclc/src/rclc/executor.c b/rclc/src/rclc/executor.c index d1d94b25..86736704 100644 --- a/rclc/src/rclc/executor.c +++ b/rclc/src/rclc/executor.c @@ -1194,6 +1194,11 @@ rclc_executor_spin_some(rclc_executor_t * executor, const uint64_t timeout_ns) RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "spin_some"); + if (!rcl_context_is_valid(executor->context)) { + PRINT_RCLC_ERROR(rclc_executor_spin_some, rcl_context_not_valid); + return RCL_RET_ERROR; + } + rclc_executor_prepare(executor); // set rmw fields to NULL @@ -1322,7 +1327,7 @@ rclc_executor_spin(rclc_executor_t * executor) RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret = RCL_RET_OK; printf("INFO: rcl_wait timeout %ld ms\n", ((executor->timeout_ns / 1000) / 1000)); - while (rcl_context_is_valid(executor->context) ) { + while (true) { ret = rclc_executor_spin_some(executor, executor->timeout_ns); if (!((ret == RCL_RET_OK) || (ret == RCL_RET_TIMEOUT))) { RCL_SET_ERROR_MSG("rclc_executor_spin_some error"); @@ -1371,8 +1376,13 @@ rcl_ret_t rclc_executor_spin_period(rclc_executor_t * executor, const uint64_t period) { RCL_CHECK_ARGUMENT_FOR_NULL(executor, RCL_RET_INVALID_ARGUMENT); - while (rcl_context_is_valid(executor->context) ) { - rclc_executor_spin_one_period(executor, period); + rcl_ret_t ret; + while (true) { + ret = rclc_executor_spin_one_period(executor, period); + if (!((ret == RCL_RET_OK) || (ret == RCL_RET_TIMEOUT))) { + RCL_SET_ERROR_MSG("rclc_executor_spin_one_period error"); + return ret; + } } // never get here return RCL_RET_OK; diff --git a/rclc_examples/src/example_parameter_server.c b/rclc_examples/src/example_parameter_server.c index 90d42221..2156495c 100644 --- a/rclc_examples/src/example_parameter_server.c +++ b/rclc_examples/src/example_parameter_server.c @@ -107,10 +107,10 @@ int main() rclc_parameter_get_int(¶m_server, "param2", ¶m2); rclc_parameter_get_double(¶m_server, "param3", ¶m3); - // Optional prepare for avoiding allocations during spin - rclc_executor_prepare(&executor); + // Optional prepare for avoiding allocations during spin + rclc_executor_prepare(&executor); - rclc_executor_spin(&executor); + rclc_executor_spin(&executor); // clean up rc = rclc_executor_fini(&executor); diff --git a/rclc_examples/src/example_sub_context.c b/rclc_examples/src/example_sub_context.c index 3341fe38..33a3c5ae 100644 --- a/rclc_examples/src/example_sub_context.c +++ b/rclc_examples/src/example_sub_context.c @@ -25,9 +25,10 @@ // Instead of creating some global variables, // we can define some data structures point to the local state info we care about -typedef struct { +typedef struct +{ int some_int; - char* some_text; + char * some_text; } sub_context_t; @@ -36,7 +37,7 @@ typedef struct { // subscriptions with context allow you to pass // additional state information to your subscription callback -void my_subscriber_callback_with_context(const void * msgin, void* context_void_ptr) +void my_subscriber_callback_with_context(const void * msgin, void * context_void_ptr) { const std_msgs__msg__String * msg = (const std_msgs__msg__String *)msgin; if (msg == NULL) { @@ -49,7 +50,7 @@ void my_subscriber_callback_with_context(const void * msgin, void* context_void_ printf("Callback: context is empty\n"); } else { // cast the context pointer into the appropriate type - sub_context_t * context_ptr = (sub_context_t * ) context_void_ptr; + sub_context_t * context_ptr = (sub_context_t *) context_void_ptr; // then you can access the context data printf("Callback: context contains: %s\n", context_ptr->some_text); printf("Callback: context also contains: %d\n", context_ptr->some_int); @@ -66,12 +67,12 @@ int main(int argc, const char * argv[]) rcl_ret_t rc; // within main, we can create the state information our subscriptions work with - const unsigned int n_topics=3; + const unsigned int n_topics = 3; const char * topic_names[] = {"topic_foo", "topic_bar", "topic_baz"}; - sub_context_t my_contexts[]= { - {0,"foo counting from zero"}, - {100,"bar counting from 100"}, - {300,"baz counting from 300"}, + sub_context_t my_contexts[] = { + {0, "foo counting from zero"}, + {100, "bar counting from 100"}, + {300, "baz counting from 300"}, }; rcl_publisher_t my_pubs[n_topics]; std_msgs__msg__String pub_msgs[n_topics]; @@ -97,7 +98,7 @@ int main(int argc, const char * argv[]) ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String); //initialise each publisher and subscriber - for(unsigned int i=0; i