Skip to content

Commit

Permalink
remove filter expression for status subscription
Browse files Browse the repository at this point in the history 
add a unit test for new rcl api

Signed-off-by: Chen Lihui <[email protected]>
  • Loading branch information
Chen Lihui authored and Chen Lihui committed Mar 23, 2021
1 parent de03363 commit 9dbea2e
Show file tree
Hide file tree
Showing 2 changed files with 126 additions and 72 deletions.
99 changes: 27 additions & 72 deletions rcl_action/src/rcl_action/action_client.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -718,8 +718,6 @@ rcl_ret_t set_content_filtered_topic(
size_t size;
char * feedback_filter = NULL;
char * feedback_filter_update = NULL;
char * status_filter = NULL;
char * status_filter_update = NULL;
rcl_allocator_t allocator = rcl_get_default_allocator();

feedback_filter = rcutils_strdup("", allocator);
Expand All @@ -728,17 +726,10 @@ rcl_ret_t set_content_filtered_topic(
ret = RCL_RET_BAD_ALLOC;
goto clean;
}
status_filter = rcutils_strdup("", allocator);
if (NULL == status_filter) {
RCL_SET_ERROR_MSG("failed to allocate memory for status filter string");
ret = RCL_RET_BAD_ALLOC;
goto clean;
}

rcutils_ret = rcutils_hash_map_get_size(&action_client->impl->goal_uuids, &size);
if (RCUTILS_RET_OK != rcutils_ret) {
RCL_SET_ERROR_MSG("failed to get map size of goal uuid");
ret = RCL_RET_ERROR;
RCL_RET_FROM_RCUTIL_RET(
ret, rcutils_hash_map_get_size(&action_client->impl->goal_uuids, &size));
if (RCL_RET_OK != ret) {
goto clean;
}
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "size: %zu", size);
Expand All @@ -747,66 +738,41 @@ rcl_ret_t set_content_filtered_topic(
goto set_cft;
}

rcutils_ret = rcutils_hash_map_get_next_key_and_data(
&action_client->impl->goal_uuids, NULL, uuid, &uuid_str);
if (RCUTILS_RET_OK != rcutils_ret) {
RCL_SET_ERROR_MSG("failed to get first item from the map of goal uuid");
ret = RCL_RET_ERROR;
RCL_RET_FROM_RCUTIL_RET(
ret, rcutils_hash_map_get_next_key_and_data(
&action_client->impl->goal_uuids, NULL, uuid, &uuid_str));
if (RCL_RET_OK != ret) {
goto clean;
}

feedback_filter_update =
rcutils_format_string(allocator, "goal_id.uuid = &hex(%s)", uuid_str);
if (NULL == feedback_filter_update) {
RCL_SET_ERROR_MSG("failed to fromat string for feedback filter");
RCL_SET_ERROR_MSG("failed to format string for feedback filter");
ret = RCL_RET_BAD_ALLOC;
goto clean;
}
allocator.deallocate(feedback_filter, allocator.state);
feedback_filter = feedback_filter_update;

status_filter_update = rcutils_format_string(
allocator, "status_list[*].goal_info.goal_id.uuid = &hex(%s)", uuid_str);
if (NULL == status_filter_update) {
RCL_SET_ERROR_MSG("failed to fromat string for status filter");
ret = RCL_RET_BAD_ALLOC;
goto clean;
}
allocator.deallocate(status_filter, allocator.state);
status_filter = status_filter_update;

while (true)
while (RCUTILS_RET_OK == (rcutils_ret = rcutils_hash_map_get_next_key_and_data(
&action_client->impl->goal_uuids, uuid, uuid, &uuid_str)))
{
rcutils_ret = rcutils_hash_map_get_next_key_and_data(
&action_client->impl->goal_uuids, uuid, uuid, &uuid_str);
if (RCUTILS_RET_OK != rcutils_ret) {
if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES == rcutils_ret) {
break;
}
ret = RCL_RET_ERROR;
goto clean;
}

feedback_filter_update = rcutils_format_string(
allocator, "%s or goal_id.uuid = &hex(%s)", feedback_filter, uuid_str);
if (NULL == feedback_filter_update) {
RCL_SET_ERROR_MSG("failed to fromat string for feedback filter");
RCL_SET_ERROR_MSG("failed to format string for feedback filter");
ret = RCL_RET_BAD_ALLOC;
goto clean;
}
allocator.deallocate(feedback_filter, allocator.state);
feedback_filter = feedback_filter_update;

status_filter_update = rcutils_format_string(
allocator, "%s or status_list[*].goal_info.goal_id.uuid = &hex(%s)",
status_filter, uuid_str);
if (NULL == status_filter_update) {
RCL_SET_ERROR_MSG("failed to fromat string for feedback filter");
ret = RCL_RET_BAD_ALLOC;
goto clean;
}
allocator.deallocate(status_filter, allocator.state);
status_filter = status_filter_update;
}
if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != rcutils_ret) {
RCL_RET_FROM_RCUTIL_RET(ret, rcutils_ret);
}
if (RCL_RET_OK != ret) {
goto clean;
}

set_cft:
Expand All @@ -818,35 +784,24 @@ rcl_ret_t set_content_filtered_topic(
goto clean;
}

RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "status_filter: %s", status_filter);
// TODO(iuhilnehc-ynos) to find a way to support wildcard filtering expression for array
// ret = rcl_subscription_set_cft_expression_parameters(
// &action_client->impl->status_subscription, status_filter, NULL);
// if (RCL_RET_OK != ret) {
// goto clean;
// }

ret = RCL_RET_OK;

clean:

allocator.deallocate(feedback_filter, allocator.state);
allocator.deallocate(status_filter, allocator.state);
return ret;
}

rcl_ret_t rcl_action_add_goal_uuid(
const rcl_action_client_t * action_client,
const uint8_t * uuid)
{
rcl_ret_t ret;
char * uuid_str = NULL;
rcl_allocator_t allocator = rcl_get_default_allocator();
if (!rcl_action_client_is_valid(action_client)) {
ret = RCL_RET_ACTION_CLIENT_INVALID; /* error already set */
goto end;
return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */
}
RCL_CHECK_ARGUMENT_FOR_NULL(uuid, RCL_RET_INVALID_ARGUMENT);

rcl_ret_t ret;
char * uuid_str = NULL;
rcl_allocator_t allocator = rcl_get_default_allocator();
uuid_str = to_uuid_string(uuid, allocator);
if (NULL == uuid_str) {
RCL_SET_ERROR_MSG("failed to allocate memory for uuid value");
Expand Down Expand Up @@ -888,15 +843,15 @@ rcl_ret_t rcl_action_remove_goal_uuid(
const rcl_action_client_t * action_client,
const uint8_t * uuid)
{
if (!rcl_action_client_is_valid(action_client)) {
return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */
}
RCL_CHECK_ARGUMENT_FOR_NULL(uuid, RCL_RET_INVALID_ARGUMENT);

rcl_ret_t ret;
char * uuid_str = NULL;
char * uuid_value = NULL;
rcl_allocator_t allocator = rcl_get_default_allocator();
if (!rcl_action_client_is_valid(action_client)) {
ret = RCL_RET_ACTION_CLIENT_INVALID; /* error already set */
goto end;
}

uuid_str = to_uuid_string(uuid, allocator);
if (NULL == uuid_str) {
RCL_SET_ERROR_MSG("failed to allocate memory for uuid value");
Expand Down
99 changes: 99 additions & 0 deletions rcl_action/test/rcl_action/test_action_communication.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -652,6 +652,105 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedba
test_msgs__action__Fibonacci_FeedbackMessage__fini(&outgoing_feedback);
}

TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedback_comm_cft)
{
bool is_vendor_support_cft
= (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0);

constexpr size_t size = 2;
test_msgs__action__Fibonacci_FeedbackMessage outgoing_feedback[size];
test_msgs__action__Fibonacci_FeedbackMessage incoming_feedback[size];
for (size_t i = 0; i < size; ++i) {
test_msgs__action__Fibonacci_FeedbackMessage__init(&outgoing_feedback[i]);
test_msgs__action__Fibonacci_FeedbackMessage__init(&incoming_feedback[i]);
}

uint8_t uuid[16];
init_test_uuid0(uuid);
rcl_ret_t ret = rcl_action_add_goal_uuid(&this->action_client, uuid);
if (is_vendor_support_cft) {
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
} else {
EXPECT_EQ(ret, RCL_RET_UNSUPPORTED) << rcl_get_error_string().str;
}

// Initialize feedback
for (size_t i = 0; i < size; ++i) {
ASSERT_TRUE(
rosidl_runtime_c__int32__Sequence__init(
&outgoing_feedback[i].feedback.sequence, 3));
outgoing_feedback[i].feedback.sequence.data[0] = 0;
outgoing_feedback[i].feedback.sequence.data[1] = 1;
outgoing_feedback[i].feedback.sequence.data[2] = 2;
i == 0 ? init_test_uuid0(outgoing_feedback[i].goal_id.uuid)
: init_test_uuid1(outgoing_feedback[i].goal_id.uuid);
}

// Publish feedback with valid arguments
for (size_t i = 0; i < 2; ++i) {
ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback[i]);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;

ret = rcl_wait_set_clear(&this->wait_set);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;

ret = rcl_action_wait_set_add_action_client(
&this->wait_set, &this->action_client, NULL, NULL);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;

ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
if (0 == i || !is_vendor_support_cft) {
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_action_client_wait_set_get_entities_ready(
&this->wait_set,
&this->action_client,
&this->is_feedback_ready,
&this->is_status_ready,
&this->is_goal_response_ready,
&this->is_cancel_response_ready,
&this->is_result_response_ready);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;

EXPECT_TRUE(this->is_feedback_ready);
EXPECT_FALSE(this->is_status_ready);
EXPECT_FALSE(this->is_result_response_ready);
EXPECT_FALSE(this->is_cancel_response_ready);
EXPECT_FALSE(this->is_goal_response_ready);

// Take feedback with valid arguments
ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback[i]);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;

// Check that feedback was received correctly
EXPECT_TRUE(
uuidcmp(
outgoing_feedback[i].goal_id.uuid,
incoming_feedback[i].goal_id.uuid));
ASSERT_EQ(
outgoing_feedback[i].feedback.sequence.size, incoming_feedback[i].feedback.sequence.size);
EXPECT_TRUE(
!memcmp(
outgoing_feedback[i].feedback.sequence.data,
incoming_feedback[i].feedback.sequence.data,
outgoing_feedback[i].feedback.sequence.size));
} else {
EXPECT_EQ(ret, RCL_RET_TIMEOUT) << rcl_get_error_string().str;
}
}

ret = rcl_action_remove_goal_uuid(&this->action_client, uuid);
if (is_vendor_support_cft) {
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
} else {
EXPECT_EQ(ret, RCL_RET_UNSUPPORTED) << rcl_get_error_string().str;
}

for (size_t i = 0; i < size; ++i) {
test_msgs__action__Fibonacci_FeedbackMessage__fini(&outgoing_feedback[i]);
test_msgs__action__Fibonacci_FeedbackMessage__fini(&incoming_feedback[i]);
}
}

TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_request_opts)
{
test_msgs__action__Fibonacci_SendGoal_Request outgoing_goal_request;
Expand Down

0 comments on commit 9dbea2e

Please sign in to comment.