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.

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

Add rclc_parameter Quality Declaration #144

Merged
merged 8 commits into from
Aug 13, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
6 changes: 6 additions & 0 deletions rclc/include/rclc/executor.h
Original file line number Diff line number Diff line change
Expand Up @@ -260,6 +260,7 @@ rclc_executor_add_subscription(
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer (NULL context is ignored)
* \return `RCL_RET_ERROR` if any other error occured
*/
RCLC_PUBLIC
rcl_ret_t
rclc_executor_add_subscription_with_context(
rclc_executor_t * executor,
Expand Down Expand Up @@ -504,6 +505,7 @@ rclc_executor_add_guard_condition(
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
RCLC_PUBLIC
rcl_ret_t
rclc_executor_remove_subscription(
rclc_executor_t * executor,
Expand Down Expand Up @@ -531,6 +533,7 @@ rclc_executor_remove_subscription(
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
RCLC_PUBLIC
rcl_ret_t
rclc_executor_remove_timer(
rclc_executor_t * executor,
Expand Down Expand Up @@ -558,6 +561,7 @@ rclc_executor_remove_timer(
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
RCLC_PUBLIC
rcl_ret_t
rclc_executor_remove_client(
rclc_executor_t * executor,
Expand Down Expand Up @@ -585,6 +589,7 @@ rclc_executor_remove_client(
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
RCLC_PUBLIC
rcl_ret_t
rclc_executor_remove_service(
rclc_executor_t * executor,
Expand All @@ -611,6 +616,7 @@ rclc_executor_remove_service(
* \return `RCL_RET_INVALID_ARGUMENT` if any parameter is a null pointer
* \return `RCL_RET_ERROR` if any other error occured
*/
RCLC_PUBLIC
rcl_ret_t
rclc_executor_remove_guard_condition(
rclc_executor_t * executor,
Expand Down
4 changes: 2 additions & 2 deletions rclc/test/rclc/test_executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1706,8 +1706,8 @@ TEST_F(TestDefaultExecutor, spin_period) {
}
// compute avarage time duration between calls to spin_period_callback
uint64_t duration = test_case_evaluate_spin_period();
printf("expected 'spin_period' : %ld ns\n", spin_period);
printf("actual (%d iterations) : %ld ns\n", TC_SPIN_PERIOD_MAX_INVOCATIONS, duration);
printf("expected 'spin_period' : %lu ns\n", spin_period);
printf("actual (%d iterations) : %lu ns\n", TC_SPIN_PERIOD_MAX_INVOCATIONS, duration);

uint64_t delta = 5000000; // 5 ms interval
EXPECT_LE(duration, spin_period + delta);
Expand Down
6 changes: 3 additions & 3 deletions rclc_examples/src/example_parameter_server.c
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
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
7 changes: 6 additions & 1 deletion rclc_parameter/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -65,13 +65,17 @@ ament_target_dependencies(${PROJECT_NAME}
rosidl_runtime_c
)



#################################################
# install
#################################################
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
target_compile_definitions(${PROJECT_NAME}
PRIVATE "RCLC_BUILDING_LIBRARY")
PUBLIC "RCLC_PARAMETER_BUILDING_LIBRARY")
# this alternative would also work
#add_definitions(-DRCLC_PARAMETER_BUILDING_LIBRARY)

install(
TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
Expand Down Expand Up @@ -112,6 +116,7 @@ if(BUILD_TESTING)
target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME})
ament_target_dependencies(${PROJECT_NAME}_test
rclcpp
rclc
rcl
rcutils
rosidl_generator_c
Expand Down
Loading