Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature request: check for valid ros context in spin_some #165

Merged
merged 2 commits into from
Jul 28, 2021
Merged

Feature request: check for valid ros context in spin_some #165

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c1c192b
moved rcl_context_is_valid to spin_some
JanStaschulat Jul 28, 2021
e9e9c7c
ament_uncrustify --reformat
JanStaschulat Jul 28, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 13 additions & 3 deletions rclc/src/rclc/executor.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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;
Expand Down
6 changes: 3 additions & 3 deletions rclc_examples/src/example_parameter_server.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,10 +107,10 @@ int main()
rclc_parameter_get_int(&param_server, "param2", &param2);
rclc_parameter_get_double(&param_server, "param3", &param3);

// 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);
Expand Down
39 changes: 21 additions & 18 deletions rclc_examples/src/example_sub_context.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;


Expand All @@ -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) {
Expand All @@ -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);
Expand All @@ -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];
Expand All @@ -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<n_topics; i++){
for (unsigned int i = 0; i < n_topics; i++) {
rc = rclc_publisher_init_default(
&(my_pubs[i]),
&my_node,
Expand All @@ -108,11 +109,13 @@ int main(int argc, const char * argv[])
return -1;
}
// assign message to publisher
std_msgs__msg__String__init( &( pub_msgs[i] ) );
std_msgs__msg__String__init(&( pub_msgs[i] ) );
const unsigned int PUB_MSG_CAPACITY = 40;
pub_msgs[i].data.data = malloc(PUB_MSG_CAPACITY);
pub_msgs[i].data.capacity = PUB_MSG_CAPACITY;
snprintf(pub_msgs[i].data.data, pub_msgs[i].data.capacity, "Hello World! on %s", topic_names[i]);
snprintf(
pub_msgs[i].data.data, pub_msgs[i].data.capacity, "Hello World! on %s",
topic_names[i]);
pub_msgs[i].data.size = strlen(pub_msgs[i].data.data);

// create subscription
Expand All @@ -130,7 +133,7 @@ int main(int argc, const char * argv[])
}

// one string message for subscriber
std_msgs__msg__String__init( &( sub_msgs[i] ) );
std_msgs__msg__String__init(&( sub_msgs[i] ) );
}

////////////////////////////////////////////////////////////////////////////
Expand All @@ -144,11 +147,11 @@ int main(int argc, const char * argv[])
rclc_executor_init(&executor, &support.context, num_handles, &allocator);

// add subscriptions to executor
for(unsigned int i=0; i<n_topics; i++){
for (unsigned int i = 0; i < n_topics; i++) {
// create a void* pointer to any information you want in your callback
// make sure you cast back to the the same type before accessing it.
sub_context_t * context_ptr = &( my_contexts[i] );
void* context_void_ptr = (void*) context_ptr;
void * context_void_ptr = (void *) context_ptr;

// add subscription to executor
rc = rclc_executor_add_subscription_with_context(
Expand All @@ -166,7 +169,7 @@ int main(int argc, const char * argv[])
// timeout specified in nanoseconds (here 1s)
rc = rclc_executor_spin_some(&executor, 1000 * (1000 * 1000));

for(unsigned int i=0;i<n_topics; i++) {
for (unsigned int i = 0; i < n_topics; i++) {
//publish once for each topic
rc = rcl_publish(&my_pubs[i], &pub_msgs[i], NULL);
if (rc == RCL_RET_OK) {
Expand All @@ -183,14 +186,14 @@ int main(int argc, const char * argv[])
// clean up
rc = rclc_executor_fini(&executor);

for(unsigned int i=0;i<n_topics; i++) {
for (unsigned int i = 0; i < n_topics; i++) {
rc += rcl_publisher_fini(&(my_pubs[i]), &my_node);
rc += rcl_subscription_fini(&(my_subs[i]), &my_node);
}
rc += rcl_node_fini(&my_node);
rc += rclc_support_fini(&support);

for(unsigned int i=0;i<n_topics; i++) {
for (unsigned int i = 0; i < n_topics; i++) {
std_msgs__msg__String__fini(&(pub_msgs[i]));
std_msgs__msg__String__fini(&(sub_msgs[i]));
}
Expand Down