From a3774531e9b7289574501275aafbc3f93120360e Mon Sep 17 00:00:00 2001 From: Jaison Titus Date: Tue, 14 Jan 2020 12:36:12 -0500 Subject: [PATCH] Adding required structs and methods to get a list of publishers or subscribers with their respective qos (#186) Signed-off-by: Jaison Titus Signed-off-by: Miaofei --- rmw/CMakeLists.txt | 4 +- rmw/include/rmw/get_topic_endpoint_info.h | 128 ++++++++++ rmw/include/rmw/qos_profiles.h | 13 ++ rmw/include/rmw/rmw.h | 1 + rmw/include/rmw/topic_endpoint_info.h | 192 +++++++++++++++ rmw/include/rmw/topic_endpoint_info_array.h | 108 +++++++++ rmw/include/rmw/types.h | 7 + rmw/package.xml | 1 + rmw/src/topic_endpoint_info.c | 233 +++++++++++++++++++ rmw/src/topic_endpoint_info_array.c | 99 ++++++++ rmw/test/CMakeLists.txt | 21 ++ rmw/test/test_topic_endpoint_info.cpp | 245 ++++++++++++++++++++ rmw/test/test_topic_endpoint_info_array.cpp | 65 ++++++ 13 files changed, 1116 insertions(+), 1 deletion(-) create mode 100644 rmw/include/rmw/get_topic_endpoint_info.h create mode 100644 rmw/include/rmw/topic_endpoint_info.h create mode 100644 rmw/include/rmw/topic_endpoint_info_array.h create mode 100644 rmw/src/topic_endpoint_info.c create mode 100644 rmw/src/topic_endpoint_info_array.c create mode 100644 rmw/test/test_topic_endpoint_info.cpp create mode 100644 rmw/test/test_topic_endpoint_info_array.cpp diff --git a/rmw/CMakeLists.txt b/rmw/CMakeLists.txt index 8aa54068..035074cf 100644 --- a/rmw/CMakeLists.txt +++ b/rmw/CMakeLists.txt @@ -32,10 +32,12 @@ set(rmw_sources "src/init_options.c" "src/loaned_message_sequence.c" "src/names_and_types.c" + "src/node_security_options.c" "src/publisher_options.c" "src/sanity_checks.c" "src/subscription_options.c" - "src/node_security_options.c" + "src/topic_endpoint_info_array.c" + "src/topic_endpoint_info.c" "src/validate_full_topic_name.c" "src/validate_namespace.c" "src/validate_node_name.c" diff --git a/rmw/include/rmw/get_topic_endpoint_info.h b/rmw/include/rmw/get_topic_endpoint_info.h new file mode 100644 index 00000000..efb5b42f --- /dev/null +++ b/rmw/include/rmw/get_topic_endpoint_info.h @@ -0,0 +1,128 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW__GET_TOPIC_ENDPOINT_INFO_H_ +#define RMW__GET_TOPIC_ENDPOINT_INFO_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/visibility_control.h" + +/// Retrieve the information for all publishers to a given topic. +/** + * The retrieved information will contain the publisher's node name, node namespace, + * associated topic type, publisher gid and qos profile. + * + * The node parameter must not be `NULL` and must point to a valid node. + * + * The topic_name parameter must not be `NULL` and must follow the topic naming rules + * mentioned at http://design.ros2.org/articles/topic_and_service_names.html + * Names of non-existent topics are allowed. + * In that case, this function will return an empty array. + * + * It is the responsibility of the caller to ensure that `publishers_info` parameter points + * to a valid struct of type rmw_topic_endpoint_info_array_t. + * The rmw_topic_endpoint_info_array_t struct must be zero initialized. + * \see rmw_get_zero_initialized_topic_endpoint_info_array + * + * The `allocator` will be used to allocate memory to the `info_array` member + * inside of `publishers_info`. + * Moreover, every `const char *` member inside of + * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory. + * \see rmw_topic_endpoint_info_set_topic_type + * \see rmw_topic_endpoint_info_set_node_name + * \see rmw_topic_endpoint_info_set_node_namespace + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `publishers_info` to avoid leaking memory. + * \see rmw_topic_endpoint_info_array_fini + * + * \param[in] node the handle to the node being used to query the ROS graph. + * \param[in] allocator the allocator to be used when allocating space for the array. + * \param[in] topic_name the name of the topic for which the list of publishers will be retrieved. + * \param[in] no_mangle if true, the topic name will not be mangled. + * \param[out] publishers_info an array of rmw_topic_endpoint_info_t. + * \return `RMW_RET_OK` if the query was successful, or + * \return `RMW_RET_INVALID_ARGUMENT` if the node is invalid, or + * \return `RMW_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RMW_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RMW_RET_ERROR` if an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_get_publishers_info_by_topic( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * publishers_info); + +/// Retrieve the information for all subscriptions to a given topic. +/** + * The retrieved information will contain the subscriptions's node name, node namespace, + * associated topic type, subscription gid and qos profile. + * + * The node parameter must not be `NULL` and must point to a valid node. + * + * The topic_name parameter must not be `NULL` and must follow the topic naming rules + * mentioned at http://design.ros2.org/articles/topic_and_service_names.html + * Names of non-existent topics are allowed. + * They will return an empty array. + * + * It is the responsibility of the caller to ensure that `subscriptions_info` parameter points + * to a valid struct of type rmw_topic_endpoint_info_array_t. + * The rmw_topic_endpoint_info_array_t struct must be zero initialized. + * \see rmw_get_zero_initialized_topic_endpoint_info_array + * + * The `allocator` will be used to allocate memory to the `info_array` member + * inside of `publishers_info`. + * Moreover, every `const char *` member inside of + * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory. + * \see rmw_topic_endpoint_info_set_topic_type + * \see rmw_topic_endpoint_info_set_node_name + * \see rmw_topic_endpoint_info_set_node_namespace + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `publishers_info` to avoid leaking memory. + * \see rmw_topic_endpoint_info_array_fini + * + * \param[in] node the handle to the node being used to query the ROS graph. + * \param[in] allocator the allocator to be used when allocating space for the array. + * \param[in] topic_name the name of the topic for which the list of subscriptions will be retrieved. + * \param[in] no_mangle if true, the topic name will not be mangled. + * \param[out] subscriptions_info an array of rmw_topic_endpoint_info_t.. + * \return `RMW_RET_OK` if the query was successful, or + * \return `RMW_RET_INVALID_ARGUMENT` if the node is invalid, or + * \return `RMW_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RMW_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RMW_RET_ERROR` if an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_get_subscriptions_info_by_topic( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * subscriptions_info); + +#ifdef __cplusplus +} +#endif + +#endif // RMW__GET_TOPIC_ENDPOINT_INFO_H_ diff --git a/rmw/include/rmw/qos_profiles.h b/rmw/include/rmw/qos_profiles.h index 90038307..0e39c6f2 100644 --- a/rmw/include/rmw/qos_profiles.h +++ b/rmw/include/rmw/qos_profiles.h @@ -100,6 +100,19 @@ static const rmw_qos_profile_t rmw_qos_profile_system_default = false }; +static const rmw_qos_profile_t rmw_qos_profile_unknown = +{ + RMW_QOS_POLICY_HISTORY_UNKNOWN, + RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT, + RMW_QOS_POLICY_RELIABILITY_UNKNOWN, + RMW_QOS_POLICY_DURABILITY_UNKNOWN, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, + RMW_QOS_POLICY_LIVELINESS_UNKNOWN, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false +}; + #ifdef __cplusplus } #endif diff --git a/rmw/include/rmw/rmw.h b/rmw/include/rmw/rmw.h index ce8ab8f8..2fb995f1 100644 --- a/rmw/include/rmw/rmw.h +++ b/rmw/include/rmw/rmw.h @@ -55,6 +55,7 @@ * - rmw_get_service_names_and_types() * - rmw/names_and_types.h * - rmw/get_topic_names_and_types.h + * - rmw/get_topic_endpoint_info.h * - rmw/get_service_names_and_types.h * * Further still there are some useful abstractions and utilities: diff --git a/rmw/include/rmw/topic_endpoint_info.h b/rmw/include/rmw/topic_endpoint_info.h new file mode 100644 index 00000000..b6f5ffcd --- /dev/null +++ b/rmw/include/rmw/topic_endpoint_info.h @@ -0,0 +1,192 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW__TOPIC_ENDPOINT_INFO_H_ +#define RMW__TOPIC_ENDPOINT_INFO_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/allocator.h" +#include "rmw/types.h" +#include "rmw/visibility_control.h" + +/// A structure that encapsulates the name, namespace, topic_type, gid and qos_profile +/// of publishers and subscriptions for a topic. +typedef struct RMW_PUBLIC_TYPE rmw_topic_endpoint_info_t +{ + /// Name of the node + const char * node_name; + /// Namespace of the node + const char * node_namespace; + /// The associated topic type + const char * topic_type; + /// The endpoint type + rmw_endpoint_type_t endpoint_type; + /// The GID of the endpoint + uint8_t endpoint_gid[RMW_GID_STORAGE_SIZE]; + /// QoS profile of the endpoint + rmw_qos_profile_t qos_profile; +} rmw_topic_endpoint_info_t; + +/// Return a rmw_topic_endpoint_info_t struct with members initialized to `NULL`. +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_topic_endpoint_info_t +rmw_get_zero_initialized_topic_endpoint_info(void); + +/// Finalize a rmw_topic_endpoint_info_t object. +/** + * The rmw_topic_endpoint_info_t struct has members which require memory to be allocated to them before + * setting values. + * This function reclaims any allocated resources within the object and zeroes out all other + * members. + * + * \param[inout] topic_endpoint_info object to be finalized + * \param[in] allocator the allocator used to allocate memory to the object + * \returns `RMW_RET_OK` on successfully reclaiming memory, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_fini( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rcutils_allocator_t * allocator); + +/// Set the topic_type in rmw_topic_endpoint_info_t. +/** + * rmw_topic_endpoint_info_t has a member topic_type of type const char *; + * this function allocates memory and copies the value of param passed to it. + * + * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t + * \param[in] topic_type the topic_type value to set in rmw_topic_endpoint_info_t + * \param[in] allocator the allocator that will be used to allocate memory + * \returns `RMW_RET_OK` on successfully setting the topic_type, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_topic_type( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * topic_type, + rcutils_allocator_t * allocator); + +/// Set the node_name in rmw_topic_endpoint_info_t. +/** + * rmw_topic_endpoint_info_t has a member node_name of type const char *; + * this function allocates memory and copies the value of param passed to it. + * + * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t + * \param[in] node_name the node_name value to set in rmw_topic_endpoint_info_t + * \param[in] allocator the allocator that will be used to allocate memory + * \returns `RMW_RET_OK` on successfully setting the node_name, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_node_name( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * node_name, + rcutils_allocator_t * allocator); + +/// Set the node_namespace in rmw_topic_endpoint_info_t. +/** + * rmw_topic_endpoint_info_t has a member node_namespace of type const char *; + * this function allocates memory and copies the value of param passed to it. + * + * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t + * \param[in] node_namespace the node_namespace value to set in rmw_topic_endpoint_info_t + * \param[in] allocator the allocator that will be used to allocate memory + * \returns `RMW_RET_OK` on successfully setting the node_namespace, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_node_namespace( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * node_namespace, + rcutils_allocator_t * allocator); + +/// Set the gid in rmw_topic_endpoint_info_t. +/** + * Copies the values from gid into the gid member inside topic_endpoint_info. + * + * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t + * \param[in] gid the gid value to set in rmw_topic_endpoint_info_t + * \param[in] size the size of the gid param + * \returns `RMW_RET_OK` on successfully setting the gid, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` size is greater than RMW_GID_STORAGE_SIZE, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_endpoint_type( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rmw_endpoint_type_t type); + +/// Set the gid in rmw_topic_endpoint_info_t. +/** + * Copies the values from gid into the gid member inside topic_endpoint_info. + * + * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t + * \param[in] gid the gid value to set in rmw_topic_endpoint_info_t + * \param[in] size the size of the gid param + * \returns `RMW_RET_OK` on successfully setting the gid, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` size is greater than RMW_GID_STORAGE_SIZE, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_gid( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const uint8_t gid[], + size_t size); + +/// Set the qos_profile in rmw_topic_endpoint_info_t. +/** + * rmw_topic_endpoint_info_t has a member qos_profile of type const rmw_qos_profile_t *. + * This function assigns the passed qos_profile pointer to the member. + * + * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t + * \param[in] qos_profile the qos_profile to set in rmw_topic_endpoint_info_t + * \returns `RMW_RET_OK` on successfully setting the qos_profile, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_qos_profile( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const rmw_qos_profile_t * qos_profile); + +#ifdef __cplusplus +} +#endif + +#endif // RMW__TOPIC_ENDPOINT_INFO_H_ diff --git a/rmw/include/rmw/topic_endpoint_info_array.h b/rmw/include/rmw/topic_endpoint_info_array.h new file mode 100644 index 00000000..e736b811 --- /dev/null +++ b/rmw/include/rmw/topic_endpoint_info_array.h @@ -0,0 +1,108 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW__TOPIC_ENDPOINT_INFO_ARRAY_H_ +#define RMW__TOPIC_ENDPOINT_INFO_ARRAY_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/allocator.h" +#include "rmw/topic_endpoint_info.h" +#include "rmw/visibility_control.h" + +/// Array of rmw_topic_endpoint_info_t +typedef struct RMW_PUBLIC_TYPE rmw_topic_endpoint_info_array_t +{ + /// Size of the array. + size_t count; + /// Pointer representing an array of rmw_topic_endpoint_info_t + rmw_topic_endpoint_info_t * info_array; +} rmw_topic_endpoint_info_array_t; + +/// Return a rmw_topic_endpoint_info_array_t struct with members initialized to `NULL`. +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_topic_endpoint_info_array_t +rmw_get_zero_initialized_topic_endpoint_info_array(void); + +/// Check that a rmw_topic_endpoint_info_array_t struct is zero initialized. +/** + * This function checks if the provided rmw_topic_endpoint_info_array_t is zero initialized or not. + * + * \param[in] topic_endpoint_info_array the data structure to be checked + * \returns `RMW_RET_OK` if topic_endpoint_info_array is zero initialized + * \returns `RMW_RET_INVALID_ARGUMENT` if the parameter is NULL, or + * \returns `RMW_RET_ERROR` if topic_endpoint_info_array is not zero initialized + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_array_check_zero( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array); + +/// Initialize the info_array member inside rmw_topic_endpoint_info_array_t with the given size +/** + * The rmw_topic_endpoint_info_array_t has a member variable info_array which is an array of + * type rmw_topic_endpoint_info_t. + * This function allocates memory to this array to hold n elements, + * where n is the value of the size param to this function. + * + * topic_endpoint_info_array must be zero initialized before being passed into this function. + * + * \param[inout] topic_endpoint_info_array the data structure to initialise + * \param[in] size the size of the array + * \param[in] allocator the allocator to be used to allocate space + * \returns `RMW_RET_OK` on successful init, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any of the parameters are NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if topic_endpoint_info_array is not zero initialized, or + * \returns `RMW_BAD_ALLOC` if memory allocation fails, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_array_init_with_size( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, + size_t size, + rcutils_allocator_t * allocator); + +/// Finalize a rmw_topic_endpoint_info_array_t object. +/** + * The info_array member variable inside of rmw_topic_endpoint_info_array represents an array of + * rmw_topic_endpoint_info_t. + * When initializing this array, memory is allocated for it using the allocator. + * This function reclaims any allocated resources within the object and also sets the value of count + * to 0. + * + * \param[inout] topic_endpoint_info_array object to be finalized + * \param[in] allocator the allocator used to allocate memory to the object + * \returns `RMW_RET_OK` on successfully reclaiming memory, or + * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_array_fini( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, + rcutils_allocator_t * allocator); + +#ifdef __cplusplus +} +#endif + +#endif // RMW__TOPIC_ENDPOINT_INFO_ARRAY_H_ diff --git a/rmw/include/rmw/types.h b/rmw/include/rmw/types.h index c68e00f7..cbbe5dd8 100644 --- a/rmw/include/rmw/types.h +++ b/rmw/include/rmw/types.h @@ -46,6 +46,13 @@ typedef struct RMW_PUBLIC_TYPE rmw_node_t rmw_context_t * context; } rmw_node_t; +typedef enum RMW_PUBLIC_TYPE +{ + RMW_ENDPOINT_INVALID = 0, + RMW_ENDPOINT_PUBLISHER, + RMW_ENDPOINT_SUBSCRIPTION +} rmw_endpoint_type_t; + /// Options that can be used to configure the creation of a publisher in rmw. typedef struct RMW_PUBLIC_TYPE rmw_publisher_options_t { diff --git a/rmw/package.xml b/rmw/package.xml index 50d89b10..2fa96db1 100644 --- a/rmw/package.xml +++ b/rmw/package.xml @@ -20,6 +20,7 @@ ament_cmake_gmock ament_lint_auto ament_lint_common + osrf_testing_tools_cpp ament_cmake diff --git a/rmw/src/topic_endpoint_info.c b/rmw/src/topic_endpoint_info.c new file mode 100644 index 00000000..d09dcbde --- /dev/null +++ b/rmw/src/topic_endpoint_info.c @@ -0,0 +1,233 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/topic_endpoint_info.h" + +#include "rcutils/strdup.h" +#include "rmw/error_handling.h" +#include "rmw/types.h" + +rmw_topic_endpoint_info_t +rmw_get_zero_initialized_topic_endpoint_info(void) +{ +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wmissing-field-initializers" +#endif + rmw_topic_endpoint_info_t zero = {0}; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + return zero; +} + +rmw_ret_t +_rmw_topic_endpoint_info_fini_str( + const char ** topic_endpoint_info_str, + rcutils_allocator_t * allocator) +{ + allocator->deallocate((char *) *topic_endpoint_info_str, allocator->state); + *topic_endpoint_info_str = NULL; + return RMW_RET_OK; +} + +rmw_ret_t +_rmw_topic_endpoint_info_fini_node_name( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rcutils_allocator_t * allocator) +{ + if (!topic_endpoint_info->node_name) { + RMW_SET_ERROR_MSG("topic_endpoint_info->node_name is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_endpoint_info_fini_str(&topic_endpoint_info->node_name, allocator); +} + +rmw_ret_t +_rmw_topic_endpoint_info_fini_node_namespace( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rcutils_allocator_t * allocator) +{ + if (!topic_endpoint_info->node_namespace) { + RMW_SET_ERROR_MSG("topic_endpoint_info->node_namespace is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_endpoint_info_fini_str(&topic_endpoint_info->node_namespace, allocator); +} + +rmw_ret_t +_rmw_topic_endpoint_info_fini_topic_type( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rcutils_allocator_t * allocator) +{ + if (!topic_endpoint_info->topic_type) { + RMW_SET_ERROR_MSG("topic_endpoint_info->topic_type is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_endpoint_info_fini_str(&topic_endpoint_info->topic_type, allocator); +} + +rmw_ret_t +rmw_topic_endpoint_info_fini( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rcutils_allocator_t * allocator) +{ + if (!topic_endpoint_info) { + RMW_SET_ERROR_MSG("topic_endpoint_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + rmw_ret_t ret; + ret = _rmw_topic_endpoint_info_fini_node_name(topic_endpoint_info, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + ret = _rmw_topic_endpoint_info_fini_node_namespace(topic_endpoint_info, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + ret = _rmw_topic_endpoint_info_fini_topic_type(topic_endpoint_info, allocator); + if (ret != RMW_RET_OK) { + return ret; + } + + *topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + + return RMW_RET_OK; +} + +rmw_ret_t +_rmw_topic_endpoint_info_copy_str( + const char ** topic_endpoint_info_str, + const char * str, + rcutils_allocator_t * allocator) +{ + if (!str) { + RMW_SET_ERROR_MSG("str is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!topic_endpoint_info_str) { + RMW_SET_ERROR_MSG("topic_endpoint_info_str is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + *topic_endpoint_info_str = rcutils_strdup(str, *allocator); + + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_endpoint_info_set_topic_type( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * topic_type, + rcutils_allocator_t * allocator) +{ + if (!topic_endpoint_info) { + RMW_SET_ERROR_MSG("topic_endpoint_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_endpoint_info_copy_str(&topic_endpoint_info->topic_type, topic_type, allocator); +} + +rmw_ret_t +rmw_topic_endpoint_info_set_node_name( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * node_name, + rcutils_allocator_t * allocator) +{ + if (!topic_endpoint_info) { + RMW_SET_ERROR_MSG("topic_endpoint_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_endpoint_info_copy_str(&topic_endpoint_info->node_name, node_name, allocator); +} + +rmw_ret_t +rmw_topic_endpoint_info_set_node_namespace( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * node_namespace, + rcutils_allocator_t * allocator) +{ + if (!topic_endpoint_info) { + RMW_SET_ERROR_MSG("topic_endpoint_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + return _rmw_topic_endpoint_info_copy_str( + &topic_endpoint_info->node_namespace, + node_namespace, + allocator); +} + +rmw_ret_t +rmw_topic_endpoint_info_set_endpoint_type( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rmw_endpoint_type_t type) +{ + if (!topic_endpoint_info) { + RMW_SET_ERROR_MSG("topic_endpoint_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + topic_endpoint_info->endpoint_type = type; + + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_endpoint_info_set_gid( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const uint8_t gid[], + size_t size) +{ + if (!topic_endpoint_info) { + RMW_SET_ERROR_MSG("topic_endpoint_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + if (size > RMW_GID_STORAGE_SIZE) { + RMW_SET_ERROR_MSG("size is more than RMW_GID_STORAGE_SIZE"); + return RMW_RET_INVALID_ARGUMENT; + } + memset(topic_endpoint_info->endpoint_gid, 0, RMW_GID_STORAGE_SIZE); + memcpy(topic_endpoint_info->endpoint_gid, gid, size); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_endpoint_info_set_qos_profile( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const rmw_qos_profile_t * qos_profile) +{ + if (!topic_endpoint_info) { + RMW_SET_ERROR_MSG("topic_endpoint_info is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!qos_profile) { + RMW_SET_ERROR_MSG("qos_profile is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + topic_endpoint_info->qos_profile = *qos_profile; + return RMW_RET_OK; +} diff --git a/rmw/src/topic_endpoint_info_array.c b/rmw/src/topic_endpoint_info_array.c new file mode 100644 index 00000000..aa6f4b2a --- /dev/null +++ b/rmw/src/topic_endpoint_info_array.c @@ -0,0 +1,99 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/error_handling.h" +#include "rmw/types.h" + +rmw_topic_endpoint_info_array_t +rmw_get_zero_initialized_topic_endpoint_info_array(void) +{ +#ifdef __clang__ +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wmissing-field-initializers" +#endif + rmw_topic_endpoint_info_array_t zero = {0}; +#ifdef __clang__ +# pragma clang diagnostic pop +#endif + return zero; +} + +rmw_ret_t +rmw_topic_endpoint_info_array_check_zero( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array) +{ + if (!topic_endpoint_info_array) { + RMW_SET_ERROR_MSG("topic_endpoint_info_array is null"); + return RMW_RET_INVALID_ARGUMENT; + } + if (topic_endpoint_info_array->count != 0 || topic_endpoint_info_array->info_array != NULL) { + RMW_SET_ERROR_MSG("topic_endpoint_info_array is not zeroed"); + return RMW_RET_ERROR; + } + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_endpoint_info_array_init_with_size( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, + size_t size, + rcutils_allocator_t * allocator) +{ + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + if (!topic_endpoint_info_array) { + RMW_SET_ERROR_MSG("topic_endpoint_info_array is null"); + return RMW_RET_INVALID_ARGUMENT; + } + topic_endpoint_info_array->info_array = + allocator->allocate(sizeof(*topic_endpoint_info_array->info_array) * size, allocator->state); + if (!topic_endpoint_info_array->info_array) { + RMW_SET_ERROR_MSG("failed to allocate memory for info_array"); + return RMW_RET_BAD_ALLOC; + } + return RMW_RET_OK; +} + +rmw_ret_t +rmw_topic_endpoint_info_array_fini( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, + rcutils_allocator_t * allocator) +{ + if (!allocator) { + RMW_SET_ERROR_MSG("allocator is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (!topic_endpoint_info_array) { + RMW_SET_ERROR_MSG("topic_endpoint_info_array is null"); + return RMW_RET_INVALID_ARGUMENT; + } + + rmw_ret_t ret; + // free all const char * inside the topic_endpoint_info_t + for (size_t i = 0u; i < topic_endpoint_info_array->count; i++) { + ret = rmw_topic_endpoint_info_fini(&topic_endpoint_info_array->info_array[i], allocator); + if (ret != RMW_RET_OK) { + return ret; + } + } + + allocator->deallocate(topic_endpoint_info_array->info_array, allocator->state); + topic_endpoint_info_array->info_array = NULL; + topic_endpoint_info_array->count = 0; + return RMW_RET_OK; +} diff --git a/rmw/test/CMakeLists.txt b/rmw/test/CMakeLists.txt index c6a12710..cba7748e 100644 --- a/rmw/test/CMakeLists.txt +++ b/rmw/test/CMakeLists.txt @@ -1,4 +1,5 @@ find_package(ament_cmake_gmock REQUIRED) +find_package(osrf_testing_tools_cpp REQUIRED) ament_add_gmock(test_serialized_message test_serialized_message.cpp @@ -41,3 +42,23 @@ if(TARGET test_validate_namespace) set_target_properties(test_validate_namespace PROPERTIES COMPILE_FLAGS "-std=c++14") endif() endif() + +ament_add_gmock(test_topic_endpoint_info_array + test_topic_endpoint_info_array.cpp + # Append the directory of librmw so it is found at test time. + APPEND_LIBRARY_DIRS "$" +) +if(TARGET test_topic_endpoint_info_array) + target_link_libraries(test_topic_endpoint_info_array ${PROJECT_NAME} + osrf_testing_tools_cpp::memory_tools) +endif() + +ament_add_gmock(test_topic_endpoint_info + test_topic_endpoint_info.cpp + # Append the directory of librmw so it is found at test time. + APPEND_LIBRARY_DIRS "$" +) +if(TARGET test_topic_endpoint_info) + target_link_libraries(test_topic_endpoint_info ${PROJECT_NAME} + osrf_testing_tools_cpp::memory_tools) +endif() diff --git a/rmw/test/test_topic_endpoint_info.cpp b/rmw/test/test_topic_endpoint_info.cpp new file mode 100644 index 00000000..c1fe0166 --- /dev/null +++ b/rmw/test/test_topic_endpoint_info.cpp @@ -0,0 +1,245 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gmock/gmock.h" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcutils/allocator.h" + +#include "rmw/topic_endpoint_info.h" +#include "rmw/types.h" + +char * get_mallocd_string(const char * s) +{ + size_t size = strlen(s) + 1; + char * c = reinterpret_cast(malloc(size)); + memcpy(c, s, size); + return c; +} + +TEST(test_topic_endpoint_info, set_topic_type) { + rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * val = get_mallocd_string("test_topic_type"); + rmw_ret_t ret = rmw_topic_endpoint_info_set_topic_type(&topic_endpoint_info, val, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; + ret = rmw_topic_endpoint_info_set_topic_type(&topic_endpoint_info, nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null topic_type"; + ret = rmw_topic_endpoint_info_set_topic_type(nullptr, val, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << + "Expected invalid argument for null topic_endpoint_info"; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + allocator.deallocate(const_cast(topic_endpoint_info.topic_type), allocator.state); + }); + ret = rmw_topic_endpoint_info_set_topic_type(&topic_endpoint_info, val, &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid arguments"; + // free the mallocd string and verify that the string was copied by value + free(val); + EXPECT_STREQ(topic_endpoint_info.topic_type, + "test_topic_type") << "Topic Type value is not as expected"; +} + +TEST(test_topic_endpoint_info, set_node_name) { + rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * val = get_mallocd_string("test_node_name"); + rmw_ret_t ret = rmw_topic_endpoint_info_set_node_name(&topic_endpoint_info, val, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; + ret = rmw_topic_endpoint_info_set_node_name(&topic_endpoint_info, nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null node_name"; + ret = rmw_topic_endpoint_info_set_node_name(nullptr, val, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << + "Expected invalid argument for null topic_endpoint_info"; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + allocator.deallocate(const_cast(topic_endpoint_info.node_name), allocator.state); + }); + ret = rmw_topic_endpoint_info_set_node_name(&topic_endpoint_info, val, &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid arguments"; + // free the mallocd string and verify that the string was copied by value + free(val); + EXPECT_STREQ(topic_endpoint_info.node_name, "test_node_name") << + "Node name value is not as expected"; +} + +TEST(test_topic_endpoint_info, set_node_namespace) { + rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * val = get_mallocd_string("test_node_namespace"); + rmw_ret_t ret = rmw_topic_endpoint_info_set_node_namespace(&topic_endpoint_info, val, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; + ret = rmw_topic_endpoint_info_set_node_namespace(&topic_endpoint_info, nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null node_namespace"; + ret = rmw_topic_endpoint_info_set_node_namespace(nullptr, val, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << + "Expected invalid argument for null topic_endpoint_info"; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + allocator.deallocate(const_cast(topic_endpoint_info.node_namespace), allocator.state); + }); + ret = rmw_topic_endpoint_info_set_node_namespace(&topic_endpoint_info, val, &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid arguments"; + // free the mallocd string and verify that the string was copied by value + free(val); + EXPECT_STREQ(topic_endpoint_info.node_namespace, "test_node_namespace") << + "Node namespace value is not as expected"; +} + +TEST(test_topic_endpoint_info, set_gid) { + rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + uint8_t gid[RMW_GID_STORAGE_SIZE]; + for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + gid[i] = i; + } + rmw_ret_t ret = rmw_topic_endpoint_info_set_gid(nullptr, gid, RMW_GID_STORAGE_SIZE); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << + "Expected invalid argument for null topic_endpoint_info"; + ret = rmw_topic_endpoint_info_set_gid(&topic_endpoint_info, gid, RMW_GID_STORAGE_SIZE + 1); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << + "Expected invalid argument for size greater than RMW_GID_STORAGE_SIZE"; + ret = rmw_topic_endpoint_info_set_gid(&topic_endpoint_info, gid, RMW_GID_STORAGE_SIZE); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid arguments"; + for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + EXPECT_EQ(topic_endpoint_info.endpoint_gid[i], gid[i]) << "Gid value is not as expected"; + } +} + +TEST(test_topic_endpoint_info, set_qos_profile) { + rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rmw_qos_profile_t qos_profile; + qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos_profile.depth = 0; + qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + qos_profile.deadline = {1, 0}; + qos_profile.lifespan = {2, 0}; + qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; + qos_profile.liveliness_lease_duration = {3, 0}; + qos_profile.avoid_ros_namespace_conventions = false; + + rmw_ret_t ret = rmw_topic_endpoint_info_set_qos_profile(nullptr, &qos_profile); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << + "Expected invalid argument for null topic_endpoint_info"; + ret = rmw_topic_endpoint_info_set_qos_profile(&topic_endpoint_info, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null qos_profile"; + ret = rmw_topic_endpoint_info_set_qos_profile(&topic_endpoint_info, &qos_profile); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid arguments"; + + EXPECT_EQ(topic_endpoint_info.qos_profile.history, RMW_QOS_POLICY_HISTORY_KEEP_LAST) << + "Unequal history"; + EXPECT_EQ(topic_endpoint_info.qos_profile.depth, 0u) << "Unequal depth"; + EXPECT_EQ(topic_endpoint_info.qos_profile.reliability, + RMW_QOS_POLICY_RELIABILITY_RELIABLE) << "Unequal reliability"; + EXPECT_EQ(topic_endpoint_info.qos_profile.durability, + RMW_QOS_POLICY_DURABILITY_VOLATILE) << "Unequal durability"; + EXPECT_EQ(topic_endpoint_info.qos_profile.deadline.sec, 1u) << "Unequal deadline sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.deadline.nsec, 0u) << "Unequal deadline nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.lifespan.sec, 2u) << "Unequal lifespan sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.lifespan.nsec, 0u) << "Unequal lifespan nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness, + RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) << "Unequal liveliness"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness_lease_duration.sec, + 3u) << "Unequal liveliness lease duration sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness_lease_duration.nsec, + 0u) << "Unequal liveliness lease duration nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.avoid_ros_namespace_conventions, + false) << "Unequal avoid namespace conventions"; +} + +TEST(test_topic_endpoint_info, zero_init) { + rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + EXPECT_FALSE(topic_endpoint_info.node_name); + EXPECT_FALSE(topic_endpoint_info.node_namespace); + EXPECT_FALSE(topic_endpoint_info.topic_type); + EXPECT_EQ(topic_endpoint_info.endpoint_type, RMW_ENDPOINT_INVALID) << + "Endpoint type value should be invalid"; + for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + EXPECT_EQ(topic_endpoint_info.endpoint_gid[i], 0) << "Gid value should be 0"; + } + EXPECT_EQ(topic_endpoint_info.qos_profile.history, 0) << "Non-zero history"; + EXPECT_EQ(topic_endpoint_info.qos_profile.depth, 0u) << "Non-zero depth"; + EXPECT_EQ(topic_endpoint_info.qos_profile.reliability, 0) << "Non-zero reliability"; + EXPECT_EQ(topic_endpoint_info.qos_profile.durability, 0) << "Non-zero durability"; + EXPECT_EQ(topic_endpoint_info.qos_profile.deadline.sec, 0u) << "Non-zero deadline sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.deadline.nsec, 0u) << "Non-zero deadline nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.lifespan.sec, 0u) << "Non-zero lifespan sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.lifespan.nsec, 0u) << "Non-zero lifespan nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness, 0) << "Non-zero liveliness"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness_lease_duration.sec, + 0u) << "Non-zero liveliness lease duration sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness_lease_duration.nsec, + 0u) << "Non-zero liveliness lease duration nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.avoid_ros_namespace_conventions, + false) << "Non-zero avoid namespace conventions"; +} + +TEST(test_topic_endpoint_info, fini) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_topic_endpoint_info_t topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); + rmw_qos_profile_t qos_profile; + qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos_profile.depth = 0; + qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + qos_profile.deadline = {1, 0}; + qos_profile.lifespan = {2, 0}; + qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; + qos_profile.liveliness_lease_duration = {3, 0}; + qos_profile.avoid_ros_namespace_conventions = false; + rmw_ret_t ret = rmw_topic_endpoint_info_set_qos_profile(&topic_endpoint_info, &qos_profile); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid arguments"; + uint8_t gid[RMW_GID_STORAGE_SIZE]; + for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + gid[i] = i * 12; + } + ret = rmw_topic_endpoint_info_set_endpoint_type(&topic_endpoint_info, RMW_ENDPOINT_PUBLISHER); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid rmw_endpoint_type_t arguments"; + ret = rmw_topic_endpoint_info_set_gid(&topic_endpoint_info, gid, RMW_GID_STORAGE_SIZE); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid gid arguments"; + ret = rmw_topic_endpoint_info_set_node_namespace(&topic_endpoint_info, "namespace", &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid node_namespace arguments"; + ret = rmw_topic_endpoint_info_set_node_name(&topic_endpoint_info, "name", &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid node_name arguments"; + ret = rmw_topic_endpoint_info_set_topic_type(&topic_endpoint_info, "type", &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid topic_type arguments"; + ret = rmw_topic_endpoint_info_fini(&topic_endpoint_info, nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << "Expected invalid argument for null allocator"; + ret = rmw_topic_endpoint_info_fini(nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << + "Expected invalid argument for null topic_endpoint_info"; + + ret = rmw_topic_endpoint_info_fini(&topic_endpoint_info, &allocator); + // Verify that the values inside the struct are zero-ed out and finished. + EXPECT_EQ(ret, RMW_RET_OK) << "Expected OK for valid fini arguments"; + EXPECT_FALSE(topic_endpoint_info.node_name); + EXPECT_FALSE(topic_endpoint_info.node_namespace); + EXPECT_FALSE(topic_endpoint_info.topic_type); + EXPECT_EQ(topic_endpoint_info.endpoint_type, + RMW_ENDPOINT_INVALID) << "Endpoint type value should be invalid"; + for (uint8_t i = 0; i < RMW_GID_STORAGE_SIZE; i++) { + EXPECT_EQ(topic_endpoint_info.endpoint_gid[i], 0) << "Gid value should be 0"; + } + EXPECT_EQ(topic_endpoint_info.qos_profile.history, 0) << "Non-zero history"; + EXPECT_EQ(topic_endpoint_info.qos_profile.depth, 0u) << "Non-zero depth"; + EXPECT_EQ(topic_endpoint_info.qos_profile.reliability, 0) << "Non-zero reliability"; + EXPECT_EQ(topic_endpoint_info.qos_profile.durability, 0) << "Non-zero durability"; + EXPECT_EQ(topic_endpoint_info.qos_profile.deadline.sec, 0u) << "Non-zero deadline sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.deadline.nsec, 0u) << "Non-zero deadline nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.lifespan.sec, 0u) << "Non-zero lifespan sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.lifespan.nsec, 0u) << "Non-zero lifespan nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness, 0) << "Non-zero liveliness"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness_lease_duration.sec, 0u) << + "Non-zero liveliness lease duration sec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.liveliness_lease_duration.nsec, 0u) << + "Non-zero liveliness lease duration nsec"; + EXPECT_EQ(topic_endpoint_info.qos_profile.avoid_ros_namespace_conventions, false) << + "Non-zero avoid namespace conventions"; +} diff --git a/rmw/test/test_topic_endpoint_info_array.cpp b/rmw/test/test_topic_endpoint_info_array.cpp new file mode 100644 index 00000000..eb4f80d4 --- /dev/null +++ b/rmw/test/test_topic_endpoint_info_array.cpp @@ -0,0 +1,65 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gmock/gmock.h" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcutils/allocator.h" + +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/types.h" + +TEST(test_topic_endpoint_info_array, zero_initialize) { + rmw_topic_endpoint_info_array_t arr = rmw_get_zero_initialized_topic_endpoint_info_array(); + EXPECT_EQ(arr.count, 0u); + EXPECT_FALSE(arr.info_array); +} + +TEST(test_topic_endpoint_info_array, check_zero) { + rmw_topic_endpoint_info_array_t arr = rmw_get_zero_initialized_topic_endpoint_info_array(); + EXPECT_EQ(rmw_topic_endpoint_info_array_check_zero(&arr), RMW_RET_OK); + rmw_topic_endpoint_info_array_t arr_count_not_zero = {1, nullptr}; + EXPECT_EQ(rmw_topic_endpoint_info_array_check_zero(&arr_count_not_zero), RMW_RET_ERROR); + rmw_topic_endpoint_info_t topic_endpoint_info; + rmw_topic_endpoint_info_array_t arr_info_array_not_null = {0, &topic_endpoint_info}; + EXPECT_EQ(rmw_topic_endpoint_info_array_check_zero(&arr_info_array_not_null), RMW_RET_ERROR); + EXPECT_EQ(rmw_topic_endpoint_info_array_check_zero(nullptr), RMW_RET_INVALID_ARGUMENT); +} + +TEST(test_topic_endpoint_info_array, check_init_with_size) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_topic_endpoint_info_array_t arr = rmw_get_zero_initialized_topic_endpoint_info_array(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + rmw_ret_t fini_ret = rmw_topic_endpoint_info_array_fini(&arr, &allocator); + EXPECT_EQ(fini_ret, RMW_RET_OK); + }); + EXPECT_EQ(rmw_topic_endpoint_info_array_init_with_size(&arr, 1, nullptr), + RMW_RET_INVALID_ARGUMENT); + EXPECT_EQ(rmw_topic_endpoint_info_array_init_with_size(nullptr, 1, + &allocator), RMW_RET_INVALID_ARGUMENT); + EXPECT_FALSE(arr.info_array); + rmw_ret_t ret = rmw_topic_endpoint_info_array_init_with_size(&arr, 5, &allocator); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_TRUE(arr.info_array); +} + +TEST(test_topic_endpoint_info_array, check_fini) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_topic_endpoint_info_array_t arr = rmw_get_zero_initialized_topic_endpoint_info_array(); + rmw_ret_t ret = rmw_topic_endpoint_info_array_init_with_size(&arr, 5, &allocator); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_TRUE(arr.info_array); + ret = rmw_topic_endpoint_info_array_fini(&arr, &allocator); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_FALSE(arr.info_array); +}